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 ee2c882578..5da78d4e05 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 @@ -836,12 +836,6 @@ void AttitudePositionEstimatorEKF::publishAttitude() _att.q[2] = _ekf->states[2]; _att.q[3] = _ekf->states[3]; _att.q_valid = true; - //_att.R_valid = true; - - //_att.timestamp = _last_sensor_timestamp; - //_att.roll = euler(0); - //_att.pitch = euler(1); - //_att.yaw = euler(2); _att.rollspeed = _ekf->dAngIMU.x / _ekf->dtIMU - _ekf->states[10] / _ekf->dtIMUfilt; _att.pitchspeed = _ekf->dAngIMU.y / _ekf->dtIMU - _ekf->states[11] / _ekf->dtIMUfilt; diff --git a/src/examples/px4_simple_app/px4_simple_app.c b/src/examples/px4_simple_app/px4_simple_app.c index ce0718839e..8c92720d20 100644 --- a/src/examples/px4_simple_app/px4_simple_app.c +++ b/src/examples/px4_simple_app/px4_simple_app.c @@ -106,10 +106,9 @@ int px4_simple_app_main(int argc, char *argv[]) (double)raw.accelerometer_m_s2[1], (double)raw.accelerometer_m_s2[2]); - /* set att and publish this information for other apps */ - //att.roll = raw.accelerometer_m_s2[0]; - //att.pitch = raw.accelerometer_m_s2[1]; - //att.yaw = raw.accelerometer_m_s2[2]; + /* set att and publish this information for other apps + the following does not have any meaning, it's just an example + */ att.q[0] = raw.accelerometer_m_s2[0]; att.q[1] = raw.accelerometer_m_s2[1]; att.q[2] = raw.accelerometer_m_s2[2]; 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 7c4d90480e..663e49c7a0 100644 --- a/src/modules/attitude_estimator_q/attitude_estimator_q_main.cpp +++ b/src/modules/attitude_estimator_q/attitude_estimator_q_main.cpp @@ -468,10 +468,6 @@ void AttitudeEstimatorQ::task_main() struct vehicle_attitude_s att = {}; att.timestamp = sensors.timestamp; - //att.roll = euler(0); - //att.pitch = euler(1); - //att.yaw = euler(2); - att.rollspeed = _rates(0); att.pitchspeed = _rates(1); att.yawspeed = _rates(2); @@ -483,11 +479,6 @@ void AttitudeEstimatorQ::task_main() ///* copy offsets */ //memcpy(&att.rate_offsets, _gyro_bias.data, sizeof(att.rate_offsets)); - //Matrix<3, 3> R = _q.to_dcm(); - - ///* copy rotation matrix */ - //memcpy(&att.R[0], R.data, sizeof(att.R)); - //att.R_valid = true; memcpy(&att.q[0], _q.data, sizeof(att.q)); att.q_valid = true; diff --git a/src/modules/ekf2/ekf2_main.cpp b/src/modules/ekf2/ekf2_main.cpp index a4b25bfecb..149ce4d30d 100644 --- a/src/modules/ekf2/ekf2_main.cpp +++ b/src/modules/ekf2/ekf2_main.cpp @@ -721,10 +721,6 @@ void Ekf2::task_main() // generate remaining vehicle attitude data att.timestamp = hrt_absolute_time(); - //matrix::Euler euler(q); - //att.roll = euler(0); - //att.pitch = euler(1); - //att.yaw = euler(2); att.q[0] = q(0); att.q[1] = q(1); diff --git a/src/modules/mavlink/mavlink_receiver.cpp b/src/modules/mavlink/mavlink_receiver.cpp index 43c97dc71b..1a08ae1449 100644 --- a/src/modules/mavlink/mavlink_receiver.cpp +++ b/src/modules/mavlink/mavlink_receiver.cpp @@ -1920,8 +1920,6 @@ MavlinkReceiver::handle_message_hil_state_quaternion(mavlink_message_t *msg) math::Vector<3> euler = C_nb.to_euler(); hil_attitude.timestamp = timestamp; - //memcpy(hil_attitude.R, C_nb.data, sizeof(hil_attitude.R)); - //hil_attitude.R_valid = true; hil_attitude.q[0] = q(0); hil_attitude.q[1] = q(1); @@ -1929,9 +1927,6 @@ MavlinkReceiver::handle_message_hil_state_quaternion(mavlink_message_t *msg) hil_attitude.q[3] = q(3); hil_attitude.q_valid = true; - //hil_attitude.roll = euler(0); - //hil_attitude.pitch = euler(1); - //hil_attitude.yaw = euler(2); hil_attitude.rollspeed = hil_state.rollspeed; hil_attitude.pitchspeed = hil_state.pitchspeed; hil_attitude.yawspeed = hil_state.yawspeed; @@ -1957,7 +1952,7 @@ MavlinkReceiver::handle_message_hil_state_quaternion(mavlink_message_t *msg) hil_global_pos.vel_n = hil_state.vx / 100.0f; hil_global_pos.vel_e = hil_state.vy / 100.0f; hil_global_pos.vel_d = hil_state.vz / 100.0f; - hil_global_pos.yaw = euler(1); + hil_global_pos.yaw = euler(2); hil_global_pos.eph = 2.0f; hil_global_pos.epv = 4.0f; diff --git a/src/modules/position_estimator_inav/position_estimator_inav_main.cpp b/src/modules/position_estimator_inav/position_estimator_inav_main.cpp index a7b9f53f5e..0ccb97d5ff 100644 --- a/src/modules/position_estimator_inav/position_estimator_inav_main.cpp +++ b/src/modules/position_estimator_inav/position_estimator_inav_main.cpp @@ -1337,7 +1337,7 @@ int position_estimator_inav_thread_main(int argc, char *argv[]) local_pos.z = z_est[0]; local_pos.vz = z_est[1]; matrix::Euler euler(R); - local_pos.yaw = euler(0); + local_pos.yaw = euler(2); local_pos.dist_bottom_valid = dist_bottom_valid; local_pos.eph = eph; local_pos.epv = epv;