269 lines
8.1 KiB
C
269 lines
8.1 KiB
C
/*
|
||
* can_manager.c
|
||
*
|
||
* Created on: Sep 16, 2025
|
||
* Author: herli
|
||
*/
|
||
#include "id.h"
|
||
#include "can_manager.h"
|
||
#include "can_db_symbols.h" // we’ll reference message defs here
|
||
#include "can_encode.h"
|
||
#include "timeouts.h"
|
||
#include "ford_immo.h"
|
||
#include <string.h>
|
||
|
||
static CanTxFn s_tx = NULL;
|
||
|
||
/* Handler registry keyed by message DEF, not by ID */
|
||
#ifndef CAN_MANAGER_MAX_HANDLERS
|
||
#define CAN_MANAGER_MAX_HANDLERS 8
|
||
#endif
|
||
typedef struct {
|
||
const CanMessageDef *msg;
|
||
CanRxHandler handler;
|
||
} _HandlerEntry;
|
||
|
||
static _HandlerEntry s_handlers[CAN_MANAGER_MAX_HANDLERS];
|
||
static uint8_t s_handlers_cnt = 0;
|
||
|
||
|
||
/* for request */
|
||
volatile uint16_t EMPF2_WORDS[4] = {0,0,0,0};
|
||
|
||
|
||
bool can_manager_register_handler_msg(const CanMessageDef *msg, CanRxHandler handler)
|
||
{
|
||
if (!msg || !handler) return false;
|
||
for (uint8_t i = 0; i < s_handlers_cnt; ++i) {
|
||
if (s_handlers[i].msg == msg) { s_handlers[i].handler = handler; return true; }
|
||
}
|
||
if (s_handlers_cnt >= CAN_MANAGER_MAX_HANDLERS) return false;
|
||
s_handlers[s_handlers_cnt++] = (_HandlerEntry){ msg, handler };
|
||
return true;
|
||
}
|
||
|
||
static CanRxHandler _find_override_by_msg(const CanMessageDef *msg)
|
||
{
|
||
for (uint8_t i = 0; i < s_handlers_cnt; ++i)
|
||
if (s_handlers[i].msg == msg) return s_handlers[i].handler;
|
||
return NULL;
|
||
}
|
||
|
||
void can_manager_init(CanTxFn tx_fn) { s_tx = tx_fn; }
|
||
|
||
|
||
void can_manager_boot(void)
|
||
{
|
||
if (!s_tx) return;
|
||
size_t n = 0;
|
||
const CanMessageDef* const* db = can_db_all_ptr(&n);
|
||
for (size_t i = 0; i < n; ++i) {
|
||
const CanMessageDef *m = db[i];
|
||
if (m->dir == CAN_DIR_TX && m->send_on_boot) {
|
||
uint8_t buf[8] = {0};
|
||
can_encode_message(m, buf);
|
||
s_tx(m->can_id, buf, m->dlc);
|
||
}
|
||
}
|
||
|
||
|
||
}
|
||
|
||
void can_manager_on_rx(uint16_t can_id, const uint8_t data[8], uint8_t dlc)
|
||
{
|
||
(void)dlc;
|
||
if (!s_tx) return;
|
||
|
||
const CanMessageDef *msg = can_db_find(can_id, CAN_DIR_RX);
|
||
if (!msg) return;
|
||
|
||
if (msg->symbol_count && msg->symbols)
|
||
can_decode_message(msg, data);
|
||
|
||
if(msg->can_id == MSG_ID_SEND1.can_id){ //si es el command, reseteo
|
||
Timeout_ResetByIndex(15, TIM16->CNT);
|
||
}
|
||
|
||
CanRxHandler h = msg->rx_handler ? msg->rx_handler : _find_override_by_msg(msg);
|
||
if (h) h(msg, data, s_tx);
|
||
}
|
||
|
||
bool can_manager_send_msg(const CanMessageDef *msg)
|
||
{
|
||
if (!s_tx || !msg || msg->dir != CAN_DIR_TX) return false;
|
||
uint8_t buf[8] = {0};
|
||
can_encode_message(msg, buf);
|
||
return s_tx(msg->can_id, buf, msg->dlc);
|
||
}
|
||
|
||
bool can_manager_send(uint16_t can_id)
|
||
{
|
||
const CanMessageDef *msg = can_db_find(can_id, CAN_DIR_TX);
|
||
if (!msg) return false;
|
||
return can_manager_send_msg(msg);
|
||
}
|
||
|
||
/* ---------- Startup and request trigger handlers (reply by DEF) ---------- */
|
||
static const CanMessageDef *s_startup_reply_msg = NULL; // -> MSG_ID_EMPF3
|
||
static const CanMessageDef *s_request_reply_msg = NULL; // -> MSG_ID_EMPF2
|
||
|
||
extern void Timeout_ResetByIndex(uint8_t idx, uint16_t now);
|
||
extern TIM_HandleTypeDef* htim16;
|
||
|
||
void can_manager_set_startup_reply_msg(const CanMessageDef *reply_msg)
|
||
{
|
||
s_startup_reply_msg = reply_msg;
|
||
}
|
||
uint8_t startup = 1;
|
||
|
||
/*
|
||
void can_manager_rx_startup_trigger(const CanMessageDef *msg, const uint8_t in[8], CanTxFn tx)
|
||
{
|
||
(void)msg; (void)in;
|
||
if (!tx || !s_startup_reply_msg) return;
|
||
|
||
if (startup) {
|
||
//Send by DEFINITION: use reply_msg’s ID & DLC and (if present) its tx_template
|
||
uint8_t buf[8] = {0};
|
||
can_encode_message(s_startup_reply_msg, buf); // template + symbols
|
||
tx(s_startup_reply_msg->can_id, buf, s_startup_reply_msg->dlc);
|
||
|
||
startup = (startup < 3) ? (startup + 1) : 0;
|
||
Timeout_ResetByIndex(11, (uint16_t)htim16->Instance->CNT);
|
||
}else{
|
||
//register for answering next messages, on end should also reregister for the boot sequence right??
|
||
can_manager_register_handler_msg(&MSG_STARTUP_TRIGGER_RX, can_manager_rx_boot_reply_always);
|
||
}
|
||
Timeout_ResetByIndex(15, (uint16_t)htim16->Instance->CNT);
|
||
}
|
||
*/
|
||
// Always send the boot message (EMPF3) as a reply, regardless of 'startup'
|
||
uint8_t count = 0;
|
||
|
||
uint8_t startup_finished = 0;
|
||
#if defined(T06301)
|
||
uint8_t startup_count = 9;
|
||
#elif defined(T06209) || defined(T06216) || defined(T06211)
|
||
uint8_t startup_count = 0;
|
||
uint8_t startup_sent_count = 0;
|
||
|
||
#else
|
||
#endif
|
||
extern int watchdog_started;
|
||
void can_manager_rx_boot_reply_always(const CanMessageDef *msg, const uint8_t in[8], CanTxFn tx)
|
||
{
|
||
(void)msg; (void)in;
|
||
if (!tx || !s_startup_reply_msg) return;
|
||
|
||
#if defined(T06301)
|
||
if(!watchdog_started){return;}
|
||
|
||
if(count < 2){
|
||
count++;
|
||
}
|
||
|
||
startup = 0;
|
||
startup_count = 0;
|
||
Timeout_ResetByIndex(18, TIM16->CNT); // first callback in 40 ms
|
||
Fieona_SEND3_Handler(in);
|
||
|
||
Timeout_StopByIndex(21); // first callback in 40 ms
|
||
if(count > 1){
|
||
Timeout_StartIfStopped(20, TIM16->CNT - 500);
|
||
//no deberia hacer na esto pq bootea started.
|
||
}
|
||
#elif defined(T06209) || defined(T06216) || defined(T06211)
|
||
if (!startup_sent_count){return;}
|
||
|
||
if(startup_finished){
|
||
startup_count++; //probar esto
|
||
startup_count %= 3;
|
||
if (!startup_count){
|
||
Timeout_ResetByIndex(18, TIM16->CNT); // re-arm for another 40ms
|
||
}
|
||
}else{
|
||
if(startup_count){
|
||
startup_count--;
|
||
}else{
|
||
startup_finished = 1;
|
||
}
|
||
}
|
||
#else
|
||
#endif
|
||
}
|
||
|
||
void can_manager_set_request_reply_msg(const CanMessageDef *reply_msg)
|
||
{
|
||
s_request_reply_msg = reply_msg; // set to &MSG_ID_EMPF2
|
||
}
|
||
|
||
volatile uint8_t s_empf2_pending = 0;
|
||
|
||
bool can_manager_empf2_has_pending(void) { return s_empf2_pending != 0; }
|
||
void can_manager_empf2_clear_pending(void) { s_empf2_pending = 0; }
|
||
|
||
void can_manager_rx_address_request(const CanMessageDef *msg, const uint8_t in[8], CanTxFn tx)
|
||
{
|
||
(void)msg; (void)tx;
|
||
|
||
// Parse 4 big-endian request words
|
||
uint16_t req[4];
|
||
for (int i = 0; i < 4; ++i)
|
||
req[i] = (uint16_t)((in[2*i] << 8) | in[2*i+1]);
|
||
|
||
for (int i = 0; i < 4; ++i) {
|
||
uint16_t a = req[i];
|
||
int32_t raw = 0; // signed accumulator before clamping
|
||
int found = 0;
|
||
|
||
for (size_t j = 0; j < CAN_ANSWERS_COUNT; ++j) {
|
||
const CanAddressEntry *E = &CAN_ANSWERS[j];
|
||
if (E->addr != a) continue;
|
||
found = 1;
|
||
|
||
if (E->storage == CAN_STORE_FLOAT) {
|
||
// physical float with optional scale
|
||
const float phys = *(const float*)E->ptr;
|
||
const float f = E->scale ? E->scale->factor : 1.0f;
|
||
const float o = E->scale ? E->scale->offset : 0.0f;
|
||
float scaled = (phys - o) / f;
|
||
//if (!isfinite(scaled)) scaled = 0.0f;
|
||
raw = (int32_t)llroundf(scaled);
|
||
} else {
|
||
// integer-backed: read as-is (no scaling)
|
||
switch (E->storage) {
|
||
case CAN_STORE_U32: raw = (int32_t)*(const uint32_t*)E->ptr; break;
|
||
case CAN_STORE_S32: raw = (int32_t)*(const int32_t *)E->ptr; break;
|
||
case CAN_STORE_U16: raw = (int32_t)*(const uint16_t*)E->ptr; break;
|
||
case CAN_STORE_S16: raw = (int32_t)*(const int16_t *)E->ptr; break;
|
||
case CAN_STORE_U8: raw = (int32_t)*(const uint8_t *)E->ptr; break;
|
||
case CAN_STORE_S8: raw = (int32_t)*(const int8_t *)E->ptr; break;
|
||
default: raw = 0; break;
|
||
}
|
||
}
|
||
|
||
// Clamp to 16-bit range according to raw_type
|
||
if (E->raw_type == CAN_SYM_SX) {
|
||
if (raw < -32768) raw = -32768;
|
||
if (raw > 32767) raw = 32767;
|
||
} else {
|
||
if (raw < 0) raw = 0;
|
||
if (raw > 65535) raw = 65535;
|
||
}
|
||
break;
|
||
}
|
||
|
||
uint16_t raw16 = (uint16_t)raw; // two's complement for signed values
|
||
// Pack little-endian into EMPF2_BYTES (shared TX buffer for MSG_ID_EMPF2)
|
||
EMPF2_BYTES[2*i + 0] = (uint8_t)(raw16 & 0xFF);
|
||
EMPF2_BYTES[2*i + 1] = (uint8_t)(raw16 >> 8);
|
||
}
|
||
|
||
// Mark pending and start 60ms window only if not already running
|
||
s_empf2_pending = 1;
|
||
Timeout_StartIfStopped(17, (uint16_t)TIM16->CNT);
|
||
}
|
||
|
||
|
||
|