rusefi
/
atbetaflight
mirror of https://github.com/rusefi/atbetaflight.git
1
0
Fork 0
Code Issues Packages Projects Releases Wiki Activity

Merge pull request #406 from avoid3d/throttle-correction-comment 

Throttle correction comment
This commit is contained in:
Dominic Clifton 2015-01-22 09:44:36 +01:00
parent 12a1cc5ffe 616c40a827
commit 2c6b55bf69
12 changed files with 117 additions and 111 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

49
src/main/common/maths.c
View File

@ -92,3 +92,52 @@ int scaleRange(int x, int srcMin, int srcMax, int destMin, int destMax) {
return ((a / b) - (destMax - destMin)) + destMax;
}
// Normalize a vector
void normalizeV(struct fp_vector *src, struct fp_vector *dest)
{
float length;
length = sqrtf(src->X * src->X + src->Y * src->Y + src->Z * src->Z);
if (length != 0) {
dest->X = src->X / length;
dest->Y = src->Y / length;
dest->Z = src->Z / length;
}
}
// Rotate a vector *v by the euler angles defined by the 3-vector *delta.
void rotateV(struct fp_vector *v, fp_angles_t *delta)
{
struct fp_vector v_tmp = *v;
float mat[3][3];
float cosx, sinx, cosy, siny, cosz, sinz;
float coszcosx, sinzcosx, coszsinx, sinzsinx;
cosx = cosf(delta->angles.roll);
sinx = sinf(delta->angles.roll);
cosy = cosf(delta->angles.pitch);
siny = sinf(delta->angles.pitch);
cosz = cosf(delta->angles.yaw);
sinz = sinf(delta->angles.yaw);
coszcosx = cosz * cosx;
sinzcosx = sinz * cosx;
coszsinx = sinx * cosz;
sinzsinx = sinx * sinz;
mat[0][0] = cosz * cosy;
mat[0][1] = -cosy * sinz;
mat[0][2] = siny;
mat[1][0] = sinzcosx + (coszsinx * siny);
mat[1][1] = coszcosx - (sinzsinx * siny);
mat[1][2] = -sinx * cosy;
mat[2][0] = (sinzsinx) - (coszcosx * siny);
mat[2][1] = (coszsinx) + (sinzcosx * siny);
mat[2][2] = cosy * cosx;
v->X = v_tmp.X * mat[0][0] + v_tmp.Y * mat[1][0] + v_tmp.Z * mat[2][0];
v->Y = v_tmp.X * mat[0][1] + v_tmp.Y * mat[1][1] + v_tmp.Z * mat[2][1];
v->Z = v_tmp.X * mat[0][2] + v_tmp.Y * mat[1][2] + v_tmp.Z * mat[2][2];
}

41
src/main/common/maths.h
View File

@ -21,16 +21,10 @@
#define sq(x) ((x)*(x))
#endif
#ifdef M_PI
// M_PI should be float, but previous definition may be double
# undef M_PI
#endif
#define M_PI 3.14159265358979323846f
// Use floating point M_PI instead explicitly.
#define M_PIf 3.14159265358979323846f
#define RADX10 (M_PI / 1800.0f) // 0.001745329252f
#define RAD (M_PI / 180.0f)
#define DEG2RAD(degrees) (degrees * RAD)
#define RAD (M_PIf / 180.0f)
#define min(a, b) ((a) < (b) ? (a) : (b))
#define max(a, b) ((a) > (b) ? (a) : (b))
@ -42,6 +36,31 @@ typedef struct stdev_t
int m_n;
} stdev_t;
// Floating point 3 vector.
typedef struct fp_vector {
float X;
float Y;
float Z;
} t_fp_vector_def;
typedef union {
float A[3];
t_fp_vector_def V;
} t_fp_vector;
// Floating point Euler angles.
// Be carefull, could be either of degrees or radians.
typedef struct fp_angles {
float roll;
float pitch;
float yaw;
} fp_angles_def;
typedef union {
float raw[3];
fp_angles_def angles;
} fp_angles_t;
int32_t applyDeadband(int32_t value, int32_t deadband);
int constrain(int amt, int low, int high);
@ -54,3 +73,7 @@ float devStandardDeviation(stdev_t *dev);
float degreesToRadians(int16_t degrees);
int scaleRange(int x, int srcMin, int srcMax, int destMin, int destMax);
void normalizeV(struct fp_vector *src, struct fp_vector *dest);
void rotateV(struct fp_vector *v, fp_angles_t *delta);

2
src/main/config/config.c
View File

