WebSVN - FlightCtrl - Blame - /branches/dongfang_FC

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