Mirror/inav
1
0
Fork 0
mirror of https://github.com/iNavFlight/inav.git synced 2025-07-17 05:15:23 +03:00
Code Issues Wiki Activity

Separated rc adjustments into separate module

Browse source
This commit is contained in:
Martin Budden 2017-01-19 17:12:28 +00:00
parent 857b3603c4
commit 1060f1552d
10 changed files with 540 additions and 467 deletions
Download patch file Download diff file Expand all files Collapse all files

1
Makefile
View file

@ -520,6 +520,7 @@ COMMON_SRC = \
fc/fc_hardfaults.c \
fc/fc_msp.c \
fc/mw.c \
fc/rc_adjustments.c \
fc/rc_controls.c \
fc/rc_curves.c \
fc/runtime_config.c \

1
src/main/fc/config.c
View file

@ -67,6 +67,7 @@
#include "fc/config.h"
#include "fc/controlrate_profile.h"
#include "fc/rc_adjustments.h"
#include "fc/rc_controls.h"
#include "fc/rc_curves.h"
#include "fc/runtime_config.h"

1
src/main/fc/fc_msp.c
View file

@ -45,6 +45,7 @@
#include "fc/config.h"
#include "fc/controlrate_profile.h"
#include "fc/fc_msp.h"
#include "fc/rc_adjustments.h"
#include "fc/rc_controls.h"
#include "fc/runtime_config.h"

1
src/main/fc/mw.c
View file

@ -47,6 +47,7 @@
#include "fc/config.h"
#include "fc/controlrate_profile.h"
#include "fc/rc_adjustments.h"
#include "fc/rc_controls.h"
#include "fc/rc_curves.h"
#include "fc/runtime_config.h"

423
src/main/fc/rc_adjustments.c Normal file
View file

