Subversion Repositories NaviCtrl

Rev

Rev 738 | Go to most recent revision | Only display areas with differences | Ignore whitespace | Details | Blame | Last modification | View Log | RSS feed

Rev 738 Rev 739
1 
/*#######################################################################################*/
 1 
/*#######################################################################################*/
2 
/* !!! THIS IS NOT FREE SOFTWARE !!!                                                     */
 2 
/* !!! THIS IS NOT FREE SOFTWARE !!!                                                     */
3 
/*#######################################################################################*/
 3 
/*#######################################################################################*/
4 
// ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
 4 
// ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
5 
// + www.MikroKopter.com
 5 
// + www.MikroKopter.com
6 
// ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
 6 
// ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
7 
// + Software Nutzungsbedingungen (english version: see below)
 7 
// + Software Nutzungsbedingungen (english version: see below)
8 
// + der Fa. HiSystems GmbH, Flachsmeerstrasse 2, 26802 Moormerland - nachfolgend Lizenzgeber genannt -
 8 
// + der Fa. HiSystems GmbH, Flachsmeerstrasse 2, 26802 Moormerland - nachfolgend Lizenzgeber genannt -
9 
// + Der Lizenzgeber räumt dem Kunden ein nicht-ausschließliches, zeitlich und räumlich* unbeschränktes Recht ein, die im den
 9 
// + Der Lizenzgeber räumt dem Kunden ein nicht-ausschließliches, zeitlich und räumlich* unbeschränktes Recht ein, die im den
10 
// + Mikrocontroller verwendete Firmware für die Hardware Flight-Ctrl, Navi-Ctrl, BL-Ctrl, MK3Mag & PC-Programm MikroKopter-Tool
 10 
// + Mikrocontroller verwendete Firmware für die Hardware Flight-Ctrl, Navi-Ctrl, BL-Ctrl, MK3Mag & PC-Programm MikroKopter-Tool
11 
// + - nachfolgend Software genannt - nur für private Zwecke zu nutzen.
 11 
// + - nachfolgend Software genannt - nur für private Zwecke zu nutzen.
12 
// + Der Einsatz dieser Software ist nur auf oder mit Produkten des Lizenzgebers zulässig.
 12 
// + Der Einsatz dieser Software ist nur auf oder mit Produkten des Lizenzgebers zulässig.
13 
// ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
 13 
// ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
14 
// + Die vom Lizenzgeber gelieferte Software ist urheberrechtlich geschützt. Alle Rechte an der Software sowie an sonstigen im
 14 
// + Die vom Lizenzgeber gelieferte Software ist urheberrechtlich geschützt. Alle Rechte an der Software sowie an sonstigen im
15 
// + Rahmen der Vertragsanbahnung und Vertragsdurchführung überlassenen Unterlagen stehen im Verhältnis der Vertragspartner ausschließlich dem Lizenzgeber zu.
 15 
// + Rahmen der Vertragsanbahnung und Vertragsdurchführung überlassenen Unterlagen stehen im Verhältnis der Vertragspartner ausschließlich dem Lizenzgeber zu.
16 
// + Die in der Software enthaltenen Copyright-Vermerke, Markenzeichen, andere Rechtsvorbehalte, Seriennummern sowie
 16 
// + Die in der Software enthaltenen Copyright-Vermerke, Markenzeichen, andere Rechtsvorbehalte, Seriennummern sowie
17 
// + sonstige der Programmidentifikation dienenden Merkmale dürfen vom Kunden nicht verändert oder unkenntlich gemacht werden.
 17 
// + sonstige der Programmidentifikation dienenden Merkmale dürfen vom Kunden nicht verändert oder unkenntlich gemacht werden.
18 
// + Der Kunde trifft angemessene Vorkehrungen für den sicheren Einsatz der Software. Er wird die Software gründlich auf deren
 18 
// + Der Kunde trifft angemessene Vorkehrungen für den sicheren Einsatz der Software. Er wird die Software gründlich auf deren
19 
// + Verwendbarkeit zu dem von ihm beabsichtigten Zweck testen, bevor er diese operativ einsetzt.
 19 
// + Verwendbarkeit zu dem von ihm beabsichtigten Zweck testen, bevor er diese operativ einsetzt.
20 
// + Die Haftung des Lizenzgebers wird - soweit gesetzlich zulässig - begrenzt in Höhe des typischen und vorhersehbaren
 20 
// + Die Haftung des Lizenzgebers wird - soweit gesetzlich zulässig - begrenzt in Höhe des typischen und vorhersehbaren
21 
// + Schadens. Die gesetzliche Haftung bei Personenschäden und nach dem Produkthaftungsgesetz bleibt unberührt. Dem Lizenzgeber steht jedoch der Einwand
 21 
// + Schadens. Die gesetzliche Haftung bei Personenschäden und nach dem Produkthaftungsgesetz bleibt unberührt. Dem Lizenzgeber steht jedoch der Einwand
22 
// + des Mitverschuldens offen.
 22 
// + des Mitverschuldens offen.
23 
// + Der Kunde trifft angemessene Vorkehrungen für den Fall, dass die Software ganz oder teilweise nicht ordnungsgemäß arbeitet.
 23 
// + Der Kunde trifft angemessene Vorkehrungen für den Fall, dass die Software ganz oder teilweise nicht ordnungsgemäß arbeitet.
24 
// + Er wird die Software gründlich auf deren Verwendbarkeit zu dem von ihm beabsichtigten Zweck testen, bevor er diese operativ einsetzt.
 24 
// + Er wird die Software gründlich auf deren Verwendbarkeit zu dem von ihm beabsichtigten Zweck testen, bevor er diese operativ einsetzt.
25 
// + Der Kunde wird er seine Daten vor Einsatz der Software nach dem Stand der Technik sichern.
 25 
// + Der Kunde wird er seine Daten vor Einsatz der Software nach dem Stand der Technik sichern.
26 
// + Der Kunde ist darüber unterrichtet, dass der Lizenzgeber seine Daten im zur Vertragsdurchführung erforderlichen Umfang
 26 
// + Der Kunde ist darüber unterrichtet, dass der Lizenzgeber seine Daten im zur Vertragsdurchführung erforderlichen Umfang
27 
// + und auf Grundlage der Datenschutzvorschriften erhebt, speichert, verarbeitet und, sofern notwendig, an Dritte übermittelt.
 27 
// + und auf Grundlage der Datenschutzvorschriften erhebt, speichert, verarbeitet und, sofern notwendig, an Dritte übermittelt.
28 
// + *) Die räumliche Nutzung bezieht sich nur auf den Einsatzort, nicht auf die Reichweite der programmierten Software.
 28 
// + *) Die räumliche Nutzung bezieht sich nur auf den Einsatzort, nicht auf die Reichweite der programmierten Software.
29 
// + #### ENDE DER NUTZUNGSBEDINGUNGEN ####'
 29 
// + #### ENDE DER NUTZUNGSBEDINGUNGEN ####'
30 
// +  Hinweis: Informationen über erweiterte Nutzungsrechte (wie z.B. Nutzung für nicht-private Zwecke) sind auf Anfrage per Email an info(@)hisystems.de verfügbar.
 30 
// +  Hinweis: Informationen über erweiterte Nutzungsrechte (wie z.B. Nutzung für nicht-private Zwecke) sind auf Anfrage per Email an info(@)hisystems.de verfügbar.
31 
// ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
 31 
// ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
32 
// + Software LICENSING TERMS
 32 
// + Software LICENSING TERMS
33 
// ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
 33 
// ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
34 
// + of HiSystems GmbH, Flachsmeerstrasse 2, 26802 Moormerland, Germany - the Licensor -
 34 
// + of HiSystems GmbH, Flachsmeerstrasse 2, 26802 Moormerland, Germany - the Licensor -
35 
// + The Licensor grants the customer a non-exclusive license to use the microcontroller firmware of the Flight-Ctrl, Navi-Ctrl, BL-Ctrl, and MK3Mag hardware
 35 
// + The Licensor grants the customer a non-exclusive license to use the microcontroller firmware of the Flight-Ctrl, Navi-Ctrl, BL-Ctrl, and MK3Mag hardware
36 
// + (the Software) exclusively for private purposes. The License is unrestricted with respect to time and territory*.
 36 
// + (the Software) exclusively for private purposes. The License is unrestricted with respect to time and territory*.
37 
// + The Software may only be used with the Licensor's products.
 37 
// + The Software may only be used with the Licensor's products.
38 
// + The Software provided by the Licensor is protected by copyright. With respect to the relationship between the parties to this
 38 
// + The Software provided by the Licensor is protected by copyright. With respect to the relationship between the parties to this
39 
// + agreement, all rights pertaining to the Software and other documents provided during the preparation and execution of this
 39 
// + agreement, all rights pertaining to the Software and other documents provided during the preparation and execution of this
40 
// + agreement shall be the property of the Licensor.
 40 
// + agreement shall be the property of the Licensor.
41 
// + The information contained in the Software copyright notices, trademarks, other legal reservations, serial numbers and other
 41 
// + The information contained in the Software copyright notices, trademarks, other legal reservations, serial numbers and other
42 
// + features that can be used to identify the program may not be altered or defaced by the customer.
 42 
// + features that can be used to identify the program may not be altered or defaced by the customer.
43 
// + The customer shall be responsible for taking reasonable precautions
 43 
// + The customer shall be responsible for taking reasonable precautions
44 
// + for the safe use of the Software. The customer shall test the Software thoroughly regarding its suitability for the
 44 
// + for the safe use of the Software. The customer shall test the Software thoroughly regarding its suitability for the
45 
// + intended purpose before implementing it for actual operation. The Licensor's liability shall be limited to the extent of typical and
 45 
// + intended purpose before implementing it for actual operation. The Licensor's liability shall be limited to the extent of typical and
46 
// + foreseeable damage to the extent permitted by law, notwithstanding statutory liability for bodily injury and product
 46 
// + foreseeable damage to the extent permitted by law, notwithstanding statutory liability for bodily injury and product
47 
// + liability. However, the Licensor shall be entitled to the defense of contributory negligence.
 47 
// + liability. However, the Licensor shall be entitled to the defense of contributory negligence.
48 
// + The customer will take adequate precautions in the case, that the software is not working properly. The customer will test
 48 
// + The customer will take adequate precautions in the case, that the software is not working properly. The customer will test
49 
// + the software for his purpose before any operational usage. The customer will backup his data before using the software.
 49 
// + the software for his purpose before any operational usage. The customer will backup his data before using the software.
50 
// + The customer understands that the Licensor collects, stores and processes, and, where required, forwards, customer data
 50 
// + The customer understands that the Licensor collects, stores and processes, and, where required, forwards, customer data
51 
// + to third parties to the extent necessary for executing the agreement, subject to applicable data protection and privacy regulations.
 51 
// + to third parties to the extent necessary for executing the agreement, subject to applicable data protection and privacy regulations.
52 
// + *) The territory aspect only refers to the place where the Software is used, not its programmed range.
 52 
// + *) The territory aspect only refers to the place where the Software is used, not its programmed range.
53 
// + #### END OF LICENSING TERMS ####
 53 
// + #### END OF LICENSING TERMS ####
54 
// + Note: For information on license extensions (e.g. commercial use), please contact us at info(@)hisystems.de.
 54 
// + Note: For information on license extensions (e.g. commercial use), please contact us at info(@)hisystems.de.
55 
// ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
 55 
// ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
56 
//#define MCLK96MHZ
 56 
//#define MCLK96MHZ
57 
const unsigned long _Main_Crystal = 25000;
 57 
const unsigned long _Main_Crystal = 25000;
58 
//#include <stdio.h>
 58 
//#include <stdio.h>
59 
#include <string.h>
 59 
#include <string.h>
60 
#include "91x_lib.h"
 60 
#include "91x_lib.h"
61 
#include "led.h"
 61 
#include "led.h"
62 
#include "uart0.h"
 62 
#include "uart0.h"
63 
#include "uart1.h"
 63 
#include "uart1.h"
64 
#include "uart2.h"
 64 
