| 112,7 → 112,7 |
|---|
| |
| uint16_t DebugData_Timer; |
| uint16_t Data3D_Timer; |
| uint16_t DebugData_Interval = 500; // in 1ms |
| uint16_t DebugData_Interval = 0; // in 1ms |
| uint16_t Data3D_Interval = 0; // in 1ms |
| |
| #ifdef USE_MK3MAG |
| 125,9 → 125,9 |
|---|
| "AnglePitch ", //0 |
| "AngleRoll ", |
| "AngleYaw ", |
| "GyroPitch(PID) ", |
| "GyroRoll(PID) ", |
| "GyroYaw ", //5 |
| "rc 0 ", |
| "rc 1 ", |
| "rc 2 ", //5 |
| "OffsPitch ", |
| "OffsRoll ", |
| "AttitudeControl ", |
| 264,9 → 264,9 |
|---|
| /****************************************************************/ |
| ISR(USART0_RX_vect) |
| { |
| static uint16_t crc; |
| static uint16_t checksum; |
| static uint8_t ptr_rxd_buffer = 0; |
| uint8_t crc1, crc2; |
| uint8_t checksum1, checksum2; |
| uint8_t c; |
| |
| c = UDR0; // catch the received byte |
| 277,29 → 277,29 |
|---|
| // the rxd buffer is unlocked |
| if ((ptr_rxd_buffer == 0) && (c == '#')) { // if rxd buffer is empty and syncronisation character is received |
| rxd_buffer[ptr_rxd_buffer++] = c; // copy 1st byte to buffer |
| crc = c; // init crc |
| checksum = c; // init checksum |
| } |
| #if 0 |
| else if (ptr_rxd_buffer == 1) { // handle address |
| rxd_buffer[ptr_rxd_buffer++] = c; // copy byte to rxd buffer |
| crc += c; // update crc |
| checksum += c; // update checksum |
| } |
| #endif |
| else if (ptr_rxd_buffer < RXD_BUFFER_LEN) { // collect incomming bytes |
| if (c != '\r') { // no termination character |
| rxd_buffer[ptr_rxd_buffer++] = c; // copy byte to rxd buffer |
| crc += c; // update crc |
| checksum += c; // update checksum |
| } else { // termination character was received |
| // the last 2 bytes are no subject for checksum calculation |
| // they are the checksum itself |
| crc -= rxd_buffer[ptr_rxd_buffer - 2]; |
| crc -= rxd_buffer[ptr_rxd_buffer - 1]; |
| checksum -= rxd_buffer[ptr_rxd_buffer - 2]; |
| checksum -= rxd_buffer[ptr_rxd_buffer - 1]; |
| // calculate checksum from transmitted data |
| crc %= 4096; |
| crc1 = '=' + crc / 64; |
| crc2 = '=' + crc % 64; |
| checksum %= 4096; |
| checksum1 = '=' + checksum / 64; |
| checksum2 = '=' + checksum % 64; |
| // compare checksum to transmitted checksum bytes |
| if ((crc1 == rxd_buffer[ptr_rxd_buffer - 2]) && (crc2 |
| if ((checksum1 == rxd_buffer[ptr_rxd_buffer - 2]) && (checksum2 |
| == rxd_buffer[ptr_rxd_buffer - 1])) { |
| // checksum valid |
| rxd_buffer[ptr_rxd_buffer] = '\r'; // set termination character |
| 321,14 → 321,14 |
|---|
| } |
| |
| // -------------------------------------------------------------------------- |
| void AddCRC(uint16_t datalen) { |
| uint16_t tmpCRC = 0, i; |
| void Addchecksum(uint16_t datalen) { |
| uint16_t tmpchecksum = 0, i; |
| for (i = 0; i < datalen; i++) { |
| tmpCRC += txd_buffer[i]; |
| tmpchecksum += txd_buffer[i]; |
| } |
| tmpCRC %= 4096; |
| txd_buffer[i++] = '=' + tmpCRC / 64; |
| txd_buffer[i++] = '=' + tmpCRC % 64; |
| tmpchecksum %= 4096; |
| txd_buffer[i++] = '=' + tmpchecksum / 64; |
| txd_buffer[i++] = '=' + tmpchecksum % 64; |
| txd_buffer[i++] = '\r'; |
| txd_complete = FALSE; |
| UDR0 = txd_buffer[0]; // initiates the transmittion (continued in the TXD ISR) |
| 381,7 → 381,7 |
|---|
| txd_buffer[txd_bufferIndex] = 0; |
| } |
| va_end(ap); |
| AddCRC(pt); // add checksum after data block and initates the transmission |
| Addchecksum(pt); // add checksum after data block and initates the transmission |
| } |
| */ |
| |
| 447,7 → 447,7 |
|---|
| txd_buffer[pt++] = '=' + (c & 0x3f); |
| } |
| va_end(ap); |
| AddCRC(pt); // add checksum after data block and initates the transmission |
| Addchecksum(pt); // add checksum after data block and initates the transmission |
| } |
| |
| // -------------------------------------------------------------------------- |
| 695,9 → 695,8 |
|---|
| request_DebugLabel = 0xFF; |
| } |
| |
| if (ConfirmFrame && txd_complete) { // Datensatz ohne CRC best�tigen |
| SendOutData('B', FC_ADDRESS, 1, (uint8_t*) &ConfirmFrame, |
| sizeof(ConfirmFrame)); |
| if (ConfirmFrame && txd_complete) { // Datensatz ohne checksum bestätigen |
| SendOutData('B', FC_ADDRESS, 1, (uint8_t*) &ConfirmFrame, sizeof(ConfirmFrame)); |
| ConfirmFrame = 0; |
| } |
| |
| 728,12 → 727,12 |
|---|
| |
| #ifdef USE_MK3MAG |
| if((checkDelay(Compass_Timer)) && txd_complete) { |
| ToMk3Mag.Attitude[0] = (int16_t)((10 * angle[PITCH]) / GYRO_DEG_FACTOR_PITCHROLL); // approx. 0.1 deg |
| ToMk3Mag.Attitude[1] = (int16_t)((10 * angle[ROLL]) / GYRO_DEG_FACTOR_PITCHROLL); // approx. 0.1 deg |
| ToMk3Mag.UserParam[0] = dynamicParams.userParams[0]; |
| ToMk3Mag.UserParam[1] = dynamicParams.userParams[1]; |
| ToMk3Mag.CalState = compassCalState; |
| SendOutData('w', MK3MAG_ADDRESS, 1,(uint8_t *) &ToMk3Mag,sizeof(ToMk3Mag)); |
| toMk3Mag.Attitude[0] = (int16_t)((10 * angle[PITCH]) / GYRO_DEG_FACTOR_PITCHROLL); // approx. 0.1 deg |
| toMk3Mag.Attitude[1] = (int16_t)((10 * angle[ROLL]) / GYRO_DEG_FACTOR_PITCHROLL); // approx. 0.1 deg |
| toMk3Mag.UserParam[0] = dynamicParams.userParams[0]; |
| toMk3Mag.UserParam[1] = dynamicParams.userParams[1]; |
| toMk3Mag.CalState = compassCalState; |
| SendOutData('w', MK3MAG_ADDRESS, 1,(uint8_t *) &toMk3Mag,sizeof(toMk3Mag)); |
| // the last state is 5 and should be send only once to avoid multiple flash writing |
| if(compassCalState > 4) compassCalState = 0; |
| Compass_Timer = setDelay(99); |