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

Merge beta in master

This commit is contained in:
Lorenz Meier 2015-08-23 11:21:38 +02:00
parent bfef24bf72 b235420f17
commit 53d4c5473f
10 changed files with 134 additions and 70 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

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

@ -290,6 +290,7 @@ then
if ver hwcmp PX4FMU_V1
then
set FMU_MODE serial
set GPS no
fi
fi
unset HIL
@ -320,6 +321,7 @@ then
gps start
fi
fi
unset GPS
unset GPS_FAKE
# Needs to be this early for in-air-restarts

8
makefiles/nuttx/config_px4fmu-v2_default.mk
View File

@ -143,19 +143,19 @@ MODULES += examples/rover_steering_control
#
#MODULES += examples/math_demo
# Tutorial code from
# https://pixhawk.ethz.ch/px4/dev/hello_sky
# https://px4.io/dev/px4_simple_app
#MODULES += examples/px4_simple_app
# Tutorial code from
# https://pixhawk.ethz.ch/px4/dev/daemon
# https://px4.io/dev/daemon
#MODULES += examples/px4_daemon_app
# Tutorial code from
# https://pixhawk.ethz.ch/px4/dev/debug_values
# https://px4.io/dev/debug_values
#MODULES += examples/px4_mavlink_debug
# Tutorial code from
# https://pixhawk.ethz.ch/px4/dev/example_fixedwing_control
# https://px4.io/dev/example_fixedwing_control
#MODULES += examples/fixedwing_control
# Hardware test

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

