Mirrors_Framework/paparazzi
1
0
Fork 0
mirror of https://github.com/paparazzi/paparazzi.git synced 2026-05-19 10:34:28 +08:00
Code Issues Actions 25 Packages Projects Releases Wiki Activity

Merge pull request #2085 from paparazzi/swing-integration

Browse Source 
Swing integration
This commit is contained in:
Gautier Hattenberger
2017-07-11 16:20:52 +02:00
committed by GitHub
parent 410368a37f 39b2d0b69b
commit c0836b42a0
17 changed files with 1651 additions and 693 deletions
Download Patch File Download Diff File Expand all files Collapse all files
conf/Makefile.swing
+37
View File
@@ -0,0 +1,37 @@
# Hey Emacs, this is a -*- makefile -*-
#
# Copyright (C) 2014 Freek van Tienen <freek.v.tienen@gmail.com>
# Copyright (C) 2017 Gautier Hattenberger <gautier.hattenberger@enac.fr>
#
# 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
# <http://www.gnu.org/licenses/>.
#
#
# 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
conf/airframes/examples/swing.xml
+214
View File
@@ -0,0 +1,214 @@
<!DOCTYPE airframe SYSTEM "../airframe.dtd">
<airframe name="swing">
<firmware name="rotorcraft">
<configure name="PREFIX" value="/opt/arm-2012.03/bin/arm-none-linux-gnueabi"/>
<target name="ap" board="swing"/>
<target name="nps" board="pc">
<module name="fdm" type="jsbsim"/>
</target>
<!--define name="USE_SONAR" value="TRUE"/-->
<module name="radio_control" type="datalink"/>
<module name="telemetry" type="transparent_udp"/>
<module name="motor_mixing"/>
<module name="actuators" type="swing"/>
<module name="imu" type="swing"/>
<module name="gps" type="datalink"/>
<module name="stabilization" type="int_quat"/>
<module name="ahrs" type="int_cmpl_quat"/>
<module name="ins"/>
<module name="air_data"/>
</firmware>
<commands>
<axis name="PITCH" failsafe_value="0"/>
<axis name="ROLL" failsafe_value="0"/>
<axis name="YAW" failsafe_value="0"/>
<axis name="THRUST" failsafe_value="0"/>
</commands>
<servos>
<servo name="BOTTOM_RIGHT" no="0" min="0" neutral="15" max="511"/>
<servo name="TOP_RIGHT" no="1" min="0" neutral="15" max="511"/>
<servo name="BOTTOM_LEFT" no="2" min="0" neutral="15" max="511"/>
<servo name="TOP_LEFT" no="3" min="0" neutral="15" max="511"/>
</servos>
<section name="MIXING" prefix="MOTOR_MIXING_">
<define name="TRIM_ROLL" value="0"/>
<define name="TRIM_PITCH" value="0"/>
<define name="TRIM_YAW" value="0"/>
<define name="NB_MOTOR" value="4"/>
<define name="SCALE" value="256"/>
<define name="MAX_SATURATION_OFFSET" value="3*MAX_PPRZ"/>
<!-- Time cross layout (X), with order NW (CW), NE (CCW), SE (CW), SW (CCW) -->
<define name="ROLL_COEF" value="{ -81, -81, 81, 81}"/>
<define name="PITCH_COEF" value="{ 243, -243, 243, -243}"/>
<define name="YAW_COEF" value="{ 128, -128, -128, 128}"/>
<define name="THRUST_COEF" value="{ 256, 256, 256, 256}"/>
</section>
<command_laws>
<call fun="motor_mixing_run(autopilot_get_motors_on(),FALSE,values)"/>
<set servo="BOTTOM_RIGHT" value="motor_mixing.commands[0]"/>
<set servo="TOP_RIGHT" value="motor_mixing.commands[1]"/>
<set servo="BOTTOM_LEFT" value="motor_mixing.commands[2]"/>
<set servo="TOP_LEFT" value="motor_mixing.commands[3]"/>
</command_laws>
<section name="AIR_DATA" prefix="AIR_DATA_">
<define name="CALC_AIRSPEED" value="FALSE"/>
<define name="CALC_TAS_FACTOR" value="FALSE"/>
<define name="CALC_AMSL_BARO" value="TRUE"/>
</section>
<section name="IMU" prefix="IMU_">
<!-- Magneto calibration -->
<define name="MAG_X_NEUTRAL" value="18"/>
<define name="MAG_Y_NEUTRAL" value="157"/>
<define name="MAG_Z_NEUTRAL" value="49"/>
<define name="MAG_X_SENS" value="10.5007722373" integer="16"/>
<define name="MAG_Y_SENS" value="11.1147400462" integer="16"/>
<define name="MAG_Z_SENS" value="11.6479371722" integer="16"/>
<!-- Magneto current calibration -->
<define name="MAG_X_CURRENT_COEF" value="0.0"/>
<define name="MAG_Y_CURRENT_COEF" value="0.0"/>
<define name="MAG_Z_CURRENT_COEF" value="0.0"/>
<define name="BODY_TO_IMU_PHI" value="0." unit="deg"/>
<define name="BODY_TO_IMU_THETA" value="0." unit="deg"/>
<define name="BODY_TO_IMU_PSI" value="0." unit="deg"/>
</section>
<!-- local magnetic field -->
<!-- http://wiki.paparazziuav.org/wiki/Subsystem/ahrs#Local_Magnetic_Field -->
<section name="AHRS" prefix="AHRS_">
<!-- Toulouse -->
<!--define name="H_X" value="0.513081"/>
<define name="H_Y" value="-0.00242783"/>
<define name="H_Z" value="0.858336"/-->
<!-- Delft -->
<define name="H_X" value="0.3892503"/>
<define name="H_Y" value="0.0017972"/>
<define name="H_Z" value="0.9211303"/>
</section>
<section name="INS" prefix="INS_">
<define name="SONAR_MAX_RANGE" value="2.2"/>
<define name="SONAR_UPDATE_ON_AGL" value="TRUE"/>
</section>
<section name="STABILIZATION_RATE" prefix="STABILIZATION_RATE_">
<!-- setpoints -->
<define name="SP_MAX_P" value="10000"/>
<define name="SP_MAX_Q" value="10000"/>
<define name="SP_MAX_R" value="10000"/>
<define name="DEADBAND_P" value="20"/>
<define name="DEADBAND_Q" value="20"/>
<define name="DEADBAND_R" value="200"/>
<define name="REF_TAU" value="4"/>
<!-- feedback -->
<define name="GAIN_P" value="400"/>
<define name="GAIN_Q" value="400"/>
<define name="GAIN_R" value="350"/>
<define name="IGAIN_P" value="75"/>
<define name="IGAIN_Q" value="75"/>
<define name="IGAIN_R" value="50"/>
<!-- feedforward -->
<define name="DDGAIN_P" value="300"/>
<define name="DDGAIN_Q" value="300"/>
<define name="DDGAIN_R" value="300"/>
</section>
<section name="STABILIZATION_ATTITUDE" prefix="STABILIZATION_ATTITUDE_">
<!-- setpoints -->
<define name="SP_MAX_PHI" value="45" unit="deg"/>
<define name="SP_MAX_THETA" value="45" unit="deg"/>
<define name="SP_MAX_R" value="300" unit="deg/s"/>
<define name="DEADBAND_A" value="0"/>
<define name="DEADBAND_E" value="0"/>
<define name="DEADBAND_R" value="50"/>
<!-- reference -->
<define name="REF_OMEGA_P" value="450" unit="deg/s"/>
<define name="REF_ZETA_P" value="0.9"/>
<define name="REF_MAX_P" value="600." unit="deg/s"/>
<define name="REF_MAX_PDOT" value="RadOfDeg(8000.)"/>
<define name="REF_OMEGA_Q" value="450" unit="deg/s"/>
<define name="REF_ZETA_Q" value="0.9"/>
<define name="REF_MAX_Q" value="600." unit="deg/s"/>
<define name="REF_MAX_QDOT" value="RadOfDeg(8000.)"/>
<define name="REF_OMEGA_R" value="450" unit="deg/s"/>
<define name="REF_ZETA_R" value="0.9"/>
<define name="REF_MAX_R" value="600." unit="deg/s"/>
<define name="REF_MAX_RDOT" value="RadOfDeg(8000.)"/>
<!-- feedback -->
<define name="PHI_PGAIN" value="650"/>
<define name="PHI_DGAIN" value="300"/>
<define name="PHI_IGAIN" value="0"/>
<define name="THETA_PGAIN" value="650"/>
<define name="THETA_DGAIN" value="300"/>
<define name="THETA_IGAIN" value="0"/>
<define name="PSI_PGAIN" value="300"/>
<define name="PSI_DGAIN" value="250"/>
<define name="PSI_IGAIN" value="0"/>
<!-- feedforward -->
<define name="PHI_DDGAIN" value="0"/>
<define name="THETA_DDGAIN" value="0"/>
<define name="PSI_DDGAIN" value="0"/>
</section>
<section name="GUIDANCE_V" prefix="GUIDANCE_V_">
<define name="HOVER_KP" value="283"/>
<define name="HOVER_KD" value="82"/>
<define name="HOVER_KI" value="13"/>
<define name="NOMINAL_HOVER_THROTTLE" value="0.655"/>
<define name="ADAPT_THROTTLE_ENABLED" value="TRUE"/>
</section>
<section name="GUIDANCE_H" prefix="GUIDANCE_H_">
<!-- Good weather -->
<define name="MAX_BANK" value="20" unit="deg"/>
<!-- Bad weather -->
<!-- define name="MAX_BANK" value="32" unit="deg"/ -->
<define name="PGAIN" value="79"/>
<define name="DGAIN" value="100"/>
<define name="IGAIN" value="30"/>
</section>
<section name="SIMULATOR" prefix="NPS_">
<define name="ACTUATOR_NAMES" value="nw_motor, ne_motor, se_motor, sw_motor" type="string[]"/>
<define name="JSBSIM_MODEL" value="simple_x_quad_ccw" type="string"/>
<define name="SENSORS_PARAMS" value="nps_sensors_params_default.h" type="string"/>
</section>
<section name="AUTOPILOT">
<define name="MODE_STARTUP" value="AP_MODE_KILL"/>
<define name="MODE_MANUAL" value="AP_MODE_ATTITUDE_DIRECT"/>
<define name="MODE_AUTO1" value="AP_MODE_ATTITUDE_DIRECT"/>
<define name="MODE_AUTO2" value="AP_MODE_ATTITUDE_Z_HOLD"/>
</section>
<section name="BAT">
<define name="CATASTROPHIC_BAT_LEVEL" value="3.1" unit="V"/>
<define name="CRITIC_BAT_LEVEL" value="3.3" unit="V"/>
<define name="LOW_BAT_LEVEL" value="3.5" unit="V"/>
<define name="MAX_BAT_LEVEL" value="4.1" unit="V"/>
</section>
</airframe>
conf/boards/swing.makefile
+51
View File
@@ -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
conf/firmwares/subsystems/shared/baro_board.makefile
+4
View File
@@ -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)
conf/modules/actuators_swing.xml
+17
View File
@@ -0,0 +1,17 @@
<!DOCTYPE module SYSTEM "module.dtd">
<module name="actuators_swing" dir="actuators" task="actuators">
<doc>
<description>
Actuators Driver for Swing
</description>
</doc>
<header>
<file name="actuators.h" dir="boards/swing"/>
</header>
<makefile target="ap">
<define name="ACTUATORS"/>
<file name="actuators.c" dir="$(SRC_BOARD)"/>
</makefile>
</module>
conf/modules/imu_swing.xml
+26
View File
@@ -0,0 +1,26 @@
<!DOCTYPE module SYSTEM "module.dtd">
<module name="imu_swing" dir="imu">
<doc>
<description>
Driver for IMU on the Parrot Swing drone.
- Accelerometer/Gyroscope: MPU6000 via I2C0
</description>
</doc>
<autoload name="imu_common"/>
<autoload name="imu_nps"/>
<!--autoload name="sonar_bebop"/-->
<header>
<file name="imu_swing.h" dir="subsystems/imu"/>
</header>
<init fun="imu_swing_init()"/>
<periodic fun="imu_swing_periodic()"/>
<event fun="imu_swing_event()"/>
<makefile target="!sim|nps|fbw">
<define name="USE_I2C0"/>
<define name="IMU_TYPE_H" value="subsystems/imu/imu_swing.h" type="string"/>
<file name="mpu60x0.c" dir="peripherals"/>
<file name="mpu60x0_i2c.c" dir="peripherals"/>
<file name="imu_swing.c" dir="subsystems/imu"/>
</makefile>
</module>
sw/airborne/boards/swing.h
+64
View File
@@ -0,0 +1,64 @@
/*
* Copyright (C) 2014 Freek van Tienen <freek.v.tienen@gmail.com>
* Copyright (C) 2017 Gautier Hattenberger <gautier.hattenberger@enac.fr>
*
* 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 */
sw/airborne/boards/swing/actuators.c
+126
View File
@@ -0,0 +1,126 @@
/*
* Copyright (C) 2014 Freek van Tienen <freek.v.tienen@gmail.com>
* Copyright (C) 2017 Gautier Hattenberger <gautier.hattenberger@enac.fr>
*
* 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 <sys/ioctl.h>
#include <fcntl.h>
/* 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_NB_BITS)
#endif
#define PWM_REG_RATIO_PRECISION_MASK (PWM_NB_BITS<<16)
#define PWM_REG_SATURATION (PWM_REG_RATIO_PRECISION_MASK|PWM_TOTAL_RANGE)
#include "subsystems/actuators.h"
#include "subsystems/actuators/motor_mixing.h"
#include "actuators.h"
#include "autopilot.h"
struct ActuatorsSwing actuators_swing;
static int actuators_fd;
// Start/stop PWM
enum {
SiP6_PWM0_START = (1<<0),
SiP6_PWM1_START = (1<<1),
SiP6_PWM2_START = (1<<2),
SiP6_PWM3_START = (1<<3),
};
void actuators_swing_init(void)
{
actuators_fd = open("/dev/pwm", O_RDWR);
pwm_delos_quadruplet m = {{ 1, 1, 1, 1 }};
int ret __attribute__((unused)) = ioctl(actuators_fd, PWM_DELOS_SET_SPEEDS, &m);
#if ACTUATORS_SWING_DEBUG
printf("Return Speeds: %d\n", ret);
#endif
actuators_swing_commit();
unsigned int control_reg = (SiP6_PWM0_START|SiP6_PWM1_START|SiP6_PWM2_START|SiP6_PWM3_START);
ret = ioctl(actuators_fd, PWM_DELOS_SET_CTRL, &control_reg);
#if ACTUATORS_SWING_DEBUG
printf("Return control: %d\n", ret);
#endif
}
void actuators_swing_commit(void)
{
pwm_delos_quadruplet m;
m.val[0] = actuators_swing.rpm_ref[0] & 0xffff;
m.val[1] = actuators_swing.rpm_ref[1] & 0xffff;
m.val[2] = actuators_swing.rpm_ref[2] & 0xffff;
m.val[3] = actuators_swing.rpm_ref[3] & 0xffff;
if( actuators_swing.rpm_ref[0] > (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
}
sw/airborne/boards/swing/actuators.h
+46
View File
@@ -0,0 +1,46 @@
/*
* Copyright (C) 2014 Freek van Tienen <freek.v.tienen@gmail.com>
* Copyright (C) 2017 Gautier Hattenberger <gautier.hattenberger@enac.fr>
*
* 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 <stdint.h>
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_ */
sw/airborne/boards/swing/baro_board.h
+38
View File
@@ -0,0 +1,38 @@
/*
* Copyright (C) 2017 Gautier Hattenberger <gautier.hattenberger@enac.fr>
*
* 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
* <http://www.gnu.org/licenses/>.
*/
/**
* @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 */
sw/airborne/boards/swing/board.c
+131
View File
@@ -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
* <http://www.gnu.org/licenses/>.
*/
/**
* @file boards/swing/board.c
*
* Swing specific board initialization function.
*
*/
#include "boards/swing.h"
#include "mcu.h"
#include <stdlib.h>
#include <stdio.h>
#include <string.h>
#include <fcntl.h>
#include <unistd.h>
#include <pthread.h>
#include <linux/input.h>
#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)
{
}
sw/airborne/subsystems/imu/imu_swing.c
+118
View File
@@ -0,0 +1,118 @@
/*
* Copyright (C) 2014 Freek van Tienen <freek.v.tienen@gmail.com>
* Copyright (C) 2017 Gautier Hattenberger <gautier.hattenberger@enac.fr>
*
* 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);
}
}
sw/airborne/subsystems/imu/imu_swing.h
+83
View File
@@ -0,0 +1,83 @@
/*
* Copyright (C) 2014 Freek van Tienen <freek.v.tienen@gmail.com>
* Copyright (C) 2017 Gautier Hattenberger <gautier.hattenberger@enac.fr>
*
* 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 */
sw/tools/parrot/ardrone2.py
+315 -442
View File
File diff suppressed because it is too large Load Diff
sw/tools/parrot/bebop.py
+29 -185
View File
@@ -1,6 +1,8 @@
#!/usr/bin/env python
#
# Copyright (C) 2012-2014 The Paparazzi Team
# 2015 Freek van Tienen <freek.v.tienen@gmail.com>
# 2017 Gautier Hattenberger <gautier.hattenberger@enac.fr>
#
# 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)
sw/tools/parrot/parrot_utils.py
+304 -66
View File
@@ -1,5 +1,7 @@
#
# Copyright (C) 2012-2014 The Paparazzi Team
# 2015 Freek van Tienen <freek.v.tienen@gmail.com>
# 2017 Gautier Hattenberger <gautier.hattenberger@enac.fr>
#
# 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()
sw/tools/parrot/swing.py
Executable
+48
View File
@@ -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
# <http://www.gnu.org/licenses/>.
#
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)