From 20d35e33da475d55358c6d5eac6a30dfe3757a2c Mon Sep 17 00:00:00 2001 From: Mark Charlebois Date: Fri, 24 Apr 2015 11:45:14 -0700 Subject: [PATCH] Platform header file cleanup and consolidation Removed obsolete porting cruft from px4_XXX.h files and merged the POSIX changes in PreflightCheck_posix.cpp back to PreflightCheck.cpp Signed-off-by: Mark Charlebois --- makefiles/qurt/qurt_eigen.patch | 2 +- src/modules/commander/PreflightCheck.cpp | 31 +- .../commander/PreflightCheck_posix.cpp | 344 ------------------ src/modules/commander/module.mk | 9 +- src/modules/mavlink/mavlink_main_posix.cpp | 3 +- src/platforms/px4_common.h | 1 - src/platforms/px4_config.h | 10 +- src/platforms/px4_defines.h | 23 +- src/platforms/px4_i2c.h | 23 +- 9 files changed, 47 insertions(+), 399 deletions(-) delete mode 100644 src/modules/commander/PreflightCheck_posix.cpp diff --git a/makefiles/qurt/qurt_eigen.patch b/makefiles/qurt/qurt_eigen.patch index d5cec3141a..9ea57403ba 100644 --- a/makefiles/qurt/qurt_eigen.patch +++ b/makefiles/qurt/qurt_eigen.patch @@ -1,4 +1,4 @@ -This patch is required for QuRT. compex.h defines "I" and it replaces "I" in the +This patch is required for QuRT. complex.h defines "I" and it replaces "I" in the enum definition without this patch creating an error. diff --git a/Eigen/src/Core/SolveTriangular.h b/Eigen/src/Core/SolveTriangular.h diff --git a/src/modules/commander/PreflightCheck.cpp b/src/modules/commander/PreflightCheck.cpp index 2c05fb33d1..5567b43529 100644 --- a/src/modules/commander/PreflightCheck.cpp +++ b/src/modules/commander/PreflightCheck.cpp @@ -41,6 +41,7 @@ */ #include +#include #include #include #include @@ -74,7 +75,7 @@ static bool magnometerCheck(int mavlink_fd, unsigned instance, bool optional) char s[30]; sprintf(s, "%s%u", MAG_BASE_DEVICE_PATH, instance); - int fd = open(s, 0); + int fd = px4_open(s, 0); if (fd < 0) { if (!optional) { @@ -87,7 +88,7 @@ static bool magnometerCheck(int mavlink_fd, unsigned instance, bool optional) int calibration_devid; int ret; - int devid = ioctl(fd, DEVIOCGDEVICEID, 0); + int devid = px4_ioctl(fd, DEVIOCGDEVICEID, 0); sprintf(s, "CAL_MAG%u_ID", instance); param_get(param_find(s), &(calibration_devid)); @@ -98,7 +99,7 @@ static bool magnometerCheck(int mavlink_fd, unsigned instance, bool optional) goto out; } - ret = ioctl(fd, MAGIOCSELFTEST, 0); + ret = px4_ioctl(fd, MAGIOCSELFTEST, 0); if (ret != OK) { mavlink_and_console_log_critical(mavlink_fd, @@ -108,7 +109,7 @@ static bool magnometerCheck(int mavlink_fd, unsigned instance, bool optional) } out: - close(fd); + px4_close(fd); return success; } @@ -118,7 +119,7 @@ static bool accelerometerCheck(int mavlink_fd, unsigned instance, bool optional, char s[30]; sprintf(s, "%s%u", ACCEL_BASE_DEVICE_PATH, instance); - int fd = open(s, O_RDONLY); + int fd = px4_open(s, O_RDONLY); if (fd < 0) { if (!optional) { @@ -131,7 +132,7 @@ static bool accelerometerCheck(int mavlink_fd, unsigned instance, bool optional, int calibration_devid; int ret; - int devid = ioctl(fd, DEVIOCGDEVICEID, 0); + int devid = px4_ioctl(fd, DEVIOCGDEVICEID, 0); sprintf(s, "CAL_ACC%u_ID", instance); param_get(param_find(s), &(calibration_devid)); @@ -142,7 +143,7 @@ static bool accelerometerCheck(int mavlink_fd, unsigned instance, bool optional, goto out; } - ret = ioctl(fd, ACCELIOCSELFTEST, 0); + ret = px4_ioctl(fd, ACCELIOCSELFTEST, 0); if (ret != OK) { mavlink_and_console_log_critical(mavlink_fd, @@ -154,7 +155,7 @@ static bool accelerometerCheck(int mavlink_fd, unsigned instance, bool optional, if (dynamic) { /* check measurement result range */ struct accel_report acc; - ret = read(fd, &acc, sizeof(acc)); + ret = px4_read(fd, &acc, sizeof(acc)); if (ret == sizeof(acc)) { /* evaluate values */ @@ -175,7 +176,7 @@ static bool accelerometerCheck(int mavlink_fd, unsigned instance, bool optional, } out: - close(fd); + px4_close(fd); return success; } @@ -185,7 +186,7 @@ static bool gyroCheck(int mavlink_fd, unsigned instance, bool optional) char s[30]; sprintf(s, "%s%u", GYRO_BASE_DEVICE_PATH, instance); - int fd = open(s, 0); + int fd = px4_open(s, 0); if (fd < 0) { if (!optional) { @@ -198,7 +199,7 @@ static bool gyroCheck(int mavlink_fd, unsigned instance, bool optional) int calibration_devid; int ret; - int devid = ioctl(fd, DEVIOCGDEVICEID, 0); + int devid = px4_ioctl(fd, DEVIOCGDEVICEID, 0); sprintf(s, "CAL_GYRO%u_ID", instance); param_get(param_find(s), &(calibration_devid)); @@ -209,7 +210,7 @@ static bool gyroCheck(int mavlink_fd, unsigned instance, bool optional) goto out; } - ret = ioctl(fd, GYROIOCSELFTEST, 0); + ret = px4_ioctl(fd, GYROIOCSELFTEST, 0); if (ret != OK) { mavlink_and_console_log_critical(mavlink_fd, @@ -219,7 +220,7 @@ static bool gyroCheck(int mavlink_fd, unsigned instance, bool optional) } out: - close(fd); + px4_close(fd); return success; } @@ -229,7 +230,7 @@ static bool baroCheck(int mavlink_fd, unsigned instance, bool optional) char s[30]; sprintf(s, "%s%u", BARO_BASE_DEVICE_PATH, instance); - int fd = open(s, 0); + int fd = px4_open(s, 0); if (fd < 0) { if (!optional) { @@ -240,7 +241,7 @@ static bool baroCheck(int mavlink_fd, unsigned instance, bool optional) return false; } - close(fd); + px4_close(fd); return success; } diff --git a/src/modules/commander/PreflightCheck_posix.cpp b/src/modules/commander/PreflightCheck_posix.cpp deleted file mode 100644 index 5567b43529..0000000000 --- a/src/modules/commander/PreflightCheck_posix.cpp +++ /dev/null @@ -1,344 +0,0 @@ -/**************************************************************************** -* -* 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 = px4_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 = px4_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 7b72dafd95..851ac9020d 100644 --- a/src/modules/commander/module.mk +++ b/src/modules/commander/module.mk @@ -45,15 +45,14 @@ SRCS = commander.cpp \ baro_calibration.cpp \ accelerometer_calibration.cpp \ rc_calibration.cpp \ - airspeed_calibration.cpp + airspeed_calibration.cpp \ + PreflightCheck.cpp ifdef ($(PX4_TARGET_OS),nuttx) SRCS += - state_machine_helper.cpp \ - PreflightCheck.cpp + state_machine_helper.cpp else -SRCS += state_machine_helper_posix.cpp \ - PreflightCheck_posix.cpp +SRCS += state_machine_helper_posix.cpp endif MODULE_STACKSIZE = 5000 diff --git a/src/modules/mavlink/mavlink_main_posix.cpp b/src/modules/mavlink/mavlink_main_posix.cpp index cce0f4472f..5147368f41 100644 --- a/src/modules/mavlink/mavlink_main_posix.cpp +++ b/src/modules/mavlink/mavlink_main_posix.cpp @@ -216,7 +216,8 @@ Mavlink::Mavlink() : #endif default: - px4_errx(1, "instance ID is out of range"); + warnx("instance ID is out of range"); + px4_task_exit(1); break; } diff --git a/src/platforms/px4_common.h b/src/platforms/px4_common.h index d7b7a69421..384b1bca65 100644 --- a/src/platforms/px4_common.h +++ b/src/platforms/px4_common.h @@ -4,5 +4,4 @@ #include size_t strnlen(const char *s, size_t maxlen); -//inline bool isfinite(int x) { return true; } #endif diff --git a/src/platforms/px4_config.h b/src/platforms/px4_config.h index 635d82c216..d6fcebd703 100644 --- a/src/platforms/px4_config.h +++ b/src/platforms/px4_config.h @@ -34,15 +34,17 @@ /** * @file px4_config.h - * Preserve abiility to load config information that is used in subsequent - * includes or code + Configuration flags used in code. */ #pragma once #if defined(__PX4_NUTTX) + #include -#elif defined (__PX4_POSIX) || defined (__PX4_QURT) + +#elif defined (__PX4_POSIX) + #define CONFIG_NFILE_STREAMS 1 #define CONFIG_SCHED_WORKQUEUE 1 #define CONFIG_SCHED_HPWORK 1 @@ -55,6 +57,4 @@ #define CONFIG_SCHED_INSTRUMENTATION 1 #define CONFIG_MAX_TASKS 32 -#define px4_errx(x, ...) errx(x, __VA_ARGS__) - #endif diff --git a/src/platforms/px4_defines.h b/src/platforms/px4_defines.h index 89f98942d8..8fcc9695ad 100644 --- a/src/platforms/px4_defines.h +++ b/src/platforms/px4_defines.h @@ -67,9 +67,9 @@ /* Get value of parameter by name, which is equal to the handle for ros */ #define PX4_PARAM_GET_BYNAME(_name, _destpt) ros::param::get(_name, *_destpt) -#elif defined(__PX4_NUTTX) || defined(__PX4_POSIX) || defined(__PX4_QURT) +#elif defined(__PX4_NUTTX) || defined(__PX4_POSIX) /* - * Building for NuttX or Linux + * Building for NuttX or POSIX */ #include /* Main entry point */ @@ -90,7 +90,9 @@ typedef param_t px4_param_t; #error "No target OS defined" #endif -/* NuttX Specific defines */ +/* + * NuttX Specific defines + */ #if defined(__PX4_NUTTX) /* XXX this is a hack to resolve conflicts with NuttX headers */ @@ -106,7 +108,9 @@ typedef param_t px4_param_t; #define PX4_ISFINITE(x) isfinite(x) -/* POSIX Specific defines */ +/* + * POSIX Specific defines + */ #elif defined(__PX4_POSIX) // Flag is meaningless on Linux @@ -119,6 +123,7 @@ typedef param_t px4_param_t; //STM DocID018909 Rev 8 Sect 39.1 (Unique device ID Register) #define UNIQUE_ID 0x1FFF7A10 +/* FIXME - Used to satisfy build */ #define getreg32(a) (*(volatile uint32_t *)(a)) __BEGIN_DECLS @@ -133,8 +138,10 @@ __END_DECLS #endif -/* Defines for ROS and Linux */ -#if defined(__PX4_ROS) || defined(__PX4_POSIX) || defined(__PX4_QURT) +/* + * Defines for ROS and Linux + */ +#if defined(__PX4_ROS) || defined(__PX4_POSIX) #define OK 0 #define ERROR -1 @@ -203,7 +210,9 @@ __END_DECLS #endif -/* Defines for all platforms */ +/* + *Defines for all platforms + */ /* wrapper for 2d matrices */ #define PX4_ARRAY2D(_array, _ncols, _x, _y) (_array[_x * _ncols + _y]) diff --git a/src/platforms/px4_i2c.h b/src/platforms/px4_i2c.h index 423329c557..2b72f37c5d 100644 --- a/src/platforms/px4_i2c.h +++ b/src/platforms/px4_i2c.h @@ -42,7 +42,9 @@ #define PX4_I2C_M_READ 0x0001 /* read data, from slave to master */ #if defined(__PX4_ROS) + #error "Devices not supported in ROS" + #elif defined (__PX4_NUTTX) /* * Building for NuttX @@ -64,12 +66,6 @@ typedef struct i2c_dev_s px4_i2c_dev_t; -#define px4_i2cuninitialize(x) up_i2cuninitialize(x) -#define px4_i2cinitialize(x) up_i2cinitialize(x) -#define px4_i2creset(x) up_i2creset(x) - -#define px4_interrupt_context() up_interrupt_context() - #elif defined(__PX4_POSIX) #include @@ -85,25 +81,11 @@ typedef struct { int length; } px4_i2c_msg_t; -struct px4_i2c_ops_t; // NOTE - This is a copy of the NuttX i2c_ops_s structure typedef struct { const struct px4_i2c_ops_t *ops; /* I2C vtable */ } px4_i2c_dev_t; -// FIXME - Stub implementations -inline void px4_i2cuninitialize(px4_i2c_dev_t *dev); -inline void px4_i2cuninitialize(px4_i2c_dev_t *dev) {} - -inline px4_i2c_dev_t *px4_i2cinitialize(int bus); -inline px4_i2c_dev_t *px4_i2cinitialize(int bus) { return (px4_i2c_dev_t *)0; } - -inline void px4_i2creset(px4_i2c_dev_t *dev); -inline void px4_i2creset(px4_i2c_dev_t *dev) { } - -inline bool px4_interrupt_context(void); -inline bool px4_interrupt_context(void) { return false; } - // FIXME - Empty defines for I2C ops // Original version commented out //#define I2C_SETFREQUENCY(d,f) ((d)->ops->setfrequency(d,f)) @@ -134,6 +116,7 @@ struct i2c_rdwr_ioctl_data { uint32_t nmsgs; /* number of i2c_msgs */ }; +// FIXME - The functions are not implemented on QuRT/DSPAL int ioctl(int fd, int flags, unsigned long data); int write(int fd, const char *buffer, int buflen); #endif