From f53cbfce75eb15f6bd86005c20203099ce0c6d68 Mon Sep 17 00:00:00 2001 From: Benjamin Vedder Date: Tue, 5 Jul 2022 20:15:31 +0200 Subject: [PATCH] Added more C lib support --- conf_general.h | 2 +- lispBM/c_libs/rules.mk | 1 + lispBM/c_libs/vesc_c_if.h | 180 ++++++++++++++++++++++++++++++++++++++ lispBM/lispif_c_lib.c | 93 ++++++++++++++++++++ 4 files changed, 275 insertions(+), 1 deletion(-) diff --git a/conf_general.h b/conf_general.h index 81f96e4c4..e5bf547ee 100755 --- a/conf_general.h +++ b/conf_general.h @@ -24,7 +24,7 @@ #define FW_VERSION_MAJOR 6 #define FW_VERSION_MINOR 00 // Set to 0 for building a release and iterate during beta test builds -#define FW_TEST_VERSION_NUMBER 52 +#define FW_TEST_VERSION_NUMBER 53 #include "datatypes.h" diff --git a/lispBM/c_libs/rules.mk b/lispBM/c_libs/rules.mk index 3cd38cc3d..548d63680 100644 --- a/lispBM/c_libs/rules.mk +++ b/lispBM/c_libs/rules.mk @@ -29,6 +29,7 @@ CFLAGS += -fomit-frame-pointer -falign-functions=16 -mthumb CFLAGS += -fsingle-precision-constant -Wdouble-promotion CFLAGS += -mfloat-abi=hard -mfpu=fpv4-sp-d16 -mcpu=cortex-m4 CFLAGS += -fdata-sections -ffunction-sections +CFLAGS += -DIS_VESC_LIB ifeq ($(USE_STLIB),yes) CFLAGS += -DUSE_STLIB -I$(STLIB_PATH)/inc diff --git a/lispBM/c_libs/vesc_c_if.h b/lispBM/c_libs/vesc_c_if.h index 7e1a619b4..a679bca37 100755 --- a/lispBM/c_libs/vesc_c_if.h +++ b/lispBM/c_libs/vesc_c_if.h @@ -26,6 +26,94 @@ typedef bool (*load_extension_fptr)(char*,extension_fptr); typedef void* lib_thread; +#ifdef IS_VESC_LIB +typedef uint32_t systime_t; + +typedef struct { + int id; + systime_t rx_time; + float rpm; + float current; + float duty; +} can_status_msg; + +typedef struct { + int id; + systime_t rx_time; + float amp_hours; + float amp_hours_charged; +} can_status_msg_2; + +typedef struct { + int id; + systime_t rx_time; + float watt_hours; + float watt_hours_charged; +} can_status_msg_3; + +typedef struct { + int id; + systime_t rx_time; + float temp_fet; + float temp_motor; + float current_in; + float pid_pos_now; +} can_status_msg_4; + +typedef struct { + int id; + systime_t rx_time; + float v_in; + int32_t tacho_value; +} can_status_msg_5; + +typedef struct { + int id; + systime_t rx_time; + float adc_1; + float adc_2; + float adc_3; + float ppm; +} can_status_msg_6; + +typedef enum { + HW_TYPE_VESC = 0, + HW_TYPE_VESC_BMS, + HW_TYPE_CUSTOM_MODULE +} HW_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, + FAULT_CODE_GATE_DRIVER_OVER_VOLTAGE, + FAULT_CODE_GATE_DRIVER_UNDER_VOLTAGE, + FAULT_CODE_MCU_UNDER_VOLTAGE, + FAULT_CODE_BOOTING_FROM_WATCHDOG_RESET, + FAULT_CODE_ENCODER_SPI, + FAULT_CODE_ENCODER_SINCOS_BELOW_MIN_AMPLITUDE, + FAULT_CODE_ENCODER_SINCOS_ABOVE_MAX_AMPLITUDE, + FAULT_CODE_FLASH_CORRUPTION, + FAULT_CODE_HIGH_OFFSET_CURRENT_SENSOR_1, + FAULT_CODE_HIGH_OFFSET_CURRENT_SENSOR_2, + FAULT_CODE_HIGH_OFFSET_CURRENT_SENSOR_3, + FAULT_CODE_UNBALANCED_CURRENTS, + FAULT_CODE_BRK, + FAULT_CODE_RESOLVER_LOT, + FAULT_CODE_RESOLVER_DOS, + FAULT_CODE_RESOLVER_LOS, + FAULT_CODE_FLASH_CORRUPTION_APP_CFG, + FAULT_CODE_FLASH_CORRUPTION_MC_CFG, + FAULT_CODE_ENCODER_NO_MAGNET, + FAULT_CODE_ENCODER_MAGNET_TOO_STRONG, + FAULT_CODE_PHASE_FILTER, +} mc_fault_code; +#endif + typedef enum { VESC_PIN_COMM_RX = 0, VESC_PIN_COMM_TX, @@ -49,6 +137,10 @@ typedef enum { VESC_PIN_MODE_ANALOG, } VESC_PIN_MODE; +/* + * Function pointer struct. Always add new function pointers to the end in order to not + * break compatibility with old binaries. + */ typedef struct { // LBM If load_extension_fptr lbm_add_extension; @@ -92,6 +184,94 @@ typedef struct { void (*can_set_eid_cb)(bool (*p_func)(uint32_t id, uint8_t *data, uint8_t len)); void (*can_transmit_sid)(uint32_t id, const uint8_t *data, uint8_t len); void (*can_transmit_eid)(uint32_t id, const uint8_t *data, uint8_t len); + void (*can_send_buffer)(uint8_t controller_id, uint8_t *data, unsigned int len, uint8_t send); + void (*can_set_duty)(uint8_t controller_id, float duty); + void (*can_set_current)(uint8_t controller_id, float current); + void (*can_set_current_off_delay)(uint8_t controller_id, float current, float off_delay); + void (*can_set_current_brake)(uint8_t controller_id, float current); + void (*can_set_rpm)(uint8_t controller_id, float rpm); + void (*can_set_pos)(uint8_t controller_id, float pos); + void (*can_set_current_rel)(uint8_t controller_id, float current_rel); + void (*can_set_current_rel_off_delay)(uint8_t controller_id, float current_rel, float off_delay); + void (*can_set_current_brake_rel)(uint8_t controller_id, float current_rel); + bool (*can_ping)(uint8_t controller_id, HW_TYPE *hw_type); + can_status_msg* (*can_get_status_msg_index)(int index); + can_status_msg* (*can_get_status_msg_id)(int id); + can_status_msg_2* (*can_get_status_msg_2_index)(int index); + can_status_msg_2* (*can_get_status_msg_2_id)(int id); + can_status_msg_3* (*can_get_status_msg_3_index)(int index); + can_status_msg_3* (*can_get_status_msg_3_id)(int id); + can_status_msg_4* (*can_get_status_msg_4_index)(int index); + can_status_msg_4* (*can_get_status_msg_4_id)(int id); + can_status_msg_5* (*can_get_status_msg_5_index)(int index); + can_status_msg_5* (*can_get_status_msg_5_id)(int id); + can_status_msg_6* (*can_get_status_msg_6_index)(int index); + can_status_msg_6* (*can_get_status_msg_6_id)(int id); + + // Motor Control + int (*mc_motor_now)(void); + void (*mc_select_motor_thread)(int motor); + int (*mc_get_motor_thread)(void); + bool (*mc_dccal_done)(void); + void (*mc_set_pwm_callback)(void (*p_func)(void)); + mc_fault_code (*mc_get_fault)(void); + const char* (*mc_fault_to_string)(mc_fault_code fault); + void (*mc_set_duty)(float dutyCycle); + void (*mc_set_duty_noramp)(float dutyCycle); + void (*mc_set_pid_speed)(float rpm); + void (*mc_set_pid_pos)(float pos); + void (*mc_set_current)(float current); + void (*mc_set_brake_current)(float current); + void (*mc_set_current_rel)(float val); + void (*mc_set_brake_current_rel)(float val); + void (*mc_set_handbrake)(float current); + void (*mc_set_handbrake_rel)(float val); + int (*mc_set_tachometer_value)(int steps); + void (*mc_release_motor)(void); + bool (*mc_wait_for_motor_release)(float timeout); + float (*mc_get_duty_cycle_now)(void); + float (*mc_get_sampling_frequency_now)(void); + float (*mc_get_rpm)(void); + float (*mc_get_amp_hours)(bool reset); + float (*mc_get_amp_hours_charged)(bool reset); + float (*mc_get_watt_hours)(bool reset); + float (*mc_get_watt_hours_charged)(bool reset); + float (*mc_get_tot_current)(void); + float (*mc_get_tot_current_filtered)(void); + float (*mc_get_tot_current_directional)(void); + float (*mc_get_tot_current_directional_filtered)(void); + float (*mc_get_tot_current_in)(void); + float (*mc_get_tot_current_in_filtered)(void); + float (*mc_get_input_voltage_filtered)(void); + int (*mc_get_tachometer_value)(bool reset); + int (*mc_get_tachometer_abs_value)(bool reset); + float (*mc_get_pid_pos_set)(void); + float (*mc_get_pid_pos_now)(void); + void (*mc_update_pid_pos_offset)(float angle_now, bool store); + float (*mc_temp_fet_filtered)(void); + float (*mc_temp_motor_filtered)(void); + float (*mc_get_battery_level)(float *wh_left); + float (*mc_get_speed)(void); + float (*mc_get_distance)(void); + float (*mc_get_distance_abs)(void); + uint64_t (*mc_get_odometer)(void); + void (*mc_set_odometer)(uint64_t new_odometer_meters); + void (*mc_set_current_off_delay)(float delay_sec); + float (*mc_stat_speed_avg)(void); + float (*mc_stat_speed_max)(void); + float (*mc_stat_power_avg)(void); + float (*mc_stat_power_max)(void); + float (*mc_stat_current_avg)(void); + float (*mc_stat_current_max)(void); + float (*mc_stat_temp_mosfet_avg)(void); + float (*mc_stat_temp_mosfet_max)(void); + float (*mc_stat_temp_motor_avg)(void); + float (*mc_stat_temp_motor_max)(void); + float (*mc_stat_count_time)(void); + void (*mc_stat_reset)(void); + + // More (added here to not break binary compatibility) + float (*system_time)(void); } vesc_c_if; typedef struct { diff --git a/lispBM/lispif_c_lib.c b/lispBM/lispif_c_lib.c index c8d1feccf..036f41283 100644 --- a/lispBM/lispif_c_lib.c +++ b/lispBM/lispif_c_lib.c @@ -25,6 +25,7 @@ #include "comm_can.h" #include "lispif.h" #include "lispbm.h" +#include "utils.h" #include "c_libs/vesc_c_if.h" typedef struct { @@ -52,6 +53,10 @@ static void lib_sleep_us(uint32_t us) { chThdSleepMicroseconds(us); } +static float lib_system_time(void) { + return UTILS_AGE_S(0); +} + static void* lib_malloc(size_t size) { lbm_uint alloc_size; if (size % sizeof(lbm_uint) == 0) { @@ -328,6 +333,94 @@ lbm_value ext_load_native_lib(lbm_value *args, lbm_uint argn) { cif.cif.can_set_eid_cb = comm_can_set_eid_rx_callback; cif.cif.can_transmit_sid = comm_can_transmit_sid; cif.cif.can_transmit_eid = comm_can_transmit_eid; + cif.cif.can_send_buffer = comm_can_send_buffer; + cif.cif.can_set_duty = comm_can_set_duty; + cif.cif.can_set_current = comm_can_set_current; + cif.cif.can_set_current_off_delay = comm_can_set_current_off_delay; + cif.cif.can_set_current_brake = comm_can_set_current_brake; + cif.cif.can_set_rpm = comm_can_set_rpm; + cif.cif.can_set_pos = comm_can_set_pos; + cif.cif.can_set_current_rel = comm_can_set_current_rel; + cif.cif.can_set_current_rel_off_delay = comm_can_set_current_rel_off_delay; + cif.cif.can_set_current_brake_rel = comm_can_set_current_brake_rel; + cif.cif.can_ping = comm_can_ping; + cif.cif.can_get_status_msg_index = comm_can_get_status_msg_index; + cif.cif.can_get_status_msg_id = comm_can_get_status_msg_id; + cif.cif.can_get_status_msg_2_index = comm_can_get_status_msg_2_index; + cif.cif.can_get_status_msg_2_id = comm_can_get_status_msg_2_id; + cif.cif.can_get_status_msg_3_index = comm_can_get_status_msg_3_index; + cif.cif.can_get_status_msg_3_id = comm_can_get_status_msg_3_id; + cif.cif.can_get_status_msg_4_index = comm_can_get_status_msg_4_index; + cif.cif.can_get_status_msg_4_id = comm_can_get_status_msg_4_id; + cif.cif.can_get_status_msg_5_index = comm_can_get_status_msg_5_index; + cif.cif.can_get_status_msg_5_id = comm_can_get_status_msg_5_id; + cif.cif.can_get_status_msg_6_index = comm_can_get_status_msg_6_index; + cif.cif.can_get_status_msg_6_id = comm_can_get_status_msg_6_id; + + // Motor Control + cif.cif.mc_motor_now = mc_interface_motor_now; + cif.cif.mc_select_motor_thread = mc_interface_select_motor_thread; + cif.cif.mc_get_motor_thread = mc_interface_get_motor_thread; + cif.cif.mc_dccal_done = mc_interface_dccal_done; + cif.cif.mc_set_pwm_callback = mc_interface_set_pwm_callback; + cif.cif.mc_get_fault = mc_interface_get_fault; + cif.cif.mc_fault_to_string = mc_interface_fault_to_string; + cif.cif.mc_set_duty = mc_interface_set_duty; + cif.cif.mc_set_duty_noramp = mc_interface_set_duty_noramp; + cif.cif.mc_set_pid_speed = mc_interface_set_pid_speed; + cif.cif.mc_set_pid_pos = mc_interface_set_pid_pos; + cif.cif.mc_set_current = mc_interface_set_current; + cif.cif.mc_set_brake_current = mc_interface_set_brake_current; + cif.cif.mc_set_current_rel = mc_interface_set_current_rel; + cif.cif.mc_set_brake_current_rel = mc_interface_set_brake_current_rel; + cif.cif.mc_set_handbrake = mc_interface_set_handbrake; + cif.cif.mc_set_handbrake_rel = mc_interface_set_handbrake_rel; + cif.cif.mc_set_tachometer_value = mc_interface_set_tachometer_value; + cif.cif.mc_release_motor = mc_interface_release_motor; + cif.cif.mc_wait_for_motor_release = mc_interface_wait_for_motor_release; + cif.cif.mc_get_duty_cycle_now = mc_interface_get_duty_cycle_now; + cif.cif.mc_get_sampling_frequency_now = mc_interface_get_sampling_frequency_now; + cif.cif.mc_get_rpm = mc_interface_get_rpm; + cif.cif.mc_get_amp_hours = mc_interface_get_amp_hours; + cif.cif.mc_get_amp_hours_charged = mc_interface_get_amp_hours_charged; + cif.cif.mc_get_watt_hours = mc_interface_get_watt_hours; + cif.cif.mc_get_watt_hours_charged = mc_interface_get_watt_hours_charged; + cif.cif.mc_get_tot_current = mc_interface_get_tot_current; + cif.cif.mc_get_tot_current_filtered = mc_interface_get_tot_current_filtered; + cif.cif.mc_get_tot_current_directional = mc_interface_get_tot_current_directional; + cif.cif.mc_get_tot_current_directional_filtered = mc_interface_get_tot_current_directional_filtered; + cif.cif.mc_get_tot_current_in = mc_interface_get_tot_current_in; + cif.cif.mc_get_tot_current_in_filtered = mc_interface_get_tot_current_in_filtered; + cif.cif.mc_get_input_voltage_filtered = mc_interface_get_input_voltage_filtered; + cif.cif.mc_get_tachometer_value = mc_interface_get_tachometer_value; + cif.cif.mc_get_tachometer_abs_value = mc_interface_get_tachometer_abs_value; + cif.cif.mc_get_pid_pos_set = mc_interface_get_pid_pos_set; + cif.cif.mc_get_pid_pos_now = mc_interface_get_pid_pos_now; + cif.cif.mc_update_pid_pos_offset = mc_interface_update_pid_pos_offset; + cif.cif.mc_temp_fet_filtered = mc_interface_temp_fet_filtered; + cif.cif.mc_temp_motor_filtered = mc_interface_temp_motor_filtered; + cif.cif.mc_get_battery_level = mc_interface_get_battery_level; + cif.cif.mc_get_speed = mc_interface_get_speed; + cif.cif.mc_get_distance = mc_interface_get_distance; + cif.cif.mc_get_distance_abs = mc_interface_get_distance_abs; + cif.cif.mc_get_odometer = mc_interface_get_odometer; + cif.cif.mc_set_odometer = mc_interface_set_odometer; + cif.cif.mc_set_current_off_delay = mc_interface_set_current_off_delay; + cif.cif.mc_stat_speed_avg = mc_interface_stat_speed_avg; + cif.cif.mc_stat_speed_max = mc_interface_stat_speed_max; + cif.cif.mc_stat_power_avg = mc_interface_stat_power_avg; + cif.cif.mc_stat_power_max = mc_interface_stat_power_max; + cif.cif.mc_stat_current_avg = mc_interface_stat_current_avg; + cif.cif.mc_stat_current_max = mc_interface_stat_current_max; + cif.cif.mc_stat_temp_mosfet_avg = mc_interface_stat_temp_mosfet_avg; + cif.cif.mc_stat_temp_mosfet_max = mc_interface_stat_temp_mosfet_max; + cif.cif.mc_stat_temp_motor_avg = mc_interface_stat_temp_motor_avg; + cif.cif.mc_stat_temp_motor_max = mc_interface_stat_temp_motor_max; + cif.cif.mc_stat_count_time = mc_interface_stat_count_time; + cif.cif.mc_stat_reset = mc_interface_stat_reset; + + // More + cif.cif.system_time = lib_system_time; lib_init_done = true; }