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

Merge pull request #590 from PX4/pubsub_cleanup 

Pubsub cleanup
This commit is contained in:
Lorenz Meier 2014-02-10 21:09:12 +01:00
parent f4e0e74d17 773f70a9df
commit 80100dcf19
25 changed files with 452 additions and 151 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

9
makefiles/config_px4fmu-v2_test.mk
View File

@ -18,6 +18,12 @@ MODULES += drivers/stm32/tone_alarm
MODULES += drivers/led
MODULES += drivers/boards/px4fmu-v2
MODULES += drivers/px4io
MODULES += drivers/rgbled
MODULES += drivers/mpu6000
MODULES += drivers/lsm303d
MODULES += drivers/l3gd20
MODULES += drivers/hmc5883
MODULES += drivers/ms5611
MODULES += systemcmds/perf
MODULES += systemcmds/reboot
MODULES += systemcmds/tests
@ -31,6 +37,9 @@ MODULES += systemcmds/hw_ver
MODULES += modules/systemlib
MODULES += modules/systemlib/mixer
MODULES += modules/uORB
LIBRARIES += lib/mathlib/CMSIS
MODULES += lib/mathlib
MODULES += lib/mathlib/math/filter
#
# Transitional support - add commands from the NuttX export archive.

28
src/drivers/airspeed/airspeed.cpp
View File

@ -1,6 +1,6 @@
/****************************************************************************
*
* Copyright (C) 2013 PX4 Development Team. All rights reserved.
* Copyright (c) 2013, 2014 PX4 Development Team. All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions
@ -86,6 +86,7 @@ Airspeed::Airspeed(int bus, int address, unsigned conversion_interval) :
_collect_phase(false),
_diff_pres_offset(0.0f),
_airspeed_pub(-1),
_class_instance(-1),
_conversion_interval(conversion_interval),
_sample_perf(perf_alloc(PC_ELAPSED, "airspeed_read")),
_comms_errors(perf_alloc(PC_COUNT, "airspeed_comms_errors"))
@ -102,6 +103,9 @@ Airspeed::~Airspeed()
/* make sure we are truly inactive */
stop();
if (_class_instance != -1)
unregister_class_devname(AIRSPEED_DEVICE_PATH, _class_instance);
/* free any existing reports */
if (_reports != nullptr)
delete _reports;
@ -126,13 +130,23 @@ Airspeed::init()
if (_reports == nullptr)
goto out;
/* get a publish handle on the airspeed topic */
differential_pressure_s zero_report;
memset(&zero_report, 0, sizeof(zero_report));
_airspeed_pub = orb_advertise(ORB_ID(differential_pressure), &zero_report);
/* register alternate interfaces if we have to */
_class_instance = register_class_devname(AIRSPEED_DEVICE_PATH);
if (_airspeed_pub < 0)
warnx("failed to create airspeed sensor object. Did you start uOrb?");
/* publication init */
if (_class_instance == CLASS_DEVICE_PRIMARY) {
/* advertise sensor topic, measure manually to initialize valid report */
struct differential_pressure_s arp;
measure();
_reports->get(&arp);
/* measurement will have generated a report, publish */
_airspeed_pub = orb_advertise(ORB_ID(differential_pressure), &arp);
if (_airspeed_pub < 0)
warnx("failed to create airspeed sensor object. uORB started?");
}
ret = OK;
/* sensor is ok, but we don't really know if it is within range */

4
src/drivers/airspeed/airspeed.h
View File

@ -1,6 +1,6 @@
/****************************************************************************
*
* Copyright (C) 2013 PX4 Development Team. All rights reserved.
* Copyright (c) 2013, 2014 PX4 Development Team. All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions
@ -127,6 +127,8 @@ protected:
orb_advert_t _airspeed_pub;
int _class_instance;
unsigned _conversion_interval;
perf_counter_t _sample_perf;

25
src/drivers/bma180/bma180.cpp
View File

@ -1,6 +1,6 @@
/****************************************************************************
*
* Copyright (C) 2012 PX4 Development Team. All rights reserved.
* Copyright (c) 2012-2014 PX4 Development Team. All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions
@ -153,6 +153,7 @@ private:
float _accel_range_scale;
float _accel_range_m_s2;
orb_advert_t _accel_topic;
int _class_instance;
unsigned _current_lowpass;
unsigned _current_range;
@ -238,6 +239,7 @@ BMA180::BMA180(int bus, spi_dev_e device) :
_accel_range_scale(0.0f),
_accel_range_m_s2(0.0f),
_accel_topic(-1),
_class_instance(-1),
_current_lowpass(0),
_current_range(0),
_sample_perf(perf_alloc(PC_ELAPSED, "bma180_read"))
@ -282,11 +284,6 @@ BMA180::init()
if (_reports == nullptr)
goto out;
/* advertise sensor topic */
struct accel_report zero_report;
memset(&zero_report, 0, sizeof(zero_report));
_accel_topic = orb_advertise(ORB_ID(sensor_accel), &zero_report);
/* perform soft reset (p48) */
write_reg(ADDR_RESET, SOFT_RESET);
@ -322,6 +319,19 @@ BMA180::init()
ret = ERROR;
}
_class_instance = register_class_devname(ACCEL_DEVICE_PATH);
/* advertise sensor topic, measure manually to initialize valid report */
measure();
if (_class_instance == CLASS_DEVICE_PRIMARY) {
struct accel_report arp;
_reports->get(&arp);
/* measurement will have generated a report, publish */
_accel_topic = orb_advertise(ORB_ID(sensor_accel), &arp);
}
out:
return ret;
}
@ -723,7 +733,8 @@ BMA180::measure()
poll_notify(POLLIN);
/* publish for subscribers */
orb_publish(ORB_ID(sensor_accel), _accel_topic, &report);
if (_accel_topic > 0 && !(_pub_blocked))
orb_publish(ORB_ID(sensor_accel), _accel_topic, &report);
/* stop the perf counter */
perf_end(_sample_perf);

