转到目标角度主动上报，这有两个问题，1.index不是上传时的，2.notify发送整个buffer

.vscode/settings.json

@@ -38,6 +38,7 @@
         "freertos.h": "c",
         "bitset": "c",
         "chrono": "c",
-        "algorithm": "c"
+        "algorithm": "c",
+        "port.h": "c"
     },
 }

main/main.c

@@ -80,9 +80,14 @@ void blerxcb(uint8_t *rxmessage, size_t rxsize) {
 void motor_on_event(motor_event_t event) {
   //打算使用sprintf
   if (event == kRunToPosition) {
+    ESP_LOGI(MAIN_LOG_TAG, "kRunToPosition ok!");
+
+    uint8_t buffer[128] = {0};
     //组包
     //上报
     //{ "order": "deviceStatusReport", "index": 0, "deviceState": "running", "position":13.9, "deviceException": 0, "deviceExceptionInfo": "", }
+    sprintf((char *)buffer, "{ \"order\": \"receipt\", \"code\": 0, \"info\": \"success\", \"index\": %d }", 0);
+    bleuart_notify_send(buffer,sizeof(buffer));
   }
 }
 
@@ -93,11 +98,8 @@ void app_main(void) {
   motor_init(&ble_uart_motor_structer);
   motor_reg_event_cb(motor_on_event);
 
-  uint8_t test[] = {"peiluoxisibahhh,finny noob !lol"};
-
   while (true) {
     bleuart_schedule();
-    // motor_module_schedule();
-    bleuart_notify_send(test,sizeof(test));
+    motor_module_schedule();
   }
 }

main/motor_drive.c

@@ -2,6 +2,7 @@
 
 #include "driver/uart.h"
 #include "esp_log.h"
+#include "port.h"
 
 #define MOTOR_DRIVE "MOTOR_DRIVE"
 
@@ -12,7 +13,13 @@
 #define uart_read_time_ms 200
 #define MOTOR_ID 0X01
 
+#define angular_error 1.0  //角度误差
+
 static motor_t *motor_init_structer;
+static motor_on_event_t s_onevent;
+
+static bool notify_send_flag = false;
+static double motor_position;
 
 /**
  * @brief 电机初始化
@@ -69,7 +76,33 @@ void motor_set_zero_point() {
  *
  * @return uint32_t
  */
-uint32_t motor_get_position_degree() { return 0; }
+double motor_get_position_degree() {
+  uint16_t turn_angle_data = 0;
+  size_t turn_angle_buffer_size = 5;
+  uint8_t buffer[5] = {0X3E, 0X94, MOTOR_ID, 0X00, 0XD3};
+  // char *notify_err = "Turn angle size err";
+
+  uart_flush(uart_num);
+  uart_write_bytes(uart_num, buffer, 5);
+
+  turn_angle_buffer_size = 0;
+  memset(buffer, 0, sizeof(uint8_t) * 5);
+
+  // Wait receive
+  turn_angle_buffer_size = uart_read_bytes(uart_num, buffer, 8, uart_read_time_ms / portTICK_RATE_MS);
+  if (turn_angle_buffer_size != 8 || buffer[0] != 0X3E) {
+    ESP_LOGW(MOTOR_DRIVE, "Turn angle size:%d,buffer[0] = 0X%x", turn_angle_buffer_size, buffer[0]);
+    // bluetooth_active_notify((uint8_t *)notify_err, strlen(notify_err));
+    return -1;
+  }
+
+  // Parse receive
+  // motor_drive_buffer_cmd_parse(buffer);
+
+  turn_angle_data = buffer[5] + (buffer[6] << 8);
+
+  return ((double)turn_angle_data / 100.0);
+}
 
 /**
  * @brief 电机转到多少度
@@ -142,8 +175,12 @@ void motor_run_to_postion(int rotation_direction, double position, int speed_lev
   if (position_buffer_size != 13 || buffer[0] != 0X3E) {
     ESP_LOGW(MOTOR_DRIVE, "set motor size error ,buffer_size:%d,buffer[0] = 0X%x", position_buffer_size, buffer[0]);
     // bluetooth_active_notify((uint8_t *)notify_err, strlen(notify_err));
+    return;
   }
 
+  notify_send_flag = true;
+  motor_position = position;
+
   // Parse receive
   //   motor_drive_buffer_cmd_parse(buffer);
   return;
@@ -154,7 +191,10 @@ void motor_run_to_postion(int rotation_direction, double position, int speed_lev
  *
  * @param onevent 注册的回调函数
  */
-void motor_reg_event_cb(motor_on_event_t onevent) { return; }
+void motor_reg_event_cb(motor_on_event_t onevent) {
+  s_onevent = onevent;
+  return;
+}
 
 /**
  * @brief 读取编码器
@@ -258,4 +298,49 @@ static uint8_t motor_drive_set_packages_ctr(double position, int direction) {
   // Parse receive
   // motor_drive_buffer_cmd_parse(buffer);
   return 0;
+}
+
+/**
+ * @brief 电机模块调度(验证电机是否到达指定位置，然后主动上报数据)
+ *
+ */
+void motor_module_schedule() {
+  static uint32_t time = 0;
+  //读取角度是否到达想要角度，到达上报(每隔500ms读取一次)
+  if ((notify_send_flag == true) && (port_haspassedms(time) > 500)) {
+    double now_position_degree = motor_get_position_degree();
+    double maximum = 0.0;
+    double minimum = 0.0;
+
+    if (motor_position == 0){
+      motor_position = 360;
+    }
+
+    if ((motor_position + angular_error) > 360.0) {
+      maximum = (motor_position + angular_error) - 360.0;
+    } else {
+      maximum = motor_position + angular_error;
+    }
+
+    if ((motor_position - angular_error) < 0.0) {
+      minimum = (motor_position - angular_error) + 360.0;
+    } else {
+      minimum = motor_position - angular_error;
+    }
+
+    if (maximum < minimum) {
+      maximum = maximum + 360;
+    }
+
+    if ((now_position_degree >= minimum) && (now_position_degree <= maximum)) {  //读取角度是否到达
+      s_onevent(kRunToPosition);                                                 //组包，主动上报
+      notify_send_flag = false;
+    }
+
+    // ESP_LOGI("=======", "now:%lf,run to:%lf,minimum = %lf,maximum = %lf", now_position_degree, motor_position, minimum, maximum);
+
+    time = port_get_ticket();
+  }
+
+  return;
 }

main/motor_drive.h

@@ -12,7 +12,7 @@ typedef void (*motor_on_event_t)(motor_event_t event);
 
 void motor_init(motor_t* motor);
 void motor_set_zero_point();
-uint32_t motor_get_position_degree();
+double motor_get_position_degree();
 void motor_run_to_postion(int rotation_direction, double position, int speed_level);
 void motor_reg_event_cb(motor_on_event_t onevent);