diff --git a/ArduCopter/Attitude.cpp b/ArduCopter/Attitude.cpp index c64563d494440e..412bcfbdc7fc34 100644 --- a/ArduCopter/Attitude.cpp +++ b/ArduCopter/Attitude.cpp @@ -69,6 +69,7 @@ void Copter::rate_controller_thread() attitude_control->set_notch_sample_rate(loop_rate_hz); motors->set_dt(1.0/loop_rate_hz); ins.set_rate_decimation(0); + hal.rcout->force_trigger_groups(false); was_using_rate_thread = false; } hal.scheduler->delay_microseconds(500); @@ -76,8 +77,9 @@ void Copter::rate_controller_thread() continue; } + // set up rate thread requirements using_rate_thread = true; - + hal.rcout->force_trigger_groups(true); ins.set_rate_decimation(rate_decimation); // wait for an IMU sample diff --git a/ArduCopter/Copter.cpp b/ArduCopter/Copter.cpp index 2615322ef5461a..86e85267cf4593 100644 --- a/ArduCopter/Copter.cpp +++ b/ArduCopter/Copter.cpp @@ -700,7 +700,7 @@ void Copter::one_hz_loop() if (hal.scheduler->thread_create(FUNCTOR_BIND_MEMBER(&Copter::rate_controller_thread, void), "attitude", - 2048, AP_HAL::Scheduler::PRIORITY_BOOST, 1)) { + 1024, AP_HAL::Scheduler::PRIORITY_RCOUT, 1)) { started_rate_thread = true; } }