@ -0,0 +1,423 @@
/*
* This file is part of Cleanflight.
*
* Cleanflight is free software: you can redistribute it and/or modify
* it under the terms of the GNU General Public License as published by
* the Free Software Foundation, either version 3 of the License, or
* (at your option) any later version.
*
* Cleanflight is distributed in the hope that it will be useful,
* but WITHOUT ANY WARRANTY; without even the implied warranty of
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
* GNU General Public License for more details.
*
* You should have received a copy of the GNU General Public License
* along with Cleanflight. If not, see <http://www.gnu.org/licenses/>.
*/
#include <stdbool.h>
#include <stdint.h>
#include <string.h>
#include "platform.h"
#include "blackbox/blackbox.h"
#include "blackbox/blackbox_fielddefs.h"
#include "common/axis.h"
#include "common/maths.h"
#include "common/utils.h"
#include "config/feature.h"
#include "config/parameter_group.h"
#include "config/parameter_group_ids.h"
#include "drivers/system.h"
#include "fc/config.h"
#include "fc/controlrate_profile.h"
#include "fc/rc_adjustments.h"
#include "fc/rc_curves.h"
#include "flight/pid.h"
#include "io/beeper.h"
#include "rx/rx.h"
PG_REGISTER_ARR(adjustmentRange_t, MAX_ADJUSTMENT_RANGE_COUNT, adjustmentRanges, PG_ADJUSTMENT_RANGE_CONFIG, 0);
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,
.mode = ADJUSTMENT_MODE_STEP,
.data = { .stepConfig = { .step = 1 }}
},
{
.adjustmentFunction = ADJUSTMENT_RC_EXPO,
.mode = ADJUSTMENT_MODE_STEP,
.data = { .stepConfig = { .step = 1 }}
},
{
.adjustmentFunction = ADJUSTMENT_THROTTLE_EXPO,
.mode = ADJUSTMENT_MODE_STEP,
.data = { .stepConfig = { .step = 1 }}
},
{
.adjustmentFunction = ADJUSTMENT_PITCH_ROLL_RATE,
.mode = ADJUSTMENT_MODE_STEP,
.data = { .stepConfig = { .step = 1 }}
},
{
.adjustmentFunction = ADJUSTMENT_YAW_RATE,
.mode = ADJUSTMENT_MODE_STEP,
.data = { .stepConfig = { .step = 1 }}
},
{
.adjustmentFunction = ADJUSTMENT_PITCH_ROLL_P,
.mode = ADJUSTMENT_MODE_STEP,
.data = { .stepConfig = { .step = 1 }}
},
{
.adjustmentFunction = ADJUSTMENT_PITCH_ROLL_I,
.mode = ADJUSTMENT_MODE_STEP,
.data = { .stepConfig = { .step = 1 }}
},
{
.adjustmentFunction = ADJUSTMENT_PITCH_ROLL_D,
.mode = ADJUSTMENT_MODE_STEP,
.data = { .stepConfig = { .step = 1 }}
},
{
.adjustmentFunction = ADJUSTMENT_YAW_P,
.mode = ADJUSTMENT_MODE_STEP,
.data = { .stepConfig = { .step = 1 }}
},
{
.adjustmentFunction = ADJUSTMENT_YAW_I,
.mode = ADJUSTMENT_MODE_STEP,
.data = { .stepConfig = { .step = 1 }}
},
{
.adjustmentFunction = ADJUSTMENT_YAW_D,
.mode = ADJUSTMENT_MODE_STEP,
.data = { .stepConfig = { .step = 1 }}
},
{
.adjustmentFunction = ADJUSTMENT_RATE_PROFILE,
.mode = ADJUSTMENT_MODE_SELECT,
.data = { .selectConfig = { .switchPositions = 3 }}
},
{
.adjustmentFunction = ADJUSTMENT_PITCH_RATE,
.mode = ADJUSTMENT_MODE_STEP,
.data = { .stepConfig = { .step = 1 }}
},
{
.adjustmentFunction = ADJUSTMENT_ROLL_RATE,
.mode = ADJUSTMENT_MODE_STEP,
.data = { .stepConfig = { .step = 1 }}
},
{
.adjustmentFunction = ADJUSTMENT_PITCH_P,
.mode = ADJUSTMENT_MODE_STEP,
.data = { .stepConfig = { .step = 1 }}
},
{
.adjustmentFunction = ADJUSTMENT_PITCH_I,
.mode = ADJUSTMENT_MODE_STEP,
.data = { .stepConfig = { .step = 1 }}
},
{
.adjustmentFunction = ADJUSTMENT_PITCH_D,
.mode = ADJUSTMENT_MODE_STEP,
.data = { .stepConfig = { .step = 1 }}
},
{
.adjustmentFunction = ADJUSTMENT_ROLL_P,
.mode = ADJUSTMENT_MODE_STEP,
.data = { .stepConfig = { .step = 1 }}
},
{
.adjustmentFunction = ADJUSTMENT_ROLL_I,
.mode = ADJUSTMENT_MODE_STEP,
.data = { .stepConfig = { .step = 1 }}
},
{
.adjustmentFunction = ADJUSTMENT_ROLL_D,
.mode = ADJUSTMENT_MODE_STEP,
.data = { .stepConfig = { .step = 1 }}
}
};
#define ADJUSTMENT_FUNCTION_CONFIG_INDEX_OFFSET 1
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->config == adjustmentConfig) {
// already configured
return;
}
adjustmentState->auxChannelIndex = auxSwitchChannelIndex;
adjustmentState->config = adjustmentConfig;
adjustmentState->timeoutAt = 0;
MARK_ADJUSTMENT_FUNCTION_AS_READY(index);
}
static void blackboxLogInflightAdjustmentEvent(adjustmentFunction_e adjustmentFunction, int32_t newValue) {
#ifndef BLACKBOX
UNUSED(adjustmentFunction);
UNUSED(newValue);
#else
if (feature(FEATURE_BLACKBOX)) {
flightLogEvent_inflightAdjustment_t eventData;
eventData.adjustmentFunction = adjustmentFunction;
eventData.newValue = newValue;
eventData.floatFlag = false;
blackboxLogEvent(FLIGHT_LOG_EVENT_INFLIGHT_ADJUSTMENT, (flightLogEventData_t*)&eventData);
}
#endif
}
#if 0
static void blackboxLogInflightAdjustmentEventFloat(adjustmentFunction_e adjustmentFunction, float newFloatValue) {
#ifndef BLACKBOX
UNUSED(adjustmentFunction);
UNUSED(newFloatValue);
#else
if (feature(FEATURE_BLACKBOX)) {
flightLogEvent_inflightAdjustment_t eventData;
eventData.adjustmentFunction = adjustmentFunction;
eventData.newFloatValue = newFloatValue;
eventData.floatFlag = true;
blackboxLogEvent(FLIGHT_LOG_EVENT_INFLIGHT_ADJUSTMENT, (flightLogEventData_t*)&eventData);
}
#endif
}
#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_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);
blackboxLogInflightAdjustmentEvent(ADJUSTMENT_THROTTLE_EXPO, newValue);
break;
case ADJUSTMENT_PITCH_ROLL_RATE:
case ADJUSTMENT_PITCH_RATE:
newValue = constrain((int)controlRateConfig->rates[FD_PITCH] + delta, CONTROL_RATE_CONFIG_ROLL_PITCH_RATE_MIN, CONTROL_RATE_CONFIG_ROLL_PITCH_RATE_MAX);
controlRateConfig->rates[FD_PITCH] = newValue;
blackboxLogInflightAdjustmentEvent(ADJUSTMENT_PITCH_RATE, newValue);
if (adjustmentFunction == ADJUSTMENT_PITCH_RATE) {
schedulePidGainsUpdate();
break;
}
// follow though for combined ADJUSTMENT_PITCH_ROLL_RATE
case ADJUSTMENT_ROLL_RATE:
newValue = constrain((int)controlRateConfig->rates[FD_ROLL] + delta, CONTROL_RATE_CONFIG_ROLL_PITCH_RATE_MIN, CONTROL_RATE_CONFIG_ROLL_PITCH_RATE_MAX);
controlRateConfig->rates[FD_ROLL] = newValue;
blackboxLogInflightAdjustmentEvent(ADJUSTMENT_ROLL_RATE, newValue);
schedulePidGainsUpdate();
break;
case ADJUSTMENT_YAW_RATE:
newValue = constrain((int)controlRateConfig->rates[FD_YAW] + delta, CONTROL_RATE_CONFIG_YAW_RATE_MIN, CONTROL_RATE_CONFIG_YAW_RATE_MAX);
controlRateConfig->rates[FD_YAW] = newValue;
blackboxLogInflightAdjustmentEvent(ADJUSTMENT_YAW_RATE, newValue);
schedulePidGainsUpdate();
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
pidProfileMutable()->P8[PIDPITCH] = newValue;
blackboxLogInflightAdjustmentEvent(ADJUSTMENT_PITCH_P, newValue);
if (adjustmentFunction == ADJUSTMENT_PITCH_P) {
schedulePidGainsUpdate();
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
pidProfileMutable()->P8[PIDROLL] = newValue;
blackboxLogInflightAdjustmentEvent(ADJUSTMENT_ROLL_P, newValue);
schedulePidGainsUpdate();
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
pidProfileMutable()->I8[PIDPITCH] = newValue;
blackboxLogInflightAdjustmentEvent(ADJUSTMENT_PITCH_I, newValue);
if (adjustmentFunction == ADJUSTMENT_PITCH_I) {
schedulePidGainsUpdate();
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
pidProfileMutable()->I8[PIDROLL] = newValue;
blackboxLogInflightAdjustmentEvent(ADJUSTMENT_ROLL_I, newValue);
schedulePidGainsUpdate();
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
pidProfileMutable()->D8[PIDPITCH] = newValue;
blackboxLogInflightAdjustmentEvent(ADJUSTMENT_PITCH_D, newValue);
if (adjustmentFunction == ADJUSTMENT_PITCH_D) {
schedulePidGainsUpdate();
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
pidProfileMutable()->D8[PIDROLL] = newValue;
blackboxLogInflightAdjustmentEvent(ADJUSTMENT_ROLL_D, newValue);
schedulePidGainsUpdate();
break;
case ADJUSTMENT_YAW_P:
newValue = constrain((int)pidProfile()->P8[PIDYAW] + delta, 0, 200); // FIXME magic numbers repeated in serial_cli.c
pidProfileMutable()->P8[PIDYAW] = newValue;
blackboxLogInflightAdjustmentEvent(ADJUSTMENT_YAW_P, newValue);
schedulePidGainsUpdate();
break;
case ADJUSTMENT_YAW_I:
newValue = constrain((int)pidProfile()->I8[PIDYAW] + delta, 0, 200); // FIXME magic numbers repeated in serial_cli.c
pidProfileMutable()->I8[PIDYAW] = newValue;
blackboxLogInflightAdjustmentEvent(ADJUSTMENT_YAW_I, newValue);
schedulePidGainsUpdate();
break;
case ADJUSTMENT_YAW_D:
newValue = constrain((int)pidProfile()->D8[PIDYAW] + delta, 0, 200); // FIXME magic numbers repeated in serial_cli.c
pidProfileMutable()->D8[PIDYAW] = newValue;
blackboxLogInflightAdjustmentEvent(ADJUSTMENT_YAW_D, newValue);
schedulePidGainsUpdate();
break;
default:
break;
};
}
void applySelectAdjustment(uint8_t adjustmentFunction, uint8_t position)
{
bool applied = false;
switch(adjustmentFunction) {
case ADJUSTMENT_RATE_PROFILE:
if (getCurrentControlRateProfile() != position) {
changeControlRateProfile(position);
blackboxLogInflightAdjustmentEvent(ADJUSTMENT_RATE_PROFILE, position);
applied = true;
}
break;
}
if (applied) {
beeperConfirmationBeeps(position + 1);
}
}
#define RESET_FREQUENCY_2HZ (1000 / 2)
void processRcAdjustments(const controlRateConfig_t *controlRateConfig)
{
uint8_t adjustmentIndex;
uint32_t now = millis();
bool canUseRxData = rxIsReceivingSignal();
for (adjustmentIndex = 0; adjustmentIndex < MAX_SIMULTANEOUS_ADJUSTMENT_COUNT; adjustmentIndex++) {
adjustmentState_t *adjustmentState = &adjustmentStates[adjustmentIndex];
if (!adjustmentState->config) {
continue;
}
uint8_t adjustmentFunction = adjustmentState->config->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);
}
if (!canUseRxData) {
continue;
}
uint8_t channelIndex = NON_AUX_CHANNEL_COUNT + adjustmentState->auxChannelIndex;
if (adjustmentState->config->mode == ADJUSTMENT_MODE_STEP) {
int delta;
if (rcData[channelIndex] > rxConfig()->midrc + 200) {
delta = adjustmentState->config->data.stepConfig.step;
} else if (rcData[channelIndex] < rxConfig()->midrc - 200) {
delta = 0 - adjustmentState->config->data.stepConfig.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;
}
// it is legitimate to adjust an otherwise const item here
applyStepAdjustment((controlRateConfig_t*)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);
}
}
void resetAdjustmentStates(void)
{
memset(adjustmentStates, 0, sizeof(adjustmentStates));
}
void updateAdjustmentStates(void)
{
for (int index = 0; index < MAX_ADJUSTMENT_RANGE_COUNT; index++) {
const 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);
}
}
}

