Merge pull request #1433 from paparazzi/cleanup_stereo

[stereocam] uartrotation->state2camera, streamline naming stereo_cam,…

conf/airframes/BR/ladybird_kit_bart_bluegiga.xml

@@ -195,7 +195,7 @@
  <modules main_freq="512">
    <load name="gps_ubx_ucenter.xml"/>
    <load name="send_imu_mag_current.xml"/>
-   <load name="stereoavoid.xml"/>
+   <load name="stereocam_nav_line_avoid.xml"/>
    <load name="mission_rotorcraft.xml"/>
  </modules>
 

conf/airframes/BR/ladybird_kit_bart_bluegiga_optitrack.xml

@@ -208,7 +208,7 @@
 
  <modules main_freq="512">
    <load name="send_imu_mag_current.xml"/>
-   <load name="stereoavoid.xml"/>
+   <load name="stereocam_nav_line_avoid.xml"/>
    <load name="mission_rotorcraft.xml"/>
  </modules>
 

conf/airframes/TUDelft/airframes/ardrone2_optitrack.xml

@@ -29,6 +29,7 @@
 
   <modules main_freq="512">
     <load name="bat_voltage_ardrone2.xml"/>
+    <load name="stereocam_droplet.xml" />
   </modules>
 
   <commands>

conf/airframes/TUDelft/airframes/rm_ardrone2.xml

@@ -32,7 +32,7 @@
 
   <modules main_freq="512">
     <load name="bat_voltage_ardrone2.xml"/>
-    <load name="uartrotation.xml">
+    <load name="stereocam_state2camera.xml">
 	<configure name="STEREO_UART" value="UART1"/>
         <configure name="STEREO_BAUD" value="B1000000"/>
     </load>

conf/flight_plans/rotorcraft_optitrack_stereoavoid.xml

@@ -4,7 +4,7 @@
   <header>
 #include "autopilot.h"
 #include "subsystems/electrical.h"
-#include "modules/stereoavoid/avoid_navigation.h"
+#include "modules/stereocam/nav_line_avoid/avoid_navigation.h"
 </header>
   <waypoints>
     <waypoint name="HOME" x="0.0" y="0.0"/>

conf/modules/stereocam.xml

@@ -1,6 +1,6 @@
  <!DOCTYPE module SYSTEM "module.dtd">
 
- <module name="stereocam" dir="stereo_cam">
+ <module name="stereocam" dir="stereocam">
   <doc>
     <description>
       StereoCamera read and decode.

conf/modules/stereocam_droplet.xml

@@ -0,0 +1,25 @@
+<!DOCTYPE module SYSTEM "module.dtd">
+
+<module name="stereocam_droplet" dir="stereocam">
+  <doc>
+    <description></description>
+  </doc>
+  <header>
+    <file name="droplet/stereocam_droplet.h"/>
+  </header>
+  <init fun="stereocam_droplet_init()"/>
+  <periodic fun="stereocam_droplet_periodic()" freq="20" autorun="TRUE"/>
+  <makefile target="ap">
+    <file name="droplet/stereocam_droplet.c"/>
+    <raw>
+      STEREO_UART ?= UART1
+      STEREO_BAUD ?= B9600
+      STEREO_LED ?= 3
+      STEREO_UART_LOWER=$(shell echo $(STEREO_UART) | tr A-Z a-z)
+    </raw>
+    <define name="USE_$(STEREO_UART)"/>
+    <define name="STEREO_UART" value="$(STEREO_UART_LOWER)"/>
+    <define name="$(STEREO_UART)_BAUD" value="$(STEREO_BAUD)"/>
+  </makefile>
+</module>
+

conf/modules/stereocam_nav_line_avoid.xml

@@ -1,13 +1,13 @@
 <!DOCTYPE module SYSTEM "module.dtd">
 
-<module name="stereoavoid">
+<module name="stereoavoid" dir="stereocam">
   <doc>
-    <description>Read Stereoboard Obstacle Protocol</description>
+    <description>Read Stereoboard Obstacle Protocol and plan around obstacles during nav-line navigation</description>
     <configure name="STEREO_UART" value="UARTX" description="Sets the UART port number of the connected camera (required)"/>
   </doc>
 
   <header>
