Subversion Repositories NaviCtrl

Rev

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

Rev 715 Rev 719
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 
 
 87 
 
88 
 
 88 
 
89 
#ifdef FOLLOW_ME
 89 
#ifdef FOLLOW_ME
90 
u8 TransmitAlsoToFC = 0;
 90 
u8 TransmitAlsoToFC = 0;
91 
#endif
 91 
#endif
92 
u32 TimerCheckError;
 92 
u32 TimerCheckError;
93 
u8 ErrorCode = 0;
 93 
u8 ErrorCode = 0;
94 
u16 BeepTime;
 94 
u16 BeepTime;
95 
u8  NCFlags = 0;
 95 
u8  NCFlags = 0;
96 
s16 GeoMagDec = 0; // local magnetic declination in 0.1 deg
 96 
s16 GeoMagDec = 0; // local magnetic declination in 0.1 deg
97 
u8 ErrorGpsFixLost = 0;
 97 
u8 ErrorGpsFixLost = 0;
98 
u8 FromFC_LoadWP_List = 0, FromFC_Load_SinglePoint = 0, FromFC_Save_SinglePoint = 0;
 98 
u8 FromFC_LoadWP_List = 0, FromFC_Load_SinglePoint = 0, FromFC_Save_SinglePoint = 0;
99 
u8 ToFC_MaxWpListIndex = 4;
 99 
u8 ToFC_MaxWpListIndex = 4;
100 
u8 ClearFCStatusFlags = 0;
 100 
u8 ClearFCStatusFlags = 0;
101 
u8 StopNavigation = 0;
 101 
u8 StopNavigation = 0;
102 
volatile u32 PollingTimeout = 10000;
 102 
volatile u32 PollingTimeout = 10000;
103 
Param_t Parameter;
 103 
Param_t Parameter;
104 
Partner_t Partner;
 104 
Partner_t Partner;
105 
volatile FC_t FC;
 105 
volatile FC_t FC;
106 
volatile u32 SPIWatchDog = 15000; // stop Navigation if this goes to zero
 106 
volatile u32 SPIWatchDog = 15000; // stop Navigation if this goes to zero
107 
volatile u32 SD_WatchDog = 15000; // stop Logging if this goes to zero
 107 
volatile u32 SD_WatchDog = 15000; // stop Logging if this goes to zero
108 
u32 CountGpsProcessedIn5Sec = 50,CountNewGpsDataIn5Sec = 25, FreqGpsNavProcessed = 0, FreqNewGpsData = 0;
 108 
u32 CountGpsProcessedIn5Sec = 50,CountNewGpsDataIn5Sec = 25, FreqGpsNavProcessed = 0, FreqNewGpsData = 0;
109 
u8 NewWPL_Name = 0;
 109 
u8 NewWPL_Name = 0;
110 
u32 MaxWP_Radius_in_m = 0;
 110 
u32 MaxWP_Radius_in_m = 0;
111 
s8 ErrorMSG[25];
 111 
s8 ErrorMSG[25];
112 
s8 PartnerErrorMSG[25] = "     ---                \0";
 112 
s8 PartnerErrorMSG[25] = "     ---                \0";
113 
u32 TimeSinceMotorStart = 0;
 113 
u32 TimeSinceMotorStart = 0;
114 
u8 IamMaster = NOTHING; // for Master/Slave Redundance
 114 
u8 IamMaster = NOTHING; // for Master/Slave Redundance
115 
 
 115 
 
116 
//----------------------------------------------------------------------------------------------------
 116 
//----------------------------------------------------------------------------------------------------
117 
void SCU_Config(void)
 117 
void SCU_Config(void)
118 
{
 118 
{
119 
        /* configure PLL and set it as master clock source */
 119 
        /* configure PLL and set it as master clock source */
120 
        SCU_MCLKSourceConfig(SCU_MCLK_OSC);             // set master clock source to external oscillator clock (25MHz) before diabling the PLL
 120 
        SCU_MCLKSourceConfig(SCU_MCLK_OSC);             // set master clock source to external oscillator clock (25MHz) before diabling the PLL
121 
        SCU_PLLCmd(DISABLE);                                    // now disable the PLL
 121 
        SCU_PLLCmd(DISABLE);                                    // now disable the PLL
122 
        #ifdef MCLK96MHZ
 122 
        #ifdef MCLK96MHZ
123 
        SCU_BRCLKDivisorConfig(SCU_BRCLK_Div2); // set BRCLK to MCLK/2 = 48MHz
 123 
        SCU_BRCLKDivisorConfig(SCU_BRCLK_Div2); // set BRCLK to MCLK/2 = 48MHz
124 
        SCU_PCLKDivisorConfig(SCU_PCLK_Div4);   // set PCLK     (APB bus clock) divisor to 4 (half Reference Clock)
 124 
        SCU_PCLKDivisorConfig(SCU_PCLK_Div4);   // set PCLK     (APB bus clock) divisor to 4 (half Reference Clock)
125 
        SCU_RCLKDivisorConfig(SCU_RCLK_Div2);   // set RCLK     (Reference Clock) divisor to 1 (full PPL clock)
 125 
        SCU_RCLKDivisorConfig(SCU_RCLK_Div2);   // set RCLK     (Reference Clock) divisor to 1 (full PPL clock)
126 
        SCU_HCLKDivisorConfig(SCU_HCLK_Div2);   // set HCLK     (AHB bus clock) divisor to 1 (full Reference Clock)
 126 
        SCU_HCLKDivisorConfig(SCU_HCLK_Div2);   // set HCLK     (AHB bus clock) divisor to 1 (full Reference Clock)
127 
        SCU_PLLFactorsConfig(192,25,2);                 // PLL = 96 MHz, Feedback Divider N=192, Pre-Divider M=25, Post-Divider P=2
 127 
        SCU_PLLFactorsConfig(192,25,2);                 // PLL = 96 MHz, Feedback Divider N=192, Pre-Divider M=25, Post-Divider P=2
128 
        #else
 128 
        #else
129 
        SCU_BRCLKDivisorConfig(SCU_BRCLK_Div1); // set BRCLK to MCLK = 48MHz
 129 
        SCU_BRCLKDivisorConfig(SCU_BRCLK_Div1); // set BRCLK to MCLK = 48MHz
130 
        SCU_PCLKDivisorConfig(SCU_PCLK_Div2);   // set PCLK     (APB bus clock) divisor to 2 (half Reference Clock)
 130 
        SCU_PCLKDivisorConfig(SCU_PCLK_Div2);   // set PCLK     (APB bus clock) divisor to 2 (half Reference Clock)
131 
        SCU_RCLKDivisorConfig(SCU_RCLK_Div1);   // set RCLK     (Reference Clock) divisor to 1 (full PPL clock)
 131 
        SCU_RCLKDivisorConfig(SCU_RCLK_Div1);   // set RCLK     (Reference Clock) divisor to 1 (full PPL clock)
132 
        SCU_HCLKDivisorConfig(SCU_HCLK_Div1);   // set HCLK     (AHB bus clock) divisor to 1 (full Reference Clock)
 132 
        SCU_HCLKDivisorConfig(SCU_HCLK_Div1);   // set HCLK     (AHB bus clock) divisor to 1 (full Reference Clock)
133 
        SCU_PLLFactorsConfig(192,25,3);                 // PLL = 48 MHz, Feedback Divider N=192, Pre-Divider M=25, Post-Divider P=3
 133 
        SCU_PLLFactorsConfig(192,25,3);                 // PLL = 48 MHz, Feedback Divider N=192, Pre-Divider M=25, Post-Divider P=3
134 
        #endif
 134 
        #endif
135 
        SCU_PLLCmd(ENABLE);                                     // Enable PLL (is disabled by SCU_PLLFactorsConfig)
 135 
        SCU_PLLCmd(ENABLE);                                     // Enable PLL (is disabled by SCU_PLLFactorsConfig)
136 
        SCU_MCLKSourceConfig(SCU_MCLK_PLL);     // set master clock source to PLL
 136 
        SCU_MCLKSourceConfig(SCU_MCLK_PLL);     // set master clock source to PLL
137 
}
 137 
}
138 
 
 138 
 
139 
//----------------------------------------------------------------------------------------------------
 139 
//----------------------------------------------------------------------------------------------------
140 
void GetNaviCtrlVersion(void)
 140 
void GetNaviCtrlVersion(void)
141 
{
 141 
{
142 
        u8 msg[25];
 142 
        u8 msg[25];
143 
 
 143 
 
144 
        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);
 144 
        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 
        UART1_PutString(msg);
 145 
        UART1_PutString(msg);
146 
}
 146 
}
147 
 
 147 
 
148 
//----------------------------------------------------------------------------------------------------
 148 
//----------------------------------------------------------------------------------------------------
149 
 
 149 
 
150 
void CheckErrors(void)
 150 
