zrzk
/
ardupilot
1
0
Fork 0
Code Issues Pull Requests Projects Releases Wiki Activity
Browse Source

避障状态位改全局

mission-4.1.18
zbr 3 years ago
parent
4018c34930
commit
6bb104d0ee
3 changed files with 23 additions and 20 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. 1
      ArduCopter/Copter.h
  2. 4
      ArduCopter/UserCode.cpp
  3. 38
      ArduCopter/zr_flight.cpp

1
ArduCopter/Copter.h
Unescape View File

@ -1061,6 +1061,7 @@ public: @@ -1061,6 +1061,7 @@ public:
uint8_t mkdir_step;
Mode::Number get_before_mode() const { return before_mode; };
bool far_from_home;
Proxy_Action avoid_action_step;
};
extern Copter copter;

4
ArduCopter/UserCode.cpp
Unescape View File

@ -53,7 +53,7 @@ if(current_loc.alt > avoid.get_zr_avd_alt()) @@ -53,7 +53,7 @@ if(current_loc.alt > avoid.get_zr_avd_alt())
bool enable_avoid = false;
static bool last_flag = false;
if(avoid.get_zr_mode() > 0 && (control_mode == Mode::Number::AUTO || control_mode == Mode::Number::RTL || control_mode == Mode::Number::GUIDED || control_mode == Mode::Number::CIRCLE)){
if(avoid.get_zr_mode() > 0 && (control_mode == Mode::Number::BRAKE || control_mode == Mode::Number::AUTO || control_mode == Mode::Number::RTL || control_mode == Mode::Number::GUIDED || control_mode == Mode::Number::CIRCLE)){
if(far_from_home){
enable_avoid = true;
}
@ -72,6 +72,8 @@ if(last_flag != enable_avoid){ @@ -72,6 +72,8 @@ if(last_flag != enable_avoid){
}
if(enable_avoid){
do_avoid_action();
}else{
avoid_action_step = PROXY_WAIT;
}
#endif
}

38
ArduCopter/zr_flight.cpp
Unescape View File

@ -203,21 +203,21 @@ uint8_t Copter::avoid_state(float dist) @@ -203,21 +203,21 @@ uint8_t Copter::avoid_state(float dist)
void Copter::avoid_action(uint8_t area,uint8_t state,float dist)
{
static uint8_t delay_cnt;
static Proxy_Action action;
// static Proxy_Action action;
Vector3f pos_neu_cm;
static int32_t max_climb_alt = 0;
int32_t turn_yaw;
static uint32_t rtl_wait_time = AP_HAL::millis();
switch (action)
switch (avoid_action_step)
{
case PROXY_WAIT:
if(control_mode == Mode::Number::LAND ){ // 降落模式不触发避障
break;
}
if(area == PROXY_FIND_BARRIER && state == PROXY_APPROACH)
action = PROXY_SLOW_DOWN;
avoid_action_step = PROXY_SLOW_DOWN;
if(area == PROXY_TOO_CLOSE)
action = PROXY_BRAKE;
avoid_action_step = PROXY_BRAKE;
break;
case PROXY_SLOW_DOWN:
if(state == PROXY_APPROACH){
@ -225,13 +225,13 @@ void Copter::avoid_action(uint8_t area,uint8_t state,float dist) @@ -225,13 +225,13 @@ void Copter::avoid_action(uint8_t area,uint8_t state,float dist)
// last_prx_diff = dist;
}
if(area == PROXY_TOO_CLOSE)
action = PROXY_BRAKE;
avoid_action_step = PROXY_BRAKE;
break;
case PROXY_BRAKE:
if(state != PROXY_LEAVE){
gcs().send_text(MAV_SEVERITY_CRITICAL,"障碍物距离过近:%3.2fm，刹车",dist);
set_mode(Mode::Number::BRAKE,ModeReason::AVOIDANCE);
action = PROXY_DELAY;
avoid_action_step = PROXY_DELAY;
delay_cnt = 0;
}
break;
@ -242,21 +242,21 @@ void Copter::avoid_action(uint8_t area,uint8_t state,float dist) @@ -242,21 +242,21 @@ void Copter::avoid_action(uint8_t area,uint8_t state,float dist)
// turn_yaw = current_loc.get_bearing_to(ahrs.get_home());
// bool need_turn_yaw = abs(ahrs.yaw_sensor - turn_yaw)<4500;
// if(need_turn_yaw){
// action = PROXY_CLIMB;
// avoid_action_step = PROXY_CLIMB;
max_climb_alt = current_loc.alt+avoid.get_zr_avd_max_climb();
gcs().send_text(MAV_SEVERITY_INFO,"NOTICE:准备爬升，Now:%.2f，MAX:%.2f，",current_loc.alt/100.0f,max_climb_alt/100.0f);
// }else{
// action = PROXY_RTL;
// avoid_action_step = PROXY_RTL;
// max_climb_alt = current_loc.alt;
// gcs().send_text(MAV_SEVERITY_INFO,"NOTICE:角度在安全范围:%d，直接返航",abs(ahrs.yaw_sensor - turn_yaw));
// }
action = PROXY_CLIMB;
avoid_action_step = PROXY_CLIMB;
delay_cnt = 0;
}
if(control_mode == Mode::Number::LOITER || control_mode == Mode::Number::ALT_HOLD ){
action = PROXY_WAIT;
avoid_action_step = PROXY_WAIT;
gcs().send_text(MAV_SEVERITY_INFO,"NOTICE:切换到人为控制");
}
delay_cnt+=1;
@ -273,18 +273,18 @@ void Copter::avoid_action(uint8_t area,uint8_t state,float dist) @@ -273,18 +273,18 @@ void Copter::avoid_action(uint8_t area,uint8_t state,float dist)
// mode_guided.set_destination(pos_neu_cm, false, 0, false, 0, false);
if(current_loc.alt > max_climb_alt){
action = PROXY_NEED_CTL;
avoid_action_step = PROXY_NEED_CTL;
rtl_wait_time = AP_HAL::millis();
gcs().send_text(MAV_SEVERITY_CRITICAL,"超出爬升高度限制:%.2f，请手动控制",current_loc.alt/100.0f);
}
if(area == PROXY_OUTRANGE){
action = PROXY_RTL;
avoid_action_step = PROXY_RTL;
max_climb_alt = current_loc.alt + 1000;
gcs().send_text(MAV_SEVERITY_INFO,"NOTICE:已避开障碍，准备返航");
}
if(control_mode == Mode::Number::LOITER || control_mode == Mode::Number::ALT_HOLD ){
action = PROXY_WAIT;
avoid_action_step = PROXY_WAIT;
gcs().send_text(MAV_SEVERITY_INFO,"NOTICE:切换到人为控制");
}
break;
@ -299,13 +299,13 @@ void Copter::avoid_action(uint8_t area,uint8_t state,float dist) @@ -299,13 +299,13 @@ void Copter::avoid_action(uint8_t area,uint8_t state,float dist)
// turn_yaw = current_loc.get_bearing_to(ahrs.get_home());
mode_guided.set_destination(pos_neu_cm, false, 0, false, 0, false);
if(control_mode != Mode::Number::GUIDED){
action = PROXY_WAIT;
avoid_action_step = PROXY_WAIT;
gcs().send_text(MAV_SEVERITY_INFO,"NOTICE:切换到人为控制");
}
}else{
gcs().send_text(MAV_SEVERITY_INFO,"NOTICE:开始返航");
set_mode(Mode::Number::RTL,ModeReason::AVOIDANCE);
action = PROXY_WAIT;
avoid_action_step = PROXY_WAIT;
}
break;
@ -313,16 +313,16 @@ void Copter::avoid_action(uint8_t area,uint8_t state,float dist) @@ -313,16 +313,16 @@ void Copter::avoid_action(uint8_t area,uint8_t state,float dist)
{
uint32_t tnow = AP_HAL::millis();
if(area == PROXY_OUTRANGE){
action = PROXY_WAIT;
avoid_action_step = PROXY_WAIT;
gcs().send_text(MAV_SEVERITY_INFO,"NOTICE:人为控制，已离开障碍");
}
if(tnow - rtl_wait_time > (uint32_t)(g.zr_avd_wait * 1000)){
if(avoid.get_zr_mode() == 2){
action = PROXY_WAIT;
avoid_action_step = PROXY_WAIT;
gcs().send_text(MAV_SEVERITY_INFO,"NOTICE:等待超时，原地降落");
set_mode(Mode::Number::LAND,ModeReason::AVOIDANCE);
}else{
action = PROXY_RTL_ERR;
avoid_action_step = PROXY_RTL_ERR;
gcs().send_text(MAV_SEVERITY_INFO,"NOTICE:等待超时，关闭避障返航");
set_mode(Mode::Number::RTL,ModeReason::AVOIDANCE);
}
@ -332,7 +332,7 @@ void Copter::avoid_action(uint8_t area,uint8_t state,float dist) @@ -332,7 +332,7 @@ void Copter::avoid_action(uint8_t area,uint8_t state,float dist)
case PROXY_RTL_ERR:
{
if(control_mode == Mode::Number::LAND ){ // 如果切换到Land，则可以重新开始
action = PROXY_WAIT;
avoid_action_step = PROXY_WAIT;
break;
}
}

Write Preview
Loading…
Cancel
Save