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; }