Rev 1967 | Rev 1970 | Go to most recent revision | Show entire file | Ignore whitespace | Details | Blame | Last modification | View Log | RSS feed
| Rev 1967 | Rev 1969 | ||
|---|---|---|---|
| Line 110... | Line 110... | ||
| 110 | Data3D_t Data3D; |
110 | Data3D_t Data3D; |
| 111 | UART_VersionInfo_t UART_VersionInfo; |
111 | UART_VersionInfo_t UART_VersionInfo; |
| Line 112... | Line 112... | ||
| 112 | 112 | ||
| 113 | uint16_t DebugData_Timer; |
113 | uint16_t DebugData_Timer; |
| 114 | uint16_t Data3D_Timer; |
114 | uint16_t Data3D_Timer; |
| 115 | uint16_t DebugData_Interval = 500; // in 1ms |
115 | uint16_t DebugData_Interval = 0; // in 1ms |
| Line 116... | Line 116... | ||
| 116 | uint16_t Data3D_Interval = 0; // in 1ms |
116 | uint16_t Data3D_Interval = 0; // in 1ms |
| 117 | 117 | ||
| 118 | #ifdef USE_MK3MAG |
118 | #ifdef USE_MK3MAG |
| Line 123... | Line 123... | ||
| 123 | const prog_uint8_t ANALOG_LABEL[32][16] = { |
123 | const prog_uint8_t ANALOG_LABEL[32][16] = { |
| 124 | //1234567890123456 |
124 | //1234567890123456 |
| 125 | "AnglePitch ", //0 |
125 | "AnglePitch ", //0 |
| 126 | "AngleRoll ", |
126 | "AngleRoll ", |
| 127 | "AngleYaw ", |
127 | "AngleYaw ", |
| 128 | "GyroPitch(PID) ", |
128 | "rc 0 ", |
| 129 | "GyroRoll(PID) ", |
129 | "rc 1 ", |
| 130 | "GyroYaw ", //5 |
130 | "rc 2 ", //5 |
| 131 | "OffsPitch ", |
131 | "OffsPitch ", |
| 132 | "OffsRoll ", |
132 | "OffsRoll ", |
| 133 | "AttitudeControl ", |
133 | "AttitudeControl ", |
| 134 | "AccPitch (angle)", |
134 | "AccPitch (angle)", |
| 135 | "AccRoll (angle) ", //10 |
135 | "AccRoll (angle) ", //10 |
| Line 262... | Line 262... | ||
| 262 | /****************************************************************/ |
262 | /****************************************************************/ |
| 263 | /* USART0 receiver ISR */ |
263 | /* USART0 receiver ISR */ |
| 264 | /****************************************************************/ |
264 | /****************************************************************/ |
| 265 | ISR(USART0_RX_vect) |
265 | ISR(USART0_RX_vect) |
| 266 | { |
266 | { |
| 267 | static uint16_t crc; |
267 | static uint16_t checksum; |
| 268 | static uint8_t ptr_rxd_buffer = 0; |
268 | static uint8_t ptr_rxd_buffer = 0; |
| 269 | uint8_t crc1, crc2; |
269 | uint8_t checksum1, checksum2; |
| 270 | uint8_t c; |
270 | uint8_t c; |
| Line 271... | Line 271... | ||
| 271 | 271 | ||
| Line 272... | Line 272... | ||
| 272 | c = UDR0; // catch the received byte |
272 | c = UDR0; // catch the received byte |
| 273 | 273 | ||
| Line 274... | Line 274... | ||
| 274 | if (rxd_buffer_locked) |
274 | if (rxd_buffer_locked) |
| 275 | return; // if rxd buffer is locked immediately return |
275 | return; // if rxd buffer is locked immediately return |
| 276 | 276 | ||
| 277 | // the rxd buffer is unlocked |
277 | // the rxd buffer is unlocked |
| 278 | if ((ptr_rxd_buffer == 0) && (c == '#')) { // if rxd buffer is empty and syncronisation character is received |
278 | if ((ptr_rxd_buffer == 0) && (c == '#')) { // if rxd buffer is empty and syncronisation character is received |
| 279 | rxd_buffer[ptr_rxd_buffer++] = c; // copy 1st byte to buffer |
279 | rxd_buffer[ptr_rxd_buffer++] = c; // copy 1st byte to buffer |
| 280 | crc = c; // init crc |
280 | checksum = c; // init checksum |
| 281 | } |
281 | } |
| 282 | #if 0 |
282 | #if 0 |
| 283 | else if (ptr_rxd_buffer == 1) { // handle address |
283 | else if (ptr_rxd_buffer == 1) { // handle address |
| 284 | rxd_buffer[ptr_rxd_buffer++] = c; // copy byte to rxd buffer |
284 | rxd_buffer[ptr_rxd_buffer++] = c; // copy byte to rxd buffer |
| 285 | crc += c; // update crc |
285 | checksum += c; // update checksum |
| 286 | } |
286 | } |
| 287 | #endif |
287 | #endif |
| 288 | else if (ptr_rxd_buffer < RXD_BUFFER_LEN) { // collect incomming bytes |
288 | else if (ptr_rxd_buffer < RXD_BUFFER_LEN) { // collect incomming bytes |
| 289 | if (c != '\r') { // no termination character |
289 | if (c != '\r') { // no termination character |
| 290 | rxd_buffer[ptr_rxd_buffer++] = c; // copy byte to rxd buffer |
290 | rxd_buffer[ptr_rxd_buffer++] = c; // copy byte to rxd buffer |
| 291 | crc += c; // update crc |
291 | checksum += c; // update checksum |
| 292 | } else { // termination character was received |
292 | } else { // termination character was received |
| 293 | // the last 2 bytes are no subject for checksum calculation |
293 | // the last 2 bytes are no subject for checksum calculation |
| 294 | // they are the checksum itself |
294 | // they are the checksum itself |
| 295 | crc -= rxd_buffer[ptr_rxd_buffer - 2]; |
295 | checksum -= rxd_buffer[ptr_rxd_buffer - 2]; |
| 296 | crc -= rxd_buffer[ptr_rxd_buffer - 1]; |
296 | checksum -= rxd_buffer[ptr_rxd_buffer - 1]; |
| 297 | // calculate checksum from transmitted data |
297 | // calculate checksum from transmitted data |
| 298 | crc %= 4096; |
298 | checksum %= 4096; |
| 299 | crc1 = '=' + crc / 64; |
299 | checksum1 = '=' + checksum / 64; |
| 300 | crc2 = '=' + crc % 64; |
300 | checksum2 = '=' + checksum % 64; |
| 301 | // compare checksum to transmitted checksum bytes |
301 | // compare checksum to transmitted checksum bytes |
| 302 | if ((crc1 == rxd_buffer[ptr_rxd_buffer - 2]) && (crc2 |
302 | if ((checksum1 == rxd_buffer[ptr_rxd_buffer - 2]) && (checksum2 |
| 303 | == rxd_buffer[ptr_rxd_buffer - 1])) { |
303 | == rxd_buffer[ptr_rxd_buffer - 1])) { |
| 304 | // checksum valid |
304 | // checksum valid |
| Line 319... | Line 319... | ||
| 319 | rxd_buffer_locked = FALSE; // unlock rxd buffer |
319 | rxd_buffer_locked = FALSE; // unlock rxd buffer |
| 320 | } |
320 | } |
| 321 | } |
321 | } |
| Line 322... | Line 322... | ||
| 322 | 322 | ||
| 323 | // -------------------------------------------------------------------------- |
323 | // -------------------------------------------------------------------------- |
| 324 | void AddCRC(uint16_t datalen) { |
324 | void Addchecksum(uint16_t datalen) { |
| 325 | uint16_t tmpCRC = 0, i; |
325 | uint16_t tmpchecksum = 0, i; |
| 326 | for (i = 0; i < datalen; i++) { |
326 | for (i = 0; i < datalen; i++) { |
| 327 | tmpCRC += txd_buffer[i]; |
327 | tmpchecksum += txd_buffer[i]; |
| 328 | } |
328 | } |
| 329 | tmpCRC %= 4096; |
329 | tmpchecksum %= 4096; |
| 330 | txd_buffer[i++] = '=' + tmpCRC / 64; |
330 | txd_buffer[i++] = '=' + tmpchecksum / 64; |
| 331 | txd_buffer[i++] = '=' + tmpCRC % 64; |
331 | txd_buffer[i++] = '=' + tmpchecksum % 64; |
| 332 | txd_buffer[i++] = '\r'; |
332 | txd_buffer[i++] = '\r'; |
| 333 | txd_complete = FALSE; |
333 | txd_complete = FALSE; |
| 334 | UDR0 = txd_buffer[0]; // initiates the transmittion (continued in the TXD ISR) |
334 | UDR0 = txd_buffer[0]; // initiates the transmittion (continued in the TXD ISR) |
| Line 379... | Line 379... | ||
| 379 | // We need to stuff with zero bytes at the end. |
379 | // We need to stuff with zero bytes at the end. |
| 380 | txd_buffer[txd_bufferIndex++] &= 0b00111100; |
380 | txd_buffer[txd_bufferIndex++] &= 0b00111100; |
| 381 | txd_buffer[txd_bufferIndex] = 0; |
381 | txd_buffer[txd_bufferIndex] = 0; |
| 382 | } |
382 | } |
| 383 | va_end(ap); |
383 | va_end(ap); |
| 384 | AddCRC(pt); // add checksum after data block and initates the transmission |
384 | Addchecksum(pt); // add checksum after data block and initates the transmission |
| 385 | } |
385 | } |
| 386 | */ |
386 | */ |
| Line 387... | Line 387... | ||
| 387 | 387 | ||
| 388 | void SendOutData(uint8_t cmd, uint8_t addr, uint8_t numofbuffers, ...) { // uint8_t *pdata, uint8_t len, ... |
388 | void SendOutData(uint8_t cmd, uint8_t addr, uint8_t numofbuffers, ...) { // uint8_t *pdata, uint8_t len, ... |
| Line 445... | Line 445... | ||
| 445 | txd_buffer[pt++] = '=' + (((a & 0x03) << 4) | ((b & 0xf0) >> 4)); |
445 | txd_buffer[pt++] = '=' + (((a & 0x03) << 4) | ((b & 0xf0) >> 4)); |
| 446 | txd_buffer[pt++] = '=' + (((b & 0x0f) << 2) | ((c & 0xc0) >> 6)); |
446 | txd_buffer[pt++] = '=' + (((b & 0x0f) << 2) | ((c & 0xc0) >> 6)); |
| 447 | txd_buffer[pt++] = '=' + (c & 0x3f); |
447 | txd_buffer[pt++] = '=' + (c & 0x3f); |
| 448 | } |
448 | } |
| 449 | va_end(ap); |
449 | va_end(ap); |
| 450 | AddCRC(pt); // add checksum after data block and initates the transmission |
450 | Addchecksum(pt); // add checksum after data block and initates the transmission |
| 451 | } |
451 | } |
| Line 452... | Line 452... | ||
| 452 | 452 | ||
| 453 | // -------------------------------------------------------------------------- |
453 | // -------------------------------------------------------------------------- |
| 454 | void Decode64(void) { |
454 | void Decode64(void) { |
| Line 693... | Line 693... | ||
| 693 | SendOutData('A', FC_ADDRESS, 2, (uint8_t *) &request_DebugLabel, |
693 | SendOutData('A', FC_ADDRESS, 2, (uint8_t *) &request_DebugLabel, |
| 694 | sizeof(request_DebugLabel), label, 16); |
694 | sizeof(request_DebugLabel), label, 16); |
| 695 | request_DebugLabel = 0xFF; |
695 | request_DebugLabel = 0xFF; |
| 696 | } |
696 | } |
| Line 697... | Line 697... | ||
| 697 | 697 | ||
| 698 | if (ConfirmFrame && txd_complete) { // Datensatz ohne CRC best�tigen |
698 | if (ConfirmFrame && txd_complete) { // Datensatz ohne checksum bestätigen |
| 699 | SendOutData('B', FC_ADDRESS, 1, (uint8_t*) &ConfirmFrame, |
- | |
| 700 | sizeof(ConfirmFrame)); |
699 | SendOutData('B', FC_ADDRESS, 1, (uint8_t*) &ConfirmFrame, sizeof(ConfirmFrame)); |
| 701 | ConfirmFrame = 0; |
700 | ConfirmFrame = 0; |
| Line 702... | Line 701... | ||
| 702 | } |
701 | } |
| 703 | 702 | ||
| Line 726... | Line 725... | ||
| 726 | request_ExternalControl = FALSE; |
725 | request_ExternalControl = FALSE; |
| 727 | } |
726 | } |
| Line 728... | Line 727... | ||
| 728 | 727 | ||
| 729 | #ifdef USE_MK3MAG |
728 | #ifdef USE_MK3MAG |
| 730 | if((checkDelay(Compass_Timer)) && txd_complete) { |
729 | if((checkDelay(Compass_Timer)) && txd_complete) { |
| 731 | ToMk3Mag.Attitude[0] = (int16_t)((10 * angle[PITCH]) / GYRO_DEG_FACTOR_PITCHROLL); // approx. 0.1 deg |
730 | toMk3Mag.Attitude[0] = (int16_t)((10 * angle[PITCH]) / GYRO_DEG_FACTOR_PITCHROLL); // approx. 0.1 deg |
| 732 | ToMk3Mag.Attitude[1] = (int16_t)((10 * angle[ROLL]) / GYRO_DEG_FACTOR_PITCHROLL); // approx. 0.1 deg |
731 | toMk3Mag.Attitude[1] = (int16_t)((10 * angle[ROLL]) / GYRO_DEG_FACTOR_PITCHROLL); // approx. 0.1 deg |
| 733 | ToMk3Mag.UserParam[0] = dynamicParams.userParams[0]; |
732 | toMk3Mag.UserParam[0] = dynamicParams.userParams[0]; |
| 734 | ToMk3Mag.UserParam[1] = dynamicParams.userParams[1]; |
733 | toMk3Mag.UserParam[1] = dynamicParams.userParams[1]; |
| 735 | ToMk3Mag.CalState = compassCalState; |
734 | toMk3Mag.CalState = compassCalState; |
| 736 | SendOutData('w', MK3MAG_ADDRESS, 1,(uint8_t *) &ToMk3Mag,sizeof(ToMk3Mag)); |
735 | SendOutData('w', MK3MAG_ADDRESS, 1,(uint8_t *) &toMk3Mag,sizeof(toMk3Mag)); |
| 737 | // the last state is 5 and should be send only once to avoid multiple flash writing |
736 | // the last state is 5 and should be send only once to avoid multiple flash writing |
| 738 | if(compassCalState > 4) compassCalState = 0; |
737 | if(compassCalState > 4) compassCalState = 0; |
| 739 | Compass_Timer = setDelay(99); |
738 | Compass_Timer = setDelay(99); |
| 740 | } |
739 | } |