diff options
| author | Lorenz Meier <lorenz@px4.io> | 2015-05-07 14:29:26 +0200 |
|---|---|---|
| committer | Lorenz Meier <lorenz@px4.io> | 2015-05-07 14:29:26 +0200 |
| commit | 5003875911b893884deeb97b97191016597c75b7 (patch) | |
| tree | f4d69f2d6b1b2831f528940d4f264e8f767ff438 | |
| parent | ea8ba794812256bc8acf593b0145457a5d5271fd (diff) | |
| parent | 5cf8efcc601466ed455daf41067ad93e79088d83 (diff) | |
| download | px4-firmware-5003875911b893884deeb97b97191016597c75b7.tar.gz px4-firmware-5003875911b893884deeb97b97191016597c75b7.tar.bz2 px4-firmware-5003875911b893884deeb97b97191016597c75b7.zip | |
Merge pull request #2141 from PX4/quat_sp
copy quaternion setpoint into attitude setpoint topic
| -rw-r--r-- | src/modules/mc_pos_control/mc_pos_control_main.cpp | 15 |
1 files changed, 15 insertions, 0 deletions
diff --git a/src/modules/mc_pos_control/mc_pos_control_main.cpp b/src/modules/mc_pos_control/mc_pos_control_main.cpp index 6ffb37d97..0c8b040c1 100644 --- a/src/modules/mc_pos_control/mc_pos_control_main.cpp +++ b/src/modules/mc_pos_control/mc_pos_control_main.cpp @@ -1285,6 +1285,11 @@ MulticopterPositionControl::task_main() memcpy(&_att_sp.R_body[0], R.data, sizeof(_att_sp.R_body)); _att_sp.R_valid = true; + /* copy quaternion setpoint to attitude setpoint topic */ + math::Quaternion q_sp; + q_sp.from_dcm(R); + memcpy(&_att_sp.q_d[0], &q_sp.data[0], sizeof(_att_sp.q_d)); + /* calculate euler angles, for logging only, must not be used for control */ math::Vector<3> euler = R.to_euler(); _att_sp.roll_body = euler(0); @@ -1300,6 +1305,11 @@ MulticopterPositionControl::task_main() memcpy(&_att_sp.R_body[0], R.data, sizeof(_att_sp.R_body)); _att_sp.R_valid = true; + /* copy quaternion setpoint to attitude setpoint topic */ + math::Quaternion q_sp; + q_sp.from_dcm(R); + memcpy(&_att_sp.q_d[0], &q_sp.data[0], sizeof(_att_sp.q_d)); + _att_sp.roll_body = 0.0f; _att_sp.pitch_body = 0.0f; } @@ -1385,6 +1395,11 @@ MulticopterPositionControl::task_main() math::Matrix<3,3> R_sp; R_sp.from_euler(_att_sp.roll_body,_att_sp.pitch_body,_att_sp.yaw_body); memcpy(&_att_sp.R_body[0], R_sp.data, sizeof(_att_sp.R_body)); + + /* copy quaternion setpoint to attitude setpoint topic */ + math::Quaternion q_sp; + q_sp.from_dcm(R_sp); + memcpy(&_att_sp.q_d[0], &q_sp.data[0], sizeof(_att_sp.q_d)); _att_sp.timestamp = hrt_absolute_time(); } else { |