arducopter--parachute

28 篇文章 2 订阅
22 篇文章 0 订阅

先梳理个手动开伞流程:
有两种方法:其一,通过mavlink控制,这种方式比较安全,可以在地面站上做保护提示:
handleMessage函数中包含解降落伞的case:
在这里插入图片描述
* 在传过来 MAVLINK_MSG_ID_COMMAND_LONG (76) 包中,识别command,为CMD 208为开伞,自动起飞如下在这里插入图片描述
enable parachute后,先传param1 为0 ,在开伞时选择2开伞。
在这里插入图片描述
方法二:
通过映射通道,通过高低电平控制开伞:
在这里插入图片描述
parachute_manual_release()函数,enable()得打开,着落地不开伞、高度小于10米不开,触发parachute_release()函数。
在这里插入图片描述
init_disarm_motors()来上锁电机,release()来开伞
在这里插入图片描述
在这里插入图片描述
release()之后,会赋值三个变量,update()就能搞起来
在这里插入图片描述
调用parachute()的地方:
在这里插入图片描述

  • 在参数中使能parachute,然后就能手动控制降落伞。
  • 若没解锁则退出并初始化control_loss_count = 0;
  • 若是在ACRO或者FLIP模式则退出并初始化control_loss_count = 0;
  • 若是在地面上,land_complete 则退出并且初始化cotrol_loss_count = 0;
  • 若是control_loss_count = 0 且 当前高度小于最小高度则立刻退出;(检测是否是在降落)
  • 若是角度误差小于30度则退出;
  • 然后当误差角度大于30度,开始增加control_loss_count ++,若是误差大于30度超过1s则开伞。
    这里的误差角度如何计算?
// 返回目标推力矢量和当前推力矢量之间的角度。
float get_att_error_angle_deg() const { return degrees(_thrust_error_angle); }

// thrust_heading_rotation_angles - calculates two ordered rotations to move the att_from_quat quaternion to the att_to_quat quaternion.
// 第一个旋转校正油门矢量,第二个旋转校正航向矢量。
void AC_AttitudeControl::thrust_heading_rotation_angles(Quaternion& att_to_quat, const Quaternion& att_from_quat, Vector3f& att_diff_angle, float& thrust_vec_dot)
{
    Matrix3f att_to_rot_matrix; // rotation from the target body frame to the inertial frame.
    att_to_quat.rotation_matrix(att_to_rot_matrix);
    Vector3f att_to_thrust_vec = att_to_rot_matrix*Vector3f(0.0f,0.0f,1.0f);

    Matrix3f att_from_rot_matrix; // rotation from the current body frame to the inertial frame.
    att_from_quat.rotation_matrix(att_from_rot_matrix);
    Vector3f att_from_thrust_vec = att_from_rot_matrix*Vector3f(0.0f,0.0f,1.0f);

    // the cross product of the desired and target thrust vector defines the rotation vector
    Vector3f thrust_vec_cross = att_from_thrust_vec % att_to_thrust_vec;

    // the dot product is used to calculate the angle between the target and desired thrust vectors
    thrust_vec_dot = acosf(constrain_float(att_from_thrust_vec * att_to_thrust_vec,-1.0f,1.0f));

    // Normalize the thrust rotation vector
    float thrust_vector_length = thrust_vec_cross.length();
    if(is_zero(thrust_vector_length) || is_zero(thrust_vec_dot)){
        thrust_vec_cross = Vector3f(0,0,1);
        thrust_vec_dot = 0.0f;
    }else{
        thrust_vec_cross /= thrust_vector_length;
    }
    Quaternion thrust_vec_correction_quat;
    thrust_vec_correction_quat.from_axis_angle(thrust_vec_cross, thrust_vec_dot);

    // Rotate thrust_vec_correction_quat to the att_from frame
    thrust_vec_correction_quat = att_from_quat.inverse()*thrust_vec_correction_quat*att_from_quat;

    // calculate the remaining rotation required after thrust vector is rotated transformed to the att_from frame
    Quaternion yaw_vec_correction_quat = thrust_vec_correction_quat.inverse()*att_from_quat.inverse()*att_to_quat;

    // calculate the angle error in x and y.
    Vector3f rotation;
    thrust_vec_correction_quat.to_axis_angle(rotation);
    att_diff_angle.x = rotation.x;
    att_diff_angle.y = rotation.y;

    // calculate the angle error in z (x and y should be zero here).
    yaw_vec_correction_quat.to_axis_angle(rotation);
    att_diff_angle.z = rotation.z;

    // Todo: Limit roll an pitch error based on output saturation and maximum error.

    // Limit Yaw Error based on maximum acceleration - Update to include output saturation and maximum error.
    // Currently the limit is based on the maximum acceleration using the linear part of the SQRT controller.
    // This should be updated to be based on an angle limit, saturation, or unlimited based on user defined parameters.
    if(!is_zero(_p_angle_yaw.kP()) && fabsf(att_diff_angle.z) > AC_ATTITUDE_ACCEL_Y_CONTROLLER_MAX_RADSS/_p_angle_yaw.kP()){
        att_diff_angle.z = constrain_float(wrap_PI(att_diff_angle.z), -AC_ATTITUDE_ACCEL_Y_CONTROLLER_MAX_RADSS/_p_angle_yaw.kP(), AC_ATTITUDE_ACCEL_Y_CONTROLLER_MAX_RADSS/_p_angle_yaw.kP());
        yaw_vec_correction_quat.from_axis_angle(Vector3f(0.0f,0.0f,att_diff_angle.z));
        att_to_quat = att_from_quat*thrust_vec_correction_quat*yaw_vec_correction_quat;
    }
}

  • 额外加入检测自由落体状态控制,检测1.5s:
    static int32_t parachcount;
    Vector3f AccVec0 = ins.get_accel(0);
    Vector3f AccVec1 = ins.get_accel(1);
    if ((AccVec0.length() < 4.9f)&&(AccVec1.length() < 4.9f)) {
        parachcount ++;
    }else{
        parachcount--;
    }
    parachcount = constrain_int32(parachcount, 0, 5000);
    if(parachcount > 600 ){
    parachute_release();
    }

