The sensor_fusion.h file implements the top level programming interface. More...

#include <Arduino.h>
#include <math.h>
#include <stdbool.h>
#include <stdint.h>
#include <stdio.h>
#include "board.h"
#include "build.h"
#include "driver_sensors_types.h"
#include "magnetic.h"
#include "matrix.h"
#include "orientation.h"
#include "precisionAccelerometer.h"

Include dependency graph for sensor_fusion.h:

This graph shows which files directly or indirectly include this file:

Go to the source code of this file.

Classes
struct	PhysicalSensor
	An instance of PhysicalSensor structure type should be allocated for each physical sensors (combo devices = 1) More...

struct	PressureSensor
	The PressureSensor structure stores raw and processed measurements for an altimeter. More...

struct	AccelSensor
	The AccelSensor structure stores raw and processed measurements for a 3-axis accelerometer. More...

struct	MagSensor
	The MagSensor structure stores raw and processed measurements for a 3-axis magnetic sensor. More...

struct	TempSensor
	The TempSensor structure stores raw temperature readings. More...

struct	GyroSensor
	The GyroSensor structure stores raw and processed measurements for a 3-axis gyroscope. More...

union	FifoSensor
	The FifoSensor union allows us to use common pointers for Accel, Mag & Gyro logical sensor structures. More...

struct	SV_1DOF_P_BASIC
	The SV_1DOF_P_BASIC structure contains state information for a pressure sensor/altimeter. More...

struct	SV_3DOF_G_BASIC
	This is the 3DOF basic accelerometer state vector structure. More...

struct	SV_3DOF_B_BASIC
	This is the 3DOF basic magnetometer state vector structure/. More...

struct	SV_3DOF_Y_BASIC
	SV_3DOF_Y_BASIC structure is the 3DOF basic gyroscope state vector structure. More...

struct	SV_6DOF_GB_BASIC
	SV_6DOF_GB_BASIC is the 6DOF basic accelerometer and magnetometer state vector structure. More...

struct	SV_6DOF_GY_KALMAN
	SV_6DOF_GY_KALMAN is the 6DOF Kalman filter accelerometer and gyroscope state vector structure. More...

struct	SV_9DOF_GBY_KALMAN
	SV_9DOF_GBY_KALMAN is the 9DOF Kalman filter accelerometer, magnetometer and gyroscope state vector structure. More...

struct	SV_COMMON

struct	SensorFusionGlobals
	The top level fusion structure. More...

Macros
#define	true 1
	Boolean TRUE.

#define	false 0
	Boolean FALSE.

Vector Components
Index values for accessing vector terms
#define	CHX 0
	Used to access X-channel entries in various data data structures.

#define	CHY 1
	Used to access Y-channel entries in various data data structures.

#define	CHZ 2
	Used to access Z-channel entries in various data data structures.

Math Constants
useful multiplicative conversion constants
#define	PI 3.141592654F
	pi (it is also defined in Arduino.h)

#define	PIOVER2 1.570796327F
	pi / 2

#define	FPIOVER180 0.01745329251994F
	degrees to radians conversion = pi / 180

#define	F180OVERPI 57.2957795130823F
	radians to degrees conversion = 180 / pi

#define	F180OVERPISQ 3282.8063500117F
	square of F180OVERPI

#define	ONETHIRD 0.33333333F
	one third

#define	ONESIXTH 0.166666667F
	one sixth

#define	ONESIXTEENTH 0.0625F
	one sixteenth

#define	ONEOVER12 0.083333333F
	1 / 12

#define	ONEOVER48 0.02083333333F
	1 / 48

#define	ONEOVER120 0.0083333333F
	1 / 120

#define	ONEOVER3840 0.0002604166667F
	1 / 3840

#define	ONEOVERSQRT2 0.707106781F
	1/sqrt(2)

#define	SQRT15OVER4 0.968245837F
	sqrt(15)/4

#define	GTOMSEC2 9.80665
	standard gravity in m/s2

Typedefs
typedef enum quaternion	quaternion_type
	the quaternion type to be transmitted

typedef int8_t()	initializeSensor_t(struct PhysicalSensor sensor, struct SensorFusionGlobals sfg)

typedef int8_t()	readSensor_t(struct PhysicalSensor sensor, struct SensorFusionGlobals sfg)

typedef int8_t()	readSensors_t(struct SensorFusionGlobals *sfg, uint8_t read_loop_counter)

