Start implementaion of unpacking functions

nrai_CAN_API.c

@@ -30,42 +30,127 @@
 NRAI_FT int32_t
 nrai_can_unpack_VCU2AI_Status(struct nrai_can_ai_read * s, struct can_frame * f)
 {
+        if ((s == NULL) || (f == NULL))
+                return -1;
+        if (f->can_id != NRAI_CAN_ID_VCU2AI_Status)
+                return -1;
+
+        s->HANDSHAKE                    = (f->data[0] & 0b00000001);
+        s->OP_STATE                     = (f->data[0] & 0b11110000) >> 4;
+        s->SHUTDOWN_REQUEST             = (f->data[1] & 0b00000001);
+        s->AS_SWITCH_STATUS             = (f->data[1] & 0b00000010) >> 1;
+        s->TS_SWITCH_STATUS             = (f->data[1] & 0b00000100) >> 2;
+        s->GO_SIGNAL                    = (f->data[1] & 0b00001000) >> 3;
+        s->STEERING_STATUS              = (f->data[1] & 0b00110000) >> 4;
+        s->AS_STATE                     = (f->data[2] & 0b00001111);
+        s->AMI_STATE                    = (f->data[2] & 0b11110000) >> 4;
+        s->FAULT_STATUS                 = (f->data[3] & 0b00000001);
+        s->WARNING_STATUS               = (f->data[3] & 0b00000010) >> 1;
+        s->TS_STATE                     = (f->data[3] & 0b11110000) >> 4;
+        s->WARN_BATT_TEMP_HIGH          = (f->data[4] & 0b00000001);
+        s->WARN_BATT_SOC_LOW            = (f->data[4] & 0b00000010) >> 1;
+        s->EBS_STATE                    = (f->data[4] & 0b11110000) >> 4;
+        s->AI_ESTOP_REQUEST             = (f->data[5] & 0b00000001);
+        s->HVIL_OPEN_FAULT              = (f->data[5] & 0b00000010) >> 1;
+        s->HVIL_SHORT_FAULT             = (f->data[5] & 0b00000100) >> 2;
+        s->EBS_FAULT                    = (f->data[5] & 0b00001000) >> 3;
+        s->OFFBOARD_CHARGER_FAULT       = (f->data[5] & 0b00010000) >> 4;
+        s->AI_COMMS_LOST                = (f->data[5] & 0b00100000) >> 5;
+        s->AUTONOMOUS_BRAKING_FAULT     = (f->data[5] & 0b01000000) >> 6;
+        s->MISSION_STATUS_FAULT         = (f->data[5] & 0b10000000) >> 7;
+        s->CHARGE_PROCEDURE_FAULT       = (f->data[6] & 0b00000001);
+        s->BMS_FAULT                    = (f->data[6] & 0b00000010) >> 1;
+        s->BRAKE_PLAUSIBILITY_FAULT     = (f->data[6] & 0b00000100) >> 2;
+        s->SHUTDOWN_CAUSE               = (f->data[7]);
         return 0;
 }
 
 NRAI_FT int32_t
 nrai_can_unpack_VCU2AI_Steer(struct nrai_can_ai_read * s, struct can_frame * f)
 {
+        if ((s == NULL) || (f == NULL))
+                return -1;
+        if (f->can_id != NRAI_CAN_ID_VCU2AI_Steer)
+                return -1;
+
+        s->ANGLE                = ((uint16_t)f->data[1] << 8) | f->data[0];
+        s->ANGLE_MAX            = ((uint16_t)f->data[3] << 8) | f->data[2];
+        s->ANGLE_REQUEST        = ((uint16_t)f->data[5] << 8) | f->data[4];
         return 0;
 }
 
 NRAI_FT int32_t
 nrai_can_unpack_VCU2AI_Speeds(struct nrai_can_ai_read * s, struct can_frame * f)
 {
+        if ((s == NULL) || (f == NULL))
+                return -1;
+        if (f->can_id != NRAI_CAN_ID_VCU2AI_Speeds)
+                return -1;
+
+        s->FL_WHEEL_SPEED = ((uint16_t)f->data[1] << 8) | f->data[0];
+        s->FR_WHEEL_SPEED = ((uint16_t)f->data[3] << 8) | f->data[2];
+        s->RL_WHEEL_SPEED = ((uint16_t)f->data[5] << 8) | f->data[4];
+        s->RR_WHEEL_SPEED = ((uint16_t)f->data[7] << 8) | f->data[6];
         return 0;
 }
 
 NRAI_FT int32_t
 nrai_can_unpack_VCU2AI_Brake(struct nrai_can_ai_read * s, struct can_frame * f)
 {
+        if ((s == NULL) || (f == NULL))
+                return -1;
+        if (f->can_id != NRAI_CAN_ID_VCU2AI_Brake)
+                return -1;
+
+        s->HYD_PRESS_F_pct         = f->data[0];
+        s->HYD_PRESS_F_REQ_pct     = f->data[1];
+        s->HYD_PRESS_R_pct         = f->data[2];
+        s->HYD_PRESS_R_REQ_pct     = f->data[3];
+        s->STATUS_BRK              = (f->data[4] & 0b00001111);
+        s->STATUS_EBS              = (f->data[4] & 0b11110000) >> 4;
         return 0;
 }
 
 NRAI_FT int32_t
 nrai_can_unpack_VCU2AI_Drive_F(struct nrai_can_ai_read * s, struct can_frame * f)
 {
+        if ((s == NULL) || (f == NULL))
+                return -1;
+        if (f->can_id != NRAI_CAN_ID_VCU2AI_Drive_F)
+                return -1;
+
+        s->FRONT_AXLE_TRQ               = ((uint16_t)f->data[1] << 8) | f->data[0];
+        s->FRONT_AXLE_TRQ_REQUEST       = ((uint16_t)f->data[3] << 8) | f->data[2];
+        s->FRONT_AXLE_TRQ_MAX           = ((uint16_t)f->data[5] << 8) | f->data[4];
         return 0;
 }
 
 NRAI_FT int32_t
 nrai_can_unpack_VCU2AI_Drive_R(struct nrai_can_ai_read * s, struct can_frame * f)
 {
+        if ((s == NULL) || (f == NULL))
+                return -1;
+        if (f->can_id != NRAI_CAN_ID_VCU2AI_Drive_R)
+                return -1;
+
+        s->REAR_AXLE_TRQ_Nm             = ((uint16_t)f->data[1] << 8) | f->data[0];
+        s->REAR_AXLE_TRQ_REQUEST_Nm     = ((uint16_t)f->data[3] << 8) | f->data[2];
+        s->REAR_AXLE_TRQ_MAX_Nm         = ((uint16_t)f->data[5] << 8) | f->data[4];
         return 0;
 }
 
 NRAI_FT int32_t
 nrai_can_unpack_VCU2AI_Wheel_counts(struct nrai_can_ai_read * s, struct can_frame * f)
 {
+        if ((s == NULL) || (f == NULL))
+                return -1;
+        if (f->can_id != NRAI_CAN_ID_VCU2AI_Wheel_counts)
+                return -1;
+
+        s->FL_PULSE_COUNT = ((uint16_t)f->data[1] << 8) | f->data[0];
+        s->FR_PULSE_COUNT = ((uint16_t)f->data[3] << 8) | f->data[2];
+        s->RL_PULSE_COUNT = ((uint16_t)f->data[5] << 8) | f->data[4];
+        s->RR_PULSE_COUNT = ((uint16_t)f->data[7] << 8) | f->data[6];
         return 0;
 }
 
