Subversion Repositories FlightCtrl

Rev

Details | Last modification | View Log | RSS feed

Rev Author Line No. Line
304 ingob 1
// ######################## SPI - FlightCtrl ###################
886 killagreg 2
#include <avr/io.h>
3
#include <avr/interrupt.h>
4
#include <string.h>
5
#include <stdlib.h>
6
#include "_Settings.h"
7
#include "spi.h"
8
#include "fc.h"
9
#include "rc.h"
10
#include "eeprom.h"
11
#include "uart.h"
12
#include "timer0.h"
936 killagreg 13
#include "analog.h"
304 ingob 14
 
886 killagreg 15
#define SPI_TXSYNCBYTE1 0xAA
16
#define SPI_TXSYNCBYTE2 0x83
17
#define SPI_RXSYNCBYTE1 0x81
18
#define SPI_RXSYNCBYTE2 0x55
304 ingob 19
 
886 killagreg 20
typedef enum
21
{
22
        SPI_SYNC1,
23
        SPI_SYNC2,
24
        SPI_DATA
25
} SPI_RXState_t;
708 ingob 26
 
606 ingob 27
 
886 killagreg 28
// data exchange packets to and From NaviCtrl
1078 killagreg 29
ToNaviCtrl_t                    ToNaviCtrl;
30
FromNaviCtrl_t                  FromNaviCtrl;
708 ingob 31
 
1078 killagreg 32
SPI_VersionInfo_t SPI_VersionInfo;
33
 
886 killagreg 34
// rx packet buffer
35
#define SPI_RXBUFFER_LEN sizeof(FromNaviCtrl)
36
uint8_t SPI_RxBuffer[SPI_RXBUFFER_LEN];
37
uint8_t SPI_RxBufferIndex = 0;
38
uint8_t SPI_RxBuffer_Request = 0;
720 ingob 39
 
886 killagreg 40
// tx packet buffer
41
#define SPI_TXBUFFER_LEN sizeof(ToNaviCtrl)
42
uint8_t *SPI_TxBuffer;
43
uint8_t SPI_TxBufferIndex = 0;
823 ingob 44
 
886 killagreg 45
uint8_t SPITransferCompleted, SPI_ChkSum;
46
uint8_t SPI_RxDataValid;
47
 
1078 killagreg 48
uint8_t SPI_CommandSequence[] = { SPI_CMD_USER, SPI_CMD_STICK, SPI_CMD_PARAMETER1, SPI_CMD_STICK, SPI_CMD_MISC, SPI_CMD_VERSION };
886 killagreg 49
uint8_t SPI_CommandCounter = 0;
50
 
51
/*********************************************/
52
/*  Initialize SPI interface to NaviCtrl     */
53
/*********************************************/
304 ingob 54
void SPI_MasterInit(void)
55
{
886 killagreg 56
        DDR_SPI |= (1<<DD_MOSI)|(1<<DD_SCK);    // Set MOSI and SCK output, all others input
57
        SLAVE_SELECT_DDR_PORT |= (1 << SPI_SLAVE_SELECT); // set Slave select port as output port
58
 
59
        SPCR = (1<<SPE)|(1<<MSTR)|(1<<SPR1)|(0<<SPR0)|(0<<SPIE);   // Enable SPI, Master, set clock rate fck/64
60
        SPSR = 0;//(1<<SPI2X);
61
 
62
        SLAVE_SELECT_PORT |=  (1 << SPI_SLAVE_SELECT); // Deselect Slave
63
 
64
        SPI_TxBuffer = (uint8_t *) &ToNaviCtrl; // set pointer to tx-buffer
65
        SPITransferCompleted = 1;
66
        // initialize data packet to NaviControl
67
        ToNaviCtrl.Sync1 = SPI_TXSYNCBYTE1;
68
        ToNaviCtrl.Sync2 = SPI_TXSYNCBYTE2;
69
 
70
        ToNaviCtrl.Command = SPI_CMD_USER;
911 killagreg 71
        ToNaviCtrl.IntegralNick = 0;
886 killagreg 72
        ToNaviCtrl.IntegralRoll = 0;
1078 killagreg 73
 
886 killagreg 74
        SPI_RxDataValid = 0;
1078 killagreg 75
 
76
        SPI_VersionInfo.Major = VERSION_MAJOR;
77
        SPI_VersionInfo.Minor = VERSION_MINOR;
78
        SPI_VersionInfo.Patch = VERSION_PATCH;
79
        SPI_VersionInfo.Compatible = NC_SPI_COMPATIBLE;
304 ingob 80
}
81
 
