From b72f4a3cd88966a83bf1f252d6cdfafe1ce10ea7 Mon Sep 17 00:00:00 2001
From: Andrew Tridgell <andrew@tridgell.net>
Date: Sun, 20 Aug 2023 08:49:28 +1000
Subject: [PATCH] HAL_SITL: enable GPIO and RC output in periph

---
 libraries/AP_HAL_SITL/GPIO.cpp            |  2 +-
 libraries/AP_HAL_SITL/GPIO.h              |  2 +-
 libraries/AP_HAL_SITL/HAL_SITL_Class.cpp  | 11 +++++------
 libraries/AP_HAL_SITL/RCInput.cpp         |  5 ++++-
 libraries/AP_HAL_SITL/RCInput.h           |  2 +-
 libraries/AP_HAL_SITL/RCOutput.cpp        |  3 ++-
 libraries/AP_HAL_SITL/SITL_State.cpp      |  8 ++------
 libraries/AP_HAL_SITL/SITL_State.h        |  3 ---
 libraries/AP_HAL_SITL/SITL_State_common.h |  2 ++
 9 files changed, 18 insertions(+), 20 deletions(-)

diff --git a/libraries/AP_HAL_SITL/GPIO.cpp b/libraries/AP_HAL_SITL/GPIO.cpp
index 9ad575b908..3fe8db8cc6 100644
--- a/libraries/AP_HAL_SITL/GPIO.cpp
+++ b/libraries/AP_HAL_SITL/GPIO.cpp
@@ -1,7 +1,7 @@
 
 #include "GPIO.h"
 
-#if CONFIG_HAL_BOARD == HAL_BOARD_SITL && !defined(HAL_BUILD_AP_PERIPH)
+#if CONFIG_HAL_BOARD == HAL_BOARD_SITL
 
 using namespace HALSITL;
 
diff --git a/libraries/AP_HAL_SITL/GPIO.h b/libraries/AP_HAL_SITL/GPIO.h
index fdf571b526..1a1fd92823 100644
--- a/libraries/AP_HAL_SITL/GPIO.h
+++ b/libraries/AP_HAL_SITL/GPIO.h
@@ -1,7 +1,7 @@
 #pragma once
 
 #include "AP_HAL_SITL.h"
