Subversion Repositories FlightCtrl

Compare Revisions

Ignore whitespace Rev 1868 → Rev 1869

/branches/dongfang_FC_rewrite/ENC-03_FC1.3.c
65,11 → 65,11
 
void gyro_setDefaults(void) {
staticParams.GyroD = 3;
staticParams.DriftComp = 100;
staticParams.GyroAccFactor = 2;
staticParams.DriftComp = 200;
staticParams.GyroAccFactor = 25;
staticParams.GyroAccTrim = 1;
 
// Not used.
staticParams.AngleTurnOverPitch = 85;
staticParams.AngleTurnOverRoll = 85;
// staticParams.AngleTurnOverPitch = 85;
// staticParams.AngleTurnOverRoll = 85;
}
/branches/dongfang_FC_rewrite/analog.c
78,7 → 78,8
*/
volatile int16_t rawGyroSum[3];
volatile int16_t acc[3];
volatile int16_t filteredAcc[2] = { 0, 0 };
volatile int16_t filteredAcc[2] = { 0,0 };
volatile int32_t stronglyFilteredAcc[3] = { 0,0,0 };
 
/*
* These 4 exported variables are zero-offset. The "PID" ones are used
272,6 → 273,10
acc[Z] = accOffset[Z] - sensorInputs[AD_ACC_Z];
else
acc[Z] = sensorInputs[AD_ACC_Z] - accOffset[Z];
 
stronglyFilteredAcc[Z] =
(stronglyFilteredAcc[Z] * 99 + acc[Z] * 10) / 100;
 
break;
 
case 11: // yaw gyro
288,8 → 293,13
else
acc[PITCH] = sensorInputs[AD_ACC_PITCH] - accOffset[PITCH];
 
filteredAcc[PITCH] = (filteredAcc[PITCH] * (ACC_FILTER - 1) + acc[PITCH])
/ ACC_FILTER;
filteredAcc[PITCH] =
(filteredAcc[PITCH] * (ACC_FILTER - 1) + acc[PITCH]) / ACC_FILTER;
 
stronglyFilteredAcc[PITCH] =
(stronglyFilteredAcc[PITCH] * 99 + acc[PITCH] * 10) / 100;
 
 
measureNoise(acc[PITCH], &accNoisePeak[PITCH], 1);
break;
 
298,8 → 308,12
acc[ROLL] = accOffset[ROLL] - sensorInputs[AD_ACC_ROLL];
else
acc[ROLL] = sensorInputs[AD_ACC_ROLL] - accOffset[ROLL];
filteredAcc[ROLL] = (filteredAcc[ROLL] * (ACC_FILTER - 1) + acc[ROLL])
/ ACC_FILTER;
filteredAcc[ROLL] =
(filteredAcc[ROLL] * (ACC_FILTER - 1) + acc[ROLL]) / ACC_FILTER;
 
stronglyFilteredAcc[ROLL] =
(stronglyFilteredAcc[ROLL] * 99 + acc[ROLL] * 10) / 100;
 
measureNoise(acc[ROLL], &accNoisePeak[ROLL], 1);
break;
 
497,7 → 511,7
// DebugOut.Analog[20 + axis] = gyroOffset[axis];
}
 
// Noise is relative to offset. So, reset noise measurements when changing offsets.
// Noise is relativ to offset. So, reset noise measurements when changing offsets.
gyroNoisePeak[PITCH] = gyroNoisePeak[ROLL] = 0;
 
accOffset[PITCH] = GetParamWord(PID_ACC_PITCH);
/branches/dongfang_FC_rewrite/analog.h
187,7 → 187,7
extern volatile int16_t UBat;
 
// 1:11 voltage divider, 1024 counts per 3V, and result is divided by 3.
#define UBAT_AT_5V (int16_t)((5.0 / 3.0) * (1.0/11.0) / (3.0 * 1024))
#define UBAT_AT_5V (int16_t)((5.0 * (1.0/11.0)) * 1024 / (3.0 * 3))
 
/*
* This is not really for external use - but the ENC-03 gyro modules needs it.
199,6 → 199,7
*/
extern volatile int16_t acc[3];
extern volatile int16_t filteredAcc[2];
extern volatile int32_t stronglyFilteredAcc[3];
 
/*
* Diagnostics: Gyro noise level because of motor vibrations. The variables
/branches/dongfang_FC_rewrite/attitude.c
156,15 → 156,15
************************************************************************/
 
int32_t getAngleEstimateFromAcc(uint8_t axis) {
return GYRO_ACC_FACTOR * (int32_t)filteredAcc[axis];
return GYRO_ACC_FACTOR * (int32_t) filteredAcc[axis];
}
 
void setStaticAttitudeAngles(void) {
#ifdef ATTITUDE_USE_ACC_SENSORS
angle[PITCH] = getAngleEstimateFromAcc(PITCH);
angle[ROLL] = getAngleEstimateFromAcc(ROLL);
angle[PITCH] = getAngleEstimateFromAcc(PITCH);
angle[ROLL] = getAngleEstimateFromAcc(ROLL);
#else
angle[PITCH] = angle[ROLL] = 0;
angle[PITCH] = angle[ROLL] = 0;
#endif
}
 
