From ac936a28ddc211ed3a9d436a3af0ed29fbe7499e Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Tue, 27 Sep 2016 11:58:28 +0200 Subject: [PATCH] Update examples --- .../attitude_estimator_ekf_main.cpp | 6 +++--- .../mc_att_control_multiplatform/mc_att_control_base.cpp | 8 ++++---- .../mc_pos_control_multiplatform/mc_pos_control.cpp | 6 +++--- 3 files changed, 10 insertions(+), 10 deletions(-) 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 5b69772dac..f27e7697b1 100755 --- a/src/examples/attitude_estimator_ekf/attitude_estimator_ekf_main.cpp +++ b/src/examples/attitude_estimator_ekf/attitude_estimator_ekf_main.cpp @@ -611,10 +611,10 @@ int attitude_estimator_ekf_thread_main(int argc, char *argv[]) att.yawspeed = x_aposteriori[2]; /* magnetic declination */ - matrix::Dcm R(&Rot_matrix[0]); + matrix::Dcm Ro(&Rot_matrix[0]); matrix::Dcm R_declination(&R_decl.data[0][0]); - R = R_declination * R; - matrix::Quaternion q(R); + Ro = R_declination * Ro; + matrix::Quaternion q(Ro); memcpy(&att.q[0],&q._data[0],sizeof(att.q)); att.q_valid = true; diff --git a/src/examples/mc_att_control_multiplatform/mc_att_control_base.cpp b/src/examples/mc_att_control_multiplatform/mc_att_control_base.cpp index f5697fda03..8a1b4968e4 100644 --- a/src/examples/mc_att_control_multiplatform/mc_att_control_base.cpp +++ b/src/examples/mc_att_control_multiplatform/mc_att_control_base.cpp @@ -147,11 +147,11 @@ void MulticopterAttitudeControlBase::control_attitude(float dt) if (e_R_z_cos < 0.0f) { /* for large thrust vector rotations use another rotation method: * calculate angle and axis for R -> R_sp rotation directly */ - math::Quaternion q; - q.from_dcm(R.transposed() * R_sp); - math::Vector <3> e_R_d = q.imag(); + math::Quaternion ql; + ql.from_dcm(R.transposed() * R_sp); + math::Vector <3> e_R_d = ql.imag(); e_R_d.normalize(); - e_R_d *= 2.0f * atan2f(e_R_d.length(), q(0)); + e_R_d *= 2.0f * atan2f(e_R_d.length(), ql(0)); /* use fusion of Z axis based rotation and direct rotation */ float direct_w = e_R_z_cos * e_R_z_cos * yaw_w; diff --git a/src/examples/mc_pos_control_multiplatform/mc_pos_control.cpp b/src/examples/mc_pos_control_multiplatform/mc_pos_control.cpp index 3785bd33df..a2612b8919 100644 --- a/src/examples/mc_pos_control_multiplatform/mc_pos_control.cpp +++ b/src/examples/mc_pos_control_multiplatform/mc_pos_control.cpp @@ -937,9 +937,9 @@ void MulticopterPositionControlMultiplatform::handle_vehicle_attitude(const px4 _att_sp_msg.data().R_valid = true; /* calculate euler angles, for logging only, must not be used for control */ - math::Vector<3> euler = _R.to_euler(); - _att_sp_msg.data().roll_body = euler(0); - _att_sp_msg.data().pitch_body = euler(1); + math::Vector<3> eul = _R.to_euler(); + _att_sp_msg.data().roll_body = eul(0); + _att_sp_msg.data().pitch_body = eul(1); /* yaw already used to construct rot matrix, but actual rotation matrix can have different yaw near singularity */ } else if (!_control_mode->data().flag_control_manual_enabled) {