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

Merge branch 'inav-rates-in-dps' of https://github.com/DzikuVx/cleanflight into DzikuVx-inav-rates-in-dps

Browse source
This commit is contained in:
Konstantin (DigitalEntity) Sharlaimov 2016-06-15 20:11:29 +03:00
commit f77eb4788d
15 changed files with 124 additions and 105 deletions
Download patch file Download diff file Expand all files Collapse all files

6
docs/Cli.md
View file

@ -187,13 +187,13 @@ Re-apply any new defaults as desired.
| `yaw_motor_direction` | | -1 | 1 | 1 | Profile | INT8 |
| `tri_unarmed_servo` | On tricopter mix only, if this is set to 1, servo will always be correcting regardless of armed state. to disable this, set it to 0. | OFF | ON | ON | Profile | INT8 |
| `default_rate_profile` | Default = profile number | 0 | 2 | | Profile | UINT8 |
| `rc_rate` | | 0 | 250 | 90 | Rate Profile | UINT8 |
| `rc_expo` | | 0 | 100 | 65 | Rate Profile | UINT8 |
| `rc_yaw_expo` | | 0 | 100 | 0 | Rate Profile | UINT8 |
| `thr_mid` | | 0 | 100 | 50 | Rate Profile | UINT8 |
| `thr_expo` | | 0 | 100 | 0 | Rate Profile | UINT8 |
| `roll_pitch_rate` | | 0 | 100 | 0 | Rate Profile | UINT8 |
| `yaw_rate` | | 0 | 100 | 0 | Rate Profile | UINT8 |
| `roll_rate` | Defines rotation rate on ROLL axis that UAV will try to archive on max. stick deflection. Rates are defined in tenths of degrees per second [dps/10]. That means, rate 20 represents 200dps rotation speed. Default 20 (200dps) is more less equivalent of old Cleanflight/Baseflight rate 0. Max. 180 (1800dps) is what gyro can measure.
| `pitch_rate` | Defines rotation rate on PITCH axis that UAV will try to archive on max. stick deflection. Rates are defined in tenths of degrees per second [dps/10]. That means, rate 20 represents 200dps rotation speed. Default 20 (200dps) is more less equivalent of old Cleanflight/Baseflight rate 0. Max. 180 (1800dps) is what gyro can measure. | 6 | 180 | 20 | Rate Profile | UINT8 |
| `yaw_rate` | Defines rotation rate on YAW axis that UAV will try to archive on max. stick deflection. Rates are defined in tenths of degrees per second [dps/10]. That means, rate 20 represents 200dps rotation speed. Default 20 (200dps) is more less equivalent of old Cleanflight/Baseflight rate 0. Max. 180 (1800dps) is what gyro can measure. | 2 | 180 | 20 | Rate Profile | UINT8 |
| `tpa_rate` | Throttle PID attenuation reduces influence of P on ROLL and PITCH as throttle increases. For every 1% throttle after the TPA breakpoint, P is reduced by the TPA rate. | 0 | 100 | 0 | Rate Profile | UINT8 |
| `tpa_breakpoint` | See tpa_rate. | 1000 | 2000 | 1500 | Rate Profile | UINT16 |
| `failsafe_delay` | Time in deciseconds to wait before activating failsafe when signal is lost. See [Failsafe documentation](Failsafe.md#failsafe_delay). | 0 | 200 | 10 | Profile | UINT8 |

3
docs/Profiles.md
View file

@ -31,7 +31,7 @@ Three rate profiles are supported.
Rate profiles can be selected while flying using the inflight adjustments feature.
Each normal profile has a setting called 'default_rate_profile`. When a profile is activated the
corresponding rate profile is also activated.
corresponding rate profile is also activated.
Profile 0 has a default rate profile of 0.
Profile 1 has a default rate profile of 1.
@ -54,7 +54,6 @@ e.g
# rateprofile
rateprofile 0
set rc_rate = 90
set rc_expo = 65
set thr_mid = 50
set thr_expo = 0

12
src/main/blackbox/blackbox.c
View file

@ -685,7 +685,7 @@ static void writeInterframe(void)
arraySubInt32(deltas, blackboxCurrent->axisPID_P, blackboxLast->axisPID_P, XYZ_AXIS_COUNT);
blackboxWriteSignedVBArray(deltas, XYZ_AXIS_COUNT);
/*
/*
* The PID I field changes very slowly, most of the time +-2, so use an encoding
* that can pack all three fields into one byte in that situation.
*/
@ -920,7 +920,7 @@ void startBlackbox(void)
* cache those now.
*/
blackboxBuildConditionCache();
blackboxModeActivationConditionPresent = isModeActivationConditionPresent(currentProfile->modeActivationConditions, BOXBLACKBOX);
blackboxIteration = 0;
@ -1228,7 +1228,7 @@ static bool blackboxWriteSysinfo()
blackboxPrintfHeaderLine("P interval:%d/%d", masterConfig.blackbox_rate_num, masterConfig.blackbox_rate_denom);
break;
case 4:
blackboxPrintfHeaderLine("rcRate:%d", masterConfig.controlRateProfiles[masterConfig.current_profile_index].rcRate8);
blackboxPrintfHeaderLine("rcRate:%d", 100); //For compatibility reasons write rc_rate 100
break;
case 5:
blackboxPrintfHeaderLine("minthrottle:%d", masterConfig.escAndServoConfig.minthrottle);
@ -1323,7 +1323,7 @@ static void blackboxCheckAndLogArmingBeep()
}
}
/*
/*
* Use the user's num/denom settings to decide if the P-frame of the given index should be logged, allowing the user to control
* the portion of logged loop iterations.
*/
@ -1367,7 +1367,7 @@ static void blackboxLogIteration()
writeIntraframe();
} else {
blackboxCheckAndLogArmingBeep();
if (blackboxShouldLogPFrame(blackboxPFrameIndex)) {
/*
* We assume that slow frames are only interesting in that they aid the interpretation of the main data stream.
@ -1499,7 +1499,7 @@ void handleBlackbox(void)
blackboxLogEvent(FLIGHT_LOG_EVENT_LOGGING_RESUME, (flightLogEventData_t *) &resume);
blackboxSetState(BLACKBOX_STATE_RUNNING);
blackboxLogIteration();
}

6
src/main/common/maths.c
View file

@ -187,6 +187,12 @@ int scaleRange(int x, int srcMin, int srcMax, int destMin, int destMax) {
return ((a / b) - (destMax - destMin)) + destMax;
}
int scaleRangef(float x, float srcMin, float srcMax, float destMin, float destMax) {
float a = (destMax - destMin) * (x - srcMin);
float b = srcMax - srcMin;
return ((a / b) - (destMax - destMin)) + destMax;
}
// Normalize a vector
void normalizeV(struct fp_vector *src, struct fp_vector *dest)
{

1
src/main/common/maths.h
View file

@ -139,6 +139,7 @@ float devStandardDeviation(stdev_t *dev);
float degreesToRadians(int16_t degrees);
int scaleRange(int x, int srcMin, int srcMax, int destMin, int destMax);
int scaleRangef(float x, float srcMin, float srcMax, float destMin, float destMax);
void normalizeV(struct fp_vector *src, struct fp_vector *dest);

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

@ -353,7 +353,6 @@ void resetSerialConfig(serialConfig_t *serialConfig)
}
static void resetControlRateConfig(controlRateConfig_t *controlRateConfig) {
controlRateConfig->rcRate8 = 90;
controlRateConfig->rcExpo8 = 70;
controlRateConfig->thrMid8 = 50;
controlRateConfig->thrExpo8 = 0;
@ -362,7 +361,11 @@ static void resetControlRateConfig(controlRateConfig_t *controlRateConfig) {
controlRateConfig->tpa_breakpoint = 1500;
for (uint8_t axis = 0; axis < FLIGHT_DYNAMICS_INDEX_COUNT; axis++) {
controlRateConfig->rates[axis] = 0;
if (axis == FD_YAW) {
controlRateConfig->rates[axis] = CONTROL_RATE_CONFIG_YAW_RATE_DEFAULT;
} else {
controlRateConfig->rates[axis] = CONTROL_RATE_CONFIG_ROLL_PITCH_RATE_DEFAULT;
}
}
}
@ -624,10 +627,9 @@ static void resetConf(void)
currentProfile->pidProfile.P8[PITCH] = 36;
masterConfig.failsafeConfig.failsafe_delay = 2;
masterConfig.failsafeConfig.failsafe_off_delay = 0;
currentControlRateProfile->rcRate8 = 130;
currentControlRateProfile->rates[FD_PITCH] = 20;
currentControlRateProfile->rates[FD_ROLL] = 20;
currentControlRateProfile->rates[FD_YAW] = 100;
currentControlRateProfile->rates[FD_PITCH] = CONTROL_RATE_CONFIG_ROLL_PITCH_RATE_DEFAULT;
currentControlRateProfile->rates[FD_ROLL] = CONTROL_RATE_CONFIG_ROLL_PITCH_RATE_DEFAULT;
currentControlRateProfile->rates[FD_YAW] = CONTROL_RATE_CONFIG_YAW_RATE_DEFAULT;
parseRcChannels("TAER1234", &masterConfig.rxConfig);
// { 1.0f, -0.414178f, 1.0f, -1.0f }, // REAR_R
@ -728,8 +730,7 @@ static bool isEEPROMContentValid(void)
void activateControlRateConfig(void)
{
generatePitchRollCurve(currentControlRateProfile);
generateYawCurve(currentControlRateProfile);
generateRcCurves(currentControlRateProfile);
generateThrottleCurve(currentControlRateProfile, &masterConfig.escAndServoConfig);
}

8
src/main/flight/pid.c
View file

@ -115,10 +115,14 @@ int16_t pidAngleToRcCommand(float angleDeciDegrees)
return angleDeciDegrees / 2.0f;
}
/*
Map stick positions to desired rotatrion rate in given axis.
Rotation rate in dps at full stick deflection is defined by axis rate measured in dps/10
Rate 20 means 200dps at full stick deflection
*/
float pidRcCommandToRate(int16_t stick, uint8_t rate)
{
// Map stick position from 200dps to 1200dps
return (float)((rate + 20) * stick) / 50.0f;
return scaleRangef((float) stick, (float) -500, (float) 500, (float) -rate, (float) rate) * 10;
}
#define FP_PID_RATE_P_MULTIPLIER 40.0f // betaflight - 40.0

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

@ -284,12 +284,12 @@ bool isModeActivationConditionPresent(modeActivationCondition_t *modeActivationC
for (index = 0; index < MAX_MODE_ACTIVATION_CONDITION_COUNT; index++) {
modeActivationCondition_t *modeActivationCondition = &modeActivationConditions[index];
if (modeActivationCondition->modeId == modeId && IS_RANGE_USABLE(&modeActivationCondition->range)) {
return true;
}
}
return false;
}
@ -456,16 +456,10 @@ void applyStepAdjustment(controlRateConfig_t *controlRateConfig, uint8_t adjustm
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;
generatePitchRollCurve(controlRateConfig);
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;
generatePitchRollCurve(controlRateConfig);
generateRcCurves(controlRateConfig);
blackboxLogInflightAdjustmentEvent(ADJUSTMENT_RC_EXPO, newValue);
break;
case ADJUSTMENT_THROTTLE_EXPO:
@ -476,7 +470,7 @@ void applyStepAdjustment(controlRateConfig_t *controlRateConfig, uint8_t adjustm
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);
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) {
@ -484,12 +478,12 @@ void applyStepAdjustment(controlRateConfig_t *controlRateConfig, uint8_t adjustm
}
// 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);
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);
break;
case ADJUSTMENT_YAW_RATE:
newValue = constrain((int)controlRateConfig->rates[FD_YAW] + delta, 0, CONTROL_RATE_CONFIG_YAW_RATE_MAX);
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);
break;
@ -676,4 +670,3 @@ void resetAdjustmentStates(void)
{
memset(adjustmentStates, 0, sizeof(adjustmentStates));
}

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

@ -108,11 +108,21 @@ typedef enum {
#define MIN_MODE_RANGE_STEP 0
#define MAX_MODE_RANGE_STEP ((CHANNEL_RANGE_MAX - CHANNEL_RANGE_MIN) / 25)
// Roll/pitch rates are a proportion used for mixing, so it tops out at 1.0:
#define CONTROL_RATE_CONFIG_ROLL_PITCH_RATE_MAX 100
/*
Max and min available values for rates are now stored as absolute
tenths of degrees-per-second [dsp/10]
That means, max. rotation rate 180 equals 1800dps
/* Meaningful yaw rates are effectively unbounded because they are treated as a rotation rate multiplier: */
#define CONTROL_RATE_CONFIG_YAW_RATE_MAX 255
New defaults of 200dps for pitch,roll and yaw are more less
equivalent of rates 0 from previous versions of iNav, Cleanflight, Baseflight
and so on.
*/
#define CONTROL_RATE_CONFIG_ROLL_PITCH_RATE_MAX 180
#define CONTROL_RATE_CONFIG_ROLL_PITCH_RATE_MIN 6
#define CONTROL_RATE_CONFIG_ROLL_PITCH_RATE_DEFAULT 20
#define CONTROL_RATE_CONFIG_YAW_RATE_MAX 180
#define CONTROL_RATE_CONFIG_YAW_RATE_MIN 2
#define CONTROL_RATE_CONFIG_YAW_RATE_DEFAULT 20
#define CONTROL_RATE_CONFIG_TPA_MAX 100
@ -134,7 +144,6 @@ typedef struct modeActivationCondition_s {
#define IS_RANGE_USABLE(range) ((range)->startStep < (range)->endStep)
typedef struct controlRateConfig_s {
uint8_t rcRate8;
uint8_t rcExpo8;
uint8_t thrMid8;
uint8_t thrExpo8;

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

@ -33,17 +33,21 @@ static int16_t lookupYawRC[YAW_LOOKUP_LENGTH]; // lookup table for
static int16_t lookupThrottleRC[THROTTLE_LOOKUP_LENGTH]; // lookup table for expo & mid THROTTLE
int16_t lookupThrottleRCMid; // THROTTLE curve mid point
void generatePitchRollCurve(controlRateConfig_t *controlRateConfig)
int16_t computeRcCurvePoint(uint8_t expo, uint8_t i)
{
for (int i = 0; i < PITCH_LOOKUP_LENGTH; i++) {
lookupPitchRollRC[i] = (2500 + controlRateConfig->rcExpo8 * (i * i - 25)) * i * (int32_t) controlRateConfig->rcRate8 / 2500;
}
return (2500 + expo * (i * i - 25)) * i / 25;
}
void generateYawCurve(controlRateConfig_t *controlRateConfig)
void generateRcCurves(controlRateConfig_t *controlRateConfig)
{
for (int i = 0; i < YAW_LOOKUP_LENGTH; i++) {
lookupYawRC[i] = (2500 + controlRateConfig->rcYawExpo8 * (i * i - 25)) * i / 25;
uint8_t i;
for (i = 0; i < PITCH_LOOKUP_LENGTH; i++) {
lookupPitchRollRC[i] = computeRcCurvePoint(controlRateConfig->rcExpo8, i);
}
for (i = 0; i < YAW_LOOKUP_LENGTH; i++) {
lookupYawRC[i] = computeRcCurvePoint(controlRateConfig->rcYawExpo8, i);
}
}
@ -63,22 +67,25 @@ void generateThrottleCurve(controlRateConfig_t *controlRateConfig, escAndServoCo
}
}
int16_t rcLookupPitchRoll(int32_t tmp)
int16_t rcLookupTable(int16_t lookupTable[], int32_t absoluteDeflection)
{
const int32_t tmp2 = tmp / 100;
return lookupPitchRollRC[tmp2] + (tmp - tmp2 * 100) * (lookupPitchRollRC[tmp2 + 1] - lookupPitchRollRC[tmp2]) / 100;
const int32_t lookupStep = absoluteDeflection / 100;
return lookupTable[lookupStep] + (absoluteDeflection - lookupStep * 100) * (lookupTable[lookupStep + 1] - lookupTable[lookupStep]) / 100;
}
int16_t rcLookupYaw(int32_t tmp)
int16_t rcLookupPitchRoll(int32_t absoluteDeflection)
{
const int32_t tmp2 = tmp / 100;
return (lookupYawRC[tmp2] + (tmp - tmp2 * 100) * (lookupYawRC[tmp2 + 1] - lookupYawRC[tmp2]) / 100);
return rcLookupTable(lookupPitchRollRC, absoluteDeflection);
}
int16_t rcLookupThrottle(int32_t tmp)
int16_t rcLookupYaw(int32_t absoluteDeflection)
{
const int32_t tmp2 = tmp / 100;
return lookupThrottleRC[tmp2] + (tmp - tmp2 * 100) * (lookupThrottleRC[tmp2 + 1] - lookupThrottleRC[tmp2]) / 100; // [0;1000] -> expo -> [MINTHROTTLE;MAXTHROTTLE]
return rcLookupTable(lookupYawRC, absoluteDeflection);
}
int16_t rcLookupThrottle(int32_t absoluteDeflection)
{
return rcLookupTable(lookupThrottleRC, absoluteDeflection);
}
int16_t rcLookupThrottleMid(void)

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

@ -17,8 +17,7 @@
#pragma once
void generatePitchRollCurve(controlRateConfig_t *controlRateConfig);
void generateYawCurve(controlRateConfig_t *controlRateConfig);
void generateRcCurves(controlRateConfig_t *controlRateConfig);
void generateThrottleCurve(controlRateConfig_t *controlRateConfig, escAndServoConfig_t *escAndServoConfig);
int16_t rcLookupPitchRoll(int32_t tmp);

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

@ -677,14 +677,19 @@ const clivalue_t valueTable[] = {
#endif
{ "default_rate_profile", VAR_UINT8 | PROFILE_VALUE , &masterConfig.profile[0].defaultRateProfileIndex, .config.minmax = { 0, MAX_CONTROL_RATE_PROFILE_COUNT - 1 }, 0 },
{ "rc_rate", VAR_UINT8 | CONTROL_RATE_VALUE, &masterConfig.controlRateProfiles[0].rcRate8, .config.minmax = { 0, 250 }, 0 },
{ "rc_expo", VAR_UINT8 | CONTROL_RATE_VALUE, &masterConfig.controlRateProfiles[0].rcExpo8, .config.minmax = { 0, 100 }, 0 },
{ "rc_yaw_expo", VAR_UINT8 | CONTROL_RATE_VALUE, &masterConfig.controlRateProfiles[0].rcYawExpo8, .config.minmax = { 0, 100 }, 0 },
{ "thr_mid", VAR_UINT8 | CONTROL_RATE_VALUE, &masterConfig.controlRateProfiles[0].thrMid8, .config.minmax = { 0, 100 }, 0 },
{ "thr_expo", VAR_UINT8 | CONTROL_RATE_VALUE, &masterConfig.controlRateProfiles[0].thrExpo8, .config.minmax = { 0, 100 }, 0 },
{ "roll_rate", VAR_UINT8 | CONTROL_RATE_VALUE, &masterConfig.controlRateProfiles[0].rates[FD_ROLL], .config.minmax = { 0, CONTROL_RATE_CONFIG_ROLL_PITCH_RATE_MAX }, 0 },
{ "pitch_rate", VAR_UINT8 | CONTROL_RATE_VALUE, &masterConfig.controlRateProfiles[0].rates[FD_PITCH], .config.minmax = { 0, CONTROL_RATE_CONFIG_ROLL_PITCH_RATE_MAX }, 0 },
{ "yaw_rate", VAR_UINT8 | CONTROL_RATE_VALUE, &masterConfig.controlRateProfiles[0].rates[FD_YAW], .config.minmax = { 0, CONTROL_RATE_CONFIG_YAW_RATE_MAX }, 0 },
/*
New rates are in dps/10. That means, Rate of 20 means 200dps of rotation speed on given axis.
Rate 180 (1800dps) is max. value gyro can measure reliably
*/
{ "roll_rate", VAR_UINT8 | CONTROL_RATE_VALUE, &masterConfig.controlRateProfiles[0].rates[FD_ROLL], .config.minmax = { CONTROL_RATE_CONFIG_ROLL_PITCH_RATE_MIN, CONTROL_RATE_CONFIG_ROLL_PITCH_RATE_MAX }, 0 },
{ "pitch_rate", VAR_UINT8 | CONTROL_RATE_VALUE, &masterConfig.controlRateProfiles[0].rates[FD_PITCH], .config.minmax = { CONTROL_RATE_CONFIG_ROLL_PITCH_RATE_MIN, CONTROL_RATE_CONFIG_ROLL_PITCH_RATE_MAX }, 0 },
{ "yaw_rate", VAR_UINT8 | CONTROL_RATE_VALUE, &masterConfig.controlRateProfiles[0].rates[FD_YAW], .config.minmax = { CONTROL_RATE_CONFIG_YAW_RATE_MIN, CONTROL_RATE_CONFIG_YAW_RATE_MAX }, 0 },
{ "tpa_rate", VAR_UINT8 | CONTROL_RATE_VALUE, &masterConfig.controlRateProfiles[0].dynThrPID, .config.minmax = { 0, CONTROL_RATE_CONFIG_TPA_MAX}, 0 },
{ "tpa_breakpoint", VAR_UINT16 | CONTROL_RATE_VALUE, &masterConfig.controlRateProfiles[0].tpa_breakpoint, .config.minmax = { PWM_RANGE_MIN, PWM_RANGE_MAX}, 0 },

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

@ -737,7 +737,7 @@ static bool processOutCommand(uint8_t cmdMSP)
break;
case MSP_RC_TUNING:
headSerialReply(11);
serialize8(currentControlRateProfile->rcRate8);
serialize8(100); //rcRate8 kept for compatibity reasons, this setting is no longer used
serialize8(currentControlRateProfile->rcExpo8);
for (i = 0 ; i < 3; i++) {
serialize8(currentControlRateProfile->rates[i]); // R,P,Y see flight_dynamics_index_t
@ -1220,11 +1220,11 @@ static bool processInCommand(void)
case MSP_SET_RC_TUNING:
if (currentPort->dataSize >= 10) {
currentControlRateProfile->rcRate8 = read8();
read8(); //Read rcRate8, kept for protocol compatibility reasons
currentControlRateProfile->rcExpo8 = read8();
for (i = 0; i < 3; i++) {
rate = read8();
currentControlRateProfile->rates[i] = MIN(rate, i == FD_YAW ? CONTROL_RATE_CONFIG_YAW_RATE_MAX : CONTROL_RATE_CONFIG_ROLL_PITCH_RATE_MAX);
currentControlRateProfile->rates[i] = constrain(rate, i == FD_YAW ? CONTROL_RATE_CONFIG_YAW_RATE_MIN : CONTROL_RATE_CONFIG_ROLL_PITCH_RATE_MIN, i == FD_YAW ? CONTROL_RATE_CONFIG_YAW_RATE_MAX : CONTROL_RATE_CONFIG_ROLL_PITCH_RATE_MAX);
}
rate = read8();
currentControlRateProfile->dynThrPID = MIN(rate, CONTROL_RATE_CONFIG_TPA_MAX);

62
src/main/mw.c
View file

@ -125,40 +125,46 @@ bool isCalibrating()
return (!isAccelerationCalibrationComplete() && sensors(SENSOR_ACC)) || (!isGyroCalibrationComplete());
}
void annexCode(void)
int16_t getAxisRcCommand(int16_t rawData, int16_t (*loopupTable)(int32_t), int16_t deadband)
{
int32_t tmp;
int16_t command, absoluteDeflection;
for (int axis = 0; axis < 3; axis++) {
tmp = MIN(ABS(rcData[axis] - masterConfig.rxConfig.midrc), 500);
if (axis == ROLL || axis == PITCH) {
if (currentProfile->rcControlsConfig.deadband) {
if (tmp > currentProfile->rcControlsConfig.deadband) {
tmp -= currentProfile->rcControlsConfig.deadband;
} else {
tmp = 0;
}
}
rcCommand[axis] = rcLookupPitchRoll(tmp);
} else if (axis == YAW) {
if (currentProfile->rcControlsConfig.yaw_deadband) {
if (tmp > currentProfile->rcControlsConfig.yaw_deadband) {
tmp -= currentProfile->rcControlsConfig.yaw_deadband;
} else {
tmp = 0;
}
}
rcCommand[axis] = rcLookupYaw(tmp) * -1;
}
absoluteDeflection = MIN(ABS(rawData - masterConfig.rxConfig.midrc), 500);
if (rcData[axis] < masterConfig.rxConfig.midrc) {
rcCommand[axis] = -rcCommand[axis];
if (deadband) {
if (absoluteDeflection > deadband) {
absoluteDeflection -= deadband;
} else {
absoluteDeflection = 0;
}
}
tmp = constrain(rcData[THROTTLE], masterConfig.rxConfig.mincheck, PWM_RANGE_MAX);
tmp = (uint32_t)(tmp - masterConfig.rxConfig.mincheck) * PWM_RANGE_MIN / (PWM_RANGE_MAX - masterConfig.rxConfig.mincheck); // [MINCHECK;2000] -> [0;1000]
rcCommand[THROTTLE] = rcLookupThrottle(tmp);
/*
Get command from lookup table after applying deadband
*/
command = loopupTable(absoluteDeflection);
if (rawData < masterConfig.rxConfig.midrc) {
command = -command;
}
return command;
}
void annexCode(void)
{
int32_t throttleValue;
// Compute ROLL PITCH and YAW command
rcCommand[ROLL] = getAxisRcCommand(rcData[ROLL], rcLookupPitchRoll, currentProfile->rcControlsConfig.deadband);
rcCommand[PITCH] = getAxisRcCommand(rcData[PITCH], rcLookupPitchRoll, currentProfile->rcControlsConfig.deadband);
rcCommand[YAW] = getAxisRcCommand(rcData[YAW], rcLookupYaw, currentProfile->rcControlsConfig.yaw_deadband);
//Compute THROTTLE command
throttleValue = constrain(rcData[THROTTLE], masterConfig.rxConfig.mincheck, PWM_RANGE_MAX);
throttleValue = (uint32_t)(throttleValue - masterConfig.rxConfig.mincheck) * PWM_RANGE_MIN / (PWM_RANGE_MAX - masterConfig.rxConfig.mincheck); // [MINCHECK;2000] -> [0;1000]
rcCommand[THROTTLE] = rcLookupThrottle(throttleValue);
if (FLIGHT_MODE(HEADFREE_MODE)) {
const float radDiff = degreesToRadians(DECIDEGREES_TO_DEGREES(attitude.values.yaw) - headFreeModeHold);

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

@ -186,7 +186,7 @@ static int callCounts[CALL_COUNT_ITEM_COUNT];
#define CALL_COUNTER(item) (callCounts[item])
extern "C" {
void generatePitchRollCurve(controlRateConfig_t *) {
void generateRcCurves(controlRateConfig_t *) {
callCounts[COUNTER_GENERATE_PITCH_ROLL_CURVE]++;
}
@ -237,7 +237,6 @@ static const adjustmentConfig_t rateAdjustmentConfig = {
class RcControlsAdjustmentsTest : public ::testing::Test {
protected:
controlRateConfig_t controlRateConfig = {
.rcRate8 = 90,
.rcExpo8 = 0,
.thrMid8 = 0,
.thrExpo8 = 0,
@ -256,7 +255,6 @@ protected:
rxConfig.maxcheck = DEFAULT_MAX_CHECK;
rxConfig.midrc = 1500;
controlRateConfig.rcRate8 = 90;
controlRateConfig.rcExpo8 = 0;
controlRateConfig.thrMid8 = 0;
controlRateConfig.thrExpo8 = 0;
@ -289,7 +287,6 @@ TEST_F(RcControlsAdjustmentsTest, processRcAdjustmentsSticksInMiddle)
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);
@ -299,7 +296,6 @@ TEST_F(RcControlsAdjustmentsTest, processRcAdjustmentsWithRcRateFunctionSwitchUp
{
// given
controlRateConfig_t controlRateConfig = {
.rcRate8 = 90,
.rcExpo8 = 0,
.thrMid8 = 0,
.thrExpo8 = 0,
@ -344,7 +340,6 @@ TEST_F(RcControlsAdjustmentsTest, processRcAdjustmentsWithRcRateFunctionSwitchUp
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);
@ -360,7 +355,6 @@ TEST_F(RcControlsAdjustmentsTest, processRcAdjustmentsWithRcRateFunctionSwitchUp
// when
processRcAdjustments(&controlRateConfig, &rxConfig);
EXPECT_EQ(controlRateConfig.rcRate8, 91);
EXPECT_EQ(adjustmentStateMask, expectedAdjustmentStateMask);
@ -382,7 +376,6 @@ TEST_F(RcControlsAdjustmentsTest, processRcAdjustmentsWithRcRateFunctionSwitchUp
// when
processRcAdjustments(&controlRateConfig, &rxConfig);
EXPECT_EQ(controlRateConfig.rcRate8, 91);
EXPECT_EQ(adjustmentStateMask, expectedAdjustmentStateMask);
@ -403,7 +396,6 @@ TEST_F(RcControlsAdjustmentsTest, processRcAdjustmentsWithRcRateFunctionSwitchUp
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);
@ -420,7 +412,6 @@ TEST_F(RcControlsAdjustmentsTest, processRcAdjustmentsWithRcRateFunctionSwitchUp
processRcAdjustments(&controlRateConfig, &rxConfig);
// then
EXPECT_EQ(controlRateConfig.rcRate8, 92);
EXPECT_EQ(adjustmentStateMask, expectedAdjustmentStateMask);
//
@ -434,7 +425,6 @@ TEST_F(RcControlsAdjustmentsTest, processRcAdjustmentsWithRcRateFunctionSwitchUp
processRcAdjustments(&controlRateConfig, &rxConfig);
// then
EXPECT_EQ(controlRateConfig.rcRate8, 92);
EXPECT_EQ(adjustmentStateMask, expectedAdjustmentStateMask);
//
@ -450,7 +440,6 @@ TEST_F(RcControlsAdjustmentsTest, processRcAdjustmentsWithRcRateFunctionSwitchUp
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);
@ -714,4 +703,4 @@ int16_t heading;
uint8_t stateFlags = 0;
int16_t rcData[MAX_SUPPORTED_RC_CHANNEL_COUNT];
rxRuntimeConfig_t rxRuntimeConfig;
}
}