typedef int8_t()	installSensor_t(struct SensorFusionGlobals sfg, struct PhysicalSensor sensor, uint16_t addr, uint16_t schedule, registerDeviceInfo_t busInfo, initializeSensor_t initialize, readSensor_t *read)

typedef void()	initializeFusionEngine_t(struct SensorFusionGlobals *sfg, int pin_i2c_sda1, int pin_i2c_scl)

typedef void()	runFusion_t(struct SensorFusionGlobals *sfg)

typedef void()	clearFIFOs_t(struct SensorFusionGlobals *sfg)

typedef void()	conditionSensorReadings_t(struct SensorFusionGlobals *sfg)

typedef void()	applyPerturbation_t(struct SensorFusionGlobals *sfg)

typedef void()	setStatus_t(struct SensorFusionGlobals *sfg, fusion_status_t status)

typedef fusion_status_t()	getStatus_t(struct SensorFusionGlobals *sfg)

typedef void()	updateStatus_t(struct SensorFusionGlobals *sfg)

typedef void()	ssSetStatus_t(struct StatusSubsystem *pStatus, fusion_status_t status)

typedef fusion_status_t()	ssGetStatus_t(struct StatusSubsystem *pStatus)

typedef void()	ssUpdateStatus_t(struct StatusSubsystem *pStatus)

typedef struct SV_COMMON *	SV_ptr

typedef struct SensorFusionGlobals	SensorFusionGlobals
	The top level fusion structure.

Enumerations
enum	quaternion { Q3 , Q3M , Q3G , Q6MA , Q6AG , Q9 }
	the quaternion type to be transmitted More...

enum	fusion_status_t { OFF , INITIALIZING , LOWPOWER , NORMAL , RECEIVING_WIRED , RECEIVING_WIRELESS , HARD_FAULT , SOFT_FAULT }
	Application-specific serial communications system. More...

Functions
void	initSensorFusionGlobals (SensorFusionGlobals sfg, struct StatusSubsystem pStatusSubsystem, struct ControlSubsystem *pControlSubsystem)
	utility function to insert default values in the top level structure

void	conditionSensorReadings (SensorFusionGlobals *sfg)

void	clearFIFOs (SensorFusionGlobals *sfg)
	Function to clear FIFO at the end of each fusion computation.

void	zeroArray (struct StatusSubsystem pStatus, void data, uint16_t size, uint16_t numElements, uint8_t check)

void	conditionSample (int16_t sample[3])
	conditionSample ensures that we never encounter the maximum negative two's complement value for a 16-bit variable (-32768).

void	addToFifo (union FifoSensor *sensor, uint16_t maxFifoSize, int16_t sample[3])
	addToFifo is called from within sensor driver read functions

void	ApplyAccelHAL (struct AccelSensor *Accel)
	Apply the accelerometer Hardware Abstraction Layer.

void	ApplyMagHAL (struct MagSensor *Mag)
	Apply the magnetometer Hardware Abstraction Layer.

void	ApplyGyroHAL (struct GyroSensor *Gyro)
	Apply the gyroscope Hardware Abstraction Layer.

Variables
installSensor_t	installSensor

initializeFusionEngine_t	initializeFusionEngine

runFusion_t	runFusion

readSensors_t	readSensors

applyPerturbation_t	ApplyPerturbation
	ApplyPerturbation is a reverse unit-step test function.

Detailed Description

The sensor_fusion.h file implements the top level programming interface.

Definition in file sensor_fusion.h.

Macro Definition Documentation

◆ CHX

#define CHX 0

Used to access X-channel entries in various data data structures.

Definition at line 54 of file sensor_fusion.h.

◆ CHY

#define CHY 1

Used to access Y-channel entries in various data data structures.

Definition at line 55 of file sensor_fusion.h.

◆ CHZ

#define CHZ 2

Used to access Z-channel entries in various data data structures.

Definition at line 56 of file sensor_fusion.h.

◆ F180OVERPI

#define F180OVERPI 57.2957795130823F

radians to degrees conversion = 180 / pi

Definition at line 84 of file sensor_fusion.h.

◆ F180OVERPISQ

#define F180OVERPISQ 3282.8063500117F

square of F180OVERPI

Definition at line 85 of file sensor_fusion.h.

◆ false

#define false 0

Boolean FALSE.

Definition at line 64 of file sensor_fusion.h.

◆ FPIOVER180

#define FPIOVER180 0.01745329251994F

degrees to radians conversion = pi / 180

Definition at line 83 of file sensor_fusion.h.

