Commit the Changes

OpenFlightRX.ino

@@ -7,10 +7,61 @@
  * Every power cycle:
  * 1. Read configuration from EEPROM
  * 2. Set the:
+ *
+ * REF:
+ * https://github.com/iNavFlight/INAV-Rangefinder-I2C-interface/blob/master/inav_i2c_sonar.ino
+ * https://github.com/iNavFlight/inav-rangefinder
  */
 #include "Arduino.h"
 #include <EEPROM.h>
 
+// #define USE_SONAR
+// #define DEBUG
+
+#ifdef USE_SONAR
+    #include <Wire.h>
+    #include <WireSlave.h>
+
+    
+    #ifndef TWI_RX_BUFFER_SIZE
+        #define TWI_RX_BUFFER_SIZE ( 16 )
+    #endif
+
+    // For HC-SR04
+    #define TRIGGER_PIN 2
+    #define ECHO_PIN 15
+    #define SDA_PIN 21
+    #define SCL_PIN 22
+    #define I2C_SLAVE_ADDR 0x14
+    #define STATUS_OK 0
+    #define STATUS_OUT_OF_RANGE 1
+
+    #define MAX_RANGE 300   // Range of 4 Meters
+    #define PULSE_TO_CM 59  // Multiplier pulse length to distance in [cm]
+
+    #define PULSE_TIMEOUT (MAX_RANGE * PULSE_TO_CM) 
+
+    // function that executes whenever a complete and valid packet
+    // is received from master
+    // this function is registered as an event, see setup()
+    /*
+    */
+    void receiveEvent(int howMany);
+    void requestEvent(void);
+
+    uint8_t i2c_regs[] = {
+        0, //status
+        0, //older 8 of distance
+        0, //younger 8 of distance
+    };
+
+    const byte reg_size = sizeof(i2c_regs);
+    volatile byte reg_position = 0;
+
+    uint32_t nextUpdate = 0;
+    uint32_t nextWireUpdate = 0;
+#endif
+
 // For NRF24L01 (E01) - EByte
 // #include "nRF24L01.h"
 #include "RF24.h"
@@ -119,6 +170,31 @@ void setup() {
     // Initialize EEPROM
     EEPROM.begin(512);
 
+    // For Sonar Input
+    #ifdef USE_SONAR
+        // Wire.begin(I2C_SLAVE_ADDR);
+        // Wire.onRequest(requestEvent);
+        // Wire.onReceive(receiveEvent);
+        
+        bool success = WireSlave.begin(SDA_PIN, SCL_PIN, I2C_SLAVE_ADDR);
+        if(!success) {
+            Serial.println(F("I2C slave init failed - SONAR"));
+        } else {
+            WireSlave.onRequest(requestEvent);
+            WireSlave.onReceive(receiveEvent);
+        }
+
+        // Preparing Sonar module
+        pinMode(TRIGGER_PIN, OUTPUT);
+        pinMode(ECHO_PIN, INPUT);
+        
+
+        // TODO: Dummy Data. Later will remove these
+        i2c_regs[0] = STATUS_OK;
+        i2c_regs[1] = 0x00;
+        i2c_regs[2] = 0x0F;
+    #endif
+
     // Signal...
     pinMode(INDICATOR, OUTPUT);
 
@@ -220,6 +296,42 @@ void loop() {
     nrf_recv();
     // socket_client();
     // telnet_server();
+    #ifdef USE_SONAR
+        WireSlave.update();
+        if((uint32_t) (millis() - nextUpdate) > 200) {
+            digitalWrite(TRIGGER_PIN, LOW);
+            delayMicroseconds(2);
+            digitalWrite(TRIGGER_PIN, HIGH);
+            delayMicroseconds(10);
+            digitalWrite(TRIGGER_PIN, LOW);
+
+            long duration = pulseIn(ECHO_PIN, HIGH, PULSE_TIMEOUT);
+
+            if (duration > 0) {
+                i2c_regs[0] = STATUS_OK;
+            } else {
+                i2c_regs[0] = STATUS_OUT_OF_RANGE;
+            }
+
+            uint16_t cm = (uint16_t) microsecondsToCentimeters(duration);
+            i2c_regs[1] = cm >> 8;
+            i2c_regs[2] = cm & 0xFF;
+
+            #ifdef DEBUG
+                Serial.print("Regs: ");
+                Serial.print(i2c_regs[0]);
+                Serial.print(" ");
+                Serial.print(i2c_regs[1]);
+                Serial.print(" ");
+                Serial.print(i2c_regs[2]);
+                Serial.print(" ");
+                Serial.println(cm);
+            #endif  
+
+            // WireSlave.write(i2c_regs, 3);
+            nextUpdate = millis();
+        }
+    #endif
 }
 
 uint32_t last_reconnect = 0;
@@ -466,3 +578,67 @@ void write_data(uint16_t addr, uint8_t val) {
     // timerAlarmEnable(timer);
 }
 
+#ifdef USE_SONAR
+    long microsecondsToCentimeters(long ms) {
+        // return (ms * 34 / 100 / 2) / 10;
+        return (ms * 34 / 50) / 10;
+        // return (ms * 0.17) / 10;
+    }
+
+    void requestEvent(void) {
+        /*
+        if(reg_position >= reg_size) reg_position = 0;
+        WireSlave.write(i2c_regs[reg_position]);
+        reg_position++;
+        */
+        // Wire.write(i2c_regs, 3);
+        WireSlave.write(i2c_regs, 3);
+    }
+
+    void receiveEvent(int howMany){
+        // TODO: Modify here!!!!
+        // while(1 < WireSlave.available()) {  // loop through all but the last byte
+        //    char c = WireSlave.read();      // receive byte as a character
+        //    Serial.print(c);                // print the character
+        // }
+
+        // int x = WireSlave.read();           // receive byte as an integer
+        // Serial.println(x);                  // print the integer
+        #ifdef DEBUG
+            Serial.print("howMany:");
+            Serial.println(howMany);
+        #endif
+
+        if(howMany < 1) {
+            // Sanity-check
+            return;
+        }
+
+        if(howMany > TWI_RX_BUFFER_SIZE) {
+            // Also insane number
+            return;
+        }
+
+        // reg_position = Wire.read();
+        reg_position = WireSlave.read();
+
+        howMany--;
+
+        if(!howMany){
+            // This write was only to set the buffer for next read
+            return;
+        }
+
+        // Everything above 1 byte is something we do not care, so just get it from bus as send to /dev/null
+        while(howMany--) {
+            #ifdef DEBUG
+                // Serial.println(Wire.read());
+                Serial.println(WireSlave.read());
+            #else
+                // Wire.read();
+                WireSlave.read();
+            #endif
+        }
+    }
+#endif
+