Skip to content

Commit

Permalink
Merge pull request cleanflight#658 from iNavFlight/flight-modes-cleanup
Browse files Browse the repository at this point in the history 
Flight modes cleanup
  • Loading branch information
@DzikuVx
DzikuVx committed Oct 12, 2016
2 parents f6f5d7c + 7fe0ee8 commit dac3992
Show file tree
Hide file tree
Showing 5 changed files with 40 additions and 72 deletions.
22 changes: 2 additions & 20 deletions src/main/fc/msp_fc.c
View file
Expand Up @@ -119,21 +119,15 @@ static const box_t boxes[CHECKBOX_ITEM_COUNT + 1] = {
{ BOXHEADFREE, "HEADFREE;", 6 },
{ BOXHEADADJ, "HEADADJ;", 7 },
{ BOXCAMSTAB, "CAMSTAB;", 8 },
{ BOXCAMTRIG, "CAMTRIG;", 9 },
{ BOXNAVRTH, "NAV RTH;", 10 }, // old GPS HOME
{ BOXNAVPOSHOLD, "NAV POSHOLD;", 11 }, // old GPS HOLD
{ BOXPASSTHRU, "PASSTHRU;", 12 },
{ BOXBEEPERON, "BEEPER;", 13 },
{ BOXLEDMAX, "LEDMAX;", 14 },
{ BOXLEDLOW, "LEDLOW;", 15 },
{ BOXLLIGHTS, "LLIGHTS;", 16 },
{ BOXGOV, "GOVERNOR;", 18 },
{ BOXOSD, "OSD SW;", 19 },
{ BOXTELEMETRY, "TELEMETRY;", 20 },
//{ BOXGTUNE, "GTUNE;", 21 },
{ BOXSERVO1, "SERVO1;", 23 },
{ BOXSERVO2, "SERVO2;", 24 },
{ BOXSERVO3, "SERVO3;", 25 },
{ BOXBLACKBOX, "BLACKBOX;", 26 },
{ BOXFAILSAFE, "FAILSAFE;", 27 },
{ BOXNAVWP, "NAV WP;", 28 },
Expand Down Expand Up @@ -318,14 +312,6 @@ static void initActiveBoxIds(void)
activeBoxIds[activeBoxIdCount++] = BOXTELEMETRY;
#endif

#ifdef USE_SERVOS
if (masterConfig.mixerMode == MIXER_CUSTOM_AIRPLANE) {
activeBoxIds[activeBoxIdCount++] = BOXSERVO1;
activeBoxIds[activeBoxIdCount++] = BOXSERVO2;
activeBoxIds[activeBoxIdCount++] = BOXSERVO3;
}
#endif

#ifdef BLACKBOX
if (feature(FEATURE_BLACKBOX)){
activeBoxIds[activeBoxIdCount++] = BOXBLACKBOX;
Expand All @@ -350,13 +336,10 @@ static uint32_t packFlightModeFlags(void)
IS_ENABLED(FLIGHT_MODE(HEADFREE_MODE)) << BOXHEADFREE |
IS_ENABLED(IS_RC_MODE_ACTIVE(BOXHEADADJ)) << BOXHEADADJ |
IS_ENABLED(IS_RC_MODE_ACTIVE(BOXCAMSTAB)) << BOXCAMSTAB |
IS_ENABLED(IS_RC_MODE_ACTIVE(BOXCAMTRIG)) << BOXCAMTRIG |
IS_ENABLED(FLIGHT_MODE(PASSTHRU_MODE)) << BOXPASSTHRU |
IS_ENABLED(IS_RC_MODE_ACTIVE(BOXBEEPERON)) << BOXBEEPERON |
IS_ENABLED(IS_RC_MODE_ACTIVE(BOXLEDMAX)) << BOXLEDMAX |
IS_ENABLED(IS_RC_MODE_ACTIVE(BOXLEDLOW)) << BOXLEDLOW |
IS_ENABLED(IS_RC_MODE_ACTIVE(BOXLLIGHTS)) << BOXLLIGHTS |
IS_ENABLED(IS_RC_MODE_ACTIVE(BOXGOV)) << BOXGOV |
IS_ENABLED(IS_RC_MODE_ACTIVE(BOXOSD)) << BOXOSD |
IS_ENABLED(IS_RC_MODE_ACTIVE(BOXTELEMETRY)) << BOXTELEMETRY |
IS_ENABLED(ARMING_FLAG(ARMED)) << BOXARM |
Expand Down Expand Up @@ -608,7 +591,7 @@ static bool mspFcProcessOutCommand(uint8_t cmdMSP, sbuf_t *dst, sbuf_t *src, msp
sbufWriteU8(dst, masterConfig.customServoMixer[i].speed);
sbufWriteU8(dst, masterConfig.customServoMixer[i].min);
sbufWriteU8(dst, masterConfig.customServoMixer[i].max);
sbufWriteU8(dst, masterConfig.customServoMixer[i].box);
sbufWriteU8(dst, 0);
}
break;
#endif
Expand Down Expand Up @@ -1282,7 +1265,7 @@ static mspResult_e mspFcProcessInCommand(uint8_t cmdMSP, sbuf_t *src)
masterConfig.customServoMixer[i].speed = sbufReadU8(src);
masterConfig.customServoMixer[i].min = sbufReadU8(src);
masterConfig.customServoMixer[i].max = sbufReadU8(src);
masterConfig.customServoMixer[i].box = sbufReadU8(src);
sbufReadU8(src); //Read 1 byte for `box` and ignore it
loadCustomServoMixer();
}
#endif
Expand Down Expand Up @@ -1599,4 +1582,3 @@ mspProcessCommandFnPtr mspFcInit(void)
initActiveBoxIds();
return mspFcProcessCommand;
}

