Added lbm-commands to get PPM and UAVCAN update age

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;

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();
 		}
 	}

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_ */

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:

lispBM/lispif_vesc_extensions.c

@@ -42,6 +42,7 @@
 #include "confgenerator.h"
 #include "worker.h"
 #include "app.h"
+#include "canard_driver.h"
 
 #include <math.h>
 #include <ctype.h>
@@ -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();
 	}