mirror of
https://github.com/iNavFlight/inav.git
synced 2025-07-23 00:05:28 +03:00
BOXSERVO1-3 removed
This commit is contained in:
parent
b6b6324462
commit
7fe0ee8193
5 changed files with 42 additions and 64 deletions
|
@ -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
|
||||||
|
|
|
@ -37,9 +37,6 @@ typedef enum {
|
||||||
BOXOSD,
|
BOXOSD,
|
||||||
BOXTELEMETRY,
|
BOXTELEMETRY,
|
||||||
//BOXGTUNE,
|
//BOXGTUNE,
|
||||||
BOXSERVO1,
|
|
||||||
BOXSERVO2,
|
|
||||||
BOXSERVO3,
|
|
||||||
BOXBLACKBOX,
|
BOXBLACKBOX,
|
||||||
BOXFAILSAFE,
|
BOXFAILSAFE,
|
||||||
BOXNAVWP,
|
BOXNAVWP,
|
||||||
|
|
|
@ -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++) {
|
||||||
|
|
|
@ -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);
|
||||||
|
|
||||||
|
|
|
@ -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
|
||||||
|
|
Loading…
Add table
Add a link
Reference in a new issue