From e24382afce4bde4322fe4b241d52709a7d986924 Mon Sep 17 00:00:00 2001 From: Daniel Burke Date: Thu, 26 May 2022 20:45:57 -0230 Subject: [PATCH 1/4] Patch VESC interface to work for multiple motors at once Add these files to an 'External' folder, and add some code to main to test that the interface can be set up. --- Node/.gitignore | 1 + Node/CMakeLists.txt | 15 +- Node/Core/Src/main.c | 36 +- Node/External/Inc/bldc_interface.h | 121 ++++ Node/External/Inc/bldc_interface_uart.h | 37 + Node/External/Inc/buffer.h | 44 ++ Node/External/Inc/crc.h | 33 + Node/External/Inc/datatypes.h | 485 +++++++++++++ Node/External/Inc/packet.h | 42 ++ Node/External/Src/bldc_interface.c | 913 ++++++++++++++++++++++++ Node/External/Src/bldc_interface_uart.c | 96 +++ Node/External/Src/buffer.c | 187 +++++ Node/External/Src/crc.c | 64 ++ Node/External/Src/packet.c | 169 +++++ Node/build.sh | 6 +- 15 files changed, 2244 insertions(+), 5 deletions(-) create mode 100644 Node/External/Inc/bldc_interface.h create mode 100644 Node/External/Inc/bldc_interface_uart.h create mode 100644 Node/External/Inc/buffer.h create mode 100644 Node/External/Inc/crc.h create mode 100644 Node/External/Inc/datatypes.h create mode 100644 Node/External/Inc/packet.h create mode 100644 Node/External/Src/bldc_interface.c create mode 100644 Node/External/Src/bldc_interface_uart.c create mode 100644 Node/External/Src/buffer.c create mode 100644 Node/External/Src/crc.c create mode 100644 Node/External/Src/packet.c diff --git a/Node/.gitignore b/Node/.gitignore index 608f518..0df2384 100644 --- a/Node/.gitignore +++ b/Node/.gitignore @@ -1,4 +1,5 @@ build/ +Build/ Drivers Makefile .mxproject diff --git a/Node/CMakeLists.txt b/Node/CMakeLists.txt index 3c3f62c..da9542c 100644 --- a/Node/CMakeLists.txt +++ b/Node/CMakeLists.txt @@ -43,8 +43,20 @@ set(STM32CUBEMX_GENERATED_FILES startup_stm32f334x8.s ) +set(STM32_USER_CREATED_FILES + Core/Src/vesc_packet.c +) + +set(EXTERNAL_FILES + External/Src/bldc_interface_uart.c + External/Src/bldc_interface.c + External/Src/buffer.c + External/Src/crc.c + External/Src/packet.c +) + set(EXECUTABLE ${PROJECT_NAME}.out) -add_executable(${EXECUTABLE} ${STM32CUBEMX_GENERATED_FILES}) +add_executable(${EXECUTABLE} ${STM32CUBEMX_GENERATED_FILES} ${EXTERNAL_FILES}) target_compile_definitions(${EXECUTABLE} PRIVATE -DUSE_HAL_DRIVER @@ -53,6 +65,7 @@ target_compile_definitions(${EXECUTABLE} PRIVATE target_include_directories(${EXECUTABLE} PRIVATE Core/Inc + External/Inc Drivers/STM32F3xx_HAL_Driver/Inc Drivers/CMSIS/Device/ST/STM32F3xx/Include Drivers/CMSIS/Include diff --git a/Node/Core/Src/main.c b/Node/Core/Src/main.c index 95a595c..971c1fe 100644 --- a/Node/Core/Src/main.c +++ b/Node/Core/Src/main.c @@ -18,9 +18,12 @@ /* USER CODE END Header */ /* Includes ------------------------------------------------------------------*/ #include "main.h" +#include "bldc_interface.h" +#include "bldc_interface_uart.h" #include "can.h" #include "i2c.h" #include "spi.h" +#include "stm32f3xx_hal_usart.h" #include "tim.h" #include "usart.h" #include "gpio.h" @@ -58,7 +61,20 @@ void SystemClock_Config(void); /* Private user code ---------------------------------------------------------*/ /* USER CODE BEGIN 0 */ +static void send_packet_motor_1(unsigned char* data, unsigned int len) +{ + HAL_UART_Transmit(&huart1, data, len, 0); +} + +static void send_packet_motor_2(unsigned char* data, unsigned int len) +{ + HAL_UART_Transmit(&huart2, data, len, 0); +} +static void send_packet_motor_3(unsigned char* data, unsigned int len) +{ + HAL_UART_Transmit(&huart3, data, len, 0); +} /* USER CODE END 0 */ /** @@ -68,7 +84,9 @@ void SystemClock_Config(void); int main(void) { /* USER CODE BEGIN 1 */ - + BldcInterface motor1; + BldcInterface motor2; + BldcInterface motor3; /* USER CODE END 1 */ /* MCU Configuration--------------------------------------------------------*/ @@ -98,8 +116,15 @@ int main(void) MX_TIM17_Init(); MX_I2C1_Init(); MX_SPI1_Init(); + /* USER CODE BEGIN 2 */ + bldc_interface_init(&motor1, send_packet_motor_1); + bldc_interface_init(&motor2, send_packet_motor_2); + bldc_interface_init(&motor3, send_packet_motor_3); + uint8_t motor1_data; + uint8_t motor2_data; + uint8_t motor3_data; /* USER CODE END 2 */ /* Infinite loop */ @@ -107,7 +132,16 @@ int main(void) while (1) { /* USER CODE END WHILE */ + // TODO receive more than one byte at a time + // TODO Error handling before processing bytes + HAL_UART_Receive(&huart1, &motor1_data, 1, 0); + HAL_UART_Receive(&huart2, &motor2_data, 1, 0); + HAL_UART_Receive(&huart3, &motor3_data, 1, 0); + + bldc_interface_uart_process_byte(&motor1, motor1_data); + bldc_interface_uart_process_byte(&motor2, motor2_data); + bldc_interface_uart_process_byte(&motor3, motor3_data); /* USER CODE BEGIN 3 */ } /* USER CODE END 3 */ diff --git a/Node/External/Inc/bldc_interface.h b/Node/External/Inc/bldc_interface.h new file mode 100644 index 0000000..326209b --- /dev/null +++ b/Node/External/Inc/bldc_interface.h @@ -0,0 +1,121 @@ +/* + Copyright 2016-2017 Benjamin Vedder benjamin@vedder.se + + This program is free software: you can redistribute it and/or modify + it under the terms of the GNU General Public License as published by + the Free Software Foundation, either version 3 of the License, or + (at your option) any later version. + + This program is distributed in the hope that it will be useful, + but WITHOUT ANY WARRANTY; without even the implied warranty of + MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + GNU General Public License for more details. + + You should have received a copy of the GNU General Public License + along with this program. If not, see . + */ + +#ifndef BLDC_INTERFACE_H_ +#define BLDC_INTERFACE_H_ + +#include "datatypes.h" + +typedef struct BldcInterface +{ + // Private variables + unsigned char send_buffer[1024]; + + // Private variables for received data + mc_values values; + int fw_major; + int fw_minor; + float rotor_pos; + mc_configuration mcconf; + app_configuration appconf; + float detect_cycle_int_limit; + float detect_coupling_k; + signed char detect_hall_table[8]; + signed char detect_hall_res; + float dec_ppm; + float dec_ppm_len; + float dec_adc; + float dec_adc_voltage; + float dec_chuk; + + // Function pointers + void(*send_func)(unsigned char *data, unsigned int len); + void(*forward_func)(unsigned char *data, unsigned int len); + + // Function pointers for received data + void(*rx_value_func)(mc_values *values); + void(*rx_printf_func)(char *str); + void(*rx_fw_func)(int major, int minor); + void(*rx_rotor_pos_func)(float pos); + void(*rx_mcconf_func)(mc_configuration *conf); + void(*rx_appconf_func)(app_configuration *conf); + void(*rx_detect_func)(float cycle_int_limit, float coupling_k, + const signed char *hall_table, signed char hall_res); + void(*rx_dec_ppm_func)(float val, float ms); + void(*rx_dec_adc_func)(float val, float voltage); + void(*rx_dec_chuk_func)(float val); + void(*rx_mcconf_received_func)(void); + void(*rx_appconf_received_func)(void); + void(*motor_control_set_func)(motor_control_mode mode, float value); + void(*values_requested_func)(void); +} BldcInterface; + +// interface functions +void bldc_interface_init(BldcInterface* interface, void(*func)(unsigned char *data, unsigned int len)); +void bldc_interface_set_forward_func(BldcInterface* interface, void(*func)(unsigned char *data, unsigned int len)); +void bldc_interface_send_packet(BldcInterface* interface, unsigned char *data, unsigned int len); +void bldc_interface_process_packet(BldcInterface* interface, unsigned char *data, unsigned int len); + +// Function pointer setters +void bldc_interface_set_rx_value_func(BldcInterface* interface, void(*func)(mc_values *values)); +void bldc_interface_set_rx_printf_func(BldcInterface* interface, void(*func)(char *str)); +void bldc_interface_set_rx_fw_func(BldcInterface* interface, void(*func)(int major, int minor)); +void bldc_interface_set_rx_rotor_pos_func(BldcInterface* interface, void(*func)(float pos)); +void bldc_interface_set_rx_mcconf_func(BldcInterface* interface, void(*func)(mc_configuration *conf)); +void bldc_interface_set_rx_appconf_func(BldcInterface* interface, void(*func)(app_configuration *conf)); +void bldc_interface_set_rx_detect_func(BldcInterface* interface, void(*func)(float cycle_int_limit, float coupling_k, + const signed char *hall_table, signed char hall_res)); +void bldc_interface_set_rx_dec_ppm_func(BldcInterface* interface, void(*func)(float val, float ms)); +void bldc_interface_set_rx_dec_adc_func(BldcInterface* interface, void(*func)(float val, float voltage)); +void bldc_interface_set_rx_dec_chuk_func(BldcInterface* interface, void(*func)(float val)); +void bldc_interface_set_rx_mcconf_received_func(BldcInterface* interface, void(*func)(void)); +void bldc_interface_set_rx_appconf_received_func(BldcInterface* interface, void(*func)(void)); + +void bldc_interface_set_sim_control_function(BldcInterface* interface, void(*func)(motor_control_mode mode, float value)); +void bldc_interface_set_sim_values_func(BldcInterface* interface, void(*func)(void)); + +// Setters +void bldc_interface_terminal_cmd(BldcInterface* interface, char* cmd); +void bldc_interface_set_duty_cycle(BldcInterface* interface, float dutyCycle); +void bldc_interface_set_current(BldcInterface* interface, float current); +void bldc_interface_set_current_brake(BldcInterface* interface, float current); +void bldc_interface_set_rpm(BldcInterface* interface, int rpm); +void bldc_interface_set_pos(BldcInterface* interface, float pos); +void bldc_interface_set_handbrake(BldcInterface* interface, float current); +void bldc_interface_set_servo_pos(BldcInterface* interface, float pos); +void bldc_interface_set_mcconf(BldcInterface* interface, const mc_configuration *mcconf); +void bldc_interface_set_appconf(BldcInterface* interface, const app_configuration *appconf); + +// Getters +void bldc_interface_get_fw_version(BldcInterface* interface); +void bldc_interface_get_values(BldcInterface* interface); +void bldc_interface_get_mcconf(BldcInterface* interface); +void bldc_interface_get_appconf(BldcInterface* interface); +void bldc_interface_get_decoded_ppm(BldcInterface* interface); +void bldc_interface_get_decoded_adc(BldcInterface* interface); +void bldc_interface_get_decoded_chuk(BldcInterface* interface); + +// Other functions +void bldc_interface_detect_motor_param(BldcInterface*, float current, float min_rpm, float low_duty); +void bldc_interface_reboot(BldcInterface*); +void bldc_interface_send_alive(BldcInterface*); +void send_values_to_receiver(BldcInterface*, mc_values *values); + +// Helpers +const char* bldc_interface_fault_to_string(mc_fault_code fault); + +#endif /* BLDC_INTERFACE_H_ */ diff --git a/Node/External/Inc/bldc_interface_uart.h b/Node/External/Inc/bldc_interface_uart.h new file mode 100644 index 0000000..acfa058 --- /dev/null +++ b/Node/External/Inc/bldc_interface_uart.h @@ -0,0 +1,37 @@ +/* + Copyright 2015 Benjamin Vedder benjamin@vedder.se + + This program is free software: you can redistribute it and/or modify + it under the terms of the GNU General Public License as published by + the Free Software Foundation, either version 3 of the License, or + (at your option) any later version. + + This program is distributed in the hope that it will be useful, + but WITHOUT ANY WARRANTY; without even the implied warranty of + MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + GNU General Public License for more details. + + You should have received a copy of the GNU General Public License + along with this program. If not, see . + */ + +/* + * bldc_interface_uart.h + * + * Created on: 9 okt 2015 + * Author: benjamin + */ + +#ifndef BLDC_INTERFACE_UART_H_ +#define BLDC_INTERFACE_UART_H_ + +// Includes +#include "packet.h" // For the MAX_PACKET_LEN define +#include "bldc_interface.h" + +// Functions +void bldc_interface_uart_init(BldcInterface* interface, void(*func)(unsigned char *data, unsigned int len)); +void bldc_interface_uart_process_byte(BldcInterface* interface, unsigned char b); +void bldc_interface_uart_run_timer(BldcInterface* interface); + +#endif /* BLDC_INTERFACE_UART_H_ */ diff --git a/Node/External/Inc/buffer.h b/Node/External/Inc/buffer.h new file mode 100644 index 0000000..75315ff --- /dev/null +++ b/Node/External/Inc/buffer.h @@ -0,0 +1,44 @@ +/* + Copyright 2012-2018 Benjamin Vedder benjamin@vedder.se + + This program is free software: you can redistribute it and/or modify + it under the terms of the GNU General Public License as published by + the Free Software Foundation, either version 3 of the License, or + (at your option) any later version. + + This program is distributed in the hope that it will be useful, + but WITHOUT ANY WARRANTY; without even the implied warranty of + MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + GNU General Public License for more details. + + You should have received a copy of the GNU General Public License + along with this program. If not, see . + */ + +#ifndef BUFFER_H_ +#define BUFFER_H_ + +#include + +void buffer_append_int16(uint8_t* buffer, int16_t number, int32_t *index); +void buffer_append_uint16(uint8_t* buffer, uint16_t number, int32_t *index); +void buffer_append_int32(uint8_t* buffer, int32_t number, int32_t *index); +void buffer_append_uint32(uint8_t* buffer, uint32_t number, int32_t *index); +void buffer_append_int64(uint8_t* buffer, int64_t number, int32_t *index); +void buffer_append_uint64(uint8_t* buffer, uint64_t number, int32_t *index); +void buffer_append_float16(uint8_t* buffer, float number, float scale, int32_t *index); +void buffer_append_float32(uint8_t* buffer, float number, float scale, int32_t *index); +void buffer_append_double64(uint8_t* buffer, double number, double scale, int32_t *index); +void buffer_append_float32_auto(uint8_t* buffer, float number, int32_t *index); +int16_t buffer_get_int16(const uint8_t *buffer, int32_t *index); +uint16_t buffer_get_uint16(const uint8_t *buffer, int32_t *index); +int32_t buffer_get_int32(const uint8_t *buffer, int32_t *index); +uint32_t buffer_get_uint32(const uint8_t *buffer, int32_t *index); +int64_t buffer_get_int64(const uint8_t *buffer, int32_t *index); +uint64_t buffer_get_uint64(const uint8_t *buffer, int32_t *index); +float buffer_get_float16(const uint8_t *buffer, float scale, int32_t *index); +float buffer_get_float32(const uint8_t *buffer, float scale, int32_t *index); +double buffer_get_double64(const uint8_t *buffer, double scale, int32_t *index); +float buffer_get_float32_auto(const uint8_t *buffer, int32_t *index); + +#endif /* BUFFER_H_ */ diff --git a/Node/External/Inc/crc.h b/Node/External/Inc/crc.h new file mode 100644 index 0000000..7d533fc --- /dev/null +++ b/Node/External/Inc/crc.h @@ -0,0 +1,33 @@ +/* + Copyright 2012-2014 Benjamin Vedder benjamin@vedder.se + + This program is free software: you can redistribute it and/or modify + it under the terms of the GNU General Public License as published by + the Free Software Foundation, either version 3 of the License, or + (at your option) any later version. + + This program is distributed in the hope that it will be useful, + but WITHOUT ANY WARRANTY; without even the implied warranty of + MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + GNU General Public License for more details. + + You should have received a copy of the GNU General Public License + along with this program. If not, see . + */ + +/* + * crc.h + * + * Created on: 26 feb 2012 + * Author: benjamin + */ + +#ifndef CRC_H_ +#define CRC_H_ + +/* + * Functions + */ +unsigned short crc16(unsigned char *buf, unsigned int len); + +#endif /* CRC_H_ */ diff --git a/Node/External/Inc/datatypes.h b/Node/External/Inc/datatypes.h new file mode 100644 index 0000000..c3046b6 --- /dev/null +++ b/Node/External/Inc/datatypes.h @@ -0,0 +1,485 @@ +/* + Copyright 2012-2018 Benjamin Vedder benjamin@vedder.se + + This program is free software: you can redistribute it and/or modify + it under the terms of the GNU General Public License as published by + the Free Software Foundation, either version 3 of the License, or + (at your option) any later version. + + This program is distributed in the hope that it will be useful, + but WITHOUT ANY WARRANTY; without even the implied warranty of + MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + GNU General Public License for more details. + + You should have received a copy of the GNU General Public License + along with this program. If not, see . + */ + +#ifndef DATATYPES_H_ +#define DATATYPES_H_ + +#include +#include + +// CAN status sent by VESC +typedef struct { + int id; + uint32_t rx_time; + float rpm; + float current; + float duty; +} can_status_msg; + +typedef enum { + PWM_MODE_NONSYNCHRONOUS_HISW = 0, // This mode is not recommended + PWM_MODE_SYNCHRONOUS, // The recommended and most tested mode + PWM_MODE_BIPOLAR // Some glitches occasionally, can kill MOSFETs +} mc_pwm_mode; + +typedef enum { + COMM_MODE_INTEGRATE = 0, + COMM_MODE_DELAY +} mc_comm_mode; + +typedef enum { + SENSOR_MODE_SENSORLESS = 0, + SENSOR_MODE_SENSORED, + SENSOR_MODE_HYBRID +} mc_sensor_mode; + +// Auxiliary output mode +typedef enum { + OUT_AUX_MODE_OFF = 0, + OUT_AUX_MODE_ON_AFTER_2S, + OUT_AUX_MODE_ON_AFTER_5S, + OUT_AUX_MODE_ON_AFTER_10S +} out_aux_mode; + +typedef enum { + FOC_SENSOR_MODE_SENSORLESS = 0, + FOC_SENSOR_MODE_ENCODER, + FOC_SENSOR_MODE_HALL +} mc_foc_sensor_mode; + +typedef enum { + MOTOR_TYPE_BLDC = 0, + MOTOR_TYPE_DC, + MOTOR_TYPE_FOC +} mc_motor_type; + +typedef enum { + FAULT_CODE_NONE = 0, + FAULT_CODE_OVER_VOLTAGE, + FAULT_CODE_UNDER_VOLTAGE, + FAULT_CODE_DRV, + FAULT_CODE_ABS_OVER_CURRENT, + FAULT_CODE_OVER_TEMP_FET, + FAULT_CODE_OVER_TEMP_MOTOR +} mc_fault_code; + +typedef struct { + float v_in; + float temp_mos; + float temp_motor; + float current_motor; + float current_in; + float id; + float iq; + float rpm; + float duty_now; + float amp_hours; + float amp_hours_charged; + float watt_hours; + float watt_hours_charged; + int tachometer; + int tachometer_abs; + mc_fault_code fault_code; + float pid_pos; + uint8_t vesc_id; +} mc_values; + +typedef enum { + SENSOR_PORT_MODE_HALL = 0, + SENSOR_PORT_MODE_ABI, + SENSOR_PORT_MODE_AS5047_SPI +} sensor_port_mode; + +typedef enum { + DRV8301_OC_LIMIT = 0, + DRV8301_OC_LATCH_SHUTDOWN, + DRV8301_OC_REPORT_ONLY, + DRV8301_OC_DISABLED +} drv8301_oc_mode; + +typedef enum { + MOTOR_CONTROL_DUTY = 0, + MOTOR_CONTROL_CURRENT, + MOTOR_CONTROL_CURRENT_BRAKE, + MOTOR_CONTROL_RPM, + MOTOR_CONTROL_POS +} motor_control_mode; + +typedef enum { + CAN_BAUD_125K = 0, + CAN_BAUD_250K, + CAN_BAUD_500K, + CAN_BAUD_1M +} CAN_BAUD; + +typedef struct { + // Switching and drive + mc_pwm_mode pwm_mode; + mc_comm_mode comm_mode; + mc_motor_type motor_type; + mc_sensor_mode sensor_mode; + // Limits + float l_current_max; + float l_current_min; + float l_in_current_max; + float l_in_current_min; + float l_abs_current_max; + float l_min_erpm; + float l_max_erpm; + float l_erpm_start; + float l_max_erpm_fbrake; + float l_max_erpm_fbrake_cc; + float l_min_vin; + float l_max_vin; + float l_battery_cut_start; + float l_battery_cut_end; + bool l_slow_abs_current; + float l_temp_fet_start; + float l_temp_fet_end; + float l_temp_motor_start; + float l_temp_motor_end; + float l_temp_accel_dec; + float l_min_duty; + float l_max_duty; + float l_watt_max; + float l_watt_min; + // Overridden limits (Computed during runtime) + float lo_current_max; + float lo_current_min; + float lo_in_current_max; + float lo_in_current_min; + float lo_current_motor_max_now; + float lo_current_motor_min_now; + // Sensorless (bldc) + float sl_min_erpm; + float sl_min_erpm_cycle_int_limit; + float sl_max_fullbreak_current_dir_change; + float sl_cycle_int_limit; + float sl_phase_advance_at_br; + float sl_cycle_int_rpm_br; + float sl_bemf_coupling_k; + // Hall sensor + int8_t hall_table[8]; + float hall_sl_erpm; + // FOC + float foc_current_kp; + float foc_current_ki; + float foc_f_sw; + float foc_dt_us; + float foc_encoder_offset; + bool foc_encoder_inverted; + float foc_encoder_ratio; + float foc_motor_l; + float foc_motor_r; + float foc_motor_flux_linkage; + float foc_observer_gain; + float foc_observer_gain_slow; + float foc_pll_kp; + float foc_pll_ki; + float foc_duty_dowmramp_kp; + float foc_duty_dowmramp_ki; + float foc_openloop_rpm; + float foc_sl_openloop_hyst; + float foc_sl_openloop_time; + float foc_sl_d_current_duty; + float foc_sl_d_current_factor; + mc_foc_sensor_mode foc_sensor_mode; + uint8_t foc_hall_table[8]; + float foc_sl_erpm; + bool foc_sample_v0_v7; + bool foc_sample_high_current; + float foc_sat_comp; + bool foc_temp_comp; + float foc_temp_comp_base_temp; + float foc_current_filter_const; + // Speed PID + float s_pid_kp; + float s_pid_ki; + float s_pid_kd; + float s_pid_kd_filter; + float s_pid_min_erpm; + bool s_pid_allow_braking; + // Pos PID + float p_pid_kp; + float p_pid_ki; + float p_pid_kd; + float p_pid_kd_filter; + float p_pid_ang_div; + // Current controller + float cc_startup_boost_duty; + float cc_min_current; + float cc_gain; + float cc_ramp_step_max; + // Misc + int32_t m_fault_stop_time_ms; + float m_duty_ramp_step; + float m_current_backoff_gain; + uint32_t m_encoder_counts; + sensor_port_mode m_sensor_port_mode; + bool m_invert_direction; + drv8301_oc_mode m_drv8301_oc_mode; + int m_drv8301_oc_adj; + float m_bldc_f_sw_min; + float m_bldc_f_sw_max; + float m_dc_f_sw; + float m_ntc_motor_beta; + out_aux_mode m_out_aux_mode; +} mc_configuration; + +// Applications to use +typedef enum { + APP_NONE = 0, + APP_PPM, + APP_ADC, + APP_UART, + APP_PPM_UART, + APP_ADC_UART, + APP_NUNCHUK, + APP_NRF, + APP_CUSTOM +} app_use; + +// Throttle curve mode +typedef enum { + THR_EXP_EXPO = 0, + THR_EXP_NATURAL, + THR_EXP_POLY +} thr_exp_mode; + +// PPM control types +typedef enum { + PPM_CTRL_TYPE_NONE = 0, + PPM_CTRL_TYPE_CURRENT, + PPM_CTRL_TYPE_CURRENT_NOREV, + PPM_CTRL_TYPE_CURRENT_NOREV_BRAKE, + PPM_CTRL_TYPE_DUTY, + PPM_CTRL_TYPE_DUTY_NOREV, + PPM_CTRL_TYPE_PID, + PPM_CTRL_TYPE_PID_NOREV +} ppm_control_type; + +typedef struct { + ppm_control_type ctrl_type; + float pid_max_erpm; + float hyst; + float pulse_start; + float pulse_end; + float pulse_center; + bool median_filter; + bool safe_start; + float throttle_exp; + float throttle_exp_brake; + thr_exp_mode throttle_exp_mode; + float ramp_time_pos; + float ramp_time_neg; + bool multi_esc; + bool tc; + float tc_max_diff; +} ppm_config; + +// ADC control types +typedef enum { + ADC_CTRL_TYPE_NONE = 0, + ADC_CTRL_TYPE_CURRENT, + ADC_CTRL_TYPE_CURRENT_REV_CENTER, + ADC_CTRL_TYPE_CURRENT_REV_BUTTON, + ADC_CTRL_TYPE_CURRENT_REV_BUTTON_BRAKE_ADC, + ADC_CTRL_TYPE_CURRENT_NOREV_BRAKE_CENTER, + ADC_CTRL_TYPE_CURRENT_NOREV_BRAKE_BUTTON, + ADC_CTRL_TYPE_CURRENT_NOREV_BRAKE_ADC, + ADC_CTRL_TYPE_DUTY, + ADC_CTRL_TYPE_DUTY_REV_CENTER, + ADC_CTRL_TYPE_DUTY_REV_BUTTON, + ADC_CTRL_TYPE_PID, + ADC_CTRL_TYPE_PID_REV_CENTER, + ADC_CTRL_TYPE_PID_REV_BUTTON +} adc_control_type; + +typedef struct { + adc_control_type ctrl_type; + float hyst; + float voltage_start; + float voltage_end; + float voltage_center; + float voltage2_start; + float voltage2_end; + bool use_filter; + bool safe_start; + bool cc_button_inverted; + bool rev_button_inverted; + bool voltage_inverted; + bool voltage2_inverted; + float throttle_exp; + float throttle_exp_brake; + thr_exp_mode throttle_exp_mode; + float ramp_time_pos; + float ramp_time_neg; + bool multi_esc; + bool tc; + float tc_max_diff; + uint32_t update_rate_hz; +} adc_config; + +// Nunchuk control types +typedef enum { + CHUK_CTRL_TYPE_NONE = 0, + CHUK_CTRL_TYPE_CURRENT, + CHUK_CTRL_TYPE_CURRENT_NOREV +} chuk_control_type; + +typedef struct { + chuk_control_type ctrl_type; + float hyst; + float ramp_time_pos; + float ramp_time_neg; + float stick_erpm_per_s_in_cc; + float throttle_exp; + float throttle_exp_brake; + thr_exp_mode throttle_exp_mode; + bool multi_esc; + bool tc; + float tc_max_diff; +} chuk_config; + +// NRF Datatypes +typedef enum { + NRF_SPEED_250K = 0, + NRF_SPEED_1M, + NRF_SPEED_2M +} NRF_SPEED; + +typedef enum { + NRF_POWER_M18DBM = 0, + NRF_POWER_M12DBM, + NRF_POWER_M6DBM, + NRF_POWER_0DBM, + NRF_POWER_OFF +} NRF_POWER; + +typedef enum { + NRF_AW_3 = 0, + NRF_AW_4, + NRF_AW_5 +} NRF_AW; + +typedef enum { + NRF_CRC_DISABLED = 0, + NRF_CRC_1B, + NRF_CRC_2B +} NRF_CRC; + +typedef enum { + NRF_RETR_DELAY_250US = 0, + NRF_RETR_DELAY_500US, + NRF_RETR_DELAY_750US, + NRF_RETR_DELAY_1000US, + NRF_RETR_DELAY_1250US, + NRF_RETR_DELAY_1500US, + NRF_RETR_DELAY_1750US, + NRF_RETR_DELAY_2000US, + NRF_RETR_DELAY_2250US, + NRF_RETR_DELAY_2500US, + NRF_RETR_DELAY_2750US, + NRF_RETR_DELAY_3000US, + NRF_RETR_DELAY_3250US, + NRF_RETR_DELAY_3500US, + NRF_RETR_DELAY_3750US, + NRF_RETR_DELAY_4000US +} NRF_RETR_DELAY; + +typedef struct { + NRF_SPEED speed; + NRF_POWER power; + NRF_CRC crc_type; + NRF_RETR_DELAY retry_delay; + unsigned char retries; + unsigned char channel; + unsigned char address[3]; + bool send_crc_ack; +} nrf_config; + +typedef struct { + // Settings + uint8_t controller_id; + uint32_t timeout_msec; + float timeout_brake_current; + bool send_can_status; + uint32_t send_can_status_rate_hz; + CAN_BAUD can_baud_rate; + + // Application to use + app_use app_to_use; + + // PPM application settings + ppm_config app_ppm_conf; + + // ADC application settings + adc_config app_adc_conf; + + // UART application settings + uint32_t app_uart_baudrate; + + // Nunchuk application settings + chuk_config app_chuk_conf; + + // NRF application settings + nrf_config app_nrf_conf; +} app_configuration; + +// Communication commands +typedef enum { + COMM_FW_VERSION = 0, + COMM_JUMP_TO_BOOTLOADER, + COMM_ERASE_NEW_APP, + COMM_WRITE_NEW_APP_DATA, + COMM_GET_VALUES, + COMM_SET_DUTY, + COMM_SET_CURRENT, + COMM_SET_CURRENT_BRAKE, + COMM_SET_RPM, + COMM_SET_POS, + COMM_SET_HANDBRAKE, + COMM_SET_DETECT, + COMM_SET_SERVO_POS, + COMM_SET_MCCONF, + COMM_GET_MCCONF, + COMM_GET_MCCONF_DEFAULT, + COMM_SET_APPCONF, + COMM_GET_APPCONF, + COMM_GET_APPCONF_DEFAULT, + COMM_SAMPLE_PRINT, + COMM_TERMINAL_CMD, + COMM_PRINT, + COMM_ROTOR_POSITION, + COMM_EXPERIMENT_SAMPLE, + COMM_DETECT_MOTOR_PARAM, + COMM_DETECT_MOTOR_R_L, + COMM_DETECT_MOTOR_FLUX_LINKAGE, + COMM_DETECT_ENCODER, + COMM_DETECT_HALL_FOC, + COMM_REBOOT, + COMM_ALIVE, + COMM_GET_DECODED_PPM, + COMM_GET_DECODED_ADC, + COMM_GET_DECODED_CHUK, + COMM_FORWARD_CAN, + COMM_SET_CHUCK_DATA, + COMM_CUSTOM_APP_DATA, + COMM_NRF_START_PAIRING +} COMM_PACKET_ID; + +#endif /* DATATYPES_H_ */ diff --git a/Node/External/Inc/packet.h b/Node/External/Inc/packet.h new file mode 100644 index 0000000..489650b --- /dev/null +++ b/Node/External/Inc/packet.h @@ -0,0 +1,42 @@ +/* + Copyright 2012-2014 Benjamin Vedder benjamin@vedder.se + + This program is free software: you can redistribute it and/or modify + it under the terms of the GNU General Public License as published by + the Free Software Foundation, either version 3 of the License, or + (at your option) any later version. + + This program is distributed in the hope that it will be useful, + but WITHOUT ANY WARRANTY; without even the implied warranty of + MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + GNU General Public License for more details. + + You should have received a copy of the GNU General Public License + along with this program. If not, see . + */ + +/* + * packet.h + * + * Created on: 21 mar 2013 + * Author: benjamin + */ + +#ifndef PACKET_H_ +#define PACKET_H_ + +#include + +// Settings +#define PACKET_RX_TIMEOUT 2 +#define PACKET_HANDLERS 1 +#define PACKET_MAX_PL_LEN 512 + +// Functions +void packet_init(void (*s_func)(unsigned char *data, unsigned int len), + void (*p_func)(unsigned char *data, unsigned int len), int handler_num); +void packet_process_byte(uint8_t rx_data, int handler_num); +void packet_timerfunc(void); +void packet_send_packet(unsigned char *data, unsigned int len, int handler_num); + +#endif /* PACKET_H_ */ diff --git a/Node/External/Src/bldc_interface.c b/Node/External/Src/bldc_interface.c new file mode 100644 index 0000000..45f8db9 --- /dev/null +++ b/Node/External/Src/bldc_interface.c @@ -0,0 +1,913 @@ +/* + Copyright 2016-2018 Benjamin Vedder benjamin@vedder.se + + This program is free software: you can redistribute it and/or modify + it under the terms of the GNU General Public License as published by + the Free Software Foundation, either version 3 of the License, or + (at your option) any later version. + + This program is distributed in the hope that it will be useful, + but WITHOUT ANY WARRANTY; without even the implied warranty of + MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + GNU General Public License for more details. + + You should have received a copy of the GNU General Public License + along with this program. If not, see . + */ + +/* + * bldc_interface.c + * + * Compatible Firmware Versions + * 3.39 + * 3.40 + * + */ + +#include "bldc_interface.h" +#include "buffer.h" +#include + +// Private variables +// static unsigned char send_buffer[1024]; + +// Private variables for received data +// static mc_values values; +// static int fw_major; +// static int fw_minor; +// static float rotor_pos; +// static mc_configuration mcconf; +// static app_configuration appconf; +// static float detect_cycle_int_limit; +// static float detect_coupling_k; +// static signed char detect_hall_table[8]; +// static signed char detect_hall_res; +// static float dec_ppm; +// static float dec_ppm_len; +// static float dec_adc; +// static float dec_adc_voltage; +// static float dec_chuk; + +// Private functions +void send_packet_no_fwd(BldcInterface* interface, unsigned char *data, unsigned int len); + +// Function pointers +// static void(*send_func)(unsigned char *data, unsigned int len) = 0; +// static void(*forward_func)(unsigned char *data, unsigned int len) = 0; + +// Function pointers for received data +// static void(*rx_value_func)(mc_values *values) = 0; +// static void(*rx_printf_func)(char *str) = 0; +// static void(*rx_fw_func)(int major, int minor) = 0; +// static void(*rx_rotor_pos_func)(float pos) = 0; +// static void(*rx_mcconf_func)(mc_configuration *conf) = 0; +// static void(*rx_appconf_func)(app_configuration *conf) = 0; +// static void(*rx_detect_func)(float cycle_int_limit, float coupling_k, +// const signed char *hall_table, signed char hall_res) = 0; +// static void(*rx_dec_ppm_func)(float val, float ms) = 0; +// static void(*rx_dec_adc_func)(float val, float voltage) = 0; +// static void(*rx_dec_chuk_func)(float val) = 0; +// static void(*rx_mcconf_received_func)(void) = 0; +// static void(*rx_appconf_received_func)(void) = 0; +// static void(*motor_control_set_func)(motor_control_mode mode, float value) = 0; +// static void(*values_requested_func)(void) = 0; + +void bldc_interface_init(BldcInterface* interface, void(*func)(unsigned char *data, unsigned int len)) { + interface->send_func = func; +} + +void bldc_interface_set_forward_func(BldcInterface* interface, void(*func)(unsigned char *data, unsigned int len)) { + interface->forward_func = func; +} + +/** + * Send a packet using the set send function. + * + * @param data + * The packet data. + * + * @param len + * The data length. + */ +void bldc_interface_send_packet(BldcInterface* interface, unsigned char *data, unsigned int len) { + if (interface->send_func) { + interface->send_func(data, len); + } +} + +/** + * Process a received buffer with commands and data. + * + * @param data + * The buffer to process. + * + * @param len + * The length of the buffer. + */ +void bldc_interface_process_packet(BldcInterface* interface, unsigned char *data, unsigned int len) { + if (!len) { + return; + } + + if (interface->forward_func) { + interface->forward_func(data, len); + return; + } + + int32_t ind = 0; + int i = 0; + unsigned char id = data[0]; + data++; + len--; + + switch (id) { + case COMM_FW_VERSION: + if (len == 2) { + ind = 0; + interface->fw_major = data[ind++]; + interface->fw_minor = data[ind++]; + } else { + interface->fw_major = -1; + interface->fw_minor = -1; + } + break; + + case COMM_ERASE_NEW_APP: + case COMM_WRITE_NEW_APP_DATA: + // TODO + break; + + case COMM_GET_VALUES: + ind = 0; + interface->values.temp_mos = buffer_get_float16(data, 1e1, &ind); + interface->values.temp_motor = buffer_get_float16(data, 1e1, &ind); + interface->values.current_motor = buffer_get_float32(data, 1e2, &ind); + interface->values.current_in = buffer_get_float32(data, 1e2, &ind); + interface->values.id = buffer_get_float32(data, 1e2, &ind); + interface->values.iq = buffer_get_float32(data, 1e2, &ind); + interface->values.duty_now = buffer_get_float16(data, 1e3, &ind); + interface->values.rpm = buffer_get_float32(data, 1e0, &ind); + interface->values.v_in = buffer_get_float16(data, 1e1, &ind); + interface->values.amp_hours = buffer_get_float32(data, 1e4, &ind); + interface->values.amp_hours_charged = buffer_get_float32(data, 1e4, &ind); + interface->values.watt_hours = buffer_get_float32(data, 1e4, &ind); + interface->values.watt_hours_charged = buffer_get_float32(data, 1e4, &ind); + interface->values.tachometer = buffer_get_int32(data, &ind); + interface->values.tachometer_abs = buffer_get_int32(data, &ind); + interface->values.fault_code = (mc_fault_code)data[ind++]; + + if (ind < (int)len) { + interface->values.pid_pos = buffer_get_float32(data, 1e6, &ind); + } else { + interface->values.pid_pos = 0.0; + } + + if (ind < (int)len) { + interface->values.vesc_id = data[ind++]; + } else { + interface->values.vesc_id = 255; + } + + if (interface->rx_value_func) { + interface->rx_value_func(&interface->values); + } + break; + + case COMM_PRINT: + if (interface->rx_printf_func) { + data[len] = '\0'; + interface->rx_printf_func((char*)data); + } + break; + + case COMM_SAMPLE_PRINT: + // TODO + break; + + case COMM_ROTOR_POSITION: + ind = 0; + interface->rotor_pos = buffer_get_float32(data, 100000.0, &ind); + + if (interface->rx_rotor_pos_func) { + interface->rx_rotor_pos_func(interface->rotor_pos); + } + break; + + case COMM_EXPERIMENT_SAMPLE: + // TODO + break; + + case COMM_GET_MCCONF: + case COMM_GET_MCCONF_DEFAULT: + ind = 0; + interface->mcconf.pwm_mode = data[ind++]; + interface->mcconf.comm_mode = data[ind++]; + interface->mcconf.motor_type = data[ind++]; + interface->mcconf.sensor_mode = data[ind++]; + + interface->mcconf.l_current_max = buffer_get_float32_auto(data, &ind); + interface->mcconf.l_current_min = buffer_get_float32_auto(data, &ind); + interface->mcconf.l_in_current_max = buffer_get_float32_auto(data, &ind); + interface->mcconf.l_in_current_min = buffer_get_float32_auto(data, &ind); + interface->mcconf.l_abs_current_max = buffer_get_float32_auto(data, &ind); + interface->mcconf.l_min_erpm = buffer_get_float32_auto(data, &ind); + interface->mcconf.l_max_erpm = buffer_get_float32_auto(data, &ind); + interface->mcconf.l_erpm_start = buffer_get_float32_auto(data, &ind); + interface->mcconf.l_max_erpm_fbrake = buffer_get_float32_auto(data, &ind); + interface->mcconf.l_max_erpm_fbrake_cc = buffer_get_float32_auto(data, &ind); + interface->mcconf.l_min_vin = buffer_get_float32_auto(data, &ind); + interface->mcconf.l_max_vin = buffer_get_float32_auto(data, &ind); + interface->mcconf.l_battery_cut_start = buffer_get_float32_auto(data, &ind); + interface->mcconf.l_battery_cut_end = buffer_get_float32_auto(data, &ind); + interface->mcconf.l_slow_abs_current = data[ind++]; + interface->mcconf.l_temp_fet_start = buffer_get_float32_auto(data, &ind); + interface->mcconf.l_temp_fet_end = buffer_get_float32_auto(data, &ind); + interface->mcconf.l_temp_motor_start = buffer_get_float32_auto(data, &ind); + interface->mcconf.l_temp_motor_end = buffer_get_float32_auto(data, &ind); + interface->mcconf.l_temp_accel_dec = buffer_get_float32_auto(data, &ind); + interface->mcconf.l_min_duty = buffer_get_float32_auto(data, &ind); + interface->mcconf.l_max_duty = buffer_get_float32_auto(data, &ind); + interface->mcconf.l_watt_max = buffer_get_float32_auto(data, &ind); + interface->mcconf.l_watt_min = buffer_get_float32_auto(data, &ind); + + interface->mcconf.lo_current_max = interface->mcconf.l_current_max; + interface->mcconf.lo_current_min = interface->mcconf.l_current_min; + interface->mcconf.lo_in_current_max = interface->mcconf.l_in_current_max; + interface->mcconf.lo_in_current_min = interface->mcconf.l_in_current_min; + interface->mcconf.lo_current_motor_max_now = interface->mcconf.l_current_max; + interface->mcconf.lo_current_motor_min_now = interface->mcconf.l_current_min; + + interface->mcconf.sl_min_erpm = buffer_get_float32_auto(data, &ind); + interface->mcconf.sl_min_erpm_cycle_int_limit = buffer_get_float32_auto(data, &ind); + interface->mcconf.sl_max_fullbreak_current_dir_change = buffer_get_float32_auto(data, &ind); + interface->mcconf.sl_cycle_int_limit = buffer_get_float32_auto(data, &ind); + interface->mcconf.sl_phase_advance_at_br = buffer_get_float32_auto(data, &ind); + interface->mcconf.sl_cycle_int_rpm_br = buffer_get_float32_auto(data, &ind); + interface->mcconf.sl_bemf_coupling_k = buffer_get_float32_auto(data, &ind); + + memcpy(interface->mcconf.hall_table, data + ind, 8); + ind += 8; + interface->mcconf.hall_sl_erpm = buffer_get_float32_auto(data, &ind); + + interface->mcconf.foc_current_kp = buffer_get_float32_auto(data, &ind); + interface->mcconf.foc_current_ki = buffer_get_float32_auto(data, &ind); + interface->mcconf.foc_f_sw = buffer_get_float32_auto(data, &ind); + interface->mcconf.foc_dt_us = buffer_get_float32_auto(data, &ind); + interface->mcconf.foc_encoder_inverted = data[ind++]; + interface->mcconf.foc_encoder_offset = buffer_get_float32_auto(data, &ind); + interface->mcconf.foc_encoder_ratio = buffer_get_float32_auto(data, &ind); + interface->mcconf.foc_sensor_mode = data[ind++]; + interface->mcconf.foc_pll_kp = buffer_get_float32_auto(data, &ind); + interface->mcconf.foc_pll_ki = buffer_get_float32_auto(data, &ind); + interface->mcconf.foc_motor_l = buffer_get_float32_auto(data, &ind); + interface->mcconf.foc_motor_r = buffer_get_float32_auto(data, &ind); + interface->mcconf.foc_motor_flux_linkage = buffer_get_float32_auto(data, &ind); + interface->mcconf.foc_observer_gain = buffer_get_float32_auto(data, &ind); + interface->mcconf.foc_observer_gain_slow = buffer_get_float32_auto(data, &ind); + interface->mcconf.foc_duty_dowmramp_kp = buffer_get_float32_auto(data, &ind); + interface->mcconf.foc_duty_dowmramp_ki = buffer_get_float32_auto(data, &ind); + interface->mcconf.foc_openloop_rpm = buffer_get_float32_auto(data, &ind); + interface->mcconf.foc_sl_openloop_hyst = buffer_get_float32_auto(data, &ind); + interface->mcconf.foc_sl_openloop_time = buffer_get_float32_auto(data, &ind); + interface->mcconf.foc_sl_d_current_duty = buffer_get_float32_auto(data, &ind); + interface->mcconf.foc_sl_d_current_factor = buffer_get_float32_auto(data, &ind); + memcpy(interface->mcconf.foc_hall_table, data + ind, 8); + ind += 8; + interface->mcconf.foc_sl_erpm = buffer_get_float32_auto(data, &ind); + interface->mcconf.foc_sample_v0_v7 = data[ind++]; + interface->mcconf.foc_sample_high_current = data[ind++]; + interface->mcconf.foc_sat_comp = buffer_get_float32_auto(data, &ind); + interface->mcconf.foc_temp_comp = data[ind++]; + interface->mcconf.foc_temp_comp_base_temp = buffer_get_float32_auto(data, &ind); + interface->mcconf.foc_current_filter_const = buffer_get_float32_auto(data, &ind); + + interface->mcconf.s_pid_kp = buffer_get_float32_auto(data, &ind); + interface->mcconf.s_pid_ki = buffer_get_float32_auto(data, &ind); + interface->mcconf.s_pid_kd = buffer_get_float32_auto(data, &ind); + interface->mcconf.s_pid_kd_filter = buffer_get_float32_auto(data, &ind); + interface->mcconf.s_pid_min_erpm = buffer_get_float32_auto(data, &ind); + interface->mcconf.s_pid_allow_braking = data[ind++]; + + interface->mcconf.p_pid_kp = buffer_get_float32_auto(data, &ind); + interface->mcconf.p_pid_ki = buffer_get_float32_auto(data, &ind); + interface->mcconf.p_pid_kd = buffer_get_float32_auto(data, &ind); + interface->mcconf.p_pid_kd_filter = buffer_get_float32_auto(data, &ind); + interface->mcconf.p_pid_ang_div = buffer_get_float32_auto(data, &ind); + + interface->mcconf.cc_startup_boost_duty = buffer_get_float32_auto(data, &ind); + interface->mcconf.cc_min_current = buffer_get_float32_auto(data, &ind); + interface->mcconf.cc_gain = buffer_get_float32_auto(data, &ind); + interface->mcconf.cc_ramp_step_max = buffer_get_float32_auto(data, &ind); + + interface->mcconf.m_fault_stop_time_ms = buffer_get_int32(data, &ind); + interface->mcconf.m_duty_ramp_step = buffer_get_float32_auto(data, &ind); + interface->mcconf.m_current_backoff_gain = buffer_get_float32_auto(data, &ind); + interface->mcconf.m_encoder_counts = buffer_get_uint32(data, &ind); + interface->mcconf.m_sensor_port_mode = data[ind++]; + interface->mcconf.m_invert_direction = data[ind++]; + interface->mcconf.m_drv8301_oc_mode = data[ind++]; + interface->mcconf.m_drv8301_oc_adj = data[ind++]; + interface->mcconf.m_bldc_f_sw_min = buffer_get_float32_auto(data, &ind); + interface->mcconf.m_bldc_f_sw_max = buffer_get_float32_auto(data, &ind); + interface->mcconf.m_dc_f_sw = buffer_get_float32_auto(data, &ind); + interface->mcconf.m_ntc_motor_beta = buffer_get_float32_auto(data, &ind); + interface->mcconf.m_out_aux_mode = data[ind++]; + + if (interface->rx_mcconf_func) { + interface->rx_mcconf_func(&interface->mcconf); + } + break; + + case COMM_GET_APPCONF: + case COMM_GET_APPCONF_DEFAULT: + ind = 0; + interface->appconf.controller_id = data[ind++]; + interface->appconf.timeout_msec = buffer_get_uint32(data, &ind); + interface->appconf.timeout_brake_current = buffer_get_float32_auto(data, &ind); + interface->appconf.send_can_status = data[ind++]; + interface->appconf.send_can_status_rate_hz = buffer_get_uint16(data, &ind); + interface->appconf.can_baud_rate = data[ind++]; + + interface->appconf.app_to_use = data[ind++]; + + interface->appconf.app_ppm_conf.ctrl_type = data[ind++]; + interface->appconf.app_ppm_conf.pid_max_erpm = buffer_get_float32_auto(data, &ind); + interface->appconf.app_ppm_conf.hyst = buffer_get_float32_auto(data, &ind); + interface->appconf.app_ppm_conf.pulse_start = buffer_get_float32_auto(data, &ind); + interface->appconf.app_ppm_conf.pulse_end = buffer_get_float32_auto(data, &ind); + interface->appconf.app_ppm_conf.pulse_center = buffer_get_float32_auto(data, &ind); + interface->appconf.app_ppm_conf.median_filter = data[ind++]; + interface->appconf.app_ppm_conf.safe_start = data[ind++]; + interface->appconf.app_ppm_conf.throttle_exp = buffer_get_float32_auto(data, &ind); + interface->appconf.app_ppm_conf.throttle_exp_brake = buffer_get_float32_auto(data, &ind); + interface->appconf.app_ppm_conf.throttle_exp_mode = data[ind++]; + interface->appconf.app_ppm_conf.ramp_time_pos = buffer_get_float32_auto(data, &ind); + interface->appconf.app_ppm_conf.ramp_time_neg = buffer_get_float32_auto(data, &ind); + interface->appconf.app_ppm_conf.multi_esc = data[ind++]; + interface->appconf.app_ppm_conf.tc = data[ind++]; + interface->appconf.app_ppm_conf.tc_max_diff = buffer_get_float32_auto(data, &ind); + + interface->appconf.app_adc_conf.ctrl_type = data[ind++]; + interface->appconf.app_adc_conf.hyst = buffer_get_float32_auto(data, &ind); + interface->appconf.app_adc_conf.voltage_start = buffer_get_float32_auto(data, &ind); + interface->appconf.app_adc_conf.voltage_end = buffer_get_float32_auto(data, &ind); + interface->appconf.app_adc_conf.voltage_center = buffer_get_float32_auto(data, &ind); + interface->appconf.app_adc_conf.voltage2_start = buffer_get_float32_auto(data, &ind); + interface->appconf.app_adc_conf.voltage2_end = buffer_get_float32_auto(data, &ind); + interface->appconf.app_adc_conf.use_filter = data[ind++]; + interface->appconf.app_adc_conf.safe_start = data[ind++]; + interface->appconf.app_adc_conf.cc_button_inverted = data[ind++]; + interface->appconf.app_adc_conf.rev_button_inverted = data[ind++]; + interface->appconf.app_adc_conf.voltage_inverted = data[ind++]; + interface->appconf.app_adc_conf.voltage2_inverted = data[ind++]; + interface->appconf.app_adc_conf.throttle_exp = buffer_get_float32_auto(data, &ind); + interface->appconf.app_adc_conf.throttle_exp_brake = buffer_get_float32_auto(data, &ind); + interface->appconf.app_adc_conf.throttle_exp_mode = data[ind++]; + interface->appconf.app_adc_conf.ramp_time_pos = buffer_get_float32_auto(data, &ind); + interface->appconf.app_adc_conf.ramp_time_neg = buffer_get_float32_auto(data, &ind); + interface->appconf.app_adc_conf.multi_esc = data[ind++]; + interface->appconf.app_adc_conf.tc = data[ind++]; + interface->appconf.app_adc_conf.tc_max_diff = buffer_get_float32_auto(data, &ind); + interface->appconf.app_adc_conf.update_rate_hz = buffer_get_uint16(data, &ind); + + interface->appconf.app_uart_baudrate = buffer_get_uint32(data, &ind); + + interface->appconf.app_chuk_conf.ctrl_type = data[ind++]; + interface->appconf.app_chuk_conf.hyst = buffer_get_float32_auto(data, &ind); + interface->appconf.app_chuk_conf.ramp_time_pos = buffer_get_float32_auto(data, &ind); + interface->appconf.app_chuk_conf.ramp_time_neg = buffer_get_float32_auto(data, &ind); + interface->appconf.app_chuk_conf.stick_erpm_per_s_in_cc = buffer_get_float32_auto(data, &ind); + interface->appconf.app_chuk_conf.throttle_exp = buffer_get_float32_auto(data, &ind); + interface->appconf.app_chuk_conf.throttle_exp_brake = buffer_get_float32_auto(data, &ind); + interface->appconf.app_chuk_conf.throttle_exp_mode = data[ind++]; + interface->appconf.app_chuk_conf.multi_esc = data[ind++]; + interface->appconf.app_chuk_conf.tc = data[ind++]; + interface->appconf.app_chuk_conf.tc_max_diff = buffer_get_float32_auto(data, &ind); + + interface->appconf.app_nrf_conf.speed = data[ind++]; + interface->appconf.app_nrf_conf.power = data[ind++]; + interface->appconf.app_nrf_conf.crc_type = data[ind++]; + interface->appconf.app_nrf_conf.retry_delay = data[ind++]; + interface->appconf.app_nrf_conf.retries = data[ind++]; + interface->appconf.app_nrf_conf.channel = data[ind++]; + memcpy(interface->appconf.app_nrf_conf.address, data + ind, 3); + ind += 3; + interface->appconf.app_nrf_conf.send_crc_ack = data[ind++]; + + if (interface->rx_appconf_func) { + interface->rx_appconf_func(&interface->appconf); + } + break; + + case COMM_DETECT_MOTOR_PARAM: + ind = 0; + interface->detect_cycle_int_limit = buffer_get_float32(data, 1000.0, &ind); + interface->detect_coupling_k = buffer_get_float32(data, 1000.0, &ind); + for (i = 0;i < 8;i++) { + interface->detect_hall_table[i] = (const signed char)(data[ind++]); + } + interface->detect_hall_res = (const signed char)(data[ind++]); + + if (interface->rx_detect_func) { + interface->rx_detect_func(interface->detect_cycle_int_limit, + interface->detect_coupling_k, + interface->detect_hall_table, + interface->detect_hall_res); + } + break; + + case COMM_DETECT_MOTOR_R_L: { + // TODO! + } break; + + case COMM_DETECT_MOTOR_FLUX_LINKAGE: { + // TODO! + } break; + + case COMM_DETECT_ENCODER: { + // TODO! + } break; + + case COMM_DETECT_HALL_FOC: { + // TODO! + } break; + + case COMM_GET_DECODED_PPM: + ind = 0; + interface->dec_ppm = buffer_get_float32(data, 1000000.0, &ind); + interface->dec_ppm_len = buffer_get_float32(data, 1000000.0, &ind); + + if (interface->rx_dec_ppm_func) { + interface->rx_dec_ppm_func(interface->dec_ppm, interface->dec_ppm_len); + } + break; + + case COMM_GET_DECODED_ADC: + ind = 0; + interface->dec_adc = buffer_get_float32(data, 1000000.0, &ind); + interface->dec_adc_voltage = buffer_get_float32(data, 1000000.0, &ind); + // TODO for adc2 + + if (interface->rx_dec_adc_func) { + interface->rx_dec_adc_func(interface->dec_adc, interface->dec_adc_voltage); + } + break; + + case COMM_GET_DECODED_CHUK: + ind = 0; + interface->dec_chuk = buffer_get_float32(data, 1000000.0, &ind); + + if (interface->rx_dec_chuk_func) { + interface->rx_dec_chuk_func(interface->dec_chuk); + } + break; + + case COMM_SET_MCCONF: + // This is a confirmation that the new mcconf is received. + if (interface->rx_mcconf_received_func) { + interface->rx_mcconf_received_func(); + } + break; + + case COMM_SET_APPCONF: + // This is a confirmation that the new appconf is received. + if (interface->rx_appconf_received_func) { + interface->rx_appconf_received_func(); + } + break; + + default: + break; + } +} + +/** + * Function pointer setters. When data that is requested with the get functions + * is received, the corresponding function pointer will be called with the + * received data. + * + * @param func + * A function to be called when the corresponding data is received. + */ + +void bldc_interface_set_rx_value_func(BldcInterface* interface, void(*func)(mc_values *values)) { + interface->rx_value_func = func; +} + +void bldc_interface_set_rx_printf_func(BldcInterface* interface, void(*func)(char *str)) { + interface->rx_printf_func = func; +} + +void bldc_interface_set_rx_fw_func(BldcInterface* interface, void(*func)(int major, int minor)) { + interface->rx_fw_func = func; +} + +void bldc_interface_set_rx_rotor_pos_func(BldcInterface* interface, void(*func)(float pos)) { + interface->rx_rotor_pos_func = func; +} + +void bldc_interface_set_rx_mcconf_func(BldcInterface* interface, void(*func)(mc_configuration *conf)) { + interface->rx_mcconf_func = func; +} + +void bldc_interface_set_rx_appconf_func(BldcInterface* interface, void(*func)(app_configuration *conf)) { + interface->rx_appconf_func = func; +} + +void bldc_interface_set_rx_detect_func(BldcInterface* interface, void(*func)(float cycle_int_limit, float coupling_k, + const signed char *hall_table, signed char hall_res)) { + interface->rx_detect_func = func; +} + +void bldc_interface_set_rx_dec_ppm_func(BldcInterface* interface, void(*func)(float val, float ms)) { + interface->rx_dec_ppm_func = func; +} + +void bldc_interface_set_rx_dec_adc_func(BldcInterface* interface, void(*func)(float val, float voltage)) { + interface->rx_dec_adc_func = func; +} + +void bldc_interface_set_rx_dec_chuk_func(BldcInterface* interface, void(*func)(float val)) { + interface->rx_dec_chuk_func = func; +} + +void bldc_interface_set_rx_mcconf_received_func(BldcInterface* interface, void(*func)(void)) { + interface->rx_mcconf_received_func = func; +} + +void bldc_interface_set_rx_appconf_received_func(BldcInterface* interface, void(*func)(void)) { + interface->rx_appconf_received_func = func; +} + +void bldc_interface_set_sim_control_function(BldcInterface* interface, void(*func)(motor_control_mode mode, float value)) { + interface->motor_control_set_func = func; +} + +void bldc_interface_set_sim_values_func(BldcInterface* interface, void(*func)(void)) { + interface->values_requested_func = func; +} + +// Setters +void bldc_interface_terminal_cmd(BldcInterface* interface, char* cmd) { + int len = strlen(cmd); + interface->send_buffer[0] = COMM_TERMINAL_CMD; + memcpy(interface->send_buffer + 1, cmd, len); + send_packet_no_fwd(interface, interface->send_buffer, len + 1); +} + +void bldc_interface_set_duty_cycle(BldcInterface* interface, float dutyCycle) { + if (interface->motor_control_set_func) { + interface->motor_control_set_func(MOTOR_CONTROL_DUTY, dutyCycle); + return; + } + int32_t send_index = 0; + interface->send_buffer[send_index++] = COMM_SET_DUTY; + buffer_append_float32(interface->send_buffer, dutyCycle, 100000.0, &send_index); + send_packet_no_fwd(interface, interface->send_buffer, send_index); +} + +void bldc_interface_set_current(BldcInterface* interface, float current) { + if (interface->motor_control_set_func) { + interface->motor_control_set_func(MOTOR_CONTROL_CURRENT, current); + return; + } + int32_t send_index = 0; + interface->send_buffer[send_index++] = COMM_SET_CURRENT; + buffer_append_float32(interface->send_buffer, current, 1000.0, &send_index); + send_packet_no_fwd(interface, interface->send_buffer, send_index); +} + +void bldc_interface_set_current_brake(BldcInterface* interface, float current) { + if (interface->motor_control_set_func) { + interface->motor_control_set_func(MOTOR_CONTROL_CURRENT_BRAKE, current); + return; + } + int32_t send_index = 0; + interface->send_buffer[send_index++] = COMM_SET_CURRENT_BRAKE; + buffer_append_float32(interface->send_buffer, current, 1000.0, &send_index); + send_packet_no_fwd(interface, interface->send_buffer, send_index); +} + +void bldc_interface_set_rpm(BldcInterface* interface, int rpm) { + if (interface->motor_control_set_func) { + interface->motor_control_set_func(MOTOR_CONTROL_RPM, rpm); + return; + } + int32_t send_index = 0; + interface->send_buffer[send_index++] = COMM_SET_RPM; + buffer_append_int32(interface->send_buffer, rpm, &send_index); + send_packet_no_fwd(interface, interface->send_buffer, send_index); +} + +void bldc_interface_set_pos(BldcInterface* interface, float pos) { + if (interface->motor_control_set_func) { + interface->motor_control_set_func(MOTOR_CONTROL_POS, pos); + return; + } + int32_t send_index = 0; + interface->send_buffer[send_index++] = COMM_SET_POS; + buffer_append_float32(interface->send_buffer, pos, 1000000.0, &send_index); + send_packet_no_fwd(interface, interface->send_buffer, send_index); +} + +void bldc_interface_set_handbrake(BldcInterface* interface, float current) { + int32_t send_index = 0; + interface->send_buffer[send_index++] = COMM_SET_HANDBRAKE; + buffer_append_float32(interface->send_buffer, current, 1e3, &send_index); + send_packet_no_fwd(interface, interface->send_buffer, send_index); +} + +void bldc_interface_set_servo_pos(BldcInterface* interface, float pos) { + int32_t send_index = 0; + interface->send_buffer[send_index++] = COMM_SET_SERVO_POS; + buffer_append_float16(interface->send_buffer, pos, 1000.0, &send_index); + send_packet_no_fwd(interface, interface->send_buffer, send_index); +} + +void bldc_interface_set_mcconf(BldcInterface* interface, const mc_configuration *mcconf) { + int32_t ind = 0; + interface->send_buffer[ind++] = COMM_SET_MCCONF; + + interface->send_buffer[ind++] = mcconf->pwm_mode; + interface->send_buffer[ind++] = mcconf->comm_mode; + interface->send_buffer[ind++] = mcconf->motor_type; + interface->send_buffer[ind++] = mcconf->sensor_mode; + + buffer_append_float32_auto(interface->send_buffer, mcconf->l_current_max, &ind); + buffer_append_float32_auto(interface->send_buffer, mcconf->l_current_min, &ind); + buffer_append_float32_auto(interface->send_buffer, mcconf->l_in_current_max, &ind); + buffer_append_float32_auto(interface->send_buffer, mcconf->l_in_current_min, &ind); + buffer_append_float32_auto(interface->send_buffer, mcconf->l_abs_current_max, &ind); + buffer_append_float32_auto(interface->send_buffer, mcconf->l_min_erpm, &ind); + buffer_append_float32_auto(interface->send_buffer, mcconf->l_max_erpm, &ind); + buffer_append_float32_auto(interface->send_buffer, mcconf->l_erpm_start, &ind); + buffer_append_float32_auto(interface->send_buffer, mcconf->l_max_erpm_fbrake, &ind); + buffer_append_float32_auto(interface->send_buffer, mcconf->l_max_erpm_fbrake_cc, &ind); + buffer_append_float32_auto(interface->send_buffer, mcconf->l_min_vin, &ind); + buffer_append_float32_auto(interface->send_buffer, mcconf->l_max_vin, &ind); + buffer_append_float32_auto(interface->send_buffer, mcconf->l_battery_cut_start, &ind); + buffer_append_float32_auto(interface->send_buffer, mcconf->l_battery_cut_end, &ind); + interface->send_buffer[ind++] = mcconf->l_slow_abs_current; + buffer_append_float32_auto(interface->send_buffer, mcconf->l_temp_fet_start, &ind); + buffer_append_float32_auto(interface->send_buffer, mcconf->l_temp_fet_end, &ind); + buffer_append_float32_auto(interface->send_buffer, mcconf->l_temp_motor_start, &ind); + buffer_append_float32_auto(interface->send_buffer, mcconf->l_temp_motor_end, &ind); + buffer_append_float32_auto(interface->send_buffer, mcconf->l_temp_accel_dec, &ind); + buffer_append_float32_auto(interface->send_buffer, mcconf->l_min_duty, &ind); + buffer_append_float32_auto(interface->send_buffer, mcconf->l_max_duty, &ind); + buffer_append_float32_auto(interface->send_buffer, mcconf->l_watt_max, &ind); + buffer_append_float32_auto(interface->send_buffer, mcconf->l_watt_min, &ind); + + buffer_append_float32_auto(interface->send_buffer, mcconf->sl_min_erpm, &ind); + buffer_append_float32_auto(interface->send_buffer, mcconf->sl_min_erpm_cycle_int_limit, &ind); + buffer_append_float32_auto(interface->send_buffer, mcconf->sl_max_fullbreak_current_dir_change, &ind); + buffer_append_float32_auto(interface->send_buffer, mcconf->sl_cycle_int_limit, &ind); + buffer_append_float32_auto(interface->send_buffer, mcconf->sl_phase_advance_at_br, &ind); + buffer_append_float32_auto(interface->send_buffer, mcconf->sl_cycle_int_rpm_br, &ind); + buffer_append_float32_auto(interface->send_buffer, mcconf->sl_bemf_coupling_k, &ind); + + memcpy(interface->send_buffer + ind, mcconf->hall_table, 8); + ind += 8; + buffer_append_float32_auto(interface->send_buffer, mcconf->hall_sl_erpm, &ind); + + buffer_append_float32_auto(interface->send_buffer, mcconf->foc_current_kp, &ind); + buffer_append_float32_auto(interface->send_buffer, mcconf->foc_current_ki, &ind); + buffer_append_float32_auto(interface->send_buffer, mcconf->foc_f_sw, &ind); + buffer_append_float32_auto(interface->send_buffer, mcconf->foc_dt_us, &ind); + interface->send_buffer[ind++] = mcconf->foc_encoder_inverted; + buffer_append_float32_auto(interface->send_buffer, mcconf->foc_encoder_offset, &ind); + buffer_append_float32_auto(interface->send_buffer, mcconf->foc_encoder_ratio, &ind); + interface->send_buffer[ind++] = mcconf->foc_sensor_mode; + buffer_append_float32_auto(interface->send_buffer, mcconf->foc_pll_kp, &ind); + buffer_append_float32_auto(interface->send_buffer, mcconf->foc_pll_ki, &ind); + buffer_append_float32_auto(interface->send_buffer, mcconf->foc_motor_l, &ind); + buffer_append_float32_auto(interface->send_buffer, mcconf->foc_motor_r, &ind); + buffer_append_float32_auto(interface->send_buffer, mcconf->foc_motor_flux_linkage, &ind); + buffer_append_float32_auto(interface->send_buffer, mcconf->foc_observer_gain, &ind); + buffer_append_float32_auto(interface->send_buffer, mcconf->foc_observer_gain_slow, &ind); + buffer_append_float32_auto(interface->send_buffer, mcconf->foc_duty_dowmramp_kp, &ind); + buffer_append_float32_auto(interface->send_buffer, mcconf->foc_duty_dowmramp_ki, &ind); + buffer_append_float32_auto(interface->send_buffer, mcconf->foc_openloop_rpm, &ind); + buffer_append_float32_auto(interface->send_buffer, mcconf->foc_sl_openloop_hyst, &ind); + buffer_append_float32_auto(interface->send_buffer, mcconf->foc_sl_openloop_time, &ind); + buffer_append_float32_auto(interface->send_buffer, mcconf->foc_sl_d_current_duty, &ind); + buffer_append_float32_auto(interface->send_buffer, mcconf->foc_sl_d_current_factor, &ind); + memcpy(interface->send_buffer + ind, mcconf->foc_hall_table, 8); + ind += 8; + buffer_append_float32_auto(interface->send_buffer, mcconf->foc_sl_erpm, &ind); + interface->send_buffer[ind++] = mcconf->foc_sample_v0_v7; + interface->send_buffer[ind++] = mcconf->foc_sample_high_current; + buffer_append_float32_auto(interface->send_buffer, mcconf->foc_sat_comp, &ind); + interface->send_buffer[ind++] = mcconf->foc_temp_comp; + buffer_append_float32_auto(interface->send_buffer, mcconf->foc_temp_comp_base_temp, &ind); + buffer_append_float32_auto(interface->send_buffer, mcconf->foc_current_filter_const, &ind); + + buffer_append_float32_auto(interface->send_buffer, mcconf->s_pid_kp, &ind); + buffer_append_float32_auto(interface->send_buffer, mcconf->s_pid_ki, &ind); + buffer_append_float32_auto(interface->send_buffer, mcconf->s_pid_kd, &ind); + buffer_append_float32_auto(interface->send_buffer, mcconf->s_pid_kd_filter, &ind); + buffer_append_float32_auto(interface->send_buffer, mcconf->s_pid_min_erpm, &ind); + interface->send_buffer[ind++] = mcconf->s_pid_allow_braking; + + buffer_append_float32_auto(interface->send_buffer, mcconf->p_pid_kp, &ind); + buffer_append_float32_auto(interface->send_buffer, mcconf->p_pid_ki, &ind); + buffer_append_float32_auto(interface->send_buffer, mcconf->p_pid_kd, &ind); + buffer_append_float32_auto(interface->send_buffer, mcconf->p_pid_kd_filter, &ind); + buffer_append_float32_auto(interface->send_buffer, mcconf->p_pid_ang_div, &ind); + + buffer_append_float32_auto(interface->send_buffer, mcconf->cc_startup_boost_duty, &ind); + buffer_append_float32_auto(interface->send_buffer, mcconf->cc_min_current, &ind); + buffer_append_float32_auto(interface->send_buffer, mcconf->cc_gain, &ind); + buffer_append_float32_auto(interface->send_buffer, mcconf->cc_ramp_step_max, &ind); + + buffer_append_int32(interface->send_buffer, mcconf->m_fault_stop_time_ms, &ind); + buffer_append_float32_auto(interface->send_buffer, mcconf->m_duty_ramp_step, &ind); + buffer_append_float32_auto(interface->send_buffer, mcconf->m_current_backoff_gain, &ind); + buffer_append_uint32(interface->send_buffer, mcconf->m_encoder_counts, &ind); + interface->send_buffer[ind++] = mcconf->m_sensor_port_mode; + interface->send_buffer[ind++] = mcconf->m_invert_direction; + interface->send_buffer[ind++] = mcconf->m_drv8301_oc_mode; + interface->send_buffer[ind++] = mcconf->m_drv8301_oc_adj; + buffer_append_float32_auto(interface->send_buffer, mcconf->m_bldc_f_sw_min, &ind); + buffer_append_float32_auto(interface->send_buffer, mcconf->m_bldc_f_sw_max, &ind); + buffer_append_float32_auto(interface->send_buffer, mcconf->m_dc_f_sw, &ind); + buffer_append_float32_auto(interface->send_buffer, mcconf->m_ntc_motor_beta, &ind); + interface->send_buffer[ind++] = mcconf->m_out_aux_mode; + + send_packet_no_fwd(interface, interface->send_buffer, ind); +} + +void bldc_interface_set_appconf(BldcInterface* interface, const app_configuration *appconf) { + int32_t ind = 0; + interface->send_buffer[ind++] = COMM_SET_APPCONF; + interface->send_buffer[ind++] = appconf->controller_id; + buffer_append_uint32(interface->send_buffer, appconf->timeout_msec, &ind); + buffer_append_float32_auto(interface->send_buffer, appconf->timeout_brake_current, &ind); + interface->send_buffer[ind++] = appconf->send_can_status; + buffer_append_uint16(interface->send_buffer, appconf->send_can_status_rate_hz, &ind); + interface->send_buffer[ind++] = appconf->can_baud_rate; + + interface->send_buffer[ind++] = appconf->app_to_use; + + interface->send_buffer[ind++] = appconf->app_ppm_conf.ctrl_type; + buffer_append_float32_auto(interface->send_buffer, appconf->app_ppm_conf.pid_max_erpm, &ind); + buffer_append_float32_auto(interface->send_buffer, appconf->app_ppm_conf.hyst, &ind); + buffer_append_float32_auto(interface->send_buffer, appconf->app_ppm_conf.pulse_start, &ind); + buffer_append_float32_auto(interface->send_buffer, appconf->app_ppm_conf.pulse_end, &ind); + buffer_append_float32_auto(interface->send_buffer, appconf->app_ppm_conf.pulse_center, &ind); + interface->send_buffer[ind++] = appconf->app_ppm_conf.median_filter; + interface->send_buffer[ind++] = appconf->app_ppm_conf.safe_start; + buffer_append_float32_auto(interface->send_buffer, appconf->app_ppm_conf.throttle_exp, &ind); + buffer_append_float32_auto(interface->send_buffer, appconf->app_ppm_conf.throttle_exp_brake, &ind); + interface->send_buffer[ind++] = appconf->app_ppm_conf.throttle_exp_mode; + buffer_append_float32_auto(interface->send_buffer, appconf->app_ppm_conf.ramp_time_pos, &ind); + buffer_append_float32_auto(interface->send_buffer, appconf->app_ppm_conf.ramp_time_neg, &ind); + interface->send_buffer[ind++] = appconf->app_ppm_conf.multi_esc; + interface->send_buffer[ind++] = appconf->app_ppm_conf.tc; + buffer_append_float32_auto(interface->send_buffer, appconf->app_ppm_conf.tc_max_diff, &ind); + + interface->send_buffer[ind++] = appconf->app_adc_conf.ctrl_type; + buffer_append_float32_auto(interface->send_buffer, appconf->app_adc_conf.hyst, &ind); + buffer_append_float32_auto(interface->send_buffer, appconf->app_adc_conf.voltage_start, &ind); + buffer_append_float32_auto(interface->send_buffer, appconf->app_adc_conf.voltage_end, &ind); + buffer_append_float32_auto(interface->send_buffer, appconf->app_adc_conf.voltage_center, &ind); + buffer_append_float32_auto(interface->send_buffer, appconf->app_adc_conf.voltage2_start, &ind); + buffer_append_float32_auto(interface->send_buffer, appconf->app_adc_conf.voltage2_end, &ind); + interface->send_buffer[ind++] = appconf->app_adc_conf.use_filter; + interface->send_buffer[ind++] = appconf->app_adc_conf.safe_start; + interface->send_buffer[ind++] = appconf->app_adc_conf.cc_button_inverted; + interface->send_buffer[ind++] = appconf->app_adc_conf.rev_button_inverted; + interface->send_buffer[ind++] = appconf->app_adc_conf.voltage_inverted; + interface->send_buffer[ind++] = appconf->app_adc_conf.voltage2_inverted; + buffer_append_float32_auto(interface->send_buffer, appconf->app_adc_conf.throttle_exp, &ind); + buffer_append_float32_auto(interface->send_buffer, appconf->app_adc_conf.throttle_exp_brake, &ind); + interface->send_buffer[ind++] = appconf->app_adc_conf.throttle_exp_mode; + buffer_append_float32_auto(interface->send_buffer, appconf->app_adc_conf.ramp_time_pos, &ind); + buffer_append_float32_auto(interface->send_buffer, appconf->app_adc_conf.ramp_time_neg, &ind); + interface->send_buffer[ind++] = appconf->app_adc_conf.multi_esc; + interface->send_buffer[ind++] = appconf->app_adc_conf.tc; + buffer_append_float32_auto(interface->send_buffer, appconf->app_adc_conf.tc_max_diff, &ind); + buffer_append_uint16(interface->send_buffer, appconf->app_adc_conf.update_rate_hz, &ind); + + buffer_append_uint32(interface->send_buffer, appconf->app_uart_baudrate, &ind); + + interface->send_buffer[ind++] = appconf->app_chuk_conf.ctrl_type; + buffer_append_float32_auto(interface->send_buffer, appconf->app_chuk_conf.hyst, &ind); + buffer_append_float32_auto(interface->send_buffer, appconf->app_chuk_conf.ramp_time_pos, &ind); + buffer_append_float32_auto(interface->send_buffer, appconf->app_chuk_conf.ramp_time_neg, &ind); + buffer_append_float32_auto(interface->send_buffer, appconf->app_chuk_conf.stick_erpm_per_s_in_cc, &ind); + buffer_append_float32_auto(interface->send_buffer, appconf->app_chuk_conf.throttle_exp, &ind); + buffer_append_float32_auto(interface->send_buffer, appconf->app_chuk_conf.throttle_exp_brake, &ind); + interface->send_buffer[ind++] = appconf->app_chuk_conf.throttle_exp_mode; + interface->send_buffer[ind++] = appconf->app_chuk_conf.multi_esc; + interface->send_buffer[ind++] = appconf->app_chuk_conf.tc; + buffer_append_float32_auto(interface->send_buffer, appconf->app_chuk_conf.tc_max_diff, &ind); + + interface->send_buffer[ind++] = appconf->app_nrf_conf.speed; + interface->send_buffer[ind++] = appconf->app_nrf_conf.power; + interface->send_buffer[ind++] = appconf->app_nrf_conf.crc_type; + interface->send_buffer[ind++] = appconf->app_nrf_conf.retry_delay; + interface->send_buffer[ind++] = appconf->app_nrf_conf.retries; + interface->send_buffer[ind++] = appconf->app_nrf_conf.channel; + memcpy(interface->send_buffer + ind, appconf->app_nrf_conf.address, 3); + ind += 3; + interface->send_buffer[ind++] = appconf->app_nrf_conf.send_crc_ack; + + send_packet_no_fwd(interface, interface->send_buffer, ind); +} + +// Getters +void bldc_interface_get_fw_version(BldcInterface* interface) { + int32_t send_index = 0; + interface->send_buffer[send_index++] = COMM_FW_VERSION; + send_packet_no_fwd(interface, interface->send_buffer, send_index); +} + +void bldc_interface_get_values(BldcInterface* interface) { + if (interface->values_requested_func) { + interface->values_requested_func(); + return; + } + int32_t send_index = 0; + interface->send_buffer[send_index++] = COMM_GET_VALUES; + send_packet_no_fwd(interface, interface->send_buffer, send_index); +} + +void bldc_interface_get_mcconf(BldcInterface* interface) { + int32_t send_index = 0; + interface->send_buffer[send_index++] = COMM_GET_MCCONF; + send_packet_no_fwd(interface, interface->send_buffer, send_index); +} + +void bldc_interface_get_appconf(BldcInterface* interface) { + int32_t send_index = 0; + interface->send_buffer[send_index++] = COMM_GET_APPCONF; + send_packet_no_fwd(interface, interface->send_buffer, send_index); +} + +void bldc_interface_get_decoded_ppm(BldcInterface* interface) { + int32_t send_index = 0; + interface->send_buffer[send_index++] = COMM_GET_DECODED_PPM; + send_packet_no_fwd(interface, interface->send_buffer, send_index); +} + +void bldc_interface_get_decoded_adc(BldcInterface* interface) { + int32_t send_index = 0; + interface->send_buffer[send_index++] = COMM_GET_DECODED_ADC; + send_packet_no_fwd(interface, interface->send_buffer, send_index); +} + +void bldc_interface_get_decoded_chuk(BldcInterface* interface) { + int32_t send_index = 0; + interface->send_buffer[send_index++] = COMM_GET_DECODED_CHUK; + send_packet_no_fwd(interface, interface->send_buffer, send_index); +} + +// Other functions +void bldc_interface_detect_motor_param(BldcInterface* interface, float current, float min_rpm, float low_duty) { + int32_t send_index = 0; + interface->send_buffer[send_index++] = COMM_DETECT_MOTOR_PARAM; + buffer_append_float32(interface->send_buffer, current, 1000.0, &send_index); + buffer_append_float32(interface->send_buffer, min_rpm, 1000.0, &send_index); + buffer_append_float32(interface->send_buffer, low_duty, 1000.0, &send_index); + send_packet_no_fwd(interface, interface->send_buffer, send_index); +} + +void bldc_interface_reboot(BldcInterface* interface) { + int32_t send_index = 0; + interface->send_buffer[send_index++] = COMM_REBOOT; + send_packet_no_fwd(interface, interface->send_buffer, send_index); +} + +void bldc_interface_send_alive(BldcInterface* interface) { + int32_t send_index = 0; + interface->send_buffer[send_index++] = COMM_ALIVE; + send_packet_no_fwd(interface, interface->send_buffer, send_index); +} + +void send_values_to_receiver(BldcInterface* interface, mc_values *values) { + if (interface->rx_value_func) { + interface->rx_value_func(values); + } +} + +// Helpers +const char* bldc_interface_fault_to_string(mc_fault_code fault) { + switch (fault) { + case FAULT_CODE_NONE: return "FAULT_CODE_NONE"; + case FAULT_CODE_OVER_VOLTAGE: return "FAULT_CODE_OVER_VOLTAGE"; + case FAULT_CODE_UNDER_VOLTAGE: return "FAULT_CODE_UNDER_VOLTAGE"; + case FAULT_CODE_DRV: return "FAULT_CODE_DRV"; + case FAULT_CODE_ABS_OVER_CURRENT: return "FAULT_CODE_ABS_OVER_CURRENT"; + case FAULT_CODE_OVER_TEMP_FET: return "FAULT_CODE_OVER_TEMP_FET"; + case FAULT_CODE_OVER_TEMP_MOTOR: return "FAULT_CODE_OVER_TEMP_MOTOR"; + default: return "Unknown fault"; + } +} + +// Private functions +void send_packet_no_fwd(BldcInterface* interface, unsigned char *data, unsigned int len) { + if (!interface->forward_func) { + bldc_interface_send_packet(interface, data, len); + } +} diff --git a/Node/External/Src/bldc_interface_uart.c b/Node/External/Src/bldc_interface_uart.c new file mode 100644 index 0000000..d459753 --- /dev/null +++ b/Node/External/Src/bldc_interface_uart.c @@ -0,0 +1,96 @@ +/* + Copyright 2015 Benjamin Vedder benjamin@vedder.se + + This program is free software: you can redistribute it and/or modify + it under the terms of the GNU General Public License as published by + the Free Software Foundation, either version 3 of the License, or + (at your option) any later version. + + This program is distributed in the hope that it will be useful, + but WITHOUT ANY WARRANTY; without even the implied warranty of + MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + GNU General Public License for more details. + + You should have received a copy of the GNU General Public License + along with this program. If not, see . + */ + +/* + * bldc_interface_uart.c + * + * Created on: 9 okt 2015 + * Author: benjamin + */ + +#include "bldc_interface_uart.h" +#include "bldc_interface.h" + +// Settings +#define PACKET_HANDLER 0 + +// Private functions +static void process_packet(BldcInterface* interface, unsigned char *data, unsigned int len); +static void send_packet_bldc_interface(BldcInterface* interface, unsigned char *data, unsigned int len); + +/** + * Initialize the UART BLDC interface and provide a function to be used for + * sending packets. + * + * @param func + * Function provided for sending packets. + */ +void bldc_interface_uart_init(BldcInterface* interface, void(*func)(unsigned char *data, unsigned int len)) { + // Initialize packet handler + packet_init(func, process_packet, PACKET_HANDLER); + + // Initialize the bldc interface and provide a send function + bldc_interface_init(interface, send_packet_bldc_interface); +} + +/** + * Process one byte received on the UART. Once a full packet is received the + * corresponding callback will be called by bldc_interface. + * + * @param b + * The byte received on the UART to process. + */ +void bldc_interface_uart_process_byte(BldcInterface* interface, unsigned char b) { + packet_process_byte(b, PACKET_HANDLER); +} + +/** + * Call this function at around 1 khz to reset the state of the packet + * interface after a timeout in case data is lost. + */ +void bldc_interface_uart_run_timer(BldcInterface* interface) { + packet_timerfunc(); +} + +/** + * Callback for the packet handled for when a whole packet is received, + * assembled and checked. + * + * @param data + * Data array pointer + * @param len + * Data array length + */ +static void process_packet(BldcInterface* interface, unsigned char *data, unsigned int len) { + // Let bldc_interface process the packet. + bldc_interface_process_packet(interface, data, len); +} + +/** + * Callback that bldc_interface uses to send packets. + * + * @param data + * Data array pointer + * @param len + * Data array length + */ +static void send_packet_bldc_interface(BldcInterface* interface, unsigned char *data, unsigned int len) { + // Pass the packet to the packet handler to add checksum, length, start and stop bytes. + packet_send_packet(data, len, PACKET_HANDLER); +} + + diff --git a/Node/External/Src/buffer.c b/Node/External/Src/buffer.c new file mode 100644 index 0000000..a001925 --- /dev/null +++ b/Node/External/Src/buffer.c @@ -0,0 +1,187 @@ +/* + Copyright 2012-2018 Benjamin Vedder benjamin@vedder.se + + This program is free software: you can redistribute it and/or modify + it under the terms of the GNU General Public License as published by + the Free Software Foundation, either version 3 of the License, or + (at your option) any later version. + + This program is distributed in the hope that it will be useful, + but WITHOUT ANY WARRANTY; without even the implied warranty of + MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + GNU General Public License for more details. + + You should have received a copy of the GNU General Public License + along with this program. If not, see . + */ + +#include "buffer.h" +#include +#include + +void buffer_append_int16(uint8_t* buffer, int16_t number, int32_t *index) { + buffer[(*index)++] = number >> 8; + buffer[(*index)++] = number; +} + +void buffer_append_uint16(uint8_t* buffer, uint16_t number, int32_t *index) { + buffer[(*index)++] = number >> 8; + buffer[(*index)++] = number; +} + +void buffer_append_int32(uint8_t* buffer, int32_t number, int32_t *index) { + buffer[(*index)++] = number >> 24; + buffer[(*index)++] = number >> 16; + buffer[(*index)++] = number >> 8; + buffer[(*index)++] = number; +} + +void buffer_append_uint32(uint8_t* buffer, uint32_t number, int32_t *index) { + buffer[(*index)++] = number >> 24; + buffer[(*index)++] = number >> 16; + buffer[(*index)++] = number >> 8; + buffer[(*index)++] = number; +} + +void buffer_append_int64(uint8_t* buffer, int64_t number, int32_t *index) { + buffer[(*index)++] = number >> 56; + buffer[(*index)++] = number >> 48; + buffer[(*index)++] = number >> 40; + buffer[(*index)++] = number >> 32; + buffer[(*index)++] = number >> 24; + buffer[(*index)++] = number >> 16; + buffer[(*index)++] = number >> 8; + buffer[(*index)++] = number; +} + +void buffer_append_uint64(uint8_t* buffer, uint64_t number, int32_t *index) { + buffer[(*index)++] = number >> 56; + buffer[(*index)++] = number >> 48; + buffer[(*index)++] = number >> 40; + buffer[(*index)++] = number >> 32; + buffer[(*index)++] = number >> 24; + buffer[(*index)++] = number >> 16; + buffer[(*index)++] = number >> 8; + buffer[(*index)++] = number; +} + +void buffer_append_float16(uint8_t* buffer, float number, float scale, int32_t *index) { + buffer_append_int16(buffer, (int16_t)(number * scale), index); +} + +void buffer_append_float32(uint8_t* buffer, float number, float scale, int32_t *index) { + buffer_append_int32(buffer, (int32_t)(number * scale), index); +} + +void buffer_append_double64(uint8_t* buffer, double number, double scale, int32_t *index) { + buffer_append_int64(buffer, (int64_t)(number * scale), index); +} + +void buffer_append_float32_auto(uint8_t* buffer, float number, int32_t *index) { + int e = 0; + float sig = frexpf(number, &e); + float sig_abs = fabsf(sig); + uint32_t sig_i = 0; + + if (sig_abs >= 0.5) { + sig_i = (uint32_t)((sig_abs - 0.5f) * 2.0f * 8388608.0f); + e += 126; + } + + uint32_t res = ((e & 0xFF) << 23) | (sig_i & 0x7FFFFF); + if (sig < 0) { + res |= 1 << 31; + } + + buffer_append_uint32(buffer, res, index); +} + +int16_t buffer_get_int16(const uint8_t *buffer, int32_t *index) { + int16_t res = ((uint16_t) buffer[*index]) << 8 | + ((uint16_t) buffer[*index + 1]); + *index += 2; + return res; +} + +uint16_t buffer_get_uint16(const uint8_t *buffer, int32_t *index) { + uint16_t res = ((uint16_t) buffer[*index]) << 8 | + ((uint16_t) buffer[*index + 1]); + *index += 2; + return res; +} + +int32_t buffer_get_int32(const uint8_t *buffer, int32_t *index) { + int32_t res = ((uint32_t) buffer[*index]) << 24 | + ((uint32_t) buffer[*index + 1]) << 16 | + ((uint32_t) buffer[*index + 2]) << 8 | + ((uint32_t) buffer[*index + 3]); + *index += 4; + return res; +} + +uint32_t buffer_get_uint32(const uint8_t *buffer, int32_t *index) { + uint32_t res = ((uint32_t) buffer[*index]) << 24 | + ((uint32_t) buffer[*index + 1]) << 16 | + ((uint32_t) buffer[*index + 2]) << 8 | + ((uint32_t) buffer[*index + 3]); + *index += 4; + return res; +} + +int64_t buffer_get_int64(const uint8_t *buffer, int32_t *index) { + int64_t res = ((uint64_t) buffer[*index]) << 56 | + ((uint64_t) buffer[*index + 1]) << 48 | + ((uint64_t) buffer[*index + 2]) << 40 | + ((uint64_t) buffer[*index + 3]) << 32 | + ((uint64_t) buffer[*index + 4]) << 24 | + ((uint64_t) buffer[*index + 5]) << 16 | + ((uint64_t) buffer[*index + 6]) << 8 | + ((uint64_t) buffer[*index + 7]); + *index += 8; + return res; +} + +uint64_t buffer_get_uint64(const uint8_t *buffer, int32_t *index) { + uint64_t res = ((uint64_t) buffer[*index]) << 56 | + ((uint64_t) buffer[*index + 1]) << 48 | + ((uint64_t) buffer[*index + 2]) << 40 | + ((uint64_t) buffer[*index + 3]) << 32 | + ((uint64_t) buffer[*index + 4]) << 24 | + ((uint64_t) buffer[*index + 5]) << 16 | + ((uint64_t) buffer[*index + 6]) << 8 | + ((uint64_t) buffer[*index + 7]); + *index += 8; + return res; +} + +float buffer_get_float16(const uint8_t *buffer, float scale, int32_t *index) { + return (float)buffer_get_int16(buffer, index) / scale; +} + +float buffer_get_float32(const uint8_t *buffer, float scale, int32_t *index) { + return (float)buffer_get_int32(buffer, index) / scale; +} + +double buffer_get_double64(const uint8_t *buffer, double scale, int32_t *index) { + return (double)buffer_get_int64(buffer, index) / scale; +} + +float buffer_get_float32_auto(const uint8_t *buffer, int32_t *index) { + uint32_t res = buffer_get_uint32(buffer, index); + + int e = (res >> 23) & 0xFF; + uint32_t sig_i = res & 0x7FFFFF; + bool neg = res & (1 << 31); + + float sig = 0.0; + if (e != 0 || sig_i != 0) { + sig = (float)sig_i / (8388608.0 * 2.0) + 0.5; + e -= 126; + } + + if (neg) { + sig = -sig; + } + + return ldexpf(sig, e); +} diff --git a/Node/External/Src/crc.c b/Node/External/Src/crc.c new file mode 100644 index 0000000..9768b84 --- /dev/null +++ b/Node/External/Src/crc.c @@ -0,0 +1,64 @@ +/* + Copyright 2012-2014 Benjamin Vedder benjamin@vedder.se + + This program is free software: you can redistribute it and/or modify + it under the terms of the GNU General Public License as published by + the Free Software Foundation, either version 3 of the License, or + (at your option) any later version. + + This program is distributed in the hope that it will be useful, + but WITHOUT ANY WARRANTY; without even the implied warranty of + MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + GNU General Public License for more details. + + You should have received a copy of the GNU General Public License + along with this program. If not, see . + */ + +/* + * crc.c + * + * Created on: 26 feb 2012 + * Author: benjamin + */ +#include "crc.h" + +// CRC Table +const unsigned short crc16_tab[] = { 0x0000, 0x1021, 0x2042, 0x3063, 0x4084, + 0x50a5, 0x60c6, 0x70e7, 0x8108, 0x9129, 0xa14a, 0xb16b, 0xc18c, 0xd1ad, + 0xe1ce, 0xf1ef, 0x1231, 0x0210, 0x3273, 0x2252, 0x52b5, 0x4294, 0x72f7, + 0x62d6, 0x9339, 0x8318, 0xb37b, 0xa35a, 0xd3bd, 0xc39c, 0xf3ff, 0xe3de, + 0x2462, 0x3443, 0x0420, 0x1401, 0x64e6, 0x74c7, 0x44a4, 0x5485, 0xa56a, + 0xb54b, 0x8528, 0x9509, 0xe5ee, 0xf5cf, 0xc5ac, 0xd58d, 0x3653, 0x2672, + 0x1611, 0x0630, 0x76d7, 0x66f6, 0x5695, 0x46b4, 0xb75b, 0xa77a, 0x9719, + 0x8738, 0xf7df, 0xe7fe, 0xd79d, 0xc7bc, 0x48c4, 0x58e5, 0x6886, 0x78a7, + 0x0840, 0x1861, 0x2802, 0x3823, 0xc9cc, 0xd9ed, 0xe98e, 0xf9af, 0x8948, + 0x9969, 0xa90a, 0xb92b, 0x5af5, 0x4ad4, 0x7ab7, 0x6a96, 0x1a71, 0x0a50, + 0x3a33, 0x2a12, 0xdbfd, 0xcbdc, 0xfbbf, 0xeb9e, 0x9b79, 0x8b58, 0xbb3b, + 0xab1a, 0x6ca6, 0x7c87, 0x4ce4, 0x5cc5, 0x2c22, 0x3c03, 0x0c60, 0x1c41, + 0xedae, 0xfd8f, 0xcdec, 0xddcd, 0xad2a, 0xbd0b, 0x8d68, 0x9d49, 0x7e97, + 0x6eb6, 0x5ed5, 0x4ef4, 0x3e13, 0x2e32, 0x1e51, 0x0e70, 0xff9f, 0xefbe, + 0xdfdd, 0xcffc, 0xbf1b, 0xaf3a, 0x9f59, 0x8f78, 0x9188, 0x81a9, 0xb1ca, + 0xa1eb, 0xd10c, 0xc12d, 0xf14e, 0xe16f, 0x1080, 0x00a1, 0x30c2, 0x20e3, + 0x5004, 0x4025, 0x7046, 0x6067, 0x83b9, 0x9398, 0xa3fb, 0xb3da, 0xc33d, + 0xd31c, 0xe37f, 0xf35e, 0x02b1, 0x1290, 0x22f3, 0x32d2, 0x4235, 0x5214, + 0x6277, 0x7256, 0xb5ea, 0xa5cb, 0x95a8, 0x8589, 0xf56e, 0xe54f, 0xd52c, + 0xc50d, 0x34e2, 0x24c3, 0x14a0, 0x0481, 0x7466, 0x6447, 0x5424, 0x4405, + 0xa7db, 0xb7fa, 0x8799, 0x97b8, 0xe75f, 0xf77e, 0xc71d, 0xd73c, 0x26d3, + 0x36f2, 0x0691, 0x16b0, 0x6657, 0x7676, 0x4615, 0x5634, 0xd94c, 0xc96d, + 0xf90e, 0xe92f, 0x99c8, 0x89e9, 0xb98a, 0xa9ab, 0x5844, 0x4865, 0x7806, + 0x6827, 0x18c0, 0x08e1, 0x3882, 0x28a3, 0xcb7d, 0xdb5c, 0xeb3f, 0xfb1e, + 0x8bf9, 0x9bd8, 0xabbb, 0xbb9a, 0x4a75, 0x5a54, 0x6a37, 0x7a16, 0x0af1, + 0x1ad0, 0x2ab3, 0x3a92, 0xfd2e, 0xed0f, 0xdd6c, 0xcd4d, 0xbdaa, 0xad8b, + 0x9de8, 0x8dc9, 0x7c26, 0x6c07, 0x5c64, 0x4c45, 0x3ca2, 0x2c83, 0x1ce0, + 0x0cc1, 0xef1f, 0xff3e, 0xcf5d, 0xdf7c, 0xaf9b, 0xbfba, 0x8fd9, 0x9ff8, + 0x6e17, 0x7e36, 0x4e55, 0x5e74, 0x2e93, 0x3eb2, 0x0ed1, 0x1ef0 }; + +unsigned short crc16(unsigned char *buf, unsigned int len) { + unsigned int i; + unsigned short cksum = 0; + for (i = 0; i < len; i++) { + cksum = crc16_tab[(((cksum >> 8) ^ *buf++) & 0xFF)] ^ (cksum << 8); + } + return cksum; +} diff --git a/Node/External/Src/packet.c b/Node/External/Src/packet.c new file mode 100644 index 0000000..f4292db --- /dev/null +++ b/Node/External/Src/packet.c @@ -0,0 +1,169 @@ +/* + Copyright 2012-2014 Benjamin Vedder benjamin@vedder.se + + This program is free software: you can redistribute it and/or modify + it under the terms of the GNU General Public License as published by + the Free Software Foundation, either version 3 of the License, or + (at your option) any later version. + + This program is distributed in the hope that it will be useful, + but WITHOUT ANY WARRANTY; without even the implied warranty of + MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + GNU General Public License for more details. + + You should have received a copy of the GNU General Public License + along with this program. If not, see . + */ + +/* + * packet.c + * + * Created on: 21 mar 2013 + * Author: benjamin + */ + +#include +#include "packet.h" +#include "crc.h" + +typedef struct { + volatile unsigned char rx_state; + volatile unsigned char rx_timeout; + void(*send_func)(unsigned char *data, unsigned int len); + void(*process_func)(unsigned char *data, unsigned int len); + unsigned int payload_length; + unsigned char rx_buffer[PACKET_MAX_PL_LEN]; + unsigned char tx_buffer[PACKET_MAX_PL_LEN + 6]; + unsigned int rx_data_ptr; + unsigned char crc_low; + unsigned char crc_high; +} PACKET_STATE_t; + +static PACKET_STATE_t handler_states[PACKET_HANDLERS]; + +void packet_init(void (*s_func)(unsigned char *data, unsigned int len), + void (*p_func)(unsigned char *data, unsigned int len), int handler_num) { + handler_states[handler_num].send_func = s_func; + handler_states[handler_num].process_func = p_func; +} + +void packet_send_packet(unsigned char *data, unsigned int len, int handler_num) { + if (len > PACKET_MAX_PL_LEN) { + return; + } + + int b_ind = 0; + + if (len <= 256) { + handler_states[handler_num].tx_buffer[b_ind++] = 2; + handler_states[handler_num].tx_buffer[b_ind++] = len; + } else { + handler_states[handler_num].tx_buffer[b_ind++] = 3; + handler_states[handler_num].tx_buffer[b_ind++] = len >> 8; + handler_states[handler_num].tx_buffer[b_ind++] = len & 0xFF; + } + + memcpy(handler_states[handler_num].tx_buffer + b_ind, data, len); + b_ind += len; + + unsigned short crc = crc16(data, len); + handler_states[handler_num].tx_buffer[b_ind++] = (uint8_t)(crc >> 8); + handler_states[handler_num].tx_buffer[b_ind++] = (uint8_t)(crc & 0xFF); + handler_states[handler_num].tx_buffer[b_ind++] = 3; + + if (handler_states[handler_num].send_func) { + handler_states[handler_num].send_func(handler_states[handler_num].tx_buffer, b_ind); + } +} + +/** + * Call this function every millisecond. + */ +void packet_timerfunc(void) { + int i = 0; + for (i = 0;i < PACKET_HANDLERS;i++) { + if (handler_states[i].rx_timeout) { + handler_states[i].rx_timeout--; + } else { + handler_states[i].rx_state = 0; + } + } +} + +void packet_process_byte(uint8_t rx_data, int handler_num) { + switch (handler_states[handler_num].rx_state) { + case 0: + if (rx_data == 2) { + // 1 byte PL len + handler_states[handler_num].rx_state += 2; + handler_states[handler_num].rx_timeout = PACKET_RX_TIMEOUT; + handler_states[handler_num].rx_data_ptr = 0; + handler_states[handler_num].payload_length = 0; + } else if (rx_data == 3) { + // 2 byte PL len + handler_states[handler_num].rx_state++; + handler_states[handler_num].rx_timeout = PACKET_RX_TIMEOUT; + handler_states[handler_num].rx_data_ptr = 0; + handler_states[handler_num].payload_length = 0; + } else { + handler_states[handler_num].rx_state = 0; + } + break; + + case 1: + handler_states[handler_num].payload_length = (unsigned int)rx_data << 8; + handler_states[handler_num].rx_state++; + handler_states[handler_num].rx_timeout = PACKET_RX_TIMEOUT; + break; + + case 2: + handler_states[handler_num].payload_length |= (unsigned int)rx_data; + if (handler_states[handler_num].payload_length > 0 && + handler_states[handler_num].payload_length <= PACKET_MAX_PL_LEN) { + handler_states[handler_num].rx_state++; + handler_states[handler_num].rx_timeout = PACKET_RX_TIMEOUT; + } else { + handler_states[handler_num].rx_state = 0; + } + break; + + case 3: + handler_states[handler_num].rx_buffer[handler_states[handler_num].rx_data_ptr++] = rx_data; + if (handler_states[handler_num].rx_data_ptr == handler_states[handler_num].payload_length) { + handler_states[handler_num].rx_state++; + } + handler_states[handler_num].rx_timeout = PACKET_RX_TIMEOUT; + break; + + case 4: + handler_states[handler_num].crc_high = rx_data; + handler_states[handler_num].rx_state++; + handler_states[handler_num].rx_timeout = PACKET_RX_TIMEOUT; + break; + + case 5: + handler_states[handler_num].crc_low = rx_data; + handler_states[handler_num].rx_state++; + handler_states[handler_num].rx_timeout = PACKET_RX_TIMEOUT; + break; + + case 6: + if (rx_data == 3) { + if (crc16(handler_states[handler_num].rx_buffer, handler_states[handler_num].payload_length) + == ((unsigned short)handler_states[handler_num].crc_high << 8 + | (unsigned short)handler_states[handler_num].crc_low)) { + // Packet received! + if (handler_states[handler_num].process_func) { + handler_states[handler_num].process_func(handler_states[handler_num].rx_buffer, + handler_states[handler_num].payload_length); + } + } + } + handler_states[handler_num].rx_state = 0; + break; + + default: + handler_states[handler_num].rx_state = 0; + break; + } +} diff --git a/Node/build.sh b/Node/build.sh index 0d1ebe9..49698ae 100755 --- a/Node/build.sh +++ b/Node/build.sh @@ -1,9 +1,9 @@ #!/bin/bash -if [ ! -d "build" ]; then +if [ ! -d "Build" ]; then echo "Making build directory..." - mkdir build + mkdir Build fi -pushd build +pushd Build cmake -DCMAKE_TOOLCHAIN_FILE=../arm-none-eabi-gcc.cmake -DCMAKE_BUILD_TYPE=Debug .. make mv compile_commands.json .. From 0849130d9984134b2a72ff6cab07dc22e6d62a59 Mon Sep 17 00:00:00 2001 From: Daniel Burke Date: Thu, 26 May 2022 20:47:02 -0230 Subject: [PATCH 2/4] Create script to help with creating source files This script will create a pair of .h/.c files with the name given as an argument to the script. It will also add them to the gitignore. --- Node/create_new_c_file.sh | 13 +++++++++++++ 1 file changed, 13 insertions(+) create mode 100755 Node/create_new_c_file.sh diff --git a/Node/create_new_c_file.sh b/Node/create_new_c_file.sh new file mode 100755 index 0000000..bc30265 --- /dev/null +++ b/Node/create_new_c_file.sh @@ -0,0 +1,13 @@ +#!/bin/bash +file_name=$1 + +touch Core/Inc/$1.h +touch Core/Src/$1.c + +echo "// Placeholder" >> Core/Inc/$1.h +echo "// Placeholder" >> Core/Src/$1.c + +echo "!Core/Inc/$1.h" >> .gitignore +echo "!Core/Src/$1.c" >> .gitignore + +git add -f Core/Inc/$1.h Core/Src/$1.c From 040075090a998564563be8e1f50781dfcfa91033 Mon Sep 17 00:00:00 2001 From: Daniel Burke Date: Thu, 26 May 2022 22:27:20 -0230 Subject: [PATCH 3/4] Make packet functionality compatible with multiple motors --- Node/Core/Src/main.c | 7 +++---- Node/External/Inc/packet.h | 6 ++++-- Node/External/Src/bldc_interface_uart.c | 6 +++--- Node/External/Src/packet.c | 8 ++++---- 4 files changed, 14 insertions(+), 13 deletions(-) diff --git a/Node/Core/Src/main.c b/Node/Core/Src/main.c index 971c1fe..0feb4cf 100644 --- a/Node/Core/Src/main.c +++ b/Node/Core/Src/main.c @@ -118,9 +118,9 @@ int main(void) MX_SPI1_Init(); /* USER CODE BEGIN 2 */ - bldc_interface_init(&motor1, send_packet_motor_1); - bldc_interface_init(&motor2, send_packet_motor_2); - bldc_interface_init(&motor3, send_packet_motor_3); + bldc_interface_uart_init(&motor1, send_packet_motor_1); + bldc_interface_uart_init(&motor2, send_packet_motor_2); + bldc_interface_uart_init(&motor3, send_packet_motor_3); uint8_t motor1_data; uint8_t motor2_data; @@ -138,7 +138,6 @@ int main(void) HAL_UART_Receive(&huart2, &motor2_data, 1, 0); HAL_UART_Receive(&huart3, &motor3_data, 1, 0); - bldc_interface_uart_process_byte(&motor1, motor1_data); bldc_interface_uart_process_byte(&motor2, motor2_data); bldc_interface_uart_process_byte(&motor3, motor3_data); diff --git a/Node/External/Inc/packet.h b/Node/External/Inc/packet.h index 489650b..9b4c286 100644 --- a/Node/External/Inc/packet.h +++ b/Node/External/Inc/packet.h @@ -27,6 +27,8 @@ #include +#include "bldc_interface.h" + // Settings #define PACKET_RX_TIMEOUT 2 #define PACKET_HANDLERS 1 @@ -34,8 +36,8 @@ // Functions void packet_init(void (*s_func)(unsigned char *data, unsigned int len), - void (*p_func)(unsigned char *data, unsigned int len), int handler_num); -void packet_process_byte(uint8_t rx_data, int handler_num); + void (*p_func)(BldcInterface* interface, unsigned char *data, unsigned int len), int handler_num); +void packet_process_byte(BldcInterface* interface, uint8_t rx_data, int handler_num); void packet_timerfunc(void); void packet_send_packet(unsigned char *data, unsigned int len, int handler_num); diff --git a/Node/External/Src/bldc_interface_uart.c b/Node/External/Src/bldc_interface_uart.c index d459753..39fdc5e 100644 --- a/Node/External/Src/bldc_interface_uart.c +++ b/Node/External/Src/bldc_interface_uart.c @@ -30,7 +30,7 @@ // Private functions static void process_packet(BldcInterface* interface, unsigned char *data, unsigned int len); -static void send_packet_bldc_interface(BldcInterface* interface, unsigned char *data, unsigned int len); +static void send_packet_bldc_interface(unsigned char *data, unsigned int len); /** * Initialize the UART BLDC interface and provide a function to be used for @@ -55,7 +55,7 @@ void bldc_interface_uart_init(BldcInterface* interface, void(*func)(unsigned cha * The byte received on the UART to process. */ void bldc_interface_uart_process_byte(BldcInterface* interface, unsigned char b) { - packet_process_byte(b, PACKET_HANDLER); + packet_process_byte(interface, b, PACKET_HANDLER); } /** @@ -88,7 +88,7 @@ static void process_packet(BldcInterface* interface, unsigned char *data, unsign * @param len * Data array length */ -static void send_packet_bldc_interface(BldcInterface* interface, unsigned char *data, unsigned int len) { +static void send_packet_bldc_interface(unsigned char *data, unsigned int len) { // Pass the packet to the packet handler to add checksum, length, start and stop bytes. packet_send_packet(data, len, PACKET_HANDLER); } diff --git a/Node/External/Src/packet.c b/Node/External/Src/packet.c index f4292db..07aa1f4 100644 --- a/Node/External/Src/packet.c +++ b/Node/External/Src/packet.c @@ -30,7 +30,7 @@ typedef struct { volatile unsigned char rx_state; volatile unsigned char rx_timeout; void(*send_func)(unsigned char *data, unsigned int len); - void(*process_func)(unsigned char *data, unsigned int len); + void(*process_func)(BldcInterface* interface, unsigned char *data, unsigned int len); unsigned int payload_length; unsigned char rx_buffer[PACKET_MAX_PL_LEN]; unsigned char tx_buffer[PACKET_MAX_PL_LEN + 6]; @@ -42,7 +42,7 @@ typedef struct { static PACKET_STATE_t handler_states[PACKET_HANDLERS]; void packet_init(void (*s_func)(unsigned char *data, unsigned int len), - void (*p_func)(unsigned char *data, unsigned int len), int handler_num) { + void (*p_func)(BldcInterface* interface, unsigned char *data, unsigned int len), int handler_num) { handler_states[handler_num].send_func = s_func; handler_states[handler_num].process_func = p_func; } @@ -90,7 +90,7 @@ void packet_timerfunc(void) { } } -void packet_process_byte(uint8_t rx_data, int handler_num) { +void packet_process_byte(BldcInterface* interface, uint8_t rx_data, int handler_num) { switch (handler_states[handler_num].rx_state) { case 0: if (rx_data == 2) { @@ -154,7 +154,7 @@ void packet_process_byte(uint8_t rx_data, int handler_num) { | (unsigned short)handler_states[handler_num].crc_low)) { // Packet received! if (handler_states[handler_num].process_func) { - handler_states[handler_num].process_func(handler_states[handler_num].rx_buffer, + handler_states[handler_num].process_func(interface, handler_states[handler_num].rx_buffer, handler_states[handler_num].payload_length); } } From bdaf8628032756dfc9d9e708cb159850797a9938 Mon Sep 17 00:00:00 2001 From: Daniel Burke Date: Thu, 26 May 2022 22:38:03 -0230 Subject: [PATCH 4/4] Fix helper script to actually use file_name variable --- Node/create_new_c_file.sh | 14 +++++++------- 1 file changed, 7 insertions(+), 7 deletions(-) diff --git a/Node/create_new_c_file.sh b/Node/create_new_c_file.sh index bc30265..e61888b 100755 --- a/Node/create_new_c_file.sh +++ b/Node/create_new_c_file.sh @@ -1,13 +1,13 @@ #!/bin/bash file_name=$1 -touch Core/Inc/$1.h -touch Core/Src/$1.c +touch Core/Inc/$file_name.h +touch Core/Src/$file_name.c -echo "// Placeholder" >> Core/Inc/$1.h -echo "// Placeholder" >> Core/Src/$1.c +echo "// Placeholder" >> Core/Inc/$file_name.h +echo "// Placeholder" >> Core/Src/$file_name.c -echo "!Core/Inc/$1.h" >> .gitignore -echo "!Core/Src/$1.c" >> .gitignore +echo "!Core/Inc/$file_name.h" >> .gitignore +echo "!Core/Src/$file_name.c" >> .gitignore -git add -f Core/Inc/$1.h Core/Src/$1.c +git add -f Core/Inc/$file_name.h Core/Src/$file_name.c