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

Fix setpoint rate sign, optimize code, cleanup from review

Browse source 
Also adds a `scaleRangef()` function to math.c
This commit is contained in:
Bruce Luckcuck 2018-06-27 19:27:22 -04:00
parent 0cd61c3a0f
commit 9b49d481e3
4 changed files with 37 additions and 53 deletions
Download patch file Download diff file Expand all files Collapse all files

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

@ -176,6 +176,12 @@ int scaleRange(int x, int srcFrom, int srcTo, int destFrom, int destTo) {
return (a / b) + destFrom;
}
float scaleRangef(float x, float srcFrom, float srcTo, float destFrom, float destTo) {
float a = (destTo - destFrom) * (x - srcFrom);
float b = srcTo - srcFrom;
return (a / b) + destFrom;
}
// Normalize a vector
void normalizeV(struct fp_vector *src, struct fp_vector *dest)
{

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

@ -102,6 +102,7 @@ float devStandardDeviation(stdev_t *dev);
float degreesToRadians(int16_t degrees);
int scaleRange(int x, int srcFrom, int srcTo, int destFrom, int destTo);
float scaleRangef(float x, float srcFrom, float srcTo, float destFrom, float destTo);
void normalizeV(struct fp_vector *src, struct fp_vector *dest);

57
src/main/fc/fc_rc.c
View file

@ -145,22 +145,33 @@ float applyRaceFlightRates(const int axis, float rcCommandf, const float rcComma
return angleRate;
}
static void calculateSetpointRate(int axis, bool zeroAxis)
static void calculateSetpointRate(int axis)
{
float rcCommandf = 0;
if (!zeroAxis) {
// scale rcCommandf to range [-1.0, 1.0]
rcCommandf = rcCommand[axis] / 500.0f;
}
rcDeflection[axis] = rcCommandf;
const float rcCommandfAbs = ABS(rcCommandf);
rcDeflectionAbs[axis] = rcCommandfAbs;
float angleRate;
float angleRate = applyRates(axis, rcCommandf, rcCommandfAbs);
#ifdef USE_GPS_RESCUE
if ((axis == FD_YAW) && FLIGHT_MODE(GPS_RESCUE_MODE)) {
// If GPS Rescue is active then override the setpointRate used in the
// pid controller with the value calculated from the desired heading logic.
angleRate = gpsRescueGetYawRate();
// Treat the stick input as centered to avoid any stick deflection base modifications (like acceleration limit)
rcDeflection[axis] = 0;
rcDeflectionAbs[axis] = 0;
} else
#endif
{
// scale rcCommandf to range [-1.0, 1.0]
float rcCommandf = rcCommand[axis] / 500.0f;
rcDeflection[axis] = rcCommandf;
const float rcCommandfAbs = ABS(rcCommandf);
rcDeflectionAbs[axis] = rcCommandfAbs;
angleRate = applyRates(axis, rcCommandf, rcCommandfAbs);
}
setpointRate[axis] = constrainf(angleRate, -SETPOINT_RATE_LIMIT, SETPOINT_RATE_LIMIT); // Rate limit protection (deg/sec)
DEBUG_SET(DEBUG_ANGLERATE, axis, angleRate);
setpointRate[axis] = constrainf(angleRate, -SETPOINT_RATE_LIMIT, SETPOINT_RATE_LIMIT); // Rate limit protection (deg/sec)
}
static void scaleRcCommandToFpvCamAngle(void)
@ -552,27 +563,7 @@ FAST_CODE void processRcCommand(void)
#if defined(SIMULATOR_BUILD)
#pragma GCC diagnostic pop
#endif
bool useGpsRescueYaw = false;
#ifdef USE_GPS_RESCUE
// Logic improvements to not override rcCommand when GPS Rescue is active.
// Instead only modify the flight control values derived from rcCommand
// like setpointRate[] and rcDeflection[].
if ((axis == FD_YAW) && FLIGHT_MODE(GPS_RESCUE_MODE)) {
// If GPS Rescue is active then override the yaw commanded input
// with a zero (centered) stick input. Otherwise the pilot's yaw inputs
// will get calculated into the rcDeflection values affecting downstream logic
useGpsRescueYaw = true;
}
#endif
calculateSetpointRate(axis, useGpsRescueYaw);
#ifdef USE_GPS_RESCUE
if (useGpsRescueYaw) {
// If GPS Rescue is active then override the setpointRate used in the
// pid controller with the value calculated from the desired heading logic.
setpointRate[axis] = gpsRescueGetYawRate();
}
#endif
calculateSetpointRate(axis);
}
DEBUG_SET(DEBUG_RC_INTERPOLATION, 3, setpointRate[0]);

