Subversion Repositories NaviCtrl

Rev

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

Rev 300 Rev 306
1 
/*#######################################################################################*/
 1 
/*#######################################################################################*/
2 
/* !!! THIS IS NOT FREE SOFTWARE !!!                                                     */
 2 
/* !!! THIS IS NOT FREE SOFTWARE !!!                                                     */
3 
/*#######################################################################################*/
 3 
/*#######################################################################################*/
4 
// ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
 4 
// ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
5 
// + Copyright (c) 2008 Ingo Busker, Holger Buss
 5 
// + Copyright (c) 2008 Ingo Busker, Holger Buss
6 
// + Nur für den privaten Gebrauch / NON-COMMERCIAL USE ONLY
 6 
// + Nur für den privaten Gebrauch / NON-COMMERCIAL USE ONLY
7 
// + FOR NON COMMERCIAL USE ONLY
 7 
// + FOR NON COMMERCIAL USE ONLY
8 
// + www.MikroKopter.com
 8 
// + www.MikroKopter.com
9 
// ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
 9 
// ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
10 
// + Es gilt für das gesamte Projekt (Hardware, Software, Binärfiles, Sourcecode und Dokumentation),
 10 
// + Es gilt für das gesamte Projekt (Hardware, Software, Binärfiles, Sourcecode und Dokumentation),
11 
// + dass eine Nutzung (auch auszugsweise) nur für den privaten (nicht-kommerziellen) Gebrauch zulässig ist.
 11 
// + dass eine Nutzung (auch auszugsweise) nur für den privaten (nicht-kommerziellen) Gebrauch zulässig ist.
12 
// + Sollten direkte oder indirekte kommerzielle Absichten verfolgt werden, ist mit uns (info@mikrokopter.de) Kontakt
 12 
// + Sollten direkte oder indirekte kommerzielle Absichten verfolgt werden, ist mit uns (info@mikrokopter.de) Kontakt
13 
// + bzgl. der Nutzungsbedingungen aufzunehmen.
 13 
// + bzgl. der Nutzungsbedingungen aufzunehmen.
14 
// + Eine kommerzielle Nutzung ist z.B.Verkauf von MikroKoptern, Bestückung und Verkauf von Platinen oder Bausätzen,
 14 
// + Eine kommerzielle Nutzung ist z.B.Verkauf von MikroKoptern, Bestückung und Verkauf von Platinen oder Bausätzen,
15 
// + Verkauf von Luftbildaufnahmen, usw.
 15 
// + Verkauf von Luftbildaufnahmen, usw.
16 
// ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
 16 
// ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
17 
// + Werden Teile des Quellcodes (mit oder ohne Modifikation) weiterverwendet oder veröffentlicht,
 17 
// + Werden Teile des Quellcodes (mit oder ohne Modifikation) weiterverwendet oder veröffentlicht,
18 
// + unterliegen sie auch diesen Nutzungsbedingungen und diese Nutzungsbedingungen incl. Copyright müssen dann beiliegen
 18 
// + unterliegen sie auch diesen Nutzungsbedingungen und diese Nutzungsbedingungen incl. Copyright müssen dann beiliegen
19 
// ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
 19 
// ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
20 
// + Sollte die Software (auch auszugesweise) oder sonstige Informationen des MikroKopter-Projekts
 20 
// + Sollte die Software (auch auszugesweise) oder sonstige Informationen des MikroKopter-Projekts
21 
// + auf anderen Webseiten oder sonstigen Medien veröffentlicht werden, muss unsere Webseite "http://www.mikrokopter.de"
 21 
// + auf anderen Webseiten oder sonstigen Medien veröffentlicht werden, muss unsere Webseite "http://www.mikrokopter.de"
22 
// + eindeutig als Ursprung verlinkt werden
 22 
// + eindeutig als Ursprung verlinkt werden
23 
// ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
 23 
// ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
24 
// + Keine Gewähr auf Fehlerfreiheit, Vollständigkeit oder Funktion
 24 
// + Keine Gewähr auf Fehlerfreiheit, Vollständigkeit oder Funktion
25 
// + Benutzung auf eigene Gefahr
 25 
// + Benutzung auf eigene Gefahr
26 
// + Wir übernehmen keinerlei Haftung für direkte oder indirekte Personen- oder Sachschäden
 26 
// + Wir übernehmen keinerlei Haftung für direkte oder indirekte Personen- oder Sachschäden
27 
// ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
 27 
// ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
28 
// + Die Portierung oder Nutzung der Software (oder Teile davon) auf andere Systeme (ausser der Hardware von www.mikrokopter.de) ist nur
 28 
// + Die Portierung oder Nutzung der Software (oder Teile davon) auf andere Systeme (ausser der Hardware von www.mikrokopter.de) ist nur
29 
// + mit unserer Zustimmung zulässig
 29 
// + mit unserer Zustimmung zulässig
30 
// ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
 30 
// ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
31 
// + Die Funktion printf_P() unterliegt ihrer eigenen Lizenz und ist hiervon nicht betroffen
 31 
// + Die Funktion printf_P() unterliegt ihrer eigenen Lizenz und ist hiervon nicht betroffen
32 
// ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
 32 
// ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
33 
// + Redistributions of source code (with or without modifications) must retain the above copyright notice,
 33 
// + Redistributions of source code (with or without modifications) must retain the above copyright notice,
34 
// + this list of conditions and the following disclaimer.
 34 
// + this list of conditions and the following disclaimer.
35 
// +   * Neither the name of the copyright holders nor the names of contributors may be used to endorse or promote products derived
 35 
// +   * Neither the name of the copyright holders nor the names of contributors may be used to endorse or promote products derived
36 
// +     from this software without specific prior written permission.
 36 
// +     from this software without specific prior written permission.
37 
// +   * The use of this project (hardware, software, binary files, sources and documentation) is only permitted
 37 
// +   * The use of this project (hardware, software, binary files, sources and documentation) is only permitted
38 
// +     for non-commercial use (directly or indirectly)
 38 
// +     for non-commercial use (directly or indirectly)
39 
// +     Commercial use (for excample: selling of MikroKopters, selling of PCBs, assembly, ...) is only permitted
 39 
// +     Commercial use (for excample: selling of MikroKopters, selling of PCBs, assembly, ...) is only permitted
40 
// +     with our written permission
 40 
// +     with our written permission
41 
// +   * If sources or documentations are redistributet on other webpages, out webpage (http://www.MikroKopter.de) must be
 41 
// +   * If sources or documentations are redistributet on other webpages, out webpage (http://www.MikroKopter.de) must be
42 
// +     clearly linked as origin
 42 
// +     clearly linked as origin
43 
// +   * porting the sources to other systems or using the software on other systems (except hardware from www.mikrokopter.de) is not allowed
 43 
// +   * porting the sources to other systems or using the software on other systems (except hardware from www.mikrokopter.de) is not allowed
44 
//
 44 
//
45 
// +  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
 45 
// +  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
46 
// +  AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
 46 
// +  AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
47 
// +  IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
 47 
// +  IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
48 
// +  ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE
 48 
// +  ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE
49 
// +  LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
 49 
// +  LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
50 
// +  CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
 50 
// +  CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
51 
// +  SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
 51 
// +  SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
52 
// +  INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
 52 
// +  INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
53 
// +  CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
 53 
// +  CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
54 
// +  ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
 54 
// +  ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
55 
// +  POSSIBILITY OF SUCH DAMAGE.
 55 
// +  POSSIBILITY OF SUCH DAMAGE.
56 
// ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
 56 
// ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
57 
 
 57 
 
58 
#include <string.h>
 58 
#include <string.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 
 
 71 
 
72 
#define SPI_RXSYNCBYTE1 0xAA
 72 
#define SPI_RXSYNCBYTE1 0xAA
73 
#define SPI_RXSYNCBYTE2 0x83
 73 
#define SPI_RXSYNCBYTE2 0x83
74 
#define SPI_TXSYNCBYTE1 0x81
 74 
#define SPI_TXSYNCBYTE1 0x81
75 
#define SPI_TXSYNCBYTE2 0x55
 75 
#define SPI_TXSYNCBYTE2 0x55
76 
 
 76 
 
77 
//communication packets
 77 
//communication packets
78 
FromFlightCtrl_t   FromFlightCtrl;
 78 
FromFlightCtrl_t   FromFlightCtrl;
79 
ToFlightCtrl_t     ToFlightCtrl;
 79 
ToFlightCtrl_t     ToFlightCtrl;
80 
#define SPI0_TIMEOUT 500 // 500ms
 80 
#define SPI0_TIMEOUT 500 // 500ms
81 
volatile u32 SPI0_Timeout = 0;
 81 
volatile u32 SPI0_Timeout = 0;
82 
 
 82 
 
83 
// tx packet buffer
 83 
// tx packet buffer
84 
#define SPI_TXBUFFER_LEN (2 + sizeof(ToFlightCtrl)) // 2 bytes at start are for synchronization
 84 
