WebSVN - MK3Mag - Diff - Rev 3 and 5 - /branches/MK3Mag V0.14 Code Redesign Killagreg/main.c

+/*#######################################################################################
+Flight Control
+#######################################################################################*/
+// ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
+// + 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 (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 sonstigen Medien veröffentlicht werden, muss unsere Webseite "http://www.mikrokopter.de"
+// + eindeutig als Ursprung verlinkt 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
+// ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
+// + Die Portierung der Software (oder Teile davon) auf andere Systeme (ausser der Hardware von www.mikrokopter.de) ist nur
+// + 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.
+// +   * Neither the name of the copyright holders nor the names of contributors may be used to endorse or promote products derived
+// +     from this software without specific prior written permission.
+// +   * The use of this project (hardware, software, binary files, sources and documentation) is only permittet
+// +     for non-commercial use (directly or indirectly)
+// +     Commercial use (for excample: selling of MikroKopters, selling of PCBs, assembly, ...) is only permitted
+// +     with our written permission
+// +   * If sources or documentations are redistributet on other webpages, out webpage (http://www.MikroKopter.de) must be
+// +     clearly linked as origin // +   * porting to systems other than hardware from www.mikrokopter.de is not allowed
+// +  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
+// +  AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
+// +  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.
+// ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
  signed int OffsetN, OffsetR, OffsetZ;
 /*
  signed int CalTabelleHorizontalN[4] = {67,-450,77,-411};    // Vier Messwerte 90° Horizontal gedreht
  signed int CalTabelleHorizontalR[4] = {-310,175,183,-333};
  signed int CalTabelleHorizontalZ[4] = {-100,-390,-400,100}; // Messwinkel des Z-Sensors auch horizontal gemessen
 */
  signed int CalTabelleHorizontalN[4] = {  75,-180,-440,-166};    // Vier Messwerte 90° Horizontal gedreht
  signed int CalTabelleHorizontalR[4] = {  -5,-230,  66, 246};
  signed int CalTabelleHorizontalZ[4] = {-290,-280,  44,  33}; // Messwinkel des Z-Sensors auch horizontal gemessen
   signed int RawMagnet1a,RawMagnet1b;  // reine AD-Messung
   signed int RawMagnet2a,RawMagnet2b;
   signed int RawMagnet3a,RawMagnet3b;
   signed int UncalMagnetN,UncalMagnetR,UncalMagnetZ;  // Messwert-Delta ohne Offset- und Verstärker korrektur
   signed int MagnetN,MagnetR,MagnetZ;                 // Kalibrierte Messerte
   unsigned int PwmHeading = 0;
   unsigned char PC_Connected = 0;
   int Heading;
 #include "main.h"
 //############################################################################
 //
 void Wait(unsigned char dauer)
 //############################################################################
+{
     dauer = (unsigned char)TCNT0 + dauer;
     while((TCNT0 - dauer) & 0x80);
+}
 void CalcFields(void)
+{
  UncalMagnetN = (2 * UncalMagnetN + (RawMagnet1a - RawMagnet1b)) / 3;
  UncalMagnetR = (2 * UncalMagnetR + (RawMagnet3a - RawMagnet3b)) / 3;
  UncalMagnetZ = (2 * UncalMagnetZ + (RawMagnet2a - RawMagnet2b)) / 3;
  MagnetN = UncalMagnetN - OffsetN;
  MagnetR = UncalMagnetR - OffsetR;
  MagnetZ = UncalMagnetZ - OffsetZ;
+}
 //------------------------------------------------------
 void CalcHeading(void)
