Mirror
/
ardupilot
mirror of https://github.com/ArduPilot/ardupilot
5
0
Fork 0
Code Issues Packages Projects Releases Wiki Activity

AP_Proximity: Improve get_status handling

This commit is contained in:
rishabsingh3003 2024-03-04 13:54:40 -05:00 committed by Andrew Tridgell
parent 6a87f59d59
commit a42f537e61
2 changed files with 16 additions and 6 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

20
libraries/AP_Proximity/AP_Proximity.cpp
View File

@ -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;
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
@ -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

2
libraries/AP_Proximity/AP_Proximity.h
View File

@ -115,6 +115,8 @@ public:
// 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