From 6ace516b3f79466d21f8fbb92b6d4a114e15389e Mon Sep 17 00:00:00 2001 From: Benjamin Vedder Date: Sun, 13 Mar 2022 20:56:26 +0100 Subject: [PATCH] Added CAN status message 6 with ADC and PPM values --- comm_can.c | 76 +++++++++++++++++++++++++++++++++ comm_can.h | 3 ++ conf_general.h | 2 +- confgenerator.h | 2 +- datatypes.h | 10 +++++ lispBM/README.md | 14 ++++++ lispBM/lispif_vesc_extensions.c | 43 +++++++++++++++++++ terminal.c | 25 +++++++---- 8 files changed, 165 insertions(+), 10 deletions(-) diff --git a/comm_can.c b/comm_can.c index a7e61b99..eba6918f 100644 --- a/comm_can.c +++ b/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]; diff --git a/comm_can.h b/comm_can.h index 7b6b0430..a11ddbb0 100644 --- a/comm_can.h +++ b/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_ */ diff --git a/conf_general.h b/conf_general.h index 8b8a1a36..8db6b008 100755 --- a/conf_general.h +++ b/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" diff --git a/confgenerator.h b/confgenerator.h index 375e76ea..974c2f2a 100644 --- a/confgenerator.h +++ b/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); diff --git a/datatypes.h b/datatypes.h index 22a37c88..7c145ef3 100644 --- a/datatypes.h +++ b/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; diff --git a/lispBM/README.md b/lispBM/README.md index a175d23d..72558367 100644 --- a/lispBM/README.md +++ b/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) diff --git a/lispBM/lispif_vesc_extensions.c b/lispBM/lispif_vesc_extensions.c index 009acc1a..141451f7 100644 --- a/lispBM/lispif_vesc_extensions.c +++ b/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); diff --git a/terminal.c b/terminal.c index 9956471a..37a056e2 100644 --- a/terminal.c +++ b/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);