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

增加避障功能，停下，转向HOME点，升高

mission-4.1.18
孤帆远影Faraway 3 years ago
parent
915d2f1dba
commit
8cb96636f2
7 changed files with 268 additions and 6 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. 30
      ArduCopter/Copter.h
  2. 7
      ArduCopter/UserCode.cpp
  3. 201
      ArduCopter/zr_flight.cpp
  4. 7
      libraries/AC_Avoidance/AC_Avoid.cpp
  5. 14
      libraries/AC_Avoidance/AC_Avoid.h
  6. 10
      libraries/AC_WPNav/AC_Loiter.cpp
  7. 5
      libraries/AC_WPNav/AC_Loiter.h

30
ArduCopter/Copter.h
Unescape View File

@ -1007,6 +1007,31 @@ private: @@ -1007,6 +1007,31 @@ private:
void exit_mode(Mode *&old_flightmode, Mode *&new_flightmode);
bool avoid_rtl_action(float max_speed);
bool avoid_climb_action();
enum Proxy_State {
PROXY_HOLD=0,
PROXY_APPROACH=1,
PROXY_LEAVE=2
};
enum Proxy_Area {
PROXY_OUTRANGE=0,
PROXY_FIND_BARRIER=1,
PROXY_TOO_CLOSE=2
};
enum Proxy_Action {
PROXY_WAIT=0,
PROXY_SLOW_DOWN,
PROXY_BRAKE,
PROXY_DELAY,
PROXY_CLIMB,
PROXY_NEED_CTL,
PROXY_RTL
};
Proxy_State proxy_state;
Proxy_Area proxy_area;
public:
void mavlink_delay_cb(); // GCS_Mavlink.cpp
void failsafe_check(); // failsafe.cpp
@ -1023,6 +1048,11 @@ public: @@ -1023,6 +1048,11 @@ public:
void zr_set_uav_state_to_uavcan();
void zr_camera_mkdir();
void zr_mkdir_in_takeoff();
uint8_t avoid_area_detect(float dist);
uint8_t avoid_state(float dist);
void avoid_action(uint8_t area,uint8_t state,float dist);
uint8_t BinarySearch2f(float a[], float value, int low, int high);
int8_t in_debug_mode;
uint8_t cacl_volt_pst;

7
ArduCopter/UserCode.cpp
Unescape View File

@ -46,6 +46,13 @@ void Copter::userhook_50Hz() @@ -46,6 +46,13 @@ void Copter::userhook_50Hz()
void Copter::userhook_MediumLoop()
{
// put your 10Hz code here
#if CONFIG_HAL_BOARD == HAL_BOARD_SITL
if(current_loc.alt > avoid.get_zr_avd_alt())
do_avoid_action();
#else
if(current_loc.alt > avoid.get_zr_avd_alt() && avoid.get_zr_mode()>0 && in_debug_mode > 0)
do_avoid_action();
#endif
}
#endif

201
ArduCopter/zr_flight.cpp
Unescape View File

