1 /* 2 * Copyright (c) 2015, Freescale Semiconductor, Inc. 3 * Copyright 2016-2017 NXP 4 * All rights reserved. 5 * 6 * SPDX-License-Identifier: BSD-3-Clause 7 */ 8 9 // Sensor fusion requires a fairly extensive set of data structures, which are 10 // defined in this file. The top level structure is shown near the bottom. The 11 // size of this structure (SensorFusionGlobals) varies dramatically as a function 12 // of which fusion variations have been selected in build.h. 13 14 /*! \file sensor_fusion.h 15 \brief The sensor_fusion.h file implements the top level programming interface 16 */ 17 18 #ifndef SENSOR_FUSION_TYPES_H 19 #define SENSOR_FUSION_TYPES_H 20 21 // Standard includes that everyone needs 22 #include "math.h" 23 #include "stdbool.h" 24 #include "stdio.h" 25 #include "stdint.h" 26 27 28 #include "issdk_hal.h" // Hardware Abstraction Layer board dependencies beyond those generated in board.h by PEX 29 #include "build.h" // This is where the build parameters are defined 30 #include "magnetic.h" // Magnetic calibration functions/structures 31 #include "precisionAccelerometer.h" // Accel calibration functions/structures 32 #include "orientation.h" // Functions for manipulating orientations 33 #include "register_io_spi.h" 34 35 /// @name Integer Typedefs 36 /// Typedefs to map common integer types to standard form 37 ///@{ 38 typedef unsigned char byte; 39 typedef int8_t int8; 40 typedef int16_t int16; 41 typedef int32_t int32; 42 typedef uint8_t uint8; 43 typedef uint16_t uint16; 44 typedef uint32_t uint32; 45 ///@} 46 47 /// the quaternion type to be transmitted 48 typedef enum quaternion { 49 Q3, ///< Quaternion derived from 3-axis accel (tilt) 50 Q3M, ///< Quaternion derived from 3-axis mag only (auto compass algorithm) 51 Q3G, ///< Quaternion derived from 3-axis gyro only (rotation) 52 Q6MA, ///< Quaternion derived from 3-axis accel + 3 axis mag (eCompass) 53 Q6AG, ///< Quaternion derived from 3-axis accel + 3-axis gyro (gaming) 54 Q9 ///< Quaternion derived from full 9-axis sensor fusion 55 } quaternion_type; 56 57 /// @name Vector Components 58 /// Index values for accessing vector terms 59 ///@{ 60 #define CHX 0 ///< Used to access X-channel entries in various data data structures 61 #define CHY 1 ///< Used to access Y-channel entries in various data data structures 62 #define CHZ 2 ///< Used to access Z-channel entries in various data data structures 63 ///@} 64 65 // booleans 66 #define true 1 ///< Boolean TRUE 67 #define false 0 ///< Boolean FALSE 68 69 /// @name Generic bit-field values 70 /// Generic bit-field values 71 ///@{ 72 #define B0 (1 << 0) 73 #define B1 (1 << 1) 74 #define B2 (1 << 2) 75 #define B3 (1 << 3) 76 ///@} 77 78 /// @name Math Constants 79 /// useful multiplicative conversion constants 80 ///@{ 81 #define PI 3.141592654F ///< pi 82 #define PIOVER2 1.570796327F ///< pi / 2 83 #define FPIOVER180 0.01745329251994F ///< degrees to radians conversion = pi / 180 84 #define F180OVERPI 57.2957795130823F ///< radians to degrees conversion = 180 / pi 85 #define F180OVERPISQ 3282.8063500117F ///< square of F180OVERPI 86 #define ONETHIRD 0.33333333F ///< one third 87 #define ONESIXTH 0.166666667F ///< one sixth 88 #define ONESIXTEENTH 0.0625F ///< one sixteenth 89 #define ONEOVER12 0.083333333F ///< 1 / 12 90 #define ONEOVER48 0.02083333333F ///< 1 / 48 91 #define ONEOVER120 0.0083333333F ///< 1 / 120 92 #define ONEOVER3840 0.0002604166667F ///< 1 / 3840 93 #define ONEOVERSQRT2 0.707106781F ///< 1/sqrt(2) 94 #define SQRT15OVER4 0.968245837F ///< sqrt(15)/4 95 #define GTOMSEC2 9.80665 ///< standard gravity in m/s2 96 ///@} 97 98 // Placeholder structures (redefined later, but needed now for pointer definitions) 99 struct SensorFusionGlobals; ///< Top level structure has pointers to everything else 100 struct StatusSubsystem; ///< Application-specific status subsystem 101 struct PhysicalSensor; ///< We'll have one of these for each physical sensor (FXOS8700 = 1 physical sensor) 102 struct ControlSubsystem; ///< Application-specific serial communications system 103 104 typedef enum { /// These are the state definitions for the status subsystem 105 OFF, ///< Application hasn't started 106 INITIALIZING, ///< Initializing sensors and algorithms 107 LOWPOWER, ///< Running in reduced power mode 108 NORMAL, ///< Operation is Nominal 109 RECEIVING_WIRED, ///< Receiving commands over wired interface (momentary) 110 RECEIVING_WIRELESS, ///< Receiving commands over wireless interface (momentary) 111 HARD_FAULT, ///< Non-recoverable FAULT = something went very wrong 112 SOFT_FAULT ///< Recoverable FAULT = something went wrong, but we can keep going 113 } fusion_status_t; 114 115 // declare typedefs for function prototypes that need to be installed 116 typedef int8_t (initializeSensor_t) ( 117 struct PhysicalSensor *sensor, 118 struct SensorFusionGlobals *sfg 119 ) ; 120 typedef int8_t (readSensor_t) ( 121 struct PhysicalSensor *sensor, 122 struct SensorFusionGlobals *sfg 123 ) ; 124 typedef int8_t (readSensors_t) ( 125 struct SensorFusionGlobals *sfg, 126 uint16_t read_loop_counter 127 ) ; 128 typedef int8_t (installSensor_t) ( 129 struct SensorFusionGlobals *sfg, ///< Global data structure pointer 130 struct PhysicalSensor *sensor, ///< SF Structure to store sensor configuration 131 uint16_t addr, ///< I2C address or SPI_ADDR 132 uint16_t schedule, ///< Specifies sampling interval 133 void *bus_driver, ///< I2C or SPI handle 134 registerDeviceInfo_t *busInfo, ///< information required for bus power management 135 initializeSensor_t *initialize, ///< SF Sensor Initialization Function pointer 136 readSensor_t *read ///< SF Sensor Read Function pointer 137 ); 138 #define SPI_ADDR 0x00 // Use SPI_ADDR as the address parameter to the installSensor function for SPI-based sensors. 139 // 0x00 is reserved for I2C General Call, and will therefore never occur for any sensor type 140 141 typedef void (initializeFusionEngine_t) (struct SensorFusionGlobals *sfg); 142 typedef void (runFusion_t) (struct SensorFusionGlobals *sfg); 143 typedef void (clearFIFOs_t) (struct SensorFusionGlobals *sfg); 144 typedef void (conditionSensorReadings_t) (struct SensorFusionGlobals *sfg); 145 typedef void (applyPerturbation_t) (struct SensorFusionGlobals *sfg) ; 146 typedef void (setStatus_t) (struct SensorFusionGlobals *sfg, fusion_status_t status); 147 typedef void (updateStatus_t) (struct SensorFusionGlobals *sfg); 148 typedef void (ssSetStatus_t) (struct StatusSubsystem *pStatus, fusion_status_t status); 149 typedef void (ssUpdateStatus_t) (struct StatusSubsystem *pStatus); 150 151 /// \brief An instance of PhysicalSensor structure type should be allocated for each physical sensors (combo devices = 1) 152 /// 153 /// These structures sit 'on-top-of' the pre-7.0 sensor fusion structures and give us the ability to do run 154 /// time driver installation. 155 struct PhysicalSensor { 156 registerDeviceInfo_t deviceInfo; ///< I2C device context 157 void *bus_driver; ///< should be of type (ARM_DRIVER_I2C* for I2C-based sensors, ARM_DRIVER_SPI* for SPI) 158 registerDeviceInfo_t *busInfo; ///< information required for bus power management 159 uint16_t addr; ///< I2C address if applicable 160 uint16_t isInitialized; ///< Bitfields to indicate sensor is active (use SensorBitFields from build.h) 161 spiSlaveSpecificParams_t slaveParams; ///< SPI specific parameters. Not used for I2C. 162 struct PhysicalSensor *next; ///< pointer to next sensor in this linked list 163 uint16_t schedule; ///< Parameter to control sensor sampling rate 164 initializeSensor_t *initialize; ///< pointer to function to initialize sensor using the supplied drivers 165 readSensor_t *read; ///< pointer to function to read sensor using the supplied drivers 166 }; 167 168 // Now start "standard" sensor fusion structure definitions 169 170 /// \brief The PressureSensor structure stores raw and processed measurements for an altimeter. 171 /// 172 /// The PressureSensor structure stores raw and processed measurements, as well as 173 /// metadata for a pressure sensor/altimeter. 174 struct PressureSensor 175 { 176 uint8_t iWhoAmI; ///< sensor whoami 177 bool isEnabled; ///< true if the device is sampling 178 int32_t iH; ///< most recent unaveraged height (counts) 179 int32_t iP; ///< most recent unaveraged pressure (counts) 180 float fH; ///< most recent unaveraged height (m) 181 float fT; ///< most recent unaveraged temperature (C) 182 float fmPerCount; ///< meters per count 183 float fCPerCount; ///< degrees Celsius per count 184 int16_t iT; ///< most recent unaveraged temperature (counts) 185 }; 186 187 /// \brief The AccelSensor structure stores raw and processed measurements for a 3-axis accelerometer. 188 /// 189 /// The AccelSensor structure stores raw and processed measurements, as well as metadata 190 /// for a single 3-axis accelerometer. This structure is 191 /// normally "fed" by the sensor driver and "consumed" by the fusion routines. 192 struct AccelSensor 193 { 194 uint8_t iWhoAmI; ///< sensor whoami 195 bool isEnabled; ///< true if the device is sampling 196 uint8_t iFIFOCount; ///< number of measurements read from FIFO 197 uint16_t iFIFOExceeded; ///< Number of samples received in excess of software FIFO size 198 int16_t iGsFIFO[ACCEL_FIFO_SIZE][3]; ///< FIFO measurements (counts) 199 // End of common fields which can be referenced via FifoSensor union type 200 float fGs[3]; ///< averaged measurement (g) 201 float fGc[3]; ///< averaged precision calibrated measurement (g) 202 float fgPerCount; ///< g per count 203 float fCountsPerg; ///< counts per g 204 int16_t iGs[3]; ///< averaged measurement (counts) 205 int16_t iGc[3]; ///< averaged precision calibrated measurement (counts) 206 int16_t iCountsPerg; ///< counts per g 207 }; 208 209 /// \brief The MagSensor structure stores raw and processed measurements for a 3-axis magnetic sensor. 210 /// 211 /// The MagSensor structure stores raw and processed measurements, as well as metadata 212 /// for a single 3-axis magnetometer. This structure is 213 /// normally "fed" by the sensor driver and "consumed" by the fusion routines. 214 struct MagSensor 215 { 216 uint8_t iWhoAmI; ///< sensor whoami 217 bool isEnabled; ///< true if the device is sampling 218 uint8_t iFIFOCount; ///< number of measurements read from FIFO 219 uint16_t iFIFOExceeded; ///< Number of samples received in excess of software FIFO size 220 int16_t iBsFIFO[MAG_FIFO_SIZE][3]; ///< FIFO measurements (counts) 221 // End of common fields which can be referenced via FifoSensor union type 222 float fBs[3]; ///< averaged un-calibrated measurement (uT) 223 float fBc[3]; ///< averaged calibrated measurement (uT) 224 float fuTPerCount; ///< uT per count 225 float fCountsPeruT; ///< counts per uT 226 int16_t iBs[3]; ///< averaged uncalibrated measurement (counts) 227 int16_t iBc[3]; ///< averaged calibrated measurement (counts) 228 int16_t iCountsPeruT; ///< counts per uT 229 }; 230 231 /// \brief The GyroSensor structure stores raw and processed measurements for a 3-axis gyroscope. 232 /// 233 /// The GyroSensor structure stores raw and processed measurements, as well as metadata 234 /// for a single 3-axis gyroscope. This structure is 235 /// normally "fed" by the sensor driver and "consumed" by the fusion routines. 236 struct GyroSensor 237 { 238 uint8_t iWhoAmI; ///< sensor whoami 239 bool isEnabled; ///< true if the device is sampling 240 uint8_t iFIFOCount; ///< number of measurements read from FIFO 241 uint16_t iFIFOExceeded; ///< Number of samples received in excess of software FIFO size 242 int16_t iYsFIFO[GYRO_FIFO_SIZE][3]; ///< FIFO measurements (counts) 243 // End of common fields which can be referenced via FifoSensor union type 244 float fYs[3]; ///< averaged measurement (deg/s) 245 float fDegPerSecPerCount; ///< deg/s per count 246 int16_t iCountsPerDegPerSec; ///< counts per deg/s 247 int16_t iYs[3]; ///< average measurement (counts) 248 }; 249 250 /// \brief The FifoSensor union allows us to use common pointers for Accel, Mag & Gyro logical sensor structures. 251 /// 252 /// Common elements include: iWhoAmI, isEnabled, iFIFOCount, iFIFOExceeded and the FIFO itself. 253 union FifoSensor { 254 struct GyroSensor Gyro; 255 struct MagSensor Mag; 256 struct AccelSensor Accel; 257 }; 258 259 /// The SV_1DOF_P_BASIC structure contains state information for a pressure sensor/altimeter. 260 struct SV_1DOF_P_BASIC 261 { 262 float fLPH; ///< low pass filtered height (m) 263 float fLPT; ///< low pass filtered temperature (C) 264 float fdeltat; ///< fusion time interval (s) 265 float flpf; ///< low pass filter coefficient 266 int32_t systick; ///< systick timer 267 int8_t resetflag; ///< flag to request re-initialization on next pass 268 }; 269 270 /// This is the 3DOF basic accelerometer state vector structure. 271 struct SV_3DOF_G_BASIC 272 { 273 // start: elements common to all motion state vectors 274 float fLPPhi; ///< low pass roll (deg) 275 float fLPThe; ///< low pass pitch (deg) 276 float fLPPsi; ///< low pass yaw (deg) 277 float fLPRho; ///< low pass compass (deg) 278 float fLPChi; ///< low pass tilt from vertical (deg) 279 float fLPR[3][3]; ///< low pass filtered orientation matrix 280 Quaternion fLPq; ///< low pass filtered orientation quaternion 281 float fLPRVec[3]; ///< rotation vector 282 float fOmega[3]; ///< angular velocity (deg/s) 283 int32_t systick; ///< systick timer 284 // end: elements common to all motion state vectors 285 float fR[3][3]; ///< unfiltered orientation matrix 286 Quaternion fq; ///< unfiltered orientation quaternion 287 float fdeltat; ///< fusion time interval (s) 288 float flpf; ///< low pass filter coefficient 289 int8_t resetflag; ///< flag to request re-initialization on next pass 290 }; 291 292 /// This is the 3DOF basic magnetometer state vector structure/ 293 struct SV_3DOF_B_BASIC 294 { 295 // start: elements common to all motion state vectors 296 float fLPPhi; ///< low pass roll (deg) 297 float fLPThe; ///< low pass pitch (deg) 298 float fLPPsi; ///< low pass yaw (deg) 299 float fLPRho; ///< low pass compass (deg) 300 float fLPChi; ///< low pass tilt from vertical (deg) 301 float fLPR[3][3]; ///< low pass filtered orientation matrix 302 Quaternion fLPq; ///< low pass filtered orientation quaternion 303 float fLPRVec[3]; ///< rotation vector 304 float fOmega[3]; ///< angular velocity (deg/s) 305 int32_t systick; ///< systick timer 306 // end: elements common to all motion state vectors 307 float fR[3][3]; ///< unfiltered orientation matrix 308 Quaternion fq; ///< unfiltered orientation quaternion 309 float fdeltat; ///< fusion time interval (s) 310 float flpf; ///< low pass filter coefficient 311 int8_t resetflag; ///< flag to request re-initialization on next pass 312 }; 313 314 /// SV_3DOF_Y_BASIC structure is the 3DOF basic gyroscope state vector structure. 315 struct SV_3DOF_Y_BASIC 316 { 317 // start: elements common to all motion state vectors 318 float fPhi; ///< roll (deg) 319 float fThe; ///< pitch (deg) 320 float fPsi; ///< yaw (deg) 321 float fRho; ///< compass (deg) 322 float fChi; ///< tilt from vertical (deg) 323 float fR[3][3]; ///< unfiltered orientation matrix 324 Quaternion fq; ///< unfiltered orientation quaternion 325 float fRVec[3]; ///< rotation vector 326 float fOmega[3]; ///< angular velocity (deg/s) 327 int32_t systick; ///< systick timer 328 // end: elements common to all motion state vectors 329 float fdeltat; ///< fusion filter sampling interval (s) 330 int8_t resetflag; ///< flag to request re-initialization on next pass 331 }; 332 333 /// SV_6DOF_GB_BASIC is the 6DOF basic accelerometer and magnetometer state vector structure. 334 struct SV_6DOF_GB_BASIC 335 { 336 // start: elements common to all motion state vectors 337 float fLPPhi; ///< low pass roll (deg) 338 float fLPThe; ///< low pass pitch (deg) 339 float fLPPsi; ///< low pass yaw (deg) 340 float fLPRho; ///< low pass compass (deg) 341 float fLPChi; ///< low pass tilt from vertical (deg) 342 float fLPR[3][3]; ///< low pass filtered orientation matrix 343 Quaternion fLPq; ///< low pass filtered orientation quaternion 344 float fLPRVec[3]; ///< rotation vector 345 float fOmega[3]; ///< virtual gyro angular velocity (deg/s) 346 int32_t systick; ///< systick timer 347 // end: elements common to all motion state vectors 348 float fR[3][3]; ///< unfiltered orientation matrix 349 Quaternion fq; ///< unfiltered orientation quaternion 350 float fDelta; ///< unfiltered inclination angle (deg) 351 float fLPDelta; ///< low pass filtered inclination angle (deg) 352 float fdeltat; ///< fusion time interval (s) 353 float flpf; ///< low pass filter coefficient 354 int8_t resetflag; ///< flag to request re-initialization on next pass 355 }; 356 357 /// SV_6DOF_GY_KALMAN is the 6DOF Kalman filter accelerometer and gyroscope state vector structure. 358 struct SV_6DOF_GY_KALMAN 359 { 360 // start: elements common to all motion state vectors 361 float fPhiPl; ///< roll (deg) 362 float fThePl; ///< pitch (deg) 363 float fPsiPl; ///< yaw (deg) 364 float fRhoPl; ///< compass (deg) 365 float fChiPl; ///< tilt from vertical (deg) 366 float fRPl[3][3]; ///< a posteriori orientation matrix 367 Quaternion fqPl; ///< a posteriori orientation quaternion 368 float fRVecPl[3]; ///< rotation vector 369 float fOmega[3]; ///< average angular velocity (deg/s) 370 int32_t systick; ///< systick timer; 371 // end: elements common to all motion state vectors 372 float fQw6x6[6][6]; ///< covariance matrix Qw 373 float fK6x3[6][3]; ///< kalman filter gain matrix K 374 float fQwCT6x3[6][3]; ///< Qw.C^T matrix 375 float fQv; ///< measurement noise covariance matrix leading diagonal 376 float fZErr[3]; ///< measurement error vector 377 float fqgErrPl[3]; ///< gravity vector tilt orientation quaternion error (dimensionless) 378 float fbPl[3]; ///< gyro offset (deg/s) 379 float fbErrPl[3]; ///< gyro offset error (deg/s) 380 float fAccGl[3]; ///< linear acceleration (g) in global frame 381 float fdeltat; ///< sensor fusion interval (s) 382 float fAlphaOver2; ///< PI / 180 * fdeltat / 2 383 float fAlphaSqOver4; ///< (PI / 180 * fdeltat)^2 / 4 384 float fAlphaSqQvYQwbOver12; ///< (PI / 180 * fdeltat)^2 * (QvY + Qwb) / 12 385 float fAlphaQwbOver6; ///< (PI / 180 * fdeltat) * Qwb / 6 386 float fQwbOver3; ///< Qwb / 3 387 float fMaxGyroOffsetChange; ///< maximum permissible gyro offset change per iteration (deg/s) 388 int8_t resetflag; ///< flag to request re-initialization on next pass 389 }; 390 391 /// SV_9DOF_GBY_KALMAN is the 9DOF Kalman filter accelerometer, magnetometer and gyroscope state vector structure. 392 struct SV_9DOF_GBY_KALMAN 393 { 394 // start: elements common to all motion state vectors 395 float fPhiPl; ///< roll (deg) 396 float fThePl; ///< pitch (deg) 397 float fPsiPl; ///< yaw (deg) 398 float fRhoPl; ///< compass (deg) 399 float fChiPl; ///< tilt from vertical (deg) 400 float fRPl[3][3]; ///< a posteriori orientation matrix 401 Quaternion fqPl; ///< a posteriori orientation quaternion 402 float fRVecPl[3]; ///< rotation vector 403 float fOmega[3]; ///< average angular velocity (deg/s) 404 int32_t systick; ///< systick timer; 405 // end: elements common to all motion state vectors 406 float fQw9x9[9][9]; ///< covariance matrix Qw 407 float fK9x6[9][6]; ///< kalman filter gain matrix K 408 float fQwCT9x6[9][6]; ///< Qw.C^T matrix 409 float fZErr[6]; ///< measurement error vector 410 float fQv6x1[6]; ///< measurement noise covariance matrix leading diagonal 411 float fDeltaPl; ///< a posteriori inclination angle from Kalman filter (deg) 412 float fsinDeltaPl; ///< sin(fDeltaPl) 413 float fcosDeltaPl; ///< cos(fDeltaPl) 414 float fqgErrPl[3]; ///< gravity vector tilt orientation quaternion error (dimensionless) 415 float fqmErrPl[3]; ///< geomagnetic vector tilt orientation quaternion error (dimensionless) 416 float fbPl[3]; ///< gyro offset (deg/s) 417 float fbErrPl[3]; ///< gyro offset error (deg/s) 418 float fAccGl[3]; ///< linear acceleration (g) in global frame 419 float fVelGl[3]; ///< velocity (m/s) in global frame 420 float fDisGl[3]; ///< displacement (m) in global frame 421 float fdeltat; ///< sensor fusion interval (s) 422 float fgdeltat; ///< g (m/s2) * fdeltat 423 float fAlphaOver2; ///< PI / 180 * fdeltat / 2 424 float fAlphaSqOver4; ///< (PI / 180 * fdeltat)^2 / 4 425 float fAlphaSqQvYQwbOver12; ///< (PI / 180 * fdeltat)^2 * (QvY + Qwb) / 12 426 float fAlphaQwbOver6; ///< (PI / 180 * fdeltat) * Qwb / 6 427 float fQwbOver3; ///< Qwb / 3 428 float fMaxGyroOffsetChange; ///< maximum permissible gyro offset change per iteration (deg/s) 429 int8_t iFirstAccelMagLock; ///< denotes that 9DOF orientation has locked to 6DOF eCompass 430 int8_t resetflag; ///< flag to request re-initialization on next pass 431 }; 432 433 /// Excluding SV_1DOF_P_BASIC, Any of the SV_ fusion structures above could 434 /// be cast to type SV_COMMON for dereferencing. 435 struct SV_COMMON { 436 float fPhi; ///< roll (deg) 437 float fThe; ///< pitch (deg) 438 float fPsi; ///< yaw (deg) 439 float fRho; ///< compass (deg) 440 float fChi; ///< tilt from vertical (deg) 441 float fRM[3][3]; ///< orientation matrix 442 Quaternion fq; ///< orientation quaternion 443 float fRVec[3]; ///< rotation vector 444 float fOmega[3]; ///< average angular velocity (deg/s) 445 int32_t systick; ///< systick timer; 446 }; 447 typedef struct SV_COMMON *SV_ptr; 448 449 /// \brief The top level fusion structure 450 /// 451 /// The top level fusion structure grows/shrinks based upon flag definitions 452 /// contained in build.h. These same flags will populate the .iFlags field for 453 /// run-time access. 454 typedef struct SensorFusionGlobals 455 { 456 // Subsystem Pointers 457 ///@{ 458 /// @name SubsystemPointers 459 /// The Status and Control subsystems can be used as-is, or completely 460 /// replaced with alternate implementations, as long as those implementations 461 /// provide the same interfaces defined in control.h and status.h. 462 struct ControlSubsystem *pControlSubsystem; 463 struct StatusSubsystem *pStatusSubsystem; 464 ///@} 465 ///@{ 466 /// @name MiscFields 467 uint32_t iFlags; ///< a bit-field of sensors and algorithms used 468 struct PhysicalSensor *pSensors; ///< a linked list of physical sensors 469 volatile uint8_t iPerturbation; ///< test perturbation to be applied 470 // Book-keeping variables 471 int32_t loopcounter; ///< counter incrementing each iteration of sensor fusion (typically 25Hz) 472 int32_t systick_I2C; ///< systick counter to benchmark I2C reads 473 int32_t systick_Spare; ///< systick counter for counts spare waiting for timing interrupt 474 ///@} 475 ///@{ 476 /// @name SensorRelatedStructures 477 /// These structures provide homes for sensor readings, as well as 478 /// various calibration functions. Only those needed for a specific 479 /// build are included. 480 #if F_1DOF_P_BASIC 481 struct PressureSensor Pressure; ///< pressure sensor storage 482 #endif 483 #if F_USING_ACCEL 484 struct AccelSensor Accel; ///< accelerometer storage 485 struct AccelCalibration AccelCal; ///< structures for accel calibration 486 struct AccelBuffer AccelBuffer; ///< storage for points used for calibration 487 #endif 488 #if F_USING_MAG 489 struct MagSensor Mag; ///< magnetometer storage 490 struct MagCalibration MagCal; ///< mag cal storage 491 struct MagBuffer MagBuffer; ///< mag cal constellation points 492 #endif 493 #if F_USING_GYRO 494 struct GyroSensor Gyro; ///< gyro storage 495 #endif 496 ///@} 497 ///@{ 498 /// @name FusionSpecificStructures 499 #if F_1DOF_P_BASIC 500 struct SV_1DOF_P_BASIC SV_1DOF_P_BASIC; ///< Pressure 501 #endif 502 #if F_3DOF_G_BASIC 503 struct SV_3DOF_G_BASIC SV_3DOF_G_BASIC; ///< Gravity 504 #endif 505 #if F_3DOF_B_BASIC 506 struct SV_3DOF_B_BASIC SV_3DOF_B_BASIC; ///< Magnetic 507 #endif 508 #if F_3DOF_Y_BASIC 509 struct SV_3DOF_Y_BASIC SV_3DOF_Y_BASIC; ///< Gyro 510 #endif 511 #if F_6DOF_GB_BASIC // 6DOF accel and mag eCompass: (accel + mag) 512 struct SV_6DOF_GB_BASIC SV_6DOF_GB_BASIC; ///< eCompass (Gravity + Magnetic) 513 #endif 514 #if F_6DOF_GY_KALMAN 515 struct SV_6DOF_GY_KALMAN SV_6DOF_GY_KALMAN; ///< 6-axis gravity + angular rates Kalman storage 516 #endif 517 #if F_9DOF_GBY_KALMAN 518 struct SV_9DOF_GBY_KALMAN SV_9DOF_GBY_KALMAN; ///< 9-axis storage 519 #endif 520 ///@} 521 ///@{ 522 /// @name FunctionPointers 523 /// Function pointers (the SF library external interface) 524 installSensor_t *installSensor; ///< function for installing a new sensor into t 525 initializeFusionEngine_t *initializeFusionEngine ; ///< set sensor fusion structures to initial values 526 applyPerturbation_t *applyPerturbation ; ///< apply step function for testing purposes 527 readSensors_t *readSensors; ///< read all physical sensors 528 runFusion_t *runFusion; ///< run the fusion routines 529 conditionSensorReadings_t *conditionSensorReadings; ///< preprocessing step for sensor fusion 530 clearFIFOs_t *clearFIFOs; ///< clear sensor FIFOs 531 setStatus_t *setStatus; ///< change status indicator immediately 532 setStatus_t *queueStatus; ///< queue status change for next regular interval 533 updateStatus_t *updateStatus; ///< status=next status 534 updateStatus_t *testStatus; ///< increment to next enumerated status value (test only) 535 ///@} 536 } SensorFusionGlobals; 537 538 // The following functions are defined in sensor_fusion.c 539 void initSensorFusionGlobals( 540 SensorFusionGlobals *sfg, ///< Global data structure pointer 541 struct StatusSubsystem *pStatusSubsystem, ///< Status subsystem pointer 542 struct ControlSubsystem *pControlSubsystem ///< Control subsystem pointer 543 ); 544 installSensor_t installSensor; 545 initializeFusionEngine_t initializeFusionEngine ; 546 /// conditionSensorReadings() transforms raw software FIFO readings into forms that 547 /// can be consumed by the sensor fusion engine. This include sample averaging 548 /// and (in the case of the gyro) integrations, applying hardware abstraction layers, 549 /// and calibration functions. 550 /// This function is normally involved via the "sfg." global pointer. 551 void conditionSensorReadings( 552 SensorFusionGlobals *sfg ///< Global data structure pointer 553 ); 554 void clearFIFOs( 555 SensorFusionGlobals *sfg ///< Global data structure pointer 556 ); 557 runFusion_t runFusion; 558 readSensors_t readSensors; 559 void zeroArray( 560 struct StatusSubsystem *pStatus, ///< Status subsystem pointer 561 void* data, ///< pointer to array to be zeroed 562 uint16_t size, ///< data type size = 8, 16 or 32 563 uint16_t numElements, ///< number of elements to zero out 564 uint8_t check ///< true if you would like to verify writes, false otherwise 565 ); 566 /// \brief conditionSample ensures that we never encounter the maximum negative two's complement 567 /// value for a 16-bit variable (-32768). 568 /// 569 /// conditionSample ensures that we never encounter the maximum negative two's complement 570 /// value for a 16-bit variable (-32768). This value cannot be negated, because the maximum 571 /// positive value is +32767. We need the ability to negate to gaurantee that subsequent 572 /// HAL operations can be run successfully. 573 void conditionSample( 574 int16_t sample[3] ///< 16-bit register value from triaxial sensor read 575 ); 576 577 // The following functions are defined in <hal_board_name>.c. 578 // Please note that these are board-dependent 579 580 /// \brief addToFifo is called from within sensor driver read functions 581 /// 582 /// addToFifo is called from within sensor driver read functions to transfer new readings into 583 /// the sensor structure corresponding to accel, gyro or mag. This function ensures that the software 584 /// FIFOs are not overrun. 585 /// 586 /// example usage: if (status==SENSOR_ERROR_NONE) addToFifo((FifoSensor*) &(sfg->Mag), MAG_FIFO_SIZE, sample); 587 void addToFifo( 588 union FifoSensor *sensor, ///< pointer to structure of type AccelSensor, MagSensor or GyroSensor 589 uint16_t maxFifoSize, ///< the size of the software (not hardware) FIFO 590 int16_t sample[3] ///< the sample to add 591 ); 592 /// \brief Apply the accelerometer Hardware Abstraction Layer 593 void ApplyAccelHAL( 594 struct AccelSensor *Accel ///< pointer to accelerometer logical sensor 595 ); 596 /// \brief Apply the magnetometer Hardware Abstraction Layer 597 void ApplyMagHAL( 598 struct MagSensor *Mag ///< pointer to magnetometer logical sensor 599 ); 600 /// \brief Apply the gyroscope Hardware Abstraction Layer 601 void ApplyGyroHAL( 602 struct GyroSensor *Gyro ///< pointer to gyroscope logical sensor 603 ); 604 /// \brief ApplyPerturbation is a reverse unit-step test function 605 /// 606 /// The ApplyPerturbation function applies a user-specified step function to 607 /// prior fusion results which is then "released" in the next fusion cycle. 608 /// When used in conjuction with the NXP Sensor Fusion Toolbox, this provides 609 /// a visual indication of the dynamic behavior of the library. ApplyPerturbation() 610 /// is defined in debug.c. 611 612 // The following function is defined in debug.c: 613 applyPerturbation_t ApplyPerturbation; 614 615 #include "matrix.h" // this is only down here so we can take advantage of _t style typedefs above 616 617 #endif // SENSOR_FUSION_TYPES_H 618
