FaSTTUBe/ams-slave-22
61
0
Fork 0
Code Issues Pull Requests Projects Releases Wiki Activity

Frequency hopping using master heartbeats

Browse Source
This commit is contained in:
jazzpi 2022-08-02 21:18:22 +02:00
parent 9fb5e145ad
commit 03080cd69a
6 changed files with 193 additions and 58 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

3
Core/Inc/AMS_CAN.h
View File

@ -18,9 +18,12 @@
#define CAN_ID_SLAVE_ERROR 0x001
#define CAN_ID_CLOCK_SYNC 0x002
#define CAN_ID_MASTER_HEARTBEAT 0x010
#define CAN_ID_AMS_SLAVE_HEARTBEAT_BASE 0x600
#define CAN_HEARTBEAT_TX_TIMEOUT 10 /* ms */
extern CAN_HandleTypeDef* ams_can_handle;
void ams_can_init(CAN_HandleTypeDef* ams, CAN_HandleTypeDef* car);
void ams_can_handle_ams_msg(CAN_RxHeaderTypeDef* header, uint8_t* data);

31
Core/Inc/ClockSync.h
View File

@ -1,17 +1,42 @@
#ifndef INC_CLOCK_SYNC_H_
#define INC_CLOCK_SYNC_H_
#include "stm32f4xx_hal.h"
#include "stm32f4xx_hal_can.h"
#include <stdint.h>
#define CLOCK_TARGET_FREQ 16000000 // Hz
#define HSI_TRIM_FREQ 48000 // Hz
#define CLOCK_SYNC_INTERVAL 1000 // ms
#define CLOCK_SYNC_SANITY_INTERVAL_MIN 900
#define CLOCK_SYNC_SANITY_INTERVAL_MAX 1100
#define MASTER_HEARTBEAT_INTERVAL 100 // ms
#define MASTER_HEARTBEAT_SANITY_INTERVAL_MAX 110
#define MASTER_HEARTBEAT_DESYNC_THRESH \
(2 * MASTER_HEARTBEAT_SANITY_INTERVAL_MAX)
#define FREQ_HOPPING_TRIM_STEPS 2
#define RCC_CR_HSITRIM_MAX 31
#define CLOCK_SYNC_MAX_TRANSMIT_ERRORS 64
#define STARTUP_CLOCK_SYNC_TIME 5000 // ms
typedef enum {
CLOCK_SYNC_NORMAL_OPERATION,
CLOCK_SYNC_FREQ_HOPPING
} ClockSyncState;
void clock_sync_handle_frame();
extern ClockSyncState clock_sync_state;
void clock_sync_init(CAN_HandleTypeDef* can_handle);
void clock_sync_update();
ClockSyncState clock_sync_update_normal_operation();
ClockSyncState clock_sync_update_freq_hopping();
void clock_sync_start_normal_operation();
void clock_sync_start_freq_hopping();
void clock_sync_handle_clock_sync_frame(uint8_t counter);
void clock_sync_handle_master_heartbeat();
void clock_sync_startup_check();
void trim_hsi(int32_t delta);
uint8_t get_hsi_trim();
void set_hsi_trim(uint8_t trim);
void trim_hsi_by(int32_t delta);
#endif // INC_CLOCK_SYNC_H_

47
Core/Src/AMS_CAN.c
View File