◆ GTOMSEC2

#define GTOMSEC2 9.80665

standard gravity in m/s2

Definition at line 95 of file sensor_fusion.h.

◆ ONEOVER12

#define ONEOVER12 0.083333333F

1 / 12

Definition at line 89 of file sensor_fusion.h.

◆ ONEOVER120

#define ONEOVER120 0.0083333333F

1 / 120

Definition at line 91 of file sensor_fusion.h.

◆ ONEOVER3840

#define ONEOVER3840 0.0002604166667F

1 / 3840

Definition at line 92 of file sensor_fusion.h.

◆ ONEOVER48

#define ONEOVER48 0.02083333333F

1 / 48

Definition at line 90 of file sensor_fusion.h.

◆ ONEOVERSQRT2

#define ONEOVERSQRT2 0.707106781F

1/sqrt(2)

Definition at line 93 of file sensor_fusion.h.

◆ ONESIXTEENTH

#define ONESIXTEENTH 0.0625F

one sixteenth

Definition at line 88 of file sensor_fusion.h.

◆ ONESIXTH

#define ONESIXTH 0.166666667F

one sixth

Definition at line 87 of file sensor_fusion.h.

◆ ONETHIRD

#define ONETHIRD 0.33333333F

one third

Definition at line 86 of file sensor_fusion.h.

◆ PI

#define PI 3.141592654F

pi (it is also defined in Arduino.h)

Definition at line 80 of file sensor_fusion.h.

◆ PIOVER2

#define PIOVER2 1.570796327F

pi / 2

Definition at line 82 of file sensor_fusion.h.

◆ SQRT15OVER4

#define SQRT15OVER4 0.968245837F

sqrt(15)/4

Definition at line 94 of file sensor_fusion.h.

◆ true

#define true 1

Boolean TRUE.

Definition at line 61 of file sensor_fusion.h.

Typedef Documentation

◆ applyPerturbation_t

typedef void() applyPerturbation_t(struct SensorFusionGlobals *sfg)

Definition at line 142 of file sensor_fusion.h.

◆ clearFIFOs_t

typedef void() clearFIFOs_t(struct SensorFusionGlobals *sfg)

Definition at line 140 of file sensor_fusion.h.

◆ conditionSensorReadings_t

typedef void() conditionSensorReadings_t(struct SensorFusionGlobals *sfg)

Definition at line 141 of file sensor_fusion.h.

◆ getStatus_t

typedef fusion_status_t() getStatus_t(struct SensorFusionGlobals *sfg)

Definition at line 144 of file sensor_fusion.h.

◆ initializeFusionEngine_t

typedef void() initializeFusionEngine_t(struct SensorFusionGlobals *sfg, int pin_i2c_sda1, int pin_i2c_scl)

Definition at line 138 of file sensor_fusion.h.

◆ initializeSensor_t

typedef int8_t() initializeSensor_t(struct PhysicalSensor *sensor, struct SensorFusionGlobals *sfg)

Definition at line 116 of file sensor_fusion.h.

◆ installSensor_t

typedef int8_t() installSensor_t(struct SensorFusionGlobals *sfg, struct PhysicalSensor *sensor, uint16_t addr, uint16_t schedule, registerDeviceInfo_t *busInfo, initializeSensor_t *initialize, readSensor_t *read)

Parameters

sfg	Global data structure pointer
sensor	SF Structure to store sensor configuration
addr	I2C address or SPI_ADDR
schedule	Specifies sampling interval
busInfo	information required for bus power management
initialize	SF Sensor Initialization Function pointer
read	SF Sensor Read Function pointer

Definition at line 128 of file sensor_fusion.h.

◆ readSensor_t

typedef int8_t() readSensor_t(struct PhysicalSensor *sensor, struct SensorFusionGlobals *sfg)

Definition at line 120 of file sensor_fusion.h.

◆ readSensors_t

typedef int8_t() readSensors_t(struct SensorFusionGlobals *sfg, uint8_t read_loop_counter)

Definition at line 124 of file sensor_fusion.h.

◆ runFusion_t

typedef void() runFusion_t(struct SensorFusionGlobals *sfg)

Definition at line 139 of file sensor_fusion.h.

◆ SensorFusionGlobals

typedef struct SensorFusionGlobals SensorFusionGlobals

The top level fusion structure.

The top level fusion structure grows/shrinks based upon flag definitions contained in build.h. These same flags will populate the .iFlags field for run-time access.

◆ setStatus_t