@ -2,9 +2,6 @@ @@ -2,9 +2,6 @@
#include "Copter.h"
void Copter::do_avoid_action(){
}
// bool Copter::close_to_EKF_origin(const Location& loc)
// {
// // check distance to EKF origin
@ -137,7 +134,205 @@ void Copter::zr_SuperSlowLoop(){ @@ -137,7 +134,205 @@ void Copter::zr_SuperSlowLoop(){
break;
}
}
}
void Copter::do_avoid_action(){
float proxy_dist = 0;
#if CONFIG_HAL_BOARD == HAL_BOARD_SITL
uint16_t rc10_in = RC_Channels::rc_channel(CH_6)->get_radio_in();
proxy_dist = (rc10_in - 1100)/900.0*50.0;
#else
if(avoid.get_zr_mode() == 1){
AP_Proximity *proximity = AP::proximity();
if(proximity == nullptr)
return;
proximity->get_horizontal_distance(0,proxy_dist);
}
if(avoid.get_zr_mode() == 2){
uint16_t rc10_in = RC_Channels::rc_channel(CH_12)->get_radio_in();
proxy_dist = (rc10_in - 1100)/900.0*50.0;
}
#endif
avoid_area_detect(proxy_dist);
avoid_state(proxy_dist);
avoid_action(proxy_area,proxy_state,proxy_dist);
}
uint8_t Copter::avoid_area_detect(float dist)
{
Proxy_Area area;
if(dist < avoid.get_zr_avd_min()){
if(area != PROXY_TOO_CLOSE) // 前一个状态不是障碍物过近
area = PROXY_OUTRANGE;
area = PROXY_OUTRANGE;
}
else if(dist< avoid.get_zr_avd_trig())
area = PROXY_TOO_CLOSE;
else if(dist<avoid.get_zr_avd_max())
area = PROXY_FIND_BARRIER;
else {
area = PROXY_OUTRANGE;
}
proxy_area = area;
return area;
}
uint8_t Copter::avoid_state(float dist)
{
Proxy_State state;
static float previous_dist = 0;
float delt_dist = dist - previous_dist;
previous_dist = dist;
if(delt_dist > 1)
state = PROXY_LEAVE;
else if(delt_dist < -1)
state = PROXY_APPROACH;
else
state = PROXY_HOLD;
proxy_state = state;
return state;
}
void Copter::avoid_action(uint8_t area,uint8_t state,float dist)
{
static uint8_t delay_cnt;
static Proxy_Action action;
static const float get_org_max_speed = wp_nav->get_default_speed_xy();
static float avoid_slow_speed = 100.0f;
Vector3f pos_neu_cm;
static int32_t max_climb_alt = 0;
int32_t turn_yaw;
switch (action)
{
case PROXY_WAIT:
if(area == PROXY_FIND_BARRIER && state == PROXY_APPROACH)
action = PROXY_SLOW_DOWN;
if(area == PROXY_TOO_CLOSE)
action = PROXY_BRAKE;
break;
case PROXY_SLOW_DOWN:
if(state == PROXY_APPROACH){
avoid_slow_speed = (dist - avoid.get_zr_avd_trig())/(avoid.get_zr_avd_max() - avoid.get_zr_avd_trig())*loiter_nav->get_loiter_speed();
avoid_slow_speed = MAX(avoid_slow_speed,0);
gcs().send_text(MAV_SEVERITY_INFO,"NOTICE: 发现障碍物，距离:%3.2fm,v:%.2f",dist,avoid_slow_speed);
wp_nav->set_speed_xy(avoid_slow_speed);
loiter_nav->set_loiter_speed(avoid_slow_speed);
// last_prx_diff = dist;
}
if(area == PROXY_TOO_CLOSE)
action = 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;
delay_cnt = 0;
}
break;
case PROXY_DELAY:
if(delay_cnt > 30){
set_mode(Mode::Number::GUIDED,ModeReason::AVOIDANCE);
// 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;
max_climb_alt = current_loc.alt+avoid.get_zr_avd_max_climb();
gcs().send_text(MAV_SEVERITY_INFO,"NOTICE:准备爬升，Now:%d，MAX:%d，",current_loc.alt,max_climb_alt);
// }else{
// action = 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;
delay_cnt = 0;
}
if(control_mode == Mode::Number::LOITER || control_mode == Mode::Number::ALT_HOLD ){
action = PROXY_WAIT;
gcs().send_text(MAV_SEVERITY_INFO,"NOTICE:切换到人为控制");
}
delay_cnt+=1;
break;
case PROXY_CLIMB:
if (!current_loc.get_vector_from_origin_NEU(pos_neu_cm)) {
gcs().send_text(MAV_SEVERITY_INFO,"NOTICE:无法获取位置");
break;
}
pos_neu_cm.z += 1000;
turn_yaw = current_loc.get_bearing_to(ahrs.get_home());
mode_guided.set_destination(pos_neu_cm, true, turn_yaw, true, 200, false);
// mode_guided.set_destination(pos_neu_cm, false, 0, false, 0, false);
if(current_loc.alt > max_climb_alt){
action = PROXY_NEED_CTL;
gcs().send_text(MAV_SEVERITY_CRITICAL,"超出爬升高度限制:%d，请手动控制",current_loc.alt);
}
if(area == PROXY_OUTRANGE){
action = 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;
gcs().send_text(MAV_SEVERITY_INFO,"NOTICE:切换到人为控制");
}
break;
case PROXY_RTL:
if(current_loc.alt < max_climb_alt)
{
if (!current_loc.get_vector_from_origin_NEU(pos_neu_cm)) {
gcs().send_text(MAV_SEVERITY_INFO,"NOTICE:无法获取位置");
break;
}
pos_neu_cm.z += max_climb_alt - current_loc.alt + 500;
// 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;
gcs().send_text(MAV_SEVERITY_INFO,"NOTICE:切换到人为控制");
}
}else{
gcs().send_text(MAV_SEVERITY_INFO,"NOTICE:开始返航");
set_mode(Mode::Number::RTL,ModeReason::AVOIDANCE);
wp_nav->set_speed_xy(get_org_max_speed);
loiter_nav->set_loiter_speed(get_org_max_speed);
action = PROXY_WAIT;
}
break;
case PROXY_NEED_CTL:
if(area == PROXY_OUTRANGE){
action = PROXY_WAIT;
gcs().send_text(MAV_SEVERITY_INFO,"NOTICE:人为控制，已离开障碍");
}
break;
default:
break;
}
}
#endif