#define SPI_TXBUFFER_LEN (2 + sizeof(ToFlightCtrl)) // 2 bytes at start are for synchronization
85 
volatile u8 SPI_TxBuffer[SPI_TXBUFFER_LEN];
 85 
volatile u8 SPI_TxBuffer[SPI_TXBUFFER_LEN];
86 
volatile u8 SPI_TxBufferIndex = 0;
 86 
volatile u8 SPI_TxBufferIndex = 0;
87 
u8 *Ptr_TxChksum = NULL ;  // pointer to checksum in TxBuffer
 87 
u8 *Ptr_TxChksum = NULL ;  // pointer to checksum in TxBuffer
88 
 
 88 
 
89 
// rx packet buffer
 89 
// rx packet buffer
90 
#define SPI_RXBUFFER_LEN sizeof(FromFlightCtrl)
 90 
#define SPI_RXBUFFER_LEN sizeof(FromFlightCtrl)
91 
volatile u8 SPI_RxBuffer[SPI_RXBUFFER_LEN];
 91 
volatile u8 SPI_RxBuffer[SPI_RXBUFFER_LEN];
92 
volatile u8 SPI_RxBufferIndex = 0;
 92 
volatile u8 SPI_RxBufferIndex = 0;
93 
volatile u8 SPI_RxBuffer_Request = 0;
 93 
volatile u8 SPI_RxBuffer_Request = 0;
94 
#define SPI_COMMAND_INDEX 0
 94 
#define SPI_COMMAND_INDEX 0
95 
 
 95 
 
96 
s32 Kalman_K = 32;
 96 
s32 Kalman_K = 32;
97 
s32 Kalman_MaxDrift = 5 * 16;
 97 
s32 Kalman_MaxDrift = 5 * 16;
98 
s32 Kalman_MaxFusion = 64;
 98 
s32 Kalman_MaxFusion = 64;
99 
s32 ToFcGpsZ = 0;
 99 
s32 ToFcGpsZ = 0;
100 
 
 100 
 
101 
u8 SPI_CommandSequence[] = { SPI_NCCMD_VERSION, SPI_NCCMD_KALMAN, SPI_NCCMD_GPSINFO ,SPI_NCCMD_KALMAN, SPI_NCCMD_KALMAN};
 101 
u8 SPI_CommandSequence[] = { SPI_NCCMD_VERSION, SPI_NCCMD_KALMAN, SPI_NCCMD_GPSINFO ,SPI_NCCMD_KALMAN, SPI_NCCMD_KALMAN};
102 
u8 SPI_CommandCounter = 0;
 102 
u8 SPI_CommandCounter = 0;
103 
s32 ToFC_Rotate_C = 64, ToFC_Rotate_S = 0;
 103 
s32 ToFC_Rotate_C = 64, ToFC_Rotate_S = 0;
104 
s32 HeadFreeStartAngle = 0;
 104 
s32 HeadFreeStartAngle = 0;
105 
s16 FC_WP_EventChannel = 0; // gibt einen Schaltkanal an die FC weiter, wenn der Wegpunkt erreicht wurde
 105 
s16 FC_WP_EventChannel = 0; // gibt einen Schaltkanal an die FC weiter, wenn der Wegpunkt erreicht wurde
106 
u32 ToFC_AltitudeRate = 0;
 106 
u32 ToFC_AltitudeRate = 0;
107 
s32 ToFC_AltitudeSetpoint = 0;
 107 
s32 ToFC_AltitudeSetpoint = 0;
108 
u8  FromFC_VarioCharacter = ' ';
 108 
u8  FromFC_VarioCharacter = ' ';
109 
 
 109 
 
110 
SPI_Version_t FC_Version;
 110 
SPI_Version_t FC_Version;
111 
 
 111 
 
112 
//--------------------------------------------------------------
 112 
//--------------------------------------------------------------
113 
void SSP0_IRQHandler(void)
 113 
void SSP0_IRQHandler(void)
114 
{
 114 
{
115 
        static u8 rxchksum = 0;
 115 
        static u8 rxchksum = 0;
116 
        u8 rxdata;
 116 
        u8 rxdata;
117 
 
 117 
 
118 
        #define SPI_SYNC1       0
 118 
        #define SPI_SYNC1       0
119 
        #define SPI_SYNC2       1
 119 
        #define SPI_SYNC2       1
120 
        #define SPI_DATA        2
 120 
        #define SPI_DATA        2
121 
        static u8 SPI_State = SPI_SYNC1;
 121 
        static u8 SPI_State = SPI_SYNC1;
122 
 
 122 
 
123 
        IENABLE;
 123 
        IENABLE;
124 
 
 124 
 
125 
        // clear pending bits
 125 
        // clear pending bits
126 
        SSP_ClearITPendingBit(SSP0, SSP_IT_RxTimeOut);
 126 
        SSP_ClearITPendingBit(SSP0, SSP_IT_RxTimeOut);
127 
        SSP_ClearITPendingBit(SSP0, SSP_IT_RxFifo);
 127 
        SSP_ClearITPendingBit(SSP0, SSP_IT_RxFifo);
128 
 
 128 
 
129 
        // while RxFIFO not empty
 129 
        // while RxFIFO not empty
130 
        while (SSP_GetFlagStatus(SSP0, SSP_FLAG_RxFifoNotEmpty) == SET)
 130 
        while (SSP_GetFlagStatus(SSP0, SSP_FLAG_RxFifoNotEmpty) == SET)
131 
        {
 131 
        {
132 
                rxdata =  SSP0->DR; // catch the received byte
 132 
                rxdata =  SSP0->DR; // catch the received byte
133 
                // Fill TxFIFO while its not full or end of packet is reached
 133 
                // Fill TxFIFO while its not full or end of packet is reached
134 
                while (SSP_GetFlagStatus(SSP0, SSP_FLAG_TxFifoNotFull) == SET)
 134 
                while (SSP_GetFlagStatus(SSP0, SSP_FLAG_TxFifoNotFull) == SET)
135 
                {
 135 
                {
136 
                        if (SPI_TxBufferIndex  < SPI_TXBUFFER_LEN)   // still data to send ?
 136 
                        if (SPI_TxBufferIndex  < SPI_TXBUFFER_LEN)   // still data to send ?
137 
                        {
 137 
                        {
138 
                                SSP0->DR = SPI_TxBuffer[SPI_TxBufferIndex];       // send a byte
 138 
                                SSP0->DR = SPI_TxBuffer[SPI_TxBufferIndex];       // send a byte
139 
                                *Ptr_TxChksum += SPI_TxBuffer[SPI_TxBufferIndex]; // update checksum
 139 
                                *Ptr_TxChksum += SPI_TxBuffer[SPI_TxBufferIndex]; // update checksum
140 
                                SPI_TxBufferIndex++; // pointer to next byte
 140 
                                SPI_TxBufferIndex++; // pointer to next byte
141 
                        }
 141 
                        }
142 
                        else // end of packet is reached reset and copy data to tx buffer
 142 
                        else // end of packet is reached reset and copy data to tx buffer
143 
                        {
 143 
                        {
144 
                                SPI_TxBufferIndex = 0;  // reset buffer index
 144 
                                SPI_TxBufferIndex = 0;  // reset buffer index
145 
                                ToFlightCtrl.Chksum = 0;  // initialize checksum
 145 
                                ToFlightCtrl.Chksum = 0;  // initialize checksum
146 
                                ToFlightCtrl.BeepTime = BeepTime;  // set beeptime
 146 
                                ToFlightCtrl.BeepTime = BeepTime;  // set beeptime
147 
                                BeepTime = 0; // reset local beeptime
 147 
                                BeepTime = 0; // reset local beeptime
148 
                                // copy contents of ToFlightCtrl->SPI_TxBuffer
 148 
                                // copy contents of ToFlightCtrl->SPI_TxBuffer
149 
                                memcpy((u8 *) &(SPI_TxBuffer[2]), (u8 *) &ToFlightCtrl, sizeof(ToFlightCtrl));
 149 
                                memcpy((u8 *) &(SPI_TxBuffer[2]), (u8 *) &ToFlightCtrl, sizeof(ToFlightCtrl));
150 
                        }
 150 
                        }
151 
                }
 151 
                }
152 
                switch (SPI_State)
 152 
                switch (SPI_State)
153 
                {
 153 
                {
154 
                        case SPI_SYNC1:
 154 
                        case SPI_SYNC1:
155 
                                SPI_RxBufferIndex = 0; // reset buffer index
 155 
                                SPI_RxBufferIndex = 0; // reset buffer index
156 
                                rxchksum = rxdata;     // init checksum
 156 
                                rxchksum = rxdata;     // init checksum
157 
                                if (rxdata == SPI_RXSYNCBYTE1)
 157 
                                if (rxdata == SPI_RXSYNCBYTE1)
158 
                                {   // 1st syncbyte ok
 158 
                                {   // 1st syncbyte ok
159 
                                        SPI_State = SPI_SYNC2;  // step to sync2
 159 
                                        SPI_State = SPI_SYNC2;  // step to sync2
160 
                                }
 160 
                                }
161 
                                break;
 161 
                                break;
162 
                        case SPI_SYNC2:
 162 
                        case SPI_SYNC2:
163 
                                if (rxdata == SPI_RXSYNCBYTE2)
 163 
                                if (rxdata == SPI_RXSYNCBYTE2)
164 
                                {  // 2nd Syncbyte ok
 164 
                                {  // 2nd Syncbyte ok
165 
                                        rxchksum += rxdata;
 165 
                                        rxchksum += rxdata;
166 
                                        SPI_State = SPI_DATA;
 166 
                                        SPI_State = SPI_DATA;
167 
                                }  // 2nd Syncbyte does not match
 167 
                                }  // 2nd Syncbyte does not match
168 
                                else
 168 
                                else
169 
                                {
 169 
                                {
170 
                                        SPI_State  = SPI_SYNC1; //jump back to sync1
 170 
                                        SPI_State  = SPI_SYNC1; //jump back to sync1
171 
                                }
 171 
                                }
172 
                                break;
 172 
                                break;
173 
                        case SPI_DATA:
 173 
                        case SPI_DATA:
174 
                                SPI_RxBuffer[SPI_RxBufferIndex++]= rxdata; // copy databyte to rx buffer
 174 
                                SPI_RxBuffer[SPI_RxBufferIndex++]= rxdata; // copy databyte to rx buffer
175 
                                if (SPI_RxBufferIndex >= SPI_RXBUFFER_LEN) // end of packet is reached
 175 
                                if (SPI_RxBufferIndex >= SPI_RXBUFFER_LEN) // end of packet is reached
176 
                                {
 176 
                                {
177 
                                        if (rxdata == rxchksum) // verify checksum byte
 177 
                                        if (rxdata == rxchksum) // verify checksum byte
178 
                                        {
 178 
                                        {
179 
                                                // copy SPI_RxBuffer -> FromFlightCtrl
 179 
                                                // copy SPI_RxBuffer -> FromFlightCtrl
180 
                                                if(!SPI_RxBuffer_Request) // block writing to FromFlightCtrl on reading access
 180 
                                                if(!SPI_RxBuffer_Request) // block writing to FromFlightCtrl on reading access
181 
                                                {
 181 
                                                {
182 
                                                        memcpy((u8 *) &FromFlightCtrl, (u8 *) SPI_RxBuffer, sizeof(FromFlightCtrl));
 182 
                                                        memcpy((u8 *) &FromFlightCtrl, (u8 *) SPI_RxBuffer, sizeof(FromFlightCtrl));
183 
                                                        SPI_RxBuffer_Request = 1;
 183 
                                                        SPI_RxBuffer_Request = 1;
184 
                                                }
 184 
                                                }
185 
                                                // reset timeout counter on good packet
 185 
                                                // reset timeout counter on good packet
186 
                                                SPI0_Timeout = SetDelay(SPI0_TIMEOUT);
 186 
                                                SPI0_Timeout = SetDelay(SPI0_TIMEOUT);
187 
                                                DebugOut.Analog[13]++;
 187 
                                                DebugOut.Analog[13]++;
188 
                                        }
 188 
                                        }
189 
                                        else // bad checksum byte
 189 
                                        else // bad checksum byte
190 
                                        {
 190 
                                        {
191 
                                                DebugOut.Analog[12]++; // increase SPI chksum error counter
 191 
                                                DebugOut.Analog[12]++; // increase SPI chksum error counter
192 
                                        }
 192 
                                        }
193 
                                        SPI_State  = SPI_SYNC1; // reset state
 193 
                                        SPI_State  = SPI_SYNC1; // reset state
194 
                                }
 194 
                                }
195 
                                else // end of packet not reached
 195 
                                else // end of packet not reached
196 
                                {
 196 
                                {
197 
                                        rxchksum += rxdata;      // update checksum
 197 
                                        rxchksum += rxdata;      // update checksum
198 
                                }
 198 
                                }
199 
                                break;
 199 
                                break;
200 
                        default:
 200 
                        default:
201 
                                SPI_State  = SPI_SYNC1;
 201 
                                SPI_State  = SPI_SYNC1;
202 
                                break;
 202 
                                break;
203 
                }
 203 
                }
204 
        }
 204 
        }
205 
 
 205 
 
206 
        IDISABLE;
 206 
        IDISABLE;
207 
}
 207 
}
208 
 
 208 
 
