fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-10-21 09:13:33 +08:00
Code Issues Packages Projects Releases Wiki Activity

merge from upstream

Browse Source
This commit is contained in:
luft27
2015-08-03 15:37:39 +03:00
parent bcc7b3d110 5fd88f4d3e
commit 0ba1774fcb
390 changed files with 15141 additions and 6508 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/drivers/boards/aerocore/board_config.h
+12
View File
@@ -112,6 +112,18 @@ __BEGIN_DECLS
#define GPIO_GPIO10_OUTPUT (GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_2MHz|GPIO_OUTPUT_CLEAR|GPIO_PORTB|GPIO_PIN5)
#define GPIO_GPIO11_OUTPUT (GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_2MHz|GPIO_OUTPUT_CLEAR|GPIO_PORTB|GPIO_PIN8)
/*
* ADC channels
*
* These are the channel numbers of the ADCs of the microcontroller that can be used by the Px4 Firmware in the adc driver
*/
#define ADC_CHANNELS (1 << 10) | (1 << 11) | (1 << 12) | (1 << 13)
// ADC defines to be used in sensors.cpp to read from a particular channel
#define ADC_BATTERY_VOLTAGE_CHANNEL 10
#define ADC_BATTERY_CURRENT_CHANNEL ((uint8_t)(-1))
#define ADC_AIRSPEED_VOLTAGE_CHANNEL ((uint8_t)(-1))
/* PWM
*
* Eight PWM outputs are configured.
src/drivers/boards/px4fmu-v1/board_config.h
+12
View File
@@ -120,6 +120,18 @@ __BEGIN_DECLS
#define PX4_I2C_OBDEV_PX4IO_BL 0x18
#define PX4_I2C_OBDEV_PX4IO 0x1a
/*
* ADC channels
*
* These are the channel numbers of the ADCs of the microcontroller that can be used by the Px4 Firmware in the adc driver
*/
#define ADC_CHANNELS (1 << 10) | (1 << 11) | (1 << 12) | (1 << 13)
// ADC defines to be used in sensors.cpp to read from a particular channel
#define ADC_BATTERY_VOLTAGE_CHANNEL 10
#define ADC_BATTERY_CURRENT_CHANNEL ((uint8_t)(-1))
#define ADC_AIRSPEED_VOLTAGE_CHANNEL 11
/* User GPIOs
*
* GPIO0-1 are the buffered high-power GPIOs.
src/drivers/boards/px4fmu-v2/board_config.h
+13
View File
@@ -146,6 +146,19 @@ __BEGIN_DECLS
#define PX4_I2C_OBDEV_LED 0x55
#define PX4_I2C_OBDEV_HMC5883 0x1e
/*
* ADC channels
*
* These are the channel numbers of the ADCs of the microcontroller that can be used by the Px4 Firmware in the adc driver
*/
#define ADC_CHANNELS (1 << 2) | (1 << 3) | (1 << 4) | (1 << 10) | (1 << 11) | (1 << 12) | (1 << 13) | (1 << 14) | (1 << 15)
// ADC defines to be used in sensors.cpp to read from a particular channel
#define ADC_BATTERY_VOLTAGE_CHANNEL 2
#define ADC_BATTERY_CURRENT_CHANNEL 3
#define ADC_5V_RAIL_SENSE 4
#define ADC_AIRSPEED_VOLTAGE_CHANNEL 15
/* User GPIOs
*
* GPIO0-5 are the PWM servo outputs.
src/drivers/device/device_posix.cpp
+7 -10
View File
@@ -85,11 +85,10 @@ Device::log(const char *fmt, ...)
PX4_INFO("[%s] ", _name);
va_start(ap, fmt);
PX4_INFO( fmt, ap );
//vprintf(fmt, ap);
vprintf(fmt, ap);
va_end(ap);
//printf("\n");
//fflush(stdout);
printf("\n");
fflush(stdout);
}
void
@@ -98,14 +97,12 @@ Device::debug(const char *fmt, ...)
va_list ap;
if (_debug_enabled) {
PX4_INFO("<%s> ", _name);
//printf("<%s> ", _name);
printf("<%s> ", _name);
va_start(ap, fmt);
//vprintf(fmt, ap);
PX4_INFO(fmt, ap);
vprintf(fmt, ap);
va_end(ap);
//printf("\n");
//fflush(stdout);
printf("\n");
fflush(stdout);
}
}
src/drivers/device/vdev.cpp
+2 -1
View File
@@ -64,7 +64,7 @@ private:
px4_dev_t() {}
};
#define PX4_MAX_DEV 100
#define PX4_MAX_DEV 200
static px4_dev_t *devmap[PX4_MAX_DEV];
/*
@@ -324,6 +324,7 @@ VDev::ioctl(file_t *filep, int cmd, unsigned long arg)
case DEVIOCGDEVICEID:
ret = (int)_device_id.devid;
PX4_INFO("IOCTL DEVIOCGDEVICEID %d", ret);
break;
default:
break;
}
src/drivers/device/vdev_posix.cpp
+11 -36
View File
@@ -43,6 +43,7 @@
#include "device.h"
#include "vfile.h"
#include <hrt_work.h>
#include <stdlib.h>
#include <stdio.h>
#include <string.h>
@@ -53,29 +54,14 @@ using namespace device;
extern "C" {
struct timerData {
sem_t &sem;
struct timespec &ts;
timerData(sem_t &s, struct timespec &t) : sem(s), ts(t) {}
~timerData() {}
};
static void *timer_handler(void *data)
static void timer_cb(void *data)
{
struct timerData *td = (struct timerData *)data;
if (td->ts.tv_sec) {
sleep(td->ts.tv_sec);
}
usleep(td->ts.tv_nsec/1000);
sem_post(&(td->sem));
sem_t *p_sem = (sem_t *)data;
sem_post(p_sem);
PX4_DEBUG("timer_handler: Timer expired");
return 0;
}
#define PX4_MAX_FD 100
#define PX4_MAX_FD 200
static device::file_t *filemap[PX4_MAX_FD] = {};
int px4_errno;
@@ -117,6 +103,7 @@ int px4_open(const char *path, int flags, ...)
ret = dev->open(filemap[i]);
}
else {
PX4_WARN("exceeded maximum number of file descriptors!");
ret = -ENOENT;
}
}
@@ -201,7 +188,7 @@ int px4_ioctl(int fd, int cmd, unsigned long arg)
px4_errno = -ret;
}
return (ret == 0) ? PX4_OK : PX4_ERROR;
return ret;
}
int px4_poll(px4_pollfd_struct_t *fds, nfds_t nfds, int timeout)
@@ -210,7 +197,6 @@ int px4_poll(px4_pollfd_struct_t *fds, nfds_t nfds, int timeout)
int count = 0;
int ret;
unsigned int i;
struct timespec ts;
PX4_DEBUG("Called px4_poll timeout = %d", timeout);
sem_init(&sem, 0, 0);
@@ -238,25 +224,15 @@ int px4_poll(px4_pollfd_struct_t *fds, nfds_t nfds, int timeout)
{
if (timeout >= 0)
{
pthread_t pt;
void *res;
// Use a work queue task
work_s _hpwork;
ts.tv_sec = timeout/1000;
ts.tv_nsec = (timeout % 1000)*1000000;
// Create a timer to unblock
struct timerData td(sem, ts);
int rv = pthread_create(&pt, NULL, timer_handler, (void *)&td);
if (rv != 0) {
count = -1;
goto cleanup;
}
hrt_work_queue(&_hpwork, (worker_t)&timer_cb, (void *)&sem, 1000*timeout);
sem_wait(&sem);
// Make sure timer thread is killed before sem goes
// out of scope
(void)pthread_cancel(pt);
(void)pthread_join(pt, &res);
hrt_work_cancel(&_hpwork);
}
else
{
@@ -282,7 +258,6 @@ int px4_poll(px4_pollfd_struct_t *fds, nfds_t nfds, int timeout)
}
}
cleanup:
sem_destroy(&sem);
return count;
src/drivers/drv_pwm_output.h
+3 -3
View File
@@ -84,7 +84,7 @@ __BEGIN_DECLS
/**
* Highest maximum PWM in us
*/
#define PWM_HIGHEST_MAX 2100
#define PWM_HIGHEST_MAX 2150
/**
* Default maximum PWM in us
@@ -94,7 +94,7 @@ __BEGIN_DECLS
/**
* Lowest PWM allowed as the maximum PWM
*/
#define PWM_LOWEST_MAX 1400
#define PWM_LOWEST_MAX 950
/**
* Do not output a channel with this value
@@ -156,7 +156,7 @@ ORB_DECLARE(output_pwm);
#define PWM_SERVO_DISARM _PX4_IOC(_PWM_SERVO_BASE, 1)
/** get default servo update rate */
#define PWM_SERVO_GET_DEFAULT_UPDATE_RATE _IOC(_PWM_SERVO_BASE, 2)
#define PWM_SERVO_GET_DEFAULT_UPDATE_RATE _PX4_IOC(_PWM_SERVO_BASE, 2)
/** set alternate servo update rate */
#define PWM_SERVO_SET_UPDATE_RATE _PX4_IOC(_PWM_SERVO_BASE, 3)
src/drivers/drv_tone_alarm.h
+1
View File
@@ -152,6 +152,7 @@ enum {
TONE_EKF_WARNING_TUNE,
TONE_BARO_WARNING_TUNE,
TONE_SINGLE_BEEP_TUNE,
TONE_HOME_SET,
TONE_NUMBER_OF_TUNES
};
src/drivers/gps/gps.cpp
+33 -15
View File
@@ -310,7 +310,7 @@ GPS::task_main()
_report_gps_pos.cog_rad = 0.0f;
_report_gps_pos.vel_ned_valid = true;
//no time and satellite information simulated
/* no time and satellite information simulated */
if (!(_pub_blocked)) {
@@ -351,28 +351,46 @@ GPS::task_main()
unlock();
/* the Ashtech driver lies about successful configuration and the
* MTK driver is not well tested, so we really only trust the UBX
* driver for an advance publication
*/
if (_Helper->configure(_baudrate) == 0) {
unlock();
//Publish initial report that we have access to a GPS
//Make sure to clear any stale data in case driver is reset
/* reset report */
memset(&_report_gps_pos, 0, sizeof(_report_gps_pos));
_report_gps_pos.timestamp_position = hrt_absolute_time();
_report_gps_pos.timestamp_variance = hrt_absolute_time();
_report_gps_pos.timestamp_velocity = hrt_absolute_time();
_report_gps_pos.timestamp_time = hrt_absolute_time();
if (!(_pub_blocked)) {
if (_report_gps_pos_pub != nullptr) {
orb_publish(ORB_ID(vehicle_gps_position), _report_gps_pos_pub, &_report_gps_pos);
if (_mode == GPS_DRIVER_MODE_UBX) {
/* Publish initial report that we have access to a GPS,
* but set all critical state fields to indicate we have
* no valid position lock
*/
} else {
_report_gps_pos_pub = orb_advertise(ORB_ID(vehicle_gps_position), &_report_gps_pos);
_report_gps_pos.timestamp_time = hrt_absolute_time();
/* reset the timestamps for data, because we have no data yet */
_report_gps_pos.timestamp_position = 0;
_report_gps_pos.timestamp_variance = 0;
_report_gps_pos.timestamp_velocity = 0;
/* set a massive variance */
_report_gps_pos.eph = 10000.0f;
_report_gps_pos.epv = 10000.0f;
_report_gps_pos.fix_type = 0;
if (!(_pub_blocked)) {
if (_report_gps_pos_pub != nullptr) {
orb_publish(ORB_ID(vehicle_gps_position), _report_gps_pos_pub, &_report_gps_pos);
} else {
_report_gps_pos_pub = orb_advertise(ORB_ID(vehicle_gps_position), &_report_gps_pos);
}
}
}
// GPS is obviously detected successfully, reset statistics
_Helper->reset_update_rates();
/* GPS is obviously detected successfully, reset statistics */
_Helper->reset_update_rates();
}
int helper_ret;
while ((helper_ret = _Helper->receive(TIMEOUT_5HZ)) > 0 && !_task_should_exit) {
src/drivers/gps/ubx.cpp
+10 -10
View File
@@ -83,7 +83,7 @@
/**** Warning macros, disable to save memory */
#define UBX_WARN(s, ...) {warnx(s, ## __VA_ARGS__);}
#define UBX_DEBUG(s, ...) {/*warnx(s, ## __VA_ARGS__);*/}
UBX::UBX(const int &fd, struct vehicle_gps_position_s *gps_position, struct satellite_info_s *satellite_info) :
_fd(fd),
@@ -212,7 +212,7 @@ UBX::configure(unsigned &baudrate)
} else {
_use_nav_pvt = true;
}
UBX_WARN("%susing NAV-PVT", _use_nav_pvt ? "" : "not ");
UBX_DEBUG("%susing NAV-PVT", _use_nav_pvt ? "" : "not ");
if (!_use_nav_pvt) {
configure_message_rate(UBX_MSG_NAV_TIMEUTC, 5);
@@ -271,9 +271,9 @@ UBX::wait_for_ack(const uint16_t msg, const unsigned timeout, const bool report)
ret = 0; // ACK received ok
} else if (report) {
if (_ack_state == UBX_ACK_GOT_NAK) {
UBX_WARN("ubx msg 0x%04x NAK", SWAP16((unsigned)msg));
UBX_DEBUG("ubx msg 0x%04x NAK", SWAP16((unsigned)msg));
} else {
UBX_WARN("ubx msg 0x%04x ACK timeout", SWAP16((unsigned)msg));
UBX_DEBUG("ubx msg 0x%04x ACK timeout", SWAP16((unsigned)msg));
}
}
@@ -566,7 +566,7 @@ UBX::payload_rx_init()
break;
case UBX_RXMSG_DISABLE: // disable unexpected messages
UBX_WARN("ubx msg 0x%04x len %u unexpected", SWAP16((unsigned)_rx_msg), (unsigned)_rx_payload_length);
UBX_DEBUG("ubx msg 0x%04x len %u unexpected", SWAP16((unsigned)_rx_msg), (unsigned)_rx_payload_length);
{
hrt_abstime t = hrt_absolute_time();
@@ -574,7 +574,7 @@ UBX::payload_rx_init()
if (t > _disable_cmd_last + DISABLE_MSG_INTERVAL) {
/* don't attempt for every message to disable, some might not be disabled */
_disable_cmd_last = t;
UBX_WARN("ubx disabling msg 0x%04x", SWAP16((unsigned)_rx_msg));
UBX_DEBUG("ubx disabling msg 0x%04x", SWAP16((unsigned)_rx_msg));
configure_message_rate(_rx_msg, 0);
}
}
@@ -677,16 +677,16 @@ UBX::payload_rx_add_mon_ver(const uint8_t b)
// Part 1 complete: decode Part 1 buffer and calculate hash for SW&HW version strings
_ubx_version = fnv1_32_str(_buf.payload_rx_mon_ver_part1.swVersion, FNV1_32_INIT);
_ubx_version = fnv1_32_str(_buf.payload_rx_mon_ver_part1.hwVersion, _ubx_version);
UBX_WARN("VER hash 0x%08x", _ubx_version);
UBX_WARN("VER hw \"%10s\"", _buf.payload_rx_mon_ver_part1.hwVersion);
UBX_WARN("VER sw \"%30s\"", _buf.payload_rx_mon_ver_part1.swVersion);
UBX_DEBUG("VER hash 0x%08x", _ubx_version);
UBX_DEBUG("VER hw \"%10s\"", _buf.payload_rx_mon_ver_part1.hwVersion);
UBX_DEBUG("VER sw \"%30s\"", _buf.payload_rx_mon_ver_part1.swVersion);
}
// fill Part 2 buffer
unsigned buf_index = (_rx_payload_index - sizeof(ubx_payload_rx_mon_ver_part1_t)) % sizeof(ubx_payload_rx_mon_ver_part2_t);
_buf.raw[buf_index] = b;
if (buf_index == sizeof(ubx_payload_rx_mon_ver_part2_t) - 1) {
// Part 2 complete: decode Part 2 buffer
UBX_WARN("VER ext \" %30s\"", _buf.payload_rx_mon_ver_part2.extension);
UBX_DEBUG("VER ext \" %30s\"", _buf.payload_rx_mon_ver_part2.extension);
}
}
src/drivers/hil/hil.cpp
+94 -19
View File
@@ -1,6 +1,6 @@
/****************************************************************************
*
* Copyright (c) 2012-2014 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
@@ -123,7 +123,7 @@ private:
bool _primary_pwm_device;
volatile bool _task_should_exit;
bool _armed;
static bool _armed;
MixerGroup *_mixers;
@@ -163,6 +163,8 @@ HIL *g_hil;
} // namespace
bool HIL::_armed = false;
HIL::HIL() :
#ifdef __PX4_NUTTX
CDev
@@ -180,7 +182,6 @@ HIL::HIL() :
_num_outputs(0),
_primary_pwm_device(false),
_task_should_exit(false),
_armed(false),
_mixers(nullptr)
{
_debug_enabled = true;
@@ -280,36 +281,42 @@ HIL::set_mode(Mode mode)
debug("MODE_2PWM");
/* multi-port with flow control lines as PWM */
_update_rate = 50; /* default output rate */
_num_outputs = 2;
break;
case MODE_4PWM:
debug("MODE_4PWM");
/* multi-port as 4 PWM outs */
_update_rate = 50; /* default output rate */
_num_outputs = 4;
break;
case MODE_8PWM:
debug("MODE_8PWM");
/* multi-port as 8 PWM outs */
_update_rate = 50; /* default output rate */
_num_outputs = 8;
break;
case MODE_12PWM:
debug("MODE_12PWM");
/* multi-port as 12 PWM outs */
_update_rate = 50; /* default output rate */
_num_outputs = 12;
break;
case MODE_16PWM:
debug("MODE_16PWM");
/* multi-port as 16 PWM outs */
_update_rate = 50; /* default output rate */
_num_outputs = 16;
break;
case MODE_NONE:
debug("MODE_NONE");
/* disable servo outputs and set a very low update rate */
_update_rate = 10;
_num_outputs = 0;
break;
default:
@@ -448,6 +455,7 @@ HIL::task_main()
/* get new value */
orb_copy(ORB_ID(actuator_armed), _t_armed, &aa);
_armed = aa.armed && !aa.lockdown;
}
}
@@ -471,7 +479,12 @@ HIL::control_callback(uintptr_t handle,
{
const actuator_controls_s *controls = (actuator_controls_s *)handle;
input = controls->control[control_index];
if (_armed) {
input = controls->control[control_index];
} else {
/* clamp actuator to zero if not armed */
input = 0.0f;
}
return 0;
}
@@ -480,13 +493,6 @@ HIL::ioctl(device::file_t *filp, int cmd, unsigned long arg)
{
int ret;
debug("ioctl 0x%04x 0x%08x", cmd, arg);
// /* try it as a GPIO ioctl first */
// ret = HIL::gpio_ioctl(filp, cmd, arg);
// if (ret != -ENOTTY)
// return ret;
/* if we are in valid PWM mode, try it as a PWM ioctl as well */
switch(_mode) {
case MODE_2PWM:
@@ -540,6 +546,62 @@ HIL::pwm_ioctl(device::file_t *filp, int cmd, unsigned long arg)
// HIL always outputs at the alternate (usually faster) rate
break;
case PWM_SERVO_GET_DEFAULT_UPDATE_RATE:
*(uint32_t *)arg = 400;
break;
case PWM_SERVO_GET_UPDATE_RATE:
*(uint32_t *)arg = 400;
break;
case PWM_SERVO_GET_SELECT_UPDATE_RATE:
*(uint32_t *)arg = 0;
break;
case PWM_SERVO_GET_FAILSAFE_PWM: {
struct pwm_output_values *pwm = (struct pwm_output_values *)arg;
for (unsigned i = 0; i < _num_outputs; i++) {
pwm->values[i] = 850;
}
pwm->channel_count = _num_outputs;
break;
}
case PWM_SERVO_GET_DISARMED_PWM: {
struct pwm_output_values *pwm = (struct pwm_output_values *)arg;
for (unsigned i = 0; i < _num_outputs; i++) {
pwm->values[i] = 900;
}
pwm->channel_count = _num_outputs;
break;
}
case PWM_SERVO_GET_MIN_PWM: {
struct pwm_output_values *pwm = (struct pwm_output_values *)arg;
for (unsigned i = 0; i < _num_outputs; i++) {
pwm->values[i] = 1000;
}
pwm->channel_count = _num_outputs;
break;
}
case PWM_SERVO_GET_MAX_PWM: {
struct pwm_output_values *pwm = (struct pwm_output_values *)arg;
for (unsigned i = 0; i < _num_outputs; i++) {
pwm->values[i] = 2000;
}
pwm->channel_count = _num_outputs;
break;
}
case PWM_SERVO_SET(2):
case PWM_SERVO_SET(3):
if (_mode != MODE_4PWM) {
@@ -560,18 +622,26 @@ HIL::pwm_ioctl(device::file_t *filp, int cmd, unsigned long arg)
break;
case PWM_SERVO_GET(2):
case PWM_SERVO_GET(7):
case PWM_SERVO_GET(6):
case PWM_SERVO_GET(5):
case PWM_SERVO_GET(4):
if (_num_outputs < 8) {
ret = -EINVAL;
break;
}
case PWM_SERVO_GET(3):
if (_mode != MODE_4PWM) {
case PWM_SERVO_GET(2):
if (_num_outputs < 4) {
ret = -EINVAL;
break;
}
/* FALLTHROUGH */
case PWM_SERVO_GET(0):
case PWM_SERVO_GET(1): {
// channel = cmd - PWM_SERVO_SET(0);
// *(servo_position_t *)arg = up_pwm_servo_get(channel);
case PWM_SERVO_GET(1):
case PWM_SERVO_GET(0): {
*(servo_position_t *)arg = 1500;
break;
}
@@ -583,11 +653,16 @@ HIL::pwm_ioctl(device::file_t *filp, int cmd, unsigned long arg)
break;
}
case PWM_SERVO_GET_COUNT:
case MIXERIOCGETOUTPUTCOUNT:
if (_mode == MODE_4PWM) {
*(unsigned *)arg = 4;
if (_mode == MODE_8PWM) {
*(unsigned *)arg = 8;
} else if (_mode == MODE_4PWM) {
*(unsigned *)arg = 4;
} else {
*(unsigned *)arg = 2;
}
src/drivers/hmc5883/hmc5883.cpp
+5 -5
View File
@@ -1124,8 +1124,8 @@ int HMC5883::calibrate(struct file *filp, unsigned enable)
}
}
/* read the sensor up to 50x, stopping when we have 10 good values */
for (uint8_t i = 0; i < 50 && good_count < 10; i++) {
/* read the sensor up to 100x, stopping when we have 30 good values */
for (uint8_t i = 0; i < 100 && good_count < 30; i++) {
struct pollfd fds;
/* wait for data to be ready */
@@ -1172,9 +1172,9 @@ int HMC5883::calibrate(struct file *filp, unsigned enable)
scaling[2] = sum_excited[2] / good_count;
/* set scaling in device */
mscale_previous.x_scale = scaling[0];
mscale_previous.y_scale = scaling[1];
mscale_previous.z_scale = scaling[2];
mscale_previous.x_scale = 1.0f / scaling[0];
mscale_previous.y_scale = 1.0f / scaling[1];
mscale_previous.z_scale = 1.0f / scaling[2];
ret = OK;
src/drivers/l3gd20/l3gd20.cpp
+6 -4
View File
@@ -179,6 +179,8 @@ static const int ERROR = -1;
#define L3GD20_DEFAULT_FILTER_FREQ 30
#define L3GD20_TEMP_OFFSET_CELSIUS 40
#define L3GD20_MAX_OFFSET 0.45f /**< max offset: 25 degrees/s */
#ifdef PX4_SPI_BUS_EXT
#define EXTERNAL_BUS PX4_SPI_BUS_EXT
#else
@@ -1102,18 +1104,18 @@ L3GD20::test_error()
int
L3GD20::self_test()
{
/* evaluate gyro offsets, complain if offset -> zero or larger than 6 dps */
if (fabsf(_gyro_scale.x_offset) > 0.1f || fabsf(_gyro_scale.x_offset) < 0.000001f)
/* evaluate gyro offsets, complain if offset -> zero or larger than 25 dps */
if (fabsf(_gyro_scale.x_offset) > L3GD20_MAX_OFFSET || fabsf(_gyro_scale.x_offset) < 0.000001f)
return 1;
if (fabsf(_gyro_scale.x_scale - 1.0f) > 0.3f)
return 1;
if (fabsf(_gyro_scale.y_offset) > 0.1f || fabsf(_gyro_scale.y_offset) < 0.000001f)
if (fabsf(_gyro_scale.y_offset) > L3GD20_MAX_OFFSET || fabsf(_gyro_scale.y_offset) < 0.000001f)
return 1;
if (fabsf(_gyro_scale.y_scale - 1.0f) > 0.3f)
return 1;
if (fabsf(_gyro_scale.z_offset) > 0.1f || fabsf(_gyro_scale.z_offset) < 0.000001f)
if (fabsf(_gyro_scale.z_offset) > L3GD20_MAX_OFFSET || fabsf(_gyro_scale.z_offset) < 0.000001f)
return 1;
if (fabsf(_gyro_scale.z_scale - 1.0f) > 0.3f)
return 1;
src/drivers/ll40ls/ll40ls.cpp
+2
View File
@@ -329,6 +329,8 @@ test(const bool use_i2c, const int bus)
}
warnx("periodic read %u", i);
warnx("valid %u", (float)report.current_distance > report.min_distance
&& (float)report.current_distance < report.max_distance ? 1 : 0);
warnx("measurement: %0.3f m", (double)report.current_distance);
warnx("time: %lld", report.timestamp);
}
src/drivers/mb12xx/mb12xx.cpp
+4
View File
@@ -832,6 +832,7 @@ test()
}
warnx("single read");
warnx("measurement: %0.2f m", (double)report.current_distance);
warnx("time: %llu", report.timestamp);
/* start the sensor polling at 2Hz */
@@ -860,6 +861,9 @@ test()
}
warnx("periodic read %u", i);
warnx("valid %u", (float)report.current_distance > report.min_distance
&& (float)report.current_distance < report.max_distance ? 1 : 0);
warnx("measurement: %0.3f", (double)report.current_distance);
warnx("time: %llu", report.timestamp);
}
src/drivers/mpu6000/mpu6000.cpp
+6 -2
View File
@@ -913,10 +913,14 @@ MPU6000::gyro_self_test()
/*
* Maximum deviation of 20 degrees, according to
* http://www.invensense.com/mems/gyro/documents/PS-MPU-6000A-00v3.4.pdf
* http://www.farnell.com/datasheets/1788002.pdf
* Section 6.1, initial ZRO tolerance
*
* 20 dps (0.34 rad/s) initial offset
* and 20 dps temperature drift, so 0.34 rad/s * 2
*/
const float max_offset = 0.34f;
const float max_offset = 2.0f * 0.34f;
/* 30% scale error is chosen to catch completely faulty units but
* to let some slight scale error pass. Requires a rate table or correlation
* with mag rotations + data fit to
src/drivers/ms5611/ms5611_i2c.cpp
+4 -4
View File
@@ -71,8 +71,8 @@ public:
virtual ~MS5611_I2C();
virtual int init();
virtual int dev_read(unsigned offset, void *data, unsigned count);
virtual int dev_ioctl(unsigned operation, unsigned &arg);
virtual int read(unsigned offset, void *data, unsigned count);
virtual int ioctl(unsigned operation, unsigned &arg);
#ifdef __PX4_NUTTX
protected:
@@ -139,7 +139,7 @@ MS5611_I2C::init()
}
int
MS5611_I2C::dev_read(unsigned offset, void *data, unsigned count)
MS5611_I2C::read(unsigned offset, void *data, unsigned count)
{
union _cvt {
uint8_t b[4];
@@ -162,7 +162,7 @@ MS5611_I2C::dev_read(unsigned offset, void *data, unsigned count)
}
int
MS5611_I2C::dev_ioctl(unsigned operation, unsigned &arg)
MS5611_I2C::ioctl(unsigned operation, unsigned &arg)
{
int ret;
src/drivers/ms5611/ms5611_nuttx.cpp
+18 -10
View File
@@ -154,10 +154,12 @@ protected:
/**
* Initialize the automatic measurement state machine and start it.
*
* @param delay_ticks the number of queue ticks before executing the next cycle
*
* @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_cycle();
void start_cycle(unsigned delay_ticks = 1);
/**
* Stop the automatic measurement state machine.
@@ -515,7 +517,7 @@ MS5611::ioctl(struct file *filp, int cmd, unsigned long arg)
}
void
MS5611::start_cycle()
MS5611::start_cycle(unsigned delay_ticks)
{
/* reset the report ring and state machine */
@@ -524,7 +526,7 @@ MS5611::start_cycle()
_reports->flush();
/* schedule a cycle to start things */
work_queue(HPWORK, &_work, (worker_t)&MS5611::cycle_trampoline, this, 1);
work_queue(HPWORK, &_work, (worker_t)&MS5611::cycle_trampoline, this, delay_ticks);
}
void
@@ -564,8 +566,11 @@ MS5611::cycle()
}
/* issue a reset command to the sensor */
_interface->ioctl(IOCTL_RESET, dummy);
/* reset the collection state machine and try again */
start_cycle();
/* reset the collection state machine and try again - we need
* to wait 2.8 ms after issuing the sensor reset command
* according to the MS5611 datasheet
*/
start_cycle(USEC2TICK(2800));
return;
}
@@ -594,7 +599,6 @@ MS5611::cycle()
/* measurement phase */
ret = measure();
if (ret != OK) {
//log("measure error %d", ret);
/* issue a reset command to the sensor */
_interface->ioctl(IOCTL_RESET, dummy);
/* reset the collection state machine and try again */
@@ -1182,26 +1186,30 @@ ms5611_main(int argc, char *argv[])
/*
* Start/load the driver.
*/
if (!strcmp(verb, "start"))
if (!strcmp(verb, "start")) {
ms5611::start(busid);
}
/*
* Test the driver/device.
*/
if (!strcmp(verb, "test"))
if (!strcmp(verb, "test")) {
ms5611::test(busid);
}
/*
* Reset the driver.
*/
if (!strcmp(verb, "reset"))
if (!strcmp(verb, "reset")) {
ms5611::reset(busid);
}
/*
* Print driver information.
*/
if (!strcmp(verb, "info"))
if (!strcmp(verb, "info")) {
ms5611::info();
}
/*
* Perform MSL pressure calibration given an altitude in metres
src/drivers/px4flow/px4flow.cpp
+83 -50
View File
@@ -636,9 +636,13 @@ namespace px4flow
#endif
const int ERROR = -1;
PX4FLOW *g_dev;
PX4FLOW *g_dev = nullptr;
bool start_in_progress = false;
void start();
const int START_RETRY_COUNT = 5;
const int START_RETRY_TIMEOUT = 1000;
int start();
void stop();
void test();
void reset();
@@ -647,74 +651,103 @@ void info();
/**
* Start the driver.
*/
void
int
start()
{
int fd;
/* entry check: */
if (start_in_progress) {
warnx("start already in progress");
return 1;
}
start_in_progress = true;
if (g_dev != nullptr) {
errx(1, "already started");
start_in_progress = false;
warnx("already started");
return 1;
}
/* create the driver */
g_dev = new PX4FLOW(PX4_I2C_BUS_EXPANSION);
warnx("scanning I2C buses for device..");
if (g_dev == nullptr) {
goto fail;
}
int retry_nr = 0;
while (1) {
const int busses_to_try[] = {
PX4_I2C_BUS_EXPANSION,
#ifdef PX4_I2C_BUS_ESC
PX4_I2C_BUS_ESC,
#endif
PX4_I2C_BUS_ONBOARD,
-1
};
if (OK != g_dev->init()) {
#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);
const int *cur_bus = busses_to_try;
while(*cur_bus != -1) {
/* create the driver */
/* warnx("trying bus %d", *cur_bus); */
g_dev = new PX4FLOW(*cur_bus);
if (g_dev == nullptr) {
goto fail;
/* this is a fatal error */
break;
}
if (OK != g_dev->init()) {
goto fail;
/* init the driver: */
if (OK == g_dev->init()) {
/* success! */
break;
}
#ifdef PX4_I2C_BUS_ESC
/* destroy it again because it failed. */
delete g_dev;
g_dev = nullptr;
/* try next! */
cur_bus++;
}
/* check whether we found it: */
if (*cur_bus != -1) {
/* check for failure: */
if (g_dev == nullptr) {
break;
}
/* set the poll rate to default, starts automatic data collection */
fd = open(PX4FLOW0_DEVICE_PATH, O_RDONLY);
if (fd < 0) {
break;
}
if (ioctl(fd, SENSORIOCSPOLLRATE, SENSOR_POLLRATE_MAX) < 0) {
break;
}
/* success! */
start_in_progress = false;
return 0;
}
if (retry_nr < START_RETRY_COUNT) {
/* lets not be too verbose */
// warnx("PX4FLOW not found on I2C busses. Retrying in %d ms. Giving up in %d retries.", START_RETRY_TIMEOUT, START_RETRY_COUNT - retry_nr);
usleep(START_RETRY_TIMEOUT * 1000);
retry_nr++;
} else {
break;
}
#endif
}
/* set the poll rate to default, starts automatic data collection */
fd = open(PX4FLOW0_DEVICE_PATH, O_RDONLY);
if (fd < 0) {
goto fail;
}
if (ioctl(fd, SENSORIOCSPOLLRATE, SENSOR_POLLRATE_MAX) < 0) {
goto fail;
}
exit(0);
fail:
if (g_dev != nullptr) {
delete g_dev;
g_dev = nullptr;
}
errx(1, "no PX4FLOW connected over I2C");
start_in_progress = false;
return 1;
}
/**
@@ -864,7 +897,7 @@ px4flow_main(int argc, char *argv[])
* Start/load the driver.
*/
if (!strcmp(argv[1], "start")) {
px4flow::start();
return px4flow::start();
}
/*
src/drivers/px4fmu/fmu.cpp
+8 -7
View File
@@ -164,7 +164,7 @@ private:
unsigned _num_failsafe_set;
unsigned _num_disarmed_set;
static bool arm_nothrottle() { return (_armed.ready_to_arm && !_armed.armed); }
static bool arm_nothrottle() { return (_armed.prearmed && !_armed.armed); }
static void task_main_trampoline(int argc, char *argv[]);
void task_main();
@@ -294,7 +294,8 @@ PX4FMU::PX4FMU() :
memset(_controls, 0, sizeof(_controls));
memset(_poll_fds, 0, sizeof(_poll_fds));
_debug_enabled = true;
/* only enable this during development */
_debug_enabled = false;
}
PX4FMU::~PX4FMU()
@@ -341,9 +342,9 @@ PX4FMU::init()
_class_instance = register_class_devname(PWM_OUTPUT_BASE_DEVICE_PATH);
if (_class_instance == CLASS_DEVICE_PRIMARY) {
log("default PWM output device");
/* lets not be too verbose */
} else if (_class_instance < 0) {
log("FAILED registering class device");
warnx("FAILED registering class device");
}
/* reset GPIOs */
@@ -535,11 +536,11 @@ PX4FMU::subscribe()
_poll_fds_num = 0;
for (unsigned i = 0; i < actuator_controls_s::NUM_ACTUATOR_CONTROL_GROUPS; i++) {
if (sub_groups & (1 << i)) {
warnx("subscribe to actuator_controls_%d", i);
debug("subscribe to actuator_controls_%d", i);
_control_subs[i] = orb_subscribe(_control_topics[i]);
}
if (unsub_groups & (1 << i)) {
warnx("unsubscribe from actuator_controls_%d", i);
debug("unsubscribe from actuator_controls_%d", i);
::close(_control_subs[i]);
_control_subs[i] = -1;
}
@@ -741,7 +742,7 @@ PX4FMU::task_main()
orb_copy(ORB_ID(actuator_armed), _armed_sub, &_armed);
/* update the armed status and check that we're not locked down */
bool set_armed = (_armed.armed || _armed.ready_to_arm) && !_armed.lockdown;
bool set_armed = (_armed.armed || _armed.prearmed) && !_armed.lockdown;
if (_servo_armed != set_armed) {
_servo_armed = set_armed;
src/drivers/sf0x/sf0x.cpp
+4 -2
View File
@@ -854,7 +854,7 @@ test()
}
warnx("single read");
warnx("val: %0.2f m", (double)report.current_distance);
warnx("measurement: %0.2f m", (double)report.current_distance);
warnx("time: %llu", report.timestamp);
/* start the sensor polling at 2 Hz rate */
@@ -885,7 +885,9 @@ test()
}
warnx("read #%u", i);
warnx("val: %0.3f m", (double)report.current_distance);
warnx("valid %u", (float)report.current_distance > report.min_distance
&& (float)report.current_distance < report.max_distance ? 1 : 0);
warnx("measurement: %0.3f m", (double)report.current_distance);
warnx("time: %llu", report.timestamp);
}
src/drivers/stm32/adc/adc.cpp
+2 -13
View File
@@ -410,19 +410,8 @@ int
adc_main(int argc, char *argv[])
{
if (g_adc == nullptr) {
#ifdef CONFIG_ARCH_BOARD_PX4FMU_V1
/* XXX this hardcodes the default channel set for PX4FMUv1 - should be configurable */
g_adc = new ADC((1 << 10) | (1 << 11) | (1 << 12) | (1 << 13));
#endif
#ifdef CONFIG_ARCH_BOARD_PX4FMU_V2
/* XXX this hardcodes the default channel set for PX4FMUv2 - should be configurable */
g_adc = new ADC((1 << 2) | (1 << 3) | (1 << 4) |
(1 << 10) | (1 << 11) | (1 << 12) | (1 << 13) | (1 << 14) | (1 << 15));
#endif
#ifdef CONFIG_ARCH_BOARD_AEROCORE
/* XXX this hardcodes the default channel set for AeroCore - should be configurable */
g_adc = new ADC((1 << 10) | (1 << 11) | (1 << 12) | (1 << 13));
#endif
/* XXX this hardcodes the default channel set for the board in board_config.h - should be configurable */
g_adc = new ADC(ADC_CHANNELS);
if (g_adc == nullptr)
errx(1, "couldn't allocate the ADC driver");
src/drivers/stm32/tone_alarm/tone_alarm.cpp
+2
View File
@@ -339,6 +339,7 @@ ToneAlarm::ToneAlarm() :
_default_tunes[TONE_EKF_WARNING_TUNE] = "MFT255L8ddd#d#eeff"; // ekf warning
_default_tunes[TONE_BARO_WARNING_TUNE] = "MFT255L4gf#fed#d"; // baro warning
_default_tunes[TONE_SINGLE_BEEP_TUNE] = "MFT100a8"; // single beep
_default_tunes[TONE_HOME_SET] = "MFT100L4>G#6A#6B#4";
_tune_names[TONE_STARTUP_TUNE] = "startup"; // startup tune
_tune_names[TONE_ERROR_TUNE] = "error"; // ERROR tone
@@ -354,6 +355,7 @@ ToneAlarm::ToneAlarm() :
_tune_names[TONE_EKF_WARNING_TUNE] = "ekf_warning"; // ekf warning
_tune_names[TONE_BARO_WARNING_TUNE] = "baro_warning"; // baro warning
_tune_names[TONE_SINGLE_BEEP_TUNE] = "beep"; // single beep
_tune_names[TONE_HOME_SET] = "home_set";
}
ToneAlarm::~ToneAlarm()
src/drivers/trone/trone.cpp
+8 -2
View File
@@ -125,6 +125,7 @@ private:
work_s _work;
ringbuffer::RingBuffer *_reports;
bool _sensor_ok;
uint8_t _valid;
int _measure_ticks;
bool _collect_phase;
int _class_instance;
@@ -211,7 +212,7 @@ static const uint8_t crc_table[] = {
0xfa, 0xfd, 0xf4, 0xf3
};
/* static uint8_t crc8(uint8_t *p, uint8_t len) {
static uint8_t crc8(uint8_t *p, uint8_t len) {
uint16_t i;
uint16_t crc = 0x0;
@@ -221,7 +222,7 @@ static const uint8_t crc_table[] = {
}
return crc & 0xFF;
}*/
}
/*
* Driver 'main' command.
@@ -234,6 +235,7 @@ TRONE::TRONE(int bus, int address) :
_max_distance(TRONE_MAX_DISTANCE),
_reports(nullptr),
_sensor_ok(false),
_valid(0),
_measure_ticks(0),
_collect_phase(false),
_class_instance(-1),
@@ -592,6 +594,10 @@ TRONE::collect()
/* TODO: set proper ID */
report.id = 0;
// This validation check can be used later
_valid = crc8(val, 2) == val[2] && (float)report.current_distance > report.min_distance
&& (float)report.current_distance < report.max_distance ? 1 : 0;
/* publish it, if we are the primary */
if (_distance_sensor_topic != nullptr) {
orb_publish(ORB_ID(distance_sensor), _distance_sensor_topic, &report);