@ -19,16 +19,16 @@
#include <stdint.h>
static CAN_HandleTypeDef* handle_ams;
CAN_HandleTypeDef* ams_can_handle;
void ams_can_init(CAN_HandleTypeDef* ams_handle,
CAN_HandleTypeDef* car_handle) {
handle_ams = ams_handle;
ams_can_handle = ams_handle;
// Start peripheral
if (HAL_CAN_Start(handle_ams) != HAL_OK) {
handle_ams = car_handle;
if (HAL_CAN_Start(handle_ams) != HAL_OK) {
if (HAL_CAN_Start(ams_can_handle) != HAL_OK) {
ams_can_handle = car_handle;
if (HAL_CAN_Start(ams_can_handle) != HAL_OK) {
Error_Handler();
}
}
@ -48,18 +48,24 @@ void ams_can_init(CAN_HandleTypeDef* ams_handle,
can_filter.FilterScale = CAN_FILTERSCALE_32BIT;
// If we use CAN1, the slave filter should start after our filter bank. If we
// use CAN2, it should start at our filter bank.
if (handle_ams == ams_handle) {
can_filter.SlaveStartFilterBank = 1;
if (ams_can_handle == ams_handle) {
can_filter.SlaveStartFilterBank = 14;
} else {
can_filter.SlaveStartFilterBank = 0;
}
if (HAL_CAN_ConfigFilter(handle_ams, &can_filter) != HAL_OK) {
if (HAL_CAN_ConfigFilter(ams_can_handle, &can_filter) != HAL_OK) {
Error_Handler();
}
can_filter.FilterBank++;
can_filter.FilterIdHigh = CAN_ID_MASTER_HEARTBEAT << (16 - 11);
can_filter.FilterIdLow = 0;
if (HAL_CAN_ConfigFilter(ams_can_handle, &can_filter) != HAL_OK) {
Error_Handler();
}
// Activate RX notifications
if (HAL_CAN_ActivateNotification(handle_ams, CAN_IT_RX_FIFO0_MSG_PENDING) !=
HAL_OK) {
if (HAL_CAN_ActivateNotification(ams_can_handle,
CAN_IT_RX_FIFO0_MSG_PENDING) != HAL_OK) {
Error_Handler();
}
}
@ -75,7 +81,7 @@ void HAL_CAN_RxFifo0MsgPendingCallback(CAN_HandleTypeDef* handle) {
Error_Handler();
}
if (handle == handle_ams) {
if (handle == ams_can_handle) {
ams_can_handle_ams_msg(&header, data);
} else {
Error_Handler();
@ -89,7 +95,10 @@ void ams_can_handle_ams_msg(CAN_RxHeaderTypeDef* header, uint8_t* data) {
switch (header->StdId) {
case CAN_ID_CLOCK_SYNC:
clock_sync_handle_frame();
clock_sync_handle_clock_sync_frame(data[0]);
break;
case CAN_ID_MASTER_HEARTBEAT:
clock_sync_handle_master_heartbeat();
break;
}
}
@ -112,10 +121,10 @@ void ams_can_send_heartbeat() {
data[2 * i + 0] = v & 0xFF;
data[2 * i + 1] = v >> 8;
}
if (ams_can_wait_for_free_mailboxes(handle_ams, 1,
if (ams_can_wait_for_free_mailboxes(ams_can_handle, 1,
CAN_HEARTBEAT_TX_TIMEOUT) == HAL_OK) {
uint32_t mailbox;
HAL_CAN_AddTxMessage(handle_ams, &header, data, &mailbox);
HAL_CAN_AddTxMessage(ams_can_handle, &header, data, &mailbox);
}
}
@ -129,10 +138,10 @@ void ams_can_send_heartbeat() {
data[2 * i + 0] = temp & 0xFF;
data[2 * i + 1] = temp >> 8;
}
if (ams_can_wait_for_free_mailboxes(handle_ams, 1,
if (ams_can_wait_for_free_mailboxes(ams_can_handle, 1,
CAN_HEARTBEAT_TX_TIMEOUT) == HAL_OK) {
uint32_t mailbox;
HAL_CAN_AddTxMessage(handle_ams, &header, data, &mailbox);
HAL_CAN_AddTxMessage(ams_can_handle, &header, data, &mailbox);
}
}
}
@ -150,11 +159,11 @@ void ams_can_send_error(AMS_ErrorCode error_code,
data[0] = slave_id;
data[1] = error_code;
HAL_CAN_AbortTxRequest(handle_ams,
HAL_CAN_AbortTxRequest(ams_can_handle,
CAN_TX_MAILBOX0 | CAN_TX_MAILBOX1 | CAN_TX_MAILBOX2);
uint32_t mailbox;
HAL_CAN_AddTxMessage(handle_ams, &header, data, &mailbox);
ams_can_wait_for_free_mailboxes(handle_ams, 3, transmission_timeout);
HAL_CAN_AddTxMessage(ams_can_handle, &header, data, &mailbox);
ams_can_wait_for_free_mailboxes(ams_can_handle, 3, transmission_timeout);
}
HAL_StatusTypeDef ams_can_wait_for_free_mailboxes(CAN_HandleTypeDef* handle,

161
Core/Src/ClockSync.c
View File

@ -1,24 +1,125 @@
#include "ClockSync.h"
#include "AMS_CAN.h"
#include "stm32f412rx.h"
#include "stm32f4xx_hal.h"
#include "stm32f4xx_hal_can.h"
#include <stdint.h>
static uint32_t last_clock_sync_frame = 0;
ClockSyncState clock_sync_state = CLOCK_SYNC_FREQ_HOPPING;
void clock_sync_handle_frame() {
static uint32_t last_clock_sync_frame_time = 0;
static uint32_t last_master_heartbeat_time = 0;
static uint32_t freq_hopping_start_trim = 0;
static uint32_t freq_hopping_iteration = 0;
void clock_sync_update() {
ClockSyncState next_state;
switch (clock_sync_state) {
case CLOCK_SYNC_NORMAL_OPERATION:
next_state = clock_sync_update_normal_operation();
break;
case CLOCK_SYNC_FREQ_HOPPING:
next_state = clock_sync_update_freq_hopping();
break;
default:
// Shouldn't ever happen?
next_state = CLOCK_SYNC_FREQ_HOPPING;
}
if (next_state != clock_sync_state) {
switch (next_state) {
case CLOCK_SYNC_NORMAL_OPERATION:
clock_sync_start_normal_operation();
break;
case CLOCK_SYNC_FREQ_HOPPING:
clock_sync_start_freq_hopping();
break;
}
}
clock_sync_state = next_state;
}
void clock_sync_start_normal_operation() {}
void clock_sync_start_freq_hopping() {
freq_hopping_start_trim = get_hsi_trim();
freq_hopping_iteration = 0;
}
ClockSyncState clock_sync_update_normal_operation() {
uint32_t now = HAL_GetTick();
uint8_t transmit_errors =
(ams_can_handle->Instance->ESR & CAN_ESR_TEC_Msk) >> CAN_ESR_TEC_Pos;
if (now - last_master_heartbeat_time > MASTER_HEARTBEAT_DESYNC_THRESH ||
transmit_errors > CLOCK_SYNC_MAX_TRANSMIT_ERRORS) {
return CLOCK_SYNC_FREQ_HOPPING;
}
return CLOCK_SYNC_NORMAL_OPERATION;
}
ClockSyncState clock_sync_update_freq_hopping() {
uint32_t now = HAL_GetTick();
if (now - last_clock_sync_frame_time < CLOCK_SYNC_SANITY_INTERVAL_MAX) {
// We've re-sync'd!
return CLOCK_SYNC_NORMAL_OPERATION;
}
if (now - last_master_heartbeat_time > MASTER_HEARTBEAT_SANITY_INTERVAL_MAX) {
int32_t trim_delta = (freq_hopping_iteration + 1) * FREQ_HOPPING_TRIM_STEPS;
if (freq_hopping_iteration % 2 == 0) {
trim_delta = -trim_delta;
}
int32_t new_trim = freq_hopping_start_trim + trim_delta;
if (new_trim < 0) {
new_trim += RCC_CR_HSITRIM_MAX + 1;
} else if (new_trim > RCC_CR_HSITRIM_MAX) {
new_trim -= RCC_CR_HSITRIM_MAX + 1;
}
set_hsi_trim(new_trim);
freq_hopping_iteration++;
if ((freq_hopping_iteration + 1) * FREQ_HOPPING_TRIM_STEPS >
RCC_CR_HSITRIM_MAX) {
// The next delta would be too large, start again
freq_hopping_iteration = 0;
}
}
return CLOCK_SYNC_FREQ_HOPPING;
}
void clock_sync_handle_clock_sync_frame(uint8_t counter) {
static uint32_t f_pre_trim = CLOCK_TARGET_FREQ;
static int32_t trimmed_last_frame = 0;
static int32_t last_trim_delta = HSI_TRIM_FREQ;
static uint8_t last_clock_sync_frame_counter = 0;
uint32_t now = HAL_GetTick();
if (last_clock_sync_frame != 0) {
uint32_t n_measured = now - last_clock_sync_frame;
uint32_t n_measured = now - last_clock_sync_frame_time;
uint8_t expected_counter = last_clock_sync_frame_counter + 1;
/* Sanity checks:
* - Are we actually in normal operation mode?
* - Have we received a sync frame before?
* - Did the counter increment by one (mod 2^8)? I.e., did we miss a frame?
* - Is the measured time elapsed within feasible bounds?
*/
if (clock_sync_state == CLOCK_SYNC_NORMAL_OPERATION &&
last_clock_sync_frame_time != 0 && counter == expected_counter &&
n_measured >= CLOCK_SYNC_SANITY_INTERVAL_MIN &&
n_measured <= CLOCK_SYNC_SANITY_INTERVAL_MAX) {
uint32_t f_real = n_measured * (CLOCK_TARGET_FREQ / CLOCK_SYNC_INTERVAL);
if (trimmed_last_frame) {
last_trim_delta = ((int32_t)(f_pre_trim - f_real)) / trimmed_last_frame;
if (last_trim_delta < 0) {
// Update trim delta
last_trim_delta = f_pre_trim - f_real;
if (last_trim_delta == 0) {
last_trim_delta = HSI_TRIM_FREQ;
} else if (last_trim_delta < 0) {
last_trim_delta = -last_trim_delta;
}
trimmed_last_frame = 0;
@ -27,44 +128,42 @@ void clock_sync_handle_frame() {
int32_t delta_f = CLOCK_TARGET_FREQ - f_real;
int32_t delta_quants = delta_f / last_trim_delta;
if (delta_quants != 0) {
int32_t trim_delta = (now < STARTUP_CLOCK_SYNC_TIME) ? 2 : 1;
if (delta_quants < 0) {
trim_delta = -trim_delta;
}
trim_hsi(trim_delta);
// We were able to receive the frame, so we should be reasonably close. It
// should thus be enough to trim by -1 or 1.
int32_t trim_delta = (delta_quants < 0) ? -1 : 1;
trim_hsi_by(trim_delta);
f_pre_trim = f_real;
trimmed_last_frame = trim_delta;
trimmed_last_frame = 1;
}
}
last_clock_sync_frame = now;
last_clock_sync_frame_time = now;
last_clock_sync_frame_counter = counter;
}
void clock_sync_startup_check() {
static uint32_t last_startup_trim = 0;
uint32_t now = HAL_GetTick();
if (now - last_clock_sync_frame > CLOCK_SYNC_INTERVAL &&
now - last_startup_trim > CLOCK_SYNC_INTERVAL) {
// The slave is probably warm, which increases the clock frequency. If the
// frequency is too high, we won't receive any CAN frames anymore, so let's
// try trimming down a bit.
trim_hsi(-2);
last_startup_trim = now;
}
void clock_sync_handle_master_heartbeat() {
last_master_heartbeat_time = HAL_GetTick();
}
void trim_hsi(int32_t delta) {
uint8_t get_hsi_trim() {
return (RCC->CR & RCC_CR_HSITRIM_Msk) >> RCC_CR_HSITRIM_Pos;
}
void set_hsi_trim(uint8_t trim) {
uint32_t rcc_cr = RCC->CR;
// Clear current trim and overwrite with new trim
rcc_cr = (rcc_cr & ~RCC_CR_HSITRIM_Msk) |
((trim << RCC_CR_HSITRIM_Pos) & RCC_CR_HSITRIM_Msk);
RCC->CR = rcc_cr;
}
void trim_hsi_by(int32_t delta) {
// Determine current trim
int32_t trim = (rcc_cr & RCC_CR_HSITRIM_Msk) >> RCC_CR_HSITRIM_Pos;
int32_t trim = get_hsi_trim();
trim += delta;
if (trim > RCC_CR_HSITRIM_MAX) {
trim = RCC_CR_HSITRIM_MAX;
} else if (trim < 0) {
trim = 0;
}
// Clear current trim and overwrite with new trim
rcc_cr = (rcc_cr & ~RCC_CR_HSITRIM_Msk) |
((trim << RCC_CR_HSITRIM_Pos) & RCC_CR_HSITRIM_Msk);
RCC->CR = rcc_cr;
set_hsi_trim(trim);
}

7
Core/Src/main.c
View File

@ -215,11 +215,10 @@ int main(void) {
check_error_conditions();
}
fan_ctrl_update();
// Only start sending CAN frames once the clock is somewhat synchronized
if (HAL_GetTick() > STARTUP_CLOCK_SYNC_TIME) {
clock_sync_update();
// Only start sending CAN frames once the clock is synchronized
if (clock_sync_state == CLOCK_SYNC_NORMAL_OPERATION) {
ams_can_send_heartbeat();
} else {
clock_sync_startup_check();
}
delay_period();
}

2
Makefile
View File

@ -1,5 +1,5 @@
##########################################################################################################################
# File automatically-generated by tool: [projectgenerator] version: [3.17.1] date: [Sat Jul 30 20:29:35 CEST 2022]
# File automatically-generated by tool: [projectgenerator] version: [3.17.1] date: [Tue Aug 02 21:11:16 CEST 2022]
##########################################################################################################################
# ------------------------------------------------