Mirror
/
ardupilot
mirror of https://github.com/ArduPilot/ardupilot
5
0
Fork 0
Code Issues Packages Projects Releases Wiki Activity

AP_GPS: Isolate the header to be only set once 

Also wrap up some common whitespace issues
This commit is contained in:
Michael du Breuil 2021-06-11 13:26:50 -07:00 committed by Andrew Tridgell
parent 9c1ca781f6
commit eedcb9aded
1 changed files with 15 additions and 29 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

44
libraries/AP_GPS/AP_GPS_NOVA.cpp
View File

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