From f4bb885c90e36cff4f038d3f90f4e67ed4b662a3 Mon Sep 17 00:00:00 2001
From: Benjamin Vedder <benjamin@vedder.se>
Date: Tue, 28 Feb 2023 18:35:50 +0100
Subject: [PATCH] Added canget-vin

---
 lispBM/README.md                | 14 ++++++++++++++
 lispBM/lispif_vesc_extensions.c | 11 +++++++++++
 2 files changed, 25 insertions(+)

diff --git a/lispBM/README.md b/lispBM/README.md
index fc95b0e2..2b45e6fd 100644
--- a/lispBM/README.md
+++ b/lispBM/README.md
@@ -1621,6 +1621,20 @@ Get ADC channel ch from the VESC with id on the CAN-bus. Note that CAN status me
 
 ---
 
+#### canget-vin
+
+| Platforms | Firmware |
+|---|---|
+| ESC, Express | 6.02+ |
+
+```clj
+(canget-vin id)
+```
+
+Get input voltage on ESC with id on the CAN-bus.
+
+---
+
 #### can-list-devs
 
 | Platforms | Firmware |
diff --git a/lispBM/lispif_vesc_extensions.c b/lispBM/lispif_vesc_extensions.c
index ff303a9c..ac37da54 100644
--- a/lispBM/lispif_vesc_extensions.c
+++ b/lispBM/lispif_vesc_extensions.c
@@ -1722,6 +1722,16 @@ static lbm_value ext_can_get_adc(lbm_value *args, lbm_uint argn) {
 	}
 }
 
+static lbm_value ext_can_get_vin(lbm_value *args, lbm_uint argn) {
+	LBM_CHECK_ARGN_NUMBER(1);
+	can_status_msg_5 *stat5 = comm_can_get_status_msg_5_id(lbm_dec_as_i32(args[0]));
+	if (stat5) {
+		return lbm_enc_float(stat5->v_in);
+	} else {
+		return lbm_enc_float(0.0);
+	}
+}
+
 static int cmp_int (const void * a, const void * b) {
 	return ( *(int*)a - *(int*)b );
 }
@@ -3839,6 +3849,7 @@ void lispif_load_vesc_extensions(void) {
 	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("canget-vin", ext_can_get_vin);
 
 	lbm_add_extension("can-list-devs", ext_can_list_devs);
 	lbm_add_extension("can-scan", ext_can_scan);