From 19162ba5bee94b8a45e5fc398bb4e7fa8e0bfbc7 Mon Sep 17 00:00:00 2001 From: Mark Charlebois Date: Tue, 21 Apr 2015 17:14:52 -0700 Subject: [PATCH] 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 --- Makefile | 3 + Tools/posix_run.sh | 2 +- posix-configs/posixtest/init/rc.S | 2 +- .../commander/PreflightCheck_posix.cpp | 344 ++++++++++++++++++ src/modules/commander/module.mk | 9 +- src/platforms/posix/drivers/adcsim/adcsim.cpp | 4 +- ...o_start_posix.cpp => hello_start_qurt.cpp} | 0 7 files changed, 355 insertions(+), 9 deletions(-) create mode 100644 src/modules/commander/PreflightCheck_posix.cpp rename src/platforms/qurt/tests/hello/{hello_start_posix.cpp => hello_start_qurt.cpp} (100%) diff --git a/Makefile b/Makefile index 88ce8e5de6..73ff8c1bf1 100644 --- a/Makefile +++ b/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 diff --git a/Tools/posix_run.sh b/Tools/posix_run.sh index ebe7a77745..500261fce8 100755 --- a/Tools/posix_run.sh +++ b/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 diff --git a/posix-configs/posixtest/init/rc.S b/posix-configs/posixtest/init/rc.S index f10f57ec37..bc4b6a4b61 100644 --- a/posix-configs/posixtest/init/rc.S +++ b/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 diff --git a/src/modules/commander/PreflightCheck_posix.cpp b/src/modules/commander/PreflightCheck_posix.cpp new file mode 100644 index 0000000000..1f8791fe33 --- /dev/null +++ b/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 +* @author Johan Jansen +*/ + +#include +#include +#include +#include +#include +#include +#include +#include +#include + +#include +#include +#include + +#include +#include +#include +#include +#include +#include + +#include + +#include + +#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; +} + +} diff --git a/src/modules/commander/module.mk b/src/modules/commander/module.mk index 851ac9020d..7b72dafd95 100644 --- a/src/modules/commander/module.mk +++ b/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 diff --git a/src/platforms/posix/drivers/adcsim/adcsim.cpp b/src/platforms/posix/drivers/adcsim/adcsim.cpp index 78ffbf1db7..db167331f9 100644 --- a/src/platforms/posix/drivers/adcsim/adcsim.cpp +++ b/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"); diff --git a/src/platforms/qurt/tests/hello/hello_start_posix.cpp b/src/platforms/qurt/tests/hello/hello_start_qurt.cpp similarity index 100% rename from src/platforms/qurt/tests/hello/hello_start_posix.cpp rename to src/platforms/qurt/tests/hello/hello_start_qurt.cpp