Rangefinder arming checks

APMrover2/Parameters.cpp

@@ -742,7 +742,6 @@ const AP_Param::ConversionInfo conversion_table[] = {
     { Parameters::k_param_serial2_baud,       0,      AP_PARAM_INT16, "SERIAL2_BAUD" },
     { Parameters::k_param_throttle_min_old,   0,      AP_PARAM_INT8,  "MOT_THR_MIN" },
     { Parameters::k_param_throttle_max_old,   0,      AP_PARAM_INT8,  "MOT_THR_MAX" },
-
     { Parameters::k_param_compass_enabled_deprecated,       0,      AP_PARAM_INT8, "COMPASS_ENABLE" },
     { Parameters::k_param_pivot_turn_angle_old,   0,  AP_PARAM_INT16,  "WP_PIVOT_ANGLE" },
     { Parameters::k_param_waypoint_radius_old,    0,  AP_PARAM_FLOAT,  "WP_RADIUS" },
@@ -754,6 +753,7 @@ const AP_Param::ConversionInfo conversion_table[] = {
     { Parameters::k_param_g2,                34,      AP_PARAM_FLOAT,  "SAIL_ANGLE_IDEAL" },
     { Parameters::k_param_g2,                35,      AP_PARAM_FLOAT,  "SAIL_HEEL_MAX" },
     { Parameters::k_param_g2,                36,      AP_PARAM_FLOAT,  "SAIL_NO_GO_ANGLE" },
+    { Parameters::k_param_arming,             2,     AP_PARAM_INT16,  "ARMING_CHECK" },
 };
 
 void Rover::load_parameters(void)

APMrover2/system.cpp

@@ -160,6 +160,10 @@ void Rover::init_ardupilot()
 
     // flag that initialisation has completed
     initialised = true;
+
+#if AP_PARAM_KEY_DUMP
+    AP_Param::show_all(hal.console, true);
+#endif
 }
 
 //*********************************************************************************

ArduCopter/Parameters.cpp

@@ -1043,8 +1043,8 @@ const AP_Param::ConversionInfo conversion_table[] = {
     { Parameters::Parameters::k_param_ch10_option_old,   0,      AP_PARAM_INT8,  "RC10_OPTION" },
     { Parameters::Parameters::k_param_ch11_option_old,   0,      AP_PARAM_INT8,  "RC11_OPTION" },
     { Parameters::Parameters::k_param_ch12_option_old,   0,      AP_PARAM_INT8,  "RC12_OPTION" },
-
     { Parameters::k_param_compass_enabled_deprecated,    0,      AP_PARAM_INT8, "COMPASS_ENABLE" },
+    { Parameters::k_param_arming,             2,     AP_PARAM_INT16,  "ARMING_CHECK" },
 };
 
 void Copter::load_parameters(void)

ArduCopter/system.cpp

@@ -255,6 +255,10 @@ void Copter::init_ardupilot()
 
     // flag that initialisation has completed
     ap.initialised = true;
+
+#if AP_PARAM_KEY_DUMP
+    AP_Param::show_all(hal.console, true);
+#endif
 }
 
 

ArduPlane/Parameters.cpp

@@ -1302,8 +1302,8 @@ const AP_Param::ConversionInfo conversion_table[] = {
     { Parameters::k_param_fs_batt_mah,        0,      AP_PARAM_FLOAT, "BATT_LOW_MAH" },
 
     { Parameters::k_param_arming,             3,      AP_PARAM_INT8,  "ARMING_RUDDER" },
-
     { Parameters::k_param_compass_enabled_deprecated,       0,      AP_PARAM_INT8, "COMPASS_ENABLE" },
+    { Parameters::k_param_arming,           128,     AP_PARAM_INT16,  "ARMING_CHECK" },
 };
 
 void Plane::load_parameters(void)

ArduPlane/system.cpp

@@ -184,6 +184,10 @@ void Plane::init_ardupilot()
 
     // disable safety if requested
     BoardConfig.init_safety();
+
+#if AP_PARAM_KEY_DUMP
+    AP_Param::show_all(hal.console, true);
+#endif
 }
 
 //********************************************************************************

ArduSub/Parameters.cpp