void CheckErrors(void)
151 
{
 151 
{
152 
    static s32 no_error_delay = 0;
 152 
    static s32 no_error_delay = 0;
153 
        s32 newErrorCode = 0;
 153 
        s32 newErrorCode = 0;
154 
        UART_VersionInfo.HardwareError[0] = 0;
 154 
        UART_VersionInfo.HardwareError[0] = 0;
155 
 
 155 
 
156 
        if(CheckDelay(I2CBus(Compass_I2CPort)->Timeout) || (Compass_Heading < 0)) DebugOut.StatusRed |= AMPEL_COMPASS;
 156 
        if(CheckDelay(I2CBus(Compass_I2CPort)->Timeout) || (Compass_Heading < 0)) DebugOut.StatusRed |= AMPEL_COMPASS;
157 
        else DebugOut.StatusRed &= ~AMPEL_COMPASS; // MK3Mag green status
 157 
        else DebugOut.StatusRed &= ~AMPEL_COMPASS; // MK3Mag green status
158 
 
 158 
 
159 
        if((FC.Error[1] & FC_ERROR1_I2C) || (FC.Error[1] & FC_ERROR1_BL_MISSING)) DebugOut.StatusRed |= AMPEL_BL;
 159 
        if((FC.Error[1] & FC_ERROR1_I2C) || (FC.Error[1] & FC_ERROR1_BL_MISSING)) DebugOut.StatusRed |= AMPEL_BL;
160 
        else DebugOut.StatusRed &= ~AMPEL_BL; // BL-Ctrl green status
 160 
        else DebugOut.StatusRed &= ~AMPEL_BL; // BL-Ctrl green status
161 
 
 161 
 
162 
        if(UART_VersionInfo.HardwareError[0] || UART_VersionInfo.HardwareError[1]) DebugOut.StatusRed |= AMPEL_NC;
 162 
        if(UART_VersionInfo.HardwareError[0] || UART_VersionInfo.HardwareError[1]) DebugOut.StatusRed |= AMPEL_NC;
163 
        else DebugOut.StatusRed &= ~AMPEL_NC;
 163 
        else DebugOut.StatusRed &= ~AMPEL_NC;
164 
 
 164 
 
165 
        if(CheckDelay(I2CBus(Compass_I2CPort)->Timeout))
 165 
        if(CheckDelay(I2CBus(Compass_I2CPort)->Timeout))
166 
        {
 166 
        {
167 
                LED_RED_ON;
 167 
                LED_RED_ON;
168 
                sprintf(ErrorMSG,"no compass communica");
 168 
                sprintf(ErrorMSG,"no compass communica");
169 
                //Reset Compass communication
 169 
                //Reset Compass communication
170 
                if(Compass_I2CPort == NCMAG_PORT_EXTERN) I2CBus_Init(I2C0);     else I2CBus_Init(I2C1);
 170 
                if(Compass_I2CPort == NCMAG_PORT_EXTERN) I2CBus_Init(I2C0);     else I2CBus_Init(I2C1);
171 
                Compass_Init();
 171 
                Compass_Init();
172 
                newErrorCode = 4;
 172 
                newErrorCode = 4;
173 
                StopNavigation = 1;
 173 
                StopNavigation = 1;
174 
                UART_VersionInfo.HardwareError[0] |= NC_ERROR0_COMPASS_RX;
 174 
                UART_VersionInfo.HardwareError[0] |= NC_ERROR0_COMPASS_RX;
175 
                DebugOut.StatusRed |= AMPEL_COMPASS;
 175 
                DebugOut.StatusRed |= AMPEL_COMPASS;
176 
        }
 176 
        }
177 
        else if(CompassValueErrorCount > 30)
 177 
        else if(CompassValueErrorCount > 30)
178 
        {
 178 
        {
179 
                LED_RED_ON;
 179 
                LED_RED_ON;
180 
                sprintf(ErrorMSG,"compass sensor error");
 180 
                sprintf(ErrorMSG,"compass sensor error");
181 
                newErrorCode = 34;
 181 
                newErrorCode = 34;
182 
                StopNavigation = 1;
 182 
                StopNavigation = 1;
183 
                UART_VersionInfo.HardwareError[0] |= NC_ERROR0_COMPASS_VALUE;
 183 
                UART_VersionInfo.HardwareError[0] |= NC_ERROR0_COMPASS_VALUE;
184 
                if(Compass_I2CPort == NCMAG_PORT_EXTERN) I2CBus_Init(I2C0);     else I2CBus_Init(I2C1);
 184 
                if(Compass_I2CPort == NCMAG_PORT_EXTERN) I2CBus_Init(I2C0);     else I2CBus_Init(I2C1);
185 
                Compass_Init();
 185 
                Compass_Init();
186 
        }
 186 
        }
187 
        else if((FCCalibActive || CompassCalState) && FC_Version.Hardware)
 187 
        else if((FCCalibActive || CompassCalState) && FC_Version.Hardware)
188 
        {
 188 
        {
189 
                sprintf(ErrorMSG,"Calibrate... ");
 189 
                sprintf(ErrorMSG,"Calibrate... ");
190 
                newErrorCode = 0;
 190 
                newErrorCode = 0;
191 
                ErrorCode = 0;
 191 
                ErrorCode = 0;
192 
                no_error_delay = 1;
 192 
                no_error_delay = 1;
193 
        }
 193 
        }
194 
        else if(CheckDelay(SPI0_Timeout))
 194 
        else if(CheckDelay(SPI0_Timeout))
195 
        {
 195 
        {
196 
                LED_RED_ON;
 196 
                LED_RED_ON;
197 
                sprintf(ErrorMSG,"no FC communication ");
 197 
                sprintf(ErrorMSG,"no FC communication ");
198 
                newErrorCode = 3;
 198 
                newErrorCode = 3;
199 
                StopNavigation = 1;
 199 
                StopNavigation = 1;
200 
                DebugOut.StatusGreen &= ~AMPEL_FC; // status of FC Present
 200 
                DebugOut.StatusGreen &= ~AMPEL_FC; // status of FC Present
201 
                DebugOut.StatusGreen &= ~AMPEL_BL; // status of BL Present
 201 
                DebugOut.StatusGreen &= ~AMPEL_BL; // status of BL Present
202 
                UART_VersionInfo.HardwareError[0] |= NC_ERROR0_SPI_RX;
 202 
                UART_VersionInfo.HardwareError[0] |= NC_ERROR0_SPI_RX;
203 
        }
 203 
        }
204 
        else if(FC_Version.Compatible != FC_SPI_COMPATIBLE)
 204 
        else if(FC_Version.Compatible != FC_SPI_COMPATIBLE)
205 
        {
 205 
        {
206 
                LED_RED_ON;
 206 
                LED_RED_ON;
207 
#ifndef FOLLOW_ME
 207 
#ifndef FOLLOW_ME
208 
                sprintf(ErrorMSG,"FC not compatible ");
 208 
                sprintf(ErrorMSG,"FC not compatible ");
209 
#else
 209 
#else
210 
                sprintf(ErrorMSG,"! FollowMe only ! ");
 210 
                sprintf(ErrorMSG,"! FollowMe only ! ");
211 
#endif
 211 
#endif
212 
                newErrorCode = 1;
 212 
                newErrorCode = 1;
213 
                StopNavigation = 1;
 213 
                StopNavigation = 1;
214 
                UART_VersionInfo.HardwareError[0] |= NC_ERROR0_FC_INCOMPATIBLE;
 214 
                UART_VersionInfo.HardwareError[0] |= NC_ERROR0_FC_INCOMPATIBLE;
215 
                DebugOut.StatusRed |= AMPEL_NC;
 215 
                DebugOut.StatusRed |= AMPEL_NC;
216 
        }
 216 
        }
217 
        else if(FC.Error[0] & FC_ERROR0_GYRO_NICK)
 217 
        else if(FC.Error[0] & FC_ERROR0_GYRO_NICK)
218 
        {
 218 
        {
219 
                LED_RED_ON;
 219 
                LED_RED_ON;
220 
                sprintf(ErrorMSG,"ERR: FC Nick Gyro");
 220 
                sprintf(ErrorMSG,"ERR: FC Nick Gyro");
221 
                newErrorCode = 10;
 221 
                newErrorCode = 10;
222 
        }
 222 
        }
223 
        else if(FC.Error[0] & FC_ERROR0_GYRO_ROLL)
 223 
        else if(FC.Error[0] & FC_ERROR0_GYRO_ROLL)
224 
        {
 224 
        {
225 
                LED_RED_ON;
 225 
                LED_RED_ON;
226 
                sprintf(ErrorMSG,"ERR: FC Roll Gyro");
 226 
                sprintf(ErrorMSG,"ERR: FC Roll Gyro");
227 
                newErrorCode = 11;
 227 
                newErrorCode = 11;
228 
        }
 228 
        }
229 
        else if(FC.Error[0] & FC_ERROR0_GYRO_YAW)
 229 
        else if(FC.Error[0] & FC_ERROR0_GYRO_YAW)
230 
        {
 230 
        {
231 
                LED_RED_ON;
 231 
                LED_RED_ON;
232 
                sprintf(ErrorMSG,"ERR: FC Yaw Gyro");
 232 
                sprintf(ErrorMSG,"ERR: FC Yaw Gyro");
233 
                newErrorCode = 12;
 233 
                newErrorCode = 12;
234 
        }
 234 
        }
235 
        else if(FC.Error[0] & FC_ERROR0_ACC_NICK)
 235 
        else if(FC.Error[0] & FC_ERROR0_ACC_NICK)
236 
        {
 236 
        {
237 
                LED_RED_ON;
 237 
                LED_RED_ON;
238 
                sprintf(ErrorMSG,"ERR: FC Nick ACC");
 238 
                sprintf(ErrorMSG,"ERR: FC Nick ACC");
239 
                newErrorCode = 13;
 239 
                newErrorCode = 13;
240 
        }
 240 
        }
241 
        else if(FC.Error[0] & FC_ERROR0_ACC_ROLL)
 241 
        else if(FC.Error[0] & FC_ERROR0_ACC_ROLL)
242 
        {
 242 
        {
243 
                LED_RED_ON;
 243 
                LED_RED_ON;
244 
                sprintf(ErrorMSG,"ERR: FC Roll ACC");
 244 
                sprintf(ErrorMSG,"ERR: FC Roll ACC");
245 
                newErrorCode = 14;
 245 
                newErrorCode = 14;
246 
        }
 246 
        }
247 
        else if(FC.Error[0] & FC_ERROR0_ACC_TOP)
 247 
        else if(FC.Error[0] & FC_ERROR0_ACC_TOP)
248 
        {
 248 
        {
249 
                LED_RED_ON;
 249 
                LED_RED_ON;
250 
                sprintf(ErrorMSG,"ERR:FC Z-ACC");
 250 
                sprintf(ErrorMSG,"ERR:FC Z-ACC");
251 
                newErrorCode = 15;
 251 
                newErrorCode = 15;
252 
        }
 252 
        }
253 
        else if(NC_To_FC_Flags & NC_TO_FC_FLYING_RANGE)
 253 
        else if(NC_To_FC_Flags & NC_TO_FC_FLYING_RANGE)
254 
        {
 254 
        {
255 
                LED_RED_ON;
 255 
                LED_RED_ON;
256 
                sprintf(ErrorMSG,"ERR:Flying range!");
 256 
                sprintf(ErrorMSG,"ERR:Flying range!");
257 
                newErrorCode = 28;
 257 
                newErrorCode = 28;
258 
        }
 258 
        }
259 
        else if(FC.Error[0] & FC_ERROR0_PRESSURE)
 259 
        else if(FC.Error[0] & FC_ERROR0_PRESSURE)
260 
        {
 260 
        {
261 
                LED_RED_ON;
 261 
                LED_RED_ON;
262 
                sprintf(ErrorMSG,"ERR:Pressure sensor");
 262 
                sprintf(ErrorMSG,"ERR:Pressure sensor");
263 
                newErrorCode = 16;
 263 
                newErrorCode = 16;
264 
        }
 264 
        }
265 
        else if(FC.Error[1] &  FC_ERROR1_I2C)
 265 
        else if(FC.Error[1] &  FC_ERROR1_I2C)
266 
        {
 266 
        {
267 
                LED_RED_ON;
 267 
                LED_RED_ON;
268 
                sprintf(ErrorMSG,"ERR:I2C FC to BL");
 268 
                sprintf(ErrorMSG,"ERR:I2C FC to BL");
269 
                newErrorCode = 17;
 269 
                newErrorCode = 17;
270 
        }
 270 
        }
271 
        else if(FC.Error[1] &  FC_ERROR1_BL_MISSING)
 271 
        else if(FC.Error[1] &  FC_ERROR1_BL_MISSING)
272 
        {
 272 
        {
273 
                LED_RED_ON;
 273 
                LED_RED_ON;
274 
                sprintf(ErrorMSG,"ERR: Bl Missing");
 274 
                sprintf(ErrorMSG,"ERR: Bl Missing");
275 
                newErrorCode = 18;
 275 
                newErrorCode = 18;
276 
        }
 276 
        }
277 
        else if(FC.Error[1] &  FC_ERROR1_MIXER)
 277 
        else if(FC.Error[1] &  FC_ERROR1_MIXER)
278 
        {
 278 
        {
279 
                LED_RED_ON;
 279 
                LED_RED_ON;
280 
                sprintf(ErrorMSG,"Mixer Error");
 280 
                sprintf(ErrorMSG,"Mixer Error");
281 
                newErrorCode = 19;
 281 
                newErrorCode = 19;
282 
        }
 282 
        }
283 
        else if(CheckDelay(UBX_Timeout) && Parameter.GlobalConfig & FC_CFG_GPS_AKTIV)
 283 
        else if(CheckDelay(UBX_Timeout) && Parameter.GlobalConfig & FC_CFG_GPS_AKTIV)
284 
        {
 284 
        {
285 
                LED_RED_ON;
 285 
                LED_RED_ON;
286 
                sprintf(ErrorMSG,"no GPS communication");
 286 
                sprintf(ErrorMSG,"no GPS communication");
287 
                UART_VersionInfo.HardwareError[0] |= NC_ERROR0_GPS_RX;
 287 
                UART_VersionInfo.HardwareError[0] |= NC_ERROR0_GPS_RX;
288 
                UART_VersionInfo.Flags &= ~NC_VERSION_FLAG_GPS_PRESENT;
 288 
                UART_VersionInfo.Flags &= ~NC_VERSION_FLAG_GPS_PRESENT;
289 
                newErrorCode = 5;
 289 
                newErrorCode = 5;
290 
                StopNavigation = 1;
 290 
                StopNavigation = 1;
291 
        }
 291 
        }
292 
        else if(Compass_Heading < 0 && NCMAG_Present && !NCMAG_IsCalibrated)
 292 
        else if(Compass_Heading < 0 && NCMAG_Present && !NCMAG_IsCalibrated)
293 
        {
 293 
        {
294 
                LED_RED_ON;
 294 
                LED_RED_ON;
295 
                sprintf(ErrorMSG,"compass not calibr.");
 295 
                sprintf(ErrorMSG,"compass not calibr.");
296 
                newErrorCode = 31;
 296 
                newErrorCode = 31;
297 
                StopNavigation = 1;
 297 
                StopNavigation = 1;
298 
                UART_VersionInfo.HardwareError[0] |= NC_ERROR0_COMPASS_VALUE;
 298 
                UART_VersionInfo.HardwareError[0] |= NC_ERROR0_COMPASS_VALUE;
299 
        }
 299 
        }
300 
        else if(Compass_Heading < 0)
 300 
        else if(Compass_Heading < 0)
301 
        {
 301 
        {
302 
                LED_RED_ON;
 302 
                LED_RED_ON;
303 
                sprintf(ErrorMSG,"bad compass value ");
 303 
                sprintf(ErrorMSG,"bad compass value ");
304 
                newErrorCode = 6;
 304 
                newErrorCode = 6;
305 
                StopNavigation = 1;
 305 
                StopNavigation = 1;
306 
                UART_VersionInfo.HardwareError[0] |= NC_ERROR0_COMPASS_VALUE;
 306 
                UART_VersionInfo.HardwareError[0] |= NC_ERROR0_COMPASS_VALUE;
307 
        }
 307 
        }
308 
        else if((FC.Error[1] &  FC_ERROR1_SPI_RX))
 308 
        else if((FC.Error[1] &  FC_ERROR1_SPI_RX))
309 
        {
 309 
        {
310 
                LED_RED_ON;
 310 
                LED_RED_ON;
311 
                sprintf(ErrorMSG,"FC spi rx error ");
 311 
                sprintf(ErrorMSG,"FC spi rx error ");
312 
                newErrorCode = 8;
 312 
                newErrorCode = 8;
313 
                StopNavigation = 1;
 313 
                StopNavigation = 1;
314 
        }
 314 
        }
315 
        else if(FC.Error[0] &  FC_ERROR0_CAREFREE)
 315 
        else if(FC.Error[0] &  FC_ERROR0_CAREFREE)
316 
        {
 316 
        {
317 
                LED_RED_ON;
 317 
                LED_RED_ON;
318 
                sprintf(ErrorMSG,"FC: Carefree Error");
 318 
                sprintf(ErrorMSG,"FC: Carefree Error");
319 
                newErrorCode = 20;
 319 
                newErrorCode = 20;
320 
        }
 320 
        }
321 
        else if(FC.BAT_Voltage < 45)
 321 
        else if(FC.BAT_Voltage < 45)
322 
        {
 322 
        {
323 
         LED_RED_ON;
 323 
         LED_RED_ON;
324 
         sprintf(ErrorMSG,"ERR:Power Supply");
 324 
         sprintf(ErrorMSG,"ERR:Power Supply");
325 
         newErrorCode = 41;
 325 
         newErrorCode = 41;
326 
        }
 326 
        }
327 
        else
 327 
        else
328 
        if(FC.Error[1] & FC_ERROR1_RC_VOLTAGE)
 328 
        if(FC.Error[1] & FC_ERROR1_RC_VOLTAGE)
329 
        {
 329 
        {
330 
         LED_RED_ON;
 330 
         LED_RED_ON;
331 
         sprintf(ErrorMSG,"ERR: 5V RC-Supply");
 331 
         sprintf(ErrorMSG,"ERR: 5V RC-Supply");
332 
         newErrorCode = 40;
 332 
         newErrorCode = 40;
333 
        }
 333 
        }
334 
        else if(FC.Error[1] &  FC_ERROR1_PPM)
 334 
        else if(FC.Error[1] &  FC_ERROR1_PPM)
335 
        {
 335 
        {
336 
                LED_RED_ON;
 336 
                LED_RED_ON;
337 
                sprintf(ErrorMSG,"RC Signal lost ");
 337 
                sprintf(ErrorMSG,"RC Signal lost ");
338 
                newErrorCode = 7;
 338 
                newErrorCode = 7;
339 
        }
 339 
        }
340 
        else if(ErrorGpsFixLost)
 340 
        else if(ErrorGpsFixLost)
341 
        {
 341 
        {
342 
                LED_RED_ON;
 342 
                LED_RED_ON;
343 
                sprintf(ErrorMSG,"GPS Fix lost    ");
 343 
                sprintf(ErrorMSG,"GPS Fix lost    ");
344 
                newErrorCode = 21;
 344 
                newErrorCode = 21;
345 
        }
 345 
        }
346 
        else if(ErrorDisturbedEarthMagnetField)
 346 
        else if(ErrorDisturbedEarthMagnetField)
347 
        {
 347 
        {
348 
                LED_RED_ON;
 348 
                LED_RED_ON;
349 
                sprintf(ErrorMSG,"Magnet error    ");
 349 
                sprintf(ErrorMSG,"Magnet error    ");
350 
                newErrorCode = 22;
 350 
                newErrorCode = 22;
351 
                DebugOut.StatusRed |= AMPEL_COMPASS | AMPEL_NC;
 351 
                DebugOut.StatusRed |= AMPEL_COMPASS | AMPEL_NC;
352 
                UART_VersionInfo.HardwareError[0] |= NC_ERROR0_COMPASS_VALUE;
 352 
                UART_VersionInfo.HardwareError[0] |= NC_ERROR0_COMPASS_VALUE;
353 
        }
 353 
        }
354 
        else if(((ErrorCheck_BL_MinOfMaxPWM == 40 && (TimeSinceMotorStart > 3))  || (ErrorCheck_BL_MinOfMaxPWM == 39)) && !ErrorCode)
 354 
        else if(((ErrorCheck_BL_MinOfMaxPWM == 40 && (TimeSinceMotorStart > 3))  || (ErrorCheck_BL_MinOfMaxPWM == 39)) && !ErrorCode)
355 
        {
 355 
        {
356 
                LED_RED_ON;
 356 
                LED_RED_ON;
357 
                sprintf(ErrorMSG,"ERR:Motor restart  ");
 357 
                sprintf(ErrorMSG,"ERR:Motor restart  ");
358 
                newErrorCode = 23;
 358 
                newErrorCode = 23;
359 
                DebugOut.StatusRed |= AMPEL_BL;
 359 
                DebugOut.StatusRed |= AMPEL_BL;
360 
        }
 360 
        }
361 
        else if(BL_MinOfMaxPWM < 30 && !ErrorCode)
 361 
        else if(BL_MinOfMaxPWM < 30 && !ErrorCode)
362 
        {
 362 
        {
363 
                u16 i;
 363 
                u16 i;
364 
                for(i = 0; i < 12; i++) if(Motor[i].MaxPWM == BL_MinOfMaxPWM) break;
 364 
                for(i = 0; i < 12; i++) if(Motor[i].MaxPWM == BL_MinOfMaxPWM) break;
365 
                LED_RED_ON;
 365 
                LED_RED_ON;
366 
                sprintf(ErrorMSG,"ERR:BL%2d Test:%2d ",i+1,BL_MinOfMaxPWM);
 366 
                sprintf(ErrorMSG,"ERR:BL%2d Test:%2d ",i+1,BL_MinOfMaxPWM);
367 
                newErrorCode = 32;
 367 
                newErrorCode = 32;
368 
                DebugOut.StatusRed |= AMPEL_BL;
 368 
                DebugOut.StatusRed |= AMPEL_BL;
369 
        }
 369 
        }
370 
        else if(ErrorCheck_BL_MinOfMaxPWM < 248 && (TimeSinceMotorStart > 3) && !ErrorCode)
 370 
        else if(ErrorCheck_BL_MinOfMaxPWM < 248 && (TimeSinceMotorStart > 3) && !ErrorCode)
371 
        {
 371 
        {
372 
                LED_RED_ON;
 372 
                LED_RED_ON;
373 
                sprintf(ErrorMSG,"ERR:BL Limitation   ");
 373 
                sprintf(ErrorMSG,"ERR:BL Limitation   ");
374 
                newErrorCode = 24;
 374 
                newErrorCode = 24;
375 
                DebugOut.StatusRed |= AMPEL_BL;
 375 
                DebugOut.StatusRed |= AMPEL_BL;
376 
        }
 376 
        }
377 
        else if((NCFlags & NC_FLAG_RANGE_LIMIT) && (FC.StatusFlags & FC_STATUS_FLY) && !ErrorCode)
 377 
        else if((NCFlags & NC_FLAG_RANGE_LIMIT) && (FC.StatusFlags & FC_STATUS_FLY) && !ErrorCode)
378 
        {
 378 
        {
379 
                LED_RED_ON;
 379 
                LED_RED_ON;
380 
                sprintf(ErrorMSG,"ERR:GPS WP range ");
 380 
                sprintf(ErrorMSG,"ERR:GPS WP range ");
381 
                newErrorCode = 25;
 381 
                newErrorCode = 25;
382 
                DebugOut.StatusRed |= AMPEL_NC;
 382 
                DebugOut.StatusRed |= AMPEL_NC;
383 
        }
 383 
        }
384 
        else if((!SD_SWITCH || (SDCardInfo.Valid == 0)) && Parameter.GlobalConfig3 & CFG3_NO_SDCARD_NO_START && !(FC.StatusFlags & FC_STATUS_FLY))
 384 
        else if((!SD_SWITCH || (SDCardInfo.Valid == 0)) && Parameter.GlobalConfig3 & CFG3_NO_SDCARD_NO_START && !(FC.StatusFlags & FC_STATUS_FLY))
385 
        {
 385 
        {
386 
                LED_RED_ON;
 386 
                LED_RED_ON;
387 
                sprintf(ErrorMSG,"ERR:No SD-Card  ");
 387 
                sprintf(ErrorMSG,"ERR:No SD-Card  ");
388 
                newErrorCode = 26;
 388 
                newErrorCode = 26;
389 
                DebugOut.StatusRed |= AMPEL_NC;
 389 
                DebugOut.StatusRed |= AMPEL_NC;
390 
        }
 390 
        }
391 
        else if((SD_LoggingError || (SD_WatchDog < 2000 && SD_WatchDog != 0)) && Parameter.GlobalConfig3 & CFG3_NO_SDCARD_NO_START)
 391 
        else if((SD_LoggingError || (SD_WatchDog < 2000 && SD_WatchDog != 0)) && Parameter.GlobalConfig3 & CFG3_NO_SDCARD_NO_START)
392 
        {
 392 
        {
393 
                LED_RED_ON;
 393 
                LED_RED_ON;
394 
                sprintf(ErrorMSG,"ERR:SD Logging abort");
 394 
                sprintf(ErrorMSG,"ERR:SD Logging abort");
395 
                newErrorCode = 27;
 395 
                newErrorCode = 27;
396 
                DebugOut.StatusRed |= AMPEL_NC;
 396 
                DebugOut.StatusRed |= AMPEL_NC;
397 
                SD_LoggingError = 0;
 397 
                SD_LoggingError = 0;
398 
        }
 398 
        }
399 
        else if(((AbsoluteFlyingAltitude) && (NaviData.Altimeter / 20 >= AbsoluteFlyingAltitude)) && (FC.StatusFlags & FC_STATUS_FLY))
 399 
        else if(((AbsoluteFlyingAltitude) && (NaviData.Altimeter / 20 >= AbsoluteFlyingAltitude)) && (FC.StatusFlags & FC_STATUS_FLY))
400 
        {
 400 
        {
401 
                LED_RED_ON;
 401 
                LED_RED_ON;
402 
                sprintf(ErrorMSG,"ERR:Max Altitude ");
 402 
                sprintf(ErrorMSG,"ERR:Max Altitude ");
403 
                newErrorCode = 29;
 403 
                newErrorCode = 29;
404 
                DebugOut.StatusRed |= AMPEL_NC;
 404 
                DebugOut.StatusRed |= AMPEL_NC;
405 
        }
 405 
        }
406 
        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)))
 406 
        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 
        {
 407 
        {
408 
                LED_RED_ON;
 408 
                LED_RED_ON;
409 
                sprintf(ErrorMSG,"No GPS Fix      ");
 409 
                sprintf(ErrorMSG,"No GPS Fix      ");
410 
                newErrorCode = 30;
 410 
                newErrorCode = 30;
411 
        }
 411 
        }
