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) stopped_clock_usec(0)
{ {
signal(SIGCONT, sigcont_handler); 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) void SITLScheduler::init(void *unused)
@ -134,6 +136,38 @@ uint32_t SITLScheduler::millis()
extern AVR_SITL::SITL_State *g_state; 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) 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 we are using a synthetic clock. We want to wait until
the stop_clock() call advances the clock the stop_clock() call advances the clock
*/ */
pthread_barrier_wait(&clock_barrier); clock_barrier_wait();
} else { } else {
usleep(usec - dtime); 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 wait until the main thread is waiting for us. This ensures
that any processing is complete before we advance the clock that any processing is complete before we advance the clock
*/ */
pthread_barrier_wait(&clock_barrier); clock_barrier_wait();
} }
stopped_clock_usec = time_usec; stopped_clock_usec = time_usec;
/* /*
wait again to ensure the main thread can't get behind the FDM 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); _run_io_procs(false);
} }

8
libraries/AP_HAL_AVR_SITL/Scheduler.h
View File

@ -74,10 +74,16 @@ private:
#endif #endif
void stop_clock(uint64_t time_usec); void stop_clock(uint64_t time_usec);
void clock_barrier_wait();
bool _initialized; bool _initialized;
volatile uint64_t stopped_clock_usec; 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
#endif // __AP_HAL_SITL_SCHEDULER_H__ #endif // __AP_HAL_SITL_SCHEDULER_H__