typedef void() setStatus_t(struct SensorFusionGlobals *sfg, fusion_status_t status)

Definition at line 143 of file sensor_fusion.h.

◆ ssGetStatus_t

typedef fusion_status_t() ssGetStatus_t(struct StatusSubsystem *pStatus)

Definition at line 147 of file sensor_fusion.h.

◆ ssSetStatus_t

typedef void() ssSetStatus_t(struct StatusSubsystem *pStatus, fusion_status_t status)

Definition at line 146 of file sensor_fusion.h.

◆ ssUpdateStatus_t

typedef void() ssUpdateStatus_t(struct StatusSubsystem *pStatus)

Definition at line 148 of file sensor_fusion.h.

◆ SV_ptr

typedef struct SV_COMMON* SV_ptr

Definition at line 453 of file sensor_fusion.h.

◆ updateStatus_t

typedef void() updateStatus_t(struct SensorFusionGlobals *sfg)

Definition at line 145 of file sensor_fusion.h.

Enumeration Type Documentation

◆ fusion_status_t

enum fusion_status_t

Application-specific serial communications system.

Enumerator
OFF	These are the state definitions for the status subsystem. Application hasn't started
INITIALIZING	Initializing sensors and algorithms.
LOWPOWER	Running in reduced power mode.
NORMAL	Operation is Nominal.
RECEIVING_WIRED	Receiving commands over wired interface (momentary)
RECEIVING_WIRELESS	Receiving commands over wireless interface (momentary)
HARD_FAULT	Non-recoverable FAULT = something went very wrong.
SOFT_FAULT	Recoverable FAULT = something went wrong, but we can keep going.

Definition at line 104 of file sensor_fusion.h.

◆ quaternion

enum quaternion

the quaternion type to be transmitted

Enumerator
Q3	Quaternion derived from 3-axis accel (tilt)
Q3M	Quaternion derived from 3-axis mag only (auto compass algorithm)
Q3G	Quaternion derived from 3-axis gyro only (rotation)
Q6MA	Quaternion derived from 3-axis accel + 3 axis mag (eCompass)
Q6AG	Quaternion derived from 3-axis accel + 3-axis gyro (gaming)
Q9	Quaternion derived from full 9-axis sensor fusion.

Definition at line 42 of file sensor_fusion.h.

Function Documentation

◆ addToFifo()

void addToFifo	(	union FifoSensor *	sensor,
		uint16_t	maxFifoSize,
		int16_t	sample[3]
	)

addToFifo is called from within sensor driver read functions

addToFifo is called from within sensor driver read functions to transfer new readings into the sensor structure corresponding to accel, gyro or mag. This function ensures that the software FIFOs are not overrun.

example usage: if (status==SENSOR_ERROR_NONE) addToFifo((FifoSensor*) &(sfg->Mag), MAG_FIFO_SIZE, sample);

Parameters

sensor	pointer to structure of type AccelSensor, MagSensor or GyroSensor
maxFifoSize	the size of the software (not hardware) FIFO
sample	the sample to add

Definition at line 560 of file sensor_fusion.c.

◆ ApplyAccelHAL()

void ApplyAccelHAL ( struct AccelSensor * Accel )

Apply the accelerometer Hardware Abstraction Layer.

Parameters

Accel pointer to accelerometer logical sensor

Definition at line 58 of file hal_axis_remap.c.

◆ ApplyGyroHAL()

void ApplyGyroHAL ( struct GyroSensor * Gyro )

Apply the gyroscope Hardware Abstraction Layer.

Parameters

Gyro	pointer to gyroscope logical sensor

Definition at line 104 of file hal_axis_remap.c.

◆ ApplyMagHAL()

void ApplyMagHAL ( struct MagSensor * Mag )

Apply the magnetometer Hardware Abstraction Layer.

Parameters

Mag	pointer to magnetometer logical sensor

Definition at line 81 of file hal_axis_remap.c.

◆ clearFIFOs()

void clearFIFOs ( SensorFusionGlobals * sfg )

Function to clear FIFO at the end of each fusion computation.

Parameters

sfg	Global data structure pointer

Definition at line 384 of file sensor_fusion.c.

◆ conditionSample()

void conditionSample ( int16_t sample[3] )

conditionSample ensures that we never encounter the maximum negative two's complement value for a 16-bit variable (-32768).

conditionSample ensures that we never encounter the maximum negative two's complement value for a 16-bit variable (-32768). This value cannot be negated, because the maximum positive value is +32767. We need the ability to negate to gaurantee that subsequent HAL operations can be run successfully.

