Commit cfd8f7ed authored by Paweł Spychalski's avatar Paweł Spychalski Committed by Konstantin Sharlaimov
Browse files

Flaperon flight mode (#307)

* basic flaperon functionality
* flaperon configuration
* FLAPERON can be activated only on MIXER_AIRPLANE and MIXER_CUSTOM_AIRPLANE
* state FLAPERON_AVAILABLE added
parent e53d2b1a
......@@ -604,6 +604,10 @@ static void resetConf(void)
// gimbal
currentProfile->gimbalConfig.mode = GIMBAL_MODE_NORMAL;
currentProfile->flaperon_throw_offset = FLAPERON_THROW_DEFAULT;
currentProfile->flaperon_throw_inverted = 0;
#endif
// custom mixer. clear by defaults.
......
......@@ -40,5 +40,8 @@ typedef struct profile_s {
servoParam_t servoConf[MAX_SUPPORTED_SERVOS]; // servo configuration
// gimbal-related configuration
gimbalConfig_t gimbalConfig;
uint16_t flaperon_throw_offset;
uint8_t flaperon_throw_inverted;
#endif
} profile_t;
......@@ -45,6 +45,7 @@ typedef enum {
UNUSED_MODE2 = (1 << 11), // old G-Tune
NAV_WP_MODE = (1 << 12),
HEADING_LOCK = (1 << 13),
FLAPERON = (1 << 14),
} flightModeFlags_e;
extern uint16_t flightModeFlags;
......@@ -54,12 +55,14 @@ extern uint16_t flightModeFlags;
#define FLIGHT_MODE(mask) (flightModeFlags & (mask))
typedef enum {
GPS_FIX_HOME = (1 << 0),
GPS_FIX = (1 << 1),
CALIBRATE_MAG = (1 << 2),
SMALL_ANGLE = (1 << 3),
FIXED_WING = (1 << 4), // set when in flying_wing or airplane mode. currently used by althold selection code
ANTI_WINDUP = (1 << 5),
GPS_FIX_HOME = (1 << 0),
GPS_FIX = (1 << 1),
CALIBRATE_MAG = (1 << 2),
SMALL_ANGLE = (1 << 3),
FIXED_WING = (1 << 4), // set when in flying_wing or airplane mode. currently used by althold selection code
ANTI_WINDUP = (1 << 5),
//PID_ATTENUATE = (1 << 6),
FLAPERON_AVAILABLE = (1 << 7)
} stateFlags_t;
#define DISABLE_STATE(mask) (stateFlags &= ~(mask))
......
......@@ -53,6 +53,7 @@
#include "config/runtime_config.h"
#include "config/config.h"
#include "config/config_profile.h"
//#define MIXER_DEBUG
......@@ -355,6 +356,15 @@ void mixerUseConfigs(
}
#ifdef USE_SERVOS
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;
......@@ -404,6 +414,12 @@ void mixerInit(mixerMode_e mixerMode, motorMixer_t *initialCustomMotorMixers, se
DISABLE_STATE(FIXED_WING);
}
if (currentMixerMode == MIXER_AIRPLANE || currentMixerMode == MIXER_CUSTOM_AIRPLANE) {
ENABLE_STATE(FLAPERON_AVAILABLE);
} else {
DISABLE_STATE(FLAPERON_AVAILABLE);
}
customMixers = initialCustomMotorMixers;
customServoMixers = initialCustomServoMixers;
......@@ -464,7 +480,7 @@ void mixerUsePWMIOConfiguration(void)
int i;
motorCount = 0;
if (currentMixerMode == MIXER_CUSTOM || currentMixerMode == MIXER_CUSTOM_TRI || currentMixerMode == MIXER_CUSTOM_AIRPLANE) {
// load custom mixer into currentMixer
for (i = 0; i < MAX_SUPPORTED_MOTORS; i++) {
......@@ -779,7 +795,7 @@ void mixTable(void)
#ifdef USE_SERVOS
void servoMixer(void)
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];
......@@ -835,13 +851,26 @@ void servoMixer(void)
int16_t min = currentServoMixer[i].min * servo_width / 100 - servo_width / 2;
int16_t max = currentServoMixer[i].max * servo_width / 100 - servo_width / 2;
if (currentServoMixer[i].speed == 0)
if (currentServoMixer[i].speed == 0) {
currentOutput[i] = input[from];
else {
if (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])
} 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);
......
......@@ -164,6 +164,10 @@ typedef enum {
#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
......@@ -235,7 +239,7 @@ int servoDirection(int servoIndex, int fromChannel);
void mixerResetDisarmedMotors(void);
void mixTable(void);
void writeMotors(void);
void servoMixer(void);
void servoMixer(uint16_t flaperon_throw_offset, uint8_t flaperon_throw_inverted);
void processServoTilt(void);
void stopMotors(void);
void StopPwmAllMotors(void);
......
......@@ -51,6 +51,7 @@ typedef enum {
BOXGCSNAV,
BOXHEADINGLOCK,
BOXSURFACE,
BOXFLAPERON,
CHECKBOX_ITEM_COUNT
} boxId_e;
......
......@@ -702,6 +702,8 @@ const clivalue_t valueTable[] = {
{ "yaw_jump_prevention_limit", VAR_UINT16 | MASTER_VALUE, &masterConfig.mixerConfig.yaw_jump_prevention_limit, .config.minmax = { YAW_JUMP_PREVENTION_LIMIT_LOW, YAW_JUMP_PREVENTION_LIMIT_HIGH }, 0 },
#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 },
......@@ -715,7 +717,7 @@ const clivalue_t valueTable[] = {
/*
New rates are in dps/10. That means, Rate of 20 means 200dps of rotation speed on given axis.
Rate 180 (1800dps) is max. value gyro can measure reliably
Rate 180 (1800dps) is max. value gyro can measure reliably
*/
{ "roll_rate", VAR_UINT8 | CONTROL_RATE_VALUE, &masterConfig.controlRateProfiles[0].rates[FD_ROLL], .config.minmax = { CONTROL_RATE_CONFIG_ROLL_PITCH_RATE_MIN, CONTROL_RATE_CONFIG_ROLL_PITCH_RATE_MAX }, 0 },
{ "pitch_rate", VAR_UINT8 | CONTROL_RATE_VALUE, &masterConfig.controlRateProfiles[0].rates[FD_PITCH], .config.minmax = { CONTROL_RATE_CONFIG_ROLL_PITCH_RATE_MIN, CONTROL_RATE_CONFIG_ROLL_PITCH_RATE_MAX }, 0 },
......
......@@ -143,6 +143,7 @@ static const box_t boxes[CHECKBOX_ITEM_COUNT + 1] = {
{ BOXGCSNAV, "GCS NAV;", 31 },
{ BOXHEADINGLOCK, "HEADING LOCK;", 32 },
{ BOXSURFACE, "SURFACE;", 33 },
{ BOXFLAPERON, "FLAPERON;", 34 },
{ CHECKBOX_ITEM_COUNT, NULL, 0xFF }
};
......@@ -437,8 +438,17 @@ void mspInit(void)
}
#endif
if (isFixedWing)
if (isFixedWing) {
activeBoxIds[activeBoxIdCount++] = BOXPASSTHRU;
}
/*
* FLAPERON mode active only in case of airplane and custom airplane. Activating on
* flying wing can cause bad thing
*/
if (masterConfig.mixerMode == MIXER_AIRPLANE || masterConfig.mixerMode == MIXER_CUSTOM_AIRPLANE) {
activeBoxIds[activeBoxIdCount++] = BOXFLAPERON;
}
activeBoxIds[activeBoxIdCount++] = BOXBEEPERON;
......@@ -511,6 +521,7 @@ static uint32_t packFlightModeFlags(void)
IS_ENABLED(IS_RC_MODE_ACTIVE(BOXGCSNAV)) << BOXGCSNAV |
IS_ENABLED(FLIGHT_MODE(HEADING_LOCK)) << BOXHEADINGLOCK |
IS_ENABLED(IS_RC_MODE_ACTIVE(BOXSURFACE)) << BOXSURFACE |
IS_ENABLED(FLIGHT_MODE(FLAPERON)) << BOXFLAPERON |
IS_ENABLED(IS_RC_MODE_ACTIVE(BOXHOMERESET)) << BOXHOMERESET;
for (i = 0; i < activeBoxIdCount; i++) {
......
......@@ -398,6 +398,17 @@ void processRx(void)
DISABLE_FLIGHT_MODE(HEADING_LOCK);
}
/*
Flaperon mode
*/
if (IS_RC_MODE_ACTIVE(BOXFLAPERON) && STATE(FLAPERON_AVAILABLE)) {
if (!FLIGHT_MODE(FLAPERON)) {
ENABLE_FLIGHT_MODE(FLAPERON);
}
} else {
DISABLE_FLIGHT_MODE(FLAPERON);
}
#if defined(MAG)
if (sensors(SENSOR_ACC) || sensors(SENSOR_MAG)) {
if (IS_RC_MODE_ACTIVE(BOXMAG)) {
......@@ -599,7 +610,7 @@ void taskMainPidLoop(void)
#ifdef USE_SERVOS
if (isMixerUsingServos()) {
servoMixer();
servoMixer(currentProfile->flaperon_throw_offset, currentProfile->flaperon_throw_inverted);
}
if (feature(FEATURE_SERVO_TILT)) {
......
Markdown is supported
0% or .
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment