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