172,26 → 172,26
* Neutral Readings
************************************************************************/
void attitude_setNeutral(void) {
// Servo_Off(); // disable servo output. TODO: Why bother? The servos are going to make a jerk anyway.
dynamicParams.AxisCoupling1 = dynamicParams.AxisCoupling2 = 0;
// Servo_Off(); // disable servo output. TODO: Why bother? The servos are going to make a jerk anyway.
dynamicParams.AxisCoupling1 = dynamicParams.AxisCoupling2 = 0;
 
driftComp[PITCH] = driftComp[ROLL] = yawGyroDrift = driftCompYaw = 0;
correctionSum[PITCH] = correctionSum[ROLL] = 0;
driftComp[PITCH] = driftComp[ROLL] = yawGyroDrift = driftCompYaw = 0;
correctionSum[PITCH] = correctionSum[ROLL] = 0;
 
// Calibrate hardware.
analog_calibrate();
// Calibrate hardware.
analog_calibrate();
 
// reset gyro integrals to acc guessing
setStaticAttitudeAngles();
yawAngleDiff = 0;
// reset gyro integrals to acc guessing
setStaticAttitudeAngles();
yawAngleDiff = 0;
 
// update compass course to current heading
compassCourse = compassHeading;
// update compass course to current heading
compassCourse = compassHeading;
 
// Inititialize YawGyroIntegral value with current compass heading
yawGyroHeading = (int32_t) compassHeading * GYRO_DEG_FACTOR_YAW;
// Inititialize YawGyroIntegral value with current compass heading
yawGyroHeading = (int32_t) compassHeading * GYRO_DEG_FACTOR_YAW;
 
// Servo_On(); //enable servo output
// Servo_On(); //enable servo output
}
 
/************************************************************************
201,22 → 201,24
* The rate variable end up in a range of about [-1024, 1023].
*************************************************************************/
void getAnalogData(void) {
uint8_t axis;
uint8_t axis;
 
for (axis = PITCH; axis <= ROLL; axis++) {
rate_PID[axis] = gyro_PID[axis] / HIRES_GYRO_INTEGRATION_FACTOR + driftComp[axis];
rate_ATT[axis] = gyro_ATT[axis] / HIRES_GYRO_INTEGRATION_FACTOR + driftComp[axis];
differential[axis] = gyroD[axis];
averageAcc[axis] += acc[axis];
}
for (axis = PITCH; axis <= ROLL; axis++) {
rate_PID[axis] = gyro_PID[axis] / HIRES_GYRO_INTEGRATION_FACTOR
+ driftComp[axis];
rate_ATT[axis] = gyro_ATT[axis] / HIRES_GYRO_INTEGRATION_FACTOR
+ driftComp[axis];
differential[axis] = gyroD[axis];
averageAcc[axis] += acc[axis];
}
 
averageAccCount++;
yawRate = yawGyro + driftCompYaw;
averageAccCount++;
yawRate = yawGyro + driftCompYaw;
 
// We are done reading variables from the analog module.
// Interrupt-driven sensor reading may restart.
analogDataReady = 0;
analog_start();
// We are done reading variables from the analog module.
// Interrupt-driven sensor reading may restart.
analogDataReady = 0;
analog_start();
}
 
/*
226,60 → 228,61
* changed accordingly.
*/
void trigAxisCoupling(void) {
int16_t cospitch = int_cos(angle[PITCH]);
int16_t cosroll = int_cos(angle[ROLL]);
int16_t sinroll = int_sin(angle[ROLL]);
J5HIGH;
int16_t cospitch = int_cos(angle[PITCH]);
int16_t cosroll = int_cos(angle[ROLL]);
int16_t sinroll = int_sin(angle[ROLL]);
 
ACRate[PITCH] = (((int32_t) rate_ATT[PITCH] * cosroll - (int32_t) yawRate
* sinroll) >> MATH_UNIT_FACTOR_LOG);
ACRate[PITCH] = (((int32_t) rate_ATT[PITCH] * cosroll - (int32_t) yawRate
* sinroll) >> MATH_UNIT_FACTOR_LOG);
 
ACRate[ROLL] = rate_ATT[ROLL] +
(((((int32_t)rate_ATT[PITCH] * sinroll + (int32_t)yawRate * cosroll)
>> MATH_UNIT_FACTOR_LOG)
* int_tan(angle[PITCH])) >> MATH_UNIT_FACTOR_LOG);
ACRate[ROLL] = rate_ATT[ROLL] + (((((int32_t) rate_ATT[PITCH] * sinroll
+ (int32_t) yawRate * cosroll) >> MATH_UNIT_FACTOR_LOG) * int_tan(
angle[PITCH])) >> MATH_UNIT_FACTOR_LOG);
 
ACYawRate = ((int32_t) rate_ATT[PITCH] * sinroll) / cospitch
+ ((int32_t) yawRate * cosroll) / cospitch;
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();
} else {
ACRate[PITCH] = rate_ATT[PITCH];
ACRate[ROLL] = rate_ATT[ROLL];
ACYawRate = yawRate;
}
// 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();
} else {
ACRate[PITCH] = rate_ATT[PITCH];
ACRate[ROLL] = rate_ATT[ROLL];
ACYawRate = yawRate;
}
 
/*
* Yaw
* Calculate yaw gyro integral (~ to rotation angle)
* Limit yawGyroHeading proportional to 0 deg to 360 deg
*/
yawGyroHeading += ACYawRate;
yawAngleDiff += yawRate;
/*
* Yaw
* Calculate yaw gyro integral (~ to rotation angle)
* Limit yawGyroHeading proportional to 0 deg to 360 deg
*/
yawGyroHeading += ACYawRate;
yawAngleDiff += yawRate;
 
