mirror of
https://github.com/betaflight/betaflight.git
synced 2025-07-19 22:35:23 +03:00
Merge pull request #1364 from martinbudden/bf_servo_split2
Split mixer and servo code
This commit is contained in:
borisbstyle
GitHub
commit
b5a70451ec
12 changed files with 654 additions and 598 deletions
1
Makefile
1
Makefile
|
|
@ -508,6 +508,7 @@ COMMON_SRC = \
|
||||||
flight/imu.c \
|
flight/imu.c \
|
||||||
flight/mixer.c \
|
flight/mixer.c \
|
||||||
flight/pid.c \
|
flight/pid.c \
|
||||||
|
flight/servos.c \
|
||||||
io/beeper.c \
|
io/beeper.c \
|
||||||
io/serial.c \
|
io/serial.c \
|
||||||
io/serial_4way.c \
|
io/serial_4way.c \
|
||||||
|
|
|
||||||
|
|
@ -29,6 +29,7 @@
|
||||||
|
|
||||||
#include "flight/failsafe.h"
|
#include "flight/failsafe.h"
|
||||||
#include "flight/mixer.h"
|
#include "flight/mixer.h"
|
||||||
|
#include "flight/servos.h"
|
||||||
#include "flight/imu.h"
|
#include "flight/imu.h"
|
||||||
#include "flight/navigation.h"
|
#include "flight/navigation.h"
|
||||||
|
|
||||||
|
|
@ -65,10 +66,11 @@ typedef struct master_s {
|
||||||
// motor/esc/servo related stuff
|
// motor/esc/servo related stuff
|
||||||
motorMixer_t customMotorMixer[MAX_SUPPORTED_MOTORS];
|
motorMixer_t customMotorMixer[MAX_SUPPORTED_MOTORS];
|
||||||
motorConfig_t motorConfig;
|
motorConfig_t motorConfig;
|
||||||
servoConfig_t servoConfig;
|
|
||||||
flight3DConfig_t flight3DConfig;
|
flight3DConfig_t flight3DConfig;
|
||||||
|
|
||||||
#ifdef USE_SERVOS
|
#ifdef USE_SERVOS
|
||||||
|
servoConfig_t servoConfig;
|
||||||
|
servoMixerConfig_t servoMixerConfig;
|
||||||
servoMixer_t customServoMixer[MAX_SERVO_RULES];
|
servoMixer_t customServoMixer[MAX_SERVO_RULES];
|
||||||
// Servo-related stuff
|
// Servo-related stuff
|
||||||
servoParam_t servoConf[MAX_SUPPORTED_SERVOS]; // servo configuration
|
servoParam_t servoConf[MAX_SUPPORTED_SERVOS]; // servo configuration
|
||||||
|
|
|
||||||
|
|
@ -72,6 +72,7 @@
|
||||||
#include "telemetry/telemetry.h"
|
#include "telemetry/telemetry.h"
|
||||||
|
|
||||||
#include "flight/mixer.h"
|
#include "flight/mixer.h"
|
||||||
|
#include "flight/servos.h"
|
||||||
#include "flight/pid.h"
|
#include "flight/pid.h"
|
||||||
#include "flight/imu.h"
|
#include "flight/imu.h"
|
||||||
#include "flight/failsafe.h"
|
#include "flight/failsafe.h"
|
||||||
|
|
@ -407,13 +408,17 @@ void resetRcControlsConfig(rcControlsConfig_t *rcControlsConfig)
|
||||||
void resetMixerConfig(mixerConfig_t *mixerConfig)
|
void resetMixerConfig(mixerConfig_t *mixerConfig)
|
||||||
{
|
{
|
||||||
mixerConfig->yaw_motor_direction = 1;
|
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)
|
uint8_t getCurrentProfile(void)
|
||||||
{
|
{
|
||||||
return masterConfig.current_profile_index;
|
return masterConfig.current_profile_index;
|
||||||
|
|
@ -573,13 +578,13 @@ void createDefaultConfig(master_t *config)
|
||||||
config->auto_disarm_delay = 5;
|
config->auto_disarm_delay = 5;
|
||||||
config->small_angle = 25;
|
config->small_angle = 25;
|
||||||
|
|
||||||
resetMixerConfig(&config->mixerConfig);
|
|
||||||
|
|
||||||
config->airplaneConfig.fixedwing_althold_dir = 1;
|
config->airplaneConfig.fixedwing_althold_dir = 1;
|
||||||
|
|
||||||
// Motor/ESC/Servo
|
// Motor/ESC/Servo
|
||||||
|
resetMixerConfig(&config->mixerConfig);
|
||||||
resetMotorConfig(&config->motorConfig);
|
resetMotorConfig(&config->motorConfig);
|
||||||
#ifdef USE_SERVOS
|
#ifdef USE_SERVOS
|
||||||
|
resetServoMixerConfig(&config->servoMixerConfig);
|
||||||
resetServoConfig(&config->servoConfig);
|
resetServoConfig(&config->servoConfig);
|
||||||
#endif
|
#endif
|
||||||
resetFlight3DConfig(&config->flight3DConfig);
|
resetFlight3DConfig(&config->flight3DConfig);
|
||||||
|
|
@ -774,7 +779,7 @@ void activateConfig(void)
|
||||||
);
|
);
|
||||||
|
|
||||||
#ifdef USE_SERVOS
|
#ifdef USE_SERVOS
|
||||||
servoUseConfigs(masterConfig.servoConf, &masterConfig.gimbalConfig);
|
servoUseConfigs(&masterConfig.servoMixerConfig, masterConfig.servoConf, &masterConfig.gimbalConfig);
|
||||||
#endif
|
#endif
|
||||||
|
|
||||||
imuRuntimeConfig.dcm_kp = masterConfig.dcm_kp / 10000.0f;
|
imuRuntimeConfig.dcm_kp = masterConfig.dcm_kp / 10000.0f;
|
||||||
|
|
|
||||||
|
|
@ -65,6 +65,7 @@
|
||||||
#include "scheduler/scheduler.h"
|
#include "scheduler/scheduler.h"
|
||||||
|
|
||||||
#include "flight/mixer.h"
|
#include "flight/mixer.h"
|
||||||
|
#include "flight/servos.h"
|
||||||
#include "flight/pid.h"
|
#include "flight/pid.h"
|
||||||
#include "flight/failsafe.h"
|
#include "flight/failsafe.h"
|
||||||
#include "flight/gtune.h"
|
#include "flight/gtune.h"
|
||||||
|
|
@ -740,7 +741,7 @@ void subTaskMainSubprocesses(void)
|
||||||
if (isUsingSticksForArming() && rcData[THROTTLE] <= masterConfig.rxConfig.mincheck
|
if (isUsingSticksForArming() && rcData[THROTTLE] <= masterConfig.rxConfig.mincheck
|
||||||
#ifndef USE_QUAD_MIXER_ONLY
|
#ifndef USE_QUAD_MIXER_ONLY
|
||||||
#ifdef USE_SERVOS
|
#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
|
#endif
|
||||||
&& masterConfig.mixerMode != MIXER_AIRPLANE
|
&& masterConfig.mixerMode != MIXER_AIRPLANE
|
||||||
&& masterConfig.mixerMode != MIXER_FLYING_WING
|
&& masterConfig.mixerMode != MIXER_FLYING_WING
|
||||||
|
|
@ -794,6 +795,8 @@ void subTaskMotorUpdate(void)
|
||||||
mixTable(¤tProfile->pidProfile);
|
mixTable(¤tProfile->pidProfile);
|
||||||
|
|
||||||
#ifdef USE_SERVOS
|
#ifdef USE_SERVOS
|
||||||
|
// motor outputs are used as sources for servo mixing, so motors must be calculated using mixTable() before servos.
|
||||||
|
servoTable();
|
||||||
filterServos();
|
filterServos();
|
||||||
writeServos();
|
writeServos();
|
||||||
#endif
|
#endif
|
||||||
|
|
|
||||||
|
|
@ -17,14 +17,12 @@
|
||||||
|
|
||||||
#include <stdbool.h>
|
#include <stdbool.h>
|
||||||
#include <stdint.h>
|
#include <stdint.h>
|
||||||
#include <stdlib.h>
|
|
||||||
#include <string.h>
|
#include <string.h>
|
||||||
#include <math.h>
|
#include <math.h>
|
||||||
|
|
||||||
#include "platform.h"
|
#include "platform.h"
|
||||||
|
|
||||||
#include "build/build_config.h"
|
#include "build/build_config.h"
|
||||||
#include "build/debug.h"
|
|
||||||
|
|
||||||
#include "common/axis.h"
|
#include "common/axis.h"
|
||||||
#include "common/maths.h"
|
#include "common/maths.h"
|
||||||
|
|
@ -32,18 +30,11 @@
|
||||||
|
|
||||||
#include "drivers/system.h"
|
#include "drivers/system.h"
|
||||||
#include "drivers/pwm_output.h"
|
#include "drivers/pwm_output.h"
|
||||||
#include "drivers/sensor.h"
|
|
||||||
#include "drivers/accgyro.h"
|
#include "io/motors.h"
|
||||||
#include "drivers/system.h"
|
|
||||||
|
|
||||||
#include "rx/rx.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 "sensors/battery.h"
|
||||||
|
|
||||||
#include "flight/mixer.h"
|
#include "flight/mixer.h"
|
||||||
|
|
@ -66,20 +57,12 @@ static mixerConfig_t *mixerConfig;
|
||||||
static flight3DConfig_t *flight3DConfig;
|
static flight3DConfig_t *flight3DConfig;
|
||||||
static motorConfig_t *motorConfig;
|
static motorConfig_t *motorConfig;
|
||||||
static airplaneConfig_t *airplaneConfig;
|
static airplaneConfig_t *airplaneConfig;
|
||||||
static rxConfig_t *rxConfig;
|
rxConfig_t *rxConfig;
|
||||||
static bool syncMotorOutputWithPidLoop = false;
|
static bool syncMotorOutputWithPidLoop = false;
|
||||||
|
|
||||||
static mixerMode_e currentMixerMode;
|
mixerMode_e currentMixerMode;
|
||||||
static motorMixer_t currentMixer[MAX_SUPPORTED_MOTORS];
|
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[] = {
|
static const motorMixer_t mixerQuadX[] = {
|
||||||
{ 1.0f, -1.0f, 1.0f, -1.0f }, // REAR_R
|
{ 1.0f, -1.0f, 1.0f, -1.0f }, // REAR_R
|
||||||
|
|
@ -245,92 +228,6 @@ const mixer_t mixers[] = {
|
||||||
};
|
};
|
||||||
#endif
|
#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 motorMixer_t *customMixers;
|
||||||
|
|
||||||
static uint16_t disarmMotorOutput, minMotorOutputNormal, maxMotorOutputNormal, deadbandMotor3dHigh, deadbandMotor3dLow;
|
static uint16_t disarmMotorOutput, minMotorOutputNormal, maxMotorOutputNormal, deadbandMotor3dHigh, deadbandMotor3dLow;
|
||||||
|
|
@ -377,51 +274,6 @@ void mixerUseConfigs(
|
||||||
initEscEndpoints();
|
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)
|
void mixerInit(mixerMode_e mixerMode, motorMixer_t *initialCustomMixers)
|
||||||
{
|
{
|
||||||
currentMixerMode = mixerMode;
|
currentMixerMode = mixerMode;
|
||||||
|
|
@ -431,23 +283,6 @@ void mixerInit(mixerMode_e mixerMode, motorMixer_t *initialCustomMixers)
|
||||||
|
|
||||||
#ifndef USE_QUAD_MIXER_ONLY
|
#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)
|
void mixerConfigureOutput(void)
|
||||||
{
|
{
|
||||||
int i;
|
int i;
|
||||||
|
|
@ -474,14 +309,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
|
// in 3D mode, mixer gain has to be halved
|
||||||
if (feature(FEATURE_3D)) {
|
if (feature(FEATURE_3D)) {
|
||||||
if (motorCount > 1) {
|
if (motorCount > 1) {
|
||||||
|
|
@ -493,42 +320,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();
|
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)
|
void mixerLoadMix(int index, motorMixer_t *customMixers)
|
||||||
{
|
{
|
||||||
int i;
|
int i;
|
||||||
|
|
@ -568,90 +362,6 @@ void mixerResetDisarmedMotors(void)
|
||||||
motor_disarmed[i] = feature(FEATURE_3D) ? flight3DConfig->neutral3d : motorConfig->mincommand;
|
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)
|
void writeMotors(void)
|
||||||
{
|
{
|
||||||
for (uint8_t i = 0; i < motorCount; i++) {
|
for (uint8_t i = 0; i < motorCount; i++) {
|
||||||
|
|
@ -686,91 +396,10 @@ void stopPwmAllMotors(void)
|
||||||
delayMicroseconds(1500);
|
delayMicroseconds(1500);
|
||||||
}
|
}
|
||||||
|
|
||||||
#ifndef USE_QUAD_MIXER_ONLY
|
void mixTable(pidProfile_t *pidProfile)
|
||||||
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)
|
|
||||||
{
|
{
|
||||||
uint32_t i = 0;
|
uint32_t i = 0;
|
||||||
float vbatCompensationFactor = 1;
|
float vbatCompensationFactor = 1;
|
||||||
pidProfile_t *pidProfile = (pidProfile_t *) pidProfilePtr;
|
|
||||||
|
|
||||||
// Scale roll/pitch/yaw uniformly to fit within throttle range
|
// Scale roll/pitch/yaw uniformly to fit within throttle range
|
||||||
// Initial mixer concept by bdoiron74 reused and optimized for Air Mode
|
// Initial mixer concept by bdoiron74 reused and optimized for Air Mode
|
||||||
|
|
@ -892,94 +521,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
|
#pragma once
|
||||||
|
|
||||||
#define MAX_SUPPORTED_MOTORS 12
|
#define MAX_SUPPORTED_MOTORS 12
|
||||||
#define MAX_SUPPORTED_SERVOS 8
|
|
||||||
|
|
||||||
#define QUAD_MOTOR_COUNT 4
|
#define QUAD_MOTOR_COUNT 4
|
||||||
|
|
||||||
|
|
@ -77,11 +76,6 @@ typedef struct mixer_s {
|
||||||
|
|
||||||
typedef struct mixerConfig_s {
|
typedef struct mixerConfig_s {
|
||||||
int8_t yaw_motor_direction;
|
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;
|
} mixerConfig_t;
|
||||||
|
|
||||||
typedef struct flight3DConfig_s {
|
typedef struct flight3DConfig_s {
|
||||||
|
|
@ -97,105 +91,6 @@ typedef struct airplaneConfig_s {
|
||||||
|
|
||||||
#define CHANNEL_FORWARDING_DISABLED (uint8_t)0xFF
|
#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 const mixer_t mixers[];
|
||||||
extern int16_t motor[MAX_SUPPORTED_MOTORS];
|
extern int16_t motor[MAX_SUPPORTED_MOTORS];
|
||||||
extern int16_t motor_disarmed[MAX_SUPPORTED_MOTORS];
|
extern int16_t motor_disarmed[MAX_SUPPORTED_MOTORS];
|
||||||
|
|
@ -211,21 +106,13 @@ void mixerUseConfigs(
|
||||||
|
|
||||||
void writeAllMotors(int16_t mc);
|
void writeAllMotors(int16_t mc);
|
||||||
void mixerLoadMix(int index, motorMixer_t *customMixers);
|
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 mixerInit(mixerMode_e mixerMode, motorMixer_t *customMotorMixers);
|
||||||
|
|
||||||
void mixerConfigureOutput(void);
|
void mixerConfigureOutput(void);
|
||||||
|
|
||||||
void mixerResetDisarmedMotors(void);
|
void mixerResetDisarmedMotors(void);
|
||||||
void mixTable(void *pidProfilePtr);
|
struct pidProfile_s;
|
||||||
|
void mixTable(struct pidProfile_s *pidProfile);
|
||||||
void syncMotors(bool enabled);
|
void syncMotors(bool enabled);
|
||||||
void writeMotors(void);
|
void writeMotors(void);
|
||||||
void stopMotors(void);
|
void stopMotors(void);
|
||||||
|
|
|
||||||
490
src/main/flight/servos.c
Executable file
490
src/main/flight/servos.c
Executable file
|
|
@ -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
|
||||||
129
src/main/flight/servos.h
Normal file
129
src/main/flight/servos.h
Normal file
|
|
@ -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
|
#pragma once
|
||||||
|
|
||||||
#include "drivers/io.h"
|
#include "drivers/io_types.h"
|
||||||
#include "flight/mixer.h"
|
#include "flight/mixer.h"
|
||||||
|
|
||||||
typedef struct motorConfig_s {
|
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 } },
|
{ "pidsum_limit", VAR_FLOAT | PROFILE_VALUE, &masterConfig.profile[0].pidProfile.pidSumLimit, .config.minmax = { 0.1, 1.0 } },
|
||||||
#ifdef USE_SERVOS
|
#ifdef USE_SERVOS
|
||||||
{ "servo_center_pulse", VAR_UINT16 | MASTER_VALUE, &masterConfig.servoConfig.servoCenterPulse, .config.minmax = { PWM_RANGE_ZERO, PWM_RANGE_MAX } },
|
{ "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 } },
|
{ "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.mixerConfig.servo_lowpass_freq, .config.minmax = { 10, 400} },
|
{ "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.mixerConfig.servo_lowpass_enable, .config.lookup = { TABLE_OFF_ON } },
|
{ "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 } },
|
{ "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 } },
|
{ "gimbal_mode", VAR_UINT8 | MASTER_VALUE | MODE_LOOKUP, &masterConfig.gimbalConfig.mode, .config.lookup = { TABLE_GIMBAL_MODE } },
|
||||||
#endif
|
#endif
|
||||||
|
|
|
||||||
|
|
@ -17,8 +17,8 @@
|
||||||
|
|
||||||
#pragma once
|
#pragma once
|
||||||
|
|
||||||
#include "drivers/io.h"
|
#include "drivers/io_types.h"
|
||||||
#include "flight/mixer.h"
|
#include "flight/servos.h"
|
||||||
|
|
||||||
typedef struct servoConfig_s {
|
typedef struct servoConfig_s {
|
||||||
// PWM values, in milliseconds, common range is 1000-2000 (1 to 2ms)
|
// PWM values, in milliseconds, common range is 1000-2000 (1 to 2ms)
|
||||||
|
|
|
||||||
|
|
@ -288,7 +288,7 @@ void init(void)
|
||||||
#endif
|
#endif
|
||||||
|
|
||||||
mixerConfigureOutput();
|
mixerConfigureOutput();
|
||||||
// pwmInit() needs to be called as soon as possible for ESC compatibility reasons
|
servoConfigureOutput();
|
||||||
systemState |= SYSTEM_STATE_MOTORS_READY;
|
systemState |= SYSTEM_STATE_MOTORS_READY;
|
||||||
|
|
||||||
#ifdef BEEPER
|
#ifdef BEEPER
|
||||||
|
|
|
||||||
Loading…
Add table
Add a link
Reference in a new issue