diff --git a/bms.c b/bms.c
index a32010da..314b3708 100644
--- a/bms.c
+++ b/bms.c
@@ -29,6 +29,8 @@
 #include "utils.h"
 #include "datatypes.h"
 #include "comm_can.h"
+#include "commands.h"
+#include "comm_usb.h"
 #include <string.h>
 #include <math.h>
 
@@ -63,6 +65,42 @@ bool bms_process_can_frame(uint32_t can_id, uint8_t *data8, int len, bool is_ext
 			uint8_t id = can_id & 0xFF;
 			CAN_PACKET_ID cmd = can_id >> 8;
 
+			switch (cmd) {
+			case CAN_PACKET_BMS_SOC_SOH_TEMP_STAT:
+			case CAN_PACKET_BMS_V_TOT:
+			case CAN_PACKET_BMS_I:
+			case CAN_PACKET_BMS_AH_WH:
+			case CAN_PACKET_BMS_V_CELL:
+			case CAN_PACKET_BMS_BAL:
+			case CAN_PACKET_BMS_TEMPS:
+			case CAN_PACKET_BMS_HUM:
+			{
+				unsigned char fwd_data[11];
+				unsigned int fwd_len = 0;
+				fwd_data[fwd_len++] = COMM_BMS_FWD_CAN_RX;
+				fwd_data[fwd_len++] = id;
+				fwd_data[fwd_len++] = cmd;
+				memcpy(fwd_data + fwd_len, data8, len);
+				fwd_len += len;
+
+				switch (m_conf.fwd_can_mode) {
+				case BMS_FWD_CAN_MODE_DISABLED:
+					break;
+
+				case BMS_FWD_CAN_MODE_USB_ONLY:
+					comm_usb_send_packet(fwd_data, fwd_len);
+					break;
+
+				case BMS_FWD_CAN_MODE_ANY:
+					commands_send_packet(fwd_data, fwd_len);
+					break;
+				}
+
+				default:
+					break;
+			}
+			}
+
 			switch (cmd) {
 			case CAN_PACKET_BMS_SOC_SOH_TEMP_STAT: {
 				used_data = true;
diff --git a/datatypes.h b/datatypes.h
index 291d2dce..37ce1611 100644
--- a/datatypes.h
+++ b/datatypes.h
@@ -59,7 +59,8 @@ typedef enum {
 	FOC_SENSOR_MODE_SENSORLESS = 0,
 	FOC_SENSOR_MODE_ENCODER,
 	FOC_SENSOR_MODE_HALL,
-	FOC_SENSOR_MODE_HFI
+	FOC_SENSOR_MODE_HFI,
+	FOC_SENSOR_MODE_HFI_START
 } mc_foc_sensor_mode;
 
 // Auxiliary output mode
@@ -228,12 +229,19 @@ typedef enum {
 	BMS_TYPE_VESC
 } BMS_TYPE;
 
+typedef enum {
+	BMS_FWD_CAN_MODE_DISABLED = 0,
+	BMS_FWD_CAN_MODE_USB_ONLY,
+	BMS_FWD_CAN_MODE_ANY
+} BMS_FWD_CAN_MODE;
+
 typedef struct {
 	BMS_TYPE type;
 	float t_limit_start;
 	float t_limit_end;
 	float soc_limit_start;
 	float soc_limit_end;
+	BMS_FWD_CAN_MODE fwd_can_mode;
 } bms_config;
 
 typedef struct {
@@ -934,6 +942,8 @@ typedef enum {
 	// Power switch commands
 	COMM_PSW_GET_STATUS,
 	COMM_PSW_SWITCH,
+
+	COMM_BMS_FWD_CAN_RX,
 } COMM_PACKET_ID;
 
 // CAN commands
diff --git a/mcconf/mcconf_default.h b/mcconf/mcconf_default.h
index 43f5654a..7aee5ffc 100644
--- a/mcconf/mcconf_default.h
+++ b/mcconf/mcconf_default.h
@@ -369,7 +369,7 @@
 #define MCCONF_FOC_SL_ERPM_HFI			2000.0	// ERPM above which only the observer is used
 #endif
 #ifndef MCCONF_FOC_HFI_START_SAMPLES
-#define MCCONF_FOC_HFI_START_SAMPLES	65 // Sample this often at start to resolve ambiguity
+#define MCCONF_FOC_HFI_START_SAMPLES	15 // Sample this often at start to resolve ambiguity
 #endif
 #ifndef MCCONF_FOC_HFI_OBS_OVR_SEC
 #define MCCONF_FOC_HFI_OBS_OVR_SEC		0.001 // Continue using observer for this long when entering HFI speed
@@ -532,5 +532,8 @@
 #ifndef MCCONF_BMS_SOC_LIMIT_END
 #define MCCONF_BMS_SOC_LIMIT_END		0
 #endif
+#ifndef MCCONF_BMS_FWD_CAN_MODE
+#define MCCONF_BMS_FWD_CAN_MODE			BMS_FWD_CAN_MODE_DISABLED
+#endif
 
 #endif /* MCCONF_DEFAULT_H_ */