WebSVN - FlightCtrl - Diff - Rev 1961 and 1963 - /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),
// + dass eine Nutzung (auch auszugsweise) nur fï¿½r den privaten und nicht-kommerziellen Gebrauch zulässig ist.
// + Sollten direkte oder indirekte kommerzielle Absichten verfolgt werden, ist mit uns (info@mikrokopter.de) Kontakt
// + bzgl. der Nutzungsbedingungen aufzunehmen.
// + Eine kommerzielle Nutzung ist z.B.Verkauf von MikroKoptern, Bestückung und Verkauf von Platinen oder Bausätzen,
// + Verkauf von Luftbildaufnahmen, usw.
// ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
// + Werden Teile des Quellcodes (mit oder ohne Modifikation) weiterverwendet oder veröffentlicht,
// + unterliegen sie auch diesen Nutzungsbedingungen und diese Nutzungsbedingungen incl. Copyright müssen dann beiliegen
// ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
// + Sollte die Software (auch auszugesweise) oder sonstige Informationen des MikroKopter-Projekts
// + auf anderen Webseiten oder Medien veröffentlicht werden, muss unsere Webseite "http://www.mikrokopter.de"
// + eindeutig als Ursprung verlinkt und genannt werden
// ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
// + Keine Gewähr auf Fehlerfreiheit, Vollständigkeit oder Funktion
// + Benutzung auf eigene Gefahr
// + Wir übernehmen keinerlei Haftung für direkte oder indirekte Personen- oder Sachschäden
// ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++

// +  IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
// +  ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE
// +  LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
// +  CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
// +  SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
// +  INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
// +  CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
// +  ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
// +  POSSIBILITY OF SUCH DAMAGE.
// ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
 
