From 54c001376d431fc7fd3a69c3fed5223ddfab91a1 Mon Sep 17 00:00:00 2001 From: chfriedrich98 Date: Wed, 24 Sep 2025 10:19:45 +0200 Subject: [PATCH] driver: hiwonder encoder motor module --- boards/auterion/fmu-v6s/init/rc.board_sensors | 4 + boards/auterion/fmu-v6s/rover.px4board | 8 +- src/drivers/drv_sensor.h | 2 + src/drivers/hiwonder_emm/CMakeLists.txt | 44 ++++ src/drivers/hiwonder_emm/HiwonderEMM.cpp | 233 ++++++++++++++++++ src/drivers/hiwonder_emm/HiwonderEMM.hpp | 128 ++++++++++ src/drivers/hiwonder_emm/Kconfig | 5 + src/drivers/hiwonder_emm/module.yaml | 26 ++ 8 files changed, 447 insertions(+), 3 deletions(-) create mode 100644 src/drivers/hiwonder_emm/CMakeLists.txt create mode 100644 src/drivers/hiwonder_emm/HiwonderEMM.cpp create mode 100644 src/drivers/hiwonder_emm/HiwonderEMM.hpp create mode 100644 src/drivers/hiwonder_emm/Kconfig create mode 100644 src/drivers/hiwonder_emm/module.yaml diff --git a/boards/auterion/fmu-v6s/init/rc.board_sensors b/boards/auterion/fmu-v6s/init/rc.board_sensors index 4c4be283d7..a711613fa3 100644 --- a/boards/auterion/fmu-v6s/init/rc.board_sensors +++ b/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 diff --git a/boards/auterion/fmu-v6s/rover.px4board b/boards/auterion/fmu-v6s/rover.px4board index 52d414fd50..90da47302b 100644 --- a/boards/auterion/fmu-v6s/rover.px4board +++ b/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 diff --git a/src/drivers/drv_sensor.h b/src/drivers/drv_sensor.h index 1f74122014..a3295a21e3 100644 --- a/src/drivers/drv_sensor.h +++ b/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 */ diff --git a/src/drivers/hiwonder_emm/CMakeLists.txt b/src/drivers/hiwonder_emm/CMakeLists.txt new file mode 100644 index 0000000000..198de763af --- /dev/null +++ b/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 + ) diff --git a/src/drivers/hiwonder_emm/HiwonderEMM.cpp b/src/drivers/hiwonder_emm/HiwonderEMM.cpp new file mode 100644 index 0000000000..6dc8fa7a98 --- /dev/null +++ b/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); +} diff --git a/src/drivers/hiwonder_emm/HiwonderEMM.hpp b/src/drivers/hiwonder_emm/HiwonderEMM.hpp new file mode 100644 index 0000000000..bdcab6c5b1 --- /dev/null +++ b/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 +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +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, 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); +}; diff --git a/src/drivers/hiwonder_emm/Kconfig b/src/drivers/hiwonder_emm/Kconfig new file mode 100644 index 0000000000..eda4a7f8f5 --- /dev/null +++ b/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 diff --git a/src/drivers/hiwonder_emm/module.yaml b/src/drivers/hiwonder_emm/module.yaml new file mode 100644 index 0000000000..95d9b4dd50 --- /dev/null +++ b/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