AI-FCW Forward Collision Warning System

/*

  Example guide:
  https://www.amebaiot.com/en/amebapro2-arduino-neuralnework-object-detection/

  NN Model Selection
  Select Neural Network(NN) task and models using modelSelect(nntask, objdetmodel, facedetmodel, facerecogmodel).
  Replace with NA_MODEL if they are not necessary for your selected NN Task.

  NN task
  =======
  OBJECT_DETECTION/ FACE_DETECTION/ FACE_RECOGNITION

  Models
  =======
  YOLOv3 model         DEFAULT_YOLOV3TINY   / CUSTOMIZED_YOLOV3TINY
  YOLOv4 model         DEFAULT_YOLOV4TINY   / CUSTOMIZED_YOLOV4TINY
  YOLOv7 model         DEFAULT_YOLOV7TINY   / CUSTOMIZED_YOLOV7TINY
  SCRFD model          DEFAULT_SCRFD        / CUSTOMIZED_SCRFD
  MobileFaceNet model  DEFAULT_MOBILEFACENET/ CUSTOMIZED_MOBILEFACENET
  No model             NA_MODEL
*/

#include "WiFi.h"
#include "StreamIO.h"
#include "VideoStream.h"
#include "RTSP.h"
#include "NNObjectDetection.h"
#include "VideoStreamOverlay.h"
#include "ObjectClassList.h"
#include <DFPlayer.h>
DFPlayer mp3;
#define CHANNEL 0
#define CHANNELNN 3
// Lower resolution for NN processing
#define NNWIDTH 576
#define NNHEIGHT 320
char text_str2[80];
char text_str1[20];  // 声明一个字符数组，用于存储字符串

int currentValue = 0;                  // current value
int lastValue = 0;                     // Last value
unsigned long lastTime = 0;            // Last changed time
const unsigned long interval = 10000;  // 10 seconds interval
unsigned long lastTriggerTime = 0;     // Variable to store the time of the last trigger
unsigned long lastTriggerTime1 = 0;    // Variable to store the time of the last trigger
float lastSpeed = 0.0;
unsigned long lTime = 0;

bool flag = 0;  // Flag value
float carr;
float groundSpeedKmh;
float slope(float x1, float y1, float x2, float y2) {
  return (y2 - y1) / (x2 - x1);
}

float intercept(float x, float y, float m) {
  return y - m * x;
}

bool isBetweenLines(float x, float y, float mA, float bA, float mB, float bB) {
  float yA = mA * x + bA;
  float yB = mB * x + bB;

  return (y >= yA && y <= yB) || (y <= yA && y >= yB);
}
float x1_A = 1535, y1_A = 997;
float x2_A = 939, y2_A = 582;

// Coordinates of line B：B(218,992)-(911,582)
float x1_B = 218, y1_B = 992;
float x2_B = 911, y2_B = 582;

// Calculate slope and intercept
float mA = slope(x1_A, y1_A, x2_A, y2_A);
float bA = intercept(x1_A, y1_A, mA);

float mB = slope(x1_B, y1_B, x2_B, y2_B);
float bB = intercept(x1_B, y1_B, mB);

int points[11][2] = {
  { 1535, 997 },
  { 1480, 959 },
  { 1426, 921 },
  { 1372, 883 },
  { 1318, 846 },
  { 1264, 808 },
  { 1209, 770 },
  { 1155, 732 },
  { 1101, 695 },
  { 1047, 657 },
  { 939, 582 }
};

int points2[11][2] = {
  { 218, 992 },
  { 281, 953 },
  { 344, 914 },
  { 407, 876 },
  { 470, 837 },
  { 533, 799 },
  { 596, 760 },
  { 659, 722 },
  { 722, 683 },
  { 785, 645 },
  { 911, 582 }
};
CameraSetting configCam;
VideoSetting config(VIDEO_FHD, 30, VIDEO_H264, 0);
VideoSetting configNN(NNWIDTH, NNHEIGHT, 10, VIDEO_RGB, 0);
NNObjectDetection ObjDet;
RTSP rtsp;
StreamIO videoStreamer(1, 1);
StreamIO videoStreamerNN(1, 1);

