ERROR macro: get rid of the many 'oddly, ERROR is not defined for c++', use PX4_ERROR

This commit is contained in:
Beat Küng 2016-09-20 10:30:18 +02:00 committed by Julian Oes
parent c606554da3
commit 241fd629ce
42 changed files with 207 additions and 504 deletions

View File

@ -124,7 +124,7 @@ Airspeed::~Airspeed()
int
Airspeed::init()
{
int ret = ERROR;
int ret = PX4_ERROR;
/* do I2C init (and probe) first */
if (I2C::init() != OK) {

View File

@ -39,6 +39,7 @@
*/
#include <px4_config.h>
#include <px4_defines.h>
#include <drivers/device/i2c.h>
@ -82,12 +83,6 @@
/* Default I2C bus */
#define PX4_I2C_BUS_DEFAULT PX4_I2C_BUS_EXPANSION
/* Oddly, ERROR is not defined for C++ */
#ifdef ERROR
# undef ERROR
#endif
static const int ERROR = -1;
#ifndef CONFIG_SCHED_WORKQUEUE
# error This requires CONFIG_SCHED_WORKQUEUE.
#endif

View File

@ -37,6 +37,7 @@
*/
#include <px4_config.h>
#include <px4_defines.h>
#include <sys/types.h>
#include <stdint.h>
@ -68,12 +69,6 @@
#define ACCEL_DEVICE_PATH "/dev/bma180"
/* oddly, ERROR is not defined for c++ */
#ifdef ERROR
# undef ERROR
#endif
static const int ERROR = -1;
#define DIR_READ (1<<7)
#define DIR_WRITE (0<<7)
@ -275,7 +270,7 @@ BMA180::~BMA180()
int
BMA180::init()
{
int ret = ERROR;
int ret = PX4_ERROR;
/* do SPI init (and probe) first */
if (SPI::init() != OK) {
@ -321,7 +316,7 @@ BMA180::init()
ret = OK;
} else {
ret = ERROR;
ret = PX4_ERROR;
}
_class_instance = register_class_devname(ACCEL_DEVICE_PATH);

View File

@ -265,12 +265,6 @@ ETSAirspeed::cycle()
namespace ets_airspeed
{
/* oddly, ERROR is not defined for c++ */
#ifdef ERROR
# undef ERROR
#endif
const int ERROR = -1;
ETSAirspeed *g_dev;
void start(int i2c_bus);

View File

@ -40,6 +40,7 @@
#include <nuttx/config.h>
#include <drivers/device/device.h>
#include <px4_defines.h>
#include <sys/types.h>
#include <stdint.h>
@ -88,12 +89,6 @@
#define SR04_CONVERSION_INTERVAL 100000 /* 100ms for one sonar */
/* oddly, ERROR is not defined for c++ */
#ifdef ERROR
# undef ERROR
#endif
static const int ERROR = -1;
#ifndef CONFIG_SCHED_WORKQUEUE
# error This requires CONFIG_SCHED_WORKQUEUE.
#endif
@ -269,18 +264,18 @@ HC_SR04::~HC_SR04()
int
HC_SR04::init()
{
int ret = ERROR;
int ret = PX4_ERROR;
/* do I2C init (and probe) first */
if (CDev::init() != OK) {
return ERROR;
return PX4_ERROR;
}
/* allocate basic report buffers */
_reports = new ringbuffer::RingBuffer(2, sizeof(distance_sensor_s));
if (_reports == nullptr) {
return ERROR;
return PX4_ERROR;
}
_class_instance = register_class_devname(RANGE_FINDER_BASE_DEVICE_PATH);
@ -739,12 +734,6 @@ HC_SR04::print_info()
namespace hc_sr04
{
/* oddly, ERROR is not defined for c++ */
#ifdef ERROR
# undef ERROR
#endif
const int ERROR = -1;
HC_SR04 *g_dev;
void start();

View File

@ -38,6 +38,7 @@
*/
#include <px4_config.h>
#include <px4_defines.h>
#include <drivers/device/i2c.h>
@ -123,12 +124,6 @@ enum HMC5883_BUS {
HMC5883_BUS_SPI
};
/* oddly, ERROR is not defined for c++ */
#ifdef ERROR
# undef ERROR
#endif
static const int ERROR = -1;
#ifndef CONFIG_SCHED_WORKQUEUE
# error This requires CONFIG_SCHED_WORKQUEUE.
#endif
@ -416,7 +411,7 @@ HMC5883::~HMC5883()
int
HMC5883::init()
{
int ret = ERROR;
int ret = PX4_ERROR;
ret = CDev::init();
@ -1236,7 +1231,7 @@ out:
if (check_scale()) {
/* failed */
warnx("FAILED: SCALE");
ret = ERROR;
ret = PX4_ERROR;
}
}
@ -1439,12 +1434,6 @@ HMC5883::print_info()
namespace hmc5883
{
/* oddly, ERROR is not defined for c++ */
#ifdef ERROR
# undef ERROR
#endif
const int ERROR = -1;
/*
list of supported bus configurations
*/

View File

@ -43,6 +43,7 @@
#include <fcntl.h>
#include <px4_config.h>
#include <px4_defines.h>
#include <poll.h>
#include <stdlib.h>
#include <stdio.h>
@ -57,12 +58,6 @@
#define DEFAULT_UART "/dev/ttyS0"; /**< USART1 */
/* Oddly, ERROR is not defined for C++ */
#ifdef ERROR
# undef ERROR
#endif
static const int ERROR = -1;
static int thread_should_exit = false; /**< Deamon exit flag */
static int thread_running = false; /**< Deamon status flag */
static int deamon_task; /**< Handle of deamon task / thread */
@ -133,7 +128,7 @@ recv_data(int uart, uint8_t *buffer, size_t *size, uint8_t *id)
}
}
return ERROR;
return PX4_ERROR;
}
int

View File

@ -45,6 +45,7 @@
#include <fcntl.h>
#include <px4_config.h>
#include <px4_defines.h>
#include <poll.h>
#include <stdlib.h>
#include <stdio.h>
@ -60,12 +61,6 @@
#define DEFAULT_UART "/dev/ttyS6"; /**< Serial4 */
/* Oddly, ERROR is not defined for C++ */
#ifdef ERROR
# undef ERROR
#endif
static const int ERROR = -1;
static int thread_should_exit = false; /**< Deamon exit flag */
static int thread_running = false; /**< Deamon status flag */
static int deamon_task; /**< Handle of deamon task / thread */
@ -124,7 +119,7 @@ recv_req_id(int uart, uint8_t *id)
/* if we have a binary mode request */
if (mode != BINARY_MODE_REQUEST_ID) {
return ERROR;
return PX4_ERROR;
}
/* Read the device ID being polled */
@ -132,10 +127,10 @@ recv_req_id(int uart, uint8_t *id)
} else {
warnx("UART timeout on TX/RX port");
return ERROR;
return PX4_ERROR;
}
return OK;
return PX4_OK;
}
int
@ -165,7 +160,7 @@ send_data(int uart, uint8_t *buffer, size_t size)
uint8_t dummy[size];
read(uart, &dummy, size);
return OK;
return PX4_OK;
}
int
@ -217,10 +212,9 @@ hott_telemetry_thread_main(int argc, char *argv[])
while (!thread_should_exit) {
// Listen for and serve poll from the receiver.
if (recv_req_id(uart, &id) == OK) {
if (recv_req_id(uart, &id) == PX4_OK) {
if (!connected) {
connected = true;
warnx("OK");
}
switch (id) {

View File

@ -40,6 +40,7 @@
*/
#include <px4_config.h>
#include <px4_defines.h>
#include <sys/types.h>
#include <stdint.h>
@ -74,12 +75,6 @@
#define L3GD20_DEVICE_PATH "/dev/l3gd20"
/* oddly, ERROR is not defined for c++ */
#ifdef ERROR
# undef ERROR
#endif
static const int ERROR = -1;
/* Orientation on board */
#define SENSOR_BOARD_ROTATION_000_DEG 0
#define SENSOR_BOARD_ROTATION_090_DEG 1
@ -476,7 +471,7 @@ L3GD20::~L3GD20()
int
L3GD20::init()
{
int ret = ERROR;
int ret = PX4_ERROR;
/* do SPI init (and probe) first */
if (SPI::init() != OK) {

View File

@ -40,6 +40,7 @@
*/
#include <px4_config.h>
#include <px4_defines.h>
#include <drivers/device/i2c.h>
@ -115,12 +116,6 @@ enum LIS3MDL_BUS {
LIS3MDL_BUS_SPI
};
/* oddly, ERROR is not defined for c++ */
#ifdef ERROR
# undef ERROR
#endif
static const int ERROR = -1;
#ifndef CONFIG_SCHED_WORKQUEUE
# error This requires CONFIG_SCHED_WORKQUEUE.
#endif
@ -406,7 +401,7 @@ LIS3MDL::~LIS3MDL()
int
LIS3MDL::init()
{
int ret = ERROR;
int ret = PX4_ERROR;
ret = CDev::init();
@ -1179,7 +1174,7 @@ out:
if (check_scale()) {
/* failed */
warnx("FAILED: SCALE");
ret = ERROR;
ret = PX4_ERROR;
}
}
@ -1325,12 +1320,6 @@ LIS3MDL::print_info()
namespace lis3mdl
{
/* oddly, ERROR is not defined for c++ */
#ifdef ERROR
# undef ERROR
#endif
const int ERROR = -1;
/*
list of supported bus configurations
*/

View File

@ -40,6 +40,7 @@
*/
#include "LidarLiteI2C.h"
#include <px4_defines.h>
#include <semaphore.h>
#include <fcntl.h>
#include <errno.h>
@ -48,12 +49,6 @@
#include <stdio.h>
#include <drivers/drv_hrt.h>
/* oddly, ERROR is not defined for c++ */
#ifdef ERROR
# undef ERROR
#endif
static const int ERROR = -1;
LidarLiteI2C::LidarLiteI2C(int bus, const char *path, int address) :
I2C("LL40LS", path, bus, address, 100000),
_work{},
@ -110,7 +105,7 @@ LidarLiteI2C::~LidarLiteI2C()
int LidarLiteI2C::init()
{
int ret = ERROR;
int ret = PX4_ERROR;
/* do I2C init (and probe) first */
if (I2C::init() != OK) {

View File

@ -45,14 +45,10 @@
#include "LidarLitePWM.h"
#include <stdio.h>
#include <string.h>
#include <px4_defines.h>
#include <drivers/drv_hrt.h>
#include <drivers/drv_pwm_input.h>
/* oddly, ERROR is not defined for c++ */
#ifdef __cplusplus
static const int ERROR = -1;
#endif
LidarLitePWM::LidarLitePWM(const char *path) :
CDev("LidarLitePWM", path),
_work{},
@ -95,15 +91,15 @@ int LidarLitePWM::init()
/* do regular cdev init */
int ret = CDev::init();
if (ret != OK) {
return ERROR;
if (ret != PX4_OK) {
return PX4_ERROR;
}
/* allocate basic report buffers */
_reports = new ringbuffer::RingBuffer(2, sizeof(struct distance_sensor_s));
if (_reports == nullptr) {
return ERROR;
return PX4_ERROR;
}
_class_instance = register_class_devname(RANGE_FINDER_BASE_DEVICE_PATH);
@ -119,7 +115,7 @@ int LidarLitePWM::init()
DEVICE_DEBUG("failed to create distance_sensor object. Did you start uOrb?");
}
return OK;
return PX4_OK;
}
void LidarLitePWM::print_info()
@ -172,11 +168,11 @@ int LidarLitePWM::measure()
{
perf_begin(_sample_perf);
if (OK != collect()) {
if (PX4_OK != collect()) {
DEVICE_DEBUG("collection error");
perf_count(_read_errors);
perf_end(_sample_perf);
return ERROR;
return PX4_ERROR;
}
_range.timestamp = hrt_absolute_time();
@ -206,7 +202,7 @@ int LidarLitePWM::measure()
poll_notify(POLLIN);
perf_end(_sample_perf);
return OK;
return PX4_OK;
}
ssize_t LidarLitePWM::read(struct file *filp, char *buffer, size_t buflen)
@ -266,12 +262,12 @@ int LidarLitePWM::collect()
int fd = ::open(PWMIN0_DEVICE_PATH, O_RDONLY);
if (fd == -1) {
return ERROR;
return PX4_ERROR;
}
if (::read(fd, &_pwm, sizeof(_pwm)) == sizeof(_pwm)) {
::close(fd);
return OK;
return PX4_OK;
}
::close(fd);
@ -284,7 +280,7 @@ int LidarLitePWM::reset_sensor()
int fd = ::open(PWMIN0_DEVICE_PATH, O_RDONLY);
if (fd == -1) {
return ERROR;
return PX4_ERROR;
}
int ret = ::ioctl(fd, SENSORIOCRESET, 0);

View File

@ -70,12 +70,6 @@ extern "C" __EXPORT int ll40ls_main(int argc, char *argv[]);
namespace ll40ls
{
/* oddly, ERROR is not defined for c++ */
#ifdef ERROR
# undef ERROR
#endif
const int ERROR = -1;
LidarLiteI2C *g_dev_int;
LidarLiteI2C *g_dev_ext;
LidarLitePWM *g_dev_pwm;
@ -132,7 +126,7 @@ void start(const bool use_i2c, const int bus)
g_dev_ext = new LidarLiteI2C(PX4_I2C_BUS_EXPANSION, LL40LS_DEVICE_PATH_EXT);
if (g_dev_ext != nullptr && OK != g_dev_ext->init()) {
if (g_dev_ext != nullptr && PX4_OK != g_dev_ext->init()) {
delete g_dev_ext;
g_dev_ext = nullptr;
@ -152,7 +146,7 @@ void start(const bool use_i2c, const int bus)
g_dev_int = new LidarLiteI2C(PX4_I2C_BUS_ONBOARD, LL40LS_DEVICE_PATH_INT);
if (g_dev_int != nullptr && OK != g_dev_int->init()) {
if (g_dev_int != nullptr && PX4_OK != g_dev_int->init()) {
/* tear down the failing onboard instance */
delete g_dev_int;
g_dev_int = nullptr;
@ -203,7 +197,7 @@ void start(const bool use_i2c, const int bus)
} else {
g_dev_pwm = new LidarLitePWM(LL40LS_DEVICE_PATH_PWM);
if (g_dev_pwm != nullptr && OK != g_dev_pwm->init()) {
if (g_dev_pwm != nullptr && PX4_OK != g_dev_pwm->init()) {
delete g_dev_pwm;
g_dev_pwm = nullptr;
warnx("failed to init PWM");
@ -321,7 +315,7 @@ test(const bool use_i2c, const int bus)
warnx("time: %lld", report.timestamp);
/* start the sensor polling at 2Hz */
if (OK != ioctl(fd, SENSORIOCSPOLLRATE, 2)) {
if (PX4_OK != ioctl(fd, SENSORIOCSPOLLRATE, 2)) {
errx(1, "failed to set 2Hz poll rate");
}
@ -353,7 +347,7 @@ test(const bool use_i2c, const int bus)
}
/* reset the sensor polling to default rate */
if (OK != ioctl(fd, SENSORIOCSPOLLRATE, SENSOR_POLLRATE_DEFAULT)) {
if (PX4_OK != ioctl(fd, SENSORIOCSPOLLRATE, SENSOR_POLLRATE_DEFAULT)) {
errx(1, "failed to set default poll rate");
}

View File

@ -187,12 +187,6 @@ enum LPS25H_BUS {
LPS25H_BUS_SPI
};
/* oddly, ERROR is not defined for c++ */
#ifdef ERROR
# undef ERROR
#endif
static const int ERROR = -1;
#ifndef CONFIG_SCHED_WORKQUEUE
# error This requires CONFIG_SCHED_WORKQUEUE.
#endif
@ -818,12 +812,6 @@ LPS25H::print_info()
namespace lps25h
{
/* oddly, ERROR is not defined for c++ */
#ifdef ERROR
# undef ERROR
#endif
const int ERROR = -1;
/*
list of supported bus configurations
*/

View File

@ -37,6 +37,7 @@
*/
#include <px4_config.h>
#include <px4_defines.h>
#include <sys/types.h>
#include <sys/stat.h>
@ -71,12 +72,6 @@
#include <mathlib/math/filter/LowPassFilter2p.hpp>
#include <lib/conversion/rotation.h>
/* oddly, ERROR is not defined for c++ */
#ifdef ERROR
# undef ERROR
#endif
static const int ERROR = -1;
/* SPI protocol address bits */
#define DIR_READ (1<<7)
#define DIR_WRITE (0<<7)
@ -647,7 +642,7 @@ LSM303D::~LSM303D()
int
LSM303D::init()
{
int ret = ERROR;
int ret = PX4_ERROR;
/* do SPI init (and probe) first */
if (SPI::init() != OK) {
@ -2060,7 +2055,7 @@ test()
warnx("accel range: %8.4f m/s^2", (double)accel_report.range_m_s2);
if (ERROR == (ret = ioctl(fd_accel, ACCELIOCGLOWPASS, 0))) {
if (PX4_ERROR == (ret = ioctl(fd_accel, ACCELIOCGLOWPASS, 0))) {
warnx("accel antialias filter bandwidth: fail");
} else {

View File

@ -92,13 +92,6 @@
#define MB12XX_CONVERSION_INTERVAL 100000 /* 60ms for one sonar */
#define TICKS_BETWEEN_SUCCESIVE_FIRES 100000 /* 30ms between each sonar measurement (watch out for interference!) */
/* oddly, ERROR is not defined for c++ */
#ifdef ERROR
# undef ERROR
#endif
static const int ERROR = -1;
#ifndef CONFIG_SCHED_WORKQUEUE
# error This requires CONFIG_SCHED_WORKQUEUE.
#endif
@ -251,7 +244,7 @@ MB12XX::~MB12XX()
int
MB12XX::init()
{
int ret = ERROR;
int ret = PX4_ERROR;
/* do I2C init (and probe) first */
if (I2C::init() != OK) {
@ -729,12 +722,6 @@ MB12XX::print_info()
namespace mb12xx
{
/* oddly, ERROR is not defined for c++ */
#ifdef ERROR
# undef ERROR
#endif
const int ERROR = -1;
MB12XX *g_dev;
void start();

View File

@ -399,12 +399,6 @@ MEASAirspeed::voltage_correction(float &diff_press_pa, float &temperature)
namespace meas_airspeed
{
/* oddly, ERROR is not defined for c++ */
#ifdef ERROR
# undef ERROR
#endif
const int ERROR = -1;
MEASAirspeed *g_dev = nullptr;
void start(int i2c_bus);

View File

@ -83,12 +83,6 @@ enum MS5611_BUS {
MS5611_BUS_SPI_EXTERNAL
};
/* oddly, ERROR is not defined for c++ */
#ifdef ERROR
# undef ERROR
#endif
static const int ERROR = -1;
#ifndef CONFIG_SCHED_WORKQUEUE
# error This requires CONFIG_SCHED_WORKQUEUE.
#endif

View File

@ -47,6 +47,7 @@
*/
#include <px4_config.h>
#include <px4_defines.h>
#include <drivers/device/i2c.h>
@ -108,12 +109,6 @@
*/
#define PCA9685_SCALE ((PCA9685_PWMMAX - PCA9685_PWMCENTER)/(M_DEG_TO_RAD_F * PCA9685_MAXSERVODEG)) // scales from rad to PWM
/* oddly, ERROR is not defined for c++ */
#ifdef ERROR
# undef ERROR
#endif
static const int ERROR = -1;
class PCA9685 : public device::I2C
{
public:
@ -421,7 +416,7 @@ PCA9685::setPin(uint8_t num, uint16_t val, bool invert)
}
}
return ERROR;
return PX4_ERROR;
}
int

View File

@ -40,6 +40,7 @@
*/
#include <px4_config.h>
#include <px4_defines.h>
#include <drivers/device/i2c.h>
@ -90,12 +91,6 @@
#define PX4FLOW_MAX_DISTANCE 5.0f
#define PX4FLOW_MIN_DISTANCE 0.3f
/* oddly, ERROR is not defined for c++ */
#ifdef ERROR
# undef ERROR
#endif
static const int ERROR = -1;
#ifndef CONFIG_SCHED_WORKQUEUE
# error This requires CONFIG_SCHED_WORKQUEUE.
#endif
@ -226,7 +221,7 @@ PX4FLOW::~PX4FLOW()
int
PX4FLOW::init()
{
int ret = ERROR;
int ret = PX4_ERROR;
/* do I2C init (and probe) first */
if (I2C::init() != OK) {
@ -663,12 +658,6 @@ PX4FLOW::print_info()
namespace px4flow
{
/* oddly, ERROR is not defined for c++ */
#ifdef ERROR
# undef ERROR
#endif
const int ERROR = -1;
PX4FLOW *g_dev = nullptr;
bool start_in_progress = false;

View File

@ -77,12 +77,6 @@
/* Configuration Constants */
/* oddly, ERROR is not defined for c++ */
#ifdef ERROR
# undef ERROR
#endif
static const int ERROR = -1;
#ifndef CONFIG_SCHED_WORKQUEUE
# error This requires CONFIG_SCHED_WORKQUEUE.
#endif
@ -756,12 +750,6 @@ SF0X::print_info()
namespace sf0x
{
/* oddly, ERROR is not defined for c++ */
#ifdef ERROR
# undef ERROR
#endif
const int ERROR = -1;
SF0X *g_dev;
void start(const char *port);

View File

@ -42,6 +42,7 @@
*/
#include <px4_config.h>
#include <px4_defines.h>
#include <drivers/device/i2c.h>
@ -81,12 +82,6 @@
#define SF1XX_DEVICE_PATH "/dev/sf1xx"
/* oddly, ERROR is not defined for c++ */
#ifdef ERROR
# undef ERROR
#endif
static const int ERROR = -1;
#ifndef CONFIG_SCHED_WORKQUEUE
# error This requires CONFIG_SCHED_WORKQUEUE.
#endif
@ -234,7 +229,7 @@ SF1XX::~SF1XX()
int
SF1XX::init()
{
int ret = ERROR;
int ret = PX4_ERROR;
int hw_model;
param_get(param_find("SENS_EN_SF1XX"), &hw_model);
@ -653,12 +648,6 @@ SF1XX::print_info()
namespace sf1xx
{
/* oddly, ERROR is not defined for c++ */
#ifdef ERROR
# undef ERROR
#endif
const int ERROR = -1;
SF1XX *g_dev;
void start();

View File

@ -38,6 +38,7 @@
*/
#include <px4_config.h>
#include <px4_defines.h>
#include <drivers/device/i2c.h>
@ -92,12 +93,6 @@
#define TICKS_BETWEEN_SUCCESIVE_FIRES 100000 /* 30ms between each sonar measurement (watch out for interference!) */
/* oddly, ERROR is not defined for c++ */
#ifdef ERROR
# undef ERROR
#endif
static const int ERROR = -1;
#ifndef CONFIG_SCHED_WORKQUEUE
# error This requires CONFIG_SCHED_WORKQUEUE.
#endif
@ -250,7 +245,7 @@ SRF02::~SRF02()
int
SRF02::init()
{
int ret = ERROR;
int ret = PX4_ERROR;
/* do I2C init (and probe) first */
if (I2C::init() != OK) {
@ -731,12 +726,6 @@ SRF02::print_info()
namespace srf02
{
/* oddly, ERROR is not defined for c++ */
#ifdef ERROR
# undef ERROR
#endif
const int ERROR = -1;
SRF02 *g_dev;
void start();

View File

@ -40,6 +40,7 @@
*/
#include <px4_config.h>
#include <px4_defines.h>
#include <drivers/device/i2c.h>
@ -93,13 +94,6 @@
#define SRF02_CONVERSION_INTERVAL 100000 /* 60ms for one sonar */
#define TICKS_BETWEEN_SUCCESIVE_FIRES 100000 /* 30ms between each sonar measurement (watch out for interference!) */
/* oddly, ERROR is not defined for c++ */
#ifdef ERROR
# undef ERROR
#endif
static const int ERROR = -1;
#ifndef CONFIG_SCHED_WORKQUEUE
# error This requires CONFIG_SCHED_WORKQUEUE.
#endif
@ -252,7 +246,7 @@ SRF02_I2C::~SRF02_I2C()
int
SRF02_I2C::init()
{
int ret = ERROR;
int ret = PX4_ERROR;
/* do I2C init (and probe) first */
if (I2C::init() != OK) {
@ -732,12 +726,6 @@ SRF02_I2C::print_info()
namespace srf02_i2c
{
/* oddly, ERROR is not defined for c++ */
#ifdef ERROR
# undef ERROR
#endif
const int ERROR = -1;
SRF02_I2C *g_dev;
void start();

View File

@ -39,6 +39,7 @@
*/
#include <px4_config.h>
#include <px4_defines.h>
#include <drivers/device/i2c.h>
@ -90,12 +91,6 @@
#define TRONE_CONVERSION_INTERVAL 50000 /* 50ms */
/* oddly, ERROR is not defined for c++ */
#ifdef ERROR
# undef ERROR
#endif
static const int ERROR = -1;
#ifndef CONFIG_SCHED_WORKQUEUE
# error This requires CONFIG_SCHED_WORKQUEUE.
#endif
@ -279,7 +274,7 @@ TRONE::~TRONE()
int
TRONE::init()
{
int ret = ERROR;
int ret = PX4_ERROR;
/* do I2C init (and probe) first */
if (I2C::init() != OK) {
@ -722,12 +717,6 @@ TRONE::print_info()
namespace trone
{
/* oddly, ERROR is not defined for c++ */
#ifdef ERROR
# undef ERROR
#endif
const int ERROR = -1;
TRONE *g_dev;
void start();

View File

@ -51,17 +51,6 @@
#define MIN_TAKEOFF_THRUST 0.2f
#define RATES_I_LIMIT 0.3f
namespace mc_att_control
{
/* oddly, ERROR is not defined for c++ */
#ifdef ERROR
# undef ERROR
#endif
static const int ERROR = -1;
}
MulticopterAttitudeControlMultiplatform::MulticopterAttitudeControlMultiplatform() :
MulticopterAttitudeControlBase(),
_actuators_0_circuit_breaker_enabled(false),

View File

@ -127,13 +127,13 @@
#include "calibration_routines.h"
#include "commander_helper.h"
#include <px4_defines.h>
#include <px4_posix.h>
#include <px4_time.h>
#include <unistd.h>
#include <stdio.h>
#include <poll.h>
#include <fcntl.h>
//#include <sys/prctl.h>
#include <math.h>
#include <poll.h>
#include <float.h>
@ -148,12 +148,6 @@
#include <systemlib/mavlink_log.h>
#include <uORB/topics/vehicle_attitude.h>
/* oddly, ERROR is not defined for c++ */
#ifdef ERROR
# undef ERROR
#endif
static const int ERROR = -1;
static const char *sensor_name = "accel";
static int32_t device_id[max_accel_sens];
@ -189,7 +183,7 @@ int do_accel_calibration(orb_advert_t *mavlink_log_pub)
accel_scale.z_offset = 0.0f;
accel_scale.z_scale = 1.0f;
int res = OK;
int res = PX4_OK;
char str[30];
@ -209,38 +203,38 @@ int do_accel_calibration(orb_advert_t *mavlink_log_pub)
res = px4_ioctl(fd, ACCELIOCSSCALE, (long unsigned int)&accel_scale);
px4_close(fd);
if (res != OK) {
if (res != PX4_OK) {
calibration_log_critical(mavlink_log_pub, CAL_ERROR_RESET_CAL_MSG, s);
}
#else
(void)sprintf(str, "CAL_ACC%u_XOFF", s);
res = param_set(param_find(str), &accel_scale.x_offset);
if (res != OK) {
if (res != PX4_OK) {
PX4_ERR("unable to reset %s", str);
}
(void)sprintf(str, "CAL_ACC%u_YOFF", s);
res = param_set(param_find(str), &accel_scale.y_offset);
if (res != OK) {
if (res != PX4_OK) {
PX4_ERR("unable to reset %s", str);
}
(void)sprintf(str, "CAL_ACC%u_ZOFF", s);
res = param_set(param_find(str), &accel_scale.z_offset);
if (res != OK) {
if (res != PX4_OK) {
PX4_ERR("unable to reset %s", str);
}
(void)sprintf(str, "CAL_ACC%u_XSCALE", s);
res = param_set(param_find(str), &accel_scale.x_scale);
if (res != OK) {
if (res != PX4_OK) {
PX4_ERR("unable to reset %s", str);
}
(void)sprintf(str, "CAL_ACC%u_YSCALE", s);
res = param_set(param_find(str), &accel_scale.y_scale);
if (res != OK) {
if (res != PX4_OK) {
PX4_ERR("unable to reset %s", str);
}
(void)sprintf(str, "CAL_ACC%u_ZSCALE", s);
res = param_set(param_find(str), &accel_scale.z_scale);
if (res != OK) {
if (res != PX4_OK) {
PX4_ERR("unable to reset %s", str);
}
#endif
@ -251,23 +245,23 @@ int do_accel_calibration(orb_advert_t *mavlink_log_pub)
unsigned active_sensors;
/* measure and calculate offsets & scales */
if (res == OK) {
if (res == PX4_OK) {
calibrate_return cal_return = do_accel_calibration_measurements(mavlink_log_pub, accel_offs, accel_T, &active_sensors);
if (cal_return == calibrate_return_cancelled) {
// Cancel message already displayed, nothing left to do
return ERROR;
return PX4_ERROR;
} else if (cal_return == calibrate_return_ok) {
res = OK;
res = PX4_OK;
} else {
res = ERROR;
res = PX4_ERROR;
}
}
if (res != OK) {
if (res != PX4_OK) {
if (active_sensors == 0) {
calibration_log_critical(mavlink_log_pub, CAL_ERROR_SENSOR_MSG);
}
return ERROR;
return PX4_ERROR;
}
/* measurements completed successfully, rotate calibration values */
@ -296,7 +290,7 @@ int do_accel_calibration(orb_advert_t *mavlink_log_pub)
bool failed = false;
failed = failed || (OK != param_set_no_notification(param_find("CAL_ACC_PRIME"), &(device_id_primary)));
failed = failed || (PX4_OK != param_set_no_notification(param_find("CAL_ACC_PRIME"), &(device_id_primary)));
PX4_DEBUG("found offset %d: x: %.6f, y: %.6f, z: %.6f", i,
@ -310,23 +304,23 @@ int do_accel_calibration(orb_advert_t *mavlink_log_pub)
/* set parameters */
(void)sprintf(str, "CAL_ACC%u_XOFF", i);
failed |= (OK != param_set_no_notification(param_find(str), &(accel_scale.x_offset)));
failed |= (PX4_OK != param_set_no_notification(param_find(str), &(accel_scale.x_offset)));
(void)sprintf(str, "CAL_ACC%u_YOFF", i);
failed |= (OK != param_set_no_notification(param_find(str), &(accel_scale.y_offset)));
failed |= (PX4_OK != param_set_no_notification(param_find(str), &(accel_scale.y_offset)));
(void)sprintf(str, "CAL_ACC%u_ZOFF", i);
failed |= (OK != param_set_no_notification(param_find(str), &(accel_scale.z_offset)));
failed |= (PX4_OK != param_set_no_notification(param_find(str), &(accel_scale.z_offset)));
(void)sprintf(str, "CAL_ACC%u_XSCALE", i);
failed |= (OK != param_set_no_notification(param_find(str), &(accel_scale.x_scale)));
failed |= (PX4_OK != param_set_no_notification(param_find(str), &(accel_scale.x_scale)));
(void)sprintf(str, "CAL_ACC%u_YSCALE", i);
failed |= (OK != param_set_no_notification(param_find(str), &(accel_scale.y_scale)));
failed |= (PX4_OK != param_set_no_notification(param_find(str), &(accel_scale.y_scale)));
(void)sprintf(str, "CAL_ACC%u_ZSCALE", i);
failed |= (OK != param_set_no_notification(param_find(str), &(accel_scale.z_scale)));
failed |= (PX4_OK != param_set_no_notification(param_find(str), &(accel_scale.z_scale)));
(void)sprintf(str, "CAL_ACC%u_ID", i);
failed |= (OK != param_set_no_notification(param_find(str), &(device_id[i])));
failed |= (PX4_OK != param_set_no_notification(param_find(str), &(device_id[i])));
if (failed) {
calibration_log_critical(mavlink_log_pub, CAL_ERROR_SET_PARAMS_MSG, i);
return ERROR;
return PX4_ERROR;
}
#if !defined(__PX4_QURT) && !defined(__PX4_POSIX_EAGLE) && !defined(__PX4_POSIX_RPI) && !defined(__PX4_POSIX_BEBOP)
@ -335,23 +329,23 @@ int do_accel_calibration(orb_advert_t *mavlink_log_pub)
if (fd < 0) {
calibration_log_critical(mavlink_log_pub, CAL_QGC_FAILED_MSG, "sensor does not exist");
res = ERROR;
res = PX4_ERROR;
} else {
res = px4_ioctl(fd, ACCELIOCSSCALE, (long unsigned int)&accel_scale);
px4_close(fd);
}
if (res != OK) {
if (res != PX4_OK) {
calibration_log_critical(mavlink_log_pub, CAL_ERROR_APPLY_CAL_MSG, i);
}
#endif
}
if (res == OK) {
if (res == PX4_OK) {
/* auto-save to EEPROM */
res = param_save_default();
if (res != OK) {
if (res != PX4_OK) {
calibration_log_critical(mavlink_log_pub, CAL_ERROR_SAVE_PARAMS_MSG);
}
@ -579,7 +573,7 @@ int mat_invert3(float src[3][3], float dst[3][3])
src[0][2] * (src[1][0] * src[2][1] - src[1][1] * src[2][0]);
if (fabsf(det) < FLT_EPSILON) {
return ERROR; // Singular matrix
return PX4_ERROR; // Singular matrix
}
dst[0][0] = (src[1][1] * src[2][2] - src[1][2] * src[2][1]) / det;
@ -592,7 +586,7 @@ int mat_invert3(float src[3][3], float dst[3][3])
dst[1][2] = (src[0][2] * src[1][0] - src[0][0] * src[1][2]) / det;
dst[2][2] = (src[0][0] * src[1][1] - src[0][1] * src[1][0]) / det;
return OK;
return PX4_OK;
}
calibrate_return calculate_calibration_values(unsigned sensor, float (&accel_ref)[max_accel_sens][detect_orientation_side_count][3], float (&accel_T)[max_accel_sens][3][3], float (&accel_offs)[max_accel_sens][3], float g)
@ -616,7 +610,7 @@ calibrate_return calculate_calibration_values(unsigned sensor, float (&accel_ref
/* calculate inverse matrix for A */
float mat_A_inv[3][3];
if (mat_invert3(mat_A, mat_A_inv) != OK) {
if (mat_invert3(mat_A, mat_A_inv) != PX4_OK) {
return calibrate_return_error;
}

View File

@ -41,6 +41,7 @@
#include "calibration_routines.h"
#include "commander_helper.h"
#include <px4_defines.h>
#include <px4_posix.h>
#include <px4_time.h>
#include <stdio.h>
@ -56,12 +57,6 @@
#include <systemlib/err.h>
#include <systemlib/airspeed.h>
/* oddly, ERROR is not defined for c++ */
#ifdef ERROR
# undef ERROR
#endif
static const int ERROR = -1;
static const char *sensor_name = "dpress";
static void feedback_calibration_failed(orb_advert_t *mavlink_log_pub)
@ -72,7 +67,7 @@ static void feedback_calibration_failed(orb_advert_t *mavlink_log_pub)
int do_airspeed_calibration(orb_advert_t *mavlink_log_pub)
{
int result = OK;
int result = PX4_OK;
unsigned calibration_counter = 0;
const unsigned maxcount = 2400;
@ -96,7 +91,7 @@ int do_airspeed_calibration(orb_advert_t *mavlink_log_pub)
int fd = px4_open(AIRSPEED0_DEVICE_PATH, 0);
if (fd > 0) {
if (OK == px4_ioctl(fd, AIRSPEEDIOCSSCALE, (long unsigned int)&airscale)) {
if (PX4_OK == px4_ioctl(fd, AIRSPEEDIOCSSCALE, (long unsigned int)&airscale)) {
paramreset_successful = true;
} else {
@ -165,7 +160,7 @@ int do_airspeed_calibration(orb_advert_t *mavlink_log_pub)
int fd_scale = px4_open(AIRSPEED0_DEVICE_PATH, 0);
airscale.offset_pa = diff_pres_offset;
if (fd_scale > 0) {
if (OK != px4_ioctl(fd_scale, AIRSPEEDIOCSSCALE, (long unsigned int)&airscale)) {
if (PX4_OK != px4_ioctl(fd_scale, AIRSPEEDIOCSSCALE, (long unsigned int)&airscale)) {
calibration_log_critical(mavlink_log_pub, "[cal] airspeed offset update failed");
}
@ -280,6 +275,6 @@ normal_return:
return result;
error_return:
result = ERROR;
result = PX4_ERROR;
goto normal_return;
}

View File

@ -41,20 +41,15 @@
#include <poll.h>
#include <math.h>
#include <fcntl.h>
#include <px4_defines.h>
#include <drivers/drv_hrt.h>
#include <uORB/topics/sensor_combined.h>
#include <drivers/drv_baro.h>
#include <systemlib/mavlink_log.h>
#include <systemlib/param/param.h>
/* oddly, ERROR is not defined for c++ */
#ifdef ERROR
# undef ERROR
#endif
static const int ERROR = -1;
int do_baro_calibration(orb_advert_t *mavlink_log_pub)
{
// TODO implement this
return ERROR;
return PX4_ERROR;
}

View File

@ -65,6 +65,7 @@
#include <drivers/drv_tone_alarm.h>
#include <geo/geo.h>
#include <navigator/navigation.h>
#include <px4_defines.h>
#include <px4_config.h>
#include <px4_posix.h>
#include <px4_tasks.h>
@ -107,12 +108,6 @@
#include <uORB/topics/vtol_vehicle_status.h>
#include <uORB/uORB.h>
/* oddly, ERROR is not defined for c++ */
#ifdef ERROR
# undef ERROR
#endif
static const int ERROR = -1;
typedef enum VEHICLE_MODE_FLAG
{
VEHICLE_MODE_FLAG_CUSTOM_MODE_ENABLED=1, /* 0b00000001 Reserved for future use. | */
@ -1347,7 +1342,7 @@ int commander_thread_main(int argc, char *argv[])
if (status_pub == nullptr) {
warnx("ERROR: orb_advertise for topic vehicle_status failed (uorb app running?).\n");
warnx("exiting.");
px4_task_exit(ERROR);
px4_task_exit(PX4_ERROR);
}
/* Initialize armed with all false */
@ -3780,7 +3775,7 @@ void *commander_low_prio_loop(void *arg)
case vehicle_command_s::VEHICLE_CMD_PREFLIGHT_CALIBRATION: {
int calib_ret = ERROR;
int calib_ret = PX4_ERROR;
/* try to go to INIT/PREFLIGHT arming state */
if (TRANSITION_DENIED == arming_state_transition(&status,

View File

@ -67,12 +67,6 @@
using namespace DriverFramework;
/* oddly, ERROR is not defined for c++ */
#ifdef ERROR
# undef ERROR
#endif
static const int ERROR = -1;
#define VEHICLE_TYPE_QUADROTOR 2
#define VEHICLE_TYPE_COAXIAL 3
#define VEHICLE_TYPE_HELICOPTER 4
@ -136,7 +130,7 @@ int buzzer_init()
if (!h_buzzer.isValid()) {
PX4_WARN("Buzzer: px4_open fail\n");
return ERROR;
return PX4_ERROR;
}
return PX4_OK;
@ -273,7 +267,7 @@ int led_init()
if (!h_leds.isValid()) {
PX4_WARN("LED: getHandle fail\n");
return ERROR;
return PX4_ERROR;
}
/* the blue LED is only available on FMUv1 & AeroCore but not FMUv2 */
@ -285,7 +279,7 @@ int led_init()
/* we consider the amber led mandatory */
if (h_leds.ioctl(LED_ON, LED_AMBER)) {
PX4_WARN("Amber LED: ioctl fail\n");
return ERROR;
return PX4_ERROR;
}
/* switch amber off */

View File

@ -51,6 +51,7 @@
#include <sys/ioctl.h>
#include <systemlib/err.h>
#include <fcntl.h>
#include <px4_defines.h>
#include <px4_posix.h>
#include <px4_time.h>
#include "drivers/drv_pwm_output.h"
@ -60,12 +61,6 @@
#include <drivers/drv_hrt.h>
#include <systemlib/mavlink_log.h>
/* oddly, ERROR is not defined for c++ */
#ifdef ERROR
# undef ERROR
#endif
static const int ERROR = -1;
int check_if_batt_disconnected(orb_advert_t *mavlink_log_pub) {
struct battery_status_s battery;
memset(&battery,0,sizeof(battery));
@ -74,14 +69,14 @@ int check_if_batt_disconnected(orb_advert_t *mavlink_log_pub) {
if (battery.voltage_filtered_v > 3.0f && !(hrt_absolute_time() - battery.timestamp > 500000)) {
mavlink_log_info(mavlink_log_pub, "Please disconnect battery and try again!");
return ERROR;
return PX4_ERROR;
}
return OK;
return PX4_OK;
}
int do_esc_calibration(orb_advert_t *mavlink_log_pub, struct actuator_armed_s* armed)
{
int return_code = OK;
int return_code = PX4_OK;
int fd = -1;
@ -119,19 +114,19 @@ int do_esc_calibration(orb_advert_t *mavlink_log_pub, struct actuator_armed_s* a
}
/* tell IO/FMU that its ok to disable its safety with the switch */
if (px4_ioctl(fd, PWM_SERVO_SET_ARM_OK, 0) != OK) {
if (px4_ioctl(fd, PWM_SERVO_SET_ARM_OK, 0) != PX4_OK) {
calibration_log_critical(mavlink_log_pub, CAL_QGC_FAILED_MSG, "Unable to disable safety switch");
goto Error;
}
/* tell IO/FMU that the system is armed (it will output values if safety is off) */
if (px4_ioctl(fd, PWM_SERVO_ARM, 0) != OK) {
if (px4_ioctl(fd, PWM_SERVO_ARM, 0) != PX4_OK) {
calibration_log_critical(mavlink_log_pub, CAL_QGC_FAILED_MSG, "Unable to arm system");
goto Error;
}
/* tell IO to switch off safety without using the safety switch */
if (px4_ioctl(fd, PWM_SERVO_SET_FORCE_SAFETY_OFF, 0) != OK) {
if (px4_ioctl(fd, PWM_SERVO_SET_FORCE_SAFETY_OFF, 0) != PX4_OK) {
calibration_log_critical(mavlink_log_pub, CAL_QGC_FAILED_MSG, "Unable to force safety off");
goto Error;
}
@ -175,26 +170,26 @@ Out:
orb_unsubscribe(batt_sub);
}
if (fd != -1) {
if (px4_ioctl(fd, PWM_SERVO_SET_FORCE_SAFETY_ON, 0) != OK) {
if (px4_ioctl(fd, PWM_SERVO_SET_FORCE_SAFETY_ON, 0) != PX4_OK) {
calibration_log_info(mavlink_log_pub, CAL_QGC_WARNING_MSG, "Safety switch still off");
}
if (px4_ioctl(fd, PWM_SERVO_DISARM, 0) != OK) {
if (px4_ioctl(fd, PWM_SERVO_DISARM, 0) != PX4_OK) {
calibration_log_info(mavlink_log_pub, CAL_QGC_WARNING_MSG, "Servos still armed");
}
if (px4_ioctl(fd, PWM_SERVO_CLEAR_ARM_OK, 0) != OK) {
if (px4_ioctl(fd, PWM_SERVO_CLEAR_ARM_OK, 0) != PX4_OK) {
calibration_log_info(mavlink_log_pub, CAL_QGC_WARNING_MSG, "Safety switch still deactivated");
}
px4_close(fd);
}
armed->in_esc_calibration_mode = false;
if (return_code == OK) {
if (return_code == PX4_OK) {
calibration_log_info(mavlink_log_pub, CAL_QGC_DONE_MSG, "esc");
}
return return_code;
Error:
return_code = ERROR;
return_code = PX4_ERROR;
goto Out;
}

View File

@ -43,6 +43,7 @@
#include "commander_helper.h"
#include <px4_posix.h>
#include <px4_defines.h>
#include <px4_time.h>
#include <stdio.h>
#include <unistd.h>
@ -58,12 +59,6 @@
#include <systemlib/err.h>
#include <systemlib/mcu_version.h>
/* oddly, ERROR is not defined for c++ */
#ifdef ERROR
# undef ERROR
#endif
static const int ERROR = -1;
static const char *sensor_name = "gyro";
static const unsigned max_gyros = 3;
@ -151,7 +146,7 @@ static calibrate_return gyro_calibration_worker(int cancel_sub, void* data)
int do_gyro_calibration(orb_advert_t *mavlink_log_pub)
{
int res = OK;
int res = PX4_OK;
gyro_worker_data_t worker_data = {};
calibration_log_info(mavlink_log_pub, CAL_QGC_STARTED_MSG, sensor_name);
@ -178,9 +173,9 @@ int do_gyro_calibration(orb_advert_t *mavlink_log_pub)
worker_data.gyro_sensor_sub[s] = -1;
(void)sprintf(str, "CAL_GYRO%u_ID", s);
res = param_set_no_notification(param_find(str), &(worker_data.device_id[s]));
if (res != OK) {
if (res != PX4_OK) {
calibration_log_critical(mavlink_log_pub, "[cal] Unable to reset CAL_GYRO%u_ID", s);
return ERROR;
return PX4_ERROR;
}
// Reset all offsets to 0 and scales to 1
@ -193,40 +188,40 @@ int do_gyro_calibration(orb_advert_t *mavlink_log_pub)
res = px4_ioctl(fd, GYROIOCSSCALE, (long unsigned int)&gyro_scale_zero);
px4_close(fd);
if (res != OK) {
if (res != PX4_OK) {
calibration_log_critical(mavlink_log_pub, CAL_ERROR_RESET_CAL_MSG, s);
return ERROR;
return PX4_ERROR;
}
}
#else
(void)sprintf(str, "CAL_GYRO%u_XOFF", s);
res = param_set(param_find(str), &gyro_scale_zero.x_offset);
if (res != OK) {
if (res != PX4_OK) {
PX4_ERR("unable to reset %s", str);
}
(void)sprintf(str, "CAL_GYRO%u_YOFF", s);
res = param_set(param_find(str), &gyro_scale_zero.y_offset);
if (res != OK) {
if (res != PX4_OK) {
PX4_ERR("unable to reset %s", str);
}
(void)sprintf(str, "CAL_GYRO%u_ZOFF", s);
res = param_set(param_find(str), &gyro_scale_zero.z_offset);
if (res != OK) {
if (res != PX4_OK) {
PX4_ERR("unable to reset %s", str);
}
(void)sprintf(str, "CAL_GYRO%u_XSCALE", s);
res = param_set(param_find(str), &gyro_scale_zero.x_scale);
if (res != OK) {
if (res != PX4_OK) {
PX4_ERR("unable to reset %s", str);
}
(void)sprintf(str, "CAL_GYRO%u_YSCALE", s);
res = param_set(param_find(str), &gyro_scale_zero.y_scale);
if (res != OK) {
if (res != PX4_OK) {
PX4_ERR("unable to reset %s", str);
}
(void)sprintf(str, "CAL_GYRO%u_ZSCALE", s);
res = param_set(param_find(str), &gyro_scale_zero.z_scale);
if (res != OK) {
if (res != PX4_OK) {
PX4_ERR("unable to reset %s", str);
}
#endif
@ -263,7 +258,7 @@ int do_gyro_calibration(orb_advert_t *mavlink_log_pub)
unsigned try_count = 0;
unsigned max_tries = 20;
res = ERROR;
res = PX4_ERROR;
do {
// Calibrate gyro and ensure user didn't move
@ -271,11 +266,11 @@ int do_gyro_calibration(orb_advert_t *mavlink_log_pub)
if (cal_return == calibrate_return_cancelled) {
// Cancel message already sent, we are done here
res = ERROR;
res = PX4_ERROR;
break;
} else if (cal_return == calibrate_return_error) {
res = ERROR;
res = PX4_ERROR;
} else {
/* check offsets */
@ -294,19 +289,19 @@ int do_gyro_calibration(orb_advert_t *mavlink_log_pub)
fabsf(zdiff) > maxoff) {
calibration_log_critical(mavlink_log_pub, "[cal] motion, retrying..");
res = ERROR;
res = PX4_ERROR;
} else {
res = OK;
res = PX4_OK;
}
}
try_count++;
} while (res == ERROR && try_count <= max_tries);
} while (res == PX4_ERROR && try_count <= max_tries);
if (try_count >= max_tries) {
calibration_log_critical(mavlink_log_pub, "[cal] ERROR: Motion during calibration");
res = ERROR;
res = PX4_ERROR;
}
calibrate_cancel_unsubscribe(cancel_sub);
@ -315,26 +310,26 @@ int do_gyro_calibration(orb_advert_t *mavlink_log_pub)
px4_close(worker_data.gyro_sensor_sub[s]);
}
if (res == OK) {
if (res == PX4_OK) {
/* set offset parameters to new values */
bool failed = false;
failed = failed || (OK != param_set_no_notification(param_find("CAL_GYRO_PRIME"), &(device_id_primary)));
failed = failed || (PX4_OK != param_set_no_notification(param_find("CAL_GYRO_PRIME"), &(device_id_primary)));
for (unsigned s = 0; s < max_gyros; s++) {
if (worker_data.device_id[s] != 0) {
char str[30];
(void)sprintf(str, "CAL_GYRO%u_XOFF", s);
failed |= (OK != param_set_no_notification(param_find(str), &(worker_data.gyro_scale[s].x_offset)));
failed |= (PX4_OK != param_set_no_notification(param_find(str), &(worker_data.gyro_scale[s].x_offset)));
(void)sprintf(str, "CAL_GYRO%u_YOFF", s);
failed |= (OK != param_set_no_notification(param_find(str), &(worker_data.gyro_scale[s].y_offset)));
failed |= (PX4_OK != param_set_no_notification(param_find(str), &(worker_data.gyro_scale[s].y_offset)));
(void)sprintf(str, "CAL_GYRO%u_ZOFF", s);
failed |= (OK != param_set_no_notification(param_find(str), &(worker_data.gyro_scale[s].z_offset)));
failed |= (PX4_OK != param_set_no_notification(param_find(str), &(worker_data.gyro_scale[s].z_offset)));
(void)sprintf(str, "CAL_GYRO%u_ID", s);
failed |= (OK != param_set_no_notification(param_find(str), &(worker_data.device_id[s])));
failed |= (PX4_OK != param_set_no_notification(param_find(str), &(worker_data.device_id[s])));
#if !defined(__PX4_QURT) && !defined(__PX4_POSIX_EAGLE) && !defined(__PX4_POSIX_RPI) && !defined(__PX4_POSIX_BEBOP)
/* apply new scaling and offsets */
@ -349,7 +344,7 @@ int do_gyro_calibration(orb_advert_t *mavlink_log_pub)
res = px4_ioctl(fd, GYROIOCSSCALE, (long unsigned int)&worker_data.gyro_scale[s]);
px4_close(fd);
if (res != OK) {
if (res != PX4_OK) {
calibration_log_critical(mavlink_log_pub, CAL_ERROR_APPLY_CAL_MSG, 1);
}
#endif
@ -358,7 +353,7 @@ int do_gyro_calibration(orb_advert_t *mavlink_log_pub)
if (failed) {
calibration_log_critical(mavlink_log_pub, "[cal] ERROR: failed to set offset params");
res = ERROR;
res = PX4_ERROR;
}
}
@ -369,11 +364,11 @@ int do_gyro_calibration(orb_advert_t *mavlink_log_pub)
/* store last 32bit number - not unique, but unique in a given set */
(void)param_set(param_find("CAL_BOARD_ID"), &mcu_id[2]);
if (res == OK) {
if (res == PX4_OK) {
/* auto-save to EEPROM */
res = param_save_default();
if (res != OK) {
if (res != PX4_OK) {
calibration_log_critical(mavlink_log_pub, CAL_ERROR_SAVE_PARAMS_MSG);
}
}
@ -381,7 +376,7 @@ int do_gyro_calibration(orb_advert_t *mavlink_log_pub)
/* if there is a any preflight-check system response, let the barrage of messages through */
usleep(200000);
if (res == OK) {
if (res == PX4_OK) {
calibration_log_info(mavlink_log_pub, CAL_QGC_DONE_MSG, sensor_name);
} else {
calibration_log_info(mavlink_log_pub, CAL_QGC_FAILED_MSG, sensor_name);

View File

@ -42,6 +42,7 @@
#include "calibration_routines.h"
#include "calibration_messages.h"
#include <px4_defines.h>
#include <px4_posix.h>
#include <px4_time.h>
#include <stdio.h>
@ -60,12 +61,6 @@
#include <systemlib/param/param.h>
#include <systemlib/err.h>
/* oddly, ERROR is not defined for c++ */
#ifdef ERROR
# undef ERROR
#endif
static const int ERROR = -1;
static const char *sensor_name = "mag";
static constexpr unsigned max_mags = 3;
static constexpr float mag_sphere_radius = 0.2f;
@ -111,7 +106,7 @@ int do_mag_calibration(orb_advert_t *mavlink_log_pub)
mscale_null.z_offset = 0.0f;
mscale_null.z_scale = 1.0f;
int result = OK;
int result = PX4_OK;
// Determine which mags are available and reset each
@ -128,39 +123,39 @@ int do_mag_calibration(orb_advert_t *mavlink_log_pub)
// Reset mag id to mag not available
(void)sprintf(str, "CAL_MAG%u_ID", cur_mag);
result = param_set_no_notification(param_find(str), &(device_ids[cur_mag]));
if (result != OK) {
if (result != PX4_OK) {
calibration_log_info(mavlink_log_pub, "[cal] Unable to reset CAL_MAG%u_ID", cur_mag);
break;
}
#else
(void)sprintf(str, "CAL_MAG%u_XOFF", cur_mag);
result = param_set(param_find(str), &mscale_null.x_offset);
if (result != OK) {
if (result != PX4_OK) {
PX4_ERR("unable to reset %s", str);
}
(void)sprintf(str, "CAL_MAG%u_YOFF", cur_mag);
result = param_set(param_find(str), &mscale_null.y_offset);
if (result != OK) {
if (result != PX4_OK) {
PX4_ERR("unable to reset %s", str);
}
(void)sprintf(str, "CAL_MAG%u_ZOFF", cur_mag);
result = param_set(param_find(str), &mscale_null.z_offset);
if (result != OK) {
if (result != PX4_OK) {
PX4_ERR("unable to reset %s", str);
}
(void)sprintf(str, "CAL_MAG%u_XSCALE", cur_mag);
result = param_set(param_find(str), &mscale_null.x_scale);
if (result != OK) {
if (result != PX4_OK) {
PX4_ERR("unable to reset %s", str);
}
(void)sprintf(str, "CAL_MAG%u_YSCALE", cur_mag);
result = param_set(param_find(str), &mscale_null.y_scale);
if (result != OK) {
if (result != PX4_OK) {
PX4_ERR("unable to reset %s", str);
}
(void)sprintf(str, "CAL_MAG%u_ZSCALE", cur_mag);
result = param_set(param_find(str), &mscale_null.z_scale);
if (result != OK) {
if (result != PX4_OK) {
PX4_ERR("unable to reset %s", str);
}
#endif
@ -181,18 +176,18 @@ int do_mag_calibration(orb_advert_t *mavlink_log_pub)
// Reset mag scale
result = px4_ioctl(fd, MAGIOCSSCALE, (long unsigned int)&mscale_null);
if (result != OK) {
if (result != PX4_OK) {
calibration_log_critical(mavlink_log_pub, CAL_ERROR_RESET_CAL_MSG, cur_mag);
}
/* calibrate range */
if (result == OK) {
if (result == PX4_OK) {
result = px4_ioctl(fd, MAGIOCCALIBRATE, fd);
if (result != OK) {
if (result != PX4_OK) {
calibration_log_info(mavlink_log_pub, "[cal] Skipped scale calibration, sensor %u", cur_mag);
/* this is non-fatal - mark it accordingly */
result = OK;
result = PX4_OK;
}
}
@ -201,11 +196,11 @@ int do_mag_calibration(orb_advert_t *mavlink_log_pub)
}
// Calibrate all mags at the same time
if (result == OK) {
if (result == PX4_OK) {
switch (mag_calibrate_all(mavlink_log_pub)) {
case calibrate_return_cancelled:
// Cancel message already displayed, we're done here
result = ERROR;
result = PX4_ERROR;
break;
case calibrate_return_ok:
@ -215,7 +210,7 @@ int do_mag_calibration(orb_advert_t *mavlink_log_pub)
/* if there is a any preflight-check system response, let the barrage of messages through */
usleep(200000);
if (result == OK) {
if (result == PX4_OK) {
calibration_log_info(mavlink_log_pub, CAL_QGC_PROGRESS_MSG, 100);
usleep(20000);
calibration_log_info(mavlink_log_pub, CAL_QGC_DONE_MSG, sensor_name);
@ -680,7 +675,7 @@ calibrate_return mag_calibrate_all(orb_advert_t *mavlink_log_pub)
}
if (result == calibrate_return_ok) {
if (px4_ioctl(fd_mag, MAGIOCGSCALE, (long unsigned int)&mscale) != OK) {
if (px4_ioctl(fd_mag, MAGIOCGSCALE, (long unsigned int)&mscale) != PX4_OK) {
calibration_log_critical(mavlink_log_pub, "[cal] ERROR: failed to get current calibration #%u", cur_mag);
result = calibrate_return_error;
}
@ -693,7 +688,7 @@ calibrate_return mag_calibrate_all(orb_advert_t *mavlink_log_pub)
mscale.z_offset = sphere_z[cur_mag];
#if !defined(__PX4_QURT) && !defined(__PX4_POSIX_RPI) && !defined(__PX4_POSIX_BEBOP)
if (px4_ioctl(fd_mag, MAGIOCSSCALE, (long unsigned int)&mscale) != OK) {
if (px4_ioctl(fd_mag, MAGIOCSSCALE, (long unsigned int)&mscale) != PX4_OK) {
calibration_log_critical(mavlink_log_pub, CAL_ERROR_APPLY_CAL_MSG, cur_mag);
result = calibrate_return_error;
}
@ -713,22 +708,22 @@ calibrate_return mag_calibrate_all(orb_advert_t *mavlink_log_pub)
/* set parameters */
(void)sprintf(str, "CAL_MAG%u_ID", cur_mag);
failed |= (OK != param_set_no_notification(param_find(str), &(device_ids[cur_mag])));
failed |= (PX4_OK != param_set_no_notification(param_find(str), &(device_ids[cur_mag])));
(void)sprintf(str, "CAL_MAG%u_XOFF", cur_mag);
failed |= (OK != param_set_no_notification(param_find(str), &(mscale.x_offset)));
failed |= (PX4_OK != param_set_no_notification(param_find(str), &(mscale.x_offset)));
(void)sprintf(str, "CAL_MAG%u_YOFF", cur_mag);
failed |= (OK != param_set_no_notification(param_find(str), &(mscale.y_offset)));
failed |= (PX4_OK != param_set_no_notification(param_find(str), &(mscale.y_offset)));
(void)sprintf(str, "CAL_MAG%u_ZOFF", cur_mag);
failed |= (OK != param_set_no_notification(param_find(str), &(mscale.z_offset)));
failed |= (PX4_OK != param_set_no_notification(param_find(str), &(mscale.z_offset)));
// FIXME: scaling is not used right now on QURT
#ifndef __PX4_QURT
(void)sprintf(str, "CAL_MAG%u_XSCALE", cur_mag);
failed |= (OK != param_set_no_notification(param_find(str), &(mscale.x_scale)));
failed |= (PX4_OK != param_set_no_notification(param_find(str), &(mscale.x_scale)));
(void)sprintf(str, "CAL_MAG%u_YSCALE", cur_mag);
failed |= (OK != param_set_no_notification(param_find(str), &(mscale.y_scale)));
failed |= (PX4_OK != param_set_no_notification(param_find(str), &(mscale.y_scale)));
(void)sprintf(str, "CAL_MAG%u_ZSCALE", cur_mag);
failed |= (OK != param_set_no_notification(param_find(str), &(mscale.z_scale)));
failed |= (PX4_OK != param_set_no_notification(param_find(str), &(mscale.z_scale)));
#endif
if (failed) {

View File

@ -38,6 +38,7 @@
#include <px4_posix.h>
#include <px4_time.h>
#include <px4_defines.h>
#include "rc_calibration.h"
#include "commander_helper.h"
@ -50,12 +51,6 @@
#include <systemlib/param/param.h>
#include <systemlib/err.h>
/* oddly, ERROR is not defined for c++ */
#ifdef ERROR
# undef ERROR
#endif
static const int ERROR = -1;
int do_trim_calibration(orb_advert_t *mavlink_log_pub)
{
int sub_man = orb_subscribe(ORB_ID(manual_control_setpoint));
@ -66,7 +61,7 @@ int do_trim_calibration(orb_advert_t *mavlink_log_pub)
if (!changed) {
mavlink_log_critical(mavlink_log_pub, "no inputs, aborting");
return ERROR;
return PX4_ERROR;
}
orb_copy(ORB_ID(manual_control_setpoint), sub_man, &sp);
@ -108,10 +103,10 @@ int do_trim_calibration(orb_advert_t *mavlink_log_pub)
if (save_ret != 0 || p1r != 0 || p2r != 0 || p3r != 0) {
mavlink_log_critical(mavlink_log_pub, "TRIM: PARAM SET FAIL");
px4_close(sub_man);
return ERROR;
return PX4_ERROR;
}
mavlink_log_info(mavlink_log_pub, "trim cal done");
px4_close(sub_man);
return OK;
return PX4_OK;
}

View File

@ -108,7 +108,7 @@ public:
/**
* Start the main task.
*
* @return OK on success.
* @return PX4_OK on success.
*/
int start();
@ -344,12 +344,6 @@ private:
namespace att_control
{
/* oddly, ERROR is not defined for c++ */
#ifdef ERROR
# undef ERROR
#endif
static const int ERROR = -1;
FixedwingAttitudeControl *g_control = nullptr;
}
@ -583,7 +577,7 @@ FixedwingAttitudeControl::parameters_update()
_wheel_ctrl.set_integrator_max(_parameters.w_integrator_max);
_wheel_ctrl.set_max_rate(math::radians(_parameters.w_rmax));
return OK;
return PX4_OK;
}
void
@ -1221,7 +1215,7 @@ FixedwingAttitudeControl::start()
return -errno;
}
return OK;
return PX4_OK;
}
int fw_att_control_main(int argc, char *argv[])
@ -1245,7 +1239,7 @@ int fw_att_control_main(int argc, char *argv[])
return 1;
}
if (OK != att_control::g_control->start()) {
if (PX4_OK != att_control::g_control->start()) {
delete att_control::g_control;
att_control::g_control = nullptr;
warn("start failed");

View File

@ -525,12 +525,6 @@ private:
namespace l1_control
{
/* oddly, ERROR is not defined for c++ */
#ifdef ERROR
# undef ERROR
#endif
static const int ERROR = -1;
FixedwingPositionControl *g_control = nullptr;
}

View File

@ -40,6 +40,7 @@
*/
#include <px4_config.h>
#include <px4_defines.h>
#include <px4_getopt.h>
#include <stdio.h>
#include <stdlib.h>
@ -92,12 +93,6 @@
#error MAVLINK_CRC_EXTRA has to be defined on PX4 systems
#endif
/* oddly, ERROR is not defined for c++ */
#ifdef ERROR
# undef ERROR
#endif
static const int ERROR = -1;
#define DEFAULT_REMOTE_PORT_UDP 14550 ///< GCS port per MAVLink spec
#define DEFAULT_DEVICE_NAME "/dev/ttyS1"
#define MAX_DATA_RATE 10000000 ///< max data rate in bytes/s
@ -439,8 +434,8 @@ Mavlink::destroy_all_instances()
iterations++;
if (iterations > 1000) {
warnx("ERROR: Couldn't stop all mavlink instances.");
return ERROR;
PX4_ERR("Couldn't stop all mavlink instances.");
return PX4_ERROR;
}
}
@ -695,7 +690,7 @@ int Mavlink::mavlink_open_uart(int baud, const char *uart_name)
case 1000000: speed = B1000000; break;
default:
warnx("ERROR: Unsupported baudrate: %d\n\tsupported examples:\n\t9600, 19200, 38400, 57600\t\n115200\n230400\n460800\n921600\n1000000\n",
PX4_ERR("Unsupported baudrate: %d\n\tsupported examples:\n\t9600, 19200, 38400, 57600\t\n115200\n230400\n460800\n921600\n1000000\n",
baud);
return -EINVAL;
}
@ -862,7 +857,7 @@ Mavlink::set_hil_enabled(bool hil_enabled)
configure_stream("HIL_CONTROLS", 0.0f);
} else {
ret = ERROR;
ret = PX4_ERROR;
}
return ret;
@ -1362,7 +1357,7 @@ Mavlink::configure_stream(const char *stream_name, const float rate)
/* if we reach here, the stream list does not contain the stream */
warnx("stream %s not found", stream_name);
return ERROR;
return PX4_ERROR;
}
void
@ -1432,7 +1427,7 @@ Mavlink::message_buffer_init(int size)
int ret;
if (_message_buffer.data == 0) {
ret = ERROR;
ret = PX4_ERROR;
_message_buffer.size = 0;
} else {
@ -1824,7 +1819,7 @@ Mavlink::task_main(int argc, char *argv[])
if (err_flag) {
usage();
return ERROR;
return PX4_ERROR;
}
if (_datarate == 0) {
@ -1839,7 +1834,7 @@ Mavlink::task_main(int argc, char *argv[])
if (get_protocol() == SERIAL) {
if (Mavlink::instance_exists(_device_name, this)) {
warnx("%s already running", _device_name);
return ERROR;
return PX4_ERROR;
}
PX4_INFO("mode: %s, data rate: %d B/s on %s @ %dB",
@ -1853,7 +1848,7 @@ Mavlink::task_main(int argc, char *argv[])
if (_uart_fd < 0 && _mode != MAVLINK_MODE_CONFIG) {
warn("could not open %s", _device_name);
return ERROR;
return PX4_ERROR;
} else if (_uart_fd < 0 && _mode == MAVLINK_MODE_CONFIG) {
/* the config link is optional */
@ -1863,7 +1858,7 @@ Mavlink::task_main(int argc, char *argv[])
} else if (get_protocol() == UDP) {
if (Mavlink::get_instance_for_network_port(_network_port) != nullptr) {
warnx("port %d already occupied", _network_port);
return ERROR;
return PX4_ERROR;
}
PX4_INFO("mode: %s, data rate: %d B/s on udp port %hu remote port %hu",

View File

@ -47,18 +47,13 @@
#include <lib/geo/geo.h>
#include <systemlib/err.h>
#include <drivers/drv_hrt.h>
#include <px4_defines.h>
#include <dataman/dataman.h>
#include <navigator/navigation.h>
#include <uORB/topics/mission.h>
#include <uORB/topics/mission_result.h>
/* oddly, ERROR is not defined for c++ */
#ifdef ERROR
# undef ERROR
#endif
static const int ERROR = -1;
int MavlinkMissionManager::_dataman_id = 0;
bool MavlinkMissionManager::_dataman_init = false;
unsigned MavlinkMissionManager::_count = 0;
@ -178,7 +173,7 @@ MavlinkMissionManager::update_active_mission(int dataman_id, unsigned count, int
orb_publish(ORB_ID(offboard_mission), _offboard_mission_pub, &mission);
}
return OK;
return PX4_OK;
} else {
warnx("WPM: ERROR: can't save mission state");
@ -187,7 +182,7 @@ MavlinkMissionManager::update_active_mission(int dataman_id, unsigned count, int
_mavlink->send_statustext_critical("Mission storage: Unable to write to microSD");
}
return ERROR;
return PX4_ERROR;
}
}
@ -507,7 +502,7 @@ MavlinkMissionManager::handle_mission_set_current(const mavlink_message_t *msg)
_time_last_recv = hrt_absolute_time();
if (wpc.seq < _count) {
if (update_active_mission(_dataman_id, _count, wpc.seq) == OK) {
if (update_active_mission(_dataman_id, _count, wpc.seq) == PX4_OK) {
if (_verbose) { warnx("WPM: MISSION_SET_CURRENT seq=%d OK", wpc.seq); }
} else {
@ -792,7 +787,7 @@ MavlinkMissionManager::handle_mission_item_both(const mavlink_message_t *msg)
int ret = parse_mavlink_mission_item(&wp, &mission_item);
if (ret != OK) {
if (ret != PX4_OK) {
if (_verbose) { warnx("WPM: MISSION_ITEM ERROR: seq %u invalid item", wp.seq); }
_mavlink->send_statustext_critical("IGN MISSION_ITEM: Busy");
@ -831,7 +826,7 @@ MavlinkMissionManager::handle_mission_item_both(const mavlink_message_t *msg)
_state = MAVLINK_WPM_STATE_IDLE;
if (update_active_mission(_transfer_dataman_id, _transfer_count, _transfer_current_seq) == OK) {
if (update_active_mission(_transfer_dataman_id, _transfer_count, _transfer_current_seq) == PX4_OK) {
send_mission_ack(_transfer_partner_sysid, _transfer_partner_compid, MAV_MISSION_ACCEPTED);
} else {
@ -860,7 +855,7 @@ MavlinkMissionManager::handle_mission_clear_all(const mavlink_message_t *msg)
/* don't touch mission items storage itself, but only items count in mission state */
_time_last_recv = hrt_absolute_time();
if (update_active_mission(_dataman_id == 0 ? 1 : 0, 0, 0) == OK) {
if (update_active_mission(_dataman_id == 0 ? 1 : 0, 0, 0) == PX4_OK) {
if (_verbose) { warnx("WPM: CLEAR_ALL OK"); }
send_mission_ack(_transfer_partner_sysid, _transfer_partner_compid, MAV_MISSION_ACCEPTED);
@ -1050,7 +1045,7 @@ MavlinkMissionManager::format_mavlink_mission_item(const struct mission_item_s *
break;
default:
return ERROR;
return PX4_ERROR;
}
} else {
@ -1122,11 +1117,11 @@ MavlinkMissionManager::format_mavlink_mission_item(const struct mission_item_s *
break;
default:
return ERROR;
return PX4_ERROR;
}
}
return OK;
return PX4_OK;
}

View File

@ -49,6 +49,7 @@
#include <stdio.h>
#include <ctype.h>
#include <px4_config.h>
#include <px4_defines.h>
#include <unistd.h>
#include <geo/geo.h>
#include <drivers/drv_hrt.h>
@ -56,13 +57,6 @@
#define GEOFENCE_RANGE_WARNING_LIMIT 5000000
/* Oddly, ERROR is not defined for C++ */
#ifdef ERROR
# undef ERROR
#endif
static const int ERROR = -1;
Geofence::Geofence(Navigator *navigator) :
SuperBlock(NULL, "GF"),
_navigator(navigator),
@ -323,7 +317,7 @@ Geofence::loadFromFile(const char *filename)
int pointCounter = 0;
bool gotVertical = false;
const char commentChar = '#';
int rc = ERROR;
int rc = PX4_ERROR;
/* Make sure no data is left in the datamanager */
clearDm();
@ -332,7 +326,7 @@ Geofence::loadFromFile(const char *filename)
fp = fopen(GEOFENCE_FILENAME, "r");
if (fp == NULL) {
return ERROR;
return PX4_ERROR;
}
/* create geofence points from valid lines and store in DM */
@ -408,7 +402,7 @@ Geofence::loadFromFile(const char *filename)
_vertices_count = pointCounter;
warnx("Geofence: imported successfully");
mavlink_log_info(_navigator->get_mavlink_log_pub(), "Geofence imported");
rc = OK;
rc = PX4_OK;
} else {
warnx("Geofence: import error");
@ -423,5 +417,5 @@ error:
int Geofence::clearDm()
{
dm_clear(DM_KEY_FENCE_POINTS);
return OK;
return PX4_OK;
}

View File

@ -39,8 +39,7 @@
*/
#include <px4_config.h>
//#include <drivers/device/i2c.h>
#include <px4_defines.h>
#include <sys/types.h>
#include <stdint.h>
@ -55,10 +54,6 @@
#include <math.h>
#include <unistd.h>
//#include <nuttx/arch.h>
//#include <nuttx/wqueue.h>
//#include <nuttx/clock.h>
#include <px4_workqueue.h>
#include <arch/board/board.h>
@ -79,12 +74,6 @@
/* Default I2C bus */
#define PX4_I2C_BUS_DEFAULT PX4_I2C_BUS_EXPANSION
/* Oddly, ERROR is not defined for C++ */
#ifdef ERROR
# undef ERROR
#endif
static const int ERROR = -1;
#ifndef CONFIG_SCHED_WORKQUEUE
# error This requires CONFIG_SCHED_WORKQUEUE.
#endif

View File

@ -37,6 +37,7 @@
*/
#include <sys/types.h>
#include <px4_defines.h>
#include <inttypes.h>
#include <stdio.h>
#include <stdbool.h>
@ -72,12 +73,6 @@ using namespace DriverFramework;
#define TIMEOUT_5HZ 500
#define RATE_MEASUREMENT_PERIOD 5000000
/* oddly, ERROR is not defined for c++ */
#ifdef ERROR
# undef ERROR
#endif
static const int ERROR = -1;
/* class for dynamic allocation of satellite info data */
class GPS_Sat_Info
{
@ -220,7 +215,7 @@ GPSSIM::~GPSSIM()
int
GPSSIM::init()
{
int ret = ERROR;
int ret = PX4_ERROR;
/* do regular cdev init */
if (VirtDevObj::init() != OK) {