Mirror/betaflight
1
0
Fork 0
mirror of https://github.com/betaflight/betaflight.git synced 2025-07-17 13:25:30 +03:00
Code Issues Wiki Activity

First cut of custom servo mixers.

Browse source 
Main rule logic and MSP commands ported from baseflight.

Gimbal mixer updated to use rules.  This allows us to remove more
conditional logic.  Operation of gimbal servos is now different.
This commit is contained in:
Dominic Clifton 2015-06-28 22:27:49 +01:00
parent c142e3dfe9
commit ea6a6ccb3d
9 changed files with 617 additions and 204 deletions
Download patch file Download diff file Expand all files Collapse all files

9
src/main/config/config.c
View file

@ -347,10 +347,6 @@ static void setControlRateProfile(uint8_t profileIndex)
static void resetConf(void)
{
int i;
#ifdef USE_SERVOS
int8_t servoRates[MAX_SUPPORTED_SERVOS] = { 30, 30, 100, 100, 100, 100, 100, 100 };
;
#endif
// Clear all configuration
memset(&masterConfig, 0, sizeof(master_t));
@ -417,7 +413,6 @@ static void resetConf(void)
resetMixerConfig(&masterConfig.mixerConfig);
masterConfig.airplaneConfig.flaps_speed = 0;
masterConfig.airplaneConfig.fixedwing_althold_dir = 1;
// Motor/ESC/Servo
@ -482,7 +477,7 @@ static void resetConf(void)
currentProfile->servoConf[i].min = DEFAULT_SERVO_MIN;
currentProfile->servoConf[i].max = DEFAULT_SERVO_MAX;
currentProfile->servoConf[i].middle = DEFAULT_SERVO_MIDDLE;
currentProfile->servoConf[i].rate = servoRates[i];
currentProfile->servoConf[i].rate = 100;
currentProfile->servoConf[i].angleAtMin = DEFAULT_SERVO_MIN_ANGLE;
currentProfile->servoConf[i].angleAtMax = DEFAULT_SERVO_MAX_ANGLE;
currentProfile->servoConf[i].forwardFromChannel = CHANNEL_FORWARDING_DISABLED;
@ -498,7 +493,7 @@ static void resetConf(void)
// custom mixer. clear by defaults.
for (i = 0; i < MAX_SUPPORTED_MOTORS; i++)
masterConfig.customMixer[i].throttle = 0.0f;
masterConfig.customMotorMixer[i].throttle = 0.0f;
#ifdef LED_STRIP
applyDefaultColors(masterConfig.colors, CONFIGURABLE_COLOR_COUNT);

6
src/main/config/config_master.h
View file

@ -28,8 +28,10 @@ typedef struct master_t {
uint16_t looptime; // imu loop time in us
uint8_t emf_avoidance; // change pll settings to avoid noise in the uhf band
motorMixer_t customMixer[MAX_SUPPORTED_MOTORS]; // custom mixtable
motorMixer_t customMotorMixer[MAX_SUPPORTED_MOTORS];
#ifdef USE_SERVOS
servoMixer_t customServoMixer[MAX_SERVO_RULES];
#endif
// motor/esc/servo related stuff
escAndServoConfig_t escAndServoConfig;
flight3DConfig_t flight3DConfig;

358
src/main/flight/mixer.c
View file

@ -18,6 +18,7 @@
#include <stdbool.h>
#include <stdint.h>
#include <stdlib.h>
#include <string.h>
#include "platform.h"
#include "debug.h"
@ -90,6 +91,7 @@ typedef enum {
//#define MIXER_DEBUG
uint8_t motorCount = 0;
int16_t motor[MAX_SUPPORTED_MOTORS];
int16_t motor_disarmed[MAX_SUPPORTED_MOTORS];
@ -99,10 +101,13 @@ static escAndServoConfig_t *escAndServoConfig;
static airplaneConfig_t *airplaneConfig;
static rxConfig_t *rxConfig;
static motorMixer_t currentMixer[MAX_SUPPORTED_MOTORS];
static mixerMode_e currentMixerMode;
static motorMixer_t currentMixer[MAX_SUPPORTED_MOTORS];
#ifdef USE_SERVOS
static uint8_t servoRuleCount = 0;
static servoMixer_t currentServoMixer[MAX_SERVO_RULES];
static gimbalConfig_t *gimbalConfig;
int16_t servo[MAX_SUPPORTED_SERVOS];
static int useServo;
@ -231,6 +236,11 @@ static const motorMixer_t mixerDualcopter[] = {
{ 1.0f, 0.0f, 0.0f, 1.0f }, // RIGHT
};
static const motorMixer_t mixerThrustVector[] = {
{ 1.0f, 0.0f, 0.0f, -0.5f }, // LEFT
{ 1.0f, 0.0f, 0.0f, 0.5f }, // RIGHT
};
// Keep synced with mixerMode_e
const mixer_t mixers[] = {
// motors, servos, motor mixer
@ -242,7 +252,7 @@ const mixer_t mixers[] = {
{ 0, 1, NULL }, // * MIXER_GIMBAL
{ 6, 0, mixerY6 }, // MIXER_Y6
{ 6, 0, mixerHex6P }, // MIXER_HEX6
{ 1, 1, NULL }, // * MIXER_FLYING_WING
{ 2, 1, mixerThrustVector }, // * MIXER_FLYING_WING
{ 4, 0, mixerY4 }, // MIXER_Y4
{ 6, 0, mixerHex6X }, // MIXER_HEX6X
{ 8, 0, mixerOctoX8 }, // MIXER_OCTOX8
@ -258,9 +268,91 @@ const mixer_t mixers[] = {
{ 1, 1, NULL }, // MIXER_SINGLECOPTER
{ 4, 0, mixerAtail4 }, // MIXER_ATAIL4
{ 0, 0, NULL }, // MIXER_CUSTOM
{ 1, 1, NULL }, // MIXER_CUSTOM_PLANE
};
#endif
#ifdef USE_SERVOS
// mixer rule format servo, input, rate, speed, min, max, box
static const servoMixer_t servoMixerAirplane[] = {
{ SERVO_FLAPPERON_1, INPUT_ROLL, 100, 0, 0, 100, 0 },
{ SERVO_FLAPPERON_2, INPUT_ROLL, 100, 0, 0, 100, 0 },
{ SERVO_RUDDER, INPUT_YAW, 100, 0, 0, 100, 0 },
{ SERVO_ELEVATOR, INPUT_PITCH, 100, 0, 0, 100, 0 },
};
static const servoMixer_t servoMixerFlyingWing[] = {
{ SERVO_FLAPPERON_1, INPUT_ROLL, 100, 0, 0, 100, 0 },
{ SERVO_FLAPPERON_1, INPUT_PITCH, 100, 0, 0, 100, 0 },
{ SERVO_FLAPPERON_2, INPUT_ROLL, 100, 0, 0, 100, 0 },
{ SERVO_FLAPPERON_2, INPUT_PITCH, 100, 0, 0, 100, 0 },
};
static const servoMixer_t servoMixerBI[] = {
{ SERVO_BIPLANE_LEFT, INPUT_YAW, 100, 0, 0, 100, 0 },
{ SERVO_BIPLANE_LEFT, INPUT_PITCH, 100, 0, 0, 100, 0 },
{ SERVO_BIPLANE_RIGHT, INPUT_YAW, 100, 0, 0, 100, 0 },
{ SERVO_BIPLANE_RIGHT, INPUT_PITCH, 100, 0, 0, 100, 0 },
};
static const servoMixer_t servoMixerTri[] = {
{ SERVO_RUDDER, INPUT_YAW, 100, 0, 0, 100, 0 },
};
static const servoMixer_t servoMixerDual[] = {
{ SERVO_DUALCOPTER_LEFT, INPUT_PITCH, 100, 0, 0, 100, 0 },
{ SERVO_DUALCOPTER_RIGHT, INPUT_ROLL, 100, 0, 0, 100, 0 },
};
static const servoMixer_t servoMixerSingle[] = {
{ SERVO_SINGLECOPTER_1, INPUT_YAW, 100, 0, 0, 100, 0 },
{ SERVO_SINGLECOPTER_1, INPUT_PITCH, 100, 0, 0, 100, 0 },
{ SERVO_SINGLECOPTER_2, INPUT_YAW, 100, 0, 0, 100, 0 },
{ SERVO_SINGLECOPTER_2, INPUT_PITCH, 100, 0, 0, 100, 0 },
{ SERVO_SINGLECOPTER_3, INPUT_YAW, 100, 0, 0, 100, 0 },
{ SERVO_SINGLECOPTER_3, INPUT_ROLL, 100, 0, 0, 100, 0 },
{ SERVO_SINGLECOPTER_4, INPUT_YAW, 100, 0, 0, 100, 0 },
{ SERVO_SINGLECOPTER_4, INPUT_ROLL, 100, 0, 0, 100, 0 },
};
static const servoMixer_t servoMixerGimbal[] = {
{ SERVO_GIMBAL_PITCH, INPUT_GIMBAL_PITCH, 125, 0, 0, 100, 0 },
{ SERVO_GIMBAL_ROLL, INPUT_GIMBAL_ROLL, 125, 0, 0, 100, 0 },
};
const mixerRules_t servoMixers[] = {
{ 0, NULL }, // entry 0
{ 1, servoMixerTri }, // MULTITYPE_TRI
{ 0, NULL }, // MULTITYPE_QUADP
{ 0, NULL }, // MULTITYPE_QUADX
{ 4, servoMixerBI }, // MULTITYPE_BI
{ 2, servoMixerGimbal }, // * MULTITYPE_GIMBAL
{ 0, NULL }, // MULTITYPE_Y6
{ 0, NULL }, // MULTITYPE_HEX6
{ 4, servoMixerFlyingWing },// * MULTITYPE_FLYING_WING
{ 0, NULL }, // MULTITYPE_Y4
{ 0, NULL }, // MULTITYPE_HEX6X
{ 0, NULL }, // MULTITYPE_OCTOX8
{ 0, NULL }, // MULTITYPE_OCTOFLATP
{ 0, NULL }, // MULTITYPE_OCTOFLATX
{ 4, servoMixerAirplane }, // * MULTITYPE_AIRPLANE
{ 0, NULL }, // * MULTITYPE_HELI_120_CCPM
{ 0, NULL }, // * MULTITYPE_HELI_90_DEG
{ 0, NULL }, // MULTITYPE_VTAIL4
{ 0, NULL }, // MULTITYPE_HEX6H
{ 0, NULL }, // * MULTITYPE_PPM_TO_SERVO
{ 2, servoMixerDual }, // MULTITYPE_DUALCOPTER
{ 8, servoMixerSingle }, // MULTITYPE_SINGLECOPTER
{ 0, NULL }, // MULTITYPE_ATAIL4
{ 0, NULL }, // MULTITYPE_CUSTOM
{ 0, NULL }, // MULTITYPE_CUSTOM_PLANE
};
static servoMixer_t *customServoMixers;
#endif
static motorMixer_t *customMixers;
void mixerUseConfigs(
@ -297,15 +389,10 @@ int16_t determineServoMiddleOrForwardFromChannel(servoIndex_e servoIndex)
return servoConf[servoIndex].middle;
}
int servoDirection(servoIndex_e servoIndex, int lr)
int servoDirection(int servoIndex, int fromChannel)
{
// servo.rate is overloaded for servos that don't have a rate, but only need direction
// bit set = negative, clear = positive
// rate[2] = ???_direction
// rate[1] = roll_direction
// rate[0] = pitch_direction
// servo.rate is also used as gimbal gain multiplier (yeah)
if (servoConf[servoIndex].rate & lr)
// determine the direction (reversed or not) from the direction bitfield of the servo
if (servoConf[servoIndex].reversedChannels & (1 << fromChannel))
return -1;
else
return 1;
@ -313,11 +400,32 @@ int servoDirection(servoIndex_e servoIndex, int lr)
#endif
#ifndef USE_QUAD_MIXER_ONLY
void mixerInit(mixerMode_e mixerMode, motorMixer_t *initialCustomMixers)
void loadCustomServoMixer(void)
{
uint8_t i;
// reset settings
servoRuleCount = 0;
memset(currentServoMixer, 0, sizeof(currentServoMixer));
// load custom mixer into currentServoMixer
for (i = 0; i < MAX_SERVO_RULES; i++) {
// check if done
if (customServoMixers[i].rate == 0)
break;
currentServoMixer[i] = customServoMixers[i];
servoRuleCount++;
}
}
void mixerInit(mixerMode_e mixerMode, motorMixer_t *initialCustomMotorMixers, servoMixer_t *initialCustomServoMixers)
{
currentMixerMode = mixerMode;
customMixers = initialCustomMixers;
customMixers = initialCustomMotorMixers;
customServoMixers = initialCustomServoMixers;
// enable servos for mixes that require them. note, this shifts motor counts.
useServo = mixers[currentMixerMode].useServo;
@ -355,6 +463,14 @@ void mixerUsePWMOutputConfiguration(pwmOutputConfiguration_t *pwmOutputConfigura
}
}
if (useServo) {
servoRuleCount = servoMixers[currentMixerMode].servoRuleCount;
if (servoMixers[currentMixerMode].rule) {
for (i = 0; i < servoRuleCount; i++)
currentServoMixer[i] = servoMixers[currentMixerMode].rule[i];
}
}
// in 3D mode, mixer gain has to be halved
if (feature(FEATURE_3D)) {
if (motorCount > 1) {
@ -368,14 +484,36 @@ void mixerUsePWMOutputConfiguration(pwmOutputConfiguration_t *pwmOutputConfigura
// set flag that we're on something with wings
if (currentMixerMode == MIXER_FLYING_WING ||
currentMixerMode == MIXER_AIRPLANE)
currentMixerMode == MIXER_AIRPLANE ||
currentMixerMode == MIXER_CUSTOM_AIRPLANE
) {
ENABLE_STATE(FIXED_WING);
else
if (currentMixerMode == MIXER_CUSTOM_AIRPLANE) {
loadCustomServoMixer();
}
} else {
DISABLE_STATE(FIXED_WING);
}
mixerResetMotors();
}
void servoMixerLoadMix(int index, servoMixer_t *customServoMixers)
{
int i;
// we're 1-based
index++;
// clear existing
for (i = 0; i < MAX_SERVO_RULES; i++)
customServoMixers[i].targetChannel = customServoMixers[i].fromChannel = customServoMixers[i].rate = customServoMixers[i].box = 0;
for (i = 0; i < servoMixers[index].servoRuleCount; i++)
customServoMixers[i] = servoMixers[index].rule[i];
}
void mixerLoadMix(int index, motorMixer_t *customMixers)
{
int i;
@ -491,12 +629,21 @@ void writeServos(void)
}
break;
case MIXER_CUSTOM_AIRPLANE:
pwmWriteServo(servoIndex++, servo[SERVO_FLAPPERON_1]);
pwmWriteServo(servoIndex++, servo[SERVO_FLAPPERON_2]);
pwmWriteServo(servoIndex++, servo[SERVO_RUDDER]);
pwmWriteServo(servoIndex++, servo[SERVO_ELEVATOR]);
pwmWriteServo(servoIndex++, servo[SERVO_FLAPS]);
pwmWriteServo(servoIndex++, servo[SERVO_THROTTLE]);
break;
default:
break;
}
// Two servos for SERVO_TILT, if enabled
if (feature(FEATURE_SERVO_TILT)) {
if (feature(FEATURE_SERVO_TILT) || currentMixerMode == MIXER_GIMBAL) {
updateGimbalServos(servoIndex);
servoIndex += 2;
}
@ -545,52 +692,88 @@ void StopPwmAllMotors()
}
#ifndef USE_QUAD_MIXER_ONLY
static void airplaneMixer(void)
static void servoMixer(void)
{
int16_t flapperons[2] = { 0, 0 };
int i;
int16_t input[INPUT_ITEM_COUNT];
static int16_t currentOutput[MAX_SERVO_RULES];
uint8_t i;
if (!ARMING_FLAG(ARMED))
servo[SERVO_THROTTLE] = escAndServoConfig->mincommand; // Kill throttle when disarmed
else
servo[SERVO_THROTTLE] = constrain(rcCommand[THROTTLE], escAndServoConfig->minthrottle, escAndServoConfig->maxthrottle);
motor[0] = servo[SERVO_THROTTLE];
if (airplaneConfig->flaps_speed) {
// configure SERVO3 middle point in GUI to using an AUX channel for FLAPS control
// use servo min, servo max and servo rate for proper endpoints adjust
static int16_t slow_LFlaps;
int16_t lFlap = determineServoMiddleOrForwardFromChannel(SERVO_FLAPS);
lFlap = constrain(lFlap, servoConf[SERVO_FLAPS].min, servoConf[SERVO_FLAPS].max);
lFlap = escAndServoConfig->servoCenterPulse - lFlap;
if (slow_LFlaps < lFlap)
slow_LFlaps += airplaneConfig->flaps_speed;
else if (slow_LFlaps > lFlap)
slow_LFlaps -= airplaneConfig->flaps_speed;
servo[SERVO_FLAPS] = ((int32_t)servoConf[SERVO_FLAPS].rate * slow_LFlaps) / 100L;
servo[SERVO_FLAPS] += escAndServoConfig->servoCenterPulse;
}
if (FLIGHT_MODE(PASSTHRU_MODE)) { // Direct passthru from RX
servo[SERVO_FLAPPERON_1] = rcCommand[ROLL] + flapperons[0];
servo[SERVO_FLAPPERON_2] = rcCommand[ROLL] + flapperons[1];
servo[SERVO_RUDDER] = rcCommand[YAW];
servo[SERVO_ELEVATOR] = rcCommand[PITCH];
if (FLIGHT_MODE(PASSTHRU_MODE)) {
// Direct passthru from RX
input[INPUT_ROLL] = rcCommand[ROLL];
input[INPUT_PITCH] = rcCommand[PITCH];
input[INPUT_YAW] = rcCommand[YAW];
} else {
// Assisted modes (gyro only or gyro+acc according to AUX configuration in Gui
servo[SERVO_FLAPPERON_1] = axisPID[ROLL] + flapperons[0];
servo[SERVO_FLAPPERON_2] = axisPID[ROLL] + flapperons[1];
servo[SERVO_RUDDER] = axisPID[YAW];
servo[SERVO_ELEVATOR] = axisPID[PITCH];
input[INPUT_ROLL] = axisPID[ROLL];
input[INPUT_PITCH] = axisPID[PITCH];
input[INPUT_YAW] = axisPID[YAW];
// Reverse yaw servo when inverted in 3D mode
if (feature(FEATURE_3D) && (rcData[THROTTLE] < rxConfig->midrc)) {
input[INPUT_YAW] *= -1;
}
}
for (i = SERVO_PLANE_INDEX_MIN; i <= SERVO_PLANE_INDEX_MAX; i++) {
servo[i] = ((int32_t)servoConf[i].rate * servo[i]) / 100L; // servo rates
input[INPUT_GIMBAL_PITCH] = scaleRange(inclination.values.pitchDeciDegrees, -1800, 1800, -500, +500);
input[INPUT_GIMBAL_ROLL] = scaleRange(inclination.values.rollDeciDegrees, -1800, 1800, -500, +500);
debug[0] = input[INPUT_GIMBAL_PITCH];
debug[1] = input[INPUT_GIMBAL_ROLL];
input[INPUT_THROTTLE] = motor[0];
// center the RC input value around the RC middle value
// by subtracting the RC middle value from the RC input value, we get:
// data - middle = input
// 2000 - 1500 = +500
// 1500 - 1500 = 0
// 1000 - 1500 = -500
input[INPUT_AUX1] = rcData[AUX1] - rxConfig->midrc;
input[INPUT_AUX2] = rcData[AUX2] - rxConfig->midrc;
input[INPUT_AUX3] = rcData[AUX3] - rxConfig->midrc;
input[INPUT_AUX4] = rcData[AUX4] - rxConfig->midrc;
input[INPUT_RC_ROLL] = rcData[ROLL] - rxConfig->midrc;
input[INPUT_RC_PITCH] = rcData[PITCH] - rxConfig->midrc;
input[INPUT_RC_YAW] = rcData[YAW] - rxConfig->midrc;
input[INPUT_RC_THROTTLE] = rcData[THROTTLE] - rxConfig->midrc;
for (i = 0; i < MAX_SUPPORTED_SERVOS; i++)
servo[i] = 0;
// 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].fromChannel;
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]);
}
// FIXME if the 'from' really only works for 0 to 8 currently. e.g. See INPUT_ROLL and INPUT_RC_ROLL. (0 vs 8, the intention is likely to use 0 in both cases)
// Really, there should be a translation from INPUT_* to 'fromChannel' and servoDirection() should only be used if needed.
servo[target] += servoDirection(target, from) * constrain(((int32_t)currentOutput[i] * currentServoMixer[i].rate) / 100, min, max);
} else {
currentOutput[i] = 0;
}
}
for (i = 0; i < MAX_SUPPORTED_SERVOS; i++) {
servo[i] = ((int32_t)servoConf[i].rate * servo[i]) / 100L;
servo[i] += determineServoMiddleOrForwardFromChannel(i);
}
}
#endif
void mixTable(void)
@ -598,12 +781,11 @@ void mixTable(void)
uint32_t i;
if (motorCount >= 4 && mixerConfig->yaw_jump_prevention_limit < YAW_JUMP_PREVENTION_LIMIT_HIGH) {
// prevent "yaw jump" during yaw correction (500 is disabled jump protection)
// prevent "yaw jump" during yaw correction
axisPID[YAW] = constrain(axisPID[YAW], -mixerConfig->yaw_jump_prevention_limit - ABS(rcCommand[YAW]), mixerConfig->yaw_jump_prevention_limit + ABS(rcCommand[YAW]));
}
// motors for non-servo mixes
if (motorCount > 1) {
for (i = 0; i < motorCount; i++) {
motor[i] =
rcCommand[THROTTLE] * currentMixer[i].throttle +
@ -611,71 +793,27 @@ void mixTable(void)
axisPID[ROLL] * currentMixer[i].roll +
-mixerConfig->yaw_direction * axisPID[YAW] * currentMixer[i].yaw;
}
}
#if !defined(USE_QUAD_MIXER_ONLY) || defined(USE_SERVOS)
int8_t yawDirection3D = 1;
// Reverse yaw servo when inverted in 3D mode
if (feature(FEATURE_3D) && (rcData[THROTTLE] < rxConfig->midrc)) {
yawDirection3D = -1;
}
// airplane / servo mixes
switch (currentMixerMode) {
case MIXER_CUSTOM_AIRPLANE:
case MIXER_FLYING_WING:
case MIXER_AIRPLANE:
case MIXER_BI:
servo[SERVO_BIPLANE_LEFT] = (servoDirection(SERVO_BIPLANE_LEFT, 2) * axisPID[YAW]) + (servoDirection(SERVO_BIPLANE_LEFT, 1) * axisPID[PITCH]) + determineServoMiddleOrForwardFromChannel(SERVO_BIPLANE_LEFT);
servo[SERVO_BIPLANE_RIGHT] = (servoDirection(SERVO_BIPLANE_RIGHT, 2) * axisPID[YAW]) + (servoDirection(SERVO_BIPLANE_RIGHT, 1) * axisPID[PITCH]) + determineServoMiddleOrForwardFromChannel(SERVO_BIPLANE_RIGHT);
break;
case MIXER_TRI:
servo[SERVO_RUDDER] = (servoDirection(SERVO_RUDDER, 1) * axisPID[YAW] * yawDirection3D) + determineServoMiddleOrForwardFromChannel(SERVO_RUDDER);
case MIXER_DUALCOPTER:
case MIXER_SINGLECOPTER:
case MIXER_GIMBAL:
servoMixer();
break;
/*
case MIXER_GIMBAL:
servo[SERVO_GIMBAL_PITCH] = (((int32_t)servoConf[SERVO_GIMBAL_PITCH].rate * inclination.values.pitchDeciDegrees) / 50) + determineServoMiddleOrForwardFromChannel(SERVO_GIMBAL_PITCH);
servo[SERVO_GIMBAL_ROLL] = (((int32_t)servoConf[SERVO_GIMBAL_ROLL].rate * inclination.values.rollDeciDegrees) / 50) + determineServoMiddleOrForwardFromChannel(SERVO_GIMBAL_ROLL);
break;
case MIXER_AIRPLANE:
airplaneMixer();
break;
case MIXER_FLYING_WING:
if (!ARMING_FLAG(ARMED))
servo[SERVO_THROTTLE] = escAndServoConfig->mincommand;
else
servo[SERVO_THROTTLE] = constrain(rcCommand[THROTTLE], escAndServoConfig->minthrottle, escAndServoConfig->maxthrottle);
motor[0] = servo[SERVO_THROTTLE];
if (FLIGHT_MODE(PASSTHRU_MODE)) {
// do not use sensors for correction, simple 2 channel mixing
servo[SERVO_FLAPPERON_1] = (servoDirection(SERVO_FLAPPERON_1, 1) * rcCommand[PITCH]) + (servoDirection(SERVO_FLAPPERON_1, 2) * rcCommand[ROLL]);
servo[SERVO_FLAPPERON_2] = (servoDirection(SERVO_FLAPPERON_2, 1) * rcCommand[PITCH]) + (servoDirection(SERVO_FLAPPERON_2, 2) * rcCommand[ROLL]);
} else {
// use sensors to correct (gyro only or gyro + acc)
servo[SERVO_FLAPPERON_1] = (servoDirection(SERVO_FLAPPERON_1, 1) * axisPID[PITCH]) + (servoDirection(SERVO_FLAPPERON_1, 2) * axisPID[ROLL]);
servo[SERVO_FLAPPERON_2] = (servoDirection(SERVO_FLAPPERON_2, 1) * axisPID[PITCH]) + (servoDirection(SERVO_FLAPPERON_2, 2) * axisPID[ROLL]);
}
servo[SERVO_FLAPPERON_1] += determineServoMiddleOrForwardFromChannel(SERVO_FLAPPERON_1);
servo[SERVO_FLAPPERON_2] += determineServoMiddleOrForwardFromChannel(SERVO_FLAPPERON_2);
break;
case MIXER_DUALCOPTER:
for (i = SERVO_DUALCOPTER_INDEX_MIN; i <= SERVO_DUALCOPTER_INDEX_MAX; i++) {
servo[i] = axisPID[SERVO_DUALCOPTER_INDEX_MAX - i] * servoDirection(i, 1); // mix and setup direction
servo[i] += determineServoMiddleOrForwardFromChannel(i);
}
break;
case MIXER_SINGLECOPTER:
for (i = SERVO_SINGLECOPTER_INDEX_MIN; i <= SERVO_SINGLECOPTER_INDEX_MAX; i++) {
servo[i] = (axisPID[YAW] * servoDirection(i, 2)) + (axisPID[(SERVO_SINGLECOPTER_INDEX_MAX - i) >> 1] * servoDirection(i, 1)); // mix and setup direction
servo[i] += determineServoMiddleOrForwardFromChannel(i);
}
motor[0] = rcCommand[THROTTLE];
break;
*/
default:
break;
@ -684,8 +822,8 @@ void mixTable(void)
// do camstab
if (feature(FEATURE_SERVO_TILT)) {
// center at fixed position, or vary either pitch or roll by RC channel
servo[SERVO_GIMBAL_PITCH] = determineServoMiddleOrForwardFromChannel(0);
servo[SERVO_GIMBAL_ROLL] = determineServoMiddleOrForwardFromChannel(1);
servo[SERVO_GIMBAL_PITCH] = determineServoMiddleOrForwardFromChannel(SERVO_GIMBAL_PITCH);
servo[SERVO_GIMBAL_ROLL] = determineServoMiddleOrForwardFromChannel(SERVO_GIMBAL_ROLL);
if (IS_RC_MODE_ACTIVE(BOXCAMSTAB)) {
if (gimbalConfig->gimbal_flags & GIMBAL_MIXTILT) {
@ -699,11 +837,9 @@ void mixTable(void)
}
// constrain servos
if (useServo) {
for (i = 0; i < MAX_SUPPORTED_SERVOS; i++) {
servo[i] = constrain(servo[i], servoConf[i].min, servoConf[i].max); // limit the values
}
}
#endif
if (ARMING_FLAG(ARMED)) {

50
src/main/flight/mixer.h
View file

@ -48,7 +48,8 @@ typedef enum mixerMode
MIXER_DUALCOPTER = 20,
MIXER_SINGLECOPTER = 21,
MIXER_ATAIL4 = 22,
MIXER_CUSTOM = 23
MIXER_CUSTOM = 23,
MIXER_CUSTOM_AIRPLANE = 24
} mixerMode_e;
// Custom mixer data per motor
@ -85,18 +86,57 @@ typedef struct flight3DConfig_s {
} flight3DConfig_t;
typedef struct airplaneConfig_t {
uint8_t flaps_speed; // airplane mode flaps, 0 = no flaps, > 0 = flap speed, larger = faster
int8_t fixedwing_althold_dir; // +1 or -1 for pitch/althold gain. later check if need more than just sign
} airplaneConfig_t;
#define CHANNEL_FORWARDING_DISABLED (uint8_t)0xFF
#ifdef USE_SERVOS
enum {
INPUT_ROLL = 0,
INPUT_PITCH,
INPUT_YAW,
INPUT_THROTTLE,
INPUT_AUX1,
INPUT_AUX2,
INPUT_AUX3,
INPUT_AUX4,
INPUT_RC_ROLL,
INPUT_RC_PITCH,
INPUT_RC_YAW,
INPUT_RC_THROTTLE,
INPUT_GIMBAL_PITCH,
INPUT_GIMBAL_ROLL,
INPUT_ITEM_COUNT
} servoInput_e;
typedef struct servoMixer_t {
uint8_t targetChannel; // servo that receives the output of the rule
uint8_t fromChannel; // 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
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)
#define MAX_SERVO_SPEED UINT8_MAX
#define MAX_SERVO_BOXES 3
// Custom mixer configuration
typedef struct mixerRules_t {
uint8_t servoRuleCount;
const servoMixer_t *rule;
} mixerRules_t;
typedef struct servoParam_t {
int16_t min; // servo min
int16_t max; // servo max
int16_t middle; // servo middle
int8_t rate; // range [-100;+100] ; can be used to adjust a rate 0-100% and a direction
int8_t rate; // range [-125;+125] ; can be used to adjust a rate 0-125% and a direction
uint32_t reversedChannels; // the direction of servo movement for each input channel of the servo mixer, bit set=inverted
uint8_t angleAtMin; // range [0;180] the measured angle in degrees from the middle when the servo is at the 'min' value.
uint8_t angleAtMax; // range [0;180] the measured angle in degrees from the middle when the servo is at the 'max' value.
int8_t forwardFromChannel; // RX channel index, 0 based. See CHANNEL_FORWARDING_DISABLED
@ -128,6 +168,10 @@ void mixerUseConfigs(
void writeAllMotors(int16_t mc);
void mixerLoadMix(int index, motorMixer_t *customMixers);
#ifdef USE_SERVOS
void servoMixerLoadMix(int index, servoMixer_t *customServoMixers);
void loadCustomServoMixer(void);
#endif
void mixerResetMotors(void);
void mixTable(void);
void writeMotors(void);

5
src/main/io/rc_controls.h
View file

@ -43,12 +43,15 @@ typedef enum {
BOXTELEMETRY,
BOXAUTOTUNE,
BOXSONAR,
BOXSERVO1,
BOXSERVO2,
BOXSERVO3,
CHECKBOX_ITEM_COUNT
} boxId_e;
extern uint32_t rcModeActivationMask;
#define IS_RC_MODE_ACTIVE(modeId) ((1 << modeId) & rcModeActivationMask)
#define IS_RC_MODE_ACTIVE(modeId) ((1 << (modeId)) & rcModeActivationMask)
#define ACTIVATE_RC_MODE(modeId) (rcModeActivationMask |= (1 << modeId))
typedef enum rc_alias {

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

@ -111,6 +111,7 @@ static void cliReboot(void);
static void cliSave(char *cmdline);
static void cliSerial(char *cmdline);
static void cliServo(char *cmdline);
static void cliServoMix(char *cmdline);
static void cliSet(char *cmdline);
static void cliGet(char *cmdline);
static void cliStatus(char *cmdline);
@ -151,7 +152,7 @@ static const char * const mixerNames[] = {
"FLYING_WING", "Y4", "HEX6X", "OCTOX8", "OCTOFLATP", "OCTOFLATX",
"AIRPLANE", "HELI_120_CCPM", "HELI_90_DEG", "VTAIL4",
"HEX6H", "PPM_TO_SERVO", "DUALCOPTER", "SINGLECOPTER",
"ATAIL4", "CUSTOM", NULL
"ATAIL4", "CUSTOM", "CUSTOMAIRPLANE", NULL
};
#endif
@ -226,6 +227,9 @@ const clicmd_t cmdTable[] = {
{ "servo", "servo config", cliServo },
#endif
{ "set", "name=value or blank or * for list", cliSet },
#ifdef USE_SERVOS
{ "smix", "design custom servo mixer", cliServoMix },
#endif
{ "status", "show system status", cliStatus },
{ "version", "", cliVersion },
};
@ -285,8 +289,6 @@ const clivalue_t valueTable[] = {
{ "auto_disarm_delay", VAR_UINT8 | MASTER_VALUE, &masterConfig.auto_disarm_delay, 0, 60 },
{ "small_angle", VAR_UINT8 | MASTER_VALUE, &masterConfig.small_angle, 0, 180 },
{ "flaps_speed", VAR_UINT8 | MASTER_VALUE, &masterConfig.airplaneConfig.flaps_speed, 0, 100 },
{ "fixedwing_althold_dir", VAR_INT8 | MASTER_VALUE, &masterConfig.airplaneConfig.fixedwing_althold_dir, -1, 1 },
{ "reboot_character", VAR_UINT8 | MASTER_VALUE, &masterConfig.serialConfig.reboot_character, 48, 126 },
@ -739,20 +741,20 @@ static void cliCMix(char *cmdline)
if (isEmpty(cmdline)) {
cliPrint("Custom mixer: \r\nMotor\tThr\tRoll\tPitch\tYaw\r\n");
for (i = 0; i < MAX_SUPPORTED_MOTORS; i++) {
if (masterConfig.customMixer[i].throttle == 0.0f)
if (masterConfig.customMotorMixer[i].throttle == 0.0f)
break;
num_motors++;
printf("#%d:\t", i + 1);
printf("%s\t", ftoa(masterConfig.customMixer[i].throttle, buf));
printf("%s\t", ftoa(masterConfig.customMixer[i].roll, buf));
printf("%s\t", ftoa(masterConfig.customMixer[i].pitch, buf));
printf("%s\r\n", ftoa(masterConfig.customMixer[i].yaw, buf));
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));
printf("%s\r\n", ftoa(masterConfig.customMotorMixer[i].yaw, buf));
}
mixsum[0] = mixsum[1] = mixsum[2] = 0.0f;
for (i = 0; i < num_motors; i++) {
mixsum[0] += masterConfig.customMixer[i].roll;
mixsum[1] += masterConfig.customMixer[i].pitch;
mixsum[2] += masterConfig.customMixer[i].yaw;
mixsum[0] += masterConfig.customMotorMixer[i].roll;
mixsum[1] += masterConfig.customMotorMixer[i].pitch;
mixsum[2] += masterConfig.customMotorMixer[i].yaw;
}
cliPrint("Sanity check:\t");
for (i = 0; i < 3; i++)
@ -762,7 +764,7 @@ static void cliCMix(char *cmdline)
} else if (strncasecmp(cmdline, "reset", 5) == 0) {
// erase custom mixer
for (i = 0; i < MAX_SUPPORTED_MOTORS; i++)
masterConfig.customMixer[i].throttle = 0.0f;
masterConfig.customMotorMixer[i].throttle = 0.0f;
} else if (strncasecmp(cmdline, "load", 4) == 0) {
ptr = strchr(cmdline, ' ');
if (ptr) {
@ -773,7 +775,7 @@ static void cliCMix(char *cmdline)
break;
}
if (strncasecmp(ptr, mixerNames[i], len) == 0) {
mixerLoadMix(i, masterConfig.customMixer);
mixerLoadMix(i, masterConfig.customMotorMixer);
printf("Loaded %s mix\r\n", mixerNames[i]);
cliCMix("");
break;
@ -786,22 +788,22 @@ static void cliCMix(char *cmdline)
if (--i < MAX_SUPPORTED_MOTORS) {
ptr = strchr(ptr, ' ');
if (ptr) {
masterConfig.customMixer[i].throttle = fastA2F(++ptr);
masterConfig.customMotorMixer[i].throttle = fastA2F(++ptr);
check++;
}
ptr = strchr(ptr, ' ');
if (ptr) {
masterConfig.customMixer[i].roll = fastA2F(++ptr);
masterConfig.customMotorMixer[i].roll = fastA2F(++ptr);
check++;
}
ptr = strchr(ptr, ' ');
if (ptr) {
masterConfig.customMixer[i].pitch = fastA2F(++ptr);
masterConfig.customMotorMixer[i].pitch = fastA2F(++ptr);
check++;
}
ptr = strchr(ptr, ' ');
if (ptr) {
masterConfig.customMixer[i].yaw = fastA2F(++ptr);
masterConfig.customMotorMixer[i].yaw = fastA2F(++ptr);
check++;
}
if (check != 4) {
@ -942,6 +944,138 @@ static void cliServo(char *cmdline)
#endif
}
static void cliServoMix(char *cmdline)
{
#ifndef USE_SERVOS
UNUSED(cmdline);
#else
int i;
uint8_t len;
char *ptr;
int args[8], check = 0;
len = strlen(cmdline);
if (len == 0) {
printf("Custom servo mixer: \r\nchange mixer: smix rule\ttarget_channel\tinput_channel\trate\tspeed\t\tmin\tmax\tbox\r\n");
printf("reset mixer: smix reset\r\nload mixer: smix load\r\nchange direction of channel: smix direction\r\n");
for (i = 0; i < MAX_SERVO_RULES; i++) {
if (masterConfig.customServoMixer[i].rate == 0)
break;
printf("#%d:\t", i + 1);
printf("%d\t", masterConfig.customServoMixer[i].targetChannel + 1);
printf("%d\t", masterConfig.customServoMixer[i].fromChannel + 1);
printf("%d\t", masterConfig.customServoMixer[i].rate);
printf("%d\t", masterConfig.customServoMixer[i].speed);
printf("%d\t", masterConfig.customServoMixer[i].min);
printf("%d\t", masterConfig.customServoMixer[i].max);
printf("%d\r\n", masterConfig.customServoMixer[i].box);
}
printf("\r\n");
return;
} else if (strncasecmp(cmdline, "reset", 5) == 0) {
// erase custom mixer
memset(masterConfig.customServoMixer, 0, sizeof(masterConfig.customServoMixer));
for (i = 0; i < MAX_SUPPORTED_SERVOS; i++) {
currentProfile->servoConf[i].reversedChannels = 0;
}
} else if (strncasecmp(cmdline, "load", 4) == 0) {
ptr = strchr(cmdline, ' ');
if (ptr) {
len = strlen(++ptr);
for (i = 0; ; i++) {
if (mixerNames[i] == NULL) {
printf("Invalid mixer type...\r\n");
break;
}
if (strncasecmp(ptr, mixerNames[i], len) == 0) {
servoMixerLoadMix(i, masterConfig.customServoMixer);
printf("Loaded %s mix...\r\n", mixerNames[i]);
cliServoMix("");
break;
}
}
}
} else if (strncasecmp(cmdline, "direction", 9) == 0) {
enum {SERVO = 0, INPUT, DIRECTION, ARGS_COUNT};
int servoIndex, channel;
ptr = strchr(cmdline, ' ');
len = strlen(ptr);
if (len == 0) {
printf("change the direction a servo reacts to a input channel: \r\nservo input -1|1\r\n");
printf("s");
for (channel = 0; channel < INPUT_ITEM_COUNT; channel++)
printf("\ti%d", channel + 1);
printf("\r\n");
for (servoIndex = 0; servoIndex < MAX_SUPPORTED_SERVOS; servoIndex++) {
printf("%d", servoIndex + 1);
for (channel = 0; channel < INPUT_ITEM_COUNT; channel++)
printf("\t%s ", (currentProfile->servoConf[servoIndex].reversedChannels & (1 << channel)) ? "r" : "n");
printf("\r\n");
}
return;
}
ptr = strtok(ptr, " ");
while (ptr != NULL && check < ARGS_COUNT) {
args[check++] = atoi(ptr);
ptr = strtok(NULL, " ");
}
if (ptr != NULL || check != ARGS_COUNT) {
printf("Wrong number of arguments, needs servo input direction\r\n");
return;
}
if (args[SERVO] >= 1 && args[SERVO] <= MAX_SUPPORTED_SERVOS && args[INPUT] >= 1 && args[INPUT] <= INPUT_ITEM_COUNT && (args[DIRECTION] == -1 || args[DIRECTION] == 1)) {
args[SERVO] -= 1;
args[INPUT] -= 1;
if (args[DIRECTION] == -1)
currentProfile->servoConf[args[SERVO]].reversedChannels |= 1 << args[INPUT];
else
currentProfile->servoConf[args[SERVO]].reversedChannels &= ~(1 << args[INPUT]);
} else
printf("ERR: Wrong range for arguments\r\n");
cliServoMix("direction");
} else {
enum {RULE = 0, TARGET, INPUT, RATE, SPEED, MIN, MAX, BOX, ARGS_COUNT};
ptr = strtok(cmdline, " ");
while (ptr != NULL && check < ARGS_COUNT) {
args[check++] = atoi(ptr);
ptr = strtok(NULL, " ");
}
if (ptr != NULL || check != ARGS_COUNT) {
printf("ERR: Wrong number of arguments, needs rule target_channel input_channel rate speed min max box\r\n");
return;
}
i = args[RULE] - 1;
if (i >= 0 && i < MAX_SERVO_RULES &&
args[TARGET] > 0 && args[TARGET] <= MAX_SUPPORTED_SERVOS &&
args[INPUT] >= 1 && args[INPUT] <= INPUT_ITEM_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].rate = args[RATE];
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
printf("ERR: Wrong range for arguments\r\n");
}
#endif
}
#ifdef USE_FLASHFS
static void cliFlashInfo(char *cmdline)
@ -1056,7 +1190,7 @@ static const char* const sectionBreak = "\r\n";
static void cliDump(char *cmdline)
{
unsigned int i;
unsigned int i, channel;
char buf[16];
uint32_t mask;
@ -1086,14 +1220,15 @@ static void cliDump(char *cmdline)
#ifndef USE_QUAD_MIXER_ONLY
printf("mixer %s\r\n", mixerNames[masterConfig.mixerMode - 1]);
if (masterConfig.customMixer[0].throttle != 0.0f) {
printf("cmix reset\r\n");
for (i = 0; i < MAX_SUPPORTED_MOTORS; i++) {
if (masterConfig.customMixer[i].throttle == 0.0f)
if (masterConfig.customMotorMixer[i].throttle == 0.0f)
break;
thr = masterConfig.customMixer[i].throttle;
roll = masterConfig.customMixer[i].roll;
pitch = masterConfig.customMixer[i].pitch;
yaw = masterConfig.customMixer[i].yaw;
thr = masterConfig.customMotorMixer[i].throttle;
roll = masterConfig.customMotorMixer[i].roll;
pitch = masterConfig.customMotorMixer[i].pitch;
yaw = masterConfig.customMotorMixer[i].yaw;
printf("cmix %d", i + 1);
if (thr < 0)
cliWrite(' ');
@ -1108,8 +1243,46 @@ static void cliDump(char *cmdline)
cliWrite(' ');
printf("%s\r\n", ftoa(yaw, buf));
}
printf("cmix %d 0 0 0 0\r\n", i + 1);
// print custom servo mixer if exists
printf("smix reset\r\n");
for (i = 0; i < MAX_SERVO_RULES; i++) {
if (masterConfig.customServoMixer[i].rate == 0)
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,
masterConfig.customServoMixer[i].rate,
masterConfig.customServoMixer[i].speed,
masterConfig.customServoMixer[i].min,
masterConfig.customServoMixer[i].max,
masterConfig.customServoMixer[i].box
);
}
// print servo directions
for (i = 0; i < MAX_SUPPORTED_SERVOS; i++) {
for (channel = 0; channel < INPUT_ITEM_COUNT; channel++) {
if (currentProfile->servoConf[i].reversedChannels & (1 << channel)) {
printf("smix direction %d %d -1\r\n", i + 1 , channel + 1);
}
}
}
// print servo config
for (i = 0; i < MAX_SUPPORTED_SERVOS; i++) {
printf("servo %d %d %d %d %d\r\n", i + 1,
currentProfile->servoConf[i].min,
currentProfile->servoConf[i].middle,
currentProfile->servoConf[i].max,
currentProfile->servoConf[i].rate
);
}
#endif
cliPrint("\r\n\r\n# feature\r\n");

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

@ -131,7 +131,7 @@ void useRcControlsConfig(modeActivationCondition_t *modeActivationConditions, es
#define MSP_PROTOCOL_VERSION 0
#define API_VERSION_MAJOR 1 // increment when major changes are made
#define API_VERSION_MINOR 11 // increment when any change is made, reset to zero when major changes are released after changing API_VERSION_MAJOR
#define API_VERSION_MINOR 12 // increment when any change is made, reset to zero when major changes are released after changing API_VERSION_MAJOR
#define API_VERSION_LENGTH 2
@ -305,10 +305,13 @@ 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 INBUF_SIZE 64
#define SERVO_CHUNK_SIZE 13
#define SERVO_CHUNK_SIZE 7
// FIXME This is now too big!
#define INBUF_SIZE (SERVO_CHUNK_SIZE * MAX_SUPPORTED_SERVOS)
typedef struct box_e {
const uint8_t boxId; // see boxId_e
@ -341,6 +344,10 @@ static const box_t boxes[CHECKBOX_ITEM_COUNT + 1] = {
{ BOXTELEMETRY, "TELEMETRY;", 20 },
{ BOXAUTOTUNE, "AUTOTUNE;", 21 },
{ BOXSONAR, "SONAR;", 22 },
{ BOXSERVO1, "SERVO1;", 23 },
{ BOXSERVO2, "SERVO2;", 24 },
{ BOXSERVO3, "SERVO3;", 25 },
{ CHECKBOX_ITEM_COUNT, NULL, 0xFF }
};
@ -681,6 +688,14 @@ void mspInit(serialConfig_t *serialConfig)
activeBoxIds[activeBoxIdCount++] = BOXSONAR;
}
#ifdef USE_SERVOS
if (masterConfig.mixerMode == MIXER_CUSTOM_AIRPLANE) {
activeBoxIds[activeBoxIdCount++] = BOXSERVO1;
activeBoxIds[activeBoxIdCount++] = BOXSERVO2;
activeBoxIds[activeBoxIdCount++] = BOXSERVO3;
}
#endif
memset(mspPorts, 0x00, sizeof(mspPorts));
mspAllocateSerialPorts(serialConfig);
}
@ -764,7 +779,7 @@ static bool processOutCommand(uint8_t cmdMSP)
serialize8(MW_VERSION);
serialize8(masterConfig.mixerMode);
serialize8(MSP_PROTOCOL_VERSION);
serialize32(CAP_DYNBALANCE | (masterConfig.airplaneConfig.flaps_speed ? CAP_FLAPS : 0)); // "capability"
serialize32(CAP_DYNBALANCE); // "capability"
break;
case MSP_STATUS:
@ -828,7 +843,7 @@ static bool processOutCommand(uint8_t cmdMSP)
s_struct((uint8_t *)&servo, MAX_SUPPORTED_SERVOS * 2);
break;
case MSP_SERVO_CONF:
headSerialReply(MAX_SUPPORTED_SERVOS * 9);
headSerialReply(MAX_SUPPORTED_SERVOS * SERVO_CHUNK_SIZE);
for (i = 0; i < MAX_SUPPORTED_SERVOS; i++) {
serialize16(currentProfile->servoConf[i].min);
serialize16(currentProfile->servoConf[i].max);
@ -836,6 +851,7 @@ static bool processOutCommand(uint8_t cmdMSP)
serialize8(currentProfile->servoConf[i].rate);
serialize8(currentProfile->servoConf[i].angleAtMin);
serialize8(currentProfile->servoConf[i].angleAtMax);
serialize32(currentProfile->servoConf[i].reversedChannels);
}
break;
case MSP_CHANNEL_FORWARDING:
@ -844,6 +860,19 @@ static bool processOutCommand(uint8_t cmdMSP)
serialize8(currentProfile->servoConf[i].forwardFromChannel);
}
break;
case MSP_SERVOMIX_CONF:
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].rate);
serialize8(masterConfig.customServoMixer[i].speed);
serialize8(masterConfig.customServoMixer[i].min);
serialize8(masterConfig.customServoMixer[i].max);
serialize8(masterConfig.customServoMixer[i].box);
}
break;
#endif
case MSP_MOTOR:
s_struct((uint8_t *)motor, 16);
@ -1420,18 +1449,11 @@ static bool processInCommand(void)
for (i = 0; i < MAX_SUPPORTED_SERVOS && i < servoCount; i++) {
currentProfile->servoConf[i].min = read16();
currentProfile->servoConf[i].max = read16();
// provide temporary support for old clients that try and send a channel index instead of a servo middle
uint16_t potentialServoMiddleOrChannelToForward = read16();
if (potentialServoMiddleOrChannelToForward < MAX_SUPPORTED_SERVOS) {
currentProfile->servoConf[i].forwardFromChannel = potentialServoMiddleOrChannelToForward;
}
if (potentialServoMiddleOrChannelToForward >= PWM_RANGE_MIN && potentialServoMiddleOrChannelToForward <= PWM_RANGE_MAX) {
currentProfile->servoConf[i].middle = potentialServoMiddleOrChannelToForward;
}
currentProfile->servoConf[i].middle = read16();
currentProfile->servoConf[i].rate = read8();
currentProfile->servoConf[i].angleAtMin = read8();
currentProfile->servoConf[i].angleAtMax = read8();
currentProfile->servoConf[i].reversedChannels = read32();
}
}
#endif
@ -1443,6 +1465,22 @@ static bool processInCommand(void)
}
#endif
break;
case MSP_SET_SERVOMIX_CONF:
#ifdef USE_SERVOS
for (i = 0; i < MAX_SERVO_RULES; i++) {
masterConfig.customServoMixer[i].targetChannel = read8();
masterConfig.customServoMixer[i].fromChannel = 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();
#endif
break;
case MSP_RESET_CONF:
if (!ARMING_FLAG(ARMED)) {
resetEEPROM();

12
src/main/main.c
View file

@ -106,7 +106,11 @@ void mspInit(serialConfig_t *serialConfig);
void cliInit(serialConfig_t *serialConfig);
void failsafeInit(rxConfig_t *intialRxConfig);
pwmOutputConfiguration_t *pwmInit(drv_pwm_config_t *init);
void mixerInit(mixerMode_e mixerMode, motorMixer_t *customMixers);
#ifdef USE_SERVOS
void mixerInit(mixerMode_e mixerMode, motorMixer_t *customMotorMixers, servoMixer_t *customServoMixers);
#else
void mixerInit(mixerMode_e mixerMode, motorMixer_t *customMotorMixers);
#endif
void mixerUsePWMOutputConfiguration(pwmOutputConfiguration_t *pwmOutputConfiguration);
void rxInit(rxConfig_t *rxConfig);
void gpsInit(serialConfig_t *serialConfig, gpsConfig_t *initialGpsConfig);
@ -197,7 +201,11 @@ void init(void)
serialInit(&masterConfig.serialConfig, feature(FEATURE_SOFTSERIAL));
mixerInit(masterConfig.mixerMode, masterConfig.customMixer);
#ifdef USE_SERVOS
mixerInit(masterConfig.mixerMode, masterConfig.customMotorMixer, masterConfig.customServoMixer);
#else
mixerInit(masterConfig.mixerMode, masterConfig.customMotorMixer);
#endif
memset(&pwm_params, 0, sizeof(pwm_params));

30
src/test/unit/flight_mixer_unittest.cc
View file

@ -48,7 +48,7 @@ extern "C" {
extern uint8_t servoCount;
void forwardAuxChannelsToServos(uint8_t firstServoIndex);
void mixerInit(mixerMode_e mixerMode, motorMixer_t *initialCustomMixers);
void mixerInit(mixerMode_e mixerMode, motorMixer_t *initialCustomMixers, servoMixer_t *initialCustomServoMixers);
void mixerUsePWMOutputConfiguration(pwmOutputConfiguration_t *pwmOutputConfiguration);
}
@ -144,12 +144,16 @@ TEST(FlightMixerTest, TestForwardAuxChannelsToServosWithLessRemainingServosThanA
EXPECT_EQ(1250, servos[MAX_SUPPORTED_SERVOS - 0 - 1].value);
}
TEST(FlightMixerTest, TestTricopterServo)
{
// given
mixerConfig_t mixerConfig;
memset(&mixerConfig, 0, sizeof(mixerConfig));
rxConfig_t rxConfig;
memset(&rxConfig, 0, sizeof(rxConfig));
mixerConfig.tri_unarmed_servo = 1;
escAndServoConfig_t escAndServoConfig;
@ -175,13 +179,16 @@ TEST(FlightMixerTest, TestTricopterServo)
&escAndServoConfig,
&mixerConfig,
NULL,
NULL
&rxConfig
);
motorMixer_t customMixer[MAX_SUPPORTED_MOTORS];
memset(&customMixer, 0, sizeof(customMixer));
mixerInit(MIXER_TRI, customMixer);
servoMixer_t customServoMixer[MAX_SUPPORTED_SERVOS];
memset(&customServoMixer, 0, sizeof(customServoMixer));
mixerInit(MIXER_TRI, customMixer, customServoMixer);
// and
pwmOutputConfiguration_t pwmOutputConfiguration = {
@ -213,8 +220,11 @@ TEST(FlightMixerTest, TestQuadMotors)
mixerConfig_t mixerConfig;
memset(&mixerConfig, 0, sizeof(mixerConfig));
//servoParam_t servoConf[MAX_SUPPORTED_SERVOS];
//memset(&servoConf, 0, sizeof(servoConf));
rxConfig_t rxConfig;
memset(&rxConfig, 0, sizeof(rxConfig));
servoParam_t servoConf[MAX_SUPPORTED_SERVOS];
memset(&servoConf, 0, sizeof(servoConf));
escAndServoConfig_t escAndServoConfig;
memset(&escAndServoConfig, 0, sizeof(escAndServoConfig));
@ -225,19 +235,23 @@ TEST(FlightMixerTest, TestQuadMotors)
};
mixerUseConfigs(
NULL,// servoConf,
servoConf,
&gimbalConfig,
NULL,
&escAndServoConfig,
&mixerConfig,
NULL,
NULL
&rxConfig
);
motorMixer_t customMixer[MAX_SUPPORTED_MOTORS];
memset(&customMixer, 0, sizeof(customMixer));
mixerInit(MIXER_QUADX, customMixer);
servoMixer_t customServoMixer[MAX_SUPPORTED_SERVOS];
memset(&customServoMixer, 0, sizeof(customServoMixer));
mixerInit(MIXER_QUADX, customMixer, customServoMixer);
// and
pwmOutputConfiguration_t pwmOutputConfiguration = {