Subversion Repositories NaviCtrl

Rev

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

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