diff --git a/src/main/fc/fc_msp.c b/src/main/fc/fc_msp.c index d321f0c69d..d5dbc274a7 100644 --- a/src/main/fc/fc_msp.c +++ b/src/main/fc/fc_msp.c @@ -1574,7 +1574,7 @@ static void mspFcDataFlashReadCommand(sbuf_t *dst, sbuf_t *src) // Request payload: // uint32_t - address to read from // uint16_t - size of block to read (optional) - if (dataSize >= sizeof(uint32_t) + sizeof(uint16_t)) { + if (dataSize == sizeof(uint32_t) + sizeof(uint16_t)) { readLength = sbufReadU16(src); } else { @@ -1625,7 +1625,7 @@ static mspResult_e mspFcProcessInCommand(uint16_t cmdMSP, sbuf_t *src) #endif case MSP_SET_ARMING_CONFIG: - if (dataSize >= 2) { + if (dataSize == 2) { sbufReadU8(src); //Swallow the first byte, used to be auto_disarm_delay armingConfigMutable()->disarm_kill_switch = !!sbufReadU8(src); } else @@ -1640,7 +1640,7 @@ static mspResult_e mspFcProcessInCommand(uint16_t cmdMSP, sbuf_t *src) break; case MSP2_SET_PID: - if (dataSize >= PID_ITEM_COUNT * 4) { + if (dataSize == PID_ITEM_COUNT * 4) { for (int i = 0; i < PID_ITEM_COUNT; i++) { pidBankMutable()->pid[i].P = sbufReadU8(src); pidBankMutable()->pid[i].I = sbufReadU8(src); @@ -1655,7 +1655,7 @@ static mspResult_e mspFcProcessInCommand(uint16_t cmdMSP, sbuf_t *src) case MSP_SET_MODE_RANGE: sbufReadU8Safe(&tmp_u8, src); - if ((dataSize >= 5) && (tmp_u8 < MAX_MODE_ACTIVATION_CONDITION_COUNT)) { + if ((dataSize == 5) && (tmp_u8 < MAX_MODE_ACTIVATION_CONDITION_COUNT)) { modeActivationCondition_t *mac = modeActivationConditionsMutable(tmp_u8); tmp_u8 = sbufReadU8(src); const box_t *box = findBoxByPermanentId(tmp_u8); @@ -1676,7 +1676,7 @@ static mspResult_e mspFcProcessInCommand(uint16_t cmdMSP, sbuf_t *src) case MSP_SET_ADJUSTMENT_RANGE: sbufReadU8Safe(&tmp_u8, src); - if ((dataSize >= 7) && (tmp_u8 < MAX_ADJUSTMENT_RANGE_COUNT)) { + if ((dataSize == 7) && (tmp_u8 < MAX_ADJUSTMENT_RANGE_COUNT)) { adjustmentRange_t *adjRange = adjustmentRangesMutable(tmp_u8); tmp_u8 = sbufReadU8(src); if (tmp_u8 < MAX_SIMULTANEOUS_ADJUSTMENT_COUNT) { @@ -1695,7 +1695,7 @@ static mspResult_e mspFcProcessInCommand(uint16_t cmdMSP, sbuf_t *src) break; case MSP_SET_RC_TUNING: - if ((dataSize >= 10) && (dataSize <= 11)) { + if ((dataSize == 10) || (dataSize == 11)) { sbufReadU8(src); //Read rcRate8, kept for protocol compatibility reasons // need to cast away const to set controlRateProfile ((controlRateConfig_t*)currentControlRateProfile)->stabilized.rcExpo8 = sbufReadU8(src); @@ -1763,7 +1763,7 @@ static mspResult_e mspFcProcessInCommand(uint16_t cmdMSP, sbuf_t *src) break; case MSP_SET_MISC: - if (dataSize >= 22) { + if (dataSize == 22) { sbufReadU16(src); // midrc sbufReadU16(src); //Was min_throttle @@ -1914,7 +1914,7 @@ static mspResult_e mspFcProcessInCommand(uint16_t cmdMSP, sbuf_t *src) break; case MSP_SET_MOTOR: - if (dataSize >= 8 * sizeof(uint16_t)) { + if (dataSize == 8 * sizeof(uint16_t)) { for (int i = 0; i < 8; i++) { const int16_t disarmed = sbufReadU16(src); if (i < MAX_SUPPORTED_MOTORS) { @@ -1947,7 +1947,7 @@ static mspResult_e mspFcProcessInCommand(uint16_t cmdMSP, sbuf_t *src) case MSP_SET_SERVO_MIX_RULE: sbufReadU8Safe(&tmp_u8, src); - if ((dataSize >= 9) && (tmp_u8 < MAX_SERVO_RULES)) { + if ((dataSize == 9) && (tmp_u8 < MAX_SERVO_RULES)) { customServoMixersMutable(tmp_u8)->targetChannel = sbufReadU8(src); customServoMixersMutable(tmp_u8)->inputSource = sbufReadU8(src); customServoMixersMutable(tmp_u8)->rate = sbufReadU16(src); @@ -2019,7 +2019,7 @@ static mspResult_e mspFcProcessInCommand(uint16_t cmdMSP, sbuf_t *src) break; case MSP_SET_3D: - if (dataSize >= 6) { + if (dataSize == 6) { reversibleMotorsConfigMutable()->deadband_low = sbufReadU16(src); reversibleMotorsConfigMutable()->deadband_high = sbufReadU16(src); reversibleMotorsConfigMutable()->neutral = sbufReadU16(src); @@ -2028,7 +2028,7 @@ static mspResult_e mspFcProcessInCommand(uint16_t cmdMSP, sbuf_t *src) break; case MSP_SET_RC_DEADBAND: - if (dataSize >= 5) { + if (dataSize == 5) { rcControlsConfigMutable()->deadband = sbufReadU8(src); rcControlsConfigMutable()->yaw_deadband = sbufReadU8(src); rcControlsConfigMutable()->alt_hold_deadband = sbufReadU8(src); @@ -2042,7 +2042,7 @@ static mspResult_e mspFcProcessInCommand(uint16_t cmdMSP, sbuf_t *src) break; case MSP_SET_SENSOR_ALIGNMENT: - if (dataSize >= 4) { + if (dataSize == 4) { gyroConfigMutable()->gyro_align = sbufReadU8(src); accelerometerConfigMutable()->acc_align = sbufReadU8(src); #ifdef USE_MAG @@ -2060,7 +2060,7 @@ static mspResult_e mspFcProcessInCommand(uint16_t cmdMSP, sbuf_t *src) break; case MSP_SET_ADVANCED_CONFIG: - if (dataSize >= 9) { + if (dataSize == 9) { sbufReadU8(src); // gyroConfig()->gyroSyncDenominator sbufReadU8(src); // BF: masterConfig.pid_process_denom sbufReadU8(src); // BF: motorConfig()->useUnsyncedPwm @@ -2114,7 +2114,7 @@ static mspResult_e mspFcProcessInCommand(uint16_t cmdMSP, sbuf_t *src) break; case MSP_SET_PID_ADVANCED: - if (dataSize >= 17) { + if (dataSize == 17) { sbufReadU16(src); // pidProfileMutable()->rollPitchItermIgnoreRate sbufReadU16(src); // pidProfileMutable()->yawItermIgnoreRate sbufReadU16(src); //pidProfile()->yaw_p_limit @@ -2137,7 +2137,7 @@ static mspResult_e mspFcProcessInCommand(uint16_t cmdMSP, sbuf_t *src) break; case MSP_SET_INAV_PID: - if (dataSize >= 15) { + if (dataSize == 15) { sbufReadU8(src); //Legacy, no longer in use async processing value sbufReadU16(src); //Legacy, no longer in use async processing value sbufReadU16(src); //Legacy, no longer in use async processing value @@ -2155,7 +2155,7 @@ static mspResult_e mspFcProcessInCommand(uint16_t cmdMSP, sbuf_t *src) break; case MSP_SET_SENSOR_CONFIG: - if (dataSize >= 6) { + if (dataSize == 6) { accelerometerConfigMutable()->acc_hardware = sbufReadU8(src); #ifdef USE_BARO barometerConfigMutable()->baro_hardware = sbufReadU8(src); @@ -2187,7 +2187,7 @@ static mspResult_e mspFcProcessInCommand(uint16_t cmdMSP, sbuf_t *src) break; case MSP_SET_NAV_POSHOLD: - if (dataSize >= 13) { + if (dataSize == 13) { navConfigMutable()->general.flags.user_control_mode = sbufReadU8(src); navConfigMutable()->general.max_auto_speed = sbufReadU16(src); navConfigMutable()->general.max_auto_climb_rate = sbufReadU16(src); @@ -2201,7 +2201,7 @@ static mspResult_e mspFcProcessInCommand(uint16_t cmdMSP, sbuf_t *src) break; case MSP_SET_RTH_AND_LAND_CONFIG: - if (dataSize >= 19) { + if (dataSize == 21) { navConfigMutable()->general.min_rth_distance = sbufReadU16(src); navConfigMutable()->general.flags.rth_climb_first = sbufReadU8(src); navConfigMutable()->general.flags.rth_climb_ignore_emerg = sbufReadU8(src); @@ -2220,7 +2220,7 @@ static mspResult_e mspFcProcessInCommand(uint16_t cmdMSP, sbuf_t *src) break; case MSP_SET_FW_CONFIG: - if (dataSize >= 12) { + if (dataSize == 12) { currentBatteryProfileMutable->nav.fw.cruise_throttle = sbufReadU16(src); currentBatteryProfileMutable->nav.fw.min_throttle = sbufReadU16(src); currentBatteryProfileMutable->nav.fw.max_throttle = sbufReadU16(src); @@ -2274,7 +2274,7 @@ static mspResult_e mspFcProcessInCommand(uint16_t cmdMSP, sbuf_t *src) break; case MSP_SET_POSITION_ESTIMATION_CONFIG: - if (dataSize >= 12) { + if (dataSize == 12) { positionEstimationConfigMutable()->w_z_baro_p = constrainf(sbufReadU16(src) / 100.0f, 0.0f, 10.0f); positionEstimationConfigMutable()->w_z_gps_p = constrainf(sbufReadU16(src) / 100.0f, 0.0f, 10.0f); positionEstimationConfigMutable()->w_z_gps_v = constrainf(sbufReadU16(src) / 100.0f, 0.0f, 10.0f); @@ -2448,7 +2448,7 @@ static mspResult_e mspFcProcessInCommand(uint16_t cmdMSP, sbuf_t *src) #ifdef USE_GPS case MSP_SET_RAW_GPS: - if (dataSize >= 14) { + if (dataSize == 14) { if (sbufReadU8(src)) { ENABLE_STATE(GPS_FIX); } else { @@ -2476,7 +2476,7 @@ static mspResult_e mspFcProcessInCommand(uint16_t cmdMSP, sbuf_t *src) #endif case MSP_SET_WP: - if (dataSize >= 21) { + if (dataSize == 21) { const uint8_t msp_wp_no = sbufReadU8(src); // get the waypoint number navWaypoint_t msp_wp; msp_wp.action = sbufReadU8(src); // action @@ -2492,7 +2492,7 @@ static mspResult_e mspFcProcessInCommand(uint16_t cmdMSP, sbuf_t *src) return MSP_RESULT_ERROR; break; case MSP2_COMMON_SET_RADAR_POS: - if (dataSize >= 19) { + if (dataSize == 19) { const uint8_t msp_radar_no = MIN(sbufReadU8(src), RADAR_MAX_POIS - 1); // Radar poi number, 0 to 3 radar_pois[msp_radar_no].state = sbufReadU8(src); // 0=undefined, 1=armed, 2=lost radar_pois[msp_radar_no].gps.lat = sbufReadU32(src); // lat 10E7 @@ -2506,7 +2506,7 @@ static mspResult_e mspFcProcessInCommand(uint16_t cmdMSP, sbuf_t *src) break; case MSP_SET_FEATURE: - if (dataSize >= 4) { + if (dataSize == 4) { featureClearAll(); featureSet(sbufReadU32(src)); // features bitmap rxUpdateRSSISource(); // For FEATURE_RSSI_ADC @@ -2515,7 +2515,7 @@ static mspResult_e mspFcProcessInCommand(uint16_t cmdMSP, sbuf_t *src) break; case MSP_SET_BOARD_ALIGNMENT: - if (dataSize >= 6) { + if (dataSize == 6) { boardAlignmentMutable()->rollDeciDegrees = sbufReadU16(src); boardAlignmentMutable()->pitchDeciDegrees = sbufReadU16(src); boardAlignmentMutable()->yawDeciDegrees = sbufReadU16(src); @@ -2524,7 +2524,7 @@ static mspResult_e mspFcProcessInCommand(uint16_t cmdMSP, sbuf_t *src) break; case MSP_SET_VOLTAGE_METER_CONFIG: - if (dataSize >= 4) { + if (dataSize == 4) { #ifdef USE_ADC batteryMetersConfigMutable()->voltage.scale = sbufReadU8(src) * 10; currentBatteryProfileMutable->voltage.cellMin = sbufReadU8(src) * 10; @@ -2541,7 +2541,7 @@ static mspResult_e mspFcProcessInCommand(uint16_t cmdMSP, sbuf_t *src) break; case MSP_SET_CURRENT_METER_CONFIG: - if (dataSize >= 7) { + if (dataSize == 7) { batteryMetersConfigMutable()->current.scale = sbufReadU16(src); batteryMetersConfigMutable()->current.offset = sbufReadU16(src); batteryMetersConfigMutable()->current.type = sbufReadU8(src); @@ -2551,7 +2551,7 @@ static mspResult_e mspFcProcessInCommand(uint16_t cmdMSP, sbuf_t *src) break; case MSP_SET_MIXER: - if (dataSize >= 1) { + if (dataSize == 1) { sbufReadU8(src); //This is ignored, no longer supporting mixerMode mixerUpdateStateFlags(); // Required for correct preset functionality } else @@ -2559,7 +2559,7 @@ static mspResult_e mspFcProcessInCommand(uint16_t cmdMSP, sbuf_t *src) break; case MSP_SET_RX_CONFIG: - if (dataSize >= 24) { + if (dataSize == 24) { rxConfigMutable()->serialrx_provider = sbufReadU8(src); rxConfigMutable()->maxcheck = sbufReadU16(src); sbufReadU16(src); // midrc @@ -2590,7 +2590,7 @@ static mspResult_e mspFcProcessInCommand(uint16_t cmdMSP, sbuf_t *src) break; case MSP_SET_FAILSAFE_CONFIG: - if (dataSize >= 20) { + if (dataSize == 20) { failsafeConfigMutable()->failsafe_delay = sbufReadU8(src); failsafeConfigMutable()->failsafe_off_delay = sbufReadU8(src); currentBatteryProfileMutable->failsafe_throttle = sbufReadU16(src); @@ -2610,7 +2610,7 @@ static mspResult_e mspFcProcessInCommand(uint16_t cmdMSP, sbuf_t *src) case MSP_SET_RSSI_CONFIG: sbufReadU8Safe(&tmp_u8, src); - if ((dataSize >= 1) && (tmp_u8 <= MAX_SUPPORTED_RC_CHANNEL_COUNT)) { + if ((dataSize == 1) && (tmp_u8 <= MAX_SUPPORTED_RC_CHANNEL_COUNT)) { rxConfigMutable()->rssi_channel = tmp_u8; rxUpdateRSSISource(); } else { @@ -2619,7 +2619,7 @@ static mspResult_e mspFcProcessInCommand(uint16_t cmdMSP, sbuf_t *src) break; case MSP_SET_RX_MAP: - if (dataSize >= MAX_MAPPABLE_RX_INPUTS) { + if (dataSize == MAX_MAPPABLE_RX_INPUTS) { for (int i = 0; i < MAX_MAPPABLE_RX_INPUTS; i++) { rxConfigMutable()->rcmap[i] = sbufReadU8(src); } @@ -2657,7 +2657,7 @@ static mspResult_e mspFcProcessInCommand(uint16_t cmdMSP, sbuf_t *src) #ifdef USE_LED_STRIP case MSP_SET_LED_COLORS: - if (dataSize >= LED_CONFIGURABLE_COLOR_COUNT * 4) { + if (dataSize == LED_CONFIGURABLE_COLOR_COUNT * 4) { for (int i = 0; i < LED_CONFIGURABLE_COLOR_COUNT; i++) { hsvColor_t *color = &ledStripConfigMutable()->colors[i]; color->h = sbufReadU16(src); @@ -2669,7 +2669,7 @@ static mspResult_e mspFcProcessInCommand(uint16_t cmdMSP, sbuf_t *src) break; case MSP_SET_LED_STRIP_CONFIG: - if (dataSize >= 5) { + if (dataSize == 5) { tmp_u8 = sbufReadU8(src); if (tmp_u8 >= LED_MAX_STRIP_LENGTH || dataSize != (1 + 4)) { return MSP_RESULT_ERROR; @@ -2682,7 +2682,7 @@ static mspResult_e mspFcProcessInCommand(uint16_t cmdMSP, sbuf_t *src) break; case MSP_SET_LED_STRIP_MODECOLOR: - if (dataSize >= 3) { + if (dataSize == 3) { ledModeIndex_e modeIdx = sbufReadU8(src); int funIdx = sbufReadU8(src); int color = sbufReadU8(src); @@ -2709,7 +2709,7 @@ static mspResult_e mspFcProcessInCommand(uint16_t cmdMSP, sbuf_t *src) #endif case MSP_SET_RTC: - if (dataSize >= 6) { + if (dataSize == 6) { // Use seconds and milliseconds to make senders // easier to implement. Generating a 64 bit value // might not be trivial in some platforms. @@ -2754,14 +2754,17 @@ static mspResult_e mspFcProcessInCommand(uint16_t cmdMSP, sbuf_t *src) break; case MSP2_INAV_SET_MIXER: - mixerConfigMutable()->motorDirectionInverted = sbufReadU8(src); - sbufReadU16(src); // Was yaw_jump_prevention_limit - mixerConfigMutable()->platformType = sbufReadU8(src); - mixerConfigMutable()->hasFlaps = sbufReadU8(src); - mixerConfigMutable()->appliedMixerPreset = sbufReadU16(src); - sbufReadU8(src); //Read and ignore MAX_SUPPORTED_MOTORS - sbufReadU8(src); //Read and ignore MAX_SUPPORTED_SERVOS - mixerUpdateStateFlags(); + if (dataSize == 9) { + mixerConfigMutable()->motorDirectionInverted = sbufReadU8(src); + sbufReadU16(src); // Was yaw_jump_prevention_limit + mixerConfigMutable()->platformType = sbufReadU8(src); + mixerConfigMutable()->hasFlaps = sbufReadU8(src); + mixerConfigMutable()->appliedMixerPreset = sbufReadU16(src); + sbufReadU8(src); //Read and ignore MAX_SUPPORTED_MOTORS + sbufReadU8(src); //Read and ignore MAX_SUPPORTED_SERVOS + mixerUpdateStateFlags(); + } else + return MSP_RESULT_ERROR; break; #if defined(USE_OSD)