diff --git a/src/modules/mc_att_control/mc_att_control_main.cpp b/src/modules/mc_att_control/mc_att_control_main.cpp index 3f85467123..f1ac17f16b 100644 --- a/src/modules/mc_att_control/mc_att_control_main.cpp +++ b/src/modules/mc_att_control/mc_att_control_main.cpp @@ -835,7 +835,7 @@ MulticopterAttitudeControl::sensor_correction_poll() } /* update the latest gyro selection */ - if (_sensor_correction.selected_gyro_instance < sizeof(_sensor_gyro_sub) / sizeof(_sensor_gyro_sub[0])) { + if (_sensor_correction.selected_gyro_instance < _gyro_count) { _selected_gyro = _sensor_correction.selected_gyro_instance; } } @@ -1109,6 +1109,10 @@ MulticopterAttitudeControl::task_main() _gyro_count = math::min(orb_group_count(ORB_ID(sensor_gyro)), MAX_GYRO_COUNT); + if (_gyro_count == 0) { + _gyro_count = 1; + } + for (unsigned s = 0; s < _gyro_count; s++) { _sensor_gyro_sub[s] = orb_subscribe_multi(ORB_ID(sensor_gyro), s); }