driver: hiwonder encoder motor module

boards/auterion/fmu-v6s/init/rc.board_sensors

@@ -37,6 +37,10 @@ then
 	set INA_CONFIGURED yes
 fi
 
+if param compare HIWONDER_EMM_EN 1
+then
+	hiwonder_emm start
+fi
 
 if param compare SENS_EN_INA228 1
 then

boards/auterion/fmu-v6s/rover.px4board

@@ -5,12 +5,14 @@ CONFIG_MODULES_FW_AUTOTUNE_ATTITUDE_CONTROL=n
 CONFIG_MODULES_FW_MODE_MANAGER=n
 CONFIG_MODULES_FW_LATERAL_LONGITUDINAL_CONTROL=n
 CONFIG_MODULES_FW_RATE_CONTROL=n
-CONFIG_MODULES_LANDING_TARGET_ESTIMATOR=n
 CONFIG_MODULES_MC_ATT_CONTROL=n
 CONFIG_MODULES_MC_AUTOTUNE_ATTITUDE_CONTROL=n
 CONFIG_MODULES_MC_HOVER_THRUST_ESTIMATOR=n
 CONFIG_MODULES_MC_POS_CONTROL=n
 CONFIG_MODULES_MC_RATE_CONTROL=n
 CONFIG_MODULES_VTOL_ATT_CONTROL=n
-# CONFIG_EKF2_WIND is not set
-CONFIG_MODULES_DIFFERENTIAL_DRIVE=y
+CONFIG_DRIVERS_ROBOCLAW=y
+CONFIG_MODULES_ROVER_ACKERMANN=y
+CONFIG_MODULES_ROVER_DIFFERENTIAL=y
+CONFIG_MODULES_ROVER_MECANUM=y
+CONFIG_DRIVERS_HIWONDER_EMM=y

src/drivers/drv_sensor.h

@@ -260,6 +260,8 @@
 
 #define DRV_INS_DEVTYPE_SBG		0xEC
 
+#define DRV_MOTOR_DEVTYPE_HIWONDER_EMM  0xEB
+
 #define DRV_DEVTYPE_UNUSED		0xff
 
 #endif /* _DRV_SENSOR_H */

src/drivers/hiwonder_emm/CMakeLists.txt

@@ -0,0 +1,44 @@
+############################################################################
+#
+#   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.
+#
+############################################################################
+px4_add_module(
+	MODULE drivers__hiwonder_emm
+	MAIN hiwonder_emm
+	COMPILE_FLAGS
+	SRCS
+		HiwonderEMM.cpp
+		HiwonderEMM.hpp
+	MODULE_CONFIG
+		module.yaml
+	DEPENDS
+		mixer_module
+	)

src/drivers/hiwonder_emm/HiwonderEMM.cpp

