static msg_t sensorsRead (void*arg)
{
float acc_x, acc_y, acc_z;
float gyro_x, gyro_y, gyro_z;
uint8_t temp;
/*sensors initializing*/
gyroInit (&I2CD2, IMU01A_GYRO);
accInit (&I2CD2, IMU01A_ACC);
chprintf ((BaseSequentialStream *)&SD2, "Configuration done\n\r");
while (true)
{
/*gyroscope and temperature reading*/
gyroRead (&I2CD2, IMU01A_GYRO, &gyro_x, &gyro_y, &gyro_z);
tempRead (&I2CD2, IMU01A_GYRO, &temp);
/*accelerometer reading*/
accRead (&I2CD2, IMU01A_ACC, &acc_x, &acc_y, &acc_z);
/*printing out magnetometer values*/
chprintf ((BaseSequentialStream *)&SD2, "%f gX %f gY %f gZ ", gyro_x, gyro_y, gyro_z);
/*printing out accelerometer and temperature values*/
chprintf ((BaseSequentialStream *)&SD2, "%f aX %f aY %f aZ ", acc_x, acc_y, acc_z);
chprintf ((BaseSequentialStream *)&SD2, "%d T\n\r", temp);
}
}
bool sensorsAutodetect(void)
{
// gyro must be initialised before accelerometer
bool gyroDetected = gyroInit();
#ifdef USE_ACC
if (gyroDetected) {
accInit(gyro.targetLooptime);
}
#endif
#ifdef USE_MAG
compassInit();
#endif
#ifdef USE_BARO
baroDetect(&baro.dev, barometerConfig()->baro_hardware);
#endif
#ifdef USE_RANGEFINDER
rangefinderInit();
#endif
#ifdef USE_ADC_INTERNAL
adcInternalInit();
#endif
return gyroDetected;
}
//======================== compass ===================
uint8_t compassInit(void)
{ int ret1,ret2,ret3;
ret1=gyroInit();
ret2=accInit();
ret3=magInit();
//
return ret3;
}
void ACC_Init(uint8 task_id){
ACC_TaskID= task_id;
spiInit(SPI_MASTER);
accInit();
uartInit( HAL_UART_BR_57600 );
osal_set_event( ACC_TaskID, start_acc );
}
/*********************************************************************
* @fn accelEnablerChangeCB
*
* @brief Called by the Accelerometer Profile when the Enabler Attribute
* is changed.
*
* @param none
*
* @return none
*/
static void accelEnablerChangeCB( void )
{
bStatus_t status = Accel_GetParameter( ACCEL_ENABLER, &accelEnabler );
if (status == SUCCESS){
if (accelEnabler)
{
// Initialize accelerometer
accInit();
// Setup timer for accelerometer task
osal_start_timerEx( keyfobapp_TaskID, KFD_ACCEL_READ_EVT, ACCEL_READ_PERIOD );
} else
{
// Stop the acceleromter
osal_stop_timerEx( keyfobapp_TaskID, KFD_ACCEL_READ_EVT);
}
} else
{
//??
}
}
