Subversion Repositories NaviCtrl

Rev

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

Rev 285 Rev 286
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 
 
 70 
 
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_AltitudeSpeed = 0;
 106 
u32 ToFC_AltitudeRate = 0;
107 
s32 ToFC_AltitudeSetpoint = 0;
 107 
s32 ToFC_AltitudeSetpoint = 0;
108 
 
 108 
 
109 
SPI_Version_t FC_Version;
 109 
SPI_Version_t FC_Version;
110 
 
 110 
 
111 
//--------------------------------------------------------------
 111 
//--------------------------------------------------------------
112 
void SSP0_IRQHandler(void)
 112 
void SSP0_IRQHandler(void)
113 
{
 113 
{
114 
        static u8 rxchksum = 0;
 114 
        static u8 rxchksum = 0;
115 
        u8 rxdata;
 115 
        u8 rxdata;
116 
 
 116 
 
117 
        #define SPI_SYNC1       0
 117 
        #define SPI_SYNC1       0
118 
        #define SPI_SYNC2       1
 118 
        #define SPI_SYNC2       1
119 
        #define SPI_DATA        2
 119 
        #define SPI_DATA        2
120 
        static u8 SPI_State = SPI_SYNC1;
 120 
        static u8 SPI_State = SPI_SYNC1;
121 
 
 121 
 
122 
        IENABLE;
 122 
        IENABLE;
123 
 
 123 
 
124 
        // clear pending bits
 124 
        // clear pending bits
125 
        SSP_ClearITPendingBit(SSP0, SSP_IT_RxTimeOut);
 125 
        SSP_ClearITPendingBit(SSP0, SSP_IT_RxTimeOut);
126 
        SSP_ClearITPendingBit(SSP0, SSP_IT_RxFifo);
 126 
        SSP_ClearITPendingBit(SSP0, SSP_IT_RxFifo);
127 
 
 127 
 
128 
        // while RxFIFO not empty
 128 
        // while RxFIFO not empty
129 
        while (SSP_GetFlagStatus(SSP0, SSP_FLAG_RxFifoNotEmpty) == SET)
 129 
        while (SSP_GetFlagStatus(SSP0, SSP_FLAG_RxFifoNotEmpty) == SET)
130 
        {
 130 
        {
131 
                rxdata =  SSP0->DR; // catch the received byte
 131 
                rxdata =  SSP0->DR; // catch the received byte
132 
                // Fill TxFIFO while its not full or end of packet is reached
 132 
                // Fill TxFIFO while its not full or end of packet is reached
133 
                while (SSP_GetFlagStatus(SSP0, SSP_FLAG_TxFifoNotFull) == SET)
 133 
                while (SSP_GetFlagStatus(SSP0, SSP_FLAG_TxFifoNotFull) == SET)
134 
                {
 134 
                {
135 
                        if (SPI_TxBufferIndex  < SPI_TXBUFFER_LEN)   // still data to send ?
 135 
                        if (SPI_TxBufferIndex  < SPI_TXBUFFER_LEN)   // still data to send ?
136 
                        {
 136 
                        {
137 
                                SSP0->DR = SPI_TxBuffer[SPI_TxBufferIndex];       // send a byte
 137 
                                SSP0->DR = SPI_TxBuffer[SPI_TxBufferIndex];       // send a byte
138 
                                *Ptr_TxChksum += SPI_TxBuffer[SPI_TxBufferIndex]; // update checksum
 138 
                                *Ptr_TxChksum += SPI_TxBuffer[SPI_TxBufferIndex]; // update checksum
139 
                                SPI_TxBufferIndex++; // pointer to next byte
 139 
                                SPI_TxBufferIndex++; // pointer to next byte
140 
                        }
 140 
                        }
141 
                        else // end of packet is reached reset and copy data to tx buffer
 141 
                        else // end of packet is reached reset and copy data to tx buffer
142 
                        {
 142 
                        {
143 
                                SPI_TxBufferIndex = 0;  // reset buffer index
 143 
                                SPI_TxBufferIndex = 0;  // reset buffer index
144 
                                ToFlightCtrl.Chksum = 0;  // initialize checksum
 144 
                                ToFlightCtrl.Chksum = 0;  // initialize checksum
145 
                                ToFlightCtrl.BeepTime = BeepTime;  // set beeptime
 145 
                                ToFlightCtrl.BeepTime = BeepTime;  // set beeptime
146 
                                BeepTime = 0; // reset local beeptime
 146 
                                BeepTime = 0; // reset local beeptime
147 
                                // copy contents of ToFlightCtrl->SPI_TxBuffer
 147 
                                // copy contents of ToFlightCtrl->SPI_TxBuffer
148 
                                memcpy((u8 *) &(SPI_TxBuffer[2]), (u8 *) &ToFlightCtrl, sizeof(ToFlightCtrl));
 148 
                                memcpy((u8 *) &(SPI_TxBuffer[2]), (u8 *) &ToFlightCtrl, sizeof(ToFlightCtrl));
149 
                        }
 149 
                        }
150 
                }
 150 
                }
151 
                switch (SPI_State)
 151 
                switch (SPI_State)
152 
                {
 152 
                {
153 
                        case SPI_SYNC1:
 153 
                        case SPI_SYNC1:
154 
                                SPI_RxBufferIndex = 0; // reset buffer index
 154 
                                SPI_RxBufferIndex = 0; // reset buffer index
155 
                                rxchksum = rxdata;     // init checksum
 155 
                                rxchksum = rxdata;     // init checksum
156 
                                if (rxdata == SPI_RXSYNCBYTE1)
 156 
                                if (rxdata == SPI_RXSYNCBYTE1)
157 
                                {   // 1st syncbyte ok
 157 
                                {   // 1st syncbyte ok
158 
                                        SPI_State = SPI_SYNC2;  // step to sync2
 158 
                                        SPI_State = SPI_SYNC2;  // step to sync2
159 
                                }
 159 
                                }
160 
                                break;
 160 
                                break;
161 
                        case SPI_SYNC2:
 161 
                        case SPI_SYNC2:
162 
                                if (rxdata == SPI_RXSYNCBYTE2)
 162 
                                if (rxdata == SPI_RXSYNCBYTE2)
163 
                                {  // 2nd Syncbyte ok
 163 
                                {  // 2nd Syncbyte ok
164 
                                        rxchksum += rxdata;
 164 
                                        rxchksum += rxdata;
165 
                                        SPI_State = SPI_DATA;
 165 
                                        SPI_State = SPI_DATA;
166 
                                }  // 2nd Syncbyte does not match
 166 
                                }  // 2nd Syncbyte does not match
167 
                                else
 167 
                                else
168 
                                {
 168 
                                {
169 
                                        SPI_State  = SPI_SYNC1; //jump back to sync1
 169 
                                        SPI_State  = SPI_SYNC1; //jump back to sync1
170 
                                }
 170 
                                }
171 
                                break;
 171 
                                break;
172 
                        case SPI_DATA:
 172 
                        case SPI_DATA:
173 
                                SPI_RxBuffer[SPI_RxBufferIndex++]= rxdata; // copy databyte to rx buffer
 173 
                                SPI_RxBuffer[SPI_RxBufferIndex++]= rxdata; // copy databyte to rx buffer
174 
                                if (SPI_RxBufferIndex >= SPI_RXBUFFER_LEN) // end of packet is reached
 174 
                                if (SPI_RxBufferIndex >= SPI_RXBUFFER_LEN) // end of packet is reached
175 
                                {
 175 
                                {
176 
                                        if (rxdata == rxchksum) // verify checksum byte
 176 
                                        if (rxdata == rxchksum) // verify checksum byte
177 
                                        {
 177 
                                        {
178 
                                                // copy SPI_RxBuffer -> FromFlightCtrl
 178 
                                                // copy SPI_RxBuffer -> FromFlightCtrl
179 
                                                if(!SPI_RxBuffer_Request) // block writing to FromFlightCtrl on reading access
 179 
                                                if(!SPI_RxBuffer_Request) // block writing to FromFlightCtrl on reading access
180 
                                                {
 180 
                                                {
181 
                                                        memcpy((u8 *) &FromFlightCtrl, (u8 *) SPI_RxBuffer, sizeof(FromFlightCtrl));
 181 
                                                        memcpy((u8 *) &FromFlightCtrl, (u8 *) SPI_RxBuffer, sizeof(FromFlightCtrl));
182 
                                                        SPI_RxBuffer_Request = 1;
 182 
                                                        SPI_RxBuffer_Request = 1;
183 
                                                }
 183 
                                                }
184 
                                                // reset timeout counter on good packet
 184 
                                                // reset timeout counter on good packet
185 
                                                SPI0_Timeout = SetDelay(SPI0_TIMEOUT);
 185 
                                                SPI0_Timeout = SetDelay(SPI0_TIMEOUT);
186 
                                                DebugOut.Analog[13]++;
 186 
                                                DebugOut.Analog[13]++;
187 
                                        }
 187 
                                        }
188 
                                        else // bad checksum byte
 188 
                                        else // bad checksum byte
189 
                                        {
 189 
                                        {
190 
                                                DebugOut.Analog[12]++; // increase SPI chksum error counter
 190 
                                                DebugOut.Analog[12]++; // increase SPI chksum error counter
191 
                                        }
 191 
                                        }
192 
                                        SPI_State  = SPI_SYNC1; // reset state
 192 
                                        SPI_State  = SPI_SYNC1; // reset state
193 
                                }
 193 
                                }
194 
                                else // end of packet not reached
 194 
                                else // end of packet not reached
195 
                                {
 195 
                                {
196 
                                        rxchksum += rxdata;      // update checksum
 196 
                                        rxchksum += rxdata;      // update checksum
197 
                                }
 197 
                                }
198 
                                break;
 198 
                                break;
199 
                        default:
 199 
                        default:
200 
                                SPI_State  = SPI_SYNC1;
 200 
                                SPI_State  = SPI_SYNC1;
201 
                                break;
 201 
                                break;
202 
                }
 202 
                }
203 
        }
 203 
        }
204 
 
 204 
 
205 
        IDISABLE;
 205 
        IDISABLE;
206 
}
 206 
}
207 
 
 207 
 
