Mirror/ardupilot
5
0
Fork 0
mirror of https://github.com/ArduPilot/ardupilot synced 2025-02-21 15:23:57 -04:00
Code Issues Packages Projects Releases Wiki Activity

HAL_SITL: added millis64() and micros64()

Browse Source
This commit is contained in:
Andrew Tridgell 2014-08-20 07:57:45 +10:00
parent 7439d34a5d
commit 5a55101703
3 changed files with 37 additions and 23 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

54
libraries/AP_HAL_AVR_SITL/Scheduler.cpp
View File

@ -70,52 +70,64 @@ double SITLScheduler::_cyg_sec()
}
#endif
uint32_t SITLScheduler::_micros()
uint64_t SITLScheduler::_micros64()
{
#ifdef __CYGWIN__
return (uint32_t)(_cyg_sec() * 1.0e6);
return (uint64_t)(_cyg_sec() * 1.0e6);
#else
struct timeval tp;
gettimeofday(&tp,NULL);
return 1.0e6*((tp.tv_sec + (tp.tv_usec*1.0e-6)) -
(_sketch_start_time.tv_sec +
(_sketch_start_time.tv_usec*1.0e-6)));
uint64_t ret = 1.0e6*((tp.tv_sec + (tp.tv_usec*1.0e-6)) -
(_sketch_start_time.tv_sec +
(_sketch_start_time.tv_usec*1.0e-6)));
return ret;
#endif
}
uint64_t SITLScheduler::micros64()
{
return _micros64();
}
uint32_t SITLScheduler::micros()
{
return _micros();
return micros64() & 0xFFFFFFFF;
}
uint64_t SITLScheduler::millis64()
{
#ifdef __CYGWIN__
// 1000 ms in a second
return (uint64_t)(_cyg_sec() * 1000);
#else
struct timeval tp;
gettimeofday(&tp,NULL);
uint64_t ret = 1.0e3*((tp.tv_sec + (tp.tv_usec*1.0e-6)) -
(_sketch_start_time.tv_sec +
(_sketch_start_time.tv_usec*1.0e-6)));
return ret;
#endif
}
uint32_t SITLScheduler::millis()
{
#ifdef __CYGWIN__
// 1000 ms in a second
return (uint32_t)(_cyg_sec() * 1000);
#else
struct timeval tp;
gettimeofday(&tp,NULL);
return 1.0e3*((tp.tv_sec + (tp.tv_usec*1.0e-6)) -
(_sketch_start_time.tv_sec +
(_sketch_start_time.tv_usec*1.0e-6)));
#endif
return millis64() & 0xFFFFFFFF;
}
void SITLScheduler::delay_microseconds(uint16_t usec)
{
uint32_t start = micros();
while (micros() - start < usec) {
usleep(usec - (micros() - start));
uint64_t start = micros64();
while (micros64() - start < usec) {
usleep(usec - (micros64() - start));
}
}
void SITLScheduler::delay(uint16_t ms)
{
uint32_t start = micros();
uint64_t start = micros64();
while (ms > 0) {
while ((micros() - start) >= 1000) {
while ((micros64() - start) >= 1000) {
ms--;
if (ms == 0) break;
start += 1000;

4
libraries/AP_HAL_AVR_SITL/Scheduler.h
View File

@ -19,6 +19,8 @@ public:
void delay(uint16_t ms);
uint32_t millis();
uint32_t micros();
uint64_t millis64();
uint64_t micros64();
void delay_microseconds(uint16_t us);
void register_delay_callback(AP_HAL::Proc, uint16_t min_time_ms);
@ -43,7 +45,7 @@ public:
void sitl_end_atomic();
// callable from interrupt handler
static uint32_t _micros();
static uint64_t _micros64();
static void timer_event() { _run_timer_procs(true); _run_io_procs(true); }
private:

2
libraries/AP_HAL_AVR_SITL/sitl_ins.cpp
View File

@ -52,7 +52,7 @@ float SITL_State::_gyro_drift(void)
return 0;
}
double period = _sitl->drift_time * 2;
double minutes = fmod(_scheduler->_micros() / 60.0e6, period);
double minutes = fmod(_scheduler->_micros64() / 60.0e6, period);
if (minutes < period/2) {
return minutes * ToRad(_sitl->drift_speed);
}