WIP: output modules native units

PX4 · Dec 5, 2023 · 7de3728 · 7de3728
1 parent d259386
commit 7de3728
Show file tree

Hide file tree

Showing 37 changed files with 138 additions and 129 deletions.
diff --git a/Tools/module_config/generate_params.py b/Tools/module_config/generate_params.py
@@ -302,12 +302,6 @@ def add_local_param(param_name, param_def):
         for key, label, param_suffix, description in standard_params_array:
             if key in standard_params:
 
-                # values must be in range of an uint16_t
-                if standard_params[key]['min'] < 0:
-                    raise Exception('minimum value for {:} expected >= 0 (got {:})'.format(key, standard_params[key]['min']))
-                if standard_params[key]['max'] >= 1<<16:
-                    raise Exception('maximum value for {:} expected <= {:} (got {:})'.format(key, 1<<16, standard_params[key]['max']))
-
                 if key == 'failsafe':
                     standard_params[key]['default'] = -1
                     standard_params[key]['min'] = -1
@@ -317,7 +311,7 @@ def add_local_param(param_name, param_def):
                         'short': channel_label+' ${i} '+label+' Value',
                         'long': description
                         },
-                    'type': 'int32',
+                    'type': 'float',
                     'instance_start': instance_start,
                     'instance_start_label': instance_start_label,
                     'num_instances': num_channels,

diff --git a/src/drivers/actuators/modal_io/modal_io.cpp b/src/drivers/actuators/modal_io/modal_io.cpp
@@ -877,7 +877,7 @@ void ModalIo::update_leds(vehicle_control_mode_s mode, led_control_s control)
 	}
 }
 
-void ModalIo::mix_turtle_mode(uint16_t outputs[MAX_ACTUATORS])
+void ModalIo::mix_turtle_mode(float outputs[MAX_ACTUATORS])
 {
 	bool use_pitch = true;
 	bool use_roll  = true;
@@ -1052,7 +1052,7 @@ void ModalIo::mix_turtle_mode(uint16_t outputs[MAX_ACTUATORS])
 }
 
 /* OutputModuleInterface */
