Mirror/inav
1
0
Fork 0
mirror of https://github.com/iNavFlight/inav.git synced 2025-07-22 15:55:40 +03:00
Code Issues Wiki Activity

Flaperon flight mode (#307)

Browse source 
* basic flaperon functionality
* flaperon configuration
* FLAPERON can be activated only on MIXER_AIRPLANE and MIXER_CUSTOM_AIRPLANE
* state FLAPERON_AVAILABLE added
This commit is contained in:
Paweł Spychalski 2016-07-21 13:53:09 +02:00 • committed by Konstantin Sharlaimov
parent e53d2b1a92
commit cfd8f7ed82
9 changed files with 84 additions and 16 deletions
Download patch file Download diff file Expand all files Collapse all files

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

@ -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.

3
src/main/config/config_profile.h
View file

@ -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;

15
src/main/config/runtime_config.h
View file

@ -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))

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

@ -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);

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

@ -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);

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

@ -51,6 +51,7 @@ typedef enum {
BOXGCSNAV,
BOXHEADINGLOCK,
BOXSURFACE,
BOXFLAPERON,
CHECKBOX_ITEM_COUNT
} boxId_e;

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

@ -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 },

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

@ -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++) {

13
src/main/mw.c
View file

@ -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)) {