Subversion Repositories NaviCtrl

Rev

Rev 44 | Go to most recent revision | Details | Compare with Previous | Last modification | View Log | RSS feed

Rev Author Line No. Line
1 ingob 1
/*#######################################################################################*/
2
/* !!! THIS IS NOT FREE SOFTWARE !!!                                                     */
3
/*#######################################################################################*/
4
// ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
5
// + Copyright (c) 2008 Ingo Busker, Holger Buss
171 ingob 6
// + Nur für den privaten Gebrauch / NON-COMMERCIAL USE ONLY
1 ingob 7
// + FOR NON COMMERCIAL USE ONLY
8
// + www.MikroKopter.com
9
// ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
41 ingob 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.
12
// + Sollten direkte oder indirekte kommerzielle Absichten verfolgt werden, ist mit uns (info@mikrokopter.de) Kontakt
13
// + bzgl. der Nutzungsbedingungen aufzunehmen.
1 ingob 14
// + Eine kommerzielle Nutzung ist z.B.Verkauf von MikroKoptern, Bestückung und Verkauf von Platinen oder Bausätzen,
15
// + Verkauf von Luftbildaufnahmen, usw.
16
// ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
41 ingob 17
// + Werden Teile des Quellcodes (mit oder ohne Modifikation) weiterverwendet oder veröffentlicht,
1 ingob 18
// + unterliegen sie auch diesen Nutzungsbedingungen und diese Nutzungsbedingungen incl. Copyright müssen dann beiliegen
19
// ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
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"
22
// + eindeutig als Ursprung verlinkt werden
23
// ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
24
// + Keine Gewähr auf Fehlerfreiheit, Vollständigkeit oder Funktion
25
// + Benutzung auf eigene Gefahr
26
// + Wir übernehmen keinerlei Haftung für direkte oder indirekte Personen- oder Sachschäden
27
// ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
171 ingob 28
// + Die Portierung oder Nutzung der Software (oder Teile davon) auf andere Systeme (ausser der Hardware von www.mikrokopter.de) ist nur
1 ingob 29
// + mit unserer Zustimmung zulässig
30
// ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
31
// + Die Funktion printf_P() unterliegt ihrer eigenen Lizenz und ist hiervon nicht betroffen
32
// ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
41 ingob 33
// + Redistributions of source code (with or without modifications) must retain the above copyright notice,
1 ingob 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
36
// +     from this software without specific prior written permission.
41 ingob 37
// +   * The use of this project (hardware, software, binary files, sources and documentation) is only permitted
1 ingob 38
// +     for non-commercial use (directly or indirectly)
41 ingob 39
// +     Commercial use (for excample: selling of MikroKopters, selling of PCBs, assembly, ...) is only permitted
1 ingob 40
// +     with our written permission
41 ingob 41
// +   * If sources or documentations are redistributet on other webpages, out webpage (http://www.MikroKopter.de) must be
42
// +     clearly linked as origin
171 ingob 43
// +   * porting the sources to other systems or using the software on other systems (except hardware from www.mikrokopter.de) is not allowed
1 ingob 44
//
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
47
// +  IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
48
// +  ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE
49
// +  LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
50
// +  CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
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
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
41 ingob 55
// +  POSSIBILITY OF SUCH DAMAGE.
1 ingob 56
// ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
41 ingob 57
 
58
#include <string.h>
59
#include "91x_lib.h"
60
#include "led.h"
61
#include "GPS.h"
62
#include "uart1.h"
63
#include "spi_slave.h"
64
#include "i2c.h"
65
#include "timer.h"
1 ingob 66
#include "main.h"
41 ingob 67
#include "fifo.h"
1 ingob 68
 
69
 
41 ingob 70
#define SPI_RXSYNCBYTE1 0xAA
71
#define SPI_RXSYNCBYTE2 0x83
72
#define SPI_TXSYNCBYTE1 0x81
73
#define SPI_TXSYNCBYTE2 0x55
1 ingob 74
 
41 ingob 75
typedef enum
76
{
77
        SPI_SYNC1,
78
        SPI_SYNC2,
79
        SPI_DATA
80
} SPI_State_t;
1 ingob 81
 
41 ingob 82
//communication packets
83
volatile FromFlightCtrl_t   FromFlightCtrl;
84
volatile ToFlightCtrl_t     ToFlightCtrl;
1 ingob 85
 
