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;
}
/*================ AP_INERTIALSENSOR PUBLIC INTERFACE ==================== */
bool AP_InertialSensor_MPU6000_I2C::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_PX4::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;
}
// This takes about 20us to run
bool AP_InertialSensor_Flymaple::update(void)
{
while (sample_available() == false) {
hal.scheduler->delay(1);
}
Vector3f accel_scale = _accel_scale.get();
// Not really needed since Flymaple _accumulate runs in the main thread
hal.scheduler->suspend_timer_procs();
// base the time on the gyro timestamp, as that is what is
// multiplied by time to integrate in DCM
_delta_time = (_last_gyro_timestamp - _last_update_usec) * 1.0e-6f;
_last_update_usec = _last_gyro_timestamp;
_accel = _accel_filtered;
_accel_samples = 0;
_gyro = _gyro_filtered;
_gyro_samples = 0;
hal.scheduler->resume_timer_procs();
// add offsets and rotation
_accel.rotate(_board_orientation);
// Adjust for chip scaling to get m/s/s
_accel *= FLYMAPLE_ACCELEROMETER_SCALE_M_S;
// Now the calibration scale factor
_accel.x *= accel_scale.x;
_accel.y *= accel_scale.y;
_accel.z *= accel_scale.z;
_accel -= _accel_offset;
_gyro.rotate(_board_orientation);
// Adjust for chip scaling to get radians/sec
_gyro *= FLYMAPLE_GYRO_SCALE_R_S;
_gyro -= _gyro_offset;
if (_last_filter_hz != _mpu6000_filter) {
_set_filter_frequency(_mpu6000_filter);
_last_filter_hz = _mpu6000_filter;
}
return true;
}