调用parachute_check()的地方:
在这里插入图片描述
在400hz的fast_loop()里:
在这里插入图片描述
fast_loop()怎么调度起来的?
在这里插入图片描述

  • 0
    点赞
  • 2
    收藏
    觉得还不错? 一键收藏
  • 打赏
    打赏
  • 0
    评论
以下是利用四阶龙格库塔法在 Matlab 中进行数值仿真,得到关于飞机开伞后稳定下降的速度与开伞时海拔高度的关系曲线的仿真代码: ```matlab % 飞机伞降回收模型 % 作者:AI智能创作助手 % 日期:2021年6月30日 clc; clear; % 常数定义 g = 9.81; % 重力加速度,单位:m/s^2 m = 1000; % 飞机质量,单位:kg S = 50; % 机翼面积,单位:m^2 Cd = 0.3; % 阻力系数 rho = 1.225; % 空气密度,单位:kg/m^3 k = 0.5 * Cd * S * rho; % 阻力系数 % 初始条件 h0 = 5000; % 初始高度,单位:m v0 = 200; % 初始速度,单位:m/s t0 = 0; % 初始时间,单位:s dt = 0.1; % 时间步长,单位:s tf = 300; % 模拟时间,单位:s % 飞机运动方程 f = @(t, Y) [Y(4); Y(5); Y(6); -k/m * Y(5)^2 - g; k/m * Y(5)*Y(4); 0]; % 飞机伞降回收模型 h_open = 1000; % 开伞高度,单位:m Cd_parachute = 1.75; % 伞降伞阻力系数 k_parachute = 0.5 * Cd_parachute * S; % 伞降伞阻力系数 f_parachute = @(t, Y) [Y(4); Y(5); Y(6); -k/m * Y(5)^2 - k_parachute/m * Y(5)*abs(Y(5)) - g; k/m * Y(5)*Y(4); 0]; options = odeset('Events', @parachute_event); % 仿真计算 Y0 = [0; 0; h0; v0*cosd(45); 0; v0*sind(45)]; % 初始状态向量 T = t0:dt:tf; Y = zeros(length(T), length(Y0)); Y(1, :) = Y0; for i = 2:length(T) if Y(i-1, 3) >= h_open [T_temp, Y_temp] = ode45(f, [T(i-1), T(i)], Y(i-1,:), options); else [T_temp, Y_temp] = ode45(f_parachute, [T(i-1), T(i)], Y(i-1,:), options); end Y(i, :) = Y_temp(end, :); end % 绘制结果 figure; plot(Y(:, 3), Y(:, 5)); xlabel('海拔高度,单位:m'); ylabel('速度,单位:m/s'); title('飞机开伞后稳定下降的速度与开伞时海拔高度的关系曲线'); % 事件函数,当高度小于开伞高度时触发 function [value, isterminal, direction] = parachute_event(t, Y) h_open = 1000; value = Y(3) - h_open; isterminal = 1; direction = -1; end ``` 在仿真结果中,横轴为海拔高度,单位为米,纵轴为速度,单位为米/秒。通过该仿真代码,可以得到飞机开伞后稳定下降的速度与开伞时海拔高度的关系曲线。

“相关推荐”对你有帮助么?

  • 非常没帮助
  • 没帮助
  • 一般
  • 有帮助
  • 非常有帮助
提交
评论
添加红包

请填写红包祝福语或标题

红包个数最小为10个

红包金额最低5元

当前余额3.43前往充值 >
需支付:10.00
成就一亿技术人!
领取后你会自动成为博主和红包主的粉丝 规则
hope_wisdom
发出的红包

打赏作者

Gkbytes

你的鼓励将是我创作的最大动力

¥1 ¥2 ¥4 ¥6 ¥10 ¥20
扫码支付:¥1
获取中
扫码支付

您的余额不足,请更换扫码支付或充值

打赏作者

实付
使用余额支付
点击重新获取
扫码支付
钱包余额 0

抵扣说明:

1.余额是钱包充值的虚拟货币,按照1:1的比例进行支付金额的抵扣。
2.余额无法直接购买下载,可以购买VIP、付费专栏及课程。

余额充值