mirror of
https://github.com/iNavFlight/inav.git
synced 2025-07-23 00:05:28 +03:00
Merge branch 'inav-rates-in-dps' of https://github.com/DzikuVx/cleanflight into DzikuVx-inav-rates-in-dps
This commit is contained in:
commit
f77eb4788d
15 changed files with 124 additions and 105 deletions
|
@ -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 |
|
||||||
|
|
|
@ -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
|
||||||
|
|
|
@ -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);
|
||||||
|
|
|
@ -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)
|
||||||
{
|
{
|
||||||
|
|
|
@ -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);
|
||||||
|
|
||||||
|
|
|
@ -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);
|
||||||
}
|
}
|
||||||
|
|
||||||
|
|
|
@ -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
|
||||||
|
|
|
@ -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));
|
||||||
}
|
}
|
||||||
|
|
||||||
|
|
|
@ -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;
|
||||||
|
|
|
@ -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)
|
||||||
|
|
|
@ -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);
|
||||||
|
|
|
@ -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 },
|
||||||
|
|
||||||
|
|
|
@ -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);
|
||||||
|
|
|
@ -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);
|
||||||
|
|
|
@ -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);
|
||||||
|
|
Loading…
Add table
Add a link
Reference in a new issue