diff --git a/conf/airframes/BR/ladybird_kit_bart_bluegiga.xml b/conf/airframes/BR/ladybird_kit_bart_bluegiga.xml index 8cab2a30fd..075d5a2b22 100644 --- a/conf/airframes/BR/ladybird_kit_bart_bluegiga.xml +++ b/conf/airframes/BR/ladybird_kit_bart_bluegiga.xml @@ -195,7 +195,7 @@ - + diff --git a/conf/airframes/BR/ladybird_kit_bart_bluegiga_optitrack.xml b/conf/airframes/BR/ladybird_kit_bart_bluegiga_optitrack.xml index 08775b4e62..db72ede35b 100644 --- a/conf/airframes/BR/ladybird_kit_bart_bluegiga_optitrack.xml +++ b/conf/airframes/BR/ladybird_kit_bart_bluegiga_optitrack.xml @@ -208,7 +208,7 @@ - + diff --git a/conf/airframes/TUDelft/airframes/ardrone2_optitrack.xml b/conf/airframes/TUDelft/airframes/ardrone2_optitrack.xml index 86d311c5a5..98eca2c027 100644 --- a/conf/airframes/TUDelft/airframes/ardrone2_optitrack.xml +++ b/conf/airframes/TUDelft/airframes/ardrone2_optitrack.xml @@ -29,6 +29,7 @@ + diff --git a/conf/airframes/TUDelft/airframes/rm_ardrone2.xml b/conf/airframes/TUDelft/airframes/rm_ardrone2.xml index 6dac0deb6c..db55d23041 100644 --- a/conf/airframes/TUDelft/airframes/rm_ardrone2.xml +++ b/conf/airframes/TUDelft/airframes/rm_ardrone2.xml @@ -32,7 +32,7 @@ - + diff --git a/conf/flight_plans/rotorcraft_optitrack_stereoavoid.xml b/conf/flight_plans/rotorcraft_optitrack_stereoavoid.xml index 6e52578566..4abfcf0417 100644 --- a/conf/flight_plans/rotorcraft_optitrack_stereoavoid.xml +++ b/conf/flight_plans/rotorcraft_optitrack_stereoavoid.xml @@ -4,7 +4,7 @@
#include "autopilot.h" #include "subsystems/electrical.h" -#include "modules/stereoavoid/avoid_navigation.h" +#include "modules/stereocam/nav_line_avoid/avoid_navigation.h"
diff --git a/conf/modules/stereocam.xml b/conf/modules/stereocam.xml index 5b9ee35b72..8063e6366b 100644 --- a/conf/modules/stereocam.xml +++ b/conf/modules/stereocam.xml @@ -1,6 +1,6 @@ - + StereoCamera read and decode. diff --git a/conf/modules/stereocam_droplet.xml b/conf/modules/stereocam_droplet.xml new file mode 100644 index 0000000000..91e12b933c --- /dev/null +++ b/conf/modules/stereocam_droplet.xml @@ -0,0 +1,25 @@ + + + + + + +
+ +
+ + + + + + STEREO_UART ?= UART1 + STEREO_BAUD ?= B9600 + STEREO_LED ?= 3 + STEREO_UART_LOWER=$(shell echo $(STEREO_UART) | tr A-Z a-z) + + + + + + + diff --git a/conf/modules/stereoavoid.xml b/conf/modules/stereocam_nav_line_avoid.xml similarity index 67% rename from conf/modules/stereoavoid.xml rename to conf/modules/stereocam_nav_line_avoid.xml index b83fd4536b..1ca37ca6d7 100644 --- a/conf/modules/stereoavoid.xml +++ b/conf/modules/stereocam_nav_line_avoid.xml @@ -1,13 +1,13 @@ - + - Read Stereoboard Obstacle Protocol + Read Stereoboard Obstacle Protocol and plan around obstacles during nav-line navigation
- +
@@ -23,13 +23,13 @@ - + - + - + diff --git a/conf/modules/uartrotation.xml b/conf/modules/stereocam_state2camera.xml similarity index 81% rename from conf/modules/uartrotation.xml rename to conf/modules/stereocam_state2camera.xml index 2a707fc581..cace9c3111 100644 --- a/conf/modules/uartrotation.xml +++ b/conf/modules/stereocam_state2camera.xml @@ -1,6 +1,6 @@ - + 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 @@
- +
- - + + STEREO_UART_LOWER=$(shell echo $(STEREO_UART) | tr A-Z a-z) ap.CFLAGS += -DUSE_$(STEREO_UART) diff --git a/sw/airborne/modules/stereocam/droplet/stereocam_droplet.c b/sw/airborne/modules/stereocam/droplet/stereocam_droplet.c new file mode 100644 index 0000000000..1196770a3f --- /dev/null +++ b/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 + * . + */ +/** + * @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 + +} + + + diff --git a/sw/airborne/modules/stereocam/droplet/stereocam_droplet.h b/sw/airborne/modules/stereocam/droplet/stereocam_droplet.h new file mode 100644 index 0000000000..8450d53c2c --- /dev/null +++ b/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 + * . + */ +/** + * @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 + diff --git a/sw/airborne/modules/stereoavoid/avoid_navigation.c b/sw/airborne/modules/stereocam/nav_line_avoid/avoid_navigation.c similarity index 92% rename from sw/airborne/modules/stereoavoid/avoid_navigation.c rename to sw/airborne/modules/stereocam/nav_line_avoid/avoid_navigation.c index 69672874c2..c8fe7e605b 100644 --- a/sw/airborne/modules/stereoavoid/avoid_navigation.c +++ b/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 { diff --git a/sw/airborne/modules/stereoavoid/avoid_navigation.h b/sw/airborne/modules/stereocam/nav_line_avoid/avoid_navigation.h similarity index 83% rename from sw/airborne/modules/stereoavoid/avoid_navigation.h rename to sw/airborne/modules/stereocam/nav_line_avoid/avoid_navigation.h index 6563d959f3..b884b46473 100644 --- a/sw/airborne/modules/stereoavoid/avoid_navigation.h +++ b/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 + +#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]; diff --git a/sw/airborne/modules/stereoavoid/stereo_avoid.c b/sw/airborne/modules/stereocam/nav_line_avoid/stereo_avoid.c similarity index 97% rename from sw/airborne/modules/stereoavoid/stereo_avoid.c rename to sw/airborne/modules/stereocam/nav_line_avoid/stereo_avoid.c index 267a60c10b..910ea7543c 100644 --- a/sw/airborne/modules/stereoavoid/stereo_avoid.c +++ b/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 */ diff --git a/sw/airborne/modules/stereoavoid/stereo_avoid.h b/sw/airborne/modules/stereocam/nav_line_avoid/stereo_avoid.h similarity index 94% rename from sw/airborne/modules/stereoavoid/stereo_avoid.h rename to sw/airborne/modules/stereocam/nav_line_avoid/stereo_avoid.h index e0d50778c7..ff7170d285 100644 --- a/sw/airborne/modules/stereoavoid/stereo_avoid.h +++ b/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 * */ diff --git a/sw/airborne/modules/stereoavoid/stereo_avoid_sim.c b/sw/airborne/modules/stereocam/nav_line_avoid/stereo_avoid_sim.c similarity index 100% rename from sw/airborne/modules/stereoavoid/stereo_avoid_sim.c rename to sw/airborne/modules/stereocam/nav_line_avoid/stereo_avoid_sim.c diff --git a/sw/airborne/modules/stereo_cam/uartrotation.c b/sw/airborne/modules/stereocam/state2camera/state2camera.c similarity index 90% rename from sw/airborne/modules/stereo_cam/uartrotation.c rename to sw/airborne/modules/stereocam/state2camera/state2camera.c index c9d5649647..a064e7ff82 100644 --- a/sw/airborne/modules/stereo_cam/uartrotation.c +++ b/sw/airborne/modules/stereocam/state2camera/state2camera.c @@ -18,16 +18,16 @@ * . */ /** - * @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; diff --git a/sw/airborne/modules/stereo_cam/uartrotation.h b/sw/airborne/modules/stereocam/state2camera/state2camera.h similarity index 93% rename from sw/airborne/modules/stereo_cam/uartrotation.h rename to sw/airborne/modules/stereocam/state2camera/state2camera.h index 5e3803d1e0..3541a7952e 100644 --- a/sw/airborne/modules/stereo_cam/uartrotation.h +++ b/sw/airborne/modules/stereocam/state2camera/state2camera.h @@ -18,14 +18,16 @@ * . */ /** - * @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 extern void write_serial_rot(void); + #endif diff --git a/sw/airborne/modules/stereo_cam/stereocam.c b/sw/airborne/modules/stereocam/stereocam.c similarity index 94% rename from sw/airborne/modules/stereo_cam/stereocam.c rename to sw/airborne/modules/stereocam/stereocam.c index 90c78f005e..49bcb6b29c 100644 --- a/sw/airborne/modules/stereo_cam/stereocam.c +++ b/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; diff --git a/sw/airborne/modules/stereo_cam/stereocam.h b/sw/airborne/modules/stereocam/stereocam.h similarity index 96% rename from sw/airborne/modules/stereo_cam/stereocam.h rename to sw/airborne/modules/stereocam/stereocam.h index 8434cfcff1..045cfb4511 100644 --- a/sw/airborne/modules/stereo_cam/stereocam.h +++ b/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 */ diff --git a/sw/airborne/modules/stereo_cam/stereoprotocol.c b/sw/airborne/modules/stereocam/stereoprotocol.c similarity index 86% rename from sw/airborne/modules/stereo_cam/stereoprotocol.c rename to sw/airborne/modules/stereocam/stereoprotocol.c index 02448be974..cc3ebe54ec 100644 --- a/sw/airborne/modules/stereo_cam/stereoprotocol.c +++ b/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 */ diff --git a/sw/airborne/modules/stereo_cam/stereoprotocol.h b/sw/airborne/modules/stereocam/stereoprotocol.h similarity index 68% rename from sw/airborne/modules/stereo_cam/stereoprotocol.h rename to sw/airborne/modules/stereocam/stereoprotocol.h index 24f4ec7c5f..3a2fb01b24 100644 --- a/sw/airborne/modules/stereo_cam/stereoprotocol.h +++ b/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 #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