AP_DroneCAN: allow BRD_SAFETY_MASK to work on CAN ESCs and servos

this allows for testing of a fwd motor or control surfaces while not
allowing for VTOL ESCs to run. This makes CAN actuators behave the
same as direct PWM actuators

libraries/AP_DroneCAN/AP_DroneCAN.cpp

@@ -377,9 +377,8 @@ void AP_DroneCAN::loop(void)
 #if AP_DRONECAN_HOBBYWING_ESC_SUPPORT
         hobbywing_ESC_update();
 #endif
-        if (_SRV_armed) {
+        if (_SRV_armed_mask != 0) {
-            if (_servo_bm > 0) {
+            // we have active servos
-                // if we have any Servos in bitmask
             uint32_t now = AP_HAL::native_micros();
             const uint32_t servo_period_us = 1000000UL / unsigned(_servo_rate_hz.get());
             if (now - _SRV_last_send_us >= servo_period_us) {
@@ -396,7 +395,6 @@ void AP_DroneCAN::loop(void)
         }
     }
 }
-}
 
 #if AP_DRONECAN_HOBBYWING_ESC_SUPPORT
 void AP_DroneCAN::hobbywing_ESC_update(void)
@@ -520,7 +518,7 @@ void AP_DroneCAN::SRV_send_actuator(void)
              * physically possible throws at [-1:1] limits.
              */
 