208 
//--------------------------------------------------------------
 208 
//--------------------------------------------------------------
209 
void SPI0_Init(void)
 209 
void SPI0_Init(void)
210 
{
 210 
{
211 
        GPIO_InitTypeDef GPIO_InitStructure;
 211 
        GPIO_InitTypeDef GPIO_InitStructure;
212 
        SSP_InitTypeDef   SSP_InitStructure;
 212 
        SSP_InitTypeDef   SSP_InitStructure;
213 
 
 213 
 
214 
        UART1_PutString("\r\n SPI init...");
 214 
        UART1_PutString("\r\n SPI init...");
215 
 
 215 
 
216 
        SCU_APBPeriphClockConfig(__GPIO2 ,ENABLE);
 216 
        SCU_APBPeriphClockConfig(__GPIO2 ,ENABLE);
217 
        SCU_APBPeriphClockConfig(__SSP0 ,ENABLE);
 217 
        SCU_APBPeriphClockConfig(__SSP0 ,ENABLE);
218 
 
 218 
 
219 
        GPIO_DeInit(GPIO2);
 219 
        GPIO_DeInit(GPIO2);
220 
        //SSP0_CLK, SSP0_MOSI, SSP0_NSS pins
 220 
        //SSP0_CLK, SSP0_MOSI, SSP0_NSS pins
221 
        GPIO_StructInit(&GPIO_InitStructure);
 221 
        GPIO_StructInit(&GPIO_InitStructure);
222 
        GPIO_InitStructure.GPIO_Direction = GPIO_PinInput;
 222 
        GPIO_InitStructure.GPIO_Direction = GPIO_PinInput;
223 
        GPIO_InitStructure.GPIO_Pin = GPIO_Pin_4 | GPIO_Pin_5 | GPIO_Pin_7;
 223 
        GPIO_InitStructure.GPIO_Pin = GPIO_Pin_4 | GPIO_Pin_5 | GPIO_Pin_7;
224 
        GPIO_InitStructure.GPIO_Type = GPIO_Type_PushPull ;
 224 
        GPIO_InitStructure.GPIO_Type = GPIO_Type_PushPull ;
225 
        GPIO_InitStructure.GPIO_IPInputConnected = GPIO_IPInputConnected_Enable;
 225 
        GPIO_InitStructure.GPIO_IPInputConnected = GPIO_IPInputConnected_Enable;
226 
        GPIO_InitStructure.GPIO_Alternate = GPIO_InputAlt1; //SSP0_SCLK, SSP0_MOSI, SSP0_NSS
 226 
        GPIO_InitStructure.GPIO_Alternate = GPIO_InputAlt1; //SSP0_SCLK, SSP0_MOSI, SSP0_NSS
227 
        GPIO_Init (GPIO2, &GPIO_InitStructure);
 227 
        GPIO_Init (GPIO2, &GPIO_InitStructure);
228 
 
 228 
 
229 
        // SSP0_MISO pin GPIO2.6
 229 
        // SSP0_MISO pin GPIO2.6
230 
        GPIO_StructInit(&GPIO_InitStructure);
 230 
        GPIO_StructInit(&GPIO_InitStructure);
231 
        GPIO_InitStructure.GPIO_Direction = GPIO_PinOutput;
 231 
        GPIO_InitStructure.GPIO_Direction = GPIO_PinOutput;
232 
        GPIO_InitStructure.GPIO_Pin = GPIO_Pin_6;
 232 
        GPIO_InitStructure.GPIO_Pin = GPIO_Pin_6;
233 
        GPIO_InitStructure.GPIO_Type = GPIO_Type_PushPull ;
 233 
        GPIO_InitStructure.GPIO_Type = GPIO_Type_PushPull ;
234 
        GPIO_InitStructure.GPIO_IPInputConnected = GPIO_IPInputConnected_Enable;
 234 
        GPIO_InitStructure.GPIO_IPInputConnected = GPIO_IPInputConnected_Enable;
235 
        GPIO_InitStructure.GPIO_Alternate = GPIO_OutputAlt2; //SSP0_MISO
 235 
        GPIO_InitStructure.GPIO_Alternate = GPIO_OutputAlt2; //SSP0_MISO
236 
        GPIO_Init (GPIO2, &GPIO_InitStructure);
 236 
        GPIO_Init (GPIO2, &GPIO_InitStructure);
237 
 
 237 
 
238 
        SSP_DeInit(SSP0);
 238 
        SSP_DeInit(SSP0);
239 
        SSP_StructInit(&SSP_InitStructure);
 239 
        SSP_StructInit(&SSP_InitStructure);
240 
        SSP_InitStructure.SSP_FrameFormat = SSP_FrameFormat_Motorola;
 240 
        SSP_InitStructure.SSP_FrameFormat = SSP_FrameFormat_Motorola;
241 
        SSP_InitStructure.SSP_Mode = SSP_Mode_Slave;
 241 
        SSP_InitStructure.SSP_Mode = SSP_Mode_Slave;
242 
        SSP_InitStructure.SSP_SlaveOutput = SSP_SlaveOutput_Enable;
 242 
        SSP_InitStructure.SSP_SlaveOutput = SSP_SlaveOutput_Enable;
243 
        SSP_InitStructure.SSP_CPHA = SSP_CPHA_1Edge;
 243 
        SSP_InitStructure.SSP_CPHA = SSP_CPHA_1Edge;
244 
        SSP_InitStructure.SSP_CPOL = SSP_CPOL_Low;
 244 
        SSP_InitStructure.SSP_CPOL = SSP_CPOL_Low;
245 
        SSP_InitStructure.SSP_ClockRate = 0;
 245 
        SSP_InitStructure.SSP_ClockRate = 0;
246 
 
 246 
 
247 
        SSP_Init(SSP0, &SSP_InitStructure);
 247 
        SSP_Init(SSP0, &SSP_InitStructure);
248 
        SSP_ITConfig(SSP0, SSP_IT_RxFifo | SSP_IT_RxTimeOut, ENABLE);
 248 
        SSP_ITConfig(SSP0, SSP_IT_RxFifo | SSP_IT_RxTimeOut, ENABLE);
249 
 
 249 
 
250 
        SSP_Cmd(SSP0, ENABLE);
 250 
        SSP_Cmd(SSP0, ENABLE);
251 
        // initialize the syncbytes in the tx buffer
 251 
        // initialize the syncbytes in the tx buffer
252 
        SPI_TxBuffer[0] = SPI_TXSYNCBYTE1;
 252 
        SPI_TxBuffer[0] = SPI_TXSYNCBYTE1;
253 
        SPI_TxBuffer[1] = SPI_TXSYNCBYTE2;
 253 
        SPI_TxBuffer[1] = SPI_TXSYNCBYTE2;
254 
        // set the pointer to the checksum byte in the tx buffer
 254 
        // set the pointer to the checksum byte in the tx buffer
255 
        Ptr_TxChksum = (u8 *) &(((ToFlightCtrl_t *) &(SPI_TxBuffer[2]))->Chksum);
 255 
        Ptr_TxChksum = (u8 *) &(((ToFlightCtrl_t *) &(SPI_TxBuffer[2]))->Chksum);
256 
 
 256 
 
257 
        ToFlightCtrl.GPSStick.Nick = 0;
 257 
        ToFlightCtrl.GPSStick.Nick = 0;
258 
        ToFlightCtrl.GPSStick.Roll = 0;
 258 
        ToFlightCtrl.GPSStick.Roll = 0;
259 
        ToFlightCtrl.GPSStick.Yaw = 0;
 259 
        ToFlightCtrl.GPSStick.Yaw = 0;
260 
 
 260 
 
261 
        VIC_Config(SSP0_ITLine, VIC_IRQ, PRIORITY_SPI0);
 261 
        VIC_Config(SSP0_ITLine, VIC_IRQ, PRIORITY_SPI0);
262 
        VIC_ITCmd(SSP0_ITLine, ENABLE);
 262 
        VIC_ITCmd(SSP0_ITLine, ENABLE);
263 
 
 263 
 
264 
        SPI0_Timeout = SetDelay(4*SPI0_TIMEOUT);
 264 
        SPI0_Timeout = SetDelay(4*SPI0_TIMEOUT);
265 
 
 265 
 
266 
        UART1_PutString("ok");
 266 
        UART1_PutString("ok");
267 
}
 267 
}
268 
 
 268 
 
