diff --git a/libraries/GCS_MAVLink/GCS_Common.cpp b/libraries/GCS_MAVLink/GCS_Common.cpp index 4edd4dfaf1109f7399ed83e74344b7fc18085478..38278077ff427d53227016ee0a0bee2c448b6648 100644 --- a/libraries/GCS_MAVLink/GCS_Common.cpp +++ b/libraries/GCS_MAVLink/GCS_Common.cpp @@ -1044,13 +1044,25 @@ void GCS_MAVLINK::send_raw_imu(const AP_InertialSensor &ins, const Compass &comp void GCS_MAVLINK::send_scaled_pressure(AP_Baro &barometer) { - float pressure = barometer.get_pressure(); + uint32_t now = hal.scheduler->millis(); + float pressure = barometer.get_pressure(0); mavlink_msg_scaled_pressure_send( chan, - hal.scheduler->millis(), + now, pressure*0.01f, // hectopascal - (pressure - barometer.get_ground_pressure())*0.01f, // hectopascal - barometer.get_temperature()*100); // 0.01 degrees C + (pressure - barometer.get_ground_pressure(0))*0.01f, // hectopascal + barometer.get_temperature(0)*100); // 0.01 degrees C +#if BARO_MAX_INSTANCES > 1 + if (barometer.num_instances() > 1) { + pressure = barometer.get_pressure(1); + mavlink_msg_scaled_pressure2_send( + chan, + now, + pressure*0.01f, // hectopascal + (pressure - barometer.get_ground_pressure(1))*0.01f, // hectopascal + barometer.get_temperature(1)*100); // 0.01 degrees C + } +#endif } void GCS_MAVLINK::send_sensor_offsets(const AP_InertialSensor &ins, const Compass &compass, AP_Baro &barometer)