|
|
@@ -29,50 +29,6 @@ uint16_t ACCEL_SampleRate_Table[15] = {
|
|
|
4095,
|
|
|
};
|
|
|
|
|
|
-Icm20948::Icm20948(){
|
|
|
- _activeDevice = -1;
|
|
|
- _spi_manager = NULL;
|
|
|
- _sensor_ready = false;
|
|
|
-}
|
|
|
-
|
|
|
-void Icm20948::setDevice(IcmSpiManager *spi, icm20948_Config *config) {
|
|
|
- _activeDevice = spi->addSlave(config->pinCS);
|
|
|
- _spi_manager = spi->getInstance();
|
|
|
- _sensor_ready = false;
|
|
|
-
|
|
|
- accSensor = new SensorAccel(spi, _activeDevice);
|
|
|
- gyroSensor = new SensorGyro(spi, _activeDevice);
|
|
|
- magSensor = new SensorMag(spi, _activeDevice);
|
|
|
-
|
|
|
- _is_present = this->icm20948_who_am_i();
|
|
|
- if (_is_present == false){
|
|
|
- return;
|
|
|
- }
|
|
|
-// while(!this->icm20948_who_am_i());
|
|
|
-
|
|
|
- this->accSensor->data.type = ICM20948_ACCEL; // + _activeDevice;
|
|
|
- this->gyroSensor->data.type = ICM20948_GYRO; // + _activeDevice;
|
|
|
- this->magSensor->data.type = ICM20948_MAG; // + _activeDevice;
|
|
|
-
|
|
|
- this->Reset();
|
|
|
- this->Wakeup();
|
|
|
-
|
|
|
- this->SetInterruptSource(config->int_source);
|
|
|
-
|
|
|
- this->gyroSensor->SetLowPassFilter(config->gyro.low_pass_filter);
|
|
|
- this->accSensor->SetLowPassFilter(config->accel.low_pass_filter);
|
|
|
-
|
|
|
- this->gyroSensor->SetSampleRate(config->gyro.sample_rate);
|
|
|
- this->accSensor->SetSampleRate(config->accel.sample_rate);
|
|
|
-
|
|
|
- this->accSensor->SetRange(config->accel.full_scale);
|
|
|
- this->gyroSensor->SetRange(config->gyro.full_scale);
|
|
|
-
|
|
|
- this->ak09916_init(&config->mag);
|
|
|
-
|
|
|
-
|
|
|
- _sensor_ready = true;
|
|
|
-}
|
|
|
|
|
|
Icm20948::Icm20948(IcmSpiManager *spi, icm20948_Config *config){
|
|
|
|
|
|
@@ -438,7 +394,7 @@ bool SensorAccel::Read(axisesI* localData)
|
|
|
localData->x = (int16_t)(temp[0] << 8 | temp[1]);
|
|
|
localData->y = (int16_t)(temp[2] << 8 | temp[3]);
|
|
|
localData->z = (int16_t)(temp[4] << 8 | temp[5]) + this->_scaleFactor;
|
|
|
- // Add scale factor because calibraiton function offset gravity acceleration.
|
|
|
+ // Add scale factor because calibration function offset gravity acceleration.
|
|
|
|
|
|
return true;
|
|
|
}
|