diff --git a/flight/modules/Stabilization/stabilization.c b/flight/modules/Stabilization/stabilization.c index 9bc5d1957..186c10739 100644 --- a/flight/modules/Stabilization/stabilization.c +++ b/flight/modules/Stabilization/stabilization.c @@ -757,13 +757,17 @@ static float stab_powf(float x, uint8_t p) static void SettingsBankUpdatedCb(__attribute__((unused)) UAVObjEvent *ev) { - StabilizationBankData bank, oldBank; - - StabilizationBankGet(&oldBank); - if (cur_flight_mode < 0 || cur_flight_mode >= NUM_FMS_POSITIONS) { return; } + if ((settings.FlightModeMap[cur_flight_mode] == 0 && ev->obj != StabilizationSettingsBank1Handle()) || + (settings.FlightModeMap[cur_flight_mode] == 1 && ev->obj != StabilizationSettingsBank2Handle()) || + (settings.FlightModeMap[cur_flight_mode] == 2 && ev->obj != StabilizationSettingsBank3Handle()) || + settings.FlightModeMap[cur_flight_mode] > 2) { + return; + } + + StabilizationBankData bank; switch (settings.FlightModeMap[cur_flight_mode]) { case 0: @@ -777,16 +781,8 @@ static void SettingsBankUpdatedCb(__attribute__((unused)) UAVObjEvent *ev) case 2: StabilizationSettingsBank3Get((StabilizationSettingsBank3Data *)&bank); break; - - default: - memset(&bank, 0, sizeof(StabilizationBankDataPacked)); -// return; //bank number is invalid. All we can do is ignore it. - } - - // Need to do this to prevent an infinite loop - if (memcmp(&oldBank, &bank, sizeof(StabilizationBankDataPacked)) != 0) { - StabilizationBankSet(&bank); } + StabilizationBankSet(&bank); } static void BankUpdatedCb(__attribute__((unused)) UAVObjEvent *ev)