调用流程: 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 状态进程: 位置控制