diff --git a/RTOS_Dual_Sensor_Single_Board_Testing/Core/Src/main.c b/RTOS_Dual_Sensor_Single_Board_Testing/Core/Src/main.c
index 412fba7..de7e78f 100644
--- a/RTOS_Dual_Sensor_Single_Board_Testing/Core/Src/main.c
+++ b/RTOS_Dual_Sensor_Single_Board_Testing/Core/Src/main.c
@@ -53,10 +53,17 @@ DMA_HandleTypeDef hdma_i2c1_rx;
 UART_HandleTypeDef huart2;
 
 osThreadId defaultTaskHandle;
+osThreadId SensorReadingHandle;
+osThreadId Sensor2ReadingHandle;
 /* USER CODE BEGIN PV */
 MPU6050_HandleTypeDef hmpu1;
+MPU6050_HandleTypeDef hmpu2;
 MadgwickFilter madgwick;
 
+// sensor reading flags
+uint8_t sensor1_flag = 0;
+uint8_t sensor2_flag = 0;
+
 
 char output[128];
 int16_t copy_raw_data[6];			//[ax, ay, az, gx, gy, gz]
@@ -78,6 +85,8 @@ static void MX_DMA_Init(void);
 static void MX_USART2_UART_Init(void);
 static void MX_I2C1_Init(void);
 void StartDefaultTask(void const * argument);
+void StartSensorReading(void const * argument);
+void StartSensor2Reading(void const * argument);
 
 /* USER CODE BEGIN PFP */
 
@@ -149,6 +158,14 @@ int main(void)
   osThreadDef(defaultTask, StartDefaultTask, osPriorityNormal, 0, 1024);
   defaultTaskHandle = osThreadCreate(osThread(defaultTask), NULL);
 
+  /* definition and creation of SensorReading */
+  osThreadDef(SensorReading, StartSensorReading, osPriorityIdle, 0, 1024);
+  SensorReadingHandle = osThreadCreate(osThread(SensorReading), NULL);
+
+  /* definition and creation of Sensor2Reading */
+  osThreadDef(Sensor2Reading, StartSensor2Reading, osPriorityNormal, 0, 1024);
+  Sensor2ReadingHandle = osThreadCreate(osThread(Sensor2Reading), NULL);
+
   /* USER CODE BEGIN RTOS_THREADS */
   /* add threads, ... */
   /* USER CODE END RTOS_THREADS */
@@ -432,6 +449,83 @@ void StartDefaultTask(void const * argument)
   /* USER CODE END 5 */
 }
 
+/* USER CODE BEGIN Header_StartSensorReading */
+/**
+* @brief Function implementing the SensorReading thread.
+* @param argument: Not used
+* @retval None
+*/
+/* USER CODE END Header_StartSensorReading */
+void StartSensorReading(void const * argument)
+{
+  /* USER CODE BEGIN StartSensorReading */
+	// set the sensor struct
+	hmpu1.address = GYR1_Add;
+	hmpu1.hi2c = &hi2c1;
+	hmpu1.accel_range = 0x02;
+	hmpu1.gyro_range = 0x02;
+	hmpu1.accel_factor = 4096.0;
+	hmpu1.gyro_factor = 32.8;
+	hmpu1.calibrate = 0;
+
+	hmpu2.address = GYR2_Add;
+	hmpu2.hi2c = &hi2c1;
+	hmpu2.accel_range = 0x02;
+	hmpu2.gyro_range = 0x02;
+	hmpu2.accel_factor = 4096.0;
+	hmpu2.gyro_factor = 32.8;
+	hmpu2.calibrate = 0;
+	// initialize and calibrate both sensors
+	Init_MPU6050(&hmpu1, &huart2);
+	Calibrate_MPU(&hmpu1, &huart2);
+
+	Init_MPU6050(&hmpu2, &huart2);
+	Calibrate_MPU(&hmpu2, &huart2);
+
+	// hold the sensor data
+	uint8_t binarySensorData[14];
+	int16_t rawData[6];
+
+  /* Infinite loop */
+  for(;;)
+  {
+
+    osDelay(1);
+  }
+  /* USER CODE END StartSensorReading */
+}
+
+/* USER CODE BEGIN Header_StartSensor2Reading */
+/**
+* @brief Function implementing the Sensor2Reading thread.
+* @param argument: Not used
+* @retval None
+*/
+/* USER CODE END Header_StartSensor2Reading */
+void StartSensor2Reading(void const * argument)
+{
+  /* USER CODE BEGIN StartSensor2Reading */
+	// Initialize MPU struct
+	hmpu2.address = GYR2_Add;
+	hmpu2.hi2c = &hi2c1;
+	hmpu2.accel_range = 0x02;
+	hmpu2.gyro_range = 0x02;
+	hmpu2.accel_factor = 4096.0;
+	hmpu2.gyro_factor = 32.8;
+	hmpu2.calibrate = 0;
+
+	// init, calibrate and start mpu
+	Init_MPU6050(&hmpu2, &huart2);
+	Calibrate_MPU(&hmpu2, &huart2);
+	start_dma_transfer(&hmpu2);
+  /* Infinite loop */
+  for(;;)
+  {
+    osDelay(1);
+  }
+  /* USER CODE END StartSensor2Reading */
+}
+
 /**
   * @brief  Period elapsed callback in non blocking mode
   * @note   This function is called  when TIM6 interrupt took place, inside
diff --git a/RTOS_Dual_Sensor_Single_Board_Testing/RTOS_Dual_Sensor_Single_Board_Testing.ioc b/RTOS_Dual_Sensor_Single_Board_Testing/RTOS_Dual_Sensor_Single_Board_Testing.ioc
index 2e5f14b..4688ac6 100644
--- a/RTOS_Dual_Sensor_Single_Board_Testing/RTOS_Dual_Sensor_Single_Board_Testing.ioc
+++ b/RTOS_Dual_Sensor_Single_Board_Testing/RTOS_Dual_Sensor_Single_Board_Testing.ioc
@@ -14,8 +14,9 @@ Dma.I2C1_RX.0.Priority=DMA_PRIORITY_LOW
 Dma.I2C1_RX.0.RequestParameters=Instance,Direction,PeriphInc,MemInc,PeriphDataAlignment,MemDataAlignment,Mode,Priority,FIFOMode
 Dma.Request0=I2C1_RX
 Dma.RequestsNb=1
-FREERTOS.IPParameters=Tasks01,configUSE_IDLE_HOOK,configENABLE_FPU
-FREERTOS.Tasks01=defaultTask,0,1024,StartDefaultTask,Default,NULL,Dynamic,NULL,NULL
+FREERTOS.FootprintOK=true
+FREERTOS.IPParameters=Tasks01,configUSE_IDLE_HOOK,configENABLE_FPU,FootprintOK
+FREERTOS.Tasks01=defaultTask,0,1024,StartDefaultTask,Default,NULL,Dynamic,NULL,NULL;SensorReading,-3,1024,StartSensorReading,Default,NULL,Dynamic,NULL,NULL;Sensor2Reading,0,1024,StartSensor2Reading,Default,NULL,Dynamic,NULL,NULL
 FREERTOS.configENABLE_FPU=1
 FREERTOS.configUSE_IDLE_HOOK=0
 File.Version=6