diff --git a/firmware/batmon.c b/firmware/batmon.c
index 7f9be9f..aafc715 100644
--- a/firmware/batmon.c
+++ b/firmware/batmon.c
@@ -1,11 +1,18 @@
 #include <avr/io.h>
 #include "batmon.h"
+#include "systick.h"
+
+static uint8_t vbat = 0;
 
 void batmon_init()
 {
+	TCCR0A = 0;
+	TCCR0B = (1 << CS02);
+	TIMSK0 = (1 << TOIE0);
+
 	ADCSRA = (1 << ADPS2) | (1 << ADPS1); // 125 kHz ADC clock
 	ADMUX = ADC_CHANNEL | (1 << REFS0) | (1 << REFS1) | (1 << ADLAR); // Internal 1.2V bandgap, left adj.
-	ADCSRB = 0; // Manual start
+	ADCSRB = 0;
 #if ADC_CHANNEL < 7
 	DIDR0 = (1 << ADC_CHANNEL); // Disconnect digital channel
 #endif
@@ -16,10 +23,30 @@ void batmon_init()
 
 	// Enable ADC
 	ADCSRA |= (1 << ADEN);
+	ADCSRA |= (1 << ADSC);
+}
+
+uint8_t batmon_proc()
+{
+	static uint16_t sum = 0;
+	static uint8_t cnt = 0;
+
+	if (ADCSRA & (1 << ADIF)) {
+		sum += ADCH;
+		if (!(cnt++)) {
+			vbat = sum >> 8;
+			sum = 0;
+		}
+		vbat = ADCH;
+		ADCSRA &= ~(1 << ADIF);
+		ADCSRA |= (1 << ADSC); // TODO: I want this to work off TIMER0
+		if (vbat < BAT_THRES_POWER_OFF)
+			return 1;
+	}
+	return 0;
 }
 
 inline uint8_t batmon_get_voltage()
 {
-	ADCSRA |= (1 << ADSC); // TODO: Check whether this increases the power consumption of the chip by much
-	return ADCH;
+	return vbat;
 }
diff --git a/firmware/batmon.h b/firmware/batmon.h
index 9ee5c92..fd5976f 100644
--- a/firmware/batmon.h
+++ b/firmware/batmon.h
@@ -6,7 +6,11 @@
 #define PORT_ADCC(t) t##D
 #define PIN_ADCC (1 << 1)
 
+#define BAT_THRES_POWER_ON 60 // TODO: Minimum ADC value to allow turning the device on
+#define BAT_THRES_POWER_OFF 50 // TODO: ADC value to (hard) power off the device.
+
 void batmon_init();
 uint8_t batmon_get_voltage();
+uint8_t batmon_proc();
 
 #endif // BATMON_H