diff --git a/ArduCopter/Copter.h b/ArduCopter/Copter.h index 5fd5b015f5..2435b802fc 100644 --- a/ArduCopter/Copter.h +++ b/ArduCopter/Copter.h @@ -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: 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; diff --git a/ArduCopter/UserCode.cpp b/ArduCopter/UserCode.cpp index cf13e7a20f..7cf74ff328 100644 --- a/ArduCopter/UserCode.cpp +++ b/ArduCopter/UserCode.cpp @@ -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 diff --git a/ArduCopter/zr_flight.cpp b/ArduCopter/zr_flight.cpp index ac8f8a7faf..a9e447cbb9 100644 --- a/ArduCopter/zr_flight.cpp +++ b/ArduCopter/zr_flight.cpp @@ -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(){ 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 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 \ No newline at end of file diff --git a/libraries/AC_Avoidance/AC_Avoid.cpp b/libraries/AC_Avoidance/AC_Avoid.cpp index 4fe87078fe..baedfadb9a 100644 --- a/libraries/AC_Avoidance/AC_Avoid.cpp +++ b/libraries/AC_Avoidance/AC_Avoid.cpp @@ -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 }; diff --git a/libraries/AC_Avoidance/AC_Avoid.h b/libraries/AC_Avoidance/AC_Avoid.h index 927b0c9929..8541203dbd 100644 --- a/libraries/AC_Avoidance/AC_Avoid.h +++ b/libraries/AC_Avoidance/AC_Avoid.h @@ -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: 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; diff --git a/libraries/AC_WPNav/AC_Loiter.cpp b/libraries/AC_WPNav/AC_Loiter.cpp index 9143edcb24..708ab6190a 100644 --- a/libraries/AC_WPNav/AC_Loiter.cpp +++ b/libraries/AC_WPNav/AC_Loiter.cpp @@ -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() } // 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() // 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) // 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)); diff --git a/libraries/AC_WPNav/AC_Loiter.h b/libraries/AC_WPNav/AC_Loiter.h index 694d01845b..79e96bbc80 100644 --- a/libraries/AC_WPNav/AC_Loiter.h +++ b/libraries/AC_WPNav/AC_Loiter.h @@ -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: Vector2f _predicted_euler_rate; float _brake_timer; float _brake_accel; + + float speed_cms; };