Update sth && add "cancel target"

- set sita as rad
- fix the change between little to big end
- sth else

1 files changed, 10 insertions, 3 deletions

diff --git a/udp2lcm/udp2lcm.c b/udp2lcm/udp2lcm.c
index 9ac380f..36100da 100644
--- a/udp2lcm/udp2lcm.c
+++ b/udp2lcm/udp2lcm.c
@@ -116,14 +116,22 @@ void parseCmd(const char *buffer, int bytesReceived) {
         x = swapEndian(*(int16_t *)&buffer[3]);
         y = swapEndian(*(int16_t *)&buffer[5]);
         sita = 0;
-        // printf("End point: x: %d, y: %d, sita: %d\n", x, y, sita);
-        // printf("Original buffer: ", buffer);
         robotCtrlInit(&robotCtrlData, 0, 20, 0, 3, 0, 0, 0);
         robotCtrlData.dparams[0] = (double)x / 20;
         robotCtrlData.dparams[1] = (double)y / 20;
         robotCtrlData.dparams[2] = sita;
         robot_control_t_publish(lcm, "ROBOT_CONTROL", &robotCtrlData);
         freeRobotCtrl(&robotCtrlData);
+    } else if (buffer[0] == 4) {
+        // 取消导航，删除上一个目标点
+        robotCtrlInit(&robotCtrlData, 0, 23, 0, 0, 0, 0, 0);
+        robot_control_t_publish(lcm, "ROBOT_CONTROL", &robotCtrlData);
+    } else {
+        fprintf(stderr, "Error: Invalid command: ");
+        for (int i = 0; i < 9; i++) {
+            fprintf(stderr, "%02hhx ", buffer[i]);
+        }
+        fprintf(stderr, "\n");
     }
 }
 
@@ -147,7 +155,6 @@ void poseHandler(const lcm_recv_buf_t *rbuf, const char *channel,
     heartBeat[7] = sita >> 8;
     heartBeat[8] = sita;
     pthread_mutex_unlock(&heartBeatMutex);
-    printf("curpose: x: %d, y: %d, sita: %d\n", x, y, sita);
 }
 
 int16_t swapEndian(int16_t val) { return (val << 8) | ((val >> 8) & 0xFF); }
\ No newline at end of file