fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-05-18 21:39:06 +08:00
Code Issues Packages Projects Releases Wiki Activity

Implement Spektrum DSM pairing in V2

Browse Source 
- Bind control for V2
- Relays and accessory power not supported on V2 hardware
This commit is contained in:
Jean Cyr 2013-09-18 20:04:22 -04:00
parent 626f433630
commit 89d3e1db28
8 changed files with 83 additions and 30 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

2
src/drivers/boards/px4io-v2/board_config.h
View File

@ -84,7 +84,7 @@
/* Power switch controls ******************************************************/
#define GPIO_SPEKTRUM_PWR_EN (GPIO_OUTPUT|GPIO_CNF_OUTPP|GPIO_MODE_50MHz|GPIO_OUTPUT_CLEAR|GPIO_PORTC|GPIO_PIN13)
#define GPIO_SPEKTRUM_PWR_EN (GPIO_OUTPUT|GPIO_CNF_OUTPP|GPIO_MODE_50MHz|GPIO_OUTPUT_SET|GPIO_PORTC|GPIO_PIN13)
#define GPIO_SERVO_FAULT_DETECT (GPIO_INPUT|GPIO_CNF_INPULLUP|GPIO_MODE_INPUT|GPIO_PORTA|GPIO_PIN15)

4
src/drivers/boards/px4io-v2/px4iov2_init.c
View File

@ -111,9 +111,7 @@ __EXPORT void stm32_boardinitialize(void)
stm32_configgpio(GPIO_BTN_SAFETY);
/* spektrum power enable is active high - disable it by default */
/* XXX might not want to do this on warm restart? */
stm32_gpiowrite(GPIO_SPEKTRUM_PWR_EN, false);
/* spektrum power enable is active high - enable it by default */
stm32_configgpio(GPIO_SPEKTRUM_PWR_EN);
stm32_configgpio(GPIO_SERVO_FAULT_DETECT);

67
src/drivers/px4io/px4io.cpp
View File

