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); |