CANlib.c

#include "CANlib.h"
#include "evil_macros.h"

static Frame lastMessage;
static Can_ErrorID_T lastError = Can_Error_NO_RX;

#define DEFINE(name) \
  Can_ErrorID_T name ##_Write(name ## _T *type) { \
    Frame frame; \
    pack_ ## name(type, &frame); \
    return Can_RawWrite(&frame); \
  }

typedef enum {
  LITTLE,
  BIG,
} ENDIAN_T;

typedef union {
  uint8_t byte[8];
  uint64_t bitstring;
} DATA_T;

void data_transfer(DATA_T *in, DATA_T *out) {
  uint8_t i;
  for (i = 0; i < 8; i++) {
    (*out).byte[7-i] = (*in).byte[i];
  }
}

void to_bitstring(uint8_t in[], uint64_t *out) {
  data_transfer((DATA_T*)in, (DATA_T*)out);
}

void from_bitstring(uint64_t *in, uint8_t out[]) {
  data_transfer((DATA_T*)in, (DATA_T*)out);
}

// Shameless copypasta-ing from Stack Overflow for trivial endian swap.
// https://stackoverflow.com/a/2637138
uint16_t swap_uint16(uint16_t val) {
  return (val << 8) | (val >> 8 );
}

int16_t swap_int16(int16_t val) {
  return (val << 8) | ((val >> 8) & 0xFF);
}

uint32_t swap_uint32(uint32_t val) {
  val = ((val << 8) & 0xFF00FF00 ) | ((val >> 8) & 0xFF00FF );
  return (val << 16) | (val >> 16);
}

int32_t swap_int32(int32_t val) {
  val = ((val << 8) & 0xFF00FF00) | ((val >> 8) & 0xFF00FF );
  return (val << 16) | ((val >> 16) & 0xFFFF);
}

#ifdef CAN_ARCHITECTURE_LPC11CX4
#include "drivers/lpc11cx4.c"
#elif CAN_ARCHITECTURE_AVR
#include "drivers/avr.c"
#elif CAN_ARCHITECTURE_STM32F2XX
#include "drivers/stm32f2xx.c"
#elif CAN_ARCHITECTURE_TEST

void Can_Init(uint32_t baudrate) {
  // TODO test harness
}

Can_ErrorID_T Can_RawWrite(Frame *frame) {
  // TODO test harness
  return 0;
}

Can_ErrorID_T Can_RawRead(Frame *frame) {
  // TODO test harness
  return 0;
}

#else
#error "You need to define a driver architecture!"
#endif

// TODO this is a bit of a hack...unknown reads should follow same as regular reads
// and use of Can_RawRead must be banned.
Can_ErrorID_T Can_Unknown_Read(Frame *frame) {
  if (lastError == Can_Error_NONE) {
    frame->id = lastMessage.id;
    frame->len = lastMessage.len;
    uint8_t i;
    for (i = 0; i < 8; i++) {
      frame->data[i] = lastMessage.data[i];
    }
    lastError = Can_Error_NO_RX;
    return Can_Error_NONE;
  } else {
    return lastError;
  }
}

Can_ErrorID_T Can_Error_Read(void) {
  Can_ErrorID_T cachedError = lastError;
  lastError = Can_Error_NO_RX;
  return cachedError;
}

void init_can0_can_node(void) {
	Can_Init(500000);
	Can_SetFilter(0x700, 0);
}

void init_can0_rear_can_node(void) {
	Can_Init(500000);
}

void init_can0_vcu(void) {
	Can_Init(500000);
	Can_SetFilter(0x700, 0);
}

void init_can0_dash(void) {
	Can_Init(500000);
	Can_SetFilter(0x700, 0);
}

void init_can0_bms(void) {
	Can_Init(500000);
	Can_SetFilter(0x0, 0);
}

