Subversion Repositories NaviCtrl

Rev

Go to most recent revision | Details | Last modification | View Log | RSS feed

Rev Author Line No. Line
1 ingob 1
/*#######################################################################################*/
2
/* !!! THIS IS NOT FREE SOFTWARE !!!                                                     */
3
/*#######################################################################################*/
4
// ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
5
// + www.MikroKopter.com
6
// ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
360 holgerb 7
// + Software Nutzungsbedingungen (english version: see below)
8
// + der Fa. HiSystems GmbH, Flachsmeerstrasse 2, 26802 Moormerland - nachfolgend Lizenzgeber genannt -
9
// + Der Lizenzgeber räumt dem Kunden ein nicht-ausschließliches, zeitlich und räumlich* unbeschränktes Recht ein, die im den
489 killagreg 10
// + Mikrocontroller verwendete Firmware für die Hardware Flight-Ctrl, Navi-Ctrl, BL-Ctrl, MK3Mag & PC-Programm MikroKopter-Tool
360 holgerb 11
// + - nachfolgend Software genannt - nur für private Zwecke zu nutzen.
12
// + Der Einsatz dieser Software ist nur auf oder mit Produkten des Lizenzgebers zulässig.
1 ingob 13
// ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
360 holgerb 14
// + Die vom Lizenzgeber gelieferte Software ist urheberrechtlich geschützt. Alle Rechte an der Software sowie an sonstigen im
15
// + Rahmen der Vertragsanbahnung und Vertragsdurchführung überlassenen Unterlagen stehen im Verhältnis der Vertragspartner ausschließlich dem Lizenzgeber zu.
16
// + Die in der Software enthaltenen Copyright-Vermerke, Markenzeichen, andere Rechtsvorbehalte, Seriennummern sowie
17
// + sonstige der Programmidentifikation dienenden Merkmale dürfen vom Kunden nicht verändert oder unkenntlich gemacht werden.
18
// + Der Kunde trifft angemessene Vorkehrungen für den sicheren Einsatz der Software. Er wird die Software gründlich auf deren
19
// + Verwendbarkeit zu dem von ihm beabsichtigten Zweck testen, bevor er diese operativ einsetzt.
20
// + Die Haftung des Lizenzgebers wird - soweit gesetzlich zulässig - begrenzt in Höhe des typischen und vorhersehbaren
489 killagreg 21
// + Schadens. Die gesetzliche Haftung bei Personenschäden und nach dem Produkthaftungsgesetz bleibt unberührt. Dem Lizenzgeber steht jedoch der Einwand
360 holgerb 22
// + des Mitverschuldens offen.
23
// + Der Kunde trifft angemessene Vorkehrungen für den Fall, dass die Software ganz oder teilweise nicht ordnungsgemäß arbeitet.
24
// + Er wird die Software gründlich auf deren Verwendbarkeit zu dem von ihm beabsichtigten Zweck testen, bevor er diese operativ einsetzt.
25
// + Der Kunde wird er seine Daten vor Einsatz der Software nach dem Stand der Technik sichern.
26
// + Der Kunde ist darüber unterrichtet, dass der Lizenzgeber seine Daten im zur Vertragsdurchführung erforderlichen Umfang
27
// + und auf Grundlage der Datenschutzvorschriften erhebt, speichert, verarbeitet und, sofern notwendig, an Dritte übermittelt.
28
// + *) Die räumliche Nutzung bezieht sich nur auf den Einsatzort, nicht auf die Reichweite der programmierten Software.
29
// + #### ENDE DER NUTZUNGSBEDINGUNGEN ####'
30
// +  Hinweis: Informationen über erweiterte Nutzungsrechte (wie z.B. Nutzung für nicht-private Zwecke) sind auf Anfrage per Email an info(@)hisystems.de verfügbar.
1 ingob 31
// ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
360 holgerb 32
// + Software LICENSING TERMS
1 ingob 33
// ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
360 holgerb 34
// + of HiSystems GmbH, Flachsmeerstrasse 2, 26802 Moormerland, Germany - the Licensor -
489 killagreg 35
// + The Licensor grants the customer a non-exclusive license to use the microcontroller firmware of the Flight-Ctrl, Navi-Ctrl, BL-Ctrl, and MK3Mag hardware
360 holgerb 36
// + (the Software) exclusively for private purposes. The License is unrestricted with respect to time and territory*.
37
// + The Software may only be used with the Licensor's products.
38
// + The Software provided by the Licensor is protected by copyright. With respect to the relationship between the parties to this
39
// + agreement, all rights pertaining to the Software and other documents provided during the preparation and execution of this
40
// + agreement shall be the property of the Licensor.
41
// + The information contained in the Software copyright notices, trademarks, other legal reservations, serial numbers and other
42
// + features that can be used to identify the program may not be altered or defaced by the customer.
43
// + The customer shall be responsible for taking reasonable precautions
44
// + for the safe use of the Software. The customer shall test the Software thoroughly regarding its suitability for the
45
// + intended purpose before implementing it for actual operation. The Licensor's liability shall be limited to the extent of typical and
46
// + foreseeable damage to the extent permitted by law, notwithstanding statutory liability for bodily injury and product
47
// + liability. However, the Licensor shall be entitled to the defense of contributory negligence.
48
// + The customer will take adequate precautions in the case, that the software is not working properly. The customer will test
49
// + the software for his purpose before any operational usage. The customer will backup his data before using the software.
50
// + The customer understands that the Licensor collects, stores and processes, and, where required, forwards, customer data
51
// + to third parties to the extent necessary for executing the agreement, subject to applicable data protection and privacy regulations.
52
// + *) The territory aspect only refers to the place where the Software is used, not its programmed range.
53
// + #### END OF LICENSING TERMS ####
54
// + Note: For information on license extensions (e.g. commercial use), please contact us at info(@)hisystems.de.
1 ingob 55
// ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
41 ingob 56
 
57
#include <string.h>
331 holgerb 58
#include <math.h>
489 killagreg 59
#include <stdlib.h>
41 ingob 60
#include "91x_lib.h"
61
#include "led.h"
215 killagreg 62
#include "gps.h"
41 ingob 63
#include "uart1.h"
64
#include "spi_slave.h"
242 killagreg 65
#include "compass.h"
119 killagreg 66
#include "timer1.h"
67
#include "timer2.h"
136 killagreg 68
#include "config.h"
1 ingob 69
#include "main.h"
294 holgerb 70
#include "params.h"
426 holgerb 71
#include "settings.h"
1 ingob 72
 
41 ingob 73
#define SPI_RXSYNCBYTE1 0xAA
74
#define SPI_RXSYNCBYTE2 0x83
75
#define SPI_TXSYNCBYTE1 0x81
76
#define SPI_TXSYNCBYTE2 0x55
1 ingob 77
 
41 ingob 78
//communication packets
215 killagreg 79
FromFlightCtrl_t   FromFlightCtrl;
80
ToFlightCtrl_t     ToFlightCtrl;
397 holgerb 81
#define SPI0_TIMEOUT     2500  //ms
146 killagreg 82
volatile u32 SPI0_Timeout = 0;
338 holgerb 83
u8 Logging_FCStatusFlags1 = 0,Logging_FCStatusFlags2 = 0;
489 killagreg 84
u8 SpeakHoTT = 0;
578 holgerb 85
u32 LoggingGasFilter = 0, LoggingGasCnt = 0;
618 holgerb 86
u8 Out1TriggerUpdateBlocked = 0;
87
u8 Out1TriggerUpdateNewData = 0;
1 ingob 88
 
41 ingob 89
// tx packet buffer
90
#define SPI_TXBUFFER_LEN (2 + sizeof(ToFlightCtrl)) // 2 bytes at start are for synchronization
378 holgerb 91
volatile u8 SPI_TxBuffer[SPI_TXBUFFER_LEN + 10];
41 ingob 92
volatile u8 SPI_TxBufferIndex = 0;
93
u8 *Ptr_TxChksum = NULL ;  // pointer to checksum in TxBuffer
1 ingob 94
 
41 ingob 95
// rx packet buffer
96
#define SPI_RXBUFFER_LEN sizeof(FromFlightCtrl)
378 holgerb 97
volatile u8 SPI_RxBuffer[SPI_RXBUFFER_LEN+10];
41 ingob 98
volatile u8 SPI_RxBufferIndex = 0;
99
volatile u8 SPI_RxBuffer_Request = 0;
1 ingob 100
#define SPI_COMMAND_INDEX 0
101
 
149 killagreg 102
s32 Kalman_K = 32;
41 ingob 103
s32 Kalman_MaxDrift = 5 * 16;
104
s32 Kalman_MaxFusion = 64;
330 holgerb 105
s32 Kalman_Kompass = 32;
148 holgerb 106
s32 ToFcGpsZ = 0;
392 holgerb 107
u8 CompassCalState = 0;
1 ingob 108
 