412 
        else if((FreqNewGpsData <= 35 || FreqNewGpsData > 60) && TimeSinceMotorStart > 15)
 412 
        else if((FreqNewGpsData <= 35 || FreqNewGpsData > 60) && TimeSinceMotorStart > 15)
413 
        {
 413 
        {
414 
                LED_RED_ON;
 414 
                LED_RED_ON;
415 
                sprintf(ErrorMSG,"GPS Update rate ");
 415 
                sprintf(ErrorMSG,"GPS Update rate ");
416 
                newErrorCode = 38;
 416 
                newErrorCode = 38;
417 
        }
 417 
        }
418 
        else if(NC_GPS_ModeCharacter == 'F')
 418 
        else if(NC_GPS_ModeCharacter == 'F')
419 
        {                                                                
 419 
        {                                                                
420 
                sprintf(ErrorMSG,"FAILSAFE pos.!  ");
 420 
                sprintf(ErrorMSG,"FAILSAFE pos.!  ");
421 
                newErrorCode = 35;
 421 
                newErrorCode = 35;
422 
        }
 422 
        }
423 
        else if(FC.StatusFlags3 & FC_STATUS3_REDUNDANCE_ERROR)
 423 
        else if(FC.StatusFlags3 & FC_STATUS3_REDUNDANCE_ERROR)
