forked from Archive/PX4-Autopilot
Merge remote-tracking branch 'upstream/master' into diff_press_filter
Conflicts: src/modules/sdlog2/sdlog2.c
This commit is contained in:
commit
07cff8e0bd
|
@ -10,11 +10,11 @@ sh /etc/init.d/rc.mc_defaults
|
|||
if [ $DO_AUTOCONFIG == yes ]
|
||||
then
|
||||
# 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_I 0.0
|
||||
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_I 0.0
|
||||
param set MC_PITCHRATE_D 0.004
|
||||
|
|
|
@ -23,13 +23,15 @@ then
|
|||
param set MC_PITCHRATE_I 0.0
|
||||
param set MC_PITCHRATE_D 0.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_I 0.0
|
||||
param set MC_YAWRATE_D 0.0
|
||||
param set MC_YAW_FF 0.15
|
||||
param set BAT_V_SCALING 0.00838095238
|
||||
fi
|
||||
|
||||
set OUTPUT_MODE ardrone
|
||||
set USE_IO no
|
||||
set MIXER skip
|
||||
# set MAV_TYPE because no specific mixer is set
|
||||
set MAV_TYPE 2
|
||||
|
|
|
@ -118,6 +118,7 @@ then
|
|||
set MKBLCTRL_MODE none
|
||||
set FMU_MODE pwm
|
||||
set MAV_TYPE none
|
||||
set LOAD_DEFAULT_APPS yes
|
||||
|
||||
#
|
||||
# Set DO_AUTOCONFIG flag to use it in AUTOSTART scripts
|
||||
|
@ -465,7 +466,10 @@ then
|
|||
sh /etc/init.d/rc.interface
|
||||
|
||||
# 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
|
||||
|
||||
#
|
||||
|
@ -521,7 +525,10 @@ then
|
|||
sh /etc/init.d/rc.interface
|
||||
|
||||
# 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
|
||||
|
||||
#
|
||||
|
|
|
@ -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()
|
|
@ -56,6 +56,7 @@ MODULES += systemcmds/tests
|
|||
MODULES += systemcmds/config
|
||||
MODULES += systemcmds/nshterm
|
||||
MODULES += systemcmds/hw_ver
|
||||
MODULES += systemcmds/dumpfile
|
||||
|
||||
#
|
||||
# General system control
|
||||
|
|
|
@ -27,6 +27,7 @@ MODULES += drivers/l3gd20
|
|||
MODULES += drivers/hmc5883
|
||||
MODULES += drivers/ms5611
|
||||
MODULES += drivers/mb12xx
|
||||
MODULES += drivers/sf0x
|
||||
MODULES += drivers/gps
|
||||
MODULES += drivers/hil
|
||||
MODULES += drivers/hott/hott_telemetry
|
||||
|
@ -63,6 +64,7 @@ MODULES += systemcmds/config
|
|||
MODULES += systemcmds/nshterm
|
||||
MODULES += systemcmds/mtd
|
||||
MODULES += systemcmds/hw_ver
|
||||
MODULES += systemcmds/dumpfile
|
||||
|
||||
#
|
||||
# General system control
|
||||
|
@ -71,6 +73,7 @@ MODULES += modules/commander
|
|||
MODULES += modules/navigator
|
||||
MODULES += modules/mavlink
|
||||
MODULES += modules/mavlink_onboard
|
||||
MODULES += modules/gpio_led
|
||||
|
||||
#
|
||||
# Estimation modules (EKF/ SO3 / other filters)
|
||||
|
|
|
@ -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 */
|
|
@ -37,7 +37,7 @@
|
|||
*
|
||||
* Driver for the Maxbotix sonar range finders connected via I2C.
|
||||
*/
|
||||
|
||||
|
||||
#include <nuttx/config.h>
|
||||
|
||||
#include <drivers/device/i2c.h>
|
||||
|
@ -84,7 +84,7 @@
|
|||
/* Device limits */
|
||||
#define MB12XX_MIN_DISTANCE (0.20f)
|
||||
#define MB12XX_MAX_DISTANCE (7.65f)
|
||||
|
||||
|
||||
#define MB12XX_CONVERSION_INTERVAL 60000 /* 60ms */
|
||||
|
||||
/* oddly, ERROR is not defined for c++ */
|
||||
|
@ -102,17 +102,17 @@ class MB12XX : public device::I2C
|
|||
public:
|
||||
MB12XX(int bus = MB12XX_BUS, int address = MB12XX_BASEADDR);
|
||||
virtual ~MB12XX();
|
||||
|
||||
|
||||
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();
|
||||
|
||||
|
@ -124,13 +124,13 @@ private:
|
|||
bool _sensor_ok;
|
||||
int _measure_ticks;
|
||||
bool _collect_phase;
|
||||
|
||||
|
||||
orb_advert_t _range_finder_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.
|
||||
|
@ -139,7 +139,7 @@ private:
|
|||
* @return True if the device is present.
|
||||
*/
|
||||
int probe_address(uint8_t address);
|
||||
|
||||
|
||||
/**
|
||||
* Initialise the automatic measurement state machine and start it.
|
||||
*
|
||||
|
@ -147,12 +147,12 @@ private:
|
|||
* 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 MB12XX_MIN_DISTANCE
|
||||
|
@ -162,7 +162,7 @@ private:
|
|||
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.
|
||||
|
@ -177,8 +177,8 @@ private:
|
|||
* @param arg Instance pointer for the driver that is polling.
|
||||
*/
|
||||
static void cycle_trampoline(void *arg);
|
||||
|
||||
|
||||
|
||||
|
||||
};
|
||||
|
||||
/*
|
||||
|
@ -200,8 +200,8 @@ MB12XX::MB12XX(int bus, int address) :
|
|||
_buffer_overflows(perf_alloc(PC_COUNT, "mb12xx_buffer_overflows"))
|
||||
{
|
||||
// enable debug() calls
|
||||
_debug_enabled = true;
|
||||
|
||||
_debug_enabled = false;
|
||||
|
||||
// work_cancel in the dtor will explode if we don't do this...
|
||||
memset(&_work, 0, sizeof(_work));
|
||||
}
|
||||
|
@ -212,8 +212,9 @@ MB12XX::~MB12XX()
|
|||
stop();
|
||||
|
||||
/* free any existing reports */
|
||||
if (_reports != nullptr)
|
||||
if (_reports != nullptr) {
|
||||
delete _reports;
|
||||
}
|
||||
}
|
||||
|
||||
int
|
||||
|
@ -222,22 +223,25 @@ MB12XX::init()
|
|||
int ret = ERROR;
|
||||
|
||||
/* do I2C init (and probe) first */
|
||||
if (I2C::init() != OK)
|
||||
if (I2C::init() != OK) {
|
||||
goto out;
|
||||
}
|
||||
|
||||
/* allocate basic report buffers */
|
||||
_reports = new RingBuffer(2, sizeof(range_finder_report));
|
||||
|
||||
if (_reports == nullptr)
|
||||
if (_reports == nullptr) {
|
||||
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)
|
||||
if (_range_finder_topic < 0) {
|
||||
debug("failed to create sensor_range_finder object. Did you start uOrb?");
|
||||
}
|
||||
|
||||
ret = OK;
|
||||
/* sensor is ok, but we don't really know if it is within range */
|
||||
|
@ -256,13 +260,13 @@ void
|
|||
MB12XX::set_minimum_distance(float min)
|
||||
{
|
||||
_min_distance = min;
|
||||
}
|
||||
}
|
||||
|
||||
void
|
||||
MB12XX::set_maximum_distance(float max)
|
||||
{
|
||||
_max_distance = max;
|
||||
}
|
||||
}
|
||||
|
||||
float
|
||||
MB12XX::get_minimum_distance()
|
||||
|
@ -284,20 +288,20 @@ MB12XX::ioctl(struct file *filp, int cmd, unsigned long arg)
|
|||
case SENSORIOCSPOLLRATE: {
|
||||
switch (arg) {
|
||||
|
||||
/* switching to manual polling */
|
||||
/* switching to manual polling */
|
||||
case SENSOR_POLLRATE_MANUAL:
|
||||
stop();
|
||||
_measure_ticks = 0;
|
||||
return OK;
|
||||
|
||||
/* external signalling (DRDY) not supported */
|
||||
/* external signalling (DRDY) not supported */
|
||||
case SENSOR_POLLRATE_EXTERNAL:
|
||||
|
||||
/* zero would be bad */
|
||||
/* zero would be bad */
|
||||
case 0:
|
||||
return -EINVAL;
|
||||
|
||||
/* set default/max polling rate */
|
||||
/* set default/max polling rate */
|
||||
case SENSOR_POLLRATE_MAX:
|
||||
case SENSOR_POLLRATE_DEFAULT: {
|
||||
/* 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);
|
||||
|
||||
/* if we need to start the poll state machine, do it */
|
||||
if (want_start)
|
||||
if (want_start) {
|
||||
start();
|
||||
}
|
||||
|
||||
return OK;
|
||||
}
|
||||
|
||||
/* adjust to a legal polling interval in Hz */
|
||||
/* adjust to a legal polling interval in Hz */
|
||||
default: {
|
||||
/* do we need to start internal polling? */
|
||||
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);
|
||||
|
||||
/* check against maximum rate */
|
||||
if (ticks < USEC2TICK(MB12XX_CONVERSION_INTERVAL))
|
||||
if (ticks < USEC2TICK(MB12XX_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)
|
||||
if (want_start) {
|
||||
start();
|
||||
}
|
||||
|
||||
return OK;
|
||||
}
|
||||
|
@ -338,45 +345,49 @@ MB12XX::ioctl(struct file *filp, int cmd, unsigned long arg)
|
|||
}
|
||||
|
||||
case SENSORIOCGPOLLRATE:
|
||||
if (_measure_ticks == 0)
|
||||
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)) {
|
||||
/* 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 -ENOMEM;
|
||||
|
||||
return OK;
|
||||
}
|
||||
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;
|
||||
|
||||
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 I2C::ioctl(filp, cmd, arg);
|
||||
|
@ -391,8 +402,9 @@ MB12XX::read(struct file *filp, char *buffer, size_t buflen)
|
|||
int ret = 0;
|
||||
|
||||
/* buffer must be large enough */
|
||||
if (count < 1)
|
||||
if (count < 1) {
|
||||
return -ENOSPC;
|
||||
}
|
||||
|
||||
/* if automatic measurement is enabled */
|
||||
if (_measure_ticks > 0) {
|
||||
|
@ -453,14 +465,14 @@ MB12XX::measure()
|
|||
uint8_t cmd = MB12XX_TAKE_RANGE_REG;
|
||||
ret = transfer(&cmd, 1, nullptr, 0);
|
||||
|
||||
if (OK != ret)
|
||||
{
|
||||
if (OK != ret) {
|
||||
perf_count(_comms_errors);
|
||||
log("i2c::transfer returned %d", ret);
|
||||
return ret;
|
||||
}
|
||||
|
||||
ret = OK;
|
||||
|
||||
|
||||
return ret;
|
||||
}
|
||||
|
||||
|
@ -468,32 +480,31 @@ int
|
|||
MB12XX::collect()
|
||||
{
|
||||
int ret = -EIO;
|
||||
|
||||
|
||||
/* read from the sensor */
|
||||
uint8_t val[2] = {0, 0};
|
||||
|
||||
|
||||
perf_begin(_sample_perf);
|
||||
|
||||
|
||||
ret = transfer(nullptr, 0, &val[0], 2);
|
||||
|
||||
if (ret < 0)
|
||||
{
|
||||
|
||||
if (ret < 0) {
|
||||
log("error reading from sensor: %d", ret);
|
||||
perf_count(_comms_errors);
|
||||
perf_end(_sample_perf);
|
||||
return ret;
|
||||
}
|
||||
|
||||
|
||||
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;
|
||||
|
||||
/* 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.error_count = perf_event_count(_comms_errors);
|
||||
report.distance = si_units;
|
||||
report.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);
|
||||
|
||||
|
@ -519,17 +530,19 @@ MB12XX::start()
|
|||
|
||||
/* schedule a cycle to start things */
|
||||
work_queue(HPWORK, &_work, (worker_t)&MB12XX::cycle_trampoline, this, 1);
|
||||
|
||||
|
||||
/* notify about state change */
|
||||
struct subsystem_info_s info = {
|
||||
true,
|
||||
true,
|
||||
true,
|
||||
SUBSYSTEM_TYPE_RANGEFINDER};
|
||||
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);
|
||||
}
|
||||
|
@ -583,8 +596,9 @@ MB12XX::cycle()
|
|||
}
|
||||
|
||||
/* measurement phase */
|
||||
if (OK != measure())
|
||||
if (OK != measure()) {
|
||||
log("measure error");
|
||||
}
|
||||
|
||||
/* next phase is collection */
|
||||
_collect_phase = true;
|
||||
|
@ -635,33 +649,37 @@ start()
|
|||
{
|
||||
int fd;
|
||||
|
||||
if (g_dev != nullptr)
|
||||
if (g_dev != nullptr) {
|
||||
errx(1, "already started");
|
||||
}
|
||||
|
||||
/* create the driver */
|
||||
g_dev = new MB12XX(MB12XX_BUS);
|
||||
|
||||
if (g_dev == nullptr)
|
||||
if (g_dev == nullptr) {
|
||||
goto fail;
|
||||
}
|
||||
|
||||
if (OK != g_dev->init())
|
||||
if (OK != g_dev->init()) {
|
||||
goto fail;
|
||||
}
|
||||
|
||||
/* set the poll rate to default, starts automatic data collection */
|
||||
fd = open(RANGE_FINDER_DEVICE_PATH, O_RDONLY);
|
||||
|
||||
if (fd < 0)
|
||||
if (fd < 0) {
|
||||
goto fail;
|
||||
}
|
||||
|
||||
if (ioctl(fd, SENSORIOCSPOLLRATE, SENSOR_POLLRATE_DEFAULT) < 0)
|
||||
if (ioctl(fd, SENSORIOCSPOLLRATE, SENSOR_POLLRATE_DEFAULT) < 0) {
|
||||
goto fail;
|
||||
}
|
||||
|
||||
exit(0);
|
||||
|
||||
fail:
|
||||
|
||||
if (g_dev != nullptr)
|
||||
{
|
||||
if (g_dev != nullptr) {
|
||||
delete g_dev;
|
||||
g_dev = nullptr;
|
||||
}
|
||||
|
@ -674,15 +692,14 @@ fail:
|
|||
*/
|
||||
void stop()
|
||||
{
|
||||
if (g_dev != nullptr)
|
||||
{
|
||||
if (g_dev != nullptr) {
|
||||
delete g_dev;
|
||||
g_dev = nullptr;
|
||||
}
|
||||
else
|
||||
{
|
||||
|
||||
} else {
|
||||
errx(1, "driver not running");
|
||||
}
|
||||
|
||||
exit(0);
|
||||
}
|
||||
|
||||
|
@ -700,22 +717,25 @@ test()
|
|||
|
||||
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);
|
||||
}
|
||||
|
||||
/* do a simple demand read */
|
||||
sz = read(fd, &report, sizeof(report));
|
||||
|
||||
if (sz != 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))
|
||||
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++) {
|
||||
|
@ -726,20 +746,27 @@ test()
|
|||
fds.events = POLLIN;
|
||||
ret = poll(&fds, 1, 2000);
|
||||
|
||||
if (ret != 1)
|
||||
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))
|
||||
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");
|
||||
}
|
||||
|
||||
|
@ -751,14 +778,17 @@ reset()
|
|||
{
|
||||
int fd = open(RANGE_FINDER_DEVICE_PATH, O_RDONLY);
|
||||
|
||||
if (fd < 0)
|
||||
if (fd < 0) {
|
||||
err(1, "failed ");
|
||||
}
|
||||
|
||||
if (ioctl(fd, SENSORIOCRESET, 0) < 0)
|
||||
if (ioctl(fd, SENSORIOCRESET, 0) < 0) {
|
||||
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");
|
||||
}
|
||||
|
||||
exit(0);
|
||||
}
|
||||
|
@ -769,8 +799,9 @@ reset()
|
|||
void
|
||||
info()
|
||||
{
|
||||
if (g_dev == nullptr)
|
||||
if (g_dev == nullptr) {
|
||||
errx(1, "driver not running");
|
||||
}
|
||||
|
||||
printf("state @ %p\n", g_dev);
|
||||
g_dev->print_info();
|
||||
|
@ -786,32 +817,37 @@ mb12xx_main(int argc, char *argv[])
|
|||
/*
|
||||
* Start/load the driver.
|
||||
*/
|
||||
if (!strcmp(argv[1], "start"))
|
||||
if (!strcmp(argv[1], "start")) {
|
||||
mb12xx::start();
|
||||
|
||||
/*
|
||||
* Stop the driver
|
||||
*/
|
||||
if (!strcmp(argv[1], "stop"))
|
||||
mb12xx::stop();
|
||||
}
|
||||
|
||||
/*
|
||||
* Stop the driver
|
||||
*/
|
||||
if (!strcmp(argv[1], "stop")) {
|
||||
mb12xx::stop();
|
||||
}
|
||||
|
||||
/*
|
||||
* Test the driver/device.
|
||||
*/
|
||||
if (!strcmp(argv[1], "test"))
|
||||
if (!strcmp(argv[1], "test")) {
|
||||
mb12xx::test();
|
||||
}
|
||||
|
||||
/*
|
||||
* Reset the driver.
|
||||
*/
|
||||
if (!strcmp(argv[1], "reset"))
|
||||
if (!strcmp(argv[1], "reset")) {
|
||||
mb12xx::reset();
|
||||
}
|
||||
|
||||
/*
|
||||
* Print driver information.
|
||||
*/
|
||||
if (!strcmp(argv[1], "info") || !strcmp(argv[1], "status"))
|
||||
if (!strcmp(argv[1], "info") || !strcmp(argv[1], "status")) {
|
||||
mb12xx::info();
|
||||
}
|
||||
|
||||
errx(1, "unrecognized command, try 'start', 'test', 'reset' or 'info'");
|
||||
}
|
||||
|
|
|
@ -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
|
|
@ -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'");
|
||||
}
|
|
@ -1921,12 +1921,14 @@ PX4IO::print_status()
|
|||
io_reg_get(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_PWM_ALTRATE));
|
||||
#endif
|
||||
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++)
|
||||
printf(" %u", io_reg_get(PX4IO_PAGE_CONTROLS, i));
|
||||
for (unsigned i = 0; i < _max_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++) {
|
||||
unsigned base = PX4IO_P_RC_CONFIG_STRIDE * i;
|
||||
|
|
|
@ -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
|
|
@ -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'");
|
||||
}
|
|
@ -51,7 +51,6 @@
|
|||
#include <systemlib/err.h>
|
||||
#include <uORB/uORB.h>
|
||||
#include <uORB/topics/vehicle_status.h>
|
||||
#include <uORB/topics/actuator_armed.h>
|
||||
#include <poll.h>
|
||||
#include <drivers/drv_gpio.h>
|
||||
#include <modules/px4iofirmware/protocol.h>
|
||||
|
@ -63,8 +62,6 @@ struct gpio_led_s {
|
|||
int pin;
|
||||
struct vehicle_status_s status;
|
||||
int vehicle_status_sub;
|
||||
struct actuator_armed_s armed;
|
||||
int actuator_armed_sub;
|
||||
bool led_state;
|
||||
int counter;
|
||||
};
|
||||
|
@ -81,6 +78,7 @@ void gpio_led_cycle(FAR void *arg);
|
|||
int gpio_led_main(int argc, char *argv[])
|
||||
{
|
||||
if (argc < 2) {
|
||||
#ifdef CONFIG_ARCH_BOARD_PX4FMU_V1
|
||||
errx(1, "usage: gpio_led {start|stop} [-p <1|2|a1|a2|r1|r2>]\n"
|
||||
"\t-p\tUse pin:\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\ta2\tPX4IO ACC2\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 {
|
||||
|
||||
|
@ -98,37 +103,70 @@ int gpio_led_main(int argc, char *argv[])
|
|||
}
|
||||
|
||||
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 (!strcmp(argv[2], "-p")) {
|
||||
#ifdef CONFIG_ARCH_BOARD_PX4FMU_V1
|
||||
|
||||
if (!strcmp(argv[3], "1")) {
|
||||
use_io = false;
|
||||
pin = GPIO_EXT_1;
|
||||
pin_name = "PX4FMU GPIO_EXT1";
|
||||
|
||||
} else if (!strcmp(argv[3], "2")) {
|
||||
use_io = false;
|
||||
pin = GPIO_EXT_2;
|
||||
pin_name = "PX4FMU GPIO_EXT2";
|
||||
|
||||
} else if (!strcmp(argv[3], "a1")) {
|
||||
use_io = true;
|
||||
pin = PX4IO_P_SETUP_RELAYS_ACC1;
|
||||
pin_name = "PX4IO ACC1";
|
||||
|
||||
} else if (!strcmp(argv[3], "a2")) {
|
||||
use_io = true;
|
||||
pin = PX4IO_P_SETUP_RELAYS_ACC2;
|
||||
pin_name = "PX4IO ACC2";
|
||||
|
||||
} else if (!strcmp(argv[3], "r1")) {
|
||||
use_io = true;
|
||||
pin = PX4IO_P_SETUP_RELAYS_POWER1;
|
||||
pin_name = "PX4IO RELAY1";
|
||||
|
||||
} else if (!strcmp(argv[3], "r2")) {
|
||||
use_io = true;
|
||||
pin = PX4IO_P_SETUP_RELAYS_POWER2;
|
||||
pin_name = "PX4IO RELAY2";
|
||||
|
||||
} else {
|
||||
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 {
|
||||
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);
|
||||
}
|
||||
|
||||
|
@ -186,6 +209,7 @@ void gpio_led_start(FAR void *arg)
|
|||
|
||||
if (priv->use_io) {
|
||||
gpio_dev = PX4IO_DEVICE_PATH;
|
||||
|
||||
} else {
|
||||
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 */
|
||||
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));
|
||||
|
||||
/* subscribe to vehicle status topic */
|
||||
priv->vehicle_status_sub = orb_subscribe(ORB_ID(vehicle_status));
|
||||
|
||||
/* 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;
|
||||
|
||||
/* check for status updates*/
|
||||
bool status_updated;
|
||||
orb_check(priv->vehicle_status_sub, &status_updated);
|
||||
bool 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_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 */
|
||||
int pattern = 0;
|
||||
|
||||
if (priv->armed.armed) {
|
||||
if (priv->status.battery_warning == VEHICLE_BATTERY_WARNING_NONE) {
|
||||
if (priv->status.arming_state == ARMING_STATE_ARMED_ERROR) {
|
||||
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)
|
||||
|
||||
} else {
|
||||
pattern = 0x2A; // *_*_*_ fast blink (armed, battery warning)
|
||||
pattern = 0x3e; // *****_ slow blink (armed, battery low or failsafe)
|
||||
}
|
||||
|
||||
} else {
|
||||
if (priv->armed.ready_to_arm) {
|
||||
pattern = 0x00; // ______ off (disarmed, preflight check)
|
||||
} else if (priv->status.arming_state == ARMING_STATE_STANDBY) {
|
||||
pattern = 0x38; // ***___ slow blink (disarmed, ready)
|
||||
|
||||
} else if (priv->armed.ready_to_arm && priv->status.battery_warning == VEHICLE_BATTERY_WARNING_NONE) {
|
||||
pattern = 0x38; // ***___ slow blink (disarmed, ready)
|
||||
} else if (priv->status.arming_state == ARMING_STATE_STANDBY_ERROR) {
|
||||
pattern = 0x28; // *_*___ slow double blink (disarmed, error)
|
||||
|
||||
} else {
|
||||
pattern = 0x28; // *_*___ slow double blink (disarmed, not good to arm)
|
||||
}
|
||||
}
|
||||
|
||||
/* blink pattern */
|
||||
|
@ -266,6 +287,7 @@ void gpio_led_cycle(FAR void *arg)
|
|||
|
||||
if (led_state_new) {
|
||||
ioctl(priv->gpio_fd, GPIO_SET, priv->pin);
|
||||
|
||||
} else {
|
||||
ioctl(priv->gpio_fd, GPIO_CLEAR, priv->pin);
|
||||
}
|
||||
|
@ -273,8 +295,9 @@ void gpio_led_cycle(FAR void *arg)
|
|||
|
||||
priv->counter++;
|
||||
|
||||
if (priv->counter > 5)
|
||||
if (priv->counter > 5) {
|
||||
priv->counter = 0;
|
||||
}
|
||||
|
||||
/* repeat cycle at 5 Hz */
|
||||
if (gpio_led_started) {
|
||||
|
|
|
@ -267,7 +267,7 @@ mixer_callback(uintptr_t handle,
|
|||
uint8_t control_index,
|
||||
float &control)
|
||||
{
|
||||
if (control_group > 3)
|
||||
if (control_group >= PX4IO_CONTROL_GROUPS)
|
||||
return -1;
|
||||
|
||||
switch (source) {
|
||||
|
|
|
@ -53,7 +53,7 @@
|
|||
*/
|
||||
#define PX4IO_SERVO_COUNT 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_MAPPED_CONTROL_CHANNELS 8 /**< This is the maximum number of channels mapped/used */
|
||||
|
||||
|
|
|
@ -74,8 +74,9 @@ bool logbuffer_write(struct logbuffer_s *lb, void *ptr, int size)
|
|||
// bytes available to write
|
||||
int available = lb->read_ptr - lb->write_ptr - 1;
|
||||
|
||||
if (available < 0)
|
||||
if (available < 0) {
|
||||
available += lb->size;
|
||||
}
|
||||
|
||||
if (size > available) {
|
||||
// buffer overflow
|
||||
|
|
File diff suppressed because it is too large
Load Diff
|
@ -268,13 +268,13 @@ struct log_DIST_s {
|
|||
/* --- TELE - TELEMETRY STATUS --- */
|
||||
#define LOG_TELE_MSG 22
|
||||
struct log_TELE_s {
|
||||
uint8_t rssi;
|
||||
uint8_t remote_rssi;
|
||||
uint8_t noise;
|
||||
uint8_t remote_noise;
|
||||
uint16_t rxerrors;
|
||||
uint16_t fixed;
|
||||
uint8_t txbuf;
|
||||
uint8_t rssi;
|
||||
uint8_t remote_rssi;
|
||||
uint8_t noise;
|
||||
uint8_t remote_noise;
|
||||
uint16_t rxerrors;
|
||||
uint16_t fixed;
|
||||
uint8_t txbuf;
|
||||
};
|
||||
|
||||
/********** SYSTEM MESSAGES, ID > 0x80 **********/
|
||||
|
|
|
@ -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;
|
||||
}
|
|
@ -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
|
Loading…
Reference in New Issue