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..019bab70d2 --- /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..26c6f56ef5 --- /dev/null +++ b/sw/airborne/boards/swing/actuators.c @@ -0,0 +1,125 @@ +/* + * 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 + +#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..42a72f2f7c --- /dev/null +++ b/sw/airborne/boards/swing/board.c @@ -0,0 +1,182 @@ +/* + * 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" + +#if 0 +static int stop_gracefull(char *process_name) +{ + /* "pidof" always in /bin on Bebop firmware tested 1.98, 2.0.57, no need for "which" */ + char pidof_commandline[200] = "/bin/pidof "; + strcat(pidof_commandline, process_name); + /* Bebop Busybox a + $ cat /proc/sys/kernel/pid_max + Gives max 32768, makes sure it never kills existing other process + */ + char pid[7] = ""; + int ret = 0; /* Return code of kill system call */ + FILE *fp; + + //while (ret == 0) { + fp = popen(pidof_commandline, "r"); + if (fp != NULL) { /* Could open the pidof with line */ + if (fgets(pid, sizeof(pid) - 1, fp) != NULL) { + //printf("Process ID deducted: \"%s\"\n", pid); + if (atoi(pid) > 0) { /* To make sure we end 0 > There is a real process id found */ + char kill_command_and_process[200] = "kill -STOP "; /* BTW there is no pkill on this Busybox */ + strcat(kill_command_and_process, pid); + ret = system(kill_command_and_process); + /* No need to wait */ + return ret; + } + } else { + ret = 256; /* Could not get handle */ + pclose(fp); + } + } else { + ret = 256; /* fp NULL, so no process, just return */ + return 0; + } + //} /* end while */ + return 0; +} +#endif + +/** + * Battery reading thread + * TODO something better ? + */ +static void *bat_read(void *data __attribute__((unused))) +{ + FILE *fp; + char path[1035]; + + 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 + usleep(100000); + } + + return NULL; +} + +/** + * Check button thread + * TODO something better ? + */ +static void *button_read(void *data __attribute__((unused))) +{ + struct input_event ev; + ssize_t n; + + 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 */ + printf("read\n"); + n = read(fd_button, &ev, sizeof(ev)); + printf("done %d\n", n); + 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)); + //ret = system("pstart delosd"); + //ret = system("pstart dragon-prog"); + ret = system("reboot.sh"); + exit(0); + } + + // Wait 100ms + //usleep(100000); + } + + return NULL; +} + +void board_init(void) +{ + /* + * Stop original process + * Don't kill to avoid automatic restart + * + * - /usr/bin/dragon-prog + * + */ + //stop_gracefull("delosd"); + //stop_gracefull("dragon-prog"); + 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/parrot_utils.py b/sw/tools/parrot/parrot_utils.py index 0de8f953a1..11a0f1e10f 100644 --- a/sw/tools/parrot/parrot_utils.py +++ b/sw/tools/parrot/parrot_utils.py @@ -86,17 +86,17 @@ def split_into_path_and_file(name): return name.rsplit('/', 1) # Execute a command -def execute_command(tn, command): +def execute_command(tn, command, prompt='#'): tn.write(command + '\n') - return tn.read_until('# ')[len(command) + 2:-4] + return tn.read_until(prompt+' ')[len(command) + 2:-4] # Check the version -def check_version(tn, directory): - return ParrotVersion(execute_command(tn, 'cat ' + directory + '/version.txt').strip()) +def check_version(tn, directory, prompt='#'): + return ParrotVersion(execute_command(tn, 'cat ' + directory + '/version.txt', prompt).strip()) # Check what currently is running on the drone -def check_running(tn): - ps_aux = execute_command(tn, 'ps') +def check_running(tn, prompt='#'): + ps_aux = execute_command(tn, 'ps', prompt) running = "" @@ -111,12 +111,12 @@ def check_running(tn): return running[1:] # Check the filesystem -def check_filesystem(tn): - return execute_command(tn, 'df -h') +def check_filesystem(tn, prompt='#'): + return execute_command(tn, 'df -h', prompt) # Reboot the drone -def reboot(tn): - execute_command(tn, 'reboot') +def reboot(tn, prompt='#'): + execute_command(tn, 'reboot', prompt) # Upload ftp and catch memory-full error def uploadfile(ftp, filename, content): @@ -131,12 +131,12 @@ def uploadfile(ftp, filename, content): # Connect with telnet and ftp, wait until login -def connect(host): +def connect(host, prompt='#'): try: tn = telnetlib.Telnet(host, timeout=3) ftp = FTP(host) ftp.login() - tn.read_until('# ') + tn.read_until(prompt+' ') return tn, ftp except: print('Could not connect to Parrot UAV (host: ' + host + ')') diff --git a/sw/tools/parrot/swing.py b/sw/tools/parrot/swing.py new file mode 100755 index 0000000000..15684b14da --- /dev/null +++ b/sw/tools/parrot/swing.py @@ -0,0 +1,198 @@ +#!/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 +import re +import argparse +import os +from time import sleep + +import parrot_utils + +PROMPT = '$' +FTP_ROOT = '/data/edu/' + + +# Read from config.ini TODO +def read_from_config(name, config=''): + if config == '': + config = parrot_utils.execute_command('cat /data/config.ini', prompt=PROMPT) + search = re.search(name + '[^=]+=[\r\n\t ]([^\r\n\t ]+)', config) + if search is None: + return '' + else: + return search.group(1) + +# 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', prompt=PROMPT) + else: + parrot_utils.execute_command('sed -i "s/\(' + name + ' *= *\).*/\\1' + value + '/g" /data/config.ini', prompt=PROMPT) + + +def swing_status(): + #config_ini = parrot_utils.execute_command(tn, 'cat /data/config.ini', prompt=PROMPT) + + #print('==================== Swing Status ====================') + #print('Version:\t\t' + str(parrot_utils.check_version(tn, '', prompt=PROMPT))) + # Request the filesystem status + print('\n=================== Filesystem Status =======================') + print(parrot_utils.check_filesystem(tn, prompt=PROMPT)) + + +# Parse the arguments +parser = argparse.ArgumentParser(description='Swing python helper. Use swing.py -h for help') +parser.add_argument('--host', metavar='HOST', default='192.168.4.1', + help='the ip address of Swing') +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 Swing') +subparsers.add_parser('reboot', help='Reboot the Swing') +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 = subparsers.add_parser('upload_file', help='Upload a file to the Swing') +subparser_upload.add_argument('file', help='Filename') +subparser_upload.add_argument('folder', help='Destination subfolder (base destination folder is '+FTP_ROOT+')') +subparser_download = subparsers.add_parser('download_file', help='Download a file from the Swing') +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 '+FTP_ROOT+')') +subparser_download_dir = subparsers.add_parser('download_dir', help='Download all files from a folder from the Swing') +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 '+FTP_ROOT+')') +subparser_rm_dir = subparsers.add_parser('rm_dir', help='Remove a directory and all its files from the Swing') +subparser_rm_dir.add_argument('folder', help='Remote subfolder (base folder is '+FTP_ROOT+')') +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 Swing') +subparser_start.add_argument('program', help='the program to start') +subparser_kill = subparsers.add_parser('kill', help='Kill a program on the Swing') +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, prompt=PROMPT) + +# Check the Swing status +if args.command == 'status': + print("Connected to Swing at " + args.host) + swing_status() + +# Reboot the drone +elif args.command == 'reboot': + parrot_utils.reboot(tn, prompt=PROMPT) + print('The Swing is rebooting...') + +# Kill a program +elif args.command == 'kill': + parrot_utils.execute_command(tn, 'killall -9 ' + args.program, prompt=PROMPT) + print('Program "' + args.program + '" is now killed') + +# Start a program +elif args.command == 'start': + parrot_utils.execute_command(tn, args.start + ' &', prompt=PROMPT) + 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 "+ FTP_ROOT + modfile[1], prompt=PROMPT)) + +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], prompt=PROMPT) + sleep(1) + parrot_utils.execute_command(tn, "mkdir -p " + FTP_ROOT + args.folder, prompt=PROMPT) + 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) + parrot_utils.execute_command(tn, "chmod 777 " + FTP_ROOT + args.folder + "/" + f[1], prompt=PROMPT) + parrot_utils.execute_command(tn, FTP_ROOT + args.folder + "/" + f[1] + " > /dev/null 2>&1 &", prompt=PROMPT) + print("#pragma message: Upload and Start of ap.elf to Swing Succes!") + +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 " + FTP_ROOT + args.folder, prompt=PROMPT) + print('Uploading \'' + f[1] + "\' from " + f[0] + " to " + FTP_ROOT + args.folder) + parrot_utils.uploadfile(ftp, args.folder + "/" + f[1], file(args.file, "rb")) + print("#pragma message: Upload of " + f[1] + " to Swing Succes!") + +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 Swing 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 Swing Failed!") + else: + fd.close() + +elif args.command == 'download_dir': + # Split filename and path + files = parrot_utils.execute_command(tn, 'find ' + FTP_ROOT + args.folder + ' -name \'*.*\'', prompt=PROMPT) + # 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 Swing Failed!") + else: + fd.close() + print("#pragma message: End download of folder " + args.folder + " from Swing") + +elif args.command == 'rm_dir': + # Split filename and path + print("Deleting folder " + FTP_ROOT + args.folder + " from Swing") + print(parrot_utils.execute_command(tn, 'rm -r ' + FTP_ROOT + args.folder, prompt=PROMPT)) + + + +# Close the telnet and python script +parrot_utils.disconnect(tn, ftp) +exit(0)