Skip to content

AP_NavEKF3: Check if rangefinder is in range (if in use by EK3_SRCx_POSZ) #29537

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

Open
rishabsingh3003 wants to merge 5 commits into
base: master
Choose a base branch
from
+65 −29
Open

AP_NavEKF3: Check if rangefinder is in range (if in use by EK3_SRCx_POSZ) #29537

Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
5 commits
Select commit Hold shift + click to select a range
6a23815
AP_NavEKF3: Check if rangefinder is in range if in use
rishabsingh3003 Mar 16, 2025
4bd313e
AP_NavEKF: Improve SourceZ check
rishabsingh3003 Mar 16, 2025
fc83c9e
AP_AHRS: Check for valid height source seperately
rishabsingh3003 Mar 16, 2025
db5047c
ArduCopter: AHRS now checks for valid height source seperately
rishabsingh3003 Mar 16, 2025
3757144
Plane: AHRS now checks for valid height source seperately
rishabsingh3003 Mar 16, 2025
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
5 changes: 4 additions & 1 deletion ArduCopter/AP_Arming_Copter.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -445,10 +445,13 @@ bool AP_Arming_Copter::mandatory_gps_checks(bool display_failure)
// check if flight mode requires GPS
bool mode_requires_gps = copter.flightmode->requires_GPS();

// check if flight mode requires height estimate
const bool requires_height = !copter.flightmode->has_manual_throttle();