424 
        {                                                                
 424 
        {                                                                
425 
                sprintf(ErrorMSG,"ERR:Redundancy  ");
 425 
                sprintf(ErrorMSG,"ERR:Redundancy  ");
426 
                newErrorCode = 36;
 426 
                newErrorCode = 36;
427 
        }
 427 
        }
428 
        else if(FC.StatusFlags3 & FC_STATUS3_REDUNDANCE_TEST)
 428 
        else if(FC.StatusFlags3 & FC_STATUS3_REDUNDANCE_TEST)
429 
        {                                                                
 429 
        {                                                                
430 
                sprintf(ErrorMSG,"Redundancy test ");
 430 
                sprintf(ErrorMSG,"Redundancy test ");
431 
                newErrorCode = 37;
 431 
                newErrorCode = 37;
432 
        }
 432 
        }
433 
        else if(CanbusTimeOut == 1)
 433 
        else if(CanbusTimeOut == 1)
434 
        {                                                                
 434 
        {                                                                
435 
                sprintf(ErrorMSG,"ERR: Canbus");
 435 
                sprintf(ErrorMSG,"ERR: Canbus");
436 
CanbusInit();
 436 
CanbusInit();
437 
                newErrorCode = 39;
 437 
                newErrorCode = 39;
438 
        }
 438 
        }
