diff --git a/driver/servo_dec.c b/driver/servo_dec.c index ac493340e..25d818646 100644 --- a/driver/servo_dec.c +++ b/driver/servo_dec.c @@ -31,7 +31,7 @@ #define TIMER_FREQ 1000000 // Private variables -static volatile systime_t last_update_time; +static volatile systime_t last_update_time = 0; static volatile float servo_pos[SERVO_NUM]; static volatile float pulse_start = 1.0; static volatile float pulse_end = 2.0; diff --git a/libcanard/canard_driver.c b/libcanard/canard_driver.c index 3f22b72c3..414941913 100755 --- a/libcanard/canard_driver.c +++ b/libcanard/canard_driver.c @@ -74,10 +74,19 @@ typedef struct { uavcan_equipment_esc_Status msg; } status_msg_wrapper_t; +typedef struct { + float rawval; + float rpmval; + systime_t rawtime; + systime_t rpmtime; +} cmd_info_data; + // Private variables static CanardInstance canard_ins; static uint8_t canard_memory_pool[1024]; +static cmd_info_data can1_cmd = {0}; #ifdef HW_CAN2_DEV +static cmd_info_data can2_cmd = {0}; static CanardInstance canard_ins_if2; static uint8_t canard_memory_pool_if2[1024]; #endif @@ -327,6 +336,12 @@ static param_t* getParamByName(char * name) void canard_driver_init(void) { debug_level = 0; + memset(&can1_cmd, 0, sizeof(can1_cmd)); + +#ifdef HW_CAN2_DEV + memset(&can2_cmd, 0, sizeof(can2_cmd)); +#endif + for (int i = 0;i < STATUS_MSGS_TO_STORE;i++) { stat_msgs[i].id = -1; } @@ -340,6 +355,44 @@ void canard_driver_init(void) { terminal_debug_on); } +uavcan_cmd_info canard_driver_last_rawcmd(int can_if) { + uavcan_cmd_info res = {0}; + res.age = UTILS_AGE_S(0); + + if (can_if == 1) { + res.value = can1_cmd.rawval; + res.age = UTILS_AGE_S(can1_cmd.rawtime); + } + +#ifdef HW_CAN2_DEV + if (can_if == 2) { + res.value = can2_cmd.rawval; + res.age = UTILS_AGE_S(can2_cmd.rawtime); + } +#endif + + return res; +} + +uavcan_cmd_info canard_driver_last_rpmcmd(int can_if) { + uavcan_cmd_info res = {0}; + res.age = UTILS_AGE_S(0); + + if (can_if == 1) { + res.value = can1_cmd.rpmval; + res.age = UTILS_AGE_S(can1_cmd.rpmtime); + } + +#ifdef HW_CAN2_DEV + if (can_if == 2) { + res.value = can2_cmd.rpmval; + res.age = UTILS_AGE_S(can2_cmd.rpmtime); + } +#endif + + return res; +} + /* * Calculate the total system current consumption based on the reported consumption by other * ESCs transmitting data on the bus. @@ -520,6 +573,18 @@ static void handle_esc_raw_command(CanardInstance* ins, CanardRxTransfer* transf if (cmd.cmd.len > app_get_configuration()->uavcan_esc_index) { float raw_val = ((float)cmd.cmd.data[app_get_configuration()->uavcan_esc_index]) / 8192.0; + if (ins == &canard_ins) { + can1_cmd.rawtime = chVTGetSystemTimeX(); + can1_cmd.rawval = raw_val; + } + +#ifdef HW_CAN2_DEV + if (ins == &canard_ins_if2) { + can2_cmd.rawtime = chVTGetSystemTimeX(); + can2_cmd.rawval = raw_val; + } +#endif + volatile const app_configuration *conf = app_get_configuration(); app_disable_output(100); @@ -566,7 +631,21 @@ static void handle_esc_rpm_command(CanardInstance* ins, CanardRxTransfer* transf if (uavcan_equipment_esc_RPMCommand_decode_internal(transfer, transfer->payload_len, &cmd, &tmp, 0) >= 0) { if (cmd.rpm.len > app_get_configuration()->uavcan_esc_index) { - mc_interface_set_pid_speed(cmd.rpm.data[app_get_configuration()->uavcan_esc_index]); + float rpm_val = cmd.rpm.data[app_get_configuration()->uavcan_esc_index]; + + if (ins == &canard_ins) { + can1_cmd.rpmtime = chVTGetSystemTimeX(); + can1_cmd.rpmval = rpm_val; + } + +#ifdef HW_CAN2_DEV + if (ins == &canard_ins_if2) { + can2_cmd.rpmtime = chVTGetSystemTimeX(); + can2_cmd.rpmval = rpm_val; + } +#endif + + mc_interface_set_pid_speed(rpm_val); timeout_reset(); } } diff --git a/libcanard/canard_driver.h b/libcanard/canard_driver.h index 1fd0f1be0..d24a1bd0b 100644 --- a/libcanard/canard_driver.h +++ b/libcanard/canard_driver.h @@ -23,6 +23,13 @@ #include "ch.h" #include "hal.h" +typedef struct { + float age; + float value; +} uavcan_cmd_info; + void canard_driver_init(void); +uavcan_cmd_info canard_driver_last_rawcmd(int can_if); +uavcan_cmd_info canard_driver_last_rpmcmd(int can_if); #endif /* LIBCANARD_CANARD_DRIVER_H_ */ diff --git a/lispBM/README.md b/lispBM/README.md index 2c127a0a5..c7bc3614f 100644 --- a/lispBM/README.md +++ b/lispBM/README.md @@ -65,6 +65,14 @@ Read the decoded value on the PPM input and returns 0.0 to 1.0. Note that the PP Note that control type can be set to Off in the PPM app to get the input without running the motor automatically, which is useful when running the motor from lisp. +#### get-ppm-age + +```clj +(get-ppm-age) +``` + +Get the age of the last PPM update in seconds. Can be used to determine if there is any valid PPM-signal. + #### get-encoder ```clj @@ -1999,6 +2007,29 @@ This example creates an extension called ext-test that takes a number as an argu (print (ext-test 4)) ; Should print 12 ``` +## UAVCAN + +#### uavcan-last-rawcmd + +```clj +(uavcan-last-rawcmd canInterface) +``` + +Get the last raw uavcan-command and its age. Returns a list where the first element is the value and the second element is the age. canInterface is the interface, which can be 1 or 2. Interface 2 is only valid if the hardware has dual CAN-buses. Example: + +```clj +(print (ix (uavcan-last-rawcmd 1) 0)) ; Print the value +(print (ix (uavcan-last-rawcmd 1) 1)) ; Print the age in seconds +``` + +#### uavcan-last-rpmcmd + +```clj +(uavcan-last-rpmcmd canInterface) +``` + +Same as uavcan-last-rawcmd, but for the last rpm-command. + ## How to update To update from remote repository: diff --git a/lispBM/lispif_vesc_extensions.c b/lispBM/lispif_vesc_extensions.c index c062eb8d9..445fab7d1 100644 --- a/lispBM/lispif_vesc_extensions.c +++ b/lispBM/lispif_vesc_extensions.c @@ -42,6 +42,7 @@ #include "confgenerator.h" #include "worker.h" #include "app.h" +#include "canard_driver.h" #include #include @@ -468,6 +469,11 @@ static lbm_value ext_get_ppm(lbm_value *args, lbm_uint argn) { return lbm_enc_float(servodec_get_servo(0)); } +static lbm_value ext_get_ppm_age(lbm_value *args, lbm_uint argn) { + (void)args; (void)argn; + return lbm_enc_float((float)servodec_get_time_since_update() / 1000.0); +} + static lbm_value ext_get_encoder(lbm_value *args, lbm_uint argn) { (void)args; (void)argn; return lbm_enc_float(encoder_read_deg()); @@ -3284,6 +3290,26 @@ static lbm_value ext_me_loopforeach(lbm_value *args, lbm_uint argn) { lbm_enc_sym(sym_brk))))); } +static lbm_value ext_uavcan_last_rawcmd(lbm_value *args, lbm_uint argn) { + CHECK_ARGN_NUMBER(1); + int can_if = lbm_dec_as_i32(args[0]); + uavcan_cmd_info info = canard_driver_last_rawcmd(can_if); + lbm_value out_list = lbm_enc_sym(SYM_NIL); + out_list = lbm_cons(lbm_enc_float(info.age), out_list); + out_list = lbm_cons(lbm_enc_float(info.value), out_list); + return out_list; +} + +static lbm_value ext_uavcan_last_rpmcmd(lbm_value *args, lbm_uint argn) { + CHECK_ARGN_NUMBER(1); + int can_if = lbm_dec_as_i32(args[0]); + uavcan_cmd_info info = canard_driver_last_rpmcmd(can_if); + lbm_value out_list = lbm_enc_sym(SYM_NIL); + out_list = lbm_cons(lbm_enc_float(info.age), out_list); + out_list = lbm_cons(lbm_enc_float(info.value), out_list); + return out_list; +} + static lbm_value ext_empty(lbm_value *args, lbm_uint argn) { (void)args;(void)argn; return ENC_SYM_TRUE; @@ -3319,6 +3345,7 @@ void lispif_load_vesc_extensions(void) { lbm_add_extension("print", ext_print); lbm_add_extension("timeout-reset", ext_reset_timeout); lbm_add_extension("get-ppm", ext_get_ppm); + lbm_add_extension("get-ppm-age", ext_get_ppm_age); lbm_add_extension("get-encoder", ext_get_encoder); lbm_add_extension("set-servo", ext_set_servo); lbm_add_extension("get-vin", ext_get_vin); @@ -3487,6 +3514,10 @@ void lispif_load_vesc_extensions(void) { lbm_add_extension("load-native-lib", ext_load_native_lib); lbm_add_extension("unload-native-lib", ext_unload_native_lib); + // UAVCAN + lbm_add_extension("uavcan-last-rawcmd", ext_uavcan_last_rawcmd); + lbm_add_extension("uavcan-last-rpmcmd", ext_uavcan_last_rpmcmd); + if (ext_callback) { ext_callback(); }