px4源码----位置估算（position_estimator_inav_params.h）_px4高度估算

#pragma once

#include <parameters/param.h>

struct position_estimator_inav_params {
	float w_z_baro;                     //权重  z轴  气压计位置   0.5
	float w_z_gps_p;					//权重  z轴  GPS位置     0.005f
	float w_z_gps_v;					//权重  z轴  GPS速度     0.0f
	float w_z_vision_p;					//权重  z轴  视觉位置     5.0f
	float w_z_lidar;                    //权重  z轴  激光雷达位置  3.0f

	float w_xy_gps_p;					//权重  xy轴  GPS位置  1.0f
	float w_xy_gps_v;					//权重  xy轴  GPS速度  2.0f
	float w_xy_vision_p;				//权重  xy轴  视觉位置  7.0f
	float w_xy_vision_v;				//权重  xy轴  视觉速度  0.0f

	float w_mocap_p;					//权重        MOCAP系统  10.0f
	float w_xy_flow;					//权重  xy轴  光流位置      0.8f
	float w_xy_res_v;					//权重  xy轴  用于重置速度   0.5f
	float w_gps_flow;					//权重  xy轴  当光流可用时，将GPS权重乘以该权重  0.1f
	float w_acc_bias;					//权重        加速度计偏差估计  0.05f

	float flow_k;						//光流比例因子    1.35f   0~10
	float flow_q_min;					//最小可接受光流质量 0.3f  0~1
	float lidar_err;					//新表面声纳最大误差  0.2f
	float land_t;						//地面探测器时间  3.0f   如果在这段时间内没有在低油门下发生高度变化，车辆假定着陆
	float land_disp;					//地面探测器高度扩散阈值 0.7f
	float land_thr;						//地面探测器油门阈值 0.2f
	int32_t no_vision;					//禁用视觉输入  0
	float delay_gps;					//GPS延时补偿 0.2f

	float flow_module_offset_x;			//X方向的光流模块偏移（旋转中心）  0.0f
	float flow_module_offset_y;			//Y方向的光流模块偏移（旋转中心）  0.0f
	int32_t disable_mocap;				//如果使用GPS，则设置为0
	int32_t enable_lidar_alt_est;		//用于高度估计的激光雷达 0
	float lidar_calibration_offset;		//激光雷达校准偏差 0.0f
	int32_t att_ext_hdg_m;				//外部航向使用模式（从运动捕捉/视觉）0
};

struct position_estimator_inav_param_handles {
	param_t w_z_baro;      //权重  z轴  气压计位置   0.5
	param_t w_z_gps_p;	   //权重  z轴  GPS位置     0.005f
	param_t w_z_gps_v;     //权重  z轴  GPS速度     0.0f
	param_t w_z_vision_p;  //权重  z轴  视觉位置     5.0f
	param_t w_z_lidar;     //权重  z轴  激光雷达位置  3.0f

	param_t w_xy_gps_p;    //权重  xy轴  GPS位置  1.0f
	param_t w_xy_gps_v;    //权重  xy轴  GPS速度  2.0f
	param_t w_xy_vision_p; //权重  xy轴  视觉位置  7.0f
	param_t w_xy_vision_v; //权重  xy轴  视觉速度  0.0f

	param_t w_mocap_p;     //权重        MOCAP系统  10.0f
	param_t w_xy_flow;     //权重  xy轴  光流位置      0.8f
	param_t w_xy_res_v;    //权重  xy轴  用于重置速度   0.5f
	param_t w_gps_flow;    //权重  xy轴  当光流可用时，将GPS权重乘以该权重  0.1f
	param_t w_acc_bias;    //权重        加速度计偏差估计  0.05f

	param_t flow_k;        //光流比例因子    1.35f   0~10
	param_t flow_q_min;    //最小可接受光流质量 0.3f  0~1
	param_t lidar_err;     //新表面声纳最大误差  0.2f
	param_t land_t;        //地面探测器时间  3.0f   如果在这段时间内没有在低油门下发生高度变化，车辆假定着陆
	param_t land_disp;     //地面探测器高度扩散阈值 0.7f
	param_t land_thr;      //地面探测器油门阈值 0.2f
	param_t no_vision;     //禁用视觉输入  0
	param_t delay_gps;     //GPS延时补偿 0.2f

	param_t flow_module_offset_x;       //X方向的光流模块偏移（旋转中心）  0.0f
	param_t flow_module_offset_y;       //Y方向的光流模块偏移（旋转中心）  0.0f
	param_t disable_mocap;              //如果使用GPS，则设置为0
	param_t enable_lidar_alt_est;       //用于高度估计的激光雷达 0
	param_t lidar_calibration_offset;   //激光雷达校准偏差 0.0f
	param_t att_ext_hdg_m;              //外部航向使用模式（从运动捕捉/视觉）0
};

#define CBRK_NO_VISION_KEY	328754

/**
 * Initialize all parameter handles and values
 *
 */
int inav_parameters_init(struct position_estimator_inav_param_handles *h);

/**
 * Update all parameters
 *
 */
int inav_parameters_update(const struct position_estimator_inav_param_handles *h,
			   struct position_estimator_inav_params *p);

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90