diff options
Diffstat (limited to 'apps/px4')
-rw-r--r-- | apps/px4/attitude_estimator_bm/Makefile | 5 | ||||
-rw-r--r-- | apps/px4/attitude_estimator_bm/attitude_estimator_bm.c | 39 | ||||
-rw-r--r-- | apps/px4/px4io/driver/px4io.cpp | 5 |
3 files changed, 40 insertions, 9 deletions
diff --git a/apps/px4/attitude_estimator_bm/Makefile b/apps/px4/attitude_estimator_bm/Makefile index 358b062c0..cf971ae05 100644 --- a/apps/px4/attitude_estimator_bm/Makefile +++ b/apps/px4/attitude_estimator_bm/Makefile @@ -37,9 +37,6 @@ APPNAME = attitude_estimator_bm PRIORITY = SCHED_PRIORITY_MAX - 10 -STACKSIZE = 3000 - -# XXX this is *horribly* broken -INCLUDES = $(TOPDIR)/../mavlink/include/mavlink +STACKSIZE = 4096 include $(APPDIR)/mk/app.mk diff --git a/apps/px4/attitude_estimator_bm/attitude_estimator_bm.c b/apps/px4/attitude_estimator_bm/attitude_estimator_bm.c index 3c6c8eec3..8551b5e1c 100644 --- a/apps/px4/attitude_estimator_bm/attitude_estimator_bm.c +++ b/apps/px4/attitude_estimator_bm/attitude_estimator_bm.c @@ -33,7 +33,10 @@ * ****************************************************************************/ -/* @file Black Magic Attitude Estimator */ +/** + * @file attitude_estimator_bm.c + * Black Magic Attitude Estimator + */ @@ -49,6 +52,7 @@ #include <uORB/uORB.h> #include <uORB/topics/sensor_combined.h> #include <uORB/topics/vehicle_attitude.h> +#include <uORB/topics/vehicle_status.h> #include <math.h> #include <errno.h> @@ -135,14 +139,22 @@ int attitude_estimator_bm_main(int argc, char *argv[]) /* rate-limit raw data updates to 200Hz */ //orb_set_interval(sub_raw, 5); + bool hil_enabled = false; + bool publishing = false; + /* advertise attitude */ int pub_att = orb_advertise(ORB_ID(vehicle_attitude), &att); + publishing = true; struct pollfd fds[] = { { .fd = sub_raw, .events = POLLIN }, }; - // int paramcounter = 100; + /* subscribe to system status */ + struct vehicle_status_s vstatus = {0}; + int vstatus_sub = orb_subscribe(ORB_ID(vehicle_status)); + + unsigned int loopcounter = 0; /* Main loop*/ while (true) { @@ -217,8 +229,29 @@ int attitude_estimator_bm_main(int argc, char *argv[]) att.R[0][2] = x_n_b.z; // XXX add remaining entries + if (loopcounter % 250 == 0) { + /* Check HIL state */ + orb_copy(ORB_ID(vehicle_status), vstatus_sub, &vstatus); + /* switching from non-HIL to HIL mode */ + if ((vstatus.mode & VEHICLE_MODE_FLAG_HIL_ENABLED) && !hil_enabled) { + hil_enabled = true; + publishing = false; + close(pub_att); + + /* switching from HIL to non-HIL mode */ + + } else if (!publishing && !hil_enabled) { + /* advertise the topic and make the initial publication */ + pub_att = orb_advertise(ORB_ID(vehicle_attitude), &att); + hil_enabled = false; + publishing = true; + } + } + // Broadcast - orb_publish(ORB_ID(vehicle_attitude), pub_att, &att); + if (publishing) orb_publish(ORB_ID(vehicle_attitude), pub_att, &att); + + loopcounter++; } /* Should never reach here */ diff --git a/apps/px4/px4io/driver/px4io.cpp b/apps/px4/px4io/driver/px4io.cpp index c3f14e3a4..3a3ff6407 100644 --- a/apps/px4/px4io/driver/px4io.cpp +++ b/apps/px4/px4io/driver/px4io.cpp @@ -32,7 +32,8 @@ ****************************************************************************/ /** - * @file Driver for the PX4IO board. + * @file px4io.cpp + * Driver for the PX4IO board. * * PX4IO is connected via serial (or possibly some other interface at a later * point). @@ -555,6 +556,6 @@ px4io_main(int argc, char *argv[]) - printf("need a verb, only support 'start'\n"); + printf("need a verb, only support 'start' and 'update'\n"); return ERROR; } |