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

AP_Rangefinder: fail RF prearm checks for NoData/NotConnected statuses 

Earlier we only failed RF checks if we could not detect an RF instance but we should also fail it if the RF is not connected or we receive no data.
This commit is contained in:
Shiv Tyagi 2021-11-05 22:55:51 +05:30 committed by Randy Mackay
parent c83ff858b8
commit 516eafa45b
1 changed files with 20 additions and 3 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

23
libraries/AP_RangeFinder/AP_RangeFinder.cpp
View File

@ -796,9 +796,26 @@ void RangeFinder::Log_RFND() const
bool RangeFinder::prearm_healthy(char *failure_msg, const uint8_t failure_msg_len) const
{
for (uint8_t i = 0; i < RANGEFINDER_MAX_INSTANCES; i++) {
if (((Type)params[i].type.get() != Type::NONE) && (drivers[i] == nullptr)) {
hal.util->snprintf(failure_msg, failure_msg_len, "Rangefinder %X was not detected", i + 1);
return false;
if ((Type)params[i].type.get() == Type::NONE) {
continue;
}
if (drivers[i] == nullptr) {
hal.util->snprintf(failure_msg, failure_msg_len, "Rangefinder %X: Not Detected", i + 1);
return false;
}
switch (drivers[i]->status()) {
case Status::NoData:
hal.util->snprintf(failure_msg, failure_msg_len, "Rangefinder %X: No Data", i + 1);
return false;
case Status::NotConnected:
hal.util->snprintf(failure_msg, failure_msg_len, "Rangefinder %X: Not Connected", i + 1);
return false;
case Status::OutOfRangeLow:
case Status::OutOfRangeHigh:
case Status::Good:
break;
}
}