-#if CONFIG_HAL_BOARD == HAL_BOARD_SITL && !defined(HAL_BUILD_AP_PERIPH)
+#if CONFIG_HAL_BOARD == HAL_BOARD_SITL
 
 class HALSITL::GPIO : public AP_HAL::GPIO {
 public:
diff --git a/libraries/AP_HAL_SITL/HAL_SITL_Class.cpp b/libraries/AP_HAL_SITL/HAL_SITL_Class.cpp
index 123a4de11e..c157472f8c 100644
--- a/libraries/AP_HAL_SITL/HAL_SITL_Class.cpp
+++ b/libraries/AP_HAL_SITL/HAL_SITL_Class.cpp
@@ -29,6 +29,7 @@
 #include <AP_HAL_Empty/AP_HAL_Empty_Private.h>
 #include <AP_InternalError/AP_InternalError.h>
 #include <AP_Logger/AP_Logger.h>
+#include <AP_RCProtocol/AP_RCProtocol_config.h>
 
 using namespace HALSITL;
 
@@ -37,15 +38,13 @@ HAL_SITL& hal_sitl = (HAL_SITL&)AP_HAL::get_HAL();
 static Storage sitlStorage;
 static SITL_State sitlState;
 static Scheduler sitlScheduler(&sitlState);
-#if !defined(HAL_BUILD_AP_PERIPH)
-static RCInput  sitlRCInput(&sitlState);
-static RCOutput sitlRCOutput(&sitlState);
-static GPIO sitlGPIO(&sitlState);
+#if AP_RCPROTOCOL_ENABLED
+static RCInput sitlRCInput(&sitlState);
 #else
 static Empty::RCInput  sitlRCInput;
-static Empty::RCOutput sitlRCOutput;
-static Empty::GPIO sitlGPIO;
 #endif
+static RCOutput sitlRCOutput(&sitlState);
+static GPIO sitlGPIO(&sitlState);
 static AnalogIn sitlAnalogIn(&sitlState);
 static DSP dspDriver;
 
diff --git a/libraries/AP_HAL_SITL/RCInput.cpp b/libraries/AP_HAL_SITL/RCInput.cpp
index 62242958f1..98e28a6db0 100644
--- a/libraries/AP_HAL_SITL/RCInput.cpp
+++ b/libraries/AP_HAL_SITL/RCInput.cpp
@@ -1,5 +1,8 @@
 #include <AP_HAL/AP_HAL.h>
-#if CONFIG_HAL_BOARD == HAL_BOARD_SITL && !defined(HAL_BUILD_AP_PERIPH)
+#include <AP_RCProtocol/AP_RCProtocol_config.h>
+
+#if CONFIG_HAL_BOARD == HAL_BOARD_SITL && AP_RCPROTOCOL_ENABLED
+
 
 #include "RCInput.h"
 #include <SITL/SITL.h>
diff --git a/libraries/AP_HAL_SITL/RCInput.h b/libraries/AP_HAL_SITL/RCInput.h
index e991a40132..b327c87d4e 100644
--- a/libraries/AP_HAL_SITL/RCInput.h
+++ b/libraries/AP_HAL_SITL/RCInput.h
@@ -2,7 +2,7 @@
 #pragma once
 
 #include <AP_HAL/AP_HAL.h>
-#if CONFIG_HAL_BOARD == HAL_BOARD_SITL && !defined(HAL_BUILD_AP_PERIPH)
+#if CONFIG_HAL_BOARD == HAL_BOARD_SITL
 #define SITL_RC_INPUT_CHANNELS 16
 
 #include "AP_HAL_SITL.h"
diff --git a/libraries/AP_HAL_SITL/RCOutput.cpp b/libraries/AP_HAL_SITL/RCOutput.cpp
index b92f546d5f..3a39d31c2b 100644
--- a/libraries/AP_HAL_SITL/RCOutput.cpp
+++ b/libraries/AP_HAL_SITL/RCOutput.cpp
@@ -50,7 +50,8 @@ void RCOutput::disable_ch(uint8_t ch)
 void RCOutput::write(uint8_t ch, uint16_t period_us)
 {
     if (safety_state == AP_HAL::Util::SAFETY_DISARMED) {
-        const uint32_t safety_mask = AP_BoardConfig::get_singleton()->get_safety_mask();
+        const auto *board_config = AP_BoardConfig::get_singleton();
+        const uint32_t safety_mask = board_config != nullptr? board_config->get_safety_mask() : 0;
         if (!(safety_mask & (1U<<ch))) {
             // implement safety pwm value
             period_us = 0;
diff --git a/libraries/AP_HAL_SITL/SITL_State.cpp b/libraries/AP_HAL_SITL/SITL_State.cpp
index f132df7eb0..56a66c709b 100644
--- a/libraries/AP_HAL_SITL/SITL_State.cpp
+++ b/libraries/AP_HAL_SITL/SITL_State.cpp
@@ -704,12 +704,8 @@ void SITL_State::multicast_servo_update(struct sitl_input &input)
 {
     for (uint8_t i=0; i<SITL_NUM_CHANNELS; i++) {
         const uint32_t mask = (1U<<i);
-        if (mc_servo[i] != 0) {
-            // we consider an output active if it has ever seen
-            // a non-zero value
-            servo_active_mask |= mask;
-        }
-        if (servo_active_mask & mask) {
+        const uint32_t can_mask = uint32_t(_sitl->can_servo_mask.get());
+        if (can_mask & mask) {
             input.servos[i] = mc_servo[i];
         }
     }
diff --git a/libraries/AP_HAL_SITL/SITL_State.h b/libraries/AP_HAL_SITL/SITL_State.h
index 4f9c3cf564..1bb57e161b 100644
--- a/libraries/AP_HAL_SITL/SITL_State.h
+++ b/libraries/AP_HAL_SITL/SITL_State.h
@@ -19,8 +19,6 @@ class HALSITL::SITL_State : public SITL_State_Common {
 public:
     void init(int argc, char * const argv[]);
 
-    uint16_t pwm_input[SITL_RC_INPUT_CHANNELS];
-    bool new_rc_input;
     void loop_hook(void);
     uint16_t base_port(void) const {
         return _base_port;
@@ -130,7 +128,6 @@ private:
     void multicast_state_send(void);
     void multicast_servo_update(struct sitl_input &input);
 
-    uint32_t servo_active_mask;
     uint16_t mc_servo[SITL_NUM_CHANNELS];
     void check_servo_input(void);
 };
diff --git a/libraries/AP_HAL_SITL/SITL_State_common.h b/libraries/AP_HAL_SITL/SITL_State_common.h
index 0223f5c5a0..dbbe4ef2e5 100644
--- a/libraries/AP_HAL_SITL/SITL_State_common.h
+++ b/libraries/AP_HAL_SITL/SITL_State_common.h
@@ -96,6 +96,8 @@ public:
     float voltage2_pin_voltage;  // pin 15
     float current2_pin_voltage;  // pin 14
 
+    uint16_t pwm_input[SITL_RC_INPUT_CHANNELS];
+    bool new_rc_input;
     uint16_t pwm_output[SITL_NUM_CHANNELS];
     bool output_ready = false;