当前位置:   article > 正文

PX4代码学习系列博客(6)——offboard模式位置控制代码分析_px4 offbord

px4 offbord

这篇我觉得分析得不错

 

可以结合这篇博文来看

https://blog.csdn.net/sinat_16643223/article/details/107874844

是不是可以和这个图结合起来看

https://www.bilibili.com/video/BV1H5411t7Fx?from=search&seid=12190066108915088657

 

 

摘自:https://blog.csdn.net/iamqianrenzhan/article/details/78066369

PX4代码学习系列博客(6)——offboard模式位置控制代码分析

仟人斩 2017-09-22 19:49:03 2651 收藏 6

分类专栏: px4

版权

分析offboard模式的代码需要用到以下几个模块

local_position_estimator
mavlink
mc_pos_control
mc_att_control
mixer

程序数据走向

一般的offboard模式需要给飞控发送两条mavlink消息:

  1. //飞控在接收到mavlink设置点消息的时候执行下面函数
  2. void MavlinkReceiver::handle_message_set_position_target_local_ned(mavlink_message_t *msg)
  3. {
  4. //这个函数把消息解析后发布了两个消息:
  5. //offboard_control_mode和position_setpoint_triplet
  6. }
  1. //飞控在接收到mavlink当前点消息用来进行外部位置估计的时候执行下面函数
  2. void MavlinkReceiver::handle_message_att_pos_mocap(mavlink_message_t *msg)
  3. {
  4. //这个函数把消息解析后发布消息:
  5. //att_pos_mocap
  6. }

首先分析handle_message_set_position_target_local_ned这个函数发出的两条消息各是什么内容
发出的offboard_control_mode_s结构体:

  1. offboard_control_mode.timestamp = hrt_absolute_time();
  2. offboard_control_mode.ignore_position = (bool)(set_position_target_local_ned.type_mask & 0x7);
  3. offboard_control_mode.ignore_alt_hold = (bool)(set_position_target_local_ned.type_mask & 0x4);
  4. offboard_control_mode.ignore_velocity = (bool)(set_position_target_local_ned.type_mask & 0x38);
  5. offboard_control_mode.ignore_acceleration_force = (bool)(set_position_target_local_ned.type_mask & 0x1C0);
  6. bool is_force_sp = (bool)(set_position_target_local_ned.type_mask & (1 << 9));
  7. offboard_control_mode.ignore_attitude = (bool)(set_position_target_local_ned.type_mask & 0x400);
  8. offboard_control_mode.ignore_bodyrate = (bool)(set_position_target_local_ned.type_mask & 0x800);

读取的vehicle_control_mode_s结构体判断当前是否是offboard模式,如果是则发送position_setpoint_triplet消息
发出的position_setpoint_triplet_s结构体:

  1. pos_sp_triplet.current.type
  2. pos_sp_triplet.timestamp = hrt_absolute_time();
  3. pos_sp_triplet.previous.valid = false;
  4. pos_sp_triplet.next.valid = false;
  5. pos_sp_triplet.current.valid = true;
  6. pos_sp_triplet.current.position_valid = true;
  7. pos_sp_triplet.current.x = set_position_target_local_ned.x;
  8. pos_sp_triplet.current.y = set_position_target_local_ned.y;
  9. pos_sp_triplet.current.z = set_position_target_local_ned.z;
  10. pos_sp_triplet.current.velocity_valid = true;
  11. pos_sp_triplet.current.vx = set_position_target_local_ned.vx;
  12. pos_sp_triplet.current.vy = set_position_target_local_ned.vy;
  13. pos_sp_triplet.current.vz = set_position_target_local_ned.vz;
  14. pos_sp_triplet.current.acceleration_valid = true;
  15. pos_sp_triplet.current.a_x = set_position_target_local_ned.afx;
  16. pos_sp_triplet.current.a_y = set_position_target_local_ned.afy;
  17. pos_sp_triplet.current.a_z = set_position_target_local_ned.afz;
  18. pos_sp_triplet.current.acceleration_is_force = is_force_sp;
  19. pos_sp_triplet.current.yaw_valid = true;
  20. pos_sp_triplet.current.yaw = set_position_target_local_ned.yaw;
  21. pos_sp_triplet.current.yawspeed_valid = true;
  22. pos_sp_triplet.current.yawspeed = set_position_target_local_ned.yaw_rate;

local_position_estimator

在Firmware\src\modules\local_position_estimator\sensors下的mocap.cpp中有这么一行

static const uint32_t       MOCAP_TIMEOUT =     200000; // 0.2 s
  •  

这代表att_pos_mocap消息的发送频率必须大于5Hz,不然就会timeout,但是在实际测试过程中,发送的频率在30Hz以上才能稳定不timeout,这是因为它的程序写的太苛刻,只要每两帧有效数据的时间戳间隔大于0.2s就会timeout。

