mirror of
https://github.com/betaflight/betaflight.git
synced 2025-07-15 20:35:33 +03:00
b5e18a90a2
* optimize for code size. * consolidate error message handling. * replace similar error messages with identical ones. * shorten all strings where possible. * made less verbose. This was required for the CC3D OPBL build.
932 lines
32 KiB
C
Executable file
932 lines
32 KiB
C
Executable file
/*
|
|
* 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"
|
|
#include "debug.h"
|
|
|
|
#include "build_config.h"
|
|
|
|
#include "common/axis.h"
|
|
#include "common/maths.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 "io/rc_controls.h"
|
|
|
|
#include "sensors/sensors.h"
|
|
#include "sensors/acceleration.h"
|
|
|
|
#include "flight/mixer.h"
|
|
#include "flight/failsafe.h"
|
|
#include "flight/pid.h"
|
|
#include "flight/imu.h"
|
|
#include "flight/lowpass.h"
|
|
|
|
#include "config/runtime_config.h"
|
|
#include "config/config.h"
|
|
|
|
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_BIPLANE_LEFT = 4,
|
|
SERVO_BIPLANE_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;
|
|
|
|
#define SERVO_PLANE_INDEX_MIN SERVO_FLAPS
|
|
#define SERVO_PLANE_INDEX_MAX SERVO_ELEVATOR
|
|
|
|
#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 MIXER_DEBUG
|
|
|
|
uint8_t motorCount = 0;
|
|
|
|
int16_t motor[MAX_SUPPORTED_MOTORS];
|
|
int16_t motor_disarmed[MAX_SUPPORTED_MOTORS];
|
|
|
|
static mixerConfig_t *mixerConfig;
|
|
static flight3DConfig_t *flight3DConfig;
|
|
static escAndServoConfig_t *escAndServoConfig;
|
|
static airplaneConfig_t *airplaneConfig;
|
|
static rxConfig_t *rxConfig;
|
|
|
|
static mixerMode_e currentMixerMode;
|
|
static motorMixer_t currentMixer[MAX_SUPPORTED_MOTORS];
|
|
|
|
|
|
#ifdef USE_SERVOS
|
|
static uint8_t servoRuleCount = 0;
|
|
static servoMixer_t currentServoMixer[MAX_SERVO_RULES];
|
|
static gimbalConfig_t *gimbalConfig;
|
|
int16_t servo[MAX_SUPPORTED_SERVOS];
|
|
static int useServo;
|
|
STATIC_UNIT_TESTED uint8_t servoCount;
|
|
static servoParam_t *servoConf;
|
|
static lowpass_t lowpassFilters[MAX_SUPPORTED_SERVOS];
|
|
#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
|
|
{ 1.0f, 1.0f, 1.0f, 1.0f }, // REAR_L
|
|
{ 1.0f, 1.0f, -1.0f, -1.0f }, // FRONT_L
|
|
};
|
|
#ifndef USE_QUAD_MIXER_ONLY
|
|
static const motorMixer_t mixerTri[] = {
|
|
{ 1.0f, 0.0f, 1.333333f, 0.0f }, // REAR
|
|
{ 1.0f, -1.0f, -0.666667f, 0.0f }, // RIGHT
|
|
{ 1.0f, 1.0f, -0.666667f, 0.0f }, // LEFT
|
|
};
|
|
|
|
static const motorMixer_t mixerQuadP[] = {
|
|
{ 1.0f, 0.0f, 1.0f, -1.0f }, // REAR
|
|
{ 1.0f, -1.0f, 0.0f, 1.0f }, // RIGHT
|
|
{ 1.0f, 1.0f, 0.0f, 1.0f }, // LEFT
|
|
{ 1.0f, 0.0f, -1.0f, -1.0f }, // FRONT
|
|
};
|
|
|
|
static const motorMixer_t mixerBi[] = {
|
|
{ 1.0f, 1.0f, 0.0f, 0.0f }, // LEFT
|
|
{ 1.0f, -1.0f, 0.0f, 0.0f }, // RIGHT
|
|
};
|
|
|
|
static const motorMixer_t mixerY6[] = {
|
|
{ 1.0f, 0.0f, 1.333333f, 1.0f }, // REAR
|
|
{ 1.0f, -1.0f, -0.666667f, -1.0f }, // RIGHT
|
|
{ 1.0f, 1.0f, -0.666667f, -1.0f }, // LEFT
|
|
{ 1.0f, 0.0f, 1.333333f, -1.0f }, // UNDER_REAR
|
|
{ 1.0f, -1.0f, -0.666667f, 1.0f }, // UNDER_RIGHT
|
|
{ 1.0f, 1.0f, -0.666667f, 1.0f }, // UNDER_LEFT
|
|
};
|
|
|
|
static const motorMixer_t mixerHex6P[] = {
|
|
{ 1.0f, -0.866025f, 0.5f, 1.0f }, // REAR_R
|
|
{ 1.0f, -0.866025f, -0.5f, -1.0f }, // FRONT_R
|
|
{ 1.0f, 0.866025f, 0.5f, 1.0f }, // REAR_L
|
|
{ 1.0f, 0.866025f, -0.5f, -1.0f }, // FRONT_L
|
|
{ 1.0f, 0.0f, -1.0f, 1.0f }, // FRONT
|
|
{ 1.0f, 0.0f, 1.0f, -1.0f }, // REAR
|
|
};
|
|
|
|
static const motorMixer_t mixerY4[] = {
|
|
{ 1.0f, 0.0f, 1.0f, -1.0f }, // REAR_TOP CW
|
|
{ 1.0f, -1.0f, -1.0f, 0.0f }, // FRONT_R CCW
|
|
{ 1.0f, 0.0f, 1.0f, 1.0f }, // REAR_BOTTOM CCW
|
|
{ 1.0f, 1.0f, -1.0f, 0.0f }, // FRONT_L CW
|
|
};
|
|
|
|
static const motorMixer_t mixerHex6X[] = {
|
|
{ 1.0f, -0.5f, 0.866025f, 1.0f }, // REAR_R
|
|
{ 1.0f, -0.5f, -0.866025f, 1.0f }, // FRONT_R
|
|
{ 1.0f, 0.5f, 0.866025f, -1.0f }, // REAR_L
|
|
{ 1.0f, 0.5f, -0.866025f, -1.0f }, // FRONT_L
|
|
{ 1.0f, -1.0f, 0.0f, -1.0f }, // RIGHT
|
|
{ 1.0f, 1.0f, 0.0f, 1.0f }, // LEFT
|
|
};
|
|
|
|
static const motorMixer_t mixerOctoX8[] = {
|
|
{ 1.0f, -1.0f, 1.0f, -1.0f }, // REAR_R
|
|
{ 1.0f, -1.0f, -1.0f, 1.0f }, // FRONT_R
|
|
{ 1.0f, 1.0f, 1.0f, 1.0f }, // REAR_L
|
|
{ 1.0f, 1.0f, -1.0f, -1.0f }, // FRONT_L
|
|
{ 1.0f, -1.0f, 1.0f, 1.0f }, // UNDER_REAR_R
|
|
{ 1.0f, -1.0f, -1.0f, -1.0f }, // UNDER_FRONT_R
|
|
{ 1.0f, 1.0f, 1.0f, -1.0f }, // UNDER_REAR_L
|
|
{ 1.0f, 1.0f, -1.0f, 1.0f }, // UNDER_FRONT_L
|
|
};
|
|
|
|
static const motorMixer_t mixerOctoFlatP[] = {
|
|
{ 1.0f, 0.707107f, -0.707107f, 1.0f }, // FRONT_L
|
|
{ 1.0f, -0.707107f, -0.707107f, 1.0f }, // FRONT_R
|
|
{ 1.0f, -0.707107f, 0.707107f, 1.0f }, // REAR_R
|
|
{ 1.0f, 0.707107f, 0.707107f, 1.0f }, // REAR_L
|
|
{ 1.0f, 0.0f, -1.0f, -1.0f }, // FRONT
|
|
{ 1.0f, -1.0f, 0.0f, -1.0f }, // RIGHT
|
|
{ 1.0f, 0.0f, 1.0f, -1.0f }, // REAR
|
|
{ 1.0f, 1.0f, 0.0f, -1.0f }, // LEFT
|
|
};
|
|
|
|
static const motorMixer_t mixerOctoFlatX[] = {
|
|
{ 1.0f, 1.0f, -0.5f, 1.0f }, // MIDFRONT_L
|
|
{ 1.0f, -0.5f, -1.0f, 1.0f }, // FRONT_R
|
|
{ 1.0f, -1.0f, 0.5f, 1.0f }, // MIDREAR_R
|
|
{ 1.0f, 0.5f, 1.0f, 1.0f }, // REAR_L
|
|
{ 1.0f, 0.5f, -1.0f, -1.0f }, // FRONT_L
|
|
{ 1.0f, -1.0f, -0.5f, -1.0f }, // MIDFRONT_R
|
|
{ 1.0f, -0.5f, 1.0f, -1.0f }, // REAR_R
|
|
{ 1.0f, 1.0f, 0.5f, -1.0f }, // MIDREAR_L
|
|
};
|
|
|
|
static const motorMixer_t mixerVtail4[] = {
|
|
{ 1.0f, 0.0f, 1.0f, 1.0f }, // REAR_R
|
|
{ 1.0f, -1.0f, -1.0f, 0.0f }, // FRONT_R
|
|
{ 1.0f, 0.0f, 1.0f, -1.0f }, // REAR_L
|
|
{ 1.0f, 1.0f, -1.0f, -0.0f }, // FRONT_L
|
|
};
|
|
|
|
static const motorMixer_t mixerAtail4[] = {
|
|
{ 1.0f, 0.0f, 1.0f, 1.0f }, // REAR_R
|
|
{ 1.0f, -1.0f, -1.0f, 0.0f }, // FRONT_R
|
|
{ 1.0f, 0.0f, 1.0f, -1.0f }, // REAR_L
|
|
{ 1.0f, 1.0f, -1.0f, -0.0f }, // FRONT_L
|
|
};
|
|
|
|
static const motorMixer_t mixerHex6H[] = {
|
|
{ 1.0f, -1.0f, 1.0f, -1.0f }, // REAR_R
|
|
{ 1.0f, -1.0f, -1.0f, 1.0f }, // FRONT_R
|
|
{ 1.0f, 1.0f, 1.0f, 1.0f }, // REAR_L
|
|
{ 1.0f, 1.0f, -1.0f, -1.0f }, // FRONT_L
|
|
{ 1.0f, 0.0f, 0.0f, 0.0f }, // RIGHT
|
|
{ 1.0f, 0.0f, 0.0f, 0.0f }, // LEFT
|
|
};
|
|
|
|
static const motorMixer_t mixerDualcopter[] = {
|
|
{ 1.0f, 0.0f, 0.0f, -1.0f }, // LEFT
|
|
{ 1.0f, 0.0f, 0.0f, 1.0f }, // RIGHT
|
|
};
|
|
|
|
static const motorMixer_t mixerThrustVector[] = {
|
|
{ 1.0f, 0.0f, 0.0f, -0.5f }, // LEFT
|
|
{ 1.0f, 0.0f, 0.0f, 0.5f }, // RIGHT
|
|
};
|
|
|
|
// Keep synced with mixerMode_e
|
|
const mixer_t mixers[] = {
|
|
// motors, servos, motor mixer
|
|
{ 0, 0, NULL }, // entry 0
|
|
{ 3, 1, mixerTri }, // MIXER_TRI
|
|
{ 4, 0, mixerQuadP }, // MIXER_QUADP
|
|
{ 4, 0, mixerQuadX }, // MIXER_QUADX
|
|
{ 2, 1, mixerBi }, // MIXER_BI
|
|
{ 0, 1, NULL }, // * MIXER_GIMBAL
|
|
{ 6, 0, mixerY6 }, // MIXER_Y6
|
|
{ 6, 0, mixerHex6P }, // MIXER_HEX6
|
|
{ 2, 1, mixerThrustVector }, // * MIXER_FLYING_WING
|
|
{ 4, 0, mixerY4 }, // MIXER_Y4
|
|
{ 6, 0, mixerHex6X }, // MIXER_HEX6X
|
|
{ 8, 0, mixerOctoX8 }, // MIXER_OCTOX8
|
|
{ 8, 0, mixerOctoFlatP }, // MIXER_OCTOFLATP
|
|
{ 8, 0, mixerOctoFlatX }, // MIXER_OCTOFLATX
|
|
{ 1, 1, NULL }, // * MIXER_AIRPLANE
|
|
{ 0, 1, NULL }, // * MIXER_HELI_120_CCPM
|
|
{ 0, 1, NULL }, // * MIXER_HELI_90_DEG
|
|
{ 4, 0, mixerVtail4 }, // MIXER_VTAIL4
|
|
{ 6, 0, mixerHex6H }, // MIXER_HEX6H
|
|
{ 0, 1, NULL }, // * MIXER_PPM_TO_SERVO
|
|
{ 2, 1, mixerDualcopter }, // MIXER_DUALCOPTER
|
|
{ 1, 1, NULL }, // MIXER_SINGLECOPTER
|
|
{ 4, 0, mixerAtail4 }, // MIXER_ATAIL4
|
|
{ 0, 0, NULL }, // MIXER_CUSTOM
|
|
{ 1, 1, NULL }, // MIXER_CUSTOM_AIRPLANE
|
|
{ 3, 1, NULL }, // MIXER_CUSTOM_TRI
|
|
};
|
|
#endif
|
|
|
|
#ifdef USE_SERVOS
|
|
// mixer rule format servo, input, rate, speed, min, max, box
|
|
static const servoMixer_t servoMixerAirplane[] = {
|
|
{ SERVO_FLAPPERON_1, INPUT_ROLL, 100, 0, 0, 100, 0 },
|
|
{ SERVO_FLAPPERON_2, INPUT_ROLL, 100, 0, 0, 100, 0 },
|
|
{ SERVO_RUDDER, INPUT_YAW, 100, 0, 0, 100, 0 },
|
|
{ SERVO_ELEVATOR, INPUT_PITCH, 100, 0, 0, 100, 0 },
|
|
};
|
|
|
|
static const servoMixer_t servoMixerFlyingWing[] = {
|
|
{ SERVO_FLAPPERON_1, INPUT_ROLL, 100, 0, 0, 100, 0 },
|
|
{ SERVO_FLAPPERON_1, INPUT_PITCH, 100, 0, 0, 100, 0 },
|
|
{ SERVO_FLAPPERON_2, INPUT_ROLL, 100, 0, 0, 100, 0 },
|
|
{ SERVO_FLAPPERON_2, INPUT_PITCH, 100, 0, 0, 100, 0 },
|
|
};
|
|
|
|
static const servoMixer_t servoMixerBI[] = {
|
|
{ SERVO_BIPLANE_LEFT, INPUT_YAW, 100, 0, 0, 100, 0 },
|
|
{ SERVO_BIPLANE_LEFT, INPUT_PITCH, 100, 0, 0, 100, 0 },
|
|
{ SERVO_BIPLANE_RIGHT, INPUT_YAW, 100, 0, 0, 100, 0 },
|
|
{ SERVO_BIPLANE_RIGHT, INPUT_PITCH, 100, 0, 0, 100, 0 },
|
|
};
|
|
|
|
static const servoMixer_t servoMixerTri[] = {
|
|
{ SERVO_RUDDER, INPUT_YAW, 100, 0, 0, 100, 0 },
|
|
};
|
|
|
|
static const servoMixer_t servoMixerDual[] = {
|
|
{ SERVO_DUALCOPTER_LEFT, INPUT_PITCH, 100, 0, 0, 100, 0 },
|
|
{ SERVO_DUALCOPTER_RIGHT, INPUT_ROLL, 100, 0, 0, 100, 0 },
|
|
};
|
|
|
|
static const servoMixer_t servoMixerSingle[] = {
|
|
{ SERVO_SINGLECOPTER_1, INPUT_YAW, 100, 0, 0, 100, 0 },
|
|
{ SERVO_SINGLECOPTER_1, INPUT_PITCH, 100, 0, 0, 100, 0 },
|
|
{ SERVO_SINGLECOPTER_2, INPUT_YAW, 100, 0, 0, 100, 0 },
|
|
{ SERVO_SINGLECOPTER_2, INPUT_PITCH, 100, 0, 0, 100, 0 },
|
|
{ SERVO_SINGLECOPTER_3, INPUT_YAW, 100, 0, 0, 100, 0 },
|
|
{ SERVO_SINGLECOPTER_3, INPUT_ROLL, 100, 0, 0, 100, 0 },
|
|
{ SERVO_SINGLECOPTER_4, INPUT_YAW, 100, 0, 0, 100, 0 },
|
|
{ SERVO_SINGLECOPTER_4, INPUT_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
|
|
{ 1, servoMixerTri }, // MULTITYPE_TRI
|
|
{ 0, NULL }, // MULTITYPE_QUADP
|
|
{ 0, NULL }, // MULTITYPE_QUADX
|
|
{ 4, servoMixerBI }, // MULTITYPE_BI
|
|
{ 2, servoMixerGimbal }, // * MULTITYPE_GIMBAL
|
|
{ 0, NULL }, // MULTITYPE_Y6
|
|
{ 0, NULL }, // MULTITYPE_HEX6
|
|
{ 4, servoMixerFlyingWing },// * MULTITYPE_FLYING_WING
|
|
{ 0, NULL }, // MULTITYPE_Y4
|
|
{ 0, NULL }, // MULTITYPE_HEX6X
|
|
{ 0, NULL }, // MULTITYPE_OCTOX8
|
|
{ 0, NULL }, // MULTITYPE_OCTOFLATP
|
|
{ 0, NULL }, // MULTITYPE_OCTOFLATX
|
|
{ 4, 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
|
|
{ 2, servoMixerDual }, // MULTITYPE_DUALCOPTER
|
|
{ 8, servoMixerSingle }, // MULTITYPE_SINGLECOPTER
|
|
{ 0, NULL }, // MULTITYPE_ATAIL4
|
|
{ 0, NULL }, // MULTITYPE_CUSTOM
|
|
{ 0, NULL }, // MULTITYPE_CUSTOM_PLANE
|
|
{ 0, NULL }, // MULTITYPE_CUSTOM_TRI
|
|
};
|
|
|
|
static servoMixer_t *customServoMixers;
|
|
#endif
|
|
|
|
|
|
static motorMixer_t *customMixers;
|
|
|
|
void mixerUseConfigs(
|
|
#ifdef USE_SERVOS
|
|
servoParam_t *servoConfToUse,
|
|
gimbalConfig_t *gimbalConfigToUse,
|
|
#endif
|
|
flight3DConfig_t *flight3DConfigToUse,
|
|
escAndServoConfig_t *escAndServoConfigToUse,
|
|
mixerConfig_t *mixerConfigToUse,
|
|
airplaneConfig_t *airplaneConfigToUse,
|
|
rxConfig_t *rxConfigToUse)
|
|
{
|
|
#ifdef USE_SERVOS
|
|
servoConf = servoConfToUse;
|
|
gimbalConfig = gimbalConfigToUse;
|
|
#endif
|
|
flight3DConfig = flight3DConfigToUse;
|
|
escAndServoConfig = escAndServoConfigToUse;
|
|
mixerConfig = mixerConfigToUse;
|
|
airplaneConfig = airplaneConfigToUse;
|
|
rxConfig = rxConfigToUse;
|
|
}
|
|
|
|
#ifdef USE_SERVOS
|
|
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;
|
|
}
|
|
|
|
// FIXME rename 'fromChannel' to inputSource
|
|
int servoDirection(int servoIndex, int fromChannel)
|
|
{
|
|
// determine the direction (reversed or not) from the direction bitfield of the servo
|
|
if (servoConf[servoIndex].reversedSources & (1 << fromChannel))
|
|
return -1;
|
|
else
|
|
return 1;
|
|
}
|
|
#endif
|
|
|
|
#ifndef USE_QUAD_MIXER_ONLY
|
|
|
|
void loadCustomServoMixer(void)
|
|
{
|
|
uint8_t i;
|
|
|
|
// reset settings
|
|
servoRuleCount = 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;
|
|
|
|
currentServoMixer[i] = customServoMixers[i];
|
|
servoRuleCount++;
|
|
}
|
|
}
|
|
|
|
void mixerInit(mixerMode_e mixerMode, motorMixer_t *initialCustomMotorMixers, servoMixer_t *initialCustomServoMixers)
|
|
{
|
|
currentMixerMode = mixerMode;
|
|
|
|
customMixers = initialCustomMotorMixers;
|
|
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 mixerUsePWMOutputConfiguration(pwmOutputConfiguration_t *pwmOutputConfiguration)
|
|
{
|
|
int i;
|
|
|
|
servoCount = pwmOutputConfiguration->servoCount;
|
|
|
|
if (currentMixerMode == MIXER_CUSTOM || currentMixerMode == MIXER_CUSTOM_TRI) {
|
|
// load custom mixer into currentMixer
|
|
for (i = 0; i < MAX_SUPPORTED_MOTORS; i++) {
|
|
// check if done
|
|
if (customMixers[i].throttle == 0.0f)
|
|
break;
|
|
currentMixer[i] = customMixers[i];
|
|
motorCount++;
|
|
}
|
|
} else {
|
|
motorCount = mixers[currentMixerMode].motorCount;
|
|
// copy motor-based mixers
|
|
if (mixers[currentMixerMode].motor) {
|
|
for (i = 0; i < motorCount; i++)
|
|
currentMixer[i] = mixers[currentMixerMode].motor[i];
|
|
}
|
|
}
|
|
|
|
if (useServo) {
|
|
servoRuleCount = servoMixers[currentMixerMode].servoRuleCount;
|
|
if (servoMixers[currentMixerMode].rule) {
|
|
for (i = 0; i < servoRuleCount; i++)
|
|
currentServoMixer[i] = servoMixers[currentMixerMode].rule[i];
|
|
}
|
|
}
|
|
|
|
// in 3D mode, mixer gain has to be halved
|
|
if (feature(FEATURE_3D)) {
|
|
if (motorCount > 1) {
|
|
for (i = 0; i < motorCount; i++) {
|
|
currentMixer[i].pitch *= 0.5f;
|
|
currentMixer[i].roll *= 0.5f;
|
|
currentMixer[i].yaw *= 0.5f;
|
|
}
|
|
}
|
|
}
|
|
|
|
// 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 {
|
|
if (currentMixerMode == MIXER_CUSTOM_TRI) {
|
|
loadCustomServoMixer();
|
|
}
|
|
DISABLE_STATE(FIXED_WING);
|
|
}
|
|
|
|
mixerResetMotors();
|
|
}
|
|
|
|
|
|
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].fromChannel = customServoMixers[i].rate = customServoMixers[i].box = 0;
|
|
|
|
for (i = 0; i < servoMixers[index].servoRuleCount; i++)
|
|
customServoMixers[i] = servoMixers[index].rule[i];
|
|
}
|
|
|
|
void mixerLoadMix(int index, motorMixer_t *customMixers)
|
|
{
|
|
int i;
|
|
|
|
// we're 1-based
|
|
index++;
|
|
// clear existing
|
|
for (i = 0; i < MAX_SUPPORTED_MOTORS; i++)
|
|
customMixers[i].throttle = 0.0f;
|
|
|
|
// do we have anything here to begin with?
|
|
if (mixers[index].motor != NULL) {
|
|
for (i = 0; i < mixers[index].motorCount; i++)
|
|
customMixers[i] = mixers[index].motor[i];
|
|
}
|
|
}
|
|
|
|
#else
|
|
|
|
void mixerInit(mixerMode_e mixerMode, motorMixer_t *initialCustomMixers)
|
|
{
|
|
currentMixerMode = mixerMode;
|
|
|
|
customMixers = initialCustomMixers;
|
|
}
|
|
|
|
void mixerUsePWMOutputConfiguration(pwmOutputConfiguration_t *pwmOutputConfiguration)
|
|
{
|
|
UNUSED(pwmOutputConfiguration);
|
|
motorCount = 4;
|
|
#ifdef USE_SERVOS
|
|
servoCount = 0;
|
|
#endif
|
|
|
|
uint8_t i;
|
|
for (i = 0; i < motorCount; i++) {
|
|
currentMixer[i] = mixerQuadX[i];
|
|
}
|
|
|
|
mixerResetMotors();
|
|
}
|
|
#endif
|
|
|
|
void mixerResetMotors(void)
|
|
{
|
|
int i;
|
|
// set disarmed motor values
|
|
for (i = 0; i < MAX_SUPPORTED_MOTORS; i++)
|
|
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;
|
|
|
|
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_BI:
|
|
pwmWriteServo(servoIndex++, servo[SERVO_BIPLANE_LEFT]);
|
|
pwmWriteServo(servoIndex++, servo[SERVO_BIPLANE_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_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;
|
|
|
|
case MIXER_CUSTOM_AIRPLANE:
|
|
pwmWriteServo(servoIndex++, servo[SERVO_FLAPPERON_1]);
|
|
pwmWriteServo(servoIndex++, servo[SERVO_FLAPPERON_2]);
|
|
pwmWriteServo(servoIndex++, servo[SERVO_RUDDER]);
|
|
pwmWriteServo(servoIndex++, servo[SERVO_ELEVATOR]);
|
|
pwmWriteServo(servoIndex++, servo[SERVO_FLAPS]);
|
|
pwmWriteServo(servoIndex++, servo[SERVO_THROTTLE]);
|
|
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 (gimbalConfig->gimbal_flags & GIMBAL_FORWARDAUX) {
|
|
forwardAuxChannelsToServos(servoIndex);
|
|
servoIndex += MAX_AUX_CHANNEL_COUNT;
|
|
}
|
|
}
|
|
#endif
|
|
|
|
void writeMotors(void)
|
|
{
|
|
uint8_t i;
|
|
|
|
for (i = 0; i < motorCount; i++)
|
|
pwmWriteMotor(i, motor[i]);
|
|
|
|
|
|
if (feature(FEATURE_ONESHOT125)) {
|
|
pwmCompleteOneshotMotorUpdate(motorCount);
|
|
}
|
|
}
|
|
|
|
void writeAllMotors(int16_t mc)
|
|
{
|
|
uint8_t i;
|
|
|
|
// Sends commands to all motors
|
|
for (i = 0; i < motorCount; i++)
|
|
motor[i] = mc;
|
|
writeMotors();
|
|
}
|
|
|
|
void stopMotors(void)
|
|
{
|
|
writeAllMotors(escAndServoConfig->mincommand);
|
|
|
|
delay(50); // give the timers and ESCs a chance to react.
|
|
}
|
|
|
|
void StopPwmAllMotors()
|
|
{
|
|
pwmShutdownPulsesForAllMotors(motorCount);
|
|
}
|
|
|
|
#ifndef USE_QUAD_MIXER_ONLY
|
|
static void servoMixer(void)
|
|
{
|
|
int16_t input[INPUT_SOURCE_COUNT];
|
|
static int16_t currentOutput[MAX_SERVO_RULES];
|
|
uint8_t i;
|
|
|
|
if (FLIGHT_MODE(PASSTHRU_MODE)) {
|
|
// Direct passthru from RX
|
|
input[INPUT_ROLL] = rcCommand[ROLL];
|
|
input[INPUT_PITCH] = rcCommand[PITCH];
|
|
input[INPUT_YAW] = rcCommand[YAW];
|
|
} else {
|
|
// Assisted modes (gyro only or gyro+acc according to AUX configuration in Gui
|
|
input[INPUT_ROLL] = axisPID[ROLL];
|
|
input[INPUT_PITCH] = axisPID[PITCH];
|
|
input[INPUT_YAW] = axisPID[YAW];
|
|
|
|
// Reverse yaw servo when inverted in 3D mode
|
|
if (feature(FEATURE_3D) && (rcData[THROTTLE] < rxConfig->midrc)) {
|
|
input[INPUT_YAW] *= -1;
|
|
}
|
|
}
|
|
|
|
input[INPUT_GIMBAL_PITCH] = scaleRange(inclination.values.pitchDeciDegrees, -1800, 1800, -500, +500);
|
|
input[INPUT_GIMBAL_ROLL] = scaleRange(inclination.values.rollDeciDegrees, -1800, 1800, -500, +500);
|
|
|
|
input[INPUT_THROTTLE] = motor[0];
|
|
|
|
// 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_AUX1] = rcData[AUX1] - rxConfig->midrc;
|
|
input[INPUT_AUX2] = rcData[AUX2] - rxConfig->midrc;
|
|
input[INPUT_AUX3] = rcData[AUX3] - rxConfig->midrc;
|
|
input[INPUT_AUX4] = rcData[AUX4] - rxConfig->midrc;
|
|
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;
|
|
|
|
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].fromChannel; // FIXME rename 'from' to 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)
|
|
{
|
|
uint32_t i;
|
|
|
|
if (motorCount >= 4 && mixerConfig->yaw_jump_prevention_limit < YAW_JUMP_PREVENTION_LIMIT_HIGH) {
|
|
// prevent "yaw jump" during yaw correction
|
|
axisPID[YAW] = constrain(axisPID[YAW], -mixerConfig->yaw_jump_prevention_limit - ABS(rcCommand[YAW]), mixerConfig->yaw_jump_prevention_limit + ABS(rcCommand[YAW]));
|
|
}
|
|
|
|
// motors for non-servo mixes
|
|
for (i = 0; i < motorCount; i++) {
|
|
motor[i] =
|
|
rcCommand[THROTTLE] * currentMixer[i].throttle +
|
|
axisPID[PITCH] * currentMixer[i].pitch +
|
|
axisPID[ROLL] * currentMixer[i].roll +
|
|
-mixerConfig->yaw_direction * axisPID[YAW] * currentMixer[i].yaw;
|
|
}
|
|
|
|
#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_BI:
|
|
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 * inclination.values.pitchDeciDegrees) / 50) + determineServoMiddleOrForwardFromChannel(SERVO_GIMBAL_PITCH);
|
|
servo[SERVO_GIMBAL_ROLL] = (((int32_t)servoConf[SERVO_GIMBAL_ROLL].rate * inclination.values.rollDeciDegrees) / 50) + determineServoMiddleOrForwardFromChannel(SERVO_GIMBAL_ROLL);
|
|
break;
|
|
*/
|
|
|
|
default:
|
|
break;
|
|
}
|
|
|
|
// do camstab
|
|
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->gimbal_flags & GIMBAL_MIXTILT) {
|
|
servo[SERVO_GIMBAL_PITCH] -= (-(int32_t)servoConf[SERVO_GIMBAL_PITCH].rate) * inclination.values.pitchDeciDegrees / 50 - (int32_t)servoConf[SERVO_GIMBAL_ROLL].rate * inclination.values.rollDeciDegrees / 50;
|
|
servo[SERVO_GIMBAL_ROLL] += (-(int32_t)servoConf[SERVO_GIMBAL_PITCH].rate) * inclination.values.pitchDeciDegrees / 50 + (int32_t)servoConf[SERVO_GIMBAL_ROLL].rate * inclination.values.rollDeciDegrees / 50;
|
|
} else {
|
|
servo[SERVO_GIMBAL_PITCH] += (int32_t)servoConf[SERVO_GIMBAL_PITCH].rate * inclination.values.pitchDeciDegrees / 50;
|
|
servo[SERVO_GIMBAL_ROLL] += (int32_t)servoConf[SERVO_GIMBAL_ROLL].rate * inclination.values.rollDeciDegrees / 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
|
|
|
|
if (ARMING_FLAG(ARMED)) {
|
|
|
|
bool isFailsafeActive = failsafeIsActive();
|
|
|
|
// Find the maximum motor output.
|
|
int16_t maxMotor = motor[0];
|
|
for (i = 1; i < motorCount; i++) {
|
|
// If one motor is above the maxthrottle threshold, we reduce the value
|
|
// of all motors by the amount of overshoot. That way, only one motor
|
|
// is at max and the relative power of each motor is preserved.
|
|
if (motor[i] > maxMotor) {
|
|
maxMotor = motor[i];
|
|
}
|
|
}
|
|
|
|
int16_t maxThrottleDifference = 0;
|
|
if (maxMotor > escAndServoConfig->maxthrottle) {
|
|
maxThrottleDifference = maxMotor - escAndServoConfig->maxthrottle;
|
|
}
|
|
|
|
for (i = 0; i < motorCount; i++) {
|
|
// this is a way to still have good gyro corrections if at least one motor reaches its max.
|
|
motor[i] -= maxThrottleDifference;
|
|
|
|
if (feature(FEATURE_3D)) {
|
|
if ((rcData[THROTTLE]) > rxConfig->midrc) {
|
|
motor[i] = constrain(motor[i], flight3DConfig->deadband3d_high, escAndServoConfig->maxthrottle);
|
|
} else {
|
|
motor[i] = constrain(motor[i], escAndServoConfig->mincommand, flight3DConfig->deadband3d_low);
|
|
}
|
|
} else {
|
|
if (isFailsafeActive) {
|
|
motor[i] = constrain(motor[i], escAndServoConfig->mincommand, escAndServoConfig->maxthrottle);
|
|
} else {
|
|
// If we're at minimum throttle and FEATURE_MOTOR_STOP enabled,
|
|
// do not spin the motors.
|
|
motor[i] = constrain(motor[i], escAndServoConfig->minthrottle, escAndServoConfig->maxthrottle);
|
|
if ((rcData[THROTTLE]) < rxConfig->mincheck) {
|
|
if (feature(FEATURE_MOTOR_STOP)) {
|
|
motor[i] = escAndServoConfig->mincommand;
|
|
} else if (mixerConfig->pid_at_min_throttle == 0) {
|
|
motor[i] = escAndServoConfig->minthrottle;
|
|
}
|
|
}
|
|
}
|
|
}
|
|
}
|
|
} else {
|
|
for (i = 0; i < motorCount; i++) {
|
|
motor[i] = motor_disarmed[i];
|
|
}
|
|
}
|
|
}
|
|
|
|
#ifdef USE_SERVOS
|
|
bool isMixerUsingServos(void)
|
|
{
|
|
return useServo;
|
|
}
|
|
#endif
|
|
|
|
void filterServos(void)
|
|
{
|
|
#ifdef USE_SERVOS
|
|
int16_t servoIdx;
|
|
|
|
#if defined(MIXER_DEBUG)
|
|
uint32_t startTime = micros();
|
|
#endif
|
|
|
|
if (mixerConfig->servo_lowpass_enable) {
|
|
for (servoIdx = 0; servoIdx < MAX_SUPPORTED_SERVOS; servoIdx++) {
|
|
servo[servoIdx] = (int16_t)lowpassFixed(&lowpassFilters[servoIdx], servo[servoIdx], mixerConfig->servo_lowpass_freq);
|
|
|
|
// 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
|
|
}
|
|
|