Mirror/ardupilot
5
0
Fork 0
mirror of https://github.com/ArduPilot/ardupilot synced 2025-01-11 02:18:29 -04:00
Code Issues Packages Projects Releases Wiki Activity

AP_InertialSensor: improved HIL timing consistancy

Browse Source 
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 ) { bool AP_InertialSensor_HIL::update( void ) {
uint32_t now = hal.scheduler->micros(); uint32_t now = hal.scheduler->micros();
_last_update_usec = now; _last_sample_usec = now;
return true; return true;
} }
@ -45,10 +45,13 @@ float AP_InertialSensor_HIL::get_gyro_drift_rate(void) {
bool AP_InertialSensor_HIL::_sample_available() bool AP_InertialSensor_HIL::_sample_available()
{ {
uint16_t ret = (hal.scheduler->micros() - _last_update_usec) uint32_t tnow = hal.scheduler->micros();
/ _sample_period_usec; bool have_sample = false;
while (tnow - _last_sample_usec > _sample_period_usec) {
return ret > 0; have_sample = true;
_last_sample_usec += _sample_period_usec;
}
return have_sample;
} }
bool AP_InertialSensor_HIL::wait_for_sample(uint16_t timeout_ms) 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()) { if (_sample_available()) {
return true; return true;
} }
uint32_t start = hal.scheduler->millis(); uint32_t start = hal.scheduler->micros();
while ((hal.scheduler->millis() - start) < timeout_ms) { 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()) { if (_sample_available()) {
return true; return true;
} }

2
libraries/AP_InertialSensor/AP_InertialSensor_HIL.h
View File

@ -28,7 +28,7 @@ private:
bool _sample_available(); bool _sample_available();
uint16_t _init_sensor( Sample_rate sample_rate ); uint16_t _init_sensor( Sample_rate sample_rate );
uint32_t _sample_period_usec; 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_accel_usec[INS_MAX_INSTANCES];
uint32_t _last_gyro_usec[INS_MAX_INSTANCES]; uint32_t _last_gyro_usec[INS_MAX_INSTANCES];
}; };