209 
//--------------------------------------------------------------
 209 
//--------------------------------------------------------------
210 
void SPI0_Init(void)
 210 
void SPI0_Init(void)
211 
{
 211 
{
212 
        GPIO_InitTypeDef GPIO_InitStructure;
 212 
        GPIO_InitTypeDef GPIO_InitStructure;
213 
        SSP_InitTypeDef   SSP_InitStructure;
 213 
        SSP_InitTypeDef   SSP_InitStructure;
214 
 
 214 
 
215 
        UART1_PutString("\r\n SPI init...");
 215 
        UART1_PutString("\r\n SPI init...");
216 
 
 216 
 
217 
        SCU_APBPeriphClockConfig(__GPIO2 ,ENABLE);
 217 
        SCU_APBPeriphClockConfig(__GPIO2 ,ENABLE);
218 
        SCU_APBPeriphClockConfig(__SSP0 ,ENABLE);
 218 
        SCU_APBPeriphClockConfig(__SSP0 ,ENABLE);
219 
 
 219 
 
220 
        GPIO_DeInit(GPIO2);
 220 
        GPIO_DeInit(GPIO2);
221 
        //SSP0_CLK, SSP0_MOSI, SSP0_NSS pins
 221 
        //SSP0_CLK, SSP0_MOSI, SSP0_NSS pins
222 
        GPIO_StructInit(&GPIO_InitStructure);
 222 
        GPIO_StructInit(&GPIO_InitStructure);
223 
        GPIO_InitStructure.GPIO_Direction = GPIO_PinInput;
 223 
        GPIO_InitStructure.GPIO_Direction = GPIO_PinInput;
224 
        GPIO_InitStructure.GPIO_Pin = GPIO_Pin_4 | GPIO_Pin_5 | GPIO_Pin_7;
 224 
        GPIO_InitStructure.GPIO_Pin = GPIO_Pin_4 | GPIO_Pin_5 | GPIO_Pin_7;
225 
        GPIO_InitStructure.GPIO_Type = GPIO_Type_PushPull ;
 225 
        GPIO_InitStructure.GPIO_Type = GPIO_Type_PushPull ;
226 
        GPIO_InitStructure.GPIO_IPInputConnected = GPIO_IPInputConnected_Enable;
 226 
        GPIO_InitStructure.GPIO_IPInputConnected = GPIO_IPInputConnected_Enable;
227 
        GPIO_InitStructure.GPIO_Alternate = GPIO_InputAlt1; //SSP0_SCLK, SSP0_MOSI, SSP0_NSS
 227 
        GPIO_InitStructure.GPIO_Alternate = GPIO_InputAlt1; //SSP0_SCLK, SSP0_MOSI, SSP0_NSS
228 
        GPIO_Init (GPIO2, &GPIO_InitStructure);
 228 
        GPIO_Init (GPIO2, &GPIO_InitStructure);
229 
 
 229 
 
230 
        // SSP0_MISO pin GPIO2.6
 230 
        // SSP0_MISO pin GPIO2.6
231 
        GPIO_StructInit(&GPIO_InitStructure);
 231 
        GPIO_StructInit(&GPIO_InitStructure);
232 
        GPIO_InitStructure.GPIO_Direction = GPIO_PinOutput;
 232 
        GPIO_InitStructure.GPIO_Direction = GPIO_PinOutput;
233 
        GPIO_InitStructure.GPIO_Pin = GPIO_Pin_6;
 233 
        GPIO_InitStructure.GPIO_Pin = GPIO_Pin_6;
234 
        GPIO_InitStructure.GPIO_Type = GPIO_Type_PushPull ;
 234 
        GPIO_InitStructure.GPIO_Type = GPIO_Type_PushPull ;
235 
        GPIO_InitStructure.GPIO_IPInputConnected = GPIO_IPInputConnected_Enable;
 235 
        GPIO_InitStructure.GPIO_IPInputConnected = GPIO_IPInputConnected_Enable;
236 
        GPIO_InitStructure.GPIO_Alternate = GPIO_OutputAlt2; //SSP0_MISO
 236 
        GPIO_InitStructure.GPIO_Alternate = GPIO_OutputAlt2; //SSP0_MISO
237 
        GPIO_Init (GPIO2, &GPIO_InitStructure);
 237 
        GPIO_Init (GPIO2, &GPIO_InitStructure);
238 
 
 238 
 
239 
        SSP_DeInit(SSP0);
 239 
        SSP_DeInit(SSP0);
240 
        SSP_StructInit(&SSP_InitStructure);
 240 
        SSP_StructInit(&SSP_InitStructure);
241 
        SSP_InitStructure.SSP_FrameFormat = SSP_FrameFormat_Motorola;
 241 
        SSP_InitStructure.SSP_FrameFormat = SSP_FrameFormat_Motorola;
242 
        SSP_InitStructure.SSP_Mode = SSP_Mode_Slave;
 242 
        SSP_InitStructure.SSP_Mode = SSP_Mode_Slave;
243 
        SSP_InitStructure.SSP_SlaveOutput = SSP_SlaveOutput_Enable;
 243 
        SSP_InitStructure.SSP_SlaveOutput = SSP_SlaveOutput_Enable;
244 
        SSP_InitStructure.SSP_CPHA = SSP_CPHA_1Edge;
 244 
        SSP_InitStructure.SSP_CPHA = SSP_CPHA_1Edge;
245 
        SSP_InitStructure.SSP_CPOL = SSP_CPOL_Low;
 245 
        SSP_InitStructure.SSP_CPOL = SSP_CPOL_Low;
246 
        SSP_InitStructure.SSP_ClockRate = 0;
 246 
        SSP_InitStructure.SSP_ClockRate = 0;
247 
 
 247 
 
248 
        SSP_Init(SSP0, &SSP_InitStructure);
 248 
        SSP_Init(SSP0, &SSP_InitStructure);
249 
        SSP_ITConfig(SSP0, SSP_IT_RxFifo | SSP_IT_RxTimeOut, ENABLE);
 249 
        SSP_ITConfig(SSP0, SSP_IT_RxFifo | SSP_IT_RxTimeOut, ENABLE);
250 
 
 250 
 
251 
        SSP_Cmd(SSP0, ENABLE);
 251 
        SSP_Cmd(SSP0, ENABLE);
252 
        // initialize the syncbytes in the tx buffer
 252 
        // initialize the syncbytes in the tx buffer
253 
        SPI_TxBuffer[0] = SPI_TXSYNCBYTE1;
 253 
        SPI_TxBuffer[0] = SPI_TXSYNCBYTE1;
254 
        SPI_TxBuffer[1] = SPI_TXSYNCBYTE2;
 254 
        SPI_TxBuffer[1] = SPI_TXSYNCBYTE2;
255 
        // set the pointer to the checksum byte in the tx buffer
 255 
        // set the pointer to the checksum byte in the tx buffer
256 
        Ptr_TxChksum = (u8 *) &(((ToFlightCtrl_t *) &(SPI_TxBuffer[2]))->Chksum);
 256 
        Ptr_TxChksum = (u8 *) &(((ToFlightCtrl_t *) &(SPI_TxBuffer[2]))->Chksum);
257 
 
 257 
 
258 
        ToFlightCtrl.GPSStick.Nick = 0;
 258 
        ToFlightCtrl.GPSStick.Nick = 0;
259 
        ToFlightCtrl.GPSStick.Roll = 0;
 259 
        ToFlightCtrl.GPSStick.Roll = 0;
260 
        ToFlightCtrl.GPSStick.Yaw = 0;
 260 
        ToFlightCtrl.GPSStick.Yaw = 0;
261 
 
 261 
 
262 
        VIC_Config(SSP0_ITLine, VIC_IRQ, PRIORITY_SPI0);
 262 
        VIC_Config(SSP0_ITLine, VIC_IRQ, PRIORITY_SPI0);
263 
        VIC_ITCmd(SSP0_ITLine, ENABLE);
 263 
        VIC_ITCmd(SSP0_ITLine, ENABLE);
264 
 
 264 
 
265 
        SPI0_Timeout = SetDelay(4*SPI0_TIMEOUT);
 265 
        SPI0_Timeout = SetDelay(4*SPI0_TIMEOUT);
266 
 
 266 
 
267 
        UART1_PutString("ok");
 267 
        UART1_PutString("ok");
268 
}
 268 
}
269 
 
 269 
 