269 
 
 269 
 
270 
//------------------------------------------------------
 270 
//------------------------------------------------------
271 
void SPI0_UpdateBuffer(void)
 271 
void SPI0_UpdateBuffer(void)
272 
{
 272 
{
273 
        static u32 timeout = 0;
 273 
        static u32 timeout = 0;
274 
        static u8 counter = 50;
 274 
        static u8 counter = 50;
275 
        static u8 CompassCalState = 0;
 275 
        static u8 CompassCalState = 0;
276 
        static u8 FCCalibActive = 0;
 276 
        static u8 FCCalibActive = 0;
277 
 
 277 
 
278 
        if (SPI_RxBuffer_Request)
 278 
        if (SPI_RxBuffer_Request)
279 
        {
 279 
        {
280 
                // avoid sending data via SPI during the update of the  ToFlightCtrl structure
 280 
                // avoid sending data via SPI during the update of the  ToFlightCtrl structure
281 
                VIC_ITCmd(SSP0_ITLine, DISABLE); // disable SPI interrupt
 281 
                VIC_ITCmd(SSP0_ITLine, DISABLE); // disable SPI interrupt
282 
                ToFlightCtrl.CompassHeading = Compass_Heading;
 282 
                ToFlightCtrl.CompassHeading = Compass_Heading;
283 
                DebugOut.Analog[10] = ToFlightCtrl.CompassHeading;
 283 
                DebugOut.Analog[10] = ToFlightCtrl.CompassHeading;
284 
                if(ToFlightCtrl.CompassHeading >= 0) ToFlightCtrl.CompassHeading = (360 + ToFlightCtrl.CompassHeading + FromFlightCtrl.GyroYaw / 12) % 360;
 284 
                if(ToFlightCtrl.CompassHeading >= 0) ToFlightCtrl.CompassHeading = (360 + ToFlightCtrl.CompassHeading + FromFlightCtrl.GyroYaw / 12) % 360;
285 
//              ToFlightCtrl.MagVecX = MagVector.X;
 285 
                ToFlightCtrl.MagVecX = MagVector.X;
286 
//              ToFlightCtrl.MagVecY = MagVector.Y;
 286 
                ToFlightCtrl.MagVecY = MagVector.Y;
287 
//              ToFlightCtrl.MagVecZ = MagVector.Z;
 287 
                ToFlightCtrl.MagVecZ = MagVector.Z;
288 
                ToFlightCtrl.NCStatus = 0;
 288 
                ToFlightCtrl.NCStatus = 0;
289 
                // cycle spi commands
 289 
                // cycle spi commands
290 
                ToFlightCtrl.Command = SPI_CommandSequence[SPI_CommandCounter++];
 290 
                ToFlightCtrl.Command = SPI_CommandSequence[SPI_CommandCounter++];
291 
                // restart command cycle at the end
 291 
                // restart command cycle at the end
292 
                if (SPI_CommandCounter >= sizeof(SPI_CommandSequence)) SPI_CommandCounter = 0;
 292 
                if (SPI_CommandCounter >= sizeof(SPI_CommandSequence)) SPI_CommandCounter = 0;
293 
 
 293 
 
294 
                switch (ToFlightCtrl.Command)
 294 
                switch (ToFlightCtrl.Command)
295 
                {
 295 
                {
296 
                        case  SPI_NCCMD_KALMAN:
 296 
                        case  SPI_NCCMD_KALMAN:
297 
                                CalcHeadFree();
 297 
                                CalcHeadFree();
298 
                                ToFlightCtrl.Param.sByte[0] = (s8) Kalman_K;
 298 
                                ToFlightCtrl.Param.sByte[0] = (s8) Kalman_K;
299 
                                ToFlightCtrl.Param.sByte[1] = (s8) Kalman_MaxFusion;
 299 
                                ToFlightCtrl.Param.sByte[1] = (s8) Kalman_MaxFusion;
300 
                                ToFlightCtrl.Param.sByte[2] = (s8) Kalman_MaxDrift;
 300 
                                ToFlightCtrl.Param.sByte[2] = (s8) Kalman_MaxDrift;
301 
                                ToFlightCtrl.Param.Byte[3]      = (u8) SerialLinkOkay;
 301 
                                ToFlightCtrl.Param.Byte[3]      = (u8) SerialLinkOkay;
302 
                                ToFlightCtrl.Param.sByte[4] = (s8) ToFcGpsZ;
 302 
                                ToFlightCtrl.Param.sByte[4] = (s8) ToFcGpsZ;
303 
                                ToFlightCtrl.Param.Byte[5] = (s8) ToFC_Rotate_C;
 303 
                                ToFlightCtrl.Param.Byte[5] = (s8) ToFC_Rotate_C;
304 
                                ToFlightCtrl.Param.Byte[6] = (s8) ToFC_Rotate_S;
 304 
                                ToFlightCtrl.Param.Byte[6] = (s8) ToFC_Rotate_S;
305 
                //ToFlightCtrl.Param.Byte[7] =
305 
                //ToFlightCtrl.Param.Byte[7] =
306 
                                if(CAM_Orientation.UpdateMask & CAM_UPDATE_AZIMUTH)
 306 
                                if(CAM_Orientation.UpdateMask & CAM_UPDATE_AZIMUTH)
307 
                                {
 307 
                                {
308 
                                        ToFlightCtrl.Param.sInt[4] = CAM_Orientation.Azimuth;
 308 
                                        ToFlightCtrl.Param.sInt[4] = CAM_Orientation.Azimuth;
309 
                                        CAM_Orientation.UpdateMask &= ~CAM_UPDATE_AZIMUTH;
 309 
                                        CAM_Orientation.UpdateMask &= ~CAM_UPDATE_AZIMUTH;
310 
                                }
 310 
                                }
311 
                                else
 311 
                                else
312 
                                {
 312 
                                {
313 
                                        ToFlightCtrl.Param.sInt[4] = -1;
 313 
                                        ToFlightCtrl.Param.sInt[4] = -1;
314 
                                }
 314 
                                }
315 
                                ToFlightCtrl.Param.sInt[5] = CAM_Orientation.Elevation;
 315 
                                ToFlightCtrl.Param.sInt[5] = CAM_Orientation.Elevation;
316 
                                break;
 316 
                                break;
317 
 
 317 
 
318 
                        case SPI_NCCMD_VERSION:
 318 
                        case SPI_NCCMD_VERSION:
319 
                                ToFlightCtrl.Param.Byte[0] = VERSION_MAJOR;
 319 
                                ToFlightCtrl.Param.Byte[0] = VERSION_MAJOR;
320 
                                ToFlightCtrl.Param.Byte[1] = VERSION_MINOR;
 320 
                                ToFlightCtrl.Param.Byte[1] = VERSION_MINOR;
321 
                                ToFlightCtrl.Param.Byte[2] = VERSION_PATCH;
 321 
                                ToFlightCtrl.Param.Byte[2] = VERSION_PATCH;
322 
                                ToFlightCtrl.Param.Byte[3] = FC_SPI_COMPATIBLE;
 322 
                                ToFlightCtrl.Param.Byte[3] = FC_SPI_COMPATIBLE;
323 
                                ToFlightCtrl.Param.Byte[4] = Version_HW;
 323 
                                ToFlightCtrl.Param.Byte[4] = Version_HW;
324 
                                ToFlightCtrl.Param.Byte[5] = DebugOut.Status[0];
 324 
                                ToFlightCtrl.Param.Byte[5] = DebugOut.Status[0];
325 
                                ToFlightCtrl.Param.Byte[6] = DebugOut.Status[1];
 325 
                                ToFlightCtrl.Param.Byte[6] = DebugOut.Status[1];
326 
                                ToFlightCtrl.Param.Byte[7] = ErrorCode;
 326 
                                ToFlightCtrl.Param.Byte[7] = ErrorCode;
327 
                                break;
 327 
                                break;
328 
 
 328 
 
329 
                        case SPI_NCCMD_GPSINFO:
 329 
                        case SPI_NCCMD_GPSINFO:
330 
                                ToFlightCtrl.Param.Byte[0] = GPSData.Flags;
 330 
                                ToFlightCtrl.Param.Byte[0] = GPSData.Flags;
331 
                                ToFlightCtrl.Param.Byte[1] = GPSData.NumOfSats;
 331 
                                ToFlightCtrl.Param.Byte[1] = GPSData.NumOfSats;
332 
                                ToFlightCtrl.Param.Byte[2] = GPSData.SatFix;
 332 
                                ToFlightCtrl.Param.Byte[2] = GPSData.SatFix;
333 
                                ToFlightCtrl.Param.Byte[3] = GPSData.Speed_Ground / 100; // m/s
 333 
                                ToFlightCtrl.Param.Byte[3] = GPSData.Speed_Ground / 100; // m/s
334 
                                ToFlightCtrl.Param.Int[2]  = NaviData.HomePositionDeviation.Distance; // dm   //4&5
 334 
                                ToFlightCtrl.Param.Int[2]  = NaviData.HomePositionDeviation.Distance; // dm   //4&5
335 
                                ToFlightCtrl.Param.sInt[3] = NaviData.HomePositionDeviation.Bearing;  // deg  //6&7
 335 
                                ToFlightCtrl.Param.sInt[3] = NaviData.HomePositionDeviation.Bearing;  // deg  //6&7
336 
                                ToFlightCtrl.Param.Byte[8] = (s8)(FC_WP_EventChannel - 110);
 336 
                                ToFlightCtrl.Param.Byte[8] = (s8)(FC_WP_EventChannel - 110);
337 
                                ToFlightCtrl.Param.Byte[9] = (u8) ToFC_AltitudeSpeed;
 337 
                                ToFlightCtrl.Param.Byte[9] = (u8) ToFC_AltitudeRate;
338 
                                ToFlightCtrl.Param.sInt[5] = (s16) ToFC_AltitudeSetpoint;
 338 
                                ToFlightCtrl.Param.sInt[5] = (s16) ToFC_AltitudeSetpoint;
339 
                                break;
 339 
                                break;
340 
                        default:
 340 
                        default:
341 
                                break;
 341 
                                break;
342 
// 0 = 0,1
 342 
// 0 = 0,1
343 
// 1 = 2,3
 343 
// 1 = 2,3
344 
// 2 = 4,5
 344 
// 2 = 4,5
345 
// 3 = 6,7
 345 
// 3 = 6,7
346 
// 4 = 8,9
 346 
// 4 = 8,9
347 
// 5 = 10,11
 347 
// 5 = 10,11
348 
                }
 348 
                }
349 
                VIC_ITCmd(SSP0_ITLine, ENABLE);         // enable SPI interrupt
 349 
                VIC_ITCmd(SSP0_ITLine, ENABLE);         // enable SPI interrupt
350 
 
 350 
 
351 
 
 351 
 
352 
                switch(FromFlightCtrl.Command)
 352 
                switch(FromFlightCtrl.Command)
353 
                {
 353 
                {
354 
                        case SPI_FCCMD_USER:
 354 
                        case SPI_FCCMD_USER:
355 
                                Parameter.User1 = FromFlightCtrl.Param.Byte[0];
 355 
                                Parameter.User1 = FromFlightCtrl.Param.Byte[0];
356 
                                Parameter.User2 = FromFlightCtrl.Param.Byte[1];
 356 
                                Parameter.User2 = FromFlightCtrl.Param.Byte[1];
357 
                                Parameter.User3 = FromFlightCtrl.Param.Byte[2];
 357 
                                Parameter.User3 = FromFlightCtrl.Param.Byte[2];
358 
                                Parameter.User4 = FromFlightCtrl.Param.Byte[3];
 358 
                                Parameter.User4 = FromFlightCtrl.Param.Byte[3];
359 
                                Parameter.User5 = FromFlightCtrl.Param.Byte[4];
 359 
                                Parameter.User5 = FromFlightCtrl.Param.Byte[4];
360 
                                Parameter.User6 = FromFlightCtrl.Param.Byte[5];
 360 
                                Parameter.User6 = FromFlightCtrl.Param.Byte[5];
361 
                                Parameter.User7 = FromFlightCtrl.Param.Byte[6];
 361 
                                Parameter.User7 = FromFlightCtrl.Param.Byte[6];
362 
                                Parameter.User8 = FromFlightCtrl.Param.Byte[7];
 362 
                                Parameter.User8 = FromFlightCtrl.Param.Byte[7];
363 
                                if(ClearFCStatusFlags)
 363 
                                if(ClearFCStatusFlags)
364 
                                {
 364 
                                {
365 
                                        FC.StatusFlags = 0;
 365 
                                        FC.StatusFlags = 0;
366 
                                        ClearFCStatusFlags = 0;
 366 
                                        ClearFCStatusFlags = 0;
367 
                                }
 367 
                                }
368 
                                FC.StatusFlags |= FromFlightCtrl.Param.Byte[8];
 368 
                                FC.StatusFlags |= FromFlightCtrl.Param.Byte[8];
369 
                                if(FC.StatusFlags&FC_STATUS_CALIBRATE && !FCCalibActive)
 369 
                                if(FC.StatusFlags&FC_STATUS_CALIBRATE && !FCCalibActive)
370 
                                {
 370 
                                {
371 
                                        Compass_Init();
 371 
                                        Compass_Init();
372 
                                        FCCalibActive = 1;
 372 
                                        FCCalibActive = 1;
373 
                                }
 373 
                                }
374 
                                else
 374 
                                else
375 
                                {
 375 
                                {
376 
                                        FCCalibActive = 0;
 376 
                                        FCCalibActive = 0;
377 
                                }
 377 
                                }
378 
                                Parameter.ActiveSetting = FromFlightCtrl.Param.Byte[9];
 378 
                                Parameter.ActiveSetting = FromFlightCtrl.Param.Byte[9];
379 
                                DebugOut.Analog[5] = FC.StatusFlags;
 379 
                                DebugOut.Analog[5] = FC.StatusFlags;
380 
                                NaviData.FCStatusFlags = FC.StatusFlags;
 380 
                                NaviData.FCStatusFlags = FC.StatusFlags;
381 
                                HeadFreeStartAngle = (s32) FromFlightCtrl.Param.Byte[10] * 20; // convert to 0.1°
 381 
                                HeadFreeStartAngle = (s32) FromFlightCtrl.Param.Byte[10] * 20; // convert to 0.1°
382 
                                break;
 382 
                                break;
383 
 
 383 
 
384 
                        case SPI_FCCMD_ACCU:
 384 
                        case SPI_FCCMD_ACCU:
385 
                                FC.BAT_Current = FromFlightCtrl.Param.Int[0];
 385 
                                FC.BAT_Current = FromFlightCtrl.Param.Int[0];
386 
                                FC.BAT_UsedCapacity = FromFlightCtrl.Param.Int[1];
 386 
                                FC.BAT_UsedCapacity = FromFlightCtrl.Param.Int[1];
387 
                                FC.BAT_Voltage = FromFlightCtrl.Param.Byte[4];
 387 
                                FC.BAT_Voltage = FromFlightCtrl.Param.Byte[4];
388 
                                Parameter.LowVoltageWarning = FromFlightCtrl.Param.Byte[5];
 388 
                                Parameter.LowVoltageWarning = FromFlightCtrl.Param.Byte[5];
389 
                                NaviData.UBat = FC.BAT_Voltage;
 389 
                                NaviData.UBat = FC.BAT_Voltage;
390 
                                NaviData.Current = FC.BAT_Current;
 390 
                                NaviData.Current = FC.BAT_Current;
391 
                                NaviData.UsedCapacity = FC.BAT_UsedCapacity;
 391 
                                NaviData.UsedCapacity = FC.BAT_UsedCapacity;
392 
                                break;
 392 
                                break;
393 
 
 393 
 
394 
#define CHK_POTI(b,a) { if(a < 248) b = a; else b = FC.Poti[255 - a]; }
 394 
#define CHK_POTI(b,a) { if(a < 248) b = a; else b = FC.Poti[255 - a]; }
395 
#define CHK_POTI_MM(b,a,min,max) {CHK_POTI(b,a); LIMIT_MIN_MAX(b, min, max); }
 395 
#define CHK_POTI_MM(b,a,min,max) {CHK_POTI(b,a); LIMIT_MIN_MAX(b, min, max); }
396 
 
 396 
 
397 
                        case SPI_FCCMD_PARAMETER1:
 397 
                        case SPI_FCCMD_PARAMETER1:
398 
                                CHK_POTI_MM(Parameter.NaviGpsModeControl,FromFlightCtrl.Param.Byte[0],0,255);
 398 
                                CHK_POTI_MM(Parameter.NaviGpsModeControl,FromFlightCtrl.Param.Byte[0],0,255);
399 
                                CHK_POTI_MM(Parameter.NaviGpsGain,FromFlightCtrl.Param.Byte[1],0,255);
 399 
                                CHK_POTI_MM(Parameter.NaviGpsGain,FromFlightCtrl.Param.Byte[1],0,255);
400 
                                CHK_POTI_MM(Parameter.NaviGpsP,FromFlightCtrl.Param.Byte[2],0,255);
 400 
                                CHK_POTI_MM(Parameter.NaviGpsP,FromFlightCtrl.Param.Byte[2],0,255);
401 
                                CHK_POTI_MM(Parameter.NaviGpsI,FromFlightCtrl.Param.Byte[3],0,255);
 401 
                                CHK_POTI_MM(Parameter.NaviGpsI,FromFlightCtrl.Param.Byte[3],0,255);
402 
                                CHK_POTI_MM(Parameter.NaviGpsD,FromFlightCtrl.Param.Byte[4],0,255);
 402 
                                CHK_POTI_MM(Parameter.NaviGpsD,FromFlightCtrl.Param.Byte[4],0,255);
403 
                                CHK_POTI_MM(Parameter.NaviGpsACC,FromFlightCtrl.Param.Byte[5],0,255);
 403 
                                CHK_POTI_MM(Parameter.NaviGpsACC,FromFlightCtrl.Param.Byte[5],0,255);
404 
                                Parameter.NaviGpsMinSat = FromFlightCtrl.Param.Byte[6];
 404 
                                Parameter.NaviGpsMinSat = FromFlightCtrl.Param.Byte[6];
405 
                                Parameter.NaviStickThreshold = FromFlightCtrl.Param.Byte[7];
 405 
                                Parameter.NaviStickThreshold = FromFlightCtrl.Param.Byte[7];
406 
                                CHK_POTI_MM(Parameter.NaviOperatingRadius,FromFlightCtrl.Param.Byte[8],0,255);
 406 
                                CHK_POTI_MM(Parameter.NaviOperatingRadius,FromFlightCtrl.Param.Byte[8],0,255);
407 
                                CHK_POTI_MM(Parameter.NaviWindCorrection,FromFlightCtrl.Param.Byte[9],0,255);
 407 
                                CHK_POTI_MM(Parameter.NaviWindCorrection,FromFlightCtrl.Param.Byte[9],0,255);
408 
                                CHK_POTI_MM(Parameter.NaviSpeedCompensation,FromFlightCtrl.Param.Byte[10],0,255);
 408 
                                CHK_POTI_MM(Parameter.NaviSpeedCompensation,FromFlightCtrl.Param.Byte[10],0,255);
409 
                                CHK_POTI_MM(Parameter.NaviAngleLimitation,FromFlightCtrl.Param.Byte[11],0,255);
 409 
                                CHK_POTI_MM(Parameter.NaviAngleLimitation,FromFlightCtrl.Param.Byte[11],0,255);
410 
                                break;
 410 
                                break;
411 
 
 411 
 
412 
                        case SPI_FCCMD_STICK:
 412 
                        case SPI_FCCMD_STICK:
413 
                                FC.StickGas     = FromFlightCtrl.Param.sByte[0];
 413 
                                FC.StickGas     = FromFlightCtrl.Param.sByte[0];
414 
                                FC.StickYaw     = FromFlightCtrl.Param.sByte[1];
 414 
                                FC.StickYaw     = FromFlightCtrl.Param.sByte[1];
415 
                                FC.StickRoll    = FromFlightCtrl.Param.sByte[2];
 415 
                                FC.StickRoll    = FromFlightCtrl.Param.sByte[2];
416 
                                FC.StickNick    = FromFlightCtrl.Param.sByte[3];
 416 
                                FC.StickNick    = FromFlightCtrl.Param.sByte[3];
417 
                                FC.Poti[0]              = FromFlightCtrl.Param.Byte[4];
 417 
                                FC.Poti[0]              = FromFlightCtrl.Param.Byte[4];
418 
                                FC.Poti[1]              = FromFlightCtrl.Param.Byte[5];
 418 
                                FC.Poti[1]              = FromFlightCtrl.Param.Byte[5];
419 
                                FC.Poti[2]              = FromFlightCtrl.Param.Byte[6];
 419 
                                FC.Poti[2]              = FromFlightCtrl.Param.Byte[6];
420 
                                FC.Poti[3]              = FromFlightCtrl.Param.Byte[7];
 420 
                                FC.Poti[3]              = FromFlightCtrl.Param.Byte[7];
421 
                                FC.Poti[4]              = FromFlightCtrl.Param.Byte[8];
 421 
                                FC.Poti[4]              = FromFlightCtrl.Param.Byte[8];
422 
                                FC.Poti[5]              = FromFlightCtrl.Param.Byte[9];
 422 
                                FC.Poti[5]              = FromFlightCtrl.Param.Byte[9];
423 
                                FC.Poti[6]              = FromFlightCtrl.Param.Byte[10];
 423 
                                FC.Poti[6]              = FromFlightCtrl.Param.Byte[10];
424 
                                FC.Poti[7]              = FromFlightCtrl.Param.Byte[11];
 424 
                                FC.Poti[7]              = FromFlightCtrl.Param.Byte[11];
425 
                                break;
 425 
                                break;
426 
 
 426 
 
427 
                        case SPI_FCCMD_MISC:
 427 
                        case SPI_FCCMD_MISC:
428 
                                if(CompassCalState != FromFlightCtrl.Param.Byte[0])
 428 
                                if(CompassCalState != FromFlightCtrl.Param.Byte[0])
429 
                                {       // put only new CompassCalState into queue to send via I2C
 429 
                                {       // put only new CompassCalState into queue to send via I2C
430 
                                        CompassCalState = FromFlightCtrl.Param.Byte[0];
 430 
                                        CompassCalState = FromFlightCtrl.Param.Byte[0];
431 
                                        Compass_SetCalState(CompassCalState);
 431 
                                        Compass_SetCalState(CompassCalState);
432 
                                }
 432 
                                }
433 
                                Parameter.NaviPH_LoginTime = FromFlightCtrl.Param.Byte[1];
 433 
                                Parameter.NaviPH_LoginTime = FromFlightCtrl.Param.Byte[1];
434 
                                NaviData.Variometer = (NaviData.Variometer + 2 * (FromFlightCtrl.Param.sInt[1] - NaviData.Altimeter)) / 2; // provisorisch
 434 
                                NaviData.Variometer = (NaviData.Variometer + 2 * (FromFlightCtrl.Param.sInt[1] - NaviData.Altimeter)) / 2; // provisorisch
435 
                                NaviData.Altimeter = FromFlightCtrl.Param.sInt[1]; // is located at byte 2 and 3
 435 
                                NaviData.Altimeter = FromFlightCtrl.Param.sInt[1]; // is located at byte 2 and 3
436 
                                NaviData.SetpointAltitude = FromFlightCtrl.Param.sInt[2]; // is located at byte 4 and 5
 436 
                                NaviData.SetpointAltitude = FromFlightCtrl.Param.sInt[2]; // is located at byte 4 and 5
437 
                                CHK_POTI_MM(Parameter.NaviGpsPLimit,FromFlightCtrl.Param.Byte[6],0,255);
 437 
                                CHK_POTI_MM(Parameter.NaviGpsPLimit,FromFlightCtrl.Param.Byte[6],0,255);
438 
                                CHK_POTI_MM(Parameter.NaviGpsILimit,FromFlightCtrl.Param.Byte[7],0,255);
 438 
                                CHK_POTI_MM(Parameter.NaviGpsILimit,FromFlightCtrl.Param.Byte[7],0,255);
439 
                                CHK_POTI_MM(Parameter.NaviGpsDLimit,FromFlightCtrl.Param.Byte[8],0,255);
 439 
                                CHK_POTI_MM(Parameter.NaviGpsDLimit,FromFlightCtrl.Param.Byte[8],0,255);
440 
                                FC.RC_Quality   = FromFlightCtrl.Param.Byte[9];
 440 
                                FC.RC_Quality   = FromFlightCtrl.Param.Byte[9];
441 
                                FC.RC_RSSI              = FromFlightCtrl.Param.Byte[10];
 441 
                                FC.RC_RSSI              = FromFlightCtrl.Param.Byte[10];
442 
                                NaviData.RC_Quality = FC.RC_Quality;
 442 
                                NaviData.RC_Quality = FC.RC_Quality;
443 
                                NaviData.RC_RSSI = FC.RC_RSSI;
 443 
                                NaviData.RC_RSSI = FC.RC_RSSI;
444 
                                NaviData.Gas    = (FC.BAT_Voltage * (u32) FromFlightCtrl.Param.Byte[11]) / (u32) Parameter.LowVoltageWarning;
 444 
                                NaviData.Gas    = (FC.BAT_Voltage * (u32) FromFlightCtrl.Param.Byte[11]) / (u32) Parameter.LowVoltageWarning;
445 
                                break;
 445 
                                break;
446 
 
 446 
 
447 
                        case SPI_FCCMD_SERVOS:
 447 
                        case SPI_FCCMD_SERVOS:
448 
                                ServoParams.Refresh             = FromFlightCtrl.Param.Byte[0];
 448 
                                ServoParams.Refresh             = FromFlightCtrl.Param.Byte[0];
449 
                                ServoParams.CompInvert  = FromFlightCtrl.Param.Byte[1];
 449 
                                ServoParams.CompInvert  = FromFlightCtrl.Param.Byte[1];
450 
                                ServoParams.NickControl = FromFlightCtrl.Param.Byte[2];
 450 
                                ServoParams.NickControl = FromFlightCtrl.Param.Byte[2];
451 
                                ServoParams.NickComp    = FromFlightCtrl.Param.Byte[3];
 451 
                                ServoParams.NickComp    = FromFlightCtrl.Param.Byte[3];
452 
                                ServoParams.NickMin             = FromFlightCtrl.Param.Byte[4];
 452 
                                ServoParams.NickMin             = FromFlightCtrl.Param.Byte[4];
453 
                                ServoParams.NickMax             = FromFlightCtrl.Param.Byte[5];
 453 
                                ServoParams.NickMax             = FromFlightCtrl.Param.Byte[5];
454 
                                ServoParams.RollControl = FromFlightCtrl.Param.Byte[6];
 454 
                                ServoParams.RollControl = FromFlightCtrl.Param.Byte[6];
455 
                                ServoParams.RollComp    = FromFlightCtrl.Param.Byte[7];
 455 
                                ServoParams.RollComp    = FromFlightCtrl.Param.Byte[7];
456 
                                ServoParams.RollMin             = FromFlightCtrl.Param.Byte[8];
 456 
                                ServoParams.RollMin             = FromFlightCtrl.Param.Byte[8];
457 
                                ServoParams.RollMax             = FromFlightCtrl.Param.Byte[9];
 457 
                                ServoParams.RollMax             = FromFlightCtrl.Param.Byte[9];
458 
                                break;
 458 
                                break;
459 
 
 459 
 
460 
                        case SPI_FCCMD_VERSION:
 460 
                        case SPI_FCCMD_VERSION:
461 
                                FC_Version.Major                = FromFlightCtrl.Param.Byte[0];
 461 
                                FC_Version.Major                = FromFlightCtrl.Param.Byte[0];
462 
                                FC_Version.Minor                = FromFlightCtrl.Param.Byte[1];
 462 
                                FC_Version.Minor                = FromFlightCtrl.Param.Byte[1];
463 
                                FC_Version.Patch                = FromFlightCtrl.Param.Byte[2];
 463 
                                FC_Version.Patch                = FromFlightCtrl.Param.Byte[2];
464 
                                FC_Version.Compatible   = FromFlightCtrl.Param.Byte[3];
 464 
                                FC_Version.Compatible   = FromFlightCtrl.Param.Byte[3];
465 
                                FC_Version.Hardware             = FromFlightCtrl.Param.Byte[4];
 465 
                                FC_Version.Hardware             = FromFlightCtrl.Param.Byte[4];
466 
                                FC.Error[0]                     = FromFlightCtrl.Param.Byte[5];
 466 
                                FC.Error[0]                     = FromFlightCtrl.Param.Byte[5];
467 
                                FC.Error[1]                     = FromFlightCtrl.Param.Byte[6];
 467 
                                FC.Error[1]                     = FromFlightCtrl.Param.Byte[6];
468 
                                FC.Error[2]                     = FromFlightCtrl.Param.Byte[7];
 468 
                                FC.Error[2]                     = FromFlightCtrl.Param.Byte[7];
469 
                                FC.Error[3]                     = FromFlightCtrl.Param.Byte[8];
 469 
                                FC.Error[3]                     = FromFlightCtrl.Param.Byte[8];
470 
                                FC.Error[4]                     = FromFlightCtrl.Param.Byte[9];
 470 
                                FC.Error[4]                     = FromFlightCtrl.Param.Byte[9];
471 
                                DebugOut.Status[0] |= 0x01; // status of FC Present
 471 
                                DebugOut.Status[0] |= 0x01; // status of FC Present
472 
                                DebugOut.Status[0] |= 0x02; // status of BL Present
 472 
                                DebugOut.Status[0] |= 0x02; // status of BL Present
473 
                                if(FC.Error[0] || FC.Error[1] || FC.Error[2] || FC.Error[3] || FC.Error[4]) DebugOut.Status[1] |= 0x01;
 473 
                                if(FC.Error[0] || FC.Error[1] || FC.Error[2] || FC.Error[3] || FC.Error[4]) DebugOut.Status[1] |= 0x01;
474 
                                else DebugOut.Status[1] &= ~0x01;
 474 
                                else DebugOut.Status[1] &= ~0x01;
475 
                                break;
 475 
                                break;
476 
                        default:
 476 
                        default:
477 
                                break;
 477 
                                break;
478 
                }
 478 
                }
479 
 
 479 
 
480 
                // every time we got new data from the FC via SPI call the navigation routine
 480 
                // every time we got new data from the FC via SPI call the navigation routine
481 
                // and update GPSStick that are returned to FC
 481 
                // and update GPSStick that are returned to FC
482 
                GPS_Navigation(&GPSData, &(ToFlightCtrl.GPSStick));
 482 
                GPS_Navigation(&GPSData, &(ToFlightCtrl.GPSStick));
483 
                ClearFCStatusFlags = 1;
 483 
                ClearFCStatusFlags = 1;
484 
 
 484 
 
485 
                if(counter)
 485 
                if(counter)
486 
                {
 486 
                {
487 
                        counter--;                                       // count down to enable servo
 487 
                        counter--;                                       // count down to enable servo
488 
                        if(!counter) TIMER2_Init();  // enable Servo Output
 488 
                        if(!counter) TIMER2_Init();  // enable Servo Output
489 
                }
 489 
                }
490 
 
 490 
 
491 
                SPI_RxBuffer_Request = 0;
 491 
                SPI_RxBuffer_Request = 0;
492 
                timeout = SetDelay(80); // 80 ms, new data are send every 20 ms
 492 
                timeout = SetDelay(80); // 80 ms, new data are send every 20 ms
493 
 
 493 
 
494 
                DebugOut.Analog[0] = FromFlightCtrl.AngleNick;
 494 
                DebugOut.Analog[0] = FromFlightCtrl.AngleNick;
495 
                DebugOut.Analog[1] = FromFlightCtrl.AngleRoll;
 495 
                DebugOut.Analog[1] = FromFlightCtrl.AngleRoll;
496 
                DebugOut.Analog[2] = FromFlightCtrl.AccNick;
 496 
                DebugOut.Analog[2] = FromFlightCtrl.AccNick;
497 
                DebugOut.Analog[3] = FromFlightCtrl.AccRoll;
 497 
                DebugOut.Analog[3] = FromFlightCtrl.AccRoll;
498 
                DebugOut.Analog[11] = FromFlightCtrl.GyroHeading/10;// in deg
 498 
                DebugOut.Analog[11] = FromFlightCtrl.GyroHeading/10;// in deg
499 
                Data3D.AngleNick = FromFlightCtrl.AngleNick;            // in 0.1 deg
 499 
                Data3D.AngleNick = FromFlightCtrl.AngleNick;            // in 0.1 deg
500 
                Data3D.AngleRoll = FromFlightCtrl.AngleRoll;            // in 0.1 deg
 500 
                Data3D.AngleRoll = FromFlightCtrl.AngleRoll;            // in 0.1 deg
501 
                Data3D.Heading   = FromFlightCtrl.GyroHeading;          // in 0.1 deg
 501 
                Data3D.Heading   = FromFlightCtrl.GyroHeading;          // in 0.1 deg
502 
        }       // EOF if(SPI_RxBuffer_Request)
 502 
        }       // EOF if(SPI_RxBuffer_Request)
503 
        else // no new SPI data
 503 
        else // no new SPI data
504 
        {
 504 
        {
505 
                if(CheckDelay(timeout) && (counter == 0))
 505 
                if(CheckDelay(timeout) && (counter == 0))
506 
                {
 506 
                {
507 
                        TIMER2_Deinit();  // disable Servo Output
 507 
                        TIMER2_Deinit();  // disable Servo Output
508 
                        counter = 50;     // reset counter for enabling Servo Output
 508 
                        counter = 50;     // reset counter for enabling Servo Output
509 
                }
 509 
                }
510 
        }
 510 
        }
511 
}
 511 
}
512 
 
 512 
 
