[modules] Add OPA controller

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"
   />
   <aircraft

conf/airframes/TUDELFT/tudelft_outback.xml

@@ -28,6 +28,7 @@
       <module name="guidance"      type="indi"/>
       <module name="intermcu"      type="uart"/>
       <module name="current_sensor"/>
+      <module name="opa_controller"/>
 
       <module name="geo_mag"/>
       <module name="air_data"/>
@@ -48,6 +49,7 @@
 
       <module name="actuators"     type="pwm"/>
       <module name="intermcu"      type="uart"/>
+      <module name="opa_controller"/>
       <module name="heli_throttle_curve"/>
       <module name="heli_swashplate_mixing"/>
 

conf/modules/opa_controller.xml

@@ -0,0 +1,34 @@
+<!DOCTYPE module SYSTEM "module.dtd">
+
+<module name="opa_controller" dir="boards">
+  <doc>
+    <description>Controller for OPA board functionalities</description>
+  </doc>
+  <settings>
+    <dl_settings>
+      <dl_settings NAME="Board">
+         <dl_setting var="opa_controller_vision_power" min="0" step="1" max="1" shortname="vision_pwr" values="OFF|ON" />
+         <dl_setting var="opa_controller_ftd_disarm" min="0" step="1" max="1" shortname="ftd_disarm" values="NONE|DISARM" module="boards/opa_controller_ap" handler="disarm" />
+      </dl_settings>
+    </dl_settings>
+  </settings>
+  <header>
+    <file name="opa_controller_ap.h"/>
+  </header>
+  <init fun="opa_controller_init()"/>
+  <periodic fun="opa_controller_periodic()" freq="20" autorun="TRUE"/>
+
+  <!-- FBW (FTD) part of the controller -->
+  <makefile target="fbw">
+    <file name="opa_controller_fbw.c"/>
+
+    <configure name="ARMING_LED" default="none"/>
+    <define name="ARMING_LED" value="$(ARMING_LED)" cond="ifneq ($(ARMING_LED),none)"/>
+  </makefile>
+
+  <!-- AP part of the controller -->
+  <makefile target="ap">
+    <file name="opa_controller_ap.c"/>
+  </makefile>
+</module>
+

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
  */

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
+ * <http://www.gnu.org/licenses/>.
+ */
+/**
+ * @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);
+}
+

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
+ * <http://www.gnu.org/licenses/>.
+ */
+/**
+ * @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
+

sw/airborne/modules/boards/opa_controller_fbw.c

@@ -0,0 +1,127 @@
+/*
+ * Copyright (C) Freek van Tienen <freek.v.tienen@gmail.com>
+ *
+ * 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
+ * <http://www.gnu.org/licenses/>.
+ */
+/**
+ * @file "modules/boards/opa_controller.c"
+ * @author Freek van Tienen <freek.v.tienen@gmail.com>
+ * 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
+}

sw/airborne/modules/boards/opa_controller_fbw.h

@@ -0,0 +1,37 @@
+/*
+ * Copyright (C) Freek van Tienen <freek.v.tienen@gmail.com>
+ *
+ * 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
+ * <http://www.gnu.org/licenses/>.
+ */
+/**
+ * @file "modules/boards/opa_controller.h"
+ * @author Freek van Tienen <freek.v.tienen@gmail.com>
+ * 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
+

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;

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;
   }
 }
 

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);
+      intermcu.cmd_status |= DL_IMCU_COMMANDS_status(imcu_msg_buf);
-      autopilot_motors_on = status & 0x1;
+
+      // 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];
       }