固定翼姿态控制流程
固定翼控制流程
| 主文件夹 |
子文件 |
|
| fw_att_control位于src/moudle文件夹下 |
fw_att_control_main.c |
主文件 |
| fw_att_control_params.c |
主文件参数 |
|
| CMakeList |
|
|
|
attitude_fw位于src/lib/ecl/attitude_fw文件夹下
|
ecl_controller.c |
姿态控制器类 |
| ecl_controller.h |
|
|
| ecl_pitch_controller.c |
俯仰控制器 |
|
| ecl_pitch_controller.h |
|
|
| ecl_roll_controller.c |
滚转控制器 |
|
| ecl_roll_controller.h |
|
|
| ecl_yaw_controller.c |
偏航控制器 |
|
| ecl_yaw_controller.h |
|
|
| ecl_wheel_controller.c |
轮控制器 |
|
| ecl_wheel_controller.h |
|
-
入口函数
int fw_att_control_main(int argc, char *argv[])，检查是否启动
2.开始函数
Int FixedwingAttitudeControl::start()
其中px4_task_spawn_cmd开启fw_control进程
进入主进程FixedwingAttitudeControl::task_main_trampoline()
进入主任务FixedwingAttitudeControl::task_main()
3.数据订阅和数据更新
/*do subscriptions进行相关数据订阅 */
_att_sub = orb_subscribe(ORB_ID(vehicle_attitude));//姿态数据,滚转角速度,俯仰角速度,偏航角速度,四元数和四元数重置计数
_att_sp_sub = orb_subscribe(ORB_ID(vehicle_attitude_setpoint));//姿态角，起落架设置，航向控制设置
_vcontrol_mode_sub = orb_subscribe(ORB_ID(vehicle_control_mode));//控制模式,包括是否手动,是否控制爬升率等标志
_params_sub = orb_subscribe(ORB_ID(parameter_update));//用于通知一个或多个参数的更改
_manual_sub = orb_subscribe(ORB_ID(manual_control_setpoint));//手动控制的航点,包括杆位置,模式切换开关等属性变量
_global_pos_sub = orb_subscribe(ORB_ID(vehicle_global_position));//WGS84坐标系下的位置,经纬度,三轴速度等
_vehicle_status_sub = orb_subscribe(ORB_ID(vehicle_status));//无人机状态,包括arming state|hil state|navigation state等
_vehicle_land_detected_sub = orb_subscribe(ORB_ID(vehicle_land_detected));//检测降落
_battery_status_sub = orb_subscribe(ORB_ID(battery_status));//电池状态
Int FixedwingAttitudeControl::parameters_update()//参数更新函数
4.POLL轮询
vehicle_setpoint_poll(); // 姿态设定值轮询
vehicle_control_mode_poll(); //控制模式轮询
vehicle_manual_poll(); //手动控制轮询
vehicle_status_poll(); //无人机状态轮询
vehicle_land_detected_poll(); //降落轮询
battery_status_poll(); //电池状态轮询
_sub_airspeed.update();//更新空速
5.如果姿态改变，运行控制器
/*等待数据100毫秒 wait for up to 100ms for data */
int pret = px4_poll(&fds[0], (sizeof(fds) / sizeof(fds[0])), 100);
/* 只更新改变的参数only update parameters if they changed */
bool params_updated = false;
orb_check(_params_sub, ¶ms_updated);
if (params_updated) {
/* read from param to clear updated flag */
parameter_update_s update;
orb_copy(ORB_ID(parameter_update), _params_sub, &update);
/* update parameters from storage */
parameters_update();//更新参数函数
}
if (fds[0].revents & POLLIN) {
static uint64_t last_run = 0;
float deltaT = (hrt_absolute_time() - last_run) / 1000000.0f; //记录延迟时间
last_run = hrt_absolute_time();
/* guard against too large deltaT's 延迟时间应该在1s内*/
if (deltaT > 1.0f) {
deltaT = 0.01f;
}
/* 复制本地姿态数据load local copies */
orb_copy(ORB_ID(vehicle_attitude), _att_sub, &_att);
6.得到当前的控制状态数据
/* 由控制状态四元数得到当前旋转矩阵和欧拉角get current rotation matrix and euler angles from control state quaternions */
math::Quater.nion q_att(_att.q[0], _att.q[1], _att.q[2], _att.q[3]);
_R = q_att.to_dcm();
math::Vector<3> euler_angles;
euler_angles = _R.to_euler();
_roll = euler_angles(0);
_pitch = euler_angles(1);
_yaw = euler_angles(2);
//如果飞机类型是VTOL，则交换旋转矩阵的一三列
if (_vehicle_status.is_vtol && _parameters.vtol_type == vtol_type::TAILSITTER) {//尾座式垂直起降无人机
/* vehicle is a tailsitter, we need to modify the estimated attitude for fw mode
* 垂直起降如果是尾座式，我们需要修改飞控模式的估计姿态
* Since the VTOL airframe is initialized as a multicopter we need to
* modify the estimated attitude for the fixed wing operation.
* Since the neutral position of the vehicle in fixed wing mode is -90 degrees rotated around
* the pitch axis compared to the neutral position of the vehicle in multicopter mode
* 由于飞行器在固定翼模式中的中立位置于飞行器在多旋翼模式中的中立位置相比绕俯仰轴旋转-90度
*
* we need to swap the roll and the yaw axis (1st and 3rd column) in the rotation matrix.
* Additionally, in order to get the correct sign of the pitch, we need to multiply
* the new x axis of the rotation matrix with -1
* 我们需要交换旋转矩阵中的滚转和偏航轴（第一列和第三列），另外，为了得到正确俯仰信号，我们需要给旋转矩阵新的轴乘以-1
*
* original: modified:
*
* Rxx Ryx Rzx -Rzx Ryx Rxx
* Rxy Ryy Rzy -Rzy Ryy Rxy
* Rxz Ryz Rzz -Rzz Ryz Rxz
* */
math::Matrix<3, 3> R_adapted = _R; //modified rotation matrix
/* move z to x */
R_adapted(0, 0) = _R(0, 2);
R_adapted(1, 0) = _R(1, 2);
R_adapted(2, 0) = _R(2, 2);
/* move x to z */
R_adapted(0, 2) = _R(0, 0);
R_adapted(1, 2) = _R(1, 0);
R_adapted(2, 2) = _R(2, 0);
/* change direction of pitch (convert to right handed system) */
R_adapted(0, 0) = -R_adapted(0, 0);
R_adapted(1, 0) = -R_adapted(1, 0);
R_adapted(2, 0) = -R_adapted(2, 0);
euler_angles = R_adapted.to_euler(); //adapted euler angles for fixed wing operation
/* 填入新的姿态数据fill in new attitude data */
_R = R_adapted;
_roll = euler_angles(0);
_pitch = euler_angles(1);
_yaw = euler_angles(2);
/* lastly, roll- and yawspeed have to be swaped 最后，滚动-和偏航速度必须被交换*/
float helper = _att.rollspeed;
_att.rollspeed = -_att.yawspeed;
_att.yawspeed = helper;
}
_sub_airspeed.update();
//获取传感器和状态数据的初始更新，内部函数是检测是否更新，若更新了数据，就copy
vehicle_setpoint_poll();
vehicle_control_mode_poll();
vehicle_manual_poll();
global_pos_poll();
vehicle_status_poll();
vehicle_land_detected_poll();
battery_status_poll();
//检查我们是否处于纯角速率和姿态控制模式，手动输入是否高于设定值，用纯角速率模式 Check if we are in rattitude mode and the pilot is above the threshold on pitch
if (_vcontrol_mode.flag_control_rattitude_enabled) {
if (fabsf(_manual.y) > _parameters.rattitude_thres ||
fabsf(_manual.x)>_parameters.rattitude_thres) {
_parameters.rattitude_thres=0.8
_vcontrol_mode.flag_control_attitude_enabled = false;
}//manual.y表示手动输入滚转，_manual.x表示手动输入俯仰
}
7.运行姿态控制器
在姿态控制里，先稳定roll和pitch再稳定yaw。
7.1姿态控制模式
外环角度控制
_roll_ctrl.control_attitude(control_input);//滚转控制器，返回值为_rate_setpoint
_pitch_ctrl.control_attitude(control_input);//俯仰控制器
_yaw_ctrl.control_attitude(control_input); //最后运行，因为取决于滚动和俯仰姿态的输出runs last, because is depending on output of roll and pitch attitude
_wheel_ctrl.control_attitude(control_input);//机轮控制器
内环角速率控制
滚转角速率控制
float roll_u = _roll_ctrl.control_euler_rate(control_input);
_actuators.control[actuator_controls_s::INDEX_ROLL] = (PX4_ISFINITE(roll_u)) ?
roll_u + _parameters.trim_roll :_parameters.trim_roll;//给舵机相应的控制位赋值
俯仰角速率控制
float pitch_u = _pitch_ctrl.control_euler_rate(control_input);
_actuators.control[actuator_controls_s::INDEX_PITCH] = (PX4_ISFINITE(pitch_u)) ?
pitch_u + _parameters.trim_pitch :_parameters.trim_pitch;
偏航角速率控制
if (_parameters.w_en && _att_sp.fw_control_yaw) {
//_parameters.w_en=0使能对轮子的联动控制
yaw_u = _wheel_ctrl.control_bodyrate(control_input);
} else {//只通过方向舵来控制偏航
yaw_u = _yaw_ctrl.control_euler_rate(control_input);
}
_actuators.control[actuator_controls_s::INDEX_YAW] = (PX4_ISFINITE(yaw_u)) ? yaw_u + _parameters.trim_yaw : _parameters.trim_yaw;
7.2纯角速率控制模式
_roll_ctrl.set_bodyrate_setpoint(_manual.y*_parameters.acro_max_x_rate_rad);
/_parameters.acro_max_x_rate_rad=90
_pitch_ctrl.set_bodyrate_setpoint(-_manual.x * _parameters.acro_max_y_rate_rad);
_yaw_ctrl.set_bodyrate_setpoint(_manual.r * _parameters.acro_max_z_rate_rad);
float roll_u = _roll_ctrl.control_bodyrate(control_input);
_actuators.control[actuator_controls_s::INDEX_ROLL] = (PX4_ISFINITE(roll_u)) ?
roll_u + _parameters.trim_roll : _parameters.trim_roll;
float pitch_u = _pitch_ctrl.control_bodyrate(control_input);
_actuators.control[actuator_controls_s::INDEX_PITCH] = (PX4_ISFINITE(pitch_u)) ?
pitch_u + _parameters.trim_pitch :_parameters.trim_pitch;
float yaw_u = _yaw_ctrl.control_bodyrate(control_input);
_actuators.control[actuator_controls_s::INDEX_YAW] = (PX4_ISFINITE(yaw_u)) ?
yaw_u + _parameters.trim_yaw :_parameters.trim_yaw;
_actuators.control[actuator_controls_s::INDEX_THROTTLE] =(PX4_ISFINITE(throttle_sp) &&//!(_vehicle_status.engine_failure ||!_vehicle_status.engine_failure_cmd) ? throttle_sp : 0.0f;
7.3完全手动控制
_actuators.control[actuator_controls_s::INDEX_ROLL] = _manual.y * _parameters.man_roll_scale + _parameters.trim_roll;
_actuators.control[actuator_controls_s::INDEX_PITCH] =
-_manual.x * _parameters.man_pitch_scale +_parameters.trim_pitch;
_actuators.control[actuator_controls_s::INDEX_YAW] = _manual.r * _parameters.man_yaw_scale + _parameters.trim_yaw;
_actuators.control[actuator_controls_s::INDEX_THROTTLE] = _manual.z;
8.舵机数据发布
/* 延迟发布舵机设定值 lazily publish the setpoint only once available */
_actuators.timestamp = hrt_absolute_time();
_actuators.timestamp_sample = _att.timestamp;
_actuators_airframe.timestamp = hrt_absolute_time();
_actuators_airframe.timestamp_sample = _att.timestamp;
/* publish the actuator controls 发布舵机控制*/
if (_vcontrol_mode.flag_control_rates_enabled ||_vcontrol_mode.flag_control_attitude_enabled ||_vcontrol_mode.flag_control_manual_enabled) {
if (_actuators_0_pub != nullptr) {orb_publish(_actuators_id, _actuators_0_pub, &_actuators);
} else if (_actuators_id) {_actuators_0_pub = orb_advertise(_actuators_id, &_actuators);
}//如果是空指针，就公告它
if (_actuators_2_pub != nullptr) {/* publish the actuator controls*/
orb_publish(ORB_ID(actuator_controls_2), _actuators_2_pub, &_actuators_airframe);
} else {/* advertise and publish */
_actuators_2_pub = orb_advertise(ORB_ID(actuator_controls_2), &_actuators_airframe);
}