WebSVN - FlightCtrl - Diff - Rev 2026 and 2032 - /branches/dongfang_FC

 // ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
 // + Copyright (c) 04.2007 Holger Buss
-// + Nur für den privaten Gebrauch
+// + Nur f�r den privaten Gebrauch
 // + www.MikroKopter.com
 // ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
-// + Es gilt für das gesamte Projekt (Hardware, Software, Binärfiles, Sourcecode und Dokumentation),
+// + 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.
+// + 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,
+// + 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,
+// + 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
+// + 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"
+// + 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
+// + 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
+// + Wir �bernehmen keinerlei Haftung f�r direkte oder indirekte Personen- oder Sachsch�den
 // ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
 // + Die Portierung der Software (oder Teile davon) auf andere Systeme (ausser der Hardware von www.mikrokopter.de) ist nur
-// + mit unserer Zustimmung zulässig
+// + mit unserer Zustimmung zul�ssig
 // ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
 // + Die Funktion printf_P() unterliegt ihrer eigenen Lizenz und ist hiervon nicht betroffen
 // ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
 // + Redistributions of source code (with or without modifications) must retain the above copyright notice,
 // + this list of conditions and the following disclaimer.
  * it is hardly worth the trouble.
  ************************************************************************/
 int32_t getAngleEstimateFromAcc(uint8_t axis) {
   //int32_t correctionTerm = (dynamicParams.levelCorrection[axis] - 128) * 256L;
+  return GYRO_ACC_FACTOR * filteredAcc[axis];// + correctionTerm;
-  return GYRO_ACC_FACTOR * (int32_t) filteredAcc[axis];// + correctionTerm;
+  // return 342L * filteredAcc[axis];
+}
 void setStaticAttitudeAngles(void) {
 /************************************************************************
  * Neutral Readings
  ************************************************************************/
 void attitude_setNeutral(void) {
   // Servo_Off(); // disable servo output. TODO: Why bother? The servos are going to make a jerk anyway.
-  dynamicParams.axisCoupling1 = dynamicParams.axisCoupling2 = 0;
+  // dynamicParams.axisCoupling1 = dynamicParams.axisCoupling2 = 0;
   driftComp[PITCH] = driftComp[ROLL] = yawGyroDrift = driftCompYaw = 0;
   correctionSum[PITCH] = correctionSum[ROLL] = 0;
     /*
      * Add to each sum: The amount by which the angle is changed just below.
      */
     for (axis = PITCH; axis <= ROLL; axis++) {
       accDerived = getAngleEstimateFromAcc(axis);
-      debugOut.analog[9 + axis] = (10 * accDerived) / GYRO_DEG_FACTOR_PITCHROLL;
+      //debugOut.analog[9 + axis] = accDerived / (GYRO_DEG_FACTOR_PITCHROLL / 10);
       // 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[9] = 0;
-    debugOut.analog[10] = 0;
+    //debugOut.analog[10] = 0;
     // experiment: Kill drift compensation updates when not flying smooth.
     // correctionSum[PITCH] = correctionSum[ROLL] = 0;
     debugOut.digital[0] |= DEBUG_ACC0THORDER;
+  }
+  int32_t accDerived = getAngleEstimateFromAcc(0);
+  debugOut.analog[9 + 0] = accDerived / (GYRO_DEG_FACTOR_PITCHROLL / 10);
+  accDerived = getAngleEstimateFromAcc(1);
+  debugOut.analog[9 + 1] = accDerived / (GYRO_DEG_FACTOR_PITCHROLL / 10);
+}
 /************************************************************************
  * This is an attempt to correct not the error in the angle integrals

Subversion Repositories FlightCtrl

(root)/branches/dongfang_FC_rewrite/attitude.c - Rev 2026 → 2032