Rev 1967 | Rev 1970 | Go to most recent revision | Show entire file | Regard 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 | } |