Mirror/ardupilot
5
0
Fork 0
mirror of https://github.com/ArduPilot/ardupilot synced 2025-01-03 14:38:30 -04:00
Code Issues Packages Projects Releases Wiki Activity

AP_NavEKF_Source; pre_arm_check may skip position checks

Browse Source
This commit is contained in:
Randy Mackay 2021-01-21 12:42:47 +09:00 committed by Andrew Tridgell
parent 62932f884f
commit 8ecac27777
2 changed files with 88 additions and 84 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

169
libraries/AP_NavEKF/AP_NavEKF_Source.cpp
View File

@ -299,7 +299,8 @@ void AP_NavEKF_Source::mark_configured_in_storage()
}
// returns false if we fail arming checks, in which case the buffer will be populated with a failure message
bool AP_NavEKF_Source::pre_arm_check(char *failure_msg, uint8_t failure_msg_len) const
// requires_position should be true if 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
{
auto &dal = AP::dal();
bool baro_required = false;
@ -314,91 +315,93 @@ bool AP_NavEKF_Source::pre_arm_check(char *failure_msg, uint8_t failure_msg_len)
// check source params are valid
for (uint8_t i=0; i<AP_NAKEKF_SOURCE_SET_MAX; i++) {
// check posxy
switch ((SourceXY)_source_set[i].posxy.get()) {
case SourceXY::NONE:
break;
case SourceXY::GPS:
gps_required = true;
break;
case SourceXY::BEACON:
beacon_required = true;
break;
case SourceXY::EXTNAV:
visualodom_required = true;
break;
case SourceXY::OPTFLOW:
case SourceXY::WHEEL_ENCODER:
default:
// invalid posxy value
hal.util->snprintf(failure_msg, failure_msg_len, "Check EK3_SRC%d_POSXY", (int)i+1);
return false;
}
if (requires_position) {
// check posxy
switch ((SourceXY)_source_set[i].posxy.get()) {
case SourceXY::NONE:
break;
case SourceXY::GPS:
gps_required = true;
break;
case SourceXY::BEACON:
beacon_required = true;
break;
case SourceXY::EXTNAV:
visualodom_required = true;
break;
case SourceXY::OPTFLOW:
case SourceXY::WHEEL_ENCODER:
default:
// invalid posxy value
hal.util->snprintf(failure_msg, failure_msg_len, "Check EK3_SRC%d_POSXY", (int)i+1);
return false;
}
// check velxy
switch ((SourceXY)_source_set[i].velxy.get()) {
case SourceXY::NONE:
break;
case SourceXY::GPS:
gps_required = true;
break;
case SourceXY::OPTFLOW:
optflow_required = true;
break;
case SourceXY::EXTNAV:
visualodom_required = true;
break;
case SourceXY::WHEEL_ENCODER:
wheelencoder_required = true;
break;
case SourceXY::BEACON:
default:
// invalid velxy value
hal.util->snprintf(failure_msg, failure_msg_len, "Check EK3_SRC%d_VELXY", (int)i+1);
return false;
}
// check velxy
switch ((SourceXY)_source_set[i].velxy.get()) {
case SourceXY::NONE:
break;
case SourceXY::GPS:
gps_required = true;
break;
case SourceXY::OPTFLOW:
optflow_required = true;
break;
case SourceXY::EXTNAV:
visualodom_required = true;
break;
case SourceXY::WHEEL_ENCODER:
wheelencoder_required = true;
break;
case SourceXY::BEACON:
default:
// invalid velxy value
hal.util->snprintf(failure_msg, failure_msg_len, "Check EK3_SRC%d_VELXY", (int)i+1);
return false;
}
// check posz
switch ((SourceZ)_source_set[i].posz.get()) {
case SourceZ::BARO:
baro_required = true;
break;
case SourceZ::RANGEFINDER:
rangefinder_required = true;
break;
case SourceZ::GPS:
gps_required = true;
break;
case SourceZ::BEACON:
beacon_required = true;
break;
case SourceZ::EXTNAV:
visualodom_required = true;
break;
case SourceZ::NONE:
default:
// invalid posz value
hal.util->snprintf(failure_msg, failure_msg_len, "Check EK3_SRC%d_POSZ", (int)i+1);
return false;
}
// check posz
switch ((SourceZ)_source_set[i].posz.get()) {
case SourceZ::BARO:
baro_required = true;
break;
case SourceZ::RANGEFINDER:
rangefinder_required = true;
break;
case SourceZ::GPS:
gps_required = true;
break;
case SourceZ::BEACON:
beacon_required = true;
break;
case SourceZ::EXTNAV:
visualodom_required = true;
break;
case SourceZ::NONE:
default:
// invalid posz value
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 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

3
libraries/AP_NavEKF/AP_NavEKF_Source.h
View File

@ -90,7 +90,8 @@ public:
void mark_configured_in_storage();
// returns false if we fail arming checks, in which case the buffer will be populated with a failure message
bool pre_arm_check(char *failure_msg, uint8_t failure_msg_len) const;
// 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;
// return true if ext nav is enabled on any source
bool ext_nav_enabled(void) const;