diff options
author | Lorenz Meier <lm@inf.ethz.ch> | 2012-09-12 17:22:24 +0200 |
---|---|---|
committer | Lorenz Meier <lm@inf.ethz.ch> | 2012-09-12 17:22:24 +0200 |
commit | c815aff842c127488c3885d6cb88ebfaf3dcd3bf (patch) | |
tree | 1fa0891c516b6386a6e87bba91eee4228ce2e274 /apps/px4 | |
parent | 31ecc4d5df16e41e397d689e794c72c36f8c3233 (diff) | |
download | px4-firmware-c815aff842c127488c3885d6cb88ebfaf3dcd3bf.tar.gz px4-firmware-c815aff842c127488c3885d6cb88ebfaf3dcd3bf.tar.bz2 px4-firmware-c815aff842c127488c3885d6cb88ebfaf3dcd3bf.zip |
Deamonized GPS app, fixed GPS issues, reworking RC input
Diffstat (limited to 'apps/px4')
-rw-r--r-- | apps/px4/ground_estimator/ground_estimator.c | 47 |
1 files changed, 45 insertions, 2 deletions
diff --git a/apps/px4/ground_estimator/ground_estimator.c b/apps/px4/ground_estimator/ground_estimator.c index 0bd8f09f1..bbb0564ee 100644 --- a/apps/px4/ground_estimator/ground_estimator.c +++ b/apps/px4/ground_estimator/ground_estimator.c @@ -41,6 +41,9 @@ #include <unistd.h> #include <stdio.h> +#include <uORB/uORB.h> +#include <uORB/topics/sensor_combined.h> + static bool thread_should_exit = false; /**< ground_estimator exit flag */ static bool thread_running = false; /**< ground_estimator status flag */ static int ground_estimator_task; /**< Handle of ground_estimator task / thread */ @@ -64,9 +67,49 @@ int ground_estimator_thread_main(int argc, char *argv[]) { printf("[ground_estimator] starting\n"); + /* subscribe to raw data */ + int sub_raw = orb_subscribe(ORB_ID(sensor_combined)); + bool publishing = false; + + /* advertise attitude */ + struct debug_key_value_s dbg = { .key = "velx ", .value = 0.0f }; + orb_advert_t pub_att = orb_advertise(ORB_ID(debug_key_value), &dbg); + + float position[3] = {}; + float velocity[3] = {}; + + uint64_t last_time = 0; + while (!thread_should_exit) { - printf("Hello ground_estimator!\n"); - sleep(10); + + /* wait for sensor update */ + int ret = poll(fds, 1, 1000); + + if (ret < 0) { + /* XXX this is seriously bad - should be an emergency */ + } else if (ret == 0) { + /* XXX this means no sensor data - should be critical or emergency */ + printf("[attitude estimator bm] WARNING: Not getting sensor data - sensor app running?\n"); + } else { + struct sensor_combined_s s; + orb_copy(ORB_ID(sensor_combined), sub_raw, &s); + + uint64_t dt = s.timestamp - last_time; + + /* Integration */ + position[0] += velocity[0] * dt; + position[1] += velocity[1] * dt; + position[2] += velocity[2] * dt; + + velocity[0] += velocity[0] * 0.01f + 0.99f * s.acceleration_m_s2[0] * dt; + velocity[1] += velocity[1] * 0.01f + 0.99f * s.acceleration_m_s2[1] * dt; + velocity[2] += velocity[2] * 0.01f + 0.99f * s.acceleration_m_s2[2] * dt; + + dbg.value = position[0]; + + orb_publish(ORB_ID(debug_key_value), pub_dbg, &dbg); + } + } printf("[ground_estimator] exiting.\n"); |