Subversion Repositories NaviCtrl

Rev

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

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