From 0acf4903a09a3e9896ca732c3c8fa748b74a49c4 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?=E5=AD=A4=E5=B8=86=E8=BF=9C=E5=BD=B1Faraway?= Date: Sun, 12 Dec 2021 18:12:31 +0800 Subject: [PATCH] =?UTF-8?q?=E8=B5=B7=E9=99=8D=E6=B5=81=E7=A8=8B=E4=BC=98?= =?UTF-8?q?=E5=8C=96=EF=BC=8C=E8=87=AA=E5=8A=A8=E6=A8=A1=E5=BC=8F=E8=B5=B7?= =?UTF-8?q?=E9=A3=9E=E6=82=AC=E5=81=9C=E4=BC=98=E5=8C=96?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- ArduCopter/mode.cpp | 2 + ArduCopter/mode.h | 1 + ArduCopter/mode_auto.cpp | 123 +++++++++++++++++++++++++-------------- ArduCopter/mode_rtl.cpp | 13 +++-- 4 files changed, 91 insertions(+), 48 deletions(-) diff --git a/ArduCopter/mode.cpp b/ArduCopter/mode.cpp index b340a6f116..edc309a7ca 100644 --- a/ArduCopter/mode.cpp +++ b/ArduCopter/mode.cpp @@ -848,6 +848,8 @@ bool Copter::update_updownStatus(const uint8_t status) void Copter::update_updownStatus2(uint8_t status) { copter.updownStatus = (UpDownState)status; + + gcs().send_text(MAV_SEVERITY_INFO, "updownStatus2:%d",copter.updownStatus); if (status == 3) { set_mode(Mode::Number::AUTO, ModeReason::REQUEST_CMD); diff --git a/ArduCopter/mode.h b/ArduCopter/mode.h index d848e3bdff..5bd0af3a63 100644 --- a/ArduCopter/mode.h +++ b/ArduCopter/mode.h @@ -509,6 +509,7 @@ private: uint16_t wp_nav_index; // nav 航点计数 bool first_wp_dec; // nav 航点计数 + uint8_t updown_state; // 起降状态 Location last_loc{}; }; diff --git a/ArduCopter/mode_auto.cpp b/ArduCopter/mode_auto.cpp index 225439676f..6755f1204f 100644 --- a/ArduCopter/mode_auto.cpp +++ b/ArduCopter/mode_auto.cpp @@ -46,7 +46,9 @@ bool ModeAuto::init(bool ignore_checks) // start/resume the mission (based on MIS_RESTART parameter) mission.start_or_resume(); wp_nav_index = 0; // nav 重新开始计数 + updown_state = 0; // 起飞悬停状态清0 first_wp_dec = false; + gcs().send_text(MAV_SEVERITY_INFO, "auto init:up:%d,wp:%d",updown_state,wp_nav_index); return true; } else { return false; @@ -145,8 +147,12 @@ void ModeAuto::takeoff_start(const Location& dest_loc) Location dest(dest_loc); - // gcs().send_text(MAV_SEVERITY_INFO, "NOTICE: dl:%d,%d,%d",dest_loc.lat,dest_loc.lng,dest_loc.alt); - // last_loc = dest_loc; + last_loc = dest_loc; + if (g.zr_tk_req_alt > 0 && updown_state == 0){ + updown_state = 1; + dest.alt = g.zr_tk_req_alt; + gcs().send_text(MAV_SEVERITY_INFO, "updownStatus:%d",copter.updownStatus); + } if (!copter.current_loc.initialised()) { // vehicle doesn't know where it is ATM. We should not // initialise our takeoff destination without knowing this! @@ -227,6 +233,7 @@ void ModeAuto::wp_start(const Location& dest_loc) if (copter.updownStatus < UpDown_InMission) { copter.updownStatus = UpDown_InMission; + gcs().send_text(MAV_SEVERITY_INFO, "updownStatus:%d",copter.updownStatus); gcs().send_message(MSG_ZR_FLYING_STATUS); } } @@ -757,36 +764,53 @@ void ModeAuto::takeoff_run() { if (motors->armed() || copter.ap.auto_armed) { - int32_t alt_rev = 0; - if (copter.current_loc.get_alt_cm(Location::AltFrame::ABOVE_HOME, alt_rev)) + if(g.zr_tk_delay > 0 && g.zr_tk_req_alt > 0 && \ + copter.updownStatus < UpDown_RequestClimb && \ + updown_state == 2) { - if (g.zr_tk_req_alt > 0 && alt_rev >= g.zr_tk_req_alt - - && copter.updownStatus < UpDown_RequestClimb) - { - copter.updownStatus = UpDown_RequestClimb; - countdown = g.zr_tk_delay; - - gcs().send_message(MSG_ZR_FLYING_STATUS); - - last_takeoff_request_time = AP_HAL::millis(); - if(g.zr_tk_delay>0){ - is_takeoff_delay_enable =true; - }else{ - is_takeoff_delay_enable =false; + updown_state = 3; + copter.updownStatus = UpDown_RequestClimb; + gcs().send_text(MAV_SEVERITY_INFO, "updownStatus:%d",copter.updownStatus); + gcs().send_message(MSG_ZR_FLYING_STATUS); + countdown = g.zr_tk_delay; + Vector3f stopping_point; + wp_nav->get_wp_stopping_point(stopping_point); + + // initialise waypoint controller target to stopping point + wp_nav->set_wp_destination(stopping_point); + // hold yaw at current heading + auto_yaw.set_mode(AUTO_YAW_HOLD); + }else if(copter.position_ok() && copter.updownStatus == UpDown_RequestClimb ){ + if ((AP_HAL::millis() - last_takeoff_request_time) > 1000){ + last_takeoff_request_time = AP_HAL::millis(); + if (countdown > 0){ + gcs().send_text(MAV_SEVERITY_INFO, "NOTICE: 继续任务倒计时(秒)：%d", countdown); } - set_mode(Mode::Number::BRAKE, ModeReason::REQUEST_CMD); - } - - else if (g.zr_tk_req_alt > 0 && alt_rev < g.zr_tk_req_alt - && alt_rev > 0 - &&(AP_HAL::millis()-last_time_send_updownload>1000)) - { - last_time_send_updownload = AP_HAL::millis(); - copter.updownStatus = UpDown_TakeOffClimb; - // gcs().send_message(MSG_ZR_FLYING_STATUS); //TODO - } - } + else if (countdown <= 0){ + gcs().send_text(MAV_SEVERITY_INFO, "NOTICE: 执行继续任务"); + copter.updownStatus = UpDown_ContinueClimb; + gcs().send_text(MAV_SEVERITY_INFO, "updownStatus:%d",copter.updownStatus); + gcs().send_message(MSG_ZR_FLYING_STATUS); + updown_state = 4; + takeoff_start(last_loc); + } + countdown--; // TODO Actually 1 second longer + } + // set motors to full range + motors->set_desired_spool_state(AP_Motors::DesiredSpoolState::THROTTLE_UNLIMITED); + + // run waypoint and z-axis position controller + copter.failsafe_terrain_set_status(wp_nav->update_wpnav()); + + pos_control->update_z_controller(); + attitude_control->input_euler_angle_roll_pitch_euler_rate_yaw(wp_nav->get_roll(), wp_nav->get_pitch(), 0); + return; + + }else if(copter.updownStatus > UpDown_RequestClimb){ + updown_state = 4; + // gcs().send_text(MAV_SEVERITY_INFO, "NOTICE: 执行继续任务"); + + } } if(g.zr_8_circle == 1){ const bool reached_wp_dest = copter.wp_nav->reached_wp_destination(); @@ -933,8 +957,8 @@ void ModeAuto::circle_run() if(circle_init_yaw == false){ // 如果需要转向到下一个航点 if (abs(wrap_180_cd(ahrs.yaw_sensor-initial_armed_bearing)) <= 200) { //转到角度 circle_init_yaw = true; - gcs().send_text(MAV_SEVERITY_INFO, "[%d]init ok:%d -- %d, %d",circle_init_yaw,initial_armed_bearing,ahrs.yaw_sensor,abs(wrap_180_cd(ahrs.yaw_sensor-initial_armed_bearing))); - }else{ // 转向 + gcs().send_text(MAV_SEVERITY_INFO, "circ init: y: %d -- %d ",initial_armed_bearing,ahrs.yaw_sensor); + }else{ attitude_control->input_euler_angle_roll_pitch_yaw(wp_nav->get_roll(), wp_nav->get_pitch(), initial_armed_bearing ,true); return; } @@ -1176,6 +1200,9 @@ Location ModeAuto::terrain_adjusted_location(const AP_Mission::Mission_Command& void ModeAuto::do_takeoff(const AP_Mission::Mission_Command& cmd) { // Set wp navigation target to safe altitude above current position + // 如果是Mission Start进入Auto,是先执行这里 ，再执行初始化 + wp_nav_index = 0; // nav 重新开始计数 + updown_state = 0; // 起飞悬停状态清0 takeoff_start(cmd.content.location); } @@ -1650,7 +1677,22 @@ bool ModeAuto::verify_takeoff() }else{ // 常规起飞判断，增加设置绕圈相关初始设置 copter.circle_nav->set_circle_8_finish(false); first_wp_dec = false; - return reached_wp_dest; + if(g.zr_tk_req_alt > 0 ){ + + if(reached_wp_dest && updown_state <2){ + updown_state = 2; + }else if(updown_state >= 4){ + return reached_wp_dest; + } + static uint8_t last_state = 0; + if(last_state != updown_state){ + last_state = updown_state; + } + return false; + }else{ + return reached_wp_dest; + } + } } @@ -1956,23 +1998,18 @@ bool ModeAuto::verify_nav_wp(const AP_Mission::Mission_Command& cmd) // play a tone AP_Notify::events.waypoint_complete = 1; } - wp_nav_index += 1; // nav 计数，用于是否第一个航点绕8字判断 + if(g.zr_8_circle == 2 && !first_wp_dec){ bool cyc_ret = copter.circle_nav->get_circle_8_finish(); if(cyc_ret){ - - // calculate stopping point - Vector3f stopping_point; - wp_nav->get_wp_stopping_point(stopping_point); - // initialise waypoint controller target to stopping point - wp_nav->set_wp_destination(stopping_point); - - gcs().send_text(MAV_SEVERITY_INFO, "Reached command #%i,nav:%d",cmd.index,wp_nav_index); - first_wp_dec = true; + gcs().send_text(MAV_SEVERITY_INFO, "circle ok,Reached command #%i,nav:%d",cmd.index,wp_nav_index); + first_wp_dec = true; + wp_nav_index += 1; // nav 计数 } return cyc_ret; }else{ gcs().send_text(MAV_SEVERITY_INFO, "Reached command #%i,nav:%d",cmd.index,wp_nav_index); + wp_nav_index += 1; // nav 计数 return true; } } diff --git a/ArduCopter/mode_rtl.cpp b/ArduCopter/mode_rtl.cpp index 3efa50b9ef..40ec47051b 100644 --- a/ArduCopter/mode_rtl.cpp +++ b/ArduCopter/mode_rtl.cpp @@ -99,8 +99,9 @@ if(g.zr_8_circle == 2){ if (copter.updownStatus < UpDown_RequestDescent) { copter.updownStatus = UpDown_RequestDescent; - countdown = g.zr_rtl_delay; - gcs().send_message(MSG_ZR_FLYING_STATUS); + countdown = g.zr_rtl_delay; + gcs().send_text(MAV_SEVERITY_INFO, "updownStatus:%d",copter.updownStatus); + gcs().send_message(MSG_ZR_FLYING_STATUS); last_decent_time = AP_HAL::millis(); if(g.zr_rtl_delay>0){ is_rtl_delay_enable = true; @@ -118,7 +119,8 @@ if(g.zr_8_circle == 2){ else if(countdown<=0){ gcs().send_text(MAV_SEVERITY_INFO,"NOTICE: 开始着陆"); copter.updownStatus = UpDown_ContinueDescent; - gcs().send_message(MSG_ZR_FLYING_STATUS); + gcs().send_text(MAV_SEVERITY_INFO, "updownStatus:%d",copter.updownStatus); + gcs().send_message(MSG_ZR_FLYING_STATUS); _state = RTL_Land; } countdown --; //TODO shiji countdown+1 s @@ -326,8 +328,9 @@ void ModeRTL::loiterathome_run() // rtl_descent_start - initialise descent to final alt void ModeRTL::descent_start() { - copter.updownStatus = UpDown_RTLDescent; - gcs().send_message(MSG_ZR_FLYING_STATUS); + copter.updownStatus = UpDown_RTLDescent; + gcs().send_text(MAV_SEVERITY_INFO, "updownStatus:%d",copter.updownStatus); + gcs().send_message(MSG_ZR_FLYING_STATUS); _state = RTL_FinalDescent; _state_complete = false;