char ssid[] = "koukileo";  // your network SSID (name)
char pass[] = "N7567772";  // your network password
int status = WL_IDLE_STATUS;

IPAddress ip;
int rtsp_portnum;

void setup() {
  Serial.begin(115200);
  Serial3.begin(9600);  // Used to read data from the GPS module
  Serial2.begin(9600);
  byte data[] = { 0x7E, 0xFF, 0x06, 0x12, 0x00, 0x00, 0xFF, 0xFD, 0xEA, 0xEF };
  int dataSize = sizeof(data) / sizeof(data[0]);  // Calculate the size of the array
  // Send each byte in the array
  for (int i = 0; i < dataSize; i++) {
    Serial2.write(data[i]);
  }
  // attempt to connect to Wifi network:
  while (status != WL_CONNECTED) {
    Serial.print("Attempting to connect to WPA SSID: ");
    Serial.println(ssid);
    status = WiFi.begin(ssid, pass);

    // wait 2 seconds for connection:
    delay(2000);
  }
  ip = WiFi.localIP();

  // Configure camera video channels with video format information
  // Adjust the bitrate based on your WiFi network quality
  config.setBitrate(2 * 1920 * 1080);  // Recommend to use 2Mbps for RTSP streaming to prevent network congestion
  Camera.configVideoChannel(CHANNEL, config);
  Camera.configVideoChannel(CHANNELNN, configNN);
  Camera.videoInit();

  // Configure RTSP with corresponding video format information
  rtsp.configVideo(config);
  rtsp.begin();
  rtsp_portnum = rtsp.getPort();

  // Configure object detection with corresponding video format information
  // Select Neural Network(NN) task and models
  ObjDet.configVideo(configNN);
  ObjDet.modelSelect(OBJECT_DETECTION, DEFAULT_YOLOV4TINY, NA_MODEL, NA_MODEL);
  ObjDet.begin();

  // Configure StreamIO object to stream data from video channel to RTSP
  videoStreamer.registerInput(Camera.getStream(CHANNEL));
  videoStreamer.registerOutput(rtsp);
  if (videoStreamer.begin() != 0) {
    Serial.println("StreamIO link start failed");
  }

  // Start data stream from video channel
  Camera.channelBegin(CHANNEL);

  // Configure StreamIO object to stream data from RGB video channel to object detection
  videoStreamerNN.registerInput(Camera.getStream(CHANNELNN));
  videoStreamerNN.setStackSize();
  videoStreamerNN.setTaskPriority();
  videoStreamerNN.registerOutput(ObjDet);
  if (videoStreamerNN.begin() != 0) {
    Serial.println("StreamIO link start failed");
  }

  // Start video channel for NN
  Camera.channelBegin(CHANNELNN);

  // Start OSD drawing on RTSP video channel
  OSD.configVideo(CHANNEL, config);
  OSD.configVideo(1, config);
  OSD.begin();
}

