Skip to content

Commit

Permalink
parser_v2: fixed a few decoding
Browse files Browse the repository at this point in the history
  • Loading branch information
@rdu-weston
rdu-weston committed Aug 1, 2024
1 parent fbfad8a commit e523ba2
Showing 1 changed file with 24 additions and 28 deletions.
52 changes: 24 additions & 28 deletions src/protocol_v2/agilex_msg_parser_v2.c
View file
Original file line number Diff line number Diff line change
Expand Up @@ -159,7 +159,8 @@ bool DecodeCanFrameV2(const struct can_frame *rx_frame, AgxMessage *msg) {
(uint16_t)(frame->rpm.high_byte) << 8);
msg->body.actuator_hs_state_msg.current =
(int16_t)((uint16_t)(frame->current.low_byte) |
(uint16_t)(frame->current.high_byte) << 8) * 0.1;
(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 |
Expand Down Expand Up @@ -504,30 +505,31 @@ bool EncodeCanFrameV2(const AgxMessage *msg, struct can_frame *tx_frame) {
tx_frame->can_id = CAN_MSG_RC_STATE_ID;
tx_frame->can_dlc = 8;
RcStateFrame frame;
frame.sws = 0;
// switch a
if (msg->body.rc_state_msg.swa == RC_SWITCH_UP)
frame.sws = RC_SWA_UP_MASK;
frame.sws |= RC_SWA_UP_MASK;
else if (msg->body.rc_state_msg.swa == RC_SWITCH_DOWN)
frame.sws = RC_SWA_DOWN_MASK;
frame.sws |= RC_SWA_DOWN_MASK;
// switch b
if (msg->body.rc_state_msg.swb == RC_SWITCH_UP)
frame.sws = RC_SWB_UP_MASK;
frame.sws |= RC_SWB_UP_MASK;
else if (msg->body.rc_state_msg.swb == RC_SWITCH_MIDDLE)
frame.sws = RC_SWB_MIDDLE_MASK;
frame.sws |= RC_SWB_MIDDLE_MASK;
else if (msg->body.rc_state_msg.swb == RC_SWITCH_DOWN)
frame.sws = RC_SWB_DOWN_MASK;
frame.sws |= RC_SWB_DOWN_MASK;
// switch c
if (msg->body.rc_state_msg.swc == RC_SWITCH_UP)
frame.sws = RC_SWC_UP_MASK;
frame.sws |= RC_SWC_UP_MASK;
else if (msg->body.rc_state_msg.swc == RC_SWITCH_MIDDLE)
frame.sws = RC_SWC_MIDDLE_MASK;
frame.sws |= RC_SWC_MIDDLE_MASK;
else if (msg->body.rc_state_msg.swc == RC_SWITCH_DOWN)
frame.sws = RC_SWC_DOWN_MASK;
frame.sws |= RC_SWC_DOWN_MASK;
// switch d
if (msg->body.rc_state_msg.swd == RC_SWITCH_UP)
frame.sws = RC_SWD_UP_MASK;
frame.sws |= RC_SWD_UP_MASK;
else if (msg->body.rc_state_msg.swd == RC_SWITCH_DOWN)
frame.sws = RC_SWD_DOWN_MASK;
frame.sws |= RC_SWD_DOWN_MASK;
frame.stick_right_v = msg->body.rc_state_msg.stick_right_v;
frame.stick_right_h = msg->body.rc_state_msg.stick_right_h;
frame.stick_left_v = msg->body.rc_state_msg.stick_left_v;
Expand All @@ -541,20 +543,17 @@ bool EncodeCanFrameV2(const AgxMessage *msg, struct can_frame *tx_frame) {
CAN_MSG_ACTUATOR1_HS_STATE_ID;
tx_frame->can_dlc = 8;
ActuatorHSStateFrame frame;
int16_t rpm =
(int16_t)(msg->body.actuator_hs_state_msg.rpm);
int16_t current =
(int16_t)(msg->body.actuator_hs_state_msg.current * 10);
int32_t pulse_count =
(int32_t)(msg->body.actuator_hs_state_msg.pulse_count);
int16_t rpm = msg->body.actuator_hs_state_msg.rpm;
int16_t current = msg->body.actuator_hs_state_msg.current * 10;
int32_t pulse_count = msg->body.actuator_hs_state_msg.pulse_count;
frame.rpm.high_byte = (uint8_t)(rpm >> 8);
frame.rpm.low_byte = (uint8_t)(rpm & 0x00ff);
frame.current.high_byte = (uint8_t)(current >> 8);
frame.current.low_byte = (uint8_t)(current & 0x00ff);
frame.pulse_count.lsb = (uint16_t)(pulse_count & 0x000000ff);
frame.pulse_count.low_byte = (uint16_t)((pulse_count >> 8) & 0x000000ff);
frame.pulse_count.high_byte = (uint16_t)((pulse_count >> 16) & 0x000000ff);
frame.pulse_count.msb = (uint16_t)((pulse_count >> 24) & 0x000000ff);
frame.pulse_count.lsb = (uint8_t)(pulse_count & 0x000000ff);
frame.pulse_count.low_byte = (uint8_t)((pulse_count >> 8) & 0x000000ff);
frame.pulse_count.high_byte = (uint8_t)((pulse_count >> 16) & 0x000000ff);
frame.pulse_count.msb = (uint8_t)((pulse_count >> 24) & 0x000000ff);
memcpy(tx_frame->data, (uint8_t *)(&frame), tx_frame->can_dlc);
break;
}
Expand All @@ -563,7 +562,7 @@ bool EncodeCanFrameV2(const AgxMessage *msg, struct can_frame *tx_frame) {
CAN_MSG_ACTUATOR1_LS_STATE_ID;
tx_frame->can_dlc = 8;
ActuatorLSStateFrame frame;
int16_t driver_voltage =
int16_t driver_voltage =
(int16_t)(msg->body.actuator_ls_state_msg.driver_voltage * 10);
int16_t driver_temp =
(int16_t)(msg->body.actuator_ls_state_msg.driver_temp);
Expand Down Expand Up @@ -640,12 +639,9 @@ bool EncodeCanFrameV2(const AgxMessage *msg, struct can_frame *tx_frame) {
BmsBasicFrame frame;
frame.battery_soc = msg->body.bms_basic_msg.battery_soc;
frame.battery_soh = msg->body.bms_basic_msg.battery_soh;
int16_t voltage =
(int16_t)(msg->body.bms_basic_msg.voltage * 10);
int16_t current =
(int16_t)(msg->body.bms_basic_msg.current * 10);
int16_t temperature =
(int16_t)(msg->body.bms_basic_msg.temperature * 10);
int16_t voltage = (int16_t)(msg->body.bms_basic_msg.voltage * 10);
int16_t current = (int16_t)(msg->body.bms_basic_msg.current * 10);
int16_t temperature = (int16_t)(msg->body.bms_basic_msg.temperature * 10);
frame.voltage.high_byte = (uint8_t)(voltage >> 8);
frame.voltage.low_byte = (uint8_t)(voltage & 0x00ff);
frame.current.high_byte = (uint8_t)(current >> 8);
Expand Down

0 comments on commit e523ba2

Please sign in to comment.