M5Stack（MPU9250）控制TELLO

vany5921

使用MPU9250连接特洛无人机实现体感遥控

[mw_shl_code=arduino,true]#define LOAD_FONT2
#define LOAD_FONT4
#include <M5Stack.h>
#include "utility/MPU9250.h"
#include <WiFi.h>
#include <WiFiUdp.h>
// TELLO的SSID
const char* TELLO_SSID = "TELLO-XXXXXX";  // Tello的SSID
// TELLO的IP
const char* TELLO_IP = "192.168.10.1";
// TELLO_PORT
const int PORT = 8889;
// UDP
WiFiUDP Udp;
char packetBuffer[255];
String message = "";

MPU9250 IMU;

float x;
float y;
char msgx[6];
char msgy[6];
String status_msg;

// 遥控命令_字符串变量
char command_str[20];

void setup() {
  //M5Stack初始化
  M5.begin();

  //---标题设置
  M5.Lcd.fillRect(0,0,320,30,TFT_BLUE);
  M5.Lcd.drawCentreString("Tello Controller",160,2,4);
  //---X, Y表示
  M5.Lcd.setTextColor(TFT_YELLOW,TFT_BLACK);
  M5.Lcd.drawCentreString(" Accel-X : ",20,30,2);
  sprintf(msgx,"%-2.2f",x);
  M5.Lcd.drawCentreString(msgx,88,30,2);

  M5.Lcd.drawCentreString(" Accel-Y : ",240,30,2);
  sprintf(msgy,"%-2.2f",y);
  M5.Lcd.drawCentreString(msgy,294,30,2);

  M5.Lcd.fillRect(0,217,320,20,TFT_LIGHTGREY);
  //按键字符
  M5.Lcd.setTextColor(TFT_BLACK,TFT_YELLOW);
  M5.Lcd.drawCentreString(" TAKE OFF ",64,220,2);
  M5.Lcd.setTextColor(TFT_BLACK,TFT_RED);
  M5.Lcd.drawCentreString(" LANDING  ",250,220,2);
  M5.Lcd.setTextColor(TFT_BLACK,TFT_CYAN);
  M5.Lcd.drawCentreString("CW/CCW_U/D",160,220,2);
  //---方向图标
  M5.Lcd.fillTriangle(159,40,189,60,129,60,TFT_GREEN);
  M5.Lcd.fillTriangle(159,160,189,140,129,140,TFT_GREEN);
  M5.Lcd.fillTriangle(269,100,220,80,220,120,TFT_GREEN);
  M5.Lcd.fillTriangle(98,80,98,120,49,100,TFT_GREEN);
  //---方向显示文字
  M5.Lcd.setTextColor(TFT_WHITE,TFT_BLACK);
  M5.Lcd.drawCentreString("FORWARD",160,64,2);
  M5.Lcd.drawCentreString("BACK",160,120,2);
  M5.Lcd.drawCentreString("LEFT",120,92,2);
  M5.Lcd.drawCentreString("RIGHT",200,92,2);
  //---消息框
  M5.Lcd.drawRoundRect(0,180,319,30,4,TFT_WHITE);
  //---消息框标题文字
  M5.Lcd.setTextColor(TFT_WHITE,TFT_DARKGREEN);
  M5.Lcd.drawCentreString("<Message>",38,170,1);

  //M5.Lcd.setTextColor(TFT_WHITE,TFT_BLACK);
  //M5.Lcd.drawString(msg,4,190,1);

  //Wire初始化
  Wire.begin();
  //MPU9250初始化
  IMU.initMPU9250();
  //WiFi通信开始
  WiFi.begin(TELLO_SSID, "");
  //WiFi连接状态
  while (WiFi.status() != WL_CONNECTED) {
        print_msg("Now, WiFi Connecting......");
        delay(500);
  }
  print_msg("WiFi Connected.");
  // UDP
  Udp.begin(PORT);
  //Telloへ”command”发送命令
  print_msg("sendMessage commend");
  tello_command_exec("command");  
}

void loop() {
  // put your main code here, to run repeatedly:
  //获取X，Y数据
  if (IMU.readByte(MPU9250_ADDRESS, INT_STATUS) & 0x01){
    IMU.readAccelData(IMU.accelCount);
    IMU.getAres();
    x = IMU.accelCount[0] * IMU.aRes;
    y = IMU.accelCount[1] * IMU.aRes;
    sprintf(msgx,"%-2.2f",x);
    M5.Lcd.drawCentreString("      ",88,30,2);
    M5.Lcd.drawCentreString(msgx,88,30,2);
    sprintf(msgy,"%-2.2f",y);
    M5.Lcd.drawCentreString("      ",294,30,2);
    M5.Lcd.drawCentreString(msgy,294,30,2);
    print_msg("Operation Start!");

//按键处理
    //A键按下
    if(M5.BtnA.wasPressed()) {
      //起飞
      print_msg("TAKE OFF");
      tello_command_exec("takeoff");
    }

    //B键按下
    if(M5.BtnB.isPressed()) {
      //---对应日本手右摇杆
      M5.Lcd.setTextColor(TFT_WHITE,TFT_BLACK);
      M5.Lcd.drawCentreString("  DOWN  ",160,64,2);
      M5.Lcd.drawCentreString("  UP  ",160,120,2);
      M5.Lcd.drawCentreString(" CCW",120,92,2);
      M5.Lcd.drawCentreString("  CW ",200,92,2);
    } else {
      //---对应日本手左摇杆
      M5.Lcd.setTextColor(TFT_WHITE,TFT_BLACK);
      M5.Lcd.drawCentreString("FORWARD",160,64,2);
      M5.Lcd.drawCentreString("BACK",160,120,2);
      M5.Lcd.drawCentreString("LEFT",120,92,2);
      M5.Lcd.drawCentreString("RIGHT",200,92,2);
    }

    //C键按下   
    if(M5.BtnC.wasPressed()) {
      //降落
      print_msg("LAND");
      tello_command_exec("land");
    }

    // 根据倾斜发送指令
    if (fabs(x)> 0.3 || fabs(y)> 0.3){
        // 根据斜率x，y创建遥控命令命令的字符串


//按下B键
        if( M5.BtnB.isPressed() ) {
            sprintf(command_str,"rc 0 0 %d %d",int(y*100), int(-x*100) ); // 上升下降旋转
        } else {
            sprintf(command_str,"rc %d %d 0 0",int(-x*100), int(-y*100) ); // 前后左右移动
        }
        tello_command_exec(command_str);  // 发送遥控器命令
    } else {
        // 不倾斜时停止发送命令
        tello_command_exec("rc 0 0 0 0");
    }
    delay(50);
  }
  M5.update();
}

/////////////////////////////
/////////////////////////////
//定义函数
// 在画面信息区域显示状况信息
void print_msg(String status_msg){
  M5.Lcd.setTextColor(TFT_WHITE,TFT_BLACK);
  M5.Lcd.drawString("                          ",4,190,1);
  M5.Lcd.drawString(status_msg,4,190,1);
  status_msg="";
}

// 向Tello发送消息&执行命令
void tello_command_exec(char* tello_command){
  Udp.beginPacket(TELLO_IP, PORT);
  Udp.printf(tello_command);
  Udp.endPacket();
  delay(10);
}

// 从Tello接收消息
String listenMessage() {
  int packetSize = Udp.parsePacket();
  if (packetSize) {
    IPAddress remoteIp = Udp.remoteIP();
    int len = Udp.read(packetBuffer, 255);
    if (len > 0) {
      packetBuffer[len] = 0;
    }
  }
  delay(10);
  return (char*) packetBuffer;
}[/mw_shl_code]