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

Merged beta_mavlink2 / master

Browse Source
This commit is contained in:
Lorenz Meier
2014-03-16 12:12:28 +01:00
parent 939b8464a2 955bd402d2
commit ed197b5b59
22 changed files with 2919 additions and 744 deletions
Download Patch File Download Diff File Expand all files Collapse all files
ROMFS/px4fmu_common/init.d/10016_3dr_iris
+2 -2
View File
@@ -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
ROMFS/px4fmu_common/init.d/4008_ardrone
+3 -1
View File
@@ -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
ROMFS/px4fmu_common/init.d/rcS
+7
View File
@@ -120,6 +120,7 @@ then
set MAVLINK_FLAGS default
set EXIT_ON_END no
set MAV_TYPE none
set LOAD_DEFAULT_APPS yes
#
# Set DO_AUTOCONFIG flag to use it in AUTOSTART scripts
@@ -473,8 +474,11 @@ then
sh /etc/init.d/rc.interface
# Start standard fixedwing apps
if [ LOAD_DEFAULT_APPS == yes ]
then
sh /etc/init.d/rc.fw_apps
fi
fi
#
# Multicopters setup
@@ -529,8 +533,11 @@ then
sh /etc/init.d/rc.interface
# Start standard multicopter apps
if [ LOAD_DEFAULT_APPS == yes ]
then
sh /etc/init.d/rc.mc_apps
fi
fi
#
# Generic setup (autostart ID not found)
Tools/fetch_log.py
+133
View File
@@ -0,0 +1,133 @@
#!/usr/bin/env python
############################################################################
#
# Copyright (C) 2012, 2013 PX4 Development Team. All rights reserved.
#
# Redistribution and use in source and binary forms, with or without
# modification, are permitted provided that the following conditions
# are met:
#
# 1. Redistributions of source code must retain the above copyright
# notice, this list of conditions and the following disclaimer.
# 2. Redistributions in binary form must reproduce the above copyright
# notice, this list of conditions and the following disclaimer in
# the documentation and/or other materials provided with the
# distribution.
# 3. Neither the name PX4 nor the names of its contributors may be
# used to endorse or promote products derived from this software
# without specific prior written permission.
#
# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
# "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
# LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
# FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
# COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
# INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
# BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
# OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
# AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
# LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
# ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
# POSSIBILITY OF SUCH DAMAGE.
#
############################################################################
#
# Log fetcher
#
# Print list of logs:
# python fetch_log.py
#
# Fetch log:
# python fetch_log.py sess001/log001.bin
#
import serial, time, sys, os
def wait_for_string(ser, s, timeout=1.0, debug=False):
t0 = time.time()
buf = []
res = []
n = 0
while (True):
c = ser.read()
if debug:
sys.stderr.write(c)
buf.append(c)
if len(buf) > len(s):
res.append(buf.pop(0))
n += 1
if n % 10000 == 0:
sys.stderr.write(str(n) + "\n")
if "".join(buf) == s:
break
if timeout > 0.0 and time.time() - t0 > timeout:
raise Exception("Timeout while waiting for: " + s)
return "".join(res)
def exec_cmd(ser, cmd, timeout):
ser.write(cmd + "\n")
ser.flush()
wait_for_string(ser, cmd + "\r\n", timeout)
return wait_for_string(ser, "nsh> \x1b[K", timeout)
def ls_dir(ser, dir, timeout=1.0):
res = []
for line in exec_cmd(ser, "ls -l " + dir, timeout).splitlines()[1:]:
res.append((line[20:], int(line[11:19].strip()), line[1] == "d"))
return res
def list_logs(ser):
logs_dir = "/fs/microsd/log"
res = []
for d in ls_dir(ser, logs_dir):
if d[2]:
sess_dir = d[0][:-1]
for f in ls_dir(ser, logs_dir + "/" + sess_dir):
log_file = f[0]
log_size = f[1]
res.append(sess_dir + "/" + log_file + "\t" + str(log_size))
return "\n".join(res)
def fetch_log(ser, fn, timeout):
cmd = "dumpfile " + fn
ser.write(cmd + "\n")
ser.flush()
wait_for_string(ser, cmd + "\r\n", timeout, True)
res = wait_for_string(ser, "\n", timeout, True)
data = []
if res.startswith("OK"):
size = int(res.split()[1])
n = 0
print "Reading data:"
while (n < size):
buf = ser.read(min(size - n, 8192))
data.append(buf)
n += len(buf)
sys.stdout.write(".")
sys.stdout.flush()
print
else:
raise Exception("Error reading log")
wait_for_string(ser, "nsh> \x1b[K", timeout)
return "".join(data)
def main():
dev = "/dev/tty.usbmodem1"
ser = serial.Serial(dev, "115200", timeout=0.2)
if len(sys.argv) < 2:
print list_logs(ser)
else:
log_file = sys.argv[1]
data = fetch_log(ser, "/fs/microsd/log/" + log_file, 1.0)
try:
os.mkdir(log_file.split("/")[0])
except:
pass
fout = open(log_file, "wb")
fout.write(data)
fout.close()
ser.close()
if __name__ == "__main__":
main()
makefiles/config_px4fmu-v1_default.mk
+1
View File
@@ -56,6 +56,7 @@ MODULES += systemcmds/tests
MODULES += systemcmds/config
MODULES += systemcmds/nshterm
MODULES += systemcmds/hw_ver
MODULES += systemcmds/dumpfile
#
# General system control
makefiles/config_px4fmu-v2_default.mk
+3
View File
@@ -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
@@ -70,6 +72,7 @@ MODULES += systemcmds/hw_ver
MODULES += modules/commander
MODULES += modules/navigator
MODULES += modules/mavlink
MODULES += modules/gpio_led
#
# Estimation modules (EKF/ SO3 / other filters)
src/drivers/drv_px4flow.h
+84
View File
@@ -0,0 +1,84 @@
/****************************************************************************
*
* Copyright (C) 2013 PX4 Development Team. All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions
* are met:
*
* 1. Redistributions of source code must retain the above copyright
* notice, this list of conditions and the following disclaimer.
* 2. Redistributions in binary form must reproduce the above copyright
* notice, this list of conditions and the following disclaimer in
* the documentation and/or other materials provided with the
* distribution.
* 3. Neither the name PX4 nor the names of its contributors may be
* used to endorse or promote products derived from this software
* without specific prior written permission.
*
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
* OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
* AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
* POSSIBILITY OF SUCH DAMAGE.
*
****************************************************************************/
/**
* @file Rangefinder driver interface.
*/
#ifndef _DRV_PX4FLOW_H
#define _DRV_PX4FLOW_H
#include <stdint.h>
#include <sys/ioctl.h>
#include "drv_sensor.h"
#include "drv_orb_dev.h"
#define PX4FLOW_DEVICE_PATH "/dev/px4flow"
/**
* Optical flow in NED body frame in SI units.
*
* @see http://en.wikipedia.org/wiki/International_System_of_Units
*/
struct px4flow_report {
uint64_t timestamp; /**< in microseconds since system start */
int16_t flow_raw_x; /**< flow in pixels in X direction, not rotation-compensated */
int16_t flow_raw_y; /**< flow in pixels in Y direction, not rotation-compensated */
float flow_comp_x_m; /**< speed over ground in meters, rotation-compensated */
float flow_comp_y_m; /**< speed over ground in meters, rotation-compensated */
float ground_distance_m; /**< Altitude / distance to ground in meters */
uint8_t quality; /**< Quality of the measurement, 0: bad quality, 255: maximum quality */
uint8_t sensor_id; /**< id of the sensor emitting the flow value */
};
/*
* ObjDev tag for px4flow data.
*/
ORB_DECLARE(optical_flow);
/*
* ioctl() definitions
*
* px4flow drivers also implement the generic sensor driver
* interfaces from drv_sensor.h
*/
#define _PX4FLOWIOCBASE (0x7700)
#define __PX4FLOWIOC(_n) (_IOC(_PX4FLOWIOCBASE, _n))
#endif /* _DRV_PX4FLOW_H */
src/drivers/mb12xx/mb12xx.cpp
+83 -47
View File
@@ -200,7 +200,7 @@ 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,9 +212,10 @@ MB12XX::~MB12XX()
stop();
/* free any existing reports */
if (_reports != nullptr)
if (_reports != nullptr) {
delete _reports;
}
}
int
MB12XX::init()
@@ -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 */
@@ -307,8 +311,9 @@ 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;
}
@@ -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,21 +345,25 @@ 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))
if ((arg < 1) || (arg > 100)) {
return -EINVAL;
}
irqstate_t flags = irqsave();
if (!_reports->resize(arg)) {
irqrestore(flags);
return -ENOMEM;
}
irqrestore(flags);
return OK;
@@ -365,18 +376,18 @@ MB12XX::ioctl(struct file *filp, int cmd, unsigned long arg)
/* XXX implement this */
return -EINVAL;
case RANGEFINDERIOCSETMINIUMDISTANCE:
{
case RANGEFINDERIOCSETMINIUMDISTANCE: {
set_minimum_distance(*(float *)arg);
return 0;
}
break;
case RANGEFINDERIOCSETMAXIUMDISTANCE:
{
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,12 +465,12 @@ 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;
@@ -476,8 +488,7 @@ MB12XX::collect()
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);
@@ -525,11 +536,13 @@ MB12XX::start()
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"))
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'");
}
src/drivers/px4flow/module.mk
+40
View File
@@ -0,0 +1,40 @@
############################################################################
#
# Copyright (c) 2013 PX4 Development Team. All rights reserved.
#
# Redistribution and use in source and binary forms, with or without
# modification, are permitted provided that the following conditions
# are met:
#
# 1. Redistributions of source code must retain the above copyright
# notice, this list of conditions and the following disclaimer.
# 2. Redistributions in binary form must reproduce the above copyright
# notice, this list of conditions and the following disclaimer in
# the documentation and/or other materials provided with the
# distribution.
# 3. Neither the name PX4 nor the names of its contributors may be
# used to endorse or promote products derived from this software
# without specific prior written permission.
#
# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
# "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
# LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
# FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
# COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
# INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
# BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
# OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
# AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
# LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
# ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
# POSSIBILITY OF SUCH DAMAGE.
#
############################################################################
#
# Makefile to build the PX4FLOW driver.
#
MODULE_COMMAND = px4flow
SRCS = px4flow.cpp
src/drivers/px4flow/px4flow.cpp
+806
View File
File diff suppressed because it is too large Load Diff
src/drivers/px4io/px4io.cpp
+4 -2
View File
@@ -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));
printf(" %d", (int16_t) io_reg_get(PX4IO_PAGE_CONTROLS, group * PX4IO_PROTOCOL_MAX_CONTROL_COUNT + i));
printf("\n");
}
for (unsigned i = 0; i < _max_rc_input; i++) {
unsigned base = PX4IO_P_RC_CONFIG_STRIDE * i;
src/drivers/sf0x/module.mk
+40
View File
@@ -0,0 +1,40 @@
############################################################################
#
# Copyright (c) 2014 PX4 Development Team. All rights reserved.
#
# Redistribution and use in source and binary forms, with or without
# modification, are permitted provided that the following conditions
# are met:
#
# 1. Redistributions of source code must retain the above copyright
# notice, this list of conditions and the following disclaimer.
# 2. Redistributions in binary form must reproduce the above copyright
# notice, this list of conditions and the following disclaimer in
# the documentation and/or other materials provided with the
# distribution.
# 3. Neither the name PX4 nor the names of its contributors may be
# used to endorse or promote products derived from this software
# without specific prior written permission.
#
# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
# "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
# LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
# FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
# COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
# INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
# BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
# OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
# AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
# LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
# ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
# POSSIBILITY OF SUCH DAMAGE.
#
############################################################################
#
# Makefile to build the Lightware laser range finder driver.
#
MODULE_COMMAND = sf0x
SRCS = sf0x.cpp
src/drivers/sf0x/sf0x.cpp
+977
View File
File diff suppressed because it is too large Load Diff
src/modules/gpio_led/gpio_led.c
+64 -41
View File
@@ -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->armed.ready_to_arm && priv->status.battery_warning == VEHICLE_BATTERY_WARNING_NONE) {
} else if (priv->status.arming_state == ARMING_STATE_STANDBY) {
pattern = 0x38; // ***___ slow blink (disarmed, ready)
} else {
pattern = 0x28; // *_*___ slow double blink (disarmed, not good to arm)
}
} else if (priv->status.arming_state == ARMING_STATE_STANDBY_ERROR) {
pattern = 0x28; // *_*___ slow double blink (disarmed, error)
}
/* 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) {
src/modules/mavlink/mavlink_main.cpp
+17 -20
View File
@@ -1583,7 +1583,7 @@ Mavlink::configure_stream_threadsafe(const char *stream_name, const float rate)
/* copy stream name */
unsigned n = strlen(stream_name) + 1;
char *s = new char[n];
memcpy(s, stream_name, n);
strcpy(s, stream_name);
/* set subscription task */
_subscribe_to_stream_rate = rate;
@@ -1945,36 +1945,33 @@ Mavlink::stream(int argc, char *argv[])
const char *stream_name = nullptr;
int ch;
argc -= 1;
argv += 1;
argc -= 2;
argv += 2;
/* don't exit from getopt loop to leave getopt global variables in consistent state,
* set error flag instead */
bool err_flag = false;
while ((ch = getopt(argc, argv, "r:d:s:")) != EOF) {
switch (ch) {
case 'r':
rate = strtod(optarg, nullptr);
int i = 0;
while (i < argc) {
if (0 == strcmp(argv[i], "-r") && i < argc - 1 ) {
rate = strtod(argv[i+1], nullptr);
if (rate < 0.0f) {
err_flag = true;
}
break;
case 'd':
device_name = optarg;
break;
case 's':
stream_name = optarg;
break;
default:
i++;
} else if (0 == strcmp(argv[i], "-d") && i < argc - 1 ) {
device_name = argv[i+1];
i++;
} else if (0 == strcmp(argv[i], "-s") && i < argc - 1 ) {
stream_name = argv[i+1];
i++;
} else {
err_flag = true;
break;
}
i++;
}
if (!err_flag && rate >= 0.0 && stream_name != nullptr) {
src/modules/px4iofirmware/mixer.cpp
+1 -1
View File
@@ -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) {
src/modules/px4iofirmware/px4io.h
+1 -1
View File
@@ -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 */
src/modules/sdlog2/logbuffer.c
+2 -1
View File
@@ -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
src/modules/sdlog2/sdlog2.c
+139 -273
View File
File diff suppressed because it is too large Load Diff
src/systemcmds/dumpfile/dumpfile.c
+116
View File
@@ -0,0 +1,116 @@
/****************************************************************************
*
* Copyright (c) 2014 PX4 Development Team. All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions
* are met:
*
* 1. Redistributions of source code must retain the above copyright
* notice, this list of conditions and the following disclaimer.
* 2. Redistributions in binary form must reproduce the above copyright
* notice, this list of conditions and the following disclaimer in
* the documentation and/or other materials provided with the
* distribution.
* 3. Neither the name PX4 nor the names of its contributors may be
* used to endorse or promote products derived from this software
* without specific prior written permission.
*
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
* OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
* AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
* POSSIBILITY OF SUCH DAMAGE.
*
****************************************************************************/
/**
* @file dumpfile.c
*
* Dump file utility. Prints file size and contents in binary mode (don't replace LF with CR LF) to stdout.
*
* @author Anton Babushkin <anton.babushkin@me.com>
*/
#include <nuttx/config.h>
#include <unistd.h>
#include <stdio.h>
#include <string.h>
#include <stdlib.h>
#include <fcntl.h>
#include <termios.h>
#include <systemlib/err.h>
__EXPORT int dumpfile_main(int argc, char *argv[]);
int
dumpfile_main(int argc, char *argv[])
{
if (argc < 2) {
errx(1, "usage: dumpfile <filename>");
}
/* open input file */
FILE *f;
f = fopen(argv[1], "r");
if (f == NULL) {
printf("ERROR\n");
exit(1);
}
/* get file size */
fseek(f, 0L, SEEK_END);
int size = ftell(f);
fseek(f, 0L, SEEK_SET);
printf("OK %d\n", size);
/* configure stdout */
int out = fileno(stdout);
struct termios tc;
struct termios tc_old;
tcgetattr(out, &tc);
/* save old terminal attributes to restore it later on exit */
memcpy(&tc_old, &tc, sizeof(tc));
/* don't add CR on each LF*/
tc.c_oflag &= ~ONLCR;
if (tcsetattr(out, TCSANOW, &tc) < 0) {
warnx("ERROR setting stdout attributes");
exit(1);
}
char buf[512];
int nread;
/* dump file */
while ((nread = fread(buf, 1, sizeof(buf), f)) > 0) {
if (write(out, buf, nread) <= 0) {
warnx("error dumping file");
break;
}
}
fsync(out);
fclose(f);
/* restore old terminal attributes */
if (tcsetattr(out, TCSANOW, &tc_old) < 0) {
warnx("ERROR restoring stdout attributes");
exit(1);
}
return OK;
}
src/systemcmds/dumpfile/module.mk
+41
View File
@@ -0,0 +1,41 @@
############################################################################
#
# Copyright (c) 2014 PX4 Development Team. All rights reserved.
#
# Redistribution and use in source and binary forms, with or without
# modification, are permitted provided that the following conditions
# are met:
#
# 1. Redistributions of source code must retain the above copyright
# notice, this list of conditions and the following disclaimer.
# 2. Redistributions in binary form must reproduce the above copyright
# notice, this list of conditions and the following disclaimer in
# the documentation and/or other materials provided with the
# distribution.
# 3. Neither the name PX4 nor the names of its contributors may be
# used to endorse or promote products derived from this software
# without specific prior written permission.
#
# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
# "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
# LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
# FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
# COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
# INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
# BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
# OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
# AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
# LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
# ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
# POSSIBILITY OF SUCH DAMAGE.
#
############################################################################
#
# Dump file utility
#
MODULE_COMMAND = dumpfile
SRCS = dumpfile.c
MAXOPTIMIZATION = -Os