#include <sensor_fusion_class.h>

Collaboration diagram for SensorFusion:

[legend]

Public Member Functions
	SensorFusion ()

bool	InstallSensor (uint8_t sensor_i2c_addr, SensorType sensor_type)
	Install Sensor in linked list The max length of the list is checked, and if there is room, the given sensor is inserted at the head of the list. An accelerometer and magnetometer may be combined in one IC - if that is the case then only one call is required to install, provided the associated _Init() and _Read() function reads both the accel & magnetometer data. The Init() and Read() functions of each sensor are defined in driver_. files.

bool	InitializeInputOutputSubsystem (const Stream serial_port=NULL, const void tcp_client=NULL)

void	Begin (int pin_i2c_sda=-1, int pin_i2c_scl=-1)

void	UpdateWiFiStream (void *tcp_client)
	Update the TCP client pointer. Call when a new TCP connection is made, as reported by WiFiServer::available() When tcp_client is NULL, output via WiFi is not attempted.

void	ReadSensors (void)
	Reads all sensors. Applies HAL remapping, removes invalid values, and stores data for later processing. Depending on whether a sensor has a built-in FIFO buffer, that sensor may not be read each time this method is called. See kLoopsPerMagRead, etc., in sensor_fusion_class.h.

void	RunFusion (void)
	Apply fusion algorithm to sensor raw data. Sensor readings contained in global struct are calibrated and processed. Status is updated and displayed. Loop counter used for coordinating sensor reads is reset.

void	ProduceToolboxOutput (void)
	Generate and send out data, formatted for NXP Orientation Sensor Toolbox. It is not mandatory to call this routine, if Toolbox output is not needed.

bool	SendArbitraryData (const char *buffer, uint16_t data_length)

void	ProcessCommands (void)
	Process any incoming commands. Commands may arrive by serial or WiFi connection, depending on which of these is enabled (if any). It is not mandatory to call this routine, if command responses are not needed.

void	InjectCommand (const char *command)
	Inject and process a single command in the control subsystem.

void	SaveMagneticCalibration (void)
	Save current magnetic calibration to non-volatile memory.

bool	IsDataValid (void)

int	GetSystemStatus (void)

float	GetHeadingDegrees (void)

float	GetPitchDegrees (void)

float	GetRollDegrees (void)

float	GetHeadingRadians (void)

float	GetPitchRadians (void)

float	GetRollRadians (void)

float	GetAccelXGees (void)

float	GetAccelYGees (void)

float	GetAccelZGees (void)

float	GetAccelXMPerSS (void)

float	GetAccelYMPerSS (void)

float	GetAccelZMPerSS (void)

float	GetTurnRateDegPerS (void)

float	GetPitchRateDegPerS (void)

float	GetRollRateDegPerS (void)

float	GetTurnRateRadPerS (void)

float	GetPitchRateRadPerS (void)

float	GetRollRateRadPerS (void)

float	GetTemperatureC (void)

float	GetTemperatureK (void)

void	GetOrientationQuaternion (Quaternion *quat)
	Return the orientation as a quaternion.

float	GetMagneticFitError (void)

float	GetMagneticFitErrorTrial (void)

float	GetMagneticBMag (void)

float	GetMagneticBMagTrial (void)

float	GetMagneticInclinationDeg (void)

float	GetMagneticInclinationRad (void)

float	GetMagneticNoiseCovariance (void)

float	GetMagneticCalSolver (void)

Private Member Functions
void	InitializeStatusSubsystem (void)
	Initialize the Status reporting system. Status is indicated by LEDs, but other means can be added.

void	InitializeSensorFusionGlobals (void)
	Set the starting values of variables contained in sfg_.

Private Attributes
SensorFusionGlobals *	sfg_
	Primary sensor fusion data structure.

ControlSubsystem *	control_subsystem_
	command and data streaming structure

StatusSubsystem *	status_subsystem_
	visual status indicator structure

PhysicalSensor *	sensors_
	linked list of up to 4 sensors

uint8_t	num_sensors_installed_
	tracks how many sensors have been added to list

