dark0707
/
px4-firmware
forked from Archive/PX4-Autopilot
1
0
Fork 0
Code Pull Requests Activity

sensors: prevent high frequency updating of sensor corrections 

The use of a float to integer cast was causing high frequency reporting when the float value was close to the rounding boundary.
This commit is contained in:
Paul Riseborough 2017-01-22 10:30:40 +11:00 committed by Lorenz Meier
parent 62694d92d2
commit f2f5034832
2 changed files with 8 additions and 14 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

18
src/modules/sensors/temperature_compensation.cpp
View File

@ -372,10 +372,8 @@ int TemperatureCompensation::apply_corrections_gyro(int topic_instance, math::Ve
sensor_data(axis_index) = (sensor_data(axis_index) - offsets[axis_index]) * scales[axis_index]; sensor_data(axis_index) = (sensor_data(axis_index) - offsets[axis_index]) * scales[axis_index];
} }
int8_t temperaturei = (int8_t)temperature; if (fabsf(temperature - _gyro_data.last_temperature[topic_instance]) > 1.0f) {
_gyro_data.last_temperature[topic_instance] = temperature;
if (temperaturei != _gyro_data.last_temperature[topic_instance]) {
_gyro_data.last_temperature[topic_instance] = temperaturei;
return 2; return 2;
} }
@ -403,10 +401,8 @@ int TemperatureCompensation::apply_corrections_accel(int topic_instance, math::V
sensor_data(axis_index) = (sensor_data(axis_index) - offsets[axis_index]) * scales[axis_index]; sensor_data(axis_index) = (sensor_data(axis_index) - offsets[axis_index]) * scales[axis_index];
} }
int8_t temperaturei = (int8_t)temperature; if (fabsf(temperature - _accel_data.last_temperature[topic_instance]) > 1.0f) {
_accel_data.last_temperature[topic_instance] = temperature;
if (temperaturei != _accel_data.last_temperature[topic_instance]) {
_accel_data.last_temperature[topic_instance] = temperaturei;
return 2; return 2;
} }
@ -432,10 +428,8 @@ int TemperatureCompensation::apply_corrections_baro(int topic_instance, float &s
*scales = _parameters.baro_cal_data[mapping].scale; *scales = _parameters.baro_cal_data[mapping].scale;
sensor_data = (sensor_data - *offsets) * *scales; sensor_data = (sensor_data - *offsets) * *scales;
int8_t temperaturei = (int8_t)temperature; if (fabsf(temperature - _baro_data.last_temperature[topic_instance]) > 1.0f) {
_baro_data.last_temperature[topic_instance] = temperature;
if (temperaturei != _baro_data.last_temperature[topic_instance]) {
_baro_data.last_temperature[topic_instance] = temperaturei;
return 2; return 2;
} }

4
src/modules/sensors/temperature_compensation.h
View File

@ -252,10 +252,10 @@ private:
struct PerSensorData { struct PerSensorData {
PerSensorData() PerSensorData()
{ {
for (int i = 0; i < SENSOR_COUNT_MAX; ++i) { device_mapping[i] = 255; last_temperature[i] = -100; } for (int i = 0; i < SENSOR_COUNT_MAX; ++i) { device_mapping[i] = 255; last_temperature[i] = -100.0f; }
} }
uint8_t device_mapping[SENSOR_COUNT_MAX]; /// map a topic instance to the parameters index uint8_t device_mapping[SENSOR_COUNT_MAX]; /// map a topic instance to the parameters index
int8_t last_temperature[SENSOR_COUNT_MAX]; float last_temperature[SENSOR_COUNT_MAX];
}; };
PerSensorData _gyro_data; PerSensorData _gyro_data;
PerSensorData _accel_data; PerSensorData _accel_data;