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;
911 killagreg 70
        ToNaviCtrl.IntegralNick = 0;
886 killagreg 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
911 killagreg 85
        ToNaviCtrl.IntegralNick = (int16_t) (IntegralNick / 108);
886 killagreg 86
        ToNaviCtrl.IntegralRoll  = (int16_t) (IntegralRoll  / 108);
87
        ToNaviCtrl.GyroHeading = YawGyroHeading / YAW_GYRO_DEG_FACTOR;
911 killagreg 88
        ToNaviCtrl.GyroNick = Reading_GyroNick;
886 killagreg 89
        ToNaviCtrl.GyroRoll = Reading_GyroRoll;
90
        ToNaviCtrl.GyroYaw = Reading_GyroYaw;
911 killagreg 91
        ToNaviCtrl.AccNick = (int16_t) ACC_AMPLIFY * (NaviAccNick / NaviCntAcc);
886 killagreg 92
        ToNaviCtrl.AccRoll =  (int16_t) ACC_AMPLIFY * (NaviAccRoll / NaviCntAcc);
911 killagreg 93
        NaviCntAcc = 0; NaviAccNick = 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:
911 killagreg 109
                        tmp = PPM_in[ParamSet.ChannelAssignment[CH_GAS]];  if(tmp > 127) tmp = 127; else if(tmp < -128) tmp = -128;
886 killagreg 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;
911 killagreg 115
                        tmp = PPM_in[ParamSet.ChannelAssignment[CH_NICK]]; if(tmp > 127) tmp = 127; else if(tmp < -128) tmp = -128;
886 killagreg 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;
913 killagreg 122
                        ToNaviCtrl.Param.Byte[9] = (uint8_t) MotorsOn;
886 killagreg 123
                        break;
124
 
125
                case SPI_CMD_CAL_COMPASS:
126
                        if(CompassCalState > 5)
127
                        {
128
                                CompassCalState = 0;
129
                                ToNaviCtrl.Param.Byte[0] = 5;
130
                        }
131
                        else
132
                        {
133
                                ToNaviCtrl.Param.Byte[0] = CompassCalState;
134
                        }
135
                        break;
136
        }
137
 
138
 
139
        sei(); // enable all interrupts
140
 
141
        // analyze content of packet from NaviCtrl if valid
142
        if (SPI_RxDataValid)
143
        {
144
                // update gps controls
911 killagreg 145
                if(abs(FromNaviCtrl.GPS_Nick) < 512 && abs(FromNaviCtrl.GPS_Roll) < 512 && (ParamSet.GlobalConfig & CFG_GPS_ACTIVE))
886 killagreg 146
                {
911 killagreg 147
                        GPS_Nick = FromNaviCtrl.GPS_Nick;
886 killagreg 148
                        GPS_Roll  = FromNaviCtrl.GPS_Roll;
149
                }
150
                // update compass readings
151
                if(FromNaviCtrl.CompassHeading <= 360)
152
                {
153
                        CompassHeading = FromNaviCtrl.CompassHeading;
154
                }
155
                if(CompassHeading < 0) CompassOffCourse = 0;
156
                else CompassOffCourse = ((540 + CompassHeading - CompassCourse) % 360) - 180;
157
        // NaviCtrl wants to beep?
158
                if (FromNaviCtrl.BeepTime > BeepTime && !CompassCalState) BeepTime = FromNaviCtrl.BeepTime;
159
 
160
                switch (FromNaviCtrl.Command)
161
                {
162
                        case  SPI_CMD_OSD_DATA:
163
                                //                                ToFlightCtrl.Param.Byte[0] = OsdBar;
164
                                //                                ToFlightCtrl.Param.Int[1]  = Distance;
165
                                break;
166
 
167
                        case  SPI_CMD_GPS_POS:
168
                                //                                ToFlightCtrl.Param.Long[0]  = GPS_Data.Longitude;
169
                                //                                ToFlightCtrl.Param.Long[1]  = GPS_Data.Latitude;
170
                                break;
171
 
172
                        case  SPI_CMD_GPS_TARGET:
173
                                //                                ToFlightCtrl.Param.Long[0]  = GPS_Data.TargetLongitude;
174
                                //                                ToFlightCtrl.Param.Long[1]  = GPS_Data.TargetLatitude;
175
                                break;
176
 
177
                        default:
178
                                break;
179
                }
180
        }
181
        else // no valid data from NaviCtrl
182
        {
183
                // disable GPS control
911 killagreg 184
                GPS_Nick = 0;
886 killagreg 185
                GPS_Roll = 0;
186
        }
304 ingob 187
}
188
 
886 killagreg 189
 
190
 
191
/*********************************************/
192
/*  Start Transmission of packet to NaviCtrl */
193
/*********************************************/
194
void SPI_StartTransmitPacket(void)
304 ingob 195
{
708 ingob 196
 
886 killagreg 197
        if (!SPITransferCompleted) return; // return immediately if transfer is in progress
198
        else // transmission was completed
199
        {
200
                SLAVE_SELECT_PORT &=  ~(1 << SPI_SLAVE_SELECT);  // Select slave
691 ingob 201
 
886 killagreg 202
                // cyclic commands
203
                ToNaviCtrl.Command = SPI_CommandSequence[SPI_CommandCounter++];
204
                if (SPI_CommandCounter >= sizeof(SPI_CommandSequence)) SPI_CommandCounter = 0;
205
 
206
                SPITransferCompleted = 0; // tranfer is in progress
207
                UpdateSPI_Buffer();    // update data in ToNaviCtrl
208
 
209
                SPI_TxBufferIndex = 1; //proceed with 2nd byte
210
 
211
                // -- Debug-Output ---
212
                //----
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
                asm volatile ("nop"); asm volatile ("nop"); asm volatile ("nop"); asm volatile ("nop");            asm volatile ("nop"); asm volatile ("nop"); asm volatile ("nop"); asm volatile ("nop");
216
                ToNaviCtrl.Chksum = ToNaviCtrl.Sync1; // init checksum
217
                SPDR = ToNaviCtrl.Sync1; // send first byte
218
        }
304 ingob 219
}
220
 