can0_T identify_can0(Frame* frame){
	switch(frame->id) {
		case can0_FrontCanNodeOutput_can_id:
			return can0_FrontCanNodeOutput;
		case can0_FrontCanNodeWheelSpeed_can_id:
			return can0_FrontCanNodeWheelSpeed;
		case can0_CellTemperatures_can_id:
			return can0_CellTemperatures;
		case can0_CellVoltages_can_id:
			return can0_CellVoltages;
		case can0_BMSHeartbeat_can_id:
			return can0_BMSHeartbeat;
		case can0_Dash_Request_can_id:
			return can0_Dash_Request;
		case can0_BMSRequest_can_id:
			return can0_BMSRequest;
		case can0_VcuToDash_can_id:
			return can0_VcuToDash;
		case can0_RearCanNodeWheelSpeed_can_id:
			return can0_RearCanNodeWheelSpeed;
		case can0_CurrentSensor_Current_can_id:
			return can0_CurrentSensor_Current;
		case can0_CurrentSensor_Voltage_can_id:
			return can0_CurrentSensor_Voltage;
		case can0_CurrentSensor_Power_can_id:
			return can0_CurrentSensor_Power;
		case can0_CurrentSensor_Energy_can_id:
			return can0_CurrentSensor_Energy;
		case can0_MCCommand_can_id:
			return can0_MCCommand;
		case can0_MCVoltage_can_id:
			return can0_MCVoltage;
	default:
		return CAN_UNKNOWN_MSG;
	}
}

CAN_PACK(can0_FrontCanNodeOutput) {
	uint64_t bitstring = 0;
	bitstring = INSERT(type_in->requested_torque, bitstring, 0, 16);
	bitstring = INSERT(type_in->is_throttle_implausible, bitstring, 16, 1);
	bitstring = INSERT(type_in->has_brake_throttle_conflict, bitstring, 17, 1);
	from_bitstring(&bitstring, can_out->data);
	can_out->id = can0_FrontCanNodeOutput_can_id;
	can_out->len = 3;
	can_out->extended = false;
}

CAN_UNPACK(can0_FrontCanNodeOutput) {
	uint64_t bitstring = 0;
	to_bitstring(can_in->data, &bitstring);
	type_out->requested_torque = SIGN(EXTRACT(bitstring, 0, 16), 16);
	type_out->is_throttle_implausible = EXTRACT(bitstring, 16, 1);
	type_out->has_brake_throttle_conflict = EXTRACT(bitstring, 17, 1);
}

CAN_PACK(can0_FrontCanNodeWheelSpeed) {
	uint64_t bitstring = 0;
	bitstring = INSERT(type_in->front_right_wheel_speed, bitstring, 0, 32);
	bitstring = INSERT(type_in->front_left_wheel_speed, bitstring, 32, 32);
	from_bitstring(&bitstring, can_out->data);
	can_out->id = can0_FrontCanNodeWheelSpeed_can_id;
	can_out->len = 8;
	can_out->extended = false;
}

CAN_UNPACK(can0_FrontCanNodeWheelSpeed) {
	uint64_t bitstring = 0;
	to_bitstring(can_in->data, &bitstring);
	type_out->front_right_wheel_speed = EXTRACT(bitstring, 0, 32);
	type_out->front_left_wheel_speed = EXTRACT(bitstring, 32, 32);
}

CAN_PACK(can0_CellTemperatures) {
	uint64_t bitstring = 0;
	bitstring = INSERT(type_in->min, bitstring, 0, 10);
	bitstring = INSERT(type_in->argmin, bitstring, 10, 9);
	bitstring = INSERT(type_in->max, bitstring, 19, 10);
	bitstring = INSERT(type_in->argmax, bitstring, 29, 9);
	from_bitstring(&bitstring, can_out->data);
	can_out->id = can0_CellTemperatures_can_id;
	can_out->len = 5;
	can_out->extended = false;
}

CAN_UNPACK(can0_CellTemperatures) {
	uint64_t bitstring = 0;
	to_bitstring(can_in->data, &bitstring);
	type_out->min = EXTRACT(bitstring, 0, 10);
	type_out->argmin = EXTRACT(bitstring, 10, 9);
	type_out->max = EXTRACT(bitstring, 19, 10);
	type_out->argmax = EXTRACT(bitstring, 29, 9);
}

CAN_PACK(can0_CellVoltages) {
	uint64_t bitstring = 0;
	bitstring = INSERT(type_in->min, bitstring, 30, 10);
	bitstring = INSERT(type_in->argmin, bitstring, 40, 7);
	bitstring = INSERT(type_in->max, bitstring, 47, 10);
	bitstring = INSERT(type_in->argmax, bitstring, 57, 7);
	from_bitstring(&bitstring, can_out->data);
	can_out->id = can0_CellVoltages_can_id;
	can_out->len = 8;
	can_out->extended = false;
}

