ardupilot/libraries/AC_ZR_APP/AC_ZR_Serial_API.h

/*
   This program is free software: you can redistribute it and/or modify
   it under the terms of the GNU General Public License as published by
   the Free Software Foundation, either version 3 of the License, or
   (at your option) any later version.

   This program is distributed in the hope that it will be useful,
   but WITHOUT ANY WARRANTY; without even the implied warranty of
   MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
   GNU General Public License for more details.

   You should have received a copy of the GNU General Public License
   along with this program.  If not, see <http://www.gnu.org/licenses/>.
 */
#pragma once

#include <AP_HAL/AP_HAL.h>
#include <AP_Param/AP_Param.h>
#include <AP_SerialManager/AP_SerialManager.h>
#include <GCS_MAVLink/GCS.h>
#include <GCS_MAVLink/GCS_MAVLink.h>

#define MSG_HEAD 0xFE
#define FLIGHT_DATA_LEN 40
#define CONTROL_DATA_LEN 21
#define VEHICLE_STATUS_LEN 13
#define CONTROL_ASK_LEN 7

class AC_ZR_Serial_API
{
public:
    AC_ZR_Serial_API();
        /* Do not allow copies */
    AC_ZR_Serial_API(const AC_ZR_Serial_API &other) = delete;
    AC_ZR_Serial_API &operator=(const AC_ZR_Serial_API&) = delete;

    static AC_ZR_Serial_API *get_singleton() {
        return _singleton;
    }
    
    // 消息类型
    enum class ZR_Msg_Type : uint8_t {
        MSG_ASK = 0,        // 应答消息
        MSG_FLIGHT_DATA ,   // 飞行数据
        MSG_VEHICLE_STATUS,  //  飞机状态
        MSG_CONTROL_TKOFF , // 飞行控制：起飞
        MSG_CONTROL_POS ,   // 飞行控制：位置控制模式
        MSG_CONTROL_VEL ,   // 飞行控制：速度控制模式，大地坐标系
        MSG_CONTROL_VEL_P,   // 飞行控制：速度控制模式
        MSG_CONTROL_VEL_LOCK_YAW ,   // 悬停，锁定航向
        MSG_CONTROL_MODE,   // 飞行控制：速度控制模式
        
    };

    enum class  ZR_Msg_ASK : uint8_t{
        MSG_ASK_OK = 0,	    // 成功
        MSG_ASK_ERRCRC ,    // crc校验错误
        MSG_ASK_ERRTYPE ,   // 消息类型错误
        MSG_ASK_ERRMODE ,   // 飞行模式错误
        MSG_ASK_OUTRANGE    // 控制数据超出限制
    };


    /// Startup initialisation.
    void init(const AP_SerialManager& serial_manager);
    void update(void);
    void write_data_to_port(AP_HAL::UARTDriver *_port,const uint8_t *data, uint8_t len);
    void read_data_from_port(AP_HAL::UARTDriver *_port,uint8_t *data, uint8_t *len);
    
    void get_api_data(uint8_t *data, uint8_t len); 
    void get_vehicle_NEU_pos(Vector3f vec_neu);
    void get_vehicle_euler_angles(Vector3f euler);

    void get_vehicle_flight_data(Vector3l pos,Vector3l vel,Vector3f euler);

    void get_vehicle_status(uint8_t mode,uint8_t in_air,uint32_t home_distance,uint16_t volt_mv,uint8_t bat_remaining);

    bool get_control_data(uint8_t mode,ZR_Msg_Type &type,Vector3l &data,float &yaw_deg);
    // 接收控制指令的应答
    void set_control_ask(uint8_t msg_id,ZR_Msg_Type receive_type,ZR_Msg_ASK ask);

    bool data_trans_init;

    uint8_t get_param_debug(){ return parm_msg_debug; };
    void print_data(const char *fmt, ...)  const;
    void print_msg(const char *msg)  const;
    
    static const struct AP_Param::GroupInfo var_info[];
protected:
    AP_Int8         parm_msg_debug;
    AP_Int8         parm_data_freq;
    AP_Int8         parm_status_freq;
private:

    bool check_uart(AP_HAL::UARTDriver *_port);
    void process_packet(const uint8_t *b);

    static AC_ZR_Serial_API *_singleton;
    AP_HAL::UARTDriver *_zr_api_port;
    uint32_t serial_last_data_time;
    uint32_t control_data_last_time;

    uint8_t serial_data_from_device[CONTROL_DATA_LEN];
    uint8_t mav_data_from_host[FLIGHT_DATA_LEN];

    uint8_t serial_data_len,mav_data_len,mav_len;
    bool get_mav_data;

    struct PACKED flight_data_parser_s
    {
        uint8_t msg_head;
        ZR_Msg_Type msg_type;
        uint8_t length;
        // 3
        int32_t lat;
        int32_t lng;
        int32_t alt;
        // 15
        int32_t vx;
        int32_t vy;
        int32_t vz;
        // 27
        float roll;
        float pitch;
        float yaw;
        // 39
        uint8_t crc;
    };
    
    union PACKED flight_data_parser_u
    {
        flight_data_parser_s msg;
        uint8_t data[FLIGHT_DATA_LEN];
    }flight_data_parser;


    struct PACKED zr_flight_control_data_s
    {   // 0
        uint8_t head;
        ZR_Msg_Type type;
        uint8_t length;
        uint8_t msg_id;
        // 4
        int32_t x;
        int32_t y;
        int32_t z;
        // 16
        float yaw;
        // 10
        uint8_t crc;
    };
    union PACKED flight_control_parser_u
    {
        zr_flight_control_data_s msg;
        uint8_t data[CONTROL_DATA_LEN];
    }flight_control_parser;
    
    struct PACKED flight_control_ask_s
    {
        uint8_t msg_head;
        ZR_Msg_Type msg_type;
        uint8_t length;
        uint8_t msg_id;
        // 4
        ZR_Msg_Type receive_type;
        ZR_Msg_ASK msg_ask;
        uint8_t crc;
    };
    union PACKED flight_control_ask_parser_u
    {
        flight_control_ask_s msg;
        uint8_t data[CONTROL_ASK_LEN];
    }flight_control_ask;

    struct PACKED vehicle_status_parser_s
    {
        uint8_t msg_head;
        ZR_Msg_Type msg_type;
        uint8_t length;
        // 3
        uint8_t fly_mode;
        uint8_t fly_status;
        // 5
        uint32_t home_distance;
        // 9
        uint16_t volt_mv;
        uint8_t bat_remaining;

        // 12
        uint8_t crc;
    };
    union PACKED vehicle_status_parser_u
    {
        vehicle_status_parser_s msg;
        uint8_t data[VEHICLE_STATUS_LEN];
    }vehicle_status;

    struct control_t {

        uint32_t time_stamp;
        ZR_Msg_Type type;
        Vector3l data;
        float yaw_deg;
    } control_data;

    HAL_Semaphore sem;
    uint8_t *pktbuf;
    uint16_t pktoffset;
    uint16_t bufsize;

};