Skip to content

refactor: simplify and reduce flash usage in osd.c and fc_msp.c via shared helpers #11573

New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Open
sensei-hacker wants to merge 8 commits into
base: maintenance-10.x
Choose a base branch
from
+175 −359
Open

refactor: simplify and reduce flash usage in osd.c and fc_msp.c via shared helpers #11573

Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
8 commits
Select commit Hold shift + click to select a range
f7fdb06
refactor: extract repeated switch-case bodies in osd.c and fc_msp.c
sensei-hacker May 19, 2026
d14ac4b
refactor: extract mspDeserializeServoParams helper in fc_msp.c
sensei-hacker May 19, 2026
814d1d5
refactor: extract shared rendering helpers in osd.c
sensei-hacker May 19, 2026
2c459d9
refactor: add sbufWriteAxisU16, sbufReadAxisU16, mspReadRates helpers
sensei-hacker May 19, 2026
6a754d4
fix: address code review findings
sensei-hacker May 20, 2026
fc292b0
refactor: simplify osdFormatVelocityStr and add mspSerializeServoParams
sensei-hacker May 20, 2026
e667465
fix: address code review findings (post-rebase)
sensei-hacker May 20, 2026
8de6b22
revert: restore sbufWriteU8(dst, -1) in mspSerializeServoMixer
sensei-hacker May 20, 2026
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
170 changes: 80 additions & 90 deletions src/main/fc/fc_msp.c
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -383,6 +383,68 @@ static void serializeDataflashReadReply(sbuf_t *dst, uint32_t address, uint16_t
}
#endif

// Calibration data is stored as int16_t but transmitted as raw bytes via sbufWriteU16.
static void sbufWriteAxisU16(sbuf_t *dst, const int16_t *arr)
{
sbufWriteU16(dst, arr[X]);
sbufWriteU16(dst, arr[Y]);
sbufWriteU16(dst, arr[Z]);
}

static void sbufReadAxisU16(sbuf_t *src, int16_t *arr)
{
arr[X] = sbufReadU16(src);
arr[Y] = sbufReadU16(src);
arr[Z] = sbufReadU16(src);
}

static void mspReadRates(sbuf_t *src, uint8_t *rates)
{
for (int i = 0; i < 3; ++i) {
uint8_t v = sbufReadU8(src);
rates[i] = (i == FD_YAW) ? constrain(v, SETTING_YAW_RATE_MIN, SETTING_YAW_RATE_MAX)
: constrain(v, SETTING_CONSTANT_ROLL_PITCH_RATE_MIN, SETTING_CONSTANT_ROLL_PITCH_RATE_MAX);
}
}

static void mspDeserializeServoParams(sbuf_t *src, uint8_t servoIndex)
{
servoParamsMutable(servoIndex)->min = sbufReadU16(src);
servoParamsMutable(servoIndex)->max = sbufReadU16(src);
servoParamsMutable(servoIndex)->middle = sbufReadU16(src);
servoParamsMutable(servoIndex)->rate = sbufReadU8(src);
servoComputeScalingFactors(servoIndex);
}

static void mspSerializeServoParams(sbuf_t *dst, const servoParam_t *sp)
{
sbufWriteU16(dst, sp->min);
sbufWriteU16(dst, sp->max);
sbufWriteU16(dst, sp->middle);
sbufWriteU8(dst, sp->rate);
}

static void mspSerializeMotorMixer(sbuf_t *dst, const motorMixer_t *m)
{
sbufWriteU16(dst, constrainf(m->throttle + 2.0f, 0.0f, 4.0f) * 1000);
sbufWriteU16(dst, constrainf(m->roll + 2.0f, 0.0f, 4.0f) * 1000);
sbufWriteU16(dst, constrainf(m->pitch + 2.0f, 0.0f, 4.0f) * 1000);
sbufWriteU16(dst, constrainf(m->yaw + 2.0f, 0.0f, 4.0f) * 1000);
}

static void mspSerializeServoMixer(sbuf_t *dst, const servoMixer_t *m)
{
sbufWriteU8(dst, m->targetChannel);
sbufWriteU8(dst, m->inputSource);
sbufWriteU16(dst, m->rate);
sbufWriteU8(dst, m->speed);
#ifdef USE_PROGRAMMING_FRAMEWORK
sbufWriteU8(dst, m->conditionId);
#else
sbufWriteU8(dst, -1);
#endif
}

