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

 /*#######################################################################################
 MK3Mag 3D-Magnet sensor
 !!! THIS IS NOT FREE SOFTWARE !!!
 #######################################################################################*/
 // ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
 // + Copyright (c) 05.2008 Holger Buss
 // + Thanks to Ilja Fähnrich (P_Latzhalter)
 // + Nur für den privaten Gebrauch
 // + www.MikroKopter.com
 // ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
 // + Die Portierung der Software (oder Teile davon) auf andere Systeme (ausser der Hardware von www.mikrokopter.de) ist nur
 // + mit unserer Zustimmung zulässig
 // ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
 // + 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.
 // + AUSNAHME: Ein bei www.mikrokopter.de erworbener vorbestückter MK3Mag darf als Baugruppe auch in kommerziellen Systemen verbaut werden
 // + Im Zweifelsfall bitte anfragen bei: info@mikrokopter.de
 // ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
 // + 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
 // + Benutzung auf eigene Gefahr
 // + Wir übernehmen keinerlei Haftung für direkte oder indirekte Personen- oder Sachschäden
 // ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
 // + 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.
 // +   * PORTING this software (or parts of it) to systems (other than hardware from www.mikrokopter.de) is NOT allowed
 // +   * 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
 // +     Exception: A preassembled MK3Mag, purchased from www.mikrokopter.de may be used as a part of commercial systems
 // +     In case of doubt please contact: info@MikroKopter.de
 // +   * If sources or documentations are redistributet on other webpages, our webpage (http://www.MikroKopter.de) must be
 // +     clearly linked as origin
 // +  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.
 // ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
+#include <avr/interrupt.h>
+#include <math.h>
+#include <stdlib.h>
-signed int OffsetN, OffsetR, OffsetZ;
-  signed int RawMagnet1a,RawMagnet1b;  // raw AD-Data
-  signed int RawMagnet2a,RawMagnet2b;
-  signed int RawMagnet3a,RawMagnet3b;
-  signed int Xmin = 0, Xmax = 0; Ymin = 0, Ymax = 0; Zmin = 0, Zmax = 0;
-  signed int UncalMagnetN,UncalMagnetR,UncalMagnetZ;  // Messwert-Delta ohne Offset- und Verstärker korrektur
-  signed int MagnetN,MagnetR,MagnetZ;
-  unsigned int PwmHeading = 0;
-  unsigned int PC_Connected = 0;
   unsigned int Heading;
+#include "main.h"
+#include "timer0.h"
+#include "twislave.h"
+#include "led.h"
+#include "analog.h"
-#include "main.h"
-uint16_t eeXmin EEMEM = 0;
-uint16_t eeXmax EEMEM = 0;
-uint16_t eeYmin EEMEM = 0;
-uint16_t eeYmax EEMEM = 0;
+#include "uart.h"
-uint16_t eeZmin EEMEM = 0;
-uint16_t eeZmax EEMEM = 0;
+int16_t RawMagnet1a, RawMagnet1b;  // raw AD-Data
-//############################################################################
+int16_t RawMagnet2a, RawMagnet2b;
+int16_t RawMagnet3a, RawMagnet3b;
+typedef struct
+{
+        int16_t Range;
+        int16_t Offset;
+} Scaling_t;
+typedef struct
+{
+        Scaling_t X;
+        Scaling_t Y;
-//
+        Scaling_t Z;
+} Calibration_t;
+Calibration_t eeCalibration EEMEM; // calibration data in EEProm
+Calibration_t Calibration;         // calibration data in RAM
-void Wait(unsigned char dauer)
+int16_t UncalMagnetX, UncalMagnetY, UncalMagnetZ;  // sensor signal difference without Scaling
+int16_t MagnetX, MagnetY, MagnetZ;                               // rescaled magnetic field readings
-//############################################################################
 uint8_t PC_Connected = 0;
     dauer = (unsigned char)TCNT0 + dauer;