6 changes: 0 additions & 6 deletions src/main/fc/rc_controls.h
View file
Expand Up @@ -28,21 +28,15 @@ typedef enum {
BOXHEADFREE,
BOXHEADADJ,
BOXCAMSTAB,
BOXCAMTRIG,
BOXNAVRTH, // old GPSHOME
BOXNAVPOSHOLD, // old GPSHOLD
BOXPASSTHRU,
BOXBEEPERON,
BOXLEDMAX,
BOXLEDLOW,
BOXLLIGHTS,
BOXGOV,
BOXOSD,
BOXTELEMETRY,
//BOXGTUNE,
BOXSERVO1,
BOXSERVO2,
BOXSERVO3,
BOXBLACKBOX,
BOXFAILSAFE,
BOXNAVWP,
Expand Down
77 changes: 36 additions & 41 deletions src/main/flight/servos.c
View file
Expand Up @@ -85,23 +85,23 @@ static bool servoFilterIsSet;
#define COUNT_SERVO_RULES(rules) (sizeof(rules) / sizeof(servoMixer_t))
// mixer rule format servo, input, rate, speed, min, max, box
static const servoMixer_t servoMixerAirplane[] = {
{ SERVO_FLAPPERON_1, INPUT_STABILIZED_ROLL, 100, 0, 0, 100, 0 },
{ SERVO_FLAPPERON_2, INPUT_STABILIZED_ROLL, 100, 0, 0, 100, 0 },
{ SERVO_RUDDER, INPUT_STABILIZED_YAW, 100, 0, 0, 100, 0 },
{ SERVO_ELEVATOR, INPUT_STABILIZED_PITCH, 100, 0, 0, 100, 0 },
{ SERVO_THROTTLE, INPUT_STABILIZED_THROTTLE, 100, 0, 0, 100, 0 },
{ SERVO_FLAPPERON_1, INPUT_STABILIZED_ROLL, 100, 0, 0, 100 },
{ SERVO_FLAPPERON_2, INPUT_STABILIZED_ROLL, 100, 0, 0, 100 },
{ SERVO_RUDDER, INPUT_STABILIZED_YAW, 100, 0, 0, 100 },
{ SERVO_ELEVATOR, INPUT_STABILIZED_PITCH, 100, 0, 0, 100 },
{ SERVO_THROTTLE, INPUT_STABILIZED_THROTTLE, 100, 0, 0, 100 },
};

static const servoMixer_t servoMixerFlyingWing[] = {
{ SERVO_FLAPPERON_1, INPUT_STABILIZED_ROLL, 100, 0, 0, 100, 0 },
{ SERVO_FLAPPERON_1, INPUT_STABILIZED_PITCH, 100, 0, 0, 100, 0 },
{ SERVO_FLAPPERON_2, INPUT_STABILIZED_ROLL, -100, 0, 0, 100, 0 },
{ SERVO_FLAPPERON_2, INPUT_STABILIZED_PITCH, 100, 0, 0, 100, 0 },
{ SERVO_THROTTLE, INPUT_STABILIZED_THROTTLE, 100, 0, 0, 100, 0 },
{ SERVO_FLAPPERON_1, INPUT_STABILIZED_ROLL, 100, 0, 0, 100 },
{ SERVO_FLAPPERON_1, INPUT_STABILIZED_PITCH, 100, 0, 0, 100 },
{ SERVO_FLAPPERON_2, INPUT_STABILIZED_ROLL, -100, 0, 0, 100 },
{ SERVO_FLAPPERON_2, INPUT_STABILIZED_PITCH, 100, 0, 0, 100 },
{ SERVO_THROTTLE, INPUT_STABILIZED_THROTTLE, 100, 0, 0, 100 },
};

static const servoMixer_t servoMixerTri[] = {
{ SERVO_RUDDER, INPUT_STABILIZED_YAW, 100, 0, 0, 100, 0 },
{ SERVO_RUDDER, INPUT_STABILIZED_YAW, 100, 0, 0, 100 },
};

const mixerRules_t servoMixers[] = {
Expand Down Expand Up @@ -274,7 +274,7 @@ void servoMixerLoadMix(int index, servoMixer_t *customServoMixers)
index++;
// clear existing
for (i = 0; i < MAX_SERVO_RULES; i++)
customServoMixers[i].targetChannel = customServoMixers[i].inputSource = customServoMixers[i].rate = customServoMixers[i].box = 0;
customServoMixers[i].targetChannel = customServoMixers[i].inputSource = customServoMixers[i].rate = 0;

for (i = 0; i < servoMixers[index].servoRuleCount; i++) {
customServoMixers[i] = servoMixers[index].rule[i];
Expand Down Expand Up @@ -377,40 +377,35 @@ void servoMixer(uint16_t flaperon_throw_offset, uint8_t flaperon_throw_inverted)

// mix servos according to rules
for (i = 0; i < servoRuleCount; i++) {
// 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].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;

if (currentServoMixer[i].speed == 0) {
currentOutput[i] = input[from];
} else {
if (currentOutput[i] < input[from]) {
currentOutput[i] = constrain(currentOutput[i] + currentServoMixer[i].speed, currentOutput[i], input[from]);
} else if (currentOutput[i] > input[from]) {
currentOutput[i] = constrain(currentOutput[i] - currentServoMixer[i].speed, input[from], currentOutput[i]);
}
uint8_t target = currentServoMixer[i].targetChannel;
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;

if (currentServoMixer[i].speed == 0) {
currentOutput[i] = input[from];
} else {
if (currentOutput[i] < input[from]) {
currentOutput[i] = constrain(currentOutput[i] + currentServoMixer[i].speed, currentOutput[i], input[from]);
} else if (currentOutput[i] > input[from]) {
currentOutput[i] = constrain(currentOutput[i] - currentServoMixer[i].speed, input[from], currentOutput[i]);
}
}

/*
Flaperon fligh mode
*/
if (FLIGHT_MODE(FLAPERON) && (target == SERVO_FLAPPERON_1 || target == SERVO_FLAPPERON_2)) {
int8_t multiplier = 1;
/*
Flaperon fligh mode
*/
if (FLIGHT_MODE(FLAPERON) && (target == SERVO_FLAPPERON_1 || target == SERVO_FLAPPERON_2)) {
int8_t multiplier = 1;

if (flaperon_throw_inverted == 1) {
multiplier = -1;
}
currentOutput[i] += flaperon_throw_offset * getFlaperonDirection(target) * multiplier;
if (flaperon_throw_inverted == 1) {
multiplier = -1;
}

servo[target] += servoDirection(target, from) * constrain(((int32_t)currentOutput[i] * currentServoMixer[i].rate) / 100, min, max);
} else {
currentOutput[i] = 0;
currentOutput[i] += flaperon_throw_offset * getFlaperonDirection(target) * multiplier;
}

servo[target] += servoDirection(target, from) * constrain(((int32_t)currentOutput[i] * currentServoMixer[i].rate) / 100, min, max);
}

for (i = 0; i < MAX_SUPPORTED_SERVOS; i++) {
Expand Down
2 changes: 0 additions & 2 deletions src/main/flight/servos.h
View file
Expand Up @@ -90,7 +90,6 @@ typedef struct servoMixer_s {
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
int8_t max; // lower bound of rule range [0;100]% of servo max-min
uint8_t box; // active rule if box is enabled, range [0;3], 0=no box, 1=BOXSERVO1, 2=BOXSERVO2, 3=BOXSERVO3
} servoMixer_t;

#define MAX_SERVO_RULES (2 * MAX_SUPPORTED_SERVOS)
Expand Down Expand Up @@ -129,4 +128,3 @@ void loadCustomServoMixer(void);
int servoDirection(int servoIndex, int fromChannel);
void servosUseConfigs(servoMixerConfig_t *servoConfigToUse, servoParam_t *servoParamsToUse, gimbalConfig_t *gimbalConfigToUse, struct rxConfig_s *rxConfigToUse);
void servosInit(servoMixer_t *customServoMixers);

5 changes: 2 additions & 3 deletions src/main/io/serial_cli.c
View file
Expand Up @@ -1622,7 +1622,7 @@ static void cliServoMix(char *cmdline)
masterConfig.customServoMixer[i].speed,
masterConfig.customServoMixer[i].min,
masterConfig.customServoMixer[i].max,
masterConfig.customServoMixer[i].box
0
);
}
cliPrintf("\r\n");
Expand Down Expand Up @@ -1721,7 +1721,6 @@ static void cliServoMix(char *cmdline)
masterConfig.customServoMixer[i].speed = args[SPEED];
masterConfig.customServoMixer[i].min = args[MIN];
masterConfig.customServoMixer[i].max = args[MAX];
masterConfig.customServoMixer[i].box = args[BOX];
cliServoMix("");
} else {
cliShowParseError();
Expand Down Expand Up @@ -1992,7 +1991,7 @@ static void cliDump(char *cmdline)
masterConfig.customServoMixer[i].speed,
masterConfig.customServoMixer[i].min,
masterConfig.customServoMixer[i].max,
masterConfig.customServoMixer[i].box
0
);
}
#endif // USE_SERVOS
Expand Down

0 comments on commit dac3992

Please sign in to comment.