Added CAN status message 6 with ADC and PPM values

comm_can.c

@@ -39,6 +39,7 @@
 #include "shutdown.h"
 #include "bms.h"
 #include "encoder_cfg.h"
+#include "servo_dec.h"
 #ifdef USE_LISPBM
 #include "lispif.h"
 #endif
@@ -83,6 +84,7 @@ static can_status_msg_2 stat_msgs_2[CAN_STATUS_MSGS_TO_STORE];
 static can_status_msg_3 stat_msgs_3[CAN_STATUS_MSGS_TO_STORE];
 static can_status_msg_4 stat_msgs_4[CAN_STATUS_MSGS_TO_STORE];
 static can_status_msg_5 stat_msgs_5[CAN_STATUS_MSGS_TO_STORE];
+static can_status_msg_6 stat_msgs_6[CAN_STATUS_MSGS_TO_STORE];
 static io_board_adc_values io_board_adc_1_4[CAN_STATUS_MSGS_TO_STORE];
 static io_board_adc_values io_board_adc_5_8[CAN_STATUS_MSGS_TO_STORE];
 static io_board_digial_inputs io_board_digital_in[CAN_STATUS_MSGS_TO_STORE];
@@ -119,6 +121,7 @@ void comm_can_init(void) {
 		stat_msgs_3[i].id = -1;
 		stat_msgs_4[i].id = -1;
 		stat_msgs_5[i].id = -1;
+		stat_msgs_6[i].id = -1;
 
 		io_board_adc_1_4[i].id = -1;
 		io_board_adc_5_8[i].id = -1;
@@ -915,6 +918,42 @@ can_status_msg_5 *comm_can_get_status_msg_5_id(int id) {
 	return 0;
 }
 
+/**
+ * Get status message 6 by index.
+ *
+ * @param index
+ * Index in the array
+ *
+ * @return
+ * The message or 0 for an invalid index.
+ */
+can_status_msg_6 *comm_can_get_status_msg_6_index(int index) {
+	if (index < CAN_STATUS_MSGS_TO_STORE) {
+		return &stat_msgs_6[index];
+	} else {
+		return 0;
+	}
+}
+
+/**
+ * Get status message 6 by id.
+ *
+ * @param id
+ * Id of the controller that sent the status message.
+ *
+ * @return
+ * The message or 0 for an invalid id.
+ */
+can_status_msg_6 *comm_can_get_status_msg_6_id(int id) {
+	for (int i = 0;i < CAN_STATUS_MSGS_TO_STORE;i++) {
+		if (stat_msgs_6[i].id == id) {
+			return &stat_msgs_6[i];
+		}
+	}
+
+	return 0;
+}
+
 io_board_adc_values *comm_can_get_io_board_adc_1_4_index(int index) {
 	if (index < CAN_STATUS_MSGS_TO_STORE) {
 		return &io_board_adc_1_4[index];
@@ -1114,6 +1153,17 @@ void comm_can_send_status5(uint8_t id, bool replace) {
 			buffer, send_index, replace);
 }
 
+void comm_can_send_status6(uint8_t id, bool replace) {
+	int32_t send_index = 0;
+	uint8_t buffer[8];
+	buffer_append_float16(buffer, ADC_VOLTS(ADC_IND_EXT), 1e3, &send_index);
+	buffer_append_float16(buffer, ADC_VOLTS(ADC_IND_EXT2), 1e3, &send_index);
+	buffer_append_float16(buffer, ADC_VOLTS(ADC_IND_EXT3), 1e3, &send_index);
+	buffer_append_float16(buffer, servodec_get_servo(0), 1e3, &send_index);
+	comm_can_transmit_eid_replace(id | ((uint32_t)CAN_PACKET_STATUS_6 << 8),
+			buffer, send_index, replace);
+}
+
 #if CAN_ENABLE
 static THD_FUNCTION(cancom_read_thread, arg) {
 	(void)arg;
@@ -1262,6 +1312,7 @@ static THD_FUNCTION(cancom_status_internal_thread, arg) {
 		comm_can_send_status3(utils_second_motor_id(), true);
 		comm_can_send_status4(utils_second_motor_id(), true);
 		comm_can_send_status5(utils_second_motor_id(), true);
+		comm_can_send_status6(utils_second_motor_id(), true);
 		chThdSleepMilliseconds(2);
 	}
 }
@@ -1310,6 +1361,15 @@ static void send_can_status(uint8_t msgs, uint8_t id) {
 #ifdef HW_HAS_DUAL_MOTORS
 		mc_interface_select_motor_thread(2);
 		comm_can_send_status5(utils_second_motor_id(), false);
+#endif
+	}
+
+	if ((msgs >> 5) & 1) {
+		mc_interface_select_motor_thread(1);
+		comm_can_send_status6(id, false);
+#ifdef HW_HAS_DUAL_MOTORS
+		mc_interface_select_motor_thread(2);
+		comm_can_send_status6(utils_second_motor_id(), false);
 #endif
 	}
 }
@@ -1816,6 +1876,22 @@ static void decode_msg(uint32_t eid, uint8_t *data8, int len, bool is_replaced)
 		}
 		break;
 
