[Multi Encoder Access][WIP] implement a sample to access multiple encoders via I2C MultiPlexer

aerial_robot_nerve/neuron/neuron_f4/STM32CubeIDE/.project

@@ -72,11 +72,6 @@
 			<type>1</type>
 			<locationURI>$%7BPARENT-1-PROJECT_LOC%7D/Src/main.cpp</locationURI>
 		</link>
-		<link>
-			<name>Application/User/main.cpp</name>
-			<type>1</type>
-			<locationURI>$%7BPARENT-1-PROJECT_LOC%7D/Src/main.cpp</locationURI>
-		</link>
 		<link>
 			<name>Application/User/spi.c</name>
 			<type>1</type>

aerial_robot_nerve/spinal/mcu_project/boards/stm32H7/STM32CubeIDE/.project

@@ -67,11 +67,6 @@
 			<type>1</type>
 			<locationURI>PARENT-1-PROJECT_LOC/Src/lwip.c</locationURI>
 		</link>
-		<link>
-			<name>Application/User/main.c</name>
-			<type>1</type>
-			<locationURI>PARENT-1-PROJECT_LOC/Src/main.c</locationURI>
-		</link>
 		<link>
 			<name>Application/User/main.cpp</name>
 			<type>1</type>

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<int, MagEncoder> 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<int, MagEncoder>::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 */
 

aerial_robot_nerve/spinal/mcu_project/boards/stm32H7_v2/STM32CubeIDE/.project

@@ -67,11 +67,6 @@
 			<type>1</type>
 			<locationURI>PARENT-1-PROJECT_LOC/Src/lwip.c</locationURI>
 		</link>
-		<link>
-			<name>Application/User/main.c</name>
-			<type>1</type>
-			<locationURI>PARENT-1-PROJECT_LOC/Src/main.c</locationURI>
-		</link>
 		<link>
 			<name>Application/User/main.cpp</name>
 			<type>1</type>

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 <string>
 
 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;

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 !!!

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;
+  }
+};

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