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

Add second stage to frequency hopping

Browse Source
This commit is contained in:
jazzpi 2022-08-02 23:28:16 +02:00
parent 03080cd69a
commit 76427bdede
2 changed files with 81 additions and 26 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

13
Core/Inc/ClockSync.h
View File

@ -16,12 +16,15 @@
#define MASTER_HEARTBEAT_DESYNC_THRESH \ #define MASTER_HEARTBEAT_DESYNC_THRESH \
(2 * MASTER_HEARTBEAT_SANITY_INTERVAL_MAX) (2 * MASTER_HEARTBEAT_SANITY_INTERVAL_MAX)
#define FREQ_HOPPING_TRIM_STEPS 2 #define FREQ_HOPPING_TRIM_STEPS 2
#define FREQ_HOPPING_STAGE2_FRAMES 3
#define FREQ_HOPPING_STAGE2_MAX_ATTEMPTS 6
#define RCC_CR_HSITRIM_MAX 31 #define RCC_CR_HSITRIM_MAX 31
#define CLOCK_SYNC_MAX_TRANSMIT_ERRORS 64 #define CLOCK_SYNC_MAX_TRANSMIT_ERRORS 64
typedef enum { typedef enum {
CLOCK_SYNC_NORMAL_OPERATION, CLOCK_SYNC_NORMAL_OPERATION,
CLOCK_SYNC_FREQ_HOPPING CLOCK_SYNC_FREQ_HOPPING_STAGE1,
CLOCK_SYNC_FREQ_HOPPING_STAGE2
} ClockSyncState; } ClockSyncState;
extern ClockSyncState clock_sync_state; extern ClockSyncState clock_sync_state;
@ -29,9 +32,11 @@ extern ClockSyncState clock_sync_state;
void clock_sync_init(CAN_HandleTypeDef* can_handle); void clock_sync_init(CAN_HandleTypeDef* can_handle);
void clock_sync_update(); void clock_sync_update();
ClockSyncState clock_sync_update_normal_operation(); ClockSyncState clock_sync_update_normal_operation();
ClockSyncState clock_sync_update_freq_hopping(); ClockSyncState clock_sync_update_freq_hopping_stage1();
ClockSyncState clock_sync_update_freq_hopping_stage2();
void clock_sync_start_normal_operation(); void clock_sync_start_normal_operation();
void clock_sync_start_freq_hopping(); void clock_sync_start_freq_hopping_stage1();
void clock_sync_start_freq_hopping_stage2();
void clock_sync_handle_clock_sync_frame(uint8_t counter); void clock_sync_handle_clock_sync_frame(uint8_t counter);
void clock_sync_handle_master_heartbeat(); void clock_sync_handle_master_heartbeat();
void clock_sync_startup_check(); void clock_sync_startup_check();
@ -39,4 +44,6 @@ uint8_t get_hsi_trim();
void set_hsi_trim(uint8_t trim); void set_hsi_trim(uint8_t trim);
void trim_hsi_by(int32_t delta); void trim_hsi_by(int32_t delta);
uint8_t calculate_freq_hopping_trim(uint32_t freq_hopping_iteration);
#endif // INC_CLOCK_SYNC_H_ #endif // INC_CLOCK_SYNC_H_

94
Core/Src/ClockSync.c
View File

