diff options
author | Lorenz Meier <lm@inf.ethz.ch> | 2014-01-02 22:17:16 +0100 |
---|---|---|
committer | Lorenz Meier <lm@inf.ethz.ch> | 2014-01-02 22:17:16 +0100 |
commit | 8ad4b72fc76d702e7eac27c0aecfdb2885a8331d (patch) | |
tree | 41b1db2db95fdbfc4066aedbab5aec80036a2e59 /src/modules/fw_att_pos_estimator/fw_att_pos_estimator_main.cpp | |
parent | 2bf4e7912436e3fde31567a9419a5d861d58b6c5 (diff) | |
download | px4-firmware-8ad4b72fc76d702e7eac27c0aecfdb2885a8331d.tar.gz px4-firmware-8ad4b72fc76d702e7eac27c0aecfdb2885a8331d.tar.bz2 px4-firmware-8ad4b72fc76d702e7eac27c0aecfdb2885a8331d.zip |
Compile fixes, publishing estimated attitude now
Diffstat (limited to 'src/modules/fw_att_pos_estimator/fw_att_pos_estimator_main.cpp')
-rw-r--r-- | src/modules/fw_att_pos_estimator/fw_att_pos_estimator_main.cpp | 44 |
1 files changed, 34 insertions, 10 deletions
diff --git a/src/modules/fw_att_pos_estimator/fw_att_pos_estimator_main.cpp b/src/modules/fw_att_pos_estimator/fw_att_pos_estimator_main.cpp index 1ed526e83..5de97f9b9 100644 --- a/src/modules/fw_att_pos_estimator/fw_att_pos_estimator_main.cpp +++ b/src/modules/fw_att_pos_estimator/fw_att_pos_estimator_main.cpp @@ -577,20 +577,44 @@ FixedwingEstimator::task_main() if (_initialized) { math::Quaternion q(states[0], states[1], states[2], states[3]); + math::Dcm R(q); + math::EulerAngles euler(R); - _att.q = q; + for (int i = 0; i < 3; i++) for (int j = 0; j < 3; j++) + _att.R[i][j] = R(i, j); + + _att.timestamp = _gyro.timestamp; + _att.q[0] = states[0]; + _att.q[1] = states[1]; + _att.q[2] = states[2]; + _att.q[3] = states[3]; _att.q_valid = true; - _att.R_valid = false; + _att.R_valid = true; + + _att.timestamp = _gyro.timestamp; + _att.roll = euler.getPhi(); + _att.pitch = euler.getTheta(); + _att.yaw = euler.getPsi(); + // XXX find the right state + _att.rollspeed = _gyro.x - states[11]; + _att.pitchspeed = _gyro.y - states[12]; + _att.yawspeed = _gyro.z - states[13]; + // gyro offsets + _att.rate_offsets[0] = states[11]; + _att.rate_offsets[1] = states[12]; + _att.rate_offsets[2] = states[13]; + + /* lazily publish the attitude only once available */ + if (_att_pub > 0) { + /* publish the attitude setpoint */ + orb_publish(ORB_ID(vehicle_attitude), _att_pub, &_att); - // /* lazily publish the attitude only once available */ - // if (_att_pub > 0) { - // /* publish the attitude setpoint */ - // orb_publish(ORB_ID(vehicle_attitude), _att_pub, &_att); + } else { + /* advertise and publish */ + _att_pub = orb_advertise(ORB_ID(vehicle_attitude), &_att); + } - // } else { - // /* advertise and publish */ - // _att_pub = orb_advertise(ORB_ID(vehicle_attitude), &_att); - // } + _global_pos.timestamp = _gyro.timestamp; // /* lazily publish the position only once available */ // if (_global_pos_pub > 0) { |