Mirror/betaflight
1
0
Fork 0
mirror of https://github.com/betaflight/betaflight.git synced 2025-07-23 16:25:31 +03:00
Code Issues Wiki Activity

Merge pull request #1066 from cleanflight/custom-servo-mixers

Browse source 
Custom servo mixers
This commit is contained in:
Dominic Clifton 2015-07-09 23:48:53 +01:00
commit 783a4c4bfa
19 changed files with 1031 additions and 520 deletions
Download patch file Download diff file Expand all files Collapse all files

5
docs/Cli.md
View file

@ -75,7 +75,8 @@ Re-apply any new defaults as desired.
|------------------|------------------------------------------------| |------------------|------------------------------------------------|
| `adjrange` | show/set adjustment ranges settings | | `adjrange` | show/set adjustment ranges settings |
| `aux` | show/set aux settings | | `aux` | show/set aux settings |
| `cmix` | design custom mixer | | `mmix` | design custom motor mixer |
| `smix` | design custom servo mixer |
| `color` | configure colors | | `color` | configure colors |
| `defaults` | reset to defaults and reboot | | `defaults` | reset to defaults and reboot |
| `dump` | print configurable settings in a pastable form | | `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 | | `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_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 | | `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_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 | | `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 | | `accxy_deadband` | | 0 | 100 | 40 | Profile | UINT8 |

5
docs/Migrating from baseflight.md
View file

@ -98,3 +98,8 @@ reason: renamed to `3d_neutral` for consistency
### alt_hold_throttle_neutral ### alt_hold_throttle_neutral
reason: renamed to `alt_hold_deadband` for consistency reason: renamed to `alt_hold_deadband` for consistency
### gimbal_flags
reason: seperation of features.
see `gimbal_mode` and `CHANNEL_FORWARDING` feature

140
docs/Mixer.md
View file

@ -14,31 +14,33 @@ You can also use the Command Line Interface (CLI) to set the mixer type:
## Supported Mixer Types ## Supported Mixer Types
| Name | Description | Motors | Servos | | Name | Description | Motors | Servos |
| ------------- | ------------------------- | -------------- | ---------------- | | ---------------- | ------------------------- | -------------- | ---------------- |
| TRI | Tricopter | M1-M3 | S1 | | TRI | Tricopter | M1-M3 | S1 |
| QUADP | Quadcopter-Plus | M1-M4 | None | | QUADP | Quadcopter-Plus | M1-M4 | None |
| QUADX | Quadcopter-X | M1-M4 | None | | QUADX | Quadcopter-X | M1-M4 | None |
| BI | Bicopter (left/right) | M1-M2 | S1, S2 | | BI | Bicopter (left/right) | M1-M2 | S1, S2 |
| GIMBAL | Gimbal control | N/A | S1, S2 | | GIMBAL | Gimbal control | N/A | S1, S2 |
| Y6 | Y6-copter | M1-M6 | None | | Y6 | Y6-copter | M1-M6 | None |
| HEX6 | Hexacopter-Plus | M1-M6 | None | | HEX6 | Hexacopter-Plus | M1-M6 | None |
| FLYING_WING | Fixed wing; elevons | M1 | S1, S2 | | FLYING_WING | Fixed wing; elevons | M1 | S1, S2 |
| Y4 | Y4-copter | M1-M4 | None | | Y4 | Y4-copter | M1-M4 | None |
| HEX6X | Hexacopter-X | M1-M6 | None | | HEX6X | Hexacopter-X | M1-M6 | None |
| OCTOX8 | Octocopter-X (over/under) | M1-M8 | None | | OCTOX8 | Octocopter-X (over/under) | M1-M8 | None |
| OCTOFLATP | Octocopter-FlatPlus | M1-M8 | None | | OCTOFLATP | Octocopter-FlatPlus | M1-M8 | None |
| OCTOFLATX | Octocopter-FlatX | M1-M8 | None | | OCTOFLATX | Octocopter-FlatX | M1-M8 | None |
| AIRPLANE | Fixed wing; Ax2, R, E | M1 | S1, S2, S3, S4 | | AIRPLANE | Fixed wing; Ax2, R, E | M1 | S1, S2, S3, S4 |
| HELI_120_CCPM | | | | | HELI_120_CCPM | | | |
| HELI_90_DEG | | | | | HELI_90_DEG | | | |
| VTAIL4 | Quadcopter with V-Tail | M1-M4 | N/A | | VTAIL4 | Quadcopter with V-Tail | M1-M4 | N/A |
| HEX6H | Hexacopter-H | M1-M6 | None | | HEX6H | Hexacopter-H | M1-M6 | None |
| PPM_TO_SERVO | | | | | PPM_TO_SERVO | | | |
| DUALCOPTER | Dualcopter | M1-M2 | S1, S2 | | DUALCOPTER | Dualcopter | M1-M2 | S1, S2 |
| SINGLECOPTER | Conventional helicopter | M1 | S1 | | SINGLECOPTER | Conventional helicopter | M1 | S1 |
| ATAIL4 | Quadcopter with A-Tail | M1-M4 | N/A | | ATAIL4 | Quadcopter with A-Tail | M1-M4 | N/A |
| CUSTOM | User-defined | | | | CUSTOM | User-defined | | |
| CUSTOM AIRPLANE | User-defined airplane | | |
| CUSTOM TRICOPTER | User-defined tricopter | | |
## Servo filtering ## Servo filtering
@ -76,8 +78,8 @@ Custom motor mixing allows for completely customized motor configurations. Each
Steps to configure custom mixer in the CLI: Steps to configure custom mixer in the CLI:
1. Use `mixer custom` to enable the custom mixing. 1. Use `mixer custom` to enable the custom mixing.
2. Use `cmix reset` to erase the any existing custom mixing. 2. Use `mmix reset` to erase the any existing custom mixing.
3. Issue a cmix statement for each motor. 3. Issue a `mmix` statement for each motor.
The cmix statement has the following syntax: `cmix n THROTTLE ROLL PITCH YAW` 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. | | 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. | | 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 ### 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. 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` 1. Use `mixer custom`
2. Use `cmix reset` 2. Use `mmix 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. 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 `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. 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 `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. 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 `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. 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 ### Example 2: A HEX-U Copter
@ -121,13 +159,37 @@ HEX6-U
.5...FC...2. .5...FC...2.
............ ............
...6....1... ...6....1...
``` ```
|Command| Roll | Pitch | Yaw | |Command| Roll | Pitch | Yaw |
| ----- | ---- | ----- | --- | | ----- | ---- | ----- | --- |
| Use `cmix 1 1.0, -0.5, 1.0, -1.0` | half negative | full positive | CW | | Use `mmix 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 `mmix 1 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 `mmix 2 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 `mmix 3 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 `mmix 4 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 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`)

2
src/main/blackbox/blackbox.c
View file

@ -366,7 +366,7 @@ static bool testBlackboxConditionUncached(FlightLogFieldCondition condition)
return motorCount >= condition - FLIGHT_LOG_FIELD_CONDITION_AT_LEAST_MOTORS_1 + 1; return motorCount >= condition - FLIGHT_LOG_FIELD_CONDITION_AT_LEAST_MOTORS_1 + 1;
case FLIGHT_LOG_FIELD_CONDITION_TRICOPTER: 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_0:
case FLIGHT_LOG_FIELD_CONDITION_NONZERO_PID_D_1: case FLIGHT_LOG_FIELD_CONDITION_NONZERO_PID_D_1:

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

