Sonar depth and fish finder on an Arduino Uno

// Sonar depth finder and fish finder: an HC-SR04 and a scrolling OLED.
//
//   pin 9    HC-SR04 TRIG
//   pin 10   HC-SR04 ECHO
//   pin 8    piezo (shallow-water alarm)
//   A4, A5   OLED SDA and SCL (address 0x3C)
//
// # How sonar measures
//
// Sonar is a shout and a stopwatch. The sensor sends a burst of ultrasound
// down at the bottom and starts timing; the sound travels to the seabed,
// bounces, and comes back. The echo pin stays high for the whole round trip,
// and pulseIn() tells us how many microseconds that was. The sound went there
// AND back, so the depth is half of speed x time:
//
//   depth = speed of sound x echo time / 2
//
// # Sound is much faster in water
//
// In air at 20 C sound travels at about 343 m/s. In sea water it is about
// 1480 m/s, four times faster, because water is much harder to squeeze than
// air, so a squeezed region pushes its neighbour along sooner. The echo time
// for the same depth is therefore four times shorter in water. Work it out
// for 10 m: in air the echo takes 58 ms, in water only 13.5 ms.
//
// That is why the sketch has to know which one it is in. The very same echo
// time of 23 ms means 4 m when the pulse flew through air and 17 m when it
// swam through water. Change IN_WATER below to false and every depth shrinks
// to a quarter.
//
// The HC-SR04 here stands in for the sonar head. A real fish finder uses a
// sealed transducer (200 kHz is common) glued to the hull, because the cheap
// 40 kHz air module is not made to go in the sea, but the sum is the same.
//
// # What the screen shows
//
// The picture scrolls to the left, one column a ping, so the newest reading
// is at the right edge and the line is the shape of the seabed under the boat.
// A thin stripe of dots below the line is the sand.
//
// A fish is a short run of echoes that come back much shallower than the
// bottom, with bottom on either side. The sketch keeps a running guess of
// where the bottom is. When the echoes jump up more than 1.5 m above it, it
// counts how many pings they last. A dozen pings or fewer and then the bottom
// returns: something swam under the boat, so it draws a little fish at that
// depth, lets the seabed line run on underneath it, and counts it. Echoes that
// stay up longer than that are not a fish but a shelf, and the bottom moves up
// to meet them. Under 3 m the piezo sounds a shallow-water alarm.
//
// Drag the range slider on the sensor: a slow slope is a beach, a short jump
// to a small number and back is a fish.

#include <Wire.h>
#include <Adafruit_GFX.h>
#include <Adafruit_SSD1306.h>

const int TRIG_PIN = 9;
const int ECHO_PIN = 10;
const int BUZZER_PIN = 8;

const bool IN_WATER = true;
const float SPEED_IN_AIR = 343.0;     // metres per second at 20 C
const float SPEED_IN_WATER = 1480.0;  // metres per second, sea water at 10 C

// The picture: 128 columns wide, depth 0 to 16 m down the 52 rows from y = 12.
const int TOP = 12;
const int ROWS = 52;
const float MAX_DEPTH = 16.0;
const float SHALLOW = 3.0;

const unsigned long PING_EVERY = 70;   // ms between pings (the HC-SR04 ignores a trigger inside 60)
const unsigned long DRAW_EVERY = 1000; // at most this long between pictures
const float FISH_GAP = 1.5;            // metres above the bottom that is not the bottom
const int MAX_FISH_RUN = 12;           // pings; a longer rise is a shelf

Adafruit_SSD1306 display(128, 64, &Wire, -1);

// One byte a column: the seabed's depth in eighths of a metre, 0 for no echo.
uint8_t profile[128];

// The fish on the screen: which column and how deep (eighths of a metre).
const int MAX_FISH = 6;
int8_t fishX[MAX_FISH];
uint8_t fishD[MAX_FISH];
uint8_t fishNow = 0;

float depth = 0;    // the last ping
float bottom = 0;   // the running guess of where the seabed is
int run = 0;        // pings in a row that came back above the bottom
float runSum = 0;   // their depths added up
int fishCount = 0;
unsigned long lastPing = 0;
unsigned long lastDraw = 0;
unsigned long lastAlarm = 0;
int fresh = 0;      // pings since the last picture

// One ping, in metres. Returns 0 when nothing came back.
float ping() {
  digitalWrite(TRIG_PIN, LOW);
  delayMicroseconds(2);
  digitalWrite(TRIG_PIN, HIGH);  // a 10 microsecond pulse starts a burst
  delayMicroseconds(10);
  digitalWrite(TRIG_PIN, LOW);
  unsigned long echo = pulseIn(ECHO_PIN, HIGH, 30000);
  if (echo == 0) return 0;
  float speed = IN_WATER ? SPEED_IN_WATER : SPEED_IN_AIR;
  return speed * (echo / 1000000.0) / 2.0;
}

uint8_t eighths(float metres) {
  int e = (int)(metres * 8);
  return e > 127 ? 127 : (uint8_t)e;
}

int rowOf(float metres) {
  if (metres > MAX_DEPTH) metres = MAX_DEPTH;
  return TOP + (int)(metres / MAX_DEPTH * (ROWS - 1));
}

