PX4模块设计之三十六：MulticopterPositionControl模块

1. MulticopterPositionControl模块简介

### Description
The controller has two loops: a P loop for position error and a PID loop for velocity error.
Output of the velocity controller is thrust vector that is split to thrust direction
(i.e. rotation matrix for multicopter orientation) and thrust scalar (i.e. multicopter thrust itself).

The controller doesn't use Euler angles for its work, they are generated only for more human-friendly control and
logging.

mc_pos_control <command> [arguments...]
 Commands:
   start
     [vtol]      VTOL mode

   stop

   status        print status info

注：print_usage函数是具体对应实现。

class MulticopterPositionControl : public ModuleBase<MulticopterPositionControl>, public control::SuperBlock,
	public ModuleParams, public px4::ScheduledWorkItem

注：MulticopterPositionControl模块采用了ModuleBase和WorkQueue设计。

2. 模块入口函数

2.1 主入口mc_pos_control_main

同样继承了ModuleBase，由ModuleBase的main来完成模块入口。

mc_pos_control_main
 └──> return MulticopterPositionControl::main(argc, argv)

2.2 自定义子命令custom_command

模块仅支持start/stop/status命令，不支持其他自定义命令。

MulticopterPositionControl::custom_command
 └──> return print_usage("unknown command");

3. MulticopterPositionControl模块重要函数

3.1 task_spawn

这里主要初始化了MulticopterPositionControl对象，后续通过WorkQueue来完成进行轮询。

MulticopterPositionControl::task_spawn
 ├──> vtol = false
 ├──> <argc > 1><strcmp(argv[1], "vtol") == 0>
 │   └──> vtol = true
 ├──> MulticopterPositionControl *instance = new MulticopterPositionControl(vtol)
 ├──> <instance>
 │   ├──> _object.store(instance)
 │   ├──> _task_id = task_id_is_work_queue
 │   └──> <instance->init()>
 │       └──> return PX4_OK
 ├──> <else>
 │   └──> PX4_ERR("alloc failed")
 ├──> delete instance
 ├──> _object.store(nullptr)
 └──> _task_id = -1

3.2 instantiate

注：鉴于该模块不采用任务(线程)，所以ModuleBase::run_trampoline无需执行，所以可以不实现。

3.3 init

在task_spawn中使用，对_local_pos_sub成员变量进行事件回调注册(当有vehicle_local_position消息时，会调用SubscriptionCallbackWorkItem::ScheduleNow，再触发RCUpdate::Run过程)，同时在MulticopterPositionControl::init过程，触发第一次ScheduleNow。

MulticopterPositionControl::init
 ├──> <!_local_pos_sub.registerCallback()>
 │   ├──> PX4_ERR("callback registration failed")
 │   └──> return false
 ├──> _time_stamp_last_loop = hrt_absolute_time()
 ├──> ScheduleNow()
 └──> return true

3.4 Run

在Run过程中调用ScheduleDelayed(100_ms)规划下一次定时轮询。