26
src/main/flight/gps_rescue.c
View file

@ -50,7 +50,7 @@
#include "gps_rescue.h"
#define GPS_RESCUE_MAX_YAW_RATE 100 // 100 deg/sec max yaw
#define GPS_RESCUE_MAX_YAW_RATE 360 // deg/sec max yaw rate
#define GPS_RESCUE_RATE_SCALE_DEGREES 45 // Scale the commanded yaw rate when the error is less then this angle
PG_REGISTER_WITH_RESET_TEMPLATE(gpsRescueConfig_t, gpsRescueConfig, PG_GPS_RESCUE, 0);
@ -405,7 +405,7 @@ void rescueAttainPosition()
// Very similar to maghold function on betaflight/cleanflight
void setBearing(int16_t desiredHeading)
{
float errorAngle = DECIDEGREES_TO_DEGREES(attitude.values.yaw) - desiredHeading;
float errorAngle = (attitude.values.yaw / 10.0f) - desiredHeading;
// Determine the most efficient direction to rotate
if (errorAngle <= -180) {
@ -415,20 +415,11 @@ void setBearing(int16_t desiredHeading)
}
errorAngle *= -GET_DIRECTION(rcControlsConfig()->yaw_control_reversed);
const int errorAngleSign = (errorAngle < 0) ? -1 : 1;
const float errorAngleAbs = ABS(errorAngle);
// Calculate a desired yaw rate based on a maximum limit beyond
// an error window and then scale the requested rate down inside
// the window as error approaches 0.
if (errorAngleAbs > GPS_RESCUE_RATE_SCALE_DEGREES) {
rescueYaw = GPS_RESCUE_MAX_YAW_RATE;
} else {
rescueYaw = (errorAngleAbs / GPS_RESCUE_RATE_SCALE_DEGREES) * GPS_RESCUE_MAX_YAW_RATE;
}
rescueYaw = (errorAngleAbs > GPS_RESCUE_RATE_SCALE_DEGREES) ? GPS_RESCUE_MAX_YAW_RATE : errorAngleAbs;
rescueYaw *= errorAngleSign;
rescueYaw = -constrainf(errorAngle / GPS_RESCUE_RATE_SCALE_DEGREES * GPS_RESCUE_MAX_YAW_RATE, -GPS_RESCUE_MAX_YAW_RATE, GPS_RESCUE_MAX_YAW_RATE);
}
float gpsRescueGetYawRate(void)
@ -438,16 +429,11 @@ float gpsRescueGetYawRate(void)
float gpsRescueGetThrottle(void)
{
float commandedThrottle = 0;
const int minCheck = MAX(rxConfig()->mincheck, 1000);
// Calculated a desired commanded throttle scaled from 0.0 to 1.0 for use in the mixer.
// We need to compensate for min_check since the throttle value set by gps rescue
// is based on the raw rcCommand value commanded by the pilot.
if (rescueThrottle > minCheck) {
// Calculated a desired commanded throttle scaled from 0.0 to 1.0 for use in the mixer.
commandedThrottle = (float)(rescueThrottle - minCheck) / (PWM_RANGE_MAX - minCheck);
commandedThrottle = constrainf(commandedThrottle, 0.0f, 1.0f);
}
float commandedThrottle = scaleRangef(rescueThrottle, MAX(rxConfig()->mincheck, PWM_RANGE_MIN), PWM_RANGE_MAX, 0.0f, 1.0f);
commandedThrottle = constrainf(commandedThrottle, 0.0f, 1.0f);
return commandedThrottle;
}