aboutsummaryrefslogtreecommitdiff
diff options
context:
space:
mode:
Diffstat
-rw-r--r--Documentation/dsm_bind.odtbin0 -> 27043 bytes
-rw-r--r--Documentation/dsm_bind.pdfbin0 -> 34300 bytes
-rw-r--r--src/drivers/drv_pwm_output.h9
-rw-r--r--src/drivers/px4io/px4io.cpp138
-rw-r--r--src/modules/px4iofirmware/controls.c11
-rw-r--r--src/modules/px4iofirmware/dsm.c41
-rw-r--r--src/modules/px4iofirmware/protocol.h9
-rw-r--r--src/modules/px4iofirmware/px4io.h1
-rw-r--r--src/modules/px4iofirmware/registers.c4
-rw-r--r--src/modules/sensors/sensor_params.c1
-rw-r--r--src/modules/sensors/sensors.cpp12
11 files changed, 208 insertions, 18 deletions
diff --git a/Documentation/dsm_bind.odt b/Documentation/dsm_bind.odt
new file mode 100644
index 000000000..66ea1f1be
--- /dev/null
+++ b/Documentation/dsm_bind.odt
Binary files differ
diff --git a/Documentation/dsm_bind.pdf b/Documentation/dsm_bind.pdf
new file mode 100644
index 000000000..e62d1ed83
--- /dev/null
+++ b/Documentation/dsm_bind.pdf
Binary files differ
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..1adefdea5 100644
--- a/src/drivers/px4io/px4io.cpp
+++ b/src/drivers/px4io/px4io.cpp
@@ -129,6 +129,16 @@ public:
*/
void print_status();
+ inline void set_dsm_vcc_ctl(bool enable)
+ {
+ _dsm_vcc_ctl = enable;
+ };
+
+ inline bool get_dsm_vcc_ctl()
+ {
+ return _dsm_vcc_ctl;
+ };
+
private:
// XXX
unsigned _max_actuators;
@@ -173,6 +183,12 @@ private:
uint64_t _battery_last_timestamp;
/**
+ * Relay1 is dedicated to controlling DSM receiver power
+ */
+
+ bool _dsm_vcc_ctl;
+
+ /**
* Trampoline to the worker task
*/
static void task_main_trampoline(int argc, char *argv[]);
@@ -313,7 +329,7 @@ PX4IO *g_dev;
}
PX4IO::PX4IO() :
- I2C("px4io", "/dev/px4io", PX4_I2C_BUS_ONBOARD, PX4_I2C_OBDEV_PX4IO, 320000),
+ I2C("px4io", GPIO_DEVICE_PATH, PX4_I2C_BUS_ONBOARD, PX4_I2C_OBDEV_PX4IO, 320000),
_max_actuators(0),
_max_controls(0),
_max_rc_input(0),
@@ -338,7 +354,8 @@ PX4IO::PX4IO() :
_battery_amp_per_volt(90.0f/5.0f), // this matches the 3DR current sensor
_battery_amp_bias(0),
_battery_mamphour_total(0),
- _battery_last_timestamp(0)
+ _battery_last_timestamp(0),
+ _dsm_vcc_ctl(false)
{
/* we need this potentially before it could be set in task_main */
g_dev = this;
@@ -700,8 +717,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 +1286,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" : ""),
@@ -1372,7 +1388,8 @@ PX4IO::print_status()
}
int
-PX4IO::ioctl(file *filep, int cmd, unsigned long arg)
+PX4IO::ioctl(file * /*filep*/, int cmd, unsigned long arg)
+/* Make it obvious that file * isn't used here */
{
int ret = OK;
@@ -1424,6 +1441,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 */
@@ -1466,18 +1503,31 @@ PX4IO::ioctl(file *filep, int cmd, unsigned long arg)
break;
}
- case GPIO_RESET:
- ret = io_reg_set(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_RELAYS, 0);
+ case GPIO_RESET: {
+ uint32_t bits = (1 << _max_relays) - 1;
+ /* don't touch relay1 if it's controlling RX vcc */
+ if (_dsm_vcc_ctl)
+ bits &= ~1;
+ ret = io_reg_modify(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_RELAYS, bits, 0);
break;
+ }
case GPIO_SET:
arg &= ((1 << _max_relays) - 1);
- ret = io_reg_modify(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_RELAYS, 0, arg);
+ /* don't touch relay1 if it's controlling RX vcc */
+ if (_dsm_vcc_ctl & (arg & 1))
+ ret = -EINVAL;
+ else
+ ret = io_reg_modify(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_RELAYS, 0, arg);
break;
case GPIO_CLEAR:
arg &= ((1 << _max_relays) - 1);
- ret = io_reg_modify(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_RELAYS, arg, 0);
+ /* don't touch relay1 if it's controlling RX vcc */
+ if (_dsm_vcc_ctl & (arg & 1))
+ ret = -EINVAL;
+ else
+ ret = io_reg_modify(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_RELAYS, arg, 0);
break;
case GPIO_GET:
@@ -1614,10 +1664,65 @@ start(int argc, char *argv[])
errx(1, "driver init failed");
}
+ int dsm_vcc_ctl;
+
+ if (param_get(param_find("RC_RL1_DSM_VCC"), &dsm_vcc_ctl) == OK) {
+ if (dsm_vcc_ctl) {
+ g_dev->set_dsm_vcc_ctl(true);
+ g_dev->ioctl(nullptr, DSM_BIND_POWER_UP, 0);
+ }
+ }
exit(0);
}
void
+bind(int argc, char *argv[])
+{
+ int pulses;
+
+ if (g_dev == nullptr)
+ errx(1, "px4io must be started first");
+
+ if (!g_dev->get_dsm_vcc_ctl())
+ 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]);
+
+ g_dev->ioctl(nullptr, 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");
+ g_dev->ioctl(nullptr, DSM_BIND_STOP, 0);
+ g_dev->ioctl(nullptr, DSM_BIND_POWER_UP, 0);
+ close(console);
+ exit(0);
+ }
+ }
+ }
+}
+
+void
test(void)
{
int fd;
@@ -1626,7 +1731,7 @@ test(void)
int direction = 1;
int ret;
- fd = open("/dev/px4io", O_WRONLY);
+ fd = open(GPIO_DEVICE_PATH, O_WRONLY);
if (fd < 0)
err(1, "failed to open device");
@@ -1800,7 +1905,7 @@ px4io_main(int argc, char *argv[])
* We can cheat and call the driver directly, as it
* doesn't reference filp in ioctl()
*/
- g_dev->ioctl(NULL, PX4IO_INAIR_RESTART_ENABLE, 1);
+ g_dev->ioctl(nullptr, PX4IO_INAIR_RESTART_ENABLE, 1);
} else {
errx(1, "not loaded");
}
@@ -1844,7 +1949,7 @@ px4io_main(int argc, char *argv[])
/* we can cheat and call the driver directly, as it
* doesn't reference filp in ioctl()
*/
- int ret = g_dev->ioctl(NULL, PX4IO_SET_DEBUG, level);
+ int ret = g_dev->ioctl(nullptr, PX4IO_SET_DEBUG, level);
if (ret != 0) {
printf("SET_DEBUG failed - %d\n", ret);
exit(1);
@@ -1918,6 +2023,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..43d96fb06 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 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 f6f4d60c7..252c1b7a9 100644
--- a/src/modules/sensors/sensor_params.c
+++ b/src/modules/sensors/sensor_params.c
@@ -155,6 +155,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 1ded14a91..ae5a55109 100644
--- a/src/modules/sensors/sensors.cpp
+++ b/src/modules/sensors/sensors.cpp
@@ -230,6 +230,8 @@ private:
float rc_scale_flaps;
float battery_voltage_scaling;
+
+ int rc_rl1_DSM_VCC_control;
} _parameters; /**< local copies of interesting parameters */
struct {
@@ -278,6 +280,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 */
@@ -514,6 +518,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();
}
@@ -730,6 +737,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;
}
generated by cgit v1.2.3 (git 2.39.1) at 2024-09-08 11:20:49 +0000