Sync state_machine_helper_posix to state_machine_helper

state_machine_helper_posix.cpp was out of sync with
state_machine_helper_posix.cpp.

Added debug to detect when sensors is started before uorb.

Signed-off-by: Mark Charlebois <charlebm@gmail.com>

src/modules/commander/state_machine_helper_posix.cpp

@@ -53,18 +53,17 @@
 #include <uORB/topics/vehicle_status.h>
 #include <uORB/topics/actuator_controls.h>
 #include <uORB/topics/differential_pressure.h>
-#include <uORB/topics/airspeed.h>
 #include <systemlib/systemlib.h>
 #include <systemlib/param/param.h>
 #include <systemlib/err.h>
 #include <drivers/drv_hrt.h>
 #include <drivers/drv_accel.h>
-#include <drivers/drv_airspeed.h>
 #include <drivers/drv_device.h>
 #include <mavlink/mavlink_log.h>
 
 #include "state_machine_helper.h"
 #include "commander_helper.h"
+#include "PreflightCheck.h"
 
 /* oddly, ERROR is not defined for c++ */
 #ifdef ERROR
@@ -78,12 +77,12 @@ static const int ERROR = -1;
 // though the transition is marked as true additional checks must be made. See arming_state_transition
 // code for those checks.
 static const bool arming_transitions[vehicle_status_s::ARMING_STATE_MAX][vehicle_status_s::ARMING_STATE_MAX] = {
-	//                                  INIT,  STANDBY, ARMED, ARMED_ERROR, STANDBY_ERROR, REBOOT, IN_AIR_RESTORE
+	//                                                    INIT,  STANDBY, ARMED, ARMED_ERROR, STANDBY_ERROR, REBOOT, IN_AIR_RESTORE
-	{ /* vehicle_status_s::ARMING_STATE_INIT */           true,  true,    false, false,       false,         false,  false },
+	{ /* vehicle_status_s::ARMING_STATE_INIT */           true,  true,    false, false,       true,          false,  false },
 	{ /* vehicle_status_s::ARMING_STATE_STANDBY */        true,  true,    true,  true,        false,         false,  false },
 	{ /* vehicle_status_s::ARMING_STATE_ARMED */          false, true,    true,  false,       false,         false,  true },
 	{ /* vehicle_status_s::ARMING_STATE_ARMED_ERROR */    false, false,   true,  true,        false,         false,  false },
-	{ /* vehicle_status_s::ARMING_STATE_STANDBY_ERROR */  true,  true,    false, true,        true,          false,  false },
+	{ /* vehicle_status_s::ARMING_STATE_STANDBY_ERROR */  true,  true,    true,  true,        true,          false,  false },
 	{ /* vehicle_status_s::ARMING_STATE_REBOOT */         true,  true,    false, false,       true,          true,   true },
 	{ /* vehicle_status_s::ARMING_STATE_IN_AIR_RESTORE */ false, false,   false, false,       false,         false,  false }, // NYI
 };
