Copter: support send_extended_sys_state

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;
+}

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;

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();

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) {

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()
 {

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) {

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;
+}