aboutsummaryrefslogtreecommitdiff
path: root/src/drivers
diff options
context:
space:
mode:
Diffstat (limited to 'src/drivers')
-rw-r--r--src/drivers/drv_rc_input.h7
-rw-r--r--src/drivers/l3gd20/l3gd20.cpp2
-rw-r--r--src/drivers/lsm303d/lsm303d.cpp2
-rw-r--r--src/drivers/mpu6000/mpu6000.cpp15
-rw-r--r--src/drivers/ms5611/ms5611_spi.cpp2
-rw-r--r--src/drivers/px4io/px4io.cpp109
-rw-r--r--src/drivers/stm32/drv_hrt.c45
7 files changed, 129 insertions, 53 deletions
diff --git a/src/drivers/drv_rc_input.h b/src/drivers/drv_rc_input.h
index 78ffad9d7..6f773b82a 100644
--- a/src/drivers/drv_rc_input.h
+++ b/src/drivers/drv_rc_input.h
@@ -1,6 +1,6 @@
/****************************************************************************
*
- * Copyright (C) 2012 PX4 Development Team. All rights reserved.
+ * Copyright (c) 2012, 2013 PX4 Development Team. All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions
@@ -60,7 +60,7 @@
/**
* Maximum number of R/C input channels in the system. S.Bus has up to 18 channels.
*/
-#define RC_INPUT_MAX_CHANNELS 18
+#define RC_INPUT_MAX_CHANNELS 20
/**
* Input signal type, value is a control position from zero to 100
@@ -89,6 +89,9 @@ struct rc_input_values {
/** number of channels actually being seen */
uint32_t channel_count;
+ /** receive signal strength indicator (RSSI): < 0: Undefined, 0: no signal, 1000: full reception */
+ int32_t rssi;
+
/** Input source */
enum RC_INPUT_SOURCE input_source;
diff --git a/src/drivers/l3gd20/l3gd20.cpp b/src/drivers/l3gd20/l3gd20.cpp
index d639acba1..670e51b97 100644
--- a/src/drivers/l3gd20/l3gd20.cpp
+++ b/src/drivers/l3gd20/l3gd20.cpp
@@ -316,7 +316,7 @@ private:
};
L3GD20::L3GD20(int bus, const char* path, spi_dev_e device) :
- SPI("L3GD20", path, bus, device, SPIDEV_MODE3, 8000000),
+ SPI("L3GD20", path, bus, device, SPIDEV_MODE3, 11*1000*1000 /* will be rounded to 10.4 MHz, within margins for L3GD20 */),
_call_interval(0),
_reports(nullptr),
_gyro_range_scale(0.0f),
diff --git a/src/drivers/lsm303d/lsm303d.cpp b/src/drivers/lsm303d/lsm303d.cpp
index 11e5b95a7..969b5e25f 100644
--- a/src/drivers/lsm303d/lsm303d.cpp
+++ b/src/drivers/lsm303d/lsm303d.cpp
@@ -486,7 +486,7 @@ private:
LSM303D::LSM303D(int bus, const char* path, spi_dev_e device) :
- SPI("LSM303D", path, bus, device, SPIDEV_MODE3, 8000000),
+ SPI("LSM303D", path, bus, device, SPIDEV_MODE3, 11*1000*1000 /* will be rounded to 10.4 MHz, within safety margins for LSM303D */),
_mag(new LSM303D_mag(this)),
_call_accel_interval(0),
_call_mag_interval(0),
diff --git a/src/drivers/mpu6000/mpu6000.cpp b/src/drivers/mpu6000/mpu6000.cpp
index c95d11c83..bbc595af4 100644
--- a/src/drivers/mpu6000/mpu6000.cpp
+++ b/src/drivers/mpu6000/mpu6000.cpp
@@ -170,7 +170,7 @@
SPI speed
*/
#define MPU6000_LOW_BUS_SPEED 1000*1000
-#define MPU6000_HIGH_BUS_SPEED 10*1000*1000
+#define MPU6000_HIGH_BUS_SPEED 11*1000*1000 /* will be rounded to 10.4 MHz, within margins for MPU6K */
class MPU6000_gyro;
@@ -505,17 +505,26 @@ out:
void MPU6000::reset()
{
+ // if the mpu6000 is initialised after the l3gd20 and lsm303d
+ // then if we don't do an irqsave/irqrestore here the mpu6000
+ // frequenctly comes up in a bad state where all transfers
+ // come as zero
+ irqstate_t state;
+ state = irqsave();
- // Chip reset
write_reg(MPUREG_PWR_MGMT_1, BIT_H_RESET);
up_udelay(10000);
- // Wake up device and select GyroZ clock (better performance)
+ // Wake up device and select GyroZ clock. Note that the
+ // MPU6000 starts up in sleep mode, and it can take some time
+ // for it to come out of sleep
write_reg(MPUREG_PWR_MGMT_1, MPU_CLK_SEL_PLLGYROZ);
up_udelay(1000);
// Disable I2C bus (recommended on datasheet)
write_reg(MPUREG_USER_CTRL, BIT_I2C_IF_DIS);
+ irqrestore(state);
+
up_udelay(1000);
// SAMPLE RATE
diff --git a/src/drivers/ms5611/ms5611_spi.cpp b/src/drivers/ms5611/ms5611_spi.cpp
index bc4074c55..26216e840 100644
--- a/src/drivers/ms5611/ms5611_spi.cpp
+++ b/src/drivers/ms5611/ms5611_spi.cpp
@@ -121,7 +121,7 @@ MS5611_spi_interface(ms5611::prom_u &prom_buf)
}
MS5611_SPI::MS5611_SPI(int bus, spi_dev_e device, ms5611::prom_u &prom_buf) :
- SPI("MS5611_SPI", nullptr, bus, device, SPIDEV_MODE3, 6*1000*1000),
+ SPI("MS5611_SPI", nullptr, bus, device, SPIDEV_MODE3, 11*1000*1000 /* will be rounded to 10.4 MHz */),
_prom(prom_buf)
{
}
diff --git a/src/drivers/px4io/px4io.cpp b/src/drivers/px4io/px4io.cpp
index b8edeed69..4e9daf910 100644
--- a/src/drivers/px4io/px4io.cpp
+++ b/src/drivers/px4io/px4io.cpp
@@ -237,6 +237,7 @@ private:
unsigned _update_interval; ///< Subscription interval limiting send rate
bool _rc_handling_disabled; ///< If set, IO does not evaluate, but only forward the RC values
+ unsigned _rc_chan_count; ///< Internal copy of the last seen number of RC channels
volatile int _task; ///< worker task id
volatile bool _task_should_exit; ///< worker terminate flag
@@ -244,7 +245,9 @@ private:
int _mavlink_fd; ///< mavlink file descriptor. This is opened by class instantiation and Doesn't appear to be usable in main thread.
int _thread_mavlink_fd; ///< mavlink file descriptor for thread.
- perf_counter_t _perf_update; ///< local performance counter
+ perf_counter_t _perf_update; ///<local performance counter for status updates
+ perf_counter_t _perf_write; ///<local performance counter for PWM control writes
+ perf_counter_t _perf_chan_count; ///<local performance counter for channel number changes
/* cached IO state */
uint16_t _status; ///< Various IO status flags
@@ -462,11 +465,14 @@ PX4IO::PX4IO(device::Device *interface) :
_max_transfer(16), /* sensible default */
_update_interval(0),
_rc_handling_disabled(false),
+ _rc_chan_count(0),
_task(-1),
_task_should_exit(false),
_mavlink_fd(-1),
_thread_mavlink_fd(-1),
- _perf_update(perf_alloc(PC_ELAPSED, "px4io update")),
+ _perf_update(perf_alloc(PC_ELAPSED, "io update")),
+ _perf_write(perf_alloc(PC_ELAPSED, "io write")),
+ _perf_chan_count(perf_alloc(PC_COUNT, "io rc #")),
_status(0),
_alarms(0),
_t_actuator_controls_0(-1),
@@ -917,7 +923,23 @@ PX4IO::task_main()
/* re-upload RC input config as it may have changed */
io_set_rc_config();
+
+ /* re-set the battery scaling */
+ int32_t voltage_scaling_val;
+ param_t voltage_scaling_param;
+
+ /* set battery voltage scaling */
+ param_get(voltage_scaling_param = param_find("BAT_V_SCALE_IO"), &voltage_scaling_val);
+
+ /* send scaling voltage to IO */
+ uint16_t scaling = voltage_scaling_val;
+ int pret = io_reg_set(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_VBATT_SCALE, &scaling, 1);
+
+ if (pret != OK) {
+ log("voltage scaling upload failed");
+ }
}
+
}
perf_end(_perf_update);
@@ -1101,7 +1123,12 @@ PX4IO::io_set_rc_config()
if ((ichan >= 0) && (ichan < (int)_max_rc_input))
input_map[ichan - 1] = 3;
- ichan = 4;
+ param_get(param_find("RC_MAP_MODE_SW"), &ichan);
+
+ if ((ichan >= 0) && (ichan < (int)_max_rc_input))
+ input_map[ichan - 1] = 4;
+
+ ichan = 5;
for (unsigned i = 0; i < _max_rc_input; i++)
if (input_map[i] == -1)
@@ -1379,6 +1406,11 @@ PX4IO::io_get_raw_rc_input(rc_input_values &input_rc)
*/
channel_count = regs[0];
+ if (channel_count != _rc_chan_count)
+ perf_count(_perf_chan_count);
+
+ _rc_chan_count = channel_count;
+
if (channel_count > 9) {
ret = io_reg_get(PX4IO_PAGE_RAW_RC_INPUT, PX4IO_P_RAW_RC_BASE + 9, &regs[prolog + 9], channel_count - 9);
@@ -2227,7 +2259,10 @@ PX4IO::write(file * /*filp*/, const char *buffer, size_t len)
count = _max_actuators;
if (count > 0) {
+
+ perf_begin(_perf_write);
int ret = io_reg_set(PX4IO_PAGE_DIRECT_PWM, 0, (uint16_t *)buffer, count);
+ perf_end(_perf_write);
if (ret != OK)
return ret;
@@ -2306,7 +2341,7 @@ void
start(int argc, char *argv[])
{
if (g_dev != nullptr)
- errx(1, "already loaded");
+ errx(0, "already loaded");
/* allocate the interface */
device::Device *interface = get_interface();
@@ -2639,6 +2674,39 @@ px4io_main(int argc, char *argv[])
if_test((argc > 2) ? strtol(argv[2], NULL, 0) : 0);
}
+ if (!strcmp(argv[1], "forceupdate")) {
+ /*
+ force update of the IO firmware without requiring
+ the user to hold the safety switch down
+ */
+ if (argc <= 3) {
+ warnx("usage: px4io forceupdate MAGIC filename");
+ exit(1);
+ }
+ if (g_dev == nullptr) {
+ warnx("px4io is not started, still attempting upgrade");
+ } else {
+ uint16_t arg = atol(argv[2]);
+ int ret = g_dev->ioctl(nullptr, PX4IO_REBOOT_BOOTLOADER, arg);
+ if (ret != OK) {
+ printf("reboot failed - %d\n", ret);
+ exit(1);
+ }
+
+ // tear down the px4io instance
+ delete g_dev;
+ }
+
+ // upload the specified firmware
+ const char *fn[2];
+ fn[0] = argv[3];
+ fn[1] = nullptr;
+ PX4IO_Uploader *up = new PX4IO_Uploader;
+ up->upload(&fn[0]);
+ delete up;
+ exit(0);
+ }
+
/* commands below here require a started driver */
if (g_dev == nullptr)
@@ -2724,39 +2792,6 @@ px4io_main(int argc, char *argv[])
exit(0);
}
- if (!strcmp(argv[1], "forceupdate")) {
- /*
- force update of the IO firmware without requiring
- the user to hold the safety switch down
- */
- if (argc <= 3) {
- printf("usage: px4io forceupdate MAGIC filename\n");
- exit(1);
- }
- if (g_dev == nullptr) {
- printf("px4io is not started\n");
- exit(1);
- }
- uint16_t arg = atol(argv[2]);
- int ret = g_dev->ioctl(nullptr, PX4IO_REBOOT_BOOTLOADER, arg);
- if (ret != OK) {
- printf("reboot failed - %d\n", ret);
- exit(1);
- }
-
- // tear down the px4io instance
- delete g_dev;
-
- // upload the specified firmware
- const char *fn[2];
- fn[0] = argv[3];
- fn[1] = nullptr;
- PX4IO_Uploader *up = new PX4IO_Uploader;
- up->upload(&fn[0]);
- delete up;
- exit(0);
- }
-
if (!strcmp(argv[1], "checkcrc")) {
/*
check IO CRC against CRC of a file
diff --git a/src/drivers/stm32/drv_hrt.c b/src/drivers/stm32/drv_hrt.c
index 1bd251bc2..36226c941 100644
--- a/src/drivers/stm32/drv_hrt.c
+++ b/src/drivers/stm32/drv_hrt.c
@@ -338,7 +338,12 @@ static void hrt_call_invoke(void);
# define PPM_MIN_START 2500 /* shortest valid start gap */
/* decoded PPM buffer */
-#define PPM_MAX_CHANNELS 12
+#define PPM_MIN_CHANNELS 5
+#define PPM_MAX_CHANNELS 20
+
+/* Number of same-sized frames required to 'lock' */
+#define PPM_CHANNEL_LOCK 2 /* should be less than the input timeout */
+
__EXPORT uint16_t ppm_buffer[PPM_MAX_CHANNELS];
__EXPORT unsigned ppm_decoded_channels = 0;
__EXPORT uint64_t ppm_last_valid_decode = 0;
@@ -440,7 +445,7 @@ hrt_ppm_decode(uint32_t status)
if (status & SR_OVF_PPM)
goto error;
- /* how long since the last edge? */
+ /* how long since the last edge? - this handles counter wrapping implicitely. */
width = count - ppm.last_edge;
ppm.last_edge = count;
@@ -455,14 +460,38 @@ hrt_ppm_decode(uint32_t status)
*/
if (width >= PPM_MIN_START) {
- /* export the last set of samples if we got something sensible */
- if (ppm.next_channel > 4) {
- for (i = 0; i < ppm.next_channel && i < PPM_MAX_CHANNELS; i++)
- ppm_buffer[i] = ppm_temp_buffer[i];
+ /*
+ * If the number of channels changes unexpectedly, we don't want
+ * to just immediately jump on the new count as it may be a result
+ * of noise or dropped edges. Instead, take a few frames to settle.
+ */
+ if (ppm.next_channel != ppm_decoded_channels) {
+ static unsigned new_channel_count;
+ static unsigned new_channel_holdoff;
+
+ if (new_channel_count != ppm.next_channel) {
+ /* start the lock counter for the new channel count */
+ new_channel_count = ppm.next_channel;
+ new_channel_holdoff = PPM_CHANNEL_LOCK;
+
+ } else if (new_channel_holdoff > 0) {
+ /* this frame matched the last one, decrement the lock counter */
+ new_channel_holdoff--;
+
+ } else {
+ /* we have seen PPM_CHANNEL_LOCK frames with the new count, accept it */
+ ppm_decoded_channels = new_channel_count;
+ new_channel_count = 0;
+ }
- ppm_decoded_channels = i;
- ppm_last_valid_decode = hrt_absolute_time();
+ } else {
+ /* frame channel count matches expected, let's use it */
+ if (ppm.next_channel > PPM_MIN_CHANNELS) {
+ for (i = 0; i < ppm.next_channel; i++)
+ ppm_buffer[i] = ppm_temp_buffer[i];
+ ppm_last_valid_decode = hrt_absolute_time();
+ }
}
/* reset for the next frame */
generated by cgit v1.2.3 (git 2.39.1) at 2024-09-10 11:14:33 +0000