TT无人机扩展模块库分析(操控函数)1

/**
 * 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();
    }
}