-    <file name="stereo_avoid.h"/>
+    <file name="nav_line_avoid/stereo_avoid.h"/>
   </header>
 
   <init fun="stereo_avoid_init()"/>
@@ -23,13 +23,13 @@
     <define name="USE_$(STEREO_UART)"/>
     <define name="STEREO_UART" value="$(STEREO_UART_LOWER)"/>
     <define name="$(STEREO_UART)_BAUD" value="$(STEREO_BAUD)"/>
-    <file name="avoid_navigation.c"/>
+    <file name="nav_line_avoid/avoid_navigation.c"/>
   </makefile>
   <makefile  target="ap">
-    <file name="stereo_avoid.c"/>
+    <file name="nav_line_avoid/stereo_avoid.c"/>
   </makefile>
   <makefile  target="nps">
-    <file name="stereo_avoid_sim.c"/>
+    <file name="nav_line_avoid/stereo_avoid_sim.c"/>
   </makefile>
 </module>
 

conf/modules/stereocam_state2camera.xml

@@ -1,6 +1,6 @@
 <!DOCTYPE module SYSTEM "module.dtd">
 
-<module name="uartrotation" dir="stereo_cam">
+<module name="stereocam_state2camera" dir="stereocam">
   <doc>
     <description>Writes rotation to the uart bus. Meant for sending the rotation
 	to the ODROID board or to the stereo camera boards. 
@@ -11,12 +11,12 @@
     <configure name="STEREO_BAUD" value="BXXXXX" description="Sets the BAUD rate of the connected camera (required: must be same as camera)"/>
   </doc>
   <header>
-    <file name="uartrotation.h"/>
+    <file name="state2camera/state2camera.h"/>
   </header>
   <periodic fun="write_serial_rot()" freq="512."/>
   <makefile>
-    <file name="uartrotation.c"/>
-    <file name="stereoprotocol.c" dir="modules/stereo_cam"/>
+    <file name="state2camera/state2camera.c"/>
+    <file name="stereoprotocol.c" dir="modules/stereocam"/>
 	<raw>
 		STEREO_UART_LOWER=$(shell echo $(STEREO_UART) | tr A-Z a-z)
 		ap.CFLAGS += -DUSE_$(STEREO_UART)

sw/airborne/modules/stereocam/droplet/stereocam_droplet.c

