diff --git a/.gitignore b/.gitignore index 6ae650ef9..7e6d8bbe3 100644 --- a/.gitignore +++ b/.gitignore @@ -11,3 +11,4 @@ confxml.h *.cproject *.project *.settings +*.vscode \ No newline at end of file diff --git a/.gitmodules b/.gitmodules new file mode 100644 index 000000000..42c5c3631 --- /dev/null +++ b/.gitmodules @@ -0,0 +1,3 @@ +[submodule "c_libs/RVfplib"] + path = c_libs/RVfplib + url = git@github.com:pulp-platform/RVfplib.git diff --git a/README.md b/README.md index 1c26bc5de..37bf19a91 100644 --- a/README.md +++ b/README.md @@ -91,6 +91,11 @@ make clean make VESC_TOOL=/path/to/vesc_tool ``` +For building native libraries for VESC Express with c_libs you will need to specify the architecture: +```sh +make ARCH=esp32 +``` + ### Notes * Make sure that you build a VESC Package file (\*.vescpkg) in the main directory of your package (e.g. euc/euc.vescpkg) diff --git a/c_libs/link_esp32.ld b/c_libs/link_esp32.ld new file mode 100644 index 000000000..40e276c57 --- /dev/null +++ b/c_libs/link_esp32.ld @@ -0,0 +1,11 @@ +SECTIONS +{ + . = 0; + .program_ptr : ALIGN(4) { *(.program_ptr) } /* offset 0 */ + .init_fun : ALIGN(4) { *(.init_fun) } + + .text : ALIGN(4) { *(.text*) *(.rodata*) } + .data : ALIGN(4) { *(.data*) } + .bss : ALIGN(4) { *(.bss*) } + .got : ALIGN(4) { *(.got*) } +} \ No newline at end of file diff --git a/c_libs/rules.mk b/c_libs/rules.mk index 9f2016f93..90b73b5c8 100644 --- a/c_libs/rules.mk +++ b/c_libs/rules.mk @@ -1,3 +1,15 @@ +# ====================================================================== +# Unified Makefile for STM32F4 (default) and ESP32-C3 (minimal) +# ====================================================================== + +# ------------------------- Arch selection ------------------------------ +ARCH ?= stm32 +ifeq ($(ESP_PLATFORM),true) + ARCH := esp32 +endif + +# CROSS prefix per arch (override if you like) +ifeq ($(ARCH),stm32) CC = arm-none-eabi-gcc LD = arm-none-eabi-gcc @@ -67,4 +79,159 @@ $(TARGET): $(OBJECTS) clean: rm -f $(OBJECTS) $(TARGET).elf $(TARGET).list $(TARGET).lisp $(TARGET).bin +else ifeq ($(ARCH),esp32) +# ====================================================================== +# ESP32-C3 Native-lib rules (PIC blob for LispBM) +# - RISC-V (rv32imc / ilp32, software float) +# - Section GC + (optional) LTO +# - Generates: .elf, .bin (with header), .lisp, .list, .map +# ====================================================================== + +# ---- toolchain (override CROSS if needed) ----------------------------- +CROSS ?= riscv32-esp-elf- +CC := $(CROSS)gcc +AR := $(CROSS)ar +OBJDUMP := $(CROSS)objdump +OBJCOPY := $(CROSS)objcopy +NM := $(CROSS)nm +READELF := $(CROSS)readelf +PYTHON ?= python3 + +# ---- project defaults (override if you like) -------------------------- +TARGET ?= package_lib +SOURCES ?= + +# ---- paths ------------------------------------------------------------ +VESC_C_LIB_PATH ?= ../.. +LINKER_SCRIPT ?= $(VESC_C_LIB_PATH)/link_esp32.ld + +# ---- RVfplib (required) ----------------------------------------------- +# Expect RVfplib sources at $(VESC_C_LIB_PATH)RVfplib/ +# Always produce and link $(VESC_C_LIB_PATH)librvfp.a +RVFP_SRC_DIR := $(VESC_C_LIB_PATH)/RVfplib +RVFP_VARIANT ?= rvfp_nd_s +RVFP_LIB := $(RVFP_SRC_DIR)/build/lib/lib$(RVFP_VARIANT).a + +# ---- toggles ---------------------------------------------------------- +USE_LTO ?= no # Use LTO if your riscv32-esp-elf toolchain supports it +VERBOSE_LINK ?= yes # Emit a link map and (optionally) GC diagnostics + +# Exported entry symbols (kept even with GC/LTO) +ENTRY_SYMS ?= init + +# ---- common flags ----------------------------------------------------- +CFLAGS_COMMON = \ + -Os -Wall -Wextra -Wundef -std=gnu99 \ + -ffunction-sections -fdata-sections \ + -fPIC -fvisibility=hidden \ + -I$(VESC_C_LIB_PATH) -DIS_VESC_LIB \ + -DESP_PLATFORM=true \ + -march=rv32imc_zicsr_zifencei -mabi=ilp32 \ + -mno-save-restore \ + -mcmodel=medany \ + -msmall-data-limit=0 \ + -Wdouble-promotion -Wfloat-conversion \ + -Werror=implicit-function-declaration \ + -Werror=incompatible-pointer-types \ + -Werror=int-conversion \ + -Werror=return-type + +ifeq ($(USE_LTO),yes) + CFLAGS_COMMON += -flto + LDFLAGS_LTO = -flto +else + LDFLAGS_LTO = +endif + +# Dependency generation +CFLAGS_DEPS = -MMD + +CFLAGS += $(CFLAGS_COMMON) $(CFLAGS_DEPS) + +# ---- link flags ------------------------------------------------------- +LDFLAGS = \ + -static \ + -Wl,--gc-sections \ + -Wl,--no-relax \ + -Wl,--no-warn-rwx-segments \ + -T $(LINKER_SCRIPT) \ + -Wl,-Map=$(TARGET).map \ + $(LDFLAGS_LTO) + +# Keep required entry points +LDFLAGS += $(foreach s,$(ENTRY_SYMS),-Wl,--undefined=$(s)) + +# Helpful traces (you can comment these out if too chatty) +LDFLAGS += -Wl,--trace-symbol=__mulsf3 -Wl,--trace-symbol=__divsf3 +LDFLAGS += -Wl,--trace-symbol=__addsf3 -Wl,--trace-symbol=__subsf3 +LDFLAGS += -Wl,--trace-symbol=__eqsf2 -Wl,--trace-symbol=__nesf2 + +# Diagnostics +ifeq ($(VERBOSE_LINK),yes) + # Uncomment to see stripped sections: + # LDFLAGS += -Wl,--print-gc-sections +endif + +# ---- required soft-float libs (group avoids cyclic deps) -------------- +# Always link the locally-built librvfp.a first +LDGROUP := -Wl,--start-group $(RVFP_LIB) -Wl,--end-group + +# ---- derived names ---------------------------------------------------- +OBJECTS := $(SOURCES:.c=.o) +DEPS := $(SOURCES:.c=.d) +ELF := $(TARGET).elf +BIN := $(TARGET).bin +LISP := $(TARGET).lisp +LIST := $(TARGET).list +MAP := $(TARGET).map + +# ---- phony ------------------------------------------------------------ +.PHONY: default all clean nmcheck disasm rvfp + +default: $(TARGET) +all: default + +# ---- auto-build librvfp.a (no checks; assumes RVfplib exists) -------- +$(RVFP_LIB): + @echo ">> Building RVfplib ($(RVFP_VARIANT)) with $(CROSS)" + $(MAKE) -C "$(RVFP_SRC_DIR)" \ + RISCV_PREFIX=$(CROSS) ARCH=rv32imc ABI=ilp32 \ + CC=$(CC) AR=$(AR) NM=$(NM) OBJDUMP=$(OBJDUMP) + +rvfp: $(RVFP_LIB) + +# ---- build rules ------------------------------------------------------ +$(TARGET): $(ELF) + $(OBJCOPY) -O binary $< $@.temp + echo 'cafebabe' | xxd -r -p > $@.bin + cat $@.temp >> $@.bin + rm $@.temp + $(PYTHON) $(VESC_C_LIB_PATH)/conv.py -f $@.bin -n $(TARGET) > $(LISP) + +$(ELF): $(OBJECTS) $(RVFP_LIB) + $(CC) $(OBJECTS) $(CFLAGS) $(LDFLAGS) $(LDGROUP) -o $@ + $(OBJDUMP) -D $@ > $(LIST) + +%.o: %.c + $(CC) $(CFLAGS) -c $< -o $@ + +clean: + rm -f $(OBJECTS) $(DEPS) $(ELF) $(BIN) $(LISP) $(LIST) $(MAP) $(ADD_TO_CLEAN) + +# ---- helpers ---------------------------------------------------------- +nmcheck: $(ELF) + @echo "# soft-float helpers present (T=defined, U=undefined):" + $(NM) $(ELF) | egrep '__subsf3|__addsf3|__mulsf3|__divsf3|__eqsf2|__nesf2' || true + @echo + @echo "# relocations against helpers (should usually be none):" + $(READELF) -r $(ELF) | egrep '__subsf3|__addsf3|__mulsf3|__divsf3|__eqsf2|__nesf2' || true + +disasm: $(ELF) + $(OBJDUMP) -drwC --disassemble=buffer_get_float16 $(ELF) | sed -n '1,200p' + +# Include auto-generated dependencies +-include $(DEPS) +else + $(error Unknown ARCH=$(ARCH); use stm32 or esp32) +endif \ No newline at end of file diff --git a/c_libs/vesc_c_if.h b/c_libs/vesc_c_if.h index a6e778034..6b7c9b687 100644 --- a/c_libs/vesc_c_if.h +++ b/c_libs/vesc_c_if.h @@ -24,9 +24,56 @@ #include #include -#ifdef IS_VESC_LIB +#define NATIVE_LIB_MAGIC 0xCAFEBABE + +#if defined(ESP_PLATFORM) || defined(IS_VESC_LIB) typedef uint32_t systime_t; +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; + +typedef struct { + double lat; + double lon; + float height; + float speed; + float hdop; + int32_t ms_today; + int8_t yy; + int8_t mo; + int8_t dd; + systime_t last_update; +} gnss_data; +#endif +#ifdef IS_VESC_LIB typedef struct { int id; systime_t rx_time; @@ -80,37 +127,6 @@ typedef enum { 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; - typedef union { uint32_t as_u32; int32_t as_i32; @@ -146,19 +162,6 @@ typedef enum { CAN_BAUD_100K } CAN_BAUD; -typedef struct { - double lat; - double lon; - float height; - float speed; - float hdop; - int32_t ms_today; - int8_t yy; - int8_t mo; - int8_t dd; - systime_t last_update; -} gnss_data; - // LBM typedef uint32_t lbm_value; typedef uint32_t lbm_type; @@ -297,6 +300,11 @@ typedef enum { CFG_PARAM_si_battery_cells, CFG_PARAM_si_battery_ah, CFG_PARAM_si_motor_nl_current, + + // Motor FOC Parameters + CFG_PARAM_foc_motor_r, + CFG_PARAM_foc_motor_l, + CFG_PARAM_foc_motor_flux_linkage, } CFG_PARAM; typedef struct { @@ -374,22 +382,25 @@ typedef struct { int (*printf)(const char *str, ...); void* (*malloc)(size_t bytes); void (*free)(void *prt); - lib_thread (*spawn)(void (*fun)(void *arg), size_t stack_size, char *name, void *arg); + lib_thread (*spawn)(void (*fun)(void *arg), size_t stack_size, const char *name, void *arg); void (*request_terminate)(lib_thread thd); bool (*should_terminate)(void); void** (*get_arg)(uint32_t prog_addr); - + + #ifndef ESP_PLATFORM // ST IO void (*set_pad_mode)(void *gpio, uint32_t pin, uint32_t mode); void (*set_pad)(void *gpio, uint32_t pin); void (*clear_pad)(void *gpio, uint32_t pin); + // Abstract IO bool (*io_set_mode)(VESC_PIN pin, VESC_PIN_MODE mode); bool (*io_write)(VESC_PIN pin, int state); bool (*io_read)(VESC_PIN pin); float (*io_read_analog)(VESC_PIN pin); bool (*io_get_st_pin)(VESC_PIN vesc_pin, void **gpio, uint32_t *pin); + #endif // CAN void (*can_set_sid_cb)(bool (*p_func)(uint32_t id, uint8_t *data, uint8_t len)); @@ -420,6 +431,7 @@ typedef struct { can_status_msg_6* (*can_get_status_msg_6_index)(int index); can_status_msg_6* (*can_get_status_msg_6_id)(int id); + #ifndef ESP_PLATFORM // Motor Control int (*mc_motor_now)(void); void (*mc_select_motor_thread)(int motor); @@ -481,17 +493,25 @@ typedef struct { float (*mc_stat_temp_motor_max)(void); float (*mc_stat_count_time)(void); void (*mc_stat_reset)(void); + #endif // Comm void (*commands_process_packet)(unsigned char *data, unsigned int len, void(*reply_func)(unsigned char *data, unsigned int len)); + #ifdef ESP_PLATFORM + void (*send_app_data)(unsigned char *data, unsigned int len, int interface, int can_id); + #else void (*send_app_data)(unsigned char *data, unsigned int len); + #endif + bool (*set_app_data_handler)(void(*func)(unsigned char *data, unsigned int len)); + #ifndef ESP_PLATFORM // UART bool (*uart_start)(uint32_t baudrate, bool half_duplex); - bool (*uart_write)(uint8_t *data, uint32_t size); + bool (*uart_write)(const uint8_t *data, uint32_t size); int32_t (*uart_read)(void); + #endif // Packets void (*packet_init)(void (*s_func)(unsigned char *, unsigned int), @@ -510,7 +530,7 @@ typedef struct { void (*imu_get_accel)(float *accel); void (*imu_get_gyro)(float *gyro); void (*imu_get_mag)(float *mag); - void (*imu_derotate)(float *input, float *output); + void (*imu_derotate)(const float *input, float *output); void (*imu_get_accel_derotated)(float *accel); void (*imu_get_gyro_derotated)(float *gyro); void (*imu_get_quaternions)(float *q); @@ -529,14 +549,16 @@ typedef struct { bool (*read_eeprom_var)(eeprom_var *v, int address); bool (*store_eeprom_var)(eeprom_var *v, int address); + #ifndef ESP_PLATFORM // Timeout void (*timeout_reset)(void); bool (*timeout_has_timeout)(void); float (*timeout_secs_since_update)(void); + #endif // Plot - void (*plot_init)(char *namex, char *namey); - void (*plot_add_graph)(char *name); + void (*plot_init)(const char *namex, const char *namey); + void (*plot_add_graph)(const char *name); void (*plot_set_graph)(int graph); void (*plot_send_points)(float x, float y); @@ -547,23 +569,29 @@ typedef struct { int (*get_cfg_xml)(uint8_t **data)); void (*conf_custom_clear_configs)(void); + #ifndef ESP_PLATFORM // Settings (TODO: Add more types) float (*get_cfg_float)(CFG_PARAM p); int (*get_cfg_int)(CFG_PARAM p); bool (*set_cfg_float)(CFG_PARAM p, float value); bool (*set_cfg_int)(CFG_PARAM p, int value); bool (*store_cfg)(void); + #endif + #ifndef ESP_PLATFORM // GNSS-struct that can be both read and updated volatile gnss_data* (*mc_gnss)(void); + #endif // Mutex lib_mutex (*mutex_create)(void); // Use VESC_IF->free on the mutex when done with it void (*mutex_lock)(lib_mutex); void (*mutex_unlock)(lib_mutex); + #ifndef ESP_PLATFORM // Get ST io-pin from lbm symbol (this is only safe from extensions) bool (*lbm_symbol_to_io)(lbm_uint sym, void **gpio, uint32_t *pin); + #endif // High resolution timer for short busy-wait sleeps and time measurement uint32_t (*timer_time_now)(void); @@ -580,33 +608,38 @@ typedef struct { // IMU AHRS functions and read callback void (*imu_set_read_callback)(void (*func)(float *acc, float *gyro, float *mag, float dt)); void (*ahrs_init_attitude_info)(ATTITUDE_INFO *att); - void (*ahrs_update_initial_orientation)(float *accelXYZ, float *magXYZ, ATTITUDE_INFO *att); - void (*ahrs_update_mahony_imu)(float *gyroXYZ, float *accelXYZ, float dt, ATTITUDE_INFO *att); - void (*ahrs_update_madgwick_imu)(float *gyroXYZ, float *accelXYZ, float dt, ATTITUDE_INFO *att); - float (*ahrs_get_roll)(ATTITUDE_INFO *att); - float (*ahrs_get_pitch)(ATTITUDE_INFO *att); - float (*ahrs_get_yaw)(ATTITUDE_INFO *att); - + void (*ahrs_update_initial_orientation)(const float *accelXYZ, const float *magXYZ, ATTITUDE_INFO *att); + void (*ahrs_update_mahony_imu)(const float *gyroXYZ, const float *accelXYZ, float dt, ATTITUDE_INFO *att); + void (*ahrs_update_madgwick_imu)(const float *gyroXYZ, const float *accelXYZ, float dt, ATTITUDE_INFO *att); + float (*ahrs_get_roll)(const ATTITUDE_INFO *att); + float (*ahrs_get_pitch)(const ATTITUDE_INFO *att); + float (*ahrs_get_yaw)(const ATTITUDE_INFO *att); + + #ifndef ESP_PLATFORM // Set custom encoder callbacks void (*encoder_set_custom_callbacks)( float (*read_deg)(void), bool (*has_fault)(void), char* (*print_info)(void)); + #endif // Store backup data bool (*store_backup_data)(void); - // Input Devices + // Input Devices remote_state (*get_remote_state)(void); float (*get_ppm)(void); // Get decoded PPM, range -1.0 to 1.0. If the decoder is not running it will be started. float (*get_ppm_age)(void); // Get time since a pulse was decoded in seconds bool (*app_is_output_disabled)(void); // True if apps should disable their output. + #ifndef ESP_PLATFORM // NVM bool (*read_nvm)(uint8_t *v, unsigned int len, unsigned int address); bool (*write_nvm)(uint8_t *v, unsigned int len, unsigned int address); bool (*wipe_nvm)(void); + #endif + #ifndef ESP_PLATFORM // FOC float (*foc_get_id)(void); float (*foc_get_iq)(void); @@ -616,6 +649,7 @@ typedef struct { void (*foc_set_openloop_phase)(float current, float phase); void (*foc_set_openloop_duty)(float dutyCycle, float rpm); void (*foc_set_openloop_duty_phase)(float dutyCycle, float phase); + #endif // Functions below were added in firmware 6.05 @@ -642,13 +676,15 @@ typedef struct { systime_t (*system_time_ticks)(void); void (*sleep_ticks)(systime_t ticks); + #ifndef ESP_PLATFORM // FOC Audio bool (*foc_beep)(float freq, float time, float voltage); bool (*foc_play_tone)(int channel, float freq, float voltage); void (*foc_stop_audio)(bool reset); - bool (*foc_set_audio_sample_table)(int channel, float *samples, int len); + bool (*foc_set_audio_sample_table)(int channel, const float *samples, int len); const float* (*foc_get_audio_sample_table)(int channel); bool (*foc_play_audio_samples)(const int8_t *samples, int num_samp, float f_samp, float voltage); + #endif // Semaphore lib_semaphore (*sem_create)(void); // Use VESC_IF->free on the semaphore when done with it @@ -656,6 +692,26 @@ typedef struct { void (*sem_signal)(lib_semaphore); bool (*sem_wait_to)(lib_semaphore, systime_t); // Returns false on timeout void (*sem_reset)(lib_semaphore); + + // Functions below were added in firmware 6.06 + + // Set priority of current thread + // Range: -5 to 5, -5 is lowest, 0 is normal, 5 is highest + void (*thread_set_priority)(int priority); + #ifndef ESP_PLATFORM + // Disable shutdown (for hw with momentary button / auto shutdown support) + void (*shutdown_disable)(bool disable); + #endif + + #ifdef ESP_PLATFORM + // Functions below were added in firmware 7.00 and are VESC Express specific + bool (*rgbled_init)(int pin); + void (*rgbled_deinit)(); + void (*rgbled_update)(uint8_t * data, size_t size); + int (*aes_ctr_crypt)(const uint8_t *key, size_t key_len, + uint8_t counter[16], + uint8_t *buf, size_t start, size_t len); + #endif } vesc_c_if; typedef struct { @@ -664,14 +720,21 @@ typedef struct { uint32_t base_addr; } lib_info; -// System tick rate. Can be used to convert system ticks to time -#define SYSTEM_TICK_RATE_HZ 10000 + // VESC-interface with function pointers +#ifdef ESP_PLATFORM +// System tick rate. Can be used to convert system ticks to time +#define SYSTEM_TICK_RATE_HZ 1000 +#define VESC_IF ((vesc_c_if*)(0x3FCCF800)) +#else +// System tick rate. Can be used to convert system ticks to time +#define SYSTEM_TICK_RATE_HZ 10000 #define VESC_IF ((vesc_c_if*)(0x1000F800)) +#endif // Put this at the beginning of your source file -#define HEADER static volatile int __attribute__((__section__(".program_ptr"))) prog_ptr; +#define HEADER volatile int __attribute__((__section__(".program_ptr"))) prog_ptr; // Init function #define INIT_FUN bool __attribute__((__section__(".init_fun"))) init @@ -685,5 +748,7 @@ typedef struct { // The argument that was set in the init function (same as the one you get in stop_fun) #define ARG (*VESC_IF->get_arg(PROG_ADDR)) +extern volatile int prog_ptr; + #endif // VESC_C_IF_H diff --git a/refloat/vesc_pkg_lib/vesc_c_if.h b/refloat/vesc_pkg_lib/vesc_c_if.h index f612bb907..6b7c9b687 100755 --- a/refloat/vesc_pkg_lib/vesc_c_if.h +++ b/refloat/vesc_pkg_lib/vesc_c_if.h @@ -24,9 +24,56 @@ #include #include -#ifdef IS_VESC_LIB +#define NATIVE_LIB_MAGIC 0xCAFEBABE + +#if defined(ESP_PLATFORM) || defined(IS_VESC_LIB) typedef uint32_t systime_t; +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; + +typedef struct { + double lat; + double lon; + float height; + float speed; + float hdop; + int32_t ms_today; + int8_t yy; + int8_t mo; + int8_t dd; + systime_t last_update; +} gnss_data; +#endif +#ifdef IS_VESC_LIB typedef struct { int id; systime_t rx_time; @@ -80,37 +127,6 @@ typedef enum { 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; - typedef union { uint32_t as_u32; int32_t as_i32; @@ -146,19 +162,6 @@ typedef enum { CAN_BAUD_100K } CAN_BAUD; -typedef struct { - double lat; - double lon; - float height; - float speed; - float hdop; - int32_t ms_today; - int8_t yy; - int8_t mo; - int8_t dd; - systime_t last_update; -} gnss_data; - // LBM typedef uint32_t lbm_value; typedef uint32_t lbm_type; @@ -297,6 +300,11 @@ typedef enum { CFG_PARAM_si_battery_cells, CFG_PARAM_si_battery_ah, CFG_PARAM_si_motor_nl_current, + + // Motor FOC Parameters + CFG_PARAM_foc_motor_r, + CFG_PARAM_foc_motor_l, + CFG_PARAM_foc_motor_flux_linkage, } CFG_PARAM; typedef struct { @@ -374,22 +382,25 @@ typedef struct { int (*printf)(const char *str, ...); void* (*malloc)(size_t bytes); void (*free)(void *prt); - lib_thread (*spawn)(void (*fun)(void *arg), size_t stack_size, char *name, void *arg); + lib_thread (*spawn)(void (*fun)(void *arg), size_t stack_size, const char *name, void *arg); void (*request_terminate)(lib_thread thd); bool (*should_terminate)(void); void** (*get_arg)(uint32_t prog_addr); - + + #ifndef ESP_PLATFORM // ST IO void (*set_pad_mode)(void *gpio, uint32_t pin, uint32_t mode); void (*set_pad)(void *gpio, uint32_t pin); void (*clear_pad)(void *gpio, uint32_t pin); + // Abstract IO bool (*io_set_mode)(VESC_PIN pin, VESC_PIN_MODE mode); bool (*io_write)(VESC_PIN pin, int state); bool (*io_read)(VESC_PIN pin); float (*io_read_analog)(VESC_PIN pin); bool (*io_get_st_pin)(VESC_PIN vesc_pin, void **gpio, uint32_t *pin); + #endif // CAN void (*can_set_sid_cb)(bool (*p_func)(uint32_t id, uint8_t *data, uint8_t len)); @@ -420,6 +431,7 @@ typedef struct { can_status_msg_6* (*can_get_status_msg_6_index)(int index); can_status_msg_6* (*can_get_status_msg_6_id)(int id); + #ifndef ESP_PLATFORM // Motor Control int (*mc_motor_now)(void); void (*mc_select_motor_thread)(int motor); @@ -481,17 +493,25 @@ typedef struct { float (*mc_stat_temp_motor_max)(void); float (*mc_stat_count_time)(void); void (*mc_stat_reset)(void); + #endif // Comm void (*commands_process_packet)(unsigned char *data, unsigned int len, void(*reply_func)(unsigned char *data, unsigned int len)); + #ifdef ESP_PLATFORM + void (*send_app_data)(unsigned char *data, unsigned int len, int interface, int can_id); + #else void (*send_app_data)(unsigned char *data, unsigned int len); + #endif + bool (*set_app_data_handler)(void(*func)(unsigned char *data, unsigned int len)); + #ifndef ESP_PLATFORM // UART bool (*uart_start)(uint32_t baudrate, bool half_duplex); - bool (*uart_write)(uint8_t *data, uint32_t size); + bool (*uart_write)(const uint8_t *data, uint32_t size); int32_t (*uart_read)(void); + #endif // Packets void (*packet_init)(void (*s_func)(unsigned char *, unsigned int), @@ -510,7 +530,7 @@ typedef struct { void (*imu_get_accel)(float *accel); void (*imu_get_gyro)(float *gyro); void (*imu_get_mag)(float *mag); - void (*imu_derotate)(float *input, float *output); + void (*imu_derotate)(const float *input, float *output); void (*imu_get_accel_derotated)(float *accel); void (*imu_get_gyro_derotated)(float *gyro); void (*imu_get_quaternions)(float *q); @@ -529,14 +549,16 @@ typedef struct { bool (*read_eeprom_var)(eeprom_var *v, int address); bool (*store_eeprom_var)(eeprom_var *v, int address); + #ifndef ESP_PLATFORM // Timeout void (*timeout_reset)(void); bool (*timeout_has_timeout)(void); float (*timeout_secs_since_update)(void); + #endif // Plot - void (*plot_init)(char *namex, char *namey); - void (*plot_add_graph)(char *name); + void (*plot_init)(const char *namex, const char *namey); + void (*plot_add_graph)(const char *name); void (*plot_set_graph)(int graph); void (*plot_send_points)(float x, float y); @@ -547,23 +569,29 @@ typedef struct { int (*get_cfg_xml)(uint8_t **data)); void (*conf_custom_clear_configs)(void); + #ifndef ESP_PLATFORM // Settings (TODO: Add more types) float (*get_cfg_float)(CFG_PARAM p); int (*get_cfg_int)(CFG_PARAM p); bool (*set_cfg_float)(CFG_PARAM p, float value); bool (*set_cfg_int)(CFG_PARAM p, int value); bool (*store_cfg)(void); + #endif + #ifndef ESP_PLATFORM // GNSS-struct that can be both read and updated volatile gnss_data* (*mc_gnss)(void); + #endif // Mutex lib_mutex (*mutex_create)(void); // Use VESC_IF->free on the mutex when done with it void (*mutex_lock)(lib_mutex); void (*mutex_unlock)(lib_mutex); + #ifndef ESP_PLATFORM // Get ST io-pin from lbm symbol (this is only safe from extensions) bool (*lbm_symbol_to_io)(lbm_uint sym, void **gpio, uint32_t *pin); + #endif // High resolution timer for short busy-wait sleeps and time measurement uint32_t (*timer_time_now)(void); @@ -580,34 +608,38 @@ typedef struct { // IMU AHRS functions and read callback void (*imu_set_read_callback)(void (*func)(float *acc, float *gyro, float *mag, float dt)); void (*ahrs_init_attitude_info)(ATTITUDE_INFO *att); - void (*ahrs_update_initial_orientation)(float *accelXYZ, float *magXYZ, ATTITUDE_INFO *att); - void (*ahrs_update_mahony_imu)(float *gyroXYZ, float *accelXYZ, float dt, ATTITUDE_INFO *att); - void (*ahrs_update_madgwick_imu)(float *gyroXYZ, float *accelXYZ, float dt, ATTITUDE_INFO *att); - float (*ahrs_get_roll)(ATTITUDE_INFO *att); - float (*ahrs_get_pitch)(ATTITUDE_INFO *att); - float (*ahrs_get_yaw)(ATTITUDE_INFO *att); - + void (*ahrs_update_initial_orientation)(const float *accelXYZ, const float *magXYZ, ATTITUDE_INFO *att); + void (*ahrs_update_mahony_imu)(const float *gyroXYZ, const float *accelXYZ, float dt, ATTITUDE_INFO *att); + void (*ahrs_update_madgwick_imu)(const float *gyroXYZ, const float *accelXYZ, float dt, ATTITUDE_INFO *att); + float (*ahrs_get_roll)(const ATTITUDE_INFO *att); + float (*ahrs_get_pitch)(const ATTITUDE_INFO *att); + float (*ahrs_get_yaw)(const ATTITUDE_INFO *att); + + #ifndef ESP_PLATFORM // Set custom encoder callbacks void (*encoder_set_custom_callbacks)( float (*read_deg)(void), bool (*has_fault)(void), char* (*print_info)(void)); + #endif // Store backup data bool (*store_backup_data)(void); - // Input Devices + // Input Devices remote_state (*get_remote_state)(void); float (*get_ppm)(void); // Get decoded PPM, range -1.0 to 1.0. If the decoder is not running it will be started. float (*get_ppm_age)(void); // Get time since a pulse was decoded in seconds bool (*app_is_output_disabled)(void); // True if apps should disable their output. - // Interfaces added in firmware 6.2: + #ifndef ESP_PLATFORM // NVM bool (*read_nvm)(uint8_t *v, unsigned int len, unsigned int address); bool (*write_nvm)(uint8_t *v, unsigned int len, unsigned int address); bool (*wipe_nvm)(void); + #endif + #ifndef ESP_PLATFORM // FOC float (*foc_get_id)(void); float (*foc_get_iq)(void); @@ -617,6 +649,7 @@ typedef struct { void (*foc_set_openloop_phase)(float current, float phase); void (*foc_set_openloop_duty)(float dutyCycle, float rpm); void (*foc_set_openloop_duty_phase)(float dutyCycle, float phase); + #endif // Functions below were added in firmware 6.05 @@ -643,13 +676,15 @@ typedef struct { systime_t (*system_time_ticks)(void); void (*sleep_ticks)(systime_t ticks); + #ifndef ESP_PLATFORM // FOC Audio bool (*foc_beep)(float freq, float time, float voltage); bool (*foc_play_tone)(int channel, float freq, float voltage); void (*foc_stop_audio)(bool reset); - bool (*foc_set_audio_sample_table)(int channel, float *samples, int len); + bool (*foc_set_audio_sample_table)(int channel, const float *samples, int len); const float* (*foc_get_audio_sample_table)(int channel); bool (*foc_play_audio_samples)(const int8_t *samples, int num_samp, float f_samp, float voltage); + #endif // Semaphore lib_semaphore (*sem_create)(void); // Use VESC_IF->free on the semaphore when done with it @@ -657,6 +692,26 @@ typedef struct { void (*sem_signal)(lib_semaphore); bool (*sem_wait_to)(lib_semaphore, systime_t); // Returns false on timeout void (*sem_reset)(lib_semaphore); + + // Functions below were added in firmware 6.06 + + // Set priority of current thread + // Range: -5 to 5, -5 is lowest, 0 is normal, 5 is highest + void (*thread_set_priority)(int priority); + #ifndef ESP_PLATFORM + // Disable shutdown (for hw with momentary button / auto shutdown support) + void (*shutdown_disable)(bool disable); + #endif + + #ifdef ESP_PLATFORM + // Functions below were added in firmware 7.00 and are VESC Express specific + bool (*rgbled_init)(int pin); + void (*rgbled_deinit)(); + void (*rgbled_update)(uint8_t * data, size_t size); + int (*aes_ctr_crypt)(const uint8_t *key, size_t key_len, + uint8_t counter[16], + uint8_t *buf, size_t start, size_t len); + #endif } vesc_c_if; typedef struct { @@ -665,14 +720,21 @@ typedef struct { uint32_t base_addr; } lib_info; -// System tick rate. Can be used to convert system ticks to time -#define SYSTEM_TICK_RATE_HZ 10000 + // VESC-interface with function pointers +#ifdef ESP_PLATFORM +// System tick rate. Can be used to convert system ticks to time +#define SYSTEM_TICK_RATE_HZ 1000 +#define VESC_IF ((vesc_c_if*)(0x3FCCF800)) +#else +// System tick rate. Can be used to convert system ticks to time +#define SYSTEM_TICK_RATE_HZ 10000 #define VESC_IF ((vesc_c_if*)(0x1000F800)) +#endif // Put this at the beginning of your source file -#define HEADER static volatile int __attribute__((__section__(".program_ptr"))) prog_ptr; +#define HEADER volatile int __attribute__((__section__(".program_ptr"))) prog_ptr; // Init function #define INIT_FUN bool __attribute__((__section__(".init_fun"))) init @@ -686,5 +748,7 @@ typedef struct { // The argument that was set in the init function (same as the one you get in stop_fun) #define ARG (*VESC_IF->get_arg(PROG_ADDR)) +extern volatile int prog_ptr; + #endif // VESC_C_IF_H