ARDUINO VIDOR 4000 - CONTROL TELLO BY RYZE

#include "defines.h"
#include "arduino_secrets.h"
#include <SPI.h>
#include <WiFiNINA_Generic.h>
#include <WiFiUdp_Generic.h>
#include <Wire.h>
#include <ADXL345.h>

WiFiUDP Udp;
ADXL345 adxl;

char ssid[] = SECRET_SSID;
char pass[] = SECRET_PASS;
int keyIndex = 0;
int status = WL_IDLE_STATUS;
unsigned int localPort = 9000; // local port to listen on
char telloIP[] = "192.168.10.1";
char telloGatewayIP[] = "0.0.0.0";
uint16_t telloPort = 8889;
uint16_t telloStatePort = 8890;
uint16_t telloVideo = 11111;
char packetBuffer[255]; //buffer to hold incoming packet
bool telloReady = false;
String a, b, c, d, t;
int x, y, z;
double xyz[3];
static unsigned long lastTimeItHappened = 0;
char msgData[90]; //Message Buffer
String LF;

void initBoard() {
  adxl.powerOn();
  //set activity/ inactivity thresholds (0-255)
  adxl.setActivityThreshold(75); //62.5mg per increment
  adxl.setInactivityThreshold(75); //62.5mg per increment
  adxl.setTimeInactivity(10); // how many seconds of no activity is inactive?
  //look of activity movement on this axes - 1 == on; 0 == off
  adxl.setActivityX(1);
  adxl.setActivityY(1);
  adxl.setActivityZ(1);
  //look of inactivity movement on this axes - 1 == on; 0 == off
  adxl.setInactivityX(1);
  adxl.setInactivityY(1);
  adxl.setInactivityZ(1);
  //look of tap movement on this axes - 1 == on; 0 == off
  adxl.setTapDetectionOnX(0);
  adxl.setTapDetectionOnY(0);
  adxl.setTapDetectionOnZ(1);

  //set values for what is a tap, and what is a double tap (0-255)
  adxl.setTapThreshold(50); //62.5mg per increment
  adxl.setTapDuration(15); //625us per increment
  adxl.setDoubleTapLatency(80); //1.25ms per increment
  adxl.setDoubleTapWindow(200); //1.25ms per increment
  //set values for what is considered freefall (0-255)
  adxl.setFreeFallThreshold(7); //(5 - 9) recommended - 62.5mg per increment
  adxl.setFreeFallDuration(45); //(20 - 70) recommended - 5ms per increment
  //setting all interrupts to take place on int pin 1
  //I had issues with int pin 2, was unable to reset it
  //adxl.setInterruptMapping( ADXL345_INT_SINGLE_TAP_BIT,   ADXL345_INT1_PIN );
  adxl.setInterruptMapping( ADXL345_INT_DOUBLE_TAP_BIT,   ADXL345_INT1_PIN );
  //adxl.setInterruptMapping( ADXL345_INT_FREE_FALL_BIT,    ADXL345_INT1_PIN );
  //adxl.setInterruptMapping( ADXL345_INT_ACTIVITY_BIT,     ADXL345_INT1_PIN );
  //adxl.setInterruptMapping( ADXL345_INT_INACTIVITY_BIT,   ADXL345_INT1_PIN );
  //register interrupt actions - 1 == on; 0 == off
  //adxl.setInterrupt( ADXL345_INT_SINGLE_TAP_BIT, 0);
  adxl.setInterrupt( ADXL345_INT_DOUBLE_TAP_BIT, 1);
  //adxl.setInterrupt( ADXL345_INT_FREE_FALL_BIT,  0);
  //adxl.setInterrupt( ADXL345_INT_ACTIVITY_BIT,   0);
  //adxl.setInterrupt( ADXL345_INT_INACTIVITY_BIT, 0);

  //Initialize serial and wait for port to open:
  Serial.begin(19200);
  Serial.print(BOARD_NAME);
  Serial.print(", ");
  Serial.println(WIFININA_GENERIC_VERSION);

  // check for the WiFi module:
  if (WiFi.status() == WL_NO_MODULE)
  {
    Serial.println(F("Communication with WiFi module failed!"));
    // don't continue
    while (true);
  }

  String fv = WiFi.firmwareVersion();
  if (fv < WIFI_FIRMWARE_LATEST_VERSION)
  {
    Serial.print(F("NINA FW v"));
    Serial.print(fv);
    Serial.print(F(" -> Newer NINA FW v"));
    Serial.print(WIFI_FIRMWARE_LATEST_VERSION);
    Serial.println("\n");
  }
}

void connectWifi() {
  // attempt to connect to Wifi network:
  while (status != WL_CONNECTED)
  {
    Serial.print(F("Trying to connect to SSID: "));
    Serial.println(ssid);
    status = WiFi.begin(ssid, pass);
    delay(1000);
  }
  Serial.println(F("Connected!"));
}

