diff options
author | px4dev <px4@purgatory.org> | 2013-07-14 13:04:44 -0700 |
---|---|---|
committer | px4dev <px4@purgatory.org> | 2013-07-14 13:04:44 -0700 |
commit | dca9019f75ddf31a5477db935252eb009a0be655 (patch) | |
tree | 6fd0661f9e2c7035097bf2b044b33e95ae352caf /src/modules/px4iofirmware | |
parent | 6cf120831289368015b7b4f51db4f99f418e7129 (diff) | |
parent | 60ce9759d9d5a9b5f2e9fd218852fa595cc7bebd (diff) | |
download | px4-firmware-dca9019f75ddf31a5477db935252eb009a0be655.tar.gz px4-firmware-dca9019f75ddf31a5477db935252eb009a0be655.tar.bz2 px4-firmware-dca9019f75ddf31a5477db935252eb009a0be655.zip |
Merge branch 'master' of https://github.com/PX4/Firmware into fmuv2_bringup
Diffstat (limited to 'src/modules/px4iofirmware')
-rw-r--r-- | src/modules/px4iofirmware/controls.c | 11 | ||||
-rw-r--r-- | src/modules/px4iofirmware/dsm.c | 41 | ||||
-rw-r--r-- | src/modules/px4iofirmware/protocol.h | 10 | ||||
-rw-r--r-- | src/modules/px4iofirmware/px4io.h | 1 | ||||
-rw-r--r-- | src/modules/px4iofirmware/registers.c | 12 |
5 files changed, 67 insertions, 8 deletions
diff --git a/src/modules/px4iofirmware/controls.c b/src/modules/px4iofirmware/controls.c index 5a95a8aa9..74037d1c2 100644 --- a/src/modules/px4iofirmware/controls.c +++ b/src/modules/px4iofirmware/controls.c @@ -95,9 +95,16 @@ controls_tick() { */ perf_begin(c_gather_dsm); - bool dsm_updated = dsm_input(r_raw_rc_values, &r_raw_rc_count); - if (dsm_updated) + uint16_t temp_count = r_raw_rc_count; + bool dsm_updated = dsm_input(r_raw_rc_values, &temp_count); + if (dsm_updated) { r_status_flags |= PX4IO_P_STATUS_FLAGS_RC_DSM; + r_raw_rc_count = temp_count & 0x7fff; + if (temp_count & 0x8000) + r_status_flags |= PX4IO_P_STATUS_FLAGS_RC_DSM11; + else + r_status_flags &= ~PX4IO_P_STATUS_FLAGS_RC_DSM11; + } perf_end(c_gather_dsm); perf_begin(c_gather_sbus); diff --git a/src/modules/px4iofirmware/dsm.c b/src/modules/px4iofirmware/dsm.c index ea35e5513..ab6e3fec4 100644 --- a/src/modules/px4iofirmware/dsm.c +++ b/src/modules/px4iofirmware/dsm.c @@ -40,6 +40,7 @@ */ #include <nuttx/config.h> +#include <nuttx/arch.h> #include <fcntl.h> #include <unistd.h> @@ -101,6 +102,41 @@ dsm_init(const char *device) return dsm_fd; } +void +dsm_bind(uint16_t cmd, int pulses) +{ + const uint32_t usart1RxAsOutp = GPIO_OUTPUT|GPIO_CNF_OUTPP|GPIO_MODE_50MHz|GPIO_OUTPUT_SET|GPIO_PORTA|GPIO_PIN10; + + if (dsm_fd < 0) + return; + + switch (cmd) { + case dsm_bind_power_down: + // power down DSM satellite + POWER_RELAY1(0); + break; + case dsm_bind_power_up: + POWER_RELAY1(1); + dsm_guess_format(true); + break; + case dsm_bind_set_rx_out: + stm32_configgpio(usart1RxAsOutp); + break; + case dsm_bind_send_pulses: + for (int i = 0; i < pulses; i++) { + stm32_gpiowrite(usart1RxAsOutp, false); + up_udelay(50); + stm32_gpiowrite(usart1RxAsOutp, true); + up_udelay(50); + } + break; + case dsm_bind_reinit_uart: + // Restore USART rx pin + stm32_configgpio(GPIO_USART1_RX); + break; + } +} + bool dsm_input(uint16_t *values, uint16_t *num_values) { @@ -218,7 +254,7 @@ dsm_guess_format(bool reset) /* * Iterate the set of sensible sniffed channel sets and see whether - * decoding in 10 or 11-bit mode has yielded anything we recognise. + * decoding in 10 or 11-bit mode has yielded anything we recognize. * * XXX Note that due to what seem to be bugs in the DSM2 high-resolution * stream, we may want to sniff for longer in some cases when we think we @@ -349,6 +385,9 @@ dsm_decode(hrt_abstime frame_time, uint16_t *values, uint16_t *num_values) values[channel] = value; } + if (channel_shift == 11) + *num_values |= 0x8000; + /* * XXX Note that we may be in failsafe here; we need to work out how to detect that. */ diff --git a/src/modules/px4iofirmware/protocol.h b/src/modules/px4iofirmware/protocol.h index fa57dfc3f..dc5c40638 100644 --- a/src/modules/px4iofirmware/protocol.h +++ b/src/modules/px4iofirmware/protocol.h @@ -106,6 +106,7 @@ #define PX4IO_P_STATUS_FLAGS_ARM_SYNC (1 << 9) /* the arming state between IO and FMU is in sync */ #define PX4IO_P_STATUS_FLAGS_INIT_OK (1 << 10) /* initialisation of the IO completed without error */ #define PX4IO_P_STATUS_FLAGS_FAILSAFE (1 << 11) /* failsafe is active */ +#define PX4IO_P_STATUS_FLAGS_RC_DSM11 (1 << 12) /* DSM input is 11 bit data */ #define PX4IO_P_STATUS_ALARMS 3 /* alarm flags - alarms latch, write 1 to a bit to clear it */ #define PX4IO_P_STATUS_ALARMS_VBATT_LOW (1 << 0) /* [1] VBatt is very close to regulator dropout */ @@ -169,7 +170,14 @@ #define PX4IO_P_SETUP_VBATT_SCALE 6 /* [1] battery voltage correction factor (float) */ #define PX4IO_P_SETUP_VSERVO_SCALE 6 /* [2] servo voltage correction factor (float) */ - /* 7 */ +#define PX4IO_P_SETUP_DSM 7 /* DSM bind state */ +enum { /* DSM bind states */ + dsm_bind_power_down = 0, + dsm_bind_power_up, + dsm_bind_set_rx_out, + dsm_bind_send_pulses, + dsm_bind_reinit_uart +}; /* 8 */ #define PX4IO_P_SETUP_SET_DEBUG 9 /* debug level for IO board */ diff --git a/src/modules/px4iofirmware/px4io.h b/src/modules/px4iofirmware/px4io.h index ccf175e45..bd78f2638 100644 --- a/src/modules/px4iofirmware/px4io.h +++ b/src/modules/px4iofirmware/px4io.h @@ -200,6 +200,7 @@ extern void controls_init(void); extern void controls_tick(void); extern int dsm_init(const char *device); extern bool dsm_input(uint16_t *values, uint16_t *num_values); +extern void dsm_bind(uint16_t cmd, int pulses); extern int sbus_init(const char *device); extern bool sbus_input(uint16_t *values, uint16_t *num_values); diff --git a/src/modules/px4iofirmware/registers.c b/src/modules/px4iofirmware/registers.c index 873ee73f1..9606faa86 100644 --- a/src/modules/px4iofirmware/registers.c +++ b/src/modules/px4iofirmware/registers.c @@ -360,16 +360,16 @@ registers_set_one(uint8_t page, uint8_t offset, uint16_t value) value &= PX4IO_P_SETUP_RELAYS_VALID; r_setup_relays = value; #ifdef POWER_RELAY1 - POWER_RELAY1(value & (1 << 0) ? 1 : 0); + POWER_RELAY1((value & PX4IO_P_SETUP_RELAYS_POWER1) ? 1 : 0); #endif #ifdef POWER_RELAY2 - POWER_RELAY2(value & (1 << 1) ? 1 : 0); + POWER_RELAY2((value & PX4IO_P_SETUP_RELAYS_POWER2) ? 1 : 0); #endif #ifdef POWER_ACC1 - POWER_ACC1(value & (1 << 2) ? 1 : 0); + POWER_ACC1((value & PX4IO_P_SETUP_RELAYS_ACC1) ? 1 : 0); #endif #ifdef POWER_ACC2 - POWER_ACC2(value & (1 << 3) ? 1 : 0); + POWER_ACC2((value & PX4IO_P_SETUP_RELAYS_ACC2) ? 1 : 0); #endif break; @@ -382,6 +382,10 @@ registers_set_one(uint8_t page, uint8_t offset, uint16_t value) isr_debug(0, "set debug %u\n", (unsigned)r_page_setup[PX4IO_P_SETUP_SET_DEBUG]); break; + case PX4IO_P_SETUP_DSM: + dsm_bind(value & 0x0f, (value >> 4) & 7); + break; + default: return -1; } |