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

Merge remote-tracking branch 'upstream/master' into diff_press_filter 

Conflicts:
	src/modules/sdlog2/sdlog2.c
This commit is contained in:
Thomas Gubler 2014-03-15 12:28:29 +01:00
parent 0784ef9189 e590d96c0a
commit 07cff8e0bd
21 changed files with 2902 additions and 725 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

4
ROMFS/px4fmu_common/init.d/10016_3dr_iris
View File

@ -10,11 +10,11 @@ sh /etc/init.d/rc.mc_defaults
if [ $DO_AUTOCONFIG == yes ] if [ $DO_AUTOCONFIG == yes ]
then then
# TODO tune roll/pitch separately # TODO tune roll/pitch separately
param set MC_ROLL_P 9.0 param set MC_ROLL_P 7.0
param set MC_ROLLRATE_P 0.13 param set MC_ROLLRATE_P 0.13
param set MC_ROLLRATE_I 0.0 param set MC_ROLLRATE_I 0.0
param set MC_ROLLRATE_D 0.004 param set MC_ROLLRATE_D 0.004
param set MC_PITCH_P 9.0 param set MC_PITCH_P 7.0
param set MC_PITCHRATE_P 0.13 param set MC_PITCHRATE_P 0.13
param set MC_PITCHRATE_I 0.0 param set MC_PITCHRATE_I 0.0
param set MC_PITCHRATE_D 0.004 param set MC_PITCHRATE_D 0.004

4
ROMFS/px4fmu_common/init.d/4008_ardrone
View File

@ -23,13 +23,15 @@ then
param set MC_PITCHRATE_I 0.0 param set MC_PITCHRATE_I 0.0
param set MC_PITCHRATE_D 0.0 param set MC_PITCHRATE_D 0.0
param set MC_YAW_P 1.0 param set MC_YAW_P 1.0
param set MC_YAW_D 0.1
param set MC_YAWRATE_P 0.15 param set MC_YAWRATE_P 0.15
param set MC_YAWRATE_I 0.0 param set MC_YAWRATE_I 0.0
param set MC_YAWRATE_D 0.0 param set MC_YAWRATE_D 0.0
param set MC_YAW_FF 0.15 param set MC_YAW_FF 0.15
param set BAT_V_SCALING 0.00838095238
fi fi
set OUTPUT_MODE ardrone set OUTPUT_MODE ardrone
set USE_IO no set USE_IO no
set MIXER skip set MIXER skip
# set MAV_TYPE because no specific mixer is set
set MAV_TYPE 2

11
ROMFS/px4fmu_common/init.d/rcS
View File

@ -118,6 +118,7 @@ then
set MKBLCTRL_MODE none set MKBLCTRL_MODE none
set FMU_MODE pwm set FMU_MODE pwm
set MAV_TYPE none set MAV_TYPE none
set LOAD_DEFAULT_APPS yes
# #
# Set DO_AUTOCONFIG flag to use it in AUTOSTART scripts # Set DO_AUTOCONFIG flag to use it in AUTOSTART scripts
@ -465,7 +466,10 @@ then
sh /etc/init.d/rc.interface sh /etc/init.d/rc.interface
# Start standard fixedwing apps # Start standard fixedwing apps
sh /etc/init.d/rc.fw_apps if [ LOAD_DEFAULT_APPS == yes ]
then
sh /etc/init.d/rc.fw_apps
fi
fi fi
# #
@ -521,7 +525,10 @@ then
sh /etc/init.d/rc.interface sh /etc/init.d/rc.interface
# Start standard multicopter apps # Start standard multicopter apps
sh /etc/init.d/rc.mc_apps if [ LOAD_DEFAULT_APPS == yes ]
then
sh /etc/init.d/rc.mc_apps
fi
fi fi
# #

133
Tools/fetch_log.py Normal file
View File

@ -0,0 +1,133 @@
#!/usr/bin/env python
############################################################################
#
# 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
# 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.
#
############################################################################
#
# Log fetcher
#
# Print list of logs:
# python fetch_log.py
#
# Fetch log:
# python fetch_log.py sess001/log001.bin
#
import serial, time, sys, os
def wait_for_string(ser, s, timeout=1.0, debug=False):
t0 = time.time()
buf = []
res = []
n = 0
while (True):
c = ser.read()
if debug:
sys.stderr.write(c)
buf.append(c)
if len(buf) > len(s):
res.append(buf.pop(0))
n += 1
if n % 10000 == 0:
sys.stderr.write(str(n) + "\n")
if "".join(buf) == s:
break
if timeout > 0.0 and time.time() - t0 > timeout:
raise Exception("Timeout while waiting for: " + s)
return "".join(res)
def exec_cmd(ser, cmd, timeout):
ser.write(cmd + "\n")
ser.flush()
wait_for_string(ser, cmd + "\r\n", timeout)
return wait_for_string(ser, "nsh> \x1b[K", timeout)
def ls_dir(ser, dir, timeout=1.0):
res = []
for line in exec_cmd(ser, "ls -l " + dir, timeout).splitlines()[1:]:
res.append((line[20:], int(line[11:19].strip()), line[1] == "d"))
return res
def list_logs(ser):
logs_dir = "/fs/microsd/log"
res = []
for d in ls_dir(ser, logs_dir):
if d[2]:
sess_dir = d[0][:-1]
for f in ls_dir(ser, logs_dir + "/" + sess_dir):
log_file = f[0]
log_size = f[1]
res.append(sess_dir + "/" + log_file + "\t" + str(log_size))
return "\n".join(res)
def fetch_log(ser, fn, timeout):
cmd = "dumpfile " + fn
ser.write(cmd + "\n")
ser.flush()
wait_for_string(ser, cmd + "\r\n", timeout, True)
res = wait_for_string(ser, "\n", timeout, True)
data = []
if res.startswith("OK"):
size = int(res.split()[1])
n = 0
print "Reading data:"
while (n < size):
buf = ser.read(min(size - n, 8192))
data.append(buf)
n += len(buf)
sys.stdout.write(".")
sys.stdout.flush()
print
else:
raise Exception("Error reading log")
wait_for_string(ser, "nsh> \x1b[K", timeout)
return "".join(data)
def main():
dev = "/dev/tty.usbmodem1"
ser = serial.Serial(dev, "115200", timeout=0.2)
if len(sys.argv) < 2:
print list_logs(ser)
else:
log_file = sys.argv[1]
data = fetch_log(ser, "/fs/microsd/log/" + log_file, 1.0)
try:
os.mkdir(log_file.split("/")[0])
except:
pass
fout = open(log_file, "wb")
fout.write(data)
fout.close()
ser.close()
if __name__ == "__main__":
main()

1
makefiles/config_px4fmu-v1_default.mk
View File

@ -56,6 +56,7 @@ MODULES += systemcmds/tests
MODULES += systemcmds/config MODULES += systemcmds/config
MODULES += systemcmds/nshterm MODULES += systemcmds/nshterm
MODULES += systemcmds/hw_ver MODULES += systemcmds/hw_ver
MODULES += systemcmds/dumpfile
# #
# General system control # General system control

3
makefiles/config_px4fmu-v2_default.mk
View File

@ -27,6 +27,7 @@ MODULES += drivers/l3gd20
MODULES += drivers/hmc5883 MODULES += drivers/hmc5883
MODULES += drivers/ms5611 MODULES += drivers/ms5611
MODULES += drivers/mb12xx MODULES += drivers/mb12xx
MODULES += drivers/sf0x
MODULES += drivers/gps MODULES += drivers/gps
MODULES += drivers/hil MODULES += drivers/hil
MODULES += drivers/hott/hott_telemetry MODULES += drivers/hott/hott_telemetry
@ -63,6 +64,7 @@ MODULES += systemcmds/config
MODULES += systemcmds/nshterm MODULES += systemcmds/nshterm
MODULES += systemcmds/mtd MODULES += systemcmds/mtd
MODULES += systemcmds/hw_ver MODULES += systemcmds/hw_ver
MODULES += systemcmds/dumpfile
# #
# General system control # General system control
@ -71,6 +73,7 @@ MODULES += modules/commander
MODULES += modules/navigator MODULES += modules/navigator
MODULES += modules/mavlink MODULES += modules/mavlink
MODULES += modules/mavlink_onboard MODULES += modules/mavlink_onboard
MODULES += modules/gpio_led
# #
# Estimation modules (EKF/ SO3 / other filters) # Estimation modules (EKF/ SO3 / other filters)

84
src/drivers/drv_px4flow.h Normal file
View File

@ -0,0 +1,84 @@
/****************************************************************************
*
* Copyright (C) 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
* 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 Rangefinder driver interface.
*/
#ifndef _DRV_PX4FLOW_H
#define _DRV_PX4FLOW_H
#include <stdint.h>
#include <sys/ioctl.h>
#include "drv_sensor.h"
#include "drv_orb_dev.h"
#define PX4FLOW_DEVICE_PATH "/dev/px4flow"
/**
* Optical flow in NED body frame in SI units.
*
* @see http://en.wikipedia.org/wiki/International_System_of_Units
*/
struct px4flow_report {
uint64_t timestamp; /**< in microseconds since system start */
int16_t flow_raw_x; /**< flow in pixels in X direction, not rotation-compensated */
int16_t flow_raw_y; /**< flow in pixels in Y direction, not rotation-compensated */
float flow_comp_x_m; /**< speed over ground in meters, rotation-compensated */
float flow_comp_y_m; /**< speed over ground in meters, rotation-compensated */
float ground_distance_m; /**< Altitude / distance to ground in meters */
uint8_t quality; /**< Quality of the measurement, 0: bad quality, 255: maximum quality */
uint8_t sensor_id; /**< id of the sensor emitting the flow value */
};
/*
* ObjDev tag for px4flow data.
*/
ORB_DECLARE(optical_flow);
/*
* ioctl() definitions
*
* px4flow drivers also implement the generic sensor driver
* interfaces from drv_sensor.h
*/
#define _PX4FLOWIOCBASE (0x7700)
#define __PX4FLOWIOC(_n) (_IOC(_PX4FLOWIOCBASE, _n))
#endif /* _DRV_PX4FLOW_H */

186
src/drivers/mb12xx/mb12xx.cpp
View File

