bool AP_InertialSensor_VRBRAIN::update(void)
{
if (!wait_for_sample(100)) {
return false;
}
// get the latest sample from the sensor drivers
_get_sample();
for (uint8_t k=0; k<_num_accel_instances; k++) {
_previous_accel[k] = _accel[k];
_accel[k] = _accel_in[k];
_accel[k].rotate(_board_orientation);
_accel[k].x *= _accel_scale[k].get().x;
_accel[k].y *= _accel_scale[k].get().y;
_accel[k].z *= _accel_scale[k].get().z;
_accel[k] -= _accel_offset[k];
}
for (uint8_t k=0; k<_num_gyro_instances; k++) {
_gyro[k] = _gyro_in[k];
_gyro[k].rotate(_board_orientation);
_gyro[k] -= _gyro_offset[k];
}
if (_last_filter_hz != _mpu6000_filter) {
_set_filter_frequency(_mpu6000_filter);
_last_filter_hz = _mpu6000_filter;
}
_have_sample_available = false;
return true;
}
bool AP_InertialSensor_PX4::update(void)
{
Vector3f accel_scale = _accel_scale.get();
// get the latest sample from the sensor drivers
_get_sample();
_previous_accel = _accel;
_accel = _accel_in;
_gyro = _gyro_in;
// add offsets and rotation
_accel.rotate(_board_orientation);
_accel.x *= accel_scale.x;
_accel.y *= accel_scale.y;
_accel.z *= accel_scale.z;
_accel -= _accel_offset;
_gyro.rotate(_board_orientation);
_gyro -= _gyro_offset;
if (_last_filter_hz != _mpu6000_filter) {
_set_filter_frequency(_mpu6000_filter);
_last_filter_hz = _mpu6000_filter;
}
_have_sample_available = false;
return true;
}
/**
try to detect bad accel/gyro sensors
*/
bool AP_InertialSensor_PX4::healthy(void)
{
if (_sample_time_usec == 0) {
// not initialised yet, show as healthy to prevent scary GCS
// warnings
return true;
}
uint64_t tnow = hrt_absolute_time();
if ((tnow - _last_accel_timestamp) > 2*_sample_time_usec ||
(tnow - _last_gyro_timestamp) > 2*_sample_time_usec) {
// see if new samples are available
_get_sample();
tnow = hrt_absolute_time();
}
if ((tnow - _last_accel_timestamp) > 2*_sample_time_usec) {
// accels have not updated
return false;
}
if ((tnow - _last_gyro_timestamp) > 2*_sample_time_usec) {
// gyros have not updated
return false;
}
if (fabs(_accel.x) > 30 && fabs(_accel.y) > 30 && fabs(_accel.z) > 30 &&
(_previous_accel - _accel).length() < 0.01f) {
// unchanging accel, large in all 3 axes. This is a likely
// accelerometer failure of the LSM303d
return false;
}
return true;
}
bool AP_InertialSensor_PX4::update(void)
{
// get the latest sample from the sensor drivers
_get_sample();
for (uint8_t k=0; k<_num_accel_instances; k++) {
Vector3f accel = _accel_in[k];
// calling _rotate_and_offset_accel sets the sensor healthy,
// so we only want to do this if we have new data from it
if (_last_accel_timestamp[k] != _last_accel_update_timestamp[k]) {
_rotate_and_offset_accel(_accel_instance[k], accel);
_last_accel_update_timestamp[k] = _last_accel_timestamp[k];
}
}
for (uint8_t k=0; k<_num_gyro_instances; k++) {
Vector3f gyro = _gyro_in[k];
// calling _rotate_and_offset_accel sets the sensor healthy,
// so we only want to do this if we have new data from it
if (_last_gyro_timestamp[k] != _last_gyro_update_timestamp[k]) {
_rotate_and_offset_gyro(_gyro_instance[k], gyro);
_last_gyro_update_timestamp[k] = _last_gyro_timestamp[k];
}
}
if (_last_filter_hz != _imu.get_filter()) {
_set_filter_frequency(_imu.get_filter());
_last_filter_hz = _imu.get_filter();
}
return true;
}
bool AP_InertialSensor_PX4::accel_sample_available(void)
{
_get_sample();
for (uint8_t i=0; i<_num_accel_instances; i++) {
if (_last_accel_timestamp[i] != _last_accel_update_timestamp[i]) {
return true;
}
}
return false;
}
bool AP_InertialSensor_PX4::update(void)
{
// get the latest sample from the sensor drivers
_get_sample();
for (uint8_t k=0; k<_num_accel_instances; k++) {
update_accel(_accel_instance[k]);
}
for (uint8_t k=0; k<_num_gyro_instances; k++) {
update_gyro(_gyro_instance[k]);
}
return true;
}
