zrzk
/
px4_autopilot
1
0
Fork 0
Code Issues Pull Requests Projects Releases Wiki Activity
Browse Source

sensors: remove sensors_init 

The initialization code is redundant and incomplete (only the first sensor
is done). I verified that all drivers already set this on startup.
For the mags, they all set their maximum supported update rate.
For the baro, the call can silently fail, as for example the MS5611 which
does not support 150Hz update. But it also sets the maximum in
initialization.

Tested on Pixhawk & pixracer.
sbg
Beat Küng 8 years ago committed by Lorenz Meier
parent
019386a5a0
commit
9597e708df
4 changed files with 2 additions and 284 deletions
Whitespace
Show all changes Ignore whitespace when comparing lines Ignore changes in amount of whitespace Ignore changes in whitespace at EOL
Split View
Diff Options
Show Stats Download Patch File Download Diff File
  1. 1
      src/modules/sensors/CMakeLists.txt
  2. 15
      src/modules/sensors/sensors.cpp
  3. 220
      src/modules/sensors/sensors_init.cpp
  4. 50
      src/modules/sensors/sensors_init.h

1
src/modules/sensors/CMakeLists.txt
Unescape View File

@ -41,7 +41,6 @@ px4_add_module( @@ -41,7 +41,6 @@ px4_add_module(
voted_sensors_update.cpp
rc_update.cpp
sensors.cpp
sensors_init.cpp
parameters.cpp
temperature_compensation.cpp

15
src/modules/sensors/sensors.cpp
Unescape View File

@ -91,7 +91,6 @@ @@ -91,7 +91,6 @@
#include <DevMgr.hpp>
#include "sensors_init.h"
#include "parameters.h"
#include "rc_update.h"
#include "voted_sensors_update.h"
@ -158,11 +157,10 @@ public: @@ -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 @@ -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 = {};

220
src/modules/sensors/sensors_init.cpp
Unescape View File

@ -1,220 +0,0 @@ @@ -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 <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;
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 */

50
src/modules/sensors/sensors_init.h
Unescape View File

@ -1,50 +0,0 @@ @@ -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 <julian@oes.ch>
*/
namespace sensors
{
int sensors_init(void);
} /* namespace sensors */
Write Preview
Loading…
Cancel
Save