Merge pull request westonrobot#50 from ROSETEA-lab/main

Negative motor currents are not correctly converted, overflow to 65535
agilexrobotics · Apr 19, 2024 · 15cfdcb · 15cfdcb
2 parents 2b26909 + 7769abf
commit 15cfdcb
Show file tree

Hide file tree

Showing 2 changed files with 4 additions and 6 deletions.
diff --git a/src/protocol_v1/agilex_msg_parser_v1.c b/src/protocol_v1/agilex_msg_parser_v1.c
@@ -78,9 +78,8 @@ bool DecodeCanFrameV1(const struct can_frame *rx_frame, AgxMessage *msg) {
       msg->type = AgxMsgActuatorStateV1;
       msg->body.v1_actuator_state_msg.motor_id =
           rx_frame->can_id - CAN_MSG_ACTUATOR1_STATE_ID;
-      msg->body.v1_actuator_state_msg.current =
-          ((((uint16_t)rx_frame->data[0]) << 8) | (uint16_t)rx_frame->data[1]) /
-          10.0f;
+      msg->body.v1_actuator_state_msg.current = (int16_t)((
+          ((uint16_t)rx_frame->data[0]) << 8) | (uint16_t)rx_frame->data[1]) * 0.1;
       msg->body.v1_actuator_state_msg.rpm = (int16_t)((
           (((uint16_t)rx_frame->data[2]) << 8) | (uint16_t)rx_frame->data[3]));
       msg->body.v1_actuator_state_msg.driver_temp = rx_frame->data[4];

diff --git a/src/protocol_v2/agilex_msg_parser_v2.c b/src/protocol_v2/agilex_msg_parser_v2.c
@@ -158,9 +158,8 @@ bool DecodeCanFrameV2(const struct can_frame *rx_frame, AgxMessage *msg) {
           (int16_t)((uint16_t)(frame->rpm.low_byte) |
                     (uint16_t)(frame->rpm.high_byte) << 8);
       msg->body.actuator_hs_state_msg.current =
-          ((uint16_t)(frame->current.low_byte) |
-           (uint16_t)(frame->current.high_byte) << 8) *
-          0.1;
+          (int16_t)((uint16_t)(frame->current.low_byte) |
+                    (uint16_t)(frame->current.high_byte) << 8) * 0.1;
       msg->body.actuator_hs_state_msg.pulse_count =
           (int32_t)((uint32_t)(frame->pulse_count.lsb) |
                     (uint32_t)(frame->pulse_count.low_byte) << 8 |