mirror of https://github.com/rusefi/bldc.git
1022 lines
33 KiB
C
1022 lines
33 KiB
C
/*
|
|
Copyright 2022 Benjamin Vedder benjamin@vedder.se
|
|
|
|
This file is part of the VESC firmware.
|
|
|
|
The VESC firmware 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.
|
|
|
|
The VESC firmware 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 <http://www.gnu.org/licenses/>.
|
|
*/
|
|
|
|
#include "ch.h"
|
|
#include "hal.h"
|
|
#include "hw.h"
|
|
#include "packet.h"
|
|
#include "mc_interface.h"
|
|
#include "commands.h"
|
|
#include "comm_can.h"
|
|
#include "lispif.h"
|
|
#include "lispbm.h"
|
|
#include "utils.h"
|
|
#include "c_libs/vesc_c_if.h"
|
|
#include "worker.h"
|
|
#include "app.h"
|
|
#include "mempools.h"
|
|
#include "imu.h"
|
|
#include "terminal.h"
|
|
#include "timeout.h"
|
|
#include "conf_custom.h"
|
|
#include "timer.h"
|
|
#include "ahrs.h"
|
|
#include "encoder.h"
|
|
#include "conf_general.h"
|
|
#include "servo_dec.h"
|
|
#include "servo_simple.h"
|
|
#include "flash_helper.h"
|
|
#include "mcpwm_foc.h"
|
|
|
|
// Function prototypes otherwise missing
|
|
void packet_init(void (*s_func)(unsigned char *data, unsigned int len),
|
|
void (*p_func)(unsigned char *data, unsigned int len), PACKET_STATE_t *state);
|
|
void packet_reset(PACKET_STATE_t *state);
|
|
void packet_process_byte(uint8_t rx_data, PACKET_STATE_t *state);
|
|
void packet_send_packet(unsigned char *data, unsigned int len, PACKET_STATE_t *state);
|
|
|
|
typedef struct {
|
|
char *name;
|
|
void *arg;
|
|
void (*func)(void*);
|
|
void *w_mem;
|
|
} lib_thd_info;
|
|
|
|
#define LIB_NUM_MAX 10
|
|
static lib_info loaded_libs[LIB_NUM_MAX] = {0};
|
|
|
|
static bool lib_init_done = false;
|
|
|
|
__attribute__((section(".libif"))) static volatile union {
|
|
vesc_c_if cif;
|
|
char pad[2048];
|
|
} cif;
|
|
|
|
static thread_t* lib_running_threads[20];
|
|
static size_t lib_running_threads_cnt = 0;
|
|
|
|
static void lib_sleep_ms(uint32_t ms) {
|
|
chThdSleepMilliseconds(ms);
|
|
}
|
|
|
|
static void lib_sleep_us(uint32_t us) {
|
|
chThdSleepMicroseconds(us);
|
|
}
|
|
|
|
static float lib_system_time(void) {
|
|
return UTILS_AGE_S(0);
|
|
}
|
|
|
|
static THD_FUNCTION(lib_thd, arg) {
|
|
lib_thd_info *t = (lib_thd_info*)arg;
|
|
chRegSetThreadName(t->name);
|
|
t->func(t->arg);
|
|
lbm_free(t->w_mem);
|
|
lbm_free(t);
|
|
}
|
|
|
|
lib_thread lispif_spawn(void (*func)(void*), size_t stack_size, char *name, void *arg) {
|
|
if (!utils_is_func_valid(func)) {
|
|
commands_printf_lisp("Invalid function address. Make sure that the function is static.");
|
|
return 0;
|
|
}
|
|
|
|
void *mem = lbm_malloc_reserve(stack_size);
|
|
|
|
if (mem) {
|
|
lib_thd_info *info = lbm_malloc_reserve(sizeof(lib_thd_info));
|
|
|
|
if (info) {
|
|
info->arg = arg;
|
|
info->func = func;
|
|
info->name = name;
|
|
info->w_mem = mem;
|
|
|
|
thread_t *thd = chThdCreateStatic(mem, stack_size, NORMALPRIO, lib_thd, info);
|
|
|
|
if (lib_running_threads_cnt < (sizeof(lib_running_threads) / sizeof(lib_running_threads[0]))) {
|
|
lib_running_threads[lib_running_threads_cnt++] = thd;
|
|
}
|
|
|
|
return (lib_thread)thd;
|
|
}
|
|
}
|
|
|
|
return 0;
|
|
}
|
|
|
|
static void lib_request_terminate(lib_thread thd) {
|
|
chThdTerminate((thread_t*)thd);
|
|
|
|
int timeout = 2000;
|
|
while (!chThdTerminatedX((thread_t*)thd)) {
|
|
chThdSleepMilliseconds(1);
|
|
timeout--;
|
|
|
|
// Not terminating, reset using wdt to not start lbm after reboot.
|
|
if (timeout == 0) {
|
|
commands_printf_lisp("Not terminating, crashing...");
|
|
chThdSleepMilliseconds(20);
|
|
chSysLock();
|
|
for (;;) {__NOP();}
|
|
}
|
|
}
|
|
|
|
}
|
|
|
|
static bool lib_should_terminate(void) {
|
|
return chThdShouldTerminateX();
|
|
}
|
|
|
|
static void** lib_get_arg(uint32_t prog_addr) {
|
|
for (int i = 0;i < LIB_NUM_MAX;i++) {
|
|
if (loaded_libs[i].base_addr == prog_addr) {
|
|
return &loaded_libs[i].arg;
|
|
}
|
|
}
|
|
return 0;
|
|
}
|
|
|
|
static void lib_set_pad_mode(void *gpio, uint32_t pin, uint32_t mode) {
|
|
palSetPadMode((stm32_gpio_t*)gpio, pin, mode);
|
|
}
|
|
|
|
static void lib_set_pad(void *gpio, uint32_t pin) {
|
|
palSetPad((stm32_gpio_t*)gpio, pin);
|
|
}
|
|
|
|
static void lib_clear_pad(void *gpio, uint32_t pin) {
|
|
palClearPad((stm32_gpio_t*)gpio, pin);
|
|
}
|
|
|
|
static bool get_gpio(VESC_PIN io, stm32_gpio_t **port, uint32_t *pin, bool *is_analog) {
|
|
bool res = false;
|
|
*is_analog = false;
|
|
|
|
switch (io) {
|
|
case VESC_PIN_COMM_RX:
|
|
#ifdef HW_UART_RX_PORT
|
|
*port = HW_UART_RX_PORT; *pin = HW_UART_RX_PIN;
|
|
res = true;
|
|
#endif
|
|
break;
|
|
case VESC_PIN_COMM_TX:
|
|
#ifdef HW_UART_TX_PORT
|
|
*port = HW_UART_TX_PORT; *pin = HW_UART_TX_PIN;
|
|
res = true;
|
|
#endif
|
|
break;
|
|
case VESC_PIN_SWDIO:
|
|
*port = GPIOA; *pin = 13;
|
|
res = true;
|
|
break;
|
|
case VESC_PIN_SWCLK:
|
|
*port = GPIOA; *pin = 14;
|
|
res = true;
|
|
break;
|
|
case VESC_PIN_HALL1:
|
|
*port = HW_HALL_ENC_GPIO1; *pin = HW_HALL_ENC_PIN1;
|
|
res = true;
|
|
break;
|
|
case VESC_PIN_HALL2:
|
|
*port = HW_HALL_ENC_GPIO2; *pin = HW_HALL_ENC_PIN2;
|
|
res = true;
|
|
break;
|
|
case VESC_PIN_HALL3:
|
|
*port = HW_HALL_ENC_GPIO3; *pin = HW_HALL_ENC_PIN3;
|
|
res = true;
|
|
break;
|
|
case VESC_PIN_ADC1:
|
|
#ifdef HW_ADC_EXT_GPIO
|
|
*port = HW_ADC_EXT_GPIO; *pin = HW_ADC_EXT_PIN;
|
|
*is_analog = true;
|
|
res = true;
|
|
#endif
|
|
break;
|
|
case VESC_PIN_ADC2:
|
|
#ifdef HW_ADC_EXT2_GPIO
|
|
*port = HW_ADC_EXT2_GPIO; *pin = HW_ADC_EXT2_PIN;
|
|
*is_analog = true;
|
|
res = true;
|
|
#endif
|
|
break;
|
|
case VESC_PIN_HALL4:
|
|
#ifdef HW_HALL_ENC_GPIO4
|
|
*port = HW_HALL_ENC_GPIO4; *pin = HW_HALL_ENC_PIN4;
|
|
res = true;
|
|
#endif
|
|
break;
|
|
case VESC_PIN_HALL5:
|
|
#ifdef HW_HALL_ENC_GPIO5
|
|
*port = HW_HALL_ENC_GPIO5; *pin = HW_HALL_ENC_PIN5;
|
|
res = true;
|
|
#endif
|
|
break;
|
|
case VESC_PIN_HALL6:
|
|
#ifdef HW_HALL_ENC_GPIO6
|
|
*port = HW_HALL_ENC_GPIO6; *pin = HW_HALL_ENC_PIN6;
|
|
res = true;
|
|
#endif
|
|
break;
|
|
case VESC_PIN_PPM:
|
|
#ifdef HW_ICU_GPIO
|
|
*port = HW_ICU_GPIO; *pin = HW_ICU_PIN;
|
|
res = true;
|
|
#endif
|
|
break;
|
|
}
|
|
|
|
return res;
|
|
}
|
|
|
|
static bool lib_io_set_mode(VESC_PIN pin_vesc, VESC_PIN_MODE mode) {
|
|
stm32_gpio_t *gpio;
|
|
uint32_t pin;
|
|
bool is_analog;
|
|
bool res = false;
|
|
|
|
if (get_gpio(pin_vesc, &gpio, &pin, &is_analog)) {
|
|
switch (mode) {
|
|
case VESC_PIN_MODE_INPUT_NOPULL:
|
|
palSetPadMode(gpio, pin, PAL_MODE_INPUT);
|
|
res = true;
|
|
break;
|
|
case VESC_PIN_MODE_INPUT_PULL_UP:
|
|
palSetPadMode(gpio, pin, PAL_MODE_INPUT_PULLUP);
|
|
res = true;
|
|
break;
|
|
case VESC_PIN_MODE_INPUT_PULL_DOWN:
|
|
palSetPadMode(gpio, pin, PAL_MODE_INPUT_PULLDOWN);
|
|
res = true;
|
|
break;
|
|
case VESC_PIN_MODE_OUTPUT:
|
|
palSetPadMode(gpio, pin, PAL_MODE_OUTPUT_PUSHPULL);
|
|
res = true;
|
|
break;
|
|
case VESC_PIN_MODE_OUTPUT_OPEN_DRAIN:
|
|
palSetPadMode(gpio, pin, PAL_MODE_OUTPUT_OPENDRAIN);
|
|
res = true;
|
|
break;
|
|
case VESC_PIN_MODE_OUTPUT_OPEN_DRAIN_PULL_UP:
|
|
palSetPadMode(gpio, pin, PAL_MODE_OUTPUT_OPENDRAIN | PAL_STM32_PUDR_PULLUP);
|
|
res = true;
|
|
break;
|
|
case VESC_PIN_MODE_OUTPUT_OPEN_DRAIN_PULL_DOWN:
|
|
palSetPadMode(gpio, pin, PAL_MODE_OUTPUT_OPENDRAIN | PAL_STM32_PUDR_PULLDOWN);
|
|
res = true;
|
|
break;
|
|
case VESC_PIN_MODE_ANALOG:
|
|
if (is_analog) {
|
|
palSetPadMode(gpio, pin, PAL_STM32_MODE_ANALOG);
|
|
res = true;
|
|
}
|
|
break;
|
|
}
|
|
}
|
|
|
|
return res;
|
|
}
|
|
|
|
static bool lib_io_write(VESC_PIN pin_vesc, int state) {
|
|
stm32_gpio_t *gpio;
|
|
uint32_t pin;
|
|
bool is_analog;
|
|
bool res = false;
|
|
|
|
if (get_gpio(pin_vesc, &gpio, &pin, &is_analog)) {
|
|
palWritePad(gpio, pin, state);
|
|
res = true;
|
|
}
|
|
|
|
return res;
|
|
}
|
|
|
|
static bool lib_io_read(VESC_PIN pin_vesc) {
|
|
stm32_gpio_t *gpio;
|
|
uint32_t pin;
|
|
bool is_analog;
|
|
bool res = false;
|
|
|
|
if (get_gpio(pin_vesc, &gpio, &pin, &is_analog)) {
|
|
res = palReadPad(gpio, pin);
|
|
}
|
|
|
|
return res;
|
|
}
|
|
|
|
static float lib_io_read_analog(VESC_PIN pin_vesc) {
|
|
float res = -1.0;
|
|
|
|
if (pin_vesc == VESC_PIN_ADC1) {
|
|
res = ADC_VOLTS(ADC_IND_EXT);
|
|
} else if (pin_vesc == VESC_PIN_ADC2) {
|
|
res = ADC_VOLTS(ADC_IND_EXT2);
|
|
}
|
|
|
|
return res;
|
|
}
|
|
|
|
static bool lib_io_get_st_pin(VESC_PIN vesc_pin, void **gpio, uint32_t *pin) {
|
|
bool analog;
|
|
return get_gpio(vesc_pin, (stm32_gpio_t**)gpio, pin, &analog);
|
|
}
|
|
|
|
static bool lib_symbol_to_io(lbm_uint sym, void **gpio, uint32_t *pin) {
|
|
return lispif_symbol_to_io(sym, (stm32_gpio_t**)gpio, pin);
|
|
}
|
|
|
|
static SerialConfig uart_cfg = {
|
|
2500000, 0, 0, 0
|
|
};
|
|
|
|
static bool lib_uart_start(uint32_t baudrate, bool half_duplex) {
|
|
app_configuration *appconf = mempools_alloc_appconf();
|
|
conf_general_read_app_configuration(appconf);
|
|
if (appconf->app_to_use == APP_UART ||
|
|
appconf->app_to_use == APP_PPM_UART ||
|
|
appconf->app_to_use == APP_ADC_UART) {
|
|
appconf->app_to_use = APP_NONE;
|
|
conf_general_store_app_configuration(appconf);
|
|
app_set_configuration(appconf);
|
|
}
|
|
mempools_free_appconf(appconf);
|
|
|
|
uart_cfg.speed = baudrate;
|
|
uart_cfg.cr3 = half_duplex ? USART_CR3_HDSEL : 0;
|
|
|
|
sdStop(&HW_UART_DEV);
|
|
sdStart(&HW_UART_DEV, &uart_cfg);
|
|
|
|
palSetPadMode(HW_UART_TX_PORT, HW_UART_TX_PIN, PAL_MODE_ALTERNATE(HW_UART_GPIO_AF));
|
|
if (!half_duplex) {
|
|
palSetPadMode(HW_UART_RX_PORT, HW_UART_RX_PIN, PAL_MODE_ALTERNATE(HW_UART_GPIO_AF));
|
|
}
|
|
|
|
return true;
|
|
}
|
|
|
|
static void wait_uart_tx_task(void *arg) {
|
|
(void)arg;
|
|
while(!chOQIsEmptyI(&HW_UART_DEV.oqueue)){
|
|
chThdSleepMilliseconds(1);
|
|
}
|
|
chThdSleepMilliseconds(1);
|
|
HW_UART_DEV.usart->CR1 |= USART_CR1_RE;
|
|
}
|
|
|
|
static bool lib_uart_write(uint8_t *data, uint32_t size) {
|
|
if (uart_cfg.cr3 & USART_CR3_HDSEL) {
|
|
HW_UART_DEV.usart->CR1 &= ~USART_CR1_RE;
|
|
sdWrite(&HW_UART_DEV, data, size);
|
|
worker_execute(wait_uart_tx_task, 0);
|
|
} else{
|
|
sdWrite(&HW_UART_DEV, data, size);
|
|
}
|
|
|
|
return true;
|
|
}
|
|
|
|
static int32_t lib_uart_read(void) {
|
|
return sdGetTimeout(&HW_UART_DEV, TIME_IMMEDIATE);
|
|
}
|
|
|
|
static float lib_ts_to_age_s(systime_t ts) {
|
|
return UTILS_AGE_S(ts);
|
|
}
|
|
|
|
static float lib_get_cfg_float(CFG_PARAM p) {
|
|
float res = 0.0;
|
|
|
|
const volatile mc_configuration *mcconf = mc_interface_get_configuration();
|
|
const app_configuration *appconf = app_get_configuration();
|
|
|
|
switch (p) {
|
|
case CFG_PARAM_l_current_max: res = mcconf->l_current_max; break;
|
|
case CFG_PARAM_l_current_min: res = mcconf->l_current_min; break;
|
|
case CFG_PARAM_l_in_current_max: res = mcconf->l_in_current_max; break;
|
|
case CFG_PARAM_l_in_current_min: res = mcconf->l_in_current_min; break;
|
|
case CFG_PARAM_l_abs_current_max: res = mcconf->l_abs_current_max; break;
|
|
case CFG_PARAM_l_min_erpm: res = mcconf->l_min_erpm; break;
|
|
case CFG_PARAM_l_max_erpm: res = mcconf->l_max_erpm; break;
|
|
case CFG_PARAM_l_erpm_start: res = mcconf->l_erpm_start; break;
|
|
case CFG_PARAM_l_max_erpm_fbrake: res = mcconf->l_max_erpm_fbrake; break;
|
|
case CFG_PARAM_l_max_erpm_fbrake_cc: res = mcconf->l_max_erpm_fbrake_cc; break;
|
|
case CFG_PARAM_l_min_vin: res = mcconf->l_min_vin; break;
|
|
case CFG_PARAM_l_max_vin: res = mcconf->l_max_vin; break;
|
|
case CFG_PARAM_l_battery_cut_start: res = mcconf->l_battery_cut_start; break;
|
|
case CFG_PARAM_l_battery_cut_end: res = mcconf->l_battery_cut_end; break;
|
|
case CFG_PARAM_l_temp_fet_start: res = mcconf->l_temp_fet_start; break;
|
|
case CFG_PARAM_l_temp_fet_end: res = mcconf->l_temp_fet_end; break;
|
|
case CFG_PARAM_l_temp_motor_start: res = mcconf->l_temp_motor_start; break;
|
|
case CFG_PARAM_l_temp_motor_end: res = mcconf->l_temp_motor_end; break;
|
|
case CFG_PARAM_l_temp_accel_dec: res = mcconf->l_temp_accel_dec; break;
|
|
case CFG_PARAM_l_min_duty: res = mcconf->l_min_duty; break;
|
|
case CFG_PARAM_l_max_duty: res = mcconf->l_max_duty; break;
|
|
|
|
case CFG_PARAM_IMU_accel_confidence_decay: res = appconf->imu_conf.accel_confidence_decay; break;
|
|
case CFG_PARAM_IMU_mahony_kp: res = appconf->imu_conf.mahony_kp; break;
|
|
case CFG_PARAM_IMU_mahony_ki: res = appconf->imu_conf.mahony_ki; break;
|
|
case CFG_PARAM_IMU_madgwick_beta: res = appconf->imu_conf.madgwick_beta; break;
|
|
case CFG_PARAM_IMU_rot_roll: res = appconf->imu_conf.rot_roll; break;
|
|
case CFG_PARAM_IMU_rot_pitch: res = appconf->imu_conf.rot_pitch; break;
|
|
case CFG_PARAM_IMU_rot_yaw: res = appconf->imu_conf.rot_yaw; break;
|
|
default: break;
|
|
}
|
|
|
|
return res;
|
|
}
|
|
|
|
static int lib_get_cfg_int(CFG_PARAM p) {
|
|
int res = 0.0;
|
|
|
|
const app_configuration *conf = app_get_configuration();
|
|
|
|
switch (p) {
|
|
case CFG_PARAM_app_can_mode: res = conf->can_mode; break;
|
|
default: break;
|
|
}
|
|
|
|
return res;
|
|
}
|
|
|
|
static bool lib_set_cfg_float(CFG_PARAM p, float value) {
|
|
bool res = false;
|
|
|
|
mc_configuration *mcconf = (mc_configuration*)mc_interface_get_configuration();
|
|
int changed_mc = 0;
|
|
|
|
app_configuration *appconf = mempools_alloc_appconf();
|
|
*appconf = *app_get_configuration();
|
|
int changed_app = 0;
|
|
|
|
// Safe changes that can be done instantly on the pointer. It is not that good to do
|
|
// it this way, but it is much faster.
|
|
// TODO: Check regularly and make sure that these stay safe.
|
|
switch (p) {
|
|
case CFG_PARAM_l_current_max: mcconf->l_current_max = value; changed_mc = 1; res = true; break;
|
|
case CFG_PARAM_l_current_min: mcconf->l_current_min = value; changed_mc = 1; res = true; break;
|
|
case CFG_PARAM_l_in_current_max: mcconf->l_in_current_max = value; changed_mc = 1; res = true; break;
|
|
case CFG_PARAM_l_in_current_min: mcconf->l_in_current_min = value; changed_mc = 1; res = true; break;
|
|
case CFG_PARAM_l_abs_current_max: mcconf->l_abs_current_max = value; changed_mc = 1; res = true; break;
|
|
case CFG_PARAM_l_min_erpm: mcconf->l_min_erpm = value; changed_mc = 1; res = true; break;
|
|
case CFG_PARAM_l_max_erpm: mcconf->l_max_erpm = value; changed_mc = 1; res = true; break;
|
|
case CFG_PARAM_l_erpm_start: mcconf->l_erpm_start = value; changed_mc = 1; res = true; break;
|
|
case CFG_PARAM_l_max_erpm_fbrake: mcconf->l_max_erpm_fbrake = value; changed_mc = 1; res = true; break;
|
|
case CFG_PARAM_l_max_erpm_fbrake_cc: mcconf->l_max_erpm_fbrake_cc = value; changed_mc = 1; res = true; break;
|
|
case CFG_PARAM_l_min_vin: mcconf->l_min_vin = value; changed_mc = 1; res = true; break;
|
|
case CFG_PARAM_l_max_vin: mcconf->l_max_vin = value; changed_mc = 1; res = true; break;
|
|
case CFG_PARAM_l_battery_cut_start: mcconf->l_battery_cut_start = value; changed_mc = 1; res = true; break;
|
|
case CFG_PARAM_l_battery_cut_end: mcconf->l_battery_cut_end = value; changed_mc = 1; res = true; break;
|
|
case CFG_PARAM_l_temp_fet_start: mcconf->l_temp_fet_start = value; changed_mc = 1; res = true; break;
|
|
case CFG_PARAM_l_temp_fet_end: mcconf->l_temp_fet_end = value; changed_mc = 1; res = true; break;
|
|
case CFG_PARAM_l_temp_motor_start: mcconf->l_temp_motor_start = value; changed_mc = 1; res = true; break;
|
|
case CFG_PARAM_l_temp_motor_end: mcconf->l_temp_motor_end = value; changed_mc = 1; res = true; break;
|
|
case CFG_PARAM_l_temp_accel_dec: mcconf->l_temp_accel_dec = value; changed_mc = 1; res = true; break;
|
|
case CFG_PARAM_l_min_duty: mcconf->l_min_duty = value; changed_mc = 1; res = true; break;
|
|
case CFG_PARAM_l_max_duty: mcconf->l_max_duty = value; changed_mc = 1; res = true; break;
|
|
|
|
case CFG_PARAM_IMU_accel_confidence_decay: appconf->imu_conf.accel_confidence_decay = value; changed_app = 1; res = true; break;
|
|
case CFG_PARAM_IMU_mahony_kp: appconf->imu_conf.mahony_kp = value; changed_app = 1; res = true; break;
|
|
case CFG_PARAM_IMU_mahony_ki: appconf->imu_conf.mahony_ki = value; changed_app = 1; res = true; break;
|
|
case CFG_PARAM_IMU_madgwick_beta: appconf->imu_conf.madgwick_beta = value; changed_app = 1; res = true; break;
|
|
case CFG_PARAM_IMU_rot_roll: appconf->imu_conf.rot_roll = value; changed_app = 1; res = true; break;
|
|
case CFG_PARAM_IMU_rot_pitch: appconf->imu_conf.rot_pitch = value; changed_app = 1; res = true; break;
|
|
case CFG_PARAM_IMU_rot_yaw: appconf->imu_conf.rot_yaw = value; changed_app = 1; res = true; break;
|
|
default: break;
|
|
}
|
|
|
|
if (changed_mc > 0) {
|
|
commands_apply_mcconf_hw_limits(mcconf);
|
|
}
|
|
|
|
if (changed_app > 0) {
|
|
app_set_configuration(appconf);
|
|
}
|
|
|
|
mempools_free_appconf(appconf);
|
|
|
|
return res;
|
|
}
|
|
|
|
static bool lib_set_cfg_int(CFG_PARAM p, int value) {
|
|
bool res = false;
|
|
|
|
app_configuration *appconf = mempools_alloc_appconf();
|
|
*appconf = *app_get_configuration();
|
|
|
|
switch (p) {
|
|
case CFG_PARAM_app_can_mode: appconf->can_mode = value; res = true; break;
|
|
case CFG_PARAM_app_can_baud_rate: appconf->can_baud_rate = value; res = true; break;
|
|
default: break;
|
|
}
|
|
|
|
if (res) {
|
|
app_set_configuration(appconf);
|
|
}
|
|
|
|
mempools_free_appconf(appconf);
|
|
|
|
return res;
|
|
}
|
|
|
|
static bool lib_store_cfg(void) {
|
|
mc_configuration *mcconf = mempools_alloc_mcconf();
|
|
*mcconf = *mc_interface_get_configuration();
|
|
bool res_mc = conf_general_store_mc_configuration(mcconf, mc_interface_get_motor_thread() == 2);
|
|
mempools_free_mcconf(mcconf);
|
|
|
|
app_configuration *appconf = mempools_alloc_appconf();
|
|
*appconf = *app_get_configuration();
|
|
bool res_app = conf_general_store_app_configuration(appconf);
|
|
mempools_free_appconf(appconf);
|
|
|
|
return res_mc && res_app;
|
|
}
|
|
|
|
static bool lib_create_byte_array(lbm_value *value, lbm_uint num_elt) {
|
|
return lbm_heap_allocate_array(value, num_elt);
|
|
}
|
|
|
|
static bool lib_eval_is_paused(void) {
|
|
return lbm_get_eval_state() == EVAL_CPS_STATE_PAUSED;
|
|
}
|
|
|
|
static lib_mutex lib_mutex_create(void) {
|
|
mutex_t *m = lbm_malloc_reserve(sizeof(mutex_t));
|
|
chMtxObjectInit(m);
|
|
return (lib_mutex)m;
|
|
}
|
|
|
|
static void lib_mutex_lock(lib_mutex m) {
|
|
chMtxLock((mutex_t*)m);
|
|
}
|
|
|
|
static void lib_mutex_unlock(lib_mutex m) {
|
|
chMtxUnlock((mutex_t*)m);
|
|
}
|
|
|
|
static remote_state lib_get_remote_state(void) {
|
|
remote_state res;
|
|
res.js_x = app_nunchuk_get_decoded_x();
|
|
res.js_y = app_nunchuk_get_decoded_y();
|
|
res.bt_c = app_nunchuk_get_bt_c();
|
|
res.bt_z = app_nunchuk_get_bt_z();
|
|
res.is_rev = app_nunchuk_get_is_rev();
|
|
res.age_s = app_nunchuk_get_update_age();
|
|
return res;
|
|
}
|
|
|
|
static float lib_get_ppm_age(void) {
|
|
return (float)servodec_get_time_since_update() / 1000.0;
|
|
}
|
|
|
|
static bool lib_add_extension(char *sym_str, extension_fptr ext) {
|
|
if (sym_str[0] != 'e' ||
|
|
sym_str[1] != 'x' ||
|
|
sym_str[2] != 't' ||
|
|
sym_str[3] != '-') {
|
|
commands_printf_lisp("Error: Extensions must start with ext-");
|
|
return false;
|
|
}
|
|
|
|
return lbm_add_extension(sym_str, ext);
|
|
}
|
|
|
|
lbm_value ext_load_native_lib(lbm_value *args, lbm_uint argn) {
|
|
lbm_value res = lbm_enc_sym(SYM_EERROR);
|
|
|
|
if (argn != 1 || !lbm_is_array_r(args[0])) {
|
|
return res;
|
|
}
|
|
|
|
lbm_array_header_t *array = (lbm_array_header_t *)lbm_car(args[0]);
|
|
|
|
if (!lib_init_done) {
|
|
memset((char*)cif.pad, 0, 2048);
|
|
|
|
// LBM
|
|
cif.cif.lbm_add_extension = lib_add_extension;
|
|
cif.cif.lbm_block_ctx_from_extension = lbm_block_ctx_from_extension;
|
|
cif.cif.lbm_unblock_ctx = lbm_unblock_ctx;
|
|
cif.cif.lbm_get_current_cid = lbm_get_current_cid;
|
|
cif.cif.lbm_set_error_reason = lbm_set_error_reason;
|
|
cif.cif.lbm_pause_eval_with_gc = lbm_pause_eval_with_gc;
|
|
cif.cif.lbm_continue_eval = lbm_continue_eval;
|
|
cif.cif.lbm_send_message = lbm_send_message;
|
|
cif.cif.lbm_eval_is_paused = lib_eval_is_paused;
|
|
|
|
cif.cif.lbm_cons = lbm_cons;
|
|
cif.cif.lbm_car = lbm_car;
|
|
cif.cif.lbm_cdr = lbm_cdr;
|
|
cif.cif.lbm_list_destructive_reverse = lbm_list_destructive_reverse;
|
|
cif.cif.lbm_create_byte_array = lib_create_byte_array;
|
|
|
|
cif.cif.lbm_add_symbol_const = lbm_add_symbol_const;
|
|
cif.cif.lbm_get_symbol_by_name = lbm_get_symbol_by_name;
|
|
|
|
cif.cif.lbm_enc_i = lbm_enc_i;
|
|
cif.cif.lbm_enc_u = lbm_enc_u;
|
|
cif.cif.lbm_enc_char = lbm_enc_char;
|
|
cif.cif.lbm_enc_float = lbm_enc_float;
|
|
cif.cif.lbm_enc_u32 = lbm_enc_u32;
|
|
cif.cif.lbm_enc_i32 = lbm_enc_i32;
|
|
cif.cif.lbm_enc_sym = lbm_enc_sym;
|
|
|
|
cif.cif.lbm_dec_as_float = lbm_dec_as_float;
|
|
cif.cif.lbm_dec_as_u32 = lbm_dec_as_u32;
|
|
cif.cif.lbm_dec_as_i32 = lbm_dec_as_i32;
|
|
cif.cif.lbm_dec_char = lbm_dec_char;
|
|
cif.cif.lbm_dec_str = lbm_dec_str;
|
|
cif.cif.lbm_dec_sym = lbm_dec_sym;
|
|
|
|
cif.cif.lbm_is_byte_array = lbm_is_array_r;
|
|
cif.cif.lbm_is_cons = lbm_is_cons;
|
|
cif.cif.lbm_is_number = lbm_is_number;
|
|
cif.cif.lbm_is_char = lbm_is_char;
|
|
cif.cif.lbm_is_symbol = lbm_is_symbol;
|
|
|
|
cif.cif.lbm_enc_sym_nil = ENC_SYM_NIL;
|
|
cif.cif.lbm_enc_sym_true = ENC_SYM_TRUE;
|
|
cif.cif.lbm_enc_sym_terror = ENC_SYM_TERROR;
|
|
cif.cif.lbm_enc_sym_eerror = ENC_SYM_EERROR;
|
|
cif.cif.lbm_enc_sym_merror = ENC_SYM_MERROR;
|
|
|
|
cif.cif.lbm_is_symbol_nil = lbm_is_symbol_nil;
|
|
cif.cif.lbm_is_symbol_true = lbm_is_symbol_true;
|
|
|
|
// Os
|
|
cif.cif.sleep_ms = lib_sleep_ms;
|
|
cif.cif.sleep_us = lib_sleep_us;
|
|
cif.cif.system_time = lib_system_time;
|
|
cif.cif.ts_to_age_s = lib_ts_to_age_s;
|
|
cif.cif.printf = commands_printf_lisp;
|
|
cif.cif.malloc = lbm_malloc_reserve;
|
|
cif.cif.free = lbm_free;
|
|
cif.cif.spawn = lispif_spawn;
|
|
cif.cif.request_terminate = lib_request_terminate;
|
|
cif.cif.should_terminate = lib_should_terminate;
|
|
cif.cif.get_arg = lib_get_arg;
|
|
|
|
// ST IO
|
|
cif.cif.set_pad_mode = lib_set_pad_mode;
|
|
cif.cif.set_pad = lib_set_pad;
|
|
cif.cif.clear_pad = lib_clear_pad;
|
|
|
|
// Abstract IO
|
|
cif.cif.io_set_mode = lib_io_set_mode;
|
|
cif.cif.io_write = lib_io_write;
|
|
cif.cif.io_read = lib_io_read;
|
|
cif.cif.io_read_analog = lib_io_read_analog;
|
|
cif.cif.io_get_st_pin = lib_io_get_st_pin;
|
|
|
|
// CAN
|
|
cif.cif.can_set_sid_cb = comm_can_set_sid_rx_callback;
|
|
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;
|
|
|
|
// Comm
|
|
cif.cif.commands_process_packet = commands_process_packet;
|
|
cif.cif.send_app_data = commands_send_app_data;
|
|
cif.cif.set_app_data_handler = commands_set_app_data_handler;
|
|
|
|
// UART
|
|
cif.cif.uart_start = lib_uart_start;
|
|
cif.cif.uart_write = lib_uart_write;
|
|
cif.cif.uart_read = lib_uart_read;
|
|
|
|
// Packets
|
|
cif.cif.packet_init = packet_init;
|
|
cif.cif.packet_reset = packet_reset;
|
|
cif.cif.packet_process_byte = packet_process_byte;
|
|
cif.cif.packet_send_packet = packet_send_packet;
|
|
|
|
// IMU
|
|
cif.cif.imu_startup_done = imu_startup_done;
|
|
cif.cif.imu_get_roll = imu_get_roll;
|
|
cif.cif.imu_get_pitch = imu_get_pitch;
|
|
cif.cif.imu_get_yaw = imu_get_yaw;
|
|
cif.cif.imu_get_rpy = imu_get_rpy;
|
|
cif.cif.imu_get_accel = imu_get_accel;
|
|
cif.cif.imu_get_gyro = imu_get_gyro;
|
|
cif.cif.imu_get_mag = imu_get_mag;
|
|
cif.cif.imu_derotate = imu_derotate;
|
|
cif.cif.imu_get_accel_derotated = imu_get_accel_derotated;
|
|
cif.cif.imu_get_gyro_derotated = imu_get_gyro_derotated;
|
|
cif.cif.imu_get_quaternions = imu_get_quaternions;
|
|
cif.cif.imu_get_calibration = imu_get_calibration;
|
|
cif.cif.imu_set_yaw = imu_set_yaw;
|
|
|
|
// Terminal
|
|
cif.cif.terminal_register_command_callback = terminal_register_command_callback;
|
|
cif.cif.terminal_unregister_callback = terminal_unregister_callback;
|
|
|
|
// EEPROM
|
|
cif.cif.read_eeprom_var = conf_general_read_eeprom_var_custom;
|
|
cif.cif.store_eeprom_var = conf_general_store_eeprom_var_custom;
|
|
|
|
// Timeout
|
|
cif.cif.timeout_reset = timeout_reset;
|
|
cif.cif.timeout_has_timeout = timeout_has_timeout;
|
|
cif.cif.timeout_secs_since_update = timeout_secs_since_update;
|
|
|
|
// Plot
|
|
cif.cif.plot_init = commands_init_plot;
|
|
cif.cif.plot_add_graph = commands_plot_add_graph;
|
|
cif.cif.plot_set_graph = commands_plot_set_graph;
|
|
cif.cif.plot_send_points = commands_send_plot_points;
|
|
|
|
// Custom config
|
|
cif.cif.conf_custom_add_config = conf_custom_add_config;
|
|
cif.cif.conf_custom_clear_configs = conf_custom_clear_configs;
|
|
|
|
// Settings
|
|
cif.cif.get_cfg_float = lib_get_cfg_float;
|
|
cif.cif.get_cfg_int = lib_get_cfg_int;
|
|
cif.cif.set_cfg_float = lib_set_cfg_float;
|
|
cif.cif.set_cfg_int = lib_set_cfg_int;
|
|
cif.cif.store_cfg = lib_store_cfg;
|
|
|
|
// GNSS-struct that can be both read and updated
|
|
cif.cif.mc_gnss = mc_interface_gnss;
|
|
|
|
// Mutex
|
|
cif.cif.mutex_create = lib_mutex_create;
|
|
cif.cif.mutex_lock = lib_mutex_lock;
|
|
cif.cif.mutex_unlock = lib_mutex_unlock;
|
|
|
|
// Get ST io-pin from lbm symbol (this is only safe from extensions)
|
|
cif.cif.lbm_symbol_to_io = lib_symbol_to_io;
|
|
|
|
// High resolution timer for short busy-wait sleeps and time measurement
|
|
cif.cif.timer_time_now = timer_time_now;
|
|
cif.cif.timer_seconds_elapsed_since = timer_seconds_elapsed_since;
|
|
cif.cif.timer_sleep = timer_sleep;
|
|
|
|
// System lock (with counting)
|
|
cif.cif.sys_lock = utils_sys_lock_cnt;
|
|
cif.cif.sys_unlock = utils_sys_unlock_cnt;
|
|
|
|
// Unregister pointers to previously used reply function
|
|
cif.cif.commands_unregister_reply_func = commands_unregister_reply_func;
|
|
|
|
// IMU AHRS functions and read callback
|
|
cif.cif.imu_set_read_callback = imu_set_read_callback;
|
|
cif.cif.ahrs_init_attitude_info = ahrs_init_attitude_info;
|
|
cif.cif.ahrs_update_initial_orientation = ahrs_update_initial_orientation;
|
|
cif.cif.ahrs_update_mahony_imu = ahrs_update_mahony_imu;
|
|
cif.cif.ahrs_update_madgwick_imu = ahrs_update_madgwick_imu;
|
|
cif.cif.ahrs_get_roll = ahrs_get_roll;
|
|
cif.cif.ahrs_get_pitch = ahrs_get_pitch;
|
|
cif.cif.ahrs_get_yaw = ahrs_get_yaw;
|
|
|
|
// Set custom encoder callbacks
|
|
cif.cif.encoder_set_custom_callbacks = encoder_set_custom_callbacks;
|
|
|
|
// Store backup data
|
|
cif.cif.store_backup_data = conf_general_store_backup_data;
|
|
|
|
// Input Devices
|
|
cif.cif.get_remote_state = lib_get_remote_state;
|
|
cif.cif.get_ppm = lispif_get_ppm;
|
|
cif.cif.get_ppm_age = lib_get_ppm_age;
|
|
cif.cif.app_is_output_disabled = app_is_output_disabled;
|
|
|
|
// NVM
|
|
cif.cif.read_nvm = flash_helper_read_nvm;
|
|
cif.cif.write_nvm = flash_helper_write_nvm;
|
|
cif.cif.wipe_nvm = flash_helper_wipe_nvm;
|
|
|
|
// FOC
|
|
cif.cif.foc_get_id = mcpwm_foc_get_id_filter;
|
|
cif.cif.foc_get_iq = mcpwm_foc_get_iq_filter;
|
|
cif.cif.foc_get_vd = mcpwm_foc_get_vd;
|
|
cif.cif.foc_get_vq = mcpwm_foc_get_vq;
|
|
cif.cif.foc_set_openloop_current = mc_interface_set_openloop_current;
|
|
cif.cif.foc_set_openloop_phase = mc_interface_set_openloop_phase;
|
|
cif.cif.foc_set_openloop_duty = mc_interface_set_openloop_duty;
|
|
cif.cif.foc_set_openloop_duty_phase = mc_interface_set_openloop_duty_phase;
|
|
|
|
// Flat values
|
|
cif.cif.lbm_start_flatten = lbm_start_flatten;
|
|
cif.cif.lbm_finish_flatten = lbm_finish_flatten;
|
|
cif.cif.f_b = f_b;
|
|
cif.cif.f_cons = f_cons;
|
|
cif.cif.f_float = f_float;
|
|
cif.cif.f_i = f_i;
|
|
cif.cif.f_i32 = f_i32;
|
|
cif.cif.f_i64 = f_i64;
|
|
cif.cif.f_lbm_array = f_lbm_array;
|
|
cif.cif.f_sym = f_sym;
|
|
cif.cif.f_u32 = f_u32;
|
|
cif.cif.f_u64 = f_u64;
|
|
|
|
// Unblock unboxed
|
|
cif.cif.lbm_unblock_ctx_unboxed = lbm_unblock_ctx_unboxed;
|
|
|
|
lib_init_done = true;
|
|
}
|
|
|
|
uint32_t addr = (uint32_t)array->data;
|
|
|
|
for (int i = 0;i < LIB_NUM_MAX;i++) {
|
|
if (loaded_libs[i].stop_fun != NULL && loaded_libs[i].base_addr == addr) {
|
|
lbm_set_error_reason("Library already loaded");
|
|
return res;
|
|
}
|
|
}
|
|
|
|
bool ok = false;
|
|
for (int i = 0;i < LIB_NUM_MAX;i++) {
|
|
if (loaded_libs[i].stop_fun == NULL) {
|
|
loaded_libs[i].base_addr = addr;
|
|
addr += 4; // Skip program pointer
|
|
addr |= 1; // Ensure that thumb mode is used (??)
|
|
ok = ((bool(*)(lib_info *info))addr)(&loaded_libs[i]);
|
|
|
|
if (loaded_libs[i].stop_fun != NULL && !utils_is_func_valid(loaded_libs[i].stop_fun)) {
|
|
loaded_libs[i].stop_fun = NULL;
|
|
lbm_set_error_reason("Invalid stop function. Make sure that it is static.");
|
|
return res;
|
|
}
|
|
|
|
break;
|
|
}
|
|
}
|
|
|
|
if (ok) {
|
|
res = lbm_enc_sym(SYM_TRUE);
|
|
} else {
|
|
lbm_set_error_reason("Library init failed");
|
|
}
|
|
|
|
return res;
|
|
}
|
|
|
|
lbm_value ext_unload_native_lib(lbm_value *args, lbm_uint argn) {
|
|
lbm_value res = lbm_enc_sym(SYM_EERROR);
|
|
|
|
if (argn != 1 || !lbm_is_array_r(args[0])) {
|
|
return res;
|
|
}
|
|
|
|
lbm_array_header_t *array = (lbm_array_header_t *)lbm_car(args[0]);
|
|
uint32_t addr = (uint32_t)array->data;
|
|
|
|
bool ok = false;
|
|
for (int i = 0;i < LIB_NUM_MAX;i++) {
|
|
if (loaded_libs[i].stop_fun != NULL && loaded_libs[i].base_addr == addr) {
|
|
loaded_libs[i].stop_fun(loaded_libs[i].arg);
|
|
loaded_libs[i].stop_fun = NULL;
|
|
res = lbm_enc_sym(SYM_TRUE);
|
|
ok = true;
|
|
}
|
|
}
|
|
|
|
if (!ok) {
|
|
lbm_set_error_reason("Library not loaded");
|
|
}
|
|
|
|
return res;
|
|
}
|
|
|
|
void lispif_stop_lib(void) {
|
|
for (int i = 0;i < LIB_NUM_MAX;i++) {
|
|
if (loaded_libs[i].stop_fun != NULL) {
|
|
loaded_libs[i].stop_fun(loaded_libs[i].arg);
|
|
loaded_libs[i].stop_fun = NULL;
|
|
}
|
|
}
|
|
|
|
for (size_t i = 0;i < lib_running_threads_cnt;i++) {
|
|
if (!chThdTerminatedX(lib_running_threads[i])) {
|
|
lib_request_terminate(lib_running_threads[i]);
|
|
}
|
|
}
|
|
|
|
lib_running_threads_cnt = 0;
|
|
}
|
|
|
|
float lispif_get_ppm(void) {
|
|
if (!servodec_is_running()) {
|
|
servo_simple_stop();
|
|
servodec_init(0);
|
|
}
|
|
|
|
const ppm_config* cfg = &(app_get_configuration()->app_ppm_conf);
|
|
servodec_set_pulse_options(cfg->pulse_start, cfg->pulse_end, cfg->median_filter);
|
|
|
|
float servo_val = servodec_get_servo(0);
|
|
float servo_ms = utils_map(servo_val, -1.0, 1.0, cfg->pulse_start, cfg->pulse_end);
|
|
|
|
// Mapping with respect to center pulsewidth
|
|
if (servo_ms < cfg->pulse_center) {
|
|
servo_val = utils_map(servo_ms, cfg->pulse_start, cfg->pulse_center, -1.0, 0.0);
|
|
} else {
|
|
servo_val = utils_map(servo_ms, cfg->pulse_center, cfg->pulse_end, 0.0, 1.0);
|
|
}
|
|
|
|
return servo_val;
|
|
}
|