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

AP_InertialSensor: improved HIL timing consistancy 

try to keep the average time for each sample right
This commit is contained in:
Andrew Tridgell 2014-08-09 21:22:51 +10:00
parent 56f7d9285f
commit f8ceccce32
2 changed files with 15 additions and 8 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

21
libraries/AP_InertialSensor/AP_InertialSensor_HIL.cpp
View File

@ -30,7 +30,7 @@ uint16_t AP_InertialSensor_HIL::_init_sensor( Sample_rate sample_rate ) {
bool AP_InertialSensor_HIL::update( void ) {
uint32_t now = hal.scheduler->micros();
_last_update_usec = now;
_last_sample_usec = now;
return true;
}
@ -45,10 +45,13 @@ float AP_InertialSensor_HIL::get_gyro_drift_rate(void) {
bool AP_InertialSensor_HIL::_sample_available()
{
uint16_t ret = (hal.scheduler->micros() - _last_update_usec)
/ _sample_period_usec;
return ret > 0;
uint32_t tnow = hal.scheduler->micros();
bool have_sample = false;
while (tnow - _last_sample_usec > _sample_period_usec) {
have_sample = true;
_last_sample_usec += _sample_period_usec;
}
return have_sample;
}
bool AP_InertialSensor_HIL::wait_for_sample(uint16_t timeout_ms)
@ -56,9 +59,13 @@ bool AP_InertialSensor_HIL::wait_for_sample(uint16_t timeout_ms)
if (_sample_available()) {
return true;
}
uint32_t start = hal.scheduler->millis();
uint32_t start = hal.scheduler->micros();
while ((hal.scheduler->millis() - start) < timeout_ms) {
hal.scheduler->delay(1);
uint32_t tnow = hal.scheduler->micros();
uint32_t tdelay = (_last_sample_usec + _sample_period_usec) - tnow;
if (tdelay < 100000) {
hal.scheduler->delay_microseconds(tdelay);
}
if (_sample_available()) {
return true;
}

2
libraries/AP_InertialSensor/AP_InertialSensor_HIL.h
View File

@ -28,7 +28,7 @@ private:
bool _sample_available();
uint16_t _init_sensor( Sample_rate sample_rate );
uint32_t _sample_period_usec;
uint32_t _last_update_usec;
uint32_t _last_sample_usec;
uint32_t _last_accel_usec[INS_MAX_INSTANCES];
uint32_t _last_gyro_usec[INS_MAX_INSTANCES];
};