Skip to content
New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Mixer refactoring - make FC unaware of anything besides mixer rules #2978

Merged
merged 19 commits into from Apr 21, 2018
Merged
Show file tree
Hide file tree
Changes from all commits
Commits
File filter

Filter by extension

Filter by extension

Conversations
Failed to load comments.
Jump to
Jump to file
Failed to load files.
Diff view
Diff view
4 changes: 3 additions & 1 deletion docs/Cli.md
Expand Up @@ -79,7 +79,6 @@ Re-apply any new defaults as desired.
| `help` | |
| `led` | configure leds |
| `map` | mapping of rc channel order |
| `mixer` | mixer name or list |
| `motor` | get/set motor output value |
| `play_sound` | index, or none for next |
| `profile` | index (0 to 2) |
Expand Down Expand Up @@ -414,6 +413,9 @@ Re-apply any new defaults as desired.
| rssi_adc_channel | - | ADC channel to use for analog RSSI input. Defaults to board RSSI input (if available). 0 = disabled |
| current_adc_channel | - | ADC channel to use for analog current sensor input. Defaults to board CURRENT sensor input (if available). 0 = disabled |
| airspeed_adc_channel | 0 | ADC channel to use for analog pitot tube (airspeed) sensor. If board doesn't have a dedicated connector for analog airspeed sensor will default to 0 |
| platform_type | "MULTIROTOR" | Defines UAV platform type. Allowed values: "MULTIROTOR", "AIRPLANE", "HELICOPTER", "TRICOPTER", "ROVER", "BOAT". Currently only MULTIROTOR, AIRPLANE and TRICOPTER types are implemented |
| has_flaps | OFF | Defines is UAV is capable of having flaps. If ON and AIRPLANE `platform_type` is used, **FLAPERON** flight mode will be available for the pilot |
| mixer_preset | -1 | ID of mixer preset applied in a Configurator. **Do not modify manually**. Used only for backup/restore reasons. |

This Markdown table is made by MarkdwonTableMaker addon for google spreadsheet.
Original Spreadsheet used to make this table can be found here https://docs.google.com/spreadsheets/d/1ubjYdMGmZ2aAMUNYkdfe3hhIF7wRfIjcuPOi_ysmp00/edit?usp=sharing
106 changes: 41 additions & 65 deletions docs/Mixer.md
@@ -1,83 +1,31 @@
# Mixer
# Mixer and platform type

INAV supports a number of mixing configurations as well as custom mixing. Mixer configurations determine how the servos and motors work together to control the aircraft.

## Configuration

To use a built-in mixing configuration, you can use the Chrome configuration GUI. It includes images of the various mixer types to assist in making the proper connections. See the Configuration section of the documentation for more information on the GUI.

You can also use the Command Line Interface (CLI) to set the mixer type:

1. Use `mixer list` to see a list of supported mixes
2. Select a mixer. For example, to select TRI, use `mixer TRI`
3. You must use `save` to preserve your changes

## Supported Mixer Types

| Name | Description | Motors | Servos |
| ---------------- | ------------------------- | -------------- | ---------------- |
| TRI | Tricopter | M1-M3 | S1 |
| QUADP | Quadcopter-Plus | M1-M4 | None |
| QUADX | Quadcopter-X | M1-M4 | None |
| BI | Bicopter (left/right) | M1-M2 | S1, S2 |
| GIMBAL | Gimbal control | N/A | S1, S2 |
| Y6 | Y6-copter | M1-M6 | None |
| HEX6 | Hexacopter-Plus | M1-M6 | None |
| FLYING_WING | Fixed wing; elevons | M1 | S1, S2 |
| Y4 | Y4-copter | M1-M4 | None |
| HEX6X | Hexacopter-X | M1-M6 | None |
| OCTOX8 | Octocopter-X (over/under) | M1-M8 | None |
| OCTOFLATP | Octocopter-FlatPlus | M1-M8 | None |
| OCTOFLATX | Octocopter-FlatX | M1-M8 | None |
| AIRPLANE | Fixed wing; Ax2, R, E | M1 | S1, S2, S3, S4 |
| HELI_120_CCPM | | | |
| HELI_90_DEG | | | |
| VTAIL4 | Quadcopter with V-Tail | M1-M4 | N/A |
| HEX6H | Hexacopter-H | M1-M6 | None |
| PPM_TO_SERVO | | | |
| DUALCOPTER | Dualcopter | M1-M2 | S1, S2 |
| SINGLECOPTER | Conventional helicopter | M1 | S1 |
| ATAIL4 | Quadcopter with A-Tail | M1-M4 | N/A |
| CUSTOM | User-defined | | |
| CUSTOM AIRPLANE | User-defined airplane | | |
| CUSTOM TRICOPTER | User-defined tricopter | | |
INAV Configurator provides graphical user interface for mixer configuration. All supported vehicle types are configurable with _mixer presets_ using Configurator. `mmix` and `smix` manual configuration in CLI should be used only for backup/restore purposes.

