Mirror/inav
1
0
Fork 0
mirror of https://github.com/iNavFlight/inav.git synced 2025-07-25 09:16:01 +03:00
Code Issues Wiki Activity

BOXSERVO1-3 removed

Browse source
This commit is contained in:
Pawel Spychalski (DzikuVx) 2016-10-04 21:45:16 +02:00
parent b6b6324462
commit 7fe0ee8193
5 changed files with 42 additions and 64 deletions
Download patch file Download diff file Expand all files Collapse all files

15
src/main/fc/msp_fc.c
View file

@ -128,9 +128,6 @@ static const box_t boxes[CHECKBOX_ITEM_COUNT + 1] = {
{ BOXOSD, "OSD SW;", 19 }, { BOXOSD, "OSD SW;", 19 },
{ BOXTELEMETRY, "TELEMETRY;", 20 }, { BOXTELEMETRY, "TELEMETRY;", 20 },
//{ BOXGTUNE, "GTUNE;", 21 }, //{ BOXGTUNE, "GTUNE;", 21 },
{ BOXSERVO1, "SERVO1;", 23 },
{ BOXSERVO2, "SERVO2;", 24 },
{ BOXSERVO3, "SERVO3;", 25 },
{ BOXBLACKBOX, "BLACKBOX;", 26 }, { BOXBLACKBOX, "BLACKBOX;", 26 },
{ BOXFAILSAFE, "FAILSAFE;", 27 }, { BOXFAILSAFE, "FAILSAFE;", 27 },
{ BOXNAVWP, "NAV WP;", 28 }, { BOXNAVWP, "NAV WP;", 28 },
@ -315,14 +312,6 @@ static void initActiveBoxIds(void)
activeBoxIds[activeBoxIdCount++] = BOXTELEMETRY; activeBoxIds[activeBoxIdCount++] = BOXTELEMETRY;
#endif #endif
#ifdef USE_SERVOS
if (masterConfig.mixerMode == MIXER_CUSTOM_AIRPLANE) {
activeBoxIds[activeBoxIdCount++] = BOXSERVO1;
activeBoxIds[activeBoxIdCount++] = BOXSERVO2;
activeBoxIds[activeBoxIdCount++] = BOXSERVO3;
}
#endif
#ifdef BLACKBOX #ifdef BLACKBOX
if (feature(FEATURE_BLACKBOX)){ if (feature(FEATURE_BLACKBOX)){
activeBoxIds[activeBoxIdCount++] = BOXBLACKBOX; activeBoxIds[activeBoxIdCount++] = BOXBLACKBOX;
@ -602,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].speed);
sbufWriteU8(dst, masterConfig.customServoMixer[i].min); sbufWriteU8(dst, masterConfig.customServoMixer[i].min);
sbufWriteU8(dst, masterConfig.customServoMixer[i].max); sbufWriteU8(dst, masterConfig.customServoMixer[i].max);
sbufWriteU8(dst, masterConfig.customServoMixer[i].box); sbufWriteU8(dst, 0);
} }
break; break;
#endif #endif
@ -1276,7 +1265,7 @@ static mspResult_e mspFcProcessInCommand(uint8_t cmdMSP, sbuf_t *src)
masterConfig.customServoMixer[i].speed = sbufReadU8(src); masterConfig.customServoMixer[i].speed = sbufReadU8(src);
masterConfig.customServoMixer[i].min = sbufReadU8(src); masterConfig.customServoMixer[i].min = sbufReadU8(src);
masterConfig.customServoMixer[i].max = 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(); loadCustomServoMixer();
} }
#endif #endif

3
src/main/fc/rc_controls.h
View file