41 ingob 86
// tx packet buffer
87
#define SPI_TXBUFFER_LEN (2 + sizeof(ToFlightCtrl)) // 2 bytes at start are for synchronization
88
volatile u8 SPI_TxBuffer[SPI_TXBUFFER_LEN];
89
volatile u8 SPI_TxBufferIndex = 0;
90
u8 *Ptr_TxChksum = NULL ;  // pointer to checksum in TxBuffer
1 ingob 91
 
41 ingob 92
// rx packet buffer
93
#define SPI_RXBUFFER_LEN sizeof(FromFlightCtrl)
94
volatile u8 SPI_RxBuffer[SPI_RXBUFFER_LEN];
95
volatile u8 SPI_RxBufferIndex = 0;
96
volatile u8 SPI_RxBuffer_Request = 0;
1 ingob 97
#define SPI_COMMAND_INDEX 0
98
 
41 ingob 99
s32 FC_Kalman_K = 32;
100
s32 Kalman_MaxDrift = 5 * 16;
101
s32 Kalman_MaxFusion = 64;
102
u32 CheckSPIOkay = 0;
1 ingob 103
 
41 ingob 104
u8 SPI_CommandSequence[] = { SPI_KALMAN, SPI_CMD_OSD_DATA, SPI_CMD_GPS_POS, SPI_KALMAN, SPI_CMD_GPS_TARGET};
105
u8 SPI_CommandCounter = 0;
1 ingob 106
 
41 ingob 107
SPI_Version_t FC_Version;
1 ingob 108
 
41 ingob 109
u8 CompassCalStateQueue[10];
110
fifo_t CompassCalcStateFiFo;
1 ingob 111
 
41 ingob 112
u8 CompassCalState = 0;
1 ingob 113
 
