Steering zero

Core/Inc/can_handler.h

@@ -17,12 +17,16 @@
 #include "can.h"
 #include "cmsis_os.h"
 
-//void can1_callback(CAN_HandleTypeDef *hcan);
+void can1_callback(CAN_HandleTypeDef *hcan);
 
 void vCanDispatch(void *pv_params);
 extern osThreadId_t can_dispatch_handle;
 extern const osThreadAttr_t can_dispatch_attributes;
 
+void vCanReceive(void *pv_params);
+extern osThreadId_t can_receive_thread;
+extern const osThreadAttr_t can_receive_attributes;
+
 int8_t queue_can_msg(can_msg_t msg);
 void can1_init();
 

Core/Inc/monitor.h

@@ -5,6 +5,10 @@
 #include "stm32f4xx_hal.h"
 #include "msb_conf.h"
 
+#ifdef CAN_ENABLE // TODO change to wheel sensor flag  
+void record_wheel_angle(uint8_t *data);
+#endif
+
 #ifdef SENSOR_TEMP
 /* Defining Temperature Monitor Task */
 void vTempMonitor(void *pv_params);

Core/Inc/msb.h

@@ -38,12 +38,12 @@ int32_t imu_data_get(stmdev_ctx_t *ctx, stmdev_ctx_t *aux_ctx,
 		     lsm6dso_md_t *imu_md_temp, lsm6dso_data_t *imu_data_temp);
 
 #ifdef SENSOR_SHOCKPOT
-void shockpot_read(uint32_t shockpot_sense);
+void shockpot_read(uint32_t *shockpot_sense);
 #endif
 
 #ifdef SENSOR_STRAIN
-void strain1_read(uint32_t strain1);
-void strain2_read(uint32_t strain2);
+void strain1_read(uint32_t *strain1);
+void strain2_read(uint32_t *strain2);
 #endif
 
 #ifdef SENSOR_IMU
@@ -76,6 +76,7 @@ void strain2_read(uint32_t strain2);
 void motion_fx_init(void);
 void process_motion_fx(MFX_input_t *data_in, MFX_output_t *data_out,
 		       float delta_time);
+void imu_zero(uint8_t yaw_byte, uint8_t pitch_byte, uint8_t roll_byte);
 
 #endif
 

Core/Inc/msb_conf.h

@@ -5,8 +5,8 @@
 
 // MONITOR
 #define DELAY_TEMP_SENSOR_REFRESH 500
-#define DELAY_IMU_REFRESH	  50
-#define DELAY_SHOCKPOT_REFRESH	  500
+#define DELAY_IMU_REFRESH	  10
+#define DELAY_SHOCKPOT_REFRESH	  5
 #define DELAY_STRAIN_REFRESH	  500
 #define DELAY_TOF_REFRESH	  500
 #define DELAY_WHEEL_TEMP_REFRESH  500
@@ -17,30 +17,35 @@
 #define DELAY_CAN_DISPATCH 2
 
 // CAN IDS
-#define CANID_TEMP_SENSOR     0x602
-#define CANID_IMU_ACCEL	      0x603
-#define CANID_IMU_GYRO	      0x604
-#define CANID_STRAIN_SENSE    0x605
-#define CANID_SHOCK_SENSE     0x606
-#define CANID_TOF	      0x607
-#define CANID_WHEEL_TEMP      0x608
-#define CANID_IMU_ORIENTATION 0x609
+#define CANID_TEMP_SENSOR	 0x602
+#define CANID_IMU_ACCEL		 0x603
+#define CANID_IMU_GYRO		 0x604
+#define CANID_STRAIN_SENSE	 0x605
+#define CANID_SHOCK_SENSE	 0x606
+#define CANID_TOF		 0x607
+#define CANID_WHEEL_TEMP	 0x608
+#define CANID_IMU_ORIENTATION	 0x609
+#define CANID_IMUZERO_FRONTLEFT	 0x60A
+#define CANID_IMUZERO_FRONTRIGHT 0x60B
+#define CANID_IMUZERO_BACKLEFT	 0x60C
+#define CANID_IMUZERO_BACKRIGHT	 0x60E
+#define CANID_WHEEL_ANGLE	 0x630
 
 // Sensors to use, comment out to disable
 
 // internal
-// #define CAN_ENABLE
+#define CAN_ENABLE
 
 // on central
-#define SENSOR_TEMP	// SHT30
-#define SENSOR_SHOCKPOT // ADC1
-#define SENSOR_STRAIN	// ADC1
-#define SENSOR_TOF       // VL6180X
+//#define SENSOR_TEMP	// SHT30
+//#define SENSOR_SHOCKPOT // ADC1
+//#define SENSOR_STRAIN	// ADC1
+//#define SENSOR_TOF	// VL6180X
 
 #define SENSOR_IMU // LSM6DSO
 
 // VERBOSE LOGGING
-#define LOG_VERBOSE
+//#define LOG_VERBOSE
 
 // Note
 // STACK SIZES
