87 lines
2.4 KiB
C
87 lines
2.4 KiB
C
#include "shunt_monitoring.h"
|
|
|
|
#include <string.h>
|
|
|
|
#include "can.h"
|
|
#include "ts_state_machine.h"
|
|
#include "util.h"
|
|
|
|
#include "can-halal.h"
|
|
|
|
ShuntData shunt_data;
|
|
|
|
void shunt_init() {
|
|
shunt_data.current = 0;
|
|
shunt_data.voltage_veh = 0;
|
|
shunt_data.voltage_bat = 0;
|
|
shunt_data.voltage3 = 0;
|
|
shunt_data.busbartemp = 0;
|
|
shunt_data.power = 0;
|
|
shunt_data.energy = 0;
|
|
shunt_data.current_counter = 0;
|
|
shunt_data.last_message = 0;
|
|
shunt_data.last_current_message = 0;
|
|
}
|
|
|
|
void shunt_check() {
|
|
int is_error = 0;
|
|
TSErrorKind error_type = TS_ERRORKIND_NONE;
|
|
if (HAL_GetTick() - shunt_data.last_message > SHUNT_TIMEOUT) {
|
|
is_error = 1;
|
|
error_type = TS_ERRORKIND_SHUNT_TIMEOUT;
|
|
can_send_error(TS_ERRORKIND_SHUNT_TIMEOUT, 0);
|
|
} else if (shunt_data.current >= SHUNT_THRESH_OVERCURRENT) {
|
|
is_error = 1;
|
|
error_type = TS_ERRORKIND_SHUNT_OVERCURRENT;
|
|
can_send_error(TS_ERRORKIND_SHUNT_OVERCURRENT, 0);
|
|
} else if (shunt_data.busbartemp >= SHUNT_THRESH_OVERTEMP) {
|
|
is_error = 1;
|
|
error_type = TS_ERRORKIND_SHUNT_OVERTEMP;
|
|
can_send_error(TS_ERRORKIND_SHUNT_OVERTEMP, 0);
|
|
}
|
|
ts_sm_set_error_source(TS_ERROR_SOURCE_SHUNT, error_type, is_error);
|
|
}
|
|
|
|
void shunt_handle_can_msg(uint16_t id, const uint8_t *data) {
|
|
shunt_data.last_message = HAL_GetTick();
|
|
|
|
// All result messages contain a big-endian 6-byte integer
|
|
uint64_t result = ftcan_unmarshal_unsigned(&data, 6);
|
|
|
|
switch (id) {
|
|
case CAN_ID_SHUNT_CURRENT:
|
|
shunt_data.current = result;
|
|
if (shunt_data.last_current_message > 0) {
|
|
uint32_t now = HAL_GetTick();
|
|
float dt = (now - shunt_data.last_current_message) * 0.001f;
|
|
shunt_data.current_counter += shunt_data.current * dt;
|
|
}
|
|
shunt_data.last_current_message = HAL_GetTick();
|
|
break;
|
|
case CAN_ID_SHUNT_VOLTAGE1:
|
|
shunt_data.voltage_bat = result;
|
|
break;
|
|
case CAN_ID_SHUNT_VOLTAGE2:
|
|
shunt_data.voltage_veh = result;
|
|
break;
|
|
case CAN_ID_SHUNT_VOLTAGE3:
|
|
shunt_data.voltage3 = result;
|
|
break;
|
|
case CAN_ID_SHUNT_TEMP:
|
|
shunt_data.busbartemp = result;
|
|
break;
|
|
case CAN_ID_SHUNT_POWER:
|
|
shunt_data.power = result;
|
|
break;
|
|
case CAN_ID_SHUNT_CURRENT_COUNTER:
|
|
// TODO: Use this when we get the shunt to emit current counter data (the
|
|
// shunt apparently emits As, not mAs)
|
|
|
|
// shunt_data.current_counter = result * 1000;
|
|
break;
|
|
case CAN_ID_SHUNT_ENERGY_COUNTER:
|
|
shunt_data.energy = result;
|
|
break;
|
|
}
|
|
}
|