CAN_UNPACK(can0_CellVoltages) {
	uint64_t bitstring = 0;
	to_bitstring(can_in->data, &bitstring);
	type_out->min = EXTRACT(bitstring, 30, 10);
	type_out->argmin = EXTRACT(bitstring, 40, 7);
	type_out->max = EXTRACT(bitstring, 47, 10);
	type_out->argmax = EXTRACT(bitstring, 57, 7);
}

CAN_PACK(can0_BMSHeartbeat) {
	uint64_t bitstring = 0;
	bitstring = INSERT(type_in->error_pec, bitstring, 0, 1);
	bitstring = INSERT(type_in->error_cvst, bitstring, 1, 1);
	bitstring = INSERT(type_in->error_owt, bitstring, 2, 1);
	bitstring = INSERT(type_in->error_eeprom, bitstring, 3, 1);
	bitstring = INSERT(type_in->error_cell_under_voltage, bitstring, 4, 1);
	bitstring = INSERT(type_in->error_cell_over_voltage, bitstring, 5, 1);
	bitstring = INSERT(type_in->error_cell_under_temp, bitstring, 6, 1);
	bitstring = INSERT(type_in->error_cell_over_temp, bitstring, 7, 1);
	bitstring = INSERT(type_in->error_over_current, bitstring, 8, 1);
	bitstring = INSERT(type_in->error_can, bitstring, 9, 1);
	bitstring = INSERT(type_in->error_conflicting_mode_requests, bitstring, 10, 1);
	bitstring = INSERT(type_in->error_vcu_dead, bitstring, 11, 1);
	bitstring = INSERT(type_in->error_control_flow, bitstring, 12, 1);
	bitstring = INSERT(type_in->error_blown_fuse, bitstring, 13, 1);
	bitstring = INSERT(type_in->error_contactor_welded, bitstring, 14, 1);
	bitstring = INSERT(type_in->soc, bitstring, 15, 8);
	from_bitstring(&bitstring, can_out->data);
	can_out->id = can0_BMSHeartbeat_can_id;
	can_out->len = 3;
	can_out->extended = false;
}

CAN_UNPACK(can0_BMSHeartbeat) {
	uint64_t bitstring = 0;
	to_bitstring(can_in->data, &bitstring);
	type_out->error_pec = EXTRACT(bitstring, 0, 1);
	type_out->error_cvst = EXTRACT(bitstring, 1, 1);
	type_out->error_owt = EXTRACT(bitstring, 2, 1);
	type_out->error_eeprom = EXTRACT(bitstring, 3, 1);
	type_out->error_cell_under_voltage = EXTRACT(bitstring, 4, 1);
	type_out->error_cell_over_voltage = EXTRACT(bitstring, 5, 1);
	type_out->error_cell_under_temp = EXTRACT(bitstring, 6, 1);
	type_out->error_cell_over_temp = EXTRACT(bitstring, 7, 1);
	type_out->error_over_current = EXTRACT(bitstring, 8, 1);
	type_out->error_can = EXTRACT(bitstring, 9, 1);
	type_out->error_conflicting_mode_requests = EXTRACT(bitstring, 10, 1);
	type_out->error_vcu_dead = EXTRACT(bitstring, 11, 1);
	type_out->error_control_flow = EXTRACT(bitstring, 12, 1);
	type_out->error_blown_fuse = EXTRACT(bitstring, 13, 1);
	type_out->error_contactor_welded = EXTRACT(bitstring, 14, 1);
	type_out->soc = EXTRACT(bitstring, 15, 8);
}

CAN_PACK(can0_Dash_Request) {
	uint64_t bitstring = 0;
	bitstring = INSERT(type_in->type, bitstring, 0, 3);
	from_bitstring(&bitstring, can_out->data);
	can_out->id = can0_Dash_Request_can_id;
	can_out->len = 1;
	can_out->extended = false;
}

CAN_UNPACK(can0_Dash_Request) {
	uint64_t bitstring = 0;
	to_bitstring(can_in->data, &bitstring);
	type_out->type = (can0_Dash_Request_type_T)EXTRACT(bitstring, 0, 3);
}

