diff --git a/conf/Makefile.swing b/conf/Makefile.swing new file mode 100644 index 0000000000..8d4824c06e --- /dev/null +++ b/conf/Makefile.swing @@ -0,0 +1,37 @@ +# Hey Emacs, this is a -*- makefile -*- +# +# Copyright (C) 2014 Freek van Tienen +# Copyright (C) 2017 Gautier Hattenberger +# +# 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 +# . +# + +# +# This Makefile uses the generic Makefile.arm-linux and adds upload rules for the Parrot Swing +# + +include $(PAPARAZZI_SRC)/conf/Makefile.arm-linux + +DRONE = $(PAPARAZZI_SRC)/sw/tools/parrot/swing.py + +# Program the device and start it. +upload program: $(OBJDIR)/$(TARGET).elf + $(Q)$(PREFIX)-strip $(OBJDIR)/$(TARGET).elf + $(Q)$(DRONE) --host=$(HOST) upload_file_and_run $(OBJDIR)/$(TARGET).elf $(SUB_DIR) + +# Listing of phony targets. +.PHONY : upload program diff --git a/conf/airframes/examples/swing.xml b/conf/airframes/examples/swing.xml new file mode 100644 index 0000000000..f60000d072 --- /dev/null +++ b/conf/airframes/examples/swing.xml @@ -0,0 +1,214 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
+ + + + + + + + + + + + +
+ + + + + + + + + +
+ + + +
+ +
+ + + + + + + + + + + + + + + + +
+ + + +
+ + + + + + +
+ +
+ + +
+ +
+ + + + + + + + + + + + + + + + + + + + + + +
+ +
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
+ +
+ + + + + +
+ +
+ + + + + + + +
+ +
+ + + +
+ +
+ + + + +
+ +
+ + + + +
+ diff --git a/conf/boards/swing.makefile b/conf/boards/swing.makefile new file mode 100644 index 0000000000..56cbb246e9 --- /dev/null +++ b/conf/boards/swing.makefile @@ -0,0 +1,51 @@ +# Hey Emacs, this is a -*- makefile -*- +# +# swing.makefile +# +# http://wiki.paparazziuav.org/wiki/Swing +# + +BOARD=swing +BOARD_CFG=\"boards/$(BOARD).h\" + +ARCH=linux +$(TARGET).ARCHDIR = $(ARCH) +# include conf/Makefile.swing (with specific upload rules) instead of only Makefile.linux: +ap.MAKEFILE = swing + +FLOAT_ABI = +ARCH_CFLAGS = -march=armv5 + +# ----------------------------------------------------------------------- +USER=foobar +HOST?=192.168.4.1 +SUB_DIR=paparazzi +FTP_DIR=/data/edu +TARGET_DIR=$(FTP_DIR)/$(SUB_DIR) +# ----------------------------------------------------------------------- + +# The datalink default uses UDP +MODEM_HOST ?= 192.168.4.255 + +# The GPS sensor is connected internally +GPS_PORT ?= UART1 +GPS_BAUD ?= B230400 + +# handle linux signals by hand +$(TARGET).CFLAGS += -DUSE_LINUX_SIGNAL -D_GNU_SOURCE + +# board specific init function +$(TARGET).srcs += $(SRC_BOARD)/board.c + +# Link static (Done for GLIBC) +$(TARGET).CFLAGS += -DLINUX_LINK_STATIC +$(TARGET).LDFLAGS += -static + +# ----------------------------------------------------------------------- + +# default LED configuration +RADIO_CONTROL_LED ?= none +BARO_LED ?= none +AHRS_ALIGNER_LED ?= none +GPS_LED ?= none +SYS_TIME_LED ?= 0 diff --git a/conf/firmwares/subsystems/shared/baro_board.makefile b/conf/firmwares/subsystems/shared/baro_board.makefile index 6e4cb1611e..82258aad6e 100644 --- a/conf/firmwares/subsystems/shared/baro_board.makefile +++ b/conf/firmwares/subsystems/shared/baro_board.makefile @@ -47,6 +47,10 @@ else ifeq ($(BOARD), disco) BARO_BOARD_SRCS += peripherals/ms5611_i2c.c BARO_BOARD_SRCS += boards/baro_board_ms5611_i2c.c +# Swing baro +else ifeq ($(BOARD), swing) + BARO_BOARD_SRCS += $(SRC_BOARD)/baro_board.c + # Lisa/M baro else ifeq ($(BOARD), lisa_m) ifeq ($(BOARD_VERSION), 1.0) diff --git a/conf/modules/actuators_swing.xml b/conf/modules/actuators_swing.xml new file mode 100644 index 0000000000..93f4c889f5 --- /dev/null +++ b/conf/modules/actuators_swing.xml @@ -0,0 +1,17 @@ + + + + + + Actuators Driver for Swing + + +
+ +
+ + + + + + diff --git a/conf/modules/imu_swing.xml b/conf/modules/imu_swing.xml new file mode 100644 index 0000000000..856cbc30a9 --- /dev/null +++ b/conf/modules/imu_swing.xml @@ -0,0 +1,26 @@ + + + + + + Driver for IMU on the Parrot Swing drone. + - Accelerometer/Gyroscope: MPU6000 via I2C0 + + + + + +
+ +
+ + + + + + + + + + + diff --git a/sw/airborne/boards/swing.h b/sw/airborne/boards/swing.h new file mode 100644 index 0000000000..f1982df995 --- /dev/null +++ b/sw/airborne/boards/swing.h @@ -0,0 +1,64 @@ +/* + * Copyright (C) 2014 Freek van Tienen + * Copyright (C) 2017 Gautier Hattenberger + * + * 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. + * + */ + +#ifndef CONFIG_SWING +#define CONFIG_SWING + +#define BOARD_SWING + +#include "std.h" + +/** uart connected to GPS internally */ +#define UART1_DEV /dev/ttyPA1 +#define GPS_UBX_ENABLE_NMEA_DATA_MASK 0xff +/** FTDI cable for stereoboard or external GPS */ +#define UART2_DEV /dev/ttyUSB0 + +/* Default actuators driver */ +#define DEFAULT_ACTUATORS "boards/swing/actuators.h" +#define ActuatorDefaultSet(_x,_y) ActuatorsSwingSet(_x,_y) +#define ActuatorsDefaultInit() ActuatorsSwingInit() +#define ActuatorsDefaultCommit() ActuatorsSwingCommit() + +/* by default activate onboard baro */ +#ifndef USE_BARO_BOARD +#define USE_BARO_BOARD 1 +#endif + +/* The ADC from the sonar */ +#if USE_ADC0 +#define ADC0_ID 0 +#define ADC0_CHANNELS 2 +#define ADC0_CHANNELS_CNT 1 +#define ADC0_BUF_LENGTH 8192 +#endif + +/* The SPI from the sonar */ +#if USE_SPI0 +#define SPI0_MODE 0 +#define SPI0_BITS_PER_WORD 8 +#define SPI0_MAX_SPEED_HZ 320000 +#endif + +#endif /* CONFIG_SWING */ + diff --git a/sw/airborne/boards/swing/actuators.c b/sw/airborne/boards/swing/actuators.c new file mode 100644 index 0000000000..806657559d --- /dev/null +++ b/sw/airborne/boards/swing/actuators.c @@ -0,0 +1,126 @@ +/* + * Copyright (C) 2014 Freek van Tienen + * Copyright (C) 2017 Gautier Hattenberger + * + * 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/swing/actuators.c + * Actuator driver for the swing + */ + + +#include +#include + +/* build ioctl unique identifiers for R/W operations */ +#define PWM_MAGIC 'p' +typedef struct { unsigned int val[4]; } __attribute__ ((packed)) pwm_delos_quadruplet; +#define PWM_DELOS_SET_RATIOS _IOR(PWM_MAGIC, 9, pwm_delos_quadruplet*) +#define PWM_DELOS_SET_SPEEDS _IOR(PWM_MAGIC, 10, pwm_delos_quadruplet*) +#define PWM_DELOS_SET_CTRL _IOR(PWM_MAGIC, 11, unsigned int) +#define PWM_DELOS_REQUEST _IO(PWM_MAGIC, 12) + +#define PWM_NB_BITS (9) + +/* PWM can take value between 0 and 511 */ +#ifndef PWM_TOTAL_RANGE +#define PWM_TOTAL_RANGE (1< (PWM_TOTAL_RANGE) ) { m.val[0] = PWM_REG_SATURATION; } + if( actuators_swing.rpm_ref[1] > (PWM_TOTAL_RANGE) ) { m.val[1] = PWM_REG_SATURATION; } + if( actuators_swing.rpm_ref[2] > (PWM_TOTAL_RANGE) ) { m.val[2] = PWM_REG_SATURATION; } + if( actuators_swing.rpm_ref[3] > (PWM_TOTAL_RANGE) ) { m.val[3] = PWM_REG_SATURATION; } + + if( actuators_swing.rpm_ref[0] < 0 ) { m.val[0] = 0; } + if( actuators_swing.rpm_ref[1] < 0 ) { m.val[1] = 0; } + if( actuators_swing.rpm_ref[2] < 0 ) { m.val[2] = 0; } + if( actuators_swing.rpm_ref[3] < 0 ) { m.val[3] = 0; } + + /* The upper 16-bit word of the ratio register contains the number + * of bits used to code the ratio command */ + m.val[0] |= PWM_REG_RATIO_PRECISION_MASK; + m.val[1] |= PWM_REG_RATIO_PRECISION_MASK; + m.val[2] |= PWM_REG_RATIO_PRECISION_MASK; + m.val[3] |= PWM_REG_RATIO_PRECISION_MASK; + + int ret __attribute__((unused)) = ioctl(actuators_fd, PWM_DELOS_SET_RATIOS, &m); + +#if ACTUATORS_SWING_DEBUG + RunOnceEvery(512, printf("Return ratios: %d (ratios: %d %d %d %d, pwm: %d %d %d %d\n", + ret, + m.val[0], m.val[1], m.val[2], m.val[3], + actuators_swing.rpm_ref[0], + actuators_swing.rpm_ref[1], + actuators_swing.rpm_ref[2], + actuators_swing.rpm_ref[3]) + ); +#endif +} + diff --git a/sw/airborne/boards/swing/actuators.h b/sw/airborne/boards/swing/actuators.h new file mode 100644 index 0000000000..95b4075209 --- /dev/null +++ b/sw/airborne/boards/swing/actuators.h @@ -0,0 +1,46 @@ +/* + * Copyright (C) 2014 Freek van Tienen + * Copyright (C) 2017 Gautier Hattenberger + * + * 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/swing/actuators.h + * Actuator driver for the swing + */ + +#ifndef ACTUATORS_SWING_H_ +#define ACTUATORS_SWING_H_ + +#include + +struct ActuatorsSwing { + uint16_t rpm_ref[4]; ///< Reference RPM +}; + +#define ActuatorsSwingSet(_i, _v) { actuators_swing.rpm_ref[_i] = _v; } +#define ActuatorsSwingCommit() actuators_swing_commit(); +#define ActuatorsSwingInit() actuators_swing_init(); + +extern struct ActuatorsSwing actuators_swing; +extern void actuators_swing_commit(void); +extern void actuators_swing_init(void); + +#endif /* ACTUATORS_SWING_H_ */ + diff --git a/sw/airborne/boards/swing/baro_board.h b/sw/airborne/boards/swing/baro_board.h new file mode 100644 index 0000000000..3d8def3064 --- /dev/null +++ b/sw/airborne/boards/swing/baro_board.h @@ -0,0 +1,38 @@ +/* + * Copyright (C) 2017 Gautier Hattenberger + * + * 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 boards/swing/baro_board.h + * Paparazzi Swing Baro Sensor implementation. + * Sensor is LPS22HB (I2C) from ST but is accessed through sysfs interface + */ + +#ifndef BOARDS_SWING_BARO_H +#define BOARDS_SWING_BARO_H + +// only for printing the baro type during compilation +#ifndef BARO_BOARD +#define BARO_BOARD BARO_SWING +#endif + +extern void baro_event(void); +#define BaroEvent baro_event + +#endif /* BOARDS_SWING_BARO_H */ diff --git a/sw/airborne/boards/swing/board.c b/sw/airborne/boards/swing/board.c new file mode 100644 index 0000000000..1e982ad8fd --- /dev/null +++ b/sw/airborne/boards/swing/board.c @@ -0,0 +1,131 @@ +/* + * Copyright (C) 2017 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, see + * . + */ + +/** + * @file boards/swing/board.c + * + * Swing specific board initialization function. + * + */ + +#include "boards/swing.h" +#include "mcu.h" +#include +#include +#include +#include +#include +#include +#include +#include "subsystems/electrical.h" + +/** + * Battery reading thread + */ +static void *bat_read(void *data __attribute__((unused))) +{ + FILE *fp; + char path[16]; + + while (TRUE) { + /* Open the command for reading. */ + fp = popen("cat /sys/devices/platform/p6-spi.2/spi2.0/vbat", "r"); + if (fp == NULL) { + printf("Failed to read battery\n" ); + } + else { + /* Read the output a line at a time - output it. */ + while (fgets(path, sizeof(path)-1, fp) != NULL) { + int raw_bat = atoi(path); + // convert to decivolt + // from /bin/mcu_vbat.sh: MILLIVOLTS_VALUE=$(( ($RAW_VALUE * 4250) / 1023 )) + electrical.vsupply = ((raw_bat * 4250) / 1023) / 100; + } + /* close */ + pclose(fp); + } + + // Wait 100ms + // reading is done at 10Hz like the electrical_periodic from rotorcraft main program + usleep(100000); + } + + return NULL; +} + +/** + * Check button thread + */ +static void *button_read(void *data __attribute__((unused))) +{ + struct input_event ev; + ssize_t n; + + /* Open power button event sysfs file */ + int fd_button = open("/dev/input/pm_mcu_event", O_RDONLY); + if (fd_button == -1) { + printf("Unable to open mcu_event to read power button state\n"); + return NULL; + } + + while (TRUE) { + /* Check power button (read is blocking) */ + n = read(fd_button, &ev, sizeof(ev)); + if (n == sizeof(ev) && ev.type == EV_KEY && ev.code == KEY_POWER && ev.value > 0) { + printf("Stopping Paparazzi from power button and rebooting\n"); + usleep(1000); + int ret __attribute__((unused)) = system("reboot.sh"); + exit(0); + } + } + + return NULL; +} + +void board_init(void) +{ + /* + * Stop original processes using pstop/ptart commands + * Don't kill to avoid automatic restart + * + */ + int ret __attribute__((unused)); + ret = system("pstop delosd"); + ret = system("pstop dragon-prog"); + usleep(50000); /* Give 50ms time to end on a busy system */ + + /* Start bat reading thread */ + pthread_t bat_thread; + if (pthread_create(&bat_thread, NULL, bat_read, NULL) != 0) { + printf("[swing_board] Could not create battery reading thread!\n"); + } + + /* Start button reading thread */ + pthread_t button_thread; + if (pthread_create(&button_thread, NULL, button_read, NULL) != 0) { + printf("[swing_board] Could not create button reading thread!\n"); + } + +} + +void board_init2(void) +{ +} + diff --git a/sw/airborne/subsystems/imu/imu_swing.c b/sw/airborne/subsystems/imu/imu_swing.c new file mode 100644 index 0000000000..d35dcc1b72 --- /dev/null +++ b/sw/airborne/subsystems/imu/imu_swing.c @@ -0,0 +1,118 @@ +/* + * Copyright (C) 2014 Freek van Tienen + * Copyright (C) 2017 Gautier Hattenberger + * + * 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_swing.c + * Driver for the Swing accelerometer and gyroscope + */ + +#include "subsystems/imu.h" +#include "subsystems/abi.h" +#include "mcu_periph/i2c.h" + + +/* defaults suitable for Swing */ +#ifndef SWING_MPU_I2C_DEV +#define SWING_MPU_I2C_DEV i2c0 +#endif +PRINT_CONFIG_VAR(SWING_MPU_I2C_DEV) + +#if !defined SWING_LOWPASS_FILTER && !defined SWING_SMPLRT_DIV +#if (PERIODIC_FREQUENCY == 60) || (PERIODIC_FREQUENCY == 120) +/* Accelerometer: Bandwidth 44Hz, Delay 4.9ms + * Gyroscope: Bandwidth 42Hz, Delay 4.8ms sampling 1kHz + */ +#define SWING_LOWPASS_FILTER MPU60X0_DLPF_42HZ +#define SWING_SMPLRT_DIV 9 +PRINT_CONFIG_MSG("Gyro/Accel output rate is 100Hz at 1kHz internal sampling") +#elif PERIODIC_FREQUENCY == 512 +/* Accelerometer: Bandwidth 260Hz, Delay 0ms + * Gyroscope: Bandwidth 256Hz, Delay 0.98ms sampling 8kHz + */ +#define SWING_LOWPASS_FILTER MPU60X0_DLPF_256HZ +#define SWING_SMPLRT_DIV 3 +PRINT_CONFIG_MSG("Gyro/Accel output rate is 2kHz at 8kHz internal sampling") +#endif +#endif +PRINT_CONFIG_VAR(SWING_SMPLRT_DIV) +PRINT_CONFIG_VAR(SWING_LOWPASS_FILTER) + +PRINT_CONFIG_VAR(SWING_GYRO_RANGE) +PRINT_CONFIG_VAR(SWING_ACCEL_RANGE) + +/** Basic Navstik IMU data */ +struct ImuSwing imu_swing; + +/** + * Navstik IMU initializtion of the MPU-60x0 and HMC58xx + */ +void imu_swing_init(void) +{ + /* MPU-60X0 */ + mpu60x0_i2c_init(&imu_swing.mpu, &(SWING_MPU_I2C_DEV), MPU60X0_ADDR); + imu_swing.mpu.config.smplrt_div = SWING_SMPLRT_DIV; + imu_swing.mpu.config.dlpf_cfg = SWING_LOWPASS_FILTER; + imu_swing.mpu.config.gyro_range = SWING_GYRO_RANGE; + imu_swing.mpu.config.accel_range = SWING_ACCEL_RANGE; +} + +/** + * Handle all the periodic tasks of the Navstik IMU components. + * Read the MPU60x0 every periodic call + */ +void imu_swing_periodic(void) +{ + // Start reading the latest gyroscope data + mpu60x0_i2c_periodic(&imu_swing.mpu); +} + +/** + * Handle all the events of the Navstik IMU components. + * When there is data available convert it to the correct axis and save it in the imu structure. + */ +void imu_swing_event(void) +{ + uint32_t now_ts = get_sys_time_usec(); + + /* MPU-60x0 event taks */ + mpu60x0_i2c_event(&imu_swing.mpu); + + if (imu_swing.mpu.data_available) { + /* default orientation of the MPU is upside down and in plane mode + * turn it to have rotorcraft mode by default */ + RATES_ASSIGN(imu.gyro_unscaled, + -imu_swing.mpu.data_rates.rates.r, + -imu_swing.mpu.data_rates.rates.q, + -imu_swing.mpu.data_rates.rates.p); + VECT3_ASSIGN(imu.accel_unscaled, + -imu_swing.mpu.data_accel.vect.z, + -imu_swing.mpu.data_accel.vect.y, + -imu_swing.mpu.data_accel.vect.x); + + imu_swing.mpu.data_available = false; + imu_scale_gyro(&imu); + imu_scale_accel(&imu); + AbiSendMsgIMU_GYRO_INT32(IMU_BOARD_ID, now_ts, &imu.gyro); + AbiSendMsgIMU_ACCEL_INT32(IMU_BOARD_ID, now_ts, &imu.accel); + } +} + diff --git a/sw/airborne/subsystems/imu/imu_swing.h b/sw/airborne/subsystems/imu/imu_swing.h new file mode 100644 index 0000000000..6e948708f2 --- /dev/null +++ b/sw/airborne/subsystems/imu/imu_swing.h @@ -0,0 +1,83 @@ +/* + * Copyright (C) 2014 Freek van Tienen + * Copyright (C) 2017 Gautier Hattenberger + * + * 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_swing.h + * Interface for the Swing accelerometer and gyroscope + */ + + +#ifndef IMU_SWING_H +#define IMU_SWING_H + +#include "generated/airframe.h" +#include "subsystems/imu.h" + +#include "peripherals/mpu60x0_i2c.h" + +#ifndef SWING_GYRO_RANGE +#define SWING_GYRO_RANGE MPU60X0_GYRO_RANGE_1000 +#endif + +#ifndef SWING_ACCEL_RANGE +#define SWING_ACCEL_RANGE MPU60X0_ACCEL_RANGE_8G +#endif + +// Set default sensitivity based on range if needed +#if !defined IMU_GYRO_P_SENS & !defined IMU_GYRO_Q_SENS & !defined IMU_GYRO_R_SENS +#define IMU_GYRO_P_SENS MPU60X0_GYRO_SENS[SWING_GYRO_RANGE] +#define IMU_GYRO_P_SENS_NUM MPU60X0_GYRO_SENS_FRAC[SWING_GYRO_RANGE][0] +#define IMU_GYRO_P_SENS_DEN MPU60X0_GYRO_SENS_FRAC[SWING_GYRO_RANGE][1] +#define IMU_GYRO_Q_SENS MPU60X0_GYRO_SENS[SWING_GYRO_RANGE] +#define IMU_GYRO_Q_SENS_NUM MPU60X0_GYRO_SENS_FRAC[SWING_GYRO_RANGE][0] +#define IMU_GYRO_Q_SENS_DEN MPU60X0_GYRO_SENS_FRAC[SWING_GYRO_RANGE][1] +#define IMU_GYRO_R_SENS MPU60X0_GYRO_SENS[SWING_GYRO_RANGE] +#define IMU_GYRO_R_SENS_NUM MPU60X0_GYRO_SENS_FRAC[SWING_GYRO_RANGE][0] +#define IMU_GYRO_R_SENS_DEN MPU60X0_GYRO_SENS_FRAC[SWING_GYRO_RANGE][1] +#endif + +// Set default sensitivity based on range if needed +#if !defined IMU_ACCEL_X_SENS & !defined IMU_ACCEL_Y_SENS & !defined IMU_ACCEL_Z_SENS +#define IMU_ACCEL_X_SENS MPU60X0_ACCEL_SENS[SWING_ACCEL_RANGE] +#define IMU_ACCEL_X_SENS_NUM MPU60X0_ACCEL_SENS_FRAC[SWING_ACCEL_RANGE][0] +#define IMU_ACCEL_X_SENS_DEN MPU60X0_ACCEL_SENS_FRAC[SWING_ACCEL_RANGE][1] +#define IMU_ACCEL_Y_SENS MPU60X0_ACCEL_SENS[SWING_ACCEL_RANGE] +#define IMU_ACCEL_Y_SENS_NUM MPU60X0_ACCEL_SENS_FRAC[SWING_ACCEL_RANGE][0] +#define IMU_ACCEL_Y_SENS_DEN MPU60X0_ACCEL_SENS_FRAC[SWING_ACCEL_RANGE][1] +#define IMU_ACCEL_Z_SENS MPU60X0_ACCEL_SENS[SWING_ACCEL_RANGE] +#define IMU_ACCEL_Z_SENS_NUM MPU60X0_ACCEL_SENS_FRAC[SWING_ACCEL_RANGE][0] +#define IMU_ACCEL_Z_SENS_DEN MPU60X0_ACCEL_SENS_FRAC[SWING_ACCEL_RANGE][1] +#endif + + +/** Everything that is in the swing IMU */ +struct ImuSwing { + struct Mpu60x0_I2c mpu; ///< The MPU gyro/accel device +}; + +extern struct ImuSwing imu_swing; + +extern void imu_swing_init(void); +extern void imu_swing_periodic(void); +extern void imu_swing_event(void); + +#endif /* IMU_SWING_H */ diff --git a/sw/tools/parrot/ardrone2.py b/sw/tools/parrot/ardrone2.py index ecc0d22b53..2c091b5ff3 100755 --- a/sw/tools/parrot/ardrone2.py +++ b/sw/tools/parrot/ardrone2.py @@ -1,6 +1,8 @@ #!/usr/bin/env python # # Copyright (C) 2012-2014 The Paparazzi Team +# 2015 Freek van Tienen +# 2017 Gautier Hattenberger # # This file is part of Paparazzi. # @@ -20,456 +22,327 @@ # from __future__ import print_function +from parrot_utils import ParrotUtils import re import argparse -import os from time import sleep -import parrot_utils +class Ardrone2(ParrotUtils): + uav_name = 'Ardrone 2' + config_file = '/data/config.ini' + version_file = '/firmware/version.txt' -# Read from config.ini -def read_from_config(name, config=''): - if config == '': - config = parrot_utils.execute_command(tn, 'cat /data/config.ini') - search = re.search(name + '[^=]+=[\r\n\t ]([^\r\n\t ]+)', config) - if search is None: - return '' - else: - return search.group(1) + # Read from config.ini + def read_from_config(self, name): + # Only read file once + if self.config_content == '': + self.config_content = self.execute_command('cat ' + self.config_file) -# Write to config -def write_to_config(name, value): - if read_from_config(name) == '': - parrot_utils.execute_command(tn, 'echo "' + name + ' = ' + value + '\" >> /data/config.ini') - else: - parrot_utils.execute_command(tn, 'sed -i "s/\(' + name + ' *= *\).*/\\1' + value + '/g" /data/config.ini') - -# Check if vision framework is installed -def check_vision_installed(): - du_opt = parrot_utils.execute_command(tn,'du -d 2 /data/video/opt') - return '/data/video/opt/arm/gst' in du_opt or '/data/video/opt/arm/lib' in du_opt or '/data/video/opt/arm/tidsp-binaries-23.i3.8' in du_opt - -# Check if the vision framework is running -def check_vision_running(): - du_opt = parrot_utils.execute_command(tn,'du -d 2 /opt') - return '/opt/arm/gst' in du_opt and '/opt/arm/lib' in du_opt and '/opt/arm/tidsp-binaries-23.i3.8' in du_opt - -# Check if autoboot is installed -def check_autoboot(): - check_update = parrot_utils.execute_command(tn,'grep "START_PAPARAZZI" /bin/check_update.sh') - wifi_setup = parrot_utils.execute_command(tn,'grep "BASE_ADRESS" /bin/wifi_setup.sh') - if "START_PAPARAZZI" in check_update and "BASE_ADRESS" in wifi_setup: - return True - else: - return False - -# Check if custom wifi_setup script is installed -def check_wifi_setup(): - check_wifi = parrot_utils.execute_command(tn,'grep "static_ip_address_base" /bin/wifi_setup.sh') - if "static_ip_address_base" in check_wifi: - return True - else: - return False - -# Install the vision framework -def ardrone2_install_vision(): - print('Uploading GST') - parrot_utils.uploadfile(ftp, "arm_light.tgz", file("bin/arm_light.tgz", "rb")) - print(parrot_utils.execute_command(tn,"cd /data/video && tar -xzf arm_light.tgz")) - print(parrot_utils.execute_command(tn,"rm -rf /data/video/arm_light.tgz")) - print('Now Starting Vision') - ardrone2_start_vision() - -# Remove the vision framework -def ardrone2_remove_vision(): - parrot_utils.execute_command(tn,"rm -rf /opt/arm") - parrot_utils.execute_command(tn,"rm -rf /lib/dsp") - parrot_utils.execute_command(tn,"rm -rf /data/video/opt") - -# Start the vision framework -def ardrone2_start_vision(): - # Mount the directories - parrot_utils.execute_command(tn,"mkdir -p /opt/arm") - parrot_utils.execute_command(tn,"mkdir -p /lib/dsp") - parrot_utils.execute_command(tn,"mount --bind /data/video/opt/arm /opt/arm") - parrot_utils.execute_command(tn,"mount --bind /data/video/opt/arm/lib/dsp /lib/dsp") - # Start The DSP programs - parrot_utils.execute_command(tn,"kill -9 `pidof program.elf`") - parrot_utils.execute_command(tn,"kill -9 `pidof gst-launch-0.10`") - parrot_utils.execute_command(tn,"export PATH=/opt/arm/gst/bin:$PATH") - parrot_utils.execute_command(tn,"export DSP_PATH=/opt/arm/tidsp-binaries-23.i3.8/") - parrot_utils.execute_command(tn,"/bin/dspbridge/cexec.out -T /opt/arm/tidsp-binaries-23.i3.8/baseimage.dof -v") - parrot_utils.execute_command(tn,"/bin/dspbridge/dynreg.out -r /opt/arm/tidsp-binaries-23.i3.8/m4venc_sn.dll64P -v") - # Show result - parrot_utils.execute_command(tn,"ls -altr /opt/arm/gst/bin") - -# Install autoboot script -def ardrone2_install_autoboot(): - print('Uploading autoboot script') - parrot_utils.uploadfile(ftp, "check_update.sh", file("ardrone2/check_update.sh", "rb")) - print(parrot_utils.execute_command(tn,"mv /data/video/check_update.sh /bin/check_update.sh")) - print(parrot_utils.execute_command(tn,"chmod 777 /bin/check_update.sh")) - -# Install network script -def ardrone2_install_network_script(): - print('Uploading Wifi script') - parrot_utils.uploadfile(ftp, "wifi_setup.sh", file("ardrone2/wifi_setup.sh", "rb")) - print(parrot_utils.execute_command(tn,"mv /data/video/wifi_setup.sh /bin/wifi_setup.sh")) - print(parrot_utils.execute_command(tn,"chmod 777 /bin/wifi_setup.sh")) - -# Install olsr deamon -def ardrone2_install_olsrd(): - print('Uploading olsr deamon') - parrot_utils.uploadfile(ftp, "olsrd", file("ardrone2/olsrd", "rb")) - parrot_utils.uploadfile(ftp, "olsrd.conf", file("ardrone2/olsrd.conf", "rb")) - print(parrot_utils.execute_command(tn,"mv /data/video/olsrd /bin/olsrd")) - print(parrot_utils.execute_command(tn,"chmod 777 /bin/olsrd")) - print(parrot_utils.execute_command(tn,"mkdir -p /etc/olsrd")) - print(parrot_utils.execute_command(tn,"mv /data/video/olsrd.conf /etc/olsrd")) - print(parrot_utils.execute_command(tn,"rm -f /var/run && ln -s /tmp /var/run")) # olsrd needs /var/run folder, symlinked to /tmp - -# Set network SSID -def ardrone2_set_ssid(name): - write_to_config('ssid_single_player', name) - print('The network ID (SSID) of the ARDrone 2 is changed to ' + name) - -# Set IP address -def ardrone2_set_ip_address(address): - splitted_ip = address.split(".") - write_to_config('static_ip_address_base', splitted_ip[0] + '.' + splitted_ip[1] + '.' + splitted_ip[2] + '.') - write_to_config('static_ip_address_probe', splitted_ip[3]) - print('The IP Address of the ARDrone 2 is changed to ' + address) - -# Set wifi mode (0: master, 1: ad-hoc, 2: managed, *: master) -def ardrone2_set_wifi_mode(mode): - modes = { 'master' : '0', 'ad-hoc' : '1', 'managed' : '2', 'ad-hoc-olsr' : '3' } - try: - val = modes[mode] - except: - print('Unexpected wifi mode, setting to master (default)') - val = modes['master'] - write_to_config('wifi_mode', val) - print('The Wifi mode of the ARDrone2 is changed to ' + mode + ' (' + val + ')') - -# Set network channel -def ardrone2_set_wifi_channel(chan): - write_to_config('wifi_channel', chan) - print('The network channel of the ARDrone 2 is changed to ' + chan) - -def ardrone2_status(): - config_ini = parrot_utils.execute_command(tn,'cat /data/config.ini') - - print('======================== ARDrone 2 Status ========================') - print('Version:\t\t' + str(parrot_utils.check_version(tn, '/firmware'))) - print('Host:\t\t\t' + args.host + ' (' + read_from_config('static_ip_address_base', config_ini) + - read_from_config('static_ip_address_probe', config_ini) + ' after boot)') - - print('Serial number:\t\t' + read_from_config('drone_serial', config_ini)) - print('Network id:\t\t' + read_from_config('ssid_single_player', config_ini)) - print('Motor software:\t\t' + - read_from_config('motor1_soft', config_ini) + '\t' + read_from_config('motor2_soft', config_ini) + '\t' + - read_from_config('motor3_soft', config_ini) + '\t' + read_from_config('motor4_soft', config_ini)) - print('Motor hardware:\t\t' + - 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)) - - sleep(2.0) #Wait running process reporting back lag - print('Currently running:\t' + parrot_utils.check_running(tn)) - autorun = {'': 'Native', '0': 'Native', '1': 'Paparazzi'} - if check_autoboot(): - print('Autorun at start:\tInstalled booting ' + autorun[read_from_config('start_paparazzi', config_ini)]) - else: - print('Autorun at start:\tNot installed') - - # Check if the vision framework is installed and running - vision_framework = "" - if check_vision_installed(): - vision_framework += "Installed" - if check_vision_running(): - vision_framework += " and running" - print('Vision framework:\t' + vision_framework) - - # Request the filesystem status - print('\n======================== Filesystem Status ========================') - print(parrot_utils.check_filesystem(tn)) - - -# Parse the arguments -parser = argparse.ArgumentParser(description='ARDrone 2 python helper. Use ardrone2.py -h for help') -parser.add_argument('--host', metavar='HOST', default='192.168.1.1', - help='the ip address of ardrone2') -subparsers = parser.add_subparsers(title='Command to execute', metavar='command', dest='command') - -# All the subcommands and arguments -subparsers.add_parser('status', help='Request the status of the ARDrone 2') -subparsers.add_parser('reboot', help='Reboot the ARDrone 2') -subparsers.add_parser('installvision', help='Install the vision framework') -subparser_upload_gst = subparsers.add_parser('upload_gst_module', - help='Upload, configure and move a gstreamer0.10 module libXXX.so') -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 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)') -subparser_download = subparsers.add_parser('download_file', help='Download a file from the ARDrone 2') -subparser_download.add_argument('file', help='Filename (with the path on the local machine)') -subparser_download.add_argument('folder', help='Remote subfolder (base folder is /data/video)') -subparser_download_dir = subparsers.add_parser('download_dir', help='Download all files from a folder from the ARDrone 2') -subparser_download_dir.add_argument('dest', help='destination folder (on the local machine)') -subparser_download_dir.add_argument('folder', help='Remote subfolder (base folder is /data/video)') -subparser_rm_dir = subparsers.add_parser('rm_dir', help='Remove a directory and all its files from the ARDrone 2') -subparser_rm_dir.add_argument('folder', help='Remote subfolder (base folder is /data/video)') -subparser_insmod = subparsers.add_parser('insmod', help='Upload and insert kernel module') -subparser_insmod.add_argument('file', help='Filename of *.ko kernel module') -subparsers.add_parser('startvision', help='Start the vision framework') -subparser_start = subparsers.add_parser('start', help='Start a program on the ARDrone 2') -subparser_start.add_argument('program', help='the program to start') -subparser_kill = subparsers.add_parser('kill', help='Kill a program on the ARDrone 2') -subparser_kill.add_argument('program', help='the program to kill') -subparser_networkid = subparsers.add_parser('networkid', help='Set the network ID(SSID) of the ARDrone 2') -subparser_networkid.add_argument('name', help='the new network ID(SSID)') -subparser_ipaddress = subparsers.add_parser('ipaddress', help='Set the IP address of the ARDrone 2') -subparser_ipaddress.add_argument('address', help='the new IP address') -subparser_wifimode = subparsers.add_parser('wifimode', help='Set the Wifi mode the ARDrone 2') -subparser_wifimode.add_argument('mode', help='the new Wifi mode', choices=['master', 'ad-hoc', 'managed', 'ad-hoc-olsr']) -subparser_configure_network = subparsers.add_parser('configure_network', help='Configure the network on the ARDrone 2') -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', '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'], - 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'], - help='what to start on boot') - -args = parser.parse_args() - -# Connect with telnet and ftp -tn, ftp = parrot_utils.connect(args.host) - -# Check the ARDrone 2 status -if args.command == 'status': - ardrone2_status() - -# Reboot the drone -elif args.command == 'reboot': - parrot_utils.reboot(tn) - print('The ARDrone 2 is rebooting...') - -# Kill a program -elif args.command == 'kill': - parrot_utils.execute_command(tn,'killall -9 ' + args.program + ' &') - print('Program "' + args.program + '" is now killed') - -# Start a program -elif args.command == 'start': - parrot_utils.execute_command(tn,args.start + ' &') - print('Program "' + args.start + '" is now started') - -# Change the network ID -elif args.command == 'networkid': - ardrone2_set_ssid(args.name) - - if raw_input("Shall I restart the ARDrone 2? (y/N) ").lower() == 'y': - parrot_utils.reboot(tn) - -# Change the IP address -elif args.command == 'ipaddress': - ardrone2_set_ip_address(args.address) - - if raw_input("Shall I restart the ARDrone 2? (y/N) ").lower() == 'y': - parrot_utils.reboot(tn) - -# 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) - -# Install and configure network -elif args.command == 'configure_network': - config_ini = parrot_utils.execute_command(tn,'cat /data/config.ini') - print('=== Current network setup ===') - print('Network id:\t' + read_from_config('ssid_single_player', config_ini)) - print('Host:\t\t' + args.host + ' (' + read_from_config('static_ip_address_base', config_ini) + - read_from_config('static_ip_address_probe', config_ini) + ' after boot)') - print('Mode:\t\t' + read_from_config('wifi_mode', config_ini)) - print('Channel:\t' + read_from_config('wifi_channel', config_ini)) - print('=============================') - if check_wifi_setup(): - print('Custom Wifi script already installed') - if raw_input("Shall I reinstall the Wifi script (y/N) ").lower() == 'y': - ardrone2_install_network_script() - else: - if raw_input("Shall I install custom Wifi script (recommanded) (y/N) ").lower() == 'y': - ardrone2_install_network_script() - if raw_input("Shall I install olsrd (ad-hoc wireless mesh routing deamon) (y/N) ").lower() == 'y': - ardrone2_install_olsrd() - ardrone2_set_ssid(args.name) - ardrone2_set_ip_address(args.address) - ardrone2_set_wifi_mode(args.mode) - ardrone2_set_wifi_channel(args.channel) - config_ini = parrot_utils.execute_command(tn,'cat /data/config.ini') - print('== New network setup after boot ==') - print('Network id:\t' + read_from_config('ssid_single_player', config_ini)) - print('Host:\t\t' + read_from_config('static_ip_address_base', config_ini) + - read_from_config('static_ip_address_probe', config_ini)) - print('Mode:\t\t' + read_from_config('wifi_mode', config_ini)) - print('Channel:\t' + read_from_config('wifi_channel', config_ini)) - print('==================================') - - if raw_input("Shall I restart the ARDrone 2? (y/N) ").lower() == 'y': - parrot_utils.reboot(tn) - -# Install and configure autostart -elif args.command == 'install_autostart': - if check_autoboot(): - print('Custom autostart script already installed') - if raw_input("Shall I reinstall the autostart script (y/N) ").lower() == 'y': - ardrone2_install_autoboot() - else: - ardrone2_install_autoboot() - autorun = {'native': '0', 'paparazzi': '1'} - write_to_config('start_paparazzi', autorun[args.type]) - print('The autostart on boot is changed to ' + args.type) - - if raw_input("Shall I restart the ARDrone 2? (y/N) ").lower() == 'y': - parrot_utils.reboot(tn) - -# Change the autostart -elif args.command == 'autostart': - autorun = {'native': '0', 'paparazzi': '1'} - write_to_config('start_paparazzi', autorun[args.type]) - print('The autostart on boot is changed to ' + args.type) - -# Install Vision framework -elif args.command == 'installvision': - if check_vision_installed(): - print('Vision framework already installed') - if raw_input("Shall I reinstall the vision framework? (y/N) ").lower() == 'y': - ardrone2_remove_vision() - ardrone2_install_vision() - - ardrone2_install_vision() - print('Vision framework installed') - -# Start Vision framework -elif args.command == 'startvision': - if check_vision_running(): - print('Vision framework already started') - else: - if not check_vision_installed(): - print('No vision framework installed') - if raw_input("Shall I install the vision framework? (y/N) ").lower() == 'y': - ardrone2_install_vision() - - if check_vision_installed(): - ardrone2_start_vision() - print('Vision framework started') - -elif args.command == 'upload_gst_module': - print('Uploading ...' + args.file) - parrot_utils.uploadfile(ftp, args.file, file(args.file, "rb")) - parrot_utils.execute_command(tn,"chmod 777 /data/video/" + args.file) - parrot_utils.execute_command(tn,"mv /data/video/" + args.file + " /data/video/opt/arm/gst/lib/gstreamer-0.10") - if check_vision_running(): - print('Info: Vision framework already started') - else: - if not check_vision_installed(): - print('Warning: No vision framework installed') - if raw_input("Warning: Shall I install the vision framework? (y/N) ").lower() == 'y': - ardrone2_install_vision() - - if check_vision_installed(): - ardrone2_start_vision() - print('#pragma message: Vision framework started') - print('#pragma message: Vision Plugin Uploaded and DSP Started.') - - -elif args.command == 'insmod': - modfile = parrot_utils.split_into_path_and_file(args.file) - print('Uploading \'' + modfile[1]) - parrot_utils.uploadfile(ftp, modfile[1], file(args.file, "rb")) - print(parrot_utils.execute_command(tn,"insmod /data/video/" + modfile[1])) - -elif args.command == 'upload_file_and_run': - # Split filename and path - f = parrot_utils.split_into_path_and_file(args.file) - - print("Kill running " + f[1] + " and make folder " + args.folder) - parrot_utils.execute_command(tn,"killall -9 " + f[1] + ' &') - sleep(1) - parrot_utils.execute_command(tn, "mkdir -p /data/video/" + args.folder) - print('Uploading \'' + f[1] + "\' from " + f[0] + " to " + args.folder) - print("#pragma message: Please wait, uploading can take some time...") - parrot_utils.uploadfile(ftp, args.folder + "/" + f[1], file(args.file, "rb")) - sleep(0.5) - parrot_utils.execute_command(tn, "chmod 777 /data/video/" + args.folder + "/" + f[1]) - parrot_utils.execute_command(tn, "/data/video/" + args.folder + "/" + f[1] + " > /dev/null 2>&1 &") - print("#pragma message: Upload to, and start of Autopilot on, ARDrone2 successful !") - -elif args.command == 'upload_file': - # Split filename and path - f = parrot_utils.split_into_path_and_file(args.file) - - parrot_utils.execute_command(tn,"mkdir -p /data/video/" + args.folder) - print('Uploading \'' + f[1] + "\' from " + f[0] + " to /data/video/" + args.folder) - parrot_utils.uploadfile(ftp, args.folder + "/" + f[1], file(args.file, "rb")) - print("#pragma message: Upload of " + f[1] + " to ARDrone2 successful !") - -elif args.command == 'download_file': - # Split filename and path - f = parrot_utils.split_into_path_and_file(args.file) - # Open file and download - try: - file = open(args.file, 'wb') - print('Downloading \'' + f[1] + "\' from " + args.folder + " to " + f[0]) - ftp.retrbinary("RETR " + args.folder + "/" + f[1], file.write) - print("#pragma message: Download of " + f[1] + " from ARDrone2 successful !") - except IOError: - print("#pragma message: Fail to open file " + args.file) - except: - os.remove(args.file) - print("#pragma message: Download of " + f[1] + " from ARDrone2 Failed!") - else: - file.close() - -elif args.command == 'download_dir': - # Split filename and path - files = parrot_utils.execute_command(tn, 'find /data/video/' + args.folder + ' -name \'*.*\'') - # Create dest dir if needed - if not os.path.exists(args.dest): - os.mkdir(args.dest) - # Open file and download - for f in files.split(): - file_name = parrot_utils.split_into_path_and_file(f) - file_source = args.folder + '/' + file_name[1] - file_dest = args.dest + '/' + file_name[1] - try: - file = open(file_dest, 'wb') - print('Downloading \'' + f + "\' to " + file_dest) - ftp.retrbinary("RETR " + file_source, file.write) - except IOError: - print("#pragma message: Fail to open file " + file_dest) - except: - os.remove(file_dest) - print("#pragma message: Download of " + f + " from ARDrone2 Failed!") + # Search for the name + search = re.search(name + '[^=]+=[\r\n\t ]([^\r\n\t ]+)',self.config_content) + if search is None: + return None else: - file.close() - print("#pragma message: End download of folder " + args.folder + " from ARDrone2") + return search.group(1) -elif args.command == 'rm_dir': - # Split filename and path - print("Deleting folder /data/video/" + args.folder + " from ARDrone2...") - print(parrot_utils.execute_command(tn, 'rm -r /data/video/' + args.folder)) + # Write to config + def write_to_config(self, name, value): + if self.read_from_config(name) == None: + self.execute_command('echo "' + name + ' = ' + value + '\" >> ' + self.config_file) + else: + self.execute_command('sed -i "s/\(' + name + ' *= *\).*/\\1' + value + '/g" ' + self.config_file) + + def uav_status(self): + print('Parrot version:\t\t' + self.check_version()) + print('Host address:\t\t' + self.address + + ' (' + self.read_from_config('static_ip_address_base', config_ini) + + self.read_from_config('static_ip_address_probe', config_ini) + ' after boot)') + print('Serial number:\t\t' + self.read_from_config('drone_serial')) + print('Network id:\t\t' + self.read_from_config('ssid_single_player')) + print('Motor software:\t\t' + + self.read_from_config('motor1_soft', config_ini) + '\t' + read_from_config('motor2_soft', config_ini) + '\t' + + self.read_from_config('motor3_soft', config_ini) + '\t' + read_from_config('motor4_soft', config_ini)) + print('Motor hardware:\t\t' + + self.read_from_config('motor1_hard', config_ini) + '\t' + read_from_config('motor2_hard', config_ini) + '\t' + + self.read_from_config('motor3_hard', config_ini) + '\t' + read_from_config('motor4_hard', config_ini)) + sleep(2.0) #Wait running process reporting back lag + print('Currently running:\t' + self.check_running()) + autorun = {'': 'Native', '0': 'Native', '1': 'Paparazzi'} + if self.check_autoboot(): + print('Autorun at start:\tInstalled booting ' + autorun[self.read_from_config('start_paparazzi', config_ini)]) + else: + print('Autorun at start:\tNot installed') + + # Check if the vision framework is installed and running + vision_framework = "" + if self.check_vision_installed(): + vision_framework += "Installed" + if self.check_vision_running(): + vision_framework += " and running" + print('Vision framework:\t' + vision_framework) -# Close the telnet and python script -parrot_utils.disconnect(tn, ftp) -exit(0) + # Check if vision framework is installed + def check_vision_installed(self): + du_opt = self.execute_command('du -d 2 /data/video/opt') + return '/data/video/opt/arm/gst' in du_opt or '/data/video/opt/arm/lib' in du_opt or '/data/video/opt/arm/tidsp-binaries-23.i3.8' in du_opt + + # Check if the vision framework is running + def check_vision_running(self): + du_opt = self.execute_command('du -d 2 /opt') + return '/opt/arm/gst' in du_opt and '/opt/arm/lib' in du_opt and '/opt/arm/tidsp-binaries-23.i3.8' in du_opt + + # Check if autoboot is installed + def check_autoboot(self): + check_update = self.execute_command('grep "START_PAPARAZZI" /bin/check_update.sh') + wifi_setup = self.execute_command('grep "BASE_ADRESS" /bin/wifi_setup.sh') + if "START_PAPARAZZI" in check_update and "BASE_ADRESS" in wifi_setup: + return True + else: + return False + + # Check if custom wifi_setup script is installed + def check_wifi_setup(self): + check_wifi = self.execute_command('grep "static_ip_address_base" /bin/wifi_setup.sh') + if "static_ip_address_base" in check_wifi: + return True + else: + return False + + # Install the vision framework + def ardrone2_install_vision(self): + print('Uploading GST') + self.upload_file("arm_light.tgz") + print(self.execute_command("cd /data/video && tar -xzf arm_light.tgz")) + print(self.execute_command("rm -rf /data/video/arm_light.tgz")) + print('Now Starting Vision') + self.ardrone2_start_vision() + + # Remove the vision framework + def ardrone2_remove_vision(self): + self.execute_command("rm -rf /opt/arm") + self.execute_command("rm -rf /lib/dsp") + self.execute_command("rm -rf /data/video/opt") + + # Start the vision framework + def ardrone2_start_vision(self): + # Mount the directories + self.execute_command("mkdir -p /opt/arm") + self.execute_command("mkdir -p /lib/dsp") + self.execute_command("mount --bind /data/video/opt/arm /opt/arm") + self.execute_command("mount --bind /data/video/opt/arm/lib/dsp /lib/dsp") + # Start The DSP programs + self.execute_command("kill -9 `pidof program.elf`") + self.execute_command("kill -9 `pidof gst-launch-0.10`") + self.execute_command("export PATH=/opt/arm/gst/bin:$PATH") + self.execute_command("export DSP_PATH=/opt/arm/tidsp-binaries-23.i3.8/") + self.execute_command("/bin/dspbridge/cexec.out -T /opt/arm/tidsp-binaries-23.i3.8/baseimage.dof -v") + self.execute_command("/bin/dspbridge/dynreg.out -r /opt/arm/tidsp-binaries-23.i3.8/m4venc_sn.dll64P -v") + # Show result + self.execute_command("ls -altr /opt/arm/gst/bin") + + # Install autoboot script + def ardrone2_install_autoboot(self): + print('Uploading autoboot script') + self.upload_file("ardrone2/check_update.sh") + print(self.execute_command("mv /data/video/check_update.sh /bin/check_update.sh")) + print(self.execute_command("chmod 777 /bin/check_update.sh")) + + # Install network script + def ardrone2_install_network_script(self): + print('Uploading Wifi script') + self.upload_file("ardrone2/wifi_setup.sh") + print(self.execute_command("mv /data/video/wifi_setup.sh /bin/wifi_setup.sh")) + print(self.execute_command("chmod 777 /bin/wifi_setup.sh")) + + # Install olsr deamon + def ardrone2_install_olsrd(self): + print('Uploading olsr deamon') + self.upload_file("ardrone2/olsrd") + self.upload_file("ardrone2/olsrd.conf") + print(self.execute_command("mv /data/video/olsrd /bin/olsrd")) + print(self.execute_command("chmod 777 /bin/olsrd")) + print(self.execute_command("mkdir -p /etc/olsrd")) + print(self.execute_command("mv /data/video/olsrd.conf /etc/olsrd")) + print(self.execute_command("rm -f /var/run && ln -s /tmp /var/run")) # olsrd needs /var/run folder, symlinked to /tmp + + # Set network SSID + def ardrone2_set_ssid(self, name): + write_to_config('ssid_single_player', name) + print('The network ID (SSID) of the ARDrone 2 is changed to ' + name) + + # Set IP address + def ardrone2_set_ip_address(self, address): + splitted_ip = address.split(".") + write_to_config('static_ip_address_base', splitted_ip[0] + '.' + splitted_ip[1] + '.' + splitted_ip[2] + '.') + write_to_config('static_ip_address_probe', splitted_ip[3]) + print('The IP Address of the ARDrone 2 is changed to ' + address) + + # Set wifi mode (0: master, 1: ad-hoc, 2: managed, *: master) + def ardrone2_set_wifi_mode(self, mode): + modes = { 'master' : '0', 'ad-hoc' : '1', 'managed' : '2', 'ad-hoc-olsr' : '3' } + try: + val = modes[mode] + except: + print('Unexpected wifi mode, setting to master (default)') + val = modes['master'] + write_to_config('wifi_mode', val) + print('The Wifi mode of the ARDrone2 is changed to ' + mode + ' (' + val + ')') + + # Set network channel + def ardrone2_set_wifi_channel(self, chan): + write_to_config('wifi_channel', chan) + print('The network channel of the ARDrone 2 is changed to ' + chan) + + def init_extra_parser(self): + # Parse custom arguments + self.subparsers.add_parser('installvision', help='Install the vision framework') + self.subparsers.add_parser('startvision', help='Start the vision framework') + ss = self.subparsers.add_parser('upload_gst_module', + help='Upload, configure and move a gstreamer0.10 module libXXX.so') + ss.add_argument('file', help='Filename of *.so module') + + ss = self.subparsers.add_parser('networkid', help='Set the network ID(SSID) of the ARDrone 2') + ss.add_argument('name', help='the new network ID(SSID)') + ss = self.subparsers.add_parser('ipaddress', help='Set the IP address of the ARDrone 2') + ss.add_argument('address', help='the new IP address') + ss = self.subparsers.add_parser('wifimode', help='Set the Wifi mode the ARDrone 2') + ss.add_argument('mode', help='the new Wifi mode', choices=['master', 'ad-hoc', 'managed', 'ad-hoc-olsr']) + ss = self.subparsers.add_parser('configure_network', help='Configure the network on the ARDrone 2') + ss.add_argument('name', help='the new network ID(SSID)') + ss.add_argument('address', help='the new IP address') + ss.add_argument('mode', help='the new Wifi mode', choices=['master', 'ad-hoc', 'managed', 'ad-hoc-olsr']) + ss.add_argument('--channel', help='the wifi channel (auto or 1 to 11)', default='auto') + ss = self.subparsers.add_parser('install_autostart', help='Install custom autostart script and set what to start on boot for the ARDrone 2') + ss.add_argument('type', choices=['native', 'paparazzi'], + help='what to start on boot') + ss = self.subparsers.add_parser('autostart', help='Set what to start on boot for the ARDrone 2') + ss.add_argument('type', choices=['native', 'paparazzi'], + help='what to start on boot') + + def parse_extra_args(self, args): + + # Change the network ID + if args.command == 'networkid': + self.ardrone2_set_ssid(args.name) + if raw_input("Shall I restart the ARDrone 2? (y/N) ").lower() == 'y': + self.reboot() + + # Change the IP address + elif args.command == 'ipaddress': + self.ardrone2_set_ip_address(args.address) + if raw_input("Shall I restart the ARDrone 2? (y/N) ").lower() == 'y': + self.reboot() + + # Change the wifi mode + elif args.command == 'wifimode': + self.ardrone2_set_wifi_mode(args.mode) + if raw_input("Shall I restart the ARDrone 2? (y/N) ").lower() == 'y': + self.reboot() + + # Install and configure network + elif args.command == 'configure_network': + config_ini = self.execute_command('cat /data/config.ini') + print('=== Current network setup ===') + print('Network id:\t' + self.read_from_config('ssid_single_player', config_ini)) + print('Host:\t\t' + args.host + ' (' + read_from_config('static_ip_address_base', config_ini) + + self.read_from_config('static_ip_address_probe', config_ini) + ' after boot)') + print('Mode:\t\t' + self.read_from_config('wifi_mode', config_ini)) + print('Channel:\t' + self.read_from_config('wifi_channel', config_ini)) + print('=============================') + if self.check_wifi_setup(): + print('Custom Wifi script already installed') + if raw_input("Shall I reinstall the Wifi script (y/N) ").lower() == 'y': + self.ardrone2_install_network_script() + else: + if raw_input("Shall I install custom Wifi script (recommanded) (y/N) ").lower() == 'y': + self.ardrone2_install_network_script() + if raw_input("Shall I install olsrd (ad-hoc wireless mesh routing deamon) (y/N) ").lower() == 'y': + self.ardrone2_install_olsrd() + self.ardrone2_set_ssid(args.name) + self.ardrone2_set_ip_address(args.address) + self.ardrone2_set_wifi_mode(args.mode) + self.ardrone2_set_wifi_channel(args.channel) + config_ini = self.execute_command('cat /data/config.ini') + print('== New network setup after boot ==') + print('Network id:\t' + self.read_from_config('ssid_single_player', config_ini)) + print('Host:\t\t' + self.read_from_config('static_ip_address_base', config_ini) + + self.read_from_config('static_ip_address_probe', config_ini)) + print('Mode:\t\t' + self.read_from_config('wifi_mode', config_ini)) + print('Channel:\t' + self.read_from_config('wifi_channel', config_ini)) + print('==================================') + + if raw_input("Shall I restart the ARDrone 2? (y/N) ").lower() == 'y': + self.reboot() + + # Install and configure autostart + elif args.command == 'install_autostart': + if self.check_autoboot(): + print('Custom autostart script already installed') + if raw_input("Shall I reinstall the autostart script (y/N) ").lower() == 'y': + self.ardrone2_install_autoboot() + else: + self.ardrone2_install_autoboot() + autorun = {'native': '0', 'paparazzi': '1'} + self.write_to_config('start_paparazzi', autorun[args.type]) + print('The autostart on boot is changed to ' + args.type) + + if raw_input("Shall I restart the ARDrone 2? (y/N) ").lower() == 'y': + self.reboot() + + # Change the autostart + elif args.command == 'autostart': + autorun = {'native': '0', 'paparazzi': '1'} + self.write_to_config('start_paparazzi', autorun[args.type]) + print('The autostart on boot is changed to ' + args.type) + + # Install Vision framework + elif args.command == 'installvision': + if self.check_vision_installed(): + print('Vision framework already installed') + if raw_input("Shall I reinstall the vision framework? (y/N) ").lower() == 'y': + self.ardrone2_remove_vision() + self.ardrone2_install_vision() + + self.ardrone2_install_vision() + print('Vision framework installed') + + # Start Vision framework + elif args.command == 'startvision': + if self.check_vision_running(): + print('Vision framework already started') + else: + if not self.check_vision_installed(): + print('No vision framework installed') + if raw_input("Shall I install the vision framework? (y/N) ").lower() == 'y': + self.ardrone2_install_vision() + + if self.check_vision_installed(): + self.ardrone2_start_vision() + print('Vision framework started') + + elif args.command == 'upload_gst_module': + print('Uploading ...' + args.file) + self.upload_file(args.file) + self.execute_command("chmod 777 /data/video/" + args.file) + self.execute_command("mv /data/video/" + args.file + " /data/video/opt/arm/gst/lib/gstreamer-0.10") + if self.check_vision_running(): + print('Info: Vision framework already started') + else: + if not self.check_vision_installed(): + print('Warning: No vision framework installed') + if raw_input("Warning: Shall I install the vision framework? (y/N) ").lower() == 'y': + self.ardrone2_install_vision() + + if self.check_vision_installed(): + self.ardrone2_start_vision() + print('#pragma message: Vision framework started') + print('#pragma message: Vision Plugin Uploaded and DSP Started.') + + +if __name__ == "__main__": + ardrone2 = Ardrone2() + ardrone2.parse_args() + exit(0) + diff --git a/sw/tools/parrot/bebop.py b/sw/tools/parrot/bebop.py index 49999f8dd1..eabd322b31 100755 --- a/sw/tools/parrot/bebop.py +++ b/sw/tools/parrot/bebop.py @@ -1,6 +1,8 @@ #!/usr/bin/env python # # Copyright (C) 2012-2014 The Paparazzi Team +# 2015 Freek van Tienen +# 2017 Gautier Hattenberger # # This file is part of Paparazzi. # @@ -20,192 +22,34 @@ # from __future__ import print_function -import re -import argparse -import os -from time import sleep +from parrot_utils import ParrotUtils -import parrot_utils +class Bebop(ParrotUtils): + uav_name = 'Bebop' + address = '192.168.42.1' + version_file = '/version.txt' + upload_path = '/data/ftp/' + check_version_before_run = True + update_time_before_run = True + + def uav_status(self): + print('Parrot version:\t\t' + self.check_version()) + + def init_extra_parser(self): + + # Parse the extra arguments + self.parser.add_argument('--min_version', metavar='MIN', default='3.3.0', + help='force minimum version allowed') + self.parser.add_argument('--max_version', metavar='MAX', default='4.0.5', + help='force maximum version allowed') + + def parse_extra_args(self, args): + # nothing here + pass -# Read from config.ini TODO -def read_from_config(name, config=''): - if config == '': - config = parrot_utils.execute_command('cat /data/config.ini') - search = re.search(name + '[^=]+=[\r\n\t ]([^\r\n\t ]+)', config) - if search is None: - return '' - else: - return search.group(1) +if __name__ == "__main__": + bebop = Bebop() + bebop.parse_args() + exit(0) -# Write to config TODO -def write_to_config(name, value): - if read_from_config(name) == '': - parrot_utils.execute_command('echo "' + name + ' = ' + value + '\" >> /data/config.ini') - else: - parrot_utils.execute_command('sed -i "s/\(' + name + ' *= *\).*/\\1' + value + '/g" /data/config.ini') - - -def bebop_status(): - #config_ini = parrot_utils.execute_command(tn, 'cat /data/config.ini') - - print('======================== Bebop Status ========================') - print('Version:\t\t' + str(parrot_utils.check_version(tn, ''))) - # Request the filesystem status - print('\n=================== Filesystem Status =======================') - print(parrot_utils.check_filesystem(tn)) - - -# Parse the arguments -parser = argparse.ArgumentParser(description='Bebop helper tool. Use bebop.py -h for help') -parser.add_argument('--host', metavar='HOST', default='192.168.42.1', - help='the ip address of bebop') -parser.add_argument('--min_version', metavar='MIN', default='3.3.0', - help='force minimum version allowed') -parser.add_argument('--max_version', metavar='MAX', default='4.0.5', - help='force maximum version allowed') -subparsers = parser.add_subparsers(title='Command to execute', metavar='command', dest='command') - -# All the subcommands and arguments -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 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)') -subparser_download = subparsers.add_parser('download_file', help='Download a file from the Bebop') -subparser_download.add_argument('file', help='Filename (with the path on the local machine)') -subparser_download.add_argument('folder', help='Remote subfolder (base folder is /data/ftp)') -subparser_download_dir = subparsers.add_parser('download_dir', help='Download all files from a folder from the Bebop') -subparser_download_dir.add_argument('dest', help='destination folder (on the local machine)') -subparser_download_dir.add_argument('folder', help='Remote subfolder (base folder is /data/ftp)') -subparser_rm_dir = subparsers.add_parser('rm_dir', help='Remove a directory and all its files from the Bebop') -subparser_rm_dir.add_argument('folder', help='Remote subfolder (base folder is /data/ftp)') -subparser_insmod = subparsers.add_parser('insmod', help='Upload and insert kernel module') -subparser_insmod.add_argument('file', help='Filename of *.ko kernel module') -subparser_start = subparsers.add_parser('start', help='Start a program on the Bebop') -subparser_start.add_argument('program', help='the program to start') -subparser_kill = subparsers.add_parser('kill', help='Kill a program on the Bebop') -subparser_kill.add_argument('program', help='the program to kill') - -args = parser.parse_args() - -# Connect with telnet and ftp -tn, ftp = parrot_utils.connect(args.host) - -# Check the Bebop status -if args.command == 'status': - print("Connected to Bebop at " + args.host) - bebop_status() - -# Reboot the drone -elif args.command == 'reboot': - parrot_utils.reboot(tn) - print('The Bebop is rebooting...') - -# Kill a program -elif args.command == 'kill': - parrot_utils.execute_command(tn, 'killall -9 ' + args.program) - print('Program "' + args.program + '" is now killed') - -# Start a program -elif args.command == 'start': - parrot_utils.execute_command(tn, args.start + ' &') - print('Program "' + args.start + '" is now started') - - -elif args.command == 'insmod': - modfile = parrot_utils.split_into_path_and_file(args.file) - print('Uploading \'' + modfile[1]) - parrot_utils.uploadfile(ftp, modfile[1], file(args.file, "rb")) - print(parrot_utils.execute_command(tn, "insmod /data/ftp/" + modfile[1])) - -elif args.command == 'upload_file_and_run': - # Split filename and path - f = parrot_utils.split_into_path_and_file(args.file) - - #check firmware version - v = parrot_utils.check_version(tn, '') - print("Checking Bebop firmware version... " + str(v) ) - if ((v < parrot_utils.ParrotVersion(args.min_version)) or (v > parrot_utils.ParrotVersion(args.max_version))): - print("Error: please upgrade your Bebop firmware to version between " + args.min_version + " and " + args.max_version + "!") - else: - print("Kill running " + f[1] + " and make folder " + args.folder) - parrot_utils.execute_command(tn,"killall -9 " + f[1]) - sleep(1) - parrot_utils.execute_command(tn, "mkdir -p /data/ftp/" + args.folder) - print('Uploading \'' + f[1] + "\' from " + f[0] + " to " + args.folder) - parrot_utils.uploadfile(ftp, args.folder + "/" + f[1], file(args.file, "rb")) - sleep(0.5) - - from datetime import datetime - parrot_utils.execute_command(tn, "date --set '" + datetime.now().strftime('%Y-%m-%d %H:%M:%S') + "'") - print("Set date on Bebop to " + datetime.now().strftime('%Y-%m-%d %H:%M:%S')) - - parrot_utils.execute_command(tn, "chmod 777 /data/ftp/" + args.folder + "/" + f[1]) - parrot_utils.execute_command(tn, "/data/ftp/" + args.folder + "/" + f[1] + " > /dev/null 2>&1 &") - print("#pragma message: Upload and Start of ap.elf to Bebop succesful !") - - -elif args.command == 'upload_file': - # Split filename and path - f = parrot_utils.split_into_path_and_file(args.file) - - parrot_utils.execute_command(tn, "mkdir -p /data/ftp/" + args.folder) - print('Uploading \'' + f[1] + "\' from " + f[0] + " to /data/ftp/" + args.folder) - parrot_utils.uploadfile(ftp, args.folder + "/" + f[1], file(args.file, "rb")) - print("#pragma message: Upload of " + f[1] + " to Bebop succesful !") - -elif args.command == 'download_file': - # Split filename and path - f = parrot_utils.split_into_path_and_file(args.file) - # Open file and download - try: - fd = open(args.file, 'wb') - print('Downloading \'' + f[1] + "\' from " + args.folder + " to " + f[0]) - ftp.retrbinary("RETR " + args.folder + "/" + f[1], fd.write) - print("#pragma message: Download of " + f[1] + " from Bebop Succes!") - except IOError: - print("#pragma message: Fail to open file " + args.file) - except: - os.remove(args.file) - print("#pragma message: Download of " + f[1] + " from Bebop Failed!") - else: - fd.close() - -elif args.command == 'download_dir': - # Split filename and path - files = parrot_utils.execute_command(tn, 'find /data/ftp/' + args.folder + ' -name \'*.*\'') - # Create dest dir if needed - if not os.path.exists(args.dest): - os.mkdir(args.dest) - # Open file and download - for f in files.split(): - file_name = parrot_utils.split_into_path_and_file(f) - file_source = args.folder + '/' + file_name[1] - file_dest = args.dest + '/' + file_name[1] - try: - fd = open(file_dest, 'wb') - print('Downloading \'' + f + "\' to " + file_dest) - ftp.retrbinary("RETR " + file_source, fd.write) - except IOError: - print("#pragma message: Fail to open file " + file_dest) - except: - os.remove(file_dest) - print("#pragma message: Download of " + f + " from Bebop Failed!") - else: - fd.close() - print("#pragma message: End download of folder " + args.folder + " from Bebop") - -elif args.command == 'rm_dir': - # Split filename and path - print("Deleting folder /data/ftp/" + args.folder + " from Bebop") - print(parrot_utils.execute_command(tn, 'rm -r /data/ftp/' + args.folder)) - - - -# Close the telnet and python script -parrot_utils.disconnect(tn, ftp) -exit(0) diff --git a/sw/tools/parrot/parrot_utils.py b/sw/tools/parrot/parrot_utils.py index 0de8f953a1..5da91e77f6 100644 --- a/sw/tools/parrot/parrot_utils.py +++ b/sw/tools/parrot/parrot_utils.py @@ -1,5 +1,7 @@ # # Copyright (C) 2012-2014 The Paparazzi Team +# 2015 Freek van Tienen +# 2017 Gautier Hattenberger # # This file is part of Paparazzi. # @@ -21,9 +23,13 @@ from __future__ import print_function import socket import telnetlib +import os import sys from ftplib import FTP +from time import sleep import ftplib +import argparse +import re class ParrotVersion(object): def __init__(self): @@ -69,84 +75,316 @@ class ParrotVersion(object): def __ge__(self, other): return self.version() >= other.version() +class ParrotUtils: -# Check if IP is valid -def is_ip(address): - try: - socket.inet_aton(address) - ip = True - except socket.error: - ip = False - return ip + # Programs that could be running on the drone + runnable_programs = [ + ('program.elf', 'Parrot native UAV application'), + ('dragon-prog', 'Parrot native UAV application'), + ('ap.elf', 'Paparazzi'), + ('gst-launch', 'GStreamer') + ] -# Helper function -def split_into_path_and_file(name): - if name.count('/') <= 0: - return ["./", name] - return name.rsplit('/', 1) + # Default values + version_file = '/update/version.txt' + upload_path = '/data/video/' + uav_name = 'Parrot UAV' + address = '192.168.1.1' + prompt = '# ' + check_version_before_run = False + update_time_before_run = False -# Execute a command -def execute_command(tn, command): - tn.write(command + '\n') - return tn.read_until('# ')[len(command) + 2:-4] + # Initialize defaults + def __init__(self): + self.config_content = '' + self.init_parser() + self.init_extra_parser() -# Check the version -def check_version(tn, directory): - return ParrotVersion(execute_command(tn, 'cat ' + directory + '/version.txt').strip()) + # Connect with telnet and ftp, wait until login + def connect(self): + try: + self.tn = telnetlib.Telnet(self.address, timeout=3) + self.ftp = FTP(self.address) + self.ftp.login() + self.tn.read_until(self.prompt) + return True + except: + print('Could not connect to the ' + self.uav_name + ' (address: ' + self.address + ')') + print('Check if the ' + self.uav_name + ' is turned on and the computer is connected over wifi or bluetooth.') + if self.address == '192.168.42.1': + print("If you are using Bebop 1 or 2, don't forget pressing the power button 4 times after the Bebop has booted!") + exit(2) -# Check what currently is running on the drone -def check_running(tn): - ps_aux = execute_command(tn, 'ps') - running = "" + # Close the telnet and ftp + def disconnect(self): + self.tn.close() + self.ftp.close() + + # Execute a command + def execute_command(self, command): + self.tn.write(command + '\n') + s = self.tn.read_until(self.prompt) + if s.endswith('[JS] $ '): + s = s[len(command) + 2:-8] + elif s.endswith('[RS.edu] $ '): + s = s[len(command) + 2:-12] + else: + s = s[len(command) + 2:-4] + return s + + # Upload ftp and catch memory-full error + def upload(self, filename, content): + try: + self.ftp.storbinary("STOR " + filename, content) + except ftplib.error_temp: + print('FTP UPLOAD ERROR: Uploading the file to the ' + self.uav_name + ' failed!') + print('Check if the Filesystem of the ' + self.uav_name + ' isn\'t full:') + print(self.check_filesystem()) + sys.exit() + except: + print('FTP UPLOAD ERROR: Uploading the file to the ' + self.uav_name + ' failed!') + print('FTP uploading failed with the following error: ', sys.exc_info()[0]) + print('Check if the Filesystem of the ' + self.uav_name + ' isn\'t full:') + print(self.check_filesystem()) + sys.exit() + + # Download a file from the drone + def download(self, filename, folder): + # Split filename and path + f = self.split_into_path_and_file(filename) + # Open file and download + try: + fd = open(filename, 'wb') + print('Downloading \'' + f[1] + "\' from " + folder + " to " + f[0]) + self.ftp.retrbinary("RETR " + folder + "/" + f[1], fd.write) + print('#pragma message: Download of "' + f[1] + '" from the ' + self.uav_name + ' success!') + except IOError: + print('#error: Fail to open local file "' + filename + '"') + except: + os.remove(filename) + print('#error: Download of "' + filename + '" from ' + self.uav_name + ' Failed!') + else: + fd.close() + + # Download a folder from the drone + def download_folder(self, dest, folder): + # find files + files = self.execute_command('find ' + self.upload_path + folder + ' -name \'*.*\'') + # Create dest dir if needed + if not os.path.exists(dest): + os.mkdir(dest) + # Open file and download + for f in files.split(): + file_dest = dest + '/' + file_name[1] + self.download(file_dest, folder) + print("#pragma message: End download of folder " + args.folder + " from " + self.uav_name) - if 'dragon-prog' in ps_aux: - running += ' Native (dragon-prog),' - if 'ap.elf' in ps_aux: - running += ' Paparazzi (ap.elf),' - if 'program.elf' in ps_aux: - running += ' Native (program.elf),' - if 'gst-launch' in ps_aux: - running += ' GStreamer (gst-launch)' - return running[1:] + # helper function to split file name and path + def split_into_path_and_file(self, name): + if name.count('/') <= 0: + return ["./", name] + return name.rsplit('/', 1) -# Check the filesystem -def check_filesystem(tn): - return execute_command(tn, 'df -h') + def is_ip(address): + try: + socket.inet_aton(address) + ip = True + except socket.error: + ip = False + return ip -# Reboot the drone -def reboot(tn): - execute_command(tn, 'reboot') + # Check what currently is running on the drone + def check_running(self): + ps_aux = self.execute_command('ps') + running = "" -# Upload ftp and catch memory-full error -def uploadfile(ftp, filename, content): - try: - ftp.storbinary("STOR " + filename, content) - except ftplib.error_temp: - print("FTP UPLOAD ERROR: Uploading FAILED: Probably your drone onboard storage memory is full. Remove old JPG or other data?") - sys.exit() - except: - print("FTP UPLOAD ERROR: Maybe your drone onboard storage memory is full? Remove old JPG or other data?)", sys.exc_info()[0]) - sys.exit() + # Go trough all programings + for prog in self.runnable_programs: + if prog[0] in ps_aux: + running += ' '+prog[1]+' ('+prog[0]+')' + + # Don't print the first space + return running[1:] + + # Check the filesystem + def check_filesystem(self): + return self.execute_command('df -h') + + # Get the version of the drone + def check_version(self): + if self.version_file is not None: + return ParrotVersion(execute_command('cat ' + self.version_file).strip()) + else: + return "Unknown version" + + # Default status + def status(self): + print('======================== ' + self.uav_name + ' Status ========================') + self.uav_status() + + print('\n======================== Filesystem Status ========================') + print(self.check_filesystem()) + + # Reboot the drone + def reboot(self): + self.execute_command('reboot') + print('The ' + self.uav_name + ' is now rebooting') + + # Kill a running program + def kill_program(self, name): + self.execute_command('killall -9 ' + name) + print('Program "' + name + '" is now killed') + + # Start a new program + def start_program(self, name): + self.execute_command('chmod 777 ' + name) + self.execute_command(name + ' > /dev/null 2>&1 &') + print('Program "' + name + '" is now started') + + # Create a new directory + def create_directory(self, name): + self.execute_command('mkdir -p ' + name) + print('Created new directory "' + name + '"') + + # Remove a directory + def remove_directory(self, name): + self.sexecute_command('rm -r ' + name) + print('Removed directory "' + name + '"') + + # Upload a new file + def upload_file(self, name, folder=""): + f = self.split_into_path_and_file(name) + + # First kill the running program + self.kill_program(f[1]) + sleep(1) + + # Make the upload directory and upload the file + self.create_directory(self.upload_path + folder) + if len(folder) > 0: + self.upload(folder + '/' + f[1], file(name, "rb")) + else: + self.upload(f[1], file(name, "rb")) + sleep(0.5) + print('Succesfully uploaded "' + name + '" to folder "' + folder + '"') + + # Upload and run a new program + def upload_and_run(self, name, folder, min_ver=None, max_ver=None): + if self.check_version_before_run and min_ver is not None and max_ver is not None: + v = self.check_version() + print("Checking " + self.uav_name + " firmware version... " + str(v) ) + if ((v < self.ParrotVersion(min_ver)) or (v > self.ParrotVersion(max_ver))): + print("Error: please upgrade your " + self.uav_name + " firmware to version between " + min_ver + " and " + max_ver + "!") + return + + f = self.split_into_path_and_file(name) + + # Upload the file + self.upload_file(name, folder) + + if self.update_time_before_run: + from datetime import datetime + self.execute_command("date --set '" + datetime.now().strftime('%Y-%m-%d %H:%M:%S') + "'") + print("Set date on " + self.uav_name + " to " + datetime.now().strftime('%Y-%m-%d %H:%M:%S')) + + # Make the file executable and execute it + self.start_program(self.upload_path + folder + '/' + f[1]) + print('#pragma message: Succesfully started "' + f[1] + '" on ' + self.uav_name) + + def insmod(self, modname): + f = parrot_utils.split_into_path_and_file(modname) + print('Uploading \'' + f[1]) + self.upload_file(modname) + print(self.execute_command("insmod " + self.upload_path + '/' + modfile[1])) + +##################################################################### + + # Main argument parser setup + def init_parser(self): + self.parser = argparse.ArgumentParser(description=self.uav_name + ' python helper. Use ' + sys.argv[0] + ' -h for help') + self.parser.add_argument('--host', metavar='HOST', default=self.address, + help='the ip address of ' + self.uav_name) + self.subparsers = self.parser.add_subparsers(title='Command to execute', metavar='command', dest='command') + + # Add commands + self.subparsers.add_parser('status', help='Request the status of the ' + self.uav_name) + self.subparsers.add_parser('reboot', help='Reboot the ' + self.uav_name) + + ss = self.subparsers.add_parser('kill', help='Kill a program on the ' + self.uav_name) + ss.add_argument('program', help='The program to kill') + + ss = self.subparsers.add_parser('start', help='Start a program on the ' + self.uav_name) + ss.add_argument('program', help='The program to start (base folder is the ftp folder)') + + ss = self.subparsers.add_parser('upload', help='Upload a file to the ' + self.uav_name) + ss.add_argument('file', help='Filename') + ss.add_argument('folder', help='Destination subfolder (base folder is the ftp folder)') + + ss = self.subparsers.add_parser('download', help='Download a file from the ' + self.uav_name) + ss.add_argument('file', help='Remote filename (could include folder)') + ss.add_argument('save_file', help='Destination file on local computer') + + ss = self.subparsers.add_parser('download_dir', help='Download all files from a folder from the ' + self.uav_name) + ss.add_argument('dest', help='destination folder (on the local machine)') + ss.add_argument('folder', help='Remote subfolder (base folder is the ftp folder)') + + ss = self.subparsers.add_parser('upload_file_and_run', help='Upload and run software (for instance the Paparazzi autopilot)') + ss.add_argument('file', help='Filename of an executable') + ss.add_argument('folder', help='Remote destination folder (base folder is the ftp folder)') + + ss = self.subparsers.add_parser('mkdir', help='Make a new directory on the ' + self.uav_name) + ss.add_argument('folder', help='Remote subfolder (base folder is the ftp folder)') + + ss = self.subparsers.add_parser('rmdir', help='Remove a directory and all its files from the ' + self.uav_name) + ss.add_argument('folder', help='Remote subfolder (base folder is the ftp folder)') + + ss = self.subparsers.add_parser('insmod', help='Upload and insert kernel module') + ss.add_argument('file', help='Filename of *.ko kernel module') -# Connect with telnet and ftp, wait until login -def connect(host): - try: - tn = telnetlib.Telnet(host, timeout=3) - ftp = FTP(host) - ftp.login() - tn.read_until('# ') - return tn, ftp - except: - print('Could not connect to Parrot UAV (host: ' + host + ')') - if host == '192.168.42.1': - print("Check whether your WiFi is connected and don't forget pressing the power button 4 times after the Bebop has booted!") - exit(2) + # Main function to parse arguments + def parse_args(self): + args = self.parser.parse_args() + + # First connect to the drone + self.address = args.host + if self.connect() == False: + return False + + # Parse the command line arguments + if args.command == 'status': + self.status() + elif args.command == 'reboot': + self.reboot() + elif args.command == 'kill': + self.kill_program(args.program) + elif args.command == 'start': + self.start_program(self.upload_path + args.program) + elif args.command == 'upload': + self.upload_file(args.file, args.folder) + elif args.command == 'download': + self.download(args.file, args.save_file) + elif args.command == 'upload_file_and_run': + if hasattr(args, 'min_version') and hasattr(args, 'max_version'): + self.upload_and_run(args.file, args.folder, args.min_version, args.max_version) + else: + self.upload_and_run(args.file, args.folder) + elif args.command == 'mkdir': + self.create_directory(self.upload_path + args.folder) + elif args.command == 'rmdir': + self.remove_directory(self.upload_path + args.folder) + elif args.command == 'insmod': + self.insmod(args.file) + else: + if self.parse_extra_args(args) == False: + self.disconnect() + return False + + # Disconnect + self.disconnect() + return True + -# Close the telnet and ftp -def disconnect(tn, ftp): - tn.close() - ftp.close() diff --git a/sw/tools/parrot/swing.py b/sw/tools/parrot/swing.py new file mode 100755 index 0000000000..ac4771c2ac --- /dev/null +++ b/sw/tools/parrot/swing.py @@ -0,0 +1,48 @@ +#!/usr/bin/env python +# +# Copyright (C) 2012-2017 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, see +# . +# + +from __future__ import print_function +from parrot_utils import ParrotUtils + +class Swing(ParrotUtils): + uav_name = 'Swing' + address = '192.168.4.1' + version_file = None + upload_path = '/data/edu/' + prompt = '$ ' + + def uav_status(self): + print('Parrot version:\t\t' + self.check_version()) + + def init_extra_parser(self): + # nothing here + pass + + def parse_extra_args(self, args): + # nothing here + pass + + +if __name__ == "__main__": + swing = Swing() + swing.parse_args() + exit(0) +