Parameters

sample 16-bit register value from triaxial sensor read

Definition at line 547 of file sensor_fusion.c.

◆ conditionSensorReadings()

void conditionSensorReadings ( SensorFusionGlobals * sfg )

conditionSensorReadings() transforms raw software FIFO readings into forms that can be consumed by the sensor fusion engine. This include sample averaging and (in the case of the gyro) integrations, applying hardware abstraction layers, and calibration functions. This function is normally involved via the "sfg." global pointer.

conditionSensorReadings() transforms raw software FIFO readings into forms that can be consumed by the sensor fusion engine. This include sample averaging and (in the case of the gyro) integrations, applying hardware abstraction layers, and calibration functions. This function is normally invoked via the "sfg." global pointer.

Parameters

sfg	Global data structure pointer

Definition at line 325 of file sensor_fusion.c.

◆ initSensorFusionGlobals()

void initSensorFusionGlobals	(	SensorFusionGlobals *	sfg,
		struct StatusSubsystem *	pStatusSubsystem,
		struct ControlSubsystem *	pControlSubsystem
	)

utility function to insert default values in the top level structure

Parameters

sfg	Global data structure pointer
pStatusSubsystem	Status subsystem pointer
pControlSubsystem	Control subsystem pointer

Definition at line 56 of file sensor_fusion.c.

◆ zeroArray()

void zeroArray	(	struct StatusSubsystem *	pStatus,
		void *	data,
		uint16_t	size,
		uint16_t	numElements,
		uint8_t	check
	)

Parameters

pStatus	Status subsystem pointer
data	pointer to array to be zeroed
size	data type size = 8, 16 or 32
numElements	number of elements to zero out
check	true if you would like to verify writes, false otherwise

Definition at line 340 of file sensor_fusion.c.

Variable Documentation

◆ ApplyPerturbation

applyPerturbation_t ApplyPerturbation

ApplyPerturbation is a reverse unit-step test function.

The ApplyPerturbation function applies a user-specified step function to prior fusion results which is then "released" in the next fusion cycle. When used in conjuction with the NXP Sensor Fusion Toolbox, this provides a visual indication of the dynamic behavior of the library. ApplyPerturbation() is defined in fusion_testing.c.

Definition at line 623 of file sensor_fusion.h.

◆ initializeFusionEngine

initializeFusionEngine_t initializeFusionEngine

Definition at line 555 of file sensor_fusion.h.

◆ installSensor

installSensor_t installSensor

Definition at line 554 of file sensor_fusion.h.

◆ readSensors

readSensors_t readSensors

Definition at line 568 of file sensor_fusion.h.

◆ runFusion

runFusion_t runFusion

Definition at line 567 of file sensor_fusion.h.

Classes

Macros

Typedefs

Enumerations

Functions

Variables

Detailed Description

Macro Definition Documentation

◆ CHX

◆ CHY

◆ CHZ

◆ F180OVERPI

◆ F180OVERPISQ

◆ false

◆ FPIOVER180

◆ GTOMSEC2

◆ ONEOVER12

◆ ONEOVER120

◆ ONEOVER3840

◆ ONEOVER48

◆ ONEOVERSQRT2

◆ ONESIXTEENTH

◆ ONESIXTH

◆ ONETHIRD

◆ PI

◆ PIOVER2

◆ SQRT15OVER4

◆ true

Typedef Documentation

◆ applyPerturbation_t

◆ clearFIFOs_t

◆ conditionSensorReadings_t

◆ getStatus_t

◆ initializeFusionEngine_t

◆ initializeSensor_t

◆ installSensor_t

◆ readSensor_t

◆ readSensors_t

◆ runFusion_t

◆ SensorFusionGlobals

◆ setStatus_t

◆ ssGetStatus_t

◆ ssSetStatus_t

◆ ssUpdateStatus_t

◆ SV_ptr

◆ updateStatus_t

Enumeration Type Documentation

◆ fusion_status_t

◆ quaternion

Function Documentation

◆ addToFifo()

◆ ApplyAccelHAL()

◆ ApplyGyroHAL()

◆ ApplyMagHAL()

◆ clearFIFOs()

◆ conditionSample()

◆ conditionSensorReadings()

◆ initSensorFusionGlobals()

◆ zeroArray()

Variable Documentation

◆ ApplyPerturbation

◆ initializeFusionEngine

◆ installSensor

◆ readSensors

◆ runFusion