hpm6750/controller_yy_app/controlware/control_inc/pilot_navigation.h

#ifndef __PILOT_NAVIGATION_H
#define __PILOT_NAVIGATION_H

#include <stdbool.h>
#include <stdint.h>

#define MODE_COORDINATE 1 // 高速过弯
#define MODE_LOITERTURN 2 // 悬停转弯
#define MODE_LOCKHEAD 3   // 机头锁定
#define MODE_THROWING 4   // 抛物
#define MODE_LANDING 5    // 降落
#define MODE_CIRCLE 6     // 环绕
#define MODE_AUTOTURN 7   // 自动转弯
#define MODE_EXITWP 10

#define TASK_NO 1             // 无任务
#define TASK_PHOTO_DISTANCE 2 // 启动定距离拍照
#define TASK_PHOTO_TIME 3     // 启动定时间拍照

#define ARRIVE_POINT_RADIUS 250.0f

#define vector2(TYPE) \
    typedef struct    \
    {                 \
        TYPE x;       \
        TYPE y;       \
    } vector2_##TYPE

vector2(float);
vector2(double);
vector2(int);
vector2(short);

#define vector3(TYPE) \
    typedef struct    \
    {                 \
        TYPE x;       \
        TYPE y;       \
        TYPE z;       \
    } vector3_##TYPE

vector3(float);
vector3(double);
vector3(int);
vector3(short);

struct rec_pos
{
    int lng;
    int lat;
    int alt;
    int gps_alt;
    int16_t yaw;
};
extern struct rec_pos home_position;
extern struct rec_pos takeoff_position;
extern struct rec_pos poshold_position;
extern struct rec_pos wpphoto_position;
extern struct rec_pos circlecenter_position;
extern struct rec_pos target_yaw_lock_position;

extern uint16_t wp_cycle_times;
extern uint16_t wp_have_cycle_times;

void record_position(int *longitude, int *latitude, int lng, int lat);

float earth_longitude_scale(int lat);
int point_to_point_distance(int lon1, int lat1, int lon2, int lat2);

float cal_tar_vel_z(int t_alt, int c_alt, int acc_up, int acc_down);


//航点数据结构体
struct waypoint_data
{
    unsigned short wp_num;        // 航点序号
    int wp_lng;                   // 航点经度 *10^7
    int wp_lat;                   // 航点纬度 *10^7
    int wp_alt;                   // 航点高度 cm
    short wp_vel;                 // 巡航速度 cm/s
    unsigned char wp_mode;        // 航点模式
    unsigned short wp_mode_param; // 模式参数。cm、s、
    unsigned char wp_task;        // 航线任务
    unsigned short wp_task_param; // 任务参数。cm、s、
    unsigned short wp_totalnum;   // 航点总数
    short wp_param3;              // 航点机头指向
    uint8_t wp_alt_type;          // 航点高度类型
    int16_t wp_param4;            // 环绕半径 dm
};


//航点总数
extern unsigned short waypoint_totalnum;

//目标航点序号
extern unsigned short tar_wp_no;

//前一个点到目标点的距离 cm
extern int wp_prevtotar_distance;
//前一个点到目标点的方位角 deg
extern float wp_prevtotar_bearing;

//前一个点到当前点的距离 cm
extern int wp_prevtocur_distance;
//前一个点到当前点的方位角 deg
extern float wp_prevtocur_bearing;

extern int wp_curtotar_distance;
extern float wp_curtotar_bearing;
//开始点到当前点的垂线距离
extern int wp_prevtocur_verdistance;
//当前点到目标点的垂线距离
extern int wp_curtotar_verdistance;

extern bool fly_point_flag, update_point_flag, execute_command_flag;

extern unsigned int arrive_point_time;


extern float wp_curtotar_bearing_last;

extern bool reset_wp_start_time_flag;
int cal_tar_vel_xy_unac(int h_dist, int r_dist, int min_vel, int max_vel);

#endif