/* =============================================================================
 * c_comm_task.c - 板间通讯周期发送(2ms)
 * 云台板:  c_comm_master_send() 转发摇杆/控制状态(键鼠在线时用合成量)
 * 底盘板:  c_comm_slave_send() 回传发射机构上电状态
 * 接收方向在中断内由 c_comm 模块的回调完成,本任务只负责发送节拍
 * ========================================================================== */
#include "app_tasks.h"
#include "cmsis_os.h"
#include "FreeRTOS.h"
#include "task.h"
#include "c_comm.h"
#include "robot_typedef.h"
#include "robot_param.h"
#if defined(ROBOT_TYPE_GIMBAL)
#include "robot_cmd_task.h"
#include "gimbal.h"
#include "attitude.h"
#elif defined(ROBOT_TYPE_CHASSIS)
#include "chassis.h"
#endif

/* c_comm_task(2ms):板间通讯周期发送
 * 云台板:向底盘转发摇杆与控制状态(键鼠在线时用合成量替代摇杆)
 * 底盘板:向云台回传发射机构上电状态 */

void c_comm_task(void const * argument)
{
    TickType_t last_wake = xTaskGetTickCount();

    while (!app_ready)
        osDelay(2);

    for (;;)
    {
#if defined(ROBOT_TYPE_GIMBAL)
        uint8_t kb_online = (keyboard_data.last_tick != 0 &&
                             HAL_GetTick() - keyboard_data.last_tick < DEVICE_OFFLINE_TIME_MS * 10);
        uint8_t kb[12];
        kb[0] = (uint8_t)(keyboard_data.mouse_rl & 0xFF);
        kb[1] = (uint8_t)((uint16_t)keyboard_data.mouse_rl >> 8);
        kb[2] = (uint8_t)(keyboard_data.mouse_du & 0xFF);
        kb[3] = (uint8_t)((uint16_t)keyboard_data.mouse_du >> 8);
        kb[4] = (uint8_t)(keyboard_data.mouse_wheel & 0xFF);
        kb[5] = (uint8_t)((uint16_t)keyboard_data.mouse_wheel >> 8);
        kb[6] = keyboard_data.mouse_key_l;
        kb[7] = keyboard_data.mouse_key_r;
        kb[8] = (uint8_t)(keyboard_data.key_w | (keyboard_data.key_s << 1) |
                          (keyboard_data.key_a << 2) | (keyboard_data.key_d << 3) |
                          (keyboard_data.key_shift << 4) | (keyboard_data.key_ctrl << 5) |
                          (keyboard_data.key_q << 6) | (keyboard_data.key_e << 7));
        kb[9] = (uint8_t)(keyboard_data.key_alpha1 | (keyboard_data.key_alpha2 << 1));
        kb[10] = 0;
        kb[11] = 0;

        c_comm_master_send(kb_online, kb, gimbal_motor_offline(),
                           cmd_robot.spin_flag);  // 故障位+小陀螺使能位

        /* 0x1A9 姿态帧:双 INS 跟随方案的数据源 */
        c_comm_master_send_attitude(att_deg_yaw, att_ready);
#elif defined(ROBOT_TYPE_CHASSIS)
        c_comm_slave_send(chassis_motor_offline());  // 携带底盘电机故障位
#endif

        monitor_heartbeat(HB_CCOMM);
        vTaskDelayUntil(&last_wake, pdMS_TO_TICKS(2));
    }
}