|
|
|
@ -460,15 +460,15 @@ bool AP_Arming_Copter::mandatory_deadline_checks(bool display_failure)
@@ -460,15 +460,15 @@ bool AP_Arming_Copter::mandatory_deadline_checks(bool display_failure)
|
|
|
|
|
return true; |
|
|
|
|
if (!copter.deadline_ok()) |
|
|
|
|
{ |
|
|
|
|
//const AP_AHRS_NavEKF &ahrs = AP::ahrs_navekf();
|
|
|
|
|
// const char *reason = ahrs.prearm_failure_reason();
|
|
|
|
|
int32_t deadline = 0; |
|
|
|
|
copter.get_deadline_params(deadline); |
|
|
|
|
// sprintf(temp,"time is up at %d-%d-%d ", deadline / 10000, (deadline % 10000) / 100, deadline % 100);
|
|
|
|
|
|
|
|
|
|
// reason = "Trial time has expired!";
|
|
|
|
|
gcs().send_text(MAV_SEVERITY_INFO, "无法解锁,无人机授权于%d-%d-%d到期!", deadline / 10000, (deadline % 10000) / 100, deadline % 100); |
|
|
|
|
check_failed(display_failure, "Trial time has expired at %d-%d-%d !", deadline / 10000, (deadline % 10000) / 100, deadline % 100); |
|
|
|
|
if(deadline ==20200101) |
|
|
|
|
{ |
|
|
|
|
check_failed(true, "无法解锁,授权码错误!"); |
|
|
|
|
}else{
|
|
|
|
|
check_failed(true, "无法解锁,授权于%d-%d-%d到期!", deadline / 10000, (deadline % 10000) / 100, deadline % 100); |
|
|
|
|
} |
|
|
|
|
// check_failed(display_failure, "Trial time has expired at %d-%d-%d !", deadline / 10000, (deadline % 10000) / 100, deadline % 100);
|
|
|
|
|
return false; |
|
|
|
|
}else{ |
|
|
|
|
return true; |
|
|
|
@ -500,11 +500,11 @@ bool AP_Arming_Copter::mandatory_gps_checks(bool display_failure)
@@ -500,11 +500,11 @@ bool AP_Arming_Copter::mandatory_gps_checks(bool display_failure)
|
|
|
|
|
#endif |
|
|
|
|
|
|
|
|
|
// return true if GPS is not required
|
|
|
|
|
if (!mode_requires_gps && !fence_requires_gps) |
|
|
|
|
{ |
|
|
|
|
return mandatory_deadline_checks(display_failure); |
|
|
|
|
// return true;
|
|
|
|
|
} |
|
|
|
|
// if (!mode_requires_gps && !fence_requires_gps)
|
|
|
|
|
// {
|
|
|
|
|
// return mandatory_deadline_checks(display_failure);
|
|
|
|
|
// // return true;
|
|
|
|
|
// }
|
|
|
|
|
|
|
|
|
|
// ensure GPS is ok
|
|
|
|
|
if (!copter.position_ok()) { |
|
|
|
@ -520,23 +520,10 @@ bool AP_Arming_Copter::mandatory_gps_checks(bool display_failure)
@@ -520,23 +520,10 @@ bool AP_Arming_Copter::mandatory_gps_checks(bool display_failure)
|
|
|
|
|
check_failed(display_failure, "%s", reason); |
|
|
|
|
return false; |
|
|
|
|
} |
|
|
|
|
else |
|
|
|
|
{ |
|
|
|
|
return mandatory_deadline_checks(display_failure); |
|
|
|
|
// if (!copter.deadline_ok())
|
|
|
|
|
// {
|
|
|
|
|
// const char *reason = ahrs.prearm_failure_reason();
|
|
|
|
|
// //char * temp =nullptr;
|
|
|
|
|
// int32_t deadline = 0;
|
|
|
|
|
// copter.get_deadline_params(deadline);
|
|
|
|
|
// // sprintf(temp,"time is up at %d-%d-%d ", deadline / 10000, (deadline % 10000) / 100, deadline % 100);
|
|
|
|
|
|
|
|
|
|
// reason ="Trial time has expired!";
|
|
|
|
|
// // gcs().send_text(MAV_SEVERITY_INFO, "无法解锁,您的无人机授权已于%d-%d-%d到期", deadline / 10000, (deadline % 10000) / 100, deadline % 100);
|
|
|
|
|
// check_failed(display_failure, "%s", reason);
|
|
|
|
|
// return false;
|
|
|
|
|
// }
|
|
|
|
|
} |
|
|
|
|
// else
|
|
|
|
|
// {
|
|
|
|
|
// return mandatory_deadline_checks(display_failure);
|
|
|
|
|
// }
|
|
|
|
|
|
|
|
|
|
// check for GPS glitch (as reported by EKF)
|
|
|
|
|
nav_filter_status filt_status; |
|
|
|
@ -593,6 +580,13 @@ bool AP_Arming_Copter::arm_checks(AP_Arming::Method method)
@@ -593,6 +580,13 @@ bool AP_Arming_Copter::arm_checks(AP_Arming::Method method)
|
|
|
|
|
} |
|
|
|
|
#endif |
|
|
|
|
|
|
|
|
|
//arm if Trial time is not arrived add@bnsir
|
|
|
|
|
|
|
|
|
|
if( !mandatory_deadline_checks(false)){ |
|
|
|
|
return false; |
|
|
|
|
} |
|
|
|
|
|
|
|
|
|
|
|
|
|
|
Mode::Number control_mode = copter.control_mode; |
|
|
|
|
|
|
|
|
|
// always check if the current mode allows arming
|
|
|
|
@ -832,7 +826,6 @@ bool AP_Arming_Copter::disarm()
@@ -832,7 +826,6 @@ bool AP_Arming_Copter::disarm()
|
|
|
|
|
#endif |
|
|
|
|
|
|
|
|
|
AP_AHRS_NavEKF &ahrs = AP::ahrs_navekf(); |
|
|
|
|
|
|
|
|
|
// save compass offsets learned by the EKF if enabled
|
|
|
|
|
Compass &compass = AP::compass(); |
|
|
|
|
if (ahrs.use_compass() && compass.get_learn_type() == Compass::LEARN_EKF) { |
|
|
|
|