diff --git a/ArduCopter/AP_Arming.cpp b/ArduCopter/AP_Arming.cpp index 48507498f1..5b1f66e697 100644 --- a/ArduCopter/AP_Arming.cpp +++ b/ArduCopter/AP_Arming.cpp @@ -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) #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) 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) } #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() #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) { diff --git a/ArduCopter/Parameters.cpp b/ArduCopter/Parameters.cpp index 63c10b6df3..d4265a1ded 100644 --- a/ArduCopter/Parameters.cpp +++ b/ArduCopter/Parameters.cpp @@ -1629,16 +1629,16 @@ const char* Copter::get_sysid_board_id(void) int32_t nameValue1 =(int32_t) g.sysid_board_name_1st; int32_t nameValue2 = (int32_t)g.sysid_board_name_2nd; - char name[6] ; - // memset(name,0,6); + char name[7] ; + memset(name,0,7); //nameValue2 = nameValue2 & 0xffffff; - // name[5] = "a"; + name[5] = '.'; name[4] = nameValue2&0xFF; name[3] = nameValue2>>8&0xFF; name[2] = nameValue2>>16& 0xFF; name[1] = nameValue1&0xFF; name[0] = nameValue1>>8 & 0xFF; - snprintf(buf, sizeof(buf), "Version: zr-v4.0 ,Board ID: ZRZK.%s.%d",name,(int)g.sysid_board_id); + snprintf(buf, sizeof(buf), "Version: zr-v4.0 ,Board ID: ZRZK.%5s%d",name,(int)g.sysid_board_id); return buf; } diff --git a/ArduCopter/system.cpp b/ArduCopter/system.cpp index a63fdfd527..5bf942c654 100644 --- a/ArduCopter/system.cpp +++ b/ArduCopter/system.cpp @@ -340,7 +340,7 @@ bool Copter::deadline_ok() copter.get_deadline_params(deadline); int32_t gps_date =0; AP::gps().get_gps_headline_value(gps_date); - if ((deadline - gps_date) > 0) + if ((deadline - gps_date) >= 0) { return true; } diff --git a/libraries/GCS_MAVLink/GCS_Common.cpp b/libraries/GCS_MAVLink/GCS_Common.cpp index 15e1dd6529..69c56b0f05 100644 --- a/libraries/GCS_MAVLink/GCS_Common.cpp +++ b/libraries/GCS_MAVLink/GCS_Common.cpp @@ -3255,6 +3255,7 @@ void GCS_MAVLINK::handle_common_mission_message(const mavlink_message_t &msg) void GCS_MAVLINK::handle_send_autopilot_version(const mavlink_message_t &msg) { send_message(MSG_AUTOPILOT_VERSION); + send_banner(); } void GCS_MAVLINK::send_banner()