diff --git a/ArduCopter/ArduCopter.cpp b/ArduCopter/ArduCopter.cpp index a37af9ec29..51a3dc5293 100644 --- a/ArduCopter/ArduCopter.cpp +++ b/ArduCopter/ArduCopter.cpp @@ -94,7 +94,8 @@ const AP_Scheduler::Task Copter::scheduler_tasks[] = { SCHED_TASK(arm_motors_check, 10, 50), SCHED_TASK(auto_disarm_check, 10, 50), SCHED_TASK(auto_trim, 10, 75), - SCHED_TASK(update_altitude, 10, 140), + SCHED_TASK(read_rangefinder, 20, 100), + SCHED_TASK(update_altitude, 10, 100), SCHED_TASK(run_nav_updates, 50, 100), SCHED_TASK(update_thr_average, 100, 90), SCHED_TASK(three_hz_loop, 3, 75), @@ -613,9 +614,6 @@ void Copter::update_altitude() // read in baro altitude read_barometer(); - // read in rangefinder altitude - read_rangefinder(); - // write altitude info to dataflash logs if (should_log(MASK_LOG_CTUN)) { Log_Write_Control_Tuning(); diff --git a/ArduCopter/sensors.cpp b/ArduCopter/sensors.cpp index 1875a511c2..07dedfabac 100644 --- a/ArduCopter/sensors.cpp +++ b/ArduCopter/sensors.cpp @@ -60,7 +60,7 @@ void Copter::read_rangefinder(void) // reset filter if we haven't used it within the last second rangefinder_state.alt_cm_filt.reset(rangefinder_state.alt_cm); } else { - rangefinder_state.alt_cm_filt.apply(rangefinder_state.alt_cm, 0.1f); + rangefinder_state.alt_cm_filt.apply(rangefinder_state.alt_cm, 0.05f); } rangefinder_state.last_healthy_ms = now; }