diff --git a/src/modules/sensors/CMakeLists.txt b/src/modules/sensors/CMakeLists.txt index 40c801236fc..efad6e26e1e 100644 --- a/src/modules/sensors/CMakeLists.txt +++ b/src/modules/sensors/CMakeLists.txt @@ -41,7 +41,6 @@ px4_add_module( voted_sensors_update.cpp rc_update.cpp sensors.cpp - sensors_init.cpp parameters.cpp temperature_compensation.cpp diff --git a/src/modules/sensors/sensors.cpp b/src/modules/sensors/sensors.cpp index 6fcff4cc3ec..a6c0c54e69c 100644 --- a/src/modules/sensors/sensors.cpp +++ b/src/modules/sensors/sensors.cpp @@ -91,7 +91,6 @@ #include -#include "sensors_init.h" #include "parameters.h" #include "rc_update.h" #include "voted_sensors_update.h" @@ -158,11 +157,10 @@ public: void print_status(); private: - /* XXX should not be here - should be own driver */ DevHandle _h_adc; /**< ADC driver handle */ hrt_abstime _last_adc; /**< last time we took input from the ADC */ - bool _task_should_exit; /**< if true, sensor task should exit */ + volatile bool _task_should_exit; /**< if true, sensor task should exit */ int _sensors_task; /**< task handle for sensor task */ const bool _hil_enabled; /**< if true, HIL is active */ @@ -553,23 +551,14 @@ void Sensors::task_main() { - /* start individual sensors */ int ret = 0; if (!_hil_enabled) { - /* This calls a sensors_init which can have different implementations on NuttX, POSIX, QURT. */ - ret = sensors_init(); - #if !defined(__PX4_QURT) && !defined(__PX4_POSIX_RPI) && !defined(__PX4_POSIX_BEBOP) - // TODO: move adc_init into the sensors_init call. - ret = ret || adc_init(); + adc_init(); #endif } - if (ret) { - PX4_ERR("sensor initialization failed"); - } - struct sensor_combined_s raw = {}; struct sensor_preflight_s preflt = {}; diff --git a/src/modules/sensors/sensors_init.cpp b/src/modules/sensors/sensors_init.cpp deleted file mode 100644 index 404026a755d..00000000000 --- a/src/modules/sensors/sensors_init.cpp +++ /dev/null @@ -1,220 +0,0 @@ -/**************************************************************************** - * - * Copyright (c) 2016 James Wilson. 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 sensors_init.cpp - * - * Sensor initialization code, used on everything but QURT. - * - * @author James Wilson - */ - -#include -#include -#include -#include -#include - - -#include - -#include "sensors_init.h" - -using namespace DriverFramework; - -namespace sensors -{ - -#if defined(__PX4_QURT) || defined(__PX4_POSIX_RPI) || defined(__PX4_POSIX_BEBOP) - -// Sensor initialization is performed automatically when the QuRT sensor drivers -// are loaded. -// The same is true for the Raspberry Pi. - -int -sensors_init(void) -{ - return 0; -} - -#else - -/** - * Do accel-related initialisation. - */ -int accel_init(); - -/** - * Do gyro-related initialisation. - */ -int gyro_init(); - -/** - * Do mag-related initialisation. - */ -int mag_init(); - -/** - * Do baro-related initialisation. - */ -int baro_init(); - - -int -sensors_init() -{ - int ret; - int ret_combined = 0; - - ret = accel_init(); - - if (ret) { ret_combined = ret; } - - ret = gyro_init(); - - if (ret) { ret_combined = ret; } - - ret = mag_init(); - - if (ret) { ret_combined = ret; } - - ret = baro_init(); - - if (ret) { ret_combined = ret; } - - return ret_combined; -} - - -int -accel_init() -{ - DevHandle h_accel; - DevMgr::getHandle(ACCEL0_DEVICE_PATH, h_accel); - - if (!h_accel.isValid()) { - warnx("FATAL: no accelerometer found: %s (%d)", ACCEL0_DEVICE_PATH, h_accel.getError()); - return PX4_ERROR; - - } else { - - /* set the accel internal sampling rate to default rate */ - h_accel.ioctl(ACCELIOCSSAMPLERATE, ACCEL_SAMPLERATE_DEFAULT); - - /* set the driver to poll at default rate */ - h_accel.ioctl(SENSORIOCSPOLLRATE, SENSOR_POLLRATE_DEFAULT); - } - - return OK; -} - -int -gyro_init() -{ - DevHandle h_gyro; - DevMgr::getHandle(GYRO0_DEVICE_PATH, h_gyro); - - if (!h_gyro.isValid()) { - warnx("FATAL: no gyro found: %s (%d)", GYRO0_DEVICE_PATH, h_gyro.getError()); - return PX4_ERROR; - - } - - /* set the gyro internal sampling rate to default rate */ - h_gyro.ioctl(GYROIOCSSAMPLERATE, GYRO_SAMPLERATE_DEFAULT); - - /* set the driver to poll at default rate */ - h_gyro.ioctl(SENSORIOCSPOLLRATE, SENSOR_POLLRATE_DEFAULT); - - return OK; -} - -int -mag_init() -{ - int ret; - - DevHandle h_mag; - DevMgr::getHandle(MAG0_DEVICE_PATH, h_mag); - - if (!h_mag.isValid()) { - warnx("FATAL: no magnetometer found: %s (%d)", MAG0_DEVICE_PATH, h_mag.getError()); - return PX4_ERROR; - } - - /* try different mag sampling rates */ - - - ret = h_mag.ioctl(MAGIOCSSAMPLERATE, 150); - - if (ret == OK) { - /* set the pollrate accordingly */ - h_mag.ioctl(SENSORIOCSPOLLRATE, 150); - - } else { - ret = h_mag.ioctl(MAGIOCSSAMPLERATE, 100); - - /* if the slower sampling rate still fails, something is wrong */ - if (ret == OK) { - /* set the driver to poll also at the slower rate */ - h_mag.ioctl(SENSORIOCSPOLLRATE, 100); - - } else { - warnx("FATAL: mag sampling rate could not be set"); - return PX4_ERROR; - } - } - - return OK; -} - -int -baro_init() -{ - DevHandle h_baro; - DevMgr::getHandle(BARO0_DEVICE_PATH, h_baro); - - if (!h_baro.isValid()) { - warnx("FATAL: No barometer found: %s (%d)", BARO0_DEVICE_PATH, h_baro.getError()); - return PX4_ERROR; - } - - /* set the driver to poll at 150Hz */ - h_baro.ioctl(SENSORIOCSPOLLRATE, 150); - - return OK; -} - -#endif - -} /* namespace sensors */ diff --git a/src/modules/sensors/sensors_init.h b/src/modules/sensors/sensors_init.h deleted file mode 100644 index 0459eb1a9bb..00000000000 --- a/src/modules/sensors/sensors_init.h +++ /dev/null @@ -1,50 +0,0 @@ -/**************************************************************************** - * - * Copyright (c) 2016 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. - * - ****************************************************************************/ - -#pragma once - -/** - * @file sensors_init.h - * - * Sensor initialization code declaration, the specific implementations are - * in the respective cpp files.. - * - * @author Julian Oes - */ - -namespace sensors -{ - -int sensors_init(void); - -} /* namespace sensors */