Improvement: fix actuator test on uavcan ESCs that consume ArmingStatus

src/drivers/uavcan/actuators/esc.cpp

@@ -43,8 +43,6 @@
 #include <drivers/drv_hrt.h>
 #include <lib/atmosphere/atmosphere.h>
 
-#define MOTOR_BIT(x) (1<<(x))
-
 using namespace time_literals;
 
 UavcanEscController::UavcanEscController(uavcan::INode &node) :

src/drivers/uavcan/actuators/esc.hpp

@@ -47,13 +47,9 @@
 #include <uavcan/uavcan.hpp>
 #include <uavcan/equipment/esc/RawCommand.hpp>
 #include <uavcan/equipment/esc/Status.hpp>
-#include <lib/perf/perf_counter.h>
 #include <uORB/PublicationMulti.hpp>
-#include <uORB/topics/actuator_outputs.h>
 #include <uORB/topics/esc_status.h>
-#include <drivers/drv_hrt.h>
-#include <drivers/uavcan/node_info.hpp>
-#include <parameters/param.h>
+#include "../node_info.hpp"
 
 class UavcanEscController
 {

src/drivers/uavcan/arming_status.cpp

@@ -66,10 +66,9 @@ void UavcanArmingStatus::periodic_update(const uavcan::TimerEvent &)
 	if (_actuator_armed_sub.update(&actuator_armed)) {
 		uavcan::equipment::safety::ArmingStatus cmd;
 
-		if (actuator_armed.lockdown || actuator_armed.kill) {
-			cmd.status = cmd.STATUS_DISARMED;
+		bool lockdown_active = actuator_armed.lockdown || actuator_armed.termination || actuator_armed.kill;
 
-		} else if (actuator_armed.armed) {
+		if (!lockdown_active && (actuator_armed.armed || _is_actuator_test_running)) {
 			cmd.status = cmd.STATUS_FULLY_ARMED;
 
 		} else {

src/drivers/uavcan/arming_status.hpp

@@ -57,6 +57,8 @@ class UavcanArmingStatus
 	 */
 	int init();
 
+	void setActuatorTestRunning(bool running) {_is_actuator_test_running = running;}
+
 private:
 	/*
 	 * Max update rate to avoid exessive bus traffic
@@ -80,4 +82,5 @@ class UavcanArmingStatus
 
 	uORB::Subscription _actuator_armed_sub{ORB_ID(actuator_armed)};
 
+	bool _is_actuator_test_running = false;
 };

src/drivers/uavcan/uavcan_main.cpp

@@ -969,6 +969,10 @@ UavcanNode::Run()
 		}
 	}
 
+#if defined(CONFIG_UAVCAN_OUTPUTS_CONTROLLER)
+	_arming_status_controller.setActuatorTestRunning(_mixing_interface_esc.isActuatorTestRunning());
+#endif
+
 	perf_end(_cycle_perf);
 
 	pthread_mutex_unlock(&_node_mutex);

src/drivers/uavcan/uavcan_main.hpp

@@ -136,6 +136,8 @@ class UavcanMixingInterfaceESC : public OutputModuleInterface
 
 	MixingOutput &mixingOutput() { return _mixing_output; }
 
+	bool isActuatorTestRunning() const { return _mixing_output.isActuatorTestRunning(); }
+
 protected:
 	void Run() override;
 private:

src/lib/mixer_module/mixer_module.hpp

@@ -163,6 +163,7 @@ class MixingOutput : public ModuleParams
 	void setMaxTopicUpdateRate(unsigned max_topic_update_interval_us);
 
 	const actuator_armed_s &armed() const { return _armed; }
+	bool isActuatorTestRunning() const { return _actuator_test.inTestMode(); }
 
 	void setAllFailsafeValues(uint16_t value);
 	void setAllDisarmedValues(uint16_t value);