Browse Source

Update examples

sbg
Lorenz Meier 8 years ago
parent
commit
ac936a28dd
  1. 6
      src/examples/attitude_estimator_ekf/attitude_estimator_ekf_main.cpp
  2. 8
      src/examples/mc_att_control_multiplatform/mc_att_control_base.cpp
  3. 6
      src/examples/mc_pos_control_multiplatform/mc_pos_control.cpp

6
src/examples/attitude_estimator_ekf/attitude_estimator_ekf_main.cpp

@ -611,10 +611,10 @@ int attitude_estimator_ekf_thread_main(int argc, char *argv[]) @@ -611,10 +611,10 @@ int attitude_estimator_ekf_thread_main(int argc, char *argv[])
att.yawspeed = x_aposteriori[2];
/* magnetic declination */
matrix::Dcm<float> R(&Rot_matrix[0]);
matrix::Dcm<float> Ro(&Rot_matrix[0]);
matrix::Dcm<float> R_declination(&R_decl.data[0][0]);
R = R_declination * R;
matrix::Quaternion<float> q(R);
Ro = R_declination * Ro;
matrix::Quaternion<float> q(Ro);
memcpy(&att.q[0],&q._data[0],sizeof(att.q));
att.q_valid = true;

8
src/examples/mc_att_control_multiplatform/mc_att_control_base.cpp

@ -147,11 +147,11 @@ void MulticopterAttitudeControlBase::control_attitude(float dt) @@ -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;

6
src/examples/mc_pos_control_multiplatform/mc_pos_control.cpp

@ -937,9 +937,9 @@ void MulticopterPositionControlMultiplatform::handle_vehicle_attitude(const px4 @@ -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) {

Loading…
Cancel
Save