diff --git a/Tools/autotest/carbonix.py b/Tools/autotest/carbonix.py
index de2105b40b..7e803260c4 100644
--- a/Tools/autotest/carbonix.py
+++ b/Tools/autotest/carbonix.py
@@ -74,6 +74,13 @@ def TestESCTelemetry(index):
self.wait_text(lost_text, check_context=True)
self.progress("'" + lost_text + "':" + ' Success!')
+ # Check that the prearm disable parameter works
+ self.progress('Checking prearm disable parameter')
+ self.set_parameter('BIT_PREARM_DIS', 0b1)
+ self.wait_ready_to_arm()
+ self.set_parameter('BIT_PREARM_DIS', 0)
+ self.wait_not_ready_to_arm()
+
# Clear the failure
self.progress(f'Clearing ESC telemetry failure for ESC {index}')
self.context_clear_collection('STATUSTEXT')
@@ -140,6 +147,60 @@ def TestMotorFail(esc_index, servo_index, is_pusher=False):
self.disarm_vehicle()
self.context_pop()
+ def TestGPSPrearm(index):
+ '''Test GPS prearm checks'''
+ index = int(index)
+ self.context_push()
+
+ # Get the parameter names for the GPSs
+ if index == 0:
+ this_numsats = 'SIM_GPS_NUMSATS'
+ other_numsats = 'SIM_GPS2_NUMSATS'
+ this_fixtype = 'SIM_GPS_FIXTYPE'
+ elif index == 1:
+ this_numsats = 'SIM_GPS2_NUMSATS'
+ other_numsats = 'SIM_GPS_NUMSATS'
+ this_fixtype = 'SIM_GPS2_FIXTYPE'
+ else:
+ raise ValueError('Only 2 GPSs are supported')
+ self.set_parameters({this_numsats: 30, other_numsats: 30})
+ self.wait_ready_to_arm()
+
+ # Reduce the number of satellites to trigger a prearm failure
+ self.progress(f'Reducing number of satellites for GPS {index}')
+ self.set_parameter(this_numsats, 6)
+ self.wait_not_ready_to_arm()
+
+ # Check that the prearm disable parameter works
+ self.progress('Checking prearm disable parameter')
+ self.set_parameter('BIT_PREARM_DIS', 0b10)
+ self.wait_ready_to_arm()
+ self.set_parameter('BIT_PREARM_DIS', 0)
+ self.wait_not_ready_to_arm()
+
+ # Restore the number of satellites
+ self.progress(f'Restoring number of satellites for GPS {index}')
+ self.set_parameter(this_numsats, 30)
+ self.wait_ready_to_arm()
+
+ # Cause a large satellite count difference between the two GPSs
+ self.progress('Causing large satellite count difference')
+ self.set_parameters({this_numsats: 30, other_numsats: 50})
+ self.wait_not_ready_to_arm()
+
+ # Check that our script does not complain about low satellite count
+ # when there is insufficient fix (the existing GPS prearm check
+ # should be the one to complain about lack of fix)
+ self.progress('Checking low satellite count with no fix')
+ self.set_parameter(this_fixtype, 1) # No fix
+ # Disable ArduPilot's GPS prearm check to use wait_ready_to_arm
+ # (23 bits, except for bits 0 and 3)
+ self.set_parameter('ARMING_CHECK', 0x7FFFF6)
+ self.wait_ready_to_arm()
+
+ # Restore everything
+ self.context_pop()
+
# Count the number of ESCs
frame_class = self.get_parameter('Q_FRAME_CLASS')
if frame_class == 1: # Quad
@@ -177,6 +238,10 @@ def TestMotorFail(esc_index, servo_index, is_pusher=False):
if not has_engine:
TestMotorFail(num_vtols, pusher_servo, is_pusher=True)
+ self.start_subtest('Test GPS')
+ for i in range(2):
+ TestGPSPrearm(i)
+
def disabled_tests(self):
return dict()
diff --git a/libraries/AP_HAL_ChibiOS/hwdef/CarbonixCommon/aircraft_configuration/Ottano_AC_1.xml b/libraries/AP_HAL_ChibiOS/hwdef/CarbonixCommon/aircraft_configuration/Ottano_AC_1.xml
index 8f441aceb1..a1ec6af543 100644
--- a/libraries/AP_HAL_ChibiOS/hwdef/CarbonixCommon/aircraft_configuration/Ottano_AC_1.xml
+++ b/libraries/AP_HAL_ChibiOS/hwdef/CarbonixCommon/aircraft_configuration/Ottano_AC_1.xml
@@ -143,6 +143,10 @@
libraries/AP_HAL_ChibiOS/hwdef/CarbonixCommon/scripts/modules/bit_esc.lua
modules/bit_esc.lua
+
+ libraries/AP_HAL_ChibiOS/hwdef/CarbonixCommon/scripts/modules/bit_gps.lua
+ modules/bit_gps.lua
+
libraries/AP_HAL_ChibiOS/hwdef/CarbonixCommon/scripts/modules/msg.lua
modules/msg.lua
diff --git a/libraries/AP_HAL_ChibiOS/hwdef/CarbonixCommon/aircraft_configuration/Ottano_AC_2.xml b/libraries/AP_HAL_ChibiOS/hwdef/CarbonixCommon/aircraft_configuration/Ottano_AC_2.xml
index c565c47b38..e8e40fe2fd 100644
--- a/libraries/AP_HAL_ChibiOS/hwdef/CarbonixCommon/aircraft_configuration/Ottano_AC_2.xml
+++ b/libraries/AP_HAL_ChibiOS/hwdef/CarbonixCommon/aircraft_configuration/Ottano_AC_2.xml
@@ -143,6 +143,10 @@
libraries/AP_HAL_ChibiOS/hwdef/CarbonixCommon/scripts/modules/bit_esc.lua
modules/bit_esc.lua
+
+ libraries/AP_HAL_ChibiOS/hwdef/CarbonixCommon/scripts/modules/bit_gps.lua
+ modules/bit_gps.lua
+
libraries/AP_HAL_ChibiOS/hwdef/CarbonixCommon/scripts/modules/msg.lua
modules/msg.lua
diff --git a/libraries/AP_HAL_ChibiOS/hwdef/CarbonixCommon/aircraft_configuration/Volanti_AC_1.xml b/libraries/AP_HAL_ChibiOS/hwdef/CarbonixCommon/aircraft_configuration/Volanti_AC_1.xml
index 8b3697f4a0..703c7ef8d3 100644
--- a/libraries/AP_HAL_ChibiOS/hwdef/CarbonixCommon/aircraft_configuration/Volanti_AC_1.xml
+++ b/libraries/AP_HAL_ChibiOS/hwdef/CarbonixCommon/aircraft_configuration/Volanti_AC_1.xml
@@ -113,6 +113,10 @@
libraries/AP_HAL_ChibiOS/hwdef/CarbonixCommon/scripts/modules/bit_esc.lua
modules/bit_esc.lua
+
+ libraries/AP_HAL_ChibiOS/hwdef/CarbonixCommon/scripts/modules/bit_gps.lua
+ modules/bit_gps.lua
+
libraries/AP_HAL_ChibiOS/hwdef/CarbonixCommon/scripts/modules/msg.lua
modules/msg.lua
diff --git a/libraries/AP_HAL_ChibiOS/hwdef/CarbonixCommon/aircraft_configuration/Volanti_AC_2.xml b/libraries/AP_HAL_ChibiOS/hwdef/CarbonixCommon/aircraft_configuration/Volanti_AC_2.xml
index a762c47472..e5b8d31cd2 100644
--- a/libraries/AP_HAL_ChibiOS/hwdef/CarbonixCommon/aircraft_configuration/Volanti_AC_2.xml
+++ b/libraries/AP_HAL_ChibiOS/hwdef/CarbonixCommon/aircraft_configuration/Volanti_AC_2.xml
@@ -124,6 +124,10 @@
libraries/AP_HAL_ChibiOS/hwdef/CarbonixCommon/scripts/modules/bit_esc.lua
modules/bit_esc.lua
+
+ libraries/AP_HAL_ChibiOS/hwdef/CarbonixCommon/scripts/modules/bit_gps.lua
+ modules/bit_gps.lua
+
libraries/AP_HAL_ChibiOS/hwdef/CarbonixCommon/scripts/modules/msg.lua
modules/msg.lua
diff --git a/libraries/AP_HAL_ChibiOS/hwdef/CarbonixCommon/aircraft_configuration/Volanti_AC_3.xml b/libraries/AP_HAL_ChibiOS/hwdef/CarbonixCommon/aircraft_configuration/Volanti_AC_3.xml
index c334ec568a..939e96ea02 100644
--- a/libraries/AP_HAL_ChibiOS/hwdef/CarbonixCommon/aircraft_configuration/Volanti_AC_3.xml
+++ b/libraries/AP_HAL_ChibiOS/hwdef/CarbonixCommon/aircraft_configuration/Volanti_AC_3.xml
@@ -124,6 +124,10 @@
libraries/AP_HAL_ChibiOS/hwdef/CarbonixCommon/scripts/modules/bit_esc.lua
modules/bit_esc.lua
+
+ libraries/AP_HAL_ChibiOS/hwdef/CarbonixCommon/scripts/modules/bit_gps.lua
+ modules/bit_gps.lua
+
libraries/AP_HAL_ChibiOS/hwdef/CarbonixCommon/scripts/modules/msg.lua
modules/msg.lua
diff --git a/libraries/AP_HAL_ChibiOS/hwdef/CarbonixCommon/defaults.parm b/libraries/AP_HAL_ChibiOS/hwdef/CarbonixCommon/defaults.parm
index 275549223b..10f6c78b44 100644
--- a/libraries/AP_HAL_ChibiOS/hwdef/CarbonixCommon/defaults.parm
+++ b/libraries/AP_HAL_ChibiOS/hwdef/CarbonixCommon/defaults.parm
@@ -118,8 +118,8 @@ RTL_RADIUS,200
SCHED_LOOP_RATE,200 # Loop Rate for Control in flight Controller set to 200Hz from 400Hz Results SW-171.
SCR_ENABLE,1
SCR_HEAP_SIZE,200000
-SCR_LD_CHECKSUM,1341533
-SCR_RUN_CHECKSUM,1341533
+SCR_LD_CHECKSUM,907762
+SCR_RUN_CHECKSUM,907762
SCR_VM_I_COUNT,100000
SERVO1_FUNCTION,33 # Motor 1
SERVO10_FUNCTION,4 # Aileron R
diff --git a/libraries/AP_HAL_ChibiOS/hwdef/CarbonixCommon/scripts/cx_built_in_test.lua b/libraries/AP_HAL_ChibiOS/hwdef/CarbonixCommon/scripts/cx_built_in_test.lua
index 5799ca014f..ed38de54aa 100644
--- a/libraries/AP_HAL_ChibiOS/hwdef/CarbonixCommon/scripts/cx_built_in_test.lua
+++ b/libraries/AP_HAL_ChibiOS/hwdef/CarbonixCommon/scripts/cx_built_in_test.lua
@@ -1,5 +1,6 @@
local cx_msg = require("msg")
local cx_esc = require("bit_esc")
+local cx_gps = require("bit_gps")
-- Add subsystems that require Built-in-test (implemented in subsystems)
-- Each subsystem should have the following functions:
@@ -9,6 +10,7 @@ local cx_esc = require("bit_esc")
-- - built in test errors are managed by each subsystem
local subsystems = {
cx_esc,
+ cx_gps,
}
-- auth id for prearm check
@@ -34,6 +36,29 @@ local function clear_prearm_error()
arming:set_aux_auth_passed(auth_id)
end
+local function bind_param(name)
+ local p = Parameter()
+ assert(p:init(name), string.format('could not find %s parameter', name))
+ return p
+end
+
+local function bind_add_param(name, idx, default_value)
+ assert(param:add_param(PARAM_TABLE_KEY, idx, name, default_value), string.format('could not add param %s', name))
+ return bind_param(PARAM_TABLE_PREFIX .. name)
+end
+
+-- Set up EFI parameters
+PARAM_TABLE_PREFIX = 'BIT_'
+PARAM_TABLE_KEY = 1
+assert(param:add_table(PARAM_TABLE_KEY, PARAM_TABLE_PREFIX, 1), 'could not add ' .. string.sub(PARAM_TABLE_PREFIX, 1, -2) .. ' param table')
+--[[
+ // @Param: BIT_PREARM_DIS
+ // @DisplayName: Built-In-Test Prearm Bypass Mask
+ // @Description: Allows bypassing prearm checks for individual subsystems
+ // @Bitmask: 0:ESC, 1:GPS
+--]]
+local PREARM_BYPASS = bind_add_param('PREARM_DIS', 1, 0)
+
-- initialize function
local function init()
-- initialize all subsystems that are part of constructor
@@ -52,8 +77,12 @@ local function check_prearm_status()
-- Track errors in subsystems
local subsystems_with_errors = {}
local msg = ""
- for _, subsystem in pairs(subsystems) do
- local errors = subsystem:check_for_errors()
+ local disabled_mask = PREARM_BYPASS:get() or 0
+ for i, subsystem in pairs(subsystems) do
+ local errors = {}
+ if disabled_mask & (1 << (i - 1)) == 0 then
+ errors = subsystem:check_for_errors()
+ end
if #errors > 0 then
table.insert(subsystems_with_errors, subsystem.name)
for _, error in pairs(errors) do
diff --git a/libraries/AP_HAL_ChibiOS/hwdef/CarbonixCommon/scripts/modules/bit_gps.lua b/libraries/AP_HAL_ChibiOS/hwdef/CarbonixCommon/scripts/modules/bit_gps.lua
new file mode 100644
index 0000000000..45a786f8e5
--- /dev/null
+++ b/libraries/AP_HAL_ChibiOS/hwdef/CarbonixCommon/scripts/modules/bit_gps.lua
@@ -0,0 +1,65 @@
+--[[ GPS Prearm Checks
+
+This module is scrictly for pre-arm checks. No warning messages are sent to the
+GCS. GPS status messages have enough information, and update quickly enough, to
+handle alerts on the GCS side.
+
+--]]
+
+local cx_msg = require("msg")
+
+local GPS = {
+ name = "GPS",
+
+ N_GPS = 2,
+
+ MIN_SATS = 18,
+ MAX_DIFF = 8,
+
+ sat_count = {0, 0},
+ fix_type = {0, 0},
+}
+
+function GPS:init()
+ cx_msg:send(cx_msg.MAV_SEVERITY.INFO, self.name .. " init")
+end
+
+function GPS:update()
+ for i = 1, self.N_GPS do
+ if i > gps:num_sensors() then
+ self.sat_count[i] = 0
+ self.fix_type[i] = 0
+ else
+ self.sat_count[i] = gps:num_sats(i - 1)
+ self.fix_type[i] = gps:status(i - 1)
+ end
+ end
+end
+
+function GPS:check_for_errors()
+ local max_sat_count = 0
+ for i = 1, self.N_GPS do
+ if self.sat_count[i] > max_sat_count then
+ max_sat_count = self.sat_count[i]
+ end
+ end
+ local low_sat_count = {}
+ for i = 1, self.N_GPS do
+ -- We don't need to complain about sat count if we don't have a fix.
+ -- ArduPilot's existing checks will handle that for us.
+ if self.fix_type[i] >= gps.GPS_OK_FIX_3D then
+ if self.sat_count[i] < self.MIN_SATS or max_sat_count - self.sat_count[i] > self.MAX_DIFF then
+ table.insert(low_sat_count, i)
+ end
+ end
+ end
+ if #low_sat_count == 0 then
+ return {}
+ elseif #low_sat_count == 1 then
+ return {self.name .. " " .. low_sat_count[1] .. " low satellite count"}
+ else
+ return {self.name .. " low satellite counts"}
+ end
+end
+
+return GPS
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
index d7fcdd7d8c..adde2119a4 100644
--- 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,9 +80,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.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
SIM_IMU_POS_X,0.82
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
index eec4276cf1..b5be337e26 100644
--- 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,10 +68,14 @@ 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
SIM_GPS2_POS_Z,-0.06
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
index 9e7b537bc9..766311a36f 100644
--- 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,10 +29,14 @@ 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
SIM_GPS2_POS_Z,-0.06
diff --git a/libraries/SITL/SIM_GPS.h b/libraries/SITL/SIM_GPS.h
index 71b8cd8298..8d7b1ac661 100644
--- 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
index 5d743cbfe9..0ac9c5f9a2 100644
--- 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
index 2a5cbfff0e..93ef692560 100644
--- 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
index 5256103242..bbcbd99d5c 100644
--- 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;