Subversion Repositories NaviCtrl

Rev

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

Rev 386 Rev 388
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
 
56
 
57
#include <string.h>
57
#include <string.h>
58
#include <math.h>
58
#include <math.h>
59
#include "91x_lib.h"
59
#include "91x_lib.h"
60
#include "led.h"
60
#include "led.h"
61
#include "gps.h"
61
#include "gps.h"
62
#include "uart1.h"
62
#include "uart1.h"
63
#include "spi_slave.h"
63
#include "spi_slave.h"
64
#include "compass.h"
64
#include "compass.h"
65
#include "timer1.h"
65
#include "timer1.h"
66
#include "timer2.h"
66
#include "timer2.h"
67
#include "config.h"
67
#include "config.h"
68
#include "main.h"
68
#include "main.h"
69
#include "compass.h"
69
#include "compass.h"
70
#include "params.h"
70
#include "params.h"
71
#include "stdlib.h"
71
#include "stdlib.h"
72
 
72
 
73
#define SPI_RXSYNCBYTE1 0xAA
73
#define SPI_RXSYNCBYTE1 0xAA
74
#define SPI_RXSYNCBYTE2 0x83
74
#define SPI_RXSYNCBYTE2 0x83
75
#define SPI_TXSYNCBYTE1 0x81
75
#define SPI_TXSYNCBYTE1 0x81
76
#define SPI_TXSYNCBYTE2 0x55
76
#define SPI_TXSYNCBYTE2 0x55
77
 
77
 
78
//communication packets
78
//communication packets
79
FromFlightCtrl_t   FromFlightCtrl;
79
FromFlightCtrl_t   FromFlightCtrl;
80
ToFlightCtrl_t     ToFlightCtrl;
80
ToFlightCtrl_t     ToFlightCtrl;
81
#define SPI0_TIMEOUT 500 // 500ms
81
#define SPI0_TIMEOUT 500 // 500ms
82
volatile u32 SPI0_Timeout = 0;
82
volatile u32 SPI0_Timeout = 0;
83
u8 Logging_FCStatusFlags1 = 0,Logging_FCStatusFlags2 = 0;
83
u8 Logging_FCStatusFlags1 = 0,Logging_FCStatusFlags2 = 0;
84
 
84
 
85
// tx packet buffer
85
// tx packet buffer
86
#define SPI_TXBUFFER_LEN (2 + sizeof(ToFlightCtrl)) // 2 bytes at start are for synchronization
86
#define SPI_TXBUFFER_LEN (2 + sizeof(ToFlightCtrl)) // 2 bytes at start are for synchronization
87
volatile u8 SPI_TxBuffer[SPI_TXBUFFER_LEN + 10];
87
volatile u8 SPI_TxBuffer[SPI_TXBUFFER_LEN + 10];
88
volatile u8 SPI_TxBufferIndex = 0;
88
volatile u8 SPI_TxBufferIndex = 0;
89
u8 *Ptr_TxChksum = NULL ;  // pointer to checksum in TxBuffer
89
u8 *Ptr_TxChksum = NULL ;  // pointer to checksum in TxBuffer
90
 
90
 
91
// rx packet buffer
91
// rx packet buffer
92
#define SPI_RXBUFFER_LEN sizeof(FromFlightCtrl)
92
#define SPI_RXBUFFER_LEN sizeof(FromFlightCtrl)
93
volatile u8 SPI_RxBuffer[SPI_RXBUFFER_LEN+10];
93
volatile u8 SPI_RxBuffer[SPI_RXBUFFER_LEN+10];
94
volatile u8 SPI_RxBufferIndex = 0;
94
volatile u8 SPI_RxBufferIndex = 0;
95
volatile u8 SPI_RxBuffer_Request = 0;
95
volatile u8 SPI_RxBuffer_Request = 0;
96
#define SPI_COMMAND_INDEX 0
96
#define SPI_COMMAND_INDEX 0
97
 
97
 
98
s32 Kalman_K = 32;
98
s32 Kalman_K = 32;
99
s32 Kalman_MaxDrift = 5 * 16;
99
s32 Kalman_MaxDrift = 5 * 16;
100
s32 Kalman_MaxFusion = 64;
100
s32 Kalman_MaxFusion = 64;
101
s32 Kalman_Kompass = 32;
101
s32 Kalman_Kompass = 32;
102
s32 ToFcGpsZ = 0;
102
s32 ToFcGpsZ = 0;
103
 
103
 
104
u8 SPI_CommandSequence[] = { SPI_NCCMD_VERSION, SPI_NCCMD_KALMAN, SPI_NCCMD_GPSINFO ,SPI_NCCMD_KALMAN, SPI_NCCMD_HOTT_INFO, SPI_NCCMD_KALMAN, SPI_MISC, SPI_NCCMD_KALMAN };
104
u8 SPI_CommandSequence[] = { SPI_NCCMD_VERSION, SPI_NCCMD_KALMAN, SPI_NCCMD_GPSINFO ,SPI_NCCMD_KALMAN, SPI_NCCMD_HOTT_INFO, SPI_NCCMD_KALMAN, SPI_MISC, SPI_NCCMD_KALMAN };
105
u8 SPI_CommandCounter = 0;
105
u8 SPI_CommandCounter = 0;
106
s32 ToFC_Rotate_C = 64, ToFC_Rotate_S = 0;
106
s32 ToFC_Rotate_C = 64, ToFC_Rotate_S = 0;
107
s32 HeadFreeStartAngle = 0;
107
s32 HeadFreeStartAngle = 0;
108
s16 FC_WP_EventChannel = 0; // gibt einen Schaltkanal an die FC weiter, wenn der Wegpunkt erreicht wurde
108
s16 FC_WP_EventChannel = 0; // gibt einen Schaltkanal an die FC weiter, wenn der Wegpunkt erreicht wurde
109
u32 ToFC_AltitudeRate = 0;
109
u32 ToFC_AltitudeRate = 0;
110
s32 ToFC_AltitudeSetpoint = 0;
110
s32 ToFC_AltitudeSetpoint = 0;
111
u8  FromFC_VarioCharacter = ' ';
111
u8  FromFC_VarioCharacter = ' ';
112
u8 GPS_Aid_StickMultiplikator = 0;
112
u8 GPS_Aid_StickMultiplikator = 0;
113
u8 NC_GPS_ModeCharacter = ' ';
113
u8 NC_GPS_ModeCharacter = ' ';
114
u8 FCCalibActive = 0;
114
u8 FCCalibActive = 0;
115
u8 FC_is_Calibrated = 0;
115
u8 FC_is_Calibrated = 0;
116
Motor_t Motor[12];
116
Motor_t Motor[12];
117
u8 NC_To_FC_Flags = 0;
117
u8 NC_To_FC_Flags = 0;
118
u8 BL_MinOfMaxPWM;  // indication if all BL-controllers run on full power
118
u8 BL_MinOfMaxPWM;  // indication if all BL-controllers run on full power
119
u32 FC_I2C_ErrorConter;
119
u32 FC_I2C_ErrorConter;
120
SPI_Version_t FC_Version;
120
SPI_Version_t FC_Version;
121
s16 POI_KameraNick = 0;
121
s16 POI_KameraNick = 0;
122
 
122
 
