Mirrors_Framework/PX4-Autopilot
1
0
Fork 0
mirror of https://github.com/PX4/PX4-Autopilot.git synced 2026-06-06 06:43:21 +08:00
Code Issues Actions 29 Packages Projects Releases Wiki Activity

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

Browse Source
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
Download Patch File Download Diff File Expand all files Collapse all files
Debug/NuttX
+1
View File
@@ -95,6 +95,7 @@ end
document showfiles
. showfiles <TCB pointer>
. Prints the files opened by a task.
end
################################################################################
# Task display
apps/drivers/device/i2c.cpp
+1 -1
View File
@@ -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;
apps/drivers/device/spi.cpp
+1 -1
View File
@@ -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;
apps/drivers/hmc5883/hmc5883.cpp
+22 -44
View File
@@ -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);
apps/drivers/mpu6000/mpu6000.cpp
+3 -3
View File
@@ -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;
}
apps/drivers/ms5611/ms5611.cpp
+22 -44
View File
@@ -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);
apps/sensors/Makefile
+3
View File
@@ -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
apps/sensors/sensor_params.c
+108
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);
apps/sensors/sensors.cpp
+985
View File
File diff suppressed because it is too large Load Diff
apps/systemlib/err.c
+1 -1
View File
@@ -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
apps/systemlib/param/param.h
+18 -9
View File
@@ -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
apps/uORB/topics/rc_channels.h
+1 -1
View File
@@ -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. */
nuttx/configs/px4fmu/nsh/defconfig
+1 -1
View File
@@ -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