From 64ee6bf6f19fe5f9a2899c0626f005f8c4471f91 Mon Sep 17 00:00:00 2001 From: Freek van Tienen Date: Thu, 2 Jul 2015 13:35:42 +0200 Subject: [PATCH] [ardrone2] Remove the deprecated SDK version --- conf/airframes/ardrone2_sdk.xml | 145 ------------ conf/boards/ardrone2_sdk.makefile | 49 ---- conf/conf_example.xml | 11 - conf/conf_tests.xml | 11 - conf/firmwares/rotorcraft.makefile | 2 - .../shared/actuators_ardrone2.makefile | 12 +- .../subsystems/shared/imu_ardrone2.makefile | 6 - sw/airborne/arch/linux/led_hw.h | 2 +- sw/airborne/boards/ardrone/actuators_at.c | 73 ------ sw/airborne/boards/ardrone/actuators_at.h | 36 --- sw/airborne/boards/ardrone/at_com.c | 208 ----------------- sw/airborne/boards/ardrone/at_com.h | 212 ------------------ sw/airborne/boards/ardrone/electrical_dummy.c | 37 --- sw/airborne/boards/ardrone2_sdk.h | 22 -- sw/airborne/subsystems/ahrs/ahrs_ardrone2.c | 207 ----------------- sw/airborne/subsystems/ahrs/ahrs_ardrone2.h | 58 ----- sw/airborne/subsystems/gps/gps_ardrone2.c | 86 ------- sw/airborne/subsystems/gps/gps_ardrone2.h | 46 ---- sw/airborne/subsystems/imu/imu_ardrone2_sdk.c | 39 ---- sw/airborne/subsystems/imu/imu_ardrone2_sdk.h | 34 --- sw/airborne/subsystems/ins/ins_ardrone2.c | 176 --------------- sw/airborne/subsystems/ins/ins_ardrone2.h | 58 ----- sw/ext/bebop.py | 12 +- sw/tools/parrot/ardrone2.py | 14 +- sw/tools/parrot/ardrone2/check_update.sh | 21 +- sw/tools/parrot/bebop.py | 2 +- 26 files changed, 24 insertions(+), 1555 deletions(-) delete mode 100644 conf/airframes/ardrone2_sdk.xml delete mode 100644 conf/boards/ardrone2_sdk.makefile delete mode 100644 sw/airborne/boards/ardrone/actuators_at.c delete mode 100644 sw/airborne/boards/ardrone/actuators_at.h delete mode 100644 sw/airborne/boards/ardrone/at_com.c delete mode 100644 sw/airborne/boards/ardrone/at_com.h delete mode 100644 sw/airborne/boards/ardrone/electrical_dummy.c delete mode 100644 sw/airborne/boards/ardrone2_sdk.h delete mode 100644 sw/airborne/subsystems/ahrs/ahrs_ardrone2.c delete mode 100644 sw/airborne/subsystems/ahrs/ahrs_ardrone2.h delete mode 100644 sw/airborne/subsystems/gps/gps_ardrone2.c delete mode 100644 sw/airborne/subsystems/gps/gps_ardrone2.h delete mode 100644 sw/airborne/subsystems/imu/imu_ardrone2_sdk.c delete mode 100644 sw/airborne/subsystems/imu/imu_ardrone2_sdk.h delete mode 100644 sw/airborne/subsystems/ins/ins_ardrone2.c delete mode 100644 sw/airborne/subsystems/ins/ins_ardrone2.h diff --git a/conf/airframes/ardrone2_sdk.xml b/conf/airframes/ardrone2_sdk.xml deleted file mode 100644 index efa7da6706..0000000000 --- a/conf/airframes/ardrone2_sdk.xml +++ /dev/null @@ -1,145 +0,0 @@ - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - -
- - - - - - - - - - - - - - - - - - - - - -
- - -
- - - - - - - -
- -
- - - - - -
- -
- - - - -
- -
- - - - -
- -
- - - - -
- -
- - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - -
- - diff --git a/conf/boards/ardrone2_sdk.makefile b/conf/boards/ardrone2_sdk.makefile deleted file mode 100644 index 27852f94e8..0000000000 --- a/conf/boards/ardrone2_sdk.makefile +++ /dev/null @@ -1,49 +0,0 @@ -# Hey Emacs, this is a -*- makefile -*- -# -# ardrone2_sdk.makefile -# -# http://wiki.paparazziuav.org/wiki/AR.Drone_2_-_Specifications -# - -BOARD=ardrone -BOARD_VERSION=2 -BOARD_TYPE=sdk -BOARD_CFG=\"boards/$(BOARD)$(BOARD_VERSION)_$(BOARD_TYPE).h\" - -ARCH=linux -$(TARGET).ARCHDIR = $(ARCH) -# include conf/Makefile.ardrone2 (with specific upload rules) instead of only Makefile.linux: -ap.MAKEFILE = ardrone2 - -# ----------------------------------------------------------------------- -USER=foobar -HOST=192.168.1.1 -SUB_DIR=sdk -FTP_DIR=/data/video -TARGET_DIR=$(FTP_DIR)/$(SUB_DIR) -# ----------------------------------------------------------------------- -ARDRONE2_START_PAPARAZZI ?= 0 -ARDRONE2_WIFI_MODE ?= 0 -ARDRONE2_SSID ?= ardrone2_paparazzi -ARDRONE2_IP_ADDRESS_BASE ?= 192.168.1. -ARDRONE2_IP_ADDRESS_PROBE ?= 1 -# ----------------------------------------------------------------------- - -# The GPS sensor is connected trough USB so we have to define the device -GPS_PORT ?= UART1 -GPS_BAUD ?= B57600 - -# The datalink default uses UDP -MODEM_HOST ?= 192.168.1.255 - -# for distinction between SDK and RAW version -ap.CFLAGS +=-DARDRONE2_SDK - -# ----------------------------------------------------------------------- - -# default LED configuration -RADIO_CONTROL_LED ?= none -BARO_LED ?= none -AHRS_ALIGNER_LED ?= none -GPS_LED ?= none -SYS_TIME_LED ?= none diff --git a/conf/conf_example.xml b/conf/conf_example.xml index 83e9cce29b..212a2b3fdd 100644 --- a/conf/conf_example.xml +++ b/conf/conf_example.xml @@ -208,17 +208,6 @@ settings_modules="modules/gps_ubx_ucenter.xml" gui_color="red" /> - - #include BOARD_CONFIG -#if defined BOARD_ARDRONE2_SDK || defined BOARD_ARDRONE2_RAW +#if defined BOARD_ARDRONE2_RAW extern uint32_t led_hw_values; #define LED_INIT(i) { led_hw_values &= ~(1< 0 && (ahrs_ardrone2.control_state == CTRL_DEFAULT || ahrs_ardrone2.control_state == CTRL_INIT - || ahrs_ardrone2.control_state == CTRL_LANDED)) { - at_com_send_ref(REF_TAKEOFF); - } - - //Check emergency or stop engine - if ((ahrs_ardrone2.state & ARDRONE_EMERGENCY_MASK) != 0) { - at_com_send_ref(REF_EMERGENCY); - } else if(thrust < -0.9 && !(ahrs_ardrone2.control_state == CTRL_DEFAULT || - ahrs_ardrone2.control_state == CTRL_INIT || ahrs_ardrone2.control_state == CTRL_LANDED)) { - at_com_send_ref(0); - } - - //Calibration - if ((ahrs_ardrone2.state & ARDRONE_MAGNETO_NEEDS_CALIB) != 0 && - (ahrs_ardrone2.control_state == CTRL_FLYING || ahrs_ardrone2.control_state == CTRL_HOVERING)) { - at_com_send_calib(0); - } - - //Moving - if ((ahrs_ardrone2.state & ARDRONE_MAGNETO_NEEDS_CALIB) == 0 && - (ahrs_ardrone2.control_state == CTRL_FLYING || ahrs_ardrone2.control_state == CTRL_HOVERING)) { - at_com_send_pcmd(1, thrust, roll, pitch, yaw); - } - - //Keep alive (FIXME) - at_com_send_config("general:navdata_demo", "FALSE"); -} diff --git a/sw/airborne/boards/ardrone/actuators_at.h b/sw/airborne/boards/ardrone/actuators_at.h deleted file mode 100644 index b862a92c6c..0000000000 --- a/sw/airborne/boards/ardrone/actuators_at.h +++ /dev/null @@ -1,36 +0,0 @@ -/* - * Copyright (C) 2012-2013 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, write to - * the Free Software Foundation, 59 Temple Place - Suite 330, - * Boston, MA 02111-1307, USA. - */ - -/** - * @file boards/ardrone/actuators_at.h - * ardrone2-sdk actuators are driven by external software controller by AT-commands - */ - -#ifndef BOARDS_ARDRONE_ACTUATORS_AT_H -#define BOARDS_ARDRONE_ACTUATORS_AT_H - -#include "paparazzi.h" - -extern void actuators_init(void); -extern void actuators_set(pprz_t commands[]); -#define SetActuatorsFromCommands(commands, AP_MODE) actuators_set(commands) - -#endif /* BOARDS_ARDRONE_ACTUATORS_AT_H */ diff --git a/sw/airborne/boards/ardrone/at_com.c b/sw/airborne/boards/ardrone/at_com.c deleted file mode 100644 index 188a216ba9..0000000000 --- a/sw/airborne/boards/ardrone/at_com.c +++ /dev/null @@ -1,208 +0,0 @@ -/* - * Copyright (C) 2012-2013 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, write to - * the Free Software Foundation, 59 Temple Place - Suite 330, - * Boston, MA 02111-1307, USA. - */ - -/** - * @file boards/ardrone/at_com.c - * Sending and receiving of AT-commands specified by the ardrone API - */ - -#include "at_com.h" -#include "boards/ardrone2_sdk.h" -#include "generated/airframe.h" - -#include -#include -#include -#include -#include -#include - -int packet_seq = 1; //Packet sequence number - -int at_socket = -1, //AT socket connection - navdata_socket = -1; //Navdata socket connection - -struct sockaddr_in pc_addr, //Own pc address - drone_at, //Drone AT address - drone_nav, //Drone nav address - from; //From address - -bool_t at_com_ready = FALSE; //Status of the at communication -char sessionId[9]; //THe config session ID - -void at_com_send(char *command); -void init_at_config(void); - -//Init the at_com -void init_at_com(void) -{ - //Check if already initialized - if (at_com_ready) { - return; - } - - //Create the at and navdata socket - if ((at_socket = socket(AF_INET, SOCK_DGRAM, 0)) < 0) { - printf("at_com: at_socket error (%s)\n", strerror(errno)); - } - if ((navdata_socket = socket(AF_INET, SOCK_DGRAM, 0)) < 0) { - printf("at_com: navdata_socket error (%s)\n", strerror(errno)); - } - - //For recvfrom - pc_addr.sin_family = AF_INET; - pc_addr.sin_addr.s_addr = htonl(INADDR_ANY); - pc_addr.sin_port = htons(9800); - - //For sendto AT - drone_at.sin_family = AF_INET; - drone_at.sin_addr.s_addr = inet_addr(ARDRONE_IP); - drone_at.sin_port = htons(ARDRONE_AT_PORT); - - //For sendto navadata init - drone_nav.sin_family = AF_INET; - drone_nav.sin_addr.s_addr = inet_addr(ARDRONE_IP); - drone_nav.sin_port = htons(ARDRONE_NAVDATA_PORT); - - //Bind the navdata socket - if (bind(navdata_socket, (struct sockaddr *) &pc_addr, sizeof(pc_addr)) < 0) { - printf("at_com: bind error (%s)\n", strerror(errno)); - } - - //Set unicast mode on - int one = 1; - sendto(navdata_socket, &one, 4, 0, (struct sockaddr *) &drone_nav, - sizeof(drone_nav)); - - //Init at config - init_at_config(); - - //Set at_com to ready - at_com_ready = TRUE; -} - -//Init the at config -void init_at_config(void) -{ - //Generate a session id - uint32_t binaryId = (uint32_t) rand(); - binaryId = (0 != binaryId) ? binaryId : 1u; - snprintf(sessionId, 9, "%08x", binaryId); - sessionId[8] = '\0'; - - //Send session, application and user id: - at_com_send_config("custom:session_id", sessionId); - at_com_send_config("custom:application_id", "9D7BFD45"); - at_com_send_config("custom:profile_id", "2BF07F58"); - - //Send config values - at_com_send_config("control:euler_angle_max", "0.52"); - at_com_send_config("control:altitude_max", "20000"); - at_com_send_config("control:control_vz_max", "2000"); - at_com_send_config("control:control_yaw", "6.11"); - - //Send config values with the airframe.h -#ifndef ARDRONE_FLIGHT_INDOOR - at_com_send_config("control:outdoor", "TRUE"); -#else - at_com_send_config("control:outdoor", "FALSE"); -#endif -#ifndef ARDRONE_WITHOUT_SHELL - at_com_send_config("control:flight_without_shell", "FALSE"); -#else - at_com_send_config("control:flight_without_shell", "TRUE"); -#endif -#ifdef ARDRONE_OWNER_MAC - at_com_send_config("network:owner_mac", ARDRONE_OWNER_MAC); -#endif -} - -//Recieve a navdata packet -int at_com_recieve_navdata(unsigned char *buffer) -{ - int l = sizeof(from); - int n; - // FIXME(ben): not clear why recvfrom() and not recv() is used. - n = recvfrom(navdata_socket, buffer, ARDRONE_NAVDATA_BUFFER_SIZE, 0x0, - (struct sockaddr *) &from, (socklen_t *) &l); - - return n; -} - -//Send an AT command -void at_com_send(char *command) -{ - sendto(at_socket, command, strlen(command), 0, (struct sockaddr *) &drone_at, - sizeof(drone_at)); -} - -//Send a Config -void at_com_send_config(char *key, char *value) -{ - char command[256]; - sprintf(command, "AT*CONFIG_IDS=%d,\"%s\",\"2BF07F58\",\"9D7BFD45\"\r", - packet_seq++, sessionId); - at_com_send(command); - sprintf(command, "AT*CONFIG=%d,\"%s\",\"%s\"\r", packet_seq++, key, value); - at_com_send(command); -} - -//Send a Flat trim -void at_com_send_ftrim(void) -{ - char command[256]; - sprintf(command, "AT*FTRIM=%d\r", packet_seq++); - at_com_send(command); -} - -//Send a Ref -void at_com_send_ref(int bits) -{ - char command[256]; - sprintf(command, "AT*REF=%d,%d\r", packet_seq++, bits | REF_DEFAULT); - at_com_send(command); -} - -//Send a Pcmd -void at_com_send_pcmd(int mode, float thrust, float roll, float pitch, - float yaw) -{ - int f_thrust, f_roll, f_pitch, f_yaw; - char command[256]; - - //Change the floats to ints(dereferencing) - memcpy(&f_thrust, &thrust, sizeof thrust); - memcpy(&f_roll, &roll, sizeof roll); - memcpy(&f_pitch, &pitch, sizeof pitch); - memcpy(&f_yaw, &yaw, sizeof yaw); - - sprintf(command, "AT*PCMD=%d,%d,%d,%d,%d,%d\r", packet_seq++, mode, f_roll, - f_pitch, f_thrust, f_yaw); - at_com_send(command); -} - -//Send a Calib -void at_com_send_calib(int device) -{ - char command[256]; - sprintf(command, "AT*CALIB=%d,%d\r", packet_seq++, device); - at_com_send(command); -} diff --git a/sw/airborne/boards/ardrone/at_com.h b/sw/airborne/boards/ardrone/at_com.h deleted file mode 100644 index f5196399e1..0000000000 --- a/sw/airborne/boards/ardrone/at_com.h +++ /dev/null @@ -1,212 +0,0 @@ -/* - * Copyright (C) 2012-2013 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, write to - * the Free Software Foundation, 59 Temple Place - Suite 330, - * Boston, MA 02111-1307, USA. - */ - -/** - * @file boards/ardrone/at_com.h - * Sending and receiving of AT-commands specified by the ardrone API - */ - -#include "math/pprz_algebra_float.h" - -#ifndef BOARDS_ARDRONE_AT_COM_H -#define BOARDS_ARDRONE_AT_COM_H - -#define NAVDATA_HEADER (0x55667788) - -//Define the AT_REF bits -typedef enum { - REF_TAKEOFF = 1U << 9, - REF_EMERGENCY = 1U << 8, - REF_DEFAULT = 0x11540000 -} AT_REFS; - -//Define control states -typedef enum { - CTRL_DEFAULT, - CTRL_INIT, - CTRL_LANDED, - CTRL_FLYING, - CTRL_HOVERING, - CTRL_TEST, - CTRL_TRANS_TAKEOFF, - CTRL_TRANS_GOTOFIX, - CTRL_TRANS_LANDING, - CTRL_TRANS_LOOPING, - CTRL_NUM_STATES -} CTRL_STATES; - -//Define the AR.Drone states -typedef enum { - ARDRONE_FLY_MASK = 1U << 0, /*!< FLY MASK : (0) ardrone is landed, (1) ardrone is flying */ - ARDRONE_VIDEO_MASK = 1U << 1, /*!< VIDEO MASK : (0) video disable, (1) video enable */ - ARDRONE_VISION_MASK = 1U << 2, /*!< VISION MASK : (0) vision disable, (1) vision enable */ - ARDRONE_CONTROL_MASK = 1U << 3, /*!< CONTROL ALGO : (0) euler angles control, (1) angular speed control */ - ARDRONE_ALTITUDE_MASK = 1U << 4, /*!< ALTITUDE CONTROL ALGO : (0) altitude control inactive (1) altitude control active */ - ARDRONE_USER_FEEDBACK_START = 1U << 5, /*!< USER feedback : Start button state */ - ARDRONE_COMMAND_MASK = 1U << 6, /*!< Control command ACK : (0) None, (1) one received */ - ARDRONE_CAMERA_MASK = 1U << 7, /*!< CAMERA MASK : (0) camera not ready, (1) Camera ready */ - ARDRONE_TRAVELLING_MASK = 1U << 8, /*!< Travelling mask : (0) disable, (1) enable */ - ARDRONE_USB_MASK = 1U << 9, /*!< USB key : (0) usb key not ready, (1) usb key ready */ - ARDRONE_NAVDATA_DEMO_MASK = 1U << 10, /*!< Navdata demo : (0) All navdata, (1) only navdata demo */ - ARDRONE_NAVDATA_BOOTSTRAP = 1U << 11, /*!< Navdata bootstrap : (0) options sent in all or demo mode, (1) no navdata options sent */ - ARDRONE_MOTORS_MASK = 1U << 12, /*!< Motors status : (0) Ok, (1) Motors problem */ - ARDRONE_COM_LOST_MASK = 1U << 13, /*!< Communication Lost : (1) com problem, (0) Com is ok */ - ARDRONE_SOFTWARE_FAULT = 1U << 14, /*!< Software fault detected - user should land as quick as possible (1) */ - ARDRONE_VBAT_LOW = 1U << 15, /*!< VBat low : (1) too low, (0) Ok */ - ARDRONE_USER_EL = 1U << 16, /*!< User Emergency Landing : (1) User EL is ON, (0) User EL is OFF*/ - ARDRONE_TIMER_ELAPSED = 1U << 17, /*!< Timer elapsed : (1) elapsed, (0) not elapsed */ - ARDRONE_MAGNETO_NEEDS_CALIB = 1U << 18, /*!< Magnetometer calibration state : (0) Ok, no calibration needed, (1) not ok, calibration needed */ - ARDRONE_ANGLES_OUT_OF_RANGE = 1U << 19, /*!< Angles : (0) Ok, (1) out of range */ - ARDRONE_WIND_MASK = 1U << 20, /*!< WIND MASK: (0) ok, (1) Too much wind */ - ARDRONE_ULTRASOUND_MASK = 1U << 21, /*!< Ultrasonic sensor : (0) Ok, (1) deaf */ - ARDRONE_CUTOUT_MASK = 1U << 22, /*!< Cutout system detection : (0) Not detected, (1) detected */ - ARDRONE_PIC_VERSION_MASK = 1U << 23, /*!< PIC Version number OK : (0) a bad version number, (1) version number is OK */ - ARDRONE_ATCODEC_THREAD_ON = 1U << 24, /*!< ATCodec thread ON : (0) thread OFF (1) thread ON */ - ARDRONE_NAVDATA_THREAD_ON = 1U << 25, /*!< Navdata thread ON : (0) thread OFF (1) thread ON */ - ARDRONE_VIDEO_THREAD_ON = 1U << 26, /*!< Video thread ON : (0) thread OFF (1) thread ON */ - ARDRONE_ACQ_THREAD_ON = 1U << 27, /*!< Acquisition thread ON : (0) thread OFF (1) thread ON */ - ARDRONE_CTRL_WATCHDOG_MASK = 1U << 28, /*!< CTRL watchdog : (1) delay in control execution (> 5ms), (0) control is well scheduled */ - ARDRONE_ADC_WATCHDOG_MASK = 1U << 29, /*!< ADC Watchdog : (1) delay in uart2 dsr (> 5ms), (0) uart2 is good */ - ARDRONE_COM_WATCHDOG_MASK = 1U << 30, /*!< Communication Watchdog : (1) com problem, (0) Com is ok */ - ARDRONE_EMERGENCY_MASK = 1U << 31 /*!< Emergency landing : (0) no emergency, (1) emergency */ -} ARDRONE_STATES; - -//Navdata option packet without data -typedef struct _navdata_option_t { - uint16_t tag; - uint16_t size; - uint8_t data[1]; -} __attribute__((packed)) navdata_option_t; - -//Main navdata packet -typedef struct _navdata_t { - uint32_t header; /*!< Always set to NAVDATA_HEADER */ - uint32_t ardrone_state; /*!< Bit mask built from def_ardrone_state_mask_t */ - uint32_t sequence; /*!< Sequence number, incremented for each sent packet */ - uint32_t vision_defined; - - navdata_option_t options[1]; -} __attribute__((packed)) navdata_t; - -//Navdata checksum packet -typedef struct _navdata_cks_t { - uint16_t tag; - uint16_t size; - uint32_t cks; -} __attribute__((packed)) navdata_cks_t; - -//Navdata demo option -typedef struct _navdata_demo_t { - uint16_t tag; /*!< Navdata block ('option') identifier */ - uint16_t size; /*!< set this to the size of this structure */ - uint32_t ctrl_state; /*!< Flying state (landed, flying, hovering, etc.) defined in CTRL_STATES enum. */ - uint32_t vbat_flying_percentage; /*!< battery voltage filtered (mV) */ - float theta; /*!< UAV's pitch in milli-degrees */ - float phi; /*!< UAV's roll in milli-degrees */ - float psi; /*!< UAV's yaw in milli-degrees */ - int32_t altitude; /*!< UAV's altitude in centimeters */ - float vx; /*!< UAV's estimated linear velocity */ - float vy; /*!< UAV's estimated linear velocity */ - float vz; /*!< UAV's estimated linear velocity */ - uint32_t num_frames; /*!< streamed frame index */ // Not used -> To integrate in video stage. - // Camera parameters compute by detection - struct FloatMat33 detection_camera_rot; /*!< Deprecated ! Don't use ! */ - struct FloatVect3 detection_camera_trans; /*!< Deprecated ! Don't use ! */ - uint32_t detection_tag_index; /*!< Deprecated ! Don't use ! */ - uint32_t detection_camera_type; /*!< Type of tag searched in detection */ - // Camera parameters compute by drone - struct FloatMat33 drone_camera_rot; /*!< Deprecated ! Don't use ! */ - struct FloatVect3 drone_camera_trans; /*!< Deprecated ! Don't use ! */ -} __attribute__((packed)) navdata_demo_t; - -//Navdata physical measures option -typedef struct _navdata_phys_measures_t { - uint16_t tag; - uint16_t size; - - float accs_temp; - uint16_t gyro_temp; - struct FloatVect3 phys_accs; - struct FloatVect3 phys_gyros; - uint32_t alim3V3; // 3.3volt alim [LSB] - uint32_t vrefEpson; // ref volt Epson gyro [LSB] - uint32_t vrefIDG; // ref volt IDG gyro [LSB] -} __attribute__((packed)) navdata_phys_measures_t; - -//Navdata gps packet -typedef double float64_t; //TODO: Fix this nicely, but this is only used here -typedef float float32_t; //TODO: Fix this nicely, but this is only used here -typedef struct _navdata_gps_t { - uint16_t tag; /*!< Navdata block ('option') identifier */ - uint16_t size; /*!< set this to the size of this structure */ - float64_t lat; /*!< Latitude */ - float64_t lon; /*!< Longitude */ - float64_t elevation; /*!< Elevation */ - float64_t hdop; /*!< hdop */ - int32_t data_available; /*!< When there is data available */ - uint8_t unk_0[8]; - float64_t lat0; /*!< Latitude ??? */ - float64_t lon0; /*!< Longitude ??? */ - float64_t lat_fuse; /*!< Latitude fused */ - float64_t lon_fuse; /*!< Longitude fused */ - uint32_t gps_state; /*!< State of the GPS, still need to figure out */ - uint8_t unk_1[40]; - float64_t vdop; /*!< vdop */ - float64_t pdop; /*!< pdop */ - float32_t speed; /*!< speed */ - uint32_t last_frame_timestamp; /*!< Timestamp from the last frame */ - float32_t degree; /*!< Degree */ - float32_t degree_mag; /*!< Degree of the magnetic */ - uint8_t unk_2[16]; - struct { - uint8_t sat; - uint8_t cn0; - } channels[12]; - int32_t gps_plugged; /*!< When the gps is plugged */ - uint8_t unk_3[108]; - float64_t gps_time; /*!< The gps time of week */ - uint16_t week; /*!< The gps week */ - uint8_t gps_fix; /*!< The gps fix */ - uint8_t num_sattelites; /*!< Number of sattelites */ - uint8_t unk_4[24]; - float64_t ned_vel_c0; /*!< NED velocity */ - float64_t ned_vel_c1; /*!< NED velocity */ - float64_t ned_vel_c2; /*!< NED velocity */ - float64_t pos_accur_c0; /*!< Position accuracy */ - float64_t pos_accur_c1; /*!< Position accuracy */ - float64_t pos_accur_c2; /*!< Position accuracy */ - float32_t speed_acur; /*!< Speed accuracy */ - float32_t time_acur; /*!< Time accuracy */ - uint8_t unk_5[72]; - float32_t temprature; - float32_t pressure; -} __attribute__((packed)) navdata_gps_t; - -//External functions -extern void init_at_com(void); -extern int at_com_recieve_navdata(unsigned char *buffer); -extern void at_com_send_config(char *key, char *value); -extern void at_com_send_ftrim(void); -extern void at_com_send_ref(int bits); -extern void at_com_send_pcmd(int mode, float thrust, float roll, float pitch, float yaw); -extern void at_com_send_calib(int device); - -#endif /* BOARDS_ARDRONE_AT_COM_H */ diff --git a/sw/airborne/boards/ardrone/electrical_dummy.c b/sw/airborne/boards/ardrone/electrical_dummy.c deleted file mode 100644 index 0093d0a011..0000000000 --- a/sw/airborne/boards/ardrone/electrical_dummy.c +++ /dev/null @@ -1,37 +0,0 @@ -/* - * - * Copyright (C) 2009-2013 The Paparazzi Team - * - * 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 boards/ardrone/electrical_dummy.c - * dummy electrical status readings for ardrone-sdk version. - * - * Because ardrone2-sdk version does its battery updating in ahrs_adrone2.c. - */ - -#include "subsystems/electrical.h" - -struct Electrical electrical; - -void electrical_init(void) { } - -void electrical_periodic(void) { } diff --git a/sw/airborne/boards/ardrone2_sdk.h b/sw/airborne/boards/ardrone2_sdk.h deleted file mode 100644 index 12b7237ef5..0000000000 --- a/sw/airborne/boards/ardrone2_sdk.h +++ /dev/null @@ -1,22 +0,0 @@ -#ifndef CONFIG_ARDRONE2_SDK -#define CONFIG_ARDRONE2_SDK - -#define BOARD_ARDRONE2_SDK - -#define UART1_DEV "/dev/ttyUSB0" - -/* Internal communication */ -#define ARDRONE_NAVDATA_PORT 5554 -#define ARDRONE_AT_PORT 5556 -#define ARDRONE_NAVDATA_BUFFER_SIZE 4096 -#define ARDRONE_IP "192.168.1.1" - -/* Default actuators driver */ -#define DEFAULT_ACTUATORS "boards/ardrone/actuators_at.h" -#define ActuatorDefaultSet(_x,_y) {} -#define ActuatorsDefaultInit() {} -#define ActuatorsDefaultCommit() {} - -#define USE_BARO_BOARD 0 - -#endif /* CONFIG_ARDRONE2_SDK */ diff --git a/sw/airborne/subsystems/ahrs/ahrs_ardrone2.c b/sw/airborne/subsystems/ahrs/ahrs_ardrone2.c deleted file mode 100644 index 520f465394..0000000000 --- a/sw/airborne/subsystems/ahrs/ahrs_ardrone2.c +++ /dev/null @@ -1,207 +0,0 @@ -/* - * Copyright (C) 2012-2013 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, write to - * the Free Software Foundation, 59 Temple Place - Suite 330, - * Boston, MA 02111-1307, USA. - */ - -/** - * @file subsystems/ahrs/ahrs_ardrone2.c - * AHRS implementation for ardrone2-sdk based on AT-commands. - * - * Uses AT-Commands to communicate with ardrone api to retrieve AHRS data - * and also sets battery level. - */ - -#ifdef ARDRONE2_DEBUG -# include -# include -#endif - -#include "subsystems/ahrs.h" -#include "ahrs_ardrone2.h" -#include "state.h" -#include "math/pprz_algebra_float.h" -#include "boards/ardrone/at_com.h" -#include "subsystems/electrical.h" - -#ifdef USE_GPS_ARDRONE2 -#include "subsystems/gps/gps_ardrone2.h" -#endif - -struct AhrsARDrone ahrs_ardrone2; -unsigned char buffer[4096]; //Packet buffer - -#if PERIODIC_TELEMETRY -#include "subsystems/datalink/telemetry.h" - -static void send_ahrs_ad2(struct transport_tx *trans, struct link_device *dev) -{ - pprz_msg_send_AHRS_ARDRONE2(trans, dev, AC_ID, - &ahrs_ardrone2.state, - &ahrs_ardrone2.control_state, - &ahrs_ardrone2.eulers.phi, - &ahrs_ardrone2.eulers.theta, - &ahrs_ardrone2.eulers.psi, - &ahrs_ardrone2.speed.x, - &ahrs_ardrone2.speed.y, - &ahrs_ardrone2.speed.z, - &ahrs_ardrone2.accel.x, - &ahrs_ardrone2.accel.y, - &ahrs_ardrone2.accel.z, - &ahrs_ardrone2.altitude, - &ahrs_ardrone2.battery); -} -#endif - -void ahrs_ardrone2_register(void) -{ - ahrs_ardrone2_init(); - /// @todo: provide enable function - ahrs_register_impl(NULL); -} - -void ahrs_ardrone2_init(void) -{ - init_at_com(); - - //Set navdata_demo to FALSE and flat trim the ar drone - at_com_send_config("general:navdata_demo", "FALSE"); - at_com_send_ftrim(); - - ahrs_ardrone2.is_aligned = TRUE; - -#if PERIODIC_TELEMETRY - register_periodic_telemetry(DefaultPeriodic, "AHRS_ARDRONE2", send_ahrs_ad2); -#endif -} - -#ifdef ARDRONE2_DEBUG -static void dump(const void *_b, size_t s) -{ - const unsigned char *b = _b; - size_t n; - - for (n = 0; n < s; ++n) { - printf("%02x ", b[n]); - if (n % 16 == 15) { - printf("\n"); - } - } - if (n % 16 != 0) { - printf("\n"); - } -} -#endif - -void ahrs_ardrone2_propagate(void) -{ - int l; - - //Recieve the main packet - l = at_com_recieve_navdata(buffer); - navdata_t *main_packet = (navdata_t *) &buffer; - -#ifdef ARDRONE2_DEBUG - if (l < 0) { - printf("errno = %d\n", errno); - } -#endif - - //When this isn't a valid packet return - if (l < 0 || main_packet->header != NAVDATA_HEADER) { - return; - } - -#ifdef ARDRONE2_DEBUG - printf("Read %d\n", l); - dump(buffer, l); -#endif - - //Set the state - ahrs_ardrone2.state = main_packet->ardrone_state; - - //Init the option - navdata_option_t *navdata_option = (navdata_option_t *) & (main_packet->options[0]); - bool_t full_read = FALSE; - - //The possible packets - navdata_demo_t *navdata_demo; - navdata_gps_t *navdata_gps; - navdata_phys_measures_t *navdata_phys_measures; - - //Read the navdata until packet is fully readed - while (!full_read && navdata_option->size > 0) { -#ifdef ARDRONE2_DEBUG - printf("tag = %d\n", navdata_option->tag); -#endif - //Check the tag for the right option - switch (navdata_option->tag) { - case 0: //NAVDATA_DEMO - navdata_demo = (navdata_demo_t *) navdata_option; - - //Set the AHRS state - ahrs_ardrone2.control_state = navdata_demo->ctrl_state >> 16; - ahrs_ardrone2.eulers.phi = navdata_demo->phi; - ahrs_ardrone2.eulers.theta = navdata_demo->theta; - ahrs_ardrone2.eulers.psi = navdata_demo->psi; - ahrs_ardrone2.speed.x = navdata_demo->vx / 1000; - ahrs_ardrone2.speed.y = navdata_demo->vy / 1000; - ahrs_ardrone2.speed.z = navdata_demo->vz / 1000; - ahrs_ardrone2.altitude = navdata_demo->altitude / 10; - ahrs_ardrone2.battery = navdata_demo->vbat_flying_percentage; - - //Set the ned to body eulers - struct FloatEulers angles; - angles.theta = navdata_demo->theta / 180000.*M_PI; - angles.psi = navdata_demo->psi / 180000.*M_PI; - angles.phi = navdata_demo->phi / 180000.*M_PI; - stateSetNedToBodyEulers_f(&angles); - - //Update the electrical supply - electrical.vsupply = navdata_demo->vbat_flying_percentage; - break; - case 3: //NAVDATA_PHYS_MEASURES - navdata_phys_measures = (navdata_phys_measures_t *) navdata_option; - - //Set the AHRS accel state - INT32_VECT3_SCALE_2(ahrs_ardrone2.accel, navdata_phys_measures->phys_accs, 9.81, 1000) - break; -#ifdef USE_GPS_ARDRONE2 - case 27: //NAVDATA_GPS -# ifdef ARDRONE2_DEBUG - dump(navdata_option, navdata_option->size); -# endif - navdata_gps = (navdata_gps_t *) navdata_option; - - // Send the data to the gps parser - gps_ardrone2_parse(navdata_gps); - break; -#endif - case 0xFFFF: //CHECKSUM - //TODO: Check the checksum - full_read = TRUE; - break; - default: -#ifdef ARDRONE2_DEBUG - printf("NAVDATA UNKNOWN TAG: %d %d\n", navdata_option->tag, navdata_option->size); -#endif - break; - } - navdata_option = (navdata_option_t *)((uint32_t)navdata_option + navdata_option->size); - } -} diff --git a/sw/airborne/subsystems/ahrs/ahrs_ardrone2.h b/sw/airborne/subsystems/ahrs/ahrs_ardrone2.h deleted file mode 100644 index ee1f668f6c..0000000000 --- a/sw/airborne/subsystems/ahrs/ahrs_ardrone2.h +++ /dev/null @@ -1,58 +0,0 @@ -/* - * Copyright (C) 2012-2013 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, write to - * the Free Software Foundation, 59 Temple Place - Suite 330, - * Boston, MA 02111-1307, USA. - */ - -/** - * @file subsystems/ahrs/ahrs_ardrone2.h - * AHRS implementation for ardrone2-sdk based on AT-commands. - * - * Uses AT-Commands to communicate with ardrone api to retrieve AHRS data - * and also sets battery level. - */ - -#ifndef AHRS_ARDRONE2_H -#define AHRS_ARDRONE2_H - -#include "std.h" -#include "math/pprz_algebra_int.h" -#include "math/pprz_geodetic_float.h" - -struct AhrsARDrone { - uint32_t state; // ARDRONE_STATES - uint32_t control_state; // CTRL_STATES - struct FloatEulers eulers; // in radians - struct NedCoor_f speed; // in m/s - struct NedCoor_f accel; // in m/s^2 - int32_t altitude; // in cm above ground - uint32_t battery; // in percentage - struct Int32Quat ltp_to_imu_quat; - bool_t is_aligned; -}; -extern struct AhrsARDrone ahrs_ardrone2; - -#ifndef PRIMARY_AHRS -#define PRIMARY_AHRS ahrs_ardrone2 -#endif - -extern void ahrs_ardrone2_register(void); -extern void ahrs_ardrone2_init(void); -extern void ahrs_ardrone2_propagate(void); - -#endif /* AHRS_ARDRONE2_H */ diff --git a/sw/airborne/subsystems/gps/gps_ardrone2.c b/sw/airborne/subsystems/gps/gps_ardrone2.c deleted file mode 100644 index 1eb5c76b50..0000000000 --- a/sw/airborne/subsystems/gps/gps_ardrone2.c +++ /dev/null @@ -1,86 +0,0 @@ -/* - * - * Copyright (C) 2013 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, write to - * the Free Software Foundation, 59 Temple Place - Suite 330, - * Boston, MA 02111-1307, USA. - * - */ - -/** - * @file subsystems/gps/gps_ardrone2.c - * ARdrone 2 gps trough navdata for the SDK version and only works in combination with the ahrs ardrone2. - */ - -#ifdef ARDRONE2_DEBUG -# include -#endif - -#include "subsystems/gps.h" -#include "subsystems/abi.h" -#include "math/pprz_geodetic_double.h" - -void gps_impl_init(void) -{ -} - -void gps_ardrone2_parse(navdata_gps_t *navdata_gps) -{ - int i; - -#ifdef ARDRONE2_DEBUG - printf("state = %d\n", navdata_gps->gps_state); -#endif - // Set the lla double struct from the navdata - struct LlaCoor_d gps_lla_d; - gps_lla_d.lat = RadOfDeg(navdata_gps->lat); - gps_lla_d.lon = RadOfDeg(navdata_gps->lon); - gps_lla_d.alt = navdata_gps->elevation; - - // Convert it to ecef - struct EcefCoor_d gps_ecef_d; - ecef_of_lla_d(&gps_ecef_d, &gps_lla_d); - - // Convert the lla and ecef to int and set them in gps - ECEF_BFP_OF_REAL(gps.ecef_pos, gps_ecef_d); - LLA_BFP_OF_REAL(gps.lla_pos, gps_lla_d); - - // TODO: parse other stuff - gps.nb_channels = GPS_NB_CHANNELS; - - for (i = 0; i < GPS_NB_CHANNELS; i++) { - gps.svinfos[i].svid = navdata_gps->channels[i].sat; - gps.svinfos[i].cno = navdata_gps->channels[i].cn0; - } - - // Check if we have a fix TODO: check if 2D or 3D fix? - if (navdata_gps->gps_state == 1) { - gps.fix = GPS_FIX_3D; - } else { - gps.fix = GPS_FIX_NONE; - } - - gps.last_msg_ticks = sys_time.nb_sec_rem; - gps.last_msg_time = sys_time.nb_sec; - if (gps.fix == GPS_FIX_3D) { - gps.last_3dfix_ticks = sys_time.nb_sec_rem; - gps.last_3dfix_time = sys_time.nb_sec; - } - - uint32_t now_ts = get_sys_time_usec(); - AbiSendMsgGPS(GPS_ARDRONE2_ID, now_ts, &gps); -} diff --git a/sw/airborne/subsystems/gps/gps_ardrone2.h b/sw/airborne/subsystems/gps/gps_ardrone2.h deleted file mode 100644 index 804ff136d4..0000000000 --- a/sw/airborne/subsystems/gps/gps_ardrone2.h +++ /dev/null @@ -1,46 +0,0 @@ -/* - * Copyright (C) 2013 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, write to - * the Free Software Foundation, 59 Temple Place - Suite 330, - * Boston, MA 02111-1307, USA. - * - */ - -/** @file subsystems/gps/gps_ardrone2.h - * ARdrone 2 gps trough navdata for the SDK version and only works in combination with the ahrs ardrone2. - * -*/ - -#ifndef GPS_ARDRONE_H -#define GPS_ARDRONE_H - -#include "boards/ardrone/at_com.h" - -#define GPS_NB_CHANNELS 12 - -/* - * The GPS event (dummy) - */ -#define GpsEvent() {} - -void gps_ardrone2_parse(navdata_gps_t *navdata_gps); - -/* Maybe needed? -#define gps_nmea_Reset(_val) { } -*/ - -#endif /* GPS_ARDRONE_H */ diff --git a/sw/airborne/subsystems/imu/imu_ardrone2_sdk.c b/sw/airborne/subsystems/imu/imu_ardrone2_sdk.c deleted file mode 100644 index 5437cf590b..0000000000 --- a/sw/airborne/subsystems/imu/imu_ardrone2_sdk.c +++ /dev/null @@ -1,39 +0,0 @@ -/* - * Copyright (C) 2013 Dino Hensen - * - * 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 subsystems/imu/imu_ardrone2_sdk.c - * dummy IMU implementation for ardrone2-sdk. - */ - -#include "subsystems/imu.h" -#include "imu_ardrone2_sdk.h" - - -void imu_impl_init(void) -{ - -} - -void imu_periodic(void) -{ - -} diff --git a/sw/airborne/subsystems/imu/imu_ardrone2_sdk.h b/sw/airborne/subsystems/imu/imu_ardrone2_sdk.h deleted file mode 100644 index 52a5be71c4..0000000000 --- a/sw/airborne/subsystems/imu/imu_ardrone2_sdk.h +++ /dev/null @@ -1,34 +0,0 @@ -/* - * Copyright (C) 2013 Dino Hensen - * - * 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 subsystems/imu/imu_ardrone2_sdk.h - * dummy IMU implementation for ardrone2-sdk. - */ - -#ifndef IMU_ARDRONE2_SDK_H_ -#define IMU_ARDRONE2_SDK_H_ - - -#define ImuEvent() {} - - -#endif /* IMU_ARDRONE2_SDK_H_ */ diff --git a/sw/airborne/subsystems/ins/ins_ardrone2.c b/sw/airborne/subsystems/ins/ins_ardrone2.c deleted file mode 100644 index 27209aaa54..0000000000 --- a/sw/airborne/subsystems/ins/ins_ardrone2.c +++ /dev/null @@ -1,176 +0,0 @@ -/* - * Copyright (C) 2012-2013 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, write to - * the Free Software Foundation, 59 Temple Place - Suite 330, - * Boston, MA 02111-1307, USA. - */ - -/** - * @file subsystems/ins/ins_ardrone2.c - * INS implementation for ardrone2-sdk. - */ - -#include "subsystems/ins/ins_ardrone2.h" -#include "subsystems/ahrs.h" -#include "subsystems/gps.h" -#include "generated/airframe.h" -#include "generated/flight_plan.h" -#include "math/pprz_geodetic_int.h" - -#ifdef SITL -#include "nps_fdm.h" -#include -#endif - -#ifndef USE_INS_NAV_INIT -#define USE_INS_NAV_INIT TRUE -PRINT_CONFIG_MSG("USE_INS_NAV_INIT defaulting to TRUE") -#endif - -struct InsArdrone2 ins_ardrone2; - -void ins_ardrone2_init(void) -{ -#if USE_INS_NAV_INIT - struct LlaCoor_i llh_nav0; /* Height above the ellipsoid */ - llh_nav0.lat = NAV_LAT0; - llh_nav0.lon = NAV_LON0; - /* NAV_ALT0 = ground alt above msl, NAV_MSL0 = geoid-height (msl) over ellipsoid */ - llh_nav0.alt = NAV_ALT0 + NAV_MSL0; - - struct EcefCoor_i ecef_nav0; - ecef_of_lla_i(&ecef_nav0, &llh_nav0); - - ltp_def_from_ecef_i(&ins_ardrone2.ltp_def, &ecef_nav0); - ins_ardrone2.ltp_def.hmsl = NAV_ALT0; - stateSetLocalOrigin_i(&ins_ardrone2.ltp_def); - - ins_ardrone2.ltp_initialized = TRUE; -#else - ins_ardrone2.ltp_initialized = FALSE; -#endif - - INT32_VECT3_ZERO(ins_ardrone2.ltp_pos); - INT32_VECT3_ZERO(ins_ardrone2.ltp_speed); - INT32_VECT3_ZERO(ins_ardrone2.ltp_accel); -} - -void ins_reset_local_origin(void) -{ -#if USE_GPS - if (gps.fix == GPS_FIX_3D) { - ltp_def_from_ecef_i(&ins_ardrone2.ltp_def, &gps.ecef_pos); - ins_ardrone2.ltp_def.lla.alt = gps.lla_pos.alt; - ins_ardrone2.ltp_def.hmsl = gps.hmsl; - ins_ardrone2.ltp_initialized = TRUE; - stateSetLocalOrigin_i(&ins_ardrone2.ltp_def); - } - else { - ins_ardrone2.ltp_initialized = FALSE; - } -#else - ins_ardrone2.ltp_initialized = FALSE; -#endif -} - -void ins_reset_altitude_ref(void) -{ -#if USE_GPS - struct LlaCoor_i lla = { - .lat = state.ned_origin_i.lla.lat, - .lon = state.ned_origin_i.lla.lon, - .alt = gps.lla_pos.alt - }; - ltp_def_from_lla_i(&ins_ardrone2.ltp_def, &lla); - ins_ardrone2.ltp_def.hmsl = gps.hmsl; - stateSetLocalOrigin_i(&ins_ardrone2.ltp_def); -#endif -} - -void ins_ardrone2_periodic(void) -{ - /* untilt accels and speeds */ - float_rmat_transp_vmult((struct FloatVect3 *)&ins_ardrone2.ltp_accel, - stateGetNedToBodyRMat_f(), - (struct FloatVect3 *)&ahrs_ardrone2.accel); - float_rmat_transp_vmult((struct FloatVect3 *)&ins_ardrone2.ltp_speed, - stateGetNedToBodyRMat_f(), - (struct FloatVect3 *)&ahrs_ardrone2.speed); - - //Add g to the accelerations - ins_ardrone2.ltp_accel.z += 9.81; - - //Save the accelerations and speeds - stateSetAccelNed_f(&ins_ardrone2.ltp_accel); - stateSetSpeedNed_f(&ins_ardrone2.ltp_speed); - - //Don't set the height if we use the one from the gps -#if !USE_GPS_HEIGHT - //Set the height and save the position - ins_ardrone2.ltp_pos.z = -(ahrs_ardrone2.altitude * INT32_POS_OF_CM_NUM) / INT32_POS_OF_CM_DEN; - stateSetPositionNed_i(&ins_ardrone2.ltp_pos); -#endif -} - - -void ins_ardrone2_update_gps(void) -{ -#if USE_GPS - //Check for GPS fix - if (gps.fix == GPS_FIX_3D) { - //Set the initial coordinates - if (!ins_ardrone2.ltp_initialized) { - ltp_def_from_ecef_i(&ins_ardrone2.ltp_def, &gps.ecef_pos); - ins_ardrone2.ltp_def.lla.alt = gps.lla_pos.alt; - ins_ardrone2.ltp_def.hmsl = gps.hmsl; - ins_ardrone2.ltp_initialized = TRUE; - stateSetLocalOrigin_i(&ins_ardrone2.ltp_def); - } - - //Set the x and y and maybe z position in ltp and save - struct NedCoor_i ins_gps_pos_cm_ned; - ned_of_ecef_point_i(&ins_gps_pos_cm_ned, &ins_ardrone2.ltp_def, &gps.ecef_pos); - - //When we don't want to use the height of the navdata we can use the gps height -#if USE_GPS_HEIGHT - INT32_VECT3_SCALE_2(ins_ardrone2.ltp_pos, ins_gps_pos_cm_ned, INT32_POS_OF_CM_NUM, INT32_POS_OF_CM_DEN); -#else - INT32_VECT2_SCALE_2(ins_ardrone2.ltp_pos, ins_gps_pos_cm_ned, INT32_POS_OF_CM_NUM, INT32_POS_OF_CM_DEN); -#endif - - //Set the local origin - stateSetPositionNed_i(&ins_ardrone2.ltp_pos); - } -#endif /* USE_GPS */ -} - -#include "subsystems/abi.h" -static abi_event gps_ev; -static void gps_cb(uint8_t sender_id __attribute__((unused)), - uint32_t stamp __attribute__((unused)), - struct GpsState *gps_s) -{ - ins_ardrone2_update_gps(); -} - -void ins_ardrone2_register(void) -{ - ins_register_impl(ins_ardrone2_init); - - AbiBindMsgGPS(ABI_BROADCAST, &gps_ev, gps_cb); - // FIXME: ins_ardrone2_periodic is currently called via InsPeriodic hack directly from main -} diff --git a/sw/airborne/subsystems/ins/ins_ardrone2.h b/sw/airborne/subsystems/ins/ins_ardrone2.h deleted file mode 100644 index 9c703d82d0..0000000000 --- a/sw/airborne/subsystems/ins/ins_ardrone2.h +++ /dev/null @@ -1,58 +0,0 @@ -/* - * Copyright (C) 2012-2013 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, write to - * the Free Software Foundation, 59 Temple Place - Suite 330, - * Boston, MA 02111-1307, USA. - */ - -/** - * @file subsystems/ins/ins_ardrone2.h - * INS implementation for ardrone2-sdk. - */ - -#ifndef INS_ARDRONE2_SDK_H -#define INS_ARDRONE2_SDK_H - -#include "subsystems/ins.h" -#include "std.h" -#include "math/pprz_geodetic_int.h" -#include "math/pprz_algebra_float.h" - -struct InsArdrone2 { - struct LtpDef_i ltp_def; - bool_t ltp_initialized; - - float qfe; ///< not used, only dummy for INS_REF message - - /* output LTP NED */ - struct NedCoor_i ltp_pos; - struct NedCoor_f ltp_speed; - struct NedCoor_f ltp_accel; -}; - -extern struct InsArdrone2 ins_ardrone2; - -#define DefaultInsImpl ins_ardrone2 -#define InsPeriodic ins_ardrone2_periodic - -extern void ins_ardrone2_init(void); -extern void ins_ardrone2_periodic(void); -extern void ins_ardrone2_update_gps(void); - -extern void ins_ardrone2_register(void); - -#endif /* INS_ARDRONE2_SDK_H */ diff --git a/sw/ext/bebop.py b/sw/ext/bebop.py index da9c398967..76b2a0a73c 100755 --- a/sw/ext/bebop.py +++ b/sw/ext/bebop.py @@ -185,7 +185,7 @@ def bebop_status(): read_from_config('motor1_hard', config_ini) + '\t' + read_from_config('motor2_hard', config_ini) + '\t' + read_from_config('motor3_hard', config_ini) + '\t' + read_from_config('motor4_hard', config_ini)) - autorun = {'': 'Native', '0': 'Native', '1': 'Paparazzi RAW', '2': 'Paparazzi SDK'} + autorun = {'': 'Native', '0': 'Native', '1': 'Paparazzi'} if check_autoboot(): print('Autorun at start:\tInstalled booting ' + autorun[read_from_config('start_paparazzi', config_ini)]) else: @@ -219,7 +219,7 @@ subparser_upload_gst = subparsers.add_parser('upload_gst_module', subparser_upload_gst.add_argument('file', help='Filename of *.so module') subparser_upload_and_run = subparsers.add_parser('upload_file_and_run', help='Upload and run software (for instance the Paparazzi autopilot)') subparser_upload_and_run.add_argument('file', help='Filename of an executable') -subparser_upload_and_run.add_argument('folder', help='Destination subfolder (raw or sdk for Paparazzi autopilot)') +subparser_upload_and_run.add_argument('folder', help='Destination subfolder (raw for Paparazzi autopilot)') subparser_upload = subparsers.add_parser('upload_file', help='Upload a file to the Bebop') subparser_upload.add_argument('file', help='Filename') subparser_upload.add_argument('folder', help='Destination subfolder (base destination folder is /data/video)') @@ -249,10 +249,10 @@ subparser_configure_network.add_argument('name', help='the new network ID(SSID)' subparser_configure_network.add_argument('address', help='the new IP address') subparser_configure_network.add_argument('mode', help='the new Wifi mode', choices=['master', 'ad-hoc', 'managed']) subparser_install_autostart = subparsers.add_parser('install_autostart', help='Install custom autostart script and set what to start on boot for the Bebop') -subparser_install_autostart.add_argument('type', choices=['native', 'paparazzi_raw', 'paparazzi_sdk'], +subparser_install_autostart.add_argument('type', choices=['native', 'paparazzi_raw'], help='what to start on boot') subparser_autostart = subparsers.add_parser('autostart', help='Set what to start on boot for the Bebop') -subparser_autostart.add_argument('type', choices=['native', 'paparazzi_raw', 'paparazzi_sdk'], +subparser_autostart.add_argument('type', choices=['native', 'paparazzi_raw'], help='what to start on boot') args = parser.parse_args() @@ -347,7 +347,7 @@ elif args.command == 'install_autostart': bebop_install_autoboot() else: bebop_install_autoboot() - autorun = {'native': '0', 'paparazzi_raw': '1', 'paparazzi_sdk': '2'} + autorun = {'native': '0', 'paparazzi_raw': '1'} write_to_config('start_paparazzi', autorun[args.type]) print('The autostart on boot is changed to ' + args.type) @@ -356,7 +356,7 @@ elif args.command == 'install_autostart': # Change the autostart elif args.command == 'autostart': - autorun = {'native': '0', 'paparazzi_raw': '1', 'paparazzi_sdk': '2'} + autorun = {'native': '0', 'paparazzi_raw': '1'} write_to_config('start_paparazzi', autorun[args.type]) print('The autostart on boot is changed to ' + args.type) diff --git a/sw/tools/parrot/ardrone2.py b/sw/tools/parrot/ardrone2.py index ed7dd482aa..45a929ca28 100755 --- a/sw/tools/parrot/ardrone2.py +++ b/sw/tools/parrot/ardrone2.py @@ -173,7 +173,7 @@ def ardrone2_status(): read_from_config('motor1_hard', config_ini) + '\t' + read_from_config('motor2_hard', config_ini) + '\t' + read_from_config('motor3_hard', config_ini) + '\t' + read_from_config('motor4_hard', config_ini)) - autorun = {'': 'Native', '0': 'Native', '1': 'Paparazzi RAW', '2': 'Paparazzi SDK'} + autorun = {'': 'Native', '0': 'Native', '1': 'Paparazzi'} if check_autoboot(): print('Autorun at start:\tInstalled booting ' + autorun[read_from_config('start_paparazzi', config_ini)]) else: @@ -207,7 +207,7 @@ subparser_upload_gst = subparsers.add_parser('upload_gst_module', subparser_upload_gst.add_argument('file', help='Filename of *.so module') subparser_upload_and_run = subparsers.add_parser('upload_file_and_run', help='Upload and run software (for instance the Paparazzi autopilot)') subparser_upload_and_run.add_argument('file', help='Filename of an executable') -subparser_upload_and_run.add_argument('folder', help='Destination subfolder (raw or sdk for Paparazzi autopilot)') +subparser_upload_and_run.add_argument('folder', help='Destination subfolder (raw for Paparazzi autopilot)') subparser_upload = subparsers.add_parser('upload_file', help='Upload a file to the ARDrone 2') subparser_upload.add_argument('file', help='Filename') subparser_upload.add_argument('folder', help='Destination subfolder (base destination folder is /data/video)') @@ -238,10 +238,10 @@ subparser_configure_network.add_argument('address', help='the new IP address') subparser_configure_network.add_argument('mode', help='the new Wifi mode', choices=['master', 'ad-hoc', 'managed', 'ad-hoc-olsr']) subparser_configure_network.add_argument('--channel', help='the wifi channel (auto or 1 to 11)', default='auto') subparser_install_autostart = subparsers.add_parser('install_autostart', help='Install custom autostart script and set what to start on boot for the ARDrone 2') -subparser_install_autostart.add_argument('type', choices=['native', 'paparazzi_raw', 'paparazzi_sdk'], +subparser_install_autostart.add_argument('type', choices=['native', 'paparazzi_raw'], help='what to start on boot') subparser_autostart = subparsers.add_parser('autostart', help='Set what to start on boot for the ARDrone 2') -subparser_autostart.add_argument('type', choices=['native', 'paparazzi_raw', 'paparazzi_sdk'], +subparser_autostart.add_argument('type', choices=['native', 'paparazzi_raw'], help='what to start on boot') args = parser.parse_args() @@ -285,7 +285,7 @@ elif args.command == 'ipaddress': # Change the wifi mode elif args.command == 'wifimode': ardrone2_set_wifi_mode(args.mode) - + if raw_input("Shall I restart the ARDrone 2? (y/N) ").lower() == 'y': parrot_utils.reboot(tn) @@ -332,7 +332,7 @@ elif args.command == 'install_autostart': ardrone2_install_autoboot() else: ardrone2_install_autoboot() - autorun = {'native': '0', 'paparazzi_raw': '1', 'paparazzi_sdk': '2'} + autorun = {'native': '0', 'paparazzi_raw': '1'} write_to_config('start_paparazzi', autorun[args.type]) print('The autostart on boot is changed to ' + args.type) @@ -341,7 +341,7 @@ elif args.command == 'install_autostart': # Change the autostart elif args.command == 'autostart': - autorun = {'native': '0', 'paparazzi_raw': '1', 'paparazzi_sdk': '2'} + autorun = {'native': '0', 'paparazzi_raw': '1'} write_to_config('start_paparazzi', autorun[args.type]) print('The autostart on boot is changed to ' + args.type) diff --git a/sw/tools/parrot/ardrone2/check_update.sh b/sw/tools/parrot/ardrone2/check_update.sh index 3fee525f48..70117d9f8d 100755 --- a/sw/tools/parrot/ardrone2/check_update.sh +++ b/sw/tools/parrot/ardrone2/check_update.sh @@ -11,9 +11,6 @@ case $START_PAPARAZZI in 1) START_PAPARAZZI=raw ;; -2) - START_PAPARAZZI=sdk - ;; *) START_PAPARAZZI=no ;; @@ -28,7 +25,7 @@ PELF_ARGS=$(cat /tmp/.program.elf.arguments | tr '\n' ' ') echo "Check if update is necessary ..." if [ -e $UPDATE_PATH ] ; then VERSION=`cat $VERSION_PATH` - + if [ -e $ERR_PATH ] ; then CHECK_ERR=`cat $ERR_PATH` if [ "$CHECK_ERR" = "NEED_TO_FLASH" ] ; then @@ -36,17 +33,17 @@ if [ -e $UPDATE_PATH ] ; then if [ "$CHECK_PLF" = "NEED_TO_FLASH" ] ; then echo "ERR=FLASH_KO" > $ERR_PATH else - /bin/checkplf $UPDATE_PATH $VERSION > $ERR_PATH + /bin/checkplf $UPDATE_PATH $VERSION > $ERR_PATH fi else - /bin/checkplf $UPDATE_PATH $VERSION > $ERR_PATH + /bin/checkplf $UPDATE_PATH $VERSION > $ERR_PATH fi else /bin/checkplf $UPDATE_PATH $VERSION > $ERR_PATH fi - + CHECK_ERR=`cat $ERR_PATH` - if [ "$CHECK_ERR" = "NEED_TO_FLASH" ] ; then + if [ "$CHECK_ERR" = "NEED_TO_FLASH" ] ; then echo "File $UPDATE_PATH exists... Start updating..." pinst_trigger echo "Rebooting..." @@ -64,12 +61,10 @@ if [ -e $UPDATE_PATH ] ; then cp /firmware/version.txt $VERSION_PATH echo "Start Drone software..." inetd - + # Check what to start if [ "$START_PAPARAZZI" = "raw" ] ; then (/data/video/raw/ap.elf; gpio 181 -d ho 1) & - elif [ "$START_PAPARAZZI" = "sdk" ] ; then - (/data/video/sdk/ap.elf; gpio 181 -d ho 1) & else (/bin/program.elf ${PELF_ARGS}; gpio 181 -d ho 1) & fi @@ -86,10 +81,6 @@ else (/bin/program.elf ${PELF_ARGS}; gpio 181 -d ho 1) & sleep 10 (/data/video/raw/ap.elf; gpio 181 -d ho 1) & - elif [ "$START_PAPARAZZI" = "sdk" ] ; then - (/bin/program.elf ${PELF_ARGS}; gpio 181 -d ho 1) & - sleep 10 - (/data/video/sdk/ap.elf; gpio 181 -d ho 1) & else (/bin/program.elf ${PELF_ARGS}; gpio 181 -d ho 1) & fi diff --git a/sw/tools/parrot/bebop.py b/sw/tools/parrot/bebop.py index 2f7c1555aa..13923e32f7 100755 --- a/sw/tools/parrot/bebop.py +++ b/sw/tools/parrot/bebop.py @@ -67,7 +67,7 @@ subparsers.add_parser('status', help='Request the status of the Bebop') subparsers.add_parser('reboot', help='Reboot the Bebop') subparser_upload_and_run = subparsers.add_parser('upload_file_and_run', help='Upload and run software (for instance the Paparazzi autopilot)') subparser_upload_and_run.add_argument('file', help='Filename of an executable') -subparser_upload_and_run.add_argument('folder', help='Destination subfolder (raw or sdk for Paparazzi autopilot)') +subparser_upload_and_run.add_argument('folder', help='Destination subfolder (raw for Paparazzi autopilot)') subparser_upload = subparsers.add_parser('upload_file', help='Upload a file to the Bebop') subparser_upload.add_argument('file', help='Filename') subparser_upload.add_argument('folder', help='Destination subfolder (base destination folder is /data/ftp)')