439 
        else
 439 
        else
440 
        if(FC.Error[1] & FC_ERROR1_ACC_NOT_CAL)
 440 
        if(FC.Error[1] & FC_ERROR1_ACC_NOT_CAL)
441 
        {
 441 
        {
442 
         LED_RED_ON;
 442 
         LED_RED_ON;
443 
         sprintf(ErrorMSG,"ACC not calib.");
 443 
         sprintf(ErrorMSG,"ACC not calib.");
444 
         newErrorCode = 42;
 444 
         newErrorCode = 42;
445 
        }
 445 
        }
446 
        else // no error occured
 446 
        else // no error occured
447 
        {
 447 
        {
448 
                StopNavigation = 0;
 448 
                StopNavigation = 0;
449 
                LED_RED_OFF;
 449 
                LED_RED_OFF;
450 
                if(no_error_delay) { no_error_delay--;  }
 450 
                if(no_error_delay) { no_error_delay--;  }
451 
                else
 451 
                else
452 
                {
 452 
                {
453 
                        sprintf(ErrorMSG,"No Error            ");
 453 
                        sprintf(ErrorMSG,"No Error            ");
454 
                        ErrorCode = 0;
 454 
                        ErrorCode = 0;
455 
                }
 455 
                }
456 
        }
 456 
        }
457 
 
 457 
 
458 
    if(newErrorCode)
 458 
    if(newErrorCode)
459 
         {
 459 
         {
460 
          if(FC.StatusFlags & FC_STATUS_MOTOR_RUN) no_error_delay = 8; // delay the errors if the motors are running
 460 
          if(FC.StatusFlags & FC_STATUS_MOTOR_RUN) no_error_delay = 8; // delay the errors if the motors are running
461 
          ErrorCode = newErrorCode;
 461 
          ErrorCode = newErrorCode;
462 
         }
 462 
         }
463 
 FC.Error[0] = 0;
 463 
 FC.Error[0] = 0;
464 
 FC.Error[1] = 0;
 464 
 FC.Error[1] = 0;
465 
 FC.Error[2] = 0;
 465 
 FC.Error[2] = 0;
466 
 FC.Error[3] = 0;
 466 
 FC.Error[3] = 0;
467 
 FC.Error[4] = 0;
 467 
 FC.Error[4] = 0;
468 
 ErrorGpsFixLost = 0;
 468 
 ErrorGpsFixLost = 0;
469 
 ErrorCheck_BL_MinOfMaxPWM = BL_MinOfMaxPWM;
 469 
 ErrorCheck_BL_MinOfMaxPWM = BL_MinOfMaxPWM;
470 
}
 470 
}
471 
 
 471 
 
472 
unsigned char RAM_Checksum(unsigned char* pBuffer, u16 len)
 472 
unsigned char RAM_Checksum(unsigned char* pBuffer, u16 len)
473 
{
 473 
{
474 
        u8 crc = 0xAA;
 474 
        u8 crc = 0xAA;
475 
        u16 i;
 475 
        u16 i;
476 
        for(i=0; i<len; i++) crc += pBuffer[i];
 476 
        for(i=0; i<len; i++) crc += pBuffer[i];
477 
        return crc;
 477 
        return crc;
478 
}
 478 
}
479 
 
 479 
 
480 
 
 480 
 
481 
u8 Polling(void)
 481 
u8 Polling(void)
482 
{
 482 
{
483 
        static u8 running = 0, oldFcFlags = 0, count5sec, TimeoutGPS_Process;
 483 
        static u8 running = 0, oldFcFlags = 0, count5sec, TimeoutGPS_Process;
484 
        static u32 old_ms = 0;
 484 
        static u32 old_ms = 0;
485 
 
 485 
 
486 
        if(running) {/*DebugOut.Analog[]++;*/ return(1);};
 486 
        if(running) {/*DebugOut.Analog[]++;*/ return(1);};
487 
        running = 1;
 487 
        running = 1;
488 
 
 488 
 
489 
        if(CountMilliseconds != old_ms)  // 1 ms
 489 
        if(CountMilliseconds != old_ms)  // 1 ms
490 
        {
 490 
        {
491 
                if(CanbusTimeOut >= 2) CanbusTimeOut--;
 491 
                if(CanbusTimeOut >= 2) CanbusTimeOut--;
492 
                old_ms = CountMilliseconds;
 492 
                old_ms = CountMilliseconds;
493 
                Compass_Update();               // update compass communication
 493 
                Compass_Update();               // update compass communication
494 
                Analog_Update();                // get new ADC values
 494 
                Analog_Update();                // get new ADC values
495 
                CalcHeadFree();
 495 
                CalcHeadFree();
496 
                if(UART_VersionInfo.HWMajor >= 30) ProcessCanBus();
 496 
                if(UART_VersionInfo.HWMajor >= 30) ProcessCanBus();
497 
                if(!CheckDelay(SPI0_Timeout)) TimeoutGPS_Process = 0;
 497 
                if(!CheckDelay(SPI0_Timeout)) TimeoutGPS_Process = 0;
498 
                else if(CountMilliseconds - SPI0_Timeout > 30000000L) SPI0_Timeout = CountMilliseconds; // avoid too long overflows
 498 
                else if(CountMilliseconds - SPI0_Timeout > 30000000L) SPI0_Timeout = CountMilliseconds; // avoid too long overflows
499 
                if(++TimeoutGPS_Process >= 25)
 499 
                if(++TimeoutGPS_Process >= 25)
500 
                 {
 500 
                 {
501 
                  GPS_Navigation(&GPSData, &(ToFlightCtrl.GPSStick)); // process the GPS data even if the FC is not connected
 501 
                  GPS_Navigation(&GPSData, &(ToFlightCtrl.GPSStick)); // process the GPS data even if the FC is not connected
502 
                  TimeoutGPS_Process = 0;
 502 
                  TimeoutGPS_Process = 0;
503 
                 }
 503 
                 }
504 
        }
 504 
        }
505 
 
 505 
 
506 
        SPI0_UpdateBuffer();    // also calls the GPS-functions
 506 
        SPI0_UpdateBuffer();    // also calls the GPS-functions
507 
        UART0_ProcessRxData();  // GPS process request
 507 
        UART0_ProcessRxData();  // GPS process request
508 
        UART0_TransmitTxData(); // GPS send answer
 508 
        UART0_TransmitTxData(); // GPS send answer
509 
        UART1_ProcessRxData();  // PC process request
 509 
        UART1_ProcessRxData();  // PC process request
510 
        UART1_TransmitTxData(); // PC send answer
 510 
        UART1_TransmitTxData(); // PC send answer
511 
        UART2_TransmitTxData(); // FC send answer
 511 
        UART2_TransmitTxData(); // FC send answer
512 
 
 512 
 
513 
        if(!(FC.StatusFlags & FC_STATUS_MOTOR_RUN)) TimeSinceMotorStart = 0;
 513 
        if(!(FC.StatusFlags & FC_STATUS_MOTOR_RUN)) TimeSinceMotorStart = 0;
514 
 
 514 
 
515 
        if(HugeBlockFromFC.WhoAmI == 1) // contains eeprom-Data
 515 
        if(HugeBlockFromFC.WhoAmI == 1) // contains eeprom-Data
516 
         {
 516 
         {
517 
      u8 crc1, crc2;
 517 
      u8 crc1, crc2;
518 
          crc1 = HugeBlockFromFC.Data[sizeof(EE_Parameter)-1];                                                  // contains the EE_Parameter.crc
 518 
          crc1 = HugeBlockFromFC.Data[sizeof(EE_Parameter)-1];                                                  // contains the EE_Parameter.crc
519 
          crc2 = RAM_Checksum((u8 *)(&HugeBlockFromFC.Data), sizeof(EE_Parameter)-1);   // calculates the current crc
 519 
          crc2 = RAM_Checksum((u8 *)(&HugeBlockFromFC.Data), sizeof(EE_Parameter)-1);   // calculates the current crc
520 
          if(crc1 == crc2)
 520 
          if(crc1 == crc2)
521 
           {
 521 
           {
522 
                memcpy(&EE_Parameter, (u8 *) HugeBlockFromFC.Data, sizeof(EE_Parameter));
 522 
                memcpy(&EE_Parameter, (u8 *) HugeBlockFromFC.Data, sizeof(EE_Parameter));
523 
                RequestConfigFromFC = 0;
 523 
                RequestConfigFromFC = 0;
524 
           }
 524 
           }
525 
                HugeBlockFromFC.WhoAmI = 0;
 525 
                HugeBlockFromFC.WhoAmI = 0;
526 
         }
 526 
         }
527 
 
 527 
 
528 
    if(FCCalibActive)
 528 
    if(FCCalibActive)
529 
            {
 529 
            {
530 
             count5sec = 0;
 530 
             count5sec = 0;
531 
             FreqNewGpsData = 50;
 531 
             FreqNewGpsData = 50;
532 
             CountNewGpsDataIn5Sec = 25;
 532 
             CountNewGpsDataIn5Sec = 25;
533 
             CountGpsProcessedIn5Sec = 0;
 533 
             CountGpsProcessedIn5Sec = 0;
534 
             TimerCheckError = SetDelay(1000);
 534 
             TimerCheckError = SetDelay(1000);
535 
            }
 535 
            }
536 
 
 536 
 
537 
        // ---------------- Error Check Timing ----------------------------
 537 
        // ---------------- Error Check Timing ----------------------------
538 
        if(CheckDelay(TimerCheckError) || (FC.StatusFlags & FC_STATUS_START && !(oldFcFlags & FC_STATUS_START))) // Timer or FY wants to start
 538 
        if(CheckDelay(TimerCheckError) || (FC.StatusFlags & FC_STATUS_START && !(oldFcFlags & FC_STATUS_START))) // Timer or FY wants to start
539 
        {
 539 
        {
540 
                if(CheckDelay(TimerCheckError))
 540 
                if(CheckDelay(TimerCheckError))
541 
                {
 541 
                {
542 
                        if(!BLITZ_CONNECTED) BlitzSchuhConnected = 1; else BlitzSchuhConnected = 0;
 542 
                        if(!BLITZ_CONNECTED) BlitzSchuhConnected = 1; else BlitzSchuhConnected = 0;
543 
                        TimerCheckError = SetDelay(1000);
 543 
                        TimerCheckError = SetDelay(1000);
544 
                        if(CompassValueErrorCount) CompassValueErrorCount--;
 544 
                        if(CompassValueErrorCount) CompassValueErrorCount--;
545 
                        if(++count5sec == 5)
 545 
                        if(++count5sec == 5)
546 
                        {
 546 
                        {
547 
                                FreqGpsNavProcessed = CountGpsProcessedIn5Sec * 2; //400 = 40Hz
 547 
                                FreqGpsNavProcessed = CountGpsProcessedIn5Sec * 2; //400 = 40Hz
548 
                                FreqNewGpsData = CountNewGpsDataIn5Sec;                    // 50 = 5,0Hz
 548 
                                FreqNewGpsData = CountNewGpsDataIn5Sec;                    // 50 = 5,0Hz
549 
 
 549 
 
550 
                                count5sec = 0;
 550 
                                count5sec = 0;
551 
                                CountGpsProcessedIn5Sec = 0;
 551 
                                CountGpsProcessedIn5Sec = 0;
552 
                                CountNewGpsDataIn5Sec = FreqNewGpsData / 2;
 552 
                                CountNewGpsDataIn5Sec = FreqNewGpsData / 2;
553 
                                ConfirmGpsUpdateRate(FreqNewGpsData);
 553 
                                ConfirmGpsUpdateRate(FreqNewGpsData);
554 
                        }
 554 
                        }
555 
                }
 555 
                }
556 
                oldFcFlags = FC.StatusFlags;
 556 
                oldFcFlags = FC.StatusFlags;
557 
//              if(CheckDelay(SPI0_Timeout) && (DebugUART == UART1)) GPS_Navigation(&GPSData, &(ToFlightCtrl.GPSStick)); // process the GPS data even if the FC is not connected
 557 
//              if(CheckDelay(SPI0_Timeout) && (DebugUART == UART1)) GPS_Navigation(&GPSData, &(ToFlightCtrl.GPSStick)); // process the GPS data even if the FC is not connected
558 
 
 558 
 
559 
                if(!CheckDelay(SPI0_Timeout) || (DebugUART == UART1)) CheckErrors();
 559 
                if(!CheckDelay(SPI0_Timeout) || (DebugUART == UART1)) CheckErrors();
560 
 
 560 
 
561 
                if(FC.StatusFlags & FC_STATUS_FLY)
 561 
                if(FC.StatusFlags & FC_STATUS_FLY)
562 
                 {
 562 
                 {
563 
                  NaviData.FlyingTime++; // we want to count the battery-time
 563 
                  NaviData.FlyingTime++; // we want to count the battery-time
564 
                  TimeSinceMotorStart++;
 564 
                  TimeSinceMotorStart++;
565 
                 }
 565 
                 }
566 
               
 566 
               
567 
                if(SerialLinkOkay) SerialLinkOkay--;
 567 
                if(SerialLinkOkay) SerialLinkOkay--;
568 
                if(SerialLinkOkay < 250 - 6) NCFlags |= NC_FLAG_NOSERIALLINK; // 6 seconds timeout for serial communication
 568 
                if(SerialLinkOkay < 250 - 6) NCFlags |= NC_FLAG_NOSERIALLINK; // 6 seconds timeout for serial communication
569 
                else NCFlags &= ~NC_FLAG_NOSERIALLINK;
 569 
                else NCFlags &= ~NC_FLAG_NOSERIALLINK;
570 
                if(StopNavigation && (Parameter.NaviGpsModeControl >=  50) && (Parameter.GlobalConfig & FC_CFG_GPS_AKTIV)) BeepTime = 1000;
 570 
                if(StopNavigation && (Parameter.NaviGpsModeControl >=  50) && (Parameter.GlobalConfig & FC_CFG_GPS_AKTIV)) BeepTime = 1000;
571 
        }
 571 
        }
572 
        running = 0;
 572 
        running = 0;
573 
//      if(!PollingTimeout) DebugOut.Analog[17]++;
 573 
//      if(!PollingTimeout) DebugOut.Analog[17]++;
574 
        return(0);
 574 
        return(0);
575 
}
 575 
}
576 
 
 576 
 