void sendData(String t = "battery?") {
  char a[60 + 1];
  t.toCharArray(a, 60 + 1);
  Udp.beginPacket(telloIP, telloPort);
  Udp.write(a);
  Udp.endPacket();
}

char inputBuffer[90 + 1]; // Handles up to 90 bytes, with a null character termination.

const int ledPin =  LED_BUILTIN; //red led
void setup()
{
  initBoard();
  pinMode(ledPin, OUTPUT);
  if (!telloReady)
  {
    inputBuffer[0] = '\0'; //Initialize string to terminator.
    connectWifi();
    printWiFiStatus();
    Serial.println(F("\nStarting connection to UDP server..."));
    Udp.begin(localPort);
    delay(2000);
    Serial.println(F("Sending SDK control command"));
    sendData("command"); //Enable control over tello
  } else {
    digitalWrite(ledPin, LOW);
  }
}

int landed = 1;
void loop()
{

  //Serial command to UDP command Are you communicating through serial?
      if (Serial.available() > 0)
      {
        char input = Serial.read();
        static int s_len;
        if (s_len >= 90) {
        } else if (input != '\n' && input != '\r') {
          inputBuffer[s_len++] = input;
        } else {
          sendData(inputBuffer);
          Serial.print("RECEIVED MSG: ");
          Serial.println(inputBuffer);
          memset(inputBuffer, 0, sizeof(inputBuffer));
          s_len = 0;
        }
      }

  // if there's data available from the server, read a packet
    int packetSize = Udp.parsePacket();
    if (packetSize)
    {
      Serial.print(F("Received packet from tello -> size "));
      Serial.print(packetSize);
      Serial.print(F(" - IP: "));
      IPAddress remoteIp = Udp.remoteIP();
      Serial.print(remoteIp);
      Serial.print(F(", Port: "));
      Serial.print(Udp.remotePort());
      int len = Udp.read(packetBuffer, 255);
      if (len > 0)
      {
        packetBuffer[len] = 0;
      }
      Serial.print(F(" Contents: "));
      Serial.println(packetBuffer);
    }

  if (telloReady)
  {
    //ACCELEROMETER
    adxl.readXYZ(&x, &y, &z);
    adxl.getAcceleration(xyz);
    a = String(map(y, -255, 255, -100, 100)), //left/right (-100 ~ 100)
    b = String(map(-x, -255, 255, -100, 100)), //forward/back(-100 ~ 100)
    //c up/down (-100 ~ 100)
    //d yaw (-100 ~ 100)
    t = "rc " + a + " " + b + " " + c + " " + d; //TELLO: rc a b c d
    if (t != LF) {
      sendData(t); //SEND LIVE TO UDP
      c = "0";  //Cause, we using momentum moments to go up and down
      d = "0"; //Cause, we using momentum moments to turn
      //Serial.println(t);
      LF = t;
    }
    if (xyz[0] > 1) // forward momentum
    {
      Serial.println("TURN LEFT");
      sendData("ccw 90");
    }
    if (xyz[1] > 1) // right momentum
    {
      Serial.println("TURN RIGHT");
      sendData("cw 90");
    }
    if (xyz[2] > 1.4f) // up momentum
    {
      Serial.println("UP");
      c = "100";
    }

    //double tap
    byte interrupts = adxl.getInterruptSource();
    if (adxl.triggered(interrupts, ADXL345_DOUBLE_TAP)) {
      if (landed == 1)
      {
        Serial.println("TAKEOFF");
        sendData("takeoff");
        landed = 0;
        return;
      } else {
        Serial.println("LANDING");
        sendData("land");
        landed = 1;
        return;
      }
    }

    //String h = String(x) + "\n" + String(y) + "\n" + String(z) + "\n" + String(xyz[0]) + "\n" + String(xyz[1]) + "\n" + String(xyz[2]);
    //Serial.println(h);

    //Check battery every 14 seconds (Keeps connection alive)
    //if (millis() - lastTimeItHappened >= 14000) { // 14 seconds later
    //  lastTimeItHappened = millis();
    //  Serial.println(F("Request Battery?"));
    //  sendData();
    //}
  }
}

void curve(int x1, int y1, int z1, int x2, int y2, int z2, int spd)
{
  String m = "curve " + String(x1) + " " + String(y1) + " " + String(z1) + " " + String(x2) + " " + String(y2) + " " + String(z2) + " " + String(spd);
  sendData(m);
}


void printWiFiStatus()
{
  // print your board's IP address:
  IPAddress ip = WiFi.localIP();
  Serial.print(F("IP Address: "));
  Serial.println(ip);

  // print the received signal strength:
  long rssi = WiFi.RSSI();
  Serial.print(F("Signal strength (RSSI):"));
  Serial.print(rssi);
  Serial.println(F(" dBm"));
  telloReady = true;
  digitalWrite(ledPin, LOW);
}

int map(int x, int in_min, int in_max, int out_min, int out_max)
{
  return (x - in_min) * (out_max - out_min) / (in_max - in_min) + out_min;
}