AP_Arming: support auxiliary authorisation

libraries/AP_Arming/AP_Arming.cpp

@@ -112,10 +112,10 @@ const AP_Param::GroupInfo AP_Arming::var_info[] = {
     // @Param: CHECK
     // @DisplayName: Arm Checks to Perform (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,16384:Mission,32768:RangeFinder,65536:Camera
+    // @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,16384:Mission,32768:RangeFinder,65536:Camera,131072:AuxAuth
-    // @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,16384:Mission,32768:RangeFinder,65536:Camera
+    // @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,16384:Mission,32768:RangeFinder,65536:Camera,131072:AuxAuth
-    // @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,16:Camera
+    // @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,16:Camera,17:AuxAuth
-    // @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,16:Camera
+    // @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,16:Camera,17:AuxAuth
     // @User: Standard
     AP_GROUPINFO("CHECK",        8,     AP_Arming,  checks_to_perform,       ARMING_CHECK_ALL),
 
@@ -863,6 +863,121 @@ bool AP_Arming::camera_checks(bool display_failure)
     return true;
 }
 
+// request an auxiliary authorisation id.  This id should be used in subsequent calls to set_aux_auth_passed/failed
+// returns true on success
+bool AP_Arming::get_aux_auth_id(uint8_t& auth_id)
+{
+    WITH_SEMAPHORE(aux_auth_sem);
+
+    // check we have enough room to allocate another id
+    if (aux_auth_count >= aux_auth_count_max) {
+        aux_auth_error = true;
+        return false;
+    }
+
+    // allocate buffer for failure message
+    if (aux_auth_fail_msg == nullptr) {
+        aux_auth_fail_msg = (char *)calloc(aux_auth_str_len, sizeof(char));
+        if (aux_auth_fail_msg == nullptr) {
+            aux_auth_error = true;
+            return false;
+        }
+    }
+    auth_id = aux_auth_count;
+    aux_auth_count++;
+    return true;
+}
+
+// set auxiliary authorisation passed
+void AP_Arming::set_aux_auth_passed(uint8_t auth_id)
+{
+    WITH_SEMAPHORE(aux_auth_sem);
+
+    // sanity check auth_id
+    if (auth_id >= aux_auth_count) {
+        return;
+    }
+
+    aux_auth_state[auth_id] = AuxAuthStates::AUTH_PASSED;
+}
+
+// set auxiliary authorisation failed and provide failure message
+void AP_Arming::set_aux_auth_failed(uint8_t auth_id, const char* fail_msg)
+{
+    WITH_SEMAPHORE(aux_auth_sem);
+
+    // sanity check auth_id
+    if (auth_id >= aux_auth_count) {
+        return;
+    }
+
+    // update state
+    aux_auth_state[auth_id] = AuxAuthStates::AUTH_FAILED;
+
+    // store failure message if this authoriser has the lowest auth_id
+    for (uint8_t i = 0; i < auth_id; i++) {
+        if (aux_auth_state[i] == AuxAuthStates::AUTH_FAILED) {
+            return;
+        }
+    }
+    if (aux_auth_fail_msg != nullptr) {
+        if (fail_msg == nullptr) {
+            strncpy(aux_auth_fail_msg, "Auxiliary authorisation refused", aux_auth_str_len);
+        } else {
+            strncpy(aux_auth_fail_msg, fail_msg, aux_auth_str_len);
+        }
+        aux_auth_fail_msg_source = auth_id;
+    }
+}
+
+bool AP_Arming::aux_auth_checks(bool display_failure)
+{
+    // handle error cases
+    if (aux_auth_error) {
+        if (aux_auth_fail_msg == nullptr) {
+            check_failed(ARMING_CHECK_AUX_AUTH, display_failure, "memory low for auxiliary authorisation");
+        } else {
+            check_failed(ARMING_CHECK_AUX_AUTH, display_failure, "Too many auxiliary authorisers");
+        }
+        return false;
+    }
+
+    WITH_SEMAPHORE(aux_auth_sem);
+
+    // check results for each auxiliary authorisation id
+    bool some_failures = false;
+    bool failure_msg_sent = false;
+    bool waiting_for_responses = false;
+    for (uint8_t i = 0; i < aux_auth_count; i++) {
+        switch (aux_auth_state[i]) {
+        case AuxAuthStates::NO_RESPONSE:
+            waiting_for_responses = true;
+            break;
+        case AuxAuthStates::AUTH_FAILED:
+            some_failures = true;
+            if (i == aux_auth_fail_msg_source) {
+                check_failed(ARMING_CHECK_AUX_AUTH, display_failure, "%s", aux_auth_fail_msg);
+                failure_msg_sent = true;
+            }
+            break;
+        case AuxAuthStates::AUTH_PASSED:
+            break;
+        }
+    }
+
+    // send failure or waiting message
+    if (some_failures && !failure_msg_sent) {
+        check_failed(ARMING_CHECK_AUX_AUTH, display_failure, "Auxiliary authorisation refused");
+        return false;
+    } else if (waiting_for_responses) {
+        check_failed(ARMING_CHECK_AUX_AUTH, display_failure, "Waiting for auxiliary authorisation");
+        return false;
+    }
+
+    // if we got this far all auxiliary checks must have passed
+    return true;
+}
+
 bool AP_Arming::pre_arm_checks(bool report)
 {
 #if !APM_BUILD_TYPE(APM_BUILD_ArduCopter)
@@ -888,7 +1003,8 @@ bool AP_Arming::pre_arm_checks(bool report)
         &  system_checks(report)
         &  can_checks(report)
         &  proximity_checks(report)
-        &  camera_checks(report);
+        &  camera_checks(report)
+        &  aux_auth_checks(report);
 }
 
 bool AP_Arming::arm_checks(AP_Arming::Method method)