577 
// the handler will be cyclic called by the timer 1 ISR
 577 
// the handler will be cyclic called by the timer 1 ISR
578 
// used is for critical timing parts that normaly would handled
 578 
// used is for critical timing parts that normaly would handled
579 
// within the main loop that could block longer at logging activities
 579 
// within the main loop that could block longer at logging activities
580 
void EXTIT3_IRQHandler(void)  // 1ms - Takt
 580 
void EXTIT3_IRQHandler(void)  // 1ms - Takt
581 
{
 581 
{
582 
        IENABLE;
 582 
        IENABLE;
583 
        VIC_ITCmd(EXTIT3_ITLine,DISABLE); // disable irq
 583 
        VIC_ITCmd(EXTIT3_ITLine,DISABLE); // disable irq
584 
 
 584 
 
585 
        if(PollingTimeout == 0)
 585 
        if(PollingTimeout == 0)
586 
        {
 586 
        {
587 
                PollingTimeout = 5;
 587 
                PollingTimeout = 5;
588 
                //if(Polling() == 0) DebugOut.Analog[]++;
 588 
                //if(Polling() == 0) DebugOut.Analog[]++;
589 
                Polling();
 589 
                Polling();
590 
        }
 590 
        }
591 
 
 591 
 
592 
        VIC_SWITCmd(EXTIT3_ITLine,DISABLE); // clear pending bit
 592 
        VIC_SWITCmd(EXTIT3_ITLine,DISABLE); // clear pending bit
593 
        VIC_ITCmd(EXTIT3_ITLine, ENABLE); // enable irq
 593 
        VIC_ITCmd(EXTIT3_ITLine, ENABLE); // enable irq
594 
        IDISABLE;
 594 
        IDISABLE;
595 
        VIC1->VAR = 0xFF; // write any value to VIC0 Vector address register
 595 
        VIC1->VAR = 0xFF; // write any value to VIC0 Vector address register
596 
}
 596 
}
597 
 
 597 
 
598 
//----------------------------------------------------------------------------------------------------
 598 
//----------------------------------------------------------------------------------------------------
599 
int main(void)
 599 
