dark0707
/
px4-firmware
forked from Archive/PX4-Autopilot
1
0
Fork 0
Code Pull Requests Activity

hacks

This commit is contained in:
Daniel Agar 2023-08-18 20:28:39 -04:00
parent 0c1535f3e4
commit 9a86f520e8
8 changed files with 61 additions and 38 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

1
msg/EstimatorStatusFlags.msg
View File

@ -42,6 +42,7 @@ bool cs_gravity_vector # 34 - true when gravity vector measurements are
bool cs_mag # 35 - true if 3-axis magnetometer measurement fusion (mag states only) is intended
bool cs_ev_yaw_fault # 36 - true when the EV heading has been declared faulty and is no longer being used
bool cs_mag_heading_consistent # 37 - true when the heading obtained from mag data is declared consistent with the filter
bool cs_mocap # 38 - true when motion capture (mocap) fusion is intended
# fault status
uint32 fault_status_changes # number of filter fault status (fs) changes

5
src/modules/ekf2/EKF/common.h
View File

@ -117,7 +117,8 @@ enum HeightSensor : uint8_t {
GNSS = 1,
RANGE = 2,
EV = 3,
UNKNOWN = 4
MOCAP = 4,
UNKNOWN = 5
};
enum class PositionSensor : uint8_t {
@ -601,7 +602,7 @@ union filter_control_status_u {
uint64_t mag : 1; ///< 35 - true if 3-axis magnetometer measurement fusion (mag states only) is intended
uint64_t ev_yaw_fault : 1; ///< 36 - true when the EV heading has been declared faulty and is no longer being used
uint64_t mag_heading_consistent : 1; ///< 37 - true when the heading obtained from mag data is declared consistent with the filter
uint64_t mocap : 1; ///< 38 - true when motion capture (mocap) fusion is intended
} flags;
uint64_t value;
};

4
src/modules/ekf2/EKF/estimator_interface.h
View File

@ -270,6 +270,10 @@ public:
const filter_control_status_u &control_status_prev() const { return _control_status_prev; }
const decltype(filter_control_status_u::flags) &control_status_prev_flags() const { return _control_status_prev.flags; }
// TODO
void enableControlStatusMocap() { _control_status.flags.mocap = true; }
void disableControlStatusMocap() { _control_status.flags.mocap = false; }
// get EKF internal fault status
const fault_status_u &fault_status() const { return _fault_status; }
const decltype(fault_status_u::flags) &fault_status_flags() const { return _fault_status.flags; }

19
src/modules/ekf2/EKF/ev_pos_control.cpp
View File

