第一步，POSCTL模式的进入
1、从遥控器模式开关进入。
首先在PX4IO的主循环中调用了io_publish_raw_rc()，从IO芯片获取遥控器的各通道输入，发布input_rc消息；
然后，在Sensors的主循环中调用rc_poll()函数，获取input_rc消息的内容，按照通道映射和校准信息将遥控器输入值重新计算并写入manual_control_setpoint消息中进行发布，发布内容包括；
manual.y = get_rc_value(rc_channels_s::RC_CHANNELS_FUNCTION_ROLL, -1.0, 1.0);
manual.x = get_rc_value(rc_channels_s::RC_CHANNELS_FUNCTION_PITCH, -1.0, 1.0);
manual.r = get_rc_value(rc_channels_s::RC_CHANNELS_FUNCTION_YAW, -1.0, 1.0);
manual.z = get_rc_value(rc_channels_s::RC_CHANNELS_FUNCTION_THROTTLE, 0.0, 1.0);
manual.flaps = get_rc_value(rc_channels_s::RC_CHANNELS_FUNCTION_FLAPS, -1.0, 1.0);
manual.aux1 = get_rc_value(rc_channels_s::RC_CHANNELS_FUNCTION_AUX_1, -1.0, 1.0);
manual.aux2 = get_rc_value(rc_channels_s::RC_CHANNELS_FUNCTION_AUX_2, -1.0, 1.0);
manual.aux3 = get_rc_value(rc_channels_s::RC_CHANNELS_FUNCTION_AUX_3, -1.0, 1.0);
manual.aux4 = get_rc_value(rc_channels_s::RC_CHANNELS_FUNCTION_AUX_4, -1.0, 1.0);
manual.aux5 = get_rc_value(rc_channels_s::RC_CHANNELS_FUNCTION_AUX_5, -1.0, 1.0);
/* filter controls */
manual.y = math::constrain(_filter_roll.apply(manual.y), -1.f, 1.f);
manual.x = math::constrain(_filter_pitch.apply(manual.x), -1.f, 1.f);
manual.r = math::constrain(_filter_yaw.apply(manual.r), -1.f, 1.f);
manual.z = math::constrain(_filter_throttle.apply(manual.z), 0.f, 1.f);
if (_parameters.rc_map_flightmode > 0) {
/* the number of valid slots equals the index of the max marker minus one */
const int num_slots = manual_control_setpoint_s::MODE_SLOT_MAX;
/* the half width of the range of a slot is the total range
* divided by the number of slots, again divided by two
*/
const float slot_width_half = 2.0f / num_slots / 2.0f;
/* min is -1, max is +1, range is 2. We offset below min and max */
const float slot_min = -1.0f - 0.05f;
const float slot_max = 1.0f + 0.05f;
/* the slot gets mapped by first normalizing into a 0..1 interval using min
* and max. Then the right slot is obtained by multiplying with the number of
* slots. And finally we add half a slot width to ensure that integer rounding
* will take us to the correct final index.
*/
manual.mode_slot = (((((_rc.channels[_parameters.rc_map_flightmode - 1] - slot_min) * num_slots) + slot_width_half) /
(slot_max - slot_min)) + (1.0f / num_slots));
if (manual.mode_slot >= num_slots) {
manual.mode_slot = num_slots - 1;
}
}
最后，在commander的主循环中调用set_main_state_rc()函数，其中利用sp_man.mode_slot的值来进行模式切换，当开关切换到POSCTL对应的位置时，执行main_state_transition()函数将新模式写入到internal_state.main_state中，并记录之前的模式和模式切换时间。
2、从mavlink协议进入。
同样在commander的主循环中，当接收到vehicle_command消息时运行handle_command()函数。如果消息的内容是VEHICLE_CMD_DO_SET_MODE，就是根据参数的内容进行模式设定。当参数中的base_mode为CUMTOM且custom_main_mode是PX4_CUSTOM_MAIN_MODE_POSCTL，同样是调用了main_state_transition()函数将新模式写入。
case vehicle_command_s::VEHICLE_CMD_DO_SET_MODE: {
uint8_t base_mode = (uint8_t)cmd->param1;
uint8_t custom_main_mode = (uint8_t)cmd->param2;
uint8_t custom_sub_mode = (uint8_t)cmd->param3;
.......
if (base_mode & VEHICLE_MODE_FLAG_CUSTOM_MODE_ENABLED) {
/* use autopilot-specific mode */
if (custom_main_mode == PX4_CUSTOM_MAIN_MODE_MANUAL) {
/* MANUAL */
main_ret = main_state_transition(status_local, commander_state_s::MAIN_STATE_MANUAL, main_state_prev, &status_flags, &internal_state);
} else if (custom_main_mode == PX4_CUSTOM_MAIN_MODE_ALTCTL) {
/* ALTCTL */
main_ret = main_state_transition(status_local, commander_state_s::MAIN_STATE_ALTCTL, main_state_prev, &status_flags, &internal_state);
} else if (custom_main_mode == PX4_CUSTOM_MAIN_MODE_POSCTL) {
/* POSCTL */
main_ret = main_state_transition(status_local, commander_state_s::MAIN_STATE_POSCTL, main_state_prev, &status_flags, &internal_state);
......
}
break;
第二步，导航模式和控制模式的选择。
上一步，在commander中除了切换模式之外，还进行了导航模式和控制模式的设定。
在set_nav_state()函数中设定status->nav_state = vehicle_status_s::NAVIGATION_STATE_POSCTL;
在set_control_mode()函数中根据nva_state的值设定了控制相关的flag，包括：
case vehicle_status_s::NAVIGATION_STATE_POSCTL: control_mode.flag_control_manual_enabled = true; control_mode.flag_control_auto_enabled = false; control_mode.flag_control_rates_enabled = true; control_mode.flag_control_attitude_enabled = true; control_mode.flag_control_rattitude_enabled = false; control_mode.flag_control_altitude_enabled = true; control_mode.flag_control_climb_rate_enabled = true; control_mode.flag_control_position_enabled = !status.in_transition_mode; control_mode.flag_control_velocity_enabled = !status.in_transition_mode; control_mode.flag_control_acceleration_enabled = false; control_mode.flag_control_termination_enabled = false; break;
并按照一定周期发布了vehicle_control_mode和vehicle_status两个消息。
然后，在navigator的主循环中，根据在commander中设定的导航模式，选择了对应模式的实例。
case vehicle_status_s::NAVIGATION_STATE_MANUAL: case vehicle_status_s::NAVIGATION_STATE_ACRO: case vehicle_status_s::NAVIGATION_STATE_ALTCTL: case vehicle_status_s::NAVIGATION_STATE_POSCTL: case vehicle_status_s::NAVIGATION_STATE_TERMINATION: case vehicle_status_s::NAVIGATION_STATE_OFFBOARD: case vehicle_status_s::NAVIGATION_STATE_STAB: default: _pos_sp_triplet_published_invalid_once = false; _navigation_mode = nullptr; _can_loiter_at_sp = false; break;
由于这些都是手控模式，因此_navigation_mode = nullptr。也就是不会调用任何导航实例。而是调用了如下代码
/* if nothing is running, set position setpoint triplet invalid once */
if (_navigation_mode == nullptr && !_pos_sp_triplet_published_invalid_once) {
_pos_sp_triplet_published_invalid_once = true;
_pos_sp_triplet.previous.valid = false;
_pos_sp_triplet.current.valid = false;
_pos_sp_triplet.next.valid = false;
_pos_sp_triplet_updated = true;
}
if (_pos_sp_triplet_updated) {
_pos_sp_triplet.timestamp = hrt_absolute_time();
publish_position_setpoint_triplet();
_pos_sp_triplet_updated = false;
}
实际上由于_pos_sp_triplet.current.valid = false; 函数 publish_position_setpoint_triplet(); 会立即返回，并不会发布提供位置控制期望的position_setpoint_triplet消息出去。
第三步，位置控制的实现。
在mc_pos_control的主循环中首先调用do_control()函数；由于在commander中设定了control_mode.flag_control_manual_enabled = true，进而进入control_manual()函数。对于非手控的模式则进入control_non_manual()函数。
在control_manual()函数中从manual_control_setpoint消息中获取纵横垂向的杆量，并根据参数进行限幅之后给中间变量。然后转换到NED坐标系中。
math::Vector<2> req_vel_sp_xy;
req_vel_sp_xy.zero();
float req_vel_sp_z = 0.0f;
if (_control_mode.flag_control_altitude_enabled) {
/* set vertical velocity setpoint with throttle stick */
req_vel_sp_z = -scale_control(_manual.z - 0.5f, 0.5f, _params.alt_ctl_dz, _params.alt_ctl_dy); // D
/* reset alt setpoint to current altitude if needed */
reset_alt_sp();
}
if (_control_mode.flag_control_position_enabled) {
/* set horizontal velocity setpoint with roll/pitch stick */
req_vel_sp_xy(0) = math::expo_deadzone(_manual.x, _params.xy_vel_man_expo, _params.hold_xy_dz);
req_vel_sp_xy(1) = math::expo_deadzone(_manual.y, _params.xy_vel_man_expo, _params.hold_xy_dz);
/* reset position setpoint to current position if needed */
reset_pos_sp();
}
/* scale requested velocity setpoint to cruisespeed and rotate around yaw */
math::Vector<3> vel_cruise(_params.vel_cruise(0),
_params.vel_cruise(1),
(req_vel_sp_z > 0.0f) ? _params.vel_max_down : _params.vel_max_up);
math::Vector<3> req_vel_sp_scaled(req_vel_sp_xy(0), req_vel_sp_xy(1), req_vel_sp_z);
/* scale velocity setpoint to cruise speed (m/s) and rotate around yaw to NED frame */
math::Matrix<3, 3> R_input_fame;
if (_control_mode.flag_control_fixed_hdg_enabled) {
R_input_fame.from_euler(0.0f, 0.0f, _yaw_takeoff);
} else {
R_input_fame.from_euler(0.0f, 0.0f, _att_sp.yaw_body);
}
req_vel_sp_scaled = R_input_fame * req_vel_sp_scaled.emult(
vel_cruise); // in NED and scaled to actual velocity;
根据control_mode设定的flag_control_altitude_enabled和flag_control_position_enabled确定是否在赶回中后保持高度和平面位置（也就是ALTCTL模式和POSCTL模式的作用了）。最后判断是否已经着陆，如果没有着陆就执行control_position()函数，从位置偏差计算速度期望值_vel_sp，并通过消息vehicle_global_velocity_setpoint发布出去，当然还有很多边边角角的复杂计算，需要自己看代码来理解。
回到主循环中，结束了do_control()函数之后，将位置期望和速度期望赋值给_local_pos_sp，并发布消息vehicle_local_position_setpoint。接下来调用generate_attitude_setpoint()函数将姿态角期望写入_att_sp结构体中，并发布vehicle_attitude_setpoint消息。
第四步，姿态控制的实现。
目前还没看完这部分代码，等看完了再更新。
第五步，控制信号的输出。
在PX4IO的主循环中，执行io_set_control_groups()将对应控制组的控制量输出给IO芯片来产生控制电调或者舵机的PWM信号。