fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-10-19 04:23:35 +08:00
Code Issues Packages Projects Releases Wiki Activity

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

Browse Source 
Signed-off-by: Mark Charlebois <charlebm@gmail.com>

Conflicts:
	src/modules/commander/gyro_calibration.cpp
	src/modules/mavlink/mavlink_ftp.cpp
This commit is contained in:
Mark Charlebois
2015-05-16 15:04:38 -07:00
parent 6a1c28fbc1 26954ece2c
commit 36f5d47ed9
45 changed files with 714 additions and 400 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/drivers/boards/px4-stm32f4discovery/board_config.h
+3 -2
View File
@@ -60,9 +60,10 @@ __BEGIN_DECLS
****************************************************************************************************/
/* Configuration ************************************************************************************/
/* PX4FMU GPIOs ***********************************************************************************/
/* LEDs */
/* PX4-STM32F4Discovery GPIOs ***********************************************************************************/
/* LEDs */
// LED1 green, LED2 orange, LED3 red, LED4 blue
#define GPIO_LED1 (GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_50MHz|\
GPIO_OUTPUT_CLEAR|GPIO_PORTD|GPIO_PIN12)
src/drivers/l3gd20/l3gd20.cpp
+24 -33
View File
@@ -189,6 +189,14 @@ static const int ERROR = -1;
#define SENSOR_BOARD_ROTATION_DEFAULT SENSOR_BOARD_ROTATION_270_DEG
#endif
/*
we set the timer interrupt to run a bit faster than the desired
sample rate and then throw away duplicates using the data ready bit.
This time reduction is enough to cope with worst case timing jitter
due to other timers
*/
#define L3GD20_TIMER_REDUCTION 200
extern "C" { __EXPORT int l3gd20_main(int argc, char *argv[]); }
class L3GD20 : public device::SPI
@@ -236,9 +244,9 @@ private:
unsigned _read;
perf_counter_t _sample_perf;
perf_counter_t _reschedules;
perf_counter_t _errors;
perf_counter_t _bad_registers;
perf_counter_t _duplicates;
uint8_t _register_wait;
@@ -410,9 +418,9 @@ L3GD20::L3GD20(int bus, const char* path, spi_dev_e device, enum Rotation rotati
_orientation(SENSOR_BOARD_ROTATION_DEFAULT),
_read(0),
_sample_perf(perf_alloc(PC_ELAPSED, "l3gd20_read")),
_reschedules(perf_alloc(PC_COUNT, "l3gd20_reschedules")),
_errors(perf_alloc(PC_COUNT, "l3gd20_errors")),
_bad_registers(perf_alloc(PC_COUNT, "l3gd20_bad_registers")),
_duplicates(perf_alloc(PC_COUNT, "l3gd20_duplicates")),
_register_wait(0),
_gyro_filter_x(L3GD20_DEFAULT_RATE, L3GD20_DEFAULT_FILTER_FREQ),
_gyro_filter_y(L3GD20_DEFAULT_RATE, L3GD20_DEFAULT_FILTER_FREQ),
@@ -449,9 +457,9 @@ L3GD20::~L3GD20()
/* delete the perf counter */
perf_free(_sample_perf);
perf_free(_reschedules);
perf_free(_errors);
perf_free(_bad_registers);
perf_free(_duplicates);
}
int
@@ -608,7 +616,9 @@ L3GD20::ioctl(struct file *filp, int cmd, unsigned long arg)
/* update interval for next measurement */
/* XXX this is a bit shady, but no other way to adjust... */
_call.period = _call_interval = ticks;
_call_interval = ticks;
_call.period = _call_interval - L3GD20_TIMER_REDUCTION;
/* adjust filters */
float cutoff_freq_hz = _gyro_filter_x.get_cutoff_freq();
@@ -834,7 +844,10 @@ L3GD20::start()
_reports->flush();
/* start polling at the specified rate */
hrt_call_every(&_call, 1000, _call_interval, (hrt_callout)&L3GD20::measure_trampoline, this);
hrt_call_every(&_call,
1000,
_call_interval - L3GD20_TIMER_REDUCTION,
(hrt_callout)&L3GD20::measure_trampoline, this);
}
void
@@ -899,12 +912,6 @@ L3GD20::measure_trampoline(void *arg)
dev->measure();
}
#ifdef GPIO_EXTI_GYRO_DRDY
# define L3GD20_USE_DRDY 1
#else
# define L3GD20_USE_DRDY 0
#endif
void
L3GD20::check_registers(void)
{
@@ -954,33 +961,17 @@ L3GD20::measure()
check_registers();
#if L3GD20_USE_DRDY
// if the gyro doesn't have any data ready then re-schedule
// for 100 microseconds later. This ensures we don't double
// read a value and then miss the next value
if (_bus == PX4_SPI_BUS_SENSORS && stm32_gpioread(GPIO_EXTI_GYRO_DRDY) == 0) {
perf_count(_reschedules);
hrt_call_delay(&_call, 100);
perf_end(_sample_perf);
return;
}
#endif
/* fetch data from the sensor */
memset(&raw_report, 0, sizeof(raw_report));
raw_report.cmd = ADDR_OUT_TEMP | DIR_READ | ADDR_INCREMENT;
transfer((uint8_t *)&raw_report, (uint8_t *)&raw_report, sizeof(raw_report));
#if L3GD20_USE_DRDY
if (_bus == PX4_SPI_BUS_SENSORS && (raw_report.status & 0xF) != 0xF) {
/*
we waited for DRDY, but did not see DRDY on all axes
when we captured. That means a transfer error of some sort
*/
perf_count(_errors);
return;
if (!(raw_report.status & STATUS_ZYXDA)) {
perf_end(_sample_perf);
perf_count(_duplicates);
return;
}
#endif
/*
* 1) Scale raw value to SI units using scaling from datasheet.
* 2) Subtract static offset (in SI units)
@@ -1071,9 +1062,9 @@ L3GD20::print_info()
{
printf("gyro reads: %u\n", _read);
perf_print_counter(_sample_perf);
perf_print_counter(_reschedules);
perf_print_counter(_errors);
perf_print_counter(_bad_registers);
perf_print_counter(_duplicates);
_reports->print_info("report queue");
::printf("checked_next: %u\n", _checked_next);
for (uint8_t i=0; i<L3GD20_NUM_CHECKED_REGISTERS; i++) {
src/drivers/lsm303d/lsm303d.cpp
+26 -20
View File
@@ -196,6 +196,7 @@ static const int ERROR = -1;
#define REG7_CONT_MODE_M ((0<<1) | (0<<0))
#define REG_STATUS_A_NEW_ZYXADA 0x08
#define INT_CTRL_M 0x12
#define INT_SRC_M 0x13
@@ -217,6 +218,14 @@ static const int ERROR = -1;
#define EXTERNAL_BUS 0
#endif
/*
we set the timer interrupt to run a bit faster than the desired
sample rate and then throw away duplicates using the data ready bit.
This time reduction is enough to cope with worst case timing jitter
due to other timers
*/
#define LSM303D_TIMER_REDUCTION 200
extern "C" { __EXPORT int lsm303d_main(int argc, char *argv[]); }
@@ -289,9 +298,9 @@ private:
perf_counter_t _accel_sample_perf;
perf_counter_t _mag_sample_perf;
perf_counter_t _accel_reschedules;
perf_counter_t _bad_registers;
perf_counter_t _bad_values;
perf_counter_t _accel_duplicates;
uint8_t _register_wait;
@@ -561,9 +570,9 @@ LSM303D::LSM303D(int bus, const char* path, spi_dev_e device, enum Rotation rota
_mag_read(0),
_accel_sample_perf(perf_alloc(PC_ELAPSED, "lsm303d_accel_read")),
_mag_sample_perf(perf_alloc(PC_ELAPSED, "lsm303d_mag_read")),
_accel_reschedules(perf_alloc(PC_COUNT, "lsm303d_accel_resched")),
_bad_registers(perf_alloc(PC_COUNT, "lsm303d_bad_registers")),
_bad_values(perf_alloc(PC_COUNT, "lsm303d_bad_values")),
_accel_duplicates(perf_alloc(PC_COUNT, "lsm303d_accel_duplicates")),
_register_wait(0),
_accel_filter_x(LSM303D_ACCEL_DEFAULT_RATE, LSM303D_ACCEL_DEFAULT_DRIVER_FILTER_FREQ),
_accel_filter_y(LSM303D_ACCEL_DEFAULT_RATE, LSM303D_ACCEL_DEFAULT_DRIVER_FILTER_FREQ),
@@ -622,7 +631,7 @@ LSM303D::~LSM303D()
perf_free(_mag_sample_perf);
perf_free(_bad_registers);
perf_free(_bad_values);
perf_free(_accel_reschedules);
perf_free(_accel_duplicates);
}
int
@@ -874,7 +883,9 @@ LSM303D::ioctl(struct file *filp, int cmd, unsigned long arg)
/* update interval for next measurement */
/* XXX this is a bit shady, but no other way to adjust... */
_accel_call.period = _call_accel_interval = ticks;
_call_accel_interval = ticks;
_accel_call.period = _call_accel_interval - LSM303D_TIMER_REDUCTION;
/* if we need to start the poll state machine, do it */
if (want_start)
@@ -1388,7 +1399,10 @@ LSM303D::start()
_mag_reports->flush();
/* start polling at the specified rate */
hrt_call_every(&_accel_call, 1000, _call_accel_interval, (hrt_callout)&LSM303D::measure_trampoline, this);
hrt_call_every(&_accel_call,
1000,
_call_accel_interval - LSM303D_TIMER_REDUCTION,
(hrt_callout)&LSM303D::measure_trampoline, this);
hrt_call_every(&_mag_call, 1000, _call_mag_interval, (hrt_callout)&LSM303D::mag_measure_trampoline, this);
}
@@ -1466,20 +1480,6 @@ LSM303D::measure()
check_registers();
// if the accel doesn't have any data ready then re-schedule
// for 100 microseconds later. This ensures we don't double
// read a value and then miss the next value.
// Note that DRDY is not available when the lsm303d is
// connected on the external bus
#ifdef GPIO_EXTI_ACCEL_DRDY
if (_bus == PX4_SPI_BUS_SENSORS && stm32_gpioread(GPIO_EXTI_ACCEL_DRDY) == 0) {
perf_count(_accel_reschedules);
hrt_call_delay(&_accel_call, 100);
perf_end(_accel_sample_perf);
return;
}
#endif
if (_register_wait != 0) {
// we are waiting for some good transfers before using
// the sensor again.
@@ -1493,6 +1493,12 @@ LSM303D::measure()
raw_accel_report.cmd = ADDR_STATUS_A | DIR_READ | ADDR_INCREMENT;
transfer((uint8_t *)&raw_accel_report, (uint8_t *)&raw_accel_report, sizeof(raw_accel_report));
if (!(raw_accel_report.status & REG_STATUS_A_NEW_ZYXADA)) {
perf_end(_accel_sample_perf);
perf_count(_accel_duplicates);
return;
}
/*
* 1) Scale raw value to SI units using scaling from datasheet.
* 2) Subtract static offset (in SI units)
@@ -1681,7 +1687,7 @@ LSM303D::print_info()
perf_print_counter(_mag_sample_perf);
perf_print_counter(_bad_registers);
perf_print_counter(_bad_values);
perf_print_counter(_accel_reschedules);
perf_print_counter(_accel_duplicates);
_accel_reports->print_info("accel reports");
_mag_reports->print_info("mag reports");
::printf("checked_next: %u\n", _checked_next);
src/drivers/mpu6000/mpu6000.cpp
+53 -5
View File
@@ -189,6 +189,14 @@
#define MPU6000_LOW_BUS_SPEED 1000*1000
#define MPU6000_HIGH_BUS_SPEED 11*1000*1000 /* will be rounded to 10.4 MHz, within margins for MPU6K */
/*
we set the timer interrupt to run a bit faster than the desired
sample rate and then throw away duplicates by comparing
accelerometer values. This time reduction is enough to cope with
worst case timing jitter due to other timers
*/
#define MPU6000_TIMER_REDUCTION 200
class MPU6000_gyro;
class MPU6000 : public device::SPI
@@ -257,6 +265,7 @@ private:
perf_counter_t _bad_registers;
perf_counter_t _good_transfers;
perf_counter_t _reset_retries;
perf_counter_t _duplicates;
perf_counter_t _system_latency_perf;
perf_counter_t _controller_latency_perf;
@@ -287,6 +296,10 @@ private:
// last temperature reading for print_info()
float _last_temperature;
// keep last accel reading for duplicate detection
uint16_t _last_accel[3];
bool _got_duplicate;
/**
* Start automatic measurement.
*/
@@ -509,6 +522,7 @@ MPU6000::MPU6000(int bus, const char *path_accel, const char *path_gyro, spi_dev
_bad_registers(perf_alloc(PC_COUNT, "mpu6000_bad_registers")),
_good_transfers(perf_alloc(PC_COUNT, "mpu6000_good_transfers")),
_reset_retries(perf_alloc(PC_COUNT, "mpu6000_reset_retries")),
_duplicates(perf_alloc(PC_COUNT, "mpu6000_duplicates")),
_system_latency_perf(perf_alloc_once(PC_ELAPSED, "sys_latency")),
_controller_latency_perf(perf_alloc_once(PC_ELAPSED, "ctrl_latency")),
_register_wait(0),
@@ -522,7 +536,9 @@ MPU6000::MPU6000(int bus, const char *path_accel, const char *path_gyro, spi_dev
_rotation(rotation),
_checked_next(0),
_in_factory_test(false),
_last_temperature(0)
_last_temperature(0),
_last_accel{},
_got_duplicate(false)
{
// disable debug() calls
_debug_enabled = false;
@@ -576,6 +592,8 @@ MPU6000::~MPU6000()
perf_free(_bad_transfers);
perf_free(_bad_registers);
perf_free(_good_transfers);
perf_free(_reset_retries);
perf_free(_duplicates);
}
int
@@ -1198,7 +1216,15 @@ MPU6000::ioctl(struct file *filp, int cmd, unsigned long arg)
/* update interval for next measurement */
/* XXX this is a bit shady, but no other way to adjust... */
_call.period = _call_interval = ticks;
_call_interval = ticks;
/*
set call interval faster then the sample time. We
then detect when we have duplicate samples and reject
them. This prevents aliasing due to a beat between the
stm32 clock and the mpu6000 clock
*/
_call.period = _call_interval - MPU6000_TIMER_REDUCTION;
/* if we need to start the poll state machine, do it */
if (want_start)
@@ -1476,7 +1502,10 @@ MPU6000::start()
_gyro_reports->flush();
/* start polling at the specified rate */
hrt_call_every(&_call, 1000, _call_interval, (hrt_callout)&MPU6000::measure_trampoline, this);
hrt_call_every(&_call,
1000,
_call_interval-MPU6000_TIMER_REDUCTION,
(hrt_callout)&MPU6000::measure_trampoline, this);
}
void
@@ -1578,14 +1607,32 @@ MPU6000::measure()
*/
mpu_report.cmd = DIR_READ | MPUREG_INT_STATUS;
check_registers();
// sensor transfer at high clock speed
set_frequency(MPU6000_HIGH_BUS_SPEED);
if (OK != transfer((uint8_t *)&mpu_report, ((uint8_t *)&mpu_report), sizeof(mpu_report)))
return;
check_registers();
/*
see if this is duplicate accelerometer data. Note that we
can't use the data ready interrupt status bit in the status
register as that also goes high on new gyro data, and when
we run with BITS_DLPF_CFG_256HZ_NOLPF2 the gyro is being
sampled at 8kHz, so we would incorrectly think we have new
data when we are in fact getting duplicate accelerometer data.
*/
if (!_got_duplicate && memcmp(&mpu_report.accel_x[0], &_last_accel[0], 6) == 0) {
// it isn't new data - wait for next timer
perf_end(_sample_perf);
perf_count(_duplicates);
_got_duplicate = true;
return;
}
memcpy(&_last_accel[0], &mpu_report.accel_x[0], 6);
_got_duplicate = false;
/*
* Convert from big to little endian
*/
@@ -1766,6 +1813,7 @@ MPU6000::print_info()
perf_print_counter(_bad_registers);
perf_print_counter(_good_transfers);
perf_print_counter(_reset_retries);
perf_print_counter(_duplicates);
_accel_reports->print_info("accel queue");
_gyro_reports->print_info("gyro queue");
::printf("checked_next: %u\n", _checked_next);
src/drivers/px4io/px4io.cpp
+4 -1
View File
@@ -909,6 +909,9 @@ PX4IO::task_main()
goto out;
}
/* Fetch initial flight termination circuit breaker state */
_cb_flighttermination = circuit_breaker_enabled("CBRK_FLIGHTTERM", CBRK_FLIGHTTERM_KEY);
/* poll descriptor */
pollfd fds[1];
fds[0].fd = _t_actuator_controls_0;
@@ -1079,7 +1082,7 @@ PX4IO::task_main()
}
}
/* Update Circuit breakers */
/* Check if the flight termination circuit breaker has been updated */
_cb_flighttermination = circuit_breaker_enabled("CBRK_FLIGHTTERM", CBRK_FLIGHTTERM_KEY);
param_get(param_find("RC_RSSI_PWM_CHAN"), &_rssi_pwm_chan);