886 killagreg 82
 
83
/**********************************************************/
84
/*  Update Data transferd by the SPI from/to NaviCtrl     */
85
/**********************************************************/
86
void UpdateSPI_Buffer(void)
304 ingob 87
{
886 killagreg 88
        int16_t tmp;
89
        cli(); // stop all interrupts to avoid writing of new data during update of that packet.
823 ingob 90
 
886 killagreg 91
        // update content of packet to NaviCtrl
1097 killagreg 92
        ToNaviCtrl.IntegralNick = (int16_t) (IntegralNick / 130L);   // convert to multiple of 0.1°
93
        ToNaviCtrl.IntegralRoll  = (int16_t) (IntegralRoll  / 130L); // convert to multiple of 0.1°
886 killagreg 94
        ToNaviCtrl.GyroHeading = YawGyroHeading / YAW_GYRO_DEG_FACTOR;
911 killagreg 95
        ToNaviCtrl.GyroNick = Reading_GyroNick;
886 killagreg 96
        ToNaviCtrl.GyroRoll = Reading_GyroRoll;
97
        ToNaviCtrl.GyroYaw = Reading_GyroYaw;
911 killagreg 98
        ToNaviCtrl.AccNick = (int16_t) ACC_AMPLIFY * (NaviAccNick / NaviCntAcc);
1078 killagreg 99
        ToNaviCtrl.AccRoll = (int16_t) ACC_AMPLIFY * (NaviAccRoll / NaviCntAcc);
911 killagreg 100
        NaviCntAcc = 0; NaviAccNick = 0; NaviAccRoll = 0;
691 ingob 101
 
886 killagreg 102
        switch(ToNaviCtrl.Command)
103
        {
104
                case SPI_CMD_USER:
105
                        ToNaviCtrl.Param.Byte[0] = FCParam.UserParam1;
106
                        ToNaviCtrl.Param.Byte[1] = FCParam.UserParam2;
107
                        ToNaviCtrl.Param.Byte[2] = FCParam.UserParam3;
108
                        ToNaviCtrl.Param.Byte[3] = FCParam.UserParam4;
109
                        ToNaviCtrl.Param.Byte[4] = FCParam.UserParam5;
110
                        ToNaviCtrl.Param.Byte[5] = FCParam.UserParam6;
111
                        ToNaviCtrl.Param.Byte[6] = FCParam.UserParam7;
112
                        ToNaviCtrl.Param.Byte[7] = FCParam.UserParam8;
936 killagreg 113
                        ToNaviCtrl.Param.Byte[8] = MKFlags;
114
                        MKFlags &= ~(MKFLAG_CALIBRATE | MKFLAG_START); // calibrate and start are temporal states that are cleared immediately after transmitting
115
                        ToNaviCtrl.Param.Byte[9] = (uint8_t)UBat;
116
                        ToNaviCtrl.Param.Byte[10] = ParamSet.LowVoltageWarning;
117
                        ToNaviCtrl.Param.Byte[11] = GetActiveParamSet();
886 killagreg 118
                        break;
119
 
936 killagreg 120
                case SPI_CMD_PARAMETER1:
1078 killagreg 121
                        ToNaviCtrl.Param.Byte[0] = ParamSet.NaviGpsModeControl;     // Parameters for the Naviboard
122
                        ToNaviCtrl.Param.Byte[1] = ParamSet.NaviGpsGain;
123
                        ToNaviCtrl.Param.Byte[2] = ParamSet.NaviGpsP;
124
                        ToNaviCtrl.Param.Byte[3] = ParamSet.NaviGpsI;
125
                        ToNaviCtrl.Param.Byte[4] = ParamSet.NaviGpsD;
126
                        ToNaviCtrl.Param.Byte[5] = ParamSet.NaviGpsACC;
936 killagreg 127
                        ToNaviCtrl.Param.Byte[6] = ParamSet.NaviGpsMinSat;
128
                        ToNaviCtrl.Param.Byte[7] = ParamSet.NaviStickThreshold;
1078 killagreg 129
                        ToNaviCtrl.Param.Byte[8] = ParamSet.NaviOperatingRadius;
130
                        ToNaviCtrl.Param.Byte[9] = ParamSet.NaviWindCorrection;
131
            ToNaviCtrl.Param.Byte[10] = ParamSet.NaviSpeedCompensation;
132
            ToNaviCtrl.Param.Byte[11] = ParamSet.NaviAngleLimitation;
936 killagreg 133
                        break;
134
 
135
 
886 killagreg 136
                case SPI_CMD_STICK:
911 killagreg 137
                        tmp = PPM_in[ParamSet.ChannelAssignment[CH_GAS]];  if(tmp > 127) tmp = 127; else if(tmp < -128) tmp = -128;
886 killagreg 138
                        ToNaviCtrl.Param.Byte[0] = (int8_t) tmp;
139
                        tmp = PPM_in[ParamSet.ChannelAssignment[CH_YAW]]; if(tmp > 127) tmp = 127; else if(tmp < -128) tmp = -128;
140
                        ToNaviCtrl.Param.Byte[1] = (int8_t) tmp;
141
                        tmp = PPM_in[ParamSet.ChannelAssignment[CH_ROLL]]; if(tmp > 127) tmp = 127; else if(tmp < -128) tmp = -128;
142
                        ToNaviCtrl.Param.Byte[2] = (int8_t) tmp;
911 killagreg 143
                        tmp = PPM_in[ParamSet.ChannelAssignment[CH_NICK]]; if(tmp > 127) tmp = 127; else if(tmp < -128) tmp = -128;
886 killagreg 144
                        ToNaviCtrl.Param.Byte[3] = (int8_t) tmp;
145
                        ToNaviCtrl.Param.Byte[4] = (uint8_t) Poti1;
146
                        ToNaviCtrl.Param.Byte[5] = (uint8_t) Poti2;
147
                        ToNaviCtrl.Param.Byte[6] = (uint8_t) Poti3;
148
                        ToNaviCtrl.Param.Byte[7] = (uint8_t) Poti4;
149
                        ToNaviCtrl.Param.Byte[8] = (uint8_t) RC_Quality;
150
                        break;
151
 
1078 killagreg 152
                case SPI_CMD_MISC:
153
                        ToNaviCtrl.Param.Byte[0] = CompassCalState;
154
                        if(CompassCalState > 4)
155
                        { // jump from 5 to 0
156
                                CompassCalState  = 0;
886 killagreg 157
                        }
1078 killagreg 158
                        ToNaviCtrl.Param.Int[1] = ReadingHeight;
886 killagreg 159
                        break;
1078 killagreg 160
 
161
                case SPI_CMD_VERSION:
162
                        ToNaviCtrl.Param.Byte[0] = SPI_VersionInfo.Major;
163
                        ToNaviCtrl.Param.Byte[1] = SPI_VersionInfo.Minor;
164
                        ToNaviCtrl.Param.Byte[2] = SPI_VersionInfo.Patch;
165
                        ToNaviCtrl.Param.Byte[3] = SPI_VersionInfo.Compatible;
166
                        break;
167
 
168
                default:
169
                        break;
886 killagreg 170
        }
171
 
172
 
173
        sei(); // enable all interrupts
174
 
175
        // analyze content of packet from NaviCtrl if valid
176
        if (SPI_RxDataValid)
177
        {
178
                // update gps controls
911 killagreg 179
                if(abs(FromNaviCtrl.GPS_Nick) < 512 && abs(FromNaviCtrl.GPS_Roll) < 512 && (ParamSet.GlobalConfig & CFG_GPS_ACTIVE))
886 killagreg 180
                {
936 killagreg 181
                        GPS_Nick        = FromNaviCtrl.GPS_Nick;
182
                        GPS_Roll        = FromNaviCtrl.GPS_Roll;
886 killagreg 183
                }
184
                // update compass readings
185
                if(FromNaviCtrl.CompassHeading <= 360)
186
                {
187
                        CompassHeading = FromNaviCtrl.CompassHeading;
188
                }
189
                if(CompassHeading < 0) CompassOffCourse = 0;
190
                else CompassOffCourse = ((540 + CompassHeading - CompassCourse) % 360) - 180;
191
        // NaviCtrl wants to beep?
192
                if (FromNaviCtrl.BeepTime > BeepTime && !CompassCalState) BeepTime = FromNaviCtrl.BeepTime;
193
 
194
                switch (FromNaviCtrl.Command)
195
                {
1078 killagreg 196
                        case SPI_KALMAN:
1080 killagreg 197
                                FCParam.Kalman_K = FromNaviCtrl.Param.Byte[0];
198
                                FCParam.Kalman_MaxFusion = FromNaviCtrl.Param.Byte[1];
199
                                FCParam.Kalman_MaxDrift = FromNaviCtrl.Param.Byte[2];
1078 killagreg 200
                                break;
201
 
886 killagreg 202
                        default:
203
                                break;
204
                }
205
        }
206
        else // no valid data from NaviCtrl
207
        {
208
                // disable GPS control
911 killagreg 209
                GPS_Nick = 0;
886 killagreg 210
                GPS_Roll = 0;
211
        }
304 ingob 212
}
213
 