if (yawGyroHeading >= YAWOVER360) {
yawGyroHeading -= YAWOVER360; // 360 deg. wrap
} else if (yawGyroHeading < 0) {
yawGyroHeading += YAWOVER360;
}
if (yawGyroHeading >= YAWOVER360) {
yawGyroHeading -= YAWOVER360; // 360 deg. wrap
} else if (yawGyroHeading < 0) {
yawGyroHeading += YAWOVER360;
}
 
/*
* Pitch axis integration and range boundary wrap.
*/
for (axis = PITCH; axis <= ROLL; axis++) {
angle[axis] += ACRate[axis];
if (angle[axis] > PITCHROLLOVER180) {
angle[axis] -= PITCHROLLOVER360;
} else if (angle[axis] <= -PITCHROLLOVER180) {
angle[axis] += PITCHROLLOVER360;
}
}
/*
* Pitch axis integration and range boundary wrap.
*/
for (axis = PITCH; axis <= ROLL; axis++) {
angle[axis] += ACRate[axis];
if (angle[axis] > PITCHROLLOVER180) {
angle[axis] -= PITCHROLLOVER360;
} else if (angle[axis] <= -PITCHROLLOVER180) {
angle[axis] += PITCHROLLOVER360;
}
}
J5LOW;
}
 
/************************************************************************
290,58 → 293,58
* That should only be necessary with drifty gyros like ENC-03.
************************************************************************/
void correctIntegralsByAcc0thOrder(void) {
// TODO: Consider changing this to: Only correct when integrals are less than ...., or only correct when angular velocities
// are less than ....., or reintroduce Kalman.
// Well actually the Z axis acc. check is not so silly.
uint8_t axis;
int32_t temp;
if (!looping && acc[Z] >= -dynamicParams.UserParams[7] && acc[Z]
<= dynamicParams.UserParams[7]) {
DebugOut.Digital[0] |= DEBUG_ACC0THORDER;
// TODO: Consider changing this to: Only correct when integrals are less than ...., or only correct when angular velocities
// are less than ....., or reintroduce Kalman.
// Well actually the Z axis acc. check is not so silly.
uint8_t axis;
int32_t temp;
if (!looping && acc[Z] >= -dynamicParams.UserParams[7] && acc[Z]
<= dynamicParams.UserParams[7]) {
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;
uint8_t permilleAcc = staticParams.GyroAccFactor; // NOTE!!! The meaning of this value has changed!!
uint8_t debugFullWeight = 1;
int32_t accDerived;
 
if ((controlYaw < -64) || (controlYaw > 64)) { // reduce further if yaw stick is active
permilleAcc /= 2;
debugFullWeight = 0;
}
if ((controlYaw < -64) || (controlYaw > 64)) { // reduce further if yaw stick is active
permilleAcc /= 2;
debugFullWeight = 0;
}
 
if ((maxControl[PITCH] > 64) || (maxControl[ROLL] > 64)) { // reduce effect during stick commands
permilleAcc /= 2;
debugFullWeight = 0;
}
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;
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.
*/
for (axis = PITCH; axis <= ROLL; axis++) {
accDerived = getAngleEstimateFromAcc(axis);
DebugOut.Analog[9 + axis] = (10 * accDerived) / GYRO_DEG_FACTOR_PITCHROLL;
/*
* Add to each sum: The amount by which the angle is changed just below.
*/
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.
temp = angle[axis]; //(permilleAcc * (accDerived - angle[axis])) / 1000;
angle[axis] = ((int32_t) (1000L - permilleAcc) * temp
+ (int32_t) permilleAcc * accDerived) / 1000L;
correctionSum[axis] += angle[axis] - temp;
}
} else {
DebugOut.Digital[0] &= ~DEBUG_ACC0THORDER;
DebugOut.Digital[1] &= ~DEBUG_ACC0THORDER;
DebugOut.Analog[9] = 0;
DebugOut.Analog[10] = 0;
// 1000 * the correction amount that will be added to the gyro angle in next line.
temp = angle[axis]; //(permilleAcc * (accDerived - angle[axis])) / 1000;
angle[axis] = ((int32_t) (1000L - permilleAcc) * temp
+ (int32_t) permilleAcc * accDerived) / 1000L;
correctionSum[axis] += angle[axis] - temp;
}
} else {
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;
}
DebugOut.Analog[16] = 0;
DebugOut.Analog[17] = 0;
// experiment: Kill drift compensation updates when not flying smooth.
correctionSum[PITCH] = correctionSum[ROLL] = 0;
}
}
 
