Browse Source

AP_NavEKF_Source: add ExtNav yaw source

zr-v5.1
Randy Mackay 4 years ago committed by Andrew Tridgell
parent
commit
fca8eb0778
  1. 9
      libraries/AP_NavEKF/AP_NavEKF_Source.cpp
  2. 3
      libraries/AP_NavEKF/AP_NavEKF_Source.h

9
libraries/AP_NavEKF/AP_NavEKF_Source.cpp

@ -52,7 +52,7 @@ const AP_Param::GroupInfo AP_NavEKF_Source::var_info[] = { @@ -52,7 +52,7 @@ const AP_Param::GroupInfo AP_NavEKF_Source::var_info[] = {
// @Param: 1_YAW
// @DisplayName: Yaw Source
// @Description: Yaw Source
// @Values: 0:None, 1:Compass, 2:External, 3:External with Compass Fallback
// @Values: 0:None, 1:Compass, 2:External, 3:External with Compass Fallback, 6:ExternalNav
// @User: Advanced
AP_GROUPINFO("1_YAW", 5, AP_NavEKF_Source, _source_set[0].yaw, (int8_t)AP_NavEKF_Source::SourceYaw::COMPASS),
@ -88,7 +88,7 @@ const AP_Param::GroupInfo AP_NavEKF_Source::var_info[] = { @@ -88,7 +88,7 @@ const AP_Param::GroupInfo AP_NavEKF_Source::var_info[] = {
// @Param: 2_YAW
// @DisplayName: Yaw Source (Secondary)
// @Description: Yaw Source (Secondary)
// @Values: 0:None, 1:Compass, 2:External, 3:External with Compass Fallback
// @Values: 0:None, 1:Compass, 2:External, 3:External with Compass Fallback, 6:ExternalNav
// @User: Advanced
AP_GROUPINFO("2_YAW", 10, AP_NavEKF_Source, _source_set[1].yaw, (int8_t)AP_NavEKF_Source::SourceYaw::NONE),
#endif
@ -125,7 +125,7 @@ const AP_Param::GroupInfo AP_NavEKF_Source::var_info[] = { @@ -125,7 +125,7 @@ const AP_Param::GroupInfo AP_NavEKF_Source::var_info[] = {
// @Param: 3_YAW
// @DisplayName: Yaw Source (Tertiary)
// @Description: Yaw Source (Tertiary)
// @Values: 0:None, 1:Compass, 2:External, 3:External with Compass Fallback
// @Values: 0:None, 1:Compass, 2:External, 3:External with Compass Fallback, 6:ExternalNav
// @User: Advanced
AP_GROUPINFO("3_YAW", 15, AP_NavEKF_Source, _source_set[2].yaw, (int8_t)AP_NavEKF_Source::SourceYaw::NONE),
#endif
@ -412,6 +412,9 @@ bool AP_NavEKF_Source::pre_arm_check(char *failure_msg, uint8_t failure_msg_len) @@ -412,6 +412,9 @@ bool AP_NavEKF_Source::pre_arm_check(char *failure_msg, uint8_t failure_msg_len)
case SourceYaw::EXTERNAL_COMPASS_FALLBACK:
compass_required = true;
break;
case SourceYaw::EXTNAV:
visualodom_required = true;
break;
default:
// invalid yaw value
hal.util->snprintf(failure_msg, failure_msg_len, "Check EK3_SRC%d_YAW", (int)i+1);

3
libraries/AP_NavEKF/AP_NavEKF_Source.h

@ -41,7 +41,8 @@ public: @@ -41,7 +41,8 @@ public:
NONE = 0,
COMPASS = 1,
EXTERNAL = 2,
EXTERNAL_COMPASS_FALLBACK = 3
EXTERNAL_COMPASS_FALLBACK = 3,
EXTNAV = 6
};
// enum for OPTIONS parameter

Loading…
Cancel
Save