From 421727274251e5fc81ca5ae503a180b7326a480d Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Thu, 2 Jan 2014 20:17:53 +0100 Subject: Filter returns some results. Needs careful range and unit checking, but on track --- .../fw_att_pos_estimator/fw_att_pos_estimator_main.cpp | 16 ++++++++-------- 1 file changed, 8 insertions(+), 8 deletions(-) (limited to 'src/modules') 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..59fb3dc31 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 @@ -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 { -- cgit v1.2.3