update to new isotp stack, send voltages every ~50 ms
This commit is contained in:
parent
2bbe11267a
commit
162f58abe2
SSH Key Fingerprint:
SHA256:mXpyZkK7RDiJ7qeHCKJX108woM0cl5TrCvNBJASu6lM
@ -14,7 +14,7 @@ extern int16_t min_temp;
|
||||
extern struct {int16_t min; int16_t max;} module_temps[N_BMS];
|
||||
extern float module_std_deviation[N_BMS];
|
||||
|
||||
extern int16_t cellTemps[N_BMS][N_CELLS];
|
||||
extern int16_t cellTemps[N_BMS][10];
|
||||
|
||||
HAL_StatusTypeDef battery_init(SPI_HandleTypeDef* hspi);
|
||||
HAL_StatusTypeDef battery_update();
|
||||
|
||||
@ -11,6 +11,11 @@
|
||||
#define CAN_ID_AMS_ERROR 0x00C
|
||||
#define CAN_ID_SLAVE_STATUS_BASE 0x080
|
||||
#define CAN_ID_AMS_SIGNALS 0x090
|
||||
|
||||
//TEMPORARY!!
|
||||
#define CAN_ID_AMS_DETAILS 0x091
|
||||
#define CAN_ID_AMS_DETAILS_FC 0x092
|
||||
|
||||
#define CAN_ID_SLAVE_LOG 0x4F4
|
||||
#define CAN_ID_SHUNT_BASE 0x520
|
||||
#define CAN_ID_SHUNT_CURRENT 0x521
|
||||
@ -22,12 +27,10 @@
|
||||
#define CAN_ID_SHUNT_CURRENT_COUNTER 0x527
|
||||
#define CAN_ID_SHUNT_ENERGY_COUNTER 0x528
|
||||
|
||||
//TEMP until final IDs are defined
|
||||
#define CAN_ID_LOG_FC 0x132
|
||||
|
||||
void can_init(FDCAN_HandleTypeDef *handle);
|
||||
HAL_StatusTypeDef can_send_status();
|
||||
HAL_StatusTypeDef can_send_error(TSErrorKind kind, uint8_t arg);
|
||||
HAL_StatusTypeDef can_send_details();
|
||||
|
||||
void ftcan_msg_received_cb(uint16_t id, size_t datalen, const uint8_t *data);
|
||||
|
||||
|
||||
@ -74,20 +74,19 @@ struct ADBMS6830_Internal_Status {
|
||||
#define MAXIMUM_GPIO 10
|
||||
|
||||
typedef struct {
|
||||
int16_t cellVoltages[MAXIMUM_CELL_VOLTAGES];
|
||||
int16_t auxVoltages[MAXIMUM_AUX_VOLTAGES];
|
||||
uint64_t bmsID;
|
||||
|
||||
struct ADBMS6830_Internal_Status status;
|
||||
int16_t internalDieTemp;
|
||||
uint16_t analogSupplyVoltage;
|
||||
uint16_t digitalSupplyVoltage;
|
||||
uint16_t sumOfCellMeasurements;
|
||||
uint16_t refVoltage;
|
||||
|
||||
uint32_t overVoltage; // Bitfield of overvoltage flags
|
||||
uint32_t underVoltage; // " undervoltage "
|
||||
|
||||
int16_t internalDieTemp;
|
||||
uint16_t refVoltage;
|
||||
uint16_t analogSupplyVoltage;
|
||||
uint16_t digitalSupplyVoltage;
|
||||
|
||||
int16_t cellVoltages[MAXIMUM_CELL_VOLTAGES];
|
||||
int16_t auxVoltages[MAXIMUM_AUX_VOLTAGES];
|
||||
} Cell_Module;
|
||||
|
||||
extern uint32_t error_sources; // Bitfield of error sources
|
||||
|
||||
@ -1,7 +1,8 @@
|
||||
/*
|
||||
* Author: Kilian
|
||||
* Date: 2025-04-20
|
||||
* Description: This file contains a TX-only implementation of the ISO-TP protocol.
|
||||
* Description: This file contains an implementation of the ISO-TP protocol
|
||||
* with both transmission and reception capabilities.
|
||||
*
|
||||
* References:
|
||||
* https://munich.dissec.to/kb/chapters/isotp/isotp.html#protocol-basics-iso-15765-2
|
||||
@ -10,6 +11,7 @@
|
||||
* https://ramn.readthedocs.io/en/latest/userguide/isotp_tutorial.html
|
||||
*/
|
||||
#include "isotp.h"
|
||||
#include "isotp_user_defs.h"
|
||||
|
||||
#include <stdint.h>
|
||||
#include <stddef.h>
|
||||
@ -21,6 +23,9 @@
|
||||
#define MAX_ISOTP_DATA_FIRST 6
|
||||
#define MAX_ISOTP_DATA_CONSECUTIVE 7
|
||||
|
||||
// check endianness
|
||||
static_assert(__BYTE_ORDER__ == __ORDER_LITTLE_ENDIAN__, "Endianness is not little endian, this code is not portable!");
|
||||
|
||||
typedef union [[gnu::packed]] {
|
||||
struct {
|
||||
uint8_t header;
|
||||
@ -45,10 +50,17 @@ enum {
|
||||
};
|
||||
|
||||
struct isotp_message_state {
|
||||
#if ISOTP_RX
|
||||
uint8_t * data;
|
||||
#else
|
||||
const uint8_t * data;
|
||||
size_t remaining;
|
||||
#endif
|
||||
uint32_t timestamp;
|
||||
uint16_t remaining;
|
||||
#if ISOTP_RX
|
||||
uint16_t data_len;
|
||||
#endif
|
||||
uint16_t id;
|
||||
uint32_t timestamp; // Timestamp for timeout handling
|
||||
enum : uint8_t {
|
||||
ISOTP_NEW_MESSAGE = 0,
|
||||
ISOTP_WAITING_FLOW_CONTROL = 1,
|
||||
@ -66,56 +78,123 @@ struct isotp_message_state {
|
||||
uint8_t st_min; // in ms, we round up to 1 ms if sub 1 ms
|
||||
uint8_t st_min_counter;
|
||||
} flow_control;
|
||||
uint8_t index;
|
||||
uint8_t seq_n;
|
||||
};
|
||||
|
||||
static struct isotp_message_state tx_queue[MAX_ISOTP_MESSAGES_IN_FLIGHT] = {[0 ... MAX_ISOTP_MESSAGES_IN_FLIGHT - 1] = {.state=ISOTP_DONE }};
|
||||
|
||||
[[gnu::weak]] bool isotp_can_transmit(uint16_t, const uint8_t *, size_t) { return false; }
|
||||
[[gnu::weak]] uint32_t isotp_can_get_time() { return 0; }
|
||||
#if ISOTP_RX
|
||||
static struct isotp_message_state rx_queue[MAX_ISOTP_MESSAGES_IN_FLIGHT] = {[0 ... MAX_ISOTP_MESSAGES_IN_FLIGHT - 1] = {.state=ISOTP_DONE }};
|
||||
#endif
|
||||
|
||||
#define INVALID_CAN_ID 0xFFFF
|
||||
|
||||
static struct {uint16_t src_id; uint16_t dst_id;} connections[MAX_ISOTP_CONNECTIONS] = {
|
||||
[0 ... MAX_ISOTP_CONNECTIONS - 1] = {.src_id=INVALID_CAN_ID, .dst_id=INVALID_CAN_ID}
|
||||
};
|
||||
|
||||
uint32_t last_called;
|
||||
|
||||
isotp_status_t isotp_init() {
|
||||
// platform-specific initialization
|
||||
last_called = isotp_can_get_time() - 100;
|
||||
return (isotp_status_t) {ISOTP_OK};
|
||||
last_called = isotp_get_time() - 100;
|
||||
return ISOTP_OK;
|
||||
}
|
||||
|
||||
isotp_status_t isotp_add_message(uint16_t id, const uint8_t * data, size_t datalen) {
|
||||
#if ISOTP_RX
|
||||
#define DATA_TYPE uint8_t *
|
||||
#else
|
||||
#define DATA_TYPE const uint8_t *
|
||||
#endif
|
||||
|
||||
// Internal helper structure to return both status and index
|
||||
typedef struct {
|
||||
isotp_status_t status;
|
||||
size_t index; // Index of available slot if status is ISOTP_OK
|
||||
} isotp_validation_result_t;
|
||||
|
||||
// Internal helper function to validate message parameters and find available slot
|
||||
[[gnu::access(none, 2, 3)]]
|
||||
static isotp_validation_result_t isotp_validate_message(uint16_t id, const uint8_t *data, size_t datalen) {
|
||||
isotp_validation_result_t result = {.status = ISOTP_ERROR, .index = 0};
|
||||
|
||||
if (datalen > MAX_ISOTP_DATA_SIZE_MESSAGE) {
|
||||
return ISOTP_DATA_TOO_LONG;
|
||||
result.status = ISOTP_DATA_TOO_LONG;
|
||||
return result;
|
||||
}
|
||||
|
||||
if (data == NULL || datalen == 0) {
|
||||
return ISOTP_INVALID_PARAMETER;
|
||||
result.status = ISOTP_INVALID_PARAMETER;
|
||||
return result;
|
||||
}
|
||||
|
||||
// Check if connection exists
|
||||
bool connection_found = false;
|
||||
for (size_t i = 0; i < MAX_ISOTP_CONNECTIONS; i++) {
|
||||
if (connections[i].dst_id == id) {
|
||||
connection_found = true;
|
||||
break;
|
||||
}
|
||||
}
|
||||
|
||||
if (!connection_found) {
|
||||
result.status = ISOTP_CONNECTION_NOT_FOUND;
|
||||
return result;
|
||||
}
|
||||
|
||||
// Check if message with same ID is already in flight
|
||||
for (size_t i = 0; i < MAX_ISOTP_MESSAGES_IN_FLIGHT; i++) {
|
||||
if (!(tx_queue[i].state == ISOTP_DONE || tx_queue[i].state == ISOTP_FAILED) && tx_queue[i].id == id) {
|
||||
return ISOTP_MESSAGE_ALREADY_IN_FLIGHT;
|
||||
result.status = ISOTP_MESSAGE_ALREADY_IN_FLIGHT;
|
||||
return result;
|
||||
}
|
||||
}
|
||||
|
||||
// Find available slot
|
||||
for (size_t i = 0; i < MAX_ISOTP_MESSAGES_IN_FLIGHT; i++) {
|
||||
if (tx_queue[i].state == ISOTP_DONE || tx_queue[i].state == ISOTP_FAILED) {
|
||||
tx_queue[i] = (struct isotp_message_state) {data, datalen, id, 0, ISOTP_NEW_MESSAGE, {}, 1};
|
||||
return ISOTP_OK;
|
||||
result.status = ISOTP_OK;
|
||||
result.index = i;
|
||||
return result;
|
||||
}
|
||||
}
|
||||
|
||||
return ISOTP_TOO_MANY_MESSAGES;
|
||||
result.status = ISOTP_TOO_MANY_MESSAGES;
|
||||
return result;
|
||||
}
|
||||
|
||||
isotp_status_t isotp_add_message(uint16_t id, const uint8_t * data, size_t datalen) {
|
||||
isotp_validation_result_t result = isotp_validate_message(id, data, datalen);
|
||||
|
||||
if (result.status == ISOTP_OK) {
|
||||
// Add message to the queue at the found index
|
||||
tx_queue[result.index] = (struct isotp_message_state) {
|
||||
.data = (DATA_TYPE) data,
|
||||
.id = id,
|
||||
.state = ISOTP_NEW_MESSAGE,
|
||||
.seq_n = 1,
|
||||
.remaining = datalen
|
||||
};
|
||||
}
|
||||
|
||||
return result.status;
|
||||
}
|
||||
|
||||
isotp_status_t isotp_try_add_message(uint16_t id, const uint8_t * data, size_t datalen) {
|
||||
// Just validate without adding to the queue
|
||||
isotp_validation_result_t result = isotp_validate_message(id, data, datalen);
|
||||
return result.status;
|
||||
}
|
||||
|
||||
/* Macro to check CAN transmission result and handle errors */
|
||||
#define TRY_CAN_TRANSMIT(id, data, size) \
|
||||
if (!isotp_can_transmit(id, data, size)) { \
|
||||
if (!isotp_transmit(id, data, size)) { \
|
||||
tx_queue[i].state = ISOTP_FAILED; \
|
||||
return ISOTP_CAN_TRANSMIT_ERROR; \
|
||||
}
|
||||
|
||||
isotp_status_t isotp_update() {
|
||||
uint32_t now = isotp_can_get_time();
|
||||
uint32_t now = isotp_get_time();
|
||||
uint32_t delta = now - last_called;
|
||||
for (size_t i = 0; i < MAX_ISOTP_MESSAGES_IN_FLIGHT; i++) {
|
||||
|
||||
@ -160,7 +239,7 @@ isotp_status_t isotp_update() {
|
||||
tx_queue[i].flow_control.block_size--;
|
||||
}
|
||||
|
||||
msg.consecutive.header = ISOTP_CONSECUTIVE_FRAME | (tx_queue[i].index & 0x0F);
|
||||
msg.consecutive.header = ISOTP_CONSECUTIVE_FRAME | (tx_queue[i].seq_n & 0x0F);
|
||||
size_t bytes_to_send = tx_queue[i].remaining <= MAX_ISOTP_DATA_CONSECUTIVE ? tx_queue[i].remaining : MAX_ISOTP_DATA_CONSECUTIVE;
|
||||
memcpy(msg.consecutive.data, tx_queue[i].data, bytes_to_send);
|
||||
|
||||
@ -177,7 +256,7 @@ isotp_status_t isotp_update() {
|
||||
|
||||
tx_queue[i].data += bytes_to_send;
|
||||
tx_queue[i].remaining -= bytes_to_send;
|
||||
tx_queue[i].index++;
|
||||
tx_queue[i].seq_n++;
|
||||
|
||||
if (tx_queue[i].remaining == 0) {
|
||||
tx_queue[i].state = ISOTP_DONE;
|
||||
@ -199,17 +278,161 @@ isotp_status_t isotp_update() {
|
||||
break;
|
||||
}
|
||||
}
|
||||
|
||||
#if ISOTP_RX
|
||||
for (size_t i = 0; i < MAX_ISOTP_MESSAGES_IN_FLIGHT; i++) {
|
||||
if (rx_queue[i].state == ISOTP_READY) {
|
||||
if (now - rx_queue[i].timestamp > ISOTP_RX_TIMEOUT) {
|
||||
rx_queue[i].state = ISOTP_FAILED;
|
||||
}
|
||||
}
|
||||
}
|
||||
#endif
|
||||
last_called = now;
|
||||
return ISOTP_OK;
|
||||
}
|
||||
|
||||
isotp_status_t isotp_handle_flow_control(uint16_t id, const uint8_t * data, size_t datalen) {
|
||||
isotp_status_t isotp_add_connection(uint16_t src_id, uint16_t dst_id) {
|
||||
for (size_t i = 0; i < MAX_ISOTP_CONNECTIONS; i++) {
|
||||
if (connections[i].src_id == INVALID_CAN_ID) {
|
||||
connections[i].src_id = src_id;
|
||||
connections[i].dst_id = dst_id;
|
||||
return ISOTP_OK;
|
||||
}
|
||||
}
|
||||
return ISOTP_TOO_MANY_CONNECTIONS;
|
||||
}
|
||||
|
||||
enum FC_status : uint8_t {
|
||||
ISOTP_FC_CTS = 0,
|
||||
ISOTP_FC_WAIT = 1,
|
||||
ISOTP_FC_ABORT = 2
|
||||
};
|
||||
|
||||
#if ISOTP_RX
|
||||
static isotp_status_t isotp_send_fc(uint16_t id, enum FC_status status, uint8_t block_size, uint8_t st_min) {
|
||||
isotp_message msg = {};
|
||||
msg.single.header = ISOTP_FLOW_CONTROL | (status & 0x0F);
|
||||
msg.single.data[0] = block_size;
|
||||
msg.single.data[1] = st_min;
|
||||
|
||||
#if ISOTP_PADDING
|
||||
memset(msg.single.data + 2, ISOTP_PADDING, MAX_ISOTP_DATA_SINGLE - 2);
|
||||
if (!isotp_transmit(id, (const uint8_t *)&msg, MAX_CAN_DATA_SIZE)) {
|
||||
return ISOTP_CAN_TRANSMIT_ERROR;
|
||||
}
|
||||
#else
|
||||
if (!isotp_transmit(id, (const uint8_t *)&msg, 3)) {
|
||||
return ISOTP_CAN_TRANSMIT_ERROR;
|
||||
}
|
||||
#endif
|
||||
|
||||
return ISOTP_OK;
|
||||
}
|
||||
|
||||
isotp_status_t isotp_handle_first_frame(uint16_t id, uint16_t dst_id, const uint8_t * data, size_t datalen) {
|
||||
if (datalen < 3) {
|
||||
return ISOTP_NOT_FLOW_CONTROL;
|
||||
return ISOTP_INVALID_FRAME;
|
||||
}
|
||||
|
||||
if (data == NULL) {
|
||||
uint16_t length = ((data[0] & 0x0F) << 8) | data[1];
|
||||
if (length > MAX_ISOTP_DATA_SIZE_MESSAGE) {
|
||||
return ISOTP_INVALID_FRAME;
|
||||
}
|
||||
|
||||
for (size_t i = 0; i < MAX_ISOTP_MESSAGES_IN_FLIGHT; i++) {
|
||||
if (rx_queue[i].state == ISOTP_DONE || rx_queue[i].state == ISOTP_FAILED) {
|
||||
uint8_t* rx_buffer = isotp_get_rx_buffer(length);
|
||||
if (rx_buffer == NULL) {
|
||||
isotp_send_fc(dst_id, ISOTP_FC_ABORT, 0, 0); // Abort
|
||||
return ISOTP_BUFFER_CREATION_ERROR;
|
||||
}
|
||||
|
||||
rx_queue[i] = (struct isotp_message_state) {
|
||||
.data = rx_buffer,
|
||||
.id = id,
|
||||
.remaining = length,
|
||||
.data_len = length,
|
||||
.state = ISOTP_NEW_MESSAGE,
|
||||
.seq_n = 1 // Initialize sequence number
|
||||
};
|
||||
memcpy(rx_queue[i].data, data + 2, MAX_ISOTP_DATA_FIRST);
|
||||
rx_queue[i].remaining -= MAX_ISOTP_DATA_FIRST;
|
||||
rx_queue[i].data += MAX_ISOTP_DATA_FIRST;
|
||||
rx_queue[i].timestamp = isotp_get_time();
|
||||
rx_queue[i].state = ISOTP_READY;
|
||||
isotp_send_fc(dst_id, ISOTP_FC_CTS, 0, ISOTP_RX_MIN_ST); // Send flow control frame
|
||||
return ISOTP_OK;
|
||||
}
|
||||
}
|
||||
isotp_send_fc(dst_id, ISOTP_FC_ABORT, 0, 0); // Abort
|
||||
return ISOTP_TOO_MANY_MESSAGES;
|
||||
}
|
||||
|
||||
isotp_status_t isotp_handle_consecutive_frame(uint16_t id, uint16_t dst_id, const uint8_t * data, size_t datalen) {
|
||||
if (datalen < 2) {
|
||||
return ISOTP_INVALID_FRAME;
|
||||
}
|
||||
|
||||
uint8_t seq_n = data[0] & 0x0F;
|
||||
|
||||
for (size_t i = 0; i < MAX_ISOTP_MESSAGES_IN_FLIGHT; i++) {
|
||||
if (rx_queue[i].state == ISOTP_DONE || rx_queue[i].state == ISOTP_FAILED || rx_queue[i].id != id) {
|
||||
continue;
|
||||
}
|
||||
|
||||
if (rx_queue[i].seq_n != seq_n) {
|
||||
isotp_send_fc(dst_id, ISOTP_FC_ABORT, 0, 0); // Abort
|
||||
rx_queue[i].state = ISOTP_FAILED;
|
||||
isotp_free_rx_buffer(rx_queue[i].data - rx_queue[i].data_len + rx_queue[i].remaining); // Free the buffer
|
||||
return ISOTP_INVALID_SEQUENCE; // Sequence number mismatch
|
||||
}
|
||||
|
||||
rx_queue[i].seq_n++;
|
||||
rx_queue[i].seq_n &= 0x0F; // Wrap around sequence number
|
||||
|
||||
|
||||
size_t bytes_to_copy = rx_queue[i].remaining <= MAX_ISOTP_DATA_CONSECUTIVE ? rx_queue[i].remaining : MAX_ISOTP_DATA_CONSECUTIVE;
|
||||
memcpy(rx_queue[i].data, data + 1, bytes_to_copy);
|
||||
rx_queue[i].data += bytes_to_copy;
|
||||
rx_queue[i].remaining -= bytes_to_copy;
|
||||
rx_queue[i].timestamp = isotp_get_time(); // Update timestamp for timeout handling
|
||||
|
||||
if (rx_queue[i].remaining == 0) {
|
||||
rx_queue[i].state = ISOTP_DONE;
|
||||
isotp_on_message_received(dst_id, rx_queue[i].data - rx_queue[i].data_len, rx_queue[i].data_len);
|
||||
isotp_free_rx_buffer(rx_queue[i].data - rx_queue[i].data_len); // Free the buffer after message is received
|
||||
}
|
||||
|
||||
return ISOTP_OK;
|
||||
}
|
||||
|
||||
isotp_send_fc(dst_id, ISOTP_FC_ABORT, 0, 0); // Abort
|
||||
return ISOTP_INVALID_PARAMETER;
|
||||
}
|
||||
|
||||
isotp_status_t isotp_handle_single_frame(uint16_t id, const uint8_t * data, size_t datalen) {
|
||||
if (datalen < 2) {
|
||||
return ISOTP_INVALID_FRAME;
|
||||
}
|
||||
|
||||
uint8_t length = data[0] & 0x0F;
|
||||
if (length > MAX_ISOTP_DATA_SINGLE) {
|
||||
return ISOTP_INVALID_FRAME;
|
||||
}
|
||||
|
||||
if (datalen < (unsigned) (length + 1)) {
|
||||
return ISOTP_INVALID_FRAME;
|
||||
}
|
||||
|
||||
isotp_on_message_received(id, data + 1, length); // Send the correct length of data
|
||||
return ISOTP_OK;
|
||||
}
|
||||
#endif // ISOTP_RX
|
||||
|
||||
static isotp_status_t isotp_handle_flow_control(uint16_t id, const uint8_t * data, size_t datalen) {
|
||||
if (datalen < 3) {
|
||||
return ISOTP_INVALID_FRAME;
|
||||
}
|
||||
|
||||
for (size_t i = 0; i < MAX_ISOTP_MESSAGES_IN_FLIGHT; i++) {
|
||||
@ -217,30 +440,25 @@ isotp_status_t isotp_handle_flow_control(uint16_t id, const uint8_t * data, size
|
||||
continue;
|
||||
}
|
||||
|
||||
uint8_t type = data[0] & 0xF0;
|
||||
uint8_t status = data[0] & 0x0F;
|
||||
uint8_t block_size = data[1];
|
||||
uint8_t st_min = data[2];
|
||||
|
||||
if (type != ISOTP_FLOW_CONTROL) {
|
||||
return ISOTP_NOT_FLOW_CONTROL;
|
||||
}
|
||||
|
||||
switch (status) {
|
||||
case 0: // Continue
|
||||
case ISOTP_FC_CTS: // Continue
|
||||
tx_queue[i].flow_control.state = ISOTP_CONTINUE;
|
||||
break;
|
||||
case 1: // Wait
|
||||
case ISOTP_FC_WAIT: // Wait
|
||||
tx_queue[i].flow_control.state = ISOTP_WAIT;
|
||||
tx_queue[i].state = ISOTP_WAITING_FLOW_CONTROL;
|
||||
tx_queue[i].timestamp = isotp_can_get_time(); // Update timestamp for timeout handling
|
||||
tx_queue[i].timestamp = isotp_get_time(); // Update timestamp for timeout handling
|
||||
return ISOTP_OK;
|
||||
case 2: // Abort
|
||||
case ISOTP_FC_ABORT: // Abort
|
||||
tx_queue[i].flow_control.state = ISOTP_ABORT;
|
||||
tx_queue[i].state = ISOTP_FAILED;
|
||||
return ISOTP_OK;
|
||||
default:
|
||||
return ISOTP_NOT_FLOW_CONTROL;
|
||||
return ISOTP_INVALID_FRAME;
|
||||
}
|
||||
|
||||
tx_queue[i].flow_control.block_size = block_size;
|
||||
@ -253,3 +471,38 @@ isotp_status_t isotp_handle_flow_control(uint16_t id, const uint8_t * data, size
|
||||
return ISOTP_INVALID_PARAMETER;
|
||||
}
|
||||
|
||||
isotp_status_t isotp_handle_incoming(uint16_t id, const uint8_t * data, size_t datalen) {
|
||||
if (data == NULL) {
|
||||
return ISOTP_INVALID_PARAMETER;
|
||||
}
|
||||
|
||||
uint16_t dst_id = INVALID_CAN_ID;
|
||||
for (size_t i = 0; i < MAX_ISOTP_CONNECTIONS; i++) {
|
||||
if (connections[i].src_id == id) {
|
||||
dst_id = connections[i].dst_id;
|
||||
break;
|
||||
}
|
||||
}
|
||||
|
||||
if (dst_id == INVALID_CAN_ID) {
|
||||
return ISOTP_CONNECTION_NOT_FOUND;
|
||||
}
|
||||
|
||||
uint8_t type = data[0] & 0xF0;
|
||||
|
||||
switch (type) {
|
||||
#if ISOTP_RX
|
||||
case ISOTP_SINGLE_FRAME:
|
||||
return isotp_handle_single_frame(id, data, datalen);
|
||||
case ISOTP_FIRST_FRAME:
|
||||
return isotp_handle_first_frame(id, dst_id, data, datalen);
|
||||
case ISOTP_CONSECUTIVE_FRAME:
|
||||
return isotp_handle_consecutive_frame(id, dst_id, data, datalen);
|
||||
#endif
|
||||
case ISOTP_FLOW_CONTROL:
|
||||
return isotp_handle_flow_control(dst_id, data, datalen);
|
||||
default:
|
||||
return ISOTP_INVALID_FRAME;
|
||||
}
|
||||
}
|
||||
|
||||
|
||||
@ -5,18 +5,27 @@
|
||||
#include <stdint.h>
|
||||
|
||||
#define MAX_ISOTP_MESSAGES_IN_FLIGHT 10
|
||||
#define MAX_ISOTP_CONNECTIONS 10
|
||||
#define ISOTP_FC_WAIT_TIMEOUT 1000 // Timeout for flow control frames (ms)
|
||||
|
||||
#define ISOTP_RX false // enable RX support
|
||||
#define ISOTP_RX_TIMEOUT 1000 // Timeout for incoming messages (ms)
|
||||
#define ISOTP_RX_MIN_ST 0 // Minimum ST for flow control (ms)
|
||||
//#define ISOTP_PADDING 0xAA // optional padding byte
|
||||
|
||||
typedef enum : int16_t {
|
||||
typedef enum isotp_status {
|
||||
ISOTP_OK = 0,
|
||||
ISOTP_ERROR = -1,
|
||||
ISOTP_DATA_TOO_LONG = -2,
|
||||
ISOTP_INVALID_PARAMETER = -3,
|
||||
ISOTP_TOO_MANY_MESSAGES = -4,
|
||||
ISOTP_MESSAGE_ALREADY_IN_FLIGHT = -5,
|
||||
ISOTP_NOT_FLOW_CONTROL = -6,
|
||||
ISOTP_INVALID_FRAME = -6,
|
||||
ISOTP_CAN_TRANSMIT_ERROR = -7,
|
||||
ISOTP_TOO_MANY_CONNECTIONS = -8,
|
||||
ISOTP_CONNECTION_NOT_FOUND = -9,
|
||||
ISOTP_BUFFER_CREATION_ERROR = -10,
|
||||
ISOTP_INVALID_SEQUENCE = -11,
|
||||
} isotp_status_t;
|
||||
|
||||
static inline const char *isotp_status_to_string(isotp_status_t status) {
|
||||
@ -27,15 +36,21 @@ static inline const char *isotp_status_to_string(isotp_status_t status) {
|
||||
case ISOTP_INVALID_PARAMETER: return "ISOTP_INVALID_PARAMETER";
|
||||
case ISOTP_TOO_MANY_MESSAGES: return "ISOTP_TOO_MANY_MESSAGES";
|
||||
case ISOTP_MESSAGE_ALREADY_IN_FLIGHT: return "ISOTP_MESSAGE_ALREADY_IN_FLIGHT";
|
||||
case ISOTP_NOT_FLOW_CONTROL: return "ISOTP_NOT_FLOW_CONTROL";
|
||||
case ISOTP_INVALID_FRAME: return "ISOTP_INVALID_FRAME";
|
||||
case ISOTP_CAN_TRANSMIT_ERROR: return "ISOTP_CAN_TRANSMIT_ERROR";
|
||||
case ISOTP_TOO_MANY_CONNECTIONS: return "ISOTP_TOO_MANY_CONNECTIONS";
|
||||
case ISOTP_CONNECTION_NOT_FOUND: return "ISOTP_CONNECTION_NOT_FOUND";
|
||||
case ISOTP_BUFFER_CREATION_ERROR: return "ISOTP_BUFFER_CREATION_ERROR";
|
||||
case ISOTP_INVALID_SEQUENCE: return "ISOTP_INVALID_SEQUENCE";
|
||||
default: return "UNKNOWN_STATUS";
|
||||
}
|
||||
}
|
||||
|
||||
isotp_status_t isotp_init();
|
||||
[[gnu::access(read_only, 2, 3)]] isotp_status_t isotp_add_message(uint16_t id, const uint8_t *data, size_t datalen);
|
||||
isotp_status_t isotp_update();
|
||||
[[gnu::access(read_only, 2, 3)]] isotp_status_t isotp_handle_flow_control(uint16_t id, const uint8_t *data, size_t datalen);
|
||||
[[gnu::access(read_only, 2, 3)]] isotp_status_t isotp_handle_incoming(uint16_t id, const uint8_t *data, size_t datalen);
|
||||
isotp_status_t isotp_add_connection(uint16_t src_id, uint16_t dst_id);
|
||||
[[gnu::access(read_only, 2, 3)]] isotp_status_t isotp_add_message(uint16_t id, const uint8_t * data, size_t datalen);
|
||||
[[gnu::access(none, 2, 3)]] isotp_status_t isotp_try_add_message(uint16_t id, const uint8_t * data, size_t datalen);
|
||||
|
||||
#endif // ISOTP_H
|
||||
15
AMS_Master_Code/Core/Lib/isotp/isotp_user_defs.h
Normal file
15
AMS_Master_Code/Core/Lib/isotp/isotp_user_defs.h
Normal file
@ -0,0 +1,15 @@
|
||||
#pragma once
|
||||
#ifndef ISOTP_USER_DEFS_H
|
||||
#define ISOTP_USER_DEFS_H
|
||||
#include "isotp.h"
|
||||
|
||||
bool isotp_transmit(uint16_t id, const uint8_t * data, size_t len);
|
||||
uint32_t isotp_get_time();
|
||||
|
||||
#if ISOTP_RX
|
||||
void isotp_on_message_received(uint16_t id, const uint8_t * data, size_t len);
|
||||
void isotp_free_rx_buffer(void * buffer);
|
||||
[[gnu::malloc]] uint8_t * isotp_get_rx_buffer(uint16_t len);
|
||||
#endif
|
||||
|
||||
#endif // ISOTP_USER_DEFS_H
|
||||
@ -100,10 +100,13 @@ void isotp_log_backend_init(void) {
|
||||
/* Register the backend */
|
||||
isotp_backend_registered = log_register_backend(&isotp_backend);
|
||||
|
||||
/* Initialize the ISO-TP stack if registration was successful */
|
||||
if (isotp_backend_registered) {
|
||||
isotp_init();
|
||||
/* Set up ISO-TP connection */
|
||||
isotp_status_t status = isotp_add_connection(ISOTP_LOG_FC_CAN_ID, ISOTP_LOG_CAN_ID);
|
||||
if (status != ISOTP_OK) {
|
||||
log_error("Failed to add ISO-TP connection: %s", isotp_status_to_string(status));
|
||||
return;
|
||||
}
|
||||
log_info("ISO-TP backend initialized");
|
||||
}
|
||||
|
||||
/* ISO-TP backend initialization */
|
||||
|
||||
@ -9,9 +9,12 @@
|
||||
|
||||
|
||||
/* ISO-TP CAN ID configuration */
|
||||
#ifndef ISOTP_LOG_CAN_ID
|
||||
#ifndef ISOTP_LOG_CAN_ID //TEMPORARY!!!!!
|
||||
#define ISOTP_LOG_CAN_ID 0x123 // CAN ID to use for ISO-TP log messages
|
||||
#endif
|
||||
#ifndef ISOTP_LOG_FC_CAN_ID
|
||||
#define ISOTP_LOG_FC_CAN_ID 0x132 // CAN ID for flow control messages
|
||||
#endif
|
||||
|
||||
/* ISO-TP backend API */
|
||||
void isotp_log_backend_init(void);
|
||||
|
||||
@ -23,7 +23,7 @@ int16_t max_temp = INT16_MIN;
|
||||
typeof(module_temps) module_temps = {[0 ... N_BMS - 1] = {INT16_MAX, INT16_MIN}};
|
||||
float module_std_deviation[N_BMS] = {};
|
||||
|
||||
int16_t cellTemps[N_BMS][N_CELLS];
|
||||
int16_t cellTemps[N_BMS][10] = {};
|
||||
|
||||
static bool error_window[MAX_ERRORS_WINDOW_SIZE] = {};
|
||||
static size_t error_window_index = 0;
|
||||
|
||||
@ -1,7 +1,9 @@
|
||||
#include "can.h"
|
||||
|
||||
#include "ADBMS_Driver.h"
|
||||
#include "imd_monitoring.h"
|
||||
#include "isotp.h"
|
||||
#include "isotp_user_defs.h"
|
||||
#include "isotp_log_backend.h"
|
||||
#include "log.h"
|
||||
#include "main.h"
|
||||
@ -14,12 +16,13 @@
|
||||
|
||||
#include <math.h>
|
||||
#include <stdint.h>
|
||||
#include <string.h>
|
||||
|
||||
bool isotp_can_transmit(uint16_t id, const uint8_t *data, size_t datalen) {
|
||||
bool isotp_transmit(uint16_t id, const uint8_t *data, size_t datalen) {
|
||||
return ftcan_transmit(id, data, datalen) == HAL_OK;
|
||||
}
|
||||
|
||||
uint32_t isotp_can_get_time() {
|
||||
uint32_t isotp_get_time() {
|
||||
return HAL_GetTick();
|
||||
}
|
||||
|
||||
@ -54,6 +57,86 @@ HAL_StatusTypeDef can_send_status() {
|
||||
return ftcan_transmit(CAN_ID_AMS_SIGNALS, data, 1);
|
||||
}
|
||||
|
||||
HAL_StatusTypeDef can_send_details() {
|
||||
static uint8_t module_index = 0;
|
||||
static uint8_t data[103] = {}; //sizeof(Cell_Module) + 10 + 1
|
||||
auto module = &modules[module_index];
|
||||
auto data_ptr = &data[1];
|
||||
|
||||
isotp_status_t status = isotp_try_add_message(CAN_ID_AMS_DETAILS_FC, data, sizeof(data));
|
||||
switch (status) {
|
||||
case ISOTP_OK:
|
||||
break;
|
||||
case ISOTP_MESSAGE_ALREADY_IN_FLIGHT:
|
||||
return HAL_BUSY;
|
||||
default:
|
||||
log_warning("isotp_try_add_message failed: %s", isotp_status_to_string(status));
|
||||
return HAL_ERROR;
|
||||
}
|
||||
|
||||
data[0] = module_index;
|
||||
data_ptr = ftcan_marshal_unsigned(data_ptr, module->bmsID, 8);
|
||||
data_ptr = ftcan_marshal_unsigned(data_ptr, module->status.CS_FLT, 2);
|
||||
data_ptr = ftcan_marshal_unsigned(data_ptr, module->status.CCTS, 2);
|
||||
|
||||
// Marshal status bits into a single byte
|
||||
uint8_t status_bits = 0;
|
||||
status_bits |= module->status.SMED << 0;
|
||||
status_bits |= module->status.SED << 1;
|
||||
status_bits |= module->status.CMED << 2;
|
||||
status_bits |= module->status.CED << 3;
|
||||
status_bits |= module->status.VD_UV << 4;
|
||||
status_bits |= module->status.VD_OV << 5;
|
||||
status_bits |= module->status.VA_UV << 6;
|
||||
status_bits |= module->status.VA_OV << 7;
|
||||
*(data_ptr++) = status_bits;
|
||||
|
||||
// Marshal the rest of the status bits
|
||||
status_bits = 0;
|
||||
status_bits |= module->status.OSCCHK << 0;
|
||||
status_bits |= module->status.TMODCHK << 1;
|
||||
status_bits |= module->status.THSD << 2;
|
||||
status_bits |= module->status.SLEEP << 3;
|
||||
status_bits |= module->status.SPIFLT << 4;
|
||||
status_bits |= module->status.COMPARE << 5;
|
||||
status_bits |= module->status.VDE << 6;
|
||||
status_bits |= module->status.VDEL << 7;
|
||||
*(data_ptr++) = status_bits;
|
||||
|
||||
// Marshal voltage flags
|
||||
data_ptr = ftcan_marshal_unsigned(data_ptr, module->overVoltage, 4);
|
||||
data_ptr = ftcan_marshal_unsigned(data_ptr, module->underVoltage, 4);
|
||||
|
||||
// Marshal temperature and voltages
|
||||
data_ptr = ftcan_marshal_signed(data_ptr, module->internalDieTemp, 2);
|
||||
data_ptr = ftcan_marshal_unsigned(data_ptr, module->analogSupplyVoltage, 2);
|
||||
data_ptr = ftcan_marshal_unsigned(data_ptr, module->digitalSupplyVoltage, 2);
|
||||
data_ptr = ftcan_marshal_unsigned(data_ptr, module->refVoltage, 2);
|
||||
|
||||
// Marshal cell voltages
|
||||
for (int i = 0; i < 16; i++) {
|
||||
data_ptr = ftcan_marshal_signed(data_ptr, module->cellVoltages[i], 2);
|
||||
}
|
||||
|
||||
// Marshal auxiliary voltages
|
||||
for (int i = 0; i < 10; i++) {
|
||||
data_ptr = ftcan_marshal_signed(data_ptr, module->auxVoltages[i], 2);
|
||||
}
|
||||
|
||||
// Marshal temperature data
|
||||
for (int i = 0; i < 10; i++) {
|
||||
data_ptr = ftcan_marshal_signed(data_ptr, cellTemps[module_index][i], 2);
|
||||
}
|
||||
|
||||
if ((status = isotp_add_message(CAN_ID_AMS_DETAILS, data, sizeof(data))) != ISOTP_OK) {
|
||||
log_warning("isotp_add_message failed unexpectedly: %s", isotp_status_to_string(status));
|
||||
return HAL_ERROR;
|
||||
}
|
||||
|
||||
module_index = (module_index + 1) % N_BMS;
|
||||
return HAL_OK;
|
||||
}
|
||||
|
||||
HAL_StatusTypeDef can_send_error(TSErrorKind kind, uint8_t arg) {
|
||||
uint8_t data[2];
|
||||
data[0] = kind;
|
||||
@ -68,8 +151,8 @@ void ftcan_msg_received_cb(uint16_t id, size_t len, const uint8_t *data) {
|
||||
}
|
||||
|
||||
switch (id) {
|
||||
case CAN_ID_LOG_FC:
|
||||
auto status = isotp_handle_flow_control(ISOTP_LOG_CAN_ID, data, len);
|
||||
case ISOTP_LOG_FC_CAN_ID:
|
||||
auto status = isotp_handle_incoming(id, data, len);
|
||||
if (status != ISOTP_OK) {
|
||||
log_debug("Error when handling flow control: %s", isotp_status_to_string(status));
|
||||
}
|
||||
|
||||
@ -198,6 +198,8 @@ int main(void)
|
||||
|
||||
// init for master functions
|
||||
can_init(&hfdcan1);
|
||||
isotp_init();
|
||||
isotp_add_connection(CAN_ID_AMS_DETAILS_FC, CAN_ID_AMS_DETAILS);
|
||||
|
||||
// for testing. in the final code can log streaming will be enabled by can message
|
||||
isotp_log_enable_streaming(LOG_LEVEL_INFO);
|
||||
@ -246,6 +248,7 @@ int main(void)
|
||||
soc_update();
|
||||
imd_update();
|
||||
can_send_status();
|
||||
can_send_details();
|
||||
isotp_update();
|
||||
|
||||
loop_delay();
|
||||
|
||||
Loading…
x
Reference in New Issue
Block a user