@ -613,7 +613,7 @@ void activateConfig(void)
imuRuntimeConfig.acc_unarmedcal = currentProfile->acc_unarmedcal;;
imuRuntimeConfig.small_angle = masterConfig.small_angle;
configureImu(
configureIMU(
&imuRuntimeConfig,
&currentProfile->pidProfile,
&currentProfile->accDeadband

2
src/main/drivers/accgyro_spi_mpu6000.c
View File

@ -319,7 +319,7 @@ bool mpu6000SpiGyroDetect(gyro_t *gyro, uint16_t lpf)
gyro->read = mpu6000SpiGyroRead;
// 16.4 dps/lsb scalefactor
gyro->scale = 1.0f / 16.4f;
//gyro->scale = (4.0f / 16.4f) * (M_PI / 180.0f) * 0.000001f;
//gyro->scale = (4.0f / 16.4f) * (M_PIf / 180.0f) * 0.000001f;
delay(100);
return true;
}

2
src/main/drivers/accgyro_spi_mpu6500.c
View File

@ -128,7 +128,7 @@ bool mpu6500SpiGyroDetect(gyro_t *gyro, uint16_t lpf)
// 16.4 dps/lsb scalefactor
gyro->scale = 1.0f / 16.4f;
//gyro->scale = (4.0f / 16.4f) * (M_PI / 180.0f) * 0.000001f;
//gyro->scale = (4.0f / 16.4f) * (M_PIf / 180.0f) * 0.000001f;
// default lpf is 42Hz
if (lpf >= 188)

22
src/main/flight/flight.h
View File

@ -61,28 +61,6 @@ typedef enum {
#define FLIGHT_DYNAMICS_INDEX_COUNT 3
typedef struct fp_vector {
float X;
float Y;
float Z;
} t_fp_vector_def;
typedef union {
float A[3];
t_fp_vector_def V;
} t_fp_vector;
typedef struct fp_angles {
float roll;
float pitch;
float yaw;
} fp_angles_def;
typedef union {
float raw[3];
fp_angles_def angles;
} fp_angles_t;
typedef struct int16_flightDynamicsTrims_s {
int16_t roll;
int16_t pitch;

91
src/main/flight/imu.c
View File

@ -79,28 +79,28 @@ imuRuntimeConfig_t *imuRuntimeConfig;
pidProfile_t *pidProfile;
accDeadband_t *accDeadband;
void configureImu(imuRuntimeConfig_t *initialImuRuntimeConfig, pidProfile_t *initialPidProfile, accDeadband_t *initialAccDeadband)
void configureIMU(imuRuntimeConfig_t *initialImuRuntimeConfig, pidProfile_t *initialPidProfile, accDeadband_t *initialAccDeadband)
{
imuRuntimeConfig = initialImuRuntimeConfig;
pidProfile = initialPidProfile;
accDeadband = initialAccDeadband;
}
void imuInit()
void initIMU()
{
smallAngle = lrintf(acc_1G * cosf(RAD * imuRuntimeConfig->small_angle));
smallAngle = lrintf(acc_1G * cosf(degreesToRadians(imuRuntimeConfig->small_angle)));
accVelScale = 9.80665f / acc_1G / 10000.0f;
gyroScaleRad = gyro.scale * (M_PI / 180.0f) * 0.000001f;
gyroScaleRad = gyro.scale * (M_PIf / 180.0f) * 0.000001f;
}
void calculateThrottleAngleScale(uint16_t throttle_correction_angle)
{
throttleAngleScale = (1800.0f / M_PI) * (900.0f / throttle_correction_angle);
throttleAngleScale = (1800.0f / M_PIf) * (900.0f / throttle_correction_angle);
}
void calculateAccZLowPassFilterRCTimeConstant(float accz_lpf_cutoff)
{
fc_acc = 0.5f / (M_PI * accz_lpf_cutoff); // calculate RC time constant used in the accZ lpf
fc_acc = 0.5f / (M_PIf * accz_lpf_cutoff); // calculate RC time constant used in the accZ lpf
}
void computeIMU(rollAndPitchTrims_t *accelerometerTrims, uint8_t mixerMode)
@ -145,56 +145,6 @@ void computeIMU(rollAndPitchTrims_t *accelerometerTrims, uint8_t mixerMode)
t_fp_vector EstG;
// Normalize a vector
void normalizeV(struct fp_vector *src, struct fp_vector *dest)
{
float length;
length = sqrtf(src->X * src->X + src->Y * src->Y + src->Z * src->Z);
if (length != 0) {
dest->X = src->X / length;
dest->Y = src->Y / length;
dest->Z = src->Z / length;
}
}
// Rotate Estimated vector(s) with small angle approximation, according to the gyro data
void rotateV(struct fp_vector *v, fp_angles_t *delta)
{
struct fp_vector v_tmp = *v;
// This does a "proper" matrix rotation using gyro deltas without small-angle approximation
float mat[3][3];
float cosx, sinx, cosy, siny, cosz, sinz;
float coszcosx, sinzcosx, coszsinx, sinzsinx;
cosx = cosf(delta->angles.roll);
sinx = sinf(delta->angles.roll);
cosy = cosf(delta->angles.pitch);
siny = sinf(delta->angles.pitch);
cosz = cosf(delta->angles.yaw);
sinz = sinf(delta->angles.yaw);
coszcosx = cosz * cosx;
sinzcosx = sinz * cosx;
coszsinx = sinx * cosz;
sinzsinx = sinx * sinz;
mat[0][0] = cosz * cosy;
mat[0][1] = -cosy * sinz;
mat[0][2] = siny;
mat[1][0] = sinzcosx + (coszsinx * siny);
mat[1][1] = coszcosx - (sinzsinx * siny);
mat[1][2] = -sinx * cosy;
mat[2][0] = (sinzsinx) - (coszcosx * siny);
mat[2][1] = (coszsinx) + (sinzcosx * siny);
mat[2][2] = cosy * cosx;
v->X = v_tmp.X * mat[0][0] + v_tmp.Y * mat[1][0] + v_tmp.Z * mat[2][0];
v->Y = v_tmp.X * mat[0][1] + v_tmp.Y * mat[1][1] + v_tmp.Z * mat[2][1];
v->Z = v_tmp.X * mat[0][2] + v_tmp.Y * mat[1][2] + v_tmp.Z * mat[2][2];
}
void accSum_reset(void)
{
accSum[0] = 0;
@ -248,7 +198,13 @@ void acc_calc(uint32_t deltaT)
accSumCount++;
}
// baseflight calculation by Luggi09 originates from arducopter
/*
* Baseflight calculation by Luggi09 originates from arducopter
* ============================================================
*
* Calculate the heading of the craft (in degrees clockwise from North)
* when given a 3-vector representing the direction of North.
*/
static int16_t calculateHeading(t_fp_vector *vec)
{
int16_t head;
@ -259,8 +215,12 @@ static int16_t calculateHeading(t_fp_vector *vec)
float sinePitch = sinf(anglerad[AI_PITCH]);
float Xh = vec->A[X] * cosinePitch + vec->A[Y] * sineRoll * sinePitch + vec->A[Z] * sinePitch * cosineRoll;
float Yh = vec->A[Y] * cosineRoll - vec->A[Z] * sineRoll;
float hd = (atan2f(Yh, Xh) * 1800.0f / M_PI + magneticDeclination) / 10.0f;
//TODO: Replace this comment with an explanation of why Yh and Xh can never simultanoeusly be zero,
// or handle the case in which they are and (atan2f(0, 0) is undefined.
float hd = (atan2f(Yh, Xh) * 1800.0f / M_PIf + magneticDeclination) / 10.0f;
head = lrintf(hd);
// Arctan returns a value in the range -180 to 180 degrees. We 'normalize' negative angles to be positive.
if (head < 0)
head += 360;
@ -318,8 +278,8 @@ static void getEstimatedAttitude(void)
// Attitude of the estimated vector
anglerad[AI_ROLL] = atan2f(EstG.V.Y, EstG.V.Z);
anglerad[AI_PITCH] = atan2f(-EstG.V.X, sqrtf(EstG.V.Y * EstG.V.Y + EstG.V.Z * EstG.V.Z));
inclination.values.rollDeciDegrees = lrintf(anglerad[AI_ROLL] * (1800.0f / M_PI));
inclination.values.pitchDeciDegrees = lrintf(anglerad[AI_PITCH] * (1800.0f / M_PI));
inclination.values.rollDeciDegrees = lrintf(anglerad[AI_ROLL] * (1800.0f / M_PIf));
inclination.values.pitchDeciDegrees = lrintf(anglerad[AI_PITCH] * (1800.0f / M_PIf));
if (sensors(SENSOR_MAG)) {
rotateV(&EstM.V, &deltaGyroAngle);
@ -338,16 +298,21 @@ static void getEstimatedAttitude(void)
acc_calc(deltaT); // rotate acc vector into earth frame
}
// correction of throttle in lateral wind,
// Correction of throttle in lateral wind.
int16_t calculateThrottleAngleCorrection(uint8_t throttle_correction_value)
{
float cosZ = EstG.V.Z / sqrtf(EstG.V.X * EstG.V.X + EstG.V.Y * EstG.V.Y + EstG.V.Z * EstG.V.Z);
if (cosZ <= 0.015f) { // we are inverted, vertical or with a small angle < 0.86 deg
/*
* Use 0 as the throttle angle correction if we are inverted, vertical or with a
* small angle < 0.86 deg
* TODO: Define this small angle in config.
*/
if (cosZ <= 0.015f) {
return 0;
}
int angle = lrintf(acosf(cosZ) * throttleAngleScale);
if (angle > 900)
angle = 900;
return lrintf(throttle_correction_value * sinf(angle / (900.0f * M_PI / 2.0f)));
return lrintf(throttle_correction_value * sinf(angle / (900.0f * M_PIf / 2.0f)));
}

2
src/main/flight/imu.h
View File

@ -30,7 +30,7 @@ typedef struct imuRuntimeConfig_s {
int8_t small_angle;
} imuRuntimeConfig_t;
void configureImu(imuRuntimeConfig_t *initialImuRuntimeConfig, pidProfile_t *initialPidProfile, accDeadband_t *initialAccDeadband);
void configureIMU(imuRuntimeConfig_t *initialImuRuntimeConfig, pidProfile_t *initialPidProfile, accDeadband_t *initialAccDeadband);
void calculateEstimatedAltitude(uint32_t currentTime);
void computeIMU(rollAndPitchTrims_t *accelerometerTrims, uint8_t mixerMode);

2
src/main/flight/navigation.c
View File

@ -163,7 +163,7 @@ static int32_t get_D(int32_t input, float *dt, PID *pid, PID_PARAM *pid_param)
// Low pass filter cut frequency for derivative calculation
// Set to "1 / ( 2 * PI * gps_lpf )
float pidFilter = (1.0f / (2.0f * M_PI * (float)gpsProfile->gps_lpf));
float pidFilter = (1.0f / (2.0f * M_PIf * (float)gpsProfile->gps_lpf));
// discrete low pass filter, cuts out the
// high frequency noise that can drive the controller crazy
pid->derivative = pid->last_derivative + (*dt / (pidFilter + *dt)) * (pid->derivative - pid->last_derivative);

4
src/main/main.c
View File

@ -104,7 +104,7 @@ void beepcodeInit(failsafe_t *initialFailsafe);
void gpsInit(serialConfig_t *serialConfig, gpsConfig_t *initialGpsConfig);
void navigationInit(gpsProfile_t *initialGpsProfile, pidProfile_t *pidProfile);
bool sensorsAutodetect(sensorAlignmentConfig_t *sensorAlignmentConfig, uint16_t gyroLpf, uint8_t accHardwareToUse, int8_t magHardwareToUse, int16_t magDeclinationFromConfig);
void imuInit(void);
void initIMU(void);
void displayInit(rxConfig_t *intialRxConfig);
void ledStripInit(ledConfig_t *ledConfigsToUse, hsvColor_t *colorsToUse, failsafe_t* failsafeToUse);
void loop(void);
@ -264,7 +264,7 @@ void init(void)
LED0_OFF;
LED1_OFF;
imuInit();
initIMU();
mixerInit(masterConfig.mixerMode, masterConfig.customMixer);
#ifdef MAG

1
src/test/Makefile
View File

@ -149,6 +149,7 @@ flight_imu_unittest : \
$(OBJECT_DIR)/flight/imu.o \
$(OBJECT_DIR)/flight/altitudehold.o \
$(OBJECT_DIR)/flight_imu_unittest.o \
$(OBJECT_DIR)/common/maths.o \
$(OBJECT_DIR)/gtest_main.a
$(CXX) $(CXX_FLAGS) -lpthread $^ -o $(OBJECT_DIR)/$@

10
src/test/unit/flight_imu_unittest.cc
View File

@ -85,18 +85,8 @@ void updateAccelerationReadings(rollAndPitchTrims_t *rollAndPitchTrims)
UNUSED(rollAndPitchTrims);
}
int32_t applyDeadband(int32_t, int32_t) { return 0; }
uint32_t micros(void) { return 0; }
bool isBaroCalibrationComplete(void) { return true; }
void performBaroCalibrationCycle(void) {}
int32_t baroCalculateAltitude(void) { return 0; }
int constrain(int amt, int low, int high)
{
UNUSED(amt);
UNUSED(low);
UNUSED(high);
return 0;
}
}