/************************************************************************
357,25 → 360,26
// 2 times / sec. = 488/2
#define DRIFTCORRECTION_TIME 256L
void driftCorrection(void) {
static int16_t timer = DRIFTCORRECTION_TIME;
int16_t deltaCorrection;
uint8_t axis;
if (!--timer) {
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) / DRIFTCORRECTION_TIME;
// Add the delta to the compensation. So positive delta means, gyro should have higher value.
driftComp[axis] += deltaCorrection / staticParams.GyroAccTrim;
CHECK_MIN_MAX(driftComp[axis], -staticParams.DriftComp, staticParams.DriftComp);
// DebugOut.Analog[11 + axis] = correctionSum[axis];
DebugOut.Analog[16 + axis] = correctionSum[axis];
DebugOut.Analog[18 + axis] = deltaCorrection / staticParams.GyroAccTrim;
DebugOut.Analog[28 + axis] = driftComp[axis];
static int16_t timer = DRIFTCORRECTION_TIME;
int16_t deltaCorrection;
uint8_t axis;
if (!--timer) {
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)
/ DRIFTCORRECTION_TIME;
// Add the delta to the compensation. So positive delta means, gyro should have higher value.
driftComp[axis] += deltaCorrection / staticParams.GyroAccTrim;
CHECK_MIN_MAX(driftComp[axis], -staticParams.DriftComp, staticParams.DriftComp);
// DebugOut.Analog[11 + axis] = correctionSum[axis];
DebugOut.Analog[16 + axis] = correctionSum[axis];
DebugOut.Analog[18 + axis] = deltaCorrection / staticParams.GyroAccTrim;
DebugOut.Analog[28 + axis] = driftComp[axis];
 
correctionSum[axis] = 0;
}
}
correctionSum[axis] = 0;
}
}
}
 
/************************************************************************
382,102 → 386,101
* Main procedure.
************************************************************************/
void calculateFlightAttitude(void) {
// part1: 550 usec.
// part1a: 550 usec.
// part1b: 60 usec.
getAnalogData();
// end part1b
integrate();
// end part1a
// part1: 550 usec.
// part1a: 550 usec.
// part1b: 60 usec.
getAnalogData();
// end part1b
integrate();
// end part1a
 
DebugOut.Analog[6] = stronglyFilteredAcc[PITCH];
DebugOut.Analog[7] = stronglyFilteredAcc[ROLL];
DebugOut.Analog[8] = stronglyFilteredAcc[Z];
 
DebugOut.Analog[6] = ACRate[PITCH];
DebugOut.Analog[7] = ACRate[ROLL];
DebugOut.Analog[8] = ACYawRate;
DebugOut.Analog[3] = rate_PID[PITCH];
DebugOut.Analog[4] = rate_PID[ROLL];
DebugOut.Analog[5] = yawRate;
 
DebugOut.Analog[3] = rate_PID[PITCH];
DebugOut.Analog[4] = rate_PID[ROLL];
DebugOut.Analog[5] = yawRate;
 
#ifdef ATTITUDE_USE_ACC_SENSORS
correctIntegralsByAcc0thOrder();
driftCorrection();
correctIntegralsByAcc0thOrder();
driftCorrection();
#endif
// end part1
// end part1
}
 
void updateCompass(void) {
int16_t w, v, r, correction, error;
int16_t w, v, r, correction, error;
 
if (compassCalState && !(MKFlags & MKFLAG_MOTOR_RUN)) {
if (controlMixer_testCompassCalState()) {
compassCalState++;
if (compassCalState < 5)
beepNumber(compassCalState);
else
beep(1000);
}
} else {
// 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 if (abs(yawRate) > 128) { // spinning fast
error = 0;
} else {
// compassHeading - yawGyroHeading, on a -180..179 deg interval.
error = ((540 + compassHeading - (yawGyroHeading / GYRO_DEG_FACTOR_YAW))
% 360) - 180;
}
if (!ignoreCompassTimer && w < 25) {
yawGyroDrift += error;
// Basically this gets set if we are in "fix" mode, and when starting.
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;
if (compassCalState && !(MKFlags & MKFLAG_MOTOR_RUN)) {
if (controlMixer_testCompassCalState()) {
compassCalState++;
if (compassCalState < 5)
beepNumber(compassCalState);
else
beep(1000);
}
} else {
// 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 if (abs(yawRate) > 128) { // spinning fast
error = 0;
} else {
// compassHeading - yawGyroHeading, on a -180..179 deg interval.
error = ((540 + compassHeading - (yawGyroHeading / GYRO_DEG_FACTOR_YAW))
% 360) - 180;
}
if (!ignoreCompassTimer && w < 25) {
yawGyroDrift += error;
// Basically this gets set if we are in "fix" mode, and when starting.
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;
*/
w = dynamicParams.CompassYawEffect - (w * dynamicParams.CompassYawEffect)
/ 32;
/*
w = (w * dynamicParams.CompassYawEffect) / 32;
w = dynamicParams.CompassYawEffect - w;
*/
w = dynamicParams.CompassYawEffect - (w * dynamicParams.CompassYawEffect)
/ 32;
 
// As readable formula:
// w = dynamicParams.CompassYawEffect * (1-w/32);
// As readable formula:
// w = dynamicParams.CompassYawEffect * (1-w/32);
 
if (w >= 0) { // maxAttitudeAngle < 32
if (!ignoreCompassTimer) {
v = 64 + (maxControl[PITCH] + maxControl[ROLL]) / 8;
// yawGyroHeading - compassCourse on a -180..179 degree interval.
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
ignoreCompassTimer--;
}
} else { // ignore compass at extreme attitudes for a while
ignoreCompassTimer = 500;
}
}
if (w >= 0) { // maxAttitudeAngle < 32
if (!ignoreCompassTimer) {
v = 64 + (maxControl[PITCH] + maxControl[ROLL]) / 8;
// yawGyroHeading - compassCourse on a -180..179 degree interval.
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
ignoreCompassTimer--;
}
} else { // ignore compass at extreme attitudes for a while
ignoreCompassTimer = 500;
}
}
}
 
/*
/branches/dongfang_FC_rewrite/attitude.h
106,14 → 106,6
void updateCompass(void);
 
/*
* Interval wrap-over values for attitude integrals
*/
extern long turnOver180Pitch, turnOver180Roll;
 
