fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-07-14 18:10:35 +08:00
Code Issues Packages Projects Releases Wiki Activity

Filter returns some results. Needs careful range and unit checking, but on track

Browse Source
This commit is contained in:
Lorenz Meier
2014-01-02 20:17:53 +01:00
parent 2bf4e79124
commit 4217272742
1 changed files with 8 additions and 8 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/modules/fw_att_pos_estimator/fw_att_pos_estimator_main.cpp
+8 -8
View File
@@ -578,9 +578,9 @@ FixedwingEstimator::task_main()
math::Quaternion q(states[0], states[1], states[2], states[3]);
_att.q = q;
_att.q_valid = true;
_att.R_valid = false;
// _att.q[0] = q.r;
// _att.q_valid = true;
// _att.R_valid = false;
// /* lazily publish the attitude only once available */
// if (_att_pub > 0) {
@@ -623,7 +623,7 @@ FixedwingEstimator::start()
_estimator_task = task_spawn_cmd("fw_att_pos_estimator",
SCHED_DEFAULT,
SCHED_PRIORITY_MAX - 40,
12048,
6000,
(main_t)&FixedwingEstimator::task_main_trampoline,
nullptr);
@@ -676,11 +676,11 @@ int fw_att_pos_estimator_main(int argc, char *argv[])
math::EulerAngles euler(q);
printf("attitude: roll: %8.4f, pitch %8.4f, yaw: %8.4f degrees",
math::degrees(euler.getPhi()), math::degrees(euler.getTheta()), math::degrees(euler.getPsi()));
(double)math::degrees(euler.getPhi()), (double)math::degrees(euler.getTheta()), (double)math::degrees(euler.getPsi()));
printf("states [1-3]: %8.4f, %8.4f, %8.4f\n", states[0], states[1], states[2]);
printf("states [4-6]: %8.4f, %8.4f, %8.4f\n", states[3], states[4], states[5]);
printf("states [7-9]: %8.4f, %8.4f, %8.4f\n", states[6], states[7], states[8]);
printf("states [1-3]: %8.4f, %8.4f, %8.4f\n", (double)states[0], (double)states[1], (double)states[2]);
printf("states [4-6]: %8.4f, %8.4f, %8.4f\n", (double)states[3], (double)states[4], (double)states[5]);
printf("states [7-9]: %8.4f, %8.4f, %8.4f\n", (double)states[6], (double)states[7], (double)states[8]);
exit(0);
} else {