@@ -651,8 +651,8 @@ const AP_Param::ConversionInfo conversion_table[] = {
     { Parameters::k_param_fs_batt_voltage,   0,      AP_PARAM_FLOAT,  "BATT_LOW_VOLT" },
     { Parameters::k_param_fs_batt_mah,       0,      AP_PARAM_FLOAT,  "BATT_LOW_MAH" },
     { Parameters::k_param_failsafe_battery_enabled,       0,      AP_PARAM_INT8,  "BATT_FS_LOW_ACT" },
-
     { Parameters::k_param_compass_enabled_deprecated,       0,      AP_PARAM_INT8, "COMPASS_ENABLE" },
+    { Parameters::k_param_arming,            2,     AP_PARAM_INT16,  "ARMING_CHECK" },
 };
 
 void Sub::load_parameters()

ArduSub/system.cpp

@@ -207,6 +207,10 @@ void Sub::init_ardupilot()
 
     // flag that initialisation has completed
     ap.initialised = true;
+
+#if AP_PARAM_KEY_DUMP
+    AP_Param::show_all(hal.console, true);
+#endif
 }
 
 

libraries/AP_Arming/AP_Arming.cpp

@@ -28,6 +28,7 @@
 #include <AP_InternalError/AP_InternalError.h>
 #include <AP_GPS/AP_GPS.h>
 #include <AP_Baro/AP_Baro.h>
+#include <AP_RangeFinder/AP_RangeFinder.h>
 
 #if HAL_WITH_UAVCAN
   #include <AP_BoardConfig/AP_BoardConfig_CAN.h>
@@ -68,15 +69,7 @@ const AP_Param::GroupInfo AP_Arming::var_info[] = {
                              AP_PARAM_NO_SHIFT,
                              AP_PARAM_FRAME_PLANE | AP_PARAM_FRAME_ROVER),
 
-    // @Param: CHECK
-    // @DisplayName: Arm Checks to Peform (bitmask)
-    // @Description: Checks prior to arming motor. This is a bitmask of checks that will be performed before allowing arming. The default is no checks, allowing arming at any time. You can select whatever checks you prefer by adding together the values of each check type to set this parameter. For example, to only allow arming when you have GPS lock and no RC failsafe you would set ARMING_CHECK to 72. For most users it is recommended that you set this to 1 to enable all checks.
-    // @Values: 0:None,1:All,2:Barometer,4:Compass,8:GPS Lock,16:INS(INertial Sensors - accels & gyros),32:Parameters(unused),64:RC Channels,128:Board voltage,256:Battery Level,1024:LoggingAvailable,2048:Hardware safety switch,4096:GPS configuration,8192:System
-    // @Values{Plane}: 0:None,1:All,2:Barometer,4:Compass,8:GPS Lock,16:INS(INertial Sensors - accels & gyros),32:Parameters(unused),64:RC Channels,128:Board voltage,256:Battery Level,512:Airspeed,1024:LoggingAvailable,2048:Hardware safety switch,4096:GPS configuration,8192:System
-    // @Bitmask: 0:All,1:Barometer,2:Compass,3:GPS lock,4:INS,5:Parameters,6:RC Channels,7:Board voltage,8:Battery Level,10:Logging Available,11:Hardware safety switch,12:GPS Configuration,13:System
-    // @Bitmask{Plane}: 0:All,1:Barometer,2:Compass,3:GPS lock,4:INS,5:Parameters,6:RC Channels,7:Board voltage,8:Battery Level,9:Airspeed,10:Logging Available,11:Hardware safety switch,12:GPS Configuration,13:System
-    // @User: Standard
-    AP_GROUPINFO("CHECK",        2,     AP_Arming,  checks_to_perform,       ARMING_CHECK_ALL),
+    // 2 was the CHECK paramter stored in a AP_Int16
 
     // @Param: ACCTHRESH
     // @DisplayName: Accelerometer error threshold
