ArduCopter: report crosstrack_error

ArduCopter/GCS_Mavlink.cpp

@@ -168,7 +168,7 @@ void NOINLINE Copter::send_nav_controller_output(mavlink_channel_t chan)
         MIN(flightmode->wp_distance() * 1.0e-2f, UINT16_MAX),
         pos_control->get_alt_error() * 1.0e-2f,
         0,
-        0);
+        flightmode->crosstrack_error() * 1.0e-2f);
 }
 
 int16_t GCS_MAVLINK_Copter::vfr_hud_throttle() const

ArduCopter/mode.h

@@ -93,6 +93,7 @@ protected:
     virtual void run_autopilot() {}
     virtual uint32_t wp_distance() const { return 0; }
     virtual int32_t wp_bearing() const { return 0; }
+    virtual float crosstrack_error() const { return 0.0f;}
     virtual bool get_wp(Location_Class &loc) { return false; };
     virtual bool in_guided_mode() const { return false; }
 
@@ -280,6 +281,7 @@ protected:
 
     uint32_t wp_distance() const override;
     int32_t wp_bearing() const override;
+    float crosstrack_error() const override { return wp_nav->crosstrack_error();}
     bool get_wp(Location_Class &loc) override;
     void run_autopilot() override;
 
@@ -780,6 +782,7 @@ protected:
 
     uint32_t wp_distance() const override;
     int32_t wp_bearing() const override;
+    float crosstrack_error() const override;
 
 private:
 
@@ -957,6 +960,7 @@ protected:
 
     uint32_t wp_distance() const override;
     int32_t wp_bearing() const override;
+    float crosstrack_error() const override { return wp_nav->crosstrack_error();}
 
     void descent_start();
     void descent_run();
@@ -1018,6 +1022,7 @@ protected:
 
     uint32_t wp_distance() const override;
     int32_t wp_bearing() const override;
+    float crosstrack_error() const override { return wp_nav->crosstrack_error();}
 
 private:
 

ArduCopter/mode_guided.cpp

@@ -769,3 +769,12 @@ int32_t Copter::ModeGuided::wp_bearing() const
         return 0;
     }
 }
+
+float Copter::ModeGuided::crosstrack_error() const
+{
+    if (mode() == Guided_WP) {
+        return wp_nav->crosstrack_error();
+    } else {
+        return 0;
+    }
+}