多旋翼无人机抛飞实现流程_APM固件

多旋翼无人机抛飞实现流程_APM固件

- 简要介绍了自动抛飞功能的实现方法

调用流程：
ArduCopter.cpp->fast_loop()->update_flight_mode()(fight_mode.cpp)->throw_run

抛飞初始化

当机架构型为直升机或多旋翼机已经arm时,取消初始化。禁止进入抛飞模式
抛飞过程有两个重要内容：1、手持无人机抛飞时，使电机不转（保证安全）
                                        2、无人机处于空中时及时，控制姿态保证不坠机

抛飞run（状态机形式实现抛飞检测及控制）

状态机共包含五种状态：

状态一：Throw_Disarmed - 电机未解锁
进入条件：电机未arm，积分锁开启
状态进程：
设置电机不转动
调用函数：set_desired_spool_state（具体实现方法请查看AP_MotorsMulticopter.cpp）
设置油门数据为0，消除姿态控制的误差并设置姿态控制的积分量为0
调用函数：set_throttle_out_unstabilized->relax_attitude_controllers(AC_AttitudeControl.CPP)

.

状态二：Throw_Detecting - 电机解锁（未需安装）并等待抛飞（抛飞检测）
进入条件：电机已解锁，并当前条件为Throw_Disarmed
状态进程
设置电机不转动
调用函数：set_desired_spool_state（当throw_motor_start变量为0时，此时_spool_state的赋值为DESIRED_SHUT_DOWN）
同状态一
设置waiting_for_throw为true，并提醒用户

.

状态三:Throw_Uprighting - 电机抛飞已经被检测到，并姿态已经保持直立
进入条件：抛飞动作已被检测到，并当前条件为Throw_Detecting
抛飞检测：
该项为本次内容中最为重要的，抛飞检测的调用在状态机if判断中被调用
实现流程：
抛飞状态检测共有四个变量：
（1）high_speed 速度矢量模大于5m/s
（2）free_falling z轴加速度大于-0.25g时，判断处于自由落体状态。但该条件误判率较高
（3）changing_height 判断z轴速度，从而判断是否处于空中状态
（4）no_throw_action 加速度矢量模小于g，判断无人机不处于抛飞状态
结合这四个变量，可判断有较大概率处于抛出的状态
判断条件为（high_speed|free_falling）&changing_height&no_throw_action
当判断条件为真时，将留出500ms为进一步检测抛飞状态：
判断条件为：500ms内z轴速度变化超过：-2.5m/s

至此,如果最终结果为真.那么将进行Throw_Uprighting姿态控制

状态进程：
取消电机量限制
取消姿态误差和积分限制
设置期望姿态角分别为（0，0，0）
设置油门量为0.5

.

状态四：Throw_HgtStabilise - 无人机已经达到预定高度，并保持稳定
进入条件：检查进程：
与状态（3）类似，设置高度控制器达到指定高度

.

状态五：ThrowPosHold - 无人机达到预定位置并保持稳定
进入条件：检查无人机高度误差是否小于0.
5m 且当前状态为Throw_HgtStabilise
状态进程：
位置控制