drivers/actuators/modalai_esc: update to use mixer module and control allocator properly

- update motor mapping to use new UART_ESC_FUNC* auto generated params
 - add support for actuator_test msg to support Actuator Testing in QGC
 - modalai_fc-vX targets start driver if configured
 - keep track of ESC spin direction in own param
 - set ramp up param in MixerOutput to false

boards/modalai/fc-v1/init/rc.board_sensors

@@ -3,6 +3,11 @@
 # ModalAI FC-v1 specific board sensors init
 #------------------------------------------------------------------------------
 
+if param greater UART_ESC_CONFIG 0
+then
+	modalai_esc start
+fi
+
 board_adc start
 
 # Start Digital power monitors

boards/modalai/fc-v2/init/rc.board_sensors

@@ -3,11 +3,10 @@
 # ModalAI FC-v2 specific board sensors init
 #------------------------------------------------------------------------------
 
-# VOXL ESC - TODO
+if param greater UART_ESC_CONFIG 0
-#if param greater UART_ESC_CONFIG 0
+then
-#then
+	modalai_esc start
-#	modalai_esc -d /dev/ttyS4 start
+fi
-#fi
 
 board_adc start
 

src/drivers/actuators/modalai_esc/CMakeLists.txt

@@ -48,4 +48,6 @@ px4_add_module(
 	DEPENDS
 		px4_work_queue
 		mixer_module
+	MODULE_CONFIG
+		module.yaml
 	)

src/drivers/actuators/modalai_esc/modalai_esc.cpp

@@ -36,8 +36,6 @@
 #include "modalai_esc.hpp"
 #include "modalai_esc_serial.hpp"
 
-#define MODALAI_ESC_DEVICE_PATH		"/dev/uart_esc"
-
 // utility for running on VOXL and using driver as a bridge
 #define MODALAI_ESC_VOXL_BRIDGE_PORT	"/dev/ttyS4"
 
@@ -47,7 +45,8 @@
 const char *_device;
 
 ModalaiEsc::ModalaiEsc() :