270 
 
 270 
 
271 
//------------------------------------------------------
 271 
//------------------------------------------------------
272 
void SPI0_UpdateBuffer(void)
 272 
void SPI0_UpdateBuffer(void)
273 
{
 273 
{
274 
        static u32 timeout = 0;
 274 
        static u32 timeout = 0;
275 
        static u8 counter = 50;
 275 
        static u8 counter = 50;
276 
        static u8 CompassCalState = 0;
 276 
        static u8 CompassCalState = 0;
277 
        static u8 FCCalibActive = 0;
 277 
        static u8 FCCalibActive = 0;
278 
        s16 tmp;
 278 
        s16 tmp;
279 
 
 279 
 
280 
        if (SPI_RxBuffer_Request)
 280 
        if (SPI_RxBuffer_Request)
281 
        {
 281 
        {
282 
                // avoid sending data via SPI during the update of the  ToFlightCtrl structure
 282 
                // avoid sending data via SPI during the update of the  ToFlightCtrl structure
283 
                VIC_ITCmd(SSP0_ITLine, DISABLE); // disable SPI interrupt
 283 
                VIC_ITCmd(SSP0_ITLine, DISABLE); // disable SPI interrupt
284 
                ToFlightCtrl.CompassHeading = Compass_Heading;
 284 
                ToFlightCtrl.CompassHeading = Compass_Heading;
285 
                DebugOut.Analog[10] = ToFlightCtrl.CompassHeading;
 285 
                DebugOut.Analog[10] = ToFlightCtrl.CompassHeading;
286 
                if(ToFlightCtrl.CompassHeading >= 0) ToFlightCtrl.CompassHeading = (360 + ToFlightCtrl.CompassHeading + FromFlightCtrl.GyroYaw / 12) % 360;
 286 
                if(ToFlightCtrl.CompassHeading >= 0) ToFlightCtrl.CompassHeading = (360 + ToFlightCtrl.CompassHeading + FromFlightCtrl.GyroYaw / 12) % 360;
287 
                ToFlightCtrl.MagVecX = MagVector.X;
 287 
                ToFlightCtrl.MagVecX = MagVector.X;
288 
                ToFlightCtrl.MagVecY = MagVector.Y;
 288 
                ToFlightCtrl.MagVecY = MagVector.Y;
289 
                ToFlightCtrl.MagVecZ = MagVector.Z;
 289 
                ToFlightCtrl.MagVecZ = MagVector.Z;
290 
                ToFlightCtrl.NCStatus = 0;
 290 
                ToFlightCtrl.NCStatus = 0;
291 
                // cycle spi commands
 291 
                // cycle spi commands
292 
                ToFlightCtrl.Command = SPI_CommandSequence[SPI_CommandCounter++];
 292 
                ToFlightCtrl.Command = SPI_CommandSequence[SPI_CommandCounter++];
293 
                // restart command cycle at the end
 293 
                // restart command cycle at the end
294 
                if (SPI_CommandCounter >= sizeof(SPI_CommandSequence)) SPI_CommandCounter = 0;
 294 
                if (SPI_CommandCounter >= sizeof(SPI_CommandSequence)) SPI_CommandCounter = 0;
295 
 
 295 
 
296 
                switch (ToFlightCtrl.Command)
 296 
                switch (ToFlightCtrl.Command)
297 
                {
 297 
                {
298 
                        case  SPI_NCCMD_KALMAN:
 298 
                        case  SPI_NCCMD_KALMAN:
299 
                                CalcHeadFree();
 299 
                                CalcHeadFree();
300 
                                ToFlightCtrl.Param.sByte[0] = (s8) Kalman_K;
 300 
                                ToFlightCtrl.Param.sByte[0] = (s8) Kalman_K;
301 
                                ToFlightCtrl.Param.sByte[1] = (s8) Kalman_MaxFusion;
 301 
                                ToFlightCtrl.Param.sByte[1] = (s8) Kalman_MaxFusion;
302 
                                ToFlightCtrl.Param.sByte[2] = (s8) Kalman_MaxDrift;
 302 
                                ToFlightCtrl.Param.sByte[2] = (s8) Kalman_MaxDrift;
303 
                                ToFlightCtrl.Param.Byte[3]      = (u8) SerialLinkOkay;
 303 
                                ToFlightCtrl.Param.Byte[3]      = (u8) SerialLinkOkay;
304 
                                ToFlightCtrl.Param.sByte[4] = (s8) ToFcGpsZ;
 304 
                                ToFlightCtrl.Param.sByte[4] = (s8) ToFcGpsZ;
305 
                                ToFlightCtrl.Param.Byte[5] = (s8) ToFC_Rotate_C;
 305 
                                ToFlightCtrl.Param.Byte[5] = (s8) ToFC_Rotate_C;
306 
                                ToFlightCtrl.Param.Byte[6] = (s8) ToFC_Rotate_S;
 306 
                                ToFlightCtrl.Param.Byte[6] = (s8) ToFC_Rotate_S;
307 
                //ToFlightCtrl.Param.Byte[7] =
307 
                //ToFlightCtrl.Param.Byte[7] =
308 
                                if(CAM_Orientation.UpdateMask & CAM_UPDATE_AZIMUTH)
 308 
                                if(CAM_Orientation.UpdateMask & CAM_UPDATE_AZIMUTH)
309 
                                {
 309 
                                {
310 
                                        ToFlightCtrl.Param.sInt[4] = CAM_Orientation.Azimuth;
 310 
                                        ToFlightCtrl.Param.sInt[4] = CAM_Orientation.Azimuth;
311 
                                        CAM_Orientation.UpdateMask &= ~CAM_UPDATE_AZIMUTH;
 311 
                                        CAM_Orientation.UpdateMask &= ~CAM_UPDATE_AZIMUTH;
312 
                                }
 312 
                                }
313 
                                else
 313 
                                else
314 
                                {
 314 
                                {
315 
                                        ToFlightCtrl.Param.sInt[4] = -1;
 315 
                                        ToFlightCtrl.Param.sInt[4] = -1;
316 
                                }
 316 
                                }
317 
 
 317 
 
318 
                                if(NCRARAM_STATE_VALID == NCParams_GetValue(NCPARAMS_NEW_CAMERA_ELEVATION, &tmp))  // Elevation set via 'j' command
 318 
                                if(NCRARAM_STATE_VALID == NCParams_GetValue(NCPARAMS_NEW_CAMERA_ELEVATION, &tmp))  // Elevation set via 'j' command
319 
                                {
 319 
                                {
320 
                                        ToFlightCtrl.Param.sInt[5] = tmp;
 320 
                                        ToFlightCtrl.Param.sInt[5] = tmp;
321 
                                }
 321 
                                }
322 
                                else
 322 
                                else
323 
                                {
 323 
                                {
324 
                                        if(FC.StatusFlags2 & FC_STATUS2_CAREFREE) ToFlightCtrl.Param.sInt[5] = CAM_Orientation.Elevation; // only, if carefree is active
 324 
                                        if(FC.StatusFlags2 & FC_STATUS2_CAREFREE) ToFlightCtrl.Param.sInt[5] = CAM_Orientation.Elevation; // only, if carefree is active
325 
                                        else ToFlightCtrl.Param.sInt[5] = 0;
 325 
                                        else ToFlightCtrl.Param.sInt[5] = 0;
326 
                                }
 326 
                                }
327 
                                break;
 327 
                                break;
328 
 
 328 
 
329 
                        case SPI_NCCMD_VERSION:
 329 
                        case SPI_NCCMD_VERSION:
330 
                                ToFlightCtrl.Param.Byte[0] = VERSION_MAJOR;
 330 
                                ToFlightCtrl.Param.Byte[0] = VERSION_MAJOR;
331 
                                ToFlightCtrl.Param.Byte[1] = VERSION_MINOR;
 331 
                                ToFlightCtrl.Param.Byte[1] = VERSION_MINOR;
332 
                                ToFlightCtrl.Param.Byte[2] = VERSION_PATCH;
 332 
                                ToFlightCtrl.Param.Byte[2] = VERSION_PATCH;
333 
                                ToFlightCtrl.Param.Byte[3] = FC_SPI_COMPATIBLE;
 333 
                                ToFlightCtrl.Param.Byte[3] = FC_SPI_COMPATIBLE;
334 
                                ToFlightCtrl.Param.Byte[4] = Version_HW;
 334 
                                ToFlightCtrl.Param.Byte[4] = Version_HW;
335 
                                ToFlightCtrl.Param.Byte[5] = DebugOut.Status[0];
 335 
                                ToFlightCtrl.Param.Byte[5] = DebugOut.Status[0];
336 
                                ToFlightCtrl.Param.Byte[6] = DebugOut.Status[1];
 336 
                                ToFlightCtrl.Param.Byte[6] = DebugOut.Status[1];
337 
                                ToFlightCtrl.Param.Byte[7] = ErrorCode;
 337 
                                ToFlightCtrl.Param.Byte[7] = ErrorCode;
338 
                                break;
 338 
                                break;
339 
 
 339 
 
340 
                        case SPI_NCCMD_GPSINFO:
 340 
                        case SPI_NCCMD_GPSINFO:
341 
                                ToFlightCtrl.Param.Byte[0] = GPSData.Flags;
 341 
                                ToFlightCtrl.Param.Byte[0] = GPSData.Flags;
342 
                                ToFlightCtrl.Param.Byte[1] = GPSData.NumOfSats;
 342 
                                ToFlightCtrl.Param.Byte[1] = GPSData.NumOfSats;
343 
                                ToFlightCtrl.Param.Byte[2] = GPSData.SatFix;
 343 
                                ToFlightCtrl.Param.Byte[2] = GPSData.SatFix;
344 
                                ToFlightCtrl.Param.Byte[3] = GPSData.Speed_Ground / 100; // m/s
 344 
                                ToFlightCtrl.Param.Byte[3] = GPSData.Speed_Ground / 100; // m/s
345 
                                ToFlightCtrl.Param.Int[2]  = NaviData.HomePositionDeviation.Distance; // dm   //4&5
 345 
                                ToFlightCtrl.Param.Int[2]  = NaviData.HomePositionDeviation.Distance; // dm   //4&5
346 
                                ToFlightCtrl.Param.sInt[3] = NaviData.HomePositionDeviation.Bearing;  // deg  //6&7
 346 
                                ToFlightCtrl.Param.sInt[3] = NaviData.HomePositionDeviation.Bearing;  // deg  //6&7
- 
 
 347 
                                if(FC_WP_EventChannel > 254) FC_WP_EventChannel = 254;
347 
                                ToFlightCtrl.Param.Byte[8] = (s8)(FC_WP_EventChannel - 110);
 348 
                                ToFlightCtrl.Param.Byte[8] = (s8)(FC_WP_EventChannel - 127);
348 
 
 - 
 
349 
                                if(NCRARAM_STATE_VALID == NCParams_GetValue(NCPARAMS_ALTITUDE_RATE, &tmp))
 349 
                                if(NCRARAM_STATE_VALID == NCParams_GetValue(NCPARAMS_ALTITUDE_RATE, &tmp))
350 
                                {
 350 
                                {
351 
                                        ToFlightCtrl.Param.Byte[9] = (u8)tmp;
 351 
                                        ToFlightCtrl.Param.Byte[9] = (u8)tmp;
352 
                                }
 352 
                                }
353 
                                else
 353 
                                else
354 
                                {
 354 
                                {
355 
                                        ToFlightCtrl.Param.Byte[9] = (u8)ToFC_AltitudeRate;
 355 
                                        ToFlightCtrl.Param.Byte[9] = (u8)ToFC_AltitudeRate;
356 
                                }
 356 
                                }
357 
                                if(NCRARAM_STATE_VALID == NCParams_GetValue(NCPARAMS_ALTITUDE_SETPOINT, &tmp))
 357 
                                if(NCRARAM_STATE_VALID == NCParams_GetValue(NCPARAMS_ALTITUDE_SETPOINT, &tmp))
358 
                                {
 358 
                                {
359 
                                        ToFlightCtrl.Param.sInt[5] = tmp;
 359 
                                        ToFlightCtrl.Param.sInt[5] = tmp;
360 
                                }
 360 
                                }
361 
                                else
 361 
                                else
362 
                                {
 362 
                                {
363 
                                        ToFlightCtrl.Param.sInt[5] = (s16)ToFC_AltitudeSetpoint;
 363 
                                        ToFlightCtrl.Param.sInt[5] = (s16)ToFC_AltitudeSetpoint;
364 
                                }
 364 
                                }
365 
//                              DebugOut.Analog[25] = (s16)ToFlightCtrl.Param.Byte[9];
 365 
//                              DebugOut.Analog[25] = (s16)ToFlightCtrl.Param.Byte[9];
366 
//                              DebugOut.Analog[20] = ToFlightCtrl.Param.sInt[5];
 366 
//                              DebugOut.Analog[20] = ToFlightCtrl.Param.sInt[5];
367 
                                break;
 367 
                                break;
368 
 
 368 
 
369 
                        default:
 369 
                        default:
370 
                                break;
 370 
                                break;
371 
// 0 = 0,1
 371 
// 0 = 0,1
372 
// 1 = 2,3
 372 
// 1 = 2,3
373 
// 2 = 4,5
 373 
// 2 = 4,5
374 
// 3 = 6,7
 374 
// 3 = 6,7
375 
// 4 = 8,9
 375 
// 4 = 8,9
376 
// 5 = 10,11
 376 
// 5 = 10,11
377 
                }
 377 
                }
378 
                VIC_ITCmd(SSP0_ITLine, ENABLE);         // enable SPI interrupt
 378 
                VIC_ITCmd(SSP0_ITLine, ENABLE);         // enable SPI interrupt
379 
 
 379 
 
380 
 
 380 
 
381 
                switch(FromFlightCtrl.Command)
 381 
                switch(FromFlightCtrl.Command)
382 
                {
 382 
                {
383 
                        case SPI_FCCMD_USER:
 383 
                        case SPI_FCCMD_USER:
384 
                                Parameter.User1 = FromFlightCtrl.Param.Byte[0];
 384 
                                Parameter.User1 = FromFlightCtrl.Param.Byte[0];
385 
                                Parameter.User2 = FromFlightCtrl.Param.Byte[1];
 385 
                                Parameter.User2 = FromFlightCtrl.Param.Byte[1];
386 
                                Parameter.User3 = FromFlightCtrl.Param.Byte[2];
 386 
                                Parameter.User3 = FromFlightCtrl.Param.Byte[2];
387 
                                Parameter.User4 = FromFlightCtrl.Param.Byte[3];
 387 
                                Parameter.User4 = FromFlightCtrl.Param.Byte[3];
388 
                                Parameter.User5 = FromFlightCtrl.Param.Byte[4];
 388 
                                Parameter.User5 = FromFlightCtrl.Param.Byte[4];
389 
                                Parameter.User6 = FromFlightCtrl.Param.Byte[5];
 389 
                                Parameter.User6 = FromFlightCtrl.Param.Byte[5];
390 
                                Parameter.User7 = FromFlightCtrl.Param.Byte[6];
 390 
                                Parameter.User7 = FromFlightCtrl.Param.Byte[6];
391 
                                Parameter.User8 = FromFlightCtrl.Param.Byte[7];
 391 
                                Parameter.User8 = FromFlightCtrl.Param.Byte[7];
392 
                                if(ClearFCStatusFlags)
 392 
                                if(ClearFCStatusFlags)
393 
                                {
 393 
                                {
394 
                                        FC.StatusFlags = 0;
 394 
                                        FC.StatusFlags = 0;
395 
                                        ClearFCStatusFlags = 0;
 395 
                                        ClearFCStatusFlags = 0;
396 
                                }
 396 
                                }
397 
                                FC.StatusFlags |= FromFlightCtrl.Param.Byte[8];
 397 
                                FC.StatusFlags |= FromFlightCtrl.Param.Byte[8];
398 
                                if(FC.StatusFlags&FC_STATUS_CALIBRATE && !FCCalibActive)
 398 
                                if(FC.StatusFlags&FC_STATUS_CALIBRATE && !FCCalibActive)
399 
                                {
 399 
                                {
400 
                                        Compass_Init();
 400 
                                        Compass_Init();
401 
                                        FCCalibActive = 1;
 401 
                                        FCCalibActive = 1;
402 
                                }
 402 
                                }
403 
                                else
 403 
                                else
404 
                                {
 404 
                                {
405 
                                        FCCalibActive = 0;
 405 
                                        FCCalibActive = 0;
406 
                                }
 406 
                                }
407 
                                Parameter.ActiveSetting = FromFlightCtrl.Param.Byte[9];
 407 
                                Parameter.ActiveSetting = FromFlightCtrl.Param.Byte[9];
408 
                                DebugOut.Analog[5] = FC.StatusFlags;
 408 
                                DebugOut.Analog[5] = FC.StatusFlags;
409 
                                FC.StatusFlags2 = FromFlightCtrl.Param.Byte[11];
 409 
                                FC.StatusFlags2 = FromFlightCtrl.Param.Byte[11];
410 
                                NaviData.FCStatusFlags = FC.StatusFlags;
 410 
                                NaviData.FCStatusFlags = FC.StatusFlags;
411 
                                NaviData.FCStatusFlags2 = FC.StatusFlags2;
 411 
                                NaviData.FCStatusFlags2 = FC.StatusFlags2;
412 
                                HeadFreeStartAngle = (s32) FromFlightCtrl.Param.Byte[10] * 20; // convert to 0.1°
 412 
                                HeadFreeStartAngle = (s32) FromFlightCtrl.Param.Byte[10] * 20; // convert to 0.1°
413 
                                break;
 413 
                                break;
414 
 
 414 
 
415 
                        case SPI_FCCMD_ACCU:
 415 
                        case SPI_FCCMD_ACCU:
416 
                                FC.BAT_Current = FromFlightCtrl.Param.Int[0];
 416 
                                FC.BAT_Current = FromFlightCtrl.Param.Int[0];
417 
                                FC.BAT_UsedCapacity = FromFlightCtrl.Param.Int[1];
 417 
                                FC.BAT_UsedCapacity = FromFlightCtrl.Param.Int[1];
418 
                                FC.BAT_Voltage = FromFlightCtrl.Param.Byte[4];
 418 
                                FC.BAT_Voltage = FromFlightCtrl.Param.Byte[4];
419 
                                Parameter.LowVoltageWarning = FromFlightCtrl.Param.Byte[5];
 419 
                                Parameter.LowVoltageWarning = FromFlightCtrl.Param.Byte[5];
420 
                                FromFC_VarioCharacter = FromFlightCtrl.Param.Byte[6];
 420 
                                FromFC_VarioCharacter = FromFlightCtrl.Param.Byte[6];
421 
                                if(FromFC_VarioCharacter == '+' || FromFC_VarioCharacter == '-') // manual setpoint clears the NC-Parameter command
 421 
                                if(FromFC_VarioCharacter == '+' || FromFC_VarioCharacter == '-') // manual setpoint clears the NC-Parameter command
422 
                                 {
 422 
                                 {
423 
                                  NCParams_ClearValue(NCPARAMS_ALTITUDE_RATE);
 423 
                                  NCParams_ClearValue(NCPARAMS_ALTITUDE_RATE);
424 
                                 }
 424 
                                 }
425 
 
 425 
 
426 
                                NaviData.UBat = FC.BAT_Voltage;
 426 
                                NaviData.UBat = FC.BAT_Voltage;
427 
                                NaviData.Current = FC.BAT_Current;
 427 
                                NaviData.Current = FC.BAT_Current;
428 
                                NaviData.UsedCapacity = FC.BAT_UsedCapacity;
 428 
                                NaviData.UsedCapacity = FC.BAT_UsedCapacity;
429 
                                break;
 429 
                                break;
430 
 
 430 
 
431 
#define CHK_POTI(b,a) { if(a < 248) b = a; else b = FC.Poti[255 - a]; }
 431 
#define CHK_POTI(b,a) { if(a < 248) b = a; else b = FC.Poti[255 - a]; }
432 
#define CHK_POTI_MM(b,a,min,max) {CHK_POTI(b,a); LIMIT_MIN_MAX(b, min, max); }
 432 
#define CHK_POTI_MM(b,a,min,max) {CHK_POTI(b,a); LIMIT_MIN_MAX(b, min, max); }
433 
 
 433 
 
434 
                        case SPI_FCCMD_PARAMETER1:
 434 
                        case SPI_FCCMD_PARAMETER1:
435 
                                CHK_POTI_MM(Parameter.NaviGpsModeControl,FromFlightCtrl.Param.Byte[0],0,255);
 435 
                                CHK_POTI_MM(Parameter.NaviGpsModeControl,FromFlightCtrl.Param.Byte[0],0,255);
436 
                                CHK_POTI_MM(Parameter.NaviGpsGain,FromFlightCtrl.Param.Byte[1],0,255);
 436 
                                CHK_POTI_MM(Parameter.NaviGpsGain,FromFlightCtrl.Param.Byte[1],0,255);
437 
                                CHK_POTI_MM(Parameter.NaviGpsP,FromFlightCtrl.Param.Byte[2],0,255);
 437 
                                CHK_POTI_MM(Parameter.NaviGpsP,FromFlightCtrl.Param.Byte[2],0,255);
438 
                                CHK_POTI_MM(Parameter.NaviGpsI,FromFlightCtrl.Param.Byte[3],0,255);
 438 
                                CHK_POTI_MM(Parameter.NaviGpsI,FromFlightCtrl.Param.Byte[3],0,255);
439 
                                CHK_POTI_MM(Parameter.NaviGpsD,FromFlightCtrl.Param.Byte[4],0,255);
 439 
                                CHK_POTI_MM(Parameter.NaviGpsD,FromFlightCtrl.Param.Byte[4],0,255);
440 
                                CHK_POTI_MM(Parameter.NaviGpsACC,FromFlightCtrl.Param.Byte[5],0,255);
 440 
                                CHK_POTI_MM(Parameter.NaviGpsACC,FromFlightCtrl.Param.Byte[5],0,255);
441 
                                Parameter.NaviGpsMinSat = FromFlightCtrl.Param.Byte[6];
 441 
                                Parameter.NaviGpsMinSat = FromFlightCtrl.Param.Byte[6];
442 
                                Parameter.NaviStickThreshold = FromFlightCtrl.Param.Byte[7];
 442 
                                Parameter.NaviStickThreshold = FromFlightCtrl.Param.Byte[7];
443 
                                CHK_POTI_MM(Parameter.NaviOperatingRadius,FromFlightCtrl.Param.Byte[8],0,255);
 443 
                                CHK_POTI_MM(Parameter.NaviOperatingRadius,FromFlightCtrl.Param.Byte[8],0,255);
444 
                                CHK_POTI_MM(Parameter.NaviWindCorrection,FromFlightCtrl.Param.Byte[9],0,255);
 444 
                                CHK_POTI_MM(Parameter.NaviWindCorrection,FromFlightCtrl.Param.Byte[9],0,255);
445 
                                CHK_POTI_MM(Parameter.NaviSpeedCompensation,FromFlightCtrl.Param.Byte[10],0,255);
 445 
                                CHK_POTI_MM(Parameter.NaviSpeedCompensation,FromFlightCtrl.Param.Byte[10],0,255);
446 
                                CHK_POTI_MM(Parameter.NaviAngleLimitation,FromFlightCtrl.Param.Byte[11],0,255);
 446 
                                CHK_POTI_MM(Parameter.NaviAngleLimitation,FromFlightCtrl.Param.Byte[11],0,255);
447 
                                break;
 447 
                                break;
448 
 
 448 
 
449 
                        case SPI_FCCMD_STICK:
 449 
                        case SPI_FCCMD_STICK:
450 
                                FC.StickGas     = FromFlightCtrl.Param.sByte[0];
 450 
                                FC.StickGas     = FromFlightCtrl.Param.sByte[0];
451 
                                FC.StickYaw     = FromFlightCtrl.Param.sByte[1];
 451 
                                FC.StickYaw     = FromFlightCtrl.Param.sByte[1];
452 
                                FC.StickRoll    = FromFlightCtrl.Param.sByte[2];
 452 
                                FC.StickRoll    = FromFlightCtrl.Param.sByte[2];
453 
                                FC.StickNick    = FromFlightCtrl.Param.sByte[3];
 453 
                                FC.StickNick    = FromFlightCtrl.Param.sByte[3];
454 
                                FC.Poti[0]              = FromFlightCtrl.Param.Byte[4];
 454 
                                FC.Poti[0]              = FromFlightCtrl.Param.Byte[4];
455 
                                FC.Poti[1]              = FromFlightCtrl.Param.Byte[5];
 455 
                                FC.Poti[1]              = FromFlightCtrl.Param.Byte[5];
456 
                                FC.Poti[2]              = FromFlightCtrl.Param.Byte[6];
 456 
                                FC.Poti[2]              = FromFlightCtrl.Param.Byte[6];
457 
                                FC.Poti[3]              = FromFlightCtrl.Param.Byte[7];
 457 
                                FC.Poti[3]              = FromFlightCtrl.Param.Byte[7];
458 
                                FC.Poti[4]              = FromFlightCtrl.Param.Byte[8];
 458 
                                FC.Poti[4]              = FromFlightCtrl.Param.Byte[8];
459 
                                FC.Poti[5]              = FromFlightCtrl.Param.Byte[9];
 459 
                                FC.Poti[5]              = FromFlightCtrl.Param.Byte[9];
460 
                                FC.Poti[6]              = FromFlightCtrl.Param.Byte[10];
 460 
                                FC.Poti[6]              = FromFlightCtrl.Param.Byte[10];
461 
                                FC.Poti[7]              = FromFlightCtrl.Param.Byte[11];
 461 
                                FC.Poti[7]              = FromFlightCtrl.Param.Byte[11];
462 
                                break;
 462 
                                break;
463 
 
 463 
 
464 
                        case SPI_FCCMD_MISC:
 464 
                        case SPI_FCCMD_MISC:
465 
                                if(CompassCalState != FromFlightCtrl.Param.Byte[0])
 465 
                                if(CompassCalState != FromFlightCtrl.Param.Byte[0])
466 
                                {       // put only new CompassCalState into queue to send via I2C
 466 
                                {       // put only new CompassCalState into queue to send via I2C
467 
                                        CompassCalState = FromFlightCtrl.Param.Byte[0];
 467 
                                        CompassCalState = FromFlightCtrl.Param.Byte[0];
468 
                                        Compass_SetCalState(CompassCalState);
 468 
                                        Compass_SetCalState(CompassCalState);
469 
                                }
 469 
                                }
470 
                                Parameter.NaviPH_LoginTime = FromFlightCtrl.Param.Byte[1];
 470 
                                Parameter.NaviPH_LoginTime = FromFlightCtrl.Param.Byte[1];
471 
                                NaviData.Variometer = (NaviData.Variometer + 2 * (FromFlightCtrl.Param.sInt[1] - NaviData.Altimeter)) / 2; // provisorisch
 471 
                                NaviData.Variometer = (NaviData.Variometer + 2 * (FromFlightCtrl.Param.sInt[1] - NaviData.Altimeter)) / 2; // provisorisch
472 
                                NaviData.Altimeter = FromFlightCtrl.Param.sInt[1]; // in 5cm
 472 
                                NaviData.Altimeter = FromFlightCtrl.Param.sInt[1]; // in 5cm
473 
                                NaviData.SetpointAltitude = FromFlightCtrl.Param.sInt[2]; // in 5cm
 473 
                                NaviData.SetpointAltitude = FromFlightCtrl.Param.sInt[2]; // in 5cm
474 
                                CHK_POTI_MM(Parameter.NaviGpsPLimit,FromFlightCtrl.Param.Byte[6],0,255);
 474 
                                CHK_POTI_MM(Parameter.NaviGpsPLimit,FromFlightCtrl.Param.Byte[6],0,255);
475 
                                CHK_POTI_MM(Parameter.NaviGpsILimit,FromFlightCtrl.Param.Byte[7],0,255);
 475 
                                CHK_POTI_MM(Parameter.NaviGpsILimit,FromFlightCtrl.Param.Byte[7],0,255);
476 
                                CHK_POTI_MM(Parameter.NaviGpsDLimit,FromFlightCtrl.Param.Byte[8],0,255);
 476 
                                CHK_POTI_MM(Parameter.NaviGpsDLimit,FromFlightCtrl.Param.Byte[8],0,255);
477 
                                FC.RC_Quality   = FromFlightCtrl.Param.Byte[9];
 477 
                                FC.RC_Quality   = FromFlightCtrl.Param.Byte[9];
478 
                                FC.RC_RSSI              = FromFlightCtrl.Param.Byte[10];
 478 
                                FC.RC_RSSI              = FromFlightCtrl.Param.Byte[10];
479 
                                if(!FC.RC_RSSI) NaviData.RC_Quality = FC.RC_Quality; else NaviData.RC_Quality = FC.RC_RSSI;
 479 
                                if(!FC.RC_RSSI) NaviData.RC_Quality = FC.RC_Quality; else NaviData.RC_Quality = FC.RC_RSSI;
480 
//                              NaviData.RC_RSSI = FC.RC_RSSI;
 480 
//                              NaviData.RC_RSSI = FC.RC_RSSI;
481 
                                NaviData.Gas    = (FC.BAT_Voltage * (u32) FromFlightCtrl.Param.Byte[11]) / (u32) Parameter.LowVoltageWarning;
 481 
                                NaviData.Gas    = (FC.BAT_Voltage * (u32) FromFlightCtrl.Param.Byte[11]) / (u32) Parameter.LowVoltageWarning;
482 
                                break;
 482 
                                break;
483 
 
 483 
 
484 
                        case SPI_FCCMD_SERVOS:
 484 
                        case SPI_FCCMD_SERVOS:
485 
                                ServoParams.Refresh             = FromFlightCtrl.Param.Byte[0];
 485 
                                ServoParams.Refresh             = FromFlightCtrl.Param.Byte[0];
486 
                                ServoParams.CompInvert  = FromFlightCtrl.Param.Byte[1];
 486 
                                ServoParams.CompInvert  = FromFlightCtrl.Param.Byte[1];
487 
                                ServoParams.NickControl = FromFlightCtrl.Param.Byte[2];
 487 
                                ServoParams.NickControl = FromFlightCtrl.Param.Byte[2];
488 
                                ServoParams.NickComp    = FromFlightCtrl.Param.Byte[3];
 488 
                                ServoParams.NickComp    = FromFlightCtrl.Param.Byte[3];
489 
                                ServoParams.NickMin             = FromFlightCtrl.Param.Byte[4];
 489 
                                ServoParams.NickMin             = FromFlightCtrl.Param.Byte[4];
490 
                                ServoParams.NickMax             = FromFlightCtrl.Param.Byte[5];
 490 
                                ServoParams.NickMax             = FromFlightCtrl.Param.Byte[5];
491 
                                ServoParams.RollControl = FromFlightCtrl.Param.Byte[6];
 491 
                                ServoParams.RollControl = FromFlightCtrl.Param.Byte[6];
492 
                                ServoParams.RollComp    = FromFlightCtrl.Param.Byte[7];
 492 
                                ServoParams.RollComp    = FromFlightCtrl.Param.Byte[7];
493 
                                ServoParams.RollMin             = FromFlightCtrl.Param.Byte[8];
 493 
                                ServoParams.RollMin             = FromFlightCtrl.Param.Byte[8];
494 
                                ServoParams.RollMax             = FromFlightCtrl.Param.Byte[9];
 494 
                                ServoParams.RollMax             = FromFlightCtrl.Param.Byte[9];
495 
                                break;
 495 
                                break;
496 
 
 496 
 
497 
                        case SPI_FCCMD_VERSION:
 497 
                        case SPI_FCCMD_VERSION:
498 
                                FC_Version.Major                = FromFlightCtrl.Param.Byte[0];
 498 
                                FC_Version.Major                = FromFlightCtrl.Param.Byte[0];
499 
                                FC_Version.Minor                = FromFlightCtrl.Param.Byte[1];
 499 
                                FC_Version.Minor                = FromFlightCtrl.Param.Byte[1];
500 
                                FC_Version.Patch                = FromFlightCtrl.Param.Byte[2];
 500 
                                FC_Version.Patch                = FromFlightCtrl.Param.Byte[2];
501 
                                FC_Version.Compatible   = FromFlightCtrl.Param.Byte[3];
 501 
                                FC_Version.Compatible   = FromFlightCtrl.Param.Byte[3];
502 
                                FC_Version.Hardware             = FromFlightCtrl.Param.Byte[4];
 502 
                                FC_Version.Hardware             = FromFlightCtrl.Param.Byte[4];
503 
                                FC.Error[0]                     = FromFlightCtrl.Param.Byte[5];
 503 
                                FC.Error[0]                     = FromFlightCtrl.Param.Byte[5];
504 
                                FC.Error[1]                     = FromFlightCtrl.Param.Byte[6];
 504 
                                FC.Error[1]                     = FromFlightCtrl.Param.Byte[6];
505 
                                FC.Error[2]                     = FromFlightCtrl.Param.Byte[7];
 505 
                                FC.Error[2]                     = FromFlightCtrl.Param.Byte[7];
506 
                                FC.Error[3]                     = FromFlightCtrl.Param.Byte[8];
 506 
                                FC.Error[3]                     = FromFlightCtrl.Param.Byte[8];
507 
                                FC.Error[4]                     = FromFlightCtrl.Param.Byte[9];
 507 
                                FC.Error[4]                     = FromFlightCtrl.Param.Byte[9];
508 
                                DebugOut.Status[0] |= 0x01; // status of FC Present
 508 
                                DebugOut.Status[0] |= 0x01; // status of FC Present
509 
                                DebugOut.Status[0] |= 0x02; // status of BL Present
 509 
                                DebugOut.Status[0] |= 0x02; // status of BL Present
510 
                                if(FC.Error[0] || FC.Error[1] || FC.Error[2] || FC.Error[3] || FC.Error[4]) DebugOut.Status[1] |= 0x01;
 510 
                                if(FC.Error[0] || FC.Error[1] || FC.Error[2] || FC.Error[3] || FC.Error[4]) DebugOut.Status[1] |= 0x01;
511 
                                else DebugOut.Status[1] &= ~0x01;
 511 
                                else DebugOut.Status[1] &= ~0x01;
512 
                                break;
 512 
                                break;
513 
                        default:
 513 
                        default:
514 
                                break;
 514 
                                break;
515 
                }
 515 
                }
516 
 
 516 
 
517 
                // every time we got new data from the FC via SPI call the navigation routine
 517 
                // every time we got new data from the FC via SPI call the navigation routine
518 
                // and update GPSStick that are returned to FC
 518 
                // and update GPSStick that are returned to FC
519 
                GPS_Navigation(&GPSData, &(ToFlightCtrl.GPSStick));
 519 
                GPS_Navigation(&GPSData, &(ToFlightCtrl.GPSStick));
520 
                ClearFCStatusFlags = 1;
 520 
                ClearFCStatusFlags = 1;
521 
 
 521 
 
522 
                if(counter)
 522 
                if(counter)
523 
                {
 523 
                {
524 
                        counter--;                                       // count down to enable servo
 524 
                        counter--;                                       // count down to enable servo
525 
                        if(!counter) TIMER2_Init();  // enable Servo Output
 525 
                        if(!counter) TIMER2_Init();  // enable Servo Output
526 
                }
 526 
                }
527 
 
 527 
 
528 
                SPI_RxBuffer_Request = 0;
 528 
                SPI_RxBuffer_Request = 0;
529 
                timeout = SetDelay(80); // 80 ms, new data are send every 20 ms
 529 
                timeout = SetDelay(80); // 80 ms, new data are send every 20 ms
530 
 
 530 
 
531 
                DebugOut.Analog[0] = FromFlightCtrl.AngleNick;
 531 
                DebugOut.Analog[0] = FromFlightCtrl.AngleNick;
532 
                DebugOut.Analog[1] = FromFlightCtrl.AngleRoll;
 532 
                DebugOut.Analog[1] = FromFlightCtrl.AngleRoll;
533 
                DebugOut.Analog[2] = FromFlightCtrl.AccNick;
 533 
                DebugOut.Analog[2] = FromFlightCtrl.AccNick;
534 
                DebugOut.Analog[3] = FromFlightCtrl.AccRoll;
 534 
                DebugOut.Analog[3] = FromFlightCtrl.AccRoll;
535 
                DebugOut.Analog[11] = FromFlightCtrl.GyroHeading/10;// in deg
 535 
                DebugOut.Analog[11] = FromFlightCtrl.GyroHeading/10;// in deg
536 
                Data3D.AngleNick = FromFlightCtrl.AngleNick;            // in 0.1 deg
 536 
                Data3D.AngleNick = FromFlightCtrl.AngleNick;            // in 0.1 deg
537 
                Data3D.AngleRoll = FromFlightCtrl.AngleRoll;            // in 0.1 deg
 537 
                Data3D.AngleRoll = FromFlightCtrl.AngleRoll;            // in 0.1 deg
538 
                Data3D.Heading   = FromFlightCtrl.GyroHeading;          // in 0.1 deg
 538 
                Data3D.Heading   = FromFlightCtrl.GyroHeading;          // in 0.1 deg
539 
        }       // EOF if(SPI_RxBuffer_Request)
 539 
        }       // EOF if(SPI_RxBuffer_Request)
540 
        else // no new SPI data
 540 
        else // no new SPI data
541 
        {
 541 
        {
542 
                if(CheckDelay(timeout) && (counter == 0))
 542 
                if(CheckDelay(timeout) && (counter == 0))
543 
                {
 543 
                {
544 
                        TIMER2_Deinit();  // disable Servo Output
 544 
                        TIMER2_Deinit();  // disable Servo Output
545 
                        counter = 50;     // reset counter for enabling Servo Output
 545 
                        counter = 50;     // reset counter for enabling Servo Output
546 
                }
 546 
                }
547 
        }
 547 
        }
548 
}
 548 
}
549 
 
 549 
 
