0,0 → 1,353 |
// gyro readings |
int16_t GyroNick, GyroRoll, GyroYaw; |
int16_t TrimNick, TrimRoll; |
|
|
int32_t IntegralGyroYaw = 0; |
int32_t ReadingIntegralGyroYaw = 0; |
|
// compass course |
int16_t CompassHeading = -1; // negative angle indicates invalid data. |
int16_t CompassCourse = -1; |
int16_t CompassOffCourse = 0; |
uint8_t CompassCalState = 0; |
|
uint16_t BadCompassHeading = 500; |
|
int32_t YawGyroHeading; // Yaw Gyro Integral supported by compass |
int16_t YawGyroDrift; |
|
uint8_t GyroYawPFactor, GyroYawIFactor; // the PD factors for the yae control |
|
int16_t StickNick = 0, StickRoll = 0, StickYaw = 0, StickGas = 0; |
int16_t GPSStickNick = 0, GPSStickRoll = 0; |
|
// stick values derived by uart inputs |
int16_t ExternStickNick = 0, ExternStickRoll = 0, ExternStickYaw = 0, ExternHeightValue = -20; |
|
/************************************************************************/ |
/* Neutral Readings */ |
/************************************************************************/ |
void SetNeutral(uint8_t AccAdjustment) |
{ |
... |
AdBiasGyroYaw = (int16_t)((Sum_3 + GYRO_BIAS_AVERAGE / 2) / GYRO_BIAS_AVERAGE); |
|
GyroYaw = 0; |
|
CompassCourse = CompassHeading; |
// Inititialize YawGyroIntegral value with current compass heading |
YawGyroHeading = (int32_t)CompassHeading * GYRO_DEG_FACTOR; |
YawGyroDrift = 0; |
|
|
// Something completely different: Here's why the turn-over's were vars. |
TurnOver180Nick = ((int32_t) ParamSet.AngleTurnOverNick * 2500L) +15000L; |
TurnOver180Roll = ((int32_t) ParamSet.AngleTurnOverRoll * 2500L) +15000L; |
|
} |
|
/************************************************************************/ |
/* Averaging Measurement Readings */ |
/************************************************************************/ |
void Mean(void) |
{ |
|
GyroYaw = AdBiasGyroYaw - AdValueGyroYaw; |
// Yaw |
// calculate yaw gyro integral (~ to rotation angle) |
YawGyroHeading += GyroYaw; |
ReadingIntegralGyroYaw += GyroYaw; |
|
|
// Coupling fraction |
if(! LoopingNick && !LoopingRoll && (ParamSet.GlobalConfig & CFG_AXIS_COUPLING_ACTIVE)) |
{ |
.... |
YawGyroHeading += tmp14; |
if(!FCParam.AxisCouplingYawCorrection) ReadingIntegralGyroYaw -= tmp14 / 2; // force yaw |
if(abs(GyroYaw > 64)) |
{ |
if(labs(tmpl) > 128 || labs(tmpl2) > 128) FunnelCourse = 1; |
} |
|
} |
|
// Yaw |
// limit YawGyroHeading proportional to 0? to 360? |
if(YawGyroHeading >= (360L * GYRO_DEG_FACTOR)) YawGyroHeading -= 360L * GYRO_DEG_FACTOR; // 360? Wrap |
|
if(YawGyroHeading < 0) YawGyroHeading += 360L * GYRO_DEG_FACTOR; |
|
IntegralGyroYaw = ReadingIntegralGyroYaw; |
|
} |
|
void SetCompassCalState(void) |
{ |
static uint8_t stick = 1; |
|
// if nick is centered or top set stick to zero |
if(PPM_in[ParamSet.ChannelAssignment[CH_NICK]] > -20) stick = 0; |
// if nick is down trigger to next cal state |
if((PPM_in[ParamSet.ChannelAssignment[CH_NICK]] < -70) && !stick) |
{ |
stick = 1; |
CompassCalState++; |
if(CompassCalState < 5) Beep(CompassCalState); |
else BeepTime = 1000; |
} |
} |
|
|
|
/************************************************************************/ |
/* MotorControl */ |
/************************************************************************/ |
void MotorControl(void) |
{ |
int16_t h, tmp_int; |
|
// Mixer Fractions that are combined for Motor Control |
int16_t YawMixFraction, GasMixFraction, NickMixFraction, RollMixFraction; |
|
int16_t PDPartYaw; |
static int32_t SetPointYaw = 0; |
static uint16_t UpdateCompassCourse = 0; |
|
Mean(); |
GRN_ON; |
|
if(RC_Quality > 140) |
{ |
if(ModelIsFlying < 256) |
{ |
StickYaw = 0; |
if(ModelIsFlying == 250) |
{ |
UpdateCompassCourse = 1; |
ReadingIntegralGyroYaw = 0; |
SetPointYaw = 0; |
} |
} |
else MKFlags |= (MKFLAG_FLY); // set fly flag |
|
...... |
|
if(ParamSet.GlobalConfig & (CFG_COMPASS_ACTIVE|CFG_GPS_ACTIVE)) |
{ |
// if roll stick is centered and nick stick is down |
if (abs(PPM_in[ParamSet.ChannelAssignment[CH_ROLL]]) < 30 && PPM_in[ParamSet.ChannelAssignment[CH_NICK]] < -70) |
{ |
CompassCalState = 1; |
BeepTime = 1000; |
} |
|
(R/C data): |
|
// mapping of yaw |
StickYaw = -PPM_in[ParamSet.ChannelAssignment[CH_YAW]]; |
// (range of -2 .. 2 is set to zero, to avoid unwanted yaw trimming on compass correction) |
if(ParamSet.GlobalConfig & (CFG_COMPASS_ACTIVE|CFG_GPS_ACTIVE)) |
{ |
if (StickYaw > 2) StickYaw-= 2; |
else if (StickYaw< -2) StickYaw += 2; |
else StickYaw = 0; |
} |
|
if(ExternControl.Config & 0x01 && FCParam.ExternalControl > 128) |
{ |
StickYaw += ExternControl.Yaw; |
// disable I part of gyro control feedback |
if(ParamSet.GlobalConfig & CFG_HEADING_HOLD) GyroIFactor = 0; |
|
|
|
|
// MeasurementCounter is incremented in the isr of analog.c |
if(MeasurementCounter >= BALANCE_NUMBER) // averaging number has reached |
{ |
if(ParamSet.DriftComp) |
{ |
if(YawGyroDrift > BALANCE_NUMBER/2) AdBiasGyroYaw++; |
if(YawGyroDrift < -BALANCE_NUMBER/2) AdBiasGyroYaw--; |
} |
YawGyroDrift = 0; |
|
|
// Yawing |
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
if(abs(StickYaw) > 15 ) // yaw stick is activated |
{ |
BadCompassHeading = 1000; |
if(!(ParamSet.GlobalConfig & CFG_COMPASS_FIX)) |
{ |
UpdateCompassCourse = 1; |
} |
} |
// exponential stick sensitivity in yawring rate |
tmp_int = (int32_t) ParamSet.StickYawP * ((int32_t)StickYaw * abs(StickYaw)) / 512L; // expo y = ax + bx? |
tmp_int += (ParamSet.StickYawP * StickYaw) / 4; |
|
SetPointYaw = tmp_int; |
// trimm drift of ReadingIntegralGyroYaw with SetPointYaw(StickYaw) |
ReadingIntegralGyroYaw -= tmp_int; |
// limit the effect |
CHECK_MIN_MAX(ReadingIntegralGyroYaw, -50000, 50000) |
|
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
// Compass |
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
// compass code is used if Compass option is selected |
if(ParamSet.GlobalConfig & (CFG_COMPASS_ACTIVE|CFG_GPS_ACTIVE)) |
{ |
int16_t w, v, r,correction, error; |
|
if(CompassCalState && !(MKFlags & MKFLAG_MOTOR_RUN) ) |
{ |
SetCompassCalState(); |
#ifdef USE_KILLAGREG |
MM3_Calibrate(); |
#endif |
} |
else |
{ |
#ifdef USE_KILLAGREG |
static uint8_t updCompass = 0; |
if (!updCompass--) |
{ |
updCompass = 49; // update only at 2ms*50 = 100ms (10Hz) |
MM3_Heading(); |
} |
#endif |
|
// get maximum attitude angle |
w = abs(IntegralGyroNick / 512); |
v = abs(IntegralGyroRoll / 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)) % 360) - 180; |
if(abs(GyroYaw) > 128) // spinning fast |
{ |
error = 0; |
} |
if(!BadCompassHeading && w < 25) |
{ |
YawGyroDrift += error; |
if(UpdateCompassCourse) |
{ |
BeepTime = 200; |
YawGyroHeading = (int32_t)CompassHeading * GYRO_DEG_FACTOR; |
CompassCourse = (int16_t)(YawGyroHeading / GYRO_DEG_FACTOR); |
UpdateCompassCourse = 0; |
} |
} |
YawGyroHeading += (error * 8) / correction; |
w = (w * FCParam.CompassYawEffect) / 32; |
w = FCParam.CompassYawEffect - w; |
if(w >= 0) |
{ |
if(!BadCompassHeading) |
{ |
v = 64 + (MaxStickNick + MaxStickRoll) / 8; |
// calc course deviation |
r = ((540 + (YawGyroHeading / GYRO_DEG_FACTOR) - CompassCourse) % 360) - 180; |
v = (r * w) / v; // align to compass course |
// limit yaw rate |
w = 3 * FCParam.CompassYawEffect; |
if (v > w) v = w; |
else if (v < -w) v = -w; |
ReadingIntegralGyroYaw += v; |
} |
else |
{ // wait a while |
BadCompassHeading--; |
} |
} |
else |
{ // ignore compass at extreme attitudes for a while |
BadCompassHeading = 500; |
} |
} |
} |
|
#if (defined (USE_KILLAGREG) || defined (USE_MK3MAG)) |
PDPartYaw = (int32_t)(GyroYaw * 2 * (int32_t)GyroYawPFactor) / (256L / CONTROL_SCALING) + (int32_t)(IntegralGyroYaw * GyroYawIFactor) / (2 * (44000 / CONTROL_SCALING)); |
|
|
|
|
YawMixFraction = PDPartYaw - SetPointYaw * CONTROL_SCALING; // yaw controller |
#define MIN_YAWGAS (40 * CONTROL_SCALING) // yaw also below this gas value |
// limit YawMixFraction |
if(GasMixFraction > MIN_YAWGAS) |
{ |
CHECK_MIN_MAX(YawMixFraction, -(GasMixFraction / 2), (GasMixFraction / 2)); |
} |
else |
{ |
CHECK_MIN_MAX(YawMixFraction, -(MIN_YAWGAS / 2), (MIN_YAWGAS / 2)); |
} |
tmp_int = ParamSet.GasMax * CONTROL_SCALING; |
CHECK_MIN_MAX(YawMixFraction, -(tmp_int - GasMixFraction), (tmp_int - GasMixFraction)); |
|
|
|
|
|
|
|
|
--------------- |
|
|
|
anal-lyse: |
|
YawMixFraction = PDPartYaw - SetPointYaw * CONTROL_SCALING; |
OK |
|
SetPointYaw->setPointYaw: |
- static |
- Set to 0 at take off |
- Set to yaw stick val (StickYawP/512 * StickYaw^2 + StickYawP/4 * StickYaw) |
OK |
|
PDPartYaw: |
- nonstatic nonglobal |
- = (int32_t)(GyroYaw * 2 * (int32_t)GyroYawPFactor) |
/ |
(256L / CONTROL_SCALING) |
+ (int32_t)(IntegralGyroYaw * GyroYawIFactor) |
/ |
(2 * (44000 / CONTROL_SCALING)); |
OK |
|
GyroYaw: |
- global |
- = AdBiasGyroYaw - AdValueGyroYaw (pretty much the raw offset gyro) |
OK |
|
YawGyroHeading->yawGyroHeading: |
- GyroYaw is summed to it at each iteration |
- It is wrapped around at <0 and >=360. |
- It is used -- where???? To adjust CompassCourse... |
OK |
|
IntegralGyroYaw->yawAngle: Deviation of heading from desired??? |
- GyroYaw is summed to it at each iteration |
- SetPointYaw is subtracted from it (!) at each iteration. |
- It is NOT wrapped, but just limited to +/- 50000 |
- It is corrected / pulled in axis coupling and by the compass. |
OK (Except that with the compass) |
|
BadCompassHeading: Need to update the "CompassCourse". |
|
CompassHeading: Opdateret fra mm3mag. |
|
CompassOffCourse: CompassHeading - CompassCourse. Opdateret fra mm3mag. |
|
|
UpdateCompassCourse: Set CompassCourse to the value of CompassHeading and set YawgyroHeading = compassHeading * GYRO_DEG_FACTOR |