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

AP_HAL: allow for microsecond resolution in scheduler->stop_clock() 

this makes 400Hz in the replay code possible
This commit is contained in:
Andrew Tridgell 2014-02-26 19:33:39 +11:00
parent 6fc5c12f1d
commit 3fa2207a2d
3 changed files with 12 additions and 33 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

6
libraries/AP_HAL/Scheduler.h
View File

@ -45,11 +45,9 @@ public:
virtual void set_timer_speed(uint16_t speed_hz) {}
/**
optional function to shift forward in time, used by log replay
optional function to stop clock at a given time, used by log replay
*/
virtual void time_shift(uint32_t shift_ms) {}
virtual void stop_clock(uint32_t time_ms) {}
virtual void stop_clock(uint64_t time_usec) {}
};
#endif // __AP_HAL_SCHEDULER_H__

34
libraries/AP_HAL_Linux/Scheduler.cpp
View File

@ -87,8 +87,8 @@ void LinuxScheduler::_microsleep(uint32_t usec)
void LinuxScheduler::delay(uint16_t ms)
{
if (stopped_clock_ms) {
stopped_clock_ms += ms;
if (stopped_clock_usec) {
stopped_clock_usec += 1000UL*ms;
return;
}
uint32_t start = millis();
@ -106,8 +106,8 @@ void LinuxScheduler::delay(uint16_t ms)
uint32_t LinuxScheduler::millis()
{
if (stopped_clock_ms) {
return stopped_clock_ms;
if (stopped_clock_usec) {
return stopped_clock_usec/1000;
}
struct timespec ts;
clock_gettime(CLOCK_MONOTONIC, &ts);
@ -118,8 +118,8 @@ uint32_t LinuxScheduler::millis()
uint32_t LinuxScheduler::micros()
{
if (stopped_clock_ms) {
return stopped_clock_ms * 1000;
if (stopped_clock_usec) {
return stopped_clock_usec;
}
struct timespec ts;
clock_gettime(CLOCK_MONOTONIC, &ts);
@ -326,27 +326,9 @@ void LinuxScheduler::reboot(bool hold_in_bootloader)
for(;;);
}
void LinuxScheduler::time_shift(uint32_t shift_ms)
void LinuxScheduler::stop_clock(uint64_t time_usec)
{
if (shift_ms == 0) {
return;
}
// produce a minimal delay so other threads get a chance to run
_microsleep(100);
// shift the sketch start time so that time advances
int64_t new_ns = _sketch_start_time.tv_nsec - (shift_ms * 1.0e6);
while (new_ns < 0) {
new_ns += 1.0e9;
_sketch_start_time.tv_sec -= 1;
}
_sketch_start_time.tv_nsec = new_ns;
}
void LinuxScheduler::stop_clock(uint32_t time_ms)
{
stopped_clock_ms = time_ms;
stopped_clock_usec = time_usec;
}
#endif // CONFIG_HAL_BOARD

5
libraries/AP_HAL_Linux/Scheduler.h
View File

@ -39,8 +39,7 @@ public:
void panic(const prog_char_t *errormsg);
void reboot(bool hold_in_bootloader);
void time_shift(uint32_t shift_ms);
void stop_clock(uint32_t time_ms);
void stop_clock(uint64_t time_usec);
private:
struct timespec _sketch_start_time;
@ -79,7 +78,7 @@ private:
void _run_io(void);
void _setup_realtime(uint32_t size);
uint32_t stopped_clock_ms;
uint64_t stopped_clock_usec;
};
#endif // CONFIG_HAL_BOARD