diff options
| -rw-r--r-- | src/drivers/drv_pwm_output.h | 9 | ||||
| -rw-r--r-- | src/drivers/px4io/px4io.cpp | 96 | ||||
| -rw-r--r-- | src/modules/px4iofirmware/controls.c | 13 | ||||
| -rw-r--r-- | src/modules/px4iofirmware/dsm.c | 43 | ||||
| -rw-r--r-- | src/modules/px4iofirmware/protocol.h | 9 | ||||
| -rw-r--r-- | src/modules/px4iofirmware/px4io.h | 1 | ||||
| -rw-r--r-- | src/modules/px4iofirmware/registers.c | 4 | ||||
| -rw-r--r-- | src/modules/sensors/sensor_params.c | 1 | ||||
| -rw-r--r-- | src/modules/sensors/sensors.cpp | 12 |
9 files changed, 178 insertions, 10 deletions
diff --git a/src/drivers/drv_pwm_output.h b/src/drivers/drv_pwm_output.h index 56af71059..52a667403 100644 --- a/src/drivers/drv_pwm_output.h +++ b/src/drivers/drv_pwm_output.h @@ -115,6 +115,15 @@ ORB_DECLARE(output_pwm); /** clear the 'ARM ok' bit, which deactivates the safety switch */ #define PWM_SERVO_CLEAR_ARM_OK _IOC(_PWM_SERVO_BASE, 6) +/** start DSM bind */ +#define DSM_BIND_START _IOC(_PWM_SERVO_BASE, 7) + +/** stop DSM bind */ +#define DSM_BIND_STOP _IOC(_PWM_SERVO_BASE, 8) + +/** Power up DSM receiver */ +#define DSM_BIND_POWER_UP _IOC(_PWM_SERVO_BASE, 9) + /** set a single servo to a specific value */ #define PWM_SERVO_SET(_servo) _IOC(_PWM_SERVO_BASE, 0x20 + _servo) diff --git a/src/drivers/px4io/px4io.cpp b/src/drivers/px4io/px4io.cpp index 19163cebe..54b9d50e4 100644 --- a/src/drivers/px4io/px4io.cpp +++ b/src/drivers/px4io/px4io.cpp @@ -89,6 +89,8 @@ #define PX4IO_SET_DEBUG _IOC(0xff00, 0) #define PX4IO_INAIR_RESTART_ENABLE _IOC(0xff00, 1) +static int dsm_vcc_ctl; + class PX4IO : public device::I2C { public: @@ -700,8 +702,6 @@ PX4IO::io_set_control_state() int PX4IO::set_failsafe_values(const uint16_t *vals, unsigned len) { - uint16_t regs[_max_actuators]; - if (len > _max_actuators) /* fail with error */ return E2BIG; @@ -1271,13 +1271,14 @@ PX4IO::print_status() printf("%u bytes free\n", io_reg_get(PX4IO_PAGE_STATUS, PX4IO_P_STATUS_FREEMEM)); uint16_t flags = io_reg_get(PX4IO_PAGE_STATUS, PX4IO_P_STATUS_FLAGS); - printf("status 0x%04x%s%s%s%s%s%s%s%s%s%s%s%s\n", + printf("status 0x%04x%s%s%s%s%s%s%s%s%s%s%s%s%s\n", flags, ((flags & PX4IO_P_STATUS_FLAGS_ARMED) ? " ARMED" : ""), ((flags & PX4IO_P_STATUS_FLAGS_OVERRIDE) ? " OVERRIDE" : ""), ((flags & PX4IO_P_STATUS_FLAGS_RC_OK) ? " RC_OK" : " RC_FAIL"), ((flags & PX4IO_P_STATUS_FLAGS_RC_PPM) ? " PPM" : ""), - ((flags & PX4IO_P_STATUS_FLAGS_RC_DSM) ? " DSM" : ""), + (((flags & PX4IO_P_STATUS_FLAGS_RC_DSM) && (!(flags & PX4IO_P_STATUS_FLAGS_RC_DSM11))) ? " DSM10" : ""), + (((flags & PX4IO_P_STATUS_FLAGS_RC_DSM) && (flags & PX4IO_P_STATUS_FLAGS_RC_DSM11)) ? " DSM11" : ""), ((flags & PX4IO_P_STATUS_FLAGS_RC_SBUS) ? " SBUS" : ""), ((flags & PX4IO_P_STATUS_FLAGS_FMU_OK) ? " FMU_OK" : " FMU_FAIL"), ((flags & PX4IO_P_STATUS_FLAGS_RAW_PWM) ? " RAW_PPM" : ""), @@ -1424,6 +1425,26 @@ PX4IO::ioctl(file *filep, int cmd, unsigned long arg) *(unsigned *)arg = _max_actuators; break; + case DSM_BIND_START: + io_reg_set(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_DSM, dsm_bind_power_down); + usleep(500000); + io_reg_set(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_DSM, dsm_bind_set_rx_out); + usleep(1000); + io_reg_set(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_DSM, dsm_bind_power_up); + usleep(100000); + io_reg_set(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_DSM, dsm_bind_send_pulses | (arg << 4)); + break; + + case DSM_BIND_STOP: + io_reg_set(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_DSM, dsm_bind_power_down); + io_reg_set(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_DSM, dsm_bind_reinit_uart); + usleep(500000); + break; + + case DSM_BIND_POWER_UP: + io_reg_set(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_DSM, dsm_bind_power_up); + break; + case PWM_SERVO_SET(0) ... PWM_SERVO_SET(PWM_OUTPUT_MAX_CHANNELS - 1): { /* TODO: we could go lower for e.g. TurboPWM */ @@ -1614,10 +1635,72 @@ start(int argc, char *argv[]) errx(1, "driver init failed"); } + if (param_get(param_find("RC_RL1_DSM_VCC"), &dsm_vcc_ctl) == OK) { + if (dsm_vcc_ctl) { + int fd = open("/dev/px4io", O_WRONLY); + if (fd < 0) + errx(1, "failed to open device"); + ioctl(fd, DSM_BIND_POWER_UP, 0); + close(fd); + } + } exit(0); } void +bind(int argc, char *argv[]) +{ + int fd, pulses; + + if (g_dev == nullptr) + errx(1, "px4io must be started first"); + + if (dsm_vcc_ctl == 0) + errx(1, "DSM bind feature not enabled"); + + if (argc < 3) + errx(0, "needs argument, use dsm2 or dsmx"); + + if (!strcmp(argv[2], "dsm2")) + pulses = 3; + else if (!strcmp(argv[2], "dsmx")) + pulses = 7; + else + errx(1, "unknown parameter %s, use dsm2 or dsmx", argv[2]); + + fd = open("/dev/px4io", O_WRONLY); + + if (fd < 0) + errx(1, "failed to open device"); + + ioctl(fd, DSM_BIND_START, pulses); + + /* Open console directly to grab CTRL-C signal */ + int console = open("/dev/console", O_NONBLOCK | O_RDONLY | O_NOCTTY); + if (!console) + errx(1, "failed opening console"); + + warnx("This command will only bind DSM if satellite VCC (red wire) is controlled by relay 1."); + warnx("Press CTRL-C or 'c' when done."); + + for (;;) { + usleep(500000L); + /* Check if user wants to quit */ + char c; + if (read(console, &c, 1) == 1) { + if (c == 0x03 || c == 0x63) { + warnx("Done\n"); + ioctl(fd, DSM_BIND_STOP, 0); + ioctl(fd, DSM_BIND_POWER_UP, 0); + close(fd); + close(console); + exit(0); + } + } + } +} + +void test(void) { int fd; @@ -1918,6 +2001,9 @@ px4io_main(int argc, char *argv[]) if (!strcmp(argv[1], "monitor")) monitor(); + if (!strcmp(argv[1], "bind")) + bind(argc, argv); + out: - errx(1, "need a command, try 'start', 'stop', 'status', 'test', 'monitor', 'debug', 'recovery', 'limit', 'current', 'failsafe' or 'update'"); + errx(1, "need a command, try 'start', 'stop', 'status', 'test', 'monitor', 'debug', 'recovery', 'limit', 'current', 'failsafe', 'bind', or 'update'"); } diff --git a/src/modules/px4iofirmware/controls.c b/src/modules/px4iofirmware/controls.c index 3cf9ca149..9561c9b1e 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); @@ -138,7 +145,7 @@ controls_tick() { /* map raw inputs to mapped inputs */ /* XXX mapping should be atomic relative to protocol */ - for (unsigned i = 0; i < r_raw_rc_count; i++) { + for (unsigned i = 0; i < (r_raw_rc_count & 0x7fff); i++) { /* map the input channel */ uint16_t *conf = &r_page_rc_input_config[i * PX4IO_P_RC_CONFIG_STRIDE]; diff --git a/src/modules/px4iofirmware/dsm.c b/src/modules/px4iofirmware/dsm.c index ea35e5513..79e892503 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 @@ -303,7 +339,7 @@ dsm_decode(hrt_abstime frame_time, uint16_t *values, uint16_t *num_values) for (unsigned i = 0; i < DSM_FRAME_CHANNELS; i++) { uint8_t *dp = &frame[2 + (2 * i)]; - uint16_t raw = (dp[0] << 8) | dp[1]; + uint16_t raw = ((uint16_t)dp[0] << 8) | dp[1]; unsigned channel, value; if (!dsm_decode_channel(raw, channel_shift, &channel, &value)) @@ -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 674f9dddd..6ee5c2834 100644 --- a/src/modules/px4iofirmware/protocol.h +++ b/src/modules/px4iofirmware/protocol.h @@ -105,6 +105,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) /* VBatt is very close to regulator dropout */ @@ -157,6 +158,14 @@ #define PX4IO_P_SETUP_PWM_ALTRATE 4 /* 'high' PWM frame output rate in Hz */ #define PX4IO_P_SETUP_RELAYS 5 /* bitmask of relay/switch outputs, 0 = off, 1 = on */ #define PX4IO_P_SETUP_VBATT_SCALE 6 /* battery voltage correction factor (float) */ +#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 +}; #define PX4IO_P_SETUP_SET_DEBUG 9 /* debug level for IO board */ /* autopilot control values, -10000..10000 */ diff --git a/src/modules/px4iofirmware/px4io.h b/src/modules/px4iofirmware/px4io.h index 272cdb7bf..83feeb9b6 100644 --- a/src/modules/px4iofirmware/px4io.h +++ b/src/modules/px4iofirmware/px4io.h @@ -184,6 +184,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 df7d6dcd3..805eb7ecc 100644 --- a/src/modules/px4iofirmware/registers.c +++ b/src/modules/px4iofirmware/registers.c @@ -360,6 +360,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; } diff --git a/src/modules/sensors/sensor_params.c b/src/modules/sensors/sensor_params.c index 230060148..a11c13568 100644 --- a/src/modules/sensors/sensor_params.c +++ b/src/modules/sensors/sensor_params.c @@ -151,6 +151,7 @@ PARAM_DEFINE_FLOAT(RC14_REV, 1.0f); PARAM_DEFINE_FLOAT(RC14_DZ, 0.0f); PARAM_DEFINE_INT32(RC_TYPE, 1); /** 1 = FUTABA, 2 = Spektrum, 3 = Graupner HoTT, 4 = Turnigy 9x */ +PARAM_DEFINE_INT32(RC_RL1_DSM_VCC, 0); /* Relay 1 controls DSM VCC */ /* default is conversion factor for the PX4IO / PX4IOAR board, the factor for PX4FMU standalone is different */ PARAM_DEFINE_FLOAT(BAT_V_SCALING, (3.3f * 52.0f / 5.0f / 4095.0f)); diff --git a/src/modules/sensors/sensors.cpp b/src/modules/sensors/sensors.cpp index 6b6aeedee..626cbade4 100644 --- a/src/modules/sensors/sensors.cpp +++ b/src/modules/sensors/sensors.cpp @@ -229,6 +229,8 @@ private: float rc_scale_flaps; float battery_voltage_scaling; + + int rc_rl1_DSM_VCC_control; } _parameters; /**< local copies of interesting parameters */ struct { @@ -276,6 +278,8 @@ private: param_t rc_scale_flaps; param_t battery_voltage_scaling; + + param_t rc_rl1_DSM_VCC_control; } _parameter_handles; /**< handles for interesting parameters */ @@ -509,6 +513,9 @@ Sensors::Sensors() : _parameter_handles.battery_voltage_scaling = param_find("BAT_V_SCALING"); + /* DSM VCC relay control */ + _parameter_handles.rc_rl1_DSM_VCC_control = param_find("RC_RL1_DSM_VCC"); + /* fetch initial parameter values */ parameters_update(); } @@ -722,6 +729,11 @@ Sensors::parameters_update() warnx("Failed updating voltage scaling param"); } + /* relay 1 DSM VCC control */ + if (param_get(_parameter_handles.rc_rl1_DSM_VCC_control, &(_parameters.rc_rl1_DSM_VCC_control)) != OK) { + warnx("Failed updating relay 1 DSM VCC control"); + } + return OK; } |