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

#include <stdio.h>
#include "sensor_fusion.h"
#include "magnetic.h"
#include "drivers.h"
#include "sensor_drv.h"
#include "status.h"
#include "control.h"
#include "fusion.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)

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 More...

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

int8_t	initializeSensors (SensorFusionGlobals *sfg)

void	processMagData (SensorFusionGlobals *sfg)

int8_t	readSensors (SensorFusionGlobals *sfg, uint16_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. More...

void	runFusion (SensorFusionGlobals *sfg)

void	initializeFusionEngine (SensorFusionGlobals *sfg)

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). More...

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

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 548 of file sensor_fusion.c.

References FifoSensor::Accel, CHX, CHY, CHZ, AccelSensor::iFIFOCount, AccelSensor::iFIFOExceeded, and AccelSensor::iGsFIFO.

Referenced by FXLS8471Q_Read(), FXOS8700_Init(), FXOS8700_ReadMagData(), and MAG3110_Read().

Here is the caller graph for this function:

◆ 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 380 of file sensor_fusion.c.

References MagSensor::iFIFOCount, MagSensor::iFIFOExceeded, and SensorFusionGlobals::Mag.

Referenced by initializeFusionEngine(), initSensorFusionGlobals(), and runFusion().

Here is the caller graph for this function:

◆ 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 536 of file sensor_fusion.c.

References CHX, CHY, and CHZ.

Referenced by FXLS8471Q_Read(), FXOS8700_Init(), FXOS8700_ReadMagData(), and MAG3110_Read().

Here is the caller graph for this function:

◆ 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.

Parameters

sfg	Global data structure pointer

Definition at line 322 of file sensor_fusion.c.

References MagSensor::isEnabled, SensorFusionGlobals::Mag, and processMagData().

Referenced by initSensorFusionGlobals().

Here is the call graph for this function:

Here is the caller graph for this function:

◆ initializeFusionEngine()

void initializeFusionEngine ( SensorFusionGlobals * sfg )

This function is responsible for initializing the system prior to starting the main fusion loop. This function is normally involved via the "sfg." global pointer.

Definition at line 488 of file sensor_fusion.c.

References ControlSubsystem::AccelCalPacketOn, ARM_systick_delay_ms(), ARM_systick_enable(), clearFIFOs(), CORE_SYSTICK_HZ, ControlSubsystem::DefaultQuaternionPacketType, F_3DOF_B_BASIC, F_3DOF_Y_BASIC, F_6DOF_GB_BASIC, F_6DOF_GY_KALMAN, F_9DOF_GBY_KALMAN, fInitializeAccelCalibration(), fInitializeFusion(), fInitializeMagCalibration(), HARD_FAULT, SensorFusionGlobals::iFlags, initializeSensors(), INITIALIZING, SensorFusionGlobals::loopcounter, SensorFusionGlobals::MagBuffer, SensorFusionGlobals::MagCal, NORMAL, SensorFusionGlobals::pControlSubsystem, Q3, Q3G, Q3M, Q6AG, Q6MA, Q9, ControlSubsystem::QuaternionPacketType, SENSOR_ERROR_NONE, SensorFusionGlobals::setStatus, and status.

Referenced by initSensorFusionGlobals().

Here is the call graph for this function:

Here is the caller graph for this function:

◆ initializeSensors()

int8_t initializeSensors ( SensorFusionGlobals * sfg )

Definition at line 171 of file sensor_fusion.c.

References ControlSubsystem::AccelCalPacketOn, ApplyAccelHAL(), CHX, CHZ, fInvertAccelCal(), fUpdateAccelBuffer(), PhysicalSensor::initialize, PhysicalSensor::next, SensorFusionGlobals::pControlSubsystem, SensorFusionGlobals::pSensors, SensorFusionGlobals::setStatus, sfg, SOFT_FAULT, and status.

Referenced by initializeFusionEngine().

Here is the call graph for this function:

Here is the caller graph for this function:

◆ 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 77 of file sensor_fusion.c.

Referenced by main().

Here is the call graph for this function:

Here is the caller graph for this function:

◆ installSensor()

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

installSensor is used to instantiate a physical sensor driver into the sensor fusion system. This function is normally involved 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	Parameter to control sensor sampling rate
bus_driver	ISSDK sensor bus driver (usually KSDK I2C bus)
busInfo	information required for bus power management
initialize	pointer to sensor initialization function
read	pointer to sensor read function

