Rev 1821 | Go to most recent revision | Only display areas with differences | Ignore whitespace | Details | Blame | Last modification | View Log | RSS feed
Rev 1821 | Rev 1868 | ||
---|---|---|---|
1 | // ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
1 | // ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
2 | // + Copyright (c) 04.2007 Holger Buss |
2 | // + Copyright (c) 04.2007 Holger Buss |
3 | // + Nur für den privaten Gebrauch |
3 | // + Nur für den privaten Gebrauch |
4 | // + www.MikroKopter.com |
4 | // + www.MikroKopter.com |
5 | // ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
5 | // ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
6 | // + Es gilt für das gesamte Projekt (Hardware, Software, Binärfiles, Sourcecode und Dokumentation), |
6 | // + Es gilt für das gesamte Projekt (Hardware, Software, Binärfiles, Sourcecode und Dokumentation), |
7 | // + dass eine Nutzung (auch auszugsweise) nur für den privaten (nicht-kommerziellen) Gebrauch zulässig ist. |
7 | // + dass eine Nutzung (auch auszugsweise) nur für den privaten (nicht-kommerziellen) Gebrauch zulässig ist. |
8 | // + Sollten direkte oder indirekte kommerzielle Absichten verfolgt werden, ist mit uns (info@mikrokopter.de) Kontakt |
8 | // + Sollten direkte oder indirekte kommerzielle Absichten verfolgt werden, ist mit uns (info@mikrokopter.de) Kontakt |
9 | // + bzgl. der Nutzungsbedingungen aufzunehmen. |
9 | // + bzgl. der Nutzungsbedingungen aufzunehmen. |
10 | // + Eine kommerzielle Nutzung ist z.B.Verkauf von MikroKoptern, Bestückung und Verkauf von Platinen oder Bausätzen, |
10 | // + Eine kommerzielle Nutzung ist z.B.Verkauf von MikroKoptern, Bestückung und Verkauf von Platinen oder Bausätzen, |
11 | // + Verkauf von Luftbildaufnahmen, usw. |
11 | // + Verkauf von Luftbildaufnahmen, usw. |
12 | // ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
12 | // ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
13 | // + Werden Teile des Quellcodes (mit oder ohne Modifikation) weiterverwendet oder veröffentlicht, |
13 | // + Werden Teile des Quellcodes (mit oder ohne Modifikation) weiterverwendet oder veröffentlicht, |
14 | // + unterliegen sie auch diesen Nutzungsbedingungen und diese Nutzungsbedingungen incl. Copyright müssen dann beiliegen |
14 | // + unterliegen sie auch diesen Nutzungsbedingungen und diese Nutzungsbedingungen incl. Copyright müssen dann beiliegen |
15 | // ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
15 | // ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
16 | // + Sollte die Software (auch auszugesweise) oder sonstige Informationen des MikroKopter-Projekts |
16 | // + Sollte die Software (auch auszugesweise) oder sonstige Informationen des MikroKopter-Projekts |
17 | // + auf anderen Webseiten oder sonstigen Medien veröffentlicht werden, muss unsere Webseite "http://www.mikrokopter.de" |
17 | // + auf anderen Webseiten oder sonstigen Medien veröffentlicht werden, muss unsere Webseite "http://www.mikrokopter.de" |
18 | // + eindeutig als Ursprung verlinkt werden |
18 | // + eindeutig als Ursprung verlinkt werden |
19 | // ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
19 | // ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
20 | // + Keine Gewähr auf Fehlerfreiheit, Vollständigkeit oder Funktion |
20 | // + Keine Gewähr auf Fehlerfreiheit, Vollständigkeit oder Funktion |
21 | // + Benutzung auf eigene Gefahr |
21 | // + Benutzung auf eigene Gefahr |
22 | // + Wir übernehmen keinerlei Haftung für direkte oder indirekte Personen- oder Sachschäden |
22 | // + Wir übernehmen keinerlei Haftung für direkte oder indirekte Personen- oder Sachschäden |
23 | // ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
23 | // ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
24 | // + Die Portierung der Software (oder Teile davon) auf andere Systeme (ausser der Hardware von www.mikrokopter.de) ist nur |
24 | // + Die Portierung der Software (oder Teile davon) auf andere Systeme (ausser der Hardware von www.mikrokopter.de) ist nur |
25 | // + mit unserer Zustimmung zulässig |
25 | // + mit unserer Zustimmung zulässig |
26 | // ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
26 | // ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
27 | // + Die Funktion printf_P() unterliegt ihrer eigenen Lizenz und ist hiervon nicht betroffen |
27 | // + Die Funktion printf_P() unterliegt ihrer eigenen Lizenz und ist hiervon nicht betroffen |
28 | // ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
28 | // ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
29 | // + Redistributions of source code (with or without modifications) must retain the above copyright notice, |
29 | // + Redistributions of source code (with or without modifications) must retain the above copyright notice, |
30 | // + this list of conditions and the following disclaimer. |
30 | // + this list of conditions and the following disclaimer. |
31 | // + * Neither the name of the copyright holders nor the names of contributors may be used to endorse or promote products derived |
31 | // + * Neither the name of the copyright holders nor the names of contributors may be used to endorse or promote products derived |
32 | // + from this software without specific prior written permission. |
32 | // + from this software without specific prior written permission. |
33 | // + * The use of this project (hardware, software, binary files, sources and documentation) is only permittet |
33 | // + * The use of this project (hardware, software, binary files, sources and documentation) is only permittet |
34 | // + for non-commercial use (directly or indirectly) |
34 | // + for non-commercial use (directly or indirectly) |
35 | // + Commercial use (for excample: selling of MikroKopters, selling of PCBs, assembly, ...) is only permitted |
35 | // + Commercial use (for example: selling of MikroKopters, selling of PCBs, assembly, ...) is only permitted |
36 | // + with our written permission |
36 | // + with our written permission |
37 | // + * If sources or documentations are redistributet on other webpages, out webpage (http://www.MikroKopter.de) must be |
37 | // + * If sources or documentations are redistributet on other webpages, out webpage (http://www.MikroKopter.de) must be |
38 | // + clearly linked as origin |
38 | // + clearly linked as origin |
39 | // + * porting to systems other than hardware from www.mikrokopter.de is not allowed |
39 | // + * porting to systems other than hardware from www.mikrokopter.de is not allowed |
40 | // + THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" |
40 | // + THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" |
41 | // + AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE |
41 | // + AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE |
42 | // + IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE |
42 | // + IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE |
43 | // + ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE |
43 | // + ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE |
44 | // + LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR |
44 | // + LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR |
45 | // + CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF |
45 | // + CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF |
46 | // + SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS |
46 | // + SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS |
47 | // + INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN// + CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) |
47 | // + INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN// + CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) |
48 | // + ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE |
48 | // + ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE |
49 | // + POSSIBILITY OF SUCH DAMAGE. |
49 | // + POSSIBILITY OF SUCH DAMAGE. |
50 | // ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
50 | // ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
51 | #include <avr/io.h> |
51 | #include <avr/io.h> |
52 | #include <avr/interrupt.h> |
52 | #include <avr/interrupt.h> |
53 | #include <string.h> |
53 | #include <string.h> |
54 | #include <stdlib.h> |
54 | #include <stdlib.h> |
55 | #include "spi.h" |
55 | #include "spi.h" |
56 | #include "rc.h" |
56 | #include "rc.h" |
57 | #include "eeprom.h" |
57 | #include "eeprom.h" |
58 | #include "uart0.h" |
58 | #include "uart0.h" |
59 | #include "timer0.h" |
59 | #include "timer0.h" |
60 | #include "analog.h" |
60 | #include "analog.h" |
61 | #include "attitude.h" |
61 | #include "attitude.h" |
62 | #include "GPSControl.h" |
62 | #include "GPSControl.h" |
63 | #include "flight.h" |
63 | #include "flight.h" |
64 | 64 | ||
65 | //----------------------------------------- |
65 | //----------------------------------------- |
66 | #define DDR_SPI DDRB |
66 | #define DDR_SPI DDRB |
67 | #define DD_SS PB4 |
67 | #define DD_SS PB4 |
68 | #define DD_SCK PB7 |
68 | #define DD_SCK PB7 |
69 | #define DD_MOSI PB5 |
69 | #define DD_MOSI PB5 |
70 | #define DD_MISO PB6 |
70 | #define DD_MISO PB6 |
71 | 71 | ||
72 | // for compatibility reasons gcc3.x <-> gcc4.x |
72 | // for compatibility reasons gcc3.x <-> gcc4.x |
73 | #ifndef SPCR |
73 | #ifndef SPCR |
74 | #define SPCR SPCR0 |
74 | #define SPCR SPCR0 |
75 | #endif |
75 | #endif |
76 | #ifndef SPIE |
76 | #ifndef SPIE |
77 | #define SPIE SPIE0 |
77 | #define SPIE SPIE0 |
78 | #endif |
78 | #endif |
79 | #ifndef SPE |
79 | #ifndef SPE |
80 | #define SPE SPE0 |
80 | #define SPE SPE0 |
81 | #endif |
81 | #endif |
82 | #ifndef DORD |
82 | #ifndef DORD |
83 | #define DORD DORD0 |
83 | #define DORD DORD0 |
84 | #endif |
84 | #endif |
85 | #ifndef MSTR |
85 | #ifndef MSTR |
86 | #define MSTR MSTR0 |
86 | #define MSTR MSTR0 |
87 | #endif |
87 | #endif |
88 | #ifndef CPOL |
88 | #ifndef CPOL |
89 | #define CPOL CPOL0 |
89 | #define CPOL CPOL0 |
90 | #endif |
90 | #endif |
91 | #ifndef CPHA |
91 | #ifndef CPHA |
92 | #define CPHA CPHA0 |
92 | #define CPHA CPHA0 |
93 | #endif |
93 | #endif |
94 | #ifndef SPR1 |
94 | #ifndef SPR1 |
95 | #define SPR1 SPR01 |
95 | #define SPR1 SPR01 |
96 | #endif |
96 | #endif |
97 | #ifndef SPR0 |
97 | #ifndef SPR0 |
98 | #define SPR0 SPR00 |
98 | #define SPR0 SPR00 |
99 | #endif |
99 | #endif |
100 | 100 | ||
101 | #ifndef SPDR |
101 | #ifndef SPDR |
102 | #define SPDR SPDR0 |
102 | #define SPDR SPDR0 |
103 | #endif |
103 | #endif |
104 | 104 | ||
105 | #ifndef SPSR |
105 | #ifndef SPSR |
106 | #define SPSR SPSR0 |
106 | #define SPSR SPSR0 |
107 | #endif |
107 | #endif |
108 | #ifndef SPIF |
108 | #ifndef SPIF |
109 | #define SPIF SPIF0 |
109 | #define SPIF SPIF0 |
110 | #endif |
110 | #endif |
111 | #ifndef WCOL |
111 | #ifndef WCOL |
112 | #define WCOL WCOL0 |
112 | #define WCOL WCOL0 |
113 | #endif |
113 | #endif |
114 | #ifndef SPI2X |
114 | #ifndef SPI2X |
115 | #define SPI2X SPI2X0 |
115 | #define SPI2X SPI2X0 |
116 | #endif |
116 | #endif |
117 | // ------------------------- |
117 | // ------------------------- |
118 | 118 | ||
119 | #define SLAVE_SELECT_DDR_PORT DDRC |
119 | #define SLAVE_SELECT_DDR_PORT DDRC |
120 | #define SLAVE_SELECT_PORT PORTC |
120 | #define SLAVE_SELECT_PORT PORTC |
121 | #define SPI_SLAVE_SELECT PC5 |
121 | #define SPI_SLAVE_SELECT PC5 |
122 | 122 | ||
123 | #define SPI_TXSYNCBYTE1 0xAA |
123 | #define SPI_TXSYNCBYTE1 0xAA |
124 | #define SPI_TXSYNCBYTE2 0x83 |
124 | #define SPI_TXSYNCBYTE2 0x83 |
125 | #define SPI_RXSYNCBYTE1 0x81 |
125 | #define SPI_RXSYNCBYTE1 0x81 |
126 | #define SPI_RXSYNCBYTE2 0x55 |
126 | #define SPI_RXSYNCBYTE2 0x55 |
127 | 127 | ||
128 | typedef enum { |
128 | typedef enum { |
129 | SPI_SYNC1, SPI_SYNC2, SPI_DATA |
129 | SPI_SYNC1, SPI_SYNC2, SPI_DATA |
130 | } SPI_RXState_t; |
130 | } SPI_RXState_t; |
131 | 131 | ||
132 | // data exchange packets to and From NaviCtrl |
132 | // data exchange packets to and From NaviCtrl |
133 | ToNaviCtrl_t toNaviCtrl; |
133 | ToNaviCtrl_t toNaviCtrl; |
134 | FromNaviCtrl_t fromNaviCtrl; |
134 | FromNaviCtrl_t fromNaviCtrl; |
135 | SPI_VersionInfo_t SPI_VersionInfo; |
135 | SPI_VersionInfo_t SPI_VersionInfo; |
136 | 136 | ||
137 | // rx packet buffer |
137 | // rx packet buffer |
138 | #define SPI_RXBUFFER_LEN sizeof(fromNaviCtrl) |
138 | #define SPI_RXBUFFER_LEN sizeof(fromNaviCtrl) |
139 | uint8_t SPI_RxBuffer[SPI_RXBUFFER_LEN]; |
139 | uint8_t SPI_RxBuffer[SPI_RXBUFFER_LEN]; |
140 | uint8_t SPI_RxBufferIndex = 0; |
140 | uint8_t SPI_RxBufferIndex = 0; |
141 | uint8_t SPI_RxBuffer_Request = 0; |
141 | uint8_t SPI_RxBuffer_Request = 0; |
142 | 142 | ||
143 | // tx packet buffer |
143 | // tx packet buffer |
144 | #define SPI_TXBUFFER_LEN sizeof(toNaviCtrl) |
144 | #define SPI_TXBUFFER_LEN sizeof(toNaviCtrl) |
145 | uint8_t *SPI_TxBuffer; |
145 | uint8_t *SPI_TxBuffer; |
146 | uint8_t SPI_TxBufferIndex = 0; |
146 | uint8_t SPI_TxBufferIndex = 0; |
147 | 147 | ||
148 | uint8_t SPITransferCompleted, SPI_ChkSum; |
148 | uint8_t SPITransferCompleted, SPI_ChkSum; |
149 | uint8_t SPI_RxDataValid = 0; |
149 | uint8_t SPI_RxDataValid = 0; |
150 | uint8_t NCDataOkay = 0; |
150 | uint8_t NCDataOkay = 0; |
151 | uint8_t NCSerialDataOkay = 0; |
151 | uint8_t NCSerialDataOkay = 0; |
152 | 152 | ||
153 | uint8_t SPI_CommandSequence[] = { SPI_CMD_USER, SPI_CMD_STICK, |
153 | uint8_t SPI_CommandSequence[] = { SPI_CMD_USER, SPI_CMD_STICK, |
154 | SPI_CMD_PARAMETER1, SPI_CMD_STICK, SPI_CMD_MISC, SPI_CMD_VERSION }; |
154 | SPI_CMD_PARAMETER1, SPI_CMD_STICK, SPI_CMD_MISC, SPI_CMD_VERSION }; |
155 | uint8_t SPI_CommandCounter = 0; |
155 | uint8_t SPI_CommandCounter = 0; |
156 | 156 | ||
157 | /*********************************************/ |
157 | /*********************************************/ |
158 | /* Initialize SPI interface to NaviCtrl */ |
158 | /* Initialize SPI interface to NaviCtrl */ |
159 | /*********************************************/ |
159 | /*********************************************/ |
160 | void SPI_MasterInit(void) { |
160 | void SPI_MasterInit(void) { |
161 | DDR_SPI |= (1 << DD_MOSI) | (1 << DD_SCK); // Set MOSI and SCK output, all others input |
161 | DDR_SPI |= (1 << DD_MOSI) | (1 << DD_SCK); // Set MOSI and SCK output, all others input |
162 | SLAVE_SELECT_DDR_PORT |= (1 << SPI_SLAVE_SELECT); // set Slave select port as output port |
162 | SLAVE_SELECT_DDR_PORT |= (1 << SPI_SLAVE_SELECT); // set Slave select port as output port |
163 | 163 | ||
164 | SPCR = (1 << SPE) | (1 << MSTR) | (1 << SPR1) | (0 << SPR0) | (0 << SPIE); // Enable SPI, Master, set clock rate fck/64 |
164 | SPCR = (1 << SPE) | (1 << MSTR) | (1 << SPR1) | (0 << SPR0) | (0 << SPIE); // Enable SPI, Master, set clock rate fck/64 |
165 | SPSR = 0;//(1<<SPI2X); |
165 | SPSR = 0;//(1<<SPI2X); |
166 | 166 | ||
167 | SLAVE_SELECT_PORT |= (1 << SPI_SLAVE_SELECT); // Deselect Slave |
167 | SLAVE_SELECT_PORT |= (1 << SPI_SLAVE_SELECT); // Deselect Slave |
168 | 168 | ||
169 | SPI_TxBuffer = (uint8_t *) &toNaviCtrl; // set pointer to tx-buffer |
169 | SPI_TxBuffer = (uint8_t *) &toNaviCtrl; // set pointer to tx-buffer |
170 | SPITransferCompleted = 1; |
170 | SPITransferCompleted = 1; |
171 | // initialize data packet to NaviControl |
171 | // initialize data packet to NaviControl |
172 | toNaviCtrl.Sync1 = SPI_TXSYNCBYTE1; |
172 | toNaviCtrl.Sync1 = SPI_TXSYNCBYTE1; |
173 | toNaviCtrl.Sync2 = SPI_TXSYNCBYTE2; |
173 | toNaviCtrl.Sync2 = SPI_TXSYNCBYTE2; |
174 | 174 | ||
175 | toNaviCtrl.Command = SPI_CMD_USER; |
175 | toNaviCtrl.Command = SPI_CMD_USER; |
176 | toNaviCtrl.IntegralPitch = 0; |
176 | toNaviCtrl.IntegralPitch = 0; |
177 | toNaviCtrl.IntegralRoll = 0; |
177 | toNaviCtrl.IntegralRoll = 0; |
178 | NCSerialDataOkay = 0; |
178 | NCSerialDataOkay = 0; |
179 | NCDataOkay = 0; |
179 | NCDataOkay = 0; |
180 | 180 | ||
181 | SPI_RxDataValid = 0; |
181 | SPI_RxDataValid = 0; |
182 | 182 | ||
183 | SPI_VersionInfo.Major = VERSION_MAJOR; |
183 | SPI_VersionInfo.Major = VERSION_MAJOR; |
184 | SPI_VersionInfo.Minor = VERSION_MINOR; |
184 | SPI_VersionInfo.Minor = VERSION_MINOR; |
185 | SPI_VersionInfo.Patch = VERSION_PATCH; |
185 | SPI_VersionInfo.Patch = VERSION_PATCH; |
186 | SPI_VersionInfo.Compatible = NC_SPI_COMPATIBLE; |
186 | SPI_VersionInfo.Compatible = NC_SPI_COMPATIBLE; |
187 | } |
187 | } |
188 | 188 | ||
189 | /**********************************************************/ |
189 | /**********************************************************/ |
190 | /* Update Data transferd by the SPI from/to NaviCtrl */ |
190 | /* Update Data transferd by the SPI from/to NaviCtrl */ |
191 | /**********************************************************/ |
191 | /**********************************************************/ |
192 | void UpdateSPI_Buffer(void) { |
192 | void UpdateSPI_Buffer(void) { |
193 | uint8_t i; |
193 | uint8_t i; |
194 | int16_t tmp; |
194 | int16_t tmp; |
195 | cli(); |
195 | cli(); |
196 | // stop all interrupts to avoid writing of new data during update of that packet. |
196 | // stop all interrupts to avoid writing of new data during update of that packet. |
197 | 197 | ||
198 | // update content of packet to NaviCtrl |
198 | // update content of packet to NaviCtrl |
199 | toNaviCtrl.IntegralPitch = (int16_t) ((10 * angle[PITCH]) |
199 | toNaviCtrl.IntegralPitch = (int16_t) ((10 * angle[PITCH]) |
200 | / GYRO_DEG_FACTOR_PITCHROLL); // convert to multiple of 0.1° |
200 | / GYRO_DEG_FACTOR_PITCHROLL); // convert to multiple of 0.1° |
201 | toNaviCtrl.IntegralRoll = (int16_t) ((10 * angle[ROLL]) |
201 | toNaviCtrl.IntegralRoll = (int16_t) ((10 * angle[ROLL]) |
202 | / GYRO_DEG_FACTOR_PITCHROLL); // convert to multiple of 0.1° |
202 | / GYRO_DEG_FACTOR_PITCHROLL); // convert to multiple of 0.1° |
203 | toNaviCtrl.GyroHeading = (int16_t) ((10 * yawGyroHeading) |
203 | toNaviCtrl.GyroHeading = (int16_t) ((10 * yawGyroHeading) |
204 | / GYRO_DEG_FACTOR_YAW); // convert to multiple of 0.1° |
204 | / GYRO_DEG_FACTOR_YAW); // convert to multiple of 0.1° |
205 | toNaviCtrl.GyroPitch = rate_ATT[PITCH]; |
205 | toNaviCtrl.GyroPitch = rate_ATT[PITCH]; |
206 | toNaviCtrl.GyroRoll = rate_ATT[ROLL]; |
206 | toNaviCtrl.GyroRoll = rate_ATT[ROLL]; |
207 | toNaviCtrl.GyroYaw = yawRate; |
207 | toNaviCtrl.GyroYaw = yawRate; |
208 | toNaviCtrl.AccPitch = (10 * getAngleEstimateFromAcc(PITCH)) |
208 | toNaviCtrl.AccPitch = (10 * getAngleEstimateFromAcc(PITCH)) |
209 | / GYRO_DEG_FACTOR_PITCHROLL; // convert to multiple of 0.1° |
209 | / GYRO_DEG_FACTOR_PITCHROLL; // convert to multiple of 0.1° |
210 | toNaviCtrl.AccRoll = (10 * getAngleEstimateFromAcc(ROLL)) |
210 | toNaviCtrl.AccRoll = (10 * getAngleEstimateFromAcc(ROLL)) |
211 | / GYRO_DEG_FACTOR_PITCHROLL; // convert to multiple of 0.1° |
211 | / GYRO_DEG_FACTOR_PITCHROLL; // convert to multiple of 0.1° |
212 | 212 | ||
213 | 213 | ||
214 | // TODO: What are these little bastards? |
214 | // TODO: What are these little bastards? |
215 | 215 | ||
216 | averageAcc[PITCH] = averageAcc[ROLL] = averageAccCount = 0; |
216 | averageAcc[PITCH] = averageAcc[ROLL] = averageAccCount = 0; |
217 | 217 | ||
218 | switch (toNaviCtrl.Command) { |
218 | switch (toNaviCtrl.Command) { |
219 | case SPI_CMD_USER: |
219 | case SPI_CMD_USER: |
220 | for (i = 0; i < sizeof(dynamicParams.UserParams); i++) { |
220 | for (i = 0; i < sizeof(dynamicParams.UserParams); i++) { |
221 | toNaviCtrl.Param.Byte[i] = dynamicParams.UserParams[i]; |
221 | toNaviCtrl.Param.Byte[i] = dynamicParams.UserParams[i]; |
222 | } |
222 | } |
223 | toNaviCtrl.Param.Byte[8] = MKFlags; |
223 | toNaviCtrl.Param.Byte[8] = MKFlags; |
224 | MKFlags &= ~(MKFLAG_CALIBRATE | MKFLAG_START); // calibrate and start are temporal states that are cleared immediately after transmitting |
224 | MKFlags &= ~(MKFLAG_CALIBRATE | MKFLAG_START); // calibrate and start are temporal states that are cleared immediately after transmitting |
225 | toNaviCtrl.Param.Byte[9] = (uint8_t) UBat; |
225 | toNaviCtrl.Param.Byte[9] = (uint8_t) UBat; |
226 | toNaviCtrl.Param.Byte[10] = staticParams.LowVoltageWarning; |
226 | toNaviCtrl.Param.Byte[10] = staticParams.LowVoltageWarning; |
227 | toNaviCtrl.Param.Byte[11] = getActiveParamSet(); |
227 | toNaviCtrl.Param.Byte[11] = getActiveParamSet(); |
228 | break; |
228 | break; |
229 | 229 | ||
230 | case SPI_CMD_PARAMETER1: |
230 | case SPI_CMD_PARAMETER1: |
231 | toNaviCtrl.Param.Byte[0] = staticParams.NaviGpsModeControl; // Parameters for the Naviboard |
231 | toNaviCtrl.Param.Byte[0] = staticParams.NaviGpsModeControl; // Parameters for the Naviboard |
232 | toNaviCtrl.Param.Byte[1] = staticParams.NaviGpsGain; |
232 | toNaviCtrl.Param.Byte[1] = staticParams.NaviGpsGain; |
233 | toNaviCtrl.Param.Byte[2] = staticParams.NaviGpsP; |
233 | toNaviCtrl.Param.Byte[2] = staticParams.NaviGpsP; |
234 | toNaviCtrl.Param.Byte[3] = staticParams.NaviGpsI; |
234 | toNaviCtrl.Param.Byte[3] = staticParams.NaviGpsI; |
235 | toNaviCtrl.Param.Byte[4] = staticParams.NaviGpsD; |
235 | toNaviCtrl.Param.Byte[4] = staticParams.NaviGpsD; |
236 | toNaviCtrl.Param.Byte[5] = staticParams.NaviGpsACC; |
236 | toNaviCtrl.Param.Byte[5] = staticParams.NaviGpsACC; |
237 | toNaviCtrl.Param.Byte[6] = staticParams.NaviGpsMinSat; |
237 | toNaviCtrl.Param.Byte[6] = staticParams.NaviGpsMinSat; |
238 | toNaviCtrl.Param.Byte[7] = staticParams.NaviStickThreshold; |
238 | toNaviCtrl.Param.Byte[7] = staticParams.NaviStickThreshold; |
239 | toNaviCtrl.Param.Byte[8] = staticParams.NaviOperatingRadius; |
239 | toNaviCtrl.Param.Byte[8] = staticParams.NaviOperatingRadius; |
240 | toNaviCtrl.Param.Byte[9] = staticParams.NaviWindCorrection; |
240 | toNaviCtrl.Param.Byte[9] = staticParams.NaviWindCorrection; |
241 | toNaviCtrl.Param.Byte[10] = staticParams.NaviSpeedCompensation; |
241 | toNaviCtrl.Param.Byte[10] = staticParams.NaviSpeedCompensation; |
242 | toNaviCtrl.Param.Byte[11] = staticParams.NaviAngleLimitation; |
242 | toNaviCtrl.Param.Byte[11] = staticParams.NaviAngleLimitation; |
243 | break; |
243 | break; |
244 | 244 | ||
245 | case SPI_CMD_STICK: |
245 | case SPI_CMD_STICK: |
246 | tmp = PPM_in[staticParams.ChannelAssignment[CH_THROTTLE]]; |
246 | tmp = PPM_in[staticParams.ChannelAssignment[CH_THROTTLE]]; |
247 | if (tmp > 127) |
247 | if (tmp > 127) |
248 | tmp = 127; |
248 | tmp = 127; |
249 | else if (tmp < -128) |
249 | else if (tmp < -128) |
250 | tmp = -128; |
250 | tmp = -128; |
251 | toNaviCtrl.Param.Byte[0] = (int8_t) tmp; |
251 | toNaviCtrl.Param.Byte[0] = (int8_t) tmp; |
252 | tmp = PPM_in[staticParams.ChannelAssignment[CH_YAW]]; |
252 | tmp = PPM_in[staticParams.ChannelAssignment[CH_YAW]]; |
253 | if (tmp > 127) |
253 | if (tmp > 127) |
254 | tmp = 127; |
254 | tmp = 127; |
255 | else if (tmp < -128) |
255 | else if (tmp < -128) |
256 | tmp = -128; |
256 | tmp = -128; |
257 | toNaviCtrl.Param.Byte[1] = (int8_t) tmp; |
257 | toNaviCtrl.Param.Byte[1] = (int8_t) tmp; |
258 | tmp = PPM_in[staticParams.ChannelAssignment[CH_ROLL]]; |
258 | tmp = PPM_in[staticParams.ChannelAssignment[CH_ROLL]]; |
259 | if (tmp > 127) |
259 | if (tmp > 127) |
260 | tmp = 127; |
260 | tmp = 127; |
261 | else if (tmp < -128) |
261 | else if (tmp < -128) |
262 | tmp = -128; |
262 | tmp = -128; |
263 | toNaviCtrl.Param.Byte[2] = (int8_t) tmp; |
263 | toNaviCtrl.Param.Byte[2] = (int8_t) tmp; |
264 | tmp = PPM_in[staticParams.ChannelAssignment[CH_PITCH]]; |
264 | tmp = PPM_in[staticParams.ChannelAssignment[CH_PITCH]]; |
265 | if (tmp > 127) |
265 | if (tmp > 127) |
266 | tmp = 127; |
266 | tmp = 127; |
267 | else if (tmp < -128) |
267 | else if (tmp < -128) |
268 | tmp = -128; |
268 | tmp = -128; |
269 | toNaviCtrl.Param.Byte[3] = (int8_t) tmp; |
269 | toNaviCtrl.Param.Byte[3] = (int8_t) tmp; |
270 | toNaviCtrl.Param.Byte[4] = (uint8_t) variables[0]; |
270 | toNaviCtrl.Param.Byte[4] = (uint8_t) variables[0]; |
271 | toNaviCtrl.Param.Byte[5] = (uint8_t) variables[1]; |
271 | toNaviCtrl.Param.Byte[5] = (uint8_t) variables[1]; |
272 | toNaviCtrl.Param.Byte[6] = (uint8_t) variables[2]; |
272 | toNaviCtrl.Param.Byte[6] = (uint8_t) variables[2]; |
273 | toNaviCtrl.Param.Byte[7] = (uint8_t) variables[3]; |
273 | toNaviCtrl.Param.Byte[7] = (uint8_t) variables[3]; |
274 | toNaviCtrl.Param.Byte[8] = (uint8_t) RC_Quality; |
274 | toNaviCtrl.Param.Byte[8] = (uint8_t) RC_Quality; |
275 | break; |
275 | break; |
276 | 276 | ||
277 | case SPI_CMD_MISC: |
277 | case SPI_CMD_MISC: |
278 | toNaviCtrl.Param.Byte[0] = compassCalState; |
278 | toNaviCtrl.Param.Byte[0] = compassCalState; |
279 | if (compassCalState > 4) { // jump from 5 to 0 |
279 | if (compassCalState > 4) { // jump from 5 to 0 |
280 | compassCalState = 0; |
280 | compassCalState = 0; |
281 | } |
281 | } |
282 | toNaviCtrl.Param.Byte[1] = staticParams.NaviPHLoginTime; |
282 | toNaviCtrl.Param.Byte[1] = staticParams.NaviPHLoginTime; |
283 | // TODO: Height and in the correct scaling... |
283 | // TODO: Height and in the correct scaling... |
284 | toNaviCtrl.Param.Int[1] = 0; //readingHeight; // at address of Byte 2 and 3 |
284 | toNaviCtrl.Param.Int[1] = 0; //readingHeight; // at address of Byte 2 and 3 |
285 | toNaviCtrl.Param.Byte[4] = staticParams.NaviGpsPLimit; |
285 | toNaviCtrl.Param.Byte[4] = staticParams.NaviGpsPLimit; |
286 | toNaviCtrl.Param.Byte[5] = staticParams.NaviGpsILimit; |
286 | toNaviCtrl.Param.Byte[5] = staticParams.NaviGpsILimit; |
287 | toNaviCtrl.Param.Byte[6] = staticParams.NaviGpsDLimit; |
287 | toNaviCtrl.Param.Byte[6] = staticParams.NaviGpsDLimit; |
288 | break; |
288 | break; |
289 | 289 | ||
290 | case SPI_CMD_VERSION: |
290 | case SPI_CMD_VERSION: |
291 | toNaviCtrl.Param.Byte[0] = SPI_VersionInfo.Major; |
291 | toNaviCtrl.Param.Byte[0] = SPI_VersionInfo.Major; |
292 | toNaviCtrl.Param.Byte[1] = SPI_VersionInfo.Minor; |
292 | toNaviCtrl.Param.Byte[1] = SPI_VersionInfo.Minor; |
293 | toNaviCtrl.Param.Byte[2] = SPI_VersionInfo.Patch; |
293 | toNaviCtrl.Param.Byte[2] = SPI_VersionInfo.Patch; |
294 | toNaviCtrl.Param.Byte[3] = SPI_VersionInfo.Compatible; |
294 | toNaviCtrl.Param.Byte[3] = SPI_VersionInfo.Compatible; |
295 | toNaviCtrl.Param.Byte[4] = BoardRelease; |
295 | toNaviCtrl.Param.Byte[4] = BoardRelease; |
296 | break; |
296 | break; |
297 | default: |
297 | default: |
298 | break; |
298 | break; |
299 | } |
299 | } |
300 | 300 | ||
301 | sei(); |
301 | sei(); |
302 | // enable all interrupts |
302 | // enable all interrupts |
303 | 303 | ||
304 | // analyze content of packet from NaviCtrl if valid |
304 | // analyze content of packet from NaviCtrl if valid |
305 | if (SPI_RxDataValid) { |
305 | if (SPI_RxDataValid) { |
306 | // update gps controls |
306 | // update gps controls |
307 | if (abs(fromNaviCtrl.GPSStickPitch) < 512 && abs(fromNaviCtrl.GPSStickRoll) |
307 | if (abs(fromNaviCtrl.GPSStickPitch) < 512 && abs(fromNaviCtrl.GPSStickRoll) |
308 | < 512 && (staticParams.GlobalConfig & CFG_GPS_ACTIVE)) { |
308 | < 512 && (staticParams.GlobalConfig & CFG_GPS_ACTIVE)) { |
309 | GPSStickPitch = fromNaviCtrl.GPSStickPitch; |
309 | GPSStickPitch = fromNaviCtrl.GPSStickPitch; |
310 | GPSStickRoll = fromNaviCtrl.GPSStickRoll; |
310 | GPSStickRoll = fromNaviCtrl.GPSStickRoll; |
311 | NCDataOkay = 250; |
311 | NCDataOkay = 250; |
312 | } |
312 | } |
313 | // update compass readings |
313 | // update compass readings |
314 | if (fromNaviCtrl.CompassHeading <= 360) { |
314 | if (fromNaviCtrl.CompassHeading <= 360) { |
315 | compassHeading = fromNaviCtrl.CompassHeading; |
315 | compassHeading = fromNaviCtrl.CompassHeading; |
316 | } |
316 | } |
317 | //if(compassHeading < 0) compassOffCourse = 0; |
317 | //if(compassHeading < 0) compassOffCourse = 0; |
318 | //else compassOffCourse = ((540 + compassHeading - compassCourse) % 360) - 180; |
318 | //else compassOffCourse = ((540 + compassHeading - compassCourse) % 360) - 180; |
319 | // NaviCtrl wants to beep? |
319 | // NaviCtrl wants to beep? |
320 | if (fromNaviCtrl.BeepTime > BeepTime && !compassCalState) |
320 | if (fromNaviCtrl.BeepTime > BeepTime && !compassCalState) |
321 | BeepTime = fromNaviCtrl.BeepTime; |
321 | BeepTime = fromNaviCtrl.BeepTime; |
322 | 322 | ||
323 | switch (fromNaviCtrl.Command) { |
323 | switch (fromNaviCtrl.Command) { |
324 | case SPI_KALMAN: |
324 | case SPI_KALMAN: |
325 | dynamicParams.KalmanK = fromNaviCtrl.Param.Byte[0]; |
325 | dynamicParams.KalmanK = fromNaviCtrl.Param.Byte[0]; |
326 | dynamicParams.KalmanMaxFusion = fromNaviCtrl.Param.Byte[1]; |
326 | dynamicParams.KalmanMaxFusion = fromNaviCtrl.Param.Byte[1]; |
327 | dynamicParams.KalmanMaxDrift = fromNaviCtrl.Param.Byte[2]; |
327 | dynamicParams.KalmanMaxDrift = fromNaviCtrl.Param.Byte[2]; |
328 | NCSerialDataOkay = fromNaviCtrl.Param.Byte[3]; |
328 | NCSerialDataOkay = fromNaviCtrl.Param.Byte[3]; |
329 | break; |
329 | break; |
330 | 330 | ||
331 | default: |
331 | default: |
332 | break; |
332 | break; |
333 | } |
333 | } |
334 | } else { // no valid data from NaviCtrl |
334 | } else { // no valid data from NaviCtrl |
335 | // disable GPS control |
335 | // disable GPS control |
336 | GPSStickPitch = 0; |
336 | GPSStickPitch = 0; |
337 | GPSStickRoll = 0; |
337 | GPSStickRoll = 0; |
338 | } |
338 | } |
339 | } |
339 | } |
340 | 340 | ||
341 | /*********************************************/ |
341 | /*********************************************/ |
342 | /* Start Transmission of packet to NaviCtrl */ |
342 | /* Start Transmission of packet to NaviCtrl */ |
343 | /*********************************************/ |
343 | /*********************************************/ |
344 | void SPI_StartTransmitPacket(void) { |
344 | void SPI_StartTransmitPacket(void) { |
345 | if (!SPITransferCompleted) |
345 | if (!SPITransferCompleted) |
346 | return; // return immediately if transfer is in progress |
346 | return; // return immediately if transfer is in progress |
347 | else // transmission was completed |
347 | else // transmission was completed |
348 | { |
348 | { |
349 | SLAVE_SELECT_PORT &= ~(1 << SPI_SLAVE_SELECT); // Select slave |
349 | SLAVE_SELECT_PORT &= ~(1 << SPI_SLAVE_SELECT); // Select slave |
350 | 350 | ||
351 | // cyclic commands |
351 | // cyclic commands |
352 | toNaviCtrl.Command = SPI_CommandSequence[SPI_CommandCounter++]; |
352 | toNaviCtrl.Command = SPI_CommandSequence[SPI_CommandCounter++]; |
353 | if (SPI_CommandCounter >= sizeof(SPI_CommandSequence)) |
353 | if (SPI_CommandCounter >= sizeof(SPI_CommandSequence)) |
354 | SPI_CommandCounter = 0; |
354 | SPI_CommandCounter = 0; |
355 | 355 | ||
356 | SPITransferCompleted = 0; // transfer is in progress |
356 | SPITransferCompleted = 0; // transfer is in progress |
357 | UpdateSPI_Buffer(); // update data in toNaviCtrl |
357 | UpdateSPI_Buffer(); // update data in toNaviCtrl |
358 | 358 | ||
359 | SPI_TxBufferIndex = 1; //proceed with 2nd byte |
359 | SPI_TxBufferIndex = 1; //proceed with 2nd byte |
360 | 360 | ||
361 | // -- Debug-Output --- |
361 | // -- Debug-Output --- |
362 | //---- |
362 | //---- |
363 | asm volatile ("nop"); |
363 | asm volatile ("nop"); |
364 | asm volatile ("nop"); |
364 | asm volatile ("nop"); |
365 | asm volatile ("nop"); |
365 | asm volatile ("nop"); |
366 | asm volatile ("nop"); |
366 | asm volatile ("nop"); |
367 | asm volatile ("nop"); |
367 | asm volatile ("nop"); |
368 | asm volatile ("nop"); |
368 | asm volatile ("nop"); |
369 | asm volatile ("nop"); |
369 | asm volatile ("nop"); |
370 | asm volatile ("nop"); |
370 | asm volatile ("nop"); |
371 | asm volatile ("nop"); |
371 | asm volatile ("nop"); |
372 | asm volatile ("nop"); |
372 | asm volatile ("nop"); |
373 | asm volatile ("nop"); |
373 | asm volatile ("nop"); |
374 | asm volatile ("nop"); |
374 | asm volatile ("nop"); |
375 | asm volatile ("nop"); |
375 | asm volatile ("nop"); |
376 | asm volatile ("nop"); |
376 | asm volatile ("nop"); |
377 | asm volatile ("nop"); |
377 | asm volatile ("nop"); |
378 | asm volatile ("nop"); |
378 | asm volatile ("nop"); |
379 | asm volatile ("nop"); |
379 | asm volatile ("nop"); |
380 | asm volatile ("nop"); |
380 | asm volatile ("nop"); |
381 | asm volatile ("nop"); |
381 | asm volatile ("nop"); |
382 | asm volatile ("nop"); |
382 | asm volatile ("nop"); |
383 | asm volatile ("nop"); |
383 | asm volatile ("nop"); |
384 | asm volatile ("nop"); |
384 | asm volatile ("nop"); |
385 | asm volatile ("nop"); |
385 | asm volatile ("nop"); |
386 | asm volatile ("nop"); |
386 | asm volatile ("nop"); |
387 | toNaviCtrl.Chksum = toNaviCtrl.Sync1; // init checksum |
387 | toNaviCtrl.Chksum = toNaviCtrl.Sync1; // init checksum |
388 | SPDR = toNaviCtrl.Sync1; // send first byte |
388 | SPDR = toNaviCtrl.Sync1; // send first byte |
389 | } |
389 | } |
390 | } |
390 | } |
391 | 391 | ||
392 | //------------------------------------------------------ |
392 | //------------------------------------------------------ |
393 | // This is the spi data transfer between FlightCtrl and NaviCtrl |
393 | // This is the spi data transfer between FlightCtrl and NaviCtrl |
394 | // Every time this routine is called within the mainloop one byte of the packet to |
394 | // Every time this routine is called within the mainloop one byte of the packet to |
395 | // the NaviCtrl and one byte of the packet from the NaviCtrl is possible transfered |
395 | // the NaviCtrl and one byte of the packet from the NaviCtrl is possible transfered |
396 | 396 | ||
397 | void SPI_TransmitByte(void) { |
397 | void SPI_TransmitByte(void) { |
398 | static SPI_RXState_t SPI_RXState = SPI_SYNC1; |
398 | static SPI_RXState_t SPI_RXState = SPI_SYNC1; |
399 | uint8_t rxdata; |
399 | uint8_t rxdata; |
400 | static uint8_t rxchksum; |
400 | static uint8_t rxchksum; |
401 | 401 | ||
402 | if (SPITransferCompleted) |
402 | if (SPITransferCompleted) |
403 | return; // return immediatly if transfer was completed |
403 | return; // return immediatly if transfer was completed |
404 | if (!(SPSR & (1 << SPIF))) |
404 | if (!(SPSR & (1 << SPIF))) |
405 | return; // return if no SPI-IRQ pending |
405 | return; // return if no SPI-IRQ pending |
406 | SendSPI = 4; // mait 4 * 0.102 ms for the next call of SPI_TransmitByte() in the main loop |
406 | SendSPI = 4; // mait 4 * 0.102 ms for the next call of SPI_TransmitByte() in the main loop |
407 | 407 | ||
408 | SLAVE_SELECT_PORT |= (1 << SPI_SLAVE_SELECT); // DeselectSlave |
408 | SLAVE_SELECT_PORT |= (1 << SPI_SLAVE_SELECT); // DeselectSlave |
409 | 409 | ||
410 | rxdata = SPDR; // save spi data register |
410 | rxdata = SPDR; // save spi data register |
411 | 411 | ||
412 | switch (SPI_RXState) { |
412 | switch (SPI_RXState) { |
413 | case SPI_SYNC1: // first sync byte |
413 | case SPI_SYNC1: // first sync byte |
414 | SPI_RxBufferIndex = 0; // set pointer to start of rx buffer |
414 | SPI_RxBufferIndex = 0; // set pointer to start of rx buffer |
415 | rxchksum = rxdata; // initialize checksum |
415 | rxchksum = rxdata; // initialize checksum |
416 | if (rxdata == SPI_RXSYNCBYTE1) { // 1st Syncbyte found |
416 | if (rxdata == SPI_RXSYNCBYTE1) { // 1st Syncbyte found |
417 | SPI_RXState = SPI_SYNC2; // trigger to state for second sync byte |
417 | SPI_RXState = SPI_SYNC2; // trigger to state for second sync byte |
418 | } |
418 | } |
419 | break; |
419 | break; |
420 | 420 | ||
421 | case SPI_SYNC2: // second sync byte |
421 | case SPI_SYNC2: // second sync byte |
422 | if (rxdata == SPI_RXSYNCBYTE2) { // 2nd Syncbyte found |
422 | if (rxdata == SPI_RXSYNCBYTE2) { // 2nd Syncbyte found |
423 | rxchksum += rxdata; // update checksum |
423 | rxchksum += rxdata; // update checksum |
424 | SPI_RXState = SPI_DATA; // trigger to state for second sync byte |
424 | SPI_RXState = SPI_DATA; // trigger to state for second sync byte |
425 | } else // 2nd Syncbyte not found |
425 | } else // 2nd Syncbyte not found |
426 | { |
426 | { |
427 | SPI_RXState = SPI_SYNC1; // jump back to 1st sync byte |
427 | SPI_RXState = SPI_SYNC1; // jump back to 1st sync byte |
428 | } |
428 | } |
429 | break; |
429 | break; |
430 | 430 | ||
431 | case SPI_DATA: // data bytes |
431 | case SPI_DATA: // data bytes |
432 | SPI_RxBuffer[SPI_RxBufferIndex++] = rxdata; // copy data byte to spi buffer |
432 | SPI_RxBuffer[SPI_RxBufferIndex++] = rxdata; // copy data byte to spi buffer |
433 | // if all bytes are received of a packet from the NaviCtrl |
433 | // if all bytes are received of a packet from the NaviCtrl |
434 | if (SPI_RxBufferIndex >= SPI_RXBUFFER_LEN) { // last byte transfered is the checksum of the packet |
434 | if (SPI_RxBufferIndex >= SPI_RXBUFFER_LEN) { // last byte transfered is the checksum of the packet |
435 | if (rxdata == rxchksum) { // checksum matching? |
435 | if (rxdata == rxchksum) { // checksum matching? |
436 | // copy SPI_RxBuffer -> FromFlightCtrl |
436 | // copy SPI_RxBuffer -> FromFlightCtrl |
437 | uint8_t *ptr = (uint8_t *) &fromNaviCtrl; |
437 | uint8_t *ptr = (uint8_t *) &fromNaviCtrl; |
438 | cli(); |
438 | cli(); |
439 | memcpy(ptr, (uint8_t *) SPI_RxBuffer, sizeof(fromNaviCtrl)); |
439 | memcpy(ptr, (uint8_t *) SPI_RxBuffer, sizeof(fromNaviCtrl)); |
440 | sei(); |
440 | sei(); |
441 | SPI_RxDataValid = 1; |
441 | SPI_RxDataValid = 1; |
442 | } else { // checksum does not match |
442 | } else { // checksum does not match |
443 | SPI_RxDataValid = 0; // reset valid flag |
443 | SPI_RxDataValid = 0; // reset valid flag |
444 | } |
444 | } |
445 | SPI_RXState = SPI_SYNC1; // reset state sync |
445 | SPI_RXState = SPI_SYNC1; // reset state sync |
446 | } else { // not all bytes transfered |
446 | } else { // not all bytes transfered |
447 | rxchksum += rxdata; // update checksum |
447 | rxchksum += rxdata; // update checksum |
448 | } |
448 | } |
449 | break; |
449 | break; |
450 | }// eof switch(SPI_RXState) |
450 | }// eof switch(SPI_RXState) |
451 | 451 | ||
452 | // if still some bytes left for transmission to NaviCtrl |
452 | // if still some bytes left for transmission to NaviCtrl |
453 | if (SPI_TxBufferIndex < SPI_TXBUFFER_LEN) { |
453 | if (SPI_TxBufferIndex < SPI_TXBUFFER_LEN) { |
454 | SLAVE_SELECT_PORT &= ~(1 << SPI_SLAVE_SELECT); // SelectSlave |
454 | SLAVE_SELECT_PORT &= ~(1 << SPI_SLAVE_SELECT); // SelectSlave |
455 | asm volatile ("nop"); |
455 | asm volatile ("nop"); |
456 | asm volatile ("nop"); |
456 | asm volatile ("nop"); |
457 | asm volatile ("nop"); |
457 | asm volatile ("nop"); |
458 | asm volatile ("nop"); |
458 | asm volatile ("nop"); |
459 | asm volatile ("nop"); |
459 | asm volatile ("nop"); |
460 | asm volatile ("nop"); |
460 | asm volatile ("nop"); |
461 | asm volatile ("nop"); |
461 | asm volatile ("nop"); |
462 | asm volatile ("nop"); |
462 | asm volatile ("nop"); |
463 | asm volatile ("nop"); |
463 | asm volatile ("nop"); |
464 | asm volatile ("nop"); |
464 | asm volatile ("nop"); |
465 | asm volatile ("nop"); |
465 | asm volatile ("nop"); |
466 | asm volatile ("nop"); |
466 | asm volatile ("nop"); |
467 | asm volatile ("nop"); |
467 | asm volatile ("nop"); |
468 | asm volatile ("nop"); |
468 | asm volatile ("nop"); |
469 | asm volatile ("nop"); |
469 | asm volatile ("nop"); |
470 | asm volatile ("nop"); |
470 | asm volatile ("nop"); |
471 | asm volatile ("nop"); |
471 | asm volatile ("nop"); |
472 | asm volatile ("nop"); |
472 | asm volatile ("nop"); |
473 | asm volatile ("nop"); |
473 | asm volatile ("nop"); |
474 | asm volatile ("nop"); |
474 | asm volatile ("nop"); |
475 | asm volatile ("nop"); |
475 | asm volatile ("nop"); |
476 | asm volatile ("nop"); |
476 | asm volatile ("nop"); |
477 | asm volatile ("nop"); |
477 | asm volatile ("nop"); |
478 | asm volatile ("nop"); |
478 | asm volatile ("nop"); |
479 | 479 | ||
480 | SPDR = SPI_TxBuffer[SPI_TxBufferIndex]; // transmit byte |
480 | SPDR = SPI_TxBuffer[SPI_TxBufferIndex]; // transmit byte |
481 | toNaviCtrl.Chksum += SPI_TxBuffer[SPI_TxBufferIndex]; // update checksum for everey byte that was sent |
481 | toNaviCtrl.Chksum += SPI_TxBuffer[SPI_TxBufferIndex]; // update checksum for everey byte that was sent |
482 | SPI_TxBufferIndex++; |
482 | SPI_TxBufferIndex++; |
483 | } else { |
483 | } else { |
484 | //Transfer of all bytes of the packet to NaviCtrl completed |
484 | //Transfer of all bytes of the packet to NaviCtrl completed |
485 | SPITransferCompleted = 1; |
485 | SPITransferCompleted = 1; |
486 | } |
486 | } |
487 | } |
487 | } |
488 | 488 |