diff --git a/conf/airframes/AGGIEAIR/aggieair_minion_rp3_lia.xml b/conf/airframes/AGGIEAIR/aggieair_minion_rp3_lia.xml index 513cf85d26..cb63f8a9f8 100644 --- a/conf/airframes/AGGIEAIR/aggieair_minion_rp3_lia.xml +++ b/conf/airframes/AGGIEAIR/aggieair_minion_rp3_lia.xml @@ -7,7 +7,7 @@ AggieAir RP3 Minion - + diff --git a/conf/conf_tests.xml b/conf/conf_tests.xml index 7e3949fc26..53dd9fc0ce 100644 --- a/conf/conf_tests.xml +++ b/conf/conf_tests.xml @@ -263,17 +263,6 @@ settings_modules="modules/gps_ubx_ucenter.xml modules/ahrs_float_cmpl_rmat.xml modules/stabilization_int_euler.xml modules/nav_basic_rotorcraft.xml modules/guidance_rotorcraft.xml modules/gps.xml modules/imu_common.xml" gui_color="white" /> - + diff --git a/conf/modules/copilot.xml b/conf/modules/copilot.xml index 4fe64de2c5..f9a4ad3367 100644 --- a/conf/modules/copilot.xml +++ b/conf/modules/copilot.xml @@ -35,7 +35,27 @@ - + + + # attach the correct file based on firmware + RESULT=0 + + ifneq (,$(findstring ROTORCRAFT,$($(TARGET).CFLAGS))) + # Rotorcraft + RESULT=1 + $(TARGET).srcs += $(SRC_MODULES)/mission/copilot_rotorcraft.c + endif + + ifneq (,$(findstring FIXEDWING,$($(TARGET).CFLAGS))) + # Fixedwing + $(TARGET).srcs += $(SRC_MODULES)/mission/copilot_fixedwing.c + RESULT=1 + endif + + ifeq ($(RESULT), 0) + $(error Error: Copilot module can be compiled only in Fixeding or Rotorcraft firmware. Please check your firmware.) + endif + diff --git a/sw/airborne/modules/mission/copilot.h b/sw/airborne/modules/mission/copilot.h index d13873aaa0..e9e2d3790f 100644 --- a/sw/airborne/modules/mission/copilot.h +++ b/sw/airborne/modules/mission/copilot.h @@ -46,6 +46,11 @@ #include "subsystems/datalink/datalink.h" #include "modules/datalink/extra_pprz_dl.h" +#include "pprz_mutex.h" + +PPRZ_MUTEX_DECL(copilot_cam_snapshot_mtx); +PPRZ_MUTEX_DECL(copilot_cam_payload_mtx); +PPRZ_MUTEX_DECL(copilot_status_mtx); struct CameraPayload { float timestamp; diff --git a/sw/airborne/modules/mission/copilot.c b/sw/airborne/modules/mission/copilot_common.c similarity index 75% rename from sw/airborne/modules/mission/copilot.c rename to sw/airborne/modules/mission/copilot_common.c index d3b90b5eb2..797a0f72ea 100644 --- a/sw/airborne/modules/mission/copilot.c +++ b/sw/airborne/modules/mission/copilot_common.c @@ -20,7 +20,7 @@ * */ /** - * @file "modules/mission/copilot.c" + * @file "modules/mission/copilot_common.c" * * Mission Computer module, interfacing the mission computer (also known as Copilot), * based losely on @@ -43,14 +43,6 @@ #include "modules/mission/copilot.h" #include "subsystems/datalink/telemetry.h" -#include - -#include "pprz_mutex.h" - -// needed for WP_MOVED confirmation -#include "firmwares/fixedwing/nav.h" -#include "subsystems/navigation/common_nav.h" -#include "math/pprz_geodetic_float.h" bool send_cam_snapshot; bool send_cam_payload; @@ -196,52 +188,3 @@ void copilot_parse_copilot_status_dl(uint8_t *buf) PPRZ_MUTEX_UNLOCK(copilot_status_mtx); } - -/** - * If MOVE_WP from GCS - * - processed in firmware_parse_msg(dev, trans, buf); with regular buffer - * - reponse over telemetry (regular buffer) - * - here send WP_MOVED over extra_dl - * - * If MOVE_WP from extra_dl - * - processed in firmware_parse_msg(dev, trans, buf); with extra buffer - * - response over extra_dl - * - send an update to GCS - * - * In both cases, the MOVE_WP message was already processed in firmware_parse - * here we are taking care only about propagating the change - * - */ -void copilot_parse_move_wp_dl(uint8_t *buf) -{ - if (DL_MOVE_WP_ac_id(buf) == AC_ID) { - uint8_t wp_id = DL_MOVE_WP_wp_id(buf); - - /* Computes from (lat, long) in the referenced UTM zone */ - struct LlaCoor_f lla; - lla.lat = RadOfDeg((float)(DL_MOVE_WP_lat(buf) / 1e7)); - lla.lon = RadOfDeg((float)(DL_MOVE_WP_lon(buf) / 1e7)); - lla.alt = ((float)(DL_MOVE_WP_alt(buf)))/1000.; - struct UtmCoor_f utm; - utm.zone = nav_utm_zone0; - utm_of_lla_f(&utm, &lla); - - // Waypoint range is limited. Computes the UTM pos back from the relative - // coordinates */ - utm.east = waypoints[wp_id].x + nav_utm_east0; - utm.north = waypoints[wp_id].y + nav_utm_north0; - - if (buf == extra_dl_buffer) { - // MOVE_WP came from extra_dl, respond over telemetry - DOWNLINK_SEND_WP_MOVED(DefaultChannel, DefaultDevice, - &wp_id, &utm.east, &utm.north, &utm.alt, &nav_utm_zone0); - } - - if (buf == dl_buffer) { - // MOVE_WP came over telemetry, respond over extra_dl - DOWNLINK_SEND_WP_MOVED(extra_pprz_tp, EXTRA_DOWNLINK_DEVICE, - &wp_id, &utm.east, &utm.north, &utm.alt, &nav_utm_zone0); - } - - } -} diff --git a/sw/airborne/modules/mission/copilot_fixedwing.c b/sw/airborne/modules/mission/copilot_fixedwing.c new file mode 100644 index 0000000000..362ccb1ea8 --- /dev/null +++ b/sw/airborne/modules/mission/copilot_fixedwing.c @@ -0,0 +1,103 @@ +/* + * Copyright (C) 2016 2017 Michal Podhradsky + * + * 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/mission/copilot_fixedwing.c" + * + * Mission Computer module, interfacing the mission computer (also known as Copilot), + * based losely on + * ISaAC: The Intelligent Safety and Airworthiness Co-Pilot module + * Based on paper "A Payload Verification and Management Framework + * for Small UAV-based Personal Remote Sensing Systems" by Cal Coopmans + * and Chris Coffin. Link: http://ieeexplore.ieee.org/abstract/document/6309316/ + * + * More info can be found on http://wiki.paparazziuav.org/wiki/Mission_computer + * + * Copilot is intended mainly for mapping applications. + * + * This module processes messages from Copilot, and either forwards them to the GCS + * (such as CAMERA_SNAPSHOT or CAMERA_PAYLOAD messages), or responds to them as necessary + * (such as MOVE_WP). + * + * The module assumes the source of the messages is trusted (i.e. not authentication besides + * AC_ID check is performed). + */ + +#include "modules/mission/copilot.h" +#include "subsystems/datalink/telemetry.h" + +// needed for WP_MOVED confirmation +#include "firmwares/fixedwing/nav.h" +#include "subsystems/navigation/common_nav.h" +#include "math/pprz_geodetic_float.h" + +/** + * If MOVE_WP from GCS + * - processed in firmware_parse_msg(dev, trans, buf); with regular buffer + * - reponse over telemetry (regular buffer) + * - here send WP_MOVED over extra_dl + * + * If MOVE_WP from extra_dl + * - processed in firmware_parse_msg(dev, trans, buf); with extra buffer + * - response over extra_dl + * - send an update to GCS + * + * In both cases, the MOVE_WP message was already processed in firmware_parse + * here we are taking care only about propagating the change + * + */ +void copilot_parse_move_wp_dl(uint8_t *buf) +{ + if (DL_MOVE_WP_ac_id(buf) == AC_ID) { + uint8_t wp_id = DL_MOVE_WP_wp_id(buf); + + if (wp_id >= nb_waypoint) { + return; + } + + /* Computes from (lat, long) in the referenced UTM zone */ + struct LlaCoor_f lla; + lla.lat = RadOfDeg((float)(DL_MOVE_WP_lat(buf) / 1e7)); + lla.lon = RadOfDeg((float)(DL_MOVE_WP_lon(buf) / 1e7)); + lla.alt = ((float)(DL_MOVE_WP_alt(buf)))/1000.; + struct UtmCoor_f utm; + utm.zone = nav_utm_zone0; + utm_of_lla_f(&utm, &lla); + + // Waypoint range is limited. Computes the UTM pos back from the relative + // coordinates */ + utm.east = waypoints[wp_id].x + nav_utm_east0; + utm.north = waypoints[wp_id].y + nav_utm_north0; + + if (buf == extra_dl_buffer) { + // MOVE_WP came from extra_dl, respond over telemetry + DOWNLINK_SEND_WP_MOVED(DefaultChannel, DefaultDevice, + &wp_id, &utm.east, &utm.north, &utm.alt, &nav_utm_zone0); + } + + if (buf == dl_buffer) { + // MOVE_WP came over telemetry, respond over extra_dl + DOWNLINK_SEND_WP_MOVED(extra_pprz_tp, EXTRA_DOWNLINK_DEVICE, + &wp_id, &utm.east, &utm.north, &utm.alt, &nav_utm_zone0); + } + + } +} diff --git a/sw/airborne/modules/mission/copilot_rotorcraft.c b/sw/airborne/modules/mission/copilot_rotorcraft.c new file mode 100644 index 0000000000..9e7fc6bb78 --- /dev/null +++ b/sw/airborne/modules/mission/copilot_rotorcraft.c @@ -0,0 +1,121 @@ +/* + * Copyright (C) 2016 2017 Michal Podhradsky + * + * 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/mission/copilot_rotorcraft.c" + * + * Mission Computer module, interfacing the mission computer (also known as Copilot), + * based losely on + * ISaAC: The Intelligent Safety and Airworthiness Co-Pilot module + * Based on paper "A Payload Verification and Management Framework + * for Small UAV-based Personal Remote Sensing Systems" by Cal Coopmans + * and Chris Coffin. Link: http://ieeexplore.ieee.org/abstract/document/6309316/ + * + * More info can be found on http://wiki.paparazziuav.org/wiki/Mission_computer + * + * Copilot is intended mainly for mapping applications. + * + * This module processes messages from Copilot, and either forwards them to the GCS + * (such as CAMERA_SNAPSHOT or CAMERA_PAYLOAD messages), or responds to them as necessary + * (such as MOVE_WP). + * + * The module assumes the source of the messages is trusted (i.e. not authentication besides + * AC_ID check is performed). + */ + +#include "modules/mission/copilot.h" +#include "subsystems/datalink/telemetry.h" + +// needed for WP_MOVED confirmation +#include "firmwares/rotorcraft/navigation.h" +//#include "math/pprz_geodetic_float.h" +#include "math/pprz_geodetic_int.h" +//#include "state.h" + +/** + * If MOVE_WP from GCS + * - processed in firmware_parse_msg(dev, trans, buf); with regular buffer + * - reponse over telemetry (regular buffer) + * - here send WP_MOVED over extra_dl + * + * If MOVE_WP from extra_dl + * - processed in firmware_parse_msg(dev, trans, buf); with extra buffer + * - response over extra_dl + * - send an update to GCS + * + * In both cases, the MOVE_WP message was already processed in firmware_parse + * here we are taking care only about propagating the change + * + */ +void copilot_parse_move_wp_dl(uint8_t *buf) +{ + if (DL_MOVE_WP_ac_id(buf) == AC_ID) { + if (stateIsLocalCoordinateValid()) { + uint8_t wp_id = DL_MOVE_WP_wp_id(buf); + if (wp_id >= nb_waypoint) { + return; + } + + struct LlaCoor_i lla; + lla.lat = DL_MOVE_WP_lat(buf); + lla.lon = DL_MOVE_WP_lon(buf); + /* WP_alt from message is alt above MSL in mm + * lla.alt is above ellipsoid in mm + */ + lla.alt = DL_MOVE_WP_alt(buf) - state.ned_origin_i.hmsl + + state.ned_origin_i.lla.alt; + + + if (waypoint_is_global(wp_id)) { + /* lla->alt is above ellipsoid, WP_MOVED_LLA has hmsl alt */ + int32_t hmsl = lla.alt - state.ned_origin_i.lla.alt + state.ned_origin_i.hmsl; + + if (buf == extra_dl_buffer) { + // MOVE_WP came from extra_dl, respond over telemetry + DOWNLINK_SEND_WP_MOVED_LLA(DefaultChannel, DefaultDevice, &wp_id, + &lla.lat, &lla.lon, &hmsl); + } + + if (buf == dl_buffer) { + // MOVE_WP came over telemetry, respond over extra_dl + DOWNLINK_SEND_WP_MOVED_LLA(extra_pprz_tp, DefaultDevice, &wp_id, + &lla.lat, &lla.lon, &hmsl); + } + } else { + if (buf == extra_dl_buffer) { + // MOVE_WP came from extra_dl, respond over telemetry + DOWNLINK_SEND_WP_MOVED_ENU(DefaultChannel, DefaultDevice, &wp_id, + &waypoints[wp_id].enu_i.x, + &waypoints[wp_id].enu_i.y, + &waypoints[wp_id].enu_i.z); + } + + if (buf == dl_buffer) { + // MOVE_WP came over telemetry, respond over extra_dl + DOWNLINK_SEND_WP_MOVED_ENU(extra_pprz_tp, DefaultDevice, &wp_id, + &waypoints[wp_id].enu_i.x, + &waypoints[wp_id].enu_i.y, + &waypoints[wp_id].enu_i.z); + } + } + } + } +}