550 
//------------------------------------------------------
 550 
//------------------------------------------------------
551 
void SPI0_GetFlightCtrlVersion(void)
 551 
void SPI0_GetFlightCtrlVersion(void)
552 
{
 552 
{
553 
        u32 timeout;
 553 
        u32 timeout;
554 
        u8 repeat;
 554 
        u8 repeat;
555 
        u8 msg[64];
 555 
        u8 msg[64];
556 
 
 556 
 
557 
        UART1_PutString("\r\n Looking for FlightControl");
 557 
        UART1_PutString("\r\n Looking for FlightControl");
558 
        FC_Version.Major = 0xFF;
 558 
        FC_Version.Major = 0xFF;
559 
        FC_Version.Minor = 0xFF;
 559 
        FC_Version.Minor = 0xFF;
560 
        FC_Version.Patch = 0xFF;
 560 
        FC_Version.Patch = 0xFF;
561 
        FC_Version.Compatible = 0xFF;
 561 
        FC_Version.Compatible = 0xFF;
562 
 
 562 
 
563 
        // polling FC version info
 563 
        // polling FC version info
564 
        repeat = 0;
 564 
        repeat = 0;
565 
        do
 565 
        do
566 
        {
 566 
        {
567 
                timeout = SetDelay(250);
 567 
                timeout = SetDelay(250);
568 
                do
 568 
                do
569 
                {
 569 
                {
570 
                        SPI0_UpdateBuffer();
 570 
                        SPI0_UpdateBuffer();
571 
                        if (FC_Version.Major != 0xFF)  break;
 571 
                        if (FC_Version.Major != 0xFF)  break;
572 
                }while (!CheckDelay(timeout));
 572 
                }while (!CheckDelay(timeout));
573 
                UART1_PutString(".");
 573 
                UART1_PutString(".");
574 
                repeat++;
 574 
                repeat++;
575 
        }while((FC_Version.Major == 0xFF) && (repeat < 40)); // 40*250ms = 10s
 575 
        }while((FC_Version.Major == 0xFF) && (repeat < 40)); // 40*250ms = 10s
576 
        // if we got it
 576 
        // if we got it
577 
        if (FC_Version.Major != 0xFF)
 577 
        if (FC_Version.Major != 0xFF)
578 
        {
 578 
        {
579 
                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);
 579 
                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);
580 
                UART1_PutString(msg);
 580 
                UART1_PutString(msg);
581 
        }
 581 
        }
582 
        else UART1_PutString("\n\r not found!");
 582 
        else UART1_PutString("\n\r not found!");
583 
}
 583 
}
584 
 
 584 
 
585 
 
 585 
 
586 
 
 586