#include "uart2.h"
65 
#include "gps.h"
 65 
#include "gps.h"
66 
#include "i2c.h"
 66 
#include "i2c.h"
67 
#include "compass.h"
 67 
#include "compass.h"
68 
#include "ncmag.h"
 68 
#include "ncmag.h"
69 
#include "timer1.h"
 69 
#include "timer1.h"
70 
#include "timer2.h"
 70 
#include "timer2.h"
71 
#include "analog.h"
 71 
#include "analog.h"
72 
#include "spi_slave.h"
 72 
#include "spi_slave.h"
73 
#include "fat16.h"
 73 
#include "fat16.h"
74 
#include "sdc.h"
 74 
#include "sdc.h"
75 
#include "logging.h"
 75 
#include "logging.h"
76 
#include "params.h"
 76 
#include "params.h"
77 
#include "settings.h"
 77 
#include "settings.h"
78 
#include "config.h"
 78 
#include "config.h"
79 
#include "main.h"
 79 
#include "main.h"
80 
#include "debug.h"
 80 
#include "debug.h"
81 
#include "eeprom.h"
 81 
#include "eeprom.h"
82 
#include "ssc.h"
 82 
#include "ssc.h"
83 
#include "sdc.h"
 83 
#include "sdc.h"
84 
#include "uart1.h"
 84 
#include "uart1.h"
85 
#include "canbus.h"
 85 
#include "canbus.h"
86 
#include "triggerlog.h"
 86 
#include "triggerlog.h"
87 
#include "CamCtrl.h"
 87 
#include "CamCtrl.h"
88 
 
 88 
 
89 
 
 89 
 
90 
#ifdef FOLLOW_ME
 90 
#ifdef FOLLOW_ME
91 
u8 TransmitAlsoToFC = 0;
 91 
u8 TransmitAlsoToFC = 0;
92 
#endif
 92 
#endif
93 
u32 TimerCheckError;
 93 
u32 TimerCheckError;
94 
u8 ErrorCode = 0;
 94 
u8 ErrorCode = 0;
95 
u16 BeepTime;
 95 
u16 BeepTime;
96 
u8  NCFlags = 0;
 96 
u8  NCFlags = 0;
97 
s16 GeoMagDec = 0; // local magnetic declination in 0.1 deg
 97 
s16 GeoMagDec = 0; // local magnetic declination in 0.1 deg
98 
u8 ErrorGpsFixLost = 0;
 98 
u8 ErrorGpsFixLost = 0;
99 
u8 FromFC_LoadWP_List = 0, FromFC_Load_SinglePoint = 0, FromFC_Save_SinglePoint = 0;
 99 
u8 FromFC_LoadWP_List = 0, FromFC_Load_SinglePoint = 0, FromFC_Save_SinglePoint = 0;
100 
u8 ToFC_MaxWpListIndex = 4;
 100 
u8 ToFC_MaxWpListIndex = 4;
101 
u8 ClearFCStatusFlags = 0;
 101 
u8 ClearFCStatusFlags = 0;
102 
u8 StopNavigation = 0;
 102 
u8 StopNavigation = 0;
103 
volatile u32 PollingTimeout = 10000;
 103 
volatile u32 PollingTimeout = 10000;
104 
Param_t Parameter;
 104 
Param_t Parameter;
105 
Partner_t Partner;
 105 
Partner_t Partner;
106 
volatile FC_t FC;
 106 
volatile FC_t FC;
107 
volatile u32 SPIWatchDog = 15000; // stop Navigation if this goes to zero
 107 
volatile u32 SPIWatchDog = 15000; // stop Navigation if this goes to zero
108 
volatile u32 SD_WatchDog = 15000; // stop Logging if this goes to zero
 108 
volatile u32 SD_WatchDog = 15000; // stop Logging if this goes to zero
109 
u32 CountGpsProcessedIn5Sec = 50,CountNewGpsDataIn5Sec = 25, FreqGpsNavProcessed = 0, FreqNewGpsData = 0;
 109 
u32 CountGpsProcessedIn5Sec = 50,CountNewGpsDataIn5Sec = 25, FreqGpsNavProcessed = 0, FreqNewGpsData = 0;
110 
u8 NewWPL_Name = 0;
 110 
u8 NewWPL_Name = 0;
111 
u32 MaxWP_Radius_in_m = 0;
 111 
u32 MaxWP_Radius_in_m = 0;
112 
s8 ErrorMSG[25];
 112 
s8 ErrorMSG[25];
113 
s8 PartnerErrorMSG[25] = "     ---                \0";
 113 
s8 PartnerErrorMSG[25] = "     ---                \0";
114 
u32 TimeSinceMotorStart = 0;
 114 
u32 TimeSinceMotorStart = 0;
115 
u8 IamMaster = NOTHING; // for Master/Slave Redundance
 115 
u8 IamMaster = NOTHING; // for Master/Slave Redundance
116 
 
 116 
 
117 
//----------------------------------------------------------------------------------------------------
 117 
//----------------------------------------------------------------------------------------------------
118 
void SCU_Config(void)
 118 
void SCU_Config(void)
119 
{
 119 
{
120 
        /* configure PLL and set it as master clock source */
 120 
        /* configure PLL and set it as master clock source */
121 
        SCU_MCLKSourceConfig(SCU_MCLK_OSC);             // set master clock source to external oscillator clock (25MHz) before diabling the PLL
 121 
        SCU_MCLKSourceConfig(SCU_MCLK_OSC);             // set master clock source to external oscillator clock (25MHz) before diabling the PLL
122 
        SCU_PLLCmd(DISABLE);                                    // now disable the PLL
 122 
        SCU_PLLCmd(DISABLE);                                    // now disable the PLL
123 
        #ifdef MCLK96MHZ
 123 
        #ifdef MCLK96MHZ
124 
        SCU_BRCLKDivisorConfig(SCU_BRCLK_Div2); // set BRCLK to MCLK/2 = 48MHz
 124 
        SCU_BRCLKDivisorConfig(SCU_BRCLK_Div2); // set BRCLK to MCLK/2 = 48MHz
125 
        SCU_PCLKDivisorConfig(SCU_PCLK_Div4);   // set PCLK     (APB bus clock) divisor to 4 (half Reference Clock)
 125 
        SCU_PCLKDivisorConfig(SCU_PCLK_Div4);   // set PCLK     (APB bus clock) divisor to 4 (half Reference Clock)
126 
        SCU_RCLKDivisorConfig(SCU_RCLK_Div2);   // set RCLK     (Reference Clock) divisor to 1 (full PPL clock)
 126 
        SCU_RCLKDivisorConfig(SCU_RCLK_Div2);   // set RCLK     (Reference Clock) divisor to 1 (full PPL clock)
127 
        SCU_HCLKDivisorConfig(SCU_HCLK_Div2);   // set HCLK     (AHB bus clock) divisor to 1 (full Reference Clock)
 127 
        SCU_HCLKDivisorConfig(SCU_HCLK_Div2);   // set HCLK     (AHB bus clock) divisor to 1 (full Reference Clock)
128 
        SCU_PLLFactorsConfig(192,25,2);                 // PLL = 96 MHz, Feedback Divider N=192, Pre-Divider M=25, Post-Divider P=2
 128 
        SCU_PLLFactorsConfig(192,25,2);                 // PLL = 96 MHz, Feedback Divider N=192, Pre-Divider M=25, Post-Divider P=2
129 
        #else
 129 
        #else
130 
        SCU_BRCLKDivisorConfig(SCU_BRCLK_Div1); // set BRCLK to MCLK = 48MHz
 130 
        SCU_BRCLKDivisorConfig(SCU_BRCLK_Div1); // set BRCLK to MCLK = 48MHz
131 
        SCU_PCLKDivisorConfig(SCU_PCLK_Div2);   // set PCLK     (APB bus clock) divisor to 2 (half Reference Clock)
 131 
        SCU_PCLKDivisorConfig(SCU_PCLK_Div2);   // set PCLK     (APB bus clock) divisor to 2 (half Reference Clock)
132 
        SCU_RCLKDivisorConfig(SCU_RCLK_Div1);   // set RCLK     (Reference Clock) divisor to 1 (full PPL clock)
 132 
        SCU_RCLKDivisorConfig(SCU_RCLK_Div1);   // set RCLK     (Reference Clock) divisor to 1 (full PPL clock)
133 
        SCU_HCLKDivisorConfig(SCU_HCLK_Div1);   // set HCLK     (AHB bus clock) divisor to 1 (full Reference Clock)
 133 
        SCU_HCLKDivisorConfig(SCU_HCLK_Div1);   // set HCLK     (AHB bus clock) divisor to 1 (full Reference Clock)
134 
        SCU_PLLFactorsConfig(192,25,3);                 // PLL = 48 MHz, Feedback Divider N=192, Pre-Divider M=25, Post-Divider P=3
 134 
        SCU_PLLFactorsConfig(192,25,3);                 // PLL = 48 MHz, Feedback Divider N=192, Pre-Divider M=25, Post-Divider P=3
135 
        #endif
 135 
        #endif
136 
        SCU_PLLCmd(ENABLE);                                     // Enable PLL (is disabled by SCU_PLLFactorsConfig)
 136 
        SCU_PLLCmd(ENABLE);                                     // Enable PLL (is disabled by SCU_PLLFactorsConfig)
137 
        SCU_MCLKSourceConfig(SCU_MCLK_PLL);     // set master clock source to PLL
 137 
        SCU_MCLKSourceConfig(SCU_MCLK_PLL);     // set master clock source to PLL
138 
}
 138 
}
139 
 
 139 
 
140 
//----------------------------------------------------------------------------------------------------
 140 
//----------------------------------------------------------------------------------------------------
141 
void GetNaviCtrlVersion(void)
 141 
void GetNaviCtrlVersion(void)
142 
{
 142 
{
143 
        u8 msg[25];
 143 
        u8 msg[25];
144 
 
 144 
 
145 
        sprintf(msg,"\n\r NaviCtrl HW: V%d.%d SW: V%d.%02d%c", UART_VersionInfo.HWMajor/10, UART_VersionInfo.HWMajor%10, VERSION_MAJOR,  VERSION_MINOR, 'a'+ VERSION_PATCH);
 145 
        sprintf(msg,"\n\r NaviCtrl HW: V%d.%d SW: V%d.%02d%c", UART_VersionInfo.HWMajor/10, UART_VersionInfo.HWMajor%10, VERSION_MAJOR,  VERSION_MINOR, 'a'+ VERSION_PATCH);
146 
        UART1_PutString(msg);
 146 
        UART1_PutString(msg);
147 
}
 147 
}
148 
 
 148 
 
149 
//----------------------------------------------------------------------------------------------------
 149 
//----------------------------------------------------------------------------------------------------
150 
 
 150 
 
151 
void CheckErrors(void)
 151 
