M5Stack（MPU9250）控制TELLO
使用MPU9250连接特洛无人机实现体感遥控#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;
String message = "";
MPU9250 IMU;
float x;
float y;
char msgx;
char msgy;
String status_msg;
// 遥控命令_字符串变量
char command_str;
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 * IMU.aRes;
y = IMU.accelCount * 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 = 0;
}
}
delay(10);
return (char*) packetBuffer;
}
页:
[1]