mirror of
https://github.com/ArduPilot/ardupilot
synced 2025-01-11 10:28:29 -04:00
desktop: make the main loop a bit more responsive
this introduces less delays on linux than usleep()
This commit is contained in:
parent
6f44415b19
commit
3633d846f6
@ -2,6 +2,8 @@
|
||||
#include <unistd.h>
|
||||
#include <time.h>
|
||||
#include <sys/time.h>
|
||||
#include <sched.h>
|
||||
#include <wiring.h>
|
||||
|
||||
void setup(void);
|
||||
void loop(void);
|
||||
@ -13,11 +15,16 @@ int main(void)
|
||||
gettimeofday(&sketch_start_time, NULL);
|
||||
setup();
|
||||
while (true) {
|
||||
struct timespec ts;
|
||||
struct timeval tv;
|
||||
unsigned long t1, t2;
|
||||
t1 = micros();
|
||||
loop();
|
||||
ts.tv_sec = 0;
|
||||
ts.tv_nsec = 100;
|
||||
nanosleep(&ts, NULL);
|
||||
t2 = micros();
|
||||
if (t2 - t1 < 2) {
|
||||
tv.tv_sec = 0;
|
||||
tv.tv_usec = 1;
|
||||
select(0, NULL, NULL, NULL, &tv);
|
||||
}
|
||||
}
|
||||
return 0;
|
||||
}
|
||||
|
Loading…
Reference in New Issue
Block a user