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

AP_VisualOdom: pre-arm failure string prefix moved to AP_Arming

Browse Source
This commit is contained in:
Randy Mackay 2020-04-07 13:12:15 +09:00
parent 3d75dc7815
commit aa720b0ae6
2 changed files with 8 additions and 8 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

6
libraries/AP_VisualOdom/AP_VisualOdom.cpp
View File

@ -76,7 +76,7 @@ AP_VisualOdom::AP_VisualOdom()
AP_Param::setup_object_defaults(this, var_info);
#if CONFIG_HAL_BOARD == HAL_BOARD_SITL
if (_singleton != nullptr) {
AP_HAL::panic("VisualOdom must be singleton");
AP_HAL::panic("must be singleton");
}
#endif
_singleton = this;
@ -188,13 +188,13 @@ bool AP_VisualOdom::pre_arm_check(char *failure_msg, uint8_t failure_msg_len) co
// check healthy
if (!healthy()) {
hal.util->snprintf(failure_msg, failure_msg_len, "VisualOdom not healthy");
hal.util->snprintf(failure_msg, failure_msg_len, "not healthy");
return false;
}
// if no backend we must have failed to create because out of memory
if (_driver == nullptr) {
hal.util->snprintf(failure_msg, failure_msg_len, "VisualOdom out of memory");
hal.util->snprintf(failure_msg, failure_msg_len, "out of memory");
return false;
}

10
libraries/AP_VisualOdom/AP_VisualOdom_IntelT265.cpp
View File

@ -123,7 +123,7 @@ bool AP_VisualOdom_IntelT265::align_sensor_to_vehicle(const Vector3f &position,
Vector3f angle_diff;
ahrs_quat.angular_difference(att_corrected).to_axis_angle(angle_diff);
_yaw_trim = angle_diff.z;
gcs().send_text(MAV_SEVERITY_CRITICAL, "VisOdom: yaw shifted %d to %d deg",
gcs().send_text(MAV_SEVERITY_CRITICAL, "VisualOdom: yaw shifted %d to %d deg",
//(int)degrees(_yaw_trim - yaw_trim_orig),
(int)degrees(_yaw_trim),
(int)degrees(sens_yaw + _yaw_trim));
@ -157,7 +157,7 @@ bool AP_VisualOdom_IntelT265::pre_arm_check(char *failure_msg, uint8_t failure_m
{
// exit immediately if not healthy
if (!healthy()) {
hal.util->snprintf(failure_msg, failure_msg_len, "VisualOdom not healthy");
hal.util->snprintf(failure_msg, failure_msg_len, "not healthy");
return false;
}
@ -170,7 +170,7 @@ bool AP_VisualOdom_IntelT265::pre_arm_check(char *failure_msg, uint8_t failure_m
// get ahrs attitude
Quaternion ahrs_quat;
if (!AP::ahrs().get_quaternion(ahrs_quat)) {
hal.util->snprintf(failure_msg, failure_msg_len, "VisualOdom waiting for AHRS attitude");
hal.util->snprintf(failure_msg, failure_msg_len, "waiting for AHRS attitude");
return false;
}
@ -181,14 +181,14 @@ bool AP_VisualOdom_IntelT265::pre_arm_check(char *failure_msg, uint8_t failure_m
// check if roll and pitch is different by > 10deg (using NED so cannot determine whether roll or pitch specifically)
const float rp_diff_deg = degrees(safe_sqrt(sq(angle_diff.x)+sq(angle_diff.y)));
if (rp_diff_deg > 10.0f) {
hal.util->snprintf(failure_msg, failure_msg_len, "VisualOdom roll/pitch diff %4.1f deg (>10)",(double)rp_diff_deg);
hal.util->snprintf(failure_msg, failure_msg_len, "roll/pitch diff %4.1f deg (>10)",(double)rp_diff_deg);
return false;
}
// check if yaw is different by > 10deg
const float yaw_diff_deg = degrees(fabsf(angle_diff.z));
if (yaw_diff_deg > 10.0f) {
hal.util->snprintf(failure_msg, failure_msg_len, "VisualOdom yaw diff %4.1f deg (>10)",(double)yaw_diff_deg);
hal.util->snprintf(failure_msg, failure_msg_len, "yaw diff %4.1f deg (>10)",(double)yaw_diff_deg);
return false;
}