108
src/main/fc/rc_adjustments.h Normal file
View file

@ -0,0 +1,108 @@
/*
* This file is part of Cleanflight.
*
* Cleanflight is free software: you can redistribute it and/or modify
* it under the terms of the GNU General Public License as published by
* the Free Software Foundation, either version 3 of the License, or
* (at your option) any later version.
*
* Cleanflight is distributed in the hope that it will be useful,
* but WITHOUT ANY WARRANTY; without even the implied warranty of
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
* GNU General Public License for more details.
*
* You should have received a copy of the GNU General Public License
* along with Cleanflight. If not, see <http://www.gnu.org/licenses/>.
*/
#pragma once
#include "config/parameter_group.h"
#include "fc/rc_controls.h"
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,
ADJUSTMENT_RATE_PROFILE,
ADJUSTMENT_PITCH_RATE,
ADJUSTMENT_ROLL_RATE,
ADJUSTMENT_PITCH_P,
ADJUSTMENT_PITCH_I,
ADJUSTMENT_PITCH_D,
ADJUSTMENT_ROLL_P,
ADJUSTMENT_ROLL_I,
ADJUSTMENT_ROLL_D,
} adjustmentFunction_e;
#define ADJUSTMENT_FUNCTION_COUNT 21
typedef enum {
ADJUSTMENT_MODE_STEP,
ADJUSTMENT_MODE_SELECT
} adjustmentMode_e;
typedef struct adjustmentStepConfig_s {
uint8_t step;
} adjustmentStepConfig_t;
typedef struct adjustmentSelectConfig_s {
uint8_t switchPositions;
} adjustmentSelectConfig_t;
typedef union adjustmentConfig_u {
adjustmentStepConfig_t stepConfig;
adjustmentSelectConfig_t selectConfig;
} adjustmentData_t;
typedef struct adjustmentConfig_s {
uint8_t adjustmentFunction;
uint8_t mode;
adjustmentData_t data;
} 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
typedef struct adjustmentState_s {
uint8_t auxChannelIndex;
const adjustmentConfig_t *config;
uint32_t timeoutAt;
} adjustmentState_t;
#ifndef MAX_SIMULTANEOUS_ADJUSTMENT_COUNT
#define MAX_SIMULTANEOUS_ADJUSTMENT_COUNT 4 // enough for 4 x 3position switches / 4 aux channel
#endif
#define MAX_ADJUSTMENT_RANGE_COUNT 12 // enough for 2 * 6pos switches.
PG_DECLARE_ARR(adjustmentRange_t, MAX_ADJUSTMENT_RANGE_COUNT, adjustmentRanges);
void resetAdjustmentStates(void);
void configureAdjustment(uint8_t index, uint8_t auxChannelIndex, const adjustmentConfig_t *adjustmentConfig);
void updateAdjustmentStates(void);
struct controlRateConfig_s;
void processRcAdjustments(const struct controlRateConfig_s *controlRateConfig);

