Copter: update current_loc at 400hz

ArduCopter/ArduCopter.cpp

@@ -300,9 +300,6 @@ void Copter::rc_loop()
 // ---------------------------
 void Copter::throttle_loop()
 {
-    // get altitude and climb rate from inertial lib
-    read_inertial_altitude();
-
     // update throttle_low_comp value (controls priority of throttle vs attitude control)
     update_throttle_thr_mix();
 

ArduCopter/Copter.h

@@ -877,7 +877,6 @@ private:
     bool heli_stabilize_init(bool ignore_checks);
     void heli_stabilize_run();
     void read_inertia();
-    void read_inertial_altitude();
     bool land_complete_maybe();
     void update_land_and_crash_detectors();
     void update_land_detector();
@@ -906,7 +905,6 @@ private:
     void motors_output();
     void lost_vehicle_check();
     void run_nav_updates(void);
-    void calc_position();
     void calc_distance_and_bearing();
     void calc_wp_distance();
     void calc_wp_bearing();

ArduCopter/inertia.cpp

@@ -7,11 +7,11 @@ void Copter::read_inertia()
 {
     // inertial altitude estimates
     inertial_nav.update(G_Dt);
-}
 
-// read_inertial_altitude - pull altitude and climb rate from inertial nav library
-void Copter::read_inertial_altitude()
-{
+    // pull position from interial nav library
+    current_loc.lng = inertial_nav.get_longitude();
+    current_loc.lat = inertial_nav.get_latitude();
+
     // exit immediately if we do not have an altitude estimate
     if (!inertial_nav.get_filter_status().flags.vert_pos) {
         return;

ArduCopter/navigation.cpp

@@ -7,9 +7,6 @@
 // To-Do - rename and move this function to make it's purpose more clear
 void Copter::run_nav_updates(void)
 {
-    // fetch position from inertial navigation
-    calc_position();
-
     // calculate distance and bearing for reporting and autopilot decisions
     calc_distance_and_bearing();
 
@@ -17,14 +14,6 @@ void Copter::run_nav_updates(void)
     run_autopilot();
 }
 
-// calc_position - get lat and lon positions from inertial nav library
-void Copter::calc_position()
-{
-    // pull position from interial nav library
-    current_loc.lng = inertial_nav.get_longitude();
-    current_loc.lat = inertial_nav.get_latitude();
-}
-
 // calc_distance_and_bearing - calculate distance and bearing to next waypoint and home
 void Copter::calc_distance_and_bearing()
 {