HAL_PX4: added delay_microseconds_boost()

implemented using hrt callback with sem_post wrapper
This commit is contained in:
Andrew Tridgell 2015-02-16 11:53:08 +11:00
parent c63540f7b1
commit 18131eae13
4 changed files with 63 additions and 11 deletions

View File

@ -93,7 +93,7 @@ extern const AP_HAL::HAL& hal;
/* /*
set the priority of the main APM task set the priority of the main APM task
*/ */
static void set_priority(uint8_t priority) void hal_px4_set_priority(uint8_t priority)
{ {
struct sched_param param; struct sched_param param;
param.sched_priority = priority; param.sched_priority = priority;
@ -108,7 +108,7 @@ static void set_priority(uint8_t priority)
*/ */
static void loop_overtime(void *) static void loop_overtime(void *)
{ {
set_priority(APM_OVERTIME_PRIORITY); hal_px4_set_priority(APM_OVERTIME_PRIORITY);
px4_ran_overtime = true; px4_ran_overtime = true;
} }
@ -134,7 +134,7 @@ static int main_loop(int argc, char **argv)
run setup() at low priority to ensure CLI doesn't hang the run setup() at low priority to ensure CLI doesn't hang the
system, and to allow initial sensor read loops to run system, and to allow initial sensor read loops to run
*/ */
set_priority(APM_STARTUP_PRIORITY); hal_px4_set_priority(APM_STARTUP_PRIORITY);
schedulerInstance.hal_initialized(); schedulerInstance.hal_initialized();
@ -150,7 +150,7 @@ static int main_loop(int argc, char **argv)
/* /*
switch to high priority for main loop switch to high priority for main loop
*/ */
set_priority(APM_MAIN_PRIORITY); hal_px4_set_priority(APM_MAIN_PRIORITY);
while (!_px4_thread_should_exit) { while (!_px4_thread_should_exit) {
perf_begin(perf_loop); perf_begin(perf_loop);
@ -170,7 +170,7 @@ static int main_loop(int argc, char **argv)
we ran over 1s in loop(), and our priority was lowered we ran over 1s in loop(), and our priority was lowered
to let a driver run. Set it back to high priority now. to let a driver run. Set it back to high priority now.
*/ */
set_priority(APM_MAIN_PRIORITY); hal_px4_set_priority(APM_MAIN_PRIORITY);
perf_count(perf_overrun); perf_count(perf_overrun);
px4_ran_overtime = false; px4_ran_overtime = false;
} }

View File

@ -17,6 +17,8 @@ public:
void init(int argc, char * const argv[]) const; void init(int argc, char * const argv[]) const;
}; };
void hal_px4_set_priority(uint8_t priority);
extern const HAL_PX4 AP_HAL_PX4; extern const HAL_PX4 AP_HAL_PX4;
#endif // CONFIG_HAL_BOARD == HAL_BOARD_PX4 #endif // CONFIG_HAL_BOARD == HAL_BOARD_PX4

View File

@ -114,6 +114,39 @@ void PX4Scheduler::delay_microseconds(uint16_t usec)
perf_end(_perf_delay); perf_end(_perf_delay);
} }
/*
wrapper around sem_post that boosts main thread priority
*/
static void sem_post_boost(sem_t *sem)
{
hal_px4_set_priority(APM_MAIN_PRIORITY_BOOST);
sem_post(sem);
}
/*
return the main thread to normal priority
*/
static void set_normal_priority(void *sem)
{
hal_px4_set_priority(APM_MAIN_PRIORITY);
}
/*
a varient of delay_microseconds that boosts priority to
APM_MAIN_PRIORITY_BOOST for APM_MAIN_PRIORITY_BOOST_USEC
microseconds when the time completes. This significantly improves
the regularity of timing of the main loop as it takes
*/
void PX4Scheduler::delay_microseconds_boost(uint16_t usec)
{
sem_t wait_semaphore;
static struct hrt_call wait_call;
sem_init(&wait_semaphore, 0, 0);
hrt_call_after(&wait_call, usec, (hrt_callout)sem_post_boost, &wait_semaphore);
sem_wait(&wait_semaphore);
hrt_call_after(&wait_call, APM_MAIN_PRIORITY_BOOST_USEC, (hrt_callout)set_normal_priority, NULL);
}
void PX4Scheduler::delay(uint16_t ms) void PX4Scheduler::delay(uint16_t ms)
{ {
if (in_timerprocess()) { if (in_timerprocess()) {

View File

@ -12,12 +12,28 @@
#define PX4_SCHEDULER_MAX_TIMER_PROCS 8 #define PX4_SCHEDULER_MAX_TIMER_PROCS 8
#define APM_MAIN_PRIORITY 180 #define APM_MAIN_PRIORITY_BOOST 241
#define APM_TIMER_PRIORITY 181 #define APM_MAIN_PRIORITY 180
#define APM_UART_PRIORITY 60 #define APM_TIMER_PRIORITY 181
#define APM_IO_PRIORITY 59 #define APM_UART_PRIORITY 60
#define APM_OVERTIME_PRIORITY 10 #define APM_IO_PRIORITY 59
#define APM_STARTUP_PRIORITY 10 #define APM_OVERTIME_PRIORITY 10
#define APM_STARTUP_PRIORITY 10
/* how long to boost priority of the main thread for each main
loop. This needs to be long enough for all interrupt-level drivers
(mostly SPI drivers) to run, and for the main loop of the vehicle
code to start the AHRS update.
Priority boosting of the main thread in delay_microseconds_boost()
avoids the problem that drivers in hpwork all happen to run right
at the start of the period where the main vehicle loop is calling
wait_for_sample(). That causes main loop timing jitter, which
reduces performance. Using the priority boost the main loop
temporarily runs at a priority higher than hpwork and the timer
thread, which results in much more consistent loop timing.
*/
#define APM_MAIN_PRIORITY_BOOST_USEC 150
#define APM_MAIN_THREAD_STACK_SIZE 8192 #define APM_MAIN_THREAD_STACK_SIZE 8192
@ -34,6 +50,7 @@ public:
uint64_t millis64(); uint64_t millis64();
uint64_t micros64(); uint64_t micros64();
void delay_microseconds(uint16_t us); 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_delay_callback(AP_HAL::Proc, uint16_t min_time_ms);
void register_timer_process(AP_HAL::MemberProc); void register_timer_process(AP_HAL::MemberProc);
void register_io_process(AP_HAL::MemberProc); void register_io_process(AP_HAL::MemberProc);