/*
* Returns true if the command was processd, false otherwise.
* May set mspPostProcessFunc to a function to be called once the command has been processed
Expand Down Expand Up @@ -538,10 +600,7 @@ static bool mspFcProcessOutCommand(uint16_t cmdMSP, sbuf_t *dst, mspPostProcessF
break;
case MSP_SERVO_CONFIGURATIONS:
for (int i = 0; i < MAX_SUPPORTED_SERVOS; i++) {
sbufWriteU16(dst, servoParams(i)->min);
sbufWriteU16(dst, servoParams(i)->max);
sbufWriteU16(dst, servoParams(i)->middle);
sbufWriteU8(dst, servoParams(i)->rate);
mspSerializeServoParams(dst, servoParams(i));
sbufWriteU8(dst, 0);
sbufWriteU8(dst, 0);
sbufWriteU8(dst, 255); // used to be forwardFromChannel, not used anymore, send 0xff for compatibility reasons
Expand All @@ -550,10 +609,7 @@ static bool mspFcProcessOutCommand(uint16_t cmdMSP, sbuf_t *dst, mspPostProcessF
break;
case MSP2_INAV_SERVO_CONFIG:
for (int i = 0; i < MAX_SUPPORTED_SERVOS; i++) {
sbufWriteU16(dst, servoParams(i)->min);
sbufWriteU16(dst, servoParams(i)->max);
sbufWriteU16(dst, servoParams(i)->middle);
sbufWriteU8(dst, servoParams(i)->rate);
mspSerializeServoParams(dst, servoParams(i));
}
break;
case MSP_SERVO_MIX_RULES:
Expand All @@ -569,27 +625,11 @@ static bool mspFcProcessOutCommand(uint16_t cmdMSP, sbuf_t *dst, mspPostProcessF
break;
case MSP2_INAV_SERVO_MIXER:
for (int i = 0; i < MAX_SERVO_RULES; i++) {
sbufWriteU8(dst, customServoMixers(i)->targetChannel);
sbufWriteU8(dst, customServoMixers(i)->inputSource);
sbufWriteU16(dst, customServoMixers(i)->rate);
sbufWriteU8(dst, customServoMixers(i)->speed);
#ifdef USE_PROGRAMMING_FRAMEWORK
sbufWriteU8(dst, customServoMixers(i)->conditionId);
#else
sbufWriteU8(dst, -1);
#endif
mspSerializeServoMixer(dst, customServoMixers(i));
}
if(MAX_MIXER_PROFILE_COUNT==1) break;
for (int i = 0; i < MAX_SERVO_RULES; i++) {
sbufWriteU8(dst, mixerServoMixersByIndex(nextMixerProfileIndex)[i].targetChannel);
sbufWriteU8(dst, mixerServoMixersByIndex(nextMixerProfileIndex)[i].inputSource);
sbufWriteU16(dst, mixerServoMixersByIndex(nextMixerProfileIndex)[i].rate);
sbufWriteU8(dst, mixerServoMixersByIndex(nextMixerProfileIndex)[i].speed);
#ifdef USE_PROGRAMMING_FRAMEWORK
sbufWriteU8(dst, mixerServoMixersByIndex(nextMixerProfileIndex)[i].conditionId);
#else
sbufWriteU8(dst, -1);
#endif
mspSerializeServoMixer(dst, &mixerServoMixersByIndex(nextMixerProfileIndex)[i]);
}
break;
#ifdef USE_PROGRAMMING_FRAMEWORK
Expand Down Expand Up @@ -649,17 +689,11 @@ static bool mspFcProcessOutCommand(uint16_t cmdMSP, sbuf_t *dst, mspPostProcessF
#endif
case MSP2_COMMON_MOTOR_MIXER:
for (uint8_t i = 0; i < MAX_SUPPORTED_MOTORS; i++) {
sbufWriteU16(dst, constrainf(primaryMotorMixer(i)->throttle + 2.0f, 0.0f, 4.0f) * 1000);
sbufWriteU16(dst, constrainf(primaryMotorMixer(i)->roll + 2.0f, 0.0f, 4.0f) * 1000);
sbufWriteU16(dst, constrainf(primaryMotorMixer(i)->pitch + 2.0f, 0.0f, 4.0f) * 1000);
sbufWriteU16(dst, constrainf(primaryMotorMixer(i)->yaw + 2.0f, 0.0f, 4.0f) * 1000);
mspSerializeMotorMixer(dst, primaryMotorMixer(i));
}
if (MAX_MIXER_PROFILE_COUNT==1) break;
for (uint8_t i = 0; i < MAX_SUPPORTED_MOTORS; i++) {
sbufWriteU16(dst, constrainf(mixerMotorMixersByIndex(nextMixerProfileIndex)[i].throttle + 2.0f, 0.0f, 4.0f) * 1000);
sbufWriteU16(dst, constrainf(mixerMotorMixersByIndex(nextMixerProfileIndex)[i].roll + 2.0f, 0.0f, 4.0f) * 1000);
sbufWriteU16(dst, constrainf(mixerMotorMixersByIndex(nextMixerProfileIndex)[i].pitch + 2.0f, 0.0f, 4.0f) * 1000);
sbufWriteU16(dst, constrainf(mixerMotorMixersByIndex(nextMixerProfileIndex)[i].yaw + 2.0f, 0.0f, 4.0f) * 1000);
mspSerializeMotorMixer(dst, &mixerMotorMixersByIndex(nextMixerProfileIndex)[i]);
}
break;

Expand Down Expand Up @@ -1453,17 +1487,11 @@ static bool mspFcProcessOutCommand(uint16_t cmdMSP, sbuf_t *dst, mspPostProcessF

case MSP_CALIBRATION_DATA:
sbufWriteU8(dst, accGetCalibrationAxisFlags());
sbufWriteU16(dst, accelerometerConfig()->accZero.raw[X]);
sbufWriteU16(dst, accelerometerConfig()->accZero.raw[Y]);
sbufWriteU16(dst, accelerometerConfig()->accZero.raw[Z]);
sbufWriteU16(dst, accelerometerConfig()->accGain.raw[X]);
sbufWriteU16(dst, accelerometerConfig()->accGain.raw[Y]);
sbufWriteU16(dst, accelerometerConfig()->accGain.raw[Z]);
sbufWriteAxisU16(dst, accelerometerConfig()->accZero.raw);
sbufWriteAxisU16(dst, accelerometerConfig()->accGain.raw);

#ifdef USE_MAG
sbufWriteU16(dst, compassConfig()->magZero.raw[X]);
sbufWriteU16(dst, compassConfig()->magZero.raw[Y]);
sbufWriteU16(dst, compassConfig()->magZero.raw[Z]);
sbufWriteAxisU16(dst, compassConfig()->magZero.raw);
#else
sbufWriteU16(dst, 0);
sbufWriteU16(dst, 0);
Expand Down Expand Up @@ -2098,15 +2126,7 @@ static mspResult_e mspFcProcessInCommand(uint16_t cmdMSP, sbuf_t *src)
sbufReadU8(src); //Read rcRate8, kept for protocol compatibility reasons
// need to cast away const to set controlProfile
((controlConfig_t*)currentControlProfile)->stabilized.rcExpo8 = sbufReadU8(src);
for (int i = 0; i < 3; i++) {
tmp_u8 = sbufReadU8(src);
if (i == FD_YAW) {
((controlConfig_t*)currentControlProfile)->stabilized.rates[i] = constrain(tmp_u8, SETTING_YAW_RATE_MIN, SETTING_YAW_RATE_MAX);
}
else {
((controlConfig_t*)currentControlProfile)->stabilized.rates[i] = constrain(tmp_u8, SETTING_CONSTANT_ROLL_PITCH_RATE_MIN, SETTING_CONSTANT_ROLL_PITCH_RATE_MAX);
}
}
mspReadRates(src, ((controlConfig_t*)currentControlProfile)->stabilized.rates);
tmp_u8 = sbufReadU8(src);
((controlConfig_t*)currentControlProfile)->throttle.dynPID = MIN(tmp_u8, SETTING_TPA_RATE_MAX);
((controlConfig_t*)currentControlProfile)->throttle.rcMid8 = sbufReadU8(src);
Expand Down Expand Up @@ -2135,26 +2155,12 @@ static mspResult_e mspFcProcessInCommand(uint16_t cmdMSP, sbuf_t *src)
// stabilized
currentControlProfile_p->stabilized.rcExpo8 = sbufReadU8(src);
currentControlProfile_p->stabilized.rcYawExpo8 = sbufReadU8(src);
for (uint8_t i = 0; i < 3; ++i) {
tmp_u8 = sbufReadU8(src);
if (i == FD_YAW) {
currentControlProfile_p->stabilized.rates[i] = constrain(tmp_u8, SETTING_YAW_RATE_MIN, SETTING_YAW_RATE_MAX);
} else {
currentControlProfile_p->stabilized.rates[i] = constrain(tmp_u8, SETTING_CONSTANT_ROLL_PITCH_RATE_MIN, SETTING_CONSTANT_ROLL_PITCH_RATE_MAX);
}
}
mspReadRates(src, currentControlProfile_p->stabilized.rates);

// manual
currentControlProfile_p->manual.rcExpo8 = sbufReadU8(src);
currentControlProfile_p->manual.rcYawExpo8 = sbufReadU8(src);
for (uint8_t i = 0; i < 3; ++i) {
tmp_u8 = sbufReadU8(src);
if (i == FD_YAW) {
currentControlProfile_p->manual.rates[i] = constrain(tmp_u8, SETTING_YAW_RATE_MIN, SETTING_YAW_RATE_MAX);
} else {
currentControlProfile_p->manual.rates[i] = constrain(tmp_u8, SETTING_CONSTANT_ROLL_PITCH_RATE_MIN, SETTING_CONSTANT_ROLL_PITCH_RATE_MAX);
}
}
mspReadRates(src, currentControlProfile_p->manual.rates);

} else {
return MSP_RESULT_ERROR;
Expand Down Expand Up @@ -2346,15 +2352,11 @@ static mspResult_e mspFcProcessInCommand(uint16_t cmdMSP, sbuf_t *src)
if (tmp_u8 >= MAX_SUPPORTED_SERVOS) {
return MSP_RESULT_ERROR;
} else {
servoParamsMutable(tmp_u8)->min = sbufReadU16(src);
servoParamsMutable(tmp_u8)->max = sbufReadU16(src);
servoParamsMutable(tmp_u8)->middle = sbufReadU16(src);
servoParamsMutable(tmp_u8)->rate = sbufReadU8(src);
mspDeserializeServoParams(src, tmp_u8);
sbufReadU8(src);
sbufReadU8(src);
sbufReadU8(src); // used to be forwardFromChannel, ignored
sbufReadU32(src); // used to be reversedSources
servoComputeScalingFactors(tmp_u8);
}
break;

Expand All @@ -2366,11 +2368,7 @@ static mspResult_e mspFcProcessInCommand(uint16_t cmdMSP, sbuf_t *src)
if (tmp_u8 >= MAX_SUPPORTED_SERVOS) {
return MSP_RESULT_ERROR;
} else {
servoParamsMutable(tmp_u8)->min = sbufReadU16(src);
servoParamsMutable(tmp_u8)->max = sbufReadU16(src);
servoParamsMutable(tmp_u8)->middle = sbufReadU16(src);
servoParamsMutable(tmp_u8)->rate = sbufReadU8(src);
servoComputeScalingFactors(tmp_u8);
mspDeserializeServoParams(src, tmp_u8);
}
break;

Expand Down Expand Up @@ -2821,17 +2819,11 @@ static mspResult_e mspFcProcessInCommand(uint16_t cmdMSP, sbuf_t *src)

case MSP_SET_CALIBRATION_DATA:
if (dataSize >= 18) {
accelerometerConfigMutable()->accZero.raw[X] = sbufReadU16(src);
accelerometerConfigMutable()->accZero.raw[Y] = sbufReadU16(src);
accelerometerConfigMutable()->accZero.raw[Z] = sbufReadU16(src);
accelerometerConfigMutable()->accGain.raw[X] = sbufReadU16(src);
accelerometerConfigMutable()->accGain.raw[Y] = sbufReadU16(src);
accelerometerConfigMutable()->accGain.raw[Z] = sbufReadU16(src);
sbufReadAxisU16(src, accelerometerConfigMutable()->accZero.raw);
sbufReadAxisU16(src, accelerometerConfigMutable()->accGain.raw);

#ifdef USE_MAG
compassConfigMutable()->magZero.raw[X] = sbufReadU16(src);
compassConfigMutable()->magZero.raw[Y] = sbufReadU16(src);
compassConfigMutable()->magZero.raw[Z] = sbufReadU16(src);
sbufReadAxisU16(src, compassConfigMutable()->magZero.raw);
#else
sbufReadU16(src);
sbufReadU16(src);
Expand All @@ -2844,9 +2836,7 @@ static mspResult_e mspFcProcessInCommand(uint16_t cmdMSP, sbuf_t *src)
#endif
#ifdef USE_MAG
if (dataSize >= 22) {
compassConfigMutable()->magGain[X] = sbufReadU16(src);
compassConfigMutable()->magGain[Y] = sbufReadU16(src);
compassConfigMutable()->magGain[Z] = sbufReadU16(src);
sbufReadAxisU16(src, compassConfigMutable()->magGain);
}
#else
if (dataSize >= 22) {
Expand Down
Loading
Loading