513 
//------------------------------------------------------
 513 
//------------------------------------------------------
514 
void SPI0_GetFlightCtrlVersion(void)
 514 
void SPI0_GetFlightCtrlVersion(void)
515 
{
 515 
{
516 
        u32 timeout;
 516 
        u32 timeout;
517 
        u8 repeat;
 517 
        u8 repeat;
518 
        u8 msg[64];
 518 
        u8 msg[64];
519 
 
 519 
 
520 
        UART1_PutString("\r\n Looking for FlightContol");
 520 
        UART1_PutString("\r\n Looking for FlightContol");
521 
        FC_Version.Major = 0xFF;
 521 
        FC_Version.Major = 0xFF;
522 
        FC_Version.Minor = 0xFF;
 522 
        FC_Version.Minor = 0xFF;
523 
        FC_Version.Patch = 0xFF;
 523 
        FC_Version.Patch = 0xFF;
524 
        FC_Version.Compatible = 0xFF;
 524 
        FC_Version.Compatible = 0xFF;
525 
 
 525 
 
526 
        // polling FC version info
 526 
        // polling FC version info
527 
        repeat = 0;
 527 
        repeat = 0;
528 
        do
 528 
        do
529 
        {
 529 
        {
530 
                timeout = SetDelay(250);
 530 
                timeout = SetDelay(250);
531 
                do
 531 
                do
532 
                {
 532 
                {
533 
                        SPI0_UpdateBuffer();
 533 
                        SPI0_UpdateBuffer();
534 
                        if (FC_Version.Major != 0xFF)  break;
 534 
                        if (FC_Version.Major != 0xFF)  break;
535 
                }while (!CheckDelay(timeout));
 535 
                }while (!CheckDelay(timeout));
536 
                UART1_PutString(".");
 536 
                UART1_PutString(".");
537 
                repeat++;
 537 
                repeat++;
538 
        }while((FC_Version.Major == 0xFF) && (repeat < 40)); // 40*250ms = 10s
 538 
        }while((FC_Version.Major == 0xFF) && (repeat < 40)); // 40*250ms = 10s
539 
        // if we got it
 539 
        // if we got it
540 
        if (FC_Version.Major != 0xFF)
 540 
        if (FC_Version.Major != 0xFF)
541 
        {
 541 
        {
542 
                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);
 542 
                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);
543 
                UART1_PutString(msg);
 543 
                UART1_PutString(msg);
544 
        }
 544 
        }
545 
        else UART1_PutString("\n\r not found!");
 545 
        else UART1_PutString("\n\r not found!");
546 
}
 546 
}
547 
 
 547 
 
548 
 
 548 
 
549 
 
 549