Simplify sonar usage

include/sonar.h

@@ -15,21 +15,17 @@
 #define ECHO_PIN    PINC
 #define ECHO_BIT    PC5
 
-#define SPEED_OF_SOUND      343 // (m/s)
-#define US_PER_CM           58 // Time for sound to travel distance of 1cm
+#define SPEED_OF_SOUND      343 // 20°C dry air (m/s)
 #define MAX_SONAR_RANGE     8 // Trigger + echo (m)
-#define DELAY_BETWEEN_TESTS	500 // Timeout for return signal
+#define SONAR_DELAY	        60 // Timeout for return signal
 #define TIMER_MAX           65535 // For 16-bit timer
-
-#define CYCLES_PER_US   F_CPU / 1000000
+#define CYCLES_PER_US       (F_CPU / 1000000)
 #define SONAR_TIMEOUT 	    (F_CPU * MAX_SONAR_RANGE) / SPEED_OF_SOUND
 
-#define TRIG_ERROR      -1
-#define ECHO_ERROR      -2
+#define SONAR_ERROR 0
 
 
 void init_sonar(void);
-void trigger_sonar(void);
-int32_t read_sonar(void);
+uint16_t read_sonar(void);
 
 #endif /* SONAR_H */

src/main.c

@@ -36,10 +36,18 @@ int main(void)
 
     // Use green LED as power LED
     SET(LED_PORT, LED_GREEN_BIT);
-    SET(LED_PORT, LED_BLUE_BIT); // Temporary for debug
 
-    run_servos();
-    read_temp();
+    /* run_servos(); */
+    /* read_temp(); */
+    while (1) {
+        uint16_t distance_int_cm = read_sonar();
+
+        if (distance_int_cm <= 10 && distance_int_cm != SONAR_ERROR)
+            SET(LED_PORT, LED_BLUE_BIT);
+        else
+            CLR(LED_PORT, LED_BLUE_BIT);
+        _delay_ms(SONAR_DELAY);
+    }
 
     return 0;
 }

src/sonar.c

@@ -2,9 +2,10 @@
 #include <util/delay.h>
 #include "sonar.h"
 
-volatile uint32_t overflow_counter = 0;
-volatile uint32_t timeout_val = SONAR_TIMEOUT;
-volatile uint32_t no_of_cycles = 0;
+#include "led.h"
+
+volatile uint32_t trig_counter;
+volatile uint32_t no_of_ticks;
 
 void init_sonar(void)
 {
@@ -12,49 +13,42 @@ void init_sonar(void)
     CLR(ECHO_DDR, ECHO_BIT);
 }
 
-void trigger_sonar(void)
+uint16_t read_sonar(void)
 {
-    CLR(TRIG_PORT, TRIG_BIT);
-    _delay_us(1);
+    // Hold high for 10us while TRIGGER sends 40KHz burst
     SET(TRIG_PORT, TRIG_BIT);
-    _delay_us(12);
+    _delay_us(10);
     CLR(TRIG_PORT, TRIG_BIT);
-    _delay_us(1);
-}
 
-int32_t read_sonar(void)
-{
-    int dist_in_cm = 0;
-    trigger_sonar();
-
-    // While echo pin is low
+    trig_counter = SONAR_TIMEOUT;
+    // Wait for ECHO to receive TRIGGER burst
     while (!(CHK(ECHO_PIN, ECHO_BIT))) {
-        if (--timeout_val == 0)
-            return TRIG_ERROR; // Received no response from echo
+        if (--trig_counter == 0) {
+            return SONAR_ERROR;
+        }
     }
 
     TCNT1 = 0;
-    SET(TCCR1B, CS10);
-    SET(TIMSK1, TOIE1);
-    overflow_counter = 0;
+    SET(TCCR1B, CS10); // Prescaler = 1
+    SET(TIMSK1, TOIE1); // Enable overflow interrupts
+    no_of_ticks = 0; // Reset overflow counter
+
     sei();
-
-    // While echo pin is high
+    // Count cycles until ECHO goes low
     while (CHK(ECHO_PIN, ECHO_BIT)) {
-        if (((overflow_counter * TIMER_MAX) + TCNT1) > SONAR_TIMEOUT)
-            return ECHO_ERROR; // No echo within sonar range
+        if (no_of_ticks + TCNT1 > SONAR_TIMEOUT) {
+            return SONAR_ERROR;
+        }
     }
-
     TCCR1B = 0x00;
     cli();
-    no_of_cycles = ((overflow_counter * TIMER_MAX) + TCNT1);
-    dist_in_cm = (no_of_cycles / (US_PER_CM * CYCLES_PER_US));
 
-    return dist_in_cm;
+    no_of_ticks += TCNT1;
+
+    return (uint16_t)(no_of_ticks / (CYCLES_PER_US * 58));
 }
 
 ISR(TIMER1_OVF_vect)
 {
-    ++overflow_counter;
-    TCNT1 = 0;
+    no_of_ticks += TIMER_MAX;
 }