libraries/AP_Arming/AP_Arming.h

@@ -34,6 +34,7 @@ public:
         ARMING_CHECK_MISSION     = (1U << 14),
         ARMING_CHECK_RANGEFINDER = (1U << 15),
         ARMING_CHECK_CAMERA      = (1U << 16),
+        ARMING_CHECK_AUX_AUTH    = (1U << 17),
     };
 
     enum class Method {
@@ -106,6 +107,11 @@ public:
 
     RudderArming get_rudder_arming_type() const { return (RudderArming)_rudder_arming.get(); }
 
+    // auxiliary authorisation methods
+    bool get_aux_auth_id(uint8_t& auth_id);
+    void set_aux_auth_passed(uint8_t auth_id);
+    void set_aux_auth_failed(uint8_t auth_id, const char* fail_msg);
+
     static const struct AP_Param::GroupInfo        var_info[];
 
 protected:
@@ -152,6 +158,8 @@ protected:
 
     bool camera_checks(bool display_failure);
 
+    bool aux_auth_checks(bool display_failure);
+
     virtual bool system_checks(bool report);
 
     bool can_checks(bool report);
@@ -191,6 +199,20 @@ private:
         MIS_ITEM_CHECK_RALLY         = (1 << 5),
         MIS_ITEM_CHECK_MAX
     };
+
+    // auxiliary authorisation
+    static const uint8_t aux_auth_count_max = 3;    // maximum number of auxiliary authorisers
+    static const uint8_t aux_auth_str_len = 42;     // maximum length of failure message (50-8 for "PreArm: ")
+    enum class AuxAuthStates : uint8_t {
+        NO_RESPONSE = 0,
+        AUTH_FAILED,
+        AUTH_PASSED
+    } aux_auth_state[aux_auth_count_max] = {};  // state of each auxiliary authorisation
+    uint8_t aux_auth_count;     // number of auxiliary authorisers
+    uint8_t aux_auth_fail_msg_source;   // authorisation id who set aux_auth_fail_msg
+    char* aux_auth_fail_msg;    // buffer for holding failure messages
+    bool aux_auth_error;        // true if too many auxiliary authorisers
+    HAL_Semaphore aux_auth_sem; // semaphore for accessing the aux_auth_state and aux_auth_fail_msg
 };
 
 namespace AP {