### 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 [arguments...]
Commands:
start
[vtol] VTOL mode
stop
status print status info
注:print_usage函数是具体对应实现。
class MulticopterRateControl : public ModuleBase, public ModuleParams, public px4::WorkItem
注:MulticopterRateControl模块采用了ModuleBase和WorkQueue设计。
同样继承了ModuleBase,由ModuleBase的main来完成模块入口。
mc_rate_control_main
└──> return MulticopterRateControl::main(argc, argv)
模块仅支持start/stop/status命令,不支持其他自定义命令。
MulticopterRateControl::custom_command
└──> return print_usage("unknown command")
这里主要初始化了MulticopterRateControl::task_spawn
对象,后续通过WorkQueue来完成进行轮询。
MulticopterRateControl::task_spawn
├──> bool vtol = false
├──> 1>
│ └──> vtol = true
├──> MulticopterRateControl *instance = new MulticopterRateControl(vtol)
├──>
│ ├──> _object.store(instance)
│ ├──> _task_id = task_id_is_work_queue
│ └──> <(instance->init()>
│ └──> return PX4_OK
├──>
│ └──> PX4_ERR("alloc failed")
├──> delete instance
├──> _object.store(nullptr)
├──> _task_id = -1
└──> return PX4_ERROR
注:鉴于该模块不采用任务(线程),所以ModuleBase::run_trampoline无需执行,所以可以不实现。
在task_spawn中使用,对_vehicle_angular_velocity_sub成员变量进行事件回调注册(当有vehicle_angular_velocity消息时,会调用SubscriptionCallbackWorkItem::ScheduleNow,再触发MulticopterRateControl::Run过程)。
MulticopterRateControl::init
├──>
│ ├──> PX4_ERR("callback registration failed")
│ └──> return false
└──> return true;
根据输入参数及业务逻辑计算输出量,并发布消息。
MulticopterRateControl::Run
├──> [优雅退出处理]
├──> <_parameter_update_sub.updated()> // Check if parameters have changed
│ ├──> _parameter_update_sub.copy(¶m_update)
│ ├──> updateParams()
│ └──> parameters_updated()
├──>
│ └──> 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_accel{v_angular_acceleration.xyz}
├──> Vector3f rates{angular_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)>
│ ├──>
│ │ ├──> 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")
│ └──>
│ └──>_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
├──> <_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
├──>
│ └──> _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)>
│ ├──>
│ │ └──>
│ │ ├──> FLT_EPSILON>
│ │ │ └──> saturation_positive(i) = true
│ │ └──>
│ │ └──> 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_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>
│ │ │ └──> 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> // publish actuator controls
├──> actuator_controls_s actuators{}
├──> actuators.timestamp = hrt_absolute_time()
└──> 【发布actuator_controls_0消息】_actuator_controls_0_pub.publish(actuators)
具体逻辑业务后续再做深入,从模块代码角度:
uORB::Subscription _battery_status_sub{ORB_ID(battery_status)};
uORB::Subscription _control_allocator_status_sub{ORB_ID(control_allocator_status)};
uORB::Subscription _landing_gear_sub{ORB_ID(landing_gear)};
uORB::Subscription _manual_control_setpoint_sub{ORB_ID(manual_control_setpoint)};
uORB::Subscription _vehicle_angular_acceleration_sub{ORB_ID(vehicle_angular_acceleration)};
uORB::Subscription _vehicle_control_mode_sub{ORB_ID(vehicle_control_mode)};
uORB::Subscription _vehicle_land_detected_sub{ORB_ID(vehicle_land_detected)};
uORB::Subscription _vehicle_rates_setpoint_sub{ORB_ID(vehicle_rates_setpoint)};
uORB::Subscription _vehicle_status_sub{ORB_ID(vehicle_status)};
uORB::SubscriptionInterval _parameter_update_sub{ORB_ID(parameter_update), 1_s};
uORB::SubscriptionCallbackWorkItem _vehicle_angular_velocity_sub{this, ORB_ID(vehicle_angular_velocity)};
uORB::Publication _actuator_controls_0_pub;
uORB::Publication _actuator_controls_status_0_pub{ORB_ID(actuator_controls_status_0)};
uORB::PublicationMulti _controller_status_pub{ORB_ID(rate_ctrl_status)};
uORB::Publication _vehicle_rates_setpoint_pub{ORB_ID(vehicle_rates_setpoint)};
uORB::Publication _vehicle_thrust_setpoint_pub{ORB_ID(vehicle_thrust_setpoint)};
uORB::Publication _vehicle_torque_setpoint_pub{ORB_ID(vehicle_torque_setpoint)};
【1】PX4开源软件框架简明简介
【2】PX4模块设计之十一:Built-In框架
【3】PX4模块设计之十二:High Resolution Timer设计
【4】PX4模块设计之十三:WorkQueue设计
【5】PX4模块设计之十七:ModuleBase模块
【6】PX4模块设计之三十:Hysteresis类
【7】PX4 modules_main