WebSVN - FlightCtrl - /branches/dongfang_FC

// 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
Subversion Repositories FlightCtrl

(root)/branches/dongfang_FC_rewrite/yaw-analysis.txt @ 1917 - Rev
