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

#include <stdio.h>
#include "sensor_fusion.h"
#include "control.h"
#include "fusion.h"
#include "hal_i2c.h"
#include "hal_timer.h"
#include "status.h"

Include dependency graph for sensor_fusion.c:

Go to the source code of this file.

Functions
void	setStatus (SensorFusionGlobals *sfg, fusion_status_t status)

fusion_status_t	getStatus (SensorFusionGlobals *sfg)

void	queueStatus (SensorFusionGlobals *sfg, fusion_status_t status)

void	updateStatus (SensorFusionGlobals *sfg)

void	testStatus (SensorFusionGlobals *sfg)

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

int8_t	installSensor (SensorFusionGlobals sfg, struct PhysicalSensor pSensor, uint16_t addr, uint16_t schedule, registerDeviceInfo_t busInfo, initializeSensor_t initialize, readSensor_t *read)

int8_t	initializeSensors (SensorFusionGlobals *sfg)

void	processMagData (SensorFusionGlobals *sfg)

int8_t	readSensors (SensorFusionGlobals *sfg, uint8_t read_loop_counter)

void	conditionSensorReadings (SensorFusionGlobals *sfg)

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

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

void	runFusion (SensorFusionGlobals *sfg)

void	initializeFusionEngine (SensorFusionGlobals *sfg, int pin_i2c_sda, int pin_i2c_scl)

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

Detailed Description

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

Definition in file sensor_fusion.c.

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.

◆ 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 invoked via the "sfg." global pointer.

Parameters

sfg	Global data structure pointer

Definition at line 325 of file sensor_fusion.c.

◆ getStatus()

fusion_status_t getStatus ( SensorFusionGlobals * sfg )

Poor man's inheritance for status subsystem getStatus command This function is normally invoked via the "sfg." global pointer.

Definition at line 31 of file sensor_fusion.c.

◆ initializeFusionEngine()

void initializeFusionEngine	(	SensorFusionGlobals *	sfg,
		int	pin_i2c_sda,
		int	pin_i2c_scl
	)

This function is responsible for initializing the system prior to starting the main fusion loop. I2C is initted, sensors configured, calibrations loaded. This function is normally invoked via the "sfg." global pointer. Fusion system status is set to: INITIALIZING at the start of this function, HARD_FAULT if a problem occurs initializing the I2C hardware, SOFT_FAULT if a sensor doesn't initialize (it could be corrected later), NORMAL when function ends, assuming no problem occurred

Definition at line 497 of file sensor_fusion.c.

◆ initializeSensors()

int8_t initializeSensors ( SensorFusionGlobals * sfg )

Definition at line 151 of file sensor_fusion.c.

◆ initSensorFusionGlobals()

void initSensorFusionGlobals	(	SensorFusionGlobals *	sfg,
		StatusSubsystem *	pStatusSubsystem,
		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.

◆ installSensor()

int8_t installSensor	(	SensorFusionGlobals *	sfg,
		struct PhysicalSensor *	pSensor,
		uint16_t	addr,
		uint16_t	schedule,
		registerDeviceInfo_t *	busInfo,
		initializeSensor_t *	initialize,
		readSensor_t *	read
	)

installSensor is used to instantiate a physical sensor driver into the sensor fusion system. It doesn't actually communicate with the sensor. This function is normally invoked via the "sfg." global pointer.

Parameters

sfg	top level fusion structure
pSensor	pointer to structure describing physical sensor
addr	I2C address for sensor (if applicable)
schedule	Sensor is read each time loop_count % schedule == 0
busInfo	information required for bus power management
initialize	pointer to sensor initialization function
read	pointer to sensor read function

Definition at line 112 of file sensor_fusion.c.

◆ processMagData()

void processMagData ( SensorFusionGlobals * sfg )

Definition at line 204 of file sensor_fusion.c.

◆ queueStatus()

void queueStatus	(	SensorFusionGlobals *	sfg,
		fusion_status_t	status
	)

Poor man's inheritance for status subsystem queueStatus command. This function is normally involved via the "sfg." global pointer.

Definition at line 38 of file sensor_fusion.c.

◆ readSensors()

int8_t readSensors	(	SensorFusionGlobals *	sfg,
		uint8_t	read_loop_counter
	)

readSensors traverses the linked list of physical sensors, calling the individual read functions one by one. This function is normally invoked via the "sfg." global pointer. If a sensor is flagged as uninitialized, an attempt is made to initialize it. If a sensor does not respond, it is marked as unintialized.

Parameters

sfg	pointer to global sensor fusion data structure
read_loop_counter	current loop counter (used for multirate processing)

Definition at line 277 of file sensor_fusion.c.

◆ runFusion()

void runFusion ( SensorFusionGlobals * sfg )

runFusion the top level call that actually runs the sensor fusion. This is a utility function which manages the various defines in build.h. You should feel free to drop down a level and implement only those portions of fFuseSensors() that your application needs. This function is normally involved via the "sfg." global pointer.

Definition at line 407 of file sensor_fusion.c.

◆ setStatus()

void setStatus	(	SensorFusionGlobals *	sfg,
		fusion_status_t	status
	)

Poor man's inheritance for status subsystem setStatus command This function is normally invoked via the "sfg." global pointer.

Definition at line 24 of file sensor_fusion.c.

◆ testStatus()

void testStatus ( SensorFusionGlobals * sfg )

Definition at line 50 of file sensor_fusion.c.

◆ updateStatus()

void updateStatus ( SensorFusionGlobals * sfg )

Poor man's inheritance for status subsystem updateStatus command. This function is normally involved via the "sfg." global pointer.

Definition at line 45 of file sensor_fusion.c.

◆ zeroArray()

void zeroArray	(	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.

Functions

Detailed Description

Function Documentation

◆ addToFifo()

◆ clearFIFOs()

◆ conditionSample()

◆ conditionSensorReadings()

◆ getStatus()

◆ initializeFusionEngine()

◆ initializeSensors()

◆ initSensorFusionGlobals()

◆ installSensor()

◆ processMagData()

◆ queueStatus()

◆ readSensors()

◆ runFusion()

◆ setStatus()

◆ testStatus()

◆ updateStatus()

◆ zeroArray()