Skip to content
New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Jump to bottom

AP_Proximity: get_status() returns first good sensor status #25913

Merged
merged 2 commits into from Mar 7, 2024
Merged

AP_Proximity: get_status() returns first good sensor status #25913

Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
2 commits
Select commit Hold shift + click to select a range
45c393a
AP_Proximity: Improve get_status handling
rishabsingh3003 Mar 4, 2024
e65c4c3
AC_Avoid: Remove sensor status checks
rishabsingh3003 Mar 4, 2024
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Jump to
Jump to file
Failed to load files.
Diff view
Diff view
11 changes: 0 additions & 11 deletions libraries/AC_Avoidance/AC_Avoid.cpp
View file
Open in desktop
Expand Up @@ -1139,10 +1139,6 @@ void AC_Avoid::adjust_velocity_proximity(float kP, float accel_cmss, Vector3f &d
}

AP_Proximity &_proximity = *proximity;
// check for status of the sensor
if (_proximity.get_status() != AP_Proximity::Status::Good) {
return;
}
// get total number of obstacles
const uint8_t obstacle_num = _proximity.get_obstacle_count();
if (obstacle_num == 0) {
Expand Down Expand Up @@ -1434,14 +1430,7 @@ void AC_Avoid::get_proximity_roll_pitch_pct(float &roll_positive, float &roll_ne
return;
}
AP_Proximity &_proximity = *proximity;

// exit immediately if proximity sensor is not present
if (_proximity.get_status() != AP_Proximity::Status::Good) {
return;
}

const uint8_t obj_count = _proximity.get_object_count();

// if no objects return
if (obj_count == 0) {
return;
Expand Down
20 changes: 14 additions & 6 deletions libraries/AP_Proximity/AP_Proximity.cpp
View file
Open in desktop
Expand Up @@ -308,17 +308,19 @@ AP_Proximity::Status AP_Proximity::get_instance_status(uint8_t instance) const
return state[instance].status;
}

// Returns status of first good sensor. If no good sensor found, returns status of last instance sensor
AP_Proximity::Status AP_Proximity::get_status() const
{
Status sensors_status = Status::NotConnected;
Copy link
Contributor

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

We don't really need this out here; could have a constant return value and a more-local variable introduced for get_instance_status

Copy link
Contributor Author

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

I kept it that way because the status could be "not connected" or "no data".. I would like the sensor to return the value that is true for a sensor, rather than an arbitrary constant

for (uint8_t i=0; i<num_instances; i++) {
const Status sensors_status = get_instance_status(i);
if (sensors_status != Status::Good) {
// return first bad status
sensors_status = get_instance_status(i);
if (sensors_status == Status::Good) {
// return first good status
return sensors_status;
}
}
// All valid sensors seem to be working
return Status::Good;
// no good sensor found
return sensors_status;
}

// return proximity backend for Lua scripting
Expand Down Expand Up @@ -459,7 +461,13 @@ bool AP_Proximity::sensor_enabled() const

bool AP_Proximity::sensor_failed() const
{
return get_status() != Status::Good;
for (uint8_t i=0; i<num_instances; i++) {
if (get_instance_status(i) != Status::Good) {
// return first bad status
return true;
}
}
return false;
}

// get distance in meters upwards, returns true on success
Expand Down
2 changes: 2 additions & 0 deletions libraries/AP_Proximity/AP_Proximity.h
View file
Open in desktop
Expand Up @@ -115,6 +115,8 @@ class AP_Proximity

// return sensor health
Status get_instance_status(uint8_t instance) const;

// Returns status of first good sensor. If no good sensor found, returns status of last instance sensor
Status get_status() const;

// prearm checks
Expand Down