Mirror/inav
1
0
Fork 0
mirror of https://github.com/iNavFlight/inav.git synced 2025-07-24 00:35:34 +03:00
Code Issues Wiki Activity

Merge pull request #756 from martinbudden/inav_msp_sbuf_optimise

Browse source 
Use sbufWriteData for greater efficiency
This commit is contained in:
Konstantin Sharlaimov 2016-11-06 18:38:42 +10:00 • committed by GitHub
commit a3b81e36e2
1 changed files with 36 additions and 47 deletions
Download patch file Download diff file Expand all files Collapse all files

83
src/main/fc/fc_msp.c
View file

@ -237,9 +237,7 @@ static void serializeBoxNamesReply(sbuf_t *dst)
const box_t *box = findBoxByActiveBoxId(activeBoxId);
if (box) {
const int len = strlen(box->boxName);
for (int j = 0; j < len; j++) {
sbufWriteU8(dst, box->boxName[j]);
}
sbufWriteData(dst, box->boxName, len);
}
}
}
@ -463,7 +461,6 @@ static bool mspFcProcessOutCommand(uint8_t cmdMSP, sbuf_t *dst, sbuf_t *src, msp
#if !defined(NAV) && !defined(USE_FLASHFS)
UNUSED(src);
#endif
uint32_t i;
#ifdef NAV
int8_t msp_wp_no;
@ -479,9 +476,7 @@ static bool mspFcProcessOutCommand(uint8_t cmdMSP, sbuf_t *dst, sbuf_t *src, msp
break;
case MSP_FC_VARIANT:
for (i = 0; i < FLIGHT_CONTROLLER_IDENTIFIER_LENGTH; i++) {
sbufWriteU8(dst, flightControllerIdentifier[i]);
}
sbufWriteData(dst, flightControllerIdentifier, FLIGHT_CONTROLLER_IDENTIFIER_LENGTH);
break;
case MSP_FC_VERSION:
@ -491,9 +486,7 @@ static bool mspFcProcessOutCommand(uint8_t cmdMSP, sbuf_t *dst, sbuf_t *src, msp
break;
case MSP_BOARD_INFO:
for (i = 0; i < BOARD_IDENTIFIER_LENGTH; i++) {
sbufWriteU8(dst, boardIdentifier[i]);
}
sbufWriteData(dst, boardIdentifier, BOARD_IDENTIFIER_LENGTH);
#ifdef NAZE
sbufWriteU16(dst, hardwareRevision);
#else
@ -502,16 +495,9 @@ static bool mspFcProcessOutCommand(uint8_t cmdMSP, sbuf_t *dst, sbuf_t *src, msp
break;
case MSP_BUILD_INFO:
for (i = 0; i < BUILD_DATE_LENGTH; i++) {
sbufWriteU8(dst, buildDate[i]);
}
for (i = 0; i < BUILD_TIME_LENGTH; i++) {
sbufWriteU8(dst, buildTime[i]);
}
for (i = 0; i < GIT_SHORT_REVISION_LENGTH; i++) {
sbufWriteU8(dst, shortGitRevision[i]);
}
sbufWriteData(dst, buildDate, BUILD_DATE_LENGTH);
sbufWriteData(dst, buildTime, BUILD_TIME_LENGTH);
sbufWriteData(dst, shortGitRevision, GIT_SHORT_REVISION_LENGTH);
break;
// DEPRECATED - Use MSP_API_VERSION
@ -560,12 +546,15 @@ static bool mspFcProcessOutCommand(uint8_t cmdMSP, sbuf_t *dst, sbuf_t *src, msp
{
// Hack scale due to choice of units for sensor data in multiwii
const uint8_t scale = (acc.acc_1G > 1024) ? 8 : 1;
for (i = 0; i < 3; i++)
for (int i = 0; i < 3; i++) {
sbufWriteU16(dst, accADC[i] / scale);
for (i = 0; i < 3; i++)
}
for (int i = 0; i < 3; i++) {
sbufWriteU16(dst, gyroADC[i]);
for (i = 0; i < 3; i++)
}
for (int i = 0; i < 3; i++) {
sbufWriteU16(dst, magADC[i]);
}
}
break;
@ -575,7 +564,7 @@ static bool mspFcProcessOutCommand(uint8_t cmdMSP, sbuf_t *dst, sbuf_t *src, msp
sbufWriteData(dst, &servo, MAX_SUPPORTED_SERVOS * 2);
break;
case MSP_SERVO_CONFIGURATIONS:
for (i = 0; i < MAX_SUPPORTED_SERVOS; i++) {
for (int i = 0; i < MAX_SUPPORTED_SERVOS; i++) {
sbufWriteU16(dst, currentProfile->servoConf[i].min);
sbufWriteU16(dst, currentProfile->servoConf[i].max);
sbufWriteU16(dst, currentProfile->servoConf[i].middle);
@ -587,7 +576,7 @@ static bool mspFcProcessOutCommand(uint8_t cmdMSP, sbuf_t *dst, sbuf_t *src, msp
}
break;
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].inputSource);
sbufWriteU8(dst, masterConfig.customServoMixer[i].rate);
@ -606,8 +595,9 @@ static bool mspFcProcessOutCommand(uint8_t cmdMSP, sbuf_t *dst, sbuf_t *src, msp
break;
case MSP_RC:
for (i = 0; i < rxRuntimeConfig.channelCount; i++)
for (int i = 0; i < rxRuntimeConfig.channelCount; i++) {
sbufWriteU16(dst, rcData[i]);
}
break;
case MSP_ATTITUDE:
@ -656,7 +646,7 @@ static bool mspFcProcessOutCommand(uint8_t cmdMSP, sbuf_t *dst, sbuf_t *src, msp
case MSP_RC_TUNING:
sbufWriteU8(dst, 100); //rcRate8 kept for compatibity reasons, this setting is no longer used
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->dynThrPID);
@ -667,7 +657,7 @@ static bool mspFcProcessOutCommand(uint8_t cmdMSP, sbuf_t *dst, sbuf_t *src, msp
break;
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.I8[i]);
sbufWriteU8(dst, currentProfile->pidProfile.D8[i]);
@ -685,7 +675,7 @@ static bool mspFcProcessOutCommand(uint8_t cmdMSP, sbuf_t *dst, sbuf_t *src, msp
break;
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 = &currentProfile->modeActivationConditions[i];
const box_t *box = findBoxByActiveBoxId(mac->modeId);
sbufWriteU8(dst, box ? box->permanentId : 0);
@ -696,7 +686,7 @@ static bool mspFcProcessOutCommand(uint8_t cmdMSP, sbuf_t *dst, sbuf_t *src, msp
break;
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 = &currentProfile->adjustmentRanges[i];
sbufWriteU8(dst, adjRange->adjustmentIndex);
sbufWriteU8(dst, adjRange->auxChannelIndex);
@ -712,7 +702,7 @@ static bool mspFcProcessOutCommand(uint8_t cmdMSP, sbuf_t *dst, sbuf_t *src, msp
break;
case MSP_BOXIDS:
for (i = 0; i < activeBoxIdCount; i++) {
for (int i = 0; i < activeBoxIdCount; i++) {
const box_t *box = findBoxByActiveBoxId(activeBoxIds[i]);
if (!box) {
continue;
@ -753,7 +743,7 @@ static bool mspFcProcessOutCommand(uint8_t cmdMSP, sbuf_t *dst, sbuf_t *src, msp
case MSP_MOTOR_PINS:
// 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);
break;
@ -824,8 +814,9 @@ static bool mspFcProcessOutCommand(uint8_t cmdMSP, sbuf_t *dst, sbuf_t *src, msp
// output some useful QA statistics
// 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
}
break;
case MSP_UID:
@ -888,7 +879,7 @@ static bool mspFcProcessOutCommand(uint8_t cmdMSP, sbuf_t *dst, sbuf_t *src, msp
break;
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);
sbufWriteU16(dst, RXFAIL_STEP_TO_CHANNEL_VALUE(masterConfig.rxConfig.failsafe_channel_configurations[i].step));
}
@ -899,8 +890,7 @@ static bool mspFcProcessOutCommand(uint8_t cmdMSP, sbuf_t *dst, sbuf_t *src, msp
break;
case MSP_RX_MAP:
for (i = 0; i < MAX_MAPPABLE_RX_INPUTS; i++)
sbufWriteU8(dst, masterConfig.rxConfig.rcmap[i]);
sbufWriteData(dst, masterConfig.rxConfig.rcmap, MAX_MAPPABLE_RX_INPUTS);
break;
case MSP_BF_CONFIG:
@ -919,7 +909,7 @@ static bool mspFcProcessOutCommand(uint8_t cmdMSP, sbuf_t *dst, sbuf_t *src, msp
break;
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)) {
continue;
};
@ -934,7 +924,7 @@ static bool mspFcProcessOutCommand(uint8_t cmdMSP, sbuf_t *dst, sbuf_t *src, msp
#ifdef LED_STRIP
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.colors[i];
sbufWriteU16(dst, color->h);
sbufWriteU8(dst, color->s);
@ -943,7 +933,7 @@ static bool mspFcProcessOutCommand(uint8_t cmdMSP, sbuf_t *dst, sbuf_t *src, msp
break;
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.ledConfigs[i];
sbufWriteU32(dst, *ledConfig);
}
@ -997,8 +987,7 @@ static bool mspFcProcessOutCommand(uint8_t cmdMSP, sbuf_t *dst, sbuf_t *src, msp
break;
case MSP_BF_BUILD_INFO:
for (i = 0; i < 11; i++)
sbufWriteU8(dst, buildDate[i]); // MMM DD YYYY as ascii, MMM = Jan/Feb... etc
sbufWriteData(dst, buildDate, 11); // MMM DD YYYY as ascii, MMM = Jan/Feb... etc
sbufWriteU32(dst, 0); // future exp
sbufWriteU32(dst, 0); // future exp
break;
@ -1106,7 +1095,7 @@ static mspResult_e mspFcProcessInCommand(uint8_t cmdMSP, sbuf_t *src)
} else {
uint16_t frame[MAX_SUPPORTED_RC_CHANNEL_COUNT];
for (i = 0; i < channelCount; i++) {
for (int i = 0; i < channelCount; i++) {
frame[i] = sbufReadU16(src);
}
@ -1131,7 +1120,7 @@ static mspResult_e mspFcProcessInCommand(uint8_t cmdMSP, sbuf_t *src)
case MSP_SET_PID:
signalRequiredPIDCoefficientsUpdate();
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.I8[i] = sbufReadU8(src);
currentProfile->pidProfile.D8[i] = sbufReadU8(src);
@ -1183,7 +1172,7 @@ static mspResult_e mspFcProcessInCommand(uint8_t cmdMSP, sbuf_t *src)
if (dataSize >= 10) {
sbufReadU8(src); //Read rcRate8, kept for protocol compatibility reasons
currentControlRateProfile->rcExpo8 = sbufReadU8(src);
for (i = 0; i < 3; i++) {
for (int i = 0; i < 3; i++) {
rate = sbufReadU8(src);
if (i == FD_YAW) {
currentControlRateProfile->rates[i] = constrain(rate, CONTROL_RATE_CONFIG_YAW_RATE_MIN, CONTROL_RATE_CONFIG_YAW_RATE_MAX);
@ -1240,7 +1229,7 @@ static mspResult_e mspFcProcessInCommand(uint8_t cmdMSP, sbuf_t *src)
break;
case MSP_SET_MOTOR:
for (i = 0; i < 8; i++) {
for (int i = 0; i < 8; i++) {
const int16_t disarmed = sbufReadU16(src);
if (i < MAX_SUPPORTED_MOTORS) {
motor_disarmed[i] = disarmed;
@ -1484,7 +1473,7 @@ static mspResult_e mspFcProcessInCommand(uint8_t cmdMSP, sbuf_t *src)
break;
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);
}
break;
@ -1541,7 +1530,7 @@ static mspResult_e mspFcProcessInCommand(uint8_t cmdMSP, sbuf_t *src)
#ifdef LED_STRIP
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.colors[i];
color->h = sbufReadU16(src);
color->s = sbufReadU8(src);