@@ -50,5 +55,5 @@
 #define TEMP_MONITOR_STACK_SIZE	    64 * 16 // temp_monitor_handle
 #define IMU_MONITOR_STACK_SIZE	    128 * 32 // imu_monitor_handle
 #define TOF_MONITOR_STACK_SIZE	    128 * 8 // tof_monitor_handle
-#define SHOCKPOT_MONITOR_STACK_SIZE 64 * 8 // shockpot_monitor_handle
+#define SHOCKPOT_MONITOR_STACK_SIZE 128 * 8 // shockpot_monitor_handle
 #define STRAIN_MONITOR_STACK_SIZE   64 * 8 // strain_monitor_handle

Core/Src/can_handler.c

@@ -12,6 +12,8 @@
 #include "can_handler.h"
 #include "can.h"
 #include "msb_conf.h"
+#include "msb.h"
+#include "monitor.h"
 
 #include "stdio.h"
 #include <assert.h>
@@ -20,10 +22,14 @@
 
 #define CAN_MSG_QUEUE_SIZE 25 /* messages */
 static osMessageQueueId_t can_outbound_queue;
+static osMessageQueueId_t can_inbound_queue;
 
-static uint16_t id_list[4] = { 0x000, 0x000, 0x000, 0x002 };
+static uint16_t id_list[4] = {
+	0x000, CANID_WHEEL_ANGLE, 0x000, 0x002
+}; // id_list[0] is reserved for the IMU Zero CAN ID, which is added in can1_init().
 
 extern CAN_HandleTypeDef hcan1;
+extern device_loc_t device_loc;
 
 can_t *can1;
 
@@ -35,11 +41,58 @@ void can1_init()
 
 	can1->hcan = &hcan1;
 	assert(!can_init(can1));
+
+	/* Add the correct IMU Zero CAN ID to the filter depending on the location of the MSB. */
+	switch (device_loc) {
+	case DEVICE_FRONT_LEFT:
+		id_list[0] = CANID_IMUZERO_FRONTLEFT;
+		break;
+	case DEVICE_FRONT_RIGHT:
+		id_list[0] = CANID_IMUZERO_FRONTRIGHT;
+		break;
+	case DEVICE_BACK_LEFT:
+		id_list[0] = CANID_IMUZERO_BACKLEFT;
+		break;
+	case DEVICE_BACK_RIGHT:
+		id_list[0] = CANID_IMUZERO_BACKRIGHT;
+		break;
+	}
+
 	assert(!can_add_filter_standard(can1, id_list));
 #endif
 
 	can_outbound_queue =
 		osMessageQueueNew(CAN_MSG_QUEUE_SIZE, sizeof(can_msg_t), NULL);
+	can_inbound_queue =
+		osMessageQueueNew(CAN_MSG_QUEUE_SIZE, sizeof(can_msg_t), NULL);
+}
+
+/* Callback to be called when we get a CAN message */
+void can1_callback(CAN_HandleTypeDef *hcan)
+{
+	CAN_RxHeaderTypeDef rx_header;
+	can_msg_t new_msg;
+
+	/* Read in CAN message */
+	if (HAL_CAN_GetRxMessage(hcan, CAN_RX_FIFO0, &rx_header,
+				 new_msg.data) != HAL_OK) {
+		printf("Failed to receive CAN message.\n");
+		return;
+	}
+
+	new_msg.len = rx_header.DLC;
+
+	if (rx_header.IDE == CAN_ID_EXT) {
+		// If the message has an extended CAN ID, save the message accordingly.
+		new_msg.id = rx_header.ExtId;
+		new_msg.id_is_extended = true;
+	} else {
+		// If the message has a standard CAN ID, save the message accordingly.
+		new_msg.id = rx_header.StdId;
+		new_msg.id_is_extended = false;
+	}
+
+	osMessageQueuePut(can_inbound_queue, &new_msg, 0U, 0U);
 }
 
 osThreadId_t can_dispatch_handle;