@ -350,10 +350,6 @@ static void setControlRateProfile(uint8_t profileIndex)
static void resetConf(void) static void resetConf(void)
{ {
int i; int i;
#ifdef USE_SERVOS
int8_t servoRates[MAX_SUPPORTED_SERVOS] = { 30, 30, 100, 100, 100, 100, 100, 100 };
;
#endif
// Clear all configuration // Clear all configuration
memset(&masterConfig, 0, sizeof(master_t)); memset(&masterConfig, 0, sizeof(master_t));
@ -420,7 +416,6 @@ static void resetConf(void)
resetMixerConfig(&masterConfig.mixerConfig); resetMixerConfig(&masterConfig.mixerConfig);
masterConfig.airplaneConfig.flaps_speed = 0;
masterConfig.airplaneConfig.fixedwing_althold_dir = 1; masterConfig.airplaneConfig.fixedwing_althold_dir = 1;
// Motor/ESC/Servo // Motor/ESC/Servo
@ -485,14 +480,14 @@ static void resetConf(void)
currentProfile->servoConf[i].min = DEFAULT_SERVO_MIN; currentProfile->servoConf[i].min = DEFAULT_SERVO_MIN;
currentProfile->servoConf[i].max = DEFAULT_SERVO_MAX; currentProfile->servoConf[i].max = DEFAULT_SERVO_MAX;
currentProfile->servoConf[i].middle = DEFAULT_SERVO_MIDDLE; 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].angleAtMin = DEFAULT_SERVO_MIN_ANGLE;
currentProfile->servoConf[i].angleAtMax = DEFAULT_SERVO_MAX_ANGLE; currentProfile->servoConf[i].angleAtMax = DEFAULT_SERVO_MAX_ANGLE;
currentProfile->servoConf[i].forwardFromChannel = CHANNEL_FORWARDING_DISABLED; currentProfile->servoConf[i].forwardFromChannel = CHANNEL_FORWARDING_DISABLED;
} }
// gimbal // gimbal
currentProfile->gimbalConfig.gimbal_flags = GIMBAL_NORMAL; currentProfile->gimbalConfig.mode = GIMBAL_MODE_NORMAL;
#endif #endif
#ifdef GPS #ifdef GPS
@ -501,7 +496,7 @@ static void resetConf(void)
// custom mixer. clear by defaults. // custom mixer. clear by defaults.
for (i = 0; i < MAX_SUPPORTED_MOTORS; i++) for (i = 0; i < MAX_SUPPORTED_MOTORS; i++)
masterConfig.customMixer[i].throttle = 0.0f; masterConfig.customMotorMixer[i].throttle = 0.0f;
#ifdef LED_STRIP #ifdef LED_STRIP
applyDefaultColors(masterConfig.colors, CONFIGURABLE_COLOR_COUNT); applyDefaultColors(masterConfig.colors, CONFIGURABLE_COLOR_COUNT);
@ -547,52 +542,52 @@ static void resetConf(void)
parseRcChannels("TAER1234", &masterConfig.rxConfig); parseRcChannels("TAER1234", &masterConfig.rxConfig);
// { 1.0f, -0.5f, 1.0f, -1.0f }, // REAR_R // { 1.0f, -0.5f, 1.0f, -1.0f }, // REAR_R
masterConfig.customMixer[0].throttle = 1.0f; masterConfig.customMotorMixer[0].throttle = 1.0f;
masterConfig.customMixer[0].roll = -0.5f; masterConfig.customMotorMixer[0].roll = -0.5f;
masterConfig.customMixer[0].pitch = 1.0f; masterConfig.customMotorMixer[0].pitch = 1.0f;
masterConfig.customMixer[0].yaw = -1.0f; masterConfig.customMotorMixer[0].yaw = -1.0f;
// { 1.0f, -0.5f, -1.0f, 1.0f }, // FRONT_R // { 1.0f, -0.5f, -1.0f, 1.0f }, // FRONT_R
masterConfig.customMixer[1].throttle = 1.0f; masterConfig.customMotorMixer[1].throttle = 1.0f;
masterConfig.customMixer[1].roll = -0.5f; masterConfig.customMotorMixer[1].roll = -0.5f;
masterConfig.customMixer[1].pitch = -1.0f; masterConfig.customMotorMixer[1].pitch = -1.0f;
masterConfig.customMixer[1].yaw = 1.0f; masterConfig.customMotorMixer[1].yaw = 1.0f;
// { 1.0f, 0.5f, 1.0f, 1.0f }, // REAR_L // { 1.0f, 0.5f, 1.0f, 1.0f }, // REAR_L
masterConfig.customMixer[2].throttle = 1.0f; masterConfig.customMotorMixer[2].throttle = 1.0f;
masterConfig.customMixer[2].roll = 0.5f; masterConfig.customMotorMixer[2].roll = 0.5f;
masterConfig.customMixer[2].pitch = 1.0f; masterConfig.customMotorMixer[2].pitch = 1.0f;
masterConfig.customMixer[2].yaw = 1.0f; masterConfig.customMotorMixer[2].yaw = 1.0f;
// { 1.0f, 0.5f, -1.0f, -1.0f }, // FRONT_L // { 1.0f, 0.5f, -1.0f, -1.0f }, // FRONT_L
masterConfig.customMixer[3].throttle = 1.0f; masterConfig.customMotorMixer[3].throttle = 1.0f;
masterConfig.customMixer[3].roll = 0.5f; masterConfig.customMotorMixer[3].roll = 0.5f;
masterConfig.customMixer[3].pitch = -1.0f; masterConfig.customMotorMixer[3].pitch = -1.0f;
masterConfig.customMixer[3].yaw = -1.0f; masterConfig.customMotorMixer[3].yaw = -1.0f;
// { 1.0f, -1.0f, -0.5f, -1.0f }, // MIDFRONT_R // { 1.0f, -1.0f, -0.5f, -1.0f }, // MIDFRONT_R
masterConfig.customMixer[4].throttle = 1.0f; masterConfig.customMotorMixer[4].throttle = 1.0f;
masterConfig.customMixer[4].roll = -1.0f; masterConfig.customMotorMixer[4].roll = -1.0f;
masterConfig.customMixer[4].pitch = -0.5f; masterConfig.customMotorMixer[4].pitch = -0.5f;
masterConfig.customMixer[4].yaw = -1.0f; masterConfig.customMotorMixer[4].yaw = -1.0f;
// { 1.0f, 1.0f, -0.5f, 1.0f }, // MIDFRONT_L // { 1.0f, 1.0f, -0.5f, 1.0f }, // MIDFRONT_L
masterConfig.customMixer[5].throttle = 1.0f; masterConfig.customMotorMixer[5].throttle = 1.0f;
masterConfig.customMixer[5].roll = 1.0f; masterConfig.customMotorMixer[5].roll = 1.0f;
masterConfig.customMixer[5].pitch = -0.5f; masterConfig.customMotorMixer[5].pitch = -0.5f;
masterConfig.customMixer[5].yaw = 1.0f; masterConfig.customMotorMixer[5].yaw = 1.0f;
// { 1.0f, -1.0f, 0.5f, 1.0f }, // MIDREAR_R // { 1.0f, -1.0f, 0.5f, 1.0f }, // MIDREAR_R
masterConfig.customMixer[6].throttle = 1.0f; masterConfig.customMotorMixer[6].throttle = 1.0f;
masterConfig.customMixer[6].roll = -1.0f; masterConfig.customMotorMixer[6].roll = -1.0f;
masterConfig.customMixer[6].pitch = 0.5f; masterConfig.customMotorMixer[6].pitch = 0.5f;
masterConfig.customMixer[6].yaw = 1.0f; masterConfig.customMotorMixer[6].yaw = 1.0f;
// { 1.0f, 1.0f, 0.5f, -1.0f }, // MIDREAR_L // { 1.0f, 1.0f, 0.5f, -1.0f }, // MIDREAR_L
masterConfig.customMixer[7].throttle = 1.0f; masterConfig.customMotorMixer[7].throttle = 1.0f;
masterConfig.customMixer[7].roll = 1.0f; masterConfig.customMotorMixer[7].roll = 1.0f;
masterConfig.customMixer[7].pitch = 0.5f; masterConfig.customMotorMixer[7].pitch = 0.5f;
masterConfig.customMixer[7].yaw = -1.0f; masterConfig.customMotorMixer[7].yaw = -1.0f;
#endif #endif
// copy first profile into remaining profile // copy first profile into remaining profile

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

@ -42,7 +42,8 @@ typedef enum {
FEATURE_LED_STRIP = 1 << 16, FEATURE_LED_STRIP = 1 << 16,
FEATURE_DISPLAY = 1 << 17, FEATURE_DISPLAY = 1 << 17,
FEATURE_ONESHOT125 = 1 << 18, FEATURE_ONESHOT125 = 1 << 18,
FEATURE_BLACKBOX = 1 << 19 FEATURE_BLACKBOX = 1 << 19,
FEATURE_CHANNEL_FORWARDING = 1 << 20
} features_e; } features_e;
void handleOneshotFeatureChangeOnRestart(void); void handleOneshotFeatureChangeOnRestart(void);

6
src/main/config/config_master.h
View file

