zrzk
/
zr-v4
1
0
Fork 0
Code Issues Pull Requests Projects Releases Wiki Activity
Browse Source

断点续飞优化，绕8优化,增加非自动模式绕8初始化，全局WPNAV航点号，RTK没飞航线不绕8

celiu-4.0.17-rc8
zbr 3 years ago
parent
4d670c1b93
commit
82802f45dc
8 changed files with 47 additions and 88 deletions
Whitespace
Show all changes Ignore whitespace when comparing lines Ignore changes in amount of whitespace Ignore changes in whitespace at EOL
Split View
Diff Options
Show Stats Download Patch File Download Diff File
  1. 2
      ArduCopter/Copter.h
  2. 56
      ArduCopter/mode_auto.cpp
  3. 4
      ArduCopter/mode_rtl.cpp
  4. 3
      ArduCopter/zr_flight.cpp
  5. 61
      libraries/AC_WPNav/AC_Circle.cpp
  6. 2
      libraries/AC_WPNav/AC_Circle.h
  7. 5
      libraries/AP_Mission/AP_Mission.cpp
  8. 2
      modules/mavlink

2
ArduCopter/Copter.h
Unescape View File

@ -1078,6 +1078,8 @@ public: @@ -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;

56
ArduCopter/mode_auto.cpp
Unescape View File

@ -48,7 +48,7 @@ bool ModeAuto::init(bool ignore_checks) @@ -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) @@ -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) @@ -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() @@ -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() @@ -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() @@ -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) @@ -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) @@ -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() @@ -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() @@ -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) @@ -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 @@ -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;

4
ArduCopter/mode_rtl.cpp
Unescape View File

@ -44,7 +44,7 @@ void ModeRTL::run(bool disarm_on_land) @@ -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() @@ -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);

3
ArduCopter/zr_flight.cpp
Unescape View File

@ -97,6 +97,9 @@ void Copter::zr_SuperSlowLoop(){ @@ -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()

61
libraries/AC_WPNav/AC_Circle.cpp
Unescape View File

@ -117,6 +117,11 @@ void AC_Circle::init() @@ -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() @@ -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() @@ -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() @@ -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

2
libraries/AC_WPNav/AC_Circle.h
Unescape View File

@ -80,7 +80,7 @@ public: @@ -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:

5
libraries/AP_Mission/AP_Mission.cpp
Unescape View File

@ -2013,8 +2013,6 @@ AP_Mission *mission() @@ -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 @@ -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 @@ -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)
{

2
modules/mavlink

@ -1 +1 @@ @@ -1 +1 @@
Subproject commit 5fc095d17043557589ecc6a28d62fb234081fc87
Subproject commit 72b407db3991ed74631fa59a5e826615f569819f
Write Preview
Loading…
Cancel
Save