@ -200,7 +200,7 @@ MB12XX::MB12XX(int bus, int address) :
_buffer_overflows(perf_alloc(PC_COUNT, "mb12xx_buffer_overflows")) _buffer_overflows(perf_alloc(PC_COUNT, "mb12xx_buffer_overflows"))
{ {
// enable debug() calls // enable debug() calls
_debug_enabled = true; _debug_enabled = false;
// work_cancel in the dtor will explode if we don't do this... // work_cancel in the dtor will explode if we don't do this...
memset(&_work, 0, sizeof(_work)); memset(&_work, 0, sizeof(_work));
@ -212,8 +212,9 @@ MB12XX::~MB12XX()
stop(); stop();
/* free any existing reports */ /* free any existing reports */
if (_reports != nullptr) if (_reports != nullptr) {
delete _reports; delete _reports;
}
} }
int int
@ -222,22 +223,25 @@ MB12XX::init()
int ret = ERROR; int ret = ERROR;
/* do I2C init (and probe) first */ /* do I2C init (and probe) first */
if (I2C::init() != OK) if (I2C::init() != OK) {
goto out; goto out;
}
/* allocate basic report buffers */ /* allocate basic report buffers */
_reports = new RingBuffer(2, sizeof(range_finder_report)); _reports = new RingBuffer(2, sizeof(range_finder_report));
if (_reports == nullptr) if (_reports == nullptr) {
goto out; goto out;
}
/* get a publish handle on the range finder topic */ /* get a publish handle on the range finder topic */
struct range_finder_report zero_report; struct range_finder_report zero_report;
memset(&zero_report, 0, sizeof(zero_report)); memset(&zero_report, 0, sizeof(zero_report));
_range_finder_topic = orb_advertise(ORB_ID(sensor_range_finder), &zero_report); _range_finder_topic = orb_advertise(ORB_ID(sensor_range_finder), &zero_report);
if (_range_finder_topic < 0) if (_range_finder_topic < 0) {
debug("failed to create sensor_range_finder object. Did you start uOrb?"); debug("failed to create sensor_range_finder object. Did you start uOrb?");
}
ret = OK; ret = OK;
/* sensor is ok, but we don't really know if it is within range */ /* sensor is ok, but we don't really know if it is within range */
@ -284,20 +288,20 @@ MB12XX::ioctl(struct file *filp, int cmd, unsigned long arg)
case SENSORIOCSPOLLRATE: { case SENSORIOCSPOLLRATE: {
switch (arg) { switch (arg) {
/* switching to manual polling */ /* switching to manual polling */
case SENSOR_POLLRATE_MANUAL: case SENSOR_POLLRATE_MANUAL:
stop(); stop();
_measure_ticks = 0; _measure_ticks = 0;
return OK; return OK;
/* external signalling (DRDY) not supported */ /* external signalling (DRDY) not supported */
case SENSOR_POLLRATE_EXTERNAL: case SENSOR_POLLRATE_EXTERNAL:
/* zero would be bad */ /* zero would be bad */
case 0: case 0:
return -EINVAL; return -EINVAL;
/* set default/max polling rate */ /* set default/max polling rate */
case SENSOR_POLLRATE_MAX: case SENSOR_POLLRATE_MAX:
case SENSOR_POLLRATE_DEFAULT: { case SENSOR_POLLRATE_DEFAULT: {
/* do we need to start internal polling? */ /* do we need to start internal polling? */
@ -307,13 +311,14 @@ MB12XX::ioctl(struct file *filp, int cmd, unsigned long arg)
_measure_ticks = USEC2TICK(MB12XX_CONVERSION_INTERVAL); _measure_ticks = USEC2TICK(MB12XX_CONVERSION_INTERVAL);
/* if we need to start the poll state machine, do it */ /* if we need to start the poll state machine, do it */
if (want_start) if (want_start) {
start(); start();
}
return OK; return OK;
} }
/* adjust to a legal polling interval in Hz */ /* adjust to a legal polling interval in Hz */
default: { default: {
/* do we need to start internal polling? */ /* do we need to start internal polling? */
bool want_start = (_measure_ticks == 0); bool want_start = (_measure_ticks == 0);
@ -322,15 +327,17 @@ MB12XX::ioctl(struct file *filp, int cmd, unsigned long arg)
unsigned ticks = USEC2TICK(1000000 / arg); unsigned ticks = USEC2TICK(1000000 / arg);
/* check against maximum rate */ /* check against maximum rate */
if (ticks < USEC2TICK(MB12XX_CONVERSION_INTERVAL)) if (ticks < USEC2TICK(MB12XX_CONVERSION_INTERVAL)) {
return -EINVAL; return -EINVAL;
}
/* update interval for next measurement */ /* update interval for next measurement */
_measure_ticks = ticks; _measure_ticks = ticks;
/* if we need to start the poll state machine, do it */ /* if we need to start the poll state machine, do it */
if (want_start) if (want_start) {
start(); start();
}
return OK; return OK;
} }
@ -338,25 +345,29 @@ MB12XX::ioctl(struct file *filp, int cmd, unsigned long arg)
} }
case SENSORIOCGPOLLRATE: case SENSORIOCGPOLLRATE:
if (_measure_ticks == 0) if (_measure_ticks == 0) {
return SENSOR_POLLRATE_MANUAL; return SENSOR_POLLRATE_MANUAL;
}
return (1000 / _measure_ticks); return (1000 / _measure_ticks);
case SENSORIOCSQUEUEDEPTH: { case SENSORIOCSQUEUEDEPTH: {
/* lower bound is mandatory, upper bound is a sanity check */ /* lower bound is mandatory, upper bound is a sanity check */
if ((arg < 1) || (arg > 100)) if ((arg < 1) || (arg > 100)) {
return -EINVAL; return -EINVAL;
}
irqstate_t flags = irqsave();
if (!_reports->resize(arg)) {
irqrestore(flags);
return -ENOMEM;
}
irqstate_t flags = irqsave();
if (!_reports->resize(arg)) {
irqrestore(flags); irqrestore(flags);
return -ENOMEM;
}
irqrestore(flags);
return OK; return OK;
} }
case SENSORIOCGQUEUEDEPTH: case SENSORIOCGQUEUEDEPTH:
return _reports->size(); return _reports->size();
@ -365,18 +376,18 @@ MB12XX::ioctl(struct file *filp, int cmd, unsigned long arg)
/* XXX implement this */ /* XXX implement this */
return -EINVAL; return -EINVAL;
case RANGEFINDERIOCSETMINIUMDISTANCE: case RANGEFINDERIOCSETMINIUMDISTANCE: {
{ set_minimum_distance(*(float *)arg);
set_minimum_distance(*(float *)arg); return 0;
return 0; }
} break;
break;
case RANGEFINDERIOCSETMAXIUMDISTANCE: case RANGEFINDERIOCSETMAXIUMDISTANCE: {
{ set_maximum_distance(*(float *)arg);
set_maximum_distance(*(float *)arg); return 0;
return 0; }
} break;
break;
default: default:
/* give it to the superclass */ /* give it to the superclass */
return I2C::ioctl(filp, cmd, arg); return I2C::ioctl(filp, cmd, arg);
@ -391,8 +402,9 @@ MB12XX::read(struct file *filp, char *buffer, size_t buflen)
int ret = 0; int ret = 0;
/* buffer must be large enough */ /* buffer must be large enough */
if (count < 1) if (count < 1) {
return -ENOSPC; return -ENOSPC;
}
/* if automatic measurement is enabled */ /* if automatic measurement is enabled */
if (_measure_ticks > 0) { if (_measure_ticks > 0) {
@ -453,12 +465,12 @@ MB12XX::measure()
uint8_t cmd = MB12XX_TAKE_RANGE_REG; uint8_t cmd = MB12XX_TAKE_RANGE_REG;
ret = transfer(&cmd, 1, nullptr, 0); ret = transfer(&cmd, 1, nullptr, 0);
if (OK != ret) if (OK != ret) {
{
perf_count(_comms_errors); perf_count(_comms_errors);
log("i2c::transfer returned %d", ret); log("i2c::transfer returned %d", ret);
return ret; return ret;
} }
ret = OK; ret = OK;
return ret; return ret;
@ -476,8 +488,7 @@ MB12XX::collect()
ret = transfer(nullptr, 0, &val[0], 2); ret = transfer(nullptr, 0, &val[0], 2);
if (ret < 0) if (ret < 0) {
{
log("error reading from sensor: %d", ret); log("error reading from sensor: %d", ret);
perf_count(_comms_errors); perf_count(_comms_errors);
perf_end(_sample_perf); perf_end(_sample_perf);
@ -485,12 +496,12 @@ MB12XX::collect()
} }
uint16_t distance = val[0] << 8 | val[1]; uint16_t distance = val[0] << 8 | val[1];
float si_units = (distance * 1.0f)/ 100.0f; /* cm to m */ float si_units = (distance * 1.0f) / 100.0f; /* cm to m */
struct range_finder_report report; struct range_finder_report report;
/* this should be fairly close to the end of the measurement, so the best approximation of the time */ /* this should be fairly close to the end of the measurement, so the best approximation of the time */
report.timestamp = hrt_absolute_time(); report.timestamp = hrt_absolute_time();
report.error_count = perf_event_count(_comms_errors); report.error_count = perf_event_count(_comms_errors);
report.distance = si_units; report.distance = si_units;
report.valid = si_units > get_minimum_distance() && si_units < get_maximum_distance() ? 1 : 0; report.valid = si_units > get_minimum_distance() && si_units < get_maximum_distance() ? 1 : 0;
@ -525,11 +536,13 @@ MB12XX::start()
true, true,
true, true,
true, true,
SUBSYSTEM_TYPE_RANGEFINDER}; SUBSYSTEM_TYPE_RANGEFINDER
};
static orb_advert_t pub = -1; static orb_advert_t pub = -1;
if (pub > 0) { if (pub > 0) {
orb_publish(ORB_ID(subsystem_info), pub, &info); orb_publish(ORB_ID(subsystem_info), pub, &info);
} else { } else {
pub = orb_advertise(ORB_ID(subsystem_info), &info); pub = orb_advertise(ORB_ID(subsystem_info), &info);
} }
@ -583,8 +596,9 @@ MB12XX::cycle()
} }
/* measurement phase */ /* measurement phase */
if (OK != measure()) if (OK != measure()) {
log("measure error"); log("measure error");
}
/* next phase is collection */ /* next phase is collection */
_collect_phase = true; _collect_phase = true;
@ -635,33 +649,37 @@ start()
{ {
int fd; int fd;
if (g_dev != nullptr) if (g_dev != nullptr) {
errx(1, "already started"); errx(1, "already started");
}
/* create the driver */ /* create the driver */
g_dev = new MB12XX(MB12XX_BUS); g_dev = new MB12XX(MB12XX_BUS);
if (g_dev == nullptr) if (g_dev == nullptr) {
goto fail; goto fail;
}
if (OK != g_dev->init()) if (OK != g_dev->init()) {
goto fail; goto fail;
}
/* set the poll rate to default, starts automatic data collection */ /* set the poll rate to default, starts automatic data collection */
fd = open(RANGE_FINDER_DEVICE_PATH, O_RDONLY); fd = open(RANGE_FINDER_DEVICE_PATH, O_RDONLY);
if (fd < 0) if (fd < 0) {
goto fail; goto fail;
}
if (ioctl(fd, SENSORIOCSPOLLRATE, SENSOR_POLLRATE_DEFAULT) < 0) if (ioctl(fd, SENSORIOCSPOLLRATE, SENSOR_POLLRATE_DEFAULT) < 0) {
goto fail; goto fail;
}
exit(0); exit(0);
fail: fail:
if (g_dev != nullptr) if (g_dev != nullptr) {
{
delete g_dev; delete g_dev;
g_dev = nullptr; g_dev = nullptr;
} }
@ -674,15 +692,14 @@ fail:
*/ */
void stop() void stop()
{ {
if (g_dev != nullptr) if (g_dev != nullptr) {
{
delete g_dev; delete g_dev;
g_dev = nullptr; g_dev = nullptr;
}
else } else {
{
errx(1, "driver not running"); errx(1, "driver not running");
} }
exit(0); exit(0);
} }
@ -700,22 +717,25 @@ test()
int fd = open(RANGE_FINDER_DEVICE_PATH, O_RDONLY); int fd = open(RANGE_FINDER_DEVICE_PATH, O_RDONLY);
if (fd < 0) if (fd < 0) {
err(1, "%s open failed (try 'mb12xx start' if the driver is not running", RANGE_FINDER_DEVICE_PATH); err(1, "%s open failed (try 'mb12xx start' if the driver is not running", RANGE_FINDER_DEVICE_PATH);
}
/* do a simple demand read */ /* do a simple demand read */
sz = read(fd, &report, sizeof(report)); sz = read(fd, &report, sizeof(report));
if (sz != sizeof(report)) if (sz != sizeof(report)) {
err(1, "immediate read failed"); err(1, "immediate read failed");
}
warnx("single read"); warnx("single read");
warnx("measurement: %0.2f m", (double)report.distance); warnx("measurement: %0.2f m", (double)report.distance);
warnx("time: %lld", report.timestamp); warnx("time: %lld", report.timestamp);
/* start the sensor polling at 2Hz */ /* start the sensor polling at 2Hz */
if (OK != ioctl(fd, SENSORIOCSPOLLRATE, 2)) if (OK != ioctl(fd, SENSORIOCSPOLLRATE, 2)) {
errx(1, "failed to set 2Hz poll rate"); errx(1, "failed to set 2Hz poll rate");
}
/* read the sensor 5x and report each value */ /* read the sensor 5x and report each value */
for (unsigned i = 0; i < 5; i++) { for (unsigned i = 0; i < 5; i++) {
@ -726,20 +746,27 @@ test()
fds.events = POLLIN; fds.events = POLLIN;
ret = poll(&fds, 1, 2000); ret = poll(&fds, 1, 2000);
if (ret != 1) if (ret != 1) {
errx(1, "timed out waiting for sensor data"); errx(1, "timed out waiting for sensor data");
}
/* now go get it */ /* now go get it */
sz = read(fd, &report, sizeof(report)); sz = read(fd, &report, sizeof(report));
if (sz != sizeof(report)) if (sz != sizeof(report)) {
err(1, "periodic read failed"); err(1, "periodic read failed");
}
warnx("periodic read %u", i); warnx("periodic read %u", i);
warnx("measurement: %0.3f", (double)report.distance); warnx("measurement: %0.3f", (double)report.distance);
warnx("time: %lld", report.timestamp); warnx("time: %lld", report.timestamp);
} }
/* reset the sensor polling to default rate */
if (OK != ioctl(fd, SENSORIOCSPOLLRATE, SENSOR_POLLRATE_DEFAULT)) {
errx(1, "failed to set default poll rate");
}
errx(0, "PASS"); errx(0, "PASS");
} }
@ -751,14 +778,17 @@ reset()
{ {
int fd = open(RANGE_FINDER_DEVICE_PATH, O_RDONLY); int fd = open(RANGE_FINDER_DEVICE_PATH, O_RDONLY);
if (fd < 0) if (fd < 0) {
err(1, "failed "); err(1, "failed ");
}
if (ioctl(fd, SENSORIOCRESET, 0) < 0) if (ioctl(fd, SENSORIOCRESET, 0) < 0) {
err(1, "driver reset failed"); err(1, "driver reset failed");
}
if (ioctl(fd, SENSORIOCSPOLLRATE, SENSOR_POLLRATE_DEFAULT) < 0) if (ioctl(fd, SENSORIOCSPOLLRATE, SENSOR_POLLRATE_DEFAULT) < 0) {
err(1, "driver poll restart failed"); err(1, "driver poll restart failed");
}
exit(0); exit(0);
} }
@ -769,8 +799,9 @@ reset()
void void
info() info()
{ {
if (g_dev == nullptr) if (g_dev == nullptr) {
errx(1, "driver not running"); errx(1, "driver not running");
}
printf("state @ %p\n", g_dev); printf("state @ %p\n", g_dev);
g_dev->print_info(); g_dev->print_info();
@ -786,32 +817,37 @@ mb12xx_main(int argc, char *argv[])
/* /*
* Start/load the driver. * Start/load the driver.
*/ */
if (!strcmp(argv[1], "start")) if (!strcmp(argv[1], "start")) {
mb12xx::start(); mb12xx::start();
}
/* /*
* Stop the driver * Stop the driver
*/ */
if (!strcmp(argv[1], "stop")) if (!strcmp(argv[1], "stop")) {
mb12xx::stop(); mb12xx::stop();
}
/* /*
* Test the driver/device. * Test the driver/device.
*/ */
if (!strcmp(argv[1], "test")) if (!strcmp(argv[1], "test")) {
mb12xx::test(); mb12xx::test();
}
/* /*
* Reset the driver. * Reset the driver.
*/ */
if (!strcmp(argv[1], "reset")) if (!strcmp(argv[1], "reset")) {
mb12xx::reset(); mb12xx::reset();
}
/* /*
* Print driver information. * Print driver information.
*/ */
if (!strcmp(argv[1], "info") || !strcmp(argv[1], "status")) if (!strcmp(argv[1], "info") || !strcmp(argv[1], "status")) {
mb12xx::info(); mb12xx::info();
}
errx(1, "unrecognized command, try 'start', 'test', 'reset' or 'info'"); errx(1, "unrecognized command, try 'start', 'test', 'reset' or 'info'");
} }

