forked from Archive/PX4-Autopilot
Merge branch 'local/c++_sensors' into px4dev_new_driver
This commit is contained in:
px4dev
commit
bdfcff9bc9
|
|
@ -95,6 +95,7 @@ end
|
|||
document showfiles
|
||||
. showfiles <TCB pointer>
|
||||
. Prints the files opened by a task.
|
||||
end
|
||||
|
||||
################################################################################
|
||||
# Task display
|
||||
|
|
|
|||
|
|
@ -99,7 +99,7 @@ I2C::init()
|
|||
}
|
||||
|
||||
// tell the world where we are
|
||||
log("on bus %d at 0x%02x", _bus, _address);
|
||||
log("on I2C bus %d at 0x%02x", _bus, _address);
|
||||
|
||||
out:
|
||||
return ret;
|
||||
|
|
|
|||
|
|
@ -111,7 +111,7 @@ SPI::init()
|
|||
}
|
||||
|
||||
// tell the workd where we are
|
||||
log("on bus %d at %d", _bus, _device);
|
||||
log("on SPI bus %d at %d", _bus, _device);
|
||||
|
||||
out:
|
||||
return ret;
|
||||
|
|
|
|||
|
|
@ -147,12 +147,9 @@ private:
|
|||
|
||||
orb_advert_t _mag_topic;
|
||||
|
||||
unsigned _reads;
|
||||
unsigned _measure_errors;
|
||||
unsigned _read_errors;
|
||||
unsigned _buf_overflows;
|
||||
|
||||
perf_counter_t _sample_perf;
|
||||
perf_counter_t _comms_errors;
|
||||
perf_counter_t _buffer_overflows;
|
||||
|
||||
/**
|
||||
* Test whether the device supported by the driver is present at a
|
||||
|
|
@ -256,11 +253,9 @@ HMC5883::HMC5883(int bus) :
|
|||
_oldest_report(0),
|
||||
_reports(nullptr),
|
||||
_mag_topic(-1),
|
||||
_reads(0),
|
||||
_measure_errors(0),
|
||||
_read_errors(0),
|
||||
_buf_overflows(0),
|
||||
_sample_perf(perf_alloc(PC_ELAPSED, "hmc5883_read"))
|
||||
_sample_perf(perf_alloc(PC_ELAPSED, "hmc5883_read")),
|
||||
_comms_errors(perf_alloc(PC_COUNT, "hmc5883_comms_errors")),
|
||||
_buffer_overflows(perf_alloc(PC_COUNT, "hmc5883_buffer_overflows"))
|
||||
{
|
||||
// enable debug() calls
|
||||
_debug_enabled = true;
|
||||
|
|
@ -303,6 +298,12 @@ HMC5883::init()
|
|||
goto out;
|
||||
_oldest_report = _next_report = 0;
|
||||
|
||||
/* get a publish handle on the mag topic */
|
||||
memset(&_reports[0], 0, sizeof(_reports[0]));
|
||||
_mag_topic = orb_advertise(ORB_ID(sensor_mag), &_reports[0]);
|
||||
if (_mag_topic < 0)
|
||||
debug("failed to create sensor_mag object");
|
||||
|
||||
ret = OK;
|
||||
out:
|
||||
return ret;
|
||||
|
|
@ -313,7 +314,7 @@ HMC5883::probe()
|
|||
{
|
||||
uint8_t data[3] = {0, 0, 0};
|
||||
|
||||
_retries = 3;
|
||||
_retries = 10;
|
||||
if (read_reg(ADDR_ID_A, data[0]) ||
|
||||
read_reg(ADDR_ID_B, data[1]) ||
|
||||
read_reg(ADDR_ID_C, data[2]))
|
||||
|
|
@ -356,8 +357,6 @@ HMC5883::read(struct file *filp, char *buffer, size_t buflen)
|
|||
}
|
||||
}
|
||||
|
||||
_reads++;
|
||||
|
||||
/* if there was no data, warn the caller */
|
||||
return ret ? ret : -EAGAIN;
|
||||
}
|
||||
|
|
@ -385,7 +384,6 @@ HMC5883::read(struct file *filp, char *buffer, size_t buflen)
|
|||
/* state machine will have generated a report, copy it out */
|
||||
memcpy(buffer, _reports, sizeof(*_reports));
|
||||
ret = sizeof(*_reports);
|
||||
_reads++;
|
||||
|
||||
} while (0);
|
||||
|
||||
|
|
@ -548,31 +546,13 @@ HMC5883::cycle_trampoline(void *arg)
|
|||
void
|
||||
HMC5883::cycle()
|
||||
{
|
||||
/*
|
||||
* We have to publish the mag topic in the context of the workq
|
||||
* in order to ensure that the descriptor is valid when we go to publish.
|
||||
*
|
||||
* @bug We can't really ever be torn down and restarted, since this
|
||||
* descriptor will never be closed and on the restart we will be
|
||||
* unable to re-advertise.
|
||||
*/
|
||||
if (_mag_topic == -1) {
|
||||
struct mag_report m;
|
||||
|
||||
/* if this fails (e.g. no object in the system) we will cope */
|
||||
memset(&m, 0, sizeof(m));
|
||||
_mag_topic = orb_advertise(ORB_ID(sensor_mag), &m);
|
||||
|
||||
if (_mag_topic < 0)
|
||||
debug("failed to create sensor_mag object");
|
||||
}
|
||||
|
||||
/* collection phase? */
|
||||
if (_collect_phase) {
|
||||
|
||||
/* perform collection */
|
||||
if (OK != collect()) {
|
||||
log("FATAL collection error - restarting\n");
|
||||
log("collection error");
|
||||
/* restart the measurement state machine */
|
||||
start();
|
||||
return;
|
||||
}
|
||||
|
|
@ -596,10 +576,8 @@ HMC5883::cycle()
|
|||
}
|
||||
|
||||
/* measurement phase */
|
||||
if (OK != measure()) {
|
||||
log("FATAL measure error - restarting\n");
|
||||
start();
|
||||
}
|
||||
if (OK != measure())
|
||||
log("measure error");
|
||||
|
||||
/* next phase is collection */
|
||||
_collect_phase = true;
|
||||
|
|
@ -622,7 +600,7 @@ HMC5883::measure()
|
|||
ret = write_reg(ADDR_MODE, MODE_REG_SINGLE_MODE);
|
||||
|
||||
if (OK != ret)
|
||||
_measure_errors++;
|
||||
perf_count(_comms_errors);
|
||||
|
||||
return ret;
|
||||
}
|
||||
|
|
@ -661,6 +639,7 @@ HMC5883::collect()
|
|||
ret = transfer(&cmd, 1, (uint8_t *)&hmc_report, sizeof(hmc_report));
|
||||
|
||||
if (ret != OK) {
|
||||
perf_count(_comms_errors);
|
||||
debug("data/status read error");
|
||||
goto out;
|
||||
}
|
||||
|
|
@ -692,7 +671,7 @@ HMC5883::collect()
|
|||
|
||||
/* if we are running up against the oldest report, toss it */
|
||||
if (_next_report == _oldest_report) {
|
||||
_buf_overflows++;
|
||||
perf_count(_buffer_overflows);
|
||||
INCREMENT(_oldest_report, _num_reports);
|
||||
}
|
||||
|
||||
|
|
@ -737,10 +716,9 @@ HMC5883::meas_to_float(uint8_t in[2])
|
|||
void
|
||||
HMC5883::print_info()
|
||||
{
|
||||
printf("reads: %u\n", _reads);
|
||||
printf("measure errors: %u\n", _measure_errors);
|
||||
printf("read errors: %u\n", _read_errors);
|
||||
printf("read overflows: %u\n", _buf_overflows);
|
||||
perf_print_counter(_sample_perf);
|
||||
perf_print_counter(_comms_errors);
|
||||
perf_print_counter(_buffer_overflows);
|
||||
printf("poll interval: %u ticks\n", _measure_ticks);
|
||||
printf("report queue: %u (%u/%u @ %p)\n",
|
||||
_num_reports, _oldest_report, _next_report, _reports);
|
||||
|
|
|
|||
|
|
@ -298,8 +298,8 @@ MPU6000::MPU6000(int bus, spi_dev_e device) :
|
|||
_reads(0),
|
||||
_sample_perf(perf_alloc(PC_ELAPSED, "mpu6000_read"))
|
||||
{
|
||||
// enable debug() calls
|
||||
_debug_enabled = true;
|
||||
// disable debug() calls
|
||||
_debug_enabled = false;
|
||||
|
||||
// default accel scale factors
|
||||
_accel_scale.x_offset = 0;
|
||||
|
|
@ -463,7 +463,7 @@ MPU6000::probe()
|
|||
case MPU6000_REV_D8:
|
||||
case MPU6000_REV_D9:
|
||||
case MPU6000_REV_D10:
|
||||
log("ID 0x%02x", _product);
|
||||
debug("ID 0x%02x", _product);
|
||||
return OK;
|
||||
}
|
||||
|
||||
|
|
|
|||
|
|
@ -132,12 +132,9 @@ private:
|
|||
|
||||
orb_advert_t _baro_topic;
|
||||
|
||||
unsigned _reads;
|
||||
unsigned _measure_errors;
|
||||
unsigned _read_errors;
|
||||
unsigned _buf_overflows;
|
||||
|
||||
perf_counter_t _sample_perf;
|
||||
perf_counter_t _comms_errors;
|
||||
perf_counter_t _buffer_overflows;
|
||||
|
||||
/**
|
||||
* Test whether the device supported by the driver is present at a
|
||||
|
|
@ -252,11 +249,9 @@ MS5611::MS5611(int bus) :
|
|||
_dT(0),
|
||||
_temp64(0),
|
||||
_baro_topic(-1),
|
||||
_reads(0),
|
||||
_measure_errors(0),
|
||||
_read_errors(0),
|
||||
_buf_overflows(0),
|
||||
_sample_perf(perf_alloc(PC_ELAPSED, "ms5611_read"))
|
||||
_sample_perf(perf_alloc(PC_ELAPSED, "ms5611_read")),
|
||||
_comms_errors(perf_alloc(PC_COUNT, "ms5611_comms_errors")),
|
||||
_buffer_overflows(perf_alloc(PC_COUNT, "ms5611_buffer_overflows"))
|
||||
{
|
||||
// enable debug() calls
|
||||
_debug_enabled = true;
|
||||
|
|
@ -292,6 +287,12 @@ MS5611::init()
|
|||
|
||||
_oldest_report = _next_report = 0;
|
||||
|
||||
/* get a publish handle on the baro topic */
|
||||
memset(&_reports[0], 0, sizeof(_reports[0]));
|
||||
_baro_topic = orb_advertise(ORB_ID(sensor_baro), &_reports[0]);
|
||||
if (_baro_topic < 0)
|
||||
debug("failed to create sensor_baro object");
|
||||
|
||||
ret = OK;
|
||||
out:
|
||||
return ret;
|
||||
|
|
@ -300,7 +301,7 @@ out:
|
|||
int
|
||||
MS5611::probe()
|
||||
{
|
||||
_retries = 3;
|
||||
_retries = 10;
|
||||
if((OK == probe_address(MS5611_ADDRESS_1)) ||
|
||||
(OK == probe_address(MS5611_ADDRESS_2))) {
|
||||
_retries = 1;
|
||||
|
|
@ -358,8 +359,6 @@ MS5611::read(struct file *filp, char *buffer, size_t buflen)
|
|||
}
|
||||
}
|
||||
|
||||
_reads++;
|
||||
|
||||
/* if there was no data, warn the caller */
|
||||
return ret ? ret : -EAGAIN;
|
||||
}
|
||||
|
|
@ -399,7 +398,6 @@ MS5611::read(struct file *filp, char *buffer, size_t buflen)
|
|||
/* state machine will have generated a report, copy it out */
|
||||
memcpy(buffer, _reports, sizeof(*_reports));
|
||||
ret = sizeof(*_reports);
|
||||
_reads++;
|
||||
|
||||
} while (0);
|
||||
|
||||
|
|
@ -541,31 +539,14 @@ MS5611::cycle_trampoline(void *arg)
|
|||
void
|
||||
MS5611::cycle()
|
||||
{
|
||||
/*
|
||||
* We have to publish the baro topic in the context of the workq
|
||||
* in order to ensure that the descriptor is valid when we go to publish.
|
||||
*
|
||||
* @bug We can't really ever be torn down and restarted, since this
|
||||
* descriptor will never be closed and on the restart we will be
|
||||
* unable to re-advertise.
|
||||
*/
|
||||
if (_baro_topic == -1) {
|
||||
struct baro_report b;
|
||||
|
||||
/* if this fails (e.g. no object in the system) we will cope */
|
||||
memset(&b, 0, sizeof(b));
|
||||
_baro_topic = orb_advertise(ORB_ID(sensor_baro), &b);
|
||||
|
||||
if (_baro_topic < 0)
|
||||
debug("failed to create sensor_baro object");
|
||||
}
|
||||
|
||||
/* collection phase? */
|
||||
if (_collect_phase) {
|
||||
|
||||
/* perform collection */
|
||||
if (OK != collect()) {
|
||||
log("FATAL collection error - restarting\n");
|
||||
log("collection error");
|
||||
/* reset the collection state machine and try again */
|
||||
start();
|
||||
return;
|
||||
}
|
||||
|
|
@ -592,10 +573,8 @@ MS5611::cycle()
|
|||
}
|
||||
|
||||
/* measurement phase */
|
||||
if (OK != measure()) {
|
||||
log("FATAL measure error - restarting\n");
|
||||
start();
|
||||
}
|
||||
if (OK != measure())
|
||||
log("measure error");
|
||||
|
||||
/* next phase is collection */
|
||||
_collect_phase = true;
|
||||
|
|
@ -623,7 +602,7 @@ MS5611::measure()
|
|||
ret = transfer(&cmd_data, 1, nullptr, 0);
|
||||
|
||||
if (OK != ret)
|
||||
_measure_errors++;
|
||||
perf_count(_comms_errors);
|
||||
|
||||
return ret;
|
||||
}
|
||||
|
|
@ -643,7 +622,7 @@ MS5611::collect()
|
|||
_reports[_next_report].timestamp = hrt_absolute_time();
|
||||
|
||||
if (OK != transfer(&cmd, 1, &data[0], 3)) {
|
||||
_read_errors++;
|
||||
perf_count(_comms_errors);
|
||||
return -EIO;
|
||||
}
|
||||
|
||||
|
|
@ -691,7 +670,7 @@ MS5611::collect()
|
|||
|
||||
/* if we are running up against the oldest report, toss it */
|
||||
if (_next_report == _oldest_report) {
|
||||
_buf_overflows++;
|
||||
perf_count(_buffer_overflows);
|
||||
INCREMENT(_oldest_report, _num_reports);
|
||||
}
|
||||
|
||||
|
|
@ -774,10 +753,9 @@ MS5611::crc4(uint16_t *n_prom)
|
|||
void
|
||||
MS5611::print_info()
|
||||
{
|
||||
printf("reads: %u\n", _reads);
|
||||
printf("measure errors: %u\n", _measure_errors);
|
||||
printf("read errors: %u\n", _read_errors);
|
||||
printf("read overflows: %u\n", _buf_overflows);
|
||||
perf_print_counter(_sample_perf);
|
||||
perf_print_counter(_comms_errors);
|
||||
perf_print_counter(_buffer_overflows);
|
||||
printf("poll interval: %u ticks\n", _measure_ticks);
|
||||
printf("report queue: %u (%u/%u @ %p)\n",
|
||||
_num_reports, _oldest_report, _next_report, _reports);
|
||||
|
|
|
|||
|
|
@ -39,4 +39,7 @@ APPNAME = sensors
|
|||
PRIORITY = SCHED_PRIORITY_MAX-5
|
||||
STACKSIZE = 4096
|
||||
|
||||
CXXSRCS = sensors.cpp
|
||||
CSRCS = sensor_params.c
|
||||
|
||||
include $(APPDIR)/mk/app.mk
|
||||
|
|
|
|||
|
|
@ -0,0 +1,108 @@
|
|||
/****************************************************************************
|
||||
*
|
||||
* Copyright (C) 2012 PX4 Development Team. All rights reserved.
|
||||
* Author: @author Lorenz Meier <lm@inf.ethz.ch>
|
||||
* @author Thomas Gubler <thomasgubler@student.ethz.ch>
|
||||
* @author Julian Oes <joes@student.ethz.ch>
|
||||
*
|
||||
* 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 sensor_params.c
|
||||
*
|
||||
* Parameters defined by the sensors task.
|
||||
*/
|
||||
|
||||
#include <nuttx/config.h>
|
||||
|
||||
#include <systemlib/param/param.h>
|
||||
|
||||
PARAM_DEFINE_FLOAT(SENSOR_GYRO_XOFF, 0.0f);
|
||||
PARAM_DEFINE_FLOAT(SENSOR_GYRO_YOFF, 0.0f);
|
||||
PARAM_DEFINE_FLOAT(SENSOR_GYRO_ZOFF, 0.0f);
|
||||
|
||||
PARAM_DEFINE_FLOAT(SENSOR_MAG_XOFF, 0.0f);
|
||||
PARAM_DEFINE_FLOAT(SENSOR_MAG_YOFF, 0.0f);
|
||||
PARAM_DEFINE_FLOAT(SENSOR_MAG_ZOFF, 0.0f);
|
||||
|
||||
PARAM_DEFINE_FLOAT(SENSOR_ACC_XOFF, 0.0f);
|
||||
PARAM_DEFINE_FLOAT(SENSOR_ACC_YOFF, 0.0f);
|
||||
PARAM_DEFINE_FLOAT(SENSOR_ACC_ZOFF, 0.0f);
|
||||
|
||||
PARAM_DEFINE_FLOAT(RC1_MIN, 1000.0f);
|
||||
PARAM_DEFINE_FLOAT(RC1_TRIM, 1500.0f);
|
||||
PARAM_DEFINE_FLOAT(RC1_MAX, 2000.0f);
|
||||
PARAM_DEFINE_FLOAT(RC1_REV, 1.0f);
|
||||
|
||||
PARAM_DEFINE_FLOAT(RC2_MIN, 1000);
|
||||
PARAM_DEFINE_FLOAT(RC2_TRIM, 1500);
|
||||
PARAM_DEFINE_FLOAT(RC2_MAX, 2000);
|
||||
PARAM_DEFINE_FLOAT(RC2_REV, 1.0f);
|
||||
|
||||
PARAM_DEFINE_FLOAT(RC3_MIN, 1000);
|
||||
PARAM_DEFINE_FLOAT(RC3_TRIM, 1500);
|
||||
PARAM_DEFINE_FLOAT(RC3_MAX, 2000);
|
||||
PARAM_DEFINE_FLOAT(RC3_REV, 1.0f);
|
||||
|
||||
PARAM_DEFINE_FLOAT(RC4_MIN, 1000);
|
||||
PARAM_DEFINE_FLOAT(RC4_TRIM, 1500);
|
||||
PARAM_DEFINE_FLOAT(RC4_MAX, 2000);
|
||||
PARAM_DEFINE_FLOAT(RC4_REV, 1.0f);
|
||||
|
||||
PARAM_DEFINE_FLOAT(RC5_MIN, 1000);
|
||||
PARAM_DEFINE_FLOAT(RC5_TRIM, 1500);
|
||||
PARAM_DEFINE_FLOAT(RC5_MAX, 2000);
|
||||
PARAM_DEFINE_FLOAT(RC5_REV, 1.0f);
|
||||
|
||||
PARAM_DEFINE_FLOAT(RC6_MIN, 1000);
|
||||
PARAM_DEFINE_FLOAT(RC6_TRIM, 1500);
|
||||
PARAM_DEFINE_FLOAT(RC6_MAX, 2000);
|
||||
PARAM_DEFINE_FLOAT(RC6_REV, 1.0f);
|
||||
|
||||
PARAM_DEFINE_FLOAT(RC7_MIN, 1000);
|
||||
PARAM_DEFINE_FLOAT(RC7_TRIM, 1500);
|
||||
PARAM_DEFINE_FLOAT(RC7_MAX, 2000);
|
||||
PARAM_DEFINE_FLOAT(RC7_REV, 1.0f);
|
||||
|
||||
PARAM_DEFINE_FLOAT(RC8_MIN, 1000);
|
||||
PARAM_DEFINE_FLOAT(RC8_TRIM, 1500);
|
||||
PARAM_DEFINE_FLOAT(RC8_MAX, 2000);
|
||||
PARAM_DEFINE_FLOAT(RC8_REV, 1.0f);
|
||||
|
||||
PARAM_DEFINE_INT32(RC_TYPE, 1); // 1 = FUTABA
|
||||
|
||||
/* default is conversion factor for the PX4IO / PX4IOAR board, the factor for PX4FMU standalone is different */
|
||||
PARAM_DEFINE_FLOAT(BAT_V_SCALING, (3.3f * 52.0f / 5.0f / 4095.0f));
|
||||
|
||||
PARAM_DEFINE_INT32(RC_MAP_ROLL, 1);
|
||||
PARAM_DEFINE_INT32(RC_MAP_PITCH, 2);
|
||||
PARAM_DEFINE_INT32(RC_MAP_THROTTLE, 3);
|
||||
PARAM_DEFINE_INT32(RC_MAP_YAW, 4);
|
||||
PARAM_DEFINE_INT32(RC_MAP_MODE_SW, 5);
|
||||
|
|
@ -0,0 +1,985 @@
|
|||
/****************************************************************************
|
||||
*
|
||||
* Copyright (C) 2012 PX4 Development Team. All rights reserved.
|
||||
* Author: @author Lorenz Meier <lm@inf.ethz.ch>
|
||||
* @author Thomas Gubler <thomasgubler@student.ethz.ch>
|
||||
* @author Julian Oes <joes@student.ethz.ch>
|
||||
*
|
||||
* 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.cpp
|
||||
*
|
||||
* Sensor readout process.
|
||||
*/
|
||||
|
||||
#include <nuttx/config.h>
|
||||
|
||||
#include <fcntl.h>
|
||||
#include <sys/prctl.h>
|
||||
#include <poll.h>
|
||||
#include <nuttx/analog/adc.h>
|
||||
#include <unistd.h>
|
||||
#include <stdlib.h>
|
||||
#include <string.h>
|
||||
#include <stdbool.h>
|
||||
#include <stdio.h>
|
||||
#include <errno.h>
|
||||
#include <float.h>
|
||||
|
||||
#include <arch/board/up_hrt.h>
|
||||
#include <arch/board/drv_bma180.h>
|
||||
#include <arch/board/drv_l3gd20.h>
|
||||
|
||||
#include <drivers/drv_accel.h>
|
||||
#include <drivers/drv_gyro.h>
|
||||
#include <drivers/drv_mag.h>
|
||||
#include <drivers/drv_baro.h>
|
||||
|
||||
#include <arch/board/up_adc.h>
|
||||
|
||||
#include <systemlib/systemlib.h>
|
||||
#include <systemlib/param/param.h>
|
||||
#include <systemlib/err.h>
|
||||
#include <systemlib/perf_counter.h>
|
||||
|
||||
#include <uORB/uORB.h>
|
||||
#include <uORB/topics/sensor_combined.h>
|
||||
#include <uORB/topics/rc_channels.h>
|
||||
#include <uORB/topics/manual_control_setpoint.h>
|
||||
#include <uORB/topics/vehicle_status.h>
|
||||
|
||||
#include "sensors.h"
|
||||
|
||||
#define SENSOR_INTERVAL_MICROSEC 2000
|
||||
|
||||
#define GYRO_HEALTH_COUNTER_LIMIT_ERROR 20 /* 40 ms downtime at 500 Hz update rate */
|
||||
#define ACC_HEALTH_COUNTER_LIMIT_ERROR 20 /* 40 ms downtime at 500 Hz update rate */
|
||||
#define MAGN_HEALTH_COUNTER_LIMIT_ERROR 100 /* 1000 ms downtime at 100 Hz update rate */
|
||||
#define BARO_HEALTH_COUNTER_LIMIT_ERROR 50 /* 500 ms downtime at 100 Hz update rate */
|
||||
#define ADC_HEALTH_COUNTER_LIMIT_ERROR 10 /* 100 ms downtime at 100 Hz update rate */
|
||||
|
||||
#define GYRO_HEALTH_COUNTER_LIMIT_OK 5
|
||||
#define ACC_HEALTH_COUNTER_LIMIT_OK 5
|
||||
#define MAGN_HEALTH_COUNTER_LIMIT_OK 5
|
||||
#define BARO_HEALTH_COUNTER_LIMIT_OK 5
|
||||
#define ADC_HEALTH_COUNTER_LIMIT_OK 5
|
||||
|
||||
#define ADC_BATTERY_VOLATGE_CHANNEL 10
|
||||
|
||||
#define BAT_VOL_INITIAL 12.f
|
||||
#define BAT_VOL_LOWPASS_1 0.99f
|
||||
#define BAT_VOL_LOWPASS_2 0.01f
|
||||
#define VOLTAGE_BATTERY_IGNORE_THRESHOLD_VOLTS 3.5f
|
||||
|
||||
#ifdef CONFIG_HRT_PPM
|
||||
extern "C" {
|
||||
extern uint16_t ppm_buffer[];
|
||||
extern unsigned ppm_decoded_channels;
|
||||
extern uint64_t ppm_last_valid_decode;
|
||||
}
|
||||
|
||||
/* PPM Settings */
|
||||
# define PPM_MIN 1000
|
||||
# define PPM_MAX 2000
|
||||
/* Internal resolution is 10000 */
|
||||
# define PPM_SCALE 10000/((PPM_MAX-PPM_MIN)/2)
|
||||
# define PPM_MID (PPM_MIN+PPM_MAX)/2
|
||||
#endif
|
||||
|
||||
/**
|
||||
* Sensor app start / stop handling function
|
||||
*
|
||||
* @ingroup apps
|
||||
*/
|
||||
extern "C" __EXPORT int sensors_main(int argc, char *argv[]);
|
||||
|
||||
class Sensors
|
||||
{
|
||||
public:
|
||||
Sensors();
|
||||
~Sensors();
|
||||
|
||||
int start();
|
||||
void stop();
|
||||
|
||||
private:
|
||||
static const unsigned _rc_max_chan_count = 8;
|
||||
|
||||
/* legacy sensor descriptors */
|
||||
int _fd_bma180;
|
||||
int _fd_gyro_l3gd20;
|
||||
|
||||
#if CONFIG_HRT_PPM
|
||||
hrt_abstime _ppm_last_valid;
|
||||
void ppm_poll();
|
||||
#endif
|
||||
|
||||
/* XXX should not be here - should be own driver */
|
||||
int _fd_adc;
|
||||
hrt_abstime _last_adc;
|
||||
|
||||
bool _task_should_exit;
|
||||
int _sensors_task;
|
||||
|
||||
bool _hil_enabled;
|
||||
bool _publishing;
|
||||
|
||||
int _gyro_sub;
|
||||
int _accel_sub;
|
||||
int _mag_sub;
|
||||
int _baro_sub;
|
||||
int _vstatus_sub;
|
||||
|
||||
orb_advert_t _sensor_pub;
|
||||
orb_advert_t _manual_control_pub;
|
||||
orb_advert_t _rc_pub;
|
||||
|
||||
perf_counter_t _loop_perf;
|
||||
|
||||
struct rc_channels_s _rc;
|
||||
|
||||
struct {
|
||||
int min[_rc_max_chan_count];
|
||||
int trim[_rc_max_chan_count];
|
||||
int max[_rc_max_chan_count];
|
||||
int rev[_rc_max_chan_count];
|
||||
|
||||
float gyro_offset[3];
|
||||
float mag_offset[3];
|
||||
float acc_offset[3];
|
||||
|
||||
int rc_type;
|
||||
|
||||
int rc_map_roll;
|
||||
int rc_map_pitch;
|
||||
int rc_map_yaw;
|
||||
int rc_map_throttle;
|
||||
int rc_map_mode_sw;
|
||||
|
||||
float battery_voltage_scaling;
|
||||
} _parameters;
|
||||
|
||||
struct {
|
||||
param_t min[_rc_max_chan_count];
|
||||
param_t trim[_rc_max_chan_count];
|
||||
param_t max[_rc_max_chan_count];
|
||||
param_t rev[_rc_max_chan_count];
|
||||
param_t rc_type;
|
||||
|
||||
param_t gyro_offset[3];
|
||||
param_t mag_offset[3];
|
||||
param_t acc_offset[3];
|
||||
|
||||
param_t rc_map_roll;
|
||||
param_t rc_map_pitch;
|
||||
param_t rc_map_yaw;
|
||||
param_t rc_map_throttle;
|
||||
param_t rc_map_mode_sw;
|
||||
|
||||
param_t battery_voltage_scaling;
|
||||
} _parameter_handles;
|
||||
|
||||
|
||||
int parameters_update();
|
||||
|
||||
void accel_init();
|
||||
void gyro_init();
|
||||
void mag_init();
|
||||
void baro_init();
|
||||
void adc_init();
|
||||
|
||||
void accel_poll(struct sensor_combined_s &raw);
|
||||
void gyro_poll(struct sensor_combined_s &raw);
|
||||
void mag_poll(struct sensor_combined_s &raw);
|
||||
void baro_poll(struct sensor_combined_s &raw);
|
||||
|
||||
void vehicle_status_poll();
|
||||
void adc_poll(struct sensor_combined_s &raw);
|
||||
|
||||
static void task_main_trampoline(int argc, char *argv[]);
|
||||
void task_main() __attribute__((noreturn));
|
||||
|
||||
};
|
||||
|
||||
namespace sensors
|
||||
{
|
||||
|
||||
/* oddly, ERROR is not defined for c++ */
|
||||
#ifdef ERROR
|
||||
# undef ERROR
|
||||
#endif
|
||||
static const int ERROR = -1;
|
||||
|
||||
Sensors *g_sensors;
|
||||
}
|
||||
|
||||
Sensors::Sensors() :
|
||||
_fd_bma180(-1),
|
||||
_fd_gyro_l3gd20(-1),
|
||||
_ppm_last_valid(0),
|
||||
|
||||
_fd_adc(-1),
|
||||
_last_adc(0),
|
||||
|
||||
_task_should_exit(false),
|
||||
_sensors_task(-1),
|
||||
_hil_enabled(false),
|
||||
_publishing(true),
|
||||
|
||||
/* subscriptions */
|
||||
_gyro_sub(-1),
|
||||
_accel_sub(-1),
|
||||
_mag_sub(-1),
|
||||
_baro_sub(-1),
|
||||
_vstatus_sub(-1),
|
||||
|
||||
/* publications */
|
||||
_sensor_pub(-1),
|
||||
_manual_control_pub(-1),
|
||||
_rc_pub(-1),
|
||||
|
||||
/* performance counters */
|
||||
_loop_perf(perf_alloc(PC_ELAPSED, "sensor task update"))
|
||||
{
|
||||
/* min values */
|
||||
_parameter_handles.min[0] = param_find("RC1_MIN");
|
||||
_parameter_handles.min[1] = param_find("RC2_MIN");
|
||||
_parameter_handles.min[2] = param_find("RC3_MIN");
|
||||
_parameter_handles.min[3] = param_find("RC4_MIN");
|
||||
_parameter_handles.min[4] = param_find("RC5_MIN");
|
||||
_parameter_handles.min[5] = param_find("RC6_MIN");
|
||||
_parameter_handles.min[6] = param_find("RC7_MIN");
|
||||
_parameter_handles.min[7] = param_find("RC8_MIN");
|
||||
|
||||
/* trim values */
|
||||
_parameter_handles.trim[0] = param_find("RC1_TRIM");
|
||||
_parameter_handles.trim[1] = param_find("RC2_TRIM");
|
||||
_parameter_handles.trim[2] = param_find("RC3_TRIM");
|
||||
_parameter_handles.trim[3] = param_find("RC4_TRIM");
|
||||
_parameter_handles.trim[4] = param_find("RC5_TRIM");
|
||||
_parameter_handles.trim[5] = param_find("RC6_TRIM");
|
||||
_parameter_handles.trim[6] = param_find("RC7_TRIM");
|
||||
_parameter_handles.trim[7] = param_find("RC8_TRIM");
|
||||
|
||||
/* max values */
|
||||
_parameter_handles.max[0] = param_find("RC1_MAX");
|
||||
_parameter_handles.max[1] = param_find("RC2_MAX");
|
||||
_parameter_handles.max[2] = param_find("RC3_MAX");
|
||||
_parameter_handles.max[3] = param_find("RC4_MAX");
|
||||
_parameter_handles.max[4] = param_find("RC5_MAX");
|
||||
_parameter_handles.max[5] = param_find("RC6_MAX");
|
||||
_parameter_handles.max[6] = param_find("RC7_MAX");
|
||||
_parameter_handles.max[7] = param_find("RC8_MAX");
|
||||
|
||||
/* channel reverse */
|
||||
_parameter_handles.rev[0] = param_find("RC1_REV");
|
||||
_parameter_handles.rev[1] = param_find("RC2_REV");
|
||||
_parameter_handles.rev[2] = param_find("RC3_REV");
|
||||
_parameter_handles.rev[3] = param_find("RC4_REV");
|
||||
_parameter_handles.rev[4] = param_find("RC5_REV");
|
||||
_parameter_handles.rev[5] = param_find("RC6_REV");
|
||||
_parameter_handles.rev[6] = param_find("RC7_REV");
|
||||
_parameter_handles.rev[7] = param_find("RC8_REV");
|
||||
|
||||
_parameter_handles.rc_type = param_find("RC_TYPE");
|
||||
|
||||
_parameter_handles.rc_map_roll = param_find("RC_MAP_ROLL");
|
||||
_parameter_handles.rc_map_pitch = param_find("RC_MAP_PITCH");
|
||||
_parameter_handles.rc_map_yaw = param_find("RC_MAP_YAW");
|
||||
_parameter_handles.rc_map_throttle = param_find("RC_MAP_THROTTLE");
|
||||
_parameter_handles.rc_map_mode_sw = param_find("RC_MAP_MODE_SW");
|
||||
|
||||
/* gyro offsets */
|
||||
_parameter_handles.gyro_offset[0] = param_find("SENSOR_GYRO_XOFF");
|
||||
_parameter_handles.gyro_offset[1] = param_find("SENSOR_GYRO_YOFF");
|
||||
_parameter_handles.gyro_offset[2] = param_find("SENSOR_GYRO_ZOFF");
|
||||
|
||||
/* accel offsets */
|
||||
_parameter_handles.acc_offset[0] = param_find("SENSOR_ACC_XOFF");
|
||||
_parameter_handles.acc_offset[1] = param_find("SENSOR_ACC_YOFF");
|
||||
_parameter_handles.acc_offset[2] = param_find("SENSOR_ACC_ZOFF");
|
||||
|
||||
/* mag offsets */
|
||||
_parameter_handles.mag_offset[0] = param_find("SENSOR_MAG_XOFF");
|
||||
_parameter_handles.mag_offset[1] = param_find("SENSOR_MAG_YOFF");
|
||||
_parameter_handles.mag_offset[2] = param_find("SENSOR_MAG_ZOFF");
|
||||
|
||||
_parameter_handles.battery_voltage_scaling = param_find("BAT_V_SCALING");
|
||||
|
||||
/* fetch initial parameter values */
|
||||
parameters_update();
|
||||
}
|
||||
|
||||
Sensors::~Sensors()
|
||||
{
|
||||
if (_sensors_task != -1) {
|
||||
|
||||
/* task wakes up every 100ms or so at the longest */
|
||||
_task_should_exit = true;
|
||||
|
||||
unsigned i = 0;
|
||||
do {
|
||||
/* wait 20ms */
|
||||
usleep(20000);
|
||||
|
||||
/* if we have given up, kill it */
|
||||
if (++i > 50) {
|
||||
task_delete(_sensors_task);
|
||||
break;
|
||||
}
|
||||
} while (_sensors_task != -1);
|
||||
}
|
||||
|
||||
sensors::g_sensors = nullptr;
|
||||
}
|
||||
|
||||
int
|
||||
Sensors::parameters_update()
|
||||
{
|
||||
const unsigned int nchans = 8;
|
||||
|
||||
/* min values */
|
||||
for (unsigned int i = 0; i < nchans; i++) {
|
||||
param_get(_parameter_handles.min[i], &(_parameters.min[i]));
|
||||
}
|
||||
|
||||
/* trim values */
|
||||
for (unsigned int i = 0; i < nchans; i++) {
|
||||
param_get(_parameter_handles.trim[i], &(_parameters.trim[i]));
|
||||
}
|
||||
|
||||
/* max values */
|
||||
for (unsigned int i = 0; i < nchans; i++) {
|
||||
param_get(_parameter_handles.max[i], &(_parameters.max[i]));
|
||||
}
|
||||
|
||||
/* channel reverse */
|
||||
for (unsigned int i = 0; i < nchans; i++) {
|
||||
param_get(_parameter_handles.rev[i], &(_parameters.rev[i]));
|
||||
}
|
||||
|
||||
/* remote control type */
|
||||
param_get(_parameter_handles.rc_type, &(_parameters.rc_type));
|
||||
|
||||
/* channel mapping */
|
||||
param_get(_parameter_handles.rc_map_roll, &(_parameters.rc_map_roll));
|
||||
param_get(_parameter_handles.rc_map_pitch, &(_parameters.rc_map_pitch));
|
||||
param_get(_parameter_handles.rc_map_yaw, &(_parameters.rc_map_yaw));
|
||||
param_get(_parameter_handles.rc_map_throttle, &(_parameters.rc_map_throttle));
|
||||
param_get(_parameter_handles.rc_map_mode_sw, &(_parameters.rc_map_mode_sw));
|
||||
|
||||
/* gyro offsets */
|
||||
param_get(_parameter_handles.gyro_offset[0], &(_parameters.gyro_offset[0]));
|
||||
param_get(_parameter_handles.gyro_offset[1], &(_parameters.gyro_offset[1]));
|
||||
param_get(_parameter_handles.gyro_offset[2], &(_parameters.gyro_offset[2]));
|
||||
|
||||
/* accel offsets */
|
||||
param_get(_parameter_handles.acc_offset[0], &(_parameters.acc_offset[0]));
|
||||
param_get(_parameter_handles.acc_offset[1], &(_parameters.acc_offset[1]));
|
||||
param_get(_parameter_handles.acc_offset[2], &(_parameters.acc_offset[2]));
|
||||
|
||||
/* mag offsets */
|
||||
param_get(_parameter_handles.mag_offset[0], &(_parameters.mag_offset[0]));
|
||||
param_get(_parameter_handles.mag_offset[1], &(_parameters.mag_offset[1]));
|
||||
param_get(_parameter_handles.mag_offset[2], &(_parameters.mag_offset[2]));
|
||||
|
||||
/* scaling of ADC ticks to battery voltage */
|
||||
param_get(_parameter_handles.battery_voltage_scaling, &(_parameters.battery_voltage_scaling));
|
||||
|
||||
return OK;
|
||||
}
|
||||
|
||||
void
|
||||
Sensors::accel_init()
|
||||
{
|
||||
int fd;
|
||||
|
||||
fd = open(ACCEL_DEVICE_PATH, 0);
|
||||
if (fd < 0) {
|
||||
warn("%s", ACCEL_DEVICE_PATH);
|
||||
|
||||
/* fall back to bma180 here (new driver would be better...) */
|
||||
_fd_bma180 = open("/dev/bma180", O_RDONLY);
|
||||
if (_fd_bma180 < 0) {
|
||||
warn("/dev/bma180");
|
||||
errx(1, "FATAL: no accelerometer found");
|
||||
}
|
||||
|
||||
/* discard first (junk) reading */
|
||||
int16_t junk_buf[3];
|
||||
read(_fd_bma180, junk_buf, sizeof(junk_buf));
|
||||
|
||||
warnx("using BMA180");
|
||||
} else {
|
||||
/* set the accel internal sampling rate up to at leat 500Hz */
|
||||
if (OK != ioctl(fd, ACCELIOCSSAMPLERATE, 500))
|
||||
warn("WARNING: failed to set minimum 500Hz sample rate for accel");
|
||||
|
||||
/* set the driver to poll at 500Hz */
|
||||
if (OK != ioctl(fd, SENSORIOCSPOLLRATE, 500))
|
||||
warn("WARNING: failed to set 500Hz poll rate for accel");
|
||||
|
||||
warnx("using system accel");
|
||||
close(fd);
|
||||
}
|
||||
}
|
||||
|
||||
void
|
||||
Sensors::gyro_init()
|
||||
{
|
||||
int fd;
|
||||
|
||||
fd = open(GYRO_DEVICE_PATH, 0);
|
||||
if (fd < 0) {
|
||||
warn("%s", GYRO_DEVICE_PATH);
|
||||
|
||||
/* fall back to bma180 here (new driver would be better...) */
|
||||
_fd_gyro_l3gd20 = open("/dev/l3gd20", O_RDONLY);
|
||||
if (_fd_gyro_l3gd20 < 0) {
|
||||
warn("/dev/l3gd20");
|
||||
errx(1, "FATAL: no gyro found");
|
||||
}
|
||||
|
||||
/* discard first (junk) reading */
|
||||
int16_t junk_buf[3];
|
||||
read(_fd_gyro_l3gd20, junk_buf, sizeof(junk_buf));
|
||||
|
||||
warn("using L3GD20");
|
||||
} else {
|
||||
/* set the gyro internal sampling rate up to at leat 500Hz */
|
||||
if (OK != ioctl(fd, GYROIOCSSAMPLERATE, 500))
|
||||
warn("WARNING: failed to set minimum 500Hz sample rate for gyro");
|
||||
|
||||
/* set the driver to poll at 500Hz */
|
||||
if (OK != ioctl(fd, SENSORIOCSPOLLRATE, 500))
|
||||
warn("WARNING: failed to set 500Hz poll rate for gyro");
|
||||
|
||||
warnx("using system gyro");
|
||||
close(fd);
|
||||
}
|
||||
}
|
||||
|
||||
void
|
||||
Sensors::mag_init()
|
||||
{
|
||||
int fd;
|
||||
|
||||
fd = open(MAG_DEVICE_PATH, 0);
|
||||
if (fd < 0) {
|
||||
warn("%s", MAG_DEVICE_PATH);
|
||||
errx(1, "FATAL: no magnetometer found");
|
||||
}
|
||||
|
||||
/* set the mag internal poll rate to at least 150Hz */
|
||||
if (OK != ioctl(fd, MAGIOCSSAMPLERATE, 150))
|
||||
warn("WARNING: failed to set minimum 150Hz sample rate for mag");
|
||||
|
||||
/* set the driver to poll at 150Hz */
|
||||
if (OK != ioctl(fd, SENSORIOCSPOLLRATE, 150))
|
||||
warn("WARNING: failed to set 150Hz poll rate for mag");
|
||||
|
||||
close(fd);
|
||||
}
|
||||
|
||||
void
|
||||
Sensors::baro_init()
|
||||
{
|
||||
int fd;
|
||||
|
||||
fd = open(BARO_DEVICE_PATH, 0);
|
||||
if (fd < 0) {
|
||||
warn("%s", BARO_DEVICE_PATH);
|
||||
errx(1, "FATAL: no barometer found");
|
||||
}
|
||||
|
||||
/* set the driver to poll at 150Hz */
|
||||
if (OK != ioctl(fd, SENSORIOCSPOLLRATE, 150))
|
||||
warn("WARNING: failed to set 150Hz poll rate for baro");
|
||||
|
||||
close(fd);
|
||||
}
|
||||
|
||||
void
|
||||
Sensors::adc_init()
|
||||
{
|
||||
|
||||
_fd_adc = open("/dev/adc0", O_RDONLY | O_NONBLOCK);
|
||||
if (_fd_adc < 0) {
|
||||
warn("/dev/adc0");
|
||||
errx(1, "FATAL: no ADC found");
|
||||
}
|
||||
}
|
||||
|
||||
void
|
||||
Sensors::accel_poll(struct sensor_combined_s &raw)
|
||||
{
|
||||
struct accel_report accel_report;
|
||||
|
||||
if (_fd_bma180 >= 0) {
|
||||
/* do ORB emulation for BMA180 */
|
||||
int16_t buf[3];
|
||||
|
||||
read(_fd_bma180, buf, sizeof(buf));
|
||||
|
||||
accel_report.timestamp = hrt_absolute_time();
|
||||
accel_report.x_raw = buf[0];
|
||||
accel_report.y_raw = buf[1];
|
||||
accel_report.z_raw = buf[2];
|
||||
|
||||
/* XXX scale raw values to readings */
|
||||
accel_report.x = 0;
|
||||
accel_report.y = 0;
|
||||
accel_report.z = 0;
|
||||
|
||||
} else {
|
||||
orb_copy(ORB_ID(sensor_accel), _accel_sub, &accel_report);
|
||||
}
|
||||
|
||||
raw.accelerometer_m_s2[0] = accel_report.x;
|
||||
raw.accelerometer_m_s2[1] = accel_report.y;
|
||||
raw.accelerometer_m_s2[2] = accel_report.z;
|
||||
|
||||
raw.accelerometer_raw[0] = accel_report.x_raw;
|
||||
raw.accelerometer_raw[1] = accel_report.y_raw;
|
||||
raw.accelerometer_raw[2] = accel_report.z_raw;
|
||||
|
||||
raw.accelerometer_raw_counter++;
|
||||
}
|
||||
|
||||
void
|
||||
Sensors::gyro_poll(struct sensor_combined_s &raw)
|
||||
{
|
||||
struct gyro_report gyro_report;
|
||||
|
||||
if (_fd_gyro_l3gd20 >= 0) {
|
||||
/* do ORB emulation for L3GD20 */
|
||||
int16_t buf[3];
|
||||
|
||||
read(_fd_gyro_l3gd20, buf, sizeof(buf));
|
||||
|
||||
gyro_report.timestamp = hrt_absolute_time();
|
||||
gyro_report.x_raw = buf[0];
|
||||
gyro_report.y_raw = buf[1];
|
||||
gyro_report.z_raw = buf[2];
|
||||
|
||||
/* XXX scale raw values to readings */
|
||||
gyro_report.x = 0;
|
||||
gyro_report.y = 0;
|
||||
gyro_report.z = 0;
|
||||
} else {
|
||||
orb_copy(ORB_ID(sensor_gyro), _gyro_sub, &gyro_report);
|
||||
|
||||
raw.gyro_rad_s[0] = gyro_report.x;
|
||||
raw.gyro_rad_s[1] = gyro_report.y;
|
||||
raw.gyro_rad_s[2] = gyro_report.z;
|
||||
|
||||
raw.gyro_raw[0] = gyro_report.x_raw;
|
||||
raw.gyro_raw[1] = gyro_report.y_raw;
|
||||
raw.gyro_raw[2] = gyro_report.z_raw;
|
||||
|
||||
raw.gyro_raw_counter++;
|
||||
}
|
||||
}
|
||||
|
||||
void
|
||||
Sensors::mag_poll(struct sensor_combined_s &raw)
|
||||
{
|
||||
struct mag_report mag_report;
|
||||
|
||||
orb_copy(ORB_ID(sensor_mag), _mag_sub, &mag_report);
|
||||
|
||||
raw.magnetometer_ga[0] = mag_report.x;
|
||||
raw.magnetometer_ga[1] = mag_report.y;
|
||||
raw.magnetometer_ga[2] = mag_report.z;
|
||||
|
||||
raw.magnetometer_raw[0] = mag_report.x_raw;
|
||||
raw.magnetometer_raw[1] = mag_report.y_raw;
|
||||
raw.magnetometer_raw[2] = mag_report.z_raw;
|
||||
|
||||
raw.magnetometer_raw_counter++;
|
||||
}
|
||||
|
||||
void
|
||||
Sensors::baro_poll(struct sensor_combined_s &raw)
|
||||
{
|
||||
struct baro_report baro_report;
|
||||
|
||||
orb_copy(ORB_ID(sensor_baro), _baro_sub, &baro_report);
|
||||
|
||||
raw.baro_pres_mbar = baro_report.pressure; // Pressure in mbar
|
||||
raw.baro_alt_meter = baro_report.altitude; // Altitude in meters
|
||||
raw.baro_temp_celcius = baro_report.temperature; // Temperature in degrees celcius
|
||||
|
||||
raw.baro_raw_counter++;
|
||||
}
|
||||
|
||||
void
|
||||
Sensors::vehicle_status_poll()
|
||||
{
|
||||
struct vehicle_status_s vstatus;
|
||||
bool vstatus_updated;
|
||||
|
||||
/* Check HIL state if vehicle status has changed */
|
||||
orb_check(_vstatus_sub, &vstatus_updated);
|
||||
if (vstatus_updated) {
|
||||
|
||||
orb_copy(ORB_ID(vehicle_status), _vstatus_sub, &vstatus);
|
||||
|
||||
/* switching from non-HIL to HIL mode */
|
||||
//printf("[sensors] Vehicle mode: %i \t AND: %i, HIL: %i\n", vstatus.mode, vstatus.mode & VEHICLE_MODE_FLAG_HIL_ENABLED, hil_enabled);
|
||||
if (vstatus.flag_hil_enabled && !_hil_enabled) {
|
||||
_hil_enabled = true;
|
||||
_publishing = false;
|
||||
|
||||
/* switching from HIL to non-HIL mode */
|
||||
|
||||
} else if (!_publishing && !_hil_enabled) {
|
||||
_hil_enabled = false;
|
||||
_publishing = true;
|
||||
}
|
||||
|
||||
/* update parameters */
|
||||
parameters_update();
|
||||
|
||||
/* Update RC scalings and function mappings */
|
||||
_rc.chan[0].scaling_factor = (1.0f / ((_parameters.max[0] - _parameters.min[0]) / 2.0f) * _parameters.rev[0]);
|
||||
_rc.chan[0].mid = _parameters.trim[0];
|
||||
|
||||
_rc.chan[1].scaling_factor = (1.0f / ((_parameters.max[1] - _parameters.min[1]) / 2.0f) * _parameters.rev[1]);
|
||||
_rc.chan[1].mid = _parameters.trim[1];
|
||||
|
||||
_rc.chan[2].scaling_factor = (1.0f / ((_parameters.max[2] - _parameters.min[2]) / 2.0f) * _parameters.rev[2]);
|
||||
_rc.chan[2].mid = _parameters.trim[2];
|
||||
|
||||
_rc.chan[3].scaling_factor = (1.0f / ((_parameters.max[3] - _parameters.min[3]) / 2.0f) * _parameters.rev[3]);
|
||||
_rc.chan[3].mid = _parameters.trim[3];
|
||||
|
||||
_rc.chan[4].scaling_factor = (1.0f / ((_parameters.max[4] - _parameters.min[4]) / 2.0f) * _parameters.rev[4]);
|
||||
_rc.chan[4].mid = _parameters.trim[4];
|
||||
|
||||
_rc.chan[5].scaling_factor = (1.0f / ((_parameters.max[5] - _parameters.min[5]) / 2.0f) * _parameters.rev[5]);
|
||||
_rc.chan[5].mid = _parameters.trim[5];
|
||||
|
||||
_rc.chan[6].scaling_factor = (1.0f / ((_parameters.max[6] - _parameters.min[6]) / 2.0f) * _parameters.rev[6]);
|
||||
_rc.chan[6].mid = _parameters.trim[6];
|
||||
|
||||
_rc.chan[7].scaling_factor = (1.0f / ((_parameters.max[7] - _parameters.min[7]) / 2.0f) * _parameters.rev[7]);
|
||||
_rc.chan[7].mid = _parameters.trim[7];
|
||||
|
||||
_rc.function[0] = _parameters.rc_map_throttle - 1;
|
||||
_rc.function[1] = _parameters.rc_map_roll - 1;
|
||||
_rc.function[2] = _parameters.rc_map_pitch - 1;
|
||||
_rc.function[3] = _parameters.rc_map_yaw - 1;
|
||||
_rc.function[4] = _parameters.rc_map_mode_sw - 1;
|
||||
}
|
||||
}
|
||||
|
||||
void
|
||||
Sensors::adc_poll(struct sensor_combined_s &raw)
|
||||
{
|
||||
#pragma pack(push,1)
|
||||
struct adc_msg4_s {
|
||||
uint8_t am_channel1; /**< The 8-bit ADC Channel 1 */
|
||||
int32_t am_data1; /**< ADC convert result 1 (4 bytes) */
|
||||
uint8_t am_channel2; /**< The 8-bit ADC Channel 2 */
|
||||
int32_t am_data2; /**< ADC convert result 2 (4 bytes) */
|
||||
uint8_t am_channel3; /**< The 8-bit ADC Channel 3 */
|
||||
int32_t am_data3; /**< ADC convert result 3 (4 bytes) */
|
||||
uint8_t am_channel4; /**< The 8-bit ADC Channel 4 */
|
||||
int32_t am_data4; /**< ADC convert result 4 (4 bytes) */
|
||||
} buf_adc;
|
||||
#pragma pack(pop)
|
||||
|
||||
if (hrt_absolute_time() - _last_adc >= 10000) {
|
||||
read(_fd_adc, &buf_adc, sizeof(buf_adc));
|
||||
|
||||
if (ADC_BATTERY_VOLATGE_CHANNEL == buf_adc.am_channel1) {
|
||||
/* Voltage in volts */
|
||||
raw.battery_voltage_v = (BAT_VOL_LOWPASS_1 * (raw.battery_voltage_v + BAT_VOL_LOWPASS_2 * (buf_adc.am_data1 * _parameters.battery_voltage_scaling)));
|
||||
|
||||
if ((buf_adc.am_data1 * _parameters.battery_voltage_scaling) < VOLTAGE_BATTERY_IGNORE_THRESHOLD_VOLTS) {
|
||||
raw.battery_voltage_valid = false;
|
||||
raw.battery_voltage_v = 0.f;
|
||||
|
||||
} else {
|
||||
raw.battery_voltage_valid = true;
|
||||
}
|
||||
|
||||
raw.battery_voltage_counter++;
|
||||
}
|
||||
_last_adc = hrt_absolute_time();
|
||||
}
|
||||
}
|
||||
|
||||
#if CONFIG_HRT_PPM
|
||||
void
|
||||
Sensors::ppm_poll()
|
||||
{
|
||||
struct manual_control_setpoint_s manual_control;
|
||||
|
||||
/* check to see whether a new frame has been decoded */
|
||||
if (_ppm_last_valid == ppm_last_valid_decode)
|
||||
return;
|
||||
/* require at least two chanels to consider the signal valid */
|
||||
if (ppm_decoded_channels < 2)
|
||||
return;
|
||||
|
||||
/* we are accepting this decode */
|
||||
_ppm_last_valid = ppm_last_valid_decode;
|
||||
|
||||
/* Read out values from HRT */
|
||||
for (unsigned int i = 0; i < ppm_decoded_channels; i++) {
|
||||
_rc.chan[i].raw = ppm_buffer[i];
|
||||
/* Set the range to +-, then scale up */
|
||||
_rc.chan[i].scale = (ppm_buffer[i] - _rc.chan[i].mid) * _rc.chan[i].scaling_factor * 10000;
|
||||
_rc.chan[i].scaled = (ppm_buffer[i] - _rc.chan[i].mid) * _rc.chan[i].scaling_factor;
|
||||
}
|
||||
|
||||
_rc.chan_count = ppm_decoded_channels;
|
||||
_rc.timestamp = ppm_last_valid_decode;
|
||||
|
||||
/* roll input */
|
||||
manual_control.roll = _rc.chan[_rc.function[ROLL]].scaled;
|
||||
if (manual_control.roll < -1.0f) manual_control.roll = -1.0f;
|
||||
if (manual_control.roll > 1.0f) manual_control.roll = 1.0f;
|
||||
|
||||
/* pitch input */
|
||||
manual_control.pitch = _rc.chan[_rc.function[PITCH]].scaled;
|
||||
if (manual_control.pitch < -1.0f) manual_control.pitch = -1.0f;
|
||||
if (manual_control.pitch > 1.0f) manual_control.pitch = 1.0f;
|
||||
|
||||
/* yaw input */
|
||||
manual_control.yaw = _rc.chan[_rc.function[YAW]].scaled;
|
||||
if (manual_control.yaw < -1.0f) manual_control.yaw = -1.0f;
|
||||
if (manual_control.yaw > 1.0f) manual_control.yaw = 1.0f;
|
||||
|
||||
/* throttle input */
|
||||
manual_control.throttle = (_rc.chan[_rc.function[THROTTLE]].scaled+1.0f)/2.0f;
|
||||
if (manual_control.throttle < 0.0f) manual_control.throttle = 0.0f;
|
||||
if (manual_control.throttle > 1.0f) manual_control.throttle = 1.0f;
|
||||
|
||||
/* mode switch input */
|
||||
manual_control.override_mode_switch = _rc.chan[_rc.function[OVERRIDE]].scaled;
|
||||
if (manual_control.override_mode_switch < -1.0f) manual_control.override_mode_switch = -1.0f;
|
||||
if (manual_control.override_mode_switch > 1.0f) manual_control.override_mode_switch = 1.0f;
|
||||
|
||||
orb_publish(ORB_ID(rc_channels), _rc_pub, &_rc);
|
||||
orb_publish(ORB_ID(manual_control_setpoint), _manual_control_pub, &manual_control);
|
||||
}
|
||||
#endif
|
||||
|
||||
void
|
||||
Sensors::task_main_trampoline(int argc, char *argv[])
|
||||
{
|
||||
sensors::g_sensors->task_main();
|
||||
}
|
||||
|
||||
void
|
||||
Sensors::task_main()
|
||||
{
|
||||
|
||||
/* inform about start */
|
||||
printf("[sensors] Initializing..\n");
|
||||
fflush(stdout);
|
||||
|
||||
/* start individual sensors */
|
||||
accel_init();
|
||||
gyro_init();
|
||||
mag_init();
|
||||
baro_init();
|
||||
adc_init();
|
||||
|
||||
/*
|
||||
* do subscriptions
|
||||
*/
|
||||
_gyro_sub = orb_subscribe(ORB_ID(sensor_gyro));
|
||||
_accel_sub = orb_subscribe(ORB_ID(sensor_accel));
|
||||
_mag_sub = orb_subscribe(ORB_ID(sensor_mag));
|
||||
_baro_sub = orb_subscribe(ORB_ID(sensor_baro));
|
||||
_vstatus_sub = orb_subscribe(ORB_ID(vehicle_status));
|
||||
|
||||
/* rate limit vehicle status updates to 5Hz */
|
||||
orb_set_interval(_vstatus_sub, 200);
|
||||
|
||||
/*
|
||||
* do advertisements
|
||||
*/
|
||||
struct sensor_combined_s raw;
|
||||
raw.timestamp = hrt_absolute_time();
|
||||
raw.battery_voltage_v = BAT_VOL_INITIAL;
|
||||
raw.adc_voltage_v[0] = 0.9f;
|
||||
raw.adc_voltage_v[1] = 0.0f;
|
||||
raw.adc_voltage_v[2] = 0.0f;
|
||||
raw.battery_voltage_counter = 0;
|
||||
raw.battery_voltage_valid = false;
|
||||
|
||||
/* get a set of initial values */
|
||||
accel_poll(raw);
|
||||
gyro_poll(raw);
|
||||
mag_poll(raw);
|
||||
baro_poll(raw);
|
||||
|
||||
/* advertise the sensor_combined topic and make the initial publication */
|
||||
_sensor_pub = orb_advertise(ORB_ID(sensor_combined), &raw);
|
||||
|
||||
/* advertise the manual_control topic */
|
||||
{
|
||||
struct manual_control_setpoint_s manual_control;
|
||||
manual_control.mode = ROLLPOS_PITCHPOS_YAWRATE_THROTTLE;
|
||||
manual_control.roll = 0.0f;
|
||||
manual_control.pitch = 0.0f;
|
||||
manual_control.yaw = 0.0f;
|
||||
manual_control.throttle = 0.0f;
|
||||
|
||||
_manual_control_pub = orb_advertise(ORB_ID(manual_control_setpoint), &manual_control);
|
||||
}
|
||||
|
||||
/* advertise the rc topic */
|
||||
{
|
||||
struct rc_channels_s rc;
|
||||
memset(&rc, 0, sizeof(rc));
|
||||
_rc_pub = orb_advertise(ORB_ID(rc_channels), &rc);
|
||||
}
|
||||
|
||||
/* wakeup source(s) */
|
||||
struct pollfd fds[1];
|
||||
|
||||
/* use the gyro to pace output - XXX BROKEN if we are using the L3GD20 */
|
||||
fds[0].fd = _gyro_sub;
|
||||
fds[0].events = POLLIN;
|
||||
|
||||
while (!_task_should_exit) {
|
||||
|
||||
/* wait for up to 500ms for data */
|
||||
int pret = poll(&fds[0], (sizeof(fds) / sizeof(fds[0])), 100);
|
||||
|
||||
/* timed out - periodic check for _task_should_exit, etc. */
|
||||
if (pret == 0)
|
||||
continue;
|
||||
|
||||
/* this is undesirable but not much we can do - might want to flag unhappy status */
|
||||
if (pret < 0) {
|
||||
warn("poll error %d, %d", pret, errno);
|
||||
continue;
|
||||
}
|
||||
|
||||
perf_begin(_loop_perf);
|
||||
|
||||
/* check vehicle status for changes to publication state */
|
||||
vehicle_status_poll();
|
||||
|
||||
/* store the time closest to all measurements (this is bogus, sensor timestamps should be propagated...) */
|
||||
raw.timestamp = hrt_absolute_time();
|
||||
|
||||
/* copy most recent sensor data */
|
||||
accel_poll(raw);
|
||||
gyro_poll(raw);
|
||||
mag_poll(raw);
|
||||
baro_poll(raw);
|
||||
|
||||
/* check battery voltage */
|
||||
adc_poll(raw);
|
||||
|
||||
/* Inform other processes that new data is available to copy */
|
||||
if (_publishing)
|
||||
orb_publish(ORB_ID(sensor_combined), _sensor_pub, &raw);
|
||||
|
||||
#ifdef CONFIG_HRT_PPM
|
||||
/* Look for new r/c input data */
|
||||
ppm_poll();
|
||||
#endif
|
||||
|
||||
perf_end(_loop_perf);
|
||||
}
|
||||
|
||||
printf("[sensors] exiting.\n");
|
||||
|
||||
_sensors_task = -1;
|
||||
_exit(0);
|
||||
}
|
||||
|
||||
int
|
||||
Sensors::start()
|
||||
{
|
||||
ASSERT(_sensors_task == -1);
|
||||
|
||||
/* start the task */
|
||||
_sensors_task = task_create("sensor_task",
|
||||
SCHED_PRIORITY_MAX - 5,
|
||||
4096,
|
||||
(main_t)&Sensors::task_main_trampoline,
|
||||
nullptr);
|
||||
|
||||
if (_sensors_task < 0) {
|
||||
warn("task start failed");
|
||||
return -errno;
|
||||
}
|
||||
return OK;
|
||||
}
|
||||
|
||||
int sensors_main(int argc, char *argv[])
|
||||
{
|
||||
if (argc < 1)
|
||||
errx(1, "usage: sensors {start|stop|status}");
|
||||
|
||||
if (!strcmp(argv[1], "start")) {
|
||||
|
||||
if (sensors::g_sensors != nullptr)
|
||||
errx(1, "sensors task already running");
|
||||
|
||||
sensors::g_sensors = new Sensors;
|
||||
if (sensors::g_sensors == nullptr)
|
||||
errx(1, "sensors task alloc failed");
|
||||
|
||||
if (OK != sensors::g_sensors->start())
|
||||
err(1, "sensors task start failed");
|
||||
exit(0);
|
||||
}
|
||||
|
||||
if (!strcmp(argv[1], "stop")) {
|
||||
if (sensors::g_sensors == nullptr)
|
||||
errx(1, "sensors task not running");
|
||||
delete sensors::g_sensors;
|
||||
sensors::g_sensors = nullptr;
|
||||
exit(0);
|
||||
}
|
||||
|
||||
if (!strcmp(argv[1], "status")) {
|
||||
if (sensors::g_sensors) {
|
||||
errx(0, "task is running");
|
||||
} else {
|
||||
errx(1, "task is not running");
|
||||
}
|
||||
}
|
||||
|
||||
errx(1, "unrecognized command");
|
||||
}
|
||||
|
||||
|
|
@ -155,7 +155,7 @@ warn(const char *fmt, ...)
|
|||
void
|
||||
vwarn(const char *fmt, va_list args)
|
||||
{
|
||||
warnerr_core(NOCODE, fmt, args);
|
||||
warnerr_core(errno, fmt, args);
|
||||
}
|
||||
|
||||
void
|
||||
|
|
|
|||
|
|
@ -47,10 +47,13 @@
|
|||
|
||||
#include <stdint.h>
|
||||
#include <stdbool.h>
|
||||
#include <sys/types.h>
|
||||
|
||||
/** Maximum size of the parameter backing file */
|
||||
#define PARAM_FILE_MAXSIZE 4096
|
||||
|
||||
__BEGIN_DECLS
|
||||
|
||||
/**
|
||||
* Parameter types.
|
||||
*/
|
||||
|
|
@ -192,6 +195,10 @@ __EXPORT void param_foreach(void (*func)(void *arg, param_t param), void *arg,
|
|||
* Note that these structures are not known by name; they are
|
||||
* collected into a section that is iterated by the parameter
|
||||
* code.
|
||||
*
|
||||
* Note that these macros cannot be used in C++ code due to
|
||||
* their use of designated initializers. They should probably
|
||||
* be refactored to avoid the use of a union for param_value_u.
|
||||
*/
|
||||
|
||||
/** define an int32 parameter */
|
||||
|
|
@ -199,9 +206,9 @@ __EXPORT void param_foreach(void (*func)(void *arg, param_t param), void *arg,
|
|||
static const \
|
||||
__attribute__((used, section("__param"))) \
|
||||
struct param_info_s __param__##_name = { \
|
||||
.name = #_name, \
|
||||
.type = PARAM_TYPE_INT32, \
|
||||
.val.i = _default \
|
||||
#_name, \
|
||||
PARAM_TYPE_INT32, \
|
||||
.val.i = _default \
|
||||
}
|
||||
|
||||
/** define a float parameter */
|
||||
|
|
@ -209,9 +216,9 @@ __EXPORT void param_foreach(void (*func)(void *arg, param_t param), void *arg,
|
|||
static const \
|
||||
__attribute__((used, section("__param"))) \
|
||||
struct param_info_s __param__##_name = { \
|
||||
.name = #_name, \
|
||||
.type = PARAM_TYPE_FLOAT, \
|
||||
.val.f = _default \
|
||||
#_name, \
|
||||
PARAM_TYPE_FLOAT, \
|
||||
.val.f = _default \
|
||||
}
|
||||
|
||||
/** define a parameter that points to a structure */
|
||||
|
|
@ -219,9 +226,9 @@ __EXPORT void param_foreach(void (*func)(void *arg, param_t param), void *arg,
|
|||
static const \
|
||||
__attribute__((used, section("__param"))) \
|
||||
struct param_info_s __param__##_name = { \
|
||||
.name = #_name, \
|
||||
.type = PARAM_TYPE_STRUCT + sizeof(_default), \
|
||||
.val.p = &_default; \
|
||||
#_name, \
|
||||
PARAM_TYPE_STRUCT + sizeof(_default), \
|
||||
.val.p = &_default; \
|
||||
}
|
||||
|
||||
/**
|
||||
|
|
@ -245,4 +252,6 @@ struct param_info_s {
|
|||
union param_value_u val;
|
||||
};
|
||||
|
||||
__END_DECLS
|
||||
|
||||
#endif
|
||||
|
|
|
|||
|
|
@ -96,7 +96,7 @@ struct rc_channels_s {
|
|||
uint8_t chan_count; /**< maximum number of valid channels */
|
||||
|
||||
/*String array to store the names of the functions*/
|
||||
const char function_name[RC_CHANNELS_FUNCTION_MAX][20];
|
||||
char function_name[RC_CHANNELS_FUNCTION_MAX][20];
|
||||
uint8_t function[RC_CHANNELS_FUNCTION_MAX];
|
||||
uint8_t rssi; /**< Overall receive signal strength */
|
||||
}; /**< radio control channels. */
|
||||
|
|
|
|||
|
|
@ -543,7 +543,7 @@ CONFIG_DEV_CONSOLE=y
|
|||
CONFIG_DEV_LOWCONSOLE=n
|
||||
CONFIG_MUTEX_TYPES=n
|
||||
CONFIG_PRIORITY_INHERITANCE=y
|
||||
CONFIG_SEM_PREALLOCHOLDERS=0
|
||||
CONFIG_SEM_PREALLOCHOLDERS=8
|
||||
CONFIG_SEM_NNESTPRIO=8
|
||||
CONFIG_FDCLONE_DISABLE=n
|
||||
CONFIG_FDCLONE_STDIO=y
|
||||
|
|
|
|||
Loading…
Reference in New Issue