Merge pull request #58 from xrobot-org/dev

Fix dart

hw/bsp/rm-c/drivers/bsp_can.c

@@ -3,6 +3,7 @@
 #include "FreeRTOS.h"
 #include "main.h"
 #include "semphr.h"
+#include "string.h"
 #include "task.h"
 
 typedef struct {

hw/mcu/default/bsp_def.h

@@ -16,12 +16,20 @@ extern "C" {
 #define XB_TO_STR(_arg) #_arg
 #define XB_DEF2STR(_arg) XB_TO_STR(_arg)
 
-#if MCU_DEBUG_BUILD
-#define XB_ASSERT(arg)                                         \
-  if (!(arg))                                                  \
-    while (1) {                                                \
-      printf("Assert error at %s:%d\r\n", __FILE__, __LINE__); \
+__attribute__((unused)) static void xb_verify_failed(const char* file,
+                                                     uint32_t line) {
+  while (1) {
+    while (1) {
+      printf("Assert error at %s:%d\r\n", file, line);
     }
+  }
+}
+
+#if MCU_DEBUG_BUILD
+#define XB_ASSERT(arg)                    \
+  if (!(arg)) {                           \
+    xb_verify_failed(__FILE__, __LINE__); \
+  }
 #else
 #define XB_ASSERT(arg) (void)0;
 #endif

src/component/comp_cmd.cpp

@@ -14,7 +14,7 @@ CMD::CMD(Mode mode)
   CMD::self_ = this;
 
   auto op_ctrl_callback = [](Data& data, CMD* cmd) {
-    ASSERT(data.ctrl_source < CTRL_SOURCE_NUM);
+    XB_ASSERT(data.ctrl_source < CTRL_SOURCE_NUM);
     memcpy(&(cmd->data_[data.ctrl_source]), &data, sizeof(Data));
 
     if (!cmd->data_[CTRL_SOURCE_RC].online && cmd->online_) {

src/component/comp_mixer.cpp

@@ -37,36 +37,36 @@ Mixer::Mixer(Mixer::Mode mode) : mode_(mode) {
       return;
 
     default:
-      ASSERT(0);
+      XB_ASSERT(false);
   }
 }
 
 bool Mixer::Apply(Component::Type::MoveVector &move_vec, float *out) {
   switch (this->mode_) {
     case MECANUM:
-      ASSERT(this->len_ == 4);
+      XB_ASSERT(this->len_ == 4);
       out[0] = move_vec.vx - move_vec.vy + move_vec.wz;
       out[1] = move_vec.vx + move_vec.vy + move_vec.wz;
       out[2] = -move_vec.vx + move_vec.vy + move_vec.wz;
       out[3] = -move_vec.vx - move_vec.vy + move_vec.wz;
       break;
 
     case PARLFIX4:
-      ASSERT(this->len_ == 4);
+      XB_ASSERT(this->len_ == 4);
       out[0] = -move_vec.vy;
       out[1] = move_vec.vy;
       out[2] = move_vec.vy;
       out[3] = -move_vec.vy;
       break;
 
     case PARLFIX2:
-      ASSERT(this->len_ == 2);
+      XB_ASSERT(this->len_ == 2);
       out[0] = -move_vec.vx;
       out[1] = move_vec.vx;
       break;
 
     case SINGLE:
-      ASSERT(this->len_ == 1);
+      XB_ASSERT(this->len_ == 1);
       out[0] = move_vec.vy;
       break;
 

src/component/comp_pid.cpp

@@ -20,7 +20,7 @@ using namespace Component;
 PID::PID(PID::Param &param, float sample_freq)
     : param_(param), dfilter_(sample_freq, param.d_cutoff_freq) {
   float dt_min = 1.0f / sample_freq;
-  ASSERT(isfinite(dt_min));
+  XB_ASSERT(isfinite(dt_min));
   this->dt_min_ = dt_min;
 
   this->Reset();

src/component/comp_utils.cpp

@@ -4,7 +4,7 @@
 
 #include "comp_utils.hpp"
 
-#include <cstring>
+#include "bsp_def.h"
 
 /**
  * @brief 计算平方根倒数
@@ -46,7 +46,7 @@ float abs_clampf(float x, float limit) { return MIN(limit, MAX(x, -limit)); }
  * @param hi 上限
  */
 void clampf(float *origin, float lo, float hi) {
-  ASSERT(origin);
+  XB_ASSERT(origin);
   *origin = MIN(hi, MAX(*origin, lo));
 }
 
@@ -101,23 +101,6 @@ float bullet_speed_to_fric_rpm(float bullet_speed, float fric_radius,
   return 60.0f * bullet_speed / (M_2PI * fric_radius);
 }
 
-bool gyro_is_stable(Component::Type::Vector3 *gyro) {
-  return ((gyro->x < 0.03f) && (gyro->y < 0.03f) && (gyro->z < 0.03f));
-}
-
-/**
- * @brief 断言失败处理
- *
- * @param file 文件名
- * @param line 行号
- */
-void verify_failed(const char *file, uint32_t line) {
-  static_cast<void>(file);
-  static_cast<void>(line);
-  while (1) {
-  }
-}
-
 int float_to_uint(float x, float x_min, float x_max, int bits) {
   /// Converts a float to an unsigned int, given range and number of bits ///
   float span = x_max - x_min;

src/component/comp_utils.hpp

@@ -4,10 +4,6 @@
 
 #pragma once
 
-#include <cfloat>
-#include <cmath>
-#include <cstdint>
-
 #include "comp_type.hpp"
 
 /**
@@ -58,64 +54,6 @@
 
 #endif
 
-#ifdef MCU_DEBUG_BUILD
-
-/**
- * @brief 如果表达式的值为假则运行处理函数
- *
- */
-#define ASSERT(expr)                     \
-  do {                                   \
-    if (!(expr)) {                       \
-      verify_failed(__FILE__, __LINE__); \
-    }                                    \
-  } while (0)
-#else
-
-/**
- * @brief 未定DEBUG，表达式不会运行，断言被忽略
- *
- */
-#define ASSERT(expr) ((void)(0))
-#endif
-
-#ifdef MCU_DEBUG_BUILD
-
-/**
- * @brief 如果表达式的值为假则运行处理函数
- *
- */
-#define VERIFY(expr)                     \
-  do {                                   \
-    if (!(expr)) {                       \
-      verify_failed(__FILE__, __LINE__); \
-    }                                    \
-  } while (0)
-#else
-
-/**
- * @brief 表达式会运行，忽略表达式结果
- *
- */
-#define VERIFY(expr) ((void)(expr))
-#endif
-
-/**
- * @brief 获取结构体或者联合成员的容器
- *
- */
-#define CONTAINER_OF(ptr, type, member)                \
-  ({                                                   \
-    const typeof(((type *)0)->member) *__mptr = (ptr); \
-    (type *)((char *)__mptr - offsetof(type, member)); \
-  })
-
-/**
- * @brief 获取数组长度
- *
- */
-#define ARRAY_LEN(array) (sizeof((array)) / sizeof(*(array)))
-
 /**
  * @brief 计算平方根倒数
  *
@@ -161,16 +99,6 @@ float signf(float x);
 float bullet_speed_to_fric_rpm(float bullet_speed, float fric_radius,
                                bool is17mm);
 
-bool gyro_is_stable(Component::Type::Vector3 *gyro);
-
-/**
- * @brief 断言失败处理
- *
- * @param file 文件名
- * @param line 行号
- */
-void verify_failed(const char *file, uint32_t line);
-
 int float_to_uint(float x, float x_min, float x_max, int bits);
 
 float uint_to_float(int x_int, float x_min, float x_max, int bits);

src/device/can/dev_can.cpp

@@ -60,7 +60,7 @@ bool Can::SendExtPack(bsp_can_t can, Pack& pack) {
 
 bool Can::Subscribe(Message::Topic<Can::Pack>& tp, bsp_can_t can,
                     uint32_t index, uint32_t num) {
-  ASSERT(num > 0);
+  XB_ASSERT(num > 0);
 
   can_tp_[can]->RangeDivide(tp, sizeof(Pack), offsetof(Pack, index),
                             om_member_size_of(Pack, index), index, num);

src/device/canfd/dev_can.cpp

@@ -105,7 +105,7 @@ bool Can::SendFDExtPack(bsp_can_t can, uint32_t id, uint8_t* data,
 
 bool Can::Subscribe(Message::Topic<Can::Pack>& tp, bsp_can_t can,
                     uint32_t index, uint32_t num) {
-  ASSERT(num > 0);
+  XB_ASSERT(num > 0);
 
   can_tp_[can]->RangeDivide(tp, sizeof(Pack), offsetof(Pack, index),
                             om_member_size_of(Pack, index), index, num);
@@ -114,7 +114,7 @@ bool Can::Subscribe(Message::Topic<Can::Pack>& tp, bsp_can_t can,
 
 bool Can::SubscribeFD(Message::Topic<Can::FDPack>& tp, bsp_can_t can,
                       uint32_t index, uint32_t num) {
-  ASSERT(num > 0);
+  XB_ASSERT(num > 0);
 
   canfd_tp_[can]->RangeDivide(tp, sizeof(FDPack), offsetof(FDPack, index),
                               om_member_size_of(Pack, index), index, num);

src/device/cap/dev_cap.cpp

@@ -8,7 +8,7 @@
 using namespace Device;
 
 Cap::Cap(Cap::Param &param) : param_(param), info_tp_("cap_info") {
-  ASSERT(param.cutoff_volt > 3.0f && param.cutoff_volt < 24.0f);
+  XB_ASSERT(param.cutoff_volt > 3.0f && param.cutoff_volt < 24.0f);
 
   out_.power_limit_ = 40.0f;
 

src/device/mech/dev_mech.hpp

@@ -282,7 +282,7 @@ class SteeringMech {
         error = stream.target_angle_.yaw - stream.angle_.yaw;
         break;
       default:
-        ASSERT(false);
+        XB_ASSERT(false);
         break;
     }
 
@@ -308,7 +308,7 @@ class SteeringMech {
         stream.angle_.yaw += setpoint;
         break;
       default:
-        ASSERT(false);
+        XB_ASSERT(false);
         break;
     }
 
@@ -329,7 +329,7 @@ class SteeringMech {
       PosStream& stream, SteeringMech<MotorType, LimitType, Num>& mech_1,
       SteeringMech<MotorType, LimitType, Num>& mech_2) {
     if (mech_1.param_.axis != mech_2.param_.axis) {
-      ASSERT(false);
+      XB_ASSERT(false);
     }
 
     if (!mech_1.Ready() || !mech_2.Ready()) {
@@ -351,7 +351,7 @@ class SteeringMech {
         error = stream.target_angle_.yaw - stream.angle_.yaw;
         break;
       default:
-        ASSERT(false);
+        XB_ASSERT(false);
         break;
     }
 
@@ -427,7 +427,7 @@ class SteeringMech {
         break;
       }
       default:
-        ASSERT(false);
+        XB_ASSERT(false);
         break;
     }
 
@@ -558,7 +558,7 @@ class LinearMech {
         error = stream.target_pos_.z - stream.pos_.z;
         break;
       default:
-        ASSERT(false);
+        XB_ASSERT(false);
         break;
     }
 
@@ -583,7 +583,7 @@ class LinearMech {
         stream.pos_.z += setpoint;
         break;
       default:
-        ASSERT(false);
+        XB_ASSERT(false);
         break;
     }
 

src/device/motor/dev_mit_motor.cpp

@@ -95,7 +95,7 @@ void MitMotor::Decode(Can::Pack &rx) {
 /* MIT电机协议只提供pd位置控制 */
 void MitMotor::Control(float output) {
   static_cast<void>(output);
-  ASSERT(false);
+  XB_ASSERT(false);
 }
 
 void MitMotor::SetCurrent(float current) {

src/device/motor/dev_rm_motor.cpp

@@ -32,18 +32,18 @@ RMMotor::RMMotor(const Param &param, const char *name)
   switch (param.id_control) {
     case M3508_M2006_CTRL_ID_BASE:
       this->index_ = 0;
-      ASSERT(param.id_feedback > 0x200 && param.id_feedback <= 0x204);
+      XB_ASSERT(param.id_feedback > 0x200 && param.id_feedback <= 0x204);
       break;
     case M3508_M2006_CTRL_ID_EXTAND:
       this->index_ = 1;
-      ASSERT(param.id_feedback > 0x204 && param.id_feedback <= 0x208);
+      XB_ASSERT(param.id_feedback > 0x204 && param.id_feedback <= 0x208);
       break;
     case GM6020_CTRL_ID_EXTAND:
       this->index_ = 2;
-      ASSERT(param.id_feedback > 0x208 && param.id_feedback <= 0x20B);
+      XB_ASSERT(param.id_feedback > 0x208 && param.id_feedback <= 0x20B);
       break;
     default:
-      ASSERT(false);
+      XB_ASSERT(false);
   }
   switch (param.model) {
     case MOTOR_M2006:
@@ -79,6 +79,12 @@ RMMotor::RMMotor(const Param &param, const char *name)
 
   Can::Subscribe(motor_tp, this->param_.can, this->param_.id_feedback, 1);
 
+  if ((motor_tx_map_[this->param_.can][this->index_] & (1 << (this->num_))) !=
+      0) {
+    /* Error: ID duplicate */
+    XB_ASSERT(false);
+  }
+
   motor_tx_map_[this->param_.can][this->index_] |= 1 << (this->num_);
 }
 

src/device/motor/dev_rmd_motor.cpp

@@ -36,6 +36,11 @@ RMDMotor::RMDMotor(const Param &param, const char *name)
 
   Can::Subscribe(motor_tp, this->param_.can, this->param_.num + 0x141, 1);
 
+  if ((motor_tx_map_[this->param_.can] & (1 << (this->param_.num))) != 0) {
+    /* Error: ID duplicate */
+    XB_ASSERT(false);
+  }
+
   motor_tx_map_[this->param_.can] |= 1 << (this->param_.num);
 }
 

src/device/simulator/dev_rm_motor.cpp

@@ -26,7 +26,7 @@ void RMMotor::Control(float output) {
       output *= T_6020;
       break;
     default:
-      ASSERT(false);
+      XB_ASSERT(false);
       return;
   }