123
//--------------------------------------------------------------
123
//--------------------------------------------------------------
124
void SSP0_IRQHandler(void)
124
void SSP0_IRQHandler(void)
125
{
125
{
126
        static u8 rxchksum = 0;
126
        static u8 rxchksum = 0;
127
        u8 rxdata;
127
        u8 rxdata;
128
 
128
 
129
        #define SPI_SYNC1       0
129
        #define SPI_SYNC1       0
130
        #define SPI_SYNC2       1
130
        #define SPI_SYNC2       1
131
        #define SPI_DATA        2
131
        #define SPI_DATA        2
132
        static u8 SPI_State = SPI_SYNC1;
132
        static u8 SPI_State = SPI_SYNC1;
133
        //IENABLE;
133
        //IENABLE;
134
 
134
 
135
        // clear pending bits
135
        // clear pending bits
136
        SSP_ClearITPendingBit(SSP0, SSP_IT_RxTimeOut);
136
        SSP_ClearITPendingBit(SSP0, SSP_IT_RxTimeOut);
137
        SSP_ClearITPendingBit(SSP0, SSP_IT_RxFifo);
137
        SSP_ClearITPendingBit(SSP0, SSP_IT_RxFifo);
138
 
138
 
139
        // while RxFIFO not empty
139
        // while RxFIFO not empty
140
        while(SSP_GetFlagStatus(SSP0, SSP_FLAG_RxFifoNotEmpty) == SET)
140
        while(SSP_GetFlagStatus(SSP0, SSP_FLAG_RxFifoNotEmpty) == SET)
141
        {
141
        {
142
                rxdata =  SSP0->DR; // catch the received byte
142
                rxdata =  SSP0->DR; // catch the received byte
143
                // Fill TxFIFO while its not full or end of packet is reached
143
                // Fill TxFIFO while its not full or end of packet is reached
144
                while (SSP_GetFlagStatus(SSP0, SSP_FLAG_TxFifoNotFull) == SET)
144
                while (SSP_GetFlagStatus(SSP0, SSP_FLAG_TxFifoNotFull) == SET)
145
                {
145
                {
146
                        if (SPI_TxBufferIndex  < SPI_TXBUFFER_LEN)   // still data to send ?
146
                        if (SPI_TxBufferIndex  < SPI_TXBUFFER_LEN)   // still data to send ?
147
                        {
147
                        {
148
                                SSP0->DR = SPI_TxBuffer[SPI_TxBufferIndex];       // send a byte
148
                                SSP0->DR = SPI_TxBuffer[SPI_TxBufferIndex];       // send a byte
149
                                *Ptr_TxChksum += SPI_TxBuffer[SPI_TxBufferIndex]; // update checksum
149
                                *Ptr_TxChksum += SPI_TxBuffer[SPI_TxBufferIndex]; // update checksum
150
                                if(SPIWatchDog == 0) *Ptr_TxChksum += 1; // disturbe this packet to stop the communication!
150
                                if(SPIWatchDog == 0) *Ptr_TxChksum += 1; // disturbe this packet to stop the communication!
151
                                SPI_TxBufferIndex++; // pointer to next byte
151
                                SPI_TxBufferIndex++; // pointer to next byte
152
                        }
152
                        }
153
                        else // end of packet is reached reset and copy data to tx buffer
153
                        else // end of packet is reached reset and copy data to tx buffer
154
                        {
154
                        {
155
                                SPI_TxBufferIndex = 0;  // reset buffer index
155
                                SPI_TxBufferIndex = 0;  // reset buffer index
156
                                ToFlightCtrl.Chksum = 0;  // initialize checksum
156
                                ToFlightCtrl.Chksum = 0;  // initialize checksum
157
                                ToFlightCtrl.BeepTime = BeepTime;  // set beeptime
157
                                ToFlightCtrl.BeepTime = BeepTime;  // set beeptime
158
                                BeepTime = 0; // reset local beeptime
158
                                BeepTime = 0; // reset local beeptime
159
                                // copy contents of ToFlightCtrl->SPI_TxBuffer
159
                                // copy contents of ToFlightCtrl->SPI_TxBuffer
160
                                memcpy((u8 *) &(SPI_TxBuffer[2]), (u8 *) &ToFlightCtrl, sizeof(ToFlightCtrl));
160
                                memcpy((u8 *) &(SPI_TxBuffer[2]), (u8 *) &ToFlightCtrl, sizeof(ToFlightCtrl));
161
                        }
161
                        }
162
                }
162
                }
163
                switch (SPI_State)
163
                switch (SPI_State)
164
                {
164
                {
165
                        case SPI_SYNC1:
165
                        case SPI_SYNC1:
166
                                SPI_RxBufferIndex = 0; // reset buffer index
166
                                SPI_RxBufferIndex = 0; // reset buffer index
167
                                rxchksum = rxdata;     // init checksum
167
                                rxchksum = rxdata;     // init checksum
168
                                if (rxdata == SPI_RXSYNCBYTE1)
168
                                if (rxdata == SPI_RXSYNCBYTE1)
169
                                {   // 1st syncbyte ok
169
                                {   // 1st syncbyte ok
170
                                        SPI_State = SPI_SYNC2;  // step to sync2
170
                                        SPI_State = SPI_SYNC2;  // step to sync2
171
                                }
171
                                }
172
                                break;
172
                                break;
173
                        case SPI_SYNC2:
173
                        case SPI_SYNC2:
174
                                if (rxdata == SPI_RXSYNCBYTE2)
174
                                if (rxdata == SPI_RXSYNCBYTE2)
175
                                {  // 2nd Syncbyte ok
175
                                {  // 2nd Syncbyte ok
176
                                        rxchksum += rxdata;
176
                                        rxchksum += rxdata;
177
                                        SPI_State = SPI_DATA;
177
                                        SPI_State = SPI_DATA;
178
                                }  // 2nd Syncbyte does not match
178
                                }  // 2nd Syncbyte does not match
179
                                else
179
                                else
180
                                {
180
                                {
181
                                        SPI_State  = SPI_SYNC1; //jump back to sync1
181
                                        SPI_State  = SPI_SYNC1; //jump back to sync1
182
                                }
182
                                }
183
                                break;
183
                                break;
184
                        case SPI_DATA:
184
                        case SPI_DATA:
185
                                SPI_RxBuffer[SPI_RxBufferIndex++]= rxdata; // copy databyte to rx buffer
185
                                SPI_RxBuffer[SPI_RxBufferIndex++]= rxdata; // copy databyte to rx buffer
186
                                if(SPI_RxBufferIndex >= SPI_RXBUFFER_LEN)  // end of packet is reached
186
                                if(SPI_RxBufferIndex >= SPI_RXBUFFER_LEN)  // end of packet is reached
187
                                {
187
                                {
188
                                        if (rxdata == rxchksum) // verify checksum byte
188
                                        if (rxdata == rxchksum) // verify checksum byte
189
                                        {
189
                                        {
190
                                                // copy SPI_RxBuffer -> FromFlightCtrl
190
                                                // copy SPI_RxBuffer -> FromFlightCtrl
191
                                                if(!SPI_RxBuffer_Request) // block writing to FromFlightCtrl on reading access
191
                                                if(!SPI_RxBuffer_Request) // block writing to FromFlightCtrl on reading access
192
                                                {
192
                                                {
193
                                                        memcpy((u8 *) &FromFlightCtrl, (u8 *) SPI_RxBuffer, sizeof(FromFlightCtrl));
193
                                                        memcpy((u8 *) &FromFlightCtrl, (u8 *) SPI_RxBuffer, sizeof(FromFlightCtrl));
194
                                                        SPI_RxBuffer_Request = 1;
194
                                                        SPI_RxBuffer_Request = 1;
195
                                                }
195
                                                }
196
                                                // reset timeout counter on good packet
196
                                                // reset timeout counter on good packet
197
                                                SPI0_Timeout = SetDelay(SPI0_TIMEOUT);
197
                                                SPI0_Timeout = SetDelay(SPI0_TIMEOUT);
198
                                                DebugOut.Analog[13]++;
198
                                                DebugOut.Analog[13]++;
199
                                        }
199
                                        }
200
                                        else // bad checksum byte
200
                                        else // bad checksum byte
201
                                        {
201
                                        {
202
                                                DebugOut.Analog[12]++; // increase SPI chksum error counter
202
                                                DebugOut.Analog[12]++; // increase SPI chksum error counter
203
                                        }
203
                                        }
204
                                        SPI_State  = SPI_SYNC1; // reset state
204
                                        SPI_State  = SPI_SYNC1; // reset state
205
                                }
205
                                }
206
                                else // end of packet not reached
206
                                else // end of packet not reached
207
                                {
207
                                {
208
                                        rxchksum += rxdata;      // update checksum
208
                                        rxchksum += rxdata;      // update checksum
209
                                }
209
                                }
210
                                break;
210
                                break;
211
                        default:
211
                        default:
212
                                SPI_State  = SPI_SYNC1;
212
                                SPI_State  = SPI_SYNC1;
213
                                break;
213
                                break;
214
                }
214
                }
215
        }
215
        }
216
 
216
 
217
//      IDISABLE;
217
//      IDISABLE;
218
        VIC1->VAR = 0xFF; // write any value to VIC0 Vector address register
218
        VIC1->VAR = 0xFF; // write any value to VIC0 Vector address register
219
}
219
}
220
 
220
 
221
//--------------------------------------------------------------
221
//--------------------------------------------------------------
222
void SPI0_Init(void)
222
void SPI0_Init(void)
223
{
223
{
224
        GPIO_InitTypeDef GPIO_InitStructure;
224
        GPIO_InitTypeDef GPIO_InitStructure;
225
        SSP_InitTypeDef   SSP_InitStructure;
225
        SSP_InitTypeDef   SSP_InitStructure;
226
 
226
 
227
        UART1_PutString("\r\n SPI init...");
227
        UART1_PutString("\r\n SPI init...");
228
 
228
 
229
        SCU_APBPeriphClockConfig(__GPIO2 ,ENABLE);
229
        SCU_APBPeriphClockConfig(__GPIO2 ,ENABLE);
230
        SCU_APBPeriphClockConfig(__SSP0 ,ENABLE);
230
        SCU_APBPeriphClockConfig(__SSP0 ,ENABLE);
231
 
231
 
232
        GPIO_DeInit(GPIO2);
232
        GPIO_DeInit(GPIO2);
233
        //SSP0_CLK, SSP0_MOSI, SSP0_NSS pins
233
        //SSP0_CLK, SSP0_MOSI, SSP0_NSS pins
234
        GPIO_StructInit(&GPIO_InitStructure);
234
        GPIO_StructInit(&GPIO_InitStructure);
235
        GPIO_InitStructure.GPIO_Direction = GPIO_PinInput;
235
        GPIO_InitStructure.GPIO_Direction = GPIO_PinInput;
236
        GPIO_InitStructure.GPIO_Pin = GPIO_Pin_4 | GPIO_Pin_5 | GPIO_Pin_7;
236
        GPIO_InitStructure.GPIO_Pin = GPIO_Pin_4 | GPIO_Pin_5 | GPIO_Pin_7;
237
        GPIO_InitStructure.GPIO_Type = GPIO_Type_PushPull ;
237
        GPIO_InitStructure.GPIO_Type = GPIO_Type_PushPull ;
238
        GPIO_InitStructure.GPIO_IPInputConnected = GPIO_IPInputConnected_Enable;
238
        GPIO_InitStructure.GPIO_IPInputConnected = GPIO_IPInputConnected_Enable;
239
        GPIO_InitStructure.GPIO_Alternate = GPIO_InputAlt1; //SSP0_SCLK, SSP0_MOSI, SSP0_NSS
239
        GPIO_InitStructure.GPIO_Alternate = GPIO_InputAlt1; //SSP0_SCLK, SSP0_MOSI, SSP0_NSS
240
        GPIO_Init (GPIO2, &GPIO_InitStructure);
240
        GPIO_Init (GPIO2, &GPIO_InitStructure);
241
 
241
 
242
        // SSP0_MISO pin GPIO2.6
242
        // SSP0_MISO pin GPIO2.6
243
        GPIO_StructInit(&GPIO_InitStructure);
243
        GPIO_StructInit(&GPIO_InitStructure);
244
        GPIO_InitStructure.GPIO_Direction = GPIO_PinOutput;
244
        GPIO_InitStructure.GPIO_Direction = GPIO_PinOutput;
245
        GPIO_InitStructure.GPIO_Pin = GPIO_Pin_6;
245
        GPIO_InitStructure.GPIO_Pin = GPIO_Pin_6;
246
        GPIO_InitStructure.GPIO_Type = GPIO_Type_PushPull ;
246
        GPIO_InitStructure.GPIO_Type = GPIO_Type_PushPull ;
247
        GPIO_InitStructure.GPIO_IPInputConnected = GPIO_IPInputConnected_Enable;
247
        GPIO_InitStructure.GPIO_IPInputConnected = GPIO_IPInputConnected_Enable;
248
        GPIO_InitStructure.GPIO_Alternate = GPIO_OutputAlt2; //SSP0_MISO
248
        GPIO_InitStructure.GPIO_Alternate = GPIO_OutputAlt2; //SSP0_MISO
249
        GPIO_Init (GPIO2, &GPIO_InitStructure);
249
        GPIO_Init (GPIO2, &GPIO_InitStructure);
250
 
250
 
251
        SSP_DeInit(SSP0);
251
        SSP_DeInit(SSP0);
252
        SSP_StructInit(&SSP_InitStructure);
252
        SSP_StructInit(&SSP_InitStructure);
253
        SSP_InitStructure.SSP_FrameFormat = SSP_FrameFormat_Motorola;
253
        SSP_InitStructure.SSP_FrameFormat = SSP_FrameFormat_Motorola;
254
        SSP_InitStructure.SSP_Mode = SSP_Mode_Slave;
254
        SSP_InitStructure.SSP_Mode = SSP_Mode_Slave;
255
        SSP_InitStructure.SSP_SlaveOutput = SSP_SlaveOutput_Enable;
255
        SSP_InitStructure.SSP_SlaveOutput = SSP_SlaveOutput_Enable;
256
        SSP_InitStructure.SSP_CPHA = SSP_CPHA_1Edge;
256
        SSP_InitStructure.SSP_CPHA = SSP_CPHA_1Edge;
257
        SSP_InitStructure.SSP_CPOL = SSP_CPOL_Low;
257
        SSP_InitStructure.SSP_CPOL = SSP_CPOL_Low;
258
        SSP_InitStructure.SSP_ClockRate = 0;
258
        SSP_InitStructure.SSP_ClockRate = 0;
259
 
259
 
260
        SSP_Init(SSP0, &SSP_InitStructure);
260
        SSP_Init(SSP0, &SSP_InitStructure);
261
        SSP_ITConfig(SSP0, SSP_IT_RxFifo | SSP_IT_RxTimeOut, ENABLE);
261
        SSP_ITConfig(SSP0, SSP_IT_RxFifo | SSP_IT_RxTimeOut, ENABLE);
262
        SSP_Cmd(SSP0, ENABLE);
262
        SSP_Cmd(SSP0, ENABLE);
263
        // initialize the syncbytes in the tx buffer
263
        // initialize the syncbytes in the tx buffer
264
        SPI_TxBuffer[0] = SPI_TXSYNCBYTE1;
264
        SPI_TxBuffer[0] = SPI_TXSYNCBYTE1;
265
        SPI_TxBuffer[1] = SPI_TXSYNCBYTE2;
265
        SPI_TxBuffer[1] = SPI_TXSYNCBYTE2;
266
        // set the pointer to the checksum byte in the tx buffer
266
        // set the pointer to the checksum byte in the tx buffer
267
        Ptr_TxChksum = (u8 *) &(((ToFlightCtrl_t *) &(SPI_TxBuffer[2]))->Chksum);
267
        Ptr_TxChksum = (u8 *) &(((ToFlightCtrl_t *) &(SPI_TxBuffer[2]))->Chksum);
268
 
268
 
269
        ToFlightCtrl.GPSStick.Nick = 0;
269
        ToFlightCtrl.GPSStick.Nick = 0;
270
        ToFlightCtrl.GPSStick.Roll = 0;
270
        ToFlightCtrl.GPSStick.Roll = 0;
271
        ToFlightCtrl.GPSStick.Yaw = 0;
271
        ToFlightCtrl.GPSStick.Yaw = 0;
272
 
272
 
273
        VIC_Config(SSP0_ITLine, VIC_IRQ, PRIORITY_SPI0);
273
        VIC_Config(SSP0_ITLine, VIC_IRQ, PRIORITY_SPI0);
274
        VIC_ITCmd(SSP0_ITLine, ENABLE);
274
        VIC_ITCmd(SSP0_ITLine, ENABLE);
275
 
275
 
276
        SPI0_Timeout = SetDelay(4*SPI0_TIMEOUT);
276
        SPI0_Timeout = SetDelay(4*SPI0_TIMEOUT);
277
 
277
 
278
        UART1_PutString("ok");
278
        UART1_PutString("ok");
279
}
279
}
280
 
