diff --git a/conf/airframes/TUDELFT/tudelft_conf.xml b/conf/airframes/TUDELFT/tudelft_conf.xml index 5f0493a4d6..06d324069f 100644 --- a/conf/airframes/TUDELFT/tudelft_conf.xml +++ b/conf/airframes/TUDELFT/tudelft_conf.xml @@ -216,7 +216,7 @@ telemetry="telemetry/rotorcraft_with_logger.xml" flight_plan="flight_plans/dummy.xml" settings="settings/rotorcraft_basic.xml settings/control/stabilization_att_int.xml settings/control/rotorcraft_guidance.xml" - settings_modules="modules/geo_mag.xml modules/air_data.xml modules/temp_adc.xml modules/logger_sd_spi_direct.xml modules/gps_ubx_ucenter.xml" + settings_modules="modules/opa_controller.xml modules/geo_mag.xml modules/air_data.xml modules/temp_adc.xml modules/logger_sd_spi_direct.xml modules/gps_ubx_ucenter.xml" gui_color="#ffffdffac31f" /> + @@ -48,6 +49,7 @@ + diff --git a/conf/modules/opa_controller.xml b/conf/modules/opa_controller.xml new file mode 100644 index 0000000000..93fcf979fe --- /dev/null +++ b/conf/modules/opa_controller.xml @@ -0,0 +1,34 @@ + + + + + Controller for OPA board functionalities + + + + + + + + + +
+ +
+ + + + + + + + + + + + + + + + + diff --git a/sw/airborne/boards/opa_ftd_1.0.h b/sw/airborne/boards/opa_ftd_1.0.h index d6b5477df1..55cde95397 100644 --- a/sw/airborne/boards/opa_ftd_1.0.h +++ b/sw/airborne/boards/opa_ftd_1.0.h @@ -30,13 +30,33 @@ #define AHB_CLK 168000000 /* - * MCU Power control + * Power control */ + +/* MCU power */ #define MCU_PWR GPIOB #define MCU_PWR_PIN GPIO5 #define MCU_PWR_ON gpio_set #define MCU_PWR_OFF gpio_clear +/* Autopilot power */ +#define AP_PWR GPIOB +#define AP_PWR_PIN GPIO0 +#define AP_PWR_ON gpio_set +#define AP_PWR_OFF gpio_clear + +/* Main power (25V) */ +#define MAIN_PWR GPIOC +#define MAIN_PWR_PIN GPIO12 +#define MAIN_PWR_ON gpio_set +#define MAIN_PWR_OFF gpio_clear + +/* Balancer power */ +#define BAL_PWR GPIOC +#define BAL_PWR_PIN GPIO2 +#define BAL_PWR_ON gpio_set +#define BAL_PWR_OFF gpio_clear + /* * Buttons */ @@ -45,6 +65,14 @@ #define BTN_ESTOP GPIOB #define BTN_ESTOP_PIN GPIO12 +/* Arming button */ +#define BTN_ARMING GPIOB +#define BTN_ARMING_PIN GPIO14 + +/* On/Off button */ +#define BTN_ON GPIOC +#define BTN_ON_PIN GPIO1 + /* * Onboard LEDs */ diff --git a/sw/airborne/modules/boards/opa_controller_ap.c b/sw/airborne/modules/boards/opa_controller_ap.c new file mode 100644 index 0000000000..ee83f1ed24 --- /dev/null +++ b/sw/airborne/modules/boards/opa_controller_ap.c @@ -0,0 +1,59 @@ +/* + * Copyright (C) C. De Wagter + * + * 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/boards/opa_controller_ap.c" + * @author C. De Wagter + * Controller for OPA-AP board functionalities + */ + +#include "modules/boards/opa_controller_ap.h" +#include "subsystems/intermcu.h" +#include "generated/airframe.h" +#include "mcu_periph/gpio.h" + +bool opa_controller_vision_power = false; +bool opa_controller_ftd_disarm = false; + +void opa_controller_ap_disarm(bool action) { + intermcu.cmd_status |= 1 << 1; + opa_controller_ftd_disarm = action; +} + +void opa_controller_init() { + /* Enable Vision Power Control: Default Power Off */ + opa_controller_vision_power = false; + opa_controller_ftd_disarm = false; + + gpio_setup_output(VISION_PWR, VISION_PWR_PIN); + VISION_PWR_OFF(VISION_PWR, VISION_PWR_PIN); +} + +void opa_controller_periodic() { + /* Update the vision control power */ + if (opa_controller_vision_power) { + VISION_PWR_ON(VISION_PWR, VISION_PWR_PIN); + } else { + VISION_PWR_OFF(VISION_PWR, VISION_PWR_PIN); + } + + /* Set the FTD Disarm state */ + opa_controller_ftd_disarm = intermcu.cmd_status & (1 << 1); +} + diff --git a/sw/airborne/modules/boards/opa_controller_ap.h b/sw/airborne/modules/boards/opa_controller_ap.h new file mode 100644 index 0000000000..635e12c3c0 --- /dev/null +++ b/sw/airborne/modules/boards/opa_controller_ap.h @@ -0,0 +1,40 @@ +/* + * Copyright (C) C. De Wagter + * + * 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/boards/opa_controller_ap.h" + * @author C. De Wagter + * Controlelr for OPA-AP board functionalities + */ + +#ifndef OPA_CONTROLLER_AP_H +#define OPA_CONTROLLER_AP_H + +#include "std.h" + +extern bool opa_controller_vision_power; +extern bool opa_controller_ftd_disarm; +extern void opa_controller_ap_disarm(bool take); + + +extern void opa_controller_init(void); +extern void opa_controller_periodic(void); + +#endif + diff --git a/sw/airborne/modules/boards/opa_controller_fbw.c b/sw/airborne/modules/boards/opa_controller_fbw.c new file mode 100644 index 0000000000..3a3eaa1e09 --- /dev/null +++ b/sw/airborne/modules/boards/opa_controller_fbw.c @@ -0,0 +1,127 @@ +/* + * Copyright (C) Freek van Tienen + * + * 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/boards/opa_controller.c" + * @author Freek van Tienen + * Controller for OPA board functionalities + */ + +#include "modules/boards/opa_controller_fbw.h" +#include "subsystems/intermcu.h" +#include "generated/airframe.h" +#include "mcu_periph/gpio.h" +#include "led.h" + +#define OFF_TIMER 5*1 // FIXME: make nicer +#define MIN_ARMING_TRIG 1*1 //FIXME: make nicer + +/* Whether the autopilot arming LED is on */ +static bool arming_led = false; +extern bool autopilot_motors_on; +bool opa_controller_disarm = false; + + +void opa_controller_init(void) { + /* Setup E-Stop, Arming and On/Off button as input */ + gpio_setup_input(BTN_ESTOP, BTN_ESTOP_PIN); + gpio_setup_input(BTN_ARMING, BTN_ARMING_PIN); + gpio_setup_input(BTN_ON, BTN_ON_PIN); + + /* Enable Autopilot power */ + gpio_setup_output(AP_PWR, AP_PWR_PIN); + AP_PWR_ON(AP_PWR, AP_PWR_PIN); + + /* Enable Main power (25V) */ + gpio_setup_output(MAIN_PWR, MAIN_PWR_PIN); + MAIN_PWR_OFF(MAIN_PWR, MAIN_PWR_PIN); + + /* Enable Balancer power */ + gpio_setup_output(BAL_PWR, BAL_PWR_PIN); + BAL_PWR_OFF(BAL_PWR, BAL_PWR_PIN); + +#if defined ARMING_LED + /* Disable the arming LED */ + LED_OFF(ARMING_LED); +#endif +} + +void opa_controller_periodic(void) { + static uint16_t off_cnt = 0; + static uint16_t arming_cnt = 0; + + /* Check E-Stop and power off Main power if pressed */ + if(!gpio_get(BTN_ESTOP, BTN_ESTOP_PIN)) { + AP_PWR_OFF(AP_PWR, AP_PWR_PIN); + MAIN_PWR_OFF(MAIN_PWR, MAIN_PWR_PIN); + BAL_PWR_OFF(BAL_PWR, BAL_PWR_PIN); + RADIO_CONTROL_POWER_OFF(RADIO_CONTROL_POWER,RADIO_CONTROL_POWER_PIN); + MCU_PWR_OFF(MCU_PWR, MCU_PWR_PIN); + arming_led = false; + return; + } + + /* AP soft disarm */ + if (intermcu.cmd_status & (1 << 1)) { + MAIN_PWR_OFF(MAIN_PWR, MAIN_PWR_PIN); + BAL_PWR_OFF(BAL_PWR, BAL_PWR_PIN); + arming_led = false; + intermcu.cmd_status &= ~(1 << 1); + } + + /* Check On/Off button and disable if pressed for 3 seconds */ + if(gpio_get(BTN_ON, BTN_ON_PIN)) { + if(off_cnt >= OFF_TIMER) { + MAIN_PWR_OFF(MAIN_PWR, MAIN_PWR_PIN); + BAL_PWR_OFF(BAL_PWR, BAL_PWR_PIN); + arming_led = false; + } else { + off_cnt++; + } + } + else { + off_cnt = 0; + } + + /* Check Arming button and set LED */ + if(!gpio_get(BTN_ARMING, BTN_ARMING_PIN)) { + if (arming_cnt >= MIN_ARMING_TRIG) { + arming_led = true; + MAIN_PWR_ON(MAIN_PWR, MAIN_PWR_PIN); + BAL_PWR_ON(BAL_PWR, BAL_PWR_PIN); + //AP_PWR_ON(AP_PWR, AP_PWR_PIN); + //RADIO_CONTROL_POWER_ON(RADIO_CONTROL_POWER,RADIO_CONTROL_POWER_PIN); + } else { + arming_cnt++; + } + } + else { + arming_cnt = 0; + } + +#if defined ARMING_LED + /* Update Arming LED */ + if (arming_led) { + LED_OFF(ARMING_LED); + } else { + LED_ON(ARMING_LED); + } + // autopilot_motors_on; +#endif +} diff --git a/sw/airborne/modules/boards/opa_controller_fbw.h b/sw/airborne/modules/boards/opa_controller_fbw.h new file mode 100644 index 0000000000..0a381d2a1d --- /dev/null +++ b/sw/airborne/modules/boards/opa_controller_fbw.h @@ -0,0 +1,37 @@ +/* + * Copyright (C) Freek van Tienen + * + * 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/boards/opa_controller.h" + * @author Freek van Tienen + * Controller for OPA board functionalities + */ + +#ifndef OPA_CONTROLLER_H +#define OPA_CONTROLLER_H + +#include "std.h" + +extern bool opa_controller_disarm; + +extern void opa_controller_init(void); +extern void opa_controller_periodic(void); + +#endif + diff --git a/sw/airborne/subsystems/intermcu.h b/sw/airborne/subsystems/intermcu.h index 4ac35ffb5f..722de9b0bb 100644 --- a/sw/airborne/subsystems/intermcu.h +++ b/sw/airborne/subsystems/intermcu.h @@ -58,6 +58,7 @@ struct intermcu_t { uint8_t time_since_last_frame; ///< Time since last frame bool enabled; ///< If the InterMCU communication is enabled bool msg_available; ///< If we have an InterMCU message + uint8_t cmd_status; ///< The status information that needs to be transfered #ifdef BOARD_PX4IO enum intermcu_PX4_baud_status stable_px4_baud; diff --git a/sw/airborne/subsystems/intermcu/intermcu_ap.c b/sw/airborne/subsystems/intermcu/intermcu_ap.c index ebd8232bf9..ede1af3dd2 100644 --- a/sw/airborne/subsystems/intermcu/intermcu_ap.c +++ b/sw/airborne/subsystems/intermcu/intermcu_ap.c @@ -41,7 +41,7 @@ struct intermcu_t intermcu = { .device = (&((INTERMCU_LINK).device)), .enabled = true, - .msg_available = false + .msg_available = false, }; uint8_t imcu_msg_buf[128] __attribute__((aligned)); ///< The InterMCU message buffer static struct fbw_status_t fbw_status; @@ -91,8 +91,10 @@ void intermcu_set_enabled(bool value) void intermcu_set_actuators(pprz_t *command_values, uint8_t ap_mode __attribute__((unused))) { if (intermcu.enabled) { + intermcu.cmd_status |= (autopilot_motors_on & 0x1) << 0; pprz_msg_send_IMCU_COMMANDS(&(intermcu.transport.trans_tx), intermcu.device, - INTERMCU_AP, &autopilot_motors_on, COMMANDS_NB, command_values); //TODO: Append more status + INTERMCU_AP, &intermcu.cmd_status, COMMANDS_NB, command_values); //TODO: Append more status + intermcu.cmd_status = 0; } } diff --git a/sw/airborne/subsystems/intermcu/intermcu_fbw.c b/sw/airborne/subsystems/intermcu/intermcu_fbw.c index f9eb597fcf..074eb4579d 100644 --- a/sw/airborne/subsystems/intermcu/intermcu_fbw.c +++ b/sw/airborne/subsystems/intermcu/intermcu_fbw.c @@ -30,6 +30,7 @@ #include "subsystems/electrical.h" #include "mcu_periph/uart.h" + #include "modules/spektrum_soft_bind/spektrum_soft_bind_fbw.h" #include BOARD_CONFIG @@ -150,8 +151,12 @@ static void intermcu_parse_msg(void (*commands_frame_handler)(void)) uint8_t i; uint8_t size = DL_IMCU_COMMANDS_values_length(imcu_msg_buf); int16_t *new_commands = DL_IMCU_COMMANDS_values(imcu_msg_buf); - uint8_t status = DL_IMCU_COMMANDS_status(imcu_msg_buf); - autopilot_motors_on = status & 0x1; + intermcu.cmd_status |= DL_IMCU_COMMANDS_status(imcu_msg_buf); + + // read the autopilot status and then clear it + autopilot_motors_on = intermcu.cmd_status & (1 << 0); + intermcu.cmd_status &= ~(1 << 0); + for (i = 0; i < size; i++) { intermcu_commands[i] = new_commands[i]; }