-bool ModalIo::updateOutputs(bool stop_motors, uint16_t outputs[MAX_ACTUATORS],
+bool ModalIo::updateOutputs(bool stop_motors, float outputs[MAX_ACTUATORS],
 			    unsigned num_outputs, unsigned num_control_groups_updated)
 {
 	if (num_outputs != MODAL_IO_OUTPUT_CHANNELS) {

diff --git a/src/drivers/actuators/modal_io/modal_io.hpp b/src/drivers/actuators/modal_io/modal_io.hpp
@@ -75,7 +75,7 @@ class ModalIo : public ModuleBase<ModalIo>, public OutputModuleInterface
 	int print_status() override;
 
 	/** @see OutputModuleInterface */
-	bool updateOutputs(bool stop_motors, uint16_t outputs[MAX_ACTUATORS],
+	bool updateOutputs(bool stop_motors, float outputs[MAX_ACTUATORS],
 			   unsigned num_outputs, unsigned num_control_groups_updated) override;
 
 	virtual int	init();
@@ -225,6 +225,6 @@ class ModalIo : public ModuleBase<ModalIo>, public OutputModuleInterface
 	int 			parse_response(uint8_t *buf, uint8_t len, bool print_feedback);
 	int			flush_uart_rx();
 	int			check_for_esc_timeout();
-	void			mix_turtle_mode(uint16_t outputs[]);
+	void			mix_turtle_mode(float outputs[]);
 	void			handle_actuator_test();
 };
diff --git a/src/drivers/cyphal/Actuators/EscClient.hpp b/src/drivers/cyphal/Actuators/EscClient.hpp
@@ -97,15 +97,15 @@ class UavcanEscController : public UavcanPublisher
 		}
 	};
 
-	void update_outputs(bool stop_motors, uint16_t outputs[MAX_ACTUATORS], unsigned num_outputs)
+	void update_outputs(bool stop_motors, float outputs[MAX_ACTUATORS], unsigned num_outputs)
 	{
 		if (_port_id > 0) {
 			reg_udral_service_actuator_common_sp_Vector31_0_1 msg_sp {0};
 			size_t payload_size = reg_udral_service_actuator_common_sp_Vector31_0_1_SERIALIZATION_BUFFER_SIZE_BYTES_;
 
 			for (uint8_t i = 0; i < MAX_ACTUATORS; i++) {
 				if (i < num_outputs) {
-					msg_sp.value[i] = static_cast<float>(outputs[i]);
+					msg_sp.value[i] = outputs[i];
 
 				} else {
 					// "unset" values published as NaN

diff --git a/src/drivers/cyphal/Cyphal.cpp b/src/drivers/cyphal/Cyphal.cpp
@@ -392,7 +392,7 @@ void CyphalNode::sendHeartbeat()
 	}
 }
 
-bool UavcanMixingInterface::updateOutputs(bool stop_motors, uint16_t outputs[MAX_ACTUATORS], unsigned num_outputs,
+bool UavcanMixingInterface::updateOutputs(bool stop_motors, float outputs[MAX_ACTUATORS], unsigned num_outputs,
 		unsigned num_control_groups_updated)
 {
 	// Note: This gets called from MixingOutput from within its update() function

diff --git a/src/drivers/cyphal/Cyphal.hpp b/src/drivers/cyphal/Cyphal.hpp
@@ -87,7 +87,7 @@ class UavcanMixingInterface : public OutputModuleInterface
 		  _node_mutex(node_mutex),
 		  _pub_manager(pub_manager) {}
 
-	bool updateOutputs(bool stop_motors, uint16_t outputs[MAX_ACTUATORS],
+	bool updateOutputs(bool stop_motors, float outputs[MAX_ACTUATORS],
 			   unsigned num_outputs, unsigned num_control_groups_updated) override;
 
 	void printInfo() { _mixing_output.printStatus(); }

diff --git a/src/drivers/dshot/DShot.cpp b/src/drivers/dshot/DShot.cpp
@@ -336,7 +336,7 @@ void DShot::mixerChanged()
 	update_telemetry_num_motors();
 }
 
-bool DShot::updateOutputs(bool stop_motors, uint16_t outputs[MAX_ACTUATORS],
+bool DShot::updateOutputs(bool stop_motors, float outputs[MAX_ACTUATORS],
 			  unsigned num_outputs, unsigned num_control_groups_updated)
 {
 	if (!_outputs_on) {
@@ -388,7 +388,7 @@ bool DShot::updateOutputs(bool stop_motors, uint16_t outputs[MAX_ACTUATORS],
 
 		for (int i = 0; i < (int)num_outputs; i++) {
 
-			uint16_t output = outputs[i];
+			uint16_t output = math::constrain((int)lroundf(outputs[i]), 0, UINT16_MAX);
 
 			if (output == DSHOT_DISARM_VALUE) {
 				up_dshot_motor_command(i, DShot_cmd_motor_stop, telemetry_index == requested_telemetry_index);

diff --git a/src/drivers/dshot/DShot.h b/src/drivers/dshot/DShot.h
@@ -93,7 +93,7 @@ class DShot final : public ModuleBase<DShot>, public OutputModuleInterface
 
 	bool telemetry_enabled() const { return _telemetry != nullptr; }
 
-	bool updateOutputs(bool stop_motors, uint16_t outputs[MAX_ACTUATORS],
+	bool updateOutputs(bool stop_motors, float outputs[MAX_ACTUATORS],
 			   unsigned num_outputs, unsigned num_control_groups_updated) override;
 
 private:

diff --git a/src/drivers/linux_pwm_out/linux_pwm_out.cpp b/src/drivers/linux_pwm_out/linux_pwm_out.cpp
@@ -95,10 +95,17 @@ int LinuxPWMOut::task_spawn(int argc, char *argv[])
 	return PX4_ERROR;
 }
 
-bool LinuxPWMOut::updateOutputs(bool stop_motors, uint16_t outputs[MAX_ACTUATORS],
+bool LinuxPWMOut::updateOutputs(bool stop_motors, float outputs[MAX_ACTUATORS],
 				unsigned num_outputs, unsigned num_control_groups_updated)
 {
-	_pwm_out->send_output_pwm(outputs, num_outputs);
+	uint16_t out[MAX_ACTUATORS] {};
+
+	for (unsigned i = 0; i < num_outputs; ++i) {
+		out[i] = math::constrain((int)lroundf(outputs[i]), 0, UINT16_MAX);
+	}
+
+	_pwm_out->send_output_pwm(out, num_outputs);
+
 	return true;
 }
 

diff --git a/src/drivers/linux_pwm_out/linux_pwm_out.hpp b/src/drivers/linux_pwm_out/linux_pwm_out.hpp
@@ -71,7 +71,7 @@ class LinuxPWMOut : public ModuleBase<LinuxPWMOut>, public OutputModuleInterface
 
 	int init();
 
-	bool updateOutputs(bool stop_motors, uint16_t outputs[MAX_ACTUATORS],
+	bool updateOutputs(bool stop_motors, float outputs[MAX_ACTUATORS],
 			   unsigned num_outputs, unsigned num_control_groups_updated) override;
 
 private:

diff --git a/src/drivers/pca9685_pwm_out/PCA9685.cpp b/src/drivers/pca9685_pwm_out/PCA9685.cpp
@@ -95,15 +95,18 @@ int PCA9685::init()
 	return PX4_OK;
 }
 
-int PCA9685::updatePWM(const uint16_t *outputs, unsigned num_outputs)
+int PCA9685::updatePWM(const float outputs[PCA9685_PWM_CHANNEL_COUNT], unsigned num_outputs)
 {
 	if (num_outputs > PCA9685_PWM_CHANNEL_COUNT) {
 		num_outputs = PCA9685_PWM_CHANNEL_COUNT;
 		PX4_DEBUG("PCA9685 can only drive up to 16 channels");
 	}
 
 	uint16_t out[PCA9685_PWM_CHANNEL_COUNT];
-	memcpy(out, outputs, sizeof(uint16_t) * num_outputs);
+
+	for (unsigned i = 0; i < num_outputs; ++i) {
+		out[i] = (uint16_t)roundl((outputs[i] * _Freq * PCA9685_PWM_RES / (float)1e6)); // convert us to 12 bit resolution
+	}
 
 	for (unsigned i = 0; i < num_outputs; ++i) {
 		out[i] = calcRawFromPulse(out[i]);

diff --git a/src/drivers/pca9685_pwm_out/PCA9685.h b/src/drivers/pca9685_pwm_out/PCA9685.h
@@ -100,7 +100,7 @@ class PCA9685 : public device::I2C
 	 *
 	 * *output: pulse width, us
 	 */
-	int updatePWM(const uint16_t *outputs, unsigned num_outputs);
+	int updatePWM(const float outputs[PCA9685_PWM_CHANNEL_COUNT], unsigned num_outputs);
 
 	/*
 	 * Set PWM frequency to new value.

diff --git a/src/drivers/pca9685_pwm_out/main.cpp b/src/drivers/pca9685_pwm_out/main.cpp
@@ -70,7 +70,7 @@ class PCA9685Wrapper : public ModuleBase<PCA9685Wrapper>, public OutputModuleInt
 	static int custom_command(int argc, char *argv[]);
 	static int print_usage(const char *reason = nullptr);
 
-	bool updateOutputs(bool stop_motors, uint16_t *outputs, unsigned num_outputs,
+	bool updateOutputs(bool stop_motors, float outputs[MAX_ACTUATORS], unsigned num_outputs,
 			   unsigned num_control_groups_updated) override;
 
 	int print_status() override;
@@ -136,7 +136,7 @@ int PCA9685Wrapper::init()
 	return PX4_OK;
 }
 
-bool PCA9685Wrapper::updateOutputs(bool stop_motors, uint16_t *outputs, unsigned num_outputs,
+bool PCA9685Wrapper::updateOutputs(bool stop_motors, float outputs[MAX_ACTUATORS], unsigned num_outputs,
 				   unsigned num_control_groups_updated)
 {
 	if (state != STATE::RUNNING) { return false; }

diff --git a/src/drivers/pwm_out/PWMOut.cpp b/src/drivers/pwm_out/PWMOut.cpp
@@ -125,7 +125,7 @@ bool PWMOut::update_pwm_out_state(bool on)
 	return true;
 }
 
-bool PWMOut::updateOutputs(bool stop_motors, uint16_t outputs[MAX_ACTUATORS],
+bool PWMOut::updateOutputs(bool stop_motors, float outputs[MAX_ACTUATORS],
 			   unsigned num_outputs, unsigned num_control_groups_updated)
 {
 	/* output to the servos */
@@ -137,7 +137,8 @@ bool PWMOut::updateOutputs(bool stop_motors, uint16_t outputs[MAX_ACTUATORS],
 			}
 
 			if (_pwm_mask & (1 << i)) {
-				up_pwm_servo_set(i, outputs[i]);
+				uint16_t output = math::constrain((int)lroundf(outputs[i]), 0, UINT16_MAX);
+				up_pwm_servo_set(i, output);
 			}
 		}
 	}

diff --git a/src/drivers/pwm_out/PWMOut.hpp b/src/drivers/pwm_out/PWMOut.hpp
@@ -71,7 +71,7 @@ class PWMOut final : public ModuleBase<PWMOut>, public OutputModuleInterface
 	/** @see ModuleBase::print_status() */
 	int print_status() override;
 
-	bool updateOutputs(bool stop_motors, uint16_t outputs[MAX_ACTUATORS],
+	bool updateOutputs(bool stop_motors, float outputs[MAX_ACTUATORS],
 			   unsigned num_outputs, unsigned num_control_groups_updated) override;
 
 private:

diff --git a/src/drivers/px4io/px4io.cpp b/src/drivers/px4io/px4io.cpp
@@ -153,7 +153,7 @@ class PX4IO : public cdev::CDev, public ModuleBase<PX4IO>, public OutputModuleIn
 
 	uint16_t		system_status() const { return _status; }
 
-	bool updateOutputs(bool stop_motors, uint16_t outputs[MAX_ACTUATORS], unsigned num_outputs,
+	bool updateOutputs(bool stop_motors, float outputs[MAX_ACTUATORS], unsigned num_outputs,
 			   unsigned num_control_groups_updated) override;
 
 private:
@@ -361,12 +361,18 @@ PX4IO::~PX4IO()
 	perf_free(_interface_write_perf);
 }
 
-bool PX4IO::updateOutputs(bool stop_motors, uint16_t outputs[MAX_ACTUATORS],
+bool PX4IO::updateOutputs(bool stop_motors, float outputs[MAX_ACTUATORS],
 			  unsigned num_outputs, unsigned num_control_groups_updated)
 {
 	if (!_test_fmu_fail) {
 		/* output to the servos */
-		io_reg_set(PX4IO_PAGE_DIRECT_PWM, 0, outputs, num_outputs);
+		uint16_t outputs_uint16[MAX_ACTUATORS] {};
+
+		for (unsigned i = 0; i < num_outputs; i++) {
+			outputs_uint16[i] = math::constrain((int)lroundf(outputs[i]), 0, UINT16_MAX);
+		}
+
+		io_reg_set(PX4IO_PAGE_DIRECT_PWM, 0, outputs_uint16, num_outputs);
 	}
 
 	return true;

diff --git a/src/drivers/roboclaw/Roboclaw.cpp b/src/drivers/roboclaw/Roboclaw.cpp
@@ -154,11 +154,11 @@ int Roboclaw::initializeUART()
 	}
 }
 
-bool Roboclaw::updateOutputs(bool stop_motors, uint16_t outputs[MAX_ACTUATORS],
-			     unsigned num_outputs, unsigned num_control_groups_updated)
+bool Roboclaw::updateOutputs(bool stop_motors, float outputs[MAX_ACTUATORS], unsigned num_outputs,
+			     unsigned num_control_groups_updated)
 {
-	float right_motor_output = ((float)outputs[0] - 128.0f) / 127.f;
-	float left_motor_output = ((float)outputs[1] - 128.0f) / 127.f;
+	float right_motor_output = (outputs[0] - 128.0f) / 127.f;
+	float left_motor_output = (outputs[1] - 128.0f) / 127.f;
 
 	if (stop_motors) {
 		setMotorSpeed(Motor::Right, 0.f);

diff --git a/src/drivers/roboclaw/Roboclaw.hpp b/src/drivers/roboclaw/Roboclaw.hpp
@@ -77,8 +77,8 @@ class Roboclaw : public ModuleBase<Roboclaw>, public OutputModuleInterface
 	void Run() override;
 
 	/** @see OutputModuleInterface */
-	bool updateOutputs(bool stop_motors, uint16_t outputs[MAX_ACTUATORS],
-			   unsigned num_outputs, unsigned num_control_groups_updated) override;
+	bool updateOutputs(bool stop_motors, float outputs[MAX_ACTUATORS], unsigned num_outputs,
+			   unsigned num_control_groups_updated) override;
 
 	void setMotorSpeed(Motor motor, float value); ///< rev/sec
 	void setMotorDutyCycle(Motor motor, float value);

diff --git a/src/drivers/tap_esc/TAP_ESC.cpp b/src/drivers/tap_esc/TAP_ESC.cpp
@@ -241,7 +241,7 @@ void TAP_ESC::send_tune_packet(EscbusTunePacket &tune_packet)
 	tap_esc_common::send_packet(_uart_fd, buzzer_packet, -1);
 }
 
-bool TAP_ESC::updateOutputs(bool stop_motors, uint16_t outputs[MAX_ACTUATORS], unsigned num_outputs,
+bool TAP_ESC::updateOutputs(bool stop_motors, float outputs[MAX_ACTUATORS], unsigned num_outputs,
 			    unsigned num_control_groups_updated)
 {
 	if (_initialized) {

diff --git a/src/drivers/tap_esc/TAP_ESC.hpp b/src/drivers/tap_esc/TAP_ESC.hpp
@@ -99,7 +99,7 @@ class TAP_ESC : public ModuleBase<TAP_ESC>, public OutputModuleInterface
 
 	int init();
 
-	bool updateOutputs(bool stop_motors, uint16_t outputs[MAX_ACTUATORS],
+	bool updateOutputs(bool stop_motors, float outputs[MAX_ACTUATORS],
 			   unsigned num_outputs, unsigned num_control_groups_updated) override;
 
 private:

diff --git a/src/drivers/uavcan/actuators/esc.cpp b/src/drivers/uavcan/actuators/esc.cpp
@@ -70,7 +70,7 @@ UavcanEscController::init()
 }
 
 void
-UavcanEscController::update_outputs(bool stop_motors, uint16_t outputs[MAX_ACTUATORS], unsigned num_outputs)
+UavcanEscController::update_outputs(bool stop_motors, float outputs[MAX_ACTUATORS], unsigned num_outputs)
 {
 	/*
 	 * Rate limiting - we don't want to congest the bus
@@ -90,7 +90,7 @@ UavcanEscController::update_outputs(bool stop_motors, uint16_t outputs[MAX_ACTUA
 	uavcan::equipment::esc::RawCommand msg;
 
 	for (unsigned i = 0; i < num_outputs; i++) {
-		if (stop_motors || outputs[i] == DISARMED_OUTPUT_VALUE) {
+		if (stop_motors || !PX4_ISFINITE(outputs[i]) || (outputs[i] >= DISARMED_OUTPUT_VALUE)) {
 			msg.cmd.push_back(static_cast<unsigned>(0));
 
 		} else {

diff --git a/src/drivers/uavcan/actuators/esc.hpp b/src/drivers/uavcan/actuators/esc.hpp
@@ -69,7 +69,7 @@ class UavcanEscController
 
 	int init();
 
-	void update_outputs(bool stop_motors, uint16_t outputs[MAX_ACTUATORS], unsigned num_outputs);
+	void update_outputs(bool stop_motors, float outputs[MAX_ACTUATORS], unsigned num_outputs);
 
 	/**
 	 * Sets the number of rotors and enable timer

diff --git a/src/drivers/uavcan/actuators/servo.cpp b/src/drivers/uavcan/actuators/servo.cpp
@@ -45,15 +45,15 @@ UavcanServoController::UavcanServoController(uavcan::INode &node) :
 }
 
 void
-UavcanServoController::update_outputs(bool stop_motors, uint16_t outputs[MAX_ACTUATORS], unsigned num_outputs)
+UavcanServoController::update_outputs(bool stop_motors, float outputs[MAX_ACTUATORS], unsigned num_outputs)
 {
 	uavcan::equipment::actuator::ArrayCommand msg;
 
 	for (unsigned i = 0; i < num_outputs; ++i) {
 		uavcan::equipment::actuator::Command cmd;
 		cmd.actuator_id = i;
 		cmd.command_type = uavcan::equipment::actuator::Command::COMMAND_TYPE_UNITLESS;
-		cmd.command_value = (float)outputs[i] / 500.f - 1.f; // [-1, 1]
+		cmd.command_value = outputs[i];
 
 		msg.commands.push_back(cmd);
 	}

diff --git a/src/drivers/uavcan/actuators/servo.hpp b/src/drivers/uavcan/actuators/servo.hpp
@@ -52,7 +52,7 @@ class UavcanServoController
 	UavcanServoController(uavcan::INode &node);
 	~UavcanServoController() = default;
 
-	void update_outputs(bool stop_motors, uint16_t outputs[MAX_ACTUATORS], unsigned num_outputs);
+	void update_outputs(bool stop_motors, float outputs[MAX_ACTUATORS], unsigned num_outputs);
 
 private:
 	uavcan::INode								&_node;

diff --git a/src/drivers/uavcan/module.yaml b/src/drivers/uavcan/module.yaml
@@ -12,16 +12,16 @@ actuator_output:
       group_label: 'ESCs'
       channel_label: 'ESC'
       standard_params:
-        min: { min: 0, max: 8191, default: 1 }
-        max: { min: 0, max: 8191, default: 8191 }
+        min: { min: -8192, max: 8191, default: 1 }
+        max: { min: -8192, max: 8191, default: 8191 }
         failsafe: { min: 0, max: 8191 }
       num_channels: 8
     - param_prefix: UAVCAN_SV
       group_label: 'Servos'
       channel_label: 'Servo'
       standard_params:
-        disarmed: { min: 0, max: 1000, default: 500 }
-        min: { min: 0, max: 1000, default: 0 }
-        max: { min: 0, max: 1000, default: 1000 }
-        failsafe: { min: 0, max: 1000 }
+        disarmed: { min: -1, max: 1, default: 0 }
+        min: { min: -1, max: 1, default: -1 }
+        max: { min: -1, max: 1, default: 1 }
+        failsafe: { min: -1, max: 1 }
       num_channels: 8
diff --git a/src/drivers/uavcan/uavcan_main.cpp b/src/drivers/uavcan/uavcan_main.cpp
@@ -975,7 +975,7 @@ UavcanNode::Run()
 }
 
 #if defined(CONFIG_UAVCAN_OUTPUTS_CONTROLLER)
-bool UavcanMixingInterfaceESC::updateOutputs(bool stop_motors, uint16_t outputs[MAX_ACTUATORS], unsigned num_outputs,
+bool UavcanMixingInterfaceESC::updateOutputs(bool stop_motors, float outputs[MAX_ACTUATORS], unsigned num_outputs,
 		unsigned num_control_groups_updated)
 {
 	_esc_controller.update_outputs(stop_motors, outputs, num_outputs);
@@ -1005,7 +1005,7 @@ void UavcanMixingInterfaceESC::mixerChanged()
 	_esc_controller.set_rotor_count(rotor_count);
 }
 
-bool UavcanMixingInterfaceServo::updateOutputs(bool stop_motors, uint16_t outputs[MAX_ACTUATORS], unsigned num_outputs,
+bool UavcanMixingInterfaceServo::updateOutputs(bool stop_motors, float outputs[MAX_ACTUATORS], unsigned num_outputs,
 		unsigned num_control_groups_updated)
 {
 	_servo_controller.update_outputs(stop_motors, outputs, num_outputs);