|
|
@@ -30,7 +30,7 @@ uint16_t ACCEL_SampleRate_Table[15] = {
|
|
|
};
|
|
|
|
|
|
|
|
|
-Icm20948::Icm20948(SpiManager *spi, icm20948_Config *config){
|
|
|
+Icm20948::Icm20948(IcmSpiManager *spi, icm20948_Config *config){
|
|
|
|
|
|
_activeDevice = spi->addSlave(config->pinCS);
|
|
|
_spi = spi;
|
|
|
@@ -81,16 +81,19 @@ void Icm20948::Calibrate(icm20948_Config *config)
|
|
|
this->accSensor->Calibrate();
|
|
|
this->gyroSensor->Calibrate();
|
|
|
|
|
|
- this->gyroSensor->SetLowPassFilter(config->gyro.low_pass_filter);
|
|
|
- this->accSensor->SetLowPassFilter(config->accel.low_pass_filter);
|
|
|
+ if (config != NULL)
|
|
|
+ {
|
|
|
+ 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->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->accSensor->SetRange(config->accel.full_scale);
|
|
|
+ this->gyroSensor->SetRange(config->gyro.full_scale);
|
|
|
|
|
|
- this->ak09916_init(&config->mag);
|
|
|
+ this->ak09916_init(&config->mag);
|
|
|
+ }
|
|
|
}
|
|
|
|
|
|
bool Icm20948::icm20948_who_am_i()
|
|
|
@@ -317,7 +320,7 @@ uint8_t Icm20948::GetIndex(void)
|
|
|
* 3) Sensor Mag
|
|
|
* ---------------------------------------------- */
|
|
|
|
|
|
-Sensor::Sensor(SpiManager *spi, int8_t activeDevice){
|
|
|
+Sensor::Sensor(IcmSpiManager *spi, int8_t activeDevice){
|
|
|
_spi = spi;
|
|
|
_activeDevice = activeDevice;
|
|
|
_scaleFactor = 1.0f;
|
|
|
@@ -336,20 +339,20 @@ uint8_t Sensor::GetDataStatus(void)
|
|
|
return new_val & 0x01;
|
|
|
}
|
|
|
|
|
|
-bool Sensor::_read_without_scale_factor(axises* data)
|
|
|
+bool Sensor::_read_without_scale_factor(axisesI* data)
|
|
|
{
|
|
|
return this->Read(data);
|
|
|
}
|
|
|
|
|
|
-SensorGyro::SensorGyro(SpiManager *spi, int8_t activeDevice):Sensor(spi, activeDevice){
|
|
|
+SensorGyro::SensorGyro(IcmSpiManager *spi, int8_t activeDevice):Sensor(spi, activeDevice){
|
|
|
|
|
|
}
|
|
|
|
|
|
-SensorAccel::SensorAccel(SpiManager *spi, int8_t activeDevice):Sensor(spi, activeDevice){
|
|
|
+SensorAccel::SensorAccel(IcmSpiManager *spi, int8_t activeDevice):Sensor(spi, activeDevice){
|
|
|
|
|
|
}
|
|
|
|
|
|
-SensorMag::SensorMag(SpiManager *spi, int8_t activeDevice):Sensor(spi, activeDevice){
|
|
|
+SensorMag::SensorMag(IcmSpiManager *spi, int8_t activeDevice):Sensor(spi, activeDevice){
|
|
|
|
|
|
}
|
|
|
|
|
|
@@ -370,26 +373,27 @@ uint8_t Sensor::GetRange(void){
|
|
|
return 0;
|
|
|
}
|
|
|
|
|
|
-bool SensorGyro::Read(axises* data)
|
|
|
+//bool SensorGyro::Read(axisesI* data)
|
|
|
+bool SensorGyro::Read(axisesI* localData)
|
|
|
{
|
|
|
|
|
|
uint8_t* temp = this->_spi->read_multiple_reg(this->_activeDevice, ub_0, B0_GYRO_XOUT_H, 6);
|
|
|
|
|
|
- data->x = (int16_t)(temp[0] << 8 | temp[1]);
|
|
|
- data->y = (int16_t)(temp[2] << 8 | temp[3]);
|
|
|
- data->z = (int16_t)(temp[4] << 8 | temp[5]);
|
|
|
+ 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]);
|
|
|
|
|
|
return true;
|
|
|
}
|
|
|
|
|
|
|
|
|
-bool SensorAccel::Read(axises* data)
|
|
|
+bool SensorAccel::Read(axisesI* localData)
|
|
|
{
|
|
|
uint8_t* temp = this->_spi->read_multiple_reg(this->_activeDevice, ub_0, B0_ACCEL_XOUT_H, 6);
|
|
|
|
|
|
- data->x = (int16_t)(temp[0] << 8 | temp[1]);
|
|
|
- data->y = (int16_t)(temp[2] << 8 | temp[3]);
|
|
|
- data->z = (int16_t)(temp[4] << 8 | temp[5]) + this->_scaleFactor;
|
|
|
+ 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.
|
|
|
|
|
|
return true;
|
|
|
@@ -398,20 +402,20 @@ bool SensorAccel::Read(axises* data)
|
|
|
/**
|
|
|
* Do not use this function to read final data. It is only for calibration of sensor.
|
|
|
*/
|
|
|
-bool SensorAccel::_read_without_scale_factor(axises* data)
|
|
|
+bool SensorAccel::_read_without_scale_factor(axisesI* localData)
|
|
|
{
|
|
|
uint8_t* temp = this->_spi->read_multiple_reg(this->_activeDevice, ub_0, B0_ACCEL_XOUT_H, 6);
|
|
|
|
|
|
- data->x = (int16_t)(temp[0] << 8 | temp[1]);
|
|
|
- data->y = (int16_t)(temp[2] << 8 | temp[3]);
|
|
|
- data->z = (int16_t)(temp[4] << 8 | temp[5]);
|
|
|
+ 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]);
|
|
|
// Add scale factor because calibraiton function offset gravity acceleration.
|
|
|
|
|
|
return true;
|
|
|
}
|
|
|
|
|
|
|
|
|
-bool SensorMag::Read(axises* data)
|
|
|
+bool SensorMag::Read(axisesI* localData)
|
|
|
{
|
|
|
uint8_t* temp;
|
|
|
uint8_t drdy; // data ready
|
|
|
@@ -425,60 +429,60 @@ bool SensorMag::Read(axises* data)
|
|
|
hofl = this->_spi->read_single_external_reg(this->_activeDevice, MAG_ST2) & 0x08;
|
|
|
if(hofl) return false;
|
|
|
|
|
|
- data->x = (int16_t)(temp[1] << 8 | temp[0]);
|
|
|
- data->y = (int16_t)(temp[3] << 8 | temp[2]);
|
|
|
- data->z = (int16_t)(temp[5] << 8 | temp[4]);
|
|
|
+ localData->x = (int16_t)(temp[1] << 8 | temp[0]);
|
|
|
+ localData->y = (int16_t)(temp[3] << 8 | temp[2]);
|
|
|
+ localData->z = (int16_t)(temp[5] << 8 | temp[4]);
|
|
|
|
|
|
return true;
|
|
|
}
|
|
|
|
|
|
-bool SensorGyro::ReadUnit(axises* data)
|
|
|
+bool SensorGyro::ReadUnit(axisesF *result)
|
|
|
{
|
|
|
- this->Read(data);
|
|
|
+ this->Read(&this->data);
|
|
|
|
|
|
- data->x /= this->_scaleFactor;
|
|
|
- data->y /= this->_scaleFactor;
|
|
|
- data->z /= this->_scaleFactor;
|
|
|
+ result->x = this->data.x / this->_scaleFactor;
|
|
|
+ result->y = this->data.y / this->_scaleFactor;
|
|
|
+ result->z = this->data.z / this->_scaleFactor;
|
|
|
|
|
|
return true;
|
|
|
}
|
|
|
|
|
|
-bool SensorAccel::ReadUnit(axises* data)
|
|
|
+bool SensorAccel::ReadUnit(axisesF *result)
|
|
|
{
|
|
|
- this->Read(data);
|
|
|
+ this->Read(&this->data);
|
|
|
|
|
|
- data->x /= this->_scaleFactor;
|
|
|
- data->y /= this->_scaleFactor;
|
|
|
- data->z /= this->_scaleFactor;
|
|
|
+ result->x = this->data.x / this->_scaleFactor;
|
|
|
+ result->y = this->data.y / this->_scaleFactor;
|
|
|
+ result->z = this->data.z / this->_scaleFactor;
|
|
|
|
|
|
return true;
|
|
|
}
|
|
|
|
|
|
-bool SensorMag::ReadUnit(axises* data)
|
|
|
+bool SensorMag::ReadUnit(axisesF *result)
|
|
|
{
|
|
|
- axises temp;
|
|
|
- bool new_data = this->Read(&temp);
|
|
|
+ bool new_data = this->Read(&this->data);
|
|
|
if(!new_data) return false;
|
|
|
|
|
|
- data->x = (float)(temp.x * 0.15);
|
|
|
- data->y = (float)(temp.y * 0.15);
|
|
|
- data->z = (float)(temp.z * 0.15);
|
|
|
+ result->x = (float)(data.x * 0.15);
|
|
|
+ result->y = (float)(data.y * 0.15);
|
|
|
+ result->z = (float)(data.z * 0.15);
|
|
|
|
|
|
return true;
|
|
|
}
|
|
|
|
|
|
|
|
|
-axises *SensorAccel::GetData()
|
|
|
+axisesI *SensorAccel::GetData()
|
|
|
{
|
|
|
return &this->data;
|
|
|
}
|
|
|
|
|
|
-axises *SensorGyro::GetData()
|
|
|
+
|
|
|
+axisesI *SensorGyro::GetData()
|
|
|
{
|
|
|
return &this->data;
|
|
|
}
|
|
|
|
|
|
-axises *SensorMag::GetData()
|
|
|
+axisesI *SensorMag::GetData()
|
|
|
{
|
|
|
return &this->data;
|
|
|
}
|
|
|
@@ -709,7 +713,7 @@ uint8_t SensorGyro::GetRange(void){
|
|
|
|
|
|
void SensorGyro::Calibrate(void)
|
|
|
{
|
|
|
- axises temp;
|
|
|
+ axisesI temp;
|
|
|
int32_t gyro_bias[3] = {0};
|
|
|
|
|
|
for(int i = 0; i < 100; i++)
|
|
|
@@ -754,7 +758,7 @@ void SensorGyro::SaveOffset(void)
|
|
|
|
|
|
void SensorAccel::Calibrate()
|
|
|
{
|
|
|
- axises temp;
|
|
|
+ axisesI temp;
|
|
|
uint8_t* temp2;
|
|
|
uint8_t* temp3;
|
|
|
uint8_t* temp4;
|