/**
* Gamesir joystick control handling
* - Receive command from the joystick
* - Control the drone by received command
*
* @param arg Parameter about task control
*/
void gamesir_task(void *arg)
{
uint8_t command_init = 0;
uint8_t mac_init = 0;
uint8_t stop_cnt = 5;
for (;;)
{
if (mac_init == 0)
{
CommonSerial.print("[TELLO] getmac?");
delay(100);
#ifdef __DEFAULT_LOG__
Serial.println("gamesir_task(): mac is ok?");
#endif
if (rmtt_joystick_mac_is_valid())
{
#ifdef __DEFAULT_LOG__
Serial.println("gamesir_task(): ble mac init");
#endif
p_tt_gamesir->Init(get_rmtt_joystick_mac());
mac_init = 1;
}
}
else if ((command_init == 0) && (p_tt_gamesir->GetConnectedStatus()))
{
tt_sdk.SDKOn();
delay(100);
if (rmtt_bool_is_valid())
{
command_init = 1;
}
}
else if (p_tt_gamesir->DataIsValid())
{
// Serial.println("data is update");
PlainData data = p_tt_gamesir->GetData();
int lx = ((float)data.left_x_3d - 512) / 512.0 * 100;
int ly = ((float)data.left_y_3d - 512) / 512.0 * 100;
int rx = ((float)data.right_x_3d - 512) / 512.0 * 100;
int ry = ((float)data.right_y_3d - 512) / 512.0 * 100;
if ((data.btn3 == 0x01) && (data.L2))
{
tt_sdk.Flip('f');
}
else if ((data.btn3 == 0x03) && (data.L2))
{
tt_sdk.Flip('r');
}
else if ((data.btn3 == 0x05) && (data.L2))
{
tt_sdk.Flip('b');
}
else if ((data.btn3 == 0x07) && (data.L2))
{
tt_sdk.Flip('l');
}
else if ((data.Y) && (data.R2))
{
if (takeoff_status == 0)
{
tt_sdk.TakeOff();
takeoff_status = 1;
}
else
{
tt_sdk.Land();
takeoff_status = 0;
}
}
else
{
#ifdef BLE_JAPAN_CTRL
tt_sdk.SetRC(lx, -ly, -ry, rx);
#else
tt_sdk.SetRC(rx, -ry, -ly, lx);
#endif
}
}
/* 避免rc指令粘包 */
delay(10);
/* Regularly send data packet to ensure the drone
floating steadily after the controller was offline*/
if ((now_time - last_clean_time > 300) && command_init)
{
if (p_tt_gamesir->GetDataOffline())
{
if (stop_cnt)
{
tt_sdk.SetRC(0, 0, 0, 0);
stop_cnt--;
}
}
else
{
/* keepactive */
CommonSerial.print("[TELLO] keepalive");
stop_cnt = 5;
}
last_clean_time = millis();
}
else
{
}
now_time = millis();
}
}
* Gamesir游戏杆控制处理
*-从操纵杆接收命令
*-通过收到的命令控制无人机
*
* @param arg有关任务控制的参数
*
unsigned char这个东西我上面解读过
cnt变量多次出现,我去查一下
可以看到计数变量
for(;;)死循环
开始一个判断,如果mac的指令未初始化继续往下运行.
下面又是许多的判断语句,符合各种情况去处理
这个现象叫做粘包,就是指两次结果粘到一起了.看这个地方解决很简单,就是延时了一下.
打印,得到mac的地址没有?
延时一下,就开始继续运行,此时调试的打印开关打开.
手柄任务:mac地址ok嘛~
如果地址有效为正~打印:手柄(蓝牙地址初始化)
这个是初始化的函数,参数是这个手柄的蓝牙Mac.
接着
这是这个函数
函数声明
第一个是参数的声明
第一次判断
任务的结尾,吧标志位更改
如果这个指令初始的值为0和和链接的状态信息与运算为真就开始执行.
我们看到这个函数的命名空间是Protocol里面定义
下文看
SDK控制开始
*获取是否收到布尔类型数据的有效状态
*来自无人机
普通数据有个类
接着这个地方判断成立的条件是数据有效
后面结合SDK解读
这段有的地方不懂,翻转?
降落
我们打开这个看看下面
日本手用左手控制油门
下期我们来对飞行指令解包