Definition at line 131 of file sensor_fusion.c.

References PhysicalSensor::addr, PhysicalSensor::bus_driver, PhysicalSensor::deviceInfo, registerDeviceInfo_t::deviceInstance, registerDeviceInfo_t::functionParam, registerDeviceInfo_t::idleFunction, PhysicalSensor::initialize, PhysicalSensor::next, spiSlaveSpecificParams_t::pReadPreprocessFN, SensorFusionGlobals::pSensors, spiSlaveSpecificParams_t::pTargetSlavePinID, spiSlaveSpecificParams_t::pWritePreprocessFN, PhysicalSensor::read, PhysicalSensor::schedule, PhysicalSensor::slaveParams, spiSlaveSpecificParams_t::spiCmdLen, and spiSlaveSpecificParams_t::ssActiveValue.

Referenced by initSensorFusionGlobals().

Here is the caller graph for this function:

◆ processMagData()

void processMagData ( SensorFusionGlobals * sfg )

Definition at line 223 of file sensor_fusion.c.

References ApplyGyroHAL(), ApplyMagHAL(), CHX, CHZ, MagSensor::fBs, fInvertMagCal(), fRunMagCalibration(), MagSensor::fuTPerCount, MagSensor::iBs, MagSensor::iBsFIFO, MagSensor::iFIFOCount, MagSensor::iFIFOExceeded, MagCalibration::iMagBufferReadOnly, iUpdateMagBuffer(), SensorFusionGlobals::loopcounter, SensorFusionGlobals::Mag, SensorFusionGlobals::MagBuffer, SensorFusionGlobals::MagCal, SensorFusionGlobals::setStatus, and SOFT_FAULT.

Referenced by conditionSensorReadings().

Here is the call graph for this function:

Here is the caller graph for this function:

◆ 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 59 of file sensor_fusion.c.

References SensorFusionGlobals::pStatusSubsystem, and StatusSubsystem::queue.

Referenced by initSensorFusionGlobals().

Here is the caller graph for this function:

◆ readSensors()

int8_t readSensors	(	SensorFusionGlobals *	sfg,
		uint16_t	read_loop_counter
	)

readSensors traverses the linked list of physical sensors, calling the individual read functions one by one. This function is normally involved via the "sfg." global pointer.

Parameters

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

Definition at line 293 of file sensor_fusion.c.

References HARD_FAULT, PhysicalSensor::isInitialized, PhysicalSensor::next, SensorFusionGlobals::pSensors, PhysicalSensor::read, PhysicalSensor::schedule, SENSOR_ERROR_INIT, SensorFusionGlobals::setStatus, and status.

Referenced by initSensorFusionGlobals().

Here is the caller graph for this function:

◆ 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 403 of file sensor_fusion.c.

References clearFIFOs(), fFuseSensors(), SensorFusionGlobals::Mag, and SensorFusionGlobals::MagCal.

Referenced by initSensorFusionGlobals().

Here is the call graph for this function:

Here is the caller graph for this function:

◆ setStatus()

void setStatus	(	SensorFusionGlobals *	sfg,
		fusion_status_t	status
	)

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

Definition at line 52 of file sensor_fusion.c.

References SensorFusionGlobals::pStatusSubsystem, and StatusSubsystem::set.

Referenced by initSensorFusionGlobals().

Here is the caller graph for this function:

◆ testStatus()

void testStatus ( SensorFusionGlobals * sfg )

Definition at line 71 of file sensor_fusion.c.

References SensorFusionGlobals::pStatusSubsystem, and StatusSubsystem::test.

Referenced by initSensorFusionGlobals().

Here is the caller graph for this function:

◆ 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 66 of file sensor_fusion.c.

References SensorFusionGlobals::pStatusSubsystem, and StatusSubsystem::update.

Referenced by initSensorFusionGlobals().

Here is the caller graph for this function:

◆ 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 337 of file sensor_fusion.c.

References data, HARD_FAULT, and StatusSubsystem::set.

Functions

Detailed Description

Function Documentation

◆ addToFifo()

◆ clearFIFOs()

◆ conditionSample()

◆ conditionSensorReadings()

◆ initializeFusionEngine()

◆ initializeSensors()

◆ initSensorFusionGlobals()

◆ installSensor()

◆ processMagData()

◆ queueStatus()

◆ readSensors()

◆ runFusion()

◆ setStatus()

◆ testStatus()

◆ updateStatus()

◆ zeroArray()