## Servo configuration
User interface is described in [this video](https://www.youtube.com/watch?v=0cLFu-5syi0)

The cli `servo` command defines the settings for the servo outputs.
The cli mixer `smix` command controllers how the mixer maps internal FC data (RC input, PID stabilization output, channel forwarding, etc) to servo outputs.
## Platform type

## Servo filtering
INAV can be used on a variety of vehicle types configured via Configurator or `platform_type` CLI property. Certain settings applies only when specific platform type is selected. For example, _flaps_ can be configured only if **AIRPLANE** platform type is used. The same goes for flight modes, output mappings, stabilization algorithms, etc.

A low-pass filter can be enabled for the servos. It may be useful for avoiding structural modes in the airframe, for example.
Currently, following platform types are supported:

### Configuration
* MULTIROTOR
* AIRPLANE
* TRICOPTER

Currently, it can only be configured via the CLI:

Use `set servo_lpf_hz=20` to enable filtering. This will set servo low pass filter to 20Hz.

### Tuning

One method for tuning the filter cutoff is as follows:

1. Ensure your vehicle can move at least somewhat freely in the troublesome axis. For example, if you are having yaw oscillations on a tricopter, ensure that the copter is supported in a way that allows it to rotate left and right to at least some degree. Suspension near the CG is ideal. Alternatively, you can just fly the vehicle and trigger the problematic condition you are trying to eliminate, although tuning will be more tedious.

2. Tap the vehicle at its end in the axis under evaluation. Directly commanding the servo in question to move may also be used. In the tricopter example, tap the end of the tail boom from the side, or command a yaw using your transmitter.

3. If your vehicle oscillates for several seconds or even continues oscillating indefinitely, then the filter cutoff frequency should be reduced. Reduce the value of `servo_lowpass_freq` by half its current value and repeat the previous step.

4. If the oscillations are dampened within roughly a second or are no longer present, then you are done. Be sure to run `save`.

## Custom Motor Mixing
## Motor Mixing

Custom motor mixing allows for completely customized motor configurations. Each motor must be defined with a custom mixing table for that motor. The mix must reflect how close each motor is with reference to the CG (Center of Gravity) of the flight controller. A motor closer to the CG of the flight controller will need to travel less distance than a motor further away.

Steps to configure custom mixer in the CLI:

1. Use `mixer custom` to enable the custom mixing.
2. Use `mmix reset` to erase any existing custom mixing.
3. Issue a `mmix` statement for each motor.
1. Use `mmix reset` to erase any existing custom mixing.
1. Issue a `mmix` statement for each motor.

The mmix statement has the following syntax: `mmix n THROTTLE ROLL PITCH YAW`

Expand All @@ -91,7 +39,7 @@ The mmix statement has the following syntax: `mmix n THROTTLE ROLL PITCH YAW`

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
## Servo Mixing

Custom servo mixing rules can be applied to each servo. Rules are applied in the CLI using `smix`. Rules link flight controller stabilization and receiver signals to physical PWM output pins on the FC board. Currently, pin id's 0 and 1 can only be used for motor outputs. Other pins may or may not work depending on the board you are using.

Expand Down Expand Up @@ -176,6 +124,34 @@ i.e. when mixing rudder servo slot (`5`) using Stabilised YAW input source (`2`)

`smix reverse` is a per-profile setting. So ensure you configure it for your profiles as required.

## Servo configuration

The cli `servo` command defines the settings for the servo outputs.
The cli mixer `smix` command controllers how the mixer maps internal FC data (RC input, PID stabilization output, channel forwarding, etc) to servo outputs.

## Servo filtering

A low-pass filter can be enabled for the servos. It may be useful for avoiding structural modes in the airframe, for example.

### Configuration

Currently, it can only be configured via the CLI:

Use `set servo_lpf_hz=20` to enable filtering. This will set servo low pass filter to 20Hz.

### Tuning

One method for tuning the filter cutoff is as follows:

1. Ensure your vehicle can move at least somewhat freely in the troublesome axis. For example, if you are having yaw oscillations on a tricopter, ensure that the copter is supported in a way that allows it to rotate left and right to at least some degree. Suspension near the CG is ideal. Alternatively, you can just fly the vehicle and trigger the problematic condition you are trying to eliminate, although tuning will be more tedious.

2. Tap the vehicle at its end in the axis under evaluation. Directly commanding the servo in question to move may also be used. In the tricopter example, tap the end of the tail boom from the side, or command a yaw using your transmitter.

3. If your vehicle oscillates for several seconds or even continues oscillating indefinitely, then the filter cutoff frequency should be reduced. Reduce the value of `servo_lowpass_freq` by half its current value and repeat the previous step.

4. If the oscillations are dampened within roughly a second or are no longer present, then you are done. Be sure to run `save`.


### Example 1: A KK2.0 wired motor setup
Here's an example of an X configuration quad, but the motors are still wired using the KK board motor numbering scheme.

Expand Down
4 changes: 1 addition & 3 deletions src/main/blackbox/blackbox.c
Expand Up @@ -476,7 +476,7 @@ static bool testBlackboxConditionUncached(FlightLogFieldCondition condition)
return getMotorCount() >= condition - FLIGHT_LOG_FIELD_CONDITION_AT_LEAST_MOTORS_1 + 1;

case FLIGHT_LOG_FIELD_CONDITION_TRICOPTER:
return mixerConfig()->mixerMode == MIXER_TRI || mixerConfig()->mixerMode == MIXER_CUSTOM_TRI;
return mixerConfig()->platformType == PLATFORM_TRICOPTER;

case FLIGHT_LOG_FIELD_CONDITION_NONZERO_PID_D_0:
case FLIGHT_LOG_FIELD_CONDITION_NONZERO_PID_D_1:
Expand Down Expand Up @@ -1176,10 +1176,8 @@ static void loadMainState(timeUs_t currentTimeUs)

blackboxCurrent->rssi = getRSSI();

#ifdef USE_SERVOS
//Tail servo for tricopters
blackboxCurrent->servo[5] = servo[5];
#endif

#ifdef NAV_BLACKBOX
blackboxCurrent->navState = navCurrentState;
Expand Down
3 changes: 0 additions & 3 deletions src/main/config/config_master.h
Expand Up @@ -28,10 +28,7 @@

#include "fc/config.h"
#include "fc/rc_controls.h"

#ifdef USE_SERVOS
#include "flight/servos.h"
#endif

#include "io/osd.h"
#include "io/ledstrip.h"
Expand Down
37 changes: 2 additions & 35 deletions src/main/drivers/pwm_mapping.c
Expand Up @@ -177,23 +177,19 @@ pwmIOConfiguration_t *pwmInit(drv_pwm_config_t *init)
}

