59,10 → 59,14 |
#include "attitude.h" |
#include "dongfangMath.h" |
|
// For scope debugging only! |
#include "rc.h" |
|
// where our main data flow comes from. |
#include "analog.h" |
|
#include "configuration.h" |
#include "output.h" |
|
// Some calculations are performed depending on some stick related things. |
#include "controlMixer.h" |
84,7 → 88,7 |
* The variables are overwritten at each attitude calculation invocation - the values |
* are not preserved or reused. |
*/ |
int16_t rate[2], yawRate; |
int16_t rate_ATT[2], yawRate; |
|
// With different (less) filtering |
int16_t rate_PID[2]; |
105,20 → 109,19 |
* Gyro integrals. These are the rotation angles of the airframe compared to the |
* horizontal plane, yaw relative to yaw at start. |
*/ |
int32_t angle[2], yawAngle; |
int32_t angle[2], yawAngleDiff; |
|
int readingHeight = 0; |
|
// compass course |
int16_t compassHeading = -1; // negative angle indicates invalid data. |
int16_t compassCourse = -1; |
int16_t compassOffCourse = 0; |
int16_t compassHeading = -1; // negative angle indicates invalid data. |
int16_t compassCourse = -1; |
int16_t compassOffCourse = 0; |
uint16_t updateCompassCourse = 0; |
uint8_t compassCalState = 0; |
|
// uint8_t FunnelCourse = 0; |
uint8_t compassCalState = 0; |
uint16_t badCompassHeading = 500; |
int32_t yawGyroHeading; // Yaw Gyro Integral supported by compass |
int16_t yawGyroDrift; |
|
#define PITCHROLLOVER180 (GYRO_DEG_FACTOR_PITCHROLL * 180L) |
#define PITCHROLLOVER360 (GYRO_DEG_FACTOR_PITCHROLL * 360L) |
126,10 → 129,13 |
|
int16_t correctionSum[2] = {0,0}; |
|
// For NaviCTRL use. |
int16_t averageAcc[2] = {0,0}, averageAccCount = 0; |
|
/* |
* Experiment: Compensating for dynamic-induced gyro biasing. |
*/ |
int16_t dynamicOffset[2] = {0,0}, dynamicOffsetYaw = 0; |
int16_t driftComp[2] = {0,0}, driftCompYaw = 0; |
// int16_t savedDynamicOffsetPitch = 0, savedDynamicOffsetRoll = 0; |
// int32_t dynamicCalPitch, dynamicCalRoll, dynamicCalYaw; |
// int16_t dynamicCalCount; |
164,18 → 170,18 |
// Servo_Off(); // disable servo output. TODO: Why bother? The servos are going to make a jerk anyway. |
dynamicParams.AxisCoupling1 = dynamicParams.AxisCoupling2 = 0; |
|
dynamicOffset[PITCH] = dynamicOffset[ROLL] = 0; |
driftComp[PITCH] = driftComp[ROLL] = yawGyroDrift = driftCompYaw = 0; |
correctionSum[PITCH] = correctionSum[ROLL] = 0; |
|
// Calibrate hardware. |
analog_calibrate(); |
|
|
// reset gyro readings |
rate[PITCH] = rate[ROLL] = yawRate = 0; |
// rate_ATT[PITCH] = rate_ATT[ROLL] = yawRate = 0; |
|
// reset gyro integrals to acc guessing |
setStaticAttitudeAngles(); |
yawAngle = 0; |
yawAngleDiff = 0; |
|
// update compass course to current heading |
compassCourse = compassHeading; |
191,19 → 197,20 |
* TODO: Ultimately, the analog module could do this (instead of dumping |
* the values into variables). |
* The rate variable end up in a range of about [-1024, 1023]. |
* When scaled down by CONTROL_SCALING, the interval is about [-256, 256]. |
*************************************************************************/ |
void getAnalogData(void) { |
uint8_t axis; |
|
for (axis=PITCH; axis <=ROLL; axis++) { |
rate_PID[axis] = (gyro_PID[axis] + dynamicOffset[axis]) / HIRES_GYRO_INTEGRATION_FACTOR; |
rate[axis] = (gyro_ATT[axis] + dynamicOffset[axis]) / HIRES_GYRO_INTEGRATION_FACTOR; |
rate_PID[axis] = (gyro_PID[axis] + driftComp[axis]) / HIRES_GYRO_INTEGRATION_FACTOR; |
rate_ATT[axis] = (gyro_ATT[axis] + driftComp[axis]) / HIRES_GYRO_INTEGRATION_FACTOR; |
differential[axis] = gyroD[axis]; |
averageAcc[axis] += acc[axis]; |
} |
|
yawRate = yawGyro + dynamicOffsetYaw; |
|
averageAccCount++; |
yawRate = yawGyro + driftCompYaw; |
|
// We are done reading variables from the analog module. |
// Interrupt-driven sensor reading may restart. |
analogDataReady = 0; |
221,23 → 228,23 |
int16_t cosroll = int_cos(angle[ROLL]); |
int16_t sinroll = int_sin(angle[ROLL]); |
int16_t tanpitch = int_tan(angle[PITCH]); |
#define ANTIOVF 1024 |
ACRate[PITCH] = ((int32_t) rate[PITCH] * cosroll - (int32_t)yawRate * sinroll) / (int32_t)MATH_UNIT_FACTOR; |
ACRate[ROLL] = rate[ROLL] + (((int32_t)rate[PITCH] * sinroll / ANTIOVF * tanpitch + (int32_t)yawRate * int_cos(angle[ROLL]) / ANTIOVF * tanpitch) / ((int32_t)MATH_UNIT_FACTOR / ANTIOVF * MATH_UNIT_FACTOR)); |
ACYawRate = ((int32_t) rate[PITCH] * sinroll) / cospitch + ((int32_t)yawRate * cosroll) / cospitch; |
#define ANTIOVF 512 |
ACRate[PITCH] = ((int32_t) rate_ATT[PITCH] * cosroll - (int32_t)yawRate * sinroll) / (int32_t)MATH_UNIT_FACTOR; |
ACRate[ROLL] = rate_ATT[ROLL] + (((int32_t)rate_ATT[PITCH] * sinroll / ANTIOVF * tanpitch + (int32_t)yawRate * int_cos(angle[ROLL]) / ANTIOVF * tanpitch) / ((int32_t)MATH_UNIT_FACTOR / ANTIOVF * MATH_UNIT_FACTOR)); |
ACYawRate = ((int32_t) rate_ATT[PITCH] * sinroll) / cospitch + ((int32_t)yawRate * cosroll) / cospitch; |
} |
|
// 480 usec with axis coupling - almost no time without. |
void integrate(void) { |
// First, perform axis coupling. If disabled xxxRate is just copied to ACxxxRate. |
uint8_t axis; |
|
if(!looping && (staticParams.GlobalConfig & CFG_AXIS_COUPLING_ACTIVE)) { |
// The rotary rate limiter bit is abused for selecting axis coupling algorithm instead. |
trigAxisCoupling(); |
trigAxisCoupling(); |
} else { |
ACRate[PITCH] = rate[PITCH]; |
ACRate[ROLL] = rate[ROLL]; |
ACYawRate = yawRate; |
ACRate[PITCH] = rate_ATT[PITCH]; |
ACRate[ROLL] = rate_ATT[ROLL]; |
ACYawRate = yawRate; |
} |
|
/* |
246,9 → 253,7 |
* Limit yawGyroHeading proportional to 0 deg to 360 deg |
*/ |
yawGyroHeading += ACYawRate; |
|
// Why is yawAngle not wrapped 'round? |
yawAngle += ACYawRate; |
yawAngleDiff += yawRate; |
|
if(yawGyroHeading >= YAWOVER360) { |
yawGyroHeading -= YAWOVER360; // 360 deg. wrap |
283,21 → 288,26 |
uint8_t axis; |
int32_t correction; |
if(!looping && acc[Z] >= -dynamicParams.UserParams[7] && acc[Z] <= dynamicParams.UserParams[7]) { |
DebugOut.Digital[0] = 1; |
DebugOut.Digital[0] |= DEBUG_ACC0THORDER; |
|
uint8_t permilleAcc = staticParams.GyroAccFactor; // NOTE!!! The meaning of this value has changed!! |
uint8_t debugFullWeight = 1; |
int32_t accDerived; |
|
if((maxControl[PITCH] > 64) || (maxControl[ROLL] > 64)) { // reduce effect during stick commands |
if((controlYaw < -64) || (controlYaw > 64)) { // reduce further if yaw stick is active |
permilleAcc /= 2; |
debugFullWeight = 0; |
} |
|
if(abs(controlYaw) > 25) { // reduce further if yaw stick is active |
|
if((maxControl[PITCH] > 64) || (maxControl[ROLL] > 64)) { // reduce effect during stick commands |
permilleAcc /= 2; |
debugFullWeight = 0; |
} |
|
if (debugFullWeight) |
DebugOut.Digital[1] |= DEBUG_ACC0THORDER; |
else |
DebugOut.Digital[1] &= ~DEBUG_ACC0THORDER; |
|
/* |
* Add to each sum: The amount by which the angle is changed just below. |
305,26 → 315,23 |
for (axis=PITCH; axis<=ROLL; axis++) { |
accDerived = getAngleEstimateFromAcc(axis); |
DebugOut.Analog[9 + axis] = (10 * accDerived) / GYRO_DEG_FACTOR_PITCHROLL; |
|
|
// 1000 * the correction amount that will be added to the gyro angle in next line. |
correction = angle[axis]; //(permilleAcc * (accDerived - angle[axis])) / 1000; |
angle[axis] = ((int32_t)(1000 - permilleAcc) * angle[axis] + (int32_t)permilleAcc * accDerived) / 1000L; |
|
correctionSum[axis] += angle[axis] - correction; |
|
// There should not be a risk of overflow here, since the integrals do not exceed a few 100000. |
// change = ((1000 - permilleAcc) * angle[axis] + permilleAcc * accDerived) / 1000 - angle[axis] |
// = (1000 * angle[axis] - permilleAcc * angle[axis] + permilleAcc * accDerived) / 1000 - angle[axis] |
// = (- permilleAcc * angle[axis] + permilleAcc * accDerived) / 1000 |
// = permilleAcc * (accDerived - angle[axis]) / 1000 |
|
// Experiment: Do not acutally apply the correction. See if drift compensation does that. |
// angle[axis] += correction / 1000; |
angle[axis] = ((int32_t)(1000L - permilleAcc) * angle[axis] + (int32_t)permilleAcc * accDerived) / 1000L; |
correctionSum[axis] += angle[axis] - correction; |
DebugOut.Analog[16+axis] = angle[axis] - correction; |
} |
|
DebugOut.Digital[1] = debugFullWeight; |
} else { |
DebugOut.Digital[0] = 0; |
DebugOut.Digital[0] &= ~DEBUG_ACC0THORDER; |
DebugOut.Digital[1] &= ~DEBUG_ACC0THORDER; |
DebugOut.Analog[9] = 0; |
DebugOut.Analog[10] = 0; |
|
DebugOut.Analog[16] = 0; |
DebugOut.Analog[17] = 0; |
// experiment: Kill drift compensation updates when not flying smooth. |
correctionSum[PITCH] = correctionSum[ROLL] = 0; |
} |
} |
|
348,12 → 355,15 |
timer = DRIFTCORRECTION_TIME; |
for (axis=PITCH; axis<=ROLL; axis++) { |
// Take the sum of corrections applied, add it to delta |
deltaCorrection = ((correctionSum[axis] + DRIFTCORRECTION_TIME / 2) * HIRES_GYRO_INTEGRATION_FACTOR) / DRIFTCORRECTION_TIME; |
deltaCorrection = (correctionSum[axis] * HIRES_GYRO_INTEGRATION_FACTOR + DRIFTCORRECTION_TIME / 2) / DRIFTCORRECTION_TIME; |
// Add the delta to the compensation. So positive delta means, gyro should have higher value. |
dynamicOffset[axis] += deltaCorrection / staticParams.GyroAccTrim; |
CHECK_MIN_MAX(dynamicOffset[axis], -staticParams.DriftComp, staticParams.DriftComp); |
DebugOut.Analog[11 + axis] = correctionSum[axis]; |
DebugOut.Analog[28 + axis] = dynamicOffset[axis]; |
driftComp[axis] += deltaCorrection / staticParams.GyroAccTrim; |
CHECK_MIN_MAX(driftComp[axis], -staticParams.DriftComp, staticParams.DriftComp); |
// DebugOut.Analog[11 + axis] = correctionSum[axis]; |
|
DebugOut.Analog[18+axis] = deltaCorrection / staticParams.GyroAccTrim; |
DebugOut.Analog[28+axis] = driftComp[axis]; |
|
correctionSum[axis] = 0; |
} |
} |
363,8 → 373,14 |
* Main procedure. |
************************************************************************/ |
void calculateFlightAttitude(void) { |
// part1: 550 usec. |
// part1a: 550 usec. |
// part1b: 60 usec. |
getAnalogData(); |
// end part1b |
integrate(); |
// end part1a |
|
|
DebugOut.Analog[6] = ACRate[PITCH]; |
DebugOut.Analog[7] = ACRate[ROLL]; |
378,60 → 394,63 |
correctIntegralsByAcc0thOrder(); |
driftCorrection(); |
#endif |
// end part1 |
} |
|
/* |
void updateCompass(void) { |
void updateCompass(void) { |
int16_t w, v, r,correction, error; |
|
if(compassCalState && !(MKFlags & MKFLAG_MOTOR_RUN)) { |
setCompassCalState(); |
if (controlMixer_testCompassCalState()) { |
compassCalState++; |
if(compassCalState < 5) beepNumber(compassCalState); |
else beep(1000); |
} |
} else { |
// get maximum attitude angle |
w = abs(pitchAngle / 512); |
v = abs(rollAngle / 512); |
if(v > w) w = v; |
correction = w / 8 + 1; |
// calculate the deviation of the yaw gyro heading and the compass heading |
if (compassHeading < 0) error = 0; // disable yaw drift compensation if compass heading is undefined |
else error = ((540 + compassHeading - (yawGyroHeading / GYRO_DEG_FACTOR_YAW)) % 360) - 180; |
if(abs(yawRate) > 128) { // spinning fast |
error = 0; |
// get maximum attitude angle |
w = abs(angle[PITCH] / 512); |
v = abs(angle[ROLL] / 512); |
if(v > w) w = v; |
correction = w / 8 + 1; |
// calculate the deviation of the yaw gyro heading and the compass heading |
if (compassHeading < 0) error = 0; // disable yaw drift compensation if compass heading is undefined |
else error = ((540 + compassHeading - (yawGyroHeading / GYRO_DEG_FACTOR_YAW)) % 360) - 180; |
if(abs(yawRate) > 128) { // spinning fast |
error = 0; |
} |
if(!badCompassHeading && w < 25) { |
yawGyroDrift += error; |
if(updateCompassCourse) { |
beep(200); |
yawGyroHeading = (int32_t)compassHeading * GYRO_DEG_FACTOR_YAW; |
compassCourse = compassHeading; //(int16_t)(yawGyroHeading / GYRO_DEG_FACTOR_YAW); |
updateCompassCourse = 0; |
} |
} |
yawGyroHeading += (error * 8) / correction; |
w = (w * dynamicParams.CompassYawEffect) / 32; |
w = dynamicParams.CompassYawEffect - w; |
if(w >= 0) { |
if(!badCompassHeading) { |
v = 64 + (maxControl[PITCH] + maxControl[ROLL]) / 8; |
// calc course deviation |
r = ((540 + (yawGyroHeading / GYRO_DEG_FACTOR_YAW) - compassCourse) % 360) - 180; |
v = (r * w) / v; // align to compass course |
// limit yaw rate |
w = 3 * dynamicParams.CompassYawEffect; |
if (v > w) v = w; |
else if (v < -w) v = -w; |
yawAngleDiff += v; |
} |
else |
{ // wait a while |
badCompassHeading--; |
} |
} else { // ignore compass at extreme attitudes for a while |
badCompassHeading = 500; |
} |
} |
if(!badCompassHeading && w < 25) { |
if(updateCompassCourse) { |
beep(200); |
yawGyroHeading = (int32_t)compassHeading * GYRO_DEG_FACTOR_YAW; |
compassCourse = (int16_t)(yawGyroHeading / GYRO_DEG_FACTOR_YAW); |
updateCompassCourse = 0; |
} |
} |
yawGyroHeading += (error * 8) / correction; |
w = (w * dynamicParams.CompassYawEffect) / 32; |
w = dynamicParams.CompassYawEffect - w; |
if(w >= 0) { |
if(!badCompassHeading) { |
v = 64 + (maxControlPitch + maxControlRoll) / 8; |
// calc course deviation |
r = ((540 + (yawGyroHeading / GYRO_DEG_FACTOR_YAW) - compassCourse) % 360) - 180; |
v = (r * w) / v; // align to compass course |
// limit yaw rate |
w = 3 * dynamicParams.CompassYawEffect; |
if (v > w) v = w; |
else if (v < -w) v = -w; |
yawAngle += v; |
} |
else |
{ // wait a while |
badCompassHeading--; |
} |
} |
else { // ignore compass at extreme attitudes for a while |
badCompassHeading = 500; |
} |
} |
} |
*/ |
} |
|
/* |
* This is part of an experiment to measure average sensor offsets caused by motor vibration, |
457,13 → 476,13 |
// Param6: Manual mode. The offsets are taken from Param7 and Param8. |
if (dynamicParams.UserParam6 || 1) { // currently always enabled. |
// manual mode |
dynamicOffsetPitch = dynamicParams.UserParam7 - 128; |
dynamicOffsetRoll = dynamicParams.UserParam8 - 128; |
driftCompPitch = dynamicParams.UserParam7 - 128; |
driftCompRoll = dynamicParams.UserParam8 - 128; |
} else { |
// use the sampled value (does not seem to work so well....) |
dynamicOffsetPitch = savedDynamicOffsetPitch = -dynamicCalPitch / dynamicCalCount; |
dynamicOffsetRoll = savedDynamicOffsetRoll = -dynamicCalRoll / dynamicCalCount; |
dynamicOffsetYaw = -dynamicCalYaw / dynamicCalCount; |
driftCompPitch = savedDynamicOffsetPitch = -dynamicCalPitch / dynamicCalCount; |
driftCompRoll = savedDynamicOffsetRoll = -dynamicCalRoll / dynamicCalCount; |
driftCompYaw = -dynamicCalYaw / dynamicCalCount; |
} |
|
// keep resetting these meanwhile, to avoid accumulating errors. |