@@ -0,0 +1,164 @@
+/*
+ * Copyright (C) C. DW
+ *
+ * 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/stereocam/droplet/stereocam_droplet.c"
+ * @author C. DW
+ *
+ */
+
+#include "modules/stereocam/droplet/stereocam_droplet.h"
+
+// Know waypoint numbers and blocks
+#include "generated/flight_plan.h"
+
+#include "firmwares/rotorcraft/navigation.h"
+
+
+
+// Serial Port
+#include "mcu_periph/uart.h"
+PRINT_CONFIG_VAR(STEREO_UART)
+
+// define coms link for stereocam
+#define STEREO_PORT   (&((STEREO_UART).device))
+struct link_device *xdev = STEREO_PORT;
+
+#define StereoGetch() STEREO_PORT ->get_byte(STEREO_PORT->periph)
+#define StereoSend1(c) STEREO_PORT->put_byte(STEREO_PORT->periph, c)
+#define StereoUartSend1(c) StereoSend1(c)
+#define StereoSend(_dat,_len) { for (uint8_t i = 0; i< (_len); i++) StereoSend1(_dat[i]); };
+#define StereoUartSetBaudrate(_b) uart_periph_set_baudrate(STEREO_PORT, _b);
+#define StereoChAvailable()(xdev->char_available(xdev->periph))
+
+
+
+
+
+
+// Downlink
+#ifndef DOWNLINK_DEVICE
+#define DOWNLINK_DEVICE DOWNLINK_AP_DEVICE
+#endif
+#include "messages.h"
+#include "subsystems/datalink/downlink.h"
+
+#include "led.h"
+
+
+
+
+
+// Module data
+struct AvoidNavigationStruct {
+  uint8_t mode; ///< 0 = straight, 1 =  right, 2 = left, ...
+  uint8_t stereo_bin[8];
+  uint8_t timeout;
+};
+
+struct AvoidNavigationStruct avoid_navigation_data;
+
+
+
+
+
+
+
+static void stereo_parse(uint8_t c);
+static void stereo_parse(uint8_t c)
+{
+  // Protocol is one byte only: store last instance
+  avoid_navigation_data.stereo_bin[0] = c;
+  avoid_navigation_data.timeout = 20;
+}
+
+
+void stereocam_droplet_init(void)
+{
+  // Do nothing
+  avoid_navigation_data.mode = 0;
+  avoid_navigation_data.timeout = 0;
+}
+void stereocam_droplet_periodic(void)
+{
+
+  static float heading = 0;
+
+  // Read Serial
+  while (StereoChAvailable()) {
+    stereo_parse(StereoGetch());
+  }
+
+  if (avoid_navigation_data.timeout <= 0)
+    return;
+
+  avoid_navigation_data.timeout --;
+
+  // Results
+  DOWNLINK_SEND_PAYLOAD(DefaultChannel, DefaultDevice, 1, avoid_navigation_data.stereo_bin);
+
+  volatile bool_t once = TRUE;
+  // Move waypoint with constant speed in current direction
+  if (
+    (avoid_navigation_data.stereo_bin[0] == 97) ||
+    (avoid_navigation_data.stereo_bin[0] == 100)
+  ) {
+    once = TRUE;
+    struct EnuCoor_f enu;
+    enu.x = waypoint_get_x(WP_GOAL);
+    enu.y = waypoint_get_y(WP_GOAL);
+    enu.z = waypoint_get_alt(WP_GOAL);
+    float sin_heading = sinf(ANGLE_FLOAT_OF_BFP(nav_heading));
+    float cos_heading = cosf(ANGLE_FLOAT_OF_BFP(nav_heading));
+    enu.x += (sin_heading * 1.3 / 20);
+    enu.y += (cos_heading * 1.3 / 20);
+    waypoint_set_enu(WP_GOAL, &enu);
+  } else if (avoid_navigation_data.stereo_bin[0] == 98) {
+    // STOP!!!
+    if (once) {
+      NavSetWaypointHere(WP_GOAL);
+      once = FALSE;
+    }
+  } else {
+    once = TRUE;
+  }
+
+
+  switch (avoid_navigation_data.stereo_bin[0]) {
+    case 99:     // Turn
+      heading += 4;
+      if (heading > 360) { heading = 0; }
+      nav_set_heading_rad(RadOfDeg(heading));
+      break;
+    default:    // do nothing
+      break;
+  }
+
+#ifdef STEREO_LED
+  if (obstacle_detected) {
+    LED_ON(STEREO_LED);
+  } else {
+    LED_OFF(STEREO_LED);
+  }
+#endif
+
+}
+
+
+

sw/airborne/modules/stereocam/droplet/stereocam_droplet.h

@@ -0,0 +1,35 @@
+/*
+ * Copyright (C) C. DW
+ *
+ * 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/stereocam/droplet/stereocam_droplet.h"
+ * @author C. DW
+ *
+ */
+
+#ifndef STEREOCAM_DROPLET_H
+#define STEREOCAM_DROPLET_H
+
+
+
+extern void stereocam_droplet_init(void);
+extern void stereocam_droplet_periodic(void);
+
+#endif
+

sw/airborne/modules/stereocam/nav_line_avoid/avoid_navigation.c

@@ -19,6 +19,12 @@
  * Boston, MA 02111-1307, USA.
  */
 
+/**
+ * @file modules/stereocam/nav_line_avoid/avoid_navigation.c
+ *
+ */
+
+
 // Own Header
 #include "avoid_navigation.h"
 
@@ -42,6 +48,8 @@
 
 
 
+
+
 struct AvoidNavigationStruct avoid_navigation_data;
 bool_t obstacle_detected = FALSE;
 int32_t counter = 0;
@@ -85,8 +93,8 @@ void run_avoid_navigation_onvision(void)
           struct EnuCoor_i *pos = stateGetPositionEnu_i();
           float sin_heading = sinf(ANGLE_FLOAT_OF_BFP(nav_heading));
           float cos_heading = cosf(ANGLE_FLOAT_OF_BFP(nav_heading));