280
 
281
 
281
 
282
//------------------------------------------------------
282
//------------------------------------------------------
283
void SPI0_UpdateBuffer(void)
283
void SPI0_UpdateBuffer(void)
284
{
284
{
285
        static u32 timeout = 0;
285
        static u32 timeout = 0;
286
        static u8 counter = 50,hott_index = 0;
286
        static u8 counter = 50,hott_index = 0;
287
        static u8 CompassCalState = 0;
287
        static u8 CompassCalState = 0;
288
        s16 tmp;
288
        s16 tmp;
289
        s32 i1,i2;
289
        s32 i1,i2;
290
       
290
       
291
       
291
       
292
    SPIWatchDog = 3500;    // stop communication to FC after this timeout
292
    SPIWatchDog = 3500;    // stop communication to FC after this timeout
293
        if(SPI_RxBuffer_Request)
293
        if(SPI_RxBuffer_Request)
294
        {
294
        {
295
                // avoid sending data via SPI during the update of the  ToFlightCtrl structure
295
                // avoid sending data via SPI during the update of the  ToFlightCtrl structure
296
                VIC_ITCmd(SSP0_ITLine, DISABLE); // disable SPI interrupt
296
                VIC_ITCmd(SSP0_ITLine, DISABLE); // disable SPI interrupt
297
                ToFlightCtrl.CompassHeading = Compass_Heading;
297
                ToFlightCtrl.CompassHeading = Compass_Heading;
298
//ToFlightCtrl.CompassHeading += 360 + ((s32) Poti8 - 128);
-
 
299
//ToFlightCtrl.CompassHeading %= 360;
-
 
300
                DebugOut.Analog[10] = ToFlightCtrl.CompassHeading;
298
                DebugOut.Analog[10] = ToFlightCtrl.CompassHeading;
301
                if(ToFlightCtrl.CompassHeading >= 0) ToFlightCtrl.CompassHeading = (360 + ToFlightCtrl.CompassHeading + FromFlightCtrl.GyroYaw / 12) % 360;
299
                if(ToFlightCtrl.CompassHeading >= 0) ToFlightCtrl.CompassHeading = (360 + ToFlightCtrl.CompassHeading + FromFlightCtrl.GyroYaw / 12) % 360;
302
//              ToFlightCtrl.MagVecX = MagVector.X;
300
//              ToFlightCtrl.MagVecX = MagVector.X;
303
//              ToFlightCtrl.MagVecY = MagVector.Y;
301
//              ToFlightCtrl.MagVecY = MagVector.Y;
304
                ToFlightCtrl.MagVecZ = MagVector.Z;
302
                ToFlightCtrl.MagVecZ = MagVector.Z;
305
                ToFlightCtrl.NCStatus = 0;
303
                ToFlightCtrl.NCStatus = 0;
306
                // cycle spi commands
304
                // cycle spi commands
307
                ToFlightCtrl.Command = SPI_CommandSequence[SPI_CommandCounter++];
305
                ToFlightCtrl.Command = SPI_CommandSequence[SPI_CommandCounter++];
308
                // restart command cycle at the end
306
                // restart command cycle at the end
309
                if (SPI_CommandCounter >= sizeof(SPI_CommandSequence)) SPI_CommandCounter = 0;
307
                if (SPI_CommandCounter >= sizeof(SPI_CommandSequence)) SPI_CommandCounter = 0;
310
#define FLAG_GPS_AID 0x01
308
#define FLAG_GPS_AID 0x01
311
                switch (ToFlightCtrl.Command)
309
                switch (ToFlightCtrl.Command)
312
                {
310
                {
313
                        case  SPI_NCCMD_KALMAN:  // wird am häufigsten betätigt
311
                        case  SPI_NCCMD_KALMAN:  // wird am häufigsten betätigt
314
                                ToFlightCtrl.Param.sByte[0] = (s8) Kalman_K;
312
                                ToFlightCtrl.Param.sByte[0] = (s8) Kalman_K;
315
                                ToFlightCtrl.Param.sByte[1] = (s8) Kalman_MaxFusion;
313
                                ToFlightCtrl.Param.sByte[1] = (s8) Kalman_MaxFusion;
316
                                ToFlightCtrl.Param.sByte[2] = (s8) Kalman_MaxDrift;
314
                                ToFlightCtrl.Param.sByte[2] = (s8) Kalman_MaxDrift;
317
                                ToFlightCtrl.Param.Byte[3]      = (u8) Kalman_Kompass;
315
                                ToFlightCtrl.Param.Byte[3]      = (u8) Kalman_Kompass;
318
                                ToFlightCtrl.Param.sByte[4] = (s8) ToFcGpsZ;
316
                                ToFlightCtrl.Param.sByte[4] = (s8) ToFcGpsZ;
319
                                ToFlightCtrl.Param.Byte[5] = (s8) ToFC_Rotate_C;
317
                                ToFlightCtrl.Param.Byte[5] = (s8) ToFC_Rotate_C;
320
                                ToFlightCtrl.Param.Byte[6] = (s8) ToFC_Rotate_S;
318
                                ToFlightCtrl.Param.Byte[6] = (s8) ToFC_Rotate_S;
321
                ToFlightCtrl.Param.Byte[7] = GPS_Aid_StickMultiplikator;
319
                ToFlightCtrl.Param.Byte[7] = GPS_Aid_StickMultiplikator;
322
                                if(CAM_Orientation.UpdateMask & CAM_UPDATE_AZIMUTH)
320
                                if(CAM_Orientation.UpdateMask & CAM_UPDATE_AZIMUTH)
323
                                {
321
                                {
324
                                        ToFlightCtrl.Param.sInt[4] = CAM_Orientation.Azimuth;
322
                                        ToFlightCtrl.Param.sInt[4] = CAM_Orientation.Azimuth;
325
                                        CAM_Orientation.UpdateMask &= ~CAM_UPDATE_AZIMUTH;
323
                                        CAM_Orientation.UpdateMask &= ~CAM_UPDATE_AZIMUTH;
326
                                }
324
                                }
327
                                else
325
                                else
328
                                {
326
                                {
329
                                        ToFlightCtrl.Param.sInt[4] = -1;
327
                                        ToFlightCtrl.Param.sInt[4] = -1;
330
                                }
328
                                }
331
 
329
 
332
                                if(NCRARAM_STATE_VALID == NCParams_GetValue(NCPARAMS_NEW_CAMERA_ELEVATION, &tmp))  // Elevation set via 'j' command
330
                                if(NCRARAM_STATE_VALID == NCParams_GetValue(NCPARAMS_NEW_CAMERA_ELEVATION, &tmp))  // Elevation set via 'j' command
333
                                {
331
                                {
334
                                        POI_KameraNick = tmp;
332
                                        POI_KameraNick = tmp;
335
                                }
333
                                }
336
                                else
334
                                else
337
                                {
335
                                {
338
                                        //if(FC.StatusFlags2 & FC_STATUS2_CAREFREE) // only, if carefree is active
336
                                        //if(FC.StatusFlags2 & FC_STATUS2_CAREFREE) // only, if carefree is active
339
                                        POI_KameraNick = CAM_Orientation.Elevation;
337
                                        POI_KameraNick = CAM_Orientation.Elevation;
340
                                        //else ToFlightCtrl.Param.sInt[5] = 0;
338
                                        //else ToFlightCtrl.Param.sInt[5] = 0;
341
                                }
339
                                }
342
                                ToFlightCtrl.Param.sInt[5] = POI_KameraNick;
340
                                ToFlightCtrl.Param.sInt[5] = POI_KameraNick;
343
                                break;
341
                                break;
344
 
342
 
345
                        case SPI_NCCMD_VERSION:
343
                        case SPI_NCCMD_VERSION:
346
                                //+++++++++++++++++++++++++++++++++++++++++++++++++++
344
                                //+++++++++++++++++++++++++++++++++++++++++++++++++++
347
                        //+ higher than the maximum allowed altitude
345
                        //+ higher than the maximum allowed altitude
348
                                //+++++++++++++++++++++++++++++++++++++++++++++++++++
346
                                //+++++++++++++++++++++++++++++++++++++++++++++++++++
349
                                ToFlightCtrl.Param.Byte[0] = VERSION_MAJOR;
347
                                ToFlightCtrl.Param.Byte[0] = VERSION_MAJOR;
350
                                ToFlightCtrl.Param.Byte[1] = VERSION_MINOR;
348
                                ToFlightCtrl.Param.Byte[1] = VERSION_MINOR;
351
                                ToFlightCtrl.Param.Byte[2] = VERSION_PATCH;
349
                                ToFlightCtrl.Param.Byte[2] = VERSION_PATCH;
352
                                ToFlightCtrl.Param.Byte[3] = FC_SPI_COMPATIBLE;
350
                                ToFlightCtrl.Param.Byte[3] = FC_SPI_COMPATIBLE;
353
                                ToFlightCtrl.Param.Byte[4] = Version_HW;
351
                                ToFlightCtrl.Param.Byte[4] = Version_HW;
354
                                ToFlightCtrl.Param.Byte[5] = DebugOut.StatusGreen;
352
                                ToFlightCtrl.Param.Byte[5] = DebugOut.StatusGreen;
355
                                ToFlightCtrl.Param.Byte[6] = DebugOut.StatusRed;
353
                                ToFlightCtrl.Param.Byte[6] = DebugOut.StatusRed;
356
                                ToFlightCtrl.Param.Byte[7] = ErrorCode;
354
                                ToFlightCtrl.Param.Byte[7] = ErrorCode;
357
                                ToFlightCtrl.Param.Byte[8] = NC_GPS_ModeCharacter;
355
                                ToFlightCtrl.Param.Byte[8] = NC_GPS_ModeCharacter;
358
                                ToFlightCtrl.Param.Byte[9] = SerialLinkOkay;
356
                                ToFlightCtrl.Param.Byte[9] = SerialLinkOkay;
359
                                ToFlightCtrl.Param.Byte[10] = NC_To_FC_Flags;
357
                                ToFlightCtrl.Param.Byte[10] = NC_To_FC_Flags;
360
                                if(AbsoluteFlyingAltitude > 255) ToFlightCtrl.Param.Byte[11] = 0; // then the limitation of the FC doesn't work
358
                                if(AbsoluteFlyingAltitude > 255) ToFlightCtrl.Param.Byte[11] = 0; // then the limitation of the FC doesn't work
361
                                else ToFlightCtrl.Param.Byte[11] = AbsoluteFlyingAltitude;
359
                                else ToFlightCtrl.Param.Byte[11] = AbsoluteFlyingAltitude;
362
                                break;
360
                                break;
363
                        case SPI_MISC:
361
                        case SPI_MISC:
364
                                ToFlightCtrl.Param.Byte[0] = EarthMagneticFieldFiltered/5;
362
                                ToFlightCtrl.Param.Byte[0] = EarthMagneticFieldFiltered/5;
365
                                ToFlightCtrl.Param.Byte[1] = EarthMagneticInclination;
363
                                ToFlightCtrl.Param.Byte[1] = EarthMagneticInclination;
366
                                ToFlightCtrl.Param.Byte[2] = EarthMagneticInclinationTheoretic;
364
                                ToFlightCtrl.Param.Byte[2] = EarthMagneticInclinationTheoretic;
367
                                ToFlightCtrl.Param.Byte[3] = 0;
365
                                ToFlightCtrl.Param.Byte[3] = 0;
368
                                ToFlightCtrl.Param.Byte[4] = 0;
366
                                ToFlightCtrl.Param.Byte[4] = 0;
369
                                ToFlightCtrl.Param.Byte[5] = 0;
367
                                ToFlightCtrl.Param.Byte[5] = 0;
370
                                ToFlightCtrl.Param.Byte[6] = 0;
368
                                ToFlightCtrl.Param.Byte[6] = 0;
371
                                ToFlightCtrl.Param.Byte[7] = 0;
369
                                ToFlightCtrl.Param.Byte[7] = 0;
372
                                ToFlightCtrl.Param.Byte[8] = 0;
370
                                ToFlightCtrl.Param.Byte[8] = 0;
373
                                ToFlightCtrl.Param.Byte[9] = 0;
371
                                ToFlightCtrl.Param.Byte[9] = 0;
374
                                ToFlightCtrl.Param.Byte[10] = 0;
372
                                ToFlightCtrl.Param.Byte[10] = 0;
375
                                ToFlightCtrl.Param.Byte[11] = 0;
373
                                ToFlightCtrl.Param.Byte[11] = 0;
376
                                break;
374
                                break;
377
 
375
 
378
                        case SPI_NCCMD_GPSINFO:
376
                        case SPI_NCCMD_GPSINFO:
379
                                ToFlightCtrl.Param.Byte[0] = GPSData.Flags;
377
                                ToFlightCtrl.Param.Byte[0] = GPSData.Flags;
380
                                ToFlightCtrl.Param.Byte[1] = GPSData.NumOfSats;
378
                                ToFlightCtrl.Param.Byte[1] = GPSData.NumOfSats;
381
                                ToFlightCtrl.Param.Byte[2] = GPSData.SatFix;
379
                                ToFlightCtrl.Param.Byte[2] = GPSData.SatFix;
382
                                ToFlightCtrl.Param.Byte[3] = GPSData.Speed_Ground / 100; // m/s
380
                                ToFlightCtrl.Param.Byte[3] = GPSData.Speed_Ground / 100; // m/s
383
                                ToFlightCtrl.Param.Int[2]  = NaviData.HomePositionDeviation.Distance; // dm   //4&5
381
                                ToFlightCtrl.Param.Int[2]  = NaviData.HomePositionDeviation.Distance; // dm   //4&5
384
                                ToFlightCtrl.Param.sInt[3] = NaviData.HomePositionDeviation.Bearing;  // deg  //6&7
382
                                ToFlightCtrl.Param.sInt[3] = NaviData.HomePositionDeviation.Bearing;  // deg  //6&7
385
                                if(FC_WP_EventChannel > 254) FC_WP_EventChannel = 254;
383
                                if(FC_WP_EventChannel > 254) FC_WP_EventChannel = 254;
386
                                ToFlightCtrl.Param.Byte[8] = (s8)(FC_WP_EventChannel - 127);
384
                                ToFlightCtrl.Param.Byte[8] = (s8)(FC_WP_EventChannel - 127);
387
                                if(NCRARAM_STATE_VALID == NCParams_GetValue(NCPARAMS_ALTITUDE_RATE, &tmp))
385
                                if(NCRARAM_STATE_VALID == NCParams_GetValue(NCPARAMS_ALTITUDE_RATE, &tmp))
388
                                {
386
                                {
389
                                        ToFlightCtrl.Param.Byte[9] = (u8)tmp;
387
                                        ToFlightCtrl.Param.Byte[9] = (u8)tmp;
390
                                }
388
                                }
391
                                else
389
                                else
392
                                {
390
                                {
393
                                        ToFlightCtrl.Param.Byte[9] = (u8)ToFC_AltitudeRate;
391
                                        ToFlightCtrl.Param.Byte[9] = (u8)ToFC_AltitudeRate;
394
                                }
392
                                }
395
                                if(NCRARAM_STATE_VALID == NCParams_GetValue(NCPARAMS_ALTITUDE_SETPOINT, &tmp))
393
                                if(NCRARAM_STATE_VALID == NCParams_GetValue(NCPARAMS_ALTITUDE_SETPOINT, &tmp))
396
                                {
394
                                {
397
                                        ToFlightCtrl.Param.sInt[5] = tmp;
395
                                        ToFlightCtrl.Param.sInt[5] = tmp;
398
                                }
396
                                }
399
                                else
397
                                else
400
                                {
398
                                {
401
                                        ToFlightCtrl.Param.sInt[5] = (s16)ToFC_AltitudeSetpoint;
399
                                        ToFlightCtrl.Param.sInt[5] = (s16)ToFC_AltitudeSetpoint;
402
                                }
400
                                }
403
                                break;
401
                                break;
404
/*
-
 
405
typedef struct
-
 
406
{
-
 
407
  unsigned char StartByte;  //0         // 0x7C
-
 
408
  unsigned char Packet_ID;  //1         // 0x89  - Vario ID
-
 
409
  unsigned char WarnBeep;   //2         // Anzahl der Töne 0..36
-
 
410
  unsigned char Heading;        //3     // 1 = 2°
-
 
411
  unsigned int Speed;           //4+5   // in km/h
-
 
412
  unsigned char Lat_North;      //6    
-
 
413
  unsigned char Lat_G;      //7
-
 
414
  unsigned char Lat_M;      //8
-
 
415
  unsigned char Lat_Sek1;       //9    
-
 
416
  unsigned char Lat_Sek2;       //10    
-
 
417
  unsigned char Lon_East;       //11    
-
 
418
  unsigned char Lon_G;      //12
-
 
419
  unsigned char Lon_M;      //13
-
 
420
  unsigned char Lon_Sek1;       //14    
-
 
421
  unsigned char Lon_Sek2;       //15    
-
 
422
  unsigned int Distance;        //16+17    // 9000 = 0m
-
 
423
  unsigned int Altitude;        //18+19    // 500 = 0m
-
 
424
  unsigned int m_sec;           //20+21    // 3000 = 0
-
 
425
  unsigned int m_3sec;          //22+23    // 3000 = 0
-
 
426
  unsigned int m_10sec;         //24+25    // 3000 = 0
-
 
427
  unsigned char NullByte;       // 0x00
-
 
428
  unsigned char NullByte1;      // 0x00
-
 
429
  unsigned char EndByte;                // 0x7D
-
 
430
} GPSPacket_t;
-
 
431
*/
-
 
432
/*
-
 
433
typedef struct
-
 
434
{
-
 
435
  unsigned char StartByte;      //0 0x7C
-
 
436
  unsigned char Packet_ID;      //1 HOTT_GENERAL_PACKET_ID     
-
 
437
  unsigned char WarnBeep;       //2 Anzahl der Töne 0..36
-
 
438
  unsigned char VoltageCell1;   //3 208 = 4,16V  (Voltage * 50 = Wert)
-
 
439
  unsigned char VoltageCell2;   //4
-
 
440
  unsigned char VoltageCell3;   //5
-
 
441
  unsigned char VoltageCell4;   //6
-
 
442
  unsigned char VoltageCell5;   //7
-
 
443
  unsigned char VoltageCell6;   //8
-
 
444
  unsigned int  Battery1;               //9   51  = 5,1V
-
 
445
  unsigned int  Battery2;               //11  51  = 5,1V
-
 
446
  unsigned char Temperature1;   //13  44 = 24°C, 0 = -20°C
-
 
447
  unsigned char Temperature2;   //14  44 = 24°C, 0 = -20°C
-
 
448
  unsigned char FuelPercent;    //15
-
 
449
    signed int  FuelCapacity;   //16
-
 
450
  unsigned int  Rpm;  
-
 
451
  unsigned int  Altitude;      
-
 
452
  unsigned int  m_sec;              // 3000 = 0
-
 
453
  unsigned char m_3sec;             // 120 = 0
-
 
454
  unsigned int  Current;                // 1 = 0.1A
-
 
455
  unsigned int  InputVoltage;   // 66  = 6,6V
-
 
456
  unsigned int  Capacity;               // 1  = 10mAh
-
 
457
  unsigned char NullByte1;      // 0x00
-
 
458
  unsigned char NullByte2;      // 0x00
-
 
459
  unsigned char EndByte;                // 0x7D
-
 
460
} HoTTGeneral_t;
-
 
461
*/
-
 
462
                        case SPI_NCCMD_HOTT_INFO:
402
                        case SPI_NCCMD_HOTT_INFO:
463
                                switch(hott_index++)
403
                                switch(hott_index++)
464
                                {
404
                                {
465
                                case 0:
405
                                case 0:
466
                                        //Dezimalgrad            --> Grad mit Dezimalminuten     --> Grad, Minuten, Sekunden
406
                                        //Dezimalgrad            --> Grad mit Dezimalminuten     --> Grad, Minuten, Sekunden
467
                                        //53.285788 7.4847269    --> N53° 17.14728 E7° 29.08362  --> N53° 17' 8.837" E7° 29' 5.017" 
407
                                        //53.285788 7.4847269    --> N53° 17.14728 E7° 29.08362  --> N53° 17' 8.837" E7° 29' 5.017" 
468
                                        ToFlightCtrl.Param.Byte[11] = HOTT_GPS_PACKET_ID;
408
                                        ToFlightCtrl.Param.Byte[11] = HOTT_GPS_PACKET_ID;
469
                                        ToFlightCtrl.Param.Byte[0] = 3; // index
409
                                        ToFlightCtrl.Param.Byte[0] = 3; // index
470
                                        ToFlightCtrl.Param.Byte[1] = 9-1;       // how many
410
                                        ToFlightCtrl.Param.Byte[1] = 9-1;       // how many
471
                                        //-----------------------------
411
                                        //-----------------------------
472
                                        ToFlightCtrl.Param.Byte[2] = NaviData.HomePositionDeviation.Bearing / 2;
412
                                        ToFlightCtrl.Param.Byte[2] = NaviData.HomePositionDeviation.Bearing / 2;
473
                                        i1 = GPSData.Speed_Ground; // in cm/sec
413
                                        i1 = GPSData.Speed_Ground; // in cm/sec
474
                                        i1 *= 36;
414
                                        i1 *= 36;
475
                                        i1 /= 1000;
415
                                        i1 /= 1000;
476
                                        ToFlightCtrl.Param.Byte[3] = i1 % 256;
416
                                        ToFlightCtrl.Param.Byte[3] = i1 % 256;
477
                                        ToFlightCtrl.Param.Byte[4] = i1 / 256;
417
                                        ToFlightCtrl.Param.Byte[4] = i1 / 256;
478
                                        //-----------------------------
418
                                        //-----------------------------
479
                                        if(GPSData.Position.Latitude < 0) ToFlightCtrl.Param.Byte[2]  = 1; // 1 = S
419
                                        if(GPSData.Position.Latitude < 0) ToFlightCtrl.Param.Byte[2]  = 1; // 1 = S
480
                                        else ToFlightCtrl.Param.Byte[5]  = 0; // 1 = S
420
                                        else ToFlightCtrl.Param.Byte[5]  = 0; // 1 = S
481
                                        i1 = abs(GPSData.Position.Latitude)/10000000L;
421
                                        i1 = abs(GPSData.Position.Latitude)/10000000L;
482
                                        i2 = abs(GPSData.Position.Latitude)%10000000L;
422
                                        i2 = abs(GPSData.Position.Latitude)%10000000L;
483
                                        i1 *= 100;
423
                                        i1 *= 100;
484
                                        i1 += i2 / 100000;
424
                                        i1 += i2 / 100000;
485
                                        i2  = i2 % 100000;
425
                                        i2  = i2 % 100000;
486
                                        i2 /= 10;
426
                                        i2 /= 10;
487
                                        ToFlightCtrl.Param.Byte[6]  = i1 % 256;
427
                                        ToFlightCtrl.Param.Byte[6]  = i1 % 256;
488
                                        ToFlightCtrl.Param.Byte[7]  = i1 / 256;
428
                                        ToFlightCtrl.Param.Byte[7]  = i1 / 256;
489
                                        ToFlightCtrl.Param.Byte[8]  = i2 % 256;
429
                                        ToFlightCtrl.Param.Byte[8]  = i2 % 256;
490
                                        ToFlightCtrl.Param.Byte[9]  = i2 / 256;
430
                                        ToFlightCtrl.Param.Byte[9]  = i2 / 256;
491
                                        break;
431
                                        break;
492
                                case 1:
432
                                case 1:
493
                                        ToFlightCtrl.Param.Byte[11] = HOTT_GPS_PACKET_ID;
433
                                        ToFlightCtrl.Param.Byte[11] = HOTT_GPS_PACKET_ID;
494
                                        ToFlightCtrl.Param.Byte[0] = 11;        // index
434
                                        ToFlightCtrl.Param.Byte[0] = 11;        // index
495
                                        ToFlightCtrl.Param.Byte[1] = 8-1;       // how many
435
                                        ToFlightCtrl.Param.Byte[1] = 8-1;       // how many
496
                                        //-----------------------------
436
                                        //-----------------------------
497
                                        if(GPSData.Position.Longitude < 0) ToFlightCtrl.Param.Byte[2]  = 1; // 1 = E
437
                                        if(GPSData.Position.Longitude < 0) ToFlightCtrl.Param.Byte[2]  = 1; // 1 = E
498
                                        else ToFlightCtrl.Param.Byte[2]  = 0; // 1 = S
438
                                        else ToFlightCtrl.Param.Byte[2]  = 0; // 1 = S
499
                                        i1 = abs(GPSData.Position.Longitude)/10000000L;
439
                                        i1 = abs(GPSData.Position.Longitude)/10000000L;
500
                                        i2 = abs(GPSData.Position.Longitude)%10000000L;
440
                                        i2 = abs(GPSData.Position.Longitude)%10000000L;
501
                                        i1 *= 100;
441
                                        i1 *= 100;
502
                                        i1 += i2 / 100000;
442
                                        i1 += i2 / 100000;
503
                                        i2  = i2 % 100000;
443
                                        i2  = i2 % 100000;
504
                                        i2 /= 10;
444
                                        i2 /= 10;
505
                                        ToFlightCtrl.Param.Byte[3]  = i1 % 256;
445
                                        ToFlightCtrl.Param.Byte[3]  = i1 % 256;
506
                                        ToFlightCtrl.Param.Byte[4]  = i1 / 256;
446
                                        ToFlightCtrl.Param.Byte[4]  = i1 / 256;
507
                                        ToFlightCtrl.Param.Byte[5]  = i2 % 256;
447
                                        ToFlightCtrl.Param.Byte[5]  = i2 % 256;
508
                                        ToFlightCtrl.Param.Byte[6]  = i2 / 256;
448
                                        ToFlightCtrl.Param.Byte[6]  = i2 / 256;
509
                                        //-----------------------------
449
                                        //-----------------------------
510
                                        i1 = NaviData.HomePositionDeviation.Distance / 10; // dann in m 
450
                                        i1 = NaviData.HomePositionDeviation.Distance / 10; // dann in m 
511
                                        ToFlightCtrl.Param.Byte[7]  = i1 % 256;
451
                                        ToFlightCtrl.Param.Byte[7]  = i1 % 256;
512
                                        ToFlightCtrl.Param.Byte[8]  = i1 / 256;
452
                                        ToFlightCtrl.Param.Byte[8]  = i1 / 256;
513
                                        break;
453
                                        break;
514
                                 case 2:
454
                                 case 2:
515
                                        ToFlightCtrl.Param.Byte[11] = HOTT_GENERAL_PACKET_ID;
455
                                        ToFlightCtrl.Param.Byte[11] = HOTT_GENERAL_PACKET_ID;
516
                                        ToFlightCtrl.Param.Byte[0] = 5; // index
456
                                        ToFlightCtrl.Param.Byte[0] = 5; // index
517
                                        ToFlightCtrl.Param.Byte[1] = 2; // how many
457
                                        ToFlightCtrl.Param.Byte[1] = 2; // how many
518
                                        ToFlightCtrl.Param.Byte[2] = EarthMagneticField / (5 * 2);
458
                                        ToFlightCtrl.Param.Byte[2] = EarthMagneticField / (5 * 2);
519
                                        ToFlightCtrl.Param.Byte[3] = EarthMagneticInclination / 2;
459
                                        ToFlightCtrl.Param.Byte[3] = EarthMagneticInclination / 2;
520
                                        hott_index = 0;
460
                                        hott_index = 0;
521
                                        break;
461
                                        break;
522
                                 default:
462
                                 default:
523
                                        ToFlightCtrl.Param.Byte[0] = 255;
463
                                        ToFlightCtrl.Param.Byte[0] = 255;
524
                                        hott_index = 0;
464
                                        hott_index = 0;
525
                                        break;
465
                                        break;
526
                                }
466
                                }
527
                                break;
467
                                break;
528
                        default:
468
                        default:
529
                                break;
469
                                break;
530
// 0 = 0,1
470
// 0 = 0,1
531
// 1 = 2,3
471
// 1 = 2,3
532
// 2 = 4,5
472
// 2 = 4,5
533
// 3 = 6,7
473
// 3 = 6,7
534
// 4 = 8,9
474
// 4 = 8,9
535
// 5 = 10,11
475
// 5 = 10,11
536
                }
476
                }
537
                VIC_ITCmd(SSP0_ITLine, ENABLE);         // enable SPI interrupt
477
                VIC_ITCmd(SSP0_ITLine, ENABLE);         // enable SPI interrupt
538
                switch(FromFlightCtrl.Command)
478
                switch(FromFlightCtrl.Command)
539
                {
479
                {
540
                        case SPI_FCCMD_USER:
480
                        case SPI_FCCMD_USER:
541
                                Parameter.User1 = FromFlightCtrl.Param.Byte[0];
481
                                Parameter.User1 = FromFlightCtrl.Param.Byte[0];
542
                                Parameter.User2 = FromFlightCtrl.Param.Byte[1];
482
                                Parameter.User2 = FromFlightCtrl.Param.Byte[1];
543
                                Parameter.User3 = FromFlightCtrl.Param.Byte[2];
483
                                Parameter.User3 = FromFlightCtrl.Param.Byte[2];
544
                                Parameter.User4 = FromFlightCtrl.Param.Byte[3];
484
                                Parameter.User4 = FromFlightCtrl.Param.Byte[3];
545
                                Parameter.User5 = FromFlightCtrl.Param.Byte[4];
485
                                Parameter.User5 = FromFlightCtrl.Param.Byte[4];
546
                                Parameter.User6 = FromFlightCtrl.Param.Byte[5];
486
                                Parameter.User6 = FromFlightCtrl.Param.Byte[5];
547
                                Parameter.User7 = FromFlightCtrl.Param.Byte[6];
487
                                Parameter.User7 = FromFlightCtrl.Param.Byte[6];
548
                                Parameter.User8 = FromFlightCtrl.Param.Byte[7];
488
                                Parameter.User8 = FromFlightCtrl.Param.Byte[7];
549
                                if(ClearFCStatusFlags)
489
                                if(ClearFCStatusFlags)
550
                                {
490
                                {
551
                                        FC.StatusFlags = 0;
491
                                        FC.StatusFlags = 0;
552
                                        ClearFCStatusFlags = 0;
492
                                        ClearFCStatusFlags = 0;
553
                                }
493
                                }
554
                                FC.StatusFlags |= FromFlightCtrl.Param.Byte[8];
494
                                FC.StatusFlags |= FromFlightCtrl.Param.Byte[8];
555
                                if(FC.StatusFlags & FC_STATUS_CALIBRATE && !FCCalibActive)
495
                                if(FC.StatusFlags & FC_STATUS_CALIBRATE && !FCCalibActive)
556
                                {
496
                                {
557
                                        HeadFreeStartAngle = Compass_Heading * 10;
497
                                        HeadFreeStartAngle = Compass_Heading * 10;
558
                                        Compass_Init();
498
                                        Compass_Init();
559
                                        FCCalibActive = 10;
499
                                        FCCalibActive = 10;
560
                                        FC_is_Calibrated = 0;
500
                                        FC_is_Calibrated = 0;
561
                                }
501
                                }
562
                                else
502
                                else
563
                                {
503
                                {
564
                                        if(FCCalibActive) if(--FCCalibActive == 0) FC_is_Calibrated = 1;
504
                                        if(FCCalibActive) if(--FCCalibActive == 0) FC_is_Calibrated = 1;
565
                                }
505
                                }
566
                                if(FC.StatusFlags & FC_STATUS_START)
506
                                if(FC.StatusFlags & FC_STATUS_START)
567
                             {
507
                             {
568
                                   if(Compass_Heading != -1) HeadFreeStartAngle = Compass_Heading * 10; else
508
                                   if(Compass_Heading != -1) HeadFreeStartAngle = Compass_Heading * 10; else
569
                                   HeadFreeStartAngle = FromFlightCtrl.GyroHeading;
509
                                   HeadFreeStartAngle = FromFlightCtrl.GyroHeading;
570
                                 }
510
                                 }
571
 
511
 
572
                                 if((Parameter.ExtraConfig & CFG_TEACHABLE_CAREFREE))
512
                                 if((Parameter.ExtraConfig & CFG_TEACHABLE_CAREFREE))
573
                                  {
513
                                  {
574
                                   if(!(FC.StatusFlags2 & FC_STATUS2_CAREFREE)) // CF ist jetzt ausgeschaltet -> neue Richtung lernen
514
                                   if(!(FC.StatusFlags2 & FC_STATUS2_CAREFREE)) // CF ist jetzt ausgeschaltet -> neue Richtung lernen
575
                                    {
515
                                    {
576
                                         if((NaviData.HomePositionDeviation.Distance > 200) && (NCFlags & NC_FLAG_GPS_OK))       // nur bei ausreichender Distance -> 20m
516
                                         if((NaviData.HomePositionDeviation.Distance > 200) && (NCFlags & NC_FLAG_GPS_OK))       // nur bei ausreichender Distance -> 20m
577
                                         {
517
                                         {
578
                                          HeadFreeStartAngle = (10 * NaviData.HomePositionDeviation.Bearing + 1800 + 3600 -  Parameter.OrientationAngle * 150) % 3600; // in 0.1°
518
                                          HeadFreeStartAngle = (10 * NaviData.HomePositionDeviation.Bearing + 1800 + 3600 -  Parameter.OrientationAngle * 150) % 3600; // in 0.1°
579
                                         }
519
                                         }
580
                                         else                                                                                            // Ansonsten die aktuelle Richtung übernehmen
520
                                         else                                                                                            // Ansonsten die aktuelle Richtung übernehmen
581
                                          HeadFreeStartAngle = (3600 + FromFlightCtrl.GyroHeading /*+ Parameter.OrientationAngle * 150*/) % 3600; // in 0.1°      
521
                                          HeadFreeStartAngle = (3600 + FromFlightCtrl.GyroHeading /*+ Parameter.OrientationAngle * 150*/) % 3600; // in 0.1°      
582
                                        }
522
                                        }
583
                                }
523
                                }
584
 
-
 
585
//DebugOut.Analog[16] = HeadFreeStartAngle;
-
 
586
 
524
 
587
                                Parameter.ActiveSetting = FromFlightCtrl.Param.Byte[9];
525
                                Parameter.ActiveSetting = FromFlightCtrl.Param.Byte[9];
588
                                DebugOut.Analog[5] = FC.StatusFlags;
526
                                DebugOut.Analog[5] = FC.StatusFlags;
589
                                NaviData.FCStatusFlags = FC.StatusFlags;
527
                                NaviData.FCStatusFlags = FC.StatusFlags;
590
                                FC.StatusFlags2 = FromFlightCtrl.Param.Byte[11];
528
                                FC.StatusFlags2 = FromFlightCtrl.Param.Byte[11];
591
                                NaviData.FCStatusFlags2 = (NaviData.FCStatusFlags2 & (FC_STATUS2_OUT1_ACTIVE | FC_STATUS2_OUT2_ACTIVE)) | (FC.StatusFlags2 & (0xff - (FC_STATUS2_OUT1_ACTIVE | FC_STATUS2_OUT2_ACTIVE)));
529
                                NaviData.FCStatusFlags2 = (NaviData.FCStatusFlags2 & (FC_STATUS2_OUT1_ACTIVE | FC_STATUS2_OUT2_ACTIVE)) | (FC.StatusFlags2 & (0xff - (FC_STATUS2_OUT1_ACTIVE | FC_STATUS2_OUT2_ACTIVE)));
592
 
530
 
593
                                if((!(LastTransmittedFCStatusFlags2 & FC_STATUS2_OUT1_ACTIVE)) && (FC.StatusFlags2 & FC_STATUS2_OUT1_ACTIVE)) NaviData.FCStatusFlags2 |= FC_STATUS2_OUT1_ACTIVE;
531
                                if((!(LastTransmittedFCStatusFlags2 & FC_STATUS2_OUT1_ACTIVE)) && (FC.StatusFlags2 & FC_STATUS2_OUT1_ACTIVE)) NaviData.FCStatusFlags2 |= FC_STATUS2_OUT1_ACTIVE;
594
                                else
532
                                else
595
                                if(((LastTransmittedFCStatusFlags2 & FC_STATUS2_OUT1_ACTIVE)) && !(FC.StatusFlags2 & FC_STATUS2_OUT1_ACTIVE)) NaviData.FCStatusFlags2 &= ~FC_STATUS2_OUT1_ACTIVE;
533
                                if(((LastTransmittedFCStatusFlags2 & FC_STATUS2_OUT1_ACTIVE)) && !(FC.StatusFlags2 & FC_STATUS2_OUT1_ACTIVE)) NaviData.FCStatusFlags2 &= ~FC_STATUS2_OUT1_ACTIVE;
596
 
534
 
597
                                if((!(LastTransmittedFCStatusFlags2 & FC_STATUS2_OUT2_ACTIVE)) && (FC.StatusFlags2 & FC_STATUS2_OUT2_ACTIVE)) NaviData.FCStatusFlags2 |= FC_STATUS2_OUT2_ACTIVE;
535
                                if((!(LastTransmittedFCStatusFlags2 & FC_STATUS2_OUT2_ACTIVE)) && (FC.StatusFlags2 & FC_STATUS2_OUT2_ACTIVE)) NaviData.FCStatusFlags2 |= FC_STATUS2_OUT2_ACTIVE;
598
                                else
536
                                else
599
                                if(((LastTransmittedFCStatusFlags2 & FC_STATUS2_OUT2_ACTIVE)) && !(FC.StatusFlags2 & FC_STATUS2_OUT2_ACTIVE)) NaviData.FCStatusFlags2 &= ~FC_STATUS2_OUT2_ACTIVE;
537
                                if(((LastTransmittedFCStatusFlags2 & FC_STATUS2_OUT2_ACTIVE)) && !(FC.StatusFlags2 & FC_STATUS2_OUT2_ACTIVE)) NaviData.FCStatusFlags2 &= ~FC_STATUS2_OUT2_ACTIVE;
600
 
538
 
601
                                Logging_FCStatusFlags1 |= FC.StatusFlags;
539
                                Logging_FCStatusFlags1 |= FC.StatusFlags;
602
                                Logging_FCStatusFlags2 |= FC.StatusFlags2;
540
                                Logging_FCStatusFlags2 |= FC.StatusFlags2;
603
                                Parameter.ComingHomeAltitude = FromFlightCtrl.Param.Byte[10];
541
                                Parameter.ComingHomeAltitude = FromFlightCtrl.Param.Byte[10];
604
                                break;
542
                                break;
605
 
543
 
606
                        case SPI_FCCMD_ACCU:
544
                        case SPI_FCCMD_ACCU:
607
                                FC.BAT_Current = FromFlightCtrl.Param.Int[0];
545
                                FC.BAT_Current = FromFlightCtrl.Param.Int[0];
608
                                FC.BAT_UsedCapacity = FromFlightCtrl.Param.Int[1];
546
                                FC.BAT_UsedCapacity = FromFlightCtrl.Param.Int[1];
609
                                FC.BAT_Voltage = FromFlightCtrl.Param.Byte[4];
547
                                FC.BAT_Voltage = FromFlightCtrl.Param.Byte[4];
610
                                Parameter.LowVoltageWarning = FromFlightCtrl.Param.Byte[5];
548
                                Parameter.LowVoltageWarning = FromFlightCtrl.Param.Byte[5];
611
                                FromFC_VarioCharacter = FromFlightCtrl.Param.Byte[6];
549
                                FromFC_VarioCharacter = FromFlightCtrl.Param.Byte[6];
612
                                Motor[FromFlightCtrl.Param.Byte[7]].MaxPWM = FromFlightCtrl.Param.Byte[8];
550
                                Motor[FromFlightCtrl.Param.Byte[7]].MaxPWM = FromFlightCtrl.Param.Byte[8];
613
                                Motor[FromFlightCtrl.Param.Byte[7]].State = FromFlightCtrl.Param.Byte[9];
551
                                Motor[FromFlightCtrl.Param.Byte[7]].State = FromFlightCtrl.Param.Byte[9];
614
                                Motor[FromFlightCtrl.Param.Byte[7]].Temperature = FromFlightCtrl.Param.Byte[10];
552
                                Motor[FromFlightCtrl.Param.Byte[7]].Temperature = FromFlightCtrl.Param.Byte[10];
615
                                Motor[FromFlightCtrl.Param.Byte[7]].Current = FromFlightCtrl.Param.Byte[11];
553
                                Motor[FromFlightCtrl.Param.Byte[7]].Current = FromFlightCtrl.Param.Byte[11];
616
                                if(FromFC_VarioCharacter == '+' || FromFC_VarioCharacter == '-') // manual setpoint clears the NC-Parameter command
554
                                if(FromFC_VarioCharacter == '+' || FromFC_VarioCharacter == '-') // manual setpoint clears the NC-Parameter command
617
                                 {
555
                                 {
618
                                  NCParams_ClearValue(NCPARAMS_ALTITUDE_RATE);
556
                                  NCParams_ClearValue(NCPARAMS_ALTITUDE_RATE);
619
                                 }
557
                                 }
620
                                NaviData.UBat = FC.BAT_Voltage;
558
                                NaviData.UBat = FC.BAT_Voltage;
621
                                NaviData.Current = FC.BAT_Current;
559
                                NaviData.Current = FC.BAT_Current;
622
                                NaviData.UsedCapacity = FC.BAT_UsedCapacity;
560
                                NaviData.UsedCapacity = FC.BAT_UsedCapacity;
623
                                break;
561
                                break;
624
 
562
 
625
#define CHK_POTI(b,a) { if(a < 248) b = a; else b = FC.Poti[255 - a]; }
563
#define CHK_POTI(b,a) { if(a < 248) b = a; else b = FC.Poti[255 - a]; }
626
#define CHK_POTI_MM(b,a,min,max) {CHK_POTI(b,a); LIMIT_MIN_MAX(b, min, max); }
564
#define CHK_POTI_MM(b,a,min,max) {CHK_POTI(b,a); LIMIT_MIN_MAX(b, min, max); }
627
 
565
 
628
                        case SPI_FCCMD_PARAMETER1:
566
                        case SPI_FCCMD_PARAMETER1:
629
                                CHK_POTI_MM(Parameter.NaviGpsModeControl,FromFlightCtrl.Param.Byte[0],0,255);
567
                                CHK_POTI_MM(Parameter.NaviGpsModeControl,FromFlightCtrl.Param.Byte[0],0,255);
630
                                CHK_POTI_MM(Parameter.NaviGpsGain,FromFlightCtrl.Param.Byte[1],0,255);
568
                                CHK_POTI_MM(Parameter.NaviGpsGain,FromFlightCtrl.Param.Byte[1],0,255);
631
                                CHK_POTI_MM(Parameter.NaviGpsP,FromFlightCtrl.Param.Byte[2],0,255);
569
                                CHK_POTI_MM(Parameter.NaviGpsP,FromFlightCtrl.Param.Byte[2],0,255);
632
                                CHK_POTI_MM(Parameter.NaviGpsI,FromFlightCtrl.Param.Byte[3],0,255);
570
                                CHK_POTI_MM(Parameter.NaviGpsI,FromFlightCtrl.Param.Byte[3],0,255);
633
                                CHK_POTI_MM(Parameter.NaviGpsD,FromFlightCtrl.Param.Byte[4],0,255);
571
                                CHK_POTI_MM(Parameter.NaviGpsD,FromFlightCtrl.Param.Byte[4],0,255);
634
                                CHK_POTI_MM(Parameter.NaviGpsACC,FromFlightCtrl.Param.Byte[5],0,255);
572
                                CHK_POTI_MM(Parameter.NaviGpsACC,FromFlightCtrl.Param.Byte[5],0,255);
635
                                Parameter.NaviGpsMinSat = FromFlightCtrl.Param.Byte[6];
573
                                Parameter.NaviGpsMinSat = FromFlightCtrl.Param.Byte[6];
636
                                Parameter.NaviStickThreshold = FromFlightCtrl.Param.Byte[7];
574
                                Parameter.NaviStickThreshold = FromFlightCtrl.Param.Byte[7];
637
                                CHK_POTI_MM(Parameter.NaviOperatingRadius,FromFlightCtrl.Param.Byte[8],0,255);
575
                                CHK_POTI_MM(Parameter.NaviOperatingRadius,FromFlightCtrl.Param.Byte[8],0,255);
638
                                CHK_POTI_MM(Parameter.NaviWindCorrection,FromFlightCtrl.Param.Byte[9],0,255);
576
                                CHK_POTI_MM(Parameter.NaviWindCorrection,FromFlightCtrl.Param.Byte[9],0,255);
639
                                CHK_POTI_MM(Parameter.NaviAccCompensation,FromFlightCtrl.Param.Byte[10],0,255);
577
                                CHK_POTI_MM(Parameter.NaviAccCompensation,FromFlightCtrl.Param.Byte[10],0,255);
640
                                CHK_POTI_MM(Parameter.NaviAngleLimitation,FromFlightCtrl.Param.Byte[11],0,255);
578
                                CHK_POTI_MM(Parameter.NaviAngleLimitation,FromFlightCtrl.Param.Byte[11],0,255);
641
                                break;
579
                                break;
642
 
580
 
643
                        case SPI_FCCMD_STICK:
581
                        case SPI_FCCMD_STICK:
644
                                FC.StickGas     = FromFlightCtrl.Param.sByte[0];
582
                                FC.StickGas     = FromFlightCtrl.Param.sByte[0];
645
                                FC.StickYaw     = FromFlightCtrl.Param.sByte[1];
583
                                FC.StickYaw     = FromFlightCtrl.Param.sByte[1];
646
                                FC.StickRoll    = FromFlightCtrl.Param.sByte[2];
584
                                FC.StickRoll    = FromFlightCtrl.Param.sByte[2];
647
                                FC.StickNick    = FromFlightCtrl.Param.sByte[3];
585
                                FC.StickNick    = FromFlightCtrl.Param.sByte[3];
648
                                FC.Poti[0]              = FromFlightCtrl.Param.Byte[4];
586
                                FC.Poti[0]              = FromFlightCtrl.Param.Byte[4];
649
                                FC.Poti[1]              = FromFlightCtrl.Param.Byte[5];
587
                                FC.Poti[1]              = FromFlightCtrl.Param.Byte[5];
650
                                FC.Poti[2]              = FromFlightCtrl.Param.Byte[6];
588
                                FC.Poti[2]              = FromFlightCtrl.Param.Byte[6];
651
                                FC.Poti[3]              = FromFlightCtrl.Param.Byte[7];
589
                                FC.Poti[3]              = FromFlightCtrl.Param.Byte[7];
652
                                FC.Poti[4]              = FromFlightCtrl.Param.Byte[8];
590
                                FC.Poti[4]              = FromFlightCtrl.Param.Byte[8];
653
                                FC.Poti[5]              = FromFlightCtrl.Param.Byte[9];
591
                                FC.Poti[5]              = FromFlightCtrl.Param.Byte[9];
654
                                FC.Poti[6]              = FromFlightCtrl.Param.Byte[10];
592
                                FC.Poti[6]              = FromFlightCtrl.Param.Byte[10];
655
                                FC.Poti[7]              = FromFlightCtrl.Param.Byte[11];
593
                                FC.Poti[7]              = FromFlightCtrl.Param.Byte[11];
656
                                break;
594
                                break;
657
 
595
 
658
                        case SPI_FCCMD_MISC:
596
                        case SPI_FCCMD_MISC:
659
                                if(CompassCalState != FromFlightCtrl.Param.Byte[0])
597
                                if(CompassCalState != FromFlightCtrl.Param.Byte[0])
660
                                {       // put only new CompassCalState into queue to send via I2C
598
                                {       // put only new CompassCalState into queue to send via I2C
661
                                        if(FromFlightCtrl.Param.Byte[0] == CompassCalState+1)
599
                                        if(FromFlightCtrl.Param.Byte[0] == CompassCalState+1)
662
                                         {
600
                                         {
663
                                           CompassCalState = FromFlightCtrl.Param.Byte[0];
601
                                           CompassCalState = FromFlightCtrl.Param.Byte[0];
664
                                           Compass_SetCalState(CompassCalState);
602
                                           Compass_SetCalState(CompassCalState);
665
                                         }
603
                                         }
666
                                         else CompassCalState = 0;
604
                                         else CompassCalState = 0;
667
                                }
605
                                }
668
                                Parameter.NaviPH_LoginTime = FromFlightCtrl.Param.Byte[1];
606
                                Parameter.NaviPH_LoginTime = FromFlightCtrl.Param.Byte[1];
669
                                NaviData.Variometer = (NaviData.Variometer + 2 * (FromFlightCtrl.Param.sInt[1] - NaviData.Altimeter)) / 2; // provisorisch
607
                                NaviData.Variometer = (NaviData.Variometer + 2 * (FromFlightCtrl.Param.sInt[1] - NaviData.Altimeter)) / 2; // provisorisch
670
                                NaviData.Altimeter = FromFlightCtrl.Param.sInt[1]; // in 5cm
608
                                NaviData.Altimeter = FromFlightCtrl.Param.sInt[1]; // in 5cm
671
                                NaviData.SetpointAltitude = FromFlightCtrl.Param.sInt[2]; // in 5cm
609
                                NaviData.SetpointAltitude = FromFlightCtrl.Param.sInt[2]; // in 5cm
672
                                CHK_POTI_MM(Parameter.NaviGpsPLimit,FromFlightCtrl.Param.Byte[6],0,255);
610
                                CHK_POTI_MM(Parameter.NaviGpsPLimit,FromFlightCtrl.Param.Byte[6],0,255);
673
                                CHK_POTI_MM(Parameter.NaviGpsILimit,FromFlightCtrl.Param.Byte[7],0,255);
611
                                CHK_POTI_MM(Parameter.NaviGpsILimit,FromFlightCtrl.Param.Byte[7],0,255);
674
                                CHK_POTI_MM(Parameter.NaviGpsDLimit,FromFlightCtrl.Param.Byte[8],0,255);
612
                                CHK_POTI_MM(Parameter.NaviGpsDLimit,FromFlightCtrl.Param.Byte[8],0,255);
675
                                FC.RC_Quality   = FromFlightCtrl.Param.Byte[9];
613
                                FC.RC_Quality   = FromFlightCtrl.Param.Byte[9];
676
                                FC.RC_RSSI              = FromFlightCtrl.Param.Byte[10];
614
                                FC.RC_RSSI              = FromFlightCtrl.Param.Byte[10];
677
                                if(!FC.RC_RSSI) NaviData.RC_Quality = FC.RC_Quality; else NaviData.RC_Quality = FC.RC_RSSI;
615
                                if(!FC.RC_RSSI) NaviData.RC_Quality = FC.RC_Quality; else NaviData.RC_Quality = FC.RC_RSSI;
678
//                              NaviData.RC_RSSI = FC.RC_RSSI;
616
//                              NaviData.RC_RSSI = FC.RC_RSSI;
679
                                NaviData.Gas    = (FC.BAT_Voltage * (u32) FromFlightCtrl.Param.Byte[11]) / (u32) Parameter.LowVoltageWarning;
617
                                NaviData.Gas    = (FC.BAT_Voltage * (u32) FromFlightCtrl.Param.Byte[11]) / (u32) Parameter.LowVoltageWarning;
680
                                break;
618
                                break;
681
 
619
 
682
                        case SPI_FCCMD_SERVOS:
620
                        case SPI_FCCMD_SERVOS:
683
                                ServoParams.Refresh             = FromFlightCtrl.Param.Byte[0];
621
                                ServoParams.Refresh             = FromFlightCtrl.Param.Byte[0];
684
                                ServoParams.CompInvert  = FromFlightCtrl.Param.Byte[1];
622
                                ServoParams.CompInvert  = FromFlightCtrl.Param.Byte[1];
685
                                ServoParams.NickControl = FromFlightCtrl.Param.Byte[2];
623
                                ServoParams.NickControl = FromFlightCtrl.Param.Byte[2];
686
                                ServoParams.NickComp    = FromFlightCtrl.Param.Byte[3];
624
                                ServoParams.NickComp    = FromFlightCtrl.Param.Byte[3];
687
                                ServoParams.NickMin             = FromFlightCtrl.Param.Byte[4];
625
                                ServoParams.NickMin             = FromFlightCtrl.Param.Byte[4];
688
                                ServoParams.NickMax             = FromFlightCtrl.Param.Byte[5];
626
                                ServoParams.NickMax             = FromFlightCtrl.Param.Byte[5];
689
                                ServoParams.RollControl = FromFlightCtrl.Param.Byte[6];
627
                                ServoParams.RollControl = FromFlightCtrl.Param.Byte[6];
690
                                ServoParams.RollComp    = FromFlightCtrl.Param.Byte[7];
628
                                ServoParams.RollComp    = FromFlightCtrl.Param.Byte[7];
691
                                ServoParams.RollMin             = FromFlightCtrl.Param.Byte[8];
629
                                ServoParams.RollMin             = FromFlightCtrl.Param.Byte[8];
692
                                ServoParams.RollMax             = FromFlightCtrl.Param.Byte[9];
630
                                ServoParams.RollMax             = FromFlightCtrl.Param.Byte[9];
693
                                BL_MinOfMaxPWM = FromFlightCtrl.Param.Byte[10];
631
                                BL_MinOfMaxPWM = FromFlightCtrl.Param.Byte[10];
694
                                break;
632
                                break;
695
 
633
 
696
                        case SPI_FCCMD_VERSION:
634
                        case SPI_FCCMD_VERSION:
697
                                FC_Version.Major                = FromFlightCtrl.Param.Byte[0];
635
                                FC_Version.Major                = FromFlightCtrl.Param.Byte[0];
698
                                FC_Version.Minor                = FromFlightCtrl.Param.Byte[1];
636
                                FC_Version.Minor                = FromFlightCtrl.Param.Byte[1];
699
                                FC_Version.Patch                = FromFlightCtrl.Param.Byte[2];
637
                                FC_Version.Patch                = FromFlightCtrl.Param.Byte[2];
700
                                FC_Version.Compatible   = FromFlightCtrl.Param.Byte[3];
638
                                FC_Version.Compatible   = FromFlightCtrl.Param.Byte[3];
701
                                FC_Version.Hardware             = FromFlightCtrl.Param.Byte[4];
639
                                FC_Version.Hardware             = FromFlightCtrl.Param.Byte[4];
702
                                FC.Error[0]                     |= FromFlightCtrl.Param.Byte[5];
640
                                FC.Error[0]                     |= FromFlightCtrl.Param.Byte[5];
703
                                FC.Error[1]                     |= FromFlightCtrl.Param.Byte[6];
641
                                FC.Error[1]                     |= FromFlightCtrl.Param.Byte[6];
704
                                FC.Error[2]                     |= FromFlightCtrl.Param.Byte[7];
642
                                FC.Error[2]                     |= FromFlightCtrl.Param.Byte[7];
705
                                Parameter.GlobalConfig = FromFlightCtrl.Param.Byte[8];
643
                                Parameter.GlobalConfig = FromFlightCtrl.Param.Byte[8];
706
                                Parameter.ExtraConfig = FromFlightCtrl.Param.Byte[9];
644
                                Parameter.ExtraConfig = FromFlightCtrl.Param.Byte[9];
707
                                Parameter.OrientationAngle = FromFlightCtrl.Param.Byte[10];
645
                                Parameter.OrientationAngle = FromFlightCtrl.Param.Byte[10];
708
                                Parameter.GlobalConfig3 = FromFlightCtrl.Param.Byte[11];
646
                                Parameter.GlobalConfig3 = FromFlightCtrl.Param.Byte[11];
709
                                DebugOut.StatusGreen |= AMPEL_FC; // status of FC Present
647
                                DebugOut.StatusGreen |= AMPEL_FC; // status of FC Present
710
                                DebugOut.StatusGreen |= AMPEL_BL; // status of BL Present
648
                                DebugOut.StatusGreen |= AMPEL_BL; // status of BL Present
711
                                if(FC.Error[0] || FC.Error[1] || FC.Error[2] || FC.Error[3] || FC.Error[4]) DebugOut.StatusRed |= AMPEL_FC;
649
                                if(FC.Error[0] || FC.Error[1] || FC.Error[2] || FC.Error[3] || FC.Error[4]) DebugOut.StatusRed |= AMPEL_FC;
712
                                else DebugOut.StatusRed &= ~AMPEL_FC;
650
                                else DebugOut.StatusRed &= ~AMPEL_FC;
713
                                break;
651
                                break;
714
                        default:
652
                        default:
715
                                break;
653
                                break;
716
                }
654
                }
717
 
655
 
718
                DebugOut.Analog[0] = FromFlightCtrl.AngleNick;
656
                DebugOut.Analog[0] = FromFlightCtrl.AngleNick;
719
                DebugOut.Analog[1] = FromFlightCtrl.AngleRoll;
657
                DebugOut.Analog[1] = FromFlightCtrl.AngleRoll;
720
                DebugOut.Analog[2] = FromFlightCtrl.AccNick;
658
                DebugOut.Analog[2] = FromFlightCtrl.AccNick;
721
                DebugOut.Analog[3] = FromFlightCtrl.AccRoll;
659
                DebugOut.Analog[3] = FromFlightCtrl.AccRoll;
722
                DebugOut.Analog[11] = FromFlightCtrl.GyroHeading/10;// in deg
660
                DebugOut.Analog[11] = FromFlightCtrl.GyroHeading/10;// in deg
723
                Data3D.AngleNick = FromFlightCtrl.AngleNick;            // in 0.1 deg
661
                Data3D.AngleNick = FromFlightCtrl.AngleNick;            // in 0.1 deg
724
                Data3D.AngleRoll = FromFlightCtrl.AngleRoll;            // in 0.1 deg
662
                Data3D.AngleRoll = FromFlightCtrl.AngleRoll;            // in 0.1 deg
725
                Data3D.Heading   = FromFlightCtrl.GyroHeading;          // in 0.1 deg
663
                Data3D.Heading   = FromFlightCtrl.GyroHeading;          // in 0.1 deg
726
                // every time we got new data from the FC via SPI call the navigation routine
664
                // every time we got new data from the FC via SPI call the navigation routine
727
                // and update GPSStick that are returned to FC
665
                // and update GPSStick that are returned to FC
728
                SPI_RxBuffer_Request = 0;
666
                SPI_RxBuffer_Request = 0;
729
 
667
 
730
 
668
 
731
/*
669
/*
732
//+++++++++++++++++++++++++++++++++++++++++++++++++++
670
//+++++++++++++++++++++++++++++++++++++++++++++++++++
733
*/
671
*/
734
/*
-
 
735
if(Parameter.User8 < 100) FC.StatusFlags = 0;
-
 
736
else
-
 
737
if(Parameter.User8 < 150) FC.StatusFlags = FC_STATUS_START;
-
 
738
else FC.StatusFlags = FC_STATUS_FLY | FC_STATUS_MOTOR_RUN;
-
 
739
BL_MinOfMaxPWM = 255;
-
 
740
*/
-
 
741
                GPS_Navigation(&GPSData, &(ToFlightCtrl.GPSStick));
672
                GPS_Navigation(&GPSData, &(ToFlightCtrl.GPSStick));
742
                ClearFCStatusFlags = 1;
673
                ClearFCStatusFlags = 1;
743
                if(counter)
674
                if(counter)
744
                {
675
                {
745
                        counter--;                                       // count down to enable servo
676
                        counter--;                                       // count down to enable servo
746
                        if(!counter) TIMER2_Init();  // enable Servo Output
677
                        if(!counter) TIMER2_Init();  // enable Servo Output
747
                }
678
                }
748
                timeout = SetDelay(80); // 80 ms, new data are send every 20 ms
679
                timeout = SetDelay(80); // 80 ms, new data are send every 20 ms
749
 
680
 
750
        }       // EOF if(SPI_RxBuffer_Request)
681
        }       // EOF if(SPI_RxBuffer_Request)
751
        else // no new SPI data
682
        else // no new SPI data
752
        {
683
        {
753
                if(CheckDelay(timeout) && (counter == 0))
684
                if(CheckDelay(timeout) && (counter == 0))
754
                {
685
                {
755
                        TIMER2_Deinit();  // disable Servo Output
686
                        TIMER2_Deinit();  // disable Servo Output
756
                        counter = 50;     // reset counter for enabling Servo Output
687
                        counter = 50;     // reset counter for enabling Servo Output
757
                }
688
                }
758
        }
689
        }
759
}
690
}
760
 