@@ -107,6 +100,16 @@ const AP_Param::GroupInfo AP_Arming::var_info[] = {
     // @User: Advanced
     AP_GROUPINFO("MIS_ITEMS",    7,     AP_Arming, _required_mission_items, 0),
 
+    // @Param: CHECK
+    // @DisplayName: Arm Checks to Peform (bitmask)
+    // @Description: Checks prior to arming motor. This is a bitmask of checks that will be performed before allowing arming. The default is no checks, allowing arming at any time. You can select whatever checks you prefer by adding together the values of each check type to set this parameter. For example, to only allow arming when you have GPS lock and no RC failsafe you would set ARMING_CHECK to 72. For most users it is recommended that you set this to 1 to enable all checks.
+    // @Values: 0:None,1:All,2:Barometer,4:Compass,8:GPS Lock,16:INS(INertial Sensors - accels & gyros),32:Parameters(unused),64:RC Channels,128:Board voltage,256:Battery Level,1024:LoggingAvailable,2048:Hardware safety switch,4096:GPS configuration,8192:System
+    // @Values{Plane}: 0:None,1:All,2:Barometer,4:Compass,8:GPS Lock,16:INS(INertial Sensors - accels & gyros),32:Parameters(unused),64:RC Channels,128:Board voltage,256:Battery Level,512:Airspeed,1024:LoggingAvailable,2048:Hardware safety switch,4096:GPS configuration,8192:System
+    // @Bitmask: 0:All,1:Barometer,2:Compass,3:GPS lock,4:INS,5:Parameters,6:RC Channels,7:Board voltage,8:Battery Level,10:Logging Available,11:Hardware safety switch,12:GPS Configuration,13:System,14:Mission,15:Rangefinder
+    // @Bitmask{Plane}: 0:All,1:Barometer,2:Compass,3:GPS lock,4:INS,5:Parameters,6:RC Channels,7:Board voltage,8:Battery Level,9:Airspeed,10:Logging Available,11:Hardware safety switch,12:GPS Configuration,13:System,14:Mission,15:Rangefinder
+    // @User: Standard
+    AP_GROUPINFO("CHECK",        8,     AP_Arming,  checks_to_perform,       ARMING_CHECK_ALL),
+
     AP_GROUPEND
 };
 
@@ -594,6 +597,24 @@ bool AP_Arming::mission_checks(bool report)
     return true;
 }
 
