From 8fbbf383ae7f070cf245f28073a448f0f502abb6 Mon Sep 17 00:00:00 2001 From: Isaak Devos Date: Thu, 23 May 2024 12:00:52 +0200 Subject: [PATCH] Project layout refactor --- software/MySrc/Common/cli.c | 165 ++++++++ software/MySrc/Common/cli.h | 39 ++ software/MySrc/Common/cli_cmd_callbacks.c | 178 ++++++++ software/MySrc/Common/cli_cmd_callbacks.h | 12 + software/MySrc/Common/log.c | 113 +++++ software/MySrc/Common/log.h | 49 +++ software/MySrc/Common/misc.c | 164 ++++++++ software/MySrc/Common/misc.h | 38 ++ software/MySrc/Common/version.h | 19 + software/MySrc/Drivers/BMI270_CONFIG.h | 448 ++++++++++++++++++++ software/MySrc/Drivers/crsf.c | 237 +++++++++++ software/MySrc/Drivers/crsf.h | 111 +++++ software/MySrc/Drivers/dshot.c | 244 +++++++++++ software/MySrc/Drivers/dshot.h | 88 ++++ software/MySrc/Drivers/gyro.c | 220 ++++++++++ software/MySrc/Drivers/gyro.h | 33 ++ software/MySrc/Drivers/imu.c | 476 ++++++++++++++++++++++ software/MySrc/Drivers/imu.h | 44 ++ software/MySrc/Drivers/spi.c | 249 +++++++++++ software/MySrc/Drivers/spi.h | 24 ++ software/MySrc/Drivers/spi_soft.c | 221 ++++++++++ software/MySrc/Drivers/spi_soft.h | 32 ++ software/MySrc/Flight/mixer.c | 36 ++ software/MySrc/Flight/mixer.h | 48 +++ software/MySrc/Flight/motors.c | 124 ++++++ software/MySrc/Flight/motors.h | 23 ++ software/MySrc/Flight/pid.c | 73 ++++ software/MySrc/Flight/pid.h | 53 +++ software/MySrc/Flight/pid_controller.c | 138 +++++++ software/MySrc/Flight/pid_controller.h | 36 ++ software/MySrc/Flight/set_point.c | 37 ++ software/MySrc/Flight/set_point.h | 29 ++ software/MySrc/mymain.c | 156 +++++++ software/MySrc/mymain.h | 22 + 34 files changed, 3979 insertions(+) create mode 100644 software/MySrc/Common/cli.c create mode 100644 software/MySrc/Common/cli.h create mode 100644 software/MySrc/Common/cli_cmd_callbacks.c create mode 100644 software/MySrc/Common/cli_cmd_callbacks.h create mode 100644 software/MySrc/Common/log.c create mode 100644 software/MySrc/Common/log.h create mode 100644 software/MySrc/Common/misc.c create mode 100644 software/MySrc/Common/misc.h create mode 100644 software/MySrc/Common/version.h create mode 100644 software/MySrc/Drivers/BMI270_CONFIG.h create mode 100644 software/MySrc/Drivers/crsf.c create mode 100644 software/MySrc/Drivers/crsf.h create mode 100644 software/MySrc/Drivers/dshot.c create mode 100644 software/MySrc/Drivers/dshot.h create mode 100644 software/MySrc/Drivers/gyro.c create mode 100644 software/MySrc/Drivers/gyro.h create mode 100644 software/MySrc/Drivers/imu.c create mode 100644 software/MySrc/Drivers/imu.h create mode 100644 software/MySrc/Drivers/spi.c create mode 100644 software/MySrc/Drivers/spi.h create mode 100644 software/MySrc/Drivers/spi_soft.c create mode 100644 software/MySrc/Drivers/spi_soft.h create mode 100644 software/MySrc/Flight/mixer.c create mode 100644 software/MySrc/Flight/mixer.h create mode 100644 software/MySrc/Flight/motors.c create mode 100644 software/MySrc/Flight/motors.h create mode 100644 software/MySrc/Flight/pid.c create mode 100644 software/MySrc/Flight/pid.h create mode 100644 software/MySrc/Flight/pid_controller.c create mode 100644 software/MySrc/Flight/pid_controller.h create mode 100644 software/MySrc/Flight/set_point.c create mode 100644 software/MySrc/Flight/set_point.h create mode 100644 software/MySrc/mymain.c create mode 100644 software/MySrc/mymain.h diff --git a/software/MySrc/Common/cli.c b/software/MySrc/Common/cli.c new file mode 100644 index 0000000..8ecc8d0 --- /dev/null +++ b/software/MySrc/Common/cli.c @@ -0,0 +1,165 @@ +/* + + This file is part of the CLI module. + It contains the functionallity to handle the CLI commands. + + You should NOT ADD COMMANDS HERE. Use cli_cmd_callbacks.c instead. + + */ + +#include "cli.h" + +#include + +#include "usbd_cdc_if.h" + +#include "log.h" +#include "version.h" +#include "misc.h" +#include "cli_cmd_callbacks.h" +#include "main.h" + +void cli_handle_cmd(cli_handle_t *cli_h); + +void cli_process(void *arg) { + if (arg == NULL) { + LOGE("cli_process: arg is NULL! Not processing cli... unsafe!"); + return; + } + cli_handle_t *cli_h = (cli_handle_t *)arg; + + // read data + if (cli_h->new_data_flag) { // if new data data in buffer + cli_h->new_data_flag = false; + LOG("-> %s", cli_h->cli_rx_buffer); + cli_handle_cmd(cli_h); + } + + //write data (from buffer) +} + +void cli_init(cli_handle_t *cli_h) { + LOGD("CLI Init..."); + + cli_h->cli_connected_flag = false; + + uint8_t version_string[10]; + VERSION_TO_STRING(CLI_VERSION, version_string); + LOGD("CLI is on version %s", version_string); + + //init the cmd_list array to NULL + for (size_t i = 0; i < MAX_CMD_COUNT; i++) { + cli_h->cmd_list[i].cmd_str = NULL; + } + + cli_h->new_data_flag = false; + + add_commands(cli_h); + + //wait for connection + if (cli_h->cli_connected_flag == false && cli_h->halt_until_connected_flag == true) { + LOGD("Waiting for connection..."); + while (cli_h->cli_connected_flag == false) { + LED_toggle(); + delay(100); + if (cli_h->new_data_flag == true) { + cli_h->new_data_flag = false; + + // check for "connect" cmd + if (strcmp_ign(cli_h->cli_rx_buffer, (uint8_t *)"connect") == 0) { + cli_h->cli_connected_flag = true; + LOGD("Connection confirmed"); + } + //check for "dfu" cmd + if (strcmp_ign(cli_h->cli_rx_buffer, (uint8_t *)"dfu") == 0) { + reboot_into_dfu(); + } + } + } + } +} + +void cli_handle_cmd(cli_handle_t *cli_h) { + // find the command index + uint8_t *raw_cmd_str = cli_h->cli_rx_buffer; + + // take first word as command + uint8_t *cmd_str = (uint8_t *)strtok((char *)raw_cmd_str, " "); + + //clean arg array + for (int i = 0; i < MAX_ARGS_COUNT; i++) { + cli_h->last_args[i] = (uint8_t*)""; + } + + // array to hold the arguments + uint8_t arg_count = 0; + + // get the rest of the arguments + while (arg_count < MAX_ARGS_COUNT) { + cli_h->last_args[arg_count] = (uint8_t *)strtok(NULL, " "); + if (cli_h->last_args[arg_count] == NULL) { + break; + } + clean_str(cli_h->last_args[arg_count]); // removes any \n or \r + arg_count++; + } + cli_h->last_args_count = arg_count; //save last count so cmd callbacks know how many args there are + + int8_t cmd_index = -1; + + for (int8_t i = 0; i < MAX_CMD_COUNT; i++) { + if (cli_h->cmd_list[i].cmd_str != NULL) { + if (strcmp_ign(cli_h->cmd_list[i].cmd_str, cmd_str) == 0) { + cmd_index = i; + break; + } + } + } + + if (cmd_index == -1) { + LOGE("The command you entered does not exist: %s, try again", cmd_str); + return; + } + + if (cli_h->cmd_list[cmd_index].cmd_callback != NULL) { + cli_h->cmd_list[cmd_index].cmd_callback(cli_h); + } else { + LOGE("The command you entered has no callback, it is most likely not implemented yet: %s", cmd_str); + } +} + + +void cli_add_cmd(cli_handle_t *cli_h, uint8_t *cmd_str, callback_t cmd_callback) { + //check if cmd_str is already an existing command + for (size_t i = 0; i < MAX_CMD_COUNT; i++) { + if (strcmp_ign(cmd_str, (uint8_t *)cli_h->cmd_list[i].cmd_str) == 0) { + LOGE((uint8_t *)"Command already exists, not adding... %s", cmd_str); + return; + } + } + + //find an empty slot in the command list + for (size_t i = 0; i < MAX_CMD_COUNT; i++) { + if (cli_h->cmd_list[i].cmd_str == NULL) { + cli_h->cmd_list[i].cmd_str = cmd_str; + cli_h->cmd_list[i].cmd_callback = cmd_callback; + + if (cmd_callback == NULL) { + LOGW((uint8_t *)"Command added without callback: %s", cmd_str); + return; + } + + LOGD((uint8_t *)"Command added: %s", cmd_str); + return; + } + } + + LOGE((uint8_t *)"No empty slot found for command %s, Increase MAX_CMD_COUNT", cmd_str); +} + +// cli_rx_callback gets called when cli_rx_buffer gets updated with new data +void cli_rx_callback(cli_handle_t* cli_h) { + cli_h->new_data_flag = true; + // don't copy data here... will break receive function. + //TODO: Add a true callback... +} \ No newline at end of file diff --git a/software/MySrc/Common/cli.h b/software/MySrc/Common/cli.h new file mode 100644 index 0000000..2ea1b26 --- /dev/null +++ b/software/MySrc/Common/cli.h @@ -0,0 +1,39 @@ +// +// Created by Isaak on 2/21/2024. +// + +#ifndef BETTERFLIGHT_CLI_H +#define BETTERFLIGHT_CLI_H + +#include "version.h" +#include "misc.h" + +#define MAX_CMD_COUNT 64 +#define MAX_ARGS_COUNT 16 + +typedef struct { + uint8_t *cmd_str; + callback_t cmd_callback; +}cli_cmd_t; + +typedef struct { + cli_cmd_t cmd_list[MAX_CMD_COUNT]; + version_t version; + uint8_t cli_rx_buffer[64]; + uint8_t cli_tx_buffer[64]; + bool new_data_flag; + bool cli_connected_flag; + bool halt_until_connected_flag; + uint8_t last_cmd_index; + uint8_t *last_args[MAX_ARGS_COUNT]; + uint8_t last_args_count; +} cli_handle_t; + +void cli_init(cli_handle_t *cli_h); +void cli_process(void *arg); + +void cli_rx_callback(cli_handle_t* cli_h); +void cli_add_cmd(cli_handle_t* cli_h, uint8_t *cmd_str, callback_t cmd_callback); + + +#endif //BETTERFLIGHT_CLI_H diff --git a/software/MySrc/Common/cli_cmd_callbacks.c b/software/MySrc/Common/cli_cmd_callbacks.c new file mode 100644 index 0000000..62536a5 --- /dev/null +++ b/software/MySrc/Common/cli_cmd_callbacks.c @@ -0,0 +1,178 @@ +/* + + This file is part of the CLI module. + It contains the callback functions that are called when a command is received. + + You have to be here and NOT IN CLI.C to add new commands + + To add a command: + 1. Add a new callback function + 2. Add the callback function and cli command string to the add_commands function (below cb's) +*/ + +#include "cli_cmd_callbacks.h" + +#include +#include + +#include "cli.h" +#include "log.h" +#include "version.h" +#include "misc.h" +#include "main.h" +#include "mymain.h" +#include "motors.h" + + +void cli_cb_status(cli_handle_t *cli_h) { + LOGI("Status: OK"); +} + +void cli_cb_help(cli_handle_t *cli_h) { + LOG("[RSP] Available commands:"); + for (uint8_t i = 0; i < MAX_CMD_COUNT; i++) { + if (cli_h->cmd_list[i].cmd_str != NULL) { + LOG("%s", cli_h->cmd_list[i].cmd_str); + } + } +} + +void cli_cb_dfu(cli_handle_t *cli_h) { + LOGI("Rebooting into DFU mode..."); + reboot_into_dfu(); +} + +void cli_cb_reboot(cli_handle_t *cli_h) { + LOGI("Rebooting..."); + HAL_Delay(100); //wait for msg to be sent + NVIC_SystemReset(); +} + +void cli_cb_version(cli_handle_t *cli_h) { + uint8_t version_string[10]; + VERSION_TO_STRING(CLI_VERSION, version_string); + LOG("[RSP] %s", version_string); +} + +void cli_cb_connect(cli_handle_t *cli_h) { + LOG((uint8_t *) "[RSP] Confirm Connection"); + cli_h->cli_connected_flag = true; +} + +void cli_cb_disconnect(cli_handle_t *cli_h) { + LOG((uint8_t *) "[RSP] Confirm Disconnection"); + cli_h->cli_connected_flag = false; +} + +void cli_cb_clidemo(cli_handle_t *cli_h) { + LOGD("This is a Debug message"); + LOGI("This is an Info message"); + LOGW("This is a Warning message"); + LOGE("This is an Error message"); + LOG("This is a normal message (should not be used...)"); + LOG("[RSP] This is a response message to a cli command"); +} + +callback_t cli_cb_none(cli_handle_t *cli_h) { + LOGI("Command not implemented yet"); +} + +// --- MOTORS --- // + +void cli_cb_set_motor(cli_handle_t *cli_h) { + LOG("[RSP] Setting motor %s to %s", (char*)cli_h->last_args[0], (char*)cli_h->last_args[1]); + + if (cli_h->last_args_count != 2) { + LOGE("Invalid number of arguments, 2 expected"); + return; + } + + uint8_t motor_nr = char_to_uint16((char*)cli_h->last_args[0]); + if (motor_nr < 1 || motor_nr > 4) { + LOGE("Invalid motor number, 1 to 4 expected"); + return; + } + + uint16_t motor_value = char_to_uint16((char*)cli_h->last_args[1]); + motor_set_throttle(&motors_h, motor_nr, motor_value); +} + +void cli_cb_set_motors(cli_handle_t *cli_h) { + uint16_t motor_values[4]; + if (cli_h->last_args_count == 1) { + for (uint8_t i = 0; i < 4; i++) { + motor_values[i] = (char_to_uint16((char*)cli_h->last_args[0])); + } + } else if (cli_h->last_args_count == 4) { + for (uint8_t i = 0; i < 4; ++i) { + motor_values[i] = (char_to_uint16((char*)cli_h->last_args[i])); + delay(1); + } + } else { + LOGE("Invalid number of arguments, 1 to set all motors to the same value, 4 to set each motor individually!"); + return; + } + + LOG("[RSP] Set Motor valuese to: m1: %d m2: %d m3: %d m4: %d", motor_values[0], motor_values[1], motor_values[2], motor_values[3]); + motors_set_throttle_arr(&motors_h, motor_values); +} + + +void cli_cb_stop_motors(cli_handle_t *cli_h) { + motors_stop(&motors_h); + LOG("[RSP] Motors stopped"); +} + +void cli_cb_beep_motors(cli_handle_t *cli_h) { + motors_beep(&motors_h); + LOG("[RSP] Motors beeped"); +} + +void cli_cb_set_motor_direction(cli_handle_t *cli_h) { + LOG("[RSP] Setting motor (%s) direction to %s", (char*)cli_h->last_args[0], (char*)cli_h->last_args[1]); + + if (cli_h->last_args_count != 2) { + LOGE("Invalid number of arguments, 2 expected"); + return; + } + + uint8_t motor_nr = char_to_uint16((char*)cli_h->last_args[0]); + if (motor_nr < 1 || motor_nr > 4) { + LOGE("Invalid motor number, 1 to 4 expected"); + return; + } + + bool motor_dir = char_to_uint16((char*)cli_h->last_args[1]); + if (motor_dir != 0 && motor_dir != 1) { + LOGE("Invalid motor direction, 0 or 1 expected"); + return; + } + + motor_set_direction(&motors_h, motor_nr, motor_dir); +} + +// --- EOF MOTORS --- // + +void cli_cb_sendsc(cli_handle_t *cli_h) { + LOG("[RSP] Sending motor %d cmd %d" , char_to_uint16((char*)cli_h->last_args[0]), char_to_uint16((char*)cli_h->last_args[1])); + dshot_send_special_command(motors_h.dshot_hs[char_to_uint16((char*)cli_h->last_args[0]) - 1], char_to_uint16((char*)cli_h->last_args[1])); +} + + +void add_commands(cli_handle_t *cli_h) { + cli_add_cmd(cli_h, (uint8_t *)"help", (callback_t) cli_cb_help); + cli_add_cmd(cli_h, (uint8_t *)"status", (callback_t) cli_cb_status); + cli_add_cmd(cli_h, (uint8_t *)"version", (callback_t) cli_cb_version); + cli_add_cmd(cli_h, (uint8_t *)"connect", (callback_t) cli_cb_connect); + cli_add_cmd(cli_h, (uint8_t *)"save", NULL); + cli_add_cmd(cli_h, (uint8_t *)"dfu", (callback_t) cli_cb_dfu); + cli_add_cmd(cli_h, (uint8_t *)"reboot", (callback_t) cli_cb_reboot); + cli_add_cmd(cli_h, (uint8_t *)"disconnect", (callback_t) cli_cb_disconnect); + cli_add_cmd(cli_h, (uint8_t *)"demo", (callback_t) cli_cb_clidemo); + cli_add_cmd(cli_h, (uint8_t *)"setmotor", (callback_t) cli_cb_set_motor); + cli_add_cmd(cli_h, (uint8_t *)"setmotors", (callback_t) cli_cb_set_motors); + cli_add_cmd(cli_h, (uint8_t *)"stopmotors", (callback_t) cli_cb_stop_motors); + cli_add_cmd(cli_h, (uint8_t *)"beepmotors", (callback_t) cli_cb_beep_motors); + cli_add_cmd(cli_h, (uint8_t *)"setmotordir", (callback_t) cli_cb_set_motor_direction); + cli_add_cmd(cli_h, (uint8_t *)"sendsc", (callback_t) cli_cb_sendsc); +} diff --git a/software/MySrc/Common/cli_cmd_callbacks.h b/software/MySrc/Common/cli_cmd_callbacks.h new file mode 100644 index 0000000..3e46555 --- /dev/null +++ b/software/MySrc/Common/cli_cmd_callbacks.h @@ -0,0 +1,12 @@ +// +// Created by Isaak on 2/27/2024. +// + +#ifndef BETTERFLIGHT_CLI_CMD_CALLBACKS_H +#define BETTERFLIGHT_CLI_CMD_CALLBACKS_H + +#include "cli.h" + +void add_commands(cli_handle_t *cli_h); + +#endif //BETTERFLIGHT_CLI_CMD_CALLBACKS_H diff --git a/software/MySrc/Common/log.c b/software/MySrc/Common/log.c new file mode 100644 index 0000000..8ae0fe8 --- /dev/null +++ b/software/MySrc/Common/log.c @@ -0,0 +1,113 @@ +// +// Created by Isaak on 2/20/2024. +// +#include "log.h" + +#include +#include +#include +#include + +#include "misc.h" + +log_handle_t logging; + +void log_init(log_level_t log_level, bool color) { + logging.level = log_level; + logging.color = color; +} + +log_level_t get_log_level() { + return logging.level; +} + +uint8_t * get_log_level_color_attr(log_level_t log_level) { + if (logging.color) { + switch (log_level) { + case LOG_LEVEL_DEBUG: + (uint8_t*)ANSI_COLOR(COLOR_DEBUG); + case LOG_LEVEL_INFO: + (uint8_t*)ANSI_COLOR(COLOR_INFO); + case LOG_LEVEL_WARN: + (uint8_t*)ANSI_COLOR(COLOR_WARN); + case LOG_LEVEL_ERROR: + (uint8_t*)ANSI_COLOR(COLOR_ERROR); + default: + return (uint8_t*)""; + } + } else { + return (uint8_t*)""; + } +} + +void log_msg(log_level_t log_level, const uint8_t *format, va_list args) { + if (log_level < logging.level) { + return; + } + + switch (log_level) { + case LOG_LEVEL_DEBUG: + printf("[DEBUG] "); + break; + case LOG_LEVEL_INFO: + printf("[INFO] "); + break; + case LOG_LEVEL_WARN: + printf("[WARN] "); + break; + case LOG_LEVEL_ERROR: + printf("[ERR] "); + break; + default: + break; + } + vprintf((char*)format, args); + printf("\n"); +} + +void LOGD(const uint8_t *format, ...) { + va_list args; + va_start(args, format); + log_msg(LOG_LEVEL_DEBUG, format, args); + va_end(args); +} + +void LOGI(const uint8_t *format, ...) { + va_list args; + va_start(args, format); + log_msg(LOG_LEVEL_INFO, format, args); + va_end(args); +} + +void LOGW(const uint8_t *format, ...) { + va_list args; + va_start(args, format); + log_msg(LOG_LEVEL_WARN, format, args); + va_end(args); +} + +void LOGE(const uint8_t *format, ...) { + va_list args; + va_start(args, format); + log_msg(LOG_LEVEL_ERROR, format, args); + va_end(args); +} + +void LOG( const uint8_t *format, ...) { + va_list args; + va_start(args, format); + + vprintf((char*)format, args); + printf("\r\n"); + + va_end(args); +} + +// for debugging ascii buffers / strings +void LOG_ascii_hex_dump(uint8_t *data) { + printf("[DEBUG] ASCII HEX DUMP: "); + for (int i = 0; i < strlen((char*)data); i++) { + printf("%02X ", data[i]); + } + printf("\n"); +} \ No newline at end of file diff --git a/software/MySrc/Common/log.h b/software/MySrc/Common/log.h new file mode 100644 index 0000000..e1c1734 --- /dev/null +++ b/software/MySrc/Common/log.h @@ -0,0 +1,49 @@ +// +// Created by Isaak on 2/20/2024. +// + +#ifndef BETTERFLIGHT_LOG_H +#define BETTERFLIGHT_LOG_H + +#define ANSI_COLOR(code) ((code) == 0 ? "" : "\033[" #code "m") +#include "misc.h" +#include + +typedef enum { + LOG_LEVEL_DEBUG = 0, + LOG_LEVEL_INFO, + LOG_LEVEL_WARN, + LOG_LEVEL_ERROR +} log_level_t; + +typedef struct { + log_level_t level; + bool color; +} log_handle_t; + +typedef enum { + COLOR_RESET = 0, + COLOR_RED = 31, + COLOR_GREEN = 32, + COLOR_YELLOW = 33, + COLOR_BLUE = 34, + COLOR_MAGENTA = 35, + COLOR_CYAN = 36, + COLOR_WHITE = 37, + COLOR_DEBUG = COLOR_CYAN, + COLOR_INFO = COLOR_GREEN, + COLOR_WARN = COLOR_YELLOW, + COLOR_ERROR = COLOR_RED, + COLOR_HIGHLIGHT = COLOR_MAGENTA +} type_color_t; + +void log_init(log_level_t log_level, bool color); +log_level_t get_log_level(); +void LOGD(const uint8_t *format, ...); +void LOGI(const uint8_t *format, ...); +void LOGW(const uint8_t *format, ...); +void LOGE(const uint8_t *format, ...); +void LOG(const uint8_t *format, ...); +void LOG_ascii_hex_dump(uint8_t *data); + +#endif //BETTERFLIGHT_LOG_H diff --git a/software/MySrc/Common/misc.c b/software/MySrc/Common/misc.c new file mode 100644 index 0000000..d5b2375 --- /dev/null +++ b/software/MySrc/Common/misc.c @@ -0,0 +1,164 @@ +// +// Created by Isaak on 2/10/2024. +// + + +#include "misc.h" + +#include +#include +#include +#include +#include + + +#include "main.h" +#include "log.h" + + +volatile uint32_t millis = 0; + +void LED_toggle(void) +{ + HAL_GPIO_TogglePin(LED1_GPIO_Port, LED1_Pin); +} + +void LED_on(void) +{ + HAL_GPIO_WritePin(LED1_GPIO_Port, LED1_Pin, GPIO_PIN_RESET); +} + +void LED_off(void) +{ + HAL_GPIO_WritePin(LED1_GPIO_Port, LED1_Pin, GPIO_PIN_SET); +} + +void LED_set(bool state) +{ + if (state) + LED_on(); + else + LED_off(); +} + +// TODO: potentially buggy, but works for now (I think it reverses the order of the pattern) +/* LED_blink_pattern blinks the LED in a given pattern, saves space (lines of code) */ +void LED_blink_pattern(uint8_t count, int num_durations, ...) { + va_list durations; + va_start(durations, num_durations); + uint16_t * duration_array = (uint16_t*)malloc(num_durations * sizeof(uint16_t)); + if (duration_array == NULL) { + LOGE("Failed to allocate memory for duration_array"); + return; + } + for (uint8_t i = 0; i < num_durations; i++) { + duration_array[i] = va_arg(durations, int); + } + va_end(durations); + LED_off(); // turn it off so first blink is on + for (uint8_t i = 0; i < count; i++) { + for (uint8_t j = 0; j < (num_durations - 1); j++) { + LED_toggle(); + HAL_Delay(duration_array[j]); + } + } + LED_off(); // for safety... + free(duration_array); +} + +// none_blocking_delay has to be polled, and calls the given function (without parameters) when x ms have passed. +void none_blocking_delay(uint32_t ms, uint64_t *last_millis, callback_t callback, void *arg) { + if (*last_millis == 0) { // if not set, set last_millis + *last_millis = millis; + } else if ((millis - *last_millis) >= ms) { // if enough time has passed + if (callback != NULL) { // if callback is valid + callback(arg); + } + *last_millis = 0; // reset last_millis + } +} + + +void clean_str(uint8_t *str) { + uint8_t *src, *dst; + for (src = dst = str; *src != '\0'; src++) { + if (*src != '\n' && *src != '\r') { + *dst++ = *src; + } + } + *dst = '\0'; +} + +// strcmp_ign (ign -> ignore) compares two strings, ignoring newlines and carriage returns +int8_t strcmp_ign(const uint8_t *str1, const uint8_t *str2) { + // make copy to avoid modifying original strings + uint8_t *copy1 = (uint8_t *)strdup((char *)str1); + uint8_t *copy2 = (uint8_t *)strdup((char *)str2); + + // remove newlines and carriage returns + clean_str(copy1); + clean_str(copy2); + + int8_t result = (int8_t)strcmp((char *)copy1, (char *)copy2); + + free(copy1); + free(copy2); + + return result; +} + +/* will write DEADBEEE on a known location in RAM, and reset the MCU */ +/* it requires you to have code in the startup file to check for this magic data and if present goto the bootloader */ +void reboot_into_dfu() { + *((uint32_t *)0x20000000) = 0xDEADBEEE; // magic data that is checked for in startup code + NVIC_SystemReset(); // reset mcu +} + +void delay(uint32_t ms) { + HAL_Delay(ms); +} + +// not working +void delay_us(uint32_t us) { + uint32_t mcu_clock_speed = get_mcu_clock_speed(); + uint32_t startTick = SysTick->VAL; // current tick count + uint32_t ticks = (mcu_clock_speed / 1000000) * us; //tick count for us delay + while ((SysTick->VAL - startTick) < ticks); + LOGD("get_mcu_clock_speed: %d", get_mcu_clock_speed()); + delay(1); + LOGD("ticks: %d", ticks); + delay(1); + LOGD("startTick: %d", startTick); + delay(1); + LOGD("end tick: %d", SysTick->VAL); +} + +uint16_t char_to_uint16(char *str) { + char *endptr; + unsigned long num = strtoul(str, &endptr, 10); + + // Check for conversion errors + if (endptr == str || *endptr != '\0' || errno == ERANGE || num > UINT16_MAX) { + return 0; // Return 0 if conversion failed or number is out of range + } + + return (uint16_t)num; +} + +uint32_t get_mcu_clock_speed(void) { + SystemCoreClockUpdate(); + return SystemCoreClock; +} + +// takes an 16 bit intiger and returns a string with the binary representation +uint8_t* byte_to_binary_str(uint16_t x) { + static uint8_t b[17]; + // Ensure b is at least 17 characters long + b[16] = '\0'; + + for (int i = 15; i >= 0; i--) { + b[15 - i] = (x & (1 << i)) ? '1' : '0'; + } + + return b; +} diff --git a/software/MySrc/Common/misc.h b/software/MySrc/Common/misc.h new file mode 100644 index 0000000..12cee2b --- /dev/null +++ b/software/MySrc/Common/misc.h @@ -0,0 +1,38 @@ +// +// Created by Isaak on 2/10/2024. +// + +#ifndef BETTERFLIGHT_MISC_H +#define BETTERFLIGHT_MISC_H + +#include +#include +#include + +typedef void (*callback_t)(void *arg); + +extern volatile uint32_t millis; + +void LED_toggle(void); +void LED_on(void); +void LED_off(void); +void LED_set(bool state); +void LED_blink_pattern(uint8_t count, int args_count, ...); + +void none_blocking_delay(uint32_t ms, uint64_t *last_millis ,callback_t callback, void *arg); + +void clean_str(uint8_t *str); +int8_t strcmp_ign(const uint8_t *str1, const uint8_t *str2); + +void reboot_into_dfu(); + +void delay(uint32_t ms); +void delay_us(uint32_t us); + +uint16_t char_to_uint16(char *str); + +uint32_t get_mcu_clock_speed(void); + +uint8_t *byte_to_binary_str(uint16_t x); + +#endif //BETTERFLIGHT_MISC_H diff --git a/software/MySrc/Common/version.h b/software/MySrc/Common/version.h new file mode 100644 index 0000000..afb37a9 --- /dev/null +++ b/software/MySrc/Common/version.h @@ -0,0 +1,19 @@ +// +// Created by Isaak on 2/21/2024. +// + +#ifndef BETTERFLIGHT_VERSION_H +#define BETTERFLIGHT_VERSION_H + +typedef struct { + int major; + int minor; + int patch; +} version_t; + +#define VERSION_TO_STRING(version, version_string) sprintf((char*)version_string, "%d.%d.%d", version.major, version.minor, version.patch) + +#define CLI_VERSION (version_t){0, 1, 3} +#define FW_VERSION (version_t){0, 1, 0} + +#endif //BETTERFLIGHT_VERSION_H diff --git a/software/MySrc/Drivers/BMI270_CONFIG.h b/software/MySrc/Drivers/BMI270_CONFIG.h new file mode 100644 index 0000000..495e21b --- /dev/null +++ b/software/MySrc/Drivers/BMI270_CONFIG.h @@ -0,0 +1,448 @@ +// +// Created by Isaak on 1/3/2024. +// + +#ifndef BETTERFLIGHT_BMI270_CONFIG_H +#define BETTERFLIGHT_BMI270_CONFIG_H + +#include + +#define BMI270_CONFIG_ARRAY_SIZE 8192 + +const uint8_t BMI_270_CONF_DATA_ARRAY[] = { + 0xc8, 0x2e, 0x00, 0x2e, 0x80, 0x2e, 0x3d, 0xb1, 0xc8, 0x2e, 0x00, 0x2e, 0x80, 0x2e, 0x91, 0x03, 0x80, 0x2e, 0xbc, + 0xb0, 0x80, 0x2e, 0xa3, 0x03, 0xc8, 0x2e, 0x00, 0x2e, 0x80, 0x2e, 0x00, 0xb0, 0x50, 0x30, 0x21, 0x2e, 0x59, 0xf5, + 0x10, 0x30, 0x21, 0x2e, 0x6a, 0xf5, 0x80, 0x2e, 0x3b, 0x03, 0x00, 0x00, 0x00, 0x00, 0x08, 0x19, 0x01, 0x00, 0x22, + 0x00, 0x75, 0x00, 0x00, 0x10, 0x00, 0x10, 0xd1, 0x00, 0xb3, 0x43, 0x80, 0x2e, 0x00, 0xc1, 0x80, 0x2e, 0x00, 0xc1, + 0x80, 0x2e, 0x00, 0xc1, 0x80, 0x2e, 0x00, 0xc1, 0x80, 0x2e, 0x00, 0xc1, 0x80, 0x2e, 0x00, 0xc1, 0x80, 0x2e, 0x00, + 0xc1, 0x80, 0x2e, 0x00, 0xc1, 0x80, 0x2e, 0x00, 0xc1, 0x80, 0x2e, 0x00, 0xc1, 0x80, 0x2e, 0x00, 0xc1, 0x80, 0x2e, + 0x00, 0xc1, 0x80, 0x2e, 0x00, 0xc1, 0x80, 0x2e, 0x00, 0xc1, 0x80, 0x2e, 0x00, 0xc1, 0x80, 0x2e, 0x00, 0xc1, 0x80, + 0x2e, 0x00, 0xc1, 0x80, 0x2e, 0x00, 0xc1, 0x80, 0x2e, 0x00, 0xc1, 0x80, 0x2e, 0x00, 0xc1, 0x80, 0x2e, 0x00, 0xc1, + 0x80, 0x2e, 0x00, 0xc1, 0x80, 0x2e, 0x00, 0xc1, 0x80, 0x2e, 0x00, 0xc1, 0x80, 0x2e, 0x00, 0xc1, 0x80, 0x2e, 0x00, + 0xc1, 0x80, 0x2e, 0x00, 0xc1, 0x80, 0x2e, 0x00, 0xc1, 0x80, 0x2e, 0x00, 0xc1, 0x80, 0x2e, 0x00, 0xc1, 0x80, 0x2e, + 0x00, 0xc1, 0x80, 0x2e, 0x00, 0xc1, 0x80, 0x2e, 0x00, 0xc1, 0x80, 0x2e, 0x00, 0xc1, 0x80, 0x2e, 0x00, 0xc1, 0x80, + 0x2e, 0x00, 0xc1, 0x80, 0x2e, 0x00, 0xc1, 0x80, 0x2e, 0x00, 0xc1, 0x80, 0x2e, 0x00, 0xc1, 0x80, 0x2e, 0x00, 0xc1, + 0x80, 0x2e, 0x00, 0xc1, 0x80, 0x2e, 0x00, 0xc1, 0xe0, 0x5f, 0x00, 0x00, 0x00, 0x00, 0x01, 0x00, 0x00, 0x00, 0x00, + 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x92, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, + 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x08, 0x19, 0x00, 0x00, 0x88, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x05, + 0xe0, 0xaa, 0x38, 0x05, 0xe0, 0x90, 0x30, 0xfa, 0x00, 0x96, 0x00, 0x4b, 0x09, 0x11, 0x00, 0x11, 0x00, 0x02, 0x00, + 0x2d, 0x01, 0xd4, 0x7b, 0x3b, 0x01, 0xdb, 0x7a, 0x04, 0x00, 0x3f, 0x7b, 0xcd, 0x6c, 0xc3, 0x04, 0x85, 0x09, 0xc3, + 0x04, 0xec, 0xe6, 0x0c, 0x46, 0x01, 0x00, 0x27, 0x00, 0x19, 0x00, 0x96, 0x00, 0xa0, 0x00, 0x01, 0x00, 0x0c, 0x00, + 0xf0, 0x3c, 0x00, 0x01, 0x01, 0x00, 0x03, 0x00, 0x01, 0x00, 0x0e, 0x00, 0x00, 0x00, 0x32, 0x00, 0x05, 0x00, 0xee, + 0x06, 0x04, 0x00, 0xc8, 0x00, 0x00, 0x00, 0x04, 0x00, 0xa8, 0x05, 0xee, 0x06, 0x00, 0x04, 0xbc, 0x02, 0xb3, 0x00, + 0x85, 0x07, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, + 0x00, 0x00, 0x00, 0xb4, 0x00, 0x01, 0x00, 0xb9, 0x00, 0x01, 0x00, 0x98, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, + 0x00, 0x00, 0x01, 0x00, 0x80, 0x00, 0x04, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, + 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, + 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, + 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, + 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x80, 0x2e, 0x00, 0xc1, 0xfd, 0x2d, 0xde, + 0x00, 0xeb, 0x00, 0xda, 0x00, 0x00, 0x0c, 0xff, 0x0f, 0x00, 0x04, 0xc0, 0x00, 0x5b, 0xf5, 0xc9, 0x01, 0x1e, 0xf2, + 0x80, 0x00, 0x3f, 0xff, 0x19, 0xf4, 0x58, 0xf5, 0x66, 0xf5, 0x64, 0xf5, 0xc0, 0xf1, 0xf0, 0x00, 0xe0, 0x00, 0xcd, + 0x01, 0xd3, 0x01, 0xdb, 0x01, 0xff, 0x7f, 0xff, 0x01, 0xe4, 0x00, 0x74, 0xf7, 0xf3, 0x00, 0xfa, 0x00, 0xff, 0x3f, + 0xca, 0x03, 0x6c, 0x38, 0x56, 0xfe, 0x44, 0xfd, 0xbc, 0x02, 0xf9, 0x06, 0x00, 0xfc, 0x12, 0x02, 0xae, 0x01, 0x58, + 0xfa, 0x9a, 0xfd, 0x77, 0x05, 0xbb, 0x02, 0x96, 0x01, 0x95, 0x01, 0x7f, 0x01, 0x82, 0x01, 0x89, 0x01, 0x87, 0x01, + 0x88, 0x01, 0x8a, 0x01, 0x8c, 0x01, 0x8f, 0x01, 0x8d, 0x01, 0x92, 0x01, 0x91, 0x01, 0xdd, 0x00, 0x9f, 0x01, 0x7e, + 0x01, 0xdb, 0x00, 0xb6, 0x01, 0x70, 0x69, 0x26, 0xd3, 0x9c, 0x07, 0x1f, 0x05, 0x9d, 0x00, 0x00, 0x08, 0xbc, 0x05, + 0x37, 0xfa, 0xa2, 0x01, 0xaa, 0x01, 0xa1, 0x01, 0xa8, 0x01, 0xa0, 0x01, 0xa8, 0x05, 0xb4, 0x01, 0xb4, 0x01, 0xce, + 0x00, 0xd0, 0x00, 0xfc, 0x00, 0xc5, 0x01, 0xff, 0xfb, 0xb1, 0x00, 0x00, 0x38, 0x00, 0x30, 0xfd, 0xf5, 0xfc, 0xf5, + 0xcd, 0x01, 0xa0, 0x00, 0x5f, 0xff, 0x00, 0x40, 0xff, 0x00, 0x00, 0x80, 0x6d, 0x0f, 0xeb, 0x00, 0x7f, 0xff, 0xc2, + 0xf5, 0x68, 0xf7, 0xb3, 0xf1, 0x67, 0x0f, 0x5b, 0x0f, 0x61, 0x0f, 0x80, 0x0f, 0x58, 0xf7, 0x5b, 0xf7, 0x83, 0x0f, + 0x86, 0x00, 0x72, 0x0f, 0x85, 0x0f, 0xc6, 0xf1, 0x7f, 0x0f, 0x6c, 0xf7, 0x00, 0xe0, 0x00, 0xff, 0xd1, 0xf5, 0x87, + 0x0f, 0x8a, 0x0f, 0xff, 0x03, 0xf0, 0x3f, 0x8b, 0x00, 0x8e, 0x00, 0x90, 0x00, 0xb9, 0x00, 0x2d, 0xf5, 0xca, 0xf5, + 0xcb, 0x01, 0x20, 0xf2, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, + 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, + 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, + 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, + 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, + 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, + 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, + 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, + 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, + 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, + 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, + 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x30, 0x50, 0x98, 0x2e, + 0xd7, 0x0e, 0x50, 0x32, 0x98, 0x2e, 0xfa, 0x03, 0x00, 0x30, 0xf0, 0x7f, 0x00, 0x2e, 0x00, 0x2e, 0xd0, 0x2e, 0x00, + 0x2e, 0x01, 0x80, 0x08, 0xa2, 0xfb, 0x2f, 0x98, 0x2e, 0xba, 0x03, 0x21, 0x2e, 0x19, 0x00, 0x01, 0x2e, 0xee, 0x00, + 0x00, 0xb2, 0x07, 0x2f, 0x01, 0x2e, 0x19, 0x00, 0x00, 0xb2, 0x03, 0x2f, 0x01, 0x50, 0x03, 0x52, 0x98, 0x2e, 0x07, + 0xcc, 0x01, 0x2e, 0xdd, 0x00, 0x00, 0xb2, 0x27, 0x2f, 0x05, 0x2e, 0x8a, 0x00, 0x05, 0x52, 0x98, 0x2e, 0xc7, 0xc1, + 0x03, 0x2e, 0xe9, 0x00, 0x40, 0xb2, 0xf0, 0x7f, 0x08, 0x2f, 0x01, 0x2e, 0x19, 0x00, 0x00, 0xb2, 0x04, 0x2f, 0x00, + 0x30, 0x21, 0x2e, 0xe9, 0x00, 0x98, 0x2e, 0xb4, 0xb1, 0x01, 0x2e, 0x18, 0x00, 0x00, 0xb2, 0x10, 0x2f, 0x05, 0x50, + 0x98, 0x2e, 0x4d, 0xc3, 0x05, 0x50, 0x98, 0x2e, 0x5a, 0xc7, 0x98, 0x2e, 0xf9, 0xb4, 0x98, 0x2e, 0x54, 0xb2, 0x98, + 0x2e, 0x67, 0xb6, 0x98, 0x2e, 0x17, 0xb2, 0x10, 0x30, 0x21, 0x2e, 0x77, 0x00, 0x01, 0x2e, 0xef, 0x00, 0x00, 0xb2, + 0x04, 0x2f, 0x98, 0x2e, 0x7a, 0xb7, 0x00, 0x30, 0x21, 0x2e, 0xef, 0x00, 0x01, 0x2e, 0xd4, 0x00, 0x04, 0xae, 0x0b, + 0x2f, 0x01, 0x2e, 0xdd, 0x00, 0x00, 0xb2, 0x07, 0x2f, 0x05, 0x52, 0x98, 0x2e, 0x8e, 0x0e, 0x00, 0xb2, 0x02, 0x2f, + 0x10, 0x30, 0x21, 0x2e, 0x7d, 0x00, 0x01, 0x2e, 0x7d, 0x00, 0x00, 0x90, 0x90, 0x2e, 0xf1, 0x02, 0x01, 0x2e, 0xd7, + 0x00, 0x00, 0xb2, 0x04, 0x2f, 0x98, 0x2e, 0x2f, 0x0e, 0x00, 0x30, 0x21, 0x2e, 0x7b, 0x00, 0x01, 0x2e, 0x7b, 0x00, + 0x00, 0xb2, 0x12, 0x2f, 0x01, 0x2e, 0xd4, 0x00, 0x00, 0x90, 0x02, 0x2f, 0x98, 0x2e, 0x1f, 0x0e, 0x09, 0x2d, 0x98, + 0x2e, 0x81, 0x0d, 0x01, 0x2e, 0xd4, 0x00, 0x04, 0x90, 0x02, 0x2f, 0x50, 0x32, 0x98, 0x2e, 0xfa, 0x03, 0x00, 0x30, + 0x21, 0x2e, 0x7b, 0x00, 0x01, 0x2e, 0x7c, 0x00, 0x00, 0xb2, 0x90, 0x2e, 0x09, 0x03, 0x01, 0x2e, 0x7c, 0x00, 0x01, + 0x31, 0x01, 0x08, 0x00, 0xb2, 0x04, 0x2f, 0x98, 0x2e, 0x47, 0xcb, 0x10, 0x30, 0x21, 0x2e, 0x77, 0x00, 0x81, 0x30, + 0x01, 0x2e, 0x7c, 0x00, 0x01, 0x08, 0x00, 0xb2, 0x61, 0x2f, 0x03, 0x2e, 0x89, 0x00, 0x01, 0x2e, 0xd4, 0x00, 0x98, + 0xbc, 0x98, 0xb8, 0x05, 0xb2, 0x0f, 0x58, 0x23, 0x2f, 0x07, 0x90, 0x09, 0x54, 0x00, 0x30, 0x37, 0x2f, 0x15, 0x41, + 0x04, 0x41, 0xdc, 0xbe, 0x44, 0xbe, 0xdc, 0xba, 0x2c, 0x01, 0x61, 0x00, 0x0f, 0x56, 0x4a, 0x0f, 0x0c, 0x2f, 0xd1, + 0x42, 0x94, 0xb8, 0xc1, 0x42, 0x11, 0x30, 0x05, 0x2e, 0x6a, 0xf7, 0x2c, 0xbd, 0x2f, 0xb9, 0x80, 0xb2, 0x08, 0x22, + 0x98, 0x2e, 0xc3, 0xb7, 0x21, 0x2d, 0x61, 0x30, 0x23, 0x2e, 0xd4, 0x00, 0x98, 0x2e, 0xc3, 0xb7, 0x00, 0x30, 0x21, + 0x2e, 0x5a, 0xf5, 0x18, 0x2d, 0xe1, 0x7f, 0x50, 0x30, 0x98, 0x2e, 0xfa, 0x03, 0x0f, 0x52, 0x07, 0x50, 0x50, 0x42, + 0x70, 0x30, 0x0d, 0x54, 0x42, 0x42, 0x7e, 0x82, 0xe2, 0x6f, 0x80, 0xb2, 0x42, 0x42, 0x05, 0x2f, 0x21, 0x2e, 0xd4, + 0x00, 0x10, 0x30, 0x98, 0x2e, 0xc3, 0xb7, 0x03, 0x2d, 0x60, 0x30, 0x21, 0x2e, 0xd4, 0x00, 0x01, 0x2e, 0xd4, 0x00, + 0x06, 0x90, 0x18, 0x2f, 0x01, 0x2e, 0x76, 0x00, 0x0b, 0x54, 0x07, 0x52, 0xe0, 0x7f, 0x98, 0x2e, 0x7a, 0xc1, 0xe1, + 0x6f, 0x08, 0x1a, 0x40, 0x30, 0x08, 0x2f, 0x21, 0x2e, 0xd4, 0x00, 0x20, 0x30, 0x98, 0x2e, 0xaf, 0xb7, 0x50, 0x32, + 0x98, 0x2e, 0xfa, 0x03, 0x05, 0x2d, 0x98, 0x2e, 0x38, 0x0e, 0x00, 0x30, 0x21, 0x2e, 0xd4, 0x00, 0x00, 0x30, 0x21, + 0x2e, 0x7c, 0x00, 0x18, 0x2d, 0x01, 0x2e, 0xd4, 0x00, 0x03, 0xaa, 0x01, 0x2f, 0x98, 0x2e, 0x45, 0x0e, 0x01, 0x2e, + 0xd4, 0x00, 0x3f, 0x80, 0x03, 0xa2, 0x01, 0x2f, 0x00, 0x2e, 0x02, 0x2d, 0x98, 0x2e, 0x5b, 0x0e, 0x30, 0x30, 0x98, + 0x2e, 0xce, 0xb7, 0x00, 0x30, 0x21, 0x2e, 0x7d, 0x00, 0x50, 0x32, 0x98, 0x2e, 0xfa, 0x03, 0x01, 0x2e, 0x77, 0x00, + 0x00, 0xb2, 0x24, 0x2f, 0x98, 0x2e, 0xf5, 0xcb, 0x03, 0x2e, 0xd5, 0x00, 0x11, 0x54, 0x01, 0x0a, 0xbc, 0x84, 0x83, + 0x86, 0x21, 0x2e, 0xc9, 0x01, 0xe0, 0x40, 0x13, 0x52, 0xc4, 0x40, 0x82, 0x40, 0xa8, 0xb9, 0x52, 0x42, 0x43, 0xbe, + 0x53, 0x42, 0x04, 0x0a, 0x50, 0x42, 0xe1, 0x7f, 0xf0, 0x31, 0x41, 0x40, 0xf2, 0x6f, 0x25, 0xbd, 0x08, 0x08, 0x02, + 0x0a, 0xd0, 0x7f, 0x98, 0x2e, 0xa8, 0xcf, 0x06, 0xbc, 0xd1, 0x6f, 0xe2, 0x6f, 0x08, 0x0a, 0x80, 0x42, 0x98, 0x2e, + 0x58, 0xb7, 0x00, 0x30, 0x21, 0x2e, 0xee, 0x00, 0x21, 0x2e, 0x77, 0x00, 0x21, 0x2e, 0xdd, 0x00, 0x80, 0x2e, 0xf4, + 0x01, 0x1a, 0x24, 0x22, 0x00, 0x80, 0x2e, 0xec, 0x01, 0x10, 0x50, 0xfb, 0x7f, 0x98, 0x2e, 0xf3, 0x03, 0x57, 0x50, + 0xfb, 0x6f, 0x01, 0x30, 0x71, 0x54, 0x11, 0x42, 0x42, 0x0e, 0xfc, 0x2f, 0xc0, 0x2e, 0x01, 0x42, 0xf0, 0x5f, 0x80, + 0x2e, 0x00, 0xc1, 0xfd, 0x2d, 0x01, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x9a, 0x01, + 0x34, 0x03, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, + 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, + 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, + 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, + 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, + 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, + 0x00, 0x00, 0x20, 0x50, 0xe7, 0x7f, 0xf6, 0x7f, 0x06, 0x32, 0x0f, 0x2e, 0x61, 0xf5, 0xfe, 0x09, 0xc0, 0xb3, 0x04, + 0x2f, 0x17, 0x30, 0x2f, 0x2e, 0xef, 0x00, 0x2d, 0x2e, 0x61, 0xf5, 0xf6, 0x6f, 0xe7, 0x6f, 0xe0, 0x5f, 0xc8, 0x2e, + 0x20, 0x50, 0xe7, 0x7f, 0xf6, 0x7f, 0x46, 0x30, 0x0f, 0x2e, 0xa4, 0xf1, 0xbe, 0x09, 0x80, 0xb3, 0x06, 0x2f, 0x0d, + 0x2e, 0xd4, 0x00, 0x84, 0xaf, 0x02, 0x2f, 0x16, 0x30, 0x2d, 0x2e, 0x7b, 0x00, 0x86, 0x30, 0x2d, 0x2e, 0x60, 0xf5, + 0xf6, 0x6f, 0xe7, 0x6f, 0xe0, 0x5f, 0xc8, 0x2e, 0x01, 0x2e, 0x77, 0xf7, 0x09, 0xbc, 0x0f, 0xb8, 0x00, 0xb2, 0x10, + 0x50, 0xfb, 0x7f, 0x10, 0x30, 0x0b, 0x2f, 0x03, 0x2e, 0x8a, 0x00, 0x96, 0xbc, 0x9f, 0xb8, 0x40, 0xb2, 0x05, 0x2f, + 0x03, 0x2e, 0x68, 0xf7, 0x9e, 0xbc, 0x9f, 0xb8, 0x40, 0xb2, 0x07, 0x2f, 0x03, 0x2e, 0x7e, 0x00, 0x41, 0x90, 0x01, + 0x2f, 0x98, 0x2e, 0xdc, 0x03, 0x03, 0x2c, 0x00, 0x30, 0x21, 0x2e, 0x7e, 0x00, 0xfb, 0x6f, 0xf0, 0x5f, 0xb8, 0x2e, + 0x20, 0x50, 0xe0, 0x7f, 0xfb, 0x7f, 0x00, 0x2e, 0x27, 0x50, 0x98, 0x2e, 0x3b, 0xc8, 0x29, 0x50, 0x98, 0x2e, 0xa7, + 0xc8, 0x01, 0x50, 0x98, 0x2e, 0x55, 0xcc, 0xe1, 0x6f, 0x2b, 0x50, 0x98, 0x2e, 0xe0, 0xc9, 0xfb, 0x6f, 0x00, 0x30, + 0xe0, 0x5f, 0x21, 0x2e, 0x7e, 0x00, 0xb8, 0x2e, 0x73, 0x50, 0x01, 0x30, 0x57, 0x54, 0x11, 0x42, 0x42, 0x0e, 0xfc, + 0x2f, 0xb8, 0x2e, 0x21, 0x2e, 0x59, 0xf5, 0x10, 0x30, 0xc0, 0x2e, 0x21, 0x2e, 0x4a, 0xf1, 0x90, 0x50, 0xf7, 0x7f, + 0xe6, 0x7f, 0xd5, 0x7f, 0xc4, 0x7f, 0xb3, 0x7f, 0xa1, 0x7f, 0x90, 0x7f, 0x82, 0x7f, 0x7b, 0x7f, 0x98, 0x2e, 0x35, + 0xb7, 0x00, 0xb2, 0x90, 0x2e, 0x97, 0xb0, 0x03, 0x2e, 0x8f, 0x00, 0x07, 0x2e, 0x91, 0x00, 0x05, 0x2e, 0xb1, 0x00, + 0x3f, 0xba, 0x9f, 0xb8, 0x01, 0x2e, 0xb1, 0x00, 0xa3, 0xbd, 0x4c, 0x0a, 0x05, 0x2e, 0xb1, 0x00, 0x04, 0xbe, 0xbf, + 0xb9, 0xcb, 0x0a, 0x4f, 0xba, 0x22, 0xbd, 0x01, 0x2e, 0xb3, 0x00, 0xdc, 0x0a, 0x2f, 0xb9, 0x03, 0x2e, 0xb8, 0x00, + 0x0a, 0xbe, 0x9a, 0x0a, 0xcf, 0xb9, 0x9b, 0xbc, 0x01, 0x2e, 0x97, 0x00, 0x9f, 0xb8, 0x93, 0x0a, 0x0f, 0xbc, 0x91, + 0x0a, 0x0f, 0xb8, 0x90, 0x0a, 0x25, 0x2e, 0x18, 0x00, 0x05, 0x2e, 0xc1, 0xf5, 0x2e, 0xbd, 0x2e, 0xb9, 0x01, 0x2e, + 0x19, 0x00, 0x31, 0x30, 0x8a, 0x04, 0x00, 0x90, 0x07, 0x2f, 0x01, 0x2e, 0xd4, 0x00, 0x04, 0xa2, 0x03, 0x2f, 0x01, + 0x2e, 0x18, 0x00, 0x00, 0xb2, 0x0c, 0x2f, 0x19, 0x50, 0x05, 0x52, 0x98, 0x2e, 0x4d, 0xb7, 0x05, 0x2e, 0x78, 0x00, + 0x80, 0x90, 0x10, 0x30, 0x01, 0x2f, 0x21, 0x2e, 0x78, 0x00, 0x25, 0x2e, 0xdd, 0x00, 0x98, 0x2e, 0x3e, 0xb7, 0x00, + 0xb2, 0x02, 0x30, 0x01, 0x30, 0x04, 0x2f, 0x01, 0x2e, 0x19, 0x00, 0x00, 0xb2, 0x00, 0x2f, 0x21, 0x30, 0x01, 0x2e, + 0xea, 0x00, 0x08, 0x1a, 0x0e, 0x2f, 0x23, 0x2e, 0xea, 0x00, 0x33, 0x30, 0x1b, 0x50, 0x0b, 0x09, 0x01, 0x40, 0x17, + 0x56, 0x46, 0xbe, 0x4b, 0x08, 0x4c, 0x0a, 0x01, 0x42, 0x0a, 0x80, 0x15, 0x52, 0x01, 0x42, 0x00, 0x2e, 0x01, 0x2e, + 0x18, 0x00, 0x00, 0xb2, 0x1f, 0x2f, 0x03, 0x2e, 0xc0, 0xf5, 0xf0, 0x30, 0x48, 0x08, 0x47, 0xaa, 0x74, 0x30, 0x07, + 0x2e, 0x7a, 0x00, 0x61, 0x22, 0x4b, 0x1a, 0x05, 0x2f, 0x07, 0x2e, 0x66, 0xf5, 0xbf, 0xbd, 0xbf, 0xb9, 0xc0, 0x90, + 0x0b, 0x2f, 0x1d, 0x56, 0x2b, 0x30, 0xd2, 0x42, 0xdb, 0x42, 0x01, 0x04, 0xc2, 0x42, 0x04, 0xbd, 0xfe, 0x80, 0x81, + 0x84, 0x23, 0x2e, 0x7a, 0x00, 0x02, 0x42, 0x02, 0x32, 0x25, 0x2e, 0x62, 0xf5, 0x05, 0x2e, 0xd6, 0x00, 0x81, 0x84, + 0x25, 0x2e, 0xd6, 0x00, 0x02, 0x31, 0x25, 0x2e, 0x60, 0xf5, 0x05, 0x2e, 0x8a, 0x00, 0x0b, 0x50, 0x90, 0x08, 0x80, + 0xb2, 0x0b, 0x2f, 0x05, 0x2e, 0xca, 0xf5, 0xf0, 0x3e, 0x90, 0x08, 0x25, 0x2e, 0xca, 0xf5, 0x05, 0x2e, 0x59, 0xf5, + 0xe0, 0x3f, 0x90, 0x08, 0x25, 0x2e, 0x59, 0xf5, 0x90, 0x6f, 0xa1, 0x6f, 0xb3, 0x6f, 0xc4, 0x6f, 0xd5, 0x6f, 0xe6, + 0x6f, 0xf7, 0x6f, 0x7b, 0x6f, 0x82, 0x6f, 0x70, 0x5f, 0xc8, 0x2e, 0xc0, 0x50, 0x90, 0x7f, 0xe5, 0x7f, 0xd4, 0x7f, + 0xc3, 0x7f, 0xb1, 0x7f, 0xa2, 0x7f, 0x87, 0x7f, 0xf6, 0x7f, 0x7b, 0x7f, 0x00, 0x2e, 0x01, 0x2e, 0x60, 0xf5, 0x60, + 0x7f, 0x98, 0x2e, 0x35, 0xb7, 0x02, 0x30, 0x63, 0x6f, 0x15, 0x52, 0x50, 0x7f, 0x62, 0x7f, 0x5a, 0x2c, 0x02, 0x32, + 0x1a, 0x09, 0x00, 0xb3, 0x14, 0x2f, 0x00, 0xb2, 0x03, 0x2f, 0x09, 0x2e, 0x18, 0x00, 0x00, 0x91, 0x0c, 0x2f, 0x43, + 0x7f, 0x98, 0x2e, 0x97, 0xb7, 0x1f, 0x50, 0x02, 0x8a, 0x02, 0x32, 0x04, 0x30, 0x25, 0x2e, 0x64, 0xf5, 0x15, 0x52, + 0x50, 0x6f, 0x43, 0x6f, 0x44, 0x43, 0x25, 0x2e, 0x60, 0xf5, 0xd9, 0x08, 0xc0, 0xb2, 0x36, 0x2f, 0x98, 0x2e, 0x3e, + 0xb7, 0x00, 0xb2, 0x06, 0x2f, 0x01, 0x2e, 0x19, 0x00, 0x00, 0xb2, 0x02, 0x2f, 0x50, 0x6f, 0x00, 0x90, 0x0a, 0x2f, + 0x01, 0x2e, 0x79, 0x00, 0x00, 0x90, 0x19, 0x2f, 0x10, 0x30, 0x21, 0x2e, 0x79, 0x00, 0x00, 0x30, 0x98, 0x2e, 0xdc, + 0x03, 0x13, 0x2d, 0x01, 0x2e, 0xc3, 0xf5, 0x0c, 0xbc, 0x0f, 0xb8, 0x12, 0x30, 0x10, 0x04, 0x03, 0xb0, 0x26, 0x25, + 0x21, 0x50, 0x03, 0x52, 0x98, 0x2e, 0x4d, 0xb7, 0x10, 0x30, 0x21, 0x2e, 0xee, 0x00, 0x02, 0x30, 0x60, 0x7f, 0x25, + 0x2e, 0x79, 0x00, 0x60, 0x6f, 0x00, 0x90, 0x05, 0x2f, 0x00, 0x30, 0x21, 0x2e, 0xea, 0x00, 0x15, 0x50, 0x21, 0x2e, + 0x64, 0xf5, 0x15, 0x52, 0x23, 0x2e, 0x60, 0xf5, 0x02, 0x32, 0x50, 0x6f, 0x00, 0x90, 0x02, 0x2f, 0x03, 0x30, 0x27, + 0x2e, 0x78, 0x00, 0x07, 0x2e, 0x60, 0xf5, 0x1a, 0x09, 0x00, 0x91, 0xa3, 0x2f, 0x19, 0x09, 0x00, 0x91, 0xa0, 0x2f, + 0x90, 0x6f, 0xa2, 0x6f, 0xb1, 0x6f, 0xc3, 0x6f, 0xd4, 0x6f, 0xe5, 0x6f, 0x7b, 0x6f, 0xf6, 0x6f, 0x87, 0x6f, 0x40, + 0x5f, 0xc8, 0x2e, 0xc0, 0x50, 0xe7, 0x7f, 0xf6, 0x7f, 0x26, 0x30, 0x0f, 0x2e, 0x61, 0xf5, 0x2f, 0x2e, 0x7c, 0x00, + 0x0f, 0x2e, 0x7c, 0x00, 0xbe, 0x09, 0xa2, 0x7f, 0x80, 0x7f, 0x80, 0xb3, 0xd5, 0x7f, 0xc4, 0x7f, 0xb3, 0x7f, 0x91, + 0x7f, 0x7b, 0x7f, 0x0b, 0x2f, 0x23, 0x50, 0x1a, 0x25, 0x12, 0x40, 0x42, 0x7f, 0x74, 0x82, 0x12, 0x40, 0x52, 0x7f, + 0x00, 0x2e, 0x00, 0x40, 0x60, 0x7f, 0x98, 0x2e, 0x6a, 0xd6, 0x81, 0x30, 0x01, 0x2e, 0x7c, 0x00, 0x01, 0x08, 0x00, + 0xb2, 0x42, 0x2f, 0x03, 0x2e, 0x89, 0x00, 0x01, 0x2e, 0x89, 0x00, 0x97, 0xbc, 0x06, 0xbc, 0x9f, 0xb8, 0x0f, 0xb8, + 0x00, 0x90, 0x23, 0x2e, 0xd8, 0x00, 0x10, 0x30, 0x01, 0x30, 0x2a, 0x2f, 0x03, 0x2e, 0xd4, 0x00, 0x44, 0xb2, 0x05, + 0x2f, 0x47, 0xb2, 0x00, 0x30, 0x2d, 0x2f, 0x21, 0x2e, 0x7c, 0x00, 0x2b, 0x2d, 0x03, 0x2e, 0xfd, 0xf5, 0x9e, 0xbc, + 0x9f, 0xb8, 0x40, 0x90, 0x14, 0x2f, 0x03, 0x2e, 0xfc, 0xf5, 0x99, 0xbc, 0x9f, 0xb8, 0x40, 0x90, 0x0e, 0x2f, 0x03, + 0x2e, 0x49, 0xf1, 0x25, 0x54, 0x4a, 0x08, 0x40, 0x90, 0x08, 0x2f, 0x98, 0x2e, 0x35, 0xb7, 0x00, 0xb2, 0x10, 0x30, + 0x03, 0x2f, 0x50, 0x30, 0x21, 0x2e, 0xd4, 0x00, 0x10, 0x2d, 0x98, 0x2e, 0xaf, 0xb7, 0x00, 0x30, 0x21, 0x2e, 0x7c, + 0x00, 0x0a, 0x2d, 0x05, 0x2e, 0x69, 0xf7, 0x2d, 0xbd, 0x2f, 0xb9, 0x80, 0xb2, 0x01, 0x2f, 0x21, 0x2e, 0x7d, 0x00, + 0x23, 0x2e, 0x7c, 0x00, 0xe0, 0x31, 0x21, 0x2e, 0x61, 0xf5, 0xf6, 0x6f, 0xe7, 0x6f, 0x80, 0x6f, 0xa2, 0x6f, 0xb3, + 0x6f, 0xc4, 0x6f, 0xd5, 0x6f, 0x7b, 0x6f, 0x91, 0x6f, 0x40, 0x5f, 0xc8, 0x2e, 0x60, 0x51, 0x0a, 0x25, 0x36, 0x88, + 0xf4, 0x7f, 0xeb, 0x7f, 0x00, 0x32, 0x31, 0x52, 0x32, 0x30, 0x13, 0x30, 0x98, 0x2e, 0x15, 0xcb, 0x0a, 0x25, 0x33, + 0x84, 0xd2, 0x7f, 0x43, 0x30, 0x05, 0x50, 0x2d, 0x52, 0x98, 0x2e, 0x95, 0xc1, 0xd2, 0x6f, 0x27, 0x52, 0x98, 0x2e, + 0xd7, 0xc7, 0x2a, 0x25, 0xb0, 0x86, 0xc0, 0x7f, 0xd3, 0x7f, 0xaf, 0x84, 0x29, 0x50, 0xf1, 0x6f, 0x98, 0x2e, 0x4d, + 0xc8, 0x2a, 0x25, 0xae, 0x8a, 0xaa, 0x88, 0xf2, 0x6e, 0x2b, 0x50, 0xc1, 0x6f, 0xd3, 0x6f, 0xf4, 0x7f, 0x98, 0x2e, + 0xb6, 0xc8, 0xe0, 0x6e, 0x00, 0xb2, 0x32, 0x2f, 0x33, 0x54, 0x83, 0x86, 0xf1, 0x6f, 0xc3, 0x7f, 0x04, 0x30, 0x30, + 0x30, 0xf4, 0x7f, 0xd0, 0x7f, 0xb2, 0x7f, 0xe3, 0x30, 0xc5, 0x6f, 0x56, 0x40, 0x45, 0x41, 0x28, 0x08, 0x03, 0x14, + 0x0e, 0xb4, 0x08, 0xbc, 0x82, 0x40, 0x10, 0x0a, 0x2f, 0x54, 0x26, 0x05, 0x91, 0x7f, 0x44, 0x28, 0xa3, 0x7f, 0x98, + 0x2e, 0xd9, 0xc0, 0x08, 0xb9, 0x33, 0x30, 0x53, 0x09, 0xc1, 0x6f, 0xd3, 0x6f, 0xf4, 0x6f, 0x83, 0x17, 0x47, 0x40, + 0x6c, 0x15, 0xb2, 0x6f, 0xbe, 0x09, 0x75, 0x0b, 0x90, 0x42, 0x45, 0x42, 0x51, 0x0e, 0x32, 0xbc, 0x02, 0x89, 0xa1, + 0x6f, 0x7e, 0x86, 0xf4, 0x7f, 0xd0, 0x7f, 0xb2, 0x7f, 0x04, 0x30, 0x91, 0x6f, 0xd6, 0x2f, 0xeb, 0x6f, 0xa0, 0x5e, + 0xb8, 0x2e, 0x03, 0x2e, 0x97, 0x00, 0x1b, 0xbc, 0x60, 0x50, 0x9f, 0xbc, 0x0c, 0xb8, 0xf0, 0x7f, 0x40, 0xb2, 0xeb, + 0x7f, 0x2b, 0x2f, 0x03, 0x2e, 0x7f, 0x00, 0x41, 0x40, 0x01, 0x2e, 0xc8, 0x00, 0x01, 0x1a, 0x11, 0x2f, 0x37, 0x58, + 0x23, 0x2e, 0xc8, 0x00, 0x10, 0x41, 0xa0, 0x7f, 0x38, 0x81, 0x01, 0x41, 0xd0, 0x7f, 0xb1, 0x7f, 0x98, 0x2e, 0x64, + 0xcf, 0xd0, 0x6f, 0x07, 0x80, 0xa1, 0x6f, 0x11, 0x42, 0x00, 0x2e, 0xb1, 0x6f, 0x01, 0x42, 0x11, 0x30, 0x01, 0x2e, + 0xfc, 0x00, 0x00, 0xa8, 0x03, 0x30, 0xcb, 0x22, 0x4a, 0x25, 0x01, 0x2e, 0x7f, 0x00, 0x3c, 0x89, 0x35, 0x52, 0x05, + 0x54, 0x98, 0x2e, 0xc4, 0xce, 0xc1, 0x6f, 0xf0, 0x6f, 0x98, 0x2e, 0x95, 0xcf, 0x04, 0x2d, 0x01, 0x30, 0xf0, 0x6f, + 0x98, 0x2e, 0x95, 0xcf, 0xeb, 0x6f, 0xa0, 0x5f, 0xb8, 0x2e, 0x03, 0x2e, 0xb3, 0x00, 0x02, 0x32, 0xf0, 0x30, 0x03, + 0x31, 0x30, 0x50, 0x8a, 0x08, 0x08, 0x08, 0xcb, 0x08, 0xe0, 0x7f, 0x80, 0xb2, 0xf3, 0x7f, 0xdb, 0x7f, 0x25, 0x2f, + 0x03, 0x2e, 0xca, 0x00, 0x41, 0x90, 0x04, 0x2f, 0x01, 0x30, 0x23, 0x2e, 0xca, 0x00, 0x98, 0x2e, 0x3f, 0x03, 0xc0, + 0xb2, 0x05, 0x2f, 0x03, 0x2e, 0xda, 0x00, 0x00, 0x30, 0x41, 0x04, 0x23, 0x2e, 0xda, 0x00, 0x98, 0x2e, 0x92, 0xb2, + 0x10, 0x25, 0xf0, 0x6f, 0x00, 0xb2, 0x05, 0x2f, 0x01, 0x2e, 0xda, 0x00, 0x02, 0x30, 0x10, 0x04, 0x21, 0x2e, 0xda, + 0x00, 0x40, 0xb2, 0x01, 0x2f, 0x23, 0x2e, 0xc8, 0x01, 0xdb, 0x6f, 0xe0, 0x6f, 0xd0, 0x5f, 0x80, 0x2e, 0x95, 0xcf, + 0x01, 0x30, 0xe0, 0x6f, 0x98, 0x2e, 0x95, 0xcf, 0x11, 0x30, 0x23, 0x2e, 0xca, 0x00, 0xdb, 0x6f, 0xd0, 0x5f, 0xb8, + 0x2e, 0xd0, 0x50, 0x0a, 0x25, 0x33, 0x84, 0x55, 0x50, 0xd2, 0x7f, 0xe2, 0x7f, 0x03, 0x8c, 0xc0, 0x7f, 0xbb, 0x7f, + 0x00, 0x30, 0x05, 0x5a, 0x39, 0x54, 0x51, 0x41, 0xa5, 0x7f, 0x96, 0x7f, 0x80, 0x7f, 0x98, 0x2e, 0xd9, 0xc0, 0x05, + 0x30, 0xf5, 0x7f, 0x20, 0x25, 0x91, 0x6f, 0x3b, 0x58, 0x3d, 0x5c, 0x3b, 0x56, 0x98, 0x2e, 0x67, 0xcc, 0xc1, 0x6f, + 0xd5, 0x6f, 0x52, 0x40, 0x50, 0x43, 0xc1, 0x7f, 0xd5, 0x7f, 0x10, 0x25, 0x98, 0x2e, 0xfe, 0xc9, 0x10, 0x25, 0x98, + 0x2e, 0x74, 0xc0, 0x86, 0x6f, 0x30, 0x28, 0x92, 0x6f, 0x82, 0x8c, 0xa5, 0x6f, 0x6f, 0x52, 0x69, 0x0e, 0x39, 0x54, + 0xdb, 0x2f, 0x19, 0xa0, 0x15, 0x30, 0x03, 0x2f, 0x00, 0x30, 0x21, 0x2e, 0x81, 0x01, 0x0a, 0x2d, 0x01, 0x2e, 0x81, + 0x01, 0x05, 0x28, 0x42, 0x36, 0x21, 0x2e, 0x81, 0x01, 0x02, 0x0e, 0x01, 0x2f, 0x98, 0x2e, 0xf3, 0x03, 0x57, 0x50, + 0x12, 0x30, 0x01, 0x40, 0x98, 0x2e, 0xfe, 0xc9, 0x51, 0x6f, 0x0b, 0x5c, 0x8e, 0x0e, 0x3b, 0x6f, 0x57, 0x58, 0x02, + 0x30, 0x21, 0x2e, 0x95, 0x01, 0x45, 0x6f, 0x2a, 0x8d, 0xd2, 0x7f, 0xcb, 0x7f, 0x13, 0x2f, 0x02, 0x30, 0x3f, 0x50, + 0xd2, 0x7f, 0xa8, 0x0e, 0x0e, 0x2f, 0xc0, 0x6f, 0x53, 0x54, 0x02, 0x00, 0x51, 0x54, 0x42, 0x0e, 0x10, 0x30, 0x59, + 0x52, 0x02, 0x30, 0x01, 0x2f, 0x00, 0x2e, 0x03, 0x2d, 0x50, 0x42, 0x42, 0x42, 0x12, 0x30, 0xd2, 0x7f, 0x80, 0xb2, + 0x03, 0x2f, 0x00, 0x30, 0x21, 0x2e, 0x80, 0x01, 0x12, 0x2d, 0x01, 0x2e, 0xc9, 0x00, 0x02, 0x80, 0x05, 0x2e, 0x80, + 0x01, 0x11, 0x30, 0x91, 0x28, 0x00, 0x40, 0x25, 0x2e, 0x80, 0x01, 0x10, 0x0e, 0x05, 0x2f, 0x01, 0x2e, 0x7f, 0x01, + 0x01, 0x90, 0x01, 0x2f, 0x98, 0x2e, 0xf3, 0x03, 0x00, 0x2e, 0xa0, 0x41, 0x01, 0x90, 0xa6, 0x7f, 0x90, 0x2e, 0xe3, + 0xb4, 0x01, 0x2e, 0x95, 0x01, 0x00, 0xa8, 0x90, 0x2e, 0xe3, 0xb4, 0x5b, 0x54, 0x95, 0x80, 0x82, 0x40, 0x80, 0xb2, + 0x02, 0x40, 0x2d, 0x8c, 0x3f, 0x52, 0x96, 0x7f, 0x90, 0x2e, 0xc2, 0xb3, 0x29, 0x0e, 0x76, 0x2f, 0x01, 0x2e, 0xc9, + 0x00, 0x00, 0x40, 0x81, 0x28, 0x45, 0x52, 0xb3, 0x30, 0x98, 0x2e, 0x0f, 0xca, 0x5d, 0x54, 0x80, 0x7f, 0x00, 0x2e, + 0xa1, 0x40, 0x72, 0x7f, 0x82, 0x80, 0x82, 0x40, 0x60, 0x7f, 0x98, 0x2e, 0xfe, 0xc9, 0x10, 0x25, 0x98, 0x2e, 0x74, + 0xc0, 0x62, 0x6f, 0x05, 0x30, 0x87, 0x40, 0xc0, 0x91, 0x04, 0x30, 0x05, 0x2f, 0x05, 0x2e, 0x83, 0x01, 0x80, 0xb2, + 0x14, 0x30, 0x00, 0x2f, 0x04, 0x30, 0x05, 0x2e, 0xc9, 0x00, 0x73, 0x6f, 0x81, 0x40, 0xe2, 0x40, 0x69, 0x04, 0x11, + 0x0f, 0xe1, 0x40, 0x16, 0x30, 0xfe, 0x29, 0xcb, 0x40, 0x02, 0x2f, 0x83, 0x6f, 0x83, 0x0f, 0x22, 0x2f, 0x47, 0x56, + 0x13, 0x0f, 0x12, 0x30, 0x77, 0x2f, 0x49, 0x54, 0x42, 0x0e, 0x12, 0x30, 0x73, 0x2f, 0x00, 0x91, 0x0a, 0x2f, 0x01, + 0x2e, 0x8b, 0x01, 0x19, 0xa8, 0x02, 0x30, 0x6c, 0x2f, 0x63, 0x50, 0x00, 0x2e, 0x17, 0x42, 0x05, 0x42, 0x68, 0x2c, + 0x12, 0x30, 0x0b, 0x25, 0x08, 0x0f, 0x50, 0x30, 0x02, 0x2f, 0x21, 0x2e, 0x83, 0x01, 0x03, 0x2d, 0x40, 0x30, 0x21, + 0x2e, 0x83, 0x01, 0x2b, 0x2e, 0x85, 0x01, 0x5a, 0x2c, 0x12, 0x30, 0x00, 0x91, 0x2b, 0x25, 0x04, 0x2f, 0x63, 0x50, + 0x02, 0x30, 0x17, 0x42, 0x17, 0x2c, 0x02, 0x42, 0x98, 0x2e, 0xfe, 0xc9, 0x10, 0x25, 0x98, 0x2e, 0x74, 0xc0, 0x05, + 0x2e, 0xc9, 0x00, 0x81, 0x84, 0x5b, 0x30, 0x82, 0x40, 0x37, 0x2e, 0x83, 0x01, 0x02, 0x0e, 0x07, 0x2f, 0x5f, 0x52, + 0x40, 0x30, 0x62, 0x40, 0x41, 0x40, 0x91, 0x0e, 0x01, 0x2f, 0x21, 0x2e, 0x83, 0x01, 0x05, 0x30, 0x2b, 0x2e, 0x85, + 0x01, 0x12, 0x30, 0x36, 0x2c, 0x16, 0x30, 0x15, 0x25, 0x81, 0x7f, 0x98, 0x2e, 0xfe, 0xc9, 0x10, 0x25, 0x98, 0x2e, + 0x74, 0xc0, 0x19, 0xa2, 0x16, 0x30, 0x15, 0x2f, 0x05, 0x2e, 0x97, 0x01, 0x80, 0x6f, 0x82, 0x0e, 0x05, 0x2f, 0x01, + 0x2e, 0x86, 0x01, 0x06, 0x28, 0x21, 0x2e, 0x86, 0x01, 0x0b, 0x2d, 0x03, 0x2e, 0x87, 0x01, 0x5f, 0x54, 0x4e, 0x28, + 0x91, 0x42, 0x00, 0x2e, 0x82, 0x40, 0x90, 0x0e, 0x01, 0x2f, 0x21, 0x2e, 0x88, 0x01, 0x02, 0x30, 0x13, 0x2c, 0x05, + 0x30, 0xc0, 0x6f, 0x08, 0x1c, 0xa8, 0x0f, 0x16, 0x30, 0x05, 0x30, 0x5b, 0x50, 0x09, 0x2f, 0x02, 0x80, 0x2d, 0x2e, + 0x82, 0x01, 0x05, 0x42, 0x05, 0x80, 0x00, 0x2e, 0x02, 0x42, 0x3e, 0x80, 0x00, 0x2e, 0x06, 0x42, 0x02, 0x30, 0x90, + 0x6f, 0x3e, 0x88, 0x01, 0x40, 0x04, 0x41, 0x4c, 0x28, 0x01, 0x42, 0x07, 0x80, 0x10, 0x25, 0x24, 0x40, 0x00, 0x40, + 0x00, 0xa8, 0xf5, 0x22, 0x23, 0x29, 0x44, 0x42, 0x7a, 0x82, 0x7e, 0x88, 0x43, 0x40, 0x04, 0x41, 0x00, 0xab, 0xf5, + 0x23, 0xdf, 0x28, 0x43, 0x42, 0xd9, 0xa0, 0x14, 0x2f, 0x00, 0x90, 0x02, 0x2f, 0xd2, 0x6f, 0x81, 0xb2, 0x05, 0x2f, + 0x63, 0x54, 0x06, 0x28, 0x90, 0x42, 0x85, 0x42, 0x09, 0x2c, 0x02, 0x30, 0x5b, 0x50, 0x03, 0x80, 0x29, 0x2e, 0x7e, + 0x01, 0x2b, 0x2e, 0x82, 0x01, 0x05, 0x42, 0x12, 0x30, 0x2b, 0x2e, 0x83, 0x01, 0x45, 0x82, 0x00, 0x2e, 0x40, 0x40, + 0x7a, 0x82, 0x02, 0xa0, 0x08, 0x2f, 0x63, 0x50, 0x3b, 0x30, 0x15, 0x42, 0x05, 0x42, 0x37, 0x80, 0x37, 0x2e, 0x7e, + 0x01, 0x05, 0x42, 0x12, 0x30, 0x01, 0x2e, 0xc9, 0x00, 0x02, 0x8c, 0x40, 0x40, 0x84, 0x41, 0x7a, 0x8c, 0x04, 0x0f, + 0x03, 0x2f, 0x01, 0x2e, 0x8b, 0x01, 0x19, 0xa4, 0x04, 0x2f, 0x2b, 0x2e, 0x82, 0x01, 0x98, 0x2e, 0xf3, 0x03, 0x12, + 0x30, 0x81, 0x90, 0x61, 0x52, 0x08, 0x2f, 0x65, 0x42, 0x65, 0x42, 0x43, 0x80, 0x39, 0x84, 0x82, 0x88, 0x05, 0x42, + 0x45, 0x42, 0x85, 0x42, 0x05, 0x43, 0x00, 0x2e, 0x80, 0x41, 0x00, 0x90, 0x90, 0x2e, 0xe1, 0xb4, 0x65, 0x54, 0xc1, + 0x6f, 0x80, 0x40, 0x00, 0xb2, 0x43, 0x58, 0x69, 0x50, 0x44, 0x2f, 0x55, 0x5c, 0xb7, 0x87, 0x8c, 0x0f, 0x0d, 0x2e, + 0x96, 0x01, 0xc4, 0x40, 0x36, 0x2f, 0x41, 0x56, 0x8b, 0x0e, 0x2a, 0x2f, 0x0b, 0x52, 0xa1, 0x0e, 0x0a, 0x2f, 0x05, + 0x2e, 0x8f, 0x01, 0x14, 0x25, 0x98, 0x2e, 0xfe, 0xc9, 0x4b, 0x54, 0x02, 0x0f, 0x69, 0x50, 0x05, 0x30, 0x65, 0x54, + 0x15, 0x2f, 0x03, 0x2e, 0x8e, 0x01, 0x4d, 0x5c, 0x8e, 0x0f, 0x3a, 0x2f, 0x05, 0x2e, 0x8f, 0x01, 0x98, 0x2e, 0xfe, + 0xc9, 0x4f, 0x54, 0x82, 0x0f, 0x05, 0x30, 0x69, 0x50, 0x65, 0x54, 0x30, 0x2f, 0x6d, 0x52, 0x15, 0x30, 0x42, 0x8c, + 0x45, 0x42, 0x04, 0x30, 0x2b, 0x2c, 0x84, 0x43, 0x6b, 0x52, 0x42, 0x8c, 0x00, 0x2e, 0x85, 0x43, 0x15, 0x30, 0x24, + 0x2c, 0x45, 0x42, 0x8e, 0x0f, 0x20, 0x2f, 0x0d, 0x2e, 0x8e, 0x01, 0xb1, 0x0e, 0x1c, 0x2f, 0x23, 0x2e, 0x8e, 0x01, + 0x1a, 0x2d, 0x0e, 0x0e, 0x17, 0x2f, 0xa1, 0x0f, 0x15, 0x2f, 0x23, 0x2e, 0x8d, 0x01, 0x13, 0x2d, 0x98, 0x2e, 0x74, + 0xc0, 0x43, 0x54, 0xc2, 0x0e, 0x0a, 0x2f, 0x65, 0x50, 0x04, 0x80, 0x0b, 0x30, 0x06, 0x82, 0x0b, 0x42, 0x79, 0x80, + 0x41, 0x40, 0x12, 0x30, 0x25, 0x2e, 0x8c, 0x01, 0x01, 0x42, 0x05, 0x30, 0x69, 0x50, 0x65, 0x54, 0x84, 0x82, 0x43, + 0x84, 0xbe, 0x8c, 0x84, 0x40, 0x86, 0x41, 0x26, 0x29, 0x94, 0x42, 0xbe, 0x8e, 0xd5, 0x7f, 0x19, 0xa1, 0x43, 0x40, + 0x0b, 0x2e, 0x8c, 0x01, 0x84, 0x40, 0xc7, 0x41, 0x5d, 0x29, 0x27, 0x29, 0x45, 0x42, 0x84, 0x42, 0xc2, 0x7f, 0x01, + 0x2f, 0xc0, 0xb3, 0x1d, 0x2f, 0x05, 0x2e, 0x94, 0x01, 0x99, 0xa0, 0x01, 0x2f, 0x80, 0xb3, 0x13, 0x2f, 0x80, 0xb3, + 0x18, 0x2f, 0xc0, 0xb3, 0x16, 0x2f, 0x12, 0x40, 0x01, 0x40, 0x92, 0x7f, 0x98, 0x2e, 0x74, 0xc0, 0x92, 0x6f, 0x10, + 0x0f, 0x20, 0x30, 0x03, 0x2f, 0x10, 0x30, 0x21, 0x2e, 0x7e, 0x01, 0x0a, 0x2d, 0x21, 0x2e, 0x7e, 0x01, 0x07, 0x2d, + 0x20, 0x30, 0x21, 0x2e, 0x7e, 0x01, 0x03, 0x2d, 0x10, 0x30, 0x21, 0x2e, 0x7e, 0x01, 0xc2, 0x6f, 0x01, 0x2e, 0xc9, + 0x00, 0xbc, 0x84, 0x02, 0x80, 0x82, 0x40, 0x00, 0x40, 0x90, 0x0e, 0xd5, 0x6f, 0x02, 0x2f, 0x15, 0x30, 0x98, 0x2e, + 0xf3, 0x03, 0x41, 0x91, 0x05, 0x30, 0x07, 0x2f, 0x67, 0x50, 0x3d, 0x80, 0x2b, 0x2e, 0x8f, 0x01, 0x05, 0x42, 0x04, + 0x80, 0x00, 0x2e, 0x05, 0x42, 0x02, 0x2c, 0x00, 0x30, 0x00, 0x30, 0xa2, 0x6f, 0x98, 0x8a, 0x86, 0x40, 0x80, 0xa7, + 0x05, 0x2f, 0x98, 0x2e, 0xf3, 0x03, 0xc0, 0x30, 0x21, 0x2e, 0x95, 0x01, 0x06, 0x25, 0x1a, 0x25, 0xe2, 0x6f, 0x76, + 0x82, 0x96, 0x40, 0x56, 0x43, 0x51, 0x0e, 0xfb, 0x2f, 0xbb, 0x6f, 0x30, 0x5f, 0xb8, 0x2e, 0x01, 0x2e, 0xb8, 0x00, + 0x01, 0x31, 0x41, 0x08, 0x40, 0xb2, 0x20, 0x50, 0xf2, 0x30, 0x02, 0x08, 0xfb, 0x7f, 0x01, 0x30, 0x10, 0x2f, 0x05, + 0x2e, 0xcc, 0x00, 0x81, 0x90, 0xe0, 0x7f, 0x03, 0x2f, 0x23, 0x2e, 0xcc, 0x00, 0x98, 0x2e, 0x55, 0xb6, 0x98, 0x2e, + 0x1d, 0xb5, 0x10, 0x25, 0xfb, 0x6f, 0xe0, 0x6f, 0xe0, 0x5f, 0x80, 0x2e, 0x95, 0xcf, 0x98, 0x2e, 0x95, 0xcf, 0x10, + 0x30, 0x21, 0x2e, 0xcc, 0x00, 0xfb, 0x6f, 0xe0, 0x5f, 0xb8, 0x2e, 0x00, 0x51, 0x05, 0x58, 0xeb, 0x7f, 0x2a, 0x25, + 0x89, 0x52, 0x6f, 0x5a, 0x89, 0x50, 0x13, 0x41, 0x06, 0x40, 0xb3, 0x01, 0x16, 0x42, 0xcb, 0x16, 0x06, 0x40, 0xf3, + 0x02, 0x13, 0x42, 0x65, 0x0e, 0xf5, 0x2f, 0x05, 0x40, 0x14, 0x30, 0x2c, 0x29, 0x04, 0x42, 0x08, 0xa1, 0x00, 0x30, + 0x90, 0x2e, 0x52, 0xb6, 0xb3, 0x88, 0xb0, 0x8a, 0xb6, 0x84, 0xa4, 0x7f, 0xc4, 0x7f, 0xb5, 0x7f, 0xd5, 0x7f, 0x92, + 0x7f, 0x73, 0x30, 0x04, 0x30, 0x55, 0x40, 0x42, 0x40, 0x8a, 0x17, 0xf3, 0x08, 0x6b, 0x01, 0x90, 0x02, 0x53, 0xb8, + 0x4b, 0x82, 0xad, 0xbe, 0x71, 0x7f, 0x45, 0x0a, 0x09, 0x54, 0x84, 0x7f, 0x98, 0x2e, 0xd9, 0xc0, 0xa3, 0x6f, 0x7b, + 0x54, 0xd0, 0x42, 0xa3, 0x7f, 0xf2, 0x7f, 0x60, 0x7f, 0x20, 0x25, 0x71, 0x6f, 0x75, 0x5a, 0x77, 0x58, 0x79, 0x5c, + 0x75, 0x56, 0x98, 0x2e, 0x67, 0xcc, 0xb1, 0x6f, 0x62, 0x6f, 0x50, 0x42, 0xb1, 0x7f, 0xb3, 0x30, 0x10, 0x25, 0x98, + 0x2e, 0x0f, 0xca, 0x84, 0x6f, 0x20, 0x29, 0x71, 0x6f, 0x92, 0x6f, 0xa5, 0x6f, 0x76, 0x82, 0x6a, 0x0e, 0x73, 0x30, + 0x00, 0x30, 0xd0, 0x2f, 0xd2, 0x6f, 0xd1, 0x7f, 0xb4, 0x7f, 0x98, 0x2e, 0x2b, 0xb7, 0x15, 0xbd, 0x0b, 0xb8, 0x02, + 0x0a, 0xc2, 0x6f, 0xc0, 0x7f, 0x98, 0x2e, 0x2b, 0xb7, 0x15, 0xbd, 0x0b, 0xb8, 0x42, 0x0a, 0xc0, 0x6f, 0x08, 0x17, + 0x41, 0x18, 0x89, 0x16, 0xe1, 0x18, 0xd0, 0x18, 0xa1, 0x7f, 0x27, 0x25, 0x16, 0x25, 0x98, 0x2e, 0x79, 0xc0, 0x8b, + 0x54, 0x90, 0x7f, 0xb3, 0x30, 0x82, 0x40, 0x80, 0x90, 0x0d, 0x2f, 0x7d, 0x52, 0x92, 0x6f, 0x98, 0x2e, 0x0f, 0xca, + 0xb2, 0x6f, 0x90, 0x0e, 0x06, 0x2f, 0x8b, 0x50, 0x14, 0x30, 0x42, 0x6f, 0x51, 0x6f, 0x14, 0x42, 0x12, 0x42, 0x01, + 0x42, 0x00, 0x2e, 0x31, 0x6f, 0x98, 0x2e, 0x74, 0xc0, 0x41, 0x6f, 0x80, 0x7f, 0x98, 0x2e, 0x74, 0xc0, 0x82, 0x6f, + 0x10, 0x04, 0x43, 0x52, 0x01, 0x0f, 0x05, 0x2e, 0xcb, 0x00, 0x00, 0x30, 0x04, 0x30, 0x21, 0x2f, 0x51, 0x6f, 0x43, + 0x58, 0x8c, 0x0e, 0x04, 0x30, 0x1c, 0x2f, 0x85, 0x88, 0x41, 0x6f, 0x04, 0x41, 0x8c, 0x0f, 0x04, 0x30, 0x16, 0x2f, + 0x84, 0x88, 0x00, 0x2e, 0x04, 0x41, 0x04, 0x05, 0x8c, 0x0e, 0x04, 0x30, 0x0f, 0x2f, 0x82, 0x88, 0x31, 0x6f, 0x04, + 0x41, 0x04, 0x05, 0x8c, 0x0e, 0x04, 0x30, 0x08, 0x2f, 0x83, 0x88, 0x00, 0x2e, 0x04, 0x41, 0x8c, 0x0f, 0x04, 0x30, + 0x02, 0x2f, 0x21, 0x2e, 0xad, 0x01, 0x14, 0x30, 0x00, 0x91, 0x14, 0x2f, 0x03, 0x2e, 0xa1, 0x01, 0x41, 0x90, 0x0e, + 0x2f, 0x03, 0x2e, 0xad, 0x01, 0x14, 0x30, 0x4c, 0x28, 0x23, 0x2e, 0xad, 0x01, 0x46, 0xa0, 0x06, 0x2f, 0x81, 0x84, + 0x8d, 0x52, 0x48, 0x82, 0x82, 0x40, 0x21, 0x2e, 0xa1, 0x01, 0x42, 0x42, 0x5c, 0x2c, 0x02, 0x30, 0x05, 0x2e, 0xaa, + 0x01, 0x80, 0xb2, 0x02, 0x30, 0x55, 0x2f, 0x03, 0x2e, 0xa9, 0x01, 0x92, 0x6f, 0xb3, 0x30, 0x98, 0x2e, 0x0f, 0xca, + 0xb2, 0x6f, 0x90, 0x0f, 0x00, 0x30, 0x02, 0x30, 0x4a, 0x2f, 0xa2, 0x6f, 0x87, 0x52, 0x91, 0x00, 0x85, 0x52, 0x51, + 0x0e, 0x02, 0x2f, 0x00, 0x2e, 0x43, 0x2c, 0x02, 0x30, 0xc2, 0x6f, 0x7f, 0x52, 0x91, 0x0e, 0x02, 0x30, 0x3c, 0x2f, + 0x51, 0x6f, 0x81, 0x54, 0x98, 0x2e, 0xfe, 0xc9, 0x10, 0x25, 0xb3, 0x30, 0x21, 0x25, 0x98, 0x2e, 0x0f, 0xca, 0x32, + 0x6f, 0xc0, 0x7f, 0xb3, 0x30, 0x12, 0x25, 0x98, 0x2e, 0x0f, 0xca, 0x42, 0x6f, 0xb0, 0x7f, 0xb3, 0x30, 0x12, 0x25, + 0x98, 0x2e, 0x0f, 0xca, 0xb2, 0x6f, 0x90, 0x28, 0x83, 0x52, 0x98, 0x2e, 0xfe, 0xc9, 0xc2, 0x6f, 0x90, 0x0f, 0x00, + 0x30, 0x02, 0x30, 0x1d, 0x2f, 0x05, 0x2e, 0xa1, 0x01, 0x80, 0xb2, 0x12, 0x30, 0x0f, 0x2f, 0x42, 0x6f, 0x03, 0x2e, + 0xab, 0x01, 0x91, 0x0e, 0x02, 0x30, 0x12, 0x2f, 0x52, 0x6f, 0x03, 0x2e, 0xac, 0x01, 0x91, 0x0f, 0x02, 0x30, 0x0c, + 0x2f, 0x21, 0x2e, 0xaa, 0x01, 0x0a, 0x2c, 0x12, 0x30, 0x03, 0x2e, 0xcb, 0x00, 0x8d, 0x58, 0x08, 0x89, 0x41, 0x40, + 0x11, 0x43, 0x00, 0x43, 0x25, 0x2e, 0xa1, 0x01, 0xd4, 0x6f, 0x8f, 0x52, 0x00, 0x43, 0x3a, 0x89, 0x00, 0x2e, 0x10, + 0x43, 0x10, 0x43, 0x61, 0x0e, 0xfb, 0x2f, 0x03, 0x2e, 0xa0, 0x01, 0x11, 0x1a, 0x02, 0x2f, 0x02, 0x25, 0x21, 0x2e, + 0xa0, 0x01, 0xeb, 0x6f, 0x00, 0x5f, 0xb8, 0x2e, 0x91, 0x52, 0x10, 0x30, 0x02, 0x30, 0x95, 0x56, 0x52, 0x42, 0x4b, + 0x0e, 0xfc, 0x2f, 0x8d, 0x54, 0x88, 0x82, 0x93, 0x56, 0x80, 0x42, 0x53, 0x42, 0x40, 0x42, 0x42, 0x86, 0x83, 0x54, + 0xc0, 0x2e, 0xc2, 0x42, 0x00, 0x2e, 0xa3, 0x52, 0x00, 0x51, 0x52, 0x40, 0x47, 0x40, 0x1a, 0x25, 0x01, 0x2e, 0x97, + 0x00, 0x8f, 0xbe, 0x72, 0x86, 0xfb, 0x7f, 0x0b, 0x30, 0x7c, 0xbf, 0xa5, 0x50, 0x10, 0x08, 0xdf, 0xba, 0x70, 0x88, + 0xf8, 0xbf, 0xcb, 0x42, 0xd3, 0x7f, 0x6c, 0xbb, 0xfc, 0xbb, 0xc5, 0x0a, 0x90, 0x7f, 0x1b, 0x7f, 0x0b, 0x43, 0xc0, + 0xb2, 0xe5, 0x7f, 0xb7, 0x7f, 0xa6, 0x7f, 0xc4, 0x7f, 0x90, 0x2e, 0x1c, 0xb7, 0x07, 0x2e, 0xd2, 0x00, 0xc0, 0xb2, + 0x0b, 0x2f, 0x97, 0x52, 0x01, 0x2e, 0xcd, 0x00, 0x82, 0x7f, 0x98, 0x2e, 0xbb, 0xcc, 0x0b, 0x30, 0x37, 0x2e, 0xd2, + 0x00, 0x82, 0x6f, 0x90, 0x6f, 0x1a, 0x25, 0x00, 0xb2, 0x8b, 0x7f, 0x14, 0x2f, 0xa6, 0xbd, 0x25, 0xbd, 0xb6, 0xb9, + 0x2f, 0xb9, 0x80, 0xb2, 0xd4, 0xb0, 0x0c, 0x2f, 0x99, 0x54, 0x9b, 0x56, 0x0b, 0x30, 0x0b, 0x2e, 0xb1, 0x00, 0xa1, + 0x58, 0x9b, 0x42, 0xdb, 0x42, 0x6c, 0x09, 0x2b, 0x2e, 0xb1, 0x00, 0x8b, 0x42, 0xcb, 0x42, 0x86, 0x7f, 0x73, 0x84, + 0xa7, 0x56, 0xc3, 0x08, 0x39, 0x52, 0x05, 0x50, 0x72, 0x7f, 0x63, 0x7f, 0x98, 0x2e, 0xc2, 0xc0, 0xe1, 0x6f, 0x62, + 0x6f, 0xd1, 0x0a, 0x01, 0x2e, 0xcd, 0x00, 0xd5, 0x6f, 0xc4, 0x6f, 0x72, 0x6f, 0x97, 0x52, 0x9d, 0x5c, 0x98, 0x2e, + 0x06, 0xcd, 0x23, 0x6f, 0x90, 0x6f, 0x99, 0x52, 0xc0, 0xb2, 0x04, 0xbd, 0x54, 0x40, 0xaf, 0xb9, 0x45, 0x40, 0xe1, + 0x7f, 0x02, 0x30, 0x06, 0x2f, 0xc0, 0xb2, 0x02, 0x30, 0x03, 0x2f, 0x9b, 0x5c, 0x12, 0x30, 0x94, 0x43, 0x85, 0x43, + 0x03, 0xbf, 0x6f, 0xbb, 0x80, 0xb3, 0x20, 0x2f, 0x06, 0x6f, 0x26, 0x01, 0x16, 0x6f, 0x6e, 0x03, 0x45, 0x42, 0xc0, + 0x90, 0x29, 0x2e, 0xce, 0x00, 0x9b, 0x52, 0x14, 0x2f, 0x9b, 0x5c, 0x00, 0x2e, 0x93, 0x41, 0x86, 0x41, 0xe3, 0x04, + 0xae, 0x07, 0x80, 0xab, 0x04, 0x2f, 0x80, 0x91, 0x0a, 0x2f, 0x86, 0x6f, 0x73, 0x0f, 0x07, 0x2f, 0x83, 0x6f, 0xc0, + 0xb2, 0x04, 0x2f, 0x54, 0x42, 0x45, 0x42, 0x12, 0x30, 0x04, 0x2c, 0x11, 0x30, 0x02, 0x2c, 0x11, 0x30, 0x11, 0x30, + 0x02, 0xbc, 0x0f, 0xb8, 0xd2, 0x7f, 0x00, 0xb2, 0x0a, 0x2f, 0x01, 0x2e, 0xfc, 0x00, 0x05, 0x2e, 0xc7, 0x01, 0x10, + 0x1a, 0x02, 0x2f, 0x21, 0x2e, 0xc7, 0x01, 0x03, 0x2d, 0x02, 0x2c, 0x01, 0x30, 0x01, 0x30, 0xb0, 0x6f, 0x98, 0x2e, + 0x95, 0xcf, 0xd1, 0x6f, 0xa0, 0x6f, 0x98, 0x2e, 0x95, 0xcf, 0xe2, 0x6f, 0x9f, 0x52, 0x01, 0x2e, 0xce, 0x00, 0x82, + 0x40, 0x50, 0x42, 0x0c, 0x2c, 0x42, 0x42, 0x11, 0x30, 0x23, 0x2e, 0xd2, 0x00, 0x01, 0x30, 0xb0, 0x6f, 0x98, 0x2e, + 0x95, 0xcf, 0xa0, 0x6f, 0x01, 0x30, 0x98, 0x2e, 0x95, 0xcf, 0x00, 0x2e, 0xfb, 0x6f, 0x00, 0x5f, 0xb8, 0x2e, 0x83, + 0x86, 0x01, 0x30, 0x00, 0x30, 0x94, 0x40, 0x24, 0x18, 0x06, 0x00, 0x53, 0x0e, 0x4f, 0x02, 0xf9, 0x2f, 0xb8, 0x2e, + 0xa9, 0x52, 0x00, 0x2e, 0x60, 0x40, 0x41, 0x40, 0x0d, 0xbc, 0x98, 0xbc, 0xc0, 0x2e, 0x01, 0x0a, 0x0f, 0xb8, 0xab, + 0x52, 0x53, 0x3c, 0x52, 0x40, 0x40, 0x40, 0x4b, 0x00, 0x82, 0x16, 0x26, 0xb9, 0x01, 0xb8, 0x41, 0x40, 0x10, 0x08, + 0x97, 0xb8, 0x01, 0x08, 0xc0, 0x2e, 0x11, 0x30, 0x01, 0x08, 0x43, 0x86, 0x25, 0x40, 0x04, 0x40, 0xd8, 0xbe, 0x2c, + 0x0b, 0x22, 0x11, 0x54, 0x42, 0x03, 0x80, 0x4b, 0x0e, 0xf6, 0x2f, 0xb8, 0x2e, 0x9f, 0x50, 0x10, 0x50, 0xad, 0x52, + 0x05, 0x2e, 0xd3, 0x00, 0xfb, 0x7f, 0x00, 0x2e, 0x13, 0x40, 0x93, 0x42, 0x41, 0x0e, 0xfb, 0x2f, 0x98, 0x2e, 0xa5, + 0xb7, 0x98, 0x2e, 0x87, 0xcf, 0x01, 0x2e, 0xd9, 0x00, 0x00, 0xb2, 0xfb, 0x6f, 0x0b, 0x2f, 0x01, 0x2e, 0x69, 0xf7, + 0xb1, 0x3f, 0x01, 0x08, 0x01, 0x30, 0xf0, 0x5f, 0x23, 0x2e, 0xd9, 0x00, 0x21, 0x2e, 0x69, 0xf7, 0x80, 0x2e, 0x7a, + 0xb7, 0xf0, 0x5f, 0xb8, 0x2e, 0x01, 0x2e, 0xc0, 0xf8, 0x03, 0x2e, 0xfc, 0xf5, 0x15, 0x54, 0xaf, 0x56, 0x82, 0x08, + 0x0b, 0x2e, 0x69, 0xf7, 0xcb, 0x0a, 0xb1, 0x58, 0x80, 0x90, 0xdd, 0xbe, 0x4c, 0x08, 0x5f, 0xb9, 0x59, 0x22, 0x80, + 0x90, 0x07, 0x2f, 0x03, 0x34, 0xc3, 0x08, 0xf2, 0x3a, 0x0a, 0x08, 0x02, 0x35, 0xc0, 0x90, 0x4a, 0x0a, 0x48, 0x22, + 0xc0, 0x2e, 0x23, 0x2e, 0xfc, 0xf5, 0x10, 0x50, 0xfb, 0x7f, 0x98, 0x2e, 0x56, 0xc7, 0x98, 0x2e, 0x49, 0xc3, 0x10, + 0x30, 0xfb, 0x6f, 0xf0, 0x5f, 0x21, 0x2e, 0xcc, 0x00, 0x21, 0x2e, 0xca, 0x00, 0xb8, 0x2e, 0x03, 0x2e, 0xd3, 0x00, + 0x16, 0xb8, 0x02, 0x34, 0x4a, 0x0c, 0x21, 0x2e, 0x2d, 0xf5, 0xc0, 0x2e, 0x23, 0x2e, 0xd3, 0x00, 0x03, 0xbc, 0x21, + 0x2e, 0xd5, 0x00, 0x03, 0x2e, 0xd5, 0x00, 0x40, 0xb2, 0x10, 0x30, 0x21, 0x2e, 0x77, 0x00, 0x01, 0x30, 0x05, 0x2f, + 0x05, 0x2e, 0xd8, 0x00, 0x80, 0x90, 0x01, 0x2f, 0x23, 0x2e, 0x6f, 0xf5, 0xc0, 0x2e, 0x21, 0x2e, 0xd9, 0x00, 0x11, + 0x30, 0x81, 0x08, 0x01, 0x2e, 0x6a, 0xf7, 0x71, 0x3f, 0x23, 0xbd, 0x01, 0x08, 0x02, 0x0a, 0xc0, 0x2e, 0x21, 0x2e, + 0x6a, 0xf7, 0x30, 0x25, 0x00, 0x30, 0x21, 0x2e, 0x5a, 0xf5, 0x10, 0x50, 0x21, 0x2e, 0x7b, 0x00, 0x21, 0x2e, 0x7c, + 0x00, 0xfb, 0x7f, 0x98, 0x2e, 0xc3, 0xb7, 0x40, 0x30, 0x21, 0x2e, 0xd4, 0x00, 0xfb, 0x6f, 0xf0, 0x5f, 0x03, 0x25, + 0x80, 0x2e, 0xaf, 0xb7, 0x80, 0x2e, 0x00, 0xc1, 0x80, 0x2e, 0x00, 0xc1, 0x80, 0x2e, 0x00, 0xc1, 0x80, 0x2e, 0x00, + 0xc1, 0x80, 0x2e, 0x00, 0xc1, 0x80, 0x2e, 0x00, 0xc1, 0x80, 0x2e, 0x00, 0xc1, 0x80, 0x2e, 0x00, 0xc1, 0x80, 0x2e, + 0x00, 0xc1, 0x80, 0x2e, 0x00, 0xc1, 0x80, 0x2e, 0x00, 0xc1, 0x80, 0x2e, 0x00, 0xc1, 0x80, 0x2e, 0x00, 0xc1, 0x80, + 0x2e, 0x00, 0xc1, 0x80, 0x2e, 0x00, 0xc1, 0x01, 0x2e, 0x5d, 0xf7, 0x08, 0xbc, 0x80, 0xac, 0x0e, 0xbb, 0x02, 0x2f, + 0x00, 0x30, 0x41, 0x04, 0x82, 0x06, 0xc0, 0xa4, 0x00, 0x30, 0x11, 0x2f, 0x40, 0xa9, 0x03, 0x2f, 0x40, 0x91, 0x0d, + 0x2f, 0x00, 0xa7, 0x0b, 0x2f, 0x80, 0xb3, 0xb3, 0x58, 0x02, 0x2f, 0x90, 0xa1, 0x26, 0x13, 0x20, 0x23, 0x80, 0x90, + 0x10, 0x30, 0x01, 0x2f, 0xcc, 0x0e, 0x00, 0x2f, 0x00, 0x30, 0xb8, 0x2e, 0xb5, 0x50, 0x18, 0x08, 0x08, 0xbc, 0x88, + 0xb6, 0x0d, 0x17, 0xc6, 0xbd, 0x56, 0xbc, 0xb7, 0x58, 0xda, 0xba, 0x04, 0x01, 0x1d, 0x0a, 0x10, 0x50, 0x05, 0x30, + 0x32, 0x25, 0x45, 0x03, 0xfb, 0x7f, 0xf6, 0x30, 0x21, 0x25, 0x98, 0x2e, 0x37, 0xca, 0x16, 0xb5, 0x9a, 0xbc, 0x06, + 0xb8, 0x80, 0xa8, 0x41, 0x0a, 0x0e, 0x2f, 0x80, 0x90, 0x02, 0x2f, 0x2d, 0x50, 0x48, 0x0f, 0x09, 0x2f, 0xbf, 0xa0, + 0x04, 0x2f, 0xbf, 0x90, 0x06, 0x2f, 0xb7, 0x54, 0xca, 0x0f, 0x03, 0x2f, 0x00, 0x2e, 0x02, 0x2c, 0xb7, 0x52, 0x2d, + 0x52, 0xf2, 0x33, 0x98, 0x2e, 0xd9, 0xc0, 0xfb, 0x6f, 0xf1, 0x37, 0xc0, 0x2e, 0x01, 0x08, 0xf0, 0x5f, 0xbf, 0x56, + 0xb9, 0x54, 0xd0, 0x40, 0xc4, 0x40, 0x0b, 0x2e, 0xfd, 0xf3, 0xbf, 0x52, 0x90, 0x42, 0x94, 0x42, 0x95, 0x42, 0x05, + 0x30, 0xc1, 0x50, 0x0f, 0x88, 0x06, 0x40, 0x04, 0x41, 0x96, 0x42, 0xc5, 0x42, 0x48, 0xbe, 0x73, 0x30, 0x0d, 0x2e, + 0xd8, 0x00, 0x4f, 0xba, 0x84, 0x42, 0x03, 0x42, 0x81, 0xb3, 0x02, 0x2f, 0x2b, 0x2e, 0x6f, 0xf5, 0x06, 0x2d, 0x05, + 0x2e, 0x77, 0xf7, 0xbd, 0x56, 0x93, 0x08, 0x25, 0x2e, 0x77, 0xf7, 0xbb, 0x54, 0x25, 0x2e, 0xc2, 0xf5, 0x07, 0x2e, + 0xfd, 0xf3, 0x42, 0x30, 0xb4, 0x33, 0xda, 0x0a, 0x4c, 0x00, 0x27, 0x2e, 0xfd, 0xf3, 0x43, 0x40, 0xd4, 0x3f, 0xdc, + 0x08, 0x43, 0x42, 0x00, 0x2e, 0x00, 0x2e, 0x43, 0x40, 0x24, 0x30, 0xdc, 0x0a, 0x43, 0x42, 0x04, 0x80, 0x03, 0x2e, + 0xfd, 0xf3, 0x4a, 0x0a, 0x23, 0x2e, 0xfd, 0xf3, 0x61, 0x34, 0xc0, 0x2e, 0x01, 0x42, 0x00, 0x2e, 0x60, 0x50, 0x1a, + 0x25, 0x7a, 0x86, 0xe0, 0x7f, 0xf3, 0x7f, 0x03, 0x25, 0xc3, 0x52, 0x41, 0x84, 0xdb, 0x7f, 0x33, 0x30, 0x98, 0x2e, + 0x16, 0xc2, 0x1a, 0x25, 0x7d, 0x82, 0xf0, 0x6f, 0xe2, 0x6f, 0x32, 0x25, 0x16, 0x40, 0x94, 0x40, 0x26, 0x01, 0x85, + 0x40, 0x8e, 0x17, 0xc4, 0x42, 0x6e, 0x03, 0x95, 0x42, 0x41, 0x0e, 0xf4, 0x2f, 0xdb, 0x6f, 0xa0, 0x5f, 0xb8, 0x2e, + 0xb0, 0x51, 0xfb, 0x7f, 0x98, 0x2e, 0xe8, 0x0d, 0x5a, 0x25, 0x98, 0x2e, 0x0f, 0x0e, 0xcb, 0x58, 0x32, 0x87, 0xc4, + 0x7f, 0x65, 0x89, 0x6b, 0x8d, 0xc5, 0x5a, 0x65, 0x7f, 0xe1, 0x7f, 0x83, 0x7f, 0xa6, 0x7f, 0x74, 0x7f, 0xd0, 0x7f, + 0xb6, 0x7f, 0x94, 0x7f, 0x17, 0x30, 0xc7, 0x52, 0xc9, 0x54, 0x51, 0x7f, 0x00, 0x2e, 0x85, 0x6f, 0x42, 0x7f, 0x00, + 0x2e, 0x51, 0x41, 0x45, 0x81, 0x42, 0x41, 0x13, 0x40, 0x3b, 0x8a, 0x00, 0x40, 0x4b, 0x04, 0xd0, 0x06, 0xc0, 0xac, + 0x85, 0x7f, 0x02, 0x2f, 0x02, 0x30, 0x51, 0x04, 0xd3, 0x06, 0x41, 0x84, 0x05, 0x30, 0x5d, 0x02, 0xc9, 0x16, 0xdf, + 0x08, 0xd3, 0x00, 0x8d, 0x02, 0xaf, 0xbc, 0xb1, 0xb9, 0x59, 0x0a, 0x65, 0x6f, 0x11, 0x43, 0xa1, 0xb4, 0x52, 0x41, + 0x53, 0x41, 0x01, 0x43, 0x34, 0x7f, 0x65, 0x7f, 0x26, 0x31, 0xe5, 0x6f, 0xd4, 0x6f, 0x98, 0x2e, 0x37, 0xca, 0x32, + 0x6f, 0x75, 0x6f, 0x83, 0x40, 0x42, 0x41, 0x23, 0x7f, 0x12, 0x7f, 0xf6, 0x30, 0x40, 0x25, 0x51, 0x25, 0x98, 0x2e, + 0x37, 0xca, 0x14, 0x6f, 0x20, 0x05, 0x70, 0x6f, 0x25, 0x6f, 0x69, 0x07, 0xa2, 0x6f, 0x31, 0x6f, 0x0b, 0x30, 0x04, + 0x42, 0x9b, 0x42, 0x8b, 0x42, 0x55, 0x42, 0x32, 0x7f, 0x40, 0xa9, 0xc3, 0x6f, 0x71, 0x7f, 0x02, 0x30, 0xd0, 0x40, + 0xc3, 0x7f, 0x03, 0x2f, 0x40, 0x91, 0x15, 0x2f, 0x00, 0xa7, 0x13, 0x2f, 0x00, 0xa4, 0x11, 0x2f, 0x84, 0xbd, 0x98, + 0x2e, 0x79, 0xca, 0x55, 0x6f, 0xb7, 0x54, 0x54, 0x41, 0x82, 0x00, 0xf3, 0x3f, 0x45, 0x41, 0xcb, 0x02, 0xf6, 0x30, + 0x98, 0x2e, 0x37, 0xca, 0x35, 0x6f, 0xa4, 0x6f, 0x41, 0x43, 0x03, 0x2c, 0x00, 0x43, 0xa4, 0x6f, 0x35, 0x6f, 0x17, + 0x30, 0x42, 0x6f, 0x51, 0x6f, 0x93, 0x40, 0x42, 0x82, 0x00, 0x41, 0xc3, 0x00, 0x03, 0x43, 0x51, 0x7f, 0x00, 0x2e, + 0x94, 0x40, 0x41, 0x41, 0x4c, 0x02, 0xc4, 0x6f, 0xd1, 0x56, 0x63, 0x0e, 0x74, 0x6f, 0x51, 0x43, 0xa5, 0x7f, 0x8a, + 0x2f, 0x09, 0x2e, 0xd8, 0x00, 0x01, 0xb3, 0x21, 0x2f, 0xcb, 0x58, 0x90, 0x6f, 0x13, 0x41, 0xb6, 0x6f, 0xe4, 0x7f, + 0x00, 0x2e, 0x91, 0x41, 0x14, 0x40, 0x92, 0x41, 0x15, 0x40, 0x17, 0x2e, 0x6f, 0xf5, 0xb6, 0x7f, 0xd0, 0x7f, 0xcb, + 0x7f, 0x98, 0x2e, 0x00, 0x0c, 0x07, 0x15, 0xc2, 0x6f, 0x14, 0x0b, 0x29, 0x2e, 0x6f, 0xf5, 0xc3, 0xa3, 0xc1, 0x8f, + 0xe4, 0x6f, 0xd0, 0x6f, 0xe6, 0x2f, 0x14, 0x30, 0x05, 0x2e, 0x6f, 0xf5, 0x14, 0x0b, 0x29, 0x2e, 0x6f, 0xf5, 0x18, + 0x2d, 0xcd, 0x56, 0x04, 0x32, 0xb5, 0x6f, 0x1c, 0x01, 0x51, 0x41, 0x52, 0x41, 0xc3, 0x40, 0xb5, 0x7f, 0xe4, 0x7f, + 0x98, 0x2e, 0x1f, 0x0c, 0xe4, 0x6f, 0x21, 0x87, 0x00, 0x43, 0x04, 0x32, 0xcf, 0x54, 0x5a, 0x0e, 0xef, 0x2f, 0x15, + 0x54, 0x09, 0x2e, 0x77, 0xf7, 0x22, 0x0b, 0x29, 0x2e, 0x77, 0xf7, 0xfb, 0x6f, 0x50, 0x5e, 0xb8, 0x2e, 0x10, 0x50, + 0x01, 0x2e, 0xd4, 0x00, 0x00, 0xb2, 0xfb, 0x7f, 0x51, 0x2f, 0x01, 0xb2, 0x48, 0x2f, 0x02, 0xb2, 0x42, 0x2f, 0x03, + 0x90, 0x56, 0x2f, 0xd7, 0x52, 0x79, 0x80, 0x42, 0x40, 0x81, 0x84, 0x00, 0x40, 0x42, 0x42, 0x98, 0x2e, 0x93, 0x0c, + 0xd9, 0x54, 0xd7, 0x50, 0xa1, 0x40, 0x98, 0xbd, 0x82, 0x40, 0x3e, 0x82, 0xda, 0x0a, 0x44, 0x40, 0x8b, 0x16, 0xe3, + 0x00, 0x53, 0x42, 0x00, 0x2e, 0x43, 0x40, 0x9a, 0x02, 0x52, 0x42, 0x00, 0x2e, 0x41, 0x40, 0x15, 0x54, 0x4a, 0x0e, + 0x3a, 0x2f, 0x3a, 0x82, 0x00, 0x30, 0x41, 0x40, 0x21, 0x2e, 0x85, 0x0f, 0x40, 0xb2, 0x0a, 0x2f, 0x98, 0x2e, 0xb1, + 0x0c, 0x98, 0x2e, 0x45, 0x0e, 0x98, 0x2e, 0x5b, 0x0e, 0xfb, 0x6f, 0xf0, 0x5f, 0x00, 0x30, 0x80, 0x2e, 0xce, 0xb7, + 0xdd, 0x52, 0xd3, 0x54, 0x42, 0x42, 0x4f, 0x84, 0x73, 0x30, 0xdb, 0x52, 0x83, 0x42, 0x1b, 0x30, 0x6b, 0x42, 0x23, + 0x30, 0x27, 0x2e, 0xd7, 0x00, 0x37, 0x2e, 0xd4, 0x00, 0x21, 0x2e, 0xd6, 0x00, 0x7a, 0x84, 0x17, 0x2c, 0x42, 0x42, + 0x30, 0x30, 0x21, 0x2e, 0xd4, 0x00, 0x12, 0x2d, 0x21, 0x30, 0x00, 0x30, 0x23, 0x2e, 0xd4, 0x00, 0x21, 0x2e, 0x7b, + 0xf7, 0x0b, 0x2d, 0x17, 0x30, 0x98, 0x2e, 0x51, 0x0c, 0xd5, 0x50, 0x0c, 0x82, 0x72, 0x30, 0x2f, 0x2e, 0xd4, 0x00, + 0x25, 0x2e, 0x7b, 0xf7, 0x40, 0x42, 0x00, 0x2e, 0xfb, 0x6f, 0xf0, 0x5f, 0xb8, 0x2e, 0x70, 0x50, 0x0a, 0x25, 0x39, + 0x86, 0xfb, 0x7f, 0xe1, 0x32, 0x62, 0x30, 0x98, 0x2e, 0xc2, 0xc4, 0xb5, 0x56, 0xa5, 0x6f, 0xab, 0x08, 0x91, 0x6f, + 0x4b, 0x08, 0xdf, 0x56, 0xc4, 0x6f, 0x23, 0x09, 0x4d, 0xba, 0x93, 0xbc, 0x8c, 0x0b, 0xd1, 0x6f, 0x0b, 0x09, 0xcb, + 0x52, 0xe1, 0x5e, 0x56, 0x42, 0xaf, 0x09, 0x4d, 0xba, 0x23, 0xbd, 0x94, 0x0a, 0xe5, 0x6f, 0x68, 0xbb, 0xeb, 0x08, + 0xbd, 0xb9, 0x63, 0xbe, 0xfb, 0x6f, 0x52, 0x42, 0xe3, 0x0a, 0xc0, 0x2e, 0x43, 0x42, 0x90, 0x5f, 0xd1, 0x50, 0x03, + 0x2e, 0x25, 0xf3, 0x13, 0x40, 0x00, 0x40, 0x9b, 0xbc, 0x9b, 0xb4, 0x08, 0xbd, 0xb8, 0xb9, 0x98, 0xbc, 0xda, 0x0a, + 0x08, 0xb6, 0x89, 0x16, 0xc0, 0x2e, 0x19, 0x00, 0x62, 0x02, 0x10, 0x50, 0xfb, 0x7f, 0x98, 0x2e, 0x81, 0x0d, 0x01, + 0x2e, 0xd4, 0x00, 0x31, 0x30, 0x08, 0x04, 0xfb, 0x6f, 0x01, 0x30, 0xf0, 0x5f, 0x23, 0x2e, 0xd6, 0x00, 0x21, 0x2e, + 0xd7, 0x00, 0xb8, 0x2e, 0x01, 0x2e, 0xd7, 0x00, 0x03, 0x2e, 0xd6, 0x00, 0x48, 0x0e, 0x01, 0x2f, 0x80, 0x2e, 0x1f, + 0x0e, 0xb8, 0x2e, 0xe3, 0x50, 0x21, 0x34, 0x01, 0x42, 0x82, 0x30, 0xc1, 0x32, 0x25, 0x2e, 0x62, 0xf5, 0x01, 0x00, + 0x22, 0x30, 0x01, 0x40, 0x4a, 0x0a, 0x01, 0x42, 0xb8, 0x2e, 0xe3, 0x54, 0xf0, 0x3b, 0x83, 0x40, 0xd8, 0x08, 0xe5, + 0x52, 0x83, 0x42, 0x00, 0x30, 0x83, 0x30, 0x50, 0x42, 0xc4, 0x32, 0x27, 0x2e, 0x64, 0xf5, 0x94, 0x00, 0x50, 0x42, + 0x40, 0x42, 0xd3, 0x3f, 0x84, 0x40, 0x7d, 0x82, 0xe3, 0x08, 0x40, 0x42, 0x83, 0x42, 0xb8, 0x2e, 0xdd, 0x52, 0x00, + 0x30, 0x40, 0x42, 0x7c, 0x86, 0xb9, 0x52, 0x09, 0x2e, 0x70, 0x0f, 0xbf, 0x54, 0xc4, 0x42, 0xd3, 0x86, 0x54, 0x40, + 0x55, 0x40, 0x94, 0x42, 0x85, 0x42, 0x21, 0x2e, 0xd7, 0x00, 0x42, 0x40, 0x25, 0x2e, 0xfd, 0xf3, 0xc0, 0x42, 0x7e, + 0x82, 0x05, 0x2e, 0x7d, 0x00, 0x80, 0xb2, 0x14, 0x2f, 0x05, 0x2e, 0x89, 0x00, 0x27, 0xbd, 0x2f, 0xb9, 0x80, 0x90, + 0x02, 0x2f, 0x21, 0x2e, 0x6f, 0xf5, 0x0c, 0x2d, 0x07, 0x2e, 0x71, 0x0f, 0x14, 0x30, 0x1c, 0x09, 0x05, 0x2e, 0x77, + 0xf7, 0xbd, 0x56, 0x47, 0xbe, 0x93, 0x08, 0x94, 0x0a, 0x25, 0x2e, 0x77, 0xf7, 0xe7, 0x54, 0x50, 0x42, 0x4a, 0x0e, + 0xfc, 0x2f, 0xb8, 0x2e, 0x50, 0x50, 0x02, 0x30, 0x43, 0x86, 0xe5, 0x50, 0xfb, 0x7f, 0xe3, 0x7f, 0xd2, 0x7f, 0xc0, + 0x7f, 0xb1, 0x7f, 0x00, 0x2e, 0x41, 0x40, 0x00, 0x40, 0x48, 0x04, 0x98, 0x2e, 0x74, 0xc0, 0x1e, 0xaa, 0xd3, 0x6f, + 0x14, 0x30, 0xb1, 0x6f, 0xe3, 0x22, 0xc0, 0x6f, 0x52, 0x40, 0xe4, 0x6f, 0x4c, 0x0e, 0x12, 0x42, 0xd3, 0x7f, 0xeb, + 0x2f, 0x03, 0x2e, 0x86, 0x0f, 0x40, 0x90, 0x11, 0x30, 0x03, 0x2f, 0x23, 0x2e, 0x86, 0x0f, 0x02, 0x2c, 0x00, 0x30, + 0xd0, 0x6f, 0xfb, 0x6f, 0xb0, 0x5f, 0xb8, 0x2e, 0x40, 0x50, 0xf1, 0x7f, 0x0a, 0x25, 0x3c, 0x86, 0xeb, 0x7f, 0x41, + 0x33, 0x22, 0x30, 0x98, 0x2e, 0xc2, 0xc4, 0xd3, 0x6f, 0xf4, 0x30, 0xdc, 0x09, 0x47, 0x58, 0xc2, 0x6f, 0x94, 0x09, + 0xeb, 0x58, 0x6a, 0xbb, 0xdc, 0x08, 0xb4, 0xb9, 0xb1, 0xbd, 0xe9, 0x5a, 0x95, 0x08, 0x21, 0xbd, 0xf6, 0xbf, 0x77, + 0x0b, 0x51, 0xbe, 0xf1, 0x6f, 0xeb, 0x6f, 0x52, 0x42, 0x54, 0x42, 0xc0, 0x2e, 0x43, 0x42, 0xc0, 0x5f, 0x50, 0x50, + 0xf5, 0x50, 0x31, 0x30, 0x11, 0x42, 0xfb, 0x7f, 0x7b, 0x30, 0x0b, 0x42, 0x11, 0x30, 0x02, 0x80, 0x23, 0x33, 0x01, + 0x42, 0x03, 0x00, 0x07, 0x2e, 0x80, 0x03, 0x05, 0x2e, 0xd3, 0x00, 0x23, 0x52, 0xe2, 0x7f, 0xd3, 0x7f, 0xc0, 0x7f, + 0x98, 0x2e, 0xb6, 0x0e, 0xd1, 0x6f, 0x08, 0x0a, 0x1a, 0x25, 0x7b, 0x86, 0xd0, 0x7f, 0x01, 0x33, 0x12, 0x30, 0x98, + 0x2e, 0xc2, 0xc4, 0xd1, 0x6f, 0x08, 0x0a, 0x00, 0xb2, 0x0d, 0x2f, 0xe3, 0x6f, 0x01, 0x2e, 0x80, 0x03, 0x51, 0x30, + 0xc7, 0x86, 0x23, 0x2e, 0x21, 0xf2, 0x08, 0xbc, 0xc0, 0x42, 0x98, 0x2e, 0xa5, 0xb7, 0x00, 0x2e, 0x00, 0x2e, 0xd0, + 0x2e, 0xb0, 0x6f, 0x0b, 0xb8, 0x03, 0x2e, 0x1b, 0x00, 0x08, 0x1a, 0xb0, 0x7f, 0x70, 0x30, 0x04, 0x2f, 0x21, 0x2e, + 0x21, 0xf2, 0x00, 0x2e, 0x00, 0x2e, 0xd0, 0x2e, 0x98, 0x2e, 0x6d, 0xc0, 0x98, 0x2e, 0x5d, 0xc0, 0xed, 0x50, 0x98, + 0x2e, 0x44, 0xcb, 0xef, 0x50, 0x98, 0x2e, 0x46, 0xc3, 0xf1, 0x50, 0x98, 0x2e, 0x53, 0xc7, 0x35, 0x50, 0x98, 0x2e, + 0x64, 0xcf, 0x10, 0x30, 0x98, 0x2e, 0xdc, 0x03, 0x20, 0x26, 0xc0, 0x6f, 0x02, 0x31, 0x12, 0x42, 0xab, 0x33, 0x0b, + 0x42, 0x37, 0x80, 0x01, 0x30, 0x01, 0x42, 0xf3, 0x37, 0xf7, 0x52, 0xfb, 0x50, 0x44, 0x40, 0xa2, 0x0a, 0x42, 0x42, + 0x8b, 0x31, 0x09, 0x2e, 0x5e, 0xf7, 0xf9, 0x54, 0xe3, 0x08, 0x83, 0x42, 0x1b, 0x42, 0x23, 0x33, 0x4b, 0x00, 0xbc, + 0x84, 0x0b, 0x40, 0x33, 0x30, 0x83, 0x42, 0x0b, 0x42, 0xe0, 0x7f, 0xd1, 0x7f, 0x98, 0x2e, 0x58, 0xb7, 0xd1, 0x6f, + 0x80, 0x30, 0x40, 0x42, 0x03, 0x30, 0xe0, 0x6f, 0xf3, 0x54, 0x04, 0x30, 0x00, 0x2e, 0x00, 0x2e, 0x01, 0x89, 0x62, + 0x0e, 0xfa, 0x2f, 0x43, 0x42, 0x11, 0x30, 0xfb, 0x6f, 0xc0, 0x2e, 0x01, 0x42, 0xb0, 0x5f, 0xc1, 0x4a, 0x00, 0x00, + 0x6d, 0x57, 0x00, 0x00, 0x77, 0x8e, 0x00, 0x00, 0xe0, 0xff, 0xff, 0xff, 0xd3, 0xff, 0xff, 0xff, 0xe5, 0xff, 0xff, + 0xff, 0xee, 0xe1, 0xff, 0xff, 0x7c, 0x13, 0x00, 0x00, 0x46, 0xe6, 0xff, 0xff, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, + 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, + 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, + 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x80, 0x2e, 0x00, 0xc1, 0x80, + 0x2e, 0x00, 0xc1, 0x80, 0x2e, 0x00, 0xc1, 0x80, 0x2e, 0x00, 0xc1, 0x80, 0x2e, 0x00, 0xc1, 0x80, 0x2e, 0x00, 0xc1, + 0x80, 0x2e, 0x00, 0xc1, 0x80, 0x2e, 0x00, 0xc1, 0x80, 0x2e, 0x00, 0xc1, 0x80, 0x2e, 0x00, 0xc1, 0x80, 0x2e, 0x00, + 0xc1, 0x80, 0x2e, 0x00, 0xc1, 0x80, 0x2e, 0x00, 0xc1, 0x80, 0x2e, 0x00, 0xc1, 0x80, 0x2e, 0x00, 0xc1, 0x80, 0x2e, + 0x00, 0xc1, 0x80, 0x2e, 0x00, 0xc1, 0x80, 0x2e, 0x00, 0xc1, 0x80, 0x2e, 0x00, 0xc1, 0x80, 0x2e, 0x00, 0xc1, 0x80, + 0x2e, 0x00, 0xc1, 0x80, 0x2e, 0x00, 0xc1, 0x80, 0x2e, 0x00, 0xc1, 0x80, 0x2e, 0x00, 0xc1, 0x80, 0x2e, 0x00, 0xc1, + 0x80, 0x2e, 0x00, 0xc1, 0x80, 0x2e, 0x00, 0xc1, 0x80, 0x2e, 0x00, 0xc1, 0x80, 0x2e, 0x00, 0xc1, 0x80, 0x2e, 0x00, + 0xc1, 0x80, 0x2e, 0x00, 0xc1, 0x80, 0x2e, 0x00, 0xc1, 0x80, 0x2e, 0x00, 0xc1, 0x80, 0x2e, 0x00, 0xc1, 0x80, 0x2e, + 0x00, 0xc1, 0x80, 0x2e, 0x00, 0xc1, 0x80, 0x2e, 0x00, 0xc1, 0x80, 0x2e, 0x00, 0xc1, 0x80, 0x2e, 0x00, 0xc1, 0x80, + 0x2e, 0x00, 0xc1, 0x80, 0x2e, 0x00, 0xc1, 0x80, 0x2e, 0x00, 0xc1, 0x80, 0x2e, 0x00, 0xc1, 0x80, 0x2e, 0x00, 0xc1, + 0x80, 0x2e, 0x00, 0xc1, 0x80, 0x2e, 0x00, 0xc1, 0x80, 0x2e, 0x00, 0xc1, 0x80, 0x2e, 0x00, 0xc1, 0x80, 0x2e, 0x00, + 0xc1, 0x80, 0x2e, 0x00, 0xc1, 0x80, 0x2e, 0x00, 0xc1, 0x80, 0x2e, 0x00, 0xc1, 0x80, 0x2e, 0x00, 0xc1, 0x80, 0x2e, + 0x00, 0xc1, 0x80, 0x2e, 0x00, 0xc1, 0x80, 0x2e, 0x00, 0xc1, 0x80, 0x2e, 0x00, 0xc1, 0x80, 0x2e, 0x00, 0xc1, 0x80, + 0x2e, 0x00, 0xc1 +}; + + +#endif //BETTERFLIGHT_BMI270_CONFIG_H diff --git a/software/MySrc/Drivers/crsf.c b/software/MySrc/Drivers/crsf.c new file mode 100644 index 0000000..69e3d84 --- /dev/null +++ b/software/MySrc/Drivers/crsf.c @@ -0,0 +1,237 @@ +#include "crsf.h" +#include "main.h" +#include "log.h" +#include "misc.h" +#include "stm32f7xx_hal_uart.h" +#include "stm32f7xx_hal.h" +#include "string.h" + + + + + +crsf_receive_state_t state; + +uint8_t start_data[3]; +uint8_t start_data_saved[3]; +uint8_t incoming_data[60]; +uint8_t incoming_data_saved[60]; +uint8_t frame_length; +bool print_start_flag = false; +uint8_t crc8; +uint8_t crc8_calculate(uint8_t *data, uint8_t len); +static void crsf_uart_setup(crsf_handle_t *crsf_h); +static void unpack_channels(uint8_t const * payload, uint32_t * dest); +static void crsf_telemetry_send(crsf_handle_t *crsf_h, crsf_frametype_t frame_type, uint8_t *data); + + + +void crsf_telemetry_send(crsf_handle_t *crsf_h, crsf_frametype_t frame_type, uint8_t *data){ + //frame -> [sync/address] [len] [type] [payload] [crc8] + uint8_t crc; + uint8_t len = LEN_CORR(sizeof(data)); + + uint8_t frame[len+2]; //add 2 for sync and address + + frame[0] = CRSF_ADDRESS_CRSF_RECEIVER; //address of receiver + frame[1] = len; //length of frame + if (len > 4){ + for(int i = 0; i < len-4; i++){ + frame[i+2] = data[i]; + } + //calculate crc + crc = crc8_calculate(data, len); + + for(int i = len-4; i < len; i++){ + frame[i] = data[i]; + } + HAL_UART_Transmit(crsf_h->huart, frame, len+2, 1000); + LOGD("Telemetry sent: %s", frame); + } + else{ + LOGD("Payload too small"); + } + + + + + +} + + +void crsf_init(crsf_handle_t * crsf_h, UART_HandleTypeDef *huart) { + crsf_h->huart = huart; + crsf_uart_setup(crsf_h); +} + +static void unpack_channels(uint8_t const * payload, uint32_t * dest) +{ + + const unsigned dstBits = 32; + const unsigned inputChannelMask = (1 << CHANNEL_SIZE) - 1; + + // code from BetaFlight rx/crsf.cpp / bitpacker_unpack + uint8_t bitsMerged = 0; + uint32_t readValue = 0; + unsigned readByteIndex = 0; + for (uint8_t n = 0; n < NUM_CHANNELS; n++) + { + while (bitsMerged < CHANNEL_SIZE) + { + uint8_t readByte = payload[readByteIndex++]; + readValue |= ((uint32_t) readByte) << bitsMerged; + bitsMerged += 8; + } + //printf("rv=%x(%x) bm=%u\n", readValue, (readValue & inputChannelMask), bitsMerged); + dest[n] = (readValue & inputChannelMask); + readValue >>= CHANNEL_SIZE; + bitsMerged -= CHANNEL_SIZE; + } +} + +void crsf_process(crsf_handle_t * crsf_h){ + if(state == processing_data){ + + if(start_data_saved[2] == CRSF_FRAMETYPE_RC_CHANNELS_PACKED){ + LOGD("Processing data"); + HAL_Delay(10); + uint32_t channels[16]; + crc8 = incoming_data_saved[frame_length-2]; + uint8_t incoming_frame_lenght = frame_length; + LOGD("Frame length: %u", incoming_frame_lenght); + HAL_Delay(10); + unpack_channels(incoming_data_saved, channels); + + for (unsigned ch=0; ch<16; ++ch){ + LOGD("ch%02u=%u", ch+1, channels[ch]); + + HAL_Delay(10); + } + LOGD("CRC8=%u", crc8); + state = wait_for_sync; + HAL_UART_Receive_IT(crsf_h->huart, start_data, 3); + } + + + } + else if(state == wait_for_sync){ + HAL_UART_Receive_IT(crsf_h->huart, start_data, 3); + LOGD("Waiting for sync"); + HAL_Delay(10); + } + else if(state == receiving_data){ + + + HAL_UART_Receive_IT(crsf_h->huart, incoming_data, start_data_saved[1]); + //LOGD("Receiving data"); + if (print_start_flag == true){ + for (int i = 0; i < 3; ++i) { + + LOGD("start_data[%u]=%u ", i, start_data_saved[i]); + HAL_Delay(10); + } + print_start_flag = false; + } + + } +} + +static void crsf_uart_setup(crsf_handle_t * crsf_h) { + + LOGD("UART setup begin"); + HAL_Delay(5); + crsf_h->huart->Instance = USART2; + crsf_h->huart->Init.BaudRate = 420000; + crsf_h->huart->Init.WordLength = UART_WORDLENGTH_8B; + crsf_h->huart->Init.StopBits = UART_STOPBITS_1; + crsf_h->huart->Init.Parity = UART_PARITY_NONE; + crsf_h->huart->Init.Mode = UART_MODE_RX; + crsf_h->huart->Init.OverSampling = UART_OVERSAMPLING_16; + HAL_UART_Init((UART_HandleTypeDef *) crsf_h->huart); + LOGD("UART setup done"); + HAL_Delay(20); + state = wait_for_sync; + +} +void HAL_UART_RxCpltCallback(UART_HandleTypeDef *huart) { + + if (huart->Instance == USART2) { + if(state == wait_for_sync){ + //HAL_UART_Receive_IT(huart, start_data, 3); + if(start_data[0] == 0xC8){ + print_start_flag = true; + if(start_data[1] > 0 && start_data[1] < CRSF_MAX_PACKET_SIZE){ + start_data_saved[0] = start_data[0]; + start_data_saved[1] = start_data[1]; + start_data_saved[2] = start_data[2]; + frame_length = start_data[1]; + + HAL_UART_Receive_IT(huart, incoming_data, frame_length-1); + state = receiving_data; + } + else{ + state = receiving_data; + HAL_UART_Receive_IT(huart, start_data, 3); + } + + } + else{ + state = wait_for_sync; + HAL_UART_Receive_IT(huart, start_data, 3); + } + } + else if(state == receiving_data){ + //do something + HAL_UART_Receive_IT(huart, incoming_data, frame_length-1); + for (int i = 0; i < frame_length; ++i) { + incoming_data_saved[i] = incoming_data[i]; + } + state = processing_data; + } + + } + + +} + +void crsf_send_frame_test(UART_HandleTypeDef *huart){ + crsf_handle_t crsf_h; + crsf_h.huart = huart; + uint8_t tx_data; + tx_data = 123; + + if (huart->Instance == USART2) + { + crsf_telemetry_send(&crsf_h, CRSF_FRAMETYPE_BATTERY_SENSOR, &tx_data); + // Data received, handle it + // Example: Print received data + + + // Start receiving again + //HAL_UART_Receive_IT(&huart2, &rx_data, 1); + + } +} + +void crsf_unpack_channels_test(){ + uint32_t dst[16]; + uint8_t src[] = { + 0xc8, 0x18, 0x16, + 0xe0, 0x03, 0x1f, 0xf8, 0xc0, 0x07, 0x3e, 0xf0, 0x81, 0x0f, 0x7c, + 0xe0, 0x03, 0x1f, 0xf8, 0xc0, 0x07, 0x3e, 0xf0, 0x81, 0x0f, 0x7c, 0xad + }; + unpack_channels(&src[3], dst); + HAL_Delay(10); + for (unsigned ch=0; ch<16; ++ch){ + LOGD("ch%02u=%u ", ch, dst[ch]); + HAL_Delay(10); + } + +} + +void crsf_tests(void){ + + + //crsf_send_frame_test(&huart2); + //crsf_unpack_channels_test(); +} \ No newline at end of file diff --git a/software/MySrc/Drivers/crsf.h b/software/MySrc/Drivers/crsf.h new file mode 100644 index 0000000..b913554 --- /dev/null +++ b/software/MySrc/Drivers/crsf.h @@ -0,0 +1,111 @@ + + +#include "stm32f7xx.h" +#include "stm32f7xx_hal_uart.h" + +/* +crsf notes: + + frame -> [sync/address] [len] [type] [payload] [crc8] + + [sync] : Begin with 0xC8 or specific address for telemetry + [len] length = length.payload + 4 + [type] (crsf_frametype_t enum) + [payload] : + [crc8] : + + */ + +#define CRSF_SYNC 0XC8 +#define CRSF_ADDRESS_CRSF_RECEIVER 0xEC +#define LEN_CORR(x) x+4 +#define NUM_CHANNELS 16 +#define CHANNEL_SIZE 11 +#define CRSF_MAX_PACKET_SIZE 64 + +//payload + +typedef enum { + CRSF_FRAME_GPS_PAYLOAD_SIZE = 15, + CRSF_FRAME_BATTERY_SENSOR_PAYLOAD_SIZE = 8, + CRSF_FRAME_LINK_STATISTICS_PAYLOAD_SIZE = 10, + CRSF_FRAME_RC_CHANNELS_PAYLOAD_SIZE = 22, // 11 bits per channel * 16 channels = 22 bytes. + CRSF_FRAME_ATTITUDE_PAYLOAD_SIZE = 6, +}crsf_payload_sizes_t; + +typedef enum{ + wait_for_sync = 1, + receiving_data = 2, + processing_data = 3 +}crsf_receive_state_t; + +typedef enum { //https://github.com/crsf-wg/crsf/wiki/Packet-Types + CRSF_FRAMETYPE_GPS = 0x02, + CRSF_FRAMETYPE_VARIO = 0x07, + CRSF_FRAMETYPE_BATTERY_SENSOR = 0x08, + CRSF_FRAMETYPE_BARO_ALTITUDE = 0x09, + CRSF_FRAMETYPE_HEARTBEAT = 0x0B, //imp + CRSF_FRAMETYPE_LINK_STATISTICS = 0x14, + CRSF_FRAMETYPE_RC_CHANNELS_PACKED = 0x16, + CRSF_FRAMETYPE_SUBSET_RC_CHANNELS_PACKED = 0x17, + CRSF_FRAMETYPE_LINK_RX_ID = 0x1C, + CRSF_FRAMETYPE_LINK_TX_ID = 0x1D, + CRSF_FRAMETYPE_ATTITUDE = 0x1e, + CRSF_FRAMETYPE_FLIGHT_MODE = 0x21, + CRSF_FRAMETYPE_DEVICE_PING = 0x28, + CRSF_FRAMETYPE_DEVICE_INFO = 0x29, + CRSF_FRAMETYPE_PARAMETER_SETTINGS_ENTRY = 0x2B, + CRSF_FRAMETYPE_PARAMETER_READ = 0x2C, + CRSF_FRAMETYPE_PARAMETER_WRITE = 0x2D, + CRSF_FRAMETYPE_ELRS_STATUS = 0x2E, + CRSF_FRAMETYPE_COMMAND = 0x32, + CRSF_FRAMETYPE_RADIO_ID = 0x3a, + CRSF_FRAMETYPE_MSP_REQ = 0x7a, + CRSF_FRAMETYPE_MSP_RESP = 0x7b, + CRSF_FRAMETYPE_MSP_WRITE = 0x7c, + CRSF_FRAMETYPE_DISPLAYPORT_CMD = 0x7d +} crsf_frametype_t; + + + +typedef struct { + UART_HandleTypeDef* huart; +}crsf_handle_t; + +typedef struct __attribute__((packed)) { + unsigned ch0 : 11; + unsigned ch1 : 11; + unsigned ch2 : 11; + unsigned ch3 : 11; + unsigned ch4 : 11; + unsigned ch5 : 11; + unsigned ch6 : 11; + unsigned ch7 : 11; + unsigned ch8 : 11; + unsigned ch9 : 11; + unsigned ch10 : 11; + unsigned ch11 : 11; + unsigned ch12 : 11; + unsigned ch13 : 11; + unsigned ch14 : 11; + unsigned ch15 : 11; +}crsf_channels_t; + + + +typedef struct{ + uint8_t address; + uint8_t length; + uint8_t payload[CRSF_MAX_PACKET_SIZE-6]; // 6 = address + length + crc + uint8_t crc[2]; +}crsf_channels_frame_t; + + + + + +void crsf_init(crsf_handle_t * hcrsf, UART_HandleTypeDef *huart); +void crsf_process(crsf_handle_t * hcrsf); +void crsf_send_frame_test(UART_HandleTypeDef *huart); +void crsf_tests(void); + diff --git a/software/MySrc/Drivers/dshot.c b/software/MySrc/Drivers/dshot.c new file mode 100644 index 0000000..47f51c4 --- /dev/null +++ b/software/MySrc/Drivers/dshot.c @@ -0,0 +1,244 @@ +/* + This is a dshot driver for stm32(f7) microcontrollers that makes use of the stm32 HAL library. + It is almost 100% portable, you will just need to remove my project specific logging functions and includes. + You will also have to reconfigure the PSC and ARR values to match your peripheral clock speed. + */ + + +#include "dshot.h" +#include "main.h" +#include "log.h" +#include "stm32f7xx_hal_tim.h" + + +static void dshot_prepare_packet(uint32_t *dma_buffer ,uint16_t value); +static void dshot_dma_complete_callback(DMA_HandleTypeDef *hdma); +static uint32_t dshot_find_ccrx(dshot_handle_t *dshot_h); +static void dshot_dma_enable(DMA_HandleTypeDef *hdma, bool enable); +static void dshot_send(dshot_handle_t *dshot_h, const uint16_t value); +static dshot_cmd_info_t dshot_find_special_cmd_info(dshot_cmd_t cmd); + +const dshot_cmd_info_t DSHOT_CMD_SEND_INFO[] = { + {DSHOT_CMD_BEEP1, 1, 260}, + {DSHOT_CMD_BEEP2, 1, 260}, + {DSHOT_CMD_BEEP3, 1, 260}, + {DSHOT_CMD_BEEP4, 1, 260}, + {DSHOT_CMD_BEEP5, 1, 260}, + {DSHOT_CMD_ESC_INFO, 1, 12}, + {DSHOT_CMD_SPIN_DIRECTION_1, 6, 0}, + {DSHOT_CMD_SPIN_DIRECTION_2, 6, 0}, + {DSHOT_CMD_3D_MODE_OFF, 6, 0}, + {DSHOT_CMD_3D_MODE_ON, 6, 0}, + {DSHOT_CMD_SAVE_SETTINGS, 6, 35}, + {DSHOT_EXTENDED_TELEMETRY_ENABLE, 6, 0}, + {DSHOT_EXTENDED_TELEMETRY_DISABLE, 6, 0}, + {DSHOT_CMD_SPIN_DIRECTION_NORMAL, 6, 0}, + {DSHOT_CMD_SPIN_DIRECTION_REVERSED, 6, 0}, + {DSHOT_CMD_SIGNAL_LINE_TELEMETRY_DISABLE, 6, 0}, + {DSHOT_CMD_SIGNAL_LINE_TELEMETRY_ENABLE, 6, 0}, + {DSHOT_CMD_SIGNAL_LINE_CONTINUOUS_ERPM_TELEMETRY, 6, 0}, + {DSHOT_CMD_SIGNAL_LINE_CONTINUOUS_ERPM_PERIOD_TELEMETRY, 6, 0} +}; + + +void dshot_init(dshot_handle_t *dshot_h, TIM_HandleTypeDef *htim, DMA_HandleTypeDef *hdma, uint32_t tim_channel) { + dshot_h->htim = htim; + dshot_h->hdma = hdma; + dshot_h->tim_channel = tim_channel; + dshot_h->throttle_value = 0; + + + // set timer + __HAL_TIM_SET_PRESCALER(dshot_h->htim, DSHOT_PSC); + __HAL_TIM_SET_AUTORELOAD(dshot_h->htim, DSHOT_ARR); + HAL_TIM_PWM_Start(dshot_h->htim, dshot_h->tim_channel); + + //set dma callback function + dshot_h->hdma->XferCpltCallback = dshot_dma_complete_callback; + +} + +// CALL EVERY 1MS or esc will disconnect +void dshot_process(dshot_handle_t * dshot_h) { + + // warm user if dshot_process is not called every 2ms, esc might disarm if not send a new frame every so often + static uint64_t last_time = 0; + uint64_t current_time = millis; + if (last_time != 0 && (current_time - last_time) > 2) { + LOGW((uint8_t*)"More than 1ms has been past since DSHOT PROCESS has been called, motor may disconnect!"); + } + last_time = current_time; + + //cmd timeout + if (dshot_h->cmd_cnts->send_count == -1) { + dshot_h->cmd_cnts->delayms_after_cmd--; + if (dshot_h->cmd_cnts->delayms_after_cmd <= 0) { //if delay is over, reset sent counter + dshot_h->cmd_cnts->send_count = 0; + } +// dshot_send(dshot_h, dshot_h->throttle_value); + return; + } + + //send special command + if (dshot_h->cmd_cnts->send_count > 0) { + LOGD("send special command %d", (uint8_t)dshot_h->cmd_cnts->cmd); + LOGD("send count %d", (uint8_t)dshot_h->cmd_cnts->send_count); + + dshot_send(dshot_h, (uint16_t)dshot_h->cmd_cnts->cmd); + delay(260); //wait for beep to finish + dshot_h->cmd_cnts->send_count--; + return; + } + + //send stored value to motor + dshot_send(dshot_h, dshot_h->throttle_value); +} + +void dshot_stop(dshot_handle_t *dshot_h) { + dshot_h->throttle_value = 0; +} + +//doesn't change the speed, expects you to call dshot_process every 1ms +void dshot_set_throttle(dshot_handle_t *dshot_h, uint16_t throttle) { + if (throttle < DSHOT_MIN_THROTTLE) { + throttle = DSHOT_MIN_THROTTLE; + } else if (throttle > DSHOT_MAX_THROTTLE) { + throttle = DSHOT_MAX_THROTTLE; + } + + dshot_h->throttle_value = throttle - DSHOT_MIN_THROTTLE; +} + +void dshot_send_special_command(dshot_handle_t *dshot_h, dshot_cmd_t cmd) { + dshot_cmd_info_t cmd_info = dshot_find_special_cmd_info(cmd); + + //refuse cmd if it requires motor to be stopped and the motor is still spinning + if (cmd_info.cmd <= CMD_ONLY_WHILE_STOP_RANGE && dshot_h->throttle_value > 0) { + LOGW((uint8_t*)"Special command %d refused, motor is still spinning (for 1-36 motors should be stopped)", cmd); + return; + } + + //refuse cmd if one is already in progress + if (dshot_h->cmd_cnts->send_count != 0) { + LOGW((uint8_t*)"Special command %d refused, another special command is already in progress", cmd); + return; + } + + //set the command for the process to executed on the next cycle + dshot_h->cmd_cnts->cmd = cmd; + dshot_h->cmd_cnts->send_count = cmd_info.send_count; +} + +// ---------- static functions ---------- // + +// raw send function, use dshot_set_throttle to set motor speed, dshot_send_special_command to send special commands +static void dshot_send(dshot_handle_t *dshot_h, const uint16_t value) { + dshot_prepare_packet(dshot_h->dma_buffer, value); + + HAL_DMA_Start_IT(dshot_h->hdma, (uint32_t) dshot_h->dma_buffer, dshot_find_ccrx(dshot_h), DSHOT_DMA_BUFFER_SIZE); + dshot_dma_enable(dshot_h->hdma, true); + // when dma is done, the dma callback function will disable the dma channel. +} + +static void dshot_prepare_packet(uint32_t *dma_buffer ,uint16_t value) { + uint16_t packet = 0; + bool dshot_telemetry = false; + + packet = (value << 1) | (dshot_telemetry ? 1 : 0); + + //calculate checksum + unsigned csum = 0; + unsigned csum_data = packet; + + for (int i = 0; i < 3; i++) { + csum ^= csum_data; + csum_data >>= 4; + } + + csum &= 0xf; + packet = (packet << 4) | csum; + + // the dma buffer represents pwm duty cycles for each bit in the frame (the values depends on PSC and ARR) + // put packet in dma buffer with correct pwm duty cycle + for (int i = 0; i < DSHOT_FRAME_SIZE; i++) { + dma_buffer[i] = (packet & 0x8000) ? MOTOR_BIT_1 : MOTOR_BIT_0; + packet <<= 1; + } + dma_buffer[16] = 0; + dma_buffer[17] = 0; + +// //print the dma buffer if value is below 47 +// if (value < 47 && value != 0) { +// for (int i = 0; i < DSHOT_FRAME_SIZE + 2; i++) { +// delay(10); +// LOGD((uint8_t*)"dma_buffer[%d]: %d", i, dma_buffer[i]); +// } +// delay(10); +// } +} + + + +static void dshot_dma_complete_callback(DMA_HandleTypeDef *hdma) { + TIM_HandleTypeDef *htim = (TIM_HandleTypeDef *)((DMA_HandleTypeDef *)hdma)->Parent; + // find the correct dma channel and disable it + dshot_dma_enable(hdma, false); +} + +// true for enable, false for disable +static void dshot_dma_enable(DMA_HandleTypeDef *hdma, bool enable) { + TIM_HandleTypeDef *htim = (TIM_HandleTypeDef *)((DMA_HandleTypeDef *)hdma)->Parent; + + if (enable) { + if (hdma == htim->hdma[TIM_DMA_ID_CC1]) + __HAL_TIM_ENABLE_DMA(htim, TIM_DMA_CC1); + else if(hdma == htim->hdma[TIM_DMA_ID_CC2]) + __HAL_TIM_ENABLE_DMA(htim, TIM_DMA_CC2); + else if(hdma == htim->hdma[TIM_DMA_ID_CC3]) + __HAL_TIM_ENABLE_DMA(htim, TIM_DMA_CC3); + else if(hdma == htim->hdma[TIM_DMA_ID_CC4]) + __HAL_TIM_ENABLE_DMA(htim, TIM_DMA_CC4); + else + LOGE("DMA channel not found"); + } else { + if (hdma == htim->hdma[TIM_DMA_ID_CC1]) + __HAL_TIM_DISABLE_DMA(htim, TIM_DMA_CC1); + else if(hdma == htim->hdma[TIM_DMA_ID_CC2]) + __HAL_TIM_DISABLE_DMA(htim, TIM_DMA_CC2); + else if(hdma == htim->hdma[TIM_DMA_ID_CC3]) + __HAL_TIM_DISABLE_DMA(htim, TIM_DMA_CC3); + else if(hdma == htim->hdma[TIM_DMA_ID_CC4]) + __HAL_TIM_DISABLE_DMA(htim, TIM_DMA_CC4); + else + LOGE("DMA channel not found"); + + } +} + +static uint32_t dshot_find_ccrx(dshot_handle_t *dshot_h) { + if (dshot_h->tim_channel == TIM_CHANNEL_1) { + return (uint32_t)&dshot_h->htim->Instance->CCR1; + } else if (dshot_h->tim_channel == TIM_CHANNEL_2) { + return (uint32_t)&dshot_h->htim->Instance->CCR2; + } else if (dshot_h->tim_channel == TIM_CHANNEL_3) { + return (uint32_t)&dshot_h->htim->Instance->CCR3; + } else if (dshot_h->tim_channel == TIM_CHANNEL_4) { + return (uint32_t)&dshot_h->htim->Instance->CCR4; + } else { + LOGE("Invalid TIM channel"); + return 0; + } +} + + +static dshot_cmd_info_t dshot_find_special_cmd_info(dshot_cmd_t cmd) { + for (int i = 0; i < 19 ; i++) { + if (DSHOT_CMD_SEND_INFO[i].cmd == cmd) { + return DSHOT_CMD_SEND_INFO[i]; + } + } + // return standard info if not found, this is normal, only cmd's with special requirements are in the table + dshot_cmd_info_t standard_cmd_info = {cmd, 1, 0}; + return standard_cmd_info; +} + diff --git a/software/MySrc/Drivers/dshot.h b/software/MySrc/Drivers/dshot.h new file mode 100644 index 0000000..db70dc7 --- /dev/null +++ b/software/MySrc/Drivers/dshot.h @@ -0,0 +1,88 @@ +#ifndef BETTERFLIGHT_DSHOT_H +#define BETTERFLIGHT_DSHOT_H + +#include "main.h" +#include "misc.h" + + +#define MOTOR_BIT_0 240 // ~33% duty cycle +#define MOTOR_BIT_1 480 // ~66% duty cycle +#define DSHOT_PSC 0 // prescaler +#define DSHOT_ARR 639 // auto reload register + +#define DSHOT_FRAME_SIZE 16 +#define DSHOT_DMA_BUFFER_SIZE 18 // 16 data bits + 2 stop bits (reset) + +#define DSHOT_MIN_THROTTLE 48 +#define DSHOT_MAX_THROTTLE 2047 +#define DSHOT_RANGE (DSHOT_MAX_THROTTLE - DSHOT_MIN_THROTTLE) + +#define CMD_ONLY_WHILE_STOP_RANGE 36 // only allow special commands to be send when motors if they are from 0 to 36 + + + + +//dshot special commands, send these values to the motor instead of the throttle value to execute the command +typedef enum { + DSHOT_CMD_BEEP1 = 1, // Wait at least length of beep (260ms) before next command + DSHOT_CMD_BEEP2 = 2, // Wait at least length of beep (260ms) before next command + DSHOT_CMD_BEEP3 = 3, // Wait at least length of beep (260ms) before next command + DSHOT_CMD_BEEP4 = 4, // Wait at least length of beep (260ms) before next command + DSHOT_CMD_BEEP5 = 5, // Wait at least length of beep (260ms) before next command + DSHOT_CMD_ESC_INFO = 6, // Wait at least 12ms before next command + DSHOT_CMD_SPIN_DIRECTION_1 = 7, // Need 6x + DSHOT_CMD_SPIN_DIRECTION_2 = 8, // Need 6x + DSHOT_CMD_3D_MODE_OFF = 9, // Need 6x + DSHOT_CMD_3D_MODE_ON = 10, // Need 6x + DSHOT_CMD_SETTINGS_REQUEST = 11, // Currently not implemented + DSHOT_CMD_SAVE_SETTINGS = 12, // Need 6x, wait at least 35ms before next command + DSHOT_EXTENDED_TELEMETRY_ENABLE = 13, // Need 6x (only on EDT enabled firmware) + DSHOT_EXTENDED_TELEMETRY_DISABLE = 14, // Need 6x (only on EDT enabled firmware) + DSHOT_CMD_SPIN_DIRECTION_NORMAL = 20, // Need 6x + DSHOT_CMD_SPIN_DIRECTION_REVERSED = 21, // Need 6x + DSHOT_CMD_LED0_ON = 22, + DSHOT_CMD_LED1_ON = 23, + DSHOT_CMD_LED2_ON = 24, + DSHOT_CMD_LED3_ON = 25, + DSHOT_CMD_LED0_OFF = 26, + DSHOT_CMD_LED1_OFF = 27, + DSHOT_CMD_LED2_OFF = 28, + DSHOT_CMD_LED3_OFF = 29, + DSHOT_CMD_SIGNAL_LINE_TELEMETRY_DISABLE = 32, // Need 6x. Disables commands 42 to 47 + DSHOT_CMD_SIGNAL_LINE_TELEMETRY_ENABLE = 33, // Need 6x. Enables commands 42 to 47 + DSHOT_CMD_SIGNAL_LINE_CONTINUOUS_ERPM_TELEMETRY = 34, // Need 6x. Enables commands 42 to 47 and sends erpm if normal Dshot frame + DSHOT_CMD_SIGNAL_LINE_CONTINUOUS_ERPM_PERIOD_TELEMETRY = 35, // Need 6x. Enables commands 42 to 47 and sends erpm period if normal Dshot frame + DSHOT_CMD_SIGNAL_LINE_TEMPERATURE_TELEMETRY = 42, // 1°C per LSB + DSHOT_CMD_SIGNAL_LINE_VOLTAGE_TELEMETRY = 43, // 10mV per LSB, 40.95V max + DSHOT_CMD_SIGNAL_LINE_CURRENT_TELEMETRY = 44, // 100mA per LSB, 409.5A max + DSHOT_CMD_SIGNAL_LINE_CONSUMPTION_TELEMETRY = 45, // 10mAh per LSB, 40.95Ah max + DSHOT_CMD_SIGNAL_LINE_ERPM_TELEMETRY = 46, // 100erpm per LSB, 409500erpm max + DSHOT_CMD_SIGNAL_LINE_ERPM_PERIOD_TELEMETRY = 47 // 16us per LSB, 65520us max TBD +} dshot_cmd_t; + +// struct to keep track of special command send count +typedef struct { + dshot_cmd_t cmd; + int8_t send_count; + uint16_t delayms_after_cmd; +} dshot_cmd_info_t; + +// table to keep track of special command info +extern const dshot_cmd_info_t DSHOT_CMD_SEND_INFO[]; + +typedef struct { + uint32_t dma_buffer[DSHOT_DMA_BUFFER_SIZE]; + TIM_HandleTypeDef *htim; + DMA_HandleTypeDef *hdma; + uint32_t tim_channel; + uint16_t throttle_value; // last speed value set by user (altered by dshot_set_throttle!!) + dshot_cmd_info_t *cmd_cnts; //cmd and its counters (note: cmd_cnts->send_count = -1 means cmd timeout) +} dshot_handle_t; + +void dshot_init(dshot_handle_t *dshot_h, TIM_HandleTypeDef *htim, DMA_HandleTypeDef *hdma, uint32_t tim_channel); +void dshot_process(dshot_handle_t *dshot_h); +void dshot_set_throttle(dshot_handle_t *dshot_h, uint16_t throttle); +void dshot_stop(dshot_handle_t *dshot_h); +void dshot_send_special_command(dshot_handle_t *dshot_h, dshot_cmd_t cmd); + +#endif //BETTERFLIGHT_DSHOT_H diff --git a/software/MySrc/Drivers/gyro.c b/software/MySrc/Drivers/gyro.c new file mode 100644 index 0000000..195fec5 --- /dev/null +++ b/software/MySrc/Drivers/gyro.c @@ -0,0 +1,220 @@ +#include "gyro.h" + +#include "stm32f7xx_hal.h" + +#include "main.h" +#include "log.h" +#include "spi_soft.h" + +#include "BMI270_CONFIG.h" +#include "bmi270.h" + + +typedef enum { + REG_CHIP_ID = 0x00, + REG_CMD_REG= 0x7E, + REG_PWR_CONF = 0x7C, + REG_INIT_CTRL = 0x59, + REG_INTERNAL_STATUS = 0x21, + REG_PWR_CTRL = 0x7D, + REG_ACC_CONF = 0x40, + REG_GYR_CONF = 0x42, + REG_DATA_8_TO_19 = 0x41, + REG_DATA_START = 0x0C, +} BMI270_REG_t; + +typedef enum { // Register 0x21 INTERNAL_STATUS -> see BMI270 datasheet + INT_STATUS__NOT_INIT = 0x00, // ASIC not initialized + INT_STATUS__INIT_OK = 0x01, // ASIC initialized + INT_STATUS__INIT_ERR = 0x02, // Initialization error + INT_STATUS__DRV_ERR = 0x03, // Invalid driver + INT_STATUS__SNS_STOP = 0x04, // Sensor stopped + INT_STATUS__NVM_ERROR = 0x05, //Internal error while accessing NVM + INT_STATUS__START_UP_ERROR = 0x06, // Internal error while accessing NVM and initialization error + INT_STATUS__COMPAT_ERROR = 0x07, // Compatibility error + INT_STATUS__AXES_REMAP_ERROR = 0x20, // Incorrect axes remapping, XYZ axes must be mapped to exclusively separate axes i.e. they cannot be mapped to same axis. + INT_STATUS__ODR_50HZ_ERROR = 0x30, // The minimum bandwidth conditions are not respected for the features which require 50 Hz data. + INT_STATUS__UNKNOWN = 0xFF, +} BMI270_INT_STATUS_ERR_t; + + +#define NUM_BYTES_TO_READ 12 +#define READ_ADDR(x) (x | 0x80) + +uint8_t gyro_read_reg(gyro_handle_t *gyro_h, uint8_t reg); +void gyro_write_reg(gyro_handle_t *gyro_h, uint8_t reg, uint8_t data); +void gyro_burst_write(gyro_handle_t *gyro_h, uint8_t reg, const uint8_t *data, uint32_t len); +BMI270_INT_STATUS_ERR_t parse_internal_status_err(uint8_t reg_dump); +void log_gyro_int_status_err(BMI270_INT_STATUS_ERR_t err); + +gyro_err_t gyro_init(gyro_handle_t *gyro_h) { + + gyro_h->spi_h.sck.port = GPIOA; + gyro_h->spi_h.sck.pin = GPIO_PIN_5; + + gyro_h->spi_h.miso.port = GPIOA; + gyro_h->spi_h.miso.pin = GPIO_PIN_6; + + gyro_h->spi_h.mosi.port = GPIOA; + gyro_h->spi_h.mosi.pin = GPIO_PIN_7; + + gyro_h->spi_h.cs.port = GPIOB; + gyro_h->spi_h.cs.pin = GPIO_PIN_2; + + SPI_soft_init(&gyro_h->spi_h); + + delay(1); + + // set the CS low and high quickly to activate(?) the BMI270...? + SPI_soft_cs_low(&gyro_h->spi_h); // set cs low + delay(1); + SPI_soft_cs_high(&gyro_h->spi_h); // set cs high + delay(50); + + // check for chip id (0x24) + uint8_t chip_id_tx_buffer[] = {READ_ADDR(REG_CHIP_ID), 0x00, 0x00}; + uint8_t chip_id_rx_buffer[] = {0x00, 0x00, 0x00}; + delay(10); + SPI_soft_trx(&gyro_h->spi_h, chip_id_tx_buffer, chip_id_rx_buffer, 3); + if (chip_id_rx_buffer[2] != 0x24) { + return GYRO_ERR_CHIP_ID; + } + +// delay(1); +// gyro_write_reg(gyro_h, REG_CMD_REG, 0x6B); //soft reset + delay(10); + gyro_write_reg(gyro_h, REG_PWR_CONF, 0x00); // set power config to 0x00 (advanced power save disable) + delay(1); + + // prepare config load + gyro_write_reg(gyro_h, REG_INIT_CTRL, 0x00); + delay(1); + + // burst write to reg INIT_DATA Start with byte 0 + gyro_burst_write(gyro_h, REG_DATA_START, BMI_270_CONF_DATA_ARRAY, (uint32_t ) sizeof(BMI_270_CONF_DATA_ARRAY)); + + delay(10); + + gyro_write_reg(gyro_h, REG_INIT_CTRL, 0x01); // complete config load + + delay(1); + + // check internal status + uint8_t internal_status = gyro_read_reg(gyro_h, REG_INTERNAL_STATUS); + if (parse_internal_status_err(internal_status) != INT_STATUS__INIT_OK) { + log_gyro_int_status_err(parse_internal_status_err(internal_status)); + delay(10); // RWBA (remove when buffer added) + LOGE("internal status error reg dump (0x21): %s", byte_to_binary_str(internal_status)); + return GYRO_ERR_INTERNAL_STATUS; + } + + + return GYRO_OK; +} + +uint8_t gyro_read_reg(gyro_handle_t *gyro_h, uint8_t reg) { + + uint8_t tx_buffer[] = {(reg | 0x80), 0x00, 0x00}; // | 0x80 to set the read bit + uint8_t rx_buffer[] = {0x00, 0x00, 0x00}; + + SPI_soft_trx(&gyro_h->spi_h, tx_buffer, rx_buffer, 3); + + return rx_buffer[2]; +} + +void gyro_write_reg(gyro_handle_t *gyro_h, uint8_t reg, uint8_t data) { + + uint8_t tx_buffer[] = {reg, data}; + uint8_t rx_buffer[] = {0x00, 0x00}; + + SPI_soft_trx(&gyro_h->spi_h, tx_buffer, rx_buffer, 2); +} + +void gyro_burst_write(gyro_handle_t *gyro_h, uint8_t reg, const uint8_t *data, uint32_t arr_len) { + + uint8_t tx_buffer[arr_len + 1]; + tx_buffer[0] = reg; + + for (int i = 0; i < arr_len; i++) { + tx_buffer[i + 1] = data[i]; + } + + SPI_soft_burst_tx(&gyro_h->spi_h, tx_buffer, arr_len + 1); + +} + + +void log_gyro_err_t(gyro_err_t err) { + switch (err) { + case GYRO_OK: + LOGI("GYRO_OK"); + break; + case GYRO_ERR_CHIP_ID: + LOGE("GYRO_ERR_CHIP_ID"); + break; + case GYRO_ERR_INTERNAL_STATUS: + LOGE("GYRO_ERR_INTERNAL_STATUS"); + break; + default: + LOGE("UNKNOWN GYRO_ERR"); + break; + } +} + + +BMI270_INT_STATUS_ERR_t parse_internal_status_err(uint8_t reg_dump) { + if (!(reg_dump & 0x01)) { // if INT_STATUS__INIT_OK bit is not set + if ((reg_dump & 0x0F) > 0x07) { //check if the value is possible + return INT_STATUS__UNKNOWN; + } + return (BMI270_INT_STATUS_ERR_t) (reg_dump & 0x0F); // return the INTERNAL_STATUS error (message) (cut of the last 4 bits) + } + + // check if bit [4-7] are set and return the corresponding error + if (reg_dump & 0x10) { + return INT_STATUS__AXES_REMAP_ERROR; + } else if (reg_dump & 0x20) { + return INT_STATUS__ODR_50HZ_ERROR; + } + + // if none of the above, idk what the error is (should not happen) + return INT_STATUS__UNKNOWN; +} + +void log_gyro_int_status_err(BMI270_INT_STATUS_ERR_t err) { + switch (err) { + case INT_STATUS__NOT_INIT: + LOGE("INT_STATUS__NOT_INIT: ASIC not initialized"); + break; + case INT_STATUS__INIT_OK: + LOGI("INT_STATUS__INIT_OK: ASIC initialized"); + break; + case INT_STATUS__INIT_ERR: + LOGE("INT_STATUS__INIT_ERR: Initialization error"); + break; + case INT_STATUS__DRV_ERR: + LOGE("INT_STATUS__DRV_ERR: Invalid driver"); + break; + case INT_STATUS__SNS_STOP: + LOGE("INT_STATUS__SNS_STOP: Sensor stopped"); + break; + case INT_STATUS__NVM_ERROR: + LOGE("INT_STATUS__NVM_ERROR: Internal error while accessing NVM"); + break; + case INT_STATUS__START_UP_ERROR: + LOGE("INT_STATUS__START_UP_ERROR: Internal error while accessing NVM and initialization error"); + break; + case INT_STATUS__COMPAT_ERROR: + LOGE("INT_STATUS__COMPAT_ERROR: Compatibility error"); + break; + case INT_STATUS__AXES_REMAP_ERROR: + LOGE("INT_STATUS__AXES_REMAP_ERROR: Incorrect axes remapping, XYZ axes must be mapped to exclusively separate axes i.e. they cannot be mapped to same axis."); + break; + case INT_STATUS__ODR_50HZ_ERROR: + LOGE("INT_STATUS__ODR_50HZ_ERROR: The minimum bandwidth conditions are not respected for the features which require 50 Hz data."); + break; + case INT_STATUS__UNKNOWN: + LOGW("INT_STATUS__UNKNOWN: Unknown error (should not happen)"); + break; + } +} \ No newline at end of file diff --git a/software/MySrc/Drivers/gyro.h b/software/MySrc/Drivers/gyro.h new file mode 100644 index 0000000..4147900 --- /dev/null +++ b/software/MySrc/Drivers/gyro.h @@ -0,0 +1,33 @@ +// +// Created by Isaak on 1/3/2024. +// + +#ifndef BETTERFLIGHT_GYRO_H +#define BETTERFLIGHT_GYRO_H + +#include +#include "stm32f7xx_hal.h" +#include "spi_soft.h" + +typedef enum { + GYRO_OK = 0, + GYRO_ERR_CHIP_ID = -1, + GYRO_ERR_INTERNAL_STATUS = -3, +} gyro_err_t; + + +typedef struct +{ + SPI_handle_t spi_h; + uint8_t chip_id; + uint8_t raw_data; + uint8_t x; + uint8_t y; + uint8_t z; +} gyro_handle_t; + +gyro_err_t gyro_init(gyro_handle_t *gyro); +gyro_err_t gyro_read(gyro_handle_t *gyro); +void log_gyro_err_t(gyro_err_t err); + +#endif //BETTERFLIGHT_GYRO_H diff --git a/software/MySrc/Drivers/imu.c b/software/MySrc/Drivers/imu.c new file mode 100644 index 0000000..3073476 --- /dev/null +++ b/software/MySrc/Drivers/imu.c @@ -0,0 +1,476 @@ +/* + The BMI270 api from Bosch is used to interface with the BMI270 IMU sensor. + This code is based on the example code provided by Bosch. + -> https://github.com/boschsensortec/BMI270_SensorAPI/blob/master/bmi270_examples/accel_gyro/accel_gyro.c +*/ + + + +#include "imu.h" +#include "bmi270.h" //has bmi2.h included => has bmi2_defs.h included +#include + +#include "log.h" +#include "spi_soft.h" + + +#define GRAVITY_EARTH (9.80665f) + +#define ACCEL UINT8_C(0x00) +#define GYRO UINT8_C(0x01) + + +static int8_t set_accel_gyro_config(struct bmi2_dev *bmi); +static float lsb_to_mps2(int16_t val, float g_range, uint8_t bit_width); +static float lsb_to_dps(int16_t val, float dps, uint8_t bit_width); + +static int8_t set_accel_gyro_config(struct bmi2_dev *bmi); +static float lsb_to_mps2(int16_t val, float g_range, uint8_t bit_width); +static float lsb_to_dps(int16_t val, float dps, uint8_t bit_width); +void bmi2_error_codes_print_result(int8_t rslt); + + +// Initializes the BMI270 using the Bosch BMI270 API +IMU_err_t imu_init(IMU_handle_t *imu_h) { + + // spi init + imu_h->spi_h.sck.port = GPIOA; + imu_h->spi_h.sck.pin = GPIO_PIN_5; + + imu_h->spi_h.miso.port = GPIOA; + imu_h->spi_h.miso.pin = GPIO_PIN_6; + + imu_h->spi_h.mosi.port = GPIOA; + imu_h->spi_h.mosi.pin = GPIO_PIN_7; + + imu_h->spi_h.cs.port = GPIOB; + imu_h->spi_h.cs.pin = GPIO_PIN_2; + + SPI_soft_init(&imu_h->spi_h); + + // bmi270 init + int8_t rslt; + + imu_h->sensor_list[0] = BMI2_ACCEL; + imu_h->sensor_list[1] = BMI2_GYRO; + + struct bmi2_sens_data sensor_data = { { 0 } }; + imu_h->sensor_data = sensor_data; + + imu_h->acc_x = 0; imu_h->acc_y = 0; imu_h->acc_z = 0; + imu_h->gyr_x = 0; imu_h->gyr_y = 0; imu_h->gyr_z = 0; + + imu_h->bmi.intf_ptr = &imu_h->spi_h; + imu_h->bmi.intf = BMI2_SPI_INTF; + imu_h->bmi.read = (bmi2_read_fptr_t)imu_spi_read_reg; + imu_h->bmi.write = (bmi2_write_fptr_t)imu_spi_write_reg; + imu_h->bmi.delay_us = (bmi2_delay_fptr_t)imu_delay_us; + + LOGI("bmi270 init"); + delay(10); // RWBA + rslt = bmi270_init(&imu_h->bmi); + delay(10); // RWBA + bmi2_error_codes_print_result(rslt); + delay(10); // RWBA + + if (rslt != BMI2_OK) return IMU_ERR_BMI_INIT; + + LOGI("bmi270 set accel gyro config"); + rslt = set_accel_gyro_config(&imu_h->bmi); + + if (rslt != BMI2_OK) return IMU_ERR_ACC_GYR_CONFIG; + + rslt = bmi2_sensor_enable(imu_h->sensor_list, 2, &imu_h->bmi); + bmi2_error_codes_print_result(rslt); + + if (rslt != BMI2_OK) return IMU_ERR_SENSOR_ENABLE; + + imu_h->config.type = BMI2_ACCEL; + rslt = bmi2_get_sensor_config(&imu_h->config, 1, &imu_h->bmi); + bmi2_error_codes_print_result(rslt); + + if (rslt != BMI2_OK) return IMU_ERR_GET_SENSOR_CONFIG; + + LOGI("bmi270 succesfully initialized"); + return IMU_OK; +} + + + +// reads out the accelerometer and gyroscope data from the BMI270 and stores it in the IMU_handle_t struct +void imu_process(IMU_handle_t *imu_h) { + int8_t rslt; + + struct bmi2_sens_data sensor_data = { { 0 } }; + + rslt = bmi2_get_sensor_data(&imu_h->sensor_data, &imu_h->bmi); + bmi2_error_codes_print_result(rslt); + + + if (rslt != BMI2_OK) { + imu_h->last_err = IMU_ERR_GET_SENSOR_DATA; // save error code for later handling + return; + } + + imu_h->last_err = IMU_OK; // "reset" last error + + if (imu_h->sensor_data.status & BMI2_DRDY_ACC) { + imu_h->acc_x = lsb_to_mps2(imu_h->sensor_data.acc.x, 2, 16); + imu_h->acc_y = lsb_to_mps2(imu_h->sensor_data.acc.y, 2, 16); + imu_h->acc_z = lsb_to_mps2(imu_h->sensor_data.acc.z, 2, 16); + + } + else { + imu_h->last_err = IMU_WARN_ACC_READ_NOT_READY; + } + + if (imu_h->sensor_data.status & BMI2_DRDY_GYR) { + imu_h->gyr_x = roundf(lsb_to_dps(imu_h->sensor_data.gyr.x, 2000, 16) * 100) / 100; + imu_h->gyr_y = roundf(lsb_to_dps(imu_h->sensor_data.gyr.y, 2000, 16) * 100) / 100; + imu_h->gyr_z = roundf(lsb_to_dps(imu_h->sensor_data.gyr.z, 2000, 16) * 100) / 100; + } + else { + if (imu_h->last_err == IMU_WARN_ACC_READ_NOT_READY) // if both acc and gyro are not ready + imu_h->last_err = IMU_WARN_GYRO_AND_ACC_READ_NOT_READY; + else // if only gyro is not ready + imu_h->last_err = IMU_WARN_GYRO_READ_NOT_READY; + } + LOGD("acc: %f | %f | %f, gyr: %.2f | %.2f | %.2f", imu_h->acc_x, imu_h->acc_y, imu_h->acc_z, imu_h->gyr_x, imu_h->gyr_y, imu_h->gyr_z); +} +// alias for SPI_soft_read (to feed in spi_h handler struct) +int8_t imu_spi_read_reg(uint8_t reg_addr, uint8_t *reg_data, uint32_t len, void *intf_ptr) { + len++; // add one byte for the reg_addr + + uint8_t tx_buffer[len]; + + //empty tx buffer + for (uint32_t i = 0; i < len; i++) { + tx_buffer[i] = 0; + } + + tx_buffer[0] = reg_addr; + + uint8_t rx_buffer[len]; + + SPI_soft_trx((SPI_handle_t *)intf_ptr, tx_buffer, rx_buffer, len); + + + // Copy the received data into reg_data, starting from rx_buffer[1] + for (uint32_t i = 0; i < len - 1; i++) { + reg_data[i] = rx_buffer[i + 1]; + delay(1); + } + + return 0; +} + + +// alias for SPI_soft_write (to feed in spi_h handler struct) +int8_t imu_spi_write_reg(uint8_t reg_addr, const uint8_t *reg_data, uint32_t len, void *intf_ptr) { + uint8_t tx_buffer[len + 1]; + tx_buffer[0] = reg_addr; + + // Copy data to be written into the tx_buffer + for (uint32_t i = 0; i < len; i++) { + tx_buffer[i + 1] = reg_data[i]; + } + + uint8_t rx_buffer[len + 1]; + SPI_soft_trx((SPI_handle_t *)intf_ptr, tx_buffer, rx_buffer, len + 1); + return 0; +} + + +void imu_delay_us(uint32_t period, void *intf_ptr) { + delay(1); +} + +/*! + * @brief This internal API is used to set configurations for accel and gyro. + */ +static int8_t set_accel_gyro_config(struct bmi2_dev *bmi) +{ + /* Status of api are returned to this variable. */ + int8_t rslt; + + /* Structure to define accelerometer and gyro configuration. */ + struct bmi2_sens_config config[2]; + + /* Configure the type of feature. */ + config[ACCEL].type = BMI2_ACCEL; + config[GYRO].type = BMI2_GYRO; + + /* Get default configurations for the type of feature selected. */ + rslt = bmi2_get_sensor_config(config, 2, bmi); + bmi2_error_codes_print_result(rslt); + + /* Map data ready interrupt to interrupt pin. */ + rslt = bmi2_map_data_int(BMI2_DRDY_INT, BMI2_INT1, bmi); + bmi2_error_codes_print_result(rslt); + + if (rslt == BMI2_OK) + { + /* NOTE: The user can change the following configuration parameters according to their requirement. */ + /* Set Output Data Rate */ + config[ACCEL].cfg.acc.odr = BMI2_ACC_ODR_200HZ; + + /* Gravity range of the sensor (+/- 2G, 4G, 8G, 16G). */ + config[ACCEL].cfg.acc.range = BMI2_ACC_RANGE_2G; + + /* The bandwidth parameter is used to configure the number of sensor samples that are averaged + * if it is set to 2, then 2^(bandwidth parameter) samples + * are averaged, resulting in 4 averaged samples. + * Note1 : For more information, refer the datasheet. + * Note2 : A higher number of averaged samples will result in a lower noise level of the signal, but + * this has an adverse effect on the power consumed. + */ + config[ACCEL].cfg.acc.bwp = BMI2_ACC_NORMAL_AVG4; + + /* Enable the filter performance mode where averaging of samples + * will be done based on above set bandwidth and ODR. + * There are two modes + * 0 -> Ultra low power mode + * 1 -> High performance mode(Default) + * For more info refer datasheet. + */ + config[ACCEL].cfg.acc.filter_perf = BMI2_PERF_OPT_MODE; + + /* The user can change the following configuration parameters according to their requirement. */ + /* Set Output Data Rate */ + config[GYRO].cfg.gyr.odr = BMI2_GYR_ODR_200HZ; + + /* Gyroscope Angular Rate Measurement Range.By default the range is 2000dps. */ + config[GYRO].cfg.gyr.range = BMI2_GYR_RANGE_2000; + + /* Gyroscope bandwidth parameters. By default the gyro bandwidth is in normal mode. */ + config[GYRO].cfg.gyr.bwp = BMI2_GYR_NORMAL_MODE; + + /* Enable/Disable the noise performance mode for precision yaw rate sensing + * There are two modes + * 0 -> Ultra low power mode(Default) + * 1 -> High performance mode + */ + config[GYRO].cfg.gyr.noise_perf = BMI2_POWER_OPT_MODE; + + /* Enable/Disable the filter performance mode where averaging of samples + * will be done based on above set bandwidth and ODR. + * There are two modes + * 0 -> Ultra low power mode + * 1 -> High performance mode(Default) + */ + config[GYRO].cfg.gyr.filter_perf = BMI2_PERF_OPT_MODE; + + /* Set the accel and gyro configurations. */ + rslt = bmi2_set_sensor_config(config, 2, bmi); + bmi2_error_codes_print_result(rslt); + } + + return rslt; +} + +/*! + * @brief This function converts lsb to meter per second squared for 16 bit accelerometer at + * range 2G, 4G, 8G or 16G. + */ +static float lsb_to_mps2(int16_t val, float g_range, uint8_t bit_width) +{ + double power = 2; + + float half_scale = (float)((pow((double)power, (double)bit_width) / 2.0f)); + + return (GRAVITY_EARTH * val * g_range) / half_scale; +} + +/*! + * @brief This function converts lsb to degree per second for 16 bit gyro at + * range 125, 250, 500, 1000 or 2000dps. + */ +static float lsb_to_dps(int16_t val, float dps, uint8_t bit_width) +{ + double power = 2; + + float half_scale = (float)((pow((double)power, (double)bit_width) / 2.0f)); + + return (dps / (half_scale)) * (val); +} + +void bmi2_error_codes_print_result(int8_t rslt) +{ + switch (rslt) + { + case BMI2_OK: + + /* Do nothing */ + break; + + case BMI2_W_FIFO_EMPTY: + LOGW("Warning [%d] : FIFO empty\r\n", rslt); + break; + case BMI2_W_PARTIAL_READ: + LOGW("Warning [%d] : FIFO partial read\r\n", rslt); + break; + case BMI2_E_NULL_PTR: + LOGE("Error [%d] : Null pointer error. It occurs when the user tries to assign value (not address) to a pointer," " which has been initialized to NULL.\r\n",rslt); + break; + + case BMI2_E_COM_FAIL: + LOGE("Error [%d] : Communication failure error. It occurs due to read/write operation failure and also due " "to power failure during communication\r\n", rslt); + break; + + case BMI2_E_DEV_NOT_FOUND: + LOGE("Error [%d] : Device not found error. It occurs when the device chip id is incorrectly read\r\n",rslt); + break; + + case BMI2_E_INVALID_SENSOR: + LOGE("Error [%d] : Invalid sensor error. It occurs when there is a mismatch in the requested feature with the " "available one\r\n", rslt); + break; + + case BMI2_E_SELF_TEST_FAIL: + LOGE("Error [%d] : Self-test failed error. It occurs when the validation of accel self-test data is " "not satisfied\r\n",rslt); + break; + + case BMI2_E_INVALID_INT_PIN: + LOGE("Error [%d] : Invalid interrupt pin error. It occurs when the user tries to configure interrupt pins " "apart from INT1 and INT2\r\n",rslt); + break; + + case BMI2_E_OUT_OF_RANGE: + LOGE("Error [%d] : Out of range error. It occurs when the data exceeds from filtered or unfiltered data from " "fifo and also when the range exceeds the maximum range for accel and gyro while performing FOC\r\n",rslt); + break; + + case BMI2_E_ACC_INVALID_CFG: + LOGE("Error [%d] : Invalid Accel configuration error. It occurs when there is an error in accel configuration" " register which could be one among range, BW or filter performance in reg address 0x40\r\n",rslt); + break; + + case BMI2_E_GYRO_INVALID_CFG: + LOGE("Error [%d] : Invalid Gyro configuration error. It occurs when there is a error in gyro configuration" "register which could be one among range, BW or filter performance in reg address 0x42\r\n",rslt); + break; + + case BMI2_E_ACC_GYR_INVALID_CFG: + LOGE("Error [%d] : Invalid Accel-Gyro configuration error. It occurs when there is a error in accel and gyro" " configuration registers which could be one among range, BW or filter performance in reg address 0x40 " "and 0x42\r\n",rslt); + break; + + case BMI2_E_CONFIG_LOAD: + LOGE("Error [%d] : Configuration load error. It occurs when failure observed while loading the configuration " "into the sensor\r\n",rslt); + break; + + case BMI2_E_INVALID_PAGE: + LOGE("Error [%d] : Invalid page error. It occurs due to failure in writing the correct feature configuration " "from selected page\r\n",rslt); + break; + + case BMI2_E_SET_APS_FAIL: + LOGE("Error [%d] : APS failure error. It occurs due to failure in write of advance power mode configuration " "register\r\n",rslt); + break; + + case BMI2_E_AUX_INVALID_CFG: + LOGE("Error [%d] : Invalid AUX configuration error. It occurs when the auxiliary interface settings are not " "enabled properly\r\n",rslt); + break; + + case BMI2_E_AUX_BUSY: + LOGE("Error [%d] : AUX busy error. It occurs when the auxiliary interface buses are engaged while configuring" " the AUX\r\n",rslt); + break; + + case BMI2_E_REMAP_ERROR: + LOGE("Error [%d] : Remap error. It occurs due to failure in assigning the remap axes data for all the axes " "after change in axis position\r\n",rslt); + break; + + case BMI2_E_GYR_USER_GAIN_UPD_FAIL: + LOGE("Error [%d] : Gyro user gain update fail error. It occurs when the reading of user gain update status " "fails\r\n",rslt); + break; + + case BMI2_E_SELF_TEST_NOT_DONE: + LOGE("Error [%d] : Self-test not done error. It occurs when the self-test process is ongoing or not " "completed\r\n",rslt); + break; + + case BMI2_E_INVALID_INPUT: + LOGE("Error [%d] : Invalid input error. It occurs when the sensor input validity fails\r\n", rslt); + break; + + case BMI2_E_INVALID_STATUS: + LOGE("Error [%d] : Invalid status error. It occurs when the feature/sensor validity fails\r\n", rslt); + break; + + case BMI2_E_CRT_ERROR: + LOGE("Error [%d] : CRT error. It occurs when the CRT test has failed\r\n", rslt); + break; + + case BMI2_E_ST_ALREADY_RUNNING: + LOGE("Error [%d] : Self-test already running error. It occurs when the self-test is already running and " "another has been initiated\r\n",rslt); + break; + + case BMI2_E_CRT_READY_FOR_DL_FAIL_ABORT: + LOGE("Error [%d] : CRT ready for download fail abort error. It occurs when download in CRT fails due to wrong " "address location\r\n",rslt); + break; + + case BMI2_E_DL_ERROR: + LOGE("Error [%d] : Download error. It occurs when write length exceeds that of the maximum burst length\r\n",rslt); + break; + + case BMI2_E_PRECON_ERROR: + LOGE("Error [%d] : Pre-conditional error. It occurs when precondition to start the feature was not " "completed\r\n",rslt); + break; + + case BMI2_E_ABORT_ERROR: + LOGE("Error [%d] : Abort error. It occurs when the device was shaken during CRT test\r\n", rslt); + break; + + case BMI2_E_WRITE_CYCLE_ONGOING: + LOGE("Error [%d] : Write cycle ongoing error. It occurs when the write cycle is already running and another " "has been initiated\r\n", rslt); + break; + + case BMI2_E_ST_NOT_RUNING: + LOGE("Error [%d] : Self-test is not running error. It occurs when self-test running is disabled while it's " "running\r\n", rslt); + break; + + case BMI2_E_DATA_RDY_INT_FAILED: + LOGE("Error [%d] : Data ready interrupt error. It occurs when the sample count exceeds the FOC sample limit " "and data ready status is not updated\r\n", rslt); + break; + + case BMI2_E_INVALID_FOC_POSITION: + LOGE( "Error [%d] : Invalid FOC position error. It occurs when average FOC data is obtained for the wrong" " axes\r\n", rslt); + break; + + default: + LOGE("Error [%d] : Unknown error code\r\n", rslt); + break; + } +} + +void log_imu_err(IMU_err_t err) { + switch (err) { + case IMU_OK: + LOGI("IMU_OK"); + break; + case IMU_ERR_BMI_INIT: + LOGE("IMU_ERR_BMI_INIT (imu_init)"); + break; + case IMU_ERR_ACC_GYR_CONFIG: + LOGE("IMU_ERR_ACC_GYR_CONFIG (imu_init)"); + break; + case IMU_ERR_SENSOR_ENABLE: + LOGE("IMU_ERR_SENSOR_ENABLE (imu_init)"); + break; + case IMU_ERR_GET_SENSOR_CONFIG: + LOGE("IMU_ERR_GET_SENSOR_CONFIG (imu_init)"); + break; + case IMU_ERR_GET_SENSOR_DATA: + LOGE("IMU_ERR_GET_SENSOR_DATA (imu_process)"); + break; + case IMU_WARN_GYRO_READ_NOT_READY: + LOGW("IMU_WARN_GYRO_READ_NOT_READY (imu_process)"); + break; + case IMU_WARN_ACC_READ_NOT_READY: + LOGW("IMU_WARN_ACC_READ_NOT_READY (imu_process)"); + break; + case IMU_WARN_GYRO_AND_ACC_READ_NOT_READY: + LOGW("IMU_WARN_GYRO_AND_ACC_READ_NOT_READY (imu_process)"); + break; + default: + LOGE("Unknown IMU_err_t: %d", err); + break; + } +} + +// logs gyro and acc data on one line +void log_imu_data(IMU_handle_t *imu_h) { + LOGD("acc: %f %f %f, gyr: %f %f %f", imu_h->acc_x, imu_h->acc_y, imu_h->acc_z, imu_h->gyr_x, imu_h->gyr_y, imu_h->gyr_z); +} \ No newline at end of file diff --git a/software/MySrc/Drivers/imu.h b/software/MySrc/Drivers/imu.h new file mode 100644 index 0000000..a6baf7f --- /dev/null +++ b/software/MySrc/Drivers/imu.h @@ -0,0 +1,44 @@ + + +#ifndef BETTERFLIGHT_IMU_H +#define BETTERFLIGHT_IMU_H + +#endif //BETTERFLIGHT_IMU_H + +#include "spi_soft.h" +#include "bmi270.h" + + +typedef enum { + IMU_OK = 0, + IMU_ERR_BMI_INIT = 1, + IMU_ERR_ACC_GYR_CONFIG = 2, + IMU_ERR_SENSOR_ENABLE = 3, + IMU_ERR_GET_SENSOR_CONFIG = 4, + IMU_ERR_GET_SENSOR_DATA = 5, + IMU_WARN_GYRO_READ_NOT_READY = 6, + IMU_WARN_ACC_READ_NOT_READY = 7, + IMU_WARN_GYRO_AND_ACC_READ_NOT_READY = 8, +} IMU_err_t; + +typedef struct { + SPI_handle_t spi_h; + uint8_t sensor_list[2]; + struct bmi2_dev bmi; + struct bmi2_sens_data sensor_data; + float acc_x, acc_y, acc_z; + float gyr_x, gyr_y, gyr_z; + struct bmi2_sens_config config; + IMU_err_t last_err; //this is the last error that occured in the imu_process function. instead of returning the error code it gets saved here to be handled +} IMU_handle_t; + +// for implementation of imu.c +IMU_err_t imu_init(IMU_handle_t *imu_h); +void imu_process(IMU_handle_t *imu_h); +void log_imu_err(IMU_err_t err); +void log_gyr_acc_data(IMU_handle_t *imu_h); + +// bmi270 api needs access to these functions +int8_t imu_spi_read_reg(uint8_t reg_addr, uint8_t *reg_data, uint32_t len, void *intf_ptr); // function pointers with api from bmi270 +int8_t imu_spi_write_reg(uint8_t reg_addr, const uint8_t *reg_data, uint32_t len, void *intf_ptr); +void imu_delay_us(uint32_t period, void *intf_ptr); diff --git a/software/MySrc/Drivers/spi.c b/software/MySrc/Drivers/spi.c new file mode 100644 index 0000000..503c12d --- /dev/null +++ b/software/MySrc/Drivers/spi.c @@ -0,0 +1,249 @@ +/* + SPI driver for STM32F7xx microcontrollers + */ + + + +#include "spi.h" +#include "stm32f7xx.h" + +#include "log.h" + +#define CS_HIGH spi_h->cs.port->BSRR = spi_h->cs.pin +#define CS_LOW spi_h->cs.port->BSRR = (spi_h->cs.pin << 16) + +// SPI control macros +#define SPI_ENABLE spi_h->SPIx->CR1 |= SPI_CR1_SPE // Enable SPI peripheral +#define SPI_DISABLE spi_h->SPIx->CR1 &= ~SPI_CR1_SPE // Disable SPI peripheral + +// SPI flag read macros +#define SPI_TXE_FLAG (spi_h->SPIx->SR & SPI_SR_TXE) // TX buffer empty flag +#define SPI_RXNE_FLAG (spi_h->SPIx->SR & SPI_SR_RXNE) // RX buffer not empty flag +#define SPI_BSY_FLAG (spi_h->SPIx->SR & SPI_SR_BSY) // SPI busy flag +#define SPI_MODF_FLAG (spi_h->SPIx->SR & SPI_SR_MODF) // Mode fault flag +#define SPI_OVR_FLAG (spi_h->SPIx->SR & SPI_SR_OVR) // Overrun flag +#define SPI_FRE_FLAG (spi_h->SPIx->SR & SPI_SR_FRE) // Frame format error flag + + +// SPI data read/write macros +#define SPI_WRITE_DATA(data) spi_h->SPIx->DR = data // Write data to SPI peripheral +#define SPI_READ_DATA spi_h->SPIx->DR // Read data from SPI peripheral + + +void SPI_init(SPI_handle_t *spi_h) { + /* + + use of CMSIS to configure and control the hardware SPI peripheral + + notes: + - enable SPI CS before master sending sck (aka setting -> SPI_CR1_SPE) + master will start to communicate when SPI enabled AND TXFIFO not empty (or with the next write to TXFIFO) + + registers: + config: CR1, CR2 + status/data: + - SR: status register most important bits: + FTLVL[1:0]: FIFO transmission level (00: empty, 01: 1/4, 10: 1/2, 11: full) + FRLVL[1:0]: FIFO reception level (00: empty, 01: 1/4, 10: 1/2, 11: full) + FRE: frame format error flag + BSY: busy flag + TXE: transmit buffer empty flag + RXNE : receive buffer not empty flag + - DR: data register: + 16-bit data register, write to send, read to receive + + + reminder: + '&=' -> resetting a bit (LOW, 0) + '|=' -> setting a bit (HIGH, 1) + + */ + + //SPI_DISABLE; //make sure spi is disabled when configuring its registers + /* + // Enable GPIO clocks based on which ports are used + if (spi_h->sck.port == GPIOA || spi_h->miso.port == GPIOA || spi_h->mosi.port == GPIOA) { + RCC->AHB1ENR |= RCC_AHB1ENR_GPIOAEN; + LOGD("Found GPIOA"); + } + if (spi_h->sck.port == GPIOB || spi_h->miso.port == GPIOB || spi_h->mosi.port == GPIOB) { + __HAL_RCC_GPIOB_CLK_ENABLE(); + LOGD("Found GPIOA"); + } + if (spi_h->sck.port == GPIOC || spi_h->miso.port == GPIOC || spi_h->mosi.port == GPIOC) { + __HAL_RCC_GPIOC_CLK_ENABLE(); + } + if (spi_h->sck.port == GPIOD || spi_h->miso.port == GPIOD || spi_h->mosi.port == GPIOD) { + __HAL_RCC_GPIOD_CLK_ENABLE(); + } + + uint32_t gpio_af = GPIO_AF5_SPI1; //contains the GPIO_AF5_SPIx value + + //"select" HAL GPIO_AF_SPIx based on what SPIx is used + if (spi_h->SPIx == SPI1) { + RCC->AHB2ENR |= RCC_APB2ENR_SPI1EN; + gpio_af = GPIO_AF5_SPI1; + LOGD("Found SPI1"); + } else if (spi_h->SPIx == SPI2) { + RCC->AHB1ENR |= RCC_APB1ENR_SPI2EN; + gpio_af = GPIO_AF5_SPI2; + LOGD("Found SPI2"); + } else if (spi_h->SPIx == SPI3) { + RCC->AHB1ENR |= RCC_APB1ENR_SPI3EN; + gpio_af = GPIO_AF5_SPI3; + LOGD("Found SPI3"); + } + */ + + RCC->AHB1ENR |= RCC_AHB1ENR_GPIOAEN; + RCC->APB2ENR |= RCC_APB2ENR_SPI1EN; + + // Configure GPIO pins + GPIO_InitTypeDef GPIO_InitStruct = {0}; + + // SCK + GPIO_InitStruct.Pin = spi_h->sck.pin; + GPIO_InitStruct.Mode = GPIO_MODE_AF_PP; + GPIO_InitStruct.Pull = GPIO_NOPULL; + GPIO_InitStruct.Speed = GPIO_SPEED_FREQ_VERY_HIGH; + GPIO_InitStruct.Alternate = GPIO_AF5_SPI1; + HAL_GPIO_Init(spi_h->sck.port, &GPIO_InitStruct); + + // MISO + GPIO_InitStruct.Pin = spi_h->miso.pin; + GPIO_InitStruct.Mode = GPIO_MODE_AF_PP; + GPIO_InitStruct.Pull = GPIO_NOPULL; + GPIO_InitStruct.Speed = GPIO_SPEED_FREQ_VERY_HIGH; + GPIO_InitStruct.Alternate = GPIO_AF5_SPI1; + HAL_GPIO_Init(spi_h->miso.port, &GPIO_InitStruct); + + // MOSI + GPIO_InitStruct.Pin = spi_h->mosi.pin; + GPIO_InitStruct.Mode = GPIO_MODE_AF_PP; + GPIO_InitStruct.Pull = GPIO_NOPULL; + GPIO_InitStruct.Speed = GPIO_SPEED_FREQ_VERY_HIGH; + GPIO_InitStruct.Alternate = GPIO_AF5_SPI1; + HAL_GPIO_Init(spi_h->mosi.port, &GPIO_InitStruct); + + // CS + GPIO_InitStruct.Pin = spi_h->cs.pin; + GPIO_InitStruct.Mode = GPIO_MODE_OUTPUT_PP; + GPIO_InitStruct.Pull = GPIO_NOPULL; + GPIO_InitStruct.Speed = GPIO_SPEED_FREQ_VERY_HIGH; + HAL_GPIO_Init(spi_h->cs.port, &GPIO_InitStruct); + CS_HIGH; + + + // Clear CR1 & CR2 registers + spi_h->SPIx->CR1 = 0; + spi_h->SPIx->CR2 = 0; + spi_h->SPIx->I2SCFGR = 0; + + // CR1 register config + spi_h->SPIx->CR1 &= ~SPI_CR1_CPOL; // Clock polarity = 0 + spi_h->SPIx->CR1 |= SPI_CR1_BR_2; // BaudRate[2:0] = 100 -> fPCLK/32 + spi_h->SPIx->CR1 &= ~SPI_CR1_BR_1; + spi_h->SPIx->CR1 &= ~SPI_CR1_BR_0; + spi_h->SPIx->CR1 &= ~SPI_CR1_CPHA; // Clock phase = 0 + spi_h->SPIx->CR1 &= ~SPI_CR1_BIDIMODE; // ->(0) 2-line, (1) 1-line + spi_h->SPIx->CR1 &= ~SPI_CR1_BIDIOE; // (rx vs tx only), not relevant... + spi_h->SPIx->CR1 &= ~SPI_CR1_RXONLY; // ->(0) full duplex, (1) output disabled + spi_h->SPIx->CR1 &= ~SPI_CR1_LSBFIRST; // ->(0) MSB first, (1) LSB first + spi_h->SPIx->CR1 &= ~SPI_CR1_CRCEN; // CRC Enable: ->(0) disabled, (1) enabled + spi_h->SPIx->CR1 &= ~SPI_CR1_CRCL; // CRC length, not relevant... + spi_h->SPIx->CR1 &= ~SPI_CR1_CRCNEXT; // CRC next, not relevant... + spi_h->SPIx->CR1 |= SPI_CR1_SSM; // Software slave management: (0) hardware, ->(1) software + spi_h->SPIx->CR1 &= ~SPI_CR1_SSI; // Internal slave select, not relevant... + spi_h->SPIx->CR1 |= SPI_CR1_MSTR; // (0) slave, ->(1) master + spi_h->SPIx->CR1 &= ~SPI_CR1_SPE; // SPI enable, ->(0) disabled, (1) enabled + + // CR2 register config + spi_h->SPIx->CR2 &= ~SPI_CR2_DS_3; // DataSize[3:0] = 0111 -> 8 bits + spi_h->SPIx->CR2 |= SPI_CR2_DS_2; + spi_h->SPIx->CR2 |= SPI_CR2_DS_1; + spi_h->SPIx->CR2 |= SPI_CR2_DS_0; + spi_h->SPIx->CR2 &= ~SPI_CR2_SSOE; // SS output, not relevant... (using software slave management) + spi_h->SPIx->CR2 &= ~SPI_CR2_FRF; // ->(0) SPI Motorola mode, (1) TI mode + spi_h->SPIx->CR2 &= ~SPI_CR2_NSSP; // generate NSS pulse, not relevant... (using software slave management) + spi_h->SPIx->CR2 |= SPI_CR2_FRXTH; // FIFO reception threshold, (0) RXNE event when 16-bit, ->(1) RXNE event when 8-bit + spi_h->SPIx->CR2 &= ~SPI_CR2_RXDMAEN; // RX buffer DMA enable, ->(0) disabled, (1) enabled + spi_h->SPIx->CR2 &= ~SPI_CR2_TXDMAEN; // TX buffer DMA enable, ->(0) disabled, (1) enabled + spi_h->SPIx->CR2 &= ~SPI_CR2_LDMARX; // DMA reception, not relevant... + spi_h->SPIx->CR2 &= ~SPI_CR2_LDMATX; // DMA transmission, not relevant... + spi_h->SPIx->CR2 &= ~SPI_CR2_TXEIE; // TX buffer empty interrupt enable, (0) TXEIE interrupt masked, ->(1) TXEIE interrupt not masked (gens interrupt) + spi_h->SPIx->CR2 &= ~SPI_CR2_RXNEIE; // RX buffer not empty interrupt enable, (0) RXNEIE interrupt masked, ->(1) RXNEIE interrupt not masked (gens interrupt) + spi_h->SPIx->CR2 &= ~SPI_CR2_ERRIE; // Error interrupt enable, (0) ERRIE interrupt masked, ->(1) ERRIE interrupt enabled + + // I2SCFGR register config + spi_h->SPIx->I2SCFGR &= ~SPI_I2SCFGR_I2SMOD; // ->(0) SPI mode, (1) I2S mode + spi_h->SPIx->I2SCFGR &= ~SPI_I2SCFGR_I2SE; // ->(0) I2S peripheral disabled, (1) I2S peripheral enabled + + delay(100); + + + //wait until SPI_CR1_SPE reads as set +// while (!(spi_h->SPIx->CR1 & SPI_CR1_SPE)); + + //print out the CR1, CR2, I2SCFGR registers + LOGD("CR1: %s", byte_to_binary_str(spi_h->SPIx->CR1)); + delay(100); + LOGD("CR2: %s", byte_to_binary_str(spi_h->SPIx->CR2)); + delay(100); + LOGD("I2SCFGR: %s", byte_to_binary_str(spi_h->SPIx->I2SCFGR)); + delay(100); + LOGD("SPI status register: %s", byte_to_binary_str(spi_h->SPIx->SR)); + delay(100); + + SPI_ENABLE; + delay(100); + + LOGD("SPI status register: %s", byte_to_binary_str(spi_h->SPIx->SR)); + + +} + + +// CMSIS Hardware SPI transmit & receive +void SPI_transmit_rx(SPI_handle_t *spi_h, uint8_t *tx_data, uint8_t *rx_data, uint32_t len) { + + // check for MODF error + if (SPI_MODF_FLAG) { + LOGD("MODF error detected"); + volatile uint32_t clear_modf = spi_h->SPIx->SR; //clear MODF flag + SPI_ENABLE; //re-enable SPI + CS_HIGH; //disable CS + spi_h->SPIx->CR1 |= SPI_CR1_SPE | SPI_CR1_MSTR; //restore to re-enable + } + + // enable cs + CS_LOW; + + // Wait for TX buffer to be empty + while (!SPI_TXE_FLAG); + LOGD("TXE flag set"); + + // Write data + spi_h->SPIx->DR = 0x80; + LOGD("was able to write data"); + + // Wait for RX buffer to be filled + //while (!SPI_RXNE_FLAG); + LOGD("RXNE flag set"); + // disable cs + CS_HIGH; + + //rx_data = (uint8_t*) SPI_READ_DATA; + LOGD("Read rx data"); + +// for (uint32_t i = 0; i < len; i++) { //for each byte +// uint8_t tx_byte = tx_data[i]; +// uint8_t rx_byte = 0; +// +// +// +// +// // store received byte +// rx_data[i] = rx_byte; +// } + +} \ No newline at end of file diff --git a/software/MySrc/Drivers/spi.h b/software/MySrc/Drivers/spi.h new file mode 100644 index 0000000..c71574d --- /dev/null +++ b/software/MySrc/Drivers/spi.h @@ -0,0 +1,24 @@ + +#include "stm32f7xx.h" + +#ifndef BETTERFLIGHT_SPI_H +#define BETTERFLIGHT_SPI_H + +typedef struct { + GPIO_TypeDef *port; + uint16_t pin; +} SPI_GPIO; + + +typedef struct { + SPI_TypeDef *SPIx; // CMSIS SPI peripheral "registers" + SPI_GPIO mosi; + SPI_GPIO miso; + SPI_GPIO sck; + SPI_GPIO cs; +}SPI_handle_t; + +void SPI_init(SPI_handle_t *hspi); +void SPI_transmit_rx(SPI_handle_t *spi_h, uint8_t *tx_data, uint8_t *rx_data, uint32_t len); + +#endif //BETTERFLIGHT_SPI_H diff --git a/software/MySrc/Drivers/spi_soft.c b/software/MySrc/Drivers/spi_soft.c new file mode 100644 index 0000000..5e504b4 --- /dev/null +++ b/software/MySrc/Drivers/spi_soft.c @@ -0,0 +1,221 @@ + + + +#include "spi_soft.h" +#include "misc.h" +#include "log.h" + +__attribute__((optimize("O0"))) void SPI_Delay(uint32_t cycles); +static uint32_t calc_nops_spi_half_cycle(uint32_t mcu_clk_speed_hz, uint32_t spi_clock_speed_hz); + + + +#define MOSI_HIGH spi_h->mosi.port->BSRR = (spi_h->mosi.pin) +#define MOSI_LOW spi_h->mosi.port->BSRR = (spi_h->mosi.pin << 16) + +#define SCK_HIGH spi_h->sck.port->BSRR = (spi_h->sck.pin) +#define SCK_LOW spi_h->sck.port->BSRR = (spi_h->sck.pin << 16) + +#define CS_HIGH spi_h->cs.port->BSRR = (spi_h->cs.pin) +#define CS_LOW spi_h->cs.port->BSRR = (spi_h->cs.pin << 16) + + +// Initialize the SPI pins (requires the user to have configured the pins a bit already... (like clocks and stuff)) +void SPI_soft_init(SPI_handle_t *spi_h) { + + // calc number of NOPs to delay for half of the SPI clock cycle +// spi_h->nop_cnt = calc_nops_spi_half_cycle(get_mcu_clock_speed(), spi_speed_hz); + + GPIO_InitTypeDef GPIO_InitStruct = {0}; + + //init mosi + GPIO_InitStruct.Pin = spi_h->mosi.pin; + GPIO_InitStruct.Mode = GPIO_MODE_OUTPUT_PP; + GPIO_InitStruct.Pull = GPIO_NOPULL; + GPIO_InitStruct.Speed = GPIO_SPEED_FREQ_HIGH; + HAL_GPIO_Init(spi_h->mosi.port, &GPIO_InitStruct); + MOSI_LOW; + + //init miso + GPIO_InitStruct.Pin = spi_h->miso.pin; + GPIO_InitStruct.Mode = GPIO_MODE_INPUT; + GPIO_InitStruct.Pull = GPIO_NOPULL; + HAL_GPIO_Init(spi_h->miso.port, &GPIO_InitStruct); + + //init sck + GPIO_InitStruct.Pin = spi_h->sck.pin; + GPIO_InitStruct.Mode = GPIO_MODE_OUTPUT_PP; + GPIO_InitStruct.Pull = GPIO_NOPULL; + GPIO_InitStruct.Speed = GPIO_SPEED_FREQ_HIGH; + HAL_GPIO_Init(spi_h->sck.port, &GPIO_InitStruct); + SCK_HIGH; + + //init cs + GPIO_InitStruct.Pin = spi_h->cs.pin; + GPIO_InitStruct.Mode = GPIO_MODE_OUTPUT_PP; + GPIO_InitStruct.Pull = GPIO_NOPULL; + GPIO_InitStruct.Speed = GPIO_SPEED_FREQ_HIGH; + HAL_GPIO_Init(spi_h->cs.port, &GPIO_InitStruct); + CS_HIGH; +} + + +#define NOP_CNT 20 +void SPI_soft_trx(SPI_handle_t *spi_h, uint8_t *tx_data, uint8_t *rx_data, uint32_t len) { + + // turn off interrupts + __disable_irq(); + + // enable cs + CS_LOW; + SPI_Delay(NOP_CNT); + + for (uint32_t i = 0; i < len; i++) { //for each byte + uint8_t tx_byte = tx_data[i]; + uint8_t rx_byte = 0; + + for (uint8_t j = 0; j < 8; j++) { //for each bit + + // set mosi + if (tx_byte & 0x80) { + MOSI_HIGH; + } else { + MOSI_LOW; + } + + // shift tx_byte + tx_byte <<= 1; + + SCK_LOW; + SPI_Delay(NOP_CNT); + + // read miso + rx_byte <<= 1; + if (spi_h->miso.port->IDR & spi_h->miso.pin) { // if miso is high + rx_byte |= 0x01; // set the last bit + } + + SCK_HIGH; + SPI_Delay(NOP_CNT); + } + MOSI_LOW; + + rx_data[i] = rx_byte; + } + + // disable cs + CS_HIGH; + + // enable interrupts again + __enable_irq(); +} + +void SPI_soft_tx(SPI_handle_t *spi_h, uint8_t *tx_data, uint32_t len) { + + // turn off interrupts + __disable_irq(); + + // enable cs + CS_LOW; + SPI_Delay(NOP_CNT); + + for (uint32_t i = 0; i < len; i++) { //for each byte + uint8_t tx_byte = tx_data[i]; + + for (uint8_t j = 0; j < 8; j++) { //for each bit + + // set mosi + if (tx_byte & 0x80) { + MOSI_HIGH; + } else { + MOSI_LOW; + } + + // shift tx_byte + tx_byte <<= 1; + + SCK_LOW; + SPI_Delay(NOP_CNT); + + SCK_HIGH; + SPI_Delay(NOP_CNT); + } + MOSI_LOW; + } + + // disable cs + CS_HIGH; + + // enable interrupts again + __enable_irq(); +} + +void SPI_soft_burst_tx(SPI_handle_t *spi_h, uint8_t *tx_data, uint32_t len) { + + // turn off interrupts + __disable_irq(); + + // enable cs + CS_LOW; + SPI_Delay(NOP_CNT); + + for (uint32_t i = 0; i < len; i++) { //for each byte + uint8_t tx_byte = tx_data[i]; + + for (uint8_t j = 0; j < 8; j++) { //for each bit + + // set mosi + if (tx_byte & 0x80) { + MOSI_HIGH; + } else { + MOSI_LOW; + } + + // shift tx_byte + tx_byte <<= 1; + + SCK_LOW; + SPI_Delay(NOP_CNT); + + SCK_HIGH; + SPI_Delay(NOP_CNT); + } + MOSI_LOW; + } + + // disable cs + CS_HIGH; + + // enable interrupts again + __enable_irq(); +} + +void SPI_soft_cs_high(SPI_handle_t *spi_h) { + CS_HIGH; +} + +void SPI_soft_cs_low(SPI_handle_t *spi_h) { + CS_LOW; +} + + + +__attribute__((optimize("O0"))) void SPI_Delay(uint32_t cycles) { + while (cycles--) {__NOP();} +} + +// calc delay in nop cycles for given spi speed and mcu speed +uint32_t calc_nops_spi_half_cycle(uint32_t mcu_clk_speed_hz, uint32_t spi_clock_speed_hz) { + //calc period of spi clock in ns + uint32_t spiPeriodNs = 1000000000 / spi_clock_speed_hz; + + //calc period of mcu clock in ns + uint32_t mcuCycleDurationNs = 1000000000 / mcu_clk_speed_hz; + + //calc number of mcu cycles in one spi clock period + uint32_t nopCount = spiPeriodNs / mcuCycleDurationNs / 2; + + +// LOGD("nopCount: %d", nopCount); + return nopCount; +} \ No newline at end of file diff --git a/software/MySrc/Drivers/spi_soft.h b/software/MySrc/Drivers/spi_soft.h new file mode 100644 index 0000000..a34b72d --- /dev/null +++ b/software/MySrc/Drivers/spi_soft.h @@ -0,0 +1,32 @@ +#ifndef BETTERFLIGHT_SPI_SOFT_H +#define BETTERFLIGHT_SPI_SOFT_H + +#include "stm32f7xx_hal.h" + +typedef struct { + GPIO_TypeDef *port; + uint16_t pin; +} SPI_GPIO; + +typedef struct { + SPI_GPIO mosi; + SPI_GPIO miso; + SPI_GPIO sck; + SPI_GPIO cs; + uint32_t nop_cnt; // amount of NOPs to delay for half of the SPI clock cycle + +} SPI_handle_t; + + + +void SPI_soft_init(SPI_handle_t *spi_h); +void SPI_soft_trx(SPI_handle_t *spi_h, uint8_t *tx_data, uint8_t *rx_data, uint32_t len); +void SPI_soft_tx(SPI_handle_t *spi_h, uint8_t *tx_data, uint32_t len); +void SPI_soft_burst_tx(SPI_handle_t *spi_h, uint8_t *tx_data, uint32_t len); +void SPI_soft_cs_high(SPI_handle_t *spi_h); +void SPI_soft_cs_low(SPI_handle_t *spi_h); + + + + +#endif //BETTERFLIGHT_SPI_SOFT_H \ No newline at end of file diff --git a/software/MySrc/Flight/mixer.c b/software/MySrc/Flight/mixer.c new file mode 100644 index 0000000..00c211f --- /dev/null +++ b/software/MySrc/Flight/mixer.c @@ -0,0 +1,36 @@ +#include "mixer.h" + +void mixing(mixer_handle_t *mixer_h){ + int16_t max_roll = mixer_h->pid.roll_pid.limits.max_output; + int16_t max_pitch = mixer_h->pid.pitch_pid.limits.max_output; + int16_t max_yaw = mixer_h->pid.yaw_pid.limits.max_output; + float throttle = ((float)mixer_h->input.throttle)*mixer_h->percentages.throttle; + float roll = ((float)mixer_h->input.roll)* mixer_h->percentages.roll; + float pitch = ((float)mixer_h->input.pitch)* mixer_h->percentages.pitch; + float yaw = ((float)mixer_h->input.yaw)* mixer_h->percentages.yaw; + + LOGD("Throttle: %f, roll %f, pitch %f, yaw %f", throttle, roll, pitch, yaw); + HAL_Delay(10); + mixer_h->output.motor1 = (int16_t)((throttle + roll- pitch - yaw)); + mixer_h->output.motor2 = (int16_t)((throttle - roll - pitch + yaw)); + mixer_h->output.motor3 = (int16_t)((throttle - roll + pitch - yaw)); + mixer_h->output.motor4 = (int16_t)((throttle+ roll + pitch + yaw)); + + + + LOGD ("Motor 1: %d", mixer_h->output.motor1); + HAL_Delay(10); + LOGD ("Motor 2: %d", mixer_h->output.motor2); + HAL_Delay(10); + LOGD ("Motor 3: %d", mixer_h->output.motor3); + HAL_Delay(10); + LOGD ("Motor 4: %d", mixer_h->output.motor4); + HAL_Delay(10); +} + +void init_mixer_percentages(mixer_handle_t *mixer_h){ + mixer_h->percentages.roll = 0.7; + mixer_h->percentages.pitch = 0.7; + mixer_h->percentages.yaw = 0.4; + mixer_h->percentages.throttle = 1.0; +} \ No newline at end of file diff --git a/software/MySrc/Flight/mixer.h b/software/MySrc/Flight/mixer.h new file mode 100644 index 0000000..46e7f64 --- /dev/null +++ b/software/MySrc/Flight/mixer.h @@ -0,0 +1,48 @@ +#ifndef BETTERFLIGHT_MIXER_H +#define BETTERFLIGHT_MIXER_H +#include "stdint.h" +#include "pid.h" +#include "log.h" +#include +#include "misc.h" +#include "stm32f7xx_hal.h" + +typedef struct { + int16_t motor1; + int16_t motor2; + int16_t motor3; + int16_t motor4; +}motor_output_t; + + +typedef struct { + float roll; + float pitch; + float yaw; + float throttle; +}mixer_percentages_t; + +typedef struct{ + int16_t roll; + int16_t pitch; + int16_t yaw; + int16_t throttle; +}mixer_input_t; + +typedef struct{ + pid_handle_t roll_pid; + pid_handle_t pitch_pid; + pid_handle_t yaw_pid; +}mixer_pid_t; + +typedef struct{ + mixer_input_t input; + mixer_percentages_t percentages; + motor_output_t output; + mixer_pid_t pid; +}mixer_handle_t; + + +void mixing(mixer_handle_t *mixer_h); +void init_mixer_percentages(mixer_handle_t *mixer_h); +#endif //BETTERFLIGHT_MIXER_H diff --git a/software/MySrc/Flight/motors.c b/software/MySrc/Flight/motors.c new file mode 100644 index 0000000..b54fc6c --- /dev/null +++ b/software/MySrc/Flight/motors.c @@ -0,0 +1,124 @@ +/* + + this makes use of the dshot driver to give a higher level of 4 motor control + This does not include any PID control, only a better way to control 4 motors at a time. + + FRONT - TOP VIEW (QUAD COPTER MOTOR LAYOUT) + |4|-||-|2| + || + |3|-||-|1| + + IMPORTANT PROGRAMMING NOTE: + ARRAY_LAYOUT: Only when using the array is the first motor "0" and the last motor "3" + MOTOR_LAYOUT: When refering to motors anywhere else or not in the context of the arry, the first motor is "1" and the last motor is "4" + + */ + +#include "motors.h" +#include "dshot.h" +#include "log.h" +#include "misc.h" + + +static bool motors_are_off(motors_handle_t *motors_h); + +void motors_init(motors_handle_t *motors_h, dshot_handle_t *m1_h, dshot_handle_t *m2_h, dshot_handle_t *m3_h, dshot_handle_t *m4_h) { + motors_h->dshot_hs[0] = m1_h; + motors_h->dshot_hs[1] = m2_h; + motors_h->dshot_hs[2] = m3_h; + motors_h->dshot_hs[3] = m4_h; +} + +void motors_process(motors_handle_t *motors_h) { + for (int i = 0; i < 4; i++) { + dshot_process(motors_h->dshot_hs[i]); + } +} + +void motor_set_throttle(motors_handle_t *motors_h, uint8_t motor_num, uint16_t throttle) { + dshot_set_throttle(motors_h->dshot_hs[motor_num - 1], throttle); +} + +void motors_set_throttle_arr(motors_handle_t *motors_h, uint16_t throttle[4]) { + for (int i = 0; i < 4; i++) { + motor_set_throttle(motors_h, i + 1, throttle[i]); + } +} + +void motors_set_throttle(motors_handle_t *motors_h, uint16_t throttle_m1, uint16_t throttle_m2, uint16_t throttle_m3, uint16_t throttle_m4) { + motor_set_throttle(motors_h, 1, throttle_m1); + motor_set_throttle(motors_h, 2, throttle_m2); + motor_set_throttle(motors_h, 3, throttle_m3); + motor_set_throttle(motors_h, 4, throttle_m4); +} + +void motors_stop(motors_handle_t *motors_h) { + for (int i = 0; i < 4; i++) { + motor_stop(motors_h, i+1); + } +} + +void motor_stop(motors_handle_t *motors_h, uint8_t motor_num) { + dshot_set_throttle(motors_h->dshot_hs[motor_num - 1], 0); +} + +void motors_beep(motors_handle_t *motors_h) { + for (int i = 0; i < 4; i++) { + dshot_send_special_command(motors_h->dshot_hs[i], DSHOT_CMD_BEEP1); + } +} + +void motor_beep(motors_handle_t *motors_h, uint8_t motor_num) { + dshot_send_special_command(motors_h->dshot_hs[motor_num - 1], DSHOT_CMD_BEEP1); +} + +// takes a motor number and sets it to one of the outputs (MOTOR_LAYOUT) (swaps 2 motors from place...) +void motor_set_layout(motors_handle_t *motors_h, dshot_handle_t *m_h, uint8_t motor_num) { + //only allow this while motors are off + if (!motors_are_off(motors_h)) { + LOGE((uint8_t*)"Motors are not off, cannot change motor layout!"); + return; + } + + //save the motor that is being overwritten + dshot_handle_t *overwrite_backup = motors_h->dshot_hs[motor_num - 1]; + + motors_h->dshot_hs[motor_num - 1] = m_h; + + for (int i = 0; i < 4; i++) { + if (motors_h->dshot_hs[i]->htim->Instance == m_h->htim->Instance) { + motors_h->dshot_hs[i] = overwrite_backup; + break; + } + } +} + +void motor_set_direction(motors_handle_t *motors_h, uint8_t motor_num, bool reverse) { + if (reverse) { + dshot_send_special_command(motors_h->dshot_hs[motor_num - 1], DSHOT_CMD_SPIN_DIRECTION_REVERSED); + return; + } + dshot_send_special_command(motors_h->dshot_hs[motor_num - 1], DSHOT_CMD_SPIN_DIRECTION_NORMAL); +} + +void motors_set_direction(motors_handle_t *motors_h, bool reverse_m1, bool reverse_m2, bool reverse_m3, bool reverse_m4) { + motor_set_direction(motors_h, 1, reverse_m1); + motor_set_direction(motors_h, 2, reverse_m2); + motor_set_direction(motors_h, 3, reverse_m3); + motor_set_direction(motors_h, 4, reverse_m4); +} + +// ------- STATIC FUNCTIONS ------- // + +static bool motors_are_off(motors_handle_t *motors_h) { + for (int i = 0; i < 4; i++) { + if (motors_h->dshot_hs[i]->throttle_value != 0) { + return false; + } + } + return true; +} + + + + diff --git a/software/MySrc/Flight/motors.h b/software/MySrc/Flight/motors.h new file mode 100644 index 0000000..f3348b7 --- /dev/null +++ b/software/MySrc/Flight/motors.h @@ -0,0 +1,23 @@ + +#ifndef BETTERFLIGHT_MOTORS_H +#define BETTERFLIGHT_MOTORS_H + +#include "dshot.h" + +typedef struct { + dshot_handle_t *dshot_hs[4]; +} motors_handle_t; + +void motors_init(motors_handle_t *motors_h, dshot_handle_t *m1_h, dshot_handle_t *m2_h, dshot_handle_t *m3_h, dshot_handle_t *m4_h); +void motors_process(motors_handle_t *motors_h); +void motors_set_throttle(motors_handle_t *motors_h, uint16_t throttle_m1, uint16_t throttle_m2, uint16_t throttle_m3, uint16_t throttle_m4); +void motors_set_throttle_arr(motors_handle_t *motors_h, uint16_t throttle[4]); +void motor_set_throttle(motors_handle_t *motors_h, uint8_t motor_num, uint16_t throttle); +void motors_stop(motors_handle_t *motors_h); +void motor_stop(motors_handle_t *motors_h, uint8_t motor_num); +void motor_set_layout(motors_handle_t *motors_h, dshot_handle_t *m_h, uint8_t motor_num); +void motor_set_direction(motors_handle_t *motors_h, uint8_t motor_num, bool reverse); +void motors_set_direction(motors_handle_t *motors_h, bool reverse_m1, bool reverse_m2, bool reverse_m3, bool reverse_m4); +void motors_beep(motors_handle_t *motors_h); + +#endif //BETTERFLIGHT_MOTORS_H diff --git a/software/MySrc/Flight/pid.c b/software/MySrc/Flight/pid.c new file mode 100644 index 0000000..0fefbf6 --- /dev/null +++ b/software/MySrc/Flight/pid.c @@ -0,0 +1,73 @@ +// +// Created by Maarten on 29-2-2024. +// +#include "pid.h" + +#include + +#include "log.h" +#include "misc.h" +#include "stm32f7xx_hal.h" + + +void pid_controller_init(pid_handle_t *pid_h, float Kp, float Ki, float Kd, float T, int16_t min_output, int16_t max_output, int16_t min_input, int16_t max_input) +{ + pid_h->gains.Kp = Kp; + pid_h->gains.Ki = Ki; + pid_h->gains.Kd = Kd; + + pid_h->T = T; + + pid_h->limits.min_output = min_output; + pid_h->limits.max_output = max_output; + + pid_h->limits.min_input = min_input; + pid_h->limits.max_input = max_input; + + pid_h->integrator = 0; + pid_h->prev_error = 0; + pid_h->differentiator = 0; + pid_h->prev_measurement = 0; + pid_h->output = 0; +} + + + +void pid_controller_clear(pid_handle_t *pid_h) +{ + pid_h->integrator = 0; + pid_h->prev_error = 0; + pid_h->differentiator = 0; + pid_h->prev_measurement = 0; + pid_h->output = 0; +} + +int16_t pid_controller_update(pid_handle_t *pid_h, int16_t setp, int16_t measurement){ + + int16_t error = setp - measurement; + //LOGD("Error: %d, setp: %d, measurement %d", error, setp, measurement); + int16_t proportional = (int16_t )(pid_h->gains.Kp *(float) error); //p[n] + + pid_h->integrator = pid_h->integrator + 0.5f * pid_h->gains.Ki * pid_h->T * ((float)error + pid_h->prev_error); //i[n] + + pid_h->differentiator = -(2.0f * pid_h->gains.Kd * ( (float)measurement - pid_h->prev_measurement) + (2.0f * pid_h->tau - pid_h->T)*pid_h->differentiator); //d[n] + + pid_h->output = (int16_t) ((float)proportional + (float) pid_h->integrator + (float)pid_h->differentiator); //u[n] + + //LOGD("PID output(before limited): %d", pid_h->output); + //HAL_Delay(500); + if (pid_h->output > pid_h->limits.max_output){ + pid_h->output = pid_h->limits.max_output; + } + else if (pid_h->output < pid_h->limits.min_output){ + pid_h->output = pid_h->limits.min_output; + } + + + pid_h->prev_error = error; + pid_h->prev_measurement = measurement; + + return pid_h->output; +} + + diff --git a/software/MySrc/Flight/pid.h b/software/MySrc/Flight/pid.h new file mode 100644 index 0000000..90b278f --- /dev/null +++ b/software/MySrc/Flight/pid.h @@ -0,0 +1,53 @@ +// +// Created by Maarten on 29-2-2024. +// + +#ifndef BETTERFLIGHT_PID_H +#define BETTERFLIGHT_PID_H + +#include "stdint.h" +typedef struct{ + //gains pid + float Kp; + float Ki; + float Kd; +}pid_gain_t; + +typedef struct { + // PID limits + int16_t min_output; + int16_t max_output; //limits of the motor output + int16_t min_input; + int16_t max_input; //limits of the input sticks +}pid_limits_t; + +typedef struct{ + + pid_gain_t gains; //pid gains + + float tau; //low pass filter time constant + + pid_limits_t limits; //pid limits + + float T; // sample time (s) + + float integrator; + float prev_error; // needed for integrator + float differentiator; + float prev_measurement; // needed for differentiator + + + + int16_t output; + + + +}pid_handle_t; + +void pid_controller_init(pid_handle_t *pid_h, float Kp, float Ki, float Kd, float T, int16_t min_output, int16_t max_output, int16_t min_input, int16_t max_input); //initializes the pid controller +void pid_controller_clear(pid_handle_t *pid_h); //clears the calculations +int16_t pid_controller_update(pid_handle_t *pid_h, int16_t setp, int16_t measurement); //updates the pid controller with user input + + + +#endif //BETTERFLIGHT_PID_H diff --git a/software/MySrc/Flight/pid_controller.c b/software/MySrc/Flight/pid_controller.c new file mode 100644 index 0000000..d58826f --- /dev/null +++ b/software/MySrc/Flight/pid_controller.c @@ -0,0 +1,138 @@ +#include "pid_controller.h" +#include "pid.h" +#include "set_point.h" + +#include + +#include "log.h" +#include "misc.h" +#include "stm32f7xx_hal.h" + + +void get_set_points( drone_pids_t *drone_pids){ + + set_point_calculation(&drone_pids->set_p.yaw_set_point, drone_pids->stick_outputs.yaw_stick_output, (float) 0.2 , (float) 0.2); + set_point_calculation(&drone_pids->set_p.roll_set_point, drone_pids->stick_outputs.roll_stick_output, (float) 0.2, (float) 0.2); + set_point_calculation(&drone_pids->set_p.pitch_set_point, drone_pids->stick_outputs.pitch_stick_output, (float) 0.2, (float) 0.2); + + +} + +void set_pids(drone_pids_t *drone_pids, float x_roll_imu_output, float y_pitch_imu_output, float z_yaw_imu_output) { + + + drone_pids->set_p.yaw_set_point.imu_val = (int16_t)z_yaw_imu_output; + drone_pids->set_p.roll_set_point.imu_val = (int16_t)x_roll_imu_output; + drone_pids->set_p.pitch_set_point.imu_val = (int16_t)y_pitch_imu_output; + //LOGD("Set points (in set_pids before): %d jaw, %d roll, %d pitch\r\n", drone_pids->set_p.yaw_set_point.set_point, drone_pids->set_p.roll_set_point.set_point, drone_pids->set_p.pitch_set_point.set_point); + // LOGD("Measurement (in set_pids before): %d jaw, %d roll, %d pitch", drone_pids->set_p.yaw_set_point.imu_val, drone_pids->set_p.roll_set_point.imu_val, drone_pids->set_p.pitch_set_point.imu_val); + //HAL_Delay(100); + pid_controller_update(&drone_pids->pids.yaw_pid, drone_pids->set_p.yaw_set_point.set_point, drone_pids->set_p.yaw_set_point.imu_val); + + pid_controller_update(&drone_pids->pids.roll_pid, drone_pids->set_p.roll_set_point.set_point, drone_pids->set_p.roll_set_point.imu_val); + + pid_controller_update(&drone_pids->pids.pitch_pid, drone_pids->set_p.pitch_set_point.set_point, drone_pids->set_p.pitch_set_point.imu_val); + //LOGD("Set points (in set_pids after): %d jaw, %d roll, %d pitch\r\n", drone_pids->set_p.yaw_set_point.set_point, drone_pids->set_p.roll_set_point.set_point, drone_pids->set_p.pitch_set_point.set_point); + //LOGD("Measurement (in set_pids after): %d jaw, %d roll, %d pitch", drone_pids->set_p.yaw_set_point.imu_val, drone_pids->set_p.roll_set_point.imu_val, drone_pids->set_p.pitch_set_point.imu_val); + //HAL_Delay(10); +} + + + +void test_pid_controller(void){ + mixer_handle_t mixer_h; + drone_pids_t drone_pids; + + mixer_h.percentages.roll = 0.7; + mixer_h.percentages.pitch = 0.7; + mixer_h.percentages.yaw = 0.4; + mixer_h.percentages.throttle = 1.0; + + + + pid_controller_init(&drone_pids.pids.yaw_pid, (float).4, (float)0.3, (float)0.3, (float)0.05, -750, 750, -750, 750); + pid_controller_init(&drone_pids.pids.roll_pid, (float).4, (float)0.3, (float)0.3, (float)0.05, -750, 750, -750, 750); + pid_controller_init(&drone_pids.pids.pitch_pid, (float).4, (float)0.3, (float)0.3, (float)0.05, -750, 750, -750, 750); + + limits_init(&drone_pids.set_p.yaw_set_point, drone_pids.pids.yaw_pid.limits.min_output, drone_pids.pids.yaw_pid.limits.max_output); + limits_init(&drone_pids.set_p.roll_set_point, drone_pids.pids.roll_pid.limits.min_output, drone_pids.pids.roll_pid.limits.max_output); + limits_init(&drone_pids.set_p.pitch_set_point, drone_pids.pids.pitch_pid.limits.min_output, drone_pids.pids.pitch_pid.limits.max_output); + + + drone_pids.stick_outputs.yaw_stick_output = 435; + drone_pids.stick_outputs.roll_stick_output = 192; + drone_pids.stick_outputs.pitch_stick_output = 95; + //HAL_Delay(100); + get_set_points(&drone_pids); + + set_pids(&drone_pids,180,82,375); + + LOGD("PID output: %d roll, %d pitch, %d yaw\r\n", drone_pids.pids.roll_pid.output, drone_pids.pids.pitch_pid.output, drone_pids.pids.yaw_pid.output); + + mixer_h.pid.roll_pid = drone_pids.pids.roll_pid; + mixer_h.pid.pitch_pid = drone_pids.pids.pitch_pid; + mixer_h.pid.yaw_pid = drone_pids.pids.yaw_pid; + + + mixer_h.input.roll = drone_pids.pids.roll_pid.output; + mixer_h.input.pitch = drone_pids.pids.pitch_pid.output; + mixer_h.input.yaw = drone_pids.pids.yaw_pid.output; + mixer_h.input.throttle = 1800; + mixing(&mixer_h); + + + drone_pids.stick_outputs.yaw_stick_output = 430; + drone_pids.stick_outputs.roll_stick_output = 185; + drone_pids.stick_outputs.pitch_stick_output = 90; + //HAL_Delay(100); + get_set_points(&drone_pids); + + + set_pids(&drone_pids,185,85,380); + LOGD("PID output: %d jaw, %d roll, %d pitch\r\n", drone_pids.pids.yaw_pid.output, drone_pids.pids.roll_pid.output, drone_pids.pids.pitch_pid.output); + HAL_Delay(25); + + mixer_h.pid.roll_pid = drone_pids.pids.roll_pid; + mixer_h.pid.pitch_pid = drone_pids.pids.pitch_pid; + mixer_h.pid.yaw_pid = drone_pids.pids.yaw_pid; + + + mixer_h.input.roll = drone_pids.pids.roll_pid.output; + mixer_h.input.pitch = drone_pids.pids.pitch_pid.output; + mixer_h.input.yaw = drone_pids.pids.yaw_pid.output; + mixer_h.input.throttle = 1800; + mixing(&mixer_h); + + HAL_Delay(25); + + set_pids(&drone_pids,190,90,400); + LOGD("PID output: %d jaw, %d roll, %d pitch\r\n", drone_pids.pids.yaw_pid.output, drone_pids.pids.roll_pid.output, drone_pids.pids.pitch_pid.output); + + mixer_h.pid.roll_pid = drone_pids.pids.roll_pid; + mixer_h.pid.pitch_pid = drone_pids.pids.pitch_pid; + mixer_h.pid.yaw_pid = drone_pids.pids.yaw_pid; + + + mixer_h.input.roll = drone_pids.pids.roll_pid.output; + mixer_h.input.pitch = drone_pids.pids.pitch_pid.output; + mixer_h.input.yaw = drone_pids.pids.yaw_pid.output; + mixer_h.input.throttle = 1800; + mixing(&mixer_h); +} +void pid_process(mixer_handle_t *mixer, drone_pids_t *drone_pids){ + + + mixing(mixer); +} + +void pid_init(drone_pids_t *drone_pids){ + + pid_controller_init(&drone_pids->pids.yaw_pid, (float).4, (float)0.3, (float)0.3, (float)0.05, -750, 750, -750, 750); + pid_controller_init(&drone_pids->pids.roll_pid, (float).4, (float)0.3, (float)0.3, (float)0.05, -750, 750, -750, 750); + pid_controller_init(&drone_pids->pids.pitch_pid, (float).4, (float)0.3, (float)0.3, (float)0.05, -750, 750, -750, 750); + + limits_init(&drone_pids->set_p.yaw_set_point, drone_pids->pids.yaw_pid.limits.min_output, drone_pids->pids.yaw_pid.limits.max_output); + limits_init(&drone_pids->set_p.roll_set_point, drone_pids->pids.roll_pid.limits.min_output, drone_pids->pids.roll_pid.limits.max_output); + limits_init(&drone_pids->set_p.pitch_set_point, drone_pids->pids.pitch_pid.limits.min_output, drone_pids->pids.pitch_pid.limits.max_output); + +} diff --git a/software/MySrc/Flight/pid_controller.h b/software/MySrc/Flight/pid_controller.h new file mode 100644 index 0000000..a20e6a8 --- /dev/null +++ b/software/MySrc/Flight/pid_controller.h @@ -0,0 +1,36 @@ +#include "set_point.h" +#include "pid.h" +#include "mixer.h" +#ifndef BETTERFLIGHT_PID_CONTROLLER_H +#define BETTERFLIGHT_PID_CONTROLLER_H +typedef struct { +set_point_t yaw_set_point; +set_point_t roll_set_point; +set_point_t pitch_set_point; +}set_points_t; + +typedef struct{ + int16_t yaw_stick_output; + int16_t roll_stick_output; + int16_t pitch_stick_output; +}stick_output_t; + +typedef struct{ + pid_handle_t yaw_pid; + pid_handle_t roll_pid; + pid_handle_t pitch_pid; +}pids_t; + +typedef struct{ + set_points_t set_p; + stick_output_t stick_outputs; + pids_t pids; +}drone_pids_t; + +void get_set_points(drone_pids_t *drone_pids); +void set_pids(drone_pids_t *drone_pids, float x_imu_output, float y_imu_output, float z_imu_output); +void test_pid_controller(void); +void pid_init(drone_pids_t *drone_pids); +void pid_process(mixer_handle_t *mixer, drone_pids_t *drone_pids); + +#endif //BETTERFLIGHT_PID_CONTROLLER_H diff --git a/software/MySrc/Flight/set_point.c b/software/MySrc/Flight/set_point.c new file mode 100644 index 0000000..ed77374 --- /dev/null +++ b/software/MySrc/Flight/set_point.c @@ -0,0 +1,37 @@ +// +// Created by Maarten on 5-3-2024. +// +#include "set_point.h" + +#include + +#include "log.h" +#include "misc.h" +#define MAX(a, b) (((a) > (b)) ? (a) : (b)); + +float power5(float x) { + return x * x * x * x * x; +} + +void limits_init(set_point_t *set_point, int16_t min_imu_output, int16_t max_imu_output){ + set_point->stick_limits.min_stick_output = 1000; + set_point->stick_limits.max_stick_output = 2000; + set_point->actual_rates.rc_rates = (float) 0.5; + set_point->actual_rates.rc_expo = (float) 0.1; + set_point->actual_rates.rates = (float) 0.7; +} + +float set_point_calculation(set_point_t *set_point, int16_t stick_output,float rc_commandf, const float rc_commandfabs){ + + + + float expof = set_point->actual_rates.rc_expo / 100.0f; + expof = rc_commandfabs * (power5(rc_commandf) * expof + rc_commandf * (1 - expof)); + + const float centerSensitivity = set_point->actual_rates.rc_rates * 10.0f; + const float stickMovement = MAX(0, set_point->actual_rates.rates * 10.0f - centerSensitivity); + const float angleRate = rc_commandf * centerSensitivity + stickMovement * expof; + + set_point->set_point = (int16_t)(angleRate * (float)stick_output); + +} \ No newline at end of file diff --git a/software/MySrc/Flight/set_point.h b/software/MySrc/Flight/set_point.h new file mode 100644 index 0000000..b977ad8 --- /dev/null +++ b/software/MySrc/Flight/set_point.h @@ -0,0 +1,29 @@ +#ifndef BETTERFLIGHT_SET_POINT_H +#define BETTERFLIGHT_SET_POINT_H + +#include "stdint.h" + +typedef struct{ + int16_t min_stick_output; + int16_t max_stick_output; +}stick_limits_t; + +typedef struct{ + float rc_rates; + float rc_expo; + float rates; +}actual_rates_t; + + +typedef struct { + int16_t stick_output; + stick_limits_t stick_limits; + actual_rates_t actual_rates; + int16_t set_point; + int16_t imu_val; +} set_point_t; + +void limits_init(set_point_t *set_point, int16_t min_imu_output, int16_t max_imu_output); +float set_point_calculation(set_point_t *set_point, int16_t stick_output,float rc_commandf, const float rc_commandfabs); +float power5(float x); +#endif //BETTERFLIGHT_SET_POINT_H diff --git a/software/MySrc/mymain.c b/software/MySrc/mymain.c new file mode 100644 index 0000000..e1ec747 --- /dev/null +++ b/software/MySrc/mymain.c @@ -0,0 +1,156 @@ +#include "mymain.h" + +#include + +#include "main.h" +#include "misc.h" +#include "log.h" +#include "cli.h" +#include "dshot.h" +#include "motors.h" +#include "imu.h" +#include "crsf.h" +#include "pid_controller.h" +/* SETTINGS */ +#define LOG_LEVEL LOG_LEVEL_DEBUG +// check version.h for version settings + +/* EOF SETTINGS */ + +IMU_handle_t imu_h; +cli_handle_t cli_h; + +//motors +dshot_handle_t m1_h; // TIM1 CH2 +dshot_handle_t m2_h; // TIM1 CH1 +dshot_handle_t m3_h; // TIM8 CH4 +dshot_handle_t m4_h; // TIM8 CH3 +motors_handle_t motors_h; + + +//pid +drone_pids_t pids_h; + +//mixer +mixer_handle_t motor_mixer_h; + + +uint64_t led_toggle_last_ms = 0; +uint64_t cli_process_last_ms = 0; +uint64_t pid_controller_test_ms = 0; +uint64_t crsf_last_ms = 0; +uint64_t motors_process_last_ms = 0; +uint64_t imu_process_last_ms = 0; +uint64_t flight_ctrl_cycle_last_ms = 0; + + +cli_handle_t cli_h; +crsf_handle_t crsf_h; + +static void flight_ctrl_cycle(void); + +void myinit(void) { + cli_h.halt_until_connected_flag = true; //set to false if you don't want to wait for a connection + + log_init(LOG_LEVEL, true); + LED_on(); + + cli_init(&cli_h); //will block if halt_until_connected_flag is true + + delay(1); + LOGI("Starting Initialization..."); + delay(1); + + // ----- all initialization code goes here ----- // + + + //init imu + log_imu_err(imu_init(&imu_h)); + + //elrs init + crsf_init(&crsf_h, &huart2); + pid_init(&pids_h); + + // motors init +// motors_init(&motors_h, &m1_h, &m2_h, &m3_h, &m4_h); +// dshot_init(&m1_h, &htim1, &hdma_tim1_ch2, TIM_CHANNEL_2); +// dshot_init(&m2_h, &htim1, &hdma_tim1_ch1, TIM_CHANNEL_1); +// dshot_init(&m3_h, &htim8, &hdma_tim8_ch4_trig_com, TIM_CHANNEL_4); +// dshot_init(&m4_h, &htim8, &hdma_tim8_ch3, TIM_CHANNEL_3); +// ----- end initialization code ----- // + + delay(1); + LOGI("Finished Initialization"); + delay(1); + + + LED_blink_pattern(20, 2, 50, 50); + LED_off(); + +} + + + +/* + + folder layout core: + [flight] + - pid + - pid_controller + - motor + - mixer + - setpoint + - common_data_structs.h -> structs that are used in multiple flight files + [driver/] + spi_soft + spsi + imu + gyro + dshot + crsf + BMI270_CONFIG + [common] + - misc + - log + - cli + - cli_cmd_callbacks + - version + [root] + main + mymain + stm32f7xx_hal_conf + stm32f7xx_it.h + ioc + + + + + + + */ + + + +void mymain(void) { + while (1) { //todo has to be replaced by a scheduler + none_blocking_delay(1000, &led_toggle_last_ms, (callback_t) LED_toggle, NULL); + none_blocking_delay(25, &cli_process_last_ms, (callback_t) cli_process, &cli_h); + //none_blocking_delay(1, &motors_process_last_ms, (callback_t) motors_process, &motors_h); + flight_ctrl_cycle(); + } +} + +static void flight_ctrl_cycle(void) { + //crsf + imu =>pid's=>motor mixer + + // update imu data + if (imu_h.last_err == IMU_OK) { + imu_process(&imu_h); + } + + // update elrs channels + crsf_process(&crsf_h); + + // update pid controllers + +} \ No newline at end of file diff --git a/software/MySrc/mymain.h b/software/MySrc/mymain.h new file mode 100644 index 0000000..0e1e17f --- /dev/null +++ b/software/MySrc/mymain.h @@ -0,0 +1,22 @@ +#ifndef BETTERFLIGHT_MYMAIN_H +#define BETTERFLIGHT_MYMAIN_H + +#include "misc.h" +#include "dshot.h" +#include "cli.h" +#include "motors.h" + +extern cli_handle_t cli_h; + +//motors +//extern dshot_handle_t *m1_h; // TIM1 CH2 +//extern dshot_handle_t *m2_h; // TIM1 CH1 +//extern dshot_handle_t *m3_h; // TIM8 CH4 +//extern dshot_handle_t *m4_h; // TIM8 CH3 +//extern dshot_handle_t *m_hs[4]; +extern motors_handle_t motors_h; + +void myinit(void); +void mymain(void); + +#endif //BETTERFLIGHT_MYMAIN_H