diff --git a/libraries/GCS_MAVLink/GCS.h b/libraries/GCS_MAVLink/GCS.h index c9bfb596dd..caa3efbf0d 100644 --- a/libraries/GCS_MAVLink/GCS.h +++ b/libraries/GCS_MAVLink/GCS.h @@ -173,6 +173,7 @@ public: void send_system_time(); void send_radio_in(); void send_raw_imu(); + virtual void send_scaled_pressure3(); // allow sub to override this void send_scaled_pressure(); void send_sensor_offsets(); void send_ahrs(); diff --git a/libraries/GCS_MAVLink/GCS_Common.cpp b/libraries/GCS_MAVLink/GCS_Common.cpp index c6d1578ded..a2d933dc4c 100644 --- a/libraries/GCS_MAVLink/GCS_Common.cpp +++ b/libraries/GCS_MAVLink/GCS_Common.cpp @@ -1114,6 +1114,27 @@ void GCS_MAVLINK::send_raw_imu() mag.z); } +// sub overrides this to send on-board temperature +void GCS_MAVLINK::send_scaled_pressure3() +{ + const AP_Baro &barometer = AP::baro(); + + if (barometer.num_instances() < 3) { + return; + } + if (!HAVE_PAYLOAD_SPACE(chan, SCALED_PRESSURE3)) { + return; + } + + const float pressure = barometer.get_pressure(2); + mavlink_msg_scaled_pressure3_send( + chan, + AP_HAL::millis(), + pressure*0.01f, // hectopascal + (pressure - barometer.get_ground_pressure(2))*0.01f, // hectopascal + barometer.get_temperature(2)*100); // 0.01 degrees C +} + void GCS_MAVLINK::send_scaled_pressure() { uint32_t now = AP_HAL::millis(); @@ -1144,16 +1165,7 @@ void GCS_MAVLINK::send_scaled_pressure() barometer.get_temperature(1)*100); // 0.01 degrees C } - if (barometer.num_instances() > 2 && - HAVE_PAYLOAD_SPACE(chan, SCALED_PRESSURE3)) { - pressure = barometer.get_pressure(2); - mavlink_msg_scaled_pressure3_send( - chan, - now, - pressure*0.01f, // hectopascal - (pressure - barometer.get_ground_pressure(2))*0.01f, // hectopascal - barometer.get_temperature(2)*100); // 0.01 degrees C - } + send_scaled_pressure3(); } void GCS_MAVLINK::send_sensor_offsets()