mirror of
https://github.com/iNavFlight/inav.git
synced 2025-07-21 15:25:29 +03:00
Split servo code into servos.c
This commit is contained in:
Martin Budden
parent
9ff90aba16
commit
ddb60edfdd
17 changed files with 638 additions and 550 deletions
1
Makefile
1
Makefile
|
|
@ -403,6 +403,7 @@ COMMON_SRC = \
|
|||
flight/imu.c \
|
||||
flight/hil.c \
|
||||
flight/mixer.c \
|
||||
flight/servos.c \
|
||||
flight/pid.c \
|
||||
io/beeper.c \
|
||||
fc/rc_controls.c \
|
||||
|
|
|
|||
|
|
@ -70,6 +70,7 @@
|
|||
#include "telemetry/telemetry.h"
|
||||
|
||||
#include "flight/mixer.h"
|
||||
#include "flight/servos.h"
|
||||
#include "flight/failsafe.h"
|
||||
#include "flight/imu.h"
|
||||
#include "flight/navigation_rewrite.h"
|
||||
|
|
|
|||
|
|
@ -67,6 +67,7 @@
|
|||
#include "common/printf.h"
|
||||
|
||||
#include "flight/mixer.h"
|
||||
#include "flight/servos.h"
|
||||
#include "flight/failsafe.h"
|
||||
#include "flight/imu.h"
|
||||
#include "flight/navigation_rewrite.h"
|
||||
|
|
|
|||
|
|
@ -62,6 +62,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"
|
||||
|
|
@ -347,16 +348,21 @@ void resetRcControlsConfig(rcControlsConfig_t *rcControlsConfig) {
|
|||
rcControlsConfig->alt_hold_deadband = 50;
|
||||
}
|
||||
|
||||
void resetMixerConfig(mixerConfig_t *mixerConfig) {
|
||||
static void resetMixerConfig(mixerConfig_t *mixerConfig)
|
||||
{
|
||||
mixerConfig->yaw_motor_direction = 1;
|
||||
mixerConfig->yaw_jump_prevention_limit = 200;
|
||||
#ifdef USE_SERVOS
|
||||
mixerConfig->tri_unarmed_servo = 1;
|
||||
mixerConfig->servo_lowpass_freq = 400;
|
||||
mixerConfig->servo_lowpass_enable = 0;
|
||||
#endif
|
||||
}
|
||||
|
||||
#ifdef USE_SERVOS
|
||||
static void resetServoConfig(servoConfig_t *servoConfig)
|
||||
{
|
||||
servoConfig->tri_unarmed_servo = 1;
|
||||
servoConfig->servo_lowpass_freq = 400;
|
||||
servoConfig->servo_lowpass_enable = 0;
|
||||
}
|
||||
#endif
|
||||
|
||||
uint8_t getCurrentProfile(void)
|
||||
{
|
||||
return masterConfig.current_profile_index;
|
||||
|
|
@ -470,6 +476,9 @@ static void resetConf(void)
|
|||
masterConfig.small_angle = 25;
|
||||
|
||||
resetMixerConfig(&masterConfig.mixerConfig);
|
||||
#ifdef USE_SERVOS
|
||||
resetServoConfig(&masterConfig.servoConfig);
|
||||
#endif
|
||||
|
||||
// Motor/ESC/Servo
|
||||
resetEscAndServoConfig(&masterConfig.escAndServoConfig);
|
||||
|
|
@ -716,7 +725,7 @@ void activateConfig(void)
|
|||
|
||||
mixerUseConfigs(&masterConfig.flight3DConfig, &masterConfig.escAndServoConfig, &masterConfig.mixerConfig, &masterConfig.rxConfig);
|
||||
#ifdef USE_SERVOS
|
||||
servosUseConfigs(currentProfile->servoConf, ¤tProfile->gimbalConfig);
|
||||
servosUseConfigs(&masterConfig.servoConfig, currentProfile->servoConf, ¤tProfile->gimbalConfig);
|
||||
#endif
|
||||
|
||||
imuRuntimeConfig.dcm_kp_acc = masterConfig.dcm_kp_acc / 10000.0f;
|
||||
|
|
|
|||
|
|
@ -80,6 +80,7 @@ typedef struct master_s {
|
|||
|
||||
// mixer-related configuration
|
||||
mixerConfig_t mixerConfig;
|
||||
servoConfig_t servoConfig;
|
||||
|
||||
#ifdef GPS
|
||||
gpsConfig_t gpsConfig;
|
||||
|
|
|
|||
|
|
@ -60,6 +60,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"
|
||||
|
|
|
|||
|
|
@ -75,6 +75,7 @@
|
|||
#include "sensors/gyro.h"
|
||||
|
||||
#include "flight/mixer.h"
|
||||
#include "flight/servos.h"
|
||||
#include "flight/pid.h"
|
||||
#include "flight/imu.h"
|
||||
#include "flight/hil.h"
|
||||
|
|
|
|||
|
|
@ -77,6 +77,7 @@
|
|||
#include "blackbox/blackbox.h"
|
||||
|
||||
#include "flight/mixer.h"
|
||||
#include "flight/servos.h"
|
||||
#include "flight/pid.h"
|
||||
#include "flight/imu.h"
|
||||
#include "flight/hil.h"
|
||||
|
|
@ -565,7 +566,7 @@ void taskMainPidLoop(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.servoConfig.tri_unarmed_servo)
|
||||
#endif
|
||||
&& masterConfig.mixerMode != MIXER_AIRPLANE
|
||||
&& masterConfig.mixerMode != MIXER_FLYING_WING
|
||||
|
|
|
|||
|
|
@ -41,7 +41,6 @@
|
|||
#include "rx/rx.h"
|
||||
|
||||
#include "io/gimbal.h"
|
||||
#include "io/escservo.h"
|
||||
#include "fc/rc_controls.h"
|
||||
|
||||
|
||||
|
|
@ -50,6 +49,7 @@
|
|||
#include "sensors/gyro.h"
|
||||
|
||||
#include "flight/mixer.h"
|
||||
#include "flight/servos.h"
|
||||
#include "flight/failsafe.h"
|
||||
#include "flight/pid.h"
|
||||
#include "flight/imu.h"
|
||||
|
|
@ -70,31 +70,15 @@ int16_t motor_disarmed[MAX_SUPPORTED_MOTORS];
|
|||
|
||||
bool motorLimitReached = false;
|
||||
|
||||
static mixerConfig_t *mixerConfig;
|
||||
mixerConfig_t *mixerConfig;
|
||||
static flight3DConfig_t *flight3DConfig;
|
||||
static escAndServoConfig_t *escAndServoConfig;
|
||||
static rxConfig_t *rxConfig;
|
||||
rxConfig_t *rxConfig;
|
||||
|
||||
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 servoOutputEnabled;
|
||||
|
||||
static uint8_t mixerUsesServos;
|
||||
static uint8_t minServoIndex;
|
||||
static uint8_t maxServoIndex;
|
||||
|
||||
static servoParam_t *servoConf;
|
||||
static biquadFilter_t servoFitlerState[MAX_SUPPORTED_SERVOS];
|
||||
static bool servoFilterIsSet;
|
||||
#endif
|
||||
|
||||
static const motorMixer_t mixerQuadX[] = {
|
||||
{ 1.0f, -1.0f, 1.0f, -1.0f }, // REAR_R
|
||||
{ 1.0f, -1.0f, -1.0f, 1.0f }, // FRONT_R
|
||||
|
|
@ -282,63 +266,6 @@ const mixer_t mixers[] = {
|
|||
};
|
||||
#endif // USE_QUAD_MIXER_ONLY
|
||||
|
||||
#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 servoMixerTri[] = {
|
||||
{ SERVO_RUDDER, INPUT_STABILIZED_YAW, 100, 0, 0, 100, 0 },
|
||||
};
|
||||
|
||||
const mixerRules_t servoMixers[] = {
|
||||
{ 0, 0, 0, NULL }, // entry 0
|
||||
{ COUNT_SERVO_RULES(servoMixerTri), SERVO_RUDDER, SERVO_RUDDER, servoMixerTri }, // MULTITYPE_TRI
|
||||
{ 0, 0, 0, NULL }, // MULTITYPE_QUADP
|
||||
{ 0, 0, 0, NULL }, // MULTITYPE_QUADX
|
||||
{ 0, 0, 0, NULL }, // MULTITYPE_BI
|
||||
{ 0, 0, 0, NULL }, // MULTITYPE_GIMBAL / MIXER_GIMBAL -> disabled
|
||||
{ 0, 0, 0, NULL }, // MULTITYPE_Y6
|
||||
{ 0, 0, 0, NULL }, // MULTITYPE_HEX6
|
||||
{ COUNT_SERVO_RULES(servoMixerFlyingWing), SERVO_FLAPPERONS_MIN, SERVO_FLAPPERONS_MAX, servoMixerFlyingWing },// * MULTITYPE_FLYING_WING
|
||||
{ 0, 0, 0, NULL }, // MULTITYPE_Y4
|
||||
{ 0, 0, 0, NULL }, // MULTITYPE_HEX6X
|
||||
{ 0, 0, 0, NULL }, // MULTITYPE_OCTOX8
|
||||
{ 0, 0, 0, NULL }, // MULTITYPE_OCTOFLATP
|
||||
{ 0, 0, 0, NULL }, // MULTITYPE_OCTOFLATX
|
||||
{ COUNT_SERVO_RULES(servoMixerAirplane), SERVO_PLANE_INDEX_MIN, SERVO_PLANE_INDEX_MAX, servoMixerAirplane }, // * MULTITYPE_AIRPLANE
|
||||
{ 0, 0, 0, NULL }, // * MIXER_HELI_120_CCPM -> disabled, never fully implemented in CF
|
||||
{ 0, 0, 0, NULL }, // * MIXER_HELI_90_DEG -> disabled, never fully implemented in CF
|
||||
{ 0, 0, 0, NULL }, // MULTITYPE_VTAIL4
|
||||
{ 0, 0, 0, NULL }, // MULTITYPE_HEX6H
|
||||
{ 0, 0, 0, NULL }, // * MULTITYPE_PPM_TO_SERVO
|
||||
{ 0, 0, 0, NULL }, // MULTITYPE_DUALCOPTER
|
||||
{ 0, 0, 0, NULL }, // MULTITYPE_SINGLECOPTER
|
||||
{ 0, 0, 0, NULL }, // MULTITYPE_ATAIL4
|
||||
{ 0, 2, 5, NULL }, // MULTITYPE_CUSTOM
|
||||
{ 0, SERVO_PLANE_INDEX_MIN, SERVO_PLANE_INDEX_MAX, NULL }, // MULTITYPE_CUSTOM_PLANE
|
||||
{ 0, SERVO_RUDDER, SERVO_RUDDER, NULL }, // MULTITYPE_CUSTOM_TRI
|
||||
};
|
||||
|
||||
static servoMixer_t *customServoMixers;
|
||||
#endif
|
||||
|
||||
|
||||
static motorMixer_t *customMixers;
|
||||
|
||||
void mixerUseConfigs(
|
||||
|
|
@ -353,43 +280,6 @@ void mixerUseConfigs(
|
|||
rxConfig = rxConfigToUse;
|
||||
}
|
||||
|
||||
#ifdef USE_SERVOS
|
||||
void servosUseConfigs(servoParam_t *servoConfToUse, gimbalConfig_t *gimbalConfigToUse)
|
||||
{
|
||||
servoConf = servoConfToUse;
|
||||
gimbalConfig = gimbalConfigToUse;
|
||||
}
|
||||
|
||||
int16_t getFlaperonDirection(uint8_t servoPin) {
|
||||
if (servoPin == SERVO_FLAPPERON_2) {
|
||||
return -1;
|
||||
} else {
|
||||
return 1;
|
||||
}
|
||||
}
|
||||
|
||||
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;
|
||||
}
|
||||
#endif
|
||||
|
||||
bool isMixerEnabled(mixerMode_e mixerMode)
|
||||
{
|
||||
#ifdef USE_QUAD_MIXER_ONLY
|
||||
|
|
@ -400,74 +290,6 @@ bool isMixerEnabled(mixerMode_e mixerMode)
|
|||
#endif
|
||||
}
|
||||
|
||||
#ifdef USE_SERVOS
|
||||
void servosInit(servoMixer_t *initialCustomServoMixers)
|
||||
{
|
||||
int 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);
|
||||
} else {
|
||||
DISABLE_STATE(FIXED_WING);
|
||||
}
|
||||
|
||||
if (currentMixerMode == MIXER_AIRPLANE || currentMixerMode == MIXER_CUSTOM_AIRPLANE) {
|
||||
ENABLE_STATE(FLAPERON_AVAILABLE);
|
||||
} else {
|
||||
DISABLE_STATE(FLAPERON_AVAILABLE);
|
||||
}
|
||||
|
||||
customServoMixers = initialCustomServoMixers;
|
||||
|
||||
minServoIndex = servoMixers[currentMixerMode].minServoIndex;
|
||||
maxServoIndex = servoMixers[currentMixerMode].maxServoIndex;
|
||||
|
||||
// enable servos for mixes that require them. note, this shifts motor counts.
|
||||
mixerUsesServos = mixers[currentMixerMode].useServo || feature(FEATURE_SERVO_TILT);
|
||||
|
||||
// if we want camstab/trig, that also enables servos, even if mixer doesn't
|
||||
servoOutputEnabled = mixerUsesServos || feature(FEATURE_CHANNEL_FORWARDING);
|
||||
|
||||
// give all servos a default command
|
||||
for (i = 0; i < MAX_SUPPORTED_SERVOS; i++) {
|
||||
servo[i] = DEFAULT_SERVO_MIDDLE;
|
||||
}
|
||||
|
||||
/*
|
||||
* If mixer has predefined servo mixes, load them
|
||||
*/
|
||||
if (mixerUsesServos) {
|
||||
servoRuleCount = servoMixers[currentMixerMode].servoRuleCount;
|
||||
if (servoMixers[currentMixerMode].rule) {
|
||||
for (i = 0; i < servoRuleCount; i++)
|
||||
currentServoMixer[i] = servoMixers[currentMixerMode].rule[i];
|
||||
}
|
||||
}
|
||||
|
||||
/*
|
||||
* When we are dealing with CUSTOM mixers, load mixes defined with
|
||||
* smix and update internal variables
|
||||
*/
|
||||
if (currentMixerMode == MIXER_CUSTOM_AIRPLANE ||
|
||||
currentMixerMode == MIXER_CUSTOM_TRI ||
|
||||
currentMixerMode == MIXER_CUSTOM)
|
||||
{
|
||||
loadCustomServoMixer();
|
||||
|
||||
// If there are servo rules after all, update variables
|
||||
if (servoRuleCount > 0) {
|
||||
servoOutputEnabled = 1;
|
||||
mixerUsesServos = 1;
|
||||
}
|
||||
|
||||
}
|
||||
}
|
||||
#endif
|
||||
|
||||
// mixerInit must be called before servosInit
|
||||
void mixerInit(mixerMode_e mixerMode, motorMixer_t *initialCustomMixers)
|
||||
{
|
||||
|
|
@ -527,53 +349,6 @@ void mixerUsePWMIOConfiguration(void)
|
|||
|
||||
|
||||
#ifndef USE_QUAD_MIXER_ONLY
|
||||
#ifdef USE_SERVOS
|
||||
void loadCustomServoMixer(void)
|
||||
{
|
||||
int i;
|
||||
|
||||
// reset settings
|
||||
servoRuleCount = 0;
|
||||
minServoIndex = 255;
|
||||
maxServoIndex = 0;
|
||||
memset(currentServoMixer, 0, sizeof(currentServoMixer));
|
||||
|
||||
// load custom mixer into currentServoMixer
|
||||
for (i = 0; i < MAX_SERVO_RULES; i++) {
|
||||
// check if done
|
||||
if (customServoMixers[i].rate == 0)
|
||||
break;
|
||||
|
||||
if (customServoMixers[i].targetChannel < minServoIndex) {
|
||||
minServoIndex = customServoMixers[i].targetChannel;
|
||||
}
|
||||
|
||||
if (customServoMixers[i].targetChannel > maxServoIndex) {
|
||||
maxServoIndex = customServoMixers[i].targetChannel;
|
||||
}
|
||||
|
||||
currentServoMixer[i] = customServoMixers[i];
|
||||
servoRuleCount++;
|
||||
}
|
||||
}
|
||||
|
||||
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];
|
||||
}
|
||||
}
|
||||
#endif
|
||||
|
||||
|
||||
void mixerLoadMix(int index, motorMixer_t *customMixers)
|
||||
{
|
||||
int i;
|
||||
|
|
@ -601,57 +376,6 @@ void mixerResetDisarmedMotors(void)
|
|||
motor_disarmed[i] = feature(FEATURE_3D) ? flight3DConfig->neutral3d : escAndServoConfig->mincommand;
|
||||
}
|
||||
|
||||
#ifdef USE_SERVOS
|
||||
|
||||
STATIC_UNIT_TESTED void forwardAuxChannelsToServos(uint8_t firstServoIndex)
|
||||
{
|
||||
// start forwarding from this channel
|
||||
uint8_t channelOffset = AUX1;
|
||||
|
||||
int servoOffset;
|
||||
for (servoOffset = 0; servoOffset < MAX_AUX_CHANNEL_COUNT && channelOffset < MAX_SUPPORTED_RC_CHANNEL_COUNT; servoOffset++) {
|
||||
pwmWriteServo(firstServoIndex + servoOffset, rcData[channelOffset++]);
|
||||
}
|
||||
}
|
||||
|
||||
void writeServos(void)
|
||||
{
|
||||
int servoIndex = 0;
|
||||
|
||||
bool zeroServoValue = false;
|
||||
|
||||
/*
|
||||
* in case of tricopters, there might me a need to zero servo output when unarmed
|
||||
*/
|
||||
if ((currentMixerMode == MIXER_TRI || currentMixerMode == MIXER_CUSTOM_TRI) && !ARMING_FLAG(ARMED) && !mixerConfig->tri_unarmed_servo) {
|
||||
zeroServoValue = true;
|
||||
}
|
||||
|
||||
// Write mixer servo outputs
|
||||
// mixerUsesServos might indicate SERVO_TILT, servoRuleCount indicate if mixer alone uses servos
|
||||
if (mixerUsesServos && servoRuleCount) {
|
||||
for (int i = minServoIndex; i <= maxServoIndex; i++) {
|
||||
if (zeroServoValue) {
|
||||
pwmWriteServo(servoIndex++, 0);
|
||||
} else {
|
||||
pwmWriteServo(servoIndex++, servo[i]);
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
if (feature(FEATURE_SERVO_TILT)) {
|
||||
pwmWriteServo(servoIndex++, servo[SERVO_GIMBAL_PITCH]);
|
||||
pwmWriteServo(servoIndex++, servo[SERVO_GIMBAL_ROLL]);
|
||||
}
|
||||
|
||||
// forward AUX to remaining servo outputs (not constrained)
|
||||
if (feature(FEATURE_CHANNEL_FORWARDING)) {
|
||||
forwardAuxChannelsToServos(servoIndex);
|
||||
servoIndex += MAX_AUX_CHANNEL_COUNT;
|
||||
}
|
||||
}
|
||||
#endif
|
||||
|
||||
void writeMotors(void)
|
||||
{
|
||||
int i;
|
||||
|
|
@ -796,145 +520,3 @@ void mixTable(void)
|
|||
}
|
||||
}
|
||||
|
||||
#ifdef USE_SERVOS
|
||||
|
||||
void servoMixer(uint16_t flaperon_throw_offset, uint8_t flaperon_throw_inverted)
|
||||
{
|
||||
int16_t input[INPUT_SOURCE_COUNT]; // Range [-500:+500]
|
||||
static int16_t currentOutput[MAX_SERVO_RULES];
|
||||
int 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] = axisPID[ROLL];
|
||||
input[INPUT_STABILIZED_PITCH] = axisPID[PITCH];
|
||||
input[INPUT_STABILIZED_YAW] = axisPID[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]);
|
||||
}
|
||||
}
|
||||
|
||||
/*
|
||||
Flaperon fligh mode
|
||||
*/
|
||||
if (FLIGHT_MODE(FLAPERON) && (target == SERVO_FLAPPERON_1 || target == SERVO_FLAPPERON_2)) {
|
||||
int8_t multiplier = 1;
|
||||
|
||||
if (flaperon_throw_inverted == 1) {
|
||||
multiplier = -1;
|
||||
}
|
||||
currentOutput[i] += flaperon_throw_offset * getFlaperonDirection(target) * multiplier;
|
||||
}
|
||||
|
||||
servo[target] += servoDirection(target, from) * constrain(((int32_t)currentOutput[i] * currentServoMixer[i].rate) / 100, min, max);
|
||||
} else {
|
||||
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 processServoTilt(void) {
|
||||
// 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;
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
bool isServoOutputEnabled(void)
|
||||
{
|
||||
return servoOutputEnabled;
|
||||
}
|
||||
|
||||
bool isMixerUsingServos(void) {
|
||||
return mixerUsesServos;
|
||||
}
|
||||
|
||||
void filterServos(void)
|
||||
{
|
||||
int servoIdx;
|
||||
|
||||
if (mixerConfig->servo_lowpass_enable) {
|
||||
// Initialize servo lowpass filter (servos are calculated at looptime rate)
|
||||
if (!servoFilterIsSet) {
|
||||
for (servoIdx = 0; servoIdx < MAX_SUPPORTED_SERVOS; servoIdx++) {
|
||||
biquadFilterInitLPF(&servoFitlerState[servoIdx], mixerConfig->servo_lowpass_freq, gyro.targetLooptime);
|
||||
}
|
||||
|
||||
servoFilterIsSet = true;
|
||||
}
|
||||
|
||||
for (servoIdx = 0; servoIdx < MAX_SUPPORTED_SERVOS; servoIdx++) {
|
||||
// Apply servo lowpass filter and do sanity cheching
|
||||
servo[servoIdx] = (int16_t) biquadFilterApply(&servoFitlerState[servoIdx], (float)servo[servoIdx]);
|
||||
}
|
||||
}
|
||||
|
||||
for (servoIdx = 0; servoIdx < MAX_SUPPORTED_SERVOS; servoIdx++) {
|
||||
servo[servoIdx] = constrain(servo[servoIdx], servoConf[servoIdx].min, servoConf[servoIdx].max);
|
||||
}
|
||||
}
|
||||
#endif
|
||||
|
|
|
|||
|
|
@ -90,11 +90,6 @@ typedef struct mixer_s {
|
|||
typedef struct mixerConfig_s {
|
||||
int8_t yaw_motor_direction;
|
||||
uint16_t yaw_jump_prevention_limit; // make limit configurable (original fixed value was 100)
|
||||
#ifdef USE_SERVOS
|
||||
uint8_t tri_unarmed_servo; // send tail servo correction pulses even when unarmed
|
||||
int16_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 {
|
||||
|
|
@ -106,111 +101,6 @@ typedef struct flight3DConfig_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_ELEVATOR = 2,
|
||||
SERVO_FLAPPERON_1 = 3,
|
||||
SERVO_FLAPPERON_2 = 4,
|
||||
SERVO_RUDDER = 5,
|
||||
SERVO_THROTTLE = 6, // for internal combustion (IC) planes
|
||||
SERVO_FLAPS = 7,
|
||||
|
||||
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_ELEVATOR
|
||||
#define SERVO_PLANE_INDEX_MAX SERVO_FLAPS
|
||||
|
||||
#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
|
||||
|
||||
#define FLAPERON_THROW_DEFAULT 250
|
||||
#define FLAPERON_THROW_MIN 100
|
||||
#define FLAPERON_THROW_MAX 400
|
||||
|
||||
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;
|
||||
uint8_t minServoIndex;
|
||||
uint8_t maxServoIndex;
|
||||
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 escAndServoConfig_s;
|
||||
struct rxConfig_s;
|
||||
|
||||
extern int16_t servo[MAX_SUPPORTED_SERVOS];
|
||||
bool isServoOutputEnabled(void);
|
||||
bool isMixerUsingServos(void);
|
||||
void writeServos(void);
|
||||
void filterServos(void);
|
||||
#endif
|
||||
|
||||
extern int16_t motor[MAX_SUPPORTED_MOTORS];
|
||||
extern int16_t motor_disarmed[MAX_SUPPORTED_MOTORS];
|
||||
|
|
@ -227,13 +117,6 @@ void mixerUseConfigs(
|
|||
|
||||
void writeAllMotors(int16_t mc);
|
||||
void mixerLoadMix(int index, motorMixer_t *customMixers);
|
||||
#ifdef USE_SERVOS
|
||||
void servoMixerLoadMix(int index, servoMixer_t *customServoMixers);
|
||||
void loadCustomServoMixer(void);
|
||||
int servoDirection(int servoIndex, int fromChannel);
|
||||
void servosUseConfigs(servoParam_t *servoConfToUse, struct gimbalConfig_s *gimbalConfigToUse);
|
||||
void servosInit(servoMixer_t *customServoMixers);
|
||||
#endif
|
||||
void mixerInit(mixerMode_e mixerMode, motorMixer_t *customMotorMixers);
|
||||
void mixerUsePWMIOConfiguration(void);
|
||||
void mixerResetDisarmedMotors(void);
|
||||
|
|
|
|||
470
src/main/flight/servos.c
Executable file
470
src/main/flight/servos.c
Executable file
|
|
@ -0,0 +1,470 @@
|
|||
/*
|
||||
* 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 <stdlib.h>
|
||||
#include <string.h>
|
||||
|
||||
#include "platform.h"
|
||||
|
||||
#ifdef USE_SERVOS
|
||||
|
||||
#include "build/debug.h"
|
||||
#include "build/build_config.h"
|
||||
|
||||
#include "common/axis.h"
|
||||
#include "common/maths.h"
|
||||
#include "common/filter.h"
|
||||
|
||||
#include "drivers/system.h"
|
||||
#include "drivers/pwm_output.h"
|
||||
#include "drivers/pwm_mapping.h"
|
||||
#include "drivers/sensor.h"
|
||||
#include "drivers/accgyro.h"
|
||||
#include "drivers/system.h"
|
||||
|
||||
#include "rx/rx.h"
|
||||
|
||||
#include "io/gimbal.h"
|
||||
#include "io/escservo.h"
|
||||
#include "fc/rc_controls.h"
|
||||
#include "sensors/sensors.h"
|
||||
#include "sensors/acceleration.h"
|
||||
#include "sensors/gyro.h"
|
||||
|
||||
#include "flight/mixer.h"
|
||||
#include "flight/servos.h"
|
||||
#include "flight/failsafe.h"
|
||||
#include "flight/pid.h"
|
||||
#include "flight/imu.h"
|
||||
#include "flight/navigation_rewrite.h"
|
||||
|
||||
#include "fc/runtime_config.h"
|
||||
|
||||
#include "config/config.h"
|
||||
#include "config/config_profile.h"
|
||||
#include "config/feature.h"
|
||||
|
||||
extern mixerMode_e currentMixerMode;
|
||||
extern const mixer_t mixers[];
|
||||
extern mixerConfig_t *mixerConfig;
|
||||
extern rxConfig_t *rxConfig;
|
||||
|
||||
servoConfig_t *servoConfig;
|
||||
|
||||
static uint8_t servoRuleCount = 0;
|
||||
static servoMixer_t currentServoMixer[MAX_SERVO_RULES];
|
||||
static gimbalConfig_t *gimbalConfig;
|
||||
int16_t servo[MAX_SUPPORTED_SERVOS];
|
||||
static int servoOutputEnabled;
|
||||
|
||||
static uint8_t mixerUsesServos;
|
||||
static uint8_t minServoIndex;
|
||||
static uint8_t maxServoIndex;
|
||||
|
||||
static servoParam_t *servoConf;
|
||||
static biquadFilter_t servoFitlerState[MAX_SUPPORTED_SERVOS];
|
||||
static bool servoFilterIsSet;
|
||||
|
||||
#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 servoMixerTri[] = {
|
||||
{ SERVO_RUDDER, INPUT_STABILIZED_YAW, 100, 0, 0, 100, 0 },
|
||||
};
|
||||
|
||||
const mixerRules_t servoMixers[] = {
|
||||
{ 0, 0, 0, NULL }, // entry 0
|
||||
{ COUNT_SERVO_RULES(servoMixerTri), SERVO_RUDDER, SERVO_RUDDER, servoMixerTri }, // MULTITYPE_TRI
|
||||
{ 0, 0, 0, NULL }, // MULTITYPE_QUADP
|
||||
{ 0, 0, 0, NULL }, // MULTITYPE_QUADX
|
||||
{ 0, 0, 0, NULL }, // MULTITYPE_BI
|
||||
{ 0, 0, 0, NULL }, // MULTITYPE_GIMBAL / MIXER_GIMBAL -> disabled
|
||||
{ 0, 0, 0, NULL }, // MULTITYPE_Y6
|
||||
{ 0, 0, 0, NULL }, // MULTITYPE_HEX6
|
||||
{ COUNT_SERVO_RULES(servoMixerFlyingWing), SERVO_FLAPPERONS_MIN, SERVO_FLAPPERONS_MAX, servoMixerFlyingWing },// * MULTITYPE_FLYING_WING
|
||||
{ 0, 0, 0, NULL }, // MULTITYPE_Y4
|
||||
{ 0, 0, 0, NULL }, // MULTITYPE_HEX6X
|
||||
{ 0, 0, 0, NULL }, // MULTITYPE_OCTOX8
|
||||
{ 0, 0, 0, NULL }, // MULTITYPE_OCTOFLATP
|
||||
{ 0, 0, 0, NULL }, // MULTITYPE_OCTOFLATX
|
||||
{ COUNT_SERVO_RULES(servoMixerAirplane), SERVO_PLANE_INDEX_MIN, SERVO_PLANE_INDEX_MAX, servoMixerAirplane }, // * MULTITYPE_AIRPLANE
|
||||
{ 0, 0, 0, NULL }, // * MIXER_HELI_120_CCPM -> disabled, never fully implemented in CF
|
||||
{ 0, 0, 0, NULL }, // * MIXER_HELI_90_DEG -> disabled, never fully implemented in CF
|
||||
{ 0, 0, 0, NULL }, // MULTITYPE_VTAIL4
|
||||
{ 0, 0, 0, NULL }, // MULTITYPE_HEX6H
|
||||
{ 0, 0, 0, NULL }, // * MULTITYPE_PPM_TO_SERVO
|
||||
{ 0, 0, 0, NULL }, // MULTITYPE_DUALCOPTER
|
||||
{ 0, 0, 0, NULL }, // MULTITYPE_SINGLECOPTER
|
||||
{ 0, 0, 0, NULL }, // MULTITYPE_ATAIL4
|
||||
{ 0, 2, 5, NULL }, // MULTITYPE_CUSTOM
|
||||
{ 0, SERVO_PLANE_INDEX_MIN, SERVO_PLANE_INDEX_MAX, NULL }, // MULTITYPE_CUSTOM_PLANE
|
||||
{ 0, SERVO_RUDDER, SERVO_RUDDER, NULL }, // MULTITYPE_CUSTOM_TRI
|
||||
};
|
||||
|
||||
static servoMixer_t *customServoMixers;
|
||||
|
||||
void servosUseConfigs(servoConfig_t *servoConfigToUse, servoParam_t *servoParamsToUse, gimbalConfig_t *gimbalConfigToUse)
|
||||
{
|
||||
servoConfig = servoConfigToUse;
|
||||
servoConf = servoParamsToUse;
|
||||
gimbalConfig = gimbalConfigToUse;
|
||||
}
|
||||
|
||||
int16_t getFlaperonDirection(uint8_t servoPin) {
|
||||
if (servoPin == SERVO_FLAPPERON_2) {
|
||||
return -1;
|
||||
} else {
|
||||
return 1;
|
||||
}
|
||||
}
|
||||
|
||||
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 servosInit(servoMixer_t *initialCustomServoMixers)
|
||||
{
|
||||
int 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);
|
||||
} else {
|
||||
DISABLE_STATE(FIXED_WING);
|
||||
}
|
||||
|
||||
if (currentMixerMode == MIXER_AIRPLANE || currentMixerMode == MIXER_CUSTOM_AIRPLANE) {
|
||||
ENABLE_STATE(FLAPERON_AVAILABLE);
|
||||
} else {
|
||||
DISABLE_STATE(FLAPERON_AVAILABLE);
|
||||
}
|
||||
|
||||
customServoMixers = initialCustomServoMixers;
|
||||
|
||||
minServoIndex = servoMixers[currentMixerMode].minServoIndex;
|
||||
maxServoIndex = servoMixers[currentMixerMode].maxServoIndex;
|
||||
|
||||
// enable servos for mixes that require them. note, this shifts motor counts.
|
||||
mixerUsesServos = mixers[currentMixerMode].useServo || feature(FEATURE_SERVO_TILT);
|
||||
|
||||
// if we want camstab/trig, that also enables servos, even if mixer doesn't
|
||||
servoOutputEnabled = mixerUsesServos || feature(FEATURE_CHANNEL_FORWARDING);
|
||||
|
||||
// give all servos a default command
|
||||
for (i = 0; i < MAX_SUPPORTED_SERVOS; i++) {
|
||||
servo[i] = DEFAULT_SERVO_MIDDLE;
|
||||
}
|
||||
|
||||
/*
|
||||
* If mixer has predefined servo mixes, load them
|
||||
*/
|
||||
if (mixerUsesServos) {
|
||||
servoRuleCount = servoMixers[currentMixerMode].servoRuleCount;
|
||||
if (servoMixers[currentMixerMode].rule) {
|
||||
for (i = 0; i < servoRuleCount; i++)
|
||||
currentServoMixer[i] = servoMixers[currentMixerMode].rule[i];
|
||||
}
|
||||
}
|
||||
|
||||
/*
|
||||
* When we are dealing with CUSTOM mixers, load mixes defined with
|
||||
* smix and update internal variables
|
||||
*/
|
||||
if (currentMixerMode == MIXER_CUSTOM_AIRPLANE ||
|
||||
currentMixerMode == MIXER_CUSTOM_TRI ||
|
||||
currentMixerMode == MIXER_CUSTOM)
|
||||
{
|
||||
loadCustomServoMixer();
|
||||
|
||||
// If there are servo rules after all, update variables
|
||||
if (servoRuleCount > 0) {
|
||||
servoOutputEnabled = 1;
|
||||
mixerUsesServos = 1;
|
||||
}
|
||||
|
||||
}
|
||||
}
|
||||
void loadCustomServoMixer(void)
|
||||
{
|
||||
int i;
|
||||
|
||||
// reset settings
|
||||
servoRuleCount = 0;
|
||||
minServoIndex = 255;
|
||||
maxServoIndex = 0;
|
||||
memset(currentServoMixer, 0, sizeof(currentServoMixer));
|
||||
|
||||
// load custom mixer into currentServoMixer
|
||||
for (i = 0; i < MAX_SERVO_RULES; i++) {
|
||||
// check if done
|
||||
if (customServoMixers[i].rate == 0)
|
||||
break;
|
||||
|
||||
if (customServoMixers[i].targetChannel < minServoIndex) {
|
||||
minServoIndex = customServoMixers[i].targetChannel;
|
||||
}
|
||||
|
||||
if (customServoMixers[i].targetChannel > maxServoIndex) {
|
||||
maxServoIndex = customServoMixers[i].targetChannel;
|
||||
}
|
||||
|
||||
currentServoMixer[i] = customServoMixers[i];
|
||||
servoRuleCount++;
|
||||
}
|
||||
}
|
||||
|
||||
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;
|
||||
|
||||
int servoOffset;
|
||||
for (servoOffset = 0; servoOffset < MAX_AUX_CHANNEL_COUNT && channelOffset < MAX_SUPPORTED_RC_CHANNEL_COUNT; servoOffset++) {
|
||||
pwmWriteServo(firstServoIndex + servoOffset, rcData[channelOffset++]);
|
||||
}
|
||||
}
|
||||
|
||||
void writeServos(void)
|
||||
{
|
||||
int servoIndex = 0;
|
||||
|
||||
bool zeroServoValue = false;
|
||||
|
||||
/*
|
||||
* in case of tricopters, there might me a need to zero servo output when unarmed
|
||||
*/
|
||||
if ((currentMixerMode == MIXER_TRI || currentMixerMode == MIXER_CUSTOM_TRI) && !ARMING_FLAG(ARMED) && !servoConfig->tri_unarmed_servo) {
|
||||
zeroServoValue = true;
|
||||
}
|
||||
|
||||
// Write mixer servo outputs
|
||||
// mixerUsesServos might indicate SERVO_TILT, servoRuleCount indicate if mixer alone uses servos
|
||||
if (mixerUsesServos && servoRuleCount) {
|
||||
for (int i = minServoIndex; i <= maxServoIndex; i++) {
|
||||
if (zeroServoValue) {
|
||||
pwmWriteServo(servoIndex++, 0);
|
||||
} else {
|
||||
pwmWriteServo(servoIndex++, servo[i]);
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
if (feature(FEATURE_SERVO_TILT)) {
|
||||
pwmWriteServo(servoIndex++, servo[SERVO_GIMBAL_PITCH]);
|
||||
pwmWriteServo(servoIndex++, servo[SERVO_GIMBAL_ROLL]);
|
||||
}
|
||||
|
||||
// forward AUX to remaining servo outputs (not constrained)
|
||||
if (feature(FEATURE_CHANNEL_FORWARDING)) {
|
||||
forwardAuxChannelsToServos(servoIndex);
|
||||
servoIndex += MAX_AUX_CHANNEL_COUNT;
|
||||
}
|
||||
}
|
||||
|
||||
void servoMixer(uint16_t flaperon_throw_offset, uint8_t flaperon_throw_inverted)
|
||||
{
|
||||
int16_t input[INPUT_SOURCE_COUNT]; // Range [-500:+500]
|
||||
static int16_t currentOutput[MAX_SERVO_RULES];
|
||||
int 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] = axisPID[ROLL];
|
||||
input[INPUT_STABILIZED_PITCH] = axisPID[PITCH];
|
||||
input[INPUT_STABILIZED_YAW] = axisPID[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]);
|
||||
}
|
||||
}
|
||||
|
||||
/*
|
||||
Flaperon fligh mode
|
||||
*/
|
||||
if (FLIGHT_MODE(FLAPERON) && (target == SERVO_FLAPPERON_1 || target == SERVO_FLAPPERON_2)) {
|
||||
int8_t multiplier = 1;
|
||||
|
||||
if (flaperon_throw_inverted == 1) {
|
||||
multiplier = -1;
|
||||
}
|
||||
currentOutput[i] += flaperon_throw_offset * getFlaperonDirection(target) * multiplier;
|
||||
}
|
||||
|
||||
servo[target] += servoDirection(target, from) * constrain(((int32_t)currentOutput[i] * currentServoMixer[i].rate) / 100, min, max);
|
||||
} else {
|
||||
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 processServoTilt(void) {
|
||||
// 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;
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
bool isServoOutputEnabled(void)
|
||||
{
|
||||
return servoOutputEnabled;
|
||||
}
|
||||
|
||||
bool isMixerUsingServos(void) {
|
||||
return mixerUsesServos;
|
||||
}
|
||||
|
||||
void filterServos(void)
|
||||
{
|
||||
int servoIdx;
|
||||
|
||||
if (servoConfig->servo_lowpass_enable) {
|
||||
// Initialize servo lowpass filter (servos are calculated at looptime rate)
|
||||
if (!servoFilterIsSet) {
|
||||
for (servoIdx = 0; servoIdx < MAX_SUPPORTED_SERVOS; servoIdx++) {
|
||||
biquadFilterInit(&servoFitlerState[servoIdx], servoConfig->servo_lowpass_freq, gyro.targetLooptime);
|
||||
}
|
||||
|
||||
servoFilterIsSet = true;
|
||||
}
|
||||
|
||||
for (servoIdx = 0; servoIdx < MAX_SUPPORTED_SERVOS; servoIdx++) {
|
||||
// Apply servo lowpass filter and do sanity cheching
|
||||
servo[servoIdx] = (int16_t) biquadFilterApply(&servoFitlerState[servoIdx], (float)servo[servoIdx]);
|
||||
}
|
||||
}
|
||||
|
||||
for (servoIdx = 0; servoIdx < MAX_SUPPORTED_SERVOS; servoIdx++) {
|
||||
servo[servoIdx] = constrain(servo[servoIdx], servoConf[servoIdx].min, servoConf[servoIdx].max);
|
||||
}
|
||||
}
|
||||
|
||||
#endif
|
||||
132
src/main/flight/servos.h
Normal file
132
src/main/flight/servos.h
Normal file
|
|
@ -0,0 +1,132 @@
|
|||
/*
|
||||
* 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
|
||||
|
||||
// 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_ELEVATOR = 2,
|
||||
SERVO_FLAPPERON_1 = 3,
|
||||
SERVO_FLAPPERON_2 = 4,
|
||||
SERVO_RUDDER = 5,
|
||||
SERVO_THROTTLE = 6, // for internal combustion (IC) planes
|
||||
SERVO_FLAPS = 7,
|
||||
|
||||
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_ELEVATOR
|
||||
#define SERVO_PLANE_INDEX_MAX SERVO_FLAPS
|
||||
|
||||
#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
|
||||
|
||||
#define FLAPERON_THROW_DEFAULT 250
|
||||
#define FLAPERON_THROW_MIN 100
|
||||
#define FLAPERON_THROW_MAX 400
|
||||
|
||||
typedef struct servoConfig_s {
|
||||
uint8_t tri_unarmed_servo; // send tail servo correction pulses even when unarmed
|
||||
int16_t servo_lowpass_freq; // lowpass servo filter frequency selection; 1/1000ths of loop freq
|
||||
int8_t servo_lowpass_enable; // enable/disable lowpass filter
|
||||
} servoConfig_t;
|
||||
|
||||
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;
|
||||
uint8_t minServoIndex;
|
||||
uint8_t maxServoIndex;
|
||||
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;
|
||||
|
||||
extern int16_t servo[MAX_SUPPORTED_SERVOS];
|
||||
|
||||
bool isServoOutputEnabled(void);
|
||||
bool isMixerUsingServos(void);
|
||||
void writeServos(void);
|
||||
void filterServos(void);
|
||||
void servoMixerLoadMix(int index, servoMixer_t *customServoMixers);
|
||||
void loadCustomServoMixer(void);
|
||||
int servoDirection(int servoIndex, int fromChannel);
|
||||
void servosUseConfigs(servoConfig_t *servoConfigToUse, servoParam_t *servoParamsToUse, gimbalConfig_t *gimbalConfigToUse);
|
||||
void servosInit(servoMixer_t *customServoMixers);
|
||||
|
||||
|
|
@ -64,6 +64,7 @@
|
|||
|
||||
#include "flight/failsafe.h"
|
||||
#include "flight/mixer.h"
|
||||
#include "flight/servos.h"
|
||||
#include "flight/pid.h"
|
||||
#include "flight/imu.h"
|
||||
#include "flight/navigation_rewrite.h"
|
||||
|
|
|
|||
|
|
@ -80,6 +80,7 @@
|
|||
#include "flight/pid.h"
|
||||
#include "flight/imu.h"
|
||||
#include "flight/mixer.h"
|
||||
#include "flight/servos.h"
|
||||
#include "flight/navigation_rewrite.h"
|
||||
#include "flight/failsafe.h"
|
||||
|
||||
|
|
@ -752,9 +753,9 @@ const clivalue_t valueTable[] = {
|
|||
#ifdef USE_SERVOS
|
||||
{ "flaperon_throw_offset", VAR_INT16 | PROFILE_VALUE, &masterConfig.profile[0].flaperon_throw_offset, .config.minmax = { FLAPERON_THROW_MIN, FLAPERON_THROW_MAX}, 0 },
|
||||
{ "flaperon_throw_inverted", VAR_UINT8 | PROFILE_VALUE | MODE_LOOKUP, &masterConfig.profile[0].flaperon_throw_inverted, .config.lookup = { TABLE_OFF_ON }, 0 },
|
||||
{ "tri_unarmed_servo", VAR_INT8 | MASTER_VALUE | MODE_LOOKUP, &masterConfig.mixerConfig.tri_unarmed_servo, .config.lookup = { TABLE_OFF_ON }, 0 },
|
||||
{ "servo_lowpass_freq", VAR_INT16 | MASTER_VALUE, &masterConfig.mixerConfig.servo_lowpass_freq, .config.minmax = { 10, 400}, 0 },
|
||||
{ "servo_lowpass_enable", VAR_INT8 | MASTER_VALUE | MODE_LOOKUP, &masterConfig.mixerConfig.servo_lowpass_enable, .config.lookup = { TABLE_OFF_ON }, 0 },
|
||||
{ "tri_unarmed_servo", VAR_INT8 | MASTER_VALUE | MODE_LOOKUP, &masterConfig.servoConfig.tri_unarmed_servo, .config.lookup = { TABLE_OFF_ON }, 0 },
|
||||
{ "servo_lowpass_freq", VAR_INT16 | MASTER_VALUE, &masterConfig.servoConfig.servo_lowpass_freq, .config.minmax = { 10, 400}, 0 },
|
||||
{ "servo_lowpass_enable", VAR_INT8 | MASTER_VALUE | MODE_LOOKUP, &masterConfig.servoConfig.servo_lowpass_enable, .config.lookup = { TABLE_OFF_ON }, 0 },
|
||||
{ "gimbal_mode", VAR_UINT8 | PROFILE_VALUE | MODE_LOOKUP, &masterConfig.profile[0].gimbalConfig.mode, .config.lookup = { TABLE_GIMBAL_MODE }, 0 },
|
||||
{ "servo_center_pulse", VAR_UINT16 | MASTER_VALUE, &masterConfig.escAndServoConfig.servoCenterPulse, .config.minmax = { PWM_RANGE_ZERO, PWM_RANGE_MAX }, 0 },
|
||||
{ "servo_pwm_rate", VAR_UINT16 | MASTER_VALUE, &masterConfig.servo_pwm_rate, .config.minmax = { 50, 498 }, 0 },
|
||||
|
|
|
|||
|
|
@ -100,6 +100,7 @@
|
|||
#include "flight/pid.h"
|
||||
#include "flight/imu.h"
|
||||
#include "flight/mixer.h"
|
||||
#include "flight/servos.h"
|
||||
#include "flight/failsafe.h"
|
||||
#include "flight/navigation_rewrite.h"
|
||||
|
||||
|
|
|
|||
|
|
@ -60,6 +60,7 @@
|
|||
#include "telemetry/telemetry.h"
|
||||
|
||||
#include "flight/mixer.h"
|
||||
#include "flight/servos.h"
|
||||
#include "flight/pid.h"
|
||||
#include "flight/failsafe.h"
|
||||
|
||||
|
|
|
|||
|
|
@ -59,6 +59,7 @@
|
|||
#include "rx/rx.h"
|
||||
|
||||
#include "flight/mixer.h"
|
||||
#include "flight/servos.h"
|
||||
#include "flight/pid.h"
|
||||
#include "flight/imu.h"
|
||||
#include "flight/failsafe.h"
|
||||
|
|
|
|||
Loading…
Add table
Add a link
Reference in a new issue