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

new rcCommand scaling to desired rotation rates

Browse source
This commit is contained in:
Pawel Spychalski (DzikuVx) 2016-05-13 14:47:35 +02:00
parent 058c2bf72e
commit 122c3f5a29
3 changed files with 13 additions and 2 deletions
Download patch file Download diff file Expand all files Collapse all files

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);

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);
}
#define FP_PID_RATE_P_MULTIPLIER 40.0f // betaflight - 40.0