diff options
author | Lorenz Meier <lm@inf.ethz.ch> | 2014-02-16 17:07:02 +0100 |
---|---|---|
committer | Lorenz Meier <lm@inf.ethz.ch> | 2014-02-16 17:07:02 +0100 |
commit | 8016032a40aaab55fc1d48d1a092cb00d5da92d0 (patch) | |
tree | 09b8a96ebf6e8c6ca3d66a04ae85a92ebbcba139 /src/modules/fw_att_pos_estimator/fw_att_pos_estimator_main.cpp | |
parent | 30612eb32d1acd3139e28254bb0b7e793826a343 (diff) | |
parent | 5bb004a7113484a5461c07af31d51b3579a8596e (diff) | |
download | px4-firmware-8016032a40aaab55fc1d48d1a092cb00d5da92d0.tar.gz px4-firmware-8016032a40aaab55fc1d48d1a092cb00d5da92d0.tar.bz2 px4-firmware-8016032a40aaab55fc1d48d1a092cb00d5da92d0.zip |
Merged beta into paul_estimator
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 | 15 |
1 files changed, 8 insertions, 7 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 b32b3686f..294e1fc51 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 @@ -758,8 +758,8 @@ FixedwingEstimator::task_main() // 18-20: Body Magnetic Field Vector - milligauss (X,Y,Z) math::Quaternion q(states[0], states[1], states[2], states[3]); - math::Dcm R(q); - math::EulerAngles euler(R); + math::Matrix<3, 3> R = q.to_dcm(); + math::Vector<3> euler = R.to_euler(); for (int i = 0; i < 3; i++) for (int j = 0; j < 3; j++) _att.R[i][j] = R(i, j); @@ -773,9 +773,9 @@ FixedwingEstimator::task_main() _att.R_valid = true; _att.timestamp = last_sensor_timestamp; - _att.roll = euler.getPhi(); - _att.pitch = euler.getTheta(); - _att.yaw = euler.getPsi(); + _att.roll = euler(0); + _att.pitch = euler(1); + _att.yaw = euler(2); _att.rollspeed = angRate.x - states[10]; _att.pitchspeed = angRate.y - states[11]; @@ -867,10 +867,11 @@ FixedwingEstimator::start() void print_status() { math::Quaternion q(states[0], states[1], states[2], states[3]); - math::EulerAngles euler(q); + math::Matrix<3, 3> R = q.to_dcm(); + math::Vector<3> euler = R.to_euler(); printf("attitude: roll: %8.4f, pitch %8.4f, yaw: %8.4f degrees\n", - (double)math::degrees(euler.getPhi()), (double)math::degrees(euler.getTheta()), (double)math::degrees(euler.getPsi())); + (double)math::degrees(euler(0)), (double)math::degrees(euler(1)), (double)math::degrees(euler(2))); // State vector: // 0-3: quaternions (q0, q1, q2, q3) |