40
src/drivers/px4flow/module.mk Normal file
View File

@ -0,0 +1,40 @@
############################################################################
#
# Copyright (c) 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
# 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.
#
############################################################################
#
# Makefile to build the PX4FLOW driver.
#
MODULE_COMMAND = px4flow
SRCS = px4flow.cpp

806
src/drivers/px4flow/px4flow.cpp Normal file
View File

@ -0,0 +1,806 @@
/****************************************************************************
*
* Copyright (c) 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
* 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 px4flow.cpp
* @author Dominik Honegger
*
* Driver for the PX4FLOW module connected via I2C.
*/
#include <nuttx/config.h>
#include <drivers/device/i2c.h>
#include <sys/types.h>
#include <stdint.h>
#include <stdlib.h>
#include <stdbool.h>
#include <semaphore.h>
#include <string.h>
#include <fcntl.h>
#include <poll.h>
#include <errno.h>
#include <stdio.h>
#include <math.h>
#include <unistd.h>
#include <nuttx/arch.h>
#include <nuttx/wqueue.h>
#include <nuttx/clock.h>
#include <systemlib/perf_counter.h>
#include <systemlib/err.h>
#include <drivers/drv_hrt.h>
#include <drivers/drv_px4flow.h>
#include <drivers/device/ringbuffer.h>
#include <uORB/uORB.h>
#include <uORB/topics/subsystem_info.h>
//#include <uORB/topics/optical_flow.h>
#include <board_config.h>
/* Configuration Constants */
#define PX4FLOW_BUS PX4_I2C_BUS_EXPANSION
#define I2C_FLOW_ADDRESS 0x45 //* 7-bit address. 8-bit address is 0x8A
//range 0x42 - 0x49
/* PX4FLOW Registers addresses */
#define PX4FLOW_REG 0x00 /* Measure Register */
#define PX4FLOW_CONVERSION_INTERVAL 8000 /* 8ms 125Hz
/* oddly, ERROR is not defined for c++ */
#ifdef ERROR
# undef ERROR
#endif
static const int ERROR = -1;
#ifndef CONFIG_SCHED_WORKQUEUE
# error This requires CONFIG_SCHED_WORKQUEUE.
#endif
//struct i2c_frame
//{
// uint16_t frame_count;
// int16_t pixel_flow_x_sum;
// int16_t pixel_flow_y_sum;
// int16_t flow_comp_m_x;
// int16_t flow_comp_m_y;
// int16_t qual;
// int16_t gyro_x_rate;
// int16_t gyro_y_rate;
// int16_t gyro_z_rate;
// uint8_t gyro_range;
// uint8_t sonar_timestamp;
// int16_t ground_distance;
//};
//
//struct i2c_frame f;
class PX4FLOW : public device::I2C
{
public:
PX4FLOW(int bus = PX4FLOW_BUS, int address = I2C_FLOW_ADDRESS);
virtual ~PX4FLOW();
virtual int init();
virtual ssize_t read(struct file *filp, char *buffer, size_t buflen);
virtual int ioctl(struct file *filp, int cmd, unsigned long arg);
/**
* Diagnostics - print some basic information about the driver.
*/
void print_info();
protected:
virtual int probe();
private:
work_s _work;
RingBuffer *_reports;
bool _sensor_ok;
int _measure_ticks;
bool _collect_phase;
orb_advert_t _px4flow_topic;
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
* specific address.
*
* @param address The I2C bus address to probe.
* @return True if the device is present.
*/
int probe_address(uint8_t address);
/**
* Initialise the automatic measurement state machine and start it.
*
* @note This function is called at open and error time. It might make sense
* to make it more aggressive about resetting the bus in case of errors.
*/
void start();
/**
* Stop the automatic measurement state machine.
*/
void stop();
/**
* Perform a poll cycle; collect from the previous measurement
* and start a new one.
*/
void cycle();
int measure();
int collect();
/**
* Static trampoline from the workq context; because we don't have a
* generic workq wrapper yet.
*
* @param arg Instance pointer for the driver that is polling.
*/
static void cycle_trampoline(void *arg);
};
/*
* Driver 'main' command.
*/
extern "C" __EXPORT int px4flow_main(int argc, char *argv[]);
PX4FLOW::PX4FLOW(int bus, int address) :
I2C("PX4FLOW", PX4FLOW_DEVICE_PATH, bus, address, 400000),//400khz
_reports(nullptr),
_sensor_ok(false),
_measure_ticks(0),
_collect_phase(false),
_px4flow_topic(-1),
_sample_perf(perf_alloc(PC_ELAPSED, "px4flow_read")),
_comms_errors(perf_alloc(PC_COUNT, "px4flow_comms_errors")),
_buffer_overflows(perf_alloc(PC_COUNT, "px4flow_buffer_overflows"))
{
// enable debug() calls
_debug_enabled = true;
// work_cancel in the dtor will explode if we don't do this...
memset(&_work, 0, sizeof(_work));
}
PX4FLOW::~PX4FLOW()
{
/* make sure we are truly inactive */
stop();
/* free any existing reports */
if (_reports != nullptr)
delete _reports;
}
int
PX4FLOW::init()
{
int ret = ERROR;
/* do I2C init (and probe) first */
if (I2C::init() != OK)
goto out;
/* allocate basic report buffers */
_reports = new RingBuffer(2, sizeof(px4flow_report));
if (_reports == nullptr)
goto out;
/* get a publish handle on the px4flow topic */
struct px4flow_report zero_report;
memset(&zero_report, 0, sizeof(zero_report));
_px4flow_topic = orb_advertise(ORB_ID(optical_flow), &zero_report);
if (_px4flow_topic < 0)
debug("failed to create px4flow object. Did you start uOrb?");
ret = OK;
/* sensor is ok, but we don't really know if it is within range */
_sensor_ok = true;
out:
return ret;
}
int
PX4FLOW::probe()
{
return measure();
}
int
PX4FLOW::ioctl(struct file *filp, int cmd, unsigned long arg)
{
switch (cmd) {
case SENSORIOCSPOLLRATE: {
switch (arg) {
/* switching to manual polling */
case SENSOR_POLLRATE_MANUAL:
stop();
_measure_ticks = 0;
return OK;
/* external signalling (DRDY) not supported */
case SENSOR_POLLRATE_EXTERNAL:
/* zero would be bad */
case 0:
return -EINVAL;
/* set default/max polling rate */
case SENSOR_POLLRATE_MAX:
case SENSOR_POLLRATE_DEFAULT: {
/* do we need to start internal polling? */
bool want_start = (_measure_ticks == 0);
/* set interval for next measurement to minimum legal value */
_measure_ticks = USEC2TICK(PX4FLOW_CONVERSION_INTERVAL);
/* if we need to start the poll state machine, do it */
if (want_start)
start();
return OK;
}
/* adjust to a legal polling interval in Hz */
default: {
/* do we need to start internal polling? */
bool want_start = (_measure_ticks == 0);
/* convert hz to tick interval via microseconds */
unsigned ticks = USEC2TICK(1000000 / arg);
/* check against maximum rate */
if (ticks < USEC2TICK(PX4FLOW_CONVERSION_INTERVAL))
return -EINVAL;
/* update interval for next measurement */
_measure_ticks = ticks;
/* if we need to start the poll state machine, do it */
if (want_start)
start();
return OK;
}
}
}
case SENSORIOCGPOLLRATE:
if (_measure_ticks == 0)
return SENSOR_POLLRATE_MANUAL;
return (1000 / _measure_ticks);
case SENSORIOCSQUEUEDEPTH: {
/* lower bound is mandatory, upper bound is a sanity check */
if ((arg < 1) || (arg > 100))
return -EINVAL;
irqstate_t flags = irqsave();
if (!_reports->resize(arg)) {
irqrestore(flags);
return -ENOMEM;
}
irqrestore(flags);
return OK;
}
case SENSORIOCGQUEUEDEPTH:
return _reports->size();
case SENSORIOCRESET:
/* XXX implement this */
return -EINVAL;
default:
/* give it to the superclass */
return I2C::ioctl(filp, cmd, arg);
}
}
ssize_t
PX4FLOW::read(struct file *filp, char *buffer, size_t buflen)
{
unsigned count = buflen / sizeof(struct px4flow_report);
struct px4flow_report *rbuf = reinterpret_cast<struct px4flow_report *>(buffer);
int ret = 0;
/* buffer must be large enough */
if (count < 1)
return -ENOSPC;
/* if automatic measurement is enabled */
if (_measure_ticks > 0) {
/*
* While there is space in the caller's buffer, and reports, copy them.
* Note that we may be pre-empted by the workq thread while we are doing this;
* we are careful to avoid racing with them.
*/
while (count--) {
if (_reports->get(rbuf)) {
ret += sizeof(*rbuf);
rbuf++;
}
}
/* if there was no data, warn the caller */
return ret ? ret : -EAGAIN;
}
/* manual measurement - run one conversion */
do {
_reports->flush();
/* trigger a measurement */
if (OK != measure()) {
ret = -EIO;
break;
}
/* wait for it to complete */
usleep(PX4FLOW_CONVERSION_INTERVAL);
/* run the collection phase */
if (OK != collect()) {
ret = -EIO;
break;
}
/* state machine will have generated a report, copy it out */
if (_reports->get(rbuf)) {
ret = sizeof(*rbuf);
}
} while (0);
return ret;
}
int
PX4FLOW::measure()
{
int ret;
/*
* Send the command to begin a measurement.
*/
uint8_t cmd = PX4FLOW_REG;
ret = transfer(&cmd, 1, nullptr, 0);
if (OK != ret)
{
perf_count(_comms_errors);
log("i2c::transfer returned %d", ret);
printf("i2c::transfer flow returned %d");
return ret;
}
ret = OK;
return ret;
}
int
PX4FLOW::collect()
{
int ret = -EIO;
/* read from the sensor */
uint8_t val[22] = {0, 0,0, 0,0, 0,0, 0,0, 0,0, 0,0, 0,0, 0,0, 0,0, 0,0, 0};
perf_begin(_sample_perf);
ret = transfer(nullptr, 0, &val[0], 22);
if (ret < 0)
{
log("error reading from sensor: %d", ret);
perf_count(_comms_errors);
perf_end(_sample_perf);
return ret;
}
// f.frame_count = val[1] << 8 | val[0];
// f.pixel_flow_x_sum= val[3] << 8 | val[2];
// f.pixel_flow_y_sum= val[5] << 8 | val[4];
// f.flow_comp_m_x= val[7] << 8 | val[6];
// f.flow_comp_m_y= val[9] << 8 | val[8];
// f.qual= val[11] << 8 | val[10];
// f.gyro_x_rate= val[13] << 8 | val[12];
// f.gyro_y_rate= val[15] << 8 | val[14];
// f.gyro_z_rate= val[17] << 8 | val[16];
// f.gyro_range= val[18];
// f.sonar_timestamp= val[19];
// f.ground_distance= val[21] << 8 | val[20];
int16_t flowcx = val[7] << 8 | val[6];
int16_t flowcy = val[9] << 8 | val[8];
int16_t gdist = val[21] << 8 | val[20];
struct px4flow_report report;
report.flow_comp_x_m = float(flowcx)/1000.0f;
report.flow_comp_y_m = float(flowcy)/1000.0f;
report.flow_raw_x= val[3] << 8 | val[2];
report.flow_raw_y= val[5] << 8 | val[4];
report.ground_distance_m =float(gdist)/1000.0f;
report.quality= val[10];
report.sensor_id = 0;
report.timestamp = hrt_absolute_time();
/* publish it */
orb_publish(ORB_ID(optical_flow), _px4flow_topic, &report);
/* post a report to the ring */
if (_reports->force(&report)) {
perf_count(_buffer_overflows);
}
/* notify anyone waiting for data */
poll_notify(POLLIN);
ret = OK;
perf_end(_sample_perf);
return ret;
}
void
PX4FLOW::start()
{
/* reset the report ring and state machine */
_collect_phase = false;
_reports->flush();
/* schedule a cycle to start things */
work_queue(HPWORK, &_work, (worker_t)&PX4FLOW::cycle_trampoline, this, 1);
/* notify about state change */
struct subsystem_info_s info = {
true,
true,
true,
SUBSYSTEM_TYPE_OPTICALFLOW};
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);
}
}
void
PX4FLOW::stop()
{
work_cancel(HPWORK, &_work);
}
void
PX4FLOW::cycle_trampoline(void *arg)
{
PX4FLOW *dev = (PX4FLOW *)arg;
dev->cycle();
}
void
PX4FLOW::cycle()
{
/* collection phase? */
if (_collect_phase) {
/* perform collection */
if (OK != collect()) {
log("collection error");
/* restart the measurement state machine */
start();
return;
}
/* next phase is measurement */
_collect_phase = false;
/*
* Is there a collect->measure gap?
*/
if (_measure_ticks > USEC2TICK(PX4FLOW_CONVERSION_INTERVAL)) {
/* schedule a fresh cycle call when we are ready to measure again */
work_queue(HPWORK,
&_work,
(worker_t)&PX4FLOW::cycle_trampoline,
this,
_measure_ticks - USEC2TICK(PX4FLOW_CONVERSION_INTERVAL));
return;
}
}
/* measurement phase */
if (OK != measure())
log("measure error");
/* next phase is collection */
_collect_phase = true;
/* schedule a fresh cycle call when the measurement is done */
work_queue(HPWORK,
&_work,
(worker_t)&PX4FLOW::cycle_trampoline,
this,
USEC2TICK(PX4FLOW_CONVERSION_INTERVAL));
}
void
PX4FLOW::print_info()
{
perf_print_counter(_sample_perf);
perf_print_counter(_comms_errors);
perf_print_counter(_buffer_overflows);
printf("poll interval: %u ticks\n", _measure_ticks);
_reports->print_info("report queue");
}
/**
* Local functions in support of the shell command.
*/
namespace px4flow
{
/* oddly, ERROR is not defined for c++ */
#ifdef ERROR
# undef ERROR
#endif
const int ERROR = -1;
PX4FLOW *g_dev;
void start();
void stop();
void test();
void reset();
void info();
/**
* Start the driver.
*/
void
start()
{
int fd;
if (g_dev != nullptr)
errx(1, "already started");
/* create the driver */
g_dev = new PX4FLOW(PX4FLOW_BUS);
if (g_dev == nullptr)
goto fail;
if (OK != g_dev->init())
goto fail;
/* set the poll rate to default, starts automatic data collection */
fd = open(PX4FLOW_DEVICE_PATH, O_RDONLY);
if (fd < 0)
goto fail;
if (ioctl(fd, SENSORIOCSPOLLRATE, SENSOR_POLLRATE_MAX) < 0)
goto fail;
exit(0);
fail:
if (g_dev != nullptr)
{
delete g_dev;
g_dev = nullptr;
}
errx(1, "driver start failed");
}
/**
* Stop the driver
*/
void stop()
{
if (g_dev != nullptr)
{
delete g_dev;
g_dev = nullptr;
}
else
{
errx(1, "driver not running");
}
exit(0);
}
/**
* Perform some basic functional tests on the driver;
* make sure we can collect data from the sensor in polled
* and automatic modes.
*/
void
test()
{
struct px4flow_report report;
ssize_t sz;
int ret;
int fd = open(PX4FLOW_DEVICE_PATH, O_RDONLY);
if (fd < 0)
err(1, "%s open failed (try 'px4flow start' if the driver is not running", PX4FLOW_DEVICE_PATH);
/* do a simple demand read */
sz = read(fd, &report, sizeof(report));
if (sz != sizeof(report))
// err(1, "immediate read failed");
warnx("single read");
warnx("flowx: %0.2f m/s", (double)report.flow_comp_x_m);
warnx("flowy: %0.2f m/s", (double)report.flow_comp_y_m);
warnx("time: %lld", report.timestamp);
/* start the sensor polling at 2Hz */
if (OK != ioctl(fd, SENSORIOCSPOLLRATE, 2))
errx(1, "failed to set 2Hz poll rate");
/* read the sensor 5x and report each value */
for (unsigned i = 0; i < 5; i++) {
struct pollfd fds;
/* wait for data to be ready */
fds.fd = fd;
fds.events = POLLIN;
ret = poll(&fds, 1, 2000);
if (ret != 1)
errx(1, "timed out waiting for sensor data");
/* now go get it */
sz = read(fd, &report, sizeof(report));
if (sz != sizeof(report))
err(1, "periodic read failed");
warnx("periodic read %u", i);
warnx("flowx: %0.2f m/s", (double)report.flow_comp_x_m);
warnx("flowy: %0.2f m/s", (double)report.flow_comp_y_m);
warnx("time: %lld", report.timestamp);
}
errx(0, "PASS");
}
/**
* Reset the driver.
*/
void
reset()
{
int fd = open(PX4FLOW_DEVICE_PATH, O_RDONLY);
if (fd < 0)
err(1, "failed ");
if (ioctl(fd, SENSORIOCRESET, 0) < 0)
err(1, "driver reset failed");
if (ioctl(fd, SENSORIOCSPOLLRATE, SENSOR_POLLRATE_DEFAULT) < 0)
err(1, "driver poll restart failed");
exit(0);
}
/**
* Print a little info about the driver.
*/
void
info()
{
if (g_dev == nullptr)
errx(1, "driver not running");
printf("state @ %p\n", g_dev);
g_dev->print_info();
exit(0);
}
} // namespace
int
px4flow_main(int argc, char *argv[])
{
/*
* Start/load the driver.
*/
if (!strcmp(argv[1], "start"))
px4flow::start();
/*
* Stop the driver
*/
if (!strcmp(argv[1], "stop"))
px4flow::stop();
/*
* Test the driver/device.
*/
if (!strcmp(argv[1], "test"))
px4flow::test();
/*
* Reset the driver.
*/
if (!strcmp(argv[1], "reset"))
px4flow::reset();
/*
* Print driver information.
*/
if (!strcmp(argv[1], "info") || !strcmp(argv[1], "status"))
px4flow::info();
errx(1, "unrecognized command, try 'start', 'test', 'reset' or 'info'");
}

