aepko/Ardupilot2
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

HAL_Linux: added stop_clock hack!

Browse Source
This commit is contained in:
Andrew Tridgell 2013-12-31 20:05:07 +11:00
parent 006b9da6be
commit 117b77460e
3 changed files with 19 additions and 0 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

2
libraries/AP_HAL/Scheduler.h
View File

@ -48,6 +48,8 @@ public:
optional function to shift forward in time, used by log replay optional function to shift forward in time, used by log replay
*/ */
virtual void time_shift(uint32_t shift_ms) {} virtual void time_shift(uint32_t shift_ms) {}
virtual void stop_clock(uint32_t time_ms) {}
}; };
#endif // __AP_HAL_SCHEDULER_H__ #endif // __AP_HAL_SCHEDULER_H__

14
libraries/AP_HAL_Linux/Scheduler.cpp
View File

@ -87,6 +87,8 @@ void LinuxScheduler::_microsleep(uint32_t usec)
void LinuxScheduler::delay(uint16_t ms) void LinuxScheduler::delay(uint16_t ms)
{ {
uint32_t old_stop = stopped_clock_ms;
stopped_clock_ms = 0;
uint32_t start = millis(); uint32_t start = millis();
while ((millis() - start) < ms) { while ((millis() - start) < ms) {
@ -98,10 +100,14 @@ void LinuxScheduler::delay(uint16_t ms)
} }
} }
} }
stopped_clock_ms = old_stop;
} }
uint32_t LinuxScheduler::millis() uint32_t LinuxScheduler::millis()
{ {
if (stopped_clock_ms) {
return stopped_clock_ms;
}
struct timespec ts; struct timespec ts;
clock_gettime(CLOCK_MONOTONIC, &ts); clock_gettime(CLOCK_MONOTONIC, &ts);
return 1.0e3*((ts.tv_sec + (ts.tv_nsec*1.0e-9)) - return 1.0e3*((ts.tv_sec + (ts.tv_nsec*1.0e-9)) -
@ -111,6 +117,9 @@ uint32_t LinuxScheduler::millis()
uint32_t LinuxScheduler::micros() uint32_t LinuxScheduler::micros()
{ {
if (stopped_clock_ms) {
return stopped_clock_ms * 1000;
}
struct timespec ts; struct timespec ts;
clock_gettime(CLOCK_MONOTONIC, &ts); clock_gettime(CLOCK_MONOTONIC, &ts);
return 1.0e6*((ts.tv_sec + (ts.tv_nsec*1.0e-9)) - return 1.0e6*((ts.tv_sec + (ts.tv_nsec*1.0e-9)) -
@ -334,4 +343,9 @@ void LinuxScheduler::time_shift(uint32_t shift_ms)
_sketch_start_time.tv_nsec = new_ns; _sketch_start_time.tv_nsec = new_ns;
} }
void LinuxScheduler::stop_clock(uint32_t time_ms)
{
stopped_clock_ms = time_ms;
}
#endif // CONFIG_HAL_BOARD #endif // CONFIG_HAL_BOARD

3
libraries/AP_HAL_Linux/Scheduler.h
View File

@ -40,6 +40,7 @@ public:
void reboot(bool hold_in_bootloader); void reboot(bool hold_in_bootloader);
void time_shift(uint32_t shift_ms); void time_shift(uint32_t shift_ms);
void stop_clock(uint32_t time_ms);
private: private:
struct timespec _sketch_start_time; struct timespec _sketch_start_time;
@ -77,6 +78,8 @@ private:
void _run_timers(bool called_from_timer_thread); void _run_timers(bool called_from_timer_thread);
void _run_io(void); void _run_io(void);
void _setup_realtime(uint32_t size); void _setup_realtime(uint32_t size);
uint32_t stopped_clock_ms;
}; };
#endif // CONFIG_HAL_BOARD #endif // CONFIG_HAL_BOARD