CAN_PACK(can0_BMSRequest) {
	uint64_t bitstring = 0;
	bitstring = INSERT(type_in->state, bitstring, 0, 1);
	from_bitstring(&bitstring, can_out->data);
	can_out->id = can0_BMSRequest_can_id;
	can_out->len = 1;
	can_out->extended = false;
}

CAN_UNPACK(can0_BMSRequest) {
	uint64_t bitstring = 0;
	to_bitstring(can_in->data, &bitstring);
	type_out->state = (can0_BMSRequest_state_T)EXTRACT(bitstring, 0, 1);
}

CAN_PACK(can0_VcuToDash) {
	uint64_t bitstring = 0;
	bitstring = INSERT(type_in->rtd_light_on, bitstring, 0, 1);
	bitstring = INSERT(type_in->ams_light_on, bitstring, 1, 1);
	bitstring = INSERT(type_in->imd_light_on, bitstring, 2, 1);
	bitstring = INSERT(type_in->hv_light_on, bitstring, 3, 1);
	bitstring = INSERT(type_in->traction_control, bitstring, 4, 1);
	bitstring = INSERT(type_in->limp_mode_on, bitstring, 5, 1);
	bitstring = INSERT(type_in->lv_warning_on, bitstring, 6, 1);
	bitstring = INSERT(type_in->active_aero_on, bitstring, 7, 1);
	bitstring = INSERT(type_in->regen_on, bitstring, 8, 1);
	bitstring = INSERT(type_in->shutdown_esd_drain_open, bitstring, 9, 1);
	bitstring = INSERT(type_in->shutdown_bms_open, bitstring, 10, 1);
	bitstring = INSERT(type_in->shutdown_imd_open, bitstring, 11, 1);
	bitstring = INSERT(type_in->shutdown_bspd_open, bitstring, 12, 1);
	bitstring = INSERT(type_in->shutdown_vcu_open, bitstring, 13, 1);
	bitstring = INSERT(type_in->shutdown_precharge_open, bitstring, 14, 1);
	bitstring = INSERT(type_in->shutdown_master_reset_open, bitstring, 15, 1);
	bitstring = INSERT(type_in->shutdown_driver_reset_open, bitstring, 16, 1);
	bitstring = INSERT(type_in->heartbeat_front_can_node_dead, bitstring, 25, 1);
	bitstring = INSERT(type_in->heartbeat_rear_can_node_dead, bitstring, 26, 1);
	bitstring = INSERT(type_in->heartbeat_bms_dead, bitstring, 27, 1);
	bitstring = INSERT(type_in->heartbeat_dash_dead, bitstring, 28, 1);
	bitstring = INSERT(type_in->heartbeat_mc_dead, bitstring, 29, 1);
	bitstring = INSERT(type_in->heartbeat_current_sensor_dead, bitstring, 30, 1);
	bitstring = INSERT(type_in->tsms_off, bitstring, 31, 1);
	bitstring = INSERT(type_in->reset_latch_open, bitstring, 32, 1);
	bitstring = INSERT(type_in->precharge_running, bitstring, 33, 1);
	bitstring = INSERT(type_in->master_reset_not_initialized, bitstring, 34, 1);
	bitstring = INSERT(type_in->driver_reset_not_initialized, bitstring, 35, 1);
	bitstring = INSERT(type_in->lv_battery_voltage, bitstring, 40, 10);
	bitstring = INSERT(type_in->limp_state, bitstring, 50, 3);
	from_bitstring(&bitstring, can_out->data);
	can_out->id = can0_VcuToDash_can_id;
	can_out->len = 7;
	can_out->extended = false;
}

