From 763c75873e79ec86265b8b62882355d6feee5b3e Mon Sep 17 00:00:00 2001 From: Dominic Clifton Date: Wed, 8 Jul 2015 12:33:52 +0100 Subject: [PATCH] Use 0 based index for motor and servo mix in CLI. Update MSP_SERVO_MIX_RULES, MSP_SET_SERVO_MIX_RULE, MSP_SERVO_CONFIGURATIONS, MSP_SET_SERVO_CONFIGURATION. Delete MSP_CHANNEL_FORWARDING, MSP_SET_CHANNEL_FORWARDING. --- src/main/flight/mixer.c | 4 +-- src/main/flight/mixer.h | 5 +-- src/main/io/serial_cli.c | 38 +++++++++++----------- src/main/io/serial_msp.c | 70 +++++++++++++++------------------------- 4 files changed, 47 insertions(+), 70 deletions(-) diff --git a/src/main/flight/mixer.c b/src/main/flight/mixer.c index 2682044ef..3d3de710e 100755 --- a/src/main/flight/mixer.c +++ b/src/main/flight/mixer.c @@ -514,7 +514,7 @@ void servoMixerLoadMix(int index, servoMixer_t *customServoMixers) index++; // clear existing for (i = 0; i < MAX_SERVO_RULES; i++) - customServoMixers[i].targetChannel = customServoMixers[i].fromChannel = customServoMixers[i].rate = customServoMixers[i].box = 0; + customServoMixers[i].targetChannel = customServoMixers[i].inputSource = customServoMixers[i].rate = customServoMixers[i].box = 0; for (i = 0; i < servoMixers[index].servoRuleCount; i++) customServoMixers[i] = servoMixers[index].rule[i]; @@ -750,7 +750,7 @@ static void servoMixer(void) // consider rule if no box assigned or box is active if (currentServoMixer[i].box == 0 || IS_RC_MODE_ACTIVE(BOXSERVO1 + currentServoMixer[i].box - 1)) { uint8_t target = currentServoMixer[i].targetChannel; - uint8_t from = currentServoMixer[i].fromChannel; // FIXME rename 'from' to inputSource + uint8_t from = currentServoMixer[i].inputSource; uint16_t servo_width = servoConf[target].max - servoConf[target].min; int16_t min = currentServoMixer[i].min * servo_width / 100 - servo_width / 2; int16_t max = currentServoMixer[i].max * servo_width / 100 - servo_width / 2; diff --git a/src/main/flight/mixer.h b/src/main/flight/mixer.h index d8f6ce2a6..8078da0bc 100644 --- a/src/main/flight/mixer.h +++ b/src/main/flight/mixer.h @@ -116,10 +116,7 @@ enum { typedef struct servoMixer_t { uint8_t targetChannel; // servo that receives the output of the rule - - // FIXME rename to inputSource - uint8_t fromChannel; // input channel for this rule - + uint8_t inputSource; // input channel for this rule int8_t rate; // range [-125;+125] ; can be used to adjust a rate 0-125% and a direction uint8_t speed; // reduces the speed of the rule, 0=unlimited speed int8_t min; // lower bound of rule range [0;100]% of servo max-min diff --git a/src/main/io/serial_cli.c b/src/main/io/serial_cli.c index 51d304db4..2be7c3c39 100644 --- a/src/main/io/serial_cli.c +++ b/src/main/io/serial_cli.c @@ -786,7 +786,7 @@ static void cliMotorMix(char *cmdline) if (masterConfig.customMotorMixer[i].throttle == 0.0f) break; num_motors++; - printf("#%d:\t", i + 1); + printf("#%d:\t", i); printf("%s\t", ftoa(masterConfig.customMotorMixer[i].throttle, buf)); printf("%s\t", ftoa(masterConfig.customMotorMixer[i].roll, buf)); printf("%s\t", ftoa(masterConfig.customMotorMixer[i].pitch, buf)); @@ -817,7 +817,7 @@ static void cliMotorMix(char *cmdline) } else { ptr = cmdline; i = atoi(ptr); // get motor number - if (--i < MAX_SUPPORTED_MOTORS) { + if (i < MAX_SUPPORTED_MOTORS) { ptr = strchr(ptr, ' '); if (ptr) { masterConfig.customMotorMixer[i].throttle = fastA2F(++ptr); @@ -1001,9 +1001,9 @@ static void cliServoMix(char *cmdline) cliPrint("Rule\tServo\tSource\tRate\tSpeed\tMin\tMax\tBox\r\n"); printf("#%d:\t%d\t%d\t%d\t%d\t%d\t%d\t%d\r\n", - i + 1, - masterConfig.customServoMixer[i].targetChannel + 1, - masterConfig.customServoMixer[i].fromChannel + 1, + i, + masterConfig.customServoMixer[i].targetChannel, + masterConfig.customServoMixer[i].inputSource, masterConfig.customServoMixer[i].rate, masterConfig.customServoMixer[i].speed, masterConfig.customServoMixer[i].min, @@ -1045,11 +1045,11 @@ static void cliServoMix(char *cmdline) if (len == 0) { printf("s"); for (inputSource = 0; inputSource < INPUT_SOURCE_COUNT; inputSource++) - printf("\ti%d", inputSource + 1); + printf("\ti%d", inputSource); printf("\r\n"); for (servoIndex = 0; servoIndex < MAX_SUPPORTED_SERVOS; servoIndex++) { - printf("%d", servoIndex + 1); + printf("%d", servoIndex); for (inputSource = 0; inputSource < INPUT_SOURCE_COUNT; inputSource++) printf("\t%s ", (currentProfile->servoConf[servoIndex].reversedSources & (1 << inputSource)) ? "r" : "n"); printf("\r\n"); @@ -1068,9 +1068,7 @@ static void cliServoMix(char *cmdline) return; } - if (args[SERVO] >= 1 && args[SERVO] <= MAX_SUPPORTED_SERVOS && args[INPUT] >= 1 && args[INPUT] <= INPUT_SOURCE_COUNT && (args[REVERSE] == -1 || args[REVERSE] == 1)) { - args[SERVO] -= 1; - args[INPUT] -= 1; + if (args[SERVO] >= 0 && args[SERVO] < MAX_SUPPORTED_SERVOS && args[INPUT] >= 0 && args[INPUT] < INPUT_SOURCE_COUNT && (args[REVERSE] == -1 || args[REVERSE] == 1)) { if (args[REVERSE] == -1) currentProfile->servoConf[args[SERVO]].reversedSources |= 1 << args[INPUT]; else @@ -1092,17 +1090,17 @@ static void cliServoMix(char *cmdline) return; } - i = args[RULE] - 1; + i = args[RULE]; if (i >= 0 && i < MAX_SERVO_RULES && - args[TARGET] > 0 && args[TARGET] <= MAX_SUPPORTED_SERVOS && - args[INPUT] >= 1 && args[INPUT] <= INPUT_SOURCE_COUNT && + args[TARGET] >= 0 && args[TARGET] < MAX_SUPPORTED_SERVOS && + args[INPUT] >= 0 && args[INPUT] < INPUT_SOURCE_COUNT && args[RATE] >= -100 && args[RATE] <= 100 && args[SPEED] >= 0 && args[SPEED] <= MAX_SERVO_SPEED && args[MIN] >= 0 && args[MIN] <= 100 && args[MAX] >= 0 && args[MAX] <= 100 && args[MIN] < args[MAX] && args[BOX] >= 0 && args[BOX] <= MAX_SERVO_BOXES) { - masterConfig.customServoMixer[i].targetChannel = args[TARGET] - 1; - masterConfig.customServoMixer[i].fromChannel = args[INPUT] - 1; + masterConfig.customServoMixer[i].targetChannel = args[TARGET]; + masterConfig.customServoMixer[i].inputSource = args[INPUT]; masterConfig.customServoMixer[i].rate = args[RATE]; masterConfig.customServoMixer[i].speed = args[SPEED]; masterConfig.customServoMixer[i].min = args[MIN]; @@ -1270,7 +1268,7 @@ static void cliDump(char *cmdline) roll = masterConfig.customMotorMixer[i].roll; pitch = masterConfig.customMotorMixer[i].pitch; yaw = masterConfig.customMotorMixer[i].yaw; - printf("mmix %d", i + 1); + printf("mmix %d", i); if (thr < 0) cliWrite(' '); printf("%s", ftoa(thr, buf)); @@ -1294,9 +1292,9 @@ static void cliDump(char *cmdline) break; printf("smix %d %d %d %d %d %d %d %d\r\n", - i + 1, - masterConfig.customServoMixer[i].targetChannel + 1, - masterConfig.customServoMixer[i].fromChannel + 1, + i, + masterConfig.customServoMixer[i].targetChannel, + masterConfig.customServoMixer[i].inputSource, masterConfig.customServoMixer[i].rate, masterConfig.customServoMixer[i].speed, masterConfig.customServoMixer[i].min, @@ -1309,7 +1307,7 @@ static void cliDump(char *cmdline) for (i = 0; i < MAX_SUPPORTED_SERVOS; i++) { for (channel = 0; channel < INPUT_SOURCE_COUNT; channel++) { if (servoDirection(i, channel) < 0) { - printf("smix reverse %d %d -1\r\n", i + 1 , channel + 1); + printf("smix reverse %d %d -1\r\n", i , channel); } } } diff --git a/src/main/io/serial_msp.c b/src/main/io/serial_msp.c index d2ec6502e..80c5c2398 100644 --- a/src/main/io/serial_msp.c +++ b/src/main/io/serial_msp.c @@ -173,9 +173,6 @@ static const char * const boardIdentifier = TARGET_BOARD_IDENTIFIER; // // MSP commands for Cleanflight original features // -#define MSP_CHANNEL_FORWARDING 32 //out message Returns channel forwarding settings -#define MSP_SET_CHANNEL_FORWARDING 33 //in message Channel forwarding settings - #define MSP_MODE_RANGES 34 //out message Returns all mode ranges #define MSP_SET_MODE_RANGE 35 //in message Sets a single mode range @@ -273,7 +270,7 @@ static const char * const boardIdentifier = TARGET_BOARD_IDENTIFIER; #define MSP_PIDNAMES 117 //out message the PID names #define MSP_WP 118 //out message get a WP, WP# is in the payload, returns (WP#, lat, lon, alt, flags) WP#0-home, WP#16-poshold #define MSP_BOXIDS 119 //out message get the permanent IDs associated to BOXes -#define MSP_SERVO_CONF 120 //out message Servo settings +#define MSP_SERVO_CONFIGURATIONS 120 //out message All servo configurations. #define MSP_NAV_STATUS 121 //out message Returns navigation status #define MSP_NAV_CONFIG 122 //out message Returns navigation parameters @@ -289,7 +286,7 @@ static const char * const boardIdentifier = TARGET_BOARD_IDENTIFIER; #define MSP_SET_WP 209 //in message sets a given WP (WP#,lat, lon, alt, flags) #define MSP_SELECT_SETTING 210 //in message Select Setting Number (0-2) #define MSP_SET_HEAD 211 //in message define a new heading hold direction -#define MSP_SET_SERVO_CONF 212 //in message Servo settings +#define MSP_SET_SERVO_CONFIGURATION 212 //in message Servo settings #define MSP_SET_MOTOR 214 //in message PropBalance function #define MSP_SET_NAV_CONFIG 215 //in message Sets nav config parameters - write to the eeprom @@ -305,13 +302,12 @@ static const char * const boardIdentifier = TARGET_BOARD_IDENTIFIER; #define MSP_ACC_TRIM 240 //out message get acc angle trim values #define MSP_SET_ACC_TRIM 239 //in message set acc angle trim values #define MSP_GPSSVINFO 164 //out message get Signal Strength (only U-Blox) -#define MSP_SERVOMIX_CONF 241 //out message Returns servo mixer configuration -#define MSP_SET_SERVOMIX_CONF 242 //in message Sets servo mixer configuration +#define MSP_SERVO_MIX_RULES 241 //out message Returns servo mixer configuration +#define MSP_SET_SERVO_MIX_RULE 242 //in message Sets servo mixer configuration -#define SERVO_CHUNK_SIZE 13 +#define SERVO_CHUNK_SIZE 14 -// FIXME This is now too big! -#define INBUF_SIZE (SERVO_CHUNK_SIZE * MAX_SUPPORTED_SERVOS) +#define INBUF_SIZE 64 typedef struct box_e { const uint8_t boxId; // see boxId_e @@ -628,8 +624,6 @@ void mspReleasePortIfAllocated(serialPort_t *serialPort) void mspInit(serialConfig_t *serialConfig) { - BUILD_BUG_ON((SERVO_CHUNK_SIZE * MAX_SUPPORTED_SERVOS) > INBUF_SIZE); - // calculate used boxes based on features and fill availableBoxes[] array memset(activeBoxIds, 0xFF, sizeof(activeBoxIds)); @@ -842,7 +836,7 @@ static bool processOutCommand(uint8_t cmdMSP) case MSP_SERVO: s_struct((uint8_t *)&servo, MAX_SUPPORTED_SERVOS * 2); break; - case MSP_SERVO_CONF: + case MSP_SERVO_CONFIGURATIONS: headSerialReply(MAX_SUPPORTED_SERVOS * SERVO_CHUNK_SIZE); for (i = 0; i < MAX_SUPPORTED_SERVOS; i++) { serialize16(currentProfile->servoConf[i].min); @@ -852,19 +846,14 @@ static bool processOutCommand(uint8_t cmdMSP) serialize8(currentProfile->servoConf[i].angleAtMin); serialize8(currentProfile->servoConf[i].angleAtMax); serialize32(currentProfile->servoConf[i].reversedSources); - } - break; - case MSP_CHANNEL_FORWARDING: - headSerialReply(MAX_SUPPORTED_SERVOS); - for (i = 0; i < MAX_SUPPORTED_SERVOS; i++) { serialize8(currentProfile->servoConf[i].forwardFromChannel); } break; - case MSP_SERVOMIX_CONF: + case MSP_SERVO_MIX_RULES: headSerialReply(MAX_SERVO_RULES * sizeof(servoMixer_t)); for (i = 0; i < MAX_SERVO_RULES; i++) { serialize8(masterConfig.customServoMixer[i].targetChannel); - serialize8(masterConfig.customServoMixer[i].fromChannel); + serialize8(masterConfig.customServoMixer[i].inputSource); serialize8(masterConfig.customServoMixer[i].rate); serialize8(masterConfig.customServoMixer[i].speed); serialize8(masterConfig.customServoMixer[i].min); @@ -872,7 +861,6 @@ static bool processOutCommand(uint8_t cmdMSP) serialize8(masterConfig.customServoMixer[i].box); } break; - #endif case MSP_MOTOR: s_struct((uint8_t *)motor, 16); @@ -1439,45 +1427,39 @@ static bool processInCommand(void) for (i = 0; i < 8; i++) // FIXME should this use MAX_MOTORS or MAX_SUPPORTED_MOTORS instead of 8 motor_disarmed[i] = read16(); break; - case MSP_SET_SERVO_CONF: + case MSP_SET_SERVO_CONFIGURATION: #ifdef USE_SERVOS - if (currentPort->dataSize % SERVO_CHUNK_SIZE != 0) { - debug[0] = currentPort->dataSize; + i = read8(); + if (i >= MAX_SUPPORTED_SERVOS) { headSerialError(0); } else { - uint8_t servoCount = currentPort->dataSize / SERVO_CHUNK_SIZE; - for (i = 0; i < MAX_SUPPORTED_SERVOS && i < servoCount; i++) { - currentProfile->servoConf[i].min = read16(); - currentProfile->servoConf[i].max = read16(); - currentProfile->servoConf[i].middle = read16(); - currentProfile->servoConf[i].rate = read8(); - currentProfile->servoConf[i].angleAtMin = read8(); - currentProfile->servoConf[i].angleAtMax = read8(); - currentProfile->servoConf[i].reversedSources = read32(); - } - } -#endif - break; - case MSP_SET_CHANNEL_FORWARDING: -#ifdef USE_SERVOS - for (i = 0; i < MAX_SUPPORTED_SERVOS; i++) { + currentProfile->servoConf[i].min = read16(); + currentProfile->servoConf[i].max = read16(); + currentProfile->servoConf[i].middle = read16(); + currentProfile->servoConf[i].rate = read8(); + currentProfile->servoConf[i].angleAtMin = read8(); + currentProfile->servoConf[i].angleAtMax = read8(); + currentProfile->servoConf[i].reversedSources = read32(); currentProfile->servoConf[i].forwardFromChannel = read8(); } #endif break; - case MSP_SET_SERVOMIX_CONF: + case MSP_SET_SERVO_MIX_RULE: #ifdef USE_SERVOS - for (i = 0; i < MAX_SERVO_RULES; i++) { + i = read8(); + if (i >= MAX_SERVO_RULES) { + headSerialError(0); + } else { masterConfig.customServoMixer[i].targetChannel = read8(); - masterConfig.customServoMixer[i].fromChannel = read8(); + masterConfig.customServoMixer[i].inputSource = read8(); masterConfig.customServoMixer[i].rate = read8(); masterConfig.customServoMixer[i].speed = read8(); masterConfig.customServoMixer[i].min = read8(); masterConfig.customServoMixer[i].max = read8(); masterConfig.customServoMixer[i].box = read8(); + loadCustomServoMixer(); } - loadCustomServoMixer(); #endif break;