886 killagreg 214
 
215
 
216
/*********************************************/
217
/*  Start Transmission of packet to NaviCtrl */
218
/*********************************************/
219
void SPI_StartTransmitPacket(void)
304 ingob 220
{
708 ingob 221
 
886 killagreg 222
        if (!SPITransferCompleted) return; // return immediately if transfer is in progress
223
        else // transmission was completed
224
        {
225
                SLAVE_SELECT_PORT &=  ~(1 << SPI_SLAVE_SELECT);  // Select slave
691 ingob 226
 
886 killagreg 227
                // cyclic commands
228
                ToNaviCtrl.Command = SPI_CommandSequence[SPI_CommandCounter++];
229
                if (SPI_CommandCounter >= sizeof(SPI_CommandSequence)) SPI_CommandCounter = 0;
230
 
936 killagreg 231
                SPITransferCompleted = 0; // transfer is in progress
886 killagreg 232
                UpdateSPI_Buffer();    // update data in ToNaviCtrl
233
 
234
                SPI_TxBufferIndex = 1; //proceed with 2nd byte
235
 
236
                // -- Debug-Output ---
237
                //----
238
                asm volatile ("nop"); asm volatile ("nop"); asm volatile ("nop"); asm volatile ("nop");            asm volatile ("nop"); asm volatile ("nop"); asm volatile ("nop"); asm volatile ("nop");
239
                asm volatile ("nop"); asm volatile ("nop"); asm volatile ("nop"); asm volatile ("nop");            asm volatile ("nop"); asm volatile ("nop"); asm volatile ("nop"); asm volatile ("nop");
240
                asm volatile ("nop"); asm volatile ("nop"); asm volatile ("nop"); asm volatile ("nop");            asm volatile ("nop"); asm volatile ("nop"); asm volatile ("nop"); asm volatile ("nop");
241
                ToNaviCtrl.Chksum = ToNaviCtrl.Sync1; // init checksum
242
                SPDR = ToNaviCtrl.Sync1; // send first byte
243
        }
304 ingob 244
}
245
 
