WebSVN - FlightCtrl - Diff - Rev 1864 and 1867 - /branches/dongfang_FC



\// ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
// + Copyright (c) 04.2007 Holger Buss
// + Nur f�r den privaten Gebrauch
// + www.MikroKopter.com
// ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
// + Es gilt f�r das gesamte Projekt (Hardware, Software, Bin�rfiles, Sourcecode und Dokumentation),

/************************************************************************/
/*  Neutral Readings                                                    */
/************************************************************************/
void flight_setNeutral() {
  MKFlags |= MKFLAG_CALIBRATE;
 
  // not really used here any more.
  dynamicParams.KalmanK = -1;
  dynamicParams.KalmanMaxDrift = 0;
  dynamicParams.KalmanMaxFusion = 32;
 
  controlMixer_initVariables();
}

 
void setFlightParameters(uint8_t _Ki, uint8_t _gyroPFactor, uint8_t _gyroIFactor, uint8_t _yawPFactor, uint8_t _yawIFactor) {

   * This is the throttle part.
   */
  if(looping) {
    if(throttleTerm > staticParams.LoopGasLimit) throttleTerm = staticParams.LoopGasLimit;
  }
 
  J5LOW;

 
  /************************************************************************/
  /*  Yawing                                                              */
  /************************************************************************/

  }

 
  // Scale up to higher resolution. Hmm why is it not (from controlMixer and down) scaled already?

  throttleTerm *= CONTROL_SCALING;
 
  J5HIGH;
 
  /*
   * Compose yaw term.
   * The yaw term is limited: Absolute value is max. = the throttle term / 2.

    DebugOut.Digital[0] |= DEBUG_CLIP;
  } else if (yawTerm > (tmp_int - throttleTerm)) {
    yawTerm = (tmp_int - throttleTerm);
    DebugOut.Digital[0] |= DEBUG_CLIP;
  }
 
  J5LOW;
 
  // CHECK_MIN_MAX(yawTerm, -(tmp_int - throttleTerm), (tmp_int - throttleTerm));
  DebugOut.Digital[1] &= ~DEBUG_CLIP;
  for (axis=PITCH; axis<=ROLL; axis++) {  
    /*
     * Compose pitch and roll terms. This is finally where the sticks come into play.

    }
    CHECK_MIN_MAX(term[axis], -tmp_int, tmp_int);
  }
  // end part 3: 350 - 400 usec.

 
  J5HIGH;
 
  // +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
  // Universal Mixer
  // Each (pitch, roll, throttle, yaw) term is in the range [0..255 * CONTROL_SCALING].

    if (i < 4)
      DebugOut.Analog[22+i] = motor[i].SetPoint;
  }
  I2C_Start(TWI_STATE_MOTOR_TX);

 
  J5LOW;
 
  // +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
  // Debugging
  // +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
  if(!(--debugDataTimer)) {

    DebugOut.Analog[27] = accNoisePeak[ROLL];
    DebugOut.Analog[30] = gyroNoisePeak[PITCH];
    DebugOut.Analog[31] = gyroNoisePeak[ROLL];
    */
  }
 
}

