From d7a02bbc0c941ecb38a9d542fc436420bc338bb2 Mon Sep 17 00:00:00 2001 From: Masahiro Wada Date: Tue, 16 Jul 2024 17:32:23 +0900 Subject: [PATCH 1/2] HWDEV-2176 add new tug encoder support --- lexxpluss_apps/prj.conf | 1 + lexxpluss_apps/src/board_controller.cpp | 2 +- lexxpluss_apps/src/main.cpp | 4 + lexxpluss_apps/src/tug_encoder_controller.cpp | 123 ++++++++++++++++++ lexxpluss_apps/src/tug_encoder_controller.hpp | 42 ++++++ lexxpluss_apps/src/zcan_main.cpp | 4 + lexxpluss_apps/src/zcan_tug_encoder.hpp | 75 +++++++++++ 7 files changed, 250 insertions(+), 1 deletion(-) create mode 100644 lexxpluss_apps/src/tug_encoder_controller.cpp create mode 100644 lexxpluss_apps/src/tug_encoder_controller.hpp create mode 100644 lexxpluss_apps/src/zcan_tug_encoder.hpp diff --git a/lexxpluss_apps/prj.conf b/lexxpluss_apps/prj.conf index ef02de6..2beecf6 100644 --- a/lexxpluss_apps/prj.conf +++ b/lexxpluss_apps/prj.conf @@ -50,4 +50,5 @@ CONFIG_WDT_DISABLE_AT_BOOT=n CONFIG_MAXBOTIX=y CONFIG_ADIS16470=n CONFIG_IIM42652=y +CONFIG_I2C=y diff --git a/lexxpluss_apps/src/board_controller.cpp b/lexxpluss_apps/src/board_controller.cpp index 2d91f8e..37588b9 100644 --- a/lexxpluss_apps/src/board_controller.cpp +++ b/lexxpluss_apps/src/board_controller.cpp @@ -1260,7 +1260,7 @@ class state_controller { // Variables Implemented ac.poll(); bmu.poll(); mbd.poll(); - + switch (state) { case POWER_STATE::OFF: set_new_state(mc.is_plugged() ? POWER_STATE::POST : POWER_STATE::WAIT_SW); diff --git a/lexxpluss_apps/src/main.cpp b/lexxpluss_apps/src/main.cpp index 5bb185d..09fe594 100644 --- a/lexxpluss_apps/src/main.cpp +++ b/lexxpluss_apps/src/main.cpp @@ -38,6 +38,7 @@ #include "runaway_detector.hpp" #include "uss_controller.hpp" #include "gpio_controller.hpp" +#include "tug_encoder_controller.hpp" namespace { @@ -53,6 +54,7 @@ K_THREAD_STACK_DEFINE(pgv_controller_stack, 2048); K_THREAD_STACK_DEFINE(runaway_detector_stack, 2048); K_THREAD_STACK_DEFINE(uss_controller_stack, 2048); K_THREAD_STACK_DEFINE(gpio_controller_stack, 2048); +K_THREAD_STACK_DEFINE(tug_encoder_controller_stack, 2048); K_THREAD_STACK_DEFINE(zcan_main_stack, 2048); #define RUN(name, prio) \ @@ -268,6 +270,7 @@ int main() lexxhard::runaway_detector::init(); lexxhard::uss_controller::init(); lexxhard::gpio_controller::init(); + lexxhard::tug_encoder_controller::init(); RUN(actuator_controller, 2); RUN(adc_reader, 2); @@ -280,6 +283,7 @@ int main() RUN(pgv_controller, 1); RUN(uss_controller, 2); RUN(gpio_controller, 2); + RUN(tug_encoder_controller, 2); RUN(runaway_detector, 4); RUN(zcan_main, 5); // zcan_main thread must be started at last. diff --git a/lexxpluss_apps/src/tug_encoder_controller.cpp b/lexxpluss_apps/src/tug_encoder_controller.cpp new file mode 100644 index 0000000..b18d72c --- /dev/null +++ b/lexxpluss_apps/src/tug_encoder_controller.cpp @@ -0,0 +1,123 @@ +/* + * Copyright (c) 2024, LexxPluss Inc. + * 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. + * + * 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 +#include +#include +#include +#include +#include +#include "tug_encoder_controller.hpp" + +namespace lexxhard::tug_encoder_controller { + +LOG_MODULE_REGISTER(tug_encoder); + +char __aligned(4) msgq_buffer[8 * sizeof (msg)]; + +class tug_encoder_controller_impl { +public: + int init() { + k_msgq_init(&msgq, msgq_buffer, sizeof (msg), 8); + + dev = DEVICE_DT_GET(DT_NODELABEL(i2c2)); + if (!device_is_ready(dev)) { + LOG_ERR("TUG Encoder device not found"); + return -1; + } + + return 0; + } + + void run() { + if (!device_is_ready(dev)) { + LOG_ERR("TUG Encoder device not found"); + return; + } + + while (true) { + fetch_angle(); + while (k_msgq_put(&msgq, &message, K_NO_WAIT) != 0) + k_msgq_purge(&msgq); + k_msleep(1); + } + } + + void tug_encoder_info(const shell *shell) const { + float const angle_deg = message.angle * 360.0f / 4096.0f; + shell_print(shell, "Angle: %f[deg]\n", angle_deg); + } + +private: + void fetch_angle() { + uint8_t angle_h; + if (i2c_reg_read_byte(dev, AS5600_ADDR, AS5600_REG_ANGLE_H, &angle_h)) { + LOG_ERR("Failed to read angle high byte\n"); + return; + } + uint8_t angle_l; + if (i2c_reg_read_byte(dev, AS5600_ADDR, AS5600_REG_ANGLE_L, &angle_l)) { + LOG_ERR("Failed to read angle low byte\n"); + return; + } + + message.angle = (static_cast(angle_h) << 8) | angle_l; + } + msg message; + const device *dev{nullptr}; + + static constexpr uint8_t AS5600_ADDR{0x36}; + static constexpr uint8_t AS5600_REG_ANGLE_H{0x0E}; + static constexpr uint8_t AS5600_REG_ANGLE_L{0x0F}; +} impl; + +int tug_encoder_info(const shell *shell, size_t argc, char **argv) +{ + impl.tug_encoder_info(shell); + return 0; +} + +SHELL_STATIC_SUBCMD_SET_CREATE(sub_tug_encoder, + SHELL_CMD(info, NULL, "TUG Encoder information", tug_encoder_info), + SHELL_SUBCMD_SET_END +); +SHELL_CMD_REGISTER(tug_encoder, &sub_tug_encoder, "TUG Encoder commands", NULL); + +void init() +{ + impl.init(); +} + +void run(void *p1, void *p2, void *p3) +{ + impl.run(); +} + +k_thread thread; +k_msgq msgq; + +} + diff --git a/lexxpluss_apps/src/tug_encoder_controller.hpp b/lexxpluss_apps/src/tug_encoder_controller.hpp new file mode 100644 index 0000000..2537428 --- /dev/null +++ b/lexxpluss_apps/src/tug_encoder_controller.hpp @@ -0,0 +1,42 @@ +/* + * Copyright (c) 2024, LexxPluss Inc. + * 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. + * + * 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. + */ +#pragma once + +#include + +namespace lexxhard::tug_encoder_controller { + +#define TUG_ENCODER_CAN_DATA_LENGTH 2 + +struct msg { + uint16_t angle; +} __attribute__((aligned(2))); + +void init(); +void run(void *p1, void *p2, void *p3); +extern k_thread thread; +extern k_msgq msgq; +} + diff --git a/lexxpluss_apps/src/zcan_main.cpp b/lexxpluss_apps/src/zcan_main.cpp index 1b70cb8..0c0d1ac 100644 --- a/lexxpluss_apps/src/zcan_main.cpp +++ b/lexxpluss_apps/src/zcan_main.cpp @@ -33,6 +33,7 @@ #include "zcan_uss.hpp" #include "zcan_dfu.hpp" #include "zcan_gpio.hpp" +#include "zcan_tug_encoder.hpp" #include "zcan_main.hpp" namespace lexxhard::zcan_main { @@ -62,6 +63,7 @@ class { pgv.init(); uss.init(); gpio.init(); + tug_encoder.init(); return 0; } void run() { @@ -75,6 +77,7 @@ class { pgv.poll(); uss.poll(); gpio.poll(); + tug_encoder.poll(); k_usleep(1); } } @@ -89,6 +92,7 @@ class { lexxhard::zcan_pgv::zcan_pgv pgv; lexxhard::zcan_uss::zcan_uss uss; lexxhard::zcan_gpio::zcan_gpio gpio; + lexxhard::zcan_tug_encoder::zcan_tug_encoder tug_encoder; } impl; void init() diff --git a/lexxpluss_apps/src/zcan_tug_encoder.hpp b/lexxpluss_apps/src/zcan_tug_encoder.hpp new file mode 100644 index 0000000..7152eba --- /dev/null +++ b/lexxpluss_apps/src/zcan_tug_encoder.hpp @@ -0,0 +1,75 @@ +/* + * Copyright (c) 2023, LexxPltug_encoder Inc. + * 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. + * + * 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. + */ + +#pragma once + +#include +#include +#include +#include "tug_encoder_controller.hpp" + +#define CAN_ID_TUG_ENCODER 0x210 +#define CAN_DATA_LENGTH_TUG_ENCODER 2 + +namespace lexxhard::zcan_tug_encoder { + +LOG_MODULE_REGISTER(zcan_tug_encoder); + +class zcan_tug_encoder { +public: + void init() { + dev = DEVICE_DT_GET(DT_NODELABEL(can2)); + if (!device_is_ready(dev)){ + LOG_ERR("CAN_2 is not ready"); + return; + } + } + void poll() { + tug_encoder_controller::msg message; + + while (k_msgq_get(&tug_encoder_controller::msgq, &message, K_NO_WAIT) == 0) { + uint8_t packedData[CAN_DATA_LENGTH_TUG_ENCODER] {0}; + packedData[0] = message.angle >> 8; + packedData[1] = message.angle & 0xFF; + + // set data to CAN frame + can_frame frame{ + .id = CAN_ID_TUG_ENCODER, + .dlc = CAN_DATA_LENGTH_TUG_ENCODER, + .data = {0} + }; + + // copy packedData to CAN frame data + memcpy(frame.data, packedData, CAN_DATA_LENGTH_TUG_ENCODER); + + can_send(dev, &frame, K_MSEC(100), nullptr, nullptr); + } + } +private: + const device *dev{nullptr}; +}; + +} + From b32468a3ef3c6890bcafd7141f4f7aff36712960 Mon Sep 17 00:00:00 2001 From: Masahiro Wada Date: Thu, 8 Aug 2024 22:37:57 +0900 Subject: [PATCH 2/2] Update lexxpluss_apps/src/zcan_tug_encoder.hpp Co-authored-by: MIURA Yasuyuki --- lexxpluss_apps/src/zcan_tug_encoder.hpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/lexxpluss_apps/src/zcan_tug_encoder.hpp b/lexxpluss_apps/src/zcan_tug_encoder.hpp index 7152eba..b0990a7 100644 --- a/lexxpluss_apps/src/zcan_tug_encoder.hpp +++ b/lexxpluss_apps/src/zcan_tug_encoder.hpp @@ -1,5 +1,5 @@ /* - * Copyright (c) 2023, LexxPltug_encoder Inc. + * Copyright (c) 2024, LexxPltug_encoder Inc. * All rights reserved. * * Redistribution and use in source and binary forms, with or without