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