void CheckErrors(void)
152 
{
 152 
{
153 
    static s32 no_error_delay = 0;
 153 
    static s32 no_error_delay = 0;
154 
        s32 newErrorCode = 0;
 154 
        s32 newErrorCode = 0;
155 
        UART_VersionInfo.HardwareError[0] = 0;
 155 
        UART_VersionInfo.HardwareError[0] = 0;
156 
 
 156 
 
157 
        if(CheckDelay(I2CBus(Compass_I2CPort)->Timeout) || (Compass_Heading < 0)) DebugOut.StatusRed |= AMPEL_COMPASS;
 157 
        if(CheckDelay(I2CBus(Compass_I2CPort)->Timeout) || (Compass_Heading < 0)) DebugOut.StatusRed |= AMPEL_COMPASS;
158 
        else DebugOut.StatusRed &= ~AMPEL_COMPASS; // MK3Mag green status
 158 
        else DebugOut.StatusRed &= ~AMPEL_COMPASS; // MK3Mag green status
159 
 
 159 
 
160 
        if((FC.Error[1] & FC_ERROR1_I2C) || (FC.Error[1] & FC_ERROR1_BL_MISSING)) DebugOut.StatusRed |= AMPEL_BL;
 160 
        if((FC.Error[1] & FC_ERROR1_I2C) || (FC.Error[1] & FC_ERROR1_BL_MISSING)) DebugOut.StatusRed |= AMPEL_BL;
161 
        else DebugOut.StatusRed &= ~AMPEL_BL; // BL-Ctrl green status
 161 
        else DebugOut.StatusRed &= ~AMPEL_BL; // BL-Ctrl green status
162 
 
 162 
 
163 
        if(UART_VersionInfo.HardwareError[0] || UART_VersionInfo.HardwareError[1]) DebugOut.StatusRed |= AMPEL_NC;
 163 
        if(UART_VersionInfo.HardwareError[0] || UART_VersionInfo.HardwareError[1]) DebugOut.StatusRed |= AMPEL_NC;
164 
        else DebugOut.StatusRed &= ~AMPEL_NC;
 164 
        else DebugOut.StatusRed &= ~AMPEL_NC;
165 
 
 165 
 
166 
        if(CheckDelay(I2CBus(Compass_I2CPort)->Timeout))
 166 
        if(CheckDelay(I2CBus(Compass_I2CPort)->Timeout))
167 
        {
 167 
        {
168 
                LED_RED_ON;
 168 
                LED_RED_ON;
169 
                sprintf(ErrorMSG,"no compass communica");
 169 
                sprintf(ErrorMSG,"no compass communica");
170 
                //Reset Compass communication
 170 
                //Reset Compass communication
171 
                if(Compass_I2CPort == NCMAG_PORT_EXTERN) I2CBus_Init(I2C0);     else I2CBus_Init(I2C1);
 171 
                if(Compass_I2CPort == NCMAG_PORT_EXTERN) I2CBus_Init(I2C0);     else I2CBus_Init(I2C1);
172 
                Compass_Init();
 172 
                Compass_Init();
173 
                newErrorCode = 4;
 173 
                newErrorCode = 4;
174 
                StopNavigation = 1;
 174 
                StopNavigation = 1;
175 
                UART_VersionInfo.HardwareError[0] |= NC_ERROR0_COMPASS_RX;
 175 
                UART_VersionInfo.HardwareError[0] |= NC_ERROR0_COMPASS_RX;
176 
                DebugOut.StatusRed |= AMPEL_COMPASS;
 176 
                DebugOut.StatusRed |= AMPEL_COMPASS;
177 
        }
 177 
        }
178 
        else if(CompassValueErrorCount > 30)
 178 
        else if(CompassValueErrorCount > 30)
179 
        {
 179 
        {
180 
                LED_RED_ON;
 180 
                LED_RED_ON;
181 
                sprintf(ErrorMSG,"compass sensor error");
 181 
                sprintf(ErrorMSG,"compass sensor error");
182 
                newErrorCode = 34;
 182 
                newErrorCode = 34;
183 
                StopNavigation = 1;
 183 
                StopNavigation = 1;
184 
                UART_VersionInfo.HardwareError[0] |= NC_ERROR0_COMPASS_VALUE;
 184 
                UART_VersionInfo.HardwareError[0] |= NC_ERROR0_COMPASS_VALUE;
185 
                if(Compass_I2CPort == NCMAG_PORT_EXTERN) I2CBus_Init(I2C0);     else I2CBus_Init(I2C1);
 185 
                if(Compass_I2CPort == NCMAG_PORT_EXTERN) I2CBus_Init(I2C0);     else I2CBus_Init(I2C1);
186 
                Compass_Init();
 186 
                Compass_Init();
187 
        }
 187 
        }
188 
        else if((FCCalibActive || CompassCalState) && FC_Version.Hardware)
 188 
        else if((FCCalibActive || CompassCalState) && FC_Version.Hardware)
189 
        {
 189 
        {
190 
                sprintf(ErrorMSG,"Calibrate... ");
 190 
                sprintf(ErrorMSG,"Calibrate... ");
191 
                newErrorCode = 0;
 191 
                newErrorCode = 0;
192 
                ErrorCode = 0;
 192 
                ErrorCode = 0;
193 
                no_error_delay = 1;
 193 
                no_error_delay = 1;
194 
        }
 194 
        }
195 
        else if(CheckDelay(SPI0_Timeout))
 195 
        else if(CheckDelay(SPI0_Timeout))
196 
        {
 196 
        {
197 
                LED_RED_ON;
 197 
                LED_RED_ON;
198 
                sprintf(ErrorMSG,"no FC communication ");
 198 
                sprintf(ErrorMSG,"no FC communication ");
199 
                newErrorCode = 3;
 199 
                newErrorCode = 3;
200 
                StopNavigation = 1;
 200 
                StopNavigation = 1;
201 
                DebugOut.StatusGreen &= ~AMPEL_FC; // status of FC Present
 201 
                DebugOut.StatusGreen &= ~AMPEL_FC; // status of FC Present
202 
                DebugOut.StatusGreen &= ~AMPEL_BL; // status of BL Present
 202 
                DebugOut.StatusGreen &= ~AMPEL_BL; // status of BL Present
203 
                UART_VersionInfo.HardwareError[0] |= NC_ERROR0_SPI_RX;
 203 
                UART_VersionInfo.HardwareError[0] |= NC_ERROR0_SPI_RX;
204 
        }
 204 
        }
205 
        else if(FC_Version.Compatible != FC_SPI_COMPATIBLE)
 205 
        else if(FC_Version.Compatible != FC_SPI_COMPATIBLE)
206 
        {
 206 
        {
207 
                LED_RED_ON;
 207 
                LED_RED_ON;
208 
#ifndef FOLLOW_ME
 208 
#ifndef FOLLOW_ME
209 
                sprintf(ErrorMSG,"FC not compatible ");
 209 
                sprintf(ErrorMSG,"FC not compatible ");
210 
#else
 210 
#else
211 
                sprintf(ErrorMSG,"! FollowMe only ! ");
 211 
                sprintf(ErrorMSG,"! FollowMe only ! ");
212 
#endif
 212 
#endif
213 
                newErrorCode = 1;
 213 
                newErrorCode = 1;
214 
                StopNavigation = 1;
 214 
                StopNavigation = 1;
215 
                UART_VersionInfo.HardwareError[0] |= NC_ERROR0_FC_INCOMPATIBLE;
 215 
                UART_VersionInfo.HardwareError[0] |= NC_ERROR0_FC_INCOMPATIBLE;
216 
                DebugOut.StatusRed |= AMPEL_NC;
 216 
                DebugOut.StatusRed |= AMPEL_NC;
217 
        }
 217 
        }
218 
        else if(FC.Error[0] & FC_ERROR0_GYRO_NICK)
 218 
        else if(FC.Error[0] & FC_ERROR0_GYRO_NICK)
219 
        {
 219 
        {
220 
                LED_RED_ON;
 220 
                LED_RED_ON;
221 
                sprintf(ErrorMSG,"ERR: FC Nick Gyro");
 221 
                sprintf(ErrorMSG,"ERR: FC Nick Gyro");
222 
                newErrorCode = 10;
 222 
                newErrorCode = 10;
223 
        }
 223 
        }
224 
        else if(FC.Error[0] & FC_ERROR0_GYRO_ROLL)
 224 
        else if(FC.Error[0] & FC_ERROR0_GYRO_ROLL)
225 
        {
 225 
        {
226 
                LED_RED_ON;
 226 
                LED_RED_ON;
227 
                sprintf(ErrorMSG,"ERR: FC Roll Gyro");
 227 
                sprintf(ErrorMSG,"ERR: FC Roll Gyro");
228 
                newErrorCode = 11;
 228 
                newErrorCode = 11;
229 
        }
 229 
        }
230 
        else if(FC.Error[0] & FC_ERROR0_GYRO_YAW)
 230 
        else if(FC.Error[0] & FC_ERROR0_GYRO_YAW)
231 
        {
 231 
        {
232 
                LED_RED_ON;
 232 
                LED_RED_ON;
233 
                sprintf(ErrorMSG,"ERR: FC Yaw Gyro");
 233 
                sprintf(ErrorMSG,"ERR: FC Yaw Gyro");
234 
                newErrorCode = 12;
 234 
                newErrorCode = 12;
235 
        }
 235 
        }
236 
        else if(FC.Error[0] & FC_ERROR0_ACC_NICK)
 236 
        else if(FC.Error[0] & FC_ERROR0_ACC_NICK)
237 
        {
 237 
        {
238 
                LED_RED_ON;
 238 
                LED_RED_ON;
239 
                sprintf(ErrorMSG,"ERR: FC Nick ACC");
 239 
                sprintf(ErrorMSG,"ERR: FC Nick ACC");
240 
                newErrorCode = 13;
 240 
                newErrorCode = 13;
241 
        }
 241 
        }
242 
        else if(FC.Error[0] & FC_ERROR0_ACC_ROLL)
 242 
        else if(FC.Error[0] & FC_ERROR0_ACC_ROLL)
243 
        {
 243 
        {
244 
                LED_RED_ON;
 244 
                LED_RED_ON;
245 
                sprintf(ErrorMSG,"ERR: FC Roll ACC");
 245 
                sprintf(ErrorMSG,"ERR: FC Roll ACC");
246 
                newErrorCode = 14;
 246 
                newErrorCode = 14;
247 
        }
 247 
        }
248 
        else if(FC.Error[0] & FC_ERROR0_ACC_TOP)
 248 
        else if(FC.Error[0] & FC_ERROR0_ACC_TOP)
249 
        {
 249 
        {
250 
                LED_RED_ON;
 250 
                LED_RED_ON;
251 
                sprintf(ErrorMSG,"ERR:FC Z-ACC");
 251 
                sprintf(ErrorMSG,"ERR:FC Z-ACC");
252 
                newErrorCode = 15;
 252 
                newErrorCode = 15;
253 
        }
 253 
        }
254 
        else if(NC_To_FC_Flags & NC_TO_FC_FLYING_RANGE)
 254 
        else if(NC_To_FC_Flags & NC_TO_FC_FLYING_RANGE)
255 
        {
 255 
        {
256 
                LED_RED_ON;
 256 
                LED_RED_ON;
257 
                sprintf(ErrorMSG,"ERR:Flying range!");
 257 
                sprintf(ErrorMSG,"ERR:Flying range!");
258 
                newErrorCode = 28;
 258 
                newErrorCode = 28;
259 
        }
 259 
        }
260 
        else if(FC.Error[0] & FC_ERROR0_PRESSURE)
 260 
        else if(FC.Error[0] & FC_ERROR0_PRESSURE)
261 
        {
 261 
        {
262 
                LED_RED_ON;
 262 
                LED_RED_ON;
263 
                sprintf(ErrorMSG,"ERR:Pressure sensor");
 263 
                sprintf(ErrorMSG,"ERR:Pressure sensor");
264 
                newErrorCode = 16;
 264 
                newErrorCode = 16;
265 
        }
 265 
        }
266 
        else if(FC.Error[1] &  FC_ERROR1_I2C)
 266 
        else if(FC.Error[1] &  FC_ERROR1_I2C)
267 
        {
 267 
        {
268 
                LED_RED_ON;
 268 
                LED_RED_ON;
269 
                sprintf(ErrorMSG,"ERR:I2C FC to BL");
 269 
                sprintf(ErrorMSG,"ERR:I2C FC to BL");
270 
                newErrorCode = 17;
 270 
                newErrorCode = 17;
271 
        }
 271 
        }
272 
        else if(FC.Error[1] &  FC_ERROR1_BL_MISSING)
 272 
        else if(FC.Error[1] &  FC_ERROR1_BL_MISSING)
273 
        {
 273 
        {
274 
                LED_RED_ON;
 274 
                LED_RED_ON;
275 
                sprintf(ErrorMSG,"ERR: Bl Missing");
 275 
                sprintf(ErrorMSG,"ERR: Bl Missing");
276 
                newErrorCode = 18;
 276 
                newErrorCode = 18;
277 
        }
 277 
        }
278 
        else if(FC.Error[1] &  FC_ERROR1_MIXER)
 278 
        else if(FC.Error[1] &  FC_ERROR1_MIXER)
279 
        {
 279 
        {
280 
                LED_RED_ON;
 280 
                LED_RED_ON;
281 
                sprintf(ErrorMSG,"Mixer Error");
 281 
                sprintf(ErrorMSG,"Mixer Error");
282 
                newErrorCode = 19;
 282 
                newErrorCode = 19;
283 
        }
 283 
        }
284 
        else if(CheckDelay(UBX_Timeout) && Parameter.GlobalConfig & FC_CFG_GPS_AKTIV)
 284 
        else if(CheckDelay(UBX_Timeout) && Parameter.GlobalConfig & FC_CFG_GPS_AKTIV)
285 
        {
 285 
        {
286 
                LED_RED_ON;
 286 
                LED_RED_ON;
287 
                sprintf(ErrorMSG,"no GPS communication");
 287 
                sprintf(ErrorMSG,"no GPS communication");
288 
                UART_VersionInfo.HardwareError[0] |= NC_ERROR0_GPS_RX;
 288 
                UART_VersionInfo.HardwareError[0] |= NC_ERROR0_GPS_RX;
289 
                UART_VersionInfo.Flags &= ~NC_VERSION_FLAG_GPS_PRESENT;
 289 
                UART_VersionInfo.Flags &= ~NC_VERSION_FLAG_GPS_PRESENT;
290 
                newErrorCode = 5;
 290 
                newErrorCode = 5;
291 
                StopNavigation = 1;
 291 
                StopNavigation = 1;
292 
        }
 292 
        }
293 
        else if(Compass_Heading < 0 && NCMAG_Present && !NCMAG_IsCalibrated)
 293 
        else if(Compass_Heading < 0 && NCMAG_Present && !NCMAG_IsCalibrated)
294 
        {
 294 
        {
295 
                LED_RED_ON;
 295 
                LED_RED_ON;
296 
                sprintf(ErrorMSG,"compass not calibr.");
 296 
                sprintf(ErrorMSG,"compass not calibr.");
297 
                newErrorCode = 31;
 297 
                newErrorCode = 31;
298 
                StopNavigation = 1;
 298 
                StopNavigation = 1;
299 
                UART_VersionInfo.HardwareError[0] |= NC_ERROR0_COMPASS_VALUE;
 299 
                UART_VersionInfo.HardwareError[0] |= NC_ERROR0_COMPASS_VALUE;
300 
        }
 300 
        }
301 
        else if(Compass_Heading < 0)
 301 
        else if(Compass_Heading < 0)
302 
        {
 302 
        {
303 
                LED_RED_ON;
 303 
                LED_RED_ON;
304 
                sprintf(ErrorMSG,"bad compass value ");
 304 
                sprintf(ErrorMSG,"bad compass value ");
305 
                newErrorCode = 6;
 305 
                newErrorCode = 6;
306 
                StopNavigation = 1;
 306 
                StopNavigation = 1;
307 
                UART_VersionInfo.HardwareError[0] |= NC_ERROR0_COMPASS_VALUE;
 307 
                UART_VersionInfo.HardwareError[0] |= NC_ERROR0_COMPASS_VALUE;
308 
        }
 308 
        }
309 
        else if((FC.Error[1] &  FC_ERROR1_SPI_RX))
 309 
        else if((FC.Error[1] &  FC_ERROR1_SPI_RX))
310 
        {
 310 
        {
311 
                LED_RED_ON;
 311 
                LED_RED_ON;
312 
                sprintf(ErrorMSG,"FC spi rx error ");
 312 
                sprintf(ErrorMSG,"FC spi rx error ");
313 
                newErrorCode = 8;
 313 
                newErrorCode = 8;
314 
                StopNavigation = 1;
 314 
                StopNavigation = 1;
315 
        }
 315 
        }
316 
        else if(FC.Error[0] &  FC_ERROR0_CAREFREE)
 316 
        else if(FC.Error[0] &  FC_ERROR0_CAREFREE)
317 
        {
 317 
        {
318 
                LED_RED_ON;
 318 
                LED_RED_ON;
319 
                sprintf(ErrorMSG,"FC: Carefree Error");
 319 
                sprintf(ErrorMSG,"FC: Carefree Error");
320 
                newErrorCode = 20;
 320 
                newErrorCode = 20;
321 
        }
 321 
        }
322 
        else if(FC.BAT_Voltage < 45)
 322 
        else if(FC.BAT_Voltage < 45)
323 
        {
 323 
        {
324 
         LED_RED_ON;
 324 
         LED_RED_ON;
325 
         sprintf(ErrorMSG,"ERR:Power Supply");
 325 
         sprintf(ErrorMSG,"ERR:Power Supply");
326 
         newErrorCode = 41;
 326 
         newErrorCode = 41;
327 
        }
 327 
        }
328 
        else
 328 
        else
329 
        if(FC.Error[1] & FC_ERROR1_RC_VOLTAGE)
 329 
        if(FC.Error[1] & FC_ERROR1_RC_VOLTAGE)
330 
        {
 330 
        {
331 
         LED_RED_ON;
 331 
         LED_RED_ON;
332 
         sprintf(ErrorMSG,"ERR: 5V RC-Supply");
 332 
         sprintf(ErrorMSG,"ERR: 5V RC-Supply");
333 
         newErrorCode = 40;
 333 
         newErrorCode = 40;
334 
        }
 334 
        }
335 
        else if(FC.Error[1] &  FC_ERROR1_PPM)
 335 
        else if(FC.Error[1] &  FC_ERROR1_PPM)
336 
        {
 336 
        {
337 
                LED_RED_ON;
 337 
                LED_RED_ON;
338 
                sprintf(ErrorMSG,"RC Signal lost ");
 338 
                sprintf(ErrorMSG,"RC Signal lost ");
339 
                newErrorCode = 7;
 339 
                newErrorCode = 7;
340 
        }
 340 
        }
341 
        else if(ErrorGpsFixLost)
 341 
        else if(ErrorGpsFixLost)
342 
        {
 342 
        {
343 
                LED_RED_ON;
 343 
                LED_RED_ON;
344 
                sprintf(ErrorMSG,"GPS Fix lost    ");
 344 
                sprintf(ErrorMSG,"GPS Fix lost    ");
345 
                newErrorCode = 21;
 345 
                newErrorCode = 21;
346 
        }
 346 
        }
347 
        else if(ErrorDisturbedEarthMagnetField)
 347 
        else if(ErrorDisturbedEarthMagnetField)
348 
        {
 348 
        {
349 
                LED_RED_ON;
 349 
                LED_RED_ON;
350 
                sprintf(ErrorMSG,"Magnet error    ");
 350 
                sprintf(ErrorMSG,"Magnet error    ");
351 
                newErrorCode = 22;
 351 
                newErrorCode = 22;
352 
                DebugOut.StatusRed |= AMPEL_COMPASS | AMPEL_NC;
 352 
                DebugOut.StatusRed |= AMPEL_COMPASS | AMPEL_NC;
353 
                UART_VersionInfo.HardwareError[0] |= NC_ERROR0_COMPASS_VALUE;
 353 
                UART_VersionInfo.HardwareError[0] |= NC_ERROR0_COMPASS_VALUE;
354 
        }
 354 
        }
355 
        else if(((ErrorCheck_BL_MinOfMaxPWM == 40 && (TimeSinceMotorStart > 3))  || (ErrorCheck_BL_MinOfMaxPWM == 39)) && !ErrorCode)
 355 
        else if(((ErrorCheck_BL_MinOfMaxPWM == 40 && (TimeSinceMotorStart > 3))  || (ErrorCheck_BL_MinOfMaxPWM == 39)) && !ErrorCode)
356 
        {
 356 
        {
357 
                LED_RED_ON;
 357 
                LED_RED_ON;
358 
                sprintf(ErrorMSG,"ERR:Motor restart  ");
 358 
                sprintf(ErrorMSG,"ERR:Motor restart  ");
359 
                newErrorCode = 23;
 359 
                newErrorCode = 23;
360 
                DebugOut.StatusRed |= AMPEL_BL;
 360 
                DebugOut.StatusRed |= AMPEL_BL;
361 
        }
 361 
        }
362 
        else if(BL_MinOfMaxPWM && BL_MinOfMaxPWM < 30 && !ErrorCode)
 362 
        else if(BL_MinOfMaxPWM && BL_MinOfMaxPWM < 30 && !ErrorCode)
363 
        {
 363 
        {
364 
                u16 i;
 364 
                u16 i;
365 
                for(i = 0; i < 16; i++) if(Motor[i].MaxPWM == BL_MinOfMaxPWM) break; // find the motor number
 365 
                for(i = 0; i < 16; i++) if(Motor[i].MaxPWM == BL_MinOfMaxPWM) break; // find the motor number
366 
                LED_RED_ON;
 366 
                LED_RED_ON;
367 
                sprintf(ErrorMSG,"ERR:BL%2d Test:%2d ",i+1,BL_MinOfMaxPWM);
 367 
                sprintf(ErrorMSG,"ERR:BL%2d Test:%2d ",i+1,BL_MinOfMaxPWM);
368 
                newErrorCode = 32;
 368 
                newErrorCode = 32;
369 
                DebugOut.StatusRed |= AMPEL_BL;
 369 
                DebugOut.StatusRed |= AMPEL_BL;
370 
        }
 370 
        }
371 
        else if(ErrorCheck_BL_MinOfMaxPWM < 248 && (TimeSinceMotorStart > 3) && !ErrorCode)
 371 
        else if(ErrorCheck_BL_MinOfMaxPWM < 248 && (TimeSinceMotorStart > 3) && !ErrorCode)
372 
        {
 372 
        {
373 
                LED_RED_ON;
 373 
                LED_RED_ON;
374 
                sprintf(ErrorMSG,"ERR:BL Limitation   ");
 374 
                sprintf(ErrorMSG,"ERR:BL Limitation   ");
375 
                newErrorCode = 24;
 375 
                newErrorCode = 24;
376 
                DebugOut.StatusRed |= AMPEL_BL;
 376 
                DebugOut.StatusRed |= AMPEL_BL;
377 
        }
 377 
        }
378 
        else if((NCFlags & NC_FLAG_RANGE_LIMIT) && (FC.StatusFlags & FC_STATUS_FLY) && !ErrorCode)
 378 
        else if((NCFlags & NC_FLAG_RANGE_LIMIT) && (FC.StatusFlags & FC_STATUS_FLY) && !ErrorCode)
379 
        {
 379 
        {
380 
                LED_RED_ON;
 380 
                LED_RED_ON;
381 
                sprintf(ErrorMSG,"ERR:GPS WP range ");
 381 
                sprintf(ErrorMSG,"ERR:GPS WP range ");
382 
                newErrorCode = 25;
 382 
                newErrorCode = 25;
383 
                DebugOut.StatusRed |= AMPEL_NC;
 383 
                DebugOut.StatusRed |= AMPEL_NC;
384 
        }
 384 
        }
385 
        else if((!SD_SWITCH || (SDCardInfo.Valid == 0)) && Parameter.GlobalConfig3 & CFG3_NO_SDCARD_NO_START && !(FC.StatusFlags & FC_STATUS_FLY))
 385 
        else if((!SD_SWITCH || (SDCardInfo.Valid == 0)) && Parameter.GlobalConfig3 & CFG3_NO_SDCARD_NO_START && !(FC.StatusFlags & FC_STATUS_FLY))
386 
        {
 386 
        {
387 
                LED_RED_ON;
 387 
                LED_RED_ON;
388 
                sprintf(ErrorMSG,"ERR:No SD-Card  ");
 388 
                sprintf(ErrorMSG,"ERR:No SD-Card  ");
389 
                newErrorCode = 26;
 389 
                newErrorCode = 26;
390 
                DebugOut.StatusRed |= AMPEL_NC;
 390 
                DebugOut.StatusRed |= AMPEL_NC;
391 
        }
 391 
        }
392 
        else if((SD_LoggingError || (SD_WatchDog < 2000 && SD_WatchDog != 0)) && Parameter.GlobalConfig3 & CFG3_NO_SDCARD_NO_START)
 392 
        else if((SD_LoggingError || (SD_WatchDog < 2000 && SD_WatchDog != 0)) && Parameter.GlobalConfig3 & CFG3_NO_SDCARD_NO_START)
393 
        {
 393 
        {
394 
                LED_RED_ON;
 394 
                LED_RED_ON;
395 
                sprintf(ErrorMSG,"ERR:SD Logging abort");
 395 
                sprintf(ErrorMSG,"ERR:SD Logging abort");
396 
                newErrorCode = 27;
 396 
                newErrorCode = 27;
397 
                DebugOut.StatusRed |= AMPEL_NC;
 397 
                DebugOut.StatusRed |= AMPEL_NC;
398 
                SD_LoggingError = 0;
 398 
                SD_LoggingError = 0;
399 
        }
 399 
        }
400 
        else if(((AbsoluteFlyingAltitude) && (NaviData.Altimeter_20cm / 20 >= AbsoluteFlyingAltitude)) && (FC.StatusFlags & FC_STATUS_FLY))
 400 
        else if(((AbsoluteFlyingAltitude) && (NaviData.Altimeter_5cm / 20 >= AbsoluteFlyingAltitude)) && (FC.StatusFlags & FC_STATUS_FLY))
401 
        {
 401 
        {
402 
                LED_RED_ON;
 402 
                LED_RED_ON;
403 
                sprintf(ErrorMSG,"ERR:Max Altitude ");
 403 
                sprintf(ErrorMSG,"ERR:Max Altitude ");
404 
                newErrorCode = 29;
 404 
                newErrorCode = 29;
405 
                DebugOut.StatusRed |= AMPEL_NC;
 405 
                DebugOut.StatusRed |= AMPEL_NC;
406 
        }
 406 
        }
407 
        else if(Parameter.GlobalConfig3 & CFG3_NO_GPSFIX_NO_START && !(NCFlags & NC_FLAG_GPS_OK) && ((FC.StatusFlags & (FC_STATUS_START | FC_STATUS_MOTOR_RUN)) || (FC.StickGas < -50 && FC.StickYaw < -50)))
 407 
        else if(Parameter.GlobalConfig3 & CFG3_NO_GPSFIX_NO_START && !(NCFlags & NC_FLAG_GPS_OK) && ((FC.StatusFlags & (FC_STATUS_START | FC_STATUS_MOTOR_RUN)) || (FC.StickGas < -50 && FC.StickYaw < -50)))
408 
        {
 408 
        {
409 
                LED_RED_ON;
 409 
                LED_RED_ON;
410 
                sprintf(ErrorMSG,"No GPS Fix      ");
 410 
                sprintf(ErrorMSG,"No GPS Fix      ");
411 
                newErrorCode = 30;
 411 
                newErrorCode = 30;
412 
        }
 412 
        }
413 
        else if((FreqNewGpsData <= 35 || FreqNewGpsData > 60) && TimeSinceMotorStart > 15)
 413 
        else if((FreqNewGpsData <= 35 || FreqNewGpsData > 60) && TimeSinceMotorStart > 15)
414 
        {
 414 
        {
415 
                LED_RED_ON;
 415 
                LED_RED_ON;
416 
                sprintf(ErrorMSG,"GPS Update rate ");
 416 
                sprintf(ErrorMSG,"GPS Update rate ");
417 
                newErrorCode = 38;
 417 
                newErrorCode = 38;
418 
        }
 418 
        }
419 
        else if(NC_GPS_ModeCharacter == 'F')
 419 
        else if(NC_GPS_ModeCharacter == 'F')
420 
        {                                                                
 420 
        {                                                                
421 
                sprintf(ErrorMSG,"FAILSAFE pos.!  ");
 421 
                sprintf(ErrorMSG,"FAILSAFE pos.!  ");
422 
                newErrorCode = 35;
 422 
                newErrorCode = 35;
423 
        }
 423 
        }
424 
        else if(FC.StatusFlags3 & FC_STATUS3_REDUNDANCE_ERROR)
 424 
        else if(FC.StatusFlags3 & FC_STATUS3_REDUNDANCE_ERROR)
425 
        {                                                                
 425 
        {                                                                
426 
                sprintf(ErrorMSG,"ERR:Redundancy  ");
 426 
                sprintf(ErrorMSG,"ERR:Redundancy  ");
427 
                newErrorCode = 36;
 427 
                newErrorCode = 36;
428 
        }
 428 
        }
429 
        else if(FC.StatusFlags3 & FC_STATUS3_REDUNDANCE_TEST)
 429 
        else if(FC.StatusFlags3 & FC_STATUS3_REDUNDANCE_TEST)
430 
        {                                                                
 430 
        {                                                                
431 
                sprintf(ErrorMSG,"Redundancy test ");
 431 
                sprintf(ErrorMSG,"Redundancy test ");
432 
                newErrorCode = 37;
 432 
                newErrorCode = 37;
433 
        }
 433 
        }
434 
        else if(CanbusTimeOut == 1)
 434 
        else if(CanbusTimeOut == 1)
435 
        {                                                                
 435 
        {                                                                
436 
                sprintf(ErrorMSG,"ERR: Canbus");
 436 
                sprintf(ErrorMSG,"ERR: Canbus");
437 
CanbusInit();
 437 
CanbusInit();
438 
                newErrorCode = 39;
 438 
                newErrorCode = 39;
439 
        }
 439 
        }
440 
        else
 440 
        else
441 
        if(FC.Error[1] & FC_ERROR1_ACC_NOT_CAL)
 441 
        if(FC.Error[1] & FC_ERROR1_ACC_NOT_CAL)
442 
        {
 442 
        {
443 
         LED_RED_ON;
 443 
         LED_RED_ON;
444 
         sprintf(ErrorMSG,"ACC not calib.");
 444 
         sprintf(ErrorMSG,"ACC not calib.");
445 
         newErrorCode = 42;
 445 
         newErrorCode = 42;
446 
        }
 446 
        }
447 
        else // no error occured
 447 
        else // no error occured
448 
        {
 448 
        {
449 
                StopNavigation = 0;
 449 
                StopNavigation = 0;
450 
                LED_RED_OFF;
 450 
                LED_RED_OFF;
451 
                if(no_error_delay) { no_error_delay--;  }
 451 
                if(no_error_delay) { no_error_delay--;  }
452 
                else
 452 
                else
453 
                {
 453 
                {
454 
                        sprintf(ErrorMSG,"No Error            ");
 454 
                        sprintf(ErrorMSG,"No Error            ");
455 
                        ErrorCode = 0;
 455 
                        ErrorCode = 0;
456 
                }
 456 
                }
457 
        }
 457 
        }
458 
 
 458 
 
459 
    if(newErrorCode)
 459 
    if(newErrorCode)
460 
         {
 460 
         {
461 
          if(FC.StatusFlags & FC_STATUS_MOTOR_RUN) no_error_delay = 8; // delay the errors if the motors are running
 461 
          if(FC.StatusFlags & FC_STATUS_MOTOR_RUN) no_error_delay = 8; // delay the errors if the motors are running
462 
          ErrorCode = newErrorCode;
 462 
          ErrorCode = newErrorCode;
463 
         }
 463 
         }
464 
 FC.Error[0] = 0;
 464 
 FC.Error[0] = 0;
465 
 FC.Error[1] = 0;
 465 
 FC.Error[1] = 0;
466 
 FC.Error[2] = 0;
 466 
 FC.Error[2] = 0;
467 
 FC.Error[3] = 0;
 467 
 FC.Error[3] = 0;
468 
 FC.Error[4] = 0;
 468 
 FC.Error[4] = 0;
469 
 ErrorGpsFixLost = 0;
 469 
 ErrorGpsFixLost = 0;
470 
 ErrorCheck_BL_MinOfMaxPWM = BL_MinOfMaxPWM;
 470 
 ErrorCheck_BL_MinOfMaxPWM = BL_MinOfMaxPWM;
471 
}
 471 
}
472 
 
 472 
 
