static uint8_t applySelectAdjustment(uint8_t adjustmentFunction, uint8_t position)
{
uint8_t beeps = 0;
switch (adjustmentFunction) {
case ADJUSTMENT_RATE_PROFILE:
{
if (getCurrentControlRateProfileIndex() != position) {
changeControlRateProfile(position);
blackboxLogInflightAdjustmentEvent(ADJUSTMENT_RATE_PROFILE, position);
beeps = position + 1;
}
break;
}
case ADJUSTMENT_HORIZON_STRENGTH:
{
uint8_t newValue = constrain(position, 0, 200); // FIXME magic numbers repeated in serial_cli.c
if (pidProfile->pid[PID_LEVEL].D != newValue) {
beeps = ((newValue - pidProfile->pid[PID_LEVEL].D) / 8) + 1;
pidProfile->pid[PID_LEVEL].D = newValue;
blackboxLogInflightAdjustmentEvent(ADJUSTMENT_HORIZON_STRENGTH, position);
}
break;
}
}
if (beeps) {
beeperConfirmationBeeps(beeps);
}
return position;
}
void changeProfile(uint8_t profileIndex)
{
masterConfig.current_profile_index = profileIndex;
writeEEPROM();
readEEPROM();
beeperConfirmationBeeps(profileIndex + 1);
}
void changePidProfile(uint8_t pidProfileIndex)
{
if (pidProfileIndex >= MAX_PROFILE_COUNT) {
pidProfileIndex = MAX_PROFILE_COUNT - 1;
}
systemConfigMutable()->pidProfileIndex = pidProfileIndex;
currentPidProfile = pidProfilesMutable(pidProfileIndex);
beeperConfirmationBeeps(pidProfileIndex + 1);
}
/**
* Disables the given flight mode. A beep is sounded if the flight mode
* has changed. Returns the new 'flightModeFlags' value.
*/
uint16_t disableFlightMode(flightModeFlags_e mask)
{
uint16_t oldVal = flightModeFlags;
flightModeFlags &= ~(mask);
if (flightModeFlags != oldVal)
beeperConfirmationBeeps(1);
return flightModeFlags;
}
void mwArm(void)
{
static bool firstArmingCalibrationWasCompleted;
if (masterConfig.gyro_cal_on_first_arm && !firstArmingCalibrationWasCompleted) {
gyroSetCalibrationCycles();
armingCalibrationWasInitialised = true;
firstArmingCalibrationWasCompleted = true;
}
if (!isGyroCalibrationComplete()) return; // prevent arming before gyro is calibrated
if (ARMING_FLAG(OK_TO_ARM)) {
if (ARMING_FLAG(ARMED)) {
return;
}
if (IS_RC_MODE_ACTIVE(BOXFAILSAFE)) {
return;
}
if (!ARMING_FLAG(PREVENT_ARMING)) {
ENABLE_ARMING_FLAG(ARMED);
ENABLE_ARMING_FLAG(WAS_EVER_ARMED);
headFreeModeHold = DECIDEGREES_TO_DEGREES(attitude.values.yaw);
#ifdef BLACKBOX
if (feature(FEATURE_BLACKBOX)) {
serialPort_t *sharedBlackboxAndMspPort = findSharedSerialPort(FUNCTION_BLACKBOX, FUNCTION_MSP);
if (sharedBlackboxAndMspPort) {
mspReleasePortIfAllocated(sharedBlackboxAndMspPort);
}
startBlackbox();
}
#endif
disarmAt = millis() + masterConfig.auto_disarm_delay * 1000; // start disarm timeout, will be extended when throttle is nonzero
//beep to indicate arming
#ifdef GPS
if (feature(FEATURE_GPS) && STATE(GPS_FIX) && GPS_numSat >= 5)
beeper(BEEPER_ARMING_GPS_FIX);
else
beeper(BEEPER_ARMING);
#else
beeper(BEEPER_ARMING);
#endif
return;
}
}
if (!ARMING_FLAG(ARMED)) {
beeperConfirmationBeeps(1);
}
}
void applySelectAdjustment(uint8_t adjustmentFunction, uint8_t position)
{
bool applied = false;
switch(adjustmentFunction) {
case ADJUSTMENT_RATE_PROFILE:
if (getCurrentControlRateProfile() != position) {
changeControlRateProfile(position);
applied = true;
}
break;
}
if (applied) {
beeperConfirmationBeeps(position + 1);
}
}
void mwArm(void)
{
if (ARMING_FLAG(OK_TO_ARM)) {
if (ARMING_FLAG(ARMED)) {
//led0_op(true);
return;
}
if (IS_RC_MODE_ACTIVE(BOXFAILSAFE)) {
return;
}
if (!ARMING_FLAG(PREVENT_ARMING)) {
ENABLE_ARMING_FLAG(ARMED);
headFreeModeHold = heading;
// led2_op(1);//drona led
// led1_op(1);//drona led
// led0_op(0);//drona led
#ifdef BLACKBOX
if (feature(FEATURE_BLACKBOX)) {
serialPort_t *sharedBlackboxAndMspPort = findSharedSerialPort(FUNCTION_BLACKBOX, FUNCTION_MSP);
if (sharedBlackboxAndMspPort) {
mspReleasePortIfAllocated(sharedBlackboxAndMspPort);
}
startBlackbox();
}
#endif
disarmAt = millis() + masterConfig.auto_disarm_delay * 1000; // start disarm timeout, will be extended when throttle is nonzero
//beep to indicate arming
#ifdef GPS
if (feature(FEATURE_GPS) && STATE(GPS_FIX) && GPS_numSat >= 5)
beeper(BEEPER_ARMING_GPS_FIX);
else
beeper(BEEPER_ARMING);
#else
beeper(BEEPER_ARMING);
#endif
return;
}
}
if (!ARMING_FLAG(ARMED)) {
beeperConfirmationBeeps(1);
}
}
void mwArm(void)
{
if (ARMING_FLAG(OK_TO_ARM)) {
if (ARMING_FLAG(ARMED)) {
return;
}
if (IS_RC_MODE_ACTIVE(BOXFAILSAFE)) {
return;
}
if (!ARMING_FLAG(PREVENT_ARMING)) {
ENABLE_ARMING_FLAG(ARMED);
ENABLE_ARMING_FLAG(WAS_EVER_ARMED);
headFreeModeHold = DECIDEGREES_TO_DEGREES(attitude.values.yaw);
resetMagHoldHeading(DECIDEGREES_TO_DEGREES(attitude.values.yaw));
#ifdef BLACKBOX
if (feature(FEATURE_BLACKBOX)) {
serialPort_t *sharedBlackboxAndMspPort = findSharedSerialPort(FUNCTION_BLACKBOX, FUNCTION_MSP);
if (sharedBlackboxAndMspPort) {
mspSerialReleasePortIfAllocated(sharedBlackboxAndMspPort);
}
startBlackbox();
}
#endif
disarmAt = millis() + masterConfig.auto_disarm_delay * 1000; // start disarm timeout, will be extended when throttle is nonzero
//beep to indicate arming
#ifdef NAV
if (navigationPositionEstimateIsHealthy())
beeper(BEEPER_ARMING_GPS_FIX);
else
beeper(BEEPER_ARMING);
#else
beeper(BEEPER_ARMING);
#endif
return;
}
}
if (!ARMING_FLAG(ARMED)) {
beeperConfirmationBeeps(1);
}
}
void mwArm(void)
{
static bool firstArmingCalibrationWasCompleted;
if (armingConfig()->gyro_cal_on_first_arm && !firstArmingCalibrationWasCompleted) {
gyroSetCalibrationCycles();
armingCalibrationWasInitialised = true;
firstArmingCalibrationWasCompleted = true;
}
if (!isGyroCalibrationComplete()) return; // prevent arming before gyro is calibrated
if (ARMING_FLAG(OK_TO_ARM)) {
if (ARMING_FLAG(ARMED)) {
return;
}
if (IS_RC_MODE_ACTIVE(BOXFAILSAFE)) {
return;
}
if (!ARMING_FLAG(PREVENT_ARMING)) {
ENABLE_ARMING_FLAG(ARMED);
ENABLE_ARMING_FLAG(WAS_EVER_ARMED);
headFreeModeHold = DECIDEGREES_TO_DEGREES(attitude.values.yaw);
disarmAt = millis() + armingConfig()->auto_disarm_delay * 1000; // start disarm timeout, will be extended when throttle is nonzero
//beep to indicate arming
#ifdef GPS
if (feature(FEATURE_GPS) && STATE(GPS_FIX) && GPS_numSat >= 5)
beeper(BEEPER_ARMING_GPS_FIX);
else
beeper(BEEPER_ARMING);
#else
beeper(BEEPER_ARMING);
#endif
return;
}
}
if (!ARMING_FLAG(ARMED)) {
beeperConfirmationBeeps(1);
}
}
void mwArm(void)
{
if (ARMING_FLAG(OK_TO_ARM)) {
if (ARMING_FLAG(ARMED)) {
return;
}
if (rcModeIsActive(BOXFAILSAFE)) {
return;
}
if (!ARMING_FLAG(PREVENT_ARMING)) {
ENABLE_ARMING_FLAG(ARMED);
headFreeModeHold = DECIDEGREES_TO_DEGREES(attitude.values.yaw);
#ifdef BLACKBOX
if (feature(FEATURE_BLACKBOX)) {
serialPort_t *sharedBlackboxAndMspPort = findSharedSerialPort(FUNCTION_BLACKBOX, FUNCTION_MSP_SERVER);
if (sharedBlackboxAndMspPort) {
mspSerialReleasePortIfAllocated(sharedBlackboxAndMspPort);
}
startBlackbox();
}
#endif
disarmAt = millis() + armingConfig()->auto_disarm_delay * 1000; // start disarm timeout, will be extended when throttle is nonzero
//beep to indicate arming
#ifdef GPS
if (feature(FEATURE_GPS) && STATE(GPS_FIX) && GPS_numSat >= 5)
beeper(BEEPER_ARMING_GPS_FIX);
else
beeper(BEEPER_ARMING);
#else
beeper(BEEPER_ARMING);
#endif
return;
}
}
if (!ARMING_FLAG(ARMED)) {
beeperConfirmationBeeps(1);
}
}
void performAcclerationCalibration(void)
{
int axisIndex = getPrimaryAxisIndex(accADC);
// Check if sample is usable
if (axisIndex < 0) {
return;
}
// Top-up and first calibration cycle, reset everything
if (axisIndex == 0 && isOnFirstAccelerationCalibrationCycle()) {
for (int axis = 0; axis < 6; axis++) {
calibratedAxis[axis] = false;
accSamples[axis][X] = 0;
accSamples[axis][Y] = 0;
accSamples[axis][Z] = 0;
}
calibratedAxisCount = 0;
sensorCalibrationResetState(&calState);
}
if (!calibratedAxis[axisIndex]) {
sensorCalibrationPushSampleForOffsetCalculation(&calState, accADC);
accSamples[axisIndex][X] += accADC[X];
accSamples[axisIndex][Y] += accADC[Y];
accSamples[axisIndex][Z] += accADC[Z];
if (isOnFinalAccelerationCalibrationCycle()) {
calibratedAxis[axisIndex] = true;
calibratedAxisCount++;
beeperConfirmationBeeps(2);
}
}
if (calibratedAxisCount == 6) {
float accTmp[3];
int32_t accSample[3];
/* Calculate offset */
sensorCalibrationSolveForOffset(&calState, accTmp);
for (int axis = 0; axis < 3; axis++) {
accZero->raw[axis] = lrintf(accTmp[axis]);
}
/* Not we can offset our accumulated averages samples and calculate scale factors and calculate gains */
sensorCalibrationResetState(&calState);
for (int axis = 0; axis < 6; axis++) {
accSample[X] = accSamples[axis][X] / CALIBRATING_ACC_CYCLES - accZero->raw[X];
accSample[Y] = accSamples[axis][Y] / CALIBRATING_ACC_CYCLES - accZero->raw[Y];
accSample[Z] = accSamples[axis][Z] / CALIBRATING_ACC_CYCLES - accZero->raw[Z];
sensorCalibrationPushSampleForScaleCalculation(&calState, axis / 2, accSample, acc.acc_1G);
}
sensorCalibrationSolveForScale(&calState, accTmp);
for (int axis = 0; axis < 3; axis++) {
accGain->raw[axis] = lrintf(accTmp[axis] * 4096);
}
saveConfigAndNotify();
}
calibratingA--;
}
void processRcStickPositions(rxConfig_t *rxConfig, throttleStatus_e throttleStatus, bool retarded_arm, bool disarm_kill_switch)
{
static uint8_t rcDelayCommand; // this indicates the number of time (multiple of RC measurement at 50Hz) the sticks must be maintained to run or switch off motors
static uint8_t rcSticks; // this hold sticks position for command combos
uint8_t stTmp = 0;
int i;
// ------------------ STICKS COMMAND HANDLER --------------------
// checking sticks positions
for (i = 0; i < 4; i++) {
stTmp >>= 2;
if (rcData[i] > rxConfig->mincheck)
stTmp |= 0x80; // check for MIN
if (rcData[i] < rxConfig->maxcheck)
stTmp |= 0x40; // check for MAX
}
if (stTmp == rcSticks) {
if (rcDelayCommand < 250)
rcDelayCommand++;
} else
rcDelayCommand = 0;
rcSticks = stTmp;
// perform actions
if (!isUsingSticksToArm) {
if (rcModeIsActive(BOXARM)) {
// Arming via ARM BOX
if (throttleStatus == THROTTLE_LOW) {
if (ARMING_FLAG(OK_TO_ARM)) {
mwArm();
}
}
} else {
// Disarming via ARM BOX
if (ARMING_FLAG(ARMED) && rxIsReceivingSignal() && !failsafeIsActive() ) {
if (disarm_kill_switch) {
mwDisarm();
} else if (throttleStatus == THROTTLE_LOW) {
mwDisarm();
}
}
}
}
if (rcDelayCommand != 20) {
return;
}
if (isUsingSticksToArm) {
// Disarm on throttle down + yaw
if (rcSticks == THR_LO + YAW_LO + PIT_CE + ROL_CE) {
if (ARMING_FLAG(ARMED))
mwDisarm();
else {
beeper(BEEPER_DISARM_REPEAT); // sound tone while stick held
rcDelayCommand = 0; // reset so disarm tone will repeat
}
}
// Disarm on roll (only when retarded_arm is enabled)
if (retarded_arm && (rcSticks == THR_LO + YAW_CE + PIT_CE + ROL_LO)) {
if (ARMING_FLAG(ARMED))
mwDisarm();
else {
beeper(BEEPER_DISARM_REPEAT); // sound tone while stick held
rcDelayCommand = 0; // reset so disarm tone will repeat
}
}
}
if (ARMING_FLAG(ARMED)) {
// actions during armed
return;
}
// actions during not armed
i = 0;
if (rcSticks == THR_LO + YAW_LO + PIT_LO + ROL_CE) {
// GYRO calibration
gyroSetCalibrationCycles(CALIBRATING_GYRO_CYCLES);
#ifdef GPS
if (feature(FEATURE_GPS)) {
GPS_reset_home_position();
}
#endif
#ifdef BARO
if (sensors(SENSOR_BARO))
baroSetCalibrationCycles(10); // calibrate baro to new ground level (10 * 25 ms = ~250 ms non blocking)
#endif
return;
}
if (feature(FEATURE_INFLIGHT_ACC_CAL) && (rcSticks == THR_LO + YAW_LO + PIT_HI + ROL_HI)) {
// Inflight ACC Calibration
handleInflightCalibrationStickPosition();
return;
}
// Multiple configuration profiles
if (rcSticks == THR_LO + YAW_LO + PIT_CE + ROL_LO) // ROLL left -> Profile 1
i = 1;
else if (rcSticks == THR_LO + YAW_LO + PIT_HI + ROL_CE) // PITCH up -> Profile 2
i = 2;
else if (rcSticks == THR_LO + YAW_LO + PIT_CE + ROL_HI) // ROLL right -> Profile 3
i = 3;
if (i) {
changeProfile(i - 1);
beeperConfirmationBeeps(i);
return;
}
if (rcSticks == THR_LO + YAW_LO + PIT_LO + ROL_HI) {
saveConfigAndNotify();
}
if (isUsingSticksToArm) {
if (rcSticks == THR_LO + YAW_HI + PIT_CE + ROL_CE) {
// Arm via YAW
mwArm();
return;
}
if (retarded_arm && (rcSticks == THR_LO + YAW_CE + PIT_CE + ROL_HI)) {
// Arm via ROLL
mwArm();
return;
}
}
if (rcSticks == THR_HI + YAW_LO + PIT_LO + ROL_CE) {
// Calibrating Acc
accSetCalibrationCycles(CALIBRATING_ACC_CYCLES);
return;
}
if (rcSticks == THR_HI + YAW_HI + PIT_LO + ROL_CE) {
// Calibrating Mag
ENABLE_STATE(CALIBRATE_MAG);
return;
}
// Accelerometer Trim
rollAndPitchTrims_t accelerometerTrimsDelta;
memset(&accelerometerTrimsDelta, 0, sizeof(accelerometerTrimsDelta));
bool shouldApplyRollAndPitchTrimDelta = false;
if (rcSticks == THR_HI + YAW_CE + PIT_HI + ROL_CE) {
accelerometerTrimsDelta.values.pitch = 2;
shouldApplyRollAndPitchTrimDelta = true;
} else if (rcSticks == THR_HI + YAW_CE + PIT_LO + ROL_CE) {
accelerometerTrimsDelta.values.pitch = -2;
shouldApplyRollAndPitchTrimDelta = true;
} else if (rcSticks == THR_HI + YAW_CE + PIT_CE + ROL_HI) {
accelerometerTrimsDelta.values.roll = 2;
shouldApplyRollAndPitchTrimDelta = true;
} else if (rcSticks == THR_HI + YAW_CE + PIT_CE + ROL_LO) {
accelerometerTrimsDelta.values.roll = -2;
shouldApplyRollAndPitchTrimDelta = true;
}
if (shouldApplyRollAndPitchTrimDelta) {
applyAndSaveAccelerometerTrimsDelta(&accelerometerTrimsDelta);
rcDelayCommand = 0; // allow autorepetition
return;
}
#ifdef DISPLAY
if (rcSticks == THR_LO + YAW_CE + PIT_HI + ROL_LO) {
displayDisablePageCycling();
}
if (rcSticks == THR_LO + YAW_CE + PIT_HI + ROL_HI) {
displayEnablePageCycling();
}
#endif
}
static void applyStepAdjustment(controlRateConfig_t *controlRateConfig, uint8_t adjustmentFunction, int delta) {
int newValue;
if (delta > 0) {
beeperConfirmationBeeps(2);
} else {
beeperConfirmationBeeps(1);
}
switch(adjustmentFunction) {
case ADJUSTMENT_RC_RATE:
newValue = constrain((int)controlRateConfig->rcRate8 + delta, 0, 250); // FIXME magic numbers repeated in serial_cli.c
controlRateConfig->rcRate8 = newValue;
blackboxLogInflightAdjustmentEvent(ADJUSTMENT_RC_RATE, newValue);
break;
case ADJUSTMENT_RC_EXPO:
newValue = constrain((int)controlRateConfig->rcExpo8 + delta, 0, 100); // FIXME magic numbers repeated in serial_cli.c
controlRateConfig->rcExpo8 = newValue;
blackboxLogInflightAdjustmentEvent(ADJUSTMENT_RC_EXPO, newValue);
break;
case ADJUSTMENT_THROTTLE_EXPO:
newValue = constrain((int)controlRateConfig->thrExpo8 + delta, 0, 100); // FIXME magic numbers repeated in serial_cli.c
controlRateConfig->thrExpo8 = newValue;
generateThrottleCurve(controlRateConfig, motorConfig);
blackboxLogInflightAdjustmentEvent(ADJUSTMENT_THROTTLE_EXPO, newValue);
break;
case ADJUSTMENT_PITCH_ROLL_RATE:
case ADJUSTMENT_PITCH_RATE:
newValue = constrain((int)controlRateConfig->rates[FD_PITCH] + delta, 0, CONTROL_RATE_CONFIG_ROLL_PITCH_RATE_MAX);
controlRateConfig->rates[FD_PITCH] = newValue;
blackboxLogInflightAdjustmentEvent(ADJUSTMENT_PITCH_RATE, newValue);
if (adjustmentFunction == ADJUSTMENT_PITCH_RATE) {
break;
}
// follow though for combined ADJUSTMENT_PITCH_ROLL_RATE
case ADJUSTMENT_ROLL_RATE:
newValue = constrain((int)controlRateConfig->rates[FD_ROLL] + delta, 0, CONTROL_RATE_CONFIG_ROLL_PITCH_RATE_MAX);
controlRateConfig->rates[FD_ROLL] = newValue;
blackboxLogInflightAdjustmentEvent(ADJUSTMENT_ROLL_RATE, newValue);
break;
case ADJUSTMENT_YAW_RATE:
newValue = constrain((int)controlRateConfig->rates[FD_YAW] + delta, 0, CONTROL_RATE_CONFIG_YAW_RATE_MAX);
controlRateConfig->rates[FD_YAW] = newValue;
blackboxLogInflightAdjustmentEvent(ADJUSTMENT_YAW_RATE, newValue);
break;
case ADJUSTMENT_PITCH_ROLL_P:
case ADJUSTMENT_PITCH_P:
newValue = constrain((int)pidProfile->P8[PIDPITCH] + delta, 0, 200); // FIXME magic numbers repeated in serial_cli.c
pidProfile->P8[PIDPITCH] = newValue;
blackboxLogInflightAdjustmentEvent(ADJUSTMENT_PITCH_P, newValue);
if (adjustmentFunction == ADJUSTMENT_PITCH_P) {
break;
}
// follow though for combined ADJUSTMENT_PITCH_ROLL_P
case ADJUSTMENT_ROLL_P:
newValue = constrain((int)pidProfile->P8[PIDROLL] + delta, 0, 200); // FIXME magic numbers repeated in serial_cli.c
pidProfile->P8[PIDROLL] = newValue;
blackboxLogInflightAdjustmentEvent(ADJUSTMENT_ROLL_P, newValue);
break;
case ADJUSTMENT_PITCH_ROLL_I:
case ADJUSTMENT_PITCH_I:
newValue = constrain((int)pidProfile->I8[PIDPITCH] + delta, 0, 200); // FIXME magic numbers repeated in serial_cli.c
pidProfile->I8[PIDPITCH] = newValue;
blackboxLogInflightAdjustmentEvent(ADJUSTMENT_PITCH_I, newValue);
if (adjustmentFunction == ADJUSTMENT_PITCH_I) {
break;
}
// follow though for combined ADJUSTMENT_PITCH_ROLL_I
case ADJUSTMENT_ROLL_I:
newValue = constrain((int)pidProfile->I8[PIDROLL] + delta, 0, 200); // FIXME magic numbers repeated in serial_cli.c
pidProfile->I8[PIDROLL] = newValue;
blackboxLogInflightAdjustmentEvent(ADJUSTMENT_ROLL_I, newValue);
break;
case ADJUSTMENT_PITCH_ROLL_D:
case ADJUSTMENT_PITCH_D:
newValue = constrain((int)pidProfile->D8[PIDPITCH] + delta, 0, 200); // FIXME magic numbers repeated in serial_cli.c
pidProfile->D8[PIDPITCH] = newValue;
blackboxLogInflightAdjustmentEvent(ADJUSTMENT_PITCH_D, newValue);
if (adjustmentFunction == ADJUSTMENT_PITCH_D) {
break;
}
// follow though for combined ADJUSTMENT_PITCH_ROLL_D
case ADJUSTMENT_ROLL_D:
newValue = constrain((int)pidProfile->D8[PIDROLL] + delta, 0, 200); // FIXME magic numbers repeated in serial_cli.c
pidProfile->D8[PIDROLL] = newValue;
blackboxLogInflightAdjustmentEvent(ADJUSTMENT_ROLL_D, newValue);
break;
case ADJUSTMENT_YAW_P:
newValue = constrain((int)pidProfile->P8[PIDYAW] + delta, 0, 200); // FIXME magic numbers repeated in serial_cli.c
pidProfile->P8[PIDYAW] = newValue;
blackboxLogInflightAdjustmentEvent(ADJUSTMENT_YAW_P, newValue);
break;
case ADJUSTMENT_YAW_I:
newValue = constrain((int)pidProfile->I8[PIDYAW] + delta, 0, 200); // FIXME magic numbers repeated in serial_cli.c
pidProfile->I8[PIDYAW] = newValue;
blackboxLogInflightAdjustmentEvent(ADJUSTMENT_YAW_I, newValue);
break;
case ADJUSTMENT_YAW_D:
newValue = constrain((int)pidProfile->D8[PIDYAW] + delta, 0, 200); // FIXME magic numbers repeated in serial_cli.c
pidProfile->D8[PIDYAW] = newValue;
blackboxLogInflightAdjustmentEvent(ADJUSTMENT_YAW_D, newValue);
break;
case ADJUSTMENT_RC_RATE_YAW:
newValue = constrain((int)controlRateConfig->rcYawRate8 + delta, 0, 300); // FIXME magic numbers repeated in serial_cli.c
controlRateConfig->rcYawRate8 = newValue;
blackboxLogInflightAdjustmentEvent(ADJUSTMENT_RC_RATE_YAW, newValue);
break;
default:
break;
};
}
static int applyStepAdjustment(controlRateConfig_t *controlRateConfig, uint8_t adjustmentFunction, int delta)
{
beeperConfirmationBeeps(delta > 0 ? 2 : 1);
int newValue;
switch (adjustmentFunction) {
case ADJUSTMENT_RC_RATE:
case ADJUSTMENT_ROLL_RC_RATE:
newValue = constrain((int)controlRateConfig->rcRates[FD_ROLL] + delta, 0, CONTROL_RATE_CONFIG_RC_RATES_MAX);
controlRateConfig->rcRates[FD_ROLL] = newValue;
blackboxLogInflightAdjustmentEvent(ADJUSTMENT_ROLL_RC_RATE, newValue);
if (adjustmentFunction == ADJUSTMENT_ROLL_RC_RATE) {
break;
}
// fall through for combined ADJUSTMENT_RC_EXPO
FALLTHROUGH;
case ADJUSTMENT_PITCH_RC_RATE:
newValue = constrain((int)controlRateConfig->rcRates[FD_PITCH] + delta, 0, CONTROL_RATE_CONFIG_RC_RATES_MAX);
controlRateConfig->rcRates[FD_PITCH] = newValue;
blackboxLogInflightAdjustmentEvent(ADJUSTMENT_PITCH_RC_RATE, newValue);
break;
case ADJUSTMENT_RC_EXPO:
case ADJUSTMENT_ROLL_RC_EXPO:
newValue = constrain((int)controlRateConfig->rcExpo[FD_ROLL] + delta, 0, CONTROL_RATE_CONFIG_RC_EXPO_MAX);
controlRateConfig->rcExpo[FD_ROLL] = newValue;
blackboxLogInflightAdjustmentEvent(ADJUSTMENT_ROLL_RC_EXPO, newValue);
if (adjustmentFunction == ADJUSTMENT_ROLL_RC_EXPO) {
break;
}
// fall through for combined ADJUSTMENT_RC_EXPO
FALLTHROUGH;
case ADJUSTMENT_PITCH_RC_EXPO:
newValue = constrain((int)controlRateConfig->rcExpo[FD_PITCH] + delta, 0, CONTROL_RATE_CONFIG_RC_EXPO_MAX);
controlRateConfig->rcExpo[FD_PITCH] = newValue;
blackboxLogInflightAdjustmentEvent(ADJUSTMENT_PITCH_RC_EXPO, newValue);
break;
case ADJUSTMENT_THROTTLE_EXPO:
newValue = constrain((int)controlRateConfig->thrExpo8 + delta, 0, 100); // FIXME magic numbers repeated in cli.c
controlRateConfig->thrExpo8 = newValue;
initRcProcessing();
blackboxLogInflightAdjustmentEvent(ADJUSTMENT_THROTTLE_EXPO, newValue);
break;
case ADJUSTMENT_PITCH_ROLL_RATE:
case ADJUSTMENT_PITCH_RATE:
newValue = constrain((int)controlRateConfig->rates[FD_PITCH] + delta, 0, CONTROL_RATE_CONFIG_RATE_MAX);
controlRateConfig->rates[FD_PITCH] = newValue;
blackboxLogInflightAdjustmentEvent(ADJUSTMENT_PITCH_RATE, newValue);
if (adjustmentFunction == ADJUSTMENT_PITCH_RATE) {
break;
}
// fall through for combined ADJUSTMENT_PITCH_ROLL_RATE
FALLTHROUGH;
case ADJUSTMENT_ROLL_RATE:
newValue = constrain((int)controlRateConfig->rates[FD_ROLL] + delta, 0, CONTROL_RATE_CONFIG_RATE_MAX);
controlRateConfig->rates[FD_ROLL] = newValue;
blackboxLogInflightAdjustmentEvent(ADJUSTMENT_ROLL_RATE, newValue);
break;
case ADJUSTMENT_YAW_RATE:
newValue = constrain((int)controlRateConfig->rates[FD_YAW] + delta, 0, CONTROL_RATE_CONFIG_RATE_MAX);
controlRateConfig->rates[FD_YAW] = newValue;
blackboxLogInflightAdjustmentEvent(ADJUSTMENT_YAW_RATE, newValue);
break;
case ADJUSTMENT_PITCH_ROLL_P:
case ADJUSTMENT_PITCH_P:
newValue = constrain((int)pidProfile->pid[PID_PITCH].P + delta, 0, 200); // FIXME magic numbers repeated in cli.c
pidProfile->pid[PID_PITCH].P = newValue;
blackboxLogInflightAdjustmentEvent(ADJUSTMENT_PITCH_P, newValue);
if (adjustmentFunction == ADJUSTMENT_PITCH_P) {
break;
}
// fall through for combined ADJUSTMENT_PITCH_ROLL_P
FALLTHROUGH;
case ADJUSTMENT_ROLL_P:
newValue = constrain((int)pidProfile->pid[PID_ROLL].P + delta, 0, 200); // FIXME magic numbers repeated in cli.c
pidProfile->pid[PID_ROLL].P = newValue;
blackboxLogInflightAdjustmentEvent(ADJUSTMENT_ROLL_P, newValue);
break;
case ADJUSTMENT_PITCH_ROLL_I:
case ADJUSTMENT_PITCH_I:
newValue = constrain((int)pidProfile->pid[PID_PITCH].I + delta, 0, 200); // FIXME magic numbers repeated in cli.c
pidProfile->pid[PID_PITCH].I = newValue;
blackboxLogInflightAdjustmentEvent(ADJUSTMENT_PITCH_I, newValue);
if (adjustmentFunction == ADJUSTMENT_PITCH_I) {
break;
}
// fall through for combined ADJUSTMENT_PITCH_ROLL_I
FALLTHROUGH;
case ADJUSTMENT_ROLL_I:
newValue = constrain((int)pidProfile->pid[PID_ROLL].I + delta, 0, 200); // FIXME magic numbers repeated in cli.c
pidProfile->pid[PID_ROLL].I = newValue;
blackboxLogInflightAdjustmentEvent(ADJUSTMENT_ROLL_I, newValue);
break;
case ADJUSTMENT_PITCH_ROLL_D:
case ADJUSTMENT_PITCH_D:
newValue = constrain((int)pidProfile->pid[PID_PITCH].D + delta, 0, 200); // FIXME magic numbers repeated in cli.c
pidProfile->pid[PID_PITCH].D = newValue;
blackboxLogInflightAdjustmentEvent(ADJUSTMENT_PITCH_D, newValue);
if (adjustmentFunction == ADJUSTMENT_PITCH_D) {
break;
}
// fall through for combined ADJUSTMENT_PITCH_ROLL_D
FALLTHROUGH;
case ADJUSTMENT_ROLL_D:
newValue = constrain((int)pidProfile->pid[PID_ROLL].D + delta, 0, 200); // FIXME magic numbers repeated in cli.c
pidProfile->pid[PID_ROLL].D = newValue;
blackboxLogInflightAdjustmentEvent(ADJUSTMENT_ROLL_D, newValue);
break;
case ADJUSTMENT_YAW_P:
newValue = constrain((int)pidProfile->pid[PID_YAW].P + delta, 0, 200); // FIXME magic numbers repeated in cli.c
pidProfile->pid[PID_YAW].P = newValue;
blackboxLogInflightAdjustmentEvent(ADJUSTMENT_YAW_P, newValue);
break;
case ADJUSTMENT_YAW_I:
newValue = constrain((int)pidProfile->pid[PID_YAW].I + delta, 0, 200); // FIXME magic numbers repeated in cli.c
pidProfile->pid[PID_YAW].I = newValue;
blackboxLogInflightAdjustmentEvent(ADJUSTMENT_YAW_I, newValue);
break;
case ADJUSTMENT_YAW_D:
newValue = constrain((int)pidProfile->pid[PID_YAW].D + delta, 0, 200); // FIXME magic numbers repeated in cli.c
pidProfile->pid[PID_YAW].D = newValue;
blackboxLogInflightAdjustmentEvent(ADJUSTMENT_YAW_D, newValue);
break;
case ADJUSTMENT_RC_RATE_YAW:
newValue = constrain((int)controlRateConfig->rcRates[FD_YAW] + delta, 0, CONTROL_RATE_CONFIG_RC_RATES_MAX);
controlRateConfig->rcRates[FD_YAW] = newValue;
blackboxLogInflightAdjustmentEvent(ADJUSTMENT_RC_RATE_YAW, newValue);
break;
case ADJUSTMENT_D_SETPOINT:
newValue = constrain((int)pidProfile->dtermSetpointWeight + delta, 0, 254); // FIXME magic numbers repeated in cli.c
pidProfile->dtermSetpointWeight = newValue;
blackboxLogInflightAdjustmentEvent(ADJUSTMENT_D_SETPOINT, newValue);
break;
case ADJUSTMENT_D_SETPOINT_TRANSITION:
newValue = constrain((int)pidProfile->setpointRelaxRatio + delta, 1, 100); // FIXME magic numbers repeated in cli.c
pidProfile->setpointRelaxRatio = newValue;
blackboxLogInflightAdjustmentEvent(ADJUSTMENT_D_SETPOINT_TRANSITION, newValue);
break;
default:
newValue = -1;
break;
};
return newValue;
}
void saveConfigAndNotify(void)
{
writeEEPROM();
readEEPROM();
beeperConfirmationBeeps(1);
}
static void applyStepAdjustment(controlRateConfig_t *controlRateConfig, uint8_t adjustmentFunction, int delta)
{
if (delta > 0) {
beeperConfirmationBeeps(2);
} else {
beeperConfirmationBeeps(1);
}
switch (adjustmentFunction) {
case ADJUSTMENT_RC_EXPO:
applyAdjustmentExpo(ADJUSTMENT_RC_EXPO, &controlRateConfig->stabilized.rcExpo8, delta);
break;
case ADJUSTMENT_RC_YAW_EXPO:
applyAdjustmentExpo(ADJUSTMENT_RC_YAW_EXPO, &controlRateConfig->stabilized.rcYawExpo8, delta);
break;
case ADJUSTMENT_MANUAL_RC_EXPO:
applyAdjustmentExpo(ADJUSTMENT_MANUAL_RC_EXPO, &controlRateConfig->manual.rcExpo8, delta);
break;
case ADJUSTMENT_MANUAL_RC_YAW_EXPO:
applyAdjustmentExpo(ADJUSTMENT_MANUAL_RC_YAW_EXPO, &controlRateConfig->manual.rcYawExpo8, delta);
break;
case ADJUSTMENT_THROTTLE_EXPO:
applyAdjustmentExpo(ADJUSTMENT_THROTTLE_EXPO, &controlRateConfig->throttle.rcExpo8, delta);
break;
case ADJUSTMENT_PITCH_ROLL_RATE:
case ADJUSTMENT_PITCH_RATE:
applyAdjustmentU8(ADJUSTMENT_PITCH_RATE, &controlRateConfig->stabilized.rates[FD_PITCH], delta, CONTROL_RATE_CONFIG_ROLL_PITCH_RATE_MIN, CONTROL_RATE_CONFIG_ROLL_PITCH_RATE_MAX);
if (adjustmentFunction == ADJUSTMENT_PITCH_RATE) {
schedulePidGainsUpdate();
break;
}
// follow though for combined ADJUSTMENT_PITCH_ROLL_RATE
FALLTHROUGH;
case ADJUSTMENT_ROLL_RATE:
applyAdjustmentU8(ADJUSTMENT_ROLL_RATE, &controlRateConfig->stabilized.rates[FD_ROLL], delta, CONTROL_RATE_CONFIG_ROLL_PITCH_RATE_MIN, CONTROL_RATE_CONFIG_ROLL_PITCH_RATE_MAX);
schedulePidGainsUpdate();
break;
case ADJUSTMENT_MANUAL_PITCH_ROLL_RATE:
case ADJUSTMENT_MANUAL_ROLL_RATE:
applyAdjustmentManualRate(ADJUSTMENT_MANUAL_ROLL_RATE, &controlRateConfig->manual.rates[FD_ROLL], delta);
if (adjustmentFunction == ADJUSTMENT_MANUAL_ROLL_RATE)
break;
// follow though for combined ADJUSTMENT_MANUAL_PITCH_ROLL_RATE
FALLTHROUGH;
case ADJUSTMENT_MANUAL_PITCH_RATE:
applyAdjustmentManualRate(ADJUSTMENT_MANUAL_PITCH_RATE, &controlRateConfig->manual.rates[FD_PITCH], delta);
break;
case ADJUSTMENT_YAW_RATE:
applyAdjustmentU8(ADJUSTMENT_YAW_RATE, &controlRateConfig->stabilized.rates[FD_YAW], delta, CONTROL_RATE_CONFIG_YAW_RATE_MIN, CONTROL_RATE_CONFIG_YAW_RATE_MAX);
schedulePidGainsUpdate();
break;
case ADJUSTMENT_MANUAL_YAW_RATE:
applyAdjustmentManualRate(ADJUSTMENT_MANUAL_YAW_RATE, &controlRateConfig->manual.rates[FD_YAW], delta);
break;
case ADJUSTMENT_PITCH_ROLL_P:
case ADJUSTMENT_PITCH_P:
applyAdjustmentPID(ADJUSTMENT_PITCH_P, &pidBankMutable()->pid[PID_PITCH].P, delta);
if (adjustmentFunction == ADJUSTMENT_PITCH_P) {
schedulePidGainsUpdate();
break;
}
// follow though for combined ADJUSTMENT_PITCH_ROLL_P
FALLTHROUGH;
case ADJUSTMENT_ROLL_P:
applyAdjustmentPID(ADJUSTMENT_ROLL_P, &pidBankMutable()->pid[PID_ROLL].P, delta);
schedulePidGainsUpdate();
break;
case ADJUSTMENT_PITCH_ROLL_I:
case ADJUSTMENT_PITCH_I:
applyAdjustmentPID(ADJUSTMENT_PITCH_I, &pidBankMutable()->pid[PID_PITCH].I, delta);
if (adjustmentFunction == ADJUSTMENT_PITCH_I) {
schedulePidGainsUpdate();
break;
}
// follow though for combined ADJUSTMENT_PITCH_ROLL_I
FALLTHROUGH;
case ADJUSTMENT_ROLL_I:
applyAdjustmentPID(ADJUSTMENT_ROLL_I, &pidBankMutable()->pid[PID_ROLL].I, delta);
schedulePidGainsUpdate();
break;
case ADJUSTMENT_PITCH_ROLL_D:
case ADJUSTMENT_PITCH_D:
applyAdjustmentPID(ADJUSTMENT_PITCH_D, &pidBankMutable()->pid[PID_PITCH].D, delta);
if (adjustmentFunction == ADJUSTMENT_PITCH_D) {
schedulePidGainsUpdate();
break;
}
// follow though for combined ADJUSTMENT_PITCH_ROLL_D
FALLTHROUGH;
case ADJUSTMENT_ROLL_D:
applyAdjustmentPID(ADJUSTMENT_ROLL_D, &pidBankMutable()->pid[PID_ROLL].D, delta);
schedulePidGainsUpdate();
break;
case ADJUSTMENT_YAW_P:
applyAdjustmentPID(ADJUSTMENT_YAW_P, &pidBankMutable()->pid[PID_YAW].P, delta);
schedulePidGainsUpdate();
break;
case ADJUSTMENT_YAW_I:
applyAdjustmentPID(ADJUSTMENT_YAW_I, &pidBankMutable()->pid[PID_YAW].I, delta);
schedulePidGainsUpdate();
break;
case ADJUSTMENT_YAW_D:
applyAdjustmentPID(ADJUSTMENT_YAW_D, &pidBankMutable()->pid[PID_YAW].D, delta);
schedulePidGainsUpdate();
break;
case ADJUSTMENT_NAV_FW_CRUISE_THR:
applyAdjustmentU16(ADJUSTMENT_NAV_FW_CRUISE_THR, &navConfigMutable()->fw.cruise_throttle, delta, 1000, 2000);
break;
case ADJUSTMENT_NAV_FW_PITCH2THR:
applyAdjustmentU8(ADJUSTMENT_NAV_FW_PITCH2THR, &navConfigMutable()->fw.pitch_to_throttle, delta, 0, 100);
break;
case ADJUSTMENT_ROLL_BOARD_ALIGNMENT:
updateBoardAlignment(delta, 0);
blackboxLogInflightAdjustmentEvent(ADJUSTMENT_ROLL_BOARD_ALIGNMENT, boardAlignment()->rollDeciDegrees);
break;
case ADJUSTMENT_PITCH_BOARD_ALIGNMENT:
updateBoardAlignment(0, delta);
blackboxLogInflightAdjustmentEvent(ADJUSTMENT_PITCH_BOARD_ALIGNMENT, boardAlignment()->pitchDeciDegrees);
break;
case ADJUSTMENT_LEVEL_P:
applyAdjustmentPID(ADJUSTMENT_LEVEL_P, &pidBankMutable()->pid[PID_LEVEL].P, delta);
// TODO: Need to call something to take it into account?
break;
case ADJUSTMENT_LEVEL_I:
applyAdjustmentPID(ADJUSTMENT_LEVEL_I, &pidBankMutable()->pid[PID_LEVEL].I, delta);
// TODO: Need to call something to take it into account?
break;
case ADJUSTMENT_LEVEL_D:
applyAdjustmentPID(ADJUSTMENT_LEVEL_D, &pidBankMutable()->pid[PID_LEVEL].D, delta);
// TODO: Need to call something to take it into account?
break;
case ADJUSTMENT_POS_XY_P:
applyAdjustmentPID(ADJUSTMENT_POS_XY_P, &pidBankMutable()->pid[PID_POS_XY].P, delta);
navigationUsePIDs();
break;
case ADJUSTMENT_POS_XY_I:
applyAdjustmentPID(ADJUSTMENT_POS_XY_I, &pidBankMutable()->pid[PID_POS_XY].I, delta);
navigationUsePIDs();
break;
case ADJUSTMENT_POS_XY_D:
applyAdjustmentPID(ADJUSTMENT_POS_XY_D, &pidBankMutable()->pid[PID_POS_XY].D, delta);
navigationUsePIDs();
break;
case ADJUSTMENT_POS_Z_P:
applyAdjustmentPID(ADJUSTMENT_POS_Z_P, &pidBankMutable()->pid[PID_POS_Z].P, delta);
navigationUsePIDs();
break;
case ADJUSTMENT_POS_Z_I:
applyAdjustmentPID(ADJUSTMENT_POS_Z_I, &pidBankMutable()->pid[PID_POS_Z].I, delta);
navigationUsePIDs();
break;
case ADJUSTMENT_POS_Z_D:
applyAdjustmentPID(ADJUSTMENT_POS_Z_D, &pidBankMutable()->pid[PID_POS_Z].D, delta);
navigationUsePIDs();
break;
case ADJUSTMENT_HEADING_P:
applyAdjustmentPID(ADJUSTMENT_HEADING_P, &pidBankMutable()->pid[PID_HEADING].P, delta);
// TODO: navigationUsePIDs()?
break;
case ADJUSTMENT_VEL_XY_P:
applyAdjustmentPID(ADJUSTMENT_VEL_XY_P, &pidBankMutable()->pid[PID_VEL_XY].P, delta);
navigationUsePIDs();
break;
case ADJUSTMENT_VEL_XY_I:
applyAdjustmentPID(ADJUSTMENT_VEL_XY_I, &pidBankMutable()->pid[PID_VEL_XY].I, delta);
navigationUsePIDs();
break;
case ADJUSTMENT_VEL_XY_D:
applyAdjustmentPID(ADJUSTMENT_VEL_XY_D, &pidBankMutable()->pid[PID_VEL_XY].D, delta);
navigationUsePIDs();
break;
case ADJUSTMENT_VEL_Z_P:
applyAdjustmentPID(ADJUSTMENT_VEL_Z_P, &pidBankMutable()->pid[PID_VEL_Z].P, delta);
navigationUsePIDs();
break;
case ADJUSTMENT_VEL_Z_I:
applyAdjustmentPID(ADJUSTMENT_VEL_Z_I, &pidBankMutable()->pid[PID_VEL_Z].I, delta);
navigationUsePIDs();
break;
case ADJUSTMENT_VEL_Z_D:
applyAdjustmentPID(ADJUSTMENT_VEL_Z_D, &pidBankMutable()->pid[PID_VEL_Z].D, delta);
navigationUsePIDs();
break;
case ADJUSTMENT_FW_MIN_THROTTLE_DOWN_PITCH_ANGLE:
applyAdjustmentU16(ADJUSTMENT_FW_MIN_THROTTLE_DOWN_PITCH_ANGLE, &mixerConfigMutable()->fwMinThrottleDownPitchAngle, delta, 0, FW_MIN_THROTTLE_DOWN_PITCH_ANGLE_MAX);
break;
default:
break;
};
}
void applyStepAdjustment(controlRateConfig_t *controlRateConfig, uint8_t adjustmentFunction, int delta) {
int newValue;
float newFloatValue;
if (delta > 0) {
beeperConfirmationBeeps(2);
} else {
beeperConfirmationBeeps(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:
case ADJUSTMENT_PITCH_RATE:
newValue = (int)controlRateConfig->rates[FD_PITCH] + delta;
controlRateConfig->rates[FD_PITCH] = constrain(newValue, 0, CONTROL_RATE_CONFIG_ROLL_PITCH_RATE_MAX);
if (adjustmentFunction == ADJUSTMENT_PITCH_RATE) {
break;
}
// follow though for combined ADJUSTMENT_PITCH_ROLL_RATE
case ADJUSTMENT_ROLL_RATE:
newValue = (int)controlRateConfig->rates[FD_ROLL] + delta;
controlRateConfig->rates[FD_ROLL] = constrain(newValue, 0, CONTROL_RATE_CONFIG_ROLL_PITCH_RATE_MAX);
break;
case ADJUSTMENT_YAW_RATE:
newValue = (int)controlRateConfig->rates[FD_YAW] + delta;
controlRateConfig->rates[FD_YAW] = constrain(newValue, 0, CONTROL_RATE_CONFIG_YAW_RATE_MAX);
break;
case ADJUSTMENT_PITCH_ROLL_P:
case ADJUSTMENT_PITCH_P:
if (IS_PID_CONTROLLER_FP_BASED(pidProfile->pidController)) {
newFloatValue = pidProfile->P_f[PIDPITCH] + (float)(delta / 10.0f);
pidProfile->P_f[PIDPITCH] = constrainf(newFloatValue, 0, 100); // FIXME magic numbers repeated in serial_cli.c
} else {
newValue = (int)pidProfile->P8[PIDPITCH] + delta;
pidProfile->P8[PIDPITCH] = constrain(newValue, 0, 200); // FIXME magic numbers repeated in serial_cli.c
}
if (adjustmentFunction == ADJUSTMENT_PITCH_P) {
break;
}
// follow though for combined ADJUSTMENT_PITCH_ROLL_P
case ADJUSTMENT_ROLL_P:
if (IS_PID_CONTROLLER_FP_BASED(pidProfile->pidController)) {
newFloatValue = pidProfile->P_f[PIDROLL] + (float)(delta / 10.0f);
pidProfile->P_f[PIDROLL] = constrainf(newFloatValue, 0, 100); // FIXME magic numbers repeated in serial_cli.c
} else {
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:
case ADJUSTMENT_PITCH_I:
if (IS_PID_CONTROLLER_FP_BASED(pidProfile->pidController)) {
newFloatValue = pidProfile->I_f[PIDPITCH] + (float)(delta / 100.0f);
pidProfile->I_f[PIDPITCH] = constrainf(newFloatValue, 0, 100); // FIXME magic numbers repeated in serial_cli.c
} else {
newValue = (int)pidProfile->I8[PIDPITCH] + delta;
pidProfile->I8[PIDPITCH] = constrain(newValue, 0, 200); // FIXME magic numbers repeated in serial_cli.c
}
if (adjustmentFunction == ADJUSTMENT_PITCH_I) {
break;
}
// follow though for combined ADJUSTMENT_PITCH_ROLL_I
case ADJUSTMENT_ROLL_I:
if (IS_PID_CONTROLLER_FP_BASED(pidProfile->pidController)) {
newFloatValue = pidProfile->I_f[PIDROLL] + (float)(delta / 100.0f);
pidProfile->I_f[PIDROLL] = constrainf(newFloatValue, 0, 100); // FIXME magic numbers repeated in serial_cli.c
} else {
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:
case ADJUSTMENT_PITCH_D:
if (IS_PID_CONTROLLER_FP_BASED(pidProfile->pidController)) {
newFloatValue = pidProfile->D_f[PIDPITCH] + (float)(delta / 1000.0f);
pidProfile->D_f[PIDPITCH] = constrainf(newFloatValue, 0, 100); // FIXME magic numbers repeated in serial_cli.c
} else {
newValue = (int)pidProfile->D8[PIDPITCH] + delta;
pidProfile->D8[PIDPITCH] = constrain(newValue, 0, 200); // FIXME magic numbers repeated in serial_cli.c
}
if (adjustmentFunction == ADJUSTMENT_PITCH_D) {
break;
}
// follow though for combined ADJUSTMENT_PITCH_ROLL_D
case ADJUSTMENT_ROLL_D:
if (IS_PID_CONTROLLER_FP_BASED(pidProfile->pidController)) {
newFloatValue = pidProfile->D_f[PIDROLL] + (float)(delta / 1000.0f);
pidProfile->D_f[PIDROLL] = constrainf(newFloatValue, 0, 100); // FIXME magic numbers repeated in serial_cli.c
} else {
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:
if (IS_PID_CONTROLLER_FP_BASED(pidProfile->pidController)) {
newFloatValue = pidProfile->P_f[PIDYAW] + (float)(delta / 10.0f);
pidProfile->P_f[PIDYAW] = constrainf(newFloatValue, 0, 100); // FIXME magic numbers repeated in serial_cli.c
} else {
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:
if (IS_PID_CONTROLLER_FP_BASED(pidProfile->pidController)) {
newFloatValue = pidProfile->I_f[PIDYAW] + (float)(delta / 100.0f);
pidProfile->I_f[PIDYAW] = constrainf(newFloatValue, 0, 100); // FIXME magic numbers repeated in serial_cli.c
} else {
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:
if (IS_PID_CONTROLLER_FP_BASED(pidProfile->pidController)) {
newFloatValue = pidProfile->D_f[PIDYAW] + (float)(delta / 1000.0f);
pidProfile->D_f[PIDYAW] = constrainf(newFloatValue, 0, 100); // FIXME magic numbers repeated in serial_cli.c
} else {
newValue = (int)pidProfile->D8[PIDYAW] + delta;
pidProfile->D8[PIDYAW] = constrain(newValue, 0, 200); // FIXME magic numbers repeated in serial_cli.c
}
break;
default:
break;
};
}
void readEEPROMAndNotify(void)
{
// re-read written data
readEEPROM();
beeperConfirmationBeeps(1);
}
void applyStepAdjustment(controlRateConfig_t *controlRateConfig, uint8_t adjustmentFunction, int delta) {
if (delta > 0) {
beeperConfirmationBeeps(2);
} else {
beeperConfirmationBeeps(1);
}
switch(adjustmentFunction) {
case ADJUSTMENT_RC_RATE:
setAdjustment(&controlRateConfig->rcRate8,0,ADJUSTMENT_RC_RATE,delta,0,RC_RATE_MIN,RC_RATE_MAX);
generatePitchRollCurve(controlRateConfig);
break;
case ADJUSTMENT_RC_EXPO:
setAdjustment(&controlRateConfig->rcExpo8,0,ADJUSTMENT_RC_EXPO,delta,0,EXPO_MIN,EXPO_MAX);
generatePitchRollCurve(controlRateConfig);
break;
case ADJUSTMENT_THROTTLE_EXPO:
setAdjustment(&controlRateConfig->thrExpo8,0,ADJUSTMENT_THROTTLE_EXPO,delta,0,EXPO_MIN,EXPO_MAX);
generateThrottleCurve(controlRateConfig, escAndServoConfig);
break;
case ADJUSTMENT_PITCH_ROLL_RATE:
case ADJUSTMENT_PITCH_RATE:
setAdjustment(&controlRateConfig->rates[FD_PITCH],0,ADJUSTMENT_PITCH_RATE,delta,0,0,CONTROL_RATE_CONFIG_ROLL_PITCH_RATE_MAX);
if (adjustmentFunction == ADJUSTMENT_PITCH_RATE) {
break;
}
// follow though for combined ADJUSTMENT_PITCH_ROLL_RATE
case ADJUSTMENT_ROLL_RATE:
setAdjustment(&controlRateConfig->rates[FD_ROLL],0,ADJUSTMENT_ROLL_RATE,delta,0,0,CONTROL_RATE_CONFIG_ROLL_PITCH_RATE_MAX);
break;
case ADJUSTMENT_YAW_RATE:
setAdjustment(&controlRateConfig->rates[FD_YAW],0,ADJUSTMENT_YAW_RATE,delta,0,0,CONTROL_RATE_CONFIG_YAW_RATE_MAX);
break;
case ADJUSTMENT_PITCH_ROLL_P:
case ADJUSTMENT_PITCH_P:
setAdjustment(&pidProfile->P8[PIDPITCH],&pidProfile->P_f[PIDPITCH],ADJUSTMENT_PITCH_P,delta,10.0f,PID_MIN,PID_MAX);
if (adjustmentFunction == ADJUSTMENT_PITCH_P) {
break;
}
// follow though for combined ADJUSTMENT_PITCH_ROLL_P
case ADJUSTMENT_ROLL_P:
setAdjustment(&pidProfile->P8[PIDROLL],&pidProfile->P_f[PIDROLL],ADJUSTMENT_ROLL_P,delta,10.0f,PID_MIN,PID_MAX);
break;
case ADJUSTMENT_PITCH_ROLL_I:
case ADJUSTMENT_PITCH_I:
setAdjustment(&pidProfile->I8[PIDPITCH],&pidProfile->I_f[PIDPITCH],ADJUSTMENT_PITCH_I,delta,100.0f,PID_MIN,PID_MAX);
if (adjustmentFunction == ADJUSTMENT_PITCH_I) {
break;
}
// follow though for combined ADJUSTMENT_PITCH_ROLL_I
case ADJUSTMENT_ROLL_I:
setAdjustment(&pidProfile->I8[PIDROLL],&pidProfile->I_f[PIDROLL],ADJUSTMENT_ROLL_I,delta,100.0f,PID_MIN,PID_MAX);
break;
case ADJUSTMENT_PITCH_ROLL_D:
case ADJUSTMENT_PITCH_D:
setAdjustment(&pidProfile->D8[PIDPITCH],&pidProfile->D_f[PIDPITCH],ADJUSTMENT_PITCH_D,delta,1000.0f,PID_MIN,PID_MAX);
if (adjustmentFunction == ADJUSTMENT_PITCH_D) {
break;
}
// follow though for combined ADJUSTMENT_PITCH_ROLL_D
case ADJUSTMENT_ROLL_D:
setAdjustment(&pidProfile->D8[PIDROLL],&pidProfile->D_f[PIDROLL],ADJUSTMENT_ROLL_D,delta,1000.0f,PID_MIN,PID_MAX);
break;
case ADJUSTMENT_YAW_P:
setAdjustment(&pidProfile->P8[PIDYAW],&pidProfile->P_f[PIDYAW],ADJUSTMENT_YAW_P,delta,10.0f,PID_MIN,PID_MAX);
break;
case ADJUSTMENT_YAW_I:
setAdjustment(&pidProfile->I8[PIDYAW],&pidProfile->I_f[PIDYAW],ADJUSTMENT_YAW_I,delta,100.0f,PID_MIN,PID_MAX);
break;
case ADJUSTMENT_YAW_D:
setAdjustment(&pidProfile->D8[PIDYAW],&pidProfile->D_f[PIDYAW],ADJUSTMENT_YAW_D,delta,1000.0f,PID_MIN,PID_MAX);
break;
case ADJUSTMENT_LEVEL_P:
setAdjustment(&pidProfile->P8[PIDLEVEL],&pidProfile->A_level,ADJUSTMENT_LEVEL_P,delta,10.0f,PID_MIN,PID_MAX);
break;
case ADJUSTMENT_LEVEL_I:
setAdjustment(&pidProfile->I8[PIDLEVEL],&pidProfile->H_level,ADJUSTMENT_LEVEL_I,delta,10.0f,PID_MIN,PID_MAX);
break;
case ADJUSTMENT_LEVEL_D:
if (IS_PID_CONTROLLER_FP_BASED(pidProfile->pidController)) {
setAdjustment(&pidProfile->H_sensitivity,0,ADJUSTMENT_LEVEL_D,delta,0,PID_MIN,PID_MAX);
} else {
setAdjustment(&pidProfile->D8[PIDLEVEL],0,ADJUSTMENT_LEVEL_D,delta,0,PID_MIN,PID_MAX);
}
break;
case ADJUSTMENT_ALT_P:
setAdjustment(&pidProfile->P8[PIDALT],0,ADJUSTMENT_ALT_P,delta,0,PID_MIN,PID_MAX);
break;
case ADJUSTMENT_ALT_I:
setAdjustment(&pidProfile->I8[PIDALT],0,ADJUSTMENT_ALT_I,delta,0,PID_MIN,PID_MAX);
break;
case ADJUSTMENT_ALT_D:
setAdjustment(&pidProfile->D8[PIDALT],0,ADJUSTMENT_ALT_D,delta,0,PID_MIN,PID_MAX);
break;
case ADJUSTMENT_POS_P:
setAdjustment(&pidProfile->P8[PIDPOS],0,ADJUSTMENT_POS_P,delta,0,PID_MIN,PID_MAX);
break;
case ADJUSTMENT_POS_I:
setAdjustment(&pidProfile->I8[PIDPOS],0,ADJUSTMENT_POS_I,delta,0,PID_MIN,PID_MAX);
break;
case ADJUSTMENT_POSR_P:
setAdjustment(&pidProfile->P8[PIDPOSR],0,ADJUSTMENT_POSR_P,delta,0,PID_MIN,PID_MAX);
break;
case ADJUSTMENT_POSR_I:
setAdjustment(&pidProfile->I8[PIDPOSR],0,ADJUSTMENT_POSR_I,delta,0,PID_MIN,PID_MAX);
break;
case ADJUSTMENT_POSR_D:
setAdjustment(&pidProfile->D8[PIDPOSR],0,ADJUSTMENT_POSR_D,delta,0,PID_MIN,PID_MAX);
break;
case ADJUSTMENT_NAVR_P:
setAdjustment(&pidProfile->P8[PIDNAVR],0,ADJUSTMENT_NAVR_P,delta,0,PID_MIN,PID_MAX);
break;
case ADJUSTMENT_NAVR_I:
setAdjustment(&pidProfile->I8[PIDNAVR],0,ADJUSTMENT_NAVR_I,delta,0,PID_MIN,PID_MAX);
break;
case ADJUSTMENT_NAVR_D:
setAdjustment(&pidProfile->D8[PIDNAVR],0,ADJUSTMENT_NAVR_D,delta,0,PID_MIN,PID_MAX);
break;
case ADJUSTMENT_MAG_P:
setAdjustment(&pidProfile->P8[PIDMAG],0,ADJUSTMENT_MAG_P,delta,0,PID_MIN,PID_MAX);
break;
case ADJUSTMENT_VEL_P:
setAdjustment(&pidProfile->P8[PIDVEL],0,ADJUSTMENT_VEL_P,delta,0,PID_MIN,PID_MAX);
break;
case ADJUSTMENT_VEL_I:
setAdjustment(&pidProfile->I8[PIDVEL],0,ADJUSTMENT_VEL_I,delta,0,PID_MIN,PID_MAX);
break;
case ADJUSTMENT_VEL_D:
setAdjustment(&pidProfile->D8[PIDVEL],0,ADJUSTMENT_VEL_D,delta,0,PID_MIN,PID_MAX);
break;
default:
break;
};
}
