uint8_t AP_InertialSensor_HIL::get_gyro_count(void) const
{
if (get_gyro_health(1)) {
return 2;
}
return 1;
}
uint8_t AP_InertialSensor_VRBRAIN::_get_primary_gyro(void) const
{
for (uint8_t i=0; i<_num_gyro_instances; i++) {
if (get_gyro_health(i)) return i;
}
return 0;
}
uint8_t AP_InertialSensor_PX4::_get_primary_gyro(void) const
{
for (uint8_t i=0; i<INS_MAX_INSTANCES; i++) {
if (get_gyro_health(i)) return i;
}
return 0;
}
// return true if gyro instance should be used (must be healthy and have it's use parameter set to 1)
bool AP_InertialSensor::use_gyro(uint8_t instance) const
{
if (instance >= INS_MAX_INSTANCES) {
return false;
}
return (get_gyro_health(instance) && _use[instance]);
}
// get_gyro_health_all - return true if all gyros are healthy
bool AP_InertialSensor::get_gyro_health_all(void) const
{
for (uint8_t i=0; i<get_gyro_count(); i++) {
if (!get_gyro_health(i)) {
return false;
}
}
// return true if we have at least one gyro
return (get_gyro_count() > 0);
}
/*
get delta angles
*/
bool AP_InertialSensor::get_delta_angle(uint8_t i, Vector3f &delta_angle) const
{
if (_delta_angle_valid[i]) {
delta_angle = _delta_angle[i];
return true;
} else if (get_gyro_health(i)) {
// provide delta angle from raw gyro, so we use the same code
// at higher level
delta_angle = get_gyro(i) * get_delta_time();
return true;
}
return false;
}
/**
try to detect bad accel/gyro sensors
*/
bool AP_InertialSensor_VRBRAIN::healthy(void) const
{
return get_gyro_health(0) && get_accel_health(0);
}