7
libraries/AC_Avoidance/AC_Avoid.cpp
Unescape View File

@ -68,6 +68,13 @@ const AP_Param::GroupInfo AC_Avoid::var_info[] = { @@ -68,6 +68,13 @@ const AP_Param::GroupInfo AC_Avoid::var_info[] = {
// @User: Standard
AP_GROUPINFO("BEHAVE", 5, AC_Avoid, _behavior, AP_AVOID_BEHAVE_DEFAULT),
AP_GROUPINFO("ZR_MODE", 6, AC_Avoid, _zr_mode, 1), // 是否启用
AP_GROUPINFO("ZR_TRIG", 7, AC_Avoid, _zr_trig, 25), // 触发避障距离，单位米
AP_GROUPINFO("ZR_MAX", 8, AC_Avoid, _zr_max, 35), // 检测最大距离，单位米
AP_GROUPINFO("ZR_MIN", 9, AC_Avoid, _zr_min, 5), // 检测最小距离，单位米
AP_GROUPINFO("ZR_ALT", 10, AC_Avoid, _zr_alt, 2500), // 启用避障高度，单位厘米
AP_GROUPINFO("ZR_CLB", 11, AC_Avoid, _zr_max_climb, 3000), // 最大爬升高度设置，单位厘米
AP_GROUPEND
};

14
libraries/AC_Avoidance/AC_Avoid.h
Unescape View File

@ -82,6 +82,13 @@ public: @@ -82,6 +82,13 @@ public:
// return margin (in meters) that the vehicle should stay from objects
float get_margin() const { return _margin; }
int8_t get_zr_mode() const { return _zr_mode; }
int8_t get_zr_avd_trig() const { return _zr_trig; }
int8_t get_zr_avd_max() const { return _zr_max; }
int8_t get_zr_avd_min() const { return _zr_min; }
int16_t get_zr_avd_alt() const { return _zr_alt; }
int16_t get_zr_avd_max_climb() const { return _zr_max_climb; }
static const struct AP_Param::GroupInfo var_info[];
private:
@ -147,6 +154,13 @@ private: @@ -147,6 +154,13 @@ private:
AP_Float _margin; // vehicle will attempt to stay this distance (in meters) from objects while in GPS modes
AP_Int8 _behavior; // avoidance behaviour (slide or stop)
AP_Int8 _zr_mode;
AP_Int8 _zr_trig;
AP_Int8 _zr_max;
AP_Int8 _zr_min;
AP_Int16 _zr_alt;
AP_Int16 _zr_max_climb;
bool _proximity_enabled = true; // true if proximity sensor based avoidance is enabled (used to allow pilot to enable/disable)
static AC_Avoid *_singleton;

10
libraries/AC_WPNav/AC_Loiter.cpp
Unescape View File

@ -106,6 +106,7 @@ void AC_Loiter::init_target(const Vector3f& position) @@ -106,6 +106,7 @@ void AC_Loiter::init_target(const Vector3f& position)
_pos_control.set_desired_velocity_xy(0.0f,0.0f);
_pos_control.set_desired_accel_xy(0.0f,0.0f);
speed_cms = _speed_cms; // 获取悬停飞行最大速度
// initialise position controller if not already active
if (!_pos_control.is_active_xy()) {
_pos_control.init_xy_controller();
@ -216,7 +217,8 @@ void AC_Loiter::update() @@ -216,7 +217,8 @@ void AC_Loiter::update()
}
// initialise pos controller speed and acceleration
_pos_control.set_max_speed_xy(_speed_cms);
// _pos_control.set_max_speed_xy(_speed_cms);
_pos_control.set_max_speed_xy(speed_cms);
_pos_control.set_max_accel_xy(_accel_cmss);
calc_desired_velocity(dt);
@ -226,7 +228,8 @@ void AC_Loiter::update() @@ -226,7 +228,8 @@ void AC_Loiter::update()
// sanity check parameters
void AC_Loiter::sanity_check_params()
{
_speed_cms = MAX(_speed_cms, LOITER_SPEED_MIN);
// _speed_cms = MAX(_speed_cms, LOITER_SPEED_MIN);
speed_cms = MAX(speed_cms, LOITER_SPEED_MIN);
_accel_cmss = MIN(_accel_cmss, GRAVITY_MSS * 100.0f * tanf(ToRad(_attitude_control.lean_angle_max() * 0.01f)));
}
@ -239,7 +242,8 @@ void AC_Loiter::calc_desired_velocity(float nav_dt) @@ -239,7 +242,8 @@ void AC_Loiter::calc_desired_velocity(float nav_dt)
// calculate a loiter speed limit which is the minimum of the value set by the LOITER_SPEED
// parameter and the value set by the EKF to observe optical flow limits
float gnd_speed_limit_cms = MIN(_speed_cms, ekfGndSpdLimit*100.0f);
// float gnd_speed_limit_cms = MIN(_speed_cms, ekfGndSpdLimit*100.0f);
float gnd_speed_limit_cms = MIN(speed_cms, ekfGndSpdLimit*100.0f);
gnd_speed_limit_cms = MAX(gnd_speed_limit_cms, LOITER_SPEED_MIN);
float pilot_acceleration_max = GRAVITY_MSS*100.0f * tanf(radians(get_angle_max_cd()*0.01f));

5
libraries/AC_WPNav/AC_Loiter.h
Unescape View File

@ -54,6 +54,9 @@ public: @@ -54,6 +54,9 @@ public:
float get_roll() const { return _pos_control.get_roll(); }
float get_pitch() const { return _pos_control.get_pitch(); }
float get_loiter_speed() const { return _speed_cms; }
void set_loiter_speed(float speed) { speed_cms = speed; }
static const struct AP_Param::GroupInfo var_info[];
protected:
@ -86,4 +89,6 @@ protected: @@ -86,4 +89,6 @@ protected:
Vector2f _predicted_euler_rate;
float _brake_timer;
float _brake_accel;
float speed_cms;
};

Write Preview
Loading…
Cancel
Save