Mirrors_Framework/PX4-Autopilot
1
0
Fork 0
mirror of https://github.com/PX4/PX4-Autopilot.git synced 2026-05-27 02:06:27 +08:00
Code Issues Actions 5 Packages Projects Releases Wiki Activity

Update examples

Browse Source
This commit is contained in:
Lorenz Meier
2016-09-27 11:58:28 +02:00
parent 873ed17b14
commit ac936a28dd
3 changed files with 10 additions and 10 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/examples/attitude_estimator_ekf/attitude_estimator_ekf_main.cpp
+3 -3
View File
@@ -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;
src/examples/mc_att_control_multiplatform/mc_att_control_base.cpp
+4 -4
View File
@@ -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;
src/examples/mc_pos_control_multiplatform/mc_pos_control.cpp
+3 -3
View File
@@ -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) {