// Move the picture one column to the left.
void scroll() {
  for (int x = 0; x < 127; x++) profile[x] = profile[x + 1];
  uint8_t kept = 0;
  for (uint8_t i = 0; i < fishNow; i++) {
    if (fishX[i] > 0) {
      fishX[kept] = fishX[i] - 1;
      fishD[kept] = fishD[i];
      kept++;
    }
  }
  fishNow = kept;
}

void addFish(int column, float metres) {
  if (fishNow == MAX_FISH) {  // full: the oldest has scrolled the furthest
    for (uint8_t i = 1; i < MAX_FISH; i++) {
      fishX[i - 1] = fishX[i];
      fishD[i - 1] = fishD[i];
    }
    fishNow--;
  }
  fishX[fishNow] = column < 0 ? 0 : column;
  fishD[fishNow] = eighths(metres);
  fishNow++;
  fishCount++;
}

// One ping in, one column out.
void takePing() {
  depth = ping();
  scroll();
  if (depth == 0) {
    profile[127] = 0;
    return;
  }
  if (bottom == 0) bottom = depth;

  if (depth < bottom - FISH_GAP) {
    // Echoes above the bottom. Draw the bottom we expect for now.
    run++;
    runSum += depth;
    profile[127] = eighths(bottom);
    if (run > MAX_FISH_RUN) {
      // They are staying up: it is a shelf, not a fish. Move the bottom up and
      // redraw the columns we guessed.
      bottom = depth;
      for (int i = 0; i < run && i < 128; i++) profile[127 - i] = eighths(depth);
      run = 0;
      runSum = 0;
    }
    return;
  }

  // The bottom is back (or deeper). If echoes had been up for a short while,
  // that was a fish, and it was in the middle of the run.
  if (run > 0) {
    addFish(127 - 1 - run / 2, runSum / run);
    run = 0;
    runSum = 0;
  }
  bottom = bottom * 0.7 + depth * 0.3;
  profile[127] = eighths(depth);
}

void drawFish(int x, int y) {
  // A fish three pixels tall and seven long: <><
  display.drawPixel(x, y, SSD1306_WHITE);
  display.drawLine(x + 1, y - 1, x + 4, y - 1, SSD1306_WHITE);
  display.drawLine(x + 1, y + 1, x + 4, y + 1, SSD1306_WHITE);
  display.drawLine(x + 1, y, x + 5, y, SSD1306_WHITE);
  display.drawLine(x + 5, y - 1, x + 6, y - 2, SSD1306_WHITE);
  display.drawLine(x + 5, y + 1, x + 6, y + 2, SSD1306_WHITE);
}

void draw() {
  display.clearDisplay();
  display.setTextColor(SSD1306_WHITE);
  display.setTextSize(1);
  display.setCursor(0, 0);
  display.print("DEPTH ");
  if (depth > 0) {
    display.print(depth, 1);
    display.print("m");
  } else {
    display.print("--");
  }
  display.setCursor(84, 0);
  display.print("FISH ");
  display.print(fishCount);
  display.drawFastHLine(0, TOP - 1, 128, SSD1306_WHITE);  // the surface

  int lastY = -1;
  for (int x = 0; x < 128; x++) {
    if (profile[x] == 0) {
      lastY = -1;
      continue;
    }
    int y = rowOf(profile[x] / 8.0);
    // the seabed line joins this column to the last
    if (lastY < 0) display.drawPixel(x, y, SSD1306_WHITE);
    else display.drawLine(x - 1, lastY, x, y, SSD1306_WHITE);
    lastY = y;
    // sand below it, every fourth column
    if ((x & 3) == 0) {
      for (int sy = y + 3; sy < 64; sy += 3) display.drawPixel(x, sy, SSD1306_WHITE);
    }
  }
  for (uint8_t i = 0; i < fishNow; i++) drawFish(fishX[i] - 3, rowOf(fishD[i] / 8.0));
  display.display();
}

void setup() {
  Serial.begin(115200);
  Serial.println("Mokxi Uno: sonar depth finder");
  pinMode(TRIG_PIN, OUTPUT);
  pinMode(ECHO_PIN, INPUT);
  pinMode(BUZZER_PIN, OUTPUT);
  display.begin(SSD1306_SWITCHCAPVCC, 0x3C);
}

void loop() {
  unsigned long now = millis();

  if (now - lastPing >= PING_EVERY) {
    lastPing = now;
    takePing();
    fresh++;
    if (depth > 0 && depth < SHALLOW && now - lastAlarm >= 400) {
      lastAlarm = now;
      tone(BUZZER_PIN, 1500, 120);
    }
  }

  // Drawing takes a third of a second on the I2C bus, during which the sonar
  // cannot ping, so it draws a batch of new columns at a time.
  if (fresh >= 8 || (fresh > 0 && now - lastDraw >= DRAW_EVERY)) {
    fresh = 0;
    draw();
    lastDraw = millis();
    Serial.print("depth ");
    Serial.print(depth, 2);
    Serial.print(" m, fish ");
    Serial.println(fishCount);
  }
}