// Handle outputs - may override the PWM/PPM inputs
if (init->flyingPlatformType == PLATFORM_MULTIROTOR) {
if (init->flyingPlatformType == PLATFORM_MULTIROTOR || init->flyingPlatformType == PLATFORM_TRICOPTER) {
// Multicopter
#ifdef USE_SERVOS
if (init->useServoOutputs && (timerHardwarePtr->usageFlags & TIM_USE_MC_SERVO)) {
type = MAP_TO_SERVO_OUTPUT;
}
else if (init->useChannelForwarding && (timerHardwarePtr->usageFlags & TIM_USE_MC_CHNFW)) {
type = MAP_TO_SERVO_OUTPUT;
}
else
#endif
if (timerHardwarePtr->usageFlags & TIM_USE_MC_MOTOR) {
type = MAP_TO_MOTOR_OUTPUT;
}
}
#ifdef USE_SERVOS
else if (init->flyingPlatformType == PLATFORM_AIRPLANE || init->flyingPlatformType == PLATFORM_HELICOPTER) {
} else {
// Fixed wing or HELI (one/two motors and a lot of servos
if (timerHardwarePtr->usageFlags & TIM_USE_FW_SERVO) {
type = MAP_TO_SERVO_OUTPUT;
Expand All @@ -202,37 +198,10 @@ pwmIOConfiguration_t *pwmInit(drv_pwm_config_t *init)
type = MAP_TO_MOTOR_OUTPUT;
}
}
#endif

// If timer not mapped - skip
if (type == MAP_TO_NONE)
continue;
/*
#ifdef USE_SERVOS
if (init->useServos && !init->airplane) {
#if defined(SPRACINGF3MINI)
// remap PWM6+7 as servos
if ((timerIndex == PWM6 || timerIndex == PWM7) && timerHardwarePtr->tim == TIM15)
type = MAP_TO_SERVO_OUTPUT;
#endif

#if defined(SINGULARITY)
// remap PWM6+7 as servos
if (timerIndex == PWM6 || timerIndex == PWM7)
type = MAP_TO_SERVO_OUTPUT;
#endif
}

#if defined(SPRACINGF3MINI)
if (((timerIndex == PWM6 || timerIndex == PWM7) && timerHardwarePtr->tim == TIM15)
|| ((timerIndex == PWM8 || timerIndex == PWM9 || timerIndex == PWM10 || timerIndex == PWM11) && timerHardwarePtr->tim == TIM2)) {
type = MAP_TO_SERVO_OUTPUT;
}
#endif
}

#endif // USE_SERVOS
*/

if (type == MAP_TO_PPM_INPUT) {
#if defined(USE_RX_PPM)
Expand Down Expand Up @@ -284,7 +253,6 @@ pwmIOConfiguration_t *pwmInit(drv_pwm_config_t *init)
continue;
}

#ifdef USE_SERVOS
if (pwmServoConfig(timerHardwarePtr, pwmIOConfiguration.servoCount, init->servoPwmRate, init->servoCenterPulse, init->enablePWMOutput)) {
pwmIOConfiguration.ioConfigurations[pwmIOConfiguration.ioCount].flags = PWM_PF_SERVO | PWM_PF_OUTPUT_PROTOCOL_PWM;
pwmIOConfiguration.ioConfigurations[pwmIOConfiguration.ioCount].index = pwmIOConfiguration.servoCount;
Expand All @@ -298,7 +266,6 @@ pwmIOConfiguration_t *pwmInit(drv_pwm_config_t *init)
addBootlogEvent6(BOOT_EVENT_TIMER_CH_SKIPPED, BOOT_EVENT_FLAGS_WARNING, timerIndex, pwmIOConfiguration.motorCount, pwmIOConfiguration.servoCount, 3);
continue;
}
#endif
} else {
continue;
}
Expand Down
17 changes: 2 additions & 15 deletions src/main/drivers/pwm_mapping.h
Expand Up @@ -18,14 +18,9 @@
#pragma once

