diff --git a/obj/betaflight_CC3D.hex b/obj/betaflight_CC3D.hex old mode 100644 new mode 100755 diff --git a/obj/betaflight_NAZE.hex b/obj/betaflight_NAZE.hex old mode 100644 new mode 100755 diff --git a/src/main/config/config.c b/src/main/config/config.c index d583ee7673..03cabea721 100755 --- a/src/main/config/config.c +++ b/src/main/config/config.c @@ -397,8 +397,8 @@ static void resetConf(void) // global settings masterConfig.current_profile_index = 0; // default profile - masterConfig.gyro_cmpf_factor = 600; // default MWC - masterConfig.gyro_cmpfm_factor = 250; // default MWC + masterConfig.dcm_kp = 10000; // 1.0 * 10000 + masterConfig.dcm_ki = 30; // 0.003 * 10000 resetAccelerometerTrims(&masterConfig.accZero); @@ -484,15 +484,14 @@ static void resetConf(void) resetRollAndPitchTrims(¤tProfile->accelerometerTrims); currentProfile->mag_declination = 0; - currentProfile->acc_lpf_factor = 4; + currentProfile->acc_cut_hz = 15; currentProfile->accz_lpf_cutoff = 5.0f; currentProfile->accDeadband.xy = 40; currentProfile->accDeadband.z = 40; + currentProfile->acc_unarmedcal = 1; resetBarometerConfig(¤tProfile->barometerConfig); - currentProfile->acc_unarmedcal = 1; - // Radio parseRcChannels("AETR1234", &masterConfig.rxConfig); @@ -719,10 +718,10 @@ void activateConfig(void) &masterConfig.rxConfig ); - imuRuntimeConfig.gyro_cmpf_factor = masterConfig.gyro_cmpf_factor; - imuRuntimeConfig.gyro_cmpfm_factor = masterConfig.gyro_cmpfm_factor; - imuRuntimeConfig.acc_lpf_factor = currentProfile->acc_lpf_factor; - imuRuntimeConfig.acc_unarmedcal = currentProfile->acc_unarmedcal;; + imuRuntimeConfig.dcm_kp = masterConfig.dcm_kp / 10000.0f; + imuRuntimeConfig.dcm_ki = masterConfig.dcm_ki / 10000.0f; + imuRuntimeConfig.acc_cut_hz = currentProfile->acc_cut_hz; + imuRuntimeConfig.acc_unarmedcal = currentProfile->acc_unarmedcal; imuRuntimeConfig.small_angle = masterConfig.small_angle; imuConfigure( diff --git a/src/main/config/config_master.h b/src/main/config/config_master.h index 4e87d277f5..24813a1d27 100644 --- a/src/main/config/config_master.h +++ b/src/main/config/config_master.h @@ -48,8 +48,8 @@ typedef struct master_t { uint8_t acc_hardware; // Which acc hardware to use on boards with more than one device uint8_t acc_for_fast_looptime; // shorten acc processing time by using 1 out of 9 samples. For combination with fast looptimes. uint16_t gyro_lpf; // gyro LPF setting - values are driver specific, in case of invalid number, a reasonable default ~30-40HZ is chosen. - uint16_t gyro_cmpf_factor; // Set the Gyro Weight for Gyro/Acc complementary filter. Increasing this value would reduce and delay Acc influence on the output of the filter. - uint16_t gyro_cmpfm_factor; // Set the Gyro Weight for Gyro/Magnetometer complementary filter. Increasing this value would reduce and delay Magnetometer influence on the output of the filter + uint16_t dcm_kp; // DCM filter proportional gain ( x 10000) + uint16_t dcm_ki; // DCM filter integral gain ( x 10000) gyroConfig_t gyroConfig; diff --git a/src/main/config/config_profile.h b/src/main/config/config_profile.h index cd68f0aaa2..f102d0c115 100644 --- a/src/main/config/config_profile.h +++ b/src/main/config/config_profile.h @@ -28,7 +28,7 @@ typedef struct profile_s { rollAndPitchTrims_t accelerometerTrims; // accelerometer trim // sensor-related stuff - uint8_t acc_lpf_factor; // Set the Low Pass Filter factor for ACC. Increasing this value would reduce ACC noise (visible in GUI), but would increase ACC lag time. Zero = no filter + uint8_t acc_cut_hz; // Set the Low Pass Filter factor for ACC. Reducing this value would reduce ACC noise (visible in GUI), but would increase ACC lag time. Zero = no filter float accz_lpf_cutoff; // cutoff frequency for the low pass filter used on the acc z-axis for althold in Hz accDeadband_t accDeadband; diff --git a/src/main/flight/altitudehold.c b/src/main/flight/altitudehold.c index 2a0162b8bd..22610c43d0 100644 --- a/src/main/flight/altitudehold.c +++ b/src/main/flight/altitudehold.c @@ -168,9 +168,9 @@ void updateSonarAltHoldState(void) } } -bool isThrustFacingDownwards(rollAndPitchInclination_t *inclination) +bool isThrustFacingDownwards(attitudeEulerAngles_t *attitude) { - return ABS(inclination->values.rollDeciDegrees) < DEGREES_80_IN_DECIDEGREES && ABS(inclination->values.pitchDeciDegrees) < DEGREES_80_IN_DECIDEGREES; + return ABS(attitude->values.roll) < DEGREES_80_IN_DECIDEGREES && ABS(attitude->values.pitch) < DEGREES_80_IN_DECIDEGREES; } /* @@ -178,9 +178,9 @@ bool isThrustFacingDownwards(rollAndPitchInclination_t *inclination) * //TODO: Fix this up. We could either actually return the angle between 'down' and the normal of the craft * (my best interpretation of scalar 'tiltAngle') or rename the function. */ -int16_t calculateTiltAngle(rollAndPitchInclination_t *inclination) +int16_t calculateTiltAngle(attitudeEulerAngles_t *attitude) { - return MAX(ABS(inclination->values.rollDeciDegrees), ABS(inclination->values.pitchDeciDegrees)); + return MAX(ABS(attitude->values.roll), ABS(attitude->values.pitch)); } int32_t calculateAltHoldThrottleAdjustment(int32_t vel_tmp, float accZ_tmp, float accZ_old) @@ -189,7 +189,7 @@ int32_t calculateAltHoldThrottleAdjustment(int32_t vel_tmp, float accZ_tmp, floa int32_t error; int32_t setVel; - if (!isThrustFacingDownwards(&inclination)) { + if (!isThrustFacingDownwards(&attitude)) { return result; } @@ -260,7 +260,7 @@ void calculateEstimatedAltitude(uint32_t currentTime) #endif #ifdef SONAR - tiltAngle = calculateTiltAngle(&inclination); + tiltAngle = calculateTiltAngle(&attitude); sonarAlt = sonarRead(); sonarAlt = sonarCalculateAltitude(sonarAlt, tiltAngle); #endif diff --git a/src/main/flight/imu.c b/src/main/flight/imu.c index 73b7630d89..d486f6fba2 100644 --- a/src/main/flight/imu.c +++ b/src/main/flight/imu.c @@ -23,6 +23,7 @@ #include #include "common/maths.h" +#include "common/filter.h" #include "platform.h" #include "debug.h" @@ -45,6 +46,8 @@ #include "flight/pid.h" #include "flight/imu.h" +#include "io/gps.h" + #include "config/runtime_config.h" //#define DEBUG_IMU @@ -56,26 +59,50 @@ int32_t accSum[XYZ_AXIS_COUNT]; uint32_t accTimeSum = 0; // keep track for integration of acc int accSumCount = 0; float accVelScale; - -int16_t smallAngle = 0; - -float throttleAngleScale; float fc_acc; +float throttleAngleScale; +float smallAngleCosZ = 0; + float magneticDeclination = 0.0f; // calculated at startup from config -float gyroScaleRad; - - -rollAndPitchInclination_t inclination = { { 0, 0 } }; // absolute angle inclination in multiple of 0.1 degree 180 deg = 1800 -static float anglerad[2] = { 0.0f, 0.0f }; // absolute angle inclination in radians static imuRuntimeConfig_t *imuRuntimeConfig; static pidProfile_t *pidProfile; static accDeadband_t *accDeadband; -static accProcessor_t accProc; -static void qAccProcessingStateMachine(rollAndPitchTrims_t *accelerometerTrims); +static float q0 = 1.0f, q1 = 0.0f, q2 = 0.0f, q3 = 0.0f; // quaternion of sensor frame relative to earth frame +static float rMat[3][3]; + +attitudeEulerAngles_t attitude = { { 0, 0, 0 } }; // absolute angle inclination in multiple of 0.1 degree 180 deg = 1800 + +static float gyroScale; + +static void imuCompureRotationMatrix(void) +{ + float q1q1 = q1 * q1; + float q2q2 = q2 * q2; + float q3q3 = q3 * q3; + + float q0q1 = q0 * q1; + float q0q2 = q0 * q2; + float q0q3 = q0 * q3; + float q1q2 = q1 * q2; + float q1q3 = q1 * q3; + float q2q3 = q2 * q3; + + rMat[0][0] = 1.0f - 2.0f * q2q2 - 2.0f * q3q3; + rMat[0][1] = 2.0f * (q1q2 + -q0q3); + rMat[0][2] = 2.0f * (q1q3 - -q0q2); + + rMat[1][0] = 2.0f * (q1q2 - -q0q3); + rMat[1][1] = 1.0f - 2.0f * q1q1 - 2.0f * q3q3; + rMat[1][2] = 2.0f * (q2q3 + -q0q1); + + rMat[2][0] = 2.0f * (q1q3 + -q0q2); + rMat[2][1] = 2.0f * (q2q3 - -q0q1); + rMat[2][2] = 1.0f - 2.0f * q1q1 - 2.0f * q2q2; +} void imuConfigure( imuRuntimeConfig_t *initialImuRuntimeConfig, @@ -94,9 +121,11 @@ void imuConfigure( void imuInit(void) { - smallAngle = lrintf(acc_1G * cos_approx(degreesToRadians(imuRuntimeConfig->small_angle))); + smallAngleCosZ = cos_approx(degreesToRadians(imuRuntimeConfig->small_angle)); + gyroScale = gyro.scale * (M_PIf / 180.0f); // gyro output scaled to rad per second accVelScale = 9.80665f / acc_1G / 10000.0f; - gyroScaleRad = gyro.scale * (M_PIf / 180.0f) * 0.000001f; + + imuCompureRotationMatrix(); } float calculateThrottleAngleScale(uint16_t throttle_correction_angle) @@ -112,23 +141,6 @@ float calculateAccZLowPassFilterRCTimeConstant(float accz_lpf_cutoff) return 0.5f / (M_PIf * accz_lpf_cutoff); } -// ************************************************** -// Simplified IMU based on "Complementary Filter" -// Inspired by http://starlino.com/imu_guide.html -// -// adapted by ziss_dm : http://www.multiwii.com/forum/viewtopic.php?f=8&t=198 -// -// The following ideas was used in this project: -// 1) Rotation matrix: http://en.wikipedia.org/wiki/Rotation_matrix -// -// Currently Magnetometer uses separate CF which is used only -// for heading approximation. -// -// ************************************************** - - -t_fp_vector EstG; - void imuResetAccelerationSum(void) { accSum[0] = 0; @@ -138,88 +150,264 @@ void imuResetAccelerationSum(void) accTimeSum = 0; } -/* -* Baseflight calculation by Luggi09 originates from arducopter -* ============================================================ -* This function rotates magnetic vector to cancel actual yaw and -* pitch of craft. Then it computes it's direction in X/Y plane. -* This value is returned as compass heading, value is 0-360 degrees. -* -* Note that Earth's magnetic field is not parallel with ground unless -* you are near equator. Its inclination is considerable, >60 degrees -* towards ground in most of Europe. -* -* First we consider it in 2D: -* -* An example, the vector <1, 1> would be turned into the heading -* 45 degrees, representing it's angle clockwise from north. -* -* ***************** * -* * | <1,1> * -* * | / * -* * | / * -* * |/ * -* * * * -* * * -* * * -* * * -* * * -* ******************* -* -* //TODO: Add explanation for how it uses the Z dimension. -*/ -int16_t imuCalculateHeading(t_fp_vector *vec) +void imuTransformVectorBodyToEarth(t_fp_vector * v) { - int16_t head; + float x,y,z; - float cosineRoll = cos_approx(anglerad[AI_ROLL]); - float sineRoll = sin_approx(anglerad[AI_ROLL]); - float cosinePitch = cos_approx(anglerad[AI_PITCH]); - float sinePitch = sin_approx(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 = (atan2_approx(Yh, Xh) * 1800.0f / M_PIf + magneticDeclination) / 10.0f; - head = lrintf(hd); + /* From body frame to earth frame */ + x = rMat[0][0] * v->V.X + rMat[0][1] * v->V.Y + rMat[0][2] * v->V.Z; + y = rMat[1][0] * v->V.X + rMat[1][1] * v->V.Y + rMat[1][2] * v->V.Z; + z = rMat[2][0] * v->V.X + rMat[2][1] * v->V.Y + rMat[2][2] * v->V.Z; - // Arctan returns a value in the range -180 to 180 degrees. We 'normalize' negative angles to be positive. - if (head < 0) - head += 360; - - return head; + v->V.X = x; + v->V.Y = -y; + v->V.Z = z; } -#if 0 -void imuUpdate(rollAndPitchTrims_t *accelerometerTrims, uint8_t imuUpdateSensors) +// rotate acc into Earth frame and calculate acceleration in it +void imuCalculateAcceleration(uint32_t deltaT) { -#if defined(NAZE) || defined(DEBUG_IMU_SPEED) - uint32_t time = micros(); -#endif - if (imuUpdateSensors == ONLY_GYRO || imuUpdateSensors == ACC_AND_GYRO) { - gyroUpdate(); -#ifdef DEBUG_IMU_SPEED - debug[0] = micros() - time; // gyro read time -#endif + static int32_t accZoffset = 0; + static float accz_smooth = 0; + float dT; + t_fp_vector accel_ned; + + // deltaT is measured in us ticks + dT = (float)deltaT * 1e-6f; + + accel_ned.V.X = accSmooth[0]; + accel_ned.V.Y = accSmooth[1]; + accel_ned.V.Z = accSmooth[2]; + + imuTransformVectorBodyToEarth(&accel_ned); + + if (imuRuntimeConfig->acc_unarmedcal == 1) { + if (!ARMING_FLAG(ARMED)) { + accZoffset -= accZoffset / 64; + accZoffset += accel_ned.V.Z; + } + accel_ned.V.Z -= accZoffset / 64; // compensate for gravitation on z-axis + } else + accel_ned.V.Z -= acc_1G; + + accz_smooth = accz_smooth + (dT / (fc_acc + dT)) * (accel_ned.V.Z - accz_smooth); // low pass filter + + // apply Deadband to reduce integration drift and vibration influence + accSum[X] += applyDeadband(lrintf(accel_ned.V.X), accDeadband->xy); + accSum[Y] += applyDeadband(lrintf(accel_ned.V.Y), accDeadband->xy); + accSum[Z] += applyDeadband(lrintf(accz_smooth), accDeadband->z); + + // sum up Values for later integration to get velocity and distance + accTimeSum += deltaT; + accSumCount++; +} + +static float invSqrt(float x) +{ + return 1.0f / sqrtf(x); +} + +static bool imuUseFastGains(void) +{ + return !ARMING_FLAG(ARMED) && millis() < 20000; +} + +static void imuMahonyAHRSupdate(float dt, float gx, float gy, float gz, + bool useAcc, float ax, float ay, float az, + bool useMag, float mx, float my, float mz, + bool useYaw, float yawError) +{ + static float integralFBx = 0.0f, integralFBy = 0.0f, integralFBz = 0.0f; // integral error terms scaled by Ki + float recipNorm; + float hx, hy, bx; + float ex = 0, ey = 0, ez = 0; + float qa, qb, qc; + + // Use raw heading error (from GPS or whatever else) + if (useYaw) { + while (yawError > M_PIf) yawError -= (2.0f * M_PIf); + while (yawError < -M_PIf) yawError += (2.0f * M_PIf); + + ez += sin_approx(yawError / 2.0f); } - if (sensors(SENSOR_ACC) && (!imuUpdateSensors == ONLY_GYRO)) { -#ifdef DEBUG_IMU_SPEED - time = micros(); -#endif - qAccProcessingStateMachine(accelerometerTrims); + + // Use measured magnetic field vector + recipNorm = mx * mx + my * my + mz * mz; + if (useMag && recipNorm > 0.01f) { + // Normalise magnetometer measurement + recipNorm = invSqrt(recipNorm); + mx *= recipNorm; + my *= recipNorm; + mz *= recipNorm; + + // For magnetometer correction we make an assumption that magnetic field is perpendicular to gravity (ignore Z-component in EF). + // This way magnetic field will only affect heading and wont mess roll/pitch angles + + // (hx; hy; 0) - measured mag field vector in EF (assuming Z-component is zero) + // (bx; 0; 0) - reference mag field vector heading due North in EF (assuming Z-component is zero) + hx = rMat[0][0] * mx + rMat[0][1] * my + rMat[0][2] * mz; + hy = rMat[1][0] * mx + rMat[1][1] * my + rMat[1][2] * mz; + bx = sqrtf(hx * hx + hy * hy); + + // magnetometer error is cross product between estimated magnetic north and measured magnetic north (calculated in EF) + float ez_ef = -(hy * bx); + + // Rotate mag error vector back to BF and accumulate + ex += rMat[2][0] * ez_ef; + ey += rMat[2][1] * ez_ef; + ez += rMat[2][2] * ez_ef; + } + + // Use measured acceleration vector + recipNorm = ax * ax + ay * ay + az * az; + if (useAcc && recipNorm > 0.01f) { + // Normalise accelerometer measurement + recipNorm = invSqrt(recipNorm); + ax *= recipNorm; + ay *= recipNorm; + az *= recipNorm; + + // Error is sum of cross product between estimated direction and measured direction of gravity + ex += (ay * rMat[2][2] - az * rMat[2][1]); + ey += (az * rMat[2][0] - ax * rMat[2][2]); + ez += (ax * rMat[2][1] - ay * rMat[2][0]); + } + + // Compute and apply integral feedback if enabled + if(imuRuntimeConfig->dcm_ki > 0.0f) { + float dcmKiGain = imuRuntimeConfig->dcm_ki; + integralFBx += dcmKiGain * ex * dt; // integral error scaled by Ki + integralFBy += dcmKiGain * ey * dt; + integralFBz += dcmKiGain * ez * dt; + } + else { + integralFBx = 0.0f; // prevent integral windup + integralFBy = 0.0f; + integralFBz = 0.0f; + } + + // Calculate kP gain. If we are acquiring initial attitude (not armed and within 20 sec from powerup) scale the kP to converge faster + float dcmKpGain = imuRuntimeConfig->dcm_kp; + if (imuUseFastGains()) { + dcmKpGain *= 10; + } + + // Apply proportional and integral feedback + gx += dcmKpGain * ex + integralFBx; + gy += dcmKpGain * ey + integralFBy; + gz += dcmKpGain * ez + integralFBz; + + // Integrate rate of change of quaternion + gx *= (0.5f * dt); + gy *= (0.5f * dt); + gz *= (0.5f * dt); + + qa = q0; + qb = q1; + qc = q2; + q0 += (-qb * gx - qc * gy - q3 * gz); + q1 += (qa * gx + qc * gz - q3 * gy); + q2 += (qa * gy - qb * gz + q3 * gx); + q3 += (qa * gz + qb * gy - qc * gx); + + // Normalise quaternion + recipNorm = invSqrt(q0 * q0 + q1 * q1 + q2 * q2 + q3 * q3); + q0 *= recipNorm; + q1 *= recipNorm; + q2 *= recipNorm; + q3 *= recipNorm; + + // Pre-compute rotation matrix from quaternion + imuCompureRotationMatrix(); +} + +static void imuUpdateEulerAngles(void) +{ + /* Compute pitch/roll angles */ + attitude.values.roll = lrintf(atan2f(rMat[2][1], rMat[2][2]) * (1800.0f / M_PIf)); + attitude.values.pitch = lrintf(((0.5f * M_PIf) - acosf(-rMat[2][0])) * (1800.0f / M_PIf)); + attitude.values.yaw = lrintf((-atan2f(rMat[1][0], rMat[0][0]) * (1800.0f / M_PIf) + magneticDeclination)); + + if (attitude.values.yaw < 0) + attitude.values.yaw += 3600; + + /* Update small angle state */ + if (rMat[2][2] > smallAngleCosZ) { + ENABLE_STATE(SMALL_ANGLE); } else { - accADC[X] = 0; - accADC[Y] = 0; - accADC[Z] = 0; + DISABLE_STATE(SMALL_ANGLE); } -#ifdef DEBUG_IMU_SPEED - debug[1] = micros() - time; // acc read time - if (imuUpdateSensors == ACC_AND_GYRO) { - debug[2] = debug[0] + debug[1]; // gyro + acc read time - } -#endif } -#else + +static bool imuIsAccelerometerHealthy(void) +{ + int32_t axis; + int32_t accMagnitude = 0; + + for (axis = 0; axis < 3; axis++) { + accMagnitude += (int32_t)accADC[axis] * accADC[axis]; + } + + accMagnitude = accMagnitude * 100 / ((int32_t)acc_1G * acc_1G); + + // Accept accel readings only in range 0.85g - 1.15g + return (72 < accMagnitude) && (accMagnitude < 133); +} + +static bool isMagnetometerHealthy(void) +{ + return (magADC[X] != 0) && (magADC[Y] != 0) && (magADC[Z] != 0); +} + +static void imuCalculateEstimatedAttitude(void) +{ + static filterStatePt1_t accLPFState[3]; + static uint32_t previousIMUUpdateTime; + float rawYawError = 0; + int32_t axis; + bool useAcc = false; + bool useMag = false; + bool useYaw = false; + + uint32_t currentTime = micros(); + uint32_t deltaT = currentTime - previousIMUUpdateTime; + previousIMUUpdateTime = currentTime; + + // Smooth and use only valid accelerometer readings + if (imuIsAccelerometerHealthy()) { + // If reading is considered valid - apply filter + for (axis = 0; axis < 3; axis++) { + if (imuRuntimeConfig->acc_cut_hz > 0) { + accSmooth[axis] = filterApplyPt1(accADC[axis], &accLPFState[axis], imuRuntimeConfig->acc_cut_hz, deltaT * 1e-6); + } else { + accSmooth[axis] = accADC[axis]; + } + } + + useAcc = true; + } + + if (sensors(SENSOR_MAG) && isMagnetometerHealthy()) { + useMag = true; + } +#if defined(GPS) + else if (STATE(FIXED_WING) && sensors(SENSOR_GPS) && STATE(GPS_FIX) && GPS_numSat >= 5 && GPS_speed >= 300) { + // In case of a fixed-wing aircraft we can use GPS course over ground to correct heading + rawYawError = DECIDEGREES_TO_RADIANS(GPS_ground_course - attitude.values.yaw); + useYaw = true; + } +#endif + + imuMahonyAHRSupdate(deltaT * 1e-6f, + gyroADC[X] * gyroScale, gyroADC[Y] * gyroScale, gyroADC[Z] * gyroScale, + useAcc, accSmooth[X], accSmooth[Y], accSmooth[Z], + useMag, magADC[X], magADC[Y], magADC[Z], + useYaw, rawYawError); + + imuUpdateEulerAngles(); + + imuCalculateAcceleration(deltaT); // rotate acc vector into earth frame +} void imuUpdate(rollAndPitchTrims_t *accelerometerTrims, uint8_t imuUpdateSensors) { @@ -236,7 +424,8 @@ void imuUpdate(rollAndPitchTrims_t *accelerometerTrims, uint8_t imuUpdateSensors #ifdef DEBUG_IMU_SPEED time = micros(); #endif - qAccProcessingStateMachine(accelerometerTrims); + updateAccelerationReadings(accelerometerTrims); + imuCalculateEstimatedAttitude(); } else { accADC[X] = 0; accADC[Y] = 0; @@ -250,565 +439,18 @@ void imuUpdate(rollAndPitchTrims_t *accelerometerTrims, uint8_t imuUpdateSensors #endif } -#endif - 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); - /* * 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) { + if (rMat[2][2] <= 0.015f) { return 0; } - int angle = lrintf(acos_approx(cosZ) * throttleAngleScale); + int angle = lrintf(acosf(rMat[2][2]) * throttleAngleScale); if (angle > 900) angle = 900; return lrintf(throttle_correction_value * sin_approx(angle / (900.0f * M_PIf / 2.0f))); } - -// WITHOUT -//arm - none - eabi - size . / obj / main / cleanflight_CC3D.elf -//text data bss dec hex filename -//116324 376 12640 129340 1f93c . / obj / main / cleanflight_CC3D.elf - -////////////////////////////////////////////////////////////////////// -// 4D Quaternion / 3D Vector Math -//arm - none - eabi - size . / obj / main / cleanflight_CC3D.elf -//text data bss dec hex filename -//116284 364 12636 129284 1f904 . / obj / main / cleanflight_CC3D.elf - -typedef struct v3_s -{ - float x; - float y; - float z; -} v3_t; - -const v3_t V0 = { .x = 0.0f, .y = 0.0f, .z = 0.0f }; -const v3_t VX = { .x = 1.0f, .y = 0.0f, .z = 0.0f }; -const v3_t VY = { .x = 0.0f, .y = 1.0f, .z = 0.0f }; -const v3_t VZ = { .x = 0.0f, .y = 0.0f, .z = 1.0f }; - -typedef struct q4_s -{ - float w; - float x; - float y; - float z; -} q4_t; - -const q4_t Q0 = { .w = 1.0f, .x = 0.0f, .y = 0.0f, .z = 0.0f }; - -void MulQQ(const q4_t *a, const q4_t *b, q4_t *o) -{ - q4_t r; - r.w = a->w * b->w - a->x * b->x - a->y * b->y - a->z * b->z; - r.x = a->w * b->x + a->z * b->y - a->y * b->z + a->x * b->w; - r.y = a->w * b->y + a->x * b->z + a->y * b->w - a->z * b->x; - r.z = a->y * b->x - a->x * b->y + a->w * b->z + a->z * b->w; - *o = r; -} - -void MulQV(const q4_t *a, const v3_t *b, q4_t *o) -{ - q4_t r; - r.w = -a->x * b->x - a->y * b->y - a->z * b->z; - r.x = a->w * b->x + a->z * b->y - a->y * b->z; - r.y = a->w * b->y + a->x * b->z - a->z * b->x; - r.z = a->y * b->x - a->x * b->y + a->w * b->z; - *o = r; -} - -void MulQF(const q4_t *a, const float b, q4_t *o) -{ - q4_t r; - r.w = a->w * b; - r.x = a->x * b; - r.y = a->y * b; - r.z = a->z * b; - *o = r; -} - -void MulVF(const v3_t *a, const float b, v3_t *o) -{ - v3_t r; - r.x = a->x * b; - r.y = a->y * b; - r.z = a->z * b; - *o = r; -} - -void SumQQ(const q4_t *a, const q4_t *b, q4_t *o) -{ - q4_t r; - r.w = a->w + b->w; - r.x = a->x + b->x; - r.y = a->y + b->y; - r.z = a->z + b->z; - *o = r; -} - - -void SumVV(const v3_t *a, const v3_t *b, v3_t *o) -{ - v3_t r; - r.x = a->x + b->x; - r.y = a->y + b->y; - r.z = a->z + b->z; - *o = r; -} - -void SubQQ(const q4_t *a, const q4_t *b, q4_t *o) -{ - q4_t r; - r.w = a->w - b->w; - r.x = a->x - b->x; - r.y = a->y - b->y; - r.z = a->z - b->z; - *o = r; -} - - -void SubVV(const v3_t *a, const v3_t *b, v3_t *o) -{ - v3_t r; - r.x = a->x - b->x; - r.y = a->y - b->y; - r.z = a->z - b->z; - *o = r; -} - -void CrossQQ(const q4_t *a, const q4_t *b, q4_t *o) -{ - q4_t r; - r.w = 0.0f; - r.x = a->y * b->z - a->z * b->y; - r.y = a->z * b->x - a->x * b->z; - r.z = a->x * b->y - a->y * b->x; - *o = r; -} - -void CrossVV(const v3_t *a, const v3_t *b, v3_t *o) -{ - v3_t r; - r.x = a->y * b->z - a->z * b->y; - r.y = a->z * b->x - a->x * b->z; - r.z = a->x * b->y - a->y * b->x; - *o = r; -} - -float DotQQ(const q4_t *a, const q4_t *b) -{ - return a->w * b->w + a->x * b->x + a->y * b->y + a->z * b->z; -} - -float DotVV(const v3_t *a, const v3_t *b) -{ - return a->x * b->x + a->y * b->y + a->z * b->z; -} - -float Mag2Q(const q4_t *a) // magnitude squared -{ - return a->w*a->w + a->x*a->x + a->y*a->y + a->z*a->z; -} - -#define MagQ(a) sqrtf(Mag2Q(a)) - -float Mag2V(const v3_t *a) // magnitude squared -{ - return a->x*a->x + a->y*a->y + a->z*a->z; // TODO: optimize for unit vectors (m2 nearly 1.0) -} - -#define MagV(a) sqrtf(Mag2V(a)) - -void NormQ(const q4_t *a, q4_t *o) -{ - q4_t r; - MulQF(a, 1 / MagQ(a), &r); - *o = r; -} - -void NormV(const v3_t *a, v3_t *o) -{ - v3_t r; - float m = MagV(a); - MulVF(a, 1 / m, &r); // TODO: m nearly 0 - *o = r; -} - -void ConjQ(const q4_t *a, q4_t *o) -{ - q4_t r; - r.w = a->w; - r.x = -a->x; - r.y = -a->y; - r.z = -a->z; - *o = r; -} - -void RotateVQ(const v3_t *v, const q4_t *q, v3_t *o) //Vector rotated by a Quaternion(matches V^ = V * Matrix) -{ - // v + 2 * r X(r X v + q.w*v) --https://en.wikipedia.org/wiki/Quaternions_and_spatial_rotation#Performance_comparisons - // vector r is the three imaginary coefficients of quaternion q - - v3_t r2_rv_vw; - { - // reverse signs to change direction of rotation - v3_t r = { .x = -q->x, .y = -q->y, .z = -q->z }; - v3_t r2; - SumVV(&r, &r, &r2); - - v3_t rv_vw; - { - v3_t vw; - MulVF(v, q->w, &vw); - v3_t rv; - CrossVV(&r, v, &rv); - SumVV(&rv, &vw, &rv_vw); - } - CrossVV(&r2, &rv_vw, &r2_rv_vw); - } - SumVV(v, &r2_rv_vw, o); -} - -void quaternion_approx(const v3_t *w, q4_t *o) // (angle vector[rad]) --Small angle approximation -{ - q4_t r; - r.x = w->x / 2; - r.y = w->y / 2; - r.z = w->z / 2; - r.w = 1.0f - (0.5f * ((r.x * r.x) + (r.y * r.y) + (r.z * r.z))); - *o = r; -} - -#if 0 -void quaternion(const v3_t *w, q4_t *o) // (angle vector[rad]) --Large Rotation Quaternion -{ - float m = MagV(w); - if (m == 0.0f) - { - *o = Q0; - } - else - { - q4_t r; - float t2 = m * 0.5f; // # rotation angle divided by 2 - float sm = sin(t2) / m; // # computation minimization - r.x = w->x * sm; - r.y = w->y * sm; - r.z = w->z * sm; - r.w = cos(t2); - *o = r; - } -} -#else -# define quaternion(w,o) quaternion_approx(w,o) // I think we can get away with the approximation -// TODO - try usining sin_approx, cos_approx -#endif - -typedef struct rpy_s -{ - float r; - float p; - float y; -} rpy_t; -const rpy_t RPY0 = { .r = 0, .p = 0, .y = 0 }; - -void quaternion_from_rpy(const rpy_t *a, q4_t *o) // (degrees) yaw->pitch->roll order -{ - float cr, sr, cp, sp, cy, sy; - - { float r2 = a->r * (RAD / 2); cr = cos_approx(r2); sr = sin_approx(r2); } - { float p2 = a->p * (RAD / 2); cp = cos_approx(p2); sp = sin_approx(p2); } - { float y2 = a->y * (RAD / 2); cy = cos_approx(y2); sy = sin_approx(y2); } - - o->w = cr*cp*cy + sr*sp*sy; - o->x = sr*cp*cy - cr*sp*sy; - o->y = cr*sp*cy + sr*cp*sy; - o->z = cr*cp*sy - sr*sp*cy; -} - -void quaternion_to_rpy(const q4_t *q, rpy_t *o) // (degrees) yaw->pitch->roll order -{ - // https://en.wikipedia.org/wiki/Conversion_between_quaternions_and_Euler_angles - // Body Z - Y - X sequence - - float q0 = q->w; - float q1 = q->x; - float q2 = q->y; - float q3 = q->z; - - float p0 = MAX(-1.0f, MIN(1.0f, 2 * (q0*q2 - q3*q1))); - o->p = asin(p0); - - if (ABS(ABS(o->p) - (90 * RAD)) < (0.5f*RAD)) // vertical - { - o->y = 2 * atan2_approx(q3, q0); - o->r = 0.0f; - } - else - { - float r0 = 2 * (q0*q1 + q2*q3); - float r1 = 1 - 2 * (q1*q1 + q2*q2); - if ((r0 == 0) && (r1 == 0)) { o->r = 0.0f; } // atan(0,0)! - else { o->r = atan2_approx(r0, r1); } - - float y0 = 2 * (q0*q3 + q1*q2); - float y1 = 1 - 2 * (q2*q2 + q3*q3); - if ((y0 == 0) && (y1 == 0)) { o->y = 0.0f; } // atan(0,0)! - else { o->y = atan2_approx(y0, y1); } - } - - o->y = -o->y; // yaw inversion hack for all boards - -} - -void angle_vector(const q4_t *a, v3_t *o) // convert from quaternion to angle vector[rad] -{ - q4_t a1; - if (a->w < 0) { MulQF(a, -1, &a1); a = &a1; } - - float t2 = acos_approx(MIN(1, a->w)); // TODO acos_approx?? - - if (ABS(t2) > (0.005f * RAD)) - { - float s = sin_approx(t2) / (2 * t2); - o->x = a->x / s; - o->y = a->y / s; - o->z = a->z / s; - } - else - { - *o = V0; - } -} - -void nlerp_step(const q4_t *a, const q4_t *b, float max_th, q4_t *o) // max_th in radians (max_rate * update time) -{ - float dot = MAX(-1, MIN(1, DotQQ(a, b))); - float th = 2*acos_approx(ABS(dot)); // ABS -> change direction for shortest path - - if (th <= (0.01f*RAD)) { *o = *b; } // tiny step - else - { - float tb = MIN(1, ABS(max_th / th)); - float ta = 1-tb; - if (dot < 0) { tb = -tb; } // change direction for shortest path - - q4_t r, a1, b1; - MulQF(a, ta, &a1); - MulQF(b, tb, &b1); - SumQQ(&a1, &b1, &r); - NormQ(&r, o); - } -} - -q4_t attitude_est_e_q; -float acc_rad_scale; // adc -> G -float gyro_rads_scale; // adc -> rad/s -float cosSmallAngle; -float acc_lpf_f0, acc_lpf_f1; -v3_t gravity_lpf_b_v, acc_lpf_b_v; - -void qimuInit() -{ - cosSmallAngle = cos_approx(RAD*imuRuntimeConfig->small_angle); - - acc_rad_scale = 1.0f / acc_1G; - gyro_rads_scale = gyro.scale * RAD; - - acc_lpf_f1 = (1.0f / imuRuntimeConfig->acc_lpf_factor); - acc_lpf_f0 = 1.0f - acc_lpf_f1; - gravity_lpf_b_v = VZ; - acc_lpf_b_v = VZ; - - quaternion_from_rpy(&RPY0, &attitude_est_e_q); - accProc.state = ACCPROC_READ; -} - -////////////////////////////////////////////////////////////////////// - -static void qAccProcessingStateMachine(rollAndPitchTrims_t *accelerometerTrims) -{ - int axis; - const float gyro_drift_factor = 0.00f; - static v3_t gyro_drift_correction_b_v = { .x = 0.0f, .y = 0.0f, .z = 0.0f }; // rad/s - - const float attitude_correction_factor = 0.001f; - static v3_t attitude_correction_b_v = { .x = 0.0f, .y = 0.0f, .z = 0.0f }; // rad/s - static v3_t acc_b_v, gyro_rate_b_v; - - static int16_t normalize_counter = 0; - static uint32_t previousT = 0; - static uint32_t currentT; - - // get time step.. TODO: this should really be fixed to division of MPU sample rate - static float dT; - - bool keepProcessing = true; // (keepProcessing == true): causes all states to execute (for slow cycle times) - - do { - switch (accProc.state) { - - case ACCPROC_READ: - currentT = micros(); - dT = (currentT - previousT)*0.000001f; - previousT = currentT; - updateAccelerationReadings(accelerometerTrims); // TODO rename to accelerometerUpdate and rename many other 'Acceleration' references to be 'Accelerometer' - accProc.state++; - break; - - case ACCPROC_CHUNK_1: - for (axis = 0; axis < 3; axis++) { - accSmooth[axis] = accADC[axis]; // TODO acc_lpf - or would this work better without it? - ((float *)&acc_b_v)[axis] = accSmooth[axis] * acc_rad_scale; - ((float *)&gyro_rate_b_v)[axis] = gyroADC[axis] * gyro_rads_scale; - } - - //////////////////////////////////////////////////////////////// - // add in drift compensation - SumVV(&gyro_rate_b_v, &gyro_drift_correction_b_v, &gyro_rate_b_v); - -#ifdef DEBUG_IMU - debug[0] = gyro_drift_correction_b_v.x * 10000; - debug[1] = gyro_drift_correction_b_v.y * 10000; - debug[2] = gyro_drift_correction_b_v.z * 10000; -#endif - - //////////////////////////////////////////////////////////////// - // add in attitude estimate correction, convert to degrees - v3_t gyro_rotation_b_v; - SumVV(&gyro_rate_b_v, &attitude_correction_b_v, &gyro_rotation_b_v); - MulVF(&gyro_rotation_b_v, dT, &gyro_rotation_b_v); - - //////////////////////////////////////////////////////////////// - // Update attitude estimate with gyro data - // small angle approximation should be fine, but error does creep in at high rotational rates on multiple axes - Normalize periodically - q4_t attitude_est_update_b_q; - quaternion(&gyro_rotation_b_v, &attitude_est_update_b_q); // convert angle vector to quaternion - MulQQ(&attitude_est_update_b_q, &attitude_est_e_q, &attitude_est_e_q); // and rotate estimate - - v3_t gravity_b_v; - // Calculate expected gravity(allows drift to be compensated on all 3 axis when possible) - RotateVQ(&VZ, &attitude_est_e_q, &gravity_b_v); - - // check small angle - if (gravity_b_v.z > cosSmallAngle) { - ENABLE_STATE(SMALL_ANGLE); - } else { - DISABLE_STATE(SMALL_ANGLE); - } - - // acc_lpf - if (imuRuntimeConfig->acc_lpf_factor > 0) { - v3_t a0, a1; - MulVF(&acc_lpf_b_v, acc_lpf_f0, &a0); - MulVF(&acc_b_v, acc_lpf_f1, &a1); - SumVV(&a0, &a1, &acc_lpf_b_v); - - MulVF(&gravity_lpf_b_v, acc_lpf_f0, &a0); - MulVF(&gravity_b_v, acc_lpf_f1, &a1); - SumVV(&a0, &a1, &gravity_lpf_b_v); - } else { - acc_lpf_b_v = acc_b_v; - gravity_lpf_b_v = gravity_b_v; - } - - //////////////////////////////////////////////////////////////// - // Calculate Correction - float acc_m2 = Mag2V(&acc_b_v); -#ifdef DEBUG_IMU - debug[3] = acc_m2*1000; -#endif - if ((acc_m2 > 1.1025f) || (acc_m2 < 0.9025f)) { - attitude_correction_b_v = V0; - } else { // we're not accelerating - // Cross product to determine error - CrossVV(&acc_lpf_b_v, &gravity_lpf_b_v, &attitude_correction_b_v); - MulVF(&attitude_correction_b_v, attitude_correction_factor/dT, &attitude_correction_b_v); // convert to rate for drift correction - - if (gyro_drift_factor != 0.0f) { - // conditionally update drift for valid axes (4.5 degree check) - if (ABS(gravity_b_v.x) < 0.997f) { - gyro_drift_correction_b_v.x = gyro_drift_correction_b_v.x + (attitude_correction_b_v.x*gyro_drift_factor); - } - if (ABS(gravity_b_v.y) < 0.997f) { - gyro_drift_correction_b_v.y = gyro_drift_correction_b_v.y + (attitude_correction_b_v.y*gyro_drift_factor); - } - if (ABS(gravity_b_v.z) < 0.997f) { - gyro_drift_correction_b_v.z = gyro_drift_correction_b_v.z + (attitude_correction_b_v.z*gyro_drift_factor); - } - } - } - - // renormalize every couple of seconds - if (++normalize_counter == 1000) { - NormQ(&attitude_est_e_q, &attitude_est_e_q); - normalize_counter = 0; - } - - // convert to cleanflight values - // update inclination - rpy_t rpy; - quaternion_to_rpy(&attitude_est_e_q, &rpy); - inclination.values.rollDeciDegrees = lrintf(rpy.r * (10 / RAD)); - inclination.values.pitchDeciDegrees = lrintf(rpy.p * (10 / RAD)); - heading = rpy.y * (1 / RAD); - if (heading < 0) heading += 360; -#ifdef DEBUG_IMU - //uint32_t endT = micros(); - //debug[3] = endT - currentT; -#endif - -#if 0 - accProc.state++; - break; - - case ACCPROC_CHUNK_2: - accProc.state++; - break; - - case ACCPROC_CHUNK_3: - accProc.state++; - break; - - case ACCPROC_CHUNK_4: - accProc.state++; - break; - - case ACCPROC_CHUNK_5: - accProc.state++; - break; - - case ACCPROC_CHUNK_6: - accProc.state++; - break; - - case ACCPROC_CHUNK_7: - accProc.state = ACCPROC_COPY; - break; - - case ACCPROC_COPY: - // assign deliverables (copy local to global) -/* - memcpy(&EstG, &fsmEstG, sizeof(t_fp_vector)); - for (axis = 0; axis < 3; axis++) { - accSmooth[axis] = fsmAccSmooth[axis]; - } - memcpy(&inclination, &fsmInclination, sizeof(rollAndPitchInclination_t)); - heading = fsmHeading; -*/ -#endif - keepProcessing = false; - /* no break */ - - default: - accProc.state = ACCPROC_READ; - break; - } - } while (keepProcessing); -} diff --git a/src/main/flight/imu.h b/src/main/flight/imu.h index 98f4b93c7f..9d4a2d07c1 100644 --- a/src/main/flight/imu.h +++ b/src/main/flight/imu.h @@ -18,26 +18,27 @@ #pragma once extern int16_t throttleAngleCorrection; -extern uint32_t accTimeSum; // altitudehold.c -extern int accSumCount; // altitudehold.c -extern float accVelScale; // altitudehold.c -extern t_fp_vector EstG; // display.c -extern int16_t accSmooth[XYZ_AXIS_COUNT]; // blackbox.c, display.c, serial_msp.c, frsky.c, smartport.c -extern int32_t accSum[XYZ_AXIS_COUNT]; // altitudehold.c -extern int16_t smallAngle; // display.c +extern uint32_t accTimeSum; +extern int accSumCount; +extern float accVelScale; +extern int16_t accSmooth[XYZ_AXIS_COUNT]; +extern int32_t accSum[XYZ_AXIS_COUNT]; -typedef struct rollAndPitchInclination_s { - // absolute angle inclination in multiple of 0.1 degree 180 deg = 1800 - int16_t rollDeciDegrees; - int16_t pitchDeciDegrees; -} rollAndPitchInclination_t_def; +#define DEGREES_TO_DECIDEGREES(angle) (angle * 10) +#define DECIDEGREES_TO_DEGREES(angle) (angle / 10) +#define DECIDEGREES_TO_RADIANS(angle) ((angle / 10.0f) * 0.0174532925f) typedef union { - int16_t raw[ANGLE_INDEX_COUNT]; - rollAndPitchInclination_t_def values; -} rollAndPitchInclination_t; + int16_t raw[XYZ_AXIS_COUNT]; + struct { + // absolute angle inclination in multiple of 0.1 degree 180 deg = 1800 + int16_t roll; + int16_t pitch; + int16_t yaw; + } values; +} attitudeEulerAngles_t; -extern rollAndPitchInclination_t inclination; +extern attitudeEulerAngles_t attitude; typedef struct accDeadband_s { uint8_t xy; // set the acc deadband for xy-Axis @@ -45,10 +46,10 @@ typedef struct accDeadband_s { } accDeadband_t; typedef struct imuRuntimeConfig_s { - uint8_t acc_lpf_factor; + uint8_t acc_cut_hz; uint8_t acc_unarmedcal; - float gyro_cmpf_factor; - float gyro_cmpfm_factor; + float dcm_ki; + float dcm_kp; uint8_t small_angle; } imuRuntimeConfig_t; diff --git a/src/main/flight/mixer.c b/src/main/flight/mixer.c index 9ca3dba2dd..ac4381758d 100755 --- a/src/main/flight/mixer.c +++ b/src/main/flight/mixer.c @@ -684,8 +684,8 @@ STATIC_UNIT_TESTED void servoMixer(void) } } - input[INPUT_GIMBAL_PITCH] = scaleRange(inclination.values.pitchDeciDegrees, -1800, 1800, -500, +500); - input[INPUT_GIMBAL_ROLL] = scaleRange(inclination.values.rollDeciDegrees, -1800, 1800, -500, +500); + input[INPUT_GIMBAL_PITCH] = scaleRange(attitude.values.pitch, -1800, 1800, -500, +500); + input[INPUT_GIMBAL_ROLL] = scaleRange(attitude.values.roll, -1800, 1800, -500, +500); input[INPUT_STABILIZED_THROTTLE] = motor[0] - 1000 - 500; // Since it derives from rcCommand or mincommand and must be [-500:+500] @@ -841,8 +841,8 @@ void mixTable(void) /* case MIXER_GIMBAL: - servo[SERVO_GIMBAL_PITCH] = (((int32_t)servoConf[SERVO_GIMBAL_PITCH].rate * inclination.values.pitchDeciDegrees) / 50) + determineServoMiddleOrForwardFromChannel(SERVO_GIMBAL_PITCH); - servo[SERVO_GIMBAL_ROLL] = (((int32_t)servoConf[SERVO_GIMBAL_ROLL].rate * inclination.values.rollDeciDegrees) / 50) + determineServoMiddleOrForwardFromChannel(SERVO_GIMBAL_ROLL); + servo[SERVO_GIMBAL_PITCH] = (((int32_t)servoConf[SERVO_GIMBAL_PITCH].rate * attitude.values.pitch) / 50) + determineServoMiddleOrForwardFromChannel(SERVO_GIMBAL_PITCH); + servo[SERVO_GIMBAL_ROLL] = (((int32_t)servoConf[SERVO_GIMBAL_ROLL].rate * attitude.values.roll) / 50) + determineServoMiddleOrForwardFromChannel(SERVO_GIMBAL_ROLL); break; */ @@ -858,11 +858,11 @@ void mixTable(void) if (IS_RC_MODE_ACTIVE(BOXCAMSTAB)) { if (gimbalConfig->mode == GIMBAL_MODE_MIXTILT) { - servo[SERVO_GIMBAL_PITCH] -= (-(int32_t)servoConf[SERVO_GIMBAL_PITCH].rate) * inclination.values.pitchDeciDegrees / 50 - (int32_t)servoConf[SERVO_GIMBAL_ROLL].rate * inclination.values.rollDeciDegrees / 50; - servo[SERVO_GIMBAL_ROLL] += (-(int32_t)servoConf[SERVO_GIMBAL_PITCH].rate) * inclination.values.pitchDeciDegrees / 50 + (int32_t)servoConf[SERVO_GIMBAL_ROLL].rate * inclination.values.rollDeciDegrees / 50; + servo[SERVO_GIMBAL_PITCH] -= (-(int32_t)servoConf[SERVO_GIMBAL_PITCH].rate) * attitude.values.pitch / 50 - (int32_t)servoConf[SERVO_GIMBAL_ROLL].rate * attitude.values.roll / 50; + servo[SERVO_GIMBAL_ROLL] += (-(int32_t)servoConf[SERVO_GIMBAL_PITCH].rate) * attitude.values.pitch / 50 + (int32_t)servoConf[SERVO_GIMBAL_ROLL].rate * attitude.values.roll / 50; } else { - servo[SERVO_GIMBAL_PITCH] += (int32_t)servoConf[SERVO_GIMBAL_PITCH].rate * inclination.values.pitchDeciDegrees / 50; - servo[SERVO_GIMBAL_ROLL] += (int32_t)servoConf[SERVO_GIMBAL_ROLL].rate * inclination.values.rollDeciDegrees / 50; + servo[SERVO_GIMBAL_PITCH] += (int32_t)servoConf[SERVO_GIMBAL_PITCH].rate * attitude.values.pitch / 50; + servo[SERVO_GIMBAL_ROLL] += (int32_t)servoConf[SERVO_GIMBAL_ROLL].rate * attitude.values.roll / 50; } } } diff --git a/src/main/flight/navigation.c b/src/main/flight/navigation.c index 7e2f4a7344..057c867a67 100644 --- a/src/main/flight/navigation.c +++ b/src/main/flight/navigation.c @@ -33,8 +33,12 @@ #include "drivers/serial_uart.h" #include "drivers/gpio.h" #include "drivers/light_led.h" +#include "drivers/sensor.h" +#include "drivers/accgyro.h" #include "sensors/sensors.h" +#include "sensors/boardalignment.h" +#include "sensors/acceleration.h" #include "io/beeper.h" #include "io/serial.h" @@ -44,6 +48,7 @@ #include "flight/pid.h" #include "flight/navigation.h" #include "flight/gps_conversion.h" +#include "flight/imu.h" #include "rx/rx.h" @@ -359,7 +364,7 @@ void GPS_reset_home_position(void) GPS_home[LAT] = GPS_coord[LAT]; GPS_home[LON] = GPS_coord[LON]; GPS_calc_longitude_scaling(GPS_coord[LAT]); // need an initial value for distance and bearing calc - nav_takeoff_bearing = heading; // save takeoff heading + nav_takeoff_bearing = DECIDEGREES_TO_DEGREES(attitude.values.yaw); // save takeoff heading // Set ground altitude ENABLE_STATE(GPS_FIX_HOME); } @@ -644,8 +649,8 @@ static int32_t wrap_36000(int32_t angle) void updateGpsStateForHomeAndHoldMode(void) { - float sin_yaw_y = sin_approx(heading * 0.0174532925f); - float cos_yaw_x = cos_approx(heading * 0.0174532925f); + float sin_yaw_y = sin_approx(DECIDEGREES_TO_DEGREES(attitude.values.yaw) * 0.0174532925f); + float cos_yaw_x = cos_approx(DECIDEGREES_TO_DEGREES(attitude.values.yaw) * 0.0174532925f); if (gpsProfile->nav_slew_rate) { nav_rated[LON] += constrain(wrap_18000(nav[LON] - nav_rated[LON]), -gpsProfile->nav_slew_rate, gpsProfile->nav_slew_rate); // TODO check this on uint8 nav_rated[LAT] += constrain(wrap_18000(nav[LAT] - nav_rated[LAT]), -gpsProfile->nav_slew_rate, gpsProfile->nav_slew_rate); diff --git a/src/main/flight/pid.c b/src/main/flight/pid.c index 970ba33900..44f2c0b06a 100644 --- a/src/main/flight/pid.c +++ b/src/main/flight/pid.c @@ -51,7 +51,6 @@ extern uint16_t cycleTime; extern uint8_t motorCount; extern float dT; -int16_t heading; int16_t axisPID[3]; #ifdef BLACKBOX @@ -144,10 +143,10 @@ static void pidLuxFloat(pidProfile_t *pidProfile, controlRateConfig_t *controlRa // calculate error and limit the angle to the max inclination #ifdef GPS errorAngle = (constrainf(((float)rcCommand[axis] * ((float)max_angle_inclination / 500.0f)) + GPS_angle[axis], -((int) max_angle_inclination), - +max_angle_inclination) - inclination.raw[axis] + angleTrim->raw[axis]) / 10.0f; + +max_angle_inclination) - attitude.raw[axis] + angleTrim->raw[axis]) / 10.0f; #else errorAngle = (constrainf((float)rcCommand[axis] * ((float)max_angle_inclination / 500.0f), -((int) max_angle_inclination), - +max_angle_inclination) - inclination.raw[axis] + angleTrim->raw[axis]) / 10.0f; + +max_angle_inclination) - attitude.raw[axis] + angleTrim->raw[axis]) / 10.0f; #endif if (FLIGHT_MODE(ANGLE_MODE)) { @@ -238,10 +237,10 @@ static void pidMultiWii(pidProfile_t *pidProfile, controlRateConfig_t *controlRa // observe max inclination #ifdef GPS errorAngle = constrain(2 * rcCommand[axis] + GPS_angle[axis], -((int) max_angle_inclination), - +max_angle_inclination) - inclination.raw[axis] + angleTrim->raw[axis]; + +max_angle_inclination) - attitude.raw[axis] + angleTrim->raw[axis]; #else errorAngle = constrain(2 * rcCommand[axis], -((int) max_angle_inclination), - +max_angle_inclination) - inclination.raw[axis] + angleTrim->raw[axis]; + +max_angle_inclination) - attitude.raw[axis] + angleTrim->raw[axis]; #endif PTermACC = errorAngle * pidProfile->P8[PIDLEVEL] / 100; // 32 bits is needed for calculation: errorAngle*P8[PIDLEVEL] could exceed 32768 16 bits is ok for result @@ -341,10 +340,10 @@ static void pidMultiWii23(pidProfile_t *pidProfile, controlRateConfig_t *control // 50 degrees max inclination #ifdef GPS errorAngle = constrain(2 * rcCommand[axis] + GPS_angle[axis], -((int) max_angle_inclination), - +max_angle_inclination) - inclination.raw[axis] + angleTrim->raw[axis]; + +max_angle_inclination) - attitude.raw[axis] + angleTrim->raw[axis]; #else errorAngle = constrain(2 * rcCommand[axis], -((int) max_angle_inclination), - +max_angle_inclination) - inclination.raw[axis] + angleTrim->raw[axis]; + +max_angle_inclination) - attitude.raw[axis] + angleTrim->raw[axis]; #endif errorAngleI[axis] = constrain(errorAngleI[axis] + errorAngle, -10000, +10000); // WindUp //16 bits is ok here @@ -447,10 +446,10 @@ static void pidMultiWiiHybrid(pidProfile_t *pidProfile, controlRateConfig_t *con // observe max inclination #ifdef GPS errorAngle = constrain(2 * rcCommand[axis] + GPS_angle[axis], -((int) max_angle_inclination), - +max_angle_inclination) - inclination.raw[axis] + angleTrim->raw[axis]; + +max_angle_inclination) - attitude.raw[axis] + angleTrim->raw[axis]; #else errorAngle = constrain(2 * rcCommand[axis], -((int) max_angle_inclination), - +max_angle_inclination) - inclination.raw[axis] + angleTrim->raw[axis]; + +max_angle_inclination) - attitude.raw[axis] + angleTrim->raw[axis]; #endif PTermACC = errorAngle * pidProfile->P8[PIDLEVEL] / 100; // 32 bits is needed for calculation: errorAngle*P8[PIDLEVEL] could exceed 32768 16 bits is ok for result @@ -578,9 +577,9 @@ rollAndPitchTrims_t *angleTrim, rxConfig_t *rxConfig) rcCommandAxis = (float)rcCommand[axis]; // Calculate common values for pid controllers if (FLIGHT_MODE(ANGLE_MODE) || FLIGHT_MODE(HORIZON_MODE)) { #ifdef GPS - error = constrain(2.0f * rcCommandAxis + GPS_angle[axis], -((int) max_angle_inclination), +max_angle_inclination) - inclination.raw[axis] + angleTrim->raw[axis]; + error = constrain(2.0f * rcCommandAxis + GPS_angle[axis], -((int) max_angle_inclination), +max_angle_inclination) - attitude.raw[axis] + angleTrim->raw[axis]; #else - error = constrain(2.0f * rcCommandAxis, -((int) max_angle_inclination), +max_angle_inclination) - inclination.raw[axis] + angleTrim->raw[axis]; + error = constrain(2.0f * rcCommandAxis, -((int) max_angle_inclination), +max_angle_inclination) - attitude.raw[axis] + angleTrim->raw[axis]; #endif PTermACC = error * (float)pidProfile->P8[PIDLEVEL] * 0.008f; @@ -730,10 +729,10 @@ static void pidRewrite(pidProfile_t *pidProfile, controlRateConfig_t *controlRat // calculate error and limit the angle to max configured inclination #ifdef GPS errorAngle = constrain(2 * rcCommand[axis] + GPS_angle[axis], -((int) max_angle_inclination), - +max_angle_inclination) - inclination.raw[axis] + angleTrim->raw[axis]; // 16 bits is ok here + +max_angle_inclination) - attitude.raw[axis] + angleTrim->raw[axis]; // 16 bits is ok here #else errorAngle = constrain(2 * rcCommand[axis], -((int) max_angle_inclination), - +max_angle_inclination) - inclination.raw[axis] + angleTrim->raw[axis]; // 16 bits is ok here + +max_angle_inclination) - attitude.raw[axis] + angleTrim->raw[axis]; // 16 bits is ok here #endif if (!FLIGHT_MODE(ANGLE_MODE)) { //control is GYRO based (ACRO and HORIZON - direct sticks control is applied to rate PID diff --git a/src/main/flight/pid.h b/src/main/flight/pid.h index 52c50eb8e8..c582472fa6 100644 --- a/src/main/flight/pid.h +++ b/src/main/flight/pid.h @@ -78,9 +78,6 @@ typedef struct pidProfile_s { #endif } pidProfile_t; -#define DEGREES_TO_DECIDEGREES(angle) (angle * 10) -#define DECIDEGREES_TO_DEGREES(angle) (angle / 10.0f) - extern int16_t axisPID[XYZ_AXIS_COUNT]; extern int32_t axisPID_P[3], axisPID_I[3], axisPID_D[3]; diff --git a/src/main/io/display.c b/src/main/io/display.c index e7e0a4d66f..55c4d15283 100644 --- a/src/main/io/display.c +++ b/src/main/io/display.c @@ -481,11 +481,12 @@ void showSensorsPage(void) } #endif - tfp_sprintf(lineBuffer, format, "I&H", inclination.values.rollDeciDegrees, inclination.values.pitchDeciDegrees, heading); + tfp_sprintf(lineBuffer, format, "I&H", attitude.values.roll, attitude.values.pitch, DECIDEGREES_TO_DEGREES(attitude.values.yaw)); padLineBuffer(); i2c_OLED_set_line(rowIndex++); i2c_OLED_send_string(lineBuffer); + /* uint8_t length; ftoa(EstG.A[X], lineBuffer); @@ -509,6 +510,7 @@ void showSensorsPage(void) padLineBuffer(); i2c_OLED_set_line(rowIndex++); i2c_OLED_send_string(lineBuffer); + */ } diff --git a/src/main/io/rc_controls.c b/src/main/io/rc_controls.c index fd91111e42..07a2e1fa2c 100644 --- a/src/main/io/rc_controls.c +++ b/src/main/io/rc_controls.c @@ -216,9 +216,6 @@ void processRcStickPositions(rxConfig_t *rxConfig, throttleStatus_e throttleStat baroSetCalibrationCycles(10); // calibrate baro to new ground level (10 * 25 ms = ~250 ms non blocking) #endif - if (!sensors(SENSOR_MAG)) - heading = 0; // reset heading to zero after gyro calibration - return; } diff --git a/src/main/io/serial_cli.c b/src/main/io/serial_cli.c index 81a9cc54f4..7a2153b7b7 100644 --- a/src/main/io/serial_cli.c +++ b/src/main/io/serial_cli.c @@ -411,8 +411,8 @@ const clivalue_t valueTable[] = { { "max_angle_inclination", VAR_UINT16 | MASTER_VALUE, &masterConfig.max_angle_inclination, 100, 900 }, { "moron_threshold", VAR_UINT8 | MASTER_VALUE, &masterConfig.gyroConfig.gyroMovementCalibrationThreshold, 0, 128 }, - { "gyro_cmpf_factor", VAR_UINT16 | MASTER_VALUE, &masterConfig.gyro_cmpf_factor, 100, 1000 }, - { "gyro_cmpfm_factor", VAR_UINT16 | MASTER_VALUE, &masterConfig.gyro_cmpfm_factor, 100, 1000 }, + { "imu_dcm_kp", VAR_UINT16 | MASTER_VALUE, &masterConfig.dcm_kp, 0, 20000 }, + { "imu_dcm_ki", VAR_UINT16 | MASTER_VALUE, &masterConfig.dcm_ki, 0, 20000 }, { "alt_hold_deadband", VAR_UINT8 | PROFILE_VALUE, &masterConfig.profile[0].rcControlsConfig.alt_hold_deadband, 1, 250 }, { "alt_hold_fast_change", VAR_UINT8 | PROFILE_VALUE, &masterConfig.profile[0].rcControlsConfig.alt_hold_fast_change, 0, 1 }, @@ -459,7 +459,7 @@ const clivalue_t valueTable[] = { #endif { "acc_hardware", VAR_UINT8 | MASTER_VALUE, &masterConfig.acc_hardware, 0, ACC_MAX }, - { "acc_lpf_factor", VAR_UINT8 | PROFILE_VALUE, &masterConfig.profile[0].acc_lpf_factor, 0, 250 }, + { "acc_cut_hz", VAR_UINT8 | PROFILE_VALUE, &masterConfig.profile[0].acc_cut_hz, 0, 200 }, { "accxy_deadband", VAR_UINT8 | PROFILE_VALUE, &masterConfig.profile[0].accDeadband.xy, 0, 100 }, { "accz_deadband", VAR_UINT8 | PROFILE_VALUE, &masterConfig.profile[0].accDeadband.z, 0, 100 }, { "accz_lpf_cutoff", VAR_FLOAT | PROFILE_VALUE, &masterConfig.profile[0].accz_lpf_cutoff, 1, 20 }, diff --git a/src/main/io/serial_msp.c b/src/main/io/serial_msp.c index a1881d7e32..1ce2c000d0 100644 --- a/src/main/io/serial_msp.c +++ b/src/main/io/serial_msp.c @@ -895,9 +895,9 @@ static bool processOutCommand(uint8_t cmdMSP) break; case MSP_ATTITUDE: headSerialReply(6); - for (i = 0; i < 2; i++) - serialize16(inclination.raw[i]); - serialize16(heading); + serialize16(attitude.values.roll); + serialize16(attitude.values.pitch); + serialize16(DECIDEGREES_TO_DEGREES(attitude.values.yaw)); break; case MSP_ALTITUDE: headSerialReply(6); diff --git a/src/main/main.c b/src/main/main.c index c0b11bc51b..3dded6cc26 100644 --- a/src/main/main.c +++ b/src/main/main.c @@ -116,13 +116,13 @@ void mixerUsePWMOutputConfiguration(pwmOutputConfiguration_t *pwmOutputConfigura void rxInit(rxConfig_t *rxConfig, modeActivationCondition_t *modeActivationConditions); void gpsInit(serialConfig_t *serialConfig, gpsConfig_t *initialGpsConfig); void navigationInit(gpsProfile_t *initialGpsProfile, pidProfile_t *pidProfile); +void imuInit(void); void displayInit(rxConfig_t *intialRxConfig); void ledStripInit(ledConfig_t *ledConfigsToUse, hsvColor_t *colorsToUse); void loop(void); void spektrumBind(rxConfig_t *rxConfig); const sonarHardware_t *sonarGetHardwareConfiguration(batteryConfig_t *batteryConfig); void sonarInit(const sonarHardware_t *sonarHardware); -void qimuInit(void); #ifdef STM32F303xC // from system_stm32f30x.c @@ -398,7 +398,8 @@ void init(void) compassInit(); #endif - qimuInit(); + imuInit(); + mspInit(&masterConfig.serialConfig); #ifdef USE_CLI diff --git a/src/main/mw.c b/src/main/mw.c index e9ac504b61..60832969f5 100644 --- a/src/main/mw.c +++ b/src/main/mw.c @@ -234,7 +234,7 @@ void annexCode(void) rcCommand[THROTTLE] = lookupThrottleRC[tmp2] + (tmp - tmp2 * 100) * (lookupThrottleRC[tmp2 + 1] - lookupThrottleRC[tmp2]) / 100; // [0;1000] -> expo -> [MINTHROTTLE;MAXTHROTTLE] if (FLIGHT_MODE(HEADFREE_MODE)) { - float radDiff = degreesToRadians(heading - headFreeModeHold); + float radDiff = degreesToRadians(DECIDEGREES_TO_DEGREES(attitude.values.yaw) - headFreeModeHold); float cosDiff = cos_approx(radDiff); float sinDiff = sin_approx(radDiff); int16_t rcCommand_PITCH = rcCommand[PITCH] * cosDiff + rcCommand[ROLL] * sinDiff; @@ -338,7 +338,7 @@ void mwArm(void) } if (!ARMING_FLAG(PREVENT_ARMING)) { ENABLE_ARMING_FLAG(ARMED); - headFreeModeHold = heading; + headFreeModeHold = DECIDEGREES_TO_DEGREES(attitude.values.yaw); #ifdef BLACKBOX if (feature(FEATURE_BLACKBOX)) { @@ -412,7 +412,7 @@ void updateInflightCalibrationState(void) void updateMagHold(void) { if (ABS(rcCommand[YAW]) < 70 && FLIGHT_MODE(MAG_MODE)) { - int16_t dif = heading - magHold; + int16_t dif = DECIDEGREES_TO_DEGREES(attitude.values.yaw) - magHold; if (dif <= -180) dif += 360; if (dif >= +180) @@ -421,7 +421,7 @@ void updateMagHold(void) if (STATE(SMALL_ANGLE)) rcCommand[YAW] -= dif * currentProfile->pidProfile.P8[PIDMAG] / 30; // 18 deg } else - magHold = heading; + magHold = DECIDEGREES_TO_DEGREES(attitude.values.yaw); } typedef enum { @@ -623,7 +623,7 @@ void processRx(void) if (IS_RC_MODE_ACTIVE(BOXMAG)) { if (!FLIGHT_MODE(MAG_MODE)) { ENABLE_FLIGHT_MODE(MAG_MODE); - magHold = heading; + magHold = DECIDEGREES_TO_DEGREES(attitude.values.yaw); } } else { DISABLE_FLIGHT_MODE(MAG_MODE); @@ -636,7 +636,7 @@ void processRx(void) DISABLE_FLIGHT_MODE(HEADFREE_MODE); } if (IS_RC_MODE_ACTIVE(BOXHEADADJ)) { - headFreeModeHold = heading; // acquire new heading + headFreeModeHold = DECIDEGREES_TO_DEGREES(attitude.values.yaw); // acquire new heading } } #endif diff --git a/src/main/sensors/sensors.h b/src/main/sensors/sensors.h index 78c452f46d..92e6907eef 100644 --- a/src/main/sensors/sensors.h +++ b/src/main/sensors/sensors.h @@ -70,5 +70,3 @@ typedef struct sensorAlignmentConfig_s { sensor_align_e acc_align; // acc alignment sensor_align_e mag_align; // mag alignment } sensorAlignmentConfig_t; - -extern int16_t heading; diff --git a/src/main/telemetry/frsky.c b/src/main/telemetry/frsky.c index 04f460d027..acac1ea7f1 100644 --- a/src/main/telemetry/frsky.c +++ b/src/main/telemetry/frsky.c @@ -432,7 +432,7 @@ static void sendFuelLevel(void) static void sendHeading(void) { sendDataHead(ID_COURSE_BP); - serialize16(heading); + serialize16(DECIDEGREES_TO_DEGREES(attitude.values.yaw)); sendDataHead(ID_COURSE_AP); serialize16(0); } diff --git a/src/main/telemetry/smartport.c b/src/main/telemetry/smartport.c index 1fef66b8ac..ef5389920e 100644 --- a/src/main/telemetry/smartport.c +++ b/src/main/telemetry/smartport.c @@ -381,7 +381,7 @@ void handleSmartPortTelemetry(void) } break; case FSSP_DATAID_HEADING : - smartPortSendPackage(id, heading * 100); // given in deg, requested in 10000 = 100 deg + smartPortSendPackage(id, attitude.values.yaw * 10); // given in 10*deg, requested in 10000 = 100 deg smartPortHasRequest = 0; break; case FSSP_DATAID_ACCX :