diff --git a/ArduCopter/ArduCopter.cpp b/ArduCopter/ArduCopter.cpp index b5b14823aa..826a6d883d 100644 --- a/ArduCopter/ArduCopter.cpp +++ b/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(); diff --git a/ArduCopter/Copter.h b/ArduCopter/Copter.h index 7d9e6ffee0..8b4a797532 100644 --- a/ArduCopter/Copter.h +++ b/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(); diff --git a/ArduCopter/inertia.cpp b/ArduCopter/inertia.cpp index ebcc5b80cf..e930b83368 100644 --- a/ArduCopter/inertia.cpp +++ b/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; diff --git a/ArduCopter/navigation.cpp b/ArduCopter/navigation.cpp index a671ad3745..a2dcaa96ed 100644 --- a/ArduCopter/navigation.cpp +++ b/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() {