@@ -0,0 +1,233 @@
+/****************************************************************************
+ *
+ *   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.
+ *
+ ****************************************************************************/
+
+#include "HiwonderEMM.hpp"
+
+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()
+{
+	int ret = I2C::init();
+
+	if (ret != PX4_OK) {
+		PX4_ERR("I2C init failed. Error: %d", ret);
+		return ret;
+	}
+
+	const uint8_t set_motor_type[2] = {MOTOR_TYPE_ADDR, MOTOR_TYPE_JGB37_520_12V_110RPM};
+	ret = transfer(set_motor_type, sizeof(set_motor_type), nullptr, 0);
+
+	if (ret != PX4_OK) {
+		PX4_ERR("Failed to set motor type. Error: %d", ret);
+		return ret;
+	}
+
+	const uint8_t set_motor_polarity[2] = {MOTOR_ENCODER_POLARITY_ADDR, 0};
+	ret = transfer(set_motor_polarity, sizeof(set_motor_polarity), nullptr, 0);
+
+	if (ret != PX4_OK) {
+		PX4_ERR("Failed to set encoder polarity. Error: %d", ret);
+		return ret;
+	}
+
+	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[CHANNEL_COUNT];
+
+	for (unsigned i = 0; i < num_outputs && i < CHANNEL_COUNT; i++) {
+		speed_values[i] = (uint8_t)(outputs[i] - 128);
+	}
+
+	set_motor_speed(speed_values, CHANNEL_COUNT);
+
+	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();
+
+	if (ret != PX4_OK) {
+		PX4_ERR("I2C probe failed. Error: %d", ret);
+		return ret;
+	}
+
+	int adc_value{0};
+	ret = read_adc(adc_value);
+
+	if (ret != PX4_OK) {
+		PX4_ERR("Failed to probe Hiwonder EMM. Error: %d", ret);
+		return ret;
+	}
+
+	PX4_INFO("Hiwonder EMM found");
+
+	return PX4_OK;
+}
+
+int HiwonderEMM::read_adc(int &adc_value)
+{
+	const uint8_t cmd = ADC_BAT_ADDR;
+	uint8_t buf[2] = {};
+	const int ret = transfer(&cmd, sizeof(cmd), buf, sizeof(buf));
+
+	if (ret != PX4_OK) {
+		PX4_ERR("Failed to read ADC. Error: %d", ret);
+		adc_value = 0;
+		return ret;
+	}
+
+	adc_value = buf[0] | (buf[1] << 8);
+	return ret;
+}
+
+int HiwonderEMM::read_encoder_counts(int32_t *encoder_counts, const uint8_t count)
+{
+	const uint8_t cmd = MOTOR_ENCODER_TOTAL_ADDR;
+	uint8_t buf[CHANNEL_COUNT * 4]; // Each encoder count is a 32-bit integer (4 bytes)
+	const int ret = transfer(&cmd, sizeof(cmd), buf, sizeof(buf));
+
+	if (ret != PX4_OK) {
+		PX4_ERR("Failed to read encoder counts. Error: %d", ret);
+		return ret;
+	}
+
+	for (unsigned i = 0; i < count && i < CHANNEL_COUNT; i++) {
+		encoder_counts[i] = buf[i * 4] | (buf[i * 4 + 1] << 8) | (buf[i * 4 + 2] << 16) | (buf[i * 4 + 3] << 24);
+	}
+
+	return ret;
+}
+
+int HiwonderEMM::set_motor_speed(const uint8_t *speed_values, const uint8_t count)
+{
+	uint8_t cmd[1 + CHANNEL_COUNT];
+	cmd[0] = MOTOR_FIXED_SPEED_ADDR;
+
+	for (unsigned int i = 0; i < count && i < CHANNEL_COUNT; i++) {
+		cmd[i + 1] = speed_values[i];
+	}
+
+	const int ret = transfer(cmd, sizeof(cmd), nullptr, 0);
+
+	if (ret != PX4_OK) {
+		PX4_ERR("Failed to set motor speed. Error: %d", ret);
+	}
+
+	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.hpp

@@ -0,0 +1,128 @@
+/****************************************************************************
+ *
+ *   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 uint8_t I2C_ADDR = 0x34;                     // I2C address
+static constexpr uint8_t ADC_BAT_ADDR = 0;                    // Voltage address
+static constexpr uint8_t MOTOR_TYPE_ADDR = 0x14;              // Set the motor type
+static constexpr uint8_t MOTOR_ENCODER_POLARITY_ADDR = 21;    // Set the encoder direction polarity
+static constexpr uint8_t MOTOR_FIXED_PWM_ADDR = 31;           // Fixed PWM control, open loop
+static constexpr uint8_t MOTOR_FIXED_SPEED_ADDR = 51;         // Fixed speed control, closed loop
+static constexpr uint8_t MOTOR_ENCODER_TOTAL_ADDR = 60;       // Total pulse value of 4 encoder motors
+static constexpr uint8_t MOTOR_TYPE_WITHOUT_ENCODER = 0;      // Motor without encoder
+static constexpr uint8_t MOTOR_TYPE_TT = 1; 		      // TT encoder motor
+static constexpr uint8_t MOTOR_TYPE_N20 = 2;                  // N20 encoder motor
+static constexpr uint8_t MOTOR_TYPE_JGB37_520_12V_110RPM = 3; // JGB37 encoder motor
+static constexpr uint8_t I2CBUS = 1;                          // I2C bus number
+static constexpr uint8_t CHANNEL_COUNT = 4;                   // Number of output channels
+
+class HiwonderEMM : public ModuleBase<HiwonderEMM>, public OutputModuleInterface, public device::I2C
+{
+public:
+	HiwonderEMM();
+	~HiwonderEMM() override = default;
+
+	// I2C
+	int init() override;
+
+	// ModuleBase
+	static int task_spawn(int argc, char *argv[]);
+	static int custom_command(int argc, char *argv[]);
+	static int print_usage(const char *reason = nullptr);
+	int print_status() override;
+
+	// OutputModuleInterface
+	bool updateOutputs(uint16_t *outputs, unsigned num_outputs,
+			   unsigned num_control_groups_updated) override;
+
+protected:
+	int probe() override;
+
+private:
+	MixingOutput _mixing_output {
+		"EMM",
+		CHANNEL_COUNT,
+		*this,
+		MixingOutput::SchedulingPolicy::Auto,
+		false
+	};
+	void Run() override;
+
+	/**
+	 * @brief Read the input voltage supplied to the motor driver.
+	 * @param adc_value [mV] Reference to store the ADC value.
+	 * @return OK if the transfer was successful, -errno otherwise.
+	 */
+	int read_adc(int &adc_value);
+
+	/**
+	 * @brief Read the encoder counts for all motors.
+	 * @param encoder_counts Array to store the encoder counts for each motor.
+	 * @param count Number of encoder counts to read (should be equal to CHANNEL_COUNT).
+	 * @return OK if the transfer was successful, -errno otherwise.
+	 */
+	int read_encoder_counts(int32_t *encoder_counts, const uint8_t count);
+
+	/**
+	 * @brief Set the speed values for the motors.
+	 * @param speed_values Array of speed values for each motor in the range [0, 255].
+	 *                     128 represents stop, values below 128 represent reverse motion,
+	 *                     and values above 128 represent forward motion.
+	 * @param count Number of speed values provided in the array (should be equal to CHANNEL_COUNT).
+	 * @return OK if the transfer was successful, -errno otherwise.
+	 */
+	int set_motor_speed(const uint8_t *speed_values, const uint8_t count);
+};

src/drivers/hiwonder_emm/Kconfig

@@ -0,0 +1,5 @@
+menuconfig DRIVERS_HIWONDER_EMM
+	bool "hiwonder_emm"
+	default n
+	---help---
+		Enable support for hiwonder encoder motor module

src/drivers/hiwonder_emm/module.yaml

@@ -0,0 +1,26 @@
+module_name: Hiwonder EMM Driver
+
+actuator_output:
+  output_groups:
+    - param_prefix: EMM
+      channel_label: 'Channel'
+      standard_params:
+        disarmed: { min: 0, max: 255, default: 128 }
+        min: { min: 0, max: 0, default: 0 }
+        max: { min: 255, max: 255, default: 255 }
+        failsafe: { min: 0, max: 255}
+      num_channels: 4
+
+parameters:
+  - group: Hiwonder EMM
+    definitions:
+      HIWONDER_EMM_EN:
+        description:
+          short: Enable the Hiwonder EMM output driver
+          long: |
+            Enable the Hiwonder EMM output driver.
+        type: enum
+        values:
+              0: Disabled
+              1: Enabled
+        default: 0