多旋翼无人机抛飞实现流程_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
状态进程:
位置控制

评论
添加红包

请填写红包祝福语或标题

红包个数最小为10个

红包金额最低5元

当前余额3.43前往充值 >
需支付:10.00
成就一亿技术人!
领取后你会自动成为博主和红包主的粉丝 规则
hope_wisdom
发出的红包
实付
使用余额支付
点击重新获取
扫码支付
钱包余额 0

抵扣说明:

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

余额充值