/************************************************************************/
/* Flight Attitude                                                      */
/************************************************************************/

 

  uint8_t axis;

 

  analog_update();
 
  for (axis = PITCH; axis <= ROLL; axis++) {
    rate_PID[axis] = gyro_PID[axis] + driftComp[axis];
    rate_ATT[axis] = gyro_ATT[axis] + driftComp[axis];
    differential[axis] = gyroD[axis];

    averageAcc[axis] += acc[axis];

// 480 usec with axis coupling - almost no time without.
void integrate(void) {
  // First, perform axis coupling. If disabled xxxRate is just copied to ACxxxRate.
  uint8_t axis;

 
  if (staticParams.bitConfig & CFG_AXIS_COUPLING_ACTIVE) {
    trigAxisCoupling();
  } else {
    ACRate[PITCH] = rate_ATT[PITCH];
    ACRate[ROLL] = rate_ATT[ROLL];

    if ((maxControl[PITCH] > 64) || (maxControl[ROLL] > 64)) { // reduce effect during stick commands. Replace by controlActivity.
      permilleAcc /= 2;
      debugFullWeight = 0;
    */

 
    if (controlActivity > 10000) { // reduce effect during stick control activity
      permilleAcc /= 4;
      debugOut.digital[0] |= DEBUG_ACC0THORDER;
      if (controlActivity > 20000) { // reduce effect during stick control activity
        permilleAcc /= 4;
        debugOut.digital[1] |= DEBUG_ACC0THORDER;
      }

    for (axis = PITCH; axis <= ROLL; axis++) {
      accDerived = getAngleEstimateFromAcc(axis);
      debugOut.analog[9 + axis] = (10 * accDerived) / GYRO_DEG_FACTOR_PITCHROLL;

 
      // 1000 * the correction amount that will be added to the gyro angle in next line.
      temp = angle[axis];
      angle[axis] = ((int32_t) (1000L - permilleAcc) * temp
          + (int32_t) permilleAcc * accDerived) / 1000L;
      correctionSum[axis] += angle[axis] - temp;
    }
  } else {
    debugOut.analog[9] = 0;

    debugOut.analog[10] = 0;
 
    // experiment: Kill drift compensation updates when not flying smooth.
    // correctionSum[PITCH] = correctionSum[ROLL] = 0;

  }
}

Subversion Repositories FlightCtrl

(root)/branches/dongfang_FC_rewrite/attitude.c - Rev 1961 → 1963

Rev 1961		Rev 1963
Line 2...		Line 2...
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),
7	// + dass eine Nutzung (auch auszugsweise) nur für den privaten (nicht-kommerziellen) Gebrauch zulässig ist.	7	// + dass eine Nutzung (auch auszugsweise) nur fï¿½r den privaten und nicht-kommerziellen Gebrauch zulässig ist.
8	// + Sollten direkte oder indirekte kommerzielle Absichten verfolgt werden, ist mit uns (info@mikrokopter.de) Kontakt	8	// + Sollten direkte oder indirekte kommerzielle Absichten verfolgt werden, ist mit uns (info@mikrokopter.de) Kontakt
9	// + bzgl. der Nutzungsbedingungen aufzunehmen.	9	// + bzgl. der Nutzungsbedingungen aufzunehmen.
10	// + Eine kommerzielle Nutzung ist z.B.Verkauf von MikroKoptern, Bestückung und Verkauf von Platinen oder Bausätzen,	10	// + Eine kommerzielle Nutzung ist z.B.Verkauf von MikroKoptern, Bestückung und Verkauf von Platinen oder Bausätzen,
11	// + Verkauf von Luftbildaufnahmen, usw.	11	// + Verkauf von Luftbildaufnahmen, usw.
12	// ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++	12	// ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
13	// + Werden Teile des Quellcodes (mit oder ohne Modifikation) weiterverwendet oder veröffentlicht,	13	// + Werden Teile des Quellcodes (mit oder ohne Modifikation) weiterverwendet oder veröffentlicht,
14	// + unterliegen sie auch diesen Nutzungsbedingungen und diese Nutzungsbedingungen incl. Copyright müssen dann beiliegen	14	// + unterliegen sie auch diesen Nutzungsbedingungen und diese Nutzungsbedingungen incl. Copyright müssen dann beiliegen
15	// ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++	15	// ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
16	// + Sollte die Software (auch auszugesweise) oder sonstige Informationen des MikroKopter-Projekts	16	// + Sollte die Software (auch auszugesweise) oder sonstige Informationen des MikroKopter-Projekts
17	// + auf anderen Webseiten oder sonstigen Medien veröffentlicht werden, muss unsere Webseite "http://www.mikrokopter.de"	17	// + auf anderen Webseiten oder Medien veröffentlicht werden, muss unsere Webseite "http://www.mikrokopter.de"
18	// + eindeutig als Ursprung verlinkt werden	18	// + eindeutig als Ursprung verlinkt und genannt werden
19	// ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++	19	// ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
20	// + Keine Gewähr auf Fehlerfreiheit, Vollständigkeit oder Funktion	20	// + Keine Gewähr auf Fehlerfreiheit, Vollständigkeit oder Funktion
21	// + Benutzung auf eigene Gefahr	21	// + Benutzung auf eigene Gefahr
22	// + Wir übernehmen keinerlei Haftung für direkte oder indirekte Personen- oder Sachschäden	22	// + Wir übernehmen keinerlei Haftung für direkte oder indirekte Personen- oder Sachschäden
23	// ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++	23	// ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
Line 42...		Line 42...
42	// + IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE	42	// + IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
43	// + ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE	43	// + ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE
44	// + LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR	44	// + LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
45	// + CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF	45	// + CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
46	// + SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS	46	// + SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
-		47	// + INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
47	// + INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN// + CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)	48	// + CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
48	// + ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE	49	// + ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
49	// + POSSIBILITY OF SUCH DAMAGE.	50	// + POSSIBILITY OF SUCH DAMAGE.
50	// ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++	51	// ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
51		-
52	/************************************************************************/	52	/************************************************************************/
53	/* Flight Attitude */	53	/* Flight Attitude */
54	/************************************************************************/	54	/************************************************************************/
Line 55...		Line 55...
55		55
Line 204...		Line 204...
204	uint8_t axis;	204	uint8_t axis;
Line 205...		Line 205...
205		205
Line 206...		Line 206...
206	analog_update();	206	analog_update();
207		207
208	for (axis = PITCH; axis <= ROLL; axis++) {	208	for (axis = PITCH; axis <= ROLL; axis++) {
209	rate_PID[axis] = gyro_PID[axis] /* / HIRES_GYRO_INTEGRATION_FACTOR */ + driftComp[axis];	209	rate_PID[axis] = gyro_PID[axis] + driftComp[axis];
210	rate_ATT[axis] = gyro_ATT[axis] /* / HIRES_GYRO_INTEGRATION_FACTOR */ + driftComp[axis];	210	rate_ATT[axis] = gyro_ATT[axis] + driftComp[axis];
211	differential[axis] = gyroD[axis];	211	differential[axis] = gyroD[axis];
Line 212...		Line 212...
212	averageAcc[axis] += acc[axis];	212	averageAcc[axis] += acc[axis];
Line 246...		Line 246...
246	// 480 usec with axis coupling - almost no time without.	246	// 480 usec with axis coupling - almost no time without.
247	void integrate(void) {	247	void integrate(void) {
248	// First, perform axis coupling. If disabled xxxRate is just copied to ACxxxRate.	248	// First, perform axis coupling. If disabled xxxRate is just copied to ACxxxRate.
249	uint8_t axis;	249	uint8_t axis;
Line 250...		Line 250...
250		250
251	if (/!looping && / (staticParams.bitConfig & CFG_AXIS_COUPLING_ACTIVE)) {	251	if (staticParams.bitConfig & CFG_AXIS_COUPLING_ACTIVE) {
252	trigAxisCoupling();	252	trigAxisCoupling();
253	} else {	253	} else {
254	ACRate[PITCH] = rate_ATT[PITCH];	254	ACRate[PITCH] = rate_ATT[PITCH];
255	ACRate[ROLL] = rate_ATT[ROLL];	255	ACRate[ROLL] = rate_ATT[ROLL];
Line 312...		Line 312...
312	if ((maxControl[PITCH] > 64) \|\| (maxControl[ROLL] > 64)) { // reduce effect during stick commands. Replace by controlActivity.	312	if ((maxControl[PITCH] > 64) \|\| (maxControl[ROLL] > 64)) { // reduce effect during stick commands. Replace by controlActivity.
313	permilleAcc /= 2;	313	permilleAcc /= 2;
314	debugFullWeight = 0;	314	debugFullWeight = 0;
315	*/	315	*/
Line 316...		Line 316...
316		316
317	if (controlActivity > 10000) { // reduce effect during stick commands	317	if (controlActivity > 10000) { // reduce effect during stick control activity
318	permilleAcc /= 4;	318	permilleAcc /= 4;
319	debugOut.digital[0] \|= DEBUG_ACC0THORDER;	319	debugOut.digital[0] \|= DEBUG_ACC0THORDER;
320	if (controlActivity > 20000) { // reduce effect during stick commands	320	if (controlActivity > 20000) { // reduce effect during stick control activity
321	permilleAcc /= 4;	321	permilleAcc /= 4;
322	debugOut.digital[1] \|= DEBUG_ACC0THORDER;	322	debugOut.digital[1] \|= DEBUG_ACC0THORDER;
323	}	323	}
Line 329...		Line 329...
329	for (axis = PITCH; axis <= ROLL; axis++) {	329	for (axis = PITCH; axis <= ROLL; axis++) {
330	accDerived = getAngleEstimateFromAcc(axis);	330	accDerived = getAngleEstimateFromAcc(axis);
331	debugOut.analog[9 + axis] = (10 * accDerived) / GYRO_DEG_FACTOR_PITCHROLL;	331	debugOut.analog[9 + axis] = (10 * accDerived) / GYRO_DEG_FACTOR_PITCHROLL;
Line 332...		Line 332...
332		332
333	// 1000 * the correction amount that will be added to the gyro angle in next line.	333	// 1000 * the correction amount that will be added to the gyro angle in next line.
334	temp = angle[axis]; //(permilleAcc * (accDerived - angle[axis])) / 1000;	334	temp = angle[axis];
335	angle[axis] = ((int32_t) (1000L - permilleAcc) * temp	335	angle[axis] = ((int32_t) (1000L - permilleAcc) * temp
336	+ (int32_t) permilleAcc * accDerived) / 1000L;	336	+ (int32_t) permilleAcc * accDerived) / 1000L;
337	correctionSum[axis] += angle[axis] - temp;	337	correctionSum[axis] += angle[axis] - temp;
338	}	338	}
339	} else {	339	} else {
340	debugOut.analog[9] = 0;	340	debugOut.analog[9] = 0;
Line 341...		Line 341...
341	debugOut.analog[10] = 0;	341	debugOut.analog[10] = 0;
342		342
343	// experiment: Kill drift compensation updates when not flying smooth.	343	// experiment: Kill drift compensation updates when not flying smooth.
344	correctionSum[PITCH] = correctionSum[ROLL] = 0;	344	// correctionSum[PITCH] = correctionSum[ROLL] = 0;
Line 345...		Line 345...
345	}	345	}
346	}	346	}