-	OutputModuleInterface(MODULE_NAME, px4::wq_configurations::hp_default),
+	OutputModuleInterface(MODULE_NAME, px4::serial_port_to_wq(MODALAI_ESC_DEFAULT_PORT)),
+	_mixing_output{"UART_ESC", MODALAI_ESC_OUTPUT_CHANNELS, *this, MixingOutput::SchedulingPolicy::Auto, false, false},
 	_cycle_perf(perf_alloc(PC_ELAPSED, MODULE_NAME": cycle")),
 	_output_update_perf(perf_alloc(PC_INTERVAL, MODULE_NAME": output update interval"))
 {
@@ -123,18 +122,33 @@ int ModalaiEsc::load_params(uart_esc_params_t *params, ch_assign_t *map)
 {
 	int ret = PX4_OK;
 
+	// initialize out
+	for (int i = 0; i < MODALAI_ESC_OUTPUT_CHANNELS; i++) {
+		params->function_map[i] = (int)OutputFunction::Disabled;
+		params->direction_map[i] = 0;
+		params->motor_map[i] = 0;
+	}
+
 	param_get(param_find("UART_ESC_CONFIG"),  &params->config);
 	param_get(param_find("UART_ESC_MODE"),    &params->mode);
+	param_get(param_find("UART_ESC_BAUD"),    &params->baud_rate);
+
 	param_get(param_find("UART_ESC_T_PERC"),  &params->turtle_motor_percent);
 	param_get(param_find("UART_ESC_T_DEAD"),  &params->turtle_motor_deadband);
 	param_get(param_find("UART_ESC_T_EXPO"),  &params->turtle_motor_expo);
 	param_get(param_find("UART_ESC_T_MINF"),  &params->turtle_stick_minf);
 	param_get(param_find("UART_ESC_T_COSP"),  &params->turtle_cosphi);
-	param_get(param_find("UART_ESC_BAUD"),    &params->baud_rate);
+
-	param_get(param_find("UART_ESC_MOTOR1"),  &params->motor_map[0]);
+	param_get(param_find("UART_ESC_FUNC1"),  &params->function_map[0]);
-	param_get(param_find("UART_ESC_MOTOR2"),  &params->motor_map[1]);
+	param_get(param_find("UART_ESC_FUNC2"),  &params->function_map[1]);
-	param_get(param_find("UART_ESC_MOTOR3"),  &params->motor_map[2]);
+	param_get(param_find("UART_ESC_FUNC3"),  &params->function_map[2]);
-	param_get(param_find("UART_ESC_MOTOR4"),  &params->motor_map[3]);
+	param_get(param_find("UART_ESC_FUNC4"),  &params->function_map[3]);
+
+	param_get(param_find("UART_ESC_SDIR1"),  &params->direction_map[0]);
+	param_get(param_find("UART_ESC_SDIR2"),  &params->direction_map[1]);
+	param_get(param_find("UART_ESC_SDIR3"),  &params->direction_map[2]);
+	param_get(param_find("UART_ESC_SDIR4"),  &params->direction_map[3]);
+
 	param_get(param_find("UART_ESC_RPM_MIN"), &params->rpm_min);
 	param_get(param_find("UART_ESC_RPM_MAX"), &params->rpm_max);
 
@@ -174,6 +188,18 @@ int ModalaiEsc::load_params(uart_esc_params_t *params, ch_assign_t *map)
 		ret = PX4_ERROR;
 	}
 
+	for (int i = 0; i < MODALAI_ESC_OUTPUT_CHANNELS; i++) {
+		if (params->function_map[i] < (int)OutputFunction::Motor1 || params->function_map[i] > (int)OutputFunction::Motor4) {
+			PX4_ERR("Invalid parameter UART_ESC_FUNCX.  Only supports motors 1-4.  Please verify parameters.");
+			params->function_map[i] = 0;
+			ret = PX4_ERROR;
+
+		} else {
+			// Motor function IDs start at 100, Motor1 = 101, Motor2 = 102...
+			params->motor_map[i] = params->function_map[i] - 100;
+		}
+	}
+
 	for (int i = 0; i < MODALAI_ESC_OUTPUT_CHANNELS; i++) {
 		if (params->motor_map[i] == MODALAI_ESC_OUTPUT_DISABLED ||
 		    params->motor_map[i] < -(MODALAI_ESC_OUTPUT_CHANNELS) ||
@@ -185,7 +211,7 @@ int ModalaiEsc::load_params(uart_esc_params_t *params, ch_assign_t *map)
 
 		/* Can map -4 to 4, 0 being disabled.  Negative represents reverse direction */
 		map[i].number = abs(params->motor_map[i]);
-		map[i].direction = (params->motor_map[i] > 0) ? 1 : -1;
+		map[i].direction = (params->direction_map[i] > 0) ? -1 : 1;
 	}
 
 	return ret;
@@ -769,8 +795,11 @@ int ModalaiEsc::update_params()
 	ret = load_params(&_parameters, (ch_assign_t *)&_output_map);
 
 	if (ret == PX4_OK) {
+		_mixing_output.setAllDisarmedValues(0);
+		_mixing_output.setAllFailsafeValues(0);
 		_mixing_output.setAllMinValues(_parameters.rpm_min);
 		_mixing_output.setAllMaxValues(_parameters.rpm_max);
+
 		_rpm_fullscale = _parameters.rpm_max - _parameters.rpm_min;
 	}
 
@@ -1318,6 +1347,13 @@ void ModalaiEsc::Run()
 		}
 	}
 
