Add rate profiles and inflight switching between them. See Profiles and
Inflight Adjustment documentation for details.
This commit is contained in:
parent
cac814923c
commit
28f9fa629c
|
@ -92,8 +92,8 @@ The adjustment is made when the adjustment channel is in the high or low positio
|
|||
|
||||
### Adjustment function
|
||||
|
||||
| Value | Adjustment |
|
||||
| ----- | ---------- |
|
||||
| Value | Adjustment | Notes |
|
||||
| ----- | ---------- |------ |
|
||||
| 0 | None |
|
||||
| 1 | RC RATE |
|
||||
| 2 | RC_EXPO |
|
||||
|
@ -106,6 +106,7 @@ The adjustment is made when the adjustment channel is in the high or low positio
|
|||
| 9 | YAW_P |
|
||||
| 10 | YAW_I |
|
||||
| 11 | YAW_D |
|
||||
| 12 | RATE_PROFILE | Switch between 3 rate profiles using a 3 position switch. |
|
||||
|
||||
## Examples
|
||||
|
||||
|
@ -172,3 +173,17 @@ explained:
|
|||
* configure adjrange 8 to use adjustment slot 3 (2) so that when aux2
|
||||
(1) in the range 1850-2100 then use adjustment Yaw D (11) when aux 4
|
||||
(3) is in the appropriate position.
|
||||
|
||||
### Example 4 - Use a single 3 position switch to change between 3 different rate profiles
|
||||
|
||||
adjrange 11 3 3 900 2100 12 3
|
||||
|
||||
explained:
|
||||
|
||||
* configure adjrange 11 to use adjustment slot 4 (3) so that when aux4
|
||||
(3) in the range 900-2100 then use adjustment Rate Profile (12) when aux 4
|
||||
(3) is in the appropriate position.
|
||||
|
||||
When the switch is low, rate profile 0 is selcted.
|
||||
When the switch is medium, rate profile 1 is selcted.
|
||||
When the switch is high, rate profile 2 is selcted.
|
||||
|
|
|
@ -0,0 +1,65 @@
|
|||
# Profiles
|
||||
|
||||
A profile is a set of configuration settings.
|
||||
|
||||
Current 3 profiles are supported, The default profile is profile `0`.
|
||||
|
||||
## Changing profiles
|
||||
|
||||
Profiles can be slected using a GUI or the following stick combinations
|
||||
|
||||
| Profile | Throttle | Yaw | Pitch | Roll |
|
||||
| ------- | -------- | ----- | ------ | ------ |
|
||||
| 0 | Down | Left | Middle | Left |
|
||||
| 1 | Down | Left | Up | Middle |
|
||||
| 2 | Down | Left | Middle | Right |
|
||||
|
||||
The cli `profile` command can also be used:
|
||||
|
||||
```
|
||||
profile <index>
|
||||
```
|
||||
|
||||
# Rate Profiles
|
||||
|
||||
Cleanflight supports rate profiles in addition to regular profiles.
|
||||
|
||||
Rate profiles contain settings that adjust how your craft behaves based on control input.
|
||||
|
||||
3 rate profiles are supported.
|
||||
|
||||
Rate profiles can be selected while flying using the inflight adjustments feature.
|
||||
|
||||
Each normal profile has a setting called 'default_rate_profile`. When a profile is activated the
|
||||
corresponding rate profile is also activated.
|
||||
|
||||
Profile 0 has a default rate profile of 0.
|
||||
Profile 1 has a default rate profile of 1.
|
||||
Profile 2 has a default rate profile of 2.
|
||||
|
||||
The defaults are set this way so that it's simple to configure a profile and a rate profile at the same.
|
||||
|
||||
The current rate profile can be shown or set using the CLI `rateprofile` command:
|
||||
|
||||
```
|
||||
rateprofile <index>
|
||||
```
|
||||
|
||||
The values contained within a rate profile can be seen by using the CLI `dump rates` command.
|
||||
|
||||
e.g
|
||||
```
|
||||
# dump rates
|
||||
|
||||
# rateprofile
|
||||
rateprofile 0
|
||||
|
||||
set rc_rate = 90
|
||||
set rc_expo = 65
|
||||
set thr_mid = 50
|
||||
set thr_expo = 0
|
||||
set roll_pitch_rate = 0
|
||||
set yaw_rate = 0
|
||||
set tpa_rate = 0
|
||||
set tpa_breakpoint = 1500
|
||||
```
|
|
@ -98,10 +98,11 @@ void useRcControlsConfig(modeActivationCondition_t *modeActivationConditions, es
|
|||
// use the last flash pages for storage
|
||||
#define CONFIG_START_FLASH_ADDRESS (0x08000000 + (uint32_t)((FLASH_PAGE_SIZE * FLASH_PAGE_COUNT) - FLASH_TO_RESERVE_FOR_CONFIG))
|
||||
|
||||
master_t masterConfig; // master config struct with data independent from profiles
|
||||
profile_t *currentProfile; // profile config struct
|
||||
master_t masterConfig; // master config struct with data independent from profiles
|
||||
profile_t *currentProfile;
|
||||
controlRateConfig_t *currentControlRateProfile;
|
||||
|
||||
static const uint8_t EEPROM_CONF_VERSION = 83;
|
||||
static const uint8_t EEPROM_CONF_VERSION = 84;
|
||||
|
||||
static void resetAccelerometerTrims(flightDynamicsTrims_t *accelerometerTrims)
|
||||
{
|
||||
|
@ -231,12 +232,36 @@ void resetSerialConfig(serialConfig_t *serialConfig)
|
|||
serialConfig->reboot_character = 'R';
|
||||
}
|
||||
|
||||
static void resetControlRateConfig(controlRateConfig_t *controlRateConfig) {
|
||||
controlRateConfig->rcRate8 = 90;
|
||||
controlRateConfig->rcExpo8 = 65;
|
||||
controlRateConfig->rollPitchRate = 0;
|
||||
controlRateConfig->yawRate = 0;
|
||||
controlRateConfig->thrMid8 = 50;
|
||||
controlRateConfig->thrExpo8 = 0;
|
||||
controlRateConfig->dynThrPID = 0;
|
||||
controlRateConfig->tpa_breakpoint = 1500;
|
||||
|
||||
}
|
||||
|
||||
static void setProfile(uint8_t profileIndex)
|
||||
{
|
||||
currentProfile = &masterConfig.profile[profileIndex];
|
||||
}
|
||||
|
||||
static uint8_t currentControlRateProfileIndex = 0;
|
||||
|
||||
uint8_t getCurrentControlRateProfile(void)
|
||||
{
|
||||
return currentControlRateProfileIndex;
|
||||
}
|
||||
|
||||
static void setControlRateProfile(uint8_t profileIndex)
|
||||
{
|
||||
currentControlRateProfileIndex = profileIndex;
|
||||
currentControlRateProfile = &masterConfig.controlRateProfiles[profileIndex];
|
||||
}
|
||||
|
||||
// Default settings
|
||||
static void resetConf(void)
|
||||
{
|
||||
|
@ -322,14 +347,7 @@ static void resetConf(void)
|
|||
currentProfile->pidController = 0;
|
||||
resetPidProfile(¤tProfile->pidProfile);
|
||||
|
||||
currentProfile->controlRateConfig.rcRate8 = 90;
|
||||
currentProfile->controlRateConfig.rcExpo8 = 65;
|
||||
currentProfile->controlRateConfig.rollPitchRate = 0;
|
||||
currentProfile->controlRateConfig.yawRate = 0;
|
||||
currentProfile->dynThrPID = 0;
|
||||
currentProfile->tpa_breakpoint = 1500;
|
||||
currentProfile->controlRateConfig.thrMid8 = 50;
|
||||
currentProfile->controlRateConfig.thrExpo8 = 0;
|
||||
resetControlRateConfig(&masterConfig.controlRateProfiles[0]);
|
||||
|
||||
// for (i = 0; i < CHECKBOXITEMS; i++)
|
||||
// cfg.activate[i] = 0;
|
||||
|
@ -391,8 +409,18 @@ static void resetConf(void)
|
|||
#endif
|
||||
|
||||
// copy first profile into remaining profile
|
||||
for (i = 1; i < 3; i++)
|
||||
for (i = 1; i < MAX_PROFILE_COUNT; i++) {
|
||||
memcpy(&masterConfig.profile[i], currentProfile, sizeof(profile_t));
|
||||
}
|
||||
|
||||
// copy first control rate config into remaining profile
|
||||
for (i = 1; i < MAX_CONTROL_RATE_PROFILE_COUNT; i++) {
|
||||
memcpy(&masterConfig.controlRateProfiles[i], currentControlRateProfile, sizeof(controlRateConfig_t));
|
||||
}
|
||||
|
||||
for (i = 1; i < MAX_PROFILE_COUNT; i++) {
|
||||
masterConfig.profile[i].default_rateProfile_index = i % MAX_CONTROL_RATE_PROFILE_COUNT;
|
||||
}
|
||||
}
|
||||
|
||||
static uint8_t calculateChecksum(const uint8_t *data, uint32_t length)
|
||||
|
@ -427,12 +455,18 @@ static bool isEEPROMContentValid(void)
|
|||
return true;
|
||||
}
|
||||
|
||||
void activateControlRateConfig(void)
|
||||
{
|
||||
generatePitchRollCurve(currentControlRateProfile);
|
||||
generateThrottleCurve(currentControlRateProfile, &masterConfig.escAndServoConfig);
|
||||
}
|
||||
|
||||
void activateConfig(void)
|
||||
{
|
||||
static imuRuntimeConfig_t imuRuntimeConfig;
|
||||
|
||||
generatePitchRollCurve(¤tProfile->controlRateConfig);
|
||||
generateThrottleCurve(¤tProfile->controlRateConfig, &masterConfig.escAndServoConfig);
|
||||
activateControlRateConfig();
|
||||
|
||||
useRcControlsConfig(currentProfile->modeActivationConditions, &masterConfig.escAndServoConfig, ¤tProfile->pidProfile);
|
||||
|
||||
useGyroConfig(&masterConfig.gyroConfig);
|
||||
|
@ -565,12 +599,17 @@ void readEEPROM(void)
|
|||
|
||||
// Read flash
|
||||
memcpy(&masterConfig, (char *) CONFIG_START_FLASH_ADDRESS, sizeof(master_t));
|
||||
// Copy current profile
|
||||
if (masterConfig.current_profile_index > 2) // sanity check
|
||||
|
||||
if (masterConfig.current_profile_index > MAX_PROFILE_COUNT - 1) // sanity check
|
||||
masterConfig.current_profile_index = 0;
|
||||
|
||||
setProfile(masterConfig.current_profile_index);
|
||||
|
||||
if (currentProfile->default_rateProfile_index > MAX_CONTROL_RATE_PROFILE_COUNT - 1) // sanity check
|
||||
currentProfile->default_rateProfile_index = 0;
|
||||
|
||||
setControlRateProfile(currentProfile->default_rateProfile_index);
|
||||
|
||||
validateAndFixConfig();
|
||||
activateConfig();
|
||||
}
|
||||
|
@ -663,6 +702,15 @@ void changeProfile(uint8_t profileIndex)
|
|||
blinkLedAndSoundBeeper(2, 40, profileIndex + 1);
|
||||
}
|
||||
|
||||
void changeControlRateProfile(uint8_t profileIndex)
|
||||
{
|
||||
if (profileIndex > MAX_CONTROL_RATE_PROFILE_COUNT) {
|
||||
profileIndex = MAX_CONTROL_RATE_PROFILE_COUNT - 1;
|
||||
}
|
||||
setControlRateProfile(profileIndex);
|
||||
activateControlRateConfig();
|
||||
}
|
||||
|
||||
bool feature(uint32_t mask)
|
||||
{
|
||||
return masterConfig.enabledFeatures & mask;
|
||||
|
|
|
@ -17,6 +17,10 @@
|
|||
|
||||
#pragma once
|
||||
|
||||
#define MAX_PROFILE_COUNT 3
|
||||
#define MAX_CONTROL_RATE_PROFILE_COUNT 3
|
||||
|
||||
|
||||
typedef enum {
|
||||
FEATURE_RX_PPM = 1 << 0,
|
||||
FEATURE_VBAT = 1 << 1,
|
||||
|
@ -55,5 +59,8 @@ void ensureEEPROMContainsValidData(void);
|
|||
void saveConfigAndNotify(void);
|
||||
void changeProfile(uint8_t profileIndex);
|
||||
|
||||
uint8_t getCurrentControlRateProfile(void);
|
||||
void changeControlRateProfile(uint8_t profileIndex);
|
||||
|
||||
bool canSoftwareSerialBeUsed(void);
|
||||
|
||||
|
|
|
@ -79,8 +79,10 @@ typedef struct master_t {
|
|||
hsvColor_t colors[CONFIGURABLE_COLOR_COUNT];
|
||||
#endif
|
||||
|
||||
profile_t profile[3]; // 3 separate profiles
|
||||
uint8_t current_profile_index; // currently loaded profile
|
||||
profile_t profile[MAX_PROFILE_COUNT];
|
||||
uint8_t current_profile_index;
|
||||
controlRateConfig_t controlRateProfiles[MAX_CONTROL_RATE_PROFILE_COUNT];
|
||||
|
||||
|
||||
uint8_t magic_ef; // magic number, should be 0xEF
|
||||
uint8_t chk; // XOR checksum
|
||||
|
@ -88,3 +90,4 @@ typedef struct master_t {
|
|||
|
||||
extern master_t masterConfig;
|
||||
extern profile_t *currentProfile;
|
||||
extern controlRateConfig_t *currentControlRateProfile;
|
||||
|
|
|
@ -22,10 +22,7 @@ typedef struct profile_s {
|
|||
|
||||
pidProfile_t pidProfile;
|
||||
|
||||
controlRateConfig_t controlRateConfig;
|
||||
|
||||
uint8_t dynThrPID;
|
||||
uint16_t tpa_breakpoint; // Breakpoint where TPA is activated
|
||||
uint8_t default_rateProfile_index;
|
||||
|
||||
int16_t mag_declination; // Get your magnetic decliniation from here : http://magnetic-declination.com/
|
||||
// For example, -6deg 37min, = -637 Japan, format is [sign]dddmm (degreesminutes) default is zero.
|
||||
|
|
|
@ -418,6 +418,20 @@ void applyStepAdjustment(controlRateConfig_t *controlRateConfig, uint8_t adjustm
|
|||
};
|
||||
}
|
||||
|
||||
void changeControlRateProfile(uint8_t profileIndex);
|
||||
|
||||
void applySelectAdjustment(uint8_t adjustmentFunction, uint8_t position) {
|
||||
|
||||
queueConfirmationBeep(position + 1);
|
||||
switch(adjustmentFunction) {
|
||||
case ADJUSTMENT_RATE_PROFILE:
|
||||
if (getCurrentControlRateProfile() != position) {
|
||||
changeControlRateProfile(position);
|
||||
}
|
||||
break;
|
||||
}
|
||||
}
|
||||
|
||||
#define RESET_FREQUENCY_2HZ (1000 / 2)
|
||||
|
||||
void processRcAdjustments(controlRateConfig_t *controlRateConfig, rxConfig_t *rxConfig)
|
||||
|
@ -463,9 +477,14 @@ void processRcAdjustments(controlRateConfig_t *controlRateConfig, rxConfig_t *rx
|
|||
continue;
|
||||
}
|
||||
|
||||
MARK_ADJUSTMENT_FUNCTION_AS_BUSY(adjustmentIndex);
|
||||
applyStepAdjustment(controlRateConfig, adjustmentFunction, delta);
|
||||
} else if (adjustmentState->config->mode == ADJUSTMENT_MODE_SELECT) {
|
||||
uint16_t rangeWidth = ((2100 - 900) / adjustmentState->config->data.selectConfig.switchPositions);
|
||||
uint8_t position = (constrain(rcData[channelIndex], 900, 2100 - 1) - 900) / rangeWidth;
|
||||
|
||||
applySelectAdjustment(adjustmentFunction, position);
|
||||
}
|
||||
MARK_ADJUSTMENT_FUNCTION_AS_BUSY(adjustmentIndex);
|
||||
}
|
||||
}
|
||||
|
||||
|
|
|
@ -122,6 +122,8 @@ typedef struct controlRateConfig_s {
|
|||
uint8_t thrExpo8;
|
||||
uint8_t rollPitchRate;
|
||||
uint8_t yawRate;
|
||||
uint8_t dynThrPID;
|
||||
uint16_t tpa_breakpoint; // Breakpoint where TPA is activated
|
||||
} controlRateConfig_t;
|
||||
|
||||
extern int16_t rcCommand[4];
|
||||
|
|
|
@ -89,6 +89,7 @@ static void cliColor(char *cmdline);
|
|||
static void cliMixer(char *cmdline);
|
||||
static void cliMotor(char *cmdline);
|
||||
static void cliProfile(char *cmdline);
|
||||
static void cliRateProfile(char *cmdline);
|
||||
static void cliSave(char *cmdline);
|
||||
static void cliSet(char *cmdline);
|
||||
static void cliGet(char *cmdline);
|
||||
|
@ -163,6 +164,7 @@ const clicmd_t cmdTable[] = {
|
|||
{ "mixer", "mixer name or list", cliMixer },
|
||||
{ "motor", "get/set motor output value", cliMotor },
|
||||
{ "profile", "index (0 to 2)", cliProfile },
|
||||
{ "rateprofile", "index (0 to 2)", cliRateProfile },
|
||||
{ "save", "save and reboot", cliSave },
|
||||
{ "set", "name=value or blank or * for list", cliSet },
|
||||
{ "status", "show system status", cliStatus },
|
||||
|
@ -179,15 +181,16 @@ typedef enum {
|
|||
VAR_FLOAT = (1 << 5),
|
||||
|
||||
MASTER_VALUE = (1 << 6),
|
||||
PROFILE_VALUE = (1 << 7)
|
||||
PROFILE_VALUE = (1 << 7),
|
||||
CONTROL_RATE_VALUE = (1 << 8)
|
||||
} cliValueFlag_e;
|
||||
|
||||
#define VALUE_TYPE_MASK (VAR_UINT8 | VAR_INT8 | VAR_UINT16 | VAR_INT16 | VAR_UINT32 | VAR_FLOAT)
|
||||
#define SECTION_MASK (MASTER_VALUE | PROFILE_VALUE)
|
||||
#define SECTION_MASK (MASTER_VALUE | PROFILE_VALUE | CONTROL_RATE_VALUE)
|
||||
|
||||
typedef struct {
|
||||
const char *name;
|
||||
const uint8_t type; // cliValueFlag_e - specify one of each from VALUE_TYPE_MASK and SECTION_MASK
|
||||
const uint16_t type; // cliValueFlag_e - specify one of each from VALUE_TYPE_MASK and SECTION_MASK
|
||||
void *ptr;
|
||||
const int32_t min;
|
||||
const int32_t max;
|
||||
|
@ -221,7 +224,7 @@ const clivalue_t valueTable[] = {
|
|||
|
||||
{ "flaps_speed", VAR_UINT8 | MASTER_VALUE, &masterConfig.airplaneConfig.flaps_speed, 0, 100 },
|
||||
|
||||
{ "fixedwing_althold_dir", VAR_INT8 | MASTER_VALUE | MASTER_VALUE, &masterConfig.fixedwing_althold_dir, -1, 1 },
|
||||
{ "fixedwing_althold_dir", VAR_INT8 | MASTER_VALUE, &masterConfig.fixedwing_althold_dir, -1, 1 },
|
||||
|
||||
{ "serial_port_1_scenario", VAR_UINT8 | MASTER_VALUE, &masterConfig.serialConfig.serial_port_scenario[0], 0, SERIAL_PORT_SCENARIO_MAX },
|
||||
{ "serial_port_2_scenario", VAR_UINT8 | MASTER_VALUE, &masterConfig.serialConfig.serial_port_scenario[1], 0, SERIAL_PORT_SCENARIO_MAX },
|
||||
|
@ -311,14 +314,15 @@ const clivalue_t valueTable[] = {
|
|||
{ "yaw_direction", VAR_INT8 | PROFILE_VALUE, &masterConfig.profile[0].mixerConfig.yaw_direction, -1, 1 },
|
||||
{ "tri_unarmed_servo", VAR_INT8 | PROFILE_VALUE, &masterConfig.profile[0].mixerConfig.tri_unarmed_servo, 0, 1 },
|
||||
|
||||
{ "rc_rate", VAR_UINT8 | PROFILE_VALUE, &masterConfig.profile[0].controlRateConfig.rcRate8, 0, 250 },
|
||||
{ "rc_expo", VAR_UINT8 | PROFILE_VALUE, &masterConfig.profile[0].controlRateConfig.rcExpo8, 0, 100 },
|
||||
{ "thr_mid", VAR_UINT8 | PROFILE_VALUE, &masterConfig.profile[0].controlRateConfig.thrMid8, 0, 100 },
|
||||
{ "thr_expo", VAR_UINT8 | PROFILE_VALUE, &masterConfig.profile[0].controlRateConfig.thrExpo8, 0, 100 },
|
||||
{ "roll_pitch_rate", VAR_UINT8 | PROFILE_VALUE, &masterConfig.profile[0].controlRateConfig.rollPitchRate, 0, 100 },
|
||||
{ "yaw_rate", VAR_UINT8 | PROFILE_VALUE, &masterConfig.profile[0].controlRateConfig.yawRate, 0, 100 },
|
||||
{ "tpa_rate", VAR_UINT8 | PROFILE_VALUE, &masterConfig.profile[0].dynThrPID, 0, 100},
|
||||
{ "tpa_breakpoint", VAR_UINT16 | PROFILE_VALUE, &masterConfig.profile[0].tpa_breakpoint, PWM_RANGE_MIN, PWM_RANGE_MAX},
|
||||
{ "default_rate_profile", VAR_UINT8 | PROFILE_VALUE , &masterConfig.profile[0].default_rateProfile_index, 0, MAX_CONTROL_RATE_PROFILE_COUNT - 1 },
|
||||
{ "rc_rate", VAR_UINT8 | CONTROL_RATE_VALUE, &masterConfig.controlRateProfiles[0].rcRate8, 0, 250 },
|
||||
{ "rc_expo", VAR_UINT8 | CONTROL_RATE_VALUE, &masterConfig.controlRateProfiles[0].rcExpo8, 0, 100 },
|
||||
{ "thr_mid", VAR_UINT8 | CONTROL_RATE_VALUE, &masterConfig.controlRateProfiles[0].thrMid8, 0, 100 },
|
||||
{ "thr_expo", VAR_UINT8 | CONTROL_RATE_VALUE, &masterConfig.controlRateProfiles[0].thrExpo8, 0, 100 },
|
||||
{ "roll_pitch_rate", VAR_UINT8 | CONTROL_RATE_VALUE, &masterConfig.controlRateProfiles[0].rollPitchRate, 0, 100 },
|
||||
{ "yaw_rate", VAR_UINT8 | CONTROL_RATE_VALUE, &masterConfig.controlRateProfiles[0].yawRate, 0, 100 },
|
||||
{ "tpa_rate", VAR_UINT8 | CONTROL_RATE_VALUE, &masterConfig.controlRateProfiles[0].dynThrPID, 0, 100},
|
||||
{ "tpa_breakpoint", VAR_UINT16 | CONTROL_RATE_VALUE, &masterConfig.controlRateProfiles[0].tpa_breakpoint, PWM_RANGE_MIN, PWM_RANGE_MAX},
|
||||
|
||||
{ "failsafe_delay", VAR_UINT8 | PROFILE_VALUE, &masterConfig.profile[0].failsafeConfig.failsafe_delay, 0, 200 },
|
||||
{ "failsafe_off_delay", VAR_UINT8 | PROFILE_VALUE, &masterConfig.profile[0].failsafeConfig.failsafe_off_delay, 0, 200 },
|
||||
|
@ -696,7 +700,7 @@ static void cliColor(char *cmdline)
|
|||
}
|
||||
#endif
|
||||
|
||||
static void dumpValues(uint8_t mask)
|
||||
static void dumpValues(uint16_t mask)
|
||||
{
|
||||
uint32_t i;
|
||||
const clivalue_t *value;
|
||||
|
@ -715,10 +719,11 @@ static void dumpValues(uint8_t mask)
|
|||
|
||||
typedef enum {
|
||||
DUMP_MASTER = (1 << 0),
|
||||
DUMP_PROFILE = (1 << 1)
|
||||
DUMP_PROFILE = (1 << 1),
|
||||
DUMP_CONTROL_RATE_PROFILE = (1 << 2)
|
||||
} dumpFlags_e;
|
||||
|
||||
#define DUMP_ALL (DUMP_MASTER | DUMP_PROFILE)
|
||||
#define DUMP_ALL (DUMP_MASTER | DUMP_PROFILE | DUMP_CONTROL_RATE_PROFILE)
|
||||
|
||||
|
||||
static const char* const sectionBreak = "\r\n";
|
||||
|
@ -739,6 +744,9 @@ static void cliDump(char *cmdline)
|
|||
if (strcasecmp(cmdline, "profile") == 0) {
|
||||
dumpMask = DUMP_PROFILE; // only
|
||||
}
|
||||
if (strcasecmp(cmdline, "rates") == 0) {
|
||||
dumpMask = DUMP_CONTROL_RATE_PROFILE; // only
|
||||
}
|
||||
|
||||
if (dumpMask & DUMP_MASTER) {
|
||||
|
||||
|
@ -826,6 +834,17 @@ static void cliDump(char *cmdline)
|
|||
|
||||
dumpValues(PROFILE_VALUE);
|
||||
}
|
||||
|
||||
if (dumpMask & DUMP_CONTROL_RATE_PROFILE) {
|
||||
printf("\r\n# dump rates\r\n");
|
||||
|
||||
printf("\r\n# rateprofile\r\n");
|
||||
cliRateProfile("");
|
||||
|
||||
printSectionBreak();
|
||||
|
||||
dumpValues(CONTROL_RATE_VALUE);
|
||||
}
|
||||
}
|
||||
|
||||
static void cliEnter(void)
|
||||
|
@ -1075,7 +1094,7 @@ static void cliProfile(char *cmdline)
|
|||
return;
|
||||
} else {
|
||||
i = atoi(cmdline);
|
||||
if (i >= 0 && i <= 2) {
|
||||
if (i >= 0 && i < MAX_PROFILE_COUNT) {
|
||||
masterConfig.current_profile_index = i;
|
||||
writeEEPROM();
|
||||
readEEPROM();
|
||||
|
@ -1084,6 +1103,24 @@ static void cliProfile(char *cmdline)
|
|||
}
|
||||
}
|
||||
|
||||
static void cliRateProfile(char *cmdline)
|
||||
{
|
||||
uint8_t len;
|
||||
int i;
|
||||
|
||||
len = strlen(cmdline);
|
||||
if (len == 0) {
|
||||
printf("rateprofile %d\r\n", getCurrentControlRateProfile());
|
||||
return;
|
||||
} else {
|
||||
i = atoi(cmdline);
|
||||
if (i >= 0 && i < MAX_CONTROL_RATE_PROFILE_COUNT) {
|
||||
changeControlRateProfile(i);
|
||||
cliRateProfile("");
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
static void cliReboot(void) {
|
||||
cliPrint("\r\nRebooting");
|
||||
waitForSerialPortToFinishTransmitting(cliPort);
|
||||
|
@ -1129,6 +1166,9 @@ static void cliPrintVar(const clivalue_t *var, uint32_t full)
|
|||
if (var->type & PROFILE_VALUE) {
|
||||
ptr = ((uint8_t *)ptr) + (sizeof(profile_t) * masterConfig.current_profile_index);
|
||||
}
|
||||
if (var->type & CONTROL_RATE_VALUE) {
|
||||
ptr = ((uint8_t *)ptr) + (sizeof(controlRateConfig_t) * getCurrentControlRateProfile());
|
||||
}
|
||||
|
||||
switch (var->type & VALUE_TYPE_MASK) {
|
||||
case VAR_UINT8:
|
||||
|
@ -1170,6 +1210,9 @@ static void cliSetVar(const clivalue_t *var, const int_float_value_t value)
|
|||
if (var->type & PROFILE_VALUE) {
|
||||
ptr = ((uint8_t *)ptr) + (sizeof(profile_t) * masterConfig.current_profile_index);
|
||||
}
|
||||
if (var->type & CONTROL_RATE_VALUE) {
|
||||
ptr = ((uint8_t *)ptr) + (sizeof(controlRateConfig_t) * getCurrentControlRateProfile());
|
||||
}
|
||||
|
||||
switch (var->type & VALUE_TYPE_MASK) {
|
||||
case VAR_UINT8:
|
||||
|
|
|
@ -799,13 +799,13 @@ static bool processOutCommand(uint8_t cmdMSP)
|
|||
break;
|
||||
case MSP_RC_TUNING:
|
||||
headSerialReply(7);
|
||||
serialize8(currentProfile->controlRateConfig.rcRate8);
|
||||
serialize8(currentProfile->controlRateConfig.rcExpo8);
|
||||
serialize8(currentProfile->controlRateConfig.rollPitchRate);
|
||||
serialize8(currentProfile->controlRateConfig.yawRate);
|
||||
serialize8(currentProfile->dynThrPID);
|
||||
serialize8(currentProfile->controlRateConfig.thrMid8);
|
||||
serialize8(currentProfile->controlRateConfig.thrExpo8);
|
||||
serialize8(currentControlRateProfile->rcRate8);
|
||||
serialize8(currentControlRateProfile->rcExpo8);
|
||||
serialize8(currentControlRateProfile->rollPitchRate);
|
||||
serialize8(currentControlRateProfile->yawRate);
|
||||
serialize8(currentControlRateProfile->dynThrPID);
|
||||
serialize8(currentControlRateProfile->thrMid8);
|
||||
serialize8(currentControlRateProfile->thrExpo8);
|
||||
break;
|
||||
case MSP_PID:
|
||||
headSerialReply(3 * PID_ITEM_COUNT);
|
||||
|
@ -1098,13 +1098,13 @@ static bool processInCommand(void)
|
|||
}
|
||||
break;
|
||||
case MSP_SET_RC_TUNING:
|
||||
currentProfile->controlRateConfig.rcRate8 = read8();
|
||||
currentProfile->controlRateConfig.rcExpo8 = read8();
|
||||
currentProfile->controlRateConfig.rollPitchRate = read8();
|
||||
currentProfile->controlRateConfig.yawRate = read8();
|
||||
currentProfile->dynThrPID = read8();
|
||||
currentProfile->controlRateConfig.thrMid8 = read8();
|
||||
currentProfile->controlRateConfig.thrExpo8 = read8();
|
||||
currentControlRateProfile->rcRate8 = read8();
|
||||
currentControlRateProfile->rcExpo8 = read8();
|
||||
currentControlRateProfile->rollPitchRate = read8();
|
||||
currentControlRateProfile->yawRate = read8();
|
||||
currentControlRateProfile->dynThrPID = read8();
|
||||
currentControlRateProfile->thrMid8 = read8();
|
||||
currentControlRateProfile->thrExpo8 = read8();
|
||||
break;
|
||||
case MSP_SET_MISC:
|
||||
read16(); // powerfailmeter
|
||||
|
|
|
@ -165,13 +165,13 @@ void annexCode(void)
|
|||
static int32_t vbatCycleTime = 0;
|
||||
|
||||
// PITCH & ROLL only dynamic PID adjustemnt, depending on throttle value
|
||||
if (rcData[THROTTLE] < currentProfile->tpa_breakpoint) {
|
||||
if (rcData[THROTTLE] < currentControlRateProfile->tpa_breakpoint) {
|
||||
prop2 = 100;
|
||||
} else {
|
||||
if (rcData[THROTTLE] < 2000) {
|
||||
prop2 = 100 - (uint16_t)currentProfile->dynThrPID * (rcData[THROTTLE] - currentProfile->tpa_breakpoint) / (2000 - currentProfile->tpa_breakpoint);
|
||||
prop2 = 100 - (uint16_t)currentControlRateProfile->dynThrPID * (rcData[THROTTLE] - currentControlRateProfile->tpa_breakpoint) / (2000 - currentControlRateProfile->tpa_breakpoint);
|
||||
} else {
|
||||
prop2 = 100 - currentProfile->dynThrPID;
|
||||
prop2 = 100 - currentControlRateProfile->dynThrPID;
|
||||
}
|
||||
}
|
||||
|
||||
|
@ -188,7 +188,7 @@ void annexCode(void)
|
|||
|
||||
tmp2 = tmp / 100;
|
||||
rcCommand[axis] = lookupPitchRollRC[tmp2] + (tmp - tmp2 * 100) * (lookupPitchRollRC[tmp2 + 1] - lookupPitchRollRC[tmp2]) / 100;
|
||||
prop1 = 100 - (uint16_t)currentProfile->controlRateConfig.rollPitchRate * tmp / 500;
|
||||
prop1 = 100 - (uint16_t)currentControlRateProfile->rollPitchRate * tmp / 500;
|
||||
prop1 = (uint16_t)prop1 * prop2 / 100;
|
||||
} else if (axis == YAW) {
|
||||
if (currentProfile->yaw_deadband) {
|
||||
|
@ -199,7 +199,7 @@ void annexCode(void)
|
|||
}
|
||||
}
|
||||
rcCommand[axis] = tmp * -masterConfig.yaw_control_direction;
|
||||
prop1 = 100 - (uint16_t)currentProfile->controlRateConfig.yawRate * abs(tmp) / 500;
|
||||
prop1 = 100 - (uint16_t)currentControlRateProfile->yawRate * abs(tmp) / 500;
|
||||
}
|
||||
// FIXME axis indexes into pids. use something like lookupPidIndex(rc_alias_e alias) to reduce coupling.
|
||||
dynP8[axis] = (uint16_t)currentProfile->pidProfile.P8[axis] * prop1 / 100;
|
||||
|
@ -495,8 +495,10 @@ void processRx(void)
|
|||
|
||||
updateActivatedModes(currentProfile->modeActivationConditions);
|
||||
|
||||
updateAdjustmentStates(currentProfile->adjustmentRanges);
|
||||
processRcAdjustments(¤tProfile->controlRateConfig, &masterConfig.rxConfig);
|
||||
if (!cliMode) {
|
||||
updateAdjustmentStates(currentProfile->adjustmentRanges);
|
||||
processRcAdjustments(currentControlRateProfile, &masterConfig.rxConfig);
|
||||
}
|
||||
|
||||
bool canUseHorizonMode = true;
|
||||
|
||||
|
@ -654,7 +656,7 @@ void loop(void)
|
|||
// PID - note this is function pointer set by setPIDController()
|
||||
pid_controller(
|
||||
¤tProfile->pidProfile,
|
||||
¤tProfile->controlRateConfig,
|
||||
currentControlRateProfile,
|
||||
masterConfig.max_angle_inclination,
|
||||
¤tProfile->accelerometerTrims
|
||||
);
|
||||
|
|
|
@ -162,9 +162,10 @@ TEST(RcControlsTest, updateActivatedModesUsingValidAuxConfigurationAndRXValues)
|
|||
|
||||
enum {
|
||||
COUNTER_GENERATE_PITCH_ROLL_CURVE = 0,
|
||||
COUNTER_QUEUE_CONFIRMATION_BEEP
|
||||
COUNTER_QUEUE_CONFIRMATION_BEEP,
|
||||
COUNTER_CHANGE_CONTROL_RATE_PROFILE
|
||||
};
|
||||
#define CALL_COUNT_ITEM_COUNT 2
|
||||
#define CALL_COUNT_ITEM_COUNT 3
|
||||
|
||||
static int callCounts[CALL_COUNT_ITEM_COUNT];
|
||||
|
||||
|
@ -178,13 +179,18 @@ void generatePitchRollCurve(controlRateConfig_t *) {
|
|||
void queueConfirmationBeep(uint8_t) {
|
||||
callCounts[COUNTER_QUEUE_CONFIRMATION_BEEP]++;
|
||||
}
|
||||
|
||||
void changeControlRateProfile(uint8_t) {
|
||||
callCounts[COUNTER_CHANGE_CONTROL_RATE_PROFILE]++;
|
||||
}
|
||||
|
||||
}
|
||||
|
||||
void resetCallCounters(void) {
|
||||
memset(&callCounts, 0, sizeof(callCounts));
|
||||
}
|
||||
|
||||
uint32_t fixedMillis = 0;
|
||||
uint32_t fixedMillis;
|
||||
|
||||
extern "C" {
|
||||
uint32_t millis(void) {
|
||||
|
@ -192,6 +198,10 @@ uint32_t millis(void) {
|
|||
}
|
||||
}
|
||||
|
||||
void resetMillis(void) {
|
||||
fixedMillis = 0;
|
||||
}
|
||||
|
||||
#define DEFAULT_MIN_CHECK 1100
|
||||
#define DEFAULT_MAX_CHECK 1900
|
||||
|
||||
|
@ -216,6 +226,8 @@ TEST(RcControlsTest, processRcAdjustmentsSticksInMiddle)
|
|||
.thrExpo8 = 0,
|
||||
.rollPitchRate = 0,
|
||||
.yawRate = 0,
|
||||
.dynThrPID = 0,
|
||||
.tpa_breakpoint = 0
|
||||
};
|
||||
|
||||
// and
|
||||
|
@ -224,6 +236,7 @@ TEST(RcControlsTest, processRcAdjustmentsSticksInMiddle)
|
|||
rxConfig.maxcheck = DEFAULT_MAX_CHECK;
|
||||
rxConfig.midrc = 1500;
|
||||
|
||||
adjustmentStateMask = 0;
|
||||
memset(&adjustmentStates, 0, sizeof(adjustmentStates));
|
||||
configureAdjustment(0, AUX3 - NON_AUX_CHANNEL_COUNT, &rateAdjustmentConfig);
|
||||
|
||||
|
@ -235,6 +248,7 @@ TEST(RcControlsTest, processRcAdjustmentsSticksInMiddle)
|
|||
|
||||
// and
|
||||
resetCallCounters();
|
||||
resetMillis();
|
||||
|
||||
// when
|
||||
processRcAdjustments(&controlRateConfig, &rxConfig);
|
||||
|
@ -256,6 +270,8 @@ TEST(RcControlsTest, processRcAdjustmentsWithRcRateFunctionSwitchUp)
|
|||
.thrExpo8 = 0,
|
||||
.rollPitchRate = 0,
|
||||
.yawRate = 0,
|
||||
.dynThrPID = 0,
|
||||
.tpa_breakpoint = 0
|
||||
};
|
||||
|
||||
// and
|
||||
|
@ -265,6 +281,8 @@ TEST(RcControlsTest, processRcAdjustmentsWithRcRateFunctionSwitchUp)
|
|||
rxConfig.midrc = 1500;
|
||||
|
||||
// and
|
||||
adjustmentStateMask = 0;
|
||||
memset(&adjustmentStates, 0, sizeof(adjustmentStates));
|
||||
configureAdjustment(0, AUX3 - NON_AUX_CHANNEL_COUNT, &rateAdjustmentConfig);
|
||||
|
||||
// and
|
||||
|
@ -275,6 +293,7 @@ TEST(RcControlsTest, processRcAdjustmentsWithRcRateFunctionSwitchUp)
|
|||
|
||||
// and
|
||||
resetCallCounters();
|
||||
resetMillis();
|
||||
|
||||
// and
|
||||
rcData[AUX3] = PWM_RANGE_MAX;
|
||||
|
@ -400,7 +419,64 @@ TEST(RcControlsTest, processRcAdjustmentsWithRcRateFunctionSwitchUp)
|
|||
EXPECT_EQ(CALL_COUNTER(COUNTER_GENERATE_PITCH_ROLL_CURVE), 3);
|
||||
EXPECT_EQ(CALL_COUNTER(COUNTER_QUEUE_CONFIRMATION_BEEP), 3);
|
||||
EXPECT_EQ(adjustmentStateMask, expectedAdjustmentStateMask);
|
||||
}
|
||||
|
||||
static const adjustmentConfig_t rateProfileAdjustmentConfig = {
|
||||
.adjustmentFunction = ADJUSTMENT_RATE_PROFILE,
|
||||
.mode = ADJUSTMENT_MODE_SELECT,
|
||||
.data = { 3 }
|
||||
};
|
||||
|
||||
TEST(RcControlsTest, processRcRateProfileAdjustments)
|
||||
{
|
||||
// given
|
||||
controlRateConfig_t controlRateConfig = {
|
||||
.rcRate8 = 90,
|
||||
.rcExpo8 = 0,
|
||||
.thrMid8 = 0,
|
||||
.thrExpo8 = 0,
|
||||
.rollPitchRate = 0,
|
||||
.yawRate = 0,
|
||||
.dynThrPID = 0,
|
||||
.tpa_breakpoint = 0
|
||||
};
|
||||
|
||||
// and
|
||||
memset(&rxConfig, 0, sizeof (rxConfig));
|
||||
rxConfig.mincheck = DEFAULT_MIN_CHECK;
|
||||
rxConfig.maxcheck = DEFAULT_MAX_CHECK;
|
||||
rxConfig.midrc = 1500;
|
||||
|
||||
adjustmentStateMask = 0;
|
||||
memset(&adjustmentStates, 0, sizeof(adjustmentStates));
|
||||
|
||||
int adjustmentIndex = 3;
|
||||
configureAdjustment(adjustmentIndex, AUX4 - NON_AUX_CHANNEL_COUNT, &rateProfileAdjustmentConfig);
|
||||
|
||||
// and
|
||||
uint8_t index;
|
||||
for (index = AUX1; index < MAX_SUPPORTED_RC_CHANNEL_COUNT; index++) {
|
||||
rcData[index] = PWM_RANGE_MIDDLE;
|
||||
}
|
||||
|
||||
// and
|
||||
resetCallCounters();
|
||||
resetMillis();
|
||||
|
||||
// and
|
||||
rcData[AUX4] = PWM_RANGE_MAX;
|
||||
|
||||
// and
|
||||
uint8_t expectedAdjustmentStateMask =
|
||||
(1 << adjustmentIndex);
|
||||
|
||||
// when
|
||||
processRcAdjustments(&controlRateConfig, &rxConfig);
|
||||
|
||||
// then
|
||||
EXPECT_EQ(CALL_COUNTER(COUNTER_QUEUE_CONFIRMATION_BEEP), 1);
|
||||
EXPECT_EQ(CALL_COUNTER(COUNTER_CHANGE_CONTROL_RATE_PROFILE), 1);
|
||||
EXPECT_EQ(adjustmentStateMask, expectedAdjustmentStateMask);
|
||||
}
|
||||
|
||||
extern "C" {
|
||||
|
@ -415,6 +491,9 @@ void mwArm(void) {}
|
|||
void feature(uint32_t) {}
|
||||
void sensors(uint32_t) {}
|
||||
void mwDisarm(void) {}
|
||||
uint8_t getCurrentControlRateProfile(void) {
|
||||
return 0;
|
||||
}
|
||||
|
||||
uint8_t armingFlags = 0;
|
||||
int16_t heading;
|
||||
|
|
Loading…
Reference in New Issue