Mirror/betaflight
1
0
Fork 0
mirror of https://github.com/betaflight/betaflight.git synced 2025-07-26 01:35:41 +03:00
Code Issues Wiki Activity

Localised RC lookup into rc_curves.c

Browse source
This commit is contained in:
Martin Budden 2016-05-09 10:40:07 +01:00
parent 90bc67e2cc
commit db4da776c5
3 changed files with 36 additions and 18 deletions
Download patch file Download diff file Expand all files Collapse all files

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

@ -26,9 +26,13 @@
#include "config/config.h" #include "config/config.h"
int16_t lookupPitchRollRC[PITCH_LOOKUP_LENGTH]; // lookup table for expo & RC rate PITCH+ROLL #define PITCH_LOOKUP_LENGTH 31
int16_t lookupYawRC[YAW_LOOKUP_LENGTH]; // lookup table for expo & RC rate YAW #define YAW_LOOKUP_LENGTH 31
int16_t lookupThrottleRC[THROTTLE_LOOKUP_LENGTH]; // lookup table for expo & mid THROTTLE #define THROTTLE_LOOKUP_LENGTH 12
static int16_t lookupPitchRollRC[PITCH_LOOKUP_LENGTH]; // lookup table for expo & RC rate PITCH+ROLL
static int16_t lookupYawRC[YAW_LOOKUP_LENGTH]; // lookup table for expo & RC rate YAW
static int16_t lookupThrottleRC[THROTTLE_LOOKUP_LENGTH]; // lookup table for expo & mid THROTTLE
void generatePitchRollCurve(controlRateConfig_t *controlRateConfig) void generatePitchRollCurve(controlRateConfig_t *controlRateConfig)
@ -69,3 +73,23 @@ void generateThrottleCurve(controlRateConfig_t *controlRateConfig, escAndServoCo
lookupThrottleRC[i] = minThrottle + (int32_t) (escAndServoConfig->maxthrottle - minThrottle) * lookupThrottleRC[i] / 1000; // [MINTHROTTLE;MAXTHROTTLE] lookupThrottleRC[i] = minThrottle + (int32_t) (escAndServoConfig->maxthrottle - minThrottle) * lookupThrottleRC[i] / 1000; // [MINTHROTTLE;MAXTHROTTLE]
} }
} }
int16_t rcLookupPitchRoll(int32_t tmp)
{
const int32_t tmp2 = tmp / 20;
return lookupPitchRollRC[tmp2] + (tmp - tmp2 * 20) * (lookupPitchRollRC[tmp2 + 1] - lookupPitchRollRC[tmp2]) / 20;
}
int16_t rcLookupYaw(int32_t tmp)
{
const int32_t tmp2 = tmp / 20;
return lookupYawRC[tmp2] + (tmp - tmp2 * 20) * (lookupYawRC[tmp2 + 1] - lookupYawRC[tmp2]) / 20;
}
int16_t rcLookupThrottle(int32_t tmp)
{
const int32_t tmp2 = tmp / 100;
// [0;1000] -> expo -> [MINTHROTTLE;MAXTHROTTLE]
return lookupThrottleRC[tmp2] + (tmp - tmp2 * 100) * (lookupThrottleRC[tmp2 + 1] - lookupThrottleRC[tmp2]) / 100;
}

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

@ -17,13 +17,11 @@
#pragma once #pragma once
#define PITCH_LOOKUP_LENGTH 31
#define YAW_LOOKUP_LENGTH 31
#define THROTTLE_LOOKUP_LENGTH 12
extern int16_t lookupPitchRollRC[PITCH_LOOKUP_LENGTH]; // lookup table for expo & RC rate PITCH+ROLL
extern int16_t lookupYawRC[YAW_LOOKUP_LENGTH]; // lookup table for expo & RC rate YAW
extern int16_t lookupThrottleRC[THROTTLE_LOOKUP_LENGTH]; // lookup table for expo & mid THROTTLE
void generatePitchRollCurve(controlRateConfig_t *controlRateConfig); void generatePitchRollCurve(controlRateConfig_t *controlRateConfig);
void generateYawCurve(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 rcLookupYaw(int32_t tmp);
int16_t rcLookupThrottle(int32_t tmp);

12
src/main/mw.c
View file

@ -222,7 +222,7 @@ void scaleRcCommandToFpvCamAngle(void) {
void annexCode(void) void annexCode(void)
{ {
int32_t tmp, tmp2; int32_t tmp;
int32_t axis, prop; int32_t axis, prop;
// PITCH & ROLL only dynamic PID adjustment, depending on throttle value // PITCH & ROLL only dynamic PID adjustment, depending on throttle value
@ -246,9 +246,7 @@ void annexCode(void)
tmp = 0; tmp = 0;
} }
} }
rcCommand[axis] = rcLookupPitchRoll(tmp);
tmp2 = tmp / 20;
rcCommand[axis] = lookupPitchRollRC[tmp2] + (tmp - tmp2 * 20) * (lookupPitchRollRC[tmp2 + 1] - lookupPitchRollRC[tmp2]) / 20;
} else if (axis == YAW) { } else if (axis == YAW) {
if (masterConfig.rcControlsConfig.yaw_deadband) { if (masterConfig.rcControlsConfig.yaw_deadband) {
if (tmp > masterConfig.rcControlsConfig.yaw_deadband) { if (tmp > masterConfig.rcControlsConfig.yaw_deadband) {
@ -257,8 +255,7 @@ void annexCode(void)
tmp = 0; tmp = 0;
} }
} }
tmp2 = tmp / 20; rcCommand[axis] = rcLookupYaw(tmp) * -masterConfig.yaw_control_direction;
rcCommand[axis] = (lookupYawRC[tmp2] + (tmp - tmp2 * 20) * (lookupYawRC[tmp2 + 1] - lookupYawRC[tmp2]) / 20) * -masterConfig.yaw_control_direction;
} }
// non coupled PID reduction scaler used in PID controller 1 and PID controller 2. // non coupled PID reduction scaler used in PID controller 1 and PID controller 2.
@ -275,8 +272,7 @@ void annexCode(void)
tmp = constrain(rcData[THROTTLE], masterConfig.rxConfig.mincheck, PWM_RANGE_MAX); 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); tmp = (uint32_t)(tmp - masterConfig.rxConfig.mincheck) * PWM_RANGE_MIN / (PWM_RANGE_MAX - masterConfig.rxConfig.mincheck);
} }
tmp2 = tmp / 100; rcCommand[THROTTLE] = rcLookupThrottle(tmp);
rcCommand[THROTTLE] = lookupThrottleRC[tmp2] + (tmp - tmp2 * 100) * (lookupThrottleRC[tmp2 + 1] - lookupThrottleRC[tmp2]) / 100; // [0;1000] -> expo -> [MINTHROTTLE;MAXTHROTTLE]
if (feature(FEATURE_3D) && IS_RC_MODE_ACTIVE(BOX3DDISABLESWITCH) && !failsafeIsActive()) { if (feature(FEATURE_3D) && IS_RC_MODE_ACTIVE(BOX3DDISABLESWITCH) && !failsafeIsActive()) {
fix12_t throttleScaler = qConstruct(rcCommand[THROTTLE] - 1000, 1000); fix12_t throttleScaler = qConstruct(rcCommand[THROTTLE] - 1000, 1000);