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