PX4模块设计之三十七：MulticopterRateControl模块

Posted 2022-09-16 lida2003

PX4模块设计之三十七：MulticopterRateControl模块

• 1. MulticopterRateControl模块简介
• 2. 模块入口函数
• • 2.1 主入口mc_rate_control_main
  • 2.2 自定义子命令custom_command
• 3. MulticopterRateControl模块重要函数
• • 3.1 task_spawn
  • 3.2 instantiate
  • 3.3 init
  • 3.4 Run
• 4. 总结
• 5. 参考资料

1. MulticopterRateControl模块简介

### Description
This implements the multicopter rate controller. It takes rate setpoints (in acro mode
via `manual_control_setpoint` topic) as inputs and outputs actuator control messages.

The controller has a PID loop for angular rate error.

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

   stop

   status        print status info

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

class MulticopterRateControl : public ModuleBase<MulticopterRateControl>, public ModuleParams, public px4::WorkItem

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

2. 模块入口函数

2.1 主入口mc_rate_control_main

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

mc_rate_control_main
 └──> return MulticopterRateControl::main(argc, argv)

2.2 自定义子命令custom_command

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

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

3. MulticopterRateControl模块重要函数

3.1 task_spawn

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

MulticopterRateControl::task_spawn
 ├──> bool vtol = false
 ├──> <argc > 1><strcmp(argv[1], "vtol") == 0>
 │   └──> vtol = true
 ├──> MulticopterRateControl *instance = new MulticopterRateControl(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
 └──> return PX4_ERROR

3.2 instantiate

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

3.3 init

在task_spawn中使用，对_vehicle_angular_velocity_sub成员变量进行事件回调注册(当有vehicle_angular_velocity消息时，会调用SubscriptionCallbackWorkItem::ScheduleNow，再触发MulticopterRateControl::Run过程)。

MulticopterRateControl::init
 ├──> <!_vehicle_angular_velocity_sub.registerCallback()>
 │   ├──> PX4_ERR("callback registration failed")
 │   └──> return false
 └──> return true;

3.4 Run

根据输入参数及业务逻辑计算输出量，并发布消息。

MulticopterRateControl::Run
 ├──> [优雅退出处理]
 ├──> <_parameter_update_sub.updated()>  // Check if parameters have changed
 │   ├──> _parameter_update_sub.copy(&param_update)
 │   ├──> updateParams()
 │   └──> parameters_updated()
 ├──> <!_vehicle_angular_velocity_sub.update(&angular_velocity)>
 │   └──> return
 ├──> [grab corresponding vehicle_angular_acceleration immediately after vehicle_angular_velocity copy]
 │   ├──> vehicle_angular_acceleration_s v_angular_acceleration
 │   └──> _vehicle_angular_acceleration_sub.copy(&v_angular_acceleration)
 ├──> const hrt_abstime now = angular_velocity.timestamp_sample
 ├──> const float dt = math::constrain(((now - _last_run) * 1e-6f), 0.000125f, 0.02f)  //Guard against too small (< 0.125ms) and too large (> 20ms) dt's.
 ├──> _last_run = now
 ├──> const Vector3f angular_accelv_angular_acceleration.xyz
 ├──> Vector3f ratesangular_velocity.xyz
 ├──> _vehicle_control_mode_sub.update(&_vehicle_control_mode)  //check for updates in other topics 
 ├──> <_vehicle_land_detected_sub.updated()><_vehicle_land_detected_sub.copy(&vehicle_land_detected)>
 │   ├──> _landed = vehicle_land_detected.landed
 │   └──> _maybe_landed = vehicle_land_detected.maybe_landed
 ├──> _vehicle_status_sub.update(&_vehicle_status)
 ├──> <_landing_gear_sub.updated()><_landing_gear_sub.copy(&landing_gear)><landing_gear.landing_gear != landing_gear_s::GEAR_KEEP>
 │   ├──> <landing_gear.landing_gear == landing_gear_s::GEAR_UP && (_landed || _maybe_landed)>
 │   │   ├──> mavlink_log_critical(&_mavlink_log_pub, "Landed, unable to retract landing gear\\t")
 │   │   └──> events::send(events::ID("mc_rate_control_not_retract_landing_gear_landed"), events::Log::Error, events::LogInternal::Info, "Landed, unable to retract landing gear")
 │   └──> <else>
 │       └──>_landing_gear = landing_gear.landing_gear
 ├──> <_vehicle_control_mode.flag_control_manual_enabled && !_vehicle_control_mode.flag_control_attitude_enabled><_manual_control_setpoint_sub.update(&manual_control_setpoint)>  // manual rates control - ACRO mode
 │   ├──> const Vector3f man_rate_sp
 │	 │		math::superexpo(manual_control_setpoint.y, _param_mc_acro_expo.get(), _param_mc_acro_supexpo.get()),
 │	 │		math::superexpo(-manual_control_setpoint.x, _param_mc_acro_expo.get(), _param_mc_acro_supexpo.get()),
 │	 │		math::superexpo(manual_control_setpoint.r, _param_mc_acro_expo_y.get(), _param_mc_acro_supexpoy.get())
 │   ├──> _rates_setpoint = man_rate_sp.emult(_acro_rate_max)
 │   ├──> _thrust_setpoint(2) = -math::constrain(manual_control_setpoint.z, 0.0f, 1.0f)
 │   ├──> _thrust_setpoint(0) = _thrust_setpoint(1) = 0.f
 │   ├──> vehicle_rates_setpoint.roll = _rates_setpoint(0)
 │   ├──> vehicle_rates_setpoint.pitch = _rates_setpoint(1)
 │   ├──> vehicle_rates_setpoint.yaw = _rates_setpoint(2)
 │   ├──> _thrust_setpoint.copyTo(vehicle_rates_setpoint.thrust_body)
 │   ├──> vehicle_rates_setpoint.timestamp = hrt_absolute_time()
 │   └──> 【发布vehicle_rates_setpoint消息】_vehicle_rates_setpoint_pub.publish(vehicle_rates_setpoint)  // publish rate setpoint
 ├──> <else if (_vehicle_rates_setpoint_sub.update(&vehicle_rates_setpoint)><_vehicle_rates_setpoint_sub.copy(&vehicle_rates_setpoint)>
 │   ├──> _rates_setpoint(0) = PX4_ISFINITE(vehicle_rates_setpoint.roll)  ? vehicle_rates_setpoint.roll  : rates(0)
 │   ├──> _rates_setpoint(1) = PX4_ISFINITE(vehicle_rates_setpoint.pitch) ? vehicle_rates_setpoint.pitch : rates(1)
 │   ├──> _rates_setpoint(2) = PX4_ISFINITE(vehicle_rates_setpoint.yaw)   ? vehicle_rates_setpoint.yaw   : rates(2)
 │   └──> _thrust_setpoint = Vector3f(vehicle_rates_setpoint.thrust_body)
 ├──> <_vehicle_control_mode.flag_control_rates_enabled && !_actuators_0_circuit_breaker_enabled> // run the rate controller
 ├──> <!_vehicle_control_mode.flag_armed || _vehicle_status.vehicle_type != vehicle_status_s::VEHICLE_TYPE_ROTARY_WING>
 │   └──> _rate_control.resetIntegral()   // reset integral if disarmed
 ├──> control_allocator_status_s control_allocator_status  // update saturation status from control allocation feedback
 ├──> <_control_allocator_status_sub.update(&control_allocator_status)>
 │   ├──> <!control_allocator_status.torque_setpoint_achieved>
 │   │   └──> <for (size_t i = 0 i < 3 i++)>
 │   │       ├──> <control_allocator_status.unallocated_torque[i] > FLT_EPSILON>
 │   │       │   └──> saturation_positive(i) = true
 │   │       └──> <else if (control_allocator_status.unallocated_torque[i] < -FLT_EPSILON>
 │   │           └──> saturation_negative(i) = true
 │   └──> _rate_control.setSaturationStatus(saturation_positive, saturation_negative)  // TODO: send the unallocated value directly for better anti-windup
 ├──> const Vector3f att_control = _rate_control.update(rates, _rates_setpoint, angular_accel, dt, _maybe_landed || _landed)  // run rate controller
 ├──> [publish rate controller status]
 │   ├──> _rate_control.getRateControlStatus(rate_ctrl_status)
 │   ├──> rate_ctrl_status.timestamp = hrt_absolute_time()
 │   └──> 【发布rate_ctrl_status消息】_controller_status_pub.publish(rate_ctrl_status)
 ├──> [publish actuator controls]
 │   ├──> actuators.control[actuator_controls_s::INDEX_ROLL] = PX4_ISFINITE(att_control(0)) ? att_control(0) : 0.0f
 │   ├──> actuators.control[actuator_controls_s::INDEX_PITCH] = PX4_ISFINITE(att_control(1)) ? att_control(1) : 0.0f
 │   ├──> actuators.control[actuator_controls_s::INDEX_YAW] = PX4_ISFINITE(att_control(2)) ? att_control(2) : 0.0f
 │   ├──> actuators.control[actuator_controls_s::INDEX_THROTTLE] = PX4_ISFINITE(_thrust_setpoint(2)) ? -_thrust_setpoint(2) : 0.0f
 │   ├──> actuators.control[actuator_controls_s::INDEX_LANDING_GEAR] = _landing_gear
 │   ├──> actuators.timestamp_sample = angular_velocity.timestamp_sample
 │   ├──> <!_vehicle_status.is_vtol>
 │   │   ├──> 【发布vehicle_thrust_setpoint消息】publishTorqueSetpoint(att_control, angular_velocity.timestamp_sample)
 │   │   └──> 【发布vehicle_torque_setpoint消息】publishThrustSetpoint(angular_velocity.timestamp_sample)
 │   ├──> <_param_mc_bat_scale_en.get()>  // scale effort by battery status if enabled
 │   │   ├──><_battery_status_sub.updated()><_battery_status_sub.copy(&battery_status) && battery_status.connected && battery_status.scale > 0.f>
 │   │   │   └──> _battery_status_scale = battery_status.scale
 │   │   ├──> <_battery_status_scale > 0.0f><for (int i = 0 i < 4 i++)>
 │   │   │   └──> actuators.control[i] *= _battery_status_scale
 │   ├──> actuators.timestamp = hrt_absolute_time()
 │   ├──> 【发布actuator_controls_0消息】_actuator_controls_0_pub.publish(actuators)
 │   └──> 【发布actuator_controls_status_0消息】updateActuatorControlsStatus(actuators, dt)
 └──> <_vehicle_control_mode.flag_control_termination_enabled><!_vehicle_status.is_vtol> // publish actuator controls
     ├──> actuator_controls_s actuators
     ├──> actuators.timestamp = hrt_absolute_time()
     └──> 【发布actuator_controls_0消息】_actuator_controls_0_pub.publish(actuators)

4. 总结

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

• 输入

uORB::Subscription _battery_status_subORB_ID(battery_status);
uORB::Subscription _control_allocator_status_subORB_ID(control_allocator_status);
uORB::Subscription _landing_gear_subORB_ID(landing_gear);
uORB::Subscription _manual_control_setpoint_subORB_ID(manual_control_setpoint);
uORB::Subscription _vehicle_angular_acceleration_subORB_ID(vehicle_angular_acceleration);
uORB::Subscription _vehicle_control_mode_subORB_ID(vehicle_control_mode);
uORB::Subscription _vehicle_land_detected_subORB_ID(vehicle_land_detected);
uORB::Subscription _vehicle_rates_setpoint_subORB_ID(vehicle_rates_setpoint);
uORB::Subscription _vehicle_status_subORB_ID(vehicle_status);

uORB::SubscriptionInterval _parameter_update_subORB_ID(parameter_update), 1_s;

uORB::SubscriptionCallbackWorkItem _vehicle_angular_velocity_subthis, ORB_ID(vehicle_angular_velocity);

• 输出

uORB::Publication<actuator_controls_s>		_actuator_controls_0_pub;
uORB::Publication<actuator_controls_status_s>	_actuator_controls_status_0_pubORB_ID(actuator_controls_status_0);
uORB::PublicationMulti<rate_ctrl_status_s>	_controller_status_pubORB_ID(rate_ctrl_status);
uORB::Publication<vehicle_rates_setpoint_s>	_vehicle_rates_setpoint_pubORB_ID(vehicle_rates_setpoint);
uORB::Publication<vehicle_thrust_setpoint_s>	_vehicle_thrust_setpoint_pubORB_ID(vehicle_thrust_setpoint);
uORB::Publication<vehicle_torque_setpoint_s>	_vehicle_torque_setpoint_pubORB_ID(vehicle_torque_setpoint);

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