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

EKF: remove PX4 dependant text output

This commit is contained in:
Paul Riseborough 2016-04-21 08:55:53 +10:00
parent ac9b7a3df6
commit 349c731375
2 changed files with 0 additions and 5 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

3
EKF/ekf.cpp
View File

@ -313,7 +313,6 @@ bool Ekf::update()
// correct position and height for offset relative to IMU
Vector3f pos_offset_body = _params.ev_pos_body - _params.imu_pos_body;
Vector3f pos_offset_earth = _R_to_earth * pos_offset_body;
warnx("pos_offset_earth %lf %lf %lf", (double)pos_offset_earth(0)*100, (double)pos_offset_earth(1)*100, (double)pos_offset_earth(2)*100);
_ev_sample_delayed.posNED(0) -= pos_offset_earth(0);
_ev_sample_delayed.posNED(1) -= pos_offset_earth(1);
_ev_sample_delayed.posNED(2) -= pos_offset_earth(2);
@ -407,8 +406,6 @@ bool Ekf::initialiseFilter(void)
}
}
// warnx("_params.fusion_mode & MASK_USE_EVPOS %d", (int)(_params.fusion_mode & MASK_USE_EVPOS));
// warnx("_primary_hgt_source == VDIST_SENSOR_EV %d", (int)(_primary_hgt_source == VDIST_SENSOR_EV));
// set the default height source from the adjustable parameter
if (_hgt_counter == 0) {
if (_params.fusion_mode & MASK_USE_EVPOS) {

2
EKF/vel_pos_fusion.cpp
View File

@ -105,7 +105,6 @@ void Ekf::fuseVelPosHeight()
_vel_pos_innov[4] = _state.pos(1) - _gps_sample_delayed.pos(1);
} else if (_control_status.flags.ev_pos) {
// warnx("Fusing EV pos");
R[3] = fmaxf(_ev_sample_delayed.posErr, 0.01f);
_vel_pos_innov[3] = _state.pos(0) - _ev_sample_delayed.posNED(0);
_vel_pos_innov[4] = _state.pos(1) - _ev_sample_delayed.posNED(1);
@ -154,7 +153,6 @@ void Ekf::fuseVelPosHeight()
// innovation gate size
gate_size[5] = fmaxf(_params.range_innov_gate, 1.0f);
} else if (_control_status.flags.ev_pos) {
// warnx("fusing ext_visn_hight");
fuse_map[5] = true;
// calculate the innovation assuming the external vision observaton is in local NED frame
_vel_pos_innov[5] = _state.pos(2) - _ev_sample_delayed.posNED(2);