@ -69,7 +69,7 @@ void Ekf::controlEvPosFusion(const extVisionSample &ev_sample, const bool common
pos = ev_sample.pos - pos_offset_earth;
pos_cov = matrix::diag(ev_sample.position_var);
if (_control_status.flags.gps) {
if (_control_status.flags.gps || _control_status.flags.mocap) {
_ev_pos_b_est.setFusionActive();
} else {
@ -108,7 +108,7 @@ void Ekf::controlEvPosFusion(const extVisionSample &ev_sample, const bool common
pos_cov(i, i) = math::max(pos_cov(i, i), orientation_var_max);
}
if (_control_status.flags.gps) {
if (_control_status.flags.gps || _control_status.flags.mocap) {
_ev_pos_b_est.setFusionActive();
} else {
@ -125,13 +125,6 @@ void Ekf::controlEvPosFusion(const extVisionSample &ev_sample, const bool common
break;
}
// increase minimum variance if GPS active (position reference)
if (_control_status.flags.gps) {
for (int i = 0; i < 2; i++) {
pos_cov(i, i) = math::max(pos_cov(i, i), sq(_params.gps_pos_noise));
}
}
const Vector2f measurement{pos(0), pos(1)};
const Vector2f measurement_var{
@ -200,7 +193,7 @@ void Ekf::startEvPosFusion(const Vector2f &measurement, const Vector2f &measurem
{
// activate fusion
// TODO: (_params.position_sensor_ref == PositionSensor::EV)
if (_control_status.flags.gps) {
if (_control_status.flags.gps || _control_status.flags.mocap) {
ECL_INFO("starting %s fusion", EV_AID_SRC_NAME);
_ev_pos_b_est.setBias(-Vector2f(_state.pos.xy()) + measurement);
_ev_pos_b_est.setFusionActive();
@ -226,7 +219,7 @@ void Ekf::updateEvPosFusion(const Vector2f &measurement, const Vector2f &measure
if (quality_sufficient) {
if (!_control_status.flags.gps) {
if (!_control_status.flags.gps && !_control_status.flags.mocap) {
ECL_INFO("reset to %s", EV_AID_SRC_NAME);
_information_events.flags.reset_pos_to_vision = true;
resetHorizontalPositionTo(measurement, measurement_var);
@ -261,14 +254,14 @@ void Ekf::updateEvPosFusion(const Vector2f &measurement, const Vector2f &measure
// Data seems good, attempt a reset
ECL_WARN("%s fusion failing, resetting", EV_AID_SRC_NAME);
if (_control_status.flags.gps && !pos_xy_fusion_failing) {
if ((_control_status.flags.gps || _control_status.flags.mocap) && !pos_xy_fusion_failing) {
// reset EV position bias
_ev_pos_b_est.setBias(-Vector2f(_state.pos.xy()) + measurement);
} else {
_information_events.flags.reset_pos_to_vision = true;
if (_control_status.flags.gps) {
if (_control_status.flags.gps || _control_status.flags.mocap) {
resetHorizontalPositionTo(measurement - _ev_pos_b_est.getBias(), measurement_var + _ev_pos_b_est.getBiasVar());
_ev_pos_b_est.setBias(-Vector2f(_state.pos.xy()) + measurement);

1
src/modules/ekf2/EKF/ev_yaw_control.cpp
View File

@ -57,6 +57,7 @@ void Ekf::controlEvYawFusion(const extVisionSample &ev_sample, const bool common
bool continuing_conditions_passing = (_params.ev_ctrl & static_cast<int32_t>(EvCtrl::YAW))
&& _control_status.flags.tilt_align
&& !_control_status.flags.ev_yaw_fault
&& !_control_status.flags.mocap
&& PX4_ISFINITE(aid_src.observation)
&& PX4_ISFINITE(aid_src.observation_variance);

47
src/modules/ekf2/EKF/mocap.cpp
View File

@ -62,12 +62,20 @@ void Mocap::update(Ekf &ekf, const estimator::imuSample &imu_delayed)
if (_mocap_buffer.pop_first_older_than(imu_delayed.time_us, &mocap_sample)) {
if (!ekf.global_origin_valid()) {
return;
}
// position
{
bool pos_valid = mocap_sample.pos.isAllFinite();
//bool q_valid = mocap_sample.q.isAllFinite() && (q.min() > 0.f);
// yaw_align = false;
// _height_sensor_ref = HeightSensor::MOCAP;
//_control_status.flags.mocap = true;
// reset position and yaw on init
// position xy
if (pos_valid) {
estimator_aid_source2d_s aid_src{};
const Vector2f pos_obs_var{
@ -82,10 +90,7 @@ void Mocap::update(Ekf &ekf, const estimator::imuSample &imu_delayed)
aid_src);
aid_src.fusion_enabled = _param_ekf2_mocap_ctrl.get() & 0b001; // bit 0 Horizontal position
// TODO: check frame
if (ekf.control_status_flags().yaw_align) {
ekf.fuseHorizontalPosition(aid_src);
}
#if defined(MODULE_NAME)
aid_src.timestamp = hrt_absolute_time();
@ -93,6 +98,32 @@ void Mocap::update(Ekf &ekf, const estimator::imuSample &imu_delayed)
#endif // MODULE_NAME
}
// position z
{
estimator_aid_source1d_s aid_src{};
const float pos_obs_var = math::max(sq(mocap_sample.position_var(2)), sq(_param_ekf2_mocap_noise.get()));
ekf.updateVerticalPositionAidSrcStatus(mocap_sample.time_us,
mocap_sample.pos(2), // observation
pos_obs_var, // observation variance
math::max(_param_ekf2_mocap_gate.get(), 1.f), // innovation gate
aid_src);
if (_param_ekf2_mocap_ctrl.get()) {
aid_src.fusion_enabled = true;
ekf.fuseVerticalPosition(aid_src);
//const bool is_fusion_failing = isTimedOut(aid_src.time_last_fuse, _params.hgt_fusion_timeout_max);
}
#if defined(MODULE_NAME)
aid_src.timestamp = hrt_absolute_time();
_estimator_aid_src_mocap_z_pub.publish(aid_src);
#endif // MODULE_NAME
}
}
}

1
src/modules/ekf2/EKF2.cpp
View File

@ -1774,6 +1774,7 @@ void EKF2::PublishStatusFlags(const hrt_abstime &timestamp)
status_flags.cs_mag = _ekf.control_status_flags().mag;
status_flags.cs_ev_yaw_fault = _ekf.control_status_flags().ev_yaw_fault;
status_flags.cs_mag_heading_consistent = _ekf.control_status_flags().mag_heading_consistent;
status_flags.cs_mocap = _ekf.control_status_flags().mocap;
status_flags.fault_status_changes = _filter_fault_status_changes;
status_flags.fs_bad_mag_x = _ekf.fault_status_flags().bad_mag_x;

11
src/modules/ekf2/ekf2_params.c
View File

@ -1572,17 +1572,8 @@ PARAM_DEFINE_FLOAT(EKF2_GSF_TAS, 15.0f);
/**
* Motion capture (mocap) aiding
*
* Set bits in the following positions to enable:
* 0 : Horizontal position fusion
* 1 : Vertical position fusion
* 2 : Yaw
*
* @group EKF2
* @min 0
* @max 15
* @bit 0 Horizontal position
* @bit 1 Vertical position
* @bit 2 Yaw
* @boolean
*/
PARAM_DEFINE_INT32(EKF2_MOCAP_CTRL, 1);