41 ingob 114
//--------------------------------------------------------------
115
void SSP0_IRQHandler(void)
116
{
117
        static u8 rxchksum = 0;
118
        u8 rxdata;
119
        static SPI_State_t SPI_State = SPI_SYNC1;
1 ingob 120
 
41 ingob 121
        // clear pending bit
122
        SSP_ClearITPendingBit(SSP0, SSP_IT_RxTimeOut);
123
        // Fill TxFIFO while its not full or end of packet is reached
124
        while (SSP_GetFlagStatus(SSP0, SSP_FLAG_TxFifoNotFull) == SET)
125
        {
126
                if (SPI_TxBufferIndex < SPI_TXBUFFER_LEN)   // still data to send ?
127
                {
128
                        SSP0->DR = SPI_TxBuffer[SPI_TxBufferIndex];       // send a byte
129
                        *Ptr_TxChksum += SPI_TxBuffer[SPI_TxBufferIndex]; // update checksum
130
                        SPI_TxBufferIndex++;  // pointer to next byte
131
                }
132
                else // TxBuffer end is reached then reset and copy data to tx buffer
133
                {
134
                        SPI_TxBufferIndex = 0;   // reset buffer index
135
                        ToFlightCtrl.Chksum = 0;  // initialize checksum
136
                        ToFlightCtrl.BeepTime = BeepTime; // set beeptime
137
                        BeepTime = 0; // reset local beeptime
138
                        // copy contents of ToFlightCtrl->SPI_TxBuffer
139
                        memcpy( (u8 *) &(SPI_TxBuffer[2]), (u8 *) &ToFlightCtrl, sizeof(ToFlightCtrl));
140
                }
141
        }
142
        // while RxFIFO not empty
143
        while (SSP_GetFlagStatus(SSP0, SSP_FLAG_RxFifoNotEmpty)==SET)
144
        {
145
                rxdata =  SSP0->DR; // catch the received byte
146
                // Fill TxFIFO while its not full or end of packet is reached
147
                while (SSP_GetFlagStatus(SSP0, SSP_FLAG_TxFifoNotFull) == SET)
148
                {
149
                if (SPI_TxBufferIndex  < SPI_TXBUFFER_LEN)   // still data to send ?
150
                {
151
                                SSP0->DR = SPI_TxBuffer[SPI_TxBufferIndex];       // send a byte
152
                        *Ptr_TxChksum += SPI_TxBuffer[SPI_TxBufferIndex]; // update checksum
153
                                SPI_TxBufferIndex++; // pointer to next byte
154
                }
155
                else // end of packet is reached reset and copy data to tx buffer
156
                {
157
                        SPI_TxBufferIndex = 0;  // reset buffer index
158
                                ToFlightCtrl.Chksum = 0;  // initialize checksum
159
                        ToFlightCtrl.BeepTime = BeepTime;  // set beeptime
160
                                BeepTime = 0; // reset local beeptime
161
                                // copy contents of ToFlightCtrl->SPI_TxBuffer
162
                                memcpy((u8 *) &(SPI_TxBuffer[2]), (u8 *) &ToFlightCtrl, sizeof(ToFlightCtrl));
163
                        }
164
                }
165
                switch (SPI_State)
166
                {
167
                        case SPI_SYNC1:
168
                                SPI_RxBufferIndex = 0; // reset buffer index
169
                                rxchksum = rxdata;     // init checksum
170
                                if (rxdata == SPI_RXSYNCBYTE1)
171
                                {   // 1st syncbyte ok
172
                                        SPI_State = SPI_SYNC2;  // step to sync2
173
                                }
174
                                break;
175
                        case SPI_SYNC2:
176
                                if (rxdata == SPI_RXSYNCBYTE2)
177
                                {  // 2nd Syncbyte ok
178
                                        rxchksum += rxdata;
179
                                        SPI_State = SPI_DATA;
180
                                }  // 2nd Syncbyte does not match
181
                                else
182
                                {
183
                                        SPI_State  = SPI_SYNC1; //jump back to sync1
184
                                }
185
                                break;
186
                        case SPI_DATA:
187
                                SPI_RxBuffer[SPI_RxBufferIndex++]= rxdata; // copy databyte to rx buffer
188
                                if (SPI_RxBufferIndex >= SPI_RXBUFFER_LEN) // end of packet is reached
189
                                {
190
                                        if (rxdata == rxchksum) // verify checksum byte
191
                                        {
192
                                                // copy SPI_RxBuffer -> FromFlightCtrl
193
                                                if(!SPI_RxBuffer_Request) // block writing to FromFlightCtrl on reading access
194
                                                {
195
                                                        memcpy((u8 *) &FromFlightCtrl, (u8 *) SPI_RxBuffer, sizeof(FromFlightCtrl));
196
                                                        SPI_RxBuffer_Request = 1;
197
                                                }
198
                                                CheckSPIOkay++;
199
                                                DebugOut.Analog[13]++;
200
                                        }
201
                                        else // bad checksum byte
202
                                        {
203
                                                DebugOut.Analog[12]++; // increase SPI chksum error counter
204
                                        }
205
                                        SPI_State  = SPI_SYNC1; // reset state
206
                                }
207
                                else // end of packet not reached
208
                                {
209
                                        rxchksum += rxdata;      // update checksum
210
                                }
211
                                break;
212
                        default:
213
                                SPI_State  = SPI_SYNC1;
214
                                break;
215
                }
216
        }
1 ingob 217
}
218
 
