dark0707
/
px4-firmware
forked from Archive/PX4-Autopilot
1
0
Fork 0
Code Pull Requests Activity

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 <charlebm@gmail.com>
This commit is contained in:
Mark Charlebois 2015-04-24 11:45:14 -07:00
parent 16d6068bfd
commit 20d35e33da
9 changed files with 47 additions and 399 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

2
makefiles/qurt/qurt_eigen.patch
View File

@ -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

31
src/modules/commander/PreflightCheck.cpp
View File

@ -41,6 +41,7 @@
*/
#include <px4_config.h>
#include <px4_posix.h>
#include <unistd.h>
#include <stdlib.h>
#include <stdio.h>
@ -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;
}

344
src/modules/commander/PreflightCheck_posix.cpp
View File

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

9
src/modules/commander/module.mk
View File

@ -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

3
src/modules/mavlink/mavlink_main_posix.cpp
View File

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

1
src/platforms/px4_common.h
View File

@ -4,5 +4,4 @@
#include <types.h>
size_t strnlen(const char *s, size_t maxlen);
//inline bool isfinite(int x) { return true; }
#endif

10
src/platforms/px4_config.h
View File

@ -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 <nuttx/config.h>
#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

23
src/platforms/px4_defines.h
View File

@ -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 <platforms/px4_includes.h>
/* 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])

23
src/platforms/px4_i2c.h
View File

@ -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 <stdint.h>
@ -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