diff --git a/apps/sbus-receiver/src/freertos-tasks.c b/apps/sbus-receiver/src/freertos-tasks.c index b4d9d703..4e0798a5 100644 --- a/apps/sbus-receiver/src/freertos-tasks.c +++ b/apps/sbus-receiver/src/freertos-tasks.c @@ -24,10 +24,10 @@ static TaskHandle_t steering_task; static StaticTask_t steering_task_buf; static StackType_t steering_task_stack[2 * configMINIMAL_STACK_SIZE]; -void sbus_read_and_steer(void *unused); -void send_steering_command(steering_command_t *command); +void sbus_read_and_steer(void* unused); +void send_steering_command(steering_command_t* command); -int handle_letter(const ck_folder_t *folder, const ck_letter_t *letter); +int handle_letter(const ck_folder_t* folder, const ck_letter_t* letter); void task_init(void) { uint8_t priority = LOWEST_TASK_PRIORITY; @@ -52,11 +52,16 @@ void task_init(void) { } } -void sbus_read_and_steer(void *unused) { +void sbus_read_and_steer(void* unused) { (void)unused; init_steering(); + // SBUS frequency is roughly one message every 8 ms, + // this gives a timeout of 80 ms. + const uint8_t frame_lost_threshold = 10; + uint8_t frame_lost_count = 0; + for (;;) { sbus_message_t message; steering_command_t steering_command = neutral_steering_command(); @@ -66,10 +71,17 @@ void sbus_read_and_steer(void *unused) { continue; } - // Failsafe usually triggers if many frames are lost in a row. - // Indicates heavy connection loss. Frame lost indicateds slight connection - // loss or issue with frame. - bool read_failed = message.failsafe_activated || message.frame_lost; + // Frame lost indicates slight connection loss or issue with frame. + if (message.frame_lost) { + frame_lost_count++; + } else { + frame_lost_count = 0; + } + + // Failsafe usually triggers if many frames are lost in a row. Indicates + // heavy connection loss. + bool read_failed = + message.failsafe_activated || (frame_lost_count > frame_lost_threshold); if (read_failed) { printf("Frame lost, sending neutral command.\r\n"); @@ -81,13 +93,13 @@ void sbus_read_and_steer(void *unused) { } } -void send_steering_command(steering_command_t *command) { +void send_steering_command(steering_command_t* command) { if (!command->steering_is_on || ck_get_action_mode() == CK_ACTION_MODE_FREEZE) { return; } - ck_data_t *ck_data = get_ck_data(); + ck_data_t* ck_data = get_ck_data(); memcpy(&ck_data->steering_page->lines[1], &command->steering_angle, sizeof(command->steering_angle)); @@ -104,12 +116,12 @@ void send_steering_command(steering_command_t *command) { } } -int handle_letter(const ck_folder_t *folder, const ck_letter_t *letter) { +int handle_letter(const ck_folder_t* folder, const ck_letter_t* letter) { if (ck_get_action_mode() == CK_ACTION_MODE_FREEZE) { return APP_OK; } - ck_data_t *ck_data = get_ck_data(); + ck_data_t* ck_data = get_ck_data(); if (folder->folder_no == ck_data->buzzer_sound_folder->folder_no) { return process_buzzer_sound_letter(letter); diff --git a/libs/stm32-common/src/spi-flash.c b/libs/stm32-common/src/spi-flash.c index 0eff6f85..149f157f 100644 --- a/libs/stm32-common/src/spi-flash.c +++ b/libs/stm32-common/src/spi-flash.c @@ -24,7 +24,7 @@ #define MAX_BLOCK_TIME_MS 100 // Helpers -static int program_page(uint32_t page_address, uint8_t *bytes, size_t size); +static int program_page(uint32_t page_address, uint8_t* bytes, size_t size); static int wait_until_ready(void); static int set_write_enable_flag(void); static void start_spi_cmd(void); @@ -37,7 +37,7 @@ int spi_flash_workaround_init(void) { uint8_t data = 0xFF; // NOLINT(*-magic-numbers) if (program_page(SPI_FLASH_SIZE - 1, &data, sizeof(data)) != APP_OK) { - printf("Program didn't work, trying erase instead"); + printf("Program didn't work, trying erase instead\r\n"); return erase(SPI_FLASH_SIZE - SPI_FLASH_SECTOR_SIZE); } return APP_OK; @@ -68,7 +68,7 @@ int erase(uint32_t sector_address) { } // Erase - common_peripherals_t *peripherals = get_common_peripherals(); + common_peripherals_t* peripherals = get_common_peripherals(); start_spi_cmd(); @@ -85,7 +85,7 @@ int erase(uint32_t sector_address) { return wait_until_ready(); } -int program(uint32_t address, uint8_t *bytes, size_t size) { +int program(uint32_t address, uint8_t* bytes, size_t size) { // Bounds check if (address + size > SPI_FLASH_SIZE) { printf("error: page out of bounds\r\n"); @@ -98,7 +98,7 @@ int program(uint32_t address, uint8_t *bytes, size_t size) { while (bytes_left > 0) { uint32_t page_address = address + bytes_written; - uint8_t *chunk = &bytes[bytes_written]; + uint8_t* chunk = &bytes[bytes_written]; size_t chunk_size = bytes_left; if (chunk_size > SPI_FLASH_PAGE_SIZE) { @@ -118,7 +118,7 @@ int program(uint32_t address, uint8_t *bytes, size_t size) { } // Can read whole flash using one command -int read(uint32_t address, uint8_t *data, size_t size) { +int read(uint32_t address, uint8_t* data, size_t size) { // Bounds check if (address + size > SPI_FLASH_SIZE) { printf("error: size out of bounds\r\n"); @@ -137,7 +137,7 @@ int read(uint32_t address, uint8_t *data, size_t size) { // NOLINTEND(*-magic-numbers) // Read data - common_peripherals_t *peripherals = get_common_peripherals(); + common_peripherals_t* peripherals = get_common_peripherals(); start_spi_cmd(); @@ -178,7 +178,7 @@ int read(uint32_t address, uint8_t *data, size_t size) { return APP_OK; } -static int program_page(uint32_t page_address, uint8_t *bytes, size_t size) { +static int program_page(uint32_t page_address, uint8_t* bytes, size_t size) { // Command consists of command number followed by page address in big // endian format. If an entire page should be programmed the last address byte // should be set to 0. @@ -199,7 +199,7 @@ static int program_page(uint32_t page_address, uint8_t *bytes, size_t size) { return APP_NOT_OK; } - common_peripherals_t *peripherals = get_common_peripherals(); + common_peripherals_t* peripherals = get_common_peripherals(); // Transmit command start_spi_cmd(); @@ -226,7 +226,7 @@ static int program_page(uint32_t page_address, uint8_t *bytes, size_t size) { } static int wait_until_ready(void) { - common_peripherals_t *peripherals = get_common_peripherals(); + common_peripherals_t* peripherals = get_common_peripherals(); uint8_t cmd = READ_SR1_COMMAND; uint8_t response = SR1_BUSY; @@ -252,7 +252,7 @@ static int wait_until_ready(void) { } static int set_write_enable_flag(void) { - common_peripherals_t *peripherals = get_common_peripherals(); + common_peripherals_t* peripherals = get_common_peripherals(); uint8_t cmd = WRITE_ENABLE_COMMAND; // Set write enable flag