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