diff --git a/libraries/AP_GPS/AP_GPS_NOVA.cpp b/libraries/AP_GPS/AP_GPS_NOVA.cpp index 5596d18176..a1fc661c10 100644 --- a/libraries/AP_GPS/AP_GPS_NOVA.cpp +++ b/libraries/AP_GPS/AP_GPS_NOVA.cpp @@ -44,6 +44,10 @@ AP_GPS_NOVA::AP_GPS_NOVA(AP_GPS &_gps, AP_GPS::GPS_State &_state, { nova_msg.nova_state = nova_msg_parser::PREAMBLE1; + nova_msg.header.data[0] = NOVA_PREAMBLE1; + nova_msg.header.data[1] = NOVA_PREAMBLE2; + nova_msg.header.data[2] = NOVA_PREAMBLE3; + const char *init_str = _initialisation_blob[0]; const char *init_str1 = _initialisation_blob[1]; @@ -98,30 +102,21 @@ AP_GPS_NOVA::parse(uint8_t temp) nova_msg.read = 0; break; case nova_msg_parser::PREAMBLE2: - if (temp == NOVA_PREAMBLE2) - { + if (temp == NOVA_PREAMBLE2) { nova_msg.nova_state = nova_msg_parser::PREAMBLE3; - } - else - { + } else { nova_msg.nova_state = nova_msg_parser::PREAMBLE1; } break; case nova_msg_parser::PREAMBLE3: - if (temp == NOVA_PREAMBLE3) - { + if (temp == NOVA_PREAMBLE3) { nova_msg.nova_state = nova_msg_parser::HEADERLENGTH; - } - else - { + } else { nova_msg.nova_state = nova_msg_parser::PREAMBLE1; } break; case nova_msg_parser::HEADERLENGTH: Debug("NOVA HEADERLENGTH\n"); - nova_msg.header.data[0] = NOVA_PREAMBLE1; - nova_msg.header.data[1] = NOVA_PREAMBLE2; - nova_msg.header.data[2] = NOVA_PREAMBLE3; nova_msg.header.data[3] = temp; nova_msg.header.nova_headeru.headerlength = temp; nova_msg.nova_state = nova_msg_parser::HEADERDATA; @@ -135,8 +130,7 @@ AP_GPS_NOVA::parse(uint8_t temp) } nova_msg.header.data[nova_msg.read] = temp; nova_msg.read++; - if (nova_msg.read >= nova_msg.header.nova_headeru.headerlength) - { + if (nova_msg.read >= nova_msg.header.nova_headeru.headerlength) { nova_msg.nova_state = nova_msg_parser::DATA; } break; @@ -148,8 +142,7 @@ AP_GPS_NOVA::parse(uint8_t temp) } nova_msg.data.bytes[nova_msg.read - nova_msg.header.nova_headeru.headerlength] = temp; nova_msg.read++; - if (nova_msg.read >= (nova_msg.header.nova_headeru.messagelength + nova_msg.header.nova_headeru.headerlength)) - { + if (nova_msg.read >= (nova_msg.header.nova_headeru.messagelength + nova_msg.header.nova_headeru.headerlength)) { Debug("NOVA DATA exit\n"); nova_msg.nova_state = nova_msg_parser::CRC1; } @@ -173,12 +166,9 @@ AP_GPS_NOVA::parse(uint8_t temp) uint32_t crc = CalculateBlockCRC32((uint32_t)nova_msg.header.nova_headeru.headerlength, (uint8_t *)&nova_msg.header.data, (uint32_t)0); crc = CalculateBlockCRC32((uint32_t)nova_msg.header.nova_headeru.messagelength, (uint8_t *)&nova_msg.data, crc); - if (nova_msg.crc == crc) - { + if (nova_msg.crc == crc) { return process_message(); - } - else - { + } else { Debug("crc failed"); crc_error_counter++; } @@ -248,9 +238,7 @@ AP_GPS_NOVA::process_message(void) state.status = AP_GPS::NO_FIX; break; } - } - else - { + } else { state.status = AP_GPS::NO_FIX; } @@ -295,8 +283,7 @@ uint32_t AP_GPS_NOVA::CRC32Value(uint32_t icrc) { int i; uint32_t crc = icrc; - for ( i = 8 ; i > 0; i-- ) - { + for ( i = 8 ; i > 0; i-- ) { if ( crc & 1 ) crc = ( crc >> 1 ) ^ CRC32_POLYNOMIAL; else @@ -307,8 +294,7 @@ uint32_t AP_GPS_NOVA::CRC32Value(uint32_t icrc) uint32_t AP_GPS_NOVA::CalculateBlockCRC32(uint32_t length, uint8_t *buffer, uint32_t crc) { - while ( length-- != 0 ) - { + while ( length-- != 0 ) { crc = ((crc >> 8) & 0x00FFFFFFL) ^ (CRC32Value(((uint32_t) crc ^ *buffer++) & 0xff)); } return( crc );