diff --git a/include/sonar.h b/include/sonar.h
index a152609..1e42daa 100644
--- a/include/sonar.h
+++ b/include/sonar.h
@@ -19,7 +19,7 @@
 #define US_PER_CM           58 // Time for sound to travel distance of 1cm
 #define MAX_SONAR_RANGE     8 // Trigger + echo (m)
 #define DELAY_BETWEEN_TESTS	500 // Timeout for return signal
-#define TIMER_MAX           65535 // Depends on the timer used
+#define TIMER_MAX           65535 // For 16-bit timer
 
 #define CYCLES_PER_US   F_CPU / 1000000
 #define SONAR_TIMEOUT 	(F_CPU * MAX_SONAR_RANGE) / SPEED_OF_SOUND
@@ -30,6 +30,6 @@
 
 void init_sonar(void);
 void trigger_sonar(void);
-int read_sonar(void);
+int32_t read_sonar(void);
 
 #endif /* SONAR_H */
diff --git a/src/sonar.c b/src/sonar.c
index 3156b9e..6d7fe3f 100644
--- a/src/sonar.c
+++ b/src/sonar.c
@@ -3,7 +3,7 @@
 #include "sonar.h"
 
 volatile uint32_t overflow_counter = 0;
-volatile uint32_t trig_counter = 0;
+volatile uint32_t timeout_val = SONAR_TIMEOUT;
 volatile uint32_t no_of_cycles = 0;
 
 void init_sonar(void)
@@ -28,7 +28,7 @@ ISR(TIMER1_OVF_vect)
     TCNT1 = 0;
 }
 
-int read_sonar(void)
+int32_t read_sonar(void)
 {
     int dist_in_cm = 0;
     init_sonar();
@@ -36,7 +36,7 @@ int read_sonar(void)
 
     // While echo pin is low
     while (!(CHK(ECHO_PIN, ECHO_BIT))) {
-        if (++trig_counter > SONAR_TIMEOUT)
+        if (--timeout_val == 0)
             return TRIG_ERROR; // Received no response from echo
     }