Mirror/inav
1
0
Fork 0
mirror of https://github.com/iNavFlight/inav.git synced 2025-07-26 01:35:35 +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 | | `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 | | `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 | | `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_expo` | | 0 | 100 | 65 | Rate Profile | UINT8 |
| `rc_yaw_expo` | | 0 | 100 | 0 | Rate Profile | UINT8 | | `rc_yaw_expo` | | 0 | 100 | 0 | Rate Profile | UINT8 |
| `thr_mid` | | 0 | 100 | 50 | Rate Profile | UINT8 | | `thr_mid` | | 0 | 100 | 50 | Rate Profile | UINT8 |
| `thr_expo` | | 0 | 100 | 0 | Rate Profile | UINT8 | | `thr_expo` | | 0 | 100 | 0 | Rate Profile | UINT8 |
| `roll_pitch_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.
| `yaw_rate` | | 0 | 100 | 0 | Rate Profile | UINT8 | | `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_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 | | `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 | | `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. 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 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 0 has a default rate profile of 0.
Profile 1 has a default rate profile of 1. Profile 1 has a default rate profile of 1.
@ -54,7 +54,6 @@ e.g
# rateprofile # rateprofile
rateprofile 0 rateprofile 0
set rc_rate = 90
set rc_expo = 65 set rc_expo = 65
set thr_mid = 50 set thr_mid = 50
set thr_expo = 0 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); arraySubInt32(deltas, blackboxCurrent->axisPID_P, blackboxLast->axisPID_P, XYZ_AXIS_COUNT);
blackboxWriteSignedVBArray(deltas, XYZ_AXIS_COUNT); blackboxWriteSignedVBArray(deltas, XYZ_AXIS_COUNT);
/* /*
* The PID I field changes very slowly, most of the time +-2, so use an encoding * 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. * that can pack all three fields into one byte in that situation.
*/ */
@ -920,7 +920,7 @@ void startBlackbox(void)
* cache those now. * cache those now.
*/ */
blackboxBuildConditionCache(); blackboxBuildConditionCache();
blackboxModeActivationConditionPresent = isModeActivationConditionPresent(currentProfile->modeActivationConditions, BOXBLACKBOX); blackboxModeActivationConditionPresent = isModeActivationConditionPresent(currentProfile->modeActivationConditions, BOXBLACKBOX);
blackboxIteration = 0; blackboxIteration = 0;
@ -1228,7 +1228,7 @@ static bool blackboxWriteSysinfo()
blackboxPrintfHeaderLine("P interval:%d/%d", masterConfig.blackbox_rate_num, masterConfig.blackbox_rate_denom); blackboxPrintfHeaderLine("P interval:%d/%d", masterConfig.blackbox_rate_num, masterConfig.blackbox_rate_denom);
break; break;
case 4: case 4:
blackboxPrintfHeaderLine("rcRate:%d", masterConfig.controlRateProfiles[masterConfig.current_profile_index].rcRate8); blackboxPrintfHeaderLine("rcRate:%d", 100); //For compatibility reasons write rc_rate 100
break; break;
case 5: case 5:
blackboxPrintfHeaderLine("minthrottle:%d", masterConfig.escAndServoConfig.minthrottle); 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 * 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. * the portion of logged loop iterations.
*/ */
@ -1367,7 +1367,7 @@ static void blackboxLogIteration()
writeIntraframe(); writeIntraframe();
} else { } else {
blackboxCheckAndLogArmingBeep(); blackboxCheckAndLogArmingBeep();
if (blackboxShouldLogPFrame(blackboxPFrameIndex)) { if (blackboxShouldLogPFrame(blackboxPFrameIndex)) {
/* /*
* We assume that slow frames are only interesting in that they aid the interpretation of the main data stream. * 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); blackboxLogEvent(FLIGHT_LOG_EVENT_LOGGING_RESUME, (flightLogEventData_t *) &resume);
blackboxSetState(BLACKBOX_STATE_RUNNING); blackboxSetState(BLACKBOX_STATE_RUNNING);
blackboxLogIteration(); 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; 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 // Normalize a vector
void normalizeV(struct fp_vector *src, struct fp_vector *dest) 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); float degreesToRadians(int16_t degrees);
int scaleRange(int x, int srcMin, int srcMax, int destMin, int destMax); 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); 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) { static void resetControlRateConfig(controlRateConfig_t *controlRateConfig) {
controlRateConfig->rcRate8 = 90;
controlRateConfig->rcExpo8 = 70; controlRateConfig->rcExpo8 = 70;
controlRateConfig->thrMid8 = 50; controlRateConfig->thrMid8 = 50;
controlRateConfig->thrExpo8 = 0; controlRateConfig->thrExpo8 = 0;
@ -362,7 +361,11 @@ static void resetControlRateConfig(controlRateConfig_t *controlRateConfig) {
controlRateConfig->tpa_breakpoint = 1500; controlRateConfig->tpa_breakpoint = 1500;
for (uint8_t axis = 0; axis < FLIGHT_DYNAMICS_INDEX_COUNT; axis++) { 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; currentProfile->pidProfile.P8[PITCH] = 36;
masterConfig.failsafeConfig.failsafe_delay = 2; masterConfig.failsafeConfig.failsafe_delay = 2;
masterConfig.failsafeConfig.failsafe_off_delay = 0; masterConfig.failsafeConfig.failsafe_off_delay = 0;
currentControlRateProfile->rcRate8 = 130; currentControlRateProfile->rates[FD_PITCH] = CONTROL_RATE_CONFIG_ROLL_PITCH_RATE_DEFAULT;
currentControlRateProfile->rates[FD_PITCH] = 20; currentControlRateProfile->rates[FD_ROLL] = CONTROL_RATE_CONFIG_ROLL_PITCH_RATE_DEFAULT;
currentControlRateProfile->rates[FD_ROLL] = 20; currentControlRateProfile->rates[FD_YAW] = CONTROL_RATE_CONFIG_YAW_RATE_DEFAULT;
currentControlRateProfile->rates[FD_YAW] = 100;
parseRcChannels("TAER1234", &masterConfig.rxConfig); parseRcChannels("TAER1234", &masterConfig.rxConfig);
// { 1.0f, -0.414178f, 1.0f, -1.0f }, // REAR_R // { 1.0f, -0.414178f, 1.0f, -1.0f }, // REAR_R
@ -728,8 +730,7 @@ static bool isEEPROMContentValid(void)
void activateControlRateConfig(void) void activateControlRateConfig(void)
{ {
generatePitchRollCurve(currentControlRateProfile); generateRcCurves(currentControlRateProfile);
generateYawCurve(currentControlRateProfile);
generateThrottleCurve(currentControlRateProfile, &masterConfig.escAndServoConfig); 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; 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) float pidRcCommandToRate(int16_t stick, uint8_t rate)
{ {
// Map stick position from 200dps to 1200dps return scaleRangef((float) stick, (float) -500, (float) 500, (float) -rate, (float) rate) * 10;
return (float)((rate + 20) * stick) / 50.0f;
} }
#define FP_PID_RATE_P_MULTIPLIER 40.0f // betaflight - 40.0 #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++) { for (index = 0; index < MAX_MODE_ACTIVATION_CONDITION_COUNT; index++) {
modeActivationCondition_t *modeActivationCondition = &modeActivationConditions[index]; modeActivationCondition_t *modeActivationCondition = &modeActivationConditions[index];
if (modeActivationCondition->modeId == modeId && IS_RANGE_USABLE(&modeActivationCondition->range)) { if (modeActivationCondition->modeId == modeId && IS_RANGE_USABLE(&modeActivationCondition->range)) {
return true; return true;
} }
} }
return false; return false;
} }
@ -456,16 +456,10 @@ void applyStepAdjustment(controlRateConfig_t *controlRateConfig, uint8_t adjustm
beeperConfirmationBeeps(1); beeperConfirmationBeeps(1);
} }
switch(adjustmentFunction) { 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: case ADJUSTMENT_RC_EXPO:
newValue = constrain((int)controlRateConfig->rcExpo8 + delta, 0, 100); // FIXME magic numbers repeated in serial_cli.c newValue = constrain((int)controlRateConfig->rcExpo8 + delta, 0, 100); // FIXME magic numbers repeated in serial_cli.c
controlRateConfig->rcExpo8 = newValue; controlRateConfig->rcExpo8 = newValue;
generatePitchRollCurve(controlRateConfig); generateRcCurves(controlRateConfig);
blackboxLogInflightAdjustmentEvent(ADJUSTMENT_RC_EXPO, newValue); blackboxLogInflightAdjustmentEvent(ADJUSTMENT_RC_EXPO, newValue);
break; break;
case ADJUSTMENT_THROTTLE_EXPO: case ADJUSTMENT_THROTTLE_EXPO:
@ -476,7 +470,7 @@ void applyStepAdjustment(controlRateConfig_t *controlRateConfig, uint8_t adjustm
break; break;
case ADJUSTMENT_PITCH_ROLL_RATE: case ADJUSTMENT_PITCH_ROLL_RATE:
case ADJUSTMENT_PITCH_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; controlRateConfig->rates[FD_PITCH] = newValue;
blackboxLogInflightAdjustmentEvent(ADJUSTMENT_PITCH_RATE, newValue); blackboxLogInflightAdjustmentEvent(ADJUSTMENT_PITCH_RATE, newValue);
if (adjustmentFunction == ADJUSTMENT_PITCH_RATE) { 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 // follow though for combined ADJUSTMENT_PITCH_ROLL_RATE
case ADJUSTMENT_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; controlRateConfig->rates[FD_ROLL] = newValue;
blackboxLogInflightAdjustmentEvent(ADJUSTMENT_ROLL_RATE, newValue); blackboxLogInflightAdjustmentEvent(ADJUSTMENT_ROLL_RATE, newValue);
break; break;
case ADJUSTMENT_YAW_RATE: 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; controlRateConfig->rates[FD_YAW] = newValue;
blackboxLogInflightAdjustmentEvent(ADJUSTMENT_YAW_RATE, newValue); blackboxLogInflightAdjustmentEvent(ADJUSTMENT_YAW_RATE, newValue);
break; break;
@ -676,4 +670,3 @@ void resetAdjustmentStates(void)
{ {
memset(adjustmentStates, 0, sizeof(adjustmentStates)); 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 MIN_MODE_RANGE_STEP 0
#define MAX_MODE_RANGE_STEP ((CHANNEL_RANGE_MAX - CHANNEL_RANGE_MIN) / 25) #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: */ New defaults of 200dps for pitch,roll and yaw are more less
#define CONTROL_RATE_CONFIG_YAW_RATE_MAX 255 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 #define CONTROL_RATE_CONFIG_TPA_MAX 100
@ -134,7 +144,6 @@ typedef struct modeActivationCondition_s {
#define IS_RANGE_USABLE(range) ((range)->startStep < (range)->endStep) #define IS_RANGE_USABLE(range) ((range)->startStep < (range)->endStep)
typedef struct controlRateConfig_s { typedef struct controlRateConfig_s {
uint8_t rcRate8;
uint8_t rcExpo8; uint8_t rcExpo8;
uint8_t thrMid8; uint8_t thrMid8;
uint8_t thrExpo8; 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 static int16_t lookupThrottleRC[THROTTLE_LOOKUP_LENGTH]; // lookup table for expo & mid THROTTLE
int16_t lookupThrottleRCMid; // THROTTLE curve mid point 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++) { return (2500 + expo * (i * i - 25)) * i / 25;
lookupPitchRollRC[i] = (2500 + controlRateConfig->rcExpo8 * (i * i - 25)) * i * (int32_t) controlRateConfig->rcRate8 / 2500;
}
} }
void generateYawCurve(controlRateConfig_t *controlRateConfig) void generateRcCurves(controlRateConfig_t *controlRateConfig)
{ {
for (int i = 0; i < YAW_LOOKUP_LENGTH; i++) { uint8_t i;
lookupYawRC[i] = (2500 + controlRateConfig->rcYawExpo8 * (i * i - 25)) * i / 25;
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; const int32_t lookupStep = absoluteDeflection / 100;
return lookupPitchRollRC[tmp2] + (tmp - tmp2 * 100) * (lookupPitchRollRC[tmp2 + 1] - lookupPitchRollRC[tmp2]) / 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 rcLookupTable(lookupPitchRollRC, absoluteDeflection);
return (lookupYawRC[tmp2] + (tmp - tmp2 * 100) * (lookupYawRC[tmp2 + 1] - lookupYawRC[tmp2]) / 100);
} }
int16_t rcLookupThrottle(int32_t tmp) int16_t rcLookupYaw(int32_t absoluteDeflection)
{ {
const int32_t tmp2 = tmp / 100; return rcLookupTable(lookupYawRC, absoluteDeflection);
return lookupThrottleRC[tmp2] + (tmp - tmp2 * 100) * (lookupThrottleRC[tmp2 + 1] - lookupThrottleRC[tmp2]) / 100; // [0;1000] -> expo -> [MINTHROTTLE;MAXTHROTTLE] }
int16_t rcLookupThrottle(int32_t absoluteDeflection)
{
return rcLookupTable(lookupThrottleRC, absoluteDeflection);
} }
int16_t rcLookupThrottleMid(void) int16_t rcLookupThrottleMid(void)

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

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

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

@ -677,14 +677,19 @@ const clivalue_t valueTable[] = {
#endif #endif
{ "default_rate_profile", VAR_UINT8 | PROFILE_VALUE , &masterConfig.profile[0].defaultRateProfileIndex, .config.minmax = { 0, MAX_CONTROL_RATE_PROFILE_COUNT - 1 }, 0 }, { "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_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 }, { "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_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 }, { "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_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 }, { "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; break;
case MSP_RC_TUNING: case MSP_RC_TUNING:
headSerialReply(11); headSerialReply(11);
serialize8(currentControlRateProfile->rcRate8); serialize8(100); //rcRate8 kept for compatibity reasons, this setting is no longer used
serialize8(currentControlRateProfile->rcExpo8); serialize8(currentControlRateProfile->rcExpo8);
for (i = 0 ; i < 3; i++) { for (i = 0 ; i < 3; i++) {
serialize8(currentControlRateProfile->rates[i]); // R,P,Y see flight_dynamics_index_t 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: case MSP_SET_RC_TUNING:
if (currentPort->dataSize >= 10) { if (currentPort->dataSize >= 10) {
currentControlRateProfile->rcRate8 = read8(); read8(); //Read rcRate8, kept for protocol compatibility reasons
currentControlRateProfile->rcExpo8 = read8(); currentControlRateProfile->rcExpo8 = read8();
for (i = 0; i < 3; i++) { for (i = 0; i < 3; i++) {
rate = read8(); 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(); rate = read8();
currentControlRateProfile->dynThrPID = MIN(rate, CONTROL_RATE_CONFIG_TPA_MAX); 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()); 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++) { absoluteDeflection = MIN(ABS(rawData - masterConfig.rxConfig.midrc), 500);
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;
}
if (rcData[axis] < masterConfig.rxConfig.midrc) { if (deadband) {
rcCommand[axis] = -rcCommand[axis]; 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] Get command from lookup table after applying deadband
rcCommand[THROTTLE] = rcLookupThrottle(tmp); */
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)) { if (FLIGHT_MODE(HEADFREE_MODE)) {
const float radDiff = degreesToRadians(DECIDEGREES_TO_DEGREES(attitude.values.yaw) - headFreeModeHold); 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]) #define CALL_COUNTER(item) (callCounts[item])
extern "C" { extern "C" {
void generatePitchRollCurve(controlRateConfig_t *) { void generateRcCurves(controlRateConfig_t *) {
callCounts[COUNTER_GENERATE_PITCH_ROLL_CURVE]++; callCounts[COUNTER_GENERATE_PITCH_ROLL_CURVE]++;
} }
@ -237,7 +237,6 @@ static const adjustmentConfig_t rateAdjustmentConfig = {
class RcControlsAdjustmentsTest : public ::testing::Test { class RcControlsAdjustmentsTest : public ::testing::Test {
protected: protected:
controlRateConfig_t controlRateConfig = { controlRateConfig_t controlRateConfig = {
.rcRate8 = 90,
.rcExpo8 = 0, .rcExpo8 = 0,
.thrMid8 = 0, .thrMid8 = 0,
.thrExpo8 = 0, .thrExpo8 = 0,
@ -256,7 +255,6 @@ protected:
rxConfig.maxcheck = DEFAULT_MAX_CHECK; rxConfig.maxcheck = DEFAULT_MAX_CHECK;
rxConfig.midrc = 1500; rxConfig.midrc = 1500;
controlRateConfig.rcRate8 = 90;
controlRateConfig.rcExpo8 = 0; controlRateConfig.rcExpo8 = 0;
controlRateConfig.thrMid8 = 0; controlRateConfig.thrMid8 = 0;
controlRateConfig.thrExpo8 = 0; controlRateConfig.thrExpo8 = 0;
@ -289,7 +287,6 @@ TEST_F(RcControlsAdjustmentsTest, processRcAdjustmentsSticksInMiddle)
processRcAdjustments(&controlRateConfig, &rxConfig); processRcAdjustments(&controlRateConfig, &rxConfig);
// then // then
EXPECT_EQ(controlRateConfig.rcRate8, 90);
EXPECT_EQ(CALL_COUNTER(COUNTER_GENERATE_PITCH_ROLL_CURVE), 0); EXPECT_EQ(CALL_COUNTER(COUNTER_GENERATE_PITCH_ROLL_CURVE), 0);
EXPECT_EQ(CALL_COUNTER(COUNTER_QUEUE_CONFIRMATION_BEEP), 0); EXPECT_EQ(CALL_COUNTER(COUNTER_QUEUE_CONFIRMATION_BEEP), 0);
EXPECT_EQ(adjustmentStateMask, 0); EXPECT_EQ(adjustmentStateMask, 0);
@ -299,7 +296,6 @@ TEST_F(RcControlsAdjustmentsTest, processRcAdjustmentsWithRcRateFunctionSwitchUp
{ {
// given // given
controlRateConfig_t controlRateConfig = { controlRateConfig_t controlRateConfig = {
.rcRate8 = 90,
.rcExpo8 = 0, .rcExpo8 = 0,
.thrMid8 = 0, .thrMid8 = 0,
.thrExpo8 = 0, .thrExpo8 = 0,
@ -344,7 +340,6 @@ TEST_F(RcControlsAdjustmentsTest, processRcAdjustmentsWithRcRateFunctionSwitchUp
processRcAdjustments(&controlRateConfig, &rxConfig); processRcAdjustments(&controlRateConfig, &rxConfig);
// then // then
EXPECT_EQ(controlRateConfig.rcRate8, 91);
EXPECT_EQ(CALL_COUNTER(COUNTER_GENERATE_PITCH_ROLL_CURVE), 1); EXPECT_EQ(CALL_COUNTER(COUNTER_GENERATE_PITCH_ROLL_CURVE), 1);
EXPECT_EQ(CALL_COUNTER(COUNTER_QUEUE_CONFIRMATION_BEEP), 1); EXPECT_EQ(CALL_COUNTER(COUNTER_QUEUE_CONFIRMATION_BEEP), 1);
EXPECT_EQ(adjustmentStateMask, expectedAdjustmentStateMask); EXPECT_EQ(adjustmentStateMask, expectedAdjustmentStateMask);
@ -360,7 +355,6 @@ TEST_F(RcControlsAdjustmentsTest, processRcAdjustmentsWithRcRateFunctionSwitchUp
// when // when
processRcAdjustments(&controlRateConfig, &rxConfig); processRcAdjustments(&controlRateConfig, &rxConfig);
EXPECT_EQ(controlRateConfig.rcRate8, 91);
EXPECT_EQ(adjustmentStateMask, expectedAdjustmentStateMask); EXPECT_EQ(adjustmentStateMask, expectedAdjustmentStateMask);
@ -382,7 +376,6 @@ TEST_F(RcControlsAdjustmentsTest, processRcAdjustmentsWithRcRateFunctionSwitchUp
// when // when
processRcAdjustments(&controlRateConfig, &rxConfig); processRcAdjustments(&controlRateConfig, &rxConfig);
EXPECT_EQ(controlRateConfig.rcRate8, 91);
EXPECT_EQ(adjustmentStateMask, expectedAdjustmentStateMask); EXPECT_EQ(adjustmentStateMask, expectedAdjustmentStateMask);
@ -403,7 +396,6 @@ TEST_F(RcControlsAdjustmentsTest, processRcAdjustmentsWithRcRateFunctionSwitchUp
processRcAdjustments(&controlRateConfig, &rxConfig); processRcAdjustments(&controlRateConfig, &rxConfig);
// then // then
EXPECT_EQ(controlRateConfig.rcRate8, 92);
EXPECT_EQ(CALL_COUNTER(COUNTER_GENERATE_PITCH_ROLL_CURVE), 2); EXPECT_EQ(CALL_COUNTER(COUNTER_GENERATE_PITCH_ROLL_CURVE), 2);
EXPECT_EQ(CALL_COUNTER(COUNTER_QUEUE_CONFIRMATION_BEEP), 2); EXPECT_EQ(CALL_COUNTER(COUNTER_QUEUE_CONFIRMATION_BEEP), 2);
EXPECT_EQ(adjustmentStateMask, expectedAdjustmentStateMask); EXPECT_EQ(adjustmentStateMask, expectedAdjustmentStateMask);
@ -420,7 +412,6 @@ TEST_F(RcControlsAdjustmentsTest, processRcAdjustmentsWithRcRateFunctionSwitchUp
processRcAdjustments(&controlRateConfig, &rxConfig); processRcAdjustments(&controlRateConfig, &rxConfig);
// then // then
EXPECT_EQ(controlRateConfig.rcRate8, 92);
EXPECT_EQ(adjustmentStateMask, expectedAdjustmentStateMask); EXPECT_EQ(adjustmentStateMask, expectedAdjustmentStateMask);
// //
@ -434,7 +425,6 @@ TEST_F(RcControlsAdjustmentsTest, processRcAdjustmentsWithRcRateFunctionSwitchUp
processRcAdjustments(&controlRateConfig, &rxConfig); processRcAdjustments(&controlRateConfig, &rxConfig);
// then // then
EXPECT_EQ(controlRateConfig.rcRate8, 92);
EXPECT_EQ(adjustmentStateMask, expectedAdjustmentStateMask); EXPECT_EQ(adjustmentStateMask, expectedAdjustmentStateMask);
// //
@ -450,7 +440,6 @@ TEST_F(RcControlsAdjustmentsTest, processRcAdjustmentsWithRcRateFunctionSwitchUp
processRcAdjustments(&controlRateConfig, &rxConfig); processRcAdjustments(&controlRateConfig, &rxConfig);
// then // then
EXPECT_EQ(controlRateConfig.rcRate8, 93);
EXPECT_EQ(CALL_COUNTER(COUNTER_GENERATE_PITCH_ROLL_CURVE), 3); EXPECT_EQ(CALL_COUNTER(COUNTER_GENERATE_PITCH_ROLL_CURVE), 3);
EXPECT_EQ(CALL_COUNTER(COUNTER_QUEUE_CONFIRMATION_BEEP), 3); EXPECT_EQ(CALL_COUNTER(COUNTER_QUEUE_CONFIRMATION_BEEP), 3);
EXPECT_EQ(adjustmentStateMask, expectedAdjustmentStateMask); EXPECT_EQ(adjustmentStateMask, expectedAdjustmentStateMask);
@ -714,4 +703,4 @@ int16_t heading;
uint8_t stateFlags = 0; uint8_t stateFlags = 0;
int16_t rcData[MAX_SUPPORTED_RC_CHANNEL_COUNT]; int16_t rcData[MAX_SUPPORTED_RC_CHANNEL_COUNT];
rxRuntimeConfig_t rxRuntimeConfig; rxRuntimeConfig_t rxRuntimeConfig;
} }