diff --git a/src/examples/attitude_estimator_ekf/attitude_estimator_ekf_main.cpp b/src/examples/attitude_estimator_ekf/attitude_estimator_ekf_main.cpp index cf283cba5d..5b69772dac 100755 --- a/src/examples/attitude_estimator_ekf/attitude_estimator_ekf_main.cpp +++ b/src/examples/attitude_estimator_ekf/attitude_estimator_ekf_main.cpp @@ -609,19 +609,8 @@ int attitude_estimator_ekf_thread_main(int argc, char *argv[]) att.rollspeed = x_aposteriori[0]; att.pitchspeed = x_aposteriori[1]; att.yawspeed = x_aposteriori[2]; - // att.rollacc = x_aposteriori[3]; - // att.pitchacc = x_aposteriori[4]; - // att.yawacc = x_aposteriori[5]; - - // att.g_comp[0] = raw.accelerometer_m_s2[0] - acc(0); - // att.g_comp[1] = raw.accelerometer_m_s2[1] - acc(1); - // att.g_comp[2] = raw.accelerometer_m_s2[2] - acc(2); - - // /* copy offsets */ - // memcpy(&att.rate_offsets, &(x_aposteriori[3]), sizeof(att.rate_offsets)); /* magnetic declination */ - matrix::Dcm R(&Rot_matrix[0]); matrix::Dcm R_declination(&R_decl.data[0][0]); R = R_declination * R; diff --git a/src/examples/ekf_att_pos_estimator/ekf_att_pos_estimator_main.cpp b/src/examples/ekf_att_pos_estimator/ekf_att_pos_estimator_main.cpp index 5da78d4e05..044e9767fb 100644 --- a/src/examples/ekf_att_pos_estimator/ekf_att_pos_estimator_main.cpp +++ b/src/examples/ekf_att_pos_estimator/ekf_att_pos_estimator_main.cpp @@ -821,14 +821,6 @@ void AttitudePositionEstimatorEKF::publishAttitude() { // Output results matrix::Quaternion q(_ekf->states[0], _ekf->states[1], _ekf->states[2], _ekf->states[3]); - //math::Matrix<3, 3> R = q.to_dcm(); - //math::Vector<3> euler = R.to_euler(); - - /*for (int i = 0; i < 3; i++) { - for (int j = 0; j < 3; j++) { - PX4_R(_att.R, i, j) = R(i, j); - } - }*/ _att.timestamp = _last_sensor_timestamp; _att.q[0] = _ekf->states[0]; @@ -841,11 +833,6 @@ void AttitudePositionEstimatorEKF::publishAttitude() _att.pitchspeed = _ekf->dAngIMU.y / _ekf->dtIMU - _ekf->states[11] / _ekf->dtIMUfilt; _att.yawspeed = _ekf->dAngIMU.z / _ekf->dtIMU - _ekf->states[12] / _ekf->dtIMUfilt; - // gyro offsets - //_att.rate_offsets[0] = _ekf->states[10] / _ekf->dtIMUfilt; - //_att.rate_offsets[1] = _ekf->states[11] / _ekf->dtIMUfilt; - //_att.rate_offsets[2] = _ekf->states[12] / _ekf->dtIMUfilt; - /* lazily publish the attitude only once available */ if (_att_pub != nullptr) { /* publish the attitude */ diff --git a/src/modules/attitude_estimator_q/attitude_estimator_q_main.cpp b/src/modules/attitude_estimator_q/attitude_estimator_q_main.cpp index 663e49c7a0..8fb97526a5 100644 --- a/src/modules/attitude_estimator_q/attitude_estimator_q_main.cpp +++ b/src/modules/attitude_estimator_q/attitude_estimator_q_main.cpp @@ -472,13 +472,6 @@ void AttitudeEstimatorQ::task_main() att.pitchspeed = _rates(1); att.yawspeed = _rates(2); - //for (int i = 0; i < 3; i++) { - // att.g_comp[i] = _accel(i) - _pos_acc(i); - //} - - ///* copy offsets */ - //memcpy(&att.rate_offsets, _gyro_bias.data, sizeof(att.rate_offsets)); - memcpy(&att.q[0], _q.data, sizeof(att.q)); att.q_valid = true;