void loop() {

  if (Serial.available() > 0) {
    String input = Serial.readString();
    input.trim();

    if (input.startsWith(String("AE="))) {
      String value = input.substring(3);
      int val = value.toInt();
      configCam.setExposureMode(val);
    } else if (input.startsWith(String("AE"))) {
      configCam.getExposureMode();
    } else if (input.startsWith(String("EXPTIME="))) {
      String value = input.substring(8);
      int val = value.toInt();
      configCam.setExposureTime(val);
    } else if (input.startsWith(String("EXPTIME"))) {
      configCam.getExposureTime();
    } else if (input.startsWith(String("GAIN="))) {
      String value = input.substring(5);
      int val = value.toInt();
      configCam.setAEGain(val);
    } else if (input.startsWith(String("GAIN"))) {
      configCam.getAEGain();
    } else if (input.startsWith(String("PLF="))) {
      String value = input.substring(4);
      int val = value.toInt();
      configCam.setPowerLineFreq(val);
    } else if (input.startsWith(String("PLF"))) {
      configCam.getPowerLineFreq();
    } else if (input.startsWith(String("RESET"))) {
      configCam.reset();
    }
  }
  std::vector<ObjectDetectionResult> results = ObjDet.getResult();

  uint16_t im_h = config.height();
  uint16_t im_w = config.width();

  //Serial.print("Network URL for RTSP Streaming: ");
  //Serial.print("rtsp://");
  Serial.print(ip);
  //Serial.print(":");
  //Serial.println(rtsp_portnum);
  Serial.println(" ");

  printf("Total number of objects detected = %d\r\n", ObjDet.getResultCount());
  OSD.createBitmap(CHANNEL);
  if (Serial3.available()) {
    String line = Serial3.readStringUntil('\n');  // Read entire row of data
    if (line.startsWith("$GPVTG")) {
      // Split string and extract fields
      int idx1 = line.indexOf(',');
      int idx2 = line.indexOf(',', idx1 + 1);
      int idx3 = line.indexOf(',', idx2 + 1);
      int idx4 = line.indexOf(',', idx3 + 1);
      int idx5 = line.indexOf(',', idx4 + 1);
      int idx6 = line.indexOf(',', idx5 + 1);
      int idx7 = line.indexOf(',', idx6 + 1);
      int idx8 = line.indexOf(',', idx7 + 1);

      if (idx8 != -1) {
        String groundSpeedKmhStr = line.substring(idx7 + 1, idx8);
        groundSpeedKmh = groundSpeedKmhStr.toFloat();
        //Serial.print("Ground Speed (km/h): ");
        //Serial.println(groundSpeedKmh);
        snprintf(text_str2, sizeof(text_str2), "Ground Speed:%.1f (km/h)", groundSpeedKmh);
      }
    }
  }


  if (ObjDet.getResultCount() > 0) {
    for (int j = 0; j < 11; j++) {
      OSD.drawPoint(CHANNEL, points[j][0], points[j][1], 10, OSD_COLOR_WHITE);
      OSD.drawPoint(CHANNEL, points2[j][0], points2[j][1], 10, OSD_COLOR_WHITE);
    }
    OSD.drawLine(CHANNEL, 1535, 997, 939, 582, 10, OSD_COLOR_RED);
    OSD.drawLine(CHANNEL, 218, 992, 911, 582, 10, OSD_COLOR_RED);
    for (uint32_t i = 0; i < ObjDet.getResultCount(); i++) {
      int obj_type = results[i].type();
      if (itemList[obj_type].filter) {  // check if item should be ignored

        ObjectDetectionResult item = results[i];
        // Result coordinates are floats ranging from 0.00 to 1.00
        // Multiply with RTSP resolution to get coordinates in pixels
        int xmin = (int)(item.xMin() * im_w);
        int xmax = (int)(item.xMax() * im_w);
        int ymin = (int)(item.yMin() * im_h);
        int ymax = (int)(item.yMax() * im_h);

        // Draw boundary box
        //printf("Item %d %s:\t%d %d %d %d\n\r", i, itemList[obj_type].objectName, xmin, xmax, ymin, ymax);

        OSD.drawRect(CHANNEL, xmin, ymin, xmax, ymax, 3, OSD_COLOR_WHITE);
        // Print identification text
        // Compare strings using strcmp() or strncmp()
        if (strcmp(itemList[obj_type].objectName, "car") == 0)
          carr = ((155.5 * 1.60) / (xmax - xmin)) * 10;
        if (strcmp(itemList[obj_type].objectName, "motorbike") == 0)
          carr = ((155.5 * 0.8) / (xmax - xmin)) * 10;
        if (strcmp(itemList[obj_type].objectName, "bus") == 0)
          carr = ((155.5 * 2.50) / (xmax - xmin)) * 10;
        if (strcmp(itemList[obj_type].objectName, "truck") == 0)
          carr = ((155.5 * 2.10) / (xmax - xmin)) * 10;

        char text_str[50];
        snprintf(text_str, sizeof(text_str), "%s %d m: %.2f", itemList[obj_type].objectName, item.score(), carr);
        OSD.drawText(CHANNEL, xmin, ymin - OSD.getTextHeight(CHANNEL), text_str, OSD_COLOR_RED);

        if (!isBetweenLines((xmin + xmax) / 2, ymax, mA, bA, mB, bB)) {
          if (carr > 8) {
            lastTime = millis();
            if (flag == 1) {
              strcpy(text_str1, "Car-Moving!");
              OSD.drawText(CHANNEL, 85, 60, text_str1, OSD_COLOR_RED);
              byte data[] = { 0x7E, 0xFF, 0x06, 0x12, 0x00, 0x00, 0x01, 0xFE, 0xE8, 0xEF };
              int dataSize = sizeof(data) / sizeof(data[0]);  // Calculate the size of the array
              // Send each byte in the array
              for (int i = 0; i < dataSize; i++) {
                Serial2.write(data[i]);
              }
            }

            flag = 0;  // reset flag
          }
          if (carr < 6) {
            // If the value does not change within 10 seconds, set the flag
            if (millis() - lastTime >= interval) {
              flag = 1;
            }
          }
          if (groundSpeedKmh > 15) {
            float sqd;
            if (groundSpeedKmh > 80) {
              sqd = groundSpeedKmh / 4;
            } else if(groundSpeedKmh < 80) {
                sqd = groundSpeedKmh / 3;
              }
            if (carr < sqd) {
              unsigned long cTime = millis();
              if (cTime - lTime >= 500) {
                // Read the current speed, assuming there is a function getSpeed() to get the speed
                float currentSpeed = carr;
                // Calculate speed changes
                float speedDifference = lastSpeed - currentSpeed;
                // If the speed changes by more than 5 meters/0.5 seconds, an alarm is triggered
                if (speedDifference > 5) {
                  unsigned long currentTime1 = millis();  // Get the current time
                  if (currentTime1 - lastTriggerTime1 >= 5000) {
                    lastTriggerTime1 = currentTime1;
                    strcpy(text_str1, "Collision!");
                    OSD.drawText(CHANNEL, 85, 60, text_str1, OSD_COLOR_RED);
                    byte data[] = { 0x7E, 0xFF, 0x06, 0x12, 0x00, 0x07, 0xCF, 0xFE, 0x13, 0xEF };
                    int dataSize = sizeof(data) / sizeof(data[0]);  // Calculate the size of the array
                    // Send each byte in the array
                    for (int i = 0; i < dataSize; i++) {
                      Serial2.write(data[i]);
                    }
                  }
                }
                lastSpeed = currentSpeed;
                lTime = cTime;
              }
              unsigned long currentTime = millis();          // Get the current time
              if (currentTime - lastTriggerTime >= 10000) {  // 5000 milliseconds = 5 seconds
                // Update the last trigger time
                lastTriggerTime = currentTime;
                strcpy(text_str1, "Notice!");
                OSD.drawText(CHANNEL, 85, 60, text_str1, OSD_COLOR_RED);
                byte data[] = { 0x7E, 0xFF, 0x06, 0x12, 0x00, 0x00, 0x02, 0xFE, 0xE7, 0xEF };
                int dataSize = sizeof(data) / sizeof(data[0]);  // Calculate the size of the array
                // Send each byte in the array
                for (int i = 0; i < dataSize; i++) {
                  Serial2.write(data[i]);
                }
              }
            }
          }
        }
      }
    }
    OSD.drawText(CHANNEL, 300, 60, text_str2, OSD_COLOR_RED);
  }

  OSD.update(CHANNEL);
  delay(50);
}