Copter: calculate wp bearing and distance on demand

Move responsibility for calculating wp bearing/distance
into the FlightMode object doing the navigation

Calculating these variables was being done at 50Hz where they
were used at 10Hz max.

ArduCopter/Copter.cpp

@@ -29,10 +29,8 @@ Copter::Copter(void)
     flightmode(&flightmode_stabilize),
     control_mode(STABILIZE),
     scaleLongDown(1),
-    wp_bearing(0),
     home_bearing(0),
     home_distance(0),
-    wp_distance(0),
     simple_cos_yaw(1.0f),
     simple_sin_yaw(0.0f),
     super_simple_last_bearing(0),

ArduCopter/Copter.h

@@ -361,14 +361,10 @@ private:
     // Sometimes we need to remove the scaling for distance calcs
     float scaleLongDown;
 
-    // Location & Navigation
-    int32_t wp_bearing;
     // The location of home in relation to the vehicle in centi-degrees
     int32_t home_bearing;
     // distance between vehicle and home in cm
     int32_t home_distance;
-    // distance between vehicle and next waypoint in cm.
-    uint32_t wp_distance;
     LandStateType land_state = LandStateType_FlyToLocation; // records state of land (flying to location, descending)
 
     struct {
@@ -818,11 +814,7 @@ private:
     void motors_output();
     void lost_vehicle_check();
     void run_nav_updates(void);
-    void calc_distance_and_bearing();
-    void calc_wp_distance();
-    void calc_wp_bearing();
     void calc_home_distance_and_bearing();
-    void run_autopilot();
     Vector3f pv_location_to_vector(const Location& loc);
     float pv_alt_above_origin(float alt_above_home_cm);
     float pv_alt_above_home(float alt_above_origin_cm);

ArduCopter/FlightMode.h

@@ -50,6 +50,12 @@ protected:
     // returns a string for this flightmode, exactly 4 bytes
     virtual const char *name4() const = 0;
 
+    // navigation support functions:
+    void update_navigation();
+    virtual void run_autopilot() {}
+    virtual uint32_t wp_distance() const { return 0; }
+    virtual int32_t wp_bearing() const { return 0; }
+
     Copter &_copter;
 
     // convenience references to avoid code churn in conversion:
@@ -297,7 +303,7 @@ public:
     virtual bool allows_arming(bool from_gcs) const override { return false; };
 
     // Auto
-    AutoMode mode() { return _mode; }
+    AutoMode mode() const { return _mode; }
 
     bool loiter_start();
     void rtl_start();
@@ -321,6 +327,14 @@ protected:
     const char *name() const override { return "AUTO"; }
     const char *name4() const override { return "AUTO"; }
 
+    uint32_t wp_distance() const override {
+        return wp_nav->get_wp_distance_to_destination();
+    }
+    int32_t wp_bearing() const override {
+        return wp_nav->get_wp_bearing_to_destination();
+    }
+    void run_autopilot() override { mission.update(); }
+
 //    void get_pilot_desired_angle_rates(int16_t roll_in, int16_t pitch_in, int16_t yaw_in, float &roll_out, float &pitch_out, float &yaw_out);
 
 private:
@@ -371,6 +385,13 @@ protected:
     const char *name() const override { return "CIRCLE"; }
     const char *name4() const override { return "CIRC"; }
 
+    uint32_t wp_distance() const override {
+        return wp_nav->get_loiter_distance_to_target();
+    }
+    int32_t wp_bearing() const override {
+        return wp_nav->get_loiter_bearing_to_target();
+    }
+
 private:
 
     // Circle
@@ -406,6 +427,13 @@ protected:
     const char *name() const override { return "LOITER"; }
     const char *name4() const override { return "LOIT"; }
 
+    uint32_t wp_distance() const override {
+        return wp_nav->get_loiter_distance_to_target();
+    }
+    int32_t wp_bearing() const override {
+        return wp_nav->get_loiter_bearing_to_target();
+    }
+
 #if PRECISION_LANDING == ENABLED
     bool do_precision_loiter();
     void precision_loiter_xy();
@@ -454,7 +482,7 @@ public:
 
     bool takeoff_start(float final_alt_above_home);
 
-    GuidedMode mode() { return guided_mode; }
+    GuidedMode mode() const { return guided_mode; }
 
     void angle_control_start();
     void angle_control_run();
@@ -464,6 +492,9 @@ protected:
     const char *name() const override { return "GUIDED"; }
     const char *name4() const override { return "GUID"; }
 
+    uint32_t wp_distance() const override;
+    int32_t wp_bearing() const override;
+
 private:
 
     void pos_control_start();
@@ -551,6 +582,13 @@ protected:
     const char *name() const override { return "RTL"; }
     const char *name4() const override { return "RTL "; }
 
+    uint32_t wp_distance() const override {
+        return wp_nav->get_wp_distance_to_destination();
+    }
+    int32_t wp_bearing() const override {
+        return wp_nav->get_wp_bearing_to_destination();
+    }
+
     void descent_start();
     void descent_run();
     void land_start();
@@ -1021,6 +1059,13 @@ protected:
     const char *name() const override { return "SMARTRTL"; }
     const char *name4() const override { return "SRTL"; }
 
+    uint32_t wp_distance() const override {
+        return wp_nav->get_wp_distance_to_destination();
+    }
+    int32_t wp_bearing() const override {
+        return wp_nav->get_wp_bearing_to_destination();
+    }
+
 private:
 
     void wait_cleanup_run();

ArduCopter/GCS_Mavlink.cpp

@@ -166,8 +166,8 @@ void NOINLINE Copter::send_nav_controller_output(mavlink_channel_t chan)
         targets.x * 1.0e-2f,
         targets.y * 1.0e-2f,
         targets.z * 1.0e-2f,
-        wp_bearing * 1.0e-2f,
-        MIN(wp_distance * 1.0e-2f, UINT16_MAX),
+        flightmode->wp_bearing() * 1.0e-2f,
+        MIN(flightmode->wp_distance() * 1.0e-2f, UINT16_MAX),
         pos_control->get_alt_error() * 1.0e-2f,
         0,
         0);

