前言
CSDNhttps://mp.csdn.net/mp_blog/creation/editor/122115245
这边说的解锁是指的非自动解锁和地面站解锁
常规解锁流程=安全开关->内八解锁
这一篇会介绍整个解锁流程以及飞控内部解锁包含了那些内容
目录
是否允许使用舵面解锁
判断油门是否关闭
获取舵量.航向
主体内容
主题内容中关注的函数
1.motors->armed()
2.arming.arm(AP_Arming::Method::RUDDER)
打开libraries\AP_Arming\AP_Arming.cpp文件中的arm函数
((!do_arming_checks && mandatory_checks(true)) || (pre_arm_checks(true)&& arm_checks(method)))
pre_arm_checks(true)
ArduCopter\motors.cpp
飞控的解锁的文件就在这里
命名为arm_motors_check()函数
内容包含非常多 所以就不一一列出了 我按顺序讲
可以看到这个函数在主文件里开辟了进程
然后我们直接来看这个函数的流程
是否允许使用舵面解锁
AP_Arming::RudderArming arming_rudder = arming.get_rudder_arming_type();
if (arming_rudder == AP_Arming::RudderArming::IS_DISABLED) {
return; }
判断油门是否关闭
if (channel_throttle->get_control_in() > 0) {
arming_counter = 0;
return;
}
获取舵量.航向
int16_t yaw_in = channel_yaw->get_control_in();
主体内容
if (yaw_in > 4000) {
// increase the arming counter to a maximum of 1 beyond the auto trim counter
//最大值1 如果小于就等于1
if (arming_counter <= AUTO_TRIM_DELAY) {
arming_counter++;
}
// arm the motors and configure for flight
//准备发动机准备起飞
if (arming_counter == ARM_DELAY && !motors->armed()) {
// reset arming counter if arming fail
//解除失败就重新解除
//关键解锁函数arming.arm(AP_Arming::Method::RUDDER)
if (!arming.arm(AP_Arming::Method::RUDDER)) {
arming_counter = 0;
}
}
// arm the motors and configure for flight
//启动发动机，准备起飞
if (arming_counter == AUTO_TRIM_DELAY && motors->armed() && control_mode == Mode::Number::STABILIZE) {
auto_trim_counter = 250;
// ensure auto-disarm doesn't trigger immediately
//确保不会第一时间
auto_disarm_begin = millis();
}
// full left and rudder disarming is enabled
//左满舵解除武装
} else if ((yaw_in < -4000) && (arming_rudder == AP_Arming::RudderArming::ARMDISARM)) {
if (!flightmode->has_manual_throttle() && !ap.land_complete) {
arming_counter = 0;
return;
}
// increase the counter to a maximum of 1 beyond the disarm delay
if (arming_counter <= DISARM_DELAY) {
arming_counter++;
}
// disarm the motors
//解除引擎
if (arming_counter == DISARM_DELAY && motors->armed()) {
//解锁函数
arming.disarm();
}
// Yaw is centered so reset arming counter
} else {
arming_counter = 0;
}
}
主题内容中关注的函数
arming.arm(AP_Arming::Method::RUDDER)
注意这里要关注的函数是ArduCopter\motors.cpp中的:
arming.arm(AP_Arming::Method::RUDDER)
而这个函数是在同目录下的ArduCopter\AP_Arming.cpp文件中
这里一定要注意千万不要弄混了
我们跳转到这个函数里面,注意这个函数是在同目录下的ArduCopter\AP_Arming.cpp文件中
bool AP_Arming_Copter::arm(const AP_Arming::Method method, const bool do_arming_checks)
{
static bool in_arm_motors = false;
// exit immediately if already in this function
//如果已经在函数中
if (in_arm_motors) {
return false;
}
in_arm_motors = true;
// return true if already armed
//如果已经启动
if (copter.motors->armed()) {
in_arm_motors = false;
return true;
}
//AP_Arming::arm 是整个飞机的模式和防护 如果成功返回t
//这个很重要 这里面包括了里面的检查项目 包含了pre_arm_checks(true)
if (!AP_Arming::arm(method, do_arming_checks)) {
AP_Notify::events.arming_failed = true;
in_arm_motors = false;
return false;
}
// let logger know that we're armed (it may open logs e.g.)
AP::logger().set_vehicle_armed(true);
// disable cpu failsafe because initialising everything takes a while
copter.failsafe_disable();
// notify that arming will occur (we do this early to give plenty of warning)
AP_Notify::flags.armed = true;
// call notify update a few times to ensure the message gets out
for (uint8_t i=0; i<=10; i++) {
AP::notify().update();
}
#if HIL_MODE != HIL_MODE_DISABLED || CONFIG_HAL_BOARD == HAL_BOARD_SITL
gcs().send_text(MAV_SEVERITY_INFO, "Arming motors");
#endif
// Remember Orientation
// --------------------
copter.init_simple_bearing();
AP_AHRS_NavEKF &ahrs = AP::ahrs_navekf();
copter.initial_armed_bearing = ahrs.yaw_sensor;
if (!ahrs.home_is_set()) {
// Reset EKF altitude if home hasn't been set yet (we use EKF altitude as substitute for alt above home)
//home 点设置
ahrs.resetHeightDatum();
AP::logger().Write_Event(DATA_EKF_ALT_RESET);
// we have reset height, so arming height is zero
copter.arming_altitude_m = 0;
} else if (!ahrs.home_is_locked()) {
// Reset home position if it has already been set before (but not locked)
//重置初始位置（如果之前已设置）（但未锁定）
if (!copter.set_home_to_current_location(false)) {
// ignore failure
}
// remember the height when we armed
//记住我们武装时的高度
copter.arming_altitude_m = copter.inertial_nav.get_altitude() * 0.01;
}
////更新超级简单轴承-根据位置调整简单轴承
//应在home_轴承更新后调用
copter.update_super_simple_bearing(false);
// Reset SmartRTL return location. If activated, SmartRTL will ultimately try to land at this point
#if MODE_SMARTRTL_ENABLED == ENABLED
copter.g2.smart_rtl.set_home(copter.position_ok());
#endif
// enable gps velocity based centrefugal force compensation
ahrs.set_correct_centrifugal(true);
hal.util->set_soft_armed(true);
#if SPRAYER_ENABLED == ENABLED
// turn off sprayer's test if on
copter.sprayer.test_pump(false);
#endif
// enable output to motors
//启用电机的输出
copter.enable_motor_output();
// finally actually arm the motors
//启动发动机
copter.motors->armed(true);
AP::logger().Write_Event(DATA_ARMED);
// log flight mode in case it was changed while vehicle was disarmed
AP::logger().Write_Mode((uint8_t)copter.control_mode, copter.control_mode_reason);
// re-enable failsafe
copter.failsafe_enable();
// perf monitor ignores delay due to arming
AP::scheduler().perf_info.ignore_this_loop();
// flag exiting this function
in_arm_motors = false;
// Log time stamp of arming event
copter.arm_time_ms = millis();
// Start the arming delay
copter.ap.in_arming_delay = true;
// assumed armed without a arming, switch. Overridden in switches.cpp
copter.ap.armed_with_switch = false;
// return success
return true;
}
1.motors->armed()
检查当前是否解锁 bool类型 1解锁 0未解锁 注意这个函数是个重载函数里面是可以包含值的
if (copter.motors->armed()) {
in_arm_motors = false;
return true;
}
2.arming.arm(AP_Arming::Method::RUDDER)
这个是重点关注的函数 也就是解锁的函数 他很重要,在libraries\AP_Arming\AP_Arming.cpp里面定义了一个一样函数名的函数 这点要注意两个函数的功能不同
接下来不重要的部分我直接吧作用放在注释里
//如果已经在函数中
if (in_arm_motors) {
return false;
}
//如果已经启动
if (copter.motors->armed()) {
in_arm_motors = false;
return true;
}
注意这里到了前面说的嵌套AP_Arming::arm(method, do_arming_checks)
这个函数的主题在libraries\AP_Arming\AP_Arming.cpp文件中
if (!AP_Arming::arm(method, do_arming_checks)) {
AP_Notify::events.arming_failed = true;
in_arm_motors = false;
return false;
}
注意在这里我们跳转到 libraries\AP_Arming\AP_Arming.cpp中的
bool AP_Arming::arm(AP_Arming::Method method, const bool do_arming_checks)
前一个同名函数是为了解锁
后者则是为了检查当前飞机的状态然后更改电机启动标志位
打开libraries\AP_Arming\AP_Arming.cpp文件中的arm函数
bool AP_Arming::arm(AP_Arming::Method method, const bool do_arming_checks)
{
if (armed) { //already armed
return false;
}
if ((!do_arming_checks && mandatory_checks(true)) || (pre_arm_checks(true) && arm_checks(method))) {
armed = true;
//TODO: Log motor arming
//Can't do this from this class until there is a unified logging library
} else {
//解除失败日志通知
AP::logger().arming_failure();
armed = false;
}
return armed;
}
其中核心就是
((!do_arming_checks && mandatory_checks(true)) || (pre_arm_checks(true)&& arm_checks(method)))
pre_arm_checks(true)
飞行前的检查
bool AP_Arming_Copter::pre_arm_checks(bool display_failure)
{
const bool passed = run_pre_arm_checks(display_failure);
set_pre_arm_check(passed);
return passed;
}
我们要重点关注的是
run_pre_arm_checks(display_failure)
bool AP_Arming_Copter::run_pre_arm_checks(bool display_failure)
{
// exit immediately if already armed
if (copter.motors->armed()) {
return true;
}
// check if motor interlock and Emergency Stop aux switches are used
// at the same time. This cannot be allowed.
if (rc().find_channel_for_option(RC_Channel::AUX_FUNC::MOTOR_INTERLOCK) &&
rc().find_channel_for_option(RC_Channel::AUX_FUNC::MOTOR_ESTOP)){
check_failed(display_failure, "Interlock/E-Stop Conflict");
return false;
}
// check if motor interlock aux switch is in use
// if it is, switch needs to be in disabled position to arm
// otherwise exit immediately. This check to be repeated,
// as state can change at any time.
if (copter.ap.using_interlock && copter.ap.motor_interlock_switch) {
check_failed(display_failure, "Motor Interlock Enabled");
}
// if pre arm checks are disabled run only the mandatory checks
if (checks_to_perform == 0) {
return mandatory_checks(display_failure);
}
return fence_checks(display_failure) //围栏
& parameter_checks(display_failure) //参数
& motor_checks(display_failure) //电机
& pilot_throttle_checks(display_failure) //油门
& oa_checks(display_failure)
& gcs_failsafe_check(display_failure) & //地面站故障保护
AP_Arming::pre_arm_checks(display_failure);
}
最重要的部分这边我都已经打上了注释如果感兴趣可以自己去查看这边关注一下油门
bool AP_Arming_Copter::pilot_throttle_checks(bool display_failure)如下:
bool AP_Arming_Copter::pilot_throttle_checks(bool display_failure)
{
// check throttle is above failsafe throttle
// this is near the bottom to allow other failures to be displayed before checking pilot throttle
//检查油门是否高于故障保护油门
//这靠近底部，以便在检查先导油门之前显示其他故障
if ((checks_to_perform == ARMING_CHECK_ALL) || (checks_to_perform & ARMING_CHECK_RC)) {
if (copter.g.failsafe_throttle != FS_THR_DISABLED && copter.channel_throttle->get_radio_in() < copter.g.failsafe_throttle_value) {
#if FRAME_CONFIG == HELI_FRAME
const char *failmsg = "Collective below Failsafe";
#else
const char *failmsg = "Throttle below Failsafe";
#endif
check_failed(ARMING_CHECK_RC, display_failure, "%s", failmsg);
return false;
}
}
return true;
}
大概的意思就是查看标志位是否有其他故障 然后检查油门位置 这个函数会决定是否能在未开启遥控器的情况下解锁