Mirror/betaflight
1
0
Fork 0
mirror of https://github.com/betaflight/betaflight.git synced 2025-07-20 23:05:19 +03:00
Code Issues Wiki Activity

Added rateprofile support to MSP. Also fixed number of profiles supported by MSP.

Browse source
This commit is contained in:
Michael Keller 2016-07-26 08:21:32 +12:00
parent e21065c970
commit 03cf9b5168
1 changed files with 25 additions and 13 deletions
Download patch file Download diff file Expand all files Collapse all files

38
src/main/io/serial_msp.c
View file

@ -188,6 +188,8 @@ STATIC_UNIT_TESTED mspPort_t mspPorts[MAX_MSP_PORT_COUNT];
STATIC_UNIT_TESTED mspPort_t *currentPort; STATIC_UNIT_TESTED mspPort_t *currentPort;
STATIC_UNIT_TESTED bufWriter_t *writer; STATIC_UNIT_TESTED bufWriter_t *writer;
#define RATEPROFILE_MASK (1 << 7)
static void serialize8(uint8_t a) static void serialize8(uint8_t a)
{ {
bufWriterAppend(writer, a); bufWriterAppend(writer, a);
@ -674,7 +676,7 @@ static bool processOutCommand(uint8_t cmdMSP)
break; break;
case MSP_STATUS_EX: case MSP_STATUS_EX:
headSerialReply(14); headSerialReply(15);
serialize16(cycleTime); serialize16(cycleTime);
#ifdef USE_I2C #ifdef USE_I2C
serialize16(i2cGetErrorCounter()); serialize16(i2cGetErrorCounter());
@ -683,9 +685,10 @@ static bool processOutCommand(uint8_t cmdMSP)
#endif #endif
serialize16(sensors(SENSOR_ACC) | sensors(SENSOR_BARO) << 1 | sensors(SENSOR_MAG) << 2 | sensors(SENSOR_GPS) << 3 | sensors(SENSOR_SONAR) << 4); serialize16(sensors(SENSOR_ACC) | sensors(SENSOR_BARO) << 1 | sensors(SENSOR_MAG) << 2 | sensors(SENSOR_GPS) << 3 | sensors(SENSOR_SONAR) << 4);
serialize32(packFlightModeFlags()); serialize32(packFlightModeFlags());
serialize8(masterConfig.current_profile_index); serialize8(getCurrentProfile());
serialize16(constrain(averageSystemLoadPercent, 0, 100)); serialize16(constrain(averageSystemLoadPercent, 0, 100));
serialize8(MAX_PROFILE_COUNT); serialize8(MAX_PROFILE_COUNT);
serialize8(getCurrentControlRateProfile());
break; break;
case MSP_NAME: case MSP_NAME:
@ -1261,7 +1264,7 @@ static bool processInCommand(void)
{ {
uint32_t i; uint32_t i;
uint16_t tmp; uint16_t tmp;
uint8_t rate; uint8_t value;
#ifdef GPS #ifdef GPS
uint8_t wp_no; uint8_t wp_no;
int32_t lat = 0, lon = 0, alt = 0; int32_t lat = 0, lon = 0, alt = 0;
@ -1271,14 +1274,23 @@ static bool processInCommand(void)
#endif #endif
switch (currentPort->cmdMSP) { switch (currentPort->cmdMSP) {
case MSP_SELECT_SETTING: case MSP_SELECT_SETTING:
if (!ARMING_FLAG(ARMED)) { value = read8();
masterConfig.current_profile_index = read8(); if ((value & RATEPROFILE_MASK) == 0) {
if (masterConfig.current_profile_index > 1) { if (!ARMING_FLAG(ARMED)) {
masterConfig.current_profile_index = 0; if (value >= MAX_PROFILE_COUNT) {
value = 0;
}
changeProfile(value);
} }
writeEEPROM(); } else {
readEEPROM(); value = value & ~RATEPROFILE_MASK;
if (value >= MAX_RATEPROFILES) {
value = 0;
}
changeControlRateProfile(value);
} }
break; break;
case MSP_SET_HEAD: case MSP_SET_HEAD:
magHold = read16(); magHold = read16();
@ -1366,11 +1378,11 @@ static bool processInCommand(void)
currentControlRateProfile->rcRate8 = read8(); currentControlRateProfile->rcRate8 = read8();
currentControlRateProfile->rcExpo8 = read8(); currentControlRateProfile->rcExpo8 = read8();
for (i = 0; i < 3; i++) { for (i = 0; i < 3; i++) {
rate = read8(); value = read8();
currentControlRateProfile->rates[i] = MIN(rate, 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);
} }
rate = read8(); value = read8();
currentControlRateProfile->dynThrPID = MIN(rate, CONTROL_RATE_CONFIG_TPA_MAX); currentControlRateProfile->dynThrPID = MIN(value, CONTROL_RATE_CONFIG_TPA_MAX);
currentControlRateProfile->thrMid8 = read8(); currentControlRateProfile->thrMid8 = read8();
currentControlRateProfile->thrExpo8 = read8(); currentControlRateProfile->thrExpo8 = read8();
currentControlRateProfile->tpa_breakpoint = read16(); currentControlRateProfile->tpa_breakpoint = read16();