// No longer used.
// extern uint8_t FunnelCourse;
 
/*
* Dynamic gyro offsets. These are signed values that are subtracted from the gyro measurements,
* to help cancelling out drift and vibration noise effects. The dynamic offsets themselves
* can be updated in flight by different ways, for example:
/branches/dongfang_FC_rewrite/configuration.h
91,8 → 91,8
uint8_t AxisCoupling1; // Value: 0-250 Faktor, mit dem Yaw die Achsen Roll und Nick koppelt (NickRollMitkopplung)
uint8_t AxisCoupling2; // Value: 0-250 Faktor, mit dem Nick und Roll verkoppelt werden
uint8_t AxisCouplingYawCorrection;// Value: 0-250 Faktor, mit dem Nick und Roll verkoppelt werden
uint8_t AngleTurnOverPitch; // Value: 0-250 180ü-Punkt
uint8_t AngleTurnOverRoll; // Value: 0-250 180ü-Punkt
uint8_t unused0;
uint8_t unused1;
uint8_t GyroAccTrim; // 1/k (Koppel_ACC_Wirkung)
uint8_t DriftComp; // limit for gyrodrift compensation
uint8_t DynamicStability; // PID limit for Attitude controller
/branches/dongfang_FC_rewrite/controlMixer.c
156,8 → 156,6
int16_t* RC_PRTY = RC_getPRTY();
int16_t* EC_PRTY = EC_getPRTY();
 
DebugOut.Analog[20] = RC_PRTY[CONTROL_THROTTLE];
 
control[PITCH] = RC_PRTY[CONTROL_PITCH] + EC_PRTY[CONTROL_PITCH];
control[ROLL] = RC_PRTY[CONTROL_ROLL] + EC_PRTY[CONTROL_ROLL];
// This can be a CPU time killer if the function implementations are inefficient.
167,7 → 165,6
 
DebugOut.Analog[12] = control[PITCH];
DebugOut.Analog[13] = control[ROLL];
//DebugOut.Analog[26] = controlYaw;
 
if (controlMixer_getSignalQuality() >= SIGNAL_GOOD) {
controlMixer_updateVariables();
181,7 → 178,7
 
// part1a end.
 
/* This is not really necessary with the dead-gap feature on all sticks (see rc.c)
/* This is not really necessary with the dead-band feature on all sticks (see rc.c)
if(staticParams.GlobalConfig & (CFG_COMPASS_ACTIVE | CFG_GPS_ACTIVE)) {
if (controlYaw > 2) controlYaw-= 2;
else if (controlYaw< -2) controlYaw += 2;
/branches/dongfang_FC_rewrite/eeprom.c
99,7 → 99,7
staticParams.UserParams2[0] = 0b11010101; // All gyro filter constants 2; acc. 4
staticParams.UserParams2[1] = 2; // H&I motor smoothing.
staticParams.UserParams2[2] = 120; // Yaw I factor
staticParams.UserParams2[3] = 10; // Max Z acceleration for acc. correction of angles.
staticParams.UserParams2[3] = 100; // Max Z acceleration for acc. correction of angles.
}
 
void setOtherDefaults(void) {
/branches/dongfang_FC_rewrite/heightControl.c
66,7 → 66,7
 
if (staticParams.GlobalConfig & CFG_HEIGHT_SWITCH) {
// If switch is activated in config, the MaxHeight parameter is a switch value: ON in both ends of the range; OFF in the middle.
DebugOut.Digital[0] |= DEBUG_HEIGHT_SWITCH;
// DebugOut.Digital[0] |= DEBUG_HEIGHT_SWITCH;
if (dynamicParams.MaxHeight < 40 || dynamicParams.MaxHeight > 255 - 40) {
// Switch is ON
if (setHeightLatch <= LATCH_TIME) {
73,7 → 73,7
if (setHeightLatch == LATCH_TIME) {
// Freeze the height as target. We want to do this exactly once each time the switch is thrown ON.
targetHeight = height;
DebugOut.Digital[1] |= DEBUG_HEIGHT_SWITCH;
// DebugOut.Digital[1] |= DEBUG_HEIGHT_SWITCH;
}
// Time not yet reached.
setHeightLatch++;
81,11 → 81,11
} else {
// Switch is OFF.
setHeightLatch = 0;
DebugOut.Digital[1] &= ~DEBUG_HEIGHT_SWITCH;
// DebugOut.Digital[1] &= ~DEBUG_HEIGHT_SWITCH;
}
} else {
// Switch is not activated; take the "max-height" as the target height.
DebugOut.Digital[0] &= ~DEBUG_HEIGHT_SWITCH;
// DebugOut.Digital[0] &= ~DEBUG_HEIGHT_SWITCH;
targetHeight = (uint16_t) dynamicParams.MaxHeight * 100; //getHeight() + 10 * 100;
}
 
99,7 → 99,7
}
 
// height, in meters (so the division factor is: 100)
DebugOut.Analog[30] = height / 100;
DebugOut.Analog[30] = height / 10;
}
 
// ParamSet.GlobalConfig & CFG_HEIGHT_CONTROL
148,10 → 148,7
else if (dThrottle < -staticParams.HeightMinGas)
dThrottle = -staticParams.HeightMinGas;
 
//DebugOut.Analog[18] = dThrottle;
//DebugOut.Analog[19] = iHeight / 10000L;
 
// TODO: Eliminate repitition.
// TODO: Eliminate repetition.
if (staticParams.GlobalConfig & CFG_HEIGHT_CONTROL) {
if (!(staticParams.GlobalConfig & CFG_HEIGHT_SWITCH)
|| (dynamicParams.MaxHeight < 40 || dynamicParams.MaxHeight > 255 - 40)) {
174,6 → 171,10
// DebugOut.Analog[18] = hoverThrottle;
} else
DebugOut.Digital[0] &= ~DEBUG_HOVERTHROTTLE;
 
DebugOut.Analog[20] = dThrottle;
DebugOut.Analog[21] = hoverThrottle;
 
return throttle;
}
 
/branches/dongfang_FC_rewrite/main.c
216,95 → 216,102
I2CTimeout = 5000;
 
while (1) {
if (runFlightControl && analogDataReady) { // control interval
if (runFlightControl) { // control interval
runFlightControl = 0; // reset Flag, is enabled every 2 ms by ISR of timer0
 
//J4HIGH;
flight_control();
//J4LOW;
DebugOut.Digital[0] &= ~DEBUG_MAINLOOP_TIMER;
DebugOut.Digital[1] &= ~DEBUG_MAINLOOP_TIMER;
 
/*
* If the motors are running (MKFlags & MKFLAG_MOTOR_RUN in flight.c), transmit
* the throttle vector just computed. Otherwise, if motor test is engaged, transmit
* the test throttle vector. If no testing, stop all motors.
*/
// Obsoleted.
// transmitMotorThrottleData();
if (analogDataReady) {
J4HIGH;
flight_control();
J4LOW;
 
RED_OFF;
/*
* If the motors are running (MKFlags & MKFLAG_MOTOR_RUN in flight.c), transmit
* the throttle vector just computed. Otherwise, if motor test is engaged, transmit
* the test throttle vector. If no testing, stop all motors.
*/
// Obsoleted.
// transmitMotorThrottleData();
 
/*
Does not belong here. Instead, external control should be ignored in
controlMixer if there was no new data from there for some time.
if(externalControlActive) externalControlActive--;
else {
externalControl.config = 0;
externalStickPitch = 0;
externalStickRoll = 0;
externalStickYaw = 0;
}
*/
RED_OFF;
 
/*
Does not belong here.
if(RC_Quality) RC_Quality--;
*/
/*
Does not belong here. Instead, external control should be ignored in
controlMixer if there was no new data from there for some time.
if(externalControlActive) externalControlActive--;
else {
externalControl.config = 0;
externalStickPitch = 0;
externalStickRoll = 0;
externalStickYaw = 0;
}
*/
 
/* Does not belong here. Well since we are not supporting navi right now anyway, leave out.
#ifdef USE_NAVICTRL
if(NCDataOkay) {
if(--NCDataOkay == 0) // no data from NC
{ // set gps control sticks neutral
GPSStickPitch = 0;
GPSStickRoll = 0;
NCSerialDataOkay = 0;
}
}
#endif
*/
if (!--I2CTimeout || missingMotor) { // try to reset the i2c if motor is missing ot timeout
RED_ON;
if (!I2CTimeout) {
I2C_Reset();
I2CTimeout = 5;
/*
Does not belong here.
if(RC_Quality) RC_Quality--;
*/
 
/* Does not belong here. Well since we are not supporting navi right now anyway, leave out.
#ifdef USE_NAVICTRL
if(NCDataOkay) {
if(--NCDataOkay == 0) // no data from NC
{ // set gps control sticks neutral
GPSStickPitch = 0;
GPSStickRoll = 0;
NCSerialDataOkay = 0;
}
}
#endif
*/
if (!--I2CTimeout || missingMotor) { // try to reset the i2c if motor is missing ot timeout
RED_ON;
if (!I2CTimeout) {
I2C_Reset();
I2CTimeout = 5;
}
} else {
RED_OFF;
}
} else {
RED_OFF;
}
 
// Allow Serial Data Transmit if motors must not updated or motors are not running
if (!runFlightControl || !(MKFlags & MKFLAG_MOTOR_RUN)) {
usart0_TransmitTxData();
}
// Allow Serial Data Transmit if motors must not updated or motors are not running
if (!runFlightControl || !(MKFlags & MKFLAG_MOTOR_RUN)) {
usart0_TransmitTxData();
} else {
DebugOut.Digital[1] |= DEBUG_MAINLOOP_TIMER;
}
 
usart0_ProcessRxData();
usart0_ProcessRxData();
 
if (CheckDelay(timer)) {
if (UBat <= UBAT_AT_5V) {
// Do nothing. The voltage on the input side of the regulator is <5V;
// we must be running off USB power. Keep it quiet.
} else if (UBat < staticParams.LowVoltageWarning) {
beepBatteryAlarm();
}
if (CheckDelay(timer)) {
if (UBat <= UBAT_AT_5V) {
// Do nothing. The voltage on the input side of the regulator is <5V;
// we must be running off USB power. Keep it quiet.
} else if (UBat < staticParams.LowVoltageWarning) {
beepBatteryAlarm();
}
 
#ifdef USE_NAVICTRL
SPI_StartTransmitPacket();
SendSPI = 4;
SPI_StartTransmitPacket();
SendSPI = 4;
#endif
timer = SetDelay(20); // every 20 ms
timer = SetDelay(20); // every 20 ms
}
output_update();
}
output_update();
}
 
#ifdef USE_NAVICTRL
if(!SendSPI) {
// SendSPI is decremented in timer0.c with a rate of 9.765 kHz.
// within the SPI_TransmitByte() routine the value is set to 4.
// I.e. the SPI_TransmitByte() is called at a rate of 9.765 kHz/4= 2441.25 Hz,
// and therefore the time of transmission of a complete spi-packet (32 bytes) is 32*4/9.765 kHz = 13.1 ms.
SPI_TransmitByte();
if(!SendSPI) {
// SendSPI is decremented in timer0.c with a rate of 9.765 kHz.
// within the SPI_TransmitByte() routine the value is set to 4.
// I.e. the SPI_TransmitByte() is called at a rate of 9.765 kHz/4= 2441.25 Hz,
// and therefore the time of transmission of a complete spi-packet (32 bytes) is 32*4/9.765 kHz = 13.1 ms.
SPI_TransmitByte();
}
#endif
}
#endif
}
return (1);
}
/branches/dongfang_FC_rewrite/makefile
25,8 → 25,8
GYRO=ENC-03_FC1.3
GYRO_HW_NAME=ENC
GYRO_HW_FACTOR=1.304f
GYRO_PITCHROLL_CORRECTION=0.83f
GYRO_YAW_CORRECTION=0.93f
GYRO_PITCHROLL_CORRECTION=0.9f
GYRO_YAW_CORRECTION=0.80f
 
