diff --git a/libraries/Desktop/support/sitl_gps.cpp b/libraries/Desktop/support/sitl_gps.cpp index 223cd1202a..756b3dce2a 100644 --- a/libraries/Desktop/support/sitl_gps.cpp +++ b/libraries/Desktop/support/sitl_gps.cpp @@ -131,9 +131,29 @@ void sitl_update_gps(double latitude, double longitude, float altitude, uint32_t speed_accuracy; uint32_t heading_accuracy; } velned; + struct ubx_nav_solution { + uint32_t time; + int32_t time_nsec; + int16_t week; + uint8_t fix_type; + uint8_t fix_status; + int32_t ecef_x; + int32_t ecef_y; + int32_t ecef_z; + uint32_t position_accuracy_3d; + int32_t ecef_x_velocity; + int32_t ecef_y_velocity; + int32_t ecef_z_velocity; + uint32_t speed_accuracy; + uint16_t position_DOP; + uint8_t res; + uint8_t satellites; + uint32_t res2; + } sol; const uint8_t MSG_POSLLH = 0x2; const uint8_t MSG_STATUS = 0x3; const uint8_t MSG_VELNED = 0x12; + const uint8_t MSG_SOL = 0x6; struct gps_data d; // 5Hz, to match the real UBlox config in APM @@ -195,6 +215,11 @@ void sitl_update_gps(double latitude, double longitude, float altitude, velned.speed_accuracy = 2; velned.heading_accuracy = 4; + memset(&sol, 0, sizeof(sol)); + sol.fix_type = d.have_lock?3:0; + sol.fix_status = 221; + sol.satellites = d.have_lock?10:3; + if (gps_state.gps_fd == 0) { return; } @@ -202,4 +227,5 @@ void sitl_update_gps(double latitude, double longitude, float altitude, gps_send(MSG_POSLLH, (uint8_t*)&pos, sizeof(pos)); gps_send(MSG_STATUS, (uint8_t*)&status, sizeof(status)); gps_send(MSG_VELNED, (uint8_t*)&velned, sizeof(velned)); + gps_send(MSG_SOL, (uint8_t*)&sol, sizeof(sol)); }