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

AP_RangeFinder: Have special handling for NRA24 pre-arm checks

This commit is contained in:
rishabsingh3003 2023-07-11 17:19:53 +05:30 committed by Andrew Tridgell
parent ba08993a0b
commit 747c708dba
2 changed files with 23 additions and 6 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

19
libraries/AP_RangeFinder/AP_RangeFinder.cpp
View File

@ -794,6 +794,7 @@ bool RangeFinder::prearm_healthy(char *failure_msg, const uint8_t failure_msg_le
// backend-specific checks. This might end up drivers[i]->arming_checks(...).
switch (drivers[i]->allocated_type()) {
#if AP_RANGEFINDER_PWM_ENABLED || AP_RANGEFINDER_ANALOG_ENABLED
case Type::ANALOG:
case Type::PX4_PWM:
case Type::PWM: {
@ -819,7 +820,21 @@ bool RangeFinder::prearm_healthy(char *failure_msg, const uint8_t failure_msg_le
}
break;
}
default:
#endif
#if AP_RANGEFINDER_NRA24_CAN_ENABLED
case Type::NRA24_CAN: {
if (drivers[i]->status() == Status::NoData) {
// This sensor stops sending data if there is no relative motion. This will mostly happen during takeoff, before arming
// To avoid pre-arm failure, return true even though there is no data.
// This sensor also sends a "heartbeat" so we can differentiate between "NoData" and "NotConnected"
return true;
}
break;
}
#endif
default:
break;
}
@ -832,7 +847,7 @@ default:
return false;
case Status::OutOfRangeLow:
case Status::OutOfRangeHigh:
case Status::Good:
case Status::Good:
break;
}
}

10
libraries/AP_RangeFinder/AP_RangeFinder_NRA24_CAN.cpp
View File

@ -36,11 +36,13 @@ void AP_RangeFinder_NRA24_CAN::update(void)
state.last_reading_ms = AP_HAL::millis();
update_status();
} else if (AP_HAL::millis() - state.last_reading_ms > read_timeout_ms()) {
if (AP_HAL::millis() - last_heartbeat_ms < read_timeout_ms()) {
// don't have distance data but sensor is connected. This is a known issue when sensor is in static condition (example Copter waiting to take off). Set status to out of range to avoid pre arm error
set_status(RangeFinder::Status::OutOfRangeLow);
} else {
if (AP_HAL::millis() - last_heartbeat_ms > read_timeout_ms()) {
// no heartbeat, must be disconnected
set_status(RangeFinder::Status::NotConnected);
} else {
// Have heartbeat, just no data. Probably because this sensor doesn't output data when there is no relative motion infront of the radar.
// This case has special pre-arm check handling
set_status(RangeFinder::Status::NoData);
}
}
}