diff --git a/Core/Inc/AMS_CAN.h b/Core/Inc/AMS_CAN.h
index 5bc3e50..77c9b7a 100644
--- a/Core/Inc/AMS_CAN.h
+++ b/Core/Inc/AMS_CAN.h
@@ -18,7 +18,7 @@
 
 #define CAN_ID_SLAVE_ERROR 0x001
 #define CAN_ID_AMS_SLAVE_HEARTBEAT_BASE 0x600
-#define CAN_HEARTBEAT_TX_TIMEOUT 5 /* ms */
+#define CAN_HEARTBEAT_TX_TIMEOUT 10 /* ms */
 
 void ams_can_init(CAN_HandleTypeDef* ams, CAN_HandleTypeDef* car);
 
diff --git a/Core/Inc/BQ_Abstraction_Layer.h b/Core/Inc/BQ_Abstraction_Layer.h
index e286246..2204f3c 100644
--- a/Core/Inc/BQ_Abstraction_Layer.h
+++ b/Core/Inc/BQ_Abstraction_Layer.h
@@ -19,6 +19,8 @@
 #define CELL_OV_THRESHOLD 55100 // 4.2V
 #define CELL_UV_THRESHOLD 34100 // 2.6V
 
+#define BQ_MEASUREMENT_INTERVAL 250 // ms
+
 extern uint16_t cell_voltages[N_CELLS];
 extern uint16_t max_voltage, min_voltage;
 
diff --git a/Core/Inc/main.h b/Core/Inc/main.h
index 44e1912..df641e7 100644
--- a/Core/Inc/main.h
+++ b/Core/Inc/main.h
@@ -54,6 +54,7 @@ extern uint8_t slave_id;
 /* Exported macro ------------------------------------------------------------*/
 /* USER CODE BEGIN EM */
 #define MAIN_LOOP_PERIOD 100   /* ms */
+#define ERROR_CHECK_START 1000 /* ms */
 #define AMS_ERROR_TX_TIMEOUT 2 /* ms */
 #define THRESH_UV 32768        /* 2.5V */
 #define THRESH_OV 55050        /* 4.2V */
diff --git a/Core/Src/AMS_CAN.c b/Core/Src/AMS_CAN.c
index 8a8ce16..e24fcb3 100644
--- a/Core/Src/AMS_CAN.c
+++ b/Core/Src/AMS_CAN.c
@@ -104,8 +104,8 @@ void ams_can_send_heartbeat() {
       data[2 * i + 0] = v & 0xFF;
       data[2 * i + 1] = v >> 8;
     }