419 holgerb 109
u8 SPI_CommandSequence[] = { SPI_NCCMD_KALMAN, SPI_NCCMD_GPSINFO ,SPI_NCCMD_KALMAN, SPI_NCCMD_HOTT_INFO, SPI_NCCMD_KALMAN, SPI_MISC, SPI_NCCMD_KALMAN, SPI_NCCMD_VERSION };
41 ingob 110
u8 SPI_CommandCounter = 0;
222 holgerb 111
s32 ToFC_Rotate_C = 64, ToFC_Rotate_S = 0;
587 holgerb 112
s32 HeadFreeStartAngle = 0;       // in 0,1°
113
s32 CompassDirectionAtMotorStart = 0; // in 0,1°
577 holgerb 114
s16 FC_WP_EventChannel = 0, LogFC_WP_EventChannel = 0, FC_WP_EventChannel_Processed = 0; // gibt einen Schaltkanal an die FC weiter, wenn der Wegpunkt erreicht wurde
286 killagreg 115
u32 ToFC_AltitudeRate = 0;
285 holgerb 116
s32 ToFC_AltitudeSetpoint = 0;
298 holgerb 117
u8  FromFC_VarioCharacter = ' ';
514 holgerb 118
s16 GPS_Aid_StickMultiplikator = 0;
323 holgerb 119
u8 NC_GPS_ModeCharacter = ' ';
329 holgerb 120
u8 FCCalibActive = 0;
330 holgerb 121
u8 FC_is_Calibrated = 0;
605 holgerb 122
Motor_t Motor[MAX_MOTORS];
123
u8 Motor_Version[MAX_MOTORS]; // das kann nicht in die struct, weil der PC die Struktur bekommt
351 holgerb 124
u8 NC_To_FC_Flags = 0;
530 holgerb 125
u8 BL_MinOfMaxPWM = 255;  // indication if all BL-controllers run on full power
126
u8 Logging_BL_MinOfMaxPWM = 255;
127
u8 ErrorCheck_BL_MinOfMaxPWM = 255;
338 holgerb 128
u32 FC_I2C_ErrorConter;
41 ingob 129
SPI_Version_t FC_Version;
351 holgerb 130
s16 POI_KameraNick = 0;
460 holgerb 131
u8 NC_Wait_for_LED = 0;
463 holgerb 132
s16 GyroCompassCorrected = 0; // corrected with the magnetic declination
510 holgerb 133
s16 CompassSetpointCorrected = 0; // The compass setpoint that the FC tries to keep - corrected with the magnetic declination
134
s16 CompassSetpoint = 0;          // in 0,1°
567 holgerb 135
s16 SimulatedDirection = 0;     // only for flight simulation
530 holgerb 136
u8 AmountOfMotors = 0;
532 holgerb 137
u16 FlugMinutenGesamt;
578 holgerb 138
u8 HoverGas = 0;
625 holgerb 139
u8 LowVoltageLandingActive = 0;
41 ingob 140
//--------------------------------------------------------------
141
void SSP0_IRQHandler(void)
142
{
143
        static u8 rxchksum = 0;
144
        u8 rxdata;
1 ingob 145
 
189 killagreg 146
        #define SPI_SYNC1       0
147
        #define SPI_SYNC2       1
148
        #define SPI_DATA        2
149
        static u8 SPI_State = SPI_SYNC1;
378 holgerb 150
        //IENABLE;
189 killagreg 151
 
161 killagreg 152
        // clear pending bits
41 ingob 153
        SSP_ClearITPendingBit(SSP0, SSP_IT_RxTimeOut);
144 killagreg 154
        SSP_ClearITPendingBit(SSP0, SSP_IT_RxFifo);
189 killagreg 155
 
79 killagreg 156
        // while RxFIFO not empty
378 holgerb 157
        while(SSP_GetFlagStatus(SSP0, SSP_FLAG_RxFifoNotEmpty) == SET)
79 killagreg 158
        {
41 ingob 159
                rxdata =  SSP0->DR; // catch the received byte
160
                // Fill TxFIFO while its not full or end of packet is reached
378 holgerb 161
                while (SSP_GetFlagStatus(SSP0, SSP_FLAG_TxFifoNotFull) == SET)
161 killagreg 162
                {
79 killagreg 163
                        if (SPI_TxBufferIndex  < SPI_TXBUFFER_LEN)   // still data to send ?
164
                        {
41 ingob 165
                                SSP0->DR = SPI_TxBuffer[SPI_TxBufferIndex];       // send a byte
79 killagreg 166
                                *Ptr_TxChksum += SPI_TxBuffer[SPI_TxBufferIndex]; // update checksum
378 holgerb 167
                                if(SPIWatchDog == 0) *Ptr_TxChksum += 1; // disturbe this packet to stop the communication!
79 killagreg 168
                                SPI_TxBufferIndex++; // pointer to next byte
169
                        }
170
                        else // end of packet is reached reset and copy data to tx buffer
171
                        {
172
                                SPI_TxBufferIndex = 0;  // reset buffer index
173
                                ToFlightCtrl.Chksum = 0;  // initialize checksum
174
                                ToFlightCtrl.BeepTime = BeepTime;  // set beeptime
175
                                BeepTime = 0; // reset local beeptime
41 ingob 176
                                // copy contents of ToFlightCtrl->SPI_TxBuffer
79 killagreg 177
                                memcpy((u8 *) &(SPI_TxBuffer[2]), (u8 *) &ToFlightCtrl, sizeof(ToFlightCtrl));
178
                        }
41 ingob 179
                }
180
                switch (SPI_State)
181
                {
182
                        case SPI_SYNC1:
183
                                SPI_RxBufferIndex = 0; // reset buffer index
184
                                rxchksum = rxdata;     // init checksum
185
                                if (rxdata == SPI_RXSYNCBYTE1)
186
                                {   // 1st syncbyte ok
187
                                        SPI_State = SPI_SYNC2;  // step to sync2
188
                                }
189
                                break;
79 killagreg 190
                        case SPI_SYNC2:
41 ingob 191
                                if (rxdata == SPI_RXSYNCBYTE2)
192
                                {  // 2nd Syncbyte ok
193
                                        rxchksum += rxdata;
194
                                        SPI_State = SPI_DATA;
195
                                }  // 2nd Syncbyte does not match
196
                                else
197
                                {
198
                                        SPI_State  = SPI_SYNC1; //jump back to sync1
199
                                }
200
                                break;
201
                        case SPI_DATA:
202
                                SPI_RxBuffer[SPI_RxBufferIndex++]= rxdata; // copy databyte to rx buffer
378 holgerb 203
                                if(SPI_RxBufferIndex >= SPI_RXBUFFER_LEN)  // end of packet is reached
41 ingob 204
                                {
205
                                        if (rxdata == rxchksum) // verify checksum byte
206
                                        {
207
                                                // copy SPI_RxBuffer -> FromFlightCtrl
208
                                                if(!SPI_RxBuffer_Request) // block writing to FromFlightCtrl on reading access
209
                                                {
210
                                                        memcpy((u8 *) &FromFlightCtrl, (u8 *) SPI_RxBuffer, sizeof(FromFlightCtrl));
211
                                                        SPI_RxBuffer_Request = 1;
212
                                                }
146 killagreg 213
                                                // reset timeout counter on good packet
214
                                                SPI0_Timeout = SetDelay(SPI0_TIMEOUT);
644 holgerb 215
//                                              DebugOut.Analog[13]++;
41 ingob 216
                                        }
217
                                        else // bad checksum byte
218
                                        {
219
                                                DebugOut.Analog[12]++; // increase SPI chksum error counter
220
                                        }
221
                                        SPI_State  = SPI_SYNC1; // reset state
222
                                }
223
                                else // end of packet not reached
224
                                {
225
                                        rxchksum += rxdata;      // update checksum
226
                                }
227
                                break;
228
                        default:
229
                                SPI_State  = SPI_SYNC1;
230
                                break;
231
                }
232
        }
195 killagreg 233
 
378 holgerb 234
//      IDISABLE;
235
        VIC1->VAR = 0xFF; // write any value to VIC0 Vector address register
1 ingob 236
}
237
 