+	case CAN_PACKET_STATUS_6:
+		for (int i = 0;i < CAN_STATUS_MSGS_TO_STORE;i++) {
+			can_status_msg_6 *stat_tmp_6 = &stat_msgs_6[i];
+			if (stat_tmp_6->id == id || stat_tmp_6->id == -1) {
+				ind = 0;
+				stat_tmp_6->id = id;
+				stat_tmp_6->rx_time = chVTGetSystemTime();
+				stat_tmp_6->adc_1 = buffer_get_float16(data8, 1e3, &ind);
+				stat_tmp_6->adc_2 = buffer_get_float16(data8, 1e3, &ind);
+				stat_tmp_6->adc_3 = buffer_get_float16(data8, 1e3, &ind);
+				stat_tmp_6->ppm = buffer_get_float16(data8, 1e3, &ind);
+				break;
+			}
+		}
+		break;
+
 	case CAN_PACKET_IO_BOARD_ADC_1_TO_4:
 		for (int i = 0;i < CAN_STATUS_MSGS_TO_STORE;i++) {
 			io_board_adc_values *msg = &io_board_adc_1_4[i];

comm_can.h

@@ -68,6 +68,8 @@ can_status_msg_4 *comm_can_get_status_msg_4_index(int index);
 can_status_msg_4 *comm_can_get_status_msg_4_id(int id);
 can_status_msg_5 *comm_can_get_status_msg_5_index(int index);
 can_status_msg_5 *comm_can_get_status_msg_5_id(int id);
+can_status_msg_6 *comm_can_get_status_msg_6_index(int index);
+can_status_msg_6 *comm_can_get_status_msg_6_id(int id);
 
 io_board_adc_values *comm_can_get_io_board_adc_1_4_index(int index);
 io_board_adc_values *comm_can_get_io_board_adc_1_4_id(int id);
@@ -90,5 +92,6 @@ void comm_can_send_status2(uint8_t id, bool replace);
 void comm_can_send_status3(uint8_t id, bool replace);
 void comm_can_send_status4(uint8_t id, bool replace);
 void comm_can_send_status5(uint8_t id, bool replace);
+void comm_can_send_status6(uint8_t id, bool replace);
 
 #endif /* COMM_CAN_H_ */

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		21
+#define FW_TEST_VERSION_NUMBER		23
 
 #include "datatypes.h"
 

confgenerator.h

@@ -9,7 +9,7 @@
 
 // Constants
 #define MCCONF_SIGNATURE		1018325777
-#define APPCONF_SIGNATURE		1569897995
+#define APPCONF_SIGNATURE		573192156
 
 // Functions
 int32_t confgenerator_serialize_mcconf(uint8_t *buffer, const mc_configuration *conf);

datatypes.h

@@ -1122,6 +1122,7 @@ typedef enum {
 	CAN_PACKET_UPDATE_PID_POS_OFFSET,
 	CAN_PACKET_POLL_ROTOR_POS,
 	CAN_PACKET_NOTIFY_BOOT,
+	CAN_PACKET_STATUS_6,
 	CAN_PACKET_MAKE_ENUM_32_BITS = 0xFFFFFFFF,
 } CAN_PACKET_ID;
 
@@ -1195,6 +1196,15 @@ typedef struct {
 	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 struct {
 	int id;
 	systime_t rx_time;

lispBM/README.md

@@ -546,6 +546,20 @@ Get speed in meters per second over CAN-bus on VESC with id. The gearing, wheel
 
 Get distance traveled in meters over CAN-bus on VESC with id. As with (canget-speed id), the local configuration will be used to convert the tachometer value to meters.
 
+#### canget-ppm
+```clj
+(canget-ppm id)
+```
+
+Get PPM-input from the VESC with id on the CAN-bus. Note that CAN status message 6 as well as the PPM-app must be active on that VESC for this function to work.
+
+#### canget-adc
+```clj
+(canget-adc id ch)
+```
+
+Get ADC channel ch from the VESC with id on the CAN-bus. Note that CAN status message 6 must be active on that VESC for this function to work.
+
 #### can-list-devs
 ```clj
 (can-list-devs)

lispBM/lispif_vesc_extensions.c

@@ -334,6 +334,8 @@ static lbm_value ext_get_adc(lbm_value *args, lbm_uint argn) {
 			return lbm_enc_F(ADC_VOLTS(ADC_IND_EXT));
 		} else if (channel == 1) {
 			return lbm_enc_F(ADC_VOLTS(ADC_IND_EXT2));
+		} else if (channel == 2) {
+			return lbm_enc_F(ADC_VOLTS(ADC_IND_EXT3));
 		} else {
 			return lbm_enc_sym(SYM_EERROR);
 		}
@@ -870,6 +872,45 @@ static lbm_value ext_can_get_dist(lbm_value *args, lbm_uint argn) {
 	}
 }
 
+static lbm_value ext_can_get_ppm(lbm_value *args, lbm_uint argn) {
+	CHECK_ARGN_NUMBER(1);
+	can_status_msg_6 *stat6 = comm_can_get_status_msg_6_id(lbm_dec_as_i(args[0]));
+	if (stat6) {
+		return lbm_enc_F((float)stat6->ppm);
+	} else {
+		return lbm_enc_F(0.0);
+	}
+}
+
+static lbm_value ext_can_get_adc(lbm_value *args, lbm_uint argn) {
+	if (argn != 1 && argn != 2) {
+		return lbm_enc_sym(SYM_EERROR);
+	}
+
+	CHECK_NUMBER_ALL();
+
+	lbm_int channel = 0;
+	if (argn == 2) {
+		channel = lbm_dec_as_i(args[1]);
+	}
+
+	can_status_msg_6 *stat6 = comm_can_get_status_msg_6_id(lbm_dec_as_i(args[0]));
+
+	if (stat6) {
+		if (channel == 0) {
+			return lbm_enc_F(stat6->adc_1);
+		} else if (channel == 1) {
+			return lbm_enc_F(stat6->adc_2);
+		} else if (channel == 2) {
+			return lbm_enc_F(stat6->adc_3);
+		} else {
+			return lbm_enc_sym(SYM_EERROR);
+		}
+	} else {
+		return lbm_enc_F(-1.0);
+	}
+}
+
 static int cmp_int (const void * a, const void * b) {
 	return ( *(int*)a - *(int*)b );
 }
@@ -2004,6 +2045,8 @@ void lispif_load_vesc_extensions(void) {
 	lbm_add_extension("canget-temp-motor", ext_can_get_temp_motor);
 	lbm_add_extension("canget-speed", ext_can_get_speed);
 	lbm_add_extension("canget-dist", ext_can_get_dist);
+	lbm_add_extension("canget-ppm", ext_can_get_ppm);
+	lbm_add_extension("canget-adc", ext_can_get_adc);
 
 	lbm_add_extension("can-list-devs", ext_can_list_devs);
 	lbm_add_extension("can-scan", ext_can_scan);

terminal.c

@@ -258,7 +258,7 @@ void terminal_process_string(char *str) {
 		commands_printf("CAN devices seen on the bus the past second:\n");
 		for (int i = 0;i < CAN_STATUS_MSGS_TO_STORE;i++) {
 			can_status_msg *msg = comm_can_get_status_msg_index(i);
-			if (msg->id >= 0 && UTILS_AGE_S(msg->rx_time) < 1.0) {
+			if (msg->id >= 0 && UTILS_AGE_S(msg->rx_time) < 5.0) {
 				commands_printf("MSG1 ID    : %i", msg->id);
 				commands_printf("RX Time    : %i", msg->rx_time);
 				commands_printf("Age (s)    : %.4f", (double)UTILS_AGE_S(msg->rx_time));
@@ -268,7 +268,7 @@ void terminal_process_string(char *str) {
 			}
 
 			can_status_msg_2 *msg2 = comm_can_get_status_msg_2_index(i);
-			if (msg2->id >= 0 && UTILS_AGE_S(msg2->rx_time) < 1.0) {
+			if (msg2->id >= 0 && UTILS_AGE_S(msg2->rx_time) < 5.0) {
 				commands_printf("MSG2 ID    : %i", msg2->id);
 				commands_printf("RX Time    : %i", msg2->rx_time);
 				commands_printf("Age (s)    : %.4f", (double)UTILS_AGE_S(msg2->rx_time));
@@ -277,7 +277,7 @@ void terminal_process_string(char *str) {
 			}
 
 			can_status_msg_3 *msg3 = comm_can_get_status_msg_3_index(i);
-			if (msg3->id >= 0 && UTILS_AGE_S(msg3->rx_time) < 1.0) {
+			if (msg3->id >= 0 && UTILS_AGE_S(msg3->rx_time) < 5.0) {
 				commands_printf("MSG3 ID    : %i", msg3->id);
 				commands_printf("RX Time    : %i", msg3->rx_time);
 				commands_printf("Age (s)    : %.4f", (double)UTILS_AGE_S(msg3->rx_time));
@@ -286,7 +286,7 @@ void terminal_process_string(char *str) {
 			}
 
 			can_status_msg_4 *msg4 = comm_can_get_status_msg_4_index(i);
-			if (msg4->id >= 0 && UTILS_AGE_S(msg4->rx_time) < 1.0) {
+			if (msg4->id >= 0 && UTILS_AGE_S(msg4->rx_time) < 5.0) {
 				commands_printf("MSG4 ID    : %i", msg4->id);
 				commands_printf("RX Time    : %i", msg4->rx_time);
 				commands_printf("Age (s)    : %.4f", (double)UTILS_AGE_S(msg4->rx_time));
@@ -297,7 +297,7 @@ void terminal_process_string(char *str) {
 			}
 
 			can_status_msg_5 *msg5 = comm_can_get_status_msg_5_index(i);
-			if (msg5->id >= 0 && UTILS_AGE_S(msg5->rx_time) < 1.0) {
+			if (msg5->id >= 0 && UTILS_AGE_S(msg5->rx_time) < 5.0) {
 				commands_printf("MSG5 ID    : %i", msg5->id);
 				commands_printf("RX Time    : %i", msg5->rx_time);
 				commands_printf("Age (s)    : %.4f", (double)UTILS_AGE_S(msg5->rx_time));
@@ -305,8 +305,17 @@ void terminal_process_string(char *str) {
 				commands_printf("V In       : %.2f\n", (double)msg5->v_in);
 			}
 
+			can_status_msg_6 *msg6 = comm_can_get_status_msg_6_index(i);
+			if (msg6->id >= 0 && UTILS_AGE_S(msg6->rx_time) < 5.0) {
+				commands_printf("MSG6 ID    : %i", msg6->id);
+				commands_printf("RX Time    : %i", msg6->rx_time);
+				commands_printf("Age (s)    : %.4f", (double)UTILS_AGE_S(msg6->rx_time));
+				commands_printf("ADC 1-3    : %.3f V, %.3f V, %.3f V", (double)msg6->adc_1, (double)msg6->adc_2, (double)msg6->adc_3);
+				commands_printf("PPM	    : %.2f\n", (double)msg6->ppm);
+			}
+
 			io_board_adc_values *io_adc = comm_can_get_io_board_adc_1_4_index(i);
-			if (io_adc->id >= 0 && UTILS_AGE_S(io_adc->rx_time) < 1.0) {
+			if (io_adc->id >= 0 && UTILS_AGE_S(io_adc->rx_time) < 5.0) {
 				commands_printf("IO Board ADC 1_4");
 				commands_printf("ID                 : %i", io_adc->id);
 				commands_printf("RX Time            : %i", io_adc->rx_time);
@@ -317,7 +326,7 @@ void terminal_process_string(char *str) {
 			}
 
 			io_adc = comm_can_get_io_board_adc_5_8_index(i);
-			if (io_adc->id >= 0 && UTILS_AGE_S(io_adc->rx_time) < 1.0) {
+			if (io_adc->id >= 0 && UTILS_AGE_S(io_adc->rx_time) < 5.0) {
 				commands_printf("IO Board ADC 5_8");
 				commands_printf("ID                 : %i", io_adc->id);
 				commands_printf("RX Time            : %i", io_adc->rx_time);
@@ -328,7 +337,7 @@ void terminal_process_string(char *str) {
 			}
 
 			io_board_digial_inputs *io_in = comm_can_get_io_board_digital_in_index(i);
-			if (io_in->id >= 0 && UTILS_AGE_S(io_in->rx_time) < 1.0) {
+			if (io_in->id >= 0 && UTILS_AGE_S(io_in->rx_time) < 5.0) {
 				commands_printf("IO Board Inputs");
 				commands_printf("ID                 : %i", io_in->id);
 				commands_printf("RX Time            : %i", io_in->rx_time);