CAN_UNPACK(can0_VcuToDash) {
	uint64_t bitstring = 0;
	to_bitstring(can_in->data, &bitstring);
	type_out->rtd_light_on = EXTRACT(bitstring, 0, 1);
	type_out->ams_light_on = EXTRACT(bitstring, 1, 1);
	type_out->imd_light_on = EXTRACT(bitstring, 2, 1);
	type_out->hv_light_on = EXTRACT(bitstring, 3, 1);
	type_out->traction_control = EXTRACT(bitstring, 4, 1);
	type_out->limp_mode_on = EXTRACT(bitstring, 5, 1);
	type_out->lv_warning_on = EXTRACT(bitstring, 6, 1);
	type_out->active_aero_on = EXTRACT(bitstring, 7, 1);
	type_out->regen_on = EXTRACT(bitstring, 8, 1);
	type_out->shutdown_esd_drain_open = EXTRACT(bitstring, 9, 1);
	type_out->shutdown_bms_open = EXTRACT(bitstring, 10, 1);
	type_out->shutdown_imd_open = EXTRACT(bitstring, 11, 1);
	type_out->shutdown_bspd_open = EXTRACT(bitstring, 12, 1);
	type_out->shutdown_vcu_open = EXTRACT(bitstring, 13, 1);
	type_out->shutdown_precharge_open = EXTRACT(bitstring, 14, 1);
	type_out->shutdown_master_reset_open = EXTRACT(bitstring, 15, 1);
	type_out->shutdown_driver_reset_open = EXTRACT(bitstring, 16, 1);
	type_out->heartbeat_front_can_node_dead = EXTRACT(bitstring, 25, 1);
	type_out->heartbeat_rear_can_node_dead = EXTRACT(bitstring, 26, 1);
	type_out->heartbeat_bms_dead = EXTRACT(bitstring, 27, 1);
	type_out->heartbeat_dash_dead = EXTRACT(bitstring, 28, 1);
	type_out->heartbeat_mc_dead = EXTRACT(bitstring, 29, 1);
	type_out->heartbeat_current_sensor_dead = EXTRACT(bitstring, 30, 1);
	type_out->tsms_off = EXTRACT(bitstring, 31, 1);
	type_out->reset_latch_open = EXTRACT(bitstring, 32, 1);
	type_out->precharge_running = EXTRACT(bitstring, 33, 1);
	type_out->master_reset_not_initialized = EXTRACT(bitstring, 34, 1);
	type_out->driver_reset_not_initialized = EXTRACT(bitstring, 35, 1);
	type_out->lv_battery_voltage = EXTRACT(bitstring, 40, 10);
	type_out->limp_state = (can0_VcuToDash_limp_state_T)EXTRACT(bitstring, 50, 3);
}

CAN_PACK(can0_RearCanNodeWheelSpeed) {
	uint64_t bitstring = 0;
	bitstring = INSERT(type_in->rear_right_wheel_speed, bitstring, 0, 32);
	bitstring = INSERT(type_in->rear_left_wheel_speed, bitstring, 32, 32);
	from_bitstring(&bitstring, can_out->data);
	can_out->id = can0_RearCanNodeWheelSpeed_can_id;
	can_out->len = 8;
	can_out->extended = false;
}

CAN_UNPACK(can0_RearCanNodeWheelSpeed) {
	uint64_t bitstring = 0;
	to_bitstring(can_in->data, &bitstring);
	type_out->rear_right_wheel_speed = EXTRACT(bitstring, 0, 32);
	type_out->rear_left_wheel_speed = EXTRACT(bitstring, 32, 32);
}

CAN_PACK(can0_CurrentSensor_Current) {
	uint64_t bitstring = 0;
	bitstring = INSERT(type_in->pack_current, bitstring, 16, 32);
	from_bitstring(&bitstring, can_out->data);
	can_out->id = can0_CurrentSensor_Current_can_id;
	can_out->len = 6;
	can_out->extended = false;
}

CAN_UNPACK(can0_CurrentSensor_Current) {
	uint64_t bitstring = 0;
	to_bitstring(can_in->data, &bitstring);
	type_out->pack_current = SIGN(EXTRACT(bitstring, 16, 32), 32);
}

CAN_PACK(can0_CurrentSensor_Voltage) {
	uint64_t bitstring = 0;
	bitstring = INSERT(type_in->pack_voltage, bitstring, 16, 32);
	from_bitstring(&bitstring, can_out->data);
	can_out->id = can0_CurrentSensor_Voltage_can_id;
	can_out->len = 6;
	can_out->extended = false;
}

CAN_UNPACK(can0_CurrentSensor_Voltage) {
	uint64_t bitstring = 0;
	to_bitstring(can_in->data, &bitstring);
	type_out->pack_voltage = SIGN(EXTRACT(bitstring, 16, 32), 32);
}

CAN_PACK(can0_CurrentSensor_Power) {
	uint64_t bitstring = 0;
	bitstring = INSERT(type_in->power, bitstring, 16, 32);
	from_bitstring(&bitstring, can_out->data);
	can_out->id = can0_CurrentSensor_Power_can_id;
	can_out->len = 6;
	can_out->extended = false;
}