-            if (_SRV_conf[starting_servo].servo_pending && ((((uint32_t) 1) << starting_servo) & _servo_bm)) {
+            if (_SRV_conf[starting_servo].servo_pending && ((((uint32_t) 1) << starting_servo) & _SRV_armed_mask)) {
                 cmd.actuator_id = starting_servo + 1;
 
                 if (option_is_set(Options::USE_ACTUATOR_PWM)) {
@@ -561,7 +559,7 @@ void AP_DroneCAN::SRV_send_himark(void)
     // ServoCmd can hold maximum of 17 commands. First find the highest pending servo < 17
     int8_t highest_to_send = -1;
     for (int8_t i = 16; i >= 0; i--) {
-        if (_SRV_conf[i].servo_pending && ((1U<<i) & _servo_bm) != 0) {
+        if (_SRV_conf[i].servo_pending && ((1U<<i) & _SRV_armed_mask) != 0) {
             highest_to_send = i;
             break;
         }
@@ -573,7 +571,7 @@ void AP_DroneCAN::SRV_send_himark(void)
     com_himark_servo_ServoCmd msg {};
 
     for (uint8_t i = 0; i <= highest_to_send; i++) {
-        if ((1U<<i) & _servo_bm) {
+        if ((1U<<i) & _SRV_armed_mask) {
             const uint16_t pulse = constrain_int16(_SRV_conf[i].pulse - 1000, 0, 1000);
             msg.cmd.data[i] = pulse;
         }
@@ -596,7 +594,7 @@ void AP_DroneCAN::SRV_send_esc(void)
 
     // find out how many esc we have enabled and if they are active at all
     for (uint8_t i = esc_offset; i < DRONECAN_SRV_NUMBER; i++) {
-        if ((((uint32_t) 1) << i) & _esc_bm) {
+        if ((((uint32_t) 1) << i) & _ESC_armed_mask) {
             max_esc_num = i + 1;
             if (_SRV_conf[i].esc_pending) {
                 active_esc_num++;
@@ -609,7 +607,7 @@ void AP_DroneCAN::SRV_send_esc(void)
         k = 0;
 
         for (uint8_t i = esc_offset; i < max_esc_num && k < 20; i++) {
-            if ((((uint32_t) 1) << i) & _esc_bm) {
+            if ((((uint32_t) 1) << i) & _ESC_armed_mask) {
                 // TODO: ESC negative scaling for reverse thrust and reverse rotation
                 float scaled = cmd_max * (hal.rcout->scale_esc_to_unity(_SRV_conf[i].pulse) + 1.0) / 2.0;
 
@@ -655,7 +653,7 @@ void AP_DroneCAN::SRV_send_esc_hobbywing(void)
 
     // find out how many esc we have enabled and if they are active at all
     for (uint8_t i = esc_offset; i < DRONECAN_SRV_NUMBER; i++) {
-        if ((((uint32_t) 1) << i) & _esc_bm) {
+        if ((((uint32_t) 1) << i) & _ESC_armed_mask) {
             max_esc_num = i + 1;
             if (_SRV_conf[i].esc_pending) {
                 active_esc_num++;
@@ -668,7 +666,7 @@ void AP_DroneCAN::SRV_send_esc_hobbywing(void)
         k = 0;
 
         for (uint8_t i = esc_offset; i < max_esc_num && k < 20; i++) {
-            if ((((uint32_t) 1) << i) & _esc_bm) {
+            if ((((uint32_t) 1) << i) & _ESC_armed_mask) {
                 // TODO: ESC negative scaling for reverse thrust and reverse rotation
                 float scaled = cmd_max * (hal.rcout->scale_esc_to_unity(_SRV_conf[i].pulse) + 1.0) / 2.0;
 
@@ -707,10 +705,24 @@ void AP_DroneCAN::SRV_push_servos()
         }
     }
 
-    _SRV_armed = hal.util->safety_switch_state() != AP_HAL::Util::SAFETY_DISARMED;
+    uint32_t servo_armed_mask = _servo_bm;
+    uint32_t esc_armed_mask = _esc_bm;
+    const bool safety_off = hal.util->safety_switch_state() != AP_HAL::Util::SAFETY_DISARMED;
+    if (!safety_off) {
+        AP_BoardConfig *boardconfig = AP_BoardConfig::get_singleton();
+        if (boardconfig != nullptr) {
+            const uint32_t safety_mask = boardconfig->get_safety_mask();
+            servo_armed_mask &= safety_mask;
+            esc_armed_mask &= safety_mask;
+        } else {
+            servo_armed_mask = 0;
+            esc_armed_mask = 0;
+        }
+    }
+    _SRV_armed_mask = servo_armed_mask;
+    _ESC_armed_mask = esc_armed_mask;
 
-    if (_SRV_armed) {
+    if (_ESC_armed_mask != 0) {
-        if (_esc_bm > 0) {
         // push ESCs as fast as we can
 #if AP_DRONECAN_HOBBYWING_ESC_SUPPORT
         if (option_is_set(Options::USE_HOBBYWING_ESC)) {
@@ -722,7 +734,6 @@ void AP_DroneCAN::SRV_push_servos()
         }
     }
 }
-}
 
 // notify state send
 void AP_DroneCAN::notify_state_send()
@@ -974,7 +985,15 @@ void AP_DroneCAN::safety_state_send()
 
     { // handle SafetyState
         ardupilot_indication_SafetyState safety_msg;
-        switch (hal.util->safety_switch_state()) {
+        auto state = hal.util->safety_switch_state();
+        if (_SRV_armed_mask != 0 || _ESC_armed_mask != 0) {
+            // if we are outputting any servos or ESCs due to
+            // BRD_SAFETY_MASK then we need to advertise safety as
+            // off, this changes LEDs to indicate unsafe and allows
+            // AP_Periph ESCs and servos to run
+            state = AP_HAL::Util::SAFETY_ARMED;
+        }
+        switch (state) {
         case AP_HAL::Util::SAFETY_ARMED:
             safety_msg.status = ARDUPILOT_INDICATION_SAFETYSTATE_STATUS_SAFETY_OFF;
             safety_state.broadcast(safety_msg);

libraries/AP_DroneCAN/AP_DroneCAN.h

@@ -201,7 +201,8 @@ private:
     uint32_t _srv_send_count;
     uint32_t _fail_send_count;
 
-    uint8_t _SRV_armed;
+    uint32_t _SRV_armed_mask; // mask of servo outputs that are active
+    uint32_t _ESC_armed_mask; // mask of ESC outputs that are active
     uint32_t _SRV_last_send_us;
     HAL_Semaphore SRV_sem;