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
		120	if(RC_Quality > 140)
		121	{
		122	if(ModelIsFlying < 256)
		123	{
		124	StickYaw = 0;
		125	if(ModelIsFlying == 250)
		126	{
		127	UpdateCompassCourse = 1;
		128	ReadingIntegralGyroYaw = 0;
		129	SetPointYaw = 0;
		130	}
		131	}
		132	else MKFlags \|= (MKFLAG_FLY); // set fly flag
		133
		134	......
		135
		136	if(ParamSet.GlobalConfig & (CFG_COMPASS_ACTIVE\|CFG_GPS_ACTIVE))
		137	{
		138	// if roll stick is centered and nick stick is down
		139	if (abs(PPM_in[ParamSet.ChannelAssignment[CH_ROLL]]) < 30 && PPM_in[ParamSet.ChannelAssignment[CH_NICK]] < -70)
		140	{
		141	CompassCalState = 1;
		142	BeepTime = 1000;
		143	}
		144
		145	(R/C data):
		146
		147	// mapping of yaw
		148	StickYaw = -PPM_in[ParamSet.ChannelAssignment[CH_YAW]];
		149	// (range of -2 .. 2 is set to zero, to avoid unwanted yaw trimming on compass correction)
		150	if(ParamSet.GlobalConfig & (CFG_COMPASS_ACTIVE\|CFG_GPS_ACTIVE))
		151	{
		152	if (StickYaw > 2) StickYaw-= 2;
		153	else if (StickYaw< -2) StickYaw += 2;
		154	else StickYaw = 0;
		155	}
		156
		157	if(ExternControl.Config & 0x01 && FCParam.ExternalControl > 128)
		158	{
		159	StickYaw += ExternControl.Yaw;
		160	// disable I part of gyro control feedback
		161	if(ParamSet.GlobalConfig & CFG_HEADING_HOLD) GyroIFactor = 0;
		162
		163
		164
		165
		166	// MeasurementCounter is incremented in the isr of analog.c
		167	if(MeasurementCounter >= BALANCE_NUMBER) // averaging number has reached
		168	{
		169	if(ParamSet.DriftComp)
		170	{
		171	if(YawGyroDrift > BALANCE_NUMBER/2) AdBiasGyroYaw++;
		172	if(YawGyroDrift < -BALANCE_NUMBER/2) AdBiasGyroYaw--;
		173	}
		174	YawGyroDrift = 0;
		175
		176
		177	// Yawing
		178	// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
		179	if(abs(StickYaw) > 15 ) // yaw stick is activated
		180	{
		181	BadCompassHeading = 1000;
		182	if(!(ParamSet.GlobalConfig & CFG_COMPASS_FIX))
		183	{
		184	UpdateCompassCourse = 1;
		185	}
		186	}
		187	// exponential stick sensitivity in yawring rate
		188	tmp_int = (int32_t) ParamSet.StickYawP * ((int32_t)StickYaw * abs(StickYaw)) / 512L; // expo y = ax + bx?
		189	tmp_int += (ParamSet.StickYawP * StickYaw) / 4;
		190
		191	SetPointYaw = tmp_int;
		192	// trimm drift of ReadingIntegralGyroYaw with SetPointYaw(StickYaw)
		193	ReadingIntegralGyroYaw -= tmp_int;
		194	// limit the effect
		195	CHECK_MIN_MAX(ReadingIntegralGyroYaw, -50000, 50000)
		196
		197	// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
		198	// Compass
		199	// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
		200	// compass code is used if Compass option is selected
		201	if(ParamSet.GlobalConfig & (CFG_COMPASS_ACTIVE\|CFG_GPS_ACTIVE))
		202	{
		203	int16_t w, v, r,correction, error;
		204
		205	if(CompassCalState && !(MKFlags & MKFLAG_MOTOR_RUN) )
		206	{
		207	SetCompassCalState();
		208	#ifdef USE_KILLAGREG
		209	MM3_Calibrate();
		210	#endif
		211	}
		212	else
		213	{
		214	#ifdef USE_KILLAGREG
		215	static uint8_t updCompass = 0;
		216	if (!updCompass--)
		217	{
		218	updCompass = 49; // update only at 2ms*50 = 100ms (10Hz)
		219	MM3_Heading();
		220	}
		221	#endif
		222
		223	// get maximum attitude angle
		224	w = abs(IntegralGyroNick / 512);
		225	v = abs(IntegralGyroRoll / 512);
		226	if(v > w) w = v;
		227	correction = w / 8 + 1;
		228	// calculate the deviation of the yaw gyro heading and the compass heading
		229	if (CompassHeading < 0) error = 0; // disable yaw drift compensation if compass heading is undefined
		230	else error = ((540 + CompassHeading - (YawGyroHeading / GYRO_DEG_FACTOR)) % 360) - 180;
		231	if(abs(GyroYaw) > 128) // spinning fast
		232	{
		233	error = 0;
		234	}
		235	if(!BadCompassHeading && w < 25)
		236	{
		237	YawGyroDrift += error;
		238	if(UpdateCompassCourse)
		239	{
		240	BeepTime = 200;
		241	YawGyroHeading = (int32_t)CompassHeading * GYRO_DEG_FACTOR;
		242	CompassCourse = (int16_t)(YawGyroHeading / GYRO_DEG_FACTOR);
		243	UpdateCompassCourse = 0;
		244	}
		245	}
		246	YawGyroHeading += (error * 8) / correction;
		247	w = (w * FCParam.CompassYawEffect) / 32;
		248	w = FCParam.CompassYawEffect - w;
		249	if(w >= 0)
		250	{
		251	if(!BadCompassHeading)
		252	{
		253	v = 64 + (MaxStickNick + MaxStickRoll) / 8;
		254	// calc course deviation
		255	r = ((540 + (YawGyroHeading / GYRO_DEG_FACTOR) - CompassCourse) % 360) - 180;
		256	v = (r * w) / v; // align to compass course
		257	// limit yaw rate
		258	w = 3 * FCParam.CompassYawEffect;
		259	if (v > w) v = w;
		260	else if (v < -w) v = -w;
		261	ReadingIntegralGyroYaw += v;
		262	}
		263	else
		264	{ // wait a while
		265	BadCompassHeading--;
		266	}
		267	}
		268	else
		269	{ // ignore compass at extreme attitudes for a while
		270	BadCompassHeading = 500;
		271	}
		272	}
		273	}
		274
		275	#if (defined (USE_KILLAGREG) \|\| defined (USE_MK3MAG))
		276	PDPartYaw = (int32_t)(GyroYaw * 2 * (int32_t)GyroYawPFactor) / (256L / CONTROL_SCALING) + (int32_t)(IntegralGyroYaw * GyroYawIFactor) / (2 * (44000 / CONTROL_SCALING));
		277
		278
		279
		280
		281	YawMixFraction = PDPartYaw - SetPointYaw * CONTROL_SCALING; // yaw controller
		282	#define MIN_YAWGAS (40 * CONTROL_SCALING) // yaw also below this gas value
		283	// limit YawMixFraction
		284	if(GasMixFraction > MIN_YAWGAS)
		285	{
		286	CHECK_MIN_MAX(YawMixFraction, -(GasMixFraction / 2), (GasMixFraction / 2));
		287	}
		288	else
		289	{
		290	CHECK_MIN_MAX(YawMixFraction, -(MIN_YAWGAS / 2), (MIN_YAWGAS / 2));
		291	}
		292	tmp_int = ParamSet.GasMax * CONTROL_SCALING;
		293	CHECK_MIN_MAX(YawMixFraction, -(tmp_int - GasMixFraction), (tmp_int - GasMixFraction));
		294
		295
		296
		297
		298
		299
		300
		301
		302	---------------
		303
		304
		305
		306	anal-lyse:
		307
		308	YawMixFraction = PDPartYaw - SetPointYaw * CONTROL_SCALING;
		309	OK
		310
		311	SetPointYaw->setPointYaw:
		312	- static
		313	- Set to 0 at take off
		314	- Set to yaw stick val (StickYawP/512 * StickYaw^2 + StickYawP/4 * StickYaw)
		315	OK
		316
		317	PDPartYaw:
		318	- nonstatic nonglobal
		319	- = (int32_t)(GyroYaw * 2 * (int32_t)GyroYawPFactor)
		320	/
		321	(256L / CONTROL_SCALING)
		322	+ (int32_t)(IntegralGyroYaw * GyroYawIFactor)
		323	/
		324	(2 * (44000 / CONTROL_SCALING));
		325	OK
		326
		327	GyroYaw:
		328	- global
		329	- = AdBiasGyroYaw - AdValueGyroYaw (pretty much the raw offset gyro)
		330	OK
		331
		332	YawGyroHeading->yawGyroHeading:
		333	- GyroYaw is summed to it at each iteration
		334	- It is wrapped around at <0 and >=360.
		335	- It is used -- where???? To adjust CompassCourse...
		336	OK
		337
		338	IntegralGyroYaw->yawAngle: Deviation of heading from desired???
		339	- GyroYaw is summed to it at each iteration
		340	- SetPointYaw is subtracted from it (!) at each iteration.
		341	- It is NOT wrapped, but just limited to +/- 50000
		342	- It is corrected / pulled in axis coupling and by the compass.
		343	OK (Except that with the compass)
		344
		345	BadCompassHeading: Need to update the "CompassCourse".
		346
		347	CompassHeading: Opdateret fra mm3mag.
		348
		349	CompassOffCourse: CompassHeading - CompassCourse. Opdateret fra mm3mag.
		350
		351
		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 @ 2022 - Rev