dark0707
/
px4-firmware
forked from Archive/PX4-Autopilot
1
0
Fork 0
Code Pull Requests Activity

Merge branch 'local/c++_sensors' into px4dev_new_driver

This commit is contained in:
px4dev 2012-08-25 19:32:43 -07:00
parent 505ebc57bd 3df82e51b8
commit bdfcff9bc9
13 changed files with 1167 additions and 105 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

1
Debug/NuttX
View File

@ -95,6 +95,7 @@ end
document showfiles document showfiles
. showfiles <TCB pointer> . showfiles <TCB pointer>
. Prints the files opened by a task. . Prints the files opened by a task.
end
################################################################################ ################################################################################
# Task display # Task display

2
apps/drivers/device/i2c.cpp
View File

@ -99,7 +99,7 @@ I2C::init()
} }
// tell the world where we are // 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: out:
return ret; return ret;

2
apps/drivers/device/spi.cpp
View File

@ -111,7 +111,7 @@ SPI::init()
} }
// tell the workd where we are // tell the workd where we are
log("on bus %d at %d", _bus, _device); log("on SPI bus %d at %d", _bus, _device);
out: out:
return ret; return ret;

66
apps/drivers/hmc5883/hmc5883.cpp
View File

@ -147,12 +147,9 @@ private:
orb_advert_t _mag_topic; orb_advert_t _mag_topic;
unsigned _reads;
unsigned _measure_errors;
unsigned _read_errors;
unsigned _buf_overflows;
perf_counter_t _sample_perf; 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 * Test whether the device supported by the driver is present at a
@ -256,11 +253,9 @@ HMC5883::HMC5883(int bus) :
_oldest_report(0), _oldest_report(0),
_reports(nullptr), _reports(nullptr),
_mag_topic(-1), _mag_topic(-1),
_reads(0), _sample_perf(perf_alloc(PC_ELAPSED, "hmc5883_read")),
_measure_errors(0), _comms_errors(perf_alloc(PC_COUNT, "hmc5883_comms_errors")),
_read_errors(0), _buffer_overflows(perf_alloc(PC_COUNT, "hmc5883_buffer_overflows"))
_buf_overflows(0),
_sample_perf(perf_alloc(PC_ELAPSED, "hmc5883_read"))
{ {
// enable debug() calls // enable debug() calls
_debug_enabled = true; _debug_enabled = true;
@ -303,6 +298,12 @@ HMC5883::init()
goto out; goto out;
_oldest_report = _next_report = 0; _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; ret = OK;
out: out:
return ret; return ret;
@ -313,7 +314,7 @@ HMC5883::probe()
{ {
uint8_t data[3] = {0, 0, 0}; uint8_t data[3] = {0, 0, 0};
_retries = 3; _retries = 10;
if (read_reg(ADDR_ID_A, data[0]) || if (read_reg(ADDR_ID_A, data[0]) ||
read_reg(ADDR_ID_B, data[1]) || read_reg(ADDR_ID_B, data[1]) ||
read_reg(ADDR_ID_C, data[2])) 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 */ /* if there was no data, warn the caller */
return ret ? ret : -EAGAIN; 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 */ /* state machine will have generated a report, copy it out */
memcpy(buffer, _reports, sizeof(*_reports)); memcpy(buffer, _reports, sizeof(*_reports));
ret = sizeof(*_reports); ret = sizeof(*_reports);
_reads++;
} while (0); } while (0);
@ -548,31 +546,13 @@ HMC5883::cycle_trampoline(void *arg)
void void
HMC5883::cycle() 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? */ /* collection phase? */
if (_collect_phase) { if (_collect_phase) {
/* perform collection */ /* perform collection */
if (OK != collect()) { if (OK != collect()) {
log("FATAL collection error - restarting\n"); log("collection error");
/* restart the measurement state machine */
start(); start();
return; return;
} }
@ -596,10 +576,8 @@ HMC5883::cycle()
} }
/* measurement phase */ /* measurement phase */
if (OK != measure()) { if (OK != measure())
log("FATAL measure error - restarting\n"); log("measure error");
start();
}
/* next phase is collection */ /* next phase is collection */
_collect_phase = true; _collect_phase = true;
@ -622,7 +600,7 @@ HMC5883::measure()
ret = write_reg(ADDR_MODE, MODE_REG_SINGLE_MODE); ret = write_reg(ADDR_MODE, MODE_REG_SINGLE_MODE);
if (OK != ret) if (OK != ret)
_measure_errors++; perf_count(_comms_errors);
return ret; return ret;
} }
@ -661,6 +639,7 @@ HMC5883::collect()
ret = transfer(&cmd, 1, (uint8_t *)&hmc_report, sizeof(hmc_report)); ret = transfer(&cmd, 1, (uint8_t *)&hmc_report, sizeof(hmc_report));
if (ret != OK) { if (ret != OK) {
perf_count(_comms_errors);
debug("data/status read error"); debug("data/status read error");
goto out; goto out;
} }
@ -692,7 +671,7 @@ HMC5883::collect()
/* if we are running up against the oldest report, toss it */ /* if we are running up against the oldest report, toss it */
if (_next_report == _oldest_report) { if (_next_report == _oldest_report) {
_buf_overflows++; perf_count(_buffer_overflows);
INCREMENT(_oldest_report, _num_reports); INCREMENT(_oldest_report, _num_reports);
} }
@ -737,10 +716,9 @@ HMC5883::meas_to_float(uint8_t in[2])
void void
HMC5883::print_info() HMC5883::print_info()
{ {
printf("reads: %u\n", _reads); perf_print_counter(_sample_perf);
printf("measure errors: %u\n", _measure_errors); perf_print_counter(_comms_errors);
printf("read errors: %u\n", _read_errors); perf_print_counter(_buffer_overflows);
printf("read overflows: %u\n", _buf_overflows);
printf("poll interval: %u ticks\n", _measure_ticks); printf("poll interval: %u ticks\n", _measure_ticks);
printf("report queue: %u (%u/%u @ %p)\n", printf("report queue: %u (%u/%u @ %p)\n",
_num_reports, _oldest_report, _next_report, _reports); _num_reports, _oldest_report, _next_report, _reports);

6
apps/drivers/mpu6000/mpu6000.cpp
View File

@ -298,8 +298,8 @@ MPU6000::MPU6000(int bus, spi_dev_e device) :
_reads(0), _reads(0),
_sample_perf(perf_alloc(PC_ELAPSED, "mpu6000_read")) _sample_perf(perf_alloc(PC_ELAPSED, "mpu6000_read"))
{ {
// enable debug() calls // disable debug() calls
_debug_enabled = true; _debug_enabled = false;
// default accel scale factors // default accel scale factors
_accel_scale.x_offset = 0; _accel_scale.x_offset = 0;
@ -463,7 +463,7 @@ MPU6000::probe()
case MPU6000_REV_D8: case MPU6000_REV_D8:
case MPU6000_REV_D9: case MPU6000_REV_D9:
case MPU6000_REV_D10: case MPU6000_REV_D10:
log("ID 0x%02x", _product); debug("ID 0x%02x", _product);
return OK; return OK;
} }

66
apps/drivers/ms5611/ms5611.cpp
View File

@ -132,12 +132,9 @@ private:
orb_advert_t _baro_topic; orb_advert_t _baro_topic;
unsigned _reads;
unsigned _measure_errors;
unsigned _read_errors;
unsigned _buf_overflows;
perf_counter_t _sample_perf; 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 * Test whether the device supported by the driver is present at a
@ -252,11 +249,9 @@ MS5611::MS5611(int bus) :
_dT(0), _dT(0),
_temp64(0), _temp64(0),
_baro_topic(-1), _baro_topic(-1),
_reads(0), _sample_perf(perf_alloc(PC_ELAPSED, "ms5611_read")),
_measure_errors(0), _comms_errors(perf_alloc(PC_COUNT, "ms5611_comms_errors")),
_read_errors(0), _buffer_overflows(perf_alloc(PC_COUNT, "ms5611_buffer_overflows"))
_buf_overflows(0),
_sample_perf(perf_alloc(PC_ELAPSED, "ms5611_read"))
{ {
// enable debug() calls // enable debug() calls
_debug_enabled = true; _debug_enabled = true;
@ -292,6 +287,12 @@ MS5611::init()
_oldest_report = _next_report = 0; _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; ret = OK;
out: out:
return ret; return ret;
@ -300,7 +301,7 @@ out:
int int
MS5611::probe() MS5611::probe()
{ {
_retries = 3; _retries = 10;
if((OK == probe_address(MS5611_ADDRESS_1)) || if((OK == probe_address(MS5611_ADDRESS_1)) ||
(OK == probe_address(MS5611_ADDRESS_2))) { (OK == probe_address(MS5611_ADDRESS_2))) {
_retries = 1; _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 */ /* if there was no data, warn the caller */
return ret ? ret : -EAGAIN; 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 */ /* state machine will have generated a report, copy it out */
memcpy(buffer, _reports, sizeof(*_reports)); memcpy(buffer, _reports, sizeof(*_reports));
ret = sizeof(*_reports); ret = sizeof(*_reports);
_reads++;
} while (0); } while (0);
@ -541,31 +539,14 @@ MS5611::cycle_trampoline(void *arg)
void void
MS5611::cycle() 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? */ /* collection phase? */
if (_collect_phase) { if (_collect_phase) {
/* perform collection */ /* perform collection */
if (OK != collect()) { if (OK != collect()) {
log("FATAL collection error - restarting\n"); log("collection error");
/* reset the collection state machine and try again */
start(); start();
return; return;
} }
@ -592,10 +573,8 @@ MS5611::cycle()
} }
/* measurement phase */ /* measurement phase */
if (OK != measure()) { if (OK != measure())
log("FATAL measure error - restarting\n"); log("measure error");
start();
}
/* next phase is collection */ /* next phase is collection */
_collect_phase = true; _collect_phase = true;
@ -623,7 +602,7 @@ MS5611::measure()
ret = transfer(&cmd_data, 1, nullptr, 0); ret = transfer(&cmd_data, 1, nullptr, 0);
if (OK != ret) if (OK != ret)
_measure_errors++; perf_count(_comms_errors);
return ret; return ret;
} }
@ -643,7 +622,7 @@ MS5611::collect()
_reports[_next_report].timestamp = hrt_absolute_time(); _reports[_next_report].timestamp = hrt_absolute_time();
if (OK != transfer(&cmd, 1, &data[0], 3)) { if (OK != transfer(&cmd, 1, &data[0], 3)) {
_read_errors++; perf_count(_comms_errors);
return -EIO; return -EIO;
} }
@ -691,7 +670,7 @@ MS5611::collect()
/* if we are running up against the oldest report, toss it */ /* if we are running up against the oldest report, toss it */
if (_next_report == _oldest_report) { if (_next_report == _oldest_report) {
_buf_overflows++; perf_count(_buffer_overflows);
INCREMENT(_oldest_report, _num_reports); INCREMENT(_oldest_report, _num_reports);
} }
@ -774,10 +753,9 @@ MS5611::crc4(uint16_t *n_prom)
void void
MS5611::print_info() MS5611::print_info()
{ {
printf("reads: %u\n", _reads); perf_print_counter(_sample_perf);
printf("measure errors: %u\n", _measure_errors); perf_print_counter(_comms_errors);
printf("read errors: %u\n", _read_errors); perf_print_counter(_buffer_overflows);
printf("read overflows: %u\n", _buf_overflows);
printf("poll interval: %u ticks\n", _measure_ticks); printf("poll interval: %u ticks\n", _measure_ticks);
printf("report queue: %u (%u/%u @ %p)\n", printf("report queue: %u (%u/%u @ %p)\n",
_num_reports, _oldest_report, _next_report, _reports); _num_reports, _oldest_report, _next_report, _reports);

3
apps/sensors/Makefile
View File

@ -39,4 +39,7 @@ APPNAME = sensors
PRIORITY = SCHED_PRIORITY_MAX-5 PRIORITY = SCHED_PRIORITY_MAX-5
STACKSIZE = 4096 STACKSIZE = 4096
CXXSRCS = sensors.cpp
CSRCS = sensor_params.c
include $(APPDIR)/mk/app.mk include $(APPDIR)/mk/app.mk

108
apps/sensors/sensor_params.c Normal file
View File

@ -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);

985
apps/sensors/sensors.cpp Normal file
View File

@ -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");
}

2
apps/systemlib/err.c
View File

@ -155,7 +155,7 @@ warn(const char *fmt, ...)
void void
vwarn(const char *fmt, va_list args) vwarn(const char *fmt, va_list args)
{ {
warnerr_core(NOCODE, fmt, args); warnerr_core(errno, fmt, args);
} }
void void

27
apps/systemlib/param/param.h
View File

@ -47,10 +47,13 @@
#include <stdint.h> #include <stdint.h>
#include <stdbool.h> #include <stdbool.h>
#include <sys/types.h>
/** Maximum size of the parameter backing file */ /** Maximum size of the parameter backing file */
#define PARAM_FILE_MAXSIZE 4096 #define PARAM_FILE_MAXSIZE 4096
__BEGIN_DECLS
/** /**
* Parameter types. * 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 * Note that these structures are not known by name; they are
* collected into a section that is iterated by the parameter * collected into a section that is iterated by the parameter
* code. * 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 */ /** define an int32 parameter */
@ -199,9 +206,9 @@ __EXPORT void param_foreach(void (*func)(void *arg, param_t param), void *arg,
static const \ static const \
__attribute__((used, section("__param"))) \ __attribute__((used, section("__param"))) \
struct param_info_s __param__##_name = { \ struct param_info_s __param__##_name = { \
.name = #_name, \ #_name, \
.type = PARAM_TYPE_INT32, \ PARAM_TYPE_INT32, \
.val.i = _default \ .val.i = _default \
} }
/** define a float parameter */ /** define a float parameter */
@ -209,9 +216,9 @@ __EXPORT void param_foreach(void (*func)(void *arg, param_t param), void *arg,
static const \ static const \
__attribute__((used, section("__param"))) \ __attribute__((used, section("__param"))) \
struct param_info_s __param__##_name = { \ struct param_info_s __param__##_name = { \
.name = #_name, \ #_name, \
.type = PARAM_TYPE_FLOAT, \ PARAM_TYPE_FLOAT, \
.val.f = _default \ .val.f = _default \
} }
/** define a parameter that points to a structure */ /** 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 \ static const \
__attribute__((used, section("__param"))) \ __attribute__((used, section("__param"))) \
struct param_info_s __param__##_name = { \ struct param_info_s __param__##_name = { \
.name = #_name, \ #_name, \
.type = PARAM_TYPE_STRUCT + sizeof(_default), \ PARAM_TYPE_STRUCT + sizeof(_default), \
.val.p = &_default; \ .val.p = &_default; \
} }
/** /**
@ -245,4 +252,6 @@ struct param_info_s {
union param_value_u val; union param_value_u val;
}; };
__END_DECLS
#endif #endif

2
apps/uORB/topics/rc_channels.h
View File

@ -96,7 +96,7 @@ struct rc_channels_s {
uint8_t chan_count; /**< maximum number of valid channels */ uint8_t chan_count; /**< maximum number of valid channels */
/*String array to store the names of the functions*/ /*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 function[RC_CHANNELS_FUNCTION_MAX];
uint8_t rssi; /**< Overall receive signal strength */ uint8_t rssi; /**< Overall receive signal strength */
}; /**< radio control channels. */ }; /**< radio control channels. */

2
nuttx/configs/px4fmu/nsh/defconfig
View File

@ -543,7 +543,7 @@ CONFIG_DEV_CONSOLE=y
CONFIG_DEV_LOWCONSOLE=n CONFIG_DEV_LOWCONSOLE=n
CONFIG_MUTEX_TYPES=n CONFIG_MUTEX_TYPES=n
CONFIG_PRIORITY_INHERITANCE=y CONFIG_PRIORITY_INHERITANCE=y
CONFIG_SEM_PREALLOCHOLDERS=0 CONFIG_SEM_PREALLOCHOLDERS=8
CONFIG_SEM_NNESTPRIO=8 CONFIG_SEM_NNESTPRIO=8
CONFIG_FDCLONE_DISABLE=n CONFIG_FDCLONE_DISABLE=n
CONFIG_FDCLONE_STDIO=y CONFIG_FDCLONE_STDIO=y