@ -1,6 +1,6 @@
/****************************************************************************
*
* Copyright (c) 2013, 2014 PX4 Development Team. All rights reserved.
* Copyright (c) 2013-2015 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
@ -32,8 +32,9 @@
****************************************************************************/
/**
* @file ets_airspeed.cpp
* @author Simon Wilks
* @file airspeed.cpp
* @author Simon Wilks <simon@px4.io>
* @author Lorenz Meier <lorenz@px4.io>
*
* Driver for the Eagle Tree Airspeed V3 connected via I2C.
*/
@ -76,7 +77,7 @@
#include <drivers/airspeed/airspeed.h>
Airspeed::Airspeed(int bus, int address, unsigned conversion_interval, const char* path) :
Airspeed::Airspeed(int bus, int address, unsigned conversion_interval, const char *path) :
I2C("Airspeed", path, bus, address, 100000),
_reports(nullptr),
_buffer_overflows(perf_alloc(PC_COUNT, "airspeed_buffer_overflows")),
@ -105,12 +106,14 @@ Airspeed::~Airspeed()
/* make sure we are truly inactive */
stop();
if (_class_instance != -1)
if (_class_instance != -1) {
unregister_class_devname(AIRSPEED_BASE_DEVICE_PATH, _class_instance);
}
/* free any existing reports */
if (_reports != nullptr)
if (_reports != nullptr) {
delete _reports;
}
// free perf counters
perf_free(_sample_perf);
@ -124,13 +127,15 @@ Airspeed::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::RingBuffer(2, sizeof(differential_pressure_s));
if (_reports == nullptr)
if (_reports == nullptr) {
goto out;
}
/* register alternate interfaces if we have to */
_class_instance = register_class_devname(AIRSPEED_BASE_DEVICE_PATH);
@ -146,8 +151,9 @@ Airspeed::init()
/* measurement will have generated a report, publish */
_airspeed_pub = orb_advertise(ORB_ID(differential_pressure), &arp);
if (_airspeed_pub == nullptr)
if (_airspeed_pub == nullptr) {
warnx("uORB started?");
}
}
ret = OK;
@ -166,7 +172,7 @@ Airspeed::probe()
_retries = 4;
int ret = measure();
// drop back to 2 retries once initialised
// drop back to 2 retries once initialised
_retries = 2;
return ret;
}
@ -179,20 +185,20 @@ Airspeed::ioctl(struct file *filp, int cmd, unsigned long arg)
case SENSORIOCSPOLLRATE: {
switch (arg) {
/* switching to manual polling */
/* switching to manual polling */
case SENSOR_POLLRATE_MANUAL:
stop();
_measure_ticks = 0;
return OK;
/* external signalling (DRDY) not supported */
/* external signalling (DRDY) not supported */
case SENSOR_POLLRATE_EXTERNAL:
/* zero would be bad */
/* zero would be bad */
case 0:
return -EINVAL;
/* set default/max polling rate */
/* set default/max polling rate */
case SENSOR_POLLRATE_MAX:
case SENSOR_POLLRATE_DEFAULT: {
/* do we need to start internal polling? */
@ -202,13 +208,14 @@ Airspeed::ioctl(struct file *filp, int cmd, unsigned long arg)
_measure_ticks = USEC2TICK(_conversion_interval);
/* if we need to start the poll state machine, do it */
if (want_start)
if (want_start) {
start();
}
return OK;
}
/* adjust to a legal polling interval in Hz */
/* adjust to a legal polling interval in Hz */
default: {
/* do we need to start internal polling? */
bool want_start = (_measure_ticks == 0);
@ -217,15 +224,17 @@ Airspeed::ioctl(struct file *filp, int cmd, unsigned long arg)
unsigned ticks = USEC2TICK(1000000 / arg);
/* check against maximum rate */
if (ticks < USEC2TICK(_conversion_interval))
if (ticks < USEC2TICK(_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;
}
@ -233,21 +242,25 @@ Airspeed::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;
@ -261,16 +274,16 @@ Airspeed::ioctl(struct file *filp, int cmd, unsigned long arg)
return -EINVAL;
case AIRSPEEDIOCSSCALE: {
struct airspeed_scale *s = (struct airspeed_scale*)arg;
_diff_pres_offset = s->offset_pa;
return OK;
struct airspeed_scale *s = (struct airspeed_scale *)arg;
_diff_pres_offset = s->offset_pa;
return OK;
}
case AIRSPEEDIOCGSCALE: {
struct airspeed_scale *s = (struct airspeed_scale*)arg;
s->offset_pa = _diff_pres_offset;
s->scale = 1.0f;
return OK;
struct airspeed_scale *s = (struct airspeed_scale *)arg;
s->offset_pa = _diff_pres_offset;
s->scale = 1.0f;
return OK;
}
default:
@ -287,8 +300,9 @@ Airspeed::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) {
@ -369,6 +383,7 @@ Airspeed::update_status()
if (_subsys_pub != nullptr) {
orb_publish(ORB_ID(subsystem_info), _subsys_pub, &info);
} else {
_subsys_pub = orb_advertise(ORB_ID(subsystem_info), &info);
}
@ -402,6 +417,7 @@ Airspeed::print_info()
void
Airspeed::new_report(const differential_pressure_s &report)
{
if (!_reports->force(&report))
if (!_reports->force(&report)) {
perf_count(_buffer_overflows);
}
}

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

@ -94,7 +94,7 @@
class ETSAirspeed : public Airspeed
{
public:
ETSAirspeed(int bus, int address = I2C_ADDRESS, const char* path = ETS_PATH);
ETSAirspeed(int bus, int address = I2C_ADDRESS, const char *path = ETS_PATH);
protected:
@ -113,8 +113,8 @@ protected:
*/
extern "C" __EXPORT int ets_airspeed_main(int argc, char *argv[]);
ETSAirspeed::ETSAirspeed(int bus, int address, const char* path) : Airspeed(bus, address,
CONVERSION_INTERVAL, path)
ETSAirspeed::ETSAirspeed(int bus, int address, const char *path) : Airspeed(bus, address,
CONVERSION_INTERVAL, path)
{
}
@ -155,7 +155,8 @@ ETSAirspeed::collect()
}
uint16_t diff_pres_pa_raw = val[1] << 8 | val[0];
if (diff_pres_pa_raw == 0) {
if (diff_pres_pa_raw == 0) {
// a zero value means the pressure sensor cannot give us a
// value. We need to return, and not report a value or the
// caller could end up using this value as part of an
@ -163,7 +164,7 @@ ETSAirspeed::collect()
perf_count(_comms_errors);
DEVICE_LOG("zero value from sensor");
return -1;
}
}
// The raw value still should be compensated for the known offset
diff_pres_pa_raw -= _diff_pres_offset;
@ -175,7 +176,7 @@ ETSAirspeed::collect()
differential_pressure_s report;
report.timestamp = hrt_absolute_time();
report.error_count = perf_event_count(_comms_errors);
report.error_count = perf_event_count(_comms_errors);
// XXX we may want to smooth out the readings to remove noise.
report.differential_pressure_filtered_pa = diff_pres_pa_raw;
@ -210,6 +211,7 @@ ETSAirspeed::cycle()
/* perform collection */
ret = collect();
if (OK != ret) {
perf_count(_comms_errors);
/* restart the measurement state machine */
@ -239,6 +241,7 @@ ETSAirspeed::cycle()
/* measurement phase */
ret = measure();
if (OK != ret) {
DEVICE_DEBUG("measure error");
}
@ -287,26 +290,31 @@ start(int i2c_bus)
{
int fd;
if (g_dev != nullptr)
if (g_dev != nullptr) {
errx(1, "already started");
}
/* create the driver */
g_dev = new ETSAirspeed(i2c_bus);
if (g_dev == nullptr)
if (g_dev == nullptr) {
goto fail;
}
if (OK != g_dev->Airspeed::init())
if (OK != g_dev->Airspeed::init()) {
goto fail;
}
/* set the poll rate to default, starts automatic data collection */
fd = open(AIRSPEED0_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);
@ -351,21 +359,24 @@ test()
int fd = open(ETS_PATH, O_RDONLY);
if (fd < 0)
if (fd < 0) {
err(1, "%s open failed (try 'ets_airspeed start' if the driver is not running", ETS_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("diff pressure: %f pa", (double)report.differential_pressure_filtered_pa);
/* 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++) {
@ -376,22 +387,25 @@ 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("diff pressure: %f pa", (double)report.differential_pressure_filtered_pa);
}
/* reset the sensor polling to its default rate */
if (OK != ioctl(fd, SENSORIOCSPOLLRATE, SENSOR_POLLRATE_DEFAULT))
if (OK != ioctl(fd, SENSORIOCSPOLLRATE, SENSOR_POLLRATE_DEFAULT)) {
errx(1, "failed to set default rate");
}
errx(0, "PASS");
}
@ -404,14 +418,17 @@ reset()
{
int fd = open(ETS_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);
}
@ -422,8 +439,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();
@ -462,32 +480,37 @@ ets_airspeed_main(int argc, char *argv[])
/*
* Start/load the driver.
*/
if (!strcmp(argv[1], "start"))
if (!strcmp(argv[1], "start")) {
ets_airspeed::start(i2c_bus);
}
/*
* Stop the driver
*/
if (!strcmp(argv[1], "stop"))
if (!strcmp(argv[1], "stop")) {
ets_airspeed::stop();
}
/*
* Test the driver/device.
*/
if (!strcmp(argv[1], "test"))
if (!strcmp(argv[1], "test")) {
ets_airspeed::test();
}
/*
* Reset the driver.
*/
if (!strcmp(argv[1], "reset"))
if (!strcmp(argv[1], "reset")) {
ets_airspeed::reset();
}
/*
* Print driver information.
*/
if (!strcmp(argv[1], "info") || !strcmp(argv[1], "status"))
if (!strcmp(argv[1], "info") || !strcmp(argv[1], "status")) {
ets_airspeed::info();
}
ets_airspeed_usage();
exit(0);

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

@ -138,7 +138,7 @@ protected:
extern "C" __EXPORT int meas_airspeed_main(int argc, char *argv[]);
MEASAirspeed::MEASAirspeed(int bus, int address, const char *path) : Airspeed(bus, address,
CONVERSION_INTERVAL, path),
CONVERSION_INTERVAL, path),
_filter(MEAS_RATE, MEAS_DRIVER_FILTER_FREQ),
_t_system_power(-1),
system_power{}
@ -189,9 +189,11 @@ MEASAirspeed::collect()
break;
case 1:
/* fallthrough */
/* fallthrough */
case 2:
/* fallthrough */
/* fallthrough */
case 3:
perf_count(_comms_errors);
perf_end(_sample_perf);
@ -219,11 +221,11 @@ MEASAirspeed::collect()
are generated when the bottom port is used as the static
port on the pitot and top port is used as the dynamic port
*/
float diff_press_PSI = -((dp_raw - 0.1f*16383) * (P_max-P_min)/(0.8f*16383) + P_min);
float diff_press_PSI = -((dp_raw - 0.1f * 16383) * (P_max - P_min) / (0.8f * 16383) + P_min);
float diff_press_pa_raw = diff_press_PSI * PSI_to_Pa;
// correct for 5V rail voltage if possible
voltage_correction(diff_press_pa_raw, temperature);
// correct for 5V rail voltage if possible
voltage_correction(diff_press_pa_raw, temperature);
// the raw value still should be compensated for the known offset
diff_press_pa_raw -= _diff_pres_offset;
@ -276,6 +278,7 @@ MEASAirspeed::cycle()
/* perform collection */
ret = collect();
if (OK != ret) {
/* restart the measurement state machine */
start();
@ -304,6 +307,7 @@ MEASAirspeed::cycle()
/* measurement phase */
ret = measure();
if (OK != ret) {
DEVICE_DEBUG("measure error");
}
@ -332,18 +336,23 @@ void
MEASAirspeed::voltage_correction(float &diff_press_pa, float &temperature)
{
#ifdef CONFIG_ARCH_BOARD_PX4FMU_V2
if (_t_system_power == -1) {
_t_system_power = orb_subscribe(ORB_ID(system_power));
}
if (_t_system_power == -1) {
// not available
return;
}
bool updated = false;
orb_check(_t_system_power, &updated);
if (updated) {
orb_copy(ORB_ID(system_power), _t_system_power, &system_power);
}
if (system_power.voltage5V_v < 3.0f || system_power.voltage5V_v > 6.0f) {
// not valid, skip correction
return;
@ -354,12 +363,15 @@ MEASAirspeed::voltage_correction(float &diff_press_pa, float &temperature)
apply a piecewise linear correction, flattening at 0.5V from 5V
*/
float voltage_diff = system_power.voltage5V_v - 5.0f;
if (voltage_diff > 0.5f) {
voltage_diff = 0.5f;
}
if (voltage_diff < -0.5f) {
voltage_diff = -0.5f;
}
diff_press_pa -= voltage_diff * slope;
/*
@ -367,12 +379,15 @@ MEASAirspeed::voltage_correction(float &diff_press_pa, float &temperature)
*/
const float temp_slope = 0.887f;
voltage_diff = system_power.voltage5V_v - 5.0f;
if (voltage_diff > 0.5f) {
voltage_diff = 0.5f;
}
if (voltage_diff < -1.0f) {
voltage_diff = -1.0f;
}
temperature -= voltage_diff * temp_slope;
#endif // CONFIG_ARCH_BOARD_PX4FMU_V2
}

1
src/drivers/pwm_out_sim/module.mk
View File

@ -38,6 +38,7 @@
MODULE_COMMAND = pwm_out_sim
SRCS = pwm_out_sim.cpp
MAXOPTIMIZATION = -Os
MODULE_STACKSIZE = 1200

5
src/drivers/pwm_out_sim/pwm_out_sim.cpp
View File

@ -234,8 +234,13 @@ PWMSim::init()
_task = px4_task_spawn_cmd("pwm_out_sim",
SCHED_DEFAULT,
SCHED_PRIORITY_DEFAULT,
<<<<<<< HEAD:src/drivers/pwm_out_sim/pwm_out_sim.cpp
1200,
(px4_main_t)&PWMSim::task_main_trampoline,
=======
1000,
(main_t)&HIL::task_main_trampoline,
>>>>>>> beta:src/drivers/hil/hil.cpp
nullptr);
if (_task < 0) {

2
src/examples/px4_daemon_app/px4_daemon_app.c
View File

@ -1,6 +1,6 @@
/****************************************************************************
*
* Copyright (c) 2012, 2013 PX4 Development Team. All rights reserved.
* Copyright (c) 2012-2015 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

5
src/examples/px4_mavlink_debug/px4_mavlink_debug.c
View File

@ -1,7 +1,6 @@
/****************************************************************************
*
* Copyright (C) 2012 PX4 Development Team. All rights reserved.
* Author: @author Example User <mail@example.com>
* Copyright (c) 2012-2015 PX4 Development Team. All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions
@ -35,6 +34,8 @@
/**
* @file px4_mavlink_debug.c
* Debug application example for PX4 autopilot
*
* @author Example User <mail@example.com>
*/
#include <px4_config.h>

5
src/examples/px4_simple_app/px4_simple_app.c
View File

@ -1,7 +1,6 @@
/****************************************************************************
*
* Copyright (C) 2012 PX4 Development Team. All rights reserved.
* Author: @author Example User <mail@example.com>
* Copyright (c) 2012-2015 PX4 Development Team. All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions
@ -35,6 +34,8 @@
/**
* @file px4_simple_app.c
* Minimal application example for PX4 autopilot
*
* @author Example User <mail@example.com>
*/
#include <px4_config.h>