@ -204,6 +204,7 @@ public:
*/
int disable_rc_handling();
#ifdef CONFIG_ARCH_BOARD_PX4FMU_V1
/**
* Set the DSM VCC is controlled by relay one flag
*
@ -223,6 +224,9 @@ public:
{
return _dsm_vcc_ctl;
};
#endif
inline uint16_t system_status() const {return _status;}
private:
device::Device *_interface;
@ -274,8 +278,9 @@ private:
float _battery_mamphour_total;///<amp hours consumed so far
uint64_t _battery_last_timestamp;///<last amp hour calculation timestamp
#ifdef CONFIG_ARCH_BOARD_PX4FMU_V1
bool _dsm_vcc_ctl; ///<true if relay 1 controls DSM satellite RX power
#endif
/**
* Trampoline to the worker task
@ -461,8 +466,11 @@ PX4IO::PX4IO(device::Device *interface) :
_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),
_dsm_vcc_ctl(false)
_battery_last_timestamp(0)
#ifdef CONFIG_ARCH_BOARD_PX4FMU_V1
,_dsm_vcc_ctl(false)
#endif
{
/* we need this potentially before it could be set in task_main */
g_dev = this;
@ -855,7 +863,7 @@ PX4IO::task_main()
// See if bind parameter has been set, and reset it to -1
param_get(dsm_bind_param = param_find("RC_DSM_BIND"), &dsm_bind_val);
if (dsm_bind_val >= 0) {
if (dsm_bind_val > -1) {
dsm_bind_ioctl(dsm_bind_val);
dsm_bind_val = -1;
param_set(dsm_bind_param, &dsm_bind_val);
@ -1169,7 +1177,7 @@ PX4IO::dsm_bind_ioctl(int dsmMode)
{
if (!(_status & PX4IO_P_STATUS_FLAGS_SAFETY_OFF)) {
/* 0: dsm2, 1:dsmx */
if ((dsmMode >= 0) && (dsmMode <= 1)) {
if ((dsmMode == 0) || (dsmMode == 1)) {
mavlink_log_info(_thread_mavlink_fd, "[IO] binding dsm%c rx", (dsmMode == 0) ? '2' : 'x');
ioctl(nullptr, DSM_BIND_START, (dsmMode == 0) ? DSM2_BIND_PULSES : DSMX_BIND_PULSES);
} else {
@ -1638,11 +1646,19 @@ PX4IO::print_status()
((arming & PX4IO_P_SETUP_ARMING_FAILSAFE_CUSTOM) ? " FAILSAFE_CUSTOM" : ""),
((arming & PX4IO_P_SETUP_ARMING_INAIR_RESTART_OK) ? " INAIR_RESTART_OK" : ""),
((arming & PX4IO_P_SETUP_ARMING_ALWAYS_PWM_ENABLE) ? " ALWAYS_PWM_ENABLE" : ""));
#ifdef CONFIG_ARCH_BOARD_PX4FMU_V1
printf("rates 0x%04x default %u alt %u relays 0x%04x\n",
io_reg_get(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_PWM_RATES),
io_reg_get(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_PWM_DEFAULTRATE),
io_reg_get(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_PWM_ALTRATE),
io_reg_get(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_RELAYS));
#endif
#ifdef CONFIG_ARCH_BOARD_PX4FMU_V2
printf("rates 0x%04x default %u alt %u\n",
io_reg_get(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_PWM_RATES),
io_reg_get(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_PWM_DEFAULTRATE),
io_reg_get(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_PWM_ALTRATE));
#endif
printf("debuglevel %u\n", io_reg_get(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_SET_DEBUG));
printf("controls");
for (unsigned i = 0; i < _max_controls; i++)
@ -1783,36 +1799,58 @@ PX4IO::ioctl(file * /*filep*/, int cmd, unsigned long arg)
}
case GPIO_RESET: {
#ifdef CONFIG_ARCH_BOARD_PX4FMU_V1
uint32_t bits = (1 << _max_relays) - 1;
/* don't touch relay1 if it's controlling RX vcc */
if (_dsm_vcc_ctl)
bits &= ~PX4IO_P_SETUP_RELAYS_POWER1;
ret = io_reg_modify(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_RELAYS, bits, 0);
#endif
#ifdef CONFIG_ARCH_BOARD_PX4FMU_V2
ret = -EINVAL;
#endif
break;
}
case GPIO_SET:
#ifdef CONFIG_ARCH_BOARD_PX4FMU_V1
arg &= ((1 << _max_relays) - 1);
/* don't touch relay1 if it's controlling RX vcc */
if (_dsm_vcc_ctl & (arg & PX4IO_P_SETUP_RELAYS_POWER1))
if (_dsm_vcc_ctl & (arg & PX4IO_P_SETUP_RELAYS_POWER1)) {
ret = -EINVAL;
else
ret = io_reg_modify(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_RELAYS, 0, arg);
break;
}
ret = io_reg_modify(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_RELAYS, 0, arg);
#endif
#ifdef CONFIG_ARCH_BOARD_PX4FMU_V2
ret = -EINVAL;
#endif
break;
case GPIO_CLEAR:
#ifdef CONFIG_ARCH_BOARD_PX4FMU_V1
arg &= ((1 << _max_relays) - 1);
/* don't touch relay1 if it's controlling RX vcc */
if (_dsm_vcc_ctl & (arg & PX4IO_P_SETUP_RELAYS_POWER1))
if (_dsm_vcc_ctl & (arg & PX4IO_P_SETUP_RELAYS_POWER1)) {
ret = -EINVAL;
else
break;
}
ret = io_reg_modify(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_RELAYS, arg, 0);
#endif
#ifdef CONFIG_ARCH_BOARD_PX4FMU_V2
ret = -EINVAL;
#endif
break;
case GPIO_GET:
#ifdef CONFIG_ARCH_BOARD_PX4FMU_V1
*(uint32_t *)arg = io_reg_get(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_RELAYS);
if (*(uint32_t *)arg == _io_reg_get_error)
ret = -EIO;
#endif
#ifdef CONFIG_ARCH_BOARD_PX4FMU_V2
ret = -EINVAL;
#endif
break;
case MIXERIOCGETOUTPUTCOUNT:
@ -1990,6 +2028,7 @@ start(int argc, char *argv[])
}
}
#ifdef CONFIG_ARCH_BOARD_PX4FMU_V1
int dsm_vcc_ctl;
if (param_get(param_find("RC_RL1_DSM_VCC"), &dsm_vcc_ctl) == OK) {
@ -1998,6 +2037,7 @@ start(int argc, char *argv[])
g_dev->ioctl(nullptr, DSM_BIND_POWER_UP, 0);
}
}
#endif
exit(0);
}
@ -2037,8 +2077,10 @@ bind(int argc, char *argv[])
if (g_dev == nullptr)
errx(1, "px4io must be started first");
#ifdef CONFIG_ARCH_BOARD_PX4FMU_V1
if (!g_dev->get_dsm_vcc_ctl())
errx(1, "DSM bind feature not enabled");
#endif
if (argc < 3)
errx(0, "needs argument, use dsm2 or dsmx");
@ -2049,9 +2091,12 @@ bind(int argc, char *argv[])
pulses = DSMX_BIND_PULSES;
else
errx(1, "unknown parameter %s, use dsm2 or dsmx", argv[2]);
if (g_dev->system_status() & PX4IO_P_STATUS_FLAGS_SAFETY_OFF)
errx(1, "system must not be armed");
#ifdef CONFIG_ARCH_BOARD_PX4FMU_V1
warnx("This command will only bind DSM if satellite VCC (red wire) is controlled by relay 1.");
#endif
g_dev->ioctl(nullptr, DSM_BIND_START, pulses);
exit(0);

