fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-04-30 11:44:06 +08:00
Code Issues Packages Projects Releases Wiki Activity

Compile fixes, publishing estimated attitude now

Browse Source
This commit is contained in:
Lorenz Meier 2014-01-02 22:17:16 +01:00
parent 2bf4e79124
commit 8ad4b72fc7
1 changed files with 34 additions and 10 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

44
src/modules/fw_att_pos_estimator/fw_att_pos_estimator_main.cpp
View File

@ -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;
// /* lazily publish the attitude only once available */
// if (_att_pub > 0) {
// /* publish the attitude setpoint */
// orb_publish(ORB_ID(vehicle_attitude), _att_pub, &_att);
_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];
// } else {
// /* advertise and publish */
// _att_pub = orb_advertise(ORB_ID(vehicle_attitude), &_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);
}
_global_pos.timestamp = _gyro.timestamp;
// /* lazily publish the position only once available */
// if (_global_pos_pub > 0) {