SITL: add SIM_GPS_FIXTYPE

CarbonixUAV · Jan 2, 2025 · 0369230 · 0369230
1 parent 6fa8935
commit 0369230
Show file tree

Hide file tree

Showing 7 changed files with 60 additions and 6 deletions.
diff --git a/libraries/AP_HAL_ChibiOS/hwdef/CarbonixCommon/sitl_params/ottano-headless.parm b/libraries/AP_HAL_ChibiOS/hwdef/CarbonixCommon/sitl_params/ottano-headless.parm
@@ -80,10 +80,12 @@ SERVO3_TRIM,1000
 SIM_ARSPD_RATIO,1.6
 SIM_ARSPD2_RATIO,1.6
 SIM_ESC_TELEM,0 # Disable the built-in ESC telemetry simulation (we have lua do this instead)
+SIM_GPS_FIXTYPE,3
 SIM_GPS_NUMSATS,29
 SIM_GPS_POS_X,-0.135
 SIM_GPS_POS_Y,-1.05
 SIM_GPS2_DISABLE,0
+SIM_GPS2_FIXTYPE,3
 SIM_GPS2_NUMSATS,27
 SIM_GPS2_POS_X,-0.135
 SIM_GPS2_POS_Y,1.05

diff --git a/libraries/AP_HAL_ChibiOS/hwdef/CarbonixCommon/sitl_params/volanti-headless.parm b/libraries/AP_HAL_ChibiOS/hwdef/CarbonixCommon/sitl_params/volanti-headless.parm
@@ -68,11 +68,13 @@ SERVO3_TRIM,1000
 SIM_ARSPD_RATIO,1.6
 SIM_ARSPD2_RATIO,1.6
 SIM_ESC_TELEM,0 # Disable the built-in ESC telemetry simulation (we have lua do this instead)
+SIM_GPS_FIXTYPE,3
 SIM_GPS_NUMSATS,29
 SIM_GPS_POS_X,-0.12
 SIM_GPS_POS_Y,-0.17
 SIM_GPS_POS_Z,-0.06
 SIM_GPS2_DISABLE,0
+SIM_GPS2_FIXTYPE,3
 SIM_GPS2_NUMSATS,27
 SIM_GPS2_POS_X,-0.12
 SIM_GPS2_POS_Y,0.17

diff --git a/libraries/AP_HAL_ChibiOS/hwdef/CarbonixCommon/sitl_params/volanti-realflight.parm b/libraries/AP_HAL_ChibiOS/hwdef/CarbonixCommon/sitl_params/volanti-realflight.parm
@@ -29,11 +29,13 @@ SERVO7_FUNCTION,21 # RealFlight model uses old servo numbering
 SIM_ARSPD_RATIO,1.6
 SIM_ARSPD2_RATIO,1.6
 SIM_ESC_TELEM,0 # Disable the built-in ESC telemetry simulation (we have lua do this instead)
+SIM_GPS_FIXTYPE,3
 SIM_GPS_NUMSATS,29
 SIM_GPS_POS_X,-0.12
 SIM_GPS_POS_Y,-0.17
 SIM_GPS_POS_Z,-0.06
 SIM_GPS2_DISABLE,0
+SIM_GPS2_FIXTYPE,3
 SIM_GPS2_NUMSATS,27
 SIM_GPS2_POS_X,-0.12
 SIM_GPS2_POS_Y,0.17

diff --git a/libraries/SITL/SIM_GPS.h b/libraries/SITL/SIM_GPS.h
@@ -128,6 +128,15 @@ class GPS : public SerialDevice {
 #endif
     };
 
+    enum FixType {
+        FIX_NONE = 1,
+        FIX_2D = 2,
+        FIX_3D = 3,
+        FIX_DGPS = 4,
+        FIX_RTK_FLOAT = 5,
+        FIX_RTK_FIXED = 6,
+    };
+
     GPS(uint8_t _instance);
 
     // update state

diff --git a/libraries/SITL/SIM_GPS_UBLOX.cpp b/libraries/SITL/SIM_GPS_UBLOX.cpp
@@ -198,9 +198,23 @@ void GPS_UBlox::publish(const GPS_Data *d)
     pos.horizontal_accuracy = _sitl->gps_accuracy[instance]*1000;
     pos.vertical_accuracy = _sitl->gps_accuracy[instance]*1000;
 
+    const uint8_t fix_type = d->have_lock ? _sitl->gps_fix_type[instance] : GPS::FixType::FIX_NONE;
+    uint8_t ubx_fix_type = 0;
+    if (fix_type == GPS::FixType::FIX_2D) {
+        ubx_fix_type = 2;
+    } else if (fix_type >= GPS::FixType::FIX_3D) {
+        ubx_fix_type = 3;
+    }
+    uint8_t ubx_fix_status = 0b1100; // time/date valid
+    if (fix_type >= GPS::FixType::FIX_2D) {
+        ubx_fix_status |= d->have_lock? 0b01 : 0; // gpsfixok
+    }
+    if (fix_type >= GPS::FixType::FIX_DGPS) {
+        ubx_fix_status |= 0b10; // diffsoln
+    }
     status.time = gps_tow.ms;
