diff --git a/src/main/common/maths.c b/src/main/common/maths.c index 716d83e49..5ed16f916 100644 --- a/src/main/common/maths.c +++ b/src/main/common/maths.c @@ -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]; +} + diff --git a/src/main/common/maths.h b/src/main/common/maths.h index a4014b730..d65b87df2 100644 --- a/src/main/common/maths.h +++ b/src/main/common/maths.h @@ -36,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); @@ -48,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); diff --git a/src/main/config/config.c b/src/main/config/config.c index f582a8efd..17c656ddd 100644 --- a/src/main/config/config.c +++ b/src/main/config/config.c @@ -613,7 +613,7 @@ void activateConfig(void) imuRuntimeConfig.acc_unarmedcal = currentProfile->acc_unarmedcal;; imuRuntimeConfig.small_angle = masterConfig.small_angle; - configureImu( + configureIMU( &imuRuntimeConfig, ¤tProfile->pidProfile, ¤tProfile->accDeadband diff --git a/src/main/flight/flight.h b/src/main/flight/flight.h index ed96b43b1..95877badf 100644 --- a/src/main/flight/flight.h +++ b/src/main/flight/flight.h @@ -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; diff --git a/src/main/flight/imu.c b/src/main/flight/imu.c index e989308d2..f0a4535a0 100644 --- a/src/main/flight/imu.c +++ b/src/main/flight/imu.c @@ -79,14 +79,14 @@ 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(degreesToRadians(imuRuntimeConfig->small_angle))); accVelScale = 9.80665f / acc_1G / 10000.0f; @@ -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; + //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; @@ -338,12 +298,17 @@ 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); diff --git a/src/main/flight/imu.h b/src/main/flight/imu.h index de5ac89f0..552093508 100644 --- a/src/main/flight/imu.h +++ b/src/main/flight/imu.h @@ -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); diff --git a/src/main/main.c b/src/main/main.c index d2584fdba..a30a6bfe9 100644 --- a/src/main/main.c +++ b/src/main/main.c @@ -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 diff --git a/src/test/Makefile b/src/test/Makefile index 8e376ea51..5001c5723 100644 --- a/src/test/Makefile +++ b/src/test/Makefile @@ -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)/$@ diff --git a/src/test/unit/flight_imu_unittest.cc b/src/test/unit/flight_imu_unittest.cc index 67425aa63..44aada068 100644 --- a/src/test/unit/flight_imu_unittest.cc +++ b/src/test/unit/flight_imu_unittest.cc @@ -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; -} - }