21
src/modules/px4iofirmware/dsm.c
View File

@ -243,28 +243,35 @@ dsm_init(const char *device)
void
dsm_bind(uint16_t cmd, int pulses)
{
#if !(defined(CONFIG_ARCH_BOARD_PX4IO_V1) || defined(CONFIG_ARCH_BOARD_PX4IO_V2))
#warning DSM BIND NOT IMPLEMENTED ON UNKNOWN PLATFORM
#else
const uint32_t usart1RxAsOutp =
GPIO_OUTPUT | GPIO_CNF_OUTPP | GPIO_MODE_50MHz | GPIO_OUTPUT_SET | GPIO_PORTA | GPIO_PIN10;
if (dsm_fd < 0)
return;
#ifdef CONFIG_ARCH_BOARD_PX4IO_V2
// XXX implement
#warning DSM BIND NOT IMPLEMENTED ON PX4IO V2
#else
switch (cmd) {
case dsm_bind_power_down:
/*power down DSM satellite*/
#ifdef CONFIG_ARCH_BOARD_PX4IO_V1
POWER_RELAY1(0);
#else /* CONFIG_ARCH_BOARD_PX4IO_V2 */
POWER_SPEKTRUM(0);
#endif
break;
case dsm_bind_power_up:
/*power up DSM satellite*/
#ifdef CONFIG_ARCH_BOARD_PX4IO_V1
POWER_RELAY1(1);
#else /* CONFIG_ARCH_BOARD_PX4IO_V2 */
POWER_SPEKTRUM(1);
#endif
dsm_guess_format(true);
break;
@ -387,8 +394,10 @@ dsm_decode(hrt_abstime frame_time, uint16_t *values, uint16_t *num_values)
values[channel] = value;
}
if (dsm_channel_shift == 11)
if (dsm_channel_shift == 11) {
/* Set the 11-bit data indicator */
*num_values |= 0x8000;
}
/*
* XXX Note that we may be in failsafe here; we need to work out how to detect that.
@ -412,7 +421,7 @@ dsm_decode(hrt_abstime frame_time, uint16_t *values, uint16_t *num_values)
* Upon receiving a full dsm frame we attempt to decode it.
*
* @param[out] values pointer to per channel array of decoded values
* @param[out] num_values pointer to number of raw channel values returned
* @param[out] num_values pointer to number of raw channel values returned, high order bit 0:10 bit data, 1:11 bit data
* @return true=decoded raw channel values updated, false=no update
*/
bool

2
src/modules/px4iofirmware/protocol.h
View File

@ -165,11 +165,13 @@
#define PX4IO_P_SETUP_PWM_DEFAULTRATE 3 /* 'low' PWM frame output rate in Hz */
#define PX4IO_P_SETUP_PWM_ALTRATE 4 /* 'high' PWM frame output rate in Hz */
#if defined(CONFIG_ARCH_BOARD_PX4IO_V1) || defined(CONFIG_ARCH_BOARD_PX4FMU_V1)
#define PX4IO_P_SETUP_RELAYS 5 /* bitmask of relay/switch outputs, 0 = off, 1 = on */
#define PX4IO_P_SETUP_RELAYS_POWER1 (1<<0) /* hardware rev [1] power relay 1 */
#define PX4IO_P_SETUP_RELAYS_POWER2 (1<<1) /* hardware rev [1] power relay 2 */
#define PX4IO_P_SETUP_RELAYS_ACC1 (1<<2) /* hardware rev [1] accessory power 1 */
#define PX4IO_P_SETUP_RELAYS_ACC2 (1<<3) /* hardware rev [1] accessory power 2 */
#endif
#define PX4IO_P_SETUP_VBATT_SCALE 6 /* hardware rev [1] battery voltage correction factor (float) */
#define PX4IO_P_SETUP_VSERVO_SCALE 6 /* hardware rev [2] servo voltage correction factor (float) */

2
src/modules/px4iofirmware/px4io.h
View File

@ -100,7 +100,9 @@ extern uint16_t r_page_servo_idle[]; /* PX4IO_PAGE_IDLE_PWM */
#define r_setup_pwm_rates r_page_setup[PX4IO_P_SETUP_PWM_RATES]
#define r_setup_pwm_defaultrate r_page_setup[PX4IO_P_SETUP_PWM_DEFAULTRATE]
#define r_setup_pwm_altrate r_page_setup[PX4IO_P_SETUP_PWM_ALTRATE]
#ifdef CONFIG_ARCH_BOARD_PX4IO_V1
#define r_setup_relays r_page_setup[PX4IO_P_SETUP_RELAYS]
#endif
#define r_control_values (&r_page_controls[0])

12
src/modules/px4iofirmware/registers.c
View File

@ -145,7 +145,9 @@ volatile uint16_t r_page_setup[] =
[PX4IO_P_SETUP_PWM_RATES] = 0,
[PX4IO_P_SETUP_PWM_DEFAULTRATE] = 50,
[PX4IO_P_SETUP_PWM_ALTRATE] = 200,
#ifdef CONFIG_ARCH_BOARD_PX4IO_V1
[PX4IO_P_SETUP_RELAYS] = 0,
#endif
#ifdef ADC_VSERVO
[PX4IO_P_SETUP_VSERVO_SCALE] = 10000,
#else
@ -462,22 +464,16 @@ registers_set_one(uint8_t page, uint8_t offset, uint16_t value)
pwm_configure_rates(r_setup_pwm_rates, r_setup_pwm_defaultrate, value);
break;
#ifdef CONFIG_ARCH_BOARD_PX4IO_V1
case PX4IO_P_SETUP_RELAYS:
value &= PX4IO_P_SETUP_RELAYS_VALID;
r_setup_relays = value;
#ifdef POWER_RELAY1
POWER_RELAY1((value & PX4IO_P_SETUP_RELAYS_POWER1) ? 1 : 0);
#endif
#ifdef POWER_RELAY2
POWER_RELAY2((value & PX4IO_P_SETUP_RELAYS_POWER2) ? 1 : 0);
#endif
#ifdef POWER_ACC1
POWER_ACC1((value & PX4IO_P_SETUP_RELAYS_ACC1) ? 1 : 0);
#endif
#ifdef POWER_ACC2
POWER_ACC2((value & PX4IO_P_SETUP_RELAYS_ACC2) ? 1 : 0);
#endif
break;
#endif
case PX4IO_P_SETUP_VBATT_SCALE:
r_page_setup[PX4IO_P_SETUP_VBATT_SCALE] = value;

3
src/modules/sensors/sensor_params.c
View File

@ -164,8 +164,9 @@ PARAM_DEFINE_FLOAT(RC15_MAX, 2000);
PARAM_DEFINE_FLOAT(RC15_REV, 1.0f);
PARAM_DEFINE_FLOAT(RC15_DZ, 0.0f);
#ifdef CONFIG_ARCH_BOARD_PX4FMU_V1
PARAM_DEFINE_INT32(RC_RL1_DSM_VCC, 0); /* Relay 1 controls DSM VCC */
#endif
PARAM_DEFINE_INT32(RC_DSM_BIND, -1); /* -1 = Idle, 0 = Start DSM2 bind, 1 = Start DSMX bind */
#ifdef CONFIG_ARCH_BOARD_PX4FMU_V2