WebSVN - MK3Mag - Diff - Rev 13 and 62


/*#######################################################################################
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 / NON-COMMERCIAL USE ONLY
// + www.MikroKopter.com
// ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
// + Die Portierung oder Nutzung 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
// ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
// + 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 oder Nutzung 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.
// +   * porting the sources to other systems or using the software on other systems (except 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. 
// ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
 
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"
 
uint16_t eeXmin EEMEM = 0;
uint16_t eeXmax EEMEM = 0;
uint16_t eeYmin EEMEM = 0;
uint16_t eeYmax EEMEM = 0;
uint16_t eeZmin EEMEM = 0;
uint16_t eeZmax EEMEM = 0;
 
 
//############################################################################
//
void Wait(unsigned char dauer)
//############################################################################
{
    dauer = (unsigned char)TCNT0 + dauer;
    while((TCNT0 - dauer) & 0x80);
}
 
void CalcFields(void)
{
 UncalMagnetN = (1 * UncalMagnetN + (RawMagnet1a - RawMagnet1b)) / 2;
 UncalMagnetR = (1 * UncalMagnetR + (RawMagnet3a - RawMagnet3b)) / 2;
 UncalMagnetZ = (1 * UncalMagnetZ + (RawMagnet2a - RawMagnet2b)) / 2;
 
 OffsetN = (Xmin + Xmax) / 2;
 OffsetR = (Ymin + Ymax) / 2;
 OffsetZ = (Zmin + Zmax) / 2;
 
 MagnetN = (1024L * (long)(UncalMagnetN - OffsetN)) / (Xmax - Xmin);
 MagnetR = (1024L * (long)(UncalMagnetR - OffsetR)) / (Ymax - Ymin);
 MagnetZ = (1024L * (long)(UncalMagnetZ - OffsetZ)) / (Zmax - Zmin);
}
 
void CalcHeading(void)
{
   double nick_rad, roll_rad, Hx, Hy, Cx, Cy, Cz;
   int heading;
   
   nick_rad = ((double)ExternData.Winkel[0]) * M_PI / (double)(1800);
   roll_rad = ((double)ExternData.Winkel[1]) * M_PI / (double)(1800);
 
   Cx = MagnetN;
   Cy = MagnetR;
   Cz = MagnetZ;
 
 if(ExternData.Orientation == 1)
  {
   Cx = MagnetR;
   Cy = -MagnetN;
   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.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;
 
 if(abs(heading) < 361) Heading = heading;
 PwmHeading = Heading + 10;
}
 
 
void Calibrate(void)
{
 unsigned char cal;
 if(I2C_WriteCal.CalByte) cal = I2C_WriteCal.CalByte;
 else                     cal = ExternData.CalState;
 switch(cal)
  {
   case 0:
                LED_ON;
          break;
   case 1:
        Xmin =  10000;
        Xmax = -10000;
        Ymin =  10000;
        Ymax = -10000;
        Zmin =  10000;
        Zmax = -10000;
                LED_OFF;
          break;
   case 2:
                LED_ON;  // find Min and Max of the X- and Y-Sensors
              if(UncalMagnetN < Xmin) Xmin = UncalMagnetN;
              if(UncalMagnetN > Xmax) Xmax = UncalMagnetN;
              if(UncalMagnetR < Ymin) Ymin = UncalMagnetR;
              if(UncalMagnetR > Ymax) Ymax = UncalMagnetR;
          break;
   case 3:
                LED_OFF;
          break;
   case 4:
                LED_ON;  // find Min and Max of the Z-Sensor
          if(UncalMagnetZ < Zmin) Zmin = UncalMagnetZ;
              if(UncalMagnetZ > Zmax) Zmax = UncalMagnetZ;
          break;
   case 5:
                LED_OFF; // Save values
         if((Xmax - Xmin) > 150 && (Ymax - Ymin) > 150 && (Zmax - Zmin) > 150)
          {
               eeprom_write_word(&eeXmin, Xmin);
               eeprom_write_word(&eeXmax, Xmax);
               eeprom_write_word(&eeYmin, Ymin);
               eeprom_write_word(&eeYmax, Ymax);
               eeprom_write_word(&eeZmin, Zmin);
               eeprom_write_word(&eeZmax, Zmax);
           Delay_ms(2000);
          }
                LED_ON;
        break;
  }
}
 
 
void SetDebugValues(void)
{
         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[6] =  ExternData.Winkel[0];
         DebugOut.Analog[7] =  ExternData.Winkel[1];
                 DebugOut.Analog[8] =  Xmin;
                 DebugOut.Analog[9] =  Xmax;
                 DebugOut.Analog[10] = Ymin;   
                 DebugOut.Analog[11] = Ymax;
                 DebugOut.Analog[12] = Zmin;
                 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];
}
 
 
//############################################################################
//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    
                           
        Xmin = eeprom_read_word(&eeXmin);
        Xmax = eeprom_read_word(&eeXmax);
        Ymin = eeprom_read_word(&eeYmin);
        Ymax = eeprom_read_word(&eeYmax);
        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);
         Delay_ms(1);
 
         FLIP_HIGH;
         Delay_ms(2);
         RawMagnet1b = MessAD(0);
         RawMagnet2b = -MessAD(1);
         RawMagnet3b = MessAD(7);
         Delay_ms(1);
 
         CalcFields();
         if(ExternData.CalState || I2C_WriteCal.CalByte) Calibrate();
         else CalcHeading();
         BearbeiteRxDaten();
 
         if(PC_Connected)
          {
            DDRD  |= 0x02; // TXD-Portpin
                UCR |= (1 << TXEN);
            DatenUebertragung();
            PC_Connected--;        
          }  
          else
           {
                UCR &= ~(1 << TXEN);
            DDRD &= ~0x02; // TXD-Portpin
           }
        } // while(1)
}
 
 

Rev 13	Rev 62
1	/*#######################################################################################	1	/*#######################################################################################
2	MK3Mag 3D-Magnet sensor	2	MK3Mag 3D-Magnet sensor
3	!!! THIS IS NOT FREE SOFTWARE !!!	3	!!! THIS IS NOT FREE SOFTWARE !!!
4	#######################################################################################*/	4	#######################################################################################*/
5	// ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++	5	// ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
6	// + Copyright (c) 05.2008 Holger Buss	6	// + Copyright (c) 05.2008 Holger Buss
7	// + Thanks to Ilja Fähnrich (P_Latzhalter)	7	// + Thanks to Ilja Fähnrich (P_Latzhalter)
8	// + Nur für den privaten Gebrauch	8	// + Nur für den privaten Gebrauch / NON-COMMERCIAL USE ONLY
9	// + www.MikroKopter.com	9	// + www.MikroKopter.com
10	// ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++	10	// ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
11	// + Die Portierung der Software (oder Teile davon) auf andere Systeme (ausser der Hardware von www.mikrokopter.de) ist nur	11	// + Die Portierung oder Nutzung der Software (oder Teile davon) auf andere Systeme (ausser der Hardware von www.mikrokopter.de) ist nur
12	// + mit unserer Zustimmung zulässig	12	// + mit unserer Zustimmung zulässig
13	// ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++	13	// ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
14	// + Es gilt für das gesamte Projekt (Hardware, Software, Binärfiles, Sourcecode und Dokumentation),	14	// + Es gilt für das gesamte Projekt (Hardware, Software, Binärfiles, Sourcecode und Dokumentation),
15	// + dass eine Nutzung (auch auszugsweise) nur für den privaten (nicht-kommerziellen) Gebrauch zulässig ist.	15	// + dass eine Nutzung (auch auszugsweise) nur für den privaten (nicht-kommerziellen) Gebrauch zulässig ist.
16	// + AUSNAHME: Ein bei www.mikrokopter.de erworbener vorbestückter MK3Mag darf als Baugruppe auch in kommerziellen Systemen verbaut werden	16	// + AUSNAHME: Ein bei www.mikrokopter.de erworbener vorbestückter MK3Mag darf als Baugruppe auch in kommerziellen Systemen verbaut werden
17	// + Im Zweifelsfall bitte anfragen bei: info@mikrokopter.de	17	// + Im Zweifelsfall bitte anfragen bei: info@mikrokopter.de
18	// ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++	18	// ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
19	// + Werden Teile des Quellcodes (mit oder ohne Modifikation) weiterverwendet oder veröffentlicht,	19	// + Werden Teile des Quellcodes (mit oder ohne Modifikation) weiterverwendet oder veröffentlicht,
20	// + unterliegen sie auch diesen Nutzungsbedingungen und diese Nutzungsbedingungen incl. Copyright müssen dann beiliegen	20	// + unterliegen sie auch diesen Nutzungsbedingungen und diese Nutzungsbedingungen incl. Copyright müssen dann beiliegen
21	// ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++	21	// ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
22	// + Sollte die Software (auch auszugesweise) oder sonstige Informationen des MikroKopter-Projekts	22	// + Sollte die Software (auch auszugesweise) oder sonstige Informationen des MikroKopter-Projekts
23	// + auf anderen Webseiten oder sonstigen Medien veröffentlicht werden, muss unsere Webseite "http://www.mikrokopter.de"	23	// + auf anderen Webseiten oder sonstigen Medien veröffentlicht werden, muss unsere Webseite "http://www.mikrokopter.de"
24	// + eindeutig als Ursprung verlinkt werden	24	// + eindeutig als Ursprung verlinkt werden
25	// ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++	25	// ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
26	// + Keine Gewähr auf Fehlerfreiheit, Vollständigkeit oder Funktion	26	// + Keine Gewähr auf Fehlerfreiheit, Vollständigkeit oder Funktion
27	// + Benutzung auf eigene Gefahr	27	// + Benutzung auf eigene Gefahr
28	// + Wir übernehmen keinerlei Haftung für direkte oder indirekte Personen- oder Sachschäden	28	// + Wir übernehmen keinerlei Haftung für direkte oder indirekte Personen- oder Sachschäden
29	// ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++	29	// ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
-		30	// + Die Portierung oder Nutzung der Software (oder Teile davon) auf andere Systeme (ausser der Hardware von www.mikrokopter.de) ist nur
-		31	// + mit unserer Zustimmung zulässig
-		32	/// ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
30	// + Die Funktion printf_P() unterliegt ihrer eigenen Lizenz und ist hiervon nicht betroffen	33	// + Die Funktion printf_P() unterliegt ihrer eigenen Lizenz und ist hiervon nicht betroffen
31	// ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++	34	// ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
32	// + Redistributions of source code (with or without modifications) must retain the above copyright notice,	35	// + Redistributions of source code (with or without modifications) must retain the above copyright notice,
33	// + this list of conditions and the following disclaimer.	36	// + this list of conditions and the following disclaimer.
34	// + * PORTING this software (or parts of it) to systems (other than hardware from www.mikrokopter.de) is NOT allowed	37	// + * porting the sources to other systems or using the software on other systems (except hardware from www.mikrokopter.de) is not allowed
35	// + * Neither the name of the copyright holders nor the names of contributors may be used to endorse or promote products derived	38	// + * Neither the name of the copyright holders nor the names of contributors may be used to endorse or promote products derived
36	// + from this software without specific prior written permission.	39	// + from this software without specific prior written permission.
37	// + * The use of this project (hardware, software, binary files, sources and documentation) is only permittet	40	// + * The use of this project (hardware, software, binary files, sources and documentation) is only permittet
38	// + for non-commercial use (directly or indirectly)	41	// + for non-commercial use (directly or indirectly)
39	// + Commercial use (for excample: selling of MikroKopters, selling of PCBs, assembly, ...) is only permitted	42	// + Commercial use (for excample: selling of MikroKopters, selling of PCBs, assembly, ...) is only permitted
40	// + with our written permission	43	// + with our written permission
41	// + Exception: A preassembled MK3Mag, purchased from www.mikrokopter.de may be used as a part of commercial systems	44	// + Exception: A preassembled MK3Mag, purchased from www.mikrokopter.de may be used as a part of commercial systems
42	// + In case of doubt please contact: info@MikroKopter.de	45	// + In case of doubt please contact: info@MikroKopter.de
43	// + * If sources or documentations are redistributet on other webpages, our webpage (http://www.MikroKopter.de) must be	46	// + * If sources or documentations are redistributet on other webpages, our webpage (http://www.MikroKopter.de) must be
44	// + clearly linked as origin	47	// + clearly linked as origin
45	// + THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"	48	// + THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
46	// + AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE	49	// + AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
47	// + IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE	50	// + IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
48	// + ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE	51	// + ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE
49	// + LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR	52	// + LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
50	// + CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF	53	// + CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
51	// + SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS	54	// + SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
52	// + INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN	55	// + INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
53	// + CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)	56	// + CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
54	// + ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE	57	// + ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
55	// + POSSIBILITY OF SUCH DAMAGE.	58	// + POSSIBILITY OF SUCH DAMAGE.
56	// ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++	59	// ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
57		60
58	signed int OffsetN, OffsetR, OffsetZ;	61	signed int OffsetN, OffsetR, OffsetZ;
59		62
60	signed int RawMagnet1a,RawMagnet1b; // raw AD-Data	63	signed int RawMagnet1a,RawMagnet1b; // raw AD-Data
61	signed int RawMagnet2a,RawMagnet2b;	64	signed int RawMagnet2a,RawMagnet2b;
62	signed int RawMagnet3a,RawMagnet3b;	65	signed int RawMagnet3a,RawMagnet3b;
63	signed int Xmin = 0, Xmax = 0; Ymin = 0, Ymax = 0; Zmin = 0, Zmax = 0;	66	signed int Xmin = 0, Xmax = 0; Ymin = 0, Ymax = 0; Zmin = 0, Zmax = 0;
64	signed int UncalMagnetN,UncalMagnetR,UncalMagnetZ; // Messwert-Delta ohne Offset- und Verstärker korrektur	67	signed int UncalMagnetN,UncalMagnetR,UncalMagnetZ; // Messwert-Delta ohne Offset- und Verstärker korrektur
65	signed int MagnetN,MagnetR,MagnetZ;	68	signed int MagnetN,MagnetR,MagnetZ;
66	unsigned int PwmHeading = 0;	69	unsigned int PwmHeading = 0;
67	unsigned int PC_Connected = 0;	70	unsigned int PC_Connected = 0;
68	unsigned int Heading;	71	unsigned int Heading;
69	#include "main.h"	72	#include "main.h"
70		73
71	uint16_t eeXmin EEMEM = 0;	74	uint16_t eeXmin EEMEM = 0;
72	uint16_t eeXmax EEMEM = 0;	75	uint16_t eeXmax EEMEM = 0;
73	uint16_t eeYmin EEMEM = 0;	76	uint16_t eeYmin EEMEM = 0;
74	uint16_t eeYmax EEMEM = 0;	77	uint16_t eeYmax EEMEM = 0;
75	uint16_t eeZmin EEMEM = 0;	78	uint16_t eeZmin EEMEM = 0;
76	uint16_t eeZmax EEMEM = 0;	79	uint16_t eeZmax EEMEM = 0;
77		80
78		81
79	//############################################################################	82	//############################################################################
80	//	83	//
81	void Wait(unsigned char dauer)	84	void Wait(unsigned char dauer)
82	//############################################################################	85	//############################################################################
83	{	86	{
84	dauer = (unsigned char)TCNT0 + dauer;	87	dauer = (unsigned char)TCNT0 + dauer;
85	while((TCNT0 - dauer) & 0x80);	88	while((TCNT0 - dauer) & 0x80);
86	}	89	}
87		90
88	void CalcFields(void)	91	void CalcFields(void)
89	{	92	{
90	UncalMagnetN = (1 * UncalMagnetN + (RawMagnet1a - RawMagnet1b)) / 2;	93	UncalMagnetN = (1 * UncalMagnetN + (RawMagnet1a - RawMagnet1b)) / 2;
91	UncalMagnetR = (1 * UncalMagnetR + (RawMagnet3a - RawMagnet3b)) / 2;	94	UncalMagnetR = (1 * UncalMagnetR + (RawMagnet3a - RawMagnet3b)) / 2;
92	UncalMagnetZ = (1 * UncalMagnetZ + (RawMagnet2a - RawMagnet2b)) / 2;	95	UncalMagnetZ = (1 * UncalMagnetZ + (RawMagnet2a - RawMagnet2b)) / 2;
93		96
94	OffsetN = (Xmin + Xmax) / 2;	97	OffsetN = (Xmin + Xmax) / 2;
95	OffsetR = (Ymin + Ymax) / 2;	98	OffsetR = (Ymin + Ymax) / 2;
96	OffsetZ = (Zmin + Zmax) / 2;	99	OffsetZ = (Zmin + Zmax) / 2;
97		100
98	MagnetN = (1024L * (long)(UncalMagnetN - OffsetN)) / (Xmax - Xmin);	101	MagnetN = (1024L * (long)(UncalMagnetN - OffsetN)) / (Xmax - Xmin);
99	MagnetR = (1024L * (long)(UncalMagnetR - OffsetR)) / (Ymax - Ymin);	102	MagnetR = (1024L * (long)(UncalMagnetR - OffsetR)) / (Ymax - Ymin);
100	MagnetZ = (1024L * (long)(UncalMagnetZ - OffsetZ)) / (Zmax - Zmin);	103	MagnetZ = (1024L * (long)(UncalMagnetZ - OffsetZ)) / (Zmax - Zmin);
101	}	104	}
102		105
103	void CalcHeading(void)	106	void CalcHeading(void)
104	{	107	{
105	double nick_rad, roll_rad, Hx, Hy, Cx, Cy, Cz;	108	double nick_rad, roll_rad, Hx, Hy, Cx, Cy, Cz;
106	int heading;	109	int heading;
107		110
108	nick_rad = ((double)ExternData.Winkel[0]) * M_PI / (double)(1800);	111	nick_rad = ((double)ExternData.Winkel[0]) * M_PI / (double)(1800);
109	roll_rad = ((double)ExternData.Winkel[1]) * M_PI / (double)(1800);	112	roll_rad = ((double)ExternData.Winkel[1]) * M_PI / (double)(1800);
110		113
111	Cx = MagnetN;	114	Cx = MagnetN;
112	Cy = MagnetR;	115	Cy = MagnetR;
113	Cz = MagnetZ;	116	Cz = MagnetZ;
114		117
115	if(ExternData.Orientation == 1)	118	if(ExternData.Orientation == 1)
116	{	119	{
117	Cx = MagnetR;	120	Cx = MagnetR;
118	Cy = -MagnetN;	121	Cy = -MagnetN;
119	Cz = MagnetZ;	122	Cz = MagnetZ;
120	}	123	}
121		124
122	Hx = Cx * (double)cos(nick_rad) +	125	Hx = Cx * (double)cos(nick_rad) +
123	Cy * (double)sin(nick_rad) * (double)sin(roll_rad) -	126	Cy * (double)sin(nick_rad) * (double)sin(roll_rad) -
124	Cz * (double)sin(nick_rad) * (double)cos(roll_rad);	127	Cz * (double)sin(nick_rad) * (double)cos(roll_rad);
125		128
126	Hy = Cy * (double)cos(roll_rad) +	129	Hy = Cy * (double)cos(roll_rad) +
127	Cz * (double)sin(roll_rad);	130	Cz * (double)sin(roll_rad);
128		131
129		132
130	if(Hx == 0 && Hy < 0) heading = 90;	133	if(Hx == 0 && Hy < 0) heading = 90;
131	else if(Hx == 0 && Hy > 0) heading = 270;	134	else if(Hx == 0 && Hy > 0) heading = 270;
132	else if(Hx < 0) heading = 180 - (atan(Hy / Hx) * 180.0) / M_PI;	135	else if(Hx < 0) heading = 180 - (atan(Hy / Hx) * 180.0) / M_PI;
133	else if(Hx > 0 && Hy < 0) heading = - (atan(Hy / Hx) * 180.0) / M_PI;	136	else if(Hx > 0 && Hy < 0) heading = - (atan(Hy / Hx) * 180.0) / M_PI;
134	else if(Hx > 0 && Hy > 0) heading = 360 - (atan(Hy / Hx) * 180.0) / M_PI;	137	else if(Hx > 0 && Hy > 0) heading = 360 - (atan(Hy / Hx) * 180.0) / M_PI;
135		138
136	if(abs(heading) < 361) Heading = heading;	139	if(abs(heading) < 361) Heading = heading;
137	PwmHeading = Heading + 10;	140	PwmHeading = Heading + 10;
138	}	141	}
139		142
140		143
141	void Calibrate(void)	144	void Calibrate(void)
142	{	145	{
143	unsigned char cal;	146	unsigned char cal;
144	if(I2C_WriteCal.CalByte) cal = I2C_WriteCal.CalByte;	147	if(I2C_WriteCal.CalByte) cal = I2C_WriteCal.CalByte;
145	else cal = ExternData.CalState;	148	else cal = ExternData.CalState;
146	switch(cal)	149	switch(cal)
147	{	150	{
148	case 0:	151	case 0:
149	LED_ON;	152	LED_ON;
150	break;	153	break;
151	case 1:	154	case 1:
152	Xmin = 10000;	155	Xmin = 10000;
153	Xmax = -10000;	156	Xmax = -10000;
154	Ymin = 10000;	157	Ymin = 10000;
155	Ymax = -10000;	158	Ymax = -10000;
156	Zmin = 10000;	159	Zmin = 10000;
157	Zmax = -10000;	160	Zmax = -10000;
158	LED_OFF;	161	LED_OFF;
159	break;	162	break;
160	case 2:	163	case 2:
161	LED_ON; // find Min and Max of the X- and Y-Sensors	164	LED_ON; // find Min and Max of the X- and Y-Sensors
162	if(UncalMagnetN < Xmin) Xmin = UncalMagnetN;	165	if(UncalMagnetN < Xmin) Xmin = UncalMagnetN;
163	if(UncalMagnetN > Xmax) Xmax = UncalMagnetN;	166	if(UncalMagnetN > Xmax) Xmax = UncalMagnetN;
164	if(UncalMagnetR < Ymin) Ymin = UncalMagnetR;	167	if(UncalMagnetR < Ymin) Ymin = UncalMagnetR;
165	if(UncalMagnetR > Ymax) Ymax = UncalMagnetR;	168	if(UncalMagnetR > Ymax) Ymax = UncalMagnetR;
166	break;	169	break;
167	case 3:	170	case 3:
168	LED_OFF;	171	LED_OFF;
169	break;	172	break;
170	case 4:	173	case 4:
171	LED_ON; // find Min and Max of the Z-Sensor	174	LED_ON; // find Min and Max of the Z-Sensor
172	if(UncalMagnetZ < Zmin) Zmin = UncalMagnetZ;	175	if(UncalMagnetZ < Zmin) Zmin = UncalMagnetZ;
173	if(UncalMagnetZ > Zmax) Zmax = UncalMagnetZ;	176	if(UncalMagnetZ > Zmax) Zmax = UncalMagnetZ;
174	break;	177	break;
175	case 5:	178	case 5:
176	LED_OFF; // Save values	179	LED_OFF; // Save values
177	if((Xmax - Xmin) > 150 && (Ymax - Ymin) > 150 && (Zmax - Zmin) > 150)	180	if((Xmax - Xmin) > 150 && (Ymax - Ymin) > 150 && (Zmax - Zmin) > 150)
178	{	181	{
179	eeprom_write_word(&eeXmin, Xmin);	182	eeprom_write_word(&eeXmin, Xmin);
180	eeprom_write_word(&eeXmax, Xmax);	183	eeprom_write_word(&eeXmax, Xmax);
181	eeprom_write_word(&eeYmin, Ymin);	184	eeprom_write_word(&eeYmin, Ymin);
182	eeprom_write_word(&eeYmax, Ymax);	185	eeprom_write_word(&eeYmax, Ymax);
183	eeprom_write_word(&eeZmin, Zmin);	186	eeprom_write_word(&eeZmin, Zmin);
184	eeprom_write_word(&eeZmax, Zmax);	187	eeprom_write_word(&eeZmax, Zmax);
185	Delay_ms(2000);	188	Delay_ms(2000);
186	}	189	}
187	LED_ON;	190	LED_ON;
188	break;	191	break;
189	}	192	}
190	}	193	}
191		194
192		195
193	void SetDebugValues(void)	196	void SetDebugValues(void)
194	{	197	{
195	DebugOut.Analog[0] = MagnetN;	198	DebugOut.Analog[0] = MagnetN;
196	DebugOut.Analog[1] = MagnetR;	199	DebugOut.Analog[1] = MagnetR;
197	DebugOut.Analog[2] = MagnetZ;	200	DebugOut.Analog[2] = MagnetZ;
198	DebugOut.Analog[3] = UncalMagnetN;	201	DebugOut.Analog[3] = UncalMagnetN;
199	DebugOut.Analog[4] = UncalMagnetR;	202	DebugOut.Analog[4] = UncalMagnetR;
200	DebugOut.Analog[5] = UncalMagnetZ;	203	DebugOut.Analog[5] = UncalMagnetZ;
201	DebugOut.Analog[6] = ExternData.Winkel[0];	204	DebugOut.Analog[6] = ExternData.Winkel[0];
202	DebugOut.Analog[7] = ExternData.Winkel[1];	205	DebugOut.Analog[7] = ExternData.Winkel[1];
203	DebugOut.Analog[8] = Xmin;	206	DebugOut.Analog[8] = Xmin;
204	DebugOut.Analog[9] = Xmax;	207	DebugOut.Analog[9] = Xmax;
205	DebugOut.Analog[10] = Ymin;	208	DebugOut.Analog[10] = Ymin;
206	DebugOut.Analog[11] = Ymax;	209	DebugOut.Analog[11] = Ymax;
207	DebugOut.Analog[12] = Zmin;	210	DebugOut.Analog[12] = Zmin;
208	DebugOut.Analog[13] = Zmax;	211	DebugOut.Analog[13] = Zmax;
209	DebugOut.Analog[14] = ExternData.CalState;	212	DebugOut.Analog[14] = ExternData.CalState;
210	DebugOut.Analog[15] = Heading;	213	DebugOut.Analog[15] = Heading;
211	DebugOut.Analog[16] = ExternData.UserParameter[0];	214	DebugOut.Analog[16] = ExternData.UserParameter[0];
212	DebugOut.Analog[17] = ExternData.UserParameter[1];	215	DebugOut.Analog[17] = ExternData.UserParameter[1];
213	}	216	}
214		217
215		218
216	//############################################################################	219	//############################################################################
217	//Hauptprogramm	220	//Hauptprogramm
218	int main (void)	221	int main (void)
219	//############################################################################	222	//############################################################################
220	{	223	{
221	DDRC = 0x08;	224	DDRC = 0x08;
222	PORTC = 0x08;	225	PORTC = 0x08;
223	DDRD = 0xf4;	226	DDRD = 0xf4;
224	PORTD = 0xA0;	227	PORTD = 0xA0;
225	DDRB = 0x04;	228	DDRB = 0x04;
226	PORTB = 0x35;	229	PORTB = 0x35;
227		230
228	LED_ON;	231	LED_ON;
229		232
230	UART_Init();	233	UART_Init();
231	Timer0_Init();	234	Timer0_Init();
232	ADC_Init();	235	ADC_Init();
233	InitIC2_Slave();	236	InitIC2_Slave();
234	sei();//Globale Interrupts Einschalten	237	sei();//Globale Interrupts Einschalten
235	Debug_Timer = SetDelay(100); // Sendeintervall	238	Debug_Timer = SetDelay(100); // Sendeintervall
236		239
237	Xmin = eeprom_read_word(&eeXmin);	240	Xmin = eeprom_read_word(&eeXmin);
238	Xmax = eeprom_read_word(&eeXmax);	241	Xmax = eeprom_read_word(&eeXmax);
239	Ymin = eeprom_read_word(&eeYmin);	242	Ymin = eeprom_read_word(&eeYmin);
240	Ymax = eeprom_read_word(&eeYmax);	243	Ymax = eeprom_read_word(&eeYmax);
241	Zmin = eeprom_read_word(&eeZmin);	244	Zmin = eeprom_read_word(&eeZmin);
242	Zmax = eeprom_read_word(&eeZmax);	245	Zmax = eeprom_read_word(&eeZmax);
243		246
244	VersionInfo.Hauptversion = VERSION_HAUPTVERSION;	247	VersionInfo.Hauptversion = VERSION_HAUPTVERSION;
245	VersionInfo.Nebenversion = VERSION_NEBENVERSION;	248	VersionInfo.Nebenversion = VERSION_NEBENVERSION;
246	VersionInfo.PCKompatibel = 7;	249	VersionInfo.PCKompatibel = 7;
247		250
248	ExternData.Orientation = 0;	251	ExternData.Orientation = 0;
249	ExternData.CalState = 0;	252	ExternData.CalState = 0;
250	I2C_WriteCal.CalByte = 0;	253	I2C_WriteCal.CalByte = 0;
251		254
252	while (1)	255	while (1)
253	{	256	{
254	FLIP_LOW;	257	FLIP_LOW;
255	Delay_ms(2);	258	Delay_ms(2);
256	RawMagnet1a = MessAD(0);	259	RawMagnet1a = MessAD(0);
257	RawMagnet2a = -MessAD(1);	260	RawMagnet2a = -MessAD(1);
258	RawMagnet3a = MessAD(7);	261	RawMagnet3a = MessAD(7);
259	Delay_ms(1);	262	Delay_ms(1);
260		263
261	FLIP_HIGH;	264	FLIP_HIGH;
262	Delay_ms(2);	265	Delay_ms(2);
263	RawMagnet1b = MessAD(0);	266	RawMagnet1b = MessAD(0);
264	RawMagnet2b = -MessAD(1);	267	RawMagnet2b = -MessAD(1);
265	RawMagnet3b = MessAD(7);	268	RawMagnet3b = MessAD(7);
266	Delay_ms(1);	269	Delay_ms(1);
267		270
268	CalcFields();	271	CalcFields();
269	if(ExternData.CalState \|\| I2C_WriteCal.CalByte) Calibrate();	272	if(ExternData.CalState \|\| I2C_WriteCal.CalByte) Calibrate();
270	else CalcHeading();	273	else CalcHeading();
271	BearbeiteRxDaten();	274	BearbeiteRxDaten();
272		275
273	if(PC_Connected)	276	if(PC_Connected)
274	{	277	{
275	DDRD \|= 0x02; // TXD-Portpin	278	DDRD \|= 0x02; // TXD-Portpin
276	UCR \|= (1 << TXEN);	279	UCR \|= (1 << TXEN);
277	DatenUebertragung();	280	DatenUebertragung();
278	PC_Connected--;	281	PC_Connected--;
279	}	282	}
280	else	283	else
281	{	284	{
282	UCR &= ~(1 << TXEN);	285	UCR &= ~(1 << TXEN);
283	DDRD &= ~0x02; // TXD-Portpin	286	DDRD &= ~0x02; // TXD-Portpin
284	}	287	}
285	} // while(1)	288	} // while(1)
286	}	289	}
287		290
288		291

Subversion Repositories MK3Mag

(root)/tags/V0.14/main.c - Rev 13 → 62