From 34058cbc21080c58967df7748999f0c78ec141d7 Mon Sep 17 00:00:00 2001 From: Daniel Agar Date: Mon, 17 Apr 2017 09:31:49 -0400 Subject: [PATCH] mavlink EXTENDED_SYS_STATUS add takeoff and landing (#7064) --- mavlink/include/mavlink/v1.0 | 2 +- mavlink/include/mavlink/v2.0 | 2 +- src/modules/mavlink/mavlink_messages.cpp | 24 ++++++++++++++++++++++-- 3 files changed, 24 insertions(+), 4 deletions(-) diff --git a/mavlink/include/mavlink/v1.0 b/mavlink/include/mavlink/v1.0 index fb57c62ee8..ba39afecc6 160000 --- a/mavlink/include/mavlink/v1.0 +++ b/mavlink/include/mavlink/v1.0 @@ -1 +1 @@ -Subproject commit fb57c62ee8b14c86f423d1549a90e0f6879584dc +Subproject commit ba39afecc6ad838f4ec22fb5487b6d0a770b3bcf diff --git a/mavlink/include/mavlink/v2.0 b/mavlink/include/mavlink/v2.0 index 10d730a446..0c358ad66f 160000 --- a/mavlink/include/mavlink/v2.0 +++ b/mavlink/include/mavlink/v2.0 @@ -1 +1 @@ -Subproject commit 10d730a446995b989e88ee15f14d9c28a3a428cc +Subproject commit 0c358ad66f4a7442177780712cdff6309a85a6b1 diff --git a/src/modules/mavlink/mavlink_messages.cpp b/src/modules/mavlink/mavlink_messages.cpp index 33b5d9d05e..c574ffb524 100644 --- a/src/modules/mavlink/mavlink_messages.cpp +++ b/src/modules/mavlink/mavlink_messages.cpp @@ -3297,6 +3297,8 @@ public: private: MavlinkOrbSubscription *_status_sub; MavlinkOrbSubscription *_landed_sub; + MavlinkOrbSubscription *_pos_sp_triplet_sub; + MavlinkOrbSubscription *_control_mode_sub; mavlink_extended_sys_state_t _msg; /* do not allow top copying this class */ @@ -3307,9 +3309,10 @@ protected: 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))), + _pos_sp_triplet_sub(_mavlink->add_orb_subscription(ORB_ID(position_setpoint_triplet))), + _control_mode_sub(_mavlink->add_orb_subscription(ORB_ID(vehicle_control_mode))), _msg() { - _msg.vtol_state = MAV_VTOL_STATE_UNDEFINED; _msg.landed_state = MAV_LANDED_STATE_UNDEFINED; } @@ -3345,8 +3348,25 @@ protected: if (land_detected.landed) { _msg.landed_state = MAV_LANDED_STATE_ON_GROUND; - } else { + } else if (!land_detected.landed) { _msg.landed_state = MAV_LANDED_STATE_IN_AIR; + + vehicle_control_mode_s control_mode = {}; + position_setpoint_triplet_s pos_sp_triplet = {}; + + if (_control_mode_sub->update(&control_mode) && _pos_sp_triplet_sub->update(&pos_sp_triplet)) { + if (control_mode.flag_control_auto_enabled && pos_sp_triplet.current.valid) { + if (pos_sp_triplet.current.type == position_setpoint_s::SETPOINT_TYPE_TAKEOFF) { + _msg.landed_state = MAV_LANDED_STATE_TAKEOFF; + + } else if (pos_sp_triplet.current.type == position_setpoint_s::SETPOINT_TYPE_LAND) { + _msg.landed_state = MAV_LANDED_STATE_LANDING; + } + } + } + + } else { + _msg.landed_state = MAV_LANDED_STATE_UNDEFINED; } }