Copter: eliminate calc_home_distance_and_bearing

Calling update_simple_mode_bearing calls get-heading
rather than the other way around

This will have the advantage of not calculating home bearing
when we stop calling update_simple_mode_bearing unnecesarily

ArduCopter/ArduCopter.cpp

@@ -589,17 +589,27 @@ void Copter::update_simple_mode(void)
 // should be called after home_bearing has been updated
 void Copter::update_super_simple_bearing(bool force_update)
 {
-    // check if we are in super simple mode and at least 10m from home
-    if(force_update || (ap.simple_mode == 2 && home_distance > SUPER_SIMPLE_RADIUS)) {
+    if (!force_update) {
+        if (ap.simple_mode != 2) {
+            return;
+        }
+        if (home_distance() < SUPER_SIMPLE_RADIUS) {
+            return;
+        }
+    }
+
+    const int32_t bearing = home_bearing();
+
     // check the bearing to home has changed by at least 5 degrees
-        if (labs(super_simple_last_bearing - home_bearing) > 500) {
-            super_simple_last_bearing = home_bearing;
-            float angle_rad = radians((super_simple_last_bearing+18000)/100);
+    if (labs(super_simple_last_bearing - bearing) < 500) {
+        return;
+    }
+
+    super_simple_last_bearing = bearing;
+    const float angle_rad = radians((super_simple_last_bearing+18000)/100);
     super_simple_cos_yaw = cosf(angle_rad);
     super_simple_sin_yaw = sinf(angle_rad);
 }
-    }
-}
 
 void Copter::read_AHRS(void)
 {

ArduCopter/Copter.cpp

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

ArduCopter/Copter.h

@@ -362,11 +362,16 @@ private:
     float scaleLongDown;
 
     // The location of home in relation to the vehicle in centi-degrees
-    int32_t home_bearing;
+    int32_t home_bearing();
     // distance between vehicle and home in cm
-    int32_t home_distance;
+    uint32_t home_distance();
+
+    int32_t _home_bearing;
+    uint32_t _home_distance;
+
     LandStateType land_state = LandStateType_FlyToLocation; // records state of land (flying to location, descending)
 
+
     struct {
         PayloadPlaceStateType state = PayloadPlaceStateType_Calibrating_Hover_Start; // records state of place (descending, releasing, released, ...)
         uint32_t hover_start_timestamp; // milliseconds
@@ -814,7 +819,6 @@ private:
     void motors_output();
     void lost_vehicle_check();
     void run_nav_updates(void);
-    void calc_home_distance_and_bearing();
     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/fence.cpp

@@ -12,7 +12,7 @@ void Copter::fence_check()
     uint8_t orig_breaches = fence.get_breaches();
 
     // give fence library our current distance from home in meters
-    fence.set_home_distance(home_distance*0.01f);
+    fence.set_home_distance(home_distance()*0.01f);
 
     // check for a breach
     new_breaches = fence.check_fence(current_loc.alt/100.0f);

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_home_distance_and_bearing();
+    update_super_simple_bearing(false);
 
     // 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

@@ -5,23 +5,27 @@
 // To-Do - rename and move this function to make it's purpose more clear
 void Copter::run_nav_updates(void)
 {
-    // calculate distance and bearing for reporting and autopilot decisions
-    calc_home_distance_and_bearing();
+    update_super_simple_bearing(false);
 
     flightmode->update_navigation();
 }
 
-// calc_home_distance_and_bearing - calculate distance and bearing to home for reporting and autopilot decisions
-void Copter::calc_home_distance_and_bearing()
+uint32_t Copter::home_distance()
 {
-    // calculate home distance and bearing
     if (position_ok()) {
-        Vector3f home = pv_location_to_vector(ahrs.get_home());
-        Vector3f curr = inertial_nav.get_position();
-        home_distance = get_horizontal_distance_cm(curr, home);
-        home_bearing = get_bearing_cd(curr,home);
+        const Vector3f home = pv_location_to_vector(ahrs.get_home());
+        const Vector3f curr = inertial_nav.get_position();
+        _home_distance = get_horizontal_distance_cm(curr, home);
+    }
+    return _home_distance;
+}
 
-        // update super simple bearing (if required) because it relies on home_bearing
-        update_super_simple_bearing(false);
+int32_t Copter::home_bearing()
+{
+    if (position_ok()) {
+        const Vector3f home = pv_location_to_vector(ahrs.get_home());
+        const Vector3f curr = inertial_nav.get_position();
+        _home_bearing = get_bearing_cd(curr,home);
     }
+    return _home_bearing;
 }