Rev 1864	Rev 1867
Line 1...	Line 1...
1	// ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++	1	\// ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
2	// + Copyright (c) 04.2007 Holger Buss	2	// + Copyright (c) 04.2007 Holger Buss
3	// + Nur f�r den privaten Gebrauch	3	// + Nur f�r den privaten Gebrauch
4	// + www.MikroKopter.com	4	// + www.MikroKopter.com
5	// ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++	5	// ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
6	// + Es gilt f�r das gesamte Projekt (Hardware, Software, Bin�rfiles, Sourcecode und Dokumentation),	6	// + Es gilt f�r das gesamte Projekt (Hardware, Software, Bin�rfiles, Sourcecode und Dokumentation),
Line 118...	Line 118...
118	/************************************************************************/	118	/************************************************************************/
119	/* Neutral Readings */	119	/* Neutral Readings */
120	/************************************************************************/	120	/************************************************************************/
121	void flight_setNeutral() {	121	void flight_setNeutral() {
122	MKFlags \|= MKFLAG_CALIBRATE;	122	MKFlags \|= MKFLAG_CALIBRATE;
123		-
124	// not really used here any more.	123	// not really used here any more.
125	dynamicParams.KalmanK = -1;	124	dynamicParams.KalmanK = -1;
126	dynamicParams.KalmanMaxDrift = 0;	125	dynamicParams.KalmanMaxDrift = 0;
127	dynamicParams.KalmanMaxFusion = 32;	126	dynamicParams.KalmanMaxFusion = 32;
128		-
129	controlMixer_initVariables();	127	controlMixer_initVariables();
130	}	128	}
Line 131...	Line 129...
131		129
132	void setFlightParameters(uint8_t _Ki, uint8_t _gyroPFactor, uint8_t _gyroIFactor, uint8_t _yawPFactor, uint8_t _yawIFactor) {	130	void setFlightParameters(uint8_t _Ki, uint8_t _gyroPFactor, uint8_t _gyroIFactor, uint8_t _yawPFactor, uint8_t _yawIFactor) {
Line 260...	Line 258...
260	* This is the throttle part.	258	* This is the throttle part.
261	*/	259	*/
262	if(looping) {	260	if(looping) {
263	if(throttleTerm > staticParams.LoopGasLimit) throttleTerm = staticParams.LoopGasLimit;	261	if(throttleTerm > staticParams.LoopGasLimit) throttleTerm = staticParams.LoopGasLimit;
264	}	262	}
-		263
-		264	J5LOW;
Line 265...	Line 265...
265		265
266	/************************************************************************/	266	/************************************************************************/
267	/* Yawing */	267	/* Yawing */
268	/************************************************************************/	268	/************************************************************************/
Line 346...	Line 346...
346	}	346	}
Line 347...	Line 347...
347		347
348	// Scale up to higher resolution. Hmm why is it not (from controlMixer and down) scaled already?	348	// Scale up to higher resolution. Hmm why is it not (from controlMixer and down) scaled already?
Line -...	Line 349...
-		349	throttleTerm *= CONTROL_SCALING;
-		350
349	throttleTerm *= CONTROL_SCALING;	351	J5HIGH;
350		352
351	/*	353	/*
352	* Compose yaw term.	354	* Compose yaw term.
353	* The yaw term is limited: Absolute value is max. = the throttle term / 2.	355	* The yaw term is limited: Absolute value is max. = the throttle term / 2.
Line 386...	Line 388...
386	DebugOut.Digital[0] \|= DEBUG_CLIP;	388	DebugOut.Digital[0] \|= DEBUG_CLIP;
387	} else if (yawTerm > (tmp_int - throttleTerm)) {	389	} else if (yawTerm > (tmp_int - throttleTerm)) {
388	yawTerm = (tmp_int - throttleTerm);	390	yawTerm = (tmp_int - throttleTerm);
389	DebugOut.Digital[0] \|= DEBUG_CLIP;	391	DebugOut.Digital[0] \|= DEBUG_CLIP;
390	}	392	}
-		393
-		394	J5LOW;
-		395
391	// CHECK_MIN_MAX(yawTerm, -(tmp_int - throttleTerm), (tmp_int - throttleTerm));	396	// CHECK_MIN_MAX(yawTerm, -(tmp_int - throttleTerm), (tmp_int - throttleTerm));
392	DebugOut.Digital[1] &= ~DEBUG_CLIP;	397	DebugOut.Digital[1] &= ~DEBUG_CLIP;
393	for (axis=PITCH; axis<=ROLL; axis++) {	398	for (axis=PITCH; axis<=ROLL; axis++) {
394	/*	399	/*
395	* Compose pitch and roll terms. This is finally where the sticks come into play.	400	* Compose pitch and roll terms. This is finally where the sticks come into play.
Line 425...	Line 430...
425	}	430	}
426	CHECK_MIN_MAX(term[axis], -tmp_int, tmp_int);	431	CHECK_MIN_MAX(term[axis], -tmp_int, tmp_int);
427	}	432	}
428	// end part 3: 350 - 400 usec.	433	// end part 3: 350 - 400 usec.
Line -...	Line 434...
-		434
-		435	J5HIGH;
429		436
430	// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++	437	// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
431	// Universal Mixer	438	// Universal Mixer
432	// Each (pitch, roll, throttle, yaw) term is in the range [0..255 * CONTROL_SCALING].	439	// Each (pitch, roll, throttle, yaw) term is in the range [0..255 * CONTROL_SCALING].
Line 466...	Line 473...
466	if (i < 4)	473	if (i < 4)
467	DebugOut.Analog[22+i] = motor[i].SetPoint;	474	DebugOut.Analog[22+i] = motor[i].SetPoint;
468	}	475	}
469	I2C_Start(TWI_STATE_MOTOR_TX);	476	I2C_Start(TWI_STATE_MOTOR_TX);
Line -...	Line 477...
-		477
-		478	J5LOW;
470		479
471	// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++	480	// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
472	// Debugging	481	// Debugging
473	// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++	482	// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
474	if(!(--debugDataTimer)) {	483	if(!(--debugDataTimer)) {
Line 485...	Line 494...
485	DebugOut.Analog[27] = accNoisePeak[ROLL];	494	DebugOut.Analog[27] = accNoisePeak[ROLL];
486	DebugOut.Analog[30] = gyroNoisePeak[PITCH];	495	DebugOut.Analog[30] = gyroNoisePeak[PITCH];
487	DebugOut.Analog[31] = gyroNoisePeak[ROLL];	496	DebugOut.Analog[31] = gyroNoisePeak[ROLL];
488	*/	497	*/
489	}	498	}
490	J5LOW;	-
491	}	499	}

Subversion Repositories FlightCtrl

(root)/branches/dongfang_FC_rewrite/flight.c @ 1962 - Rev 1864 → 1867