px4 源码中的疑问和记录_px4 moduleparams(nullptr)代码解析

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)   //此函数的作用是？

 

5、在mc_att_control_main.cpp

关于发布多旋翼控制力矩的消息ID有如下定义

对应的输出组如下

对应的.msg消息文件如下

uint8 NUM_ACTUATOR_CONTROLS = 8
uint8 NUM_ACTUATOR_CONTROL_GROUPS = 4
uint8 INDEX_ROLL = 0
uint8 INDEX_PITCH = 1
uint8 INDEX_YAW = 2
uint8 INDEX_THROTTLE = 3
uint8 INDEX_FLAPS = 4
uint8 INDEX_SPOILERS = 5
uint8 INDEX_AIRBRAKES = 6
uint8 INDEX_LANDING_GEAR = 7
uint8 GROUP_INDEX_ATTITUDE = 0
uint8 GROUP_INDEX_ATTITUDE_ALTERNATE = 1
uint64 timestamp_sample        # the timestamp the data this control response is based on was sampled
float32[8] control

# TOPICS actuator_controls actuator_controls_0 actuator_controls_1 actuator_controls_2 actuator_controls_3
# TOPICS actuator_controls_virtual_fw actuator_controls_virtual_mc

而编译后生成的头文件如下src/modules/topics

 

可以看出有actuator_controls_0、actuator_controls_1、actuator_controls_2、actuator_controls_3 、actuator_controls_virtual_fw(VTOL)、actuator_controls_virtual_mc(VTOL) uORB消息，而多旋翼则将消息发布到actuator_controls_0这个消息ID

Control Groups

Control groups are grouped actuator control positions. These functional groups are centered around core flight control (group 0), auxiliary flight control (group 1), payload (group 2) and manual passthrough (group 3, e.g. for gimbal control).

Control groups 1, 2, and 3 are not sent to the actuators unless there is a change in control group 0. If, for example, aux0 was being used to control the pitch setting of a gimbal motor, the pitch would only change when the flight control motors associated with control group 0 were armed.

Control Group #0 (Flight Control)

• 0: roll (-1..1)

• 1: pitch (-1..1)

• 2: yaw (-1..1)

• 3: throttle (0..1 normal range, -1..1 for variable pitch / thrust reversers)

• 4: flaps (-1..1)

• 5: spoilers (-1..1)

• 6: airbrakes (-1..1)

• 7: landing gear (-1..1)

Control Group #1 (Flight Control VTOL/Alternate)

• 0: roll ALT (-1..1)

• 1: pitch ALT (-1..1)

• 2: yaw ALT (-1..1)

• 3: throttle ALT (0..1 normal range, -1..1 for variable pitch / thrust reversers)

• 4: reserved / aux0

• 5: reserved / aux1

• 6: reserved / aux2

• 7: reserved / aux3

Control Group #2 (Payload)

• 0: gimbal roll

• 1: gimbal pitch

• 2: gimbal yaw

• 3: gimbal shutter

• 4: reserved

• 5: reserved

• 6: reserved

• 7: reserved (parachute, -1..1)

Control Group #3 (Manual Passthrough)

• 0: RC roll

• 1: RC pitch

• 2: RC yaw

• 3: RC throttle

• 4: RC mode switch

• 5: RC aux1

• 6: RC aux2

• 7: RC aux3

Virtual Control Groups

• These groups are NOT mixer inputs, but serve as meta-channels to feed fixed wing and multicopter controller outputs into the VTOL governor module.

• Control Group #4 (Flight Control MC VIRTUAL)

  0: roll ALT (-1..1)
  1: pitch ALT (-1..1)
  2: yaw ALT (-1..1)
  3: throttle ALT (0..1 normal range, -1..1 for variable pitch / thrust reversers)
  4: reserved / aux0
  5: reserved / aux1
  6: reserved / aux2
  7: reserved / aux3

• Control Group #5 (Flight Control FW VIRTUAL)

  0: roll ALT (-1..1)
  1: pitch ALT (-1..1)
  2: yaw ALT (-1..1)
  3: throttle ALT (0..1 normal range, -1..1 for variable pitch / thrust reversers)
  4: reserved / aux0
  5: reserved / aux1
  6: reserved / aux2
  7: reserved / aux3

•