886 killagreg 221
//------------------------------------------------------
222
// This is the spi data transfer between FlightCtrl and NaviCtrl
223
// Every time this routine is called within the mainloop one byte of the packet to
224
// the NaviCtrl and one byte of the packet from the NaviCtrl is possible transfered
691 ingob 225
 
886 killagreg 226
void SPI_TransmitByte(void)
304 ingob 227
{
886 killagreg 228
        static SPI_RXState_t SPI_RXState = SPI_SYNC1;
229
        uint8_t rxdata;
230
        static uint8_t rxchksum;
823 ingob 231
 
886 killagreg 232
        if (SPITransferCompleted) return;  // return immediatly if transfer was completed
233
        if (!(SPSR & (1 << SPIF))) return; // return if no SPI-IRQ pending
234
        SendSPI = 4; // mait 4 * 0.102 ms for the next call of SPI_TransmitByte() in the main loop
823 ingob 235
 
886 killagreg 236
        SLAVE_SELECT_PORT |=  (1 << SPI_SLAVE_SELECT);   // DeselectSlave
823 ingob 237
 
886 killagreg 238
        rxdata = SPDR; // save spi data register
823 ingob 239
 
886 killagreg 240
        switch (SPI_RXState)
241
        {
242
                case SPI_SYNC1: // first sync byte
243
                        SPI_RxBufferIndex = 0; // set pointer to start of rx buffer
244
                        rxchksum = rxdata; // initialize checksum
245
                        if (rxdata == SPI_RXSYNCBYTE1 )
246
                        {       // 1st Syncbyte found
247
                                SPI_RXState  = SPI_SYNC2; // trigger to state for second sync byte
248
                        }
249
                        break;
823 ingob 250
 
886 killagreg 251
                case SPI_SYNC2: // second sync byte
252
                        if (rxdata == SPI_RXSYNCBYTE2)
253
                        {       // 2nd Syncbyte found
254
                                rxchksum += rxdata; // update checksum
255
                                SPI_RXState  = SPI_DATA;   // trigger to state for second sync byte
256
                        }
257
                        else // 2nd Syncbyte not found
258
                        {
259
                                SPI_RXState  = SPI_SYNC1; // jump back to 1st sync byte
260
                        }
261
                        break;
823 ingob 262
 
886 killagreg 263
                case SPI_DATA: // data bytes
264
                        SPI_RxBuffer[SPI_RxBufferIndex++] = rxdata;  // copy data byte to spi buffer
265
                        // if all bytes are received of a packet from the NaviCtrl
266
                        if (SPI_RxBufferIndex >= SPI_RXBUFFER_LEN)
267
                        {   // last byte transfered is the checksum of the packet
268
                                if (rxdata == rxchksum) // checksum matching?
269
                                {
270
                                        // copy SPI_RxBuffer -> FromFlightCtrl
271
                                        uint8_t *ptr = (uint8_t *)&FromNaviCtrl;
272
                                        cli();
273
                                        memcpy(ptr, (uint8_t *) SPI_RxBuffer, sizeof(FromNaviCtrl));
274
                                        sei();
275
                                        SPI_RxDataValid = 1;
276
                                        DebugOut.Analog[18]++;
277
                                }
278
                                else
279
                                {   // checksum does not match
280
                                        DebugOut.Analog[17]++;
281
                                        SPI_RxDataValid = 0; // reset valid flag
282
                                }
283
                                SPI_RXState  = SPI_SYNC1; // reset state sync
284
                        }
285
                        else // not all bytes transfered
286
                        {
287
                                rxchksum += rxdata; // update checksum
288
                        }
289
                        break;
290
        }// eof switch(SPI_RXState)
291
 
292
    // if still some bytes left for transmission to NaviCtrl
293
        if (SPI_TxBufferIndex < SPI_TXBUFFER_LEN)
294
        {
295
                SLAVE_SELECT_PORT &=  ~(1 << SPI_SLAVE_SELECT);  // SelectSlave
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
                asm volatile ("nop"); asm volatile ("nop"); asm volatile ("nop"); asm volatile ("nop"); asm volatile ("nop"); asm volatile ("nop"); asm volatile ("nop"); asm volatile ("nop");
299
 
300
                SPDR = SPI_TxBuffer[SPI_TxBufferIndex]; // transmit byte
301
                ToNaviCtrl.Chksum += SPI_TxBuffer[SPI_TxBufferIndex]; // update checksum for everey byte that was sent
302
                SPI_TxBufferIndex++;
303
        }
304
        else
305
        {
306
                //Transfer of all bytes of the packet to NaviCtrl completed
307
                SPITransferCompleted = 1;
308
        }
304 ingob 309
}
310
 
311
 
886 killagreg 312
#endif //USE_SPI_COMMUNICATION
304 ingob 313
 
886 killagreg 314