mirror of
https://github.com/paparazzi/paparazzi.git
synced 2026-05-31 03:57:45 +08:00
[ardrone2] Remove the deprecated SDK version
This commit is contained in:
Freek van Tienen
@@ -1,145 +0,0 @@
|
|||||||
<!DOCTYPE airframe SYSTEM "airframe.dtd">
|
|
||||||
|
|
||||||
<airframe name="ardrone2_sdk">
|
|
||||||
|
|
||||||
<firmware name="rotorcraft">
|
|
||||||
<target name="ap" board="ardrone2_sdk">
|
|
||||||
<define name="USE_INS_NAV_INIT" />
|
|
||||||
<define name="ROTORCRAFT_COMMANDS_YAW_ALWAYS_ENABLED"/>
|
|
||||||
<!-- <define name="ARDRONE2_DEBUG" /> -->
|
|
||||||
<!-- <define name="USE_GPS_HEIGHT" /> -->
|
|
||||||
<!-- <define name="ARDRONE_FLIGHT_INDOOR" /> -->
|
|
||||||
<!-- <define name="ARDRONE_WITHOUT_SHELL" /> -->
|
|
||||||
<!-- <define name="ARDRONE_OWNER_MAC" value="00:24:d7:47:f0:f4" /> -->
|
|
||||||
</target>
|
|
||||||
|
|
||||||
<subsystem name="radio_control" type="datalink" />
|
|
||||||
<subsystem name="telemetry" type="transparent_udp" />
|
|
||||||
<subsystem name="gps" type="ardrone2" />
|
|
||||||
<subsystem name="ahrs" type="ardrone2" />
|
|
||||||
<subsystem name="ins" type="ardrone2" />
|
|
||||||
<subsystem name="actuators" type="ardrone2" />
|
|
||||||
<subsystem name="stabilization" type="passthrough"/>
|
|
||||||
<subsystem name="imu" type="ardrone2" />
|
|
||||||
</firmware>
|
|
||||||
|
|
||||||
<modules main_freq="512">
|
|
||||||
<load name="sys_mon.xml" />
|
|
||||||
</modules>
|
|
||||||
|
|
||||||
<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 driver="Default" />
|
|
||||||
|
|
||||||
<section name="STABILIZATION_RATE" prefix="STABILIZATION_RATE_">
|
|
||||||
<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="30." unit="deg" />
|
|
||||||
<define name="SP_MAX_THETA" value="30." unit="deg" />
|
|
||||||
<define name="SP_MAX_R" value="90." unit="deg/s" />
|
|
||||||
<define name="DEADBAND_A" value="0" />
|
|
||||||
<define name="DEADBAND_E" value="0" />
|
|
||||||
<define name="DEADBAND_R" value="250" />
|
|
||||||
</section>
|
|
||||||
|
|
||||||
<section name="GUIDANCE_V" prefix="GUIDANCE_V_">
|
|
||||||
<define name="HOVER_KP" value="425" />
|
|
||||||
<define name="HOVER_KD" value="200" />
|
|
||||||
<define name="HOVER_KI" value="125" />
|
|
||||||
<define name="NOMINAL_HOVER_THROTTLE" value="0.5"/>
|
|
||||||
<define name="ADAPT_THROTTLE_ENABLED" value="FALSE"/>
|
|
||||||
</section>
|
|
||||||
|
|
||||||
<section name="GUIDANCE_H" prefix="GUIDANCE_H_">
|
|
||||||
<define name="MAX_BANK" value="30." unit="deg" />
|
|
||||||
<define name="PGAIN" value="7" />
|
|
||||||
<define name="DGAIN" value="2" />
|
|
||||||
<define name="IGAIN" value="0" />
|
|
||||||
</section>
|
|
||||||
|
|
||||||
<section name="AUTOPILOT">
|
|
||||||
<define name="MODE_STARTUP" value="AP_MODE_NAV" />
|
|
||||||
<define name="MODE_MANUAL" value="AP_MODE_RC_DIRECT" />
|
|
||||||
<define name="MODE_AUTO1" value="AP_MODE_HOVER_Z_HOLD" />
|
|
||||||
<define name="MODE_AUTO2" value="AP_MODE_NAV" />
|
|
||||||
</section>
|
|
||||||
|
|
||||||
<section name="BAT">
|
|
||||||
<define name="CATASTROPHIC_BAT_LEVEL" value=".2" unit="V" />
|
|
||||||
<define name="CRITIC_BAT_LEVEL" value="1.9" unit="V" />
|
|
||||||
<define name="LOW_BAT_LEVEL" value="2.5" unit="V" />
|
|
||||||
<define name="MAX_BAT_LEVEL" value="10.0" unit="V" />
|
|
||||||
</section>
|
|
||||||
|
|
||||||
<section name="IMU" prefix="IMU_">
|
|
||||||
<define name="ACCEL_X_NEUTRAL" value="2072" />
|
|
||||||
<define name="ACCEL_Y_NEUTRAL" value="2040" />
|
|
||||||
<define name="ACCEL_Z_NEUTRAL" value="2047" />
|
|
||||||
<define name="ACCEL_X_SENS" value="19.1079036954" integer="16" />
|
|
||||||
<define name="ACCEL_Y_SENS" value="19.5393623518" integer="16" />
|
|
||||||
<define name="ACCEL_Z_SENS" value="19.6539180847" integer="16" />
|
|
||||||
|
|
||||||
<define name="ACCEL_X_SIGN" value="1" />
|
|
||||||
<define name="ACCEL_Y_SIGN" value="-1" />
|
|
||||||
<define name="ACCEL_Z_SIGN" value="-1" />
|
|
||||||
|
|
||||||
<define name="GYRO_P_SENS_NUM" value="4359" />
|
|
||||||
<define name="GYRO_P_SENS_DEN" value="1000" />
|
|
||||||
<define name="GYRO_Q_SENS_NUM" value="4359" />
|
|
||||||
<define name="GYRO_Q_SENS_DEN" value="1000" />
|
|
||||||
<define name="GYRO_R_SENS_NUM" value="4359" />
|
|
||||||
<define name="GYRO_R_SENS_DEN" value="1000" />
|
|
||||||
|
|
||||||
<define name="GYRO_P_SIGN" value="1" />
|
|
||||||
<define name="GYRO_Q_SIGN" value="-1" />
|
|
||||||
<define name="GYRO_R_SIGN" value="-1" />
|
|
||||||
|
|
||||||
<define name="MAG_X_NEUTRAL" value="118" />
|
|
||||||
<define name="MAG_Y_NEUTRAL" value="-65" />
|
|
||||||
<define name="MAG_Z_NEUTRAL" value="110" />
|
|
||||||
<define name="MAG_X_SENS" value="14.6416865144" integer="16" />
|
|
||||||
<define name="MAG_Y_SENS" value="14.5167340835" integer="16" />
|
|
||||||
<define name="MAG_Z_SENS" value="15.1670335124" integer="16" />
|
|
||||||
|
|
||||||
<define name="MAG_X_SIGN" value="-1" />
|
|
||||||
<define name="MAG_Y_SIGN" value="1" />
|
|
||||||
<define name="MAG_Z_SIGN" value="-1" />
|
|
||||||
|
|
||||||
<!-- roll -->
|
|
||||||
<define name="BODY_TO_IMU_PHI" value="0." unit="deg" />
|
|
||||||
<!-- pitch -->
|
|
||||||
<define name="BODY_TO_IMU_THETA" value="0." unit="deg" />
|
|
||||||
<!-- yaw -->
|
|
||||||
<define name="BODY_TO_IMU_PSI" value="0." unit="deg" />
|
|
||||||
</section>
|
|
||||||
|
|
||||||
</airframe>
|
|
||||||
@@ -1,49 +0,0 @@
|
|||||||
# Hey Emacs, this is a -*- makefile -*-
|
|
||||||
#
|
|
||||||
# ardrone2_sdk.makefile
|
|
||||||
#
|
|
||||||
# http://wiki.paparazziuav.org/wiki/AR.Drone_2_-_Specifications
|
|
||||||
#
|
|
||||||
|
|
||||||
BOARD=ardrone
|
|
||||||
BOARD_VERSION=2
|
|
||||||
BOARD_TYPE=sdk
|
|
||||||
BOARD_CFG=\"boards/$(BOARD)$(BOARD_VERSION)_$(BOARD_TYPE).h\"
|
|
||||||
|
|
||||||
ARCH=linux
|
|
||||||
$(TARGET).ARCHDIR = $(ARCH)
|
|
||||||
# include conf/Makefile.ardrone2 (with specific upload rules) instead of only Makefile.linux:
|
|
||||||
ap.MAKEFILE = ardrone2
|
|
||||||
|
|
||||||
# -----------------------------------------------------------------------
|
|
||||||
USER=foobar
|
|
||||||
HOST=192.168.1.1
|
|
||||||
SUB_DIR=sdk
|
|
||||||
FTP_DIR=/data/video
|
|
||||||
TARGET_DIR=$(FTP_DIR)/$(SUB_DIR)
|
|
||||||
# -----------------------------------------------------------------------
|
|
||||||
ARDRONE2_START_PAPARAZZI ?= 0
|
|
||||||
ARDRONE2_WIFI_MODE ?= 0
|
|
||||||
ARDRONE2_SSID ?= ardrone2_paparazzi
|
|
||||||
ARDRONE2_IP_ADDRESS_BASE ?= 192.168.1.
|
|
||||||
ARDRONE2_IP_ADDRESS_PROBE ?= 1
|
|
||||||
# -----------------------------------------------------------------------
|
|
||||||
|
|
||||||
# The GPS sensor is connected trough USB so we have to define the device
|
|
||||||
GPS_PORT ?= UART1
|
|
||||||
GPS_BAUD ?= B57600
|
|
||||||
|
|
||||||
# The datalink default uses UDP
|
|
||||||
MODEM_HOST ?= 192.168.1.255
|
|
||||||
|
|
||||||
# for distinction between SDK and RAW version
|
|
||||||
ap.CFLAGS +=-DARDRONE2_SDK
|
|
||||||
|
|
||||||
# -----------------------------------------------------------------------
|
|
||||||
|
|
||||||
# default LED configuration
|
|
||||||
RADIO_CONTROL_LED ?= none
|
|
||||||
BARO_LED ?= none
|
|
||||||
AHRS_ALIGNER_LED ?= none
|
|
||||||
GPS_LED ?= none
|
|
||||||
SYS_TIME_LED ?= none
|
|
||||||
@@ -208,17 +208,6 @@
|
|||||||
settings_modules="modules/gps_ubx_ucenter.xml"
|
settings_modules="modules/gps_ubx_ucenter.xml"
|
||||||
gui_color="red"
|
gui_color="red"
|
||||||
/>
|
/>
|
||||||
<aircraft
|
|
||||||
name="ardrone2_sdk"
|
|
||||||
ac_id="200"
|
|
||||||
airframe="airframes/ardrone2_sdk.xml"
|
|
||||||
radio="radios/cockpitSX.xml"
|
|
||||||
telemetry="telemetry/default_ardrone.xml"
|
|
||||||
flight_plan="flight_plans/rotorcraft_basic.xml"
|
|
||||||
settings="settings/rotorcraft_basic.xml settings/control/rotorcraft_guidance.xml"
|
|
||||||
settings_modules=""
|
|
||||||
gui_color="blue"
|
|
||||||
/>
|
|
||||||
<aircraft
|
<aircraft
|
||||||
name="bebop"
|
name="bebop"
|
||||||
ac_id="202"
|
ac_id="202"
|
||||||
|
|||||||
@@ -340,17 +340,6 @@
|
|||||||
settings_modules="modules/gps_ubx_ucenter.xml"
|
settings_modules="modules/gps_ubx_ucenter.xml"
|
||||||
gui_color="red"
|
gui_color="red"
|
||||||
/>
|
/>
|
||||||
<aircraft
|
|
||||||
name="ardrone2_sdk"
|
|
||||||
ac_id="200"
|
|
||||||
airframe="airframes/ardrone2_sdk.xml"
|
|
||||||
radio="radios/cockpitSX.xml"
|
|
||||||
telemetry="telemetry/default_ardrone.xml"
|
|
||||||
flight_plan="flight_plans/rotorcraft_basic.xml"
|
|
||||||
settings="settings/rotorcraft_basic.xml settings/control/rotorcraft_guidance.xml"
|
|
||||||
settings_modules=""
|
|
||||||
gui_color="blue"
|
|
||||||
/>
|
|
||||||
<aircraft
|
<aircraft
|
||||||
name="bebop"
|
name="bebop"
|
||||||
ac_id="202"
|
ac_id="202"
|
||||||
|
|||||||
@@ -137,8 +137,6 @@ endif
|
|||||||
ifeq ($(BOARD), booz)
|
ifeq ($(BOARD), booz)
|
||||||
ns_CFLAGS += -DUSE_DAC
|
ns_CFLAGS += -DUSE_DAC
|
||||||
ns_srcs += $(SRC_ARCH)/mcu_periph/dac_arch.c
|
ns_srcs += $(SRC_ARCH)/mcu_periph/dac_arch.c
|
||||||
else ifeq ($(BOARD)$(BOARD_TYPE), ardronesdk)
|
|
||||||
ns_srcs += $(SRC_BOARD)/electrical_dummy.c
|
|
||||||
else ifeq ($(BOARD)$(BOARD_TYPE), ardroneraw)
|
else ifeq ($(BOARD)$(BOARD_TYPE), ardroneraw)
|
||||||
ns_srcs += $(SRC_BOARD)/electrical_raw.c
|
ns_srcs += $(SRC_BOARD)/electrical_raw.c
|
||||||
else ifeq ($(BOARD), bebop)
|
else ifeq ($(BOARD), bebop)
|
||||||
|
|||||||
@@ -1,11 +1,5 @@
|
|||||||
|
|
||||||
# Actuator drivers for both ardrone versions are conditionally included here
|
# Actuator drivers for the raw ardrone version are included here
|
||||||
# The AT-command and RAW drivers are not interchangeble
|
|
||||||
|
|
||||||
ifeq ($(BOARD_TYPE), sdk)
|
$(TARGET).CFLAGS += -DACTUATORS
|
||||||
$(TARGET).srcs += $(SRC_BOARD)/actuators_at.c
|
$(TARGET).srcs += $(SRC_BOARD)/actuators_ardrone2_raw.c
|
||||||
$(TARGET).srcs += $(SRC_BOARD)/at_com.c
|
|
||||||
else ifeq ($(BOARD_TYPE), raw)
|
|
||||||
$(TARGET).CFLAGS += -DACTUATORS
|
|
||||||
$(TARGET).srcs += $(SRC_BOARD)/actuators_ardrone2_raw.c
|
|
||||||
endif
|
|
||||||
|
|||||||
@@ -1,15 +1,9 @@
|
|||||||
# imu AR.Drone2
|
# imu AR.Drone2
|
||||||
|
|
||||||
ifeq ($(BOARD_TYPE), sdk)
|
|
||||||
imu_CFLAGS += -DIMU_TYPE_H=\"subsystems/imu/imu_ardrone2_sdk.h\" -DUSE_IMU
|
|
||||||
imu_srcs += $(SRC_SUBSYSTEMS)/imu.c
|
|
||||||
imu_srcs += $(SRC_SUBSYSTEMS)/imu/imu_ardrone2_sdk.c
|
|
||||||
else ifeq ($(BOARD_TYPE), raw)
|
|
||||||
imu_CFLAGS += -DIMU_TYPE_H=\"subsystems/imu/imu_ardrone2_raw.h\" -DUSE_IMU
|
imu_CFLAGS += -DIMU_TYPE_H=\"subsystems/imu/imu_ardrone2_raw.h\" -DUSE_IMU
|
||||||
imu_srcs += $(SRC_SUBSYSTEMS)/imu.c
|
imu_srcs += $(SRC_SUBSYSTEMS)/imu.c
|
||||||
imu_srcs += $(SRC_SUBSYSTEMS)/imu/imu_ardrone2_raw.c
|
imu_srcs += $(SRC_SUBSYSTEMS)/imu/imu_ardrone2_raw.c
|
||||||
imu_srcs += $(SRC_BOARD)/navdata.c
|
imu_srcs += $(SRC_BOARD)/navdata.c
|
||||||
endif
|
|
||||||
|
|
||||||
|
|
||||||
# add it for all targets except sim, fbw and nps
|
# add it for all targets except sim, fbw and nps
|
||||||
|
|||||||
@@ -31,7 +31,7 @@
|
|||||||
#include <stdint.h>
|
#include <stdint.h>
|
||||||
#include BOARD_CONFIG
|
#include BOARD_CONFIG
|
||||||
|
|
||||||
#if defined BOARD_ARDRONE2_SDK || defined BOARD_ARDRONE2_RAW
|
#if defined BOARD_ARDRONE2_RAW
|
||||||
extern uint32_t led_hw_values;
|
extern uint32_t led_hw_values;
|
||||||
#define LED_INIT(i) { led_hw_values &= ~(1<<i); }
|
#define LED_INIT(i) { led_hw_values &= ~(1<<i); }
|
||||||
#define LED_ON(i) { led_hw_values |= (1<<i); }
|
#define LED_ON(i) { led_hw_values |= (1<<i); }
|
||||||
|
|||||||
@@ -1,73 +0,0 @@
|
|||||||
/*
|
|
||||||
* Copyright (C) 2012-2013 Freek van Tienen
|
|
||||||
*
|
|
||||||
* This file is part of paparazzi.
|
|
||||||
*
|
|
||||||
* paparazzi is free software; you can redistribute it and/or modify
|
|
||||||
* it under the terms of the GNU General Public License as published by
|
|
||||||
* the Free Software Foundation; either version 2, or (at your option)
|
|
||||||
* any later version.
|
|
||||||
*
|
|
||||||
* paparazzi is distributed in the hope that it will be useful,
|
|
||||||
* but WITHOUT ANY WARRANTY; without even the implied warranty of
|
|
||||||
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
|
|
||||||
* GNU General Public License for more details.
|
|
||||||
*
|
|
||||||
* You should have received a copy of the GNU General Public License
|
|
||||||
* along with paparazzi; see the file COPYING. If not, write to
|
|
||||||
* the Free Software Foundation, 59 Temple Place - Suite 330,
|
|
||||||
* Boston, MA 02111-1307, USA.
|
|
||||||
*/
|
|
||||||
|
|
||||||
/**
|
|
||||||
* @file boards/ardrone/actuators_at.c
|
|
||||||
* ardrone2-sdk actuators are driven by external software controller by AT-commands
|
|
||||||
*/
|
|
||||||
|
|
||||||
#include "subsystems/ahrs/ahrs_ardrone2.h"
|
|
||||||
#include "actuators_at.h"
|
|
||||||
#include "generated/airframe.h"
|
|
||||||
#include "boards/ardrone/at_com.h"
|
|
||||||
|
|
||||||
void actuators_init(void)
|
|
||||||
{
|
|
||||||
init_at_com();
|
|
||||||
}
|
|
||||||
|
|
||||||
void actuators_set(pprz_t commands[])
|
|
||||||
{
|
|
||||||
//Calculate the thrus, roll, pitch and yaw from the PPRZ commands
|
|
||||||
float thrust = ((float)(commands[COMMAND_THRUST] - MAX_PPRZ / 2) / (float)MAX_PPRZ) * 2.0f;
|
|
||||||
float roll = ((float)commands[COMMAND_ROLL] / (float)MAX_PPRZ);
|
|
||||||
float pitch = ((float)commands[COMMAND_PITCH] / (float)MAX_PPRZ);
|
|
||||||
float yaw = ((float)commands[COMMAND_YAW] / (float)MAX_PPRZ);
|
|
||||||
|
|
||||||
//Starting engine
|
|
||||||
if(thrust > 0 && (ahrs_ardrone2.control_state == CTRL_DEFAULT || ahrs_ardrone2.control_state == CTRL_INIT
|
|
||||||
|| ahrs_ardrone2.control_state == CTRL_LANDED)) {
|
|
||||||
at_com_send_ref(REF_TAKEOFF);
|
|
||||||
}
|
|
||||||
|
|
||||||
//Check emergency or stop engine
|
|
||||||
if ((ahrs_ardrone2.state & ARDRONE_EMERGENCY_MASK) != 0) {
|
|
||||||
at_com_send_ref(REF_EMERGENCY);
|
|
||||||
} else if(thrust < -0.9 && !(ahrs_ardrone2.control_state == CTRL_DEFAULT ||
|
|
||||||
ahrs_ardrone2.control_state == CTRL_INIT || ahrs_ardrone2.control_state == CTRL_LANDED)) {
|
|
||||||
at_com_send_ref(0);
|
|
||||||
}
|
|
||||||
|
|
||||||
//Calibration
|
|
||||||
if ((ahrs_ardrone2.state & ARDRONE_MAGNETO_NEEDS_CALIB) != 0 &&
|
|
||||||
(ahrs_ardrone2.control_state == CTRL_FLYING || ahrs_ardrone2.control_state == CTRL_HOVERING)) {
|
|
||||||
at_com_send_calib(0);
|
|
||||||
}
|
|
||||||
|
|
||||||
//Moving
|
|
||||||
if ((ahrs_ardrone2.state & ARDRONE_MAGNETO_NEEDS_CALIB) == 0 &&
|
|
||||||
(ahrs_ardrone2.control_state == CTRL_FLYING || ahrs_ardrone2.control_state == CTRL_HOVERING)) {
|
|
||||||
at_com_send_pcmd(1, thrust, roll, pitch, yaw);
|
|
||||||
}
|
|
||||||
|
|
||||||
//Keep alive (FIXME)
|
|
||||||
at_com_send_config("general:navdata_demo", "FALSE");
|
|
||||||
}
|
|
||||||
@@ -1,36 +0,0 @@
|
|||||||
/*
|
|
||||||
* Copyright (C) 2012-2013 Freek van Tienen
|
|
||||||
*
|
|
||||||
* This file is part of paparazzi.
|
|
||||||
*
|
|
||||||
* paparazzi is free software; you can redistribute it and/or modify
|
|
||||||
* it under the terms of the GNU General Public License as published by
|
|
||||||
* the Free Software Foundation; either version 2, or (at your option)
|
|
||||||
* any later version.
|
|
||||||
*
|
|
||||||
* paparazzi is distributed in the hope that it will be useful,
|
|
||||||
* but WITHOUT ANY WARRANTY; without even the implied warranty of
|
|
||||||
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
|
|
||||||
* GNU General Public License for more details.
|
|
||||||
*
|
|
||||||
* You should have received a copy of the GNU General Public License
|
|
||||||
* along with paparazzi; see the file COPYING. If not, write to
|
|
||||||
* the Free Software Foundation, 59 Temple Place - Suite 330,
|
|
||||||
* Boston, MA 02111-1307, USA.
|
|
||||||
*/
|
|
||||||
|
|
||||||
/**
|
|
||||||
* @file boards/ardrone/actuators_at.h
|
|
||||||
* ardrone2-sdk actuators are driven by external software controller by AT-commands
|
|
||||||
*/
|
|
||||||
|
|
||||||
#ifndef BOARDS_ARDRONE_ACTUATORS_AT_H
|
|
||||||
#define BOARDS_ARDRONE_ACTUATORS_AT_H
|
|
||||||
|
|
||||||
#include "paparazzi.h"
|
|
||||||
|
|
||||||
extern void actuators_init(void);
|
|
||||||
extern void actuators_set(pprz_t commands[]);
|
|
||||||
#define SetActuatorsFromCommands(commands, AP_MODE) actuators_set(commands)
|
|
||||||
|
|
||||||
#endif /* BOARDS_ARDRONE_ACTUATORS_AT_H */
|
|
||||||
@@ -1,208 +0,0 @@
|
|||||||
/*
|
|
||||||
* Copyright (C) 2012-2013 Freek van Tienen
|
|
||||||
*
|
|
||||||
* This file is part of paparazzi.
|
|
||||||
*
|
|
||||||
* paparazzi is free software; you can redistribute it and/or modify
|
|
||||||
* it under the terms of the GNU General Public License as published by
|
|
||||||
* the Free Software Foundation; either version 2, or (at your option)
|
|
||||||
* any later version.
|
|
||||||
*
|
|
||||||
* paparazzi is distributed in the hope that it will be useful,
|
|
||||||
* but WITHOUT ANY WARRANTY; without even the implied warranty of
|
|
||||||
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
|
|
||||||
* GNU General Public License for more details.
|
|
||||||
*
|
|
||||||
* You should have received a copy of the GNU General Public License
|
|
||||||
* along with paparazzi; see the file COPYING. If not, write to
|
|
||||||
* the Free Software Foundation, 59 Temple Place - Suite 330,
|
|
||||||
* Boston, MA 02111-1307, USA.
|
|
||||||
*/
|
|
||||||
|
|
||||||
/**
|
|
||||||
* @file boards/ardrone/at_com.c
|
|
||||||
* Sending and receiving of AT-commands specified by the ardrone API
|
|
||||||
*/
|
|
||||||
|
|
||||||
#include "at_com.h"
|
|
||||||
#include "boards/ardrone2_sdk.h"
|
|
||||||
#include "generated/airframe.h"
|
|
||||||
|
|
||||||
#include <stdlib.h>
|
|
||||||
#include <stdio.h>
|
|
||||||
#include <netdb.h>
|
|
||||||
#include <netinet/in.h>
|
|
||||||
#include <arpa/inet.h>
|
|
||||||
#include <errno.h>
|
|
||||||
|
|
||||||
int packet_seq = 1; //Packet sequence number
|
|
||||||
|
|
||||||
int at_socket = -1, //AT socket connection
|
|
||||||
navdata_socket = -1; //Navdata socket connection
|
|
||||||
|
|
||||||
struct sockaddr_in pc_addr, //Own pc address
|
|
||||||
drone_at, //Drone AT address
|
|
||||||
drone_nav, //Drone nav address
|
|
||||||
from; //From address
|
|
||||||
|
|
||||||
bool_t at_com_ready = FALSE; //Status of the at communication
|
|
||||||
char sessionId[9]; //THe config session ID
|
|
||||||
|
|
||||||
void at_com_send(char *command);
|
|
||||||
void init_at_config(void);
|
|
||||||
|
|
||||||
//Init the at_com
|
|
||||||
void init_at_com(void)
|
|
||||||
{
|
|
||||||
//Check if already initialized
|
|
||||||
if (at_com_ready) {
|
|
||||||
return;
|
|
||||||
}
|
|
||||||
|
|
||||||
//Create the at and navdata socket
|
|
||||||
if ((at_socket = socket(AF_INET, SOCK_DGRAM, 0)) < 0) {
|
|
||||||
printf("at_com: at_socket error (%s)\n", strerror(errno));
|
|
||||||
}
|
|
||||||
if ((navdata_socket = socket(AF_INET, SOCK_DGRAM, 0)) < 0) {
|
|
||||||
printf("at_com: navdata_socket error (%s)\n", strerror(errno));
|
|
||||||
}
|
|
||||||
|
|
||||||
//For recvfrom
|
|
||||||
pc_addr.sin_family = AF_INET;
|
|
||||||
pc_addr.sin_addr.s_addr = htonl(INADDR_ANY);
|
|
||||||
pc_addr.sin_port = htons(9800);
|
|
||||||
|
|
||||||
//For sendto AT
|
|
||||||
drone_at.sin_family = AF_INET;
|
|
||||||
drone_at.sin_addr.s_addr = inet_addr(ARDRONE_IP);
|
|
||||||
drone_at.sin_port = htons(ARDRONE_AT_PORT);
|
|
||||||
|
|
||||||
//For sendto navadata init
|
|
||||||
drone_nav.sin_family = AF_INET;
|
|
||||||
drone_nav.sin_addr.s_addr = inet_addr(ARDRONE_IP);
|
|
||||||
drone_nav.sin_port = htons(ARDRONE_NAVDATA_PORT);
|
|
||||||
|
|
||||||
//Bind the navdata socket
|
|
||||||
if (bind(navdata_socket, (struct sockaddr *) &pc_addr, sizeof(pc_addr)) < 0) {
|
|
||||||
printf("at_com: bind error (%s)\n", strerror(errno));
|
|
||||||
}
|
|
||||||
|
|
||||||
//Set unicast mode on
|
|
||||||
int one = 1;
|
|
||||||
sendto(navdata_socket, &one, 4, 0, (struct sockaddr *) &drone_nav,
|
|
||||||
sizeof(drone_nav));
|
|
||||||
|
|
||||||
//Init at config
|
|
||||||
init_at_config();
|
|
||||||
|
|
||||||
//Set at_com to ready
|
|
||||||
at_com_ready = TRUE;
|
|
||||||
}
|
|
||||||
|
|
||||||
//Init the at config
|
|
||||||
void init_at_config(void)
|
|
||||||
{
|
|
||||||
//Generate a session id
|
|
||||||
uint32_t binaryId = (uint32_t) rand();
|
|
||||||
binaryId = (0 != binaryId) ? binaryId : 1u;
|
|
||||||
snprintf(sessionId, 9, "%08x", binaryId);
|
|
||||||
sessionId[8] = '\0';
|
|
||||||
|
|
||||||
//Send session, application and user id:
|
|
||||||
at_com_send_config("custom:session_id", sessionId);
|
|
||||||
at_com_send_config("custom:application_id", "9D7BFD45");
|
|
||||||
at_com_send_config("custom:profile_id", "2BF07F58");
|
|
||||||
|
|
||||||
//Send config values
|
|
||||||
at_com_send_config("control:euler_angle_max", "0.52");
|
|
||||||
at_com_send_config("control:altitude_max", "20000");
|
|
||||||
at_com_send_config("control:control_vz_max", "2000");
|
|
||||||
at_com_send_config("control:control_yaw", "6.11");
|
|
||||||
|
|
||||||
//Send config values with the airframe.h
|
|
||||||
#ifndef ARDRONE_FLIGHT_INDOOR
|
|
||||||
at_com_send_config("control:outdoor", "TRUE");
|
|
||||||
#else
|
|
||||||
at_com_send_config("control:outdoor", "FALSE");
|
|
||||||
#endif
|
|
||||||
#ifndef ARDRONE_WITHOUT_SHELL
|
|
||||||
at_com_send_config("control:flight_without_shell", "FALSE");
|
|
||||||
#else
|
|
||||||
at_com_send_config("control:flight_without_shell", "TRUE");
|
|
||||||
#endif
|
|
||||||
#ifdef ARDRONE_OWNER_MAC
|
|
||||||
at_com_send_config("network:owner_mac", ARDRONE_OWNER_MAC);
|
|
||||||
#endif
|
|
||||||
}
|
|
||||||
|
|
||||||
//Recieve a navdata packet
|
|
||||||
int at_com_recieve_navdata(unsigned char *buffer)
|
|
||||||
{
|
|
||||||
int l = sizeof(from);
|
|
||||||
int n;
|
|
||||||
// FIXME(ben): not clear why recvfrom() and not recv() is used.
|
|
||||||
n = recvfrom(navdata_socket, buffer, ARDRONE_NAVDATA_BUFFER_SIZE, 0x0,
|
|
||||||
(struct sockaddr *) &from, (socklen_t *) &l);
|
|
||||||
|
|
||||||
return n;
|
|
||||||
}
|
|
||||||
|
|
||||||
//Send an AT command
|
|
||||||
void at_com_send(char *command)
|
|
||||||
{
|
|
||||||
sendto(at_socket, command, strlen(command), 0, (struct sockaddr *) &drone_at,
|
|
||||||
sizeof(drone_at));
|
|
||||||
}
|
|
||||||
|
|
||||||
//Send a Config
|
|
||||||
void at_com_send_config(char *key, char *value)
|
|
||||||
{
|
|
||||||
char command[256];
|
|
||||||
sprintf(command, "AT*CONFIG_IDS=%d,\"%s\",\"2BF07F58\",\"9D7BFD45\"\r",
|
|
||||||
packet_seq++, sessionId);
|
|
||||||
at_com_send(command);
|
|
||||||
sprintf(command, "AT*CONFIG=%d,\"%s\",\"%s\"\r", packet_seq++, key, value);
|
|
||||||
at_com_send(command);
|
|
||||||
}
|
|
||||||
|
|
||||||
//Send a Flat trim
|
|
||||||
void at_com_send_ftrim(void)
|
|
||||||
{
|
|
||||||
char command[256];
|
|
||||||
sprintf(command, "AT*FTRIM=%d\r", packet_seq++);
|
|
||||||
at_com_send(command);
|
|
||||||
}
|
|
||||||
|
|
||||||
//Send a Ref
|
|
||||||
void at_com_send_ref(int bits)
|
|
||||||
{
|
|
||||||
char command[256];
|
|
||||||
sprintf(command, "AT*REF=%d,%d\r", packet_seq++, bits | REF_DEFAULT);
|
|
||||||
at_com_send(command);
|
|
||||||
}
|
|
||||||
|
|
||||||
//Send a Pcmd
|
|
||||||
void at_com_send_pcmd(int mode, float thrust, float roll, float pitch,
|
|
||||||
float yaw)
|
|
||||||
{
|
|
||||||
int f_thrust, f_roll, f_pitch, f_yaw;
|
|
||||||
char command[256];
|
|
||||||
|
|
||||||
//Change the floats to ints(dereferencing)
|
|
||||||
memcpy(&f_thrust, &thrust, sizeof thrust);
|
|
||||||
memcpy(&f_roll, &roll, sizeof roll);
|
|
||||||
memcpy(&f_pitch, &pitch, sizeof pitch);
|
|
||||||
memcpy(&f_yaw, &yaw, sizeof yaw);
|
|
||||||
|
|
||||||
sprintf(command, "AT*PCMD=%d,%d,%d,%d,%d,%d\r", packet_seq++, mode, f_roll,
|
|
||||||
f_pitch, f_thrust, f_yaw);
|
|
||||||
at_com_send(command);
|
|
||||||
}
|
|
||||||
|
|
||||||
//Send a Calib
|
|
||||||
void at_com_send_calib(int device)
|
|
||||||
{
|
|
||||||
char command[256];
|
|
||||||
sprintf(command, "AT*CALIB=%d,%d\r", packet_seq++, device);
|
|
||||||
at_com_send(command);
|
|
||||||
}
|
|
||||||
@@ -1,212 +0,0 @@
|
|||||||
/*
|
|
||||||
* Copyright (C) 2012-2013 Freek van Tienen
|
|
||||||
*
|
|
||||||
* This file is part of paparazzi.
|
|
||||||
*
|
|
||||||
* paparazzi is free software; you can redistribute it and/or modify
|
|
||||||
* it under the terms of the GNU General Public License as published by
|
|
||||||
* the Free Software Foundation; either version 2, or (at your option)
|
|
||||||
* any later version.
|
|
||||||
*
|
|
||||||
* paparazzi is distributed in the hope that it will be useful,
|
|
||||||
* but WITHOUT ANY WARRANTY; without even the implied warranty of
|
|
||||||
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
|
|
||||||
* GNU General Public License for more details.
|
|
||||||
*
|
|
||||||
* You should have received a copy of the GNU General Public License
|
|
||||||
* along with paparazzi; see the file COPYING. If not, write to
|
|
||||||
* the Free Software Foundation, 59 Temple Place - Suite 330,
|
|
||||||
* Boston, MA 02111-1307, USA.
|
|
||||||
*/
|
|
||||||
|
|
||||||
/**
|
|
||||||
* @file boards/ardrone/at_com.h
|
|
||||||
* Sending and receiving of AT-commands specified by the ardrone API
|
|
||||||
*/
|
|
||||||
|
|
||||||
#include "math/pprz_algebra_float.h"
|
|
||||||
|
|
||||||
#ifndef BOARDS_ARDRONE_AT_COM_H
|
|
||||||
#define BOARDS_ARDRONE_AT_COM_H
|
|
||||||
|
|
||||||
#define NAVDATA_HEADER (0x55667788)
|
|
||||||
|
|
||||||
//Define the AT_REF bits
|
|
||||||
typedef enum {
|
|
||||||
REF_TAKEOFF = 1U << 9,
|
|
||||||
REF_EMERGENCY = 1U << 8,
|
|
||||||
REF_DEFAULT = 0x11540000
|
|
||||||
} AT_REFS;
|
|
||||||
|
|
||||||
//Define control states
|
|
||||||
typedef enum {
|
|
||||||
CTRL_DEFAULT,
|
|
||||||
CTRL_INIT,
|
|
||||||
CTRL_LANDED,
|
|
||||||
CTRL_FLYING,
|
|
||||||
CTRL_HOVERING,
|
|
||||||
CTRL_TEST,
|
|
||||||
CTRL_TRANS_TAKEOFF,
|
|
||||||
CTRL_TRANS_GOTOFIX,
|
|
||||||
CTRL_TRANS_LANDING,
|
|
||||||
CTRL_TRANS_LOOPING,
|
|
||||||
CTRL_NUM_STATES
|
|
||||||
} CTRL_STATES;
|
|
||||||
|
|
||||||
//Define the AR.Drone states
|
|
||||||
typedef enum {
|
|
||||||
ARDRONE_FLY_MASK = 1U << 0, /*!< FLY MASK : (0) ardrone is landed, (1) ardrone is flying */
|
|
||||||
ARDRONE_VIDEO_MASK = 1U << 1, /*!< VIDEO MASK : (0) video disable, (1) video enable */
|
|
||||||
ARDRONE_VISION_MASK = 1U << 2, /*!< VISION MASK : (0) vision disable, (1) vision enable */
|
|
||||||
ARDRONE_CONTROL_MASK = 1U << 3, /*!< CONTROL ALGO : (0) euler angles control, (1) angular speed control */
|
|
||||||
ARDRONE_ALTITUDE_MASK = 1U << 4, /*!< ALTITUDE CONTROL ALGO : (0) altitude control inactive (1) altitude control active */
|
|
||||||
ARDRONE_USER_FEEDBACK_START = 1U << 5, /*!< USER feedback : Start button state */
|
|
||||||
ARDRONE_COMMAND_MASK = 1U << 6, /*!< Control command ACK : (0) None, (1) one received */
|
|
||||||
ARDRONE_CAMERA_MASK = 1U << 7, /*!< CAMERA MASK : (0) camera not ready, (1) Camera ready */
|
|
||||||
ARDRONE_TRAVELLING_MASK = 1U << 8, /*!< Travelling mask : (0) disable, (1) enable */
|
|
||||||
ARDRONE_USB_MASK = 1U << 9, /*!< USB key : (0) usb key not ready, (1) usb key ready */
|
|
||||||
ARDRONE_NAVDATA_DEMO_MASK = 1U << 10, /*!< Navdata demo : (0) All navdata, (1) only navdata demo */
|
|
||||||
ARDRONE_NAVDATA_BOOTSTRAP = 1U << 11, /*!< Navdata bootstrap : (0) options sent in all or demo mode, (1) no navdata options sent */
|
|
||||||
ARDRONE_MOTORS_MASK = 1U << 12, /*!< Motors status : (0) Ok, (1) Motors problem */
|
|
||||||
ARDRONE_COM_LOST_MASK = 1U << 13, /*!< Communication Lost : (1) com problem, (0) Com is ok */
|
|
||||||
ARDRONE_SOFTWARE_FAULT = 1U << 14, /*!< Software fault detected - user should land as quick as possible (1) */
|
|
||||||
ARDRONE_VBAT_LOW = 1U << 15, /*!< VBat low : (1) too low, (0) Ok */
|
|
||||||
ARDRONE_USER_EL = 1U << 16, /*!< User Emergency Landing : (1) User EL is ON, (0) User EL is OFF*/
|
|
||||||
ARDRONE_TIMER_ELAPSED = 1U << 17, /*!< Timer elapsed : (1) elapsed, (0) not elapsed */
|
|
||||||
ARDRONE_MAGNETO_NEEDS_CALIB = 1U << 18, /*!< Magnetometer calibration state : (0) Ok, no calibration needed, (1) not ok, calibration needed */
|
|
||||||
ARDRONE_ANGLES_OUT_OF_RANGE = 1U << 19, /*!< Angles : (0) Ok, (1) out of range */
|
|
||||||
ARDRONE_WIND_MASK = 1U << 20, /*!< WIND MASK: (0) ok, (1) Too much wind */
|
|
||||||
ARDRONE_ULTRASOUND_MASK = 1U << 21, /*!< Ultrasonic sensor : (0) Ok, (1) deaf */
|
|
||||||
ARDRONE_CUTOUT_MASK = 1U << 22, /*!< Cutout system detection : (0) Not detected, (1) detected */
|
|
||||||
ARDRONE_PIC_VERSION_MASK = 1U << 23, /*!< PIC Version number OK : (0) a bad version number, (1) version number is OK */
|
|
||||||
ARDRONE_ATCODEC_THREAD_ON = 1U << 24, /*!< ATCodec thread ON : (0) thread OFF (1) thread ON */
|
|
||||||
ARDRONE_NAVDATA_THREAD_ON = 1U << 25, /*!< Navdata thread ON : (0) thread OFF (1) thread ON */
|
|
||||||
ARDRONE_VIDEO_THREAD_ON = 1U << 26, /*!< Video thread ON : (0) thread OFF (1) thread ON */
|
|
||||||
ARDRONE_ACQ_THREAD_ON = 1U << 27, /*!< Acquisition thread ON : (0) thread OFF (1) thread ON */
|
|
||||||
ARDRONE_CTRL_WATCHDOG_MASK = 1U << 28, /*!< CTRL watchdog : (1) delay in control execution (> 5ms), (0) control is well scheduled */
|
|
||||||
ARDRONE_ADC_WATCHDOG_MASK = 1U << 29, /*!< ADC Watchdog : (1) delay in uart2 dsr (> 5ms), (0) uart2 is good */
|
|
||||||
ARDRONE_COM_WATCHDOG_MASK = 1U << 30, /*!< Communication Watchdog : (1) com problem, (0) Com is ok */
|
|
||||||
ARDRONE_EMERGENCY_MASK = 1U << 31 /*!< Emergency landing : (0) no emergency, (1) emergency */
|
|
||||||
} ARDRONE_STATES;
|
|
||||||
|
|
||||||
//Navdata option packet without data
|
|
||||||
typedef struct _navdata_option_t {
|
|
||||||
uint16_t tag;
|
|
||||||
uint16_t size;
|
|
||||||
uint8_t data[1];
|
|
||||||
} __attribute__((packed)) navdata_option_t;
|
|
||||||
|
|
||||||
//Main navdata packet
|
|
||||||
typedef struct _navdata_t {
|
|
||||||
uint32_t header; /*!< Always set to NAVDATA_HEADER */
|
|
||||||
uint32_t ardrone_state; /*!< Bit mask built from def_ardrone_state_mask_t */
|
|
||||||
uint32_t sequence; /*!< Sequence number, incremented for each sent packet */
|
|
||||||
uint32_t vision_defined;
|
|
||||||
|
|
||||||
navdata_option_t options[1];
|
|
||||||
} __attribute__((packed)) navdata_t;
|
|
||||||
|
|
||||||
//Navdata checksum packet
|
|
||||||
typedef struct _navdata_cks_t {
|
|
||||||
uint16_t tag;
|
|
||||||
uint16_t size;
|
|
||||||
uint32_t cks;
|
|
||||||
} __attribute__((packed)) navdata_cks_t;
|
|
||||||
|
|
||||||
//Navdata demo option
|
|
||||||
typedef struct _navdata_demo_t {
|
|
||||||
uint16_t tag; /*!< Navdata block ('option') identifier */
|
|
||||||
uint16_t size; /*!< set this to the size of this structure */
|
|
||||||
uint32_t ctrl_state; /*!< Flying state (landed, flying, hovering, etc.) defined in CTRL_STATES enum. */
|
|
||||||
uint32_t vbat_flying_percentage; /*!< battery voltage filtered (mV) */
|
|
||||||
float theta; /*!< UAV's pitch in milli-degrees */
|
|
||||||
float phi; /*!< UAV's roll in milli-degrees */
|
|
||||||
float psi; /*!< UAV's yaw in milli-degrees */
|
|
||||||
int32_t altitude; /*!< UAV's altitude in centimeters */
|
|
||||||
float vx; /*!< UAV's estimated linear velocity */
|
|
||||||
float vy; /*!< UAV's estimated linear velocity */
|
|
||||||
float vz; /*!< UAV's estimated linear velocity */
|
|
||||||
uint32_t num_frames; /*!< streamed frame index */ // Not used -> To integrate in video stage.
|
|
||||||
// Camera parameters compute by detection
|
|
||||||
struct FloatMat33 detection_camera_rot; /*!< Deprecated ! Don't use ! */
|
|
||||||
struct FloatVect3 detection_camera_trans; /*!< Deprecated ! Don't use ! */
|
|
||||||
uint32_t detection_tag_index; /*!< Deprecated ! Don't use ! */
|
|
||||||
uint32_t detection_camera_type; /*!< Type of tag searched in detection */
|
|
||||||
// Camera parameters compute by drone
|
|
||||||
struct FloatMat33 drone_camera_rot; /*!< Deprecated ! Don't use ! */
|
|
||||||
struct FloatVect3 drone_camera_trans; /*!< Deprecated ! Don't use ! */
|
|
||||||
} __attribute__((packed)) navdata_demo_t;
|
|
||||||
|
|
||||||
//Navdata physical measures option
|
|
||||||
typedef struct _navdata_phys_measures_t {
|
|
||||||
uint16_t tag;
|
|
||||||
uint16_t size;
|
|
||||||
|
|
||||||
float accs_temp;
|
|
||||||
uint16_t gyro_temp;
|
|
||||||
struct FloatVect3 phys_accs;
|
|
||||||
struct FloatVect3 phys_gyros;
|
|
||||||
uint32_t alim3V3; // 3.3volt alim [LSB]
|
|
||||||
uint32_t vrefEpson; // ref volt Epson gyro [LSB]
|
|
||||||
uint32_t vrefIDG; // ref volt IDG gyro [LSB]
|
|
||||||
} __attribute__((packed)) navdata_phys_measures_t;
|
|
||||||
|
|
||||||
//Navdata gps packet
|
|
||||||
typedef double float64_t; //TODO: Fix this nicely, but this is only used here
|
|
||||||
typedef float float32_t; //TODO: Fix this nicely, but this is only used here
|
|
||||||
typedef struct _navdata_gps_t {
|
|
||||||
uint16_t tag; /*!< Navdata block ('option') identifier */
|
|
||||||
uint16_t size; /*!< set this to the size of this structure */
|
|
||||||
float64_t lat; /*!< Latitude */
|
|
||||||
float64_t lon; /*!< Longitude */
|
|
||||||
float64_t elevation; /*!< Elevation */
|
|
||||||
float64_t hdop; /*!< hdop */
|
|
||||||
int32_t data_available; /*!< When there is data available */
|
|
||||||
uint8_t unk_0[8];
|
|
||||||
float64_t lat0; /*!< Latitude ??? */
|
|
||||||
float64_t lon0; /*!< Longitude ??? */
|
|
||||||
float64_t lat_fuse; /*!< Latitude fused */
|
|
||||||
float64_t lon_fuse; /*!< Longitude fused */
|
|
||||||
uint32_t gps_state; /*!< State of the GPS, still need to figure out */
|
|
||||||
uint8_t unk_1[40];
|
|
||||||
float64_t vdop; /*!< vdop */
|
|
||||||
float64_t pdop; /*!< pdop */
|
|
||||||
float32_t speed; /*!< speed */
|
|
||||||
uint32_t last_frame_timestamp; /*!< Timestamp from the last frame */
|
|
||||||
float32_t degree; /*!< Degree */
|
|
||||||
float32_t degree_mag; /*!< Degree of the magnetic */
|
|
||||||
uint8_t unk_2[16];
|
|
||||||
struct {
|
|
||||||
uint8_t sat;
|
|
||||||
uint8_t cn0;
|
|
||||||
} channels[12];
|
|
||||||
int32_t gps_plugged; /*!< When the gps is plugged */
|
|
||||||
uint8_t unk_3[108];
|
|
||||||
float64_t gps_time; /*!< The gps time of week */
|
|
||||||
uint16_t week; /*!< The gps week */
|
|
||||||
uint8_t gps_fix; /*!< The gps fix */
|
|
||||||
uint8_t num_sattelites; /*!< Number of sattelites */
|
|
||||||
uint8_t unk_4[24];
|
|
||||||
float64_t ned_vel_c0; /*!< NED velocity */
|
|
||||||
float64_t ned_vel_c1; /*!< NED velocity */
|
|
||||||
float64_t ned_vel_c2; /*!< NED velocity */
|
|
||||||
float64_t pos_accur_c0; /*!< Position accuracy */
|
|
||||||
float64_t pos_accur_c1; /*!< Position accuracy */
|
|
||||||
float64_t pos_accur_c2; /*!< Position accuracy */
|
|
||||||
float32_t speed_acur; /*!< Speed accuracy */
|
|
||||||
float32_t time_acur; /*!< Time accuracy */
|
|
||||||
uint8_t unk_5[72];
|
|
||||||
float32_t temprature;
|
|
||||||
float32_t pressure;
|
|
||||||
} __attribute__((packed)) navdata_gps_t;
|
|
||||||
|
|
||||||
//External functions
|
|
||||||
extern void init_at_com(void);
|
|
||||||
extern int at_com_recieve_navdata(unsigned char *buffer);
|
|
||||||
extern void at_com_send_config(char *key, char *value);
|
|
||||||
extern void at_com_send_ftrim(void);
|
|
||||||
extern void at_com_send_ref(int bits);
|
|
||||||
extern void at_com_send_pcmd(int mode, float thrust, float roll, float pitch, float yaw);
|
|
||||||
extern void at_com_send_calib(int device);
|
|
||||||
|
|
||||||
#endif /* BOARDS_ARDRONE_AT_COM_H */
|
|
||||||
@@ -1,37 +0,0 @@
|
|||||||
/*
|
|
||||||
*
|
|
||||||
* Copyright (C) 2009-2013 The Paparazzi Team
|
|
||||||
*
|
|
||||||
* This file is part of paparazzi.
|
|
||||||
*
|
|
||||||
* paparazzi is free software; you can redistribute it and/or modify
|
|
||||||
* it under the terms of the GNU General Public License as published by
|
|
||||||
* the Free Software Foundation; either version 2, or (at your option)
|
|
||||||
* any later version.
|
|
||||||
*
|
|
||||||
* paparazzi is distributed in the hope that it will be useful,
|
|
||||||
* but WITHOUT ANY WARRANTY; without even the implied warranty of
|
|
||||||
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
|
|
||||||
* GNU General Public License for more details.
|
|
||||||
*
|
|
||||||
* You should have received a copy of the GNU General Public License
|
|
||||||
* along with paparazzi; see the file COPYING. If not, write to
|
|
||||||
* the Free Software Foundation, 59 Temple Place - Suite 330,
|
|
||||||
* Boston, MA 02111-1307, USA.
|
|
||||||
*
|
|
||||||
*/
|
|
||||||
|
|
||||||
/**
|
|
||||||
* @file boards/ardrone/electrical_dummy.c
|
|
||||||
* dummy electrical status readings for ardrone-sdk version.
|
|
||||||
*
|
|
||||||
* Because ardrone2-sdk version does its battery updating in ahrs_adrone2.c.
|
|
||||||
*/
|
|
||||||
|
|
||||||
#include "subsystems/electrical.h"
|
|
||||||
|
|
||||||
struct Electrical electrical;
|
|
||||||
|
|
||||||
void electrical_init(void) { }
|
|
||||||
|
|
||||||
void electrical_periodic(void) { }
|
|
||||||
@@ -1,22 +0,0 @@
|
|||||||
#ifndef CONFIG_ARDRONE2_SDK
|
|
||||||
#define CONFIG_ARDRONE2_SDK
|
|
||||||
|
|
||||||
#define BOARD_ARDRONE2_SDK
|
|
||||||
|
|
||||||
#define UART1_DEV "/dev/ttyUSB0"
|
|
||||||
|
|
||||||
/* Internal communication */
|
|
||||||
#define ARDRONE_NAVDATA_PORT 5554
|
|
||||||
#define ARDRONE_AT_PORT 5556
|
|
||||||
#define ARDRONE_NAVDATA_BUFFER_SIZE 4096
|
|
||||||
#define ARDRONE_IP "192.168.1.1"
|
|
||||||
|
|
||||||
/* Default actuators driver */
|
|
||||||
#define DEFAULT_ACTUATORS "boards/ardrone/actuators_at.h"
|
|
||||||
#define ActuatorDefaultSet(_x,_y) {}
|
|
||||||
#define ActuatorsDefaultInit() {}
|
|
||||||
#define ActuatorsDefaultCommit() {}
|
|
||||||
|
|
||||||
#define USE_BARO_BOARD 0
|
|
||||||
|
|
||||||
#endif /* CONFIG_ARDRONE2_SDK */
|
|
||||||
@@ -1,207 +0,0 @@
|
|||||||
/*
|
|
||||||
* Copyright (C) 2012-2013 Freek van Tienen
|
|
||||||
*
|
|
||||||
* This file is part of Paparazzi.
|
|
||||||
*
|
|
||||||
* Paparazzi is free software; you can redistribute it and/or modify
|
|
||||||
* it under the terms of the GNU General Public License as published by
|
|
||||||
* the Free Software Foundation; either version 2, or (at your option)
|
|
||||||
* any later version.
|
|
||||||
*
|
|
||||||
* Paparazzi is distributed in the hope that it will be useful,
|
|
||||||
* but WITHOUT ANY WARRANTY; without even the implied warranty of
|
|
||||||
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
|
|
||||||
* GNU General Public License for more details.
|
|
||||||
*
|
|
||||||
* You should have received a copy of the GNU General Public License
|
|
||||||
* along with Paparazzi; see the file COPYING. If not, write to
|
|
||||||
* the Free Software Foundation, 59 Temple Place - Suite 330,
|
|
||||||
* Boston, MA 02111-1307, USA.
|
|
||||||
*/
|
|
||||||
|
|
||||||
/**
|
|
||||||
* @file subsystems/ahrs/ahrs_ardrone2.c
|
|
||||||
* AHRS implementation for ardrone2-sdk based on AT-commands.
|
|
||||||
*
|
|
||||||
* Uses AT-Commands to communicate with ardrone api to retrieve AHRS data
|
|
||||||
* and also sets battery level.
|
|
||||||
*/
|
|
||||||
|
|
||||||
#ifdef ARDRONE2_DEBUG
|
|
||||||
# include <errno.h>
|
|
||||||
# include <stdio.h>
|
|
||||||
#endif
|
|
||||||
|
|
||||||
#include "subsystems/ahrs.h"
|
|
||||||
#include "ahrs_ardrone2.h"
|
|
||||||
#include "state.h"
|
|
||||||
#include "math/pprz_algebra_float.h"
|
|
||||||
#include "boards/ardrone/at_com.h"
|
|
||||||
#include "subsystems/electrical.h"
|
|
||||||
|
|
||||||
#ifdef USE_GPS_ARDRONE2
|
|
||||||
#include "subsystems/gps/gps_ardrone2.h"
|
|
||||||
#endif
|
|
||||||
|
|
||||||
struct AhrsARDrone ahrs_ardrone2;
|
|
||||||
unsigned char buffer[4096]; //Packet buffer
|
|
||||||
|
|
||||||
#if PERIODIC_TELEMETRY
|
|
||||||
#include "subsystems/datalink/telemetry.h"
|
|
||||||
|
|
||||||
static void send_ahrs_ad2(struct transport_tx *trans, struct link_device *dev)
|
|
||||||
{
|
|
||||||
pprz_msg_send_AHRS_ARDRONE2(trans, dev, AC_ID,
|
|
||||||
&ahrs_ardrone2.state,
|
|
||||||
&ahrs_ardrone2.control_state,
|
|
||||||
&ahrs_ardrone2.eulers.phi,
|
|
||||||
&ahrs_ardrone2.eulers.theta,
|
|
||||||
&ahrs_ardrone2.eulers.psi,
|
|
||||||
&ahrs_ardrone2.speed.x,
|
|
||||||
&ahrs_ardrone2.speed.y,
|
|
||||||
&ahrs_ardrone2.speed.z,
|
|
||||||
&ahrs_ardrone2.accel.x,
|
|
||||||
&ahrs_ardrone2.accel.y,
|
|
||||||
&ahrs_ardrone2.accel.z,
|
|
||||||
&ahrs_ardrone2.altitude,
|
|
||||||
&ahrs_ardrone2.battery);
|
|
||||||
}
|
|
||||||
#endif
|
|
||||||
|
|
||||||
void ahrs_ardrone2_register(void)
|
|
||||||
{
|
|
||||||
ahrs_ardrone2_init();
|
|
||||||
/// @todo: provide enable function
|
|
||||||
ahrs_register_impl(NULL);
|
|
||||||
}
|
|
||||||
|
|
||||||
void ahrs_ardrone2_init(void)
|
|
||||||
{
|
|
||||||
init_at_com();
|
|
||||||
|
|
||||||
//Set navdata_demo to FALSE and flat trim the ar drone
|
|
||||||
at_com_send_config("general:navdata_demo", "FALSE");
|
|
||||||
at_com_send_ftrim();
|
|
||||||
|
|
||||||
ahrs_ardrone2.is_aligned = TRUE;
|
|
||||||
|
|
||||||
#if PERIODIC_TELEMETRY
|
|
||||||
register_periodic_telemetry(DefaultPeriodic, "AHRS_ARDRONE2", send_ahrs_ad2);
|
|
||||||
#endif
|
|
||||||
}
|
|
||||||
|
|
||||||
#ifdef ARDRONE2_DEBUG
|
|
||||||
static void dump(const void *_b, size_t s)
|
|
||||||
{
|
|
||||||
const unsigned char *b = _b;
|
|
||||||
size_t n;
|
|
||||||
|
|
||||||
for (n = 0; n < s; ++n) {
|
|
||||||
printf("%02x ", b[n]);
|
|
||||||
if (n % 16 == 15) {
|
|
||||||
printf("\n");
|
|
||||||
}
|
|
||||||
}
|
|
||||||
if (n % 16 != 0) {
|
|
||||||
printf("\n");
|
|
||||||
}
|
|
||||||
}
|
|
||||||
#endif
|
|
||||||
|
|
||||||
void ahrs_ardrone2_propagate(void)
|
|
||||||
{
|
|
||||||
int l;
|
|
||||||
|
|
||||||
//Recieve the main packet
|
|
||||||
l = at_com_recieve_navdata(buffer);
|
|
||||||
navdata_t *main_packet = (navdata_t *) &buffer;
|
|
||||||
|
|
||||||
#ifdef ARDRONE2_DEBUG
|
|
||||||
if (l < 0) {
|
|
||||||
printf("errno = %d\n", errno);
|
|
||||||
}
|
|
||||||
#endif
|
|
||||||
|
|
||||||
//When this isn't a valid packet return
|
|
||||||
if (l < 0 || main_packet->header != NAVDATA_HEADER) {
|
|
||||||
return;
|
|
||||||
}
|
|
||||||
|
|
||||||
#ifdef ARDRONE2_DEBUG
|
|
||||||
printf("Read %d\n", l);
|
|
||||||
dump(buffer, l);
|
|
||||||
#endif
|
|
||||||
|
|
||||||
//Set the state
|
|
||||||
ahrs_ardrone2.state = main_packet->ardrone_state;
|
|
||||||
|
|
||||||
//Init the option
|
|
||||||
navdata_option_t *navdata_option = (navdata_option_t *) & (main_packet->options[0]);
|
|
||||||
bool_t full_read = FALSE;
|
|
||||||
|
|
||||||
//The possible packets
|
|
||||||
navdata_demo_t *navdata_demo;
|
|
||||||
navdata_gps_t *navdata_gps;
|
|
||||||
navdata_phys_measures_t *navdata_phys_measures;
|
|
||||||
|
|
||||||
//Read the navdata until packet is fully readed
|
|
||||||
while (!full_read && navdata_option->size > 0) {
|
|
||||||
#ifdef ARDRONE2_DEBUG
|
|
||||||
printf("tag = %d\n", navdata_option->tag);
|
|
||||||
#endif
|
|
||||||
//Check the tag for the right option
|
|
||||||
switch (navdata_option->tag) {
|
|
||||||
case 0: //NAVDATA_DEMO
|
|
||||||
navdata_demo = (navdata_demo_t *) navdata_option;
|
|
||||||
|
|
||||||
//Set the AHRS state
|
|
||||||
ahrs_ardrone2.control_state = navdata_demo->ctrl_state >> 16;
|
|
||||||
ahrs_ardrone2.eulers.phi = navdata_demo->phi;
|
|
||||||
ahrs_ardrone2.eulers.theta = navdata_demo->theta;
|
|
||||||
ahrs_ardrone2.eulers.psi = navdata_demo->psi;
|
|
||||||
ahrs_ardrone2.speed.x = navdata_demo->vx / 1000;
|
|
||||||
ahrs_ardrone2.speed.y = navdata_demo->vy / 1000;
|
|
||||||
ahrs_ardrone2.speed.z = navdata_demo->vz / 1000;
|
|
||||||
ahrs_ardrone2.altitude = navdata_demo->altitude / 10;
|
|
||||||
ahrs_ardrone2.battery = navdata_demo->vbat_flying_percentage;
|
|
||||||
|
|
||||||
//Set the ned to body eulers
|
|
||||||
struct FloatEulers angles;
|
|
||||||
angles.theta = navdata_demo->theta / 180000.*M_PI;
|
|
||||||
angles.psi = navdata_demo->psi / 180000.*M_PI;
|
|
||||||
angles.phi = navdata_demo->phi / 180000.*M_PI;
|
|
||||||
stateSetNedToBodyEulers_f(&angles);
|
|
||||||
|
|
||||||
//Update the electrical supply
|
|
||||||
electrical.vsupply = navdata_demo->vbat_flying_percentage;
|
|
||||||
break;
|
|
||||||
case 3: //NAVDATA_PHYS_MEASURES
|
|
||||||
navdata_phys_measures = (navdata_phys_measures_t *) navdata_option;
|
|
||||||
|
|
||||||
//Set the AHRS accel state
|
|
||||||
INT32_VECT3_SCALE_2(ahrs_ardrone2.accel, navdata_phys_measures->phys_accs, 9.81, 1000)
|
|
||||||
break;
|
|
||||||
#ifdef USE_GPS_ARDRONE2
|
|
||||||
case 27: //NAVDATA_GPS
|
|
||||||
# ifdef ARDRONE2_DEBUG
|
|
||||||
dump(navdata_option, navdata_option->size);
|
|
||||||
# endif
|
|
||||||
navdata_gps = (navdata_gps_t *) navdata_option;
|
|
||||||
|
|
||||||
// Send the data to the gps parser
|
|
||||||
gps_ardrone2_parse(navdata_gps);
|
|
||||||
break;
|
|
||||||
#endif
|
|
||||||
case 0xFFFF: //CHECKSUM
|
|
||||||
//TODO: Check the checksum
|
|
||||||
full_read = TRUE;
|
|
||||||
break;
|
|
||||||
default:
|
|
||||||
#ifdef ARDRONE2_DEBUG
|
|
||||||
printf("NAVDATA UNKNOWN TAG: %d %d\n", navdata_option->tag, navdata_option->size);
|
|
||||||
#endif
|
|
||||||
break;
|
|
||||||
}
|
|
||||||
navdata_option = (navdata_option_t *)((uint32_t)navdata_option + navdata_option->size);
|
|
||||||
}
|
|
||||||
}
|
|
||||||
@@ -1,58 +0,0 @@
|
|||||||
/*
|
|
||||||
* Copyright (C) 2012-2013 Freek van Tienen
|
|
||||||
*
|
|
||||||
* This file is part of Paparazzi.
|
|
||||||
*
|
|
||||||
* Paparazzi is free software; you can redistribute it and/or modify
|
|
||||||
* it under the terms of the GNU General Public License as published by
|
|
||||||
* the Free Software Foundation; either version 2, or (at your option)
|
|
||||||
* any later version.
|
|
||||||
*
|
|
||||||
* Paparazzi is distributed in the hope that it will be useful,
|
|
||||||
* but WITHOUT ANY WARRANTY; without even the implied warranty of
|
|
||||||
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
|
|
||||||
* GNU General Public License for more details.
|
|
||||||
*
|
|
||||||
* You should have received a copy of the GNU General Public License
|
|
||||||
* along with Paparazzi; see the file COPYING. If not, write to
|
|
||||||
* the Free Software Foundation, 59 Temple Place - Suite 330,
|
|
||||||
* Boston, MA 02111-1307, USA.
|
|
||||||
*/
|
|
||||||
|
|
||||||
/**
|
|
||||||
* @file subsystems/ahrs/ahrs_ardrone2.h
|
|
||||||
* AHRS implementation for ardrone2-sdk based on AT-commands.
|
|
||||||
*
|
|
||||||
* Uses AT-Commands to communicate with ardrone api to retrieve AHRS data
|
|
||||||
* and also sets battery level.
|
|
||||||
*/
|
|
||||||
|
|
||||||
#ifndef AHRS_ARDRONE2_H
|
|
||||||
#define AHRS_ARDRONE2_H
|
|
||||||
|
|
||||||
#include "std.h"
|
|
||||||
#include "math/pprz_algebra_int.h"
|
|
||||||
#include "math/pprz_geodetic_float.h"
|
|
||||||
|
|
||||||
struct AhrsARDrone {
|
|
||||||
uint32_t state; // ARDRONE_STATES
|
|
||||||
uint32_t control_state; // CTRL_STATES
|
|
||||||
struct FloatEulers eulers; // in radians
|
|
||||||
struct NedCoor_f speed; // in m/s
|
|
||||||
struct NedCoor_f accel; // in m/s^2
|
|
||||||
int32_t altitude; // in cm above ground
|
|
||||||
uint32_t battery; // in percentage
|
|
||||||
struct Int32Quat ltp_to_imu_quat;
|
|
||||||
bool_t is_aligned;
|
|
||||||
};
|
|
||||||
extern struct AhrsARDrone ahrs_ardrone2;
|
|
||||||
|
|
||||||
#ifndef PRIMARY_AHRS
|
|
||||||
#define PRIMARY_AHRS ahrs_ardrone2
|
|
||||||
#endif
|
|
||||||
|
|
||||||
extern void ahrs_ardrone2_register(void);
|
|
||||||
extern void ahrs_ardrone2_init(void);
|
|
||||||
extern void ahrs_ardrone2_propagate(void);
|
|
||||||
|
|
||||||
#endif /* AHRS_ARDRONE2_H */
|
|
||||||
@@ -1,86 +0,0 @@
|
|||||||
/*
|
|
||||||
*
|
|
||||||
* Copyright (C) 2013 Freek van Tienen <freek.v.tienen@gmail.com>
|
|
||||||
*
|
|
||||||
* This file is part of paparazzi.
|
|
||||||
*
|
|
||||||
* paparazzi is free software; you can redistribute it and/or modify
|
|
||||||
* it under the terms of the GNU General Public License as published by
|
|
||||||
* the Free Software Foundation; either version 2, or (at your option)
|
|
||||||
* any later version.
|
|
||||||
*
|
|
||||||
* paparazzi is distributed in the hope that it will be useful,
|
|
||||||
* but WITHOUT ANY WARRANTY; without even the implied warranty of
|
|
||||||
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
|
|
||||||
* GNU General Public License for more details.
|
|
||||||
*
|
|
||||||
* You should have received a copy of the GNU General Public License
|
|
||||||
* along with paparazzi; see the file COPYING. If not, write to
|
|
||||||
* the Free Software Foundation, 59 Temple Place - Suite 330,
|
|
||||||
* Boston, MA 02111-1307, USA.
|
|
||||||
*
|
|
||||||
*/
|
|
||||||
|
|
||||||
/**
|
|
||||||
* @file subsystems/gps/gps_ardrone2.c
|
|
||||||
* ARdrone 2 gps trough navdata for the SDK version and only works in combination with the ahrs ardrone2.
|
|
||||||
*/
|
|
||||||
|
|
||||||
#ifdef ARDRONE2_DEBUG
|
|
||||||
# include <stdio.h>
|
|
||||||
#endif
|
|
||||||
|
|
||||||
#include "subsystems/gps.h"
|
|
||||||
#include "subsystems/abi.h"
|
|
||||||
#include "math/pprz_geodetic_double.h"
|
|
||||||
|
|
||||||
void gps_impl_init(void)
|
|
||||||
{
|
|
||||||
}
|
|
||||||
|
|
||||||
void gps_ardrone2_parse(navdata_gps_t *navdata_gps)
|
|
||||||
{
|
|
||||||
int i;
|
|
||||||
|
|
||||||
#ifdef ARDRONE2_DEBUG
|
|
||||||
printf("state = %d\n", navdata_gps->gps_state);
|
|
||||||
#endif
|
|
||||||
// Set the lla double struct from the navdata
|
|
||||||
struct LlaCoor_d gps_lla_d;
|
|
||||||
gps_lla_d.lat = RadOfDeg(navdata_gps->lat);
|
|
||||||
gps_lla_d.lon = RadOfDeg(navdata_gps->lon);
|
|
||||||
gps_lla_d.alt = navdata_gps->elevation;
|
|
||||||
|
|
||||||
// Convert it to ecef
|
|
||||||
struct EcefCoor_d gps_ecef_d;
|
|
||||||
ecef_of_lla_d(&gps_ecef_d, &gps_lla_d);
|
|
||||||
|
|
||||||
// Convert the lla and ecef to int and set them in gps
|
|
||||||
ECEF_BFP_OF_REAL(gps.ecef_pos, gps_ecef_d);
|
|
||||||
LLA_BFP_OF_REAL(gps.lla_pos, gps_lla_d);
|
|
||||||
|
|
||||||
// TODO: parse other stuff
|
|
||||||
gps.nb_channels = GPS_NB_CHANNELS;
|
|
||||||
|
|
||||||
for (i = 0; i < GPS_NB_CHANNELS; i++) {
|
|
||||||
gps.svinfos[i].svid = navdata_gps->channels[i].sat;
|
|
||||||
gps.svinfos[i].cno = navdata_gps->channels[i].cn0;
|
|
||||||
}
|
|
||||||
|
|
||||||
// Check if we have a fix TODO: check if 2D or 3D fix?
|
|
||||||
if (navdata_gps->gps_state == 1) {
|
|
||||||
gps.fix = GPS_FIX_3D;
|
|
||||||
} else {
|
|
||||||
gps.fix = GPS_FIX_NONE;
|
|
||||||
}
|
|
||||||
|
|
||||||
gps.last_msg_ticks = sys_time.nb_sec_rem;
|
|
||||||
gps.last_msg_time = sys_time.nb_sec;
|
|
||||||
if (gps.fix == GPS_FIX_3D) {
|
|
||||||
gps.last_3dfix_ticks = sys_time.nb_sec_rem;
|
|
||||||
gps.last_3dfix_time = sys_time.nb_sec;
|
|
||||||
}
|
|
||||||
|
|
||||||
uint32_t now_ts = get_sys_time_usec();
|
|
||||||
AbiSendMsgGPS(GPS_ARDRONE2_ID, now_ts, &gps);
|
|
||||||
}
|
|
||||||
@@ -1,46 +0,0 @@
|
|||||||
/*
|
|
||||||
* Copyright (C) 2013 Freek van Tienen <freek.v.tienen@gmail.com>
|
|
||||||
*
|
|
||||||
* This file is part of paparazzi.
|
|
||||||
*
|
|
||||||
* paparazzi is free software; you can redistribute it and/or modify
|
|
||||||
* it under the terms of the GNU General Public License as published by
|
|
||||||
* the Free Software Foundation; either version 2, or (at your option)
|
|
||||||
* any later version.
|
|
||||||
*
|
|
||||||
* paparazzi is distributed in the hope that it will be useful,
|
|
||||||
* but WITHOUT ANY WARRANTY; without even the implied warranty of
|
|
||||||
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
|
|
||||||
* GNU General Public License for more details.
|
|
||||||
*
|
|
||||||
* You should have received a copy of the GNU General Public License
|
|
||||||
* along with paparazzi; see the file COPYING. If not, write to
|
|
||||||
* the Free Software Foundation, 59 Temple Place - Suite 330,
|
|
||||||
* Boston, MA 02111-1307, USA.
|
|
||||||
*
|
|
||||||
*/
|
|
||||||
|
|
||||||
/** @file subsystems/gps/gps_ardrone2.h
|
|
||||||
* ARdrone 2 gps trough navdata for the SDK version and only works in combination with the ahrs ardrone2.
|
|
||||||
*
|
|
||||||
*/
|
|
||||||
|
|
||||||
#ifndef GPS_ARDRONE_H
|
|
||||||
#define GPS_ARDRONE_H
|
|
||||||
|
|
||||||
#include "boards/ardrone/at_com.h"
|
|
||||||
|
|
||||||
#define GPS_NB_CHANNELS 12
|
|
||||||
|
|
||||||
/*
|
|
||||||
* The GPS event (dummy)
|
|
||||||
*/
|
|
||||||
#define GpsEvent() {}
|
|
||||||
|
|
||||||
void gps_ardrone2_parse(navdata_gps_t *navdata_gps);
|
|
||||||
|
|
||||||
/* Maybe needed?
|
|
||||||
#define gps_nmea_Reset(_val) { }
|
|
||||||
*/
|
|
||||||
|
|
||||||
#endif /* GPS_ARDRONE_H */
|
|
||||||
@@ -1,39 +0,0 @@
|
|||||||
/*
|
|
||||||
* Copyright (C) 2013 Dino Hensen
|
|
||||||
*
|
|
||||||
* This file is part of Paparazzi.
|
|
||||||
*
|
|
||||||
* Paparazzi is free software; you can redistribute it and/or modify
|
|
||||||
* it under the terms of the GNU General Public License as published by
|
|
||||||
* the Free Software Foundation; either version 2, or (at your option)
|
|
||||||
* any later version.
|
|
||||||
*
|
|
||||||
* Paparazzi is distributed in the hope that it will be useful,
|
|
||||||
* but WITHOUT ANY WARRANTY; without even the implied warranty of
|
|
||||||
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
|
|
||||||
* GNU General Public License for more details.
|
|
||||||
*
|
|
||||||
* You should have received a copy of the GNU General Public License
|
|
||||||
* along with Paparazzi; see the file COPYING. If not, write to
|
|
||||||
* the Free Software Foundation, 59 Temple Place - Suite 330,
|
|
||||||
* Boston, MA 02111-1307, USA.
|
|
||||||
*/
|
|
||||||
|
|
||||||
/**
|
|
||||||
* @file subsystems/imu/imu_ardrone2_sdk.c
|
|
||||||
* dummy IMU implementation for ardrone2-sdk.
|
|
||||||
*/
|
|
||||||
|
|
||||||
#include "subsystems/imu.h"
|
|
||||||
#include "imu_ardrone2_sdk.h"
|
|
||||||
|
|
||||||
|
|
||||||
void imu_impl_init(void)
|
|
||||||
{
|
|
||||||
|
|
||||||
}
|
|
||||||
|
|
||||||
void imu_periodic(void)
|
|
||||||
{
|
|
||||||
|
|
||||||
}
|
|
||||||
@@ -1,34 +0,0 @@
|
|||||||
/*
|
|
||||||
* Copyright (C) 2013 Dino Hensen
|
|
||||||
*
|
|
||||||
* This file is part of Paparazzi.
|
|
||||||
*
|
|
||||||
* Paparazzi is free software; you can redistribute it and/or modify
|
|
||||||
* it under the terms of the GNU General Public License as published by
|
|
||||||
* the Free Software Foundation; either version 2, or (at your option)
|
|
||||||
* any later version.
|
|
||||||
*
|
|
||||||
* Paparazzi is distributed in the hope that it will be useful,
|
|
||||||
* but WITHOUT ANY WARRANTY; without even the implied warranty of
|
|
||||||
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
|
|
||||||
* GNU General Public License for more details.
|
|
||||||
*
|
|
||||||
* You should have received a copy of the GNU General Public License
|
|
||||||
* along with Paparazzi; see the file COPYING. If not, write to
|
|
||||||
* the Free Software Foundation, 59 Temple Place - Suite 330,
|
|
||||||
* Boston, MA 02111-1307, USA.
|
|
||||||
*/
|
|
||||||
|
|
||||||
/**
|
|
||||||
* @file subsystems/imu/imu_ardrone2_sdk.h
|
|
||||||
* dummy IMU implementation for ardrone2-sdk.
|
|
||||||
*/
|
|
||||||
|
|
||||||
#ifndef IMU_ARDRONE2_SDK_H_
|
|
||||||
#define IMU_ARDRONE2_SDK_H_
|
|
||||||
|
|
||||||
|
|
||||||
#define ImuEvent() {}
|
|
||||||
|
|
||||||
|
|
||||||
#endif /* IMU_ARDRONE2_SDK_H_ */
|
|
||||||
@@ -1,176 +0,0 @@
|
|||||||
/*
|
|
||||||
* Copyright (C) 2012-2013 Freek van Tienen
|
|
||||||
*
|
|
||||||
* This file is part of Paparazzi.
|
|
||||||
*
|
|
||||||
* Paparazzi is free software; you can redistribute it and/or modify
|
|
||||||
* it under the terms of the GNU General Public License as published by
|
|
||||||
* the Free Software Foundation; either version 2, or (at your option)
|
|
||||||
* any later version.
|
|
||||||
*
|
|
||||||
* Paparazzi is distributed in the hope that it will be useful,
|
|
||||||
* but WITHOUT ANY WARRANTY; without even the implied warranty of
|
|
||||||
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
|
|
||||||
* GNU General Public License for more details.
|
|
||||||
*
|
|
||||||
* You should have received a copy of the GNU General Public License
|
|
||||||
* along with Paparazzi; see the file COPYING. If not, write to
|
|
||||||
* the Free Software Foundation, 59 Temple Place - Suite 330,
|
|
||||||
* Boston, MA 02111-1307, USA.
|
|
||||||
*/
|
|
||||||
|
|
||||||
/**
|
|
||||||
* @file subsystems/ins/ins_ardrone2.c
|
|
||||||
* INS implementation for ardrone2-sdk.
|
|
||||||
*/
|
|
||||||
|
|
||||||
#include "subsystems/ins/ins_ardrone2.h"
|
|
||||||
#include "subsystems/ahrs.h"
|
|
||||||
#include "subsystems/gps.h"
|
|
||||||
#include "generated/airframe.h"
|
|
||||||
#include "generated/flight_plan.h"
|
|
||||||
#include "math/pprz_geodetic_int.h"
|
|
||||||
|
|
||||||
#ifdef SITL
|
|
||||||
#include "nps_fdm.h"
|
|
||||||
#include <stdio.h>
|
|
||||||
#endif
|
|
||||||
|
|
||||||
#ifndef USE_INS_NAV_INIT
|
|
||||||
#define USE_INS_NAV_INIT TRUE
|
|
||||||
PRINT_CONFIG_MSG("USE_INS_NAV_INIT defaulting to TRUE")
|
|
||||||
#endif
|
|
||||||
|
|
||||||
struct InsArdrone2 ins_ardrone2;
|
|
||||||
|
|
||||||
void ins_ardrone2_init(void)
|
|
||||||
{
|
|
||||||
#if USE_INS_NAV_INIT
|
|
||||||
struct LlaCoor_i llh_nav0; /* Height above the ellipsoid */
|
|
||||||
llh_nav0.lat = NAV_LAT0;
|
|
||||||
llh_nav0.lon = NAV_LON0;
|
|
||||||
/* NAV_ALT0 = ground alt above msl, NAV_MSL0 = geoid-height (msl) over ellipsoid */
|
|
||||||
llh_nav0.alt = NAV_ALT0 + NAV_MSL0;
|
|
||||||
|
|
||||||
struct EcefCoor_i ecef_nav0;
|
|
||||||
ecef_of_lla_i(&ecef_nav0, &llh_nav0);
|
|
||||||
|
|
||||||
ltp_def_from_ecef_i(&ins_ardrone2.ltp_def, &ecef_nav0);
|
|
||||||
ins_ardrone2.ltp_def.hmsl = NAV_ALT0;
|
|
||||||
stateSetLocalOrigin_i(&ins_ardrone2.ltp_def);
|
|
||||||
|
|
||||||
ins_ardrone2.ltp_initialized = TRUE;
|
|
||||||
#else
|
|
||||||
ins_ardrone2.ltp_initialized = FALSE;
|
|
||||||
#endif
|
|
||||||
|
|
||||||
INT32_VECT3_ZERO(ins_ardrone2.ltp_pos);
|
|
||||||
INT32_VECT3_ZERO(ins_ardrone2.ltp_speed);
|
|
||||||
INT32_VECT3_ZERO(ins_ardrone2.ltp_accel);
|
|
||||||
}
|
|
||||||
|
|
||||||
void ins_reset_local_origin(void)
|
|
||||||
{
|
|
||||||
#if USE_GPS
|
|
||||||
if (gps.fix == GPS_FIX_3D) {
|
|
||||||
ltp_def_from_ecef_i(&ins_ardrone2.ltp_def, &gps.ecef_pos);
|
|
||||||
ins_ardrone2.ltp_def.lla.alt = gps.lla_pos.alt;
|
|
||||||
ins_ardrone2.ltp_def.hmsl = gps.hmsl;
|
|
||||||
ins_ardrone2.ltp_initialized = TRUE;
|
|
||||||
stateSetLocalOrigin_i(&ins_ardrone2.ltp_def);
|
|
||||||
}
|
|
||||||
else {
|
|
||||||
ins_ardrone2.ltp_initialized = FALSE;
|
|
||||||
}
|
|
||||||
#else
|
|
||||||
ins_ardrone2.ltp_initialized = FALSE;
|
|
||||||
#endif
|
|
||||||
}
|
|
||||||
|
|
||||||
void ins_reset_altitude_ref(void)
|
|
||||||
{
|
|
||||||
#if USE_GPS
|
|
||||||
struct LlaCoor_i lla = {
|
|
||||||
.lat = state.ned_origin_i.lla.lat,
|
|
||||||
.lon = state.ned_origin_i.lla.lon,
|
|
||||||
.alt = gps.lla_pos.alt
|
|
||||||
};
|
|
||||||
ltp_def_from_lla_i(&ins_ardrone2.ltp_def, &lla);
|
|
||||||
ins_ardrone2.ltp_def.hmsl = gps.hmsl;
|
|
||||||
stateSetLocalOrigin_i(&ins_ardrone2.ltp_def);
|
|
||||||
#endif
|
|
||||||
}
|
|
||||||
|
|
||||||
void ins_ardrone2_periodic(void)
|
|
||||||
{
|
|
||||||
/* untilt accels and speeds */
|
|
||||||
float_rmat_transp_vmult((struct FloatVect3 *)&ins_ardrone2.ltp_accel,
|
|
||||||
stateGetNedToBodyRMat_f(),
|
|
||||||
(struct FloatVect3 *)&ahrs_ardrone2.accel);
|
|
||||||
float_rmat_transp_vmult((struct FloatVect3 *)&ins_ardrone2.ltp_speed,
|
|
||||||
stateGetNedToBodyRMat_f(),
|
|
||||||
(struct FloatVect3 *)&ahrs_ardrone2.speed);
|
|
||||||
|
|
||||||
//Add g to the accelerations
|
|
||||||
ins_ardrone2.ltp_accel.z += 9.81;
|
|
||||||
|
|
||||||
//Save the accelerations and speeds
|
|
||||||
stateSetAccelNed_f(&ins_ardrone2.ltp_accel);
|
|
||||||
stateSetSpeedNed_f(&ins_ardrone2.ltp_speed);
|
|
||||||
|
|
||||||
//Don't set the height if we use the one from the gps
|
|
||||||
#if !USE_GPS_HEIGHT
|
|
||||||
//Set the height and save the position
|
|
||||||
ins_ardrone2.ltp_pos.z = -(ahrs_ardrone2.altitude * INT32_POS_OF_CM_NUM) / INT32_POS_OF_CM_DEN;
|
|
||||||
stateSetPositionNed_i(&ins_ardrone2.ltp_pos);
|
|
||||||
#endif
|
|
||||||
}
|
|
||||||
|
|
||||||
|
|
||||||
void ins_ardrone2_update_gps(void)
|
|
||||||
{
|
|
||||||
#if USE_GPS
|
|
||||||
//Check for GPS fix
|
|
||||||
if (gps.fix == GPS_FIX_3D) {
|
|
||||||
//Set the initial coordinates
|
|
||||||
if (!ins_ardrone2.ltp_initialized) {
|
|
||||||
ltp_def_from_ecef_i(&ins_ardrone2.ltp_def, &gps.ecef_pos);
|
|
||||||
ins_ardrone2.ltp_def.lla.alt = gps.lla_pos.alt;
|
|
||||||
ins_ardrone2.ltp_def.hmsl = gps.hmsl;
|
|
||||||
ins_ardrone2.ltp_initialized = TRUE;
|
|
||||||
stateSetLocalOrigin_i(&ins_ardrone2.ltp_def);
|
|
||||||
}
|
|
||||||
|
|
||||||
//Set the x and y and maybe z position in ltp and save
|
|
||||||
struct NedCoor_i ins_gps_pos_cm_ned;
|
|
||||||
ned_of_ecef_point_i(&ins_gps_pos_cm_ned, &ins_ardrone2.ltp_def, &gps.ecef_pos);
|
|
||||||
|
|
||||||
//When we don't want to use the height of the navdata we can use the gps height
|
|
||||||
#if USE_GPS_HEIGHT
|
|
||||||
INT32_VECT3_SCALE_2(ins_ardrone2.ltp_pos, ins_gps_pos_cm_ned, INT32_POS_OF_CM_NUM, INT32_POS_OF_CM_DEN);
|
|
||||||
#else
|
|
||||||
INT32_VECT2_SCALE_2(ins_ardrone2.ltp_pos, ins_gps_pos_cm_ned, INT32_POS_OF_CM_NUM, INT32_POS_OF_CM_DEN);
|
|
||||||
#endif
|
|
||||||
|
|
||||||
//Set the local origin
|
|
||||||
stateSetPositionNed_i(&ins_ardrone2.ltp_pos);
|
|
||||||
}
|
|
||||||
#endif /* USE_GPS */
|
|
||||||
}
|
|
||||||
|
|
||||||
#include "subsystems/abi.h"
|
|
||||||
static abi_event gps_ev;
|
|
||||||
static void gps_cb(uint8_t sender_id __attribute__((unused)),
|
|
||||||
uint32_t stamp __attribute__((unused)),
|
|
||||||
struct GpsState *gps_s)
|
|
||||||
{
|
|
||||||
ins_ardrone2_update_gps();
|
|
||||||
}
|
|
||||||
|
|
||||||
void ins_ardrone2_register(void)
|
|
||||||
{
|
|
||||||
ins_register_impl(ins_ardrone2_init);
|
|
||||||
|
|
||||||
AbiBindMsgGPS(ABI_BROADCAST, &gps_ev, gps_cb);
|
|
||||||
// FIXME: ins_ardrone2_periodic is currently called via InsPeriodic hack directly from main
|
|
||||||
}
|
|
||||||
@@ -1,58 +0,0 @@
|
|||||||
/*
|
|
||||||
* Copyright (C) 2012-2013 Freek van Tienen
|
|
||||||
*
|
|
||||||
* This file is part of Paparazzi.
|
|
||||||
*
|
|
||||||
* Paparazzi is free software; you can redistribute it and/or modify
|
|
||||||
* it under the terms of the GNU General Public License as published by
|
|
||||||
* the Free Software Foundation; either version 2, or (at your option)
|
|
||||||
* any later version.
|
|
||||||
*
|
|
||||||
* Paparazzi is distributed in the hope that it will be useful,
|
|
||||||
* but WITHOUT ANY WARRANTY; without even the implied warranty of
|
|
||||||
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
|
|
||||||
* GNU General Public License for more details.
|
|
||||||
*
|
|
||||||
* You should have received a copy of the GNU General Public License
|
|
||||||
* along with Paparazzi; see the file COPYING. If not, write to
|
|
||||||
* the Free Software Foundation, 59 Temple Place - Suite 330,
|
|
||||||
* Boston, MA 02111-1307, USA.
|
|
||||||
*/
|
|
||||||
|
|
||||||
/**
|
|
||||||
* @file subsystems/ins/ins_ardrone2.h
|
|
||||||
* INS implementation for ardrone2-sdk.
|
|
||||||
*/
|
|
||||||
|
|
||||||
#ifndef INS_ARDRONE2_SDK_H
|
|
||||||
#define INS_ARDRONE2_SDK_H
|
|
||||||
|
|
||||||
#include "subsystems/ins.h"
|
|
||||||
#include "std.h"
|
|
||||||
#include "math/pprz_geodetic_int.h"
|
|
||||||
#include "math/pprz_algebra_float.h"
|
|
||||||
|
|
||||||
struct InsArdrone2 {
|
|
||||||
struct LtpDef_i ltp_def;
|
|
||||||
bool_t ltp_initialized;
|
|
||||||
|
|
||||||
float qfe; ///< not used, only dummy for INS_REF message
|
|
||||||
|
|
||||||
/* output LTP NED */
|
|
||||||
struct NedCoor_i ltp_pos;
|
|
||||||
struct NedCoor_f ltp_speed;
|
|
||||||
struct NedCoor_f ltp_accel;
|
|
||||||
};
|
|
||||||
|
|
||||||
extern struct InsArdrone2 ins_ardrone2;
|
|
||||||
|
|
||||||
#define DefaultInsImpl ins_ardrone2
|
|
||||||
#define InsPeriodic ins_ardrone2_periodic
|
|
||||||
|
|
||||||
extern void ins_ardrone2_init(void);
|
|
||||||
extern void ins_ardrone2_periodic(void);
|
|
||||||
extern void ins_ardrone2_update_gps(void);
|
|
||||||
|
|
||||||
extern void ins_ardrone2_register(void);
|
|
||||||
|
|
||||||
#endif /* INS_ARDRONE2_SDK_H */
|
|
||||||
+6
-6
@@ -185,7 +185,7 @@ def bebop_status():
|
|||||||
read_from_config('motor1_hard', config_ini) + '\t' + read_from_config('motor2_hard', config_ini) + '\t' +
|
read_from_config('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))
|
read_from_config('motor3_hard', config_ini) + '\t' + read_from_config('motor4_hard', config_ini))
|
||||||
|
|
||||||
autorun = {'': 'Native', '0': 'Native', '1': 'Paparazzi RAW', '2': 'Paparazzi SDK'}
|
autorun = {'': 'Native', '0': 'Native', '1': 'Paparazzi'}
|
||||||
if check_autoboot():
|
if check_autoboot():
|
||||||
print('Autorun at start:\tInstalled booting ' + autorun[read_from_config('start_paparazzi', config_ini)])
|
print('Autorun at start:\tInstalled booting ' + autorun[read_from_config('start_paparazzi', config_ini)])
|
||||||
else:
|
else:
|
||||||
@@ -219,7 +219,7 @@ subparser_upload_gst = subparsers.add_parser('upload_gst_module',
|
|||||||
subparser_upload_gst.add_argument('file', help='Filename of *.so module')
|
subparser_upload_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 = 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('file', help='Filename of an executable')
|
||||||
subparser_upload_and_run.add_argument('folder', help='Destination subfolder (raw or sdk for Paparazzi autopilot)')
|
subparser_upload_and_run.add_argument('folder', help='Destination subfolder (raw for Paparazzi autopilot)')
|
||||||
subparser_upload = subparsers.add_parser('upload_file', help='Upload a file to the Bebop')
|
subparser_upload = subparsers.add_parser('upload_file', help='Upload a file to the Bebop')
|
||||||
subparser_upload.add_argument('file', help='Filename')
|
subparser_upload.add_argument('file', help='Filename')
|
||||||
subparser_upload.add_argument('folder', help='Destination subfolder (base destination folder is /data/video)')
|
subparser_upload.add_argument('folder', help='Destination subfolder (base destination folder is /data/video)')
|
||||||
@@ -249,10 +249,10 @@ subparser_configure_network.add_argument('name', help='the new network ID(SSID)'
|
|||||||
subparser_configure_network.add_argument('address', help='the new IP address')
|
subparser_configure_network.add_argument('address', help='the new IP address')
|
||||||
subparser_configure_network.add_argument('mode', help='the new Wifi mode', choices=['master', 'ad-hoc', 'managed'])
|
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 = subparsers.add_parser('install_autostart', help='Install custom autostart script and set what to start on boot for the Bebop')
|
||||||
subparser_install_autostart.add_argument('type', choices=['native', 'paparazzi_raw', 'paparazzi_sdk'],
|
subparser_install_autostart.add_argument('type', choices=['native', 'paparazzi_raw'],
|
||||||
help='what to start on boot')
|
help='what to start on boot')
|
||||||
subparser_autostart = subparsers.add_parser('autostart', help='Set what to start on boot for the Bebop')
|
subparser_autostart = subparsers.add_parser('autostart', help='Set what to start on boot for the Bebop')
|
||||||
subparser_autostart.add_argument('type', choices=['native', 'paparazzi_raw', 'paparazzi_sdk'],
|
subparser_autostart.add_argument('type', choices=['native', 'paparazzi_raw'],
|
||||||
help='what to start on boot')
|
help='what to start on boot')
|
||||||
|
|
||||||
args = parser.parse_args()
|
args = parser.parse_args()
|
||||||
@@ -347,7 +347,7 @@ elif args.command == 'install_autostart':
|
|||||||
bebop_install_autoboot()
|
bebop_install_autoboot()
|
||||||
else:
|
else:
|
||||||
bebop_install_autoboot()
|
bebop_install_autoboot()
|
||||||
autorun = {'native': '0', 'paparazzi_raw': '1', 'paparazzi_sdk': '2'}
|
autorun = {'native': '0', 'paparazzi_raw': '1'}
|
||||||
write_to_config('start_paparazzi', autorun[args.type])
|
write_to_config('start_paparazzi', autorun[args.type])
|
||||||
print('The autostart on boot is changed to ' + args.type)
|
print('The autostart on boot is changed to ' + args.type)
|
||||||
|
|
||||||
@@ -356,7 +356,7 @@ elif args.command == 'install_autostart':
|
|||||||
|
|
||||||
# Change the autostart
|
# Change the autostart
|
||||||
elif args.command == 'autostart':
|
elif args.command == 'autostart':
|
||||||
autorun = {'native': '0', 'paparazzi_raw': '1', 'paparazzi_sdk': '2'}
|
autorun = {'native': '0', 'paparazzi_raw': '1'}
|
||||||
write_to_config('start_paparazzi', autorun[args.type])
|
write_to_config('start_paparazzi', autorun[args.type])
|
||||||
print('The autostart on boot is changed to ' + args.type)
|
print('The autostart on boot is changed to ' + args.type)
|
||||||
|
|
||||||
|
|||||||
@@ -173,7 +173,7 @@ def ardrone2_status():
|
|||||||
read_from_config('motor1_hard', config_ini) + '\t' + read_from_config('motor2_hard', config_ini) + '\t' +
|
read_from_config('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))
|
read_from_config('motor3_hard', config_ini) + '\t' + read_from_config('motor4_hard', config_ini))
|
||||||
|
|
||||||
autorun = {'': 'Native', '0': 'Native', '1': 'Paparazzi RAW', '2': 'Paparazzi SDK'}
|
autorun = {'': 'Native', '0': 'Native', '1': 'Paparazzi'}
|
||||||
if check_autoboot():
|
if check_autoboot():
|
||||||
print('Autorun at start:\tInstalled booting ' + autorun[read_from_config('start_paparazzi', config_ini)])
|
print('Autorun at start:\tInstalled booting ' + autorun[read_from_config('start_paparazzi', config_ini)])
|
||||||
else:
|
else:
|
||||||
@@ -207,7 +207,7 @@ subparser_upload_gst = subparsers.add_parser('upload_gst_module',
|
|||||||
subparser_upload_gst.add_argument('file', help='Filename of *.so module')
|
subparser_upload_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 = 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('file', help='Filename of an executable')
|
||||||
subparser_upload_and_run.add_argument('folder', help='Destination subfolder (raw or sdk for Paparazzi autopilot)')
|
subparser_upload_and_run.add_argument('folder', help='Destination subfolder (raw for Paparazzi autopilot)')
|
||||||
subparser_upload = subparsers.add_parser('upload_file', help='Upload a file to the ARDrone 2')
|
subparser_upload = subparsers.add_parser('upload_file', help='Upload a file to the ARDrone 2')
|
||||||
subparser_upload.add_argument('file', help='Filename')
|
subparser_upload.add_argument('file', help='Filename')
|
||||||
subparser_upload.add_argument('folder', help='Destination subfolder (base destination folder is /data/video)')
|
subparser_upload.add_argument('folder', help='Destination subfolder (base destination folder is /data/video)')
|
||||||
@@ -238,10 +238,10 @@ subparser_configure_network.add_argument('address', help='the new IP address')
|
|||||||
subparser_configure_network.add_argument('mode', help='the new Wifi mode', choices=['master', 'ad-hoc', 'managed', 'ad-hoc-olsr'])
|
subparser_configure_network.add_argument('mode', help='the new Wifi mode', choices=['master', 'ad-hoc', 'managed', 'ad-hoc-olsr'])
|
||||||
subparser_configure_network.add_argument('--channel', help='the wifi channel (auto or 1 to 11)', default='auto')
|
subparser_configure_network.add_argument('--channel', help='the wifi channel (auto or 1 to 11)', default='auto')
|
||||||
subparser_install_autostart = subparsers.add_parser('install_autostart', help='Install custom autostart script and set what to start on boot for the ARDrone 2')
|
subparser_install_autostart = subparsers.add_parser('install_autostart', help='Install custom autostart script and set what to start on boot for the ARDrone 2')
|
||||||
subparser_install_autostart.add_argument('type', choices=['native', 'paparazzi_raw', 'paparazzi_sdk'],
|
subparser_install_autostart.add_argument('type', choices=['native', 'paparazzi_raw'],
|
||||||
help='what to start on boot')
|
help='what to start on boot')
|
||||||
subparser_autostart = subparsers.add_parser('autostart', help='Set what to start on boot for the ARDrone 2')
|
subparser_autostart = subparsers.add_parser('autostart', help='Set what to start on boot for the ARDrone 2')
|
||||||
subparser_autostart.add_argument('type', choices=['native', 'paparazzi_raw', 'paparazzi_sdk'],
|
subparser_autostart.add_argument('type', choices=['native', 'paparazzi_raw'],
|
||||||
help='what to start on boot')
|
help='what to start on boot')
|
||||||
|
|
||||||
args = parser.parse_args()
|
args = parser.parse_args()
|
||||||
@@ -285,7 +285,7 @@ elif args.command == 'ipaddress':
|
|||||||
# Change the wifi mode
|
# Change the wifi mode
|
||||||
elif args.command == 'wifimode':
|
elif args.command == 'wifimode':
|
||||||
ardrone2_set_wifi_mode(args.mode)
|
ardrone2_set_wifi_mode(args.mode)
|
||||||
|
|
||||||
if raw_input("Shall I restart the ARDrone 2? (y/N) ").lower() == 'y':
|
if raw_input("Shall I restart the ARDrone 2? (y/N) ").lower() == 'y':
|
||||||
parrot_utils.reboot(tn)
|
parrot_utils.reboot(tn)
|
||||||
|
|
||||||
@@ -332,7 +332,7 @@ elif args.command == 'install_autostart':
|
|||||||
ardrone2_install_autoboot()
|
ardrone2_install_autoboot()
|
||||||
else:
|
else:
|
||||||
ardrone2_install_autoboot()
|
ardrone2_install_autoboot()
|
||||||
autorun = {'native': '0', 'paparazzi_raw': '1', 'paparazzi_sdk': '2'}
|
autorun = {'native': '0', 'paparazzi_raw': '1'}
|
||||||
write_to_config('start_paparazzi', autorun[args.type])
|
write_to_config('start_paparazzi', autorun[args.type])
|
||||||
print('The autostart on boot is changed to ' + args.type)
|
print('The autostart on boot is changed to ' + args.type)
|
||||||
|
|
||||||
@@ -341,7 +341,7 @@ elif args.command == 'install_autostart':
|
|||||||
|
|
||||||
# Change the autostart
|
# Change the autostart
|
||||||
elif args.command == 'autostart':
|
elif args.command == 'autostart':
|
||||||
autorun = {'native': '0', 'paparazzi_raw': '1', 'paparazzi_sdk': '2'}
|
autorun = {'native': '0', 'paparazzi_raw': '1'}
|
||||||
write_to_config('start_paparazzi', autorun[args.type])
|
write_to_config('start_paparazzi', autorun[args.type])
|
||||||
print('The autostart on boot is changed to ' + args.type)
|
print('The autostart on boot is changed to ' + args.type)
|
||||||
|
|
||||||
|
|||||||
@@ -11,9 +11,6 @@ case $START_PAPARAZZI in
|
|||||||
1)
|
1)
|
||||||
START_PAPARAZZI=raw
|
START_PAPARAZZI=raw
|
||||||
;;
|
;;
|
||||||
2)
|
|
||||||
START_PAPARAZZI=sdk
|
|
||||||
;;
|
|
||||||
*)
|
*)
|
||||||
START_PAPARAZZI=no
|
START_PAPARAZZI=no
|
||||||
;;
|
;;
|
||||||
@@ -28,7 +25,7 @@ PELF_ARGS=$(cat /tmp/.program.elf.arguments | tr '\n' ' ')
|
|||||||
echo "Check if update is necessary ..."
|
echo "Check if update is necessary ..."
|
||||||
if [ -e $UPDATE_PATH ] ; then
|
if [ -e $UPDATE_PATH ] ; then
|
||||||
VERSION=`cat $VERSION_PATH`
|
VERSION=`cat $VERSION_PATH`
|
||||||
|
|
||||||
if [ -e $ERR_PATH ] ; then
|
if [ -e $ERR_PATH ] ; then
|
||||||
CHECK_ERR=`cat $ERR_PATH`
|
CHECK_ERR=`cat $ERR_PATH`
|
||||||
if [ "$CHECK_ERR" = "NEED_TO_FLASH" ] ; then
|
if [ "$CHECK_ERR" = "NEED_TO_FLASH" ] ; then
|
||||||
@@ -36,17 +33,17 @@ if [ -e $UPDATE_PATH ] ; then
|
|||||||
if [ "$CHECK_PLF" = "NEED_TO_FLASH" ] ; then
|
if [ "$CHECK_PLF" = "NEED_TO_FLASH" ] ; then
|
||||||
echo "ERR=FLASH_KO" > $ERR_PATH
|
echo "ERR=FLASH_KO" > $ERR_PATH
|
||||||
else
|
else
|
||||||
/bin/checkplf $UPDATE_PATH $VERSION > $ERR_PATH
|
/bin/checkplf $UPDATE_PATH $VERSION > $ERR_PATH
|
||||||
fi
|
fi
|
||||||
else
|
else
|
||||||
/bin/checkplf $UPDATE_PATH $VERSION > $ERR_PATH
|
/bin/checkplf $UPDATE_PATH $VERSION > $ERR_PATH
|
||||||
fi
|
fi
|
||||||
else
|
else
|
||||||
/bin/checkplf $UPDATE_PATH $VERSION > $ERR_PATH
|
/bin/checkplf $UPDATE_PATH $VERSION > $ERR_PATH
|
||||||
fi
|
fi
|
||||||
|
|
||||||
CHECK_ERR=`cat $ERR_PATH`
|
CHECK_ERR=`cat $ERR_PATH`
|
||||||
if [ "$CHECK_ERR" = "NEED_TO_FLASH" ] ; then
|
if [ "$CHECK_ERR" = "NEED_TO_FLASH" ] ; then
|
||||||
echo "File $UPDATE_PATH exists... Start updating..."
|
echo "File $UPDATE_PATH exists... Start updating..."
|
||||||
pinst_trigger
|
pinst_trigger
|
||||||
echo "Rebooting..."
|
echo "Rebooting..."
|
||||||
@@ -64,12 +61,10 @@ if [ -e $UPDATE_PATH ] ; then
|
|||||||
cp /firmware/version.txt $VERSION_PATH
|
cp /firmware/version.txt $VERSION_PATH
|
||||||
echo "Start Drone software..."
|
echo "Start Drone software..."
|
||||||
inetd
|
inetd
|
||||||
|
|
||||||
# Check what to start
|
# Check what to start
|
||||||
if [ "$START_PAPARAZZI" = "raw" ] ; then
|
if [ "$START_PAPARAZZI" = "raw" ] ; then
|
||||||
(/data/video/raw/ap.elf; gpio 181 -d ho 1) &
|
(/data/video/raw/ap.elf; gpio 181 -d ho 1) &
|
||||||
elif [ "$START_PAPARAZZI" = "sdk" ] ; then
|
|
||||||
(/data/video/sdk/ap.elf; gpio 181 -d ho 1) &
|
|
||||||
else
|
else
|
||||||
(/bin/program.elf ${PELF_ARGS}; gpio 181 -d ho 1) &
|
(/bin/program.elf ${PELF_ARGS}; gpio 181 -d ho 1) &
|
||||||
fi
|
fi
|
||||||
@@ -86,10 +81,6 @@ else
|
|||||||
(/bin/program.elf ${PELF_ARGS}; gpio 181 -d ho 1) &
|
(/bin/program.elf ${PELF_ARGS}; gpio 181 -d ho 1) &
|
||||||
sleep 10
|
sleep 10
|
||||||
(/data/video/raw/ap.elf; gpio 181 -d ho 1) &
|
(/data/video/raw/ap.elf; gpio 181 -d ho 1) &
|
||||||
elif [ "$START_PAPARAZZI" = "sdk" ] ; then
|
|
||||||
(/bin/program.elf ${PELF_ARGS}; gpio 181 -d ho 1) &
|
|
||||||
sleep 10
|
|
||||||
(/data/video/sdk/ap.elf; gpio 181 -d ho 1) &
|
|
||||||
else
|
else
|
||||||
(/bin/program.elf ${PELF_ARGS}; gpio 181 -d ho 1) &
|
(/bin/program.elf ${PELF_ARGS}; gpio 181 -d ho 1) &
|
||||||
fi
|
fi
|
||||||
|
|||||||
@@ -67,7 +67,7 @@ subparsers.add_parser('status', help='Request the status of the Bebop')
|
|||||||
subparsers.add_parser('reboot', help='Reboot 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 = 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('file', help='Filename of an executable')
|
||||||
subparser_upload_and_run.add_argument('folder', help='Destination subfolder (raw or sdk for Paparazzi autopilot)')
|
subparser_upload_and_run.add_argument('folder', help='Destination subfolder (raw for Paparazzi autopilot)')
|
||||||
subparser_upload = subparsers.add_parser('upload_file', help='Upload a file to the Bebop')
|
subparser_upload = subparsers.add_parser('upload_file', help='Upload a file to the Bebop')
|
||||||
subparser_upload.add_argument('file', help='Filename')
|
subparser_upload.add_argument('file', help='Filename')
|
||||||
subparser_upload.add_argument('folder', help='Destination subfolder (base destination folder is /data/ftp)')
|
subparser_upload.add_argument('folder', help='Destination subfolder (base destination folder is /data/ftp)')
|
||||||
|
|||||||
Reference in New Issue
Block a user