Browse Source

AP_Mount: don't use disabled IMUs in solo gimbal code

mission-4.1.18
Andrew Tridgell 6 years ago
parent
commit
a45e364252
  1. 2
      libraries/AP_Mount/SoloGimbal.cpp

2
libraries/AP_Mount/SoloGimbal.cpp

@ -229,7 +229,7 @@ void SoloGimbal::readVehicleDeltaAngle(uint8_t ins_index, Vector3f &dAng) { @@ -229,7 +229,7 @@ void SoloGimbal::readVehicleDeltaAngle(uint8_t ins_index, Vector3f &dAng) {
void SoloGimbal::update_fast() {
const AP_InertialSensor &ins = AP::ins();
if (ins.get_gyro_health(0) && ins.get_gyro_health(1)) {
if (ins.use_gyro(0) && ins.use_gyro(1)) {
// dual gyro mode - average first two gyros
Vector3f dAng;
readVehicleDeltaAngle(0, dAng);

Loading…
Cancel
Save