diff --git a/ArduCopter/ArduCopter.pde b/ArduCopter/ArduCopter.pde index 4291944240..b1142cb00e 100644 --- a/ArduCopter/ArduCopter.pde +++ b/ArduCopter/ArduCopter.pde @@ -858,7 +858,6 @@ static const AP_Scheduler::Task scheduler_tasks[] PROGMEM = { { rc_loop, 4, 10 }, { throttle_loop, 8, 45 }, { update_GPS, 8, 90 }, - { update_nav_mode, 4, 40 }, { update_batt_compass, 40, 72 }, { read_aux_switches, 40, 5 }, { arm_motors_check, 40, 1 }, @@ -913,7 +912,6 @@ static const AP_Scheduler::Task scheduler_tasks[] PROGMEM = { { rc_loop, 1, 100 }, { throttle_loop, 2, 450 }, { update_GPS, 2, 900 }, - { update_nav_mode, 1, 400 }, { update_batt_compass, 10, 720 }, { read_aux_switches, 10, 50 }, { arm_motors_check, 10, 10 }, @@ -1169,6 +1167,9 @@ static void ten_hz_logging_loop() if (g.log_bitmask & MASK_LOG_RCOUT) { DataFlash.Log_Write_RCOUT(); } + if (g.log_bitmask & MASK_LOG_NTUN && mode_requires_GPS(control_mode)) { + Log_Write_Nav_Tuning(); + } } // fifty_hz_logging_loop diff --git a/ArduCopter/navigation.pde b/ArduCopter/navigation.pde index e75ccc47b1..d7258defe9 100644 --- a/ArduCopter/navigation.pde +++ b/ArduCopter/navigation.pde @@ -121,8 +121,6 @@ static bool set_nav_mode(uint8_t new_nav_mode) // called at 100hz static void update_nav_mode() { - static uint8_t log_counter; // used to slow NTUN logging - // exit immediately if not auto_armed or inertial nav position bad if (!ap.auto_armed || !inertial_nav.position_ok()) { return; @@ -139,28 +137,11 @@ static void update_nav_mode() update_circle(); break; - case NAV_LOITER: - // reset target if we are still on the ground - if (ap.land_complete) { - wp_nav.init_loiter_target(); - }else{ - // call loiter controller - wp_nav.update_loiter(); - } - break; - case NAV_WP: // call waypoint controller wp_nav.update_wpnav(); break; } - - // log to dataflash at 10hz - log_counter++; - if (log_counter >= 10 && (g.log_bitmask & MASK_LOG_NTUN) && nav_mode != NAV_NONE) { - log_counter = 0; - Log_Write_Nav_Tuning(); - } } // Keeps old data out of our calculation / logs