#include "drivers/io_types.h"
#include "flight/mixer.h"

#if defined(USE_QUAD_MIXER_ONLY)
#define MAX_PWM_MOTORS 4
#define MAX_PWM_SERVOS 1
#define MAX_MOTORS 4
#define MAX_SERVOS 1

#elif defined(TARGET_MOTOR_COUNT)
#if defined(TARGET_MOTOR_COUNT)
#define MAX_PWM_MOTORS TARGET_MOTOR_COUNT
#define MAX_PWM_SERVOS 8
#define MAX_MOTORS TARGET_MOTOR_COUNT
Expand All @@ -49,12 +44,6 @@ typedef struct rangefinderIOConfig_s {
ioTag_t echoTag;
} rangefinderIOConfig_t;

typedef enum {
PLATFORM_MULTIROTOR = 0,
PLATFORM_AIRPLANE = 1,
PLATFORM_HELICOPTER = 2
} flyingPlatformType_e;

typedef struct drv_pwm_config_s {
int flyingPlatformType;

Expand All @@ -74,12 +63,10 @@ typedef struct drv_pwm_config_s {
#ifdef USE_RANGEFINDER
bool useTriggerRangefinder;
#endif
#ifdef USE_SERVOS
bool useServoOutputs;
bool useChannelForwarding; // configure additional channels as servos
uint16_t servoPwmRate;
uint16_t servoCenterPulse;
#endif
uint8_t pwmProtocolType;
uint16_t motorPwmRate;
uint16_t idlePulse; // PWM value to use when initializing the driver. set this to either PULSE_1MS (regular pwm),
Expand Down
5 changes: 0 additions & 5 deletions src/main/drivers/pwm_output.c
Expand Up @@ -50,10 +50,7 @@ typedef struct {
static pwmOutputPort_t pwmOutputPorts[MAX_PWM_OUTPUT_PORTS];

static pwmOutputPort_t *motors[MAX_PWM_MOTORS];

#ifdef USE_SERVOS
static pwmOutputPort_t *servos[MAX_PWM_SERVOS];
#endif

#ifdef BEEPER_PWM
static pwmOutputPort_t beeperPwmPort;
Expand Down Expand Up @@ -290,7 +287,6 @@ bool pwmMotorConfig(const timerHardware_t *timerHardware, uint8_t motorIndex, ui
return false;
}

#ifdef USE_SERVOS
bool pwmServoConfig(const timerHardware_t *timerHardware, uint8_t servoIndex, uint16_t servoPwmRate, uint16_t servoCenterPulse, bool enableOutput)
{
pwmOutputPort_t * port = pwmOutConfigMotor(timerHardware, PWM_TIMER_MHZ, 1000000 / servoPwmRate, servoCenterPulse, enableOutput);
Expand Down Expand Up @@ -323,7 +319,6 @@ void pwmWriteServo(uint8_t index, uint16_t value)
}
#endif
}
#endif

#ifdef BEEPER_PWM
void pwmWriteBeeper(bool onoffBeep)
Expand Down