mirror of
https://github.com/betaflight/betaflight.git
synced 2025-07-23 16:25:31 +03:00
Merge pull request #1066 from cleanflight/custom-servo-mixers
Custom servo mixers
This commit is contained in:
Dominic Clifton
commit
783a4c4bfa
19 changed files with 1031 additions and 520 deletions
|
|
@ -75,7 +75,8 @@ Re-apply any new defaults as desired.
|
|||
|------------------|------------------------------------------------|
|
||||
| `adjrange` | show/set adjustment ranges settings |
|
||||
| `aux` | show/set aux settings |
|
||||
| `cmix` | design custom mixer |
|
||||
| `mmix` | design custom motor mixer |
|
||||
| `smix` | design custom servo mixer |
|
||||
| `color` | configure colors |
|
||||
| `defaults` | reset to defaults and reboot |
|
||||
| `dump` | print configurable settings in a pastable form |
|
||||
|
|
@ -200,7 +201,7 @@ Re-apply any new defaults as desired.
|
|||
| `failsafe_throttle` | | 1000 | 2000 | 1200 | Profile | UINT16 |
|
||||
| `rx_min_usec` | Defines the shortest pulse width value used when ensuring the channel value is valid. If the receiver gives a pulse value lower than this value then the channel will be marked as bad and will default to the value of `mid_rc`. | 100 | 2000 | 985 | Profile | UINT16 |
|
||||
| `rx_max_usec` | Defines the longest pulse width value used when ensuring the channel value is valid. If the receiver gives a pulse value higher than this value then the channel will be marked as bad and will default to the value of `mid_rc`. | 100 | 3000 | 2115 | Profile | UINT16 |
|
||||
| `gimbal_flags` | When feature SERVO_TILT is enabled, this can be a combination of the following numbers: 1=normal gimbal (default), 2=tiltmix gimbal, 4=in PPM (or SERIALRX) input mode, this will forward AUX1..4 RC inputs to PWM5..8 pins | 0 | 255 | 1 | Profile | UINT8 |
|
||||
| `gimbal_mode` | When feature SERVO_TILT is enabled, this can be either 0=normal gimbal (default) or 1=tiltmix gimbal | 0 | 1 | 0 | Profile | UINT8 |
|
||||
| `acc_hardware` | This is used to suggest which accelerometer driver should load, or to force no accelerometer in case gyro-only flight is needed. Default (0) will attempt to auto-detect among enabled drivers. Otherwise, to force a particular device, set it to 2 for ADXL345, 3 for MPU6050 integrated accelerometer, 4 for MMA8452, 5 for BMA280, 6 for LSM303DLHC, 7 for SPI_MPU6000, 8 for SPI_MPU6500 or 1 to disable accelerometer alltogether - resulting in gyro-only operation. | 0 | 9 | 0 | Master | UINT8 |
|
||||
| `acc_lpf_factor` | This setting controls the Low Pass Filter factor for ACC. Increasing this value reduces ACC noise (visible in GUI), but would increase ACC lag time. Zero = no filter | 0 | 250 | 4 | Profile | UINT8 |
|
||||
| `accxy_deadband` | | 0 | 100 | 40 | Profile | UINT8 |
|
||||
|
|
|
|||
|
|
@ -98,3 +98,8 @@ reason: renamed to `3d_neutral` for consistency
|
|||
|
||||
### alt_hold_throttle_neutral
|
||||
reason: renamed to `alt_hold_deadband` for consistency
|
||||
|
||||
### gimbal_flags
|
||||
reason: seperation of features.
|
||||
|
||||
see `gimbal_mode` and `CHANNEL_FORWARDING` feature
|
||||
|
|
|
|||
|
|
@ -15,7 +15,7 @@ You can also use the Command Line Interface (CLI) to set the mixer type:
|
|||
## Supported Mixer Types
|
||||
|
||||
| Name | Description | Motors | Servos |
|
||||
| ------------- | ------------------------- | -------------- | ---------------- |
|
||||
| ---------------- | ------------------------- | -------------- | ---------------- |
|
||||
| TRI | Tricopter | M1-M3 | S1 |
|
||||
| QUADP | Quadcopter-Plus | M1-M4 | None |
|
||||
| QUADX | Quadcopter-X | M1-M4 | None |
|
||||
|
|
@ -39,6 +39,8 @@ You can also use the Command Line Interface (CLI) to set the mixer type:
|
|||
| SINGLECOPTER | Conventional helicopter | M1 | S1 |
|
||||
| ATAIL4 | Quadcopter with A-Tail | M1-M4 | N/A |
|
||||
| CUSTOM | User-defined | | |
|
||||
| CUSTOM AIRPLANE | User-defined airplane | | |
|
||||
| CUSTOM TRICOPTER | User-defined tricopter | | |
|
||||
|
||||
|
||||
## Servo filtering
|
||||
|
|
@ -76,8 +78,8 @@ Custom motor mixing allows for completely customized motor configurations. Each
|
|||
Steps to configure custom mixer in the CLI:
|
||||
|
||||
1. Use `mixer custom` to enable the custom mixing.
|
||||
2. Use `cmix reset` to erase the any existing custom mixing.
|
||||
3. Issue a cmix statement for each motor.
|
||||
2. Use `mmix reset` to erase the any existing custom mixing.
|
||||
3. Issue a `mmix` statement for each motor.
|
||||
|
||||
The cmix statement has the following syntax: `cmix n THROTTLE ROLL PITCH YAW`
|
||||
|
||||
|
|
@ -89,6 +91,42 @@ The cmix statement has the following syntax: `cmix n THROTTLE ROLL PITCH YAW`
|
|||
| PITCH | Indicates the pitch authority this motor has over the flight controller. Also accepts values nominally from 1.0 to -1.0. |
|
||||
| YAW | Indicates the direction of the motor rotation in relationship with the flight controller. 1.0 = CCW -1.0 = CW. |
|
||||
|
||||
Note: the `mmix` command may show a motor mix that is not active, custom motor mixes are only active for models that use custom mixers.
|
||||
|
||||
## Custom Servo Mixing
|
||||
|
||||
Custom servo mixing rules can be applied to each servo. Rules are applied in the order they are defined.
|
||||
|
||||
| id | Servo slot |
|
||||
| 0 | GIMBAL PITCH |
|
||||
| 1 | GIMBAL ROLL |
|
||||
| 2 | FLAPS |
|
||||
| 3 | FLAPPERON 1 (LEFT) / SINGLECOPTER_1 |
|
||||
| 4 | FLAPPERON 2 (RIGHT) / BICOPTER_LEFT / DUALCOPTER_LEFT / SINGLECOPTER_2 |
|
||||
| 5 | RUDDER / BICOPTER_RIGHT / DUALCOPTER_RIGHT / SINGLECOPTER_3 |
|
||||
| 6 | ELEVATOR / SINGLECOPTER_4 |
|
||||
| 7 | THROTTLE (Based ONLY on the first motor output) |
|
||||
|
||||
|
||||
| id | Input sources |
|
||||
| -- | ------------- |
|
||||
| 0 | Stabilised ROLL |
|
||||
| 1 | Stabilised PITCH |
|
||||
| 2 | Stabilised YAW |
|
||||
| 3 | Stabilised THROTTLE |
|
||||
| 4 | RC ROLL |
|
||||
| 5 | RC ITCH |
|
||||
| 6 | RC YAW |
|
||||
| 7 | RC THROTTLE |
|
||||
| 8 | RC AUX 1 |
|
||||
| 9 | RC AUX 2 |
|
||||
| 10 | RC AUX 3 |
|
||||
| 11 | RC AUX 4 |
|
||||
| 12 | GIMBAL PITCH |
|
||||
| 13 | GIMBAL ROLL |
|
||||
|
||||
Note: the `smix` command may show a servo mix that is not active, custom servo mixes are only active for models that use custom mixers.
|
||||
|
||||
### Example 1: A KK2.0 wired motor setup
|
||||
Here's an example of a X configuration quad, but the motors are still wired using the KK board motor numbering scheme.
|
||||
|
||||
|
|
@ -103,11 +141,11 @@ KK2.0 Motor Layout
|
|||
```
|
||||
|
||||
1. Use `mixer custom`
|
||||
2. Use `cmix reset`
|
||||
3. Use `cmix 1 1.0, 1.0, -1.0, -1.0` for the Front Left motor. It tells the flight controller the #1 motor is used, provides positive roll, provides negative pitch and is turning CW.
|
||||
4. Use `cmix 2 1.0, -1.0, -1.0, 1.0` for the Front Right motor. It still provides a negative pitch authority, but unlike the front left, it provides negative roll authority and turns CCW.
|
||||
5. Use `cmix 3 1.0, -1.0, 1.0, -1.0` for the Rear Right motor. It has negative roll, provides positive pitch when the speed is increased and turns CW.
|
||||
6. Use `cmix 4 1.0, 1.0, 1.0, 1.0` for the Rear Left motor. Increasing motor speed imparts positive roll, positive pitch and turns CCW.
|
||||
2. Use `mmix reset`
|
||||
3. Use `mmix 0 1.0, 1.0, -1.0, -1.0` for the Front Left motor. It tells the flight controller the #1 motor is used, provides positive roll, provides negative pitch and is turning CW.
|
||||
4. Use `mmix 1 1.0, -1.0, -1.0, 1.0` for the Front Right motor. It still provides a negative pitch authority, but unlike the front left, it provides negative roll authority and turns CCW.
|
||||
5. Use `mmix 2 1.0, -1.0, 1.0, -1.0` for the Rear Right motor. It has negative roll, provides positive pitch when the speed is increased and turns CW.
|
||||
6. Use `mmix 3 1.0, 1.0, 1.0, 1.0` for the Rear Left motor. Increasing motor speed imparts positive roll, positive pitch and turns CCW.
|
||||
|
||||
### Example 2: A HEX-U Copter
|
||||
|
||||
|
|
@ -121,13 +159,37 @@ HEX6-U
|
|||
.5...FC...2.
|
||||
............
|
||||
...6....1...
|
||||
|
||||
```
|
||||
|
||||
|Command| Roll | Pitch | Yaw |
|
||||
| ----- | ---- | ----- | --- |
|
||||
| Use `cmix 1 1.0, -0.5, 1.0, -1.0` | half negative | full positive | CW |
|
||||
| Use `cmix 2 1.0, -1.0, 0.0, 1.0` | full negative | none | CCW |
|
||||
| Use `cmix 3 1.0, -1.0, -1.0, -1.0` | full negative | full negative | CW |
|
||||
| Use `cmix 4 1.0, 1.0, -1.0, 1.0` | full positive | full negative | CCW |
|
||||
| Use `cmix 5 1.0, 1.0, 0.0, -1.0` | full positive | none | CW |
|
||||
| Use `cmix 6 1.0, 0.5, 1.0, 1.0` | half positive | full positive | CCW |
|
||||
| Use `mmix 1 1.0, -0.5, 1.0, -1.0` | half negative | full positive | CW |
|
||||
| Use `mmix 1 1.0, -1.0, 0.0, 1.0` | full negative | none | CCW |
|
||||
| Use `mmix 2 1.0, -1.0, -1.0, -1.0` | full negative | full negative | CW |
|
||||
| Use `mmix 3 1.0, 1.0, -1.0, 1.0` | full positive | full negative | CCW |
|
||||
| Use `mmix 4 1.0, 1.0, 0.0, -1.0` | full positive | none | CW |
|
||||
| Use `mmix 5 1.0, 0.5, 1.0, 1.0` | half positive | full positive | CCW |
|
||||
|
||||
### Example 3: Custom tricopter
|
||||
|
||||
```
|
||||
mixer CUSTOMTRI
|
||||
mmix reset
|
||||
mmix 0 1.000 0.000 1.333 0.000
|
||||
mmix 1 1.000 -1.000 -0.667 0.000
|
||||
mmix 2 1.000 1.000 -0.667 0.000
|
||||
smix reset
|
||||
smix 0 6 3 100 0 0 100 0
|
||||
```
|
||||
|
||||
|
||||
## Servo Reversing
|
||||
|
||||
Servos are reversed using the `smix reverse` command.
|
||||
|
||||
e.g. when using the TRI mixer to reverse the tail servo on a tricopter use this:
|
||||
|
||||
`smix reverse 5 2 r`
|
||||
|
||||
i.e. when mixing rudder servo slot (`5`) using Stabilised YAW input source (`2`) reverse the direction (`r`)
|
||||
|
||||
|
|
@ -366,7 +366,7 @@ static bool testBlackboxConditionUncached(FlightLogFieldCondition condition)
|
|||
return motorCount >= condition - FLIGHT_LOG_FIELD_CONDITION_AT_LEAST_MOTORS_1 + 1;
|
||||
|
||||
case FLIGHT_LOG_FIELD_CONDITION_TRICOPTER:
|
||||
return masterConfig.mixerMode == MIXER_TRI;
|
||||
return masterConfig.mixerMode == MIXER_TRI || masterConfig.mixerMode == MIXER_CUSTOM_TRI;
|
||||
|
||||
case FLIGHT_LOG_FIELD_CONDITION_NONZERO_PID_D_0:
|
||||
case FLIGHT_LOG_FIELD_CONDITION_NONZERO_PID_D_1:
|
||||
|
|
|
|||
|
|
@ -350,10 +350,6 @@ static void setControlRateProfile(uint8_t profileIndex)
|
|||
static void resetConf(void)
|
||||
{
|
||||
int i;
|
||||
#ifdef USE_SERVOS
|
||||
int8_t servoRates[MAX_SUPPORTED_SERVOS] = { 30, 30, 100, 100, 100, 100, 100, 100 };
|
||||
;
|
||||
#endif
|
||||
|
||||
// Clear all configuration
|
||||
memset(&masterConfig, 0, sizeof(master_t));
|
||||
|
|
@ -420,7 +416,6 @@ static void resetConf(void)
|
|||
|
||||
resetMixerConfig(&masterConfig.mixerConfig);
|
||||
|
||||
masterConfig.airplaneConfig.flaps_speed = 0;
|
||||
masterConfig.airplaneConfig.fixedwing_althold_dir = 1;
|
||||
|
||||
// Motor/ESC/Servo
|
||||
|
|
@ -485,14 +480,14 @@ static void resetConf(void)
|
|||
currentProfile->servoConf[i].min = DEFAULT_SERVO_MIN;
|
||||
currentProfile->servoConf[i].max = DEFAULT_SERVO_MAX;
|
||||
currentProfile->servoConf[i].middle = DEFAULT_SERVO_MIDDLE;
|
||||
currentProfile->servoConf[i].rate = servoRates[i];
|
||||
currentProfile->servoConf[i].rate = 100;
|
||||
currentProfile->servoConf[i].angleAtMin = DEFAULT_SERVO_MIN_ANGLE;
|
||||
currentProfile->servoConf[i].angleAtMax = DEFAULT_SERVO_MAX_ANGLE;
|
||||
currentProfile->servoConf[i].forwardFromChannel = CHANNEL_FORWARDING_DISABLED;
|
||||
}
|
||||
|
||||
// gimbal
|
||||
currentProfile->gimbalConfig.gimbal_flags = GIMBAL_NORMAL;
|
||||
currentProfile->gimbalConfig.mode = GIMBAL_MODE_NORMAL;
|
||||
#endif
|
||||
|
||||
#ifdef GPS
|
||||
|
|
@ -501,7 +496,7 @@ static void resetConf(void)
|
|||
|
||||
// custom mixer. clear by defaults.
|
||||
for (i = 0; i < MAX_SUPPORTED_MOTORS; i++)
|
||||
masterConfig.customMixer[i].throttle = 0.0f;
|
||||
masterConfig.customMotorMixer[i].throttle = 0.0f;
|
||||
|
||||
#ifdef LED_STRIP
|
||||
applyDefaultColors(masterConfig.colors, CONFIGURABLE_COLOR_COUNT);
|
||||
|
|
@ -547,52 +542,52 @@ static void resetConf(void)
|
|||
parseRcChannels("TAER1234", &masterConfig.rxConfig);
|
||||
|
||||
// { 1.0f, -0.5f, 1.0f, -1.0f }, // REAR_R
|
||||
masterConfig.customMixer[0].throttle = 1.0f;
|
||||
masterConfig.customMixer[0].roll = -0.5f;
|
||||
masterConfig.customMixer[0].pitch = 1.0f;
|
||||
masterConfig.customMixer[0].yaw = -1.0f;
|
||||
masterConfig.customMotorMixer[0].throttle = 1.0f;
|
||||
masterConfig.customMotorMixer[0].roll = -0.5f;
|
||||
masterConfig.customMotorMixer[0].pitch = 1.0f;
|
||||
masterConfig.customMotorMixer[0].yaw = -1.0f;
|
||||
|
||||
// { 1.0f, -0.5f, -1.0f, 1.0f }, // FRONT_R
|
||||
masterConfig.customMixer[1].throttle = 1.0f;
|
||||
masterConfig.customMixer[1].roll = -0.5f;
|
||||
masterConfig.customMixer[1].pitch = -1.0f;
|
||||
masterConfig.customMixer[1].yaw = 1.0f;
|
||||
masterConfig.customMotorMixer[1].throttle = 1.0f;
|
||||
masterConfig.customMotorMixer[1].roll = -0.5f;
|
||||
masterConfig.customMotorMixer[1].pitch = -1.0f;
|
||||
masterConfig.customMotorMixer[1].yaw = 1.0f;
|
||||
|
||||
// { 1.0f, 0.5f, 1.0f, 1.0f }, // REAR_L
|
||||
masterConfig.customMixer[2].throttle = 1.0f;
|
||||
masterConfig.customMixer[2].roll = 0.5f;
|
||||
masterConfig.customMixer[2].pitch = 1.0f;
|
||||
masterConfig.customMixer[2].yaw = 1.0f;
|
||||
masterConfig.customMotorMixer[2].throttle = 1.0f;
|
||||
masterConfig.customMotorMixer[2].roll = 0.5f;
|
||||
masterConfig.customMotorMixer[2].pitch = 1.0f;
|
||||
masterConfig.customMotorMixer[2].yaw = 1.0f;
|
||||
|
||||
// { 1.0f, 0.5f, -1.0f, -1.0f }, // FRONT_L
|
||||
masterConfig.customMixer[3].throttle = 1.0f;
|
||||
masterConfig.customMixer[3].roll = 0.5f;
|
||||
masterConfig.customMixer[3].pitch = -1.0f;
|
||||
masterConfig.customMixer[3].yaw = -1.0f;
|
||||
masterConfig.customMotorMixer[3].throttle = 1.0f;
|
||||
masterConfig.customMotorMixer[3].roll = 0.5f;
|
||||
masterConfig.customMotorMixer[3].pitch = -1.0f;
|
||||
masterConfig.customMotorMixer[3].yaw = -1.0f;
|
||||
|
||||
// { 1.0f, -1.0f, -0.5f, -1.0f }, // MIDFRONT_R
|
||||
masterConfig.customMixer[4].throttle = 1.0f;
|
||||
masterConfig.customMixer[4].roll = -1.0f;
|
||||
masterConfig.customMixer[4].pitch = -0.5f;
|
||||
masterConfig.customMixer[4].yaw = -1.0f;
|
||||
masterConfig.customMotorMixer[4].throttle = 1.0f;
|
||||
masterConfig.customMotorMixer[4].roll = -1.0f;
|
||||
masterConfig.customMotorMixer[4].pitch = -0.5f;
|
||||
masterConfig.customMotorMixer[4].yaw = -1.0f;
|
||||
|
||||
// { 1.0f, 1.0f, -0.5f, 1.0f }, // MIDFRONT_L
|
||||
masterConfig.customMixer[5].throttle = 1.0f;
|
||||
masterConfig.customMixer[5].roll = 1.0f;
|
||||
masterConfig.customMixer[5].pitch = -0.5f;
|
||||
masterConfig.customMixer[5].yaw = 1.0f;
|
||||
masterConfig.customMotorMixer[5].throttle = 1.0f;
|
||||
masterConfig.customMotorMixer[5].roll = 1.0f;
|
||||
masterConfig.customMotorMixer[5].pitch = -0.5f;
|
||||
masterConfig.customMotorMixer[5].yaw = 1.0f;
|
||||
|
||||
// { 1.0f, -1.0f, 0.5f, 1.0f }, // MIDREAR_R
|
||||
masterConfig.customMixer[6].throttle = 1.0f;
|
||||
masterConfig.customMixer[6].roll = -1.0f;
|
||||
masterConfig.customMixer[6].pitch = 0.5f;
|
||||
masterConfig.customMixer[6].yaw = 1.0f;
|
||||
masterConfig.customMotorMixer[6].throttle = 1.0f;
|
||||
masterConfig.customMotorMixer[6].roll = -1.0f;
|
||||
masterConfig.customMotorMixer[6].pitch = 0.5f;
|
||||
masterConfig.customMotorMixer[6].yaw = 1.0f;
|
||||
|
||||
// { 1.0f, 1.0f, 0.5f, -1.0f }, // MIDREAR_L
|
||||
masterConfig.customMixer[7].throttle = 1.0f;
|
||||
masterConfig.customMixer[7].roll = 1.0f;
|
||||
masterConfig.customMixer[7].pitch = 0.5f;
|
||||
masterConfig.customMixer[7].yaw = -1.0f;
|
||||
masterConfig.customMotorMixer[7].throttle = 1.0f;
|
||||
masterConfig.customMotorMixer[7].roll = 1.0f;
|
||||
masterConfig.customMotorMixer[7].pitch = 0.5f;
|
||||
masterConfig.customMotorMixer[7].yaw = -1.0f;
|
||||
#endif
|
||||
|
||||
// copy first profile into remaining profile
|
||||
|
|
|
|||
|
|
@ -42,7 +42,8 @@ typedef enum {
|
|||
FEATURE_LED_STRIP = 1 << 16,
|
||||
FEATURE_DISPLAY = 1 << 17,
|
||||
FEATURE_ONESHOT125 = 1 << 18,
|
||||
FEATURE_BLACKBOX = 1 << 19
|
||||
FEATURE_BLACKBOX = 1 << 19,
|
||||
FEATURE_CHANNEL_FORWARDING = 1 << 20
|
||||
} features_e;
|
||||
|
||||
void handleOneshotFeatureChangeOnRestart(void);
|
||||
|
|
|
|||
|
|
@ -28,8 +28,10 @@ typedef struct master_t {
|
|||
uint16_t looptime; // imu loop time in us
|
||||
uint8_t emf_avoidance; // change pll settings to avoid noise in the uhf band
|
||||
|
||||
motorMixer_t customMixer[MAX_SUPPORTED_MOTORS]; // custom mixtable
|
||||
|
||||
motorMixer_t customMotorMixer[MAX_SUPPORTED_MOTORS];
|
||||
#ifdef USE_SERVOS
|
||||
servoMixer_t customServoMixer[MAX_SERVO_RULES];
|
||||
#endif
|
||||
// motor/esc/servo related stuff
|
||||
escAndServoConfig_t escAndServoConfig;
|
||||
flight3DConfig_t flight3DConfig;
|
||||
|
|
|
|||
|
|
@ -502,7 +502,7 @@ pwmOutputConfiguration_t *pwmInit(drv_pwm_config_t *init)
|
|||
#endif
|
||||
}
|
||||
|
||||
if (init->extraServos && !init->airplane) {
|
||||
if (init->useChannelForwarding && !init->airplane) {
|
||||
#if defined(NAZE) && defined(LED_STRIP_TIMER)
|
||||
// if LED strip is active, PWM5-8 are unavailable, so map AUX1+AUX2 to PWM13+PWM14
|
||||
if (init->useLEDStrip) {
|
||||
|
|
|
|||
|
|
@ -63,7 +63,7 @@ typedef struct drv_pwm_config_t {
|
|||
#endif
|
||||
#ifdef USE_SERVOS
|
||||
bool useServos;
|
||||
bool extraServos; // configure additional 4 channels in PPM mode as servos, not motors
|
||||
bool useChannelForwarding; // configure additional channels as servos
|
||||
uint16_t servoPwmRate;
|
||||
uint16_t servoCenterPulse;
|
||||
#endif
|
||||
|
|
|
|||
|
|
@ -18,6 +18,7 @@
|
|||
#include <stdbool.h>
|
||||
#include <stdint.h>
|
||||
#include <stdlib.h>
|
||||
#include <string.h>
|
||||
|
||||
#include "platform.h"
|
||||
#include "debug.h"
|
||||
|
|
@ -62,8 +63,8 @@ typedef enum {
|
|||
SERVO_ELEVATOR = 6,
|
||||
SERVO_THROTTLE = 7, // for internal combustion (IC) planes
|
||||
|
||||
SERVO_BIPLANE_LEFT = 4,
|
||||
SERVO_BIPLANE_RIGHT = 5,
|
||||
SERVO_BICOPTER_LEFT = 4,
|
||||
SERVO_BICOPTER_RIGHT = 5,
|
||||
|
||||
SERVO_DUALCOPTER_LEFT = 4,
|
||||
SERVO_DUALCOPTER_RIGHT = 5,
|
||||
|
|
@ -76,7 +77,7 @@ typedef enum {
|
|||
} servoIndex_e;
|
||||
|
||||
#define SERVO_PLANE_INDEX_MIN SERVO_FLAPS
|
||||
#define SERVO_PLANE_INDEX_MAX SERVO_ELEVATOR
|
||||
#define SERVO_PLANE_INDEX_MAX SERVO_THROTTLE
|
||||
|
||||
#define SERVO_DUALCOPTER_INDEX_MIN SERVO_DUALCOPTER_LEFT
|
||||
#define SERVO_DUALCOPTER_INDEX_MAX SERVO_DUALCOPTER_RIGHT
|
||||
|
|
@ -90,6 +91,7 @@ typedef enum {
|
|||
//#define MIXER_DEBUG
|
||||
|
||||
uint8_t motorCount = 0;
|
||||
|
||||
int16_t motor[MAX_SUPPORTED_MOTORS];
|
||||
int16_t motor_disarmed[MAX_SUPPORTED_MOTORS];
|
||||
|
||||
|
|
@ -99,10 +101,13 @@ static escAndServoConfig_t *escAndServoConfig;
|
|||
static airplaneConfig_t *airplaneConfig;
|
||||
static rxConfig_t *rxConfig;
|
||||
|
||||
static motorMixer_t currentMixer[MAX_SUPPORTED_MOTORS];
|
||||
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;
|
||||
|
|
@ -118,7 +123,7 @@ static const motorMixer_t mixerQuadX[] = {
|
|||
{ 1.0f, 1.0f, -1.0f, -1.0f }, // FRONT_L
|
||||
};
|
||||
#ifndef USE_QUAD_MIXER_ONLY
|
||||
static const motorMixer_t mixerTri[] = {
|
||||
static const motorMixer_t mixerTricopter[] = {
|
||||
{ 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
|
||||
|
|
@ -131,7 +136,7 @@ static const motorMixer_t mixerQuadP[] = {
|
|||
{ 1.0f, 0.0f, -1.0f, -1.0f }, // FRONT
|
||||
};
|
||||
|
||||
static const motorMixer_t mixerBi[] = {
|
||||
static const motorMixer_t mixerBicopter[] = {
|
||||
{ 1.0f, 1.0f, 0.0f, 0.0f }, // LEFT
|
||||
{ 1.0f, -1.0f, 0.0f, 0.0f }, // RIGHT
|
||||
};
|
||||
|
|
@ -231,36 +236,128 @@ static const motorMixer_t mixerDualcopter[] = {
|
|||
{ 1.0f, 0.0f, 0.0f, 1.0f }, // RIGHT
|
||||
};
|
||||
|
||||
static const motorMixer_t mixerSingleProp[] = {
|
||||
{ 1.0f, 0.0f, 0.0f, 0.0f },
|
||||
};
|
||||
|
||||
// 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
|
||||
{ 1, 1, NULL }, // * 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
|
||||
// motors, use servo, motor mixer
|
||||
{ 0, false, NULL }, // entry 0
|
||||
{ 3, true, mixerTricopter }, // MIXER_TRI
|
||||
{ 4, false, mixerQuadP }, // MIXER_QUADP
|
||||
{ 4, false, mixerQuadX }, // MIXER_QUADX
|
||||
{ 2, true, mixerBicopter }, // MIXER_BICOPTER
|
||||
{ 0, true, NULL }, // * MIXER_GIMBAL
|
||||
{ 6, false, mixerY6 }, // MIXER_Y6
|
||||
{ 6, false, mixerHex6P }, // MIXER_HEX6
|
||||
{ 1, true, mixerSingleProp }, // * MIXER_FLYING_WING
|
||||
{ 4, false, mixerY4 }, // MIXER_Y4
|
||||
{ 6, false, mixerHex6X }, // MIXER_HEX6X
|
||||
{ 8, false, mixerOctoX8 }, // MIXER_OCTOX8
|
||||
{ 8, false, mixerOctoFlatP }, // MIXER_OCTOFLATP
|
||||
{ 8, false, mixerOctoFlatX }, // MIXER_OCTOFLATX
|
||||
{ 1, true, mixerSingleProp }, // * MIXER_AIRPLANE
|
||||
{ 0, true, NULL }, // * MIXER_HELI_120_CCPM
|
||||
{ 0, true, NULL }, // * MIXER_HELI_90_DEG
|
||||
{ 4, false, mixerVtail4 }, // MIXER_VTAIL4
|
||||
{ 6, false, mixerHex6H }, // MIXER_HEX6H
|
||||
{ 0, true, NULL }, // * MIXER_PPM_TO_SERVO
|
||||
{ 2, true, mixerDualcopter }, // MIXER_DUALCOPTER
|
||||
{ 1, true, NULL }, // MIXER_SINGLECOPTER
|
||||
{ 4, false, mixerAtail4 }, // MIXER_ATAIL4
|
||||
{ 0, false, NULL }, // MIXER_CUSTOM
|
||||
{ 2, true, NULL }, // MIXER_CUSTOM_AIRPLANE
|
||||
{ 3, true, NULL }, // MIXER_CUSTOM_TRI
|
||||
};
|
||||
#endif
|
||||
|
||||
#ifdef USE_SERVOS
|
||||
|
||||
#define COUNT_SERVO_RULES(rules) (sizeof(rules) / sizeof(servoMixer_t))
|
||||
// mixer rule format servo, input, rate, speed, min, max, box
|
||||
static const servoMixer_t servoMixerAirplane[] = {
|
||||
{ SERVO_FLAPPERON_1, INPUT_STABILIZED_ROLL, 100, 0, 0, 100, 0 },
|
||||
{ SERVO_FLAPPERON_2, INPUT_STABILIZED_ROLL, 100, 0, 0, 100, 0 },
|
||||
{ SERVO_RUDDER, INPUT_STABILIZED_YAW, 100, 0, 0, 100, 0 },
|
||||
{ SERVO_ELEVATOR, INPUT_STABILIZED_PITCH, 100, 0, 0, 100, 0 },
|
||||
{ SERVO_THROTTLE, INPUT_STABILIZED_THROTTLE, 100, 0, 0, 100, 0 },
|
||||
};
|
||||
|
||||
static const servoMixer_t servoMixerFlyingWing[] = {
|
||||
{ SERVO_FLAPPERON_1, INPUT_STABILIZED_ROLL, 100, 0, 0, 100, 0 },
|
||||
{ SERVO_FLAPPERON_1, INPUT_STABILIZED_PITCH, 100, 0, 0, 100, 0 },
|
||||
{ SERVO_FLAPPERON_2, INPUT_STABILIZED_ROLL, -100, 0, 0, 100, 0 },
|
||||
{ SERVO_FLAPPERON_2, INPUT_STABILIZED_PITCH, 100, 0, 0, 100, 0 },
|
||||
{ SERVO_THROTTLE, INPUT_STABILIZED_THROTTLE, 100, 0, 0, 100, 0 },
|
||||
};
|
||||
|
||||
static const servoMixer_t servoMixerBI[] = {
|
||||
{ SERVO_BICOPTER_LEFT, INPUT_STABILIZED_YAW, 100, 0, 0, 100, 0 },
|
||||
{ SERVO_BICOPTER_LEFT, INPUT_STABILIZED_PITCH, 100, 0, 0, 100, 0 },
|
||||
{ SERVO_BICOPTER_RIGHT, INPUT_STABILIZED_YAW, 100, 0, 0, 100, 0 },
|
||||
{ SERVO_BICOPTER_RIGHT, INPUT_STABILIZED_PITCH, 100, 0, 0, 100, 0 },
|
||||
};
|
||||
|
||||
static const servoMixer_t servoMixerTri[] = {
|
||||
{ SERVO_RUDDER, INPUT_STABILIZED_YAW, 100, 0, 0, 100, 0 },
|
||||
};
|
||||
|
||||
static const servoMixer_t servoMixerDual[] = {
|
||||
{ SERVO_DUALCOPTER_LEFT, INPUT_STABILIZED_PITCH, 100, 0, 0, 100, 0 },
|
||||
{ SERVO_DUALCOPTER_RIGHT, INPUT_STABILIZED_ROLL, 100, 0, 0, 100, 0 },
|
||||
};
|
||||
|
||||
static const servoMixer_t servoMixerSingle[] = {
|
||||
{ SERVO_SINGLECOPTER_1, INPUT_STABILIZED_YAW, 100, 0, 0, 100, 0 },
|
||||
{ SERVO_SINGLECOPTER_1, INPUT_STABILIZED_PITCH, 100, 0, 0, 100, 0 },
|
||||
{ SERVO_SINGLECOPTER_2, INPUT_STABILIZED_YAW, 100, 0, 0, 100, 0 },
|
||||
{ SERVO_SINGLECOPTER_2, INPUT_STABILIZED_PITCH, 100, 0, 0, 100, 0 },
|
||||
{ SERVO_SINGLECOPTER_3, INPUT_STABILIZED_YAW, 100, 0, 0, 100, 0 },
|
||||
{ SERVO_SINGLECOPTER_3, INPUT_STABILIZED_ROLL, 100, 0, 0, 100, 0 },
|
||||
{ SERVO_SINGLECOPTER_4, INPUT_STABILIZED_YAW, 100, 0, 0, 100, 0 },
|
||||
{ SERVO_SINGLECOPTER_4, INPUT_STABILIZED_ROLL, 100, 0, 0, 100, 0 },
|
||||
};
|
||||
|
||||
static const servoMixer_t servoMixerGimbal[] = {
|
||||
{ SERVO_GIMBAL_PITCH, INPUT_GIMBAL_PITCH, 125, 0, 0, 100, 0 },
|
||||
{ SERVO_GIMBAL_ROLL, INPUT_GIMBAL_ROLL, 125, 0, 0, 100, 0 },
|
||||
};
|
||||
|
||||
|
||||
const mixerRules_t servoMixers[] = {
|
||||
{ 0, NULL }, // entry 0
|
||||
{ COUNT_SERVO_RULES(servoMixerTri), servoMixerTri }, // MULTITYPE_TRI
|
||||
{ 0, NULL }, // MULTITYPE_QUADP
|
||||
{ 0, NULL }, // MULTITYPE_QUADX
|
||||
{ COUNT_SERVO_RULES(servoMixerBI), servoMixerBI }, // MULTITYPE_BI
|
||||
{ COUNT_SERVO_RULES(servoMixerGimbal), servoMixerGimbal }, // * MULTITYPE_GIMBAL
|
||||
{ 0, NULL }, // MULTITYPE_Y6
|
||||
{ 0, NULL }, // MULTITYPE_HEX6
|
||||
{ COUNT_SERVO_RULES(servoMixerFlyingWing), servoMixerFlyingWing },// * MULTITYPE_FLYING_WING
|
||||
{ 0, NULL }, // MULTITYPE_Y4
|
||||
{ 0, NULL }, // MULTITYPE_HEX6X
|
||||
{ 0, NULL }, // MULTITYPE_OCTOX8
|
||||
{ 0, NULL }, // MULTITYPE_OCTOFLATP
|
||||
{ 0, NULL }, // MULTITYPE_OCTOFLATX
|
||||
{ COUNT_SERVO_RULES(servoMixerAirplane), servoMixerAirplane }, // * MULTITYPE_AIRPLANE
|
||||
{ 0, NULL }, // * MULTITYPE_HELI_120_CCPM
|
||||
{ 0, NULL }, // * MULTITYPE_HELI_90_DEG
|
||||
{ 0, NULL }, // MULTITYPE_VTAIL4
|
||||
{ 0, NULL }, // MULTITYPE_HEX6H
|
||||
{ 0, NULL }, // * MULTITYPE_PPM_TO_SERVO
|
||||
{ COUNT_SERVO_RULES(servoMixerDual), servoMixerDual }, // MULTITYPE_DUALCOPTER
|
||||
{ COUNT_SERVO_RULES(servoMixerSingle), servoMixerSingle }, // MULTITYPE_SINGLECOPTER
|
||||
{ 0, NULL }, // MULTITYPE_ATAIL4
|
||||
{ 0, NULL }, // MULTITYPE_CUSTOM
|
||||
{ 0, NULL }, // MULTITYPE_CUSTOM_PLANE
|
||||
{ 0, NULL }, // MULTITYPE_CUSTOM_TRI
|
||||
};
|
||||
|
||||
static servoMixer_t *customServoMixers;
|
||||
#endif
|
||||
|
||||
|
||||
static motorMixer_t *customMixers;
|
||||
|
||||
void mixerUseConfigs(
|
||||
|
|
@ -297,15 +394,11 @@ int16_t determineServoMiddleOrForwardFromChannel(servoIndex_e servoIndex)
|
|||
return servoConf[servoIndex].middle;
|
||||
}
|
||||
|
||||
int servoDirection(servoIndex_e servoIndex, int lr)
|
||||
|
||||
int servoDirection(int servoIndex, int inputSource)
|
||||
{
|
||||
// servo.rate is overloaded for servos that don't have a rate, but only need direction
|
||||
// bit set = negative, clear = positive
|
||||
// rate[2] = ???_direction
|
||||
// rate[1] = roll_direction
|
||||
// rate[0] = pitch_direction
|
||||
// servo.rate is also used as gimbal gain multiplier (yeah)
|
||||
if (servoConf[servoIndex].rate & lr)
|
||||
// determine the direction (reversed or not) from the direction bitfield of the servo
|
||||
if (servoConf[servoIndex].reversedSources & (1 << inputSource))
|
||||
return -1;
|
||||
else
|
||||
return 1;
|
||||
|
|
@ -313,11 +406,32 @@ int servoDirection(servoIndex_e servoIndex, int lr)
|
|||
#endif
|
||||
|
||||
#ifndef USE_QUAD_MIXER_ONLY
|
||||
void mixerInit(mixerMode_e mixerMode, motorMixer_t *initialCustomMixers)
|
||||
|
||||
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 = initialCustomMixers;
|
||||
customMixers = initialCustomMotorMixers;
|
||||
customServoMixers = initialCustomServoMixers;
|
||||
|
||||
// enable servos for mixes that require them. note, this shifts motor counts.
|
||||
useServo = mixers[currentMixerMode].useServo;
|
||||
|
|
@ -337,7 +451,7 @@ void mixerUsePWMOutputConfiguration(pwmOutputConfiguration_t *pwmOutputConfigura
|
|||
|
||||
servoCount = pwmOutputConfiguration->servoCount;
|
||||
|
||||
if (currentMixerMode == MIXER_CUSTOM) {
|
||||
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++) {
|
||||
// check if done
|
||||
|
|
@ -355,6 +469,14 @@ void mixerUsePWMOutputConfiguration(pwmOutputConfiguration_t *pwmOutputConfigura
|
|||
}
|
||||
}
|
||||
|
||||
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) {
|
||||
|
|
@ -368,12 +490,38 @@ void mixerUsePWMOutputConfiguration(pwmOutputConfiguration_t *pwmOutputConfigura
|
|||
|
||||
// set flag that we're on something with wings
|
||||
if (currentMixerMode == MIXER_FLYING_WING ||
|
||||
currentMixerMode == MIXER_AIRPLANE)
|
||||
currentMixerMode == MIXER_AIRPLANE ||
|
||||
currentMixerMode == MIXER_CUSTOM_AIRPLANE
|
||||
) {
|
||||
ENABLE_STATE(FIXED_WING);
|
||||
else
|
||||
|
||||
if (currentMixerMode == MIXER_CUSTOM_AIRPLANE) {
|
||||
loadCustomServoMixer();
|
||||
}
|
||||
} else {
|
||||
DISABLE_STATE(FIXED_WING);
|
||||
|
||||
mixerResetMotors();
|
||||
if (currentMixerMode == MIXER_CUSTOM_TRI) {
|
||||
loadCustomServoMixer();
|
||||
}
|
||||
}
|
||||
|
||||
mixerResetDisarmedMotors();
|
||||
}
|
||||
|
||||
|
||||
void servoMixerLoadMix(int index, servoMixer_t *customServoMixers)
|
||||
{
|
||||
int i;
|
||||
|
||||
// we're 1-based
|
||||
index++;
|
||||
// clear existing
|
||||
for (i = 0; i < MAX_SERVO_RULES; i++)
|
||||
customServoMixers[i].targetChannel = customServoMixers[i].inputSource = customServoMixers[i].rate = customServoMixers[i].box = 0;
|
||||
|
||||
for (i = 0; i < servoMixers[index].servoRuleCount; i++)
|
||||
customServoMixers[i] = servoMixers[index].rule[i];
|
||||
}
|
||||
|
||||
void mixerLoadMix(int index, motorMixer_t *customMixers)
|
||||
|
|
@ -415,11 +563,11 @@ void mixerUsePWMOutputConfiguration(pwmOutputConfiguration_t *pwmOutputConfigura
|
|||
currentMixer[i] = mixerQuadX[i];
|
||||
}
|
||||
|
||||
mixerResetMotors();
|
||||
mixerResetDisarmedMotors();
|
||||
}
|
||||
#endif
|
||||
|
||||
void mixerResetMotors(void)
|
||||
void mixerResetDisarmedMotors(void)
|
||||
{
|
||||
int i;
|
||||
// set disarmed motor values
|
||||
|
|
@ -451,12 +599,13 @@ void writeServos(void)
|
|||
uint8_t servoIndex = 0;
|
||||
|
||||
switch (currentMixerMode) {
|
||||
case MIXER_BI:
|
||||
pwmWriteServo(servoIndex++, servo[SERVO_BIPLANE_LEFT]);
|
||||
pwmWriteServo(servoIndex++, servo[SERVO_BIPLANE_RIGHT]);
|
||||
case MIXER_BICOPTER:
|
||||
pwmWriteServo(servoIndex++, servo[SERVO_BICOPTER_LEFT]);
|
||||
pwmWriteServo(servoIndex++, servo[SERVO_BICOPTER_RIGHT]);
|
||||
break;
|
||||
|
||||
case MIXER_TRI:
|
||||
case MIXER_CUSTOM_TRI:
|
||||
if (mixerConfig->tri_unarmed_servo) {
|
||||
// if unarmed flag set, we always move servo
|
||||
pwmWriteServo(servoIndex++, servo[SERVO_RUDDER]);
|
||||
|
|
@ -479,6 +628,7 @@ void writeServos(void)
|
|||
pwmWriteServo(servoIndex++, servo[SERVO_DUALCOPTER_RIGHT]);
|
||||
break;
|
||||
|
||||
case MIXER_CUSTOM_AIRPLANE:
|
||||
case MIXER_AIRPLANE:
|
||||
for (int i = SERVO_PLANE_INDEX_MIN; i <= SERVO_PLANE_INDEX_MAX; i++) {
|
||||
pwmWriteServo(servoIndex++, servo[i]);
|
||||
|
|
@ -496,13 +646,13 @@ void writeServos(void)
|
|||
}
|
||||
|
||||
// Two servos for SERVO_TILT, if enabled
|
||||
if (feature(FEATURE_SERVO_TILT)) {
|
||||
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) {
|
||||
if (feature(FEATURE_CHANNEL_FORWARDING)) {
|
||||
forwardAuxChannelsToServos(servoIndex);
|
||||
servoIndex += MAX_AUX_CHANNEL_COUNT;
|
||||
}
|
||||
|
|
@ -545,52 +695,83 @@ void StopPwmAllMotors()
|
|||
}
|
||||
|
||||
#ifndef USE_QUAD_MIXER_ONLY
|
||||
static void airplaneMixer(void)
|
||||
static void servoMixer(void)
|
||||
{
|
||||
int16_t flapperons[2] = { 0, 0 };
|
||||
int i;
|
||||
int16_t input[INPUT_SOURCE_COUNT]; // Range [-500:+500]
|
||||
static int16_t currentOutput[MAX_SERVO_RULES];
|
||||
uint8_t i;
|
||||
|
||||
if (!ARMING_FLAG(ARMED))
|
||||
servo[SERVO_THROTTLE] = escAndServoConfig->mincommand; // Kill throttle when disarmed
|
||||
else
|
||||
servo[SERVO_THROTTLE] = constrain(rcCommand[THROTTLE], escAndServoConfig->minthrottle, escAndServoConfig->maxthrottle);
|
||||
motor[0] = servo[SERVO_THROTTLE];
|
||||
|
||||
if (airplaneConfig->flaps_speed) {
|
||||
// configure SERVO3 middle point in GUI to using an AUX channel for FLAPS control
|
||||
// use servo min, servo max and servo rate for proper endpoints adjust
|
||||
static int16_t slow_LFlaps;
|
||||
int16_t lFlap = determineServoMiddleOrForwardFromChannel(SERVO_FLAPS);
|
||||
|
||||
lFlap = constrain(lFlap, servoConf[SERVO_FLAPS].min, servoConf[SERVO_FLAPS].max);
|
||||
lFlap = escAndServoConfig->servoCenterPulse - lFlap;
|
||||
if (slow_LFlaps < lFlap)
|
||||
slow_LFlaps += airplaneConfig->flaps_speed;
|
||||
else if (slow_LFlaps > lFlap)
|
||||
slow_LFlaps -= airplaneConfig->flaps_speed;
|
||||
|
||||
servo[SERVO_FLAPS] = ((int32_t)servoConf[SERVO_FLAPS].rate * slow_LFlaps) / 100L;
|
||||
servo[SERVO_FLAPS] += escAndServoConfig->servoCenterPulse;
|
||||
}
|
||||
|
||||
if (FLIGHT_MODE(PASSTHRU_MODE)) { // Direct passthru from RX
|
||||
servo[SERVO_FLAPPERON_1] = rcCommand[ROLL] + flapperons[0];
|
||||
servo[SERVO_FLAPPERON_2] = rcCommand[ROLL] + flapperons[1];
|
||||
servo[SERVO_RUDDER] = rcCommand[YAW];
|
||||
servo[SERVO_ELEVATOR] = rcCommand[PITCH];
|
||||
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
|
||||
servo[SERVO_FLAPPERON_1] = axisPID[ROLL] + flapperons[0];
|
||||
servo[SERVO_FLAPPERON_2] = axisPID[ROLL] + flapperons[1];
|
||||
servo[SERVO_RUDDER] = axisPID[YAW];
|
||||
servo[SERVO_ELEVATOR] = axisPID[PITCH];
|
||||
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;
|
||||
}
|
||||
}
|
||||
|
||||
for (i = SERVO_PLANE_INDEX_MIN; i <= SERVO_PLANE_INDEX_MAX; i++) {
|
||||
servo[i] = ((int32_t)servoConf[i].rate * servo[i]) / 100L; // servo rates
|
||||
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_STABILIZED_THROTTLE] = motor[0] - 1000 - 500; // Since it derives from rcCommand or mincommand and must be [-500:+500]
|
||||
|
||||
// center the RC input value around the RC middle value
|
||||
// by subtracting the RC middle value from the RC input value, we get:
|
||||
// data - middle = input
|
||||
// 2000 - 1500 = +500
|
||||
// 1500 - 1500 = 0
|
||||
// 1000 - 1500 = -500
|
||||
input[INPUT_RC_ROLL] = rcData[ROLL] - rxConfig->midrc;
|
||||
input[INPUT_RC_PITCH] = rcData[PITCH] - rxConfig->midrc;
|
||||
input[INPUT_RC_YAW] = rcData[YAW] - rxConfig->midrc;
|
||||
input[INPUT_RC_THROTTLE] = rcData[THROTTLE] - rxConfig->midrc;
|
||||
input[INPUT_RC_AUX1] = rcData[AUX1] - rxConfig->midrc;
|
||||
input[INPUT_RC_AUX2] = rcData[AUX2] - rxConfig->midrc;
|
||||
input[INPUT_RC_AUX3] = rcData[AUX3] - rxConfig->midrc;
|
||||
input[INPUT_RC_AUX4] = rcData[AUX4] - rxConfig->midrc;
|
||||
|
||||
for (i = 0; i < MAX_SUPPORTED_SERVOS; i++)
|
||||
servo[i] = 0;
|
||||
|
||||
// mix servos according to rules
|
||||
for (i = 0; i < servoRuleCount; i++) {
|
||||
// consider rule if no box assigned or box is active
|
||||
if (currentServoMixer[i].box == 0 || IS_RC_MODE_ACTIVE(BOXSERVO1 + currentServoMixer[i].box - 1)) {
|
||||
uint8_t target = currentServoMixer[i].targetChannel;
|
||||
uint8_t from = currentServoMixer[i].inputSource;
|
||||
uint16_t servo_width = servoConf[target].max - servoConf[target].min;
|
||||
int16_t min = currentServoMixer[i].min * servo_width / 100 - servo_width / 2;
|
||||
int16_t max = currentServoMixer[i].max * servo_width / 100 - servo_width / 2;
|
||||
|
||||
if (currentServoMixer[i].speed == 0)
|
||||
currentOutput[i] = input[from];
|
||||
else {
|
||||
if (currentOutput[i] < input[from])
|
||||
currentOutput[i] = constrain(currentOutput[i] + currentServoMixer[i].speed, currentOutput[i], input[from]);
|
||||
else if (currentOutput[i] > input[from])
|
||||
currentOutput[i] = constrain(currentOutput[i] - currentServoMixer[i].speed, input[from], currentOutput[i]);
|
||||
}
|
||||
|
||||
servo[target] += servoDirection(target, from) * constrain(((int32_t)currentOutput[i] * currentServoMixer[i].rate) / 100, min, max);
|
||||
} else {
|
||||
currentOutput[i] = 0;
|
||||
}
|
||||
}
|
||||
|
||||
for (i = 0; i < MAX_SUPPORTED_SERVOS; i++) {
|
||||
servo[i] = ((int32_t)servoConf[i].rate * servo[i]) / 100L;
|
||||
servo[i] += determineServoMiddleOrForwardFromChannel(i);
|
||||
}
|
||||
}
|
||||
|
||||
#endif
|
||||
|
||||
void mixTable(void)
|
||||
|
|
@ -598,12 +779,11 @@ 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 (500 is disabled jump protection)
|
||||
// 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
|
||||
if (motorCount > 1) {
|
||||
for (i = 0; i < motorCount; i++) {
|
||||
motor[i] =
|
||||
rcCommand[THROTTLE] * currentMixer[i].throttle +
|
||||
|
|
@ -611,100 +791,6 @@ void mixTable(void)
|
|||
axisPID[ROLL] * currentMixer[i].roll +
|
||||
-mixerConfig->yaw_direction * axisPID[YAW] * currentMixer[i].yaw;
|
||||
}
|
||||
}
|
||||
|
||||
#if !defined(USE_QUAD_MIXER_ONLY) || defined(USE_SERVOS)
|
||||
int8_t yawDirection3D = 1;
|
||||
|
||||
// Reverse yaw servo when inverted in 3D mode
|
||||
if (feature(FEATURE_3D) && (rcData[THROTTLE] < rxConfig->midrc)) {
|
||||
yawDirection3D = -1;
|
||||
}
|
||||
|
||||
// airplane / servo mixes
|
||||
switch (currentMixerMode) {
|
||||
case MIXER_BI:
|
||||
servo[SERVO_BIPLANE_LEFT] = (servoDirection(SERVO_BIPLANE_LEFT, 2) * axisPID[YAW]) + (servoDirection(SERVO_BIPLANE_LEFT, 1) * axisPID[PITCH]) + determineServoMiddleOrForwardFromChannel(SERVO_BIPLANE_LEFT);
|
||||
servo[SERVO_BIPLANE_RIGHT] = (servoDirection(SERVO_BIPLANE_RIGHT, 2) * axisPID[YAW]) + (servoDirection(SERVO_BIPLANE_RIGHT, 1) * axisPID[PITCH]) + determineServoMiddleOrForwardFromChannel(SERVO_BIPLANE_RIGHT);
|
||||
break;
|
||||
|
||||
case MIXER_TRI:
|
||||
servo[SERVO_RUDDER] = (servoDirection(SERVO_RUDDER, 1) * axisPID[YAW] * yawDirection3D) + determineServoMiddleOrForwardFromChannel(SERVO_RUDDER);
|
||||
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;
|
||||
|
||||
case MIXER_AIRPLANE:
|
||||
airplaneMixer();
|
||||
break;
|
||||
|
||||
case MIXER_FLYING_WING:
|
||||
if (!ARMING_FLAG(ARMED))
|
||||
servo[SERVO_THROTTLE] = escAndServoConfig->mincommand;
|
||||
else
|
||||
servo[SERVO_THROTTLE] = constrain(rcCommand[THROTTLE], escAndServoConfig->minthrottle, escAndServoConfig->maxthrottle);
|
||||
|
||||
motor[0] = servo[SERVO_THROTTLE];
|
||||
|
||||
if (FLIGHT_MODE(PASSTHRU_MODE)) {
|
||||
// do not use sensors for correction, simple 2 channel mixing
|
||||
servo[SERVO_FLAPPERON_1] = (servoDirection(SERVO_FLAPPERON_1, 1) * rcCommand[PITCH]) + (servoDirection(SERVO_FLAPPERON_1, 2) * rcCommand[ROLL]);
|
||||
servo[SERVO_FLAPPERON_2] = (servoDirection(SERVO_FLAPPERON_2, 1) * rcCommand[PITCH]) + (servoDirection(SERVO_FLAPPERON_2, 2) * rcCommand[ROLL]);
|
||||
} else {
|
||||
// use sensors to correct (gyro only or gyro + acc)
|
||||
servo[SERVO_FLAPPERON_1] = (servoDirection(SERVO_FLAPPERON_1, 1) * axisPID[PITCH]) + (servoDirection(SERVO_FLAPPERON_1, 2) * axisPID[ROLL]);
|
||||
servo[SERVO_FLAPPERON_2] = (servoDirection(SERVO_FLAPPERON_2, 1) * axisPID[PITCH]) + (servoDirection(SERVO_FLAPPERON_2, 2) * axisPID[ROLL]);
|
||||
}
|
||||
servo[SERVO_FLAPPERON_1] += determineServoMiddleOrForwardFromChannel(SERVO_FLAPPERON_1);
|
||||
servo[SERVO_FLAPPERON_2] += determineServoMiddleOrForwardFromChannel(SERVO_FLAPPERON_2);
|
||||
break;
|
||||
|
||||
case MIXER_DUALCOPTER:
|
||||
for (i = SERVO_DUALCOPTER_INDEX_MIN; i <= SERVO_DUALCOPTER_INDEX_MAX; i++) {
|
||||
servo[i] = axisPID[SERVO_DUALCOPTER_INDEX_MAX - i] * servoDirection(i, 1); // mix and setup direction
|
||||
servo[i] += determineServoMiddleOrForwardFromChannel(i);
|
||||
}
|
||||
break;
|
||||
|
||||
case MIXER_SINGLECOPTER:
|
||||
for (i = SERVO_SINGLECOPTER_INDEX_MIN; i <= SERVO_SINGLECOPTER_INDEX_MAX; i++) {
|
||||
servo[i] = (axisPID[YAW] * servoDirection(i, 2)) + (axisPID[(SERVO_SINGLECOPTER_INDEX_MAX - i) >> 1] * servoDirection(i, 1)); // mix and setup direction
|
||||
servo[i] += determineServoMiddleOrForwardFromChannel(i);
|
||||
}
|
||||
motor[0] = rcCommand[THROTTLE];
|
||||
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(0);
|
||||
servo[SERVO_GIMBAL_ROLL] = determineServoMiddleOrForwardFromChannel(1);
|
||||
|
||||
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
|
||||
if (useServo) {
|
||||
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)) {
|
||||
|
||||
|
|
@ -758,6 +844,58 @@ void mixTable(void)
|
|||
motor[i] = motor_disarmed[i];
|
||||
}
|
||||
}
|
||||
|
||||
// motor outputs are used as sources for servo mixing, so motors must be calculated before servos.
|
||||
|
||||
#if !defined(USE_QUAD_MIXER_ONLY) || defined(USE_SERVOS)
|
||||
|
||||
// airplane / servo mixes
|
||||
switch (currentMixerMode) {
|
||||
case MIXER_CUSTOM_AIRPLANE:
|
||||
case MIXER_FLYING_WING:
|
||||
case MIXER_AIRPLANE:
|
||||
case MIXER_BICOPTER:
|
||||
case MIXER_CUSTOM_TRI:
|
||||
case MIXER_TRI:
|
||||
case MIXER_DUALCOPTER:
|
||||
case MIXER_SINGLECOPTER:
|
||||
case MIXER_GIMBAL:
|
||||
servoMixer();
|
||||
break;
|
||||
|
||||
/*
|
||||
case MIXER_GIMBAL:
|
||||
servo[SERVO_GIMBAL_PITCH] = (((int32_t)servoConf[SERVO_GIMBAL_PITCH].rate * 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;
|
||||
}
|
||||
|
||||
// camera stabilization
|
||||
if (feature(FEATURE_SERVO_TILT)) {
|
||||
// center at fixed position, or vary either pitch or roll by RC channel
|
||||
servo[SERVO_GIMBAL_PITCH] = determineServoMiddleOrForwardFromChannel(SERVO_GIMBAL_PITCH);
|
||||
servo[SERVO_GIMBAL_ROLL] = determineServoMiddleOrForwardFromChannel(SERVO_GIMBAL_ROLL);
|
||||
|
||||
if (IS_RC_MODE_ACTIVE(BOXCAMSTAB)) {
|
||||
if (gimbalConfig->mode == GIMBAL_MODE_MIXTILT) {
|
||||
servo[SERVO_GIMBAL_PITCH] -= (-(int32_t)servoConf[SERVO_GIMBAL_PITCH].rate) * 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
|
||||
}
|
||||
|
||||
#ifdef USE_SERVOS
|
||||
|
|
|
|||
|
|
@ -29,7 +29,7 @@ typedef enum mixerMode
|
|||
MIXER_TRI = 1,
|
||||
MIXER_QUADP = 2,
|
||||
MIXER_QUADX = 3,
|
||||
MIXER_BI = 4,
|
||||
MIXER_BICOPTER = 4,
|
||||
MIXER_GIMBAL = 5,
|
||||
MIXER_Y6 = 6,
|
||||
MIXER_HEX6 = 7,
|
||||
|
|
@ -48,7 +48,9 @@ typedef enum mixerMode
|
|||
MIXER_DUALCOPTER = 20,
|
||||
MIXER_SINGLECOPTER = 21,
|
||||
MIXER_ATAIL4 = 22,
|
||||
MIXER_CUSTOM = 23
|
||||
MIXER_CUSTOM = 23,
|
||||
MIXER_CUSTOM_AIRPLANE = 24,
|
||||
MIXER_CUSTOM_TRI = 25
|
||||
} mixerMode_e;
|
||||
|
||||
// Custom mixer data per motor
|
||||
|
|
@ -85,18 +87,59 @@ typedef struct flight3DConfig_s {
|
|||
} flight3DConfig_t;
|
||||
|
||||
typedef struct airplaneConfig_t {
|
||||
uint8_t flaps_speed; // airplane mode flaps, 0 = no flaps, > 0 = flap speed, larger = faster
|
||||
int8_t fixedwing_althold_dir; // +1 or -1 for pitch/althold gain. later check if need more than just sign
|
||||
} airplaneConfig_t;
|
||||
|
||||
#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;
|
||||
|
||||
typedef struct servoMixer_t {
|
||||
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_t {
|
||||
uint8_t servoRuleCount;
|
||||
const servoMixer_t *rule;
|
||||
} mixerRules_t;
|
||||
|
||||
typedef struct servoParam_t {
|
||||
int16_t min; // servo min
|
||||
int16_t max; // servo max
|
||||
int16_t middle; // servo middle
|
||||
int8_t rate; // range [-100;+100] ; can be used to adjust a rate 0-100% and a direction
|
||||
int8_t rate; // range [-125;+125] ; can be used to adjust a rate 0-125% and a direction
|
||||
uint32_t reversedSources; // the direction of servo movement for each input source of the servo mixer, bit set=inverted
|
||||
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
|
||||
|
|
@ -128,7 +171,12 @@ void mixerUseConfigs(
|
|||
|
||||
void writeAllMotors(int16_t mc);
|
||||
void mixerLoadMix(int index, motorMixer_t *customMixers);
|
||||
void mixerResetMotors(void);
|
||||
#ifdef USE_SERVOS
|
||||
void servoMixerLoadMix(int index, servoMixer_t *customServoMixers);
|
||||
void loadCustomServoMixer(void);
|
||||
int servoDirection(int servoIndex, int fromChannel);
|
||||
#endif
|
||||
void mixerResetDisarmedMotors(void);
|
||||
void mixTable(void);
|
||||
void writeMotors(void);
|
||||
void stopMotors(void);
|
||||
|
|
|
|||
|
|
@ -17,12 +17,13 @@
|
|||
|
||||
#pragma once
|
||||
|
||||
typedef enum GimbalFlags {
|
||||
GIMBAL_NORMAL = 1 << 0,
|
||||
GIMBAL_MIXTILT = 1 << 1,
|
||||
GIMBAL_FORWARDAUX = 1 << 2,
|
||||
} GimbalFlags;
|
||||
typedef enum {
|
||||
GIMBAL_MODE_NORMAL = 0,
|
||||
GIMBAL_MODE_MIXTILT = 1
|
||||
} gimbalMode_e;
|
||||
|
||||
#define GIMBAL_MODE_MAX (GIMBAL_MODE_MIXTILT)
|
||||
|
||||
typedef struct gimbalConfig_s {
|
||||
uint8_t gimbal_flags; // in servotilt mode, various things that affect stuff
|
||||
uint8_t mode;
|
||||
} gimbalConfig_t;
|
||||
|
|
|
|||
|
|
@ -43,12 +43,15 @@ typedef enum {
|
|||
BOXTELEMETRY,
|
||||
BOXAUTOTUNE,
|
||||
BOXSONAR,
|
||||
BOXSERVO1,
|
||||
BOXSERVO2,
|
||||
BOXSERVO3,
|
||||
CHECKBOX_ITEM_COUNT
|
||||
} boxId_e;
|
||||
|
||||
extern uint32_t rcModeActivationMask;
|
||||
|
||||
#define IS_RC_MODE_ACTIVE(modeId) ((1 << modeId) & rcModeActivationMask)
|
||||
#define IS_RC_MODE_ACTIVE(modeId) ((1 << (modeId)) & rcModeActivationMask)
|
||||
#define ACTIVATE_RC_MODE(modeId) (rcModeActivationMask |= (1 << modeId))
|
||||
|
||||
typedef enum rc_alias {
|
||||
|
|
|
|||
|
|
@ -98,7 +98,7 @@ static serialPort_t *cliPort;
|
|||
|
||||
static void cliAux(char *cmdline);
|
||||
static void cliAdjustmentRange(char *cmdline);
|
||||
static void cliCMix(char *cmdline);
|
||||
static void cliMotorMix(char *cmdline);
|
||||
static void cliDefaults(char *cmdline);
|
||||
static void cliDump(char *cmdLine);
|
||||
static void cliExit(char *cmdline);
|
||||
|
|
@ -111,6 +111,7 @@ static void cliReboot(void);
|
|||
static void cliSave(char *cmdline);
|
||||
static void cliSerial(char *cmdline);
|
||||
static void cliServo(char *cmdline);
|
||||
static void cliServoMix(char *cmdline);
|
||||
static void cliSet(char *cmdline);
|
||||
static void cliGet(char *cmdline);
|
||||
static void cliStatus(char *cmdline);
|
||||
|
|
@ -151,7 +152,7 @@ static const char * const mixerNames[] = {
|
|||
"FLYING_WING", "Y4", "HEX6X", "OCTOX8", "OCTOFLATP", "OCTOFLATX",
|
||||
"AIRPLANE", "HELI_120_CCPM", "HELI_90_DEG", "VTAIL4",
|
||||
"HEX6H", "PPM_TO_SERVO", "DUALCOPTER", "SINGLECOPTER",
|
||||
"ATAIL4", "CUSTOM", NULL
|
||||
"ATAIL4", "CUSTOM", "CUSTOMAIRPLANE", "CUSTOMTRI", NULL
|
||||
};
|
||||
#endif
|
||||
|
||||
|
|
@ -161,7 +162,7 @@ static const char * const featureNames[] = {
|
|||
"SERVO_TILT", "SOFTSERIAL", "GPS", "FAILSAFE",
|
||||
"SONAR", "TELEMETRY", "CURRENT_METER", "3D", "RX_PARALLEL_PWM",
|
||||
"RX_MSP", "RSSI_ADC", "LED_STRIP", "DISPLAY", "ONESHOT125",
|
||||
"BLACKBOX", NULL
|
||||
"BLACKBOX", "CHANNEL_FORWARDING", NULL
|
||||
};
|
||||
|
||||
#ifndef CJMCU
|
||||
|
|
@ -182,52 +183,91 @@ static const char * const sensorHardwareNames[4][11] = {
|
|||
|
||||
typedef struct {
|
||||
const char *name;
|
||||
const char *param;
|
||||
const char *description;
|
||||
const char *args;
|
||||
|
||||
void (*func)(char *cmdline);
|
||||
} clicmd_t;
|
||||
|
||||
#ifndef SKIP_CLI_COMMAND_HELP
|
||||
#define CLI_COMMAND_DEF(name, description, args, method) \
|
||||
{ \
|
||||
name , \
|
||||
description , \
|
||||
args , \
|
||||
method \
|
||||
}
|
||||
#else
|
||||
#define CLI_COMMAND_DEF(name, description, args, method) \
|
||||
{ \
|
||||
name, \
|
||||
NULL, \
|
||||
NULL, \
|
||||
method \
|
||||
}
|
||||
#endif
|
||||
|
||||
// should be sorted a..z for bsearch()
|
||||
const clicmd_t cmdTable[] = {
|
||||
{ "adjrange", "show/set adjustment ranges settings", cliAdjustmentRange },
|
||||
{ "aux", "show/set aux settings", cliAux },
|
||||
{ "cmix", "design custom mixer", cliCMix },
|
||||
CLI_COMMAND_DEF("adjrange", "configure adjustment ranges", NULL, cliAdjustmentRange),
|
||||
CLI_COMMAND_DEF("aux", "configure modes", NULL, cliAux),
|
||||
#ifdef LED_STRIP
|
||||
{ "color", "configure colors", cliColor },
|
||||
CLI_COMMAND_DEF("color", "configure colors", NULL, cliColor),
|
||||
#endif
|
||||
{ "defaults", "reset to defaults and reboot", cliDefaults },
|
||||
{ "dump", "dump configuration", cliDump },
|
||||
{ "exit", "", cliExit },
|
||||
{ "feature", "list or -val or val", cliFeature },
|
||||
CLI_COMMAND_DEF("defaults", "reset to defaults and reboot", NULL, cliDefaults),
|
||||
CLI_COMMAND_DEF("dump", "dump configuration",
|
||||
"[master|profile]", cliDump),
|
||||
CLI_COMMAND_DEF("exit", NULL, NULL, cliExit),
|
||||
CLI_COMMAND_DEF("feature", "configure features",
|
||||
"list\r\n"
|
||||
"\t<+|->[name]", cliFeature),
|
||||
#ifdef USE_FLASHFS
|
||||
{ "flash_erase", "erase flash chip", cliFlashErase },
|
||||
{ "flash_info", "get flash chip details", cliFlashInfo },
|
||||
{ "flash_read", "read text from the given address", cliFlashRead },
|
||||
{ "flash_write", "write text to the given address", cliFlashWrite },
|
||||
CLI_COMMAND_DEF("flash_erase", "erase flash chip", NULL, cliFlashErase),
|
||||
CLI_COMMAND_DEF("flash_info", "show flash chip info", NULL, cliFlashInfo),
|
||||
CLI_COMMAND_DEF("flash_read", NULL, "<length> <address>", cliFlashRead),
|
||||
CLI_COMMAND_DEF("flash_write", NULL, "<address> <message>", cliFlashWrite),
|
||||
#endif
|
||||
{ "get", "get variable value", cliGet },
|
||||
CLI_COMMAND_DEF("get", "get variable value",
|
||||
"[name]", cliGet),
|
||||
#ifdef GPS
|
||||
{ "gpspassthrough", "passthrough gps to serial", cliGpsPassthrough },
|
||||
CLI_COMMAND_DEF("gpspassthrough", "passthrough gps to serial", NULL, cliGpsPassthrough),
|
||||
#endif
|
||||
{ "help", "", cliHelp },
|
||||
CLI_COMMAND_DEF("help", NULL, NULL, cliHelp),
|
||||
#ifdef LED_STRIP
|
||||
{ "led", "configure leds", cliLed },
|
||||
CLI_COMMAND_DEF("led", "configure leds", NULL, cliLed),
|
||||
#endif
|
||||
{ "map", "mapping of rc channel order", cliMap },
|
||||
CLI_COMMAND_DEF("map", "configure rc channel order",
|
||||
"[<map>]", cliMap),
|
||||
#ifndef USE_QUAD_MIXER_ONLY
|
||||
{ "mixer", "mixer name or list", cliMixer },
|
||||
CLI_COMMAND_DEF("mixer", "configure mixer",
|
||||
"list\r\n"
|
||||
"\t<name>", cliMixer),
|
||||
#endif
|
||||
{ "motor", "get/set motor output value", cliMotor },
|
||||
{ "play_sound", "index, or none for next", cliPlaySound },
|
||||
{ "profile", "index (0 to 2)", cliProfile },
|
||||
{ "rateprofile", "index (0 to 2)", cliRateProfile },
|
||||
{ "save", "save and reboot", cliSave },
|
||||
{ "serial", "show/set serial settings", cliSerial },
|
||||
CLI_COMMAND_DEF("mmix", "custom motor mixer", NULL, cliMotorMix),
|
||||
CLI_COMMAND_DEF("motor", "get/set motor",
|
||||
"<index> [<value>]", cliMotor),
|
||||
CLI_COMMAND_DEF("play_sound", NULL,
|
||||
"[<index>]\r\n", cliPlaySound),
|
||||
CLI_COMMAND_DEF("profile", "change profile",
|
||||
"[<index>]", cliProfile),
|
||||
CLI_COMMAND_DEF("rateprofile", "change rate profile",
|
||||
"[<index>]", cliRateProfile),
|
||||
CLI_COMMAND_DEF("save", "save and reboot", NULL, cliSave),
|
||||
CLI_COMMAND_DEF("serial", "configure serial ports", NULL, cliSerial),
|
||||
#ifdef USE_SERVOS
|
||||
{ "servo", "servo config", cliServo },
|
||||
CLI_COMMAND_DEF("servo", "configure servos", NULL, cliServo),
|
||||
#endif
|
||||
{ "set", "name=value or blank or * for list", cliSet },
|
||||
{ "status", "show system status", cliStatus },
|
||||
{ "version", "", cliVersion },
|
||||
CLI_COMMAND_DEF("set", "change setting",
|
||||
"[<name>=<value>]", cliSet),
|
||||
#ifdef USE_SERVOS
|
||||
CLI_COMMAND_DEF("smix", "servo mixer",
|
||||
"<rule> <servo> <source> <rate> <speed> <min> <max> <box>\r\n"
|
||||
"\treset\r\n"
|
||||
"\tload <mixer>\r\n"
|
||||
"\treverse <servo> <source> r|n", cliServoMix),
|
||||
#endif
|
||||
CLI_COMMAND_DEF("status", "show status", NULL, cliStatus),
|
||||
CLI_COMMAND_DEF("version", "show version", NULL, cliVersion),
|
||||
};
|
||||
#define CMD_COUNT (sizeof(cmdTable) / sizeof(clicmd_t))
|
||||
|
||||
|
|
@ -285,8 +325,6 @@ const clivalue_t valueTable[] = {
|
|||
{ "auto_disarm_delay", VAR_UINT8 | MASTER_VALUE, &masterConfig.auto_disarm_delay, 0, 60 },
|
||||
{ "small_angle", VAR_UINT8 | MASTER_VALUE, &masterConfig.small_angle, 0, 180 },
|
||||
|
||||
{ "flaps_speed", VAR_UINT8 | MASTER_VALUE, &masterConfig.airplaneConfig.flaps_speed, 0, 100 },
|
||||
|
||||
{ "fixedwing_althold_dir", VAR_INT8 | MASTER_VALUE, &masterConfig.airplaneConfig.fixedwing_althold_dir, -1, 1 },
|
||||
|
||||
{ "reboot_character", VAR_UINT8 | MASTER_VALUE, &masterConfig.serialConfig.reboot_character, 48, 126 },
|
||||
|
|
@ -389,7 +427,7 @@ const clivalue_t valueTable[] = {
|
|||
{ "rx_max_usec", VAR_UINT16 | MASTER_VALUE, &masterConfig.rxConfig.rx_max_usec, PWM_PULSE_MIN, PWM_PULSE_MAX },
|
||||
|
||||
#ifdef USE_SERVOS
|
||||
{ "gimbal_flags", VAR_UINT8 | PROFILE_VALUE, &masterConfig.profile[0].gimbalConfig.gimbal_flags, 0, 255},
|
||||
{ "gimbal_mode", VAR_UINT8 | PROFILE_VALUE, &masterConfig.profile[0].gimbalConfig.mode, 0, GIMBAL_MODE_MAX},
|
||||
#endif
|
||||
|
||||
{ "acc_hardware", VAR_UINT8 | MASTER_VALUE, &masterConfig.acc_hardware, 0, ACC_MAX },
|
||||
|
|
@ -471,38 +509,42 @@ static void cliSetVar(const clivalue_t *var, const int_float_value_t value);
|
|||
static void cliPrintVar(const clivalue_t *var, uint32_t full);
|
||||
static void cliPrint(const char *str);
|
||||
static void cliWrite(uint8_t ch);
|
||||
|
||||
static void cliPrompt(void)
|
||||
{
|
||||
cliPrint("\r\n# ");
|
||||
}
|
||||
|
||||
static int cliCompare(const void *a, const void *b)
|
||||
static void cliShowParseError(void)
|
||||
{
|
||||
const clicmd_t *ca = a, *cb = b;
|
||||
return strncasecmp(ca->name, cb->name, strlen(cb->name));
|
||||
cliPrint("Parse error\r\n");
|
||||
}
|
||||
|
||||
static void cliShowArgumentRangeError(char *name, int min, int max)
|
||||
{
|
||||
printf("%s must be between %d and %d\r\n", name, min, max);
|
||||
}
|
||||
|
||||
static char *processChannelRangeArgs(char *ptr, channelRange_t *range, uint8_t *validArgumentCount)
|
||||
{
|
||||
int val;
|
||||
|
||||
for (int argIndex = 0; argIndex < 2; argIndex++) {
|
||||
ptr = strchr(ptr, ' ');
|
||||
if (ptr) {
|
||||
val = atoi(++ptr);
|
||||
val = CHANNEL_VALUE_TO_STEP(val);
|
||||
if (val >= MIN_MODE_RANGE_STEP && val <= MAX_MODE_RANGE_STEP) {
|
||||
if (argIndex == 0) {
|
||||
range->startStep = val;
|
||||
(*validArgumentCount)++;
|
||||
}
|
||||
}
|
||||
ptr = strchr(ptr, ' ');
|
||||
if (ptr) {
|
||||
val = atoi(++ptr);
|
||||
val = CHANNEL_VALUE_TO_STEP(val);
|
||||
if (val >= MIN_MODE_RANGE_STEP && val <= MAX_MODE_RANGE_STEP) {
|
||||
} else {
|
||||
range->endStep = val;
|
||||
}
|
||||
(*validArgumentCount)++;
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
return ptr;
|
||||
}
|
||||
|
||||
|
|
@ -557,7 +599,7 @@ static void cliAux(char *cmdline)
|
|||
memset(mac, 0, sizeof(modeActivationCondition_t));
|
||||
}
|
||||
} else {
|
||||
printf("index: must be < %u\r\n", MAX_MODE_ACTIVATION_CONDITION_COUNT);
|
||||
cliShowArgumentRangeError("index", 0, MAX_MODE_ACTIVATION_CONDITION_COUNT - 1);
|
||||
}
|
||||
}
|
||||
}
|
||||
|
|
@ -651,7 +693,7 @@ static void cliSerial(char *cmdline)
|
|||
}
|
||||
|
||||
if (validArgumentCount < 6) {
|
||||
cliPrint("Parse error\r\n");
|
||||
cliShowParseError();
|
||||
return;
|
||||
}
|
||||
|
||||
|
|
@ -684,6 +726,7 @@ static void cliAdjustmentRange(char *cmdline)
|
|||
if (i < MAX_ADJUSTMENT_RANGE_COUNT) {
|
||||
adjustmentRange_t *ar = ¤tProfile->adjustmentRanges[i];
|
||||
uint8_t validArgumentCount = 0;
|
||||
|
||||
ptr = strchr(ptr, ' ');
|
||||
if (ptr) {
|
||||
val = atoi(++ptr);
|
||||
|
|
@ -700,7 +743,9 @@ static void cliAdjustmentRange(char *cmdline)
|
|||
validArgumentCount++;
|
||||
}
|
||||
}
|
||||
|
||||
ptr = processChannelRangeArgs(ptr, &ar->range, &validArgumentCount);
|
||||
|
||||
ptr = strchr(ptr, ' ');
|
||||
if (ptr) {
|
||||
val = atoi(++ptr);
|
||||
|
|
@ -720,14 +765,15 @@ static void cliAdjustmentRange(char *cmdline)
|
|||
|
||||
if (validArgumentCount != 6) {
|
||||
memset(ar, 0, sizeof(adjustmentRange_t));
|
||||
cliShowParseError();
|
||||
}
|
||||
} else {
|
||||
printf("index: must be < %u\r\n", MAX_ADJUSTMENT_RANGE_COUNT);
|
||||
cliShowArgumentRangeError("index", 0, MAX_ADJUSTMENT_RANGE_COUNT - 1);
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
static void cliCMix(char *cmdline)
|
||||
static void cliMotorMix(char *cmdline)
|
||||
{
|
||||
#ifdef USE_QUAD_MIXER_ONLY
|
||||
UNUSED(cmdline);
|
||||
|
|
@ -736,49 +782,38 @@ static void cliCMix(char *cmdline)
|
|||
int num_motors = 0;
|
||||
uint8_t len;
|
||||
char buf[16];
|
||||
float mixsum[3];
|
||||
char *ptr;
|
||||
|
||||
if (isEmpty(cmdline)) {
|
||||
cliPrint("Custom mixer: \r\nMotor\tThr\tRoll\tPitch\tYaw\r\n");
|
||||
cliPrint("Motor\tThr\tRoll\tPitch\tYaw\r\n");
|
||||
for (i = 0; i < MAX_SUPPORTED_MOTORS; i++) {
|
||||
if (masterConfig.customMixer[i].throttle == 0.0f)
|
||||
if (masterConfig.customMotorMixer[i].throttle == 0.0f)
|
||||
break;
|
||||
num_motors++;
|
||||
printf("#%d:\t", i + 1);
|
||||
printf("%s\t", ftoa(masterConfig.customMixer[i].throttle, buf));
|
||||
printf("%s\t", ftoa(masterConfig.customMixer[i].roll, buf));
|
||||
printf("%s\t", ftoa(masterConfig.customMixer[i].pitch, buf));
|
||||
printf("%s\r\n", ftoa(masterConfig.customMixer[i].yaw, buf));
|
||||
printf("#%d:\t", i);
|
||||
printf("%s\t", ftoa(masterConfig.customMotorMixer[i].throttle, buf));
|
||||
printf("%s\t", ftoa(masterConfig.customMotorMixer[i].roll, buf));
|
||||
printf("%s\t", ftoa(masterConfig.customMotorMixer[i].pitch, buf));
|
||||
printf("%s\r\n", ftoa(masterConfig.customMotorMixer[i].yaw, buf));
|
||||
}
|
||||
mixsum[0] = mixsum[1] = mixsum[2] = 0.0f;
|
||||
for (i = 0; i < num_motors; i++) {
|
||||
mixsum[0] += masterConfig.customMixer[i].roll;
|
||||
mixsum[1] += masterConfig.customMixer[i].pitch;
|
||||
mixsum[2] += masterConfig.customMixer[i].yaw;
|
||||
}
|
||||
cliPrint("Sanity check:\t");
|
||||
for (i = 0; i < 3; i++)
|
||||
cliPrint(fabsf(mixsum[i]) > 0.01f ? "NG\t" : "OK\t");
|
||||
cliPrint("\r\n");
|
||||
return;
|
||||
} else if (strncasecmp(cmdline, "reset", 5) == 0) {
|
||||
// erase custom mixer
|
||||
for (i = 0; i < MAX_SUPPORTED_MOTORS; i++)
|
||||
masterConfig.customMixer[i].throttle = 0.0f;
|
||||
masterConfig.customMotorMixer[i].throttle = 0.0f;
|
||||
} else if (strncasecmp(cmdline, "load", 4) == 0) {
|
||||
ptr = strchr(cmdline, ' ');
|
||||
if (ptr) {
|
||||
len = strlen(++ptr);
|
||||
for (i = 0; ; i++) {
|
||||
if (mixerNames[i] == NULL) {
|
||||
cliPrint("Invalid mixer type\r\n");
|
||||
cliPrint("Invalid name\r\n");
|
||||
break;
|
||||
}
|
||||
if (strncasecmp(ptr, mixerNames[i], len) == 0) {
|
||||
mixerLoadMix(i, masterConfig.customMixer);
|
||||
printf("Loaded %s mix\r\n", mixerNames[i]);
|
||||
cliCMix("");
|
||||
mixerLoadMix(i, masterConfig.customMotorMixer);
|
||||
printf("Loaded %s\r\n", mixerNames[i]);
|
||||
cliMotorMix("");
|
||||
break;
|
||||
}
|
||||
}
|
||||
|
|
@ -786,34 +821,34 @@ static void cliCMix(char *cmdline)
|
|||
} else {
|
||||
ptr = cmdline;
|
||||
i = atoi(ptr); // get motor number
|
||||
if (--i < MAX_SUPPORTED_MOTORS) {
|
||||
if (i < MAX_SUPPORTED_MOTORS) {
|
||||
ptr = strchr(ptr, ' ');
|
||||
if (ptr) {
|
||||
masterConfig.customMixer[i].throttle = fastA2F(++ptr);
|
||||
masterConfig.customMotorMixer[i].throttle = fastA2F(++ptr);
|
||||
check++;
|
||||
}
|
||||
ptr = strchr(ptr, ' ');
|
||||
if (ptr) {
|
||||
masterConfig.customMixer[i].roll = fastA2F(++ptr);
|
||||
masterConfig.customMotorMixer[i].roll = fastA2F(++ptr);
|
||||
check++;
|
||||
}
|
||||
ptr = strchr(ptr, ' ');
|
||||
if (ptr) {
|
||||
masterConfig.customMixer[i].pitch = fastA2F(++ptr);
|
||||
masterConfig.customMotorMixer[i].pitch = fastA2F(++ptr);
|
||||
check++;
|
||||
}
|
||||
ptr = strchr(ptr, ' ');
|
||||
if (ptr) {
|
||||
masterConfig.customMixer[i].yaw = fastA2F(++ptr);
|
||||
masterConfig.customMotorMixer[i].yaw = fastA2F(++ptr);
|
||||
check++;
|
||||
}
|
||||
if (check != 4) {
|
||||
cliPrint("Wrong number of arguments, needs idx thr roll pitch yaw\r\n");
|
||||
cliShowParseError();
|
||||
} else {
|
||||
cliCMix("");
|
||||
cliMotorMix("");
|
||||
}
|
||||
} else {
|
||||
printf("Motor number must be between 1 and %d\r\n", MAX_SUPPORTED_MOTORS);
|
||||
cliShowArgumentRangeError("index", 1, MAX_SUPPORTED_MOTORS);
|
||||
}
|
||||
}
|
||||
#endif
|
||||
|
|
@ -837,10 +872,10 @@ static void cliLed(char *cmdline)
|
|||
if (i < MAX_LED_STRIP_LENGTH) {
|
||||
ptr = strchr(cmdline, ' ');
|
||||
if (!parseLedStripConfig(i, ++ptr)) {
|
||||
cliPrint("Parse error\r\n");
|
||||
cliShowParseError();
|
||||
}
|
||||
} else {
|
||||
printf("Invalid led index: must be < %u\r\n", MAX_LED_STRIP_LENGTH);
|
||||
cliShowArgumentRangeError("index", 0, MAX_LED_STRIP_LENGTH - 1);
|
||||
}
|
||||
}
|
||||
}
|
||||
|
|
@ -852,7 +887,12 @@ static void cliColor(char *cmdline)
|
|||
|
||||
if (isEmpty(cmdline)) {
|
||||
for (i = 0; i < CONFIGURABLE_COLOR_COUNT; i++) {
|
||||
printf("color %u %d,%u,%u\r\n", i, masterConfig.colors[i].h, masterConfig.colors[i].s, masterConfig.colors[i].v);
|
||||
printf("color %u %d,%u,%u\r\n",
|
||||
i,
|
||||
masterConfig.colors[i].h,
|
||||
masterConfig.colors[i].s,
|
||||
masterConfig.colors[i].v
|
||||
);
|
||||
}
|
||||
} else {
|
||||
ptr = cmdline;
|
||||
|
|
@ -860,10 +900,10 @@ static void cliColor(char *cmdline)
|
|||
if (i < CONFIGURABLE_COLOR_COUNT) {
|
||||
ptr = strchr(cmdline, ' ');
|
||||
if (!parseColor(i, ++ptr)) {
|
||||
cliPrint("Parse error\r\n");
|
||||
cliShowParseError();
|
||||
}
|
||||
} else {
|
||||
printf("Invalid color index: must be < %u\r\n", CONFIGURABLE_COLOR_COUNT);
|
||||
cliShowArgumentRangeError("index", 0, CONFIGURABLE_COLOR_COUNT - 1);
|
||||
}
|
||||
}
|
||||
}
|
||||
|
|
@ -909,7 +949,7 @@ static void cliServo(char *cmdline)
|
|||
while (*ptr) {
|
||||
if (*ptr == '-' || (*ptr >= '0' && *ptr <= '9')) {
|
||||
if (validArgumentCount >= SERVO_ARGUMENT_COUNT) {
|
||||
cliPrint("Parse error\r\n");
|
||||
cliShowParseError();
|
||||
return;
|
||||
}
|
||||
|
||||
|
|
@ -921,14 +961,14 @@ static void cliServo(char *cmdline)
|
|||
} else if (*ptr == ' ') {
|
||||
ptr++;
|
||||
} else {
|
||||
cliPrint("Parse error\r\n");
|
||||
cliShowParseError();
|
||||
return;
|
||||
}
|
||||
}
|
||||
|
||||
// Check we got the right number of args and the servo index is correct (don't validate the other values)
|
||||
if (validArgumentCount != SERVO_ARGUMENT_COUNT || arguments[0] < 0 || arguments[0] >= MAX_SUPPORTED_SERVOS) {
|
||||
cliPrint("Parse error\r\n");
|
||||
cliShowParseError();
|
||||
return;
|
||||
}
|
||||
|
||||
|
|
@ -945,6 +985,142 @@ static void cliServo(char *cmdline)
|
|||
#endif
|
||||
}
|
||||
|
||||
static void cliServoMix(char *cmdline)
|
||||
{
|
||||
#ifndef USE_SERVOS
|
||||
UNUSED(cmdline);
|
||||
#else
|
||||
int i;
|
||||
uint8_t len;
|
||||
char *ptr;
|
||||
int args[8], check = 0;
|
||||
len = strlen(cmdline);
|
||||
|
||||
if (len == 0) {
|
||||
|
||||
cliPrint("Rule\tServo\tSource\tRate\tSpeed\tMin\tMax\tBox\r\n");
|
||||
|
||||
for (i = 0; i < MAX_SERVO_RULES; i++) {
|
||||
if (masterConfig.customServoMixer[i].rate == 0)
|
||||
break;
|
||||
|
||||
printf("#%d:\t%d\t%d\t%d\t%d\t%d\t%d\t%d\r\n",
|
||||
i,
|
||||
masterConfig.customServoMixer[i].targetChannel,
|
||||
masterConfig.customServoMixer[i].inputSource,
|
||||
masterConfig.customServoMixer[i].rate,
|
||||
masterConfig.customServoMixer[i].speed,
|
||||
masterConfig.customServoMixer[i].min,
|
||||
masterConfig.customServoMixer[i].max,
|
||||
masterConfig.customServoMixer[i].box
|
||||
);
|
||||
}
|
||||
printf("\r\n");
|
||||
return;
|
||||
} else if (strncasecmp(cmdline, "reset", 5) == 0) {
|
||||
// erase custom mixer
|
||||
memset(masterConfig.customServoMixer, 0, sizeof(masterConfig.customServoMixer));
|
||||
for (i = 0; i < MAX_SUPPORTED_SERVOS; i++) {
|
||||
currentProfile->servoConf[i].reversedSources = 0;
|
||||
}
|
||||
} else if (strncasecmp(cmdline, "load", 4) == 0) {
|
||||
ptr = strchr(cmdline, ' ');
|
||||
if (ptr) {
|
||||
len = strlen(++ptr);
|
||||
for (i = 0; ; i++) {
|
||||
if (mixerNames[i] == NULL) {
|
||||
printf("Invalid name\r\n");
|
||||
break;
|
||||
}
|
||||
if (strncasecmp(ptr, mixerNames[i], len) == 0) {
|
||||
servoMixerLoadMix(i, masterConfig.customServoMixer);
|
||||
printf("Loaded %s\r\n", mixerNames[i]);
|
||||
cliServoMix("");
|
||||
break;
|
||||
}
|
||||
}
|
||||
}
|
||||
} else if (strncasecmp(cmdline, "reverse", 7) == 0) {
|
||||
enum {SERVO = 0, INPUT, REVERSE, ARGS_COUNT};
|
||||
int servoIndex, inputSource;
|
||||
ptr = strchr(cmdline, ' ');
|
||||
|
||||
len = strlen(ptr);
|
||||
if (len == 0) {
|
||||
printf("s");
|
||||
for (inputSource = 0; inputSource < INPUT_SOURCE_COUNT; inputSource++)
|
||||
printf("\ti%d", inputSource);
|
||||
printf("\r\n");
|
||||
|
||||
for (servoIndex = 0; servoIndex < MAX_SUPPORTED_SERVOS; servoIndex++) {
|
||||
printf("%d", servoIndex);
|
||||
for (inputSource = 0; inputSource < INPUT_SOURCE_COUNT; inputSource++)
|
||||
printf("\t%s ", (currentProfile->servoConf[servoIndex].reversedSources & (1 << inputSource)) ? "r" : "n");
|
||||
printf("\r\n");
|
||||
}
|
||||
return;
|
||||
}
|
||||
|
||||
ptr = strtok(ptr, " ");
|
||||
while (ptr != NULL && check < ARGS_COUNT - 1) {
|
||||
args[check++] = atoi(ptr);
|
||||
ptr = strtok(NULL, " ");
|
||||
}
|
||||
|
||||
if (ptr == NULL || check != ARGS_COUNT - 1) {
|
||||
cliShowParseError();
|
||||
return;
|
||||
}
|
||||
|
||||
if (args[SERVO] >= 0 && args[SERVO] < MAX_SUPPORTED_SERVOS
|
||||
&& args[INPUT] >= 0 && args[INPUT] < INPUT_SOURCE_COUNT
|
||||
&& (*ptr == 'r' || *ptr == 'n')) {
|
||||
if (*ptr == 'r')
|
||||
currentProfile->servoConf[args[SERVO]].reversedSources |= 1 << args[INPUT];
|
||||
else
|
||||
currentProfile->servoConf[args[SERVO]].reversedSources &= ~(1 << args[INPUT]);
|
||||
} else
|
||||
cliShowParseError();
|
||||
|
||||
cliServoMix("reverse");
|
||||
} else {
|
||||
enum {RULE = 0, TARGET, INPUT, RATE, SPEED, MIN, MAX, BOX, ARGS_COUNT};
|
||||
ptr = strtok(cmdline, " ");
|
||||
while (ptr != NULL && check < ARGS_COUNT) {
|
||||
args[check++] = atoi(ptr);
|
||||
ptr = strtok(NULL, " ");
|
||||
}
|
||||
|
||||
if (ptr != NULL || check != ARGS_COUNT) {
|
||||
cliShowParseError();
|
||||
return;
|
||||
}
|
||||
|
||||
i = args[RULE];
|
||||
if (i >= 0 && i < MAX_SERVO_RULES &&
|
||||
args[TARGET] >= 0 && args[TARGET] < MAX_SUPPORTED_SERVOS &&
|
||||
args[INPUT] >= 0 && args[INPUT] < INPUT_SOURCE_COUNT &&
|
||||
args[RATE] >= -100 && args[RATE] <= 100 &&
|
||||
args[SPEED] >= 0 && args[SPEED] <= MAX_SERVO_SPEED &&
|
||||
args[MIN] >= 0 && args[MIN] <= 100 &&
|
||||
args[MAX] >= 0 && args[MAX] <= 100 && args[MIN] < args[MAX] &&
|
||||
args[BOX] >= 0 && args[BOX] <= MAX_SERVO_BOXES) {
|
||||
masterConfig.customServoMixer[i].targetChannel = args[TARGET];
|
||||
masterConfig.customServoMixer[i].inputSource = args[INPUT];
|
||||
masterConfig.customServoMixer[i].rate = args[RATE];
|
||||
masterConfig.customServoMixer[i].speed = args[SPEED];
|
||||
masterConfig.customServoMixer[i].min = args[MIN];
|
||||
masterConfig.customServoMixer[i].max = args[MAX];
|
||||
masterConfig.customServoMixer[i].box = args[BOX];
|
||||
cliServoMix("");
|
||||
} else {
|
||||
cliShowParseError();
|
||||
}
|
||||
}
|
||||
#endif
|
||||
}
|
||||
|
||||
|
||||
#ifdef USE_FLASHFS
|
||||
|
||||
static void cliFlashInfo(char *cmdline)
|
||||
|
|
@ -961,7 +1137,7 @@ static void cliFlashErase(char *cmdline)
|
|||
{
|
||||
UNUSED(cmdline);
|
||||
|
||||
printf("Erasing, please wait...\r\n");
|
||||
printf("Erasing...\r\n");
|
||||
flashfsEraseCompletely();
|
||||
|
||||
while (!flashfsIsReady()) {
|
||||
|
|
@ -977,7 +1153,7 @@ static void cliFlashWrite(char *cmdline)
|
|||
char *text = strchr(cmdline, ' ');
|
||||
|
||||
if (!text) {
|
||||
printf("Missing text to write.\r\n");
|
||||
cliShowParseError();
|
||||
} else {
|
||||
flashfsSeekAbs(address);
|
||||
flashfsWrite((uint8_t*)text, strlen(text), true);
|
||||
|
|
@ -998,7 +1174,7 @@ static void cliFlashRead(char *cmdline)
|
|||
char *nextArg = strchr(cmdline, ' ');
|
||||
|
||||
if (!nextArg) {
|
||||
printf("Missing length argument.\r\n");
|
||||
cliShowParseError();
|
||||
} else {
|
||||
length = atoi(nextArg);
|
||||
|
||||
|
|
@ -1059,7 +1235,7 @@ static const char* const sectionBreak = "\r\n";
|
|||
|
||||
static void cliDump(char *cmdline)
|
||||
{
|
||||
unsigned int i;
|
||||
unsigned int i, channel;
|
||||
char buf[16];
|
||||
uint32_t mask;
|
||||
|
||||
|
|
@ -1089,15 +1265,16 @@ static void cliDump(char *cmdline)
|
|||
#ifndef USE_QUAD_MIXER_ONLY
|
||||
printf("mixer %s\r\n", mixerNames[masterConfig.mixerMode - 1]);
|
||||
|
||||
if (masterConfig.customMixer[0].throttle != 0.0f) {
|
||||
printf("mmix reset\r\n");
|
||||
|
||||
for (i = 0; i < MAX_SUPPORTED_MOTORS; i++) {
|
||||
if (masterConfig.customMixer[i].throttle == 0.0f)
|
||||
if (masterConfig.customMotorMixer[i].throttle == 0.0f)
|
||||
break;
|
||||
thr = masterConfig.customMixer[i].throttle;
|
||||
roll = masterConfig.customMixer[i].roll;
|
||||
pitch = masterConfig.customMixer[i].pitch;
|
||||
yaw = masterConfig.customMixer[i].yaw;
|
||||
printf("cmix %d", i + 1);
|
||||
thr = masterConfig.customMotorMixer[i].throttle;
|
||||
roll = masterConfig.customMotorMixer[i].roll;
|
||||
pitch = masterConfig.customMotorMixer[i].pitch;
|
||||
yaw = masterConfig.customMotorMixer[i].yaw;
|
||||
printf("mmix %d", i);
|
||||
if (thr < 0)
|
||||
cliWrite(' ');
|
||||
printf("%s", ftoa(thr, buf));
|
||||
|
|
@ -1111,8 +1288,36 @@ static void cliDump(char *cmdline)
|
|||
cliWrite(' ');
|
||||
printf("%s\r\n", ftoa(yaw, buf));
|
||||
}
|
||||
printf("cmix %d 0 0 0 0\r\n", i + 1);
|
||||
|
||||
// print custom servo mixer if exists
|
||||
printf("smix reset\r\n");
|
||||
|
||||
for (i = 0; i < MAX_SERVO_RULES; i++) {
|
||||
|
||||
if (masterConfig.customServoMixer[i].rate == 0)
|
||||
break;
|
||||
|
||||
printf("smix %d %d %d %d %d %d %d %d\r\n",
|
||||
i,
|
||||
masterConfig.customServoMixer[i].targetChannel,
|
||||
masterConfig.customServoMixer[i].inputSource,
|
||||
masterConfig.customServoMixer[i].rate,
|
||||
masterConfig.customServoMixer[i].speed,
|
||||
masterConfig.customServoMixer[i].min,
|
||||
masterConfig.customServoMixer[i].max,
|
||||
masterConfig.customServoMixer[i].box
|
||||
);
|
||||
}
|
||||
|
||||
// print servo directions
|
||||
for (i = 0; i < MAX_SUPPORTED_SERVOS; i++) {
|
||||
for (channel = 0; channel < INPUT_SOURCE_COUNT; channel++) {
|
||||
if (servoDirection(i, channel) < 0) {
|
||||
printf("smix reverse %d %d r\r\n", i , channel);
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
#endif
|
||||
|
||||
cliPrint("\r\n\r\n# feature\r\n");
|
||||
|
|
@ -1204,7 +1409,7 @@ static void cliExit(char *cmdline)
|
|||
bufferIndex = 0;
|
||||
cliMode = 0;
|
||||
// incase a motor was left running during motortest, clear it here
|
||||
mixerResetMotors();
|
||||
mixerResetDisarmedMotors();
|
||||
cliReboot();
|
||||
|
||||
cliPort = NULL;
|
||||
|
|
@ -1220,7 +1425,7 @@ static void cliFeature(char *cmdline)
|
|||
mask = featureMask();
|
||||
|
||||
if (len == 0) {
|
||||
cliPrint("Enabled features: ");
|
||||
cliPrint("Enabled: ");
|
||||
for (i = 0; ; i++) {
|
||||
if (featureNames[i] == NULL)
|
||||
break;
|
||||
|
|
@ -1229,7 +1434,7 @@ static void cliFeature(char *cmdline)
|
|||
}
|
||||
cliPrint("\r\n");
|
||||
} else if (strncasecmp(cmdline, "list", len) == 0) {
|
||||
cliPrint("Available features: ");
|
||||
cliPrint("Available: ");
|
||||
for (i = 0; ; i++) {
|
||||
if (featureNames[i] == NULL)
|
||||
break;
|
||||
|
|
@ -1248,7 +1453,7 @@ static void cliFeature(char *cmdline)
|
|||
|
||||
for (i = 0; ; i++) {
|
||||
if (featureNames[i] == NULL) {
|
||||
cliPrint("Invalid feature name\r\n");
|
||||
cliPrint("Invalid name\r\n");
|
||||
break;
|
||||
}
|
||||
|
||||
|
|
@ -1257,24 +1462,24 @@ static void cliFeature(char *cmdline)
|
|||
mask = 1 << i;
|
||||
#ifndef GPS
|
||||
if (mask & FEATURE_GPS) {
|
||||
cliPrint("GPS unavailable\r\n");
|
||||
cliPrint("unavailable\r\n");
|
||||
break;
|
||||
}
|
||||
#endif
|
||||
#ifndef SONAR
|
||||
if (mask & FEATURE_SONAR) {
|
||||
cliPrint("SONAR unavailable\r\n");
|
||||
cliPrint("unavailable\r\n");
|
||||
break;
|
||||
}
|
||||
#endif
|
||||
if (remove) {
|
||||
featureClear(mask);
|
||||
cliPrint("Disabled ");
|
||||
cliPrint("Disabled");
|
||||
} else {
|
||||
featureSet(mask);
|
||||
cliPrint("Enabled ");
|
||||
cliPrint("Enabled");
|
||||
}
|
||||
printf("%s\r\n", featureNames[i]);
|
||||
printf(" %s\r\n", featureNames[i]);
|
||||
break;
|
||||
}
|
||||
}
|
||||
|
|
@ -1296,9 +1501,16 @@ static void cliHelp(char *cmdline)
|
|||
|
||||
UNUSED(cmdline);
|
||||
|
||||
cliPrint("Available commands:\r\n");
|
||||
for (i = 0; i < CMD_COUNT; i++)
|
||||
printf("%s\t%s\r\n", cmdTable[i].name, cmdTable[i].param);
|
||||
for (i = 0; i < CMD_COUNT; i++) {
|
||||
cliPrint(cmdTable[i].name);
|
||||
if (cmdTable[i].description) {
|
||||
printf(" - %s", cmdTable[i].description);
|
||||
}
|
||||
if (cmdTable[i].args) {
|
||||
printf("\r\n\t%s", cmdTable[i].args);
|
||||
}
|
||||
cliPrint("\r\n");
|
||||
}
|
||||
}
|
||||
|
||||
static void cliMap(char *cmdline)
|
||||
|
|
@ -1316,12 +1528,12 @@ static void cliMap(char *cmdline)
|
|||
for (i = 0; i < 8; i++) {
|
||||
if (strchr(rcChannelLetters, cmdline[i]) && !strchr(cmdline + i + 1, cmdline[i]))
|
||||
continue;
|
||||
cliPrint("Must be any order of AETR1234\r\n");
|
||||
cliShowParseError();
|
||||
return;
|
||||
}
|
||||
parseRcChannels(cmdline, &masterConfig.rxConfig);
|
||||
}
|
||||
cliPrint("Current assignment: ");
|
||||
cliPrint("Map: ");
|
||||
for (i = 0; i < 8; i++)
|
||||
out[masterConfig.rxConfig.rcmap[i]] = rcChannelLetters[i];
|
||||
out[i] = '\0';
|
||||
|
|
@ -1337,7 +1549,7 @@ static void cliMixer(char *cmdline)
|
|||
len = strlen(cmdline);
|
||||
|
||||
if (len == 0) {
|
||||
printf("Current mixer: %s\r\n", mixerNames[masterConfig.mixerMode - 1]);
|
||||
printf("Mixer: %s\r\n", mixerNames[masterConfig.mixerMode - 1]);
|
||||
return;
|
||||
} else if (strncasecmp(cmdline, "list", len) == 0) {
|
||||
cliPrint("Available mixers: ");
|
||||
|
|
@ -1352,15 +1564,16 @@ static void cliMixer(char *cmdline)
|
|||
|
||||
for (i = 0; ; i++) {
|
||||
if (mixerNames[i] == NULL) {
|
||||
cliPrint("Invalid mixer type\r\n");
|
||||
break;
|
||||
cliPrint("Invalid name\r\n");
|
||||
return;
|
||||
}
|
||||
if (strncasecmp(cmdline, mixerNames[i], len) == 0) {
|
||||
masterConfig.mixerMode = i + 1;
|
||||
printf("Mixer set to %s\r\n", mixerNames[i]);
|
||||
break;
|
||||
}
|
||||
}
|
||||
|
||||
cliMixer("");
|
||||
}
|
||||
#endif
|
||||
|
||||
|
|
@ -1373,7 +1586,7 @@ static void cliMotor(char *cmdline)
|
|||
char *saveptr;
|
||||
|
||||
if (isEmpty(cmdline)) {
|
||||
cliPrint("Usage:\r\nmotor index [value] - show [or set] motor value\r\n");
|
||||
cliShowParseError();
|
||||
return;
|
||||
}
|
||||
|
||||
|
|
@ -1392,22 +1605,20 @@ static void cliMotor(char *cmdline)
|
|||
}
|
||||
|
||||
if (motor_index < 0 || motor_index >= MAX_SUPPORTED_MOTORS) {
|
||||
printf("No such motor, use a number [0, %d]\r\n", MAX_SUPPORTED_MOTORS);
|
||||
return;
|
||||
}
|
||||
|
||||
if (index < 2) {
|
||||
printf("Motor %d is set at %d\r\n", motor_index, motor_disarmed[motor_index]);
|
||||
cliShowArgumentRangeError("index", 0, MAX_SUPPORTED_MOTORS);
|
||||
return;
|
||||
}
|
||||
|
||||
if (index == 2) {
|
||||
if (motor_value < PWM_RANGE_MIN || motor_value > PWM_RANGE_MAX) {
|
||||
cliPrint("Invalid motor value, 1000..2000\r\n");
|
||||
cliShowArgumentRangeError("value", 1000, 2000);
|
||||
return;
|
||||
} else {
|
||||
motor_disarmed[motor_index] = motor_value;
|
||||
}
|
||||
}
|
||||
|
||||
printf("Setting motor %d to %d\r\n", motor_index, motor_value);
|
||||
motor_disarmed[motor_index] = motor_value;
|
||||
printf("motor %d: %d\r\n", motor_index, motor_disarmed[motor_index]);
|
||||
}
|
||||
|
||||
static void cliPlaySound(char *cmdline)
|
||||
|
|
@ -1647,7 +1858,7 @@ static void cliSet(char *cmdline)
|
|||
return;
|
||||
}
|
||||
}
|
||||
cliPrint("Unknown variable name\r\n");
|
||||
cliPrint("Invalid name\r\n");
|
||||
} else {
|
||||
// no equals, check for matching variables.
|
||||
cliGet(cmdline);
|
||||
|
|
@ -1676,7 +1887,7 @@ static void cliGet(char *cmdline)
|
|||
return;
|
||||
}
|
||||
|
||||
cliPrint("Unknown variable name\r\n");
|
||||
cliPrint("Invalid name\r\n");
|
||||
}
|
||||
|
||||
static void cliStatus(char *cmdline)
|
||||
|
|
@ -1784,14 +1995,12 @@ void cliProcess(void)
|
|||
} else if (!bufferIndex && c == 4) { // CTRL-D
|
||||
cliExit(cliBuffer);
|
||||
return;
|
||||
} else if (c == 12) {
|
||||
} else if (c == 12) { // NewPage / CTRL-L
|
||||
// clear screen
|
||||
cliPrint("\033[2J\033[1;1H");
|
||||
cliPrompt();
|
||||
} else if (bufferIndex && (c == '\n' || c == '\r')) {
|
||||
// enter pressed
|
||||
clicmd_t *cmd = NULL;
|
||||
clicmd_t target;
|
||||
cliPrint("\r\n");
|
||||
|
||||
// Strip comment starting with # from line
|
||||
|
|
@ -1809,11 +2018,14 @@ void cliProcess(void)
|
|||
// Process non-empty lines
|
||||
if (bufferIndex > 0) {
|
||||
cliBuffer[bufferIndex] = 0; // null terminate
|
||||
target.name = cliBuffer;
|
||||
target.param = NULL;
|
||||
|
||||
cmd = bsearch(&target, cmdTable, CMD_COUNT, sizeof cmdTable[0], cliCompare);
|
||||
if (cmd)
|
||||
const clicmd_t *cmd;
|
||||
for (cmd = cmdTable; cmd < cmdTable + CMD_COUNT; cmd++) {
|
||||
if(!strncasecmp(cliBuffer, cmd->name, strlen(cmd->name)) // command names match
|
||||
&& !isalnum((unsigned)cliBuffer[strlen(cmd->name)])) // next characted in bufffer is not alphanumeric (command is correctly terminated)
|
||||
break;
|
||||
}
|
||||
if(cmd < cmdTable + CMD_COUNT)
|
||||
cmd->func(cliBuffer + strlen(cmd->name) + 1);
|
||||
else
|
||||
cliPrint("Unknown command, try 'help'");
|
||||
|
|
|
|||
|
|
@ -131,7 +131,7 @@ void useRcControlsConfig(modeActivationCondition_t *modeActivationConditions, es
|
|||
#define MSP_PROTOCOL_VERSION 0
|
||||
|
||||
#define API_VERSION_MAJOR 1 // increment when major changes are made
|
||||
#define API_VERSION_MINOR 11 // increment when any change is made, reset to zero when major changes are released after changing API_VERSION_MAJOR
|
||||
#define API_VERSION_MINOR 12 // increment when any change is made, reset to zero when major changes are released after changing API_VERSION_MAJOR
|
||||
|
||||
#define API_VERSION_LENGTH 2
|
||||
|
||||
|
|
@ -173,9 +173,6 @@ static const char * const boardIdentifier = TARGET_BOARD_IDENTIFIER;
|
|||
//
|
||||
// MSP commands for Cleanflight original features
|
||||
//
|
||||
#define MSP_CHANNEL_FORWARDING 32 //out message Returns channel forwarding settings
|
||||
#define MSP_SET_CHANNEL_FORWARDING 33 //in message Channel forwarding settings
|
||||
|
||||
#define MSP_MODE_RANGES 34 //out message Returns all mode ranges
|
||||
#define MSP_SET_MODE_RANGE 35 //in message Sets a single mode range
|
||||
|
||||
|
|
@ -256,9 +253,9 @@ static const char * const boardIdentifier = TARGET_BOARD_IDENTIFIER;
|
|||
|
||||
#define MSP_STATUS 101 //out message cycletime & errors_count & sensor present & box activation & current setting number
|
||||
#define MSP_RAW_IMU 102 //out message 9 DOF
|
||||
#define MSP_SERVO 103 //out message 8 servos
|
||||
#define MSP_MOTOR 104 //out message 8 motors
|
||||
#define MSP_RC 105 //out message 8 rc chan and more
|
||||
#define MSP_SERVO 103 //out message servos
|
||||
#define MSP_MOTOR 104 //out message motors
|
||||
#define MSP_RC 105 //out message rc channels and more
|
||||
#define MSP_RAW_GPS 106 //out message fix, numsat, lat, lon, alt, speed, ground course
|
||||
#define MSP_COMP_GPS 107 //out message distance home, direction home
|
||||
#define MSP_ATTITUDE 108 //out message 2 angles 1 heading
|
||||
|
|
@ -273,7 +270,7 @@ static const char * const boardIdentifier = TARGET_BOARD_IDENTIFIER;
|
|||
#define MSP_PIDNAMES 117 //out message the PID names
|
||||
#define MSP_WP 118 //out message get a WP, WP# is in the payload, returns (WP#, lat, lon, alt, flags) WP#0-home, WP#16-poshold
|
||||
#define MSP_BOXIDS 119 //out message get the permanent IDs associated to BOXes
|
||||
#define MSP_SERVO_CONF 120 //out message Servo settings
|
||||
#define MSP_SERVO_CONFIGURATIONS 120 //out message All servo configurations.
|
||||
#define MSP_NAV_STATUS 121 //out message Returns navigation status
|
||||
#define MSP_NAV_CONFIG 122 //out message Returns navigation parameters
|
||||
|
||||
|
|
@ -289,7 +286,7 @@ static const char * const boardIdentifier = TARGET_BOARD_IDENTIFIER;
|
|||
#define MSP_SET_WP 209 //in message sets a given WP (WP#,lat, lon, alt, flags)
|
||||
#define MSP_SELECT_SETTING 210 //in message Select Setting Number (0-2)
|
||||
#define MSP_SET_HEAD 211 //in message define a new heading hold direction
|
||||
#define MSP_SET_SERVO_CONF 212 //in message Servo settings
|
||||
#define MSP_SET_SERVO_CONFIGURATION 212 //in message Servo settings
|
||||
#define MSP_SET_MOTOR 214 //in message PropBalance function
|
||||
#define MSP_SET_NAV_CONFIG 215 //in message Sets nav config parameters - write to the eeprom
|
||||
|
||||
|
|
@ -305,11 +302,11 @@ static const char * const boardIdentifier = TARGET_BOARD_IDENTIFIER;
|
|||
#define MSP_ACC_TRIM 240 //out message get acc angle trim values
|
||||
#define MSP_SET_ACC_TRIM 239 //in message set acc angle trim values
|
||||
#define MSP_GPSSVINFO 164 //out message get Signal Strength (only U-Blox)
|
||||
#define MSP_SERVO_MIX_RULES 241 //out message Returns servo mixer configuration
|
||||
#define MSP_SET_SERVO_MIX_RULE 242 //in message Sets servo mixer configuration
|
||||
|
||||
#define INBUF_SIZE 64
|
||||
|
||||
#define SERVO_CHUNK_SIZE 7
|
||||
|
||||
typedef struct box_e {
|
||||
const uint8_t boxId; // see boxId_e
|
||||
const char *boxName; // GUI-readable box name
|
||||
|
|
@ -341,6 +338,10 @@ static const box_t boxes[CHECKBOX_ITEM_COUNT + 1] = {
|
|||
{ BOXTELEMETRY, "TELEMETRY;", 20 },
|
||||
{ BOXAUTOTUNE, "AUTOTUNE;", 21 },
|
||||
{ BOXSONAR, "SONAR;", 22 },
|
||||
{ BOXSERVO1, "SERVO1;", 23 },
|
||||
{ BOXSERVO2, "SERVO2;", 24 },
|
||||
{ BOXSERVO3, "SERVO3;", 25 },
|
||||
|
||||
{ CHECKBOX_ITEM_COUNT, NULL, 0xFF }
|
||||
};
|
||||
|
||||
|
|
@ -621,8 +622,6 @@ void mspReleasePortIfAllocated(serialPort_t *serialPort)
|
|||
|
||||
void mspInit(serialConfig_t *serialConfig)
|
||||
{
|
||||
BUILD_BUG_ON((SERVO_CHUNK_SIZE * MAX_SUPPORTED_SERVOS) > INBUF_SIZE);
|
||||
|
||||
// calculate used boxes based on features and fill availableBoxes[] array
|
||||
memset(activeBoxIds, 0xFF, sizeof(activeBoxIds));
|
||||
|
||||
|
|
@ -681,6 +680,14 @@ void mspInit(serialConfig_t *serialConfig)
|
|||
activeBoxIds[activeBoxIdCount++] = BOXSONAR;
|
||||
}
|
||||
|
||||
#ifdef USE_SERVOS
|
||||
if (masterConfig.mixerMode == MIXER_CUSTOM_AIRPLANE) {
|
||||
activeBoxIds[activeBoxIdCount++] = BOXSERVO1;
|
||||
activeBoxIds[activeBoxIdCount++] = BOXSERVO2;
|
||||
activeBoxIds[activeBoxIdCount++] = BOXSERVO3;
|
||||
}
|
||||
#endif
|
||||
|
||||
memset(mspPorts, 0x00, sizeof(mspPorts));
|
||||
mspAllocateSerialPorts(serialConfig);
|
||||
}
|
||||
|
|
@ -764,7 +771,7 @@ static bool processOutCommand(uint8_t cmdMSP)
|
|||
serialize8(MW_VERSION);
|
||||
serialize8(masterConfig.mixerMode);
|
||||
serialize8(MSP_PROTOCOL_VERSION);
|
||||
serialize32(CAP_DYNBALANCE | (masterConfig.airplaneConfig.flaps_speed ? CAP_FLAPS : 0)); // "capability"
|
||||
serialize32(CAP_DYNBALANCE); // "capability"
|
||||
break;
|
||||
|
||||
case MSP_STATUS:
|
||||
|
|
@ -827,8 +834,8 @@ static bool processOutCommand(uint8_t cmdMSP)
|
|||
case MSP_SERVO:
|
||||
s_struct((uint8_t *)&servo, MAX_SUPPORTED_SERVOS * 2);
|
||||
break;
|
||||
case MSP_SERVO_CONF:
|
||||
headSerialReply(MAX_SUPPORTED_SERVOS * 9);
|
||||
case MSP_SERVO_CONFIGURATIONS:
|
||||
headSerialReply(MAX_SUPPORTED_SERVOS * sizeof(servoParam_t));
|
||||
for (i = 0; i < MAX_SUPPORTED_SERVOS; i++) {
|
||||
serialize16(currentProfile->servoConf[i].min);
|
||||
serialize16(currentProfile->servoConf[i].max);
|
||||
|
|
@ -836,12 +843,20 @@ static bool processOutCommand(uint8_t cmdMSP)
|
|||
serialize8(currentProfile->servoConf[i].rate);
|
||||
serialize8(currentProfile->servoConf[i].angleAtMin);
|
||||
serialize8(currentProfile->servoConf[i].angleAtMax);
|
||||
serialize32(currentProfile->servoConf[i].reversedSources);
|
||||
serialize8(currentProfile->servoConf[i].forwardFromChannel);
|
||||
}
|
||||
break;
|
||||
case MSP_CHANNEL_FORWARDING:
|
||||
headSerialReply(MAX_SUPPORTED_SERVOS);
|
||||
for (i = 0; i < MAX_SUPPORTED_SERVOS; i++) {
|
||||
serialize8(currentProfile->servoConf[i].forwardFromChannel);
|
||||
case MSP_SERVO_MIX_RULES:
|
||||
headSerialReply(MAX_SERVO_RULES * sizeof(servoMixer_t));
|
||||
for (i = 0; i < MAX_SERVO_RULES; i++) {
|
||||
serialize8(masterConfig.customServoMixer[i].targetChannel);
|
||||
serialize8(masterConfig.customServoMixer[i].inputSource);
|
||||
serialize8(masterConfig.customServoMixer[i].rate);
|
||||
serialize8(masterConfig.customServoMixer[i].speed);
|
||||
serialize8(masterConfig.customServoMixer[i].min);
|
||||
serialize8(masterConfig.customServoMixer[i].max);
|
||||
serialize8(masterConfig.customServoMixer[i].box);
|
||||
}
|
||||
break;
|
||||
#endif
|
||||
|
|
@ -1016,7 +1031,9 @@ static bool processOutCommand(uint8_t cmdMSP)
|
|||
serialize8(masterConfig.batteryConfig.vbatmaxcellvoltage);
|
||||
serialize8(masterConfig.batteryConfig.vbatwarningcellvoltage);
|
||||
break;
|
||||
|
||||
case MSP_MOTOR_PINS:
|
||||
// FIXME This is hardcoded and should not be.
|
||||
headSerialReply(8);
|
||||
for (i = 0; i < 8; i++)
|
||||
serialize8(i + 1);
|
||||
|
|
@ -1410,39 +1427,42 @@ static bool processInCommand(void)
|
|||
for (i = 0; i < 8; i++) // FIXME should this use MAX_MOTORS or MAX_SUPPORTED_MOTORS instead of 8
|
||||
motor_disarmed[i] = read16();
|
||||
break;
|
||||
case MSP_SET_SERVO_CONF:
|
||||
case MSP_SET_SERVO_CONFIGURATION:
|
||||
#ifdef USE_SERVOS
|
||||
if (currentPort->dataSize % SERVO_CHUNK_SIZE != 0) {
|
||||
debug[0] = currentPort->dataSize;
|
||||
i = read8();
|
||||
if (i >= MAX_SUPPORTED_SERVOS) {
|
||||
headSerialError(0);
|
||||
} else {
|
||||
uint8_t servoCount = currentPort->dataSize / SERVO_CHUNK_SIZE;
|
||||
for (i = 0; i < MAX_SUPPORTED_SERVOS && i < servoCount; i++) {
|
||||
currentProfile->servoConf[i].min = read16();
|
||||
currentProfile->servoConf[i].max = read16();
|
||||
|
||||
// provide temporary support for old clients that try and send a channel index instead of a servo middle
|
||||
uint16_t potentialServoMiddleOrChannelToForward = read16();
|
||||
if (potentialServoMiddleOrChannelToForward < MAX_SUPPORTED_SERVOS) {
|
||||
currentProfile->servoConf[i].forwardFromChannel = potentialServoMiddleOrChannelToForward;
|
||||
}
|
||||
if (potentialServoMiddleOrChannelToForward >= PWM_RANGE_MIN && potentialServoMiddleOrChannelToForward <= PWM_RANGE_MAX) {
|
||||
currentProfile->servoConf[i].middle = potentialServoMiddleOrChannelToForward;
|
||||
}
|
||||
currentProfile->servoConf[i].middle = read16();
|
||||
currentProfile->servoConf[i].rate = read8();
|
||||
currentProfile->servoConf[i].angleAtMin = read8();
|
||||
currentProfile->servoConf[i].angleAtMax = read8();
|
||||
}
|
||||
}
|
||||
#endif
|
||||
break;
|
||||
case MSP_SET_CHANNEL_FORWARDING:
|
||||
#ifdef USE_SERVOS
|
||||
for (i = 0; i < MAX_SUPPORTED_SERVOS; i++) {
|
||||
currentProfile->servoConf[i].reversedSources = read32();
|
||||
currentProfile->servoConf[i].forwardFromChannel = read8();
|
||||
}
|
||||
#endif
|
||||
break;
|
||||
|
||||
case MSP_SET_SERVO_MIX_RULE:
|
||||
#ifdef USE_SERVOS
|
||||
i = read8();
|
||||
if (i >= MAX_SERVO_RULES) {
|
||||
headSerialError(0);
|
||||
} else {
|
||||
masterConfig.customServoMixer[i].targetChannel = read8();
|
||||
masterConfig.customServoMixer[i].inputSource = read8();
|
||||
masterConfig.customServoMixer[i].rate = read8();
|
||||
masterConfig.customServoMixer[i].speed = read8();
|
||||
masterConfig.customServoMixer[i].min = read8();
|
||||
masterConfig.customServoMixer[i].max = read8();
|
||||
masterConfig.customServoMixer[i].box = read8();
|
||||
loadCustomServoMixer();
|
||||
}
|
||||
#endif
|
||||
break;
|
||||
|
||||
case MSP_RESET_CONF:
|
||||
if (!ARMING_FLAG(ARMED)) {
|
||||
resetEEPROM();
|
||||
|
|
|
|||
|
|
@ -106,7 +106,11 @@ void mspInit(serialConfig_t *serialConfig);
|
|||
void cliInit(serialConfig_t *serialConfig);
|
||||
void failsafeInit(rxConfig_t *intialRxConfig);
|
||||
pwmOutputConfiguration_t *pwmInit(drv_pwm_config_t *init);
|
||||
void mixerInit(mixerMode_e mixerMode, motorMixer_t *customMixers);
|
||||
#ifdef USE_SERVOS
|
||||
void mixerInit(mixerMode_e mixerMode, motorMixer_t *customMotorMixers, servoMixer_t *customServoMixers);
|
||||
#else
|
||||
void mixerInit(mixerMode_e mixerMode, motorMixer_t *customMotorMixers);
|
||||
#endif
|
||||
void mixerUsePWMOutputConfiguration(pwmOutputConfiguration_t *pwmOutputConfiguration);
|
||||
void rxInit(rxConfig_t *rxConfig);
|
||||
void gpsInit(serialConfig_t *serialConfig, gpsConfig_t *initialGpsConfig);
|
||||
|
|
@ -197,7 +201,11 @@ void init(void)
|
|||
|
||||
serialInit(&masterConfig.serialConfig, feature(FEATURE_SOFTSERIAL));
|
||||
|
||||
mixerInit(masterConfig.mixerMode, masterConfig.customMixer);
|
||||
#ifdef USE_SERVOS
|
||||
mixerInit(masterConfig.mixerMode, masterConfig.customMotorMixer, masterConfig.customServoMixer);
|
||||
#else
|
||||
mixerInit(masterConfig.mixerMode, masterConfig.customMotorMixer);
|
||||
#endif
|
||||
|
||||
memset(&pwm_params, 0, sizeof(pwm_params));
|
||||
|
||||
|
|
@ -216,7 +224,7 @@ void init(void)
|
|||
#endif
|
||||
|
||||
// when using airplane/wing mixer, servo/motor outputs are remapped
|
||||
if (masterConfig.mixerMode == MIXER_AIRPLANE || masterConfig.mixerMode == MIXER_FLYING_WING)
|
||||
if (masterConfig.mixerMode == MIXER_AIRPLANE || masterConfig.mixerMode == MIXER_FLYING_WING || masterConfig.mixerMode == MIXER_CUSTOM_AIRPLANE)
|
||||
pwm_params.airplane = true;
|
||||
else
|
||||
pwm_params.airplane = false;
|
||||
|
|
@ -241,7 +249,7 @@ void init(void)
|
|||
|
||||
#ifdef USE_SERVOS
|
||||
pwm_params.useServos = isMixerUsingServos();
|
||||
pwm_params.extraServos = currentProfile->gimbalConfig.gimbal_flags & GIMBAL_FORWARDAUX;
|
||||
pwm_params.useChannelForwarding = feature(FEATURE_CHANNEL_FORWARDING);
|
||||
pwm_params.servoCenterPulse = masterConfig.escAndServoConfig.servoCenterPulse;
|
||||
pwm_params.servoPwmRate = masterConfig.servo_pwm_rate;
|
||||
#endif
|
||||
|
|
|
|||
|
|
@ -780,7 +780,7 @@ void loop(void)
|
|||
// Allow yaw control for tricopters if the user wants the servo to move even when unarmed.
|
||||
if (isUsingSticksForArming() && rcData[THROTTLE] <= masterConfig.rxConfig.mincheck
|
||||
#ifndef USE_QUAD_MIXER_ONLY
|
||||
&& !(masterConfig.mixerMode == MIXER_TRI && masterConfig.mixerConfig.tri_unarmed_servo)
|
||||
&& !((masterConfig.mixerMode == MIXER_TRI || masterConfig.mixerMode == MIXER_CUSTOM_TRI) && masterConfig.mixerConfig.tri_unarmed_servo)
|
||||
&& masterConfig.mixerMode != MIXER_AIRPLANE
|
||||
&& masterConfig.mixerMode != MIXER_FLYING_WING
|
||||
#endif
|
||||
|
|
|
|||
|
|
@ -120,6 +120,7 @@
|
|||
// disabled some features for OPBL build due to code size.
|
||||
#undef AUTOTUNE
|
||||
#undef SONAR
|
||||
#define SKIP_CLI_COMMAND_HELP
|
||||
#endif
|
||||
|
||||
|
||||
|
|
|
|||
|
|
@ -48,7 +48,7 @@ extern "C" {
|
|||
extern uint8_t servoCount;
|
||||
void forwardAuxChannelsToServos(uint8_t firstServoIndex);
|
||||
|
||||
void mixerInit(mixerMode_e mixerMode, motorMixer_t *initialCustomMixers);
|
||||
void mixerInit(mixerMode_e mixerMode, motorMixer_t *initialCustomMixers, servoMixer_t *initialCustomServoMixers);
|
||||
void mixerUsePWMOutputConfiguration(pwmOutputConfiguration_t *pwmOutputConfiguration);
|
||||
}
|
||||
|
||||
|
|
@ -144,12 +144,16 @@ TEST(FlightMixerTest, TestForwardAuxChannelsToServosWithLessRemainingServosThanA
|
|||
EXPECT_EQ(1250, servos[MAX_SUPPORTED_SERVOS - 0 - 1].value);
|
||||
}
|
||||
|
||||
|
||||
TEST(FlightMixerTest, TestTricopterServo)
|
||||
{
|
||||
// given
|
||||
mixerConfig_t mixerConfig;
|
||||
memset(&mixerConfig, 0, sizeof(mixerConfig));
|
||||
|
||||
rxConfig_t rxConfig;
|
||||
memset(&rxConfig, 0, sizeof(rxConfig));
|
||||
|
||||
mixerConfig.tri_unarmed_servo = 1;
|
||||
|
||||
escAndServoConfig_t escAndServoConfig;
|
||||
|
|
@ -165,7 +169,7 @@ TEST(FlightMixerTest, TestTricopterServo)
|
|||
servoConf[5].forwardFromChannel = CHANNEL_FORWARDING_DISABLED;
|
||||
|
||||
gimbalConfig_t gimbalConfig = {
|
||||
.gimbal_flags = 0
|
||||
.mode = GIMBAL_MODE_NORMAL
|
||||
};
|
||||
|
||||
mixerUseConfigs(
|
||||
|
|
@ -175,13 +179,16 @@ TEST(FlightMixerTest, TestTricopterServo)
|
|||
&escAndServoConfig,
|
||||
&mixerConfig,
|
||||
NULL,
|
||||
NULL
|
||||
&rxConfig
|
||||
);
|
||||
|
||||
motorMixer_t customMixer[MAX_SUPPORTED_MOTORS];
|
||||
memset(&customMixer, 0, sizeof(customMixer));
|
||||
|
||||
mixerInit(MIXER_TRI, customMixer);
|
||||
servoMixer_t customServoMixer[MAX_SUPPORTED_SERVOS];
|
||||
memset(&customServoMixer, 0, sizeof(customServoMixer));
|
||||
|
||||
mixerInit(MIXER_TRI, customMixer, customServoMixer);
|
||||
|
||||
// and
|
||||
pwmOutputConfiguration_t pwmOutputConfiguration = {
|
||||
|
|
@ -213,31 +220,38 @@ TEST(FlightMixerTest, TestQuadMotors)
|
|||
mixerConfig_t mixerConfig;
|
||||
memset(&mixerConfig, 0, sizeof(mixerConfig));
|
||||
|
||||
//servoParam_t servoConf[MAX_SUPPORTED_SERVOS];
|
||||
//memset(&servoConf, 0, sizeof(servoConf));
|
||||
rxConfig_t rxConfig;
|
||||
memset(&rxConfig, 0, sizeof(rxConfig));
|
||||
|
||||
servoParam_t servoConf[MAX_SUPPORTED_SERVOS];
|
||||
memset(&servoConf, 0, sizeof(servoConf));
|
||||
|
||||
escAndServoConfig_t escAndServoConfig;
|
||||
memset(&escAndServoConfig, 0, sizeof(escAndServoConfig));
|
||||
escAndServoConfig.mincommand = TEST_MIN_COMMAND;
|
||||
|
||||
gimbalConfig_t gimbalConfig = {
|
||||
.gimbal_flags = 0
|
||||
.mode = GIMBAL_MODE_NORMAL
|
||||
};
|
||||
|
||||
mixerUseConfigs(
|
||||
NULL,// servoConf,
|
||||
servoConf,
|
||||
&gimbalConfig,
|
||||
NULL,
|
||||
&escAndServoConfig,
|
||||
&mixerConfig,
|
||||
NULL,
|
||||
NULL
|
||||
&rxConfig
|
||||
);
|
||||
|
||||
motorMixer_t customMixer[MAX_SUPPORTED_MOTORS];
|
||||
memset(&customMixer, 0, sizeof(customMixer));
|
||||
|
||||
mixerInit(MIXER_QUADX, customMixer);
|
||||
servoMixer_t customServoMixer[MAX_SUPPORTED_SERVOS];
|
||||
memset(&customServoMixer, 0, sizeof(customServoMixer));
|
||||
|
||||
|
||||
mixerInit(MIXER_QUADX, customMixer, customServoMixer);
|
||||
|
||||
// and
|
||||
pwmOutputConfiguration_t pwmOutputConfiguration = {
|
||||
|
|
|
|||
Loading…
Add table
Add a link
Reference in a new issue