381
src/main/fc/rc_controls.c
View file

@ -90,8 +90,6 @@ PG_RESET_TEMPLATE(armingConfig_t, armingConfig,
PG_REGISTER_ARR(modeActivationCondition_t, MAX_MODE_ACTIVATION_CONDITION_COUNT, modeActivationConditions, PG_MODE_ACTIVATION_PROFILE, 0);
PG_REGISTER(modeActivationOperatorConfig_t, modeActivationOperatorConfig, PG_MODE_ACTIVATION_OPERATOR_CONFIG, 0);
PG_REGISTER_ARR(adjustmentRange_t, MAX_ADJUSTMENT_RANGE_COUNT, adjustmentRanges, PG_ADJUSTMENT_RANGE_CONFIG, 0);
bool isUsingSticksForArming(void)
{
return isUsingSticksToArm;
@ -299,7 +297,7 @@ bool isModeActivationConditionPresent(boxId_e modeId)
return false;
}
static bool isRangeActive(uint8_t auxChannelIndex, const channelRange_t *range) {
bool isRangeActive(uint8_t auxChannelIndex, const channelRange_t *range) {
if (!IS_RANGE_USABLE(range)) {
return false;
}
@ -357,378 +355,6 @@ void updateActivatedModes(void)
}
}
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,
.mode = ADJUSTMENT_MODE_STEP,
.data = { .stepConfig = { .step = 1 }}
},
{
.adjustmentFunction = ADJUSTMENT_RC_EXPO,
.mode = ADJUSTMENT_MODE_STEP,
.data = { .stepConfig = { .step = 1 }}
},
{
.adjustmentFunction = ADJUSTMENT_THROTTLE_EXPO,
.mode = ADJUSTMENT_MODE_STEP,
.data = { .stepConfig = { .step = 1 }}
},
{
.adjustmentFunction = ADJUSTMENT_PITCH_ROLL_RATE,
.mode = ADJUSTMENT_MODE_STEP,
.data = { .stepConfig = { .step = 1 }}
},
{
.adjustmentFunction = ADJUSTMENT_YAW_RATE,
.mode = ADJUSTMENT_MODE_STEP,
.data = { .stepConfig = { .step = 1 }}
},
{
.adjustmentFunction = ADJUSTMENT_PITCH_ROLL_P,
.mode = ADJUSTMENT_MODE_STEP,
.data = { .stepConfig = { .step = 1 }}
},
{
.adjustmentFunction = ADJUSTMENT_PITCH_ROLL_I,
.mode = ADJUSTMENT_MODE_STEP,
.data = { .stepConfig = { .step = 1 }}
},
{
.adjustmentFunction = ADJUSTMENT_PITCH_ROLL_D,
.mode = ADJUSTMENT_MODE_STEP,
.data = { .stepConfig = { .step = 1 }}
},
{
.adjustmentFunction = ADJUSTMENT_YAW_P,
.mode = ADJUSTMENT_MODE_STEP,
.data = { .stepConfig = { .step = 1 }}
},
{
.adjustmentFunction = ADJUSTMENT_YAW_I,
.mode = ADJUSTMENT_MODE_STEP,
.data = { .stepConfig = { .step = 1 }}
},
{
.adjustmentFunction = ADJUSTMENT_YAW_D,
.mode = ADJUSTMENT_MODE_STEP,
.data = { .stepConfig = { .step = 1 }}
},
{
.adjustmentFunction = ADJUSTMENT_RATE_PROFILE,
.mode = ADJUSTMENT_MODE_SELECT,
.data = { .selectConfig = { .switchPositions = 3 }}
},
{
.adjustmentFunction = ADJUSTMENT_PITCH_RATE,
.mode = ADJUSTMENT_MODE_STEP,
.data = { .stepConfig = { .step = 1 }}
},
{
.adjustmentFunction = ADJUSTMENT_ROLL_RATE,
.mode = ADJUSTMENT_MODE_STEP,
.data = { .stepConfig = { .step = 1 }}
},
{
.adjustmentFunction = ADJUSTMENT_PITCH_P,
.mode = ADJUSTMENT_MODE_STEP,
.data = { .stepConfig = { .step = 1 }}
},
{
.adjustmentFunction = ADJUSTMENT_PITCH_I,
.mode = ADJUSTMENT_MODE_STEP,
.data = { .stepConfig = { .step = 1 }}
},
{
.adjustmentFunction = ADJUSTMENT_PITCH_D,
.mode = ADJUSTMENT_MODE_STEP,
.data = { .stepConfig = { .step = 1 }}
},
{
.adjustmentFunction = ADJUSTMENT_ROLL_P,
.mode = ADJUSTMENT_MODE_STEP,
.data = { .stepConfig = { .step = 1 }}
},
{
.adjustmentFunction = ADJUSTMENT_ROLL_I,
.mode = ADJUSTMENT_MODE_STEP,
.data = { .stepConfig = { .step = 1 }}
},
{
.adjustmentFunction = ADJUSTMENT_ROLL_D,
.mode = ADJUSTMENT_MODE_STEP,
.data = { .stepConfig = { .step = 1 }}
}
};
#define ADJUSTMENT_FUNCTION_CONFIG_INDEX_OFFSET 1
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->config == adjustmentConfig) {
// already configured
return;
}
adjustmentState->auxChannelIndex = auxSwitchChannelIndex;
adjustmentState->config = adjustmentConfig;
adjustmentState->timeoutAt = 0;
MARK_ADJUSTMENT_FUNCTION_AS_READY(index);
}
static void blackboxLogInflightAdjustmentEvent(adjustmentFunction_e adjustmentFunction, int32_t newValue) {
#ifndef BLACKBOX
UNUSED(adjustmentFunction);
UNUSED(newValue);
#else
if (feature(FEATURE_BLACKBOX)) {
flightLogEvent_inflightAdjustment_t eventData;
eventData.adjustmentFunction = adjustmentFunction;
eventData.newValue = newValue;
eventData.floatFlag = false;
blackboxLogEvent(FLIGHT_LOG_EVENT_INFLIGHT_ADJUSTMENT, (flightLogEventData_t*)&eventData);
}
#endif
}
#if 0
static void blackboxLogInflightAdjustmentEventFloat(adjustmentFunction_e adjustmentFunction, float newFloatValue) {
#ifndef BLACKBOX
UNUSED(adjustmentFunction);
UNUSED(newFloatValue);
#else
if (feature(FEATURE_BLACKBOX)) {
flightLogEvent_inflightAdjustment_t eventData;
eventData.adjustmentFunction = adjustmentFunction;
eventData.newFloatValue = newFloatValue;
eventData.floatFlag = true;
blackboxLogEvent(FLIGHT_LOG_EVENT_INFLIGHT_ADJUSTMENT, (flightLogEventData_t*)&eventData);
}
#endif
}
#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_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);
blackboxLogInflightAdjustmentEvent(ADJUSTMENT_THROTTLE_EXPO, newValue);
break;
case ADJUSTMENT_PITCH_ROLL_RATE:
case ADJUSTMENT_PITCH_RATE:
newValue = constrain((int)controlRateConfig->rates[FD_PITCH] + delta, CONTROL_RATE_CONFIG_ROLL_PITCH_RATE_MIN, CONTROL_RATE_CONFIG_ROLL_PITCH_RATE_MAX);
controlRateConfig->rates[FD_PITCH] = newValue;
blackboxLogInflightAdjustmentEvent(ADJUSTMENT_PITCH_RATE, newValue);
if (adjustmentFunction == ADJUSTMENT_PITCH_RATE) {
schedulePidGainsUpdate();
break;
}
// follow though for combined ADJUSTMENT_PITCH_ROLL_RATE
case ADJUSTMENT_ROLL_RATE:
newValue = constrain((int)controlRateConfig->rates[FD_ROLL] + delta, CONTROL_RATE_CONFIG_ROLL_PITCH_RATE_MIN, CONTROL_RATE_CONFIG_ROLL_PITCH_RATE_MAX);
controlRateConfig->rates[FD_ROLL] = newValue;
blackboxLogInflightAdjustmentEvent(ADJUSTMENT_ROLL_RATE, newValue);
schedulePidGainsUpdate();
break;
case ADJUSTMENT_YAW_RATE:
newValue = constrain((int)controlRateConfig->rates[FD_YAW] + delta, CONTROL_RATE_CONFIG_YAW_RATE_MIN, CONTROL_RATE_CONFIG_YAW_RATE_MAX);
controlRateConfig->rates[FD_YAW] = newValue;
blackboxLogInflightAdjustmentEvent(ADJUSTMENT_YAW_RATE, newValue);
schedulePidGainsUpdate();
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
pidProfileMutable()->P8[PIDPITCH] = newValue;
blackboxLogInflightAdjustmentEvent(ADJUSTMENT_PITCH_P, newValue);
if (adjustmentFunction == ADJUSTMENT_PITCH_P) {
schedulePidGainsUpdate();
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
pidProfileMutable()->P8[PIDROLL] = newValue;
blackboxLogInflightAdjustmentEvent(ADJUSTMENT_ROLL_P, newValue);
schedulePidGainsUpdate();
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
pidProfileMutable()->I8[PIDPITCH] = newValue;
blackboxLogInflightAdjustmentEvent(ADJUSTMENT_PITCH_I, newValue);
if (adjustmentFunction == ADJUSTMENT_PITCH_I) {
schedulePidGainsUpdate();
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
pidProfileMutable()->I8[PIDROLL] = newValue;
blackboxLogInflightAdjustmentEvent(ADJUSTMENT_ROLL_I, newValue);
schedulePidGainsUpdate();
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
pidProfileMutable()->D8[PIDPITCH] = newValue;
blackboxLogInflightAdjustmentEvent(ADJUSTMENT_PITCH_D, newValue);
if (adjustmentFunction == ADJUSTMENT_PITCH_D) {
schedulePidGainsUpdate();
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
pidProfileMutable()->D8[PIDROLL] = newValue;
blackboxLogInflightAdjustmentEvent(ADJUSTMENT_ROLL_D, newValue);
schedulePidGainsUpdate();
break;
case ADJUSTMENT_YAW_P:
newValue = constrain((int)pidProfile()->P8[PIDYAW] + delta, 0, 200); // FIXME magic numbers repeated in serial_cli.c
pidProfileMutable()->P8[PIDYAW] = newValue;
blackboxLogInflightAdjustmentEvent(ADJUSTMENT_YAW_P, newValue);
schedulePidGainsUpdate();
break;
case ADJUSTMENT_YAW_I:
newValue = constrain((int)pidProfile()->I8[PIDYAW] + delta, 0, 200); // FIXME magic numbers repeated in serial_cli.c
pidProfileMutable()->I8[PIDYAW] = newValue;
blackboxLogInflightAdjustmentEvent(ADJUSTMENT_YAW_I, newValue);
schedulePidGainsUpdate();
break;
case ADJUSTMENT_YAW_D:
newValue = constrain((int)pidProfile()->D8[PIDYAW] + delta, 0, 200); // FIXME magic numbers repeated in serial_cli.c
pidProfileMutable()->D8[PIDYAW] = newValue;
blackboxLogInflightAdjustmentEvent(ADJUSTMENT_YAW_D, newValue);
schedulePidGainsUpdate();
break;
default:
break;
};
}
void changeControlRateProfile(uint8_t profileIndex);
void applySelectAdjustment(uint8_t adjustmentFunction, uint8_t position)
{
bool applied = false;
switch(adjustmentFunction) {
case ADJUSTMENT_RATE_PROFILE:
if (getCurrentControlRateProfile() != position) {
changeControlRateProfile(position);
blackboxLogInflightAdjustmentEvent(ADJUSTMENT_RATE_PROFILE, position);
applied = true;
}
break;
}
if (applied) {
beeperConfirmationBeeps(position + 1);
}
}
#define RESET_FREQUENCY_2HZ (1000 / 2)
void processRcAdjustments(const controlRateConfig_t *controlRateConfig)
{
uint8_t adjustmentIndex;
uint32_t now = millis();
bool canUseRxData = rxIsReceivingSignal();
for (adjustmentIndex = 0; adjustmentIndex < MAX_SIMULTANEOUS_ADJUSTMENT_COUNT; adjustmentIndex++) {
adjustmentState_t *adjustmentState = &adjustmentStates[adjustmentIndex];
if (!adjustmentState->config) {
continue;
}
uint8_t adjustmentFunction = adjustmentState->config->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);
}
if (!canUseRxData) {
continue;
}
uint8_t channelIndex = NON_AUX_CHANNEL_COUNT + adjustmentState->auxChannelIndex;
if (adjustmentState->config->mode == ADJUSTMENT_MODE_STEP) {
int delta;
if (rcData[channelIndex] > rxConfig()->midrc + 200) {
delta = adjustmentState->config->data.stepConfig.step;
} else if (rcData[channelIndex] < rxConfig()->midrc - 200) {
delta = 0 - adjustmentState->config->data.stepConfig.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;
}
// it is legitimate to adjust an otherwise const item here
applyStepAdjustment((controlRateConfig_t*)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);
}
}
void updateAdjustmentStates(void)
{
for (int index = 0; index < MAX_ADJUSTMENT_RANGE_COUNT; index++) {
const 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);
}
}
}
int32_t getRcStickDeflection(int32_t axis, uint16_t midrc) {
return MIN(ABS(rcData[axis] - midrc), 500);
}
@ -743,8 +369,3 @@ void useRcControlsConfig(void)
isModeActivationConditionPresent(BOXNAVWP);
#endif
}
void resetAdjustmentStates(void)
{
memset(adjustmentStates, 0, sizeof(adjustmentStates));
}

