Mirror
/
ardupilot
mirror of https://github.com/ArduPilot/ardupilot
5
0
Fork 0
Code Issues Packages Projects Releases Wiki Activity

AP_Scheduler: fixed loop time available

This commit is contained in:
Andrew Tridgell 2018-02-10 13:09:09 +11:00
parent 24d6493453
commit e0c9d9b592
2 changed files with 11 additions and 13 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

17
libraries/AP_Scheduler/AP_Scheduler.cpp
View File

@ -85,7 +85,6 @@ void AP_Scheduler::init(const AP_Scheduler::Task *tasks, uint8_t num_tasks, uint
// setup initial performance counters
perf_info.set_loop_rate(get_loop_rate_hz());
perf_info.reset();
loop_start = AP_HAL::micros();
_log_performance_bit = log_performance_bit;
}
@ -209,14 +208,14 @@ float AP_Scheduler::load_average()
void AP_Scheduler::loop()
{
const uint32_t timer = AP_HAL::micros();
// wait for an INS sample
AP::ins().wait_for_sample();
// used by PI Loops
last_loop_time = (float)(timer - loop_start) / 1000000.0f;
loop_start = timer;
const uint32_t sample_time_us = AP_HAL::micros();
if (loop_timer_start_us == 0) {
loop_timer_start_us = sample_time_us;
}
// Execute the fast loop
// ---------------------
@ -233,7 +232,7 @@ void AP_Scheduler::loop()
// the first call to the scheduler they won't run on a later
// call until scheduler.tick() is called again
const uint32_t loop_us = get_loop_period_us();
const uint32_t time_available = (timer + loop_us) - AP_HAL::micros();
const uint32_t time_available = (sample_time_us + loop_us) - AP_HAL::micros();
run(time_available > loop_us ? 0u : time_available);
// check loop time
@ -241,7 +240,9 @@ void AP_Scheduler::loop()
// move result of AP_HAL::micros() forward:
hal.scheduler->delay_microseconds(1);
#endif
perf_info.check_loop_time(AP_HAL::micros() - loop_start);
perf_info.check_loop_time(AP_HAL::micros() - loop_timer_start_us);
loop_timer_start_us = AP_HAL::micros();
}
void AP_Scheduler::update_logging()

7
libraries/AP_Scheduler/AP_Scheduler.h
View File

@ -140,9 +140,6 @@ public:
// loop performance monitoring:
AP::PerfInfo perf_info;
// time taken for previous loop (in seconds):
float last_loop_time;
private:
// function that is called before anything in the scheduler table:
scheduler_fastloop_fn_t _fastloop_fn;
@ -187,8 +184,8 @@ private:
// number of ticks that _spare_micros is counted over
uint8_t _spare_ticks;
// timestamp of loop start:
uint32_t loop_start;
// start of loop timing
uint32_t loop_timer_start_us;
// performance counters
AP_HAL::Util::perf_counter_t *_perf_counters;