Faster sensor bus resets on timeouts, massively reworked fixed wing app, tested

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;
 }