@@ -73,24 +158,62 @@ nrai_can_unpack_VCU2AI_Wheel_counts(struct nrai_can_ai_read * s, struct can_fram
 NRAI_FT int32_t
 nrai_can_unpack_BMC_Acceleration(struct nrai_can_ai_read * s, struct can_frame * f)
 {
+        if ((s == NULL) || (f == NULL))
+                return -1;
+        if (f->can_id != NRAI_CAN_ID_BMC_Acceleration)
+                return -1;
+
+        s->Acceleration_X       = ((uint16_t)f->data[1] << 8) | f->data[0];
+        s->Acceleration_Y       = ((uint16_t)f->data[3] << 8) | f->data[2];
+        s->Acceleration_Z       = ((uint16_t)f->data[5] << 8) | f->data[4];
+        s->Temperature          = f->data[6];
+        s->VerticalAxis         = (f->data[7] & 0b00000011);
+        s->Orientation          = (f->data[7] & 0b00011100) >> 2;
         return 0;
 }
 
 NRAI_FT int32_t
 nrai_can_unpack_BMC_MagneticField(struct nrai_can_ai_read * s, struct can_frame * f)
 {
+        if ((s == NULL) || (f == NULL))
+                return -1;
+        if (f->can_id != NRAI_CAN_ID_BMC_MagneticField)
+                return -1;
+
+        s->MagneticField_X = ((uint16_t)f->data[1] << 8) | f->data[0];
+        s->MagneticField_Y = ((uint16_t)f->data[3] << 8) | f->data[2];
+        s->MagneticField_Z = ((uint16_t)f->data[5] << 8) | f->data[4];
         return 0;
 }
 
 NRAI_FT int32_t
 nrai_can_unpack_L3GD20_Rotation_A(struct nrai_can_ai_read * s, struct can_frame * f)
 {
+        if ((s == NULL) || (f == NULL))
+                return -1;
+        if (f->can_id != NRAI_CAN_ID_L3GD20_Rotation_A)
+                return -1;
+
+        s->Rotation_X = ((uint32_t)f->data[3] << 24) | 
+                                       ((uint32_t)f->data[2] << 16) |
+                                       ((uint16_t)f->data[1] << 8) | f->data[0];
+        s->Rotation_Y = ((uint32_t)f->data[7] << 24) | 
+                                       ((uint32_t)f->data[6] << 16) |
+                                       ((uint16_t)f->data[5] << 8) | f->data[4];
         return 0;
 }
 
 NRAI_FT int32_t
 nrai_can_unpack_L3GD20_Rotation_B(struct nrai_can_ai_read * s, struct can_frame * f)
 {
+        if ((s == NULL) || (f == NULL))
+                return -1;
+        if (f->can_id != NRAI_CAN_ID_L3GD20_Rotation_A)
+                return -1;
+
+        s->Rotation_Z = ((uint32_t)f->data[3] << 24) | 
+                                       ((uint32_t)f->data[2] << 16) |
+                                       ((uint16_t)f->data[1] << 8) | f->data[0];
         return 0;
 }
 
@@ -246,7 +369,7 @@ nrai_can_mkframe_AI2VCU_Steer(  struct nrai_can_ai_write * s, struct can_frame *
 }
 
 NRAI_FT int32_t
-nrai_can_mkframe_AI2VCU_Brake(struct nrai_can_ai_write * const s, struct can_frame * f)
+nrai_can_mkframe_AI2VCU_Brake(struct nrai_can_ai_write * s, struct can_frame * f)
 {
         uint32_t tmp;
         if ((s == NULL) || (f == NULL))
@@ -271,7 +394,7 @@ nrai_can_mkframe_AI2VCU_Drive_F(struct nrai_can_ai_write * s, struct can_frame *
         uint32_t tmp;
         if ((s == NULL) || (f == NULL))
                 return -1;
-        
+
         f->can_id = NRAI_CAN_ID_AI2VCU_Drive_F;
         f->len = 4;
 
@@ -293,7 +416,7 @@ nrai_can_mkframe_AI2VCU_Drive_R(struct nrai_can_ai_write * s, struct can_frame *
         uint32_t tmp;
         if ((s == NULL) || (f == NULL))
                 return -1;
-        
+
         f->can_id = NRAI_CAN_ID_AI2VCU_Drive_R;
         f->len = 4;