85 lines
1.8 KiB
C
85 lines
1.8 KiB
C
/*
|
|
* ford_immo.c
|
|
*
|
|
* Created on: Nov 21, 2025
|
|
* Author: herli
|
|
*/
|
|
#include "ford_immo.h"
|
|
#include "timeouts.h"
|
|
|
|
|
|
uint16_t FIEONA_FIRSTWORD = 0x2001;
|
|
uint16_t FIEONA_SECONDWORD = 0x2000;
|
|
|
|
static uint16_t _immo_next2 = 0x4801; // predicted next rx = f(0x2001)
|
|
static uint16_t _rx_received = 0;
|
|
static uint8_t _rx_fresh = 0;
|
|
|
|
// --- Core step function: f(x) = u16(u16(x*x) + (x >> 2)) ---
|
|
|
|
#define IMMO_FIXUP 0x2001
|
|
|
|
static uint16_t immo_f(uint16_t x)
|
|
{
|
|
uint16_t sq = (uint16_t)(x * x);
|
|
return (uint16_t)(sq + (x >> 2));
|
|
}
|
|
|
|
// --- Advance: compute next tx and prediction ---
|
|
|
|
uint16_t FIEONA_advance()
|
|
{
|
|
uint16_t rx = _rx_fresh ? _rx_received : _immo_next2;
|
|
_rx_fresh = 0;
|
|
|
|
// Phase 1: tx = f(rx), fixup if < 2
|
|
uint16_t tx = immo_f(rx);
|
|
if (tx < 2) tx = IMMO_FIXUP;
|
|
|
|
// Phase 2: next2 = f(tx), blacklist check
|
|
uint16_t next2 = immo_f(tx);
|
|
if (next2 < 2 || next2 == 0x054B || next2 == 0x4656 || next2 == 0x88FC)
|
|
next2 = IMMO_FIXUP;
|
|
|
|
FIEONA_FIRSTWORD = tx;
|
|
FIEONA_SECONDWORD = tx & 0xFF00;
|
|
_immo_next2 = next2;
|
|
return tx;
|
|
}
|
|
|
|
// --- RX Handler ---
|
|
|
|
static uint16_t last_send4_tick = 0; // last tick for this timeout
|
|
extern uint8_t count;
|
|
|
|
void Fieona_SEND4_Handler(
|
|
const struct CanMessageDef *msg,
|
|
const uint8_t in_data[8],
|
|
CanTxFn tx
|
|
)
|
|
{
|
|
if(count > 1){
|
|
last_send4_tick = TIM16->CNT;
|
|
}
|
|
// Extract first word (little endian)
|
|
uint16_t rx_word = (uint16_t)in_data[0] | ((uint16_t)in_data[1] << 8);
|
|
|
|
// Desync check (firmware validate_rx_word_and_aux behavior)
|
|
if (rx_word != _immo_next2) {
|
|
if (rx_word != IMMO_FIXUP) {
|
|
rx_word = 0;
|
|
}
|
|
}
|
|
|
|
_rx_received = rx_word;
|
|
_rx_fresh = 1;
|
|
}
|
|
|
|
void FIEONA_Poll(){
|
|
if(last_send4_tick){
|
|
uint16_t newtick = last_send4_tick - 250;
|
|
Timeout_ResetByIndex(20, newtick); // deberia mejorarlo bastante esto, en vez de
|
|
last_send4_tick = 0;
|
|
}
|
|
}
|