WebSVN - MK3Mag - Diff - Rev 51 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.
// ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
#include <avr/interrupt.h>
#include <math.h>
#include <stdlib.h>
#include <stdio.h>
 
#include "main.h"
#include "timer0.h"
#include "twislave.h"
#include "led.h"
#include "analog.h"
#include "uart.h"
 
#define CALIBRATION_VERSION 1
 
AttitudeSource_t AttitudeSource = ATTITUDE_SOURCE_ACC;
Orientation_t Orientation = ORIENTATION_FC;
 
uint16_t Led_Timer = 0;
 
typedef struct
{
        int16_t Range;
        int16_t Offset;
}  Scaling_t;
 
typedef struct
{
        Scaling_t MagX;
        Scaling_t MagY;
        Scaling_t MagZ;
        Scaling_t AccX;
        Scaling_t AccY;
        Scaling_t AccZ;
        unsigned char Version;
}  Calibration_t;
 
Calibration_t eeCalibration EEMEM;      // calibration data in EEProm
Calibration_t Calibration;              // calibration data in RAM
 
// magnet sensor variable
int16_t RawMagnet1a, RawMagnet1b;                       // raw magnet sensor data
int16_t RawMagnet2a, RawMagnet2b;
int16_t RawMagnet3a, RawMagnet3b;
int16_t UncalMagX, UncalMagY, UncalMagZ;        // sensor signal difference without Scaling
int16_t MagX = 0, MagY = 0, MagZ = 0;           // rescaled magnetic field readings
 
// acceleration sensor variables
int16_t RawAccX = 0, RawAccY = 0, RawAccZ = 0;                  // raw acceleration readings
int16_t AccX = 0, AccY = 0, AccZ = 0;                                   // rescaled acceleration readings
int16_t AccAttitudeNick = 0, AccAttitudeRoll = 0;               // nick and roll angle from acc
 
int16_t Heading = -1;                                           // the current compass heading in deg
 
 
void CalcFields(void)
{
        UncalMagX = (RawMagnet1a - RawMagnet1b);
        UncalMagY = (RawMagnet3a - RawMagnet3b);
        UncalMagZ = (RawMagnet2a - RawMagnet2b);
 
        if(Calibration.MagX.Range != 0) MagX = (1024L * (int32_t)(UncalMagX - Calibration.MagX.Offset)) / (Calibration.MagX.Range);
        else MagX = 0;
        if(Calibration.MagY.Range != 0) MagY = (1024L * (int32_t)(UncalMagY - Calibration.MagY.Offset)) / (Calibration.MagY.Range);
        else MagY = 0;
        if(Calibration.MagY.Range != 0) MagZ = (1024L * (int32_t)(UncalMagZ - Calibration.MagZ.Offset)) / (Calibration.MagZ.Range);
        else MagZ = 0;
 
        if(AccPresent)
        {
                AccX = (RawAccX - Calibration.AccX.Offset);
                AccY = (RawAccY - Calibration.AccY.Offset);
                AccZ = (Calibration.AccZ.Offset - RawAccZ);
                #if (BOARD == 10) // the hardware 1.0 has the LIS3L02AL
                // acc mode assumes orientation like FC
                if(AccX >  136) AccAttitudeNick = -800;
                else
                if(AccX < -136) AccAttitudeNick = 800;
                else                    AccAttitudeNick = (int16_t)(-1800.0 * asin((double) AccX / 138.0) / M_PI);
 
 
                if(AccY >  136) AccAttitudeRoll = 800;
                else
                if(AccY < -136) AccAttitudeRoll = -800;
                else                    AccAttitudeRoll = (int16_t)( 1800.0 * asin((double) AccY / 138.0) / M_PI);
 
                #else // the hardware 1.1 has the LIS344ALH with a different axis definition (X -> -Y, Y -> X, Z -> Z)
                // acc mode assumes orientation like FC
                if(AccY >  136) AccAttitudeNick = 800;
                else
                if(AccY < -136) AccAttitudeNick = -800;
                else                    AccAttitudeNick = (int16_t)( 1800.0 * asin((double) AccY / 138.0) / M_PI);
 
 
                if(AccX >  136) AccAttitudeRoll = 800;
                else
                if(AccX < -136) AccAttitudeRoll = -800;
                else                    AccAttitudeRoll = (int16_t)( 1800.0 * asin((double) AccX / 138.0) / M_PI);
                #endif
        }
}
 
 
void CalcHeading(void)
{
        double nick_rad, roll_rad, Hx, Hy, Cx = 0.0, Cy = 0.0, Cz = 0.0;
        int16_t nick, roll;
        int16_t heading = -1;
 
        // blink code for normal operation
        if(CheckDelay(Led_Timer))
        {
                if(Calibration.Version != CALIBRATION_VERSION) LED_GRN_TOGGLE;
                else LED_GRN_ON;
                Led_Timer = SetDelay(150);
        }
        switch(Orientation)
        {
                case ORIENTATION_NC:
                        Cx = MagX;
                        Cy = MagY;
                        Cz = MagZ;
                        break;
 
                case ORIENTATION_FC:
                        // rotation of 90 deg compared to NC setup
                        Cx = MagY;
                        Cy = -MagX;
                        Cz = MagZ;
                        break;
        }
 
        // calculate nick and roll angle in rad
        switch(AttitudeSource)
        {
                case ATTITUDE_SOURCE_I2C:
                        cli(); // stop interrupts
                        nick = I2C_WriteAttitude.Nick;
                        roll = I2C_WriteAttitude.Roll;
                        sei(); // start interrupts
                        break;
                case ATTITUDE_SOURCE_UART:
                        cli(); // stop interrupts
                        nick = ExternData.Attitude[NICK];
                        roll = ExternData.Attitude[ROLL];
                        sei(); // start interrupts
                        break;
                case ATTITUDE_SOURCE_ACC:
                        nick = AccAttitudeNick;
                        roll = AccAttitudeRoll;
                        break;
                default:
                        nick = 0;
                        roll = 0;
                break;
        }
 
        nick_rad = ((double)nick) * M_PI / (double)(1800.0);
        roll_rad = ((double)roll) * M_PI / (double)(1800.0);
 
        // calculate attitude correction
        Hx = Cx * cos(nick_rad) - Cz * sin(nick_rad);
        Hy = Cy * cos(roll_rad) + Cz * sin(roll_rad);
 
        DebugOut.Analog[27] = (int16_t)Hx;
        DebugOut.Analog[28] = (int16_t)Hy;
 
        // calculate Heading
        heading = (int16_t)((180.0 * atan2(Hy, Hx)) / M_PI);
        // atan2 returns angular range from -180 deg to 180 deg in counter clockwise notation
        // but the compass course is defined in a range from 0 deg to 360 deg clockwise notation.
        if (heading < 0) heading = -heading;
        else heading = 360 - heading;
 
//++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
   if(Calibration.Version != CALIBRATION_VERSION) heading = -1; // Version of the calibration Data does not match
//++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
        cli(); // stop interrupts
        if(abs(heading) < 361) Heading = heading;
        else (Heading = -1);
        sei(); // start interrupts
 
}
 