MulticopterPositionControl::Run
 ├──> [优雅退出处理]
 ├──> ScheduleDelayed(100_ms)  // reschedule backup
 ├──> parameters_update(false)
 ├──> <!_local_pos_sub.update(&vehicle_local_position)>
 │   └──> return
 ├──> const float dt = math::constrain(((vehicle_local_position.timestamp_sample - _time_stamp_last_loop) * 1e-6f), 0.002f, 0.04f)
 ├──> _time_stamp_last_loop = vehicle_local_position.timestamp_sample
 ├──> setDt(dt) // set _dt in controllib Block for BlockDerivative
 ├──> <_vehicle_control_mode_sub.updated()>
 │   ├──> const bool previous_position_control_enabled = _vehicle_control_mode.flag_multicopter_position_control_enabled
 │   └──> <_vehicle_control_mode_sub.update(&_vehicle_control_mode)>
 │       ├──> <!previous_position_control_enabled && _vehicle_control_mode.flag_multicopter_position_control_enabled>
 │       │   └──> _time_position_control_enabled = _vehicle_control_mode.timestamp
 │       └──> <else if (previous_position_control_enabled && !_vehicle_control_mode.flag_multicopter_position_control_enabled>
 │           └──> reset_setpoint_to_nan(_setpoint)  // clear existing setpoint when controller is no longer active
 ├──> _vehicle_land_detected_sub.update(&_vehicle_land_detected)
 ├──> <_param_mpc_use_hte.get()><_hover_thrust_estimate_sub.update(&hte)><hte.valid>
 │   └──> _control.updateHoverThrust(hte.hover_thrust)
 ├──> _trajectory_setpoint_sub.update(&_setpoint)
 ├──> <(_setpoint.timestamp != 0) && (_setpoint.timestamp < vehicle_local_position.timestamp)> // adjust existing (or older) setpoint with any EKF reset deltas
 │   ├──> <vehicle_local_position.vxy_reset_counter != _vxy_reset_counter>
 │   │   ├──> _setpoint.vx += vehicle_local_position.delta_vxy[0]
 │   │   └──> _setpoint.vy += vehicle_local_position.delta_vxy[1]
 │   ├──> <vehicle_local_position.vz_reset_counter != _vz_reset_counter>
 │   │   └──> _setpoint.vz += vehicle_local_position.delta_vz
 │   ├──> <vehicle_local_position.xy_reset_counter != _xy_reset_counter>
 │   │   ├──> _setpoint.x += vehicle_local_position.delta_xy[0]
 │   │   └──> _setpoint.y += vehicle_local_position.delta_xy[1]
 │   ├──> <vehicle_local_position.z_reset_counter != _z_reset_counter>
 │   │   └──> _setpoint.z += vehicle_local_position.delta_z
 │   └──> <vehicle_local_position.heading_reset_counter != _heading_reset_counter>
 │       └──> _setpoint.yaw = wrap_pi(_setpoint.yaw + vehicle_local_position.delta_heading)
 ├──> <vehicle_local_position.vxy_reset_counter != _vxy_reset_counter>
 │   ├──> _vel_x_deriv.reset()
 │   └──> _vel_y_deriv.reset()
 ├──> vehicle_local_position.vz_reset_counter != _vz_reset_counter) {
 │   └──> _vel_z_deriv.reset()
 ├──> [save latest reset counters]
 │   ├──> _vxy_reset_counter = vehicle_local_position.vxy_reset_counter
 │   ├──> _vz_reset_counter = vehicle_local_position.vz_reset_counter
 │   ├──> _xy_reset_counter = vehicle_local_position.xy_reset_counter
 │   ├──> _z_reset_counter = vehicle_local_position.z_reset_counter
 │   └──> _heading_reset_counter = vehicle_local_position.heading_reset_counter
 ├──> PositionControlStates states{set_vehicle_states(vehicle_local_position)}
 ├──> <_vehicle_control_mode.flag_multicopter_position_control_enabled><(_setpoint.timestamp < _time_position_control_enabled) && (vehicle_local_position.timestamp_sample > _time_position_control_enabled)>
 │   └──> _setpoint = generateFailsafeSetpoint(vehicle_local_position.timestamp_sample, states) // set failsafe setpoint if there hasn't been a new, trajectory setpoint since position control started
 ├──> <_vehicle_control_mode.flag_multicopter_position_control_enabled && (_setpoint.timestamp >= _time_position_control_enabled)>
 │   └──> _vehicle_constraints_sub.update(&_vehicle_constraints) // update vehicle constraints and handle smooth takeoff
 ├──> <!PX4_ISFINITE(_vehicle_constraints.speed_up) || (_vehicle_constraints.speed_up > _param_mpc_z_vel_max_up.get())> // fix to prevent the takeoff ramp to ramp to a too high value or get stuck because of NAN
 │   └──> _vehicle_constraints.speed_up = _param_mpc_z_vel_max_up.get() // TODO: this should get obsolete once the takeoff limiting moves into the flight tasks
 ├──> <_vehicle_control_mode.flag_control_offboard_enabled>
 │   ├──> const bool want_takeoff = _vehicle_control_mode.flag_armed && (vehicle_local_position.timestamp_sample < _setpoint.timestamp + 1_s)
 │   ├──> <want_takeoff && PX4_ISFINITE(_setpoint.z) && (_setpoint.z < states.position(2))>
 │   │   └──> _vehicle_constraints.want_takeoff = true
 │   ├──> <want_takeoff && PX4_ISFINITE(_setpoint.vz) && (_setpoint.vz < 0.f)>
 │   │   └──> _vehicle_constraints.want_takeoff = true
 │   ├──> <want_takeoff && PX4_ISFINITE(_setpoint.acceleration[2]) && (_setpoint.acceleration[2] < 0.f)>
 │   │   └──> _vehicle_constraints.want_takeoff = true
 │   └──> <else>
 │       └──> _vehicle_constraints.want_takeoff = false
 ├──> [ override with defaults ]
 │   ├──> _vehicle_constraints.speed_up = _param_mpc_z_vel_max_up.get()
 │   └──> _vehicle_constraints.speed_down = _param_mpc_z_vel_max_dn.get()
 ├──> _takeoff.updateTakeoffState(_vehicle_control_mode.flag_armed, _vehicle_land_detected.landed, _vehicle_constraints.want_takeoff, _vehicle_constraints.speed_up, false, vehicle_local_position.timestamp_sample) // handle smooth takeoff
 ├──> const bool not_taken_off             = (_takeoff.getTakeoffState() < TakeoffState::rampup)
 ├──> const bool flying                    = (_takeoff.getTakeoffState() >= TakeoffState::flight)
 ├──> const bool flying_but_ground_contact = (flying && _vehicle_land_detected.ground_contact)
 ├──> <!flying>
 │   └──> _control.setHoverThrust(_param_mpc_thr_hover.get())
 ├──> <_takeoff.getTakeoffState() == TakeoffState::rampup>
 │   └──> _setpoint.acceleration[2] = NAN  // make sure takeoff ramp is not amended by acceleration feed-forward
 ├──> <not_taken_off || flying_but_ground_contact>
 │   ├──> reset_setpoint_to_nan(_setpoint) // we are not flying yet and need to avoid any corrections
 │   ├──> _setpoint.timestamp = vehicle_local_position.timestamp_sample
 │   ├──> Vector3f(0.f, 0.f, 100.f).copyTo(_setpoint.acceleration) // High downwards acceleration to make sure there's no thrust
 │   └──> _control.resetIntegral() // prevent any integrator windup
 ├──> [limit tilt during takeoff ramupup]
 │   ├──> const float tilt_limit_deg = (_takeoff.getTakeoffState() < TakeoffState::flight) ? _param_mpc_tiltmax_lnd.get() : _param_mpc_tiltmax_air.get()
 │   ├──> _control.setTiltLimit(_tilt_limit_slew_rate.update(math::radians(tilt_limit_deg), dt))
 │   ├──> const float speed_up = _takeoff.updateRamp(dt, PX4_ISFINITE(_vehicle_constraints.speed_up) ? _vehicle_constraints.speed_up : _param_mpc_z_vel_max_up.get())
 │   └──> const float speed_down = PX4_ISFINITE(_vehicle_constraints.speed_down) ? _vehicle_constraints.speed_down :_param_mpc_z_vel_max_dn.get()
 ├──> const float minimum_thrust = flying ? _param_mpc_thr_min.get() : 0.f  // Allow ramping from zero thrust on takeoff
 ├──> _control.setThrustLimits(minimum_thrust, _param_mpc_thr_max.get())
 ├──> float max_speed_xy = _param_mpc_xy_vel_max.get()
 ├──> <PX4_ISFINITE(vehicle_local_position.vxy_max)>
 │   └──> max_speed_xy = math::min(max_speed_xy, vehicle_local_position.vxy_max)
 ├──> _control.setVelocityLimits(max_speed_xy, math::min(speed_up, _param_mpc_z_vel_max_up.get()), math::max(speed_down, 0.f))// takeoff ramp starts with negative velocity limit
 ├──> _control.setInputSetpoint(_setpoint)
 ├──> <!PX4_ISFINITE(_setpoint.z) && PX4_ISFINITE(_setpoint.vz) && (fabsf(_setpoint.vz) > FLT_EPSILON) && PX4_ISFINITE(vehicle_local_position.z_deriv) && vehicle_local_position.z_valid && vehicle_local_position.v_z_valid>// update states
 │	 │    // A change in velocity is demanded and the altitude is not controlled.
 │	 │    // Set velocity to the derivative of position
 │	 │    // because it has less bias but blend it in across the landing speed range
 │	 │    //  <  MPC_LAND_SPEED: ramp up using altitude derivative without a step
 │	 │    //  >= MPC_LAND_SPEED: use altitude derivative
 │   ├──> float weighting = fminf(fabsf(_setpoint.vz) / _param_mpc_land_speed.get(), 1.f)
 │   └──> states.velocity(2) = vehicle_local_position.z_deriv * weighting + vehicle_local_position.vz * (1.f - weighting)
 ├──> _control.setState(states)
 ├──> <!_control.update(dt)>  // Run position control, Failsafe
 │   ├──> _vehicle_constraints = {0, NAN, NAN, false, {}} // reset constraints
 │   ├──> _control.setInputSetpoint(generateFailsafeSetpoint(vehicle_local_position.timestamp_sample, states))
 │   ├──> _control.setVelocityLimits(_param_mpc_xy_vel_max.get(), _param_mpc_z_vel_max_up.get(), _param_mpc_z_vel_max_dn.get())
 │   └──> _control.update(dt)
 ├──> [Publish internal position control setpoints] 
 │   ├──> _vehicle_local_position_setpoint_s local_pos_sp{}
 │   ├──> __control.getLocalPositionSetpoint(local_pos_sp)
 │   ├──> _local_pos_sp.timestamp = hrt_absolute_time()
 │   ├──> __local_pos_sp_pub.publish(local_pos_sp)  // on top of the input/feed-forward setpoints these containt the PID corrections, This message is used by other modules (such as Landdetector) to determine vehicle intention.
 │   ├──> _vehicle_attitude_setpoint_s attitude_setpoint{}
 │   ├──> __control.getAttitudeSetpoint(attitude_setpoint)
 │   ├──> _attitude_setpoint.timestamp = hrt_absolute_time()
 │   └──> __vehicle_attitude_setpoint_pub.publish(attitude_setpoint) // Publish attitude setpoint output
 ├──> <else>// an update is necessary here because otherwise the takeoff state doesn't get skipped with non-altitude-controlled modes
 │   └──> _takeoff.updateTakeoffState(_vehicle_control_mode.flag_armed, _vehicle_land_detected.landed, false, 10.f, true, vehicle_local_position.timestamp_sample)
 ├──> const uint8_t takeoff_state = static_cast<uint8_t>(_takeoff.getTakeoffState())
 └──> <takeoff_state != _takeoff_status_pub.get().takeoff_state || !isEqualF(_tilt_limit_slew_rate.getState(), _takeoff_status_pub.get().tilt_limit)>
     ├──> _takeoff_status_pub.get().takeoff_state = takeoff_state
     ├──> _takeoff_status_pub.get().tilt_limit = _tilt_limit_slew_rate.getState()
     ├──> _takeoff_status_pub.get().timestamp = hrt_absolute_time()
     └──> _takeoff_status_pub.update()  // Publish takeoff status

4. 总结

具体逻辑业务后续再做深入，从模块代码角度:

• 输入

uORB::SubscriptionCallbackWorkItem _local_pos_sub{this, ORB_ID(vehicle_local_position)};	/**< vehicle local position */

uORB::SubscriptionInterval _parameter_update_sub{ORB_ID(parameter_update), 1_s};

uORB::Subscription _hover_thrust_estimate_sub{ORB_ID(hover_thrust_estimate)};
uORB::Subscription _trajectory_setpoint_sub{ORB_ID(trajectory_setpoint)};
uORB::Subscription _vehicle_constraints_sub{ORB_ID(vehicle_constraints)};
uORB::Subscription _vehicle_control_mode_sub{ORB_ID(vehicle_control_mode)};
uORB::Subscription _vehicle_land_detected_sub{ORB_ID(vehicle_land_detected)};

• 输出

uORB::PublicationData<takeoff_status_s>              _takeoff_status_pub{ORB_ID(takeoff_status)};
uORB::Publication<vehicle_attitude_setpoint_s>	     _vehicle_attitude_setpoint_pub{ORB_ID(vehicle_attitude_setpoint)};
uORB::Publication<vehicle_local_position_setpoint_s> _local_pos_sp_pub{ORB_ID(vehicle_local_position_setpoint)};	/**< vehicle local position setpoint publication */

5. 参考资料

【1】PX4开源软件框架简明简介
【2】PX4模块设计之十一：Built-In框架
【3】PX4模块设计之十二：High Resolution Timer设计
【4】PX4模块设计之十三：WorkQueue设计
【5】PX4模块设计之十七：ModuleBase模块
【6】PX4模块设计之三十：Hysteresis类
【7】PX4 modules_main