Mirror/ardupilot
5
0
Fork 0
mirror of https://github.com/ArduPilot/ardupilot synced 2025-01-09 09:28:31 -04:00
Code Issues Packages Projects Releases Wiki Activity

HAL_ChibiOS: implement boost_end

Browse Source 
this fixes problems with main thread priority and SPI bus delays on
fmuv3
This commit is contained in:
Andrew Tridgell 2018-05-04 18:30:25 +10:00
parent 5ae410f7ca
commit dcab51801c
2 changed files with 21 additions and 23 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

17
libraries/AP_HAL_ChibiOS/Scheduler.cpp
View File

@ -52,8 +52,6 @@ Scheduler::Scheduler()
void Scheduler::init()
{
chVTObjectInit(&_boost_timer);
// setup the timer thread - this will call tasks at 1kHz
_timer_thread_ctx = chThdCreateStatic(_timer_thread_wa,
sizeof(_timer_thread_wa),
@ -132,12 +130,13 @@ static void set_high_priority()
/*
return the main thread to normal priority
*/
static void set_normal_priority(void *ctx)
void Scheduler::boost_end(void)
{
#if APM_MAIN_PRIORITY_BOOST != APM_MAIN_PRIORITY
thread_t *task = (thread_t *)ctx;
// we don't need a reschedule as that happens automatically on ISR exit
task->realprio = APM_MAIN_PRIORITY;
if (in_main_thread() && _priority_boosted) {
_priority_boosted = false;
hal_chibios_set_priority(APM_MAIN_PRIORITY);
}
#endif
}
@ -149,9 +148,11 @@ static void set_normal_priority(void *ctx)
*/
void Scheduler::delay_microseconds_boost(uint16_t usec)
{
set_high_priority();
if (in_main_thread()) {
set_high_priority();
_priority_boosted = true;
}
delay_microseconds(usec); //Suspends Thread for desired microseconds
chVTSet(&_boost_timer, US2ST(200), set_normal_priority, chThdGetSelfX());
_called_boost = true;
}

27
libraries/AP_HAL_ChibiOS/Scheduler.h
View File

@ -38,10 +38,6 @@
#define APM_MAIN_PRIORITY_BOOST 182
#endif
#ifndef APM_MAIN_PRIORITY_BOOST_USEC
#define APM_MAIN_PRIORITY_BOOST_USEC 200
#endif
#ifndef APM_SPI_PRIORITY
// SPI priority needs to be above main priority to ensure fast sampling of IMUs can keep up
// with the data rate
@ -70,16 +66,17 @@ public:
void init();
void delay(uint16_t ms);
void delay_microseconds(uint16_t us);
void delay_microseconds_boost(uint16_t us);
void register_delay_callback(AP_HAL::Proc, uint16_t min_time_ms);
void register_timer_process(AP_HAL::MemberProc);
void register_io_process(AP_HAL::MemberProc);
void register_timer_failsafe(AP_HAL::Proc, uint32_t period_us);
void suspend_timer_procs();
void resume_timer_procs();
void reboot(bool hold_in_bootloader);
void delay(uint16_t ms) override;
void delay_microseconds(uint16_t us) override;
void delay_microseconds_boost(uint16_t us) override;
void boost_end(void) override;
void register_delay_callback(AP_HAL::Proc, uint16_t min_time_ms) override;
void register_timer_process(AP_HAL::MemberProc) override;
void register_io_process(AP_HAL::MemberProc) override;
void register_timer_failsafe(AP_HAL::Proc, uint32_t period_us) override;
void suspend_timer_procs() override;
void resume_timer_procs() override;
void reboot(bool hold_in_bootloader) override;
bool in_main_thread() const override;
void system_initialized();
@ -106,6 +103,7 @@ private:
uint16_t _min_delay_cb_ms;
AP_HAL::Proc _failsafe;
bool _called_boost;
bool _priority_boosted;
volatile bool _timer_suspended;
@ -119,7 +117,6 @@ private:
volatile bool _timer_event_missed;
virtual_timer_t _boost_timer;
thread_t* _timer_thread_ctx;
thread_t* _rcin_thread_ctx;
thread_t* _io_thread_ctx;