// always check if inertial nav has started and is ready
const auto &ahrs = AP::ahrs();
char failure_msg[100] = {};
if (!ahrs.pre_arm_check(mode_requires_gps, failure_msg, sizeof(failure_msg))) {
if (!ahrs.pre_arm_check(mode_requires_gps, failure_msg, sizeof(failure_msg), requires_height)) {
check_failed(display_failure, "AHRS: %s", failure_msg);
return false;
}
Expand Down
2 changes: 1 addition & 1 deletion ArduPlane/AP_Arming_Plane.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -239,7 +239,7 @@ bool AP_Arming_Plane::ins_checks(bool display_failure)
// additional plane specific checks
if (check_enabled(ARMING_CHECK_INS)) {
char failure_msg[50] = {};
if (!AP::ahrs().pre_arm_check(true, failure_msg, sizeof(failure_msg))) {
if (!AP::ahrs().pre_arm_check(true, failure_msg, sizeof(failure_msg), true)) {
check_failed(ARMING_CHECK_INS, display_failure, "AHRS: %s", failure_msg);
return false;
}
Expand Down
5 changes: 3 additions & 2 deletions libraries/AP_AHRS/AP_AHRS.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -2261,7 +2261,8 @@ bool AP_AHRS::healthy(void) const

// returns false if we fail arming checks, in which case the buffer will be populated with a failure message
// requires_position should be true if horizontal position configuration should be checked
bool AP_AHRS::pre_arm_check(bool requires_position, char *failure_msg, uint8_t failure_msg_len) const
// requires_height should be true if height configuration should be checked
bool AP_AHRS::pre_arm_check(bool requires_position, char *failure_msg, uint8_t failure_msg_len, bool requires_height) const
{
bool ret = true;
if (!healthy()) {
Expand Down Expand Up @@ -2322,7 +2323,7 @@ bool AP_AHRS::pre_arm_check(bool requires_position, char *failure_msg, uint8_t f
hal.util->snprintf(failure_msg, failure_msg_len, "EKF3 not started");
return false;
}
return EKF3.pre_arm_check(requires_position, failure_msg, failure_msg_len) && ret;
return EKF3.pre_arm_check(requires_position, requires_height, failure_msg, failure_msg_len) && ret;
#endif
}

Expand Down
2 changes: 1 addition & 1 deletion libraries/AP_AHRS/AP_AHRS.h
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -317,7 +317,7 @@ class AP_AHRS {

// returns false if we fail arming checks, in which case the buffer will be populated with a failure message
// requires_position should be true if horizontal position configuration should be checked
bool pre_arm_check(bool requires_position, char *failure_msg, uint8_t failure_msg_len) const;
bool pre_arm_check(bool requires_position, char *failure_msg, uint8_t failure_msg_len, bool requires_height = false) const;

// true if the AHRS has completed initialisation
bool initialised() const;
Expand Down
42 changes: 22 additions & 20 deletions libraries/AP_NavEKF/AP_NavEKF_Source.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -323,7 +323,7 @@ void AP_NavEKF_Source::mark_configured()

// returns false if we fail arming checks, in which case the buffer will be populated with a failure message
// requires_position should be true if vertical or horizontal position configuration should be checked
bool AP_NavEKF_Source::pre_arm_check(bool requires_position, char *failure_msg, uint8_t failure_msg_len) const
bool AP_NavEKF_Source::pre_arm_check(bool requires_position, bool requires_height, char *failure_msg, uint8_t failure_msg_len) const
{
auto &dal = AP::dal();
bool baro_required = false;
Expand Down Expand Up @@ -383,6 +383,27 @@ bool AP_NavEKF_Source::pre_arm_check(bool requires_position, char *failure_msg,
return false;
}

// check velz
switch ((SourceZ)_source_set[i].velz.get()) {
case SourceZ::NONE:
break;
case SourceZ::GPS:
gps_required = true;
break;
case SourceZ::EXTNAV:
visualodom_required = true;
break;
case SourceZ::BARO:
case SourceZ::RANGEFINDER:
case SourceZ::BEACON:
default:
// invalid velz value
hal.util->snprintf(failure_msg, failure_msg_len, "Check EK3_SRC%d_VELZ", (int)i+1);
return false;
}
}

if (requires_height || requires_position) {
// check posz
switch ((SourceZ)_source_set[i].posz.get()) {
case SourceZ::BARO:
Expand All @@ -407,25 +428,6 @@ bool AP_NavEKF_Source::pre_arm_check(bool requires_position, char *failure_msg,
hal.util->snprintf(failure_msg, failure_msg_len, "Check EK3_SRC%d_POSZ", (int)i+1);
return false;
}

// check velz
switch ((SourceZ)_source_set[i].velz.get()) {
case SourceZ::NONE:
break;
case SourceZ::GPS:
gps_required = true;
break;
case SourceZ::EXTNAV:
visualodom_required = true;
break;
case SourceZ::BARO:
case SourceZ::RANGEFINDER:
case SourceZ::BEACON:
default:
// invalid velz value
hal.util->snprintf(failure_msg, failure_msg_len, "Check EK3_SRC%d_VELZ", (int)i+1);
return false;
}
}

// check yaw
Expand Down
2 changes: 1 addition & 1 deletion libraries/AP_NavEKF/AP_NavEKF_Source.h
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -98,7 +98,7 @@ class AP_NavEKF_Source

// returns false if we fail arming checks, in which case the buffer will be populated with a failure message
// requires_position should be true if horizontal position configuration should be checked
bool pre_arm_check(bool requires_position, char *failure_msg, uint8_t failure_msg_len) const;
bool pre_arm_check(bool requires_position, bool requires_height, char *failure_msg, uint8_t failure_msg_len) const;

// return true if ext nav is enabled on any source
bool ext_nav_enabled(void) const;
Expand Down
33 changes: 31 additions & 2 deletions libraries/AP_NavEKF3/AP_NavEKF3.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -1127,10 +1127,11 @@ bool NavEKF3::healthy(void) const

// returns false if we fail arming checks, in which case the buffer will be populated with a failure message
// requires_position should be true if horizontal position configuration should be checked
bool NavEKF3::pre_arm_check(bool requires_position, char *failure_msg, uint8_t failure_msg_len) const
// requires_height should be true if height configuration should be checked
bool NavEKF3::pre_arm_check(bool requires_position, bool requires_height, char *failure_msg, uint8_t failure_msg_len) const
{
// check source configuration
if (!sources.pre_arm_check(requires_position, failure_msg, failure_msg_len)) {
if (!sources.pre_arm_check(requires_position, requires_height, failure_msg, failure_msg_len)) {
return false;
}

Expand All @@ -1143,6 +1144,34 @@ bool NavEKF3::pre_arm_check(bool requires_position, char *failure_msg, uint8_t f
return false;
}

// check if range finder is enabled and in range
#if AP_RANGEFINDER_ENABLED
if (requires_position || requires_height) {
const AP_NavEKF_Source::SourceZ height_source = sources.getPosZSource();
if (height_source == AP_NavEKF_Source::SourceZ::RANGEFINDER) {
const auto *_rng = dal.rangefinder();
if (_rng == nullptr) {
dal.snprintf(failure_msg, failure_msg_len, "Rangefinder not enabled");
return false;
}
bool rangefinder_healthy = false;
Copy link
Contributor

@tridge tridge Mar 18, 2025

Choose a reason for hiding this comment

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

can't we just check rngValidMeaTime_ms ?

Copy link
Contributor

@tridge tridge Mar 18, 2025

Choose a reason for hiding this comment

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

should be in the new pre_arm check in the backends, so we check all lanes, check it is fusing

for (uint8_t i = 0; i < RANGEFINDER_MAX_INSTANCES; i++) {
auto *backend = _rng->get_backend(i);
if (backend != nullptr && backend->orientation() == ROTATION_PITCH_270) {
if (backend->status() == AP_DAL_RangeFinder::Status::Good) {
rangefinder_healthy = true;
break;
}
}
}
if (!rangefinder_healthy) {
dal.snprintf(failure_msg, failure_msg_len, "Rangefinder not healthy");
return false;
}
}
}
#endif

if (!core) {
dal.snprintf(failure_msg, failure_msg_len, "no EKF3 cores");
return false;
Expand Down
3 changes: 2 additions & 1 deletion libraries/AP_NavEKF3/AP_NavEKF3.h
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -56,7 +56,8 @@ class NavEKF3 {

// returns false if we fail arming checks, in which case the buffer will be populated with a failure message
// requires_position should be true if horizontal position configuration should be checked
bool pre_arm_check(bool requires_position, char *failure_msg, uint8_t failure_msg_len) const;
// requires_height should be true if height configuration should be checked
bool pre_arm_check(bool requires_position, bool requires_height, char *failure_msg, uint8_t failure_msg_len) const;

// returns the index of the primary core
// return -1 if no primary core selected
Expand Down
Loading