Posix: Changed PreflightCheck to read Vdev

PreflightCheck was failing because it was trying to read actual
devices instad of virtual devices.

ADCSIM had a LINUXTEST ifdef that was removed.

posix_run.sh was using the wrong path

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

Makefile

@@ -146,6 +146,9 @@ testbuild:
 	$(Q) (cd $(PX4_BASE) && $(MAKE) distclean && $(MAKE) archives && $(MAKE) -j8)
 	$(Q) (zip -r Firmware.zip $(PX4_BASE)/Images)
 
+posix:
+	make PX4_TARGET_OS=posix
+
 posixrun:
 	Tools/posix_run.sh
 

Tools/posix_run.sh

@@ -23,4 +23,4 @@ if [ ! -d /eeprom ] && [ ! -w /eeprom ]
 	exit 1
 fi
 
-Build/linux_default.build/mainapp linux-configs/linuxtest/init/rc.S
+Build/posix_default.build/mainapp posix-configs/posixtest/init/rc.S

posix-configs/posixtest/init/rc.S

@@ -7,6 +7,6 @@ gyrosim start
 rgbled start
 mavlink start
 sensors start
-hil start
+hil mode_pwm
 commander start
 list_devices

src/modules/commander/PreflightCheck_posix.cpp

@@ -0,0 +1,344 @@
+/****************************************************************************
+*
+*   Copyright (c) 2012-2015 PX4 Development Team. All rights reserved.
+*
+* Redistribution and use in source and binary forms, with or without
+* modification, are permitted provided that the following conditions
+* are met:
+*
+* 1. Redistributions of source code must retain the above copyright
+*    notice, this list of conditions and the following disclaimer.
+* 2. Redistributions in binary form must reproduce the above copyright
+*    notice, this list of conditions and the following disclaimer in
+*    the documentation and/or other materials provided with the
+*    distribution.
+* 3. Neither the name PX4 nor the names of its contributors may be
+*    used to endorse or promote products derived from this software
+*    without specific prior written permission.
+*
+* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
+* OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
+* AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+* POSSIBILITY OF SUCH DAMAGE.
+*
+****************************************************************************/
+
+/**
+* @file PreflightCheck.cpp
+*
+* Preflight check for main system components
+*
+* @author Lorenz Meier <lorenz@px4.io>
+* @author Johan Jansen <jnsn.johan@gmail.com>
+*/
+
+#include <px4_config.h>
+#include <px4_posix.h>
+#include <unistd.h>
+#include <stdlib.h>
+#include <stdio.h>
+#include <string.h>
+#include <fcntl.h>
+#include <errno.h>
+#include <math.h>
+
+#include <systemlib/err.h>
+#include <systemlib/param/param.h>
+#include <systemlib/rc_check.h>
+
+#include <drivers/drv_hrt.h>
+#include <drivers/drv_mag.h>
+#include <drivers/drv_gyro.h>
+#include <drivers/drv_accel.h>
+#include <drivers/drv_baro.h>
+#include <drivers/drv_airspeed.h>
+
+#include <uORB/topics/airspeed.h>
+
+#include <mavlink/mavlink_log.h>
+
+#include "PreflightCheck.h"
+
+namespace Commander
+{
+static bool magnometerCheck(int mavlink_fd, unsigned instance, bool optional)
+{
+	bool success = true;
+
+	char s[30];
+	sprintf(s, "%s%u", MAG_BASE_DEVICE_PATH, instance);
+	int fd = px4_open(s, 0);
+
+	if (fd < 0) {
+		if (!optional) {
+			mavlink_and_console_log_critical(mavlink_fd,
+							 "PREFLIGHT FAIL: NO MAG SENSOR #%u", instance);
+		}
+
+		return false;
+	}
+
+	int calibration_devid;
+	int ret;
+	int devid = ioctl(fd, DEVIOCGDEVICEID, 0);
+	sprintf(s, "CAL_MAG%u_ID", instance);
+	param_get(param_find(s), &(calibration_devid));
+
+	if (devid != calibration_devid) {
+		mavlink_and_console_log_critical(mavlink_fd,
+						 "PREFLIGHT FAIL: MAG #%u UNCALIBRATED (NO ID)", instance);
+		success = false;
+		goto out;
+	}
+
+	ret = px4_ioctl(fd, MAGIOCSELFTEST, 0);
+
+	if (ret != OK) {
+		mavlink_and_console_log_critical(mavlink_fd,
+						 "PREFLIGHT FAIL: MAG #%u SELFTEST FAILED", instance);
+		success = false;
+		goto out;
+	}
+
+out:
+	px4_close(fd);
+	return success;
+}
+
+static bool accelerometerCheck(int mavlink_fd, unsigned instance, bool optional, bool dynamic)
+{
+	bool success = true;
+
+	char s[30];
+	sprintf(s, "%s%u", ACCEL_BASE_DEVICE_PATH, instance);
+	int fd = px4_open(s, O_RDONLY);
+
+	if (fd < 0) {
+		if (!optional) {
+			mavlink_and_console_log_critical(mavlink_fd,
+							 "PREFLIGHT FAIL: NO ACCEL SENSOR #%u", instance);
+		}
+
+		return false;
+	}
+
+	int calibration_devid;
+	int ret;
+	int devid = ioctl(fd, DEVIOCGDEVICEID, 0);
+	sprintf(s, "CAL_ACC%u_ID", instance);
+	param_get(param_find(s), &(calibration_devid));
+
+	if (devid != calibration_devid) {
+		mavlink_and_console_log_critical(mavlink_fd,
+						 "PREFLIGHT FAIL: ACCEL #%u UNCALIBRATED (NO ID)", instance);
+		success = false;
+		goto out;
+	}
+
+	ret = px4_ioctl(fd, ACCELIOCSELFTEST, 0);
+
+	if (ret != OK) {
+		mavlink_and_console_log_critical(mavlink_fd,
+						 "PREFLIGHT FAIL: ACCEL #%u SELFTEST FAILED", instance);
+		success = false;
+		goto out;
+	}
+
+	if (dynamic) {
+		/* 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_and_console_log_critical(mavlink_fd, "PREFLIGHT FAIL: ACCEL RANGE, hold still");
+				/* this is frickin' fatal */
+				success = false;
+				goto out;
+			}
+		} else {
+			mavlink_log_critical(mavlink_fd, "PREFLIGHT FAIL: ACCEL READ");
+			/* this is frickin' fatal */
+			success = false;
+			goto out;
+		}
+	}
+
+out:
+	px4_close(fd);
+	return success;
+}
+
+static bool gyroCheck(int mavlink_fd, unsigned instance, bool optional)
+{
+	bool success = true;
+
+	char s[30];
+	sprintf(s, "%s%u", GYRO_BASE_DEVICE_PATH, instance);
+	int fd = px4_open(s, 0);
+
+	if (fd < 0) {
+		if (!optional) {
+			mavlink_and_console_log_critical(mavlink_fd,
+							 "PREFLIGHT FAIL: NO GYRO SENSOR #%u", instance);
+		}
+
+		return false;
+	}
+
+	int calibration_devid;
+	int ret;
+	int devid = px4_ioctl(fd, DEVIOCGDEVICEID, 0);
+	sprintf(s, "CAL_GYRO%u_ID", instance);
+	param_get(param_find(s), &(calibration_devid));
+
+	if (devid != calibration_devid) {
+		mavlink_and_console_log_critical(mavlink_fd,
+						 "PREFLIGHT FAIL: GYRO #%u UNCALIBRATED (NO ID)", instance);
+		success = false;
+		goto out;
+	}
+
+	ret = px4_ioctl(fd, GYROIOCSELFTEST, 0);
+
+	if (ret != OK) {
+		mavlink_and_console_log_critical(mavlink_fd,
+						 "PREFLIGHT FAIL: GYRO #%u SELFTEST FAILED", instance);
+		success = false;
+		goto out;
+	}
+
+out:
+	px4_close(fd);
+	return success;
+}
+
+static bool baroCheck(int mavlink_fd, unsigned instance, bool optional)
+{
+	bool success = true;
+
+	char s[30];
+	sprintf(s, "%s%u", BARO_BASE_DEVICE_PATH, instance);
+	int fd = px4_open(s, 0);
+
+	if (fd < 0) {
+		if (!optional) {
+			mavlink_and_console_log_critical(mavlink_fd,
+							 "PREFLIGHT FAIL: NO BARO SENSOR #%u", instance);
+		}
+
+		return false;
+	}
+
+	px4_close(fd);
+	return success;
+}
+
+static bool airspeedCheck(int mavlink_fd, bool optional)
+{
+	bool success = true;
+	int ret;
+	int fd = orb_subscribe(ORB_ID(airspeed));
+
+	struct airspeed_s airspeed;
+
+	if ((ret = orb_copy(ORB_ID(airspeed), fd, &airspeed)) ||
+	    (hrt_elapsed_time(&airspeed.timestamp) > (500 * 1000))) {
+		mavlink_log_critical(mavlink_fd, "PREFLIGHT FAIL: AIRSPEED SENSOR MISSING");
+		success = false;
+		goto out;
+	}
+
+	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
+	}
+
+out:
+	close(fd);
+	return success;
+}
+
+bool preflightCheck(int mavlink_fd, bool checkMag, bool checkAcc, bool checkGyro,
+		    bool checkBaro, bool checkAirspeed, bool checkRC, bool checkDynamic)
+{
+	bool failed = false;
+
+	/* ---- MAG ---- */
+	if (checkMag) {
+		/* check all sensors, but fail only for mandatory ones */
+		for (unsigned i = 0; i < max_optional_mag_count; i++) {
+			bool required = (i < max_mandatory_mag_count);
+
+			if (!magnometerCheck(mavlink_fd, i, !required) && required) {
+				failed = true;
+			}
+		}
+	}
+
+	/* ---- ACCEL ---- */
+	if (checkAcc) {
+		/* check all sensors, but fail only for mandatory ones */
+		for (unsigned i = 0; i < max_optional_accel_count; i++) {
+			bool required = (i < max_mandatory_accel_count);
+
+			if (!accelerometerCheck(mavlink_fd, i, !required, checkDynamic) && required) {
+				failed = true;
+			}
+		}
+	}
+
+	/* ---- GYRO ---- */
+	if (checkGyro) {
+		/* check all sensors, but fail only for mandatory ones */
+		for (unsigned i = 0; i < max_optional_gyro_count; i++) {
+			bool required = (i < max_mandatory_gyro_count);
+
+			if (!gyroCheck(mavlink_fd, i, !required) && required) {
+				failed = true;
+			}
+		}
+	}
+
+	/* ---- BARO ---- */
+	if (checkBaro) {
+		/* check all sensors, but fail only for mandatory ones */
+		for (unsigned i = 0; i < max_optional_baro_count; i++) {
+			bool required = (i < max_mandatory_baro_count);
+
+			if (!baroCheck(mavlink_fd, i, !required) && required) {
+				failed = true;
+			}
+		}
+	}
+
+	/* ---- AIRSPEED ---- */
+	if (checkAirspeed) {
+		if (!airspeedCheck(mavlink_fd, true)) {
+			failed = true;
+		}
+	}
+
+	/* ---- RC CALIBRATION ---- */
+	if (checkRC) {
+		if (rc_calibration_check(mavlink_fd) != OK) {
+			failed = true;
+		}
+	}
+
+	/* Report status */
+	return !failed;
+}
+
+}

