px4ctrl understanding and tunning
状态机
Px4ctrl的状态机对象如下：
class PX4CtrlFSM
{
public:
// somedata
private:
State_t state; // Should only be changed in PX4CtrlFSM::process() function!
AutoTakeoffLand_t takeoff_land;
// ---- control related ----
Desired_State_t get_hover_des();
Desired_State_t get_cmd_des();
// ---- auto takeoff/land ----
void motors_idling(const Imu_Data_t &imu, Controller_Output_t &u);
void land_detector(const State_t state, const Desired_State_t &des, const Odom_Data_t &odom); // Detect landing
void set_start_pose_for_takeoff_land(const Odom_Data_t &odom);
Desired_State_t get_rotor_speed_up_des(const ros::Time now);
Desired_State_t get_takeoff_land_des(const double speed);
// ---- tools ----
void set_hov_with_odom();
void set_hov_with_rc();
bool toggle_offboard_mode(bool on_off); // It will only try to toggle once, so not blocked.
bool toggle_arm_disarm(bool arm); // It will only try to toggle once, so not blocked.
void reboot_FCU();
void publish_bodyrate_ctrl(const Controller_Output_t &u, const ros::Time &stamp);
void publish_attitude_ctrl(const Controller_Output_t &u, const ros::Time &stamp);
void publish_trigger(const nav_msgs::Odometry &odom_msg);
};
在 Input.cpp 里的 rc_data 的回调函数 feed()包含了通道的含义，涉及各通道切换的功能：
enter_hover_mode,is_hover_modeenter_command_mode,is_command_mode
类成员函数 feed() 是如何被绑定为 ros subscriber 的回调函数的？使用 std::bind，例子如下，bind有三个参数，第一个参数指明哪一个类成员函数，第二个参数指明类的实例，第三个参数指明将接收到的 mavros_msgs::State 消息传递给 feed 函数的第一个参数。
ros::Subscriber state_sub =
nh.subscribe<mavros_msgs::State>("/mavros/state",
10,
boost::bind(&State_Data_t::feed, &fsm.state_data, _1));
起飞流程
接下来梳理一下自动起飞的状态切换流程。首先要保证遥控器 5、6 通道已拨，分别表示进入 offboard 模式（也称 hover mode）和 command 模式，只有 两个条件 均满足，且 起飞使能（在yaml文件里配置） 和 起飞信号触发 才会从 MANUAL_CTRL 切换到 AUTO_TAKEOFF 状态，当然中间还有很多检查。在进入 AUTO_TAKEOFF 前会重置 thr2acc_ 和设置起飞的起始点（已当前的里程计的值作为起点，四自由度），然后px4发送消息使其切换为 offboard 模式并自动解锁，记录当前时间戳 takeoff_land.toggle_takeoff_land_time = now_time;。
void PX4CtrlFSM::set_start_pose_for_takeoff_land(const Odom_Data_t &odom)
{
takeoff_land.start_pose.head<3>() = odom_data.p;
takeoff_land.start_pose(3) = get_yaw_from_quaternion(odom_data.q);
takeoff_land.toggle_takeoff_land_time = ros::Time::now();
}
struct AutoTakeoffLand_t
{
bool landed{true};
ros::Time toggle_takeoff_land_time;
std::pair<bool, ros::Time> delay_trigger{std::pair<bool, ros::Time>(false, ros::Time(0))};
Eigen::Vector4d start_pose;
static constexpr double MOTORS_SPEEDUP_TIME = 3.0; // motors idle running for 3 seconds before takeoff
static constexpr double TAKEOFF_SPEEDUP_TIME = 0.5; // Time to speed up during takeoff font stage
static constexpr double DELAY_TRIGGER_TIME = 2.0; // Time to be delayed when reach at target height
};
进入 AUTO_TAKEOFF 模式后，电机需要先怠速旋转一段时间，其速度指数增长，但提供很小的推力，用于警告周围的人。怠速旋转时长到了之后，则执行起飞，具体的控制可看下文的控制器章节，简单来说即目标位置是斜坡函数，目标速度是阶跃函数，也就是期望飞行器匀速上升。当到达指定高度后，状态将会在下一时间片切到 AUTO_HOVER，在切换之前执行 set_hov_with_odom()，将当前的里程计赋值给 hov_pose（4DoF）。
进入 AUTO_HOVER 后：
- 如果没有里程计 或者 不是 hover mode，切回
MANUAL_CTRL - 如果是 command mode 且 收到了 cmd 且 没进入 emergency hover，则进一步判断是否是 offboard 模式，是的话则切换到
CMD_CTRL - 如果收到降落信号 或 emergency_hover，则进入
AUTO_LAND状态 - 最后才是 执行悬停控制
set_hov_with_rc(); des = get_hover_des(); if ((rc_data.enter_command_mode) || (takeoff_land.delay_trigger.first && now_time > takeoff_land.delay_trigger.second)) { takeoff_land.delay_trigger.first = false; publish_trigger(odom_data.msg); ROS_INFO("\033[32m[px4ctrl] TRIGGER sent, allow user command.\033[32m"); }
控制器
其位置速度P控制器是并联的，其实和串联的等价。（闭环传函推导、参数选择详见 onenote）
但如果速度环增加 I D，则串联和并联不再等价，目前的速度环增加了 I D，因此将控制器结构改成了串联形式。
改进 1：增加起飞的加速度前馈
起飞阶段的前期设置期望加速度，因为速度不可能突变，因此非零的期望加速度相当于前馈，符合系统的变化，提高相应速度。 \(a_{des} = 0.02||g||\sin(\frac{\pi}{t_{speedup}}\Delta t)\)
Desired_State_t PX4CtrlFSM::get_takeoff_land_des(const double speed)
{
ros::Time now = ros::Time::now();
double delta_t = (now - takeoff_land.toggle_takeoff_land_time).toSec() - (speed > 0 ? AutoTakeoffLand_t::MOTORS_SPEEDUP_TIME : 0); // speed > 0 means takeoff
// takeoff_land.last_set_cmd_time = now;
// takeoff_land.start_pose(2) += speed * delta_t;
Desired_State_t des;
des.p = takeoff_land.start_pose.head<3>() + Eigen::Vector3d(0, 0, speed * delta_t);
des.v = Eigen::Vector3d(0, 0, speed);
if (speed > 0) {
double des_a_z = (delta_t < AutoTakeoffLand_t::TAKEOFF_SPEEDUP_TIME) ? (0.02 * param_.gra * sin(M_PI/AutoTakeoffLand_t::TAKEOFF_SPEEDUP_TIME * delta_t)) : 0;
des.a = Eigen::Vector3d(0, 0, des_a_z);
}
else {
des.a = Eigen::Vector3d::Zero();
}
des.j = Eigen::Vector3d::Zero();
des.yaw = takeoff_land.start_pose(3);
des.yaw_rate = 0.0;
return des;
}
实验测试结果表明，确实能够提高起飞离开地面的速度，响应更快，xy偏离更少。
改进 2：在起飞阶段也进行油门估计
油门估计的原理是带记忆消逝的迭代最小二乘，利用一系列的油门值和加速度测量值，来估计二者对应直线的斜率（假设二者是线性关系）。
px4ctrl 里的油门估计存在一个假设，即无人机只受重力和油门推力，因此当无人机还受地面支持力时，是不能够使用油门估计的。
当电机怠速转动结束的一段时间后，此时推力大概率已经等于或大于重力，也就是此时不存在地面支撑力了，经验选择为怠速转动结束后的0.2s，运行油门估计函数。
实验表明，在起飞阶段使用油门估计，无论电池电压高低，四旋翼均能稳定地起飞和悬停，并且起飞到悬停的切换也不会带来跟之前一样的震荡。
改进 3：最大化相位裕度原则，简化调参
可将速度环的 P I 两个参数，转换为为阻尼因子 $\delta$ 的选择
\[P = \frac{1}{\tau \delta}\] \[I = \frac{1}{\tau^2\delta^3}\]如果是离散化，I 还需要乘上控制时间间隔 $\Delta t_{ctrl}$，或者除以控制频率。