diff --git a/ArduCopter/GCS_Mavlink.cpp b/ArduCopter/GCS_Mavlink.cpp index de24cc0af9..b4ac3c448b 100644 --- a/ArduCopter/GCS_Mavlink.cpp +++ b/ArduCopter/GCS_Mavlink.cpp @@ -1368,3 +1368,17 @@ uint64_t GCS_MAVLINK_Copter::capabilities() const MAV_PROTOCOL_CAPABILITY_COMPASS_CALIBRATION | GCS_MAVLINK::capabilities()); } + +MAV_LANDED_STATE GCS_MAVLINK_Copter::landed_state() const +{ + if (copter.ap.land_complete) { + return MAV_LANDED_STATE_ON_GROUND; + } + if (copter.flightmode->is_landing()) { + return MAV_LANDED_STATE_LANDING; + } + if (copter.flightmode->is_taking_off()) { + return MAV_LANDED_STATE_TAKEOFF; + } + return MAV_LANDED_STATE_IN_AIR; +} diff --git a/ArduCopter/GCS_Mavlink.h b/ArduCopter/GCS_Mavlink.h index 04dc9e1060..c733c8dd42 100644 --- a/ArduCopter/GCS_Mavlink.h +++ b/ArduCopter/GCS_Mavlink.h @@ -42,6 +42,9 @@ protected: void send_nav_controller_output() const override; uint64_t capabilities() const override; + virtual MAV_VTOL_STATE vtol_state() const { return MAV_VTOL_STATE_MC; }; + virtual MAV_LANDED_STATE landed_state() const; + private: void handleMessage(mavlink_message_t * msg) override; diff --git a/ArduCopter/mode.h b/ArduCopter/mode.h index 1454bec048..91a77047af 100644 --- a/ArduCopter/mode.h +++ b/ArduCopter/mode.h @@ -87,6 +87,7 @@ protected: virtual bool has_manual_throttle() const = 0; virtual bool allows_arming(bool from_gcs) const = 0; + virtual bool is_landing() const { return false; } virtual bool landing_gear_should_be_deployed() const { return false; } virtual const char *name() const = 0; @@ -176,6 +177,7 @@ protected: void auto_takeoff_attitude_run(float target_yaw_rate); // altitude below which we do no navigation in auto takeoff static float auto_takeoff_no_nav_alt_cm; + virtual bool is_taking_off() const; // pass-through functions to reduce code churn on conversion; // these are candidates for moving into the Mode base @@ -299,8 +301,11 @@ public: void spline_start(const Location& destination, bool stopped_at_start, AC_WPNav::spline_segment_end_type seg_end_type, const Location& next_destination); void nav_guided_start(); + bool is_landing() const override; bool landing_gear_should_be_deployed() const override; + bool is_taking_off() const override; + // return true if this flight mode supports user takeoff // must_nagivate is true if mode must also control horizontal position virtual bool has_user_takeoff(bool must_navigate) const override { return false; } @@ -719,6 +724,8 @@ public: void limit_set(uint32_t timeout_ms, float alt_min_cm, float alt_max_cm, float horiz_max_cm); bool limit_check(); + bool is_taking_off() const override; + bool do_user_takeoff_start(float final_alt_above_home) override; GuidedMode mode() const { return guided_mode; } @@ -790,6 +797,7 @@ public: bool has_manual_throttle() const override { return false; } bool allows_arming(bool from_gcs) const override { return false; }; bool is_autopilot() const override { return true; } + bool is_landing() const override { return true; }; bool landing_gear_should_be_deployed() const override { return true; }; void do_not_use_GPS(); @@ -902,6 +910,7 @@ public: // this should probably not be exposed bool state_complete() { return _state_complete; } + bool is_landing() const override; bool landing_gear_should_be_deployed() const override; void restart_without_terrain(); diff --git a/ArduCopter/mode_auto.cpp b/ArduCopter/mode_auto.cpp index af727cb85d..3738c7d429 100644 --- a/ArduCopter/mode_auto.cpp +++ b/ArduCopter/mode_auto.cpp @@ -349,6 +349,24 @@ void Copter::ModeAuto::nav_guided_start() } #endif //NAV_GUIDED +bool Copter::ModeAuto::is_landing() const +{ + switch(_mode) { + case Auto_Land: + return true; + case Auto_RTL: + return copter.mode_rtl.is_landing(); + default: + return false; + } + return false; +} + +bool Copter::ModeAuto::is_taking_off() const +{ + return _mode == Auto_TakeOff; +} + bool Copter::ModeAuto::landing_gear_should_be_deployed() const { switch(_mode) { diff --git a/ArduCopter/mode_guided.cpp b/ArduCopter/mode_guided.cpp index 88913deeff..c51f9a243a 100644 --- a/ArduCopter/mode_guided.cpp +++ b/ArduCopter/mode_guided.cpp @@ -139,6 +139,11 @@ void Copter::ModeGuided::posvel_control_start() auto_yaw.set_mode(AUTO_YAW_HOLD); } +bool Copter::ModeGuided::is_taking_off() const +{ + return guided_mode == Guided_TakeOff; +} + // initialise guided mode's angle controller void Copter::ModeGuided::angle_control_start() { diff --git a/ArduCopter/mode_rtl.cpp b/ArduCopter/mode_rtl.cpp index 9b90519b47..a528b3345c 100644 --- a/ArduCopter/mode_rtl.cpp +++ b/ArduCopter/mode_rtl.cpp @@ -337,6 +337,11 @@ void Copter::ModeRTL::land_start() auto_yaw.set_mode(AUTO_YAW_HOLD); } +bool Copter::ModeRTL::is_landing() const +{ + return _state == RTL_Land; +} + bool Copter::ModeRTL::landing_gear_should_be_deployed() const { switch(_state) { diff --git a/ArduCopter/takeoff.cpp b/ArduCopter/takeoff.cpp index 29747ba004..dd13c9ecff 100644 --- a/ArduCopter/takeoff.cpp +++ b/ArduCopter/takeoff.cpp @@ -173,3 +173,17 @@ void Copter::Mode::auto_takeoff_attitude_run(float target_yaw_rate) // roll & pitch from waypoint controller, yaw rate from pilot attitude_control->input_euler_angle_roll_pitch_euler_rate_yaw(nav_roll, nav_pitch, target_yaw_rate); } + +bool Copter::Mode::is_taking_off() const +{ + if (!has_user_takeoff(false)) { + return false; + } + if (ap.land_complete) { + return false; + } + if (takeoff.running()) { + return true; + } + return false; +}