rusefi
/
atbetaflight
mirror of https://github.com/rusefi/atbetaflight.git
1
0
Fork 0
Code Issues Packages Projects Releases Wiki Activity

Add rate profiles and inflight switching between them. See Profiles and 

Inflight Adjustment documentation for details.
This commit is contained in:
Dominic Clifton 2014-10-29 23:31:43 +00:00
parent cac814923c
commit 28f9fa629c
12 changed files with 346 additions and 66 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

19
docs/Inflight Adjustments.md
View File

@ -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.

65
docs/Profiles.md Normal file
View File

@ -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
```

80
src/main/config/config.c
View File

@ -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(&currentProfile->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(&currentProfile->controlRateConfig);
generateThrottleCurve(&currentProfile->controlRateConfig, &masterConfig.escAndServoConfig);
activateControlRateConfig();
useRcControlsConfig(currentProfile->modeActivationConditions, &masterConfig.escAndServoConfig, &currentProfile->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;

7
src/main/config/config.h
View File

@ -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);

7
src/main/config/config_master.h
View File

@ -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;

5
src/main/config/config_profile.h
View File

@ -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.

21
src/main/io/rc_controls.c
View File

@ -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);
}
}

2
src/main/io/rc_controls.h
View File

@ -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];

75
src/main/io/serial_cli.c
View File

@ -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:

28
src/main/io/serial_msp.c
View File

@ -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

18
src/main/mw.c
View File

@ -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(&currentProfile->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(
&currentProfile->pidProfile,
&currentProfile->controlRateConfig,
currentControlRateProfile,
masterConfig.max_angle_inclination,
&currentProfile->accelerometerTrims
);

85
src/test/unit/rc_controls_unittest.cc
View File

@ -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;