12
src/drivers/device/cdev.cpp
View File

@ -1,6 +1,6 @@
/****************************************************************************
*
* Copyright (C) 2012 PX4 Development Team. All rights reserved.
* Copyright (c) 2012-2014 PX4 Development Team. All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions
@ -38,6 +38,7 @@
*/
#include "device.h"
#include "drivers/drv_device.h"
#include <sys/ioctl.h>
#include <arch/irq.h>
@ -93,6 +94,7 @@ CDev::CDev(const char *name,
Device(name, irq),
// public
// protected
_pub_blocked(false),
// private
_devname(devname),
_registered(false),
@ -256,6 +258,14 @@ CDev::ioctl(struct file *filp, int cmd, unsigned long arg)
case DIOC_GETPRIV:
*(void **)(uintptr_t)arg = (void *)this;
return OK;
break;
case DEVIOCSPUBBLOCK:
_pub_blocked = (arg != 0);
return OK;
break;
case DEVIOCGPUBBLOCK:
return _pub_blocked;
break;
}
return -ENOTTY;

2
src/drivers/device/device.cpp
View File

@ -1,6 +1,6 @@
/****************************************************************************
*
* Copyright (C) 2012 PX4 Development Team. All rights reserved.
* Copyright (c) 2012-2014 PX4 Development Team. All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions

4
src/drivers/device/device.h
View File

@ -1,6 +1,6 @@
/****************************************************************************
*
* Copyright (C) 2012 PX4 Development Team. All rights reserved.
* Copyright (c) 2012-2014 PX4 Development Team. All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions
@ -415,6 +415,8 @@ protected:
*/
virtual int unregister_class_devname(const char *class_devname, unsigned class_instance);
bool _pub_blocked; /**< true if publishing should be blocked */
private:
static const unsigned _max_pollwaiters = 8;

62
src/drivers/drv_device.h Normal file
View File

@ -0,0 +1,62 @@
/****************************************************************************
*
* Copyright (c) 2014 PX4 Development Team. All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions
* are met:
*
* 1. Redistributions of source code must retain the above copyright
* notice, this list of conditions and the following disclaimer.
* 2. Redistributions in binary form must reproduce the above copyright
* notice, this list of conditions and the following disclaimer in
* the documentation and/or other materials provided with the
* distribution.
* 3. Neither the name PX4 nor the names of its contributors may be
* used to endorse or promote products derived from this software
* without specific prior written permission.
*
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
* OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
* AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
* POSSIBILITY OF SUCH DAMAGE.
*
****************************************************************************/
/**
* @file drv_device.h
*
* Generic device / sensor interface.
*/
#ifndef _DRV_DEVICE_H
#define _DRV_DEVICE_H
#include <stdint.h>
#include <sys/ioctl.h>
#include "drv_sensor.h"
#include "drv_orb_dev.h"
/*
* ioctl() definitions
*/
#define _DEVICEIOCBASE (0x100)
#define _DEVICEIOC(_n) (_IOC(_DEVICEIOCBASE, _n))
/** ask device to stop publishing */
#define DEVIOCSPUBBLOCK _DEVICEIOC(0)
/** check publication block status */
#define DEVIOCGPUBBLOCK _DEVICEIOC(1)
#endif /* _DRV_DEVICE_H */

8
src/drivers/ets_airspeed/ets_airspeed.cpp
View File

@ -1,6 +1,6 @@
/****************************************************************************
*
* Copyright (C) 2013 PX4 Development Team. All rights reserved.
* Copyright (c) 2013, 2014 PX4 Development Team. All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions
@ -184,8 +184,10 @@ ETSAirspeed::collect()
report.voltage = 0;
report.max_differential_pressure_pa = _max_differential_pressure_pa;
/* announce the airspeed if needed, just publish else */
orb_publish(ORB_ID(differential_pressure), _airspeed_pub, &report);
if (_airspeed_pub > 0 && !(_pub_blocked)) {
/* publish it */
orb_publish(ORB_ID(differential_pressure), _airspeed_pub, &report);
}
new_report(report);