473 
unsigned char RAM_Checksum(unsigned char* pBuffer, u16 len)
 473 
unsigned char RAM_Checksum(unsigned char* pBuffer, u16 len)
474 
{
 474 
{
475 
        u8 crc = 0xAA;
 475 
        u8 crc = 0xAA;
476 
        u16 i;
 476 
        u16 i;
477 
        for(i=0; i<len; i++) crc += pBuffer[i];
 477 
        for(i=0; i<len; i++) crc += pBuffer[i];
478 
        return crc;
 478 
        return crc;
479 
}
 479 
}
480 
 
 480 
 
481 
 
 481 
 
482 
u8 Polling(void)
 482 
u8 Polling(void)
483 
{
 483 
{
484 
        static u8 running = 0, oldFcFlags = 0, count5sec, TimeoutGPS_Process;
 484 
        static u8 running = 0, oldFcFlags = 0, count5sec, TimeoutGPS_Process;
485 
        static u32 old_ms = 0;
 485 
        static u32 old_ms = 0;
486 
 
 486 
 
487 
        if(running) {/*DebugOut.Analog[]++;*/ return(1);};
 487 
        if(running) {/*DebugOut.Analog[]++;*/ return(1);};
488 
        running = 1;
 488 
        running = 1;
489 
 
 489 
 
490 
        if(CountMilliseconds != old_ms)  // 1 ms
 490 
        if(CountMilliseconds != old_ms)  // 1 ms
491 
        {
 491 
        {
492 
                if(CanbusTimeOut >= 2) CanbusTimeOut--;
 492 
                if(CanbusTimeOut >= 2) CanbusTimeOut--;
493 
                if(CamCtrlTimeout)
 493 
                if(CamCtrlTimeout)
494 
                 {
 494 
                 {
495 
                  if(--CamCtrlTimeout == 1) CamCtrlCharacter = '?';
 495 
                  if(--CamCtrlTimeout == 1) CamCtrlCharacter = '?';
496 
                  else if(CamCtrlTimeout == 10000) CamCtrlTimeout = 0; // this is used during NC startup-Time phase
 496 
                  else if(CamCtrlTimeout == 10000) CamCtrlTimeout = 0; // this is used during NC startup-Time phase
497 
                 }
 497 
                 }
498 
                else
 498 
                else
499 
                 {
 499 
                 {
500 
                  if(TrigLogging.CountExternal) CamCtrlCharacter = TrigLogging.CountExternal % 10 + '0';
 500 
                  if(TrigLogging.CountExternal) CamCtrlCharacter = TrigLogging.CountExternal % 10 + '0';
501 
                  FromCamCtrl.CamStatus = 0;
 501 
                  FromCamCtrl.CamStatus = 0;
502 
                 };
 502 
                 };
503 
                old_ms = CountMilliseconds;
 503 
                old_ms = CountMilliseconds;
504 
                Compass_Update();               // update compass communication
 504 
                Compass_Update();               // update compass communication
505 
                Analog_Update();                // get new ADC values
 505 
                Analog_Update();                // get new ADC values
506 
                CalcHeadFree();
 506 
                CalcHeadFree();
507 
                if(CamCtrlTimeout > 1) CamCtrl_GetData(3);
 507 
                if(CamCtrlTimeout > 1) CamCtrl_GetData(3);
508 
                if(UART_VersionInfo.HWMajor >= 30) ProcessCanBus();
 508 
                if(UART_VersionInfo.HWMajor >= 30) ProcessCanBus();
509 
                if(!CheckDelay(SPI0_Timeout)) TimeoutGPS_Process = 0;
 509 
                if(!CheckDelay(SPI0_Timeout)) TimeoutGPS_Process = 0;
510 
                else if(CountMilliseconds - SPI0_Timeout > 30000000L) SPI0_Timeout = CountMilliseconds; // avoid too long overflows
 510 
                else if(CountMilliseconds - SPI0_Timeout > 30000000L) SPI0_Timeout = CountMilliseconds; // avoid too long overflows
511 
                if(++TimeoutGPS_Process >= 25)
 511 
                if(++TimeoutGPS_Process >= 25)
512 
                 {
 512 
                 {
513 
                  GPS_Navigation(&GPSData, &(ToFlightCtrl.GPSStick)); // process the GPS data even if the FC is not connected
 513 
                  GPS_Navigation(&GPSData, &(ToFlightCtrl.GPSStick)); // process the GPS data even if the FC is not connected
514 
                  TimeoutGPS_Process = 0;
 514 
                  TimeoutGPS_Process = 0;
515 
                 }
 515 
                 }
516 
        }
 516 
        }
517 
 
 517 
 
518 
        SPI0_UpdateBuffer();    // also calls the GPS-functions
 518 
        SPI0_UpdateBuffer();    // also calls the GPS-functions
519 
        UART0_ProcessRxData();  // GPS process request
 519 
        UART0_ProcessRxData();  // GPS process request
520 
        UART0_TransmitTxData(); // GPS send answer
 520 
        UART0_TransmitTxData(); // GPS send answer
521 
        UART1_ProcessRxData();  // PC process request
 521 
        UART1_ProcessRxData();  // PC process request
522 
        UART1_TransmitTxData(); // PC send answer
 522 
        UART1_TransmitTxData(); // PC send answer
523 
        UART2_TransmitTxData(); // FC send answer
 523 
        UART2_TransmitTxData(); // FC send answer
524 
 
 524 
 
525 
        if(!(FC.StatusFlags & FC_STATUS_MOTOR_RUN)) TimeSinceMotorStart = 0;
 525 
        if(!(FC.StatusFlags & FC_STATUS_MOTOR_RUN)) TimeSinceMotorStart = 0;
526 
 
 526 
 
527 
        if(HugeBlockFromFC.WhoAmI == 1) // contains eeprom-Data
 527 
        if(HugeBlockFromFC.WhoAmI == 1) // contains eeprom-Data
528 
         {
 528 
         {
529 
      u8 crc1, crc2;
 529 
      u8 crc1, crc2;
530 
          crc1 = HugeBlockFromFC.Data[sizeof(EE_Parameter)-1];                                                  // contains the EE_Parameter.crc
 530 
          crc1 = HugeBlockFromFC.Data[sizeof(EE_Parameter)-1];                                                  // contains the EE_Parameter.crc
531 
          crc2 = RAM_Checksum((u8 *)(&HugeBlockFromFC.Data), sizeof(EE_Parameter)-1);   // calculates the current crc
 531 
          crc2 = RAM_Checksum((u8 *)(&HugeBlockFromFC.Data), sizeof(EE_Parameter)-1);   // calculates the current crc
532 
          if(crc1 == crc2)
 532 
          if(crc1 == crc2)
533 
           {
 533 
           {
534 
                memcpy(&EE_Parameter, (u8 *) HugeBlockFromFC.Data, sizeof(EE_Parameter));
 534 
                memcpy(&EE_Parameter, (u8 *) HugeBlockFromFC.Data, sizeof(EE_Parameter));
535 
                RequestConfigFromFC = 0;
 535 
                RequestConfigFromFC = 0;
536 
           }
 536 
           }
537 
                HugeBlockFromFC.WhoAmI = 0;
 537 
                HugeBlockFromFC.WhoAmI = 0;
538 
         }
 538 
         }
539 
 
 539 
 
540 
    if(FCCalibActive)
 540 
    if(FCCalibActive)
541 
            {
 541 
            {
542 
             count5sec = 0;
 542 
             count5sec = 0;
543 
             FreqNewGpsData = 50;
 543 
             FreqNewGpsData = 50;
544 
             CountNewGpsDataIn5Sec = 25;
 544 
             CountNewGpsDataIn5Sec = 25;
545 
             CountGpsProcessedIn5Sec = 0;
 545 
             CountGpsProcessedIn5Sec = 0;
546 
             TimerCheckError = SetDelay(1000);
 546 
             TimerCheckError = SetDelay(1000);
547 
            }
 547 
            }
548 
 
 548 
 
549 
        // ---------------- Error Check Timing ----------------------------
 549 
        // ---------------- Error Check Timing ----------------------------
550 
        if(CheckDelay(TimerCheckError) || (FC.StatusFlags & FC_STATUS_START && !(oldFcFlags & FC_STATUS_START))) // Timer or FY wants to start
 550 
        if(CheckDelay(TimerCheckError) || (FC.StatusFlags & FC_STATUS_START && !(oldFcFlags & FC_STATUS_START))) // Timer or FY wants to start
551 
        {
 551 
        {
552 
                if(CheckDelay(TimerCheckError))
 552 
                if(CheckDelay(TimerCheckError))
553 
                {
 553 
                {
554 
                        if(!(FC.StatusFlags & FC_STATUS_FLY)) // do not change the Bit during flight
 554 
                        if(!(FC.StatusFlags & FC_STATUS_FLY)) // do not change the Bit during flight
555 
                         {
 555 
                         {
556 
                          if(!BLITZ_CONNECTED) BlitzSchuhConnected = 1;
 556 
                          if(!BLITZ_CONNECTED) BlitzSchuhConnected = 1;
557 
                          else BlitzSchuhConnected = 0;
 557 
                          else BlitzSchuhConnected = 0;
558 
                         }
 558 
                         }
559 
                        TimerCheckError = SetDelay(1000);
 559 
                        TimerCheckError = SetDelay(1000);
560 
                        if(CompassValueErrorCount) CompassValueErrorCount--;
 560 
                        if(CompassValueErrorCount) CompassValueErrorCount--;
561 
                        if(++count5sec == 5)
 561 
                        if(++count5sec == 5)
562 
                        {
 562 
                        {
563 
                                FreqGpsNavProcessed = CountGpsProcessedIn5Sec * 2; //400 = 40Hz
 563 
                                FreqGpsNavProcessed = CountGpsProcessedIn5Sec * 2; //400 = 40Hz
564 
                                FreqNewGpsData = CountNewGpsDataIn5Sec;                    // 50 = 5,0Hz
 564 
                                FreqNewGpsData = CountNewGpsDataIn5Sec;                    // 50 = 5,0Hz
565 
 
 565 
 
566 
                                count5sec = 0;
 566 
                                count5sec = 0;
567 
                                CountGpsProcessedIn5Sec = 0;
 567 
                                CountGpsProcessedIn5Sec = 0;
568 
                                CountNewGpsDataIn5Sec = FreqNewGpsData / 2;
 568 
                                CountNewGpsDataIn5Sec = FreqNewGpsData / 2;
569 
                                ConfirmGpsUpdateRate(FreqNewGpsData);
 569 
                                ConfirmGpsUpdateRate(FreqNewGpsData);
570 
                        }
 570 
                        }
571 
                }
 571 
                }
572 
                oldFcFlags = FC.StatusFlags;
 572 
                oldFcFlags = FC.StatusFlags;
573 
//              if(CheckDelay(SPI0_Timeout) && (DebugUART == UART1)) GPS_Navigation(&GPSData, &(ToFlightCtrl.GPSStick)); // process the GPS data even if the FC is not connected
 573 
//              if(CheckDelay(SPI0_Timeout) && (DebugUART == UART1)) GPS_Navigation(&GPSData, &(ToFlightCtrl.GPSStick)); // process the GPS data even if the FC is not connected
574 
 
 574 
 
575 
                if(!CheckDelay(SPI0_Timeout) || (DebugUART == UART1)) CheckErrors();
 575 
                if(!CheckDelay(SPI0_Timeout) || (DebugUART == UART1)) CheckErrors();
576 
 
 576 
 
577 
                if(FC.StatusFlags & FC_STATUS_FLY)
 577 
                if(FC.StatusFlags & FC_STATUS_FLY)
578 
                 {
 578 
                 {
579 
                  NaviData.FlyingTime++; // we want to count the battery-time
 579 
                  NaviData.FlyingTime++; // we want to count the battery-time
580 
                  TimeSinceMotorStart++;
 580 
                  TimeSinceMotorStart++;
581 
                 }
 581 
                 }
582 
 
 582 
 
583 
            if(EE_Parameter.Revision == 0) RequestConfigFromFC = 1;
 583 
            if(EE_Parameter.Revision == 0) RequestConfigFromFC = 1;
584 
                if(SerialLinkOkay) SerialLinkOkay--;
 584 
                if(SerialLinkOkay) SerialLinkOkay--;
585 
                if(SerialLinkOkay < 250 - 6) NCFlags |= NC_FLAG_NOSERIALLINK; // 6 seconds timeout for serial communication
 585 
                if(SerialLinkOkay < 250 - 6) NCFlags |= NC_FLAG_NOSERIALLINK; // 6 seconds timeout for serial communication
586 
                else NCFlags &= ~NC_FLAG_NOSERIALLINK;
 586 
                else NCFlags &= ~NC_FLAG_NOSERIALLINK;
587 
                if(StopNavigation && (Parameter.NaviGpsModeControl >=  50) && (Parameter.GlobalConfig & FC_CFG_GPS_AKTIV)) BeepTime = 1000;
 587 
                if(StopNavigation && (Parameter.NaviGpsModeControl >=  50) && (Parameter.GlobalConfig & FC_CFG_GPS_AKTIV)) BeepTime = 1000;
588 
        }
 588 
        }
589 
        running = 0;
 589 
        running = 0;
590 
//      if(!PollingTimeout) DebugOut.Analog[17]++;
 590 
//      if(!PollingTimeout) DebugOut.Analog[17]++;
591 
        return(0);
 591 
        return(0);
592 
}
 592 
}
593 
 
 593 
 
