rusefi
/
atbetaflight
mirror of https://github.com/rusefi/atbetaflight.git
1
0
Fork 0
Code Issues Packages Projects Releases Wiki Activity

Merge pull request #1520 from martinbudden/bf_msp_align2 

Minor code tidy for easier comparison with iNav
This commit is contained in:
Martin Budden 2016-11-10 16:59:07 +01:00 committed by GitHub
parent 0415285475 020bbacf03
commit 8e2457b090
1 changed files with 70 additions and 69 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

139
src/main/fc/fc_msp.c
View File

@ -85,6 +85,7 @@
#include "telemetry/telemetry.h" #include "telemetry/telemetry.h"
#include "flight/mixer.h" #include "flight/mixer.h"
#include "flight/servos.h"
#include "flight/pid.h" #include "flight/pid.h"
#include "flight/imu.h" #include "flight/imu.h"
#include "flight/failsafe.h" #include "flight/failsafe.h"
@ -100,9 +101,6 @@
#include "hardware_revision.h" #include "hardware_revision.h"
#endif #endif
#include "io/serial_4way.h"
extern uint16_t cycleTime; // FIXME dependency on mw.c extern uint16_t cycleTime; // FIXME dependency on mw.c
extern void resetProfile(profile_t *profile); extern void resetProfile(profile_t *profile);
@ -110,7 +108,7 @@ static const char * const flightControllerIdentifier = BETAFLIGHT_IDENTIFIER; //
static const char * const boardIdentifier = TARGET_BOARD_IDENTIFIER; static const char * const boardIdentifier = TARGET_BOARD_IDENTIFIER;
typedef struct box_e { typedef struct box_e {
const uint8_t boxId; // see boxId_e const uint8_t boxId; // see boxId_e
const char *boxName; // GUI-readable box name const char *boxName; // GUI-readable box name
const uint8_t permanentId; // const uint8_t permanentId; //
} box_t; } box_t;
@ -176,6 +174,9 @@ typedef enum {
MSP_SDCARD_STATE_READY = 4 MSP_SDCARD_STATE_READY = 4
} mspSDCardState_e; } mspSDCardState_e;
typedef enum {
MSP_SDCARD_FLAG_SUPPORTTED = 1,
} mspSDCardFlags_e;
#define RATEPROFILE_MASK (1 << 7) #define RATEPROFILE_MASK (1 << 7)
@ -205,8 +206,9 @@ static void mspRebootFn(serialPort_t *serialPort)
static void serializeNames(sbuf_t *dst, const char *s) static void serializeNames(sbuf_t *dst, const char *s)
{ {
const char *c; const char *c;
for (c = s; *c; c++) for (c = s; *c; c++) {
sbufWriteU8(dst, *c); sbufWriteU8(dst, *c);
}
} }
static const box_t *findBoxByActiveBoxId(uint8_t activeBoxId) static const box_t *findBoxByActiveBoxId(uint8_t activeBoxId)
@ -237,16 +239,15 @@ static const box_t *findBoxByPermenantId(uint8_t permenantId)
static void serializeBoxNamesReply(sbuf_t *dst) static void serializeBoxNamesReply(sbuf_t *dst)
{ {
int i, activeBoxId, j, flag = 1, count = 0, len; int activeBoxId, flag = 1, count = 0, len;
const box_t *box;
reset: reset:
// in first run of the loop, we grab total size of junk to be sent // in first run of the loop, we grab total size of junk to be sent
// then come back and actually send it // then come back and actually send it
for (i = 0; i < activeBoxIdCount; i++) { for (int i = 0; i < activeBoxIdCount; i++) {
activeBoxId = activeBoxIds[i]; activeBoxId = activeBoxIds[i];
box = findBoxByActiveBoxId(activeBoxId); const box_t *box = findBoxByActiveBoxId(activeBoxId);
if (!box) { if (!box) {
continue; continue;
} }
@ -255,8 +256,9 @@ reset:
if (flag) { if (flag) {
count += len; count += len;
} else { } else {
for (j = 0; j < len; j++) for (int j = 0; j < len; j++) {
sbufWriteU8(dst, box->boxName[j]); sbufWriteU8(dst, box->boxName[j]);
}
} }
} }
@ -275,7 +277,7 @@ void initActiveBoxIds(void)
activeBoxIds[activeBoxIdCount++] = BOXARM; activeBoxIds[activeBoxIdCount++] = BOXARM;
if (!feature(FEATURE_AIRMODE)) { if (!feature(FEATURE_AIRMODE)) {
activeBoxIds[activeBoxIdCount++] = BOXAIRMODE; activeBoxIds[activeBoxIdCount++] = BOXAIRMODE;
} }
if (sensors(SENSOR_ACC)) { if (sensors(SENSOR_ACC)) {
@ -331,7 +333,7 @@ void initActiveBoxIds(void)
activeBoxIds[activeBoxIdCount++] = BOXFPVANGLEMIX; activeBoxIds[activeBoxIdCount++] = BOXFPVANGLEMIX;
if (feature(FEATURE_3D)) { if (feature(FEATURE_3D)) {
activeBoxIds[activeBoxIdCount++] = BOX3DDISABLESWITCH; activeBoxIds[activeBoxIdCount++] = BOX3DDISABLESWITCH;
} }
if (feature(FEATURE_SERVO_TILT)) { if (feature(FEATURE_SERVO_TILT)) {
@ -343,7 +345,7 @@ void initActiveBoxIds(void)
} }
if (feature(FEATURE_OSD)) { if (feature(FEATURE_OSD)) {
activeBoxIds[activeBoxIdCount++] = BOXOSD; activeBoxIds[activeBoxIdCount++] = BOXOSD;
} }
#ifdef TELEMETRY #ifdef TELEMETRY
@ -369,13 +371,10 @@ void initActiveBoxIds(void)
static uint32_t packFlightModeFlags(void) static uint32_t packFlightModeFlags(void)
{ {
uint32_t i, junk, tmp;
// Serialize the flags in the order we delivered them, ignoring BOXNAMES and BOXINDEXES // Serialize the flags in the order we delivered them, ignoring BOXNAMES and BOXINDEXES
// Requires new Multiwii protocol version to fix // Requires new Multiwii protocol version to fix
// It would be preferable to setting the enabled bits based on BOXINDEX. // It would be preferable to setting the enabled bits based on BOXINDEX.
junk = 0; const uint32_t tmp = IS_ENABLED(FLIGHT_MODE(ANGLE_MODE)) << BOXANGLE |
tmp = IS_ENABLED(FLIGHT_MODE(ANGLE_MODE)) << BOXANGLE |
IS_ENABLED(FLIGHT_MODE(HORIZON_MODE)) << BOXHORIZON | IS_ENABLED(FLIGHT_MODE(HORIZON_MODE)) << BOXHORIZON |
IS_ENABLED(FLIGHT_MODE(BARO_MODE)) << BOXBARO | IS_ENABLED(FLIGHT_MODE(BARO_MODE)) << BOXBARO |
IS_ENABLED(FLIGHT_MODE(MAG_MODE)) << BOXMAG | IS_ENABLED(FLIGHT_MODE(MAG_MODE)) << BOXMAG |
@ -402,19 +401,20 @@ static uint32_t packFlightModeFlags(void)
IS_ENABLED(IS_RC_MODE_ACTIVE(BOXAIRMODE)) << BOXAIRMODE | IS_ENABLED(IS_RC_MODE_ACTIVE(BOXAIRMODE)) << BOXAIRMODE |
IS_ENABLED(IS_RC_MODE_ACTIVE(BOXFPVANGLEMIX)) << BOXFPVANGLEMIX; IS_ENABLED(IS_RC_MODE_ACTIVE(BOXFPVANGLEMIX)) << BOXFPVANGLEMIX;
for (i = 0; i < activeBoxIdCount; i++) { uint32_t ret = 0;
int flag = (tmp & (1 << activeBoxIds[i])); for (int i = 0; i < activeBoxIdCount; i++) {
if (flag) const uint32_t flag = (tmp & (1 << activeBoxIds[i]));
junk |= 1 << i; if (flag) {
ret |= 1 << i;
}
} }
return ret;
return junk;
} }
static void serializeSDCardSummaryReply(sbuf_t *dst) static void serializeSDCardSummaryReply(sbuf_t *dst)
{ {
#ifdef USE_SDCARD #ifdef USE_SDCARD
uint8_t flags = 1 /* SD card supported */ ; uint8_t flags = MSP_SDCARD_FLAG_SUPPORTTED;
uint8_t state = 0; uint8_t state = 0;
sbufWriteU8(dst, flags); sbufWriteU8(dst, flags);
@ -517,8 +517,6 @@ static void serializeDataflashReadReply(sbuf_t *dst, uint32_t address, const uin
*/ */
static bool mspFcProcessOutCommand(uint8_t cmdMSP, sbuf_t *dst, mspPostProcessFnPtr *mspPostProcessFn) static bool mspFcProcessOutCommand(uint8_t cmdMSP, sbuf_t *dst, mspPostProcessFnPtr *mspPostProcessFn)
{ {
uint32_t i;
switch (cmdMSP) { switch (cmdMSP) {
case MSP_API_VERSION: case MSP_API_VERSION:
sbufWriteU8(dst, MSP_PROTOCOL_VERSION); sbufWriteU8(dst, MSP_PROTOCOL_VERSION);
@ -527,7 +525,7 @@ static bool mspFcProcessOutCommand(uint8_t cmdMSP, sbuf_t *dst, mspPostProcessFn
break; break;
case MSP_FC_VARIANT: case MSP_FC_VARIANT:
for (i = 0; i < FLIGHT_CONTROLLER_IDENTIFIER_LENGTH; i++) { for (int i = 0; i < FLIGHT_CONTROLLER_IDENTIFIER_LENGTH; i++) {
sbufWriteU8(dst, flightControllerIdentifier[i]); sbufWriteU8(dst, flightControllerIdentifier[i]);
} }
break; break;
@ -539,7 +537,7 @@ static bool mspFcProcessOutCommand(uint8_t cmdMSP, sbuf_t *dst, mspPostProcessFn
break; break;
case MSP_BOARD_INFO: case MSP_BOARD_INFO:
for (i = 0; i < BOARD_IDENTIFIER_LENGTH; i++) { for (int i = 0; i < BOARD_IDENTIFIER_LENGTH; i++) {
sbufWriteU8(dst, boardIdentifier[i]); sbufWriteU8(dst, boardIdentifier[i]);
} }
#ifdef USE_HARDWARE_REVISION_DETECTION #ifdef USE_HARDWARE_REVISION_DETECTION
@ -550,14 +548,13 @@ static bool mspFcProcessOutCommand(uint8_t cmdMSP, sbuf_t *dst, mspPostProcessFn
break; break;
case MSP_BUILD_INFO: case MSP_BUILD_INFO:
for (i = 0; i < BUILD_DATE_LENGTH; i++) { for (int i = 0; i < BUILD_DATE_LENGTH; i++) {
sbufWriteU8(dst, buildDate[i]); sbufWriteU8(dst, buildDate[i]);
} }
for (i = 0; i < BUILD_TIME_LENGTH; i++) { for (int i = 0; i < BUILD_TIME_LENGTH; i++) {
sbufWriteU8(dst, buildTime[i]); sbufWriteU8(dst, buildTime[i]);
} }
for (int i = 0; i < GIT_SHORT_REVISION_LENGTH; i++) {
for (i = 0; i < GIT_SHORT_REVISION_LENGTH; i++) {
sbufWriteU8(dst, shortGitRevision[i]); sbufWriteU8(dst, shortGitRevision[i]);
} }
break; break;
@ -588,7 +585,7 @@ static bool mspFcProcessOutCommand(uint8_t cmdMSP, sbuf_t *dst, mspPostProcessFn
case MSP_NAME: case MSP_NAME:
{ {
const unsigned int nameLen = strlen(masterConfig.name); const unsigned int nameLen = strlen(masterConfig.name);
for (i = 0; i < nameLen; i++) { for (int i = 0; i < nameLen; i++) {
sbufWriteU8(dst, masterConfig.name[i]); sbufWriteU8(dst, masterConfig.name[i]);
} }
} }
@ -610,20 +607,24 @@ static bool mspFcProcessOutCommand(uint8_t cmdMSP, sbuf_t *dst, mspPostProcessFn
{ {
// Hack scale due to choice of units for sensor data in multiwii // Hack scale due to choice of units for sensor data in multiwii
const uint8_t scale = (acc.acc_1G > 512) ? 4 : 1; const uint8_t scale = (acc.acc_1G > 512) ? 4 : 1;
for (i = 0; i < 3; i++) for (int i = 0; i < 3; i++) {
sbufWriteU16(dst, accSmooth[i] / scale); sbufWriteU16(dst, accSmooth[i] / scale);
for (i = 0; i < 3; i++) }
for (int i = 0; i < 3; i++) {
sbufWriteU16(dst, gyroADC[i]); sbufWriteU16(dst, gyroADC[i]);
for (i = 0; i < 3; i++) }
for (int i = 0; i < 3; i++) {
sbufWriteU16(dst, magADC[i]); sbufWriteU16(dst, magADC[i]);
}
} }
break; break;
#ifdef USE_SERVOS #ifdef USE_SERVOS
case MSP_SERVO: case MSP_SERVO:
sbufWriteData(dst, &servo, MAX_SUPPORTED_SERVOS * 2); sbufWriteData(dst, &servo, MAX_SUPPORTED_SERVOS * 2);
break; break;
case MSP_SERVO_CONFIGURATIONS: case MSP_SERVO_CONFIGURATIONS:
for (i = 0; i < MAX_SUPPORTED_SERVOS; i++) { for (int i = 0; i < MAX_SUPPORTED_SERVOS; i++) {
sbufWriteU16(dst, masterConfig.servoConf[i].min); sbufWriteU16(dst, masterConfig.servoConf[i].min);
sbufWriteU16(dst, masterConfig.servoConf[i].max); sbufWriteU16(dst, masterConfig.servoConf[i].max);
sbufWriteU16(dst, masterConfig.servoConf[i].middle); sbufWriteU16(dst, masterConfig.servoConf[i].middle);
@ -635,7 +636,7 @@ static bool mspFcProcessOutCommand(uint8_t cmdMSP, sbuf_t *dst, mspPostProcessFn
} }
break; break;
case MSP_SERVO_MIX_RULES: case MSP_SERVO_MIX_RULES:
for (i = 0; i < MAX_SERVO_RULES; i++) { for (int i = 0; i < MAX_SERVO_RULES; i++) {
sbufWriteU8(dst, masterConfig.customServoMixer[i].targetChannel); sbufWriteU8(dst, masterConfig.customServoMixer[i].targetChannel);
sbufWriteU8(dst, masterConfig.customServoMixer[i].inputSource); sbufWriteU8(dst, masterConfig.customServoMixer[i].inputSource);
sbufWriteU8(dst, masterConfig.customServoMixer[i].rate); sbufWriteU8(dst, masterConfig.customServoMixer[i].rate);
@ -659,8 +660,9 @@ static bool mspFcProcessOutCommand(uint8_t cmdMSP, sbuf_t *dst, mspPostProcessFn
break; break;
case MSP_RC: case MSP_RC:
for (i = 0; i < rxRuntimeConfig.channelCount; i++) for (int i = 0; i < rxRuntimeConfig.channelCount; i++) {
sbufWriteU16(dst, rcData[i]); sbufWriteU16(dst, rcData[i]);
}
break; break;
case MSP_ATTITUDE: case MSP_ATTITUDE:
@ -708,7 +710,7 @@ static bool mspFcProcessOutCommand(uint8_t cmdMSP, sbuf_t *dst, mspPostProcessFn
case MSP_RC_TUNING: case MSP_RC_TUNING:
sbufWriteU8(dst, currentControlRateProfile->rcRate8); sbufWriteU8(dst, currentControlRateProfile->rcRate8);
sbufWriteU8(dst, currentControlRateProfile->rcExpo8); sbufWriteU8(dst, currentControlRateProfile->rcExpo8);
for (i = 0 ; i < 3; i++) { for (int i = 0 ; i < 3; i++) {
sbufWriteU8(dst, currentControlRateProfile->rates[i]); // R,P,Y see flight_dynamics_index_t sbufWriteU8(dst, currentControlRateProfile->rates[i]); // R,P,Y see flight_dynamics_index_t
} }
sbufWriteU8(dst, currentControlRateProfile->dynThrPID); sbufWriteU8(dst, currentControlRateProfile->dynThrPID);
@ -720,7 +722,7 @@ static bool mspFcProcessOutCommand(uint8_t cmdMSP, sbuf_t *dst, mspPostProcessFn
break; break;
case MSP_PID: case MSP_PID:
for (i = 0; i < PID_ITEM_COUNT; i++) { for (int i = 0; i < PID_ITEM_COUNT; i++) {
sbufWriteU8(dst, currentProfile->pidProfile.P8[i]); sbufWriteU8(dst, currentProfile->pidProfile.P8[i]);
sbufWriteU8(dst, currentProfile->pidProfile.I8[i]); sbufWriteU8(dst, currentProfile->pidProfile.I8[i]);
sbufWriteU8(dst, currentProfile->pidProfile.D8[i]); sbufWriteU8(dst, currentProfile->pidProfile.D8[i]);
@ -736,7 +738,7 @@ static bool mspFcProcessOutCommand(uint8_t cmdMSP, sbuf_t *dst, mspPostProcessFn
break; break;
case MSP_MODE_RANGES: case MSP_MODE_RANGES:
for (i = 0; i < MAX_MODE_ACTIVATION_CONDITION_COUNT; i++) { for (int i = 0; i < MAX_MODE_ACTIVATION_CONDITION_COUNT; i++) {
modeActivationCondition_t *mac = &masterConfig.modeActivationConditions[i]; modeActivationCondition_t *mac = &masterConfig.modeActivationConditions[i];
const box_t *box = &boxes[mac->modeId]; const box_t *box = &boxes[mac->modeId];
sbufWriteU8(dst, box->permanentId); sbufWriteU8(dst, box->permanentId);
@ -747,7 +749,7 @@ static bool mspFcProcessOutCommand(uint8_t cmdMSP, sbuf_t *dst, mspPostProcessFn
break; break;
case MSP_ADJUSTMENT_RANGES: case MSP_ADJUSTMENT_RANGES:
for (i = 0; i < MAX_ADJUSTMENT_RANGE_COUNT; i++) { for (int i = 0; i < MAX_ADJUSTMENT_RANGE_COUNT; i++) {
adjustmentRange_t *adjRange = &masterConfig.adjustmentRanges[i]; adjustmentRange_t *adjRange = &masterConfig.adjustmentRanges[i];
sbufWriteU8(dst, adjRange->adjustmentIndex); sbufWriteU8(dst, adjRange->adjustmentIndex);
sbufWriteU8(dst, adjRange->auxChannelIndex); sbufWriteU8(dst, adjRange->auxChannelIndex);
@ -763,7 +765,7 @@ static bool mspFcProcessOutCommand(uint8_t cmdMSP, sbuf_t *dst, mspPostProcessFn
break; break;
case MSP_BOXIDS: case MSP_BOXIDS:
for (i = 0; i < activeBoxIdCount; i++) { for (int i = 0; i < activeBoxIdCount; i++) {
const box_t *box = findBoxByActiveBoxId(activeBoxIds[i]); const box_t *box = findBoxByActiveBoxId(activeBoxIds[i]);
if (!box) { if (!box) {
continue; continue;
@ -804,8 +806,9 @@ static bool mspFcProcessOutCommand(uint8_t cmdMSP, sbuf_t *dst, mspPostProcessFn
case MSP_MOTOR_PINS: case MSP_MOTOR_PINS:
// FIXME This is hardcoded and should not be. // FIXME This is hardcoded and should not be.
for (i = 0; i < 8; i++) for (int i = 0; i < 8; i++) {
sbufWriteU8(dst, i + 1); sbufWriteU8(dst, i + 1);
}
break; break;
#ifdef GPS #ifdef GPS
@ -827,7 +830,7 @@ static bool mspFcProcessOutCommand(uint8_t cmdMSP, sbuf_t *dst, mspPostProcessFn
case MSP_GPSSVINFO: case MSP_GPSSVINFO:
sbufWriteU8(dst, GPS_numCh); sbufWriteU8(dst, GPS_numCh);
for (i = 0; i < GPS_numCh; i++){ for (int i = 0; i < GPS_numCh; i++) {
sbufWriteU8(dst, GPS_svinfo_chn[i]); sbufWriteU8(dst, GPS_svinfo_chn[i]);
sbufWriteU8(dst, GPS_svinfo_svid[i]); sbufWriteU8(dst, GPS_svinfo_svid[i]);
sbufWriteU8(dst, GPS_svinfo_quality[i]); sbufWriteU8(dst, GPS_svinfo_quality[i]);
@ -840,8 +843,9 @@ static bool mspFcProcessOutCommand(uint8_t cmdMSP, sbuf_t *dst, mspPostProcessFn
// output some useful QA statistics // output some useful QA statistics
// debug[x] = ((hse_value / 1000000) * 1000) + (SystemCoreClock / 1000000); // XX0YY [crystal clock : core clock] // debug[x] = ((hse_value / 1000000) * 1000) + (SystemCoreClock / 1000000); // XX0YY [crystal clock : core clock]
for (i = 0; i < DEBUG16_VALUE_COUNT; i++) for (int i = 0; i < DEBUG16_VALUE_COUNT; i++) {
sbufWriteU16(dst, debug[i]); // 4 variables are here for general monitoring purpose sbufWriteU16(dst, debug[i]); // 4 variables are here for general monitoring purpose
}
break; break;
// Additional commands that are not compatible with MultiWii // Additional commands that are not compatible with MultiWii
@ -910,7 +914,7 @@ static bool mspFcProcessOutCommand(uint8_t cmdMSP, sbuf_t *dst, mspPostProcessFn
break; break;
case MSP_RXFAIL_CONFIG: case MSP_RXFAIL_CONFIG:
for (i = 0; i < rxRuntimeConfig.channelCount; i++) { for (int i = 0; i < rxRuntimeConfig.channelCount; i++) {
sbufWriteU8(dst, masterConfig.rxConfig.failsafe_channel_configurations[i].mode); sbufWriteU8(dst, masterConfig.rxConfig.failsafe_channel_configurations[i].mode);
sbufWriteU16(dst, RXFAIL_STEP_TO_CHANNEL_VALUE(masterConfig.rxConfig.failsafe_channel_configurations[i].step)); sbufWriteU16(dst, RXFAIL_STEP_TO_CHANNEL_VALUE(masterConfig.rxConfig.failsafe_channel_configurations[i].step));
} }
@ -921,8 +925,9 @@ static bool mspFcProcessOutCommand(uint8_t cmdMSP, sbuf_t *dst, mspPostProcessFn
break; break;
case MSP_RX_MAP: case MSP_RX_MAP:
for (i = 0; i < MAX_MAPPABLE_RX_INPUTS; i++) for (int i = 0; i < MAX_MAPPABLE_RX_INPUTS; i++) {
sbufWriteU8(dst, masterConfig.rxConfig.rcmap[i]); sbufWriteU8(dst, masterConfig.rxConfig.rcmap[i]);
}
break; break;
case MSP_BF_CONFIG: case MSP_BF_CONFIG:
@ -941,7 +946,7 @@ static bool mspFcProcessOutCommand(uint8_t cmdMSP, sbuf_t *dst, mspPostProcessFn
break; break;
case MSP_CF_SERIAL_CONFIG: case MSP_CF_SERIAL_CONFIG:
for (i = 0; i < SERIAL_PORT_COUNT; i++) { for (int i = 0; i < SERIAL_PORT_COUNT; i++) {
if (!serialIsPortAvailable(masterConfig.serialConfig.portConfigs[i].identifier)) { if (!serialIsPortAvailable(masterConfig.serialConfig.portConfigs[i].identifier)) {
continue; continue;
}; };
@ -956,7 +961,7 @@ static bool mspFcProcessOutCommand(uint8_t cmdMSP, sbuf_t *dst, mspPostProcessFn
#ifdef LED_STRIP #ifdef LED_STRIP
case MSP_LED_COLORS: case MSP_LED_COLORS:
for (i = 0; i < LED_CONFIGURABLE_COLOR_COUNT; i++) { for (int i = 0; i < LED_CONFIGURABLE_COLOR_COUNT; i++) {
hsvColor_t *color = &masterConfig.ledStripConfig.colors[i]; hsvColor_t *color = &masterConfig.ledStripConfig.colors[i];
sbufWriteU16(dst, color->h); sbufWriteU16(dst, color->h);
sbufWriteU8(dst, color->s); sbufWriteU8(dst, color->s);
@ -965,7 +970,7 @@ static bool mspFcProcessOutCommand(uint8_t cmdMSP, sbuf_t *dst, mspPostProcessFn
break; break;
case MSP_LED_STRIP_CONFIG: case MSP_LED_STRIP_CONFIG:
for (i = 0; i < LED_MAX_STRIP_LENGTH; i++) { for (int i = 0; i < LED_MAX_STRIP_LENGTH; i++) {
ledConfig_t *ledConfig = &masterConfig.ledStripConfig.ledConfigs[i]; ledConfig_t *ledConfig = &masterConfig.ledStripConfig.ledConfigs[i];
sbufWriteU32(dst, *ledConfig); sbufWriteU32(dst, *ledConfig);
} }
@ -1017,7 +1022,7 @@ static bool mspFcProcessOutCommand(uint8_t cmdMSP, sbuf_t *dst, mspPostProcessFn
case MSP_TRANSPONDER_CONFIG: case MSP_TRANSPONDER_CONFIG:
#ifdef TRANSPONDER #ifdef TRANSPONDER
sbufWriteU8(dst, 1); //Transponder supported sbufWriteU8(dst, 1); //Transponder supported
for (i = 0; i < sizeof(masterConfig.transponderData); i++) { for (int i = 0; i < sizeof(masterConfig.transponderData); i++) {
sbufWriteU8(dst, masterConfig.transponderData[i]); sbufWriteU8(dst, masterConfig.transponderData[i]);
} }
#else #else
@ -1035,8 +1040,7 @@ static bool mspFcProcessOutCommand(uint8_t cmdMSP, sbuf_t *dst, mspPostProcessFn
sbufWriteU16(dst, masterConfig.osdProfile.cap_alarm); sbufWriteU16(dst, masterConfig.osdProfile.cap_alarm);
sbufWriteU16(dst, masterConfig.osdProfile.time_alarm); sbufWriteU16(dst, masterConfig.osdProfile.time_alarm);
sbufWriteU16(dst, masterConfig.osdProfile.alt_alarm); sbufWriteU16(dst, masterConfig.osdProfile.alt_alarm);
for (int i = 0; i < OSD_ITEM_COUNT; i++) {
for (i = 0; i < OSD_ITEM_COUNT; i++) {
sbufWriteU16(dst, masterConfig.osdProfile.item_pos[i]); sbufWriteU16(dst, masterConfig.osdProfile.item_pos[i]);
} }
#else #else
@ -1045,7 +1049,7 @@ static bool mspFcProcessOutCommand(uint8_t cmdMSP, sbuf_t *dst, mspPostProcessFn
break; break;
case MSP_BF_BUILD_INFO: case MSP_BF_BUILD_INFO:
for (i = 0; i < 11; i++) { for (int i = 0; i < 11; i++) {
sbufWriteU8(dst, buildDate[i]); // MMM DD YYYY as ascii, MMM = Jan/Feb... etc sbufWriteU8(dst, buildDate[i]); // MMM DD YYYY as ascii, MMM = Jan/Feb... etc
} }
sbufWriteU32(dst, 0); // future exp sbufWriteU32(dst, 0); // future exp
@ -1228,11 +1232,9 @@ static mspResult_e mspFcProcessInCommand(uint8_t cmdMSP, sbuf_t *src)
return MSP_RESULT_ERROR; return MSP_RESULT_ERROR;
} else { } else {
uint16_t frame[MAX_SUPPORTED_RC_CHANNEL_COUNT]; uint16_t frame[MAX_SUPPORTED_RC_CHANNEL_COUNT];
for (int i = 0; i < channelCount; i++) {
for (i = 0; i < channelCount; i++) {
frame[i] = sbufReadU16(src); frame[i] = sbufReadU16(src);
} }
rxMspFrameReceive(frame, channelCount); rxMspFrameReceive(frame, channelCount);
} }
} }
@ -1255,7 +1257,7 @@ static mspResult_e mspFcProcessInCommand(uint8_t cmdMSP, sbuf_t *src)
break; break;
case MSP_SET_PID: case MSP_SET_PID:
for (i = 0; i < PID_ITEM_COUNT; i++) { for (int i = 0; i < PID_ITEM_COUNT; i++) {
currentProfile->pidProfile.P8[i] = sbufReadU8(src); currentProfile->pidProfile.P8[i] = sbufReadU8(src);
currentProfile->pidProfile.I8[i] = sbufReadU8(src); currentProfile->pidProfile.I8[i] = sbufReadU8(src);
currentProfile->pidProfile.D8[i] = sbufReadU8(src); currentProfile->pidProfile.D8[i] = sbufReadU8(src);
@ -1307,7 +1309,7 @@ static mspResult_e mspFcProcessInCommand(uint8_t cmdMSP, sbuf_t *src)
if (dataSize >= 10) { if (dataSize >= 10) {
currentControlRateProfile->rcRate8 = sbufReadU8(src); currentControlRateProfile->rcRate8 = sbufReadU8(src);
currentControlRateProfile->rcExpo8 = sbufReadU8(src); currentControlRateProfile->rcExpo8 = sbufReadU8(src);
for (i = 0; i < 3; i++) { for (int i = 0; i < 3; i++) {
value = sbufReadU8(src); value = sbufReadU8(src);
currentControlRateProfile->rates[i] = MIN(value, i == FD_YAW ? CONTROL_RATE_CONFIG_YAW_RATE_MAX : CONTROL_RATE_CONFIG_ROLL_PITCH_RATE_MAX); currentControlRateProfile->rates[i] = MIN(value, i == FD_YAW ? CONTROL_RATE_CONFIG_YAW_RATE_MAX : CONTROL_RATE_CONFIG_ROLL_PITCH_RATE_MAX);
} }
@ -1360,7 +1362,7 @@ static mspResult_e mspFcProcessInCommand(uint8_t cmdMSP, sbuf_t *src)
break; break;
case MSP_SET_MOTOR: case MSP_SET_MOTOR:
for (i = 0; i < 8; i++) { // FIXME should this use MAX_MOTORS or MAX_SUPPORTED_MOTORS instead of 8 for (int i = 0; i < 8; i++) { // FIXME should this use MAX_MOTORS or MAX_SUPPORTED_MOTORS instead of 8
motor_disarmed[i] = convertExternalToMotor(sbufReadU16(src)); motor_disarmed[i] = convertExternalToMotor(sbufReadU16(src));
} }
break; break;
@ -1520,14 +1522,13 @@ static mspResult_e mspFcProcessInCommand(uint8_t cmdMSP, sbuf_t *src)
return MSP_RESULT_ERROR; return MSP_RESULT_ERROR;
break; break;
} }
for (int i = 0; i < sizeof(masterConfig.transponderData); i++) {
for (i = 0; i < sizeof(masterConfig.transponderData); i++) {
masterConfig.transponderData[i] = sbufReadU8(src); masterConfig.transponderData[i] = sbufReadU8(src);
} }
transponderUpdateData(masterConfig.transponderData); transponderUpdateData(masterConfig.transponderData);
break; break;
#endif #endif
#ifdef OSD #ifdef OSD
case MSP_SET_OSD_CONFIG: case MSP_SET_OSD_CONFIG:
addr = sbufReadU8(src); addr = sbufReadU8(src);
@ -1547,7 +1548,7 @@ static mspResult_e mspFcProcessInCommand(uint8_t cmdMSP, sbuf_t *src)
break; break;
case MSP_OSD_CHAR_WRITE: case MSP_OSD_CHAR_WRITE:
addr = sbufReadU8(src); addr = sbufReadU8(src);
for (i = 0; i < 54; i++) { for (int i = 0; i < 54; i++) {
font_data[i] = sbufReadU8(src); font_data[i] = sbufReadU8(src);
} }
max7456WriteNvm(addr, font_data); max7456WriteNvm(addr, font_data);
@ -1688,7 +1689,7 @@ static mspResult_e mspFcProcessInCommand(uint8_t cmdMSP, sbuf_t *src)
break; break;
case MSP_SET_RX_MAP: case MSP_SET_RX_MAP:
for (i = 0; i < MAX_MAPPABLE_RX_INPUTS; i++) { for (int i = 0; i < MAX_MAPPABLE_RX_INPUTS; i++) {
masterConfig.rxConfig.rcmap[i] = sbufReadU8(src); masterConfig.rxConfig.rcmap[i] = sbufReadU8(src);
} }
break; break;
@ -1745,7 +1746,7 @@ static mspResult_e mspFcProcessInCommand(uint8_t cmdMSP, sbuf_t *src)
#ifdef LED_STRIP #ifdef LED_STRIP
case MSP_SET_LED_COLORS: case MSP_SET_LED_COLORS:
for (i = 0; i < LED_CONFIGURABLE_COLOR_COUNT; i++) { for (int i = 0; i < LED_CONFIGURABLE_COLOR_COUNT; i++) {
hsvColor_t *color = &masterConfig.ledStripConfig.colors[i]; hsvColor_t *color = &masterConfig.ledStripConfig.colors[i];
color->h = sbufReadU16(src); color->h = sbufReadU16(src);
color->s = sbufReadU8(src); color->s = sbufReadU8(src);
@ -1780,7 +1781,7 @@ static mspResult_e mspFcProcessInCommand(uint8_t cmdMSP, sbuf_t *src)
case MSP_SET_NAME: case MSP_SET_NAME:
memset(masterConfig.name, 0, ARRAYLEN(masterConfig.name)); memset(masterConfig.name, 0, ARRAYLEN(masterConfig.name));
for (i = 0; i < MIN(MAX_NAME_LENGTH, dataSize); i++) { for (int i = 0; i < MIN(MAX_NAME_LENGTH, dataSize); i++) {
masterConfig.name[i] = sbufReadU8(src); masterConfig.name[i] = sbufReadU8(src);
} }
break; break;