#GYRO=ADXRS610_FC2.0
#GYRO_HW_NAME=ADXR
267,7 → 267,7
#AVRDUDE_PROGRAMMER = stk200
#AVRDUDE_PROGRAMMER = ponyser
AVRDUDE_PROGRAMMER = avrispv2
#falls Ponyser ausgew�hlt wird, muss sich unsere avrdude-Configdatei im Bin-Verzeichnis des Compilers befinden
#falls Ponyser ausgewaehlt wird, muss sich unsere avrdude-Configdatei im Bin-Verzeichnis des Compilers befinden
 
#AVRDUDE_PORT = com1 # programmer connected to serial device
#AVRDUDE_PORT = lpt1 # programmer connected to parallel port
/branches/dongfang_FC_rewrite/output.c
57,49 → 57,61
uint8_t flashCnt[2], flashMask[2];
// initializes the LED control outputs J16, J17
void output_init(void) {
// set PC2 & PC3 as output (control of J16 & J17)
DDRC |= (1 << DDC2) | (1 << DDC3);
OUTPUT_SET(0,0);
OUTPUT_SET(1,0);
flashCnt[0] = flashCnt[1] = 0;
flashMask[0] = flashMask[1] = 128;
// set PC2 & PC3 as output (control of J16 & J17)
DDRC |= (1 << DDC2) | (1 << DDC3);
OUTPUT_SET(0,0);
OUTPUT_SET(1,0);
flashCnt[0] = flashCnt[1] = 0;
flashMask[0] = flashMask[1] = 128;
}
 