@@ -208,14 +207,28 @@ arming_state_transition(struct vehicle_status_s *status,		///< current vehicle s
 
 		// HIL can always go to standby
 		if (status->hil_state == vehicle_status_s::HIL_STATE_ON && new_arming_state == vehicle_status_s::ARMING_STATE_STANDBY) {
+			warnx("HIL STATE ON -> STANDBY");
 			valid_transition = true;
 		}
 
 		/* Sensors need to be initialized for STANDBY state */
 		if (new_arming_state == vehicle_status_s::ARMING_STATE_STANDBY && !status->condition_system_sensors_initialized) {
+			warnx("NOT ARMING: Sensors not operational.");
 			mavlink_log_critical(mavlink_fd, "NOT ARMING: Sensors not operational.");
 			feedback_provided = true;
 			valid_transition = false;
+			status->arming_state = vehicle_status_s::ARMING_STATE_STANDBY_ERROR;
+		}
+
+		/* Check if we are trying to arm, checks look good but we are in STANDBY_ERROR */
+		if (status->arming_state == vehicle_status_s::ARMING_STATE_STANDBY_ERROR &&
+			new_arming_state == vehicle_status_s::ARMING_STATE_ARMED) {
+			if (status->condition_system_sensors_initialized) {
+				mavlink_log_critical(mavlink_fd, "Preflight check now OK, power cycle before arming");
+			} else {
+				mavlink_log_critical(mavlink_fd, "Preflight check failed, refusing to arm");
+			}
+			feedback_provided = true;
 		}
 
 		// Finish up the state transition
@@ -607,69 +620,15 @@ bool set_nav_state(struct vehicle_status_s *status, const bool data_link_loss_en
 
 int prearm_check(const struct vehicle_status_s *status, const int mavlink_fd)
 {
-	int ret;
+	/* at this point this equals the preflight check, but might add additional
-	bool failed = false;
+	 * quantities later.
-
+	 */
-	int fd = px4_open(ACCEL0_DEVICE_PATH, O_RDONLY);
+	bool checkAirspeed = false;
-
-	if (fd < 0) {
-		mavlink_log_critical(mavlink_fd, "ARM FAIL: ACCEL SENSOR MISSING");
-		failed = true;
-		goto system_eval;
-	}
-
-	ret = px4_ioctl(fd, ACCELIOCSELFTEST, 0);
-
-	if (ret != OK) {
-		mavlink_log_critical(mavlink_fd, "ARM FAIL: ACCEL CALIBRATION");
-		failed = true;
-		goto system_eval;
-	}
-
-	/* check measurement result range */
-	struct accel_report acc;
-	ret = px4_read(fd, &acc, sizeof(acc));
-
-	if (ret == sizeof(acc)) {
-		/* evaluate values */
-		float accel_magnitude = sqrtf(acc.x * acc.x + acc.y * acc.y + acc.z * acc.z);
-
-		if (accel_magnitude < 4.0f || accel_magnitude > 15.0f /* m/s^2 */) {
-			mavlink_log_critical(mavlink_fd, "ARM FAIL: ACCEL RANGE, hold still");
-			/* this is frickin' fatal */
-			failed = true;
-			goto system_eval;
-		}
-	} else {
-		mavlink_log_critical(mavlink_fd, "ARM FAIL: ACCEL READ");
-		/* this is frickin' fatal */
-		failed = true;
-		goto system_eval;
-	}
-
 	/* Perform airspeed check only if circuit breaker is not
 	 * engaged and it's not a rotary wing */
 	if (!status->circuit_breaker_engaged_airspd_check && !status->is_rotary_wing) {
-		/* accel done, close it */
+		checkAirspeed = true;
-		px4_close(fd);
-		fd = orb_subscribe(ORB_ID(airspeed));
-
-		struct airspeed_s airspeed;
-
-		if ((ret = orb_copy(ORB_ID(airspeed), fd, &airspeed)) ||
-			(hrt_elapsed_time(&airspeed.timestamp) > (50 * 1000))) {
-			mavlink_log_critical(mavlink_fd, "ARM FAIL: AIRSPEED SENSOR MISSING");
-			failed = true;
-			goto system_eval;
-		}
-
-		if (fabsf(airspeed.indicated_airspeed_m_s) > 6.0f) {
-			mavlink_log_critical(mavlink_fd, "AIRSPEED WARNING: WIND OR CALIBRATION ISSUE");
-			// XXX do not make this fatal yet
-		}
 	}
 
-system_eval:
+	return !Commander::preflightCheck(mavlink_fd, true, true, true, true, checkAirspeed, true, true);
-	px4_close(fd);
-	return (failed);
 }

src/modules/sensors/sensors_posix.cpp

@@ -2152,6 +2152,10 @@ Sensors::task_main()
 	/* advertise the sensor_combined topic and make the initial publication */
 	_sensor_pub = orb_advertise(ORB_ID(sensor_combined), &raw);
 
+	if (_sensor_pub < 0) {
+		warnx("Sensors::task_main ERROR - uORB not started");
+	}
+
 	/* wakeup source(s) */
 	struct pollfd fds[1];
 

src/modules/uORB/MuORB.cpp

@@ -420,6 +420,13 @@ ssize_t
 ORBDevNode::publish(const orb_metadata *meta, orb_advert_t handle, const void *data)
 {
 	PX4_DEBUG("ORBDevNode::publish meta = %p\n", meta);
+
+	if (handle < 0) {
+		PX4_DEBUG("ORBDevNode::publish called with invalid handle\n", meta);
+		errno = EINVAL;
+		return ERROR;
+	}
+
 	ORBDevNode *devnode = (ORBDevNode *)handle;
 	int ret;