Browse Source

remove comments

sbg
tumbili 9 years ago committed by Lorenz Meier
parent
commit
0d0fa133e6
  1. 11
      src/examples/attitude_estimator_ekf/attitude_estimator_ekf_main.cpp
  2. 13
      src/examples/ekf_att_pos_estimator/ekf_att_pos_estimator_main.cpp
  3. 7
      src/modules/attitude_estimator_q/attitude_estimator_q_main.cpp

11
src/examples/attitude_estimator_ekf/attitude_estimator_ekf_main.cpp

@ -609,19 +609,8 @@ int attitude_estimator_ekf_thread_main(int argc, char *argv[]) @@ -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<float> R(&Rot_matrix[0]);
matrix::Dcm<float> R_declination(&R_decl.data[0][0]);
R = R_declination * R;

13
src/examples/ekf_att_pos_estimator/ekf_att_pos_estimator_main.cpp

@ -821,14 +821,6 @@ void AttitudePositionEstimatorEKF::publishAttitude() @@ -821,14 +821,6 @@ void AttitudePositionEstimatorEKF::publishAttitude()
{
// Output results
matrix::Quaternion<float> 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() @@ -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 */

7
src/modules/attitude_estimator_q/attitude_estimator_q_main.cpp

@ -472,13 +472,6 @@ void AttitudeEstimatorQ::task_main() @@ -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;

Loading…
Cancel
Save