implemented power service

Author: ClemensElflein

CMakeLists.txt

@@ -57,6 +57,11 @@ target_sources(${CMAKE_PROJECT_NAME} PRIVATE
         src/status_led.h
         src/services/imu_service/imu_service.cpp
         src/services/imu_service/imu_service.hpp
+        src/services/power_service/power_service.cpp
+        src/services/power_service/power_service.hpp
+        src/services/power_service/bq_2576/bq_2576.cpp
+        src/services/power_service/bq_2576/bq_2576.hpp
+
 )
 
 # Add include paths
@@ -78,6 +83,7 @@ target_link_libraries(${CMAKE_PROJECT_NAME} PUBLIC
 )
 
 target_add_service(${CMAKE_PROJECT_NAME} ImuService ${CMAKE_CURRENT_SOURCE_DIR}/services/imu_service.json)
+target_add_service(${CMAKE_PROJECT_NAME} PowerService ${CMAKE_CURRENT_SOURCE_DIR}/services/power_service.json)
 
 set_target_properties(${CMAKE_PROJECT_NAME}
         PROPERTIES SUFFIX ".elf")

boards/XCORE/board.c

@@ -264,4 +264,7 @@ bool mmc_lld_is_write_protected(MMCDriver *mmcp) {
  */
 void boardInit(void) {
 
+  
+        initBoardPeriphs();
+    
 }

boards/XCORE/board_ex.c

@@ -0,0 +1,53 @@
+//
+// Created by clemens on 05.11.24.
+//
+#include "board_ex.h"
+
+#include "ch.h"
+#include "hal.h"
+
+static I2CConfig i2c1Config = {0};
+static I2CConfig i2c2Config = {0};
+static I2CConfig i2c4Config = {0};
+
+void initBoardPeriphs(void) {
+  /**
+   * Init I2C1
+   */
+  i2cAcquireBus(&I2CD1);
+
+  // Calculated depending on clock source, check reference manual
+  i2c1Config.timingr = 0xE14;
+
+  if (i2cStart(&I2CD1, &i2c1Config) != HAL_RET_SUCCESS) {
+    while (1)
+      ;
+  }
+  i2cReleaseBus(&I2CD1);
+  /**
+   * Init I2C4 with 100kHz
+   */
+  i2cAcquireBus(&I2CD2);
+
+  // Calculated depending on clock source, check reference manual
+  i2c2Config.timingr = 0xE14;
+
+  if (i2cStart(&I2CD2, &i2c2Config) != HAL_RET_SUCCESS) {
+    while (1)
+      ;
+  }
+  i2cReleaseBus(&I2CD2);
+  /**
+   * Init I2C4 with 100kHz
+   */
+  i2cAcquireBus(&I2CD4);
+
+  // Calculated depending on clock source, check reference manual
+  i2c4Config.timingr = 0xE14;
+
+  if (i2cStart(&I2CD4, &i2c4Config) != HAL_RET_SUCCESS) {
+    while (1)
+      ;
+  }
+  i2cReleaseBus(&I2CD4);
+}

boards/XCORE/board_ex.h

@@ -67,5 +67,7 @@
 #define PROGRAM_FLASH_SIZE_BYTES (FLASH_PAGE_COUNT * FLASH_PAGE_SIZE_BYTES)
 
 #define SPID_IMU SPID3
-
+#ifndef __ASSEMBLER__
+void initBoardPeriphs(void);
+#endif
 #endif

boards/XCORE/cfg/board.chcfg

@@ -11,7 +11,11 @@
   </configuration_settings>
   <board_name>xcore</board_name>
   <board_id>XCORE</board_id>
-  <board_functions></board_functions>
+  <board_functions>
+    <boardInit>
+        initBoardPeriphs();
+    </boardInit>
+  </board_functions>
   <headers>
     <header>board_ex.h</header>
   </headers>

boards/XCORE/mcuconf.h

@@ -270,8 +270,8 @@
 /*
  * I2C driver system settings.
  */
-#define STM32_I2C_USE_I2C1                  FALSE
-#define STM32_I2C_USE_I2C2                  FALSE
+#define STM32_I2C_USE_I2C1                  TRUE
+#define STM32_I2C_USE_I2C2                  TRUE
 #define STM32_I2C_USE_I2C3                  FALSE
 #define STM32_I2C_USE_I2C4                  TRUE
 #define STM32_I2C_BUSY_TIMEOUT              50

cfg/chconf.h

@@ -258,7 +258,7 @@ extern volatile uint32_t last_idle_tick;
  * @note    Requires @p CH_CFG_USE_MUTEXES.
  */
 #if !defined(CH_CFG_USE_MUTEXES_RECURSIVE)
-#define CH_CFG_USE_MUTEXES_RECURSIVE        FALSE
+#define CH_CFG_USE_MUTEXES_RECURSIVE        TRUE
 #endif
 
 /**

ext/ChibiOS_21.11.3/CMakeLists.txt

@@ -99,6 +99,7 @@ add_library(
         os/oslib/src/chfactory.c
         # board
         ${BOARD_DIR}/board.c
+        ${BOARD_DIR}/board_ex.c
         # LwIP
         os/various/lwip_bindings/lwipthread.c
         os/various/lwip_bindings/arch/sys_arch.c

main.cpp

@@ -17,8 +17,9 @@
 #include <xbot-service/portable/system.hpp>
 
 #include "services/imu_service/imu_service.hpp"
+#include "services/power_service/power_service.hpp"
 ImuService imu_service{4};
-
+PowerService power_service{5};
 /*
  * Application entry point.
  */
@@ -47,7 +48,6 @@ int main(void) {
 #ifdef USE_SEGGER_SYSTEMVIEW
   SYSVIEW_ChibiOS_Start(STM32_SYS_CK, STM32_SYS_CK, "I#15=SysTick");
 #endif
-  ID_EEPROM_Init();
 
   /*
    * InitGlobals() sets up global variables shared by threads. (e.g. mutex)
@@ -81,16 +81,6 @@ int main(void) {
   xbot::service::system::initSystem();
   xbot::service::Io::start();
   imu_service.start();
-
-  int i = 0;
-  while (1) {
-    chThdSleep(TIME_MS2I(100));
-    const auto idle_thread = chSysGetIdleThreadX();
-    const auto first_thread = chRegFirstThread();
-    for (thread_t* t = first_thread; t; t = chRegNextThread(t)) {
-      chprintf((BaseSequentialStream*)&RTTD0, "chprintf: %s\t %llu\n", t->name,
-               t->stats.cumulative);
-    }
-  }
+  power_service.start();
   chThdSleep(TIME_INFINITE);
 }

services/diff_drive_service.json

@@ -0,0 +1,55 @@
+{
+  "inputs": [
+    {
+      "id": 0,
+      "name": "Control Twist",
+      "type": "double[6]"
+    }
+  ],
+  "outputs": [
+    {
+      "id": 0,
+      "name": "Actual Twist",
+      "type": "double[6]"
+    },
+    {
+      "id": 1,
+      "name": "Left ESC Temperature",
+      "type": "float"
+    },
+    {
+      "id": 2,
+      "name": "Left ESC Current",
+      "type": "float"
+    },
+    {
+      "id": 3,
+      "name": "Right ESC Temperature",
+      "type": "float"
+    },
+    {
+      "id": 4,
+      "name": "Right ESC Current",
+      "type": "float"
+    },
+    {
+      "id": 5,
+      "name": "Wheel Ticks",
+      "type": "uint32_t[2]"
+    }
+  ],
+  "registers": [
+    {
+      "id": 0,
+      "name": "Wheel Ticks Per Meter",
+      "type": "double"
+    },
+    {
+      "id": 1,
+      "name": "Wheel Distance",
+      "type": "double"
+    }
+  ],
+  "type": "DiffDriveService",
+  "version": 1
+}