886 killagreg 246
//------------------------------------------------------
247
// This is the spi data transfer between FlightCtrl and NaviCtrl
248
// Every time this routine is called within the mainloop one byte of the packet to
249
// the NaviCtrl and one byte of the packet from the NaviCtrl is possible transfered
691 ingob 250
 
886 killagreg 251
void SPI_TransmitByte(void)
304 ingob 252
{
886 killagreg 253
        static SPI_RXState_t SPI_RXState = SPI_SYNC1;
254
        uint8_t rxdata;
255
        static uint8_t rxchksum;
823 ingob 256
 
886 killagreg 257
        if (SPITransferCompleted) return;  // return immediatly if transfer was completed
258
        if (!(SPSR & (1 << SPIF))) return; // return if no SPI-IRQ pending
259
        SendSPI = 4; // mait 4 * 0.102 ms for the next call of SPI_TransmitByte() in the main loop
823 ingob 260
 
886 killagreg 261
        SLAVE_SELECT_PORT |=  (1 << SPI_SLAVE_SELECT);   // DeselectSlave
823 ingob 262
 
886 killagreg 263
        rxdata = SPDR; // save spi data register
823 ingob 264
 
886 killagreg 265
        switch (SPI_RXState)
266
        {
267
                case SPI_SYNC1: // first sync byte
268
                        SPI_RxBufferIndex = 0; // set pointer to start of rx buffer
269
                        rxchksum = rxdata; // initialize checksum
270
                        if (rxdata == SPI_RXSYNCBYTE1 )
271
                        {       // 1st Syncbyte found
272
                                SPI_RXState  = SPI_SYNC2; // trigger to state for second sync byte
273
                        }
274
                        break;
823 ingob 275
 
886 killagreg 276
                case SPI_SYNC2: // second sync byte
277
                        if (rxdata == SPI_RXSYNCBYTE2)
278
                        {       // 2nd Syncbyte found
279
                                rxchksum += rxdata; // update checksum
280
                                SPI_RXState  = SPI_DATA;   // trigger to state for second sync byte
281
                        }
282
                        else // 2nd Syncbyte not found
283
                        {
284
                                SPI_RXState  = SPI_SYNC1; // jump back to 1st sync byte
285
                        }
286
                        break;
823 ingob 287
 
886 killagreg 288
                case SPI_DATA: // data bytes
289
                        SPI_RxBuffer[SPI_RxBufferIndex++] = rxdata;  // copy data byte to spi buffer
290
                        // if all bytes are received of a packet from the NaviCtrl
291
                        if (SPI_RxBufferIndex >= SPI_RXBUFFER_LEN)
292
                        {   // last byte transfered is the checksum of the packet
293
                                if (rxdata == rxchksum) // checksum matching?
294
                                {
295
                                        // copy SPI_RxBuffer -> FromFlightCtrl
296
                                        uint8_t *ptr = (uint8_t *)&FromNaviCtrl;
297
                                        cli();
298
                                        memcpy(ptr, (uint8_t *) SPI_RxBuffer, sizeof(FromNaviCtrl));
299
                                        sei();
300
                                        SPI_RxDataValid = 1;
1078 killagreg 301
                                        //DebugOut.Analog[18]++;
886 killagreg 302
                                }
303
                                else
304
                                {   // checksum does not match
1078 killagreg 305
                                        //DebugOut.Analog[17]++;
886 killagreg 306
                                        SPI_RxDataValid = 0; // reset valid flag
307
                                }
308
                                SPI_RXState  = SPI_SYNC1; // reset state sync
309
                        }
310
                        else // not all bytes transfered
311
                        {
312
                                rxchksum += rxdata; // update checksum
313
                        }
314
                        break;
315
        }// eof switch(SPI_RXState)
316
 
317
    // if still some bytes left for transmission to NaviCtrl
318
        if (SPI_TxBufferIndex < SPI_TXBUFFER_LEN)
319
        {
320
                SLAVE_SELECT_PORT &=  ~(1 << SPI_SLAVE_SELECT);  // SelectSlave
321
                asm volatile ("nop"); asm volatile ("nop"); asm volatile ("nop"); asm volatile ("nop"); asm volatile ("nop"); asm volatile ("nop"); asm volatile ("nop"); asm volatile ("nop");
322
                asm volatile ("nop"); asm volatile ("nop"); asm volatile ("nop"); asm volatile ("nop"); asm volatile ("nop"); asm volatile ("nop"); asm volatile ("nop"); asm volatile ("nop");
323
                asm volatile ("nop"); asm volatile ("nop"); asm volatile ("nop"); asm volatile ("nop"); asm volatile ("nop"); asm volatile ("nop"); asm volatile ("nop"); asm volatile ("nop");
324
 
325
                SPDR = SPI_TxBuffer[SPI_TxBufferIndex]; // transmit byte
326
                ToNaviCtrl.Chksum += SPI_TxBuffer[SPI_TxBufferIndex]; // update checksum for everey byte that was sent
327
                SPI_TxBufferIndex++;
328
        }
329
        else
330
        {
331
                //Transfer of all bytes of the packet to NaviCtrl completed
332
                SPITransferCompleted = 1;
333
        }
304 ingob 334
}
335
 
336
 
337