int main(void)
600 
{
 600 
{
601 
 
 601 
 
602 
//      static u32 ftimer =0;
 602 
//      static u32 ftimer =0;
603 
//      static u8 fstate = 0;
 603 
//      static u8 fstate = 0;
604 
//      static File_t* f = NULL;
 604 
//      static File_t* f = NULL;
605 
 
 605 
 
606 
 
 606 
 
607 
        /* Configure the system clocks */
 607 
        /* Configure the system clocks */
608 
        SCU_Config();
 608 
        SCU_Config();
609 
        /* init VIC (Vectored Interrupt Controller)     */
 609 
        /* init VIC (Vectored Interrupt Controller)     */
610 
        SCU_AHBPeriphClockConfig(__VIC,ENABLE); // enable AHB bus clock for VIC
 610 
        SCU_AHBPeriphClockConfig(__VIC,ENABLE); // enable AHB bus clock for VIC
611 
        SCU_AHBPeriphReset(__VIC, DISABLE);             // disable reset state for VIC
 611 
        SCU_AHBPeriphReset(__VIC, DISABLE);             // disable reset state for VIC
612 
        VIC_DeInit();                                                   // deinitializes the VIC module registers to their default reset values.
 612 
        VIC_DeInit();                                                   // deinitializes the VIC module registers to their default reset values.
613 
        VIC_InitDefaultVectors();
 613 
        VIC_InitDefaultVectors();
614 
 
 614 
 
615 
        // initialize timer 1 for System Clock and delay rountines
 615 
        // initialize timer 1 for System Clock and delay rountines
616 
        TIMER1_Init();
 616 
        TIMER1_Init();
617 
        // initialize the LEDs (needs Timer 1)
 617 
        // initialize the LEDs (needs Timer 1)
618 
        Led_Init();
 618 
        Led_Init();
619 
        // initialize the debug UART1
 619 
        // initialize the debug UART1
620 
        UART1_Init();
 620 
        UART1_Init();
621 
        UART1_PutString("\r\n---------------------------------------------");
 621 
        UART1_PutString("\r\n---------------------------------------------");
622 
        // initialize timer 2 for servo outputs
 622 
        // initialize timer 2 for servo outputs
623 
        //TIMER2_Init();
 623 
        //TIMER2_Init();
624 
        // initialize UART2 to FLIGHTCTRL
 624 
        // initialize UART2 to FLIGHTCTRL
625 
        UART2_Init();
 625 
        UART2_Init();
626 
        // initialize UART0 (to MKGPS or MK3MAG)
 626 
        // initialize UART0 (to MKGPS or MK3MAG)
627 
        UART0_Init();
 627 
        UART0_Init();
628 
        // initialize adc
 628 
        // initialize adc
629 
        Analog_Init();
 629 
        Analog_Init();
630 
        // initialize SPI0 to FC
 630 
        // initialize SPI0 to FC
631 
        SPI0_Init();
 631 
        SPI0_Init();
632 
        // initialize i2c busses (needs Timer 1)
 632 
        // initialize i2c busses (needs Timer 1)
633 
        I2CBus_Init(I2C0);
 633 
        I2CBus_Init(I2C0);
634 
        I2CBus_Init(I2C1);
 634 
        I2CBus_Init(I2C1);
635 
        // initialize fat16 partition on sd card (needs Timer 1)
 635 
        // initialize fat16 partition on sd card (needs Timer 1)
636 
        Fat16_Init();
 636 
        Fat16_Init();
637 
        // initialize NC params
 637 
        // initialize NC params
638 
        NCParams_Init();
 638 
        NCParams_Init();
639 
        // initialize the settings
 639 
        // initialize the settings
640 
        Settings_Init();
 640 
        Settings_Init();
641 
        // initialize logging (needs settings)
 641 
        // initialize logging (needs settings)
642 
        Logging_Init();
 642 
        Logging_Init();
643 
 
 643 
 
644 
//UART_VersionInfo.HWMajor = 30;
 644 
//UART_VersionInfo.HWMajor = 30;
645 
        LED_GRN_ON;
 645 
        LED_GRN_ON;
646 
        TimerCheckError = SetDelay(3000);
 646 
        TimerCheckError = SetDelay(3000);
647 
        UART1_PutString("\r\n++++++++++++++++++++++++++++++++++++++++++");
 647 
        UART1_PutString("\r\n++++++++++++++++++++++++++++++++++++++++++");
648 
        UART1_PutString("\n\r Version information:");
 648 
        UART1_PutString("\n\r Version information:");
649 
 
 649 
 
650 
        GetNaviCtrlVersion();
 650 
        GetNaviCtrlVersion();
651 
        DebugOut.StatusGreen = AMPEL_NC | AMPEL_COMPASS; // NC and MK3Mag
 651 
        DebugOut.StatusGreen = AMPEL_NC | AMPEL_COMPASS; // NC and MK3Mag
652 
        DebugOut.StatusRed = 0x00;
 652 
        DebugOut.StatusRed = 0x00;
653 
        UART1_PutString("\r\n++++++++++++++++++++++++++++++++++++++++++");
 653 
        UART1_PutString("\r\n++++++++++++++++++++++++++++++++++++++++++");
654 
 
 - 
 
655 
        Compass_Init();
 654 
        Compass_Init();
656 
 
 655 
 
657 
#ifdef FOLLOW_ME
 656 
#ifdef FOLLOW_ME
658 
        TransmitAlsoToFC = 1;
 657 
        TransmitAlsoToFC = 1;
659 
        UART1_PutString("\r\n++++++++++++++++++++++++++++++++++++++++++");
 658 
        UART1_PutString("\r\n++++++++++++++++++++++++++++++++++++++++++");
660 
        UART1_PutString("\n\r FOLLOW-ME Transmitter only!");
 659 
        UART1_PutString("\n\r FOLLOW-ME Transmitter only!");
661 
        UART1_PutString("\r\n++++++++++++++++++++++++++++++++++++++++++\r\n\r\n");
 660 
        UART1_PutString("\r\n++++++++++++++++++++++++++++++++++++++++++\r\n\r\n");
662 
        TransmitAlsoToFC = 0;
 661 
        TransmitAlsoToFC = 0;
663 
#else
 662 
#else
664 
        SPI0_GetFlightCtrlVersion();
 663 
        SPI0_GetFlightCtrlVersion();
665 
        if(FC_Version.Compatible != FC_SPI_COMPATIBLE)
 664 
        if(FC_Version.Compatible != FC_SPI_COMPATIBLE)
666 
        {
 665 
        {
667 
                UART1_PutString("\n\r Flight-Ctrl not compatible\n\r");
 666 
                UART1_PutString("\n\r Flight-Ctrl not compatible\n\r");
668 
                LED_RED_ON;
 667 
                LED_RED_ON;
669 
        }
 668 
        }
670 
#endif
 669 
#endif
671 
if(IamMaster == SLAVE)  UART1_PutString(" SLAVE\r\n");
 670 
if(IamMaster == SLAVE)  UART1_PutString(" SLAVE\r\n");
672 
if(IamMaster == MASTER) UART1_PutString(" MASTER\r\n");
 671 
if(IamMaster == MASTER) UART1_PutString(" MASTER\r\n");
673 
 
 672 
 
674 
        if(UBX_Setup() == 0) UBX_Setup(); // inits the GPS-Module via ubx -> try twice
 673 
        if(UBX_Setup() == 0) UBX_Setup(); // inits the GPS-Module via ubx -> try twice
675 
        GPS_Init();
 674 
        GPS_Init();
676 
        // ---------- Prepare the isr driven
 675 
        // ---------- Prepare the isr driven
677 
        // set to absolute lowest priority
 676 
        // set to absolute lowest priority
678 
    VIC_Config(EXTIT3_ITLine, VIC_IRQ, PRIORITY_SW);
 677 
    VIC_Config(EXTIT3_ITLine, VIC_IRQ, PRIORITY_SW);
679 
        // enable interrupts
 678 
        // enable interrupts
680 
    VIC_ITCmd(EXTIT3_ITLine, ENABLE);
 679 
    VIC_ITCmd(EXTIT3_ITLine, ENABLE);
681 
 
 680 
 
682 
        Debug_OK("START");
 681 
        Debug_OK("START");
683 
        UART1_PutString("\r\n");
 682 
        UART1_PutString("\r\n");
684 
        fifo_purge(&UART1_rx_fifo); // flush the whole fifo init buffer
 683 
        fifo_purge(&UART1_rx_fifo); // flush the whole fifo init buffer
685 
        LED_GRN_ON;
 684 
        LED_GRN_ON;
686 
        LED_RED_OFF;
 685 
        LED_RED_OFF;
687 
    Settings_GetParamValue(PID_SEND_NMEA,(u16 *) &NMEA_Interval);
 686 
    Settings_GetParamValue(PID_SEND_NMEA,(u16 *) &NMEA_Interval);
688 
        UART1_PutString("\r\n");
 687 
        UART1_PutString("\r\n");
689 
 
 688 
 
690 
        CompassValueErrorCount = 0;
 689 
        CompassValueErrorCount = 0;
691 
        I2CBus(Compass_I2CPort)->Timeout = SetDelay(3000);
 690 
        I2CBus(Compass_I2CPort)->Timeout = SetDelay(3000);
692 
 
 691 
 
693 
        // Intilizes the Canbus
 692 
        // Intilizes the Canbus
694 
        if(UART_VersionInfo.HWMajor >= 30) CanbusInit();
 693 
        if(UART_VersionInfo.HWMajor >= 30)
- 
 
 694 
         {
- 
 
 695 
           EXT2_Init(); // External Output EXT2
- 
 
 696 
           CanbusInit();
- 
 
 697 
         }
695 
// ++++++++++++++++++++++++++++++++++++++++++++++
 698 
// ++++++++++++++++++++++++++++++++++++++++++++++
696 
        for (;;) // the endless main loop
 699 
        for (;;) // the endless main loop
697 
        {
 700 
        {
698 
                PollingTimeout = 5;
 701 
                PollingTimeout = 5;
699 
                Polling();
 702 
                Polling();
700 
// ++++++++++++++++++++++++++++++++++++++++++++++
 703 
// ++++++++++++++++++++++++++++++++++++++++++++++
701 
                if(FromFC_LoadWP_List)
 704 
                if(FromFC_LoadWP_List)
702 
                {
 705 
                {
703 
                   WPL_Store.Index = (FromFC_LoadWP_List & ~0x80);
 706 
                   WPL_Store.Index = (FromFC_LoadWP_List & ~0x80);
704 
                   if(WPL_Store.Index <= ToFC_MaxWpListIndex)
 707 
                   if(WPL_Store.Index <= ToFC_MaxWpListIndex)
705 
                   {
 708 
                   {
706 
                                if(PointList_ReadFromFile(&WPL_Store) == WPL_OK)
 709 
                                if(PointList_ReadFromFile(&WPL_Store) == WPL_OK)
707 
                                {
 710 
                                {
708 
                                        if(FromFC_LoadWP_List & 0x80)// -> load relative
 711 
                                        if(FromFC_LoadWP_List & 0x80)// -> load relative
709 
                                        {
 712 
                                        {
710 
                                         u32 angle;
 713 
                                         u32 angle;
711 
                                         angle = (360 + GyroCompassCorrected/10 + Parameter.OrientationAngle * 15) % 360;
 714 
                                         angle = (360 + GyroCompassCorrected/10 + Parameter.OrientationAngle * 15) % 360;
712 
                                                if(NCFlags & NC_FLAG_FREE || NaviData.TargetPositionDeviation.Distance > 7*10)
 715 
                                                if(NCFlags & NC_FLAG_FREE || NaviData.TargetPositionDeviation.Distance > 7*10)
713 
                                        {  // take actual position
 716 
                                        {  // take actual position
714 
                                                if(!PointList_Move(1,&(GPSData.Position),angle)) PointList_Clear();     // try to move wp-list so that 1st entry matches the current position
717 
                                                if(!PointList_Move(1,&(GPSData.Position),angle)) PointList_Clear();     // try to move wp-list so that 1st entry matches the current position
715 
                                                }
 718 
                                                }
716 
                                        else
 719 
                                        else
717 
                                                {  // take last target position
 720 
                                                {  // take last target position
718 
                                                if(!PointList_Move(1, &(NaviData.TargetPosition),angle)) PointList_Clear();     // try to move wp-list so that 1st entry matches the current position
721 
                                                if(!PointList_Move(1, &(NaviData.TargetPosition),angle)) PointList_Clear();     // try to move wp-list so that 1st entry matches the current position
719 
                                                }
 722 
                                                }
720 
                                        }
 723 
                                        }
721 
                                        if(FC.StatusFlags & FC_STATUS_FLY) PointList_WPActive(TRUE);
 724 
                                        if(FC.StatusFlags & FC_STATUS_FLY) PointList_WPActive(TRUE);
722 
                                        GPS_pWaypoint = PointList_WPBegin(); // updates POI index
 725 
                                        GPS_pWaypoint = PointList_WPBegin(); // updates POI index
723 
                                        SpeakWaypointRached = 1;        // Speak once when the last Point is reached
 726 
                                        SpeakWaypointRached = 1;        // Speak once when the last Point is reached
724 
                                        SpeakNextWaypoint = 1;          // Speak once as soon as the Points are active
 727 
                                        SpeakNextWaypoint = 1;          // Speak once as soon as the Points are active
725 
                                        NCFlags &= ~NC_FLAG_TARGET_REACHED;
 728 
                                        NCFlags &= ~NC_FLAG_TARGET_REACHED;
726 
                                        BeepTime = 150;
 729 
                                        BeepTime = 150;
727 
                                }
 730 
                                }
728 
                        }
 731 
                        }
729 
                        FromFC_LoadWP_List = 0;
 732 
                        FromFC_LoadWP_List = 0;
730 
                }
 733 
                }
731 
// ++++++++++++++++++++++++++++++++++++++++++++++
 734 
// ++++++++++++++++++++++++++++++++++++++++++++++
732 
                if(FromFC_Save_SinglePoint)
 735 
                if(FromFC_Save_SinglePoint)
733 
                {
 736 
                {
734 
                        WPL_Store.Index = FromFC_Save_SinglePoint;
 737 
                        WPL_Store.Index = FromFC_Save_SinglePoint;
735 
                        if(WPL_Store.Index <= ToFC_MaxWpListIndex) if(PointList_SaveSinglePoint(&WPL_Store) == WPL_OK) BeepTime = 150;
 738 
                        if(WPL_Store.Index <= ToFC_MaxWpListIndex) if(PointList_SaveSinglePoint(&WPL_Store) == WPL_OK) BeepTime = 150;
736 
                        FromFC_Save_SinglePoint = 0;
 739 
                        FromFC_Save_SinglePoint = 0;
737 
                }
 740 
                }
738 
// ++++++++++++++++++++++++++++++++++++++++++++++
 741 
// ++++++++++++++++++++++++++++++++++++++++++++++
739 
                if(FromFC_Load_SinglePoint)
 742 
                if(FromFC_Load_SinglePoint)
740 
                {
 743 
                {
741 
                   WPL_Store.Index = FromFC_Load_SinglePoint;
 744 
                   WPL_Store.Index = FromFC_Load_SinglePoint;
742 
                   if(WPL_Store.Index <= ToFC_MaxWpListIndex)
 745 
                   if(WPL_Store.Index <= ToFC_MaxWpListIndex)
743 
                   {
 746 
                   {
744 
                        if(PointList_LoadSinglePoint(&WPL_Store) == WPL_OK)
 747 
                        if(PointList_LoadSinglePoint(&WPL_Store) == WPL_OK)
745 
                        {
 748 
                        {
746 
                                if(FC.StatusFlags & FC_STATUS_FLY) PointList_WPActive(TRUE);
 749 
                                if(FC.StatusFlags & FC_STATUS_FLY) PointList_WPActive(TRUE);
747 
                                GPS_pWaypoint = PointList_WPBegin(); // updates POI index
 750 
                                GPS_pWaypoint = PointList_WPBegin(); // updates POI index
748 
                                SpeakWaypointRached = 1;        // Speak once when the last Point is reached
 751 
                                SpeakWaypointRached = 1;        // Speak once when the last Point is reached
749 
                                SpeakNextWaypoint = 1;          // Speak once as soon as the Points are active
 752 
                                SpeakNextWaypoint = 1;          // Speak once as soon as the Points are active
750 
                                NCFlags &= ~NC_FLAG_TARGET_REACHED;
 753 
                                NCFlags &= ~NC_FLAG_TARGET_REACHED;
751 
                                BeepTime = 150;
 754 
                                BeepTime = 150;
752 
                        }
 755 
                        }
753 
                   }
 756 
                   }
754 
                        FromFC_Load_SinglePoint = 0;
 757 
                        FromFC_Load_SinglePoint = 0;
755 
                }
 758 
                }
756 
// ++++++++++++++++++++++++++++++++++++++++++++++
 759 
// ++++++++++++++++++++++++++++++++++++++++++++++
757 
                // ---------------- Logging  ---------------------------------------
 760 
                // ---------------- Logging  ---------------------------------------
758 
                if(SD_WatchDog)
 761 
                if(SD_WatchDog)
759 
                {
 762 
                {
760 
                        SD_WatchDog = 30000;
 763 
                        SD_WatchDog = 30000;
761 
                        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
 764 
                        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
762 
                        else
 765 
                        else
763 
                        {
 766 
                        {
764 
                         ToFC_MaxWpListIndex = 0;
 767 
                         ToFC_MaxWpListIndex = 0;
765 
                         if(FC.StatusFlags & FC_STATUS_START) SD_LoggingError = 100;
 768 
                         if(FC.StatusFlags & FC_STATUS_START) SD_LoggingError = 100;
766 
                        }
 769 
                        }
767 
                        if(!SD_WatchDog) UART1_PutString("\n\rSD-Watchdog - Logging aborted\n\r");
 770 
                        if(!SD_WatchDog) UART1_PutString("\n\rSD-Watchdog - Logging aborted\n\r");
768 
                }
 771 
                }
769 
 
 772 
 
770 
/*
 773 
/*
771 
                if(CheckDelay(ftimer))
 774 
                if(CheckDelay(ftimer))
772 
                {
 775 
                {
773 
 
 776 
 
774 
                        static s8 filename[35];
 777 
                        static s8 filename[35];
775 
                        static u8 i = 0;
 778 
                        static u8 i = 0;
776 
                        s8 dbgmsg[40];
 779 
                        s8 dbgmsg[40];
777 
 
 780 
 
778 
 
 781 
 
779 
 
 782 
 
780 
                        ftimer = SetDelay(100);
 783 
                        ftimer = SetDelay(100);
781 
                        if(FC.Poti[3]>100 && fstate == 0)
 784 
                        if(FC.Poti[3]>100 && fstate == 0)
782 
                        {
 785 
                        {
783 
                                fstate = 1;
 786 
                                fstate = 1;
784 
                                sprintf(filename, "/toast/toasta/toast%02i.txt",i++);
 787 
                                sprintf(filename, "/toast/toasta/toast%02i.txt",i++);
785 
                        }
 788 
                        }
786 
                        else if(FC.Poti[3]<100 && fstate == 2)
 789 
                        else if(FC.Poti[3]<100 && fstate == 2)
787 
                        {
 790 
                        {
788 
                                fstate = 3;
 791 
                                fstate = 3;
789 
                        }
 792 
                        }
790 
 
 793 
 
791 
                        switch(fstate)
 794 
                        switch(fstate)
792 
                        {
 795 
                        {
793 
                                case 1:
 796 
                                case 1:
794 
                                        sprintf(dbgmsg,"\r\nStart writing file: %s", filename);
 797 
                                        sprintf(dbgmsg,"\r\nStart writing file: %s", filename);
795 
                                        Debug(dbgmsg);
 798 
                                        Debug(dbgmsg);
796 
                                        f = fopen_(filename, 'a');
 799 
                                        f = fopen_(filename, 'a');
797 
                                        if(f== NULL) Fat16_Init();
 800 
                                        if(f== NULL) Fat16_Init();
798 
                                        fstate = 2;
 801 
                                        fstate = 2;
799 
                                        break;
 802 
                                        break;
800 
 
 803 
 
801 
                                case 2:
 804 
                                case 2:
802 
                                        fputs_("\r\ntest edins sdv dsivbds iv dsivb disbv idsv bisd bv d suiv dsibsivbdis fbvisdöb visdbvisdb vidbfibds ibv", f);
 805 
                                        fputs_("\r\ntest edins sdv dsivbds iv dsivb disbv idsv bisd bv d suiv dsibsivbdis fbvisdöb visdbvisdb vidbfibds ibv", f);
803 
                                        break;
 806 
                                        break;
804 
 
 807 
 
805 
                                case 3:
 808 
                                case 3:
806 
                                        sprintf(dbgmsg,"\r\nClosing file: %s", filename);
 809 
                                        sprintf(dbgmsg,"\r\nClosing file: %s", filename);
807 
                                        Debug(dbgmsg);
 810 
                                        Debug(dbgmsg);
808 
                                        fclose_(f);
 811 
                                        fclose_(f);
809 
                                        fstate = 0;
 812 
                                        fstate = 0;
810 
                                        break;
 813 
                                        break;
811 
 
 814 
 
812 
                                default:
 815 
                                default:
813 
                                        break;
 816 
                                        break;
814 
                        }
 817 
                        }
815 
                }
 818 
                }
816 
*/                                                       
 819 
*/                                                       
817 
        }
 820 
        }
818 
}
 821 
}
819 
//DebugOut.Analog[]
 822 
//DebugOut.Analog[]
820 
 
 823