WebSVN - FlightCtrl - Diff - Rev 1775 and 1964 - /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();
 
 
        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

Rev 1775	Rev 1964
1	// gyro readings	1	// gyro readings
2	int16_t GyroNick, GyroRoll, GyroYaw;	2	int16_t GyroNick, GyroRoll, GyroYaw;
3	int16_t TrimNick, TrimRoll;	3	int16_t TrimNick, TrimRoll;
4		4
5		5
6	int32_t IntegralGyroYaw = 0;	6	int32_t IntegralGyroYaw = 0;
7	int32_t ReadingIntegralGyroYaw = 0;	7	int32_t ReadingIntegralGyroYaw = 0;
8		8
9	// compass course	9	// compass course
10	int16_t CompassHeading = -1; // negative angle indicates invalid data.	10	int16_t CompassHeading = -1; // negative angle indicates invalid data.
11	int16_t CompassCourse = -1;	11	int16_t CompassCourse = -1;
12	int16_t CompassOffCourse = 0;	12	int16_t CompassOffCourse = 0;
13	uint8_t CompassCalState = 0;	13	uint8_t CompassCalState = 0;
14		14
15	uint16_t BadCompassHeading = 500;	15	uint16_t BadCompassHeading = 500;
16		16
17	int32_t YawGyroHeading; // Yaw Gyro Integral supported by compass	17	int32_t YawGyroHeading; // Yaw Gyro Integral supported by compass
18	int16_t YawGyroDrift;	18	int16_t YawGyroDrift;
19		19
20	uint8_t GyroYawPFactor, GyroYawIFactor; // the PD factors for the yae control	20	uint8_t GyroYawPFactor, GyroYawIFactor; // the PD factors for the yae control
21		21
22	int16_t StickNick = 0, StickRoll = 0, StickYaw = 0, StickGas = 0;	22	int16_t StickNick = 0, StickRoll = 0, StickYaw = 0, StickGas = 0;
23	int16_t GPSStickNick = 0, GPSStickRoll = 0;	23	int16_t GPSStickNick = 0, GPSStickRoll = 0;
24		24
25	// stick values derived by uart inputs	25	// stick values derived by uart inputs
26	int16_t ExternStickNick = 0, ExternStickRoll = 0, ExternStickYaw = 0, ExternHeightValue = -20;	26	int16_t ExternStickNick = 0, ExternStickRoll = 0, ExternStickYaw = 0, ExternHeightValue = -20;
27		27
28	/************************************************************************/	28	/************************************************************************/
29	/* Neutral Readings */	29	/* Neutral Readings */
30	/************************************************************************/	30	/************************************************************************/
31	void SetNeutral(uint8_t AccAdjustment)	31	void SetNeutral(uint8_t AccAdjustment)
32	{	32	{
33	...	33	...
34	AdBiasGyroYaw = (int16_t)((Sum_3 + GYRO_BIAS_AVERAGE / 2) / GYRO_BIAS_AVERAGE);	34	AdBiasGyroYaw = (int16_t)((Sum_3 + GYRO_BIAS_AVERAGE / 2) / GYRO_BIAS_AVERAGE);
35		35
36	GyroYaw = 0;	36	GyroYaw = 0;
37		37
38	CompassCourse = CompassHeading;	38	CompassCourse = CompassHeading;
39	// Inititialize YawGyroIntegral value with current compass heading	39	// Inititialize YawGyroIntegral value with current compass heading
40	YawGyroHeading = (int32_t)CompassHeading * GYRO_DEG_FACTOR;	40	YawGyroHeading = (int32_t)CompassHeading * GYRO_DEG_FACTOR;
41	YawGyroDrift = 0;	41	YawGyroDrift = 0;
42		42
43		43
44	// Something completely different: Here's why the turn-over's were vars.	44	// Something completely different: Here's why the turn-over's were vars.
45	TurnOver180Nick = ((int32_t) ParamSet.AngleTurnOverNick * 2500L) +15000L;	45	TurnOver180Nick = ((int32_t) ParamSet.AngleTurnOverNick * 2500L) +15000L;
46	TurnOver180Roll = ((int32_t) ParamSet.AngleTurnOverRoll * 2500L) +15000L;	46	TurnOver180Roll = ((int32_t) ParamSet.AngleTurnOverRoll * 2500L) +15000L;
47		47
48	}	48	}
49		49
50	/************************************************************************/	50	/************************************************************************/
51	/* Averaging Measurement Readings */	51	/* Averaging Measurement Readings */
52	/************************************************************************/	52	/************************************************************************/
53	void Mean(void)	53	void Mean(void)
54	{	54	{
55		55
56	GyroYaw = AdBiasGyroYaw - AdValueGyroYaw;	56	GyroYaw = AdBiasGyroYaw - AdValueGyroYaw;
57	// Yaw	57	// Yaw
58	// calculate yaw gyro integral (~ to rotation angle)	58	// calculate yaw gyro integral (~ to rotation angle)
59	YawGyroHeading += GyroYaw;	59	YawGyroHeading += GyroYaw;
60	ReadingIntegralGyroYaw += GyroYaw;	60	ReadingIntegralGyroYaw += GyroYaw;
61		61
62		62
63	// Coupling fraction	63	// Coupling fraction
64	if(! LoopingNick && !LoopingRoll && (ParamSet.GlobalConfig & CFG_AXIS_COUPLING_ACTIVE))	64	if(! LoopingNick && !LoopingRoll && (ParamSet.GlobalConfig & CFG_AXIS_COUPLING_ACTIVE))
65	{	65	{
66	....	66	....
67	YawGyroHeading += tmp14;	67	YawGyroHeading += tmp14;
68	if(!FCParam.AxisCouplingYawCorrection) ReadingIntegralGyroYaw -= tmp14 / 2; // force yaw	68	if(!FCParam.AxisCouplingYawCorrection) ReadingIntegralGyroYaw -= tmp14 / 2; // force yaw
69	if(abs(GyroYaw > 64))	69	if(abs(GyroYaw > 64))
70	{	70	{
71	if(labs(tmpl) > 128 \|\| labs(tmpl2) > 128) FunnelCourse = 1;	71	if(labs(tmpl) > 128 \|\| labs(tmpl2) > 128) FunnelCourse = 1;
72	}	72	}
73		73
74	}	74	}
75		75
76	// Yaw	76	// Yaw
77	// limit YawGyroHeading proportional to 0? to 360?	77	// limit YawGyroHeading proportional to 0? to 360?
78	if(YawGyroHeading >= (360L * GYRO_DEG_FACTOR)) YawGyroHeading -= 360L * GYRO_DEG_FACTOR; // 360? Wrap	78	if(YawGyroHeading >= (360L * GYRO_DEG_FACTOR)) YawGyroHeading -= 360L * GYRO_DEG_FACTOR; // 360? Wrap
79		79
80	if(YawGyroHeading < 0) YawGyroHeading += 360L * GYRO_DEG_FACTOR;	80	if(YawGyroHeading < 0) YawGyroHeading += 360L * GYRO_DEG_FACTOR;
81		81
82	IntegralGyroYaw = ReadingIntegralGyroYaw;	82	IntegralGyroYaw = ReadingIntegralGyroYaw;
83		83
84	}	84	}
85		85
86	void SetCompassCalState(void)	86	void SetCompassCalState(void)
87	{	87	{
88	static uint8_t stick = 1;	88	static uint8_t stick = 1;
89		89
90	// if nick is centered or top set stick to zero	90	// if nick is centered or top set stick to zero
91	if(PPM_in[ParamSet.ChannelAssignment[CH_NICK]] > -20) stick = 0;	91	if(PPM_in[ParamSet.ChannelAssignment[CH_NICK]] > -20) stick = 0;
92	// if nick is down trigger to next cal state	92	// if nick is down trigger to next cal state
93	if((PPM_in[ParamSet.ChannelAssignment[CH_NICK]] < -70) && !stick)	93	if((PPM_in[ParamSet.ChannelAssignment[CH_NICK]] < -70) && !stick)
94	{	94	{
95	stick = 1;	95	stick = 1;
96	CompassCalState++;	96	CompassCalState++;
97	if(CompassCalState < 5) Beep(CompassCalState);	97	if(CompassCalState < 5) Beep(CompassCalState);
98	else BeepTime = 1000;	98	else BeepTime = 1000;
99	}	99	}
100	}	100	}
101		101
102		102
103		103
104	/************************************************************************/	104	/************************************************************************/
105	/* MotorControl */	105	/* MotorControl */
106	/************************************************************************/	106	/************************************************************************/
107	void MotorControl(void)	107	void MotorControl(void)
108	{	108	{
109	int16_t h, tmp_int;	109	int16_t h, tmp_int;
110		110
111	// Mixer Fractions that are combined for Motor Control	111	// Mixer Fractions that are combined for Motor Control
112	int16_t YawMixFraction, GasMixFraction, NickMixFraction, RollMixFraction;	112	int16_t YawMixFraction, GasMixFraction, NickMixFraction, RollMixFraction;
113		113
114	int16_t PDPartYaw;	114	int16_t PDPartYaw;
115	static int32_t SetPointYaw = 0;	115	static int32_t SetPointYaw = 0;
116	static uint16_t UpdateCompassCourse = 0;	116	static uint16_t UpdateCompassCourse = 0;
117		117
118	Mean();	118	Mean();
119	GRN_ON;	-
120		119
121	if(RC_Quality > 140)	120	if(RC_Quality > 140)
122	{	121	{
123	if(ModelIsFlying < 256)	122	if(ModelIsFlying < 256)
124	{	123	{
125	StickYaw = 0;	124	StickYaw = 0;
126	if(ModelIsFlying == 250)	125	if(ModelIsFlying == 250)
127	{	126	{
128	UpdateCompassCourse = 1;	127	UpdateCompassCourse = 1;
129	ReadingIntegralGyroYaw = 0;	128	ReadingIntegralGyroYaw = 0;
130	SetPointYaw = 0;	129	SetPointYaw = 0;
131	}	130	}
132	}	131	}
133	else MKFlags \|= (MKFLAG_FLY); // set fly flag	132	else MKFlags \|= (MKFLAG_FLY); // set fly flag
134		133
135	......	134	......
136		135
137	if(ParamSet.GlobalConfig & (CFG_COMPASS_ACTIVE\|CFG_GPS_ACTIVE))	136	if(ParamSet.GlobalConfig & (CFG_COMPASS_ACTIVE\|CFG_GPS_ACTIVE))
138	{	137	{
139	// if roll stick is centered and nick stick is down	138	// if roll stick is centered and nick stick is down
140	if (abs(PPM_in[ParamSet.ChannelAssignment[CH_ROLL]]) < 30 && PPM_in[ParamSet.ChannelAssignment[CH_NICK]] < -70)	139	if (abs(PPM_in[ParamSet.ChannelAssignment[CH_ROLL]]) < 30 && PPM_in[ParamSet.ChannelAssignment[CH_NICK]] < -70)
141	{	140	{
142	CompassCalState = 1;	141	CompassCalState = 1;
143	BeepTime = 1000;	142	BeepTime = 1000;
144	}	143	}
145		144
146	(R/C data):	145	(R/C data):
147		146
148	// mapping of yaw	147	// mapping of yaw
149	StickYaw = -PPM_in[ParamSet.ChannelAssignment[CH_YAW]];	148	StickYaw = -PPM_in[ParamSet.ChannelAssignment[CH_YAW]];
150	// (range of -2 .. 2 is set to zero, to avoid unwanted yaw trimming on compass correction)	149	// (range of -2 .. 2 is set to zero, to avoid unwanted yaw trimming on compass correction)
151	if(ParamSet.GlobalConfig & (CFG_COMPASS_ACTIVE\|CFG_GPS_ACTIVE))	150	if(ParamSet.GlobalConfig & (CFG_COMPASS_ACTIVE\|CFG_GPS_ACTIVE))
152	{	151	{
153	if (StickYaw > 2) StickYaw-= 2;	152	if (StickYaw > 2) StickYaw-= 2;
154	else if (StickYaw< -2) StickYaw += 2;	153	else if (StickYaw< -2) StickYaw += 2;
155	else StickYaw = 0;	154	else StickYaw = 0;
156	}	155	}
157		156
158	if(ExternControl.Config & 0x01 && FCParam.ExternalControl > 128)	157	if(ExternControl.Config & 0x01 && FCParam.ExternalControl > 128)
159	{	158	{
160	StickYaw += ExternControl.Yaw;	159	StickYaw += ExternControl.Yaw;
161	// disable I part of gyro control feedback	160	// disable I part of gyro control feedback
162	if(ParamSet.GlobalConfig & CFG_HEADING_HOLD) GyroIFactor = 0;	161	if(ParamSet.GlobalConfig & CFG_HEADING_HOLD) GyroIFactor = 0;
163		162
164		163
165		164
166		165
167	// MeasurementCounter is incremented in the isr of analog.c	166	// MeasurementCounter is incremented in the isr of analog.c
168	if(MeasurementCounter >= BALANCE_NUMBER) // averaging number has reached	167	if(MeasurementCounter >= BALANCE_NUMBER) // averaging number has reached
169	{	168	{
170	if(ParamSet.DriftComp)	169	if(ParamSet.DriftComp)
171	{	170	{
172	if(YawGyroDrift > BALANCE_NUMBER/2) AdBiasGyroYaw++;	171	if(YawGyroDrift > BALANCE_NUMBER/2) AdBiasGyroYaw++;
173	if(YawGyroDrift < -BALANCE_NUMBER/2) AdBiasGyroYaw--;	172	if(YawGyroDrift < -BALANCE_NUMBER/2) AdBiasGyroYaw--;
174	}	173	}
175	YawGyroDrift = 0;	174	YawGyroDrift = 0;
176		175
177		176
178	// Yawing	177	// Yawing
179	// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++	178	// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
180	if(abs(StickYaw) > 15 ) // yaw stick is activated	179	if(abs(StickYaw) > 15 ) // yaw stick is activated
181	{	180	{
182	BadCompassHeading = 1000;	181	BadCompassHeading = 1000;
183	if(!(ParamSet.GlobalConfig & CFG_COMPASS_FIX))	182	if(!(ParamSet.GlobalConfig & CFG_COMPASS_FIX))
184	{	183	{
185	UpdateCompassCourse = 1;	184	UpdateCompassCourse = 1;
186	}	185	}
187	}	186	}
188	// exponential stick sensitivity in yawring rate	187	// exponential stick sensitivity in yawring rate
189	tmp_int = (int32_t) ParamSet.StickYawP * ((int32_t)StickYaw * abs(StickYaw)) / 512L; // expo y = ax + bx?	188	tmp_int = (int32_t) ParamSet.StickYawP * ((int32_t)StickYaw * abs(StickYaw)) / 512L; // expo y = ax + bx?
190	tmp_int += (ParamSet.StickYawP * StickYaw) / 4;	189	tmp_int += (ParamSet.StickYawP * StickYaw) / 4;
191		190
192	SetPointYaw = tmp_int;	191	SetPointYaw = tmp_int;
193	// trimm drift of ReadingIntegralGyroYaw with SetPointYaw(StickYaw)	192	// trimm drift of ReadingIntegralGyroYaw with SetPointYaw(StickYaw)
194	ReadingIntegralGyroYaw -= tmp_int;	193	ReadingIntegralGyroYaw -= tmp_int;
195	// limit the effect	194	// limit the effect
196	CHECK_MIN_MAX(ReadingIntegralGyroYaw, -50000, 50000)	195	CHECK_MIN_MAX(ReadingIntegralGyroYaw, -50000, 50000)
197		196
198	// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++	197	// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
199	// Compass	198	// Compass
200	// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++	199	// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
201	// compass code is used if Compass option is selected	200	// compass code is used if Compass option is selected
202	if(ParamSet.GlobalConfig & (CFG_COMPASS_ACTIVE\|CFG_GPS_ACTIVE))	201	if(ParamSet.GlobalConfig & (CFG_COMPASS_ACTIVE\|CFG_GPS_ACTIVE))
203	{	202	{
204	int16_t w, v, r,correction, error;	203	int16_t w, v, r,correction, error;
205		204
206	if(CompassCalState && !(MKFlags & MKFLAG_MOTOR_RUN) )	205	if(CompassCalState && !(MKFlags & MKFLAG_MOTOR_RUN) )
207	{	206	{
208	SetCompassCalState();	207	SetCompassCalState();
209	#ifdef USE_KILLAGREG	208	#ifdef USE_KILLAGREG
210	MM3_Calibrate();	209	MM3_Calibrate();
211	#endif	210	#endif
212	}	211	}
213	else	212	else
214	{	213	{
215	#ifdef USE_KILLAGREG	214	#ifdef USE_KILLAGREG
216	static uint8_t updCompass = 0;	215	static uint8_t updCompass = 0;
217	if (!updCompass--)	216	if (!updCompass--)
218	{	217	{
219	updCompass = 49; // update only at 2ms*50 = 100ms (10Hz)	218	updCompass = 49; // update only at 2ms*50 = 100ms (10Hz)
220	MM3_Heading();	219	MM3_Heading();
221	}	220	}
222	#endif	221	#endif
223		222
224	// get maximum attitude angle	223	// get maximum attitude angle
225	w = abs(IntegralGyroNick / 512);	224	w = abs(IntegralGyroNick / 512);
226	v = abs(IntegralGyroRoll / 512);	225	v = abs(IntegralGyroRoll / 512);
227	if(v > w) w = v;	226	if(v > w) w = v;
228	correction = w / 8 + 1;	227	correction = w / 8 + 1;
229	// calculate the deviation of the yaw gyro heading and the compass heading	228	// calculate the deviation of the yaw gyro heading and the compass heading
230	if (CompassHeading < 0) error = 0; // disable yaw drift compensation if compass heading is undefined	229	if (CompassHeading < 0) error = 0; // disable yaw drift compensation if compass heading is undefined
231	else error = ((540 + CompassHeading - (YawGyroHeading / GYRO_DEG_FACTOR)) % 360) - 180;	230	else error = ((540 + CompassHeading - (YawGyroHeading / GYRO_DEG_FACTOR)) % 360) - 180;
232	if(abs(GyroYaw) > 128) // spinning fast	231	if(abs(GyroYaw) > 128) // spinning fast
233	{	232	{
234	error = 0;	233	error = 0;
235	}	234	}
236	if(!BadCompassHeading && w < 25)	235	if(!BadCompassHeading && w < 25)
237	{	236	{
238	YawGyroDrift += error;	237	YawGyroDrift += error;
239	if(UpdateCompassCourse)	238	if(UpdateCompassCourse)
240	{	239	{
241	BeepTime = 200;	240	BeepTime = 200;
242	YawGyroHeading = (int32_t)CompassHeading * GYRO_DEG_FACTOR;	241	YawGyroHeading = (int32_t)CompassHeading * GYRO_DEG_FACTOR;
243	CompassCourse = (int16_t)(YawGyroHeading / GYRO_DEG_FACTOR);	242	CompassCourse = (int16_t)(YawGyroHeading / GYRO_DEG_FACTOR);
244	UpdateCompassCourse = 0;	243	UpdateCompassCourse = 0;
245	}	244	}
246	}	245	}
247	YawGyroHeading += (error * 8) / correction;	246	YawGyroHeading += (error * 8) / correction;
248	w = (w * FCParam.CompassYawEffect) / 32;	247	w = (w * FCParam.CompassYawEffect) / 32;
249	w = FCParam.CompassYawEffect - w;	248	w = FCParam.CompassYawEffect - w;
250	if(w >= 0)	249	if(w >= 0)
251	{	250	{
252	if(!BadCompassHeading)	251	if(!BadCompassHeading)
253	{	252	{
254	v = 64 + (MaxStickNick + MaxStickRoll) / 8;	253	v = 64 + (MaxStickNick + MaxStickRoll) / 8;
255	// calc course deviation	254	// calc course deviation
256	r = ((540 + (YawGyroHeading / GYRO_DEG_FACTOR) - CompassCourse) % 360) - 180;	255	r = ((540 + (YawGyroHeading / GYRO_DEG_FACTOR) - CompassCourse) % 360) - 180;
257	v = (r * w) / v; // align to compass course	256	v = (r * w) / v; // align to compass course
258	// limit yaw rate	257	// limit yaw rate
259	w = 3 * FCParam.CompassYawEffect;	258	w = 3 * FCParam.CompassYawEffect;
260	if (v > w) v = w;	259	if (v > w) v = w;
261	else if (v < -w) v = -w;	260	else if (v < -w) v = -w;
262	ReadingIntegralGyroYaw += v;	261	ReadingIntegralGyroYaw += v;
263	}	262	}
264	else	263	else
265	{ // wait a while	264	{ // wait a while
266	BadCompassHeading--;	265	BadCompassHeading--;
267	}	266	}
268	}	267	}
269	else	268	else
270	{ // ignore compass at extreme attitudes for a while	269	{ // ignore compass at extreme attitudes for a while
271	BadCompassHeading = 500;	270	BadCompassHeading = 500;
272	}	271	}
273	}	272	}
274	}	273	}
275		274
276	#if (defined (USE_KILLAGREG) \|\| defined (USE_MK3MAG))	275	#if (defined (USE_KILLAGREG) \|\| defined (USE_MK3MAG))
277	PDPartYaw = (int32_t)(GyroYaw * 2 * (int32_t)GyroYawPFactor) / (256L / CONTROL_SCALING) + (int32_t)(IntegralGyroYaw * GyroYawIFactor) / (2 * (44000 / CONTROL_SCALING));	276	PDPartYaw = (int32_t)(GyroYaw * 2 * (int32_t)GyroYawPFactor) / (256L / CONTROL_SCALING) + (int32_t)(IntegralGyroYaw * GyroYawIFactor) / (2 * (44000 / CONTROL_SCALING));
278		277
279		278
280		279
281		280
282	YawMixFraction = PDPartYaw - SetPointYaw * CONTROL_SCALING; // yaw controller	281	YawMixFraction = PDPartYaw - SetPointYaw * CONTROL_SCALING; // yaw controller
283	#define MIN_YAWGAS (40 * CONTROL_SCALING) // yaw also below this gas value	282	#define MIN_YAWGAS (40 * CONTROL_SCALING) // yaw also below this gas value
284	// limit YawMixFraction	283	// limit YawMixFraction
285	if(GasMixFraction > MIN_YAWGAS)	284	if(GasMixFraction > MIN_YAWGAS)
286	{	285	{
287	CHECK_MIN_MAX(YawMixFraction, -(GasMixFraction / 2), (GasMixFraction / 2));	286	CHECK_MIN_MAX(YawMixFraction, -(GasMixFraction / 2), (GasMixFraction / 2));
288	}	287	}
289	else	288	else
290	{	289	{
291	CHECK_MIN_MAX(YawMixFraction, -(MIN_YAWGAS / 2), (MIN_YAWGAS / 2));	290	CHECK_MIN_MAX(YawMixFraction, -(MIN_YAWGAS / 2), (MIN_YAWGAS / 2));
292	}	291	}
293	tmp_int = ParamSet.GasMax * CONTROL_SCALING;	292	tmp_int = ParamSet.GasMax * CONTROL_SCALING;
294	CHECK_MIN_MAX(YawMixFraction, -(tmp_int - GasMixFraction), (tmp_int - GasMixFraction));	293	CHECK_MIN_MAX(YawMixFraction, -(tmp_int - GasMixFraction), (tmp_int - GasMixFraction));
295		294
296		295
297		296
298		297
299		298
300		299
301		300
302		301
303	---------------	302	---------------
304		303
305		304
306		305
307	anal-lyse:	306	anal-lyse:
308		307
309	YawMixFraction = PDPartYaw - SetPointYaw * CONTROL_SCALING;	308	YawMixFraction = PDPartYaw - SetPointYaw * CONTROL_SCALING;
310	OK	309	OK
311		310
312	SetPointYaw->setPointYaw:	311	SetPointYaw->setPointYaw:
313	- static	312	- static
314	- Set to 0 at take off	313	- Set to 0 at take off
315	- Set to yaw stick val (StickYawP/512 * StickYaw^2 + StickYawP/4 * StickYaw)	314	- Set to yaw stick val (StickYawP/512 * StickYaw^2 + StickYawP/4 * StickYaw)
316	OK	315	OK
317		316
318	PDPartYaw:	317	PDPartYaw:
319	- nonstatic nonglobal	318	- nonstatic nonglobal
320	- = (int32_t)(GyroYaw * 2 * (int32_t)GyroYawPFactor)	319	- = (int32_t)(GyroYaw * 2 * (int32_t)GyroYawPFactor)
321	/	320	/
322	(256L / CONTROL_SCALING)	321	(256L / CONTROL_SCALING)
323	+ (int32_t)(IntegralGyroYaw * GyroYawIFactor)	322	+ (int32_t)(IntegralGyroYaw * GyroYawIFactor)
324	/	323	/
325	(2 * (44000 / CONTROL_SCALING));	324	(2 * (44000 / CONTROL_SCALING));
326	OK	325	OK
327		326
328	GyroYaw:	327	GyroYaw:
329	- global	328	- global
330	- = AdBiasGyroYaw - AdValueGyroYaw (pretty much the raw offset gyro)	329	- = AdBiasGyroYaw - AdValueGyroYaw (pretty much the raw offset gyro)
331	OK	330	OK
332		331
333	YawGyroHeading->yawGyroHeading:	332	YawGyroHeading->yawGyroHeading:
334	- GyroYaw is summed to it at each iteration	333	- GyroYaw is summed to it at each iteration
335	- It is wrapped around at <0 and >=360.	334	- It is wrapped around at <0 and >=360.
336	- It is used -- where???? To adjust CompassCourse...	335	- It is used -- where???? To adjust CompassCourse...
337	OK	336	OK
338		337
339	IntegralGyroYaw->yawAngle: Deviation of heading from desired???	338	IntegralGyroYaw->yawAngle: Deviation of heading from desired???
340	- GyroYaw is summed to it at each iteration	339	- GyroYaw is summed to it at each iteration
341	- SetPointYaw is subtracted from it (!) at each iteration.	340	- SetPointYaw is subtracted from it (!) at each iteration.
342	- It is NOT wrapped, but just limited to +/- 50000	341	- It is NOT wrapped, but just limited to +/- 50000
343	- It is corrected / pulled in axis coupling and by the compass.	342	- It is corrected / pulled in axis coupling and by the compass.
344	OK (Except that with the compass)	343	OK (Except that with the compass)
345		344
346	BadCompassHeading: Need to update the "CompassCourse".	345	BadCompassHeading: Need to update the "CompassCourse".
347		346
348	CompassHeading: Opdateret fra mm3mag.	347	CompassHeading: Opdateret fra mm3mag.
349		348
350	CompassOffCourse: CompassHeading - CompassCourse. Opdateret fra mm3mag.	349	CompassOffCourse: CompassHeading - CompassCourse. Opdateret fra mm3mag.
351		350
352		351
353	UpdateCompassCourse: Set CompassCourse to the value of CompassHeading and set YawgyroHeading = compassHeading * GYRO_DEG_FACTOR	352	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 @ 2156 - Rev 1775 → 1964