Split mixer and servo code
This commit is contained in:
parent
726a8d29e2
commit
31828873fa
1
Makefile
1
Makefile
|
@ -508,6 +508,7 @@ COMMON_SRC = \
|
|||
flight/imu.c \
|
||||
flight/mixer.c \
|
||||
flight/pid.c \
|
||||
flight/servos.c \
|
||||
io/beeper.c \
|
||||
io/serial.c \
|
||||
io/serial_4way.c \
|
||||
|
|
|
@ -29,6 +29,7 @@
|
|||
|
||||
#include "flight/failsafe.h"
|
||||
#include "flight/mixer.h"
|
||||
#include "flight/servos.h"
|
||||
#include "flight/imu.h"
|
||||
#include "flight/navigation.h"
|
||||
|
||||
|
@ -65,10 +66,11 @@ typedef struct master_s {
|
|||
// motor/esc/servo related stuff
|
||||
motorMixer_t customMotorMixer[MAX_SUPPORTED_MOTORS];
|
||||
motorConfig_t motorConfig;
|
||||
servoConfig_t servoConfig;
|
||||
flight3DConfig_t flight3DConfig;
|
||||
|
||||
#ifdef USE_SERVOS
|
||||
servoConfig_t servoConfig;
|
||||
servoMixerConfig_t servoMixerConfig;
|
||||
servoMixer_t customServoMixer[MAX_SERVO_RULES];
|
||||
// Servo-related stuff
|
||||
servoParam_t servoConf[MAX_SUPPORTED_SERVOS]; // servo configuration
|
||||
|
|
|
@ -72,6 +72,7 @@
|
|||
#include "telemetry/telemetry.h"
|
||||
|
||||
#include "flight/mixer.h"
|
||||
#include "flight/servos.h"
|
||||
#include "flight/pid.h"
|
||||
#include "flight/imu.h"
|
||||
#include "flight/failsafe.h"
|
||||
|
@ -407,13 +408,17 @@ void resetRcControlsConfig(rcControlsConfig_t *rcControlsConfig)
|
|||
void resetMixerConfig(mixerConfig_t *mixerConfig)
|
||||
{
|
||||
mixerConfig->yaw_motor_direction = 1;
|
||||
#ifdef USE_SERVOS
|
||||
mixerConfig->tri_unarmed_servo = 1;
|
||||
mixerConfig->servo_lowpass_freq = 400;
|
||||
mixerConfig->servo_lowpass_enable = 0;
|
||||
#endif
|
||||
}
|
||||
|
||||
#ifdef USE_SERVOS
|
||||
void resetServoMixerConfig(servoMixerConfig_t *servoMixerConfig)
|
||||
{
|
||||
servoMixerConfig->tri_unarmed_servo = 1;
|
||||
servoMixerConfig->servo_lowpass_freq = 400;
|
||||
servoMixerConfig->servo_lowpass_enable = 0;
|
||||
}
|
||||
#endif
|
||||
|
||||
uint8_t getCurrentProfile(void)
|
||||
{
|
||||
return masterConfig.current_profile_index;
|
||||
|
@ -573,13 +578,13 @@ void createDefaultConfig(master_t *config)
|
|||
config->auto_disarm_delay = 5;
|
||||
config->small_angle = 25;
|
||||
|
||||
resetMixerConfig(&config->mixerConfig);
|
||||
|
||||
config->airplaneConfig.fixedwing_althold_dir = 1;
|
||||
|
||||
// Motor/ESC/Servo
|
||||
resetMixerConfig(&config->mixerConfig);
|
||||
resetMotorConfig(&config->motorConfig);
|
||||
#ifdef USE_SERVOS
|
||||
resetServoMixerConfig(&config->servoMixerConfig);
|
||||
resetServoConfig(&config->servoConfig);
|
||||
#endif
|
||||
resetFlight3DConfig(&config->flight3DConfig);
|
||||
|
@ -774,7 +779,7 @@ void activateConfig(void)
|
|||
);
|
||||
|
||||
#ifdef USE_SERVOS
|
||||
servoUseConfigs(masterConfig.servoConf, &masterConfig.gimbalConfig);
|
||||
servoUseConfigs(&masterConfig.servoMixerConfig, masterConfig.servoConf, &masterConfig.gimbalConfig);
|
||||
#endif
|
||||
|
||||
imuRuntimeConfig.dcm_kp = masterConfig.dcm_kp / 10000.0f;
|
||||
|
|
|
@ -65,6 +65,7 @@
|
|||
#include "scheduler/scheduler.h"
|
||||
|
||||
#include "flight/mixer.h"
|
||||
#include "flight/servos.h"
|
||||
#include "flight/pid.h"
|
||||
#include "flight/failsafe.h"
|
||||
#include "flight/gtune.h"
|
||||
|
@ -740,7 +741,7 @@ void subTaskMainSubprocesses(void)
|
|||
if (isUsingSticksForArming() && rcData[THROTTLE] <= masterConfig.rxConfig.mincheck
|
||||
#ifndef USE_QUAD_MIXER_ONLY
|
||||
#ifdef USE_SERVOS
|
||||
&& !((masterConfig.mixerMode == MIXER_TRI || masterConfig.mixerMode == MIXER_CUSTOM_TRI) && masterConfig.mixerConfig.tri_unarmed_servo)
|
||||
&& !((masterConfig.mixerMode == MIXER_TRI || masterConfig.mixerMode == MIXER_CUSTOM_TRI) && masterConfig.servoMixerConfig.tri_unarmed_servo)
|
||||
#endif
|
||||
&& masterConfig.mixerMode != MIXER_AIRPLANE
|
||||
&& masterConfig.mixerMode != MIXER_FLYING_WING
|
||||
|
@ -794,6 +795,8 @@ void subTaskMotorUpdate(void)
|
|||
mixTable(¤tProfile->pidProfile);
|
||||
|
||||
#ifdef USE_SERVOS
|
||||
// motor outputs are used as sources for servo mixing, so motors must be calculated using mixTable() before servos.
|
||||
servoTable();
|
||||
filterServos();
|
||||
writeServos();
|
||||
#endif
|
||||
|
|
|
@ -17,14 +17,12 @@
|
|||
|
||||
#include <stdbool.h>
|
||||
#include <stdint.h>
|
||||
#include <stdlib.h>
|
||||
#include <string.h>
|
||||
#include <math.h>
|
||||
|
||||
#include "platform.h"
|
||||
|
||||
#include "build/build_config.h"
|
||||
#include "build/debug.h"
|
||||
|
||||
#include "common/axis.h"
|
||||
#include "common/maths.h"
|
||||
|
@ -32,18 +30,11 @@
|
|||
|
||||
#include "drivers/system.h"
|
||||
#include "drivers/pwm_output.h"
|
||||
#include "drivers/sensor.h"
|
||||
#include "drivers/accgyro.h"
|
||||
#include "drivers/system.h"
|
||||
|
||||
#include "io/motors.h"
|
||||
|
||||
#include "rx/rx.h"
|
||||
|
||||
#include "io/gimbal.h"
|
||||
#include "io/motors.h"
|
||||
#include "io/servos.h"
|
||||
|
||||
#include "sensors/sensors.h"
|
||||
#include "sensors/acceleration.h"
|
||||
#include "sensors/battery.h"
|
||||
|
||||
#include "flight/mixer.h"
|
||||
|
@ -66,20 +57,12 @@ static mixerConfig_t *mixerConfig;
|
|||
static flight3DConfig_t *flight3DConfig;
|
||||
static motorConfig_t *motorConfig;
|
||||
static airplaneConfig_t *airplaneConfig;
|
||||
static rxConfig_t *rxConfig;
|
||||
rxConfig_t *rxConfig;
|
||||
static bool syncMotorOutputWithPidLoop = false;
|
||||
|
||||
static mixerMode_e currentMixerMode;
|
||||
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;
|
||||
static servoParam_t *servoConf;
|
||||
#endif
|
||||
|
||||
static const motorMixer_t mixerQuadX[] = {
|
||||
{ 1.0f, -1.0f, 1.0f, -1.0f }, // REAR_R
|
||||
|
@ -245,92 +228,6 @@ const mixer_t mixers[] = {
|
|||
};
|
||||
#endif
|
||||
|
||||
#ifdef USE_SERVOS
|
||||
|
||||
#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 },
|
||||
};
|
||||
|
||||
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 },
|
||||
};
|
||||
|
||||
static const servoMixer_t servoMixerBI[] = {
|
||||
{ SERVO_BICOPTER_LEFT, INPUT_STABILIZED_YAW, 100, 0, 0, 100, 0 },
|
||||
{ SERVO_BICOPTER_LEFT, INPUT_STABILIZED_PITCH, 100, 0, 0, 100, 0 },
|
||||
{ SERVO_BICOPTER_RIGHT, INPUT_STABILIZED_YAW, 100, 0, 0, 100, 0 },
|
||||
{ SERVO_BICOPTER_RIGHT, INPUT_STABILIZED_PITCH, 100, 0, 0, 100, 0 },
|
||||
};
|
||||
|
||||
static const servoMixer_t servoMixerTri[] = {
|
||||
{ SERVO_RUDDER, INPUT_STABILIZED_YAW, 100, 0, 0, 100, 0 },
|
||||
};
|
||||
|
||||
static const servoMixer_t servoMixerDual[] = {
|
||||
{ SERVO_DUALCOPTER_LEFT, INPUT_STABILIZED_PITCH, 100, 0, 0, 100, 0 },
|
||||
{ SERVO_DUALCOPTER_RIGHT, INPUT_STABILIZED_ROLL, 100, 0, 0, 100, 0 },
|
||||
};
|
||||
|
||||
static const servoMixer_t servoMixerSingle[] = {
|
||||
{ SERVO_SINGLECOPTER_1, INPUT_STABILIZED_YAW, 100, 0, 0, 100, 0 },
|
||||
{ SERVO_SINGLECOPTER_1, INPUT_STABILIZED_PITCH, 100, 0, 0, 100, 0 },
|
||||
{ SERVO_SINGLECOPTER_2, INPUT_STABILIZED_YAW, 100, 0, 0, 100, 0 },
|
||||
{ SERVO_SINGLECOPTER_2, INPUT_STABILIZED_PITCH, 100, 0, 0, 100, 0 },
|
||||
{ SERVO_SINGLECOPTER_3, INPUT_STABILIZED_YAW, 100, 0, 0, 100, 0 },
|
||||
{ SERVO_SINGLECOPTER_3, INPUT_STABILIZED_ROLL, 100, 0, 0, 100, 0 },
|
||||
{ SERVO_SINGLECOPTER_4, INPUT_STABILIZED_YAW, 100, 0, 0, 100, 0 },
|
||||
{ SERVO_SINGLECOPTER_4, INPUT_STABILIZED_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
|
||||
{ COUNT_SERVO_RULES(servoMixerTri), servoMixerTri }, // MULTITYPE_TRI
|
||||
{ 0, NULL }, // MULTITYPE_QUADP
|
||||
{ 0, NULL }, // MULTITYPE_QUADX
|
||||
{ COUNT_SERVO_RULES(servoMixerBI), servoMixerBI }, // MULTITYPE_BI
|
||||
{ COUNT_SERVO_RULES(servoMixerGimbal), servoMixerGimbal }, // * MULTITYPE_GIMBAL
|
||||
{ 0, NULL }, // MULTITYPE_Y6
|
||||
{ 0, NULL }, // MULTITYPE_HEX6
|
||||
{ COUNT_SERVO_RULES(servoMixerFlyingWing), servoMixerFlyingWing },// * MULTITYPE_FLYING_WING
|
||||
{ 0, NULL }, // MULTITYPE_Y4
|
||||
{ 0, NULL }, // MULTITYPE_HEX6X
|
||||
{ 0, NULL }, // MULTITYPE_OCTOX8
|
||||
{ 0, NULL }, // MULTITYPE_OCTOFLATP
|
||||
{ 0, NULL }, // MULTITYPE_OCTOFLATX
|
||||
{ COUNT_SERVO_RULES(servoMixerAirplane), 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
|
||||
{ COUNT_SERVO_RULES(servoMixerDual), servoMixerDual }, // MULTITYPE_DUALCOPTER
|
||||
{ COUNT_SERVO_RULES(servoMixerSingle), servoMixerSingle }, // MULTITYPE_SINGLECOPTER
|
||||
{ 0, NULL }, // MULTITYPE_ATAIL4
|
||||
{ 0, NULL }, // MULTITYPE_CUSTOM
|
||||
{ 0, NULL }, // MULTITYPE_CUSTOM_PLANE
|
||||
{ 0, NULL }, // MULTITYPE_CUSTOM_TRI
|
||||
{ 0, NULL },
|
||||
};
|
||||
|
||||
static servoMixer_t *customServoMixers;
|
||||
#endif
|
||||
|
||||
static motorMixer_t *customMixers;
|
||||
|
||||
static uint16_t disarmMotorOutput, minMotorOutputNormal, maxMotorOutputNormal, deadbandMotor3dHigh, deadbandMotor3dLow;
|
||||
|
@ -370,51 +267,6 @@ void mixerUseConfigs(
|
|||
initEscEndpoints();
|
||||
}
|
||||
|
||||
#ifdef USE_SERVOS
|
||||
void servoUseConfigs(servoParam_t *servoConfToUse, gimbalConfig_t *gimbalConfigToUse)
|
||||
{
|
||||
servoConf = servoConfToUse;
|
||||
gimbalConfig = gimbalConfigToUse;
|
||||
}
|
||||
|
||||
int16_t determineServoMiddleOrForwardFromChannel(servoIndex_e servoIndex)
|
||||
{
|
||||
uint8_t channelToForwardFrom = servoConf[servoIndex].forwardFromChannel;
|
||||
|
||||
if (channelToForwardFrom != CHANNEL_FORWARDING_DISABLED && channelToForwardFrom < rxRuntimeConfig.channelCount) {
|
||||
return rcData[channelToForwardFrom];
|
||||
}
|
||||
|
||||
return servoConf[servoIndex].middle;
|
||||
}
|
||||
|
||||
|
||||
int servoDirection(int servoIndex, int inputSource)
|
||||
{
|
||||
// determine the direction (reversed or not) from the direction bitfield of the servo
|
||||
if (servoConf[servoIndex].reversedSources & (1 << inputSource))
|
||||
return -1;
|
||||
else
|
||||
return 1;
|
||||
}
|
||||
|
||||
void servoMixerInit(servoMixer_t *initialCustomServoMixers)
|
||||
{
|
||||
customServoMixers = initialCustomServoMixers;
|
||||
|
||||
// enable servos for mixes that require them. note, this shifts motor counts.
|
||||
useServo = mixers[currentMixerMode].useServo;
|
||||
// if we want camstab/trig, that also enables servos, even if mixer doesn't
|
||||
if (feature(FEATURE_SERVO_TILT))
|
||||
useServo = 1;
|
||||
|
||||
// give all servos a default command
|
||||
for (uint8_t i = 0; i < MAX_SUPPORTED_SERVOS; i++) {
|
||||
servo[i] = DEFAULT_SERVO_MIDDLE;
|
||||
}
|
||||
}
|
||||
#endif
|
||||
|
||||
void mixerInit(mixerMode_e mixerMode, motorMixer_t *initialCustomMixers)
|
||||
{
|
||||
currentMixerMode = mixerMode;
|
||||
|
@ -424,23 +276,6 @@ void mixerInit(mixerMode_e mixerMode, motorMixer_t *initialCustomMixers)
|
|||
|
||||
#ifndef USE_QUAD_MIXER_ONLY
|
||||
|
||||
void loadCustomServoMixer(void)
|
||||
{
|
||||
// reset settings
|
||||
servoRuleCount = 0;
|
||||
memset(currentServoMixer, 0, sizeof(currentServoMixer));
|
||||
|
||||
// load custom mixer into currentServoMixer
|
||||
for (uint8_t i = 0; i < MAX_SERVO_RULES; i++) {
|
||||
// check if done
|
||||
if (customServoMixers[i].rate == 0)
|
||||
break;
|
||||
|
||||
currentServoMixer[i] = customServoMixers[i];
|
||||
servoRuleCount++;
|
||||
}
|
||||
}
|
||||
|
||||
void mixerConfigureOutput(void)
|
||||
{
|
||||
int i;
|
||||
|
@ -467,14 +302,6 @@ void mixerConfigureOutput(void)
|
|||
}
|
||||
}
|
||||
|
||||
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) {
|
||||
|
@ -486,42 +313,9 @@ void mixerConfigureOutput(void)
|
|||
}
|
||||
}
|
||||
|
||||
// set flag that we're on something with wings
|
||||
if (currentMixerMode == MIXER_FLYING_WING ||
|
||||
currentMixerMode == MIXER_AIRPLANE ||
|
||||
currentMixerMode == MIXER_CUSTOM_AIRPLANE
|
||||
) {
|
||||
ENABLE_STATE(FIXED_WING);
|
||||
|
||||
if (currentMixerMode == MIXER_CUSTOM_AIRPLANE) {
|
||||
loadCustomServoMixer();
|
||||
}
|
||||
} else {
|
||||
DISABLE_STATE(FIXED_WING);
|
||||
|
||||
if (currentMixerMode == MIXER_CUSTOM_TRI) {
|
||||
loadCustomServoMixer();
|
||||
}
|
||||
}
|
||||
|
||||
mixerResetDisarmedMotors();
|
||||
}
|
||||
|
||||
|
||||
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].inputSource = 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;
|
||||
|
@ -561,90 +355,6 @@ void mixerResetDisarmedMotors(void)
|
|||
motor_disarmed[i] = feature(FEATURE_3D) ? flight3DConfig->neutral3d : motorConfig->mincommand;
|
||||
}
|
||||
|
||||
#ifdef USE_SERVOS
|
||||
|
||||
STATIC_UNIT_TESTED void forwardAuxChannelsToServos(uint8_t firstServoIndex)
|
||||
{
|
||||
// start forwarding from this channel
|
||||
uint8_t channelOffset = AUX1;
|
||||
|
||||
uint8_t servoOffset;
|
||||
for (servoOffset = 0; servoOffset < MAX_AUX_CHANNEL_COUNT && channelOffset < MAX_SUPPORTED_RC_CHANNEL_COUNT; servoOffset++) {
|
||||
pwmWriteServo(firstServoIndex + servoOffset, rcData[channelOffset++]);
|
||||
}
|
||||
}
|
||||
|
||||
static void updateGimbalServos(uint8_t firstServoIndex)
|
||||
{
|
||||
pwmWriteServo(firstServoIndex + 0, servo[SERVO_GIMBAL_PITCH]);
|
||||
pwmWriteServo(firstServoIndex + 1, servo[SERVO_GIMBAL_ROLL]);
|
||||
}
|
||||
|
||||
void writeServos(void)
|
||||
{
|
||||
uint8_t servoIndex = 0;
|
||||
|
||||
switch (currentMixerMode) {
|
||||
case MIXER_BICOPTER:
|
||||
pwmWriteServo(servoIndex++, servo[SERVO_BICOPTER_LEFT]);
|
||||
pwmWriteServo(servoIndex++, servo[SERVO_BICOPTER_RIGHT]);
|
||||
break;
|
||||
|
||||
case MIXER_TRI:
|
||||
case MIXER_CUSTOM_TRI:
|
||||
if (mixerConfig->tri_unarmed_servo) {
|
||||
// if unarmed flag set, we always move servo
|
||||
pwmWriteServo(servoIndex++, servo[SERVO_RUDDER]);
|
||||
} else {
|
||||
// otherwise, only move servo when copter is armed
|
||||
if (ARMING_FLAG(ARMED))
|
||||
pwmWriteServo(servoIndex++, servo[SERVO_RUDDER]);
|
||||
else
|
||||
pwmWriteServo(servoIndex++, 0); // kill servo signal completely.
|
||||
}
|
||||
break;
|
||||
|
||||
case MIXER_FLYING_WING:
|
||||
pwmWriteServo(servoIndex++, servo[SERVO_FLAPPERON_1]);
|
||||
pwmWriteServo(servoIndex++, servo[SERVO_FLAPPERON_2]);
|
||||
break;
|
||||
|
||||
case MIXER_DUALCOPTER:
|
||||
pwmWriteServo(servoIndex++, servo[SERVO_DUALCOPTER_LEFT]);
|
||||
pwmWriteServo(servoIndex++, servo[SERVO_DUALCOPTER_RIGHT]);
|
||||
break;
|
||||
|
||||
case MIXER_CUSTOM_AIRPLANE:
|
||||
case MIXER_AIRPLANE:
|
||||
for (int i = SERVO_PLANE_INDEX_MIN; i <= SERVO_PLANE_INDEX_MAX; i++) {
|
||||
pwmWriteServo(servoIndex++, servo[i]);
|
||||
}
|
||||
break;
|
||||
|
||||
case MIXER_SINGLECOPTER:
|
||||
for (int i = SERVO_SINGLECOPTER_INDEX_MIN; i <= SERVO_SINGLECOPTER_INDEX_MAX; i++) {
|
||||
pwmWriteServo(servoIndex++, servo[i]);
|
||||
}
|
||||
break;
|
||||
|
||||
default:
|
||||
break;
|
||||
}
|
||||
|
||||
// Two servos for SERVO_TILT, if enabled
|
||||
if (feature(FEATURE_SERVO_TILT) || currentMixerMode == MIXER_GIMBAL) {
|
||||
updateGimbalServos(servoIndex);
|
||||
servoIndex += 2;
|
||||
}
|
||||
|
||||
// forward AUX to remaining servo outputs (not constrained)
|
||||
if (feature(FEATURE_CHANNEL_FORWARDING)) {
|
||||
forwardAuxChannelsToServos(servoIndex);
|
||||
servoIndex += MAX_AUX_CHANNEL_COUNT;
|
||||
}
|
||||
}
|
||||
#endif
|
||||
|
||||
void writeMotors(void)
|
||||
{
|
||||
for (uint8_t i = 0; i < motorCount; i++) {
|
||||
|
@ -679,91 +389,10 @@ void stopPwmAllMotors(void)
|
|||
delayMicroseconds(1500);
|
||||
}
|
||||
|
||||
#ifndef USE_QUAD_MIXER_ONLY
|
||||
STATIC_UNIT_TESTED void servoMixer(void)
|
||||
{
|
||||
int16_t input[INPUT_SOURCE_COUNT]; // Range [-500:+500]
|
||||
static int16_t currentOutput[MAX_SERVO_RULES];
|
||||
uint8_t i;
|
||||
|
||||
if (FLIGHT_MODE(PASSTHRU_MODE)) {
|
||||
// Direct passthru from RX
|
||||
input[INPUT_STABILIZED_ROLL] = rcCommand[ROLL];
|
||||
input[INPUT_STABILIZED_PITCH] = rcCommand[PITCH];
|
||||
input[INPUT_STABILIZED_YAW] = rcCommand[YAW];
|
||||
} else {
|
||||
// Assisted modes (gyro only or gyro+acc according to AUX configuration in Gui
|
||||
input[INPUT_STABILIZED_ROLL] = axisPIDf[ROLL];
|
||||
input[INPUT_STABILIZED_PITCH] = axisPIDf[PITCH];
|
||||
input[INPUT_STABILIZED_YAW] = axisPIDf[YAW];
|
||||
|
||||
// Reverse yaw servo when inverted in 3D mode
|
||||
if (feature(FEATURE_3D) && (rcData[THROTTLE] < rxConfig->midrc)) {
|
||||
input[INPUT_STABILIZED_YAW] *= -1;
|
||||
}
|
||||
}
|
||||
|
||||
input[INPUT_GIMBAL_PITCH] = scaleRange(attitude.values.pitch, -1800, 1800, -500, +500);
|
||||
input[INPUT_GIMBAL_ROLL] = scaleRange(attitude.values.roll, -1800, 1800, -500, +500);
|
||||
|
||||
input[INPUT_STABILIZED_THROTTLE] = motor[0] - 1000 - 500; // Since it derives from rcCommand or mincommand and must be [-500:+500]
|
||||
|
||||
// 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_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;
|
||||
input[INPUT_RC_AUX1] = rcData[AUX1] - rxConfig->midrc;
|
||||
input[INPUT_RC_AUX2] = rcData[AUX2] - rxConfig->midrc;
|
||||
input[INPUT_RC_AUX3] = rcData[AUX3] - rxConfig->midrc;
|
||||
input[INPUT_RC_AUX4] = rcData[AUX4] - 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].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]);
|
||||
}
|
||||
|
||||
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 *pidProfilePtr)
|
||||
void mixTable(pidProfile_t *pidProfile)
|
||||
{
|
||||
uint32_t i = 0;
|
||||
float vbatCompensationFactor = 1;
|
||||
pidProfile_t *pidProfile = (pidProfile_t *) pidProfilePtr;
|
||||
|
||||
// Scale roll/pitch/yaw uniformly to fit within throttle range
|
||||
// Initial mixer concept by bdoiron74 reused and optimized for Air Mode
|
||||
|
@ -885,94 +514,4 @@ void mixTable(void *pidProfilePtr)
|
|||
}
|
||||
}
|
||||
}
|
||||
|
||||
// motor outputs are used as sources for servo mixing, so motors must be calculated before servos.
|
||||
|
||||
#if !defined(USE_QUAD_MIXER_ONLY) || defined(USE_SERVOS)
|
||||
|
||||
// airplane / servo mixes
|
||||
switch (currentMixerMode) {
|
||||
case MIXER_CUSTOM_AIRPLANE:
|
||||
case MIXER_FLYING_WING:
|
||||
case MIXER_AIRPLANE:
|
||||
case MIXER_BICOPTER:
|
||||
case MIXER_CUSTOM_TRI:
|
||||
case MIXER_TRI:
|
||||
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 * attitude.values.pitch) / 50) + determineServoMiddleOrForwardFromChannel(SERVO_GIMBAL_PITCH);
|
||||
servo[SERVO_GIMBAL_ROLL] = (((int32_t)servoConf[SERVO_GIMBAL_ROLL].rate * attitude.values.roll) / 50) + determineServoMiddleOrForwardFromChannel(SERVO_GIMBAL_ROLL);
|
||||
break;
|
||||
*/
|
||||
|
||||
default:
|
||||
break;
|
||||
}
|
||||
|
||||
// camera stabilization
|
||||
if (feature(FEATURE_SERVO_TILT)) {
|
||||
// center at fixed position, or vary either pitch or roll by RC channel
|
||||
servo[SERVO_GIMBAL_PITCH] = determineServoMiddleOrForwardFromChannel(SERVO_GIMBAL_PITCH);
|
||||
servo[SERVO_GIMBAL_ROLL] = determineServoMiddleOrForwardFromChannel(SERVO_GIMBAL_ROLL);
|
||||
|
||||
if (IS_RC_MODE_ACTIVE(BOXCAMSTAB)) {
|
||||
if (gimbalConfig->mode == GIMBAL_MODE_MIXTILT) {
|
||||
servo[SERVO_GIMBAL_PITCH] -= (-(int32_t)servoConf[SERVO_GIMBAL_PITCH].rate) * attitude.values.pitch / 50 - (int32_t)servoConf[SERVO_GIMBAL_ROLL].rate * attitude.values.roll / 50;
|
||||
servo[SERVO_GIMBAL_ROLL] += (-(int32_t)servoConf[SERVO_GIMBAL_PITCH].rate) * attitude.values.pitch / 50 + (int32_t)servoConf[SERVO_GIMBAL_ROLL].rate * attitude.values.roll / 50;
|
||||
} else {
|
||||
servo[SERVO_GIMBAL_PITCH] += (int32_t)servoConf[SERVO_GIMBAL_PITCH].rate * attitude.values.pitch / 50;
|
||||
servo[SERVO_GIMBAL_ROLL] += (int32_t)servoConf[SERVO_GIMBAL_ROLL].rate * attitude.values.roll / 50;
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
// constrain servos
|
||||
for (i = 0; i < MAX_SUPPORTED_SERVOS; i++) {
|
||||
servo[i] = constrain(servo[i], servoConf[i].min, servoConf[i].max); // limit the values
|
||||
}
|
||||
#endif
|
||||
}
|
||||
|
||||
#ifdef USE_SERVOS
|
||||
bool isMixerUsingServos(void)
|
||||
{
|
||||
return useServo;
|
||||
}
|
||||
#endif
|
||||
|
||||
void filterServos(void)
|
||||
{
|
||||
#ifdef USE_SERVOS
|
||||
static int16_t servoIdx;
|
||||
static bool servoFilterIsSet;
|
||||
static biquadFilter_t servoFilter[MAX_SUPPORTED_SERVOS];
|
||||
|
||||
#if defined(MIXER_DEBUG)
|
||||
uint32_t startTime = micros();
|
||||
#endif
|
||||
|
||||
if (mixerConfig->servo_lowpass_enable) {
|
||||
for (servoIdx = 0; servoIdx < MAX_SUPPORTED_SERVOS; servoIdx++) {
|
||||
if (!servoFilterIsSet) {
|
||||
biquadFilterInitLPF(&servoFilter[servoIdx], mixerConfig->servo_lowpass_freq, targetPidLooptime);
|
||||
servoFilterIsSet = true;
|
||||
}
|
||||
|
||||
servo[servoIdx] = lrintf(biquadFilterApply(&servoFilter[servoIdx], (float)servo[servoIdx]));
|
||||
// Sanity check
|
||||
servo[servoIdx] = constrain(servo[servoIdx], servoConf[servoIdx].min, servoConf[servoIdx].max);
|
||||
}
|
||||
}
|
||||
#if defined(MIXER_DEBUG)
|
||||
debug[0] = (int16_t)(micros() - startTime);
|
||||
#endif
|
||||
|
||||
#endif
|
||||
}
|
||||
|
||||
|
|
|
@ -18,7 +18,6 @@
|
|||
#pragma once
|
||||
|
||||
#define MAX_SUPPORTED_MOTORS 12
|
||||
#define MAX_SUPPORTED_SERVOS 8
|
||||
|
||||
#define QUAD_MOTOR_COUNT 4
|
||||
|
||||
|
@ -77,11 +76,6 @@ typedef struct mixer_s {
|
|||
|
||||
typedef struct mixerConfig_s {
|
||||
int8_t yaw_motor_direction;
|
||||
#ifdef USE_SERVOS
|
||||
uint8_t tri_unarmed_servo; // send tail servo correction pulses even when unarmed
|
||||
uint16_t servo_lowpass_freq; // lowpass servo filter frequency selection; 1/1000ths of loop freq
|
||||
int8_t servo_lowpass_enable; // enable/disable lowpass filter
|
||||
#endif
|
||||
} mixerConfig_t;
|
||||
|
||||
typedef struct flight3DConfig_s {
|
||||
|
@ -97,105 +91,6 @@ typedef struct airplaneConfig_s {
|
|||
|
||||
#define CHANNEL_FORWARDING_DISABLED (uint8_t)0xFF
|
||||
|
||||
#ifdef USE_SERVOS
|
||||
|
||||
// These must be consecutive, see 'reversedSources'
|
||||
enum {
|
||||
INPUT_STABILIZED_ROLL = 0,
|
||||
INPUT_STABILIZED_PITCH,
|
||||
INPUT_STABILIZED_YAW,
|
||||
INPUT_STABILIZED_THROTTLE,
|
||||
INPUT_RC_ROLL,
|
||||
INPUT_RC_PITCH,
|
||||
INPUT_RC_YAW,
|
||||
INPUT_RC_THROTTLE,
|
||||
INPUT_RC_AUX1,
|
||||
INPUT_RC_AUX2,
|
||||
INPUT_RC_AUX3,
|
||||
INPUT_RC_AUX4,
|
||||
INPUT_GIMBAL_PITCH,
|
||||
INPUT_GIMBAL_ROLL,
|
||||
|
||||
INPUT_SOURCE_COUNT
|
||||
} inputSource_e;
|
||||
|
||||
// target servo channels
|
||||
typedef enum {
|
||||
SERVO_GIMBAL_PITCH = 0,
|
||||
SERVO_GIMBAL_ROLL = 1,
|
||||
SERVO_FLAPS = 2,
|
||||
SERVO_FLAPPERON_1 = 3,
|
||||
SERVO_FLAPPERON_2 = 4,
|
||||
SERVO_RUDDER = 5,
|
||||
SERVO_ELEVATOR = 6,
|
||||
SERVO_THROTTLE = 7, // for internal combustion (IC) planes
|
||||
|
||||
SERVO_BICOPTER_LEFT = 4,
|
||||
SERVO_BICOPTER_RIGHT = 5,
|
||||
|
||||
SERVO_DUALCOPTER_LEFT = 4,
|
||||
SERVO_DUALCOPTER_RIGHT = 5,
|
||||
|
||||
SERVO_SINGLECOPTER_1 = 3,
|
||||
SERVO_SINGLECOPTER_2 = 4,
|
||||
SERVO_SINGLECOPTER_3 = 5,
|
||||
SERVO_SINGLECOPTER_4 = 6,
|
||||
|
||||
} servoIndex_e; // FIXME rename to servoChannel_e
|
||||
|
||||
#define SERVO_PLANE_INDEX_MIN SERVO_FLAPS
|
||||
#define SERVO_PLANE_INDEX_MAX SERVO_THROTTLE
|
||||
|
||||
#define SERVO_DUALCOPTER_INDEX_MIN SERVO_DUALCOPTER_LEFT
|
||||
#define SERVO_DUALCOPTER_INDEX_MAX SERVO_DUALCOPTER_RIGHT
|
||||
|
||||
#define SERVO_SINGLECOPTER_INDEX_MIN SERVO_SINGLECOPTER_1
|
||||
#define SERVO_SINGLECOPTER_INDEX_MAX SERVO_SINGLECOPTER_4
|
||||
|
||||
#define SERVO_FLAPPERONS_MIN SERVO_FLAPPERON_1
|
||||
#define SERVO_FLAPPERONS_MAX SERVO_FLAPPERON_2
|
||||
|
||||
typedef struct servoMixer_s {
|
||||
uint8_t targetChannel; // servo that receives the output of the 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
|
||||
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_s {
|
||||
uint8_t servoRuleCount;
|
||||
const servoMixer_t *rule;
|
||||
} mixerRules_t;
|
||||
|
||||
typedef struct servoParam_s {
|
||||
int16_t min; // servo min
|
||||
int16_t max; // servo max
|
||||
int16_t middle; // servo middle
|
||||
int8_t rate; // range [-125;+125] ; can be used to adjust a rate 0-125% and a direction
|
||||
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
|
||||
uint32_t reversedSources; // the direction of servo movement for each input source of the servo mixer, bit set=inverted
|
||||
} __attribute__ ((__packed__)) servoParam_t;
|
||||
|
||||
struct gimbalConfig_s;
|
||||
struct motorConfig_s;
|
||||
struct rxConfig_s;
|
||||
|
||||
extern int16_t servo[MAX_SUPPORTED_SERVOS];
|
||||
bool isMixerUsingServos(void);
|
||||
void writeServos(void);
|
||||
void filterServos(void);
|
||||
#endif
|
||||
|
||||
extern const mixer_t mixers[];
|
||||
extern int16_t motor[MAX_SUPPORTED_MOTORS];
|
||||
extern int16_t motor_disarmed[MAX_SUPPORTED_MOTORS];
|
||||
|
@ -211,21 +106,13 @@ void mixerUseConfigs(
|
|||
|
||||
void writeAllMotors(int16_t mc);
|
||||
void mixerLoadMix(int index, motorMixer_t *customMixers);
|
||||
#ifdef USE_SERVOS
|
||||
void servoMixerInit(servoMixer_t *customServoMixers);
|
||||
struct servoParam_s;
|
||||
struct gimbalConfig_s;
|
||||
void servoUseConfigs(struct servoParam_s *servoConfToUse, struct gimbalConfig_s *gimbalConfigToUse);
|
||||
void servoMixerLoadMix(int index, servoMixer_t *customServoMixers);
|
||||
void loadCustomServoMixer(void);
|
||||
int servoDirection(int servoIndex, int fromChannel);
|
||||
#endif
|
||||
void mixerInit(mixerMode_e mixerMode, motorMixer_t *customMotorMixers);
|
||||
|
||||
void mixerConfigureOutput(void);
|
||||
|
||||
void mixerResetDisarmedMotors(void);
|
||||
void mixTable(void *pidProfilePtr);
|
||||
struct pidProfile_s;
|
||||
void mixTable(struct pidProfile_s *pidProfile);
|
||||
void syncMotors(bool enabled);
|
||||
void writeMotors(void);
|
||||
void stopMotors(void);
|
||||
|
|
|
@ -0,0 +1,490 @@
|
|||
/*
|
||||
* This file is part of Cleanflight.
|
||||
*
|
||||
* Cleanflight is free software: you can redistribute it and/or modify
|
||||
* it under the terms of the GNU General Public License as published by
|
||||
* the Free Software Foundation, either version 3 of the License, or
|
||||
* (at your option) any later version.
|
||||
*
|
||||
* Cleanflight is distributed in the hope that it will be useful,
|
||||
* but WITHOUT ANY WARRANTY; without even the implied warranty of
|
||||
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
|
||||
* GNU General Public License for more details.
|
||||
*
|
||||
* You should have received a copy of the GNU General Public License
|
||||
* along with Cleanflight. If not, see <http://www.gnu.org/licenses/>.
|
||||
*/
|
||||
|
||||
#include <stdbool.h>
|
||||
#include <stdint.h>
|
||||
#include <string.h>
|
||||
#include <math.h>
|
||||
|
||||
#include "platform.h"
|
||||
|
||||
#ifdef USE_SERVOS
|
||||
|
||||
#include "build/build_config.h"
|
||||
|
||||
#include "common/filter.h"
|
||||
|
||||
#include "drivers/pwm_output.h"
|
||||
#include "drivers/system.h"
|
||||
|
||||
#include "rx/rx.h"
|
||||
|
||||
#include "io/gimbal.h"
|
||||
#include "io/servos.h"
|
||||
|
||||
#include "flight/mixer.h"
|
||||
#include "flight/pid.h"
|
||||
#include "flight/imu.h"
|
||||
|
||||
#include "fc/config.h"
|
||||
#include "fc/rc_controls.h"
|
||||
#include "fc/runtime_config.h"
|
||||
|
||||
#include "config/feature.h"
|
||||
|
||||
extern mixerMode_e currentMixerMode;
|
||||
extern rxConfig_t *rxConfig;
|
||||
|
||||
static servoMixerConfig_t *servoMixerConfig;
|
||||
|
||||
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;
|
||||
static servoParam_t *servoConf;
|
||||
|
||||
|
||||
#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 },
|
||||
};
|
||||
|
||||
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 },
|
||||
};
|
||||
|
||||
static const servoMixer_t servoMixerBI[] = {
|
||||
{ SERVO_BICOPTER_LEFT, INPUT_STABILIZED_YAW, 100, 0, 0, 100, 0 },
|
||||
{ SERVO_BICOPTER_LEFT, INPUT_STABILIZED_PITCH, 100, 0, 0, 100, 0 },
|
||||
{ SERVO_BICOPTER_RIGHT, INPUT_STABILIZED_YAW, 100, 0, 0, 100, 0 },
|
||||
{ SERVO_BICOPTER_RIGHT, INPUT_STABILIZED_PITCH, 100, 0, 0, 100, 0 },
|
||||
};
|
||||
|
||||
static const servoMixer_t servoMixerTri[] = {
|
||||
{ SERVO_RUDDER, INPUT_STABILIZED_YAW, 100, 0, 0, 100, 0 },
|
||||
};
|
||||
|
||||
static const servoMixer_t servoMixerDual[] = {
|
||||
{ SERVO_DUALCOPTER_LEFT, INPUT_STABILIZED_PITCH, 100, 0, 0, 100, 0 },
|
||||
{ SERVO_DUALCOPTER_RIGHT, INPUT_STABILIZED_ROLL, 100, 0, 0, 100, 0 },
|
||||
};
|
||||
|
||||
static const servoMixer_t servoMixerSingle[] = {
|
||||
{ SERVO_SINGLECOPTER_1, INPUT_STABILIZED_YAW, 100, 0, 0, 100, 0 },
|
||||
{ SERVO_SINGLECOPTER_1, INPUT_STABILIZED_PITCH, 100, 0, 0, 100, 0 },
|
||||
{ SERVO_SINGLECOPTER_2, INPUT_STABILIZED_YAW, 100, 0, 0, 100, 0 },
|
||||
{ SERVO_SINGLECOPTER_2, INPUT_STABILIZED_PITCH, 100, 0, 0, 100, 0 },
|
||||
{ SERVO_SINGLECOPTER_3, INPUT_STABILIZED_YAW, 100, 0, 0, 100, 0 },
|
||||
{ SERVO_SINGLECOPTER_3, INPUT_STABILIZED_ROLL, 100, 0, 0, 100, 0 },
|
||||
{ SERVO_SINGLECOPTER_4, INPUT_STABILIZED_YAW, 100, 0, 0, 100, 0 },
|
||||
{ SERVO_SINGLECOPTER_4, INPUT_STABILIZED_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
|
||||
{ COUNT_SERVO_RULES(servoMixerTri), servoMixerTri }, // MULTITYPE_TRI
|
||||
{ 0, NULL }, // MULTITYPE_QUADP
|
||||
{ 0, NULL }, // MULTITYPE_QUADX
|
||||
{ COUNT_SERVO_RULES(servoMixerBI), servoMixerBI }, // MULTITYPE_BI
|
||||
{ COUNT_SERVO_RULES(servoMixerGimbal), servoMixerGimbal }, // * MULTITYPE_GIMBAL
|
||||
{ 0, NULL }, // MULTITYPE_Y6
|
||||
{ 0, NULL }, // MULTITYPE_HEX6
|
||||
{ COUNT_SERVO_RULES(servoMixerFlyingWing), servoMixerFlyingWing },// * MULTITYPE_FLYING_WING
|
||||
{ 0, NULL }, // MULTITYPE_Y4
|
||||
{ 0, NULL }, // MULTITYPE_HEX6X
|
||||
{ 0, NULL }, // MULTITYPE_OCTOX8
|
||||
{ 0, NULL }, // MULTITYPE_OCTOFLATP
|
||||
{ 0, NULL }, // MULTITYPE_OCTOFLATX
|
||||
{ COUNT_SERVO_RULES(servoMixerAirplane), 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
|
||||
{ COUNT_SERVO_RULES(servoMixerDual), servoMixerDual }, // MULTITYPE_DUALCOPTER
|
||||
{ COUNT_SERVO_RULES(servoMixerSingle), servoMixerSingle }, // MULTITYPE_SINGLECOPTER
|
||||
{ 0, NULL }, // MULTITYPE_ATAIL4
|
||||
{ 0, NULL }, // MULTITYPE_CUSTOM
|
||||
{ 0, NULL }, // MULTITYPE_CUSTOM_PLANE
|
||||
{ 0, NULL }, // MULTITYPE_CUSTOM_TRI
|
||||
{ 0, NULL },
|
||||
};
|
||||
|
||||
static servoMixer_t *customServoMixers;
|
||||
|
||||
void servoUseConfigs(servoMixerConfig_t *servoMixerConfigToUse, servoParam_t *servoParamsToUse, struct gimbalConfig_s *gimbalConfigToUse)
|
||||
{
|
||||
servoMixerConfig = servoMixerConfigToUse;
|
||||
servoConf = servoParamsToUse;
|
||||
gimbalConfig = gimbalConfigToUse;
|
||||
}
|
||||
|
||||
int16_t determineServoMiddleOrForwardFromChannel(servoIndex_e servoIndex)
|
||||
{
|
||||
uint8_t channelToForwardFrom = servoConf[servoIndex].forwardFromChannel;
|
||||
|
||||
if (channelToForwardFrom != CHANNEL_FORWARDING_DISABLED && channelToForwardFrom < rxRuntimeConfig.channelCount) {
|
||||
return rcData[channelToForwardFrom];
|
||||
}
|
||||
|
||||
return servoConf[servoIndex].middle;
|
||||
}
|
||||
|
||||
|
||||
int servoDirection(int servoIndex, int inputSource)
|
||||
{
|
||||
// determine the direction (reversed or not) from the direction bitfield of the servo
|
||||
if (servoConf[servoIndex].reversedSources & (1 << inputSource))
|
||||
return -1;
|
||||
else
|
||||
return 1;
|
||||
}
|
||||
|
||||
void servoMixerInit(servoMixer_t *initialCustomServoMixers)
|
||||
{
|
||||
customServoMixers = initialCustomServoMixers;
|
||||
|
||||
// enable servos for mixes that require them. note, this shifts motor counts.
|
||||
useServo = mixers[currentMixerMode].useServo;
|
||||
// if we want camstab/trig, that also enables servos, even if mixer doesn't
|
||||
if (feature(FEATURE_SERVO_TILT))
|
||||
useServo = 1;
|
||||
|
||||
// give all servos a default command
|
||||
for (uint8_t i = 0; i < MAX_SUPPORTED_SERVOS; i++) {
|
||||
servo[i] = DEFAULT_SERVO_MIDDLE;
|
||||
}
|
||||
}
|
||||
|
||||
void loadCustomServoMixer(void)
|
||||
{
|
||||
// reset settings
|
||||
servoRuleCount = 0;
|
||||
memset(currentServoMixer, 0, sizeof(currentServoMixer));
|
||||
|
||||
// load custom mixer into currentServoMixer
|
||||
for (uint8_t i = 0; i < MAX_SERVO_RULES; i++) {
|
||||
// check if done
|
||||
if (customServoMixers[i].rate == 0)
|
||||
break;
|
||||
|
||||
currentServoMixer[i] = customServoMixers[i];
|
||||
servoRuleCount++;
|
||||
}
|
||||
}
|
||||
|
||||
void servoConfigureOutput(void)
|
||||
{
|
||||
if (useServo) {
|
||||
servoRuleCount = servoMixers[currentMixerMode].servoRuleCount;
|
||||
if (servoMixers[currentMixerMode].rule) {
|
||||
for (int i = 0; i < servoRuleCount; i++)
|
||||
currentServoMixer[i] = servoMixers[currentMixerMode].rule[i];
|
||||
}
|
||||
}
|
||||
|
||||
// set flag that we're on something with wings
|
||||
if (currentMixerMode == MIXER_FLYING_WING ||
|
||||
currentMixerMode == MIXER_AIRPLANE ||
|
||||
currentMixerMode == MIXER_CUSTOM_AIRPLANE
|
||||
) {
|
||||
ENABLE_STATE(FIXED_WING);
|
||||
|
||||
if (currentMixerMode == MIXER_CUSTOM_AIRPLANE) {
|
||||
loadCustomServoMixer();
|
||||
}
|
||||
} else {
|
||||
DISABLE_STATE(FIXED_WING);
|
||||
|
||||
if (currentMixerMode == MIXER_CUSTOM_TRI) {
|
||||
loadCustomServoMixer();
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
|
||||
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].inputSource = customServoMixers[i].rate = customServoMixers[i].box = 0;
|
||||
|
||||
for (i = 0; i < servoMixers[index].servoRuleCount; i++)
|
||||
customServoMixers[i] = servoMixers[index].rule[i];
|
||||
}
|
||||
|
||||
STATIC_UNIT_TESTED void forwardAuxChannelsToServos(uint8_t firstServoIndex)
|
||||
{
|
||||
// start forwarding from this channel
|
||||
uint8_t channelOffset = AUX1;
|
||||
|
||||
uint8_t servoOffset;
|
||||
for (servoOffset = 0; servoOffset < MAX_AUX_CHANNEL_COUNT && channelOffset < MAX_SUPPORTED_RC_CHANNEL_COUNT; servoOffset++) {
|
||||
pwmWriteServo(firstServoIndex + servoOffset, rcData[channelOffset++]);
|
||||
}
|
||||
}
|
||||
|
||||
static void updateGimbalServos(uint8_t firstServoIndex)
|
||||
{
|
||||
pwmWriteServo(firstServoIndex + 0, servo[SERVO_GIMBAL_PITCH]);
|
||||
pwmWriteServo(firstServoIndex + 1, servo[SERVO_GIMBAL_ROLL]);
|
||||
}
|
||||
|
||||
void writeServos(void)
|
||||
{
|
||||
uint8_t servoIndex = 0;
|
||||
|
||||
switch (currentMixerMode) {
|
||||
case MIXER_BICOPTER:
|
||||
pwmWriteServo(servoIndex++, servo[SERVO_BICOPTER_LEFT]);
|
||||
pwmWriteServo(servoIndex++, servo[SERVO_BICOPTER_RIGHT]);
|
||||
break;
|
||||
|
||||
case MIXER_TRI:
|
||||
case MIXER_CUSTOM_TRI:
|
||||
if (servoMixerConfig->tri_unarmed_servo) {
|
||||
// if unarmed flag set, we always move servo
|
||||
pwmWriteServo(servoIndex++, servo[SERVO_RUDDER]);
|
||||
} else {
|
||||
// otherwise, only move servo when copter is armed
|
||||
if (ARMING_FLAG(ARMED))
|
||||
pwmWriteServo(servoIndex++, servo[SERVO_RUDDER]);
|
||||
else
|
||||
pwmWriteServo(servoIndex++, 0); // kill servo signal completely.
|
||||
}
|
||||
break;
|
||||
|
||||
case MIXER_FLYING_WING:
|
||||
pwmWriteServo(servoIndex++, servo[SERVO_FLAPPERON_1]);
|
||||
pwmWriteServo(servoIndex++, servo[SERVO_FLAPPERON_2]);
|
||||
break;
|
||||
|
||||
case MIXER_DUALCOPTER:
|
||||
pwmWriteServo(servoIndex++, servo[SERVO_DUALCOPTER_LEFT]);
|
||||
pwmWriteServo(servoIndex++, servo[SERVO_DUALCOPTER_RIGHT]);
|
||||
break;
|
||||
|
||||
case MIXER_CUSTOM_AIRPLANE:
|
||||
case MIXER_AIRPLANE:
|
||||
for (int i = SERVO_PLANE_INDEX_MIN; i <= SERVO_PLANE_INDEX_MAX; i++) {
|
||||
pwmWriteServo(servoIndex++, servo[i]);
|
||||
}
|
||||
break;
|
||||
|
||||
case MIXER_SINGLECOPTER:
|
||||
for (int i = SERVO_SINGLECOPTER_INDEX_MIN; i <= SERVO_SINGLECOPTER_INDEX_MAX; i++) {
|
||||
pwmWriteServo(servoIndex++, servo[i]);
|
||||
}
|
||||
break;
|
||||
|
||||
default:
|
||||
break;
|
||||
}
|
||||
|
||||
// Two servos for SERVO_TILT, if enabled
|
||||
if (feature(FEATURE_SERVO_TILT) || currentMixerMode == MIXER_GIMBAL) {
|
||||
updateGimbalServos(servoIndex);
|
||||
servoIndex += 2;
|
||||
}
|
||||
|
||||
// forward AUX to remaining servo outputs (not constrained)
|
||||
if (feature(FEATURE_CHANNEL_FORWARDING)) {
|
||||
forwardAuxChannelsToServos(servoIndex);
|
||||
servoIndex += MAX_AUX_CHANNEL_COUNT;
|
||||
}
|
||||
}
|
||||
|
||||
STATIC_UNIT_TESTED void servoMixer(void)
|
||||
{
|
||||
int16_t input[INPUT_SOURCE_COUNT]; // Range [-500:+500]
|
||||
static int16_t currentOutput[MAX_SERVO_RULES];
|
||||
uint8_t i;
|
||||
|
||||
if (FLIGHT_MODE(PASSTHRU_MODE)) {
|
||||
// Direct passthru from RX
|
||||
input[INPUT_STABILIZED_ROLL] = rcCommand[ROLL];
|
||||
input[INPUT_STABILIZED_PITCH] = rcCommand[PITCH];
|
||||
input[INPUT_STABILIZED_YAW] = rcCommand[YAW];
|
||||
} else {
|
||||
// Assisted modes (gyro only or gyro+acc according to AUX configuration in Gui
|
||||
input[INPUT_STABILIZED_ROLL] = axisPIDf[ROLL];
|
||||
input[INPUT_STABILIZED_PITCH] = axisPIDf[PITCH];
|
||||
input[INPUT_STABILIZED_YAW] = axisPIDf[YAW];
|
||||
|
||||
// Reverse yaw servo when inverted in 3D mode
|
||||
if (feature(FEATURE_3D) && (rcData[THROTTLE] < rxConfig->midrc)) {
|
||||
input[INPUT_STABILIZED_YAW] *= -1;
|
||||
}
|
||||
}
|
||||
|
||||
input[INPUT_GIMBAL_PITCH] = scaleRange(attitude.values.pitch, -1800, 1800, -500, +500);
|
||||
input[INPUT_GIMBAL_ROLL] = scaleRange(attitude.values.roll, -1800, 1800, -500, +500);
|
||||
|
||||
input[INPUT_STABILIZED_THROTTLE] = motor[0] - 1000 - 500; // Since it derives from rcCommand or mincommand and must be [-500:+500]
|
||||
|
||||
// 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_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;
|
||||
input[INPUT_RC_AUX1] = rcData[AUX1] - rxConfig->midrc;
|
||||
input[INPUT_RC_AUX2] = rcData[AUX2] - rxConfig->midrc;
|
||||
input[INPUT_RC_AUX3] = rcData[AUX3] - rxConfig->midrc;
|
||||
input[INPUT_RC_AUX4] = rcData[AUX4] - 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].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]);
|
||||
}
|
||||
|
||||
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);
|
||||
}
|
||||
}
|
||||
|
||||
|
||||
void servoTable(void)
|
||||
{
|
||||
// airplane / servo mixes
|
||||
switch (currentMixerMode) {
|
||||
case MIXER_CUSTOM_AIRPLANE:
|
||||
case MIXER_FLYING_WING:
|
||||
case MIXER_AIRPLANE:
|
||||
case MIXER_BICOPTER:
|
||||
case MIXER_CUSTOM_TRI:
|
||||
case MIXER_TRI:
|
||||
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 * attitude.values.pitch) / 50) + determineServoMiddleOrForwardFromChannel(SERVO_GIMBAL_PITCH);
|
||||
servo[SERVO_GIMBAL_ROLL] = (((int32_t)servoConf[SERVO_GIMBAL_ROLL].rate * attitude.values.roll) / 50) + determineServoMiddleOrForwardFromChannel(SERVO_GIMBAL_ROLL);
|
||||
break;
|
||||
*/
|
||||
|
||||
default:
|
||||
break;
|
||||
}
|
||||
|
||||
// camera stabilization
|
||||
if (feature(FEATURE_SERVO_TILT)) {
|
||||
// center at fixed position, or vary either pitch or roll by RC channel
|
||||
servo[SERVO_GIMBAL_PITCH] = determineServoMiddleOrForwardFromChannel(SERVO_GIMBAL_PITCH);
|
||||
servo[SERVO_GIMBAL_ROLL] = determineServoMiddleOrForwardFromChannel(SERVO_GIMBAL_ROLL);
|
||||
|
||||
if (IS_RC_MODE_ACTIVE(BOXCAMSTAB)) {
|
||||
if (gimbalConfig->mode == GIMBAL_MODE_MIXTILT) {
|
||||
servo[SERVO_GIMBAL_PITCH] -= (-(int32_t)servoConf[SERVO_GIMBAL_PITCH].rate) * attitude.values.pitch / 50 - (int32_t)servoConf[SERVO_GIMBAL_ROLL].rate * attitude.values.roll / 50;
|
||||
servo[SERVO_GIMBAL_ROLL] += (-(int32_t)servoConf[SERVO_GIMBAL_PITCH].rate) * attitude.values.pitch / 50 + (int32_t)servoConf[SERVO_GIMBAL_ROLL].rate * attitude.values.roll / 50;
|
||||
} else {
|
||||
servo[SERVO_GIMBAL_PITCH] += (int32_t)servoConf[SERVO_GIMBAL_PITCH].rate * attitude.values.pitch / 50;
|
||||
servo[SERVO_GIMBAL_ROLL] += (int32_t)servoConf[SERVO_GIMBAL_ROLL].rate * attitude.values.roll / 50;
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
// constrain servos
|
||||
for (int i = 0; i < MAX_SUPPORTED_SERVOS; i++) {
|
||||
servo[i] = constrain(servo[i], servoConf[i].min, servoConf[i].max); // limit the values
|
||||
}
|
||||
}
|
||||
|
||||
bool isMixerUsingServos(void)
|
||||
{
|
||||
return useServo;
|
||||
}
|
||||
|
||||
void filterServos(void)
|
||||
{
|
||||
static int16_t servoIdx;
|
||||
static bool servoFilterIsSet;
|
||||
static biquadFilter_t servoFilter[MAX_SUPPORTED_SERVOS];
|
||||
|
||||
#if defined(MIXER_DEBUG)
|
||||
uint32_t startTime = micros();
|
||||
#endif
|
||||
|
||||
if (servoMixerConfig->servo_lowpass_enable) {
|
||||
for (servoIdx = 0; servoIdx < MAX_SUPPORTED_SERVOS; servoIdx++) {
|
||||
if (!servoFilterIsSet) {
|
||||
biquadFilterInitLPF(&servoFilter[servoIdx], servoMixerConfig->servo_lowpass_freq, targetPidLooptime);
|
||||
servoFilterIsSet = true;
|
||||
}
|
||||
|
||||
servo[servoIdx] = lrintf(biquadFilterApply(&servoFilter[servoIdx], (float)servo[servoIdx]));
|
||||
// Sanity check
|
||||
servo[servoIdx] = constrain(servo[servoIdx], servoConf[servoIdx].min, servoConf[servoIdx].max);
|
||||
}
|
||||
}
|
||||
#if defined(MIXER_DEBUG)
|
||||
debug[0] = (int16_t)(micros() - startTime);
|
||||
#endif
|
||||
}
|
||||
#endif
|
|
@ -0,0 +1,129 @@
|
|||
/*
|
||||
* This file is part of Cleanflight.
|
||||
*
|
||||
* Cleanflight is free software: you can redistribute it and/or modify
|
||||
* it under the terms of the GNU General Public License as published by
|
||||
* the Free Software Foundation, either version 3 of the License, or
|
||||
* (at your option) any later version.
|
||||
*
|
||||
* Cleanflight is distributed in the hope that it will be useful,
|
||||
* but WITHOUT ANY WARRANTY; without even the implied warranty of
|
||||
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
|
||||
* GNU General Public License for more details.
|
||||
*
|
||||
* You should have received a copy of the GNU General Public License
|
||||
* along with Cleanflight. If not, see <http://www.gnu.org/licenses/>.
|
||||
*/
|
||||
|
||||
#pragma once
|
||||
|
||||
#define MAX_SUPPORTED_SERVOS 8
|
||||
|
||||
// These must be consecutive, see 'reversedSources'
|
||||
enum {
|
||||
INPUT_STABILIZED_ROLL = 0,
|
||||
INPUT_STABILIZED_PITCH,
|
||||
INPUT_STABILIZED_YAW,
|
||||
INPUT_STABILIZED_THROTTLE,
|
||||
INPUT_RC_ROLL,
|
||||
INPUT_RC_PITCH,
|
||||
INPUT_RC_YAW,
|
||||
INPUT_RC_THROTTLE,
|
||||
INPUT_RC_AUX1,
|
||||
INPUT_RC_AUX2,
|
||||
INPUT_RC_AUX3,
|
||||
INPUT_RC_AUX4,
|
||||
INPUT_GIMBAL_PITCH,
|
||||
INPUT_GIMBAL_ROLL,
|
||||
|
||||
INPUT_SOURCE_COUNT
|
||||
} inputSource_e;
|
||||
|
||||
// target servo channels
|
||||
typedef enum {
|
||||
SERVO_GIMBAL_PITCH = 0,
|
||||
SERVO_GIMBAL_ROLL = 1,
|
||||
SERVO_FLAPS = 2,
|
||||
SERVO_FLAPPERON_1 = 3,
|
||||
SERVO_FLAPPERON_2 = 4,
|
||||
SERVO_RUDDER = 5,
|
||||
SERVO_ELEVATOR = 6,
|
||||
SERVO_THROTTLE = 7, // for internal combustion (IC) planes
|
||||
|
||||
SERVO_BICOPTER_LEFT = 4,
|
||||
SERVO_BICOPTER_RIGHT = 5,
|
||||
|
||||
SERVO_DUALCOPTER_LEFT = 4,
|
||||
SERVO_DUALCOPTER_RIGHT = 5,
|
||||
|
||||
SERVO_SINGLECOPTER_1 = 3,
|
||||
SERVO_SINGLECOPTER_2 = 4,
|
||||
SERVO_SINGLECOPTER_3 = 5,
|
||||
SERVO_SINGLECOPTER_4 = 6,
|
||||
|
||||
} servoIndex_e; // FIXME rename to servoChannel_e
|
||||
|
||||
#define SERVO_PLANE_INDEX_MIN SERVO_FLAPS
|
||||
#define SERVO_PLANE_INDEX_MAX SERVO_THROTTLE
|
||||
|
||||
#define SERVO_DUALCOPTER_INDEX_MIN SERVO_DUALCOPTER_LEFT
|
||||
#define SERVO_DUALCOPTER_INDEX_MAX SERVO_DUALCOPTER_RIGHT
|
||||
|
||||
#define SERVO_SINGLECOPTER_INDEX_MIN SERVO_SINGLECOPTER_1
|
||||
#define SERVO_SINGLECOPTER_INDEX_MAX SERVO_SINGLECOPTER_4
|
||||
|
||||
#define SERVO_FLAPPERONS_MIN SERVO_FLAPPERON_1
|
||||
#define SERVO_FLAPPERONS_MAX SERVO_FLAPPERON_2
|
||||
|
||||
typedef struct servoMixer_s {
|
||||
uint8_t targetChannel; // servo that receives the output of the 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
|
||||
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_s {
|
||||
uint8_t servoRuleCount;
|
||||
const servoMixer_t *rule;
|
||||
} mixerRules_t;
|
||||
|
||||
typedef struct servoParam_s {
|
||||
int16_t min; // servo min
|
||||
int16_t max; // servo max
|
||||
int16_t middle; // servo middle
|
||||
int8_t rate; // range [-125;+125] ; can be used to adjust a rate 0-125% and a direction
|
||||
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
|
||||
uint32_t reversedSources; // the direction of servo movement for each input source of the servo mixer, bit set=inverted
|
||||
} __attribute__ ((__packed__)) servoParam_t;
|
||||
|
||||
typedef struct servoMixerConfig_s{
|
||||
uint8_t tri_unarmed_servo; // send tail servo correction pulses even when unarmed
|
||||
uint16_t servo_lowpass_freq; // lowpass servo filter frequency selection; 1/1000ths of loop freq
|
||||
int8_t servo_lowpass_enable; // enable/disable lowpass filter
|
||||
} servoMixerConfig_t;
|
||||
|
||||
extern int16_t servo[MAX_SUPPORTED_SERVOS];
|
||||
|
||||
void servoTable(void);
|
||||
bool isMixerUsingServos(void);
|
||||
void writeServos(void);
|
||||
void filterServos(void);
|
||||
|
||||
void servoMixerInit(servoMixer_t *customServoMixers);
|
||||
struct gimbalConfig_s;
|
||||
void servoUseConfigs(servoMixerConfig_t *servoConfigToUse, servoParam_t *servoParamsToUse, struct gimbalConfig_s *gimbalConfigToUse);
|
||||
void servoMixerLoadMix(int index, servoMixer_t *customServoMixers);
|
||||
void loadCustomServoMixer(void);
|
||||
void servoConfigureOutput(void);
|
||||
int servoDirection(int servoIndex, int fromChannel);
|
||||
|
|
@ -17,7 +17,7 @@
|
|||
|
||||
#pragma once
|
||||
|
||||
#include "drivers/io.h"
|
||||
#include "drivers/io_types.h"
|
||||
#include "flight/mixer.h"
|
||||
|
||||
typedef struct motorConfig_s {
|
||||
|
|
|
@ -823,9 +823,9 @@ const clivalue_t valueTable[] = {
|
|||
{ "pidsum_limit", VAR_FLOAT | PROFILE_VALUE, &masterConfig.profile[0].pidProfile.pidSumLimit, .config.minmax = { 0.1, 1.0 } },
|
||||
#ifdef USE_SERVOS
|
||||
{ "servo_center_pulse", VAR_UINT16 | MASTER_VALUE, &masterConfig.servoConfig.servoCenterPulse, .config.minmax = { PWM_RANGE_ZERO, PWM_RANGE_MAX } },
|
||||
{ "tri_unarmed_servo", VAR_INT8 | MASTER_VALUE | MODE_LOOKUP, &masterConfig.mixerConfig.tri_unarmed_servo, .config.lookup = { TABLE_OFF_ON } },
|
||||
{ "servo_lowpass_freq", VAR_UINT16 | MASTER_VALUE, &masterConfig.mixerConfig.servo_lowpass_freq, .config.minmax = { 10, 400} },
|
||||
{ "servo_lowpass_enable", VAR_INT8 | MASTER_VALUE | MODE_LOOKUP, &masterConfig.mixerConfig.servo_lowpass_enable, .config.lookup = { TABLE_OFF_ON } },
|
||||
{ "tri_unarmed_servo", VAR_INT8 | MASTER_VALUE | MODE_LOOKUP, &masterConfig.servoMixerConfig.tri_unarmed_servo, .config.lookup = { TABLE_OFF_ON } },
|
||||
{ "servo_lowpass_freq", VAR_UINT16 | MASTER_VALUE, &masterConfig.servoMixerConfig.servo_lowpass_freq, .config.minmax = { 10, 400} },
|
||||
{ "servo_lowpass_enable", VAR_INT8 | MASTER_VALUE | MODE_LOOKUP, &masterConfig.servoMixerConfig.servo_lowpass_enable, .config.lookup = { TABLE_OFF_ON } },
|
||||
{ "servo_pwm_rate", VAR_UINT16 | MASTER_VALUE, &masterConfig.servoConfig.servoPwmRate, .config.minmax = { 50, 498 } },
|
||||
{ "gimbal_mode", VAR_UINT8 | MASTER_VALUE | MODE_LOOKUP, &masterConfig.gimbalConfig.mode, .config.lookup = { TABLE_GIMBAL_MODE } },
|
||||
#endif
|
||||
|
|
|
@ -17,8 +17,8 @@
|
|||
|
||||
#pragma once
|
||||
|
||||
#include "drivers/io.h"
|
||||
#include "flight/mixer.h"
|
||||
#include "drivers/io_types.h"
|
||||
#include "flight/servos.h"
|
||||
|
||||
typedef struct servoConfig_s {
|
||||
// PWM values, in milliseconds, common range is 1000-2000 (1 to 2ms)
|
||||
|
|
|
@ -288,7 +288,7 @@ void init(void)
|
|||
#endif
|
||||
|
||||
mixerConfigureOutput();
|
||||
// pwmInit() needs to be called as soon as possible for ESC compatibility reasons
|
||||
servoConfigureOutput();
|
||||
systemState |= SYSTEM_STATE_MOTORS_READY;
|
||||
|
||||
#ifdef BEEPER
|
||||
|
|
Loading…
Reference in New Issue