diff --git a/src/modules/sensors/CMakeLists.txt b/src/modules/sensors/CMakeLists.txt index 2bf8021023..80c59712e1 100644 --- a/src/modules/sensors/CMakeLists.txt +++ b/src/modules/sensors/CMakeLists.txt @@ -1,6 +1,6 @@ ############################################################################ # -# Copyright (c) 2015 PX4 Development Team. All rights reserved. +# Copyright (c) 2015-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 @@ -32,11 +32,11 @@ ############################################################################ set(SENSORS_SRCS sensors.cpp) if (${OS} STREQUAL "qurt") - list(APPEND SENSORS_SRCS - sensors_init_qurt.cpp) -elseif(${OS} STREQUAL "nuttx") - list(APPEND SENSORS_SRCS - sensors_init_nuttx.cpp) + list(APPEND SENSORS_SRCS + sensors_init_qurt.cpp) + else() + list(APPEND SENSORS_SRCS + sensors_init.cpp) endif() px4_add_module( @@ -52,4 +52,4 @@ px4_add_module( DEPENDS platforms__common ) -# vim: set noet ft=cmake fenc=utf-8 ff=unix : +# vim: set noet ft=cmake fenc=utf-8 ff=unix : diff --git a/src/modules/sensors/sensors.cpp b/src/modules/sensors/sensors.cpp index 5d508e45af..50d21a4bb6 100644 --- a/src/modules/sensors/sensors.cpp +++ b/src/modules/sensors/sensors.cpp @@ -101,13 +101,12 @@ #include #include -#include "DevMgr.hpp" +#include + +#include "sensors_init.h" using namespace DriverFramework; -/* Platform specific sensor initialization function. */ -extern int sensors_init(void); - /** * Analog layout: * FMU: @@ -397,26 +396,6 @@ private: */ int parameters_update(); - /** - * 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(); - /** * Do adc-related initialisation. */ @@ -972,104 +951,6 @@ Sensors::parameters_update() return OK; } -int -Sensors::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 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 -Sensors::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 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 -Sensors::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 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 ERROR; - } - } - - return OK; -} - -int -Sensors::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 ERROR; - } - - /* set the driver to poll at 150Hz */ - h_baro.ioctl(SENSORIOCSPOLLRATE, 150); - - return OK; -} int Sensors::adc_init() @@ -2101,39 +1982,13 @@ Sensors::task_main() /* start individual sensors */ int ret = 0; - -// TODO-JYW: TESTING-TESTING: + /* This calls a sensors_init which can have different implementations on NuttX, POSIX, QURT. */ ret = sensors_init(); -///* -// * QURT devices do not yet use DriverFramework based drivers, which is required for -// * the following initialization sequence to work correctly. -// */ -//#ifndef __PX4_QURT -// do { /* create a scope to handle exit with break */ -// ret = accel_init(); -// -// if (ret) { break; } -// -// ret = gyro_init(); -// -// if (ret) { break; } -// -// ret = mag_init(); -// -// if (ret) { break; } -// -// ret = baro_init(); -// -// if (ret) { break; } -// -// ret = adc_init(); -// -// if (ret) { break; } -// -// break; -// } while (0); -//#endif +#ifndef __PX4_QURT + // TODO: move adc_init into the sensors_init call. + ret = ret || adc_init(); +#endif if (ret) { warnx("sensor initialization failed"); diff --git a/src/modules/sensors/sensors_init.cpp b/src/modules/sensors/sensors_init.cpp new file mode 100644 index 0000000000..cee4951af2 --- /dev/null +++ b/src/modules/sensors/sensors_init.cpp @@ -0,0 +1,205 @@ +/**************************************************************************** + * + * 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; + +/* oddly, ERROR is not defined for c++ */ +#ifdef ERROR +# undef ERROR +#endif +static const int ERROR = -1; + +/** + * 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(void) +{ + int ret; + + ret = accel_init(); + + if (ret) { return ret; } + + ret = gyro_init(); + + if (ret) { return ret; } + + ret = mag_init(); + + if (ret) { return ret; } + + ret = baro_init(); + + if (ret) { return ret; } + + return 0; +} + + +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 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 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 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 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 ERROR; + } + + /* set the driver to poll at 150Hz */ + h_baro.ioctl(SENSORIOCSPOLLRATE, 150); + + return OK; +} + diff --git a/src/modules/sensors/sensors_init_nuttx.cpp b/src/modules/sensors/sensors_init.h similarity index 77% rename from src/modules/sensors/sensors_init_nuttx.cpp rename to src/modules/sensors/sensors_init.h index 2016492e20..8ded238e1c 100644 --- a/src/modules/sensors/sensors_init_nuttx.cpp +++ b/src/modules/sensors/sensors_init.h @@ -1,6 +1,6 @@ /**************************************************************************** * - * Copyright (c) 2015 James Wilson. All rights reserved. + * 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 @@ -31,41 +31,15 @@ * ****************************************************************************/ +#pragma once + /** - * @file sensors_init_nuttx.cpp + * @file sensors_init.h * - * Sensor initialization code, used only in Nuttx platform builds. + * Sensor initialization code declaration, the specific implementations are + * in the respective cpp files.. * - * @author James Wilson + * @author Julian Oes */ -int -sensors_init(void) -{ - int ret; - - ret = accel_init(); - - if (ret) { return ret; } - - ret = gyro_init(); - - if (ret) { return ret; } - - ret = mag_init(); - - if (ret) { return ret; } - - ret = baro_init(); - - if (ret) { return ret; } - - ret = adc_init(); - - if (ret) { return ret; } - - return 0; -} - - - +int sensors_init(void); diff --git a/src/modules/sensors/sensors_init_qurt.cpp b/src/modules/sensors/sensors_init_qurt.cpp index 8b7008a82e..ac439dc531 100644 --- a/src/modules/sensors/sensors_init_qurt.cpp +++ b/src/modules/sensors/sensors_init_qurt.cpp @@ -1,6 +1,6 @@ /**************************************************************************** * - * Copyright (c) 2015 James Wilson. All rights reserved. + * 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 @@ -39,6 +39,8 @@ * @author James Wilson */ +#include "sensors_init.h" + int sensors_init(void) {