Subversion Repositories FlightCtrl

Rev

Rev 942 | Only display areas with differences | Ignore whitespace | Details | Blame | Last modification | View Log | RSS feed

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