88
src/main/fc/rc_controls.h
View file

@ -162,95 +162,9 @@ throttleStatus_e calculateThrottleStatus(uint16_t deadband3d_throttle);
rollPitchStatus_e calculateRollPitchCenterStatus(void);
void processRcStickPositions(throttleStatus_e throttleStatus, bool disarm_kill_switch, bool fixed_wing_auto_arm);
bool isRangeActive(uint8_t auxChannelIndex, const channelRange_t *range);
void updateActivatedModes(void);
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,
ADJUSTMENT_RATE_PROFILE,
ADJUSTMENT_PITCH_RATE,
ADJUSTMENT_ROLL_RATE,
ADJUSTMENT_PITCH_P,
ADJUSTMENT_PITCH_I,
ADJUSTMENT_PITCH_D,
ADJUSTMENT_ROLL_P,
ADJUSTMENT_ROLL_I,
ADJUSTMENT_ROLL_D,
} adjustmentFunction_e;
#define ADJUSTMENT_FUNCTION_COUNT 21
typedef enum {
ADJUSTMENT_MODE_STEP,
ADJUSTMENT_MODE_SELECT
} adjustmentMode_e;
typedef struct adjustmentStepConfig_s {
uint8_t step;
} adjustmentStepConfig_t;
typedef struct adjustmentSelectConfig_s {
uint8_t switchPositions;
} adjustmentSelectConfig_t;
typedef union adjustmentConfig_u {
adjustmentStepConfig_t stepConfig;
adjustmentSelectConfig_t selectConfig;
} adjustmentData_t;
typedef struct adjustmentConfig_s {
uint8_t adjustmentFunction;
uint8_t mode;
adjustmentData_t data;
} 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
typedef struct adjustmentState_s {
uint8_t auxChannelIndex;
const adjustmentConfig_t *config;
uint32_t timeoutAt;
} adjustmentState_t;
#ifndef MAX_SIMULTANEOUS_ADJUSTMENT_COUNT
#define MAX_SIMULTANEOUS_ADJUSTMENT_COUNT 4 // enough for 4 x 3position switches / 4 aux channel
#endif
#define MAX_ADJUSTMENT_RANGE_COUNT 12 // enough for 2 * 6pos switches.
PG_DECLARE_ARR(adjustmentRange_t, MAX_ADJUSTMENT_RANGE_COUNT, adjustmentRanges);
void resetAdjustmentStates(void);
void configureAdjustment(uint8_t index, uint8_t auxChannelIndex, const adjustmentConfig_t *adjustmentConfig);
void updateAdjustmentStates(void);
struct controlRateConfig_s;
void processRcAdjustments(const struct controlRateConfig_s *controlRateConfig);
bool isUsingSticksForArming(void);
bool isUsingNavigationModes(void);

1
src/main/fc/serial_cli.c
View file

@ -69,6 +69,7 @@ extern uint8_t __config_end;
#include "fc/config.h"
#include "fc/controlrate_profile.h"
#include "fc/rc_adjustments.h"
#include "fc/rc_controls.h"
#include "fc/runtime_config.h"
#include "fc/serial_cli.h"

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

@ -27,6 +27,8 @@
#include "drivers/display.h"
#include "drivers/max7456.h"
#include "io/osd.h"
displayPort_t max7456DisplayPort; // Referenced from osd.c
extern uint16_t refreshTimeout;