Mirror/ardupilot
5
0
Fork 0
mirror of https://github.com/ArduPilot/ardupilot synced 2025-01-03 06:28:27 -04:00
Code Issues Packages Projects Releases Wiki Activity

AP_InertialSensor: add floating point constant designators

Browse Source
This commit is contained in:
Peter Barker 2019-04-05 09:07:44 +11:00 committed by Tom Pittenger
parent 8c68ff2e91
commit ef860db558
8 changed files with 33 additions and 33 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

6
libraries/AP_InertialSensor/AP_InertialSensor_BMI055.cpp
View File

@ -256,7 +256,7 @@ void AP_InertialSensor_BMI055::read_fifo_accel(void)
return; return;
} }
// data is 12 bits with 16g range, 7.81mg/LSB // data is 12 bits with 16g range, 7.81mg/LSB
const float scale = 7.81 * 0.001 * GRAVITY_MSS / 16.0; const float scale = 7.81 * 0.001 * GRAVITY_MSS / 16.0f;
for (uint8_t i = 0; i < num_frames; i++) { for (uint8_t i = 0; i < num_frames; i++) {
const uint8_t *d = &data[i*6]; const uint8_t *d = &data[i*6];
int16_t xyz[3] { int16_t xyz[3] {
@ -277,7 +277,7 @@ void AP_InertialSensor_BMI055::read_fifo_accel(void)
if (!dev_accel->read_registers(REGA_ACCD_TEMP, (uint8_t *)&t, 1)) { if (!dev_accel->read_registers(REGA_ACCD_TEMP, (uint8_t *)&t, 1)) {
_inc_accel_error_count(accel_instance); _inc_accel_error_count(accel_instance);
} else { } else {
float temp_degc = (0.5 * t) + 23.0; float temp_degc = (0.5f * t) + 23.0f;
_publish_temperature(accel_instance, temp_degc); _publish_temperature(accel_instance, temp_degc);
} }
} }
@ -313,7 +313,7 @@ void AP_InertialSensor_BMI055::read_fifo_gyro(void)
} }
// data is 16 bits with 2000dps range // data is 16 bits with 2000dps range
const float scale = radians(2000.0) / 32767.0; const float scale = radians(2000.0f) / 32767.0f;
for (uint8_t i = 0; i < num_frames; i++) { for (uint8_t i = 0; i < num_frames; i++) {
const uint8_t *d = &data[i*6]; const uint8_t *d = &data[i*6];
int16_t xyz[3] { int16_t xyz[3] {

4
libraries/AP_InertialSensor/AP_InertialSensor_BMI088.cpp
View File

@ -333,7 +333,7 @@ void AP_InertialSensor_BMI088::read_fifo_accel(void)
} else { } else {
uint16_t temp_uint11 = (tbuf[0]<<3) | (tbuf[1]>>5); uint16_t temp_uint11 = (tbuf[0]<<3) | (tbuf[1]>>5);
int16_t temp_int11 = temp_uint11>1023?temp_uint11-2048:temp_uint11; int16_t temp_int11 = temp_uint11>1023?temp_uint11-2048:temp_uint11;
float temp_degc = temp_int11 * 0.125 + 23; float temp_degc = temp_int11 * 0.125f + 23;
_publish_temperature(accel_instance, temp_degc); _publish_temperature(accel_instance, temp_degc);
} }
} }
@ -365,7 +365,7 @@ void AP_InertialSensor_BMI088::read_fifo_gyro(void)
} }
// data is 16 bits with 2000dps range // data is 16 bits with 2000dps range
const float scale = radians(2000.0) / 32767.0; const float scale = radians(2000.0f) / 32767.0f;
for (uint8_t i = 0; i < num_frames; i++) { for (uint8_t i = 0; i < num_frames; i++) {
const uint8_t *d = &data[i*6]; const uint8_t *d = &data[i*6];
int16_t xyz[3] { int16_t xyz[3] {

18
libraries/AP_InertialSensor/AP_InertialSensor_Backend.cpp
View File

@ -62,18 +62,18 @@ void AP_InertialSensor_Backend::_update_sensor_rate(uint16_t &count, uint32_t &s
} else { } else {
count++; count++;
if (now - start_us > 1000000UL) { if (now - start_us > 1000000UL) {
float observed_rate_hz = count * 1.0e6 / (now - start_us); float observed_rate_hz = count * 1.0e6f / (now - start_us);
#if SENSOR_RATE_DEBUG #if SENSOR_RATE_DEBUG
printf("RATE: %.1f should be %.1f\n", observed_rate_hz, rate_hz); printf("RATE: %.1f should be %.1f\n", observed_rate_hz, rate_hz);
#endif #endif
float filter_constant = 0.98; float filter_constant = 0.98f;
float upper_limit = 1.05; float upper_limit = 1.05f;
float lower_limit = 0.95; float lower_limit = 0.95f;
if (AP_HAL::millis() < 30000) { if (AP_HAL::millis() < 30000) {
// converge quickly for first 30s, then more slowly // converge quickly for first 30s, then more slowly
filter_constant = 0.8; filter_constant = 0.8f;
upper_limit = 2.0; upper_limit = 2.0f;
lower_limit = 0.5; lower_limit = 0.5f;
} }
observed_rate_hz = constrain_float(observed_rate_hz, rate_hz*lower_limit, rate_hz*upper_limit); observed_rate_hz = constrain_float(observed_rate_hz, rate_hz*lower_limit, rate_hz*upper_limit);
rate_hz = filter_constant * rate_hz + (1-filter_constant) * observed_rate_hz; rate_hz = filter_constant * rate_hz + (1-filter_constant) * observed_rate_hz;
@ -158,7 +158,7 @@ void AP_InertialSensor_Backend::_notify_new_gyro_raw_sample(uint8_t instance,
difference between the two is whether sample_us is provided. difference between the two is whether sample_us is provided.
*/ */
if (sample_us != 0 && _imu._gyro_last_sample_us[instance] != 0) { if (sample_us != 0 && _imu._gyro_last_sample_us[instance] != 0) {
dt = (sample_us - _imu._gyro_last_sample_us[instance]) * 1.0e-6; dt = (sample_us - _imu._gyro_last_sample_us[instance]) * 1.0e-6f;
} else { } else {
// don't accept below 100Hz // don't accept below 100Hz
if (_imu._gyro_raw_sample_rates[instance] < 100) { if (_imu._gyro_raw_sample_rates[instance] < 100) {
@ -291,7 +291,7 @@ void AP_InertialSensor_Backend::_notify_new_accel_raw_sample(uint8_t instance,
difference between the two is whether sample_us is provided. difference between the two is whether sample_us is provided.
*/ */
if (sample_us != 0 && _imu._accel_last_sample_us[instance] != 0) { if (sample_us != 0 && _imu._accel_last_sample_us[instance] != 0) {
dt = (sample_us - _imu._accel_last_sample_us[instance]) * 1.0e-6; dt = (sample_us - _imu._accel_last_sample_us[instance]) * 1.0e-6f;
} else { } else {
// don't accept below 100Hz // don't accept below 100Hz
if (_imu._accel_raw_sample_rates[instance] < 100) { if (_imu._accel_raw_sample_rates[instance] < 100) {

20
libraries/AP_InertialSensor/AP_InertialSensor_Invensense.cpp
View File

@ -226,30 +226,30 @@ void AP_InertialSensor_Invensense::start()
*/ */
switch (_mpu_type) { switch (_mpu_type) {
case Invensense_MPU9250: case Invensense_MPU9250:
temp_zero = 21; temp_zero = 21.0f;
temp_sensitivity = 1.0/340; temp_sensitivity = 1.0f/340;
break; break;
case Invensense_MPU6000: case Invensense_MPU6000:
case Invensense_MPU6500: case Invensense_MPU6500:
temp_zero = 36.53; temp_zero = 36.53f;
temp_sensitivity = 1.0/340; temp_sensitivity = 1.0f/340;
break; break;
case Invensense_ICM20608: case Invensense_ICM20608:
case Invensense_ICM20602: case Invensense_ICM20602:
case Invensense_ICM20601: case Invensense_ICM20601:
temp_zero = 25; temp_zero = 25.0f;
temp_sensitivity = 1.0/326.8; temp_sensitivity = 1.0f/326.8f;
break; break;
case Invensense_ICM20789: case Invensense_ICM20789:
temp_zero = 25; temp_zero = 25.0f;
temp_sensitivity = 0.003; temp_sensitivity = 0.003f;
break; break;
case Invensense_ICM20689: case Invensense_ICM20689:
temp_zero = 25; temp_zero = 25.0f;
temp_sensitivity = 0.003; temp_sensitivity = 0.003f;
break; break;
} }

4
libraries/AP_InertialSensor/AP_InertialSensor_Invensense.h
View File

@ -112,8 +112,8 @@ private:
uint8_t _gyro_instance; uint8_t _gyro_instance;
uint8_t _accel_instance; uint8_t _accel_instance;
float temp_sensitivity = 1.0/340; // degC/LSB float temp_sensitivity = 1.0f/340; // degC/LSB
float temp_zero = 36.53; // degC float temp_zero = 36.53f; // degC
float _temp_filtered; float _temp_filtered;
float _accel_scale; float _accel_scale;

2
libraries/AP_InertialSensor/AP_InertialSensor_LSM9DS0.cpp
View File

@ -674,7 +674,7 @@ void AP_InertialSensor_LSM9DS0::_set_accel_scale(accel_scale scale)
_accel_scale = (((float) scale + 1.0f) * 2.0f) / 32768.0f; _accel_scale = (((float) scale + 1.0f) * 2.0f) / 32768.0f;
if (scale == A_SCALE_16G) { if (scale == A_SCALE_16G) {
/* the datasheet shows an exception for +-16G */ /* the datasheet shows an exception for +-16G */
_accel_scale = 0.000732; _accel_scale = 0.000732f;
} }
/* convert to G/LSB to (m/s/s)/LSB */ /* convert to G/LSB to (m/s/s)/LSB */
_accel_scale *= GRAVITY_MSS; _accel_scale *= GRAVITY_MSS;

2
libraries/AP_InertialSensor/AP_InertialSensor_LSM9DS1.cpp
View File

@ -403,7 +403,7 @@ void AP_InertialSensor_LSM9DS1::_set_accel_scale(accel_scale scale)
_accel_scale = (((float) scale + 1.0f) * 2.0f) / 32768.0f; _accel_scale = (((float) scale + 1.0f) * 2.0f) / 32768.0f;
if (scale == A_SCALE_16G) { if (scale == A_SCALE_16G) {
/* the datasheet shows an exception for +-16G */ /* the datasheet shows an exception for +-16G */
_accel_scale = 0.000732; _accel_scale = 0.000732f;
} }
/* convert to G/LSB to (m/s/s)/LSB */ /* convert to G/LSB to (m/s/s)/LSB */
_accel_scale *= GRAVITY_MSS; _accel_scale *= GRAVITY_MSS;

10
libraries/AP_InertialSensor/AP_InertialSensor_SITL.cpp
View File

@ -80,7 +80,7 @@ void AP_InertialSensor_SITL::generate_accel(uint8_t instance)
yAccel += accel_noise * rand_float(); yAccel += accel_noise * rand_float();
zAccel += accel_noise * rand_float(); zAccel += accel_noise * rand_float();
} else { } else {
float t = AP_HAL::micros() * 1.0e-6; float t = AP_HAL::micros() * 1.0e-6f;
xAccel += sinf(t * 2 * M_PI * vibe_freq.x) * accel_noise; xAccel += sinf(t * 2 * M_PI * vibe_freq.x) * accel_noise;
yAccel += sinf(t * 2 * M_PI * vibe_freq.y) * accel_noise; yAccel += sinf(t * 2 * M_PI * vibe_freq.y) * accel_noise;
zAccel += sinf(t * 2 * M_PI * vibe_freq.z) * accel_noise; zAccel += sinf(t * 2 * M_PI * vibe_freq.z) * accel_noise;
@ -145,7 +145,7 @@ void AP_InertialSensor_SITL::generate_gyro(uint8_t instance)
q += gyro_noise * rand_float(); q += gyro_noise * rand_float();
r += gyro_noise * rand_float(); r += gyro_noise * rand_float();
} else { } else {
float t = AP_HAL::micros() * 1.0e-6; float t = AP_HAL::micros() * 1.0e-6f;
p += sinf(t * 2 * M_PI * vibe_freq.x) * gyro_noise; p += sinf(t * 2 * M_PI * vibe_freq.x) * gyro_noise;
q += sinf(t * 2 * M_PI * vibe_freq.y) * gyro_noise; q += sinf(t * 2 * M_PI * vibe_freq.y) * gyro_noise;
r += sinf(t * 2 * M_PI * vibe_freq.z) * gyro_noise; r += sinf(t * 2 * M_PI * vibe_freq.z) * gyro_noise;
@ -155,9 +155,9 @@ void AP_InertialSensor_SITL::generate_gyro(uint8_t instance)
// add in gyro scaling // add in gyro scaling
Vector3f scale = sitl->gyro_scale; Vector3f scale = sitl->gyro_scale;
gyro.x *= (1 + scale.x*0.01); gyro.x *= (1 + scale.x*0.01f);
gyro.y *= (1 + scale.y*0.01); gyro.y *= (1 + scale.y*0.01f);
gyro.z *= (1 + scale.z*0.01); gyro.z *= (1 + scale.z*0.01f);
_rotate_and_correct_gyro(gyro_instance[instance], gyro); _rotate_and_correct_gyro(gyro_instance[instance], gyro);