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

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

This commit is contained in:
Thomas Gubler 2014-12-04 13:21:01 +01:00
parent c37ff71e62 b4da5afcce
commit 690dc7c6ab
35 changed files with 1744 additions and 364 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

2
NuttX

@ -1 +1 @@
Subproject commit ec6b670f6d1e964700c0b0a50d14db12761e3097
Subproject commit ae4b05e2c51d07369b5d131052099ac346b0841c

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

@ -301,7 +301,6 @@ then
if [ $OUTPUT_MODE == io -o $OUTPUT_MODE == uavcan_esc ]
then
echo "[init] Use PX4IO PWM as primary output"
if px4io start
then
echo "[init] PX4IO started"
@ -314,7 +313,6 @@ then
if [ $OUTPUT_MODE == fmu -o $OUTPUT_MODE == ardrone ]
then
echo "[init] Use FMU as primary output"
if fmu mode_$FMU_MODE
then
echo "[init] FMU mode_$FMU_MODE started"
@ -338,7 +336,6 @@ then
if [ $OUTPUT_MODE == mkblctrl ]
then
echo "[init] Use MKBLCTRL as primary output"
set MKBLCTRL_ARG ""
if [ $MKBLCTRL_MODE == x ]
then
@ -361,7 +358,6 @@ then
if [ $OUTPUT_MODE == hil ]
then
echo "[init] Use HIL as primary output"
if hil mode_port2_pwm8
then
echo "[init] HIL output started"
@ -380,7 +376,6 @@ then
then
if px4io start
then
echo "[init] PX4IO started"
sh /etc/init.d/rc.io
else
echo "[init] ERROR: PX4IO start failed"

2
makefiles/config_px4fmu-v1_default.mk
View File

@ -24,6 +24,8 @@ MODULES += drivers/l3gd20
MODULES += drivers/mpu6000
MODULES += drivers/hmc5883
MODULES += drivers/ms5611
#MODULES += drivers/ll40ls
MODULES += drivers/trone
#MODULES += drivers/mb12xx
MODULES += drivers/gps
MODULES += drivers/hil

5
makefiles/config_px4fmu-v2_default.mk
View File

@ -27,13 +27,14 @@ MODULES += drivers/l3gd20
MODULES += drivers/hmc5883
MODULES += drivers/ms5611
MODULES += drivers/mb12xx
MODULES += drivers/sf0x
# MODULES += drivers/sf0x
MODULES += drivers/ll40ls
# MODULES += drivers/trone
MODULES += drivers/gps
MODULES += drivers/hil
MODULES += drivers/hott/hott_telemetry
MODULES += drivers/hott/hott_sensors
MODULES += drivers/blinkm
# MODULES += drivers/blinkm
MODULES += drivers/airspeed
MODULES += drivers/ets_airspeed
MODULES += drivers/meas_airspeed

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

@ -159,13 +159,15 @@ out:
int
Airspeed::probe()
{
/* on initial power up the device needs more than one retry
for detection. Once it is running then retries aren't
needed
/* on initial power up the device may need more than one retry
for detection. Once it is running the number of retries can
be reduced
*/
_retries = 4;
int ret = measure();
_retries = 0;
// drop back to 2 retries once initialised
_retries = 2;
return ret;
}

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

