refactor

Author: chfriedrich98

src/drivers/hiwonder_emm/CMakeLists.txt

@@ -35,9 +35,8 @@ px4_add_module(
 	MAIN hiwonder_emm
 	COMPILE_FLAGS
 	SRCS
-		main.cpp
 		HiwonderEMM.cpp
-		HiwonderEMM.h
+		HiwonderEMM.hpp
 	MODULE_CONFIG
 		module.yaml
 	DEPENDS

src/drivers/hiwonder_emm/HiwonderEMM.cpp

@@ -31,15 +31,12 @@
  *
  ****************************************************************************/
 
-#include <px4_log.h>
-#include <px4_defines.h>
-#include <cmath>
-#include "HiwonderEMM.h"
+#include "HiwonderEMM.hpp"
 
-HiwonderEMM::HiwonderEMM(int bus, int addr):
-	I2C(DRV_MOTOR_DEVTYPE_HIWONDER_EMM, MODULE_NAME, bus, addr, 400000)
+HiwonderEMM::HiwonderEMM() :
+	OutputModuleInterface(MODULE_NAME, px4::wq_configurations::hp_default), I2C(DRV_MOTOR_DEVTYPE_HIWONDER_EMM,
+			MODULE_NAME, I2CBUS, I2C_ADDR, 400000)
 {
-
 }
 
 int HiwonderEMM::init()
@@ -51,27 +48,58 @@ int HiwonderEMM::init()
 		return ret;
 	}
 
-	const uint8_t cmd[2] = {MOTOR_TYPE_ADDR, MOTOR_TYPE_JGB37_520_12V_110RPM};
-	ret = transfer(cmd, 2, nullptr, 0);
+	const uint8_t set_motor_type[2] = {MOTOR_TYPE_ADDR, MOTOR_TYPE_JGB37_520_12V_110RPM};
+	ret = transfer(set_motor_type, 2, nullptr, 0);
 
 	if (ret != PX4_OK) {
 		PX4_ERR("Failed to set motor type. Error: %d", ret);
 		return ret;
 	}
 
-	const uint8_t cmd2[2] = {MOTOR_ENCODER_POLARITY_ADDR, 0};
-	ret = transfer(cmd2, 2, nullptr, 0);
+	const uint8_t set_motor_polarity[2] = {MOTOR_ENCODER_POLARITY_ADDR, 0};
+	ret = transfer(set_motor_polarity, 2, nullptr, 0);
 
 	if (ret != PX4_OK) {
 		PX4_ERR("Failed to set encoder polarity. Error: %d", ret);
 		return ret;
 	}
 
-	PX4_INFO("Hiwonder EMM initialized");
+	this->ChangeWorkQueue(px4::device_bus_to_wq(this->get_device_id()));
+
+	PX4_INFO("Hiwonder EMM running on I2C bus %d address 0x%.2x", this->get_device_bus(), this->get_device_address());
+
+	ScheduleNow();
 
 	return PX4_OK;
 }
 
+bool HiwonderEMM::updateOutputs(uint16_t *outputs, unsigned num_outputs,
+				unsigned num_control_groups_updated)
+{
+	uint8_t speed_values[4];
+
+	for (unsigned i = 0; i < num_outputs && i < CHANNEL_COUNT; i++) {
+		speed_values[i] = (uint8_t)(outputs[i] - 128);
+	}
+
+	set_motor_speed(speed_values);
+
+	return true;
+}
+
+void HiwonderEMM::Run()
+{
+	if (should_exit()) {
+		ScheduleClear();
+		_mixing_output.unregister();
+		exit_and_cleanup();
+		return;
+	}
+
+	_mixing_output.update();
+	_mixing_output.updateSubscriptions(false);
+}
+
 int HiwonderEMM::probe()
 {
 	int ret = I2C::probe();
@@ -126,3 +154,61 @@ int HiwonderEMM::set_motor_speed(const uint8_t speed_values[4])
 
 	return ret;
 }