594 
// the handler will be cyclic called by the timer 1 ISR
 594 
// the handler will be cyclic called by the timer 1 ISR
595 
// used is for critical timing parts that normaly would handled
 595 
// used is for critical timing parts that normaly would handled
596 
// within the main loop that could block longer at logging activities
 596 
// within the main loop that could block longer at logging activities
597 
void EXTIT3_IRQHandler(void)  // 1ms - Takt
 597 
void EXTIT3_IRQHandler(void)  // 1ms - Takt
598 
{
 598 
{
599 
        IENABLE;
 599 
        IENABLE;
600 
        VIC_ITCmd(EXTIT3_ITLine,DISABLE); // disable irq
 600 
        VIC_ITCmd(EXTIT3_ITLine,DISABLE); // disable irq
601 
 
 601 
 
602 
        if(PollingTimeout == 0)
 602 
        if(PollingTimeout == 0)
603 
        {
 603 
        {
604 
                PollingTimeout = 5;
 604 
                PollingTimeout = 5;
605 
                //if(Polling() == 0) DebugOut.Analog[]++;
 605 
                //if(Polling() == 0) DebugOut.Analog[]++;
606 
                Polling();
 606 
                Polling();
607 
        }
 607 
        }
608 
 
 608 
 
609 
        VIC_SWITCmd(EXTIT3_ITLine,DISABLE); // clear pending bit
 609 
        VIC_SWITCmd(EXTIT3_ITLine,DISABLE); // clear pending bit
610 
        VIC_ITCmd(EXTIT3_ITLine, ENABLE); // enable irq
 610 
        VIC_ITCmd(EXTIT3_ITLine, ENABLE); // enable irq
611 
        IDISABLE;
 611 
        IDISABLE;
612 
        VIC1->VAR = 0xFF; // write any value to VIC0 Vector address register
 612 
        VIC1->VAR = 0xFF; // write any value to VIC0 Vector address register
613 
}
 613 
}
614 
 
 614 
 
