how can i test Fusion Library ? hi im trying to port nxp fusion library to another MCU i did download sdk that have issdk in it till now i dont have any sensor so i want to feed fusion library some data to see if it calculate right or not this is my feedeing value function like // sfg = pointer to top level (generally global) data structure for sensor fusion
int8_t FXAS21002_Init(struct PhysicalSensor *sensor, SensorFusionGlobals *sfg)
{
sfg->Gyro.iFIFOCount=0;
sensor->isInitialized = F_USING_GYRO;
sfg->Gyro.isEnabled = true;
return SENSOR_ERROR_NONE;
}
// read FXAS21002 gyro over I2C
int8_t FXAS21002_Read(struct PhysicalSensor *sensor, SensorFusionGlobals *sfg)
{
int16_t sample[3];
// place the measurements read into the gyroscope buffer structure
sample[CHX] = 200;
sample[CHY] = 100;
sample[CHZ] = 100;
conditionSample(sample); // truncate negative values to -32767
addToFifo((union FifoSensor*) &(sfg->Gyro), GYRO_FIFO_SIZE, sample);
return SENSOR_ERROR_NONE;
}
// sfg = pointer to top level (generally global) data structure for sensor fusion
int8_t FXOS8700_Init(struct PhysicalSensor *sensor, SensorFusionGlobals *sfg)
{
sfg->Accel.iFIFOCount=0;
sensor->isInitialized = F_USING_ACCEL;
sfg->Accel.isEnabled = true;
return SENSOR_ERROR_NONE;
}
// read FXAS21002 gyro over I2C
int8_t FXOS8700_Read(struct PhysicalSensor *sensor, SensorFusionGlobals *sfg)
{
int16_t sample[3];
// place the measurements read into the gyroscope buffer structure
sample[CHX] = 300;
sample[CHY] = 100;
sample[CHZ] = 100;
conditionSample(sample); // truncate negative values to -32767
addToFifo((union FifoSensor*) &(sfg->Accel), ACCEL_FIFO_SIZE, sample);
return SENSOR_ERROR_NONE;
}
// sfg = pointer to top level (generally global) data structure for sensor fusion
int8_t MPL3115_Init(struct PhysicalSensor *sensor, SensorFusionGlobals *sfg)
{
sfg->Mag.iFIFOCount=0;
sensor->isInitialized = F_USING_MAG;
sfg->Mag.isEnabled = true;
return SENSOR_ERROR_NONE;
}
// read FXAS21002 gyro over I2C
int8_t MPL3115_Read(struct PhysicalSensor *sensor, SensorFusionGlobals *sfg)
{
int16_t sample[3];
// place the measurements read into the gyroscope buffer structure
sample[CHX] = 100;
sample[CHY] = 100;
sample[CHZ] = 200;
conditionSample(sample); // truncate negative values to -32767
addToFifo((union FifoSensor*) &(sfg->Mag), MAG_FIFO_SIZE, sample);
return SENSOR_ERROR_NONE;
}
and then in the rest of main.c // Global data structures
SensorFusionGlobals sfg; ///< This is the primary sensor fusion data structure
struct ControlSubsystem controlSubsystem; ///< used for serial communications
struct StatusSubsystem statusSubsystem; ///< provides visual (usually LED) status indicator
struct PhysicalSensor sensors[3]; ///< This implementation uses up to 3 sensors
registerDeviceInfo_t i2cBusInfo ;
typedef struct _ARM_DRIVER_I2C {
char navid; ///< Pointer to \ref ARM_I2C_GetStatus : Get I2C status.
} const ARM_DRIVER_I2C;
int main(){
ARM_DRIVER_I2C* I2Cdrv ;; // defined in the .h file
initializeControlPort(&controlSubsystem); // configure pins and ports for the control sub-system
initializeStatusSubsystem(&statusSubsystem); // configure pins and ports for the status sub-system
initSensorFusionGlobals(&sfg, &statusSubsystem, &controlSubsystem); // Initialize sensor fusion structures
sfg.installSensor(&sfg, &sensors[0], 0, 1, (void*) I2Cdrv, &i2cBusInfo, FXOS8700_Init, FXOS8700_Read);
sfg.installSensor(&sfg, &sensors[1], 0, 1, (void*) I2Cdrv, &i2cBusInfo, FXAS21002_Init, FXAS21002_Read);
sfg.installSensor(&sfg, &sensors[2], 0, 1, (void*) I2Cdrv, &i2cBusInfo, MPL3115_Init, MPL3115_Read);
sfg.initializeFusionEngine(&sfg); // This will initialize sensors and magnetic calibration
int i = 0 ;
while(1){
sfg.readSensors(&sfg, 1); // Reads sensors, applies HAL and does averaging (if applicable)
sfg.conditionSensorReadings(&sfg); // magCal is run as part of this
sfg.runFusion(&sfg); // Run the actual fusion algorithms
sfg.applyPerturbation(&sfg); // apply debug perturbation (testing only)
sfg.loopcounter++; // The loop counter is used to "serialize" mag cal operations
}
} u see data that i feed is in sample variable in each sensor read function when i watch sfg. SV_9DOF_GBY_KALMAN all of the parameter is zero what valuse should i feed in accel and mag and gyro to see if it work right . do u have any sample to give me so i can try with it? my build.h #define F_USING_ACCEL 0x0001 ///< nominally 0x0001 if an accelerometer is to be used, 0x0000 otherwise
#define F_USING_MAG 0x0002 ///< nominally 0x0002 if an magnetometer is to be used, 0x0000 otherwise
#define F_USING_GYRO 0x0004 ///< nominally 0x0004 if a gyro is to be used, 0x0000 otherwise
#define F_USING_PRESSURE 0x0000 ///< nominally 0x0008 if altimeter is to be used, 0x0000 otherwise
#define F_USING_TEMPERATURE 0x0000 ///< nominally 0x0010 if temp sensor is to be used, 0x0000 otherwise
#define F_ALL_SENSORS 0x001F ///< refers to all applicable sensor types for the given physical unit
#define F_1DOF_P_BASIC 0x0000 ///< 1DOF pressure (altitude) and temperature algorithm selector - 0x0100 to include, 0x0000 otherwise
#define F_3DOF_G_BASIC 0x0200 ///< 3DOF accel tilt (accel) algorithm selector - 0x0200 to include, 0x0000 otherwise
#define F_3DOF_B_BASIC 0x0400 ///< 3DOF mag eCompass (vehicle/mag) algorithm selector - 0x0400 to include, 0x0000 otherwise
#define F_3DOF_Y_BASIC 0x0800 ///< 3DOF gyro integration algorithm selector - 0x0800 to include, 0x0000 otherwise
#define F_6DOF_GB_BASIC 0x1000 ///< 6DOF accel and mag eCompass algorithm selector - 0x1000 to include, 0x0000 otherwise
#define F_6DOF_GY_KALMAN 0x2000 ///< 6DOF accel and gyro (Kalman) algorithm selector - 0x2000 to include, 0x0000 otherwise
#define F_9DOF_GBY_KALMAN 0x4000 ///< 9DOF accel, mag and gyro algorithm selector - 0x4000 to include, 0x0000 otherwise
thanks allot SensorFusion
查看全文
