/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 { |