void Calibrate(void)
{
        uint8_t cal;
        static uint8_t calold = 0;
        static int16_t Xmin = 0, Xmax = 0, Ymin = 0, Ymax = 0, Zmin = 0, Zmax = 0;
        static uint8_t blinkcount = 0;
        static uint8_t invert_blinking = 0;
 
        // check both sources of communication for calibration request
        if(I2C_WriteCal.CalByte)        cal = I2C_WriteCal.CalByte;
        else                                            cal = ExternData.CalState;
 
 
        if(cal > 5) cal = 0;
        // blink code for current calibration state
        if(cal)
        {
                if(CheckDelay(Led_Timer) || (cal != calold))
                {
                        if(blinkcount & 0x01) if(invert_blinking) LED_GRN_ON; else LED_GRN_OFF;
                        else if(invert_blinking) LED_GRN_OFF; else LED_GRN_ON;
 
                        // end of blinkcount sequence
                        if((blinkcount + 1 ) >= (2 * cal))
                        {
                                blinkcount = 0;
                                Led_Timer = SetDelay(1500);
                        }
                        else
                        {
                                blinkcount++;
                                Led_Timer = SetDelay(100);
                        }
                }
        }
        else
        {
                if(invert_blinking) LED_GRN_ON; else LED_GRN_OFF;
        }
        // calibration state machine
        switch(cal)
        {
                case 1: // 1st step of calibration
                        // initialize ranges
                        // used to change the orientation of the MK3MAG in the horizontal plane
                        Xmin =  10000;
                        Xmax = -10000;
                        Ymin =  10000;
                        Ymax = -10000;
                        Zmin =  10000;
                        Zmax = -10000;
                        Calibration.AccX.Offset = RawAccX;
                        Calibration.AccY.Offset = RawAccY;
                        Calibration.AccZ.Offset = RawAccZ;
                        invert_blinking = 0;
                        break;
 
                case 2: // 2nd step of calibration
                        // find Min and Max of the X- and Y-Sensors during rotation in the horizontal plane
                        if(UncalMagX < Xmin) Xmin = UncalMagX;
                        if(UncalMagX > Xmax) Xmax = UncalMagX;
                        if(UncalMagY < Ymin) Ymin = UncalMagY;
                        if(UncalMagY > Ymax) Ymax = UncalMagY;
                        invert_blinking = 1;
                        break;
 
                case 3: // 3rd step of calibration
                        // used to change the orientation of the MK3MAG vertical to the horizontal plane
                        invert_blinking = 0;
                        break;
 
                case 4:
                        // find Min and Max of the Z-Sensor
                        if(UncalMagZ < Zmin) Zmin = UncalMagZ;
                        if(UncalMagZ > Zmax) Zmax = UncalMagZ;
                        invert_blinking = 1;
                        break;
 
                case 5:
                        // Save values
                        if(cal != calold) // avoid continously writing of eeprom!
                        {
                                Calibration.MagX.Range = Xmax - Xmin;
                                Calibration.MagX.Offset = (Xmin + Xmax) / 2;
                                Calibration.MagY.Range = Ymax - Ymin;
                                Calibration.MagY.Offset = (Ymin + Ymax) / 2;
                                Calibration.MagZ.Range = Zmax - Zmin;
                                Calibration.MagZ.Offset = (Zmin + Zmax) / 2;
                                if((Calibration.MagX.Range > 150) && (Calibration.MagY.Range > 150) && (Calibration.MagZ.Range > 150))
                                {
                                        Calibration.Version = CALIBRATION_VERSION;
                                        // indicate write process by setting the led off for 2 seconds
                                        LED_GRN_OFF;
                                        eeprom_write_block(&Calibration, &eeCalibration, sizeof(Calibration));
                                        Led_Timer = SetDelay(2000);
                                        // reset  blinkcode
                                        blinkcount = 0;
                                }
                        }
                        invert_blinking = 0;
                        break;
 
                default:
                        break;
        }
        calold = cal;
}
 
 
void SetDebugValues(void)
{
        DebugOut.Analog[0] =  MagX;
        DebugOut.Analog[1] =  MagY;
        DebugOut.Analog[2] =  MagZ;
        DebugOut.Analog[3] =  UncalMagX;
        DebugOut.Analog[4] =  UncalMagY;
        DebugOut.Analog[5] =  UncalMagZ;
        switch(AttitudeSource)
        {
                case ATTITUDE_SOURCE_ACC:
                        DebugOut.Analog[6] =  AccAttitudeNick;
                        DebugOut.Analog[7] =  AccAttitudeRoll;
                        break;
 
                case ATTITUDE_SOURCE_UART:
                        DebugOut.Analog[6] =  ExternData.Attitude[NICK];
                        DebugOut.Analog[7] =  ExternData.Attitude[ROLL];
                        break;
 
 
                case ATTITUDE_SOURCE_I2C:
                        DebugOut.Analog[6] =  I2C_WriteAttitude.Nick;
                        DebugOut.Analog[7] =  I2C_WriteAttitude.Roll;
                        break;
        }
        DebugOut.Analog[8] =  Calibration.MagX.Offset;
        DebugOut.Analog[9] =  Calibration.MagX.Range;
        DebugOut.Analog[10] = Calibration.MagY.Offset;
        DebugOut.Analog[11] = Calibration.MagY.Range;
        DebugOut.Analog[12] = Calibration.MagZ.Offset;
        DebugOut.Analog[13] = Calibration.MagZ.Range;
        if(I2C_WriteCal.CalByte) DebugOut.Analog[14] = I2C_WriteCal.CalByte;
        else DebugOut.Analog[14] = ExternData.CalState;
        DebugOut.Analog[15] = Heading;
        DebugOut.Analog[16] = ExternData.UserParam[0];
        DebugOut.Analog[17] = ExternData.UserParam[1];
        DebugOut.Analog[18] = AccX;
        DebugOut.Analog[19] = AccY;
        DebugOut.Analog[20] = AccZ;
        DebugOut.Analog[21] = RawAccX;
        DebugOut.Analog[22] = RawAccY;
        DebugOut.Analog[23] = RawAccZ;
        DebugOut.Analog[24] = Calibration.AccX.Offset;
        DebugOut.Analog[25] = Calibration.AccY.Offset;
        DebugOut.Analog[26] = Calibration.AccZ.Offset;
        DebugOut.Analog[29] = AttitudeSource;
}
 
