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
*/
static void set_priority(uint8_t priority)
void hal_px4_set_priority(uint8_t priority)
{
struct sched_param param;
param.sched_priority = priority;
@ -108,7 +108,7 @@ static void set_priority(uint8_t priority)
*/
static void loop_overtime(void *)
{
set_priority(APM_OVERTIME_PRIORITY);
hal_px4_set_priority(APM_OVERTIME_PRIORITY);
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
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();
@ -150,7 +150,7 @@ static int main_loop(int argc, char **argv)
/*
switch to high priority for main loop
*/
set_priority(APM_MAIN_PRIORITY);
hal_px4_set_priority(APM_MAIN_PRIORITY);
while (!_px4_thread_should_exit) {
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
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);
px4_ran_overtime = false;
}

View File

@ -17,6 +17,8 @@ public:
void init(int argc, char * const argv[]) const;
};
void hal_px4_set_priority(uint8_t priority);
extern const HAL_PX4 AP_HAL_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);
}
/*
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)
{
if (in_timerprocess()) {

View File

@ -12,6 +12,7 @@
#define PX4_SCHEDULER_MAX_TIMER_PROCS 8
#define APM_MAIN_PRIORITY_BOOST 241
#define APM_MAIN_PRIORITY 180
#define APM_TIMER_PRIORITY 181
#define APM_UART_PRIORITY 60
@ -19,6 +20,21 @@
#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
/* Scheduler implementation: */
@ -34,6 +50,7 @@ public:
uint64_t millis64();
uint64_t micros64();
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);