-    while((TCNT0 - dauer) & 0x80);
+int16_t Heading;
-}
-void CalcFields(void)
-{
- UncalMagnetN = (1 * UncalMagnetN + (RawMagnet1a - RawMagnet1b)) / 2;
- UncalMagnetR = (1 * UncalMagnetR + (RawMagnet3a - RawMagnet3b)) / 2;
- UncalMagnetZ = (1 * UncalMagnetZ + (RawMagnet2a - RawMagnet2b)) / 2;
+void CalcFields(void)
+{
- OffsetN = (Xmin + Xmax) / 2;
+        UncalMagnetX = (1 * UncalMagnetX + (RawMagnet1a - RawMagnet1b)) / 2;
- OffsetR = (Ymin + Ymax) / 2;
+        UncalMagnetY = (1 * UncalMagnetY + (RawMagnet3a - RawMagnet3b)) / 2;
- OffsetZ = (Zmin + Zmax) / 2;
+        UncalMagnetZ = (1 * UncalMagnetZ + (RawMagnet2a - RawMagnet2b)) / 2;
- MagnetN = (1024L * (long)(UncalMagnetN - OffsetN)) / (Xmax - Xmin);
+        MagnetX = (1024L * (int32_t)(UncalMagnetX - Calibration.X.Offset)) / (Calibration.X.Range);
- MagnetR = (1024L * (long)(UncalMagnetR - OffsetR)) / (Ymax - Ymin);
+        MagnetY = (1024L * (int32_t)(UncalMagnetY - Calibration.Y.Offset)) / (Calibration.Y.Range);
- MagnetZ = (1024L * (long)(UncalMagnetZ - OffsetZ)) / (Zmax - Zmin);
+        MagnetZ = (1024L * (int32_t)(UncalMagnetZ - Calibration.Z.Offset)) / (Calibration.Z.Range);
 }
+void CalcHeading(void)
-void CalcHeading(void)
+{
-{
+        double nick_rad, roll_rad, Hx, Hy, Cx, Cy, Cz;
-   double nick_rad, roll_rad, Hx, Hy, Cx, Cy, Cz;
+        int16_t heading = -1;
-   int heading;
+        // calculate nick and roll angle i rad
-   nick_rad = ((double)ExternData.Winkel[0]) * M_PI / (double)(1800);
+        nick_rad = ((double)ExternData.Attitude[NICK]) * M_PI / (double)(1800);
-   roll_rad = ((double)ExternData.Winkel[1]) * M_PI / (double)(1800);
+        roll_rad = ((double)ExternData.Attitude[ROLL]) * M_PI / (double)(1800);
-   Cx = MagnetN;
+        Cx = MagnetX;
-   Cy = MagnetR;
+        Cy = MagnetY;
-   Cz = MagnetZ;
+        Cz = MagnetZ;
- if(ExternData.Orientation == 1)
+        if(ExternData.Orientation == 1)
-  {
+        {
-   Cx = MagnetR;
+                Cx = MagnetX;
-   Cy = -MagnetN;
+                Cy = -MagnetY;
-   Cz = MagnetZ;
+                Cz = MagnetZ;
-  }
+        }
-   Hx = Cx * (double)cos(nick_rad) +
+        Hx = Cx * (double)cos(nick_rad) +
-        Cy * (double)sin(nick_rad) * (double)sin(roll_rad) -
+                Cy * (double)sin(nick_rad) * (double)sin(roll_rad) -
-        Cz * (double)sin(nick_rad) * (double)cos(roll_rad);
+                Cz * (double)sin(nick_rad) * (double)cos(roll_rad);
-   Hy = Cy * (double)cos(roll_rad) +
+        Hy = Cy * (double)cos(roll_rad) +
-        Cz * (double)sin(roll_rad);
+                Cz * (double)sin(roll_rad);
+        if(Hx == 0 && Hy < 0) heading = 90;
-   if(Hx == 0 && Hy < 0) heading = 90;
-   else if(Hx == 0 && Hy > 0) heading = 270;
+        else if(Hx == 0 && Hy > 0) heading = 270;
-   else if(Hx < 0) heading  = 180 - (atan(Hy / Hx) * 180.0) / M_PI;
+        else if(Hx < 0) heading  = 180 - (atan(Hy / Hx) * 180.0) / M_PI;
-   else if(Hx > 0 && Hy < 0) heading = - (atan(Hy / Hx) * 180.0) / M_PI;
+        else if(Hx > 0 && Hy < 0) heading = - (atan(Hy / Hx) * 180.0) / M_PI;
-   else if(Hx > 0 && Hy > 0) heading  = 360 - (atan(Hy / Hx) * 180.0) / M_PI;
+        else if(Hx > 0 && Hy > 0) heading  = 360 - (atan(Hy / Hx) * 180.0) / M_PI;
+        if(abs(heading) < 361) Heading = heading;
+}
- if(abs(heading) < 361) Heading = heading;
+void Calibrate(void)
- PwmHeading = Heading + 10;
+{
-}
+        uint8_t cal;
+        static int16_t Xmin = 0, Xmax = 0, Ymin = 0, Ymax = 0, Zmin = 0, Zmax = 0;
-void Calibrate(void)
+        // check both sources of communication for calibration request
+        if(I2C_WriteCal.CalByte) cal = I2C_WriteCal.CalByte;
+        else                     cal = ExternData.CalState;
-{
+        // calibration state machine
+        switch(cal)
- unsigned char cal;
+        {
- if(I2C_WriteCal.CalByte) cal = I2C_WriteCal.CalByte;
+                case 0: // no calibration
- else                     cal = ExternData.CalState;
+                        LED_GRN_ON;
- switch(cal)
+                        break;
-  {
-   case 0:
+                case 1: // 1st step of calibration
-                LED_ON;
+                        // initialize ranges
+                        // used to change the orientation of the MK3MAG in the horizontal plane
-          break;
+                        LED_GRN_OFF;
-   case 1:
-        Xmin =  10000;
+                        Xmin =  10000;
+                        Xmax = -10000;
-        Xmax = -10000;
+                        Ymin =  10000;
-        Ymin =  10000;
+                        Ymax = -10000;
-        Ymax = -10000;
+                        Zmin =  10000;
-        Zmin =  10000;
+                        Zmax = -10000;
-        Zmax = -10000;
+                        break;
-                LED_OFF;
+                case 2: // 2nd step of calibration
+                        // find Min and Max of the X- and Y-Sensors during rotation in the horizontal plane
-          break;
+                        LED_GRN_ON;
-   case 2:
+                        if(UncalMagnetX < Xmin) Xmin = UncalMagnetX;
+                        if(UncalMagnetX > Xmax) Xmax = UncalMagnetX;
-                LED_ON;  // find Min and Max of the X- and Y-Sensors
+                        if(UncalMagnetY < Ymin) Ymin = UncalMagnetY;
-              if(UncalMagnetN < Xmin) Xmin = UncalMagnetN;
+                        if(UncalMagnetY > Ymax) Ymax = UncalMagnetY;
+                        break;
-              if(UncalMagnetN > Xmax) Xmax = UncalMagnetN;
-              if(UncalMagnetR < Ymin) Ymin = UncalMagnetR;
+                case 3: // 3rd step of calibration
-              if(UncalMagnetR > Ymax) Ymax = UncalMagnetR;
+                        // used to change the orietation of the MK3MAG vertical to the horizontal plane
+                        LED_GRN_OFF;
-          break;
+                        break;
-   case 3:
-                LED_OFF;
+                case 4:
-          break;
-   case 4:
+                        // find Min and Max of the Z-Sensor
-                LED_ON;  // find Min and Max of the Z-Sensor
+                        LED_GRN_ON;
-          if(UncalMagnetZ < Zmin) Zmin = UncalMagnetZ;
+                        if(UncalMagnetZ < Zmin) Zmin = UncalMagnetZ;
-              if(UncalMagnetZ > Zmax) Zmax = UncalMagnetZ;
+                        if(UncalMagnetZ > Zmax) Zmax = UncalMagnetZ;
-          break;
+                        break;
+                case 5:
-   case 5:
+                        LED_GRN_OFF; // Save values
-                LED_OFF; // Save values
+                        Calibration.X.Range = Xmax - Xmin;
+                        Calibration.X.Offset = (Xmin + Xmax) / 2;
+                        Calibration.Y.Range = Ymax - Ymin;
+                        Calibration.Y.Offset = (Ymin + Ymax) / 2;
+                        Calibration.Z.Range = Zmax - Zmin;
-         if((Xmax - Xmin) > 150 && (Ymax - Ymin) > 150 && (Zmax - Zmin) > 150)
+                        Calibration.Z.Offset = (Zmin + Zmax) / 2;
-          {
+                        if((Calibration.X.Range > 150) && (Calibration.Y.Range > 150) && (Calibration.Z.Range > 150))
-               eeprom_write_word(&eeXmin, Xmin);
+                        {
-               eeprom_write_word(&eeXmax, Xmax);
+                                eeprom_write_block(&Calibration, &eeCalibration, sizeof(Calibration_t));
-               eeprom_write_word(&eeYmin, Ymin);
+                                Delay_ms(2000);
                eeprom_write_word(&eeYmax, Ymax);
-               eeprom_write_word(&eeZmin, Zmin);
+                        LED_GRN_ON;
-               eeprom_write_word(&eeZmax, Zmax);
+                        break;
-           Delay_ms(2000);
-          }
+                default:
-                LED_ON;
+                        LED_GRN_ON;
-        break;
+                        break;
-  }
+        }
 }
 void SetDebugValues(void)
 {
-         DebugOut.Analog[0] =  MagnetN;
+        DebugOut.Analog[0] =  MagnetX;
-         DebugOut.Analog[1] =  MagnetR;
+        DebugOut.Analog[1] =  MagnetY;
-         DebugOut.Analog[2] =  MagnetZ;
+        DebugOut.Analog[2] =  MagnetZ;
-                 DebugOut.Analog[3] =  UncalMagnetN;
+        DebugOut.Analog[3] =  UncalMagnetX;
-                 DebugOut.Analog[4] =  UncalMagnetR;
+        DebugOut.Analog[4] =  UncalMagnetY;
-                 DebugOut.Analog[5] =  UncalMagnetZ;
+        DebugOut.Analog[5] =  UncalMagnetZ;
-         DebugOut.Analog[6] =  ExternData.Winkel[0];
+        DebugOut.Analog[6] =  ExternData.Attitude[NICK];
-         DebugOut.Analog[7] =  ExternData.Winkel[1];
+        DebugOut.Analog[7] =  ExternData.Attitude[ROLL];
-                 DebugOut.Analog[8] =  Xmin;
-                 DebugOut.Analog[9] =  Xmax;
-                 DebugOut.Analog[10] = Ymin;
+        DebugOut.Analog[8] =  Calibration.X.Offset;
-                 DebugOut.Analog[11] = Ymax;
-                 DebugOut.Analog[12] = Zmin;
+        DebugOut.Analog[9] =  Calibration.X.Range;
-                 DebugOut.Analog[13] = Zmax;
-                 DebugOut.Analog[14] = ExternData.CalState;
-         DebugOut.Analog[15] = Heading;
-         DebugOut.Analog[16] = ExternData.UserParameter[0];
-         DebugOut.Analog[17] = ExternData.UserParameter[1];
+        DebugOut.Analog[10] = Calibration.Y.Offset;
-}
+        DebugOut.Analog[11] = Calibration.Y.Range;
-//############################################################################
-//Hauptprogramm
+        DebugOut.Analog[12] = Calibration.Z.Offset;
-int main (void)
+        DebugOut.Analog[13] = Calibration.Z.Range;
-//############################################################################
+        DebugOut.Analog[14] = ExternData.CalState;
-{
+        DebugOut.Analog[15] = Heading;
+        DebugOut.Analog[16] = ExternData.UserParam[0];
-    DDRC  = 0x08;
+        DebugOut.Analog[17] = ExternData.UserParam[1];
-    PORTC = 0x08;
-    DDRD  = 0xf4;
+}
-    PORTD = 0xA0;
-    DDRB  = 0x04;
-    PORTB = 0x35;
-    LED_ON;
-    UART_Init();
-    Timer0_Init();
+int main (void)
-    ADC_Init();
+{
-        InitIC2_Slave();
-    sei();//Globale Interrupts Einschalten
+    Led_Init();
-    Debug_Timer = SetDelay(100);   // Sendeintervall
+    LED_GRN_ON;
     TIMER0_Init();
-        Xmin = eeprom_read_word(&eeXmin);
+    USART0_Init();
-        Xmax = eeprom_read_word(&eeXmax);
+    ADC_Init();
+        I2C_Init();
         Ymin = eeprom_read_word(&eeYmin);
+    sei(); //Globale Interrupts Einschalten
+    Debug_Timer = SetDelay(100);   // Sendeintervall
+        // read calibration info from eeprom
+        eeprom_read_block(&Calibration, &eeCalibration, sizeof(Calibration_t));
+    ExternData.Orientation = 0;
+    ExternData.CalState = 0;
+    I2C_WriteCal.CalByte = 0;
+        // main loop
+    while (1)
+    {
+                FLIP_LOW;
+                Delay_ms(2);
+                RawMagnet1a = ADC_GetValue(ADC0);
+                RawMagnet2a = -ADC_GetValue(ADC1);
+                RawMagnet3a = ADC_GetValue(ADC7);
+                Delay_ms(1);
+                FLIP_HIGH;
+                Delay_ms(2);
+                RawMagnet1b = ADC_GetValue(ADC0);
-        Ymax = eeprom_read_word(&eeYmax);
+                RawMagnet2b = -ADC_GetValue(ADC1);
-        Zmin = eeprom_read_word(&eeZmin);
-        Zmax = eeprom_read_word(&eeZmax);
-    VersionInfo.Hauptversion = VERSION_HAUPTVERSION;
-    VersionInfo.Nebenversion = VERSION_NEBENVERSION;
-    VersionInfo.PCKompatibel = 7;
-    ExternData.Orientation = 0;
-    ExternData.CalState = 0;
-    I2C_WriteCal.CalByte = 0;
-    while (1)
-        {
-         FLIP_LOW;
-         Delay_ms(2);
-         RawMagnet1a = MessAD(0);
-         RawMagnet2a = -MessAD(1);
-         RawMagnet3a = MessAD(7);
+                RawMagnet3b = ADC_GetValue(ADC7);
-         Delay_ms(1);
-         FLIP_HIGH;
-         Delay_ms(2);
-         RawMagnet1b = MessAD(0);
-         RawMagnet2b = -MessAD(1);
+                Delay_ms(1);
-         RawMagnet3b = MessAD(7);
-         Delay_ms(1);
+                CalcFields();
-         CalcFields();
+                if(ExternData.CalState || I2C_WriteCal.CalByte) Calibrate();
-         if(ExternData.CalState || I2C_WriteCal.CalByte) Calibrate();
+                else CalcHeading();
-         else CalcHeading();
-         BearbeiteRxDaten();
+                // check data from USART
-         if(PC_Connected)
+        USART0_ProcessRxData();

Subversion Repositories MK3Mag

(root)/branches/MK3Mag V0.14 Code Redesign Killagreg/main.c - Rev 18 → 19