how can i test Fusion Library ?

cancel
Showing results for 
Show  only  | Search instead for 
Did you mean: 

how can i test Fusion Library ?

638 Views
navidansari
Contributor III

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

Labels (1)
0 Kudos
0 Replies