mirror of https://github.com/ArduPilot/ardupilot
Copter: base loop time remaining on variable loop rate
This commit is contained in:
parent
8d0d4fe40a
commit
02a590024d
|
@ -238,8 +238,9 @@ void Copter::loop()
|
|||
// in multiples of the main loop tick. So if they don't run on
|
||||
// the first call to the scheduler they won't run on a later
|
||||
// call until scheduler.tick() is called again
|
||||
uint32_t time_available = (timer + MAIN_LOOP_MICROS) - micros();
|
||||
scheduler.run(time_available > MAIN_LOOP_MICROS ? 0u : time_available);
|
||||
const uint32_t loop_us = scheduler.get_loop_period_us();
|
||||
const uint32_t time_available = (timer + loop_us) - micros();
|
||||
scheduler.run(time_available > loop_us ? 0u : time_available);
|
||||
}
|
||||
|
||||
|
||||
|
|
|
@ -51,7 +51,6 @@
|
|||
// run at 400Hz on all systems
|
||||
# define MAIN_LOOP_RATE 400
|
||||
# define MAIN_LOOP_SECONDS 0.0025f
|
||||
# define MAIN_LOOP_MICROS 2500
|
||||
|
||||
#ifndef ARMING_DELAY_SEC
|
||||
# define ARMING_DELAY_SEC 2.0f
|
||||
|
|
Loading…
Reference in New Issue