ArduCopter/commands_logic.cpp

@@ -1060,9 +1060,7 @@ bool Copter::verify_wait_delay()
 
 bool Copter::verify_within_distance()
 {
-    // update distance calculation
-    calc_wp_distance();
-    if (wp_distance < (uint32_t)MAX(condition_value,0)) {
+    if (flightmode->wp_distance() < (uint32_t)MAX(condition_value,0)) {
         condition_value = 0;
         return true;
     }

ArduCopter/control_guided.cpp

@@ -774,3 +774,32 @@ bool Copter::FlightMode_GUIDED::limit_check()
     // if we got this far we must be within limits
     return false;
 }
+
+
+uint32_t Copter::FlightMode_GUIDED::wp_distance() const
+{
+    switch(mode()) {
+    case Guided_WP:
+        return wp_nav->get_wp_distance_to_destination();
+        break;
+    case Guided_PosVel:
+        return pos_control->get_distance_to_target();
+        break;
+    default:
+        return 0;
+    }
+}
+
+int32_t Copter::FlightMode_GUIDED::wp_bearing() const
+{
+    switch(mode()) {
+    case Guided_WP:
+        return wp_nav->get_wp_bearing_to_destination();
+        break;
+    case Guided_PosVel:
+        return pos_control->get_bearing_to_target();
+        break;
+    default:
+        return 0;
+    }
+}

ArduCopter/flight_mode.cpp

@@ -236,3 +236,9 @@ void Copter::notify_flight_mode() {
     AP_Notify::flags.autopilot_mode = flightmode->is_autopilot();
     notify.set_flight_mode_str(flightmode->name4());
 }
+
+void Copter::FlightMode::update_navigation()
+{
+    // run autopilot to make high level decisions about control modes
+    run_autopilot();
+}

ArduCopter/motors.cpp

@@ -177,7 +177,7 @@ bool Copter::init_arm_motors(bool arming_from_gcs)
         // Reset home position if it has already been set before (but not locked)
         set_home_to_current_location(false);
     }
-    calc_distance_and_bearing();
+    calc_home_distance_and_bearing();
 
     // Reset SmartRTL return location. If activated, SmartRTL will ultimately try to land at this point
     g2.smart_rtl.reset_path(position_ok());

ArduCopter/navigation.cpp

@@ -6,82 +6,9 @@
 void Copter::run_nav_updates(void)
 {
     // calculate distance and bearing for reporting and autopilot decisions
-    calc_distance_and_bearing();
-
-    // run autopilot to make high level decisions about control modes
-    run_autopilot();
-}
-
-// calc_distance_and_bearing - calculate distance and bearing to next waypoint and home
-void Copter::calc_distance_and_bearing()
-{
-    calc_wp_distance();
-    calc_wp_bearing();
     calc_home_distance_and_bearing();
-}
 
-// calc_wp_distance - calculate distance to next waypoint for reporting and autopilot decisions
-void Copter::calc_wp_distance()
-{
-    // get target from loiter or wpinav controller
-    switch (control_mode) {
-    case LOITER:
-    case CIRCLE:
-        wp_distance = wp_nav->get_loiter_distance_to_target();
-        break;
-
-    case AUTO:
-    case RTL:
-    case SMART_RTL:
-        wp_distance = wp_nav->get_wp_distance_to_destination();
-        break;
-
-    case GUIDED:
-        if (flightmode_guided.mode() == Guided_WP) {
-            wp_distance = wp_nav->get_wp_distance_to_destination();
-            break;
-        }
-        if (flightmode_guided.mode() == Guided_PosVel) {
-            wp_distance = pos_control->get_distance_to_target();
-            break;
-        }
-        FALLTHROUGH;
-    default:
-        wp_distance = 0;
-        break;
-    }
-}
-
-// calc_wp_bearing - calculate bearing to next waypoint for reporting and autopilot decisions
-void Copter::calc_wp_bearing()
-{
-    // get target from loiter or wpinav controller
-    switch (control_mode) {
-    case LOITER:
-    case CIRCLE:
-        wp_bearing = wp_nav->get_loiter_bearing_to_target();
-        break;
-
-    case AUTO:
-    case RTL:
-    case SMART_RTL:
-        wp_bearing = wp_nav->get_wp_bearing_to_destination();
-        break;
-
-    case GUIDED:
-        if (flightmode_guided.mode() == Guided_WP) {
-            wp_bearing = wp_nav->get_wp_bearing_to_destination();
-            break;
-        }
-        if (flightmode_guided.mode() == Guided_PosVel) {
-            wp_bearing = pos_control->get_bearing_to_target();
-            break;
-        }
-        FALLTHROUGH;
-    default:
-        wp_bearing = 0;
-        break;
-    }
+    flightmode->update_navigation();
 }
 
 // calc_home_distance_and_bearing - calculate distance and bearing to home for reporting and autopilot decisions
@@ -98,13 +25,3 @@ void Copter::calc_home_distance_and_bearing()
         update_super_simple_bearing(false);
     }
 }
-
-// run_autopilot - highest level call to process mission commands
-void Copter::run_autopilot()
-{
-    if (control_mode == AUTO) {
-        // update state of mission
-        // may call commands_process.pde's start_command and verify_command functions
-        mission.update();
-    }
-}