+
+int HiwonderEMM::print_usage(const char *reason)
+{
+	if (reason) {
+		PX4_WARN("%s\n", reason);
+	}
+
+	PRINT_MODULE_DESCRIPTION(
+		R"DESCR_STR(
+### Description
+Hiwonder encoder motor module driver for PX4.
+)DESCR_STR");
+
+    PRINT_MODULE_USAGE_NAME("hiwonder_emm", "driver");
+    PRINT_MODULE_USAGE_COMMAND_DESCR("start", "Start the task");
+    PRINT_MODULE_USAGE_DEFAULT_COMMANDS();
+
+    return 0;
+}
+
+int HiwonderEMM::print_status() {
+    int ret =  ModuleBase::print_status();
+    PX4_INFO("HiwonderEMM @I2C Bus %d, address 0x%.2x",
+            this->get_device_bus(),
+            this->get_device_address());
+
+    return ret;
+}
+
+int HiwonderEMM::custom_command(int argc, char **argv) {
+    return PX4_OK;
+}
+
+int HiwonderEMM::task_spawn(int argc, char **argv) {
+	auto *instance = new HiwonderEMM();
+
+	if (instance) {
+		_object.store(instance);
+		_task_id = task_id_is_work_queue;
+
+		if (instance->init() == PX4_OK) {
+			return PX4_OK;
+		}
+
+	} else {
+		PX4_ERR("alloc failed");
+	}
+
+	delete instance;
+	_object.store(nullptr);
+	_task_id = -1;
+
+	return PX4_ERROR;
+}
+
+extern "C" __EXPORT int hiwonder_emm_main(int argc, char *argv[]){
+	return HiwonderEMM::main(argc, argv);
+}

src/drivers/hiwonder_emm/HiwonderEMM.h

@@ -0,0 +1,100 @@
+/****************************************************************************
+ *
+ *   Copyright (c) 2025 PX4 Development Team. All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ *
+ * 1. Redistributions of source code must retain the above copyright
+ *    notice, this list of conditions and the following disclaimer.
+ * 2. Redistributions in binary form must reproduce the above copyright
+ *    notice, this list of conditions and the following disclaimer in
+ *    the documentation and/or other materials provided with the
+ *    distribution.
+ * 3. Neither the name PX4 nor the names of its contributors may be
+ *    used to endorse or promote products derived from this software
+ *    without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
+ * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
+ * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ *
+ ****************************************************************************/
+
+/**
+ * @file HiwonderEMM.hpp
+ *
+ * Driver for the Hiwonder 4-channel encoder motor driver over I2C.
+ *
+ * Product: https://www.hiwonder.com/products/4-channel-encoder-motor-driver
+ *
+ */
+
+#pragma once
+
+#include <cstdint>
+#include <drivers/device/i2c.h>
+#include <px4_boardconfig.h>
+#include <px4_log.h>
+#include <drivers/device/device.h>
+#include <lib/mixer_module/mixer_module.hpp>
+#include <px4_platform_common/module.h>
+#include <lib/perf/perf_counter.h>
+#include <drivers/drv_hrt.h>
+#include <px4_platform_common/getopt.h>
+#include <px4_platform_common/sem.hpp>
+#include <px4_defines.h>
+
+static constexpr int I2C_ADDR = 0x34;                     // I2C address
+static constexpr int ADC_BAT_ADDR = 0;                    // Voltage address
+static constexpr int MOTOR_TYPE_ADDR = 0x14;              // Set the motor type
+static constexpr int MOTOR_ENCODER_POLARITY_ADDR = 21;    // Set the encoder direction polarity
+static constexpr int MOTOR_FIXED_PWM_ADDR = 31;           // Fixed PWM control, open loop
+static constexpr int MOTOR_FIXED_SPEED_ADDR = 51;         // Fixed speed control, closed loop
+static constexpr int MOTOR_ENCODER_TOTAL_ADDR = 60;       // Total pulse value of 4 encoder motors
+static constexpr int MOTOR_TYPE_WITHOUT_ENCODER = 0;      // Motor without encoder
+static constexpr int MOTOR_TYPE_TT = 1; 		  // TT encoder motor
+static constexpr int MOTOR_TYPE_N20 = 2;                  // N20 encoder motor
+static constexpr int MOTOR_TYPE_JGB37_520_12V_110RPM = 3; // JGB37 encoder motor
+static constexpr int BUS_FREQUENCY = 100000;              // I2C bus frequency
+static constexpr int I2CBUS = 1;                          // I2C bus number
+static constexpr int CHANNEL_COUNT = 4;                   // Number of output channels
+
+class HiwonderEMM : public ModuleBase<HiwonderEMM>, public OutputModuleInterface, public device::I2C
+{
+public:
+	HiwonderEMM();
+	~HiwonderEMM() override = default;
+	int init() override;
+	static int task_spawn(int argc, char *argv[]);
+	static int custom_command(int argc, char *argv[]);
+	static int print_usage(const char *reason = nullptr);
+	bool updateOutputs(uint16_t *outputs, unsigned num_outputs,
+			   unsigned num_control_groups_updated) override;
+	int print_status() override;
+
+protected:
+	int probe() override;
+
+private:
+	MixingOutput _mixing_output {
+		"EMM",
+		CHANNEL_COUNT,
+		*this,
+		MixingOutput::SchedulingPolicy::Auto,
+		false
+	};
+	void Run() override;
+	int read_adc(int &adc_value);
+	int set_motor_speed(const uint8_t speed_values[4]);
+};