@ -8,13 +8,17 @@
#include <stdint.h> #include <stdint.h>
ClockSyncState clock_sync_state = CLOCK_SYNC_FREQ_HOPPING; ClockSyncState clock_sync_state = CLOCK_SYNC_FREQ_HOPPING_STAGE1;
static uint32_t last_clock_sync_frame_time = 0; static uint32_t last_clock_sync_frame_time = 0;
static uint32_t last_master_heartbeat_time = 0; static uint32_t last_master_heartbeat_time = 0;
static uint32_t master_heartbeat_counter = 0;
static uint32_t freq_hopping_start_trim = 0; static uint32_t freq_hopping_start_trim = 0;
static uint32_t freq_hopping_iteration = 0; static uint32_t freq_hopping_iteration = 0;
static uint32_t freq_hopping_stage2_start_time = 0;
static uint32_t freq_hopping_stage2_start_counter = 0;
static uint32_t freq_hopping_stage2_attempts = 0;
void clock_sync_update() { void clock_sync_update() {
ClockSyncState next_state; ClockSyncState next_state;
@ -22,12 +26,15 @@ void clock_sync_update() {
case CLOCK_SYNC_NORMAL_OPERATION: case CLOCK_SYNC_NORMAL_OPERATION:
next_state = clock_sync_update_normal_operation(); next_state = clock_sync_update_normal_operation();
break; break;
case CLOCK_SYNC_FREQ_HOPPING: case CLOCK_SYNC_FREQ_HOPPING_STAGE1:
next_state = clock_sync_update_freq_hopping(); next_state = clock_sync_update_freq_hopping_stage1();
break;
case CLOCK_SYNC_FREQ_HOPPING_STAGE2:
next_state = clock_sync_update_freq_hopping_stage2();
break; break;
default: default:
// Shouldn't ever happen? // Shouldn't ever happen?
next_state = CLOCK_SYNC_FREQ_HOPPING; next_state = CLOCK_SYNC_FREQ_HOPPING_STAGE1;
} }
if (next_state != clock_sync_state) { if (next_state != clock_sync_state) {
@ -35,8 +42,11 @@ void clock_sync_update() {
case CLOCK_SYNC_NORMAL_OPERATION: case CLOCK_SYNC_NORMAL_OPERATION:
clock_sync_start_normal_operation(); clock_sync_start_normal_operation();
break; break;
case CLOCK_SYNC_FREQ_HOPPING: case CLOCK_SYNC_FREQ_HOPPING_STAGE1:
clock_sync_start_freq_hopping(); clock_sync_start_freq_hopping_stage1();
break;
case CLOCK_SYNC_FREQ_HOPPING_STAGE2:
clock_sync_start_freq_hopping_stage2();
break; break;
} }
} }
@ -45,42 +55,39 @@ void clock_sync_update() {
void clock_sync_start_normal_operation() {} void clock_sync_start_normal_operation() {}
void clock_sync_start_freq_hopping() { void clock_sync_start_freq_hopping_stage1() {
freq_hopping_start_trim = get_hsi_trim(); freq_hopping_start_trim = get_hsi_trim();
freq_hopping_iteration = 0; freq_hopping_iteration = 0;
} }
void clock_sync_start_freq_hopping_stage2() {
freq_hopping_start_trim = get_hsi_trim();
freq_hopping_stage2_start_time = HAL_GetTick();
freq_hopping_stage2_start_counter = master_heartbeat_counter;
freq_hopping_stage2_attempts = 0;
}
ClockSyncState clock_sync_update_normal_operation() { ClockSyncState clock_sync_update_normal_operation() {
uint32_t now = HAL_GetTick(); uint32_t now = HAL_GetTick();
uint8_t transmit_errors = uint8_t transmit_errors =
(ams_can_handle->Instance->ESR & CAN_ESR_TEC_Msk) >> CAN_ESR_TEC_Pos; (ams_can_handle->Instance->ESR & CAN_ESR_TEC_Msk) >> CAN_ESR_TEC_Pos;
if (now - last_master_heartbeat_time > MASTER_HEARTBEAT_DESYNC_THRESH || if (now - last_master_heartbeat_time > MASTER_HEARTBEAT_DESYNC_THRESH ||
transmit_errors > CLOCK_SYNC_MAX_TRANSMIT_ERRORS) { transmit_errors > CLOCK_SYNC_MAX_TRANSMIT_ERRORS) {
return CLOCK_SYNC_FREQ_HOPPING; return CLOCK_SYNC_FREQ_HOPPING_STAGE1;
} }
return CLOCK_SYNC_NORMAL_OPERATION; return CLOCK_SYNC_NORMAL_OPERATION;
} }
ClockSyncState clock_sync_update_freq_hopping() { ClockSyncState clock_sync_update_freq_hopping_stage1() {
uint32_t now = HAL_GetTick(); uint32_t now = HAL_GetTick();
if (now - last_clock_sync_frame_time < CLOCK_SYNC_SANITY_INTERVAL_MAX) { if (now - last_clock_sync_frame_time < CLOCK_SYNC_SANITY_INTERVAL_MAX) {
// We've re-sync'd! // We are at least close to re-sync'ing, go to stage 2
return CLOCK_SYNC_NORMAL_OPERATION; return CLOCK_SYNC_FREQ_HOPPING_STAGE2;
} }
if (now - last_master_heartbeat_time > MASTER_HEARTBEAT_SANITY_INTERVAL_MAX) { if (now - last_master_heartbeat_time > MASTER_HEARTBEAT_SANITY_INTERVAL_MAX) {
int32_t trim_delta = (freq_hopping_iteration + 1) * FREQ_HOPPING_TRIM_STEPS; uint8_t new_trim = calculate_freq_hopping_trim(freq_hopping_iteration);
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); set_hsi_trim(new_trim);
freq_hopping_iteration++; freq_hopping_iteration++;
@ -90,7 +97,33 @@ ClockSyncState clock_sync_update_freq_hopping() {
freq_hopping_iteration = 0; freq_hopping_iteration = 0;
} }
} }
return CLOCK_SYNC_FREQ_HOPPING; return CLOCK_SYNC_FREQ_HOPPING_STAGE1;
}
ClockSyncState clock_sync_update_freq_hopping_stage2() {
if (master_heartbeat_counter - freq_hopping_stage2_start_counter >
FREQ_HOPPING_STAGE2_FRAMES) {
// We've re-sync'd!
return CLOCK_SYNC_NORMAL_OPERATION;
}
uint32_t now = HAL_GetTick();
if (now - freq_hopping_stage2_start_time >
FREQ_HOPPING_STAGE2_FRAMES * MASTER_HEARTBEAT_SANITY_INTERVAL_MAX) {
freq_hopping_stage2_attempts++;
if (freq_hopping_stage2_attempts > FREQ_HOPPING_STAGE2_MAX_ATTEMPTS) {
// Looks like we're not really close to sync'ing, go back to stage 1
return CLOCK_SYNC_FREQ_HOPPING_STAGE1;
}
// We haven't received all heartbeats, trim further
uint8_t new_trim =
calculate_freq_hopping_trim(freq_hopping_stage2_attempts);
set_hsi_trim(new_trim);
freq_hopping_stage2_start_counter = master_heartbeat_counter;
freq_hopping_stage2_start_time = now;
}
return CLOCK_SYNC_FREQ_HOPPING_STAGE2;
} }
void clock_sync_handle_clock_sync_frame(uint8_t counter) { void clock_sync_handle_clock_sync_frame(uint8_t counter) {
@ -142,6 +175,7 @@ void clock_sync_handle_clock_sync_frame(uint8_t counter) {
void clock_sync_handle_master_heartbeat() { void clock_sync_handle_master_heartbeat() {
last_master_heartbeat_time = HAL_GetTick(); last_master_heartbeat_time = HAL_GetTick();
master_heartbeat_counter++;
} }
uint8_t get_hsi_trim() { uint8_t get_hsi_trim() {
@ -167,3 +201,17 @@ void trim_hsi_by(int32_t delta) {
} }
set_hsi_trim(trim); set_hsi_trim(trim);
} }
uint8_t calculate_freq_hopping_trim(uint32_t freq_hopping_iteration) {
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;
}
}