void flashingLight(uint8_t port, uint8_t timing, uint8_t bitmask,
uint8_t manual) {
if (timing > 250 && manual > 230) {
// "timing" is set to "manual" and the value is very high --> Set to the value in bitmask bit 7.
OUTPUT_SET(port, bitmask & 128);
} else if (timing > 250 && manual < 10) {
// "timing" is set to "manual" and the value is very low --> Set to the negated value in bitmask bit 7.
OUTPUT_SET(port, !(bitmask & 128));
} else if (!flashCnt[port]--) {
// rotating mask over bitmask...
flashCnt[port] = timing - 1;
if (flashMask[port] == 1)
flashMask[port] = 128;
else
flashMask[port] >>= 1;
OUTPUT_SET(port, flashMask[port] & bitmask);
}
uint8_t manual) {
if (timing > 250 && manual > 230) {
// "timing" is set to "manual" and the value is very high --> Set to the value in bitmask bit 7.
OUTPUT_SET(port, bitmask & 128);
} else if (timing > 250 && manual < 10) {
// "timing" is set to "manual" and the value is very low --> Set to the negated value in bitmask bit 7.
OUTPUT_SET(port, !(bitmask & 128));
} else if (!flashCnt[port]--) {
// rotating mask over bitmask...
flashCnt[port] = timing - 1;
if (flashMask[port] == 1)
flashMask[port] = 128;
else
flashMask[port] >>= 1;
OUTPUT_SET(port, flashMask[port] & bitmask);
}
}
 
void flashingLights(void) {
static int8_t delay = 0;
if (!delay--) { // 10 ms intervals
delay = 4;
flashingLight(0, staticParams.J16Timing, staticParams.J16Bitmask,
dynamicParams.J16Timing);
flashingLight(1, staticParams.J17Timing, staticParams.J17Bitmask,
dynamicParams.J17Timing);
}
static int8_t delay = 0;
if (!delay--) { // 10 ms intervals
delay = 4;
flashingLight(0, staticParams.J16Timing, staticParams.J16Bitmask,
dynamicParams.J16Timing);
flashingLight(1, staticParams.J17Timing, staticParams.J17Bitmask,
dynamicParams.J17Timing);
}
}
 
