mirror of
https://github.com/ArduPilot/ardupilot
synced 2025-02-23 00:04:02 -04:00
RCOutput_Bebop: use CLOCK_MONOTONIC instead of CLOCK_REALTIME
CLOCK_MONOTONIC is not affected by discontinuous jumps in the system time.
This commit is contained in:
parent
35dd7322d2
commit
9e5d13c1de
@ -269,6 +269,7 @@ void LinuxRCOutput_Bebop::init(void* dummy)
|
||||
int ret=0;
|
||||
struct sched_param param = { .sched_priority = RCOUT_BEBOP_RTPRIO };
|
||||
pthread_attr_t attr;
|
||||
pthread_condattr_t cond_attr;
|
||||
|
||||
_i2c_sem = hal.i2c1->get_semaphore();
|
||||
if (_i2c_sem == NULL) {
|
||||
@ -285,8 +286,10 @@ void LinuxRCOutput_Bebop::init(void* dummy)
|
||||
|
||||
pthread_mutex_lock(&_mutex);
|
||||
|
||||
ret = pthread_cond_init(&_cond, NULL);
|
||||
|
||||
pthread_condattr_init(&cond_attr);
|
||||
pthread_condattr_setclock(&cond_attr, CLOCK_MONOTONIC);
|
||||
ret = pthread_cond_init(&_cond, &cond_attr);
|
||||
pthread_condattr_destroy(&cond_attr);
|
||||
if (ret != 0) {
|
||||
perror("RCout_Bebop: failed to init cond\n");
|
||||
goto exit;
|
||||
@ -390,7 +393,7 @@ void LinuxRCOutput_Bebop::_run_rcout()
|
||||
|
||||
while (true) {
|
||||
pthread_mutex_lock(&_mutex);
|
||||
ret = clock_gettime(CLOCK_REALTIME, &ts);
|
||||
ret = clock_gettime(CLOCK_MONOTONIC, &ts);
|
||||
if (ret != 0)
|
||||
hal.console->println_P("RCOutput_Bebop: bad checksum in obs data");
|
||||
|
||||
|
Loading…
Reference in New Issue
Block a user