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[])
att.yawspeed = x_aposteriori[2]; att.yawspeed = x_aposteriori[2];
/* magnetic declination */ /* 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]); matrix::Dcm<float> R_declination(&R_decl.data[0][0]);
R = R_declination * R; Ro = R_declination * Ro;
matrix::Quaternion<float> q(R); matrix::Quaternion<float> q(Ro);
memcpy(&att.q[0],&q._data[0],sizeof(att.q)); memcpy(&att.q[0],&q._data[0],sizeof(att.q));
att.q_valid = true; 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)
if (e_R_z_cos < 0.0f) { if (e_R_z_cos < 0.0f) {
/* for large thrust vector rotations use another rotation method: /* for large thrust vector rotations use another rotation method:
* calculate angle and axis for R -> R_sp rotation directly */ * calculate angle and axis for R -> R_sp rotation directly */
math::Quaternion q; math::Quaternion ql;
q.from_dcm(R.transposed() * R_sp); ql.from_dcm(R.transposed() * R_sp);
math::Vector <3> e_R_d = q.imag(); math::Vector <3> e_R_d = ql.imag();
e_R_d.normalize(); 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 */ /* use fusion of Z axis based rotation and direct rotation */
float direct_w = e_R_z_cos * e_R_z_cos * yaw_w; 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
_att_sp_msg.data().R_valid = true; _att_sp_msg.data().R_valid = true;
/* calculate euler angles, for logging only, must not be used for control */ /* calculate euler angles, for logging only, must not be used for control */
math::Vector<3> euler = _R.to_euler(); math::Vector<3> eul = _R.to_euler();
_att_sp_msg.data().roll_body = euler(0); _att_sp_msg.data().roll_body = eul(0);
_att_sp_msg.data().pitch_body = euler(1); _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 */ /* 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) { } else if (!_control_mode->data().flag_control_manual_enabled) {

Loading…
Cancel
Save