const uint8_t	kLoopsPerMagRead
	how often a magnetometer read is performed

const uint8_t	kLoopsPerThermRead
	how often a thermometer read is performed

const uint8_t	kLoopsPerAccelRead
	how often an accelerometer read is performed

const uint8_t	kLoopsPerGyroRead
	how often a gyroscope read is performed

const uint8_t	kLoopsPerFusionCalc
	how often to fuse. Usually the max of previous 3 constants.

uint8_t	loops_per_fuse_counter_
	counts how many times through loop have been done

Detailed Description

Class that wraps the various mostly-C-style functions of the sensor fusion code into easier to use methods. Not all the lower-level functions are exposed however; for more advanced use it will be necessary to call them directly.

Definition at line 46 of file sensor_fusion_class.h.

Constructor & Destructor Documentation

◆ SensorFusion()

SensorFusion::SensorFusion ( )

Constructor creates and initializes a structure of variables used throughout the functions. It initializes the control and status subsystems as well as several subordinate data structures.

Definition at line 45 of file sensor_fusion_class.cc.

Member Function Documentation

◆ Begin()

void SensorFusion::Begin	(	int	pin_i2c_sda = `-1`,
		int	pin_i2c_scl = `-1`
	)

Initialize the Sensors. Read calibrations. Set status to Normal.

Definition at line 143 of file sensor_fusion_class.cc.

◆ GetAccelXGees()

float SensorFusion::GetAccelXGees ( void )

Returns: Return the X-axis Acceleration in gees

Definition at line 454 of file sensor_fusion_class.cc.

◆ GetAccelXMPerSS()

float SensorFusion::GetAccelXMPerSS ( void )

Returns: Return the X-axis Acceleration in m/s^2

Definition at line 461 of file sensor_fusion_class.cc.

◆ GetAccelYGees()

float SensorFusion::GetAccelYGees ( void )

Returns: Return the Y-axis Acceleration in gees

Definition at line 468 of file sensor_fusion_class.cc.

◆ GetAccelYMPerSS()

float SensorFusion::GetAccelYMPerSS ( void )

Returns: Return the Y-axis Acceleration in m/s^2

Definition at line 475 of file sensor_fusion_class.cc.

◆ GetAccelZGees()

float SensorFusion::GetAccelZGees ( void )

Returns: Return the Z-axis Acceleration in gees

Definition at line 482 of file sensor_fusion_class.cc.

◆ GetAccelZMPerSS()

float SensorFusion::GetAccelZMPerSS ( void )

Returns: Return the Z-axis Acceleration in m/s^2

Definition at line 489 of file sensor_fusion_class.cc.

◆ GetHeadingDegrees()

float SensorFusion::GetHeadingDegrees ( void )

Returns: Return the Compass Heading in degrees

Definition at line 345 of file sensor_fusion_class.cc.

◆ GetHeadingRadians()

float SensorFusion::GetHeadingRadians ( void )

Returns: Return the Compass Heading in radians

Definition at line 355 of file sensor_fusion_class.cc.

◆ GetMagneticBMag()

float SensorFusion::GetMagneticBMag ( void )

Returns: Return geomagnetic field magnitude in uT of current calibration.

This value does not change with each sensor reading - it only updates if a new calibration parameter set is calculated and becomes the currently-used calibration.

Definition at line 559 of file sensor_fusion_class.cc.

◆ GetMagneticBMagTrial()

float SensorFusion::GetMagneticBMagTrial ( void )

Returns: Return geomagnetic field magnitude in uT of trial calibration.

This value is the current best-estimate of the B field using recent magnetic sensor readings. It updates with each new sensor reading added to the buffer. If the new trial calibration parameters are accepted and replace the current calibration, then this B field becomes associated with that set.

This parameter provides an indication of magnetic interference; if it differs widely from the B value of the current calibration then one can conclude that the current calibration is not valid for the present magnetic environment.

Definition at line 547 of file sensor_fusion_class.cc.

◆ GetMagneticCalSolver()

float SensorFusion::GetMagneticCalSolver ( void )

Returns: Return solver used for current calibration [0,4,7,10]

