aepko/Ardupilot2
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

AP_NavEKF_Source: prearm check of wheelencoders

Browse Source
This commit is contained in:
Randy Mackay 2020-11-27 17:21:34 +09:00
parent 7d319f8059
commit 7f15b7b126
1 changed files with 7 additions and 1 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

8
libraries/AP_NavEKF/AP_NavEKF_Source.cpp
View File

@ -323,6 +323,7 @@ bool AP_NavEKF_Source::pre_arm_check(char *failure_msg, uint8_t failure_msg_len)
bool rangefinder_required = false;
bool visualodom_required = false;
bool optflow_required = false;
bool wheelencoder_required = false;
// check source params are valid
for (uint8_t i=0; i<AP_NAKEKF_SOURCE_SET_MAX; i++) {
@ -362,7 +363,7 @@ bool AP_NavEKF_Source::pre_arm_check(char *failure_msg, uint8_t failure_msg_len)
visualodom_required = true;
break;
case SourceXY::WHEEL_ENCODER:
// ToDo: add wheelencoder_required and test below
wheelencoder_required = true;
break;
case SourceXY::BEACON:
default:
@ -475,6 +476,11 @@ bool AP_NavEKF_Source::pre_arm_check(char *failure_msg, uint8_t failure_msg_len)
}
}
if (wheelencoder_required && !dal.wheelencoder_enabled()) {
hal.util->snprintf(failure_msg, failure_msg_len, ekf_requires_msg, "WheelEncoder");
return false;
}
return true;
}