+	if (!_outputs_on) {
+		if (_actuator_test_sub.updated()) {
+			// values are set in ActuatorTest::update, we just need to enable outputs to let them through
+			_outputs_on = true;
+		}
+	}
+
 	/* Don't process commands if outputs on */
 	if (!_outputs_on) {
 		if (_current_cmd.valid()) {
@@ -1438,10 +1474,17 @@ int ModalaiEsc::print_status()
 
 	PX4_INFO("Params: UART_ESC_CONFIG: %li", _parameters.config);
 	PX4_INFO("Params: UART_ESC_BAUD: %li", _parameters.baud_rate);
-	PX4_INFO("Params: UART_ESC_MOTOR1: %li", _parameters.motor_map[0]);
+
-	PX4_INFO("Params: UART_ESC_MOTOR2: %li", _parameters.motor_map[1]);
+	PX4_INFO("Params: UART_ESC_FUNC1: %li", _parameters.function_map[0]);
-	PX4_INFO("Params: UART_ESC_MOTOR3: %li", _parameters.motor_map[2]);
+	PX4_INFO("Params: UART_ESC_FUNC2: %li", _parameters.function_map[1]);
-	PX4_INFO("Params: UART_ESC_MOTOR4: %li", _parameters.motor_map[3]);
+	PX4_INFO("Params: UART_ESC_FUNC3: %li", _parameters.function_map[2]);
+	PX4_INFO("Params: UART_ESC_FUNC4: %li", _parameters.function_map[3]);
+
+	PX4_INFO("Params: UART_ESC_SDIR1: %li", _parameters.direction_map[0]);
+	PX4_INFO("Params: UART_ESC_SDIR2: %li", _parameters.direction_map[1]);
+	PX4_INFO("Params: UART_ESC_SDIR3: %li", _parameters.direction_map[2]);
+	PX4_INFO("Params: UART_ESC_SDIR4: %li", _parameters.direction_map[3]);
+
 	PX4_INFO("Params: UART_ESC_RPM_MIN: %li", _parameters.rpm_min);
 	PX4_INFO("Params: UART_ESC_RPM_MAX: %li", _parameters.rpm_max);
 
@@ -1449,6 +1492,8 @@ int ModalaiEsc::print_status()
 
 	for( int i = 0; i < MODALAI_ESC_OUTPUT_CHANNELS; i++){
 		PX4_INFO("-- ID: %i", i);
+		PX4_INFO("   Motor:           %i", _output_map[i].number);
+		PX4_INFO("   Direction:       %i", _output_map[i].direction);
 		PX4_INFO("   State:           %i", _esc_chans[i].state);
 		PX4_INFO("   Requested:       %i RPM", _esc_chans[i].rate_req);
 		PX4_INFO("   Measured:        %i RPM", _esc_chans[i].rate_meas);

src/drivers/actuators/modalai_esc/modalai_esc.hpp

@@ -46,6 +46,7 @@
 #include <uORB/topics/actuator_outputs.h>
 #include <uORB/topics/led_control.h>
 #include <uORB/topics/esc_status.h>
+#include <uORB/topics/actuator_test.h>
 
 #include "modalai_esc_serial.hpp"
 
@@ -146,7 +147,9 @@ private:
 		int32_t		baud_rate{MODALAI_ESC_DEFAULT_BAUD};
 		int32_t		rpm_min{MODALAI_ESC_DEFAULT_RPM_MIN};
 		int32_t		rpm_max{MODALAI_ESC_DEFAULT_RPM_MAX};
+		int32_t		function_map[MODALAI_ESC_OUTPUT_CHANNELS] {0, 0, 0, 0};
 		int32_t		motor_map[MODALAI_ESC_OUTPUT_CHANNELS] {1, 2, 3, 4};
+		int32_t		direction_map[MODALAI_ESC_OUTPUT_CHANNELS] {1, 1, 1, 1};
 	} uart_esc_params_t;
 
 	struct EscChan {
@@ -177,7 +180,7 @@ private:
 	} led_rsc_t;
 
 	ch_assign_t		_output_map[MODALAI_ESC_OUTPUT_CHANNELS] {{1, 1}, {2, 1}, {3, 1}, {4, 1}};
-	MixingOutput 		_mixing_output{"MODALAI_ESC", MODALAI_ESC_OUTPUT_CHANNELS, *this, MixingOutput::SchedulingPolicy::Auto, false, false};
+	MixingOutput 		_mixing_output;
 
 	perf_counter_t		_cycle_perf;
 	perf_counter_t		_output_update_perf;
@@ -189,6 +192,7 @@ private:
 	uORB::Subscription	_vehicle_control_mode_sub{ORB_ID(vehicle_control_mode)};
 	uORB::Subscription	_manual_control_setpoint_sub{ORB_ID(manual_control_setpoint)};
 	uORB::Subscription 	_parameter_update_sub{ORB_ID(parameter_update)};
+	uORB::Subscription 	_actuator_test_sub{ORB_ID(actuator_test)};
 	uORB::Subscription	_led_update_sub{ORB_ID(led_control)};
 
 	//uORB::Publication<actuator_outputs_s> _outputs_debug_pub{ORB_ID(actuator_outputs_debug)};
@@ -228,4 +232,5 @@ private:
 	int			flush_uart_rx();
 	int			check_for_esc_timeout();
 	void			mix_turtle_mode(uint16_t outputs[]);
+	void			handle_actuator_test();
 };

src/drivers/actuators/modalai_esc/modalai_esc_params.c

@@ -57,7 +57,7 @@ PARAM_DEFINE_INT32(UART_ESC_CONFIG, 0);
 PARAM_DEFINE_INT32(UART_ESC_BAUD, 250000);
 
 /**
- * Motor mappings for ModalAI ESC M004
+ * Motor mappings for ModalAI ESC
  *
  *  HW Channel Idexes (PX4 Indexes) (note: silkscreen shows 0 indexed)
  *         4(1)     3(4)
@@ -65,41 +65,16 @@ PARAM_DEFINE_INT32(UART_ESC_BAUD, 250000);
  *         1(3)     2(2)
  */
 
-/**
+// The following are auto generated params from control allocator pattern, put here for reference
- * UART ESC Motor 1 Mapping.  1-4 (negative for reversal).
- *
- * @group UART ESC
- * @min -4
- * @max 4
- */
-PARAM_DEFINE_INT32(UART_ESC_MOTOR1, 3);
 
-/**
+// Default ESC1 to motor2
- *UART ESC Motor 2 Mapping.  1-4 (negative for reversal).
+//PARAM_DEFINE_INT32(UART_ESC_FUNC1, 102);
- *
- * @group UART ESC
- * @min -4
- * @max 4
- */
-PARAM_DEFINE_INT32(UART_ESC_MOTOR2, 2);
 
-/**
+//PARAM_DEFINE_INT32(UART_ESC_FUNC2, 103);
- * UART ESC Motor 3 Mapping.  1-4 (negative for reversal).
- *
- * @group UART ESC
- * @min -4
- * @max 4
- */
-PARAM_DEFINE_INT32(UART_ESC_MOTOR3, 4);
 
-/**
+//PARAM_DEFINE_INT32(UART_ESC_FUNC3, 101);
- * UART ESC Motor 4 Mapping.  1-4 (negative for reversal).
+
- *
+//PARAM_DEFINE_INT32(UART_ESC_FUNC4, 104);
- * @group UART ESC
- * @min -4
- * @max 4
- */
-PARAM_DEFINE_INT32(UART_ESC_MOTOR4, 1);
 
 /**
  * UART ESC RPM Min
@@ -136,6 +111,42 @@ PARAM_DEFINE_INT32(UART_ESC_RPM_MAX, 15000);
  */
 PARAM_DEFINE_INT32(UART_ESC_MODE, 0);
 
+/**
+ * UART ESC ID 1 Spin Direction Flag
+ *
+ * @group UART ESC
+ * @value 0 - Default
+ * @value 1 - Reverse
+ */
+PARAM_DEFINE_INT32(UART_ESC_SDIR1, 0);
+
+/**
+ * UART ESC ID 2 Spin Direction Flag
+ *
+ * @group UART ESC
+ * @value 0 - Default
+ * @value 1 - Reverse
+ */
+PARAM_DEFINE_INT32(UART_ESC_SDIR2, 0);
+
+/**
+ * UART ESC ID 3 Spin Direction Flag
+ *
+ * @group UART ESC
+ * @value 0 - Default
+ * @value 1 - Reverse
+ */
+PARAM_DEFINE_INT32(UART_ESC_SDIR3, 0);
+
+/**
+ * UART ESC ID 4 Spin Direction Flag
+ *
+ * @group UART ESC
+ * @value 0 - Default
+ * @value 1 - Reverse
+ */
+PARAM_DEFINE_INT32(UART_ESC_SDIR4, 0);
+
 /**
  * UART ESC Turtle Mode Crash Flip Motor Percent
  *

src/drivers/actuators/modalai_esc/module.yaml

@@ -0,0 +1,26 @@
+module_name: MODALAI ESC Output
+actuator_output:
+  show_subgroups_if: 'UART_ESC_CONFIG>0'
+  config_parameters:
+    - param: 'UART_ESC_CONFIG'
+      label: 'Configure'
+      function: 'enable'
+    - param: 'UART_ESC_BAUD'
+      label: 'Bitrate'
+    - param: 'UART_ESC_RPM_MIN'
+      label: 'RPM Min'
+    - param: 'UART_ESC_RPM_MAX'
+      label: 'RPM Max'
+    - param: 'UART_ESC_SDIR1'
+      label: 'ESC 1 Spin Direction'
+    - param: 'UART_ESC_SDIR2'
+      label: 'ESC 2 Spin Direction'
+    - param: 'UART_ESC_SDIR3'
+      label: 'ESC 3 Spin Direction'
+    - param: 'UART_ESC_SDIR4'
+      label: 'ESC 4 Spin Direction'
+  output_groups:
+    - param_prefix: UART_ESC
+      group_label: 'ESCs'
+      channel_label: 'ESC'
+      num_channels: 4