21
src/drivers/gps/gps.cpp
View File

@ -289,11 +289,13 @@ GPS::task_main()
//no time and satellite information simulated
if (_report_pub > 0) {
orb_publish(ORB_ID(vehicle_gps_position), _report_pub, &_report);
if (!(_pub_blocked)) {
if (_report_pub > 0) {
orb_publish(ORB_ID(vehicle_gps_position), _report_pub, &_report);
} else {
_report_pub = orb_advertise(ORB_ID(vehicle_gps_position), &_report);
} else {
_report_pub = orb_advertise(ORB_ID(vehicle_gps_position), &_report);
}
}
usleep(2e5);
@ -330,11 +332,14 @@ GPS::task_main()
while (_Helper->receive(TIMEOUT_5HZ) > 0 && !_task_should_exit) {
// lock();
/* opportunistic publishing - else invalid data would end up on the bus */
if (_report_pub > 0) {
orb_publish(ORB_ID(vehicle_gps_position), _report_pub, &_report);
} else {
_report_pub = orb_advertise(ORB_ID(vehicle_gps_position), &_report);
if (!(_pub_blocked)) {
if (_report_pub > 0) {
orb_publish(ORB_ID(vehicle_gps_position), _report_pub, &_report);
} else {
_report_pub = orb_advertise(ORB_ID(vehicle_gps_position), &_report);
}
}
last_rate_count++;

7
src/drivers/gps/gps_helper.cpp
View File

@ -1,8 +1,6 @@
/****************************************************************************
*
* Copyright (C) 2012,2013 PX4 Development Team. All rights reserved.
* Author: Thomas Gubler <thomasgubler@student.ethz.ch>
* Julian Oes <joes@student.ethz.ch>
* Copyright (c) 2012-2014 PX4 Development Team. All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions
@ -41,6 +39,9 @@
/**
* @file gps_helper.cpp
*
* @author Thomas Gubler <thomasgubler@student.ethz.ch>
* @author Julian Oes <joes@student.ethz.ch>
*/
float

6
src/drivers/gps/gps_helper.h
View File

@ -1,8 +1,6 @@
/****************************************************************************
*
* Copyright (c) 2012, 2013 PX4 Development Team. All rights reserved.
* Author: Thomas Gubler <thomasgubler@student.ethz.ch>
* Julian Oes <joes@student.ethz.ch>
* Copyright (c) 2012-2014 PX4 Development Team. All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions
@ -35,6 +33,8 @@
/**
* @file gps_helper.h
* @author Thomas Gubler <thomasgubler@student.ethz.ch>
* @author Julian Oes <joes@student.ethz.ch>
*/
#ifndef GPS_HELPER_H

2
src/drivers/gps/module.mk
View File

@ -1,6 +1,6 @@
############################################################################
#
# Copyright (c) 2012, 2013 PX4 Development Team. All rights reserved.
# Copyright (c) 2012-2014 PX4 Development Team. All rights reserved.
#
# Redistribution and use in source and binary forms, with or without
# modification, are permitted provided that the following conditions

11
src/drivers/gps/mtk.cpp
View File

@ -1,8 +1,6 @@
/****************************************************************************
*
* Copyright (C) 2013 PX4 Development Team. All rights reserved.
* Author: Thomas Gubler <thomasgubler@student.ethz.ch>
* Julian Oes <joes@student.ethz.ch>
* Copyright (c) 2012, 2013 PX4 Development Team. All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions
@ -33,7 +31,12 @@
*
****************************************************************************/
/* @file mkt.cpp */
/**
* @file mtk.cpp
*
* @author Thomas Gubler <thomasgubler@student.ethz.ch>
* @author Julian Oes <joes@student.ethz.ch>
*/
#include <unistd.h>
#include <stdio.h>

11
src/drivers/gps/mtk.h
View File

@ -1,8 +1,6 @@
/****************************************************************************
*
* Copyright (C) 2008-2013 PX4 Development Team. All rights reserved.
* Author: Thomas Gubler <thomasgubler@student.ethz.ch>
* Julian Oes <joes@student.ethz.ch>
* Copyright (c) 2012, 2013 PX4 Development Team. All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions
@ -33,7 +31,12 @@
*
****************************************************************************/
/* @file mtk.h */
/**
* @file mtk.cpp
*
* @author Thomas Gubler <thomasgubler@student.ethz.ch>
* @author Julian Oes <joes@student.ethz.ch>
*/
#ifndef MTK_H_
#define MTK_H_

10
src/drivers/gps/ubx.cpp
View File

@ -1,9 +1,6 @@
/****************************************************************************
*
* Copyright (C) 2013 PX4 Development Team. All rights reserved.
* Author: Thomas Gubler <thomasgubler@student.ethz.ch>
* Julian Oes <joes@student.ethz.ch>
* Anton Babushkin <anton.babushkin@me.com>
* Copyright (c) 2012, 2013 PX4 Development Team. All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions
@ -40,8 +37,13 @@
* U-Blox protocol implementation. Following u-blox 6/7 Receiver Description
* including Prototol Specification.
*
* @author Thomas Gubler <thomasgubler@student.ethz.ch>
* @author Julian Oes <joes@student.ethz.ch>
* @author Anton Babushkin <anton.babushkin@me.com>
*
* @see http://www.u-blox.com/images/downloads/Product_Docs/u-blox6_ReceiverDescriptionProtocolSpec_%28GPS.G6-SW-10018%29.pdf
*/
#include <assert.h>
#include <math.h>
#include <poll.h>

17
src/drivers/gps/ubx.h
View File

@ -1,9 +1,6 @@
/****************************************************************************
*
* Copyright (C) 2008-2013 PX4 Development Team. All rights reserved.
* Author: Thomas Gubler <thomasgubler@student.ethz.ch>
* Julian Oes <joes@student.ethz.ch>
* Anton Babushkin <anton.babushkin@me.com>
* Copyright (c) 2012, 2013 PX4 Development Team. All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions
@ -34,7 +31,17 @@
*
****************************************************************************/
/* @file U-Blox protocol definitions */
/**
* @file ubx.h
*
* U-Blox protocol definition. Following u-blox 6/7 Receiver Description
* including Prototol Specification.
*
* @author Thomas Gubler <thomasgubler@student.ethz.ch>
* @author Julian Oes <joes@student.ethz.ch>
* @author Anton Babushkin <anton.babushkin@me.com>
*
*/
#ifndef UBX_H_
#define UBX_H_

36
src/drivers/hmc5883/hmc5883.cpp
View File

@ -1,6 +1,6 @@
/****************************************************************************
*
* Copyright (c) 2012, 2013 PX4 Development Team. All rights reserved.
* Copyright (c) 2012-2014 PX4 Development Team. All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions
@ -381,16 +381,6 @@ HMC5883::init()
reset();
_class_instance = register_class_devname(MAG_DEVICE_PATH);
if (_class_instance == CLASS_DEVICE_PRIMARY) {
/* get a publish handle on the mag topic if we are
* the primary mag */
struct mag_report zero_report;
memset(&zero_report, 0, sizeof(zero_report));
_mag_topic = orb_advertise(ORB_ID(sensor_mag), &zero_report);
if (_mag_topic < 0)
debug("failed to create sensor_mag object");
}
ret = OK;
/* sensor is ok, but not calibrated */
@ -867,9 +857,17 @@ HMC5883::collect()
/* z remains z */
new_report.z = ((report.z * _range_scale) - _scale.z_offset) * _scale.z_scale;
if (_mag_topic != -1) {
/* publish it */
orb_publish(ORB_ID(sensor_mag), _mag_topic, &new_report);
if (_class_instance == CLASS_DEVICE_PRIMARY && !(_pub_blocked)) {
if (_mag_topic != -1) {
/* publish it */
orb_publish(ORB_ID(sensor_mag), _mag_topic, &new_report);
} else {
_mag_topic = orb_advertise(ORB_ID(sensor_mag), &new_report);
if (_mag_topic < 0)
debug("failed to create sensor_mag publication");
}
}
/* post a report to the ring */
@ -1140,10 +1138,12 @@ int HMC5883::check_calibration()
SUBSYSTEM_TYPE_MAG};
static orb_advert_t pub = -1;
if (pub > 0) {
orb_publish(ORB_ID(subsystem_info), pub, &info);
} else {
pub = orb_advertise(ORB_ID(subsystem_info), &info);
if (!(_pub_blocked)) {
if (pub > 0) {
orb_publish(ORB_ID(subsystem_info), pub, &info);
} else {
pub = orb_advertise(ORB_ID(subsystem_info), &info);
}
}
}

27
src/drivers/l3gd20/l3gd20.cpp
View File

@ -1,6 +1,6 @@
/****************************************************************************
*
* Copyright (c) 2012, 2013 PX4 Development Team. All rights reserved.
* Copyright (c) 2012-2014 PX4 Development Team. All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions
@ -379,15 +379,24 @@ L3GD20::init()
goto out;
_class_instance = register_class_devname(GYRO_DEVICE_PATH);
if (_class_instance == CLASS_DEVICE_PRIMARY) {
/* advertise sensor topic */
struct gyro_report zero_report;
memset(&zero_report, 0, sizeof(zero_report));
_gyro_topic = orb_advertise(ORB_ID(sensor_gyro), &zero_report);
}
reset();
measure();
if (_class_instance == CLASS_DEVICE_PRIMARY) {
/* advertise sensor topic, measure manually to initialize valid report */
struct gyro_report grp;
_reports->get(&grp);
_gyro_topic = orb_advertise(ORB_ID(sensor_gyro), &grp);
if (_gyro_topic < 0)
debug("failed to create sensor_gyro publication");
}
ret = OK;
out:
return ret;
@ -894,8 +903,10 @@ L3GD20::measure()
poll_notify(POLLIN);
/* publish for subscribers */
if (_gyro_topic > 0)
if (_gyro_topic > 0 && !(_pub_blocked)) {
/* publish it */
orb_publish(ORB_ID(sensor_gyro), _gyro_topic, &report);
}
_read++;

71
src/drivers/lsm303d/lsm303d.cpp
View File

@ -1,6 +1,6 @@
/****************************************************************************
*
* Copyright (C) 2013 PX4 Development Team. All rights reserved.
* Copyright (c) 2013, 2014 PX4 Development Team. All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions
@ -277,15 +277,15 @@ private:
unsigned _mag_samplerate;
orb_advert_t _accel_topic;
int _class_instance;
int _accel_class_instance;
unsigned _accel_read;
unsigned _mag_read;
perf_counter_t _accel_sample_perf;
perf_counter_t _mag_sample_perf;
perf_counter_t _reg7_resets;
perf_counter_t _reg1_resets;
perf_counter_t _reg7_resets;
perf_counter_t _extreme_values;
perf_counter_t _accel_reschedules;
@ -295,8 +295,8 @@ private:
// expceted values of reg1 and reg7 to catch in-flight
// brownouts of the sensor
uint8_t _reg7_expected;
uint8_t _reg1_expected;
uint8_t _reg7_expected;
// accel logging
int _accel_log_fd;
@ -500,7 +500,7 @@ LSM303D::LSM303D(int bus, const char* path, spi_dev_e device) :
_mag_range_scale(0.0f),
_mag_samplerate(0),
_accel_topic(-1),
_class_instance(-1),
_accel_class_instance(-1),
_accel_read(0),
_mag_read(0),
_accel_sample_perf(perf_alloc(PC_ELAPSED, "lsm303d_accel_read")),
@ -551,8 +551,8 @@ LSM303D::~LSM303D()
if (_mag_reports != nullptr)
delete _mag_reports;
if (_class_instance != -1)
unregister_class_devname(ACCEL_DEVICE_PATH, _class_instance);
if (_accel_class_instance != -1)
unregister_class_devname(ACCEL_DEVICE_PATH, _accel_class_instance);
delete _mag;
@ -562,13 +562,13 @@ LSM303D::~LSM303D()
perf_free(_reg1_resets);
perf_free(_reg7_resets);
perf_free(_extreme_values);
perf_free(_accel_reschedules);
}
int
LSM303D::init()
{
int ret = ERROR;
int mag_ret;
/* do SPI init (and probe) first */
if (SPI::init() != OK) {
@ -597,13 +597,37 @@ LSM303D::init()
goto out;
}
_class_instance = register_class_devname(ACCEL_DEVICE_PATH);
if (_class_instance == CLASS_DEVICE_PRIMARY) {
// we are the primary accel device, so advertise to
// the ORB
struct accel_report zero_report;
memset(&zero_report, 0, sizeof(zero_report));
_accel_topic = orb_advertise(ORB_ID(sensor_accel), &zero_report);
/* fill report structures */
measure();
if (_mag->_mag_class_instance == CLASS_DEVICE_PRIMARY) {
/* advertise sensor topic, measure manually to initialize valid report */
struct mag_report mrp;
_mag_reports->get(&mrp);
/* measurement will have generated a report, publish */
_mag->_mag_topic = orb_advertise(ORB_ID(sensor_mag), &mrp);
if (_mag->_mag_topic < 0)
debug("failed to create sensor_mag publication");
}
_accel_class_instance = register_class_devname(ACCEL_DEVICE_PATH);
if (_accel_class_instance == CLASS_DEVICE_PRIMARY) {
/* advertise sensor topic, measure manually to initialize valid report */
struct accel_report arp;
_accel_reports->get(&arp);
/* measurement will have generated a report, publish */
_accel_topic = orb_advertise(ORB_ID(sensor_accel), &arp);
if (_accel_topic < 0)
debug("failed to create sensor_accel publication");
}
out:
@ -727,7 +751,7 @@ LSM303D::check_extremes(const accel_report *arb)
_last_log_us = now;
::dprintf(_accel_log_fd, "ARB %llu %.3f %.3f %.3f %d %d %d boot_ok=%u\r\n",
(unsigned long long)arb->timestamp,
arb->x, arb->y, arb->z,
(double)arb->x, (double)arb->y, (double)arb->z,
(int)arb->x_raw,
(int)arb->y_raw,
(int)arb->z_raw,
@ -1517,8 +1541,8 @@ LSM303D::measure()
/* notify anyone waiting for data */
poll_notify(POLLIN);
if (_accel_topic != -1) {
/* publish for subscribers */
if (_accel_topic > 0 && !(_pub_blocked)) {
/* publish it */
orb_publish(ORB_ID(sensor_accel), _accel_topic, &accel_report);
}
@ -1591,8 +1615,8 @@ LSM303D::mag_measure()
/* notify anyone waiting for data */
poll_notify(POLLIN);
if (_mag->_mag_topic != -1) {
/* publish for subscribers */
if (_mag->_mag_topic > 0 && !(_pub_blocked)) {
/* publish it */
orb_publish(ORB_ID(sensor_mag), _mag->_mag_topic, &mag_report);
}
@ -1707,13 +1731,6 @@ LSM303D_mag::init()
goto out;
_mag_class_instance = register_class_devname(MAG_DEVICE_PATH);
if (_mag_class_instance == CLASS_DEVICE_PRIMARY) {
// we are the primary mag device, so advertise to
// the ORB
struct mag_report zero_report;
memset(&zero_report, 0, sizeof(zero_report));
_mag_topic = orb_advertise(ORB_ID(sensor_mag), &zero_report);
}
out:
return ret;

8
src/drivers/meas_airspeed/meas_airspeed.cpp
View File

@ -1,6 +1,6 @@
/****************************************************************************
*
* Copyright (C) 2013 PX4 Development Team. All rights reserved.
* Copyright (c) 2013, 2014 PX4 Development Team. All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions
@ -208,8 +208,10 @@ MEASAirspeed::collect()
report.voltage = 0;
report.max_differential_pressure_pa = _max_differential_pressure_pa;
/* announce the airspeed if needed, just publish else */
orb_publish(ORB_ID(differential_pressure), _airspeed_pub, &report);
if (_airspeed_pub > 0 && !(_pub_blocked)) {
/* publish it */
orb_publish(ORB_ID(differential_pressure), _airspeed_pub, &report);
}
new_report(report);

49
src/drivers/mpu6000/mpu6000.cpp
View File

@ -1,6 +1,6 @@
/****************************************************************************
*
* Copyright (c) 2012, 2013 PX4 Development Team. All rights reserved.
* Copyright (c) 2012-2014 PX4 Development Team. All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions
@ -443,7 +443,6 @@ int
MPU6000::init()
{
int ret;
int gyro_ret;
/* do SPI init (and probe) first */
ret = SPI::init();
@ -488,16 +487,36 @@ MPU6000::init()
return ret;
}
/* fetch an initial set of measurements for advertisement */
_accel_class_instance = register_class_devname(ACCEL_DEVICE_PATH);
measure();
_accel_class_instance = register_class_devname(ACCEL_DEVICE_PATH);
if (_accel_class_instance == CLASS_DEVICE_PRIMARY) {
/* advertise accel topic */
accel_report ar;
_accel_reports->get(&ar);
_accel_topic = orb_advertise(ORB_ID(sensor_accel), &ar);
}
/* advertise sensor topic, measure manually to initialize valid report */
struct accel_report arp;
_accel_reports->get(&arp);
/* measurement will have generated a report, publish */
_accel_topic = orb_advertise(ORB_ID(sensor_accel), &arp);
if (_accel_topic < 0)
debug("failed to create sensor_accel publication");
}
if (_gyro->_gyro_class_instance == CLASS_DEVICE_PRIMARY) {
/* advertise sensor topic, measure manually to initialize valid report */
struct gyro_report grp;
_gyro_reports->get(&grp);
_gyro->_gyro_topic = orb_advertise(ORB_ID(sensor_gyro), &grp);
if (_gyro->_gyro_topic < 0)
debug("failed to create sensor_gyro publication");
}
out:
return ret;
@ -1307,10 +1326,13 @@ MPU6000::measure()
poll_notify(POLLIN);
_gyro->parent_poll_notify();
if (_accel_topic != -1) {
if (_accel_topic > 0 && !(_pub_blocked)) {
/* publish it */
orb_publish(ORB_ID(sensor_accel), _accel_topic, &arb);
}
if (_gyro->_gyro_topic != -1) {
if (_gyro->_gyro_topic > 0 && !(_pub_blocked)) {
/* publish it */
orb_publish(ORB_ID(sensor_gyro), _gyro->_gyro_topic, &grb);
}
@ -1356,11 +1378,6 @@ MPU6000_gyro::init()
}
_gyro_class_instance = register_class_devname(GYRO_DEVICE_PATH);
if (_gyro_class_instance == CLASS_DEVICE_PRIMARY) {
gyro_report gr;
memset(&gr, 0, sizeof(gr));
_gyro_topic = orb_advertise(ORB_ID(sensor_gyro), &gr);
}
out:
return ret;

87
src/drivers/ms5611/ms5611.cpp
View File

@ -1,6 +1,6 @@
/****************************************************************************
*
* Copyright (C) 2012-2013 PX4 Development Team. All rights reserved.
* Copyright (c) 2012-2014 PX4 Development Team. All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions
@ -90,6 +90,7 @@ static const int ERROR = -1;
/* internal conversion time: 9.17 ms, so should not be read at rates higher than 100 Hz */
#define MS5611_CONVERSION_INTERVAL 10000 /* microseconds */
#define MS5611_MEASUREMENT_RATIO 3 /* pressure measurements per temperature measurement */
#define MS5611_BARO_DEVICE_PATH "/dev/ms5611"
class MS5611 : public device::CDev
{
@ -132,6 +133,8 @@ protected:
orb_advert_t _baro_topic;
int _class_instance;
perf_counter_t _sample_perf;
perf_counter_t _measure_perf;
perf_counter_t _comms_errors;
@ -192,7 +195,7 @@ protected:
extern "C" __EXPORT int ms5611_main(int argc, char *argv[]);
MS5611::MS5611(device::Device *interface, ms5611::prom_u &prom_buf) :
CDev("MS5611", BARO_DEVICE_PATH),
CDev("MS5611", MS5611_BARO_DEVICE_PATH),
_interface(interface),
_prom(prom_buf.s),
_measure_ticks(0),
@ -204,6 +207,7 @@ MS5611::MS5611(device::Device *interface, ms5611::prom_u &prom_buf) :
_SENS(0),
_msl_pressure(101325),
_baro_topic(-1),
_class_instance(-1),
_sample_perf(perf_alloc(PC_ELAPSED, "ms5611_read")),
_measure_perf(perf_alloc(PC_ELAPSED, "ms5611_measure")),
_comms_errors(perf_alloc(PC_COUNT, "ms5611_comms_errors")),
@ -218,6 +222,9 @@ MS5611::~MS5611()
/* make sure we are truly inactive */
stop_cycle();
if (_class_instance != -1)
unregister_class_devname(MS5611_BARO_DEVICE_PATH, _class_instance);
/* free any existing reports */
if (_reports != nullptr)
delete _reports;
@ -251,18 +258,57 @@ MS5611::init()
goto out;
}
/* get a publish handle on the baro topic */
struct baro_report zero_report;
memset(&zero_report, 0, sizeof(zero_report));
_baro_topic = orb_advertise(ORB_ID(sensor_baro), &zero_report);
/* register alternate interfaces if we have to */
_class_instance = register_class_devname(BARO_DEVICE_PATH);
if (_baro_topic < 0) {
debug("failed to create sensor_baro object");
ret = -ENOSPC;
goto out;
}
struct baro_report brp;
/* do a first measurement cycle to populate reports with valid data */
_measure_phase = 0;
_reports->flush();
/* this do..while is goto without goto */
do {
/* do temperature first */
if (OK != measure()) {
ret = -EIO;
break;
}
usleep(MS5611_CONVERSION_INTERVAL);
if (OK != collect()) {
ret = -EIO;
break;
}
/* now do a pressure measurement */
if (OK != measure()) {
ret = -EIO;
break;
}
usleep(MS5611_CONVERSION_INTERVAL);
if (OK != collect()) {
ret = -EIO;
break;
}
/* state machine will have generated a report, copy it out */
_reports->get(&brp);
ret = OK;
if (_class_instance == CLASS_DEVICE_PRIMARY) {
_baro_topic = orb_advertise(ORB_ID(sensor_baro), &brp);
if (_baro_topic < 0)
debug("failed to create sensor_baro publication");
}
} while (0);
ret = OK;
out:
return ret;
}
@ -670,7 +716,10 @@ MS5611::collect()
report.altitude = (((pow((p / p1), (-(a * R) / g))) * T1) - T1) / a;
/* publish it */
orb_publish(ORB_ID(sensor_baro), _baro_topic, &report);
if (_baro_topic > 0 && !(_pub_blocked)) {
/* publish it */
orb_publish(ORB_ID(sensor_baro), _baro_topic, &report);
}
if (_reports->force(&report)) {
perf_count(_buffer_overflows);
@ -812,7 +861,7 @@ start()
goto fail;
/* set the poll rate to default, starts automatic data collection */
fd = open(BARO_DEVICE_PATH, O_RDONLY);
fd = open(MS5611_BARO_DEVICE_PATH, O_RDONLY);
if (fd < 0) {
warnx("can't open baro device");
goto fail;
@ -846,10 +895,10 @@ test()
ssize_t sz;
int ret;
int fd = open(BARO_DEVICE_PATH, O_RDONLY);
int fd = open(MS5611_BARO_DEVICE_PATH, O_RDONLY);
if (fd < 0)
err(1, "%s open failed (try 'ms5611 start' if the driver is not running)", BARO_DEVICE_PATH);
err(1, "%s open failed (try 'ms5611 start' if the driver is not running)", MS5611_BARO_DEVICE_PATH);
/* do a simple demand read */
sz = read(fd, &report, sizeof(report));
@ -905,7 +954,7 @@ test()
void
reset()
{
int fd = open(BARO_DEVICE_PATH, O_RDONLY);
int fd = open(MS5611_BARO_DEVICE_PATH, O_RDONLY);
if (fd < 0)
err(1, "failed ");
@ -944,10 +993,10 @@ calibrate(unsigned altitude)
float pressure;
float p1;
int fd = open(BARO_DEVICE_PATH, O_RDONLY);
int fd = open(MS5611_BARO_DEVICE_PATH, O_RDONLY);
if (fd < 0)
err(1, "%s open failed (try 'ms5611 start' if the driver is not running)", BARO_DEVICE_PATH);
err(1, "%s open failed (try 'ms5611 start' if the driver is not running)", MS5611_BARO_DEVICE_PATH);
/* start the sensor polling at max */
if (OK != ioctl(fd, SENSORIOCSPOLLRATE, SENSOR_POLLRATE_MAX))

30
src/modules/commander/state_machine_helper.cpp
View File

@ -42,6 +42,8 @@
#include <unistd.h>
#include <stdint.h>
#include <stdbool.h>
#include <dirent.h>
#include <fcntl.h>
#include <uORB/uORB.h>
#include <uORB/topics/vehicle_status.h>
@ -51,6 +53,7 @@
#include <systemlib/param/param.h>
#include <systemlib/err.h>
#include <drivers/drv_hrt.h>
#include <drivers/drv_device.h>
#include <mavlink/mavlink_log.h>
#include "state_machine_helper.h"
@ -491,6 +494,33 @@ int hil_state_transition(hil_state_t new_state, int status_pub, struct vehicle_s
current_control_mode->flag_system_hil_enabled = true;
mavlink_log_critical(mavlink_fd, "Switched to ON hil state");
valid_transition = true;
// Disable publication of all attached sensors
/* list directory */
DIR *d;
struct dirent *direntry;
d = opendir("/dev");
if (d) {
while ((direntry = readdir(d)) != NULL) {
int sensfd = ::open(direntry->d_name, 0);
int block_ret = ::ioctl(sensfd, DEVIOCSPUBBLOCK, 0);
close(sensfd);
printf("Disabling %s\n: %s", direntry->d_name, (!block_ret) ? "OK" : "FAIL");
}
closedir(d);
warnx("directory listing ok (FS mounted and readable)");
} else {
/* failed opening dir */
warnx("FAILED LISTING DEVICE ROOT DIRECTORY");
return 1;
}
}
break;

56
src/systemcmds/config/config.c
View File

@ -56,6 +56,7 @@
#include <drivers/drv_gyro.h>
#include <drivers/drv_accel.h>
#include <drivers/drv_mag.h>
#include <drivers/drv_device.h>
#include "systemlib/systemlib.h"
#include "systemlib/err.h"
@ -65,6 +66,7 @@ __EXPORT int config_main(int argc, char *argv[]);
static void do_gyro(int argc, char *argv[]);
static void do_accel(int argc, char *argv[]);
static void do_mag(int argc, char *argv[]);
static void do_device(int argc, char *argv[]);
int
config_main(int argc, char *argv[])
@ -72,20 +74,60 @@ config_main(int argc, char *argv[])
if (argc >= 2) {
if (!strcmp(argv[1], "gyro")) {
do_gyro(argc - 2, argv + 2);
}
if (!strcmp(argv[1], "accel")) {
} else if (!strcmp(argv[1], "accel")) {
do_accel(argc - 2, argv + 2);
}
if (!strcmp(argv[1], "mag")) {
} else if (!strcmp(argv[1], "mag")) {
do_mag(argc - 2, argv + 2);
} else {
do_device(argc - 1, argv + 1);
}
}
errx(1, "expected a command, try 'gyro', 'accel', 'mag'");
}
static void
do_device(int argc, char *argv[])
{
if (argc < 2) {
errx(1, "no device path provided and command provided.");
}
int fd;
int ret;
fd = open(argv[0], 0);
if (fd < 0) {
warn("%s", argv[0]);
errx(1, "FATAL: no device found");
} else {
if (argc == 2 && !strcmp(argv[1], "block")) {
/* disable the device publications */
ret = ioctl(fd, DEVIOCSPUBBLOCK, 1);
if (ret)
errx(ret,"uORB publications could not be blocked");
} else if (argc == 2 && !strcmp(argv[1], "unblock")) {
/* enable the device publications */
ret = ioctl(fd, DEVIOCSPUBBLOCK, 0);
if (ret)
errx(ret,"uORB publications could not be unblocked");
} else {
errx("no valid command: %s", argv[1]);
}
}
exit(0);
}
static void
do_gyro(int argc, char *argv[])
{
@ -124,7 +166,7 @@ do_gyro(int argc, char *argv[])
if (ret)
errx(ret,"range could not be set");
} else if(argc == 1 && !strcmp(argv[0], "check")) {
} else if (argc == 1 && !strcmp(argv[0], "check")) {
ret = ioctl(fd, GYROIOCSELFTEST, 0);
if (ret) {