void AccMeasurement(void)
{
        if(AccPresent)
        {
                RawAccX = (RawAccX + (int16_t)ADC_GetValue(ACC_X))/2;
                RawAccY = (RawAccY + (int16_t)ADC_GetValue(ACC_Y))/2;
                RawAccZ = (RawAccZ + (int16_t)ADC_GetValue(ACC_Z))/2;
        }
        else
        {
                RawAccX = 0;
                RawAccY = 0;
                RawAccZ = 0;
        }
}
 
int main (void)
{
        // reset input pullup
        DDRC &=~((1<<DDC6));
        PORTC |= (1<<PORTC6);
 
        LED_Init();
        TIMER0_Init();
        USART0_Init();
        ADC_Init();
        I2C_Init();
 
        sei(); // enable globale interrupts
 
        if(AccPresent)
        {
                USART0_Print("ACC present\n");
        }
 
        LED_GRN_ON;
 
        Led_Timer = SetDelay(200);
 
        // read calibration info from eeprom
        eeprom_read_block(&Calibration, &eeCalibration, sizeof(Calibration));
 
        ExternData.CalState = 0;
        I2C_WriteCal.CalByte = 0;
 
 
        // main loop
        while (1)
        {
                FLIP_LOW;
                Delay_ms(2);
                RawMagnet1a = ADC_GetValue(MAG_X);
                RawMagnet2a = -ADC_GetValue(MAG_Y);
                RawMagnet3a = ADC_GetValue(MAG_Z);
                AccMeasurement();
                Delay_ms(1);
 
                FLIP_HIGH;
                Delay_ms(2);
                RawMagnet1b = ADC_GetValue(MAG_X);
                RawMagnet2b = -ADC_GetValue(MAG_Y);
                RawMagnet3b = ADC_GetValue(MAG_Z);
                AccMeasurement();
                Delay_ms(1);
 
                CalcFields();
 
                if(ExternData.CalState || I2C_WriteCal.CalByte) Calibrate();
                else CalcHeading();
 
                // check data from USART
                USART0_ProcessRxData();
 
                if(NC_Connected) NC_Connected--;
                if(FC_Connected) FC_Connected--;
                // fall back to attitude estimation from acc sensor if NC or FC does'nt send attittude data
                if(!FC_Connected && ! NC_Connected)
                {
                        AttitudeSource = ATTITUDE_SOURCE_ACC;
                        Orientation = ORIENTATION_FC;
                }
 
                if(PC_Connected)
                {
                        USART0_EnableTXD();
                        USART0_TransmitTxData();
                        PC_Connected--;
                }
                else
                {
                        USART0_DisableTXD();
                }
        } // while(1)
}
 
 

