From 82802f45dc2ac209d95e8957ac4974db224e56e5 Mon Sep 17 00:00:00 2001 From: zbr Date: Tue, 14 Dec 2021 15:25:48 +0800 Subject: [PATCH] =?UTF-8?q?=E6=96=AD=E7=82=B9=E7=BB=AD=E9=A3=9E=E4=BC=98?= =?UTF-8?q?=E5=8C=96=EF=BC=8C=E7=BB=958=E4=BC=98=E5=8C=96,=E5=A2=9E?= =?UTF-8?q?=E5=8A=A0=E9=9D=9E=E8=87=AA=E5=8A=A8=E6=A8=A1=E5=BC=8F=E7=BB=95?= =?UTF-8?q?8=E5=88=9D=E5=A7=8B=E5=8C=96=EF=BC=8C=E5=85=A8=E5=B1=80WPNAV?= =?UTF-8?q?=E8=88=AA=E7=82=B9=E5=8F=B7=EF=BC=8CRTK=E6=B2=A1=E9=A3=9E?= =?UTF-8?q?=E8=88=AA=E7=BA=BF=E4=B8=8D=E7=BB=958?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- ArduCopter/Copter.h | 2 + ArduCopter/mode_auto.cpp | 56 ++++++++++++-------------- ArduCopter/mode_rtl.cpp | 4 +- ArduCopter/zr_flight.cpp | 3 ++ libraries/AC_WPNav/AC_Circle.cpp | 61 ++++++----------------------- libraries/AC_WPNav/AC_Circle.h | 2 +- libraries/AP_Mission/AP_Mission.cpp | 5 +-- modules/mavlink | 2 +- 8 files changed, 47 insertions(+), 88 deletions(-) diff --git a/ArduCopter/Copter.h b/ArduCopter/Copter.h index 0c1b7f694f..7d17eff4fa 100644 --- a/ArduCopter/Copter.h +++ b/ArduCopter/Copter.h @@ -1078,6 +1078,8 @@ public: bool far_from_home; Proxy_Action avoid_action_step; void zr_set_battery_capacity(); + uint16_t mission_nav_index; + /****** 测流 ******/ void zr_mission_flow_mode(); bool flow_start_sw; diff --git a/ArduCopter/mode_auto.cpp b/ArduCopter/mode_auto.cpp index f303706586..810854e868 100644 --- a/ArduCopter/mode_auto.cpp +++ b/ArduCopter/mode_auto.cpp @@ -48,7 +48,7 @@ bool ModeAuto::init(bool ignore_checks) 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); + // gcs().send_text(MAV_SEVERITY_INFO, "auto init:up:%d,wp:%d",updown_state,wp_nav_index); return true; } else { return false; @@ -148,7 +148,7 @@ void ModeAuto::takeoff_start(const Location& dest_loc) Location dest(dest_loc); last_loc = dest_loc; - if (g.zr_tk_req_alt > 0 && updown_state == 0){ + 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); @@ -654,7 +654,6 @@ bool ModeAuto::verify_command(const AP_Mission::Mission_Command& cmd) if (copter.flightmode != &copter.mode_auto) { return false; } - bool cmd_complete = false; switch (cmd.id) { @@ -768,16 +767,11 @@ void ModeAuto::takeoff_run() copter.updownStatus < UpDown_RequestClimb && \ updown_state == 2) { - updown_state = 3; + 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 ){ @@ -807,9 +801,11 @@ void ModeAuto::takeoff_run() return; }else if(copter.updownStatus > UpDown_RequestClimb){ - updown_state = 4; - // gcs().send_text(MAV_SEVERITY_INFO, "NOTICE: 执行继续任务"); - + if(updown_state < 4){ // 地面站弹框确定，发送新的值，状态位同步更新 + updown_state = 4; + gcs().send_text(MAV_SEVERITY_INFO, "gcs ctl updownStatus:%d",copter.updownStatus); + takeoff_start(last_loc); + } } } if(g.zr_8_circle == 1){ @@ -959,18 +955,16 @@ 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); + // 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)) { - if (auto_yaw.mode() != AUTO_YAW_FIXED) { + 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()); + // 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{ - + }else{ // 第一个WayPoint点获取失败，直接绕圈 circle_init_yaw = true; gcs().send_text(MAV_SEVERITY_INFO, "--- circle turn false ---"); } @@ -1261,7 +1255,9 @@ 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需要绕圈，后面就不赋值了 + last_loc = target_loc; + } // if no delay as well as not final waypoint set the waypoint as "fast" AP_Mission::Mission_Command temp_cmd; bool fast_waypoint = false; @@ -1282,14 +1278,15 @@ void ModeAuto::do_nav_wp(const AP_Mission::Mission_Command& 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){// 两个航点距离太近，正常是仿地降高度航点，则取再下一个航点 + 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, "-new rad:%d",wp_yaw_cd); + // 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; @@ -1682,7 +1679,6 @@ bool ModeAuto::verify_takeoff() { // have we reached our target altitude? const bool reached_wp_dest = copter.wp_nav->reached_wp_destination(); - // retract the landing gear if (reached_wp_dest) { copter.landinggear.set_position(AP_LandingGear::LandingGear_Retract); @@ -1692,17 +1688,13 @@ bool ModeAuto::verify_takeoff() }else{ // 常规起飞判断，增加设置绕圈相关初始设置 copter.circle_nav->set_circle_8_finish(false); first_wp_dec = false; - if(g.zr_tk_req_alt > 0 ){ + if(g.zr_tk_delay > 0 && g.zr_tk_req_alt > 0 ){ // 启用到高度悬停 - if(reached_wp_dest && updown_state <2){ + if(reached_wp_dest && updown_state <2){ // 原到达航点判断，到达起飞悬停位置 updown_state = 2; - }else if(updown_state >= 4){ + }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; @@ -2020,11 +2012,13 @@ bool ModeAuto::verify_nav_wp(const AP_Mission::Mission_Command& cmd) 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 计数 + copter.mission_nav_index = wp_nav_index; } 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 计数 + copter.mission_nav_index = wp_nav_index; return true; } } @@ -2064,6 +2058,8 @@ bool ModeAuto::verify_nav_wp_flow_measure(const AP_Mission::Mission_Command& cmd copter.flow_measure.set_reach_flow_wp(false); gcs().send_text(MAV_SEVERITY_INFO, "获取数据超时,序号:#%i",cmd.index); + wp_nav_index += 1; // nav 计数 + copter.mission_nav_index = wp_nav_index; return true; } return false; diff --git a/ArduCopter/mode_rtl.cpp b/ArduCopter/mode_rtl.cpp index 40ec47051b..3d41cb28b6 100644 --- a/ArduCopter/mode_rtl.cpp +++ b/ArduCopter/mode_rtl.cpp @@ -44,7 +44,7 @@ void ModeRTL::run(bool disarm_on_land) if (!motors->armed()) { return; } -if(g.zr_8_circle == 2){ +if(g.zr_8_circle == 2 && copter.mission_nav_index > 1){ static uint8_t flag = 0; if(_state == RTL_ReturnHome){ // uint8_t flag = copter.circle_nav->run_rtl_8_circle(_state != RTL_LoiterAtHome); @@ -187,7 +187,7 @@ void ModeRTL::climb_start() copter.set_mode(Mode::Number::LAND, ModeReason::TERRAIN_FAILSAFE); return; } - wp_nav->set_fast_waypoint(true); + // wp_nav->set_fast_waypoint(true); // hold current yaw during initial climb auto_yaw.set_mode(AUTO_YAW_HOLD); diff --git a/ArduCopter/zr_flight.cpp b/ArduCopter/zr_flight.cpp index e34cecd843..a1bc5ca4e6 100644 --- a/ArduCopter/zr_flight.cpp +++ b/ArduCopter/zr_flight.cpp @@ -97,6 +97,9 @@ void Copter::zr_SuperSlowLoop(){ } } } + if(control_mode != Mode::Number::AUTO && control_mode != Mode::Number::RTL){ + circle_nav->parm_reset(); + } } void Copter::zr_set_battery_capacity() diff --git a/libraries/AC_WPNav/AC_Circle.cpp b/libraries/AC_WPNav/AC_Circle.cpp index 7bd4f4df9a..a54cba2a49 100644 --- a/libraries/AC_WPNav/AC_Circle.cpp +++ b/libraries/AC_WPNav/AC_Circle.cpp @@ -117,6 +117,11 @@ void AC_Circle::init() // gcs().send_text(MAV_SEVERITY_INFO, "--- AC_Circle x:%.2f,y:%.2f \n",_center.x,_center.y); } +void AC_Circle::parm_reset(){ + stage = 0; + is_cw_turn = true; + _circle_8_finish = false; +} /// set_circle_rate - set circle rate in degrees per second void AC_Circle::set_rate(float deg_per_sec) { @@ -224,11 +229,11 @@ bool AC_Circle::turn_2_circle() target.x = _center.x + _radius * cosf(_angle); target.y = _center.y + _radius * sinf(_angle); - gcs().send_text(MAV_SEVERITY_INFO, "C2,%.2f,%.2f,%.2f,%.2f", - stopping_point.x, - stopping_point.y, - target.x, - target.y); + // gcs().send_text(MAV_SEVERITY_INFO, "C2,%.2f,%.2f,%.2f,%.2f", + // stopping_point.x, + // stopping_point.y, + // target.x, + // target.y); // gcs().send_text(MAV_SEVERITY_INFO, "[i]init ag::%.2f,y:%.2f,r:%.2f,d:%.2f\n",_angle,_ahrs.yaw,bearing_rad,bearing_rad * RAD_TO_DEG); last_bearing_rad = _ahrs.yaw; // first_rad = _ahrs.yaw; @@ -283,7 +288,7 @@ bool AC_Circle::turn_2_circle() break; case 3: // _angle = wrap_PI(_ahrs.yaw-M_PI); - gcs().send_text(MAV_SEVERITY_INFO, "[c]finish angle::%.2f,y:%.2f,t:%.2f,%.2f \n",_angle,_ahrs.yaw,target.x,target.y); + // gcs().send_text(MAV_SEVERITY_INFO, "[c]finish angle::%.2f,y:%.2f,t:%.2f,%.2f \n",_angle,_ahrs.yaw,target.x,target.y); stage = 4; break; case 4: @@ -365,50 +370,6 @@ void AC_Circle::update() _pos_control.update_xy_controller(); } - -/// update - update circle controller -void AC_Circle::turn_yaw(const Vector3f& center) -{ - // calculate dt - float dt = _pos_control.time_since_last_xy_update(); - if (dt >= 0.2f) { - dt = 0.0f; - } - - // ramp angular velocity to maximum - if (_angular_vel < _angular_vel_max) { - _angular_vel += fabsf(_angular_accel) * dt; - _angular_vel = MIN(_angular_vel, _angular_vel_max); - } - if (_angular_vel > _angular_vel_max) { - _angular_vel -= fabsf(_angular_accel) * dt; - _angular_vel = MAX(_angular_vel, _angular_vel_max); - } - - // update the target angle and total angle traveled - float angle_change = _angular_vel * dt; - _angle += angle_change; - _angle = wrap_PI(_angle); - _angle_total += angle_change; - - // set target position to center - Vector3f target; - target.x = center.x; - target.y = center.y; - target.z = _pos_control.get_alt_target(); - - // update position controller target - _pos_control.set_xy_target(target.x, target.y); - - // heading is same as _angle but converted to centi-degrees - _yaw = _angle * DEGX100; - - - // update position controller - _pos_control.update_xy_controller(); -} - - // get_closest_point_on_circle - returns closest point on the circle // circle's center should already have been set // closest point on the circle will be placed in result diff --git a/libraries/AC_WPNav/AC_Circle.h b/libraries/AC_WPNav/AC_Circle.h index 34d4d824af..3e326fe044 100644 --- a/libraries/AC_WPNav/AC_Circle.h +++ b/libraries/AC_WPNav/AC_Circle.h @@ -80,7 +80,7 @@ public: bool run_8_circle(bool reached); uint8_t run_rtl_8_circle(uint8_t *step); - void turn_yaw(const Vector3f& center); + void parm_reset(); static const struct AP_Param::GroupInfo var_info[]; private: diff --git a/libraries/AP_Mission/AP_Mission.cpp b/libraries/AP_Mission/AP_Mission.cpp index 5ef18684d2..8b4da5ce05 100644 --- a/libraries/AP_Mission/AP_Mission.cpp +++ b/libraries/AP_Mission/AP_Mission.cpp @@ -2013,8 +2013,6 @@ AP_Mission *mission() } - - /** * Modify by @Brown */ @@ -2075,7 +2073,7 @@ void AP_Mission::auto_resume_mission(uint8_t f_mode, bool is_arm,int32_t r_lat,i cmd_rtl.content.location.lng = r_lng; cmd_rtl.id = MAV_CMD_NAV_WAYPOINT; // cmd_rtl.content.location.options = 0; - cmd_rtl.p1 = 2; // 到航点后延时2s + // cmd_rtl.p1 = 2; // 到航点后延时2s // cmd_rtl.content.location.flags.relative_alt = 1; replace_cmd(cmd_rtl_index, cmd_rtl); @@ -2097,7 +2095,6 @@ void AP_Mission::auto_resume_mission(uint8_t f_mode, bool is_arm,int32_t r_lat,i switch (cmd_step) { // gcs_send_text_fmt(MAV_SEVERITY_INFO, "[d]t:%d,i:%d \n ",cmd_step,cmd_rtl_i); - case 0: if (cmd_temp.id == MAV_CMD_NAV_TAKEOFF) { diff --git a/modules/mavlink b/modules/mavlink index 5fc095d170..72b407db39 160000 --- a/modules/mavlink +++ b/modules/mavlink @@ -1 +1 @@ -Subproject commit 5fc095d17043557589ecc6a28d62fb234081fc87 +Subproject commit 72b407db3991ed74631fa59a5e826615f569819f