691
 
761
//------------------------------------------------------
692
//------------------------------------------------------
762
void SPI0_GetFlightCtrlVersion(void)
693
void SPI0_GetFlightCtrlVersion(void)
763
{
694
{
764
        u32 timeout;
695
        u32 timeout;
765
        u8 repeat;
696
        u8 repeat;
766
        u8 msg[64];
697
        u8 msg[64];
767
 
698
 
768
        UART1_PutString("\r\n Looking for FlightControl");
699
        UART1_PutString("\r\n Looking for FlightControl");
769
        FC_Version.Major = 0xFF;
700
        FC_Version.Major = 0xFF;
770
        FC_Version.Minor = 0xFF;
701
        FC_Version.Minor = 0xFF;
771
        FC_Version.Patch = 0xFF;
702
        FC_Version.Patch = 0xFF;
772
        FC_Version.Compatible = 0xFF;
703
        FC_Version.Compatible = 0xFF;
773
 
704
 
774
        // polling FC version info
705
        // polling FC version info
775
        repeat = 0;
706
        repeat = 0;
776
        do
707
        do
777
        {
708
        {
778
                timeout = SetDelay(250);
709
                timeout = SetDelay(250);
779
                do
710
                do
780
                {
711
                {
781
                        SPI0_UpdateBuffer();
712
                        SPI0_UpdateBuffer();
782
                        if (FC_Version.Major != 0xFF)  break;
713
                        if (FC_Version.Major != 0xFF)  break;
783
                }while (!CheckDelay(timeout));
714
                }while (!CheckDelay(timeout));
784
                UART1_PutString(".");
715
                UART1_PutString(".");
785
                repeat++;
716
                repeat++;
786
                FCCalibActive = 1;
717
                FCCalibActive = 1;
787
        }while((FC_Version.Major == 0xFF) && (repeat < 40)); // 40*250ms = 10s
718
        }while((FC_Version.Major == 0xFF) && (repeat < 40)); // 40*250ms = 10s
788
        // if we got it
719
        // if we got it
789
        if (FC_Version.Major != 0xFF)
720
        if (FC_Version.Major != 0xFF)
790
        {
721
        {
791
                sprintf(msg, " FC V%d.%d%c HW:%d.%d", FC_Version.Major, FC_Version.Minor, 'a'+FC_Version.Patch, FC_Version.Hardware/10,FC_Version.Hardware%10);
722
                sprintf(msg, " FC V%d.%d%c HW:%d.%d", FC_Version.Major, FC_Version.Minor, 'a'+FC_Version.Patch, FC_Version.Hardware/10,FC_Version.Hardware%10);
792
                UART1_PutString(msg);
723
                UART1_PutString(msg);
793
        }
724
        }
794
        else UART1_PutString("\n\r not found!");
725
        else UART1_PutString("\n\r not found!");
795
}
726
}
796
 
727
 
797
 
728
 
798
 
729