CX_BIT: add GPS check

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()
 

libraries/AP_HAL_ChibiOS/hwdef/CarbonixCommon/aircraft_configuration/Ottano_AC_1.xml

@@ -143,6 +143,10 @@
             <source_path>libraries/AP_HAL_ChibiOS/hwdef/CarbonixCommon/scripts/modules/bit_esc.lua</source_path>
             <destination_path>modules/bit_esc.lua</destination_path>
         </lua_script>
+        <lua_script>
+            <source_path>libraries/AP_HAL_ChibiOS/hwdef/CarbonixCommon/scripts/modules/bit_gps.lua</source_path>
+            <destination_path>modules/bit_gps.lua</destination_path>
+        </lua_script>
         <lua_script>
             <source_path>libraries/AP_HAL_ChibiOS/hwdef/CarbonixCommon/scripts/modules/msg.lua</source_path>
             <destination_path>modules/msg.lua</destination_path>

libraries/AP_HAL_ChibiOS/hwdef/CarbonixCommon/aircraft_configuration/Ottano_AC_2.xml

@@ -143,6 +143,10 @@
             <source_path>libraries/AP_HAL_ChibiOS/hwdef/CarbonixCommon/scripts/modules/bit_esc.lua</source_path>
             <destination_path>modules/bit_esc.lua</destination_path>
         </lua_script>
+        <lua_script>
+            <source_path>libraries/AP_HAL_ChibiOS/hwdef/CarbonixCommon/scripts/modules/bit_gps.lua</source_path>
+            <destination_path>modules/bit_gps.lua</destination_path>
+        </lua_script>
         <lua_script>
             <source_path>libraries/AP_HAL_ChibiOS/hwdef/CarbonixCommon/scripts/modules/msg.lua</source_path>
             <destination_path>modules/msg.lua</destination_path>

libraries/AP_HAL_ChibiOS/hwdef/CarbonixCommon/aircraft_configuration/Volanti_AC_1.xml

@@ -113,6 +113,10 @@
             <source_path>libraries/AP_HAL_ChibiOS/hwdef/CarbonixCommon/scripts/modules/bit_esc.lua</source_path>
             <destination_path>modules/bit_esc.lua</destination_path>
         </lua_script>
+        <lua_script>
+            <source_path>libraries/AP_HAL_ChibiOS/hwdef/CarbonixCommon/scripts/modules/bit_gps.lua</source_path>
+            <destination_path>modules/bit_gps.lua</destination_path>
+        </lua_script>
         <lua_script>
             <source_path>libraries/AP_HAL_ChibiOS/hwdef/CarbonixCommon/scripts/modules/msg.lua</source_path>
             <destination_path>modules/msg.lua</destination_path>

libraries/AP_HAL_ChibiOS/hwdef/CarbonixCommon/aircraft_configuration/Volanti_AC_2.xml

@@ -124,6 +124,10 @@
             <source_path>libraries/AP_HAL_ChibiOS/hwdef/CarbonixCommon/scripts/modules/bit_esc.lua</source_path>
             <destination_path>modules/bit_esc.lua</destination_path>
         </lua_script>
+        <lua_script>
+            <source_path>libraries/AP_HAL_ChibiOS/hwdef/CarbonixCommon/scripts/modules/bit_gps.lua</source_path>
+            <destination_path>modules/bit_gps.lua</destination_path>
+        </lua_script>
         <lua_script>
             <source_path>libraries/AP_HAL_ChibiOS/hwdef/CarbonixCommon/scripts/modules/msg.lua</source_path>
             <destination_path>modules/msg.lua</destination_path>

libraries/AP_HAL_ChibiOS/hwdef/CarbonixCommon/aircraft_configuration/Volanti_AC_3.xml

@@ -124,6 +124,10 @@
             <source_path>libraries/AP_HAL_ChibiOS/hwdef/CarbonixCommon/scripts/modules/bit_esc.lua</source_path>
             <destination_path>modules/bit_esc.lua</destination_path>
         </lua_script>
+        <lua_script>
+            <source_path>libraries/AP_HAL_ChibiOS/hwdef/CarbonixCommon/scripts/modules/bit_gps.lua</source_path>
+            <destination_path>modules/bit_gps.lua</destination_path>
+        </lua_script>
         <lua_script>
             <source_path>libraries/AP_HAL_ChibiOS/hwdef/CarbonixCommon/scripts/modules/msg.lua</source_path>
             <destination_path>modules/msg.lua</destination_path>

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

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

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

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

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

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

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