The solver is the number of elements (complexity) of the calibration algorithm. 0 means no calibration; 10 is the most complex.

Definition at line 617 of file sensor_fusion_class.cc.

◆ GetMagneticFitError()

float SensorFusion::GetMagneticFitError ( void )

Returns: Return magnetic fit error of current calibration

This function returns the measure of the goodness of fit of the currently-used calibration parameter set. Units are in % and a value less than 3.5% is considered good. If no calibration is available on startup (i.e. none saved in EEPROM) then a value of 0 is returned briefly during program startup, indicating that insufficient data are available to calculate a fit.

Definition at line 529 of file sensor_fusion_class.cc.

◆ GetMagneticFitErrorTrial()

float SensorFusion::GetMagneticFitErrorTrial ( void )

Returns: Return magnetic fit error of trial calibration

The background calibration task uses recent magnetic readings to periodically calculate a new set of calibration parameters. This function returns the measure of goodness of fit of the new calibration parameter set. Units are in % and a value less than 3.5% is considered good. A value of 0 is returned briefly during program startup, indicating that insufficient data are available to calculate a fit.

Definition at line 515 of file sensor_fusion_class.cc.

◆ GetMagneticInclinationDeg()

float SensorFusion::GetMagneticInclinationDeg ( void )

Returns: Return geomagnetic field inclination from horizontal in degrees.

This is the a posteriori value from Kalman filter output and updates with each sensor reading. It can be used as an indication of magnetic interference; if its value changes by more than, say, 10 degrees from recent previous values then it indicates a disturbance in the magnetic environment.

Magnetic inclination values vary over the earth's surface and are available from sources such as NOAA.

Definition at line 576 of file sensor_fusion_class.cc.

◆ GetMagneticInclinationRad()

float SensorFusion::GetMagneticInclinationRad ( void )

Returns: Return geomagnetic field inclination from horizontal in radians.

This is the a posteriori value from Kalman filter output and updates with each sensor reading. It can be used as an indication of magnetic interference; if its value changes by more than, say, 10 degrees from recent previous values then it indicates a disturbance in the magnetic environment.

Magnetic inclination values vary over the earth's surface and are available from sources such as NOAA.

Definition at line 593 of file sensor_fusion_class.cc.

◆ GetMagneticNoiseCovariance()

float SensorFusion::GetMagneticNoiseCovariance ( void )

Returns: Return measurement noise covariance value for magnetic field.

This is proportional to how heavily the magnetic readings are weighted in calculating the a posteriori orientation. This is a good measure of magnetic interference; when the noise covariance value rises above about 0.00056 it indicates the current magnetic reading does not lie close to the surface of the calibrated geomagnetic sphere.

Definition at line 607 of file sensor_fusion_class.cc.

◆ GetOrientationQuaternion()

void SensorFusion::GetOrientationQuaternion ( Quaternion * quat )

Return the orientation as a quaternion.

Parameters

quat	pointer to quaternion structure, to be filled by this method

Definition at line 497 of file sensor_fusion_class.cc.

◆ GetPitchDegrees()

float SensorFusion::GetPitchDegrees ( void )

Returns: Return the Pitch in degrees

Definition at line 362 of file sensor_fusion_class.cc.

◆ GetPitchRadians()

float SensorFusion::GetPitchRadians ( void )

Returns: Return the Pitch in radians

Definition at line 369 of file sensor_fusion_class.cc.

◆ GetPitchRateDegPerS()

float SensorFusion::GetPitchRateDegPerS ( void )

Returns: Return the Pitch Rate in degrees/s

Definition at line 426 of file sensor_fusion_class.cc.

◆ GetPitchRateRadPerS()

float SensorFusion::GetPitchRateRadPerS ( void )

Returns: Return the Pitch Rate in rad/s

Definition at line 433 of file sensor_fusion_class.cc.

◆ GetRollDegrees()

float SensorFusion::GetRollDegrees ( void )

Returns: Return the Roll in degrees

Definition at line 376 of file sensor_fusion_class.cc.

◆ GetRollRadians()

float SensorFusion::GetRollRadians ( void )

