diff --git a/ArduPlane/GCS_Mavlink.cpp b/ArduPlane/GCS_Mavlink.cpp index 97ae814ee7..bf2dae5a3c 100644 --- a/ArduPlane/GCS_Mavlink.cpp +++ b/ArduPlane/GCS_Mavlink.cpp @@ -2,6 +2,7 @@ #include "Plane.h" #include +#include MAV_TYPE GCS_Plane::frame_type() const { @@ -417,12 +418,58 @@ bool GCS_MAVLINK_Plane::try_send_message(enum ap_message id) case MSG_LANDING: plane.landing.send_landing_message(chan); break; + + case MSG_HYGROMETER: +#if AP_AIRSPEED_HYGROMETER_ENABLE + CHECK_PAYLOAD_SIZE(HYGROMETER_SENSOR); + send_hygrometer(); +#endif + break; + default: return GCS_MAVLINK::try_send_message(id); } return true; } +#if AP_AIRSPEED_HYGROMETER_ENABLE +void GCS_MAVLINK_Plane::send_hygrometer() +{ + if (!HAVE_PAYLOAD_SPACE(chan, HYGROMETER_SENSOR)) { + return; + } + + const auto *airspeed = AP::airspeed(); + if (airspeed == nullptr) { + return; + } + const uint32_t now = AP_HAL::millis(); + + for (uint8_t i=0; iget_hygrometer(idx, last_sample_ms, temperature, humidity)) { + continue; + } + if (now - last_sample_ms > 2000) { + // not updating, stop sending + continue; + } + if (!HAVE_PAYLOAD_SPACE(chan, HYGROMETER_SENSOR)) { + return; + } + + mavlink_msg_hygrometer_sensor_send( + chan, + idx, + int16_t(temperature*100), + uint16_t(humidity*100)); + last_hygrometer_send_idx = idx; + } +} +#endif // AP_AIRSPEED_HYGROMETER_ENABLE + /* default stream rates to 1Hz @@ -576,6 +623,9 @@ static const ap_message STREAM_EXTRA1_msgs[] = { MSG_LANDING, MSG_ESC_TELEMETRY, MSG_EFI_STATUS, +#if AP_AIRSPEED_HYGROMETER_ENABLE + MSG_HYGROMETER, +#endif }; static const ap_message STREAM_EXTRA2_msgs[] = { MSG_VFR_HUD diff --git a/ArduPlane/GCS_Mavlink.h b/ArduPlane/GCS_Mavlink.h index 8518baccf5..4b7304bb92 100644 --- a/ArduPlane/GCS_Mavlink.h +++ b/ArduPlane/GCS_Mavlink.h @@ -2,6 +2,7 @@ #include #include +#include class GCS_MAVLINK_Plane : public GCS_MAVLINK { @@ -71,6 +72,11 @@ private: uint8_t high_latency_wind_direction() const override; #endif // HAL_HIGH_LATENCY2_ENABLED +#if AP_AIRSPEED_HYGROMETER_ENABLE + void send_hygrometer(); + uint8_t last_hygrometer_send_idx; +#endif + MAV_VTOL_STATE vtol_state() const override; MAV_LANDED_STATE landed_state() const override;