mirror of
https://github.com/paparazzi/paparazzi.git
synced 2026-05-09 22:49:53 +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"
|
||||
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
|
||||
name="bebop"
|
||||
ac_id="202"
|
||||
|
||||
@@ -340,17 +340,6 @@
|
||||
settings_modules="modules/gps_ubx_ucenter.xml"
|
||||
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
|
||||
name="bebop"
|
||||
ac_id="202"
|
||||
|
||||
@@ -137,8 +137,6 @@ endif
|
||||
ifeq ($(BOARD), booz)
|
||||
ns_CFLAGS += -DUSE_DAC
|
||||
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)
|
||||
ns_srcs += $(SRC_BOARD)/electrical_raw.c
|
||||
else ifeq ($(BOARD), bebop)
|
||||
|
||||
@@ -1,11 +1,5 @@
|
||||
|
||||
# Actuator drivers for both ardrone versions are conditionally included here
|
||||
# The AT-command and RAW drivers are not interchangeble
|
||||
# Actuator drivers for the raw ardrone version are included here
|
||||
|
||||
ifeq ($(BOARD_TYPE), sdk)
|
||||
$(TARGET).srcs += $(SRC_BOARD)/actuators_at.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
|
||||
$(TARGET).CFLAGS += -DACTUATORS
|
||||
$(TARGET).srcs += $(SRC_BOARD)/actuators_ardrone2_raw.c
|
||||
|
||||
@@ -1,15 +1,9 @@
|
||||
# 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_srcs += $(SRC_SUBSYSTEMS)/imu.c
|
||||
imu_srcs += $(SRC_SUBSYSTEMS)/imu/imu_ardrone2_raw.c
|
||||
imu_srcs += $(SRC_BOARD)/navdata.c
|
||||
endif
|
||||
|
||||
|
||||
# add it for all targets except sim, fbw and nps
|
||||
|
||||
@@ -31,7 +31,7 @@
|
||||
#include <stdint.h>
|
||||
#include BOARD_CONFIG
|
||||
|
||||
#if defined BOARD_ARDRONE2_SDK || defined BOARD_ARDRONE2_RAW
|
||||
#if defined BOARD_ARDRONE2_RAW
|
||||
extern uint32_t led_hw_values;
|
||||
#define LED_INIT(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('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():
|
||||
print('Autorun at start:\tInstalled booting ' + autorun[read_from_config('start_paparazzi', config_ini)])
|
||||
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_and_run = subparsers.add_parser('upload_file_and_run', help='Upload and run software (for instance the Paparazzi autopilot)')
|
||||
subparser_upload_and_run.add_argument('file', help='Filename of an executable')
|
||||
subparser_upload_and_run.add_argument('folder', help='Destination subfolder (raw or sdk for Paparazzi autopilot)')
|
||||
subparser_upload_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/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('mode', help='the new Wifi mode', choices=['master', 'ad-hoc', 'managed'])
|
||||
subparser_install_autostart = subparsers.add_parser('install_autostart', help='Install custom autostart script and set what to start on boot for the Bebop')
|
||||
subparser_install_autostart.add_argument('type', choices=['native', 'paparazzi_raw', 'paparazzi_sdk'],
|
||||
subparser_install_autostart.add_argument('type', choices=['native', 'paparazzi_raw'],
|
||||
help='what to start on boot')
|
||||
subparser_autostart = subparsers.add_parser('autostart', help='Set what to start on boot for the Bebop')
|
||||
subparser_autostart.add_argument('type', choices=['native', 'paparazzi_raw', 'paparazzi_sdk'],
|
||||
subparser_autostart.add_argument('type', choices=['native', 'paparazzi_raw'],
|
||||
help='what to start on boot')
|
||||
|
||||
args = parser.parse_args()
|
||||
@@ -347,7 +347,7 @@ elif args.command == 'install_autostart':
|
||||
bebop_install_autoboot()
|
||||
else:
|
||||
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])
|
||||
print('The autostart on boot is changed to ' + args.type)
|
||||
|
||||
@@ -356,7 +356,7 @@ elif args.command == 'install_autostart':
|
||||
|
||||
# Change the 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])
|
||||
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('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():
|
||||
print('Autorun at start:\tInstalled booting ' + autorun[read_from_config('start_paparazzi', config_ini)])
|
||||
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_and_run = subparsers.add_parser('upload_file_and_run', help='Upload and run software (for instance the Paparazzi autopilot)')
|
||||
subparser_upload_and_run.add_argument('file', help='Filename of an executable')
|
||||
subparser_upload_and_run.add_argument('folder', help='Destination subfolder (raw or sdk for Paparazzi autopilot)')
|
||||
subparser_upload_and_run.add_argument('folder', help='Destination subfolder (raw for Paparazzi autopilot)')
|
||||
subparser_upload = subparsers.add_parser('upload_file', help='Upload a file to the ARDrone 2')
|
||||
subparser_upload.add_argument('file', help='Filename')
|
||||
subparser_upload.add_argument('folder', help='Destination subfolder (base destination folder is /data/video)')
|
||||
@@ -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('--channel', help='the wifi channel (auto or 1 to 11)', default='auto')
|
||||
subparser_install_autostart = subparsers.add_parser('install_autostart', help='Install custom autostart script and set what to start on boot for the ARDrone 2')
|
||||
subparser_install_autostart.add_argument('type', choices=['native', 'paparazzi_raw', 'paparazzi_sdk'],
|
||||
subparser_install_autostart.add_argument('type', choices=['native', 'paparazzi_raw'],
|
||||
help='what to start on boot')
|
||||
subparser_autostart = subparsers.add_parser('autostart', help='Set what to start on boot for the ARDrone 2')
|
||||
subparser_autostart.add_argument('type', choices=['native', 'paparazzi_raw', 'paparazzi_sdk'],
|
||||
subparser_autostart.add_argument('type', choices=['native', 'paparazzi_raw'],
|
||||
help='what to start on boot')
|
||||
|
||||
args = parser.parse_args()
|
||||
@@ -285,7 +285,7 @@ elif args.command == 'ipaddress':
|
||||
# Change the wifi mode
|
||||
elif args.command == 'wifimode':
|
||||
ardrone2_set_wifi_mode(args.mode)
|
||||
|
||||
|
||||
if raw_input("Shall I restart the ARDrone 2? (y/N) ").lower() == 'y':
|
||||
parrot_utils.reboot(tn)
|
||||
|
||||
@@ -332,7 +332,7 @@ elif args.command == 'install_autostart':
|
||||
ardrone2_install_autoboot()
|
||||
else:
|
||||
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])
|
||||
print('The autostart on boot is changed to ' + args.type)
|
||||
|
||||
@@ -341,7 +341,7 @@ elif args.command == 'install_autostart':
|
||||
|
||||
# Change the 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])
|
||||
print('The autostart on boot is changed to ' + args.type)
|
||||
|
||||
|
||||
@@ -11,9 +11,6 @@ case $START_PAPARAZZI in
|
||||
1)
|
||||
START_PAPARAZZI=raw
|
||||
;;
|
||||
2)
|
||||
START_PAPARAZZI=sdk
|
||||
;;
|
||||
*)
|
||||
START_PAPARAZZI=no
|
||||
;;
|
||||
@@ -28,7 +25,7 @@ PELF_ARGS=$(cat /tmp/.program.elf.arguments | tr '\n' ' ')
|
||||
echo "Check if update is necessary ..."
|
||||
if [ -e $UPDATE_PATH ] ; then
|
||||
VERSION=`cat $VERSION_PATH`
|
||||
|
||||
|
||||
if [ -e $ERR_PATH ] ; then
|
||||
CHECK_ERR=`cat $ERR_PATH`
|
||||
if [ "$CHECK_ERR" = "NEED_TO_FLASH" ] ; then
|
||||
@@ -36,17 +33,17 @@ if [ -e $UPDATE_PATH ] ; then
|
||||
if [ "$CHECK_PLF" = "NEED_TO_FLASH" ] ; then
|
||||
echo "ERR=FLASH_KO" > $ERR_PATH
|
||||
else
|
||||
/bin/checkplf $UPDATE_PATH $VERSION > $ERR_PATH
|
||||
/bin/checkplf $UPDATE_PATH $VERSION > $ERR_PATH
|
||||
fi
|
||||
else
|
||||
/bin/checkplf $UPDATE_PATH $VERSION > $ERR_PATH
|
||||
/bin/checkplf $UPDATE_PATH $VERSION > $ERR_PATH
|
||||
fi
|
||||
else
|
||||
/bin/checkplf $UPDATE_PATH $VERSION > $ERR_PATH
|
||||
fi
|
||||
|
||||
|
||||
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..."
|
||||
pinst_trigger
|
||||
echo "Rebooting..."
|
||||
@@ -64,12 +61,10 @@ if [ -e $UPDATE_PATH ] ; then
|
||||
cp /firmware/version.txt $VERSION_PATH
|
||||
echo "Start Drone software..."
|
||||
inetd
|
||||
|
||||
|
||||
# Check what to start
|
||||
if [ "$START_PAPARAZZI" = "raw" ] ; then
|
||||
(/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
|
||||
(/bin/program.elf ${PELF_ARGS}; gpio 181 -d ho 1) &
|
||||
fi
|
||||
@@ -86,10 +81,6 @@ else
|
||||
(/bin/program.elf ${PELF_ARGS}; gpio 181 -d ho 1) &
|
||||
sleep 10
|
||||
(/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
|
||||
(/bin/program.elf ${PELF_ARGS}; gpio 181 -d ho 1) &
|
||||
fi
|
||||
|
||||
@@ -67,7 +67,7 @@ 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 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.add_argument('file', help='Filename')
|
||||
subparser_upload.add_argument('folder', help='Destination subfolder (base destination folder is /data/ftp)')
|
||||
|
||||
Reference in New Issue
Block a user