238
//--------------------------------------------------------------
239
void SPI0_Init(void)
240
{
41 ingob 241
        GPIO_InitTypeDef GPIO_InitStructure;
242
        SSP_InitTypeDef   SSP_InitStructure;
1 ingob 243
 
110 killagreg 244
        UART1_PutString("\r\n SPI init...");
1 ingob 245
 
41 ingob 246
        SCU_APBPeriphClockConfig(__GPIO2 ,ENABLE);
247
        SCU_APBPeriphClockConfig(__SSP0 ,ENABLE);
1 ingob 248
 
41 ingob 249
        GPIO_DeInit(GPIO2);
250
        //SSP0_CLK, SSP0_MOSI, SSP0_NSS pins
251
        GPIO_StructInit(&GPIO_InitStructure);
252
        GPIO_InitStructure.GPIO_Direction = GPIO_PinInput;
253
        GPIO_InitStructure.GPIO_Pin = GPIO_Pin_4 | GPIO_Pin_5 | GPIO_Pin_7;
254
        GPIO_InitStructure.GPIO_Type = GPIO_Type_PushPull ;
196 killagreg 255
        GPIO_InitStructure.GPIO_IPInputConnected = GPIO_IPInputConnected_Enable;
41 ingob 256
        GPIO_InitStructure.GPIO_Alternate = GPIO_InputAlt1; //SSP0_SCLK, SSP0_MOSI, SSP0_NSS
257
        GPIO_Init (GPIO2, &GPIO_InitStructure);
1 ingob 258
 
41 ingob 259
        // SSP0_MISO pin GPIO2.6
260
        GPIO_StructInit(&GPIO_InitStructure);
261
        GPIO_InitStructure.GPIO_Direction = GPIO_PinOutput;
262
        GPIO_InitStructure.GPIO_Pin = GPIO_Pin_6;
263
        GPIO_InitStructure.GPIO_Type = GPIO_Type_PushPull ;
196 killagreg 264
        GPIO_InitStructure.GPIO_IPInputConnected = GPIO_IPInputConnected_Enable;
41 ingob 265
        GPIO_InitStructure.GPIO_Alternate = GPIO_OutputAlt2; //SSP0_MISO
266
        GPIO_Init (GPIO2, &GPIO_InitStructure);
1 ingob 267
 
41 ingob 268
        SSP_DeInit(SSP0);
269
        SSP_StructInit(&SSP_InitStructure);
270
        SSP_InitStructure.SSP_FrameFormat = SSP_FrameFormat_Motorola;
271
        SSP_InitStructure.SSP_Mode = SSP_Mode_Slave;
272
        SSP_InitStructure.SSP_SlaveOutput = SSP_SlaveOutput_Enable;
273
        SSP_InitStructure.SSP_CPHA = SSP_CPHA_1Edge;
274
        SSP_InitStructure.SSP_CPOL = SSP_CPOL_Low;
275
        SSP_InitStructure.SSP_ClockRate = 0;
1 ingob 276
 
41 ingob 277
        SSP_Init(SSP0, &SSP_InitStructure);
144 killagreg 278
        SSP_ITConfig(SSP0, SSP_IT_RxFifo | SSP_IT_RxTimeOut, ENABLE);
41 ingob 279
        SSP_Cmd(SSP0, ENABLE);
280
        // initialize the syncbytes in the tx buffer
281
        SPI_TxBuffer[0] = SPI_TXSYNCBYTE1;
282
        SPI_TxBuffer[1] = SPI_TXSYNCBYTE2;
283
        // set the pointer to the checksum byte in the tx buffer
284
        Ptr_TxChksum = (u8 *) &(((ToFlightCtrl_t *) &(SPI_TxBuffer[2]))->Chksum);
285
 
215 killagreg 286
        ToFlightCtrl.GPSStick.Nick = 0;
287
        ToFlightCtrl.GPSStick.Roll = 0;
419 holgerb 288
//      ToFlightCtrl.GPSStick.Yaw = 0;
215 killagreg 289
 
136 killagreg 290
        VIC_Config(SSP0_ITLine, VIC_IRQ, PRIORITY_SPI0);
41 ingob 291
        VIC_ITCmd(SSP0_ITLine, ENABLE);
292
 
146 killagreg 293
        SPI0_Timeout = SetDelay(4*SPI0_TIMEOUT);
294
 
110 killagreg 295
        UART1_PutString("ok");
1 ingob 296
}
41 ingob 297
 
222 holgerb 298
 
