From 5f14b2c597f01d094c201643437fc00dbedd943c Mon Sep 17 00:00:00 2001 From: Moju Zhao Date: Thu, 16 Jan 2025 16:33:28 +0900 Subject: [PATCH 1/3] [Firmware][STM32CubeIDE] remove unnecessary main.c in STM32CubeIDE project, which causes build warning --- aerial_robot_nerve/neuron/neuron_f4/STM32CubeIDE/.project | 5 ----- .../spinal/mcu_project/boards/stm32H7/STM32CubeIDE/.project | 5 ----- .../mcu_project/boards/stm32H7_v2/STM32CubeIDE/.project | 5 ----- 3 files changed, 15 deletions(-) diff --git a/aerial_robot_nerve/neuron/neuron_f4/STM32CubeIDE/.project b/aerial_robot_nerve/neuron/neuron_f4/STM32CubeIDE/.project index d57cfdbdf..aa08b50ca 100644 --- a/aerial_robot_nerve/neuron/neuron_f4/STM32CubeIDE/.project +++ b/aerial_robot_nerve/neuron/neuron_f4/STM32CubeIDE/.project @@ -72,11 +72,6 @@ 1 $%7BPARENT-1-PROJECT_LOC%7D/Src/main.cpp - - Application/User/main.cpp - 1 - $%7BPARENT-1-PROJECT_LOC%7D/Src/main.cpp - Application/User/spi.c 1 diff --git a/aerial_robot_nerve/spinal/mcu_project/boards/stm32H7/STM32CubeIDE/.project b/aerial_robot_nerve/spinal/mcu_project/boards/stm32H7/STM32CubeIDE/.project index c94c659dc..9758c33e1 100644 --- a/aerial_robot_nerve/spinal/mcu_project/boards/stm32H7/STM32CubeIDE/.project +++ b/aerial_robot_nerve/spinal/mcu_project/boards/stm32H7/STM32CubeIDE/.project @@ -67,11 +67,6 @@ 1 PARENT-1-PROJECT_LOC/Src/lwip.c - - Application/User/main.c - 1 - PARENT-1-PROJECT_LOC/Src/main.c - Application/User/main.cpp 1 diff --git a/aerial_robot_nerve/spinal/mcu_project/boards/stm32H7_v2/STM32CubeIDE/.project b/aerial_robot_nerve/spinal/mcu_project/boards/stm32H7_v2/STM32CubeIDE/.project index 195694d77..318ef0de2 100644 --- a/aerial_robot_nerve/spinal/mcu_project/boards/stm32H7_v2/STM32CubeIDE/.project +++ b/aerial_robot_nerve/spinal/mcu_project/boards/stm32H7_v2/STM32CubeIDE/.project @@ -67,11 +67,6 @@ 1 PARENT-1-PROJECT_LOC/Src/lwip.c - - Application/User/main.c - 1 - PARENT-1-PROJECT_LOC/Src/main.c - Application/User/main.cpp 1 From fbb86d7b3dfd1f571fc263b8c6bac41bd3491117 Mon Sep 17 00:00:00 2001 From: Moju Zhao Date: Thu, 16 Jan 2025 16:44:42 +0900 Subject: [PATCH 2/3] [Firmware][Spinal][I2C MultiPlexer] implement interface to use I2C Multiplexer --- .../sensors/i2c_multiplexer/switcher.cpp | 35 +++++++++++++++++++ .../sensors/i2c_multiplexer/switcher.h | 25 +++++++++++++ 2 files changed, 60 insertions(+) create mode 100644 aerial_robot_nerve/spinal/mcu_project/lib/Jsk_Lib/sensors/i2c_multiplexer/switcher.cpp create mode 100644 aerial_robot_nerve/spinal/mcu_project/lib/Jsk_Lib/sensors/i2c_multiplexer/switcher.h diff --git a/aerial_robot_nerve/spinal/mcu_project/lib/Jsk_Lib/sensors/i2c_multiplexer/switcher.cpp b/aerial_robot_nerve/spinal/mcu_project/lib/Jsk_Lib/sensors/i2c_multiplexer/switcher.cpp new file mode 100644 index 000000000..f51d6f306 --- /dev/null +++ b/aerial_robot_nerve/spinal/mcu_project/lib/Jsk_Lib/sensors/i2c_multiplexer/switcher.cpp @@ -0,0 +1,35 @@ +/* +****************************************************************************** +* File Name : switcher.cpp +* Description : I2C Multi Plexer Interface +****************************************************************************** +*/ + +#include "switcher.h" + +namespace I2C_MultiPlexer +{ + namespace + { + I2C_HandleTypeDef* hi2c_; + + uint8_t hub_address_ = 0x70; // the default I2C adress of PCA9547D. TODO: reconfigurable + }; + + void init(I2C_HandleTypeDef* hi2c) + { + hi2c_ = hi2c; + } + + bool changeChannel(uint8_t ch) + { + uint8_t val[1]; + val[0] = ch & 0x07; // assign the channel + val[0] = ch | 0x08; // enable switch the channel + + int i2c_status = HAL_I2C_Master_Transmit(hi2c_, hub_address_, val, 1, 100); + + if(i2c_status == HAL_OK) return true; + else return false; + } +}; diff --git a/aerial_robot_nerve/spinal/mcu_project/lib/Jsk_Lib/sensors/i2c_multiplexer/switcher.h b/aerial_robot_nerve/spinal/mcu_project/lib/Jsk_Lib/sensors/i2c_multiplexer/switcher.h new file mode 100644 index 000000000..6f3b226bd --- /dev/null +++ b/aerial_robot_nerve/spinal/mcu_project/lib/Jsk_Lib/sensors/i2c_multiplexer/switcher.h @@ -0,0 +1,25 @@ +/* +****************************************************************************** +* File Name : switcher.h +* Description : I2C Multi Plexer Interface +****************************************************************************** +*/ + +#ifndef __cplusplus +#error "Please define __cplusplus, because this is a c++ based file " +#endif + +#ifndef __I2C_MULTIPLEXER_H +#define __I2C_MULTIPLEXER_H + +#include "config.h" + +namespace I2C_MultiPlexer +{ + void init(I2C_HandleTypeDef* hi2c); + bool changeChannel(uint8_t ch); +}; + + + +#endif From fd60c321f9314a92f5d896198eb6de5095153494 Mon Sep 17 00:00:00 2001 From: Moju Zhao Date: Thu, 16 Jan 2025 17:38:26 +0900 Subject: [PATCH 3/3] [Spinal][Multi Encoder][WIP] implement a task to read the angle value from multiple mag encoders --- .../mcu_project/boards/stm32H7/Src/main.c | 37 ++++++++++++++++++- .../Jsk_Lib/sensors/encoder/mag_encoder.cpp | 6 +++ .../lib/Jsk_Lib/sensors/encoder/mag_encoder.h | 1 + 3 files changed, 42 insertions(+), 2 deletions(-) diff --git a/aerial_robot_nerve/spinal/mcu_project/boards/stm32H7/Src/main.c b/aerial_robot_nerve/spinal/mcu_project/boards/stm32H7/Src/main.c index ae01c748a..13ba48e85 100644 --- a/aerial_robot_nerve/spinal/mcu_project/boards/stm32H7/Src/main.c +++ b/aerial_robot_nerve/spinal/mcu_project/boards/stm32H7/Src/main.c @@ -41,6 +41,7 @@ #include "sensors/baro/baro_ms5611.h" #include "sensors/gps/gps_ublox.h" #include "sensors/encoder/mag_encoder.h" +#include "sensors/i2c_multiplexer/switcher.h" #include "battery_status/battery_status.h" @@ -101,6 +102,9 @@ osSemaphoreId uartTxSemHandle; /* USER CODE BEGIN PV */ osMailQId canMsgMailHandle; +osThreadId multiEncoderHandle; +std::map encoders_; + ros::NodeHandle nh_; /* sensor instances */ @@ -146,7 +150,7 @@ void servoTaskCallback(void const * argument); void coreTaskEvokeCb(void const * argument); /* USER CODE BEGIN PFP */ - +void encoderTaskCallback(void const * argument); /* USER CODE END PFP */ /* Private user code ---------------------------------------------------------*/ @@ -261,7 +265,15 @@ int main(void) Spine::init(&hfdcan1, &nh_, &estimator_, LED1_GPIO_Port, LED1_Pin); Spine::useRTOS(&canMsgMailHandle); // use RTOS for CAN in spianl #endif - + + I2C_MultiPlexer::init(&hi2c3); + // two encoders + const char* topic1 = "encoder_angle1"; + encoders_[0] = MagEncoder(topic1); + const char* topic2 = "encoder_angle2"; + encoders_[1] = MagEncoder(topic2); + encoders_.at(0).init(&hi2c3, &nh_); + encoders_.at(1).init(&hi2c3, &nh_); /* USER CODE END 2 */ /* Create the mutex(es) */ @@ -340,6 +352,11 @@ int main(void) /* USER CODE BEGIN RTOS_THREADS */ /* add threads, ... */ + + /* definition and creation of task to read from multiple encoders */ + osThreadDef(multiEncoder, encoderTaskCallback, osPriorityLow, 0, 256); + multiEncoderHandle = osThreadCreate(osThread(multiEncoder), NULL); + /* USER CODE END RTOS_THREADS */ /* Start scheduler */ @@ -1036,6 +1053,22 @@ static void MX_GPIO_Init(void) } /* USER CODE BEGIN 4 */ +void encoderTaskCallback(void const * argument) +{ + /* USER CODE BEGIN rosPublishTask */ + for(;;) + { + for(std::map::iterator it = encoders_.begin(); it != encoders_.begin(); it++) { + uint8_t ch = it->first; + int i2c_status = I2C_MultiPlexer::changeChannel(ch); + if(i2c_status == HAL_OK) it->second.update(); + } + + osDelay(1); // timer is controlled inside each `update` function + + } + /* USER CODE END rosPublishTask */ +} /* USER CODE END 4 */ diff --git a/aerial_robot_nerve/spinal/mcu_project/lib/Jsk_Lib/sensors/encoder/mag_encoder.cpp b/aerial_robot_nerve/spinal/mcu_project/lib/Jsk_Lib/sensors/encoder/mag_encoder.cpp index 0a716418d..81a3c9363 100644 --- a/aerial_robot_nerve/spinal/mcu_project/lib/Jsk_Lib/sensors/encoder/mag_encoder.cpp +++ b/aerial_robot_nerve/spinal/mcu_project/lib/Jsk_Lib/sensors/encoder/mag_encoder.cpp @@ -10,12 +10,18 @@ #endif #include "sensors/encoder/mag_encoder.h" +#include MagEncoder::MagEncoder(): angle_pub_("encoder_angle", &angle_msg_) { } +MagEncoder::MagEncoder(const char * topic_name): + angle_pub_(topic_name, &angle_msg_) +{ +} + void MagEncoder::init(I2C_HandleTypeDef* hi2c, ros::NodeHandle* nh) { hi2c_ = hi2c; diff --git a/aerial_robot_nerve/spinal/mcu_project/lib/Jsk_Lib/sensors/encoder/mag_encoder.h b/aerial_robot_nerve/spinal/mcu_project/lib/Jsk_Lib/sensors/encoder/mag_encoder.h index 0da83620e..000ec842f 100644 --- a/aerial_robot_nerve/spinal/mcu_project/lib/Jsk_Lib/sensors/encoder/mag_encoder.h +++ b/aerial_robot_nerve/spinal/mcu_project/lib/Jsk_Lib/sensors/encoder/mag_encoder.h @@ -21,6 +21,7 @@ class MagEncoder { public: MagEncoder(); + MagEncoder(const char * topic_name); ~MagEncoder(){}; static const uint8_t AS5600_I2C_ADDRESS = 0x6c; // 0x36 << 1; NOTE: STM: i2c_address << 1 !!!