@@ -62,6 +115,10 @@ void vCanDispatch(void *pv_params)
 					      &msg_from_queue, NULL,
 					      osWaitForever)) {
 #ifdef CAN_ENABLE
+while (HAL_CAN_GetTxMailboxesFreeLevel(&hcan1) == 0) {
+				osDelay(1);
+			}
+
 			msg_status = can_send_msg(can1, &msg_from_queue);
 			if (msg_status != HAL_OK) {
 				printf("Failed to send CAN message");
@@ -86,3 +143,34 @@ int8_t queue_can_msg(can_msg_t msg)
 	osMessageQueuePut(can_outbound_queue, &msg, 0U, 0U);
 	return 0;
 }
+
+osThreadId_t can_receive_thread;
+const osThreadAttr_t can_receive_attributes = {
+	.name = "CanProcessing",
+	.stack_size = 128 * 8,
+	.priority = (osPriority_t)osPriorityRealtime,
+};
+
+void vCanReceive(void *pv_params)
+{
+	can_msg_t msg;
+
+	for (;;) {
+		while (osOK ==
+		       osMessageQueueGet(can_inbound_queue, &msg, 0U, osWaitForever)) {
+			switch (msg.id) {
+			case CANID_IMUZERO_BACKLEFT:
+			case CANID_IMUZERO_BACKRIGHT:
+			case CANID_IMUZERO_FRONTLEFT:
+			case CANID_IMUZERO_FRONTRIGHT:
+				imu_zero(msg.data[0], msg.data[1], msg.data[2]);
+				break;
+			case CANID_WHEEL_ANGLE:
+				record_wheel_angle(msg.data);
+				break;
+			default:
+				break;
+			}
+		}
+	}
+}

Core/Src/main.c

@@ -121,7 +121,6 @@ int _write(int file, char* ptr, int len) {
   */
 int main(void)
 {
-
   /* USER CODE BEGIN 1 */
 
   /* USER CODE END 1 */
@@ -153,7 +152,7 @@ int main(void)
   MX_USART2_UART_Init();
   MX_CRC_Init();
   /* USER CODE BEGIN 2 */
-  printf("INIT MSB... FINDING ID");
+
   // determine MSB location
   bool loc1 = HAL_GPIO_ReadPin(Addr0_GPIO_Port, Addr0_Pin);
   bool loc2 = HAL_GPIO_ReadPin(Addr1_GPIO_Port, Addr1_Pin);
@@ -180,30 +179,6 @@ int main(void)
     device_loc = DEVICE_FRONT_LEFT;
   }
   HAL_Delay(500);
-  uint8_t devices = 0u;
-  printf("Searching for I2C devices on the bus...\n");
-  /* Values outside 0x03 and 0x77 are invalid. */
-  for (uint8_t i = 0x03u; i < 0x78u; i++)
-  {
-    HAL_IWDG_Refresh(&hiwdg);
-    uint8_t address = i << 1u ;
-    /* In case there is a positive feedback, print it out. */
-    if (HAL_OK == HAL_I2C_IsDeviceReady(&hi2c3, address, 3u, 10u))
-    {
-      printf("Device found: 0x%02X\n", address);
-      devices++;
-    }
-  }
-  /* Feedback of the total number of devices. */
-  if (0u == devices)
-  {
-    printf("No device found.\n");
-  }
-  else
-  {
-    printf("Total found devices: %d\n", devices);
-  }
-
 
   /* USER CODE END 2 */
 
@@ -216,7 +191,7 @@ int main(void)
   // determine the configuration of the device
 
   msb_init();
-  vcc5_en_write(true);
+  vcc5_en_write(false);
   can1_init();
 
   /* USER CODE END RTOS_MUTEX */
@@ -233,6 +208,9 @@ int main(void)
   /* add queues, ... */
   can_dispatch_handle = osThreadNew(vCanDispatch, NULL, &can_dispatch_attributes);
   assert(can_dispatch_handle);
+
+  can_receive_thread = osThreadNew(vCanReceive, NULL, &can_receive_attributes);
+  assert(can_receive_thread);
   /* USER CODE END RTOS_QUEUES */
 
   /* Create the thread(s) */
@@ -329,7 +307,7 @@ void SystemClock_Config(void)
   RCC_ClkInitStruct.SYSCLKSource = RCC_SYSCLKSOURCE_PLLCLK;
   RCC_ClkInitStruct.AHBCLKDivider = RCC_SYSCLK_DIV1;
   RCC_ClkInitStruct.APB1CLKDivider = RCC_HCLK_DIV2;
-  RCC_ClkInitStruct.APB2CLKDivider = RCC_HCLK_DIV1;
+  RCC_ClkInitStruct.APB2CLKDivider = RCC_HCLK_DIV4;
 
   if (HAL_RCC_ClockConfig(&RCC_ClkInitStruct, FLASH_LATENCY_2) != HAL_OK)
   {
@@ -358,7 +336,7 @@ static void MX_ADC1_Init(void)
   /** Configure the global features of the ADC (Clock, Resolution, Data Alignment and number of conversion)
   */
   hadc1.Instance = ADC1;
-  hadc1.Init.ClockPrescaler = ADC_CLOCK_SYNC_PCLK_DIV6;
+  hadc1.Init.ClockPrescaler = ADC_CLOCK_SYNC_PCLK_DIV8;
   hadc1.Init.Resolution = ADC_RESOLUTION_12B;
   hadc1.Init.ScanConvMode = ENABLE;
   hadc1.Init.ContinuousConvMode = ENABLE;
@@ -368,7 +346,7 @@ static void MX_ADC1_Init(void)
   hadc1.Init.DataAlign = ADC_DATAALIGN_RIGHT;
   hadc1.Init.NbrOfConversion = 3;
   hadc1.Init.DMAContinuousRequests = ENABLE;
-  hadc1.Init.EOCSelection = ADC_EOC_SINGLE_CONV;
+  hadc1.Init.EOCSelection = ADC_EOC_SEQ_CONV;
   if (HAL_ADC_Init(&hadc1) != HAL_OK)
   {
     Error_Handler();