diff --git a/conf/modules/spektrum_soft_bind.xml b/conf/modules/spektrum_soft_bind.xml new file mode 100644 index 0000000000..9920ada163 --- /dev/null +++ b/conf/modules/spektrum_soft_bind.xml @@ -0,0 +1,25 @@ + + + + + Puts Spektrum in binding mode through software + + + + + + + + +
+ +
+ + + + + + + + + diff --git a/sw/airborne/mcu.c b/sw/airborne/mcu.c index 8dc94e6733..6136658415 100644 --- a/sw/airborne/mcu.c +++ b/sw/airborne/mcu.c @@ -83,7 +83,7 @@ void mcu_init(void) led_init(); #endif /* for now this means using spektrum */ -#if defined RADIO_CONTROL & defined RADIO_CONTROL_SPEKTRUM_PRIMARY_PORT & defined RADIO_CONTROL_BIND_IMPL_FUNC +#if defined RADIO_CONTROL & defined RADIO_CONTROL_SPEKTRUM_PRIMARY_PORT & defined RADIO_CONTROL_BIND_IMPL_FUNC & defined SPEKTRUM_BIND_PIN_PORT RADIO_CONTROL_BIND_IMPL_FUNC(); #endif #if USE_UART0 diff --git a/sw/airborne/modules/spektrum_soft_bind/spektrum_soft_bind.c b/sw/airborne/modules/spektrum_soft_bind/spektrum_soft_bind.c new file mode 100644 index 0000000000..82213df990 --- /dev/null +++ b/sw/airborne/modules/spektrum_soft_bind/spektrum_soft_bind.c @@ -0,0 +1,30 @@ +/* + * Copyright (C) Kevin van Hecke + * + * This file is part of paparazzi + * + * paparazzi is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation; either version 2, or (at your option) + * any later version. + * + * paparazzi is distributed in the hope that it will be useful, + * but WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + * GNU General Public License for more details. + * + * You should have received a copy of the GNU General Public License + * along with paparazzi; see the file COPYING. If not, see + * . + */ +/** + * @file "modules/spektrum_soft_bind/spektrum_soft_bind.c" + * @author Kevin van Hecke + * Puts Spektrum in binding mode through software + */ + +#include "modules/spektrum_soft_bind/spektrum_soft_bind_fbw.h" + +void spektrum_soft_bind_init() {} + + diff --git a/sw/airborne/modules/spektrum_soft_bind/spektrum_soft_bind.h b/sw/airborne/modules/spektrum_soft_bind/spektrum_soft_bind.h new file mode 100644 index 0000000000..0b1289d0a3 --- /dev/null +++ b/sw/airborne/modules/spektrum_soft_bind/spektrum_soft_bind.h @@ -0,0 +1,36 @@ +/* + * Copyright (C) Kevin van Hecke + * + * This file is part of paparazzi + * + * paparazzi is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation; either version 2, or (at your option) + * any later version. + * + * paparazzi is distributed in the hope that it will be useful, + * but WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + * GNU General Public License for more details. + * + * You should have received a copy of the GNU General Public License + * along with paparazzi; see the file COPYING. If not, see + * . + */ +/** + * @file "modules/spektrum_soft_bind/spektrum_soft_bind.h" + * @author Kevin van Hecke + * Puts Spektrum in binding mode through software + */ + +#ifndef SPEKTRUM_SOFT_BIND_H +#define SPEKTRUM_SOFT_BIND_H + +#if defined FBW +#include "modules/spektrum_soft_bind/spektrum_soft_bind_fbw.h" +#else +#include "modules/spektrum_soft_bind/spektrum_soft_bind_ap.h" +#endif + +#endif + diff --git a/sw/airborne/modules/spektrum_soft_bind/spektrum_soft_bind_ap.c b/sw/airborne/modules/spektrum_soft_bind/spektrum_soft_bind_ap.c new file mode 100644 index 0000000000..e87f977742 --- /dev/null +++ b/sw/airborne/modules/spektrum_soft_bind/spektrum_soft_bind_ap.c @@ -0,0 +1,38 @@ +/* + * Copyright (C) Kevin van Hecke + * + * This file is part of paparazzi + * + * paparazzi is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation; either version 2, or (at your option) + * any later version. + * + * paparazzi is distributed in the hope that it will be useful, + * but WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + * GNU General Public License for more details. + * + * You should have received a copy of the GNU General Public License + * along with paparazzi; see the file COPYING. If not, see + * . + */ +/** + * @file "modules/spektrum_soft_bind/spektrum_soft_bind_ap.c" + * @author Kevin van Hecke + * Puts Spektrum in binding mode through software + */ + +#include "modules/spektrum_soft_bind/spektrum_soft_bind_ap.h" +#include "subsystems/intermcu/intermcu_ap.h" +#include "led.h" + +void spektrum_soft_bind_init(void) {} + +uint8_t bind_soft_value; +void spektrum_soft_bind_click(uint8_t val __attribute__((unused))) +{ + intermcu_send_spektrum_bind(); +} + + diff --git a/sw/airborne/modules/spektrum_soft_bind/spektrum_soft_bind_ap.h b/sw/airborne/modules/spektrum_soft_bind/spektrum_soft_bind_ap.h new file mode 100644 index 0000000000..6cfcbde3ff --- /dev/null +++ b/sw/airborne/modules/spektrum_soft_bind/spektrum_soft_bind_ap.h @@ -0,0 +1,37 @@ +/* + * Copyright (C) Kevin van Hecke + * + * This file is part of paparazzi + * + * paparazzi is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation; either version 2, or (at your option) + * any later version. + * + * paparazzi is distributed in the hope that it will be useful, + * but WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + * GNU General Public License for more details. + * + * You should have received a copy of the GNU General Public License + * along with paparazzi; see the file COPYING. If not, see + * . + */ +/** + * @file "modules/spektrum_soft_bind/spektrum_soft_bind_ap.h" + * @author Kevin van Hecke + * Puts Spektrum in binding mode through software + */ + +#ifndef SPEKTRUM_AP_SOFT_BIND_H +#define SPEKTRUM_AP_SOFT_BIND_H + +#include "std.h" + +extern void spektrum_soft_bind_init(void); +extern void spektrum_soft_bind_click(uint8_t val); + +extern uint8_t bind_soft_value; + +#endif + diff --git a/sw/airborne/modules/spektrum_soft_bind/spektrum_soft_bind_fbw.c b/sw/airborne/modules/spektrum_soft_bind/spektrum_soft_bind_fbw.c new file mode 100644 index 0000000000..1496f27fad --- /dev/null +++ b/sw/airborne/modules/spektrum_soft_bind/spektrum_soft_bind_fbw.c @@ -0,0 +1,54 @@ +/* + * Copyright (C) Kevin van Hecke + * + * This file is part of paparazzi + * + * paparazzi is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation; either version 2, or (at your option) + * any later version. + * + * paparazzi is distributed in the hope that it will be useful, + * but WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + * GNU General Public License for more details. + * + * You should have received a copy of the GNU General Public License + * along with paparazzi; see the file COPYING. If not, see + * . + */ +/** + * @file "modules/spektrum_soft_bind/spektrum_soft_bind_fbw.c" + * @author Kevin van Hecke + * Puts Spektrum in binding mode through software + */ + +#include "modules/spektrum_soft_bind/spektrum_soft_bind_fbw.h" +#include "subsystems/intermcu/intermcu_fbw.h" +#include "mcu.h" +#include "subsystems/radio_control.h" +#include "mcu_periph/sys_time_arch.h" + +#include "led.h" +#include "mcu_periph/gpio.h" + +void spektrum_soft_bind_init(void) +{ + gpio_setup_output(SPEKTRUM_POWER_PIN_PORT, SPEKTRUM_POWER_PIN); + gpio_set(SPEKTRUM_POWER_PIN_PORT, SPEKTRUM_POWER_PIN); +} + +void received_spektrum_soft_bind(void) +{ + + //power cycle the spektrum + gpio_clear(SPEKTRUM_POWER_PIN_PORT, SPEKTRUM_POWER_PIN); + sys_time_usleep(100000); + gpio_set(SPEKTRUM_POWER_PIN_PORT, SPEKTRUM_POWER_PIN); + + //put to bind mode + RADIO_CONTROL_BIND_IMPL_FUNC(); //basically = radio_control_spektrum_try_bind() + + SpektrumUartInit(); + +} diff --git a/sw/airborne/modules/spektrum_soft_bind/spektrum_soft_bind_fbw.h b/sw/airborne/modules/spektrum_soft_bind/spektrum_soft_bind_fbw.h new file mode 100644 index 0000000000..dcb262b003 --- /dev/null +++ b/sw/airborne/modules/spektrum_soft_bind/spektrum_soft_bind_fbw.h @@ -0,0 +1,34 @@ +/* + * Copyright (C) Kevin van Hecke + * + * This file is part of paparazzi + * + * paparazzi is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation; either version 2, or (at your option) + * any later version. + * + * paparazzi is distributed in the hope that it will be useful, + * but WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + * GNU General Public License for more details. + * + * You should have received a copy of the GNU General Public License + * along with paparazzi; see the file COPYING. If not, see + * . + */ +/** + * @file "modules/spektrum_soft_bind/spektrum_soft_bind_fbw.h" + * @author Kevin van Hecke + * Puts Spektrum in binding mode through software + */ + +#ifndef SPEKTRUM_FBW_SOFT_BIND_H +#define SPEKTRUM_FBW_SOFT_BIND_H +#include "std.h" + +extern void spektrum_soft_bind_init(void); +extern void received_spektrum_soft_bind(void); + +#endif + diff --git a/sw/airborne/subsystems/intermcu/intermcu_ap.c b/sw/airborne/subsystems/intermcu/intermcu_ap.c index 677c73ab1f..442b9e9ca2 100644 --- a/sw/airborne/subsystems/intermcu/intermcu_ap.c +++ b/sw/airborne/subsystems/intermcu/intermcu_ap.c @@ -30,6 +30,10 @@ #include "pprzlink/pprz_transport.h" #include "mcu_periph/uart.h" +#include "subsystems/datalink/telemetry.h" +#include "subsystems/electrical.h" +#include "autopilot.h" + #if COMMANDS_NB > 8 #error "INTERMCU UART CAN ONLY SEND 8 COMMANDS OR THE UART WILL BE OVERFILLED" #endif @@ -70,6 +74,11 @@ void intermcu_set_actuators(pprz_t *command_values, uint8_t ap_mode __attribute_ } } +void intermcu_send_spektrum_bind(void) +{ + if (!disable_comm) { + pprz_msg_send_IMCU_SPEKTRUM_SOFT_BIND(&(intermcu_transport.trans_tx), intermcu_device, INTERMCU_AP); + } } static inline void intermcu_parse_msg(struct transport_rx *trans, void (*rc_frame_handler)(void)) diff --git a/sw/airborne/subsystems/intermcu/intermcu_ap.h b/sw/airborne/subsystems/intermcu/intermcu_ap.h index 5e847c6db7..ea8648cdf4 100644 --- a/sw/airborne/subsystems/intermcu/intermcu_ap.h +++ b/sw/airborne/subsystems/intermcu/intermcu_ap.h @@ -32,6 +32,7 @@ void intermcu_set_actuators(pprz_t *command_values, uint8_t ap_mode); void RadioControlEvent(void (*frame_handler)(void)); +void intermcu_send_spektrum_bind(void); void disable_inter_comm(bool_t value); /* We need radio defines for the Autopilot */ diff --git a/sw/airborne/subsystems/intermcu/intermcu_fbw.c b/sw/airborne/subsystems/intermcu/intermcu_fbw.c index 8e0028e2ab..e62e65de55 100644 --- a/sw/airborne/subsystems/intermcu/intermcu_fbw.c +++ b/sw/airborne/subsystems/intermcu/intermcu_fbw.c @@ -29,6 +29,7 @@ #include "subsystems/radio_control.h" #include "mcu_periph/uart.h" #include "pprzlink/pprz_transport.h" +#include "modules/spektrum_soft_bind/spektrum_soft_bind_fbw.h" #ifdef BOARD_PX4IO #include "libopencm3/cm3/scb.h" @@ -97,6 +98,8 @@ static inline void intermcu_parse_msg(struct transport_rx *trans, void (*command uint8_t i; uint8_t size = DL_IMCU_COMMANDS_values_length(trans->payload); int16_t *new_commands = DL_IMCU_COMMANDS_values(trans->payload); + uint8_t status = DL_IMCU_COMMANDS_status(trans->payload); + autopilot_motors_on = status & 0x1; for (i = 0; i < size; i++) { intermcu_commands[i] = new_commands[i]; } @@ -107,6 +110,11 @@ static inline void intermcu_parse_msg(struct transport_rx *trans, void (*command break; } +#if defined(SPEKTRUM_HAS_SOFT_BIND_PIN) //TODO: make subscribable module parser + case DL_IMCU_SPEKTRUM_SOFT_BIND: + received_spektrum_soft_bind(); + break; +#endif default: break; } @@ -117,10 +125,15 @@ static inline void intermcu_parse_msg(struct transport_rx *trans, void (*command void InterMcuEvent(void (*frame_handler)(void)) { + /* Parse incoming bytes */ if (intermcu_device->char_available(intermcu_device->periph)) { while (intermcu_device->char_available(intermcu_device->periph) && !intermcu_transport.trans_rx.msg_received) { - parse_pprz(&intermcu_transport, intermcu_device->get_byte(intermcu_device->periph)); + unsigned char b = intermcu_device->get_byte(intermcu_device->periph); +#ifdef BOARD_PX4IO + checkPx4RebootCommand(b); +#endif + parse_pprz(&intermcu_transport, b); } if (intermcu_transport.trans_rx.msg_received) { @@ -128,3 +141,44 @@ void InterMcuEvent(void (*frame_handler)(void)) } } } +#ifdef BOARD_PX4IO +static inline void checkPx4RebootCommand(unsigned char b) +{ + if (!px4RebootTimeout) { + + + if (sys_time_check_and_ack_timer(px4bl_tid)) { + //time out the possibility to reboot to the px4 bootloader, to prevent unwanted restarts during flight + px4RebootTimeout = true; + sys_time_cancel_timer(px4bl_tid); + return; + } + + LED_ON(SYS_TIME_LED); + + if (b == px4RebootSequence[px4RebootSequenceCount]) { + px4RebootSequenceCount++; + } else { + px4RebootSequenceCount = 0; + } + + if (px4RebootSequenceCount >= 6) { // 6 = length of rebootSequence + 1 + px4RebootSequenceCount = 0; // should not be necessary... + + //send some magic back + //this is the same as the Pixhawk IO code would send + intermcu_device->put_byte(intermcu_device->periph, 0x00); + intermcu_device->put_byte(intermcu_device->periph, 0xe5); + intermcu_device->put_byte(intermcu_device->periph, 0x32); + intermcu_device->put_byte(intermcu_device->periph, 0x0a); + intermcu_device->put_byte(intermcu_device->periph, + 0x66); // dummy byte, seems to be necessary otherwise one byte is missing at the fmu side... + + while (((struct uart_periph *)(intermcu_device->periph))->tx_running) {LED_TOGGLE(SYS_TIME_LED);} // tx_running is volatile now, so LED_TOGGLE not necessary anymore + + LED_OFF(SYS_TIME_LED); + scb_reset_system(); + } + } +} +#endif