// Process bytes available from the stream
//
// The stream is assumed to contain only our custom message. If it
// contains other messages, and those messages contain the preamble bytes,
// it is possible for this code to become de-synchronised. Without
// buffering the entire message and re-processing it from the top,
// this is unavoidable.
//
// The lack of a standard header length field makes it impossible to skip
// unrecognised messages.
//
bool
AP_GPS_MTK::read(void)
{
uint8_t data;
int16_t numc;
bool parsed = false;
numc = port->available();
for (int16_t i = 0; i < numc; i++) { // Process bytes received
// read the next byte
data = port->read();
restart:
switch(_step) {
// Message preamble, class, ID detection
//
// If we fail to match any of the expected bytes, we
// reset the state machine and re-consider the failed
// byte as the first byte of the preamble. This
// improves our chances of recovering from a mismatch
// and makes it less likely that we will be fooled by
// the preamble appearing as data in some other message.
//
case 0:
if(PREAMBLE1 == data)
_step++;
break;
case 1:
if (PREAMBLE2 == data) {
_step++;
break;
}
_step = 0;
goto restart;
case 2:
if (MESSAGE_CLASS == data) {
_step++;
_ck_b = _ck_a = data; // reset the checksum accumulators
} else {
_step = 0; // reset and wait for a message of the right class
goto restart;
}
break;
case 3:
if (MESSAGE_ID == data) {
_step++;
_ck_b += (_ck_a += data);
_payload_counter = 0;
} else {
_step = 0;
goto restart;
}
break;
// Receive message data
//
case 4:
_buffer.bytes[_payload_counter++] = data;
_ck_b += (_ck_a += data);
if (_payload_counter == sizeof(_buffer))
_step++;
break;
// Checksum and message processing
//
case 5:
_step++;
if (_ck_a != data) {
_step = 0;
}
break;
case 6:
_step = 0;
if (_ck_b != data) {
break;
}
// set fix type
if (_buffer.msg.fix_type == FIX_3D) {
state.status = AP_GPS::GPS_OK_FIX_3D;
}else if (_buffer.msg.fix_type == FIX_2D) {
state.status = AP_GPS::GPS_OK_FIX_2D;
}else{
state.status = AP_GPS::NO_FIX;
}
state.location.lat = swap_int32(_buffer.msg.latitude) * 10;
state.location.lng = swap_int32(_buffer.msg.longitude) * 10;
state.location.alt = swap_int32(_buffer.msg.altitude);
state.ground_speed = swap_int32(_buffer.msg.ground_speed) * 0.01f;
state.ground_course_cd = swap_int32(_buffer.msg.ground_course) / 10000;
state.num_sats = _buffer.msg.satellites;
if (state.status >= AP_GPS::GPS_OK_FIX_2D) {
make_gps_time(0, swap_int32(_buffer.msg.utc_time)*10);
}
// we don't change _last_gps_time as we don't know the
// full date
fill_3d_velocity();
parsed = true;
}
}
return parsed;
}
bool apm::AP_GPS_GSOF::process_message(void)
{
//http://www.trimble.com/OEM_ReceiverHelp/V4.81/en/default.html#welcome.html
if (gsof_msg.packettype == 0x40) { // GSOF
int valid = 0;
// want 1 2 8 9 12
for (uint32_t a = 3; a < gsof_msg.length; a++) {
uint8_t output_type = gsof_msg.data[a];
a++;
uint8_t output_length = gsof_msg.data[a];
a++;
//Debug("GSOF type: " + output_type + " len: " + output_length);
if (output_type == 1) { // pos time
gps.state.time_week_ms = SwapUint32(gsof_msg.data, a);
gps.state.time_week = SwapUint16(gsof_msg.data, a + 4);
gps.state.num_sats = gsof_msg.data[a + 6];
uint8_t posf1 = gsof_msg.data[a + 7];
uint8_t posf2 = gsof_msg.data[a + 8];
//Debug("POSTIME: " + posf1 + " " + posf2);
if ((posf1 & 1) == 1) {
gps.state.status = gps_t::FIX_3D;
if ((posf2 & 1) == 1) {
gps.state.status = gps_t::FIX_3D_DGPS;
if ((posf2 & 4) == 4) {
gps.state.status = gps_t::FIX_3D_RTK;
}
}
} else {
gps.state.status = gps_t::NO_FIX;
}
valid++;
} else if (output_type == 2) { // position
gps.state.location.lat = gps_t::lat_lon_type {RAD_TO_DEG_DOUBLE * (SwapDouble(gsof_msg.data, a)) * 1e7}; // deg1e7
gps.state.location.lon = gps_t::lat_lon_type {RAD_TO_DEG_DOUBLE * (SwapDouble(gsof_msg.data, a + 8)) * 1e7}; // deg1e7
gps.state.location.alt = gps_t::altitude_type {SwapDouble(gsof_msg.data, a + 16) * 1e2}; //cm
gps.state.last_gps_time_ms = gps.state.time_week_ms;
valid++;
} else if (output_type == 8) { // velocity
uint8_t vflag = gsof_msg.data[a];
if ((vflag & 1) == 1) {
gps.state.ground_speed = SwapFloat(gsof_msg.data, a + 1);
gps.state.ground_course_cd = (int32_t)(ToDeg(SwapFloat(gsof_msg.data, a + 5)) * 100);
fill_3d_velocity();
gps.state.velocity.z = gps_t::velocity_type {-SwapFloat(gsof_msg.data, a + 9)};
gps.state.have_vertical_velocity = true;
}
valid++;
} else if (output_type == 9) { //dop
gps.state.hdop = (uint16_t)(SwapFloat(gsof_msg.data, a + 4) * 100);
valid++;
} else if (output_type == 12) { // position sigma
gps.state.horizontal_accuracy = (SwapFloat(gsof_msg.data, a + 4) + SwapFloat(gsof_msg.data, a + 8)) / 2;
gps.state.vertical_accuracy = SwapFloat(gsof_msg.data, a + 16);
gps.state.have_horizontal_accuracy = true;
gps.state.have_vertical_accuracy = true;
valid++;
}
a += output_length-1u;
}
if (valid == 5) {
return true;
} else {
gps.state.status = gps_t::NO_FIX;
}
}
return false;
}
bool
AP_GPS_GSOF::process_message(void)
{
//http://www.trimble.com/OEM_ReceiverHelp/V4.81/en/default.html#welcome.html
if (gsof_msg.packettype == 0x40) { // GSOF
#if gsof_DEBUGGING
uint8_t trans_number = gsof_msg.data[0];
uint8_t pageidx = gsof_msg.data[1];
uint8_t maxpageidx = gsof_msg.data[2];
Debug("GSOF page: %u of %u (trans_number=%u)",
pageidx, maxpageidx, trans_number);
#endif
int valid = 0;
// want 1 2 8 9 12
for (uint32_t a = 3; a < gsof_msg.length; a++)
{
uint8_t output_type = gsof_msg.data[a];
a++;
uint8_t output_length = gsof_msg.data[a];
a++;
//Debug("GSOF type: " + output_type + " len: " + output_length);
if (output_type == 1) // pos time
{
state.time_week_ms = SwapUint32(gsof_msg.data, a);
state.time_week = SwapUint16(gsof_msg.data, a + 4);
state.num_sats = gsof_msg.data[a + 6];
uint8_t posf1 = gsof_msg.data[a + 7];
uint8_t posf2 = gsof_msg.data[a + 8];
//Debug("POSTIME: " + posf1 + " " + posf2);
if ((posf1 & 1)) { // New position
state.status = AP_GPS::GPS_OK_FIX_3D;
if ((posf2 & 1)) { // Differential position
state.status = AP_GPS::GPS_OK_FIX_3D_DGPS;
if (posf2 & 2) { // Differential position method
if (posf2 & 4) {// Differential position method
state.status = AP_GPS::GPS_OK_FIX_3D_RTK_FIXED;
} else {
state.status = AP_GPS::GPS_OK_FIX_3D_RTK_FLOAT;
}
}
}
} else {
state.status = AP_GPS::NO_FIX;
}
valid++;
}
else if (output_type == 2) // position
{
state.location.lat = (int32_t)(RAD_TO_DEG_DOUBLE * (SwapDouble(gsof_msg.data, a)) * (double)1e7);
state.location.lng = (int32_t)(RAD_TO_DEG_DOUBLE * (SwapDouble(gsof_msg.data, a + 8)) * (double)1e7);
state.location.alt = (int32_t)(SwapDouble(gsof_msg.data, a + 16) * 100);
state.last_gps_time_ms = AP_HAL::millis();
valid++;
}
else if (output_type == 8) // velocity
{
uint8_t vflag = gsof_msg.data[a];
if ((vflag & 1) == 1)
{
state.ground_speed = SwapFloat(gsof_msg.data, a + 1);
state.ground_course = degrees(SwapFloat(gsof_msg.data, a + 5));
fill_3d_velocity();
state.velocity.z = -SwapFloat(gsof_msg.data, a + 9);
state.have_vertical_velocity = true;
}
valid++;
}
else if (output_type == 9) //dop
{
state.hdop = (uint16_t)(SwapFloat(gsof_msg.data, a + 4) * 100);
valid++;
}
else if (output_type == 12) // position sigma
{
state.horizontal_accuracy = (SwapFloat(gsof_msg.data, a + 4) + SwapFloat(gsof_msg.data, a + 8)) / 2;
state.vertical_accuracy = SwapFloat(gsof_msg.data, a + 16);
state.have_horizontal_accuracy = true;
state.have_vertical_accuracy = true;
valid++;
}
a += output_length-1u;
}
if (valid == 5) {
return true;
} else {
state.status = AP_GPS::NO_FIX;
}
}
return false;
}
bool
AP_GPS_NOVA::process_message(void)
{
uint16_t messageid = nova_msg.header.nova_headeru.messageid;
Debug("NOVA process_message messid=%u\n",messageid);
if (messageid == 42) // bestpos
{
const bestpos &bestposu = nova_msg.data.bestposu;
state.time_week = nova_msg.header.nova_headeru.week;
state.time_week_ms = (uint32_t) nova_msg.header.nova_headeru.tow;
state.last_gps_time_ms = state.time_week_ms;
state.location.lat = (int32_t) (bestposu.lat*1e7);
state.location.lng = (int32_t) (bestposu.lng*1e7);
state.location.alt = (int32_t) (bestposu.hgt*1e2);
state.num_sats = bestposu.svsused;
state.horizontal_accuracy = (float) ((bestposu.latsdev + bestposu.lngsdev)/2);
state.vertical_accuracy = (float) bestposu.hgtsdev;
state.have_horizontal_accuracy = true;
state.have_vertical_accuracy = true;
if (bestposu.solstat == 0) // have a solution
{
switch (bestposu.postype)
{
case 16:
state.status = AP_GPS::GPS_OK_FIX_3D;
break;
case 17: // psrdiff
case 18: // waas
case 20: // omnistar
case 68: // ppp_converg
case 69: // ppp
state.status = AP_GPS::GPS_OK_FIX_3D_DGPS;
break;
case 32: // l1 float
case 33: // iono float
case 34: // narrow float
state.status = AP_GPS::GPS_OK_FIX_3D_RTK_FLOAT;
break;
case 48: // l1 int
case 50: // narrow int
state.status = AP_GPS::GPS_OK_FIX_3D_RTK_FIXED;
break;
case 0: // NONE
case 1: // FIXEDPOS
case 2: // FIXEDHEIGHT
default:
state.status = AP_GPS::NO_FIX;
break;
}
}
else
{
state.status = AP_GPS::NO_FIX;
}
_new_position = true;
}
if (messageid == 99) // bestvel
{
const bestvel &bestvelu = nova_msg.data.bestvelu;
state.ground_speed = (float) bestvelu.horspd;
state.ground_course = (float) bestvelu.trkgnd;
fill_3d_velocity();
state.velocity.z = -(float) bestvelu.vertspd;
state.have_vertical_velocity = true;
_last_vel_time = (uint32_t) nova_msg.header.nova_headeru.tow;
_new_speed = true;
}
if (messageid == 174) // psrdop
{
const psrdop &psrdopu = nova_msg.data.psrdopu;
state.hdop = (uint16_t) (psrdopu.hdop*100);
state.vdop = (uint16_t) (psrdopu.htdop*100);
return false;
}
// ensure out position and velocity stay insync
if (_new_position && _new_speed && _last_vel_time == state.last_gps_time_ms) {
_new_speed = _new_position = false;
return true;
}
return false;
}