AsmodHacker

ARDUINO VIDOR 4000 - CONTROL TELLO BY RYZE

May 11th, 2021 (edited)
679
Never
Not a member of Pastebin yet? Sign Up, it unlocks many cool features!
  1. #include "defines.h"
  2. #include "arduino_secrets.h"
  3. #include <SPI.h>
  4. #include <WiFiNINA_Generic.h>
  5. #include <WiFiUdp_Generic.h>
  6. #include <Wire.h>
  7. #include <ADXL345.h>
  8.  
  9. WiFiUDP Udp;
  10. ADXL345 adxl;
  11.  
  12. char ssid[] = SECRET_SSID;
  13. char pass[] = SECRET_PASS;
  14. int keyIndex = 0;
  15. int status = WL_IDLE_STATUS;
  16. unsigned int localPort = 9000; // local port to listen on
  17. char telloIP[] = "192.168.10.1";
  18. char telloGatewayIP[] = "0.0.0.0";
  19. uint16_t telloPort = 8889;
  20. uint16_t telloStatePort = 8890;
  21. uint16_t telloVideo = 11111;
  22. char packetBuffer[255]; //buffer to hold incoming packet
  23. bool telloReady = false;
  24. String a, b, c, d, t;
  25. int x, y, z;
  26. double xyz[3];
  27. static unsigned long lastTimeItHappened = 0;
  28. char msgData[90]; //Message Buffer
  29. String LF;
  30.  
  31. void initBoard() {
  32.   adxl.powerOn();
  33.   //set activity/ inactivity thresholds (0-255)
  34.   adxl.setActivityThreshold(75); //62.5mg per increment
  35.   adxl.setInactivityThreshold(75); //62.5mg per increment
  36.   adxl.setTimeInactivity(10); // how many seconds of no activity is inactive?
  37.   //look of activity movement on this axes - 1 == on; 0 == off
  38.   adxl.setActivityX(1);
  39.   adxl.setActivityY(1);
  40.   adxl.setActivityZ(1);
  41.   //look of inactivity movement on this axes - 1 == on; 0 == off
  42.   adxl.setInactivityX(1);
  43.   adxl.setInactivityY(1);
  44.   adxl.setInactivityZ(1);
  45.   //look of tap movement on this axes - 1 == on; 0 == off
  46.   adxl.setTapDetectionOnX(0);
  47.   adxl.setTapDetectionOnY(0);
  48.   adxl.setTapDetectionOnZ(1);
  49.  
  50.   //set values for what is a tap, and what is a double tap (0-255)
  51.   adxl.setTapThreshold(50); //62.5mg per increment
  52.   adxl.setTapDuration(15); //625us per increment
  53.   adxl.setDoubleTapLatency(80); //1.25ms per increment
  54.   adxl.setDoubleTapWindow(200); //1.25ms per increment
  55.   //set values for what is considered freefall (0-255)
  56.   adxl.setFreeFallThreshold(7); //(5 - 9) recommended - 62.5mg per increment
  57.   adxl.setFreeFallDuration(45); //(20 - 70) recommended - 5ms per increment
  58.   //setting all interrupts to take place on int pin 1
  59.   //I had issues with int pin 2, was unable to reset it
  60.   //adxl.setInterruptMapping( ADXL345_INT_SINGLE_TAP_BIT,   ADXL345_INT1_PIN );
  61.   adxl.setInterruptMapping( ADXL345_INT_DOUBLE_TAP_BIT,   ADXL345_INT1_PIN );
  62.   //adxl.setInterruptMapping( ADXL345_INT_FREE_FALL_BIT,    ADXL345_INT1_PIN );
  63.   //adxl.setInterruptMapping( ADXL345_INT_ACTIVITY_BIT,     ADXL345_INT1_PIN );
  64.   //adxl.setInterruptMapping( ADXL345_INT_INACTIVITY_BIT,   ADXL345_INT1_PIN );
  65.   //register interrupt actions - 1 == on; 0 == off
  66.   //adxl.setInterrupt( ADXL345_INT_SINGLE_TAP_BIT, 0);
  67.   adxl.setInterrupt( ADXL345_INT_DOUBLE_TAP_BIT, 1);
  68.   //adxl.setInterrupt( ADXL345_INT_FREE_FALL_BIT,  0);
  69.   //adxl.setInterrupt( ADXL345_INT_ACTIVITY_BIT,   0);
  70.   //adxl.setInterrupt( ADXL345_INT_INACTIVITY_BIT, 0);
  71.  
  72.   //Initialize serial and wait for port to open:
  73.   Serial.begin(19200);
  74.   Serial.print(BOARD_NAME);
  75.   Serial.print(", ");
  76.   Serial.println(WIFININA_GENERIC_VERSION);
  77.  
  78.   // check for the WiFi module:
  79.   if (WiFi.status() == WL_NO_MODULE)
  80.   {
  81.     Serial.println(F("Communication with WiFi module failed!"));
  82.     // don't continue
  83.     while (true);
  84.   }
  85.  
  86.   String fv = WiFi.firmwareVersion();
  87.   if (fv < WIFI_FIRMWARE_LATEST_VERSION)
  88.   {
  89.     Serial.print(F("NINA FW v"));
  90.     Serial.print(fv);
  91.     Serial.print(F(" -> Newer NINA FW v"));
  92.     Serial.print(WIFI_FIRMWARE_LATEST_VERSION);
  93.     Serial.println("\n");
  94.   }
  95. }
  96.  
  97. void connectWifi() {
  98.   // attempt to connect to Wifi network:
  99.   while (status != WL_CONNECTED)
  100.   {
  101.     Serial.print(F("Trying to connect to SSID: "));
  102.     Serial.println(ssid);
  103.     status = WiFi.begin(ssid, pass);
  104.     delay(1000);
  105.   }
  106.   Serial.println(F("Connected!"));
  107. }
  108.  
  109. void sendData(String t = "battery?") {
  110.   char a[60 + 1];
  111.   t.toCharArray(a, 60 + 1);
  112.   Udp.beginPacket(telloIP, telloPort);
  113.   Udp.write(a);
  114.   Udp.endPacket();
  115. }
  116.  
  117. char inputBuffer[90 + 1]; // Handles up to 90 bytes, with a null character termination.
  118.  
  119. const int ledPin =  LED_BUILTIN; //red led
  120. void setup()
  121. {
  122.   initBoard();
  123.   pinMode(ledPin, OUTPUT);
  124.   if (!telloReady)
  125.   {
  126.     inputBuffer[0] = '\0'; //Initialize string to terminator.
  127.     connectWifi();
  128.     printWiFiStatus();
  129.     Serial.println(F("\nStarting connection to UDP server..."));
  130.     Udp.begin(localPort);
  131.     delay(2000);
  132.     Serial.println(F("Sending SDK control command"));
  133.     sendData("command"); //Enable control over tello
  134.   } else {
  135.     digitalWrite(ledPin, LOW);
  136.   }
  137. }
  138.  
  139. int landed = 1;
  140. void loop()
  141. {
  142.  
  143.   //Serial command to UDP command Are you communicating through serial?
  144.       if (Serial.available() > 0)
  145.       {
  146.         char input = Serial.read();
  147.         static int s_len;
  148.         if (s_len >= 90) {
  149.         } else if (input != '\n' && input != '\r') {
  150.           inputBuffer[s_len++] = input;
  151.         } else {
  152.           sendData(inputBuffer);
  153.           Serial.print("RECEIVED MSG: ");
  154.           Serial.println(inputBuffer);
  155.           memset(inputBuffer, 0, sizeof(inputBuffer));
  156.           s_len = 0;
  157.         }
  158.       }
  159.  
  160.   // if there's data available from the server, read a packet
  161.     int packetSize = Udp.parsePacket();
  162.     if (packetSize)
  163.     {
  164.       Serial.print(F("Received packet from tello -> size "));
  165.       Serial.print(packetSize);
  166.       Serial.print(F(" - IP: "));
  167.       IPAddress remoteIp = Udp.remoteIP();
  168.       Serial.print(remoteIp);
  169.       Serial.print(F(", Port: "));
  170.       Serial.print(Udp.remotePort());
  171.       int len = Udp.read(packetBuffer, 255);
  172.       if (len > 0)
  173.       {
  174.         packetBuffer[len] = 0;
  175.       }
  176.       Serial.print(F(" Contents: "));
  177.       Serial.println(packetBuffer);
  178.     }
  179.  
  180.   if (telloReady)
  181.   {
  182.     //ACCELEROMETER
  183.     adxl.readXYZ(&x, &y, &z);
  184.     adxl.getAcceleration(xyz);
  185.     a = String(map(y, -255, 255, -100, 100)), //left/right (-100 ~ 100)
  186.     b = String(map(-x, -255, 255, -100, 100)), //forward/back(-100 ~ 100)
  187.     //c up/down (-100 ~ 100)
  188.     //d yaw (-100 ~ 100)
  189.     t = "rc " + a + " " + b + " " + c + " " + d; //TELLO: rc a b c d
  190.     if (t != LF) {
  191.       sendData(t); //SEND LIVE TO UDP
  192.       c = "0";  //Cause, we using momentum moments to go up and down
  193.       d = "0"; //Cause, we using momentum moments to turn
  194.       //Serial.println(t);
  195.       LF = t;
  196.     }
  197.     if (xyz[0] > 1) // forward momentum
  198.     {
  199.       Serial.println("TURN LEFT");
  200.       sendData("ccw 90");
  201.     }
  202.     if (xyz[1] > 1) // right momentum
  203.     {
  204.       Serial.println("TURN RIGHT");
  205.       sendData("cw 90");
  206.     }
  207.     if (xyz[2] > 1.4f) // up momentum
  208.     {
  209.       Serial.println("UP");
  210.       c = "100";
  211.     }
  212.  
  213.     //double tap
  214.     byte interrupts = adxl.getInterruptSource();
  215.     if (adxl.triggered(interrupts, ADXL345_DOUBLE_TAP)) {
  216.       if (landed == 1)
  217.       {
  218.         Serial.println("TAKEOFF");
  219.         sendData("takeoff");
  220.         landed = 0;
  221.         return;
  222.       } else {
  223.         Serial.println("LANDING");
  224.         sendData("land");
  225.         landed = 1;
  226.         return;
  227.       }
  228.     }
  229.  
  230.     //String h = String(x) + "\n" + String(y) + "\n" + String(z) + "\n" + String(xyz[0]) + "\n" + String(xyz[1]) + "\n" + String(xyz[2]);
  231.     //Serial.println(h);
  232.  
  233.     //Check battery every 14 seconds (Keeps connection alive)
  234.     //if (millis() - lastTimeItHappened >= 14000) { // 14 seconds later
  235.     //  lastTimeItHappened = millis();
  236.     //  Serial.println(F("Request Battery?"));
  237.     //  sendData();
  238.     //}
  239.   }
  240. }
  241.  
  242. void curve(int x1, int y1, int z1, int x2, int y2, int z2, int spd)
  243. {
  244.   String m = "curve " + String(x1) + " " + String(y1) + " " + String(z1) + " " + String(x2) + " " + String(y2) + " " + String(z2) + " " + String(spd);
  245.   sendData(m);
  246. }
  247.  
  248.  
  249. void printWiFiStatus()
  250. {
  251.   // print your board's IP address:
  252.   IPAddress ip = WiFi.localIP();
  253.   Serial.print(F("IP Address: "));
  254.   Serial.println(ip);
  255.  
  256.   // print the received signal strength:
  257.   long rssi = WiFi.RSSI();
  258.   Serial.print(F("Signal strength (RSSI):"));
  259.   Serial.print(rssi);
  260.   Serial.println(F(" dBm"));
  261.   telloReady = true;
  262.   digitalWrite(ledPin, LOW);
  263. }
  264.  
  265. int map(int x, int in_min, int in_max, int out_min, int out_max)
  266. {
  267.   return (x - in_min) * (out_max - out_min) / (in_max - in_min) + out_min;
  268. }
RAW Paste Data