219
//--------------------------------------------------------------
220
void SPI0_Init(void)
221
{
41 ingob 222
        GPIO_InitTypeDef GPIO_InitStructure;
223
        SSP_InitTypeDef   SSP_InitStructure;
1 ingob 224
 
41 ingob 225
        SerialPutString("\r\n SPI init...");
1 ingob 226
 
41 ingob 227
        SCU_APBPeriphClockConfig(__GPIO2 ,ENABLE);
228
        SCU_APBPeriphClockConfig(__SSP0 ,ENABLE);
1 ingob 229
 
41 ingob 230
        GPIO_DeInit(GPIO2);
231
        //SSP0_CLK, SSP0_MOSI, SSP0_NSS pins
232
        GPIO_StructInit(&GPIO_InitStructure);
233
        GPIO_InitStructure.GPIO_Direction = GPIO_PinInput;
234
        GPIO_InitStructure.GPIO_Pin = GPIO_Pin_4 | GPIO_Pin_5 | GPIO_Pin_7;
235
        GPIO_InitStructure.GPIO_Type = GPIO_Type_PushPull ;
236
        GPIO_InitStructure.GPIO_IPConnected = GPIO_IPConnected_Enable;
237
        GPIO_InitStructure.GPIO_Alternate = GPIO_InputAlt1; //SSP0_SCLK, SSP0_MOSI, SSP0_NSS
238
        GPIO_Init (GPIO2, &GPIO_InitStructure);
1 ingob 239
 
41 ingob 240
        // SSP0_MISO pin GPIO2.6
241
        GPIO_StructInit(&GPIO_InitStructure);
242
        GPIO_InitStructure.GPIO_Direction = GPIO_PinOutput;
243
        GPIO_InitStructure.GPIO_Pin = GPIO_Pin_6;
244
        GPIO_InitStructure.GPIO_Type = GPIO_Type_PushPull ;
245
        GPIO_InitStructure.GPIO_IPConnected = GPIO_IPConnected_Enable;
246
        GPIO_InitStructure.GPIO_Alternate = GPIO_OutputAlt2; //SSP0_MISO
247
        GPIO_Init (GPIO2, &GPIO_InitStructure);
1 ingob 248
 
41 ingob 249
        SSP_DeInit(SSP0);
250
        SSP_StructInit(&SSP_InitStructure);
251
        SSP_InitStructure.SSP_FrameFormat = SSP_FrameFormat_Motorola;
252
        SSP_InitStructure.SSP_Mode = SSP_Mode_Slave;
253
        SSP_InitStructure.SSP_SlaveOutput = SSP_SlaveOutput_Enable;
254
        SSP_InitStructure.SSP_CPHA = SSP_CPHA_1Edge;
255
        SSP_InitStructure.SSP_CPOL = SSP_CPOL_Low;
256
        SSP_InitStructure.SSP_ClockRate = 0;
1 ingob 257
 
41 ingob 258
        SSP_Init(SSP0, &SSP_InitStructure);
1 ingob 259
 
41 ingob 260
        SSP_ITConfig(SSP0, SSP_IT_RxFifo | SSP_IT_TxFifo | SSP_IT_RxTimeOut, ENABLE);
1 ingob 261
 
41 ingob 262
        fifo_init(&CompassCalcStateFiFo, CompassCalStateQueue, sizeof(CompassCalStateQueue));
263
 
264
        SSP_Cmd(SSP0, ENABLE);
265
        // initialize the syncbytes in the tx buffer
266
        SPI_TxBuffer[0] = SPI_TXSYNCBYTE1;
267
        SPI_TxBuffer[1] = SPI_TXSYNCBYTE2;
268
        // set the pointer to the checksum byte in the tx buffer
269
        Ptr_TxChksum = (u8 *) &(((ToFlightCtrl_t *) &(SPI_TxBuffer[2]))->Chksum);
270
 
271
        VIC_Config(SSP0_ITLine, VIC_IRQ, 1);
272
        VIC_ITCmd(SSP0_ITLine, ENABLE);
273
 
274
        SerialPutString("ok");
1 ingob 275
}
41 ingob 276
 