Returns: Return the Roll in radians

Definition at line 383 of file sensor_fusion_class.cc.

◆ GetRollRateDegPerS()

float SensorFusion::GetRollRateDegPerS ( void )

Returns: Return the Roll Rate in degrees/s

Definition at line 440 of file sensor_fusion_class.cc.

◆ GetRollRateRadPerS()

float SensorFusion::GetRollRateRadPerS ( void )

Returns: Return the Roll Rate in rad/s

Definition at line 447 of file sensor_fusion_class.cc.

◆ GetSystemStatus()

int SensorFusion::GetSystemStatus ( void )

Returns: Fusion System status

System status is indicated by an integer. See sensor_fusion.h for details. Common values are: 1 (Initializing); 3 (Normal); and 7 (Soft Fault - usually caused by communications fault with sensor IC)

Definition at line 317 of file sensor_fusion_class.cc.

◆ GetTemperatureC()

float SensorFusion::GetTemperatureC ( void )

Returns: Return the Temperature in degrees C

This is the temperature of the sensor IC die. It is not factory-trimmed, so it very likely has an offset that should be calibrated out for best accuracy.

Definition at line 394 of file sensor_fusion_class.cc.

◆ GetTemperatureK()

float SensorFusion::GetTemperatureK ( void )

Returns: Return the Temperature in degrees K

This is the temperature of the sensor IC die. It is not factory-trimmed, so it very likely has an offset that should be calibrated out for best accuracy.

Definition at line 405 of file sensor_fusion_class.cc.

◆ GetTurnRateDegPerS()

float SensorFusion::GetTurnRateDegPerS ( void )

Returns: Return the Turn Rate in degrees

Definition at line 412 of file sensor_fusion_class.cc.

◆ GetTurnRateRadPerS()

float SensorFusion::GetTurnRateRadPerS ( void )

Returns: Return the Turn Rate in rad/s

Definition at line 419 of file sensor_fusion_class.cc.

◆ InitializeInputOutputSubsystem()

bool SensorFusion::InitializeInputOutputSubsystem	(	const Stream *	serial_port = `NULL`,
		const void *	tcp_client = `NULL`
	)

Initialize the Control subsystem, which receives external commands and sends data packets.

Parameters

serial_port	A Stream pointer to which to send output and receive commands. If NULL, then output is not attempted.
tcp_client	A WiFiClient pointer to which to send output and receive commands. If NULL, then output is not attempted.

Returns: True if the Control subsystem is initialized, else False.

Definition at line 135 of file sensor_fusion_class.cc.

◆ InitializeSensorFusionGlobals()

void SensorFusion::InitializeSensorFusionGlobals ( void )

private

Set the starting values of variables contained in sfg_.

Definition at line 637 of file sensor_fusion_class.cc.

◆ InitializeStatusSubsystem()

void SensorFusion::InitializeStatusSubsystem ( void )

private

Initialize the Status reporting system. Status is indicated by LEDs, but other means can be added.

Definition at line 628 of file sensor_fusion_class.cc.

◆ InjectCommand()

void SensorFusion::InjectCommand ( const char * command )

Inject and process a single command in the control subsystem.

Command provided by this call bypasses the usual input route (serial UART or WiFi) and is instead provided directly in this function call. This provides an alterative way of manipulating the fusion algortithm without needing to use the entire control or input subsystem, For a list of valid commands, see control_input.c in the Orientation library.

Parameters

command is a four-character sequence selected from the list of valid commands. Illegal commands are ignored without any action taken.

Definition at line 273 of file sensor_fusion_class.cc.

◆ InstallSensor()

bool SensorFusion::InstallSensor	(	uint8_t	sensor_i2c_addr,
		SensorType	sensor_type
	)

Install Sensor in linked list The max length of the list is checked, and if there is room, the given sensor is inserted at the head of the list. An accelerometer and magnetometer may be combined in one IC - if that is the case then only one call is required to install, provided the associated *_Init() and *_Read() function reads both the accel & magnetometer data. The Init() and Read() functions of each sensor are defined in driver_*.* files.

Parameters

