diff --git a/libraries/GCS_MAVLink/GCS.h b/libraries/GCS_MAVLink/GCS.h index 4463ae1b1c..fc677dfec0 100644 --- a/libraries/GCS_MAVLink/GCS.h +++ b/libraries/GCS_MAVLink/GCS.h @@ -27,6 +27,7 @@ #include #include #include +#include #include "ap_message.h" @@ -360,6 +361,12 @@ public: void send_scaled_pressure(); void send_scaled_pressure2(); virtual void send_scaled_pressure3(); // allow sub to override this +#if AP_AIRSPEED_ENABLED + // Send per instance airspeed message + // last index is used to rotate through sensors + void send_airspeed(); + uint8_t last_airspeed_idx; +#endif void send_simstate() const; void send_sim_state() const; void send_ahrs(); diff --git a/libraries/GCS_MAVLink/GCS_Common.cpp b/libraries/GCS_MAVLink/GCS_Common.cpp index 75ae487b11..5388b399cb 100644 --- a/libraries/GCS_MAVLink/GCS_Common.cpp +++ b/libraries/GCS_MAVLink/GCS_Common.cpp @@ -1125,6 +1125,9 @@ ap_message GCS_MAVLINK::mavlink_id_to_ap_message_id(const uint32_t mavlink_id) c #endif #if AP_MAVLINK_MSG_RELAY_STATUS_ENABLED { MAVLINK_MSG_ID_RELAY_STATUS, MSG_RELAY_STATUS}, +#endif +#if AP_AIRSPEED_ENABLED + { MAVLINK_MSG_ID_AIRSPEED, MSG_AIRSPEED}, #endif }; @@ -2327,6 +2330,58 @@ void GCS_MAVLINK::send_scaled_pressure3() send_scaled_pressure_instance(2, mavlink_msg_scaled_pressure3_send); } +#if AP_AIRSPEED_ENABLED +void GCS_MAVLINK::send_airspeed() +{ + AP_Airspeed *airspeed = AP_Airspeed::get_singleton(); + if (airspeed == nullptr) { + return; + } + + for (uint8_t i=0; ienabled(index)) { + continue; + } + + float temperature_float; + int16_t temperature = INT16_MAX; + if (airspeed->get_temperature(index, temperature_float)) { + temperature = int16_t(temperature_float * 100); + } + + uint8_t flags = 0; + // Set unhealthy flag + if (!airspeed->healthy(index)) { + flags |= 1U << AIRSPEED_SENSOR_FLAGS::AIRSPEED_SENSOR_UNHEALTHY; + } + + // Set using flag if the AHRS is using this sensor + const AP_AHRS &ahrs = AP::ahrs(); + if (ahrs.using_airspeed_sensor() && (ahrs.get_active_airspeed_index() == index)) { + flags |= 1U << AIRSPEED_SENSOR_FLAGS::AIRSPEED_SENSOR_USING; + } + + // Assemble message and send + const mavlink_airspeed_t msg { + airspeed : airspeed->get_airspeed(index), + raw_press : airspeed->get_differential_pressure(index), + temperature : temperature, + id : index, + flags : flags + }; + + mavlink_msg_airspeed_send_struct(chan, &msg); + + // Only send one msg per call + last_airspeed_idx = index; + return; + } + +} +#endif + #if AP_AHRS_ENABLED void GCS_MAVLINK::send_ahrs() { @@ -6288,6 +6343,13 @@ bool GCS_MAVLINK::try_send_message(const enum ap_message id) send_scaled_pressure3(); break; +#if AP_AIRSPEED_ENABLED + case MSG_AIRSPEED: + CHECK_PAYLOAD_SIZE(AIRSPEED); + send_airspeed(); + break; +#endif + case MSG_SERVO_OUTPUT_RAW: CHECK_PAYLOAD_SIZE(SERVO_OUTPUT_RAW); send_servo_output_raw(); diff --git a/libraries/GCS_MAVLink/ap_message.h b/libraries/GCS_MAVLink/ap_message.h index 2d6761db8f..347e85d797 100644 --- a/libraries/GCS_MAVLink/ap_message.h +++ b/libraries/GCS_MAVLink/ap_message.h @@ -100,5 +100,6 @@ enum ap_message : uint8_t { #if AP_MAVLINK_MSG_HIGHRES_IMU_ENABLED MSG_HIGHRES_IMU, #endif + MSG_AIRSPEED, MSG_LAST // MSG_LAST must be the last entry in this enum };