[Firmware][I2C MultiPlexer] implement interface to use I2C MultiPlexer

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