forked from Archive/PX4-Autopilot
Support binding DSM2 and DSMX satellite receivers
The px4io bind command allows you to put a DSM satellite receiver into bind mode. Since this feature requires that the dsm VCC line (red wire) be cut and routed through relay one, it is not enabled by default in order not to affect those not using a DSM satellite receiver or wising to use relay one for other purposes. NOTE: Binding DSM2 satellites in 11-bit mode is not supported due to potential bug in some DSM2 receiver streams when in 11-bit mode. Furthermore the px4io software folds 11 bit data down to 10 bits so there is no resolution advantage to to 11-bit mode. To enable the feature the RC_RL1_DSM_VCC parameter must be set to a non zero value from the console, or using QGroundControl: param set RC_RL1_DSM_VCC 1 From the console you can initiate DSM bind mode with: uorb start param set RC_RL1_DSM_VCC 1 px4io start px4io bind dsm2 For binding a DSMX satellite to a DSMX transmitter you would instead use: px4io bind dsmx Your receiver module should start a rapid flash and you can follow the normal binding sequence of your transmitter. Note: The value of parameter RC_RL1_DSM_VCC defaults to 0, so none of this will have any effect on an unmodified DSM receiver connection. For this feature to work, the power wire (red) must be cut and each side connected to a terminal on relay1 of the px4io board. This has been tested using Spektrum as well as Hobby King 'Orange' DSM satellite receivers. Both px4fmu and px4io images are updated.
This commit is contained in:
Jean Cyr
parent
697c0a1a1d
commit
3f9f2018e2
|
|
@ -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)
|
||||
|
||||
|
|
|
|||
|
|
@ -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,9 +1635,71 @@ 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)
|
||||
{
|
||||
|
|
@ -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'");
|
||||
}
|
||||
|
|
|
|||
|
|
@ -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];
|
||||
|
|
|
|||
|
|
@ -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.
|
||||
*/
|
||||
|
|
|
|||
|
|
@ -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 */
|
||||
|
|
|
|||
|
|
@ -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);
|
||||
|
||||
|
|
|
|||
|
|
@ -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;
|
||||
}
|
||||
|
|
|
|||
|
|
@ -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));
|
||||
|
|
|
|||
|
|
@ -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;
|
||||
}
|
||||
|
||||
|
|
|
|||
Loading…
Reference in New Issue