Rev 908 | Go to most recent revision | Only display areas with differences | Ignore whitespace | Details | Blame | Last modification | View Log | RSS feed
Rev 908 | Rev 911 | ||
---|---|---|---|
1 | // ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
1 | // ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
2 | // + Copyright (c) 04.2007 Holger Buss |
2 | // + Copyright (c) 04.2007 Holger Buss |
3 | // + only for non-profit use |
3 | // + only for non-profit use |
4 | // + www.MikroKopter.com |
4 | // + www.MikroKopter.com |
5 | // + see the File "License.txt" for further Informations |
5 | // + see the File "License.txt" for further Informations |
6 | // ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
6 | // ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
7 | 7 | ||
8 | #include <avr/io.h> |
8 | #include <avr/io.h> |
9 | #include <avr/interrupt.h> |
9 | #include <avr/interrupt.h> |
10 | #include <avr/wdt.h> |
10 | #include <avr/wdt.h> |
11 | 11 | ||
12 | #include "eeprom.h" |
12 | #include "eeprom.h" |
13 | #include "main.h" |
13 | #include "main.h" |
14 | #include "menu.h" |
14 | #include "menu.h" |
15 | #include "timer0.h" |
15 | #include "timer0.h" |
16 | #include "uart.h" |
16 | #include "uart.h" |
17 | #include "fc.h" |
17 | #include "fc.h" |
18 | #include "_Settings.h" |
18 | #include "_Settings.h" |
19 | #include "rc.h" |
19 | #include "rc.h" |
20 | #if defined (USE_KILLAGREG) || defined (USE_MK3MAG) |
20 | #if defined (USE_KILLAGREG) || defined (USE_MK3MAG) |
21 | #include "ubx.h" |
21 | #include "ubx.h" |
22 | #endif |
22 | #endif |
23 | #ifdef USE_MK3MAG |
23 | #ifdef USE_MK3MAG |
24 | #include "mk3mag.h" |
24 | #include "mk3mag.h" |
25 | #endif |
25 | #endif |
26 | 26 | ||
27 | 27 | ||
28 | 28 | ||
29 | #define FALSE 0 |
29 | #define FALSE 0 |
30 | #define TRUE 1 |
30 | #define TRUE 1 |
31 | 31 | ||
32 | //int8_t test __attribute__ ((section (".noinit"))); |
32 | //int8_t test __attribute__ ((section (".noinit"))); |
33 | uint8_t RequestVerInfo = FALSE; |
33 | uint8_t RequestVerInfo = FALSE; |
34 | uint8_t RequestExternalControl = FALSE; |
34 | uint8_t RequestExternalControl = FALSE; |
35 | uint8_t RequestDisplay = FALSE; |
35 | uint8_t RequestDisplay = FALSE; |
36 | uint8_t RequestDebugData = FALSE; |
36 | uint8_t RequestDebugData = FALSE; |
37 | uint8_t RequestDebugLabel = 255; |
37 | uint8_t RequestDebugLabel = 255; |
38 | uint8_t RequestChannelOnly = FALSE; |
38 | uint8_t RequestChannelOnly = FALSE; |
39 | uint8_t RemotePollDisplayLine = 0; |
39 | uint8_t RemotePollDisplayLine = 0; |
40 | 40 | ||
41 | volatile uint8_t txd_buffer[TXD_BUFFER_LEN]; |
41 | volatile uint8_t txd_buffer[TXD_BUFFER_LEN]; |
42 | volatile uint8_t rxd_buffer_locked = FALSE; |
42 | volatile uint8_t rxd_buffer_locked = FALSE; |
43 | volatile uint8_t rxd_buffer[RXD_BUFFER_LEN]; |
43 | volatile uint8_t rxd_buffer[RXD_BUFFER_LEN]; |
44 | volatile uint8_t txd_complete = TRUE; |
44 | volatile uint8_t txd_complete = TRUE; |
45 | volatile uint8_t ReceivedBytes = 0; |
45 | volatile uint8_t ReceivedBytes = 0; |
46 | 46 | ||
47 | 47 | ||
48 | uint8_t PcAccess = 100; |
48 | uint8_t PcAccess = 100; |
49 | uint8_t MotorTest[4] = {0,0,0,0}; |
49 | uint8_t MotorTest[4] = {0,0,0,0}; |
50 | uint8_t DubWiseKeys[4] = {0,0,0,0}; |
50 | uint8_t DubWiseKeys[4] = {0,0,0,0}; |
51 | uint8_t MySlaveAddr = 0; |
51 | uint8_t MySlaveAddr = 0; |
52 | uint8_t ConfirmFrame; |
52 | uint8_t ConfirmFrame; |
53 | 53 | ||
54 | DebugOut_t DebugOut; |
54 | DebugOut_t DebugOut; |
55 | ExternControl_t ExternControl; |
55 | ExternControl_t ExternControl; |
56 | VersionInfo_t VersionInfo; |
56 | VersionInfo_t VersionInfo; |
57 | 57 | ||
58 | int16_t Debug_Timer; |
58 | int16_t Debug_Timer; |
59 | 59 | ||
60 | #ifdef USE_MK3MAG |
60 | #ifdef USE_MK3MAG |
61 | int16_t Compass_Timer; |
61 | int16_t Compass_Timer; |
62 | #endif |
62 | #endif |
63 | 63 | ||
64 | 64 | ||
65 | const uint8_t ANALOG_LABEL[32][16] = |
65 | const uint8_t ANALOG_LABEL[32][16] = |
66 | { |
66 | { |
67 | //1234567890123456 |
67 | //1234567890123456 |
68 | "IntegralPitch ", //0 |
68 | "IntegralNick ", //0 |
69 | "IntegralRoll ", |
69 | "IntegralRoll ", |
70 | "AccPitch ", |
70 | "AccNick ", |
71 | "AccRoll ", |
71 | "AccRoll ", |
72 | "GyroYaw ", |
72 | "GyroYaw ", |
73 | "ReadingHeight ", //5 |
73 | "ReadingHeight ", //5 |
74 | "AccZ ", |
74 | "AccZ ", |
75 | "Thrust ", |
75 | "Gas ", |
76 | "CompassHeading ", |
76 | "CompassHeading ", |
77 | "Voltage ", |
77 | "Voltage ", |
78 | "Receiver Level ", //10 |
78 | "Receiver Level ", //10 |
79 | "YawGyroHeading ", |
79 | "YawGyroHeading ", |
80 | "Motor_Front ", |
80 | "Motor_Front ", |
81 | "Motor_Rear ", |
81 | "Motor_Rear ", |
82 | "Motor_Right ", |
82 | "Motor_Right ", |
83 | "Motor_Left ", //15 |
83 | "Motor_Left ", //15 |
84 | "Acc_Z ", |
84 | "Acc_Z ", |
85 | "SPI Error ", |
85 | "SPI Error ", |
86 | "SPI Ok ", |
86 | "SPI Ok ", |
87 | " ", |
87 | " ", |
88 | "Servo ", //20 |
88 | "Servo ", //20 |
89 | "Pitch ", |
89 | "Nick ", |
90 | "Roll ", |
90 | "Roll ", |
91 | " ", |
91 | " ", |
92 | " ", |
92 | " ", |
93 | " ", //25 |
93 | " ", //25 |
94 | " ", |
94 | " ", |
95 | " ", |
95 | " ", |
96 | " ", |
96 | " ", |
97 | " ", |
97 | " ", |
98 | "GPS_Pitch ", //30 |
98 | "GPS_Nick ", //30 |
99 | "GPS_Roll " |
99 | "GPS_Roll " |
100 | }; |
100 | }; |
101 | 101 | ||
102 | 102 | ||
103 | 103 | ||
104 | /****************************************************************/ |
104 | /****************************************************************/ |
105 | /* Initialization of the USART0 */ |
105 | /* Initialization of the USART0 */ |
106 | /****************************************************************/ |
106 | /****************************************************************/ |
107 | void USART0_Init (void) |
107 | void USART0_Init (void) |
108 | { |
108 | { |
109 | uint8_t sreg = SREG; |
109 | uint8_t sreg = SREG; |
110 | uint16_t ubrr = (uint16_t) ((uint32_t) SYSCLK/(8 * USART0_BAUD) - 1); |
110 | uint16_t ubrr = (uint16_t) ((uint32_t) SYSCLK/(8 * USART0_BAUD) - 1); |
111 | 111 | ||
112 | // disable all interrupts before configuration |
112 | // disable all interrupts before configuration |
113 | cli(); |
113 | cli(); |
114 | 114 | ||
115 | // disable RX-Interrupt |
115 | // disable RX-Interrupt |
116 | UCSR0B &= ~(1 << RXCIE0); |
116 | UCSR0B &= ~(1 << RXCIE0); |
117 | // disable TX-Interrupt |
117 | // disable TX-Interrupt |
118 | UCSR0B &= ~(1 << TXCIE0); |
118 | UCSR0B &= ~(1 << TXCIE0); |
119 | 119 | ||
120 | // set direction of RXD0 and TXD0 pins |
120 | // set direction of RXD0 and TXD0 pins |
121 | // set RXD0 (PD0) as an input pin |
121 | // set RXD0 (PD0) as an input pin |
122 | PORTD |= (1 << PORTD0); |
122 | PORTD |= (1 << PORTD0); |
123 | DDRD &= ~(1 << DDD0); |
123 | DDRD &= ~(1 << DDD0); |
124 | // set TXD0 (PD1) as an output pin |
124 | // set TXD0 (PD1) as an output pin |
125 | PORTD |= (1 << PORTD1); |
125 | PORTD |= (1 << PORTD1); |
126 | DDRD |= (1 << DDD1); |
126 | DDRD |= (1 << DDD1); |
127 | 127 | ||
128 | // USART0 Baud Rate Register |
128 | // USART0 Baud Rate Register |
129 | // set clock divider |
129 | // set clock divider |
130 | UBRR0H = (uint8_t)(ubrr >> 8); |
130 | UBRR0H = (uint8_t)(ubrr >> 8); |
131 | UBRR0L = (uint8_t)ubrr; |
131 | UBRR0L = (uint8_t)ubrr; |
132 | 132 | ||
133 | // USART0 Control and Status Register A, B, C |
133 | // USART0 Control and Status Register A, B, C |
134 | 134 | ||
135 | // enable double speed operation in |
135 | // enable double speed operation in |
136 | UCSR0A |= (1 << U2X0); |
136 | UCSR0A |= (1 << U2X0); |
137 | // enable receiver and transmitter in |
137 | // enable receiver and transmitter in |
138 | UCSR0B = (1 << TXEN0) | (1 << RXEN0); |
138 | UCSR0B = (1 << TXEN0) | (1 << RXEN0); |
139 | // set asynchronous mode |
139 | // set asynchronous mode |
140 | UCSR0C &= ~(1 << UMSEL01); |
140 | UCSR0C &= ~(1 << UMSEL01); |
141 | UCSR0C &= ~(1 << UMSEL00); |
141 | UCSR0C &= ~(1 << UMSEL00); |
142 | // no parity |
142 | // no parity |
143 | UCSR0C &= ~(1 << UPM01); |
143 | UCSR0C &= ~(1 << UPM01); |
144 | UCSR0C &= ~(1 << UPM00); |
144 | UCSR0C &= ~(1 << UPM00); |
145 | // 1 stop bit |
145 | // 1 stop bit |
146 | UCSR0C &= ~(1 << USBS0); |
146 | UCSR0C &= ~(1 << USBS0); |
147 | // 8-bit |
147 | // 8-bit |
148 | UCSR0B &= ~(1 << UCSZ02); |
148 | UCSR0B &= ~(1 << UCSZ02); |
149 | UCSR0C |= (1 << UCSZ01); |
149 | UCSR0C |= (1 << UCSZ01); |
150 | UCSR0C |= (1 << UCSZ00); |
150 | UCSR0C |= (1 << UCSZ00); |
151 | 151 | ||
152 | // flush receive buffer |
152 | // flush receive buffer |
153 | while ( UCSR0A & (1<<RXC0) ) UDR0; |
153 | while ( UCSR0A & (1<<RXC0) ) UDR0; |
154 | 154 | ||
155 | // enable interrupts at the end |
155 | // enable interrupts at the end |
156 | // enable RX-Interrupt |
156 | // enable RX-Interrupt |
157 | UCSR0B |= (1 << RXCIE0); |
157 | UCSR0B |= (1 << RXCIE0); |
158 | // enable TX-Interrupt |
158 | // enable TX-Interrupt |
159 | UCSR0B |= (1 << TXCIE0); |
159 | UCSR0B |= (1 << TXCIE0); |
160 | 160 | ||
161 | rxd_buffer_locked = FALSE; |
161 | rxd_buffer_locked = FALSE; |
162 | txd_complete = TRUE; |
162 | txd_complete = TRUE; |
163 | 163 | ||
164 | Debug_Timer = SetDelay(200); |
164 | Debug_Timer = SetDelay(200); |
165 | 165 | ||
166 | #ifdef USE_MK3MAG |
166 | #ifdef USE_MK3MAG |
167 | Compass_Timer = SetDelay(220); |
167 | Compass_Timer = SetDelay(220); |
168 | #endif |
168 | #endif |
169 | 169 | ||
170 | // restore global interrupt flags |
170 | // restore global interrupt flags |
171 | SREG = sreg; |
171 | SREG = sreg; |
172 | } |
172 | } |
173 | 173 | ||
174 | /****************************************************************/ |
174 | /****************************************************************/ |
175 | /* USART0 transmitter ISR */ |
175 | /* USART0 transmitter ISR */ |
176 | /****************************************************************/ |
176 | /****************************************************************/ |
177 | ISR(USART0_TX_vect) |
177 | ISR(USART0_TX_vect) |
178 | { |
178 | { |
179 | static uint16_t ptr_txd_buffer = 0; |
179 | static uint16_t ptr_txd_buffer = 0; |
180 | uint8_t tmp_tx; |
180 | uint8_t tmp_tx; |
181 | if(!txd_complete) // transmission not completed |
181 | if(!txd_complete) // transmission not completed |
182 | { |
182 | { |
183 | ptr_txd_buffer++; // die [0] wurde schon gesendet |
183 | ptr_txd_buffer++; // die [0] wurde schon gesendet |
184 | tmp_tx = txd_buffer[ptr_txd_buffer]; |
184 | tmp_tx = txd_buffer[ptr_txd_buffer]; |
185 | // if terminating character or end of txd buffer was reached |
185 | // if terminating character or end of txd buffer was reached |
186 | if((tmp_tx == '\r') || (ptr_txd_buffer == TXD_BUFFER_LEN)) |
186 | if((tmp_tx == '\r') || (ptr_txd_buffer == TXD_BUFFER_LEN)) |
187 | { |
187 | { |
188 | ptr_txd_buffer = 0; // reset txd pointer |
188 | ptr_txd_buffer = 0; // reset txd pointer |
189 | txd_complete = 1; // stop transmission |
189 | txd_complete = 1; // stop transmission |
190 | } |
190 | } |
191 | UDR0 = tmp_tx; // send current byte will trigger this ISR again |
191 | UDR0 = tmp_tx; // send current byte will trigger this ISR again |
192 | } |
192 | } |
193 | // transmission completed |
193 | // transmission completed |
194 | else ptr_txd_buffer = 0; |
194 | else ptr_txd_buffer = 0; |
195 | } |
195 | } |
196 | 196 | ||
197 | /****************************************************************/ |
197 | /****************************************************************/ |
198 | /* USART0 receiver ISR */ |
198 | /* USART0 receiver ISR */ |
199 | /****************************************************************/ |
199 | /****************************************************************/ |
200 | ISR(USART0_RX_vect) |
200 | ISR(USART0_RX_vect) |
201 | { |
201 | { |
202 | static uint16_t crc; |
202 | static uint16_t crc; |
203 | static uint8_t ptr_rxd_buffer = 0; |
203 | static uint8_t ptr_rxd_buffer = 0; |
204 | uint8_t crc1, crc2; |
204 | uint8_t crc1, crc2; |
205 | uint8_t c; |
205 | uint8_t c; |
206 | 206 | ||
207 | c = UDR0; // catch the received byte |
207 | c = UDR0; // catch the received byte |
208 | 208 | ||
209 | #if defined (USE_KILLAGREG) || defined (USE_MK3MAG) |
209 | #if defined (USE_KILLAGREG) || defined (USE_MK3MAG) |
210 | // If the FC 1.0 cpu is used the ublox module should be conneced to rxd of the 1st uart. |
210 | // If the FC 1.0 cpu is used the ublox module should be conneced to rxd of the 1st uart. |
211 | // The FC 1.1 /1.2 has the ATMEGA644p cpu with a 2nd uart to which the ublox should be connected. |
211 | // The FC 1.1 /1.2 has the ATMEGA644p cpu with a 2nd uart to which the ublox should be connected. |
212 | #if defined (__AVR_ATmega644P__) |
212 | #if defined (__AVR_ATmega644P__) |
213 | if(BoardRelease == 10) ubx_parser(c); |
213 | if(BoardRelease == 10) ubx_parser(c); |
214 | #else |
214 | #else |
215 | ubx_parser(c); |
215 | ubx_parser(c); |
216 | #endif |
216 | #endif |
217 | #endif |
217 | #endif |
218 | 218 | ||
219 | if(rxd_buffer_locked) return; // if rxd buffer is locked immediately return |
219 | if(rxd_buffer_locked) return; // if rxd buffer is locked immediately return |
220 | 220 | ||
221 | // the rxd buffer is unlocked |
221 | // the rxd buffer is unlocked |
222 | if((ptr_rxd_buffer == 0) && (c == '#')) // if rxd buffer is empty and syncronisation character is received |
222 | if((ptr_rxd_buffer == 0) && (c == '#')) // if rxd buffer is empty and syncronisation character is received |
223 | { |
223 | { |
224 | rxd_buffer[ptr_rxd_buffer++] = c; // copy 1st byte to buffer |
224 | rxd_buffer[ptr_rxd_buffer++] = c; // copy 1st byte to buffer |
225 | crc = c; // init crc |
225 | crc = c; // init crc |
226 | } |
226 | } |
227 | #if 0 |
227 | #if 0 |
228 | else if (ptr_rxd_buffer == 1) // handle address |
228 | else if (ptr_rxd_buffer == 1) // handle address |
229 | { |
229 | { |
230 | rxd_buffer[ptr_rxd_buffer++] = c; // copy byte to rxd buffer |
230 | rxd_buffer[ptr_rxd_buffer++] = c; // copy byte to rxd buffer |
231 | crc += c; // update crc |
231 | crc += c; // update crc |
232 | } |
232 | } |
233 | #endif |
233 | #endif |
234 | else if (ptr_rxd_buffer < RXD_BUFFER_LEN) // collect incomming bytes |
234 | else if (ptr_rxd_buffer < RXD_BUFFER_LEN) // collect incomming bytes |
235 | { |
235 | { |
236 | if(c != '\r') // no termination character |
236 | if(c != '\r') // no termination character |
237 | { |
237 | { |
238 | rxd_buffer[ptr_rxd_buffer++] = c; // copy byte to rxd buffer |
238 | rxd_buffer[ptr_rxd_buffer++] = c; // copy byte to rxd buffer |
239 | crc += c; // update crc |
239 | crc += c; // update crc |
240 | } |
240 | } |
241 | else // termination character was received |
241 | else // termination character was received |
242 | { |
242 | { |
243 | // the last 2 bytes are no subject for checksum calculation |
243 | // the last 2 bytes are no subject for checksum calculation |
244 | // they are the checksum itself |
244 | // they are the checksum itself |
245 | crc -= rxd_buffer[ptr_rxd_buffer-2]; |
245 | crc -= rxd_buffer[ptr_rxd_buffer-2]; |
246 | crc -= rxd_buffer[ptr_rxd_buffer-1]; |
246 | crc -= rxd_buffer[ptr_rxd_buffer-1]; |
247 | // calculate checksum from transmitted data |
247 | // calculate checksum from transmitted data |
248 | crc %= 4096; |
248 | crc %= 4096; |
249 | crc1 = '=' + crc / 64; |
249 | crc1 = '=' + crc / 64; |
250 | crc2 = '=' + crc % 64; |
250 | crc2 = '=' + crc % 64; |
251 | // compare checksum to transmitted checksum bytes |
251 | // compare checksum to transmitted checksum bytes |
252 | if((crc1 == rxd_buffer[ptr_rxd_buffer-2]) && (crc2 == rxd_buffer[ptr_rxd_buffer-1])) |
252 | if((crc1 == rxd_buffer[ptr_rxd_buffer-2]) && (crc2 == rxd_buffer[ptr_rxd_buffer-1])) |
253 | { // checksum valid |
253 | { // checksum valid |
254 | rxd_buffer_locked = TRUE; // lock the rxd buffer |
254 | rxd_buffer_locked = TRUE; // lock the rxd buffer |
255 | ReceivedBytes = ptr_rxd_buffer; // store number of received bytes |
255 | ReceivedBytes = ptr_rxd_buffer; // store number of received bytes |
256 | rxd_buffer[ptr_rxd_buffer] = '\r'; // set termination character |
256 | rxd_buffer[ptr_rxd_buffer] = '\r'; // set termination character |
257 | // if 2nd byte is an 'R' enable watchdog that will result in an reset |
257 | // if 2nd byte is an 'R' enable watchdog that will result in an reset |
258 | if(rxd_buffer[2] == 'R') {wdt_enable(WDTO_250MS);} // Reset-Commando |
258 | if(rxd_buffer[2] == 'R') {wdt_enable(WDTO_250MS);} // Reset-Commando |
259 | } |
259 | } |
260 | else |
260 | else |
261 | { // checksum invalid |
261 | { // checksum invalid |
262 | rxd_buffer_locked = FALSE; // unlock rxd buffer |
262 | rxd_buffer_locked = FALSE; // unlock rxd buffer |
263 | } |
263 | } |
264 | ptr_rxd_buffer = 0; // reset rxd buffer pointer |
264 | ptr_rxd_buffer = 0; // reset rxd buffer pointer |
265 | } |
265 | } |
266 | } |
266 | } |
267 | else // rxd buffer overrun |
267 | else // rxd buffer overrun |
268 | { |
268 | { |
269 | ptr_rxd_buffer = 0; // reset rxd buffer |
269 | ptr_rxd_buffer = 0; // reset rxd buffer |
270 | rxd_buffer_locked = FALSE; // unlock rxd buffer |
270 | rxd_buffer_locked = FALSE; // unlock rxd buffer |
271 | } |
271 | } |
272 | 272 | ||
273 | } |
273 | } |
274 | 274 | ||
275 | 275 | ||
276 | // -------------------------------------------------------------------------- |
276 | // -------------------------------------------------------------------------- |
277 | void AddCRC(uint16_t datalen) |
277 | void AddCRC(uint16_t datalen) |
278 | { |
278 | { |
279 | uint16_t tmpCRC = 0, i; |
279 | uint16_t tmpCRC = 0, i; |
280 | for(i = 0; i < datalen; i++) |
280 | for(i = 0; i < datalen; i++) |
281 | { |
281 | { |
282 | tmpCRC += txd_buffer[i]; |
282 | tmpCRC += txd_buffer[i]; |
283 | } |
283 | } |
284 | tmpCRC %= 4096; |
284 | tmpCRC %= 4096; |
285 | txd_buffer[i++] = '=' + tmpCRC / 64; |
285 | txd_buffer[i++] = '=' + tmpCRC / 64; |
286 | txd_buffer[i++] = '=' + tmpCRC % 64; |
286 | txd_buffer[i++] = '=' + tmpCRC % 64; |
287 | txd_buffer[i++] = '\r'; |
287 | txd_buffer[i++] = '\r'; |
288 | txd_complete = FALSE; |
288 | txd_complete = FALSE; |
289 | UDR0 = txd_buffer[0]; // initiates the transmittion (continued in the TXD ISR) |
289 | UDR0 = txd_buffer[0]; // initiates the transmittion (continued in the TXD ISR) |
290 | } |
290 | } |
291 | 291 | ||
292 | 292 | ||
293 | 293 | ||
294 | // -------------------------------------------------------------------------- |
294 | // -------------------------------------------------------------------------- |
295 | void SendOutData(uint8_t cmd,uint8_t module, uint8_t *snd, uint8_t len) |
295 | void SendOutData(uint8_t cmd,uint8_t module, uint8_t *snd, uint8_t len) |
296 | { |
296 | { |
297 | uint16_t pt = 0; |
297 | uint16_t pt = 0; |
298 | uint8_t a,b,c; |
298 | uint8_t a,b,c; |
299 | uint8_t ptr = 0; |
299 | uint8_t ptr = 0; |
300 | 300 | ||
301 | txd_buffer[pt++] = '#'; // Start character |
301 | txd_buffer[pt++] = '#'; // Start character |
302 | txd_buffer[pt++] = module; // Address (a=0; b=1,...) |
302 | txd_buffer[pt++] = module; // Address (a=0; b=1,...) |
303 | txd_buffer[pt++] = cmd; // Command |
303 | txd_buffer[pt++] = cmd; // Command |
304 | 304 | ||
305 | while(len) |
305 | while(len) |
306 | { |
306 | { |
307 | if(len) { a = snd[ptr++]; len--;} else a = 0; |
307 | if(len) { a = snd[ptr++]; len--;} else a = 0; |
308 | if(len) { b = snd[ptr++]; len--;} else b = 0; |
308 | if(len) { b = snd[ptr++]; len--;} else b = 0; |
309 | if(len) { c = snd[ptr++]; len--;} else c = 0; |
309 | if(len) { c = snd[ptr++]; len--;} else c = 0; |
310 | txd_buffer[pt++] = '=' + (a >> 2); |
310 | txd_buffer[pt++] = '=' + (a >> 2); |
311 | txd_buffer[pt++] = '=' + (((a & 0x03) << 4) | ((b & 0xf0) >> 4)); |
311 | txd_buffer[pt++] = '=' + (((a & 0x03) << 4) | ((b & 0xf0) >> 4)); |
312 | txd_buffer[pt++] = '=' + (((b & 0x0f) << 2) | ((c & 0xc0) >> 6)); |
312 | txd_buffer[pt++] = '=' + (((b & 0x0f) << 2) | ((c & 0xc0) >> 6)); |
313 | txd_buffer[pt++] = '=' + ( c & 0x3f); |
313 | txd_buffer[pt++] = '=' + ( c & 0x3f); |
314 | } |
314 | } |
315 | AddCRC(pt); // add checksum after data block and initates the transmission |
315 | AddCRC(pt); // add checksum after data block and initates the transmission |
316 | } |
316 | } |
317 | 317 | ||
318 | 318 | ||
319 | // -------------------------------------------------------------------------- |
319 | // -------------------------------------------------------------------------- |
320 | void Decode64(uint8_t *ptrOut, uint8_t len, uint8_t ptrIn, uint8_t max) |
320 | void Decode64(uint8_t *ptrOut, uint8_t len, uint8_t ptrIn, uint8_t max) |
321 | { |
321 | { |
322 | uint8_t a,b,c,d; |
322 | uint8_t a,b,c,d; |
323 | uint8_t ptr = 0; |
323 | uint8_t ptr = 0; |
324 | uint8_t x,y,z; |
324 | uint8_t x,y,z; |
325 | while(len) |
325 | while(len) |
326 | { |
326 | { |
327 | a = rxd_buffer[ptrIn++] - '='; |
327 | a = rxd_buffer[ptrIn++] - '='; |
328 | b = rxd_buffer[ptrIn++] - '='; |
328 | b = rxd_buffer[ptrIn++] - '='; |
329 | c = rxd_buffer[ptrIn++] - '='; |
329 | c = rxd_buffer[ptrIn++] - '='; |
330 | d = rxd_buffer[ptrIn++] - '='; |
330 | d = rxd_buffer[ptrIn++] - '='; |
331 | if(ptrIn > max - 2) break; |
331 | if(ptrIn > max - 2) break; |
332 | 332 | ||
333 | x = (a << 2) | (b >> 4); |
333 | x = (a << 2) | (b >> 4); |
334 | y = ((b & 0x0f) << 4) | (c >> 2); |
334 | y = ((b & 0x0f) << 4) | (c >> 2); |
335 | z = ((c & 0x03) << 6) | d; |
335 | z = ((c & 0x03) << 6) | d; |
336 | 336 | ||
337 | if(len--) ptrOut[ptr++] = x; else break; |
337 | if(len--) ptrOut[ptr++] = x; else break; |
338 | if(len--) ptrOut[ptr++] = y; else break; |
338 | if(len--) ptrOut[ptr++] = y; else break; |
339 | if(len--) ptrOut[ptr++] = z; else break; |
339 | if(len--) ptrOut[ptr++] = z; else break; |
340 | } |
340 | } |
341 | } |
341 | } |
342 | 342 | ||
343 | 343 | ||
344 | // -------------------------------------------------------------------------- |
344 | // -------------------------------------------------------------------------- |
345 | void USART0_ProcessRxData(void) |
345 | void USART0_ProcessRxData(void) |
346 | { |
346 | { |
347 | // if data in the rxd buffer are not locked immediately return |
347 | // if data in the rxd buffer are not locked immediately return |
348 | if(!rxd_buffer_locked) return; |
348 | if(!rxd_buffer_locked) return; |
349 | 349 | ||
350 | #if !defined (USE_KILLAGREG) && !defined (USE_NAVICTRL) |
350 | #if !defined (USE_KILLAGREG) && !defined (USE_NAVICTRL) |
351 | uint16_t tmp_int_arr1[1]; // local int buffer |
351 | uint16_t tmp_int_arr1[1]; // local int buffer |
352 | #endif |
352 | #endif |
353 | uint8_t tmp_char_arr2[2]; // local char buffer |
353 | uint8_t tmp_char_arr2[2]; // local char buffer |
354 | 354 | ||
355 | 355 | ||
356 | switch(rxd_buffer[2]) |
356 | switch(rxd_buffer[2]) |
357 | { |
357 | { |
358 | #ifdef USE_MK3MAG |
358 | #ifdef USE_MK3MAG |
359 | case 'K':// Compass value |
359 | case 'K':// Compass value |
360 | Decode64((uint8_t *) &tmp_int_arr1[0], sizeof(tmp_int_arr1), 3, ReceivedBytes); |
360 | Decode64((uint8_t *) &tmp_int_arr1[0], sizeof(tmp_int_arr1), 3, ReceivedBytes); |
361 | CompassHeading = tmp_int_arr1[0]; |
361 | CompassHeading = tmp_int_arr1[0]; |
362 | CompassOffCourse = ((540 + CompassHeading - CompassCourse) % 360) - 180; |
362 | CompassOffCourse = ((540 + CompassHeading - CompassCourse) % 360) - 180; |
363 | break; |
363 | break; |
364 | #endif |
364 | #endif |
365 | case 'a':// Labels of the Analog Debug outputs |
365 | case 'a':// Labels of the Analog Debug outputs |
366 | Decode64((uint8_t *) &tmp_char_arr2[0], sizeof(tmp_char_arr2), 3, ReceivedBytes); |
366 | Decode64((uint8_t *) &tmp_char_arr2[0], sizeof(tmp_char_arr2), 3, ReceivedBytes); |
367 | RequestDebugLabel = tmp_char_arr2[0]; |
367 | RequestDebugLabel = tmp_char_arr2[0]; |
368 | PcAccess = 255; |
368 | PcAccess = 255; |
369 | break; |
369 | break; |
370 | case 'b': // extern control |
370 | case 'b': // extern control |
371 | Decode64((uint8_t *) &ExternControl,sizeof(ExternControl), 3, ReceivedBytes); |
371 | Decode64((uint8_t *) &ExternControl,sizeof(ExternControl), 3, ReceivedBytes); |
372 | RemoteButtons |= ExternControl.RemoteButtons; |
372 | RemoteButtons |= ExternControl.RemoteButtons; |
373 | ConfirmFrame = ExternControl.Frame; |
373 | ConfirmFrame = ExternControl.Frame; |
374 | break; |
374 | break; |
375 | case 'c': // extern control with debug request |
375 | case 'c': // extern control with debug request |
376 | Decode64((uint8_t *) &ExternControl,sizeof(ExternControl),3,ReceivedBytes); |
376 | Decode64((uint8_t *) &ExternControl,sizeof(ExternControl),3,ReceivedBytes); |
377 | RemoteButtons |= ExternControl.RemoteButtons; |
377 | RemoteButtons |= ExternControl.RemoteButtons; |
378 | ConfirmFrame = ExternControl.Frame; |
378 | ConfirmFrame = ExternControl.Frame; |
379 | RequestDebugData = TRUE; |
379 | RequestDebugData = TRUE; |
380 | PcAccess = 255; |
380 | PcAccess = 255; |
381 | break; |
381 | break; |
382 | case 'h':// x-1 display columns |
382 | case 'h':// x-1 display columns |
383 | Decode64((uint8_t *) &tmp_char_arr2[0],sizeof(tmp_char_arr2),3,ReceivedBytes); |
383 | Decode64((uint8_t *) &tmp_char_arr2[0],sizeof(tmp_char_arr2),3,ReceivedBytes); |
384 | RemoteButtons |= tmp_char_arr2[0]; |
384 | RemoteButtons |= tmp_char_arr2[0]; |
385 | if(tmp_char_arr2[1] == 255) RequestChannelOnly = TRUE; |
385 | if(tmp_char_arr2[1] == 255) RequestChannelOnly = TRUE; |
386 | else RequestChannelOnly = FALSE; // keine Displaydaten |
386 | else RequestChannelOnly = FALSE; // keine Displaydaten |
387 | RequestDisplay = TRUE; |
387 | RequestDisplay = TRUE; |
388 | break; |
388 | break; |
389 | case 't':// motor test |
389 | case 't':// motor test |
390 | Decode64((uint8_t *) &MotorTest[0],sizeof(MotorTest),3,ReceivedBytes); |
390 | Decode64((uint8_t *) &MotorTest[0],sizeof(MotorTest),3,ReceivedBytes); |
391 | PcAccess = 255; |
391 | PcAccess = 255; |
392 | break; |
392 | break; |
393 | case 'k':// keys from DubWise |
393 | case 'k':// keys from DubWise |
394 | Decode64((uint8_t *) &DubWiseKeys[0],sizeof(DubWiseKeys),3,ReceivedBytes); |
394 | Decode64((uint8_t *) &DubWiseKeys[0],sizeof(DubWiseKeys),3,ReceivedBytes); |
395 | ConfirmFrame = DubWiseKeys[3]; |
395 | ConfirmFrame = DubWiseKeys[3]; |
396 | PcAccess = 255; |
396 | PcAccess = 255; |
397 | break; |
397 | break; |
398 | case 'v': // get version and board release |
398 | case 'v': // get version and board release |
399 | RequestVerInfo = TRUE; |
399 | RequestVerInfo = TRUE; |
400 | break; |
400 | break; |
401 | case 'g':// get external control data |
401 | case 'g':// get external control data |
402 | RequestExternalControl = TRUE; |
402 | RequestExternalControl = TRUE; |
403 | break; |
403 | break; |
404 | case 'q':// get settings |
404 | case 'q':// get settings |
405 | Decode64((uint8_t *) &tmp_char_arr2[0],sizeof(tmp_char_arr2),3,ReceivedBytes); |
405 | Decode64((uint8_t *) &tmp_char_arr2[0],sizeof(tmp_char_arr2),3,ReceivedBytes); |
406 | while(!txd_complete); |
406 | while(!txd_complete); |
407 | if(tmp_char_arr2[0] != 0xff) |
407 | if(tmp_char_arr2[0] != 0xff) |
408 | { |
408 | { |
409 | if(tmp_char_arr2[0] > 5) tmp_char_arr2[0] = 5; // limit to 5 |
409 | if(tmp_char_arr2[0] > 5) tmp_char_arr2[0] = 5; // limit to 5 |
410 | // load requested parameter set |
410 | // load requested parameter set |
411 | ParamSet_ReadFromEEProm(tmp_char_arr2[0]); |
411 | ParamSet_ReadFromEEProm(tmp_char_arr2[0]); |
412 | SendOutData('L' + tmp_char_arr2[0] -1,MySlaveAddr,(uint8_t *) &ParamSet.ChannelAssignment[0],PARAMSET_STRUCT_LEN); |
412 | SendOutData('L' + tmp_char_arr2[0] -1,MySlaveAddr,(uint8_t *) &ParamSet.ChannelAssignment[0],PARAMSET_STRUCT_LEN); |
413 | } |
413 | } |
414 | else // send active parameter set |
414 | else // send active parameter set |
415 | SendOutData('L' + GetParamByte(PID_ACTIVE_SET)-1,MySlaveAddr,(uint8_t *) &ParamSet.ChannelAssignment[0],PARAMSET_STRUCT_LEN); |
415 | SendOutData('L' + GetParamByte(PID_ACTIVE_SET)-1,MySlaveAddr,(uint8_t *) &ParamSet.ChannelAssignment[0],PARAMSET_STRUCT_LEN); |
416 | 416 | ||
417 | break; |
417 | break; |
418 | 418 | ||
419 | case 'l': |
419 | case 'l': |
420 | case 'm': |
420 | case 'm': |
421 | case 'n': |
421 | case 'n': |
422 | case 'o': |
422 | case 'o': |
423 | case 'p': // save parameterset |
423 | case 'p': // save parameterset |
424 | Decode64((uint8_t *) &ParamSet.ChannelAssignment[0],PARAMSET_STRUCT_LEN,3,ReceivedBytes); |
424 | Decode64((uint8_t *) &ParamSet.ChannelAssignment[0],PARAMSET_STRUCT_LEN,3,ReceivedBytes); |
425 | ParamSet_WriteToEEProm(rxd_buffer[2] - 'l' + 1); |
425 | ParamSet_WriteToEEProm(rxd_buffer[2] - 'l' + 1); |
426 | TurnOver180Pitch = (int32_t) ParamSet.AngleTurnOverPitch * 2500L; |
426 | TurnOver180Nick = (int32_t) ParamSet.AngleTurnOverNick * 2500L; |
427 | TurnOver180Roll = (int32_t) ParamSet.AngleTurnOverRoll * 2500L; |
427 | TurnOver180Roll = (int32_t) ParamSet.AngleTurnOverRoll * 2500L; |
428 | Beep(GetActiveParamSet()); |
428 | Beep(GetActiveParamSet()); |
429 | break; |
429 | break; |
430 | 430 | ||
431 | 431 | ||
432 | } |
432 | } |
433 | // unlock the rxd buffer after processing |
433 | // unlock the rxd buffer after processing |
434 | rxd_buffer_locked = FALSE; |
434 | rxd_buffer_locked = FALSE; |
435 | } |
435 | } |
436 | 436 | ||
437 | //############################################################################ |
437 | //############################################################################ |
438 | //Routine für die Serielle Ausgabe |
438 | //Routine für die Serielle Ausgabe |
439 | int16_t uart_putchar (int8_t c) |
439 | int16_t uart_putchar (int8_t c) |
440 | //############################################################################ |
440 | //############################################################################ |
441 | { |
441 | { |
442 | if (c == '\n') |
442 | if (c == '\n') |
443 | uart_putchar('\r'); |
443 | uart_putchar('\r'); |
444 | // wait until previous character was send |
444 | // wait until previous character was send |
445 | loop_until_bit_is_set(UCSR0A, UDRE0); |
445 | loop_until_bit_is_set(UCSR0A, UDRE0); |
446 | //Ausgabe des Zeichens |
446 | //Ausgabe des Zeichens |
447 | UDR0 = c; |
447 | UDR0 = c; |
448 | return (0); |
448 | return (0); |
449 | } |
449 | } |
450 | 450 | ||
451 | 451 | ||
452 | //--------------------------------------------------------------------------------------------- |
452 | //--------------------------------------------------------------------------------------------- |
453 | void USART0_TransmitTxData(void) |
453 | void USART0_TransmitTxData(void) |
454 | { |
454 | { |
455 | if(!txd_complete) return; |
455 | if(!txd_complete) return; |
456 | 456 | ||
457 | if(RequestExternalControl && txd_complete) // Bei Get werden die vom PC einstellbaren Werte vom PC zurückgelesen |
457 | if(RequestExternalControl && txd_complete) // Bei Get werden die vom PC einstellbaren Werte vom PC zurückgelesen |
458 | { |
458 | { |
459 | SendOutData('G',MySlaveAddr,(uint8_t *) &ExternControl,sizeof(ExternControl)); |
459 | SendOutData('G',MySlaveAddr,(uint8_t *) &ExternControl,sizeof(ExternControl)); |
460 | RequestExternalControl = FALSE; |
460 | RequestExternalControl = FALSE; |
461 | } |
461 | } |
462 | 462 | ||
463 | #ifdef USE_MK3MAG |
463 | #ifdef USE_MK3MAG |
464 | if((CheckDelay(Compass_Timer)) && txd_complete) |
464 | if((CheckDelay(Compass_Timer)) && txd_complete) |
465 | { |
465 | { |
466 | ToMk3Mag.Attitude[0] = (int16_t) (IntegralPitch / 108); // approx. 0,1 Deg |
466 | ToMk3Mag.Attitude[0] = (int16_t) (IntegralNick / 108); // approx. 0,1 Deg |
467 | ToMk3Mag.Attitude[1] = (int16_t) (IntegralRoll / 108); // approx. 0,1 Deg |
467 | ToMk3Mag.Attitude[1] = (int16_t) (IntegralRoll / 108); // approx. 0,1 Deg |
468 | ToMk3Mag.UserParam[0] = FCParam.UserParam1; |
468 | ToMk3Mag.UserParam[0] = FCParam.UserParam1; |
469 | ToMk3Mag.UserParam[1] = FCParam.UserParam2; |
469 | ToMk3Mag.UserParam[1] = FCParam.UserParam2; |
470 | ToMk3Mag.CalState = CompassCalState; |
470 | ToMk3Mag.CalState = CompassCalState; |
471 | SendOutData('w',MySlaveAddr,(uint8_t *) &ToMk3Mag,sizeof(ToMk3Mag)); |
471 | SendOutData('w',MySlaveAddr,(uint8_t *) &ToMk3Mag,sizeof(ToMk3Mag)); |
472 | // the last state is 5 and should be send only once to avoid multiple flash writing |
472 | // the last state is 5 and should be send only once to avoid multiple flash writing |
473 | if(CompassCalState > 4) CompassCalState = 0; |
473 | if(CompassCalState > 4) CompassCalState = 0; |
474 | Compass_Timer = SetDelay(99); |
474 | Compass_Timer = SetDelay(99); |
475 | } |
475 | } |
476 | #endif |
476 | #endif |
477 | 477 | ||
478 | if((CheckDelay(Debug_Timer) || RequestDebugData) && txd_complete) |
478 | if((CheckDelay(Debug_Timer) || RequestDebugData) && txd_complete) |
479 | { |
479 | { |
480 | SendOutData('D',MySlaveAddr,(uint8_t *) &DebugOut,sizeof(DebugOut)); |
480 | SendOutData('D',MySlaveAddr,(uint8_t *) &DebugOut,sizeof(DebugOut)); |
481 | RequestDebugData = FALSE; |
481 | RequestDebugData = FALSE; |
482 | Debug_Timer = SetDelay(MIN_DEBUG_INTERVALL); |
482 | Debug_Timer = SetDelay(MIN_DEBUG_INTERVALL); |
483 | } |
483 | } |
484 | 484 | ||
485 | if(RequestDebugLabel != 255) // Texte für die Analogdaten |
485 | if(RequestDebugLabel != 255) // Texte für die Analogdaten |
486 | { |
486 | { |
487 | SendOutData('A',RequestDebugLabel + '0',(uint8_t *) ANALOG_LABEL[RequestDebugLabel],16); |
487 | SendOutData('A',RequestDebugLabel + '0',(uint8_t *) ANALOG_LABEL[RequestDebugLabel],16); |
488 | RequestDebugLabel = 255; |
488 | RequestDebugLabel = 255; |
489 | } |
489 | } |
490 | if(ConfirmFrame && txd_complete) // Datensatz ohne CRC bestätigen |
490 | if(ConfirmFrame && txd_complete) // Datensatz ohne CRC bestätigen |
491 | { |
491 | { |
492 | txd_buffer[0] = '#'; |
492 | txd_buffer[0] = '#'; |
493 | txd_buffer[1] = ConfirmFrame; |
493 | txd_buffer[1] = ConfirmFrame; |
494 | txd_buffer[2] = '\r'; |
494 | txd_buffer[2] = '\r'; |
495 | txd_complete = 0; |
495 | txd_complete = 0; |
496 | ConfirmFrame = 0; |
496 | ConfirmFrame = 0; |
497 | UDR0 = txd_buffer[0]; |
497 | UDR0 = txd_buffer[0]; |
498 | } |
498 | } |
499 | if(RequestDisplay && txd_complete) |
499 | if(RequestDisplay && txd_complete) |
500 | { |
500 | { |
501 | LCD_PrintMenu(); |
501 | LCD_PrintMenu(); |
502 | RequestDisplay = FALSE; |
502 | RequestDisplay = FALSE; |
503 | if(++RemotePollDisplayLine == 4 || RequestChannelOnly) |
503 | if(++RemotePollDisplayLine == 4 || RequestChannelOnly) |
504 | { |
504 | { |
505 | SendOutData('4',0,(uint8_t *)&PPM_in,sizeof(PPM_in)); // DisplayZeile übertragen |
505 | SendOutData('4',0,(uint8_t *)&PPM_in,sizeof(PPM_in)); // DisplayZeile übertragen |
506 | RemotePollDisplayLine = -1; |
506 | RemotePollDisplayLine = -1; |
507 | } |
507 | } |
508 | else SendOutData('0' + RemotePollDisplayLine,0,(uint8_t *)&DisplayBuff[20 * RemotePollDisplayLine],20); // DisplayZeile übertragen |
508 | else SendOutData('0' + RemotePollDisplayLine,0,(uint8_t *)&DisplayBuff[20 * RemotePollDisplayLine],20); // DisplayZeile übertragen |
509 | } |
509 | } |
510 | if(RequestVerInfo && txd_complete) |
510 | if(RequestVerInfo && txd_complete) |
511 | { |
511 | { |
512 | SendOutData('V',MySlaveAddr,(uint8_t *) &VersionInfo,sizeof(VersionInfo)); |
512 | SendOutData('V',MySlaveAddr,(uint8_t *) &VersionInfo,sizeof(VersionInfo)); |
513 | RequestVerInfo = FALSE; |
513 | RequestVerInfo = FALSE; |
514 | } |
514 | } |
515 | 515 | ||
516 | } |
516 | } |
517 | 517 | ||
518 | 518 |