src/modules/commander/module.mk

@@ -45,14 +45,15 @@ SRCS		 	= commander.cpp \
 			baro_calibration.cpp \
 			accelerometer_calibration.cpp \
 			rc_calibration.cpp \
-			airspeed_calibration.cpp \
-			PreflightCheck.cpp
+			airspeed_calibration.cpp
 
 ifdef ($(PX4_TARGET_OS),nuttx)
 SRCS += 
-			state_machine_helper.cpp 
+			state_machine_helper.cpp \
+			PreflightCheck.cpp
 else
-SRCS +=			state_machine_helper_posix.cpp 
+SRCS +=			state_machine_helper_posix.cpp \
+			PreflightCheck_posix.cpp
 endif
 
 MODULE_STACKSIZE = 5000

src/platforms/posix/drivers/adcsim/adcsim.cpp

@@ -277,10 +277,8 @@ adcsim_main(int argc, char *argv[])
 	int ret = 0;
 
 	if (g_adc == nullptr) {
-#ifdef CONFIG_ARCH_BOARD_LINUXTEST
-		/* XXX this hardcodes the default channel set for LINUXTEST - should be configurable */
+		/* XXX this hardcodes the default channel set for POSIXTEST - should be configurable */
 		g_adc = new ADCSIM((1 << 10) | (1 << 11) | (1 << 12) | (1 << 13));
-#endif
 
 		if (g_adc == nullptr) {
 			warnx("couldn't allocate the ADCSIM driver");