1 ingob 299
//------------------------------------------------------
41 ingob 300
void SPI0_UpdateBuffer(void)
1 ingob 301
{
180 killagreg 302
        static u32 timeout = 0;
426 holgerb 303
        static u8 counter = 50,hott_index = 0, last_error_code = 0, enable_injecting = 0;
304
        static s16 last_wp_event = 0;
605 holgerb 305
        u8 index;
489 killagreg 306
        s16 tmp;
329 holgerb 307
        s32 i1,i2;
462 holgerb 308
/*
309
        union
310
        {
311
                unsigned char Byte[4];
312
                unsigned int Int[2];
313
                unsigned long Long;
314
        } Temp;
489 killagreg 315
*/
515 killagreg 316
        SPIWatchDog = 3500;     // stop communication to FC after this timeout
378 holgerb 317
        if(SPI_RxBuffer_Request)
41 ingob 318
        {
319
                // avoid sending data via SPI during the update of the  ToFlightCtrl structure
320
                VIC_ITCmd(SSP0_ITLine, DISABLE); // disable SPI interrupt
254 killagreg 321
                ToFlightCtrl.CompassHeading = Compass_Heading;
189 killagreg 322
                DebugOut.Analog[10] = ToFlightCtrl.CompassHeading;
510 holgerb 323
                GyroCompassCorrected = (3600 + FromFlightCtrl.GyroHeading + FC.FromFC_CompassOffset + GeoMagDec) % 3600;
228 holgerb 324
                if(ToFlightCtrl.CompassHeading >= 0) ToFlightCtrl.CompassHeading = (360 + ToFlightCtrl.CompassHeading + FromFlightCtrl.GyroYaw / 12) % 360;
419 holgerb 325
                ToFlightCtrl.MagVecX = MagVector.X;
326
                ToFlightCtrl.MagVecY = MagVector.Y;
286 killagreg 327
                ToFlightCtrl.MagVecZ = MagVector.Z;
419 holgerb 328
//              ToFlightCtrl.NCStatus = 0;
41 ingob 329
                // cycle spi commands
489 killagreg 330
                if(ErrorCode != last_error_code && enable_injecting)
515 killagreg 331
                {
332
                        ToFlightCtrl.Command = SPI_NCCMD_VERSION;
333
                        last_error_code = ErrorCode;
334
                        enable_injecting = 0;
335
                }
489 killagreg 336
                else
337
                if(FC_WP_EventChannel != last_wp_event && enable_injecting)
515 killagreg 338
                {
339
                        ToFlightCtrl.Command = SPI_NCCMD_GPSINFO;
340
                        last_wp_event = FC_WP_EventChannel;
341
                        enable_injecting = 0;
342
                }
489 killagreg 343
                else
515 killagreg 344
                {
345
                        ToFlightCtrl.Command = SPI_CommandSequence[SPI_CommandCounter++];
346
                        // restart command cycle at the end
347
                        if(SPI_CommandCounter >= sizeof(SPI_CommandSequence)) SPI_CommandCounter = 0;
348
                        if(ToFlightCtrl.Command == SPI_NCCMD_KALMAN) enable_injecting = 1;
349
                }
419 holgerb 350
 
319 holgerb 351
#define FLAG_GPS_AID 0x01
41 ingob 352
                switch (ToFlightCtrl.Command)
353
                {
329 holgerb 354
                        case  SPI_NCCMD_KALMAN:  // wird am häufigsten betätigt
149 killagreg 355
                                ToFlightCtrl.Param.sByte[0] = (s8) Kalman_K;
92 killagreg 356
                                ToFlightCtrl.Param.sByte[1] = (s8) Kalman_MaxFusion;
357
                                ToFlightCtrl.Param.sByte[2] = (s8) Kalman_MaxDrift;
330 holgerb 358
                                ToFlightCtrl.Param.Byte[3]      = (u8) Kalman_Kompass;
588 holgerb 359
                                ToFlightCtrl.Param.Byte[4] = 0; // siehe bitcodiert unten
222 holgerb 360
                                ToFlightCtrl.Param.Byte[5] = (s8) ToFC_Rotate_C;
223 killagreg 361
                                ToFlightCtrl.Param.Byte[6] = (s8) ToFC_Rotate_S;
515 killagreg 362
                                ToFlightCtrl.Param.Byte[7] = GPS_Aid_StickMultiplikator;
280 killagreg 363
                                if(CAM_Orientation.UpdateMask & CAM_UPDATE_AZIMUTH)
278 killagreg 364
                                {
544 holgerb 365
//                                      if(CAM_Orientation.Azimuth != -1) ToFlightCtrl.Param.sInt[4] = (CAM_Orientation.Azimuth + (3*360) - (FC.FromFC_CompassOffset / 10 + GeoMagDec/10 + Parameter.OrientationAngle * 15)) % 360;  // the FC uses the uncorrected comnpass value
366
                                        if(CAM_Orientation.Azimuth != -1) ToFlightCtrl.Param.sInt[4] = (CAM_Orientation.Azimuth + (3*360) - (FC.FromFC_CompassOffset / 10 + GeoMagDec/10 + Parameter.CamOrientation * 15)) % 360;  // the FC uses the uncorrected comnpass value
514 holgerb 367
                                        else CAM_Orientation.Azimuth = -1;
588 holgerb 368
                                        if(CAM_Orientation.UpdateMask & FORCE_AZIMUTH_ROTATION) ToFlightCtrl.Param.sByte[4] = 0x01;     // allows Yawing without CareFree (Yawing at Coming Home)
369
                                        CAM_Orientation.UpdateMask &= ~(CAM_UPDATE_AZIMUTH | FORCE_AZIMUTH_ROTATION);
278 killagreg 370
                                }
371
                                else
280 killagreg 372
                                {
278 killagreg 373
                                        ToFlightCtrl.Param.sInt[4] = -1;
374
                                }
294 holgerb 375
 
299 killagreg 376
                                if(NCRARAM_STATE_VALID == NCParams_GetValue(NCPARAMS_NEW_CAMERA_ELEVATION, &tmp))  // Elevation set via 'j' command
295 killagreg 377
                                {
351 holgerb 378
                                        POI_KameraNick = tmp;
299 killagreg 379
                                }
380
                                else
381
                                {
348 holgerb 382
                                        //if(FC.StatusFlags2 & FC_STATUS2_CAREFREE) // only, if carefree is active
489 killagreg 383
                                        POI_KameraNick = CAM_Orientation.Elevation;
348 holgerb 384
                                        //else ToFlightCtrl.Param.sInt[5] = 0;
295 killagreg 385
                                }
351 holgerb 386
                                ToFlightCtrl.Param.sInt[5] = POI_KameraNick;
161 killagreg 387
                                break;
58 killagreg 388
 
202 killagreg 389
                        case SPI_NCCMD_VERSION:
355 holgerb 390
                                //+++++++++++++++++++++++++++++++++++++++++++++++++++
515 killagreg 391
                                //+ higher than the maximum allowed altitude
355 holgerb 392
                                //+++++++++++++++++++++++++++++++++++++++++++++++++++
202 killagreg 393
                                ToFlightCtrl.Param.Byte[0] = VERSION_MAJOR;
394
                                ToFlightCtrl.Param.Byte[1] = VERSION_MINOR;
204 killagreg 395
                                ToFlightCtrl.Param.Byte[2] = VERSION_PATCH;
202 killagreg 396
                                ToFlightCtrl.Param.Byte[3] = FC_SPI_COMPATIBLE;
264 killagreg 397
                                ToFlightCtrl.Param.Byte[4] = Version_HW;
338 holgerb 398
                                ToFlightCtrl.Param.Byte[5] = DebugOut.StatusGreen;
399
                                ToFlightCtrl.Param.Byte[6] = DebugOut.StatusRed;
454 holgerb 400
                                ToFlightCtrl.Param.Byte[7] = ErrorCode;                          // muss in SPI_NCCMD_VERSION bleiben! (siehe oben)
323 holgerb 401
                                ToFlightCtrl.Param.Byte[8] = NC_GPS_ModeCharacter;
330 holgerb 402
                                ToFlightCtrl.Param.Byte[9] = SerialLinkOkay;
351 holgerb 403
                                ToFlightCtrl.Param.Byte[10] = NC_To_FC_Flags;
532 holgerb 404
//[11]
405
//                              if(AbsoluteFlyingAltitude > 255) ToFlightCtrl.Param.Byte[11] = 0; // then the limitation of the FC doesn't work
406
//                              else ToFlightCtrl.Param.Byte[11] = AbsoluteFlyingAltitude;
202 killagreg 407
                                break;
342 holgerb 408
                        case SPI_MISC:
409
                                ToFlightCtrl.Param.Byte[0] = EarthMagneticFieldFiltered/5;
410
                                ToFlightCtrl.Param.Byte[1] = EarthMagneticInclination;
411
                                ToFlightCtrl.Param.Byte[2] = EarthMagneticInclinationTheoretic;
397 holgerb 412
                                ToFlightCtrl.Param.Byte[3] = SpeakHoTT;
419 holgerb 413
                                ToFlightCtrl.Param.Byte[4] = NaviData.WaypointIndex; // index of current waypoints running from 0 to WaypointNumber-1
414
                                ToFlightCtrl.Param.Byte[5] = NaviData.WaypointNumber; // number of stored waypoints
415
                                ToFlightCtrl.Param.Int[3] =  NaviData.TargetPositionDeviation.Distance / 10;
416
                                ToFlightCtrl.Param.Byte[8] = NaviData.TargetHoldTime; // time in s to stay at the given target, counts down to 0 if target has been reached
503 holgerb 417
                                ToFlightCtrl.Param.Byte[9] = ToFC_MaxWpListIndex;
507 holgerb 418
                                ToFlightCtrl.Param.sInt[5] = GyroCompassCorrected / 10; // Bytes 10 & 11
578 holgerb 419
 
397 holgerb 420
                                SpeakHoTT = 0;
342 holgerb 421
                                break;
223 killagreg 422
 
204 killagreg 423
                        case SPI_NCCMD_GPSINFO:
424
                                ToFlightCtrl.Param.Byte[0] = GPSData.Flags;
425
                                ToFlightCtrl.Param.Byte[1] = GPSData.NumOfSats;
426
                                ToFlightCtrl.Param.Byte[2] = GPSData.SatFix;
232 killagreg 427
                                ToFlightCtrl.Param.Byte[3] = GPSData.Speed_Ground / 100; // m/s
258 holgerb 428
                                ToFlightCtrl.Param.Int[2]  = NaviData.HomePositionDeviation.Distance; // dm   //4&5
429
                                ToFlightCtrl.Param.sInt[3] = NaviData.HomePositionDeviation.Bearing;  // deg  //6&7
454 holgerb 430
                                if(FC_WP_EventChannel > 254) FC_WP_EventChannel = 254;    // Muss in SPI_NCCMD_GPSINFO bleiben! (siehe oben)
598 holgerb 431
                                if(FC_WP_EventChannel)
432
                                 {
433
                                  LogFC_WP_EventChannel = FC_WP_EventChannel; // to make sure that it will be logged
434
                                  NaviData_WP.WP_Eventchannel = FC_WP_EventChannel; // to make sure that it will be logged
618 holgerb 435
  Out1TriggerUpdateBlocked = 3; // makes sure that the right trigger-pos is sent in command 18
436
  NaviData_Out1Trigger.Longitude = NaviData.CurrentPosition.Longitude;
437
  NaviData_Out1Trigger.Latitude = NaviData.CurrentPosition.Latitude;
598 holgerb 438
                                 }
577 holgerb 439
                                FC_WP_EventChannel_Processed = 1;
623 holgerb 440
//DebugOut.Analog[] = FC_WP_EventChannel;
457 holgerb 441
  // ++++++++++++++++++++++++++++++++++
442
  // Waypoint event +++++++++++++++++++
443
  // ++++++++++++++++++++++++++++++++++
444
                                if(NCRARAM_STATE_VALID == NCParams_GetValue(NCPARAMS_WP_EVENT_ONCE, &tmp))
445
                                {
446
                                        ToFlightCtrl.Param.Byte[8] = (s8)(tmp - 127);
447
                                        NCParams_ClearValue(NCPARAMS_WP_EVENT_ONCE);
448
                                        NCParams_ClearValue(NCPARAMS_WP_EVENT_FOREVER);
449
                                }
450
                                else
451
                                if(NCRARAM_STATE_VALID == NCParams_GetValue(NCPARAMS_WP_EVENT_FOREVER, &tmp))
452
                                {
453
                                        ToFlightCtrl.Param.Byte[8] = (s8)(tmp - 127);
454
                                        if(tmp == 0) NCParams_ClearValue(NCPARAMS_WP_EVENT_FOREVER);
455
                                }
456
                                else ToFlightCtrl.Param.Byte[8] = (s8)(FC_WP_EventChannel - 127);
577 holgerb 457
//                              FC_WP_EventChannel = 0; // the GPS-Routine will set it again
457 holgerb 458
  // ++++++++++++++++++++++++++++++++++
299 killagreg 459
                                if(NCRARAM_STATE_VALID == NCParams_GetValue(NCPARAMS_ALTITUDE_RATE, &tmp))
460
                                {
461
                                        ToFlightCtrl.Param.Byte[9] = (u8)tmp;
295 killagreg 462
                                }
299 killagreg 463
                                else
464
                                {
465
                                        ToFlightCtrl.Param.Byte[9] = (u8)ToFC_AltitudeRate;
466
                                }
467
                                if(NCRARAM_STATE_VALID == NCParams_GetValue(NCPARAMS_ALTITUDE_SETPOINT, &tmp))
468
                                {
469
                                        ToFlightCtrl.Param.sInt[5] = tmp;
470
                                }
471
                                else
472
                                {
473
                                        ToFlightCtrl.Param.sInt[5] = (s16)ToFC_AltitudeSetpoint;
474
                                }
516 holgerb 475
//DebugOut.Analog[] = ToFlightCtrl.Param.Byte[8];
204 killagreg 476
                                break;
330 holgerb 477
                        case SPI_NCCMD_HOTT_INFO:
515 killagreg 478
                                if(NewWPL_Name) hott_index = 100;
330 holgerb 479
                                switch(hott_index++)
480
                                {
329 holgerb 481
                                case 0:
482
                                        //Dezimalgrad            --> Grad mit Dezimalminuten     --> Grad, Minuten, Sekunden
489 killagreg 483
                                        //53.28 5788 7.4847269   --> N53° 17.14728 E7° 29.08362  --> N53° 17' 8.837" E7° 29' 5.017"
329 holgerb 484
                                        ToFlightCtrl.Param.Byte[11] = HOTT_GPS_PACKET_ID;
397 holgerb 485
                                        ToFlightCtrl.Param.Byte[0] = 3+3;       // index  +3, weil bei HoTT V4 3 Bytes eingeschoben wurden
329 holgerb 486
                                        ToFlightCtrl.Param.Byte[1] = 9-1;       // how many
487
                                        //-----------------------------
489 killagreg 488
                                        ToFlightCtrl.Param.Byte[2] = GyroCompassCorrected / 20;//NaviData.HomePositionDeviation.Bearing / 2;
329 holgerb 489
                                        i1 = GPSData.Speed_Ground; // in cm/sec
490
                                        i1 *= 36;
491
                                        i1 /= 1000;
492
                                        ToFlightCtrl.Param.Byte[3] = i1 % 256;
493
                                        ToFlightCtrl.Param.Byte[4] = i1 / 256;
494
                                        //-----------------------------
405 holgerb 495
                                        if(GPSData.Position.Latitude < 0) ToFlightCtrl.Param.Byte[5]  = 1; // 1 = S
329 holgerb 496
                                        else ToFlightCtrl.Param.Byte[5]  = 0; // 1 = S
497
                                        i1 = abs(GPSData.Position.Latitude)/10000000L;
498
                                        i2 = abs(GPSData.Position.Latitude)%10000000L;
397 holgerb 499
                                        if(!(NCFlags & NC_FLAG_GPS_OK)) {i1 = 0; i2 = 0;}
489 killagreg 500
                                        i1 *= 100;
460 holgerb 501
                                // Minuten
502
                                i2 *= 6;
503
                                i2 /= 10;
329 holgerb 504
                                        i1 += i2 / 100000;
505
                                        i2  = i2 % 100000;
506
                                        i2 /= 10;
507
                                        ToFlightCtrl.Param.Byte[6]  = i1 % 256;
508
                                        ToFlightCtrl.Param.Byte[7]  = i1 / 256;
509
                                        ToFlightCtrl.Param.Byte[8]  = i2 % 256;
510
                                        ToFlightCtrl.Param.Byte[9]  = i2 / 256;
511
                                        break;
512
                                case 1:
513
                                        ToFlightCtrl.Param.Byte[11] = HOTT_GPS_PACKET_ID;
397 holgerb 514
                                        ToFlightCtrl.Param.Byte[0] = 11+3;      // index          +3, weil bei HoTT V4 3 Bytes eingeschoben wurden
329 holgerb 515
                                        ToFlightCtrl.Param.Byte[1] = 8-1;       // how many
516
                                        //-----------------------------
517
                                        if(GPSData.Position.Longitude < 0) ToFlightCtrl.Param.Byte[2]  = 1; // 1 = E
518
                                        else ToFlightCtrl.Param.Byte[2]  = 0; // 1 = S
519
                                        i1 = abs(GPSData.Position.Longitude)/10000000L;
520
                                        i2 = abs(GPSData.Position.Longitude)%10000000L;
397 holgerb 521
                                        if(!(NCFlags & NC_FLAG_GPS_OK)) {i1 = 0; i2 = 0;}
489 killagreg 522
                                        i1 *= 100;
460 holgerb 523
                                // Minuten
524
                                i2 *= 6;
525
                                i2 /= 10;
329 holgerb 526
                                        i1 += i2 / 100000;
527
                                        i2  = i2 % 100000;
528
                                        i2 /= 10;
529
                                        ToFlightCtrl.Param.Byte[3]  = i1 % 256;
530
                                        ToFlightCtrl.Param.Byte[4]  = i1 / 256;
531
                                        ToFlightCtrl.Param.Byte[5]  = i2 % 256;
532
                                        ToFlightCtrl.Param.Byte[6]  = i2 / 256;
533
                                        //-----------------------------
489 killagreg 534
                                        i1 = NaviData.HomePositionDeviation.Distance / 10; // dann in m
329 holgerb 535
                                        ToFlightCtrl.Param.Byte[7]  = i1 % 256;
536
                                        ToFlightCtrl.Param.Byte[8]  = i1 / 256;
330 holgerb 537
                                        break;
538
                                 case 2:
539
                                        ToFlightCtrl.Param.Byte[11] = HOTT_GENERAL_PACKET_ID;
397 holgerb 540
                                        ToFlightCtrl.Param.Byte[0] = 5+3;       // index   // +3, weil bei HoTT V4 3 Bytes eingeschoben wurden
330 holgerb 541
                                        ToFlightCtrl.Param.Byte[1] = 2; // how many
489 killagreg 542
                                        ToFlightCtrl.Param.Byte[2] = EarthMagneticField / (5 * 2);
330 holgerb 543
                                        ToFlightCtrl.Param.Byte[3] = EarthMagneticInclination / 2;
329 holgerb 544
                                        break;
462 holgerb 545
                                 case 3:
546
                                        ToFlightCtrl.Param.Byte[11] = JETI_GPS_PACKET_ID1;
489 killagreg 547
                                        ToFlightCtrl.Param.Byte[0] = 0; // index
462 holgerb 548
                                        ToFlightCtrl.Param.Byte[1] = 4; // how many
549
                                        //JetiExData[14].Value =  53 * 0x10000 + 23467;
489 killagreg 550
                                        if(GPSData.Position.Latitude < 0) ToFlightCtrl.Param.Byte[5] = 0x40;
551
                                        else ToFlightCtrl.Param.Byte[5] = 0x00;
462 holgerb 552
                                        ToFlightCtrl.Param.Byte[4] = abs(GPSData.Position.Latitude)/10000000L;
553
                                        i2 = abs(GPSData.Position.Latitude)%10000000L;
554
                                        i2 *= 6;
555
                                        i2 /= 1000;
556
                                        ToFlightCtrl.Param.Byte[3] = i2 / 256;
557
                                        ToFlightCtrl.Param.Byte[2] = i2 % 256;
558
                                        break;
559
                                 case 4:
560
                                        ToFlightCtrl.Param.Byte[11] = JETI_GPS_PACKET_ID2;
489 killagreg 561
                                        ToFlightCtrl.Param.Byte[0] = 0; // index
462 holgerb 562
                                        ToFlightCtrl.Param.Byte[1] = 4; // how many
489 killagreg 563
                                        if(GPSData.Position.Latitude < 0) ToFlightCtrl.Param.Byte[5] = 0x60;
564
                                        else ToFlightCtrl.Param.Byte[5] = 0x20;
462 holgerb 565
                                        ToFlightCtrl.Param.Byte[4] = abs(GPSData.Position.Longitude)/10000000L;
566
                                        i2 = abs(GPSData.Position.Longitude)%10000000L;
567
                                        i2 *= 6;
568
                                        i2 /= 1000;
569
                                        ToFlightCtrl.Param.Byte[3] = i2 / 256;
570
                                        ToFlightCtrl.Param.Byte[2] = i2 % 256;
571
                        hott_index = 0;
572
                                        break;
504 holgerb 573
 
574
                                        case 100:
575
                                        ToFlightCtrl.Param.Byte[11] = HOTT_WPL_NAME;
576
                                        ToFlightCtrl.Param.Byte[0] = 0; // index
577
                                        ToFlightCtrl.Param.Byte[2] = WPL_Store.Name[0];
578
                                        ToFlightCtrl.Param.Byte[3] = WPL_Store.Name[1];
579
                                        ToFlightCtrl.Param.Byte[4] = WPL_Store.Name[2];
580
                                        ToFlightCtrl.Param.Byte[5] = WPL_Store.Name[3];
581
                                        ToFlightCtrl.Param.Byte[6] = WPL_Store.Name[4];
582
                                        ToFlightCtrl.Param.Byte[7] = WPL_Store.Name[5];
583
                                        ToFlightCtrl.Param.Byte[8] = WPL_Store.Name[6];
584
                                        ToFlightCtrl.Param.Byte[9] = WPL_Store.Name[7];
585
                                        ToFlightCtrl.Param.Byte[10]= WPL_Store.Name[8];
586
                                        ToFlightCtrl.Param.Byte[1] = 9; // how many
587
                                        NewWPL_Name = 0;
588
                                        hott_index = 0;
589
                                        break;
489 killagreg 590
                                 default:
330 holgerb 591
                                        ToFlightCtrl.Param.Byte[0] = 255;
515 killagreg 592
                                        hott_index = 0;
593
                                        break;
329 holgerb 594
                                }
595
                                break;
41 ingob 596
                        default:
597
                                break;
285 holgerb 598
// 0 = 0,1
599
// 1 = 2,3
600
// 2 = 4,5
601
// 3 = 6,7
602
// 4 = 8,9
603
// 5 = 10,11
41 ingob 604
                }
605
                VIC_ITCmd(SSP0_ITLine, ENABLE);         // enable SPI interrupt
606
                switch(FromFlightCtrl.Command)
607
                {
202 killagreg 608
                        case SPI_FCCMD_USER:
41 ingob 609
                                Parameter.User1 = FromFlightCtrl.Param.Byte[0];
610
                                Parameter.User2 = FromFlightCtrl.Param.Byte[1];
611
                                Parameter.User3 = FromFlightCtrl.Param.Byte[2];
612
                                Parameter.User4 = FromFlightCtrl.Param.Byte[3];
613
                                Parameter.User5 = FromFlightCtrl.Param.Byte[4];
614
                                Parameter.User6 = FromFlightCtrl.Param.Byte[5];
615
                                Parameter.User7 = FromFlightCtrl.Param.Byte[6];
616
                                Parameter.User8 = FromFlightCtrl.Param.Byte[7];
567 holgerb 617
                                FC.RealStatusFlags = FromFlightCtrl.Param.Byte[8];
618
                                if(FC.RealStatusFlags & FC_STATUS_MOTOR_RUN) SimulationFlags = 0; // stop the simulation if the motors would really start
619
 
620
                                if(!(SimulationFlags & SIMULATION_ACTIVE))
41 ingob 621
                                {
567 holgerb 622
                                        if(ClearFCStatusFlags)
623
                                        {
624
                                                FC.StatusFlags = 0;
625
                                                ClearFCStatusFlags = 0;
626
                                        }
41 ingob 627
                                }
567 holgerb 628
                                FC.StatusFlags |= FC.RealStatusFlags;
320 holgerb 629
                                if(FC.StatusFlags & FC_STATUS_CALIBRATE && !FCCalibActive)
268 killagreg 630
                                {
510 holgerb 631
                                        HeadFreeStartAngle = (3600 + Compass_Heading * 10 + GeoMagDec) % 3600;
587 holgerb 632
                                        CompassDirectionAtMotorStart = HeadFreeStartAngle;
268 killagreg 633
                                        Compass_Init();
644 holgerb 634
                                        FCCalibActive = 15;
338 holgerb 635
                                        FC_is_Calibrated = 0;
644 holgerb 636
FreqNewGpsDataIn5Sec = 50;
268 killagreg 637
                                }
638
                                else
639
                                {
489 killagreg 640
                                        if(FCCalibActive)
641
                                        {
642
                                                if(--FCCalibActive == 0)
643
                                                {
644
                                                        FC_is_Calibrated = 1;
645
                                                        Compass_Check();
646
                                                }
647
                                        }
268 killagreg 648
                                }
515 killagreg 649
                                if(FC.StatusFlags & FC_STATUS_START)
489 killagreg 650
                                {
515 killagreg 651
                                        if(Compass_Heading != -1) HeadFreeStartAngle = (3600 + Compass_Heading * 10 + GeoMagDec) % 3600;
510 holgerb 652
                                        else HeadFreeStartAngle = GyroCompassCorrected;
587 holgerb 653
                                        CompassDirectionAtMotorStart = HeadFreeStartAngle;
489 killagreg 654
                                }
320 holgerb 655
 
489 killagreg 656
                                if((Parameter.ExtraConfig & CFG_TEACHABLE_CAREFREE))
657
                                {
658
                                        if(!(FC.StatusFlags2 & FC_STATUS2_CAREFREE)) // CF ist jetzt ausgeschaltet -> neue Richtung lernen
659
                                        {
660
                                                if((NaviData.HomePositionDeviation.Distance > 200) && (NCFlags & NC_FLAG_GPS_OK))        // nur bei ausreichender Distance -> 20m
661
                                                {
662
                                                        HeadFreeStartAngle = (10 * NaviData.HomePositionDeviation.Bearing + 1800 + 3600 -  Parameter.OrientationAngle * 150) % 3600; // in 0.1°
663
                                                }
664
                                                else                                                                                             // Ansonsten die aktuelle Richtung übernehmen
665
                                                HeadFreeStartAngle = GyroCompassCorrected; // in 0.1°
329 holgerb 666
                                        }
667
                                }
360 holgerb 668
                                NaviData.FCStatusFlags = FC.StatusFlags;
489 killagreg 669
                                if(FC.StatusFlags2 & FC_STATUS2_WAIT_FOR_TAKEOFF) NaviData.FCStatusFlags &= ~FC_STATUS_FLY;
598 holgerb 670
                                FC.StatusFlags2 = FromFlightCtrl.Param.Byte[9];
360 holgerb 671
                                NaviData.FCStatusFlags2 = (NaviData.FCStatusFlags2 & (FC_STATUS2_OUT1_ACTIVE | FC_STATUS2_OUT2_ACTIVE)) | (FC.StatusFlags2 & (0xff - (FC_STATUS2_OUT1_ACTIVE | FC_STATUS2_OUT2_ACTIVE)));
672
                                if((!(LastTransmittedFCStatusFlags2 & FC_STATUS2_OUT1_ACTIVE)) && (FC.StatusFlags2 & FC_STATUS2_OUT1_ACTIVE)) NaviData.FCStatusFlags2 |= FC_STATUS2_OUT1_ACTIVE;
489 killagreg 673
                                else
360 holgerb 674
                                if(((LastTransmittedFCStatusFlags2 & FC_STATUS2_OUT1_ACTIVE)) && !(FC.StatusFlags2 & FC_STATUS2_OUT1_ACTIVE)) NaviData.FCStatusFlags2 &= ~FC_STATUS2_OUT1_ACTIVE;
675
 
676
                                if((!(LastTransmittedFCStatusFlags2 & FC_STATUS2_OUT2_ACTIVE)) && (FC.StatusFlags2 & FC_STATUS2_OUT2_ACTIVE)) NaviData.FCStatusFlags2 |= FC_STATUS2_OUT2_ACTIVE;
489 killagreg 677
                                else
360 holgerb 678
                                if(((LastTransmittedFCStatusFlags2 & FC_STATUS2_OUT2_ACTIVE)) && !(FC.StatusFlags2 & FC_STATUS2_OUT2_ACTIVE)) NaviData.FCStatusFlags2 &= ~FC_STATUS2_OUT2_ACTIVE;
623 holgerb 679
//DebugOut.Analog[] = (NaviData.FCStatusFlags2 & FC_STATUS2_OUT1_ACTIVE);
338 holgerb 680
                                Logging_FCStatusFlags1 |= FC.StatusFlags;
681
                                Logging_FCStatusFlags2 |= FC.StatusFlags2;
598 holgerb 682
                                FC.BAT_Voltage = FromFlightCtrl.Param.Int[5]; // 10 & 11
683
                                DebugOut.Analog[7] = FC.BAT_Voltage;
684
                                DebugOut.Analog[5] = FC.StatusFlags;
685
                                NaviData.UBat = (u8) FC.BAT_Voltage;     // Achtung: die (u8) NaviData.UBat kann überlaufen -> das KopterTool müsste dann 25,5V drauf rechnen
41 ingob 686
                                break;
454 holgerb 687
                        case SPI_FCCMD_BL_ACCU:
223 killagreg 688
                                FC.BAT_Current = FromFlightCtrl.Param.Int[0];
480 holgerb 689
                                DebugOut.Analog[8] = FC.BAT_Current;
533 holgerb 690
                                BL_MinOfMaxPWM = FromFlightCtrl.Param.Byte[3];  
530 holgerb 691
                                if(BL_MinOfMaxPWM < Logging_BL_MinOfMaxPWM)     Logging_BL_MinOfMaxPWM = BL_MinOfMaxPWM;        // hold the value until logged
692
                                if(BL_MinOfMaxPWM < ErrorCheck_BL_MinOfMaxPWM)  ErrorCheck_BL_MinOfMaxPWM = BL_MinOfMaxPWM;             // hold the value until Error processed
471 holgerb 693
                                Parameter.NaviGpsModeControl = FromFlightCtrl.Param.Byte[4];
694
                                FromFC_VarioCharacter = FromFlightCtrl.Param.Byte[5];
611 holgerb 695
//0x40
696
//0x20
697
//0x10
698
if(FromFlightCtrl.Param.Byte[2] & 0x80) // this Flag marks a changed Out1
699
 {
618 holgerb 700
  Out1TriggerUpdateNewData = 1;
701
  if(!Out1TriggerUpdateBlocked)
702
   {
611 holgerb 703
                NaviData_Out1Trigger.Longitude = NaviData.CurrentPosition.Longitude;
704
                NaviData_Out1Trigger.Latitude = NaviData.CurrentPosition.Latitude;
618 holgerb 705
   }
706
  Out1TriggerUpdateBlocked = 0;
611 holgerb 707
 }
618 holgerb 708
if(Out1TriggerUpdateBlocked) Out1TriggerUpdateBlocked--;
611 holgerb 709
                                index = FromFlightCtrl.Param.Byte[2] & 0x0f; //MAX_MOTORS
638 holgerb 710
                                if(AmountOfMotors < index+1) AmountOfMotors = index+1;
605 holgerb 711
                                Motor[index].NotReadyCnt = FromFlightCtrl.Param.Byte[6];
712
                                Motor_Version[index] = FromFlightCtrl.Param.Byte[7];
713
                                Motor[index].MaxPWM = FromFlightCtrl.Param.Byte[8];
714
                                Motor[index].State = FromFlightCtrl.Param.Byte[9];
715
                                Motor[index].Temperature = FromFlightCtrl.Param.Byte[10];
716
                                Motor[index].Current = FromFlightCtrl.Param.Byte[11];
298 holgerb 717
                                if(FromFC_VarioCharacter == '+' || FromFC_VarioCharacter == '-') // manual setpoint clears the NC-Parameter command
515 killagreg 718
                                {
719
                                        NCParams_ClearValue(NCPARAMS_ALTITUDE_RATE);
720
                                }
567 holgerb 721
                                if(!(SimulationFlags & SIMULATION_ACTIVE)) NaviData.Current = FC.BAT_Current;
206 killagreg 722
                                NaviData.UsedCapacity = FC.BAT_UsedCapacity;
723
                                break;
202 killagreg 724
                        case SPI_FCCMD_PARAMETER1:
454 holgerb 725
                                Parameter.LowVoltageWarning = FromFlightCtrl.Param.Byte[0];
41 ingob 726
                                CHK_POTI_MM(Parameter.NaviGpsGain,FromFlightCtrl.Param.Byte[1],0,255);
727
                                CHK_POTI_MM(Parameter.NaviGpsP,FromFlightCtrl.Param.Byte[2],0,255);
728
                                CHK_POTI_MM(Parameter.NaviGpsI,FromFlightCtrl.Param.Byte[3],0,255);
729
                                CHK_POTI_MM(Parameter.NaviGpsD,FromFlightCtrl.Param.Byte[4],0,255);
730
                                CHK_POTI_MM(Parameter.NaviGpsACC,FromFlightCtrl.Param.Byte[5],0,255);
731
                                Parameter.NaviGpsMinSat = FromFlightCtrl.Param.Byte[6];
732
                                Parameter.NaviStickThreshold = FromFlightCtrl.Param.Byte[7];
532 holgerb 733
//                              CHK_POTI_MM(Parameter.NaviOperatingRadius,FromFlightCtrl.Param.Byte[8],0,255);
734
                                Parameter.NaviMaxFlyingRange = FromFlightCtrl.Param.Byte[8];
41 ingob 735
                                CHK_POTI_MM(Parameter.NaviWindCorrection,FromFlightCtrl.Param.Byte[9],0,255);
327 holgerb 736
                                CHK_POTI_MM(Parameter.NaviAccCompensation,FromFlightCtrl.Param.Byte[10],0,255);
41 ingob 737
                                CHK_POTI_MM(Parameter.NaviAngleLimitation,FromFlightCtrl.Param.Byte[11],0,255);
121 killagreg 738
                                break;
419 holgerb 739
                        case SPI_FCCMD_PARAMETER2:
605 holgerb 740
                                CHK_POTI_MM(FC.AutoPhotoDistance,FromFlightCtrl.Param.Byte[0],0,255);
623 holgerb 741
                                if(FromFlightCtrl.Param.Byte[1])
742
                                 {
743
                                  FC.FromFC_SpeakHoTT = FromFlightCtrl.Param.Byte[1];                   // will be cleared in the SD-Logging
626 holgerb 744
                                 }
745
                                if(NaviData_Flags_SpeakHoTT_Processed)
746
                                 {
623 holgerb 747
                                  NaviData_Flags.SpeakHoTT = FromFlightCtrl.Param.Byte[1];              // will be cleared after the Uart-Trasmitting
626 holgerb 748
                                  NaviData_Flags_SpeakHoTT_Processed = 0;
623 holgerb 749
                                 }
587 holgerb 750
                                FC.BAT_UsedCapacity = FromFlightCtrl.Param.Int[1]; // 2 & 3
489 killagreg 751
                                Parameter.FromFC_LowVoltageHomeActive = FromFlightCtrl.Param.Byte[4];
500 holgerb 752
                                if(FromFlightCtrl.Param.Byte[5]) FromFC_LoadWP_List = FromFlightCtrl.Param.Byte[5];
513 killagreg 753
                                if(FromFlightCtrl.Param.Byte[6]) FromFC_Load_SinglePoint = FromFlightCtrl.Param.Byte[6];
754
                                if(FromFlightCtrl.Param.Byte[7]) FromFC_Save_SinglePoint = FromFlightCtrl.Param.Byte[7];
510 holgerb 755
                                CompassSetpoint = FromFlightCtrl.Param.sInt[4] * 10; // 8 & 9
756
                                CompassSetpointCorrected = (3600 + CompassSetpoint + FC.FromFC_CompassOffset + GeoMagDec) % 3600;
587 holgerb 757
                                FC.StatusFlags3 = FromFlightCtrl.Param.Byte[10];
518 holgerb 758
                                Parameter.SingleWpSpeed = FromFlightCtrl.Param.Byte[11];
419 holgerb 759
                                break;
202 killagreg 760
                        case SPI_FCCMD_STICK:
41 ingob 761
                                FC.StickGas     = FromFlightCtrl.Param.sByte[0];
762
                                FC.StickYaw     = FromFlightCtrl.Param.sByte[1];
763
                                FC.StickRoll    = FromFlightCtrl.Param.sByte[2];
764
                                FC.StickNick    = FromFlightCtrl.Param.sByte[3];
190 killagreg 765
                                FC.Poti[0]              = FromFlightCtrl.Param.Byte[4];
766
                                FC.Poti[1]              = FromFlightCtrl.Param.Byte[5];
767
                                FC.Poti[2]              = FromFlightCtrl.Param.Byte[6];
768
                                FC.Poti[3]              = FromFlightCtrl.Param.Byte[7];
769
                                FC.Poti[4]              = FromFlightCtrl.Param.Byte[8];
770
                                FC.Poti[5]              = FromFlightCtrl.Param.Byte[9];
771
                                FC.Poti[6]              = FromFlightCtrl.Param.Byte[10];
202 killagreg 772
                                FC.Poti[7]              = FromFlightCtrl.Param.Byte[11];
515 killagreg 773
                                CHK_POTI_MM(WaypointAcceleration,WaypointAccelerationSetting,0,255); // that could be a Poti-Value
41 ingob 774
                                break;
27 holgerb 775
 
202 killagreg 776
                        case SPI_FCCMD_MISC:
41 ingob 777
                                if(CompassCalState != FromFlightCtrl.Param.Byte[0])
778
                                {       // put only new CompassCalState into queue to send via I2C
489 killagreg 779
//                                      if(FromFlightCtrl.Param.Byte[0] == CompassCalState+1 || FromFlightCtrl.Param.Byte[0] == 0)
378 holgerb 780
                                         {
781
                                           CompassCalState = FromFlightCtrl.Param.Byte[0];
782
                                           Compass_SetCalState(CompassCalState);
783
                                         }
392 holgerb 784
//                                       else CompassCalState = 0;
41 ingob 785
                                }
587 holgerb 786
                                HoverGas = FromFlightCtrl.Param.Byte[1];
205 killagreg 787
                                NaviData.Variometer = (NaviData.Variometer + 2 * (FromFlightCtrl.Param.sInt[1] - NaviData.Altimeter)) / 2; // provisorisch
567 holgerb 788
                                FC.Altimeter = FromFlightCtrl.Param.sInt[1]; // in 5cm
789
                                if(!(SimulationFlags & SIMULATION_ACTIVE)) NaviData.SetpointAltitude = FromFlightCtrl.Param.sInt[2]; // in 5cm
587 holgerb 790
                                FC.Error[0]                     |= FromFlightCtrl.Param.Byte[6];
791
                                FC.Error[1]                     |= FromFlightCtrl.Param.Byte[7];
792
                                DebugOut.StatusGreen |= AMPEL_FC; // status of FC Present
793
                                DebugOut.StatusGreen |= AMPEL_BL; // status of BL Present
794
                                if(FC.Error[0] || FC.Error[1] /* || FC.Error[2] || FC.Error[3] || FC.Error[4]*/) DebugOut.StatusRed |= AMPEL_FC;
795
                                else DebugOut.StatusRed &= ~AMPEL_FC;
796
                                FC_I2C_ErrorConter = FromFlightCtrl.Param.Byte[8];
190 killagreg 797
                                FC.RC_Quality   = FromFlightCtrl.Param.Byte[9];
598 holgerb 798
                                if(FC.RC_Quality > 160) NaviData.RC_Quality = 200; else NaviData.RC_Quality = FC.RC_Quality;
461 holgerb 799
                                NC_Wait_for_LED = FromFlightCtrl.Param.Byte[10];
623 holgerb 800
//DebugOut.Analog[] = NC_Wait_for_LED;
206 killagreg 801
                                NaviData.Gas    = (FC.BAT_Voltage * (u32) FromFlightCtrl.Param.Byte[11]) / (u32) Parameter.LowVoltageWarning;
581 holgerb 802
                                if(LoggingGasCnt == 0) LoggingGasFilter = 0;
578 holgerb 803
                                LoggingGasFilter += (u32) FromFlightCtrl.Param.Byte[11];
804
                                LoggingGasCnt++;
41 ingob 805
                                break;
161 killagreg 806
 
587 holgerb 807
                        case SPI_FCCMD_VERSION:  // slow!
119 killagreg 808
                                FC_Version.Major                = FromFlightCtrl.Param.Byte[0];
809
                                FC_Version.Minor                = FromFlightCtrl.Param.Byte[1];
810
                                FC_Version.Patch                = FromFlightCtrl.Param.Byte[2];
811
                                FC_Version.Compatible   = FromFlightCtrl.Param.Byte[3];
812
                                FC_Version.Hardware             = FromFlightCtrl.Param.Byte[4];
587 holgerb 813
                                Parameter.FromFC_LandingSpeed = FromFlightCtrl.Param.Byte[5];
814
                                Parameter.ComingHomeAltitude = FromFlightCtrl.Param.Byte[6];
815
                                CHK_POTI_MM(Parameter.AutoPhotoAltitudes,FromFlightCtrl.Param.Byte[7],0,255);
816
                                UART_VersionInfo.BL_Firmware = FromFlightCtrl.Param.Byte[8];
598 holgerb 817
                                Parameter.ActiveSetting = FromFlightCtrl.Param.Byte[9];
587 holgerb 818
                                FlugMinutenGesamt = FromFlightCtrl.Param.Int[5]; // 10 & 11
819
                                break;
820
                        case SPI_FCCMD_NEUTRAL:  // slow!
821
                                FC.AdNeutralNick = FromFlightCtrl.Param.Int[0];
822
                                FC.AdNeutralRoll = FromFlightCtrl.Param.Int[1];
823
                                FC.AdNeutralYaw = FromFlightCtrl.Param.Int[2];
824
                                Parameter.Driftkomp = FromFlightCtrl.Param.Byte[6];    
825
                                Parameter.NaviPH_LoginTime = FromFlightCtrl.Param.Byte[7];
826
                                Parameter.ReceiverType = FromFlightCtrl.Param.Byte[8];
827
                                CHK_POTI_MM(Parameter.NaviGpsPLimit,FromFlightCtrl.Param.Byte[9],0,255);
828
                                CHK_POTI_MM(Parameter.NaviGpsILimit,FromFlightCtrl.Param.Byte[10],0,255);
829
                                CHK_POTI_MM(Parameter.NaviGpsDLimit,FromFlightCtrl.Param.Byte[11],0,255);
830
                                break;
831
                        case SPI_FCCMD_SLOW2:   // slow!
832
                                FC.BoatNeutralNick = FromFlightCtrl.Param.Int[0];  // 0 & 1
833
                                FC.BoatNeutralRoll = FromFlightCtrl.Param.Int[1];  // 2 & 3
834
                                FC.BoatNeutralYaw = FromFlightCtrl.Param.Int[2];   // 4 & 5
835
                                Parameter.CamOrientation = FromFlightCtrl.Param.Byte[6];
489 killagreg 836
                                if(FromFlightCtrl.Param.Byte[7] >= 68 && FromFlightCtrl.Param.Byte[7] <= 188)
475 holgerb 837
                                {
489 killagreg 838
                                        FC.FromFC_DisableDeclination = 1;
839
                                        FC.FromFC_CompassOffset = 10 * (s8) (FromFlightCtrl.Param.Byte[7] - 128);
840
                                        GeoMagDec = 0;
841
                                }
842
                                else
475 holgerb 843
                                {
489 killagreg 844
                                        FC.FromFC_DisableDeclination = 0;
845
                                        FC.FromFC_CompassOffset = 10 * FromFlightCtrl.Param.sByte[7];
475 holgerb 846
                                }
358 holgerb 847
                                Parameter.GlobalConfig = FromFlightCtrl.Param.Byte[8];
848
                                Parameter.ExtraConfig = FromFlightCtrl.Param.Byte[9];
320 holgerb 849
                                Parameter.OrientationAngle = FromFlightCtrl.Param.Byte[10];
350 holgerb 850
                                Parameter.GlobalConfig3 = FromFlightCtrl.Param.Byte[11];
41 ingob 851
                                break;
587 holgerb 852
                        case SPI_FCCMD_SLOW3:  // slow!
853
                                ServoParams.NickControl = FromFlightCtrl.Param.Byte[0];
854
                                ServoParams.RollControl = FromFlightCtrl.Param.Byte[1];
855
                                Parameter.DescendRange = FromFlightCtrl.Param.Byte[2];
856
                                Parameter.MaximumAltitude = FromFlightCtrl.Param.Byte[3];
588 holgerb 857
                                ServoParams.CompInvert = FromFlightCtrl.Param.Byte[4];
858
                                Parameter.HomeYawMode = ((ServoParams.CompInvert & 0x18) >> 3);
598 holgerb 859
                                NaviData_Home.LipoCellCount = FromFlightCtrl.Param.Byte[5];
860
                                NaviData_Volatile.ShutterCounter = FromFlightCtrl.Param.Int[3];   // 6 & 7
625 holgerb 861
                                LowVoltageLandingActive = FromFlightCtrl.Param.Byte[8];
644 holgerb 862
                                Parameter.FailSafeTime = FromFlightCtrl.Param.Byte[9];
611 holgerb 863
// DebugOut.Analog[] = NaviData_Volatile.ShutterCounter;
587 holgerb 864
// 8
865
// 9
866
// 10
867
// 11
573 holgerb 868
                                break;
587 holgerb 869
 
41 ingob 870
                        default:
204 killagreg 871
                                break;
41 ingob 872
                }
378 holgerb 873
                DebugOut.Analog[0] = FromFlightCtrl.AngleNick;
874
                DebugOut.Analog[1] = FromFlightCtrl.AngleRoll;
875
                DebugOut.Analog[2] = FromFlightCtrl.AccNick;
876
                DebugOut.Analog[3] = FromFlightCtrl.AccRoll;
877
                DebugOut.Analog[11] = FromFlightCtrl.GyroHeading/10;// in deg
41 ingob 878
                // every time we got new data from the FC via SPI call the navigation routine
215 killagreg 879
                // and update GPSStick that are returned to FC
378 holgerb 880
                SPI_RxBuffer_Request = 0;
215 killagreg 881
                GPS_Navigation(&GPSData, &(ToFlightCtrl.GPSStick));
255 killagreg 882
                ClearFCStatusFlags = 1;
180 killagreg 883
                if(counter)
884
                {
885
                        counter--;                                       // count down to enable servo
202 killagreg 886
                        if(!counter) TIMER2_Init();  // enable Servo Output
180 killagreg 887
                }
888
                timeout = SetDelay(80); // 80 ms, new data are send every 20 ms
1 ingob 889
 
79 killagreg 890
        }       // EOF if(SPI_RxBuffer_Request)
180 killagreg 891
        else // no new SPI data
202 killagreg 892
        {
180 killagreg 893
                if(CheckDelay(timeout) && (counter == 0))
894
                {
895
                        TIMER2_Deinit();  // disable Servo Output
896
                        counter = 50;     // reset counter for enabling Servo Output
897
                }
898
        }
1 ingob 899
}
900
 