sensor_i2c_addr	is the I2C bus address of the sensor IC
sensor_type	indicates the type of sensor (e.g. magnetometer)

Returns: True if sensor installed successfully, else False

Definition at line 75 of file sensor_fusion_class.cc.

◆ IsDataValid()

bool SensorFusion::IsDataValid ( void )

Returns: Boolean indicating whether orientation data are valid

Definition at line 301 of file sensor_fusion_class.cc.

◆ ProcessCommands()

void SensorFusion::ProcessCommands ( void )

Process any incoming commands. Commands may arrive by serial or WiFi connection, depending on which of these is enabled (if any). It is not mandatory to call this routine, if command responses are not needed.

Definition at line 255 of file sensor_fusion_class.cc.

◆ ProduceToolboxOutput()

void SensorFusion::ProduceToolboxOutput ( void )

Generate and send out data, formatted for NXP Orientation Sensor Toolbox. It is not mandatory to call this routine, if Toolbox output is not needed.

Definition at line 219 of file sensor_fusion_class.cc.

◆ ReadSensors()

void SensorFusion::ReadSensors ( void )

Reads all sensors. Applies HAL remapping, removes invalid values, and stores data for later processing. Depending on whether a sensor has a built-in FIFO buffer, that sensor may not be read each time this method is called. See kLoopsPerMagRead, etc., in sensor_fusion_class.h.

Definition at line 171 of file sensor_fusion_class.cc.

◆ RunFusion()

void SensorFusion::RunFusion ( void )

Apply fusion algorithm to sensor raw data. Sensor readings contained in global struct are calibrated and processed. Status is updated and displayed. Loop counter used for coordinating sensor reads is reset.

Definition at line 185 of file sensor_fusion_class.cc.

◆ SaveMagneticCalibration()

void SensorFusion::SaveMagneticCalibration ( void )

Save current magnetic calibration to non-volatile memory.

Passes to the fusion control subsystem the command to save the current magnetic calibration parameters to EEPROM. This calibration will then be loaded each time the system restarts.

One can pass this command directly using InjectCommand(), but SaveMagneticCalibration() is provided as a convenience.

The fusion system continuously runs a time-sliced calibration routine in the background, and if a superior set of calibration parameters are calculated based on recent magnetic measurements, then these new parameters replace the current ones in RAM. However, it is only via this command that the parameters can be caused to persist beyond the next system reset.

Definition at line 294 of file sensor_fusion_class.cc.

◆ SendArbitraryData()

bool SensorFusion::SendArbitraryData	(	const char *	buffer,
		uint16_t	data_length
	)

places data from buffer into Control subsystem's output buffer, and sends it out via serial and/or wifi. Any existing data in the output buffer that hasn't already been sent will be overwritten. Returns true on success, false on problem such as data_length too long for the transmit buffer.

Definition at line 236 of file sensor_fusion_class.cc.

◆ UpdateWiFiStream()

void SensorFusion::UpdateWiFiStream ( void * tcp_client )

Update the TCP client pointer. Call when a new TCP connection is made, as reported by WiFiServer::available() When tcp_client is NULL, output via WiFi is not attempted.

Parameters

tcp_client A WiFiClient pointer used for input/output

Definition at line 159 of file sensor_fusion_class.cc.

Member Data Documentation

◆ control_subsystem_

ControlSubsystem* SensorFusion::control_subsystem_

private

command and data streaming structure

Definition at line 99 of file sensor_fusion_class.h.

◆ kLoopsPerAccelRead

const uint8_t SensorFusion::kLoopsPerAccelRead

private

Initial value:

=

1

how often an accelerometer read is performed

Definition at line 117 of file sensor_fusion_class.h.

◆ kLoopsPerFusionCalc

const uint8_t SensorFusion::kLoopsPerFusionCalc

private

Initial value:

=

1

how often to fuse. Usually the max of previous 3 constants.

Definition at line 121 of file sensor_fusion_class.h.

◆ kLoopsPerGyroRead

const uint8_t SensorFusion::kLoopsPerGyroRead

private

Initial value:

=

1

how often a gyroscope read is performed