@ -28,8 +28,10 @@ typedef struct master_t {
uint16_t looptime; // imu loop time in us uint16_t looptime; // imu loop time in us
uint8_t emf_avoidance; // change pll settings to avoid noise in the uhf band 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 // motor/esc/servo related stuff
escAndServoConfig_t escAndServoConfig; escAndServoConfig_t escAndServoConfig;
flight3DConfig_t flight3DConfig; flight3DConfig_t flight3DConfig;

2
src/main/drivers/pwm_mapping.c
View file

@ -502,7 +502,7 @@ pwmOutputConfiguration_t *pwmInit(drv_pwm_config_t *init)
#endif #endif
} }
if (init->extraServos && !init->airplane) { if (init->useChannelForwarding && !init->airplane) {
#if defined(NAZE) && defined(LED_STRIP_TIMER) #if defined(NAZE) && defined(LED_STRIP_TIMER)
// if LED strip is active, PWM5-8 are unavailable, so map AUX1+AUX2 to PWM13+PWM14 // if LED strip is active, PWM5-8 are unavailable, so map AUX1+AUX2 to PWM13+PWM14
if (init->useLEDStrip) { if (init->useLEDStrip) {

2
src/main/drivers/pwm_mapping.h
View file

@ -63,7 +63,7 @@ typedef struct drv_pwm_config_t {
#endif #endif
#ifdef USE_SERVOS #ifdef USE_SERVOS
bool useServos; 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 servoPwmRate;
uint16_t servoCenterPulse; uint16_t servoCenterPulse;
#endif #endif

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

@ -18,6 +18,7 @@
#include <stdbool.h> #include <stdbool.h>
#include <stdint.h> #include <stdint.h>
#include <stdlib.h> #include <stdlib.h>
#include <string.h>
#include "platform.h" #include "platform.h"
#include "debug.h" #include "debug.h"
@ -62,8 +63,8 @@ typedef enum {
SERVO_ELEVATOR = 6, SERVO_ELEVATOR = 6,
SERVO_THROTTLE = 7, // for internal combustion (IC) planes SERVO_THROTTLE = 7, // for internal combustion (IC) planes
SERVO_BIPLANE_LEFT = 4, SERVO_BICOPTER_LEFT = 4,
SERVO_BIPLANE_RIGHT = 5, SERVO_BICOPTER_RIGHT = 5,
SERVO_DUALCOPTER_LEFT = 4, SERVO_DUALCOPTER_LEFT = 4,
SERVO_DUALCOPTER_RIGHT = 5, SERVO_DUALCOPTER_RIGHT = 5,
@ -76,7 +77,7 @@ typedef enum {
} servoIndex_e; } servoIndex_e;
#define SERVO_PLANE_INDEX_MIN SERVO_FLAPS #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_MIN SERVO_DUALCOPTER_LEFT
#define SERVO_DUALCOPTER_INDEX_MAX SERVO_DUALCOPTER_RIGHT #define SERVO_DUALCOPTER_INDEX_MAX SERVO_DUALCOPTER_RIGHT
@ -90,6 +91,7 @@ typedef enum {
//#define MIXER_DEBUG //#define MIXER_DEBUG
uint8_t motorCount = 0; uint8_t motorCount = 0;
int16_t motor[MAX_SUPPORTED_MOTORS]; int16_t motor[MAX_SUPPORTED_MOTORS];
int16_t motor_disarmed[MAX_SUPPORTED_MOTORS]; int16_t motor_disarmed[MAX_SUPPORTED_MOTORS];
@ -99,10 +101,13 @@ static escAndServoConfig_t *escAndServoConfig;
static airplaneConfig_t *airplaneConfig; static airplaneConfig_t *airplaneConfig;
static rxConfig_t *rxConfig; static rxConfig_t *rxConfig;
static motorMixer_t currentMixer[MAX_SUPPORTED_MOTORS];
static mixerMode_e currentMixerMode; static mixerMode_e currentMixerMode;
static motorMixer_t currentMixer[MAX_SUPPORTED_MOTORS];
#ifdef USE_SERVOS #ifdef USE_SERVOS
static uint8_t servoRuleCount = 0;
static servoMixer_t currentServoMixer[MAX_SERVO_RULES];
static gimbalConfig_t *gimbalConfig; static gimbalConfig_t *gimbalConfig;
int16_t servo[MAX_SUPPORTED_SERVOS]; int16_t servo[MAX_SUPPORTED_SERVOS];
static int useServo; static int useServo;
@ -118,7 +123,7 @@ static const motorMixer_t mixerQuadX[] = {
{ 1.0f, 1.0f, -1.0f, -1.0f }, // FRONT_L { 1.0f, 1.0f, -1.0f, -1.0f }, // FRONT_L
}; };
#ifndef USE_QUAD_MIXER_ONLY #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, 0.0f, 1.333333f, 0.0f }, // REAR
{ 1.0f, -1.0f, -0.666667f, 0.0f }, // RIGHT { 1.0f, -1.0f, -0.666667f, 0.0f }, // RIGHT
{ 1.0f, 1.0f, -0.666667f, 0.0f }, // LEFT { 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 { 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 }, // LEFT
{ 1.0f, -1.0f, 0.0f, 0.0f }, // RIGHT { 1.0f, -1.0f, 0.0f, 0.0f }, // RIGHT
}; };
@ -211,10 +216,10 @@ static const motorMixer_t mixerVtail4[] = {
}; };
static const motorMixer_t mixerAtail4[] = { static const motorMixer_t mixerAtail4[] = {
{ 1.0f, 0.0f, 1.0f, 1.0f }, // REAR_R { 1.0f, 0.0f, 1.0f, 1.0f }, // REAR_R
{ 1.0f, -1.0f, -1.0f, 0.0f }, // FRONT_R { 1.0f, -1.0f, -1.0f, 0.0f }, // FRONT_R
{ 1.0f, 0.0f, 1.0f, -1.0f }, // REAR_L { 1.0f, 0.0f, 1.0f, -1.0f }, // REAR_L
{ 1.0f, 1.0f, -1.0f, -0.0f }, // FRONT_L { 1.0f, 1.0f, -1.0f, -0.0f }, // FRONT_L
}; };
static const motorMixer_t mixerHex6H[] = { static const motorMixer_t mixerHex6H[] = {
@ -231,36 +236,128 @@ static const motorMixer_t mixerDualcopter[] = {
{ 1.0f, 0.0f, 0.0f, 1.0f }, // RIGHT { 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 // Keep synced with mixerMode_e
const mixer_t mixers[] = { const mixer_t mixers[] = {
// motors, servos, motor mixer // motors, use servo, motor mixer
{ 0, 0, NULL }, // entry 0 { 0, false, NULL }, // entry 0
{ 3, 1, mixerTri }, // MIXER_TRI { 3, true, mixerTricopter }, // MIXER_TRI
{ 4, 0, mixerQuadP }, // MIXER_QUADP { 4, false, mixerQuadP }, // MIXER_QUADP
{ 4, 0, mixerQuadX }, // MIXER_QUADX { 4, false, mixerQuadX }, // MIXER_QUADX
{ 2, 1, mixerBi }, // MIXER_BI { 2, true, mixerBicopter }, // MIXER_BICOPTER
{ 0, 1, NULL }, // * MIXER_GIMBAL { 0, true, NULL }, // * MIXER_GIMBAL
{ 6, 0, mixerY6 }, // MIXER_Y6 { 6, false, mixerY6 }, // MIXER_Y6
{ 6, 0, mixerHex6P }, // MIXER_HEX6 { 6, false, mixerHex6P }, // MIXER_HEX6
{ 1, 1, NULL }, // * MIXER_FLYING_WING { 1, true, mixerSingleProp }, // * MIXER_FLYING_WING
{ 4, 0, mixerY4 }, // MIXER_Y4 { 4, false, mixerY4 }, // MIXER_Y4
{ 6, 0, mixerHex6X }, // MIXER_HEX6X { 6, false, mixerHex6X }, // MIXER_HEX6X
{ 8, 0, mixerOctoX8 }, // MIXER_OCTOX8 { 8, false, mixerOctoX8 }, // MIXER_OCTOX8
{ 8, 0, mixerOctoFlatP }, // MIXER_OCTOFLATP { 8, false, mixerOctoFlatP }, // MIXER_OCTOFLATP
{ 8, 0, mixerOctoFlatX }, // MIXER_OCTOFLATX { 8, false, mixerOctoFlatX }, // MIXER_OCTOFLATX
{ 1, 1, NULL }, // * MIXER_AIRPLANE { 1, true, mixerSingleProp }, // * MIXER_AIRPLANE
{ 0, 1, NULL }, // * MIXER_HELI_120_CCPM { 0, true, NULL }, // * MIXER_HELI_120_CCPM
{ 0, 1, NULL }, // * MIXER_HELI_90_DEG { 0, true, NULL }, // * MIXER_HELI_90_DEG
{ 4, 0, mixerVtail4 }, // MIXER_VTAIL4 { 4, false, mixerVtail4 }, // MIXER_VTAIL4
{ 6, 0, mixerHex6H }, // MIXER_HEX6H { 6, false, mixerHex6H }, // MIXER_HEX6H
{ 0, 1, NULL }, // * MIXER_PPM_TO_SERVO { 0, true, NULL }, // * MIXER_PPM_TO_SERVO
{ 2, 1, mixerDualcopter }, // MIXER_DUALCOPTER { 2, true, mixerDualcopter }, // MIXER_DUALCOPTER
{ 1, 1, NULL }, // MIXER_SINGLECOPTER { 1, true, NULL }, // MIXER_SINGLECOPTER
{ 4, 0, mixerAtail4 }, // MIXER_ATAIL4 { 4, false, mixerAtail4 }, // MIXER_ATAIL4
{ 0, 0, NULL }, // MIXER_CUSTOM { 0, false, NULL }, // MIXER_CUSTOM
{ 2, true, NULL }, // MIXER_CUSTOM_AIRPLANE
{ 3, true, NULL }, // MIXER_CUSTOM_TRI
}; };
#endif #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; static motorMixer_t *customMixers;
void mixerUseConfigs( void mixerUseConfigs(
@ -297,15 +394,11 @@ int16_t determineServoMiddleOrForwardFromChannel(servoIndex_e servoIndex)
return servoConf[servoIndex].middle; 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 // determine the direction (reversed or not) from the direction bitfield of the servo
// bit set = negative, clear = positive if (servoConf[servoIndex].reversedSources & (1 << inputSource))
// 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)
return -1; return -1;
else else
return 1; return 1;
@ -313,11 +406,32 @@ int servoDirection(servoIndex_e servoIndex, int lr)
#endif #endif
#ifndef USE_QUAD_MIXER_ONLY #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; currentMixerMode = mixerMode;
customMixers = initialCustomMixers; customMixers = initialCustomMotorMixers;
customServoMixers = initialCustomServoMixers;
// enable servos for mixes that require them. note, this shifts motor counts. // enable servos for mixes that require them. note, this shifts motor counts.
useServo = mixers[currentMixerMode].useServo; useServo = mixers[currentMixerMode].useServo;
@ -337,7 +451,7 @@ void mixerUsePWMOutputConfiguration(pwmOutputConfiguration_t *pwmOutputConfigura
servoCount = pwmOutputConfiguration->servoCount; servoCount = pwmOutputConfiguration->servoCount;
if (currentMixerMode == MIXER_CUSTOM) { if (currentMixerMode == MIXER_CUSTOM || currentMixerMode == MIXER_CUSTOM_TRI || currentMixerMode == MIXER_CUSTOM_AIRPLANE) {
// load custom mixer into currentMixer // load custom mixer into currentMixer
for (i = 0; i < MAX_SUPPORTED_MOTORS; i++) { for (i = 0; i < MAX_SUPPORTED_MOTORS; i++) {
// check if done // 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 // in 3D mode, mixer gain has to be halved
if (feature(FEATURE_3D)) { if (feature(FEATURE_3D)) {
if (motorCount > 1) { if (motorCount > 1) {
@ -368,12 +490,38 @@ void mixerUsePWMOutputConfiguration(pwmOutputConfiguration_t *pwmOutputConfigura
// set flag that we're on something with wings // set flag that we're on something with wings
if (currentMixerMode == MIXER_FLYING_WING || if (currentMixerMode == MIXER_FLYING_WING ||
currentMixerMode == MIXER_AIRPLANE) currentMixerMode == MIXER_AIRPLANE ||
currentMixerMode == MIXER_CUSTOM_AIRPLANE
) {
ENABLE_STATE(FIXED_WING); ENABLE_STATE(FIXED_WING);
else
if (currentMixerMode == MIXER_CUSTOM_AIRPLANE) {
loadCustomServoMixer();
}
} else {
DISABLE_STATE(FIXED_WING); 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) void mixerLoadMix(int index, motorMixer_t *customMixers)
@ -415,11 +563,11 @@ void mixerUsePWMOutputConfiguration(pwmOutputConfiguration_t *pwmOutputConfigura
currentMixer[i] = mixerQuadX[i]; currentMixer[i] = mixerQuadX[i];
} }
mixerResetMotors(); mixerResetDisarmedMotors();
} }
#endif #endif
void mixerResetMotors(void) void mixerResetDisarmedMotors(void)
{ {
int i; int i;
// set disarmed motor values // set disarmed motor values
@ -451,12 +599,13 @@ void writeServos(void)
uint8_t servoIndex = 0; uint8_t servoIndex = 0;
switch (currentMixerMode) { switch (currentMixerMode) {
case MIXER_BI: case MIXER_BICOPTER:
pwmWriteServo(servoIndex++, servo[SERVO_BIPLANE_LEFT]); pwmWriteServo(servoIndex++, servo[SERVO_BICOPTER_LEFT]);
pwmWriteServo(servoIndex++, servo[SERVO_BIPLANE_RIGHT]); pwmWriteServo(servoIndex++, servo[SERVO_BICOPTER_RIGHT]);
break; break;
case MIXER_TRI: case MIXER_TRI:
case MIXER_CUSTOM_TRI:
if (mixerConfig->tri_unarmed_servo) { if (mixerConfig->tri_unarmed_servo) {
// if unarmed flag set, we always move servo // if unarmed flag set, we always move servo
pwmWriteServo(servoIndex++, servo[SERVO_RUDDER]); pwmWriteServo(servoIndex++, servo[SERVO_RUDDER]);
@ -479,6 +628,7 @@ void writeServos(void)
pwmWriteServo(servoIndex++, servo[SERVO_DUALCOPTER_RIGHT]); pwmWriteServo(servoIndex++, servo[SERVO_DUALCOPTER_RIGHT]);
break; break;
case MIXER_CUSTOM_AIRPLANE:
case MIXER_AIRPLANE: case MIXER_AIRPLANE:
for (int i = SERVO_PLANE_INDEX_MIN; i <= SERVO_PLANE_INDEX_MAX; i++) { for (int i = SERVO_PLANE_INDEX_MIN; i <= SERVO_PLANE_INDEX_MAX; i++) {
pwmWriteServo(servoIndex++, servo[i]); pwmWriteServo(servoIndex++, servo[i]);
@ -496,13 +646,13 @@ void writeServos(void)
} }
// Two servos for SERVO_TILT, if enabled // Two servos for SERVO_TILT, if enabled
if (feature(FEATURE_SERVO_TILT)) { if (feature(FEATURE_SERVO_TILT) || currentMixerMode == MIXER_GIMBAL) {
updateGimbalServos(servoIndex); updateGimbalServos(servoIndex);
servoIndex += 2; servoIndex += 2;
} }
// forward AUX to remaining servo outputs (not constrained) // forward AUX to remaining servo outputs (not constrained)
if (gimbalConfig->gimbal_flags & GIMBAL_FORWARDAUX) { if (feature(FEATURE_CHANNEL_FORWARDING)) {
forwardAuxChannelsToServos(servoIndex); forwardAuxChannelsToServos(servoIndex);
servoIndex += MAX_AUX_CHANNEL_COUNT; servoIndex += MAX_AUX_CHANNEL_COUNT;
} }
@ -545,52 +695,83 @@ void StopPwmAllMotors()
} }
#ifndef USE_QUAD_MIXER_ONLY #ifndef USE_QUAD_MIXER_ONLY
static void airplaneMixer(void) static void servoMixer(void)
{ {
int16_t flapperons[2] = { 0, 0 }; int16_t input[INPUT_SOURCE_COUNT]; // Range [-500:+500]
int i; static int16_t currentOutput[MAX_SERVO_RULES];
uint8_t i;
if (!ARMING_FLAG(ARMED)) if (FLIGHT_MODE(PASSTHRU_MODE)) {
servo[SERVO_THROTTLE] = escAndServoConfig->mincommand; // Kill throttle when disarmed // Direct passthru from RX
else input[INPUT_STABILIZED_ROLL] = rcCommand[ROLL];
servo[SERVO_THROTTLE] = constrain(rcCommand[THROTTLE], escAndServoConfig->minthrottle, escAndServoConfig->maxthrottle); input[INPUT_STABILIZED_PITCH] = rcCommand[PITCH];
motor[0] = servo[SERVO_THROTTLE]; input[INPUT_STABILIZED_YAW] = rcCommand[YAW];
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];
} else { } else {
// Assisted modes (gyro only or gyro+acc according to AUX configuration in Gui // Assisted modes (gyro only or gyro+acc according to AUX configuration in Gui
servo[SERVO_FLAPPERON_1] = axisPID[ROLL] + flapperons[0]; input[INPUT_STABILIZED_ROLL] = axisPID[ROLL];
servo[SERVO_FLAPPERON_2] = axisPID[ROLL] + flapperons[1]; input[INPUT_STABILIZED_PITCH] = axisPID[PITCH];
servo[SERVO_RUDDER] = axisPID[YAW]; input[INPUT_STABILIZED_YAW] = axisPID[YAW];
servo[SERVO_ELEVATOR] = axisPID[PITCH];
// 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++) { input[INPUT_GIMBAL_PITCH] = scaleRange(inclination.values.pitchDeciDegrees, -1800, 1800, -500, +500);
servo[i] = ((int32_t)servoConf[i].rate * servo[i]) / 100L; // servo rates 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); servo[i] += determineServoMiddleOrForwardFromChannel(i);
} }
} }
#endif #endif
void mixTable(void) void mixTable(void)
@ -598,114 +779,19 @@ void mixTable(void)
uint32_t i; uint32_t i;
if (motorCount >= 4 && mixerConfig->yaw_jump_prevention_limit < YAW_JUMP_PREVENTION_LIMIT_HIGH) { 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])); 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 // motors for non-servo mixes
if (motorCount > 1) { for (i = 0; i < motorCount; i++) {
for (i = 0; i < motorCount; i++) { motor[i] =
motor[i] = rcCommand[THROTTLE] * currentMixer[i].throttle +
rcCommand[THROTTLE] * currentMixer[i].throttle + axisPID[PITCH] * currentMixer[i].pitch +
axisPID[PITCH] * currentMixer[i].pitch + axisPID[ROLL] * currentMixer[i].roll +
axisPID[ROLL] * currentMixer[i].roll + -mixerConfig->yaw_direction * axisPID[YAW] * currentMixer[i].yaw;
-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)) { if (ARMING_FLAG(ARMED)) {
bool isFailsafeActive = failsafeIsActive(); bool isFailsafeActive = failsafeIsActive();
@ -758,6 +844,58 @@ void mixTable(void)
motor[i] = motor_disarmed[i]; 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 #ifdef USE_SERVOS

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

@ -29,7 +29,7 @@ typedef enum mixerMode
MIXER_TRI = 1, MIXER_TRI = 1,
MIXER_QUADP = 2, MIXER_QUADP = 2,
MIXER_QUADX = 3, MIXER_QUADX = 3,
MIXER_BI = 4, MIXER_BICOPTER = 4,
MIXER_GIMBAL = 5, MIXER_GIMBAL = 5,
MIXER_Y6 = 6, MIXER_Y6 = 6,
MIXER_HEX6 = 7, MIXER_HEX6 = 7,
@ -48,7 +48,9 @@ typedef enum mixerMode
MIXER_DUALCOPTER = 20, MIXER_DUALCOPTER = 20,
MIXER_SINGLECOPTER = 21, MIXER_SINGLECOPTER = 21,
MIXER_ATAIL4 = 22, MIXER_ATAIL4 = 22,
MIXER_CUSTOM = 23 MIXER_CUSTOM = 23,
MIXER_CUSTOM_AIRPLANE = 24,
MIXER_CUSTOM_TRI = 25
} mixerMode_e; } mixerMode_e;
// Custom mixer data per motor // Custom mixer data per motor
@ -85,20 +87,61 @@ typedef struct flight3DConfig_s {
} flight3DConfig_t; } flight3DConfig_t;
typedef struct airplaneConfig_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 int8_t fixedwing_althold_dir; // +1 or -1 for pitch/althold gain. later check if need more than just sign
} airplaneConfig_t; } airplaneConfig_t;
#define CHANNEL_FORWARDING_DISABLED (uint8_t)0xFF #define CHANNEL_FORWARDING_DISABLED (uint8_t)0xFF
#ifdef USE_SERVOS #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 { typedef struct servoParam_t {
int16_t min; // servo min int16_t min; // servo min
int16_t max; // servo max int16_t max; // servo max
int16_t middle; // servo middle 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
uint8_t angleAtMin; // range [0;180] the measured angle in degrees from the middle when the servo is at the 'min' value. uint32_t reversedSources; // the direction of servo movement for each input source of the servo mixer, bit set=inverted
uint8_t angleAtMax; // range [0;180] the measured angle in degrees from the middle when the servo is at the 'max' value. 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 int8_t forwardFromChannel; // RX channel index, 0 based. See CHANNEL_FORWARDING_DISABLED
} servoParam_t; } servoParam_t;
@ -128,7 +171,12 @@ void mixerUseConfigs(
void writeAllMotors(int16_t mc); void writeAllMotors(int16_t mc);
void mixerLoadMix(int index, motorMixer_t *customMixers); 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 mixTable(void);
void writeMotors(void); void writeMotors(void);
void stopMotors(void); void stopMotors(void);

13
src/main/io/gimbal.h
View file

@ -17,12 +17,13 @@
#pragma once #pragma once
typedef enum GimbalFlags { typedef enum {
GIMBAL_NORMAL = 1 << 0, GIMBAL_MODE_NORMAL = 0,
GIMBAL_MIXTILT = 1 << 1, GIMBAL_MODE_MIXTILT = 1
GIMBAL_FORWARDAUX = 1 << 2, } gimbalMode_e;
} GimbalFlags;
#define GIMBAL_MODE_MAX (GIMBAL_MODE_MIXTILT)
typedef struct gimbalConfig_s { typedef struct gimbalConfig_s {
uint8_t gimbal_flags; // in servotilt mode, various things that affect stuff uint8_t mode;
} gimbalConfig_t; } gimbalConfig_t;

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

@ -43,12 +43,15 @@ typedef enum {
BOXTELEMETRY, BOXTELEMETRY,
BOXAUTOTUNE, BOXAUTOTUNE,
BOXSONAR, BOXSONAR,
BOXSERVO1,
BOXSERVO2,
BOXSERVO3,
CHECKBOX_ITEM_COUNT CHECKBOX_ITEM_COUNT
} boxId_e; } boxId_e;
extern uint32_t rcModeActivationMask; 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)) #define ACTIVATE_RC_MODE(modeId) (rcModeActivationMask |= (1 << modeId))
typedef enum rc_alias { typedef enum rc_alias {

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

@ -98,7 +98,7 @@ static serialPort_t *cliPort;
static void cliAux(char *cmdline); static void cliAux(char *cmdline);
static void cliAdjustmentRange(char *cmdline); static void cliAdjustmentRange(char *cmdline);
static void cliCMix(char *cmdline); static void cliMotorMix(char *cmdline);
static void cliDefaults(char *cmdline); static void cliDefaults(char *cmdline);
static void cliDump(char *cmdLine); static void cliDump(char *cmdLine);
static void cliExit(char *cmdline); static void cliExit(char *cmdline);
@ -111,6 +111,7 @@ static void cliReboot(void);
static void cliSave(char *cmdline); static void cliSave(char *cmdline);
static void cliSerial(char *cmdline); static void cliSerial(char *cmdline);
static void cliServo(char *cmdline); static void cliServo(char *cmdline);
static void cliServoMix(char *cmdline);
static void cliSet(char *cmdline); static void cliSet(char *cmdline);
static void cliGet(char *cmdline); static void cliGet(char *cmdline);
static void cliStatus(char *cmdline); static void cliStatus(char *cmdline);
@ -151,7 +152,7 @@ static const char * const mixerNames[] = {
"FLYING_WING", "Y4", "HEX6X", "OCTOX8", "OCTOFLATP", "OCTOFLATX", "FLYING_WING", "Y4", "HEX6X", "OCTOX8", "OCTOFLATP", "OCTOFLATX",
"AIRPLANE", "HELI_120_CCPM", "HELI_90_DEG", "VTAIL4", "AIRPLANE", "HELI_120_CCPM", "HELI_90_DEG", "VTAIL4",
"HEX6H", "PPM_TO_SERVO", "DUALCOPTER", "SINGLECOPTER", "HEX6H", "PPM_TO_SERVO", "DUALCOPTER", "SINGLECOPTER",
"ATAIL4", "CUSTOM", NULL "ATAIL4", "CUSTOM", "CUSTOMAIRPLANE", "CUSTOMTRI", NULL
}; };
#endif #endif
@ -161,7 +162,7 @@ static const char * const featureNames[] = {
"SERVO_TILT", "SOFTSERIAL", "GPS", "FAILSAFE", "SERVO_TILT", "SOFTSERIAL", "GPS", "FAILSAFE",
"SONAR", "TELEMETRY", "CURRENT_METER", "3D", "RX_PARALLEL_PWM", "SONAR", "TELEMETRY", "CURRENT_METER", "3D", "RX_PARALLEL_PWM",
"RX_MSP", "RSSI_ADC", "LED_STRIP", "DISPLAY", "ONESHOT125", "RX_MSP", "RSSI_ADC", "LED_STRIP", "DISPLAY", "ONESHOT125",
"BLACKBOX", NULL "BLACKBOX", "CHANNEL_FORWARDING", NULL
}; };
#ifndef CJMCU #ifndef CJMCU
@ -182,52 +183,91 @@ static const char * const sensorHardwareNames[4][11] = {
typedef struct { typedef struct {
const char *name; const char *name;
const char *param; const char *description;
const char *args;
void (*func)(char *cmdline); void (*func)(char *cmdline);
} clicmd_t; } 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() // should be sorted a..z for bsearch()
const clicmd_t cmdTable[] = { const clicmd_t cmdTable[] = {
{ "adjrange", "show/set adjustment ranges settings", cliAdjustmentRange }, CLI_COMMAND_DEF("adjrange", "configure adjustment ranges", NULL, cliAdjustmentRange),
{ "aux", "show/set aux settings", cliAux }, CLI_COMMAND_DEF("aux", "configure modes", NULL, cliAux),
{ "cmix", "design custom mixer", cliCMix },
#ifdef LED_STRIP #ifdef LED_STRIP
{ "color", "configure colors", cliColor }, CLI_COMMAND_DEF("color", "configure colors", NULL, cliColor),
#endif #endif
{ "defaults", "reset to defaults and reboot", cliDefaults }, CLI_COMMAND_DEF("defaults", "reset to defaults and reboot", NULL, cliDefaults),
{ "dump", "dump configuration", cliDump }, CLI_COMMAND_DEF("dump", "dump configuration",
{ "exit", "", cliExit }, "[master|profile]", cliDump),
{ "feature", "list or -val or val", cliFeature }, CLI_COMMAND_DEF("exit", NULL, NULL, cliExit),
CLI_COMMAND_DEF("feature", "configure features",
"list\r\n"
"\t<+|->[name]", cliFeature),
#ifdef USE_FLASHFS #ifdef USE_FLASHFS
{ "flash_erase", "erase flash chip", cliFlashErase }, CLI_COMMAND_DEF("flash_erase", "erase flash chip", NULL, cliFlashErase),
{ "flash_info", "get flash chip details", cliFlashInfo }, CLI_COMMAND_DEF("flash_info", "show flash chip info", NULL, cliFlashInfo),
{ "flash_read", "read text from the given address", cliFlashRead }, CLI_COMMAND_DEF("flash_read", NULL, "<length> <address>", cliFlashRead),
{ "flash_write", "write text to the given address", cliFlashWrite }, CLI_COMMAND_DEF("flash_write", NULL, "<address> <message>", cliFlashWrite),
#endif #endif
{ "get", "get variable value", cliGet }, CLI_COMMAND_DEF("get", "get variable value",
"[name]", cliGet),
#ifdef GPS #ifdef GPS
{ "gpspassthrough", "passthrough gps to serial", cliGpsPassthrough }, CLI_COMMAND_DEF("gpspassthrough", "passthrough gps to serial", NULL, cliGpsPassthrough),
#endif #endif
{ "help", "", cliHelp }, CLI_COMMAND_DEF("help", NULL, NULL, cliHelp),
#ifdef LED_STRIP #ifdef LED_STRIP
{ "led", "configure leds", cliLed }, CLI_COMMAND_DEF("led", "configure leds", NULL, cliLed),
#endif #endif
{ "map", "mapping of rc channel order", cliMap }, CLI_COMMAND_DEF("map", "configure rc channel order",
"[<map>]", cliMap),
#ifndef USE_QUAD_MIXER_ONLY #ifndef USE_QUAD_MIXER_ONLY
{ "mixer", "mixer name or list", cliMixer }, CLI_COMMAND_DEF("mixer", "configure mixer",
"list\r\n"
"\t<name>", cliMixer),
#endif #endif
{ "motor", "get/set motor output value", cliMotor }, CLI_COMMAND_DEF("mmix", "custom motor mixer", NULL, cliMotorMix),
{ "play_sound", "index, or none for next", cliPlaySound }, CLI_COMMAND_DEF("motor", "get/set motor",
{ "profile", "index (0 to 2)", cliProfile }, "<index> [<value>]", cliMotor),
{ "rateprofile", "index (0 to 2)", cliRateProfile }, CLI_COMMAND_DEF("play_sound", NULL,
{ "save", "save and reboot", cliSave }, "[<index>]\r\n", cliPlaySound),
{ "serial", "show/set serial settings", cliSerial }, 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 #ifdef USE_SERVOS
{ "servo", "servo config", cliServo }, CLI_COMMAND_DEF("servo", "configure servos", NULL, cliServo),
#endif #endif
{ "set", "name=value or blank or * for list", cliSet }, CLI_COMMAND_DEF("set", "change setting",
{ "status", "show system status", cliStatus }, "[<name>=<value>]", cliSet),
{ "version", "", cliVersion }, #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)) #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 }, { "auto_disarm_delay", VAR_UINT8 | MASTER_VALUE, &masterConfig.auto_disarm_delay, 0, 60 },
{ "small_angle", VAR_UINT8 | MASTER_VALUE, &masterConfig.small_angle, 0, 180 }, { "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 }, { "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 }, { "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 }, { "rx_max_usec", VAR_UINT16 | MASTER_VALUE, &masterConfig.rxConfig.rx_max_usec, PWM_PULSE_MIN, PWM_PULSE_MAX },
#ifdef USE_SERVOS #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 #endif
{ "acc_hardware", VAR_UINT8 | MASTER_VALUE, &masterConfig.acc_hardware, 0, ACC_MAX }, { "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 cliPrintVar(const clivalue_t *var, uint32_t full);
static void cliPrint(const char *str); static void cliPrint(const char *str);
static void cliWrite(uint8_t ch); static void cliWrite(uint8_t ch);
static void cliPrompt(void) static void cliPrompt(void)
{ {
cliPrint("\r\n# "); cliPrint("\r\n# ");
} }
static int cliCompare(const void *a, const void *b) static void cliShowParseError(void)
{ {
const clicmd_t *ca = a, *cb = b; cliPrint("Parse error\r\n");
return strncasecmp(ca->name, cb->name, strlen(cb->name)); }
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) static char *processChannelRangeArgs(char *ptr, channelRange_t *range, uint8_t *validArgumentCount)
{ {
int val; int val;
ptr = strchr(ptr, ' ');
if (ptr) { for (int argIndex = 0; argIndex < 2; argIndex++) {
val = atoi(++ptr); ptr = strchr(ptr, ' ');
val = CHANNEL_VALUE_TO_STEP(val); if (ptr) {
if (val >= MIN_MODE_RANGE_STEP && val <= MAX_MODE_RANGE_STEP) { val = atoi(++ptr);
range->startStep = val; val = CHANNEL_VALUE_TO_STEP(val);
(*validArgumentCount)++; if (val >= MIN_MODE_RANGE_STEP && val <= MAX_MODE_RANGE_STEP) {
} if (argIndex == 0) {
} range->startStep = val;
ptr = strchr(ptr, ' '); } else {
if (ptr) { range->endStep = val;
val = atoi(++ptr); }
val = CHANNEL_VALUE_TO_STEP(val); (*validArgumentCount)++;
if (val >= MIN_MODE_RANGE_STEP && val <= MAX_MODE_RANGE_STEP) { }
range->endStep = val;
(*validArgumentCount)++;
} }
} }
return ptr; return ptr;
} }
@ -557,7 +599,7 @@ static void cliAux(char *cmdline)
memset(mac, 0, sizeof(modeActivationCondition_t)); memset(mac, 0, sizeof(modeActivationCondition_t));
} }
} else { } 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) { if (validArgumentCount < 6) {
cliPrint("Parse error\r\n"); cliShowParseError();
return; return;
} }
@ -684,6 +726,7 @@ static void cliAdjustmentRange(char *cmdline)
if (i < MAX_ADJUSTMENT_RANGE_COUNT) { if (i < MAX_ADJUSTMENT_RANGE_COUNT) {
adjustmentRange_t *ar = &currentProfile->adjustmentRanges[i]; adjustmentRange_t *ar = &currentProfile->adjustmentRanges[i];
uint8_t validArgumentCount = 0; uint8_t validArgumentCount = 0;
ptr = strchr(ptr, ' '); ptr = strchr(ptr, ' ');
if (ptr) { if (ptr) {
val = atoi(++ptr); val = atoi(++ptr);
@ -700,7 +743,9 @@ static void cliAdjustmentRange(char *cmdline)
validArgumentCount++; validArgumentCount++;
} }
} }
ptr = processChannelRangeArgs(ptr, &ar->range, &validArgumentCount); ptr = processChannelRangeArgs(ptr, &ar->range, &validArgumentCount);
ptr = strchr(ptr, ' '); ptr = strchr(ptr, ' ');
if (ptr) { if (ptr) {
val = atoi(++ptr); val = atoi(++ptr);
@ -720,14 +765,15 @@ static void cliAdjustmentRange(char *cmdline)
if (validArgumentCount != 6) { if (validArgumentCount != 6) {
memset(ar, 0, sizeof(adjustmentRange_t)); memset(ar, 0, sizeof(adjustmentRange_t));
cliShowParseError();
} }
} else { } 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 #ifdef USE_QUAD_MIXER_ONLY
UNUSED(cmdline); UNUSED(cmdline);
@ -736,49 +782,38 @@ static void cliCMix(char *cmdline)
int num_motors = 0; int num_motors = 0;
uint8_t len; uint8_t len;
char buf[16]; char buf[16];
float mixsum[3];
char *ptr; char *ptr;
if (isEmpty(cmdline)) { 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++) { for (i = 0; i < MAX_SUPPORTED_MOTORS; i++) {
if (masterConfig.customMixer[i].throttle == 0.0f) if (masterConfig.customMotorMixer[i].throttle == 0.0f)
break; break;
num_motors++; num_motors++;
printf("#%d:\t", i + 1); printf("#%d:\t", i);
printf("%s\t", ftoa(masterConfig.customMixer[i].throttle, buf)); printf("%s\t", ftoa(masterConfig.customMotorMixer[i].throttle, buf));
printf("%s\t", ftoa(masterConfig.customMixer[i].roll, buf)); printf("%s\t", ftoa(masterConfig.customMotorMixer[i].roll, buf));
printf("%s\t", ftoa(masterConfig.customMixer[i].pitch, buf)); printf("%s\t", ftoa(masterConfig.customMotorMixer[i].pitch, buf));
printf("%s\r\n", ftoa(masterConfig.customMixer[i].yaw, 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; return;
} else if (strncasecmp(cmdline, "reset", 5) == 0) { } else if (strncasecmp(cmdline, "reset", 5) == 0) {
// erase custom mixer // erase custom mixer
for (i = 0; i < MAX_SUPPORTED_MOTORS; i++) 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) { } else if (strncasecmp(cmdline, "load", 4) == 0) {
ptr = strchr(cmdline, ' '); ptr = strchr(cmdline, ' ');
if (ptr) { if (ptr) {
len = strlen(++ptr); len = strlen(++ptr);
for (i = 0; ; i++) { for (i = 0; ; i++) {
if (mixerNames[i] == NULL) { if (mixerNames[i] == NULL) {
cliPrint("Invalid mixer type\r\n"); cliPrint("Invalid name\r\n");
break; break;
} }
if (strncasecmp(ptr, mixerNames[i], len) == 0) { if (strncasecmp(ptr, mixerNames[i], len) == 0) {
mixerLoadMix(i, masterConfig.customMixer); mixerLoadMix(i, masterConfig.customMotorMixer);
printf("Loaded %s mix\r\n", mixerNames[i]); printf("Loaded %s\r\n", mixerNames[i]);
cliCMix(""); cliMotorMix("");
break; break;
} }
} }
@ -786,34 +821,34 @@ static void cliCMix(char *cmdline)
} else { } else {
ptr = cmdline; ptr = cmdline;
i = atoi(ptr); // get motor number i = atoi(ptr); // get motor number
if (--i < MAX_SUPPORTED_MOTORS) { if (i < MAX_SUPPORTED_MOTORS) {
ptr = strchr(ptr, ' '); ptr = strchr(ptr, ' ');
if (ptr) { if (ptr) {
masterConfig.customMixer[i].throttle = fastA2F(++ptr); masterConfig.customMotorMixer[i].throttle = fastA2F(++ptr);
check++; check++;
} }
ptr = strchr(ptr, ' '); ptr = strchr(ptr, ' ');
if (ptr) { if (ptr) {
masterConfig.customMixer[i].roll = fastA2F(++ptr); masterConfig.customMotorMixer[i].roll = fastA2F(++ptr);
check++; check++;
} }
ptr = strchr(ptr, ' '); ptr = strchr(ptr, ' ');
if (ptr) { if (ptr) {
masterConfig.customMixer[i].pitch = fastA2F(++ptr); masterConfig.customMotorMixer[i].pitch = fastA2F(++ptr);
check++; check++;
} }
ptr = strchr(ptr, ' '); ptr = strchr(ptr, ' ');
if (ptr) { if (ptr) {
masterConfig.customMixer[i].yaw = fastA2F(++ptr); masterConfig.customMotorMixer[i].yaw = fastA2F(++ptr);
check++; check++;
} }
if (check != 4) { if (check != 4) {
cliPrint("Wrong number of arguments, needs idx thr roll pitch yaw\r\n"); cliShowParseError();
} else { } else {
cliCMix(""); cliMotorMix("");
} }
} else { } else {
printf("Motor number must be between 1 and %d\r\n", MAX_SUPPORTED_MOTORS); cliShowArgumentRangeError("index", 1, MAX_SUPPORTED_MOTORS);
} }
} }
#endif #endif
@ -837,10 +872,10 @@ static void cliLed(char *cmdline)
if (i < MAX_LED_STRIP_LENGTH) { if (i < MAX_LED_STRIP_LENGTH) {
ptr = strchr(cmdline, ' '); ptr = strchr(cmdline, ' ');
if (!parseLedStripConfig(i, ++ptr)) { if (!parseLedStripConfig(i, ++ptr)) {
cliPrint("Parse error\r\n"); cliShowParseError();
} }
} else { } 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)) { if (isEmpty(cmdline)) {
for (i = 0; i < CONFIGURABLE_COLOR_COUNT; i++) { 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 { } else {
ptr = cmdline; ptr = cmdline;
@ -860,10 +900,10 @@ static void cliColor(char *cmdline)
if (i < CONFIGURABLE_COLOR_COUNT) { if (i < CONFIGURABLE_COLOR_COUNT) {
ptr = strchr(cmdline, ' '); ptr = strchr(cmdline, ' ');
if (!parseColor(i, ++ptr)) { if (!parseColor(i, ++ptr)) {
cliPrint("Parse error\r\n"); cliShowParseError();
} }
} else { } 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) { while (*ptr) {
if (*ptr == '-' || (*ptr >= '0' && *ptr <= '9')) { if (*ptr == '-' || (*ptr >= '0' && *ptr <= '9')) {
if (validArgumentCount >= SERVO_ARGUMENT_COUNT) { if (validArgumentCount >= SERVO_ARGUMENT_COUNT) {
cliPrint("Parse error\r\n"); cliShowParseError();
return; return;
} }
@ -921,14 +961,14 @@ static void cliServo(char *cmdline)
} else if (*ptr == ' ') { } else if (*ptr == ' ') {
ptr++; ptr++;
} else { } else {
cliPrint("Parse error\r\n"); cliShowParseError();
return; return;
} }
} }
// Check we got the right number of args and the servo index is correct (don't validate the other values) // 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) { if (validArgumentCount != SERVO_ARGUMENT_COUNT || arguments[0] < 0 || arguments[0] >= MAX_SUPPORTED_SERVOS) {
cliPrint("Parse error\r\n"); cliShowParseError();
return; return;
} }
@ -945,6 +985,142 @@ static void cliServo(char *cmdline)
#endif #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 #ifdef USE_FLASHFS
static void cliFlashInfo(char *cmdline) static void cliFlashInfo(char *cmdline)
@ -961,7 +1137,7 @@ static void cliFlashErase(char *cmdline)
{ {
UNUSED(cmdline); UNUSED(cmdline);
printf("Erasing, please wait...\r\n"); printf("Erasing...\r\n");
flashfsEraseCompletely(); flashfsEraseCompletely();
while (!flashfsIsReady()) { while (!flashfsIsReady()) {
@ -977,7 +1153,7 @@ static void cliFlashWrite(char *cmdline)
char *text = strchr(cmdline, ' '); char *text = strchr(cmdline, ' ');
if (!text) { if (!text) {
printf("Missing text to write.\r\n"); cliShowParseError();
} else { } else {
flashfsSeekAbs(address); flashfsSeekAbs(address);
flashfsWrite((uint8_t*)text, strlen(text), true); flashfsWrite((uint8_t*)text, strlen(text), true);
@ -998,7 +1174,7 @@ static void cliFlashRead(char *cmdline)
char *nextArg = strchr(cmdline, ' '); char *nextArg = strchr(cmdline, ' ');
if (!nextArg) { if (!nextArg) {
printf("Missing length argument.\r\n"); cliShowParseError();
} else { } else {
length = atoi(nextArg); length = atoi(nextArg);
@ -1059,7 +1235,7 @@ static const char* const sectionBreak = "\r\n";
static void cliDump(char *cmdline) static void cliDump(char *cmdline)
{ {
unsigned int i; unsigned int i, channel;
char buf[16]; char buf[16];
uint32_t mask; uint32_t mask;
@ -1089,30 +1265,59 @@ static void cliDump(char *cmdline)
#ifndef USE_QUAD_MIXER_ONLY #ifndef USE_QUAD_MIXER_ONLY
printf("mixer %s\r\n", mixerNames[masterConfig.mixerMode - 1]); 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) for (i = 0; i < MAX_SUPPORTED_MOTORS; i++) {
break; if (masterConfig.customMotorMixer[i].throttle == 0.0f)
thr = masterConfig.customMixer[i].throttle; break;
roll = masterConfig.customMixer[i].roll; thr = masterConfig.customMotorMixer[i].throttle;
pitch = masterConfig.customMixer[i].pitch; roll = masterConfig.customMotorMixer[i].roll;
yaw = masterConfig.customMixer[i].yaw; pitch = masterConfig.customMotorMixer[i].pitch;
printf("cmix %d", i + 1); yaw = masterConfig.customMotorMixer[i].yaw;
if (thr < 0) printf("mmix %d", i);
cliWrite(' '); if (thr < 0)
printf("%s", ftoa(thr, buf)); cliWrite(' ');
if (roll < 0) printf("%s", ftoa(thr, buf));
cliWrite(' '); if (roll < 0)
printf("%s", ftoa(roll, buf)); cliWrite(' ');
if (pitch < 0) printf("%s", ftoa(roll, buf));
cliWrite(' '); if (pitch < 0)
printf("%s", ftoa(pitch, buf)); cliWrite(' ');
if (yaw < 0) printf("%s", ftoa(pitch, buf));
cliWrite(' '); if (yaw < 0)
printf("%s\r\n", ftoa(yaw, buf)); 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 #endif
cliPrint("\r\n\r\n# feature\r\n"); cliPrint("\r\n\r\n# feature\r\n");
@ -1204,7 +1409,7 @@ static void cliExit(char *cmdline)
bufferIndex = 0; bufferIndex = 0;
cliMode = 0; cliMode = 0;
// incase a motor was left running during motortest, clear it here // incase a motor was left running during motortest, clear it here
mixerResetMotors(); mixerResetDisarmedMotors();
cliReboot(); cliReboot();
cliPort = NULL; cliPort = NULL;
@ -1220,7 +1425,7 @@ static void cliFeature(char *cmdline)
mask = featureMask(); mask = featureMask();
if (len == 0) { if (len == 0) {
cliPrint("Enabled features: "); cliPrint("Enabled: ");
for (i = 0; ; i++) { for (i = 0; ; i++) {
if (featureNames[i] == NULL) if (featureNames[i] == NULL)
break; break;
@ -1229,7 +1434,7 @@ static void cliFeature(char *cmdline)
} }
cliPrint("\r\n"); cliPrint("\r\n");
} else if (strncasecmp(cmdline, "list", len) == 0) { } else if (strncasecmp(cmdline, "list", len) == 0) {
cliPrint("Available features: "); cliPrint("Available: ");
for (i = 0; ; i++) { for (i = 0; ; i++) {
if (featureNames[i] == NULL) if (featureNames[i] == NULL)
break; break;
@ -1248,7 +1453,7 @@ static void cliFeature(char *cmdline)
for (i = 0; ; i++) { for (i = 0; ; i++) {
if (featureNames[i] == NULL) { if (featureNames[i] == NULL) {
cliPrint("Invalid feature name\r\n"); cliPrint("Invalid name\r\n");
break; break;
} }
@ -1257,24 +1462,24 @@ static void cliFeature(char *cmdline)
mask = 1 << i; mask = 1 << i;
#ifndef GPS #ifndef GPS
if (mask & FEATURE_GPS) { if (mask & FEATURE_GPS) {
cliPrint("GPS unavailable\r\n"); cliPrint("unavailable\r\n");
break; break;
} }
#endif #endif
#ifndef SONAR #ifndef SONAR
if (mask & FEATURE_SONAR) { if (mask & FEATURE_SONAR) {
cliPrint("SONAR unavailable\r\n"); cliPrint("unavailable\r\n");
break; break;
} }
#endif #endif
if (remove) { if (remove) {
featureClear(mask); featureClear(mask);
cliPrint("Disabled "); cliPrint("Disabled");
} else { } else {
featureSet(mask); featureSet(mask);
cliPrint("Enabled "); cliPrint("Enabled");
} }
printf("%s\r\n", featureNames[i]); printf(" %s\r\n", featureNames[i]);
break; break;
} }
} }
@ -1296,9 +1501,16 @@ static void cliHelp(char *cmdline)
UNUSED(cmdline); UNUSED(cmdline);
cliPrint("Available commands:\r\n"); for (i = 0; i < CMD_COUNT; i++) {
for (i = 0; i < CMD_COUNT; i++) cliPrint(cmdTable[i].name);
printf("%s\t%s\r\n", cmdTable[i].name, cmdTable[i].param); 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) static void cliMap(char *cmdline)
@ -1316,12 +1528,12 @@ static void cliMap(char *cmdline)
for (i = 0; i < 8; i++) { for (i = 0; i < 8; i++) {
if (strchr(rcChannelLetters, cmdline[i]) && !strchr(cmdline + i + 1, cmdline[i])) if (strchr(rcChannelLetters, cmdline[i]) && !strchr(cmdline + i + 1, cmdline[i]))
continue; continue;
cliPrint("Must be any order of AETR1234\r\n"); cliShowParseError();
return; return;
} }
parseRcChannels(cmdline, &masterConfig.rxConfig); parseRcChannels(cmdline, &masterConfig.rxConfig);
} }
cliPrint("Current assignment: "); cliPrint("Map: ");
for (i = 0; i < 8; i++) for (i = 0; i < 8; i++)
out[masterConfig.rxConfig.rcmap[i]] = rcChannelLetters[i]; out[masterConfig.rxConfig.rcmap[i]] = rcChannelLetters[i];
out[i] = '\0'; out[i] = '\0';
@ -1337,7 +1549,7 @@ static void cliMixer(char *cmdline)
len = strlen(cmdline); len = strlen(cmdline);
if (len == 0) { if (len == 0) {
printf("Current mixer: %s\r\n", mixerNames[masterConfig.mixerMode - 1]); printf("Mixer: %s\r\n", mixerNames[masterConfig.mixerMode - 1]);
return; return;
} else if (strncasecmp(cmdline, "list", len) == 0) { } else if (strncasecmp(cmdline, "list", len) == 0) {
cliPrint("Available mixers: "); cliPrint("Available mixers: ");
@ -1352,15 +1564,16 @@ static void cliMixer(char *cmdline)
for (i = 0; ; i++) { for (i = 0; ; i++) {
if (mixerNames[i] == NULL) { if (mixerNames[i] == NULL) {
cliPrint("Invalid mixer type\r\n"); cliPrint("Invalid name\r\n");
break; return;
} }
if (strncasecmp(cmdline, mixerNames[i], len) == 0) { if (strncasecmp(cmdline, mixerNames[i], len) == 0) {
masterConfig.mixerMode = i + 1; masterConfig.mixerMode = i + 1;
printf("Mixer set to %s\r\n", mixerNames[i]);
break; break;
} }
} }
cliMixer("");
} }
#endif #endif
@ -1373,7 +1586,7 @@ static void cliMotor(char *cmdline)
char *saveptr; char *saveptr;
if (isEmpty(cmdline)) { if (isEmpty(cmdline)) {
cliPrint("Usage:\r\nmotor index [value] - show [or set] motor value\r\n"); cliShowParseError();
return; return;
} }
@ -1392,22 +1605,20 @@ static void cliMotor(char *cmdline)
} }
if (motor_index < 0 || motor_index >= MAX_SUPPORTED_MOTORS) { if (motor_index < 0 || motor_index >= MAX_SUPPORTED_MOTORS) {
printf("No such motor, use a number [0, %d]\r\n", MAX_SUPPORTED_MOTORS); cliShowArgumentRangeError("index", 0, MAX_SUPPORTED_MOTORS);
return; return;
} }
if (index < 2) { if (index == 2) {
printf("Motor %d is set at %d\r\n", motor_index, motor_disarmed[motor_index]); if (motor_value < PWM_RANGE_MIN || motor_value > PWM_RANGE_MAX) {
return; cliShowArgumentRangeError("value", 1000, 2000);
return;
} else {
motor_disarmed[motor_index] = motor_value;
}
} }
if (motor_value < PWM_RANGE_MIN || motor_value > PWM_RANGE_MAX) { printf("motor %d: %d\r\n", motor_index, motor_disarmed[motor_index]);
cliPrint("Invalid motor value, 1000..2000\r\n");
return;
}
printf("Setting motor %d to %d\r\n", motor_index, motor_value);
motor_disarmed[motor_index] = motor_value;
} }
static void cliPlaySound(char *cmdline) static void cliPlaySound(char *cmdline)
@ -1647,7 +1858,7 @@ static void cliSet(char *cmdline)
return; return;
} }
} }
cliPrint("Unknown variable name\r\n"); cliPrint("Invalid name\r\n");
} else { } else {
// no equals, check for matching variables. // no equals, check for matching variables.
cliGet(cmdline); cliGet(cmdline);
@ -1676,7 +1887,7 @@ static void cliGet(char *cmdline)
return; return;
} }
cliPrint("Unknown variable name\r\n"); cliPrint("Invalid name\r\n");
} }
static void cliStatus(char *cmdline) static void cliStatus(char *cmdline)
@ -1784,14 +1995,12 @@ void cliProcess(void)
} else if (!bufferIndex && c == 4) { // CTRL-D } else if (!bufferIndex && c == 4) { // CTRL-D
cliExit(cliBuffer); cliExit(cliBuffer);
return; return;
} else if (c == 12) { } else if (c == 12) { // NewPage / CTRL-L
// clear screen // clear screen
cliPrint("\033[2J\033[1;1H"); cliPrint("\033[2J\033[1;1H");
cliPrompt(); cliPrompt();
} else if (bufferIndex && (c == '\n' || c == '\r')) { } else if (bufferIndex && (c == '\n' || c == '\r')) {
// enter pressed // enter pressed
clicmd_t *cmd = NULL;
clicmd_t target;
cliPrint("\r\n"); cliPrint("\r\n");
// Strip comment starting with # from line // Strip comment starting with # from line
@ -1809,11 +2018,14 @@ void cliProcess(void)
// Process non-empty lines // Process non-empty lines
if (bufferIndex > 0) { if (bufferIndex > 0) {
cliBuffer[bufferIndex] = 0; // null terminate cliBuffer[bufferIndex] = 0; // null terminate
target.name = cliBuffer;
target.param = NULL;
cmd = bsearch(&target, cmdTable, CMD_COUNT, sizeof cmdTable[0], cliCompare); const clicmd_t *cmd;
if (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); cmd->func(cliBuffer + strlen(cmd->name) + 1);
else else
cliPrint("Unknown command, try 'help'"); cliPrint("Unknown command, try 'help'");

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

@ -131,7 +131,7 @@ void useRcControlsConfig(modeActivationCondition_t *modeActivationConditions, es
#define MSP_PROTOCOL_VERSION 0 #define MSP_PROTOCOL_VERSION 0
#define API_VERSION_MAJOR 1 // increment when major changes are made #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 #define API_VERSION_LENGTH 2
@ -173,9 +173,6 @@ static const char * const boardIdentifier = TARGET_BOARD_IDENTIFIER;
// //
// MSP commands for Cleanflight original features // 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_MODE_RANGES 34 //out message Returns all mode ranges
#define MSP_SET_MODE_RANGE 35 //in message Sets a single mode range #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_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_RAW_IMU 102 //out message 9 DOF
#define MSP_SERVO 103 //out message 8 servos #define MSP_SERVO 103 //out message servos
#define MSP_MOTOR 104 //out message 8 motors #define MSP_MOTOR 104 //out message motors
#define MSP_RC 105 //out message 8 rc chan and more #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_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_COMP_GPS 107 //out message distance home, direction home
#define MSP_ATTITUDE 108 //out message 2 angles 1 heading #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_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_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_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_STATUS 121 //out message Returns navigation status
#define MSP_NAV_CONFIG 122 //out message Returns navigation parameters #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_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_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_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_MOTOR 214 //in message PropBalance function
#define MSP_SET_NAV_CONFIG 215 //in message Sets nav config parameters - write to the eeprom #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_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_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_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 INBUF_SIZE 64
#define SERVO_CHUNK_SIZE 7
typedef struct box_e { typedef struct box_e {
const uint8_t boxId; // see boxId_e const uint8_t boxId; // see boxId_e
const char *boxName; // GUI-readable box name const char *boxName; // GUI-readable box name
@ -341,6 +338,10 @@ static const box_t boxes[CHECKBOX_ITEM_COUNT + 1] = {
{ BOXTELEMETRY, "TELEMETRY;", 20 }, { BOXTELEMETRY, "TELEMETRY;", 20 },
{ BOXAUTOTUNE, "AUTOTUNE;", 21 }, { BOXAUTOTUNE, "AUTOTUNE;", 21 },
{ BOXSONAR, "SONAR;", 22 }, { BOXSONAR, "SONAR;", 22 },
{ BOXSERVO1, "SERVO1;", 23 },
{ BOXSERVO2, "SERVO2;", 24 },
{ BOXSERVO3, "SERVO3;", 25 },
{ CHECKBOX_ITEM_COUNT, NULL, 0xFF } { CHECKBOX_ITEM_COUNT, NULL, 0xFF }
}; };
@ -621,8 +622,6 @@ void mspReleasePortIfAllocated(serialPort_t *serialPort)
void mspInit(serialConfig_t *serialConfig) 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 // calculate used boxes based on features and fill availableBoxes[] array
memset(activeBoxIds, 0xFF, sizeof(activeBoxIds)); memset(activeBoxIds, 0xFF, sizeof(activeBoxIds));
@ -681,6 +680,14 @@ void mspInit(serialConfig_t *serialConfig)
activeBoxIds[activeBoxIdCount++] = BOXSONAR; 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)); memset(mspPorts, 0x00, sizeof(mspPorts));
mspAllocateSerialPorts(serialConfig); mspAllocateSerialPorts(serialConfig);
} }
@ -764,7 +771,7 @@ static bool processOutCommand(uint8_t cmdMSP)
serialize8(MW_VERSION); serialize8(MW_VERSION);
serialize8(masterConfig.mixerMode); serialize8(masterConfig.mixerMode);
serialize8(MSP_PROTOCOL_VERSION); serialize8(MSP_PROTOCOL_VERSION);
serialize32(CAP_DYNBALANCE | (masterConfig.airplaneConfig.flaps_speed ? CAP_FLAPS : 0)); // "capability" serialize32(CAP_DYNBALANCE); // "capability"
break; break;
case MSP_STATUS: case MSP_STATUS:
@ -827,8 +834,8 @@ static bool processOutCommand(uint8_t cmdMSP)
case MSP_SERVO: case MSP_SERVO:
s_struct((uint8_t *)&servo, MAX_SUPPORTED_SERVOS * 2); s_struct((uint8_t *)&servo, MAX_SUPPORTED_SERVOS * 2);
break; break;
case MSP_SERVO_CONF: case MSP_SERVO_CONFIGURATIONS:
headSerialReply(MAX_SUPPORTED_SERVOS * 9); headSerialReply(MAX_SUPPORTED_SERVOS * sizeof(servoParam_t));
for (i = 0; i < MAX_SUPPORTED_SERVOS; i++) { for (i = 0; i < MAX_SUPPORTED_SERVOS; i++) {
serialize16(currentProfile->servoConf[i].min); serialize16(currentProfile->servoConf[i].min);
serialize16(currentProfile->servoConf[i].max); serialize16(currentProfile->servoConf[i].max);
@ -836,12 +843,20 @@ static bool processOutCommand(uint8_t cmdMSP)
serialize8(currentProfile->servoConf[i].rate); serialize8(currentProfile->servoConf[i].rate);
serialize8(currentProfile->servoConf[i].angleAtMin); serialize8(currentProfile->servoConf[i].angleAtMin);
serialize8(currentProfile->servoConf[i].angleAtMax); serialize8(currentProfile->servoConf[i].angleAtMax);
serialize32(currentProfile->servoConf[i].reversedSources);
serialize8(currentProfile->servoConf[i].forwardFromChannel);
} }
break; break;
case MSP_CHANNEL_FORWARDING: case MSP_SERVO_MIX_RULES:
headSerialReply(MAX_SUPPORTED_SERVOS); headSerialReply(MAX_SERVO_RULES * sizeof(servoMixer_t));
for (i = 0; i < MAX_SUPPORTED_SERVOS; i++) { for (i = 0; i < MAX_SERVO_RULES; i++) {
serialize8(currentProfile->servoConf[i].forwardFromChannel); 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; break;
#endif #endif
@ -1016,7 +1031,9 @@ static bool processOutCommand(uint8_t cmdMSP)
serialize8(masterConfig.batteryConfig.vbatmaxcellvoltage); serialize8(masterConfig.batteryConfig.vbatmaxcellvoltage);
serialize8(masterConfig.batteryConfig.vbatwarningcellvoltage); serialize8(masterConfig.batteryConfig.vbatwarningcellvoltage);
break; break;
case MSP_MOTOR_PINS: case MSP_MOTOR_PINS:
// FIXME This is hardcoded and should not be.
headSerialReply(8); headSerialReply(8);
for (i = 0; i < 8; i++) for (i = 0; i < 8; i++)
serialize8(i + 1); 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 for (i = 0; i < 8; i++) // FIXME should this use MAX_MOTORS or MAX_SUPPORTED_MOTORS instead of 8
motor_disarmed[i] = read16(); motor_disarmed[i] = read16();
break; break;
case MSP_SET_SERVO_CONF: case MSP_SET_SERVO_CONFIGURATION:
#ifdef USE_SERVOS #ifdef USE_SERVOS
if (currentPort->dataSize % SERVO_CHUNK_SIZE != 0) { i = read8();
debug[0] = currentPort->dataSize; if (i >= MAX_SUPPORTED_SERVOS) {
headSerialError(0); headSerialError(0);
} else { } else {
uint8_t servoCount = currentPort->dataSize / SERVO_CHUNK_SIZE; currentProfile->servoConf[i].min = read16();
for (i = 0; i < MAX_SUPPORTED_SERVOS && i < servoCount; i++) { currentProfile->servoConf[i].max = read16();
currentProfile->servoConf[i].min = read16(); currentProfile->servoConf[i].middle = read16();
currentProfile->servoConf[i].max = read16(); currentProfile->servoConf[i].rate = read8();
currentProfile->servoConf[i].angleAtMin = read8();
// provide temporary support for old clients that try and send a channel index instead of a servo middle currentProfile->servoConf[i].angleAtMax = read8();
uint16_t potentialServoMiddleOrChannelToForward = read16(); currentProfile->servoConf[i].reversedSources = read32();
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].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].forwardFromChannel = read8(); currentProfile->servoConf[i].forwardFromChannel = read8();
} }
#endif #endif
break; 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: case MSP_RESET_CONF:
if (!ARMING_FLAG(ARMED)) { if (!ARMING_FLAG(ARMED)) {
resetEEPROM(); resetEEPROM();

16
src/main/main.c
View file

@ -106,7 +106,11 @@ void mspInit(serialConfig_t *serialConfig);
void cliInit(serialConfig_t *serialConfig); void cliInit(serialConfig_t *serialConfig);
void failsafeInit(rxConfig_t *intialRxConfig); void failsafeInit(rxConfig_t *intialRxConfig);
pwmOutputConfiguration_t *pwmInit(drv_pwm_config_t *init); 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 mixerUsePWMOutputConfiguration(pwmOutputConfiguration_t *pwmOutputConfiguration);
void rxInit(rxConfig_t *rxConfig); void rxInit(rxConfig_t *rxConfig);
void gpsInit(serialConfig_t *serialConfig, gpsConfig_t *initialGpsConfig); void gpsInit(serialConfig_t *serialConfig, gpsConfig_t *initialGpsConfig);
@ -197,7 +201,11 @@ void init(void)
serialInit(&masterConfig.serialConfig, feature(FEATURE_SOFTSERIAL)); 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)); memset(&pwm_params, 0, sizeof(pwm_params));
@ -216,7 +224,7 @@ void init(void)
#endif #endif
// when using airplane/wing mixer, servo/motor outputs are remapped // 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; pwm_params.airplane = true;
else else
pwm_params.airplane = false; pwm_params.airplane = false;
@ -241,7 +249,7 @@ void init(void)
#ifdef USE_SERVOS #ifdef USE_SERVOS
pwm_params.useServos = isMixerUsingServos(); 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.servoCenterPulse = masterConfig.escAndServoConfig.servoCenterPulse;
pwm_params.servoPwmRate = masterConfig.servo_pwm_rate; pwm_params.servoPwmRate = masterConfig.servo_pwm_rate;
#endif #endif

2
src/main/mw.c
View file

@ -780,7 +780,7 @@ void loop(void)
// Allow yaw control for tricopters if the user wants the servo to move even when unarmed. // Allow yaw control for tricopters if the user wants the servo to move even when unarmed.
if (isUsingSticksForArming() && rcData[THROTTLE] <= masterConfig.rxConfig.mincheck if (isUsingSticksForArming() && rcData[THROTTLE] <= masterConfig.rxConfig.mincheck
#ifndef USE_QUAD_MIXER_ONLY #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_AIRPLANE
&& masterConfig.mixerMode != MIXER_FLYING_WING && masterConfig.mixerMode != MIXER_FLYING_WING
#endif #endif

1
src/main/target/CC3D/target.h
View file

@ -120,6 +120,7 @@
// disabled some features for OPBL build due to code size. // disabled some features for OPBL build due to code size.
#undef AUTOTUNE #undef AUTOTUNE
#undef SONAR #undef SONAR
#define SKIP_CLI_COMMAND_HELP
#endif #endif

34
src/test/unit/flight_mixer_unittest.cc
View file

@ -48,7 +48,7 @@ extern "C" {
extern uint8_t servoCount; extern uint8_t servoCount;
void forwardAuxChannelsToServos(uint8_t firstServoIndex); 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); void mixerUsePWMOutputConfiguration(pwmOutputConfiguration_t *pwmOutputConfiguration);
} }
@ -144,12 +144,16 @@ TEST(FlightMixerTest, TestForwardAuxChannelsToServosWithLessRemainingServosThanA
EXPECT_EQ(1250, servos[MAX_SUPPORTED_SERVOS - 0 - 1].value); EXPECT_EQ(1250, servos[MAX_SUPPORTED_SERVOS - 0 - 1].value);
} }
TEST(FlightMixerTest, TestTricopterServo) TEST(FlightMixerTest, TestTricopterServo)
{ {
// given // given
mixerConfig_t mixerConfig; mixerConfig_t mixerConfig;
memset(&mixerConfig, 0, sizeof(mixerConfig)); memset(&mixerConfig, 0, sizeof(mixerConfig));
rxConfig_t rxConfig;
memset(&rxConfig, 0, sizeof(rxConfig));
mixerConfig.tri_unarmed_servo = 1; mixerConfig.tri_unarmed_servo = 1;
escAndServoConfig_t escAndServoConfig; escAndServoConfig_t escAndServoConfig;
@ -165,7 +169,7 @@ TEST(FlightMixerTest, TestTricopterServo)
servoConf[5].forwardFromChannel = CHANNEL_FORWARDING_DISABLED; servoConf[5].forwardFromChannel = CHANNEL_FORWARDING_DISABLED;
gimbalConfig_t gimbalConfig = { gimbalConfig_t gimbalConfig = {
.gimbal_flags = 0 .mode = GIMBAL_MODE_NORMAL
}; };
mixerUseConfigs( mixerUseConfigs(
@ -175,13 +179,16 @@ TEST(FlightMixerTest, TestTricopterServo)
&escAndServoConfig, &escAndServoConfig,
&mixerConfig, &mixerConfig,
NULL, NULL,
NULL &rxConfig
); );
motorMixer_t customMixer[MAX_SUPPORTED_MOTORS]; motorMixer_t customMixer[MAX_SUPPORTED_MOTORS];
memset(&customMixer, 0, sizeof(customMixer)); 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 // and
pwmOutputConfiguration_t pwmOutputConfiguration = { pwmOutputConfiguration_t pwmOutputConfiguration = {
@ -213,31 +220,38 @@ TEST(FlightMixerTest, TestQuadMotors)
mixerConfig_t mixerConfig; mixerConfig_t mixerConfig;
memset(&mixerConfig, 0, sizeof(mixerConfig)); memset(&mixerConfig, 0, sizeof(mixerConfig));
//servoParam_t servoConf[MAX_SUPPORTED_SERVOS]; rxConfig_t rxConfig;
//memset(&servoConf, 0, sizeof(servoConf)); memset(&rxConfig, 0, sizeof(rxConfig));
servoParam_t servoConf[MAX_SUPPORTED_SERVOS];
memset(&servoConf, 0, sizeof(servoConf));
escAndServoConfig_t escAndServoConfig; escAndServoConfig_t escAndServoConfig;
memset(&escAndServoConfig, 0, sizeof(escAndServoConfig)); memset(&escAndServoConfig, 0, sizeof(escAndServoConfig));
escAndServoConfig.mincommand = TEST_MIN_COMMAND; escAndServoConfig.mincommand = TEST_MIN_COMMAND;
gimbalConfig_t gimbalConfig = { gimbalConfig_t gimbalConfig = {
.gimbal_flags = 0 .mode = GIMBAL_MODE_NORMAL
}; };
mixerUseConfigs( mixerUseConfigs(
NULL,// servoConf, servoConf,
&gimbalConfig, &gimbalConfig,
NULL, NULL,
&escAndServoConfig, &escAndServoConfig,
&mixerConfig, &mixerConfig,
NULL, NULL,
NULL &rxConfig
); );
motorMixer_t customMixer[MAX_SUPPORTED_MOTORS]; motorMixer_t customMixer[MAX_SUPPORTED_MOTORS];
memset(&customMixer, 0, sizeof(customMixer)); 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 // and
pwmOutputConfiguration_t pwmOutputConfiguration = { pwmOutputConfiguration_t pwmOutputConfiguration = {