GCS_MAVLink: send airspeed temp in scaled_pressure

This commit is contained in:
Iampete1 2020-07-18 22:34:22 +01:00 committed by Andrew Tridgell
parent 2aff8787c1
commit 73594cb19b
1 changed files with 10 additions and 2 deletions

View File

@ -1743,8 +1743,8 @@ void GCS_MAVLINK::send_scaled_pressure_instance(uint8_t instance, void (*send_fn
bool have_data = false;
float press_abs = 0.0f;
float temperature = 0.0f; // Absolute pressure temperature
float temperature_press_diff = 0.0f; // TODO: Differential pressure temperature
int16_t temperature = 0; // Absolute pressure temperature
int16_t temperature_press_diff = 0; // Differential pressure temperature
if (instance < barometer.num_instances()) {
press_abs = barometer.get_pressure(instance) * 0.01f;
temperature = barometer.get_temperature(instance)*100;
@ -1756,6 +1756,14 @@ void GCS_MAVLINK::send_scaled_pressure_instance(uint8_t instance, void (*send_fn
if (airspeed != nullptr &&
airspeed->enabled(instance)) {
press_diff = airspeed->get_differential_pressure(instance) * 0.01f;
float temp;
if (airspeed->get_temperature(instance,temp)) {
temperature_press_diff = temp * 100;
if (temperature_press_diff == 0) {
// don't send zero as that is the value for 'no data'
temperature_press_diff = 1;
}
}
have_data = true;
}