From f57c5ef4a71c8206c49cc6c7f83f6f76c8e3c593 Mon Sep 17 00:00:00 2001 From: David Jablonski Date: Sun, 1 Mar 2020 14:45:21 +0100 Subject: [PATCH] Mavlink: implemented SCALED_PRESSURE streams --- src/modules/mavlink/mavlink_messages.cpp | 159 +++++++++++++++++++++++ 1 file changed, 159 insertions(+) diff --git a/src/modules/mavlink/mavlink_messages.cpp b/src/modules/mavlink/mavlink_messages.cpp index 022e6e0d24..8a3838f614 100644 --- a/src/modules/mavlink/mavlink_messages.cpp +++ b/src/modules/mavlink/mavlink_messages.cpp @@ -85,6 +85,7 @@ #include #include #include +#include #include #include #include @@ -903,6 +904,161 @@ protected: } }; +template +class MavlinkStreamScaledPressureBase : public MavlinkStream +{ +public: + const char *get_name() const override + { + return Derived::get_name_static(); + } + + uint16_t get_id() override + { + return Derived::get_id_static(); + } + + static MavlinkStream *new_instance(Mavlink *mavlink) + { + return new Derived(mavlink); + } + +private: + MavlinkOrbSubscription *_differential_pressure_sub; + MavlinkOrbSubscription *_sensor_baro_sub; + + uint64_t _baro_timestamp; + uint64_t _dpres_timestamp; + + /* do not allow top copying this class */ + MavlinkStreamScaledPressureBase(MavlinkStreamScaledPressureBase &) = delete; + MavlinkStreamScaledPressureBase &operator = (const MavlinkStreamScaledPressureBase &) = delete; + +protected: + explicit MavlinkStreamScaledPressureBase(Mavlink *mavlink) : MavlinkStream(mavlink), + _differential_pressure_sub(_mavlink->add_orb_subscription(ORB_ID(differential_pressure))), + _sensor_baro_sub(_mavlink->add_orb_subscription(ORB_ID(sensor_baro), N)), + _baro_timestamp(0), + _dpres_timestamp(0) + {} + + bool send(const hrt_abstime t) override + { + sensor_baro_s sensor_baro{}; + differential_pressure_s differential_pressure{}; + + bool updated = false; + updated |= _sensor_baro_sub->update(&_baro_timestamp, &sensor_baro); + updated |= _differential_pressure_sub->update(&_dpres_timestamp, &differential_pressure); + + if (updated) { + typename Derived::mav_msg_type msg{}; + msg.time_boot_ms = sensor_baro.timestamp / 1000; + msg.press_abs = sensor_baro.pressure; + msg.press_diff = differential_pressure.differential_pressure_raw_pa; + msg.temperature = sensor_baro.temperature; + + + Derived::send(_mavlink->get_channel(), &msg); + + return true; + } + + return false; + } +}; + +template class MavlinkStreamScaledPressure {}; + +template <> +class MavlinkStreamScaledPressure<0> : public MavlinkStreamScaledPressureBase<0, MavlinkStreamScaledPressure<0> > +{ +public: + typedef MavlinkStreamScaledPressureBase<0, MavlinkStreamScaledPressure<0> > Base; + typedef mavlink_scaled_pressure_t mav_msg_type; + + explicit MavlinkStreamScaledPressure(Mavlink *mavlink) : Base(mavlink) {} + + static void send(mavlink_channel_t channel, const MavlinkStreamScaledPressure<0>::mav_msg_type *msg) + { + mavlink_msg_scaled_pressure_send_struct(channel, msg); + } + + static const char *get_name_static() + { + return "SCALED_PRESSURE"; + } + + static uint16_t get_id_static() + { + return MAVLINK_MSG_ID_SCALED_PRESSURE; + } + + unsigned get_size() override + { + return MAVLINK_MSG_ID_SCALED_PRESSURE_LEN + MAVLINK_NUM_NON_PAYLOAD_BYTES; + } +}; + +template <> +class MavlinkStreamScaledPressure<1> : public MavlinkStreamScaledPressureBase<1, MavlinkStreamScaledPressure<1> > +{ +public: + typedef MavlinkStreamScaledPressureBase<1, MavlinkStreamScaledPressure<1> > Base; + typedef mavlink_scaled_pressure2_t mav_msg_type; + + explicit MavlinkStreamScaledPressure(Mavlink *mavlink) : Base(mavlink) {} + + static void send(mavlink_channel_t channel, const MavlinkStreamScaledPressure<1>::mav_msg_type *msg) + { + mavlink_msg_scaled_pressure2_send_struct(channel, msg); + } + + static const char *get_name_static() + { + return "SCALED_PRESSURE2"; + } + + static uint16_t get_id_static() + { + return MAVLINK_MSG_ID_SCALED_PRESSURE2; + } + + unsigned get_size() override + { + return MAVLINK_MSG_ID_SCALED_PRESSURE2_LEN + MAVLINK_NUM_NON_PAYLOAD_BYTES; + } +}; + +template <> +class MavlinkStreamScaledPressure<2> : public MavlinkStreamScaledPressureBase<2, MavlinkStreamScaledPressure<2> > +{ +public: + typedef MavlinkStreamScaledPressureBase<2, MavlinkStreamScaledPressure<2> > Base; + typedef mavlink_scaled_pressure3_t mav_msg_type; + + explicit MavlinkStreamScaledPressure(Mavlink *mavlink) : Base(mavlink) {} + + static void send(mavlink_channel_t channel, const MavlinkStreamScaledPressure<2>::mav_msg_type *msg) + { + mavlink_msg_scaled_pressure3_send_struct(channel, msg); + } + + static const char *get_name_static() + { + return "SCALED_PRESSURE3"; + } + + static uint16_t get_id_static() + { + return MAVLINK_MSG_ID_SCALED_PRESSURE3; + } + + unsigned get_size() override + { + return MAVLINK_MSG_ID_SCALED_PRESSURE3_LEN + MAVLINK_NUM_NON_PAYLOAD_BYTES; + } +}; class MavlinkStreamScaledIMU : public MavlinkStream { @@ -5127,6 +5283,9 @@ static const StreamListItem streams_list[] = { StreamListItem(&MavlinkStreamScaledIMU::new_instance, &MavlinkStreamScaledIMU::get_name_static, &MavlinkStreamScaledIMU::get_id_static), StreamListItem(&MavlinkStreamScaledIMU2::new_instance, &MavlinkStreamScaledIMU2::get_name_static, &MavlinkStreamScaledIMU2::get_id_static), StreamListItem(&MavlinkStreamScaledIMU3::new_instance, &MavlinkStreamScaledIMU3::get_name_static, &MavlinkStreamScaledIMU3::get_id_static), + StreamListItem(&MavlinkStreamScaledPressure<0>::new_instance, &MavlinkStreamScaledPressure<0>::get_name_static, &MavlinkStreamScaledPressure<0>::get_id_static), + StreamListItem(&MavlinkStreamScaledPressure<1>::new_instance, &MavlinkStreamScaledPressure<1>::get_name_static, &MavlinkStreamScaledPressure<1>::get_id_static), + StreamListItem(&MavlinkStreamScaledPressure<2>::new_instance, &MavlinkStreamScaledPressure<2>::get_name_static, &MavlinkStreamScaledPressure<2>::get_id_static), StreamListItem(&MavlinkStreamAttitude::new_instance, &MavlinkStreamAttitude::get_name_static, &MavlinkStreamAttitude::get_id_static), StreamListItem(&MavlinkStreamAttitudeQuaternion::new_instance, &MavlinkStreamAttitudeQuaternion::get_name_static, &MavlinkStreamAttitudeQuaternion::get_id_static), StreamListItem(&MavlinkStreamVFRHUD::new_instance, &MavlinkStreamVFRHUD::get_name_static, &MavlinkStreamVFRHUD::get_id_static),