forked from Archive/PX4-Autopilot
sensors: get it to compile on NuttX and POSIX
This commit is contained in:
Julian Oes
parent
ebfcf32323
commit
c99402b04c
|
|
@ -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 :
|
||||
|
|
|
|||
|
|
@ -101,13 +101,12 @@
|
|||
#include <uORB/topics/airspeed.h>
|
||||
#include <uORB/topics/rc_parameter_map.h>
|
||||
|
||||
#include "DevMgr.hpp"
|
||||
#include <DevMgr.hpp>
|
||||
|
||||
#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");
|
||||
|
|
|
|||
|
|
@ -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 <jywilson99@hotmail.com>
|
||||
*/
|
||||
|
||||
#include <drivers/drv_accel.h>
|
||||
#include <drivers/drv_gyro.h>
|
||||
#include <drivers/drv_mag.h>
|
||||
#include <drivers/drv_baro.h>
|
||||
#include <drivers/drv_adc.h>
|
||||
|
||||
|
||||
#include <DevMgr.hpp>
|
||||
|
||||
#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;
|
||||
}
|
||||
|
||||
|
|
@ -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 <jywilson99@hotmail.com>
|
||||
* @author Julian Oes <julian@oes.ch>
|
||||
*/
|
||||
|
||||
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);
|
||||
|
|
@ -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 <jywilson99@hotmail.com>
|
||||
*/
|
||||
|
||||
#include "sensors_init.h"
|
||||
|
||||
int
|
||||
sensors_init(void)
|
||||
{
|
||||
|
|
|
|||
Loading…
Reference in New Issue