+bool AP_Arming::rangefinder_checks(bool report)
+{
+    if ((checks_to_perform & ARMING_CHECK_ALL) || (checks_to_perform & ARMING_CHECK_RANGEFINDER)) {
+        RangeFinder *range = RangeFinder::get_singleton();
+        if (range == nullptr) {
+            return true;
+        }
+
+        char buffer[MAVLINK_MSG_STATUSTEXT_FIELD_TEXT_LEN+1];
+        if (!range->prearm_healthy(buffer, ARRAY_SIZE(buffer))) {
+            check_failed(ARMING_CHECK_RANGEFINDER, report, "%s", buffer);
+            return false;
+        }
+    }
+
+    return true;
+}
+
 bool AP_Arming::servo_checks(bool report) const
 {
     bool check_passed = true;
@@ -773,6 +794,7 @@ bool AP_Arming::pre_arm_checks(bool report)
         &  logging_checks(report)
         &  manual_transmitter_checks(report)
         &  mission_checks(report)
+        &  rangefinder_checks(report)
         &  servo_checks(report)
         &  board_voltage_checks(report)
         &  system_checks(report)

libraries/AP_Arming/AP_Arming.h

@@ -17,22 +17,23 @@ class AP_Arming {
     static AP_Arming *get_singleton();
 
     enum ArmingChecks {
-        ARMING_CHECK_NONE       = 0x0000,
-        ARMING_CHECK_ALL        = 0x0001,
-        ARMING_CHECK_BARO       = 0x0002,
-        ARMING_CHECK_COMPASS    = 0x0004,
-        ARMING_CHECK_GPS        = 0x0008,
-        ARMING_CHECK_INS        = 0x0010,
-        ARMING_CHECK_PARAMETERS = 0x0020,
-        ARMING_CHECK_RC         = 0x0040,
-        ARMING_CHECK_VOLTAGE    = 0x0080,
-        ARMING_CHECK_BATTERY    = 0x0100,
-        ARMING_CHECK_AIRSPEED   = 0x0200,
-        ARMING_CHECK_LOGGING    = 0x0400,
-        ARMING_CHECK_SWITCH     = 0x0800,
-        ARMING_CHECK_GPS_CONFIG = 0x1000,
-        ARMING_CHECK_SYSTEM     = 0x2000,
-        ARMING_CHECK_MISSION    = 0x4000,
+        ARMING_CHECK_NONE        = 0x0000,
+        ARMING_CHECK_ALL         = (1U << 0),
+        ARMING_CHECK_BARO        = (1U << 1),
+        ARMING_CHECK_COMPASS     = (1U << 2),
+        ARMING_CHECK_GPS         = (1U << 3),
+        ARMING_CHECK_INS         = (1U << 4),
+        ARMING_CHECK_PARAMETERS  = (1U << 5),
+        ARMING_CHECK_RC          = (1U << 6),
+        ARMING_CHECK_VOLTAGE     = (1U << 7),
+        ARMING_CHECK_BATTERY     = (1U << 8),
+        ARMING_CHECK_AIRSPEED    = (1U << 9),
+        ARMING_CHECK_LOGGING     = (1U << 10),
+        ARMING_CHECK_SWITCH      = (1U << 11),
+        ARMING_CHECK_GPS_CONFIG  = (1U << 12),
+        ARMING_CHECK_SYSTEM      = (1U << 13),
+        ARMING_CHECK_MISSION     = (1U << 14),
+        ARMING_CHECK_RANGEFINDER = (1U << 15),
     };
 
     enum class Method {
@@ -49,6 +50,8 @@ class AP_Arming {
         YES_ZERO_PWM = 2
     };
 
+    void init(void);
+
     // these functions should not be used by Copter which holds the armed state in the motors library
     Required arming_required();
     virtual bool arm(AP_Arming::Method method, bool do_arming_checks=true);
@@ -84,7 +87,7 @@ class AP_Arming {
 
     // Parameters
     AP_Int8                 require;
-    AP_Int16                checks_to_perform;      // bitmask for which checks are required
+    AP_Int32                checks_to_perform;      // bitmask for which checks are required
     AP_Float                accel_error_threshold;
     AP_Int8                 _rudder_arming;
     AP_Int32                 _required_mission_items;
@@ -118,6 +121,8 @@ class AP_Arming {
 
     bool mission_checks(bool report);
 
+    bool rangefinder_checks(bool report);
+
     bool fence_checks(bool report);
 
     virtual bool system_checks(bool report);

libraries/AP_Param/AP_Param.cpp

@@ -1096,7 +1096,9 @@ void AP_Param::save(bool force_save)
         // when we are disarmed then loop waiting for a slot to become
         // available. This guarantees completion for large parameter
         // set loads
+        hal.scheduler->expect_delay_ms(1);
         hal.scheduler->delay_microseconds(500);
+        hal.scheduler->expect_delay_ms(0);
     }
 }
 
@@ -1118,7 +1120,9 @@ void AP_Param::flush(void)
 {
     uint16_t counter = 200; // 2 seconds max
     while (counter-- && save_queue.available()) {
+        hal.scheduler->expect_delay_ms(10);
         hal.scheduler->delay(10);
+        hal.scheduler->expect_delay_ms(0);
     }
 }
 
@@ -1745,6 +1749,9 @@ void AP_Param::convert_old_parameters(const struct ConversionInfo *conversion_ta
     for (uint8_t i=0; i<table_size; i++) {
         convert_old_parameter(&conversion_table[i], 1.0f, flags);
     }
+    // we need to flush here to prevent a later set_default_by_name()
+    // causing a save to be done on a converted parameter
+    flush();
 }
 
 /*
@@ -2325,6 +2332,7 @@ void AP_Param::show_all(AP_HAL::BetterStream *port, bool showKeyValues)
             port->printf("Key %i: Index %i: GroupElement %i  :  ", token.key, token.idx, token.group_element);
         }
         show(ap, token, type, port);
+        hal.scheduler->delay(1);
     }
 }
 #endif // AP_PARAM_KEY_DUMP

libraries/AP_RangeFinder/RangeFinder.cpp

@@ -693,6 +693,18 @@ void RangeFinder::Log_RFND()
     }
 }
 
+bool RangeFinder::prearm_healthy(char *failure_msg, const uint8_t failure_msg_len) const
+{
+    for (uint8_t i = 0; i < RANGEFINDER_MAX_INSTANCES; i++) {
+        if ((params[i].type != RangeFinder_TYPE_NONE) && (drivers[i] == nullptr)) {
+          hal.util->snprintf(failure_msg, failure_msg_len, "Rangefinder %d was not detected", i + 1);
+          return false;
+        }
+    }
+
+    return true;
+}
+
 RangeFinder *RangeFinder::_singleton;
 
 namespace AP {

libraries/AP_RangeFinder/RangeFinder.h

@@ -111,6 +111,9 @@ class RangeFinder
         return num_instances;
     }
 
+    // prearm checks
+    bool prearm_healthy(char *failure_msg, const uint8_t failure_msg_len) const;
+
     // detect and initialise any available rangefinders
     void init(enum Rotation orientation_default);