bool AP_InertialSensor_HIL::wait_for_sample(uint16_t timeout_ms)
{
if (_sample_available()) {
return true;
}
uint32_t start = hal.scheduler->millis();
while ((hal.scheduler->millis() - start) < timeout_ms) {
hal.scheduler->delay(1);
if (_sample_available()) {
return true;
}
}
return false;
}
bool AP_InertialSensor_MPU6000_Ext::wait_for_sample(uint16_t timeout_ms)
{
if (_sample_available()) {
return true;
}
uint32_t start = hal.scheduler->millis();
while ((hal.scheduler->millis() - start) < timeout_ms) {
hal.scheduler->delay_microseconds(100);
if (_sample_available()) {
return true;
}
}
return false;
}
bool AP_InertialSensor_HIL::wait_for_sample(uint16_t timeout_ms)
{
if (_sample_available()) {
return true;
}
uint32_t start = hal.scheduler->micros();
while ((hal.scheduler->millis() - start) < timeout_ms) {
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;
}
}
return false;
}
bool AP_InertialSensor_VRBRAIN::wait_for_sample(uint16_t timeout_ms)
{
if (_sample_available()) {
return true;
}
uint32_t start = hal.scheduler->millis();
while ((hal.scheduler->millis() - start) < timeout_ms) {
uint64_t tnow = hrt_absolute_time();
// we spin for the last timing_lag microseconds. Before that
// we yield the CPU to allow IO to happen
const uint16_t timing_lag = 400;
if (_last_sample_timestamp + _sample_time_usec > tnow+timing_lag) {
hal.scheduler->delay_microseconds(_last_sample_timestamp + _sample_time_usec - (tnow+timing_lag));
}
if (_sample_available()) {
return true;
}
}
return false;
}