Rev 51	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	#include <avr/interrupt.h>	60	#include <avr/interrupt.h>
58	#include <math.h>	61	#include <math.h>
59	#include <stdlib.h>	62	#include <stdlib.h>
60	#include <stdio.h>	63	#include <stdio.h>
61		64
62	#include "main.h"	65	#include "main.h"
63	#include "timer0.h"	66	#include "timer0.h"
64	#include "twislave.h"	67	#include "twislave.h"
65	#include "led.h"	68	#include "led.h"
66	#include "analog.h"	69	#include "analog.h"
67	#include "uart.h"	70	#include "uart.h"
68		71
69	#define CALIBRATION_VERSION 1	72	#define CALIBRATION_VERSION 1
70		73
71	AttitudeSource_t AttitudeSource = ATTITUDE_SOURCE_ACC;	74	AttitudeSource_t AttitudeSource = ATTITUDE_SOURCE_ACC;
72	Orientation_t Orientation = ORIENTATION_FC;	75	Orientation_t Orientation = ORIENTATION_FC;
73		76
74	uint16_t Led_Timer = 0;	77	uint16_t Led_Timer = 0;
75		78
76	typedef struct	79	typedef struct
77	{	80	{
78	int16_t Range;	81	int16_t Range;
79	int16_t Offset;	82	int16_t Offset;
80	} Scaling_t;	83	} Scaling_t;
81		84
82	typedef struct	85	typedef struct
83	{	86	{
84	Scaling_t MagX;	87	Scaling_t MagX;
85	Scaling_t MagY;	88	Scaling_t MagY;
86	Scaling_t MagZ;	89	Scaling_t MagZ;
87	Scaling_t AccX;	90	Scaling_t AccX;
88	Scaling_t AccY;	91	Scaling_t AccY;
89	Scaling_t AccZ;	92	Scaling_t AccZ;
90	unsigned char Version;	93	unsigned char Version;
91	} Calibration_t;	94	} Calibration_t;
92		95
93	Calibration_t eeCalibration EEMEM; // calibration data in EEProm	96	Calibration_t eeCalibration EEMEM; // calibration data in EEProm
94	Calibration_t Calibration; // calibration data in RAM	97	Calibration_t Calibration; // calibration data in RAM
95		98
96	// magnet sensor variable	99	// magnet sensor variable
97	int16_t RawMagnet1a, RawMagnet1b; // raw magnet sensor data	100	int16_t RawMagnet1a, RawMagnet1b; // raw magnet sensor data
98	int16_t RawMagnet2a, RawMagnet2b;	101	int16_t RawMagnet2a, RawMagnet2b;
99	int16_t RawMagnet3a, RawMagnet3b;	102	int16_t RawMagnet3a, RawMagnet3b;
100	int16_t UncalMagX, UncalMagY, UncalMagZ; // sensor signal difference without Scaling	103	int16_t UncalMagX, UncalMagY, UncalMagZ; // sensor signal difference without Scaling
101	int16_t MagX = 0, MagY = 0, MagZ = 0; // rescaled magnetic field readings	104	int16_t MagX = 0, MagY = 0, MagZ = 0; // rescaled magnetic field readings
102		105
103	// acceleration sensor variables	106	// acceleration sensor variables
104	int16_t RawAccX = 0, RawAccY = 0, RawAccZ = 0; // raw acceleration readings	107	int16_t RawAccX = 0, RawAccY = 0, RawAccZ = 0; // raw acceleration readings
105	int16_t AccX = 0, AccY = 0, AccZ = 0; // rescaled acceleration readings	108	int16_t AccX = 0, AccY = 0, AccZ = 0; // rescaled acceleration readings
106	int16_t AccAttitudeNick = 0, AccAttitudeRoll = 0; // nick and roll angle from acc	109	int16_t AccAttitudeNick = 0, AccAttitudeRoll = 0; // nick and roll angle from acc
107		110
108	int16_t Heading = -1; // the current compass heading in deg	111	int16_t Heading = -1; // the current compass heading in deg
109		112
110		113
111	void CalcFields(void)	114	void CalcFields(void)
112	{	115	{
113	UncalMagX = (RawMagnet1a - RawMagnet1b);	116	UncalMagX = (RawMagnet1a - RawMagnet1b);
114	UncalMagY = (RawMagnet3a - RawMagnet3b);	117	UncalMagY = (RawMagnet3a - RawMagnet3b);
115	UncalMagZ = (RawMagnet2a - RawMagnet2b);	118	UncalMagZ = (RawMagnet2a - RawMagnet2b);
116		119
117	if(Calibration.MagX.Range != 0) MagX = (1024L * (int32_t)(UncalMagX - Calibration.MagX.Offset)) / (Calibration.MagX.Range);	120	if(Calibration.MagX.Range != 0) MagX = (1024L * (int32_t)(UncalMagX - Calibration.MagX.Offset)) / (Calibration.MagX.Range);
118	else MagX = 0;	121	else MagX = 0;
119	if(Calibration.MagY.Range != 0) MagY = (1024L * (int32_t)(UncalMagY - Calibration.MagY.Offset)) / (Calibration.MagY.Range);	122	if(Calibration.MagY.Range != 0) MagY = (1024L * (int32_t)(UncalMagY - Calibration.MagY.Offset)) / (Calibration.MagY.Range);
120	else MagY = 0;	123	else MagY = 0;
121	if(Calibration.MagY.Range != 0) MagZ = (1024L * (int32_t)(UncalMagZ - Calibration.MagZ.Offset)) / (Calibration.MagZ.Range);	124	if(Calibration.MagY.Range != 0) MagZ = (1024L * (int32_t)(UncalMagZ - Calibration.MagZ.Offset)) / (Calibration.MagZ.Range);
122	else MagZ = 0;	125	else MagZ = 0;
123		126
124	if(AccPresent)	127	if(AccPresent)
125	{	128	{
126	AccX = (RawAccX - Calibration.AccX.Offset);	129	AccX = (RawAccX - Calibration.AccX.Offset);
127	AccY = (RawAccY - Calibration.AccY.Offset);	130	AccY = (RawAccY - Calibration.AccY.Offset);
128	AccZ = (Calibration.AccZ.Offset - RawAccZ);	131	AccZ = (Calibration.AccZ.Offset - RawAccZ);
129	#if (BOARD == 10) // the hardware 1.0 has the LIS3L02AL	132	#if (BOARD == 10) // the hardware 1.0 has the LIS3L02AL
130	// acc mode assumes orientation like FC	133	// acc mode assumes orientation like FC
131	if(AccX > 136) AccAttitudeNick = -800;	134	if(AccX > 136) AccAttitudeNick = -800;
132	else	135	else
133	if(AccX < -136) AccAttitudeNick = 800;	136	if(AccX < -136) AccAttitudeNick = 800;
134	else AccAttitudeNick = (int16_t)(-1800.0 * asin((double) AccX / 138.0) / M_PI);	137	else AccAttitudeNick = (int16_t)(-1800.0 * asin((double) AccX / 138.0) / M_PI);
135		138
136		139
137	if(AccY > 136) AccAttitudeRoll = 800;	140	if(AccY > 136) AccAttitudeRoll = 800;
138	else	141	else
139	if(AccY < -136) AccAttitudeRoll = -800;	142	if(AccY < -136) AccAttitudeRoll = -800;
140	else AccAttitudeRoll = (int16_t)( 1800.0 * asin((double) AccY / 138.0) / M_PI);	143	else AccAttitudeRoll = (int16_t)( 1800.0 * asin((double) AccY / 138.0) / M_PI);
141		144
142	#else // the hardware 1.1 has the LIS344ALH with a different axis definition (X -> -Y, Y -> X, Z -> Z)	145	#else // the hardware 1.1 has the LIS344ALH with a different axis definition (X -> -Y, Y -> X, Z -> Z)
143	// acc mode assumes orientation like FC	146	// acc mode assumes orientation like FC
144	if(AccY > 136) AccAttitudeNick = 800;	147	if(AccY > 136) AccAttitudeNick = 800;
145	else	148	else
146	if(AccY < -136) AccAttitudeNick = -800;	149	if(AccY < -136) AccAttitudeNick = -800;
147	else AccAttitudeNick = (int16_t)( 1800.0 * asin((double) AccY / 138.0) / M_PI);	150	else AccAttitudeNick = (int16_t)( 1800.0 * asin((double) AccY / 138.0) / M_PI);
148		151
149		152
150	if(AccX > 136) AccAttitudeRoll = 800;	153	if(AccX > 136) AccAttitudeRoll = 800;
151	else	154	else
152	if(AccX < -136) AccAttitudeRoll = -800;	155	if(AccX < -136) AccAttitudeRoll = -800;
153	else AccAttitudeRoll = (int16_t)( 1800.0 * asin((double) AccX / 138.0) / M_PI);	156	else AccAttitudeRoll = (int16_t)( 1800.0 * asin((double) AccX / 138.0) / M_PI);
154	#endif	157	#endif
155	}	158	}
156	}	159	}
157		160
158		161
159	void CalcHeading(void)	162	void CalcHeading(void)
160	{	163	{
161	double nick_rad, roll_rad, Hx, Hy, Cx = 0.0, Cy = 0.0, Cz = 0.0;	164	double nick_rad, roll_rad, Hx, Hy, Cx = 0.0, Cy = 0.0, Cz = 0.0;
162	int16_t nick, roll;	165	int16_t nick, roll;
163	int16_t heading = -1;	166	int16_t heading = -1;
164		167
165	// blink code for normal operation	168	// blink code for normal operation
166	if(CheckDelay(Led_Timer))	169	if(CheckDelay(Led_Timer))
167	{	170	{
168	if(Calibration.Version != CALIBRATION_VERSION) LED_GRN_TOGGLE;	171	if(Calibration.Version != CALIBRATION_VERSION) LED_GRN_TOGGLE;
169	else LED_GRN_ON;	172	else LED_GRN_ON;
170	Led_Timer = SetDelay(150);	173	Led_Timer = SetDelay(150);
171	}	174	}
172	switch(Orientation)	175	switch(Orientation)
173	{	176	{
174	case ORIENTATION_NC:	177	case ORIENTATION_NC:
175	Cx = MagX;	178	Cx = MagX;
176	Cy = MagY;	179	Cy = MagY;
177	Cz = MagZ;	180	Cz = MagZ;
178	break;	181	break;
179		182
180	case ORIENTATION_FC:	183	case ORIENTATION_FC:
181	// rotation of 90 deg compared to NC setup	184	// rotation of 90 deg compared to NC setup
182	Cx = MagY;	185	Cx = MagY;
183	Cy = -MagX;	186	Cy = -MagX;
184	Cz = MagZ;	187	Cz = MagZ;
185	break;	188	break;
186	}	189	}
187		190
188	// calculate nick and roll angle in rad	191	// calculate nick and roll angle in rad
189	switch(AttitudeSource)	192	switch(AttitudeSource)
190	{	193	{
191	case ATTITUDE_SOURCE_I2C:	194	case ATTITUDE_SOURCE_I2C:
192	cli(); // stop interrupts	195	cli(); // stop interrupts
193	nick = I2C_WriteAttitude.Nick;	196	nick = I2C_WriteAttitude.Nick;
194	roll = I2C_WriteAttitude.Roll;	197	roll = I2C_WriteAttitude.Roll;
195	sei(); // start interrupts	198	sei(); // start interrupts
196	break;	199	break;
197	case ATTITUDE_SOURCE_UART:	200	case ATTITUDE_SOURCE_UART:
198	cli(); // stop interrupts	201	cli(); // stop interrupts
199	nick = ExternData.Attitude[NICK];	202	nick = ExternData.Attitude[NICK];
200	roll = ExternData.Attitude[ROLL];	203	roll = ExternData.Attitude[ROLL];
201	sei(); // start interrupts	204	sei(); // start interrupts
202	break;	205	break;
203	case ATTITUDE_SOURCE_ACC:	206	case ATTITUDE_SOURCE_ACC:
204	nick = AccAttitudeNick;	207	nick = AccAttitudeNick;
205	roll = AccAttitudeRoll;	208	roll = AccAttitudeRoll;
206	break;	209	break;
207	default:	210	default:
208	nick = 0;	211	nick = 0;
209	roll = 0;	212	roll = 0;
210	break;	213	break;
211	}	214	}
212		215
213	nick_rad = ((double)nick) * M_PI / (double)(1800.0);	216	nick_rad = ((double)nick) * M_PI / (double)(1800.0);
214	roll_rad = ((double)roll) * M_PI / (double)(1800.0);	217	roll_rad = ((double)roll) * M_PI / (double)(1800.0);
215		218
216	// calculate attitude correction	219	// calculate attitude correction
217	Hx = Cx * cos(nick_rad) - Cz * sin(nick_rad);	220	Hx = Cx * cos(nick_rad) - Cz * sin(nick_rad);
218	Hy = Cy * cos(roll_rad) + Cz * sin(roll_rad);	221	Hy = Cy * cos(roll_rad) + Cz * sin(roll_rad);
219		222
220	DebugOut.Analog[27] = (int16_t)Hx;	223	DebugOut.Analog[27] = (int16_t)Hx;
221	DebugOut.Analog[28] = (int16_t)Hy;	224	DebugOut.Analog[28] = (int16_t)Hy;
222		225
223	// calculate Heading	226	// calculate Heading
224	heading = (int16_t)((180.0 * atan2(Hy, Hx)) / M_PI);	227	heading = (int16_t)((180.0 * atan2(Hy, Hx)) / M_PI);
225	// atan2 returns angular range from -180 deg to 180 deg in counter clockwise notation	228	// atan2 returns angular range from -180 deg to 180 deg in counter clockwise notation
226	// but the compass course is defined in a range from 0 deg to 360 deg clockwise notation.	229	// but the compass course is defined in a range from 0 deg to 360 deg clockwise notation.
227	if (heading < 0) heading = -heading;	230	if (heading < 0) heading = -heading;
228	else heading = 360 - heading;	231	else heading = 360 - heading;
229		232
230	//++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++	233	//++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
231	if(Calibration.Version != CALIBRATION_VERSION) heading = -1; // Version of the calibration Data does not match	234	if(Calibration.Version != CALIBRATION_VERSION) heading = -1; // Version of the calibration Data does not match
232	//++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++	235	//++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
233	cli(); // stop interrupts	236	cli(); // stop interrupts
234	if(abs(heading) < 361) Heading = heading;	237	if(abs(heading) < 361) Heading = heading;
235	else (Heading = -1);	238	else (Heading = -1);
236	sei(); // start interrupts	239	sei(); // start interrupts
237		240
238	}	241	}
239		242
240	void Calibrate(void)	243	void Calibrate(void)
241	{	244	{
242	uint8_t cal;	245	uint8_t cal;
243	static uint8_t calold = 0;	246	static uint8_t calold = 0;
244	static int16_t Xmin = 0, Xmax = 0, Ymin = 0, Ymax = 0, Zmin = 0, Zmax = 0;	247	static int16_t Xmin = 0, Xmax = 0, Ymin = 0, Ymax = 0, Zmin = 0, Zmax = 0;
245	static uint8_t blinkcount = 0;	248	static uint8_t blinkcount = 0;
246	static uint8_t invert_blinking = 0;	249	static uint8_t invert_blinking = 0;
247		250
248	// check both sources of communication for calibration request	251	// check both sources of communication for calibration request
249	if(I2C_WriteCal.CalByte) cal = I2C_WriteCal.CalByte;	252	if(I2C_WriteCal.CalByte) cal = I2C_WriteCal.CalByte;
250	else cal = ExternData.CalState;	253	else cal = ExternData.CalState;
251		254
252		255
253	if(cal > 5) cal = 0;	256	if(cal > 5) cal = 0;
254	// blink code for current calibration state	257	// blink code for current calibration state
255	if(cal)	258	if(cal)
256	{	259	{
257	if(CheckDelay(Led_Timer) \|\| (cal != calold))	260	if(CheckDelay(Led_Timer) \|\| (cal != calold))
258	{	261	{
259	if(blinkcount & 0x01) if(invert_blinking) LED_GRN_ON; else LED_GRN_OFF;	262	if(blinkcount & 0x01) if(invert_blinking) LED_GRN_ON; else LED_GRN_OFF;
260	else if(invert_blinking) LED_GRN_OFF; else LED_GRN_ON;	263	else if(invert_blinking) LED_GRN_OFF; else LED_GRN_ON;
261		264
262	// end of blinkcount sequence	265	// end of blinkcount sequence
263	if((blinkcount + 1 ) >= (2 * cal))	266	if((blinkcount + 1 ) >= (2 * cal))
264	{	267	{
265	blinkcount = 0;	268	blinkcount = 0;
266	Led_Timer = SetDelay(1500);	269	Led_Timer = SetDelay(1500);
267	}	270	}
268	else	271	else
269	{	272	{
270	blinkcount++;	273	blinkcount++;
271	Led_Timer = SetDelay(100);	274	Led_Timer = SetDelay(100);
272	}	275	}
273	}	276	}
274	}	277	}
275	else	278	else
276	{	279	{
277	if(invert_blinking) LED_GRN_ON; else LED_GRN_OFF;	280	if(invert_blinking) LED_GRN_ON; else LED_GRN_OFF;
278	}	281	}
279	// calibration state machine	282	// calibration state machine
280	switch(cal)	283	switch(cal)
281	{	284	{
282	case 1: // 1st step of calibration	285	case 1: // 1st step of calibration
283	// initialize ranges	286	// initialize ranges
284	// used to change the orientation of the MK3MAG in the horizontal plane	287	// used to change the orientation of the MK3MAG in the horizontal plane
285	Xmin = 10000;	288	Xmin = 10000;
286	Xmax = -10000;	289	Xmax = -10000;
287	Ymin = 10000;	290	Ymin = 10000;
288	Ymax = -10000;	291	Ymax = -10000;
289	Zmin = 10000;	292	Zmin = 10000;
290	Zmax = -10000;	293	Zmax = -10000;
291	Calibration.AccX.Offset = RawAccX;	294	Calibration.AccX.Offset = RawAccX;
292	Calibration.AccY.Offset = RawAccY;	295	Calibration.AccY.Offset = RawAccY;
293	Calibration.AccZ.Offset = RawAccZ;	296	Calibration.AccZ.Offset = RawAccZ;
294	invert_blinking = 0;	297	invert_blinking = 0;
295	break;	298	break;
296		299
297	case 2: // 2nd step of calibration	300	case 2: // 2nd step of calibration
298	// find Min and Max of the X- and Y-Sensors during rotation in the horizontal plane	301	// find Min and Max of the X- and Y-Sensors during rotation in the horizontal plane
299	if(UncalMagX < Xmin) Xmin = UncalMagX;	302	if(UncalMagX < Xmin) Xmin = UncalMagX;
300	if(UncalMagX > Xmax) Xmax = UncalMagX;	303	if(UncalMagX > Xmax) Xmax = UncalMagX;
301	if(UncalMagY < Ymin) Ymin = UncalMagY;	304	if(UncalMagY < Ymin) Ymin = UncalMagY;
302	if(UncalMagY > Ymax) Ymax = UncalMagY;	305	if(UncalMagY > Ymax) Ymax = UncalMagY;
303	invert_blinking = 1;	306	invert_blinking = 1;
304	break;	307	break;
305		308
306	case 3: // 3rd step of calibration	309	case 3: // 3rd step of calibration
307	// used to change the orientation of the MK3MAG vertical to the horizontal plane	310	// used to change the orientation of the MK3MAG vertical to the horizontal plane
308	invert_blinking = 0;	311	invert_blinking = 0;
309	break;	312	break;
310		313
311	case 4:	314	case 4:
312	// find Min and Max of the Z-Sensor	315	// find Min and Max of the Z-Sensor
313	if(UncalMagZ < Zmin) Zmin = UncalMagZ;	316	if(UncalMagZ < Zmin) Zmin = UncalMagZ;
314	if(UncalMagZ > Zmax) Zmax = UncalMagZ;	317	if(UncalMagZ > Zmax) Zmax = UncalMagZ;
315	invert_blinking = 1;	318	invert_blinking = 1;
316	break;	319	break;
317		320
318	case 5:	321	case 5:
319	// Save values	322	// Save values
320	if(cal != calold) // avoid continously writing of eeprom!	323	if(cal != calold) // avoid continously writing of eeprom!
321	{	324	{
322	Calibration.MagX.Range = Xmax - Xmin;	325	Calibration.MagX.Range = Xmax - Xmin;
323	Calibration.MagX.Offset = (Xmin + Xmax) / 2;	326	Calibration.MagX.Offset = (Xmin + Xmax) / 2;
324	Calibration.MagY.Range = Ymax - Ymin;	327	Calibration.MagY.Range = Ymax - Ymin;
325	Calibration.MagY.Offset = (Ymin + Ymax) / 2;	328	Calibration.MagY.Offset = (Ymin + Ymax) / 2;
326	Calibration.MagZ.Range = Zmax - Zmin;	329	Calibration.MagZ.Range = Zmax - Zmin;
327	Calibration.MagZ.Offset = (Zmin + Zmax) / 2;	330	Calibration.MagZ.Offset = (Zmin + Zmax) / 2;
328	if((Calibration.MagX.Range > 150) && (Calibration.MagY.Range > 150) && (Calibration.MagZ.Range > 150))	331	if((Calibration.MagX.Range > 150) && (Calibration.MagY.Range > 150) && (Calibration.MagZ.Range > 150))
329	{	332	{
330	Calibration.Version = CALIBRATION_VERSION;	333	Calibration.Version = CALIBRATION_VERSION;
331	// indicate write process by setting the led off for 2 seconds	334	// indicate write process by setting the led off for 2 seconds
332	LED_GRN_OFF;	335	LED_GRN_OFF;
333	eeprom_write_block(&Calibration, &eeCalibration, sizeof(Calibration));	336	eeprom_write_block(&Calibration, &eeCalibration, sizeof(Calibration));
334	Led_Timer = SetDelay(2000);	337	Led_Timer = SetDelay(2000);
335	// reset blinkcode	338	// reset blinkcode
336	blinkcount = 0;	339	blinkcount = 0;
337	}	340	}
338	}	341	}
339	invert_blinking = 0;	342	invert_blinking = 0;
340	break;	343	break;
341		344
342	default:	345	default:
343	break;	346	break;
344	}	347	}
345	calold = cal;	348	calold = cal;
346	}	349	}
347		350
348		351
349	void SetDebugValues(void)	352	void SetDebugValues(void)
350	{	353	{
351	DebugOut.Analog[0] = MagX;	354	DebugOut.Analog[0] = MagX;
352	DebugOut.Analog[1] = MagY;	355	DebugOut.Analog[1] = MagY;
353	DebugOut.Analog[2] = MagZ;	356	DebugOut.Analog[2] = MagZ;
354	DebugOut.Analog[3] = UncalMagX;	357	DebugOut.Analog[3] = UncalMagX;
355	DebugOut.Analog[4] = UncalMagY;	358	DebugOut.Analog[4] = UncalMagY;
356	DebugOut.Analog[5] = UncalMagZ;	359	DebugOut.Analog[5] = UncalMagZ;
357	switch(AttitudeSource)	360	switch(AttitudeSource)
358	{	361	{
359	case ATTITUDE_SOURCE_ACC:	362	case ATTITUDE_SOURCE_ACC:
360	DebugOut.Analog[6] = AccAttitudeNick;	363	DebugOut.Analog[6] = AccAttitudeNick;
361	DebugOut.Analog[7] = AccAttitudeRoll;	364	DebugOut.Analog[7] = AccAttitudeRoll;
362	break;	365	break;
363		366
364	case ATTITUDE_SOURCE_UART:	367	case ATTITUDE_SOURCE_UART:
365	DebugOut.Analog[6] = ExternData.Attitude[NICK];	368	DebugOut.Analog[6] = ExternData.Attitude[NICK];
366	DebugOut.Analog[7] = ExternData.Attitude[ROLL];	369	DebugOut.Analog[7] = ExternData.Attitude[ROLL];
367	break;	370	break;
368		371
369		372
370	case ATTITUDE_SOURCE_I2C:	373	case ATTITUDE_SOURCE_I2C:
371	DebugOut.Analog[6] = I2C_WriteAttitude.Nick;	374	DebugOut.Analog[6] = I2C_WriteAttitude.Nick;
372	DebugOut.Analog[7] = I2C_WriteAttitude.Roll;	375	DebugOut.Analog[7] = I2C_WriteAttitude.Roll;
373	break;	376	break;
374	}	377	}
375	DebugOut.Analog[8] = Calibration.MagX.Offset;	378	DebugOut.Analog[8] = Calibration.MagX.Offset;
376	DebugOut.Analog[9] = Calibration.MagX.Range;	379	DebugOut.Analog[9] = Calibration.MagX.Range;
377	DebugOut.Analog[10] = Calibration.MagY.Offset;	380	DebugOut.Analog[10] = Calibration.MagY.Offset;
378	DebugOut.Analog[11] = Calibration.MagY.Range;	381	DebugOut.Analog[11] = Calibration.MagY.Range;
379	DebugOut.Analog[12] = Calibration.MagZ.Offset;	382	DebugOut.Analog[12] = Calibration.MagZ.Offset;
380	DebugOut.Analog[13] = Calibration.MagZ.Range;	383	DebugOut.Analog[13] = Calibration.MagZ.Range;
381	if(I2C_WriteCal.CalByte) DebugOut.Analog[14] = I2C_WriteCal.CalByte;	384	if(I2C_WriteCal.CalByte) DebugOut.Analog[14] = I2C_WriteCal.CalByte;
382	else DebugOut.Analog[14] = ExternData.CalState;	385	else DebugOut.Analog[14] = ExternData.CalState;
383	DebugOut.Analog[15] = Heading;	386	DebugOut.Analog[15] = Heading;
384	DebugOut.Analog[16] = ExternData.UserParam[0];	387	DebugOut.Analog[16] = ExternData.UserParam[0];
385	DebugOut.Analog[17] = ExternData.UserParam[1];	388	DebugOut.Analog[17] = ExternData.UserParam[1];
386	DebugOut.Analog[18] = AccX;	389	DebugOut.Analog[18] = AccX;
387	DebugOut.Analog[19] = AccY;	390	DebugOut.Analog[19] = AccY;
388	DebugOut.Analog[20] = AccZ;	391	DebugOut.Analog[20] = AccZ;
389	DebugOut.Analog[21] = RawAccX;	392	DebugOut.Analog[21] = RawAccX;
390	DebugOut.Analog[22] = RawAccY;	393	DebugOut.Analog[22] = RawAccY;
391	DebugOut.Analog[23] = RawAccZ;	394	DebugOut.Analog[23] = RawAccZ;
392	DebugOut.Analog[24] = Calibration.AccX.Offset;	395	DebugOut.Analog[24] = Calibration.AccX.Offset;
393	DebugOut.Analog[25] = Calibration.AccY.Offset;	396	DebugOut.Analog[25] = Calibration.AccY.Offset;
394	DebugOut.Analog[26] = Calibration.AccZ.Offset;	397	DebugOut.Analog[26] = Calibration.AccZ.Offset;
395	DebugOut.Analog[29] = AttitudeSource;	398	DebugOut.Analog[29] = AttitudeSource;
396	}	399	}
397		400
398	void AccMeasurement(void)	401	void AccMeasurement(void)
399	{	402	{
400	if(AccPresent)	403	if(AccPresent)
401	{	404	{
402	RawAccX = (RawAccX + (int16_t)ADC_GetValue(ACC_X))/2;	405	RawAccX = (RawAccX + (int16_t)ADC_GetValue(ACC_X))/2;
403	RawAccY = (RawAccY + (int16_t)ADC_GetValue(ACC_Y))/2;	406	RawAccY = (RawAccY + (int16_t)ADC_GetValue(ACC_Y))/2;
404	RawAccZ = (RawAccZ + (int16_t)ADC_GetValue(ACC_Z))/2;	407	RawAccZ = (RawAccZ + (int16_t)ADC_GetValue(ACC_Z))/2;
405	}	408	}
406	else	409	else
407	{	410	{
408	RawAccX = 0;	411	RawAccX = 0;
409	RawAccY = 0;	412	RawAccY = 0;
410	RawAccZ = 0;	413	RawAccZ = 0;
411	}	414	}
412	}	415	}
413		416
414	int main (void)	417	int main (void)
415	{	418	{
416	// reset input pullup	419	// reset input pullup
417	DDRC &=~((1<<DDC6));	420	DDRC &=~((1<<DDC6));
418	PORTC \|= (1<<PORTC6);	421	PORTC \|= (1<<PORTC6);
419		422
420	LED_Init();	423	LED_Init();
421	TIMER0_Init();	424	TIMER0_Init();
422	USART0_Init();	425	USART0_Init();
423	ADC_Init();	426	ADC_Init();
424	I2C_Init();	427	I2C_Init();
425		428
426	sei(); // enable globale interrupts	429	sei(); // enable globale interrupts
427		430
428	if(AccPresent)	431	if(AccPresent)
429	{	432	{
430	USART0_Print("ACC present\n");	433	USART0_Print("ACC present\n");
431	}	434	}
432		435
433	LED_GRN_ON;	436	LED_GRN_ON;
434		437
435	Led_Timer = SetDelay(200);	438	Led_Timer = SetDelay(200);
436		439
437	// read calibration info from eeprom	440	// read calibration info from eeprom
438	eeprom_read_block(&Calibration, &eeCalibration, sizeof(Calibration));	441	eeprom_read_block(&Calibration, &eeCalibration, sizeof(Calibration));
439		442
440	ExternData.CalState = 0;	443	ExternData.CalState = 0;
441	I2C_WriteCal.CalByte = 0;	444	I2C_WriteCal.CalByte = 0;
442		445
443		446
444	// main loop	447	// main loop
445	while (1)	448	while (1)
446	{	449	{
447	FLIP_LOW;	450	FLIP_LOW;
448	Delay_ms(2);	451	Delay_ms(2);
449	RawMagnet1a = ADC_GetValue(MAG_X);	452	RawMagnet1a = ADC_GetValue(MAG_X);
450	RawMagnet2a = -ADC_GetValue(MAG_Y);	453	RawMagnet2a = -ADC_GetValue(MAG_Y);
451	RawMagnet3a = ADC_GetValue(MAG_Z);	454	RawMagnet3a = ADC_GetValue(MAG_Z);
452	AccMeasurement();	455	AccMeasurement();
453	Delay_ms(1);	456	Delay_ms(1);
454		457
455	FLIP_HIGH;	458	FLIP_HIGH;
456	Delay_ms(2);	459	Delay_ms(2);
457	RawMagnet1b = ADC_GetValue(MAG_X);	460	RawMagnet1b = ADC_GetValue(MAG_X);
458	RawMagnet2b = -ADC_GetValue(MAG_Y);	461	RawMagnet2b = -ADC_GetValue(MAG_Y);
459	RawMagnet3b = ADC_GetValue(MAG_Z);	462	RawMagnet3b = ADC_GetValue(MAG_Z);
460	AccMeasurement();	463	AccMeasurement();
461	Delay_ms(1);	464	Delay_ms(1);
462		465
463	CalcFields();	466	CalcFields();
464		467
465	if(ExternData.CalState \|\| I2C_WriteCal.CalByte) Calibrate();	468	if(ExternData.CalState \|\| I2C_WriteCal.CalByte) Calibrate();
466	else CalcHeading();	469	else CalcHeading();
467		470
468	// check data from USART	471	// check data from USART
469	USART0_ProcessRxData();	472	USART0_ProcessRxData();
470		473
471	if(NC_Connected) NC_Connected--;	474	if(NC_Connected) NC_Connected--;
472	if(FC_Connected) FC_Connected--;	475	if(FC_Connected) FC_Connected--;
473	// fall back to attitude estimation from acc sensor if NC or FC does'nt send attittude data	476	// fall back to attitude estimation from acc sensor if NC or FC does'nt send attittude data
474	if(!FC_Connected && ! NC_Connected)	477	if(!FC_Connected && ! NC_Connected)
475	{	478	{
476	AttitudeSource = ATTITUDE_SOURCE_ACC;	479	AttitudeSource = ATTITUDE_SOURCE_ACC;
477	Orientation = ORIENTATION_FC;	480	Orientation = ORIENTATION_FC;
478	}	481	}
479		482
480	if(PC_Connected)	483	if(PC_Connected)
481	{	484	{
482	USART0_EnableTXD();	485	USART0_EnableTXD();
483	USART0_TransmitTxData();	486	USART0_TransmitTxData();
484	PC_Connected--;	487	PC_Connected--;
485	}	488	}
486	else	489	else
487	{	490	{
488	USART0_DisableTXD();	491	USART0_DisableTXD();
489	}	492	}
490	} // while(1)	493	} // while(1)
491	}	494	}
492		495
493		496

Subversion Repositories MK3Mag

(root)/tags/V0.21b/main.c - Rev 51 → 62