diff --git a/Core/Src/BQ_Communication.c b/Core/Src/BQ_Communication.c
index 4528947..4f92376 100644
--- a/Core/Src/BQ_Communication.c
+++ b/Core/Src/BQ_Communication.c
@@ -149,6 +149,7 @@ uint8_t BQ_Read_Register(uint8_t registeraddress, uint8_t registersize,
   message[0] = FRM_WRT_R;
   message[1] = 0;
   message[2] = registeraddress;
+  // See BQ datasheet 7.5.3.1.2
   message[3] = registersize - 1;
   Calculate_CRC(message, 6);
 
@@ -162,14 +163,21 @@ uint8_t BQ_Read_Register(uint8_t registeraddress, uint8_t registersize,
   if (crccheck == 0) {
     return 0;
   }
+  // See Response Frame Init in BQ datasheet 7.5.1.1
+  uint8_t response_size = (recv_message[0] & 0x7F) + 1;
+  if (response_size != registersize) {
+    return 0;
+  }
 
   data[0] = 0;
 
   if (registersize > 4)
     registersize = 4;
 
+  // Data is returned big-endian as per 7.5.3.1.2.1
   for (int n = 0; n < registersize; n++) {
-    data[0] |= recv_message[1 + n] >> (8 * n);
+    int shift_amount = 8 * (registersize - n - 1);
+    data[0] |= recv_message[1 + n] << shift_amount;
   }
 
   return 1;
@@ -195,7 +203,7 @@ uint32_t BQ_ReadMeasurements(uint8_t* buffer, uint8_t bufferlength) {
   uint8_t recv_buf[recv_len];
   uint8_t uartstat = BQ_UART_Receive(recv_buf, recv_len);
 
-  if (Check_CRC(recv_buf, recv_len) == 0) {
+  if (uartstat != HAL_OK || Check_CRC(recv_buf, recv_len) == 0) {
     return 0;
   }
 
diff --git a/Core/Src/main.c b/Core/Src/main.c
index f5c999a..d04373c 100644
--- a/Core/Src/main.c
+++ b/Core/Src/main.c
@@ -21,7 +21,9 @@
 
 /* Private includes ----------------------------------------------------------*/
 /* USER CODE BEGIN Includes */
+#include "BQ_Abstraction_Layer.h"
 
+#include "stm32f4xx_hal.h"
 /* USER CODE END Includes */
 
 /* Private typedef -----------------------------------------------------------*/
@@ -108,7 +110,9 @@ int main(void) {
   MX_USART3_UART_Init();
   MX_USART6_UART_Init();
   /* USER CODE BEGIN 2 */
-
+  HAL_GPIO_WritePin(DCDC_CTRL_GPIO_Port, DCDC_CTRL_Pin, GPIO_PIN_SET);
+  HAL_Delay(100);
+  afe_init(&huart2);
   /* USER CODE END 2 */
 
   /* Infinite loop */
@@ -117,6 +121,7 @@ int main(void) {
     /* USER CODE END WHILE */
 
     /* USER CODE BEGIN 3 */
+    afe_measure();
   }
   /* USER CODE END 3 */
 }