diff --git a/src/modules/mavlink/mavlink_main.cpp b/src/modules/mavlink/mavlink_main.cpp index 3547341a71..e2c69d0084 100644 --- a/src/modules/mavlink/mavlink_main.cpp +++ b/src/modules/mavlink/mavlink_main.cpp @@ -1673,7 +1673,7 @@ Mavlink::task_main(int argc, char *argv[]) configure_stream("ATTITUDE_TARGET", 8.0f); configure_stream("DISTANCE_SENSOR", 0.5f); configure_stream("OPTICAL_FLOW_RAD", 5.0f); - configure_stream("VTOL_STATE", 0.5f); + configure_stream("EXTENDED_SYS_STATE", 1.0f); break; case MAVLINK_MODE_ONBOARD: @@ -1699,7 +1699,7 @@ Mavlink::task_main(int argc, char *argv[]) configure_stream("ACTUATOR_CONTROL_TARGET0", 10.0f); /* camera trigger is rate limited at the source, do not limit here */ configure_stream("CAMERA_TRIGGER", 500.0f); - configure_stream("VTOL_STATE", 2.0f); + configure_stream("EXTENDED_SYS_STATE", 2.0f); break; case MAVLINK_MODE_OSD: @@ -1714,7 +1714,7 @@ Mavlink::task_main(int argc, char *argv[]) configure_stream("SYSTEM_TIME", 1.0f); configure_stream("RC_CHANNELS", 5.0f); configure_stream("SERVO_OUTPUT_RAW_0", 1.0f); - configure_stream("VTOL_STATE", 0.5f); + configure_stream("EXTENDED_SYS_STATE", 1.0f); break; case MAVLINK_MODE_CONFIG: @@ -1740,7 +1740,7 @@ Mavlink::task_main(int argc, char *argv[]) configure_stream("HIGHRES_IMU", 50.0f); configure_stream("GPS_RAW_INT", 20.0f); configure_stream("CAMERA_TRIGGER", 500.0f); - configure_stream("VTOL_STATE", 2.0f); + configure_stream("EXTENDED_SYS_STATE", 2.0f); default: break; diff --git a/src/modules/mavlink/mavlink_messages.cpp b/src/modules/mavlink/mavlink_messages.cpp index de589a8bac..c6dde06677 100644 --- a/src/modules/mavlink/mavlink_messages.cpp +++ b/src/modules/mavlink/mavlink_messages.cpp @@ -72,6 +72,7 @@ #include #include #include +#include #include #include #include @@ -2361,76 +2362,96 @@ protected: } }; -class MavlinkStreamVtolState : public MavlinkStream +class MavlinkStreamExtendedSysState : public MavlinkStream { public: const char *get_name() const { - return MavlinkStreamVtolState::get_name_static(); + return MavlinkStreamExtendedSysState::get_name_static(); } static const char *get_name_static() { - return "VTOL_STATE"; + return "EXTENDED_SYS_STATE"; } uint8_t get_id() { - return MAVLINK_MSG_ID_VTOL_STATE; + return MAVLINK_MSG_ID_EXTENDED_SYS_STATE; } static MavlinkStream *new_instance(Mavlink *mavlink) { - return new MavlinkStreamVtolState(mavlink); + return new MavlinkStreamExtendedSysState(mavlink); } unsigned get_size() { - return MAVLINK_MSG_ID_VTOL_STATE_LEN + MAVLINK_NUM_NON_PAYLOAD_BYTES; + return MAVLINK_MSG_ID_EXTENDED_SYS_STATE_LEN + MAVLINK_NUM_NON_PAYLOAD_BYTES; } private: MavlinkOrbSubscription *_status_sub; + MavlinkOrbSubscription *_landed_sub; + mavlink_extended_sys_state_t _msg; /* do not allow top copying this class */ - MavlinkStreamVtolState(MavlinkStreamVtolState &); - MavlinkStreamVtolState &operator = (const MavlinkStreamVtolState &); + MavlinkStreamExtendedSysState(MavlinkStreamExtendedSysState &); + MavlinkStreamExtendedSysState &operator = (const MavlinkStreamExtendedSysState &); protected: - explicit MavlinkStreamVtolState(Mavlink *mavlink) : MavlinkStream(mavlink), - _status_sub(_mavlink->add_orb_subscription(ORB_ID(vehicle_status))) - {} + explicit MavlinkStreamExtendedSysState(Mavlink *mavlink) : MavlinkStream(mavlink), + _status_sub(_mavlink->add_orb_subscription(ORB_ID(vehicle_status))), + _landed_sub(_mavlink->add_orb_subscription(ORB_ID(vehicle_land_detected))), + _msg{} + { + + _msg.vtol_state = MAV_VTOL_STATE_UNDEFINED; + _msg.landed_state = MAV_LANDED_STATE_UNDEFINED; + } void send(const hrt_abstime t) { struct vehicle_status_s status; + struct vehicle_land_detected_s land_detected; + bool updated = false; if (_status_sub->update(&status)) { - mavlink_vtol_state_t msg; + updated = true; if (status.is_vtol) { if (status.is_rotary_wing) { if (status.in_transition_mode) { - msg.state = MAV_VTOL_STATE_TRANSITION_TO_FW; + _msg.vtol_state = MAV_VTOL_STATE_TRANSITION_TO_FW; } else { - msg.state = MAV_VTOL_STATE_MC; + _msg.vtol_state = MAV_VTOL_STATE_MC; } } else { if (status.in_transition_mode) { - msg.state = MAV_VTOL_STATE_TRANSITION_TO_MC; + _msg.vtol_state = MAV_VTOL_STATE_TRANSITION_TO_MC; } else { - msg.state = MAV_VTOL_STATE_FW; + _msg.vtol_state = MAV_VTOL_STATE_FW; } } - - } else { - msg.state = MAV_VTOL_STATE_UNDEFINED; } + } - _mavlink->send_message(MAVLINK_MSG_ID_VTOL_STATE, &msg); + if (_landed_sub->update(&land_detected)) { + updated = true; + + if (land_detected.landed) { + _msg.landed_state = MAV_LANDED_STATE_ON_GROUND; + + } else { + _msg.landed_state = MAV_LANDED_STATE_IN_AIR; + } + } + + if (updated) { + _mavlink->send_message(MAVLINK_MSG_ID_EXTENDED_SYS_STATE, &_msg); } } }; @@ -2470,6 +2491,6 @@ const StreamListItem *streams_list[] = { new StreamListItem(&MavlinkStreamCameraCapture::new_instance, &MavlinkStreamCameraCapture::get_name_static), new StreamListItem(&MavlinkStreamCameraTrigger::new_instance, &MavlinkStreamCameraTrigger::get_name_static), new StreamListItem(&MavlinkStreamDistanceSensor::new_instance, &MavlinkStreamDistanceSensor::get_name_static), - new StreamListItem(&MavlinkStreamVtolState::new_instance, &MavlinkStreamVtolState::get_name_static), + new StreamListItem(&MavlinkStreamExtendedSysState::new_instance, &MavlinkStreamExtendedSysState::get_name_static), nullptr };