arducopter--parachute

先梳理个手动开伞流程：
有两种方法：其一，通过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()怎么调度起来的？