10
src/drivers/px4io/px4io.cpp
View File

@ -1921,12 +1921,14 @@ PX4IO::print_status()
io_reg_get(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_PWM_ALTRATE)); io_reg_get(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_PWM_ALTRATE));
#endif #endif
printf("debuglevel %u\n", io_reg_get(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_SET_DEBUG)); printf("debuglevel %u\n", io_reg_get(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_SET_DEBUG));
printf("controls"); for (unsigned group = 0; group < 4; group++) {
printf("controls %u:", group);
for (unsigned i = 0; i < _max_controls; i++) for (unsigned i = 0; i < _max_controls; i++)
printf(" %u", io_reg_get(PX4IO_PAGE_CONTROLS, i)); printf(" %d", (int16_t) io_reg_get(PX4IO_PAGE_CONTROLS, group * PX4IO_PROTOCOL_MAX_CONTROL_COUNT + i));
printf("\n"); printf("\n");
}
for (unsigned i = 0; i < _max_rc_input; i++) { for (unsigned i = 0; i < _max_rc_input; i++) {
unsigned base = PX4IO_P_RC_CONFIG_STRIDE * i; unsigned base = PX4IO_P_RC_CONFIG_STRIDE * i;

40
src/drivers/sf0x/module.mk Normal file
View File

@ -0,0 +1,40 @@
############################################################################
#
# 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.
#
############################################################################
#
# Makefile to build the Lightware laser range finder driver.
#
MODULE_COMMAND = sf0x
SRCS = sf0x.cpp

977
src/drivers/sf0x/sf0x.cpp Normal file
View File

@ -0,0 +1,977 @@
/****************************************************************************
*
* 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 sf0x.cpp
* @author Lorenz Meier <lm@inf.ethz.ch>
* @author Greg Hulands
*
* Driver for the Lightware SF0x laser rangefinder series
*/
#include <nuttx/config.h>
#include <sys/types.h>
#include <stdint.h>
#include <stdlib.h>
#include <stdbool.h>
#include <semaphore.h>
#include <string.h>
#include <fcntl.h>
#include <poll.h>
#include <errno.h>
#include <stdio.h>
#include <math.h>
#include <unistd.h>
#include <termios.h>
#include <nuttx/arch.h>
#include <nuttx/wqueue.h>
#include <nuttx/clock.h>
#include <systemlib/perf_counter.h>
#include <systemlib/err.h>
#include <drivers/drv_hrt.h>
#include <drivers/drv_range_finder.h>
#include <drivers/device/device.h>
#include <drivers/device/ringbuffer.h>
#include <uORB/uORB.h>
#include <uORB/topics/subsystem_info.h>
#include <board_config.h>
/* Configuration Constants */
/* oddly, ERROR is not defined for c++ */
#ifdef ERROR
# undef ERROR
#endif
static const int ERROR = -1;
#ifndef CONFIG_SCHED_WORKQUEUE
# error This requires CONFIG_SCHED_WORKQUEUE.
#endif
#define SF0X_CONVERSION_INTERVAL 83334
#define SF0X_TAKE_RANGE_REG 'd'
#define SF02F_MIN_DISTANCE 0.0f
#define SF02F_MAX_DISTANCE 40.0f
#define SF0X_DEFAULT_PORT "/dev/ttyS2"
class SF0X : public device::CDev
{
public:
SF0X(const char *port = SF0X_DEFAULT_PORT);
virtual ~SF0X();
virtual int init();
virtual ssize_t read(struct file *filp, char *buffer, size_t buflen);
virtual int ioctl(struct file *filp, int cmd, unsigned long arg);
/**
* Diagnostics - print some basic information about the driver.
*/
void print_info();
protected:
virtual int probe();
private:
float _min_distance;
float _max_distance;
work_s _work;
RingBuffer *_reports;
bool _sensor_ok;
int _measure_ticks;
bool _collect_phase;
int _fd;
char _linebuf[10];
unsigned _linebuf_index;
hrt_abstime _last_read;
orb_advert_t _range_finder_topic;
perf_counter_t _sample_perf;
perf_counter_t _comms_errors;
perf_counter_t _buffer_overflows;
/**
* Initialise the automatic measurement state machine and start it.
*
* @note This function is called at open and error time. It might make sense
* to make it more aggressive about resetting the bus in case of errors.
*/
void start();
/**
* Stop the automatic measurement state machine.
*/
void stop();
/**
* Set the min and max distance thresholds if you want the end points of the sensors
* range to be brought in at all, otherwise it will use the defaults SF0X_MIN_DISTANCE
* and SF0X_MAX_DISTANCE
*/
void set_minimum_distance(float min);
void set_maximum_distance(float max);
float get_minimum_distance();
float get_maximum_distance();
/**
* Perform a poll cycle; collect from the previous measurement
* and start a new one.
*/
void cycle();
int measure();
int collect();
/**
* Static trampoline from the workq context; because we don't have a
* generic workq wrapper yet.
*
* @param arg Instance pointer for the driver that is polling.
*/
static void cycle_trampoline(void *arg);
};
/*
* Driver 'main' command.
*/
extern "C" __EXPORT int sf0x_main(int argc, char *argv[]);
SF0X::SF0X(const char *port) :
CDev("SF0X", RANGE_FINDER_DEVICE_PATH),
_min_distance(SF02F_MIN_DISTANCE),
_max_distance(SF02F_MAX_DISTANCE),
_reports(nullptr),
_sensor_ok(false),
_measure_ticks(0),
_collect_phase(false),
_fd(-1),
_linebuf_index(0),
_last_read(0),
_range_finder_topic(-1),
_sample_perf(perf_alloc(PC_ELAPSED, "sf0x_read")),
_comms_errors(perf_alloc(PC_COUNT, "sf0x_comms_errors")),
_buffer_overflows(perf_alloc(PC_COUNT, "sf0x_buffer_overflows"))
{
/* open fd */
_fd = ::open(port, O_RDWR | O_NOCTTY | O_NONBLOCK);
if (_fd < 0) {
warnx("FAIL: laser fd");
}
/* tell it to stop auto-triggering */
char stop_auto = ' ';
(void)::write(_fd, &stop_auto, 1);
usleep(100);
(void)::write(_fd, &stop_auto, 1);
struct termios uart_config;
int termios_state;
/* fill the struct for the new configuration */
tcgetattr(_fd, &uart_config);
/* clear ONLCR flag (which appends a CR for every LF) */
uart_config.c_oflag &= ~ONLCR;
/* no parity, one stop bit */
uart_config.c_cflag &= ~(CSTOPB | PARENB);
unsigned speed = B9600;
/* set baud rate */
if ((termios_state = cfsetispeed(&uart_config, speed)) < 0) {
warnx("ERR CFG: %d ISPD", termios_state);
}
if ((termios_state = cfsetospeed(&uart_config, speed)) < 0) {
warnx("ERR CFG: %d OSPD\n", termios_state);
}
if ((termios_state = tcsetattr(_fd, TCSANOW, &uart_config)) < 0) {
warnx("ERR baud %d ATTR", termios_state);
}
// disable debug() calls
_debug_enabled = false;
// work_cancel in the dtor will explode if we don't do this...
memset(&_work, 0, sizeof(_work));
}
SF0X::~SF0X()
{
/* make sure we are truly inactive */
stop();
/* free any existing reports */
if (_reports != nullptr) {
delete _reports;
}
}
int
SF0X::init()
{
int ret = ERROR;
unsigned i = 0;
/* do regular cdev init */
if (CDev::init() != OK) {
goto out;
}
/* allocate basic report buffers */
_reports = new RingBuffer(2, sizeof(range_finder_report));
if (_reports == nullptr) {
warnx("mem err");
goto out;
}
/* get a publish handle on the range finder topic */
struct range_finder_report zero_report;
memset(&zero_report, 0, sizeof(zero_report));
_range_finder_topic = orb_advertise(ORB_ID(sensor_range_finder), &zero_report);
if (_range_finder_topic < 0) {
warnx("advert err");
}
/* attempt to get a measurement 5 times */
while (ret != OK && i < 5) {
if (measure()) {
ret = ERROR;
_sensor_ok = false;
}
usleep(100000);
if (collect()) {
ret = ERROR;
_sensor_ok = false;
} else {
ret = OK;
/* sensor is ok, but we don't really know if it is within range */
_sensor_ok = true;
}
i++;
}
/* close the fd */
::close(_fd);
_fd = -1;
out:
return ret;
}
int
SF0X::probe()
{
return measure();
}
void
SF0X::set_minimum_distance(float min)
{
_min_distance = min;
}
void
SF0X::set_maximum_distance(float max)
{
_max_distance = max;
}
float
SF0X::get_minimum_distance()
{
return _min_distance;
}
float
SF0X::get_maximum_distance()
{
return _max_distance;
}
int
SF0X::ioctl(struct file *filp, int cmd, unsigned long arg)
{
switch (cmd) {
case SENSORIOCSPOLLRATE: {
switch (arg) {
/* switching to manual polling */
case SENSOR_POLLRATE_MANUAL:
stop();
_measure_ticks = 0;
return OK;
/* external signalling (DRDY) not supported */
case SENSOR_POLLRATE_EXTERNAL:
/* zero would be bad */
case 0:
return -EINVAL;
/* set default/max polling rate */
case SENSOR_POLLRATE_MAX:
case SENSOR_POLLRATE_DEFAULT: {
/* do we need to start internal polling? */
bool want_start = (_measure_ticks == 0);
/* set interval for next measurement to minimum legal value */
_measure_ticks = USEC2TICK(SF0X_CONVERSION_INTERVAL);
/* if we need to start the poll state machine, do it */
if (want_start) {
start();
}
return OK;
}
/* adjust to a legal polling interval in Hz */
default: {
/* do we need to start internal polling? */
bool want_start = (_measure_ticks == 0);
/* convert hz to tick interval via microseconds */
unsigned ticks = USEC2TICK(1000000 / arg);
/* check against maximum rate */
if (ticks < USEC2TICK(SF0X_CONVERSION_INTERVAL)) {
return -EINVAL;
}
/* update interval for next measurement */
_measure_ticks = ticks;
/* if we need to start the poll state machine, do it */
if (want_start) {
start();
}
return OK;
}
}
}
case SENSORIOCGPOLLRATE:
if (_measure_ticks == 0) {
return SENSOR_POLLRATE_MANUAL;
}
return (1000 / _measure_ticks);
case SENSORIOCSQUEUEDEPTH: {
/* lower bound is mandatory, upper bound is a sanity check */
if ((arg < 1) || (arg > 100)) {
return -EINVAL;
}
irqstate_t flags = irqsave();
if (!_reports->resize(arg)) {
irqrestore(flags);
return -ENOMEM;
}
irqrestore(flags);
return OK;
}
case SENSORIOCGQUEUEDEPTH:
return _reports->size();
case SENSORIOCRESET:
/* XXX implement this */
return -EINVAL;
case RANGEFINDERIOCSETMINIUMDISTANCE: {
set_minimum_distance(*(float *)arg);
return 0;
}
break;
case RANGEFINDERIOCSETMAXIUMDISTANCE: {
set_maximum_distance(*(float *)arg);
return 0;
}
break;
default:
/* give it to the superclass */
return CDev::ioctl(filp, cmd, arg);
}
}
ssize_t
SF0X::read(struct file *filp, char *buffer, size_t buflen)
{
unsigned count = buflen / sizeof(struct range_finder_report);
struct range_finder_report *rbuf = reinterpret_cast<struct range_finder_report *>(buffer);
int ret = 0;
/* buffer must be large enough */
if (count < 1) {
return -ENOSPC;
}
/* if automatic measurement is enabled */
if (_measure_ticks > 0) {
/*
* While there is space in the caller's buffer, and reports, copy them.
* Note that we may be pre-empted by the workq thread while we are doing this;
* we are careful to avoid racing with them.
*/
while (count--) {
if (_reports->get(rbuf)) {
ret += sizeof(*rbuf);
rbuf++;
}
}
/* if there was no data, warn the caller */
return ret ? ret : -EAGAIN;
}
/* manual measurement - run one conversion */
do {
_reports->flush();
/* trigger a measurement */
if (OK != measure()) {
ret = -EIO;
break;
}
/* wait for it to complete */
usleep(SF0X_CONVERSION_INTERVAL);
/* run the collection phase */
if (OK != collect()) {
ret = -EIO;
break;
}
/* state machine will have generated a report, copy it out */
if (_reports->get(rbuf)) {
ret = sizeof(*rbuf);
}
} while (0);
return ret;
}
int
SF0X::measure()
{
int ret;
/*
* Send the command to begin a measurement.
*/
char cmd = SF0X_TAKE_RANGE_REG;
ret = ::write(_fd, &cmd, 1);
if (ret != sizeof(cmd)) {
perf_count(_comms_errors);
log("write fail %d", ret);
return ret;
}
ret = OK;
return ret;
}
int
SF0X::collect()
{
int ret;
perf_begin(_sample_perf);
/* clear buffer if last read was too long ago */
uint64_t read_elapsed = hrt_elapsed_time(&_last_read);
if (read_elapsed > (SF0X_CONVERSION_INTERVAL * 2)) {
_linebuf_index = 0;
}
/* read from the sensor (uart buffer) */
ret = ::read(_fd, &_linebuf[_linebuf_index], sizeof(_linebuf) - _linebuf_index);
if (ret < 0) {
_linebuf[sizeof(_linebuf) - 1] = '\0';
debug("read err: %d lbi: %d buf: %s", ret, (int)_linebuf_index, _linebuf);
perf_count(_comms_errors);
perf_end(_sample_perf);
/* only throw an error if we time out */
if (read_elapsed > (SF0X_CONVERSION_INTERVAL * 2)) {
return ret;
} else {
return -EAGAIN;
}
}
_linebuf_index += ret;
if (_linebuf_index >= sizeof(_linebuf)) {
_linebuf_index = 0;
}
_last_read = hrt_absolute_time();
if (_linebuf[_linebuf_index - 2] != '\r' || _linebuf[_linebuf_index - 1] != '\n') {
/* incomplete read, reschedule ourselves */
return -EAGAIN;
}
char *end;
float si_units;
bool valid;
/* enforce line ending */
_linebuf[sizeof(_linebuf) - 1] = '\0';
if (_linebuf[0] == '-' && _linebuf[1] == '-' && _linebuf[2] == '.') {
si_units = -1.0f;
valid = false;
} else {
si_units = strtod(_linebuf, &end);
valid = true;
}
debug("val (float): %8.4f, raw: %s\n", si_units, _linebuf);
/* done with this chunk, resetting */
_linebuf_index = 0;
struct range_finder_report report;
/* this should be fairly close to the end of the measurement, so the best approximation of the time */
report.timestamp = hrt_absolute_time();
report.error_count = perf_event_count(_comms_errors);
report.distance = si_units;
report.valid = valid && (si_units > get_minimum_distance() && si_units < get_maximum_distance() ? 1 : 0);
/* publish it */
orb_publish(ORB_ID(sensor_range_finder), _range_finder_topic, &report);
if (_reports->force(&report)) {
perf_count(_buffer_overflows);
}
/* notify anyone waiting for data */
poll_notify(POLLIN);
ret = OK;
perf_end(_sample_perf);
return ret;
}
void
SF0X::start()
{
/* reset the report ring and state machine */
_collect_phase = false;
_reports->flush();
/* schedule a cycle to start things */
work_queue(HPWORK, &_work, (worker_t)&SF0X::cycle_trampoline, this, 1);
// /* notify about state change */
// struct subsystem_info_s info = {
// true,
// true,
// true,
// SUBSYSTEM_TYPE_RANGEFINDER
// };
// 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);
// }
}
void
SF0X::stop()
{
work_cancel(HPWORK, &_work);
}
void
SF0X::cycle_trampoline(void *arg)
{
SF0X *dev = static_cast<SF0X *>(arg);
dev->cycle();
}
void
SF0X::cycle()
{
/* fds initialized? */
if (_fd < 0) {
/* open fd */
_fd = ::open(SF0X_DEFAULT_PORT, O_RDWR | O_NOCTTY | O_NONBLOCK);
}
/* collection phase? */
if (_collect_phase) {
/* perform collection */
int collect_ret = collect();
if (collect_ret == -EAGAIN) {
/* reschedule to grab the missing bits, time to transmit 10 bytes @9600 bps */
work_queue(HPWORK,
&_work,
(worker_t)&SF0X::cycle_trampoline,
this,
USEC2TICK(1100));
return;
}
if (OK != collect_ret) {
log("collection error");
/* restart the measurement state machine */
start();
return;
}
/* next phase is measurement */
_collect_phase = false;
/*
* Is there a collect->measure gap?
*/
if (_measure_ticks > USEC2TICK(SF0X_CONVERSION_INTERVAL)) {
/* schedule a fresh cycle call when we are ready to measure again */
work_queue(HPWORK,
&_work,
(worker_t)&SF0X::cycle_trampoline,
this,
_measure_ticks - USEC2TICK(SF0X_CONVERSION_INTERVAL));
return;
}
}
/* measurement phase */
if (OK != measure()) {
log("measure error");
}
/* next phase is collection */
_collect_phase = true;
/* schedule a fresh cycle call when the measurement is done */
work_queue(HPWORK,
&_work,
(worker_t)&SF0X::cycle_trampoline,
this,
USEC2TICK(SF0X_CONVERSION_INTERVAL));
}
void
SF0X::print_info()
{
perf_print_counter(_sample_perf);
perf_print_counter(_comms_errors);
perf_print_counter(_buffer_overflows);
printf("poll interval: %d ticks\n", _measure_ticks);
_reports->print_info("report queue");
}
/**
* Local functions in support of the shell command.
*/
namespace sf0x
{
/* oddly, ERROR is not defined for c++ */
#ifdef ERROR
# undef ERROR
#endif
const int ERROR = -1;
SF0X *g_dev;
void start();
void stop();
void test();
void reset();
void info();
/**
* Start the driver.
*/
void
start(const char *port)
{
int fd;
if (g_dev != nullptr) {
errx(1, "already started");
}
/* create the driver */
g_dev = new SF0X(port);
if (g_dev == nullptr) {
goto fail;
}
if (OK != g_dev->init()) {
goto fail;
}
/* set the poll rate to default, starts automatic data collection */
fd = open(RANGE_FINDER_DEVICE_PATH, 0);
if (fd < 0) {
warnx("device open fail");
goto fail;
}
if (ioctl(fd, SENSORIOCSPOLLRATE, SENSOR_POLLRATE_DEFAULT) < 0) {
goto fail;
}
exit(0);
fail:
if (g_dev != nullptr) {
delete g_dev;
g_dev = nullptr;
}
errx(1, "driver start failed");
}
/**
* Stop the driver
*/
void stop()
{
if (g_dev != nullptr) {
delete g_dev;
g_dev = nullptr;
} else {
errx(1, "driver not running");
}
exit(0);
}
/**
* Perform some basic functional tests on the driver;
* make sure we can collect data from the sensor in polled
* and automatic modes.
*/
void
test()
{
struct range_finder_report report;
ssize_t sz;
int fd = open(RANGE_FINDER_DEVICE_PATH, O_RDONLY);
if (fd < 0) {
err(1, "%s open failed (try 'sf0x start' if the driver is not running", RANGE_FINDER_DEVICE_PATH);
}
/* do a simple demand read */
sz = read(fd, &report, sizeof(report));
if (sz != sizeof(report)) {
err(1, "immediate read failed");
}
warnx("single read");
warnx("measurement: %0.2f m", (double)report.distance);
warnx("time: %lld", report.timestamp);
/* start the sensor polling at 2Hz */
if (OK != ioctl(fd, SENSORIOCSPOLLRATE, 2)) {
errx(1, "failed to set 2Hz poll rate");
}
/* read the sensor 5x and report each value */
for (unsigned i = 0; i < 5; i++) {
struct pollfd fds;
/* wait for data to be ready */
fds.fd = fd;
fds.events = POLLIN;
int ret = poll(&fds, 1, 2000);
if (ret != 1) {
errx(1, "timed out waiting for sensor data");
}
/* now go get it */
sz = read(fd, &report, sizeof(report));
if (sz != sizeof(report)) {
err(1, "periodic read failed");
}
warnx("periodic read %u", i);
warnx("measurement: %0.3f", (double)report.distance);
warnx("time: %lld", report.timestamp);
}
/* reset the sensor polling to default rate */
if (OK != ioctl(fd, SENSORIOCSPOLLRATE, SENSOR_POLLRATE_DEFAULT)) {
errx(1, "failed to set default poll rate");
}
errx(0, "PASS");
}
/**
* Reset the driver.
*/
void
reset()
{
int fd = open(RANGE_FINDER_DEVICE_PATH, O_RDONLY);
if (fd < 0) {
err(1, "failed ");
}
if (ioctl(fd, SENSORIOCRESET, 0) < 0) {
err(1, "driver reset failed");
}
if (ioctl(fd, SENSORIOCSPOLLRATE, SENSOR_POLLRATE_DEFAULT) < 0) {
err(1, "driver poll restart failed");
}
exit(0);
}
/**
* Print a little info about the driver.
*/
void
info()
{
if (g_dev == nullptr) {
errx(1, "driver not running");
}
printf("state @ %p\n", g_dev);
g_dev->print_info();
exit(0);
}
} // namespace
int
sf0x_main(int argc, char *argv[])
{
/*
* Start/load the driver.
*/
if (!strcmp(argv[1], "start")) {
if (argc > 2) {
sf0x::start(argv[2]);
} else {
sf0x::start(SF0X_DEFAULT_PORT);
}
}
/*
* Stop the driver
*/
if (!strcmp(argv[1], "stop")) {
sf0x::stop();
}
/*
* Test the driver/device.
*/
if (!strcmp(argv[1], "test")) {
sf0x::test();
}
/*
* Reset the driver.
*/
if (!strcmp(argv[1], "reset")) {
sf0x::reset();
}
/*
* Print driver information.
*/
if (!strcmp(argv[1], "info") || !strcmp(argv[1], "status")) {
sf0x::info();
}
errx(1, "unrecognized command, try 'start', 'test', 'reset' or 'info'");
}

105
src/modules/gpio_led/gpio_led.c
View File

@ -51,7 +51,6 @@
#include <systemlib/err.h> #include <systemlib/err.h>
#include <uORB/uORB.h> #include <uORB/uORB.h>
#include <uORB/topics/vehicle_status.h> #include <uORB/topics/vehicle_status.h>
#include <uORB/topics/actuator_armed.h>
#include <poll.h> #include <poll.h>
#include <drivers/drv_gpio.h> #include <drivers/drv_gpio.h>
#include <modules/px4iofirmware/protocol.h> #include <modules/px4iofirmware/protocol.h>
@ -63,8 +62,6 @@ struct gpio_led_s {
int pin; int pin;
struct vehicle_status_s status; struct vehicle_status_s status;
int vehicle_status_sub; int vehicle_status_sub;
struct actuator_armed_s armed;
int actuator_armed_sub;
bool led_state; bool led_state;
int counter; int counter;
}; };
@ -81,6 +78,7 @@ void gpio_led_cycle(FAR void *arg);
int gpio_led_main(int argc, char *argv[]) int gpio_led_main(int argc, char *argv[])
{ {
if (argc < 2) { if (argc < 2) {
#ifdef CONFIG_ARCH_BOARD_PX4FMU_V1
errx(1, "usage: gpio_led {start|stop} [-p <1|2|a1|a2|r1|r2>]\n" errx(1, "usage: gpio_led {start|stop} [-p <1|2|a1|a2|r1|r2>]\n"
"\t-p\tUse pin:\n" "\t-p\tUse pin:\n"
"\t\t1\tPX4FMU GPIO_EXT1 (default)\n" "\t\t1\tPX4FMU GPIO_EXT1 (default)\n"
@ -88,7 +86,14 @@ int gpio_led_main(int argc, char *argv[])
"\t\ta1\tPX4IO ACC1\n" "\t\ta1\tPX4IO ACC1\n"
"\t\ta2\tPX4IO ACC2\n" "\t\ta2\tPX4IO ACC2\n"
"\t\tr1\tPX4IO RELAY1\n" "\t\tr1\tPX4IO RELAY1\n"
"\t\tr2\tPX4IO RELAY2"); "\t\tr2\tPX4IO RELAY2"
);
#endif
#ifdef CONFIG_ARCH_BOARD_PX4FMU_V2
errx(1, "usage: gpio_led {start|stop} [-p <n>]\n"
"\t-p <n>\tUse specified AUX OUT pin number (default: 1)"
);
#endif
} else { } else {
@ -98,37 +103,70 @@ int gpio_led_main(int argc, char *argv[])
} }
bool use_io = false; bool use_io = false;
int pin = GPIO_EXT_1;
/* by default use GPIO_EXT_1 on FMUv1 and GPIO_SERVO_1 on FMUv2 */
int pin = 1;
/* pin name to display */
#ifdef CONFIG_ARCH_BOARD_PX4FMU_V1
char *pin_name = "PX4FMU GPIO_EXT1";
#endif
#ifdef CONFIG_ARCH_BOARD_PX4FMU_V2
char pin_name[] = "AUX OUT 1";
#endif
if (argc > 2) { if (argc > 2) {
if (!strcmp(argv[2], "-p")) { if (!strcmp(argv[2], "-p")) {
#ifdef CONFIG_ARCH_BOARD_PX4FMU_V1
if (!strcmp(argv[3], "1")) { if (!strcmp(argv[3], "1")) {
use_io = false; use_io = false;
pin = GPIO_EXT_1; pin = GPIO_EXT_1;
pin_name = "PX4FMU GPIO_EXT1";
} else if (!strcmp(argv[3], "2")) { } else if (!strcmp(argv[3], "2")) {
use_io = false; use_io = false;
pin = GPIO_EXT_2; pin = GPIO_EXT_2;
pin_name = "PX4FMU GPIO_EXT2";
} else if (!strcmp(argv[3], "a1")) { } else if (!strcmp(argv[3], "a1")) {
use_io = true; use_io = true;
pin = PX4IO_P_SETUP_RELAYS_ACC1; pin = PX4IO_P_SETUP_RELAYS_ACC1;
pin_name = "PX4IO ACC1";
} else if (!strcmp(argv[3], "a2")) { } else if (!strcmp(argv[3], "a2")) {
use_io = true; use_io = true;
pin = PX4IO_P_SETUP_RELAYS_ACC2; pin = PX4IO_P_SETUP_RELAYS_ACC2;
pin_name = "PX4IO ACC2";
} else if (!strcmp(argv[3], "r1")) { } else if (!strcmp(argv[3], "r1")) {
use_io = true; use_io = true;
pin = PX4IO_P_SETUP_RELAYS_POWER1; pin = PX4IO_P_SETUP_RELAYS_POWER1;
pin_name = "PX4IO RELAY1";
} else if (!strcmp(argv[3], "r2")) { } else if (!strcmp(argv[3], "r2")) {
use_io = true; use_io = true;
pin = PX4IO_P_SETUP_RELAYS_POWER2; pin = PX4IO_P_SETUP_RELAYS_POWER2;
pin_name = "PX4IO RELAY2";
} else { } else {
errx(1, "unsupported pin: %s", argv[3]); errx(1, "unsupported pin: %s", argv[3]);
} }
#endif
#ifdef CONFIG_ARCH_BOARD_PX4FMU_V2
unsigned int n = strtoul(argv[3], NULL, 10);
if (n >= 1 && n <= 6) {
use_io = false;
pin = 1 << (n - 1);
snprintf(pin_name, sizeof(pin_name), "AUX OUT %d", n);
} else {
errx(1, "unsupported pin: %s", argv[3]);
}
#endif
} }
} }
@ -142,21 +180,6 @@ int gpio_led_main(int argc, char *argv[])
} else { } else {
gpio_led_started = true; gpio_led_started = true;
char pin_name[24];
if (use_io) {
if (pin & (PX4IO_P_SETUP_RELAYS_ACC1 | PX4IO_P_SETUP_RELAYS_ACC2)) {
sprintf(pin_name, "PX4IO ACC%i", (pin >> 3));
} else {
sprintf(pin_name, "PX4IO RELAY%i", pin);
}
} else {
sprintf(pin_name, "PX4FMU GPIO_EXT%i", pin);
}
warnx("start, using pin: %s", pin_name); warnx("start, using pin: %s", pin_name);
} }
@ -186,6 +209,7 @@ void gpio_led_start(FAR void *arg)
if (priv->use_io) { if (priv->use_io) {
gpio_dev = PX4IO_DEVICE_PATH; gpio_dev = PX4IO_DEVICE_PATH;
} else { } else {
gpio_dev = PX4FMU_DEVICE_PATH; gpio_dev = PX4FMU_DEVICE_PATH;
} }
@ -204,8 +228,10 @@ void gpio_led_start(FAR void *arg)
/* px4fmu only, px4io doesn't support GPIO_SET_OUTPUT and will ignore */ /* px4fmu only, px4io doesn't support GPIO_SET_OUTPUT and will ignore */
ioctl(priv->gpio_fd, GPIO_SET_OUTPUT, priv->pin); ioctl(priv->gpio_fd, GPIO_SET_OUTPUT, priv->pin);
/* subscribe to vehicle status topic */ /* initialize vehicle status structure */
memset(&priv->status, 0, sizeof(priv->status)); memset(&priv->status, 0, sizeof(priv->status));
/* subscribe to vehicle status topic */
priv->vehicle_status_sub = orb_subscribe(ORB_ID(vehicle_status)); priv->vehicle_status_sub = orb_subscribe(ORB_ID(vehicle_status));
/* add worker to queue */ /* add worker to queue */
@ -224,38 +250,33 @@ void gpio_led_cycle(FAR void *arg)
FAR struct gpio_led_s *priv = (FAR struct gpio_led_s *)arg; FAR struct gpio_led_s *priv = (FAR struct gpio_led_s *)arg;
/* check for status updates*/ /* check for status updates*/
bool status_updated; bool updated;
orb_check(priv->vehicle_status_sub, &status_updated); orb_check(priv->vehicle_status_sub, &updated);
if (status_updated) if (updated) {
orb_copy(ORB_ID(vehicle_status), priv->vehicle_status_sub, &priv->status); orb_copy(ORB_ID(vehicle_status), priv->vehicle_status_sub, &priv->status);
}
orb_check(priv->vehicle_status_sub, &status_updated);
if (status_updated)
orb_copy(ORB_ID(actuator_armed), priv->actuator_armed_sub, &priv->armed);
/* select pattern for current status */ /* select pattern for current status */
int pattern = 0; int pattern = 0;
if (priv->armed.armed) { if (priv->status.arming_state == ARMING_STATE_ARMED_ERROR) {
if (priv->status.battery_warning == VEHICLE_BATTERY_WARNING_NONE) { pattern = 0x2A; // *_*_*_ fast blink (armed, error)
} else if (priv->status.arming_state == ARMING_STATE_ARMED) {
if (priv->status.battery_warning == VEHICLE_BATTERY_WARNING_NONE && priv->status.failsafe_state == FAILSAFE_STATE_NORMAL) {
pattern = 0x3f; // ****** solid (armed) pattern = 0x3f; // ****** solid (armed)
} else { } else {
pattern = 0x2A; // *_*_*_ fast blink (armed, battery warning) pattern = 0x3e; // *****_ slow blink (armed, battery low or failsafe)
} }
} else { } else if (priv->status.arming_state == ARMING_STATE_STANDBY) {
if (priv->armed.ready_to_arm) { pattern = 0x38; // ***___ slow blink (disarmed, ready)
pattern = 0x00; // ______ off (disarmed, preflight check)
} else if (priv->armed.ready_to_arm && priv->status.battery_warning == VEHICLE_BATTERY_WARNING_NONE) { } else if (priv->status.arming_state == ARMING_STATE_STANDBY_ERROR) {
pattern = 0x38; // ***___ slow blink (disarmed, ready) pattern = 0x28; // *_*___ slow double blink (disarmed, error)
} else {
pattern = 0x28; // *_*___ slow double blink (disarmed, not good to arm)
}
} }
/* blink pattern */ /* blink pattern */
@ -266,6 +287,7 @@ void gpio_led_cycle(FAR void *arg)
if (led_state_new) { if (led_state_new) {
ioctl(priv->gpio_fd, GPIO_SET, priv->pin); ioctl(priv->gpio_fd, GPIO_SET, priv->pin);
} else { } else {
ioctl(priv->gpio_fd, GPIO_CLEAR, priv->pin); ioctl(priv->gpio_fd, GPIO_CLEAR, priv->pin);
} }
@ -273,8 +295,9 @@ void gpio_led_cycle(FAR void *arg)
priv->counter++; priv->counter++;
if (priv->counter > 5) if (priv->counter > 5) {
priv->counter = 0; priv->counter = 0;
}
/* repeat cycle at 5 Hz */ /* repeat cycle at 5 Hz */
if (gpio_led_started) { if (gpio_led_started) {

2
src/modules/px4iofirmware/mixer.cpp
View File

@ -267,7 +267,7 @@ mixer_callback(uintptr_t handle,
uint8_t control_index, uint8_t control_index,
float &control) float &control)
{ {
if (control_group > 3) if (control_group >= PX4IO_CONTROL_GROUPS)
return -1; return -1;
switch (source) { switch (source) {

2
src/modules/px4iofirmware/px4io.h
View File

@ -53,7 +53,7 @@
*/ */
#define PX4IO_SERVO_COUNT 8 #define PX4IO_SERVO_COUNT 8
#define PX4IO_CONTROL_CHANNELS 8 #define PX4IO_CONTROL_CHANNELS 8
#define PX4IO_CONTROL_GROUPS 2 #define PX4IO_CONTROL_GROUPS 4
#define PX4IO_RC_INPUT_CHANNELS 18 #define PX4IO_RC_INPUT_CHANNELS 18
#define PX4IO_RC_MAPPED_CONTROL_CHANNELS 8 /**< This is the maximum number of channels mapped/used */ #define PX4IO_RC_MAPPED_CONTROL_CHANNELS 8 /**< This is the maximum number of channels mapped/used */

3
src/modules/sdlog2/logbuffer.c
View File

@ -74,8 +74,9 @@ bool logbuffer_write(struct logbuffer_s *lb, void *ptr, int size)
// bytes available to write // bytes available to write
int available = lb->read_ptr - lb->write_ptr - 1; int available = lb->read_ptr - lb->write_ptr - 1;
if (available < 0) if (available < 0) {
available += lb->size; available += lb->size;
}
if (size > available) { if (size > available) {
// buffer overflow // buffer overflow

987
src/modules/sdlog2/sdlog2.c
View File

File diff suppressed because it is too large Load Diff

14
src/modules/sdlog2/sdlog2_messages.h
View File

@ -268,13 +268,13 @@ struct log_DIST_s {
/* --- TELE - TELEMETRY STATUS --- */ /* --- TELE - TELEMETRY STATUS --- */
#define LOG_TELE_MSG 22 #define LOG_TELE_MSG 22
struct log_TELE_s { struct log_TELE_s {
uint8_t rssi; uint8_t rssi;
uint8_t remote_rssi; uint8_t remote_rssi;
uint8_t noise; uint8_t noise;
uint8_t remote_noise; uint8_t remote_noise;
uint16_t rxerrors; uint16_t rxerrors;
uint16_t fixed; uint16_t fixed;
uint8_t txbuf; uint8_t txbuf;
}; };
/********** SYSTEM MESSAGES, ID > 0x80 **********/ /********** SYSTEM MESSAGES, ID > 0x80 **********/

116
src/systemcmds/dumpfile/dumpfile.c Normal file
View File

@ -0,0 +1,116 @@
/****************************************************************************
*
* 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 dumpfile.c
*
* Dump file utility. Prints file size and contents in binary mode (don't replace LF with CR LF) to stdout.
*
* @author Anton Babushkin <anton.babushkin@me.com>
*/
#include <nuttx/config.h>
#include <unistd.h>
#include <stdio.h>
#include <string.h>
#include <stdlib.h>
#include <fcntl.h>
#include <termios.h>
#include <systemlib/err.h>
__EXPORT int dumpfile_main(int argc, char *argv[]);
int
dumpfile_main(int argc, char *argv[])
{
if (argc < 2) {
errx(1, "usage: dumpfile <filename>");
}
/* open input file */
FILE *f;
f = fopen(argv[1], "r");
if (f == NULL) {
printf("ERROR\n");
exit(1);
}
/* get file size */
fseek(f, 0L, SEEK_END);
int size = ftell(f);
fseek(f, 0L, SEEK_SET);
printf("OK %d\n", size);
/* configure stdout */
int out = fileno(stdout);
struct termios tc;
struct termios tc_old;
tcgetattr(out, &tc);
/* save old terminal attributes to restore it later on exit */
memcpy(&tc_old, &tc, sizeof(tc));
/* don't add CR on each LF*/
tc.c_oflag &= ~ONLCR;
if (tcsetattr(out, TCSANOW, &tc) < 0) {
warnx("ERROR setting stdout attributes");
exit(1);
}
char buf[512];
int nread;
/* dump file */
while ((nread = fread(buf, 1, sizeof(buf), f)) > 0) {
if (write(out, buf, nread) <= 0) {
warnx("error dumping file");
break;
}
}
fsync(out);
fclose(f);
/* restore old terminal attributes */
if (tcsetattr(out, TCSANOW, &tc_old) < 0) {
warnx("ERROR restoring stdout attributes");
exit(1);
}
return OK;
}

41
src/systemcmds/dumpfile/module.mk Normal file
View File

@ -0,0 +1,41 @@
############################################################################
#
# 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.
#
############################################################################
#
# Dump file utility
#
MODULE_COMMAND = dumpfile
SRCS = dumpfile.c
MAXOPTIMIZATION = -Os