Mirror/ardupilot
5
0
Fork 0
mirror of https://github.com/ArduPilot/ardupilot synced 2025-01-03 06:28:27 -04:00
Code Issues Packages Projects Releases Wiki Activity

Rover: cope with AP_AVOIDANCE_ENABLED being false

Browse Source
This commit is contained in:
Peter Barker 2024-03-09 18:49:48 +11:00 committed by Peter Barker
parent f4bb15d8cd
commit 97b9c5d4b5
3 changed files with 12 additions and 0 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

4
Rover/Parameters.cpp
View File

@ -487,9 +487,11 @@ const AP_Param::GroupInfo ParametersG2::var_info[] = {
AP_SUBGROUPINFO(proximity, "PRX", 18, ParametersG2, AP_Proximity), AP_SUBGROUPINFO(proximity, "PRX", 18, ParametersG2, AP_Proximity),
#endif #endif
#if AP_AVOIDANCE_ENABLED
// @Group: AVOID_ // @Group: AVOID_
// @Path: ../libraries/AC_Avoidance/AC_Avoid.cpp // @Path: ../libraries/AC_Avoidance/AC_Avoid.cpp
AP_SUBGROUPINFO(avoid, "AVOID_", 19, ParametersG2, AC_Avoid), AP_SUBGROUPINFO(avoid, "AVOID_", 19, ParametersG2, AC_Avoid),
#endif
// 20 was PIVOT_TURN_RATE and should not be re-used // 20 was PIVOT_TURN_RATE and should not be re-used
@ -741,7 +743,9 @@ ParametersG2::ParametersG2(void)
#if HAL_PROXIMITY_ENABLED #if HAL_PROXIMITY_ENABLED
proximity(), proximity(),
#endif #endif
#if AP_AVOIDANCE_ENABLED
avoid(), avoid(),
#endif
#if AP_FOLLOW_ENABLED #if AP_FOLLOW_ENABLED
follow(), follow(),
#endif #endif

2
Rover/Parameters.h
View File

@ -345,8 +345,10 @@ public:
class ModeDock *mode_dock_ptr; class ModeDock *mode_dock_ptr;
#endif #endif
#if AP_AVOIDANCE_ENABLED
// avoidance library // avoidance library
AC_Avoid avoid; AC_Avoid avoid;
#endif
// pitch angle at 100% throttle // pitch angle at 100% throttle
AP_Float bal_pitch_max; AP_Float bal_pitch_max;

6
Rover/mode.cpp
View File

@ -288,6 +288,7 @@ void Mode::calc_throttle(float target_speed, bool avoidance_enabled)
// get acceleration limited target speed // get acceleration limited target speed
target_speed = attitude_control.get_desired_speed_accel_limited(target_speed, rover.G_Dt); target_speed = attitude_control.get_desired_speed_accel_limited(target_speed, rover.G_Dt);
#if AP_AVOIDANCE_ENABLED
// apply object avoidance to desired speed using half vehicle's maximum deceleration // apply object avoidance to desired speed using half vehicle's maximum deceleration
if (avoidance_enabled) { if (avoidance_enabled) {
g2.avoid.adjust_speed(0.0f, 0.5f * attitude_control.get_decel_max(), ahrs.get_yaw(), target_speed, rover.G_Dt); g2.avoid.adjust_speed(0.0f, 0.5f * attitude_control.get_decel_max(), ahrs.get_yaw(), target_speed, rover.G_Dt);
@ -298,6 +299,7 @@ void Mode::calc_throttle(float target_speed, bool avoidance_enabled)
} }
} }
} }
#endif // AP_AVOIDANCE_ENABLED
// call throttle controller and convert output to -100 to +100 range // call throttle controller and convert output to -100 to +100 range
float throttle_out = 0.0f; float throttle_out = 0.0f;
@ -425,11 +427,13 @@ void Mode::navigate_to_waypoint()
g2.wp_nav.update(rover.G_Dt); g2.wp_nav.update(rover.G_Dt);
_distance_to_destination = g2.wp_nav.get_distance_to_destination(); _distance_to_destination = g2.wp_nav.get_distance_to_destination();
#if AP_AVOIDANCE_ENABLED
// sailboats trigger tack if simple avoidance becomes active // sailboats trigger tack if simple avoidance becomes active
if (g2.sailboat.tack_enabled() && g2.avoid.limits_active()) { if (g2.sailboat.tack_enabled() && g2.avoid.limits_active()) {
// we are a sailboat trying to avoid fence, try a tack // we are a sailboat trying to avoid fence, try a tack
rover.control_mode->handle_tack_request(); rover.control_mode->handle_tack_request();
} }
#endif
// pass desired speed to throttle controller // pass desired speed to throttle controller
// do not do simple avoidance because this is already handled in the position controller // do not do simple avoidance because this is already handled in the position controller
@ -447,9 +451,11 @@ void Mode::navigate_to_waypoint()
float desired_turn_rate_rads = g2.wp_nav.get_turn_rate_rads(); float desired_turn_rate_rads = g2.wp_nav.get_turn_rate_rads();
// if simple avoidance is active at very low speed do not attempt to turn // if simple avoidance is active at very low speed do not attempt to turn
#if AP_AVOIDANCE_ENABLED
if (g2.avoid.limits_active() && (fabsf(attitude_control.get_desired_speed()) <= attitude_control.get_stop_speed())) { if (g2.avoid.limits_active() && (fabsf(attitude_control.get_desired_speed()) <= attitude_control.get_stop_speed())) {
desired_turn_rate_rads = 0.0f; desired_turn_rate_rads = 0.0f;
} }
#endif
// call turn rate steering controller // call turn rate steering controller
calc_steering_from_turn_rate(desired_turn_rate_rads); calc_steering_from_turn_rate(desired_turn_rate_rads);