px4 源码中的疑问和记录
1、在类BlockLocalPositionEstimator定义了很多私有成员如下
// general parameters
BlockParamInt _pub_agl_z;
BlockParamFloat _vxy_pub_thresh;
BlockParamFloat _z_pub_thresh;
// sonar parameters
BlockParamFloat _sonar_z_stddev;
BlockParamFloat _sonar_z_offset;
// lidar parameters
BlockParamFloat _lidar_z_stddev;
BlockParamFloat _lidar_z_offset;
// accel parameters
BlockParamFloat _accel_xy_stddev;
BlockParamFloat _accel_z_stddev;
// baro parameters
BlockParamFloat _baro_stddev;
// gps parameters
BlockParamInt _gps_on;
BlockParamFloat _gps_delay;
BlockParamFloat _gps_xy_stddev;
BlockParamFloat _gps_z_stddev;
BlockParamFloat _gps_vxy_stddev;
BlockParamFloat _gps_vz_stddev;
BlockParamFloat _gps_eph_max;
BlockParamFloat _gps_epv_max;
// vision parameters
BlockParamFloat _vision_xy_stddev;
BlockParamFloat _vision_z_stddev;
BlockParamFloat _vision_delay;
BlockParamInt _vision_on;
// mocap parameters
BlockParamFloat _mocap_p_stddev;
// flow parameters
BlockParamInt _flow_gyro_comp;
BlockParamFloat _flow_z_offset;
BlockParamFloat _flow_scale;
//BlockParamFloat _flow_board_x_offs;
//BlockParamFloat _flow_board_y_offs;
BlockParamInt _flow_min_q;
// land parameters
BlockParamFloat _land_z_stddev;
// process noise
BlockParamFloat _pn_p_noise_density;
BlockParamFloat _pn_v_noise_density;
BlockParamFloat _pn_b_noise_density;
BlockParamFloat _pn_t_noise_density;
BlockParamFloat _t_max_grade;
// init origin
BlockParamFloat _init_origin_lat;
BlockParamFloat _init_origin_lon;
// flow gyro filter
BlockHighPass _flow_gyro_x_high_pass;
BlockHighPass _flow_gyro_y_high_pass;
// stats
BlockStats<float, n_y_baro> _baroStats;
BlockStats<float, n_y_sonar> _sonarStats;
BlockStats<float, n_y_lidar> _lidarStats;
BlockStats<float, 1> _flowQStats;
BlockStats<float, n_y_vision> _visionStats;
BlockStats<float, n_y_mocap> _mocapStats;
BlockStats<double, n_y_gps> _gpsStats;
uint16_t _landCount;
// low pass
BlockLowPassVector<float, n_x> _xLowPass;
BlockLowPass _aglLowPass;
在其构造函数中有以下实现
// block parameters
_pub_agl_z(this, "PUB_AGL_Z"),
_vxy_pub_thresh(this, "VXY_PUB"),
_z_pub_thresh(this, "Z_PUB"),
_sonar_z_stddev(this, "SNR_Z"),
_sonar_z_offset(this, "SNR_OFF_Z"),
_lidar_z_stddev(this, "LDR_Z"),
_lidar_z_offset(this, "LDR_OFF_Z"),
_accel_xy_stddev(this, "ACC_XY"),
_accel_z_stddev(this, "ACC_Z"),
_baro_stddev(this, "BAR_Z"),
_gps_on(this, "GPS_ON"),
_gps_delay(this, "GPS_DELAY"),
_gps_xy_stddev(this, "GPS_XY"),
_gps_z_stddev(this, "GPS_Z"),
_gps_vxy_stddev(this, "GPS_VXY"),
_gps_vz_stddev(this, "GPS_VZ"),
_gps_eph_max(this, "EPH_MAX"),
_gps_epv_max(this, "EPV_MAX"),
_vision_xy_stddev(this, "VIS_XY"),
_vision_z_stddev(this, "VIS_Z"),
_vision_delay(this, "VIS_DELAY"),
_vision_on(this, "VIS_ON"),
_mocap_p_stddev(this, "VIC_P"),
_flow_gyro_comp(this, "FLW_GYRO_CMP"),
_flow_z_offset(this, "FLW_OFF_Z"),
_flow_scale(this, "FLW_SCALE"),
//_flow_board_x_offs(NULL, "SENS_FLW_XOFF"),
//_flow_board_y_offs(NULL, "SENS_FLW_YOFF"),
_flow_min_q(this, "FLW_QMIN"),
_land_z_stddev(this, "LAND_Z"),
_pn_p_noise_density(this, "PN_P"),
_pn_v_noise_density(this, "PN_V"),
_pn_b_noise_density(this, "PN_B"),
_pn_t_noise_density(this, "PN_T"),
_t_max_grade(this, "T_MAX_GRADE"),
// init origin
_init_origin_lat(this, "LAT"),
_init_origin_lon(this, "LON"),
// flow gyro
_flow_gyro_x_high_pass(this, "FGYRO_HP"),
_flow_gyro_y_high_pass(this, "FGYRO_HP"),
// stats
_baroStats(this, ""),
_sonarStats(this, ""),
_lidarStats(this, ""),
_flowQStats(this, ""),
_visionStats(this, ""),
_mocapStats(this, ""),
_gpsStats(this, ""),
进入到BlockParam.cpp/hpp的内部也看不懂是怎么实现的
2、mavlink_receiver.cpp
MavlinkReceiver::handle_message_set_mode(mavlink_message_t *msg)
{
mavlink_set_mode_t new_mode;
mavlink_msg_set_mode_decode(msg, &new_mode);
struct vehicle_command_s vcmd;
memset(&vcmd, 0, sizeof(vcmd));
union px4_custom_mode custom_mode;
custom_mode.data = new_mode.custom_mode;
/* copy the content of mavlink_command_long_t cmd_mavlink into command_t cmd */
vcmd.param1 = new_mode.base_mode;
vcmd.param2 = custom_mode.main_mode;
vcmd.param3 = custom_mode.sub_mode;
vcmd.param4 = 0;
vcmd.param5 = 0;
vcmd.param6 = 0;
vcmd.param7 = 0;
vcmd.command = vehicle_command_s::VEHICLE_CMD_DO_SET_MODE;
vcmd.target_system = new_mode.target_system;
vcmd.target_component = MAV_COMP_ID_ALL;
vcmd.source_system = msg->sysid;
vcmd.source_component = msg->compid;
vcmd.confirmation = 1;
if (_cmd_pub == nullptr) {
_cmd_pub = orb_advertise_queue(ORB_ID(vehicle_command), &vcmd, vehicle_command_s::ORB_QUEUE_LENGTH);
} else {
orb_publish(ORB_ID(vehicle_command), _cmd_pub, &vcmd);
}
}
3、mavlink_main.cpp
（1）
void
Mavlink::handle_message(const mavlink_message_t *msg)
{
if (!accepting_commands()) {
return;
}
/* handle packet with mission manager */
_mission_manager->handle_message(msg);//mission模式的命令和消息解析？
/* handle packet with parameter component */
_parameters_manager->handle_message(msg);//gcs通过uavcan更新飞控端的参数?????
/* handle packet with ftp component */
_mavlink_ftp->handle_message(msg);
/* handle packet with log component */
_mavlink_log_handler->handle_message(msg);
if (get_forwarding_on()) {
/* forward any messages to other mavlink instances */
Mavlink::forward_message(msg, this);
}
}
（2）
4、mavlink_message.cpp
void get_mavlink_mode_state(struct vehicle_status_s *status, uint8_t *mavlink_state,
uint8_t *mavlink_base_mode, uint32_t *mavlink_custom_mode) //此函数的作用是？