-          new_coor.x = pos->x + POS_BFP_OF_REAL(sin_heading * 3.5);
-          new_coor.y = pos->y + POS_BFP_OF_REAL(cos_heading * 3.5);
+          new_coor.x = pos->x + POS_BFP_OF_REAL(sin_heading * (NAV_LINE_AVOID_SEGMENT_LENGTH));
+          new_coor.y = pos->y + POS_BFP_OF_REAL(cos_heading * (NAV_LINE_AVOID_SEGMENT_LENGTH));
           new_coor.z = pos->z;
           waypoint_set_xy_i(WP_W1, new_coor.x, new_coor.y);
           obstacle_detected = FALSE;
@@ -105,7 +113,7 @@ void run_avoid_navigation_onvision(void)
   avoid_navigation_data.stereo_bin[3] = avoid_navigation_data.mode;
   avoid_navigation_data.stereo_bin[4] = counter;
 
-#if STEREO_LED
+#ifdef STEREO_LED
   if (obstacle_detected) {
     LED_ON(STEREO_LED);
   } else {

sw/airborne/modules/stereocam/nav_line_avoid/avoid_navigation.h

@@ -19,12 +19,27 @@
  * Boston, MA 02111-1307, USA.
  */
 
+/**
+ * @file modules/stereocam/nav_line_avoid/avoid_navigation.h
+ *
+ *  - while flying a route from A -> B
+ *  - when
+ */
+
+
 
 #ifndef AVOID_NAVIGATION_H_
-#define AVOID_NAVIGATION_H_
+#define AVOID_NAVIGATION_H
 
 #include <std.h>
 
+
+#ifndef NAV_LINE_AVOID_SEGMENT_LENGTH
+#define NAV_LINE_AVOID_SEGMENT_LENGTH 3.5
+#endif
+
+
+
 struct AvoidNavigationStruct {
   uint8_t mode; ///< 0 = nothing, 1 =  climb, 2 = sideways, ...
   uint8_t stereo_bin[8];

sw/airborne/modules/stereocam/nav_line_avoid/stereo_avoid.c

@@ -20,7 +20,7 @@
  */
 
 /**
- * @file modules/stereoavoid/stereoavoid.c
+ * @file modules/stereocam/nav_line_avoid/stereoavoid.c
  *
  * Parse avoidance messages from stereocamera to use obstacle results in navigation
  */

sw/airborne/modules/stereocam/nav_line_avoid/stereo_avoid.h

@@ -20,7 +20,7 @@
  */
 
 /**
- * @file modules/stereoavoid/stereoavoid.h
+ * @file modules/stereocam/nav_line_avoid/stereoavoid.h
  *
  */
 

sw/airborne/modules/stereocam/state2camera/state2camera.c

@@ -18,16 +18,16 @@
  * <http://www.gnu.org/licenses/>.
  */
 /**
- * @file "modules/readlocationfromodroid/readlocationfromodroid.c"
+ * @file "modules/stereocam/state2camera/state2camera.c"
  * @author Roland
  * Sends rotation using the stereoboard protocol over the UART.
  */
 
-#include "modules/stereo_cam/uartrotation.h"
+#include "modules/stereocam/state2camera/state2camera.h"
+#include "modules/stereocam/stereoprotocol.h"
 #include "subsystems/abi.h"
 #include "state.h"
 #include "mcu_periph/uart.h"
-#include "stereoprotocol.h"
 static int frame_number_sending = 0;
 float lastKnownHeight = 0.0;
 int pleaseResetOdroid = 0;

sw/airborne/modules/stereocam/state2camera/state2camera.h

@@ -18,14 +18,16 @@
  * <http://www.gnu.org/licenses/>.
  */
 /**
- * @file "modules/stereo_cam/uartrotation.h"
+ * @file "modules/stereocam/state2camera/state2camera.h"
  * @author Roland
  * Sends rotation using the stereoboard protocol over the UART.
  */
 
 #ifndef UARTROTATION_H
 #define UARTROTATION_H
+
 #include <inttypes.h>
 extern void write_serial_rot(void);
+
 #endif
 

sw/airborne/modules/stereocam/stereocam.c

@@ -20,16 +20,16 @@
  *
  */
 
-/** @file modules/stereo_cam/stereocam.c
+/** @file modules/stereocam/stereocam.c
  *  @brief interface to TU Delft serial stereocam
  *  Include stereocam.xml to your airframe file.
  *  Parameters STEREO_PORT, STEREO_BAUD, SEND_STEREO and STEREO_BUF_SIZE should be configured with stereocam.xml.
  */
 
-#include "modules/stereo_cam/stereocam.h"
+#include "modules/stereocam/stereocam.h"
 #include "mcu_periph/uart.h"
 #include "subsystems/datalink/telemetry.h"
-#include "modules/stereo_cam/stereoprotocol.h"
+#include "modules/stereocam/stereoprotocol.h"
 #ifndef SEND_STEREO
 #define SEND_STEREO TRUE
 #endif
@@ -37,7 +37,7 @@
 // define coms link for stereocam
 #define STEREO_PORT   (&((UART_LINK).device))
 struct link_device *dev = STEREO_PORT;
-#define StereoGetch() STEREO_PORT ->get_byte(STEREO_PORT->periph)
+#define StereoGetch() dev->get_byte(STEREO_PORT->periph)
 
 //typedef struct MsgProperties {
 //  uint16_t positionImageStart;

sw/airborne/modules/stereocam/stereocam.h

@@ -20,7 +20,7 @@
  *
  */
 
-/** @file modules/stereo_cam/stereocam.h
+/** @file modules/stereocam/stereocam.h
  *  @brief interface to the TU Delft serial stereocam
  */
 

sw/airborne/modules/stereocam/stereoprotocol.c

@@ -1,8 +1,27 @@
 /*
- * stereoprotocol.cpp
+ * Copyright (C) 2015 Kirk + Roland
  *
- *  Created on: Sep 23, 2015
- *      Author: Roland + Kirk
+ * 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, write to
+ * the Free Software Foundation, 59 Temple Place - Suite 330,
+ * Boston, MA 02111-1307, USA.
+ *
+ */
+
+/** @file modules/stereocam/stereoprotocol.c
+ *  @brief standard protocol for TUDelft stereocamera data transfer
  */
 
 #include "stereoprotocol.h"
@@ -38,8 +57,6 @@ uint8_t stereoprot_isEndOfMsg(uint8_t *stack, uint16_t i, uint16_t buffer_size)
   return 0;
 }
 
-
-
 /**
  * Checks if the sequence in the array is equal to 255-0-0-171,
  * as this means a new image is starting from here
@@ -143,7 +160,6 @@ uint8_t handleStereoPackage(uint8_t newByte, uint16_t buffer_size, uint16_t *ins
 }
 
 
-
 /**
  * Retrieve size of image from message
  */

sw/airborne/modules/stereocam/stereoprotocol.h

@@ -1,8 +1,27 @@
 /*
- * stereoprotocol.h
+ * Copyright (C) 2015 Kirk + Roland
  *
- *  Created on: Sep 23, 2015
- *      Author: Roland + Kirk
+ * 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, write to
+ * the Free Software Foundation, 59 Temple Place - Suite 330,
+ * Boston, MA 02111-1307, USA.
+ *
+ */
+
+/** @file modules/stereocam/stereoprotocol.h
+ *  @brief standard protocol for TUDelft stereocamera data transfer
  */
 
 #ifndef SW_AIRBORNE_MODULES_STEREO_CAM_STEREOPROTOCOL_H_
@@ -10,6 +29,7 @@
 
 #include <inttypes.h>
 #include "mcu_periph/link_device.h"
+
 struct MsgProperties {
   uint16_t positionImageStart;
   uint8_t width;
@@ -43,6 +63,7 @@ uint8_t stereoprot_isStartOfMsg(uint8_t *stack, uint16_t i, uint16_t buffer_size
 
 void WritePart(struct link_device *, uint8_t *, uint8_t);
 void stereoprot_sendArray(struct link_device *fd, uint8_t *b, uint8_t array_width, uint8_t array_height);
+
 /**
  * Get all available data from stereo com link and decode any complete messages.
  * Returns as soon as a complete message is found. Messages placed in msg_buf