CAN_UNPACK(can0_CurrentSensor_Power) {
	uint64_t bitstring = 0;
	to_bitstring(can_in->data, &bitstring);
	type_out->power = EXTRACT(bitstring, 16, 32);
}

CAN_PACK(can0_CurrentSensor_Energy) {
	uint64_t bitstring = 0;
	bitstring = INSERT(type_in->pack_energy, bitstring, 16, 32);
	from_bitstring(&bitstring, can_out->data);
	can_out->id = can0_CurrentSensor_Energy_can_id;
	can_out->len = 6;
	can_out->extended = false;
}

CAN_UNPACK(can0_CurrentSensor_Energy) {
	uint64_t bitstring = 0;
	to_bitstring(can_in->data, &bitstring);
	type_out->pack_energy = EXTRACT(bitstring, 16, 32);
}

CAN_PACK(can0_MCCommand) {
	uint64_t bitstring = 0;
	bitstring = INSERT(type_in->torque, bitstring, 0, 16);
	bitstring = INSERT(type_in->speed, bitstring, 16, 16);
	bitstring = INSERT(type_in->direction_is_counterclockwise, bitstring, 32, 1);
	bitstring = INSERT(type_in->inverter_enabled, bitstring, 40, 1);
	bitstring = INSERT(type_in->discharge_enabled, bitstring, 41, 1);
	bitstring = INSERT(type_in->torque_limit, bitstring, 48, 16);
	from_bitstring(&bitstring, can_out->data);
	can_out->id = can0_MCCommand_can_id;
	can_out->len = 8;
	can_out->extended = false;
}

CAN_UNPACK(can0_MCCommand) {
	uint64_t bitstring = 0;
	to_bitstring(can_in->data, &bitstring);
	type_out->torque = EXTRACT(bitstring, 0, 16);
	type_out->speed = EXTRACT(bitstring, 16, 16);
	type_out->direction_is_counterclockwise = EXTRACT(bitstring, 32, 1);
	type_out->inverter_enabled = EXTRACT(bitstring, 40, 1);
	type_out->discharge_enabled = EXTRACT(bitstring, 41, 1);
	type_out->torque_limit = EXTRACT(bitstring, 48, 16);
}

CAN_PACK(can0_MCVoltage) {
	uint64_t bitstring = 0;
	bitstring = INSERT(type_in->busVoltage, bitstring, 0, 16);
	bitstring = INSERT(type_in->outVoltage, bitstring, 16, 16);
	bitstring = INSERT(type_in->VAB_Vd_Voltage, bitstring, 32, 16);
	bitstring = INSERT(type_in->VBC_Vq_Voltage, bitstring, 48, 16);
	from_bitstring(&bitstring, can_out->data);
	can_out->id = can0_MCVoltage_can_id;
	can_out->len = 8;
	can_out->extended = false;
}

CAN_UNPACK(can0_MCVoltage) {
	uint64_t bitstring = 0;
	to_bitstring(can_in->data, &bitstring);
	type_out->busVoltage = SIGN(EXTRACT(bitstring, 0, 16), 16);
	type_out->outVoltage = SIGN(EXTRACT(bitstring, 16, 16), 16);
	type_out->VAB_Vd_Voltage = SIGN(EXTRACT(bitstring, 32, 16), 16);
	type_out->VBC_Vq_Voltage = SIGN(EXTRACT(bitstring, 48, 16), 16);
}

DEFINE(can0_FrontCanNodeOutput)
DEFINE(can0_FrontCanNodeWheelSpeed)
DEFINE(can0_CellTemperatures)
DEFINE(can0_CellVoltages)
DEFINE(can0_BMSHeartbeat)
DEFINE(can0_Dash_Request)
DEFINE(can0_BMSRequest)
DEFINE(can0_VcuToDash)
DEFINE(can0_RearCanNodeWheelSpeed)
DEFINE(can0_CurrentSensor_Current)
DEFINE(can0_CurrentSensor_Voltage)
DEFINE(can0_CurrentSensor_Power)
DEFINE(can0_CurrentSensor_Energy)
DEFINE(can0_MCCommand)
DEFINE(can0_MCVoltage)