fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-10-16 23:43:33 +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
Download Patch File Download Diff File Expand all files Collapse all files
src/drivers/boards/px4io-v2/board_config.h
+1 -1
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)
src/drivers/boards/px4io-v2/px4iov2_init.c
+1 -3
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);
src/drivers/px4io/px4io.cpp
+56 -11
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);