-    status.fix_type = d->have_lock?3:0;
-    status.fix_status = d->have_lock?1:0;
+    status.fix_type = ubx_fix_type;
+    status.fix_status = ubx_fix_status;
     status.differential_status = 0;
     status.res = 0;
     status.time_to_first_fix = 0;
@@ -220,8 +234,8 @@ void GPS_UBlox::publish(const GPS_Data *d)
     velned.heading_accuracy = 4;
 
     memset(&sol, 0, sizeof(sol));
-    sol.fix_type = d->have_lock?3:0;
-    sol.fix_status = 221;
+    sol.fix_type = ubx_fix_type;
+    sol.fix_status = ubx_fix_status;
     sol.satellites = d->have_lock ? _sitl->gps_numsats[instance] : 3;
     sol.time = gps_tow.ms;
     sol.week = gps_tow.week;
@@ -235,6 +249,19 @@ void GPS_UBlox::publish(const GPS_Data *d)
     dop.nDOP = 65535;
     dop.eDOP = 65535;
 
+    uint8_t ubx_pvt_flags = 0b00000000;
+    if (fix_type >= GPS::FixType::FIX_2D) {
+        ubx_pvt_flags |= 0b00000001; // gpsfixok
+    } 
+    if (fix_type >= GPS::FixType::FIX_DGPS) {
+        ubx_pvt_flags |= 0b00000010; // diffsoln
+    }
+    if (fix_type == GPS::FixType::FIX_RTK_FLOAT) {
+        ubx_pvt_flags |= 0b01000000; // carrsoln - float
+    }
+    if (fix_type == GPS::FixType::FIX_RTK_FIXED) {
+        ubx_pvt_flags |= 0b10000000; // carrsoln - fixed
+    }
     pvt.itow = gps_tow.ms;
     pvt.year = 0;
     pvt.month = 0;
@@ -245,8 +272,8 @@ void GPS_UBlox::publish(const GPS_Data *d)
     pvt.valid = 0; // invalid utc date
     pvt.t_acc = 0;
     pvt.nano = 0;
-    pvt.fix_type = d->have_lock? 0x3 : 0;
-    pvt.flags = 0b10000011; // carrsoln=fixed, psm = na, diffsoln and fixok
+    pvt.fix_type = ubx_fix_type;
+    pvt.flags = ubx_pvt_flags;
     pvt.flags2 =0;
     pvt.num_sv = d->have_lock ? _sitl->gps_numsats[instance] : 3;
     pvt.lon = d->longitude * 1.0e7;

diff --git a/libraries/SITL/SITL.cpp b/libraries/SITL/SITL.cpp
@@ -652,6 +652,12 @@ const AP_Param::GroupInfo SIM::var_gps[] = {
     // @User: Advanced
     // @Values: 0:Disabled, 1:Enabled
     AP_GROUPINFO("GPS_JAM",       16, SIM,  gps_jam[0], 0),
+    // @Param: GPS_FIXTYPE
+    // @DisplayName: GPS fix type
+    // @Description: Set the fix type (3D, RTK, etc) of the simulated GPS
+    // @User: Advanced
+    // @Values: 1:None, 2:2D, 3:3D, 4:DGPS, 5:RTK Fixed, 6:RTK Float
+    AP_GROUPINFO("GPS_FIXTYPE",  17, SIM,  gps_fix_type[0], GPS::FixType::FIX_RTK_FIXED),
     // @Param: GPS2_DISABLE
     // @DisplayName: GPS 2 disable
     // @Description: Disables GPS 2
@@ -761,6 +767,11 @@ const AP_Param::GroupInfo SIM::var_gps[] = {
     // @User: Advanced
     // @Values: 0:Disabled, 1:Enabled
     AP_GROUPINFO("GPS2_JAM",      49, SIM,  gps_jam[1], 0),
+    // @Param: GPS2_FIXTYPE
+    // @CopyFieldsFrom: SIM_GPS_FIXTYPE
+    // @DisplayName: GPS 2 fix type
+    // @Description: Set the fix type (3D, RTK, etc) of the second simulated GPS
+    AP_GROUPINFO("GPS2_FIXTYPE",  50, SIM,  gps_fix_type[1], GPS::FixType::FIX_RTK_FIXED),
 
     AP_GROUPEND
 };

diff --git a/libraries/SITL/SITL.h b/libraries/SITL/SITL.h
@@ -209,6 +209,7 @@ class SIM {
     AP_Float gps_accuracy[2];
     AP_Vector3f gps_vel_err[2]; // Velocity error offsets in NED (x = N, y = E, z = D)
     AP_Int8 gps_jam[2]; // jamming simulation enable
+    AP_Int8 gps_fix_type[2]; // GPS fix type
 
     // initial offset on GPS lat/lon, used to shift origin
     AP_Float gps_init_lat_ofs;