615 
//----------------------------------------------------------------------------------------------------
 615 
//----------------------------------------------------------------------------------------------------
616 
int main(void)
 616 
int main(void)
617 
{
 617 
{
618 
 
 618 
 
619 
//      static u32 ftimer =0;
 619 
//      static u32 ftimer =0;
620 
//      static u8 fstate = 0;
 620 
//      static u8 fstate = 0;
621 
//      static File_t* f = NULL;
 621 
//      static File_t* f = NULL;
622 
 
 622 
 
623 
 
 623 
 
624 
        /* Configure the system clocks */
 624 
        /* Configure the system clocks */
625 
        SCU_Config();
 625 
        SCU_Config();
626 
        /* init VIC (Vectored Interrupt Controller)     */
 626 
        /* init VIC (Vectored Interrupt Controller)     */
627 
        SCU_AHBPeriphClockConfig(__VIC,ENABLE); // enable AHB bus clock for VIC
 627 
        SCU_AHBPeriphClockConfig(__VIC,ENABLE); // enable AHB bus clock for VIC
628 
        SCU_AHBPeriphReset(__VIC, DISABLE);             // disable reset state for VIC
 628 
        SCU_AHBPeriphReset(__VIC, DISABLE);             // disable reset state for VIC
629 
        VIC_DeInit();                                                   // deinitializes the VIC module registers to their default reset values.
 629 
        VIC_DeInit();                                                   // deinitializes the VIC module registers to their default reset values.
630 
        VIC_InitDefaultVectors();
 630 
        VIC_InitDefaultVectors();
631 
 
 631 
 
632 
        // initialize timer 1 for System Clock and delay rountines
 632 
        // initialize timer 1 for System Clock and delay rountines
633 
        TIMER1_Init();
 633 
        TIMER1_Init();
634 
        // initialize the LEDs (needs Timer 1)
 634 
        // initialize the LEDs (needs Timer 1)
635 
        Led_Init();
 635 
        Led_Init();
636 
        // initialize the debug UART1
 636 
        // initialize the debug UART1
637 
        UART1_Init();
 637 
        UART1_Init();
638 
        UART1_PutString("\r\n---------------------------------------------");
 638 
        UART1_PutString("\r\n---------------------------------------------");
639 
        // initialize timer 2 for servo outputs
 639 
        // initialize timer 2 for servo outputs
640 
        //TIMER2_Init();
 640 
        //TIMER2_Init();
641 
        // initialize UART2 to FLIGHTCTRL
 641 
        // initialize UART2 to FLIGHTCTRL
642 
        UART2_Init();
 642 
        UART2_Init();
643 
        // initialize UART0 (to MKGPS or MK3MAG)
 643 
        // initialize UART0 (to MKGPS or MK3MAG)
644 
        UART0_Init();
 644 
        UART0_Init();
645 
        // initialize adc
 645 
        // initialize adc
646 
        Analog_Init();
 646 
        Analog_Init();
647 
        // initialize SPI0 to FC
 647 
        // initialize SPI0 to FC
648 
        SPI0_Init();
 648 
        SPI0_Init();
649 
        // initialize i2c busses (needs Timer 1)
 649 
        // initialize i2c busses (needs Timer 1)
650 
        InitCamCtrl();
 650 
        InitCamCtrl();
651 
        I2CBus_Init(I2C0);
 651 
        I2CBus_Init(I2C0);
652 
        I2CBus_Init(I2C1);
 652 
        I2CBus_Init(I2C1);
653 
        // initialize fat16 partition on sd card (needs Timer 1)
 653 
        // initialize fat16 partition on sd card (needs Timer 1)
654 
        Fat16_Init();
 654 
        Fat16_Init();
655 
        // initialize NC params
 655 
        // initialize NC params
656 
        NCParams_Init();
 656 
        NCParams_Init();
657 
        // initialize the settings
 657 
        // initialize the settings
658 
        Settings_Init();
 658 
        Settings_Init();
659 
        // initialize logging (needs settings)
 659 
        // initialize logging (needs settings)
660 
        Logging_Init();
 660 
        Logging_Init();
661 
 
 661 
 
662 
//UART_VersionInfo.HWMajor = 30;
 662 
//UART_VersionInfo.HWMajor = 30;
663 
        LED_GRN_ON;
 663 
        LED_GRN_ON;
664 
        TimerCheckError = SetDelay(3000);
 664 
        TimerCheckError = SetDelay(3000);
665 
        UART1_PutString("\r\n++++++++++++++++++++++++++++++++++++++++++");
 665 
        UART1_PutString("\r\n++++++++++++++++++++++++++++++++++++++++++");
666 
        UART1_PutString("\n\r Version information:");
 666 
        UART1_PutString("\n\r Version information:");
667 
 
 667 
 
668 
        GetNaviCtrlVersion();
 668 
        GetNaviCtrlVersion();
669 
        DebugOut.StatusGreen = AMPEL_NC | AMPEL_COMPASS; // NC and MK3Mag
 669 
        DebugOut.StatusGreen = AMPEL_NC | AMPEL_COMPASS; // NC and MK3Mag
670 
        DebugOut.StatusRed = 0x00;
 670 
        DebugOut.StatusRed = 0x00;
671 
        UART1_PutString("\r\n++++++++++++++++++++++++++++++++++++++++++");
 671 
        UART1_PutString("\r\n++++++++++++++++++++++++++++++++++++++++++");
672 
        Compass_Init();
 672 
        Compass_Init();
673 
#ifdef FOLLOW_ME
 673 
#ifdef FOLLOW_ME
674 
        TransmitAlsoToFC = 1;
 674 
        TransmitAlsoToFC = 1;
675 
        UART1_PutString("\r\n++++++++++++++++++++++++++++++++++++++++++");
 675 
        UART1_PutString("\r\n++++++++++++++++++++++++++++++++++++++++++");
676 
        UART1_PutString("\n\r FOLLOW-ME Transmitter only!");
 676 
        UART1_PutString("\n\r FOLLOW-ME Transmitter only!");
677 
        UART1_PutString("\r\n++++++++++++++++++++++++++++++++++++++++++\r\n\r\n");
 677 
        UART1_PutString("\r\n++++++++++++++++++++++++++++++++++++++++++\r\n\r\n");
678 
        TransmitAlsoToFC = 0;
 678 
        TransmitAlsoToFC = 0;
679 
#else
 679 
#else
680 
        if(UBX_Setup() == 0) UBX_Setup(); // inits the GPS-Module via ubx -> try twice
 680 
        if(UBX_Setup() == 0) UBX_Setup(); // inits the GPS-Module via ubx -> try twice
681 
 
 681 
 
682 
        SPI0_GetFlightCtrlVersion();
 682 
        SPI0_GetFlightCtrlVersion();
683 
        if(FC_Version.Compatible != FC_SPI_COMPATIBLE)
 683 
        if(FC_Version.Compatible != FC_SPI_COMPATIBLE)
684 
        {
 684 
        {
685 
                UART1_PutString("\n\r Flight-Ctrl not compatible\n\r");
 685 
                UART1_PutString("\n\r Flight-Ctrl not compatible\n\r");
686 
                LED_RED_ON;
 686 
                LED_RED_ON;
687 
        }
 687 
        }
688 
#endif
 688 
#endif
689 
if(IamMaster == SLAVE)  UART1_PutString(" SLAVE\r\n");
 689 
if(IamMaster == SLAVE)  UART1_PutString(" SLAVE\r\n");
690 
if(IamMaster == MASTER) UART1_PutString(" MASTER\r\n");
 690 
if(IamMaster == MASTER) UART1_PutString(" MASTER\r\n");
691 
 
 691 
 
692 
// +++++++++++++++++++++++++++++++++++++++
 692 
// +++++++++++++++++++++++++++++++++++++++
693 
// ++ check CamCtrl version (if connected)
 693 
// ++ check CamCtrl version (if connected)
694 
        if(Compass_I2CPort == NCMAG_PORT_INTERN)
 694 
        if(Compass_I2CPort == NCMAG_PORT_INTERN)
695 
         {
 695 
         {
696 
          if(I2CBus_LockBuffer(I2C0,3)) I2CBus_Transmission(I2C0, CAM_SLAVE_ADDRESS, &ToCamCtrl, 4, &CamCtrl_UpdateData, sizeof(FromCamCtrl));
 696 
          if(I2CBus_LockBuffer(I2C0,3)) I2CBus_Transmission(I2C0, CAM_SLAVE_ADDRESS, &ToCamCtrl, 4, &CamCtrl_UpdateData, sizeof(FromCamCtrl));
697 
         }
 697 
         }
698 
        else CamCtrlTimeout = 0; // disable CamCtrl communication if external compass is connected
 698 
        else CamCtrlTimeout = 0; // disable CamCtrl communication if external compass is connected
699 
// +++++++++++++++++++++++++++++++++++++++
 699 
// +++++++++++++++++++++++++++++++++++++++
700 
        GPS_Init();
 700 
        GPS_Init();
701 
        // ---------- Prepare the isr driven
 701 
        // ---------- Prepare the isr driven
702 
        // set to absolute lowest priority
 702 
        // set to absolute lowest priority
703 
    VIC_Config(EXTIT3_ITLine, VIC_IRQ, PRIORITY_SW);
 703 
    VIC_Config(EXTIT3_ITLine, VIC_IRQ, PRIORITY_SW);
704 
        // enable interrupts
 704 
        // enable interrupts
705 
    VIC_ITCmd(EXTIT3_ITLine, ENABLE);
 705 
    VIC_ITCmd(EXTIT3_ITLine, ENABLE);
706 
 
 706 
 
707 
        Debug_OK("START");
 707 
        Debug_OK("START");
708 
        UART1_PutString("\r\n");
 708 
        UART1_PutString("\r\n");
709 
        fifo_purge(&UART1_rx_fifo); // flush the whole fifo init buffer
 709 
        fifo_purge(&UART1_rx_fifo); // flush the whole fifo init buffer
710 
        LED_GRN_ON;
 710 
        LED_GRN_ON;
711 
        LED_RED_OFF;
 711 
        LED_RED_OFF;
712 
    Settings_GetParamValue(PID_SEND_NMEA,(u16 *) &NMEA_Interval);
 712 
    Settings_GetParamValue(PID_SEND_NMEA,(u16 *) &NMEA_Interval);
713 
        UART1_PutString("\r\n");
 713 
        UART1_PutString("\r\n");
714 
 
 714 
 
715 
        CompassValueErrorCount = 0;
 715 
        CompassValueErrorCount = 0;
716 
        I2CBus(Compass_I2CPort)->Timeout = SetDelay(3000);
 716 
        I2CBus(Compass_I2CPort)->Timeout = SetDelay(3000);
717 
 
 717 
 
718 
        // Intilizes the Canbus
 718 
        // Intilizes the Canbus
719 
        if(UART_VersionInfo.HWMajor >= 30)
 719 
        if(UART_VersionInfo.HWMajor >= 30)
720 
         {
 720 
         {
721 
           EXT2_Init(); // External Output EXT2
 721 
           EXT2_Init(); // External Output EXT2
722 
           CanbusInit();
 722 
           CanbusInit();
723 
         }
 723 
         }
724 
 if(FromCamCtrl.CamStatus)
 724 
 if(FromCamCtrl.CamStatus)
725 
  {
 725 
  {
726 
   u8 msg[30];
 726 
   u8 msg[30];
727 
   sprintf(msg, " CamCtrl found V%i.%02i \r\n",1 + FromCamCtrl.Version / 100, FromCamCtrl.Version % 100);
 727 
   sprintf(msg, " CamCtrl found V%i.%02i \r\n",1 + FromCamCtrl.Version / 100, FromCamCtrl.Version % 100);
728 
   UART1_PutString(msg);
 728 
   UART1_PutString(msg);
729 
  }
 729 
  }
730 
  else if(Compass_I2CPort == NCMAG_PORT_INTERN) UART1_PutString(" No CamCtrl connected \r\n");
 730 
  else if(Compass_I2CPort == NCMAG_PORT_INTERN) UART1_PutString(" No CamCtrl connected \r\n");
731 
// ++++++++++++++++++++++++++++++++++++++++++++++
 731 
// ++++++++++++++++++++++++++++++++++++++++++++++
732 
        for (;;) // the endless main loop
 732 
        for (;;) // the endless main loop
733 
        {
 733 
        {
734 
                PollingTimeout = 5;
 734 
                PollingTimeout = 5;
735 
                Polling();
 735 
                Polling();
736 
// ++++++++++++++++++++++++++++++++++++++++++++++
 736 
// ++++++++++++++++++++++++++++++++++++++++++++++
737 
                if(FromFC_LoadWP_List)
 737 
                if(FromFC_LoadWP_List)
738 
                {
 738 
                {
739 
                   WPL_Store.Index = (FromFC_LoadWP_List & ~0x80);
 739 
                   WPL_Store.Index = (FromFC_LoadWP_List & ~0x80);
740 
                   if(WPL_Store.Index <= ToFC_MaxWpListIndex)
 740 
                   if(WPL_Store.Index <= ToFC_MaxWpListIndex)
741 
                   {
 741 
                   {
742 
                                if(PointList_ReadFromFile(&WPL_Store) == WPL_OK)
 742 
                                if(PointList_ReadFromFile(&WPL_Store) == WPL_OK)
743 
                                {
 743 
                                {
744 
                                        if(FromFC_LoadWP_List & 0x80)// -> load relative
 744 
                                        if(FromFC_LoadWP_List & 0x80)// -> load relative
745 
                                        {
 745 
                                        {
746 
                                         u32 angle;
 746 
                                         u32 angle;
747 
                                         angle = (360 + GyroCompassCorrected/10 + Parameter.OrientationAngle * 15) % 360;
 747 
                                         angle = (360 + GyroCompassCorrected/10 + Parameter.OrientationAngle * 15) % 360;
748 
                                                if(NCFlags & NC_FLAG_FREE || NaviData.TargetPositionDeviation.Distance_dm > 7*10)
 748 
                                                if(NCFlags & NC_FLAG_FREE || NaviData.TargetPositionDeviation.Distance_dm > 7*10)
749 
                                        {  // take actual position
 749 
                                        {  // take actual position
750 
                                                if(!PointList_Move(1,&(GPSData.Position),angle)) PointList_Clear();     // try to move wp-list so that 1st entry matches the current position
750 
                                                if(!PointList_Move(1,&(GPSData.Position),angle)) PointList_Clear();     // try to move wp-list so that 1st entry matches the current position
751 
                                                }
 751 
                                                }
752 
                                        else
 752 
                                        else
753 
                                                {  // take last target position
 753 
                                                {  // take last target position
754 
                                                if(!PointList_Move(1, &(NaviData.TargetPosition),angle)) PointList_Clear();     // try to move wp-list so that 1st entry matches the current position
754 
                                                if(!PointList_Move(1, &(NaviData.TargetPosition),angle)) PointList_Clear();     // try to move wp-list so that 1st entry matches the current position
755 
                                                }
 755 
                                                }
756 
                                        }
 756 
                                        }
757 
                                        if(FC.StatusFlags & FC_STATUS_FLY) PointList_WPActive(TRUE);
 757 
                                        if(FC.StatusFlags & FC_STATUS_FLY) PointList_WPActive(TRUE);
758 
                                        GPS_pWaypoint = PointList_WPBegin(); // updates POI index
 758 
                                        GPS_pWaypoint = PointList_WPBegin(); // updates POI index
759 
                                        SpeakWaypointRached = 1;        // Speak once when the last Point is reached
 759 
                                        SpeakWaypointRached = 1;        // Speak once when the last Point is reached
760 
                                        SpeakNextWaypoint = 1;          // Speak once as soon as the Points are active
 760 
                                        SpeakNextWaypoint = 1;          // Speak once as soon as the Points are active
761 
                                        NCFlags &= ~NC_FLAG_TARGET_REACHED;
 761 
                                        NCFlags &= ~NC_FLAG_TARGET_REACHED;
762 
                                        BeepTime = 150;
 762 
                                        BeepTime = 150;
763 
                                }
 763 
                                }
764 
                        }
 764 
                        }
765 
                        FromFC_LoadWP_List = 0;
 765 
                        FromFC_LoadWP_List = 0;
766 
                }
 766 
                }
767 
// ++++++++++++++++++++++++++++++++++++++++++++++
 767 
// ++++++++++++++++++++++++++++++++++++++++++++++
768 
                if(FromFC_Save_SinglePoint)
 768 
                if(FromFC_Save_SinglePoint)
769 
                {
 769 
                {
770 
                        WPL_Store.Index = FromFC_Save_SinglePoint;
 770 
                        WPL_Store.Index = FromFC_Save_SinglePoint;
771 
                        if(WPL_Store.Index <= ToFC_MaxWpListIndex) if(PointList_SaveSinglePoint(&WPL_Store) == WPL_OK) BeepTime = 150;
 771 
                        if(WPL_Store.Index <= ToFC_MaxWpListIndex) if(PointList_SaveSinglePoint(&WPL_Store) == WPL_OK) BeepTime = 150;
772 
                        FromFC_Save_SinglePoint = 0;
 772 
                        FromFC_Save_SinglePoint = 0;
773 
                }
 773 
                }
774 
// ++++++++++++++++++++++++++++++++++++++++++++++
 774 
// ++++++++++++++++++++++++++++++++++++++++++++++
775 
                if(FromFC_Load_SinglePoint)
 775 
                if(FromFC_Load_SinglePoint)
776 
                {
 776 
                {
777 
                   WPL_Store.Index = FromFC_Load_SinglePoint;
 777 
                   WPL_Store.Index = FromFC_Load_SinglePoint;
778 
                   if(WPL_Store.Index <= ToFC_MaxWpListIndex)
 778 
                   if(WPL_Store.Index <= ToFC_MaxWpListIndex)
779 
                   {
 779 
                   {
780 
                        if(PointList_LoadSinglePoint(&WPL_Store) == WPL_OK)
 780 
                        if(PointList_LoadSinglePoint(&WPL_Store) == WPL_OK)
781 
                        {
 781 
                        {
782 
                                if(FC.StatusFlags & FC_STATUS_FLY) PointList_WPActive(TRUE);
 782 
                                if(FC.StatusFlags & FC_STATUS_FLY) PointList_WPActive(TRUE);
783 
                                GPS_pWaypoint = PointList_WPBegin(); // updates POI index
 783 
                                GPS_pWaypoint = PointList_WPBegin(); // updates POI index
784 
                                SpeakWaypointRached = 1;        // Speak once when the last Point is reached
 784 
                                SpeakWaypointRached = 1;        // Speak once when the last Point is reached
785 
                                SpeakNextWaypoint = 1;          // Speak once as soon as the Points are active
 785 
                                SpeakNextWaypoint = 1;          // Speak once as soon as the Points are active
786 
                                NCFlags &= ~NC_FLAG_TARGET_REACHED;
 786 
                                NCFlags &= ~NC_FLAG_TARGET_REACHED;
787 
                                BeepTime = 150;
 787 
                                BeepTime = 150;
788 
                        }
 788 
                        }
789 
                   }
 789 
                   }
790 
                        FromFC_Load_SinglePoint = 0;
 790 
                        FromFC_Load_SinglePoint = 0;
791 
                }
 791 
                }
792 
// ++++++++++++++++++++++++++++++++++++++++++++++
 792 
// ++++++++++++++++++++++++++++++++++++++++++++++
793 
                // ---------------- Logging  ---------------------------------------
 793 
                // ---------------- Logging  ---------------------------------------
794 
                if(SD_WatchDog)
 794 
                if(SD_WatchDog)
795 
                {
 795 
                {
796 
                        SD_WatchDog = 30000;
 796 
                        SD_WatchDog = 30000;
797 
                        if(SDCardInfo.Valid == 1) Logging_Update();  // could be block some time for at max. 2 seconds, therefore move time critical part of the mainloop into the ISR of timer 1
 797 
                        if(SDCardInfo.Valid == 1) Logging_Update();  // could be block some time for at max. 2 seconds, therefore move time critical part of the mainloop into the ISR of timer 1
798 
                        else
 798 
                        else
799 
                        {
 799 
                        {
800 
                         ToFC_MaxWpListIndex = 0;
 800 
                         ToFC_MaxWpListIndex = 0;
801 
                         if(FC.StatusFlags & FC_STATUS_START) SD_LoggingError = 100;
 801 
                         if(FC.StatusFlags & FC_STATUS_START) SD_LoggingError = 100;
802 
                        }
 802 
                        }
803 
                        if(!SD_WatchDog) UART1_PutString("\n\rSD-Watchdog - Logging aborted\n\r");
 803 
                        if(!SD_WatchDog) UART1_PutString("\n\rSD-Watchdog - Logging aborted\n\r");
804 
                }
 804 
                }
805 
 
 805 
 
806 
/*
 806 
/*
807 
                if(CheckDelay(ftimer))
 807 
                if(CheckDelay(ftimer))
808 
                {
 808 
                {
809 
 
 809 
 
810 
                        static s8 filename[35];
 810 
                        static s8 filename[35];
811 
                        static u8 i = 0;
 811 
                        static u8 i = 0;
812 
                        s8 dbgmsg[40];
 812 
                        s8 dbgmsg[40];
813 
 
 813 
 
814 
 
 814 
 
815 
 
 815 
 
816 
                        ftimer = SetDelay(100);
 816 
                        ftimer = SetDelay(100);
817 
                        if(FC.Poti[3]>100 && fstate == 0)
 817 
                        if(FC.Poti[3]>100 && fstate == 0)
818 
                        {
 818 
                        {
819 
                                fstate = 1;
 819 
                                fstate = 1;
820 
                                sprintf(filename, "/toast/toasta/toast%02i.txt",i++);
 820 
                                sprintf(filename, "/toast/toasta/toast%02i.txt",i++);
821 
                        }
 821 
                        }
822 
                        else if(FC.Poti[3]<100 && fstate == 2)
 822 
                        else if(FC.Poti[3]<100 && fstate == 2)
823 
                        {
 823 
                        {
824 
                                fstate = 3;
 824 
                                fstate = 3;
825 
                        }
 825 
                        }
826 
 
 826 
 
827 
                        switch(fstate)
 827 
                        switch(fstate)
828 
                        {
 828 
                        {
829 
                                case 1:
 829 
                                case 1:
830 
                                        sprintf(dbgmsg,"\r\nStart writing file: %s", filename);
 830 
                                        sprintf(dbgmsg,"\r\nStart writing file: %s", filename);
831 
                                        Debug(dbgmsg);
 831 
                                        Debug(dbgmsg);
832 
                                        f = fopen_(filename, 'a');
 832 
                                        f = fopen_(filename, 'a');
833 
                                        if(f== NULL) Fat16_Init();
 833 
                                        if(f== NULL) Fat16_Init();
834 
                                        fstate = 2;
 834 
                                        fstate = 2;
835 
                                        break;
 835 
                                        break;
836 
 
 836 
 
837 
                                case 2:
 837 
                                case 2:
838 
                                        fputs_("\r\ntest edins sdv dsivbds iv dsivb disbv idsv bisd bv d suiv dsibsivbdis fbvisdöb visdbvisdb vidbfibds ibv", f);
 838 
                                        fputs_("\r\ntest edins sdv dsivbds iv dsivb disbv idsv bisd bv d suiv dsibsivbdis fbvisdöb visdbvisdb vidbfibds ibv", f);
839 
                                        break;
 839 
                                        break;
840 
 
 840 
 
841 
                                case 3:
 841 
                                case 3:
842 
                                        sprintf(dbgmsg,"\r\nClosing file: %s", filename);
 842 
                                        sprintf(dbgmsg,"\r\nClosing file: %s", filename);
843 
                                        Debug(dbgmsg);
 843 
                                        Debug(dbgmsg);
844 
                                        fclose_(f);
 844 
                                        fclose_(f);
845 
                                        fstate = 0;
 845 
                                        fstate = 0;
846 
                                        break;
 846 
                                        break;
847 
 
 847 
 
848 
                                default:
 848 
                                default:
849 
                                        break;
 849 
                                        break;
850 
                        }
 850 
                        }
851 
                }
 851 
                }
852 
*/                                                       
 852 
*/                                                       
853 
        }
 853 
        }
854 
}
 854 
}
855 
//DebugOut.Analog[]
 855 
//DebugOut.Analog[]
856 
 
 856