@ -37,9 +37,6 @@ typedef enum {
BOXOSD, BOXOSD,
BOXTELEMETRY, BOXTELEMETRY,
//BOXGTUNE, //BOXGTUNE,
BOXSERVO1,
BOXSERVO2,
BOXSERVO3,
BOXBLACKBOX, BOXBLACKBOX,
BOXFAILSAFE, BOXFAILSAFE,
BOXNAVWP, BOXNAVWP,

81
src/main/flight/servos.c
View file

@ -85,23 +85,23 @@ static bool servoFilterIsSet;
#define COUNT_SERVO_RULES(rules) (sizeof(rules) / sizeof(servoMixer_t)) #define COUNT_SERVO_RULES(rules) (sizeof(rules) / sizeof(servoMixer_t))
// mixer rule format servo, input, rate, speed, min, max, box // mixer rule format servo, input, rate, speed, min, max, box
static const servoMixer_t servoMixerAirplane[] = { static const servoMixer_t servoMixerAirplane[] = {
{ SERVO_FLAPPERON_1, INPUT_STABILIZED_ROLL, 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, 0 }, { SERVO_FLAPPERON_2, INPUT_STABILIZED_ROLL, 100, 0, 0, 100 },
{ SERVO_RUDDER, INPUT_STABILIZED_YAW, 100, 0, 0, 100, 0 }, { SERVO_RUDDER, INPUT_STABILIZED_YAW, 100, 0, 0, 100 },
{ SERVO_ELEVATOR, INPUT_STABILIZED_PITCH, 100, 0, 0, 100, 0 }, { SERVO_ELEVATOR, INPUT_STABILIZED_PITCH, 100, 0, 0, 100 },
{ SERVO_THROTTLE, INPUT_STABILIZED_THROTTLE, 100, 0, 0, 100, 0 }, { SERVO_THROTTLE, INPUT_STABILIZED_THROTTLE, 100, 0, 0, 100 },
}; };
static const servoMixer_t servoMixerFlyingWing[] = { static const servoMixer_t servoMixerFlyingWing[] = {
{ SERVO_FLAPPERON_1, INPUT_STABILIZED_ROLL, 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, 0 }, { SERVO_FLAPPERON_1, INPUT_STABILIZED_PITCH, 100, 0, 0, 100 },
{ SERVO_FLAPPERON_2, INPUT_STABILIZED_ROLL, -100, 0, 0, 100, 0 }, { SERVO_FLAPPERON_2, INPUT_STABILIZED_ROLL, -100, 0, 0, 100 },
{ SERVO_FLAPPERON_2, INPUT_STABILIZED_PITCH, 100, 0, 0, 100, 0 }, { SERVO_FLAPPERON_2, INPUT_STABILIZED_PITCH, 100, 0, 0, 100 },
{ SERVO_THROTTLE, INPUT_STABILIZED_THROTTLE, 100, 0, 0, 100, 0 }, { SERVO_THROTTLE, INPUT_STABILIZED_THROTTLE, 100, 0, 0, 100 },
}; };
static const servoMixer_t servoMixerTri[] = { 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[] = { const mixerRules_t servoMixers[] = {
@ -274,7 +274,7 @@ void servoMixerLoadMix(int index, servoMixer_t *customServoMixers)
index++; index++;
// clear existing // clear existing
for (i = 0; i < MAX_SERVO_RULES; i++) 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++) { for (i = 0; i < servoMixers[index].servoRuleCount; i++) {
customServoMixers[i] = servoMixers[index].rule[i]; customServoMixers[i] = servoMixers[index].rule[i];
@ -377,40 +377,35 @@ void servoMixer(uint16_t flaperon_throw_offset, uint8_t flaperon_throw_inverted)
// mix servos according to rules // mix servos according to rules
for (i = 0; i < servoRuleCount; i++) { for (i = 0; i < servoRuleCount; i++) {
// consider rule if no box assigned or box is active uint8_t target = currentServoMixer[i].targetChannel;
if (currentServoMixer[i].box == 0 || IS_RC_MODE_ACTIVE(BOXSERVO1 + currentServoMixer[i].box - 1)) { uint8_t from = currentServoMixer[i].inputSource;
uint8_t target = currentServoMixer[i].targetChannel; uint16_t servo_width = servoConf[target].max - servoConf[target].min;
uint8_t from = currentServoMixer[i].inputSource; int16_t min = currentServoMixer[i].min * servo_width / 100 - servo_width / 2;
uint16_t servo_width = servoConf[target].max - servoConf[target].min; int16_t max = currentServoMixer[i].max * servo_width / 100 - servo_width / 2;
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) { if (currentServoMixer[i].speed == 0) {
currentOutput[i] = input[from]; 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;
if (flaperon_throw_inverted == 1) {
multiplier = -1;
}
currentOutput[i] += flaperon_throw_offset * getFlaperonDirection(target) * multiplier;
}
servo[target] += servoDirection(target, from) * constrain(((int32_t)currentOutput[i] * currentServoMixer[i].rate) / 100, min, max);
} else { } else {
currentOutput[i] = 0; 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;
if (flaperon_throw_inverted == 1) {
multiplier = -1;
}
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++) { for (i = 0; i < MAX_SUPPORTED_SERVOS; i++) {

2
src/main/flight/servos.h
View file

@ -90,7 +90,6 @@ typedef struct servoMixer_s {
uint8_t speed; // reduces the speed of the rule, 0=unlimited speed 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 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 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; } servoMixer_t;
#define MAX_SERVO_RULES (2 * MAX_SUPPORTED_SERVOS) #define MAX_SERVO_RULES (2 * MAX_SUPPORTED_SERVOS)
@ -129,4 +128,3 @@ void loadCustomServoMixer(void);
int servoDirection(int servoIndex, int fromChannel); int servoDirection(int servoIndex, int fromChannel);
void servosUseConfigs(servoMixerConfig_t *servoConfigToUse, servoParam_t *servoParamsToUse, gimbalConfig_t *gimbalConfigToUse, struct rxConfig_s *rxConfigToUse); void servosUseConfigs(servoMixerConfig_t *servoConfigToUse, servoParam_t *servoParamsToUse, gimbalConfig_t *gimbalConfigToUse, struct rxConfig_s *rxConfigToUse);
void servosInit(servoMixer_t *customServoMixers); void servosInit(servoMixer_t *customServoMixers);

5
src/main/io/serial_cli.c
View file

@ -1622,7 +1622,7 @@ static void cliServoMix(char *cmdline)
masterConfig.customServoMixer[i].speed, masterConfig.customServoMixer[i].speed,
masterConfig.customServoMixer[i].min, masterConfig.customServoMixer[i].min,
masterConfig.customServoMixer[i].max, masterConfig.customServoMixer[i].max,
masterConfig.customServoMixer[i].box 0
); );
} }
cliPrintf("\r\n"); cliPrintf("\r\n");
@ -1721,7 +1721,6 @@ static void cliServoMix(char *cmdline)
masterConfig.customServoMixer[i].speed = args[SPEED]; masterConfig.customServoMixer[i].speed = args[SPEED];
masterConfig.customServoMixer[i].min = args[MIN]; masterConfig.customServoMixer[i].min = args[MIN];
masterConfig.customServoMixer[i].max = args[MAX]; masterConfig.customServoMixer[i].max = args[MAX];
masterConfig.customServoMixer[i].box = args[BOX];
cliServoMix(""); cliServoMix("");
} else { } else {
cliShowParseError(); cliShowParseError();
@ -1992,7 +1991,7 @@ static void cliDump(char *cmdline)
masterConfig.customServoMixer[i].speed, masterConfig.customServoMixer[i].speed,
masterConfig.customServoMixer[i].min, masterConfig.customServoMixer[i].min,
masterConfig.customServoMixer[i].max, masterConfig.customServoMixer[i].max,
masterConfig.customServoMixer[i].box 0
); );
} }
#endif // USE_SERVOS #endif // USE_SERVOS