void output_update(void) {
if (!DIGITAL_DEBUG_MASK)
flashingLights();
else {
OUTPUT_SET(0, DebugOut.Digital[0] & DIGITAL_DEBUG_MASK);
OUTPUT_SET(1, DebugOut.Digital[1] & DIGITAL_DEBUG_MASK);
}
if (!DIGITAL_DEBUG_MASK)
flashingLights();
else if (DIGITAL_DEBUG_MASK == DEBUG_LEDTEST_ON) {
OUTPUT_SET(0, 1);
OUTPUT_SET(1, 1);
} else if (DIGITAL_DEBUG_MASK == DEBUG_LEDTEST_OFF) {
OUTPUT_SET(0, 0);
OUTPUT_SET(1, 0);
} else if (DIGITAL_DEBUG_MASK == DEBUG_LEDTEST_0) {
OUTPUT_SET(0, 1);
OUTPUT_SET(1, 0);
} else if (DIGITAL_DEBUG_MASK == DEBUG_LEDTEST_1) {
OUTPUT_SET(0, 0);
OUTPUT_SET(1, 1);
} else {
OUTPUT_SET(0, DebugOut.Digital[0] & DIGITAL_DEBUG_MASK);
OUTPUT_SET(1, DebugOut.Digital[1] & DIGITAL_DEBUG_MASK);
}
}
/branches/dongfang_FC_rewrite/output.h
16,7 → 16,7
#define J5TOGGLE PORTD ^= (1<<PORTD3)
 
// invert means: An "1" bit in digital debug data make a LOW on the output.
#define DIGITAL_DEBUG_INVERT 1
#define DIGITAL_DEBUG_INVERT 0
 
#define OUTPUT_HIGH(num) {PORTC |= (4 << (num));}
#define OUTPUT_LOW(num) {PORTC &= ~(4 << (num));}
44,8 → 44,13
* Digital debugs may be added as desired, and removed when the mystery
* at hand is resolved.
*/
#define DEBUG_LEDTEST 256
#define DEBUG_HEIGHT_SWITCH 1
 
#define DEBUG_LEDTEST_ON 1000
#define DEBUG_LEDTEST_OFF 1001
#define DEBUG_LEDTEST_0 1002
#define DEBUG_LEDTEST_1 1003
 
#define DEBUG_MAINLOOP_TIMER 1
#define DEBUG_HEIGHT_DIFF 2
#define DEBUG_HOVERTHROTTLE 4
#define DEBUG_ACC0THORDER 8
58,7 → 63,7
* Set to 0 for using outputs as the usual flashing lights.
* Set to one of the DEBUG_... defines h for using the outputs as debug lights.
*/
#define DIGITAL_DEBUG_MASK DEBUG_ACC0THORDER
#define DIGITAL_DEBUG_MASK DEBUG_MAINLOOP_TIMER
 
void output_init(void);
void output_update(void);
/branches/dongfang_FC_rewrite/rc.c
251,9 → 251,6
if (NewPpmData-- == 0) {
RC_PRTY[CONTROL_PITCH] = RCChannel(CH_PITCH) * staticParams.StickP
+ RCDiff(CH_PITCH) * staticParams.StickD;
 
DebugOut.Analog[21] = RCChannel(CH_THROTTLE);
 
RC_PRTY[CONTROL_ROLL] = RCChannel(CH_ROLL) * staticParams.StickP
+ RCDiff(CH_ROLL) * staticParams.StickD;
RC_PRTY[CONTROL_THROTTLE] = RCChannel(CH_THROTTLE) + RCDiff(CH_THROTTLE)
/branches/dongfang_FC_rewrite/timer0.c
55,7 → 55,8
#include "analog.h"
 
// for debugging!
#include "rc.h"
#include "uart0.h"
#include "output.h"
 
#ifdef USE_MK3MAG
#include "mk3mag.h"
141,11 → 142,12
if(SendSPI) SendSPI--; // if SendSPI is 0, the transmit of a byte via SPI bus to and from The Navicontrol is done
#endif
 
if (!cnt--) { // every 10th run (9.765kHz/10 = 976Hz)
if (!cnt--) { // every 10th run (9.765625kHz/10 = 976.5625Hz)
cnt = 9;
cnt_1ms ^= 1;
if (!cnt_1ms) {
runFlightControl = 1; // every 2nd run (976.5625 Hz/2 = 488.28125 Hz)
DebugOut.Digital[0] |= DEBUG_MAINLOOP_TIMER;
}
CountMilliseconds++; // increment millisecond counter
}
/branches/dongfang_FC_rewrite/uart0.c
129,12 → 129,12
"GyroPitch(PID) ",
"GyroRoll(PID) ",
"GyroYaw ", //5
"GyroPitch(AC) ",
"GyroRoll(AC) ",
"GyroYaw(AC) ",
"FilteredAccP ",
"FilteredAccR ",
"FilteredAccZ ",
"AccPitch (angle)",
"AccRoll (angle) ", //10
"HIRES_GYRO_INTEG",
"UBat ",
"Pitch Term ",
"Roll Term ",
"Yaw Term ",
143,8 → 143,8
"0th O Corr roll ",
"DriftCompDelta P",
"DriftCompDelta R",
"StickP ", //20
"RC T ",
"dHeightThrottle ", //20
"hoverThrottle ",
"M1 ",
"M2 ",
"M3 ",
569,11 → 569,6
{
memcpy(&staticParams, (uint8_t*) &pRxData[2], sizeof(staticParams));
ParamSet_WriteToEEProm(pRxData[0]);
/*
TODO: Remove this encapsulation breach
turnOver180Pitch = (int32_t) staticParams.AngleTurnOverPitch * 2500L;
turnOver180Roll = (int32_t) staticParams.AngleTurnOverRoll * 2500L;
*/
tempchar1 = getActiveParamSet();
beepNumber(tempchar1);
} else {