From 39022f59ea91474867124e5d3d838b3ac5f4511b Mon Sep 17 00:00:00 2001 From: zbr Date: Mon, 20 Dec 2021 18:06:47 +0800 Subject: [PATCH] =?UTF-8?q?=E7=AC=AC=E4=B8=80=E4=B8=AA=E7=82=B9=E4=B8=8D?= =?UTF-8?q?=E7=BB=95=E5=9C=88=E9=97=AE=E9=A2=98?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- ArduCopter/mode_auto.cpp | 168 +++++++++++++++++++++------------------ ArduCopter/mode_rtl.cpp | 9 ++- ArduCopter/version.h | 2 +- d100h.sh | 2 +- 4 files changed, 98 insertions(+), 83 deletions(-) diff --git a/ArduCopter/mode_auto.cpp b/ArduCopter/mode_auto.cpp index 5c79ec56a4..e7a068b151 100644 --- a/ArduCopter/mode_auto.cpp +++ b/ArduCopter/mode_auto.cpp @@ -47,7 +47,6 @@ bool ModeAuto::init(bool ignore_checks) mission.start_or_resume(); wp_nav_index = 0; // nav 重新开始计数 updown_state = 0; // 起飞悬停状态清0 - // gcs().send_text(MAV_SEVERITY_INFO, "auto init:up:%d,wp:%d",updown_state,wp_nav_index); return true; } else { return false; @@ -150,8 +149,7 @@ void ModeAuto::takeoff_start(const Location& dest_loc) if (g.zr_tk_delay > 0 && 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); - copter.Log_Write_Data(DATA_NOT_BOTTOMED, (uint16_t)copter.updownStatus); + copter.Log_Write_Data(DATA_UPDOWN_STATUS, (uint16_t)copter.updownStatus); } if (!copter.current_loc.initialised()) { // vehicle doesn't know where it is ATM. We should not @@ -233,7 +231,6 @@ 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); copter.Log_Write_Data(DATA_UPDOWN_STATUS, (uint16_t)copter.updownStatus); gcs().send_message(MSG_ZR_FLYING_STATUS); } @@ -445,7 +442,7 @@ bool ModeAuto::start_command(const AP_Mission::Mission_Command& cmd) break; case MAV_CMD_NAV_LOITER_TURNS: //18 Loiter N Times - // do_roi(cmd); + // do_roi(cmd); // 绕圈指向中间 do_circle(cmd); break; @@ -770,7 +767,6 @@ void ModeAuto::takeoff_run() { updown_state = 3; // 到达悬停高度，状态更新到等待 copter.updownStatus = UpDown_RequestClimb; - gcs().send_text(MAV_SEVERITY_INFO, "updownStatus:%d",copter.updownStatus); copter.Log_Write_Data(DATA_UPDOWN_STATUS, (uint16_t)copter.updownStatus); gcs().send_message(MSG_ZR_FLYING_STATUS); countdown = g.zr_tk_delay; @@ -785,7 +781,6 @@ void ModeAuto::takeoff_run() 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); copter.Log_Write_Data(DATA_UPDOWN_STATUS, (uint16_t)copter.updownStatus); gcs().send_message(MSG_ZR_FLYING_STATUS); updown_state = 4; @@ -806,7 +801,6 @@ void ModeAuto::takeoff_run() }else if(copter.updownStatus > UpDown_RequestClimb){ if(updown_state < 4){ // 地面站弹框确定，发送新的值，状态位同步更新 updown_state = 4; - gcs().send_text(MAV_SEVERITY_INFO, "gcs ctl updownStatus:%d",copter.updownStatus); takeoff_start(last_loc); } } @@ -819,6 +813,55 @@ void ModeAuto::takeoff_run() // called by auto_run at 100hz or more void ModeAuto::wp_run() { + // 到航点绕圈 + if(g.zr_8_circle == 2 && wp_nav_index == 0){ + const bool reached_wp_dest = copter.wp_nav->reached_wp_destination(); + static uint32_t last_100ms; + uint32_t tnow = AP_HAL::millis(); + if (tnow - last_100ms > 100) { + Vector3f stopping_point; + last_100ms = tnow; + wp_nav->get_wp_stopping_point(stopping_point); + + AP::logger().Write("ATES", "TimeUS,Indx,Rech,IniY,Ymod,px,py,pz", "QHBBBfff", + AP_HAL::micros64(), + wp_nav_index, + reached_wp_dest, + circle_init_yaw, + auto_yaw.mode(), + stopping_point.x, + stopping_point.y, + stopping_point.z ); + } + if(reached_wp_dest){ + static autopilot_yaw_mode last_yaw_mode; + if(circle_init_yaw == false){ // 如果需要转向到下一个航点 + if (abs(wrap_180_cd(ahrs.yaw_sensor-wp_yaw_cd)) <= 200) { //转到角度 + circle_init_yaw = true; + auto_yaw.set_mode(last_yaw_mode); + copter.circle_nav->init(); + gcs().send_text(MAV_SEVERITY_INFO,"Auto run circle"); + }else{ + // wp_nav->set_wp_destination(last_loc);// 设置第一个WayPoint点为期望值 + if (auto_yaw.mode() != AUTO_YAW_FIXED) { // 需要转角度，yaw mode使用AUTO_YAW_FIXED + // wp_nav->set_wp_destination(last_loc);// 设置第一个WayPoint点为期望值 + last_yaw_mode = auto_yaw.mode(); + auto_yaw.set_mode(AUTO_YAW_FIXED); + } + attitude_control->input_euler_angle_roll_pitch_yaw(wp_nav->get_roll(), wp_nav->get_pitch(), wp_yaw_cd,true); + } + return; + } + } + if(circle_init_yaw) + { // 如果需要转向到下一个航点 + copter.circle_nav->turn_2_circle(); + pos_control->update_z_controller(); + // Yaw 由 circle决定 + attitude_control->input_euler_angle_roll_pitch_yaw(copter.circle_nav->get_roll(), copter.circle_nav->get_pitch(), copter.circle_nav->get_yaw(), true); + return; + } + } // process pilot's yaw input float target_yaw_rate = 0; if (!copter.failsafe.radio) { @@ -845,39 +888,6 @@ void ModeAuto::wp_run() // call z-axis position controller (wpnav should have already updated it's alt target) pos_control->update_z_controller(); - - // 到航点绕圈 - if(g.zr_8_circle == 2 && wp_nav_index == 0){ - const bool reached_wp_dest = copter.wp_nav->reached_wp_destination(); - if(reached_wp_dest){ - static autopilot_yaw_mode last_yaw_mode; - if(circle_init_yaw == false){ // 如果需要转向到下一个航点 - if (abs(wrap_180_cd(ahrs.yaw_sensor-wp_yaw_cd)) <= 200) { //转到角度 - circle_init_yaw = true; - auto_yaw.set_mode(last_yaw_mode); - copter.circle_nav->init(); - gcs().send_text(MAV_SEVERITY_INFO,"Auto run circle"); - // gcs().send_text(MAV_SEVERITY_INFO, "circ init: y: %d -- %d ",wp_yaw_cd,ahrs.yaw_sensor); - }else{ - if (auto_yaw.mode() != AUTO_YAW_FIXED) { // 需要转角度，yaw mode使用AUTO_YAW_FIXED - last_yaw_mode = auto_yaw.mode(); - // gcs().send_text(MAV_SEVERITY_INFO, "auto_yaw: y: %d ",auto_yaw.mode()); - auto_yaw.set_mode(AUTO_YAW_FIXED); - } - attitude_control->input_euler_angle_roll_pitch_yaw(wp_nav->get_roll(), wp_nav->get_pitch(), wp_yaw_cd,true); - } - return; - }else{ // 如果需要转向到下一个航点 - copter.circle_nav->turn_2_circle(); - pos_control->update_z_controller(); - // Yaw 由 circle决定 - attitude_control->input_euler_angle_roll_pitch_yaw(copter.circle_nav->get_roll(), copter.circle_nav->get_pitch(), copter.circle_nav->get_yaw(), true); - - return; - } - } - } - // call attitude controller if (auto_yaw.mode() == AUTO_YAW_HOLD) { // roll & pitch from waypoint controller, yaw rate from pilot @@ -969,19 +979,14 @@ void ModeAuto::circle_run() if (abs(wrap_180_cd(ahrs.yaw_sensor-initial_armed_bearing)) <= 200) { //转到角度 circle_init_yaw = true; auto_yaw.set_mode(last_yaw_mode); - // gcs().send_text(MAV_SEVERITY_INFO, "circ init: y: %d -- %d ",initial_armed_bearing,ahrs.yaw_sensor); }else{ - if(wp_nav->set_wp_destination(last_loc)) { // 设置第一个WayPoint点为期望值 + // if(wp_nav->set_wp_destination(last_loc)) { // 设置第一个WayPoint点为期望值 if (auto_yaw.mode() != AUTO_YAW_FIXED) { // 需要转角度，yaw mode使用AUTO_YAW_FIXED last_yaw_mode = auto_yaw.mode(); - // gcs().send_text(MAV_SEVERITY_INFO, "auto_yaw: y: %d ",auto_yaw.mode()); auto_yaw.set_mode(AUTO_YAW_FIXED); } attitude_control->input_euler_angle_roll_pitch_yaw(wp_nav->get_roll(), wp_nav->get_pitch(), initial_armed_bearing,true); - }else{ // 第一个WayPoint点获取失败，直接绕圈 - circle_init_yaw = true; - gcs().send_text(MAV_SEVERITY_INFO, "--- circle turn false ---"); - } + // } return; } @@ -1269,8 +1274,24 @@ void ModeAuto::do_nav_wp(const AP_Mission::Mission_Command& cmd) loiter_time_max = cmd.p1; // Set wp navigation target wp_start(target_loc); - if(wp_nav_index < 2){ // 传给绕圈用，只有第一个wp需要绕圈，后面就不赋值了 + if(wp_nav_index < 1){ // 传给绕圈用，只有第一个wp需要绕圈，后面就不赋值了 last_loc = target_loc; + AP_Mission::Mission_Command next_cmd; + for(int i = cmd.index; i < cmd.index+5;i++) + { + mission.read_cmd_from_storage(i,next_cmd); // 读取原本航点 + // gcs_send_text_fmt(MAV_SEVERITY_DEBUG, "read[%d]: %d",i,cmd_rtl.id); + if (mission.is_nav_cmd(next_cmd)) { // 是否是Waypoint + float wp_distance = cmd.content.location.get_distance(next_cmd.content.location); + if(wp_distance < 5.0 && wp_nav_index < 2){ // 第一个wp需要转向相关。两个航点距离太近，正常是仿地降高度航点，则取再下一个航点 + continue; + }else{ + circle_init_yaw = false; + wp_yaw_cd = cmd.content.location.get_bearing_to(next_cmd.content.location); + break; + } + } + } } // if no delay as well as not final waypoint set the waypoint as "fast" AP_Mission::Mission_Command temp_cmd; @@ -1287,36 +1308,26 @@ void ModeAuto::do_nav_wp(const AP_Mission::Mission_Command& cmd) case MAV_CMD_NAV_SPLINE_WAYPOINT: // if next command's lat, lon is specified then do not slowdown at this waypoint if ((temp_cmd.content.location.lat != 0) || (temp_cmd.content.location.lng != 0)) { - fast_waypoint = true; - AP_Mission::Mission_Command next_2_cmd; - static int32_t last_yaw_cd; - if(mission.get_next_nav_cmd(cmd.index, next_2_cmd)){ - float wp_distance = next_2_cmd.content.location.get_distance(temp_cmd.content.location); - if(wp_distance < 5.0 && wp_nav_index < 2){ // 第一个wp需要转向相关。两个航点距离太近，正常是仿地降高度航点，则取再下一个航点 - // gcs().send_text(MAV_SEVERITY_CRITICAL, "-wp too close:%.2f",wp_distance); - if(mission.get_next_nav_cmd(cmd.index+2, temp_cmd)){ - wp_yaw_cd = next_2_cmd.content.location.get_bearing_to(temp_cmd.content.location); - // gcs().send_text(MAV_SEVERITY_CRITICAL, "-b new rad:%d",wp_yaw_cd); - } - }else{ - wp_yaw_cd = next_2_cmd.content.location.get_bearing_to(temp_cmd.content.location); - // gcs().send_text(MAV_SEVERITY_CRITICAL, "-a new rad:%d",wp_yaw_cd); - } - if(copter.wp_nav->get_fast_waypoint() == 2){ - static float last_dist = 0; - int16_t delt_cd = abs(wp_yaw_cd - last_yaw_cd); - // delt_cd = (delt_cd > 0)? delt_cd:360+delt_cd; - fast_waypoint = (delt_cd > 6000)?false:true; - // gcs().send_text(MAV_SEVERITY_CRITICAL, "%d,a:%d,d:%d,f:%d,p:%.2f",cmd.index,wp_yaw_cd,delt_cd,fast_waypoint,wp_distance); - if(wp_distance - last_dist > 0 && wp_nav_index > 0){ // P2.5转弯校准用 //TODO P2.5需要修改这里 - // gcs().send_text(MAV_SEVERITY_CRITICAL, "---- First Turn, Do Sth ----"); - } - last_dist = wp_distance; - last_yaw_cd = wp_yaw_cd; - }else{ - fast_waypoint = copter.wp_nav->get_fast_waypoint(); + // fast_waypoint = true; + + if(copter.wp_nav->get_fast_waypoint() == 2){ + static float last_dist = 0; + static int32_t last_yaw_cd; + float wp_distance = cmd.content.location.get_distance(temp_cmd.content.location); + wp_yaw_cd = cmd.content.location.get_bearing_to(temp_cmd.content.location); + + int16_t delt_cd = abs(wp_yaw_cd - last_yaw_cd); + // delt_cd = (delt_cd > 0)? delt_cd:360+delt_cd; + fast_waypoint = (delt_cd > 6000)?false:true; + if(wp_distance - last_dist > 0 && wp_nav_index > 0){ // P2.5转弯校准用 //TODO P2.5需要修改这里 + // gcs().send_text(MAV_SEVERITY_CRITICAL, "---- First Turn, Do Sth ----"); } + last_dist = wp_distance; + last_yaw_cd = wp_yaw_cd; + }else{ + fast_waypoint = copter.wp_nav->get_fast_waypoint(); } + } break; case MAV_CMD_NAV_RETURN_TO_LAUNCH: @@ -1712,7 +1723,6 @@ bool ModeAuto::verify_takeoff() }else{ return reached_wp_dest; } - } // verify_land - returns true if landing has been completed @@ -2044,7 +2054,7 @@ bool ModeAuto::verify_nav_wp_flow_measure(const AP_Mission::Mission_Command& cmd return false; } - copter.flow_measure.set_wp_index(cmd.index - copter.wp_nav->get_index_start()); // 修正测流起始航点 + copter.flow_measure.set_wp_index(wp_nav_index - copter.wp_nav->get_index_start()); // 修正测流起始航点 copter.flow_measure.set_reach_flow_wp(true); //到达航点标志置1 if(copter.flow_measure.get_flow_update_state() < 2){ loiter_time = millis(); diff --git a/ArduCopter/mode_rtl.cpp b/ArduCopter/mode_rtl.cpp index 10ab4d3df5..a6d55a3d79 100644 --- a/ArduCopter/mode_rtl.cpp +++ b/ArduCopter/mode_rtl.cpp @@ -429,8 +429,13 @@ void ModeRTL::descent_run() if (tnow - last_1s > 1000) { last_1s = tnow; - copter.Log_Write_Data(DATA_UPDOWN_SPEED,rtl_path.descent_target.alt); - copter.Log_Write_Data(DATA_UPDOWN_SPEED,copter.current_loc.alt); + // copter.Log_Write_Data(DATA_UPDOWN_SPEED,rtl_path.descent_target.alt); + // copter.Log_Write_Data(DATA_UPDOWN_SPEED,copter.current_loc.alt); + AP::logger().Write("RTES", "TimeUS,Stat,Dalt,Calt", "QBii", + AP_HAL::micros64(), + copter.updownStatus, + rtl_path.descent_target.alt, + copter.current_loc.alt); if(labs(last_alt - copter.current_loc.alt) < 30){ timeout_count += 1; }else{ diff --git a/ArduCopter/version.h b/ArduCopter/version.h index 40bb018aaa..6ee1d9d9bb 100644 --- a/ArduCopter/version.h +++ b/ArduCopter/version.h @@ -6,7 +6,7 @@ #include "ap_version.h" -#define THISFIRMWARE "ZRUAV v4.0.17-RC4" //"ArduCopter V4.0.0" +#define THISFIRMWARE "ZRUAV v4.0.17-RC5" //"ArduCopter V4.0.0" // the following line is parsed by the autotest scripts #define FIRMWARE_VERSION 4,0,17,FIRMWARE_VERSION_TYPE_OFFICIAL diff --git a/d100h.sh b/d100h.sh index b2d4417feb..0cce29b370 100755 --- a/d100h.sh +++ b/d100h.sh @@ -1,3 +1,3 @@ ./waf configure --board d100h ./waf copter -cp ./build/d100h/bin/arducopter.apj /mnt/f/_01-work/100\=data/固件/zr-v4/wsl/d100h-v4.0.16-AVDv6.px4 +cp ./build/d100h/bin/arducopter.apj /mnt/f/_01-work/100\=data/固件/编译/d100h-v4.0.17-RC5.px4