41 ingob 901
//------------------------------------------------------
902
void SPI0_GetFlightCtrlVersion(void)
903
{
154 killagreg 904
        u32 timeout;
905
        u8 repeat;
78 holgerb 906
        u8 msg[64];
41 ingob 907
 
297 ingob 908
        UART1_PutString("\r\n Looking for FlightControl");
41 ingob 909
        FC_Version.Major = 0xFF;
910
        FC_Version.Minor = 0xFF;
911
        FC_Version.Patch = 0xFF;
912
        FC_Version.Compatible = 0xFF;
913
 
165 killagreg 914
        // polling FC version info
154 killagreg 915
        repeat = 0;
41 ingob 916
        do
917
        {
154 killagreg 918
                timeout = SetDelay(250);
919
                do
920
                {
921
                        SPI0_UpdateBuffer();
922
                        if (FC_Version.Major != 0xFF)  break;
923
                }while (!CheckDelay(timeout));
924
                UART1_PutString(".");
925
                repeat++;
644 holgerb 926
                FCCalibActive = 0;
180 killagreg 927
        }while((FC_Version.Major == 0xFF) && (repeat < 40)); // 40*250ms = 10s
41 ingob 928
        // if we got it
929
        if (FC_Version.Major != 0xFF)
930
        {
587 holgerb 931
                sprintf(msg, " FC V%d.%02d%c HW:%d.%d", FC_Version.Major, FC_Version.Minor, 'a'+FC_Version.Patch, FC_Version.Hardware/10,FC_Version.Hardware%10);
110 killagreg 932
                UART1_PutString(msg);
41 ingob 933
        }
242 killagreg 934
        else UART1_PutString("\n\r not found!");
41 ingob 935
}
936
 
489 killagreg 937
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
938
// + extended Current measurement -> 200 = 20A    201 = 21A    255 = 75A (20+55)
939
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
940
u16 BL3_Current(u8 who) // in 0,1A
941
{
942
        if(Motor[who].Current <= 200) return((u16) Motor[who].Current);
943
        else
944
        {
945
                if(Motor_Version[who] & MOTOR_STATE_BL30) return(200 + 10 * (u16) (Motor[who].Current - 200));
946
                else return((u16) Motor[who].Current);
947
        }
948
}
41 ingob 949