+{
    double nick_rad, roll_rad, Hx, Hy, Cx, Cy, Cz;
    float nick, roll, XEx, XEy, YEy, YEz, YE, XE, XrawCal, YrawCal, ZrawCal, XEz;
    int heading, azimuthgrad;
    nick_rad = ((double)WinkelOut.Winkel[0]) * M_PI / (double)(180);
    roll_rad = ((double)WinkelOut.Winkel[1]) * M_PI / (double)(180);
    //roll_rad = 0;   nick_rad = 0;
 /*
    Cx =  (double) (MicroMag.Axis[Y_AXIS] + CY_OFFSET) * 0.707f - (double) (MicroMag.Axis[X_AXIS]+ CX_OFFSET) * 0.707f;
    Cy =  ((double) (MicroMag.Axis[Y_AXIS]+ CY_OFFSET) * 0.707f + (double) (MicroMag.Axis[X_AXIS]+ CX_OFFSET) * 0.707f);
    Cz = -MicroMag.Axis[Z_AXIS] + CZ_OFFSET;
 */
    Cx = MagnetN;
    Cy = MagnetR;
    Cz = -MagnetZ;
    Hx = Cx * (double)cos(nick_rad) +
         Cy * (double)sin(nick_rad) * (double)sin(roll_rad) -
         Cz * (double)sin(nick_rad) * (double)cos(roll_rad);
    Hy = Cy * (double)cos(roll_rad) +
         Cz * (double)sin(roll_rad);
    if(Hx == 0 && Hy < 0) heading = 90;
    else if(Hx == 0 && Hy > 0) heading = 270;
    else if(Hx < 0) heading  = 180 - (atan(Hy / Hx) * 180 / M_PI);
    else if(Hx > 0 && Hy < 0) heading = - (atan(Hy / Hx) * 180 / M_PI);
    else if(Hx > 0 && Hy > 0) heading  = 360 - (atan(Hy / Hx) * 180 / M_PI);
   // DebugOut.Analog[14] = heading;
   // if (FromFlightCtrl.IntegralNick > 0) heading = heading + FromFlightCtrl.IntegralNick/60;
  if(heading < 361) DebugOut.Analog[14] = heading;
  Heading = heading;
  PwmHeading = heading + 10;
 //   MicroMag.Heading = heading;
 //   DebugOut.Analog[14] = heading;
+}
 //############################################################################
 //Hauptprogramm
 int main (void)
 //############################################################################
+{
     DDRC  = 0x08;
     PORTC = 0x08;
     DDRD  = 0xf4;
     PORTD = 0xA0;
     DDRB  = 0x04;
     PORTB = 0x35;
     LED_ON;
     UART_Init();
     Timer0_Init();
     ADC_Init();
         InitIC2_Slave();
     sei();//Globale Interrupts Einschalten
     Debug_Timer = SetDelay(100);   // Sendeintervall
     OffsetN = (CalTabelleHorizontalN[0] + CalTabelleHorizontalN[1] + CalTabelleHorizontalN[2] + CalTabelleHorizontalN[3]) / 4;
     OffsetR = (CalTabelleHorizontalR[0] + CalTabelleHorizontalR[1] + CalTabelleHorizontalR[2] + CalTabelleHorizontalR[3]) / 4;
     OffsetZ = (CalTabelleHorizontalZ[0] + CalTabelleHorizontalZ[1] + CalTabelleHorizontalZ[2] + CalTabelleHorizontalZ[3]) / 4;
     VersionInfo.Hauptversion = VERSION_HAUPTVERSION;
     VersionInfo.Nebenversion = VERSION_NEBENVERSION;
     VersionInfo.PCKompatibel = 7;
     while (1)
+        {
     LED_ON;
          FLIP_LOW;
          Delay_ms(2);
          RawMagnet1a = MessAD(0);
          RawMagnet2a = MessAD(1);
          RawMagnet3a = MessAD(7);
          Delay_ms(1);
 LED_OFF;
          FLIP_HIGH;
          Delay_ms(2);
          RawMagnet1b = MessAD(0);
          RawMagnet2b = MessAD(1);
          RawMagnet3b = MessAD(7);
          Delay_ms(1);
          CalcFields();
          DebugOut.Analog[0] = MagnetN;
          DebugOut.Analog[1] = MagnetR;
          DebugOut.Analog[2] = MagnetZ;
          DebugOut.Analog[3] = UncalMagnetN;
          DebugOut.Analog[4] = UncalMagnetR;
          DebugOut.Analog[5] = UncalMagnetZ;
 /*
          DebugOut.Analog[3] = RawMagnet1a;
          DebugOut.Analog[4] = RawMagnet1b;
          DebugOut.Analog[5] = RawMagnet3a;
          DebugOut.Analog[6] = RawMagnet3b;*/
          DebugOut.Analog[6] = WinkelOut.Winkel[0];
          DebugOut.Analog[7] = WinkelOut.Winkel[1];
+         DebugOut.Analog[8] =  WinkelOut.UserParameter[0];
+         DebugOut.Analog[9] =  WinkelOut.UserParameter[1];
+         DebugOut.Analog[10] = WinkelOut.UserParameter[2];
+         DebugOut.Analog[11] = WinkelOut.UserParameter[3];
          CalcHeading();
          BearbeiteRxDaten();
          PC_Connected = 100;
          if(PC_Connected)
+          {
             PC_Connected--;
             DatenUebertragung();
             DDRD  |= 0x02; // TXD-Leitung
+          }
           else
+           {
             DDRD &= ~0x02; // TXD-Leitung
+           }
         } // while(1) - Hauptschleife
+}

Subversion Repositories MK3Mag

(root)/branches/MK3Mag V0.14 Code Redesign Killagreg/main.c @ 75 - Rev 3 → 5