diff --git a/conf/Makefile.bebop b/conf/Makefile.bebop new file mode 100644 index 0000000000..d91ea63382 --- /dev/null +++ b/conf/Makefile.bebop @@ -0,0 +1,39 @@ +# Hey Emacs, this is a -*- makefile -*- +# +# Copyright (C) 2014 Freek van Tienen +# +# This file is part of paparazzi. +# +# paparazzi is free software; you can redistribute it and/or modify +# it under the terms of the GNU General Public License as published by +# the Free Software Foundation; either version 2, or (at your option) +# any later version. +# +# paparazzi is distributed in the hope that it will be useful, +# but WITHOUT ANY WARRANTY; without even the implied warranty of +# MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the +# GNU General Public License for more details. +# +# You should have received a copy of the GNU General Public License +# along with paparazzi; see the file COPYING. If not, see +# . +# + +# +# This Makefile uses the generic Makefile.linux and adds upload rules for the ARDrone2 +# + +include $(PAPARAZZI_SRC)/conf/Makefile.linux + +DRONE = $(PAPARAZZI_SRC)/sw/ext/bebop.py + +# Allow modules or other raw makefiles to add actions to the upload +upload_extra: + + +# Program the device and start it. +upload program: upload_extra $(OBJDIR)/$(TARGET).elf + $(Q)$(DRONE) --host=$(HOST) upload_file_and_run $(OBJDIR)/$(TARGET).elf $(SUB_DIR) + +# Listing of phony targets. +.PHONY : upload_extra upload program diff --git a/conf/airframes/bebop.xml b/conf/airframes/bebop.xml new file mode 100644 index 0000000000..ddef396943 --- /dev/null +++ b/conf/airframes/bebop.xml @@ -0,0 +1,217 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
+ + + + + + + + + + + + +
+ + + + + + + + + +
+ + + + + + + + + + + + + + + + +
+ + + +
+ + + + + + +
+ +
+ + +
+ +
+ + + + + + + + + + + + + + + + + + + + + + +
+ +
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
+ +
+ + + + + +
+ +
+ + + + + + + +
+ +
+ + + + +
+ +
+ + + + +
+ +
+ + + + + +
+ diff --git a/conf/boards/bebop.makefile b/conf/boards/bebop.makefile new file mode 100644 index 0000000000..1c12666850 --- /dev/null +++ b/conf/boards/bebop.makefile @@ -0,0 +1,39 @@ +# Hey Emacs, this is a -*- makefile -*- +# +# bebop.makefile +# +# http://wiki.paparazziuav.org/wiki/Bebop +# + +BOARD=bebop +BOARD_CFG=\"boards/$(BOARD).h\" + +ARCH=linux +$(TARGET).ARCHDIR = $(ARCH) +# include conf/Makefile.bebop (with specific upload rules) instead of only Makefile.linux: +ap.MAKEFILE = bebop + +# ----------------------------------------------------------------------- +USER=foobar +HOST?=192.168.42.1 +SUB_DIR=paparazzi +FTP_DIR=/data/ftp +TARGET_DIR=$(FTP_DIR)/$(SUB_DIR) +# ----------------------------------------------------------------------- + +# The datalink default uses UDP +MODEM_HOST ?= \"192.168.42.255\" + +# The GPS sensor is connected internally +GPS_PORT ?= UART1 +GPS_BAUD ?= B230400 +$(TARGET).CFLAGS += -DUART1_DEV=\"/dev/ttyPA1\" + +# ----------------------------------------------------------------------- + +# default LED configuration +RADIO_CONTROL_LED ?= none +BARO_LED ?= none +AHRS_ALIGNER_LED ?= 1 +GPS_LED ?= none +SYS_TIME_LED ?= 0 diff --git a/conf/conf_example.xml b/conf/conf_example.xml index 2b284b0b34..e12f4b9bef 100644 --- a/conf/conf_example.xml +++ b/conf/conf_example.xml @@ -219,6 +219,17 @@ settings_modules="" gui_color="blue" /> + +# +# +# +# +# +# +# +# +# +# +# + +IMU_CFLAGS = -DUSE_IMU -DIMU_BEBOP -DIMU_TYPE_H=\"imu/imu_bebop.h\" +IMU_SRCS = $(SRC_SUBSYSTEMS)/imu.c $(SRC_SUBSYSTEMS)/imu/imu_bebop.c +IMU_SRCS += peripherals/mpu60x0.c peripherals/mpu60x0_i2c.c +IMU_SRCS += peripherals/ak8963.c + + +BEBOP_MAG_I2C_DEV ?= i2c1 +BEBOP_MPU_I2C_DEV ?= i2c2 + +BEBOP_MAG_I2C_DEV_UPPER=$(shell echo $(BEBOP_MAG_I2C_DEV) | tr a-z A-Z) +BEBOP_MAG_I2C_DEV_LOWER=$(shell echo $(BEBOP_MAG_I2C_DEV) | tr A-Z a-z) +BEBOP_MPU_I2C_DEV_UPPER=$(shell echo $(BEBOP_MPU_I2C_DEV) | tr a-z A-Z) +BEBOP_MPU_I2C_DEV_LOWER=$(shell echo $(BEBOP_MPU_I2C_DEV) | tr A-Z a-z) + +IMU_CFLAGS += -DBEBOP_MAG_I2C_DEV=$(BEBOP_MAG_I2C_DEV_LOWER) -DBEBOP_MPU_I2C_DEV=$(BEBOP_MPU_I2C_DEV_LOWER) +IMU_CFLAGS += -DUSE_$(BEBOP_MAG_I2C_DEV_UPPER)=1 -DUSE_$(BEBOP_MPU_I2C_DEV_UPPER)=1 + +ap.CFLAGS += $(IMU_CFLAGS) +ap.srcs += $(IMU_SRCS) + +test_imu.CFLAGS += $(IMU_CFLAGS) +test_imu.srcs += $(IMU_SRCS) + +# +# NPS simulator +# +include $(CFG_SHARED)/imu_nps.makefile diff --git a/conf/messages.xml b/conf/messages.xml index 77c1b40a5d..3cedc9b6bc 100644 --- a/conf/messages.xml +++ b/conf/messages.xml @@ -1907,7 +1907,21 @@ - + + + + + + + + + + + + + + + diff --git a/conf/telemetry/default_rotorcraft.xml b/conf/telemetry/default_rotorcraft.xml index e23e09bda3..b83f9e86d5 100644 --- a/conf/telemetry/default_rotorcraft.xml +++ b/conf/telemetry/default_rotorcraft.xml @@ -29,6 +29,7 @@ + diff --git a/sw/airborne/boards/bebop.h b/sw/airborne/boards/bebop.h new file mode 100644 index 0000000000..c0d1135ce9 --- /dev/null +++ b/sw/airborne/boards/bebop.h @@ -0,0 +1,40 @@ +/* + * Copyright (C) 2014 Freek van Tienen + * + * This file is part of paparazzi. + * + * paparazzi is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation; either version 2, or (at your option) + * any later version. + * + * paparazzi is distributed in the hope that it will be useful, + * but WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + * GNU General Public License for more details. + * + * You should have received a copy of the GNU General Public License + * along with paparazzi; see the file COPYING. If not, write to + * the Free Software Foundation, 59 Temple Place - Suite 330, + * Boston, MA 02111-1307, USA. + * + */ + +#ifndef CONFIG_BEBOP +#define CONFIG_BEBOP + +#define BOARD_BEBOP + +/* Default actuators driver */ +#define DEFAULT_ACTUATORS "boards/bebop/actuators.h" +#define ActuatorDefaultSet(_x,_y) ActuatorsBebopSet(_x,_y) +#define ActuatorsDefaultInit() ActuatorsBebopInit() +#define ActuatorsDefaultCommit() ActuatorsBebopCommit() + + +/* by default activate onboard baro */ +#ifndef USE_BARO_BOARD +#define USE_BARO_BOARD 1 +#endif + +#endif /* CONFIG_BEBOP */ diff --git a/sw/airborne/boards/bebop/actuators.c b/sw/airborne/boards/bebop/actuators.c new file mode 100644 index 0000000000..c647bcb4f2 --- /dev/null +++ b/sw/airborne/boards/bebop/actuators.c @@ -0,0 +1,179 @@ +/* + * + * Copyright (C) 2014 Freek van Tienen + * + * This file is part of paparazzi. + * + * paparazzi is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation; either version 2, or (at your option) + * any later version. + * + * paparazzi is distributed in the hope that it will be useful, + * but WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + * GNU General Public License for more details. + * + * You should have received a copy of the GNU General Public License + * along with paparazzi; see the file COPYING. If not, write to + * the Free Software Foundation, 59 Temple Place - Suite 330, + * Boston, MA 02111-1307, USA. + */ + +/** + * @file boards/bebop/actuators.c + * Actuator driver for the bebop + */ + +#include "subsystems/actuators.h" +#include "subsystems/actuators/motor_mixing.h" +#include "subsystems/electrical.h" +#include "actuators.h" +#include "led_hw.h" +#include "autopilot.h" + +#if PERIODIC_TELEMETRY +#include "subsystems/datalink/telemetry.h" +#include "firmwares/rotorcraft/stabilization.h" + +static void send_actuators_bebop(struct transport_tx *trans, struct link_device *dev) +{ + pprz_msg_send_BEBOP_ACTUATORS(trans, dev, AC_ID, + &stabilization_cmd[COMMAND_THRUST], + &stabilization_cmd[COMMAND_ROLL], + &stabilization_cmd[COMMAND_PITCH], + &stabilization_cmd[COMMAND_YAW], + &actuators_bebop.rpm_ref[0], + &actuators_bebop.rpm_ref[1], + &actuators_bebop.rpm_ref[2], + &actuators_bebop.rpm_ref[3], + &actuators_bebop.rpm_obs[0], + &actuators_bebop.rpm_obs[1], + &actuators_bebop.rpm_obs[2], + &actuators_bebop.rpm_obs[3]); +} +#endif + +uint32_t led_hw_values; +struct ActuatorsBebop actuators_bebop; +static uint8_t actuators_bebop_checksum(uint8_t *bytes, uint8_t size); + +void actuators_bebop_init(void) +{ + /* Initialize the I2C connection */ + actuators_bebop.i2c_trans.slave_addr = ACTUATORS_BEBOP_ADDR; + actuators_bebop.i2c_trans.status = I2CTransDone; + actuators_bebop.led = 0; + +#if PERIODIC_TELEMETRY + register_periodic_telemetry(DefaultPeriodic, "ACTUATORS_BEBOP", send_actuators_bebop); +#endif +} + +void actuators_bebop_commit(void) +{ + // Receive the status + actuators_bebop.i2c_trans.buf[0] = ACTUATORS_BEBOP_GET_OBS_DATA; + i2c_transceive(&i2c1, &actuators_bebop.i2c_trans, actuators_bebop.i2c_trans.slave_addr, 1, 13); + + // Update status + electrical.vsupply = (actuators_bebop.i2c_trans.buf[9] + (actuators_bebop.i2c_trans.buf[8] << 8)) / 100; + actuators_bebop.rpm_obs[0] = (actuators_bebop.i2c_trans.buf[1] + (actuators_bebop.i2c_trans.buf[0] << 8)); + actuators_bebop.rpm_obs[1] = (actuators_bebop.i2c_trans.buf[3] + (actuators_bebop.i2c_trans.buf[2] << 8)); + actuators_bebop.rpm_obs[2] = (actuators_bebop.i2c_trans.buf[5] + (actuators_bebop.i2c_trans.buf[4] << 8)); + actuators_bebop.rpm_obs[3] = (actuators_bebop.i2c_trans.buf[7] + (actuators_bebop.i2c_trans.buf[6] << 8)); + + // Saturate the bebop motors + //actuators_bebop_saturate(); + + // When detected a suicide + actuators_bebop.i2c_trans.buf[10] = actuators_bebop.i2c_trans.buf[10] & 0x7; + if (actuators_bebop.i2c_trans.buf[11] == 2 && actuators_bebop.i2c_trans.buf[10] != 1) { + autopilot_set_motors_on(FALSE); + } + + // Start the motors + if (actuators_bebop.i2c_trans.buf[10] != 4 && actuators_bebop.i2c_trans.buf[10] != 2 && autopilot_motors_on) { + // Reset the error + actuators_bebop.i2c_trans.buf[0] = ACTUATORS_BEBOP_CLEAR_ERROR; + i2c_transmit(&i2c1, &actuators_bebop.i2c_trans, actuators_bebop.i2c_trans.slave_addr, 1); + + // Start the motors + actuators_bebop.i2c_trans.buf[0] = ACTUATORS_BEBOP_START_PROP; + i2c_transmit(&i2c1, &actuators_bebop.i2c_trans, actuators_bebop.i2c_trans.slave_addr, 1); + } + // Stop the motors + else if (actuators_bebop.i2c_trans.buf[10] == 4 && !autopilot_motors_on) { + actuators_bebop.i2c_trans.buf[0] = ACTUATORS_BEBOP_STOP_PROP; + i2c_transmit(&i2c1, &actuators_bebop.i2c_trans, actuators_bebop.i2c_trans.slave_addr, 1); + } else if (actuators_bebop.i2c_trans.buf[10] == 4 && autopilot_motors_on) { + // Send the commands + actuators_bebop.i2c_trans.buf[0] = ACTUATORS_BEBOP_SET_REF_SPEED; + actuators_bebop.i2c_trans.buf[1] = actuators_bebop.rpm_ref[0] >> 8; + actuators_bebop.i2c_trans.buf[2] = actuators_bebop.rpm_ref[0] & 0xFF; + actuators_bebop.i2c_trans.buf[3] = actuators_bebop.rpm_ref[1] >> 8; + actuators_bebop.i2c_trans.buf[4] = actuators_bebop.rpm_ref[1] & 0xFF; + actuators_bebop.i2c_trans.buf[5] = actuators_bebop.rpm_ref[2] >> 8; + actuators_bebop.i2c_trans.buf[6] = actuators_bebop.rpm_ref[2] & 0xFF; + actuators_bebop.i2c_trans.buf[7] = actuators_bebop.rpm_ref[3] >> 8; + actuators_bebop.i2c_trans.buf[8] = actuators_bebop.rpm_ref[3] & 0xFF; + actuators_bebop.i2c_trans.buf[9] = 0x00; //UNK? +#pragma GCC diagnostic push +#pragma GCC diagnostic ignored "-Wcast-qual" + actuators_bebop.i2c_trans.buf[10] = actuators_bebop_checksum((uint8_t *)actuators_bebop.i2c_trans.buf, 9); +#pragma GCC diagnostic pop + i2c_transmit(&i2c1, &actuators_bebop.i2c_trans, actuators_bebop.i2c_trans.slave_addr, 11); + } + + // Update the LEDs + if (actuators_bebop.led != (led_hw_values & 0x3)) { + actuators_bebop.i2c_trans.buf[0] = ACTUATORS_BEBOP_TOGGLE_GPIO; + actuators_bebop.i2c_trans.buf[1] = (led_hw_values & 0x3); + i2c_transmit(&i2c1, &actuators_bebop.i2c_trans, actuators_bebop.i2c_trans.slave_addr, 2); + + actuators_bebop.led = led_hw_values & 0x3; + } +} + +static uint8_t actuators_bebop_checksum(uint8_t *bytes, uint8_t size) +{ + uint8_t checksum = 0; + for (int i = 0; i < size; i++) { + checksum = checksum ^ bytes[i]; + } + + return checksum; +} + +/*static void actuators_bebop_saturate(void) { + // Find the lowest and highest commands + int32_t max_cmd = 9000; // Should be gotton from airframe file per motor + int32_t min_cmd = 3000; // Should be gotton from airframe file per motor + for(int i = 0; i < 4; i++) { + if(actuators_bebop.rpm_ref[i] > max_cmd) + max_cmd = actuators_bebop.rpm_ref[i]; + if(actuators_bebop.rpm_ref[i] < min_cmd) + min_cmd = actuators_bebop.rpm_ref[i]; + } + + // Find the maximum motor command (Saturated motor or either MOTOR_MIXING_MAX_MOTOR) + int32_t max_motor = 9000; + for(int i = 0; i < 4; i++) { + if(actuators_bebop.rpm_obs[i] & (1<<15) && max_cmd > (actuators_bebop.rpm_obs[i] & ~(1<<15))) + max_motor = actuators_bebop.rpm_obs[i] & ~(1<<15); + } + + // Saturate the offsets + if(max_cmd > max_motor) { + int32_t saturation_offset = 9000 - max_cmd; + for(int i = 0; i < 4; i++) + actuators_bebop.rpm_ref[i] += saturation_offset; + motor_mixing.nb_saturation++; + } + else if(min_cmd < 3000) { + int32_t saturation_offset = 3000 - min_cmd; + for(int i = 0; i < 4; i++) + actuators_bebop.rpm_ref[i] += saturation_offset; + motor_mixing.nb_saturation++; + } +}*/ diff --git a/sw/airborne/boards/bebop/actuators.h b/sw/airborne/boards/bebop/actuators.h new file mode 100644 index 0000000000..f10bbb3642 --- /dev/null +++ b/sw/airborne/boards/bebop/actuators.h @@ -0,0 +1,62 @@ +/* + * + * Copyright (C) 2014 Freek van Tienen + * + * This file is part of paparazzi. + * + * paparazzi is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation; either version 2, or (at your option) + * any later version. + * + * paparazzi is distributed in the hope that it will be useful, + * but WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + * GNU General Public License for more details. + * + * You should have received a copy of the GNU General Public License + * along with paparazzi; see the file COPYING. If not, write to + * the Free Software Foundation, 59 Temple Place - Suite 330, + * Boston, MA 02111-1307, USA. + */ + +/** + * @file boards/bebop/actuators.h + * Actuator driver for the bebop + */ + +#ifndef ACTUATORS_BEBOP_RAW_H_ +#define ACTUATORS_BEBOP_RAW_H_ + +#include +#include "mcu_periph/i2c.h" + +#define ACTUATORS_BEBOP_ADDR 0x8 + +// I2C Commands +#define ACTUATORS_BEBOP_SET_REF_SPEED 0x02 ///< Set reference speed +#define ACTUATORS_BEBOP_GET_OBS_DATA 0x20 ///< Get observation data +#define ACTUATORS_BEBOP_START_PROP 0x40 ///< Start the propellers +#define ACTUATORS_BEBOP_TOGGLE_GPIO 0x4D ///< Toggle GPIO (reset, red led, green led) +#define ACTUATORS_BEBOP_STOP_PROP 0x60 ///< Stop the propellers +#define ACTUATORS_BEBOP_CLEAR_ERROR 0x80 ///< Clear all current erros +#define ACTUATORS_BEBOP_PLAY_SOUND 0x82 ///< Play a sound (0=stop, 1=short beep, 2=boot beep, 3=Be-Bop-Ah-Lula, negative=repeat) +#define ACTUATORS_BEBOP_GET_INFO 0xA0 ///< Get version information + + +struct ActuatorsBebop { + struct i2c_transaction i2c_trans; ///< I2C transaction for communicating with the bebop BLDC driver + uint16_t rpm_ref[4]; ///< Reference RPM + uint16_t rpm_obs[4]; ///< Observed RPM + uint8_t led; ///< Current led status +}; + +#define ActuatorsBebopSet(_i, _v) { actuators_bebop.rpm_ref[_i] = _v; } +#define ActuatorsBebopCommit() actuators_bebop_commit(); +#define ActuatorsBebopInit() actuators_bebop_init(); + +extern struct ActuatorsBebop actuators_bebop; +extern void actuators_bebop_commit(void); +extern void actuators_bebop_init(void); + +#endif /* ACTUATORS_BEBOP_RAW_H_ */ diff --git a/sw/airborne/boards/bebop/baro_board.h b/sw/airborne/boards/bebop/baro_board.h new file mode 100644 index 0000000000..2a91d3ebe9 --- /dev/null +++ b/sw/airborne/boards/bebop/baro_board.h @@ -0,0 +1,38 @@ +/* + * Copyright (C) 2014 Freek van Tienen + * + * This file is part of Paparazzi. + * + * Paparazzi is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation; either version 2, or (at your option) + * any later version. + * + * Paparazzi is distributed in the hope that it will be useful, + * but WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + * GNU General Public License for more details. + * + * You should have received a copy of the GNU General Public License + * along with Paparazzi; see the file COPYING. If not, write to + * the Free Software Foundation, 59 Temple Place - Suite 330, + * Boston, MA 02111-1307, USA. + */ + +/** + * @file boards/bebop/baro_board.h + * Paparazzi Bebop Baro Sensor implementation using the ms5611. + */ + +#ifndef BOARDS_BEBOP_BARO_H +#define BOARDS_BEBOP_BARO_H + +// only for printing the baro type during compilation +#ifndef BARO_BOARD +#define BARO_BOARD BARO_MS5611_I2C +#endif + +extern void baro_event(void); +#define BaroEvent baro_event + +#endif /* BOARDS_BEBOP_BARO_H */ diff --git a/sw/airborne/boards/bebop/electrical.c b/sw/airborne/boards/bebop/electrical.c new file mode 100644 index 0000000000..1d5a499f71 --- /dev/null +++ b/sw/airborne/boards/bebop/electrical.c @@ -0,0 +1,42 @@ +/* + * + * Copyright (C) 2014 Freek van Tienen + * + * This file is part of paparazzi. + * + * paparazzi is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation; either version 2, or (at your option) + * any later version. + * + * paparazzi is distributed in the hope that it will be useful, + * but WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + * GNU General Public License for more details. + * + * You should have received a copy of the GNU General Public License + * along with paparazzi; see the file COPYING. If not, write to + * the Free Software Foundation, 59 Temple Place - Suite 330, + * Boston, MA 02111-1307, USA. + * + */ + +/** + * @file boards/bebop/electrical.c + * Dummy electrical status readings for the bebop. + * Because the voltage measurements is done trough the motor controllers. + */ + +#include "subsystems/electrical.h" +#include + +struct Electrical electrical; + +void electrical_init(void) +{ + // First we try to kill the dragon-prog and its respawner if it is running (done here because initializes first) + int ret = system("killall -9 watchdog.sh; killall -9 dragon-prog"); + (void) ret; +} + +void electrical_periodic(void) { } diff --git a/sw/airborne/subsystems/imu/imu_bebop.c b/sw/airborne/subsystems/imu/imu_bebop.c new file mode 100644 index 0000000000..f48b729238 --- /dev/null +++ b/sw/airborne/subsystems/imu/imu_bebop.c @@ -0,0 +1,137 @@ +/* + * Copyright (C) 2014 Freek van Tienen + * + * This file is part of paparazzi. + * + * paparazzi is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation; either version 2, or (at your option) + * any later version. + * + * paparazzi is distributed in the hope that it will be useful, + * but WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + * GNU General Public License for more details. + * + * You should have received a copy of the GNU General Public License + * along with paparazzi; see the file COPYING. If not, write to + * the Free Software Foundation, 59 Temple Place - Suite 330, + * Boston, MA 02111-1307, USA. + */ + +/** + * @file subsystems/imu/imu_bebop.c + * Driver for the Bebop magnetometer, accelerometer and gyroscope + */ + +#include "subsystems/imu.h" +#include "mcu_periph/i2c.h" + + +/* defaults suitable for Bebop */ +#ifndef BEBOP_MAG_I2C_DEV +#define BEBOP_MAG_I2C_DEV i2c1 +#endif +PRINT_CONFIG_VAR(BEBOP_MAG_I2C_DEV) + +#ifndef BEBOP_MPU_I2C_DEV +#define BEBOP_MPU_I2C_DEV i2c2 +#endif +PRINT_CONFIG_VAR(BEBOP_MPU_I2C_DEV) + +#if !defined BEBOP_LOWPASS_FILTER && !defined BEBOP_SMPLRT_DIV +#if (PERIODIC_FREQUENCY == 60) || (PERIODIC_FREQUENCY == 120) +/* Accelerometer: Bandwidth 44Hz, Delay 4.9ms + * Gyroscope: Bandwidth 42Hz, Delay 4.8ms sampling 1kHz + */ +#define BEBOP_LOWPASS_FILTER MPU60X0_DLPF_42HZ +#define BEBOP_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 BEBOP_LOWPASS_FILTER MPU60X0_DLPF_256HZ +#define BEBOP_SMPLRT_DIV 3 +PRINT_CONFIG_MSG("Gyro/Accel output rate is 2kHz at 8kHz internal sampling") +#endif +#endif +PRINT_CONFIG_VAR(BEBOP_SMPLRT_DIV) +PRINT_CONFIG_VAR(BEBOP_LOWPASS_FILTER) + +#ifndef BEBOP_GYRO_RANGE +#define BEBOP_GYRO_RANGE MPU60X0_GYRO_RANGE_1000 +#endif +PRINT_CONFIG_VAR(BEBOP_GYRO_RANGE) + +#ifndef BEBOP_ACCEL_RANGE +#define BEBOP_ACCEL_RANGE MPU60X0_ACCEL_RANGE_8G +#endif +PRINT_CONFIG_VAR(BEBOP_ACCEL_RANGE) + +/** Basic Navstik IMU data */ +struct ImuBebop imu_bebop; + +/** + * Navstik IMU initializtion of the MPU-60x0 and HMC58xx + */ +void imu_impl_init(void) +{ + imu_bebop.accel_valid = FALSE; + imu_bebop.gyro_valid = FALSE; + imu_bebop.mag_valid = FALSE; + + /* MPU-60X0 */ + mpu60x0_i2c_init(&imu_bebop.mpu, &(BEBOP_MPU_I2C_DEV), MPU60X0_ADDR >> 1); + imu_bebop.mpu.config.smplrt_div = BEBOP_SMPLRT_DIV; + imu_bebop.mpu.config.dlpf_cfg = BEBOP_LOWPASS_FILTER; + imu_bebop.mpu.config.gyro_range = BEBOP_GYRO_RANGE; + imu_bebop.mpu.config.accel_range = BEBOP_ACCEL_RANGE; + + /* AKM8963 */ + ak8963_init(&imu_bebop.ak, &(BEBOP_MAG_I2C_DEV), AK8963_ADDR >> 1); +} + +/** + * Handle all the periodic tasks of the Navstik IMU components. + * Read the MPU60x0 every periodic call and the HMC58XX every 10th call. + */ +void imu_periodic(void) +{ + // Start reading the latest gyroscope data + mpu60x0_i2c_periodic(&imu_bebop.mpu); + + // AKM8963 + ak8963_periodic(&imu_bebop.ak); +} + +/** + * 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_bebop_event(void) +{ + /* MPU-60x0 event taks */ + mpu60x0_i2c_event(&imu_bebop.mpu); + + if (imu_bebop.mpu.data_available) { + /* default orientation of the MPU is upside down sor corrigate this here */ + RATES_ASSIGN(imu.gyro_unscaled, imu_bebop.mpu.data_rates.rates.p, -imu_bebop.mpu.data_rates.rates.q, -imu_bebop.mpu.data_rates.rates.r); + VECT3_ASSIGN(imu.accel_unscaled, imu_bebop.mpu.data_accel.vect.x, -imu_bebop.mpu.data_accel.vect.y, -imu_bebop.mpu.data_accel.vect.z); + + imu_bebop.mpu.data_available = FALSE; + imu_bebop.gyro_valid = TRUE; + imu_bebop.accel_valid = TRUE; + } + + /* AKM8963 event task */ + ak8963_event(&imu_bebop.ak); + + if (imu_bebop.ak.data_available) { + //32760 to -32760 + VECT3_ASSIGN(imu.mag_unscaled, -imu_bebop.ak.data.vect.y, imu_bebop.ak.data.vect.x, imu_bebop.ak.data.vect.z); + + imu_bebop.ak.data_available = FALSE; + imu_bebop.mag_valid = TRUE; + } +} diff --git a/sw/airborne/subsystems/imu/imu_bebop.h b/sw/airborne/subsystems/imu/imu_bebop.h new file mode 100644 index 0000000000..000f7aeda4 --- /dev/null +++ b/sw/airborne/subsystems/imu/imu_bebop.h @@ -0,0 +1,101 @@ +/* + * Copyright (C) 2014 Freek van Tienen + * + * This file is part of paparazzi. + * + * paparazzi is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation; either version 2, or (at your option) + * any later version. + * + * paparazzi is distributed in the hope that it will be useful, + * but WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + * GNU General Public License for more details. + * + * You should have received a copy of the GNU General Public License + * along with paparazzi; see the file COPYING. If not, write to + * the Free Software Foundation, 59 Temple Place - Suite 330, + * Boston, MA 02111-1307, USA. + */ + +/** + * @file subsystems/imu/imu_bebop.h + * Interface for the Bebop magnetometer, accelerometer and gyroscope + */ + + +#ifndef IMU_BEBOP_H +#define IMU_BEBOP_H + +#include "generated/airframe.h" +#include "subsystems/imu.h" + +#include "peripherals/ak8963.h" +#include "peripherals/mpu60x0_i2c.h" + +/** default gyro sensitivy and neutral from the datasheet + * MPU with 1000 deg/s has 32.8 LSB/(deg/s) + * sens = 1/32.8 * pi/180 * 2^INT32_RATE_FRAC + * sens = 1/32.8 * pi/180 * 4096 = 2.17953 + */ +#if !defined IMU_GYRO_P_SENS & !defined IMU_GYRO_Q_SENS & !defined IMU_GYRO_R_SENS +// FIXME +#define IMU_GYRO_P_SENS 2.17953 +#define IMU_GYRO_P_SENS_NUM 18271 +#define IMU_GYRO_P_SENS_DEN 8383 +#define IMU_GYRO_Q_SENS 2.17953 +#define IMU_GYRO_Q_SENS_NUM 18271 +#define IMU_GYRO_Q_SENS_DEN 8383 +#define IMU_GYRO_R_SENS 2.17953 +#define IMU_GYRO_R_SENS_NUM 18271 +#define IMU_GYRO_R_SENS_DEN 8383 +#endif + +/** default accel sensitivy from the datasheet + * MPU with 8g has 4096 LSB/g + * sens = 9.81 [m/s^2] / 4096 [LSB/g] * 2^INT32_ACCEL_FRAC = 2.4525 + */ +#if !defined IMU_ACCEL_X_SENS & !defined IMU_ACCEL_Y_SENS & !defined IMU_ACCEL_Z_SENS +// FIXME +#define IMU_ACCEL_X_SENS 2.4525 +#define IMU_ACCEL_X_SENS_NUM 981 +#define IMU_ACCEL_X_SENS_DEN 400 +#define IMU_ACCEL_Y_SENS 2.4525 +#define IMU_ACCEL_Y_SENS_NUM 981 +#define IMU_ACCEL_Y_SENS_DEN 400 +#define IMU_ACCEL_Z_SENS 2.4525 +#define IMU_ACCEL_Z_SENS_NUM 981 +#define IMU_ACCEL_Z_SENS_DEN 400 +#endif + +/** Everything that is in the bebop IMU */ +struct ImuBebop { + volatile uint8_t accel_valid; ///< Shows when the accelerometer is valid + volatile uint8_t gyro_valid; ///< Shows when the gyrometer is valid + volatile uint8_t mag_valid; ///< Shows when the Magneto is valid + struct Mpu60x0_I2c mpu; ///< The MPU gyro/accel device + struct Ak8963 ak; ///< The AK8963 mag +}; + +extern struct ImuBebop imu_bebop; +extern void imu_bebop_event(void); + +static inline void ImuEvent(void (* _gyro_handler)(void), void (* _accel_handler)(void), void (* _mag_handler)(void)) +{ + imu_bebop_event(); + if (imu_bebop.gyro_valid) { + imu_bebop.gyro_valid = FALSE; + _gyro_handler(); + } + if (imu_bebop.accel_valid) { + imu_bebop.accel_valid = FALSE; + _accel_handler(); + } + if (imu_bebop.mag_valid) { + imu_bebop.mag_valid = FALSE; + _mag_handler(); + } +} + +#endif /* IMU_BEBOP_H */ diff --git a/sw/ext/bebop.py b/sw/ext/bebop.py new file mode 100755 index 0000000000..da9c398967 --- /dev/null +++ b/sw/ext/bebop.py @@ -0,0 +1,488 @@ +#!/usr/bin/env python + +from __future__ import print_function +import re +import argparse +import socket +import telnetlib +import os +from time import sleep +from ftplib import FTP + + +# Check if IP is valid +def is_ip(address): + try: + socket.inet_aton(address) + ip = True + except socket.error: + ip = False + return ip + +# Execute a command +def execute_command(command): + tn.write(command + '\n') + return tn.read_until('# ')[len(command) + 2:-4] + +# Helper function +def split_into_path_and_file(name): + if name.count('/') <= 0: + return ["./", name] + return name.rsplit('/', 1) + +# Read from config.ini +def read_from_config(name, config=''): + if config == '': + config = 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) + +# Write to config +def write_to_config(name, value): + if read_from_config(name) == '': + execute_command('echo "' + name + ' = ' + value + '\" >> /data/config.ini') + else: + execute_command('sed -i "s/\(' + name + ' *= *\).*/\\1' + value + '/g" /data/config.ini') + +# Check the version +def check_version(): + return execute_command('cat /firmware/version.txt') + +# Check what currently is running on the drone +def check_running(): + ps_aux = execute_command('ps') + running = "" + + if 'program.elf' in ps_aux: + running += ' Native (program.elf),' + if 'ap.elf' in ps_aux: + running += ' Paparazzi (ap.elf),' + if 'gst-launch' in ps_aux: + running += ' GStreamer (gst-launch)' + return running[1:] + +# Check if vision framework is installed +def check_vision_installed(): + du_opt = 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(): + du_opt = 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(): + check_update = execute_command('grep "START_PAPARAZZI" /bin/check_update.sh') + wifi_setup = 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(): + check_wifi = execute_command('grep "static_ip_address_base" /bin/wifi_setup.sh') + if "static_ip_address_base" in check_wifi: + return True + else: + return False + +# Check the filesystem +def check_filesystem(): + return execute_command('df -h') + +# Reboot the drone +def bebop_reboot(): + execute_command('reboot') + +# Install the vision framework +def bebop_install_vision(): + print('Uploading GST') + ftp.storbinary("STOR arm_light.tgz", file("bin/arm_light.tgz", "rb")) + print(execute_command("cd /data/video && tar -xzf arm_light.tgz")) + print(execute_command("rm -rf /data/video/arm_light.tgz")) + print('Now Starting Vision') + bebop_start_vision() + +# Remove the vision framework +def bebop_remove_vision(): + execute_command("rm -rf /opt/arm") + execute_command("rm -rf /lib/dsp") + execute_command("rm -rf /data/video/opt") + +# Start the vision framework +def bebop_start_vision(): + # Mount the directories + execute_command("mkdir -p /opt/arm") + execute_command("mkdir -p /lib/dsp") + execute_command("mount --bind /data/video/opt/arm /opt/arm") + execute_command("mount --bind /data/video/opt/arm/lib/dsp /lib/dsp") + # Start The DSP programs + execute_command("kill -9 `pidof program.elf`") + execute_command("kill -9 `pidof gst-launch-0.10`") + execute_command("export PATH=/opt/arm/gst/bin:$PATH") + execute_command("export DSP_PATH=/opt/arm/tidsp-binaries-23.i3.8/") + execute_command("/bin/dspbridge/cexec.out -T /opt/arm/tidsp-binaries-23.i3.8/baseimage.dof -v") + execute_command("/bin/dspbridge/dynreg.out -r /opt/arm/tidsp-binaries-23.i3.8/m4venc_sn.dll64P -v") + # Show result + execute_command("ls -altr /opt/arm/gst/bin") + +# Install autoboot script +def bebop_install_autoboot(): + print('Uploading autoboot script') + ftp.storbinary("STOR check_update.sh", file("check_update.sh", "rb")) + print(execute_command("mv /data/video/check_update.sh /bin/check_update.sh")) + print(execute_command("chmod 777 /bin/check_update.sh")) + +# Install network script +def bebop_install_network_script(): + print('Uploading Wifi script') + ftp.storbinary("STOR wifi_setup.sh", file("wifi_setup.sh", "rb")) + print(execute_command("mv /data/video/wifi_setup.sh /bin/wifi_setup.sh")) + print(execute_command("chmod 777 /bin/wifi_setup.sh")) + +# Set network SSID +def bebop_set_ssid(name): + write_to_config('ssid_single_player', name) + print('The network ID (SSID) of the Bebop is changed to ' + name) + +# Set IP address +def bebop_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 Bebop is changed to ' + address) + +# Set wifi mode (0: master, 1: ad-hoc, 2: managed, *: master) +def bebop_set_wifi_mode(mode): + modes = { 'master' : '0', 'ad-hoc' : '1', 'managed' : '2' } + 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 Bebop is changed to ' + mode + ' (' + val + ')') + +def bebop_status(): + config_ini = execute_command('cat /data/config.ini') + + print('======================== Bebop Status ========================') + print('Version:\t\t' + check_version()) + 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('Currently running:\t' + check_running()) + 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)) + + autorun = {'': 'Native', '0': 'Native', '1': 'Paparazzi RAW', '2': 'Paparazzi SDK'} + 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(check_filesystem()) + + +# Parse the arguments +parser = argparse.ArgumentParser(description='Bebop python helper. Use bebop.py -h for help') +parser.add_argument('--host', metavar='HOST', default='192.168.42.1', + help='the ip address of bebop') +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') +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 or sdk for Paparazzi autopilot)') +subparser_upload = subparsers.add_parser('upload_file', help='Upload a file to the Bebop') +subparser_upload.add_argument('file', help='Filename') +subparser_upload.add_argument('folder', help='Destination subfolder (base destination folder is /data/video)') +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/video)') +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/video)') +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/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 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') +subparser_networkid = subparsers.add_parser('networkid', help='Set the network ID(SSID) of the Bebop') +subparser_networkid.add_argument('name', help='the new network ID(SSID)') +subparser_ipaddress = subparsers.add_parser('ipaddress', help='Set the IP address of the Bebop') +subparser_ipaddress.add_argument('address', help='the new IP address') +subparser_wifimode = subparsers.add_parser('wifimode', help='Set the Wifi mode the Bebop') +subparser_wifimode.add_argument('mode', help='the new Wifi mode', choices=['master', 'ad-hoc', 'managed']) +subparser_configure_network = subparsers.add_parser('configure_network', help='Configure the network on the Bebop') +subparser_configure_network.add_argument('name', help='the new network ID(SSID)') +subparser_configure_network.add_argument('address', help='the new IP address') +subparser_configure_network.add_argument('mode', help='the new Wifi mode', choices=['master', 'ad-hoc', 'managed']) +subparser_install_autostart = subparsers.add_parser('install_autostart', help='Install custom autostart script and set what to start on boot for the Bebop') +subparser_install_autostart.add_argument('type', choices=['native', 'paparazzi_raw', 'paparazzi_sdk'], + help='what to start on boot') +subparser_autostart = subparsers.add_parser('autostart', help='Set what to start on boot for the Bebop') +subparser_autostart.add_argument('type', choices=['native', 'paparazzi_raw', 'paparazzi_sdk'], + help='what to start on boot') + +args = parser.parse_args() + +# Connect with telnet and ftp +try: + tn = telnetlib.Telnet(args.host) + ftp = FTP(args.host) + ftp.login() +except: + print('Could not connect to Bebop (host: ' + args.host + ')') + exit(2) + +# Read until after login +tn.read_until('# ') + +# Check the Bebop status +if args.command == 'status': + bebop_status() + +# Reboot the drone +elif args.command == 'reboot': + bebop_reboot() + print('The Bebop is rebooting...') + +# Kill a program +elif args.command == 'kill': + execute_command('killall -9 ' + args.program) + print('Program "' + args.program + '" is now killed') + +# Start a program +elif args.command == 'start': + execute_command(args.start + ' &') + print('Program "' + args.start + '" is now started') + +# Change the network ID +elif args.command == 'networkid': + bebop_set_ssid(args.name) + + if raw_input("Shall I restart the Bebop? (y/N) ").lower() == 'y': + bebop_reboot() + +# Change the IP address +elif args.command == 'ipaddress': + bebop_set_ip_address(args.address) + + if raw_input("Shall I restart the Bebop? (y/N) ").lower() == 'y': + bebop_reboot() + +# Change the wifi mode +elif args.command == 'wifimode': + bebop_set_wifi_mode(args.mode) + + if raw_input("Shall I restart the Bebop? (y/N) ").lower() == 'y': + bebop_reboot() + +# Install and configure network +elif args.command == 'configure_network': + config_ini = execute_command('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('=============================') + if check_wifi_setup(): + print('Custom Wifi script already installed') + if raw_input("Shall I reinstall the Wifi script (y/N) ").lower() == 'y': + bebop_install_network_script() + else: + if raw_input("Shall I install custom Wifi script (recommanded) (y/N) ").lower() == 'y': + bebop_install_network_script() + bebop_set_ssid(args.name) + bebop_set_ip_address(args.address) + bebop_set_wifi_mode(args.mode) + config_ini = execute_command('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('==================================') + + if raw_input("Shall I restart the Bebop? (y/N) ").lower() == 'y': + bebop_reboot() + +# 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': + bebop_install_autoboot() + else: + bebop_install_autoboot() + autorun = {'native': '0', 'paparazzi_raw': '1', 'paparazzi_sdk': '2'} + 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 Bebop? (y/N) ").lower() == 'y': + bebop_reboot() + +# Change the autostart +elif args.command == 'autostart': + autorun = {'native': '0', 'paparazzi_raw': '1', 'paparazzi_sdk': '2'} + 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': + bebop_remove_vision() + bebop_install_vision() + + bebop_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': + bebop_install_vision() + + if check_vision_installed(): + bebop_start_vision() + print('Vision framework started') + +elif args.command == 'upload_gst_module': + print('Uploading ...' + args.file) + ftp.storbinary("STOR " + args.file, file(args.file, "rb")) + execute_command("chmod 777 /data/video/" + args.file) + execute_command("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': + bebop_install_vision() + + if check_vision_installed(): + bebop_start_vision() + print('#pragma message: Vision framework started') + print('#pragma message: Vision Plugin Uploaded and DSP Started.') + + +elif args.command == 'insmod': + modfile = split_into_path_and_file(args.file) + print('Uploading \'' + modfile[1]) + ftp.storbinary("STOR " + modfile[1], file(args.file, "rb")) + print(execute_command("insmod /data/video/" + modfile[1])) + +elif args.command == 'upload_file_and_run': + # Split filename and path + f = split_into_path_and_file(args.file) + + print("Kill running " + f[1] + " and make folder " + args.folder) + execute_command("killall -9 " + f[1]) + sleep(1) + execute_command("mkdir -p /data/ftp/" + args.folder) + print('Uploading \'' + f[1] + "\' from " + f[0] + " to " + args.folder) + ftp.storbinary("STOR " + args.folder + "/" + f[1], file(args.file, "rb")) + sleep(0.5) + execute_command("chmod 777 /data/ftp/" + args.folder + "/" + f[1]) + execute_command("/data/ftp/" + args.folder + "/" + f[1] + " > /dev/null 2>&1 &") + print("#pragma message: Upload and Start of ap.elf to Bebop Succes!") + +elif args.command == 'upload_file': + # Split filename and path + f = split_into_path_and_file(args.file) + + execute_command("mkdir -p /data/video/" + args.folder) + print('Uploading \'' + f[1] + "\' from " + f[0] + " to /data/video/" + args.folder) + ftp.storbinary("STOR " + args.folder + "/" + f[1], file(args.file, "rb")) + print("#pragma message: Upload of " + f[1] + " to Bebop Succes!") + +elif args.command == 'download_file': + # Split filename and path + f = 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 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: + file.close() + +elif args.command == 'download_dir': + # Split filename and path + files = execute_command('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 = 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 Bebop Failed!") + else: + file.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/video/" + args.folder + " from Bebop") + print(execute_command('rm -r /data/video/' + args.folder)) + + + +# Close the telnet and python script +tn.close() +ftp.close() +exit(0)