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

Merge branch 'inflight-adjustments'

Browse source
This commit is contained in:
Dominic Clifton 2014-10-24 23:12:45 +01:00
commit 8aeee0b5fd
13 changed files with 686 additions and 80 deletions
Download patch file Download diff file Expand all files Collapse all files

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

@ -70,6 +70,7 @@ void setPIDController(int type); // FIXME PID code needs to be in flight_pid.c/h
void mixerUseConfigs(servoParam_t *servoConfToUse, flight3DConfig_t *flight3DConfigToUse,
escAndServoConfig_t *escAndServoConfigToUse, mixerConfig_t *mixerConfigToUse,
airplaneConfig_t *airplaneConfigToUse, rxConfig_t *rxConfig, gimbalConfig_t *gimbalConfigToUse);
void useRcControlsConfig(modeActivationCondition_t *modeActivationConditions, escAndServoConfig_t *escAndServoConfigToUse, pidProfile_t *pidProfileToUse);
#define FLASH_TO_RESERVE_FOR_CONFIG 0x800
@ -100,7 +101,7 @@ void mixerUseConfigs(servoParam_t *servoConfToUse, flight3DConfig_t *flight3DCon
master_t masterConfig; // master config struct with data independent from profiles
profile_t *currentProfile; // profile config struct
static const uint8_t EEPROM_CONF_VERSION = 82;
static const uint8_t EEPROM_CONF_VERSION = 83;
static void resetAccelerometerTrims(flightDynamicsTrims_t *accelerometerTrims)
{
@ -430,9 +431,9 @@ void activateConfig(void)
{
static imuRuntimeConfig_t imuRuntimeConfig;
generatePitchCurve(&currentProfile->controlRateConfig);
generatePitchRollCurve(&currentProfile->controlRateConfig);
generateThrottleCurve(&currentProfile->controlRateConfig, &masterConfig.escAndServoConfig);
useRcControlsConfig(currentProfile->modeActivationConditions);
useRcControlsConfig(currentProfile->modeActivationConditions, &masterConfig.escAndServoConfig, &currentProfile->pidProfile);
useGyroConfig(&masterConfig.gyroConfig);
#ifdef TELEMETRY

2
src/main/config/config_profile.h
View file

@ -44,6 +44,8 @@ typedef struct profile_s {
modeActivationCondition_t modeActivationConditions[MAX_MODE_ACTIVATION_CONDITION_COUNT];
adjustmentRange_t adjustmentRanges[MAX_ADJUSTMENT_RANGE_COUNT];
// Radio/ESC-related configuration
uint8_t deadband; // introduce a deadband around the stick center for pitch and roll axis. Must be greater than zero.
uint8_t yaw_deadband; // introduce a deadband around the stick center for yaw axis. Must be greater than zero.

6
src/main/drivers/sound_beeper_stm32f10x.c
View file

@ -21,6 +21,8 @@
#include "platform.h"
#include "build_config.h"
#include "system.h"
#include "gpio.h"
@ -28,7 +30,9 @@
void initBeeperHardware(beeperConfig_t *config)
{
#ifdef BEEPER
#ifndef BEEPER
UNUSED(config);
#else
gpio_config_t gpioConfig = {
config->gpioPin,
config->gpioMode,

5
src/main/io/beeper.c
View file

@ -34,11 +34,10 @@
#include "io/beeper.h"
#define DOUBLE_PAUSE_DURATION_MILLIS (LONG_PAUSE_DURATION_MILLIS * 2)
#define LONG_PAUSE_DURATION_MILLIS 200
#define DOUBLE_PAUSE_DURATION_MILLIS (LONG_PAUSE_DURATION_MILLIS * 2)
#define SHORT_PAUSE_DURATION_MILLIS (LONG_PAUSE_DURATION_MILLIS / 4)
#define MEDIUM_PAUSE_DURATION_MILLIS (LONG_PAUSE_DURATION_MILLIS / 2)
#define SHORT_CONFIRMATION_BEEP_DURATION_MILLIS (SHORT_PAUSE_DURATION_MILLIS / 2)
static uint8_t beeperIsOn = 0, beepDone = 0;
@ -123,7 +122,7 @@ void beepcodeUpdateState(bool warn_vbat)
else if (FLIGHT_MODE(AUTOTUNE_MODE))
beep_code('S','M','S','M');
else if (toggleBeep > 0)
beep(50); // fast confirmation beep
beep(SHORT_CONFIRMATION_BEEP_DURATION_MILLIS); // fast confirmation beep
else {
beeperIsOn = 0;
BEEP_OFF;

251
src/main/io/rc_controls.c
View file

@ -27,6 +27,8 @@
#include "config/config.h"
#include "config/runtime_config.h"
#include "drivers/system.h"
#include "flight/flight.h"
#include "drivers/accgyro.h"
@ -36,10 +38,16 @@
#include "sensors/acceleration.h"
#include "io/gps.h"
#include "io/beeper.h"
#include "mw.h"
#include "rx/rx.h"
#include "io/escservo.h"
#include "io/rc_controls.h"
#include "io/rc_curves.h"
static escAndServoConfig_t *escAndServoConfig;
static pidProfile_t *pidProfile;
static bool isUsingSticksToArm = true;
@ -170,6 +178,10 @@ void processRcStickPositions(rxConfig_t *rxConfig, throttleStatus_e throttleStat
return;
}
if (rcSticks == THR_LO + YAW_LO + PIT_LO + ROL_HI) {
saveConfigAndNotify();
}
if (isUsingSticksToArm) {
if (rcSticks == THR_LO + YAW_HI + PIT_CE + ROL_CE) {
@ -225,34 +237,253 @@ void processRcStickPositions(rxConfig_t *rxConfig, throttleStatus_e throttleStat
}
}
bool isRangeActive(uint8_t auxChannelIndex, channelRange_t *range) {
if (!IS_RANGE_USABLE(range)) {
return false;
}
uint16_t channelValue = constrain(rcData[auxChannelIndex + NON_AUX_CHANNEL_COUNT], CHANNEL_RANGE_MIN, CHANNEL_RANGE_MAX - 1);
return (channelValue >= 900 + (range->startStep * 25) &&
channelValue < 900 + (range->endStep * 25));
}
void updateActivatedModes(modeActivationCondition_t *modeActivationConditions)
{
rcModeActivationMask = 0; // FIXME implement, use rcData & modeActivationConditions
rcModeActivationMask = 0;
uint8_t index;
for (index = 0; index < MAX_MODE_ACTIVATION_CONDITION_COUNT; index++) {
modeActivationCondition_t *modeActivationCondition = &modeActivationConditions[index];
if (!IS_MODE_RANGE_USABLE(modeActivationCondition)) {
continue;
}
uint16_t channelValue = constrain(rcData[modeActivationCondition->auxChannelIndex + NON_AUX_CHANNEL_COUNT], CHANNEL_RANGE_MIN, CHANNEL_RANGE_MAX - 1);
if (channelValue >= 900 + (modeActivationCondition->rangeStartStep * 25) &&
channelValue < 900 + (modeActivationCondition->rangeEndStep * 25)) {
if (isRangeActive(modeActivationCondition->auxChannelIndex, &modeActivationCondition->range)) {
ACTIVATE_RC_MODE(modeActivationCondition->modeId);
}
}
}
void useRcControlsConfig(modeActivationCondition_t *modeActivationConditions)
uint8_t adjustmentStateMask = 0;
#define MARK_ADJUSTMENT_FUNCTION_AS_BUSY(adjustmentIndex) adjustmentStateMask |= (1 << adjustmentIndex)
#define MARK_ADJUSTMENT_FUNCTION_AS_READY(adjustmentIndex) adjustmentStateMask &= ~(1 << adjustmentIndex)
#define IS_ADJUSTMENT_FUNCTION_BUSY(adjustmentIndex) (adjustmentStateMask & (1 << adjustmentIndex))
// sync with adjustmentFunction_e
static const adjustmentConfig_t defaultAdjustmentConfigs[ADJUSTMENT_FUNCTION_COUNT - 1] = {
{
.adjustmentFunction = ADJUSTMENT_RC_RATE,
.step = 1
},
{
.adjustmentFunction = ADJUSTMENT_RC_EXPO,
.step = 1
},
{
.adjustmentFunction = ADJUSTMENT_THROTTLE_EXPO,
.step = 1
},
{
.adjustmentFunction = ADJUSTMENT_PITCH_ROLL_RATE,
.step = 1
},
{
.adjustmentFunction = ADJUSTMENT_YAW_RATE,
.step = 1
},
{
.adjustmentFunction = ADJUSTMENT_PITCH_ROLL_P,
.step = 1
},
{
.adjustmentFunction = ADJUSTMENT_PITCH_ROLL_I,
.step = 1
},
{
.adjustmentFunction = ADJUSTMENT_PITCH_ROLL_D,
.step = 1
},
{
.adjustmentFunction = ADJUSTMENT_YAW_P,
.step = 1
},
{
.adjustmentFunction = ADJUSTMENT_YAW_I,
.step = 1
},
{
.adjustmentFunction = ADJUSTMENT_YAW_D,
.step = 1
}
};
#define ADJUSTMENT_FUNCTION_CONFIG_INDEX_OFFSET 1
typedef struct adjustmentState_s {
uint8_t auxChannelIndex;
uint8_t adjustmentFunction;
uint8_t step;
uint32_t timeoutAt;
} adjustmentState_t;
static adjustmentState_t adjustmentStates[MAX_SIMULTANEOUS_ADJUSTMENT_COUNT];
void configureAdjustment(uint8_t index, uint8_t auxSwitchChannelIndex, const adjustmentConfig_t *adjustmentConfig) {
adjustmentState_t *adjustmentState = &adjustmentStates[index];
if (adjustmentState->adjustmentFunction == adjustmentConfig->adjustmentFunction) {
// already configured
return;
}
adjustmentState->auxChannelIndex = auxSwitchChannelIndex;
adjustmentState->adjustmentFunction = adjustmentConfig->adjustmentFunction;
adjustmentState->step = adjustmentConfig->step;
adjustmentState->timeoutAt = 0;
MARK_ADJUSTMENT_FUNCTION_AS_READY(index);
}
void applyAdjustment(controlRateConfig_t *controlRateConfig, uint8_t adjustmentFunction, int delta) {
int newValue;
if (delta > 0) {
queueConfirmationBeep(2);
} else {
queueConfirmationBeep(1);
}
switch(adjustmentFunction) {
case ADJUSTMENT_RC_RATE:
newValue = (int)controlRateConfig->rcRate8 + delta;
controlRateConfig->rcRate8 = constrain(newValue, 0, 250); // FIXME magic numbers repeated in serial_cli.c
generatePitchRollCurve(controlRateConfig);
break;
case ADJUSTMENT_RC_EXPO:
newValue = (int)controlRateConfig->rcExpo8 + delta;
controlRateConfig->rcExpo8 = constrain(newValue, 0, 100); // FIXME magic numbers repeated in serial_cli.c
generatePitchRollCurve(controlRateConfig);
break;
case ADJUSTMENT_THROTTLE_EXPO:
newValue = (int)controlRateConfig->thrExpo8 + delta;
controlRateConfig->thrExpo8 = constrain(newValue, 0, 100); // FIXME magic numbers repeated in serial_cli.c
generateThrottleCurve(controlRateConfig, escAndServoConfig);
break;
case ADJUSTMENT_PITCH_ROLL_RATE:
newValue = (int)controlRateConfig->rollPitchRate + delta;
controlRateConfig->rollPitchRate = constrain(newValue, 0, 100); // FIXME magic numbers repeated in serial_cli.c
break;
case ADJUSTMENT_YAW_RATE:
newValue = (int)controlRateConfig->yawRate + delta;
controlRateConfig->yawRate = constrain(newValue, 0, 100); // FIXME magic numbers repeated in serial_cli.c
break;
case ADJUSTMENT_PITCH_ROLL_P:
newValue = (int)pidProfile->P8[PIDPITCH] + delta;
pidProfile->P8[PIDPITCH] = constrain(newValue, 0, 200); // FIXME magic numbers repeated in serial_cli.c
newValue = (int)pidProfile->P8[PIDROLL] + delta;
pidProfile->P8[PIDROLL] = constrain(newValue, 0, 200); // FIXME magic numbers repeated in serial_cli.c
break;
case ADJUSTMENT_PITCH_ROLL_I:
newValue = (int)pidProfile->I8[PIDPITCH] + delta;
pidProfile->I8[PIDPITCH] = constrain(newValue, 0, 200); // FIXME magic numbers repeated in serial_cli.c
newValue = (int)pidProfile->I8[PIDROLL] + delta;
pidProfile->I8[PIDROLL] = constrain(newValue, 0, 200); // FIXME magic numbers repeated in serial_cli.c
break;
case ADJUSTMENT_PITCH_ROLL_D:
newValue = (int)pidProfile->D8[PIDPITCH] + delta;
pidProfile->D8[PIDPITCH] = constrain(newValue, 0, 200); // FIXME magic numbers repeated in serial_cli.c
newValue = (int)pidProfile->D8[PIDROLL] + delta;
pidProfile->D8[PIDROLL] = constrain(newValue, 0, 200); // FIXME magic numbers repeated in serial_cli.c
break;
case ADJUSTMENT_YAW_P:
newValue = (int)pidProfile->P8[PIDYAW] + delta;
pidProfile->P8[PIDYAW] = constrain(newValue, 0, 200); // FIXME magic numbers repeated in serial_cli.c
break;
case ADJUSTMENT_YAW_I:
newValue = (int)pidProfile->I8[PIDYAW] + delta;
pidProfile->I8[PIDYAW] = constrain(newValue, 0, 200); // FIXME magic numbers repeated in serial_cli.c
break;
case ADJUSTMENT_YAW_D:
newValue = (int)pidProfile->D8[PIDYAW] + delta;
pidProfile->D8[PIDYAW] = constrain(newValue, 0, 200); // FIXME magic numbers repeated in serial_cli.c
break;
default:
break;
};
}
#define RESET_FREQUENCY_2HZ (1000 / 2)
void processRcAdjustments(controlRateConfig_t *controlRateConfig, rxConfig_t *rxConfig)
{
uint8_t adjustmentIndex;
uint32_t now = millis();
for (adjustmentIndex = 0; adjustmentIndex < MAX_SIMULTANEOUS_ADJUSTMENT_COUNT; adjustmentIndex++) {
adjustmentState_t *adjustmentState = &adjustmentStates[adjustmentIndex];
uint8_t adjustmentFunction = adjustmentState->adjustmentFunction;
if (adjustmentFunction == ADJUSTMENT_NONE) {
continue;
}
int32_t signedDiff = now - adjustmentState->timeoutAt;
bool canResetReadyStates = signedDiff >= 0L;
if (canResetReadyStates) {
adjustmentState->timeoutAt = now + RESET_FREQUENCY_2HZ;
MARK_ADJUSTMENT_FUNCTION_AS_READY(adjustmentIndex);
}
uint8_t channelIndex = NON_AUX_CHANNEL_COUNT + adjustmentState->auxChannelIndex;
int delta;
if (rcData[channelIndex] > rxConfig->midrc + 200) {
delta = adjustmentState->step;
} else if (rcData[channelIndex] < rxConfig->midrc - 200) {
delta = 0 - adjustmentState->step;
} else {
// returning the switch to the middle immediately resets the ready state
MARK_ADJUSTMENT_FUNCTION_AS_READY(adjustmentIndex);
adjustmentState->timeoutAt = now + RESET_FREQUENCY_2HZ;
continue;
}
if (IS_ADJUSTMENT_FUNCTION_BUSY(adjustmentIndex)) {
continue;
}
MARK_ADJUSTMENT_FUNCTION_AS_BUSY(adjustmentIndex);
applyAdjustment(controlRateConfig, adjustmentFunction, delta);
}
}
void updateAdjustmentStates(adjustmentRange_t *adjustmentRanges)
{
uint8_t index;
for (index = 0; index < MAX_ADJUSTMENT_RANGE_COUNT; index++) {
adjustmentRange_t *adjustmentRange = &adjustmentRanges[index];
if (isRangeActive(adjustmentRange->auxChannelIndex, &adjustmentRange->range)) {
const adjustmentConfig_t *adjustmentConfig = &defaultAdjustmentConfigs[adjustmentRange->adjustmentFunction - ADJUSTMENT_FUNCTION_CONFIG_INDEX_OFFSET];
configureAdjustment(adjustmentRange->adjustmentIndex, adjustmentRange->auxSwitchChannelIndex, adjustmentConfig);
}
}
}
void useRcControlsConfig(modeActivationCondition_t *modeActivationConditions, escAndServoConfig_t *escAndServoConfigToUse, pidProfile_t *pidProfileToUse)
{
uint8_t index;
escAndServoConfig = escAndServoConfigToUse;
pidProfile = pidProfileToUse;
for (index = 0; index < MAX_MODE_ACTIVATION_CONDITION_COUNT; index++) {
modeActivationCondition_t *modeActivationCondition = &modeActivationConditions[index];
if (modeActivationCondition->modeId == BOXARM && IS_MODE_RANGE_USABLE(modeActivationCondition)) {
if (modeActivationCondition->modeId == BOXARM && IS_RANGE_USABLE(&modeActivationCondition->range)) {
isUsingSticksToArm = false;
break;
}

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

@ -98,19 +98,22 @@ typedef enum {
#define MIN_MODE_RANGE_STEP 0
#define MAX_MODE_RANGE_STEP ((CHANNEL_RANGE_MAX - CHANNEL_RANGE_MIN) / 25)
typedef struct modeActivationCondition_s {
boxId_e modeId;
uint8_t auxChannelIndex;
// steps are 25 apart
// a value of 0 corresponds to a channel value of 900 or less
// a value of 48 corresponds to a channel value of 2100 or more
// 48 steps between 900 and 1200
uint8_t rangeStartStep;
uint8_t rangeEndStep;
typedef struct channelRange_s {
uint8_t startStep;
uint8_t endStep;
} channelRange_t;
typedef struct modeActivationCondition_s {
boxId_e modeId;
uint8_t auxChannelIndex;
channelRange_t range;
} modeActivationCondition_t;
#define IS_MODE_RANGE_USABLE(modeActivationCondition) (modeActivationCondition->rangeStartStep < modeActivationCondition->rangeEndStep)
#define IS_RANGE_USABLE(range) ((range)->startStep < (range)->endStep)
typedef struct controlRateConfig_s {
uint8_t rcRate8;
@ -127,6 +130,50 @@ bool areSticksInApModePosition(uint16_t ap_mode);
throttleStatus_e calculateThrottleStatus(rxConfig_t *rxConfig, uint16_t deadband3d_throttle);
void processRcStickPositions(rxConfig_t *rxConfig, throttleStatus_e throttleStatus, bool retarded_arm, bool disarm_kill_switch);
void updateActivatedModes(modeActivationCondition_t *modeActivationConditions);
void useRcControlsConfig(modeActivationCondition_t *modeActivationConditions);
typedef enum {
ADJUSTMENT_NONE = 0,
ADJUSTMENT_RC_RATE,
ADJUSTMENT_RC_EXPO,
ADJUSTMENT_THROTTLE_EXPO,
ADJUSTMENT_PITCH_ROLL_RATE,
ADJUSTMENT_YAW_RATE,
ADJUSTMENT_PITCH_ROLL_P,
ADJUSTMENT_PITCH_ROLL_I,
ADJUSTMENT_PITCH_ROLL_D,
ADJUSTMENT_YAW_P,
ADJUSTMENT_YAW_I,
ADJUSTMENT_YAW_D,
} adjustmentFunction_e;
#define ADJUSTMENT_FUNCTION_COUNT 12
typedef struct adjustmentConfig_s {
uint8_t adjustmentFunction;
uint8_t step;
} adjustmentConfig_t;
typedef struct adjustmentRange_s {
// when aux channel is in range...
uint8_t auxChannelIndex;
channelRange_t range;
// ..then apply the adjustment function to the auxSwitchChannel ...
uint8_t adjustmentFunction;
uint8_t auxSwitchChannelIndex;
// ... via slot
uint8_t adjustmentIndex;
} adjustmentRange_t;
#define ADJUSTMENT_INDEX_OFFSET 1
#define MAX_SIMULTANEOUS_ADJUSTMENT_COUNT 4 // enough for 4 x 3position switches / 4 aux channel
#define MAX_ADJUSTMENT_RANGE_COUNT 12 // enough for 2 * 6pos switches.
void configureAdjustment(uint8_t index, uint8_t auxChannelIndex, const adjustmentConfig_t *adjustmentConfig);
void updateAdjustmentStates(adjustmentRange_t *adjustmentRanges);
void processRcAdjustments(controlRateConfig_t *controlRateConfig, rxConfig_t *rxConfig);

2
src/main/io/rc_curves.c
View file

@ -28,7 +28,7 @@ int16_t lookupPitchRollRC[PITCH_LOOKUP_LENGTH]; // lookup table for expo & R
int16_t lookupThrottleRC[THROTTLE_LOOKUP_LENGTH]; // lookup table for expo & mid THROTTLE
void generatePitchCurve(controlRateConfig_t *controlRateConfig)
void generatePitchRollCurve(controlRateConfig_t *controlRateConfig)
{
uint8_t i;

2
src/main/io/rc_curves.h
View file

@ -22,5 +22,5 @@
extern int16_t lookupPitchRollRC[PITCH_LOOKUP_LENGTH]; // lookup table for expo & RC rate PITCH+ROLL
extern int16_t lookupThrottleRC[THROTTLE_LOOKUP_LENGTH]; // lookup table for expo & mid THROTTLE
void generatePitchCurve(controlRateConfig_t *controlRateConfig);
void generatePitchRollCurve(controlRateConfig_t *controlRateConfig);
void generateThrottleCurve(controlRateConfig_t *controlRateConfig, escAndServoConfig_t *escAndServoConfig);

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

@ -71,6 +71,7 @@
// we unset this on 'exit'
extern uint8_t cliMode;
static void cliAux(char *cmdline);
static void cliAdjustmentRange(char *cmdline);
static void cliCMix(char *cmdline);
static void cliDefaults(char *cmdline);
static void cliDump(char *cmdLine);
@ -140,6 +141,7 @@ typedef struct {
// should be sorted a..z for bsearch()
const clicmd_t cmdTable[] = {
{ "adjrange", "show/set adjustment ranges settings", cliAdjustmentRange },
{ "aux", "show/set aux settings", cliAux },
{ "cmix", "design custom mixer", cliCMix },
#ifdef LED_STRIP
@ -403,6 +405,30 @@ static int cliCompare(const void *a, const void *b)
return strncasecmp(ca->name, cb->name, strlen(cb->name));
}
static char *processChannelRangeArgs(char *ptr, channelRange_t *range, uint8_t *validArgumentCount)
{
int val;
ptr = strchr(ptr, ' ');
if (ptr) {
val = atoi(++ptr);
val = CHANNEL_VALUE_TO_STEP(val);
if (val >= MIN_MODE_RANGE_STEP && val <= MAX_MODE_RANGE_STEP) {
range->startStep = val;
(*validArgumentCount)++;
}
}
ptr = strchr(ptr, ' ');
if (ptr) {
val = atoi(++ptr);
val = CHANNEL_VALUE_TO_STEP(val);
if (val >= MIN_MODE_RANGE_STEP && val <= MAX_MODE_RANGE_STEP) {
range->endStep = val;
(*validArgumentCount)++;
}
}
return ptr;
}
static void cliAux(char *cmdline)
{
int i, val = 0;
@ -418,8 +444,8 @@ static void cliAux(char *cmdline)
i,
mac->modeId,
mac->auxChannelIndex,
MODE_STEP_TO_CHANNEL_VALUE(mac->rangeStartStep),
MODE_STEP_TO_CHANNEL_VALUE(mac->rangeEndStep)
MODE_STEP_TO_CHANNEL_VALUE(mac->range.startStep),
MODE_STEP_TO_CHANNEL_VALUE(mac->range.endStep)
);
}
} else {
@ -444,24 +470,8 @@ static void cliAux(char *cmdline)
validArgumentCount++;
}
}
ptr = strchr(ptr, ' ');
if (ptr) {
val = atoi(++ptr);
val = CHANNEL_VALUE_TO_STEP(val);
if (val >= MIN_MODE_RANGE_STEP && val <= MAX_MODE_RANGE_STEP) {
mac->rangeStartStep = val;
validArgumentCount++;
}
}
ptr = strchr(ptr, ' ');
if (ptr) {
val = atoi(++ptr);
val = CHANNEL_VALUE_TO_STEP(val);
if (val >= MIN_MODE_RANGE_STEP && val <= MAX_MODE_RANGE_STEP) {
mac->rangeEndStep = val;
validArgumentCount++;
}
}
ptr = processChannelRangeArgs(ptr, &mac->range, &validArgumentCount);
if (validArgumentCount != 4) {
memset(mac, 0, sizeof(modeActivationCondition_t));
}
@ -471,6 +481,77 @@ static void cliAux(char *cmdline)
}
}
static void cliAdjustmentRange(char *cmdline)
{
int i, val = 0;
uint8_t len;
char *ptr;
len = strlen(cmdline);
if (len == 0) {
// print out adjustment ranges channel settings
for (i = 0; i < MAX_ADJUSTMENT_RANGE_COUNT; i++) {
adjustmentRange_t *ar = &currentProfile->adjustmentRanges[i];
printf("adjrange %u %u %u %u %u %u %u\r\n",
i,
ar->adjustmentIndex,
ar->auxChannelIndex,
MODE_STEP_TO_CHANNEL_VALUE(ar->range.startStep),
MODE_STEP_TO_CHANNEL_VALUE(ar->range.endStep),
ar->adjustmentFunction,
ar->auxSwitchChannelIndex
);
}
} else {
ptr = cmdline;
i = atoi(ptr++);
if (i < MAX_ADJUSTMENT_RANGE_COUNT) {
adjustmentRange_t *ar = &currentProfile->adjustmentRanges[i];
uint8_t validArgumentCount = 0;
ptr = strchr(ptr, ' ');
if (ptr) {
val = atoi(++ptr);
if (val >= 0 && val < MAX_SIMULTANEOUS_ADJUSTMENT_COUNT) {
ar->adjustmentIndex = val;
validArgumentCount++;
}
}
ptr = strchr(ptr, ' ');
if (ptr) {
val = atoi(++ptr);
if (val >= 0 && val < MAX_AUX_CHANNEL_COUNT) {
ar->auxChannelIndex = val;
validArgumentCount++;
}
}
ptr = processChannelRangeArgs(ptr, &ar->range, &validArgumentCount);
ptr = strchr(ptr, ' ');
if (ptr) {
val = atoi(++ptr);
if (val >= 0 && val < ADJUSTMENT_FUNCTION_COUNT) {
ar->adjustmentFunction = val;
validArgumentCount++;
}
}
ptr = strchr(ptr, ' ');
if (ptr) {
val = atoi(++ptr);
if (val >= 0 && val < MAX_AUX_CHANNEL_COUNT) {
ar->auxSwitchChannelIndex = val;
validArgumentCount++;
}
}
if (validArgumentCount != 6) {
memset(ar, 0, sizeof(adjustmentRange_t));
}
} else {
printf("index: must be < %u\r\n", MAX_ADJUSTMENT_RANGE_COUNT);
}
}
}
static void cliCMix(char *cmdline)
{
int i, check = 0;
@ -737,6 +818,10 @@ static void cliDump(char *cmdline)
cliAux("");
printf("\r\n# adjrange\r\n");
cliAdjustmentRange("");
printSectionBreak();
dumpValues(PROFILE_VALUE);

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

@ -845,8 +845,8 @@ static bool processOutCommand(uint8_t cmdMSP)
const box_t *box = &boxes[mac->modeId];
serialize8(box->permanentId);
serialize8(mac->auxChannelIndex);
serialize8(mac->rangeStartStep);
serialize8(mac->rangeEndStep);
serialize8(mac->range.startStep);
serialize8(mac->range.endStep);
}
break;
case MSP_BOXNAMES:
@ -1088,8 +1088,8 @@ static bool processInCommand(void)
if (box) {
mac->modeId = box->boxId;
mac->auxChannelIndex = read8();
mac->rangeStartStep = read8();
mac->rangeEndStep = read8();
mac->range.startStep = read8();
mac->range.endStep = read8();
} else {
headSerialError(0);
}

7
src/main/mw.c
View file

@ -332,9 +332,9 @@ void handleInflightCalibrationStickPosition(void)
} else {
AccInflightCalibrationArmed = !AccInflightCalibrationArmed;
if (AccInflightCalibrationArmed) {
queueConfirmationBeep(2);
queueConfirmationBeep(4);
} else {
queueConfirmationBeep(3);
queueConfirmationBeep(6);
}
}
}
@ -495,6 +495,9 @@ void processRx(void)
updateActivatedModes(currentProfile->modeActivationConditions);
updateAdjustmentStates(currentProfile->adjustmentRanges);
processRcAdjustments(&currentProfile->controlRateConfig, &masterConfig.rxConfig);
bool canUseHorizonMode = true;
if ((IS_RC_MODE_ACTIVE(BOXANGLE) || (feature(FEATURE_FAILSAFE) && failsafe->vTable->hasTimerElapsed())) && (sensors(SENSOR_ACC))) {

9
src/main/sensors/acceleration.c
View file

@ -130,8 +130,8 @@ void performInflightAccelerationCalibration(rollAndPitchTrims_t *rollAndPitchTri
if (InflightcalibratingA == 1) {
AccInflightCalibrationActive = false;
AccInflightCalibrationMeasurementDone = true;
queueConfirmationBeep(2); // beeper to indicatiing the end of calibration
// recover saved values to maintain current flight behavior until new values are transferred
queueConfirmationBeep(5); // beeper to indicating the end of calibration
// recover saved values to maintain current flight behaviour until new values are transferred
accelerationTrims->raw[FD_ROLL] = accZero_saved[FD_ROLL];
accelerationTrims->raw[FD_PITCH] = accZero_saved[FD_PITCH];
accelerationTrims->raw[FD_YAW] = accZero_saved[FD_YAW];
@ -141,17 +141,16 @@ void performInflightAccelerationCalibration(rollAndPitchTrims_t *rollAndPitchTri
InflightcalibratingA--;
}
// Calculate average, shift Z down by acc_1G and store values in EEPROM at end of calibration
if (AccInflightCalibrationSavetoEEProm) { // the copter is landed, disarmed and the combo has been done again
if (AccInflightCalibrationSavetoEEProm) { // the aircraft is landed, disarmed and the combo has been done again
AccInflightCalibrationSavetoEEProm = false;
accelerationTrims->raw[FD_ROLL] = b[FD_ROLL] / 50;
accelerationTrims->raw[FD_PITCH] = b[FD_PITCH] / 50;
accelerationTrims->raw[FD_YAW] = b[FD_YAW] / 50 - acc_1G; // for nunchuk 200=1G
accelerationTrims->raw[FD_YAW] = b[FD_YAW] / 50 - acc_1G; // for nunchuck 200=1G
resetRollAndPitchTrims(rollAndPitchTrims);
saveConfigAndNotify();
}
}
void applyAccelerationTrims(flightDynamicsTrims_t *accelerationTrims)

275
src/test/unit/rc_controls_unittest.cc
View file

@ -74,47 +74,47 @@ TEST(RcControlsTest, updateActivatedModesUsingValidAuxConfigurationAndRXValues)
modeActivationConditions[0].modeId = (boxId_e)0;
modeActivationConditions[0].auxChannelIndex = AUX1 - NON_AUX_CHANNEL_COUNT;
modeActivationConditions[0].rangeStartStep = CHANNEL_VALUE_TO_STEP(1700);
modeActivationConditions[0].rangeEndStep = CHANNEL_VALUE_TO_STEP(2100);
modeActivationConditions[0].range.startStep = CHANNEL_VALUE_TO_STEP(1700);
modeActivationConditions[0].range.endStep = CHANNEL_VALUE_TO_STEP(2100);
modeActivationConditions[1].modeId = (boxId_e)1;
modeActivationConditions[1].auxChannelIndex = AUX2 - NON_AUX_CHANNEL_COUNT;
modeActivationConditions[1].rangeStartStep = CHANNEL_VALUE_TO_STEP(1300);
modeActivationConditions[1].rangeEndStep = CHANNEL_VALUE_TO_STEP(1700);
modeActivationConditions[1].range.startStep = CHANNEL_VALUE_TO_STEP(1300);
modeActivationConditions[1].range.endStep = CHANNEL_VALUE_TO_STEP(1700);
modeActivationConditions[2].modeId = (boxId_e)2;
modeActivationConditions[2].auxChannelIndex = AUX3 - NON_AUX_CHANNEL_COUNT;
modeActivationConditions[2].rangeStartStep = CHANNEL_VALUE_TO_STEP(900);
modeActivationConditions[2].rangeEndStep = CHANNEL_VALUE_TO_STEP(1200);
modeActivationConditions[2].range.startStep = CHANNEL_VALUE_TO_STEP(900);
modeActivationConditions[2].range.endStep = CHANNEL_VALUE_TO_STEP(1200);
modeActivationConditions[3].modeId = (boxId_e)3;
modeActivationConditions[3].auxChannelIndex = AUX4 - NON_AUX_CHANNEL_COUNT;
modeActivationConditions[3].rangeStartStep = CHANNEL_VALUE_TO_STEP(900);
modeActivationConditions[3].rangeEndStep = CHANNEL_VALUE_TO_STEP(2100);
modeActivationConditions[3].range.startStep = CHANNEL_VALUE_TO_STEP(900);
modeActivationConditions[3].range.endStep = CHANNEL_VALUE_TO_STEP(2100);
modeActivationConditions[4].modeId = (boxId_e)4;
modeActivationConditions[4].auxChannelIndex = AUX5 - NON_AUX_CHANNEL_COUNT;
modeActivationConditions[4].rangeStartStep = CHANNEL_VALUE_TO_STEP(900);
modeActivationConditions[4].rangeEndStep = CHANNEL_VALUE_TO_STEP(925);
modeActivationConditions[4].range.startStep = CHANNEL_VALUE_TO_STEP(900);
modeActivationConditions[4].range.endStep = CHANNEL_VALUE_TO_STEP(925);
EXPECT_EQ(0, modeActivationConditions[4].rangeStartStep);
EXPECT_EQ(1, modeActivationConditions[4].rangeEndStep);
EXPECT_EQ(0, modeActivationConditions[4].range.startStep);
EXPECT_EQ(1, modeActivationConditions[4].range.endStep);
modeActivationConditions[5].modeId = (boxId_e)5;
modeActivationConditions[5].auxChannelIndex = AUX6 - NON_AUX_CHANNEL_COUNT;
modeActivationConditions[5].rangeStartStep = CHANNEL_VALUE_TO_STEP(2075);
modeActivationConditions[5].rangeEndStep = CHANNEL_VALUE_TO_STEP(2100);
modeActivationConditions[5].range.startStep = CHANNEL_VALUE_TO_STEP(2075);
modeActivationConditions[5].range.endStep = CHANNEL_VALUE_TO_STEP(2100);
EXPECT_EQ(47, modeActivationConditions[5].rangeStartStep);
EXPECT_EQ(48, modeActivationConditions[5].rangeEndStep);
EXPECT_EQ(47, modeActivationConditions[5].range.startStep);
EXPECT_EQ(48, modeActivationConditions[5].range.endStep);
modeActivationConditions[6].modeId = (boxId_e)6;
modeActivationConditions[6].auxChannelIndex = AUX7 - NON_AUX_CHANNEL_COUNT;
modeActivationConditions[6].rangeStartStep = CHANNEL_VALUE_TO_STEP(925);
modeActivationConditions[6].rangeEndStep = CHANNEL_VALUE_TO_STEP(950);
modeActivationConditions[6].range.startStep = CHANNEL_VALUE_TO_STEP(925);
modeActivationConditions[6].range.endStep = CHANNEL_VALUE_TO_STEP(950);
EXPECT_EQ(1, modeActivationConditions[6].rangeStartStep);
EXPECT_EQ(2, modeActivationConditions[6].rangeEndStep);
EXPECT_EQ(1, modeActivationConditions[6].range.startStep);
EXPECT_EQ(2, modeActivationConditions[6].range.endStep);
// and
rcModeActivationMask = 0;
@ -157,6 +157,241 @@ TEST(RcControlsTest, updateActivatedModesUsingValidAuxConfigurationAndRXValues)
}
}
enum {
COUNTER_GENERATE_PITCH_ROLL_CURVE = 0,
COUNTER_QUEUE_CONFIRMATION_BEEP
};
#define CALL_COUNT_ITEM_COUNT 1
static int callCounts[CALL_COUNT_ITEM_COUNT];
#define CALL_COUNTER(item) (callCounts[item])
void generatePitchRollCurve(controlRateConfig_t *) {
callCounts[COUNTER_GENERATE_PITCH_ROLL_CURVE]++;
}
void queueConfirmationBeep(uint8_t) {
callCounts[COUNTER_QUEUE_CONFIRMATION_BEEP]++;
}
void resetCallCounters(void) {
memset(&callCounts, 0, sizeof(callCounts));
}
uint32_t fixedMillis = 0;
uint32_t millis(void) {
return fixedMillis;
}
#define DEFAULT_MIN_CHECK 1100
#define DEFAULT_MAX_CHECK 1900
rxConfig_t rxConfig;
extern uint8_t adjustmentStateMask;
static const adjustmentConfig_t rateAdjustmentConfig = {
.adjustmentFunction = ADJUSTMENT_RC_RATE,
.step = 1
};
TEST(RcControlsTest, processRcAdjustmentsSticksInMiddle)
{
// given
controlRateConfig_t controlRateConfig = {
.rcRate8 = 90,
.rcExpo8 = 0,
.thrMid8 = 0,
.thrExpo8 = 0,
.rollPitchRate = 0,
.yawRate = 0,
};
// and
memset(&rxConfig, 0, sizeof (rxConfig));
rxConfig.mincheck = DEFAULT_MIN_CHECK;
rxConfig.maxcheck = DEFAULT_MAX_CHECK;
rxConfig.midrc = 1500;
configureAdjustment(0, AUX3 - NON_AUX_CHANNEL_COUNT, &rateAdjustmentConfig);
// and
uint8_t index;
for (index = AUX1; index < MAX_SUPPORTED_RC_CHANNEL_COUNT; index++) {
rcData[index] = PWM_RANGE_MIDDLE;
}
// and
resetCallCounters();
// when
processRcAdjustments(&controlRateConfig, &rxConfig);
// then
EXPECT_EQ(controlRateConfig.rcRate8, 90);
EXPECT_EQ(CALL_COUNTER(COUNTER_GENERATE_PITCH_ROLL_CURVE), 0);
EXPECT_EQ(CALL_COUNTER(COUNTER_QUEUE_CONFIRMATION_BEEP), 0);
EXPECT_EQ(adjustmentStateMask, 0);
}
TEST(RcControlsTest, processRcAdjustmentsWithRcRateFunctionSwitchUp)
{
// given
controlRateConfig_t controlRateConfig = {
.rcRate8 = 90,
.rcExpo8 = 0,
.thrMid8 = 0,
.thrExpo8 = 0,
.rollPitchRate = 0,
.yawRate = 0,
};
// and
memset(&rxConfig, 0, sizeof (rxConfig));
rxConfig.mincheck = DEFAULT_MIN_CHECK;
rxConfig.maxcheck = DEFAULT_MAX_CHECK;
rxConfig.midrc = 1500;
// and
configureAdjustment(0, AUX3 - NON_AUX_CHANNEL_COUNT, &rateAdjustmentConfig);
// and
uint8_t index;
for (index = AUX1; index < MAX_SUPPORTED_RC_CHANNEL_COUNT; index++) {
rcData[index] = PWM_RANGE_MIDDLE;
}
// and
resetCallCounters();
// and
rcData[AUX3] = PWM_RANGE_MAX;
// and
uint8_t expectedAdjustmentStateMask =
(1 << 0);
// and
fixedMillis = 496;
// when
processRcAdjustments(&controlRateConfig, &rxConfig);
// then
EXPECT_EQ(controlRateConfig.rcRate8, 91);
EXPECT_EQ(CALL_COUNTER(COUNTER_GENERATE_PITCH_ROLL_CURVE), 1);
EXPECT_EQ(CALL_COUNTER(COUNTER_QUEUE_CONFIRMATION_BEEP), 1);
EXPECT_EQ(adjustmentStateMask, expectedAdjustmentStateMask);
//
// now pretend a short amount of time has passed, but not enough time to allow the value to have been increased
//
// given
fixedMillis = 497;
// when
processRcAdjustments(&controlRateConfig, &rxConfig);
EXPECT_EQ(controlRateConfig.rcRate8, 91);
EXPECT_EQ(adjustmentStateMask, expectedAdjustmentStateMask);
//
// moving the switch back to the middle should immediately reset the state flag without increasing the value
//
// given
rcData[AUX3] = PWM_RANGE_MIDDLE;
// and
fixedMillis = 498;
// and
expectedAdjustmentStateMask = adjustmentStateMask &
~(1 << 0);
// when
processRcAdjustments(&controlRateConfig, &rxConfig);
EXPECT_EQ(controlRateConfig.rcRate8, 91);
EXPECT_EQ(adjustmentStateMask, expectedAdjustmentStateMask);
//
// flipping the switch again, before the state reset would have occurred, allows the value to be increased again
// given
rcData[AUX3] = PWM_RANGE_MAX;
// and
expectedAdjustmentStateMask =
(1 << 0);
// and
fixedMillis = 499;
// when
processRcAdjustments(&controlRateConfig, &rxConfig);
// then
EXPECT_EQ(controlRateConfig.rcRate8, 92);
EXPECT_EQ(CALL_COUNTER(COUNTER_GENERATE_PITCH_ROLL_CURVE), 2);
EXPECT_EQ(CALL_COUNTER(COUNTER_QUEUE_CONFIRMATION_BEEP), 2);
EXPECT_EQ(adjustmentStateMask, expectedAdjustmentStateMask);
//
// leaving the switch up, after the original timer would have reset the state should now NOT cause
// the rate to increase, it should only increase after another 500ms from when the state was reset.
//
// given
fixedMillis = 500;
// when
processRcAdjustments(&controlRateConfig, &rxConfig);
// then
EXPECT_EQ(controlRateConfig.rcRate8, 92);
EXPECT_EQ(adjustmentStateMask, expectedAdjustmentStateMask);
//
// should still not be able to be increased
//
// given
fixedMillis = 997;
// when
processRcAdjustments(&controlRateConfig, &rxConfig);
// then
EXPECT_EQ(controlRateConfig.rcRate8, 92);
EXPECT_EQ(adjustmentStateMask, expectedAdjustmentStateMask);
//
// 500ms has now passed since the switch was returned to the middle, now that
// switch is still in the UP position after the timer has elapses it should
// be increased again.
//
// given
fixedMillis = 998;
// when
processRcAdjustments(&controlRateConfig, &rxConfig);
// then
EXPECT_EQ(controlRateConfig.rcRate8, 93);
EXPECT_EQ(CALL_COUNTER(COUNTER_GENERATE_PITCH_ROLL_CURVE), 3);
EXPECT_EQ(CALL_COUNTER(COUNTER_QUEUE_CONFIRMATION_BEEP), 3);
EXPECT_EQ(adjustmentStateMask, expectedAdjustmentStateMask);
}
void changeProfile(uint8_t) {}
void accSetCalibrationCycles(uint16_t) {}
void gyroSetCalibrationCycles(uint16_t) {}