Definition at line 119 of file sensor_fusion_class.h.

◆ kLoopsPerMagRead

const uint8_t SensorFusion::kLoopsPerMagRead

private

Initial value:

=

1

how often a magnetometer read is performed

Constants of the form kLoopsPer_____ set the relationship between number of sensor reads and each execution of the fusion algorithm. Normally there is a 1:1 relationship (i.e. read, fuse, read, fuse,...) but other arrangements are possible (e.g. read, read, fuse, read, read,...) The rate at which main loop() executes is set by LOOP_RATE_HZ in build.h

Definition at line 113 of file sensor_fusion_class.h.

◆ kLoopsPerThermRead

const uint8_t SensorFusion::kLoopsPerThermRead

private

Initial value:

=

1

how often a thermometer read is performed

Definition at line 115 of file sensor_fusion_class.h.

◆ loops_per_fuse_counter_

uint8_t SensorFusion::loops_per_fuse_counter_

private

Initial value:

=

0

counts how many times through loop have been done

Definition at line 123 of file sensor_fusion_class.h.

◆ num_sensors_installed_

uint8_t SensorFusion::num_sensors_installed_

private

Initial value:

=

0

tracks how many sensors have been added to list

Definition at line 102 of file sensor_fusion_class.h.

◆ sensors_

PhysicalSensor* SensorFusion::sensors_

private

linked list of up to 4 sensors

Definition at line 101 of file sensor_fusion_class.h.

◆ sfg_

SensorFusionGlobals* SensorFusion::sfg_

private

Primary sensor fusion data structure.

Definition at line 97 of file sensor_fusion_class.h.

◆ status_subsystem_

StatusSubsystem* SensorFusion::status_subsystem_

private

visual status indicator structure

Definition at line 100 of file sensor_fusion_class.h.

The documentation for this class was generated from the following files:

src/sensor_fusion_class.h
src/sensor_fusion_class.cc

Public Member Functions

Private Member Functions

Private Attributes

Detailed Description

Constructor & Destructor Documentation

◆ SensorFusion()

Member Function Documentation

◆ Begin()

◆ GetAccelXGees()

◆ GetAccelXMPerSS()

◆ GetAccelYGees()

◆ GetAccelYMPerSS()

◆ GetAccelZGees()

◆ GetAccelZMPerSS()

◆ GetHeadingDegrees()

◆ GetHeadingRadians()

◆ GetMagneticBMag()

◆ GetMagneticBMagTrial()

◆ GetMagneticCalSolver()

◆ GetMagneticFitError()

◆ GetMagneticFitErrorTrial()

◆ GetMagneticInclinationDeg()

◆ GetMagneticInclinationRad()

◆ GetMagneticNoiseCovariance()

◆ GetOrientationQuaternion()

◆ GetPitchDegrees()

◆ GetPitchRadians()

◆ GetPitchRateDegPerS()

◆ GetPitchRateRadPerS()

◆ GetRollDegrees()

◆ GetRollRadians()

◆ GetRollRateDegPerS()

◆ GetRollRateRadPerS()

◆ GetSystemStatus()

◆ GetTemperatureC()

◆ GetTemperatureK()

◆ GetTurnRateDegPerS()

◆ GetTurnRateRadPerS()

◆ InitializeInputOutputSubsystem()

◆ InitializeSensorFusionGlobals()

◆ InitializeStatusSubsystem()

◆ InjectCommand()

◆ InstallSensor()

◆ IsDataValid()

◆ ProcessCommands()

◆ ProduceToolboxOutput()

◆ ReadSensors()

◆ RunFusion()

◆ SaveMagneticCalibration()

◆ SendArbitraryData()

◆ UpdateWiFiStream()

Member Data Documentation

◆ control_subsystem_

◆ kLoopsPerAccelRead

◆ kLoopsPerFusionCalc

◆ kLoopsPerGyroRead

◆ kLoopsPerMagRead

◆ kLoopsPerThermRead

◆ loops_per_fuse_counter_

◆ num_sensors_installed_

◆ sensors_

◆ sfg_

◆ status_subsystem_