@ -1349,7 +1349,7 @@ HMC5883 *g_dev_ext = nullptr;
void start(int bus, enum Rotation rotation);
void test(int bus);
void reset(int bus);
void info(int bus);
int info(int bus);
int calibrate(int bus);
void usage();
@ -1595,17 +1595,23 @@ reset(int bus)
/**
* Print a little info about the driver.
*/
void
int
info(int bus)
{
HMC5883 *g_dev = (bus == PX4_I2C_BUS_ONBOARD?g_dev_int:g_dev_ext);
if (g_dev == nullptr)
errx(1, "driver not running");
int ret = 1;
printf("state @ %p\n", g_dev);
g_dev->print_info();
HMC5883 *g_dev = (bus == PX4_I2C_BUS_ONBOARD ? g_dev_int : g_dev_ext);
if (g_dev == nullptr) {
warnx("not running on bus %d", bus);
} else {
exit(0);
warnx("running on bus: %d (%s)\n", bus, ((PX4_I2C_BUS_ONBOARD) ? "onboard" : "offboard"));
g_dev->print_info();
ret = 0;
}
return ret;
}
void
@ -1685,8 +1691,21 @@ hmc5883_main(int argc, char *argv[])
/*
* Print driver information.
*/
if (!strcmp(verb, "info") || !strcmp(verb, "status"))
hmc5883::info(bus);
if (!strcmp(verb, "info") || !strcmp(verb, "status")) {
if (bus == -1) {
int ret = 0;
if (hmc5883::info(PX4_I2C_BUS_ONBOARD)) {
ret = 1;
}
if (hmc5883::info(PX4_I2C_BUS_EXPANSION)) {
ret = 1;
}
exit(ret);
} else {
exit(hmc5883::info(bus));
}
}
/*
* Autocalibrate the scaling

343
src/drivers/px4flow/px4flow.cpp
View File

@ -1,6 +1,6 @@
/****************************************************************************
*
* Copyright (c) 2013 PX4 Development Team. All rights reserved.
* Copyright (c) 2013, 2014 PX4 Development Team. All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions
@ -73,15 +73,13 @@
#include <board_config.h>
/* Configuration Constants */
#define PX4FLOW_BUS PX4_I2C_BUS_EXPANSION
#define I2C_FLOW_ADDRESS 0x42 //* 7-bit address. 8-bit address is 0x84
//range 0x42 - 0x49
/* PX4FLOW Registers addresses */
#define PX4FLOW_REG 0x00 /* Measure Register */
#define PX4FLOW_CONVERSION_INTERVAL 8000 /* 8ms 125Hz */
#define PX4FLOW_REG 0x16 /* Measure Register 22*/
#define PX4FLOW_CONVERSION_INTERVAL 20000 //in microseconds! 20000 = 50 Hz 100000 = 10Hz
/* oddly, ERROR is not defined for c++ */
#ifdef ERROR
# undef ERROR
@ -92,28 +90,42 @@ static const int ERROR = -1;
# 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;
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
typedef struct i2c_integral_frame {
uint16_t frame_count_since_last_readout;
int16_t pixel_flow_x_integral;
int16_t pixel_flow_y_integral;
int16_t gyro_x_rate_integral;
int16_t gyro_y_rate_integral;
int16_t gyro_z_rate_integral;
uint32_t integration_timespan;
uint32_t time_since_last_sonar_update;
uint16_t ground_distance;
int16_t gyro_temperature;
uint8_t qual;
} __attribute__((packed));
struct i2c_integral_frame f_integral;
class PX4FLOW: public device::I2C
{
public:
PX4FLOW(int bus = PX4FLOW_BUS, int address = I2C_FLOW_ADDRESS);
PX4FLOW(int bus, int address = I2C_FLOW_ADDRESS);
virtual ~PX4FLOW();
virtual int init();
@ -122,8 +134,8 @@ public:
virtual int ioctl(struct file *filp, int cmd, unsigned long arg);
/**
* Diagnostics - print some basic information about the driver.
*/
* Diagnostics - print some basic information about the driver.
*/
void print_info();
protected:
@ -144,42 +156,41 @@ private:
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.
*/
* 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.
*/
* 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.
*/
* Stop the automatic measurement state machine.
*/
void stop();
/**
* Perform a poll cycle; collect from the previous measurement
* and start a new one.
*/
* 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);
* 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);
};
@ -189,7 +200,7 @@ private:
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
I2C("PX4FLOW", PX4FLOW_DEVICE_PATH, bus, address, 400000), //400khz
_reports(nullptr),
_sensor_ok(false),
_measure_ticks(0),
@ -228,21 +239,12 @@ PX4FLOW::init()
}
/* allocate basic report buffers */
_reports = new RingBuffer(2, sizeof(struct optical_flow_s));
_reports = new RingBuffer(2, sizeof(optical_flow_s));
if (_reports == nullptr) {
goto out;
}
/* get a publish handle on the px4flow topic */
struct optical_flow_s zero_report;
memset(&zero_report, 0, sizeof(zero_report));
_px4flow_topic = orb_advertise(ORB_ID(optical_flow), &zero_report);
if (_px4flow_topic < 0) {
warnx("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;
@ -410,9 +412,6 @@ PX4FLOW::read(struct file *filp, char *buffer, size_t buflen)
break;
}
/* wait for it to complete */
usleep(PX4FLOW_CONVERSION_INTERVAL);
/* run the collection phase */
if (OK != collect()) {
ret = -EIO;
@ -442,6 +441,7 @@ PX4FLOW::measure()
if (OK != ret) {
perf_count(_comms_errors);
debug("i2c::transfer returned %d", ret);
return ret;
}
@ -453,14 +453,20 @@ PX4FLOW::measure()
int
PX4FLOW::collect()
{
int ret = -EIO;
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};
uint8_t val[47] = { 0 };
perf_begin(_sample_perf);
ret = transfer(nullptr, 0, &val[0], 22);
if (PX4FLOW_REG == 0x00) {
ret = transfer(nullptr, 0, &val[0], 47); // read 47 bytes (22+25 : frame1 + frame2)
}
if (PX4FLOW_REG == 0x16) {
ret = transfer(nullptr, 0, &val[0], 25); // read 25 bytes (only frame2)
}
if (ret < 0) {
debug("error reading from sensor: %d", ret);
@ -469,36 +475,74 @@ PX4FLOW::collect()
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];
if (PX4FLOW_REG == 0) {
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];
f_integral.frame_count_since_last_readout = val[23] << 8 | val[22];
f_integral.pixel_flow_x_integral = val[25] << 8 | val[24];
f_integral.pixel_flow_y_integral = val[27] << 8 | val[26];
f_integral.gyro_x_rate_integral = val[29] << 8 | val[28];
f_integral.gyro_y_rate_integral = val[31] << 8 | val[30];
f_integral.gyro_z_rate_integral = val[33] << 8 | val[32];
f_integral.integration_timespan = val[37] << 24 | val[36] << 16
| val[35] << 8 | val[34];
f_integral.time_since_last_sonar_update = val[41] << 24 | val[40] << 16
| val[39] << 8 | val[38];
f_integral.ground_distance = val[43] << 8 | val[42];
f_integral.gyro_temperature = val[45] << 8 | val[44];
f_integral.qual = val[46];
}
if (PX4FLOW_REG == 0x16) {
f_integral.frame_count_since_last_readout = val[1] << 8 | val[0];
f_integral.pixel_flow_x_integral = val[3] << 8 | val[2];
f_integral.pixel_flow_y_integral = val[5] << 8 | val[4];
f_integral.gyro_x_rate_integral = val[7] << 8 | val[6];
f_integral.gyro_y_rate_integral = val[9] << 8 | val[8];
f_integral.gyro_z_rate_integral = val[11] << 8 | val[10];
f_integral.integration_timespan = val[15] << 24 | val[14] << 16 | val[13] << 8 | val[12];
f_integral.time_since_last_sonar_update = val[19] << 24 | val[18] << 16 | val[17] << 8 | val[16];
f_integral.ground_distance = val[21] << 8 | val[20];
f_integral.gyro_temperature = val[23] << 8 | val[22];
f_integral.qual = val[24];
}
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 optical_flow_s 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();
report.pixel_flow_x_integral = static_cast<float>(f_integral.pixel_flow_x_integral) / 10000.0f;//convert to radians
report.pixel_flow_y_integral = static_cast<float>(f_integral.pixel_flow_y_integral) / 10000.0f;//convert to radians
report.frame_count_since_last_readout = f_integral.frame_count_since_last_readout;
report.ground_distance_m = static_cast<float>(f_integral.ground_distance) / 1000.0f;//convert to meters
report.quality = f_integral.qual; //0:bad ; 255 max quality
report.gyro_x_rate_integral = static_cast<float>(f_integral.gyro_x_rate_integral) / 10000.0f; //convert to radians
report.gyro_y_rate_integral = static_cast<float>(f_integral.gyro_y_rate_integral) / 10000.0f; //convert to radians
report.gyro_z_rate_integral = static_cast<float>(f_integral.gyro_z_rate_integral) / 10000.0f; //convert to radians
report.integration_timespan = f_integral.integration_timespan; //microseconds
report.time_since_last_sonar_update = f_integral.time_since_last_sonar_update;//microseconds
report.gyro_temperature = f_integral.gyro_temperature;//Temperature * 100 in centi-degrees Celsius
report.sensor_id = 0;
/* publish it */
orb_publish(ORB_ID(optical_flow), _px4flow_topic, &report);
if (_px4flow_topic < 0) {
_px4flow_topic = orb_advertise(ORB_ID(optical_flow), &report);
} else {
/* publish it */
orb_publish(ORB_ID(optical_flow), _px4flow_topic, &report);
}
/* post a report to the ring */
if (_reports->force(&report)) {
@ -558,50 +602,21 @@ PX4FLOW::cycle_trampoline(void *arg)
void
PX4FLOW::cycle()
{
/* collection phase? */
if (_collect_phase) {
/* perform collection */
if (OK != collect()) {
debug("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()) {
debug("measure error");
}
/* next phase is collection */
_collect_phase = true;
/* perform collection */
if (OK != collect()) {
debug("collection error");
/* restart the measurement state machine */
start();
return;
}
work_queue(HPWORK, &_work, (worker_t)&PX4FLOW::cycle_trampoline, this,
_measure_ticks);
/* 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
@ -647,14 +662,41 @@ start()
}
/* create the driver */
g_dev = new PX4FLOW(PX4FLOW_BUS);
g_dev = new PX4FLOW(PX4_I2C_BUS_EXPANSION);
if (g_dev == nullptr) {
goto fail;
}
if (OK != g_dev->init()) {
goto fail;
#ifdef PX4_I2C_BUS_ESC
delete g_dev;
/* try 2nd bus */
g_dev = new PX4FLOW(PX4_I2C_BUS_ESC);
if (g_dev == nullptr) {
goto fail;
}
if (OK != g_dev->init()) {
#endif
delete g_dev;
/* try 3rd bus */
g_dev = new PX4FLOW(PX4_I2C_BUS_ONBOARD);
if (g_dev == nullptr) {
goto fail;
}
if (OK != g_dev->init()) {
goto fail;
}
#ifdef PX4_I2C_BUS_ESC
}
#endif
}
/* set the poll rate to default, starts automatic data collection */
@ -683,7 +725,8 @@ fail:
/**
* Stop the driver
*/
void stop()
void
stop()
{
if (g_dev != nullptr) {
delete g_dev;
@ -714,6 +757,7 @@ test()
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));
@ -723,18 +767,18 @@ test()
}
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);
warnx("pixel_flow_x_integral: %i", f_integral.pixel_flow_x_integral);
warnx("pixel_flow_y_integral: %i", f_integral.pixel_flow_y_integral);
warnx("framecount_integral: %u",
f_integral.frame_count_since_last_readout);
/* start the sensor polling at 2Hz */
if (OK != ioctl(fd, SENSORIOCSPOLLRATE, 2)) {
errx(1, "failed to set 2Hz poll rate");
/* start the sensor polling at 10Hz */
if (OK != ioctl(fd, SENSORIOCSPOLLRATE, 10)) {
errx(1, "failed to set 10Hz poll rate");
}
/* read the sensor 5x and report each value */
for (unsigned i = 0; i < 5; i++) {
for (unsigned i = 0; i < 10; i++) {
struct pollfd fds;
/* wait for data to be ready */
@ -754,9 +798,22 @@ test()
}
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);
warnx("framecount_total: %u", f.frame_count);
warnx("framecount_integral: %u",
f_integral.frame_count_since_last_readout);
warnx("pixel_flow_x_integral: %i", f_integral.pixel_flow_x_integral);
warnx("pixel_flow_y_integral: %i", f_integral.pixel_flow_y_integral);
warnx("gyro_x_rate_integral: %i", f_integral.gyro_x_rate_integral);
warnx("gyro_y_rate_integral: %i", f_integral.gyro_y_rate_integral);
warnx("gyro_z_rate_integral: %i", f_integral.gyro_z_rate_integral);
warnx("integration_timespan [us]: %u", f_integral.integration_timespan);
warnx("ground_distance: %0.2f m",
(double) f_integral.ground_distance / 1000);
warnx("time since last sonar update [us]: %i",
f_integral.time_since_last_sonar_update);
warnx("quality integration average : %i", f_integral.qual);
warnx("quality : %i", f.qual);
}

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

@ -817,6 +817,11 @@ PX4IO::init()
}
/* set safety to off if circuit breaker enabled */
if (circuit_breaker_enabled("CBRK_IO_SAFETY", CBRK_IO_SAFETY_KEY)) {
(void)io_reg_set(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_FORCE_SAFETY_OFF, PX4IO_FORCE_SAFETY_MAGIC);
}
/* try to claim the generic PWM output device node as well - it's OK if we fail at this */
ret = register_driver(PWM_OUTPUT_DEVICE_PATH, &fops, 0666, (void *)this);
@ -1155,52 +1160,54 @@ PX4IO::io_set_arming_state()
actuator_armed_s armed; ///< system armed state
vehicle_control_mode_s control_mode; ///< vehicle_control_mode
orb_copy(ORB_ID(actuator_armed), _t_actuator_armed, &armed);
orb_copy(ORB_ID(vehicle_control_mode), _t_vehicle_control_mode, &control_mode);
int have_armed = orb_copy(ORB_ID(actuator_armed), _t_actuator_armed, &armed);
int have_control_mode = orb_copy(ORB_ID(vehicle_control_mode), _t_vehicle_control_mode, &control_mode);
uint16_t set = 0;
uint16_t clear = 0;
if (armed.armed) {
set |= PX4IO_P_SETUP_ARMING_FMU_ARMED;
if (have_armed == OK) {
if (armed.armed) {
set |= PX4IO_P_SETUP_ARMING_FMU_ARMED;
} else {
clear |= PX4IO_P_SETUP_ARMING_FMU_ARMED;
}
} else {
clear |= PX4IO_P_SETUP_ARMING_FMU_ARMED;
if (armed.lockdown && !_lockdown_override) {
set |= PX4IO_P_SETUP_ARMING_LOCKDOWN;
} else {
clear |= PX4IO_P_SETUP_ARMING_LOCKDOWN;
}
/* Do not set failsafe if circuit breaker is enabled */
if (armed.force_failsafe && !_cb_flighttermination) {
set |= PX4IO_P_SETUP_ARMING_FORCE_FAILSAFE;
} else {
clear |= PX4IO_P_SETUP_ARMING_FORCE_FAILSAFE;
}
// XXX this is for future support in the commander
// but can be removed if unneeded
// if (armed.termination_failsafe) {
// set |= PX4IO_P_SETUP_ARMING_TERMINATION_FAILSAFE;
// } else {
// clear |= PX4IO_P_SETUP_ARMING_TERMINATION_FAILSAFE;
// }
if (armed.ready_to_arm) {
set |= PX4IO_P_SETUP_ARMING_IO_ARM_OK;
} else {
clear |= PX4IO_P_SETUP_ARMING_IO_ARM_OK;
}
}
if (armed.lockdown && !_lockdown_override) {
set |= PX4IO_P_SETUP_ARMING_LOCKDOWN;
} else {
clear |= PX4IO_P_SETUP_ARMING_LOCKDOWN;
}
/* Do not set failsafe if circuit breaker is enabled */
if (armed.force_failsafe && !_cb_flighttermination) {
set |= PX4IO_P_SETUP_ARMING_FORCE_FAILSAFE;
} else {
clear |= PX4IO_P_SETUP_ARMING_FORCE_FAILSAFE;
}
// XXX this is for future support in the commander
// but can be removed if unneeded
// if (armed.termination_failsafe) {
// set |= PX4IO_P_SETUP_ARMING_TERMINATION_FAILSAFE;
// } else {
// clear |= PX4IO_P_SETUP_ARMING_TERMINATION_FAILSAFE;
// }
if (armed.ready_to_arm) {
set |= PX4IO_P_SETUP_ARMING_IO_ARM_OK;
} else {
clear |= PX4IO_P_SETUP_ARMING_IO_ARM_OK;
}
if (control_mode.flag_external_manual_override_ok) {
set |= PX4IO_P_SETUP_ARMING_MANUAL_OVERRIDE_OK;
} else {
clear |= PX4IO_P_SETUP_ARMING_MANUAL_OVERRIDE_OK;
if (have_control_mode == OK) {
if (control_mode.flag_external_manual_override_ok) {
set |= PX4IO_P_SETUP_ARMING_MANUAL_OVERRIDE_OK;
} else {
clear |= PX4IO_P_SETUP_ARMING_MANUAL_OVERRIDE_OK;
}
}
return io_reg_modify(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_ARMING, clear, set);
@ -2193,7 +2200,7 @@ PX4IO::ioctl(file * filep, int cmd, unsigned long arg)
struct pwm_output_values* pwm = (struct pwm_output_values*)arg;
if (pwm->channel_count > _max_actuators)
/* fail with error */
return E2BIG;
return -E2BIG;
/* copy values to registers in IO */
ret = io_reg_set(PX4IO_PAGE_FAILSAFE_PWM, 0, pwm->values, pwm->channel_count);
@ -2212,7 +2219,7 @@ PX4IO::ioctl(file * filep, int cmd, unsigned long arg)
struct pwm_output_values* pwm = (struct pwm_output_values*)arg;
if (pwm->channel_count > _max_actuators)
/* fail with error */
return E2BIG;
return -E2BIG;
/* copy values to registers in IO */
ret = io_reg_set(PX4IO_PAGE_DISARMED_PWM, 0, pwm->values, pwm->channel_count);
@ -2231,7 +2238,7 @@ PX4IO::ioctl(file * filep, int cmd, unsigned long arg)
struct pwm_output_values* pwm = (struct pwm_output_values*)arg;
if (pwm->channel_count > _max_actuators)
/* fail with error */
return E2BIG;
return -E2BIG;
/* copy values to registers in IO */
ret = io_reg_set(PX4IO_PAGE_CONTROL_MIN_PWM, 0, pwm->values, pwm->channel_count);
@ -2250,7 +2257,7 @@ PX4IO::ioctl(file * filep, int cmd, unsigned long arg)
struct pwm_output_values* pwm = (struct pwm_output_values*)arg;
if (pwm->channel_count > _max_actuators)
/* fail with error */
return E2BIG;
return -E2BIG;
/* copy values to registers in IO */
ret = io_reg_set(PX4IO_PAGE_CONTROL_MAX_PWM, 0, pwm->values, pwm->channel_count);
@ -2587,9 +2594,9 @@ PX4IO::ioctl(file * filep, int cmd, unsigned long arg)
on param_get()
*/
struct pwm_output_rc_config* config = (struct pwm_output_rc_config*)arg;
if (config->channel >= _max_actuators) {
if (config->channel >= RC_INPUT_MAX_CHANNELS) {
/* fail with error */
return E2BIG;
return -E2BIG;
}
/* copy values to registers in IO */

14
src/drivers/rgbled/rgbled.cpp
View File

@ -121,7 +121,7 @@ private:
/* for now, we only support one RGBLED */
namespace
{
RGBLED *g_rgbled;
RGBLED *g_rgbled = nullptr;
}
void rgbled_usage();
@ -680,15 +680,15 @@ rgbled_main(int argc, char *argv[])
ret = ioctl(fd, RGBLED_SET_MODE, (unsigned long)RGBLED_MODE_OFF);
close(fd);
/* delete the rgbled object if stop was requested, in addition to turning off the LED. */
if (!strcmp(verb, "stop")) {
delete g_rgbled;
g_rgbled = nullptr;
exit(0);
}
exit(ret);
}
if (!strcmp(verb, "stop")) {
delete g_rgbled;
g_rgbled = nullptr;
exit(0);
}
if (!strcmp(verb, "rgb")) {
if (argc < 5) {
errx(1, "Usage: rgbled rgb <red> <green> <blue>");

44
src/drivers/trone/module.mk Normal file
View File

@ -0,0 +1,44 @@
############################################################################
#
# 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 TeraRanger One range finder driver
#
MODULE_COMMAND = trone
SRCS = trone.cpp
MODULE_STACKSIZE = 1200
MAXOPTIMIZATION = -Os

913
src/drivers/trone/trone.cpp Normal file
View File

@ -0,0 +1,913 @@
/****************************************************************************
*
* 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 trone.cpp
* @author Luis Rodrigues
*
* Driver for the TeraRanger One range finders 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_range_finder.h>
#include <drivers/device/ringbuffer.h>
#include <uORB/uORB.h>
#include <uORB/topics/subsystem_info.h>
#include <board_config.h>
/* Configuration Constants */
#define TRONE_BUS PX4_I2C_BUS_EXPANSION
#define TRONE_BASEADDR 0x30 /* 7-bit address */
#define TRONE_DEVICE_PATH "/dev/trone"
/* TRONE Registers addresses */
#define TRONE_MEASURE_REG 0x00 /* Measure range register */
/* Device limits */
#define TRONE_MIN_DISTANCE (0.20f)
#define TRONE_MAX_DISTANCE (14.00f)
#define TRONE_CONVERSION_INTERVAL 50000 /* 50ms */
/* 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
class TRONE : public device::I2C
{
public:
TRONE(int bus = TRONE_BUS, int address = TRONE_BASEADDR);
virtual ~TRONE();
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 _class_instance;
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.
*
* @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();
/**
* 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 TRONE_MIN_DISTANCE
* and TRONE_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);
};
static const uint8_t crc_table[] = {
0x00, 0x07, 0x0e, 0x09, 0x1c, 0x1b, 0x12, 0x15, 0x38, 0x3f, 0x36, 0x31,
0x24, 0x23, 0x2a, 0x2d, 0x70, 0x77, 0x7e, 0x79, 0x6c, 0x6b, 0x62, 0x65,
0x48, 0x4f, 0x46, 0x41, 0x54, 0x53, 0x5a, 0x5d, 0xe0, 0xe7, 0xee, 0xe9,
0xfc, 0xfb, 0xf2, 0xf5, 0xd8, 0xdf, 0xd6, 0xd1, 0xc4, 0xc3, 0xca, 0xcd,
0x90, 0x97, 0x9e, 0x99, 0x8c, 0x8b, 0x82, 0x85, 0xa8, 0xaf, 0xa6, 0xa1,
0xb4, 0xb3, 0xba, 0xbd, 0xc7, 0xc0, 0xc9, 0xce, 0xdb, 0xdc, 0xd5, 0xd2,
0xff, 0xf8, 0xf1, 0xf6, 0xe3, 0xe4, 0xed, 0xea, 0xb7, 0xb0, 0xb9, 0xbe,
0xab, 0xac, 0xa5, 0xa2, 0x8f, 0x88, 0x81, 0x86, 0x93, 0x94, 0x9d, 0x9a,
0x27, 0x20, 0x29, 0x2e, 0x3b, 0x3c, 0x35, 0x32, 0x1f, 0x18, 0x11, 0x16,
0x03, 0x04, 0x0d, 0x0a, 0x57, 0x50, 0x59, 0x5e, 0x4b, 0x4c, 0x45, 0x42,
0x6f, 0x68, 0x61, 0x66, 0x73, 0x74, 0x7d, 0x7a, 0x89, 0x8e, 0x87, 0x80,
0x95, 0x92, 0x9b, 0x9c, 0xb1, 0xb6, 0xbf, 0xb8, 0xad, 0xaa, 0xa3, 0xa4,
0xf9, 0xfe, 0xf7, 0xf0, 0xe5, 0xe2, 0xeb, 0xec, 0xc1, 0xc6, 0xcf, 0xc8,
0xdd, 0xda, 0xd3, 0xd4, 0x69, 0x6e, 0x67, 0x60, 0x75, 0x72, 0x7b, 0x7c,
0x51, 0x56, 0x5f, 0x58, 0x4d, 0x4a, 0x43, 0x44, 0x19, 0x1e, 0x17, 0x10,
0x05, 0x02, 0x0b, 0x0c, 0x21, 0x26, 0x2f, 0x28, 0x3d, 0x3a, 0x33, 0x34,
0x4e, 0x49, 0x40, 0x47, 0x52, 0x55, 0x5c, 0x5b, 0x76, 0x71, 0x78, 0x7f,
0x6a, 0x6d, 0x64, 0x63, 0x3e, 0x39, 0x30, 0x37, 0x22, 0x25, 0x2c, 0x2b,
0x06, 0x01, 0x08, 0x0f, 0x1a, 0x1d, 0x14, 0x13, 0xae, 0xa9, 0xa0, 0xa7,
0xb2, 0xb5, 0xbc, 0xbb, 0x96, 0x91, 0x98, 0x9f, 0x8a, 0x8d, 0x84, 0x83,
0xde, 0xd9, 0xd0, 0xd7, 0xc2, 0xc5, 0xcc, 0xcb, 0xe6, 0xe1, 0xe8, 0xef,
0xfa, 0xfd, 0xf4, 0xf3
};
uint8_t crc8(uint8_t *p, uint8_t len){
uint16_t i;
uint16_t crc = 0x0;
while (len--) {
i = (crc ^ *p++) & 0xFF;
crc = (crc_table[i] ^ (crc << 8)) & 0xFF;
}
return crc & 0xFF;
}
/*
* Driver 'main' command.
*/
extern "C" __EXPORT int trone_main(int argc, char *argv[]);
TRONE::TRONE(int bus, int address) :
I2C("TRONE", TRONE_DEVICE_PATH, bus, address, 100000),
_min_distance(TRONE_MIN_DISTANCE),
_max_distance(TRONE_MAX_DISTANCE),
_reports(nullptr),
_sensor_ok(false),
_measure_ticks(0),
_collect_phase(false),
_class_instance(-1),
_range_finder_topic(-1),
_sample_perf(perf_alloc(PC_ELAPSED, "trone_read")),
_comms_errors(perf_alloc(PC_COUNT, "trone_comms_errors")),
_buffer_overflows(perf_alloc(PC_COUNT, "trone_buffer_overflows"))
{
// up the retries since the device misses the first measure attempts
I2C::_retries = 3;
// enable debug() calls
_debug_enabled = false;
// work_cancel in the dtor will explode if we don't do this...
memset(&_work, 0, sizeof(_work));
}
TRONE::~TRONE()
{
/* make sure we are truly inactive */
stop();
/* free any existing reports */
if (_reports != nullptr) {
delete _reports;
}
if (_class_instance != -1) {
unregister_class_devname(RANGE_FINDER_DEVICE_PATH, _class_instance);
}
// free perf counters
perf_free(_sample_perf);
perf_free(_comms_errors);
perf_free(_buffer_overflows);
}
int
TRONE::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(range_finder_report));
if (_reports == nullptr) {
goto out;
}
_class_instance = register_class_devname(RANGE_FINDER_DEVICE_PATH);
if (_class_instance == CLASS_DEVICE_PRIMARY) {
/* get a publish handle on the range finder topic */
struct range_finder_report rf_report;
measure();
_reports->get(&rf_report);
_range_finder_topic = orb_advertise(ORB_ID(sensor_range_finder), &rf_report);
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 */
_sensor_ok = true;
out:
return ret;
}
int
TRONE::probe()
{
return measure();
}
void
TRONE::set_minimum_distance(float min)
{
_min_distance = min;
}
void
TRONE::set_maximum_distance(float max)
{
_max_distance = max;
}
float
TRONE::get_minimum_distance()
{
return _min_distance;
}
float
TRONE::get_maximum_distance()
{
return _max_distance;
}
int
TRONE::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(TRONE_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(TRONE_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 I2C::ioctl(filp, cmd, arg);
}
}
ssize_t
TRONE::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(TRONE_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
TRONE::measure()
{
int ret;
/*
* Send the command to begin a measurement.
*/
const uint8_t cmd = TRONE_MEASURE_REG;
ret = transfer(&cmd, sizeof(cmd), nullptr, 0);
if (OK != ret) {
perf_count(_comms_errors);
log("i2c::transfer returned %d", ret);
return ret;
}
ret = OK;
return ret;
}
int
TRONE::collect()
{
int ret = -EIO;
/* read from the sensor */
uint8_t val[3] = {0, 0, 0};
perf_begin(_sample_perf);
ret = transfer(nullptr, 0, &val[0], 3);
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 * 0.001f; /* mm 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.distance = si_units;
report.valid = crc8(val, 2) == val[2] && si_units > get_minimum_distance() && si_units < get_maximum_distance() ? 1 : 0;
/* publish it, if we are the primary */
if (_range_finder_topic >= 0) {
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
TRONE::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)&TRONE::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
TRONE::stop()
{
work_cancel(HPWORK, &_work);
}
void
TRONE::cycle_trampoline(void *arg)
{
TRONE *dev = (TRONE *)arg;
dev->cycle();
}
void
TRONE::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(TRONE_CONVERSION_INTERVAL)) {
/* schedule a fresh cycle call when we are ready to measure again */
work_queue(HPWORK,
&_work,
(worker_t)&TRONE::cycle_trampoline,
this,
_measure_ticks - USEC2TICK(TRONE_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)&TRONE::cycle_trampoline,
this,
USEC2TICK(TRONE_CONVERSION_INTERVAL));
}
void
TRONE::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 trone
{
/* oddly, ERROR is not defined for c++ */
#ifdef ERROR
# undef ERROR
#endif
const int ERROR = -1;
TRONE *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 TRONE(TRONE_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(TRONE_DEVICE_PATH, O_RDONLY);
if (fd < 0) {
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 ret;
int fd = open(TRONE_DEVICE_PATH, O_RDONLY);
if (fd < 0) {
err(1, "%s open failed (try 'trone start' if the driver is not running", TRONE_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 50x and report each value */
for (unsigned i = 0; i < 50; 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("valid %u", report.valid);
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(TRONE_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
trone_main(int argc, char *argv[])
{
/*
* Start/load the driver.
*/
if (!strcmp(argv[1], "start")) {
trone::start();
}
/*
* Stop the driver
*/
if (!strcmp(argv[1], "stop")) {
trone::stop();
}
/*
* Test the driver/device.
*/
if (!strcmp(argv[1], "test")) {
trone::test();
}
/*
* Reset the driver.
*/
if (!strcmp(argv[1], "reset")) {
trone::reset();
}
/*
* Print driver information.
*/
if (!strcmp(argv[1], "info") || !strcmp(argv[1], "status")) {
trone::info();
}
errx(1, "unrecognized command, try 'start', 'test', 'reset' or 'info'");
}

4
src/examples/flow_position_estimator/flow_position_estimator_main.c
View File

@ -308,8 +308,8 @@ int flow_position_estimator_thread_main(int argc, char *argv[])
if (vehicle_liftoff || params.debug)
{
/* copy flow */
flow_speed[0] = flow.flow_comp_x_m;
flow_speed[1] = flow.flow_comp_y_m;
flow_speed[0] = flow.pixel_flow_x_integral / (flow.integration_timespan / 1e6f) * flow.ground_distance_m;
flow_speed[1] = flow.pixel_flow_y_integral / (flow.integration_timespan / 1e6f) * flow.ground_distance_m;
flow_speed[2] = 0.0f;
/* convert to bodyframe velocity */

10
src/modules/commander/accelerometer_calibration.cpp
View File

@ -263,7 +263,7 @@ int do_accel_calibration_measurements(int mavlink_fd, float accel_offs[3], float
const int samples_num = 2500;
float accel_ref[6][3];
bool data_collected[6] = { false, false, false, false, false, false };
const char *orientation_strs[6] = { "front", "back", "left", "right", "top", "bottom" };
const char *orientation_strs[6] = { "back", "front", "left", "right", "up", "down" };
int sensor_combined_sub = orb_subscribe(ORB_ID(sensor_combined));
@ -294,12 +294,12 @@ int do_accel_calibration_measurements(int mavlink_fd, float accel_offs[3], float
/* inform user which axes are still needed */
mavlink_log_info(mavlink_fd, "pending: %s%s%s%s%s%s",
(!data_collected[0]) ? "front " : "",
(!data_collected[1]) ? "back " : "",
(!data_collected[5]) ? "down " : "",
(!data_collected[0]) ? "back " : "",
(!data_collected[1]) ? "front " : "",
(!data_collected[2]) ? "left " : "",
(!data_collected[3]) ? "right " : "",
(!data_collected[4]) ? "up " : "",
(!data_collected[5]) ? "down " : "");
(!data_collected[4]) ? "up " : "");
/* allow user enough time to read the message */
sleep(3);

5
src/modules/commander/commander.cpp
View File

@ -1548,7 +1548,7 @@ int commander_thread_main(int argc, char *argv[])
} else {
if (status.rc_signal_lost) {
mavlink_log_critical(mavlink_fd, "RC signal regained");
mavlink_log_critical(mavlink_fd, "RC SIGNAL REGAINED after %llums",(hrt_absolute_time()-status.rc_signal_lost_timestamp)/1000);
status_changed = true;
}
}
@ -1649,8 +1649,9 @@ int commander_thread_main(int argc, char *argv[])
} else {
if (!status.rc_signal_lost) {
mavlink_log_critical(mavlink_fd, "RC SIGNAL LOST");
mavlink_log_critical(mavlink_fd, "RC SIGNAL LOST (at t=%llums)",hrt_absolute_time()/1000);
status.rc_signal_lost = true;
status.rc_signal_lost_timestamp=sp_man.timestamp;
status_changed = true;
}
}

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

@ -706,7 +706,7 @@ int prearm_check(const struct vehicle_status_s *status, const int mavlink_fd)
}
if (fabsf(airspeed.indicated_airspeed_m_s > 6.0f)) {
mavlink_log_critical(mavlink_fd, "AIRSPEED WARNING: WIND OR CALIBRATION MISSING");
mavlink_log_critical(mavlink_fd, "AIRSPEED WARNING: WIND OR CALIBRATION ISSUE");
// XXX do not make this fatal yet
}
}

2
src/modules/mavlink/mavlink_main.cpp
View File

@ -1405,7 +1405,7 @@ Mavlink::task_main(int argc, char *argv[])
configure_stream("POSITION_TARGET_GLOBAL_INT", 3.0f);
configure_stream("ATTITUDE_TARGET", 3.0f);
configure_stream("DISTANCE_SENSOR", 0.5f);
configure_stream("OPTICAL_FLOW", 5.0f);
configure_stream("OPTICAL_FLOW_RAD", 5.0f);
break;
case MAVLINK_MODE_ONBOARD:

40
src/modules/mavlink/mavlink_messages.cpp
View File

@ -1834,33 +1834,32 @@ protected:
}
};
class MavlinkStreamOpticalFlow : public MavlinkStream
class MavlinkStreamOpticalFlowRad : public MavlinkStream
{
public:
const char *get_name() const
{
return MavlinkStreamOpticalFlow::get_name_static();
return MavlinkStreamOpticalFlowRad::get_name_static();
}
static const char *get_name_static()
{
return "OPTICAL_FLOW";
return "OPTICAL_FLOW_RAD";
}
uint8_t get_id()
{
return MAVLINK_MSG_ID_OPTICAL_FLOW;
return MAVLINK_MSG_ID_OPTICAL_FLOW_RAD;
}
static MavlinkStream *new_instance(Mavlink *mavlink)
{
return new MavlinkStreamOpticalFlow(mavlink);
return new MavlinkStreamOpticalFlowRad(mavlink);
}
unsigned get_size()
{
return _flow_sub->is_published() ? (MAVLINK_MSG_ID_OPTICAL_FLOW_LEN + MAVLINK_NUM_NON_PAYLOAD_BYTES) : 0;
return _flow_sub->is_published() ? (MAVLINK_MSG_ID_OPTICAL_FLOW_RAD_LEN + MAVLINK_NUM_NON_PAYLOAD_BYTES) : 0;
}
private:
@ -1868,11 +1867,11 @@ private:
uint64_t _flow_time;
/* do not allow top copying this class */
MavlinkStreamOpticalFlow(MavlinkStreamOpticalFlow &);
MavlinkStreamOpticalFlow& operator = (const MavlinkStreamOpticalFlow &);
MavlinkStreamOpticalFlowRad(MavlinkStreamOpticalFlowRad &);
MavlinkStreamOpticalFlowRad& operator = (const MavlinkStreamOpticalFlowRad &);
protected:
explicit MavlinkStreamOpticalFlow(Mavlink *mavlink) : MavlinkStream(mavlink),
explicit MavlinkStreamOpticalFlowRad(Mavlink *mavlink) : MavlinkStream(mavlink),
_flow_sub(_mavlink->add_orb_subscription(ORB_ID(optical_flow))),
_flow_time(0)
{}
@ -1882,18 +1881,23 @@ protected:
struct optical_flow_s flow;
if (_flow_sub->update(&_flow_time, &flow)) {
mavlink_optical_flow_t msg;
mavlink_optical_flow_rad_t msg;
msg.time_usec = flow.timestamp;
msg.sensor_id = flow.sensor_id;
msg.flow_x = flow.flow_raw_x;
msg.flow_y = flow.flow_raw_y;
msg.flow_comp_m_x = flow.flow_comp_x_m;
msg.flow_comp_m_y = flow.flow_comp_y_m;
msg.integrated_x = flow.pixel_flow_x_integral;
msg.integrated_y = flow.pixel_flow_y_integral;
msg.integrated_xgyro = flow.gyro_x_rate_integral;
msg.integrated_ygyro = flow.gyro_y_rate_integral;
msg.integrated_zgyro = flow.gyro_z_rate_integral;
msg.distance = flow.ground_distance_m;
msg.quality = flow.quality;
msg.ground_distance = flow.ground_distance_m;
msg.integration_time_us = flow.integration_timespan;
msg.sensor_id = flow.sensor_id;
msg.time_delta_distance_us = flow.time_since_last_sonar_update;
msg.temperature = flow.gyro_temperature;
_mavlink->send_message(MAVLINK_MSG_ID_OPTICAL_FLOW, &msg);
_mavlink->send_message(MAVLINK_MSG_ID_OPTICAL_FLOW_RAD, &msg);
}
}
};
@ -2199,7 +2203,7 @@ StreamListItem *streams_list[] = {
new StreamListItem(&MavlinkStreamAttitudeTarget::new_instance, &MavlinkStreamAttitudeTarget::get_name_static),
new StreamListItem(&MavlinkStreamRCChannelsRaw::new_instance, &MavlinkStreamRCChannelsRaw::get_name_static),
new StreamListItem(&MavlinkStreamManualControl::new_instance, &MavlinkStreamManualControl::get_name_static),
new StreamListItem(&MavlinkStreamOpticalFlow::new_instance, &MavlinkStreamOpticalFlow::get_name_static),
new StreamListItem(&MavlinkStreamOpticalFlowRad::new_instance, &MavlinkStreamOpticalFlowRad::get_name_static),
new StreamListItem(&MavlinkStreamAttitudeControls::new_instance, &MavlinkStreamAttitudeControls::get_name_static),
new StreamListItem(&MavlinkStreamNamedValueFloat::new_instance, &MavlinkStreamNamedValueFloat::get_name_static),
new StreamListItem(&MavlinkStreamCameraCapture::new_instance, &MavlinkStreamCameraCapture::get_name_static),

40
src/modules/mavlink/mavlink_receiver.cpp
View File

@ -144,8 +144,8 @@ MavlinkReceiver::handle_message(mavlink_message_t *msg)
handle_message_command_int(msg);
break;
case MAVLINK_MSG_ID_OPTICAL_FLOW:
handle_message_optical_flow(msg);
case MAVLINK_MSG_ID_OPTICAL_FLOW_RAD:
handle_message_optical_flow_rad(msg);
break;
case MAVLINK_MSG_ID_SET_MODE:
@ -352,24 +352,27 @@ MavlinkReceiver::handle_message_command_int(mavlink_message_t *msg)
}
void
MavlinkReceiver::handle_message_optical_flow(mavlink_message_t *msg)
MavlinkReceiver::handle_message_optical_flow_rad(mavlink_message_t *msg)
{
/* optical flow */
mavlink_optical_flow_t flow;
mavlink_msg_optical_flow_decode(msg, &flow);
mavlink_optical_flow_rad_t flow;
mavlink_msg_optical_flow_rad_decode(msg, &flow);
struct optical_flow_s f;
memset(&f, 0, sizeof(f));
f.timestamp = hrt_absolute_time();
f.flow_timestamp = flow.time_usec;
f.flow_raw_x = flow.flow_x;
f.flow_raw_y = flow.flow_y;
f.flow_comp_x_m = flow.flow_comp_m_x;
f.flow_comp_y_m = flow.flow_comp_m_y;
f.ground_distance_m = flow.ground_distance;
f.timestamp = flow.time_usec;
f.integration_timespan = flow.integration_time_us;
f.pixel_flow_x_integral = flow.integrated_x;
f.pixel_flow_y_integral = flow.integrated_y;
f.gyro_x_rate_integral = flow.integrated_xgyro;
f.gyro_y_rate_integral = flow.integrated_ygyro;
f.gyro_z_rate_integral = flow.integrated_zgyro;
f.time_since_last_sonar_update = flow.time_delta_distance_us;
f.ground_distance_m = flow.distance;
f.quality = flow.quality;
f.sensor_id = flow.sensor_id;
f.gyro_temperature = flow.temperature;
if (_flow_pub < 0) {
_flow_pub = orb_advertise(ORB_ID(optical_flow), &f);
@ -389,13 +392,18 @@ MavlinkReceiver::handle_message_hil_optical_flow(mavlink_message_t *msg)
struct optical_flow_s f;
memset(&f, 0, sizeof(f));
f.timestamp = hrt_absolute_time();
f.flow_timestamp = flow.time_usec;
f.flow_raw_x = flow.integrated_x;
f.flow_raw_y = flow.integrated_y;
f.timestamp = hrt_absolute_time(); // XXX we rely on the system time for now and not flow.time_usec;
f.integration_timespan = flow.integration_time_us;
f.pixel_flow_x_integral = flow.integrated_x;
f.pixel_flow_y_integral = flow.integrated_y;
f.gyro_x_rate_integral = flow.integrated_xgyro;
f.gyro_y_rate_integral = flow.integrated_ygyro;
f.gyro_z_rate_integral = flow.integrated_zgyro;
f.time_since_last_sonar_update = flow.time_delta_distance_us;
f.ground_distance_m = flow.distance;
f.quality = flow.quality;
f.sensor_id = flow.sensor_id;
f.gyro_temperature = flow.temperature;
if (_flow_pub < 0) {
_flow_pub = orb_advertise(ORB_ID(optical_flow), &f);

2
src/modules/mavlink/mavlink_receiver.h
View File

@ -112,7 +112,7 @@ private:
void handle_message(mavlink_message_t *msg);
void handle_message_command_long(mavlink_message_t *msg);
void handle_message_command_int(mavlink_message_t *msg);
void handle_message_optical_flow(mavlink_message_t *msg);
void handle_message_optical_flow_rad(mavlink_message_t *msg);
void handle_message_hil_optical_flow(mavlink_message_t *msg);
void handle_message_set_mode(mavlink_message_t *msg);
void handle_message_vicon_position_estimate(mavlink_message_t *msg);

11
src/modules/position_estimator_inav/position_estimator_inav_main.c
View File

@ -296,7 +296,7 @@ int position_estimator_inav_thread_main(int argc, char *argv[])
float w_flow = 0.0f;
float sonar_prev = 0.0f;
hrt_abstime flow_prev = 0; // time of last flow measurement
//hrt_abstime flow_prev = 0; // time of last flow measurement
hrt_abstime sonar_time = 0; // time of last sonar measurement (not filtered)
hrt_abstime sonar_valid_time = 0; // time of last sonar measurement used for correction (filtered)
@ -489,8 +489,8 @@ int position_estimator_inav_thread_main(int argc, char *argv[])
orb_copy(ORB_ID(optical_flow), optical_flow_sub, &flow);
/* calculate time from previous update */
float flow_dt = flow_prev > 0 ? (flow.flow_timestamp - flow_prev) * 1e-6f : 0.1f;
flow_prev = flow.flow_timestamp;
// float flow_dt = flow_prev > 0 ? (flow.flow_timestamp - flow_prev) * 1e-6f : 0.1f;
// flow_prev = flow.flow_timestamp;
if ((flow.ground_distance_m > 0.31f) &&
(flow.ground_distance_m < 4.0f) &&
@ -548,8 +548,9 @@ int position_estimator_inav_thread_main(int argc, char *argv[])
/* convert raw flow to angular flow (rad/s) */
float flow_ang[2];
flow_ang[0] = flow.flow_raw_x * params.flow_k / 1000.0f / flow_dt;
flow_ang[1] = flow.flow_raw_y * params.flow_k / 1000.0f / flow_dt;
//todo check direction of x und y axis
flow_ang[0] = flow.pixel_flow_x_integral/(float)flow.integration_timespan*1000000.0f;//flow.flow_raw_x * params.flow_k / 1000.0f / flow_dt;
flow_ang[1] = flow.pixel_flow_y_integral/(float)flow.integration_timespan*1000000.0f;//flow.flow_raw_y * params.flow_k / 1000.0f / flow_dt;
/* flow measurements vector */
float flow_m[3];
flow_m[0] = -flow_ang[0] * flow_dist;

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

@ -353,12 +353,16 @@ static unsigned mixer_text_length = 0;
int
mixer_handle_text(const void *buffer, size_t length)
{
/* do not allow a mixer change while safety off */
if ((r_status_flags & PX4IO_P_STATUS_FLAGS_SAFETY_OFF)) {
/* do not allow a mixer change while safety off and FMU armed */
if ((r_status_flags & PX4IO_P_STATUS_FLAGS_SAFETY_OFF) &&
(r_setup_arming & PX4IO_P_SETUP_ARMING_FMU_ARMED)) {
return 1;
}
/* abort if we're in the mixer */
/* disable mixing, will be enabled once load is complete */
r_status_flags &= ~(PX4IO_P_STATUS_FLAGS_MIXER_OK);
/* abort if we're in the mixer - the caller is expected to retry */
if (in_mixer) {
return 1;
}
@ -367,17 +371,16 @@ mixer_handle_text(const void *buffer, size_t length)
isr_debug(2, "mix txt %u", length);
if (length < sizeof(px4io_mixdata))
if (length < sizeof(px4io_mixdata)) {
return 0;
}
unsigned text_length = length - sizeof(px4io_mixdata);
unsigned text_length = length - sizeof(px4io_mixdata);
switch (msg->action) {
case F2I_MIXER_ACTION_RESET:
isr_debug(2, "reset");
/* FIRST mark the mixer as invalid */
r_status_flags &= ~PX4IO_P_STATUS_FLAGS_MIXER_OK;
/* THEN actually delete it */
mixer_group.reset();
mixer_text_length = 0;
@ -386,9 +389,6 @@ mixer_handle_text(const void *buffer, size_t length)
case F2I_MIXER_ACTION_APPEND:
isr_debug(2, "append %d", length);
/* disable mixing during the update */
r_status_flags &= ~PX4IO_P_STATUS_FLAGS_MIXER_OK;
/* check for overflow - this would be really fatal */
if ((mixer_text_length + text_length + 1) > sizeof(mixer_text)) {
r_status_flags &= ~PX4IO_P_STATUS_FLAGS_MIXER_OK;

18
src/modules/px4iofirmware/registers.c
View File

@ -407,11 +407,11 @@ registers_set(uint8_t page, uint8_t offset, const uint16_t *values, unsigned num
/* handle text going to the mixer parser */
case PX4IO_PAGE_MIXERLOAD:
if (!(r_status_flags & PX4IO_P_STATUS_FLAGS_SAFETY_OFF) ||
(r_status_flags & PX4IO_P_STATUS_FLAGS_OUTPUTS_ARMED)) {
return mixer_handle_text(values, num_values * sizeof(*values));
}
break;
/* do not change the mixer if FMU is armed and IO's safety is off
* this state defines an active system. This check is done in the
* text handling function.
*/
return mixer_handle_text(values, num_values * sizeof(*values));
default:
/* avoid offset wrap */
@ -583,8 +583,7 @@ registers_set_one(uint8_t page, uint8_t offset, uint16_t value)
break;
case PX4IO_P_SETUP_REBOOT_BL:
if ((r_status_flags & PX4IO_P_STATUS_FLAGS_SAFETY_OFF) ||
(r_status_flags & PX4IO_P_STATUS_FLAGS_OUTPUTS_ARMED)) {
if (r_status_flags & PX4IO_P_STATUS_FLAGS_SAFETY_OFF) {
// don't allow reboot while armed
break;
}
@ -630,10 +629,9 @@ registers_set_one(uint8_t page, uint8_t offset, uint16_t value)
case PX4IO_PAGE_RC_CONFIG: {
/**
* do not allow a RC config change while outputs armed
* do not allow a RC config change while safety is off
*/
if ((r_status_flags & PX4IO_P_STATUS_FLAGS_SAFETY_OFF) ||
(r_status_flags & PX4IO_P_STATUS_FLAGS_OUTPUTS_ARMED)) {
if (r_status_flags & PX4IO_P_STATUS_FLAGS_SAFETY_OFF) {
break;
}

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

@ -496,6 +496,8 @@ static void *logwriter_thread(void *arg)
/* set name */
prctl(PR_SET_NAME, "sdlog2_writer", 0);
perf_counter_t perf_write = perf_alloc(PC_ELAPSED, "sd write");
int log_fd = open_log_file();
if (log_fd < 0) {
@ -553,7 +555,9 @@ static void *logwriter_thread(void *arg)
n = available;
}
perf_begin(perf_write);
n = write(log_fd, read_ptr, n);
perf_end(perf_write);
should_wait = (n == available) && !is_part;
@ -586,6 +590,9 @@ static void *logwriter_thread(void *arg)
fsync(log_fd);
close(log_fd);
/* free performance counter */
perf_free(perf_write);
return NULL;
}
@ -1511,11 +1518,14 @@ int sdlog2_thread_main(int argc, char *argv[])
/* --- FLOW --- */
if (copy_if_updated(ORB_ID(optical_flow), subs.flow_sub, &buf.flow)) {
log_msg.msg_type = LOG_FLOW_MSG;
log_msg.body.log_FLOW.flow_raw_x = buf.flow.flow_raw_x;
log_msg.body.log_FLOW.flow_raw_y = buf.flow.flow_raw_y;
log_msg.body.log_FLOW.flow_comp_x = buf.flow.flow_comp_x_m;
log_msg.body.log_FLOW.flow_comp_y = buf.flow.flow_comp_y_m;
log_msg.body.log_FLOW.distance = buf.flow.ground_distance_m;
log_msg.body.log_FLOW.ground_distance_m = buf.flow.ground_distance_m;
log_msg.body.log_FLOW.gyro_temperature = buf.flow.gyro_temperature;
log_msg.body.log_FLOW.gyro_x_rate_integral = buf.flow.gyro_x_rate_integral;
log_msg.body.log_FLOW.gyro_y_rate_integral = buf.flow.gyro_y_rate_integral;
log_msg.body.log_FLOW.gyro_z_rate_integral = buf.flow.gyro_z_rate_integral;
log_msg.body.log_FLOW.integration_timespan = buf.flow.integration_timespan;
log_msg.body.log_FLOW.pixel_flow_x_integral = buf.flow.pixel_flow_x_integral;
log_msg.body.log_FLOW.pixel_flow_y_integral = buf.flow.pixel_flow_y_integral;
log_msg.body.log_FLOW.quality = buf.flow.quality;
log_msg.body.log_FLOW.sensor_id = buf.flow.sensor_id;
LOGBUFFER_WRITE_AND_COUNT(FLOW);

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

@ -200,13 +200,19 @@ struct log_ARSP_s {
/* --- FLOW - OPTICAL FLOW --- */
#define LOG_FLOW_MSG 15
struct log_FLOW_s {
int16_t flow_raw_x;
int16_t flow_raw_y;
float flow_comp_x;
float flow_comp_y;
float distance;
uint8_t quality;
uint64_t timestamp;
uint8_t sensor_id;
float pixel_flow_x_integral;
float pixel_flow_y_integral;
float gyro_x_rate_integral;
float gyro_y_rate_integral;
float gyro_z_rate_integral;
float ground_distance_m;
uint32_t integration_timespan;
uint32_t time_since_last_sonar_update;
uint16_t frame_count_since_last_readout;
int16_t gyro_temperature;
uint8_t quality;
};
/* --- GPOS - GLOBAL POSITION ESTIMATE --- */

2
src/modules/uORB/Publication.cpp
View File

@ -46,6 +46,7 @@
#include "topics/vehicle_attitude_setpoint.h"
#include "topics/vehicle_rates_setpoint.h"
#include "topics/actuator_outputs.h"
#include "topics/actuator_direct.h"
#include "topics/encoders.h"
#include "topics/tecs_status.h"
@ -76,6 +77,7 @@ template class __EXPORT Publication<vehicle_global_velocity_setpoint_s>;
template class __EXPORT Publication<vehicle_attitude_setpoint_s>;
template class __EXPORT Publication<vehicle_rates_setpoint_s>;
template class __EXPORT Publication<actuator_outputs_s>;
template class __EXPORT Publication<actuator_direct_s>;
template class __EXPORT Publication<encoders_s>;
template class __EXPORT Publication<tecs_status_s>;

3
src/modules/uORB/objects_common.cpp
View File

@ -192,6 +192,9 @@ ORB_DEFINE(actuator_outputs_1, struct actuator_outputs_s);
ORB_DEFINE(actuator_outputs_2, struct actuator_outputs_s);
ORB_DEFINE(actuator_outputs_3, struct actuator_outputs_s);
#include "topics/actuator_direct.h"
ORB_DEFINE(actuator_direct, struct actuator_direct_s);
#include "topics/multirotor_motor_limits.h"
ORB_DEFINE(multirotor_motor_limits, struct multirotor_motor_limits_s);

69
src/modules/uORB/topics/actuator_direct.h Normal file
View File

@ -0,0 +1,69 @@
/****************************************************************************
*
* Copyright (C) 2012 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 actuator_direct.h
*
* Actuator direct values.
*
* Values published to this topic are the direct actuator values which
* should be passed to actuators, bypassing mixing
*/
#ifndef TOPIC_ACTUATOR_DIRECT_H
#define TOPIC_ACTUATOR_DIRECT_H
#include <stdint.h>
#include "../uORB.h"
#define NUM_ACTUATORS_DIRECT 16
/**
* @addtogroup topics
* @{
*/
struct actuator_direct_s {
uint64_t timestamp; /**< timestamp in us since system boot */
float values[NUM_ACTUATORS_DIRECT]; /**< actuator values, from -1 to 1 */
unsigned nvalues; /**< number of valid values */
};
/**
* @}
*/
/* actuator direct ORB */
ORB_DECLARE(actuator_direct);
#endif // TOPIC_ACTUATOR_DIRECT_H

24
src/modules/uORB/topics/optical_flow.h
View File

@ -55,16 +55,22 @@
*/
struct optical_flow_s {
uint64_t timestamp; /**< in microseconds since system start */
uint64_t flow_timestamp; /**< timestamp from flow sensor */
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 */
uint64_t timestamp; /**< in microseconds since system start */
uint8_t sensor_id; /**< id of the sensor emitting the flow value */
float pixel_flow_x_integral; /**< accumulated optical flow in radians around x axis */
float pixel_flow_y_integral; /**< accumulated optical flow in radians around y axis */
float gyro_x_rate_integral; /**< accumulated gyro value in radians around x axis */
float gyro_y_rate_integral; /**< accumulated gyro value in radians around y axis */
float gyro_z_rate_integral; /**< accumulated gyro value in radians around z axis */
float ground_distance_m; /**< Altitude / distance to ground in meters */
uint32_t integration_timespan; /**<accumulation timespan in microseconds */
uint32_t time_since_last_sonar_update;/**< time since last sonar update in microseconds */
uint16_t frame_count_since_last_readout;/**< number of accumulated frames in timespan */
int16_t gyro_temperature;/**< Temperature * 100 in centi-degrees Celsius */
uint8_t quality; /**< Average of quality of accumulated frames, 0: bad quality, 255: maximum quality */
};

1
src/modules/uORB/topics/vehicle_status.h
View File

@ -201,6 +201,7 @@ struct vehicle_status_s {
bool rc_signal_found_once;
bool rc_signal_lost; /**< true if RC reception lost */
uint64_t rc_signal_lost_timestamp; /**< Time at which the RC reception was lost */
bool rc_signal_lost_cmd; /**< true if RC lost mode is commanded */
bool rc_input_blocked; /**< set if RC input should be ignored */

17
src/modules/uavcan/actuators/esc.cpp
View File

@ -76,7 +76,9 @@ int UavcanEscController::init()
void UavcanEscController::update_outputs(float *outputs, unsigned num_outputs)
{
if ((outputs == nullptr) || (num_outputs > uavcan::equipment::esc::RawCommand::FieldTypes::cmd::MaxSize)) {
if ((outputs == nullptr) ||
(num_outputs > uavcan::equipment::esc::RawCommand::FieldTypes::cmd::MaxSize) ||
(num_outputs > CONNECTED_ESC_MAX)) {
perf_count(_perfcnt_invalid_input);
return;
}
@ -101,10 +103,15 @@ void UavcanEscController::update_outputs(float *outputs, unsigned num_outputs)
for (unsigned i = 0; i < num_outputs; i++) {
if (_armed_mask & MOTOR_BIT(i)) {
float scaled = (outputs[i] + 1.0F) * 0.5F * cmd_max;
if (scaled < 1.0F) {
scaled = 1.0F; // Since we're armed, we don't want to stop it completely
}
// trim negative values back to 0. Previously
// we set this to 0.1, which meant motors kept
// spinning when armed, but that should be a
// policy decision for a specific vehicle
// type, as it is not appropriate for all
// types of vehicles (eg. fixed wing).
if (scaled < 0.0F) {
scaled = 0.0F;
}
if (scaled > cmd_max) {
scaled = cmd_max;
perf_count(_perfcnt_scaling_error);

154
src/modules/uavcan/uavcan_main.cpp
View File

@ -269,6 +269,24 @@ void UavcanNode::node_spin_once()
}
}
/*
add a fd to the list of polled events. This assumes you want
POLLIN for now.
*/
int UavcanNode::add_poll_fd(int fd)
{
int ret = _poll_fds_num;
if (_poll_fds_num >= UAVCAN_NUM_POLL_FDS) {
errx(1, "uavcan: too many poll fds, exiting");
}
_poll_fds[_poll_fds_num] = ::pollfd();
_poll_fds[_poll_fds_num].fd = fd;
_poll_fds[_poll_fds_num].events = POLLIN;
_poll_fds_num += 1;
return ret;
}
int UavcanNode::run()
{
(void)pthread_mutex_lock(&_node_mutex);
@ -280,9 +298,9 @@ int UavcanNode::run()
_armed_sub = orb_subscribe(ORB_ID(actuator_armed));
_test_motor_sub = orb_subscribe(ORB_ID(test_motor));
_actuator_direct_sub = orb_subscribe(ORB_ID(actuator_direct));
actuator_outputs_s outputs;
memset(&outputs, 0, sizeof(outputs));
memset(&_outputs, 0, sizeof(_outputs));
const int busevent_fd = ::open(uavcan_stm32::BusEvent::DevName, 0);
if (busevent_fd < 0)
@ -304,11 +322,15 @@ int UavcanNode::run()
* the value returned from poll() to detect whether actuator control has timed out or not.
* Instead, all ORB events need to be checked individually (see below).
*/
_poll_fds_num = 0;
_poll_fds[_poll_fds_num] = ::pollfd();
_poll_fds[_poll_fds_num].fd = busevent_fd;
_poll_fds[_poll_fds_num].events = POLLIN;
_poll_fds_num += 1;
add_poll_fd(busevent_fd);
/*
* setup poll to look for actuator direct input if we are
* subscribed to the topic
*/
if (_actuator_direct_sub != -1) {
_actuator_direct_poll_fd_num = add_poll_fd(_actuator_direct_sub);
}
while (!_task_should_exit) {
// update actuator controls subscriptions if needed
@ -326,6 +348,8 @@ int UavcanNode::run()
node_spin_once(); // Non-blocking
bool new_output = false;
// this would be bad...
if (poll_ret < 0) {
log("poll error %d", errno);
@ -333,24 +357,39 @@ int UavcanNode::run()
} else {
// get controls for required topics
bool controls_updated = false;
unsigned poll_id = 1;
for (unsigned i = 0; i < NUM_ACTUATOR_CONTROL_GROUPS; i++) {
if (_control_subs[i] > 0) {
if (_poll_fds[poll_id].revents & POLLIN) {
if (_poll_fds[_poll_ids[i]].revents & POLLIN) {
controls_updated = true;
orb_copy(_control_topics[i], _control_subs[i], &_controls[i]);
}
poll_id++;
}
}
/*
see if we have any direct actuator updates
*/
if (_actuator_direct_sub != -1 &&
(_poll_fds[_actuator_direct_poll_fd_num].revents & POLLIN) &&
orb_copy(ORB_ID(actuator_direct), _actuator_direct_sub, &_actuator_direct) == OK &&
!_test_in_progress) {
if (_actuator_direct.nvalues > NUM_ACTUATOR_OUTPUTS) {
_actuator_direct.nvalues = NUM_ACTUATOR_OUTPUTS;
}
memcpy(&_outputs.output[0], &_actuator_direct.values[0],
_actuator_direct.nvalues*sizeof(float));
_outputs.noutputs = _actuator_direct.nvalues;
new_output = true;
}
// can we mix?
if (_test_in_progress) {
float test_outputs[NUM_ACTUATOR_OUTPUTS] = {};
test_outputs[_test_motor.motor_number] = _test_motor.value*2.0f-1.0f;
// Output to the bus
_esc_controller.update_outputs(test_outputs, NUM_ACTUATOR_OUTPUTS);
memset(&_outputs, 0, sizeof(_outputs));
if (_test_motor.motor_number < NUM_ACTUATOR_OUTPUTS) {
_outputs.output[_test_motor.motor_number] = _test_motor.value*2.0f-1.0f;
_outputs.noutputs = _test_motor.motor_number+1;
}
new_output = true;
} else if (controls_updated && (_mixers != nullptr)) {
// XXX one output group has 8 outputs max,
@ -358,39 +397,41 @@ int UavcanNode::run()
unsigned num_outputs_max = 8;
// Do mixing
outputs.noutputs = _mixers->mix(&outputs.output[0], num_outputs_max);
outputs.timestamp = hrt_absolute_time();
_outputs.noutputs = _mixers->mix(&_outputs.output[0], num_outputs_max);
// iterate actuators
for (unsigned i = 0; i < outputs.noutputs; i++) {
// last resort: catch NaN, INF and out-of-band errors
if (!isfinite(outputs.output[i])) {
/*
* Value is NaN, INF or out of band - set to the minimum value.
* This will be clearly visible on the servo status and will limit the risk of accidentally
* spinning motors. It would be deadly in flight.
*/
outputs.output[i] = -1.0f;
}
// limit outputs to valid range
// never go below min
if (outputs.output[i] < -1.0f) {
outputs.output[i] = -1.0f;
}
// never go below max
if (outputs.output[i] > 1.0f) {
outputs.output[i] = 1.0f;
}
}
// Output to the bus
_esc_controller.update_outputs(outputs.output, outputs.noutputs);
new_output = true;
}
}
if (new_output) {
// iterate actuators, checking for valid values
for (uint8_t i = 0; i < _outputs.noutputs; i++) {
// last resort: catch NaN, INF and out-of-band errors
if (!isfinite(_outputs.output[i])) {
/*
* Value is NaN, INF or out of band - set to the minimum value.
* This will be clearly visible on the servo status and will limit the risk of accidentally
* spinning motors. It would be deadly in flight.
*/
_outputs.output[i] = -1.0f;
}
// never go below min
if (_outputs.output[i] < -1.0f) {
_outputs.output[i] = -1.0f;
}
// never go above max
if (_outputs.output[i] > 1.0f) {
_outputs.output[i] = 1.0f;
}
}
// Output to the bus
_outputs.timestamp = hrt_absolute_time();
_esc_controller.update_outputs(_outputs.output, _outputs.noutputs);
}
// Check motor test state
bool updated = false;
orb_check(_test_motor_sub, &updated);
@ -459,7 +500,6 @@ UavcanNode::subscribe()
uint32_t sub_groups = _groups_required & ~_groups_subscribed;
uint32_t unsub_groups = _groups_subscribed & ~_groups_required;
// the first fd used by CAN
_poll_fds_num = 1;
for (unsigned i = 0; i < NUM_ACTUATOR_CONTROL_GROUPS; i++) {
if (sub_groups & (1 << i)) {
warnx("subscribe to actuator_controls_%d", i);
@ -472,9 +512,7 @@ UavcanNode::subscribe()
}
if (_control_subs[i] > 0) {
_poll_fds[_poll_fds_num].fd = _control_subs[i];
_poll_fds[_poll_fds_num].events = POLLIN;
_poll_fds_num++;
_poll_ids[i] = add_poll_fd(_control_subs[i]);
}
}
}
@ -572,6 +610,14 @@ UavcanNode::print_info()
(unsigned)_groups_subscribed, (unsigned)_groups_required, _poll_fds_num);
printf("ESC mixer: %s\n", (_mixers == nullptr) ? "NONE" : "OK");
if (_outputs.noutputs != 0) {
printf("ESC output: ");
for (uint8_t i=0; i<_outputs.noutputs; i++) {
printf("%d ", (int)(_outputs.output[i]*1000));
}
printf("\n");
}
// Sensor bridges
auto br = _sensor_bridges.getHead();
while (br != nullptr) {
@ -590,7 +636,7 @@ UavcanNode::print_info()
static void print_usage()
{
warnx("usage: \n"
"\tuavcan {start|status|stop}");
"\tuavcan {start|status|stop|arm|disarm}");
}
extern "C" __EXPORT int uavcan_main(int argc, char *argv[]);
@ -637,6 +683,16 @@ int uavcan_main(int argc, char *argv[])
::exit(0);
}
if (!std::strcmp(argv[1], "arm")) {
inst->arm_actuators(true);
::exit(0);
}
if (!std::strcmp(argv[1], "disarm")) {
inst->arm_actuators(false);
::exit(0);
}
if (!std::strcmp(argv[1], "stop")) {
delete inst;
::exit(0);

17
src/modules/uavcan/uavcan_main.hpp
View File

@ -42,6 +42,7 @@
#include <uORB/topics/actuator_outputs.h>
#include <uORB/topics/actuator_armed.h>
#include <uORB/topics/test_motor.h>
#include <uORB/topics/actuator_direct.h>
#include "actuators/esc.hpp"
#include "sensors/sensor_bridge.hpp"
@ -57,6 +58,9 @@
#define NUM_ACTUATOR_CONTROL_GROUPS_UAVCAN 4
#define UAVCAN_DEVICE_PATH "/dev/uavcan/esc"
// we add two to allow for actuator_direct and busevent
#define UAVCAN_NUM_POLL_FDS (NUM_ACTUATOR_CONTROL_GROUPS_UAVCAN+2)
/**
* A UAVCAN node.
*/
@ -97,6 +101,8 @@ private:
int init(uavcan::NodeID node_id);
void node_spin_once();
int run();
int add_poll_fd(int fd); ///< add a fd to poll list, returning index into _poll_fds[]
int _task = -1; ///< handle to the OS task
bool _task_should_exit = false; ///< flag to indicate to tear down the CAN driver
@ -125,6 +131,15 @@ private:
int _control_subs[NUM_ACTUATOR_CONTROL_GROUPS_UAVCAN] = {};
actuator_controls_s _controls[NUM_ACTUATOR_CONTROL_GROUPS_UAVCAN] = {};
orb_id_t _control_topics[NUM_ACTUATOR_CONTROL_GROUPS_UAVCAN] = {};
pollfd _poll_fds[NUM_ACTUATOR_CONTROL_GROUPS_UAVCAN + 1] = {}; ///< +1 for /dev/uavcan/busevent
pollfd _poll_fds[UAVCAN_NUM_POLL_FDS] = {};
unsigned _poll_fds_num = 0;
int _actuator_direct_sub = -1; ///< uORB subscription of the actuator_direct topic
uint8_t _actuator_direct_poll_fd_num;
actuator_direct_s _actuator_direct;
actuator_outputs_s _outputs;
// index into _poll_fds for each _control_subs handle
uint8_t _poll_ids[NUM_ACTUATOR_CONTROL_GROUPS_UAVCAN];
};

3
src/systemcmds/motor_test/motor_test.c
View File

@ -59,9 +59,10 @@ __EXPORT int motor_test_main(int argc, char *argv[]);
static void motor_test(unsigned channel, float value);
static void usage(const char *reason);
static orb_advert_t _test_motor_pub;
void motor_test(unsigned channel, float value)
{
orb_advert_t _test_motor_pub;
struct test_motor_s _test_motor;
_test_motor.motor_number = channel;

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

@ -0,0 +1,41 @@
############################################################################
#
# Copyright (c) 2014 PX4 Development Team. All rights reserved.
#
# Redistribution and use in source and binary forms, with or without
# modification, are permitted provided that the following conditions
# are met:
#
# 1. Redistributions of source code must retain the above copyright
# notice, this list of conditions and the following disclaimer.
# 2. Redistributions in binary form must reproduce the above copyright
# notice, this list of conditions and the following disclaimer in
# the documentation and/or other materials provided with the
# distribution.
# 3. Neither the name PX4 nor the names of its contributors may be
# used to endorse or promote products derived from this software
# without specific prior written permission.
#
# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
# "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
# LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
# FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
# COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
# INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
# BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
# OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
# AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
# LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
# ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
# POSSIBILITY OF SUCH DAMAGE.
#
############################################################################
#
# Dump file utility
#
MODULE_COMMAND = reflect
SRCS = reflect.c
MAXOPTIMIZATION = -Os

111
src/systemcmds/reflect/reflect.c Normal file
View File

@ -0,0 +1,111 @@
/****************************************************************************
*
* Copyright (c) 2014 Andrew Tridgell. 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 reflect.c
*
* simple data reflector for load testing terminals (especially USB)
*
* @author Andrew Tridgell
*/
#include <nuttx/config.h>
#include <unistd.h>
#include <stdio.h>
#include <string.h>
#include <stdlib.h>
#include <stdbool.h>
#include <assert.h>
#include <systemlib/err.h>
__EXPORT int reflect_main(int argc, char *argv[]);
// memory corruption checking
#define MAX_BLOCKS 1000
static uint32_t nblocks;
struct block {
uint32_t v[256];
};
static struct block *blocks[MAX_BLOCKS];
#define VALUE(i) ((i*7) ^ 0xDEADBEEF)
static void allocate_blocks(void)
{
while (nblocks < MAX_BLOCKS) {
blocks[nblocks] = calloc(1, sizeof(struct block));
if (blocks[nblocks] == NULL) {
break;
}
for (uint32_t i=0; i<sizeof(blocks[nblocks]->v)/sizeof(uint32_t); i++) {
blocks[nblocks]->v[i] = VALUE(i);
}
nblocks++;
}
printf("Allocated %u blocks\n", nblocks);
}
static void check_blocks(void)
{
for (uint32_t n=0; n<nblocks; n++) {
for (uint32_t i=0; i<sizeof(blocks[nblocks]->v)/sizeof(uint32_t); i++) {
assert(blocks[n]->v[i] == VALUE(i));
}
}
}
int
reflect_main(int argc, char *argv[])
{
uint32_t total = 0;
printf("Starting reflector\n");
allocate_blocks();
while (true) {
char buf[128];
ssize_t n = read(0, buf, sizeof(buf));
if (n < 0) {
break;
}
if (n > 0) {
write(1, buf, n);
}
total += n;
if (total > 1024000) {
check_blocks();
total = 0;
}
}
return OK;
}