FaSTTUBe/steering-wheel
51
0
Fork 0
Code Issues Pull Requests Packages Projects Releases Wiki Activity

add uncalibrated version of lv soc

Browse Source
This commit is contained in:
Leonard Gies
2025-07-22 18:12:43 +02:00
parent 2c470f43b2
commit 4354b03907
4 changed files with 47 additions and 0 deletions
Download Patch File Download Diff File Expand all files Collapse all files

44
Core/Src/vehicle.c
View File

@ -64,6 +64,24 @@
#define CAN_ID_STW_BUTTONS 0x401
#define CAN_ID_STW_PARAM_SET 0x402
#define NUM_LV_SOC_VOLTAGE_MAP 11
struct { // percent and voltage must be monotonically decreasing
uint8_t percent;
float voltage;
} LV_SOC_VOLTAGE_MAP[NUM_LV_SOC_VOLTAGE_MAP] = {
{.percent = 100, .voltage = 27.20}, //
{.percent = 90, .voltage = 26.80}, //
{.percent = 80, .voltage = 26.56}, //
{.percent = 70, .voltage = 26.40}, //
{.percent = 60, .voltage = 26.16}, //
{.percent = 50, .voltage = 26.08}, //
{.percent = 40, .voltage = 26.00}, //
{.percent = 30, .voltage = 25.76}, //
{.percent = 20, .voltage = 25.60}, //
{.percent = 10, .voltage = 24.00}, //
{.percent = 0, .voltage = 20.00}, //
};
void vehicle_thread_entry(ULONG hfdcan_addr) {
memset(&vehicle_state, 0, sizeof(vehicle_state));
@ -140,6 +158,31 @@ void vehicle_broadcast_buttons(GPIO_PinState *button_states) {
ftcan_transmit(CAN_ID_STW_BUTTONS, &data, 1);
}
float calculate_lv_soc(float lv_voltage) {
// check if lv_voltage is in range of voltage map
if (lv_voltage >= LV_SOC_VOLTAGE_MAP[0].voltage) {
return LV_SOC_VOLTAGE_MAP[0].percent;
}
if (lv_voltage <= LV_SOC_VOLTAGE_MAP[NUM_LV_SOC_VOLTAGE_MAP - 1].voltage) {
return LV_SOC_VOLTAGE_MAP[NUM_LV_SOC_VOLTAGE_MAP - 1].percent;
}
size_t index_upper_volt = 0;
size_t index_lower_volt = 1;
while (index_lower_volt < NUM_LV_SOC_VOLTAGE_MAP && !(lv_voltage <= LV_SOC_VOLTAGE_MAP[index_upper_volt].voltage &&
lv_voltage >= LV_SOC_VOLTAGE_MAP[index_lower_volt].voltage)) {
index_upper_volt++;
index_lower_volt++;
}
// calculate f value between 0 and 1 for linear interpolation
float f = (lv_voltage - LV_SOC_VOLTAGE_MAP[index_lower_volt].voltage) /
(LV_SOC_VOLTAGE_MAP[index_upper_volt].voltage - LV_SOC_VOLTAGE_MAP[index_lower_volt].voltage);
return LV_SOC_VOLTAGE_MAP[index_lower_volt].percent * (1.0 - f) + LV_SOC_VOLTAGE_MAP[index_upper_volt].percent * f;
}
void ftcan_msg_received_cb(uint16_t id, size_t datalen, const uint8_t *data) {
const uint8_t *ptr;
switch (id) {
@ -266,6 +309,7 @@ void ftcan_msg_received_cb(uint16_t id, size_t datalen, const uint8_t *data) {
case CAN_ID_PDU_CURRENT4:
ptr = &data[2];
vehicle_state.lv_voltage = ftcan_unmarshal_signed(&ptr, 2) * 0.001f;
vehicle_state.lv_soc = calculate_lv_soc(vehicle_state.lv_voltage);
break;
case CAN_ID_DASHBOARD:
// TODO