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

HAL_SITL: implement barriers using pthread condition variables 

this avoid pthread_barrier_*, which is not available on cygwin
This commit is contained in:
Andrew Tridgell 2015-04-28 13:14:12 +10:00
parent c974134ea4
commit 063a33ebce
2 changed files with 45 additions and 5 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

42
libraries/AP_HAL_AVR_SITL/Scheduler.cpp
View File

@ -55,7 +55,9 @@ SITLScheduler::SITLScheduler(SITL_State *sitlState) :
stopped_clock_usec(0)
{
signal(SIGCONT, sigcont_handler);
pthread_barrier_init(&clock_barrier, NULL, 2);
pthread_mutex_init(&clock_barrier.m, NULL);
pthread_cond_init(&clock_barrier.cv, NULL);
clock_barrier.state = CLOCK_WAIT_INIT;
}
void SITLScheduler::init(void *unused)
@ -134,6 +136,38 @@ uint32_t SITLScheduler::millis()
extern AVR_SITL::SITL_State *g_state;
/*
implement a barrier wait for SITL lock-step scheduling. We don't use
pthread_barrier_* as it is not available in cygwin
*/
void SITLScheduler::clock_barrier_wait(void)
{
pthread_mutex_lock(&clock_barrier.m);
while (clock_barrier.state == CLOCK_WAIT_TWO ||
clock_barrier.state == CLOCK_WAIT_THREE) {
pthread_cond_wait(&clock_barrier.cv, &clock_barrier.m);
}
if (clock_barrier.state == CLOCK_WAIT_INIT) {
clock_barrier.state = CLOCK_WAIT_ONE;
pthread_cond_signal(&clock_barrier.cv);
while (clock_barrier.state != CLOCK_WAIT_TWO) {
pthread_cond_wait(&clock_barrier.cv, &clock_barrier.m);
}
clock_barrier.state = CLOCK_WAIT_THREE;
pthread_cond_signal(&clock_barrier.cv);
} else {
clock_barrier.state = CLOCK_WAIT_TWO;
pthread_cond_signal(&clock_barrier.cv);
while (clock_barrier.state != CLOCK_WAIT_THREE) {
pthread_cond_wait(&clock_barrier.cv, &clock_barrier.m);
}
clock_barrier.state = CLOCK_WAIT_INIT;
pthread_cond_signal(&clock_barrier.cv);
}
pthread_mutex_unlock(&clock_barrier.m);
}
void SITLScheduler::delay_microseconds(uint16_t usec)
{
@ -145,7 +179,7 @@ void SITLScheduler::delay_microseconds(uint16_t usec)
we are using a synthetic clock. We want to wait until
the stop_clock() call advances the clock
*/
pthread_barrier_wait(&clock_barrier);
clock_barrier_wait();
} else {
usleep(usec - dtime);
}
@ -332,13 +366,13 @@ void SITLScheduler::stop_clock(uint64_t time_usec)
wait until the main thread is waiting for us. This ensures
that any processing is complete before we advance the clock
*/
pthread_barrier_wait(&clock_barrier);
clock_barrier_wait();
}
stopped_clock_usec = time_usec;
/*
wait again to ensure the main thread can't get behind the FDM
*/
pthread_barrier_wait(&clock_barrier);
clock_barrier_wait();
_run_io_procs(false);
}

8
libraries/AP_HAL_AVR_SITL/Scheduler.h
View File

@ -74,10 +74,16 @@ private:
#endif
void stop_clock(uint64_t time_usec);
void clock_barrier_wait();
bool _initialized;
volatile uint64_t stopped_clock_usec;
pthread_barrier_t clock_barrier;
enum clock_wait { CLOCK_WAIT_INIT, CLOCK_WAIT_ONE, CLOCK_WAIT_TWO, CLOCK_WAIT_THREE };
struct {
enum clock_wait state;
pthread_mutex_t m;
pthread_cond_t cv;
} clock_barrier;
};
#endif
#endif // __AP_HAL_SITL_SCHEDULER_H__