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

AC_Avoidance: use of AP_Proximity checks HAL_PROXIMITY_ENABLED

Browse Source
This commit is contained in:
Randy Mackay 2021-03-25 17:08:05 +09:00
parent b8f9c3b9c0
commit f67cdf5a81
1 changed files with 6 additions and 0 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

6
libraries/AC_Avoidance/AC_Avoid.cpp
View File

@ -361,6 +361,7 @@ void AC_Avoid::adjust_velocity_z(float kP, float accel_cmss, float& climb_rate_c
} }
} }
#if HAL_PROXIMITY_ENABLED
// get distance from proximity sensor // get distance from proximity sensor
float proximity_alt_diff; float proximity_alt_diff;
AP_Proximity *proximity = AP::proximity(); AP_Proximity *proximity = AP::proximity();
@ -371,6 +372,7 @@ void AC_Avoid::adjust_velocity_z(float kP, float accel_cmss, float& climb_rate_c
limit_alt = true; limit_alt = true;
} }
} }
#endif
// limit climb rate // limit climb rate
if (limit_alt) { if (limit_alt) {
@ -1084,6 +1086,7 @@ void AC_Avoid::adjust_velocity_beacon_fence(float kP, float accel_cmss, Vector2f
*/ */
void AC_Avoid::adjust_velocity_proximity(float kP, float accel_cmss, Vector3f &desired_vel_cms, Vector3f &backup_vel, float kP_z, float accel_cmss_z, float dt) void AC_Avoid::adjust_velocity_proximity(float kP, float accel_cmss, Vector3f &desired_vel_cms, Vector3f &backup_vel, float kP_z, float accel_cmss_z, float dt)
{ {
#if HAL_PROXIMITY_ENABLED
// exit immediately if proximity sensor is not present // exit immediately if proximity sensor is not present
AP_Proximity *proximity = AP::proximity(); AP_Proximity *proximity = AP::proximity();
if (!proximity) { if (!proximity) {
@ -1211,6 +1214,7 @@ void AC_Avoid::adjust_velocity_proximity(float kP, float accel_cmss, Vector3f &d
desired_vel_cms = Vector3f{safe_vel_2d.x, safe_vel_2d.y, safe_vel.z}; desired_vel_cms = Vector3f{safe_vel_2d.x, safe_vel_2d.y, safe_vel.z};
const Vector2f backup_vel_xy = _ahrs.body_to_earth2D(desired_back_vel_cms_xy); const Vector2f backup_vel_xy = _ahrs.body_to_earth2D(desired_back_vel_cms_xy);
backup_vel = Vector3f{backup_vel_xy.x, backup_vel_xy.y, desired_back_vel_cms_z}; backup_vel = Vector3f{backup_vel_xy.x, backup_vel_xy.y, desired_back_vel_cms_z};
#endif // HAL_PROXIMITY_ENABLED
} }
/* /*
@ -1371,6 +1375,7 @@ float AC_Avoid::distance_to_lean_pct(float dist_m)
// returns the maximum positive and negative roll and pitch percentages (in -1 ~ +1 range) based on the proximity sensor // returns the maximum positive and negative roll and pitch percentages (in -1 ~ +1 range) based on the proximity sensor
void AC_Avoid::get_proximity_roll_pitch_pct(float &roll_positive, float &roll_negative, float &pitch_positive, float &pitch_negative) void AC_Avoid::get_proximity_roll_pitch_pct(float &roll_positive, float &roll_negative, float &pitch_positive, float &pitch_negative)
{ {
#if HAL_PROXIMITY_ENABLED
AP_Proximity *proximity = AP::proximity(); AP_Proximity *proximity = AP::proximity();
if (proximity == nullptr) { if (proximity == nullptr) {
return; return;
@ -1414,6 +1419,7 @@ void AC_Avoid::get_proximity_roll_pitch_pct(float &roll_positive, float &roll_ne
} }
} }
} }
#endif // HAL_PROXIMITY_ENABLED
} }
// singleton instance // singleton instance