在Firmware\src\modules\local_position_estimator下的BlockLocalPositionEstimator.cpp中有这么一段

  1. if (mocapUpdated) {
  2. if (_sensorTimeout & SENSOR_MOCAP)
  3. {
  4. mocapInit();
  5. {
  6. mocapCorrect();
  7. }
  8. }

只要超时就会重新初始化。

如果外部估计的位置足够精确,可以选择不用飞控上的其他传感器,可以在参数LPE_FUSION中设置。

mc_pos_control

然后转到旋翼的位置控制模块:
在目录E:\github\Firmware\src\modules\mc_pos_control下的mc_pos_control_main.cpp文件
打开task_main函数,发现

  1. _pos_sp_triplet_sub = orb_subscribe(ORB_ID(position_setpoint_triplet));
  2. _control_mode_sub = orb_subscribe(ORB_ID(vehicle_control_mode));

它订阅了position_setpoint_triplet和vehicle_control_mode这两个消息。
然后是

  1. update_velocity_derivative();
  2. //这个函数用来更新速度偏差,如果目标位置丢失,则速度偏差为0。
  1. do_control(dt);
  2. //这里面的一些分支判断用的是vehicle_control_mode_s这个结构体。

do_control调用了

control_non_manual(dt);

control_non_manual调用了

  1. control_offboard(dt);
  2. control_position();

control_offboard用来判断是位置控制还是姿态控制并把控制量获取并发布。
我这里用的是位置和偏航角控制,所以在这个函数中执行的内容有

  1. _pos_sp(0) = _pos_sp_triplet.current.x;
  2. _pos_sp(1) = _pos_sp_triplet.current.y;
  3. _run_pos_control = true;
  4. _hold_offboard_xy = false;
  5. _att_sp.yaw_body = _pos_sp_triplet.current.yaw;

最后发布vehicle_local_position_setpoint这个消息

  1. _local_pos_sp.timestamp = hrt_absolute_time();
  2. _local_pos_sp.x = _pos_sp(0);
  3. _local_pos_sp.y = _pos_sp(1);
  4. _local_pos_sp.z = _pos_sp(2);
  5. _local_pos_sp.yaw = _att_sp.yaw_body;
  6. _local_pos_sp.vx = _vel_sp(0);
  7. _local_pos_sp.vy = _vel_sp(1);
  8. _local_pos_sp.vz = _vel_sp(2);
  9. orb_advertise(ORB_ID(vehicle_local_position_setpoint), &_local_pos_sp);

control_position调用calculate_velocity_setpoint函数和calculate_thrust_setpoint函数,calculate_velocity_setpoint根据位置偏差计算速度偏差,里面代码不长,直接贴出

  1. void MulticopterPositionControl::control_position(float dt)
  2. {
  3. calculate_velocity_setpoint(dt);
  4. if (_control_mode.flag_control_climb_rate_enabled || _control_mode.flag_control_velocity_enabled ||
  5. _control_mode.flag_control_acceleration_enabled)
  6. {
  7. calculate_thrust_setpoint(dt);
  8. } else {
  9. _reset_int_z = true;
  10. }
  11. }

在计算速度设置点函数calculate_velocity_setpoint中:

  1. /* run position & altitude controllers, if enabled (otherwise use already computed velocity setpoints) */
  2. if (_run_pos_control)
  3. {
  4. // If for any reason, we get a NaN position setpoint, we better just stay where we are.
  5. if (PX4_ISFINITE(_pos_sp(0)) && PX4_ISFINITE(_pos_sp(1)))
  6. {
  7. _vel_sp(0) = (_pos_sp(0) - _pos(0)) * _params.pos_p(0);
  8. _vel_sp(1) = (_pos_sp(1) - _pos(1)) * _params.pos_p(1);
  9. } else
  10. {
  11. _vel_sp(0) = 0.0f;
  12. _vel_sp(1) = 0.0f;
  13. warn_rate_limited("Caught invalid pos_sp in x and y");
  14. }
  15. }
  16. limit_altitude();
  17. if (_run_alt_control)
  18. {
  19. if (PX4_ISFINITE(_pos_sp(2)))
  20. {
  21. _vel_sp(2) = (_pos_sp(2) - _pos(2)) * _params.pos_p(2);
  22. } else
  23. {
  24. _vel_sp(2) = 0.0f;
  25. warn_rate_limited("Caught invalid pos_sp in z");
  26. }
  27. }
  28. //后面是对起飞速度的设置和各个方向速度的一个限制。

calculate_thrust_setpoint函数根据速度设置点计算姿态设置点然后发布姿态设置点vehicle_attitude_setpoint。

mc_att_control

再转到姿态控制模块
它订阅了vehicle_control_mode和vehicle_attitude_setpoint这两个消息。

  1. _v_control_mode_sub = orb_subscribe(ORB_ID(vehicle_control_mode));
  2. _v_att_sp_sub = orb_subscribe(ORB_ID(vehicle_attitude_setpoint));

姿态控制模块把姿态设置点转换为需要的角速度。
从位置设置点到最后的三个方向的设置角速度,中间代码仔细分析比较复杂,具体的数学公式整理起来是一个浩大的工程,一个大概的思路是:

这里写图片描述

这个模块把姿态设置点转化为角速率设置点,然后在mixer中控制电机。

声明:本文内容由网友自发贡献,不代表【wpsshop博客】立场,版权归原作者所有,本站不承担相应法律责任。如您发现有侵权的内容,请联系我们。转载请注明出处:https://www.wpsshop.cn/w/不正经/article/detail/729644?site
推荐阅读
相关标签
  

闽ICP备14008679号