1 ingob 277
//------------------------------------------------------
41 ingob 278
void SPI0_UpdateBuffer(void)
1 ingob 279
{
41 ingob 280
        if (SPI_RxBuffer_Request)
281
        {
282
                // avoid sending data via SPI during the update of the  ToFlightCtrl structure
283
                VIC_ITCmd(SSP0_ITLine, DISABLE); // disable SPI interrupt
1 ingob 284
 
41 ingob 285
                ToFlightCtrl.CompassHeading = I2C_Heading.Heading;
286
                ToFlightCtrl.GPS_Nick   = GPS_Stick.Nick;
287
                ToFlightCtrl.GPS_Roll   = GPS_Stick.Roll;
288
                ToFlightCtrl.GPS_Yaw    = GPS_Stick.Yaw;
289
                // cycle spi commands
290
                ToFlightCtrl.Command = SPI_CommandSequence[SPI_CommandCounter++];
291
                if (SPI_CommandCounter >= sizeof(SPI_CommandSequence)) SPI_CommandCounter = 0;
1 ingob 292
 
41 ingob 293
                switch (ToFlightCtrl.Command)
294
                {
295
                        case  SPI_KALMAN:
296
                                ToFlightCtrl.Param.Byte[0] = (char) FC_Kalman_K;
297
                                ToFlightCtrl.Param.Byte[1] = (char) Kalman_MaxFusion;
298
                                ToFlightCtrl.Param.Byte[2] = (char) Kalman_MaxDrift;
299
                                break;
1 ingob 300
 
41 ingob 301
                        case  SPI_CMD_GPS_POS:
302
                                ToFlightCtrl.Param.Long[0]  = GPSData.Position.Longitude;
303
                                ToFlightCtrl.Param.Long[1]  = GPSData.Position.Latitude;
304
                                break;
1 ingob 305
 
41 ingob 306
                        case  SPI_CMD_GPS_TARGET:
307
                                if(GPS_pTargetPosition != NULL)
308
                                {
309
                                        if(GPS_pTargetPosition->Status != INVALID)
310
                                        {
311
                                                ToFlightCtrl.Param.Long[0]  = GPS_pTargetPosition->Longitude;
312
                                                ToFlightCtrl.Param.Long[1]  = GPS_pTargetPosition->Latitude;
313
                                        }
314
                                        else
315
                                        {
316
                                                ToFlightCtrl.Param.Long[0]  = 0;
317
                                                ToFlightCtrl.Param.Long[1]  = 0;
318
                                        }
319
                                }
320
                                else
321
                                {
322
                                        ToFlightCtrl.Param.Long[0]  = 0;
323
                                        ToFlightCtrl.Param.Long[1]  = 0;
324
                                }
325
                                break;
1 ingob 326
 
41 ingob 327
                        default:
328
                                break;
329
                }
330
                VIC_ITCmd(SSP0_ITLine, ENABLE);         // enable SPI interrupt
1 ingob 331
 
332
 
41 ingob 333
                switch(FromFlightCtrl.Command)
334
                {
335
                        case SPI_CMD_USER:
336
                                Parameter.User1 = FromFlightCtrl.Param.Byte[0];
337
                                Parameter.User2 = FromFlightCtrl.Param.Byte[1];
338
                                Parameter.User3 = FromFlightCtrl.Param.Byte[2];
339
                                Parameter.User4 = FromFlightCtrl.Param.Byte[3];
340
                                Parameter.User5 = FromFlightCtrl.Param.Byte[4];
341
                                Parameter.User6 = FromFlightCtrl.Param.Byte[5];
342
                                Parameter.User7 = FromFlightCtrl.Param.Byte[6];
343
                                Parameter.User8 = FromFlightCtrl.Param.Byte[7];
344
                                if(ClearMKFlags)
345
                                {
346
                                        FC.MKFlags = 0;
347
                                        ClearMKFlags = 0;
348
                                }
349
                                FC.MKFlags         |= FromFlightCtrl.Param.Byte[8];
350
                                FC.UBat                 = FromFlightCtrl.Param.Byte[9];
351
                                Parameter.LowVoltageWarning = FromFlightCtrl.Param.Byte[10];
352
                                Parameter.ActiveSetting         = FromFlightCtrl.Param.Byte[11];
353
                                break;
1 ingob 354
 
41 ingob 355
 #define CHK_POTI_MM(b,a,min,max) { if(a > 250) { if(a == 251) b = FC.Poti1; else if(a == 252) b = FC.Poti2; else if(a == 253) b = FC.Poti3; else if(a == 254) b = FC.Poti4;} else b = a; if(b <= min) b = min; else if(b >= max) b = max;}
356
                        case SPI_CMD_PARAMETER1:
357
                                CHK_POTI_MM(Parameter.NaviGpsModeControl,FromFlightCtrl.Param.Byte[0],0,255);
358
                                CHK_POTI_MM(Parameter.NaviGpsGain,FromFlightCtrl.Param.Byte[1],0,255);
359
                                CHK_POTI_MM(Parameter.NaviGpsP,FromFlightCtrl.Param.Byte[2],0,255);
360
                                CHK_POTI_MM(Parameter.NaviGpsI,FromFlightCtrl.Param.Byte[3],0,255);
361
                                CHK_POTI_MM(Parameter.NaviGpsD,FromFlightCtrl.Param.Byte[4],0,255);
362
                                CHK_POTI_MM(Parameter.NaviGpsACC,FromFlightCtrl.Param.Byte[5],0,255);
363
                                Parameter.NaviGpsMinSat = FromFlightCtrl.Param.Byte[6];
364
                                Parameter.NaviStickThreshold = FromFlightCtrl.Param.Byte[7];
365
                                CHK_POTI_MM(Parameter.NaviOperatingRadius,FromFlightCtrl.Param.Byte[8],10,255);
366
                                CHK_POTI_MM(Parameter.NaviWindCorrection,FromFlightCtrl.Param.Byte[9],0,255);
367
                                CHK_POTI_MM(Parameter.NaviSpeedCompensation,FromFlightCtrl.Param.Byte[10],0,255);
368
                                CHK_POTI_MM(Parameter.NaviAngleLimitation,FromFlightCtrl.Param.Byte[11],0,255);
369
//              DebugOut.Analog[4] = Parameter.NaviRadiusAlert;
370
//              DebugOut.Analog[5] = Parameter.NaviWindCorrection;
371
//              DebugOut.Analog[6] = Parameter.NaviSpeedCompensation;
372
                        break;
1 ingob 373
 
41 ingob 374
                        case SPI_CMD_STICK:
375
                                FC.StickGas     = FromFlightCtrl.Param.sByte[0];
376
                                FC.StickYaw     = FromFlightCtrl.Param.sByte[1];
377
                                FC.StickRoll    = FromFlightCtrl.Param.sByte[2];
378
                                FC.StickNick    = FromFlightCtrl.Param.sByte[3];
379
                                FC.Poti1                = FromFlightCtrl.Param.Byte[4];
380
                                FC.Poti2                = FromFlightCtrl.Param.Byte[5];
381
                                FC.Poti3                = FromFlightCtrl.Param.Byte[6];
382
                                FC.Poti4                = FromFlightCtrl.Param.Byte[7];
383
                                FC.RC_Quality   = FromFlightCtrl.Param.Byte[8];
384
                                break;
27 holgerb 385
 
41 ingob 386
                        case SPI_CMD_MISC:
387
                                if(CompassCalState != FromFlightCtrl.Param.Byte[0])
388
                                {       // put only new CompassCalState into queue to send via I2C
389
                                        CompassCalState = FromFlightCtrl.Param.Byte[0];
390
                                        fifo_put(&CompassCalcStateFiFo, CompassCalState);
391
                                }
392
                                NaviData.Altimeter = (s16) FromFlightCtrl.Param.Int[1];
393
                                break;
1 ingob 394
 
41 ingob 395
                        case SPI_CMD_VERSION:
396
                                FC_Version.Major = FromFlightCtrl.Param.Byte[0];
397
                                FC_Version.Minor = FromFlightCtrl.Param.Byte[1];
398
                                FC_Version.Patch = FromFlightCtrl.Param.Byte[2];
399
                                FC_Version.Compatible = FromFlightCtrl.Param.Byte[3];
400
                                break;
1 ingob 401
 
41 ingob 402
                        default:
403
                        break;
404
                }
1 ingob 405
 
41 ingob 406
                // every time we got new data from the FC via SPI call the navigation routine
407
                GPS_Navigation();
408
                ClearMKFlags = 1;
1 ingob 409
 
41 ingob 410
                SPI_RxBuffer_Request = 0;
1 ingob 411
 
41 ingob 412
                DebugOut.Analog[0] = FromFlightCtrl.IntegralNick;
413
                DebugOut.Analog[1] = FromFlightCtrl.IntegralRoll;
414
                DebugOut.Analog[2] = (30*FromFlightCtrl.AccNick)/108;
415
                DebugOut.Analog[3] = (30*FromFlightCtrl.AccRoll)/108;
416
                DebugOut.Analog[25] = FromFlightCtrl.GyroHeading;
1 ingob 417
 
41 ingob 418
        }  // EOF if(SPI_RxBuffer_Request)
1 ingob 419
}
420
 
41 ingob 421
//------------------------------------------------------
422
void SPI0_GetFlightCtrlVersion(void)
423
{
424
        u16 timeout;
425
        u8 msg[20];
426
 
427
        FC_Version.Major = 0xFF;
428
        FC_Version.Minor = 0xFF;
429
        FC_Version.Patch = 0xFF;
430
        FC_Version.Compatible = 0xFF;
431
 
432
        // polling FC version info for 2 second
433
        timeout = SetDelay(2000);
434
        do
435
        {
436
                SPI0_UpdateBuffer();
437
                if (FC_Version.Major != 0xFF)  break;
438
        }while (!CheckDelay(timeout));
439
        // if we got it
440
        if (FC_Version.Major != 0xFF)
441
        {
442
                sprintf(msg, "\n\r FlightCtrl V%d.%d%c", FC_Version.Major, FC_Version.Minor, 'a'+FC_Version.Patch);
443
                SerialPutString(msg);
444
                sprintf(msg, " Compatible: %d", FC_Version.Compatible);
445
                SerialPutString(msg);
446
        }
447
        else SerialPutString("\n\r No version information from FlightCtrl.");
448
}
449
 
450