-    if (ams_can_wait_for_free_mailbox(handle_ams, CAN_HEARTBEAT_TX_TIMEOUT) ==
-        HAL_OK) {
+    if (ams_can_wait_for_free_mailboxes(handle_ams, 1,
+                                        CAN_HEARTBEAT_TX_TIMEOUT) == HAL_OK) {
       uint32_t mailbox;
       HAL_CAN_AddTxMessage(handle_ams, &header, data, &mailbox);
     }
@@ -121,8 +121,8 @@ void ams_can_send_heartbeat() {
       data[2 * i + 0] = temp & 0xFF;
       data[2 * i + 1] = temp >> 8;
     }
-    if (ams_can_wait_for_free_mailbox(handle_ams, CAN_HEARTBEAT_TX_TIMEOUT) ==
-        HAL_OK) {
+    if (ams_can_wait_for_free_mailboxes(handle_ams, 1,
+                                        CAN_HEARTBEAT_TX_TIMEOUT) == HAL_OK) {
       uint32_t mailbox;
       HAL_CAN_AddTxMessage(handle_ams, &header, data, &mailbox);
     }
diff --git a/Core/Src/BQ_Abstraction_Layer.c b/Core/Src/BQ_Abstraction_Layer.c
index 5ee6ade..bd6ac4b 100644
--- a/Core/Src/BQ_Abstraction_Layer.c
+++ b/Core/Src/BQ_Abstraction_Layer.c
@@ -66,6 +66,7 @@ void afe_init(UART_HandleTypeDef* uarthandle) {
   HAL_Delay(100);
 
   afe_check_faults();
+  lastmeasurementtime = 0;
 }
 
 void afe_shutdown() { BQ_Write_Register(DEV_CNTRL, DEV_CNTRL_SIZE, 0x40); }
@@ -77,8 +78,14 @@ void afe_wakeup() {
 }
 
 void afe_measure() {
+  if (HAL_GetTick() - lastmeasurementtime < BQ_MEASUREMENT_INTERVAL) {
+    return;
+  }
+
+  static uint32_t n_measures = 0;
   uint8_t cellvoltagebuffer[2 * N_CELLS];
   BQ_Comm_Status retval = BQ_ReadMeasurements(cellvoltagebuffer, 2 * N_CELLS);
+  n_measures++;
 
   lastmeasurementtime = HAL_GetTick();
 
@@ -98,6 +105,12 @@ void afe_measure() {
     max_voltage = max;
     min_voltage = min;
   } else if (retval == BQ_COMM_ERR_HAL) {
+    if (n_measures < 3) {
+      // There's always a timeout on the second measure for some reason. We can
+      // safely ignore it here, since if it's an actual issue it will happen
+      // again on the next measure.
+      return;
+    }
     bq_set_error_with_loc(BQ_COMM_ERR_HAL, BQ_ERROR_LOC_MEASURE);
   } else if (retval == BQ_COMM_ERR_CRC) {
     bq_set_error_with_loc(BQ_COMM_ERR_CRC, BQ_ERROR_LOC_MEASURE);
diff --git a/Core/Src/BQ_Communication.c b/Core/Src/BQ_Communication.c
index bfef417..b959af1 100644
--- a/Core/Src/BQ_Communication.c
+++ b/Core/Src/BQ_Communication.c
@@ -145,6 +145,7 @@ uint8_t BQ_Read_Register(uint8_t registeraddress, uint8_t registersize,
 }
 
 BQ_Comm_Status BQ_ReadMeasurements(uint8_t* buffer, uint8_t bufferlength) {
+  static uint32_t n_measurements = 0;
   uint8_t message[6] = {};
   message[0] = FRM_WRT_R;
   message[1] = 0x00;
@@ -157,6 +158,7 @@ BQ_Comm_Status BQ_ReadMeasurements(uint8_t* buffer, uint8_t bufferlength) {
   uint16_t recv_len = 2 * (numofcells + numofdietemps) + 3;
   uint8_t recv_buf[recv_len];
   uint8_t uartstat = BQ_UART_Receive(recv_buf, recv_len);
+  n_measurements++;
 
   if (uartstat != HAL_OK) {
     return BQ_COMM_ERR_HAL;
diff --git a/Core/Src/TMP144.c b/Core/Src/TMP144.c
index fd5101f..eeb46ed 100644
--- a/Core/Src/TMP144.c
+++ b/Core/Src/TMP144.c
@@ -45,7 +45,7 @@ HAL_StatusTypeDef tmp144_init(UART_HandleTypeDef* busbar_side,
   tmp144_bus_other.n_sensors = N_TEMP_SENSORS / 2;
 
   CHECK_STATUS(tmp144_init_reset(&tmp144_bus_busbar));
-  CHECK_STATUS(tmp144_init_reset(&tmp144_bus_other));
+  // CHECK_STATUS(tmp144_init_reset(&tmp144_bus_other));
 
   return HAL_OK;
 }
@@ -57,9 +57,10 @@ HAL_StatusTypeDef tmp144_check_timeouts() {
   if (t1 != 0 && t1 < now) {
     tmp144_bus_busbar.state = TMP144_ERROR;
   }
-  if (t2 != 0 && t2 < now) {
-    tmp144_bus_other.state = TMP144_ERROR;
-  }
+  // FIXME
+  // if (t2 != 0 && t2 < now) {
+  //   tmp144_bus_other.state = TMP144_ERROR;
+  // }
   return HAL_OK;
 }
 
@@ -170,7 +171,8 @@ HAL_StatusTypeDef tmp144_recv_temps(TMP144Bus* bus) {
 
   uint16_t max = temperatures[0];
   uint16_t min = temperatures[0];
-  for (size_t i = 1; i < N_TEMP_SENSORS; i++) {
+  // FIXME
+  for (size_t i = 1; i < 16; i++) {
     if (temperatures[i] > max) {
       max = temperatures[i];
     }
diff --git a/Core/Src/main.c b/Core/Src/main.c
index e50db88..2dbe8db 100644
--- a/Core/Src/main.c
+++ b/Core/Src/main.c
@@ -32,6 +32,8 @@
 #include "stm32f4xx_hal_gpio.h"
 #include "stm32f4xx_hal_tim.h"
 #include "stm32f4xx_hal_uart.h"
+
+#include <stdint.h>
 /* USER CODE END Includes */
 
 /* Private typedef -----------------------------------------------------------*/
@@ -129,6 +131,9 @@ void check_error_conditions() {
   }
 
   if (error) {
+    HAL_GPIO_WritePin(STAT_LED4_GPIO_Port, STAT_LED4_Pin, GPIO_PIN_SET);
+  } else {
+    HAL_GPIO_WritePin(STAT_LED4_GPIO_Port, STAT_LED4_Pin, GPIO_PIN_RESET);
   }
 }
 
@@ -136,11 +141,12 @@ void delay_period() {
   static uint32_t last_it = 0;
   uint32_t now = HAL_GetTick();
   int32_t diff = now - last_it;
-  if (diff > 100) {
+  if (diff > MAIN_LOOP_PERIOD) {
     HAL_GPIO_WritePin(STAT_LED3_GPIO_Port, STAT_LED3_Pin, GPIO_PIN_SET);
   } else {
     HAL_GPIO_WritePin(STAT_LED3_GPIO_Port, STAT_LED3_Pin, GPIO_PIN_RESET);
-    HAL_Delay(100 - diff);
+    HAL_Delay(MAIN_LOOP_PERIOD - diff);
+    now = HAL_GetTick();
   }
   last_it = now;
 }
@@ -196,6 +202,7 @@ int main(void) {
 
   /* Infinite loop */
   /* USER CODE BEGIN WHILE */
+  uint32_t main_loop_start = HAL_GetTick();
   while (1) {
     /* USER CODE END WHILE */
 
@@ -203,8 +210,11 @@ int main(void) {
     update_status_leds();
     afe_measure();
     tmp144_read_temps();
-    check_error_conditions();
+    if (HAL_GetTick() - main_loop_start < ERROR_CHECK_START) {
+      check_error_conditions();
+    }
     fan_ctrl_update();
+    ams_can_send_heartbeat();
     delay_period();
   }
   /* USER CODE END 3 */
diff --git a/STM32Make.make b/STM32Make.make
index 350ab2a..5d6bcb7 100644
--- a/STM32Make.make
+++ b/STM32Make.make
@@ -154,7 +154,8 @@ CFLAGS = $(MCU) $(C_DEFS) $(C_INCLUDES) $(OPT) -Wall -fdata-sections -ffunction-
 CXXFLAGS = $(MCU) $(CXX_DEFS) $(C_INCLUDES) $(OPT) -Wall -fdata-sections -ffunction-sections -feliminate-unused-debug-types
 
 ifeq ($(DEBUG), 1)
-CFLAGS += -g -gdwarf-2
+CFLAGS += -g -gdwarf -ggdb
+CXXFLAGS += -g -gdwarf -ggdb
 endif
 
 # Add additional flags
@@ -242,6 +243,12 @@ erase: $(BUILD_DIR)/$(TARGET).elf
 #######################################
 clean:
 	-rm -fR $(BUILD_DIR)
+
+#######################################
+# custom makefile rules
+#######################################
+
+
 	
 #######################################
 # dependencies