Fixed CAN reply mapping problem

Author: vedderb

comm_can.c

@@ -1230,7 +1230,7 @@ static void decode_msg(uint32_t eid, uint8_t *data8, int len, bool is_replaced)
 					commands_process_packet(rx_buffer, rxbuf_len, send_packet_wrapper);
 					break;
 				case 1:
-					commands_send_packet(rx_buffer, rxbuf_len);
+					commands_send_packet_can_last(rx_buffer, rxbuf_len);
 					break;
 				case 2:
 					commands_process_packet(rx_buffer, rxbuf_len, 0);
@@ -1261,7 +1261,7 @@ static void decode_msg(uint32_t eid, uint8_t *data8, int len, bool is_replaced)
 				commands_process_packet(data8 + ind, len - ind, send_packet_wrapper);
 				break;
 			case 1:
-				commands_send_packet(data8 + ind, len - ind);
+				commands_send_packet_can_last(data8 + ind, len - ind);
 				break;
 			case 2:
 				commands_process_packet(data8 + ind, len - ind, 0);

commands.c

@@ -68,6 +68,7 @@ static volatile int blocking_thread_motor = 1;
 static void(* volatile send_func)(unsigned char *data, unsigned int len) = 0;
 static void(* volatile send_func_blocking)(unsigned char *data, unsigned int len) = 0;
 static void(* volatile send_func_nrf)(unsigned char *data, unsigned int len) = 0;
+static void(* volatile send_func_can_fwd)(unsigned char *data, unsigned int len) = 0;
 static void(* volatile appdata_func)(unsigned char *data, unsigned int len) = 0;
 static disp_pos_mode display_position_mode;
 static mutex_t print_mutex;
@@ -97,6 +98,21 @@ void commands_send_packet(unsigned char *data, unsigned int len) {
 	}
 }
 
+/**
+ * Send a packet using the last can fwd function.
+ *
+ * @param data
+ * The packet data.
+ *
+ * @param len
+ * The data length.
+ */
+void commands_send_packet_can_last(unsigned char *data, unsigned int len) {
+	if (send_func_can_fwd) {
+		send_func_can_fwd(data, len);
+	}
+}
+
 /**
  * Send a packet using the set NRF51 send function. The NRF51 send function
  * is set when the COMM_EXT_NRF_PRESENT and COMM_EXT_NRF_ESB_RX_DATA commands
@@ -602,6 +618,8 @@ void commands_process_packet(unsigned char *data, unsigned int len,
 	} break;
 
 	case COMM_FORWARD_CAN: {
+		send_func_can_fwd = reply_func;
+
 #ifdef HW_HAS_DUAL_MOTORS
 		if (data[0] == utils_second_motor_id()) {
 			mc_interface_select_motor_thread(2);

commands.h

@@ -25,6 +25,7 @@
 // Functions
 void commands_init(void);
 void commands_send_packet(unsigned char *data, unsigned int len);
+void commands_send_packet_can_last(unsigned char *data, unsigned int len);
 void commands_send_packet_nrf(unsigned char *data, unsigned int len);
 void commands_send_packet_last_blocking(unsigned char *data, unsigned int len);
 void commands_process_packet(unsigned char *data, unsigned int len,