diff --git a/src/modules/mavlink/mavlink_messages.cpp b/src/modules/mavlink/mavlink_messages.cpp index deb457d332..986906e2ae 100644 --- a/src/modules/mavlink/mavlink_messages.cpp +++ b/src/modules/mavlink/mavlink_messages.cpp @@ -89,6 +89,7 @@ #include #include #include +#include #include @@ -3457,6 +3458,72 @@ protected: } }; +class MavlinkStreamMountStatus : public MavlinkStream +{ +public: + const char *get_name() const + { + return MavlinkStreamMountStatus::get_name_static(); + } + + static const char *get_name_static() + { + return "MOUNT_STATUS"; + } + + static uint16_t get_id_static() + { + return MAVLINK_MSG_ID_MOUNT_STATUS; + } + + uint16_t get_id() + { + return get_id_static(); + } + + static MavlinkStream *new_instance(Mavlink *mavlink) + { + return new MavlinkStreamMountStatus(mavlink); + } + + unsigned get_size() + { + return (_mount_status_time > 0) ? MAVLINK_MSG_ID_MOUNT_STATUS_LEN + MAVLINK_NUM_NON_PAYLOAD_BYTES : 0; + } + +private: + MavlinkOrbSubscription *_mount_status_sub; + uint64_t _mount_status_time; + + /* do not allow top copying this class */ + MavlinkStreamMountStatus(MavlinkStreamMountStatus &); + MavlinkStreamMountStatus &operator = (const MavlinkStreamMountStatus &); + +protected: + explicit MavlinkStreamMountStatus(Mavlink *mavlink) : MavlinkStream(mavlink), + _mount_status_sub(_mavlink->add_orb_subscription(ORB_ID(mount_status))), + _mount_status_time(0) + {} + + void send(const hrt_abstime t) + { + struct mount_status_s mount_status = {}; + + bool updated = _mount_status_sub->update(&_mount_status_time, &mount_status); + + if (updated) { + + mavlink_mount_status_t msg = {}; + + msg.roll = 180.0f / M_PI_F * mount_status.attitude_euler_angle[0]; + msg.pitch = 180.0f / M_PI_F * mount_status.attitude_euler_angle[1]; + msg.yaw = 180.0f / M_PI_F * mount_status.attitude_euler_angle[2]; + + mavlink_msg_mount_status_send_struct(_mavlink->get_channel(), &msg); + } + } +}; + const StreamListItem *streams_list[] = { new StreamListItem(&MavlinkStreamHeartbeat::new_instance, &MavlinkStreamHeartbeat::get_name_static, &MavlinkStreamHeartbeat::get_id_static), new StreamListItem(&MavlinkStreamStatustext::new_instance, &MavlinkStreamStatustext::get_name_static, &MavlinkStreamStatustext::get_id_static), @@ -3501,5 +3568,6 @@ const StreamListItem *streams_list[] = { new StreamListItem(&MavlinkStreamAltitude::new_instance, &MavlinkStreamAltitude::get_name_static, &MavlinkStreamAltitude::get_id_static), new StreamListItem(&MavlinkStreamADSBVehicle::new_instance, &MavlinkStreamADSBVehicle::get_name_static, &MavlinkStreamADSBVehicle::get_id_static), new StreamListItem(&MavlinkStreamWind::new_instance, &MavlinkStreamWind::get_name_static, &MavlinkStreamWind::get_id_static), + new StreamListItem(&MavlinkStreamMountStatus::new_instance, &MavlinkStreamMountStatus::get_name_static, &MavlinkStreamMountStatus::get_id_static), nullptr };