diff --git a/ArduCopter/GCS_Mavlink.cpp b/ArduCopter/GCS_Mavlink.cpp index 9250a79284..184a8f6015 100644 --- a/ArduCopter/GCS_Mavlink.cpp +++ b/ArduCopter/GCS_Mavlink.cpp @@ -579,6 +579,14 @@ const struct GCS_MAVLINK::stream_entries GCS_MAVLINK::all_stream_entries[] = { MAV_STREAM_TERMINATOR // must have this at end of stream_entries }; +MISSION_STATE GCS_MAVLINK_Copter::mission_state(const class AP_Mission &mission) const +{ + if (copter.mode_auto.paused()) { + return MISSION_STATE_PAUSED; + } + return GCS_MAVLINK::mission_state(mission); +} + bool GCS_MAVLINK_Copter::handle_guided_request(AP_Mission::Mission_Command &cmd) { #if MODE_AUTO_ENABLED == ENABLED diff --git a/ArduCopter/GCS_Mavlink.h b/ArduCopter/GCS_Mavlink.h index 2c0828aae4..e804974ead 100644 --- a/ArduCopter/GCS_Mavlink.h +++ b/ArduCopter/GCS_Mavlink.h @@ -55,6 +55,8 @@ protected: private: + MISSION_STATE mission_state(const class AP_Mission &mission) const override; + void handleMessage(const mavlink_message_t &msg) override; void handle_command_ack(const mavlink_message_t &msg) override; bool handle_guided_request(AP_Mission::Mission_Command &cmd) override; diff --git a/ArduCopter/mode.h b/ArduCopter/mode.h index 596e9482d7..43e9f8e4e9 100644 --- a/ArduCopter/mode.h +++ b/ArduCopter/mode.h @@ -462,6 +462,7 @@ public: // pause continue in auto mode bool pause() override; bool resume() override; + bool paused() const; bool loiter_start(); void rtl_start(); diff --git a/ArduCopter/mode_auto.cpp b/ArduCopter/mode_auto.cpp index 77be356074..0be26d04de 100644 --- a/ArduCopter/mode_auto.cpp +++ b/ArduCopter/mode_auto.cpp @@ -2251,4 +2251,9 @@ bool ModeAuto::resume() return true; } +bool ModeAuto::paused() const +{ + return wp_nav->paused(); +} + #endif