API tools faq
paste
Advertisement
AsmodHacker

Vidor 4000 - Tello WIP - Requires Libs BMx280i2c-MPU9250-SS1306(DISPLAY)

AsmodHacker
May 22nd, 2021
1,791
0
Never
Add comment
Not a member of Pastebin yet? Sign Up, it unlocks many cool features!
Arduino 23.88 KB | None | 0 0
raw download clone embed print report
  1. #include "defines.h"
  2. #include <SPI.h>
  3. #include <WiFiNINA_Generic.h>
  4. #include <utility/wifi_drv.h>
  5. #include <WiFiUdp_Generic.h>
  6. #include <Wire.h>
  7. #include <Adafruit_GFX.h>
  8. #include <Adafruit_SSD1306.h>
  9. #include <BMx280I2C.h>
  10. #include <MPU9250_WE.h>
  11.  
  12.  
  13. //Scheduler
  14. #include <Scheduler.h>
  15. #if defined(ARDUINO_ARCH_SAMD) && (USB_PID == 0x8D21)
  16. #define Serial SerialUSB
  17. #endif
  18. //
  19.  
  20. //I2C device found at address 0x3C  ! DISPLAY
  21. //I2C device found at address 0x60  !
  22. //I2C device found at address 0x68  ! ACCELEROMETER
  23. //I2C device found at address 0x6B  !
  24. //I2C device found at address 0x76  ! BAROMETER/TEMP SENSOR
  25.  
  26. //SENSORS/DISPLAY
  27. #define MPU9250_ADDR   0x68 // Accelerometer Address
  28. #define SCREEN_ADDRESS 0x3C // Display
  29. #define BMP280_ADDR    0x76 // Barometer
  30. #define SCREEN_WIDTH   128  // OLED display width, in pixels
  31. #define SCREEN_HEIGHT  64   // OLED display height, in pixels
  32. #define OLED_RESET     -1   // Sharing reset pin
  33.  
  34. //VIDOR 4000 RGB LED
  35. #define RGBLEDR        25
  36. #define RGBLEDG        26
  37. #define RGBLEDB        27
  38.  
  39. //Stupid things that aren't really requiered
  40. #define NUMFLAKES      10 // Number of snowflakes in the animation example
  41. #define LOGO_HEIGHT    16
  42. #define LOGO_WIDTH     16
  43.  
  44. Adafruit_SSD1306 display(SCREEN_WIDTH, SCREEN_HEIGHT, &Wire, OLED_RESET);
  45. MPU9250_WE myMPU9250 = MPU9250_WE(MPU9250_ADDR);
  46. BMx280I2C bmx280(BMP280_ADDR);
  47.  
  48. //static const unsigned char PROGMEM logo_bmp[] =
  49. //{ 0b00000000, 0b11000000,
  50. //  0b00000001, 0b11000000,
  51. //  0b00000001, 0b11000000,
  52. //  0b00000011, 0b11100000,
  53. //  0b11110011, 0b11100000,
  54. //  0b11111110, 0b11111000,
  55. //  0b01111110, 0b11111111,
  56. //  0b00110011, 0b10011111,
  57. //  0b00011111, 0b11111100,
  58. //  0b00001101, 0b01110000,
  59. //  0b00011011, 0b10100000,
  60. //  0b00111111, 0b11100000,
  61. //  0b00111111, 0b11110000,
  62. //  0b01111100, 0b11110000,
  63. //  0b01110000, 0b01110000,
  64. //  0b00000000, 0b00110000
  65. //};
  66.  
  67. /////////////////////////////////////////////////////////////////////////////////////////////
  68.  
  69. // Global variables to keep track of update rates
  70. unsigned long startTime, lastPrint, accelReadCounter, gyroReadCounter, magReadCounter, tempReadCounter;
  71. const unsigned int PRINT_RATE = 100;
  72. unsigned int localPort = 9000; // local port to listen on
  73. unsigned int localStatePort = 8890; // local state port
  74. unsigned int localVideoPort = 11111; // local video port
  75. //MY TELLOS: 627706-FDFD3B
  76. char packetBuffer[255], inputBuffer[90 + 1], msgData[90], telloIP[] = "192.168.10.1", apIP[] = "0.0.0.0", ssid[] = "TELLO-627706", pass[] = "";
  77. uint16_t telloPort = 8889, telloStatePort = 8890, telloVideo = 11111;
  78. const int ledPin =  LED_BUILTIN; //red led vidor 4000 - weirdly... it's backwards (high is low and low is high)
  79. bool telloReady = false;
  80. String a, b, c, d, t;
  81. int x, y, z, keyIndex;
  82. double xyz[3];
  83. static unsigned long lastTimeItHappened = 0;
  84. String LF;
  85. WiFiUDP Udp;
  86. WiFiClient client1;
  87. WiFiServer serverHTML(80);
  88. WiFiServer serverSTATUS(8890);
  89. WiFiServer serverVIDEO(11111);
  90. int status1 = WL_IDLE_STATUS;
  91. int status2 = WL_IDLE_STATUS;
  92.  
  93. //////////////////////////////////////////////////////////////////////////////////////////////
  94.  
  95. void initBoard() {
  96.  
  97.   //Configure rgb builtin
  98.   WiFiDrv::pinMode(RGBLEDR, OUTPUT);
  99.   WiFiDrv::pinMode(RGBLEDG, OUTPUT);
  100.   WiFiDrv::pinMode(RGBLEDB, OUTPUT);
  101.  
  102.   //Initialize serial and wait for port to open:
  103.   Serial.begin(115200);
  104.  
  105.   startTime = millis();
  106.   inputBuffer[0] = '\0'; //Initialize string buffer to terminator.
  107.  
  108.   Serial.print(BOARD_NAME);
  109.   Serial.print(", ");
  110.   Serial.println(WIFININA_GENERIC_VERSION);
  111.  
  112.   // check for the WiFi module:
  113.   if (WiFi.status() == WL_NO_MODULE)
  114.   {
  115.     Serial.println(F("Communication with WiFi module failed!"));
  116.     // don't continue
  117.     while (true);
  118.   }
  119.  
  120.   String fv = WiFi.firmwareVersion();
  121.   if (fv < WIFI_FIRMWARE_LATEST_VERSION)
  122.   {
  123.     Serial.print(F("NINA FW v"));
  124.     Serial.print(fv);
  125.     Serial.print(F(" -> Newer NINA FW v"));
  126.     Serial.print(WIFI_FIRMWARE_LATEST_VERSION);
  127.     Serial.println("\n");
  128.   }
  129.  
  130.   Wire.begin();
  131.  
  132.   if (!myMPU9250.init()) {
  133.     Serial.println("MPU9250 does not respond");
  134.   }
  135.   else {
  136.     Serial.println("MPU9250 is connected");
  137.   }
  138.  
  139.   myMPU9250.setMagOpMode(AK8963_CONT_MODE_8HZ);
  140.   myMPU9250.setAccRange(MPU9250_ACC_RANGE_16G);
  141.   myMPU9250.enableAccDLPF(true);
  142.   myMPU9250.setAccDLPF(MPU9250_DLPF_0);
  143.  
  144.   //Magnometer Ready?
  145.   myMPU9250.isMagDataReady();
  146.  
  147.   //
  148.   //begin() checks the Interface, reads the sensor ID (to differentiate between BMP280 and BME280)
  149.   //and reads compensation parameters.
  150.   Serial.println("Starting Barometer");
  151.   if (!bmx280.begin())
  152.   {
  153.     Serial.println("begin() failed. check your BMx280 Interface and I2C Address.");
  154.     while (1);
  155.   }
  156.   Serial.println("Barometer Working!");
  157.  
  158.   if (bmx280.isBME280())
  159.     Serial.println("sensor is a BME280");
  160.   else
  161.     Serial.println("sensor is a BMP280");
  162.  
  163.   //reset sensor to default parameters.
  164.   bmx280.resetToDefaults();
  165.  
  166.   //by default sensing is disabled and must be enabled by setting a non-zero
  167.   //oversampling setting.
  168.   //set an oversampling setting for pressure and temperature measurements.
  169.   bmx280.writeOversamplingPressure(BMx280MI::OSRS_P_x16);
  170.   bmx280.writeOversamplingTemperature(BMx280MI::OSRS_T_x16);
  171.  
  172.   //if sensor is a BME280, set an oversampling setting for humidity measurements.
  173.   if (bmx280.isBME280()) {
  174.     bmx280.writeOversamplingHumidity(BMx280MI::OSRS_H_x16);
  175.   }
  176.   //
  177.   //BAROMETER
  178.   Scheduler.start(THREAD3START, THREAD3LOOP);
  179.  
  180.   //SCREEN
  181.   if (!display.begin(SSD1306_SWITCHCAPVCC, SCREEN_ADDRESS)) {
  182.     Serial.println(F("SSD1306 allocation failed"));
  183.     for (;;); // Don't proceed, loop forever
  184.   }
  185.  
  186.   Serial.println("Screen Enabled");
  187.   Serial.println("Position the MPU9250 flat and don't move it - calibrating...");
  188.  
  189.   //DOF CALIBRATION
  190.   for (int i = 0; i < 15; i++)
  191.   {
  192.     //10DOF
  193.     xyzFloat gValue = myMPU9250.getGValues();
  194.     xyzFloat angle = myMPU9250.getAngles();
  195.     xyzFloat magValue = myMPU9250.getMagValues(); // returns magnetic flux density [µT]
  196.  
  197.     //SCREEN
  198.     display.clearDisplay(); // Clear display buffer
  199.     display.println(F("CALIBRATE"));
  200.     //display.fillRect(126/2 + (-angle.y), 64/2+(-angle.x), 2, 2, SSD1306_INVERSE);
  201.     display.drawLine(0, 32 + (-map(gValue.x, -1.0f, 1.0f, -32, 32)), 64,  32 + (-map(gValue.x, -1.0f, 1.0f, -32, 32)), SSD1306_WHITE);
  202.     display.drawLine(32 + (-map(gValue.y, -1.0f, 1.0f, -32, 32)), 0, 32 + (-map(gValue.y, -1.0f, 1.0f, -32, 32)), 64, SSD1306_WHITE);
  203.     //display.drawRect(0, 0, 64, 64, SSD1306_INVERSE);
  204.     display.setTextSize(1);
  205.     display.setTextColor(SSD1306_WHITE);
  206.     display.setCursor(70, 0);
  207.     display.println(F("CALIBRATE"));
  208.     display.setCursor(70, 32);
  209.     display.println(F("ANGLE"));
  210.     display.setCursor(70, 40);
  211.     display.println(String(angle.x));
  212.     display.setCursor(70, 48);
  213.     display.println(String(angle.y));
  214.     display.setCursor(70, 56);
  215.     display.println(String(angle.z));
  216.     display.setCursor(70, 64);
  217.     display.display(); // Update screen with each newly-drawn line
  218.     delay(5);
  219.     myMPU9250.autoOffsets();
  220.   }
  221.   Serial.println("Done!");
  222. }
  223.  
  224. /////////////////////////////////////////////////////////////////////////////////////////
  225.  
  226. //Set the Vidor 4000 RGB led color
  227. void setRGB(int r, int g, int b)
  228. {
  229.   WiFiDrv::analogWrite(RGBLEDR, r); // RED
  230.   WiFiDrv::analogWrite(RGBLEDG, g); // GREEN
  231.   WiFiDrv::analogWrite(RGBLEDB, b); // BLUE
  232. }
  233.  
  234. // Sends data to the tello UDP server ip
  235. void sendData(char* SIP, uint16_t SPort, String t = "battery?") {
  236.   char a[60 + 1];
  237.   t.toCharArray(a, 60 + 1);
  238.   Udp.beginPacket(SIP, SPort);
  239.   Udp.write(a);
  240.   Udp.endPacket();
  241. }
  242.  
  243. ////////////////////////////////////////////////////////////////////////////////////////////
  244.  
  245. //Creates an access point on the vidor 4000 for the tello to connect to.
  246. void createAP() {
  247.   IPAddress ip(192, 168, 10, 1);
  248.   IPAddress gateway(0, 0, 0, 0);
  249.   IPAddress subnet(255, 255, 255, 0);
  250.   IPAddress dns(0, 0, 0, 0);
  251.   WiFi.config(ip, dns, gateway, subnet);
  252.   WiFi.config(IPAddress(192, 168, 10, 1));
  253.   WiFi.setHostname("2022-GSAVAGE-CONCEPT-CONTROLLER-V1");
  254.   Serial.println("Creating Access Point");
  255.   status2 = WiFi.beginAP("TELLO-MASTER", "3ow58cnoerlsijdn");
  256.   if (status2 != WL_AP_LISTENING) {
  257.     Serial.println("Creating access point failed");
  258.     // don't continue
  259.     while (true);
  260.   }
  261.   delay(10000);
  262.   printAPStatus();
  263.   Serial.println("AP SSID: TELLO-MASTER, Pass: 3ow58cnoerlsijdn");
  264.   serverVIDEO.begin();
  265.   delay(2000);
  266.   Serial.println("TELLO VIDEO Server Listening (11111)");
  267.   serverSTATUS.begin();
  268.   delay(2000);
  269.   Serial.println("TELLO STATUS Server Listening (8890)");
  270.   serverHTML.begin();
  271.   delay(2000);
  272.   Serial.println("TELLO WEB Server Listening (80)");
  273.   Serial.println("Starting State UDP Server Port 8890");
  274.   Udp.begin(localStatePort);
  275.   delay(2000);
  276.   Serial.println("Starting Video UDP Server Port 11111");
  277.   Udp.begin(localVideoPort);
  278.   delay(2000);
  279.   telloReady = true;
  280.   Serial.println("TELLO SHOULD BE READY NOW");
  281.  
  282.   //Init Web Server Thread
  283.   Scheduler.start(THREAD2START, THREAD2LOOP);
  284.  
  285. }
  286.  
  287. //////////////////////////////////////////////////////////////////////////////////////////
  288.  
  289. void connectWifi() {
  290.  
  291.   //nUpdating display
  292.   display.clearDisplay(); // Clear display buffer
  293.   display.drawCircle(display.width() - 10, display.height() / 2, 6, SSD1306_WHITE);
  294.   display.setTextSize(2);
  295.   display.setTextColor(SSD1306_WHITE);
  296.   display.setCursor(0, 0);
  297.   display.println(F("TELLO SCAN"));
  298.   display.setTextSize(1);
  299.   display.setCursor(0, 22);
  300.   display.println(F("LOOKING FOR TELLO"));
  301.   display.println((String)telloIP);
  302.   display.setCursor(0, 38);
  303.   display.print(F("PORT:"));
  304.   display.println((String)telloPort);
  305.   display.display(); // Update screen
  306.  
  307.   // Attempt to connect to Wifi network:
  308.   IPAddress ip(192, 168, 10, 1);
  309.   IPAddress gateway(0, 0, 0, 0);
  310.   IPAddress subnet(255, 255, 255, 0);
  311.   IPAddress dns(0, 0, 0, 0);
  312.   WiFi.config(ip, dns, gateway, subnet);
  313.   WiFi.setHostname("2022-CONCEPT-CONTROLLER-V1");
  314.   while (status1 != WL_CONNECTED)
  315.   {
  316.     Serial.print(F("Trying to connect to SSID: "));
  317.     Serial.println(ssid);
  318.     status1 = WiFi.begin(ssid, pass);
  319.     delay(1000);
  320.   }
  321.   Serial.println("Connected!");
  322.  
  323.   //Print connection info
  324.   printWiFiStatus();
  325.  
  326.   // Start udp port 9000
  327.   Serial.println(F("Opening listen port"));
  328.   Udp.begin(localPort);
  329.   delay(10000);
  330.  
  331.   Serial.println(F("Sending SDK command"));
  332.  
  333.  
  334.  
  335.   //Send command to tello
  336.   sendData(telloIP, telloPort, "command"); //Enable control over tello
  337.   delay(1000);
  338.   sendData(telloIP, telloPort, "takeoff"); //Enable control over tello
  339.  
  340.   //todo
  341.   //sendData(telloIP, telloPort, "land"); //Enable control over tello
  342.   //Serial.println(F("Sending StreamOn command"));
  343.   //sendData(telloIP, telloPort, "streamon");//Enable Video Stream
  344.   //Serial.println(F("Sending AP command: ap TELLO-MASTER 3ow58cnoerlsijdn"));
  345.   //sendData(telloIP, telloPort, "ap TELLO-MASTER 3ow58cnoerlsijdn"); //Only works with EDU version
  346.  
  347.   //command tello via serial
  348.   Scheduler.start(THREAD0START, THREAD0LOOP);
  349.  
  350.   //Parse UDP Packets
  351.   Scheduler.start(THREAD1START, THREAD1LOOP);
  352.  
  353.  
  354. }
  355.  
  356. ///////////////////////////////////////////////////////////////////////////////////////
  357.  
  358.  
  359.  
  360.  
  361.  
  362.  
  363.  
  364. ////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
  365.  
  366. void setup()
  367. {
  368.  
  369.   //Start Devices
  370.   initBoard();
  371.  
  372.   //Display Circle Loop
  373.   Scheduler.start(THREAD4START, THREAD4LOOP);
  374.  
  375.   //Access Point
  376.   //createAP();
  377.  
  378.   //Tello WiFi
  379.   connectWifi();
  380.  
  381.   telloReady = true;
  382. }
  383.  
  384. //////////////////////////////////////////////////////////////////////////////////////////
  385. String CONTROL;
  386.  
  387. void loop()
  388. {
  389.  
  390.   //10DOF
  391.   xyzFloat gValue = myMPU9250.getGValues();
  392.   xyzFloat angle = myMPU9250.getAngles();
  393.   xyzFloat magValue = myMPU9250.getMagValues(); // returns magnetic flux density [µT]
  394.  
  395.   //SCREEN
  396.   display.clearDisplay(); // Clear display buffer
  397.   display.drawCircle(display.width() / 4, display.height() / 2, 31, SSD1306_WHITE);
  398.   display.drawCircle(display.width() / 4, display.height() / 2, 2, SSD1306_WHITE);
  399.   //display.fillRect(126/2 + (-angle.y), 64/2+(-angle.x), 2, 2, SSD1306_INVERSE);
  400.   display.drawLine(0, 32 + (-map(gValue.x, -1.0f, 1.0f, -32, 32)), 64,  32 + (-map(gValue.x, -1.0f, 1.0f, -32, 32)), SSD1306_WHITE);
  401.   display.drawLine(32 + (-map(gValue.y, -1.0f, 1.0f, -32, 32)), 0, 32 + (-map(gValue.y, -1.0f, 1.0f, -32, 32)), 64, SSD1306_WHITE);
  402.   //display.drawRect(0, 0, 64, 64, SSD1306_INVERSE);
  403.   display.setTextSize(0.1f);
  404.   display.setTextColor(SSD1306_WHITE);
  405.   display.setCursor(70, 0);
  406.   display.println(F("XYZ"));
  407.   display.setCursor(70, 8);
  408.   display.println(String(map(gValue.x, -1.0f, 1.0f, -100, 100)));
  409.   display.setCursor(70, 16);
  410.   display.println(String(map(gValue.y, -1.0f, 1.0f, -100, 100)));
  411.   display.setCursor(70, 24);
  412.   display.println(String(map(gValue.z, -1.0f, 1.0f, -100, 100)));
  413.   display.setCursor(70, 32);
  414.   display.println(F("ANGLE"));
  415.   display.setCursor(70, 40);
  416.   display.println(String(angle.x));
  417.   display.setCursor(70, 48);
  418.   display.println(String(angle.y));
  419.   display.setCursor(70, 56);
  420.   display.println(String(angle.z));
  421.   display.setCursor(70, 64);
  422.   display.display(); // Update screen with each newly-drawn line
  423.  
  424.   //UDP to send command string to control the axis X,Y OF THE DRONE
  425.   CONTROL = "rc " + (String)map(gValue.x, -1.0f, 1.0f, -100, 100) + " " + (String)map(gValue.y, -1.0f, 1.0f, -100, 100) + " 0 0";
  426.   //String sxyz = String(angle.x) + " " + String(angle.y) + " " + String(angle.z);
  427.   //Serial.println(CONTROL);
  428.   //Serial.println(sxyz);
  429.   sendData(telloIP, telloPort, CONTROL);
  430.  
  431.   //////////////////////////////////////////////////////////////////////////
  432.   //Check battery every 14 seconds (Keeps connection alive)
  433.   //  if (millis() - lastTimeItHappened >= 14000) { // 14 seconds later
  434.   //    lastTimeItHappened = millis();
  435.   //    //Serial.println(F("Repeating (14s) Battery?"));
  436.   //    sendData(telloIP, telloPort, "battery?");
  437.   //  }
  438. }
  439.  
  440. void THREAD0START(){}
  441. void THREAD0LOOP()
  442. {
  443.   //command tello via serial
  444.   if (Serial.available() > 0)
  445.   {
  446.     char input = Serial.read();
  447.     static int s_len;
  448.     if (s_len >= 90) {
  449.     } else if (input != '\n' && input != '\r') {
  450.       inputBuffer[s_len++] = input;
  451.     } else {
  452.       if (telloReady) {
  453.         sendData(telloIP, telloPort, inputBuffer);
  454.         Serial.print("Serial2UDP -> ");
  455.         Serial.println(inputBuffer);
  456.         memset(inputBuffer, 0, sizeof(inputBuffer));
  457.         s_len = 0;
  458.       }
  459.     }
  460.   }
  461. }
  462.  
  463. void THREAD1START(){}
  464. void THREAD1LOOP()
  465. {
  466.   // if there's data available on UDP for us
  467.   int packetSize = Udp.parsePacket();
  468.   if (packetSize)
  469.   {
  470.     Serial.print(F("UDP Received <- size "));
  471.     Serial.print(packetSize);
  472.     Serial.print(F(" - FROM IP: "));
  473.     IPAddress remoteIp = Udp.remoteIP();
  474.     Serial.print(remoteIp);
  475.     Serial.print(F(", Port: "));
  476.     Serial.print(Udp.remotePort());
  477.     int len = Udp.read(packetBuffer, 255);
  478.     if (len > 0)
  479.     {
  480.       packetBuffer[len] = 0;
  481.     }
  482.     Serial.print(F(" MSG: "));
  483.     Serial.println(packetBuffer);
  484.   }
  485. }
  486.  
  487. void THREAD2START(){}
  488. void THREAD2LOOP()
  489. {
  490.   //////////////////////////////////////////////////////////////////////
  491.   //Something comnnects to the AP
  492.   //  if (status1 != WiFi.status()) {
  493.   //    // it has changed update the variable
  494.   //    status1 = WiFi.status();
  495.   //    if (status1 == WL_CONNECTED) {
  496.   //      // a device has connected to the AP
  497.   //      Serial.println("Device connected to AP");
  498.   //      sendData(telloIP, telloPort, "command");
  499.   //      delay(1000);
  500.   //      sendData(telloIP, telloPort, "streamon");
  501.   //      delay(5000);
  502.   //      sendData(telloIP, telloPort, "takeoff");
  503.   //      delay(5000);
  504.   //      sendData(telloIP, telloPort, "land");
  505.   //    } else {
  506.   //
  507.   //      // a device has disconnected from the AP, and we are back in listening mode
  508.   //      Serial.print("Device disconnected from AP: ");
  509.   //    }
  510.   //  }
  511.  
  512.   /////////////////////////////////////////////////////////////////////////
  513.   //
  514.   //  client1 = serverHTML.available(); // listen for incoming clients
  515.   //  if (client1) { // if you get a client,
  516.   //    Serial.println("AP client connected"); // print a message out the serial port
  517.   //    String currentLine = ""; // make a String to hold incoming data from the client
  518.   //    while (client1.connected()) { // loop while the client's connected
  519.   //      if (client1.available()) { // if there's bytes to read from the client,
  520.   //        char c = client1.read(); // read a byte, then
  521.   //        Serial.write(c); // print it out the serial monitor
  522.   //        if (c == '\n') { // if the byte is a newline character
  523.   //          if (currentLine.length() == 0) {
  524.   //            client1.println("HTTP/1.1 200 OK");
  525.   //            client1.println("Content-type:text/html");
  526.   //            client1.println();
  527.   //            client1.print("<html><head></head><body>");
  528.   //            client1.print(client1);
  529.   //            //client1.print("<video src=\"");
  530.   //            //client1.print("udp://192.168.10.1:11111\"""brightsign-properties=\"""StreamTimeout:0;StreamLowLatency:0;\"");
  531.   //            //client1.print(">");
  532.   //            client1.print("Click <a href=\"/E\">HERE</a> to take control<br>Click <a href=\"/H\">HERE</a> to take off<br>Click <a href=\"/L\">HERE</a> to land<br>");
  533.   //            client1.print("</body></html>");
  534.   //            client1.println();
  535.   //            break;
  536.   //          }
  537.   //          else { // if you got a newline, then clear currentLine:
  538.   //            currentLine = "";
  539.   //          }
  540.   //        }
  541.   //        else if (c != '\r') { // anything but carriage return
  542.   //          currentLine += c; // add it to the end of the currentLine
  543.   //        }
  544.   //        // Check to see if the client request was "GET /H" or "GET /L":
  545.   //        if (currentLine.endsWith("GET /E")) {
  546.   //          sendData(telloIP, telloPort, "command");
  547.   //          sendData("192.168.4.1", telloPort, "command");
  548.   //          sendData("0.0.0.0", telloPort, "command");
  549.   //          sendData(telloIP, telloStatePort, "command");
  550.   //          sendData("192.168.4.1", telloStatePort, "command");
  551.   //          sendData("0.0.0.0", telloStatePort, "command");
  552.   //        }
  553.   //        if (currentLine.endsWith("GET /H")) {
  554.   //          sendData(telloIP, telloPort, "takeoff");
  555.   //          sendData("192.168.4.1", telloPort, "takeoff");
  556.   //          sendData("0.0.0.0", telloPort, "takeoff");
  557.   //          sendData(telloIP, telloStatePort, "takeoff");
  558.   //          sendData("192.168.4.1", telloStatePort, "takeoff");
  559.   //          sendData("0.0.0.0", telloStatePort, "takeoff");
  560.   //        }
  561.   //        if (currentLine.endsWith("GET /L")) {
  562.   //          sendData(telloIP, telloPort, "land");
  563.   //          sendData("192.168.4.1", telloPort, "land");
  564.   //          sendData("0.0.0.0", telloPort, "land");
  565.   //          sendData(telloIP, telloStatePort, "land");
  566.   //          sendData("192.168.4.1", telloStatePort, "land");
  567.   //          sendData("0.0.0.0", telloStatePort, "land");
  568.   //        }
  569.   //      }
  570.   //    }
  571.   //    client1.stop();
  572.   //  }
  573. }
  574.  
  575. void THREAD3START() {}
  576. void THREAD3LOOP() {
  577.   //BAROMETER
  578.   if (!bmx280.measure())
  579.   {
  580.     Serial.println("could not start measurement, is a measurement already running?");
  581.     return;
  582.   }
  583.   do
  584.   {
  585.     delay(100);
  586.   } while (!bmx280.hasValue());
  587.  
  588.   Serial.print("Pressure: ");
  589.   Serial.print(bmx280.getPressure());
  590.   Serial.print(" - Pressure (64 bit): ");
  591.   Serial.print(bmx280.getPressure64());
  592.   Serial.print(" - Temperature: ");
  593.   Serial.println(bmx280.getTemperature());
  594.  
  595.   //important: measurement data is read from the sensor in function hasValue() only.
  596.   //make sure to call get*() functions only after hasValue() has returned true.
  597.   if (bmx280.isBME280())
  598.   {
  599.     Serial.print("Humidity: ");
  600.     Serial.println(bmx280.getHumidity());
  601.   }
  602. }
  603.  
  604. void THREAD4START() {
  605. }
  606.  
  607. int invert = SSD1306_WHITE;
  608. int scalec = 0;
  609. void THREAD4LOOP() {
  610.   for (scalec = 0; scalec < 10; scalec++)
  611.   {
  612.     display.drawCircle(display.width() - 10, display.height() / 2, scalec, invert);
  613.     delay(10);
  614.     display.display();
  615.   }
  616.   invert = !invert;
  617. }
  618.  
  619. ///////////////////////////////////////////////////////////////////////////////////////////
  620.  
  621. void curve(int x1, int y1, int z1, int x2, int y2, int z2, int spd)
  622. {
  623.   String m = "curve " + String(x1) + " " + String(y1) + " " + String(z1) + " " + String(x2) + " " + String(y2) + " " + String(z2) + " " + String(spd);
  624.   sendData(telloIP, telloPort, m);
  625. }
  626.  
  627. ///////////////////////////////////////////////////////////////////////////////////////////
  628.  
  629. void printWiFiStatus()
  630. {
  631.   // print your board's IP address:
  632.   IPAddress ip = WiFi.localIP();
  633.   Serial.print(F("IP Address: "));
  634.   Serial.print(ip);
  635.   // print the received signal strength:
  636.   long rssi = WiFi.RSSI();
  637.   Serial.print(F(" Signal strength (RSSI):"));
  638.   Serial.print(rssi);
  639.   Serial.println(F(" dBm"));
  640. }
  641.  
  642. //////////////////////////////////////////////////////////////////////////////////////////////
  643.  
  644. void printAPStatus()
  645. {
  646.   IPAddress ip;
  647.   Serial.print("SSID: ");
  648.   Serial.println(WiFi.SSID());
  649.   ip = WiFi.localIP();
  650.   Serial.print("IP Address  : ");
  651.   Serial.println(ip);
  652.   ip = WiFi.gatewayIP();
  653.   Serial.print("IP Gateway  : ");
  654.   Serial.println(ip);
  655.   ip = WiFi.subnetMask();
  656.   Serial.print("Subnet Mask : ");
  657.   Serial.println(ip);
  658.   ip = WiFi.localIP();
  659.   Serial.print("To see this page in action, open a browser to http://");
  660.   Serial.println(ip);
  661. }
  662.  
  663. /////////////////////////////////////////////////////////////////////////////////////////
  664.  
  665. void printMacAddress(byte mac[]) {
  666.   for (int i = 5; i >= 0; i--) {
  667.     if (mac[i] < 16) {
  668.       Serial.print("0");
  669.     }
  670.     Serial.print(mac[i], HEX);
  671.     if (i > 0) {
  672.       Serial.print(":");
  673.     }
  674.   }
  675.   Serial.println();
  676. }
  677.  
  678. ////////////////////////////////////////////////////////////////////////////////////////
  679.  
  680. float map(float x, float in_min, float in_max, float out_min, float out_max)
  681. {
  682.   return (x - in_min) * (out_max - out_min) / (in_max - in_min) + out_min;
  683. }
  684.  
  685. ////////////////////////////////////////////////////////////////////////////////////////
  686.  
  687. void drawrect(void) {
  688.   display.clearDisplay();
  689.  
  690.   for (int16_t i = 0; i < display.height() / 2; i += 2) {
  691.     display.drawRect(i, i, display.width() - 2 * i, display.height() - 2 * i, SSD1306_WHITE);
  692.  
  693.     display.display(); // Update screen with each newly-drawn rectangle
  694.     delay(1);
  695.   }
  696.   delay(2000);
  697. }
  698. void drawcircle(void) {
  699.   display.clearDisplay();
  700.  
  701.   for (int16_t i = 0; i < max(display.width(), display.height()) / 2; i += 2) {
  702.     display.drawCircle(display.width() / 2, display.height() / 2, i, SSD1306_WHITE);
  703.     display.display();
  704.     delay(1);
  705.   }
  706.   delay(2000);
  707. }
  708. void drawroundrect(void) {
  709.   display.clearDisplay();
  710.  
  711.   for (int16_t i = 0; i < display.height() / 2 - 2; i += 2) {
  712.     display.drawRoundRect(i, i, display.width() - 2 * i, display.height() - 2 * i,
  713.                           display.height() / 4, SSD1306_WHITE);
  714.     display.display();
  715.     delay(1);
  716.   }
  717.   delay(2000);
  718. }
  719. void drawtriangle(void) {
  720.   display.drawTriangle(
  721.       display.width() / 2  , display.height() / 2 - i,
  722.       display.width() / 2 - i, display.height() / 2 + i,
  723.       display.width() / 2 + i, display.height() / 2 + i, SSD1306_WHITE);
  724.     display.display();
  725.     delay(1);
  726.  
  727. }
  728. ////////////////////////////////////////////////////////////////////////////////////////////////
  729. //void testdrawbitmap(void) {
  730. //  display.clearDisplay();
  731. //
  732. //  display.drawBitmap(
  733. //    (display.width()  - LOGO_WIDTH ) / 2,
  734. //    (display.height() - LOGO_HEIGHT) / 2,
  735. //    logo_bmp, LOGO_WIDTH, LOGO_HEIGHT, 1);
  736. //  display.display();
  737. //  delay(1000);
  738. //}
  739.  
  740. //////////////////////////////////////////////////////////////////////////////////////////////////
  741.  
Advertisement
Add Comment
Please, Sign In to add comment
Advertisement
Public Pastes
Advertisement
We use cookies for various purposes including analytics. By continuing to use Pastebin, you agree to our use of cookies as described in the Cookies Policy.  OK, I Understand
Not a member of Pastebin yet?
Sign Up, it unlocks many cool features!