wellton/ardrone_autonomy
1
0
Fork 0
Código Issues Pull Requests Ações Pacotes Projetos Versões Wiki Atividade

Comparar commits

merge em: wellton/ardrone_autonomy:master
Branches Tags
wellton/ardrone_autonomy:master wellton/ardrone_autonomy:autolab wellton/ardrone_autonomy:catkin wellton/ardrone_autonomy:dev-unstable wellton/ardrone_autonomy:fuerte wellton/ardrone_autonomy:gh-pages wellton/ardrone_autonomy:SDK2
wellton/ardrone_autonomy:SDK1.8
...
pull de: wellton/ardrone_autonomy:catkin
Branches Tags
wellton/ardrone_autonomy:autolab wellton/ardrone_autonomy:catkin wellton/ardrone_autonomy:dev-unstable wellton/ardrone_autonomy:fuerte wellton/ardrone_autonomy:master wellton/ardrone_autonomy:gh-pages wellton/ardrone_autonomy:SDK2
wellton/ardrone_autonomy:SDK1.8

18 Commits
master ... catkin

Autor SHA1 Mensagem Data
Mani Monajjemi 307c961160 final minor edits 2014-01-17 18:31:20 -08:00
Mani Monajjemi 4460c90674 Minor readme edits 2014-01-17 18:28:30 -08:00
Mani Monajjemi 91d567d766 Minor readme edit 2014-01-17 18:26:39 -08:00
Mani Monajjemi 96dc711ccd README modified for catkin build and ARDrone's external build. 2014-01-17 18:25:06 -08:00
Mani Monajjemi 0e5aaa6d90 Vanilla ARDroneSDK/ARDroneLib 2.0.1 is included in the driver as an archive. 2014-01-17 18:06:52 -08:00
Kenneth Bogert aa49db1ebc Fix the ardrone_lib.patch to apply against the vanilla ARDroneSDK code provided by parrot 2014-01-09 23:57:25 -05:00
Kenneth Bogert 89856b32e0 Use a git tag to only pull version 2.0.1 of the ARDroneSDK 2014-01-09 22:07:06 -05:00
Kenneth Bogert 50f8a12579 Install ardrone libs into a subfolder to avoid possible file name clashes 2014-01-09 13:45:13 -05:00
Kenneth Bogert f0f72e887e Update installation instructions 2014-01-09 13:36:06 -05:00
Kenneth Bogert 7ef6e9c47c Correct errors in CMakelist and the ardrone_lib.patch's Makefile 2014-01-09 13:32:21 -05:00
Kenneth Bogert b263bdd063 Use cmake's externalproject to download Parrot's ardrone during catkin make 2014-01-09 13:17:44 -05:00
Kenneth Bogert 87b41cf33d Add patch to be applied to ARDroneLib external project (pulled in by cmake) 2014-01-09 12:53:41 -05:00
Kenneth Bogert 4c0b9bb15a Remove Parrot's Ardrone library files 2014-01-09 12:53:18 -05:00
Mani Monajjemi e9a07f9327 Fixed bug in CMakeLists.txt to handle message dependencies. 2013-11-22 13:36:00 -08:00
Mani Monajjemi dfd2d75023 Merge branch 'master' of https://github.com/boris-il-forte/ardrone_autonomy into catkin 2013-11-22 12:40:42 -08:00
Boris_il_forte 7fdb831d7d Merge branch 'dev-unstable' 2013-11-15 15:57:42 +01:00
Boris_il_forte 46df1e5655 Added install rules 
- added rules to install ardrone_driver binary in the correct location specified by --install-space of catkin_make command
 2013-10-04 01:51:11 +02:00
Boris_il_forte 25d3049e83 Migration to Catkin build system 
- deleted manifest.xml
- updated CMakeLists.txt
- created package.xml

This patch shuold be tested. It's also neeed to change the e-mail in package.xml, it was choosen a random e-mail to make the package compile with catkin
 2013-08-14 16:39:39 +02:00
490 arquivos alterados com 220 adições e 77871 exclusões
Expandir todos os arquivos Colapsar todos os arquivos
ARDroneLib/FFMPEG/Makefile
-66
Ver Arquivo
@@ -1,66 +0,0 @@
SILENT="YES"
ifeq ($(SILENT), "YES")
SILENT_MAKE=@
REDIRECT_STDOUT=> /dev/null
REDIRECT_STDERR=2> /dev/null
endif
RELEASE_BUILD=yes
ifeq ($(RELEASE_BUILD),yes)
RELEASE_OPT="release"
else
RELEASE_OPT="debug"
endif
FFMPEG_CONFIG=both
FFMPEG_DIR_EXIST=$(shell bash -c "if [ -d ffmpeg ]; then echo \"YES\"; else echo \"NO\"; fi")
FFMPEG_ARCHIVE=$(shell bash -c "if [ -e ffmpeg-0.8.tar.bz2 ]; then echo \"ffmpeg-0.8.tar.bz2\"; else echo \"no_ffmpeg_archive_found\"; fi")
FFMPEG_ARCHIVE_ROOTDIR=$(shell tar tjf $(FFMPEG_ARCHIVE) $(REDIRECT_STDERR) | head -n 1)
ifeq "$(MAKECMDGOALS)" ""
MAKECMDGOALS=host
endif
all: extract build
$(SILENT_MAKE)echo "Build done."
extract:
ifneq "$(MAKECMDGOALS)" "clean"
ifeq ($(FFMPEG_DIR_EXIST),YES)
$(SILENT_MAKE)echo "Libs already extracted"
else
$(SILENT_MAKE)echo -n "Extracting libs ... "
$(SILENT_MAKE)tar xjf $(FFMPEG_ARCHIVE)
ifneq ($(FFMPEG_ARCHIVE_ROOTDIR), ffmpeg/)
$(SILENT_MAKE)mkdir ffmpeg $(REDIRECT_STDERR)
$(SILENT_MAKE)mv $(FFMPEG_ARCHIVE_ROOTDIR)* ffmpeg/
$(SILENT_MAKE)rm -rf $(FFMPEG_ARCHIVE_ROOTDIR)
endif
$(SILENT_MAKE)tar xzf parrot_ffmpeg_build_utils.tar.gz
$(SILENT_MAKE)echo "Done"
endif
endif
build:
ifneq "$(MAKECMDGOALS)" "clean"
$(SILENT_MAKE)echo "Building target $(MAKECMDGOALS)"
$(SILENT_MAKE)cd ffmpeg && ./autoConf.bash $(MAKECMDGOALS) $(RELEASE_OPT) $(FFMPEG_CONFIG) && cd - $(REDIRECT_STDOUT)
endif
clean:
ifeq ($(FFMPEG_DIR_EXIST),YES)
$(SILENT_MAKE)cd ffmpeg && ./autoConf.bash clean release $(REDIRECT_STDERR) ; cd - $(REDIRECT_STDOUT)
endif
$(SILENT_MAKE)rm -rf ffmpeg/
$(SILENT_MAKE)rm -rf Includes/
ifneq "$(MAKECMDGOALS)" "clean"
ifneq "$(MAKECMDGOALS)" "extract"
ifneq "$(MAKECMDGOALS)" "build"
$(MAKECMDGOALS): all
endif
endif
endif
ARDroneLib/FFMPEG/parrot_ffmpeg_build_utils.tar.gz
BIN
Ver Arquivo
Arquivo binário não exibido.
ARDroneLib/Soft/Build/Makefile
-111
Ver Arquivo
@@ -1,111 +0,0 @@
GEN_CUSTOM_HEADER:=../Common/generated_custom.h
include custom.makefile
include config.makefile
GNUTOOLS_PATH=/usr/local/$(GNUTOOLS_VERSION)/bin
define ADD_RULE_TEMPLATE
TO_BUILD+=build_$(1)
endef
# Add rule for each target
$(foreach target,$(TARGETS),$(eval $(call ADD_RULE_TEMPLATE,$(target))))
.PHONY: linux_sample svn_update $(TO_BUILD) build_libs $(MAKECMDGOALS)
all: $(GEN_CUSTOM_HEADER) build_libs $(TO_BUILD)
$(GEN_CUSTOM_HEADER): custom.makefile
@echo "#ifndef _GENERATED_CUSTOM_CONFIGURATION_H_" > $@
@echo "#define _GENERATED_CUSTOM_CONFIGURATION_H_" >> $@
@echo >> $@
@echo "#if defined(BR2_PACKAGE_BCM4318_AP)" >> $@
@echo "# define AP" >> $@
@echo "#else" >> $@
@echo "# define STA" >> $@
@echo "#endif" >> $@
@echo "#define CURRENT_NUM_VERSION_SOFT \"$(MAJOR_VERSION).$(MINOR_VERSION).$(MODIF_VERSION)\"" >> $@
@echo "#define CURRENT_BUILD_DATE \"$(shell date +%F\ %H:%M)\"" >> $@
@echo >> $@
ifeq ("$(VIDEO_YUV)","yes")
@echo "#define USE_VIDEO_YUV" >> $@
endif
ifeq ("$(RECORD_VISION_DATA)","yes")
@echo "#define RECORD_VISION_DATA" >> $@
endif
@echo >> $@
@echo "#define WIFI_NETWORK_NAME \"$(WIFI_NETWORK_NAME)\"" >> $@
@echo "#define WIFI_BROADCAST \"$(WIFI_BROADCAST)\"" >> $@
@echo "#define WIFI_ARDRONE_IP \"$(WIFI_ARDRONE_IP)\"" >> $@
@echo >> $@
@echo "#if defined(__linux__) || defined(USE_MINGW32)" >> $@
@echo "# define WIFI_MOBILE_IP \"$(WIFI_MOBILE_IP)\"" >> $@
@echo "# define WIRED_ITFNAME \"$(WIRED_ITFNAME)\"" >> $@
@echo "#endif // ! __linux__" >> $@
@echo >> $@
@echo >> $@
@echo "#endif // ! _GENERATED_CUSTOM_CONFIGURATION_H_" >> $@
ifneq "$(MAKECMDGOALS)" ""
ifneq "$(MAKECMDGOALS)" "clean"
ifneq "$(MAKECMDGOALS)" "update"
$(MAKECMDGOALS):
@echo -e "\nCannot make what you ask me to do :-("
else
$(MAKECMDGOALS): svn_update
endif
endif
endif
$(MAKECMDGOALS): build_libs $(TO_BUILD)
checkpackages:
ifeq ($(IPHONE_MODE),yes)
sh $(shell pwd)/check_dependencies.sh iphone RELEASE_BUILD=$(RELEASE_BUILD) $(MAKECMDGOALS)
else
ifeq ($(USE_LINUX),yes)
sh $(shell pwd)/check_dependencies.sh static RELEASE_BUILD=$(RELEASE_BUILD) $(MAKECMDGOALS)
else
ifeq ($(USE_ANDROID),yes)
sh $(shell pwd)/check_dependencies.sh android_no_neon RELEASE_BUILD=$(RELEASE_BUILD) $(MAKECMDGOALS)
endif
endif
endif
define GENERIC_RULES_TEMPLATE
build_$(1):
@$(MAKE) -C $(1) $(MAKECMDGOALS)
endef
$(foreach target,$(TARGETS),$(eval $(call GENERIC_RULES_TEMPLATE,$(target))))
build_libs: checkpackages
@$(MAKE) PC_TARGET=yes USE_ARDRONE_TOOL=yes TARGET=pc_ USE_MINGW32=no -C ../Lib/Build $(MAKECMDGOALS)
@$(MAKE) PC_TARGET=yes USE_ARDRONE_TOOL=no TARGET=pc_ USE_MINGW32=no -C ../Lib/Build $(MAKECMDGOALS)
ifeq ("$(MINGW32_MODE)","yes")
ifeq ($(shell which i586-mingw32msvc-gcc 2> /dev/null),)
$(warning You need MinGW32 to compile My Ardrone lib for Windows if you want. (under Debian: apt-get install mingw32))
else
# @$(MAKE) PC_TARGET=yes TARGET=mingw32_ USE_MINGW32=yes TMP_SDK_FLAGS="USE_MINGW32=yes NO_COM=yes USE_BLUEZ=no" -C ../Lib/Build $(MAKECMDGOALS)
# @$(MAKE) PC_TARGET=yes TARGET=emb_mingw32_ USE_MINGW32=yes CONTROL_DLL=yes TMP_SDK_FLAGS="USE_MINGW32=yes NO_COM=yes USE_BLUEZ=no" -C ../Lib/Build $(MAKECMDGOALS)
endif
endif
ifeq ($(WIIMOTE_SUPPORT),yes)
# @$(MAKE) PC_TARGET=yes TARGET=pc_ TMP_SDK_FLAGS="USE_BLUEZ=yes" -C ../Lib/libcwiid $(MAKECMDGOALS)
endif
define svn_update_template
cd ../.. ; \
echo "Checking out tag $(1) of $(2) ..." ; \
if [ $(1) != head ] ; then \
svn co -r $(1) https://svn.ardrone.org/repo/ARDrone_API/$(2) ; \
else \
svn co https://svn.ardrone.org/repo/ARDrone_API/$(2) ; \
fi ; \
cd Soft/Build ;
endef
svn_update:
@-$(call svn_update_template,$(SDK_VERSION),ARDroneLib)
ARDroneLib/Soft/Build/check_dependencies.sh
-91
Ver Arquivo
@@ -1,91 +0,0 @@
#!/bin/sh
## Author : stephane.piskorski@parrot.com
## Date : 19th,Oct. 2010
# Parameters :
# $1 : target : can be 'iphone (cross compile static libs for iPhone)', 'static (static library for the host)', 'host (shared library for the host)'
# $2 : RELEASE_BUILD=xxx
# $3 : 'clean' if we want to clean, or nothing
check()
{
if [ `cat $TEMPFILE | grep $1 | wc -l` -eq 0 ] ; then
echo " $1";
fi
}
verify()
{
p="$(check $1)";
if [ "$p" != "" ]; then
packages="$packages $p";
if [ "$2" != "" ];
then messages="$messages\n $p : $2";
else messages="$messages\n $p";
fi;
fi;
}
TEMPFILE=`mktemp`
if [ "$ALL_TARGETS" != "" ] ; then
TARGET_DIR=$ALL_TARGETS
else
TARGET_DIR=./targets_versions
fi
SDK_PATH=`echo $0 | sed "s:/Soft/Build/check_dependencies.sh::"`
if [ "$1" != "iphone" ] ; then
if [ "$3" = "cleanAll" ] ; then
make -C $SDK_PATH/FFMPEG clean
exit 0
elif [ "$3" = "clean" ] ; then
# do nothing here
echo "" > /dev/null
else
make -C $SDK_PATH/FFMPEG $1 $2
fi
fi
packages="";
messages="";
if [ `which apt-get` ] ; then
if [ "`apt-get -v | grep -o ubuntu`" != "" ] ; then
echo "\033[31mChecking required Ubuntu packages ...\033[0m";
dpkg -l > $TEMPFILE;
# To compile the AR.Drone project
verify "daemontools" "Mandatory to build the AR.Drone project on Ubuntu";
# To use the Wiimote in Navigation
#verify "libcwiid1-dev";
#verify "libbluetooth-dev";
# To compile Navigation
verify "libsdl1.2-dev";
verify "libgtk2.0-dev";
verify "libxml2-dev";
verify "libudev-dev";
verify "libiw-dev";
if [ "$packages" != "" ] ; then
echo "You should install the following packages to compile the AR.Drone SDK with Ubuntu:\n $messages \n";
echo "Do you want to install them now [y/n] ?";
read user_answer ;
if [ "$user_answer" = "y" ]; then
sudo apt-get install $packages;
fi
else
echo "ok.";
fi
fi
fi
ARDroneLib/Soft/Build/config.makefile
-184
Ver Arquivo
@@ -1,184 +0,0 @@
#########################################################
# Common build definitions (CUSTOM)
#########################################################
RELEASE_BUILD = yes
QUIET_BUILD = yes
VPSDK_PARALLEL_BUILD = no
#########################################################
# System utility definitions (STATIC)
#########################################################
define CHECK_UNDEFINITION
ifdef $(1)
$$(warning ERROR : $(1) defined $(2))
ERROR=1
endif
endef
define EXIT_IF_ERROR
ifeq "$$(ERROR)" "1"
$$(error There has been some errors)
endif
endef
#########################################################
# Validity control (STATIC)
#########################################################
ifdef PC_TARGET
$(eval $(call CHECK_UNDEFINITION,CONSOLE_TARGET,(should not be defined when PC_TARGET is defined)))
endif
$(eval $(call EXIT_IF_ERROR))
#########################################################
# Common definitions (STATIC)
#########################################################
ifeq "$(QUIET_BUILD)" "yes"
export MAKEFLAGS+=-s --no-print-directory
endif
COMMON_DIR:=../Common
SDK_FLAGS:="NO_EXAMPLES=yes"
SDK_FLAGS+="USE_SDK=yes"
SDK_FLAGS+="QUIET_BUILD=$(QUIET_BUILD)"
SDK_FLAGS+="RELEASE_BUILD=$(RELEASE_BUILD)"
SDK_FLAGS+="SDK_VERSION=$(SDK_VERSION)"
ifeq ($(filter NO_COM=%,$(TMP_SDK_FLAGS)),)
SDK_FLAGS+="NO_COM=no"
endif
#########################################################
# PC_TARGET specific definitions (STATIC)
#########################################################
ifdef PC_TARGET
SDK_FLAGS+="NO_COM=no"
ifeq ("$(IPHONE_MODE)","yes")
OS_DEFINE=GNU_LINUX
else
ifeq ("$(USE_LINUX)","yes")
OS_DEFINE=GNU_LINUX
else
TARGET:=$(TARGET).exe
OS_DEFINE=WINDOW
endif
endif
GENERIC_CFLAGS+=-D_MOBILE
ifeq ($(RECORD_RAW_VIDEO),yes)
GENERIC_CFLAGS+=-DRECORD_RAW_VIDEO
endif
ifeq ($(RECORD_FFMPEG_VIDEO),yes)
GENERIC_CFLAGS+=-DRECORD_FFMPEG_VIDEO
endif
ifeq ($(RECORD_ENCODED_VIDEO),yes)
GENERIC_CFLAGS+=-DRECORD_ENCODED_VIDEO
endif
GENERIC_CFLAGS+=-D$(OS_DEFINE)
ifeq ("$(IPHONE_MODE)","yes")
ifeq ($(PLATFORM_NAME),iphoneos)
GENERIC_CFLAGS+=-DTARGET_OS_IPHONE
else
GENERIC_CFLAGS+=-DTARGET_IPHONE_SIMULATOR
endif
endif
ifneq ("$(USE_MINGW32)","yes")
GENERIC_CFLAGS+=$(shell pkg-config --cflags gtk+-2.0)
GENERIC_LIBS+=$(shell pkg-config --libs gtk+-2.0)
endif
ifeq ("$(USE_LINUX)","yes")
SDK_FLAGS+="USE_LINUX=yes"
else
SDK_FLAGS+="USE_LINUX=no"
endif
SDK_FLAGS+="USE_ELINUX=no"
ifeq ("$(IPHONE_MODE)","yes")
SDK_FLAGS+="USE_IPHONE=yes"
SDK_FLAGS+="FFMPEG_SUPPORT=yes"
SDK_FLAGS+="ITTIAM_SUPPORT=no"
SDK_FLAGS+="USE_VIDEO_TCP=yes"
SDK_FLAGS+="USE_VIDEO_HD=no"
else
SDK_FLAGS+="USE_IPHONE=no"
endif
ifeq ("$(USE_NDS)","yes")
SDK_FLAGS+="USE_NDS=yes"
SDK_FLAGS+="NDS_CPU=ARM7"
else
SDK_FLAGS+="USE_NDS=no"
endif
ifeq ("$(USE_ANDROID)","yes")
SDK_FLAGS+="USE_ANDROID=yes"
SDK_FLAGS+="TOOLCHAIN_VERSION=arm-linux-androideabi-4.6"
SDK_FLAGS+="NDK_PLATFORM_VERSION=android-8"
SDK_FLAGS+="FFMPEG_SUPPORT=yes"
SDK_FLAGS+="ITTIAM_SUPPORT=no"
SDK_FLAGS+="USE_VIDEO_TCP=yes"
SDK_FLAGS+="USE_VIDEO_HD=no"
else
SDK_FLAGS+="USE_ANDROID=no"
endif
ifeq ("$(USE_LINUX)","yes")
ifeq ("$(PROJECT)","ardrone")
SDK_FLAGS+="FFMPEG_SUPPORT=yes"
SDK_FLAGS+="ITTIAM_SUPPORT=no"
SDK_FLAGS+="USE_VIDEO_TCP=no"
SDK_FLAGS+="USE_VIDEO_HD=no"
else
SDK_FLAGS+="FFMPEG_SUPPORT=yes"
SDK_FLAGS+="ITTIAM_SUPPORT=no"
SDK_FLAGS+="USE_VIDEO_TCP=yes"
SDK_FLAGS+="USE_VIDEO_HD=no"
endif
endif
ifeq ($(filter USE_BLUEZ=%,$(TMP_SDK_FLAGS)),)
SDK_FLAGS+="USE_BLUEZ=no"
endif
SDK_FLAGS+="USE_VLIB=yes"
SDK_FLAGS+="USE_BONJOUR=no"
SDK_FLAGS+="USE_WIFI=yes"
SDK_FLAGS+="USE_BROADCOM=no"
SDK_FLAGS+="USE_IWLIB=no"
SDK_FLAGS+="FF_ARCH=Intel"
SDK_FLAGS+="USE_PARROTOS_CORE=no"
SDK_FLAGS+="USE_PARROTOS_DRIVERS=no"
SDK_FLAGS+="USE_PARROTOS_DEVS=no"
SDK_FLAGS+="USE_PARROTOS_CODEC=no"
SDK_FLAGS+="USE_ARDRONELIB=yes"
SDK_FLAGS+="USE_ARDRONE_VISION=yes"
SDK_FLAGS+="USE_ARDRONE_POLARIS=no"
SDK_FLAGS+="USE_ARDRONE_VICON=no"
SDK_FLAGS+="USE_ARDRONE_TEST_BENCHS=no"
SDK_FLAGS+="USE_ARDRONE_CALIBRATION=no"
endif
export SDK_FLAGS
export GENERIC_CFLAGS
export VPSDK_PARALLEL_BUILD
ARDroneLib/Soft/Build/custom.makefile
-60
Ver Arquivo
@@ -1,60 +0,0 @@
#########################################################
# Common definitions (CUSTOM)
#########################################################
ifndef IPHONE_MODE
IPHONE_MODE = no
endif
ifndef MINGW32_MODE
MINGW32_MODE = no
endif
ifndef USE_NDS
USE_NDS = no
endif
ifndef USE_ANDROID
USE_ANDROID = no
endif
ifndef USE_LINUX
USE_LINUX = no
endif
ifndef PROJECT
# set default to ardrone2 for video TCP com.
PROJECT = ardrone2
endif
MAJOR_VERSION = 0
MINOR_VERSION = 0
MODIF_VERSION = 0
#########################################################
# ARDroneTool options definitions
#########################################################
USE_ARDRONE_TOOL=yes
USE_CHECK_WIFI_CONFIG=no
################## Wifi Options ##################
# Name of the network you want to join or create
WIFI_NETWORK_NAME = "ardronenetwork"
WIFI_BROADCAST = "192.168.1.255"
################## Video Options ##################
# Tells if we want to record video on pc side
RECORD_ENCODED_VIDEO = yes
RECORD_RAW_VIDEO = no
RECORD_FFMPEG_VIDEO = no
# Tells if we want to add vision data to video stream (in raw mode)
# Vision data are saved into file only if we define RECORD_RAW_VIDEO too
RECORD_VISION_DATA = no
# If the yuv mode is choosen then video is displayed & recorded in color
# Otherwise only luminances are displayed & recorded
VIDEO_YUV = yes
#########################################################
# Embedded definitions (CUSTOM)
#########################################################
WIFI_ARDRONE_IP = "192.168.1.1"
#########################################################
# Linux definitions (CUSTOM)
#########################################################
WIFI_MOBILE_IP = "192.168.1.2"
ARDroneLib/Soft/Common/academy_common.h
-49
Ver Arquivo
@@ -1,49 +0,0 @@
//
// academy_common.h
// ARDroneEngine
//
// Created by Frédéric D'Haeyer on 2/28/12.
// Copyright (c) 2012 Parrot SA. All rights reserved.
//
#ifndef _ACADEMY_COMMON_H_
#define _ACADEMY_COMMON_H_
#include <VP_Os/vp_os_types.h>
#define ACADEMY_USERNAME_SIZE 64
#define ACADEMY_PASSWORD_SIZE 64
typedef enum _ACADEMY_RESULT_
{
ACADEMY_RESULT_NONE = 0,
ACADEMY_RESULT_OK,
ACADEMY_RESULT_FAILED,
} ACADEMY_RESULT;
typedef enum _ACADEMY_STATE_
{
ACADEMY_STATE_NONE = 0,
ACADEMY_STATE_CONNECTION,
ACADEMY_STATE_PREPARE_PROCESS,
ACADEMY_STATE_PROCESS,
ACADEMY_STATE_FINISH_PROCESS,
ACADEMY_STATE_DISCONNECTION,
ACADEMY_STATE_MAX
} ACADEMY_STATE;
typedef struct _academy_state_t_
{
ACADEMY_STATE state;
ACADEMY_RESULT result;
} academy_state_t;
typedef struct _academy_user_t_
{
char username[ACADEMY_USERNAME_SIZE];
char password[ACADEMY_PASSWORD_SIZE];
} academy_user_t;
typedef void (*academy_callback)(academy_state_t state);
typedef void (*academy_download_new_media)(const char *mediaPath, bool_t addToQueue);
#endif // _ACADEMY_COMMON_H_
ARDroneLib/Soft/Common/ardrone_api.h
-813
Ver Arquivo
@@ -1,813 +0,0 @@
/******************************************************************************
* COPYRIGHT PARROT 2010
******************************************************************************
* PARROT A.R.Drone SDK
*---------------------------------------------------------------------------*/
/**
* @file ardrone_api.h
* @brief Data types and functions to communicate with the drone.
*
******************************************************************************/
#ifndef _ARDRONE_API_H_
#define _ARDRONE_API_H_
/*--- Libraries --------------------------------------------------------------*/
#include <sys/time.h>
#include <ardrone_common_config.h>
#include <ATcodec/ATcodec_api.h>
#include <VP_Os/vp_os_malloc.h>
#include <navdata_common.h>
#include <vision_common.h>
#include <Maths/quaternions.h>
/** @def API_WEAK
* @brief Defines the API_WEAK attribute.
* It is used to define a function which
* can be redefined by the SDK user with a custom one, without generating
* a 'multiple definition' compilation error. This is a GCC specific feature.
**/
#if defined(USE_LINUX) && defined(_EMBEDDED) && !defined(USE_MINGW32)
#define API_WEAK WEAK
#else
#define API_WEAK
#endif
/** @def ARDRONE_CONFIGURATION_SET
* @brief Sets a drone configuration value.
* This is the main method that SDK users should use to change a setting on
* the drone.
* @def ARDRONE_CONFIGURATION_SET_FUNCTION
* @brief Used internally by ARDroneLib to build the name of a function
* changing a particular drone configuration value.
* @def ARDRONE_CONFIGURATION_PROTOTYPE
* @brief Used internally by ARDroneLib to define a function changing a
* particular drone configuration value.
* **/
//Deprecated - no ack method - #define ARDRONE_CONFIGURATION_SET(NAME, VALUE) ARDRONE_CONFIGURATION_SET_FUNCTION(NAME)(VALUE)
#define ARDRONE_CONFIGURATION_SET_FUNCTION(NAME) ardrone_at_configuration_set_##NAME
#define ARDRONE_CONFIGURATION_PROTOTYPE(NAME) C_RESULT ARDRONE_CONFIGURATION_SET_FUNCTION(NAME)(void* value, char* ses_id, char* usr_id, char* app_id)
/**
* @brief Used internally by ARDroneTool as an argument to the creation of
* a control event, when changing a configuration value.
* SDK users should not deal with this; see ARDRONE_CONFIGURATION_SET to change
* a drone setting.
*/
typedef C_RESULT (*ardrone_at_configuration_set)(void* value, char* ses_id, char* usr_id, char* app_id);
/**
* @brief Used internally by ARDroneTool as argument to the miscellaneous
* settings (see AT*MISC command).
*/
enum
{
NO_CONTROL = 0,
ALTITUDE_CONTROL = 1,
ALTITUDE_VISION_CONTROL = 2,
ALTITUDE_VISION_CONTROL_TAKEOFF_TRIM = 3,
MULTICONFIGURATION = 1024
};
/**
* @brief Possible states of the drone 'control' thread.
*/
typedef enum
{
NO_CONTROL_MODE = 0, /*<! Doing nothing */
ARDRONE_UPDATE_CONTROL_MODE, /*<! Not used */
PIC_UPDATE_CONTROL_MODE, /*<! Not used */
LOGS_GET_CONTROL_MODE, /*<! Not used */
CFG_GET_CONTROL_MODE, /*<! Send active configuration file to a client through the 'control' socket UDP 5559 */
ACK_CONTROL_MODE, /*<! Reset command mask in navdata */
CUSTOM_CFG_GET_CONTROL_MODE /*<! Requests the list of custom configuration IDs */
} ARDRONE_CONTROL_MODE;
/**
* @brief Drone video channels selection values.
* A client can choose to receive video data from the horizontal camera,
* the vertical one, or both (in a Picture-In-Picture way).
*/
typedef enum
{
ZAP_CHANNEL_FIRST = 0, /*<! First element */
ZAP_CHANNEL_HORI = ZAP_CHANNEL_FIRST, /*<! Selects the horizontal camera */
ZAP_CHANNEL_VERT, /*<! Selects the vertical camera */
ZAP_CHANNEL_LARGE_HORI_SMALL_VERT, /*<! Selects the horizontal camera with vertical camera picture inserted in the left-top corner */
ZAP_CHANNEL_LARGE_VERT_SMALL_HORI, /*<! Selects the vertical camera with horizontal camera picture inserted in the left-top corner */
ZAP_CHANNEL_LAST = ZAP_CHANNEL_LARGE_VERT_SMALL_HORI, /*<! Last element */
ZAP_CHANNEL_NEXT, /*<! Selects the next available format among those above. */
} ZAP_VIDEO_CHANNEL;
/**
* @brief Values for the detection type on drone cameras.
*/
typedef enum
{
CAD_TYPE_HORIZONTAL = 0, /*<! Deprecated */
CAD_TYPE_VERTICAL, /*<! Deprecated */
CAD_TYPE_VISION, /*<! Detection of 2D horizontal tags on drone shells */
CAD_TYPE_NONE, /*<! Detection disabled */
CAD_TYPE_COCARDE, /*<! Detects a roundel under the drone */
CAD_TYPE_ORIENTED_COCARDE, /*<! Detects an oriented roundel under the drone */
CAD_TYPE_STRIPE, /*<! Detects a uniform stripe on the ground */
CAD_TYPE_H_COCARDE, /*<! Detects a roundel in front of the drone */
CAD_TYPE_H_ORIENTED_COCARDE, /*<! Detects an oriented roundel in front of the drone */
CAD_TYPE_STRIPE_V,
CAD_TYPE_MULTIPLE_DETECTION_MODE, /* The drone uses several detections at the same time */
CAD_TYPE_CAP, /*<! Detects a Cap orange and green in front of the drone */
CAD_TYPE_ORIENTED_COCARDE_BW, /*<! Detects the black and white roundel */
CAD_TYPE_VISION_V2, /*<! Detects 2nd version of shell/tag in front of the drone */
CAD_TYPE_TOWER_SIDE, /*<! Detect a tower side with the front camera */
CAD_TYPE_NUM, /*<! Number of possible values for CAD_TYPE */
} CAD_TYPE;
/* Type of tag to detect
* This enum deprecates 'CAD_TYPE' which did not allow to mix tag types and cameras */
typedef enum
{
TAG_TYPE_NONE = 0,
TAG_TYPE_SHELL_TAG ,
TAG_TYPE_ROUNDEL ,
TAG_TYPE_ORIENTED_ROUNDEL ,
TAG_TYPE_STRIPE ,
TAG_TYPE_CAP ,
TAG_TYPE_SHELL_TAG_V2 ,
TAG_TYPE_TOWER_SIDE ,
TAG_TYPE_BLACK_ROUNDEL ,
TAG_TYPE_NUM
} TAG_TYPE;
#define TAG_TYPE_MASK(tagtype) ( ((tagtype)==0)? 0 : (1<<((tagtype)-1)) )
typedef enum
{
DETECTION_SOURCE_CAMERA_HORIZONTAL=0, /*<! Tag was detected on the front camera picture */
DETECTION_SOURCE_CAMERA_VERTICAL, /*<! Tag was detected on the vertical camera picture at full speed */
DETECTION_SOURCE_CAMERA_VERTICAL_HSYNC, /*<! Tag was detected on the vertical camera picture inside the horizontal pipeline */
DETECTION_SOURCE_CAMERA_NUM,
} DETECTION_SOURCE_CAMERA;
#define DETECTION_MAKE_TYPE(source,tag) ( ((source)<<16) | (tag) )
#define DETECTION_EXTRACT_SOURCE(type) ( ((type)>>16) & 0x0FF )
#define DETECTION_EXTRACT_TAG(type) ( (type) & 0x0FF )
/**
* @brief Video bitrate control mode.
*/
typedef enum
{
VBC_MODE_DISABLED = 0, /*<! no video bitrate control */
VBC_MODE_DYNAMIC, /*<! video bitrate control active */
VBC_MANUAL /*<! video bitrate control active */
} VIDEO_BITRATE_CONTROL_MODE;
/**
* @brief 2D-tag color values, used by the detection algorithms.
*/
typedef enum
{
ARDRONE_DETECTION_COLOR_ORANGE_GREEN = 1, /*!< Cameras detect orange-green-orange tags */
ARDRONE_DETECTION_COLOR_ORANGE_YELLOW, /*!< Cameras detect orange-yellow-orange tags*/
ARDRONE_DETECTION_COLOR_ORANGE_BLUE, /*!< Cameras detect orange-blue-orange tags */
ARDRONE_DETECTION_COLOR_ARRACE_FINISH_LINE=0x10,
ARDRONE_DETECTION_COLOR_ARRACE_DONUT=0x11
} COLORS_DETECTION_TYPE, ENEMY_COLORS_TYPE;
/**
* @brief User-control mode setting in FreeFlight.
* @description Bit mask showing how a user of the FreeFlight app. controls their drone.
*/
typedef enum
{
// This is a bit to shift
// CONTROL_LEVEL_NOT_USED = 0,
CONTROL_LEVEL_COMBINED_YAW = 1, /*!< =1 : drone tilts sideward and turns simultaneously */
// CONTROL_LEVEL_NOT_USED = 2,
//4 used for CONTROL_LEVEL_CONTROL_MODE_BIT
} CONTROL_LEVEL;
/**
* @enum LED_ANIMATION_IDS
* @brief Led animation values.
* See ardrone_at_set_led_animation function.
*/
//
typedef enum LED_ANIMATION_IDS_
{
#define LED_ANIMATION(NAME, ... ) NAME ,
#include <led_animation.h>
#undef LED_ANIMATION
NUM_LED_ANIMATION
} LED_ANIMATION_IDS;
typedef enum
{
USERBOX_CMD_STOP = 0,
USERBOX_CMD_START,
USERBOX_CMD_SCREENSHOT,
USERBOX_CMD_CANCEL,
USERBOX_CMD_MAX,
} USERBOX_CMD;
typedef enum
{
FLYING_STATE_LANDED = 0,
FLYING_STATE_FLYING,
FLYING_STATE_TAKING_OFF,
FLYING_STATE_LANDING,
} FLYING_STATE;
typedef enum ARDRONE_PROGRESSIVE_CMD_FLAG_
{
ARDRONE_PROGRESSIVE_CMD_ENABLE, // 1: use progressive commands - 0: try hovering
ARDRONE_PROGRESSIVE_CMD_COMBINED_YAW_ACTIVE, // 1: activate combined yaw - 0: Deactivate combined yaw
ARDRONE_MAGNETO_CMD_ENABLE, // 1: activate the magneto piloting mode - 0: desactivate the mode
ARDRONE_PROGRESSIVE_CMD_MAX
} ARDRONE_PROGRESSIVE_CMD_FLAG;
/**
* @struct _euler_angles_t
* @brief Euler angles in float32_t format expressed in radians.
*/
typedef struct _euler_angles_t
{
float32_t theta; /*<! Drone front-back angle (pitch) */
float32_t phi; /*<! Drone left-right angle (roll) */
float32_t psi; /*<! Drone orientation (yaw) */
} euler_angles_t;
/**
* @struct _euler_angles_t
* @brief Euler angles in int32_t format expressed in radians.
*/
typedef struct _iEuler_angles_t
{
int32_t theta; /*<! Drone front-back angle (pitch) */
int32_t phi; /*<! Drone left-right angle (roll) */
int32_t psi; /*<! Drone orientation (yaw) */
} iEuler_angles_t;
/**
* @struct _angular_rates_t
* @brief Angular rates in float32_t format
*/
typedef struct _angular_rates_t
{
float32_t p; /*<! Drone front-back angular rate (pitch) */
float32_t q; /*<! Drone left-right angular rate (roll) */
float32_t r; /*<! Drone orientation angular rate (yaw) */
} angular_rates_t;
/**
* \struct _velocities_t
* \brief Velocities in float32_t format
*/
/*
typedef struct _velocities_t {
float32_t x;
float32_t y;
float32_t z;
} velocities_t;
*/
/**
* @struct _acq_vision_t
* @brief Deprecated - used internally by the drone - Vision params in float32_t
*/
#ifndef DISABLE_DEPRECATED_CODE
typedef struct _acq_vision_t
{
float32_t tx;
float32_t ty;
float32_t tz;
int32_t turn_angle;
int32_t height;
int32_t turn_finished;
bool_t flag_25Hz;
} acq_vision_t;
#endif
/**
* @struct _polaris_data_t
* @brief Used by Parrot only - drone externally-measured position in 3D-space
*/
typedef struct _polaris_data_t
{
float32_t x;
float32_t y;
float32_t psi;
bool_t defined;
int32_t time_us;
} polaris_data_t;
/**
* @struct api_control_gains_t
* @brief Drone control loop PID settings
*/
typedef struct _api_control_gains_t
{
int32_t pq_kp; /**< Gain for proportional correction in pitch (p) and roll (q) angular rate control */
int32_t r_kp; /**< Gain for proportional correction in yaw (r) angular rate control */
int32_t r_ki; /**< Gain for integral correction in yaw (r) angular rate control */
int32_t ea_kp; /**< Gain for proportional correction in Euler angle control */
int32_t ea_ki; /**< Gain for integral correction in Euler angle control */
int32_t alt_kp; /**< Gain for proportional correction in Altitude control */
int32_t alt_ki; /**< Gain for integral correction in Altitude control */
int32_t vz_kp; /**< Gain for proportional correction in Vz control */
int32_t vz_ki; /**< Gain for integral correction in Vz control */
int32_t hovering_kp; /**< Gain for proportional correction in hovering control */
int32_t hovering_ki; /**< Gain for integral correction in hovering control */
int32_t hovering_b_kp; /**< Gain for proportional correction in hovering beacon control */
int32_t hovering_b_ki; /**< Gain for integral correction in hovering beacon control */
int32_t hovering_b_kp2 ;
int32_t hovering_b_ki2 ;
int32_t hovering_b_kd2 ;
} api_control_gains_t;
/**
* @struct _api_vision_tracker_params_t
* @brief Computer vision tracking settings
*/
typedef struct _api_vision_tracker_params_t
{
int32_t coarse_scale; /**< scale of current picture with respect to original picture */
int32_t nb_pair; /**< number of searched pairs in each direction */
int32_t loss_per; /**< authorized lost pairs percentage for tracking */
int32_t nb_tracker_width; /**< number of trackers in width of current picture */
int32_t nb_tracker_height; /**< number of trackers in height of current picture */
int32_t scale; /**< distance between two pixels in a pair */
int32_t trans_max; /**< largest value of trackers translation between two adjacent pictures */
int32_t max_pair_dist; /**< largest distance of pairs research from tracker location */
int32_t noise; /**< threshold of meaningful contrast */
} api_vision_tracker_params_t;
/* Stephane
* @enum FLYING_MODE
* @brief Values for the CONTROL:flying_mode configuration.
*/
typedef enum
{
FLYING_MODE_FREE_FLIGHT = 0, /**< Normal mode, commands are enabled */
FLYING_MODE_HOVER_ON_TOP_OF_ROUNDEL = 1 << 0, /**< Commands are disabled, drones hovers on top of a roundel - roundel detection MUST be activated by the user with 'detect_type' configuration. */
FLYING_MODE_HOVER_ON_TOP_OF_ORIENTED_ROUNDEL = 1 << 1, /**< Commands are disabled, drones hovers on top of an oriented roundel - oriented roundel detection MUST be activated by the user with 'detect_type' configuration. */
} FLYING_MODE;
/**
* @enum WIFI_MODE
* @brief Values for the NETWORK:wifi_mode configuration, who set the wifi mode when drone start
*/
typedef enum
{
WIFI_MODE_INFRA = 0, /**< Access point mode */
WIFI_MODE_ADHOC, /**< Ad-Hoc mode */
WIFI_MODE_MANAGED, /**< Managed mode */
WIFI_MODE_NUM
} WIFI_MODE;
/********************************************************************
* NAVDATA FUNCTIONS
********************************************************************/
/**
* @struct _navdata_unpacked_t
* @brief Decoded navigation data.
*/
typedef struct _navdata_unpacked_t
{
uint32_t nd_seq;
uint32_t ardrone_state;
bool_t vision_defined;
uint32_t last_navdata_refresh; /*! mask showing which block was refreshed when receiving navdata */
#define NAVDATA_OPTION_DEMO(STRUCTURE,NAME,TAG) STRUCTURE NAME ;
#define NAVDATA_OPTION(STRUCTURE,NAME,TAG) STRUCTURE NAME ;
#define NAVDATA_OPTION_CKS(STRUCTURE,NAME,TAG)
#include <navdata_keys.h>
} navdata_unpacked_t;
/**
* @def ardrone_navdata_pack
* @brief Add an 'option' to the navdata network packet to be sent to a client.
* Used by the drone 'navdata' thread.
*/
#define ardrone_navdata_pack( navdata_ptr, option ) (navdata_option_t*) navdata_pack_option( (uint8_t*) navdata_ptr, \
(uint8_t*) &option, \
option.size )
/**
* @def ardrone_navdata_unpack
* @brief Extract an'option' from the navdata network packet sent by the drone.
* Used by the client 'navdata' thread inside ARDroneTool.
*/
#define ardrone_navdata_unpack( navdata_ptr, option ) (navdata_option_t*) navdata_unpack_option( (uint8_t*) navdata_ptr, \
navdata_ptr->size, \
(uint8_t*) &option, \
sizeof (option) )
/**
* @brief Add an 'option' to the navdata network packet to be sent to a client.
* Used by the drone 'navdata' thread.
* @param navdata_ptr Pointer to the buffer containing packed navdata to be sent.
*/
static INLINE uint8_t* navdata_pack_option( uint8_t* navdata_ptr, uint8_t* data, uint32_t size )
{
vp_os_memcpy(navdata_ptr, data, size);
return (navdata_ptr + size);
}
/**
* @fn navdata_unpack_option
* @brief Extract an 'option' from the navdata network packet sent by the drone.
* Used by the client 'navdata' thread inside ARDroneTool.
*/
static INLINE uint8_t* navdata_unpack_option( uint8_t* navdata_ptr, uint32_t ptrsize, uint8_t* data, uint32_t datasize )
{
uint32_t minSize = (ptrsize < datasize) ? ptrsize : datasize;
vp_os_memcpy(data, navdata_ptr, minSize);
return (navdata_ptr + ptrsize);
}
/**
* @fn navdata_next_option
* @brief Jumps to the next 'option' inside the packed navdata.
* Used by the client 'navdata' thread inside ARDroneTool.
*/
static INLINE navdata_option_t* navdata_next_option( navdata_option_t* navdata_options_ptr )
{
uint8_t* ptr;
ptr = (uint8_t*) navdata_options_ptr;
ptr += navdata_options_ptr->size;
return (navdata_option_t*) ptr;
}
/**
* @brief Creates a checksum from a packed navdata packet.
* @param nv Data to calculate the checksum.
* @param size Size of data calculate as follow : size-sizeof(navdata_cks_t).
* @return Retrieves the checksum from the navdata nv.
*/
uint32_t ardrone_navdata_compute_cks( uint8_t* nv, int32_t size ) API_WEAK;
/**
* @param navdata_unpacked navdata_unpacked in which to store the navdata.
* @param navdata One packet read from the port NAVDATA.
* @param Checksum of navdata
* @brief Disassembles a buffer of received navdata, and dispatches it inside 'navdata_unpacked' structure.
*/
C_RESULT ardrone_navdata_unpack_all(navdata_unpacked_t* navdata_unpacked, navdata_t* navdata, uint32_t* cks) API_WEAK;
/***
* @param navdata_options_ptr
* @param Tag ID of the bloc to search for.
* @brief Jumps to a specified 'option' (block of navdata) inside a navdata packed buffer.
*/
navdata_option_t* ardrone_navdata_search_option( navdata_option_t* navdata_options_ptr, navdata_tag_t tag ) API_WEAK;
/********************************************************************
* AT FUNCTIONS
********************************************************************/
/**
* @brief Initializes the AT Command managing thread of ARDroneTool.
* The AT Codec is initialized.
*/
void ardrone_at_init( const char* ip, size_t ip_len ) API_WEAK;
/**
* @brief Initializes the AT Command managing thread of ARDroneTool, by
* providing it with custom functions for I/O from/to the drone.
*/
void ardrone_at_init_with_funcs ( const char* ip, size_t ip_len, AT_CODEC_FUNCTIONS_PTRS *funcs) API_WEAK;
/**
* @fn ardrone_at_open
* @brief Opens the AT command socket.
*/
ATCODEC_RET ardrone_at_open ( void ) API_WEAK;
/**
* @brief Makes ARDroneToll send all queued AT commands to the drone.
* ARDroneTool locally buffers AT commands sent by the client program
* until this function is called.
* Usually the client program GUI calls ardrone_at_xxx commands to
* pilot the drone; these commands are then queued by ARDroneTool.
* The ARDroneTool main thread calls this functions periodically
* (every 30ms for example) to actually send to queued commands to
* the drone.
*/
ATCODEC_RET ardrone_at_send ( void ) API_WEAK;
/*
* @brief Sends input sequence number to avoid reception of old data
* @param value : sequence number
* @deprecated
* @return void
* Used on prototypes.
*/
//void ardrone_at_set_sequence( uint32_t sequence ) API_WEAK;
/**
* @brief Set the flight status command.
* @param value Bit mask containing the desired commmand.
* The bit mask contains the bit controlling take-off/landing
* and the bit signaling /resuming from an emergency.
* This mask is periodically sent to the drone by ARDroneTool
* inside an AT*REF command.
*/
void ardrone_at_set_ui_pad_value( uint32_t value ) API_WEAK;
/**
* @brief Used internally by Parrot - send misc. config. data.
* @param data used to configure control
* @return void
*/
void ardrone_at_set_pmode( int32_t pmode ) API_WEAK;
#ifndef DISABLE_DEPRECATED_CODE
/**
* @fn Tell to keep trim result
* @param yes or no
* @return C_RESULT
*/
void ardrone_at_keep_trim( bool_t keep ) API_WEAK;
/**
* @fn Reset trim/misc0 related ack's
* @return void
*/
void ardrone_at_trim_ack_reset( void ) API_WEAK;
#endif
/**
* @brief Used by Parrot - send misc. config. data for drone debugging.
* @param data are used to configure control (for instance)
* @return void
* This should not be used by SDK users. Parameters value do not have any fixed meaning.
*/
void ardrone_at_set_ui_misc( int32_t m1, int32_t m2, int32_t m3, int32_t m4 ) API_WEAK;
/**
* @brief Makes the drone play a predefined movement
* @param type type of animation
* @param duration duration of the animation in seconds
*/
void ardrone_at_set_anim( anim_mayday_t type, int32_t duration ) API_WEAK;
/**
* @brief Instructs the drone to use its current position
* as a reference for the horizontal plane.
*/
void ardrone_at_set_flat_trim( void ) API_WEAK;
/**
* @brief Sets a manual trim (offset) on the drone commands
*/
void ardrone_at_set_manual_trims( float32_t trim_pitch, float32_t trim_roll, float32_t trim_yaw ) API_WEAK;
/**
* @brief Changes PID gains of the drone control loop
* @param user_ctrl_gains gains to be set
*/
void ardrone_at_set_control_gains( api_control_gains_t* user_ctrl_gains ) API_WEAK;
/**
* @brief Changes the tracking parameters (only in UE_IHM_PO mode)
* @param params : new params
*/
void ardrone_at_set_vision_track_params( api_vision_tracker_params_t* params ) API_WEAK;
/**
* @brief Start/stop raw capture on the drone.
* @param active : 1 for start capture, 0 to stop
*/
void ardrone_at_raw_capture( uint8_t active ) API_WEAK;
/**
* @brief Capture a frame in raw format on the drone.
*/
void ardrone_at_raw_picture(void) API_WEAK;
/**
* @brief Changes the type of object to detect
*/
void ardrone_at_cad( CAD_TYPE type, float32_t tag_size ) API_WEAK;
/**
* @brief Change the broadcasted video channel
*/
void ardrone_at_zap( ZAP_VIDEO_CHANNEL channel ) API_WEAK;
/**
* @brief Plays a led animation
*/
void ardrone_at_set_led_animation ( LED_ANIMATION_IDS anim_id, float32_t freq, uint32_t duration_sec ) API_WEAK;
/**
* @fn Set vision update options (only in UE_IHM_PO mode)
* @param user_vision_option : new option
*/
void ardrone_at_set_vision_update_options( int32_t user_vision_option ) API_WEAK;
/**
* @brief Used internally at Parrot - sets the drones position as seen by polaris
* @param x_polaris : x of ardrone position seen by polaris
* @param y_polaris : y of ardrone position seen by polaris
* @param defined : tells if polaris data are valid or not
* @param time_us : time in us
*/
void ardrone_at_set_polaris_pos( float32_t x_polaris, float32_t y_polaris, float32_t psi_polaris, bool_t defined, int32_t time_us ) API_WEAK;
/**
* @brief Used internally at Parrot - sets the drones position as seen by vicon
* @param time : vicon timestamp
* @param frame_number : vicon frame_number
* @param latency : vicon latency
* @param global_translation : vicon global translation
* @param global_rotation_euler : vicon global rotation (euler angles)
*/
void ardrone_at_set_vicon_data(struct timeval time, int32_t frame_number, float32_t latency, vector31_t global_translation, vector31_t global_rotation_euler);
/**
* @brief Send to drone a identified configuration
* @param param : parameter to set or update
* @param ses_id : session id
* @param usr_id : user id
* @param app_id : application id
* @param value : value to apply to the parameter
*/
void ardrone_at_set_toy_configuration_ids(const char* param,char* ses_id, char* usr_id, char* app_id, const char* value) API_WEAK;
/**
* @brief Asks the drone to reset the communication watchdog.
* If no command is received by the drone
*/
void ardrone_at_reset_com_watchdog( void ) API_WEAK;
/**
* @brief Asks the drone to purge log files
*/
void ardrone_at_reset_logs( void ) API_WEAK;
/**
* @brief Asks the drone we receive ackknowledges
*/
void ardrone_at_update_control_mode(ARDRONE_CONTROL_MODE control_mode) API_WEAK;
/**
* @brief Asks the drone to send control mode
*/
void ardrone_at_configuration_get_ctrl_mode( void ) API_WEAK;
void ardrone_at_custom_configuration_get_ctrl_mode(void) API_WEAK;
/**
* @brief Tells the drone we received the control mode
*/
void ardrone_at_configuration_ack_ctrl_mode( void ) API_WEAK;
void ardrone_at_set_autonomous_flight( int32_t isActive );
typedef enum
{
ARDRONE_CALIBRATION_DEVICE_MAGNETOMETER = 0,
ARDRONE_CALIBRATION_DEVICE_NUMBER,
} ardrone_calibration_device_t;
/**
* @brief Start the calibration of the given device
*/
void ardrone_at_set_calibration (int32_t deviceId);
/**
* @brief Sets the drone motor command directly
* This is enables only on prototypes drones.
*/
void ardrone_at_set_pwm( int32_t p1, int32_t p2, int32_t p3, int32_t p4 ) API_WEAK;
/**
* @fn ardrone_at_set_progress_cmd
* @brief Sends the drone progressive commands
* @param flag Use 1 << value of ARDRONE_PROGRESSIVE_CMD_FLAG_XXX to use a flag
* @param phi Left/right angle between -1 to +1 - negative values bend leftward.
* @param roll Front/back angle between -1 to +1 - negative values bend forward.
* @param gaz Vertical speed - negative values make the drone go down.
* @param yaw Angular speed - negative values make the drone spin left.
* @param magneto_psi floating value between -1 to +1.
* @param magneto_psi_accuracy floating value between -1 to +1
* This function allows the client program to control the drone by giving it a front/back
* and left/right bending order, a vertical speed order, and a rotation order.
* All values are given as a percentage of the maximum bending angles (in degrees),
* vertical speed (in millimeters per second) and angular speed (in degrees per second).
*/
void ardrone_at_set_progress_cmd_with_magneto( int32_t flag, float32_t phi, float32_t theta, float32_t gaz, float32_t yaw, float32_t magneto_psi, float32_t magneto_psi_accuracy );
void ardrone_at_set_progress_cmd( int32_t flag, float32_t phi, float32_t theta, float32_t gaz, float32_t yaw);
/*****************************************************************
* AT CONFIG FUNCTIONS
*****************************************************************/
#undef ARDRONE_CONFIG_KEY_IMM
#undef ARDRONE_CONFIG_KEY_REF
#undef ARDRONE_CONFIG_KEY_STR
#define ARDRONE_CONFIG_KEY_IMM(KEY, NAME, INI_TYPE, C_TYPE, C_TYPE_PTR, RW, DEFAULT, CALLBACK)
#define ARDRONE_CONFIG_KEY_REF(KEY, NAME, INI_TYPE, C_TYPE, C_TYPE_PTR, RW, DEFAULT, CALLBACK)
#define ARDRONE_CONFIG_KEY_STR(KEY, NAME, INI_TYPE, C_TYPE, C_TYPE_PTR, RW, DEFAULT, CALLBACK)
#include <config_keys.h> // must be included before to have types
#undef ARDRONE_CONFIG_KEY_IMM
#undef ARDRONE_CONFIG_KEY_REF
#undef ARDRONE_CONFIG_KEY_STR
#define ARDRONE_CONFIG_KEY_IMM(KEY, NAME, INI_TYPE, C_TYPE, C_TYPE_PTR, RW, DEFAULT, CALLBACK) ARDRONE_CONFIGURATION_PROTOTYPE(NAME);
#define ARDRONE_CONFIG_KEY_REF(KEY, NAME, INI_TYPE, C_TYPE, C_TYPE_PTR, RW, DEFAULT, CALLBACK)
#define ARDRONE_CONFIG_KEY_STR(KEY, NAME, INI_TYPE, C_TYPE, C_TYPE_PTR, RW, DEFAULT, CALLBACK) ARDRONE_CONFIGURATION_PROTOTYPE(NAME);
#include <config_keys.h> // must be included before to have types
/********************************************************************
* CONFIG FUNCTIONS
********************************************************************/
#undef ARDRONE_CONFIG_KEY_IMM
#undef ARDRONE_CONFIG_KEY_REF
#undef ARDRONE_CONFIG_KEY_STR
#define ARDRONE_CONFIG_KEY_IMM(KEY, NAME, INI_TYPE, C_TYPE, C_TYPE_PTR, RW, DEFAULT, CALLBACK)
#define ARDRONE_CONFIG_KEY_REF(KEY, NAME, INI_TYPE, C_TYPE, C_TYPE_PTR, RW, DEFAULT, CALLBACK)
#define ARDRONE_CONFIG_KEY_STR(KEY, NAME, INI_TYPE, C_TYPE, C_TYPE_PTR, RW, DEFAULT, CALLBACK)
#include <config_keys.h> // must be included before to have types
#undef ARDRONE_CONFIG_KEY_IMM
#undef ARDRONE_CONFIG_KEY_REF
#undef ARDRONE_CONFIG_KEY_STR
#define ARDRONE_CONFIG_KEY_IMM(KEY, NAME, INI_TYPE, C_TYPE, C_TYPE_PTR, RW, DEFAULT, CALLBACK) \
C_TYPE get_##NAME(void) API_WEAK; \
C_RESULT set_##NAME(C_TYPE val) API_WEAK;
#define ARDRONE_CONFIG_KEY_REF(KEY, NAME, INI_TYPE, C_TYPE, C_TYPE_PTR, RW, DEFAULT, CALLBACK) \
C_TYPE_PTR get_##NAME(void) API_WEAK; \
C_RESULT set_##NAME(C_TYPE_PTR val) API_WEAK;
#define ARDRONE_CONFIG_KEY_STR(KEY, NAME, INI_TYPE, C_TYPE, C_TYPE_PTR, RW, DEFAULT, CALLBACK) \
C_TYPE_PTR get_##NAME(void) API_WEAK; \
C_RESULT set_##NAME(C_TYPE_PTR val) API_WEAK;
// Generate all accessors functions prototypes
#include <config_keys.h>
#undef ARDRONE_CONFIG_KEY_IMM
#undef ARDRONE_CONFIG_KEY_REF
#undef ARDRONE_CONFIG_KEY_STR
#define ARDRONE_CONFIG_KEY_IMM(KEY, NAME, INI_TYPE, C_TYPE, C_TYPE_PTR, RW, DEFAULT, CALLBACK)
#define ARDRONE_CONFIG_KEY_REF(KEY, NAME, INI_TYPE, C_TYPE, C_TYPE_PTR, RW, DEFAULT, CALLBACK)
#define ARDRONE_CONFIG_KEY_STR(KEY, NAME, INI_TYPE, C_TYPE, C_TYPE_PTR, RW, DEFAULT, CALLBACK)
#include <config_keys.h> // must be included before to have types
#undef ARDRONE_CONFIG_KEY_IMM
#undef ARDRONE_CONFIG_KEY_REF
#undef ARDRONE_CONFIG_KEY_STR
#define ARDRONE_CONFIG_KEY_IMM(KEY, NAME, INI_TYPE, C_TYPE, C_TYPE_PTR, RW, DEFAULT, CALLBACK) C_TYPE NAME;
#define ARDRONE_CONFIG_KEY_REF(KEY, NAME, INI_TYPE, C_TYPE, C_TYPE_PTR, RW, DEFAULT, CALLBACK) C_TYPE NAME;
#define ARDRONE_CONFIG_KEY_STR(KEY, NAME, INI_TYPE, C_TYPE, C_TYPE_PTR, RW, DEFAULT, CALLBACK) C_TYPE NAME;
// Fill structure fields types
typedef struct _ardrone_config_t
{
#include <config_keys.h>
}
_ATTRIBUTE_PACKED_ ardrone_config_t;
void reset_ardrone_config(void);
/********************************************************************
* USER FUNCTIONS
********************************************************************/
#define MULTICONFIG_ID_SIZE 9
#define SESSION_NAME_SIZE 1024
#define USER_NAME_SIZE 1024
#define APPLI_NAME_SIZE 1024
#define ROOT_NAME_SIZE 256
#define FLIGHT_NAME_SIZE 256
typedef struct _ardrone_user_t
{
char ident[MULTICONFIG_ID_SIZE];
char description[USER_NAME_SIZE];
} ardrone_user_t;
typedef struct _ardrone_users_t
{
int userCount;
ardrone_user_t *userList;
} ardrone_users_t;
void ardrone_refresh_user_list(void); // Ask for a userlist refresh
void ardrone_switch_to_user(const char *new_user); // Can be used to create user
void ardrone_switch_to_user_id(const char *new_user_id); // Must be used only with existing users
ardrone_users_t *ardrone_get_user_list(void); // Get a list of users (MUST BE FREED BY A ardrone_free_user_list CALL)
void ardrone_free_user_list (ardrone_users_t **users); // Free an ardrone_users_t list allocated by ardrone_get_user_list
#endif // _ARDRONE_API_H_
ARDroneLib/Soft/Common/ardrone_common_config.h
-57
Ver Arquivo
@@ -1,57 +0,0 @@
/**
* \file ardrone_common_config.h
* \brief Ardrone Specific data for configuration
* \author Sylvain Gaeremynck <sylvain.gaeremynck@parrot.com>
* \version 1.0
*/
#ifndef _ARDRONE_COMMON_CONFIG_H_
#define _ARDRONE_COMMON_CONFIG_H_
#define CAMIF_NUM_BUFFERS 2
#define COM_INPUT_LANDING_TIME (2) /* Time drone is waiting for input before landing */
/**
* \enum print_mask_t
* \brief mask to choose where to syslog
*/
typedef enum {
UART_PRINT = 1,
WIFI_PRINT = 2,
FLASH_PRINT = 4
} print_mask_t;
/**
* \enum ADC_COMMANDS
* \brief ADC commands.
*/
typedef enum
{
ADC_CMD_STARTACQ = 1, /**< command to start acquisition with ADC **/
ADC_CMD_STOPACQ = 2, /**< command to stop acquisition with ADC **/
ADC_CMD_RESYNC = 3, /**< command to resync acquisition with ADC **/
ADC_CMD_TEST = 4, /**< command to ADC send a test frame (123456) **/
ADC_CMD_VERSION = 5, /**< command to ADC send his number : version (MSB) subversion (LSB) **/
ADC_CMD_SELECT_ULTRASOUND_22Hz = 7, /**set the ultrasound at 22,22Hz **/
ADC_CMD_SELECT_ULTRASOUND_25Hz = 8, /**set the ultrasound at 25Hz **/
ADC_CMD_SEND_CALIBRE = 13, /**command to ADC to send the calibration **/
ADC_CMD_RECEVED_CALIBRE = 14, /**command to ADC to receved a new calibration **/
ADC_CMD_GET_HARD_VERSION = 15, /**get the hard version of the navboard **/
ADC_CMD_ACTIVE_SEPARATION = 16, /**enabled the separation of sources ultrasound **/
ADC_CMD_STOP_SEPARATION = 17, /**disables the ultrasound source separation **/
ADC_CMD_SEND_PROD = 18, /**command to ADC to receved the prod data **/
ADC_CMD_RECEVED_PROD = 19, /**command to ADC to send the prod data **/
ADC_CMD_ACTIVE_ETALONAGE = 20, /**command to ADC to send PWM ultrasond in continue **/
ADC_CMD_ACTIVE_ULTRASON = 21, /**command to ADC to stop send PWM ultrasond in continue **/
ADC_CMD_ACTIVE_TEST_ULTRASON = 22, /**teste de la perturbation de l'ultrason par le wifi **/
ADC_CMD_GET_CALIBRATION_PRESSION = 23, /**command to ADC to send the pressure sensor calibration **/
ADC_CMD_GET_CALIBRATION_MAGNETO = 24, /**After this command, pic sends sensitivity magnetometer**/
ADC_CMD_AK8975_SELF_TEST = 25, /**used for test magnetometer**/
ADC_CMD_RESET_OCSTUN = 26, /**set ocstun to 0**/
ADC_CMD_SET_OCSTUN = 27, /**set ocstun to RC value sent**/
ADC_CMD_RESYNC_START = 28, /**resync start**/
ADC_CMD_RESYNC_STOP = 29, /**resync stop**/
} ADC_COMMANDS;
#endif // _ARDRONE_COMMON_CONFIG_H_
ARDroneLib/Soft/Common/at_msgs.h
-56
Ver Arquivo
@@ -1,56 +0,0 @@
/**
* \file at_msgs.h
* \brief ATCodec messages declaration
* \author Aurelien Morelle <aurelien.morelle@parrot.com>
* \date 2007/04/03
* \version 1.0
*/
////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
ATCODEC_DEFINE_AT_CMD(AT_MSG_ATCMD_RC_REF_EXE, "AT*REF=%d,%d\r", 0, at_rc_ref_exe, 3 )
ATCODEC_DEFINE_AT_CMD(AT_MSG_ATCMD_PMODE_EXE, "AT*PMODE=%d,%d\r", 0, at_pmode_exe, 3 )
// old school
ATCODEC_DEFINE_AT_CMD(AT_MSG_ATCMD_MISC_EXE, "AT*MISC=%d,%d,%d,%d,%d\r", 0, at_misc_exe, 3 )
// gains
ATCODEC_DEFINE_AT_CMD(AT_MSG_ATCMD_GAIN_EXE, "AT*GAIN=%d,%d,%d,%d,%d,%d,%d,%d,%d,%d,%d,%d,%d,%d\r", 0, at_gain_exe, 3 )
ATCODEC_DEFINE_AT_CMD(AT_MSG_ATCMD_ANIM_EXE, "AT*ANIM=%d,%d,%d\r", 0, at_anim_exe, 3 )
// vision params
ATCODEC_DEFINE_AT_CMD(AT_MSG_ATCMD_VISP_EXE, "AT*VISP=%d,%d,%d,%d,%d,%d,%d,%d,%d,%d\r", 0, at_visp_exe, 3 )
// vision params
ATCODEC_DEFINE_AT_CMD(AT_MSG_ATCMD_VISO_EXE, "AT*VISO=%d,%d\r", 0, at_viso_exe, 3 )
// capture params
ATCODEC_DEFINE_AT_CMD(AT_MSG_ATCMD_RAWC_EXE, "AT*CAP=%d,%d,%d\r", 0, at_cap, 3 )
// zapper
ATCODEC_DEFINE_AT_CMD(AT_MSG_ATCMD_ZAP_EXE, "AT*ZAP=%d,%d\r", 0, at_zap, 3 )
// Change camera for arwiz detection
ATCODEC_DEFINE_AT_CMD(AT_MSG_ATCMD_CAD_EXE, "AT*CAD=%d,%d,%d\r", 0, at_cad, 3 )
// flat trim
ATCODEC_DEFINE_AT_CMD(AT_MSG_ATCMD_FTRIM_EXE, "AT*FTRIM=%d\r", 0, at_flat_trim_exe, 3 )
// manual trims
ATCODEC_DEFINE_AT_CMD(AT_MSG_ATCMD_MTRIM_EXE, "AT*MTRIM=%d,%d,%d,%d\r", 0, at_manual_trims_exe, 3 )
// send attitude
ATCODEC_DEFINE_AT_CMD(AT_MSG_ATCMD_POLARIS_EXE, "AT*POL=%d,%d,%d,%d,%d,%d\r", 0, at_pol_exe, 3 )
// sends iphone command for all axes
ATCODEC_DEFINE_AT_CMD(AT_MSG_ATCMD_PCMD_EXE, "AT*PCMD=%d,%d,%d,%d,%d,%d\r", 0, at_pcmd_exe, 3 )
// sends iphone command for all axes + device client agnetometer
ATCODEC_DEFINE_AT_CMD(AT_MSG_ATCMD_PCMD_MAG_EXE, "AT*PCMD_MAG=%d,%d,%d,%d,%d,%d,%d,%d\r", 0, at_pcmd_mag_exe, 3 )
// sends Radiocommand values for all 4 axis.
ATCODEC_DEFINE_AT_CMD(AT_MSG_ATCMD_CONFIG_EXE, "AT*CONFIG=%d,\"%s\",\"%s\"\r", 0, at_toy_configuration_exe, 3)
// control command
ATCODEC_DEFINE_AT_CMD(AT_MSG_ATCMD_CONFIG_IDS, "AT*CONFIG_IDS=%d,\"%s\",\"%s\",\"%s\"\r", 0, at_toy_configuration_ids, 4)
// control command
ATCODEC_DEFINE_AT_CMD(AT_MSG_ATCMD_CTRL_EXE, "AT*CTRL=%d,%d,%d\r", 0, at_control_exe, 3)
// led animation command
ATCODEC_DEFINE_AT_CMD(AT_MSG_ATCMD_LED_EXE, "AT*LED=%d,%d,%d,%d\r", 0, at_led_animation_exe, 3)
// reset com watchdog
ATCODEC_DEFINE_AT_CMD(AT_MSG_ATCMD_RESET_COM_WATCHDOG,"AT*COMWDG=%d\r", 0, at_reset_com_watchdog, 3)
ATCODEC_DEFINE_AT_CMD(AT_MSG_ATCMD_PWM_EXE, "AT*PWM=%d,%d,%d,%d,%d\r", 0, at_pwm, 3)
ATCODEC_DEFINE_AT_CMD(AT_MSG_ATCMD_AUTONOMOUS_FLIGHT_EXE, "AT*AFLIGHT=%d,%d\r", 0, at_autonomous_flight_exe, 3 )
// run instrument calibration
ATCODEC_DEFINE_AT_CMD(AT_MSG_ATCMD_CALIB, "AT*CALIB=%d,%d\r", 0, at_calibration, 3 )
// Vicon information
ATCODEC_DEFINE_AT_CMD(AT_MSG_ATCMD_VICON_EXE, "AT*VICON=%d,%d,%d,%d,%d,%d,%d,%d,%d,%d,%d,%d,%d,%d,%d,%d,%d,%d,%d,%d,%d,%d,%d\r", 0, at_vicon_exe, 3 )
ARDroneLib/Soft/Common/at_msgs_ids.h
-26
Ver Arquivo
@@ -1,26 +0,0 @@
/**
* \file at_msgs_ids.h
* \brief ids for ATCodec
* \author Aurelien Morelle <aurelien.morelle@parrot.com>
* \version 1.0
*/
#ifndef _AT_MSGS_IDS_H_
#define _AT_MSGS_IDS_H_
#include <ATcodec/ATcodec_api.h>
typedef struct _AT_CODEC_MSG_IDS_
{
# define ATCODEC_DEFINE_AT_CMD(ID,Str,From,Cb,Prio) AT_CODEC_MSG_ID ID;
# define ATCODEC_DEFINE_AT_RESU(ID,Str,From,Cb) AT_CODEC_MSG_ID ID;
AT_CODEC_MSG_ID AT_MSG_ATCMD_DEFAULT;
# include <at_msgs.h>
} AT_CODEC_MSG_IDS;
extern AT_CODEC_MSG_IDS ids;
#endif // _AT_MSGS_IDS_H_
ARDroneLib/Soft/Common/config.h
-337
Ver Arquivo
@@ -1,337 +0,0 @@
/**
***************************************************************************
*
* Copyright (C) 2007 Parrot S.A.
*
***************************************************************************
*/
#ifndef _CONFIG_H_
#define _CONFIG_H_
#include <VP_Os/vp_os_types.h>
#ifdef _WIN32
#include <win32_custom.h>
#else
#include <generated_custom.h>
#endif
#undef ARDRONE_PIC_VERSION
#define USE_NAVDATA_IP
#define USE_AT_IP
#define USE_VIDEO_IP
///////////////////////////////////////////////
// Video configuration
#define VIDEO_ENABLE 1
///////////////////////////////////////////////
// Vision configuration
#define VISION_ENABLE 1
// #define VISION_TEST_MODE
#define ARDRONE_VISION_DETECT
///////////////////////////////////////////////
// Navdata configuration
#define NAVDATA_ENABLE 1
#define ND_WRITE_TO_FILE
# define NAVDATA_SUBSAMPLING 13 /* 200 / 15 fps = 13.3333 */
#if defined(NAVDATA_ENABLE)
# define NAVDATA_VISION_DETECT_INCLUDED
# define NAVDATA_TRIMS_INCLUDED
# define NAVDATA_WATCHDOG
# define NAVDATA_EULER_ANGLES_INCLUDED
# define NAVDATA_PHYS_MEASURES_INCLUDED
# define NAVDATA_TIME_INCLUDED
# define NAVDATA_RAW_MEASURES_INCLUDED
# define NAVDATA_GYROS_OFFSETS_INCLUDED
# define NAVDATA_REFERENCES_INCLUDED
# define NAVDATA_RC_REFERENCES_INCLUDED
# define NAVDATA_PWM_INCLUDED
# define NAVDATA_ALTITUDE_INCLUDED
# define NAVDATA_VISION_INCLUDED
# define NAVDATA_VISION_PERF_INCLUDED
# define NAVDATA_TRACKERS_SEND
# define NAVDATA_VIDEO_STREAM_INCLUDED
# define NAVDATA_PRESSURES_INCLUDED
#endif // ! NAVDATA_ENABLE
#ifndef ARDRONE_VISION_DETECT
# undef NAVDATA_VISION_DETECT_INCLUDED
#endif // ! ARDRONE_VISION_DETECT
///////////////////////////////////////////////
// Wifi configuration
#define USE_AUTOIP VP_COM_AUTOIP_DISABLE /* VP_COM_AUTOIP_ENABLE */
#define WIFI_NETMASK "255.255.255.0"
#define WIFI_GATEWAY WIFI_ARDRONE_IP
#define WIFI_SERVER WIFI_ARDRONE_IP
#define WIFI_SECURE 0
#define WIFI_BASE_ADDR 0xc0a80100 // 192.168.1.0
#define MULTICAST_BASE_ADDR 0xe0010100 // 224.1.1.0
#define WIFI_BROADCAST_ADDR (WIFI_BASE_ADDR | 0xff) //XXX.XXX.XXX.255
// Configure infrastructure mode given wifi driver compilation
#define WIFI_INFRASTRUCTURE 0
#define WIFI_PASSKEY "9F1C3EE11CBA230B27BF1C1B6F"
#define FTP_PORT 5551
#define AUTH_PORT 5552
#define VIDEO_RECORDER_PORT 5553
#define NAVDATA_PORT 5554
#define VIDEO_PORT 5555
#define AT_PORT 5556
#define RAW_CAPTURE_PORT 5557
#define PRINTF_PORT 5558
#define CONTROL_PORT 5559
///////////////////////////////////////////////
// Wired configuration
#define WIRED_MOBILE_IP WIFI_MOBILE_IP
///////////////////////////////////////////////
// Serial link configuration
#ifdef USE_ELINUX
#define SERIAL_LINK_0 "/dev/ttyPA0"
#define SERIAL_LINK_1 "/dev/ttyPA1"
#define SERIAL_LINK_2 "/dev/ttyPA2"
#endif
#ifdef USE_LINUX
#ifdef USE_MINGW32
#define SERIAL_LINK_0 ""
#define SERIAL_LINK_1 ""
#define SERIAL_LINK_2 ""
#else
// Only USE_LINUX is defined
#define SERIAL_LINK_0 "/dev/ttyUSB0" /* Serial link for navdata & ATCmd */
#define SERIAL_LINK_1 "/dev/ttyUSB1" /* Serial link for video */
#define SERIAL_LINK_2 "/dev/ser2" /* Serial link for adc */
#endif // USE_MINGW32
#endif // USE_LINUX
#define SL0_BAUDRATE VP_COM_BAUDRATE_460800 /* baud rate for serial link 0 */
#define SL1_BAUDRATE VP_COM_BAUDRATE_460800 /* baud rate for serial link 1 */
#define SL2_BAUDRATE VP_COM_BAUDRATE_460800 /* baud rate for serial link 2 */
///////////////////////////////////////////////
// Defines & types used in shared data
#define ARDRONE_ORIENTATION_HISTORY_SIZE 256
#define TSECDEC 21 /* Defines used to format time ( seconds << 21 + useconds ) */
#define TUSECMASK ((1U << TSECDEC) - 1)
#define TSECMASK (0xffffffff & ~TUSECMASK)
#define TIME_TO_USEC (0xffffffff & ~TUSECMASK)
#define VBAT_POWERING_OFF 9000 /* Minimum Battery Voltage [mV] to prevent damaging */
/* Syslog Configuration */
#define SYSLOG_NUM_BUFFERS 4 /* Number of actives buffers. When a buffer is full, it's dumped in file */
#define SYSLOG_BUFFER_SIZE 2048 /* Max number of bytes in a syslog buffer */
#define SYSLOG_BUFFER_DUMP_SIZE 128 /* Max number of bytes wrote at once during dump */
#define DEFAULT_MISC1_VALUE 2
#define DEFAULT_MISC2_VALUE 20
#define DEFAULT_MISC3_VALUE 2000
#define DEFAULT_MISC4_VALUE 3000
typedef enum {
MISC_VAR1 = 0,
MISC_VAR2,
MISC_VAR3,
MISC_VAR4,
NB_MISC_VARS
} misc_var_t;
// Mayday scenarii
typedef enum {
ARDRONE_ANIM_PHI_M30_DEG= 0,
ARDRONE_ANIM_PHI_30_DEG,
ARDRONE_ANIM_THETA_M30_DEG,
ARDRONE_ANIM_THETA_30_DEG,
ARDRONE_ANIM_THETA_20DEG_YAW_200DEG,
ARDRONE_ANIM_THETA_20DEG_YAW_M200DEG,
ARDRONE_ANIM_TURNAROUND,
ARDRONE_ANIM_TURNAROUND_GODOWN,
ARDRONE_ANIM_YAW_SHAKE,
ARDRONE_ANIM_YAW_DANCE,
ARDRONE_ANIM_PHI_DANCE,
ARDRONE_ANIM_THETA_DANCE,
ARDRONE_ANIM_VZ_DANCE,
ARDRONE_ANIM_WAVE,
ARDRONE_ANIM_PHI_THETA_MIXED,
ARDRONE_ANIM_DOUBLE_PHI_THETA_MIXED,
ARDRONE_ANIM_FLIP_AHEAD,
ARDRONE_ANIM_FLIP_BEHIND,
ARDRONE_ANIM_FLIP_LEFT,
ARDRONE_ANIM_FLIP_RIGHT,
ARDRONE_NB_ANIM_MAYDAY
} anim_mayday_t;
// Bitfield definition for user input
typedef enum {
ARDRONE_UI_BIT_AG = 0,
ARDRONE_UI_BIT_AB = 1,
ARDRONE_UI_BIT_AD = 2,
ARDRONE_UI_BIT_AH = 3,
ARDRONE_UI_BIT_L1 = 4,
ARDRONE_UI_BIT_R1 = 5,
ARDRONE_UI_BIT_L2 = 6,
ARDRONE_UI_BIT_R2 = 7,
ARDRONE_UI_BIT_SELECT = 8,
ARDRONE_UI_BIT_START = 9,
ARDRONE_UI_BIT_TRIM_THETA = 18,
ARDRONE_UI_BIT_TRIM_PHI = 20,
ARDRONE_UI_BIT_TRIM_YAW = 22,
ARDRONE_UI_BIT_X = 24,
ARDRONE_UI_BIT_Y = 28,
} ardrone_ui_bitfield_t;
/// \enum def_ardrone_state_mask_t is a bit field representing ARDrone' state
// Define masks for ARDrone state
// 31 0
// x x x x x x x x x x x x x x x x x x x x x x x x x x x x x x x x -> state
// | | | | | | | | | | | | | | | | | | | | | | | | | | | | | | | |
// | | | | | | | | | | | | | | | | | | | | | | | | | | | | | | | FLY MASK : (0) ardrone is landed, (1) ardrone is flying
// | | | | | | | | | | | | | | | | | | | | | | | | | | | | | | VIDEO MASK : (0) video disable, (1) video enable
// | | | | | | | | | | | | | | | | | | | | | | | | | | | | | VISION MASK : (0) vision disable, (1) vision enable
// | | | | | | | | | | | | | | | | | | | | | | | | | | | | CONTROL ALGO : (0) euler angles control, (1) angular speed control
// | | | | | | | | | | | | | | | | | | | | | | | | | | | ALTITUDE CONTROL ALGO : (0) altitude control inactive (1) altitude control active
// | | | | | | | | | | | | | | | | | | | | | | | | | | USER feedback : Start button state
// | | | | | | | | | | | | | | | | | | | | | | | | | Control command ACK : (0) None, (1) one received
// | | | | | | | | | | | | | | | | | | | | | | | | Camera enable : (0) Camera enable, (1) camera disable
// | | | | | | | | | | | | | | | | | | | | | | | Travelling enable : (0) disable, (1) enable
// | | | | | | | | | | | | | | | | | | | | | | USB key : (0) usb key not ready, (1) usb key ready
// | | | | | | | | | | | | | | | | | | | | | Navdata demo : (0) All navdata, (1) only navdata demo
// | | | | | | | | | | | | | | | | | | | | Navdata bootstrap : (0) options sent in all or demo mode, (1) no navdata options sent
// | | | | | | | | | | | | | | | | | | | Motors status : (0) Ok, (1) Motors Com is down
// | | | | | | | | | | | | | | | | | | Communication Lost : (1) com problem, (0) Com is ok
// | | | | | | | | | | | | | | | | |
// | | | | | | | | | | | | | | | | VBat low : (1) too low, (0) Ok
// | | | | | | | | | | | | | | | User Emergency Landing : (1) User EL is ON, (0) User EL is OFF
// | | | | | | | | | | | | | | Timer elapsed : (1) elapsed, (0) not elapsed
// | | | | | | | | | | | | | Magnetometer calibration state : (0) Ok, no calibration needed, (1) not ok, calibration needed
// | | | | | | | | | | | | Angles : (0) Ok, (1) out of range
// | | | | | | | | | | | WIND MASK: (0) ok, (1) Too much wind
// | | | | | | | | | | Ultrasonic sensor : (0) Ok, (1) deaf
// | | | | | | | | | Cutout system detection : (0) Not detected, (1) detected
// | | | | | | | | PIC Version number OK : (0) a bad version number, (1) version number is OK
// | | | | | | | ATCodec thread ON : (0) thread OFF (1) thread ON
// | | | | | | Navdata thread ON : (0) thread OFF (1) thread ON
// | | | | | Video thread ON : (0) thread OFF (1) thread ON
// | | | | Acquisition thread ON : (0) thread OFF (1) thread ON
// | | | CTRL watchdog : (1) delay in control execution (> 5ms), (0) control is well scheduled // Check frequency of control loop
// | | ADC Watchdog : (1) delay in uart2 dsr (> 5ms), (0) uart2 is good // Check frequency of uart2 dsr (com with adc)
// | Communication Watchdog : (1) com problem, (0) Com is ok // Check if we have an active connection with a client
// Emergency landing : (0) no emergency, (1) emergency
/*
Note : ARMCC stores enums as signed 32-bit integers.
1<<31 is thus out of range, and must be defined as (-1) or its hexa representation
reference : http://infocenter.arm.com/help/topic/com.arm.doc.dui0436a/RVCT20_Errors_and_Warnings.pdf (page 7)
*/
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 */
#if defined(__ARMCC_VERSION)
ARDRONE_EMERGENCY_MASK = (int)0x80000000 /*!< Emergency landing : (0) no emergency, (1) emergency */
#else
ARDRONE_EMERGENCY_MASK = 1U << 31 /*!< Emergency landing : (0) no emergency, (1) emergency */
#endif
} def_ardrone_state_mask_t;
static INLINE uint32_t ardrone_set_state_with_mask( uint32_t state, uint32_t mask, bool_t value )
{
state &= ~mask;
if( value )
state |= mask;
return state;
}
/** Returns a bit value of state from a mask
* This function is used to test bits from a bit field like def_ardrone_state_mask_t
*
* @param state a 32 bits word we want to test
* @param mask a mask that tells the bit to test
* @return TRUE if bit is set, FALSE otherwise
*/
static INLINE bool_t ardrone_get_mask_from_state( uint32_t state, uint32_t mask )
{
return state & mask ? TRUE : FALSE;
}
/** Convert time value from proprietary format to (unsigned int) micro-second value
*
* @param time value in proprietary format
* @return time value in micro-second value (unsigned int)
*/
static INLINE uint32_t ardrone_time_to_usec( uint32_t time )
{
return ((uint32_t)(time >> TSECDEC) * 1000000 + (uint32_t)(time & TUSECMASK));
}
#ifdef DEBUG_MODE
#define POSIX_DEBUG
#endif // DEBUG_MODE
#endif // _CONFIG_H_
ARDroneLib/Soft/Common/config_keys.h
-347
Ver Arquivo
@@ -1,347 +0,0 @@
/******************************************************************************
* COPYRIGHT PARROT 2010
******************************************************************************
* PARROT A.R.Drone SDK
*---------------------------------------------------------------------------*/
/**
* @file config_keys.h
* @brief Definition of all the configuration values for the drone.
*
* This file mainly consists in a list of ARDRONE_CONFIG_KEY_xxx macros
* whose arguments describe the available configuration values.
* By redefining those macros and including this file anywhere else in the
* project, it is possible to build a set of C variables, functions or macros
* based on the configuration descriptions.
*
******************************************************************************/
#ifndef CFG_STRINGIFY
#define CFG_STRINGIFY(x) #x
#endif
#ifndef CONFIG_KEYS_STRING_TYPE_DEFINED
#define CONFIG_KEYS_STRING_TYPE_DEFINED
#define STRING_T_SIZE 128
typedef char string_t[STRING_T_SIZE];
#endif // ! CONFIG_KEYS_STRING_TYPE_DEFINED
#include <Maths/maths.h>
#include <VLIB/video_codec.h>
#ifndef CONFIG_KEYS_RW_ENUM_DEFINED
#define CONFIG_KEYS_RW_ENUM_DEFINED
/**
* @brief Describes the behaviour of a drone configuration variable.
*/
enum {
K_READ = 1, /*!< Value can be read by a remote client */
K_WRITE = 1<<1, /*!< Value can be written by a remote client */
K_NOBIND = 1<<2, /*!< Data are stored to the config.ini file, but not read from this file at startup.*/
K_SHALLOW = 1<<3, /*!< Data will no be stored to the config.ini file, nor read from this file at startup.*/
};
enum {
CAT_COMMON = 0,
CAT_APPLI ,
CAT_USER ,
CAT_SESSION,
NB_CONFIG_CATEGORIES
};
/* Size of the hexadecimal ID representing a custom configuration - example : 1234abcd */
#define CUSTOM_CONFIGURATION_ID_LENGTH 8
typedef struct{
char id[CUSTOM_CONFIGURATION_ID_LENGTH+1];
char description[1024];
} custom_configuration_t;
typedef struct
{
custom_configuration_t* list;
int nb_configurations;
} custom_configuration_list_t;
extern const char * configuration_switching_commands[NB_CONFIG_CATEGORIES+1];
extern const char * custom_configuration_headers[NB_CONFIG_CATEGORIES+1];
extern const char * custom_configuration_id_keys[NB_CONFIG_CATEGORIES+1];
extern custom_configuration_list_t available_configurations[NB_CONFIG_CATEGORIES];
/**
* \brief Checks if a character is valid in a custom configuration identifier.
* Currently it checks if the character is an hexadecimal digit.
*/
C_RESULT configuration_check_config_id_char(const char session_id_char);
/**
* \brief Checks if a string is a valid custom configuration identifier.
* Currently it checks if the string is made of 8 hexa. digits.
*/
C_RESULT configuration_check_config_id(const char * session_id);
#endif // ! CONFIG_KEYS_RW_ENUM_DEFINED
#ifndef CONFIG_KEYS_DEFINES_DEFINED
# define CONFIG_KEYS_DEFINES_DEFINED
# ifdef INSIDE_FLIGHT
# define MAX_EULER_ANGLES_REF (12000.0f * MDEG_TO_RAD) /* EA control, maximum reference [rad] */
# define MAX_OUTDOOR_EULER_ANGLES_REF (20000.0f * MDEG_TO_RAD) /* EA control, maximum reference [rad] */
# else
# define MAX_EULER_ANGLES_REF (12000.0f * MDEG_TO_RAD) /* EA control, maximum reference [rad] */
# define MAX_OUTDOOR_EULER_ANGLES_REF (20000.0f * MDEG_TO_RAD) /* EA control, maximum reference [rad] */
# endif
// GPS default Value
#define GPS_INVALID_COORDINATES 500.0
//Calibration renvoye par le PIC dans le cas ou il n'en a pas recut
#define DEFAULT_PWM_REF_GYRO 500
#define DEFAULT_GYRO_OFFSET_THR_X 4.0
#define DEFAULT_GYRO_OFFSET_THR_Y 4.0
#define DEFAULT_GYRO_OFFSET_THR_Z 0.5
#define DEFAULT_OSCTUN_VALUE 0
//#define default_accs_offset {{{ -2048.0f, 2048.0f, 2048.0f}}}
//#define default_accs_gain {1.0f, 0.0f, 0.0f, 0.0f, -1.0f, 0.0f, 0.0f, 0.0f, -1.0f }
//#define default_gyros_offset {{{ 1662.5f, 1662.5f, 1662.5f}}}
//#define default_gyros_gains {{{ 395.0f * MDEG_TO_RAD, -395.0f * MDEG_TO_RAD, -207.5f * MDEG_TO_RAD }}}
//#define default_gyros110_offset {{ 1662.5f, 1662.5f}}
//#define default_gyros110_gains {{ 87.5f * MDEG_TO_RAD, -87.5f * MDEG_TO_RAD }}
//#define default_magneto_offset {{{ 300.0f, -100.0f, 50.0f}}}
#define default_motor_version "0.0"
#define NULL_MAC "00:00:00:00:00:00"
#define DEFAULT_CONFIG_ID "00000000"
/* Selection of navdata blocks sent when starting the 'demo' mode */
#define default_navdata_options ( NAVDATA_OPTION_MASK(NAVDATA_DEMO_TAG)|NAVDATA_OPTION_MASK(NAVDATA_VISION_DETECT_TAG) )
#define COMPILE_TIME ( (2010-1970)*(365+(2010-1970)/4)*(24)*(3600) )
extern const vector31_t default_accs_offset;
extern const matrix33_t default_accs_gain;
extern const vector31_t default_gyros_offset;
extern const vector31_t default_gyros_gains;
extern const vector21_t default_gyros110_offset;
extern const vector21_t default_gyros110_gains;
extern const vector31_t default_magneto_offset;
extern const float32_t default_magneto_radius;
#define default_pwm_ref_gyro DEFAULT_PWM_REF_GYRO
#define default_gyro_offset_thr_x DEFAULT_GYRO_OFFSET_THR_X
#define default_gyro_offset_thr_y DEFAULT_GYRO_OFFSET_THR_Y
#define default_gyro_offset_thr_z DEFAULT_GYRO_OFFSET_THR_Z
#define default_osctun_value DEFAULT_OSCTUN_VALUE
#define default_euler_angle_ref_max MAX_EULER_ANGLES_REF
#define default_outdoor_euler_angle_ref_max MAX_OUTDOOR_EULER_ANGLES_REF
# define default_altitude_max (3000)
# define no_altitude_limit (100000)
# define default_altitude_min (50)
# define default_control_trim_z (0.0f * MDEG_TO_RAD)
# define default_control_iphone_tilt (20000.0f * MDEG_TO_RAD)
# define default_control_vz_max (700.0f)
# define default_outdoor_control_vz_max (1000.0f)
# define default_control_yaw (100000.0f * MDEG_TO_RAD)
# define default_outdoor_control_yaw (200000.0f * MDEG_TO_RAD)
#define default_enemy_colors (ARDRONE_DETECTION_COLOR_ORANGE_GREEN)
#define default_groundstripe_colors (ARDRONE_DETECTION_COLOR_ARRACE_FINISH_LINE)
#define default_detect_type CAD_TYPE_NONE
#define DEFAULT_APPLICATION_DESC "Default application configuration"
#define DEFAULT_PROFILE_DESC "Default profile configuration"
#define DEFAULT_SESSION_DESC "Default session configuration"
#define ARDRONE_DEFAULT_DATE "19700101_000000"
#define ARDRONE_EMPTY_STRING ""
#define CUSTOM_CONFIGURATION_DELETE_ALL_CMD "all"
#define CONFIG_HEADER 0x99aabbcc
#define HCONFIG_HEADER 0xffeeddcc
#ifndef CARD_VERSION
#define CARD_VERSION 0x0
#endif
#if CARD_VERSION == 0x0
# define DEFAULT_VIDEO_CODEC (UVLC_CODEC)
# define DEFAULT_FPS_CAMIF (0)
# define DEFAULT_FPS_CODEC (0)
#else
# if CARD_VERSION<0x20
# define DEFAULT_VIDEO_CODEC (UVLC_CODEC)
# define DEFAULT_FPS_CAMIF (20)
# define DEFAULT_FPS_CODEC (20)
# else
# define DEFAULT_VIDEO_CODEC (H264_360P_CODEC)
# define DEFAULT_FPS_CAMIF (30)
# define DEFAULT_FPS_CODEC (30)
# endif
#endif
#ifndef CURRENT_NUM_VERSION_CONFIG
#define CURRENT_NUM_VERSION_CONFIG 1
#endif
#endif // ! CONFIG_KEYS_DEFINES_DEFINED
/* ---- List of configuration properties - see the Developer Guide for a comprehensive description ---- */
/* Parameters attributes are : (KEY, NAME, INI_TYPE, C_TYPE, C_TYPE_PTR, RW, RW_CUSTOM, DEFAULT, CALLBACK)
* Key : category of parameter, used by the ini file parser ; corresponds to a section inside the config.ini file.
* Name : name of the parameter as found in the AT*CONFIG command, and as expected by ARDRONE_TOOL_CONFIGURATION_ADDEVENT
*/
#ifndef ARDRONE_CONFIG_KEY_IMM
#define ARDRONE_CONFIG_KEY_IMM(a,b,c,d,e,f,g,h)
#endif
#ifndef ARDRONE_CONFIG_KEY_STR
#define ARDRONE_CONFIG_KEY_STR(a,b,c,d,e,f,g,h)
#endif
#ifndef ARDRONE_CONFIG_KEY_REF
#define ARDRONE_CONFIG_KEY_REF(a,b,c,d,e,f,g,h)
#endif
/* If not all 10 arguments are used, defines a default macro with 8 arguments where the read/write rights (g)
* and the category (j) are discarded
*/
#ifndef ARDRONE_CONFIG_KEY_IMM_a10
#define ARDRONE_CONFIG_KEY_IMM_a10(a,b,c,d,e,f,g,h,i,j) ARDRONE_CONFIG_KEY_IMM(a,b,c,d,e,f/*,g*/,h,i/*,j*/)
#endif
#ifndef ARDRONE_CONFIG_KEY_STR_a10
#define ARDRONE_CONFIG_KEY_STR_a10(a,b,c,d,e,f,g,h,i,j) ARDRONE_CONFIG_KEY_STR(a,b,c,d,e,f,h,i)
#endif
#ifndef ARDRONE_CONFIG_KEY_REF_a10
#define ARDRONE_CONFIG_KEY_REF_a10(a,b,c,d,e,f,g,h,i,j) ARDRONE_CONFIG_KEY_REF(a,b,c,d,e,f,h,i)
#endif
ARDRONE_CONFIG_KEY_IMM_a10("general", num_version_config, INI_INT, int32_t, int32_t*, (K_READ|K_NOBIND), 0, CURRENT_NUM_VERSION_CONFIG, default_config_callback,CAT_COMMON)
ARDRONE_CONFIG_KEY_IMM_a10("general", num_version_mb, INI_INT, int32_t, int32_t*, (K_READ|K_NOBIND), 0, CARD_VERSION, default_config_callback,CAT_COMMON)
ARDRONE_CONFIG_KEY_STR_a10("general", num_version_soft, INI_STRING, string_t, char*, (K_READ|K_NOBIND), 0, CURRENT_NUM_VERSION_SOFT, default_config_callback,CAT_COMMON)
ARDRONE_CONFIG_KEY_STR_a10("general", drone_serial, INI_STRING, string_t, char*, (K_READ|K_NOBIND), 0, ARDRONE_EMPTY_STRING, default_config_callback,CAT_COMMON)
ARDRONE_CONFIG_KEY_STR_a10("general", soft_build_date, INI_STRING, string_t, char*, (K_READ|K_NOBIND), 0, CURRENT_BUILD_DATE, default_config_callback,CAT_COMMON)
ARDRONE_CONFIG_KEY_STR_a10("general", motor1_soft, INI_STRING, string_t, char*, (K_READ|K_NOBIND), 0, default_motor_version, default_config_callback,CAT_COMMON)
ARDRONE_CONFIG_KEY_STR_a10("general", motor1_hard, INI_STRING, string_t, char*, (K_READ|K_NOBIND), 0, default_motor_version, default_config_callback,CAT_COMMON)
ARDRONE_CONFIG_KEY_STR_a10("general", motor1_supplier, INI_STRING, string_t, char*, (K_READ|K_NOBIND), 0, default_motor_version, default_config_callback,CAT_COMMON)
ARDRONE_CONFIG_KEY_STR_a10("general", motor2_soft, INI_STRING, string_t, char*, (K_READ|K_NOBIND), 0, default_motor_version, default_config_callback,CAT_COMMON)
ARDRONE_CONFIG_KEY_STR_a10("general", motor2_hard, INI_STRING, string_t, char*, (K_READ|K_NOBIND), 0, default_motor_version, default_config_callback,CAT_COMMON)
ARDRONE_CONFIG_KEY_STR_a10("general", motor2_supplier, INI_STRING, string_t, char*, (K_READ|K_NOBIND), 0, default_motor_version, default_config_callback,CAT_COMMON)
ARDRONE_CONFIG_KEY_STR_a10("general", motor3_soft, INI_STRING, string_t, char*, (K_READ|K_NOBIND), 0, default_motor_version, default_config_callback,CAT_COMMON)
ARDRONE_CONFIG_KEY_STR_a10("general", motor3_hard, INI_STRING, string_t, char*, (K_READ|K_NOBIND), 0, default_motor_version, default_config_callback,CAT_COMMON)
ARDRONE_CONFIG_KEY_STR_a10("general", motor3_supplier, INI_STRING, string_t, char*, (K_READ|K_NOBIND), 0, default_motor_version, default_config_callback,CAT_COMMON)
ARDRONE_CONFIG_KEY_STR_a10("general", motor4_soft, INI_STRING, string_t, char*, (K_READ|K_NOBIND), 0, default_motor_version, default_config_callback,CAT_COMMON)
ARDRONE_CONFIG_KEY_STR_a10("general", motor4_hard, INI_STRING, string_t, char*, (K_READ|K_NOBIND), 0, default_motor_version, default_config_callback,CAT_COMMON)
ARDRONE_CONFIG_KEY_STR_a10("general", motor4_supplier, INI_STRING, string_t, char*, (K_READ|K_NOBIND), 0, default_motor_version, default_config_callback,CAT_COMMON)
ARDRONE_CONFIG_KEY_STR_a10("general", ardrone_name, INI_STRING, string_t, char*, (K_READ|K_WRITE ), 0, "My ARDrone", default_config_callback,CAT_COMMON)
ARDRONE_CONFIG_KEY_IMM_a10("general", flying_time, INI_INT, uint32_t, uint32_t*, (K_READ) , 0, 0, default_config_callback,CAT_COMMON)
ARDRONE_CONFIG_KEY_IMM_a10("general", navdata_demo, INI_BOOLEAN, bool_t, bool_t*, (K_READ|K_WRITE) , 0, FALSE, navdata_demo_config_callback,CAT_COMMON)
ARDRONE_CONFIG_KEY_IMM_a10("general", navdata_options, INI_INT, int32_t, int32_t*, (K_READ|K_WRITE|K_NOBIND|K_SHALLOW), (K_READ|K_WRITE|K_NOBIND|K_SHALLOW), default_navdata_options, navdata_options_config_callback, CAT_APPLI)
ARDRONE_CONFIG_KEY_IMM_a10("general", com_watchdog, INI_INT, int32_t, int32_t*, (K_READ|K_WRITE) , 0, COM_INPUT_LANDING_TIME, default_config_callback,CAT_COMMON)
ARDRONE_CONFIG_KEY_IMM_a10("general", video_enable, INI_BOOLEAN, bool_t, bool_t*, (K_READ|K_WRITE) , 0, TRUE, default_config_callback,CAT_COMMON)
ARDRONE_CONFIG_KEY_IMM_a10("general", vision_enable, INI_BOOLEAN, bool_t, bool_t*, (K_READ|K_WRITE) , 0, TRUE, default_config_callback,CAT_COMMON)
ARDRONE_CONFIG_KEY_IMM_a10("general", vbat_min, INI_INT, int32_t, int32_t*, (K_READ|K_NOBIND), 0, VBAT_POWERING_OFF, default_config_callback,CAT_COMMON)
ARDRONE_CONFIG_KEY_REF_a10("control", accs_offset, INI_VECTOR, vector31_t, vector31_t*, (K_READ|K_NOBIND), 0, default_accs_offset, default_config_callback,CAT_COMMON)
ARDRONE_CONFIG_KEY_REF_a10("control", accs_gains, INI_MATRIX, matrix33_t, matrix33_t*, (K_READ|K_NOBIND), 0, default_accs_gain, default_config_callback,CAT_COMMON)
ARDRONE_CONFIG_KEY_REF_a10("control", gyros_offset, INI_VECTOR, vector31_t, vector31_t*, (K_READ|K_NOBIND), 0, default_gyros_offset, default_config_callback,CAT_COMMON)
ARDRONE_CONFIG_KEY_REF_a10("control", gyros_gains, INI_VECTOR, vector31_t, vector31_t*, (K_READ|K_NOBIND), 0, default_gyros_gains, default_config_callback,CAT_COMMON)
ARDRONE_CONFIG_KEY_REF_a10("control", gyros110_offset, INI_VECTOR21, vector21_t, vector21_t*, (K_READ|K_NOBIND), 0, default_gyros110_offset, default_config_callback,CAT_COMMON)
ARDRONE_CONFIG_KEY_REF_a10("control", gyros110_gains, INI_VECTOR21, vector21_t, vector21_t*, (K_READ|K_NOBIND), 0, default_gyros110_gains, default_config_callback,CAT_COMMON)
ARDRONE_CONFIG_KEY_REF_a10("control", magneto_offset, INI_VECTOR, vector31_t, vector31_t*, (K_READ), 0, default_magneto_offset, default_config_callback,CAT_COMMON)
ARDRONE_CONFIG_KEY_REF_a10("control", magneto_radius, INI_FLOAT, float32_t, float32_t*, (K_READ), 0, default_magneto_radius, default_config_callback,CAT_COMMON)
ARDRONE_CONFIG_KEY_IMM_a10("control", gyro_offset_thr_x, INI_FLOAT, float32_t, float32_t*, (K_READ|K_NOBIND), 0, default_gyro_offset_thr_x, default_config_callback,CAT_COMMON)
ARDRONE_CONFIG_KEY_IMM_a10("control", gyro_offset_thr_y, INI_FLOAT, float32_t, float32_t*, (K_READ|K_NOBIND), 0, default_gyro_offset_thr_y, default_config_callback,CAT_COMMON)
ARDRONE_CONFIG_KEY_IMM_a10("control", gyro_offset_thr_z, INI_FLOAT, float32_t, float32_t*, (K_READ|K_NOBIND), 0, default_gyro_offset_thr_z, default_config_callback,CAT_COMMON)
ARDRONE_CONFIG_KEY_IMM_a10("control", pwm_ref_gyros, INI_INT, int32_t, int32_t*, (K_READ|K_NOBIND), 0, default_pwm_ref_gyro, default_config_callback,CAT_COMMON)
ARDRONE_CONFIG_KEY_IMM_a10("control", osctun_value, INI_INT, int32_t, int32_t*, (K_READ|K_NOBIND), 0, default_osctun_value, default_config_callback,CAT_COMMON)
ARDRONE_CONFIG_KEY_IMM_a10("control", osctun_test, INI_BOOLEAN, bool_t, bool_t*, (K_READ|K_NOBIND) , 0, FALSE, default_config_callback,CAT_COMMON)
ARDRONE_CONFIG_KEY_IMM_a10("control", control_level, INI_INT, int32_t, int32_t*, (K_READ|K_WRITE) , (K_READ|K_WRITE), 0, default_config_callback,CAT_APPLI)
ARDRONE_CONFIG_KEY_IMM_a10("control", euler_angle_max, INI_FLOAT, float32_t, float32_t*, (K_READ|K_WRITE) , (K_READ|K_WRITE), default_euler_angle_ref_max, control_changed_config_callback,CAT_USER)
ARDRONE_CONFIG_KEY_IMM_a10("control", altitude_max, INI_INT, int32_t, int32_t*, (K_READ|K_WRITE) , 0, default_altitude_max, default_config_callback,CAT_COMMON)
ARDRONE_CONFIG_KEY_IMM_a10("control", altitude_min, INI_INT, int32_t, int32_t*, (K_READ|K_WRITE) , 0, default_altitude_min, default_config_callback,CAT_COMMON)
ARDRONE_CONFIG_KEY_IMM_a10("control", control_iphone_tilt, INI_FLOAT, float32_t, float32_t*, (K_READ|K_WRITE) , (K_READ|K_WRITE), default_control_iphone_tilt, default_config_callback,CAT_USER)
ARDRONE_CONFIG_KEY_IMM_a10("control", control_vz_max, INI_FLOAT, float32_t, float32_t*, (K_READ|K_WRITE) , (K_READ|K_WRITE), default_control_vz_max, control_changed_config_callback,CAT_USER)
ARDRONE_CONFIG_KEY_IMM_a10("control", control_yaw, INI_FLOAT, float32_t, float32_t*, (K_READ|K_WRITE) , (K_READ|K_WRITE), default_control_yaw, control_changed_config_callback,CAT_USER)
ARDRONE_CONFIG_KEY_IMM_a10("control", outdoor, INI_BOOLEAN, bool_t, bool_t*, (K_READ|K_WRITE) , 0, FALSE, control_config_callback,CAT_COMMON)
ARDRONE_CONFIG_KEY_IMM_a10("control", flight_without_shell,INI_BOOLEAN, bool_t, bool_t*, (K_READ|K_WRITE) , 0, FALSE, default_config_callback,CAT_COMMON)
ARDRONE_CONFIG_KEY_IMM_a10("control", autonomous_flight, INI_BOOLEAN, bool_t, bool_t*, (K_READ|K_WRITE) , 0, FALSE, default_config_callback,CAT_COMMON)
ARDRONE_CONFIG_KEY_IMM_a10("control", manual_trim, INI_BOOLEAN, bool_t, bool_t*, (K_READ|K_WRITE), (K_READ|K_WRITE), FALSE, default_config_callback,CAT_USER)
ARDRONE_CONFIG_KEY_IMM_a10("control", indoor_euler_angle_max, INI_FLOAT, float32_t, float32_t*, (K_READ|K_WRITE), (K_READ|K_WRITE), default_euler_angle_ref_max,default_config_callback,CAT_USER)
ARDRONE_CONFIG_KEY_IMM_a10("control", indoor_control_vz_max, INI_FLOAT, float32_t, float32_t*, (K_READ|K_WRITE), (K_READ|K_WRITE), default_control_vz_max, default_config_callback,CAT_USER)
ARDRONE_CONFIG_KEY_IMM_a10("control", indoor_control_yaw, INI_FLOAT, float32_t, float32_t*, (K_READ|K_WRITE), (K_READ|K_WRITE), default_control_yaw, default_config_callback,CAT_USER)
ARDRONE_CONFIG_KEY_IMM_a10("control", outdoor_euler_angle_max, INI_FLOAT, float32_t, float32_t*, (K_READ|K_WRITE), (K_READ|K_WRITE), default_outdoor_euler_angle_ref_max, default_config_callback,CAT_USER)
ARDRONE_CONFIG_KEY_IMM_a10("control", outdoor_control_vz_max, INI_FLOAT, float32_t, float32_t*, (K_READ|K_WRITE), (K_READ|K_WRITE), default_outdoor_control_vz_max, default_config_callback,CAT_USER)
ARDRONE_CONFIG_KEY_IMM_a10("control", outdoor_control_yaw, INI_FLOAT, float32_t, float32_t*, (K_READ|K_WRITE), (K_READ|K_WRITE), default_outdoor_control_yaw, default_config_callback,CAT_USER)
ARDRONE_CONFIG_KEY_IMM_a10("control", flying_mode, INI_INT, int32_t, int32_t*, (K_READ|K_WRITE|K_NOBIND|K_SHALLOW), (K_READ|K_WRITE), 0, flying_mode_config_callback,CAT_SESSION)
ARDRONE_CONFIG_KEY_IMM_a10("control", hovering_range, INI_INT, int32_t, int32_t*, (K_READ|K_WRITE|K_NOBIND|K_SHALLOW), (K_READ|K_WRITE), 1000, default_config_callback, CAT_SESSION)
ARDRONE_CONFIG_KEY_STR_a10("control", flight_anim, INI_STRING, string_t, char*, (K_READ|K_WRITE|K_NOBIND|K_SHALLOW), 0, "0,0", flight_animation_selection_callback,CAT_COMMON)
ARDRONE_CONFIG_KEY_STR_a10("network", ssid_single_player, INI_STRING, string_t, char*, (K_READ|K_WRITE), 0, WIFI_NETWORK_NAME, default_config_callback, CAT_COMMON)
ARDRONE_CONFIG_KEY_STR_a10("network", ssid_multi_player, INI_STRING, string_t, char*, (K_READ|K_WRITE), 0, WIFI_NETWORK_NAME, default_config_callback, CAT_COMMON)
ARDRONE_CONFIG_KEY_IMM_a10("network", wifi_mode, INI_INT, int32_t, int32_t*, (K_READ|K_WRITE), 0, WIFI_MODE_INFRA, default_config_callback, CAT_COMMON)
ARDRONE_CONFIG_KEY_IMM_a10("network", wifi_rate, INI_INT, int32_t, int32_t*, (K_READ|K_WRITE|K_SHALLOW), 0, 0, wifi_rate_callback, CAT_COMMON)
ARDRONE_CONFIG_KEY_STR_a10("network", owner_mac, INI_STRING, string_t, char*, (K_READ|K_WRITE), 0, NULL_MAC, owner_mac_callback, CAT_COMMON)
ARDRONE_CONFIG_KEY_IMM_a10("pic", ultrasound_freq, INI_INT, int32_t, int32_t*, (K_READ|K_WRITE), 0, ADC_CMD_SELECT_ULTRASOUND_25Hz, ultrasound_freq_callback,CAT_COMMON)
ARDRONE_CONFIG_KEY_IMM_a10("pic", ultrasound_watchdog, INI_INT, int32_t, int32_t*, (K_READ|K_WRITE), 0, 3, default_config_callback,CAT_COMMON)
ARDRONE_CONFIG_KEY_IMM_a10("pic", pic_version , INI_INT, int32_t, int32_t*, (K_READ|K_NOBIND), 0, 0x00040030, default_config_callback,CAT_COMMON)
ARDRONE_CONFIG_KEY_IMM_a10("video", camif_fps, INI_INT, int32_t, int32_t*, (K_READ|K_NOBIND), 0, DEFAULT_FPS_CAMIF, default_config_callback,CAT_COMMON)
ARDRONE_CONFIG_KEY_IMM_a10("video", codec_fps, INI_INT, int32_t, int32_t*, (K_READ|K_NOBIND), (K_READ|K_WRITE), DEFAULT_FPS_CODEC, fps_config_callback,CAT_SESSION)
ARDRONE_CONFIG_KEY_IMM_a10("video", camif_buffers, INI_INT, int32_t, int32_t*, (K_READ|K_NOBIND), 0, CAMIF_NUM_BUFFERS, default_config_callback,CAT_COMMON)
ARDRONE_CONFIG_KEY_IMM_a10("video", num_trackers, INI_INT, int32_t, int32_t*, (K_READ|K_NOBIND), 0, 12, default_config_callback,CAT_COMMON)
ARDRONE_CONFIG_KEY_IMM_a10("video", video_codec, INI_INT, int32_t, int32_t*, (K_READ|K_SHALLOW), (K_READ|K_WRITE), DEFAULT_VIDEO_CODEC, codec_config_callback, CAT_SESSION)
ARDRONE_CONFIG_KEY_IMM_a10("video", video_slices, INI_INT, int32_t, int32_t*, (K_READ|K_SHALLOW), (K_READ|K_WRITE|K_NOBIND|K_SHALLOW), 0, slices_config_callback, CAT_SESSION)
ARDRONE_CONFIG_KEY_IMM_a10("video", video_live_socket, INI_INT, int32_t, int32_t*, (K_READ|K_SHALLOW), (K_READ|K_WRITE|K_NOBIND|K_SHALLOW), 0, video_live_socket_config_callback, CAT_SESSION)
ARDRONE_CONFIG_KEY_IMM_a10("video", video_storage_space, INI_INT, int32_t, int32_t*, (K_READ|K_SHALLOW), 0, 0, default_config_callback,CAT_COMMON)
ARDRONE_CONFIG_KEY_IMM_a10("video", bitrate, INI_INT, int32_t, int32_t*, (K_READ|K_SHALLOW), (K_READ|K_WRITE), 1000, bitrate_config_callback,CAT_APPLI)
ARDRONE_CONFIG_KEY_IMM_a10("video", max_bitrate, INI_INT, int32_t, int32_t*, (K_READ|K_SHALLOW), (K_READ|K_WRITE), 1000, bitrate_config_callback,CAT_SESSION)
ARDRONE_CONFIG_KEY_IMM_a10("video", bitrate_ctrl_mode, INI_INT, int32_t, int32_t*, (K_READ|K_SHALLOW), (K_READ|K_WRITE), 0, bitrate_config_callback,CAT_APPLI)
ARDRONE_CONFIG_KEY_IMM_a10("video", bitrate_storage, INI_INT, int32_t, int32_t*, (K_READ|K_SHALLOW), (K_READ|K_WRITE|K_NOBIND), 4000, bitrate_config_callback, CAT_APPLI)
ARDRONE_CONFIG_KEY_IMM_a10("video", video_channel, INI_INT, int32_t, int32_t*, (K_READ|K_WRITE|K_NOBIND|K_SHALLOW), (K_READ|K_WRITE), 0, video_channel_selection_callback,CAT_SESSION)
ARDRONE_CONFIG_KEY_IMM_a10("video", video_on_usb, INI_BOOLEAN, bool_t, bool_t*, (K_READ|K_WRITE), (K_READ|K_WRITE), TRUE, default_config_callback, CAT_COMMON)
ARDRONE_CONFIG_KEY_IMM_a10("video", video_file_index, INI_INT, int32_t, int32_t*, (K_READ|K_WRITE), (K_READ|K_WRITE), 1, default_config_callback, CAT_COMMON)
ARDRONE_CONFIG_KEY_STR_a10("leds", leds_anim, INI_STRING, string_t, char*, (K_READ|K_WRITE|K_NOBIND|K_SHALLOW), 0, "0,0,0", leds_animation_selection_callback,CAT_COMMON)
ARDRONE_CONFIG_KEY_IMM_a10("detect", enemy_colors, INI_INT, int32_t, int32_t*, (K_READ|K_WRITE), 0, default_enemy_colors, enemy_colors_selection_callback,CAT_COMMON)
ARDRONE_CONFIG_KEY_IMM_a10("detect", groundstripe_colors, INI_INT, int32_t, int32_t*, (K_READ|K_WRITE), (K_READ|K_WRITE), default_groundstripe_colors, groundstripe_colors_selection_callback,CAT_SESSION)
ARDRONE_CONFIG_KEY_IMM_a10("detect", enemy_without_shell, INI_INT, int32_t, int32_t*, (K_READ|K_WRITE), 0, 0, enemy_without_shell_selection_callback,CAT_COMMON)
ARDRONE_CONFIG_KEY_IMM_a10("detect", detect_type, INI_INT, int32_t, int32_t*, (K_READ|K_WRITE|K_NOBIND), (K_READ|K_WRITE|K_NOBIND), default_detect_type, detect_type_callback,CAT_SESSION)
ARDRONE_CONFIG_KEY_IMM_a10("detect", detections_select_h, INI_INT, int32_t, int32_t*, (K_READ|K_WRITE|K_NOBIND), (K_READ|K_WRITE), 0, detections_select_callback,CAT_SESSION)
ARDRONE_CONFIG_KEY_IMM_a10("detect", detections_select_v_hsync, INI_INT, int32_t, int32_t*, (K_READ|K_WRITE|K_NOBIND), (K_READ|K_WRITE), 0, detections_select_callback,CAT_SESSION)
ARDRONE_CONFIG_KEY_IMM_a10("detect", detections_select_v, INI_INT, int32_t, int32_t*, (K_READ|K_WRITE|K_NOBIND), (K_READ|K_WRITE), 0, detections_select_callback,CAT_SESSION)
ARDRONE_CONFIG_KEY_IMM_a10("syslog", output, INI_INT, int32_t, int32_t*, (K_READ|K_WRITE), 0, (UART_PRINT|WIFI_PRINT|FLASH_PRINT), default_config_callback,CAT_COMMON)
ARDRONE_CONFIG_KEY_IMM_a10("syslog", max_size, INI_INT, int32_t, int32_t*, (K_READ|K_WRITE), 0, (100*1024), default_config_callback,CAT_COMMON)
ARDRONE_CONFIG_KEY_IMM_a10("syslog", nb_files, INI_INT, int32_t, int32_t*, (K_READ|K_WRITE), 0, 5, default_config_callback,CAT_COMMON)
ARDRONE_CONFIG_KEY_STR_a10("userbox", userbox_cmd, INI_STRING, string_t, char*, (K_READ|K_WRITE|K_NOBIND|K_SHALLOW), (K_READ|K_WRITE|K_NOBIND|K_SHALLOW), "0", userbox_command_config_callback,CAT_SESSION)
ARDRONE_CONFIG_KEY_IMM_a10("gps", latitude, INI_DOUBLE, float64_t, float64_t*, (K_READ|K_WRITE|K_NOBIND), (K_READ|K_WRITE|K_NOBIND), GPS_INVALID_COORDINATES, default_config_callback,CAT_SESSION)
ARDRONE_CONFIG_KEY_IMM_a10("gps", longitude, INI_DOUBLE, float64_t, float64_t*, (K_READ|K_WRITE|K_NOBIND), (K_READ|K_WRITE|K_NOBIND), GPS_INVALID_COORDINATES, default_config_callback,CAT_SESSION)
ARDRONE_CONFIG_KEY_IMM_a10("gps", altitude, INI_DOUBLE, float64_t, float64_t*, (K_READ|K_WRITE|K_NOBIND), (K_READ|K_WRITE|K_NOBIND), 0.0, default_config_callback,CAT_SESSION)
/*- Multi configuration management -*/
ARDRONE_CONFIG_KEY_STR_a10("custom", application_id, INI_STRING, string_t, char*, (K_READ|K_WRITE|K_NOBIND|K_SHALLOW), (K_READ|K_WRITE), DEFAULT_CONFIG_ID, default_config_callback,CAT_SESSION)
ARDRONE_CONFIG_KEY_STR_a10("custom", application_desc, INI_STRING, string_t, char*, (K_READ|K_WRITE), (K_READ|K_WRITE), DEFAULT_APPLICATION_DESC, application_desc_callback,CAT_APPLI)
ARDRONE_CONFIG_KEY_STR_a10("custom", profile_id, INI_STRING, string_t, char*, (K_READ|K_WRITE|K_NOBIND|K_SHALLOW), (K_READ|K_WRITE), DEFAULT_CONFIG_ID, default_config_callback,CAT_SESSION)
ARDRONE_CONFIG_KEY_STR_a10("custom", profile_desc, INI_STRING, string_t, char*, (K_READ|K_WRITE), (K_READ|K_WRITE), DEFAULT_PROFILE_DESC, profile_desc_callback,CAT_USER)
ARDRONE_CONFIG_KEY_STR_a10("custom", session_id, INI_STRING, string_t, char*, (K_READ|K_WRITE|K_NOBIND|K_SHALLOW), (K_READ|K_WRITE), DEFAULT_CONFIG_ID, default_config_callback,CAT_SESSION)
ARDRONE_CONFIG_KEY_STR_a10("custom", session_desc, INI_STRING, string_t, char*, (K_READ|K_WRITE), (K_READ|K_WRITE), DEFAULT_SESSION_DESC, session_desc_callback,CAT_SESSION)
/* Prevents a further inclusion of config_keys from generating garbage code */
#undef ARDRONE_CONFIG_KEY_IMM_a10
#undef ARDRONE_CONFIG_KEY_REF_a10
#undef ARDRONE_CONFIG_KEY_STR_a10
#undef ARDRONE_CONFIG_KEY_IMM
#undef ARDRONE_CONFIG_KEY_REF
#undef ARDRONE_CONFIG_KEY_STR
ARDroneLib/Soft/Common/control_states.h
-222
Ver Arquivo
@@ -1,222 +0,0 @@
/**
* \file control_states.h
* \brief Control states declaration for control loop & ihm display
* \author Sylvain Gaeremynck <sylvain.gaeremynck@parrot.com>
* \version 1.0
*/
#ifndef _CONTROL_STATES_H_
#define _CONTROL_STATES_H_
#ifdef CTRL_STATES_STRING
typedef char ctrl_string_t[32];
#endif
// Macros to customize preprocessing
// If this CTRL_STATES_STRING is not defined, this file declare several enumeration
// If user defines CTRL_STATES_STRING, this file will define several array of strings.
// Each string being equivalent to an enumeration entry
#ifdef CTRL_STATES_STRING
#define CVAR(a) #a /* Using # causes the first argument after the # to be returned as a string in quotes */
#define CVARZ(a) #a
#else
#define CVARZ(a) a = 0
#define CVAR(a) a
#endif
/**
* \enum CONTROL_STATE
* \brief control loop thread states.
* \brief this defines major states
*/
#ifndef DO_NOT_INCLUDE_MAJOR_CTRL_STATES
#ifdef CTRL_STATES_STRING
static ctrl_string_t ctrl_states[] = {
#else
typedef enum {
#endif
CVARZ( CTRL_DEFAULT ),
CVAR( CTRL_INIT ),
CVAR( CTRL_LANDED ),
CVAR( CTRL_FLYING ),
CVAR( CTRL_HOVERING ),
CVAR( CTRL_TEST ),
CVAR( CTRL_TRANS_TAKEOFF ),
CVAR( CTRL_TRANS_GOTOFIX ),
CVAR( CTRL_TRANS_LANDING ),
CVAR( CTRL_TRANS_LOOPING ),
//CVAR( CTRL_TRANS_NO_VISION ),
#ifndef CTRL_STATES_STRING
CTRL_NUM_STATES
} CTRL_STATES;
#else
};
#endif
#endif
/**
* \enum FLYING_STATES
* \brief flying states.
* \brief this is one of the minor state
*/
#ifndef DO_NOT_INCLUDE_MINOR_CTRL_STATES
#ifdef CTRL_STATES_STRING
static ctrl_string_t flying_states[] = {
#else
typedef enum {
#endif
CVARZ( FLYING_OK ),
CVAR( FLYING_LOST_ALT ),
CVAR( FLYING_LOST_ALT_GO_DOWN ),
CVAR( FLYING_ALT_OUT_ZONE ),
CVAR( FLYING_COMBINED_YAW ),
CVAR( FLYING_BRAKE ),
CVAR( FLYING_NO_VISION ),
#ifndef CTRL_STATES_STRING
} FLYING_STATES;
#else
};
#endif
#endif
/**
* \enum HOVERING_STATES
* \brief flying states.
* \brief this is one of the minor state
*/
#ifndef DO_NOT_INCLUDE_MINOR_CTRL_STATES
#ifdef CTRL_STATES_STRING
static ctrl_string_t hovering_states[] = {
#else
typedef enum {
#endif
CVARZ( HOVERING_OK ),
CVAR( HOVERING_YAW ),
CVAR( HOVERING_YAW_LOST_ALT),
CVAR( HOVERING_YAW_LOST_ALT_GO_DOWN),
CVAR( HOVERING_ALT_OUT_ZONE),
CVAR( HOVERING_YAW_ALT_OUT_ZONE),
CVAR( HOVERING_LOST_ALT ),
CVAR( HOVERING_LOST_ALT_GO_DOWN ),
CVAR( HOVERING_LOST_COM ),
CVAR( LOST_COM_LOST_ALT ),
CVAR( LOST_COM_LOST_ALT_TOO_LONG ),
CVAR( LOST_COM_ALT_OK ),
CVAR( HOVERING_MAGNETO_CALIB ),
CVAR( HOVERING_DEMO )
#ifndef CTRL_STATES_STRING
} HOVERING_STATES;
#else
};
#endif
#endif
/**
* \enum TAKEOFF_TRANS_STATES
* \brief take off states.
* \brief this is one of the minor state
*/
#ifndef DO_NOT_INCLUDE_MINOR_CTRL_STATES
#ifdef CTRL_STATES_STRING
static ctrl_string_t takeoff_trans_states[] = {
#else
typedef enum {
#endif
CVARZ( TAKEOFF_GROUND ),
CVAR( TAKEOFF_AUTO ),
#ifndef CTRL_STATES_STRING
} TAKEOFF_TRANS_STATES;
#else
};
#endif
#endif
/**
* \enum GOTOFIX_TRANS_STATES
* \brief gotofix substates.
* \brief this is one of the minor state
*/
#ifndef DO_NOT_INCLUDE_MINOR_CTRL_STATES
#ifdef CTRL_STATES_STRING
static ctrl_string_t gotofix_trans_states[] = {
#else
typedef enum {
#endif
CVARZ( GOTOFIX_OK ),
CVAR( GOTOFIX_LOST_ALT ),
CVAR( GOTOFIX_YAW ),
#ifndef CTRL_STATES_STRING
} GOTOFIX_TRANS_STATES;
#else
};
#endif
#endif
/**
* \enum LANDING_TRANS_STATES
* \brief landing states.
* \brief this is one of the minor state
*/
#ifndef DO_NOT_INCLUDE_MINOR_CTRL_STATES
#ifdef CTRL_STATES_STRING
static ctrl_string_t landing_trans_states[] = {
#else
typedef enum {
#endif
CVARZ( LANDING_CLOSED_LOOP ),
CVAR( LANDING_OPEN_LOOP ),
CVAR( LANDING_OPEN_LOOP_FAST )
#ifndef CTRL_STATES_STRING
} LANDING_TRANS_STATES;
#else
};
#endif
#endif
/**
* \enum TAKEOFF_TRANS_STATES
* \brief take off states.
* \brief this is one of the minor state
*/
#ifndef DO_NOT_INCLUDE_MINOR_CTRL_STATES
#ifdef CTRL_STATES_STRING
static ctrl_string_t looping_trans_states[] = {
#else
typedef enum {
#endif
CVARZ( LOOPING_IMPULSION),
CVAR( LOOPING_OPEN_LOOP_CTRL ),
CVAR( LOOPING_PLANIF_CTRL )
#ifndef CTRL_STATES_STRING
} LOOPING_TRANS_STATES;
#else
};
#endif
#endif
#ifdef CTRL_STATES_STRING
#ifndef DO_NOT_INCLUDE_CTRL_STATES_LINK
/**
* control link tates
* \brief This array is used to link minor states & major state's strings
*/
static ctrl_string_t* control_states_link[] = {
NULL,
NULL,
NULL,
flying_states,
hovering_states,
NULL,
takeoff_trans_states,
gotofix_trans_states,
landing_trans_states,
looping_trans_states
};
#endif
#endif
#endif // _CONTROL_STATES_H_
ARDroneLib/Soft/Common/led_animation.h
-47
Ver Arquivo
@@ -1,47 +0,0 @@
/******************************************************************************
* COPYRIGHT PARROT 2010
******************************************************************************
* PARROT MODULES
*---------------------------------------------------------------------------*/
/**
* @file leds_animation.h
* @date 25th February 2010
* @brief Data types and functions to communicate with the drone.
* @author Pierre Eline
*
******************************************************************************/
// LED_ANIMATION(#name, {#nb_cycle,#nb_state,{{#led_pattern1,#delay1},{#led_pattern2,#delay2},{...,...}}})
// #name = name, example : BLINK
// #nb_cycle = number of times the animation is played (0 means infinite), example : 3
// #nb_state = number of led patterns in the animation, example : 2
// #led_pattern = led bitfield (G1 | R1 | G2 | R2 | G3 | R3 | G4 | R4), example : 0xAA all green led turned on
// #delay = delay in ms for the associated led pattern, example : 500
LED_ANIMATION(BLINK_GREEN_RED, { 0,2, { {0x55,500},{0xAA,500} } } )
LED_ANIMATION(BLINK_GREEN, { 0,2, { {0x00,500},{0xAA,500} } } )
LED_ANIMATION(BLINK_RED, { 0,2, { {0x55,500},{0x00,500} } } )
LED_ANIMATION(BLINK_ORANGE, { 0,2, { {0xFF,500},{0x00,500} } } )
LED_ANIMATION(SNAKE_GREEN_RED, { 0,8, { {0x90,200},{0x48,200},{0x24,200},{0x12,200},{0x9,200},{0x84,200},{0x42,200},{0x21,200}}})
LED_ANIMATION(FIRE, { 0,2, { {0x35,50},{0xC5,50} } } )
LED_ANIMATION(STANDARD, { 1,1, { {0xA5,100} } } )
LED_ANIMATION(RED, { 1,1, { {0x55,100} } } )
LED_ANIMATION(GREEN, { 1,1, { {0xAA,100} } } )
LED_ANIMATION(RED_SNAKE, { 0,4, { {0x40,500},{0x10,500},{0x04,500},{0x01,500}}})
LED_ANIMATION(BLANK, { 1,1, { {0x00,100} } } )
LED_ANIMATION(RIGHT_MISSILE, { 1,5, { {0x00,500},{0x04,300},{0x1C,100},{0x30,300},{0x00,500}}})
LED_ANIMATION(LEFT_MISSILE, { 1,5, { {0x00,500},{0x01,300},{0x43,100},{0xC0,300},{0x00,500}}})
LED_ANIMATION(DOUBLE_MISSILE, { 1,5, { {0x00,500},{0x05,300},{0x5F,100},{0xF0,300},{0x00,500}}})
LED_ANIMATION(FRONT_LEFT_GREEN_OTHERS_RED, { 1,1, { {0x95,100} } } )
LED_ANIMATION(FRONT_RIGHT_GREEN_OTHERS_RED, { 1,1, { {0x65,100} } } )
LED_ANIMATION(REAR_RIGHT_GREEN_OTHERS_RED, { 1,1, { {0x59,100} } } )
LED_ANIMATION(REAR_LEFT_GREEN_OTHERS_RED, { 1,1, { {0x56,100} } } )
LED_ANIMATION(LEFT_GREEN_RIGHT_RED, { 1,1, { {0x96,100} } } )
LED_ANIMATION(LEFT_RED_RIGHT_GREEN, { 1,1, { {0x69,100} } } )
LED_ANIMATION(BLINK_STANDARD, { 0,2, { {0x00,500},{0xA5,500} } } )
ARDroneLib/Soft/Common/navdata_common.h
-761
Ver Arquivo
@@ -1,761 +0,0 @@
/**
* \file navdata_common.h
* \brief Common navdata configuration
* \author Sylvain Gaeremynck <sylvain.gaeremynck@parrot.com>
*/
#ifndef _NAVDATA_COMMON_H_
#define _NAVDATA_COMMON_H_
/*------------------------------------------ NAVDATA STRUCTURES DECLARATIONS ---------------------------------------------------------------*/
#include <config.h>
#include <vision_common.h>
#include <VP_Os/vp_os_types.h>
#include <Maths/maths.h>
#include <Maths/matrices.h>
#if defined(_MSC_VER)
#define _ATTRIBUTE_PACKED_
/* Asks Visual C++ to pack structures from now on*/
#pragma pack(1)
#else
#define _ATTRIBUTE_PACKED_ __attribute__ ((packed))
#endif
// Define constants for gyrometers handling
typedef enum {
GYRO_X = 0,
GYRO_Y = 1,
GYRO_Z = 2,
NB_GYROS = 3
} def_gyro_t;
// Define constants for accelerometers handling
typedef enum {
ACC_X = 0,
ACC_Y = 1,
ACC_Z = 2,
NB_ACCS = 3
} def_acc_t;
/**
* \struct _velocities_t
* \brief Velocities in float32_t format
*/
typedef struct _velocities_t {
float32_t x;
float32_t y;
float32_t z;
} velocities_t;
// Default control loops gains TODO Put these values in flash memory
// To avoid divisions in embedded software, gains are defined as ratios where
// - the numerator is an integer
// - the denominator is an integer
/**
* \var CTRL_DEFAULT_NUM_PQ_KP
* \brief Numerator of default proportionnal gain for pitch (p) and roll (q) angular rate control loops
*/
/**
* \var CTRL_DEFAULT_NUM_EA_KP
* \brief Numerator of default proportionnal gain for Euler Angle control loops
*/
/**
* \var CTRL_DEFAULT_NUM_EA_KI
* \brief Numerator of default integral gain for Euler Angle control loops
*/
#define CTRL_DEFAULT_NUM_PQ_KP_NO_SHELL /*30000 26000 */ 40000
#define CTRL_DEFAULT_NUM_EA_KP_NO_SHELL /*7000 9000 7000 */ 8000
#define CTRL_DEFAULT_NUM_EA_KI_NO_SHELL /*4000 7000 6000 */ 7000
#define CTRL_DEFAULT_NUM_PQ_KP_SHELL /*30000 23000*/ 40000
#define CTRL_DEFAULT_NUM_EA_KP_SHELL /*9000 10000*/ 9000
#define CTRL_DEFAULT_NUM_EA_KI_SHELL /*5000 9000*/ 8000
/**
* \var CTRL_DEFAULT_NUM_H_R
* \brief Numerator of default proportionnal gain for yaw (r) angular rate control loop
*/
#define CTRL_DEFAULT_NUM_R_KP 200000
/**
* \var CTRL_DEFAULT_NUM_R_KI
* \brief Numerator of default integral gain for yaw control loops
*/
#define CTRL_DEFAULT_NUM_R_KI 3000
/**
* \var CTRL_DEFAULT_DEN_W
* \brief Denominator of default proportionnal gain of pitch (p) roll (q) and yaw (r) angular rate control loops
*/
#define CTRL_DEFAULT_DEN_W 1024.0 //2^10
/**
* \var CTRL_DEFAULT_DEN_EA
* \brief Denominator of default gains for Euler Angle control loops
*/
#define CTRL_DEFAULT_DEN_EA 1024.0 //2^10
/**
* \var CTRL_DEFAULT_NUM_ALT_KP
* \brief Numerator of default proportionnal gain for Altitude control loop
*/
#ifdef ALT_CONTROL
#define CTRL_DEFAULT_NUM_ALT_KP 210
#else
#define CTRL_DEFAULT_NUM_ALT_KP 3000
#endif
/**
* \var CTRL_DEFAULT_NUM_ALT_KI
* \brief Numerator of default integral gain for Altitude control loop
*/
#ifdef ALT_CONTROL
#define CTRL_DEFAULT_NUM_ALT_KI 100
#else
#define CTRL_DEFAULT_NUM_ALT_KI 400
#endif
/**
* \var CTRL_DEFAULT_NUM_ALT_KD
* \brief Numerator of default derivative gain for Altitude control loop
*/
#ifdef ALT_CONTROL
#define CTRL_DEFAULT_NUM_VZ_KP 100
#else
#define CTRL_DEFAULT_NUM_VZ_KP 200
#endif
/**
* \var CTRL_DEFAULT_NUM_ALT_TD
* \brief Numerator of default derivative time constant gain for Altitude control loop
*/
#define CTRL_DEFAULT_NUM_VZ_KI 100
/**
* \var CTRL_DEFAULT_DEN_ALT
* \brief Denominator of default gains for Altitude control loop
*/
#define CTRL_DEFAULT_DEN_ALT 1024.0
/**
* \var CTRL_DEFAULT_NUM_HOVER_KP
* \brief Numerator of default proportionnal gain for hovering control loop
*/
#define CTRL_DEFAULT_NUM_HOVER_KP_SHELL /*5000* 8000*/ 8000
#define CTRL_DEFAULT_NUM_HOVER_KP_NO_SHELL /*6000 12000 5000*/ 7000
/**
* \var CTRL_DEFAULT_NUM_HOVER_KP
* \brief Numerator of default proportionnal gain for hovering beacon control loop
*/
#define CTRL_DEFAULT_NUM_HOVER_B_KP_SHELL 1200
#define CTRL_DEFAULT_NUM_HOVER_B_KP_NO_SHELL 1200
/**
* \var CTRL_DEFAULT_NUM_HOVER_KI
* \brief Numerator of default integral gain for hovering control loop
*/
#define CTRL_DEFAULT_NUM_HOVER_KI_SHELL /*3000 10000*/ 8000
#define CTRL_DEFAULT_NUM_HOVER_KI_NO_SHELL /*3000 8000 5000*/ 6000
/**
* \var CTRL_DEFAULT_NUM_HOVER_KI
* \brief Numerator of default integral gain for hovering beacon control loop
*/
#define CTRL_DEFAULT_NUM_HOVER_B_KI_SHELL 500
#define CTRL_DEFAULT_NUM_HOVER_B_KI_NO_SHELL 500
/*
* \var CTRL_DEFAULT_DEN_HOVER
* \brief Numerator of default proportionnal gain for hovering control loop
*/
#define CTRL_DEFAULT_DEN_HOVER 32768.0 //2^15
#define CTRL_DEFAULT_NUM_HOVER_B_KP2_NO_SHELL 22937
#define CTRL_DEFAULT_NUM_HOVER_B_KI2_NO_SHELL 8192
#define CTRL_DEFAULT_NUM_HOVER_B_KD2_NO_SHELL 8000
#define CTRL_DEFAULT_NUM_HOVER_B_KP2_SHELL 22937
#define CTRL_DEFAULT_NUM_HOVER_B_KI2_SHELL 8192
#define CTRL_DEFAULT_NUM_HOVER_B_KD2_SHELL 8000
/* Timeout for mayday maneuvers*/
static const int32_t MAYDAY_TIMEOUT[ARDRONE_NB_ANIM_MAYDAY] = {
1000, // ARDRONE_ANIM_PHI_M30_DEG
1000, // ARDRONE_ANIM_PHI_30_DEG
1000, // ARDRONE_ANIM_THETA_M30_DEG
1000, // ARDRONE_ANIM_THETA_30_DEG
1000, // ARDRONE_ANIM_THETA_20DEG_YAW_200DEG
1000, // ARDRONE_ANIM_THETA_20DEG_YAW_M200DEG
5000, // ARDRONE_ANIM_TURNAROUND
5000, // ARDRONE_ANIM_TURNAROUND_GODOWN
2000, // ARDRONE_ANIM_YAW_SHAKE
5000, // ARDRONE_ANIM_YAW_DANCE
5000, // ARDRONE_ANIM_PHI_DANCE
5000, // ARDRONE_ANIM_THETA_DANCE
5000, // ARDRONE_ANIM_VZ_DANCE
5000, // ARDRONE_ANIM_WAVE
5000, // ARDRONE_ANIM_PHI_THETA_MIXED
5000, // ARDRONE_ANIM_DOUBLE_PHI_THETA_MIXED
15, // ARDRONE_ANIM_FLIP_AHEAD
15, // ARDRONE_ANIM_FLIP_BEHIND
15, // ARDRONE_ANIM_FLIP_LEFT
15, // ARDRONE_ANIM_FLIP_RIGHT
};
#define NAVDATA_SEQUENCE_DEFAULT 1
#define NAVDATA_HEADER 0x55667788
#define NAVDATA_MAX_SIZE 4096
#define NAVDATA_MAX_CUSTOM_TIME_SAVE 20
/* !!! Warning !!! - changing the value below would break compatibility with older applications
* DO NOT CHANGE THIS */
#define NB_NAVDATA_DETECTION_RESULTS 4
/**
* @brief Tags identifying navdata blocks in a Navdata UDP packet
* This tag is stored in the first two bytes of any navdata block (aka 'option').
*/
#define NAVDATA_OPTION_DEMO(STRUCTURE,NAME,TAG) TAG = 0,
#define NAVDATA_OPTION(STRUCTURE,NAME,TAG) TAG ,
#define NAVDATA_OPTION_CKS(STRUCTURE,NAME,TAG) NAVDATA_NUM_TAGS, TAG = 0xFFFF
typedef enum _navdata_tag_t {
#include <navdata_keys.h>
} navdata_tag_t;
#define NAVDATA_OPTION_MASK(option) ( 1 << (option) )
#define NAVDATA_OPTION_FULL_MASK ((1<<NAVDATA_NUM_TAGS)-1)
typedef struct _navdata_option_t {
uint16_t tag;
uint16_t size;
#if defined _MSC_VER || defined (__ARMCC_VERSION)
/* Do not use flexible arrays (C99 feature) with these compilers */
uint8_t data[1];
#else
uint8_t data[];
#endif
} navdata_option_t;
/**
* @brief Navdata structure sent over the network.
*/
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 */
bool_t vision_defined;
navdata_option_t options[1];
}_ATTRIBUTE_PACKED_ navdata_t;
/**
* All navdata options can be extended (new values AT THE END) except navdata_demo whose size must be constant across versions
* New navdata options may be added, but must not be sent in navdata_demo mode unless requested by navdata_options.
*/
/*----------------------------------------------------------------------------*/
/**
* @brief Minimal navigation data for all flights.
*/
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) */
float32_t theta; /*!< UAV's pitch in milli-degrees */
float32_t phi; /*!< UAV's roll in milli-degrees */
float32_t psi; /*!< UAV's yaw in milli-degrees */
int32_t altitude; /*!< UAV's altitude in centimeters */
float32_t vx; /*!< UAV's estimated linear velocity */
float32_t vy; /*!< UAV's estimated linear velocity */
float32_t 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
matrix33_t detection_camera_rot; /*!< Deprecated ! Don't use ! */
vector31_t 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
matrix33_t drone_camera_rot; /*!< Deprecated ! Don't use ! */
vector31_t drone_camera_trans; /*!< Deprecated ! Don't use ! */
}_ATTRIBUTE_PACKED_ navdata_demo_t;
/*----------------------------------------------------------------------------*/
/**
* @brief Last navdata option that *must* be included at the end of all navdata packets
* + 6 bytes
*/
typedef struct _navdata_cks_t {
uint16_t tag;
uint16_t size;
// Checksum for all navdatas (including options)
uint32_t cks;
}_ATTRIBUTE_PACKED_ navdata_cks_t;
/*----------------------------------------------------------------------------*/
/**
* @brief Timestamp
* + 6 bytes
*/
typedef struct _navdata_time_t {
uint16_t tag;
uint16_t size;
uint32_t time; /*!< 32 bit value where the 11 most significant bits represents the seconds, and the 21 least significant bits are the microseconds. */
}_ATTRIBUTE_PACKED_ navdata_time_t;
/*----------------------------------------------------------------------------*/
/**
* @brief Raw sensors measurements
*/
typedef struct _navdata_raw_measures_t {
uint16_t tag;
uint16_t size;
// +12 bytes
uint16_t raw_accs[NB_ACCS]; // filtered accelerometers
int16_t raw_gyros[NB_GYROS]; // filtered gyrometers
int16_t raw_gyros_110[2]; // gyrometers x/y 110 deg/s
uint32_t vbat_raw; // battery voltage raw (mV)
uint16_t us_debut_echo;
uint16_t us_fin_echo;
uint16_t us_association_echo;
uint16_t us_distance_echo;
uint16_t us_courbe_temps;
uint16_t us_courbe_valeur;
uint16_t us_courbe_ref;
uint16_t flag_echo_ini;
// TODO: uint16_t frame_number; // from ARDrone_Magneto
uint16_t nb_echo;
uint32_t sum_echo;
int32_t alt_temp_raw;
int16_t gradient;
}_ATTRIBUTE_PACKED_ navdata_raw_measures_t;
// split next struc into magneto_navdata_t and pressure_navdata_t
typedef struct _navdata_pressure_raw_t {
uint16_t tag;
uint16_t size;
int32_t up;
int16_t ut;
int32_t Temperature_meas;
int32_t Pression_meas;
}_ATTRIBUTE_PACKED_ navdata_pressure_raw_t;
typedef struct _navdata_magneto_t {
uint16_t tag;
uint16_t size;
int16_t mx;
int16_t my;
int16_t mz;
vector31_t magneto_raw; // magneto in the body frame, in mG
vector31_t magneto_rectified;
vector31_t magneto_offset;
float32_t heading_unwrapped;
float32_t heading_gyro_unwrapped;
float32_t heading_fusion_unwrapped;
char magneto_calibration_ok;
uint32_t magneto_state;
float32_t magneto_radius;
float32_t error_mean;
float32_t error_var;
}_ATTRIBUTE_PACKED_ navdata_magneto_t;
typedef struct _navdata_wind_speed_t {
uint16_t tag;
uint16_t size;
float32_t wind_speed; // estimated wind speed [m/s]
float32_t wind_angle; // estimated wind direction in North-East frame [rad] e.g. if wind_angle is pi/4, wind is from South-West to North-East
float32_t wind_compensation_theta;
float32_t wind_compensation_phi;
float32_t state_x1;
float32_t state_x2;
float32_t state_x3;
float32_t state_x4;
float32_t state_x5;
float32_t state_x6;
float32_t magneto_debug1;
float32_t magneto_debug2;
float32_t magneto_debug3;
}_ATTRIBUTE_PACKED_ navdata_wind_speed_t;
typedef struct _navdata_kalman_pressure_t{
uint16_t tag;
uint16_t size;
float32_t offset_pressure;
float32_t est_z;
float32_t est_zdot;
float32_t est_bias_PWM;
float32_t est_biais_pression;
float32_t offset_US;
float32_t prediction_US;
float32_t cov_alt;
float32_t cov_PWM;
float32_t cov_vitesse;
bool_t bool_effet_sol;
float32_t somme_inno;
bool_t flag_rejet_US;
float32_t u_multisinus;
float32_t gaz_altitude;
bool_t Flag_multisinus;
bool_t Flag_multisinus_debut;
}_ATTRIBUTE_PACKED_ navdata_kalman_pressure_t;
// TODO: depreciated struct ? remove it ?
typedef struct navdata_zimmu_3000_t {
uint16_t tag;
uint16_t size;
int32_t vzimmuLSB;
float32_t vzfind;
}_ATTRIBUTE_PACKED_ navdata_zimmu_3000_t;
typedef struct _navdata_phys_measures_t {
uint16_t tag;
uint16_t size;
float32_t accs_temp;
uint16_t gyro_temp;
float32_t phys_accs[NB_ACCS];
float32_t phys_gyros[NB_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;
typedef struct _navdata_gyros_offsets_t {
uint16_t tag;
uint16_t size;
float32_t offset_g[NB_GYROS];
}_ATTRIBUTE_PACKED_ navdata_gyros_offsets_t;
typedef struct _navdata_euler_angles_t {
uint16_t tag;
uint16_t size;
float32_t theta_a;
float32_t phi_a;
}_ATTRIBUTE_PACKED_ navdata_euler_angles_t;
typedef struct _navdata_references_t {
uint16_t tag;
uint16_t size;
int32_t ref_theta;
int32_t ref_phi;
int32_t ref_theta_I;
int32_t ref_phi_I;
int32_t ref_pitch;
int32_t ref_roll;
int32_t ref_yaw;
int32_t ref_psi;
float32_t vx_ref;
float32_t vy_ref;
float32_t theta_mod;
float32_t phi_mod;
float32_t k_v_x;
float32_t k_v_y;
uint32_t k_mode;
float32_t ui_time;
float32_t ui_theta;
float32_t ui_phi;
float32_t ui_psi;
float32_t ui_psi_accuracy;
int32_t ui_seq;
}_ATTRIBUTE_PACKED_ navdata_references_t;
typedef struct _navdata_trims_t {
uint16_t tag;
uint16_t size;
float32_t angular_rates_trim_r;
float32_t euler_angles_trim_theta;
float32_t euler_angles_trim_phi;
}_ATTRIBUTE_PACKED_ navdata_trims_t;
typedef struct _navdata_rc_references_t {
uint16_t tag;
uint16_t size;
int32_t rc_ref_pitch;
int32_t rc_ref_roll;
int32_t rc_ref_yaw;
int32_t rc_ref_gaz;
int32_t rc_ref_ag;
}_ATTRIBUTE_PACKED_ navdata_rc_references_t;
typedef struct _navdata_pwm_t {
uint16_t tag;
uint16_t size;
uint8_t motor1;
uint8_t motor2;
uint8_t motor3;
uint8_t motor4;
uint8_t sat_motor1;
uint8_t sat_motor2;
uint8_t sat_motor3;
uint8_t sat_motor4;
float32_t gaz_feed_forward;
float32_t gaz_altitude;
float32_t altitude_integral;
float32_t vz_ref;
int32_t u_pitch;
int32_t u_roll;
int32_t u_yaw;
float32_t yaw_u_I;
int32_t u_pitch_planif;
int32_t u_roll_planif;
int32_t u_yaw_planif;
float32_t u_gaz_planif;
uint16_t current_motor1;
uint16_t current_motor2;
uint16_t current_motor3;
uint16_t current_motor4;
//WARNING: new navdata (FC 26/07/2011)
float32_t altitude_prop;
float32_t altitude_der;
}_ATTRIBUTE_PACKED_ navdata_pwm_t;
typedef struct _navdata_altitude_t {
uint16_t tag;
uint16_t size;
int32_t altitude_vision;
float32_t altitude_vz;
int32_t altitude_ref;
int32_t altitude_raw;
float32_t obs_accZ;
float32_t obs_alt;
vector31_t obs_x;
uint32_t obs_state;
vector21_t est_vb;
uint32_t est_state ;
}_ATTRIBUTE_PACKED_ navdata_altitude_t;
typedef struct _navdata_vision_raw_t {
uint16_t tag;
uint16_t size;
float32_t vision_tx_raw;
float32_t vision_ty_raw;
float32_t vision_tz_raw;
}_ATTRIBUTE_PACKED_ navdata_vision_raw_t;
typedef struct _navdata_vision_t {
uint16_t tag;
uint16_t size;
uint32_t vision_state;
int32_t vision_misc;
float32_t vision_phi_trim;
float32_t vision_phi_ref_prop;
float32_t vision_theta_trim;
float32_t vision_theta_ref_prop;
int32_t new_raw_picture;
float32_t theta_capture;
float32_t phi_capture;
float32_t psi_capture;
int32_t altitude_capture;
uint32_t time_capture; // time in TSECDEC format (see config.h)
velocities_t body_v;
float32_t delta_phi;
float32_t delta_theta;
float32_t delta_psi;
uint32_t gold_defined;
uint32_t gold_reset;
float32_t gold_x;
float32_t gold_y;
}_ATTRIBUTE_PACKED_ navdata_vision_t;
typedef struct _navdata_vision_perf_t {
uint16_t tag;
uint16_t size;
// +44 bytes
float32_t time_szo;
float32_t time_corners;
float32_t time_compute;
float32_t time_tracking;
float32_t time_trans;
float32_t time_update;
float32_t time_custom[NAVDATA_MAX_CUSTOM_TIME_SAVE];
}_ATTRIBUTE_PACKED_ navdata_vision_perf_t;
typedef struct _navdata_trackers_send_t {
uint16_t tag;
uint16_t size;
int32_t locked[DEFAULT_NB_TRACKERS_WIDTH * DEFAULT_NB_TRACKERS_HEIGHT];
screen_point_t point[DEFAULT_NB_TRACKERS_WIDTH * DEFAULT_NB_TRACKERS_HEIGHT];
}_ATTRIBUTE_PACKED_ navdata_trackers_send_t;
typedef struct _navdata_vision_detect_t {
/* !! Change the function 'navdata_server_reset_vision_detect()' if this structure is modified !! */
uint16_t tag;
uint16_t size;
uint32_t nb_detected;
uint32_t type[NB_NAVDATA_DETECTION_RESULTS];
uint32_t xc[NB_NAVDATA_DETECTION_RESULTS];
uint32_t yc[NB_NAVDATA_DETECTION_RESULTS];
uint32_t width[NB_NAVDATA_DETECTION_RESULTS];
uint32_t height[NB_NAVDATA_DETECTION_RESULTS];
uint32_t dist[NB_NAVDATA_DETECTION_RESULTS];
float32_t orientation_angle[NB_NAVDATA_DETECTION_RESULTS];
matrix33_t rotation[NB_NAVDATA_DETECTION_RESULTS];
vector31_t translation[NB_NAVDATA_DETECTION_RESULTS];
uint32_t camera_source[NB_NAVDATA_DETECTION_RESULTS];
}_ATTRIBUTE_PACKED_ navdata_vision_detect_t;
typedef struct _navdata_vision_of_t {
uint16_t tag;
uint16_t size;
float32_t of_dx[5];
float32_t of_dy[5];
}_ATTRIBUTE_PACKED_ navdata_vision_of_t;
typedef struct _navdata_watchdog_t {
uint16_t tag;
uint16_t size;
// +4 bytes
int32_t watchdog;
}_ATTRIBUTE_PACKED_ navdata_watchdog_t;
typedef struct _navdata_adc_data_frame_t {
uint16_t tag;
uint16_t size;
uint32_t version;
uint8_t data_frame[32];
}_ATTRIBUTE_PACKED_ navdata_adc_data_frame_t;
typedef struct _navdata_video_stream_t {
uint16_t tag;
uint16_t size;
uint8_t quant; // quantizer reference used to encode frame [1:31]
uint32_t frame_size; // frame size (bytes)
uint32_t frame_number; // frame index
uint32_t atcmd_ref_seq; // atmcd ref sequence number
uint32_t atcmd_mean_ref_gap; // mean time between two consecutive atcmd_ref (ms)
float32_t atcmd_var_ref_gap;
uint32_t atcmd_ref_quality; // estimator of atcmd link quality
// drone2
uint32_t out_bitrate; // measured out throughput from the video tcp socket
uint32_t desired_bitrate; // last frame size generated by the video encoder
// misc temporary data
int32_t data1;
int32_t data2;
int32_t data3;
int32_t data4;
int32_t data5;
// queue usage
uint32_t tcp_queue_level;
uint32_t fifo_queue_level;
}_ATTRIBUTE_PACKED_ navdata_video_stream_t;
typedef enum
{
NAVDATA_HDVIDEO_STORAGE_FIFO_IS_FULL = (1<<0),
NAVDATA_HDVIDEO_USBKEY_IS_PRESENT = (1<<8),
NAVDATA_HDVIDEO_USBKEY_IS_RECORDING = (1<<9),
NAVDATA_HDVIDEO_USBKEY_IS_FULL = (1<<10)
}_navdata_hdvideo_states_t;
typedef struct _navdata_hdvideo_stream_t {
uint16_t tag;
uint16_t size;
uint32_t hdvideo_state;
uint32_t storage_fifo_nb_packets;
uint32_t storage_fifo_size;
uint32_t usbkey_size; /*! USB key in kbytes - 0 if no key present */
uint32_t usbkey_freespace; /*! USB key free space in kbytes - 0 if no key present */
uint32_t frame_number; /*! 'frame_number' PaVE field of the frame starting to be encoded for the HD stream */
uint32_t usbkey_remaining_time; /*! time in seconds */
}_ATTRIBUTE_PACKED_ navdata_hdvideo_stream_t;
typedef struct _navdata_games_t {
uint16_t tag;
uint16_t size;
uint32_t double_tap_counter;
uint32_t finish_line_counter;
}_ATTRIBUTE_PACKED_ navdata_games_t;
typedef struct _navdata_wifi_t {
uint16_t tag;
uint16_t size;
uint32_t link_quality;
}_ATTRIBUTE_PACKED_ navdata_wifi_t;
#if defined(_MSC_VER)
/* Go back to default packing policy */
#pragma pack()
#endif
#endif // _NAVDATA_COMMON_H_
ARDroneLib/Soft/Common/navdata_keys.h
-49
Ver Arquivo
@@ -1,49 +0,0 @@
#ifndef NAVDATA_OPTION_DEMO
#define NAVDATA_OPTION_DEMO(x,y,z)
#endif
#ifndef NAVDATA_OPTION
#define NAVDATA_OPTION(x,y,z)
#endif
#ifndef NAVDATA_OPTION_CKS
#define NAVDATA_OPTION_CKS(x,y,z)
#endif
NAVDATA_OPTION_DEMO ( navdata_demo_t , navdata_demo , NAVDATA_DEMO_TAG)
NAVDATA_OPTION( navdata_time_t, navdata_time , NAVDATA_TIME_TAG )
NAVDATA_OPTION( navdata_raw_measures_t, navdata_raw_measures , NAVDATA_RAW_MEASURES_TAG )
NAVDATA_OPTION( navdata_phys_measures_t, navdata_phys_measures , NAVDATA_PHYS_MEASURES_TAG )
NAVDATA_OPTION( navdata_gyros_offsets_t, navdata_gyros_offsets , NAVDATA_GYROS_OFFSETS_TAG )
NAVDATA_OPTION( navdata_euler_angles_t, navdata_euler_angles , NAVDATA_EULER_ANGLES_TAG )
NAVDATA_OPTION( navdata_references_t, navdata_references , NAVDATA_REFERENCES_TAG )
NAVDATA_OPTION( navdata_trims_t, navdata_trims , NAVDATA_TRIMS_TAG )
NAVDATA_OPTION( navdata_rc_references_t, navdata_rc_references , NAVDATA_RC_REFERENCES_TAG )
NAVDATA_OPTION( navdata_pwm_t, navdata_pwm , NAVDATA_PWM_TAG )
NAVDATA_OPTION( navdata_altitude_t, navdata_altitude , NAVDATA_ALTITUDE_TAG )
NAVDATA_OPTION( navdata_vision_raw_t, navdata_vision_raw , NAVDATA_VISION_RAW_TAG )
NAVDATA_OPTION( navdata_vision_of_t, navdata_vision_of , NAVDATA_VISION_OF_TAG )
NAVDATA_OPTION( navdata_vision_t, navdata_vision , NAVDATA_VISION_TAG )
NAVDATA_OPTION( navdata_vision_perf_t , navdata_vision_perf , NAVDATA_VISION_PERF_TAG )
NAVDATA_OPTION( navdata_trackers_send_t, navdata_trackers_send , NAVDATA_TRACKERS_SEND_TAG )
NAVDATA_OPTION( navdata_vision_detect_t, navdata_vision_detect , NAVDATA_VISION_DETECT_TAG )
NAVDATA_OPTION( navdata_watchdog_t , navdata_watchdog , NAVDATA_WATCHDOG_TAG )
NAVDATA_OPTION( navdata_adc_data_frame_t, navdata_adc_data_frame , NAVDATA_ADC_DATA_FRAME_TAG )
NAVDATA_OPTION( navdata_video_stream_t, navdata_video_stream , NAVDATA_VIDEO_STREAM_TAG )
NAVDATA_OPTION( navdata_games_t, navdata_games , NAVDATA_GAMES_TAG )
NAVDATA_OPTION( navdata_pressure_raw_t, navdata_pressure_raw , NAVDATA_PRESSURE_RAW_TAG )
NAVDATA_OPTION( navdata_magneto_t, navdata_magneto , NAVDATA_MAGNETO_TAG )
NAVDATA_OPTION( navdata_wind_speed_t, navdata_wind_speed , NAVDATA_WIND_TAG )
NAVDATA_OPTION( navdata_kalman_pressure_t,navdata_kalman_pressure , NAVDATA_KALMAN_PRESSURE_TAG )
NAVDATA_OPTION( navdata_hdvideo_stream_t ,navdata_hdvideo_stream , NAVDATA_HDVIDEO_STREAM_TAG )
NAVDATA_OPTION( navdata_wifi_t ,navdata_wifi , NAVDATA_WIFI_TAG )
// TODO: maybe navdata_zimmu_3000 coult be integrated into an existing navdata like navdata_adc_data_frame_t or ...
NAVDATA_OPTION( navdata_zimmu_3000_t, navdata_zimmu_3000 , NAVDATA_ZIMMU_3000_TAG )
NAVDATA_OPTION_CKS( navdata_cks_t, navdata_cks , NAVDATA_CKS_TAG )
#undef NAVDATA_OPTION_DEMO
#undef NAVDATA_OPTION
#undef NAVDATA_OPTION_CKS
ARDroneLib/Soft/Common/video_encapsulation.h
-85
Ver Arquivo
@@ -1,85 +0,0 @@
#ifndef _PARROT_VIDEO_ENCAPSULATION_H_
#define _PARROT_VIDEO_ENCAPSULATION_H_
#include <VP_Os/vp_os_types.h>
typedef enum {
CODEC_UNKNNOWN=0,
CODEC_VLIB,
CODEC_P264,
CODEC_MPEG4_VISUAL,
CODEC_MPEG4_AVC
}parrot_video_encapsulation_codecs_t;
typedef enum {
FRAME_TYPE_UNKNNOWN=0,
FRAME_TYPE_IDR_FRAME, /* headers followed by I-frame */
FRAME_TYPE_I_FRAME,
FRAME_TYPE_P_FRAME,
FRAME_TYPE_HEADERS
}parrot_video_encapsulation_frametypes_t;
typedef enum {
PAVE_CTRL_FRAME_DATA =0, /* The PaVE is followed by video data */
PAVE_CTRL_FRAME_ADVERTISEMENT =(1<<0), /* The PaVE is not followed by any data. Used to announce a frame which will be sent on the other socket later. */
PAVE_CTRL_LAST_FRAME_IN_STREAM =(1<<1), /* Announces the position of the last frame in the current stream */
}parrot_video_encapsulation_control_t;
/* Please keep this structure size a multiple of 8 or 16 */
/*
* Please keep 16-bit words aligned on 16-bit boundaries
* and keep 32-bit words aligned on 32-bit boundaries
*/
/*
*/
#define PAVE_CURRENT_VERSION (2)
typedef struct {
/*00*/ uint8_t signature[4];
/*04*/ uint8_t version;
/*05*/ uint8_t video_codec;
/*06*/ uint16_t header_size;
/*08*/ uint32_t payload_size; /* Amount of data following this PaVE */
/*12*/ uint16_t encoded_stream_width; /* ex: 640 */
/*14*/ uint16_t encoded_stream_height; /* ex: 368 */
/*16*/ uint16_t display_width; /* ex: 640 */
/*18*/ uint16_t display_height; /* ex: 360 */
/*20*/ uint32_t frame_number; /* frame position inside the current stream */
/*24*/ uint32_t timestamp; /* in milliseconds */
/*28*/ uint8_t total_chuncks; /* number of UDP packets containing the current decodable payload */
/*29*/ uint8_t chunck_index ; /* position of the packet - first chunk is #0 */
/*30*/ uint8_t frame_type; /* I-frame, P-frame */
/*31*/ uint8_t control; /* Special commands like end-of-stream or advertised frames */
/*32*/ uint32_t stream_byte_position_lw; /* Byte position of the current payload in the encoded stream - lower 32-bit word */
/*36*/ uint32_t stream_byte_position_uw; /* Byte position of the current payload in the encoded stream - upper 32-bit word */
/*40*/ uint16_t stream_id; /* This ID indentifies packets that should be recorded together */
/*42*/ uint8_t total_slices; /* number of slices composing the current frame */
/*43*/ uint8_t slice_index ; /* position of the current slice in the frame */
/*44*/ uint8_t header1_size; /* H.264 only : size of SPS inside payload - no SPS present if value is zero */
/*45*/ uint8_t header2_size; /* H.264 only : size of PPS inside payload - no PPS present if value is zero */
/*46*/ uint8_t reserved2[2]; /* Padding to align on 48 bytes */
/*48*/ uint32_t advertised_size; /* Size of frames announced as advertised frames */
/*52*/ uint8_t reserved3[12]; /* Padding to align on 64 bytes */
} __attribute__ ((packed)) parrot_video_encapsulation_t;
/* PaVE signature represented as a 32-bit integer in little and big endian */
#define PAVE_INT32LE_SIGNATURE (0x45566150)
#define PAVE_INT32BE_SIGNATURE (0x50615645)
#define PAVE_CHECK(x) ( (*((uint32_t*)(x)))==PAVE_INT32LE_SIGNATURE )
typedef enum
{
PAVE_STREAM_ID_SUFFIX_MP4_360p = 0,
PAVE_STREAM_ID_SUFFIX_H264_360p = 1,
PAVE_STREAM_ID_SUFFIX_H264_720p = 2
}parrot_video_encapsulation_stream_id_suffixes_t;
C_RESULT init_parrot_video_encapsulation_header(parrot_video_encapsulation_t * header);
int pave_is_same_frame(parrot_video_encapsulation_t * header1 , parrot_video_encapsulation_t * header2 );
void dumpPave (parrot_video_encapsulation_t *PaVE);
#endif
ARDroneLib/Soft/Common/vision_common.h
-42
Ver Arquivo
@@ -1,42 +0,0 @@
#ifndef _VISION_COMMON_H_
#define _VISION_COMMON_H_
// NUMBER OF TRACKERS FOR EACH TRACKING
#define NB_CORNER_TRACKERS_WIDTH 5 /* number of trackers in width of current picture */
#define NB_CORNER_TRACKERS_HEIGHT 4 /* number of trackers in height of current picture */
#define DEFAULT_NB_TRACKERS_WIDTH (NB_CORNER_TRACKERS_WIDTH+1)// + NB_BLOCK_TRACKERS_WIDTH)
#define DEFAULT_NB_TRACKERS_HEIGHT (NB_CORNER_TRACKERS_HEIGHT+1)// + NB_BLOCK_TRACKERS_HEIGHT)
#define YBUF_OFFSET 0
// use by ihm/ihm_vision.c
#define DEFAULT_CS 1
#define DEFAULT_NB_PAIRS 1
#define DEFAULT_LOSS_PER 1
#define DEFAULT_SCALE 1
#define DEFAULT_TRANSLATION_MAX 1
#define DEFAULT_MAX_PAIR_DIST 1
#define DEFAULT_NOISE 1
typedef enum _CAMIF_CAMERA_ENUM_
{
CAMIF_CAMERA_LB=0,
CAMIF_CAMERA_CRESYN,
CAMIF_CAMERA_VS6524,
CAMIF_CAMERA_OV7710,
CAMIF_CAMERA_OV7720,
CAMIF_CAMERA_OVTRULY,
CAMIF_CAMERA_OVTRULY_UPSIDE_DOWN_ONE_BLOCKLINE_LESS,
CAMIF_CAMERA_FILE,
CAMIF_CAMERA_OV7670,
CAMIF_CAMERA_OV9740,
CAMIF_CAMERA_SOC1040,
CAMIF_CAMERA_UB
}
CAMIF_CAMERA;
#endif //_VISION_COMMON_H
ARDroneLib/Soft/Lib/Build/Makefile
-186
Ver Arquivo
@@ -1,186 +0,0 @@
NO_COLOR=\033[0m
OK_COLOR=\033[32;01m
ifdef ARDRONE_CUSTOM_CONFIG
ifdef ARDRONE_BUILD_CONFIG
include $(ARDRONE_CUSTOM_CONFIG)
include $(ARDRONE_BUILD_CONFIG)
else
include ../../Build/custom.makefile
include ../../Build/config.makefile
endif
else
include ../../Build/custom.makefile
include ../../Build/config.makefile
endif
LIB_ID=ardrone
ifeq ($(USE_ARDRONE_TOOL),no)
LIB_ID:=$(LIB_ID)_notool
endif
GENERIC_TARGET_LIBRARY:=lib$(TARGET)$(LIB_ID).a
GENERIC_CFLAGS+=-DAT_MESSAGES_HEADER="\"$(COMMON_DIR)/at_msgs.h\""
GENERIC_CFLAGS+=-DUSE_NEW_ATCODEC
ifeq ("$(USE_CHECK_WIFI_CONFIG)","yes")
GENERIC_CFLAGS+=-DCHECK_WIFI_CONFIG
endif
SDK_FLAGS+="USE_APP=no"
SDK_FLAGS+="USE_LIB=yes"
SDK_FLAGS+="LIB_ID=ardrone_lib"
MATHS_DIR=Maths
CONTROL_DIR=Control
ARDRONE_TOOL_DIR=ardrone_tool
UTILS_DIR=utils
INIPARSER_DIR=iniparser3.0b/src
LIBCALIBRATION_DIR=libCalibration
GENERIC_LIBRARY_SOURCE_FILES+= \
$(INIPARSER_DIR)/iniparser.c \
$(INIPARSER_DIR)/dictionary.c \
$(MATHS_DIR)/filter.c \
$(MATHS_DIR)/maths.c \
$(MATHS_DIR)/matrices.c \
$(MATHS_DIR)/matrix3d.c \
$(MATHS_DIR)/quaternions.c \
$(MATHS_DIR)/time.c \
$(MATHS_DIR)/vision_math.c \
$(ARDRONE_TOOL_DIR)/Navdata/navdata.c \
$(ARDRONE_TOOL_DIR)/config_keys.c \
$(ARDRONE_TOOL_DIR)/ardrone_version.c \
$(UTILS_DIR)/ardrone_time.c \
$(UTILS_DIR)/ardrone_date.c \
$(UTILS_DIR)/ardrone_crc_32.c \
$(UTILS_DIR)/ardrone_gen_ids.c \
$(UTILS_DIR)/ardrone_ftp.c \
$(ARDRONE_TOOL_DIR)/Video/video_encapsulation.c
# The following files are needed to record video on the drone's flash memory
GENERIC_LIBRARY_SOURCE_FILES+= \
$(UTILS_DIR)/ardrone_video_atoms.c \
$(UTILS_DIR)/ardrone_video_encapsuler.c \
$(ARDRONE_TOOL_DIR)/Video/video_stage_encoded_recorder.c
ifneq ($(ELINUX_TARGET),yes)
#Controller only files
GENERIC_LIBRARY_SOURCE_FILES+= \
$(ARDRONE_TOOL_DIR)/Video/video_stage_tcp.c \
$(ARDRONE_TOOL_DIR)/Video/video_stage_merge_slices.c \
$(ARDRONE_TOOL_DIR)/Video/video_stage_latency_estimation.c \
$(UTILS_DIR)/ardrone_ftp.c \
$(ARDRONE_TOOL_DIR)/Video/video_stage_ffmpeg_recorder.c \
$(ARDRONE_TOOL_DIR)/Video/video_stage_ffmpeg_decoder.c \
$(ARDRONE_TOOL_DIR)/Video/video_stage_ittiam_decoder.c \
$(ARDRONE_TOOL_DIR)/Video/video_stage_decoder.c \
$(ARDRONE_TOOL_DIR)/Video/video_recorder_pipeline.c \
$(ARDRONE_TOOL_DIR)/Video/video_stage.c
ifeq ($(USE_ANDROID),yes)
GENERIC_LIBRARY_SOURCE_FILES+= \
$(UTILS_DIR)/AR_Ftw.c
endif
endif
ifneq ($(CONTROL_DLL),yes)
ifeq ($(USE_ARDRONE_TOOL),yes)
ifeq ($(PC_USE_POLARIS),yes)
GENERIC_CFLAGS+=-DPC_USE_POLARIS
endif
GENERIC_LIBRARY_SOURCE_FILES+= \
$(ARDRONE_TOOL_DIR)/AT/ardrone_at_mutex.c
GENERIC_LIBRARY_SOURCE_FILES+= \
$(ARDRONE_TOOL_DIR)/Academy/academy.c \
$(ARDRONE_TOOL_DIR)/Academy/academy_download.c \
$(ARDRONE_TOOL_DIR)/Academy/academy_upload.c \
$(ARDRONE_TOOL_DIR)/Academy/academy_stage_recorder.c \
$(ARDRONE_TOOL_DIR)/Control/ardrone_control_configuration.c \
$(ARDRONE_TOOL_DIR)/Control/ardrone_control_ack.c \
$(ARDRONE_TOOL_DIR)/Navdata/ardrone_navdata_file.c \
$(ARDRONE_TOOL_DIR)/Navdata/ardrone_general_navdata.c \
$(ARDRONE_TOOL_DIR)/Navdata/ardrone_academy_navdata.c \
$(ARDRONE_TOOL_DIR)/UI/ardrone_input.c \
$(ARDRONE_TOOL_DIR)/ardrone_api.c \
$(ARDRONE_TOOL_DIR)/ardrone_tool_configuration.c \
$(ARDRONE_TOOL_DIR)/ardrone_tool.c \
$(ARDRONE_TOOL_DIR)/Com/config_wifi.c
ifneq ($(USE_MINGW32),yes)
GENERIC_LIBRARY_SOURCE_FILES+= \
$(ARDRONE_TOOL_DIR)/Video/video_com_stage.c \
$(ARDRONE_TOOL_DIR)/Video/video_navdata_handler.c \
$(ARDRONE_TOOL_DIR)/Control/ardrone_control.c \
$(ARDRONE_TOOL_DIR)/Control/ardrone_navdata_control.c \
$(ARDRONE_TOOL_DIR)/Navdata/ardrone_navdata_client.c
ifeq ($(USE_IPHONE),no)
GENERIC_LIBRARY_SOURCE_FILES+= \
$(ARDRONE_TOOL_DIR)/Com/config_serial.c
endif
ifeq ($(USE_ARDRONE_VICON),yes)
GENERIC_CFLAGS+=-DUSE_ARDRONE_VICON
endif
endif # USE_MINGW32
endif # USE_ARDRONE_TOOL
ifneq ($(USE_MINGW32),yes)
GENERIC_LIBRARY_SOURCE_FILES+= \
$(ARDRONE_TOOL_DIR)/Video/video_stage_recorder.c
endif
ifeq ($(USE_ELINUX),yes)
HARDWARE_ARDRONE:=$(SRC_PATH)/Soft/Toy/Sources/$(CARD_HARDWARE_DIR)
ifeq ($(USE_MINGW32),yes)
HARDWARE_ARDRONE_CALIBRATION:=c:/$(CARD_HARDWARE_DIR)
else
HARDWARE_ARDRONE_CALIBRATION:=$(HARDWARE_ARDRONE)
endif
GENERIC_CFLAGS+=-DHARDWARE_ARDRONE=\"$(HARDWARE_ARDRONE)\" \
-DHARDWARE_ARDRONE_CALIBRATION=\"$(HARDWARE_ARDRONE_CALIBRATION)\"
endif # USE_ELINUX
endif # neq CONTROL_DLL
BASE_RELATIVE_PATH_FROM_SDK=../../
GENERIC_INCLUDES+= \
-I$(BASE_RELATIVE_PATH_FROM_SDK)/Soft/Common \
-I$(BASE_RELATIVE_PATH_FROM_SDK) \
-I$(BASE_RELATIVE_PATH_FROM_SDK)/Soft/Lib
export GENERIC_INCLUDES
export GENERIC_LIBRARY_SOURCE_DIR=$(BASE_RELATIVE_PATH_FROM_SDK)/Soft/Lib
export GENERIC_BINARIES_SOURCE_DIR=$(BASE_RELATIVE_PATH_FROM_SDK)/Soft/Lib
export GENERIC_CFLAGS
export GENERIC_LIBS
export GENERIC_LIBRARY_SOURCE_FILES
export GENERIC_TARGET_LIBRARY
export GENERIC_BINARIES_COMMON_SOURCE_FILES=
export GENERIC_BINARIES_SOURCE_ENTRYPOINTS=
all $(MAKECMDGOALS):
@if [ '$(MAKECMDGOALS)' != 'clean' ]; then echo "$(OK_COLOR)Building ARDroneTool/Lib$(NO_COLOR)"; fi
@$(MAKE) -C ../../../VP_SDK/Build $(TMP_SDK_FLAGS) $(SDK_FLAGS) $(MAKECMDGOALS)
makefileverbose:
@echo "Dump compilation flags:"
@echo "GENERIC_CFLAGS : $(GENERIC_CFLAGS)"
@echo "GENERIC_LIBS : $(GENERIC_LIBS)"
@echo "GENERIC_LIB_PATHS : $(GENERIC_LIB_PATHS)"
@echo "GENERIC_INCLUDES : $(GENERIC_INCLUDES)"
@echo "GENERIC_BINARIES_SOURCE_DIR : $(GENERIC_BINARIES_SOURCE_DIR)"
@echo "GENERIC_BINARIES_COMMON_SOURCE_FILES : $(GENERIC_BINARIES_COMMON_SOURCE_FILES)"
@echo "GENERIC_TARGET_BINARIES_PREFIX : $(GENERIC_TARGET_BINARIES_PREFIX)"
@echo "GENERIC_TARGET_BINARIES_DIR : $(GENERIC_TARGET_BINARIES_DIR)"
@echo "GENERIC_BINARIES_SOURCE_ENTRYPOINTS : $(GENERIC_BINARIES_SOURCE_ENTRYPOINTS)"
@echo "TMP_SDK_FLAGS: $(TMP_SDK_FLAGS)"
@echo "SDK_FLAGS: $(SDK_FLAGS)"
@echo "Make cmd goals: $(MAKECMDGOALS)"
ARDroneLib/Soft/Lib/Maths/filter.c
-252
Ver Arquivo
@@ -1,252 +0,0 @@
/**
* \file filter.c
* \brief 1st and 2nd order filter implementation
* \author Jean Baptiste Lanfrey <jean-baptiste.lanfrey@parrot.com>
* \version 1.0
*/
#define _XOPEN_SOURCE 600
#include <Maths/maths.h>
#include <Maths/filter.h>
#include <math.h>
void filter_init(uint32_t n, float32_t *old_input, float32_t initial_input, float32_t *old_output, float32_t initial_output)
{
uint32_t ii;
for (ii=0; ii<n ; ii++)
old_input[ii] = initial_input;
for (ii=0; ii<n; ii++)
old_output[ii] = initial_output;
}
float filter(uint32_t n, const float32_t *b, const float32_t *a, float32_t input, float32_t *old_input, float32_t *old_output)
{
uint32_t ii;
float32_t inno, past, output;
// innovation computation
inno = b[0] * input;
for (ii=1; ii<n+1; ii++)
inno += b[ii] * old_input[ii-1];
// past computation
past = 0.0;
for (ii=1; ii<n+1; ii++)
past -= a[ii] * old_output[ii-1];
// output = (inno + past) / a[0];
// We assume a[0] is always equal to 1
output = (inno + past);
// inputs and outputs shift
for (ii=n-1; ii>0; ii--)
old_input[ii] = old_input[ii-1];
old_input[0] = input;
for (ii=n-1; ii>0; ii--)
old_output[ii] = old_output[ii-1];
old_output[0] = output;
return output;
}
// Filtre du type Kd.p / (1+Td.p)
float32_t deriv(deriv_param_t *param, float32_t input)
{
static float32_t exp_1 = 0.3678794411714423412f; // expf(-1.0f);
float32_t exp;
float32_t td;
float32_t out;
if( f_is_zero( param->td ) )
{
// prevent from dividing by 0.
td = param->te;
exp = exp_1;
}
else
{
td = param->td;
// exp = expf(-param->te/td);
exp = exp_taylor(-param->te/td);
}
param->internal_state = exp * param->internal_state + td * (1 - exp) * param->old_input;
out = ( param->kd / td ) * ( input - param->internal_state / td );
param->old_input = input;
return out;
}
void delay_init(uint32_t m, float32_t *old_input, float32_t initial_input)
{
uint32_t ii;
for (ii=0; ii<m ; ii++)
old_input[ii] = initial_input;
}
float32_t delay(uint32_t m, float32_t input, float32_t *old_input)
{
uint32_t ii;
float32_t output;
output = old_input[m-1];
// inputs and shift
for (ii=m-1; ii>0; ii--)
old_input[ii] = old_input[ii-1];
old_input[0] = input;
return output;
}
float32_t rate_limiter(float32_t input, float32_t old_output, float32_t rate_max)
{
float32_t output;
float_or_int_t FOI_rate,FOI_rate_max;
FOI_rate.f = input - old_output;
FOI_rate_max.f = rate_max;
if( f_abs( FOI_rate ) > FOI_rate_max.f )
output = old_output + f_set_clamp( FOI_rate, FOI_rate_max );
else
output = input;
return output;
}
int32_t digitalsmooth(int32_t rawIn, int32_t *sensSmoothArray)
{
int32_t j, k, temp, top, bottom;
long total;
static int32_t i;
static int32_t sorted[filterSamples];
bool_t done;
i = (i + 1) % filterSamples;
sensSmoothArray[i] = rawIn;
for (j=0; j<filterSamples; j++)
{
sorted[j] = sensSmoothArray[j];
}
done = 0;
while(done != 1)
{
done = 1;
for (j = 0; j < (filterSamples - 1); j++)
{
if (sorted[j] > sorted[j + 1])
{
temp = sorted[j + 1];
sorted [j+1] = sorted[j] ;
sorted [j] = temp;
done = 0;
}
}
}
bottom = max(((filterSamples * 15) / 100), 1);
top = min((((filterSamples * 85) / 100) + 1 ), (filterSamples - 1));
k = 0;
total = 0;
for ( j = bottom; j< top; j++){
total += sorted[j];
k++;
}
return total / k;
}
//unwrapToPi
// takes pointer to the value of wrapped angle, pointer to the value of unwrapped angle, and the variation to add to both angles
void unwrapToPi(float32_t* former_wrapped_angle, float32_t* former_unwrapped_angle, float32_t variation)
{
if(fabsf(variation)<PI)
{
*former_unwrapped_angle += variation;
}
else
{
if(variation>=0)
{
*former_unwrapped_angle += variation - 2.0*PI;
}
else
{
*former_unwrapped_angle += variation + 2.0*PI;
}
}
wrapToPi(former_unwrapped_angle, former_wrapped_angle);
}
void wrapToPi(float32_t* input, float32_t* output)
{
*output = fmodf(*input + PI, 2*PI) - PI;
if((-PI)>(*input))
{
*output += 2*PI;
}
}
void filter64_init(uint32_t n, float64_t *old_input, float64_t initial_input, float64_t *old_output, float64_t initial_output)
{
uint32_t ii;
for (ii=0; ii<n ; ii++)
old_input[ii] = initial_input;
for (ii=0; ii<n; ii++)
old_output[ii] = initial_output;
}
float64_t filter64(uint32_t n, const float64_t *b, const float64_t *a, float64_t input, float64_t *old_input, float64_t *old_output)
{
uint32_t ii;
float64_t inno, past, output;
// innovation computation
inno = b[0] * input;
for (ii=1; ii<n+1; ii++)
inno += b[ii] * old_input[ii-1];
// past computation
past = 0.0;
for (ii=1; ii<n+1; ii++)
past -= a[ii] * old_output[ii-1];
// output = (inno + past) / a[0];
// We assume a[0] is always equal to 1
output = (inno + past);
// inputs and outputs shift
for (ii=n-1; ii>0; ii--)
old_input[ii] = old_input[ii-1];
old_input[0] = input;
for (ii=n-1; ii>0; ii--)
old_output[ii] = old_output[ii-1];
old_output[0] = output;
return output;
}
ARDroneLib/Soft/Lib/Maths/filter.h
-211
Ver Arquivo
@@ -1,211 +0,0 @@
/**
* \file filter.h
* \brief 1st and 2nd order filter implementation
* \author Jean Baptiste Lanfrey <jean-baptiste.lanfrey@parrot.com>
* \version 1.0
*/
#ifndef _FILTER_H_
#define _FILTER_H_
#include <VP_Os/vp_os_types.h>
#define NB_FIRST_ORDER 1
#define NB_SECOND_ORDER 2
#define NB_THIRD_ORDER 3
#define NB_FOURTH_ORDER 4
#define NB_SIXTH_ORDER 6
#define NB_SEVENTH_ORDER 7
#define DEFAULT_DELAY_STEP_TIME_DELAY TWENTY_STEP_TIME_DELAY
#define FORTY_STEP_TIME_DELAY 40
#define THIRTY_TWO_STEP_TIME_DELAY 32
#define TWENTY_STEP_TIME_DELAY 20
#define filterSamples 511
///////////////////////////////////////////////
// STRUCTURES
/**
* \struct _first_order_filter_
* \brief First order filter states using Matlab notation.
*/
typedef struct {
float32_t old_outputs[NB_FIRST_ORDER]; //< filter output history
float32_t old_inputs [NB_FIRST_ORDER]; //< filter input history
} first_order_filter_t;
/**
* \struct _second_order_filter_
* \brief Second order filter states using Matlab notation.
*/
typedef struct {
float32_t old_outputs[NB_SECOND_ORDER]; //< filter output history
float32_t old_inputs [NB_SECOND_ORDER]; //< filter input history
} second_order_filter_t;
/**
* \struct _second_order_filter_64
* \brief Second order filter states using Matlab notation.
*/
typedef struct {
float64_t old_outputs[NB_SECOND_ORDER]; //< filter output history
float64_t old_inputs [NB_SECOND_ORDER]; //< filter input history
} second_order_filter_64_t;
/**
* \struct _third_order_filter_
* \brief Third order filter states using Matlab notation.
*/
typedef struct {
float32_t old_outputs[NB_THIRD_ORDER]; //< filter output history
float32_t old_inputs [NB_THIRD_ORDER]; //< filter input history
} third_order_filter_t;
/**
* \struct _fourth_order_filter_
* \brief Fourth order filter states using Matlab notation.
*/
typedef struct {
float32_t old_outputs[NB_FOURTH_ORDER]; //< filter output history
float32_t old_inputs [NB_FOURTH_ORDER]; //< filter input history
} fourth_order_filter_t;
typedef struct {
float32_t old_outputs[NB_SIXTH_ORDER]; //< filter output history
float32_t old_inputs [NB_SIXTH_ORDER]; //< filter input history
} sixth_order_filter_t;
typedef struct {
float32_t old_outputs[NB_SEVENTH_ORDER]; //< filter output history
float32_t old_inputs [NB_SEVENTH_ORDER]; //< filter input history
} seventh_order_filter_t;
typedef struct _deriv_param_t {
float32_t kd;
float32_t td;
float32_t te;
float32_t internal_state;
float32_t old_input;
} deriv_param_t;
/**
* \struct _delay_
* \brief m sampling step time delay.
*/
typedef struct {
float32_t old_inputs[FORTY_STEP_TIME_DELAY]; //< input history
} delay_t;
///////////////////////////////////////////////
// FUNCTIONS
/**
* \fn Digital filter initialization.
* \brief This function is used to initialize previous values in digital filter.
* \param Filter order.
* \param address of previous inputs list.
* \param initial input value.
* \param address of previous outputs list.
* \param initial output value.
* \return void.
*
* \section History
*
* \par date: 2007-06-25 author: <florian.pantaleao.ext\@parrot.com> <jean-baptiste.lanfrey\@parrot.com>
* - first version
*/
void filter_init(uint32_t n, float32_t *old_input, float32_t initial_input, float32_t *old_output, float32_t initial_output);
/**
* \fn Filter a value.
* \brief This function uses the same notation as Matlab except that array indices start at 0
* \brief a(1)*y(n) = b(1)*x(n) + b(2)*x(n-1) + ... + b(nb+1)*x(n-nb) - a(2)*y(n-1) - ... - a(na+1)*y(n-na)
* \brief This function automatically shifts old inputs and outputs.
* \param Filter order
* \param address of B coefficients list.
* \param address of A coefficients list.
* \param input value.
* \param address of previous inputs list.
* \param address of previous outputs list.
* \return filtered value.
*
* \section History
*
* \par date: 2007-06-25 author: <florian.pantaleao.ext\@parrot.com> <jean-baptiste.lanfrey\@parrot.com>
* - first version
*/
float32_t filter(uint32_t n, const float32_t *b, const float32_t *a, float32_t input, float32_t *old_input, float32_t *old_output);
/**
* \fn Derivative filter
* \brief Y/U = kd;p/(1+Td.p)
* \param Kd, Td, Te, previous values (state and input)
* \return filtered value.
*
* \section History
*
* \par date: 2007-10-15 author: <jean-baptiste.lanfrey\@parrot.com>
* - first version
*/
float32_t deriv(deriv_param_t *param, float32_t input);
/**
* \fn Digital delay initialization
* \brief This function is used to initialize previous inputs of the digital delay.
* \param m number of sampling step time delay.
* \return void.
*
* \section History
*
* \par date: 2008-5-13 author: <yannick.foloppe.ext\@parrot.com>
* - first version
*/
void delay_init(uint32_t m, float32_t *old_input, float32_t initial_input);
/**
* \fn Delay a value
* \brief y(k)=u(k-m).
* \param m number of sampling step time delay, previous values (state and input).
* \return delayed input.
*
* \section History
*
* \par date: 2008-5-13 author: <yannick.foloppe.ext\@parrot.com>
* - first version
*/
float delay(uint32_t m, float32_t input, float32_t *old_input);
// rate limiter
// rate_max is the highest rate allowed in one sample time
// warning : rate_max must be positive
/**
* \fn rate limiter
* \brief limit the rate of an input.
* \param rate_max is the highest rate allowed in one sample time. Warning : rate_max must be positive
* \return rate limiter output.
*
* \section History
*
* \par date: 2008-7-24 author: <yannick.foloppe.ext\@parrot.com>
* - first version
*/
float32_t rate_limiter(float32_t input, float32_t old_output, float32_t rate_max);
int32_t digitalsmooth(int32_t rawIn, int32_t *sensSmoothArray);
void unwrapToPi(float32_t* former_wrapped_angle, float32_t* former_unwrapped_angle, float32_t variation);
void wrapToPi(float32_t* input, float32_t* output);
void filter64_init(uint32_t n, float64_t *old_input, float64_t initial_input, float64_t *old_output, float64_t initial_output);
float64_t filter64(uint32_t n, const float64_t *b, const float64_t *a, float64_t input, float64_t *old_input, float64_t *old_output);
#endif // FILTER
ARDroneLib/Soft/Lib/Maths/maths.c
-166
Ver Arquivo
@@ -1,166 +0,0 @@
/**
* \file maths.c
* \brief Maths library used by ARDrone
* \author Sylvain Gaeremynck <sylvain.gaeremynck@parrot.com>
* \author Jean-Baptiste Lanfrey <jean-baptiste.lanfrey@parrot.com>
* \version 1.0
*/
#include <Maths/maths.h>
float32_t f_epsilon = FLT_EPSILON;
bool_t f_is_zero( float32_t f )
{
bool_t ret;
float32_t af;
ret = FALSE;
af = (float32_t)fabsf( f );
if( af < f_epsilon )
ret = TRUE;
return ret;
}
float32_t f_zero( float32_t f )
{
if( f_is_zero(f) )
return 0.0f;
return f;
}
float32_t asin_taylor( float32_t x )
{
// float32_t xsquare = x*x;
// thrid order
return (float32_t)(x*(1.0f+x*x*0.16666667f));
// fifth order
// return (x*(1.0+xsquare*(1.0/6.0 + 3.0*xsquare/40.0);
}
float32_t atan2_taylor( float32_t num, float32_t den )
{
float32_t res;
if ( f_is_zero(den) ) {
// pi/2
res = 1.5707963268f;
}
else {
float32_t x = num/den;
// float32_t xsquare = x*x;
// thrid order
res = (float32_t)x*(1.0f-x*x*.33333333f);
// fifth order
// res = x*(1.0-xsquare*(1.0/3.0 + xsquare/5.0));
}
return (res);
}
// Polar saturation
void f_polar_sat( float32_t max, float32_t* phi, float32_t* theta)
{
float32_t alpha;
float32_t c_alpha, s_alpha;
float32_t phi_in;
float32_t theta_in;
phi_in = *phi;
theta_in = *theta;
// angle beta du plan du drone avec l'horizontal vaut beta = acosf(cosf(theta_in)*cosf(phi_in))
// on peut approximer par beta^2 = theta_in^2 + phi_in^2
if ( theta_in*theta_in + phi_in*phi_in > max*max ) {
alpha = (float32_t)atan2f(theta_in, phi_in);
sincosf( alpha, &s_alpha, &c_alpha);
*phi = max * c_alpha;
*theta = max * s_alpha;
}
}
float32_t exp_taylor( float32_t x )
{
return (float32_t)(1.0f+x+x*x*0.5f);
}
float32_t secant_taylor( float32_t x ) // secant(x) = 1 / cos(x)
{
return (float32_t)(1.0f+x*x*0.5f);
}
float32_t cos_taylor( float32_t x )
{
return (float32_t)(1.0f-x*x*0.5f);
}
float32_t sin_taylor( float32_t x )
{
return (float32_t)(x*(1.0f-x*x*.16666667f));
}
float32_t pow_taylor( float32_t x , float32_t y )
{
//x^y pour x> et y entier
float32_t coeff_3=(y*(y-1)*(y-2))/6;
float32_t coeff_2=(y*(y-1))/2;
float32_t coeff_1=y;
// float32_t puissance;
//if y positif{
return (float32_t)(coeff_3*(x-1)*(x-1)*(x-1)+coeff_2*(x-1)*(x-1)+(coeff_1*(x-1))+1);
//else
//puissance=(coeff_3*(x-1)*(x-1)*(x-1)+coeff_2*(x-1)*(x-1)+(coeff_1*(x-1))+1);
//return (float32_t) 1/puissance;
//}
}
float32_t time_navdata_in_ms(uint32_t current_time, int32_t dec)
{
//convert time given by get_current_time() in millisecond
uint32_t time_sec, time_micro_sec;
float32_t time_milli_sec;
time_sec = current_time >> dec;
time_micro_sec = current_time - (time_sec << dec);
time_milli_sec = (float32_t)(1000.0*time_sec + time_micro_sec/1000.0);
return(time_milli_sec);
}
#ifndef __ARDRONE_BIT_COUNTING_FUNCTIONS__
#define __ARDRONE_BIT_COUNTING_FUNCTIONS__
static uint32_t MASK_01010101 = (((unsigned int)(-1))/3); // 01010101010101010101010101010101
static uint32_t MASK_00110011 = (((unsigned int)(-1))/5); // 00110011001100110011001100110011
static uint32_t MASK_00001111 = (((unsigned int)(-1))/17); // 00001111000011110000111100001111
static inline int bitcountNifty_8(unsigned int n, uint32_t MASK_01010101, uint32_t MASK_00110011, uint32_t MASK_00001111)
{
n = (n & MASK_01010101) + ((n >> 1) & MASK_01010101) ;
n = (n & MASK_00110011) + ((n >> 2) & MASK_00110011) ;
n = (n & MASK_00001111) + ((n >> 4) & MASK_00001111) ;
return n;
}
static inline int bitcountNifty(unsigned int n, uint32_t MASK_01010101, uint32_t MASK_00110011, uint32_t MASK_00001111)
{
n = (n & MASK_01010101) + ((n >> 1) & MASK_01010101) ;
n = (n & MASK_00110011) + ((n >> 2) & MASK_00110011) ;
n = (n & MASK_00001111) + ((n >> 4) & MASK_00001111) ;
return n % 255 ;
}
uint32_t nb_bits_differents(uint32_t p, uint32_t q)
{
return bitcountNifty(p^q, MASK_01010101, MASK_00110011, MASK_00001111);
}
uint32_t nb_bits_differents_8(uint32_t p, uint32_t q)
{
return bitcountNifty_8(p^q, MASK_01010101, MASK_00110011, MASK_00001111);
}
#endif
ARDroneLib/Soft/Lib/Maths/maths.h
-121
Ver Arquivo
@@ -1,121 +0,0 @@
/**
* \file maths.h
* \brief Maths library used by ARDrone
* \author Sylvain Gaeremynck <sylvain.gaeremynck@parrot.com>
* \version 1.0
*/
#ifndef _MATHS_H_
#define _MATHS_H_
#include <VP_Os/vp_os_types.h>
#ifndef __USE_GNU
#define __USE_GNU
#endif // __USE_GNU
#include <math.h>
#include <float.h>
///////////////////////////////////////////////
// DEFINES
#define RAD_TO_MDEG (57295.779513f)
#define RAD_TO_DEG (57.295779513f)
#define MDEG_TO_RAD (1.745329252e-05f)
#define DEG_TO_RAD (1.745329252e-02f)
#define PI (3.1415927f)
#define GRAVITY (9.81f)
#ifndef min
#define min(a, b) (a) < (b) ? (a) : (b)
#endif
#ifndef max
#define max(a, b) (a) < (b) ? (b) : (a)
#endif
typedef struct _screen_point_t {
int32_t x;
int32_t y;
} screen_point_t;
typedef union _float_or_int_t {
float32_t f;
int32_t i;
} float_or_int_t;
extern float32_t f_epsilon;
bool_t f_is_zero( float32_t f );
float32_t f_zero( float32_t f );
static inline float32_t f_round(float32_t f, int32_t nb_decimal)
{
float32_t puissance = (float32_t) (10 ^ nb_decimal);
return ((int)(f * puissance) / puissance);
}
// Returns f's absolute value
static inline float32_t f_abs( float_or_int_t fi )
{
fi.i &= 0x7FFFFFFF;
return fi.f;
}
// Inverse f's sign
static inline void f_inv_sign( float_or_int_t* fi )
{
fi->i ^= 0x80000000;
}
// Polar saturation
void f_polar_sat( float32_t max, float32_t* phi, float32_t* theta);
// Returns clamp value with value's sign
static inline float32_t f_set_clamp( float_or_int_t value, float_or_int_t clamp_value )
{
clamp_value.i |= value.i & 0x80000000;
return clamp_value.f;
}
float32_t asin_taylor( float32_t x );
float32_t atan2_taylor( float32_t num, float32_t den );
float32_t exp_taylor( float32_t x );
float32_t secant_taylor( float32_t x );
float32_t cos_taylor( float32_t x );
float32_t sin_taylor( float32_t x );
float32_t pow_taylor( float32_t x , float32_t y );
float32_t time_navdata_in_ms( uint32_t current_time, int32_t dec );
static inline uint32_t iabs(int32_t v) { return ( v < 0 ) ? -v : v; }
#if defined( USE_MINGW32 )
static inline void sincosf(float32_t a, float32_t* out_s, float32_t* out_c)
{
__asm(
"flds %2\n"
"fsincos\n"
"fstps %1\n"
"fstps %0"
:"=m"(*out_s), "=m"(*out_c)
:"m"(a)
);
}
#elif defined( _MSC_VER ) || defined( TARGET_OS_IPHONE) || defined(TARGET_IPHONE_SIMULATOR)
static inline void sincosf(float32_t a, float32_t* out_s, float32_t* out_c)
{
*out_s = sinf(a);
*out_c = cosf(a);
}
#endif // _MSC_VER || USE_MINGW32
// 8 bits version
uint32_t nb_bits_differents_8(uint32_t p, uint32_t q);
uint32_t nb_bits_differents(uint32_t p, uint32_t q);
#endif // _MATHS_H_
ARDroneLib/Soft/Lib/Maths/matrices.c
-1140
Ver Arquivo
Diferenças do arquivo suprimidas por serem muito extensas Carregar Diff
ARDroneLib/Soft/Lib/Maths/matrices.h
-383
Ver Arquivo
@@ -1,383 +0,0 @@
/**
* \file matrices.h
* \brief Matrices library used by ARDrone
* \author Jean-Baptiste Lanfrey <jean-baptiste.lanfrey@parrot.com>
* \version 1.0
*/
#ifndef _MATRICES_H_
#define _MATRICES_H_
#include <VP_Os/vp_os_types.h>
typedef struct _matrix33_t
{
float32_t m11;
float32_t m12;
float32_t m13;
float32_t m21;
float32_t m22;
float32_t m23;
float32_t m31;
float32_t m32;
float32_t m33;
} matrix33_t;
typedef struct _vector31_t {
union {
float32_t v[3];
struct
{
float32_t x;
float32_t y;
float32_t z;
};
};
} vector31_t;
typedef union _vector21_t {
float32_t v[2];
struct
{
float32_t x;
float32_t y;
};
} vector21_t;
extern const matrix33_t matrix_id3;
extern const matrix33_t matrix_null3;
extern const vector31_t vector31_zero;
extern const vector31_t vector31_z;
// TODO Documentation
// Multiplies two matrices m1 & m2. Stores result in out.
void mul_mat( matrix33_t* out, matrix33_t *m1, matrix33_t *m2 );
void add_mat( matrix33_t* out, matrix33_t *m1, matrix33_t *m );
void mulvec_mat( vector31_t* out, matrix33_t *m1, vector31_t *v1 );
void transpose_mat( matrix33_t *out, matrix33_t *m1 );
void mulconst_mat( matrix33_t *out, matrix33_t *m1, float32_t k );
void det_mat3(float32_t *out, matrix33_t *m1);
void comatrice33(matrix33_t *out, matrix33_t *m1);
void inv_mat33(matrix33_t *out, matrix33_t* m1);
void cross_vec( vector31_t* out, vector31_t *v1, vector31_t *v2 );
void dot_vec( float32_t* out, const vector31_t *v1, const vector31_t *v2 );
void add_vec( vector31_t* out, vector31_t *v1, vector31_t *v2 );
void mulconst_vec( vector31_t *out, vector31_t *V1, float32_t k );
void skew_anti_symetric_vec( matrix33_t *out, vector31_t *v );
void vex( vector31_t *out, matrix33_t *m );
float32_t norm_vec( vector31_t *v );
bool_t normalize_vec( vector31_t* v );
void display_matrix33(matrix33_t *m1);
void display_vector31(vector31_t *v1);
// 4x4 matrix
typedef struct _matrix44_t
{
float32_t m11;
float32_t m12;
float32_t m13;
float32_t m14;
float32_t m21;
float32_t m22;
float32_t m23;
float32_t m24;
float32_t m31;
float32_t m32;
float32_t m33;
float32_t m34;
float32_t m41;
float32_t m42;
float32_t m43;
float32_t m44;
} matrix44_t;
typedef struct _vector41_t {
union {
float32_t v[4];
struct
{
float32_t x1;
float32_t x2;
float32_t x3;
float32_t x4;
};
};
} vector41_t;
extern const matrix44_t matrix_id4;
extern const matrix44_t matrix_null4;
extern const vector41_t vector41_zero;
void mul_mat44( matrix44_t* out, matrix44_t *m1, matrix44_t *m2 );
void add_mat44( matrix44_t* out, matrix44_t *m1, matrix44_t *m2 );
void mulvec_mat4( vector41_t* out, matrix44_t *m1, vector41_t *v1 );
void transpose_mat44( matrix44_t *out, matrix44_t *m1 );
void mulconst_mat44( matrix44_t *out, matrix44_t *m1, float32_t k );
void add_vec41( vector41_t* out, vector41_t *v1, vector41_t *v2 );
void mulconst_vec41( vector41_t *out, vector41_t *v1, float32_t k );
void comatrice44(matrix44_t *out, matrix44_t *m1);
void det_mat4(float32_t *out, matrix44_t *m1);
void inv_mat44(matrix44_t *out, matrix44_t* m1);
void display_matrix44(matrix44_t *m1);
void display_vector41(vector41_t *v1);
// 6x6 matrices
typedef struct _matrix66_t
{
float32_t m11;
float32_t m12;
float32_t m13;
float32_t m14;
float32_t m15;
float32_t m16;
float32_t m21;
float32_t m22;
float32_t m23;
float32_t m24;
float32_t m25;
float32_t m26;
float32_t m31;
float32_t m32;
float32_t m33;
float32_t m34;
float32_t m35;
float32_t m36;
float32_t m41;
float32_t m42;
float32_t m43;
float32_t m44;
float32_t m45;
float32_t m46;
float32_t m51;
float32_t m52;
float32_t m53;
float32_t m54;
float32_t m55;
float32_t m56;
float32_t m61;
float32_t m62;
float32_t m63;
float32_t m64;
float32_t m65;
float32_t m66;
} matrix66_t;
typedef struct _vector61_t {
union {
float32_t v[6];
struct
{
float32_t x1;
float32_t x2;
float32_t x3;
float32_t x4;
float32_t x5;
float32_t x6;
};
};
} vector61_t;
extern const matrix66_t matrix_id6;
extern const matrix66_t matrix_null6;
extern const vector61_t vector61_zero;
void mul_mat66( matrix66_t* out, matrix66_t *m1, matrix66_t *m2 );
void add_mat66( matrix66_t* out, matrix66_t *m1, matrix66_t *m2 );
void mulvec_mat6( vector61_t* out, matrix66_t *m1, vector61_t *v1 );
void transpose_mat66( matrix66_t *out, matrix66_t *m1 );
void mulconst_mat66( matrix66_t *out, matrix66_t *m1, float32_t k );
void add_vec61( vector61_t* out, vector61_t *v1, vector61_t *v2 );
void mulconst_vec61( vector61_t *out, vector61_t *v1, float32_t k );
// 2x2 matrices
typedef struct _matrix22_t
{
float32_t m11;
float32_t m12;
float32_t m21;
float32_t m22;
} matrix22_t;
extern const matrix22_t matrix_id2;
extern const matrix22_t matrix_null2;
extern const vector21_t vector21_zero;
void mul_mat22( matrix22_t* out, matrix22_t *m1, matrix22_t *m2 );
void add_mat22( matrix22_t* out, matrix22_t *m1, matrix22_t *m2 );
void mulvec_mat2( vector21_t* out, matrix22_t *m1, vector21_t *v1 );
void transpose_mat22( matrix22_t *out, matrix22_t *m1 );
void mulconst_mat22( matrix22_t *out, matrix22_t *m1, float32_t k );
void add_vec21( vector21_t* out, vector21_t *v1, vector21_t *v2 );
void mulconst_vec21( vector21_t *out, vector21_t *v1, float32_t k );
void det_mat2(float32_t *out, matrix22_t *m1);
void comatrice22(matrix22_t *out, matrix22_t *m1);
void inv_mat22(matrix22_t *out, matrix22_t* m1);
// NON-square matrices and operations between diferrent-sized matrices
typedef struct _matrix26_t
{
float32_t m11;
float32_t m12;
float32_t m13;
float32_t m14;
float32_t m15;
float32_t m16;
float32_t m21;
float32_t m22;
float32_t m23;
float32_t m24;
float32_t m25;
float32_t m26;
} matrix26_t;
typedef struct _matrix62_t
{
float32_t m11;
float32_t m12;
float32_t m21;
float32_t m22;
float32_t m31;
float32_t m32;
float32_t m41;
float32_t m42;
float32_t m51;
float32_t m52;
float32_t m61;
float32_t m62;
} matrix62_t;
typedef struct _matrix46_t
{
float32_t m11;
float32_t m12;
float32_t m13;
float32_t m14;
float32_t m15;
float32_t m16;
float32_t m21;
float32_t m22;
float32_t m23;
float32_t m24;
float32_t m25;
float32_t m26;
float32_t m31;
float32_t m32;
float32_t m33;
float32_t m34;
float32_t m35;
float32_t m36;
float32_t m41;
float32_t m42;
float32_t m43;
float32_t m44;
float32_t m45;
float32_t m46;
} matrix46_t;
typedef struct _matrix64_t
{
float32_t m11;
float32_t m12;
float32_t m13;
float32_t m14;
float32_t m21;
float32_t m22;
float32_t m23;
float32_t m24;
float32_t m31;
float32_t m32;
float32_t m33;
float32_t m34;
float32_t m41;
float32_t m42;
float32_t m43;
float32_t m44;
float32_t m51;
float32_t m52;
float32_t m53;
float32_t m54;
float32_t m61;
float32_t m62;
float32_t m63;
float32_t m64;
} matrix64_t;
extern const matrix46_t matrix_null46;
extern const matrix26_t matrix_null26;
void mulmat26vec61( vector21_t* out, matrix26_t *m1, vector61_t *v1 );
void mulmat46vec61( vector41_t* out, matrix46_t *m1, vector61_t *v1 );
void mulmat46mat66( matrix46_t* out, matrix46_t *m1, matrix66_t *m2 );
void mulmat66mat64( matrix64_t* out, matrix66_t *m1, matrix64_t *m2 );
void mulmat46mat64( matrix44_t* out, matrix46_t *m1, matrix64_t *m2 );
void mulmat64mat44( matrix64_t* out, matrix64_t *m1, matrix44_t *m2 );
void mulmat62mat22( matrix62_t* out, matrix62_t *m1, matrix22_t *m2 );
void mulmat26mat66( matrix26_t* out, matrix26_t *m1, matrix66_t *m2 );
void mulmat66mat62( matrix62_t* out, matrix66_t *m1, matrix62_t *m2 );
void mulmat26mat62( matrix22_t* out, matrix26_t *m1, matrix62_t *m2 );
void mulmat64vec41( vector61_t* out, matrix64_t *m1, vector41_t *v1 );
void mulmat62vec21( vector61_t* out, matrix62_t *m1, vector21_t *v1 );
void mulmat64mat46( matrix66_t* out, matrix64_t *m1, matrix46_t *m2 );
void mulmat62mat26( matrix66_t* out, matrix62_t *m1, matrix26_t *m2 );
void transpose_mat26( matrix62_t *out, matrix26_t *m1 );
void transpose_mat62( matrix26_t *out, matrix62_t *m1 );
void transpose_mat46( matrix64_t *out, matrix46_t *m1 );
void transpose_mat64( matrix46_t *out, matrix64_t *m1 );
#endif // _MATRICES_H_
ARDroneLib/Soft/Lib/Maths/matrix3d.c
-313
Ver Arquivo
@@ -1,313 +0,0 @@
/**
* \brief Matrix 3d implementation
* \author Sylvain Gaeremynck <sylvain.gaeremynck@parrot.fr>
* \version 1.0
* \date 04/06/2007
* \warning Subject to completion
*/
#include <Maths/matrix3d.h>
#include <Maths/matrices.h>
#include <VP_Os/vp_os_malloc.h>
#include <math.h>
matrix3d_t matrix3d_id = {
1.0f, 0.0f, 0.0f, 0.0f,
0.0f, 1.0f, 0.0f, 1.0f,
0.0f, 0.0f, 1.0f, 0.0f,
0.0f, 0.0f, 0.0f, 1.0f
};
static void vector_X( vector31_t* v )
{
v->x = 1.0f;
v->y = 0.0f;
v->z = 0.0f;
}
static void vector_Y( vector31_t* v )
{
v->x = 0.0f;
v->y = 1.0f;
v->z = 0.0f;
}
#if 0 // it is not used ... fix warning
static void vector_Z( vector31_t* v )
{
v->x = 0.0f;
v->y = 0.0f;
v->z = 1.0f;
}
#endif
void matrix3d_zero(matrix3d_t* m)
{
m->m00 = m->m01 = m->m02 = m->m03 = 0.0f;
m->m10 = m->m11 = m->m12 = m->m13 = 0.0f;
m->m20 = m->m21 = m->m22 = m->m23 = 0.0f;
m->m30 = m->m31 = m->m32 = m->m33 = 0.0f;
}
void matrix3d_identity(matrix3d_t* m)
{
m->m00 = m->m11 = m->m22 = m->m33 = 1.0f;
m->m01 = m->m02 = m->m03 = 0.0f;
m->m10 = m->m12 = m->m13 = 0.0f;
m->m20 = m->m21 = m->m23 = 0.0f;
m->m30 = m->m31 = m->m32 = 0.0f;
}
C_RESULT matrix3d_euler(matrix3d_t* m, float32_t phi, float32_t theta, float32_t psi)
{
float32_t c_phi, s_phi, c_theta, s_theta, c_psi, s_psi;
c_phi = cosf( phi );
s_phi = sinf( phi );
c_theta = cosf( theta );
s_theta = sinf( theta );
c_psi = cosf( psi );
s_psi = sinf( psi );
matrix3d_identity(m);
m->m00 = c_theta * c_psi;
m->m01 = c_theta * s_psi;
m->m02 = -s_theta;
m->m10 = s_phi * s_theta * c_psi - c_phi * s_psi;
m->m11 = s_phi * s_theta * s_psi + c_phi * c_psi;
m->m12 = s_phi * c_theta;
m->m20 = c_phi * s_theta * c_psi + s_phi * s_psi;
m->m21 = c_phi * s_theta * s_psi - s_phi * c_psi;
m->m22 = c_phi * c_theta;
return C_OK;
}
C_RESULT matrix3d_vector(matrix3d_t* m, vector31_t* pos, vector31_t* dir, vector31_t* right, vector31_t* up)
{
normalize_vec( dir );
vector_Y( up );
cross_vec( right, dir, up );
if( normalize_vec( right ) )
{
cross_vec( up, right, dir );
}
else
{
vector_X( right );
cross_vec( up, right, dir );
cross_vec( right, dir, up );
normalize_vec( right );
}
normalize_vec( up );
return matrix3d_orientation( m, pos, dir, right, up );
}
C_RESULT matrix3d_orientation(matrix3d_t* m, const vector31_t* pos, const vector31_t* dir,
const vector31_t* right, const vector31_t* up)
{
float32_t d0, d1, d2;
m->m00 = right->x;
m->m01 = up->x;
m->m02 = -dir->x;
m->m10 = right->y;
m->m11 = up->y;
m->m12 = -dir->y;
m->m20 = right->z;
m->m21 = up->z;
m->m22 = -dir->z;
dot_vec( &d0, right, pos );
dot_vec( &d1, up, pos );
dot_vec( &d2, dir, pos );
m->m30 = -d0;
m->m31 = -d1;
m->m32 = -d2;
m->m03 = 0.0f;
m->m13 = 0.0f;
m->m23 = 0.0f;
m->m33 = 1.0f;
return C_OK;
}
#define MATRIX_EXCHANGE( out, in ) temp = out; out = in; in = temp
C_RESULT matrix3d_transpose(matrix3d_t* out, matrix3d_t* in)
{
if( out != in )
{
out->m00 = in->m00;
out->m10 = in->m01;
out->m20 = in->m02;
out->m30 = in->m03;
out->m01 = in->m10;
out->m11 = in->m11;
out->m21 = in->m12;
out->m31 = in->m13;
out->m02 = in->m20;
out->m12 = in->m21;
out->m22 = in->m22;
out->m32 = in->m23;
out->m03 = in->m30;
out->m13 = in->m31;
out->m23 = in->m32;
out->m33 = in->m33;
}
else
{
float32_t temp;
MATRIX_EXCHANGE( out->m01, out->m10 );
MATRIX_EXCHANGE( out->m02, out->m20 );
MATRIX_EXCHANGE( out->m03, out->m30 );
MATRIX_EXCHANGE( out->m12, out->m21 );
MATRIX_EXCHANGE( out->m13, out->m31 );
MATRIX_EXCHANGE( out->m23, out->m32 );
}
return C_OK;
}
C_RESULT matrix3d_add(matrix3d_t* out, matrix3d_t* m1, matrix3d_t* m2)
{
out->m00 = m1->m00 + m2->m00;
out->m01 = m1->m01 + m2->m01;
out->m02 = m1->m02 + m2->m02;
out->m03 = m1->m03 + m2->m03;
out->m10 = m1->m10 + m2->m10;
out->m11 = m1->m11 + m2->m11;
out->m12 = m1->m12 + m2->m12;
out->m13 = m1->m13 + m2->m13;
out->m20 = m1->m20 + m2->m20;
out->m21 = m1->m21 + m2->m21;
out->m22 = m1->m22 + m2->m22;
out->m23 = m1->m23 + m2->m23;
out->m30 = m1->m30 + m2->m30;
out->m31 = m1->m31 + m2->m31;
out->m32 = m1->m32 + m2->m32;
out->m33 = m1->m33 + m2->m33;
return C_OK;
}
C_RESULT matrix3d_sub(matrix3d_t* out, matrix3d_t* m1, matrix3d_t* m2)
{
out->m00 = m1->m00 - m2->m00;
out->m01 = m1->m01 - m2->m01;
out->m02 = m1->m02 - m2->m02;
out->m03 = m1->m03 - m2->m03;
out->m10 = m1->m10 - m2->m10;
out->m11 = m1->m11 - m2->m11;
out->m12 = m1->m12 - m2->m12;
out->m13 = m1->m13 - m2->m13;
out->m20 = m1->m20 - m2->m20;
out->m21 = m1->m21 - m2->m21;
out->m22 = m1->m22 - m2->m22;
out->m23 = m1->m23 - m2->m23;
out->m30 = m1->m30 - m2->m30;
out->m31 = m1->m31 - m2->m31;
out->m32 = m1->m32 - m2->m32;
out->m33 = m1->m33 - m2->m33;
return C_OK;
}
#define MATRIX_MUL_PART(a,b) \
t1 = ( M1[a*4 + 0] * M2[b + 0 ] ); \
t2 = ( M1[a*4 + 1] * M2[b + 4 ] ); \
t3 = ( M1[a*4 + 2] * M2[b + 8 ] ); \
t4 = ( M1[a*4 + 3] * M2[b + 12] ); \
MOUT[a*4 + b] = ( t1 + t2 + t3 + t4 );
C_RESULT matrix3d_mul(matrix3d_t* out, matrix3d_t* m1, matrix3d_t* m2)
{
float32_t t1, t2, t3, t4;
float32_t* MOUT = (float32_t*) out;
float32_t* M1 = (float32_t*) m1;
float32_t* M2 = (float32_t*) m2;
MATRIX_MUL_PART( 0, 0 ); MATRIX_MUL_PART( 0, 1 ); MATRIX_MUL_PART( 0, 2 ); MATRIX_MUL_PART( 0, 3 );
MATRIX_MUL_PART( 1, 0 ); MATRIX_MUL_PART( 1, 1 ); MATRIX_MUL_PART( 1, 2 ); MATRIX_MUL_PART( 1, 3 );
MATRIX_MUL_PART( 2, 0 ); MATRIX_MUL_PART( 2, 1 ); MATRIX_MUL_PART( 2, 2 ); MATRIX_MUL_PART( 2, 3 );
MATRIX_MUL_PART( 3, 0 ); MATRIX_MUL_PART( 3, 1 ); MATRIX_MUL_PART( 3, 2 ); MATRIX_MUL_PART( 3, 3 );
return C_OK;
}
C_RESULT matrix3d_translate(matrix3d_t* m, vector31_t* tr)
{
m->m30 = tr->x;
m->m31 = tr->y;
m->m32 = tr->z;
m->m33 = 1.0f;
return C_OK;
}
C_RESULT matrix3d_add_translate(matrix3d_t* m, vector31_t* tr)
{
m->m30 += tr->x;
m->m31 += tr->y;
m->m32 += tr->z;
m->m33 = 1.0f;
return C_OK;
}
C_RESULT matrix3d_rotate_euler(matrix3d_t* m, float32_t phi, float32_t theta, float32_t psi)
{
matrix3d_t mat_rot;
matrix3d_t mat_res;
matrix3d_euler(&mat_rot, phi, theta, psi);
matrix3d_mul(&mat_res, &mat_rot, m);
vp_os_memcpy( (void *)m, (void *)&mat_res, sizeof(matrix3d_t) );
return C_OK;
}
C_RESULT matrix3d_rotate_axis(matrix3d_t* m, vector31_t* axis, float32_t value)
{
return C_OK;
}
C_RESULT matrix3d_transform(matrix3d_t* m1, struct _vector31_t* v1)
{
float32_t x = (m1->m00) * (v1->x) + (m1->m01) * (v1->y) + (m1->m02) * (v1->z) + m1->m30;
float32_t y = (m1->m10) * (v1->x) + (m1->m11) * (v1->y) + (m1->m12) * (v1->z) + m1->m31;
float32_t z = (m1->m20) * (v1->x) + (m1->m21) * (v1->y) + (m1->m22) * (v1->z) + m1->m32;
float32_t w = (m1->m30) * (v1->x) + (m1->m31) * (v1->y) + (m1->m32) * (v1->z) + m1->m33;
v1->x = x / w;
v1->y = y / w;
v1->z = z / w;
return C_OK;
}
ARDroneLib/Soft/Lib/Maths/matrix3d.h
-59
Ver Arquivo
@@ -1,59 +0,0 @@
/**
* \brief Matrix 3d declaration
* \author Sylvain Gaeremynck <sylvain.gaeremynck@parrot.fr>
* \version 1.0
* \date 04/06/2007
* \warning Subject to completion
*/
#ifndef _MATRIX3D_H_
#define _MATRIX3D_H_
#include <VP_Os/vp_os_types.h>
typedef struct _matrix3d_t {
float32_t m00, m01, m02, m03;
float32_t m10, m11, m12, m13;
float32_t m20, m21, m22, m23;
float32_t m30, m31, m32, m33;
} matrix3d_t;
struct _vector31_t;
extern matrix3d_t matrix3d_id; // identity matrix
// Set m to zero
void matrix3d_zero(matrix3d_t* m);
// Set m to identity
void matrix3d_identity(matrix3d_t* m);
// Initialize m from euler angles
C_RESULT matrix3d_euler(matrix3d_t* m, float32_t phi, float32_t theta, float32_t psi);
// Initialize m from a position and a direction
C_RESULT matrix3d_vector( matrix3d_t* m, struct _vector31_t* pos, struct _vector31_t* dir,
struct _vector31_t* right, struct _vector31_t* up);
// Initialize m from a position and a normalized orientation
C_RESULT matrix3d_orientation(matrix3d_t* m, const struct _vector31_t* pos, const struct _vector31_t* dir,
const struct _vector31_t* right, const struct _vector31_t* up);
// Compute transposed matrix
C_RESULT matrix3d_transpose(matrix3d_t* out, matrix3d_t* in);
// Common arithmetic operation
C_RESULT matrix3d_add(matrix3d_t* out, matrix3d_t* m1, matrix3d_t* m2);
C_RESULT matrix3d_sub(matrix3d_t* out, matrix3d_t* m1, matrix3d_t* m2);
C_RESULT matrix3d_mul(matrix3d_t* out, matrix3d_t* m1, matrix3d_t* m2);
// Common 3d transformation
C_RESULT matrix3d_translate(matrix3d_t* m, struct _vector31_t* tr);
C_RESULT matrix3d_add_translate(matrix3d_t* m, struct _vector31_t* tr);
C_RESULT matrix3d_rotate_euler(matrix3d_t* m, float32_t phi, float32_t theta, float32_t psi);
C_RESULT matrix3d_rotate_axis(matrix3d_t* m, struct _vector31_t* axis, float32_t value);
C_RESULT matrix3d_transform(matrix3d_t* m, struct _vector31_t* v);
#endif // _MATRIX3D_H_
ARDroneLib/Soft/Lib/Maths/point3d.h
-11
Ver Arquivo
@@ -1,11 +0,0 @@
#ifndef _POINT3D_H_
#define _POINT3D_H_
typedef struct _point3d_t {
float32_t x;
float32_t y;
float32_t z;
float32_t w;
} point3d_t;
#endif // _POINT3D_H_
ARDroneLib/Soft/Lib/Maths/quaternions.c
-142
Ver Arquivo
@@ -1,142 +0,0 @@
/**
* \file quaternions.c
* \brief Quaternions library used by Mykonos
* \author Francois Callou <francois.callou@parrot.com>
* \version 1.0
*/
#include <VP_Os/vp_os_assert.h>
#include <Maths/quaternions.h>
#include <Maths/maths.h>
const quaternion_t quat_unitary = { 1.0f, {{{ 0.0f, 0.0f, 0.0f}}} };
void mul_quat( quaternion_t* out, quaternion_t* q1, quaternion_t* q2)
{
vector31_t temp_v;
/// You can't have output & input pointing to the same location
VP_OS_ASSERT( out != q1 );
VP_OS_ASSERT( out != q2 );
// scalar result
out->a = q1->a*q2->a - (q1->v.x*q2->v.x + q1->v.y*q2->v.y + q1->v.z*q2->v.z);
// pure quaternion result
cross_vec( &out->v , &q1->v, &q2->v );
mulconst_vec( &temp_v, &q2->v, q1->a );
add_vec( &out->v, &out->v, &temp_v);
mulconst_vec( &temp_v, &q1->v, q2->a );
add_vec( &out->v, &out->v, &temp_v);
}
void add_quat( quaternion_t* out, quaternion_t* q1, quaternion_t* q2 )
{
// scalar result
out->a = q1->a + q2->a;
// pure quaternion result
add_vec( &out->v, &q1->v, &q2->v );
}
void mulconst_quat( quaternion_t* out, quaternion_t* q, float32_t k )
{
out->a = (q->a) * k;
mulconst_vec( &out->v, &q->v, k );
}
void conjugate_quat( quaternion_t* out, quaternion_t* q )
{
out->a = q->a;
out->v.x = -q->v.x;
out->v.y = -q->v.y;
out->v.z = -q->v.z;
}
float32_t norm_quat( quaternion_t *q )
{
return sqrtf( q->a*q->a + q->v.x * q->v.x + q->v.y * q->v.y + q->v.z * q->v.z );
}
bool_t normalize_quat( quaternion_t* q )
{
bool_t ret;
float32_t norm;
norm = norm_quat( q );
if( f_is_zero( norm ) )
{
q->a = 0.0f;
q->v.x = 0.0f;
q->v.y = 0.0f;
q->v.z = 0.0f;
ret = FALSE;
}
else
{
q->a = f_zero( q->a / norm );
q->v.x = f_zero( q->v.x / norm );
q->v.y = f_zero( q->v.y / norm );
q->v.z = f_zero( q->v.z / norm );
ret = TRUE;
}
return ret;
}
void quat_to_euler_rot_mat(matrix33_t* m, quaternion_t* q)
{
//to use with normalised quaternion
m->m11 = 1.0f - 2*q->v.y*q->v.y - 2*q->v.z*q->v.z;
m->m12 = 2*q->v.x*q->v.y - 2*q->v.z*q->a;
m->m13 = 2*q->v.z*q->v.x + 2*q->v.y*q->a;
m->m21 = 2*q->v.x*q->v.y + 2*q->v.z*q->a;
m->m22 = 1.0f - 2*q->v.x*q->v.x - 2*q->v.z*q->v.z;
m->m23 = 2*q->v.z*q->v.y - 2*q->v.x*q->a;
m->m31 = 2*q->v.z*q->v.x - 2*q->v.y*q->a;
m->m32 = 2*q->v.z*q->v.y + 2*q->v.x*q->a;
m->m33 = 1.0f - 2*q->v.x*q->v.x - 2*q->v.y*q->v.y;
}
void quat_to_euler_angles(angles_t* a, quaternion_t* q)
{
//to use with normalised quaternion
float32_t sqvx = q->v.x*q->v.x;
float32_t sqvy = q->v.y*q->v.y;
float32_t sqvz = q->v.z*q->v.z;
/* if ( f_is_zero(test -0.5) ) { // singularity at north pole
a->psi = 2 * atan2(q->a,q->v.z);
a->theta = PI/2;
a->phi = 0;
return;
}
if ( f_is_zero(test + 0.5) ) { // singularity at south pole
a->psi = -2 * atan2(q->a,q->v.z);
a->theta = - PI/2;
a->phi = 0;
return;
}*/
a->phi = atan2f(2*q->v.y*q->v.z+2*q->a*q->v.x , 1 - 2*sqvx - 2*sqvy);
a->theta = asinf(2*(q->a*q->v.y - q->v.x*q->v.z ));
a->psi = atan2f(2*q->v.x*q->v.y+2*q->a*q->v.z , 1 - 2*sqvy - 2*sqvz);
}
void euler_angles_to_quat(angles_t* a, quaternion_t* q)
{
float32_t cphi_2, sphi_2, cthe_2, sthe_2, cpsi_2, spsi_2;
sincosf((a->phi)*0.5f, &sphi_2, &cphi_2);
sincosf((a->theta)*0.5f, &sthe_2, &cthe_2);
sincosf((a->psi)*0.5f, &spsi_2, &cpsi_2);
q->a = cphi_2*cthe_2*cpsi_2 + sphi_2*sthe_2*spsi_2;
q->v.x = sphi_2*cthe_2*cpsi_2 - cphi_2*sthe_2*spsi_2;
q->v.y = cphi_2*sthe_2*cpsi_2 + sphi_2*cthe_2*spsi_2;
q->v.z = cphi_2*cthe_2*spsi_2 - sphi_2*sthe_2*cpsi_2;
}
ARDroneLib/Soft/Lib/Maths/quaternions.h
-56
Ver Arquivo
@@ -1,56 +0,0 @@
/**
* \file quaternions.c
* \brief Quaternions library used by Mykonos
* \author Fran�ois Callou <francois.callou@parrot.com>
* \version 1.0
*/
#ifndef _QUATERNIONS_H_
#define _QUATERNIONS_H_
#include <Maths/matrices.h>
#include <VP_Os/vp_os_types.h>
typedef struct _angles_t
{
float32_t phi;
float32_t theta;
float32_t psi;
} angles_t;
typedef struct _quaternion_t {
float32_t a;
vector31_t v;
} quaternion_t;
extern const quaternion_t quat_unitary;
// TODO Documentation
// Multiplies two quaternions q1 & q2. Stores result in out.
void mul_quat( quaternion_t* out, quaternion_t* q1, quaternion_t* q2);
// Adds two quaternions q1 & q2. Stores result in out.
void add_quat( quaternion_t* out, quaternion_t* q1, quaternion_t* q2 );
// Multiplies a quaternion ny a scalar
void mulconst_quat( quaternion_t *out, quaternion_t *q, float32_t k );
// Stores the conjugate quaternion in out
void conjugate_quat( quaternion_t* out, quaternion_t* q );
// Compuets the norm of a quaternion
float32_t norm_quat( quaternion_t* q );
//Normalises a quaternion
bool_t normalize_quat( quaternion_t* q );
//transformss a quaternion to the cprrsep�nding euler rotation matrix
void quat_to_euler_rot_mat(matrix33_t* m, quaternion_t* q);
//transformss a quaternion to the corrseponding euler angles
void quat_to_euler_angles(angles_t* a, quaternion_t* q);
void euler_angles_to_quat(angles_t* a, quaternion_t* q);
#endif // _QUATERNIONS_H_
ARDroneLib/Soft/Lib/Maths/time.c
-34
Ver Arquivo
@@ -1,34 +0,0 @@
#include <Maths/time.h>
#ifndef _WIN32
#include <sys/time.h>
#else
#include <sys/timeb.h>
//#include <Winsock2.h> // dont include; already included and double inclusion is buggy
int gettimeofday (struct timeval *tp, void *tz)
{
struct _timeb timebuffer;
_ftime (&timebuffer);
tp->tv_sec = (long)timebuffer.time;
tp->tv_usec = (long)timebuffer.millitm * 1000;
return 0;
}
#endif
float32_t time_in_ms_f(void)
{
float32_t time_milli_sec, f_sec, f_usec;
struct timeval tv;
gettimeofday(&tv, NULL);
f_sec = (float32_t)tv.tv_sec;
f_usec = (float32_t)tv.tv_usec;
time_milli_sec = (float32_t)(1000.0f*f_sec + f_usec/1000.0f);
return time_milli_sec;
}
ARDroneLib/Soft/Lib/Maths/time.h
-8
Ver Arquivo
@@ -1,8 +0,0 @@
#ifndef _MATHS_TIME_H_
#define _MATHS_TIME_H_
#include <VP_Os/vp_os_types.h>
float32_t time_in_ms_f(void);
#endif // _TIME_H_
ARDroneLib/Soft/Lib/Maths/vision_math.c
-255
Ver Arquivo
@@ -1,255 +0,0 @@
#include <VP_Os/vp_os_print.h>
#include <Maths/vision_math.h>
extern float32_t used_focal;
void euler_matrix(float32_t theta, float32_t phi, float32_t psi, matrix33_t *mat)
{
float32_t cthe, sthe, cphi, sphi, cpsi, spsi;
// Euler angles are now in radians
cthe = cosf(theta);
sthe = sinf(theta);
cphi = cosf(phi);
sphi = sinf(phi);
cpsi = cosf(psi);
spsi = sinf(psi);
mat->m11 = cpsi* cthe;
mat->m12 = -spsi*cphi + cpsi*sthe*sphi;
mat->m13 = spsi*sphi + cpsi*sthe*cphi;
mat->m21 = spsi*cthe;
mat->m22 = cpsi*cphi + spsi*sthe*sphi;
mat->m23 = -cpsi*sphi + spsi*sthe*cphi;
mat->m31 = -sthe;
mat->m32 = cthe*sphi;
mat->m33 = cthe*cphi;
}
void frame_euler_matrix(float32_t theta, float32_t phi, float32_t psi, matrix33_t *mat)
{
float32_t cthe, sthe, cphi, sphi, cpsi, spsi;
float32_t m11, m12, m13, m21, m22, m23, m31, m32, m33;
// R_euler*Rc with Rc = [0 -1 0; 1 0 0; 0 0 1]
// Euler angles are now in radians
cthe = cosf(theta);
sthe = sinf(theta);
cphi = cosf(phi);
sphi = sinf(phi);
cpsi = cosf(psi);
spsi = sinf(psi);
m11 = cpsi* cthe;
m12 = -spsi*cphi + cpsi*sthe*sphi;
m13 = spsi*sphi + cpsi*sthe*cphi;
m21 = spsi*cthe;
m22 = cpsi*cphi + spsi*sthe*sphi;
m23 = -cpsi*sphi + spsi*sthe*cphi;
m31 = -sthe;
m32 = cthe*sphi;
m33 = cthe*cphi;
mat->m11= m12;
mat->m12 = -m11;
mat->m13 = m13;
mat->m21 = m22;
mat->m22 = -m21;
mat->m23 = m23;
mat->m31 = m32;
mat->m32 = -m31;
mat->m33 = m33;
}
void vertical_frame_euler_matrix(float32_t theta, float32_t phi, float32_t psi, matrix33_t *mat)
{
float32_t cthe, sthe, cphi, sphi, cpsi, spsi;
float32_t m11, m12, m13, m21, m22, m23, m31, m32, m33;
// R_euler*Rc with Rc = [0 -1 0; 1 0 0; 0 0 1]
// Euler angles are now in radians
cthe = cosf(theta);
sthe = sinf(theta);
cphi = cosf(phi);
sphi = sinf(phi);
cpsi = cosf(psi);
spsi = sinf(psi);
m11 = cpsi* cthe;
m12 = -spsi*cphi + cpsi*sthe*sphi;
m13 = spsi*sphi + cpsi*sthe*cphi;
m21 = spsi*cthe;
m22 = cpsi*cphi + spsi*sthe*sphi;
m23 = -cpsi*sphi + spsi*sthe*cphi;
m31 = -sthe;
m32 = cthe*sphi;
m33 = cthe*cphi;
mat->m11= m12;
mat->m12 = -m11;
mat->m13 = m13;
mat->m21 = m22;
mat->m22 = -m21;
mat->m23 = m23;
mat->m31 = m32;
mat->m32 = -m31;
mat->m33 = m33;
}
void horizontal_frame_euler_matrix(float32_t theta, float32_t phi, float32_t psi, matrix33_t *mat)
{
float32_t cthe, sthe, cphi, sphi, cpsi, spsi;
float32_t m11, m12, m13, m21, m22, m23, m31, m32, m33;
// R_euler*Rc with Rc = [ 0 0 1; 1 0 0; 0 1 0 ]
// Euler angles are now in radians
cthe = cosf(theta);
sthe = sinf(theta);
cphi = cosf(phi);
sphi = sinf(phi);
cpsi = cosf(psi);
spsi = sinf(psi);
m11 = cpsi* cthe;
m12 = -spsi*cphi + cpsi*sthe*sphi;
m13 = spsi*sphi + cpsi*sthe*cphi;
m21 = spsi*cthe;
m22 = cpsi*cphi + spsi*sthe*sphi;
m23 = -cpsi*sphi + spsi*sthe*cphi;
m31 = -sthe;
m32 = cthe*sphi;
m33 = cthe*cphi;
mat->m11 = m12;
mat->m12 = m13;
mat->m13 = m11;
mat->m21 = m22;
mat->m22 = m23;
mat->m23 = m21;
mat->m31 = m32;
mat->m32 = m33;
mat->m33 = m31;
}
void max_euler_matrix(float32_t theta, float32_t phi, float32_t psi, matrix33_t *mat)
{
float32_t cthe, sthe, cphi, sphi, cpsi, spsi;
float32_t m11, m12, m13, m21, m22, m23, m31, m32, m33;
// R_euler*Rc with Rc = [ 0 1 0; 0 0 -1; -1 0 0 ]
// Euler angles are now in radians
cthe = cosf(theta);
sthe = sinf(theta);
cphi = cosf(phi);
sphi = sinf(phi);
cpsi = cosf(psi);
spsi = sinf(psi);
m11 = cpsi* cthe;
m12 = -spsi*cphi + cpsi*sthe*sphi;
m13 = spsi*sphi + cpsi*sthe*cphi;
m21 = spsi*cthe;
m22 = cpsi*cphi + spsi*sthe*sphi;
m23 = -cpsi*sphi + spsi*sthe*cphi;
m31 = -sthe;
m32 = cthe*sphi;
m33 = cthe*cphi;
mat->m11 = -m13;
mat->m12 = m11;
mat->m13 = -m12;
mat->m21 = -m23;
mat->m22 = m21;
mat->m23 = -m22;
mat->m31 = -m33;
mat->m32 = m31;
mat->m33 = -m32;
}
void integrated_gyros_matrix(float32_t delta_theta, float32_t delta_phi, float32_t delta_psi, matrix33_t *matproj)
{
// for the case which used integrated gyros
// projection matrix used for cancelling rotation between two frames
// ie transpose(Rc) * matrot * Rc
// with Rc = [0 -1 0; 1 0 0; 0 0 1]
// matrot = [1 delta_psi -delta_theta ; -delta_psi 1 delta_phi; delta_theta -delta_phi 1]
matproj->m11 = 1.0;
matproj->m12 = delta_psi;
matproj->m13 = delta_phi;
matproj->m21 = -delta_psi;
matproj->m22 = 1.0;
matproj->m23 = delta_theta;
matproj->m31 = -delta_phi;
matproj->m32 = -delta_theta;
matproj->m33 = 1.0;
}
void frame_euler_angles(vector31_t *angles, matrix33_t *R)
{
angles->x = (float32_t)asinf(-R->m31);
angles->y = (float32_t)atan2f(R->m32, R->m33);
angles->z = (float32_t)atan2f(R->m21, R->m11);
if (angles->z < 0)
angles->z += 2*PI;
}
void horizontal_frame_euler_angles(vector31_t *angles, matrix33_t *R)
{
angles->x = (float32_t)asinf(-R->m33);
angles->y = (float32_t)atan2f(R->m31, R->m32);
angles->z = (float32_t)atan2f(R->m23, R->m13);
if (angles->z < 0)
angles->z += 2*PI;
}
void proj_point(screen_point_t *point, screen_point_t *center, matrix33_t *mat, vector21_t *out)
{
float32_t denom;
int32_t x_centre = point->x - center->x;
int32_t y_centre = point->y - center->y;
denom = mat->m31*x_centre/used_focal + mat->m32*y_centre/used_focal + mat->m33;
out->x = (float32_t)center->x + ( mat->m11*x_centre + mat->m12*y_centre + mat->m13*used_focal) / denom;
out->y = (float32_t)center->y + ( mat->m21*x_centre + mat->m22*y_centre + mat->m23*used_focal ) / denom;
}
void proj_pointf(vector21_t *point, screen_point_t *center, matrix33_t *mat, vector21_t *out)
{
float32_t denom;
float32_t x_centre = point->x - (float32_t)center->x;
float32_t y_centre = point->y - (float32_t)center->y;
denom = mat->m31*x_centre/used_focal + mat->m32*y_centre/used_focal + mat->m33;
out->x = (float32_t)center->x + ( mat->m11*x_centre + mat->m12*y_centre + mat->m13*used_focal) / denom;
out->y = (float32_t)center->y + ( mat->m21*x_centre + mat->m22*y_centre + mat->m23*used_focal ) / denom;
}
void vision_direction_result(vector21_t *t, screen_point_t *v, int32_t threshold)
{
float32_t norm, angle;
norm = t->x*t->x + t->y*t->y;
if (norm < threshold*threshold)
{
v->x = 0;
v->y = 0;
}
else
{
angle = (float32_t)atan2f(t->y,t->x);
v->x = (angle >= -3*PI/8) * (angle <= 3*PI/8) - (angle <= -5*PI/8) - (angle >= 5*PI/8);
v->y = (angle >= PI/8) * (angle <= 7*PI/8) - (angle >= - 7*PI/8) * (angle <= -PI/8);
}
}
ARDroneLib/Soft/Lib/Maths/vision_math.h
-74
Ver Arquivo
@@ -1,74 +0,0 @@
#ifndef _STAGES_VISION_MATH_INCLUDE_
#define _STAGES_VISION_MATH_INCLUDE_
#include <Maths/maths.h>
#include <Maths/matrices.h>
/**
* @fn Compute the matrix of uav pose in space
* @param float32_t theta : euler angle
* @param float32_t phi : euler angle
* @param float32_t psi : euler angle
* @param matrix *mat
* @return VOID
*/
void
euler_matrix(float32_t theta, float32_t phi, float32_t psi, matrix33_t *mat);
void
horizontal_frame_euler_matrix(float32_t theta, float32_t phi, float32_t psi, matrix33_t *mat);
void
vertical_frame_euler_matrix(float32_t theta, float32_t phi, float32_t psi, matrix33_t *mat);
void
max_euler_matrix(float32_t theta, float32_t phi, float32_t psi, matrix33_t *mat);
/**
* @fn Compute the matrix of uav pose in space R_euler*Rc, in camera basis
* @param float32_t theta : euler angle
* @param float32_t phi : euler angle
* @param float32_t psi : euler angle
* @param matrix *mat
* @return VOID
*/
void
frame_euler_matrix(float32_t theta, float32_t phi, float32_t psi, matrix33_t *mat);
void
frame_euler_angles(vector31_t *angles, matrix33_t *R);
void
horizontal_frame_euler_angles(vector31_t *angles, matrix33_t *R);
/**
* @fn Compute the matrix which projects points from frame (t-1) to frame (t) : tr(Rc)*Rot*Rc
* @param float32_t delta_theta : integrated gyro in theta
* @param float32_t delta_phi : integrated gyro in phi
* @param float32_t delta_psi : integrated gyro in psi
* @param matrix *matproj
* @return VOID
*/
void
integrated_gyros_matrix(float32_t delta_theta, float32_t delta_phi, float32_t delta_psi, matrix33_t *matproj);
/**
* @fn compute projection of a point on a plane
* @param int x : first coordinate
* @param int y : second coordinate
* @param int centerx : first coordinate of the image center
* @param int centery : second coordinate of the image center
* @param matrix mat : projection matrix
* @param float *u : first coordinate of result
* @param float *v : second coordinate of result
* @return VOID
*/
void
proj_point(screen_point_t *point, screen_point_t *center, matrix33_t *mat, vector21_t *out);
void
proj_pointf(vector21_t *point, screen_point_t *center, matrix33_t *mat, vector21_t *out);
void
vision_direction_result(vector21_t *t, screen_point_t *v, int32_t threshold);
#endif // ! _STAGES_VISION_MATH_INCLUDE_
ARDroneLib/Soft/Lib/ardrone_tool/AT/ardrone_at_mutex.c
-997
Ver Arquivo
@@ -1,997 +0,0 @@
/********************************************************************
* COPYRIGHT PARROT 2010
********************************************************************
* PARROT MODULES
*-----------------------------------------------------------------*/
/**
* @file ardrone_at.c
* @brief .
*
* @author K. Leplat <karl.leplat.ext@parrot.com>
* @date Mon Feb 1 10:30:50 2010
*
*
*******************************************************************/
#ifdef __ARMCC_VERSION
#undef _XOPEN_SOURCE
#endif
#include <VP_Os/vp_os_assert.h>
#include <VP_Os/vp_os_print.h>
#include <ardrone_tool/Com/config_com.h>
#include <ardrone_tool/ardrone_tool.h>
#include <ardrone_tool/ardrone_version.h>
//Common
#include <ardrone_api.h>
#include <at_msgs_ids.h>
#include <config.h>
//SDK
#include <VP_Com/vp_com.h>
#include <VP_Com/vp_com_socket.h>
#include <VP_SDK/ATcodec/ATcodec_api.h>
//GNU STANDARD C LIBRARY
#include <stdio.h>
#include <stdlib.h>
#include <ctype.h>
#include <string.h>
#include <sys/socket.h>
#include <netinet/in.h>
#include <netinet/ip.h>
/********************************************************************
* Constants
*******************************************************************/
#define MAX_BUF_SIZE 256
#define AT_MUTEX_SNDBUF_SIZE 256
/********************************************************************
* Static variables and types
*******************************************************************/
AT_CODEC_MSG_IDS ids;
static uint32_t at_init = 0;
static vp_os_mutex_t at_mutex;
static uint32_t nb_sequence = 0;
static vp_com_socket_t at_socket;
static AT_CODEC_FUNCTIONS_PTRS func_ptrs;
static Write atcodec_write = NULL;
static Read atcodec_read = NULL;
// Navdata
extern float32_t nd_iphone_gaz;
extern float32_t nd_iphone_yaw;
extern int32_t nd_iphone_flag;
extern float32_t nd_iphone_phi;
extern float32_t nd_iphone_theta;
extern float32_t nd_iphone_magneto_psi;
extern float32_t nd_iphone_magneto_psi_accuracy;
/********************************************************************
* Static function declarations
*******************************************************************/
static void atcodec_init( AT_CODEC_FUNCTIONS_PTRS *funcs );
void ardrone_at_shutdown ( void );
AT_CODEC_ERROR_CODE host_init( void );
AT_CODEC_ERROR_CODE host_shutdown( void );
AT_CODEC_ERROR_CODE host_enable( void );
AT_CODEC_ERROR_CODE host_open( void );
AT_CODEC_ERROR_CODE host_close( void );
AT_CODEC_ERROR_CODE host_write(uint8_t *buffer, int32_t *len);
AT_CODEC_ERROR_CODE host_read(uint8_t *buffer, int32_t *len);
/********************************************************************
* Static functions
*******************************************************************/
AT_CODEC_ERROR_CODE host_init( void )
{
if( func_ptrs.init != NULL )
func_ptrs.init();
# undef ATCODEC_DEFINE_AT_CMD
# define ATCODEC_DEFINE_AT_CMD(ID,Str,From,Cb,Prio) \
if((ids.ID = ATcodec_Add_Defined_Message(Str)) == -1) \
{ \
return AT_CODEC_INIT_ERROR; \
}
# undef ATCODEC_DEFINE_AT_RESU
# define ATCODEC_DEFINE_AT_RESU(ID,Str,From,Cb) \
if((ids.ID = ATcodec_Add_Hashed_Message(Str,ids.From,Cb,0)) == -1) \
{ \
return AT_CODEC_INIT_ERROR; \
}
# include <at_msgs.h>
return AT_CODEC_INIT_OK;
}
AT_CODEC_ERROR_CODE host_shutdown( void )
{
if( func_ptrs.shutdown != NULL )
func_ptrs.shutdown();
ardrone_at_shutdown();
return AT_CODEC_SHUTDOWN_OK;
}
AT_CODEC_ERROR_CODE host_enable( void )
{
if( func_ptrs.enable != NULL )
return func_ptrs.enable();
/* Only used with ARDrone */
return AT_CODEC_ENABLE_OK;
}
AT_CODEC_ERROR_CODE host_open( void )
{
static bool_t init_ok = FALSE;
if( func_ptrs.open != NULL )
return func_ptrs.open();
if( !init_ok )
{
COM_CONFIG_SOCKET_AT(&at_socket, VP_COM_CLIENT, AT_PORT, wifi_ardrone_ip);
at_socket.protocol = VP_COM_UDP;
if(VP_FAILED(vp_com_init(COM_AT())))
{
PRINT ("Failed to init AT\n");
vp_com_shutdown( COM_AT() );
return AT_CODEC_OPEN_ERROR;
}
if(VP_FAILED(vp_com_open(COM_AT(), &at_socket, &atcodec_read, &atcodec_write)))
{
PRINT ("Failed to open AT\n");
return AT_CODEC_OPEN_ERROR;
}
// set send_buffer to a low value to limit latency
int32_t sendbuf = AT_MUTEX_SNDBUF_SIZE;
if ( setsockopt((int32_t)at_socket.priv, SOL_SOCKET, SO_SNDBUF, &sendbuf, sizeof(sendbuf)) )
{
PRINT ("Error setting SND_BUF for AT socket\n");
}
/*
* On android, with IP_TOS set, certain devices can't connect to AR.Drone 1
* So we just disable this functionnality to avoid these cases.
*/
#ifdef USE_ANDROID
if (IS_ARDRONE2)
{
#endif
int opt = IPTOS_PREC_NETCONTROL;
int res = setsockopt((int)at_socket.priv, IPPROTO_IP, IP_TOS, &opt, (socklen_t)sizeof(opt));
if (res)
{
perror("AT stage - setting Live video socket IP Type Of Service : ");
}
else
{
printf ("Set IP_TOS ok\n");
}
#ifdef USE_ANDROID
}
#endif
init_ok = TRUE;
}
return AT_CODEC_OPEN_OK;
}
AT_CODEC_ERROR_CODE host_close( void )
{
if( func_ptrs.close != NULL )
return func_ptrs.close();
vp_com_close(COM_AT(), &at_socket);
return AT_CODEC_CLOSE_OK;
}
AT_CODEC_ERROR_CODE host_write(uint8_t *buffer, int32_t *len)
{
if( func_ptrs.write != NULL )
return func_ptrs.write( buffer, len );
if( atcodec_write != NULL )
{
return VP_FAILED(atcodec_write(&at_socket, buffer, len)) ? AT_CODEC_WRITE_ERROR : AT_CODEC_WRITE_OK;
}
return AT_CODEC_WRITE_OK;
}
AT_CODEC_ERROR_CODE host_read(uint8_t *buffer, int32_t *len)
{
if( func_ptrs.read != NULL )
return func_ptrs.read( buffer, len );
return AT_CODEC_READ_OK;
}
static void atcodec_init( AT_CODEC_FUNCTIONS_PTRS *funcs )
{
if( funcs != NULL)
{
memcpy(&func_ptrs, funcs, sizeof(*funcs));
}
#if defined (_MSC_VER)
AT_CODEC_FUNCTIONS_PTRS ptrs =
{
/*init*/host_init,
/*shutdown*/host_shutdown,
/*enable*/host_enable,
/*open*/host_open,
/*close*/host_close,
/*write*/host_write,
/*read*/host_read
};
#else
AT_CODEC_FUNCTIONS_PTRS ptrs =
{
.init = host_init,
.shutdown = host_shutdown,
.enable = host_enable,
.open = host_open,
.close = host_close,
.read = host_read,
.write = host_write,
};
#endif
vp_os_mutex_init(&at_mutex);
ATcodec_Init_Library( &ptrs );
}
/********************************************************************
* Public functions
*******************************************************************/
void ardrone_at_set_ui_pad_value( uint32_t value )
{
if (!at_init)
return;
vp_os_mutex_lock(&at_mutex);
ATcodec_Queue_Message_valist( ids.AT_MSG_ATCMD_RC_REF_EXE,
++nb_sequence,
value );
vp_os_mutex_unlock(&at_mutex);
}
void ardrone_at_set_pmode( int32_t pmode )
{
if (!at_init)
return;
vp_os_mutex_lock(&at_mutex);
ATcodec_Queue_Message_valist( ids.AT_MSG_ATCMD_PMODE_EXE,
++nb_sequence,
pmode );
vp_os_mutex_unlock(&at_mutex);
}
void ardrone_at_set_ui_misc(int32_t m1, int32_t m2, int32_t m3, int32_t m4)
{
if (!at_init)
return;
vp_os_mutex_lock(&at_mutex);
ATcodec_Queue_Message_valist( ids.AT_MSG_ATCMD_MISC_EXE,
++nb_sequence,
m1,
m2,
m3,
m4 );
vp_os_mutex_unlock(&at_mutex);
}
void ardrone_at_set_anim( anim_mayday_t type, int32_t duration )
{
int32_t animtype = type;
if (!at_init)
return;
vp_os_mutex_lock(&at_mutex);
ATcodec_Queue_Message_valist( ids.AT_MSG_ATCMD_ANIM_EXE,
++nb_sequence,
animtype,
duration );
vp_os_mutex_unlock(&at_mutex);
}
void ardrone_at_set_flat_trim(void)
{
if (!at_init)
return;
vp_os_mutex_lock(&at_mutex);
ATcodec_Queue_Message_valist( ids.AT_MSG_ATCMD_FTRIM_EXE,
++nb_sequence );
vp_os_mutex_unlock(&at_mutex);
}
void ardrone_at_set_manual_trims(float32_t trim_pitch, float32_t trim_roll, float32_t trim_yaw)
{
float_or_int_t _trim_pitch, _trim_roll, _trim_yaw;
if (!at_init)
return;
_trim_pitch.f = trim_pitch;
_trim_roll.f = trim_roll;
_trim_yaw.f = trim_yaw;
vp_os_mutex_lock(&at_mutex);
ATcodec_Queue_Message_valist( ids.AT_MSG_ATCMD_MTRIM_EXE,
++nb_sequence,
_trim_pitch.i,
_trim_roll.i,
_trim_yaw.i);
vp_os_mutex_unlock(&at_mutex);
}
void ardrone_at_set_control_gains( api_control_gains_t* gains )
{
if (!at_init)
return;
vp_os_mutex_lock(&at_mutex);
ATcodec_Queue_Message_valist( ids.AT_MSG_ATCMD_GAIN_EXE,
++nb_sequence,
gains->pq_kp, gains->r_kp, gains->r_ki, gains->ea_kp, gains->ea_ki,
gains->alt_kp, gains->alt_ki, gains->vz_kp, gains->vz_ki,
gains->hovering_kp, gains->hovering_ki,
gains->hovering_b_kp, gains->hovering_b_ki);
vp_os_mutex_unlock(&at_mutex);
}
void ardrone_at_set_vision_track_params( api_vision_tracker_params_t* params )
{
if (!at_init)
return;
vp_os_mutex_lock(&at_mutex);
ATcodec_Queue_Message_valist( ids.AT_MSG_ATCMD_VISP_EXE,
++nb_sequence,
params->coarse_scale,
params->nb_pair,
params->loss_per,
params->nb_tracker_width,
params->nb_tracker_height,
params->scale,
params->trans_max,
params->max_pair_dist,
params->noise );
vp_os_mutex_unlock(&at_mutex);
}
void ardrone_at_raw_capture( uint8_t active )
{
if (!at_init)
return;
vp_os_mutex_lock(&at_mutex);
ATcodec_Queue_Message_valist(ids.AT_MSG_ATCMD_RAWC_EXE,
++nb_sequence,
0,
active);
vp_os_mutex_unlock(&at_mutex);
}
void ardrone_at_raw_picture(void)
{
if (!at_init)
return;
vp_os_mutex_lock(&at_mutex);
ATcodec_Queue_Message_valist(ids.AT_MSG_ATCMD_RAWC_EXE,
++nb_sequence,
1,
0);
vp_os_mutex_unlock(&at_mutex);
}
void ardrone_at_zap( ZAP_VIDEO_CHANNEL channel )
{
if (!at_init)
return;
vp_os_mutex_lock(&at_mutex);
ATcodec_Queue_Message_valist( ids.AT_MSG_ATCMD_ZAP_EXE,
++nb_sequence,
channel );
vp_os_mutex_unlock(&at_mutex);
}
void ardrone_at_cad( CAD_TYPE type, float32_t tag_size )
{
float_or_int_t size;
size.f = tag_size;
if (!at_init)
return;
vp_os_mutex_lock(&at_mutex);
ATcodec_Queue_Message_valist( ids.AT_MSG_ATCMD_CAD_EXE,
++nb_sequence,
type,
size.i);
vp_os_mutex_unlock(&at_mutex);
}
/********************************************************************
* ardrone_at_set_progress_cmd:
*-----------------------------------------------------------------*/
/**
* @param enable 1,with pitch,roll and 0,without pitch,roll.
* @param pitch Using floating value between -1 to +1.
* @param roll Using floating value between -1 to +1.
* @param gaz Using floating value between -1 to +1.
* @param yaw Using floating value between -1 to +1.
*
* @brief
*
* @DESCRIPTION
*
*******************************************************************/
void ardrone_at_set_progress_cmd(int32_t flag, float32_t phi, float32_t theta,float32_t gaz, float32_t yaw) {
float_or_int_t _phi, _theta, _gaz, _yaw;
if (!at_init)
return;
_phi.f = phi;
_theta.f = theta;
_gaz.f = gaz;
_yaw.f = yaw;
// Saving values to set them in navdata_file
nd_iphone_flag = flag;
nd_iphone_phi = phi;
nd_iphone_theta = theta;
nd_iphone_gaz = gaz;
nd_iphone_yaw = yaw;
vp_os_mutex_lock(&at_mutex);
ATcodec_Queue_Message_valist(ids.AT_MSG_ATCMD_PCMD_EXE, ++nb_sequence,
flag, _phi.i, _theta.i, _gaz.i, _yaw.i);
vp_os_mutex_unlock(&at_mutex);
}
/********************************************************************
* ardrone_at_set_progress_cmd:
*-----------------------------------------------------------------*/
/**
* @param enable 1,with pitch,roll and 0,without pitch,roll.
* @param pitch Using floating value between -1 to +1.
* @param roll Using floating value between -1 to +1.
* @param gaz Using floating value between -1 to +1.
* @param yaw Using floating value between -1 to +1.
* @param magneto_psi floating value between -1 to +1.
* @param magneto_psi_accuracy floating value between -1 to +1
*
* @brief
*
* @DESCRIPTION
*
*******************************************************************/
void ardrone_at_set_progress_cmd_with_magneto(int32_t flag, float32_t phi, float32_t theta,float32_t gaz, float32_t yaw, float32_t magneto_psi,float32_t magneto_psi_accuracy) {
float_or_int_t _phi, _theta, _gaz, _yaw, _magneto_psi, _magneto_psi_accuracy;
if (!at_init)
return;
_phi.f = phi;
_theta.f = theta;
_gaz.f = gaz;
_yaw.f = yaw;
_magneto_psi.f = magneto_psi;
_magneto_psi_accuracy.f = magneto_psi_accuracy;
// Saving values to set them in navdata_file
nd_iphone_flag = flag;
nd_iphone_phi = phi;
nd_iphone_theta = theta;
nd_iphone_gaz = gaz;
nd_iphone_yaw = yaw;
nd_iphone_magneto_psi = magneto_psi;
nd_iphone_magneto_psi_accuracy = magneto_psi_accuracy;
//printf("Sent : psi_iphone = %.4f acc = %.4f\n",magneto_psi,magneto_psi_accuracy);
vp_os_mutex_lock(&at_mutex);
ATcodec_Queue_Message_valist(ids.AT_MSG_ATCMD_PCMD_MAG_EXE, ++nb_sequence,
flag, _phi.i, _theta.i, _gaz.i, _yaw.i, _magneto_psi.i, _magneto_psi_accuracy.i);
vp_os_mutex_unlock(&at_mutex);
}
void ardrone_at_set_led_animation ( LED_ANIMATION_IDS anim_id, float32_t freq, uint32_t duration_sec )
{
float_or_int_t _freq;
if (!at_init)
return;
_freq.f = freq;
vp_os_mutex_lock(&at_mutex);
ATcodec_Queue_Message_valist(ids.AT_MSG_ATCMD_LED_EXE,
++nb_sequence,
anim_id,
_freq.i,
duration_sec);
vp_os_mutex_unlock(&at_mutex);
}
void ardrone_at_set_vision_update_options(int32_t user_vision_option)
{
if (!at_init)
return;
vp_os_mutex_lock(&at_mutex);
ATcodec_Queue_Message_valist(ids.AT_MSG_ATCMD_VISO_EXE,
++nb_sequence,
user_vision_option);
vp_os_mutex_unlock(&at_mutex);
}
/********************************************************************
* ardrone_at_set_polaris_pos:
*-----------------------------------------------------------------*/
void ardrone_at_set_polaris_pos( float32_t fx, float32_t fy, float32_t fpsi, bool_t defined, int32_t time_us )
{
float_or_int_t x, y, psi;
if (!at_init)
return;
x.f = fx;
y.f = fy;
psi.f = fpsi;
vp_os_mutex_lock(&at_mutex);
ATcodec_Queue_Message_valist( ids.AT_MSG_ATCMD_POLARIS_EXE,
++nb_sequence,
x.i,
y.i,
psi.i,
defined,
time_us );
vp_os_mutex_unlock(&at_mutex);
}
void ardrone_at_set_vicon_data(struct timeval time, int32_t frame_number, float32_t latency, vector31_t global_translation, vector31_t global_rotation_euler)
{
float_or_int_t _latency, _global_translation_x, _global_translation_y, _global_translation_z, _global_rotation_euler_x, _global_rotation_euler_y,
_global_rotation_euler_z;
if (!at_init)
return;
_latency.f = latency;
_global_translation_x.f = global_translation.x;
_global_translation_y.f = global_translation.y;
_global_translation_z.f = global_translation.z;
_global_rotation_euler_x.f = global_rotation_euler.x;
_global_rotation_euler_y.f = global_rotation_euler.y;
_global_rotation_euler_z.f = global_rotation_euler.z;
vp_os_mutex_lock(&at_mutex);
ATcodec_Queue_Message_valist( ids.AT_MSG_ATCMD_VICON_EXE,
++nb_sequence,
(int)time.tv_sec,
(int)time.tv_usec,
(int)frame_number,
_latency.i,
_global_translation_x.i,
_global_translation_y.i,
_global_translation_z.i,
_global_rotation_euler_x.i,
_global_rotation_euler_y.i,
_global_rotation_euler_z.i);
vp_os_mutex_unlock(&at_mutex);
}
/********************************************************************
* ardrone_at_set_toy_configuration_ids:
*-----------------------------------------------------------------*/
/**
* @param param A key as read from an ini file is given as "section:key".
*
* @param value
*
* @brief identified ardrone_at_set_toy_configuration
*
* @DESCRIPTION
*
*******************************************************************/
void ardrone_at_set_toy_configuration_ids(const char* param,char* ses_id, char* usr_id, char* app_id, const char* value)
{
if (!at_init)
return;
vp_os_mutex_lock(&at_mutex);
ATcodec_Queue_Message_valist( ids.AT_MSG_ATCMD_CONFIG_IDS,
++nb_sequence,
ses_id,
usr_id,
app_id );
ATcodec_Queue_Message_valist( ids.AT_MSG_ATCMD_CONFIG_EXE,
++nb_sequence,
param,
value );
vp_os_mutex_unlock(&at_mutex);
}
/********************************************************************
* ardrone_at_reset_com_watchdog:
*-----------------------------------------------------------------*/
/**
* @brief Re-connect with the ARDrone.
*
* @DESCRIPTION
*
*******************************************************************/
void ardrone_at_reset_com_watchdog(void)
{
if (!at_init)
return;
vp_os_mutex_lock(&at_mutex);
ATcodec_Queue_Message_valist( ids.AT_MSG_ATCMD_RESET_COM_WATCHDOG,
++nb_sequence );
vp_os_mutex_unlock(&at_mutex);
}
/********************************************************************
* ardrone_at_update_control_mode:
*-----------------------------------------------------------------*/
/**
* @param what_to_do
*
* @brief .
*
* @DESCRIPTION
*
*******************************************************************/
void ardrone_at_update_control_mode(ARDRONE_CONTROL_MODE control_mode)
{
if (!at_init)
return;
vp_os_mutex_lock(&at_mutex);
ATcodec_Queue_Message_valist( ids.AT_MSG_ATCMD_CTRL_EXE,
++nb_sequence,
control_mode,
0);
vp_os_mutex_unlock(&at_mutex);
}
/********************************************************************
* ardrone_at_configuration_get_ctrl_mode:
*-----------------------------------------------------------------*/
/**
* @brief Request to receive configuration file.
*
* @DESCRIPTION
*
*******************************************************************/
void ardrone_at_configuration_get_ctrl_mode(void)
{
if (!at_init)
return;
ardrone_at_update_control_mode( CFG_GET_CONTROL_MODE );
}
void ardrone_at_custom_configuration_get_ctrl_mode(void)
{
if (!at_init)
return;
ardrone_at_update_control_mode( CUSTOM_CFG_GET_CONTROL_MODE );
}
/********************************************************************
* ardrone_at_configuration_ack_ctrl_mode:
*-----------------------------------------------------------------*/
/**
* @brief Signal to the drone that the file has been received.
*
* @DESCRIPTION
*
*******************************************************************/
void ardrone_at_configuration_ack_ctrl_mode(void)
{
if (!at_init)
return;
ardrone_at_update_control_mode( ACK_CONTROL_MODE );
}
/********************************************************************
* ardrone_at_set_pwm: .
*-----------------------------------------------------------------*/
/**
* @param p1
*
* @param p2
*
* @param p3
*
* @param p4
*
* @brief .
*
* @DESCRIPTION
*
*******************************************************************/
void ardrone_at_set_pwm(int32_t p1, int32_t p2, int32_t p3, int32_t p4)
{
if (!at_init)
return;
vp_os_mutex_lock(&at_mutex);
ATcodec_Queue_Message_valist( ids.AT_MSG_ATCMD_PWM_EXE,
++nb_sequence,
p1,
p2,
p3,
p4 );
vp_os_mutex_unlock(&at_mutex);
}
/********************************************************************
* ardrone_at_set_autonomous_flight: Enables / disables the autopilot.
*-----------------------------------------------------------------*/
/**
* @param isActive Integer set to 1 to enable the autopilot.
*
* @brief Enables / disables the autopilot.
*
* @DESCRIPTION
*
*******************************************************************/
void ardrone_at_set_autonomous_flight( int32_t isActive )
{
if (!at_init)
return;
vp_os_mutex_lock(&at_mutex);
ATcodec_Queue_Message_valist( ids.AT_MSG_ATCMD_AUTONOMOUS_FLIGHT_EXE,
++nb_sequence,
isActive );
vp_os_mutex_unlock(&at_mutex);
}
/********************************************************************
* ardrone_at_set_calibration: Start calibration sequence on device
*-----------------------------------------------------------------*/
/**
* @param deviceId Integer -> device to calibrate (0 = magnetometer)
*
* @brief Start calibration sequence on device.
*
* @DESCRIPTION
*
*******************************************************************/
void ardrone_at_set_calibration( int32_t deviceId )
{
if (!at_init)
return;
vp_os_mutex_lock(&at_mutex);
ATcodec_Queue_Message_valist( ids.AT_MSG_ATCMD_CALIB,
++nb_sequence,
deviceId );
vp_os_mutex_unlock(&at_mutex);
}
static inline void strtolower(char *str, int length)
{
int i;
for(i = 0 ; (i < length) && (str[i] != '\0') ; i++)
str[i] = (char)tolower((int)str[i]);
}
#undef ARDRONE_CONFIG_KEY_IMM
#undef ARDRONE_CONFIG_KEY_REF
#undef ARDRONE_CONFIG_KEY_STR
#define ARDRONE_CONFIG_KEY_IMM(KEY, NAME, INI_TYPE, C_TYPE, C_TYPE_PTR, RW, DEFAULT, CALLBACK) \
ARDRONE_CONFIGURATION_PROTOTYPE(NAME) \
{ \
C_RESULT res = C_FAIL; \
char msg[64]; \
if(strcmp("" #INI_TYPE, "INI_FLOAT") == 0) \
{ \
res = C_OK; \
snprintf( &msg[0], sizeof(msg), "%f", *((float*)value)); \
} \
if(strcmp("" #INI_TYPE, "INI_DOUBLE") == 0) \
{ \
snprintf( &msg[0], sizeof(msg), "%lf", *((double*)value)); \
res = C_OK; \
} \
else if(strcmp("" #INI_TYPE, "INI_INT") == 0) \
{ \
snprintf( &msg[0], sizeof(msg), "%d", *((int*)value)); \
res = C_OK; \
} \
else if(strcmp("" #INI_TYPE, "INI_BOOLEAN") == 0) \
{ \
snprintf( &msg[0], sizeof(msg), *((bool_t*)value) ? "TRUE" : "FALSE"); \
res = C_OK; \
} \
if(res == C_OK) \
{ \
char tmp[64]; \
tmp[sizeof(tmp)-1]='\0'; \
strncpy(&tmp[0], KEY, sizeof(tmp)-1); \
strtolower(&tmp[0], strlen(tmp)); \
strncat(tmp, ":" #NAME, sizeof(tmp)-strlen(tmp)-1); \
ardrone_at_set_toy_configuration_ids( &tmp[0], ses_id, usr_id, app_id, &msg[0] ); \
} \
return res; \
}
#define ARDRONE_CONFIG_KEY_REF(KEY, NAME, INI_TYPE, C_TYPE, C_TYPE_PTR, RW, DEFAULT, CALLBACK)
#define ARDRONE_CONFIG_KEY_STR(KEY, NAME, INI_TYPE, C_TYPE, C_TYPE_PTR, RW, DEFAULT, CALLBACK) \
ARDRONE_CONFIGURATION_PROTOTYPE(NAME) \
{ \
C_RESULT res = C_FAIL; \
if(value != NULL) \
{ \
char tmp[64]; \
tmp[sizeof(tmp)-1]='\0'; \
strncpy(&tmp[0], KEY, sizeof(tmp)-1); \
strtolower(&tmp[0], strlen(tmp)); \
strncat(tmp, ":" #NAME, sizeof(tmp)-strlen(tmp)-1); \
ardrone_at_set_toy_configuration_ids( &tmp[0], ses_id, usr_id, app_id, ((C_TYPE_PTR)value) ); \
res = C_OK; \
} \
return res; \
}
#include <config_keys.h> // must be included before to have types
/********************************************************************
* ardrone_at_init_with_funcs: Init at command with ATCodec funcs
*-----------------------------------------------------------------*/
/**
* @param void
*
* @brief Fill structure AT codec
* and built the library AT commands.
*
* @DESCRIPTION
*
*******************************************************************/
void ardrone_at_init_with_funcs ( const char* ip, size_t ip_len, AT_CODEC_FUNCTIONS_PTRS *funcs)
{
if ( at_init )
return;
VP_OS_ASSERT( ip_len < ARDRONE_IPADDRESS_SIZE );
if(ip_len >= ARDRONE_IPADDRESS_SIZE)
ip_len = ARDRONE_IPADDRESS_SIZE - 1;
vp_os_memcpy( &wifi_ardrone_ip[0], ip, ip_len);
wifi_ardrone_ip[ip_len]='\0';
atcodec_init (funcs);
at_init = 1;
}
/********************************************************************
* ardrone_at_shutdown: Close at command.
*-----------------------------------------------------------------*/
/**
* @param void
*
* @brief
*
* @DESCRIPTION
*
*******************************************************************/
void ardrone_at_shutdown ( void )
{
if ( !at_init )
return;
ATcodec_Shutdown_Library();
vp_os_mutex_destroy(&at_mutex);
at_init = 0;
}
/********************************************************************
* ardrone_at_init: Init at command.
*-----------------------------------------------------------------*/
/**
* @param void
*
* @brief Fill structure AT codec
* and built the library AT commands.
*
* @DESCRIPTION
*
*******************************************************************/
void ardrone_at_init ( const char* ip, size_t ip_len)
{
if ( at_init )
return;
VP_OS_ASSERT( ip_len < ARDRONE_IPADDRESS_SIZE );
if(ip_len >= ARDRONE_IPADDRESS_SIZE)
ip_len = ARDRONE_IPADDRESS_SIZE - 1;
vp_os_memcpy( &wifi_ardrone_ip[0], ip, ip_len);
wifi_ardrone_ip[ip_len]='\0';
atcodec_init (NULL);
at_init = 1;
}
/********************************************************************
* ardrone_at_open: Open at command socket.
*-----------------------------------------------------------------*/
/**
* @param void
*
* @brief Open at command socket.
*
* @DESCRIPTION
*
*******************************************************************/
ATCODEC_RET ardrone_at_open ( void )
{
if ( !at_init )
return ATCODEC_FALSE;
return host_open()==AT_CODEC_OPEN_OK?ATCODEC_TRUE:ATCODEC_FALSE;
}
/********************************************************************
* ardrone_at_send: Send all pushed messages.
*-----------------------------------------------------------------*/
/**
* @param void
*
* @brief Send all pushed messages.
*
* @DESCRIPTION
*
*******************************************************************/
ATCODEC_RET ardrone_at_send ( void )
{
ATCODEC_RET res;
if ( !at_init )
return ATCODEC_FALSE;
res = ATcodec_Send_Messages();
return res;
}
ARDroneLib/Soft/Lib/ardrone_tool/Academy/academy.c
-251
Ver Arquivo
@@ -1,251 +0,0 @@
/**
* academy.c
*
* Created by Frederic D'HAEYER on 8/04/11.
* Copyright 2011 Parrot SA. All rights reserved.
*
*/
#include <ardrone_tool/Academy/academy.h>
#include <ardrone_tool/Academy/academy_download.h>
#include <ardrone_tool/Academy/academy_upload.h>
#include <utils/ardrone_date.h>
PROTO_THREAD_ROUTINE(academy, data);
char flight_dir[ROOT_NAME_SIZE] = ".";
static THREAD_HANDLE academy_thread;
static bool_t academy_started = FALSE;
typedef struct _academy_t_
{
vp_os_mutex_t mutex;
vp_os_cond_t cond;
int16_t flight_max_storing_size;
uint32_t flight_sum;
char flight_oldest_date[ARDRONE_DATE_MAXSIZE];
} academy_t;
static academy_t academy;
void academy_init(const char *flightdir, int max_storing_size)
{
if(!academy_started)
{
PA_WARNING("MEMORY SPACE ALLOWED : %d MB\n", max_storing_size);
vp_os_memset(&academy, 0, sizeof(academy_t));
vp_os_mutex_init(&academy.mutex);
vp_os_cond_init(&academy.cond, &academy.mutex);
academy.flight_max_storing_size = max_storing_size;
// copy root directory
if((flightdir != NULL) && strlen(flightdir) < ROOT_NAME_SIZE)
strcpy(flight_dir, flightdir);
academy_started = TRUE;
vp_os_thread_create(thread_academy, (THREAD_PARAMS)NULL, &academy_thread);
}
academy_upload_init();
}
void academy_shutdown(void)
{
if(academy_started)
{
academy_started = FALSE;
vp_os_mutex_lock(&academy.mutex);
vp_os_cond_signal(&academy.cond);
vp_os_mutex_unlock(&academy.mutex);
vp_os_thread_join(academy_thread);
}
academy_upload_shutdown();
}
void academy_check_memory(void)
{
vp_os_mutex_lock(&academy.mutex);
vp_os_cond_signal(&academy.cond);
vp_os_mutex_unlock(&academy.mutex);
}
void academy_setState(academy_state_t *state, ACADEMY_STATE academy_state)
{
state->state = academy_state;
state->result = ACADEMY_RESULT_NONE;
}
void academy_resetState(academy_state_t *state)
{
state->state = ACADEMY_STATE_NONE;
state->result = ACADEMY_RESULT_NONE;
}
void academy_nextState(academy_state_t *state)
{
state->state = (state->state + 1) % ACADEMY_STATE_MAX;
state->result = ACADEMY_RESULT_NONE;
}
void academy_stateOK(academy_state_t *state)
{
state->result = ACADEMY_RESULT_OK;
}
void academy_stateERROR(academy_state_t *state)
{
state->result = ACADEMY_RESULT_FAILED;
}
int academy_remove(const char* fpath, const struct stat *sb, int typeflag)
{
if(typeflag == FTW_F)
remove(fpath);
return 0;
}
int academy_compute_memory_used(const char* fpath, const struct stat *sb, int typeflag)
{
char prefix[FLIGHT_NAME_SIZE];
sprintf(prefix, "%s/flight_", flight_dir);
if(strncmp(fpath, prefix, strlen(prefix)) == 0)
{
if(typeflag == FTW_F)
{
academy.flight_sum += sb->st_size;
}
else if(typeflag == FTW_D)
{
if(academy.flight_oldest_date[0] == '\0')
{
sscanf(fpath + strlen(prefix), "%s", academy.flight_oldest_date);
}
}
}
return 0;
}
char *academy_get_next_item_with_prefix(const char *list, char **next_item, const char *prefix, bool_t isDirectory)
{
char *result = NULL;
static char directoryLine[ACADEMY_MAX_LINE];
if(*next_item == NULL)
*next_item = (char*)list;
if(*next_item && prefix)
{
char *ptr = *next_item;
while(!result && ptr)
{
ptr = strchr(*next_item, '\r');
if(ptr != NULL)
{
int length = (ptr - *next_item);
memcpy(directoryLine, *next_item, length);
directoryLine[length] = '\0';
*next_item = ptr + 2; // To remove \r\n to end of line
// One line found
ptr = strrchr(directoryLine, ' ');
if(ptr != NULL)
{
ptr++;
if((directoryLine[0] == (isDirectory ? 'd' : '-')) && (strncmp(ptr, prefix, strlen(prefix)) == 0))
result = ptr;
}
}
}
}
return result;
}
// Can remove ftp_directory if there is only files inside
_ftp_status academy_remove_ftp_directory(_ftp_t *ftp, const char *directory_name)
{
char *list = NULL;
char *ptr = NULL;
_ftp_status academy_status = ftpCd(ftp, directory_name);
if(FTP_SUCCEDED(academy_status))
{
char *next_item = NULL;
academy_status = ftpList(ftp, &list, NULL);
PA_DEBUG("list file :\n%s\n", list);
while(FTP_SUCCEDED(academy_status) && (ptr = academy_get_next_item_with_prefix(list, &next_item, "", FALSE)))
{
PA_DEBUG("Removing %s...\n", ptr);
academy_status = ftpRemove(ftp, ptr);
}
if(list != NULL)
{
vp_os_free(list);
list = NULL;
}
academy_status = ftpCd(ftp, "..");
if(FTP_SUCCEDED(academy_status))
{
PA_DEBUG("Removing %s...\n", directory_name);
academy_status = ftpRmdir(ftp, directory_name);
}
}
return academy_status;
}
DEFINE_THREAD_ROUTINE(academy, data)
{
printf("Start thread %s\n", __FUNCTION__);
bool_t needToCheckMemory = TRUE;
while( academy_started && !ardrone_tool_exit() )
{
if(!needToCheckMemory)
{
vp_os_mutex_lock(&academy.mutex);
vp_os_cond_wait(&academy.cond);
needToCheckMemory = TRUE;
vp_os_mutex_unlock(&academy.mutex);
}
while(needToCheckMemory)
{
academy.flight_sum = 0;
academy.flight_oldest_date[0] = '\0';
if(!ftw(flight_dir, &academy_compute_memory_used, ACADEMY_MAX_FD_FOR_FTW))
{
if(academy.flight_sum > MBYTE_TO_BYTE(academy.flight_max_storing_size))
{
char remove_dir[ACADEMY_MAX_FILENAME];
sprintf(remove_dir, "%s/flight_%s", flight_dir, academy.flight_oldest_date);
// remove oldest flight
PA_WARNING("Too much memory used %d MB > %d MB, removing oldest flight %s...", BYTE_TO_MBYTE(academy.flight_sum), academy.flight_max_storing_size, remove_dir);
if(!ftw(remove_dir, &academy_remove, ACADEMY_MAX_FD_FOR_FTW))
{
rmdir(remove_dir);
PA_WARNING("OK.\n");
}
}
else
{
needToCheckMemory = FALSE;
}
}
else
{
needToCheckMemory = FALSE;
}
}
}
THREAD_RETURN(C_OK);
}
ARDroneLib/Soft/Lib/ardrone_tool/Academy/academy.h
-88
Ver Arquivo
@@ -1,88 +0,0 @@
/**
* academy.h
*
* Created by Frederic D'HAEYER on 8/04/11.
* Copyright 2011 Parrot SA. All rights reserved.
*
*/
#ifndef _ACADEMY_H_
#define _ACADEMY_H_
#include <ardrone_api.h>
#include <academy_common.h>
#include <ardrone_tool/ardrone_tool.h>
#include <VP_Os/vp_os_types.h>
#include <VP_Api/vp_api_thread_helper.h>
#include <utils/ardrone_ftp.h>
#include <VP_Os/vp_os_delay.h>
#include <VP_Os/vp_os_thread.h>
#include <VP_Os/vp_os_print.h>
#include <VP_Os/vp_os_malloc.h>
#include <VP_Os/vp_os_signal.h>
#include <stdio.h>
#include <dirent.h>
#ifndef USE_ANDROID
#include <ftw.h>
#else
#include <utils/AR_Ftw.h>
#endif
#include <time.h>
#ifndef _WIN32
#include <sys/time.h>
#include <unistd.h>
#else
#include <sys/timeb.h>
#include <Winsock2.h> // for timeval structure
#endif
#ifdef DEBUG
#define PILOTACADEMY_DEBUG
#endif
#define ACADEMY_MAX_LIST 1024
#define ACADEMY_MAX_LINE 128
#define ACADEMY_MAX_FILENAME 256
#define ACADEMY_MAX_FD_FOR_FTW 20
#define PA_PREFIX "PA : "
#ifdef PILOTACADEMY_DEBUG
#define PA_DEBUG(...) \
do \
{ \
PRINT (PA_PREFIX); \
PRINT (__VA_ARGS__); \
} while (0)
#define PA_WARNING(...) \
do \
{ \
PRINT (PA_PREFIX); \
PRINT (__VA_ARGS__); \
} while (0)
#else
#define PA_DEBUG(...)
#define PA_WARNING(...) \
do \
{ \
PRINT (PA_PREFIX); \
PRINT (__VA_ARGS__); \
} while (0)
#endif
#define ACADEMY_SERVERNAME "parrot01.nyx.emencia.net"
#define ACADEMY_PORT 21
#define BYTE_TO_MBYTE(a) (a / 1048576)
#define MBYTE_TO_BYTE(a) (a * 1048576)
void academy_init(const char *flight_dir, int max_storing_size);
void academy_shutdown(void);
C_RESULT academy_connect(const char *username, const char *password, academy_callback callback);
C_RESULT academy_disconnect(void);
extern char flight_dir[];
#endif // _ACADEMY_H_
ARDroneLib/Soft/Lib/ardrone_tool/Academy/academy_download.c
-424
Ver Arquivo
@@ -1,424 +0,0 @@
/**
* academy_download.c
*
* Created by Frederic D'HAEYER on 8/04/11.
* Copyright 2011 Parrot SA. All rights reserved.
*
*/
#include <ardrone_tool/Academy/academy_download.h>
#define DISPLAY_STRING_LENGTH (128)
typedef struct _academy_download_t_
{
academy_callback callback;
academy_download_new_media new_media_callback;
academy_state_t state;
} academy_download_t;
PROTO_THREAD_ROUTINE(academy_download, data);
static bool_t academy_download_started = FALSE;
static bool_t academy_download_in_pause = FALSE;
static THREAD_HANDLE academy_download_thread;
static vp_os_mutex_t academy_download_mutex;
static vp_os_cond_t academy_download_cond;
static void academy_download_private_callback(academy_state_t state)
{
static char msg[DISPLAY_STRING_LENGTH] = { 0 };
switch(state.result)
{
case ACADEMY_RESULT_NONE:
switch(state.state)
{
case ACADEMY_STATE_CONNECTION:
snprintf (msg, DISPLAY_STRING_LENGTH, "Connecting to %s...", &wifi_ardrone_ip[0]);
break;
case ACADEMY_STATE_PREPARE_PROCESS:
snprintf (msg, DISPLAY_STRING_LENGTH, "Preparing download from %s to local directory...", &wifi_ardrone_ip[0]);
break;
case ACADEMY_STATE_PROCESS:
snprintf (msg, DISPLAY_STRING_LENGTH, "Downloading from %s to local directory...", &wifi_ardrone_ip[0]);
break;
case ACADEMY_STATE_FINISH_PROCESS:
snprintf (msg, DISPLAY_STRING_LENGTH, "Finishing download from %s to local directory...", &wifi_ardrone_ip[0]);
break;
case ACADEMY_STATE_DISCONNECTION:
snprintf (msg, DISPLAY_STRING_LENGTH, "Disconnection to %s...", &wifi_ardrone_ip[0]);
break;
case ACADEMY_STATE_NONE:
strncpy(msg, "", DISPLAY_STRING_LENGTH);
break;
default:
strncpy(msg, "", DISPLAY_STRING_LENGTH);
break;
}
break;
case ACADEMY_RESULT_OK:
strncat(msg, "OK", DISPLAY_STRING_LENGTH - strlen (msg));
break;
case ACADEMY_RESULT_FAILED:
strncat(msg, "FAILED", DISPLAY_STRING_LENGTH - strlen (msg));
break;
}
if(strlen(msg) != 0)
PA_DEBUG("===========> %s\n", msg);
}
static void academy_download_resetState(academy_download_t *academy)
{
academy_resetState(&academy->state);
if(academy->callback)
academy->callback(academy->state);
}
static void academy_download_nextState(academy_download_t *academy)
{
academy_nextState(&academy->state);
if(academy->callback)
academy->callback(academy->state);
}
static void academy_download_stateOK(academy_download_t *academy)
{
academy_stateOK(&academy->state);
if(academy->callback)
academy->callback(academy->state);
}
static void academy_download_stateERROR(academy_download_t *academy)
{
academy_stateERROR(&academy->state);
if(academy->callback)
academy->callback(academy->state);
academy_download_resetState(academy);
}
void academy_download_init(academy_download_new_media academy_download_callback_func)
{
if(!academy_download_started)
{
vp_os_mutex_init (&academy_download_mutex);
vp_os_cond_init (&academy_download_cond, &academy_download_mutex);
academy_download_started = TRUE;
vp_os_thread_create(thread_academy_download, (THREAD_PARAMS)academy_download_callback_func, &academy_download_thread);
}
}
void academy_download_shutdown(void)
{
if(academy_download_started)
{
academy_download_started = FALSE;
academy_download_resume ();
vp_os_thread_join(academy_download_thread);
}
}
DEFINE_THREAD_ROUTINE(academy_download, data)
{
char *directoryList = NULL;
bool_t shouldGoInPause = FALSE;
char *fileList = NULL;
char process_dirname[ACADEMY_MAX_FILENAME];
_ftp_t *academy_ftp = NULL;
_ftp_status academy_status;
struct stat statbuf;
char *ptr = NULL;
academy_download_t academy;
academy.new_media_callback = (academy_download_new_media)data;
academy.callback = academy_download_private_callback;
PRINT("Start thread %s \n", __FUNCTION__);
do
{
academy_download_resetState(&academy);
shouldGoInPause = FALSE;
if (TRUE == academy_download_in_pause)
{
vp_os_mutex_lock (&academy_download_mutex);
PRINT ("Academy download stage paused\n");
vp_os_cond_wait (&academy_download_cond);
vp_os_mutex_unlock (&academy_download_mutex);
}
academy_download_pause();
PRINT ("Academy download stage resumed\n");
academy_download_nextState(&academy);
while( academy_download_started && !shouldGoInPause)
{
academy_status = FTP_FAIL;
switch(academy.state.state)
{
case ACADEMY_STATE_NONE:
academy_status = FTP_SUCCESS;
break;
case ACADEMY_STATE_CONNECTION:
if(academy_ftp == NULL)
academy_ftp = ftpConnect(&wifi_ardrone_ip[0], ACADEMY_PORT, "anonymous", "", &academy_status);
if(academy_ftp != NULL)
academy_status = FTP_SUCCESS;
break;
case ACADEMY_STATE_PREPARE_PROCESS:
academy_status = ftpCd(academy_ftp, "/boxes");
if(FTP_SUCCEDED(academy_status))
{
char *next_dir = NULL;
PA_DEBUG("Enter to boxes directory\n");
academy_status = ftpList(academy_ftp, &directoryList, NULL);
// If directory is empty, we consider that it is an error
if(academy_status == FTP_SAMESIZE)
academy_status = FTP_FAIL;
if(FTP_SUCCEDED(academy_status))
{
char local_dir[ACADEMY_MAX_FILENAME];
// Search if it stay ftpremove_* directories to continue removing
PA_DEBUG("Get directory list :\n%s", directoryList);
while(FTP_SUCCEDED(academy_status) && (ptr = academy_get_next_item_with_prefix(directoryList, &next_dir, "ftpremove_downloading_", TRUE)))
{
char remove_dirname[ACADEMY_MAX_FILENAME];
strncpy(remove_dirname, ptr, ACADEMY_MAX_FILENAME);
academy_status = academy_remove_ftp_directory(academy_ftp, remove_dirname);
}
academy_status = FTP_FAIL;
next_dir = NULL;
// Search if it stay downloading_flight_* file to continue downloading.
ptr = academy_get_next_item_with_prefix(directoryList, &next_dir, "downloading_flight_", TRUE);
// Check if transfer was in progress
if(ptr != NULL)
{
// Transfer was in progress
PA_DEBUG("Transfer was in progress\n");
strncpy(process_dirname, ptr, ACADEMY_MAX_FILENAME);
snprintf(local_dir, ACADEMY_MAX_FILENAME, "%s/%s", flight_dir, process_dirname);
if((stat(local_dir, &statbuf) == 0) && S_ISDIR(statbuf.st_mode))
{
// Transfer was in progress by current device
PA_DEBUG("Transfer was in progress by current device\n");
academy_status = FTP_SUCCESS;
}
else
{
// Transfer was in progress by an other device => Create local directory
PA_DEBUG("Transfer was in progress by an other device\n");
PRINT("Creating local directory %s\n", local_dir);
if(mkdir(local_dir, 0777) >= 0)
{
PA_DEBUG("Create local directory %s\n", local_dir);
academy_status = FTP_SUCCESS;
}
}
}
else
{
PA_DEBUG(" Transfer was not in progress\n");
// Transfer was not in progress => Get oldest flight, rename remote directory and create local directory
next_dir = NULL;
ptr = academy_get_next_item_with_prefix(directoryList, &next_dir, "flight_", TRUE);
if(ptr != NULL)
{
// Prepare to process new transfer
PA_DEBUG("Prepare to process new transfer\n");
snprintf(process_dirname, ACADEMY_MAX_FILENAME, "downloading_%s", ptr);
academy_status = ftpRename(academy_ftp, ptr, process_dirname);
if(FTP_SUCCEDED(academy_status))
{
PA_DEBUG("Rename %s to %s directory\n", ptr, process_dirname);
snprintf(local_dir, ACADEMY_MAX_FILENAME, "%s/%s", flight_dir, process_dirname);
if((stat(local_dir, &statbuf) != 0) && (mkdir(local_dir, 0777) >= 0))
PA_DEBUG("Create local directory %s if not exist\n", local_dir);
academy_status = FTP_SUCCESS;
}
}
else
{
shouldGoInPause = TRUE;
}
}
if(directoryList != NULL)
{
vp_os_free(directoryList);
directoryList = NULL;
}
}
}
break;
case ACADEMY_STATE_PROCESS:
academy_status = ftpCd(academy_ftp, process_dirname);
if(FTP_SUCCEDED(academy_status))
{
PA_DEBUG("Enter to %s directory\n", process_dirname);
academy_status = ftpList(academy_ftp, &fileList, NULL);
if(FTP_SUCCEDED(academy_status))
{
char *next_file = NULL;
char media_dirname[ACADEMY_MAX_FILENAME];
char local_filename[ACADEMY_MAX_FILENAME];
bool_t userbox_ready = FALSE;
PA_DEBUG("Get file list :\n%s", fileList);
ptr = academy_get_next_item_with_prefix(fileList, &next_file, "userbox_", FALSE);
if(ptr != NULL)
{
snprintf(local_filename, ACADEMY_MAX_FILENAME, "%s/%s/%s", flight_dir, process_dirname, ptr);
PA_DEBUG("Get %s from drone to local directory...", local_filename);
academy_status = ftpGet (academy_ftp, ptr, local_filename, 1, NULL);
if(FTP_SUCCEDED(academy_status))
{
userbox_ready = TRUE;
PA_DEBUG("OK\n");
}
else
PA_DEBUG("ERROR\n");
}
snprintf(media_dirname, ACADEMY_MAX_FILENAME, "%s/media_%s", flight_dir, process_dirname + (strlen("downloading_flight_")));
//PRINT("medias directory : %s\n", media_dirname);
next_file = NULL;
while(FTP_SUCCEDED(academy_status) && (ptr = academy_get_next_item_with_prefix(fileList, &next_file, "picture_", FALSE)))
{
if((stat(media_dirname, &statbuf) != 0) && (mkdir(media_dirname, 0777) >= 0))
PA_DEBUG("Create local media directory %s if not exist\n", media_dirname);
sprintf(local_filename, "%s/%s", media_dirname, ptr);
PA_DEBUG("Get %s from drone to local directory...", local_filename);
academy_status = ftpGet (academy_ftp, ptr, local_filename, 1, NULL);
if(FTP_SUCCEDED(academy_status))
{
if (NULL != academy.new_media_callback)
{
academy.new_media_callback((const char *)local_filename, TRUE);
}
PA_DEBUG("OK\n");
}
else
{
PA_DEBUG("ERROR\n");
}
}
if (NULL != academy.new_media_callback)
{
academy.new_media_callback((const char *)NULL, FALSE);
}
if(FTP_SUCCEDED(academy_status) && !userbox_ready)
{
char local_dirname[ACADEMY_MAX_FILENAME];
snprintf(local_dirname, ACADEMY_MAX_FILENAME, "%s/%s", flight_dir, process_dirname);
remove(local_dirname);
}
}
if(FTP_SUCCEDED(academy_status))
ftpCd(academy_ftp, "..");
}
break;
case ACADEMY_STATE_FINISH_PROCESS:
{
char remove_dirname[ACADEMY_MAX_FILENAME];
char src[ACADEMY_MAX_FILENAME];
char dest[ACADEMY_MAX_FILENAME];
// Rename local directory from downloading_flight_* to flight_*
snprintf(src, ACADEMY_MAX_FILENAME, "%s/%s", flight_dir, process_dirname);
snprintf(dest, ACADEMY_MAX_FILENAME, "%s/%s", flight_dir, process_dirname + strlen("downloading_"));
rename(src, dest);
// Rename ftp remote directory from downloading_flight_* to ftpremove_downloading_flight_*
snprintf(remove_dirname, ACADEMY_MAX_FILENAME, "ftpremove_%s", process_dirname);
academy_status = ftpRename(academy_ftp, process_dirname, remove_dirname);
// Delete ftpremove_downloading_flight_*
if(FTP_SUCCEDED(academy_status))
academy_status = academy_remove_ftp_directory(academy_ftp, remove_dirname);
}
break;
case ACADEMY_STATE_DISCONNECTION:
if(academy_ftp != NULL)
ftpClose(&academy_ftp);
academy_check_memory();
academy_status = FTP_SUCCESS;
break;
default:
// Nothing to do
PA_DEBUG("THIS CASE SHOULD NOT HAPPEN\n");
break;
}
if(FTP_SUCCEDED(academy_status))
{
PA_DEBUG("continue state from %d to %d\n", academy.state.state, (academy.state.state + 1) % ACADEMY_STATE_MAX);
academy_download_stateOK(&academy);
academy_download_nextState(&academy);
}
else
{
PA_DEBUG("stop\n");
shouldGoInPause = TRUE;
academy_download_stateERROR(&academy);
if(fileList != NULL)
{
vp_os_free(fileList);
fileList = NULL;
}
if(academy_ftp)
ftpClose(&academy_ftp);
}
}
if(fileList != NULL)
{
vp_os_free(fileList);
fileList = NULL;
}
if(academy_ftp)
ftpClose(&academy_ftp);
}
while(academy_download_started && !ardrone_tool_exit());
vp_os_cond_destroy(&academy_download_cond);
vp_os_mutex_destroy(&academy_download_mutex);
THREAD_RETURN(C_OK);
}
void academy_download_pause (void)
{
vp_os_mutex_lock (&academy_download_mutex);
academy_download_in_pause = TRUE;
vp_os_mutex_unlock (&academy_download_mutex);
}
void academy_download_resume (void)
{
vp_os_mutex_lock (&academy_download_mutex);
academy_download_in_pause = FALSE;
vp_os_cond_signal (&academy_download_cond);
vp_os_mutex_unlock (&academy_download_mutex);
}
ARDroneLib/Soft/Lib/ardrone_tool/Academy/academy_download.h
-28
Ver Arquivo
@@ -1,28 +0,0 @@
/**
* academy_download.h
*
* Created by Frederic D'HAEYER on 8/04/11.
* Copyright 2011 Parrot SA. All rights reserved.
*
*/
#ifndef _ACADEMY_DOWNLOAD_H_
#define _ACADEMY_DOWNLOAD_H_
#include <ardrone_tool/Academy/academy.h>
void academy_check_memory(void);
void academy_setState(academy_state_t *state, ACADEMY_STATE academy_state);
void academy_resetState(academy_state_t *state);
void academy_nextState(academy_state_t *state);
void academy_stateOK(academy_state_t *state);
void academy_stateERROR(academy_state_t *state);
int academy_remove(const char* fpath, const struct stat *sb, int typeflag);
char *academy_get_next_item_with_prefix(const char *list, char **next_item, const char *prefix, bool_t isDirectory);
_ftp_status academy_remove_ftp_directory(_ftp_t *ftp, const char *directory_name);
void academy_download_init(academy_download_new_media callback);
void academy_download_shutdown(void);
void academy_download_pause(void);
void academy_download_resume(void);
#endif // _ACADEMY_DOWNLOAD_H_
ARDroneLib/Soft/Lib/ardrone_tool/Academy/academy_stage_recorder.c
-191
Ver Arquivo
@@ -1,191 +0,0 @@
#include <time.h>
#ifndef _WIN32
#include <sys/time.h>
#else
#include <sys/timeb.h>
#include <Winsock2.h> // for timeval structure
#endif
#include <VP_Os/vp_os_malloc.h>
#include <config.h>
#include <ardrone_tool/Academy/academy_stage_recorder.h>
#include <utils/ardrone_date.h>
#define ACADEMY_RECORD_FILE_EXTENSION "enc"
#define ACADEMY_RECORD_FILE_EXTENSION_INPROGRESS "tmp"
#ifndef ACADEMY_FILE_DEFAULT_PATH
#define ACADEMY_FILE_DEFAULT_PATH flight_dir
extern char flight_dir[];
#endif
#define ACADEMY_MEDIA_DIRPATH_FORMAT "%s/media_%s"
#define ACADEMY_MEDIA_VIDEO_FILEPATH_FORMAT "%s/video_%s.%s"
#define ACADEMY_MEDIA_DIR_ACCESS_RIGHT 0777
static void ardrone_academy_stage_recorder_internal_close(ardrone_academy_stage_recorder_config_t *cfg);
ardrone_academy_stage_recorder_config_t ardrone_academy_stage_recorder_config;
const vp_api_stage_funcs_t ardrone_academy_stage_recorder_funcs = {
(vp_api_stage_handle_msg_t) NULL,
(vp_api_stage_open_t) ardrone_academy_stage_recorder_open,
(vp_api_stage_transform_t) ardrone_academy_stage_recorder_transform,
(vp_api_stage_close_t) ardrone_academy_stage_recorder_close
};
bool_t ardrone_academy_stage_recorder_state(void)
{
return (ardrone_academy_stage_recorder_config.startRec != ACADEMY_RECORD_STOP);
}
C_RESULT
ardrone_academy_stage_recorder_handle (ardrone_academy_stage_recorder_config_t * cfg, PIPELINE_MSG msg_id, void *callback, void *param)
{
C_RESULT result = C_FAIL;
switch (msg_id)
{
case PIPELINE_MSG_START:
if(cfg->startRec == ACADEMY_RECORD_STOP)
{
time_t t;
char date[ARDRONE_DATE_MAXSIZE];
ardrone_time2date(*((uint32_t*)param), ARDRONE_FILE_DATE_FORMAT, date);
char video_filename[ACADEMY_MAX_FILENAME];
char media_dirname[ACADEMY_MAX_FILENAME];
struct stat statbuf;
sprintf(media_dirname, ACADEMY_MEDIA_DIRPATH_FORMAT, ACADEMY_FILE_DEFAULT_PATH, date);
if((stat(media_dirname, &statbuf) != 0) && (mkdir(media_dirname, ACADEMY_MEDIA_DIR_ACCESS_RIGHT) >= 0))
PA_DEBUG("Create local media directory %s if not exist\n", media_dirname);
// NO ELSE, the local directory already exists
t = time(NULL);
ardrone_time2date(t, ARDRONE_FILE_DATE_FORMAT, date);
sprintf(cfg->video_filename, ACADEMY_MEDIA_VIDEO_FILEPATH_FORMAT,
media_dirname,
date,
ACADEMY_RECORD_FILE_EXTENSION);
strcpy(video_filename, cfg->video_filename);
strcat(video_filename, ".");
strcat(video_filename, ACADEMY_RECORD_FILE_EXTENSION_INPROGRESS);
cfg->fp = fopen(video_filename, "wb");
if (cfg->fp != NULL)
{
printf("Start recording %s\n", cfg->video_filename);
cfg->startRec = ACADEMY_RECORD_START;
result = C_OK;
}
else
{
printf ("Can't open file %s\n", video_filename);
}
}
// NO ELSE - The record is already started or is starting
break;
case PIPELINE_MSG_STOP:
if(cfg->startRec != ACADEMY_RECORD_STOP)
{
cfg->startRec = ACADEMY_RECORD_STOP;
printf("Stop recording %s\n", cfg->video_filename);
ardrone_academy_stage_recorder_internal_close(cfg);
result = C_OK;
}
// NO ELSE - Record is already stopped.
break;
default:
break;
}
return (result);
}
void ardrone_academy_stage_recorder_enable(bool_t enable, uint32_t timestamp)
{
printf("Recording %s...\n", enable ? "started" : "stopped");
ardrone_academy_stage_recorder_handle (&ardrone_academy_stage_recorder_config, (enable ? PIPELINE_MSG_START : PIPELINE_MSG_STOP), NULL, &timestamp);
}
void ardrone_academy_stage_recorder_internal_close(ardrone_academy_stage_recorder_config_t *cfg)
{
char video_filename[ACADEMY_MAX_FILENAME];
strcpy(video_filename, cfg->video_filename);
strcat(video_filename, ".");
strcat(video_filename, ACADEMY_RECORD_FILE_EXTENSION_INPROGRESS);
fclose(cfg->fp);
cfg->fp=NULL;
rename(video_filename, cfg->video_filename);
}
C_RESULT ardrone_academy_stage_recorder_open(ardrone_academy_stage_recorder_config_t *cfg)
{
cfg->startRec = ACADEMY_RECORD_STOP;
return C_OK;
}
C_RESULT ardrone_academy_stage_recorder_transform(ardrone_academy_stage_recorder_config_t *cfg, vp_api_io_data_t *in, vp_api_io_data_t *out)
{
C_RESULT result = C_OK;
#ifndef _WIN32
vp_os_mutex_lock( &out->lock );
if( out->status == VP_API_STATUS_INIT )
{
out->numBuffers = 1;
out->indexBuffer = 0;
out->lineSize = NULL;
}
// NO ELSE - We need to initialize the number of buffers and the index of buffer in first.
out->size = in->size;
out->status = in->status;
out->buffers = in->buffers;
if( in->status == VP_API_STATUS_ENDED )
{
out->status = in->status;
}
else if(in->status == VP_API_STATUS_STILL_RUNNING)
{
out->status = VP_API_STATUS_PROCESSING;
}
else
{
out->status = in->status;
}
if( (cfg->fp != NULL) && (in->size > 0) && (out->status == VP_API_STATUS_PROCESSING) && (cfg->startRec == ACADEMY_RECORD_START))
{
fwrite(&in->size, 1, sizeof(int32_t), cfg->fp);
fwrite(in->buffers[in->indexBuffer], 1, in->size, cfg->fp);
}
// NO ELSE because :
// If the descriptor file is NULL, we can't to write data.
// If data size to write is 0, we don't have data to write.
// If the status is not processing, it means the record is stopping.
// If startRec is not start, it means the record is stopping or is starting.
vp_os_mutex_unlock( &out->lock );
#endif
return result;
}
C_RESULT ardrone_academy_stage_recorder_close(ardrone_academy_stage_recorder_config_t *cfg)
{
if( cfg->fp != NULL )
{
ardrone_academy_stage_recorder_internal_close(cfg);
}
// NO ELSE because :
// If the descriptor file is NULL, we can't to write data.
return C_OK;
}
ARDroneLib/Soft/Lib/ardrone_tool/Academy/academy_stage_recorder.h
-46
Ver Arquivo
@@ -1,46 +0,0 @@
#ifndef _ACADEMY_STAGE_RECORDER_H_
#define _ACADEMY_STAGE_RECORDER_H_
#include <stdio.h>
#include <VP_Api/vp_api.h>
#include <VP_Api/vp_api_picture.h>
#include <ardrone_tool/Academy/academy.h>
#ifndef _ACADEMY_RECORD_STATE_ENUM_
#define _ACADEMY_RECORD_STATE_ENUM_
typedef enum
{
ACADEMY_RECORD_STOP, // Video recording has been stopped. Stage will end and restart.
ACADEMY_RECORD_START, // Video recording has started.
}
ardrone_academy_record_state;
#endif
typedef struct _ardrone_academy_stage_recorder_config_t_ ardrone_academy_stage_recorder_config_t;
typedef void (*ardrone_academy_recorder_callback)(ardrone_academy_stage_recorder_config_t *);
struct _ardrone_academy_stage_recorder_config_t_
{
// public
vp_api_picture_t *picture;
DEST_HANDLE dest;
ardrone_academy_recorder_callback callback;
// private
FILE* fp;
ardrone_academy_record_state startRec;
char video_filename[ACADEMY_MAX_FILENAME];
};
bool_t ardrone_academy_stage_recorder_state(void);
void ardrone_academy_stage_recorder_enable(bool_t enable, uint32_t timestamps);
C_RESULT ardrone_academy_stage_recorder_open(ardrone_academy_stage_recorder_config_t *cfg);
C_RESULT ardrone_academy_stage_recorder_transform(ardrone_academy_stage_recorder_config_t *cfg, vp_api_io_data_t *in, vp_api_io_data_t *out);
C_RESULT ardrone_academy_stage_recorder_close(ardrone_academy_stage_recorder_config_t *cfg);
extern const vp_api_stage_funcs_t ardrone_academy_stage_recorder_funcs;
extern ardrone_academy_stage_recorder_config_t ardrone_academy_stage_recorder_config;
#endif // _ACADEMY_STAGE_RECORDER_H_
ARDroneLib/Soft/Lib/ardrone_tool/Academy/academy_upload.c
-396
Ver Arquivo
@@ -1,396 +0,0 @@
/**
* academy_upload.c
*
* Created by Frederic D'HAEYER on 8/04/11.
* Copyright 2011 Parrot SA. All rights reserved.
*
*/
#include <ardrone_tool/Academy/academy_upload.h>
typedef struct _academy_upload_t_
{
vp_os_mutex_t mutex;
vp_os_cond_t cond;
academy_callback callback;
academy_state_t state;
academy_user_t user;
bool_t connected;
} academy_upload_t;
PROTO_THREAD_ROUTINE(academy_upload, data);
static bool_t academy_upload_started = FALSE;
static THREAD_HANDLE academy_upload_thread;
static academy_upload_t academy_upload;
C_RESULT academy_connect(const char *username, const char *password, academy_callback callback)
{
C_RESULT result = C_FAIL;
if(academy_upload_started && (academy_upload.state.state == ACADEMY_STATE_NONE))
{
if(!academy_upload.connected)
{
_ftp_t *academy_ftp = NULL;
_ftp_status status;
strcpy(academy_upload.user.username, username ? username : "");
strcpy(academy_upload.user.password, password ? password : "");
if(callback != NULL)
academy_upload.callback = callback;
academy_ftp = ftpConnectFromName(ACADEMY_SERVERNAME, ACADEMY_PORT, academy_upload.user.username, academy_upload.user.password, &status);
if(academy_ftp != NULL)
{
ftpClose(&academy_ftp);
vp_os_mutex_lock(&academy_upload.mutex);
academy_upload.connected = TRUE;
vp_os_cond_signal(&academy_upload.cond);
vp_os_mutex_unlock(&academy_upload.mutex);
result = C_OK;
}
}
else
result = C_OK;
}
return result;
}
C_RESULT academy_disconnect(void)
{
C_RESULT result = C_FAIL;
if(academy_upload_started)
{
if(academy_upload.connected)
{
vp_os_mutex_lock(&academy_upload.mutex);
academy_upload.connected = FALSE;
vp_os_mutex_unlock(&academy_upload.mutex);
result = C_OK;
}
else
result = C_OK;
}
return result;
}
static void academy_upload_private_callback(academy_state_t state)
{
static char msg[128] = { 0 };
switch(state.result)
{
case ACADEMY_RESULT_NONE:
switch(state.state)
{
case ACADEMY_STATE_CONNECTION:
sprintf(msg, "Connecting to %s...", ACADEMY_SERVERNAME);
break;
case ACADEMY_STATE_PREPARE_PROCESS:
sprintf(msg, "Preparing upload from local directory to %s...", ACADEMY_SERVERNAME);
break;
case ACADEMY_STATE_PROCESS:
sprintf(msg, "Uploading from local directory to %s...", ACADEMY_SERVERNAME);
break;
case ACADEMY_STATE_FINISH_PROCESS:
sprintf(msg, "Finishing upload from local directory to %s...", ACADEMY_SERVERNAME);
break;
case ACADEMY_STATE_DISCONNECTION:
sprintf(msg, "Disconnection to %s...", ACADEMY_SERVERNAME);
break;
case ACADEMY_STATE_NONE:
strcpy(msg, "");
break;
default:
strcpy(msg, "");
break;
}
break;
case ACADEMY_RESULT_OK:
strcat(msg, "OK");
break;
case ACADEMY_RESULT_FAILED:
strcat(msg, "FAILED");
break;
}
if(strlen(msg) != 0)
PA_DEBUG("===========> %s\n", msg);
}
static void academy_upload_setState(academy_upload_t *academy, ACADEMY_STATE state)
{
academy_setState(&academy->state, state);
if(academy->callback)
academy->callback(academy->state);
}
static void academy_upload_resetState(academy_upload_t *academy)
{
academy_resetState(&academy->state);
if(academy->callback)
academy->callback(academy->state);
}
static void academy_upload_nextState(academy_upload_t *academy)
{
academy_nextState(&academy->state);
if(academy->callback)
academy->callback(academy->state);
}
static void academy_upload_stateOK(academy_upload_t *academy)
{
academy_stateOK(&academy->state);
if(academy->callback)
academy->callback(academy->state);
}
static void academy_upload_stateERROR(academy_upload_t *academy)
{
academy_stateERROR(&academy->state);
if(academy->callback)
academy->callback(academy->state);
academy_upload_resetState(academy);
}
void academy_upload_init(void)
{
if(!academy_upload_started)
{
vp_os_mutex_init(&academy_upload.mutex);
vp_os_cond_init(&academy_upload.cond, &academy_upload.mutex);
academy_upload.connected = FALSE;
vp_os_memset(&academy_upload.user, 0, sizeof(academy_user_t));
academy_upload_started = TRUE;
vp_os_thread_create(thread_academy_upload, (THREAD_PARAMS)&academy_upload, &academy_upload_thread);
}
}
void academy_upload_shutdown(void)
{
if(academy_upload_started)
{
academy_upload_started = FALSE;
vp_os_mutex_lock(&academy_upload.mutex);
vp_os_cond_signal(&academy_upload.cond);
vp_os_mutex_unlock(&academy_upload.mutex);
vp_os_thread_join(academy_upload_thread);
}
}
DEFINE_THREAD_ROUTINE(academy_upload, data)
{
char *directoryList = NULL;
char *fileList = NULL;
char dirname[ACADEMY_MAX_FILENAME];
_ftp_t *academy_ftp = NULL;
_ftp_status academy_status;
char *ptr = NULL;
academy_upload_t *academy = (academy_upload_t *)data;
printf("Start thread %s\n", __FUNCTION__);
while( academy_upload_started && !ardrone_tool_exit() )
{
vp_os_mutex_lock(&academy->mutex);
vp_os_memset(&academy->user, 0, sizeof(academy_user_t));
academy->callback = &academy_upload_private_callback;
academy->connected = FALSE;
academy_upload_resetState(academy);
vp_os_cond_wait(&academy->cond);
vp_os_mutex_unlock(&academy->mutex);
while(academy_upload_started && academy->connected)
{
academy_status = FTP_FAIL;
academy_upload_nextState(academy);
switch(academy->state.state)
{
case ACADEMY_STATE_CONNECTION:
{
struct dirent *dirent = NULL;
DIR *dir = NULL;
academy_status = FTP_FAIL;
// Check if flight_* directory exist in local dir
if((dir = opendir(flight_dir)) != NULL)
{
struct stat statbuf;
while(FTP_FAILED(academy_status) && (dirent = readdir(dir)) != NULL)
{
if((strncmp(dirent->d_name, "flight_", strlen("flight_")) == 0))
{
char local_dir[ACADEMY_MAX_FILENAME];
sprintf(dirname, "%s", dirent->d_name);
sprintf(local_dir, "%s/%s", flight_dir, dirname);
if((stat(local_dir, &statbuf) == 0) && S_ISDIR(statbuf.st_mode))
academy_status = FTP_SUCCESS;
}
}
closedir(dir);
}
if(FTP_SUCCEDED(academy_status))
{
if(academy_ftp == NULL)
academy_ftp = ftpConnectFromName(ACADEMY_SERVERNAME, ACADEMY_PORT, academy->user.username, academy->user.password, &academy_status);
}
}
break;
case ACADEMY_STATE_PREPARE_PROCESS:
academy_status = ftpCd(academy_ftp, "/Uploaded");
if(FTP_FAILED(academy_status))
{
ftpMkdir(academy_ftp, "/Uploaded");
academy_status = ftpCd(academy_ftp, "/Uploaded");
}
if(FTP_SUCCEDED(academy_status))
{
academy_status = ftpList(academy_ftp, &directoryList, NULL);
if(FTP_SUCCEDED(academy_status))
{
bool_t found = FALSE;
char *next_dir = NULL;
while(!found && (ptr = academy_get_next_item_with_prefix(directoryList, &next_dir, "flight_", TRUE)))
{
if(strcmp(ptr, dirname) == 0)
{
found = TRUE;
academy_upload_setState(academy, ACADEMY_STATE_FINISH_PROCESS);
}
}
if(directoryList != NULL)
{
vp_os_free(directoryList);
directoryList = NULL;
}
}
}
break;
case ACADEMY_STATE_PROCESS:
academy_status = ftpCd(academy_ftp, "/Uploading");
if(FTP_FAILED(academy_status))
{
ftpMkdir(academy_ftp, "/Uploading");
academy_status = ftpCd(academy_ftp, "/Uploading");
}
if(FTP_SUCCEDED(academy_status))
{
ftpMkdir(academy_ftp, dirname);
academy_status = ftpCd(academy_ftp, dirname);
if(FTP_SUCCEDED(academy_status))
{
char local_dir[ACADEMY_MAX_FILENAME];
struct dirent *dirent = NULL;
DIR *dir = NULL;
sprintf(local_dir, "%s/%s", flight_dir, dirname);
if((dir = opendir(local_dir)) != NULL)
{
char local_filename[ACADEMY_MAX_FILENAME];
struct stat statbuf;
while(FTP_SUCCEDED(academy_status) && ((dirent = readdir(dir)) != NULL))
{
if((strncmp(dirent->d_name, "picture_", strlen("picture_")) == 0) || (strncmp(dirent->d_name, "userbox_", strlen("userbox_")) == 0))
{
sprintf(local_filename, "%s/%s", local_dir, dirent->d_name);
if((stat(local_filename, &statbuf) == 0) && !S_ISDIR(statbuf.st_mode))
{
PA_DEBUG("Put %s from local directory to server...", dirent->d_name);
academy_status = ftpPut(academy_ftp, local_filename, dirent->d_name, 1, NULL);
if(FTP_SUCCEDED(academy_status))
PA_DEBUG("OK\n");
else
PA_DEBUG("ERROR\n");
}
}
}
closedir(dir);
}
}
}
break;
case ACADEMY_STATE_FINISH_PROCESS:
{
char local_dir[ACADEMY_MAX_FILENAME];
char src[ACADEMY_MAX_FILENAME];
char dest[ACADEMY_MAX_FILENAME];
sprintf(src, "/Uploading/%s", dirname);
sprintf(dest, "/Uploaded/%s", dirname);
academy_status = ftpRename(academy_ftp, src, dest);
// Penser à supprimer le fichier local
academy_status = FTP_FAIL;
sprintf(local_dir, "%s/%s", flight_dir, dirname);
if(!ftw(local_dir, &academy_remove, ACADEMY_MAX_FD_FOR_FTW))
{
rmdir(local_dir);
academy_status = FTP_SUCCESS;
}
}
break;
case ACADEMY_STATE_DISCONNECTION:
if(academy_ftp != NULL)
ftpClose(&academy_ftp);
academy_status = FTP_SUCCESS;
break;
default:
// Nothing to do
break;
}
if(FTP_SUCCEDED(academy_status))
{
PA_DEBUG("continue state from %d to %d\n", academy->state.state, (academy->state.state + 1) % ACADEMY_STATE_MAX);
academy_upload_stateOK(academy);
}
else
{
PA_DEBUG("stop\n");
academy_upload_stateERROR(academy);
if(fileList != NULL)
{
vp_os_free(fileList);
fileList = NULL;
}
if(academy_ftp)
ftpClose(&academy_ftp);
vp_os_delay(1000);
}
}
if(fileList != NULL)
{
vp_os_free(fileList);
fileList = NULL;
}
if(academy_ftp)
ftpClose(&academy_ftp);
} // while
THREAD_RETURN(C_OK);
}
ARDroneLib/Soft/Lib/ardrone_tool/Academy/academy_upload.h
-25
Ver Arquivo
@@ -1,25 +0,0 @@
/**
* academy_upload.h
*
* Created by Frederic D'HAEYER on 8/04/11.
* Copyright 2011 Parrot SA. All rights reserved.
*
*/
#ifndef _ACADEMY_UPLOAD_H_
#define _ACADEMY_UPLOAD_H_
#include <ardrone_tool/Academy/academy.h>
void academy_setState(academy_state_t *state, ACADEMY_STATE academy_state);
void academy_resetState(academy_state_t *state);
void academy_nextState(academy_state_t *state);
void academy_stateOK(academy_state_t *state);
void academy_stateERROR(academy_state_t *state);
int academy_remove(const char* fpath, const struct stat *sb, int typeflag);
char *academy_get_next_item_with_prefix(const char *list, char **next_item, const char *prefix, bool_t isDirectory);
_ftp_status academy_remove_ftp_directory(_ftp_t *ftp, const char *directory_name);
void academy_upload_init(void);
void academy_upload_shutdown(void);
#endif // _ACADEMY_UPLOAD_H_
ARDroneLib/Soft/Lib/ardrone_tool/Com/config_com.h
-12
Ver Arquivo
@@ -1,12 +0,0 @@
#ifndef _CONFIG_COM_H_
#define _CONFIG_COM_H_
#include <config.h>
#define NUM_ATTEMPTS 10
#include <ardrone_tool/Com/config_serial.h>
#include <ardrone_tool/Com/config_wifi.h>
#endif // _CONFIG_COM_H_
ARDroneLib/Soft/Lib/ardrone_tool/Com/config_serial.c
-108
Ver Arquivo
@@ -1,108 +0,0 @@
#include <VP_Os/vp_os_malloc.h>
#include <config.h>
#include <ardrone_tool/Com/config_com.h>
#ifndef _WIN32
vp_com_t* serial_com(void)
{
static vp_com_t com = {
VP_COM_SERIAL,
FALSE,
0,
#ifdef _WIN32
0,
#else
PTHREAD_MUTEX_INITIALIZER,
#endif
NULL,
NULL,
0,
NULL,
NULL,
NULL,
NULL,
NULL,
NULL,
NULL,
NULL,
NULL,
NULL,
NULL
};
return &com;
}
vp_com_config_t* serial_config_0(void)
{
static vp_com_serial_config_t config = {
SERIAL_LINK_0,
VP_COM_BAUDRATE_115200,
SL0_BAUDRATE,
1,
0,
0
};
return (vp_com_config_t*) &config;
}
vp_com_config_t* serial_config_1(void)
{
static vp_com_serial_config_t config = {
SERIAL_LINK_1,
VP_COM_BAUDRATE_115200,
SL1_BAUDRATE,
1,
0,
1
};
return (vp_com_config_t*) &config;
}
vp_com_config_t* serial_config_2(void)
{
static vp_com_serial_config_t config = {
SERIAL_LINK_2,
VP_COM_BAUDRATE_115200,
SL2_BAUDRATE,
1,
0,
0
};
return (vp_com_config_t*) &config;
}
vp_com_connection_t* serial_connection_0(void)
{
static vp_com_connection_t connection = { 0 };
return &connection;
}
vp_com_connection_t* serial_connection_1(void)
{
static vp_com_connection_t connection = { 0 };
return &connection;
}
vp_com_connection_t* serial_connection_2(void)
{
static vp_com_connection_t connection = { 0 };
return &connection;
}
void serial_config_socket(vp_com_socket_t* socket, VP_COM_SOCKET_TYPE type)
{
vp_os_memset(socket, 0, sizeof(vp_com_socket_t));
socket->type = type;
}
#endif
ARDroneLib/Soft/Lib/ardrone_tool/Com/config_serial.h
-53
Ver Arquivo
@@ -1,53 +0,0 @@
#ifndef _CONFIG_SERIAL_H_
#define _CONFIG_SERIAL_H_
#ifndef _CONFIG_COM_H_
#error "You cannot include config_serial.h. Please include config_com.h instead."
#endif
#include <VP_Com/vp_com.h>
/// Initializations for serial communication
/// Serial link 0 is used for navdata & ATCmd
/// Serial link 1 is used for video
/// Serial link 2 is used for communication with ADC
// Each tools must implement this functions
extern vp_com_t* serial_com(void);
extern vp_com_config_t* serial_config_0(void);
extern vp_com_config_t* serial_config_1(void);
extern vp_com_config_t* serial_config_2(void);
extern vp_com_connection_t* serial_connection_0(void);
extern vp_com_connection_t* serial_connection_1(void);
extern vp_com_connection_t* serial_connection_2(void);
void serial_config_socket(vp_com_socket_t* socket, VP_COM_SOCKET_TYPE type);
#ifdef USE_NAVDATA_SERIAL
#define COM_NAVDATA() serial_com()
#define COM_CONFIG_NAVDATA() serial_config_0()
#define COM_CONNECTION_NAVDATA() serial_connection_0()
#define COM_CONFIG_SOCKET_NAVDATA(socket, type, opt, serverhost) serial_config_socket(socket, type)
#endif // USE_NAVDATA_SERIAL
#ifdef USE_AT_SERIAL
#define COM_AT() serial_com()
#define COM_CONFIG_AT() serial_config_0()
#define COM_CONNECTION_AT() serial_connection_0()
#define COM_CONFIG_SOCKET_AT(socket, type, opt, serverhost) serial_config_socket(socket, type)
#endif // USE_NAVDATA_SERIAL
#ifdef USE_VIDEO_SERIAL
#define COM_VIDEO() serial_com()
#define COM_CONFIG_VIDEO() serial_config_1()
#define COM_CONNECTION_VIDEO() serial_connection_1()
#define COM_CONFIG_SOCKET_VIDEO(socket, type, opt, serverhost) serial_config_socket(socket, type)
#define COM_RAW_CAPTURE() serial_com()
#define COM_CONFIG_RAW_CAPTURE() serial_config_2()
#define COM_CONNECTION_RAW_CAPTURE() serial_connection_2()
#define COM_CONFIG_SOCKET_RAW_CAPTURE(socket, type, opt, serverhost) serial_config_socket(socket, type)
#endif // USE_VIDEO_SERIAL
#endif // _CONFIG_SERIAL_H_
ARDroneLib/Soft/Lib/ardrone_tool/Com/config_wifi.c
-345
Ver Arquivo
@@ -1,345 +0,0 @@
#include <VP_Os/vp_os_malloc.h>
#include <config.h>
#include <stdlib.h>
#include <unistd.h>
#include <string.h>
#include <ardrone_tool/Com/config_com.h>
#include <VP_Os/vp_os_print.h>
#include <VP_Com/vp_com_socket.h>
#include <VP_Com/vp_com.h>
#include <arpa/inet.h>
#include <sys/types.h>
#include <sys/socket.h>
#include <net/if.h>
#include <arpa/inet.h>
#include <errno.h>
#include <stdio.h>
#ifndef USE_ANDROID
#include <ifaddrs.h>
#else
#include <net/if_arp.h>
#endif
#define BUFFER_SIZE 1024
#define AUTH_MAX_NUMRETRIES 3
#define AUTH_MSG "PARROT AUTH"
#define AUTH_MSG_OK "PARROT AUTH OK"
vp_com_t* wifi_com(void)
{
static vp_com_t com = {
VP_COM_WIFI,
FALSE,
0,
#ifdef _WIN32
{ 0 },
#else // ! USE_MINGW32
PTHREAD_MUTEX_INITIALIZER,
#endif // ! USE_MINGW32
NULL,
NULL,
0,
NULL,
NULL,
NULL,
NULL,
NULL,
NULL,
NULL,
NULL,
NULL,
NULL,
NULL
};
return &com;
}
#define inaddrr(x) (*(struct in_addr *) &ifr->x[sizeof sa.sin_port])
#define IFRSIZE ((int)(size * sizeof (struct ifreq)))
C_RESULT wifi_server_auth(struct in_addr *addr)
{
Read read = NULL;
Write write = NULL;
C_RESULT result = C_FAIL;
int numretries = 1;
uint8_t recvString[BUFFER_SIZE]; /* Buffer for received string */
int recvStringLen; /* Length of received string */
struct timeval tv = {0, 500000};
int on=1;
const uint8_t msg[] = AUTH_MSG;
struct in_addr to;
struct in_addr from;
vp_com_socket_t socket;
// Initialize sending socket
to.s_addr = htonl(WIFI_BROADCAST_ADDR);
COM_CONFIG_SOCKET_AUTH(&socket, VP_COM_CLIENT, AUTH_PORT, inet_ntoa(to));
socket.protocol = VP_COM_UDP;
if(VP_FAILED(vp_com_init(COM_AUTH())))
{
printf("Failed to init Authentification\n");
vp_com_shutdown( COM_AUTH() );
return C_FAIL;
}
if(VP_FAILED(vp_com_open(COM_AUTH(), &socket, &read, &write)))
{
printf("Failed to open Authentification\n");
vp_com_shutdown( COM_AUTH() );
return C_FAIL;
}
if (setsockopt((int)socket.priv, SOL_SOCKET, SO_BROADCAST,(char *)&on,sizeof(on)) < 0)
{
printf("Failed to set socket option Authentification\n");
vp_com_close(COM_AUTH(), &socket);
vp_com_shutdown( COM_AUTH() );
return C_FAIL;
}
if (setsockopt((int)socket.priv, SOL_SOCKET, SO_RCVTIMEO, &tv, sizeof(tv)) < 0)
{
printf("Failed to set socket option Authentification\n");
vp_com_close(COM_AUTH(), &socket);
vp_com_shutdown( COM_AUTH() );
return C_FAIL;
}
do
{
int len = strlen((char*)msg);
if(write != NULL)
{
if (VP_FAILED(write(&socket, msg, &len)))
{
vp_com_close(COM_AUTH(), &socket);
vp_com_shutdown( COM_AUTH() );
return C_FAIL;
}
}
printf("Wait authentification\n");
do
{
if(read != NULL)
{
recvStringLen = BUFFER_SIZE;
if(VP_FAILED(read(&socket, recvString, &recvStringLen)))
{
vp_com_close(COM_AUTH(), &socket);
vp_com_shutdown( COM_AUTH());
return C_FAIL;
}
}
recvString[recvStringLen] = '\0';
}
while((recvStringLen != 0) && (strcmp((char *)recvString, AUTH_MSG) == 0));
}
while((strcmp((char *)recvString, AUTH_MSG_OK) != 0) && (numretries++ < AUTH_MAX_NUMRETRIES));
if(strcmp((char*)recvString, AUTH_MSG_OK) == 0)
{
from.s_addr = socket.scn;
printf("Authentification ok from %s:%d\n", inet_ntoa(from), socket.port);
memcpy(addr, &from, sizeof(struct in_addr));
result = C_OK;
}
vp_com_close(COM_AUTH(), &socket);
vp_com_shutdown( COM_AUTH() );
return result;
}
#ifdef USE_ANDROID
vp_com_config_t* wifi_config(void)
{
static vp_com_wifi_config_t config =
{
{ 0 },
WIFI_MOBILE_IP,
WIFI_NETMASK,
WIFI_BROADCAST,
WIFI_GATEWAY,
WIFI_SERVER,
WIFI_INFRASTRUCTURE,
WIFI_SECURE,
WIFI_PASSKEY,
{ 0 },
};
unsigned char *u;
int sockfd, size = 1;
struct ifreq *ifr;
struct ifconf ifc;
struct sockaddr_in sa;
if (0 > (sockfd = socket(AF_INET, SOCK_DGRAM, IPPROTO_IP))) {
fprintf(stderr, "Cannot open socket.\n");
exit(EXIT_FAILURE);
}
ifc.ifc_len = IFRSIZE;
ifc.ifc_req = NULL;
do {
++size;
/* realloc buffer size until no overflow occurs */
if (NULL == (ifc.ifc_req = vp_os_realloc(ifc.ifc_req, IFRSIZE))) {
fprintf(stderr, "Out of memory.\n");
exit(EXIT_FAILURE);
}
ifc.ifc_len = IFRSIZE;
if (ioctl(sockfd, SIOCGIFCONF, &ifc)) {
perror("ioctl SIOCFIFCONF");
exit(EXIT_FAILURE);
}
} while (IFRSIZE <= ifc.ifc_len);
ifr = ifc.ifc_req;
for (; (char *) ifr < (char *) ifc.ifc_req + ifc.ifc_len; ++ifr) {
if (ifr->ifr_addr.sa_data == (ifr + 1)->ifr_addr.sa_data) {
continue; /* duplicate, skip it */
}
if (ioctl(sockfd, SIOCGIFFLAGS, ifr)) {
continue; /* failed to get flags, skip it */
}
struct in_addr addr;
addr = inaddrr(ifr_addr.sa_data);
if ( (ntohl(addr.s_addr) & 0xFFFFFF00) == WIFI_BASE_ADDR )
{
inet_ntop(AF_INET, &addr, config.localHost, VP_COM_NAME_MAXSIZE);
//INTERFACE
memcpy(config.itfName, ifr->ifr_name, strlen(ifr->ifr_name));
//IP ADDRESS
addr.s_addr = htonl ( ntohl(addr.s_addr) - ( ( ( ntohl(addr.s_addr) & 0xFF ) - 1 ) % 5 ) );
memcpy(config.server, inet_ntoa(addr), strlen(inet_ntoa(addr)));
//NET MASK
if (0 == ioctl(sockfd, SIOCGIFNETMASK, ifr) && strcmp(
"255.255.255.255", inet_ntoa(inaddrr(ifr_addr.sa_data)))) {
addr = inaddrr(ifr_addr.sa_data);
inet_ntop(AF_INET, &addr, config.netmask, VP_COM_NAME_MAXSIZE);
}
//BROADCAST ADDRESS
if (ifr->ifr_flags & IFF_BROADCAST) {
if (0 == ioctl(sockfd, SIOCGIFBRDADDR, ifr) && strcmp("0.0.0.0",
inet_ntoa(inaddrr(ifr_addr.sa_data)))) {
addr = inaddrr(ifr_addr.sa_data);
inet_ntop(AF_INET, &addr, config.broadcast, VP_COM_NAME_MAXSIZE);
}
}
break;
}
}
close(sockfd);
return (vp_com_config_t*)&config;
}
#else
vp_com_config_t* wifi_config(void)
{
static vp_com_wifi_config_t config =
{
{ 0 },
WIFI_MOBILE_IP,
WIFI_NETMASK,
WIFI_BROADCAST,
WIFI_GATEWAY,
WIFI_SERVER,
WIFI_INFRASTRUCTURE,
WIFI_SECURE,
WIFI_PASSKEY,
{ 0 },
};
struct ifaddrs * ifAddrStructHead = NULL;
struct ifaddrs * ifAddrStruct = NULL;
struct in_addr tmpAddr;
bool_t found = FALSE;
if(strlen(config.itfName) == 0)
{
getifaddrs(&ifAddrStruct);
ifAddrStructHead = ifAddrStruct;
while (!found && (ifAddrStruct != NULL))
{
// Looking for WIFI interface's IP address corresponding to WIFI_BASE_ADDR
if (ifAddrStruct->ifa_addr->sa_family == AF_INET)
{
tmpAddr = ((struct sockaddr_in *)ifAddrStruct->ifa_addr)->sin_addr;
if ( (ntohl(tmpAddr.s_addr) & 0xFFFFFF00) == WIFI_BASE_ADDR )
{
inet_ntop(AF_INET, &tmpAddr, config.localHost, VP_COM_NAME_MAXSIZE);
memcpy(config.itfName, ifAddrStruct->ifa_name, strlen(ifAddrStruct->ifa_name));
if(VP_FAILED(wifi_server_auth(&tmpAddr)))
tmpAddr.s_addr = htonl ( ntohl(tmpAddr.s_addr) - ( ( ( ntohl(tmpAddr.s_addr) & 0xFF ) - 1 ) % 5 ) );
memcpy(config.server, inet_ntoa(tmpAddr), strlen(inet_ntoa(tmpAddr)));
tmpAddr = ((struct sockaddr_in *)ifAddrStruct->ifa_netmask)->sin_addr;
inet_ntop(AF_INET, &tmpAddr, config.netmask, VP_COM_NAME_MAXSIZE);
tmpAddr = ((struct sockaddr_in *)ifAddrStruct->ifa_broadaddr)->sin_addr;
inet_ntop(AF_INET, &tmpAddr, config.broadcast, VP_COM_NAME_MAXSIZE);
found = TRUE;
}
}
ifAddrStruct = ifAddrStruct->ifa_next;
}
if (ifAddrStructHead != NULL)
{
freeifaddrs(ifAddrStructHead);
}
}
return (vp_com_config_t*)&config;
}
#endif
vp_com_connection_t* wifi_connection(void)
{
static vp_com_wifi_connection_t connection = {
0,
WIFI_NETWORK_NAME
};
return (vp_com_connection_t*) &connection;
}
void wifi_config_socket(vp_com_socket_t* socket, VP_COM_SOCKET_TYPE type, int32_t port, const char* serverhost)
{
vp_os_memset(socket, 0, sizeof(vp_com_socket_t));
socket->type = type;
socket->protocol = VP_COM_TCP;
socket->port = port;
if(serverhost && socket->type == VP_COM_CLIENT)
{
strncpy(socket->serverHost, serverhost, VP_COM_NAME_MAXSIZE-1);
socket->serverHost[VP_COM_NAME_MAXSIZE-1]='\0';
}
}
ARDroneLib/Soft/Lib/ardrone_tool/Com/config_wifi.h
-59
Ver Arquivo
@@ -1,59 +0,0 @@
#ifndef _CONFIG_WIFI_H_
#define _CONFIG_WIFI_H_
#ifndef _CONFIG_COM_H_
#error "You cannot include config_wifi.h. Please include config_com.h instead."
#endif
#include <VP_Com/vp_com.h>
// Each tools must implement this functions
extern vp_com_t* wifi_com(void);
extern vp_com_config_t* wifi_config(void);
extern vp_com_connection_t* wifi_connection(void);
void wifi_config_socket(vp_com_socket_t* socket, VP_COM_SOCKET_TYPE type, int32_t port, const char* serverhost);
#ifdef USE_NAVDATA_IP
#define COM_NAVDATA() wifi_com()
#define COM_CONFIG_NAVDATA() wifi_config()
#define COM_CONNECTION_NAVDATA() wifi_connection()
#define COM_CONFIG_SOCKET_NAVDATA(socket, type, opt, serverhost) wifi_config_socket(socket, type, opt, serverhost)
#endif // USE_NAVDATA_IP
#ifdef USE_AT_IP
#define COM_AT() wifi_com()
#define COM_CONFIG_AT() wifi_config()
#define COM_CONNECTION_AT() wifi_connection()
#define COM_CONFIG_SOCKET_AT(socket, type, opt, serverhost) wifi_config_socket(socket, type, opt, serverhost)
#endif // USE_NAVDATA_IP
#ifdef USE_VIDEO_IP
#define COM_VIDEO() wifi_com()
#define COM_CONFIG_VIDEO() wifi_config()
#define COM_CONNECTION_VIDEO() wifi_connection()
#define COM_CONFIG_SOCKET_VIDEO(socket, type, opt, serverhost) wifi_config_socket(socket, type, opt, serverhost)
#define COM_RAW_CAPTURE() wifi_com()
#define COM_CONFIG_RAW_CAPTURE() wifi_config()
#define COM_CONNECTION_RAW_CAPTURE() wifi_connection()
#define COM_CONFIG_SOCKET_RAW_CAPTURE(socket, type, opt, serverhost) wifi_config_socket(socket, type, opt, serverhost)
#endif // USE_VIDEO_IP
#define COM_AUTH() wifi_com()
#define COM_CONFIG_AUTH() wifi_config()
#define COM_CONNECTION_AUTH() wifi_connection()
#define COM_CONFIG_SOCKET_AUTH(socket, type, opt, serverhost) wifi_config_socket(socket, type, opt, serverhost)
#define COM_CONTROL() wifi_com()
#define COM_CONFIG_CONTROL() wifi_config()
#define COM_CONNECTION_CONTROL() wifi_connection()
#define COM_CONFIG_SOCKET_CONTROL(socket, type, opt, serverhost) wifi_config_socket(socket, type, opt, serverhost)
#define COM_REMOTE_CONSOLE() wifi_com()
#define COM_CONFIG_REMOTE_CONSOLE() wifi_config()
#define COM_CONNECTION_REMOTE_CONSOLE() wifi_connection()
#define COM_CONFIG_SOCKET_REMOTE_CONSOLE(socket, type, opt, serverhost) wifi_config_socket(socket, type, opt, serverhost)
#endif // _CONFIG_WIFI_H_
ARDroneLib/Soft/Lib/ardrone_tool/Control/ardrone_control.c
-310
Ver Arquivo
@@ -1,310 +0,0 @@
#include <config.h>
#include <VP_Os/vp_os_print.h>
#include <VP_Com/vp_com.h>
#include <ardrone_api.h>
#include <ardrone_tool/ardrone_tool.h>
#include <ardrone_tool/Control/ardrone_control.h>
#include <ardrone_tool/Control/ardrone_control_configuration.h>
#include <ardrone_tool/Control/ardrone_control_ack.h>
#include <ardrone_tool/Com/config_com.h>
#include <ardrone_tool/Com/config_wifi.h>
#ifndef _WIN32
#include <sys/socket.h>
#include <sys/ioctl.h>
#include <netinet/in.h>
#include <netinet/tcp.h>
#include <unistd.h>
#endif
static vp_com_socket_t control_socket;
static Read control_read = NULL;
static Write control_write = NULL;
static vp_os_mutex_t control_mutex;
static vp_os_cond_t control_cond;
static bool_t control_waited;
static bool_t bContinue = TRUE;
static uint32_t ardrone_state;
static int32_t start_index_in_queue;
static int32_t end_index_in_queue;
static ardrone_control_event_ptr_t ardrone_control_event_queue[ARDRONE_CONTROL_MAX_NUM_EVENTS_IN_QUEUE];
static vp_os_mutex_t event_queue_mutex;
C_RESULT ardrone_control_init(void)
{
COM_CONFIG_SOCKET_CONTROL(&control_socket, VP_COM_CLIENT, CONTROL_PORT, wifi_ardrone_ip);
control_waited = FALSE;
ardrone_state = 0;
vp_os_mutex_init(&control_mutex);
vp_os_cond_init(&control_cond, &control_mutex);
vp_os_mutex_init(&event_queue_mutex);
start_index_in_queue = 0;
end_index_in_queue = (start_index_in_queue - 1) & (ARDRONE_CONTROL_MAX_NUM_EVENTS_IN_QUEUE - 1);
return C_OK;
}
C_RESULT ardrone_control_shutdown(void)
{
ardrone_control_resume_on_navdata_received(0);
/*BUG FIX : Dont destroy the mutexes here,
they are still being used by the ardrone_control thread,
while this function is called by another thread.*/
bContinue = FALSE;
return C_OK;
}
C_RESULT ardrone_control_connect_to_drone()
{
int res_open_socket;
#ifdef _WIN32
int timeout_windows=1000;/*milliseconds*/
#else
struct timeval tv;
#endif
res_open_socket = vp_com_open(COM_CONTROL(), &control_socket, &control_read, &control_write);
if( VP_SUCCEEDED(res_open_socket) )
{
tv.tv_sec = 1;
tv.tv_usec = 0;
setsockopt((int32_t)control_socket.priv,
SOL_SOCKET,
SO_RCVTIMEO,
#ifdef _WIN32
(const char*)&timeout_windows, sizeof(timeout_windows)
#else
(const char*)&tv, sizeof(tv)
#endif
);
control_socket.is_disable = FALSE;
return C_OK;
}
else
{
DEBUG_PRINT_SDK("VP_Com : Failed to open socket for control\n");
perror("FTOSFC");
return C_FAIL;
}
}
/**
* \brief Signals the client control thread that new navdata were received.
* Called by one of the navdata callbacks.
*/
C_RESULT ardrone_control_resume_on_navdata_received(uint32_t new_ardrone_state)
{
vp_os_mutex_lock(&control_mutex);
if( control_waited )
{
ardrone_state = new_ardrone_state;
vp_os_cond_signal(&control_cond);
}
vp_os_mutex_unlock(&control_mutex);
return C_OK;
}
C_RESULT ardrone_control_read(uint8_t* buffer, int32_t* size)
{
C_RESULT res = C_FAIL;
if( control_read != NULL )
{
res = control_read(&control_socket, buffer, size);
if(*size == 0)
{
res = C_FAIL;
}
}
return res;
}
C_RESULT ardrone_control_write(const uint8_t* buffer, int32_t* size)
{
C_RESULT res = C_FAIL;
if( control_write != NULL )
{
res = control_write(&control_socket, buffer, size);
}
return res;
}
C_RESULT ardrone_control_send_event( ardrone_control_event_t* event )
{
C_RESULT res;
int32_t next_index_in_queue;
res = C_FAIL;
vp_os_mutex_lock(&event_queue_mutex);
next_index_in_queue = (start_index_in_queue + 1) & (ARDRONE_CONTROL_MAX_NUM_EVENTS_IN_QUEUE - 1);
if( next_index_in_queue != end_index_in_queue )
{
ardrone_control_event_queue[start_index_in_queue] = event;
start_index_in_queue = next_index_in_queue;
res = C_OK;
}
vp_os_mutex_unlock(&event_queue_mutex);
return res;
}
DEFINE_THREAD_ROUTINE( ardrone_control, nomParams )
{
C_RESULT res_wait_navdata = C_OK;
C_RESULT res = C_OK;
uint32_t retry, current_ardrone_state;
int32_t next_index_in_queue;
ardrone_control_event_ptr_t current_event;
retry = 0;
current_event = NULL;
DEBUG_PRINT_SDK("Thread control in progress...\n");
control_socket.is_disable = TRUE;
ardrone_control_connect_to_drone();
while( bContinue
&& !ardrone_tool_exit() )
{
vp_os_mutex_lock(&control_mutex);
control_waited = TRUE;
/* Wait for new navdata to be received. */
res_wait_navdata = vp_os_cond_timed_wait(&control_cond, 1000);
vp_os_mutex_unlock(&control_mutex);
/*
* In case of timeout on the navdata, we assume that there was a problem
* with the Wifi connection.
* It is then safer to close and reopen the control socket (TCP 5559) since
* some OS might stop giving data but not signal any disconnection.
*/
if(VP_FAILED(res_wait_navdata))
{
if (FALSE == control_socket.is_disable)
{
DEBUG_PRINT_SDK("Timeout while waiting for new navdata.\n");
vp_com_close(COM_CONTROL(), &control_socket);
control_socket.is_disable = TRUE;
}
}
else
{
if(control_socket.is_disable)
{
ardrone_control_connect_to_drone();
}
if(VP_SUCCEEDED(res_wait_navdata) && (!control_socket.is_disable))
{
vp_os_mutex_lock(&control_mutex);
current_ardrone_state = ardrone_state;
control_waited = FALSE;
vp_os_mutex_unlock(&control_mutex);
if( ardrone_tool_exit() ) // Test if we received a signal because we are quitting the application
THREAD_RETURN( res );
if( current_event == NULL )
{
vp_os_mutex_lock(&event_queue_mutex);
next_index_in_queue = (end_index_in_queue + 1) & (ARDRONE_CONTROL_MAX_NUM_EVENTS_IN_QUEUE - 1);
if( next_index_in_queue != start_index_in_queue )
{ // There's an event to process
current_event = ardrone_control_event_queue[next_index_in_queue];
if( current_event != NULL )
{
if( current_event->ardrone_control_event_start != NULL )
{
current_event->ardrone_control_event_start( current_event );
}
}
end_index_in_queue = next_index_in_queue;
retry = 0;
}
vp_os_mutex_unlock(&event_queue_mutex);
}
if( current_event != NULL )
{
switch( current_event->event )
{
case CFG_GET_CONTROL_MODE:
case CUSTOM_CFG_GET_CONTROL_MODE: /* multiconfiguration support */
res = ardrone_control_configuration_run( current_ardrone_state, (ardrone_control_configuration_event_t*) current_event );
break;
case ACK_CONTROL_MODE:
res = ardrone_control_ack_run( current_ardrone_state, (ardrone_control_ack_event_t *) current_event);
break;
default:
break;
}
if( VP_FAILED(res) )
{
retry ++;
if( retry > current_event->num_retries)
current_event->status = ARDRONE_CONTROL_EVENT_FINISH_FAILURE;
}
else
{
retry = 0;
}
if( current_event->status & ARDRONE_CONTROL_EVENT_FINISH )
{
if( current_event->ardrone_control_event_end != NULL )
current_event->ardrone_control_event_end( current_event );
/* Make the thread read a new event on the next loop iteration */
current_event = NULL;
}
else
{
/* Not changing 'current_event' makes the loop process the same
* event when the next navdata packet arrives. */
}
}
}
}
}// while
/*Bug fix - mutexes were previously detroyed by another thread,
which made ardrone_control crash.*/
vp_os_mutex_destroy(&event_queue_mutex);
vp_os_cond_destroy(&control_cond);
vp_os_mutex_destroy(&control_mutex);
vp_com_close(COM_CONTROL(), &control_socket);
THREAD_RETURN( res );
}
ARDroneLib/Soft/Lib/ardrone_tool/Control/ardrone_control.h
-46
Ver Arquivo
@@ -1,46 +0,0 @@
#ifndef _ARDRONE_CONTROL_H_
#define _ARDRONE_CONTROL_H_
#include <VP_Os/vp_os_types.h>
#include <VP_Api/vp_api_thread_helper.h>
#define ARDRONE_CONTROL_MAX_NUM_EVENTS_IN_QUEUE 32
typedef enum _ardrone_control_event_status_t {
ARDRONE_CONTROL_EVENT_IDLE = 0x0000,
ARDRONE_CONTROL_EVENT_WAITING = 0x1000,
ARDRONE_CONTROL_EVENT_IN_PROGRESS = 0x2000,
ARDRONE_CONTROL_EVENT_FINISH = 0x4000,
ARDRONE_CONTROL_EVENT_FINISH_SUCCESS = 0x4001,
ARDRONE_CONTROL_EVENT_FINISH_FAILURE = 0x4002
} ardrone_control_event_status_t;
struct _ardrone_control_event_t;
typedef void (*ardrone_control_event_cb)( struct _ardrone_control_event_t* event );
typedef struct _ardrone_control_event_t {
uint32_t event; // event type
uint32_t num_retries; // number of times we'll try to execute this event
uint32_t status; // event status
ardrone_control_event_cb ardrone_control_event_start;
ardrone_control_event_cb ardrone_control_event_end;
uint8_t data[]; // User data associated with this event
} ardrone_control_event_t, *ardrone_control_event_ptr_t;
C_RESULT ardrone_control_init(void);
C_RESULT ardrone_control_shutdown(void);
C_RESULT ardrone_control_resume_on_navdata_received(uint32_t ardrone_state);
C_RESULT ardrone_control_read(uint8_t* buffer, int32_t* size);
C_RESULT ardrone_control_write(const uint8_t* buffer, int32_t* size);
C_RESULT ardrone_control_send_event( ardrone_control_event_t* event );
PROTO_THREAD_ROUTINE( ardrone_control, nomParams );
#endif // _ARDRONE_CONTROL_H_
ARDroneLib/Soft/Lib/ardrone_tool/Control/ardrone_control_ack.c
-35
Ver Arquivo
@@ -1,35 +0,0 @@
#include <config.h>
#include <ardrone_api.h>
#include <ardrone_tool/Control/ardrone_control_ack.h>
C_RESULT ardrone_control_ack_run( uint32_t ardrone_state, ardrone_control_ack_event_t* event )
{
C_RESULT res;
switch( event->ack_state )
{
case ACK_COMMAND_MASK_TRUE:
res = ( ardrone_state & ARDRONE_COMMAND_MASK ) ? C_OK : C_FAIL;
if( VP_SUCCEEDED(res) )
{
ardrone_at_update_control_mode( ACK_CONTROL_MODE);
event->ack_state = ACK_COMMAND_MASK_FALSE;
}
break;
case ACK_COMMAND_MASK_FALSE:
ardrone_at_update_control_mode( ACK_CONTROL_MODE);
res = ( ( ardrone_state & ARDRONE_COMMAND_MASK ) == 0 ) ? C_OK : C_FAIL;
if( VP_SUCCEEDED(res) )
event->status = ARDRONE_CONTROL_EVENT_FINISH_SUCCESS;
break;
default:
res = C_FAIL;
event->ack_state = ACK_COMMAND_MASK_TRUE; // Go back to a know state (we may timeout)
break;
}
return res;
}
ARDroneLib/Soft/Lib/ardrone_tool/Control/ardrone_control_ack.h
-26
Ver Arquivo
@@ -1,26 +0,0 @@
#ifndef _ARDRONE_CONTROL_ACK_H_
#define _ARDRONE_CONTROL_ACK_H_
#include <ardrone_tool/Control/ardrone_control.h>
typedef enum _ardrone_ack_state_t {
ACK_COMMAND_MASK_TRUE, // First we are waiting command mask goes true
ACK_COMMAND_MASK_FALSE, // Then we are waiting after its reset
} ardrone_ack_state_t;
typedef struct _ardrone_control_ack_event_t {
uint32_t event; // event type
uint32_t num_retries; // number of times we'll try to execute this event
uint32_t status; // event status
ardrone_control_event_cb ardrone_control_event_start;
ardrone_control_event_cb ardrone_control_event_end;
ardrone_ack_state_t ack_state;
} ardrone_control_ack_event_t;
C_RESULT ardrone_control_ack_run( uint32_t ardrone_state, ardrone_control_ack_event_t* event );
#endif // _ARDRONE_CONTROL_CONFIGURATION_H_
ARDroneLib/Soft/Lib/ardrone_tool/Control/ardrone_control_configuration.c
-378
Ver Arquivo
@@ -1,378 +0,0 @@
#include <VP_Os/vp_os_malloc.h>
#include <VP_Os/vp_os_print.h>
#include <ardrone_api.h>
#include <ardrone_tool/Control/ardrone_control_configuration.h>
#include <config_keys.h>
static uint8_t ini_buffer[ARDRONE_CONTROL_CONFIGURATION_INI_BUFFER_SIZE];
static uint32_t ini_buffer_index = 0;
static void set_state(ardrone_control_configuration_event_t* event, ardrone_config_state_t s)
{
DEBUG_PRINT_SDK("[CONTROL CONFIGURATION] Switching to state %d\n", s);
event->config_state = s;
}
static inline void noprintf(char *s,...){}
/* Activate debugging this file */
//#define DEBUG_CONFIG_RECEIVE PRINT
#define DEBUG_CONFIG_RECEIVE noprintf
//extern const char * custom_configuration_headers[NB_CONFIG_CATEGORIES];
//extern custom_configuration_list_t available_configurations[NB_CONFIG_CATEGORIES];
void ardrone_control_reset_custom_configurations_list(custom_configuration_list_t *available_configurations)
{
int i;
for (i=0;i<NB_CONFIG_CATEGORIES;i++)
{
if (available_configurations[i].list) { vp_os_free(available_configurations[i].list); }
available_configurations[i].list=NULL;
available_configurations[i].nb_configurations=0;
}
}
void ardrone_control_read_custom_configurations_list(/*in*/uint8_t * buffer,
/*in*/int buffer_size,
/*out*/custom_configuration_list_t *available_configurations)
{
custom_configuration_list_t * current_scope = NULL;
char id[CUSTOM_CONFIGURATION_ID_LENGTH+1];
char description[1024];
int index = 0;
uint8_t * pindex;
int j;
uint8_t * end_of_buffer;
index = 0;
pindex = buffer;
end_of_buffer = buffer + buffer_size;
DEBUG_CONFIG_RECEIVE("Decoding %i bytes",buffer_size);
DEBUG_CONFIG_RECEIVE("\n");
while(1)
{
//DEBUG_CONFIG_RECEIVE("Analysing <"); for (i=index;i<buffer_size;i++) DEBUG_CONFIG_RECEIVE("[%i]",buffer[i]); DEBUG_CONFIG_RECEIVE(">\n");
/* Go to the beginning of a section */
while((*pindex)!='[') { index++; pindex++; if (pindex==end_of_buffer) return; }
/* Search the end of the section name */
for (;index<buffer_size;index++) { if (buffer[index]==13 ) { buffer[index]=0; break; } } if(index==buffer_size) return;
/* Search the corresponding category */
for (j=0;j<NB_CONFIG_CATEGORIES;j++){
if ( strcmp((char*)custom_configuration_headers[j],(char*)pindex)==0 ){
/* Found the category */
current_scope = &available_configurations[j];
DEBUG_CONFIG_RECEIVE(" Found Scope <%s>\n",custom_configuration_headers[j]);
break;
}
}
if (j==NB_CONFIG_CATEGORIES) { DEBUG_CONFIG_RECEIVE("Unknown category."); return ;}
/* Reset the list */
if (current_scope!=NULL)
{
current_scope->nb_configurations = 0;
if (current_scope->list!=NULL) { vp_os_free(current_scope->list); current_scope->list = NULL; }
}
/* Points on the first ID */
index++;
pindex=buffer+index;
/* Read the IDs */
while(pindex<end_of_buffer && (*pindex)!='[' && (*pindex)!=0)
{
vp_os_memset(id,0,sizeof(id));
vp_os_memset(description,0,sizeof(description));
//DEBUG_CONFIG_RECEIVE("Now scanning <%c> %i\n",*pindex,index);
for (;index<buffer_size;index++) { if (buffer[index]==',' || buffer[index]=='\r') { buffer[index]=0; break; } } if(index==buffer_size) return;
strncpy(id,(char*)pindex,sizeof(id));
index++;
pindex=buffer+index;
for (;index<buffer_size;index++) { if (buffer[index]==0 || buffer[index]=='\r') { buffer[index]=0; break; } } if(index==buffer_size) return;
strncpy(description,(char*)pindex,sizeof(description));
DEBUG_CONFIG_RECEIVE(" Found ID <%s> description <%s>\n",id,description);
index++;
pindex=buffer+index;
/* Store the found ID */
/* Increase the size of the list by one element */
current_scope->list = vp_os_realloc(current_scope->list,sizeof(*current_scope->list)*(current_scope->nb_configurations+1));
/* Store the new element */
strncpy(current_scope->list[current_scope->nb_configurations].id,
id,
sizeof(current_scope->list[current_scope->nb_configurations].id) );
strncpy(current_scope->list[current_scope->nb_configurations].description,
description,
sizeof(current_scope->list[current_scope->nb_configurations].description) );
current_scope->nb_configurations++;
}
}
return;
}
/**
* \brief Runs a control event managing the configuration file.
* This functions handles reading the configuration information
* sent by the drone on the TCP 'control' socket on port 5559.
* Called by the 'ardrone_control' thread loop in 'ardrone_control.c'.
*/
C_RESULT ardrone_control_configuration_run( uint32_t ardrone_state, ardrone_control_configuration_event_t* event )
{
int32_t buffer_size;
char *start_buffer, *buffer;
char *value, *param, *c;
bool_t ini_buffer_end, ini_buffer_more;
C_RESULT res = C_OK;
int bytes_to_read;
/* Multiconfiguration support */
static uint8_t *custom_configuration_list_buffer = NULL;
static int custom_configuration_list_buffer_size = 0;
static int custom_configuration_list_data_size = 0;
#define CUSTOM_CFG_BLOCK_SIZE 1024
int i;
switch( event->config_state )
{
case CONFIG_REQUEST_INI:
if( ardrone_state & ARDRONE_COMMAND_MASK )
{
/* If the ACK bit is set, we must ask the drone to clear it before asking the configuration. */
DEBUG_CONFIG_RECEIVE("%s %s %i - Requesting ack. bit reset.\n",__FILE__,__FUNCTION__,__LINE__);
ardrone_at_update_control_mode(ACK_CONTROL_MODE);
}
else
{
DEBUG_CONFIG_RECEIVE("%s %s %i - Requesting the configuration.\n",__FILE__,__FUNCTION__,__LINE__);
ini_buffer_index = 0;
ardrone_at_configuration_get_ctrl_mode();
set_state(event, CONFIG_RECEIVE_INI);
}
break;
/* multi-configuration support */
case CUSTOM_CONFIG_REQUEST:
DEBUG_CONFIG_RECEIVE("%s %s %i\n",__FILE__,__FUNCTION__,__LINE__);
if( ardrone_state & ARDRONE_COMMAND_MASK )
{
DEBUG_CONFIG_RECEIVE("%s %s %i - Requesting ack. bit reset.\n",__FILE__,__FUNCTION__,__LINE__);
ardrone_at_update_control_mode(ACK_CONTROL_MODE);
}
else
{
DEBUG_CONFIG_RECEIVE("%s %s %i - Requesting the custom config.\n",__FILE__,__FUNCTION__,__LINE__);
custom_configuration_list_buffer = NULL;
custom_configuration_list_buffer_size = 0;
custom_configuration_list_data_size = 0;
ardrone_at_custom_configuration_get_ctrl_mode();
set_state(event, CUSTOM_CONFIG_RECEIVE);
}
break;
case CONFIG_RECEIVE_INI:
DEBUG_CONFIG_RECEIVE("%s %s %i - Trying to read the control socket.\n",__FILE__,__FUNCTION__,__LINE__);
// Read data coming from ARDrone
buffer_size = ARDRONE_CONTROL_CONFIGURATION_INI_BUFFER_SIZE - ini_buffer_index;
res = ardrone_control_read( &ini_buffer[ini_buffer_index], &buffer_size );
DEBUG_CONFIG_RECEIVE("Received <<%s>>\n",&ini_buffer[ini_buffer_index]);
if(VP_SUCCEEDED(res))
{
buffer_size += ini_buffer_index;
ini_buffer[buffer_size]=0; // Makes sure the buffer ends with a zero
// Parse received data
if( buffer_size > 0 )
{
//DEBUG_CONFIG_RECEIVE(" Searching value\n");
ini_buffer_end = ini_buffer[buffer_size-1] == '\0'; // End of configuration data
ini_buffer_more = ini_buffer[buffer_size-1] != '\n'; // Need more configuration data to end parsing current line
//if (ini_buffer_end) DEBUG_CONFIG_RECEIVE(" ini_buffer_end\n");
//if (ini_buffer_more) DEBUG_CONFIG_RECEIVE(" ini_buffer_more\n");
start_buffer = (char*)&ini_buffer[0];
buffer = strchr(start_buffer, '\n');
//DEBUG_CONFIG_RECEIVE("Le start buffer : <<%s>>\n",start_buffer);
while( buffer != NULL )
{
//DEBUG_CONFIG_RECEIVE(" Found an '\\n' \n");
value = start_buffer;
param = strchr(value, '=');
*buffer = '\0';
*param = '\0';
// Remove spaces at end of strings
c = param - 1;
while( *c == ' ' )
{
*c = '\0';
c = c-1;
}
c = buffer-1;
while( *c == ' ' )
{
*c = '\0';
c = c-1;
}
// Remove spaces at beginning of strings
while( *value == ' ' )
{
value = value + 1;
}
param = param + 1;
while( *param == ' ' )
{
param = param + 1;
}
DEBUG_CONFIG_RECEIVE(" Decoding from control stream : <%s>=<%s>\n",value,param);
iniparser_setstring( event->ini_dict, value, param );
start_buffer = buffer + 1;
buffer = strchr(start_buffer, '\n');
}
if( ini_buffer_end )
{
/* Reading the condfiguration is finished, we ask the drone
* to clear the ACK bit */
ardrone_at_update_control_mode(ACK_CONTROL_MODE);
set_state(event, CONFIG_RECEIVED);
}
else if( ini_buffer_more )
{
// Compute number of bytes to copy
int32_t size = (int32_t)&ini_buffer[buffer_size] - (int32_t)start_buffer;
vp_os_memcpy( &ini_buffer[0], start_buffer, size );
ini_buffer_index = size;
}
else
{
/* The previous line was completely consumed - next data
* from the network will be stored at the beginning of the
* buffer. */
ini_buffer_index = 0;
}
}
}
else
{
res = C_FAIL;
DEBUG_CONFIG_RECEIVE("%s %s %i - no data to read from the control socket.\n",__FILE__,__FUNCTION__,__LINE__);
if(!(ardrone_state & ARDRONE_COMMAND_MASK))
set_state(event, CONFIG_REQUEST_INI);
}
break;
case CUSTOM_CONFIG_RECEIVE:
DEBUG_CONFIG_RECEIVE("%s %s %i - Trying to read the control socket.\n",__FILE__,__FUNCTION__,__LINE__);
DEBUG_CONFIG_RECEIVE("%s %s %i\n",__FILE__,__FUNCTION__,__LINE__);
/* Read data until a zero byte is received */
/* Clear old data from the buffer when receiving the first bytes */
if (custom_configuration_list_buffer!=NULL && custom_configuration_list_data_size==0){
vp_os_memset(custom_configuration_list_buffer,0,custom_configuration_list_buffer_size);
}
/* Enlarge the buffer if needed */
if (custom_configuration_list_data_size==custom_configuration_list_buffer_size)
{
custom_configuration_list_buffer_size += CUSTOM_CFG_BLOCK_SIZE;
custom_configuration_list_buffer = vp_os_realloc(custom_configuration_list_buffer,custom_configuration_list_buffer_size);
}
/* Read data at the end of the buffer */
bytes_to_read = custom_configuration_list_buffer_size - custom_configuration_list_data_size;
res = ardrone_control_read( &custom_configuration_list_buffer[custom_configuration_list_data_size], &bytes_to_read );
DEBUG_CONFIG_RECEIVE(" Reading %i bytes of the custom config. list\n",bytes_to_read);
for (i=0;i<bytes_to_read;i++) { DEBUG_CONFIG_RECEIVE("<%i>",custom_configuration_list_buffer[i]); }
/* Searches a zero */
if (VP_SUCCEEDED(res))
{
custom_configuration_list_data_size += bytes_to_read;
for (i=bytes_to_read;i>0;i--){
if (custom_configuration_list_buffer[custom_configuration_list_data_size - i] == 0) {
/* Reading the condfiguration is finished, we ask the drone
* to clear the ACK bit */
DEBUG_CONFIG_RECEIVE("Finished receiving\n");
ardrone_at_update_control_mode(ACK_CONTROL_MODE);
set_state(event, CUSTOM_CONFIG_RECEIVED);
ardrone_control_reset_custom_configurations_list(available_configurations);
ardrone_control_read_custom_configurations_list(custom_configuration_list_buffer,
custom_configuration_list_data_size,
available_configurations);
/* Clean up */
vp_os_sfree((void**)&custom_configuration_list_buffer);
custom_configuration_list_buffer_size = 0;
custom_configuration_list_data_size = 0;
res = C_OK;
break;
}
}
}
else{
/* No data are available on the control socket. */
DEBUG_CONFIG_RECEIVE("%s %s %i - no data to read from the control socket.\n",__FILE__,__FUNCTION__,__LINE__);
/* Reset the buffer */
custom_configuration_list_data_size = 0;
/* The request for the configuration file should have been acknowledged by the drone.
* If not, another request is sent. */
if(!(ardrone_state & ARDRONE_COMMAND_MASK)) set_state(event, CUSTOM_CONFIG_REQUEST);
}
break;
case CONFIG_RECEIVED:
case CUSTOM_CONFIG_RECEIVED:
/* We finished reading the configuration, we wait for the drone to reset the ACK bit */
if( ardrone_state & ARDRONE_COMMAND_MASK )
{
/* If the ACK bit is set, we must ask the drone to clear it before asking the configuration. */
DEBUG_PRINT("%s %s %i - Requesting ack. bit reset.\n",__FILE__,__FUNCTION__,__LINE__);
ardrone_at_update_control_mode(ACK_CONTROL_MODE);
}
else
{
DEBUG_PRINT("%s %s %i - Finished.\n",__FILE__,__FUNCTION__,__LINE__);
event->status = ARDRONE_CONTROL_EVENT_FINISH_SUCCESS;
}
res = C_OK;
break;
default:
res = C_FAIL;
break;
}
return res;
}
ARDroneLib/Soft/Lib/ardrone_tool/Control/ardrone_control_configuration.h
-39
Ver Arquivo
@@ -1,39 +0,0 @@
#ifndef _ARDRONE_CONTROL_CONFIGURATION_H_
#define _ARDRONE_CONTROL_CONFIGURATION_H_
#include <stdio.h>
#include <iniparser3.0b/src/iniparser.h>
#include <ardrone_tool/Control/ardrone_control.h>
// Size of temporary buffer used to parse incoming configuration data
// This value should be big enough to hold a line from the config file
#define ARDRONE_CONTROL_CONFIGURATION_INI_BUFFER_SIZE 1024
typedef enum _ardrone_config_state_t {
CONFIG_REQUEST_INI,
CONFIG_RECEIVE_INI,
/* Stephane - multiconfiguration support */
CUSTOM_CONFIG_REQUEST,
CUSTOM_CONFIG_RECEIVE,
CONFIG_RECEIVED,
CUSTOM_CONFIG_RECEIVED
} ardrone_config_state_t;
typedef struct _ardrone_control_configuration_event_t {
uint32_t event; // event type
uint32_t num_retries; // number of times we'll try to execute this event
uint32_t status; // event status
ardrone_control_event_cb ardrone_control_event_start;
ardrone_control_event_cb ardrone_control_event_end;
ardrone_config_state_t config_state;
dictionary* ini_dict;
} ardrone_control_configuration_event_t;
C_RESULT ardrone_control_configuration_run( uint32_t ardrone_state, ardrone_control_configuration_event_t* event );
#endif // _ARDRONE_CONTROL_CONFIGURATION_H_
ARDroneLib/Soft/Lib/ardrone_tool/Control/ardrone_navdata_control.c
-21
Ver Arquivo
@@ -1,21 +0,0 @@
#include <ardrone_tool/Control/ardrone_navdata_control.h>
#include <ardrone_tool/Control/ardrone_control.h>
C_RESULT ardrone_navdata_control_init( void* data )
{
return C_OK;
}
C_RESULT ardrone_navdata_control_process( const navdata_unpacked_t* const navdata )
{
/* Signal the client control thread that new navdata arrived.
* The control thread can then decide, depending on the ACK bit value,
* to send AT commands to retrieve the configuration or set a configuration parameter.
*/
return ardrone_control_resume_on_navdata_received(navdata->ardrone_state);
}
C_RESULT ardrone_navdata_control_release( void )
{
return C_OK;
}
ARDroneLib/Soft/Lib/ardrone_tool/Control/ardrone_navdata_control.h
-14
Ver Arquivo
@@ -1,14 +0,0 @@
#ifndef _ARDRONE_NAVDATA_CONTROL_H_
#define _ARDRONE_NAVDATA_CONTROL_H_
#include <ardrone_tool/Navdata/ardrone_navdata_client.h>
//
// Handler that resume ardrone control thread when new navdatas are received
//
C_RESULT ardrone_navdata_control_init( void* data );
C_RESULT ardrone_navdata_control_process( const navdata_unpacked_t* const navdata );
C_RESULT ardrone_navdata_control_release( void );
#endif // _ARDRONE_NAVDATA_CONTROL_H_
ARDroneLib/Soft/Lib/ardrone_tool/Navdata/ardrone_academy_navdata.c
-679
Ver Arquivo
@@ -1,679 +0,0 @@
#include <stdio.h>
#include <sys/time.h>
#include <ardrone_api.h>
#include <control_states.h>
#include <ardrone_tool/ardrone_tool.h>
#include <ardrone_tool/ardrone_tool_configuration.h>
#include <ardrone_tool/Academy/academy_stage_recorder.h>
#include <ardrone_tool/Academy/academy_download.h>
#include <ardrone_tool/Navdata/ardrone_navdata_client.h>
#include <ardrone_tool/Navdata/ardrone_academy_navdata.h>
#include <ardrone_tool/UI/ardrone_input.h>
#include <ardrone_tool/ardrone_version.h>
#include <ardrone_tool/Video/video_com_stage.h>
#include <ardrone_tool/Video/video_stage_encoded_recorder.h>
#include <VP_Os/vp_os_assert.h>
#include <VP_Os/vp_os_print.h>
#include <utils/ardrone_date.h>
#define TAKEOFF_TIMEOUT_SEC (5)
typedef enum
{
SCREENSHOT_STATE_IDLE,
SCREENSHOT_STATE_NEEDED,
SCREENSHOT_STATE_INPROGRESS,
} SCREENSHOT_STATE;
typedef enum
{
TAKEOFF_STATE_IDLE,
TAKEOFF_STATE_NEEDED,
TAKEOFF_STATE_WAIT_USERBOX,
TAKEOFF_STATE_CANCELLING,
} TAKEOFF_STATE;
typedef enum
{
RECORD_STATE_IDLE,
RECORD_STATE_NEEDED,
RECORD_STATE_WAIT_USERBOX,
} RECORD_STATE;
typedef enum
{
USERBOX_STATE_STARTING,
USERBOX_STATE_STARTED,
USERBOX_STATE_STOPPING,
USERBOX_STATE_STOPPED,
} USERBOX_STATE;
typedef struct
{
/**
* variable to know if setting is needed
*/
bool_t wasEmergency;
bool_t needSetEmergency;
TAKEOFF_STATE takeoff_state;
time_t takeoff_time;
RECORD_STATE record_state;
USERBOX_STATE userbox_state;
SCREENSHOT_STATE screenshot_state;
time_t userbox_time;
vp_os_mutex_t aan_mutex;
uint32_t usbFreeTime;
bool_t droneStoppedUsbRecording;
bool_t takeOffWasCancelled;
/**
* Strings to display in interface
*/
bool_t isTakeOff;
bool_t isEmergency;
uint32_t lastState;
bool_t cameraIsReady;
bool_t usbIsReady;
bool_t usbRecordInProgress;
bool_t internalRecordInProgress;
} ardrone_academy_navdata_state_t;
static const codec_type_t usbRecordCodec = MP4_360P_H264_720P_CODEC;
codec_type_t deviceWifiRecordCodec = MP4_360P_H264_720P_CODEC;
static ardrone_academy_navdata_state_t navdata_state;
#define LOCK_ND_MUTEX() vp_os_mutex_lock (&navdata_state.aan_mutex)
#define UNLOCK_ND_MUTEX() vp_os_mutex_unlock (&navdata_state.aan_mutex)
#define TRYLOCK_ND_MUTEX() vp_os_mutex_trylock (&navdata_state.aan_mutex)
bool_t ardrone_academy_navdata_takeoff(void)
{
bool_t result = FALSE;
bool_t isBatteryLow = (ARDRONE_VBAT_LOW & navdata_state.lastState) ? TRUE : FALSE;
bool_t shouldNotDo = (TRUE == isBatteryLow && FALSE == navdata_state.isTakeOff) ? TRUE : FALSE;
if(navdata_state.takeoff_state == TAKEOFF_STATE_IDLE &&
FALSE == shouldNotDo)
{
LOCK_ND_MUTEX ();
navdata_state.takeoff_state = TAKEOFF_STATE_NEEDED;
navdata_state.takeoff_time = time (NULL);
UNLOCK_ND_MUTEX ();
result = TRUE;
}
return result;
}
bool_t ardrone_academy_navdata_emergency(void)
{
bool_t result = TRUE;
LOCK_ND_MUTEX ();
navdata_state.needSetEmergency = TRUE;
UNLOCK_ND_MUTEX ();
return result;
}
bool_t ardrone_academy_navdata_record(void)
{
bool_t result = FALSE;
if(navdata_state.record_state == RECORD_STATE_IDLE)
{
LOCK_ND_MUTEX ();
navdata_state.record_state = RECORD_STATE_NEEDED;
UNLOCK_ND_MUTEX ();
result = TRUE;
}
return result;
}
bool_t ardrone_academy_navdata_screenshot(void)
{
bool_t result = FALSE;
if(navdata_state.screenshot_state == SCREENSHOT_STATE_IDLE)
{
LOCK_ND_MUTEX ();
navdata_state.screenshot_state = SCREENSHOT_STATE_NEEDED;
UNLOCK_ND_MUTEX ();
result = TRUE;
}
return result;
}
bool_t ardrone_academy_navdata_get_camera_state(void)
{
return (navdata_state.cameraIsReady && (navdata_state.screenshot_state == SCREENSHOT_STATE_IDLE));
}
bool_t ardrone_academy_navdata_get_usb_state(void)
{
return navdata_state.usbIsReady;
}
bool_t ardrone_academy_navdata_get_takeoff_state(void)
{
return navdata_state.isTakeOff;
}
bool_t ardrone_academy_navdata_get_record_ready(void)
{
bool_t result = FALSE;
if(1 == ARDRONE_VERSION())
{
result = ardrone_academy_stage_recorder_state();
}
else if(2 == ARDRONE_VERSION())
{
#if defined (RECORD_ENCODED_VIDEO)
result = (video_stage_encoded_recorder_state() || navdata_state.usbRecordInProgress);
#else
PRINT("ARDRONE VERSION 2 NEEDS RECORD_ENCODED_VIDEO MACRO!!!\n");
#endif
}
else
{
PRINT("ARDRONE VERSION NOT INITIALIZED !!!\n");
}
return result;
}
bool_t ardrone_academy_navdata_get_record_state(void)
{
bool_t result = FALSE;
if(IS_ARDRONE1)
{
result = ardrone_academy_stage_recorder_state();
}
else if(IS_ARDRONE2)
{
#if defined (RECORD_ENCODED_VIDEO)
video_encoded_record_state recState = video_stage_encoded_complete_recorder_state ();
result = navdata_state.usbRecordInProgress ||
( VIDEO_ENCODED_STOPPED != recState &&
VIDEO_ENCODED_WAITING_STREAM_END != recState);
#else
PRINT("ARDRONE VERSION 2 NEEDS RECORD_ENCODED_VIDEO MACRO!!!\n");
#endif
}
else
{
PRINT("ARDRONE VERSION NOT INITIALIZED !!!\n");
}
return result;
}
bool_t ardrone_academy_navdata_get_emergency_state(void)
{
return navdata_state.isEmergency;
}
uint32_t ardrone_academy_navdata_get_remaining_usb_time (void)
{
return navdata_state.usbFreeTime;
}
bool_t ardrone_academy_navdata_check_usb_record_status (void)
{
LOCK_ND_MUTEX ();
bool_t retVal = navdata_state.droneStoppedUsbRecording;
navdata_state.droneStoppedUsbRecording = FALSE;
UNLOCK_ND_MUTEX ();
return retVal;
}
bool_t ardrone_academy_navdata_check_takeoff_cancelled (void)
{
LOCK_ND_MUTEX ();
bool_t retVal = navdata_state.takeOffWasCancelled;
navdata_state.takeOffWasCancelled = FALSE;
UNLOCK_ND_MUTEX ();
return retVal;
}
void ardrone_academy_navdata_recorder_enable(bool_t enable, uint32_t timestamp)
{
if(1 == ARDRONE_VERSION())
{
ardrone_academy_stage_recorder_enable(enable, timestamp);
}
else if(2 == ARDRONE_VERSION())
{
#if defined (RECORD_ENCODED_VIDEO)
if (1 == enable)
{
/*
Calling this function to disable the recorder is to be made outside
a navdata handler, so we can finish a video even if the drone is
disconnected (battery removed, or out of wifi range)
The disable call is made inside the UI "button press" callbacks
*/
video_stage_encoded_recorder_enable(enable, timestamp);
}
#else
PRINT("ARDRONE VERSION 2 NEEDS RECORD_ENCODED_VIDEO MACRO!!!\n");
#endif
}
else
{
PRINT("ARDRONE VERSION NOT INITIALIZED !!!\n");
}
}
FLYING_STATE ardrone_academy_navdata_get_flying_state(const navdata_unpacked_t* data)
{
FLYING_STATE tmp_state;
switch ((data->navdata_demo.ctrl_state >> 16))
{
case CTRL_FLYING:
case CTRL_HOVERING:
case CTRL_TRANS_GOTOFIX:
case CTRL_TRANS_LOOPING:
tmp_state = FLYING_STATE_FLYING;
break;
case CTRL_TRANS_TAKEOFF:
tmp_state = FLYING_STATE_TAKING_OFF;
break;
case CTRL_TRANS_LANDING:
tmp_state = FLYING_STATE_LANDING;
break;
case CTRL_DEFAULT:
case CTRL_LANDED:
default:
tmp_state = FLYING_STATE_LANDED;
break;
}
return tmp_state;
}
void ardrone_academy_navdata_userbox_cb(bool_t result)
{
if(result)
{
LOCK_ND_MUTEX ();
if(navdata_state.userbox_state == USERBOX_STATE_STARTING)
{
PRINT("Userbox started\n");
navdata_state.userbox_state = USERBOX_STATE_STARTED;
switch (navdata_state.takeoff_state)
{
case TAKEOFF_STATE_WAIT_USERBOX:
navdata_state.takeoff_state = TAKEOFF_STATE_IDLE;
ardrone_tool_set_ui_pad_start(1);
break;
case TAKEOFF_STATE_CANCELLING:
navdata_state.takeoff_state = TAKEOFF_STATE_IDLE;
ardrone_tool_set_ui_pad_start(0);
break;
default:
break;
}
if(navdata_state.record_state == RECORD_STATE_WAIT_USERBOX)
{
navdata_state.record_state = RECORD_STATE_IDLE;
if (FALSE == navdata_state.usbRecordInProgress)
{
ardrone_academy_navdata_recorder_enable(TRUE, navdata_state.userbox_time);
}
}
}
else if(navdata_state.userbox_state == USERBOX_STATE_STOPPING)
{
PRINT("Userbox stopped\n");
academy_download_resume ();
if(navdata_state.takeoff_state == TAKEOFF_STATE_WAIT_USERBOX ||
TAKEOFF_STATE_CANCELLING == navdata_state.takeoff_state)
{
navdata_state.takeoff_state = TAKEOFF_STATE_IDLE;
ardrone_tool_set_ui_pad_start(0);
}
if(navdata_state.record_state == RECORD_STATE_WAIT_USERBOX)
{
navdata_state.record_state = RECORD_STATE_IDLE;
ardrone_academy_navdata_recorder_enable(FALSE, navdata_state.userbox_time);
navdata_state.usbRecordInProgress = FALSE;
}
navdata_state.userbox_state = USERBOX_STATE_STOPPED;
}
UNLOCK_ND_MUTEX ();
}
}
void ardrone_academy_navdata_screenshot_cb(bool_t result)
{
if(result)
{
LOCK_ND_MUTEX ();
if(navdata_state.screenshot_state == SCREENSHOT_STATE_INPROGRESS)
{
navdata_state.screenshot_state = SCREENSHOT_STATE_IDLE;
}
UNLOCK_ND_MUTEX ();
}
}
void ardrone_academy_check_take_off_timeout (void)
{
static char cancelCommand[ARDRONE_DATE_MAXSIZE];
time_t elapsedTime = time (NULL) - navdata_state.takeoff_time;
if (TAKEOFF_TIMEOUT_SEC < elapsedTime)
{
switch (navdata_state.takeoff_state)
{
case TAKEOFF_STATE_WAIT_USERBOX:
snprintf (cancelCommand, ARDRONE_DATE_MAXSIZE-1, "%d", USERBOX_CMD_CANCEL);
navdata_state.userbox_state = USERBOX_STATE_STOPPING;
navdata_state.takeoff_state = TAKEOFF_STATE_CANCELLING;
ARDRONE_TOOL_CONFIGURATION_ADDEVENT (userbox_cmd, cancelCommand, ardrone_academy_navdata_userbox_cb);
navdata_state.takeOffWasCancelled = TRUE;
break;
case TAKEOFF_STATE_NEEDED:
navdata_state.takeoff_state = TAKEOFF_STATE_IDLE;
navdata_state.takeOffWasCancelled = TRUE;
break;
default:
break;
}
}
}
void ardrone_academy_navdata_userbox_switch(void)
{
static char param[ARDRONE_DATE_MAXSIZE + 8];
static char date[ARDRONE_DATE_MAXSIZE];
if(navdata_state.userbox_state == USERBOX_STATE_STOPPED)
{
navdata_state.userbox_time = time(NULL);
ardrone_time2date(navdata_state.userbox_time, ARDRONE_FILE_DATE_FORMAT, date);
sprintf(param, "%d,%s", USERBOX_CMD_START, date);
navdata_state.userbox_state = USERBOX_STATE_STARTING;
ARDRONE_TOOL_CONFIGURATION_ADDEVENT(userbox_cmd, param, ardrone_academy_navdata_userbox_cb);
}
else if(navdata_state.userbox_state == USERBOX_STATE_STARTED)
{
sprintf(param, "%d", USERBOX_CMD_STOP);
navdata_state.userbox_state = USERBOX_STATE_STOPPING;
ARDRONE_TOOL_CONFIGURATION_ADDEVENT(userbox_cmd, param, ardrone_academy_navdata_userbox_cb);
}
}
C_RESULT ardrone_academy_navdata_init(void* data)
{
navdata_state.wasEmergency = FALSE;
navdata_state.needSetEmergency = FALSE;
navdata_state.takeoff_state = TAKEOFF_STATE_IDLE;
navdata_state.record_state = TAKEOFF_STATE_IDLE;
navdata_state.userbox_state = USERBOX_STATE_STOPPED;
navdata_state.isTakeOff = FALSE;
navdata_state.isEmergency = FALSE;
navdata_state.cameraIsReady = FALSE;
navdata_state.usbIsReady = FALSE;
navdata_state.usbRecordInProgress = FALSE;
navdata_state.userbox_time = (time_t)0;
navdata_state.takeoff_time = (time_t)0;
navdata_state.usbFreeTime = 0;
navdata_state.droneStoppedUsbRecording = FALSE;
navdata_state.takeOffWasCancelled = FALSE;
navdata_state.internalRecordInProgress = FALSE;
navdata_state.lastState = 0;
vp_os_mutex_init (&navdata_state.aan_mutex);
return C_OK;
}
C_RESULT ardrone_academy_navdata_process( const navdata_unpacked_t* const pnd )
{
static bool_t prevCameraIsReady = FALSE;
static bool_t prevDroneUsbRecording = FALSE;
if (C_OK == TRYLOCK_ND_MUTEX ())
{
input_state_t* input_state = ardrone_tool_input_get_state();
bool_t recordIsReady = ! ardrone_academy_navdata_get_record_ready();
bool_t recordIsInProgress = ardrone_academy_navdata_get_record_state();
// Save ARDrone State
navdata_state.lastState = pnd->ardrone_state;
// Save remaining USB time
navdata_state.usbFreeTime = pnd->navdata_hdvideo_stream.usbkey_remaining_time;
bool_t currentDroneUsbRecording = (NAVDATA_HDVIDEO_USBKEY_IS_RECORDING == (NAVDATA_HDVIDEO_USBKEY_IS_RECORDING & pnd->navdata_hdvideo_stream.hdvideo_state));
// Check for record stop from drone
if (navdata_state.usbRecordInProgress)
{
if (TRUE == prevDroneUsbRecording &&
FALSE == currentDroneUsbRecording)
{
navdata_state.droneStoppedUsbRecording = TRUE;
navdata_state.record_state = RECORD_STATE_NEEDED;
}
}
prevDroneUsbRecording = currentDroneUsbRecording;
ardrone_academy_check_take_off_timeout ();
// Take off and Emergency management
if(navdata_state.takeoff_state == TAKEOFF_STATE_NEEDED)
{
if(pnd->ardrone_state & ARDRONE_EMERGENCY_MASK)
{
navdata_state.needSetEmergency = TRUE;
}
else
{
if(recordIsInProgress)
{
if(!(pnd->ardrone_state & ARDRONE_USER_FEEDBACK_START))
ardrone_tool_set_ui_pad_start(1);
else
ardrone_tool_set_ui_pad_start(0);
navdata_state.takeoff_state = TAKEOFF_STATE_IDLE;
}
else
{
navdata_state.takeoff_state = TAKEOFF_STATE_WAIT_USERBOX;
ardrone_academy_navdata_userbox_switch();
}
}
}
if(navdata_state.needSetEmergency)
{
ardrone_tool_set_ui_pad_select(1);
navdata_state.needSetEmergency = FALSE;
}
if(pnd->ardrone_state & ARDRONE_EMERGENCY_MASK)
{
if(!navdata_state.wasEmergency && (input_state->user_input & (1 << ARDRONE_UI_BIT_SELECT)))
{
ardrone_tool_set_ui_pad_select(0);
}
navdata_state.isEmergency = TRUE;
navdata_state.isTakeOff = FALSE;
if(!navdata_state.internalRecordInProgress && !navdata_state.usbRecordInProgress && (navdata_state.userbox_state == USERBOX_STATE_STARTED))
{
PRINT("Emergency !! Stopping userbox...\n");
ardrone_academy_navdata_userbox_switch();
}
ardrone_tool_input_start_reset();
}
else // Not emergency landing
{
if(navdata_state.wasEmergency && (input_state->user_input & (1 << ARDRONE_UI_BIT_SELECT)))
{
ardrone_tool_set_ui_pad_select(0);
}
if(!(pnd->ardrone_state & ARDRONE_TIMER_ELAPSED))
navdata_state.isEmergency = FALSE;
if(input_state->user_input & (1 << ARDRONE_UI_BIT_START))
{
navdata_state.isTakeOff = (pnd->ardrone_state & ARDRONE_USER_FEEDBACK_START) ? TRUE : FALSE;
}
else
{
navdata_state.isTakeOff = FALSE;
}
}
navdata_state.wasEmergency = (pnd->ardrone_state & ARDRONE_EMERGENCY_MASK) ? TRUE : FALSE;
// Video record management
bool_t usbRecordEnable = FALSE;
if(navdata_state.record_state == RECORD_STATE_NEEDED &&
TAKEOFF_STATE_IDLE == navdata_state.takeoff_state)
{
bool_t continueRecord = TRUE;
bool_t nextInternalRecordState = FALSE;
if (IS_LEAST_ARDRONE2)
{
static codec_type_t oldCodec;
codec_type_t cancelCodec;
if (recordIsReady && ! navdata_state.usbRecordInProgress && !navdata_state.internalRecordInProgress) // We want to enable recording
{
if ((ARDRONE_USB_MASK == (pnd->ardrone_state & ARDRONE_USB_MASK)) &&
(0 < pnd->navdata_hdvideo_stream.usbkey_remaining_time) &&
(TRUE == ardrone_control_config.video_on_usb))
{
usbRecordEnable = TRUE;
}
// Set the "non-record codec" to the codec defined as the application default
oldCodec = ardrone_application_default_config.video_codec;
cancelCodec = oldCodec;
ardrone_control_config.video_codec = (TRUE == usbRecordEnable) ? usbRecordCodec : deviceWifiRecordCodec;
PRINT ("Set codec %d -> %d\n", oldCodec, ardrone_control_config.video_codec);
nextInternalRecordState = TRUE;
}
else // We want to disable recording
{
cancelCodec = ardrone_control_config.video_codec;
ardrone_control_config.video_codec = oldCodec;
PRINT ("Reset codec %d -> %d\n", cancelCodec, oldCodec);
}
bool_t addEventSucceeded = ARDRONE_TOOL_CONFIGURATION_ADDEVENT (video_codec, &ardrone_control_config.video_codec, NULL);
if (FALSE == addEventSucceeded)
{
PRINT ("Unable to send codec switch ... retry later\n");
ardrone_control_config.video_codec = cancelCodec;
continueRecord = FALSE;
}
}
else if (IS_ARDRONE1)
{
nextInternalRecordState = !recordIsInProgress;
}
if (TRUE == continueRecord)
{
navdata_state.internalRecordInProgress = nextInternalRecordState;
switch (navdata_state.userbox_state)
{
case USERBOX_STATE_STOPPED:
navdata_state.record_state = RECORD_STATE_WAIT_USERBOX;
navdata_state.usbRecordInProgress = usbRecordEnable;
ardrone_academy_navdata_userbox_switch();
break;
case USERBOX_STATE_STARTED:
if(navdata_state.isTakeOff)
{
if(! recordIsReady)
{
PRINT("Userbox is started and record is started => Stop record\n");
ardrone_academy_navdata_recorder_enable(FALSE, navdata_state.userbox_time);
navdata_state.usbRecordInProgress = FALSE;
}
else
{
PRINT("Userbox is started and record is stopped => Start record\n");
if (FALSE == usbRecordEnable)
{
// Only activate the local recorder if we don't record on USB
ardrone_academy_navdata_recorder_enable(TRUE, navdata_state.userbox_time);
navdata_state.usbRecordInProgress = FALSE;
}
else
{
navdata_state.usbRecordInProgress = TRUE;
}
}
navdata_state.record_state = RECORD_STATE_IDLE;
}
else
{
navdata_state.record_state = RECORD_STATE_WAIT_USERBOX;
ardrone_academy_navdata_userbox_switch();
}
break;
case USERBOX_STATE_STARTING:
navdata_state.usbRecordInProgress = usbRecordEnable;
navdata_state.record_state = RECORD_STATE_WAIT_USERBOX;
break;
case USERBOX_STATE_STOPPING:
navdata_state.usbRecordInProgress = usbRecordEnable;
// Should never be here
PRINT ("Don't know what to do for USERBOX_STATE_STOPPING\n");
VP_OS_ASSERT (0 == 1); // Debug handler
break;
}
}
}
// Screenshot management
prevCameraIsReady = navdata_state.cameraIsReady;
navdata_state.cameraIsReady = (pnd->ardrone_state & ARDRONE_CAMERA_MASK) ? TRUE : FALSE;
if (TRUE == navdata_state.cameraIsReady && FALSE == prevCameraIsReady)
{
academy_download_resume ();
}
if((navdata_state.screenshot_state == SCREENSHOT_STATE_NEEDED) && navdata_state.cameraIsReady)
{
static char param[ARDRONE_DATE_MAXSIZE + 64];
static char date[ARDRONE_DATE_MAXSIZE];
time_t t = time(NULL);
ardrone_time2date(t, ARDRONE_FILE_DATE_FORMAT, date);
navdata_state.screenshot_state = SCREENSHOT_STATE_INPROGRESS;
sprintf(param, "%d,%d,%d,%s", USERBOX_CMD_SCREENSHOT, 0, 0, date);
ARDRONE_TOOL_CONFIGURATION_ADDEVENT(userbox_cmd, param, ardrone_academy_navdata_screenshot_cb);
}
// USB management
navdata_state.usbIsReady = (pnd->ardrone_state & ARDRONE_USB_MASK) ? TRUE : FALSE;
UNLOCK_ND_MUTEX ();
}
return C_OK;
}
C_RESULT ardrone_academy_navdata_release( void )
{
if(ardrone_academy_navdata_get_record_state())
{
ardrone_academy_navdata_recorder_enable(FALSE, navdata_state.userbox_time);
}
vp_os_mutex_destroy (&navdata_state.aan_mutex);
return C_OK;
}
void ardrone_academy_navdata_set_wifi_record_codec (codec_type_t newCodec)
{
deviceWifiRecordCodec = newCodec;
}
ARDroneLib/Soft/Lib/ardrone_tool/Navdata/ardrone_academy_navdata.h
-28
Ver Arquivo
@@ -1,28 +0,0 @@
#ifndef _ARDRONE_ACADEMY_NAVDATA_H_
#define _ARDRONE_ACADEMY_NAVDATA_H_
#include <ardrone_tool/Navdata/ardrone_navdata_client.h>
#include <VLIB/video_codec.h>
C_RESULT ardrone_academy_navdata_init( void* data );
C_RESULT ardrone_academy_navdata_process( const navdata_unpacked_t* const navdata );
C_RESULT ardrone_academy_navdata_release( void );
FLYING_STATE ardrone_academy_navdata_get_flying_state(const navdata_unpacked_t* data);
bool_t ardrone_academy_navdata_takeoff(void);
bool_t ardrone_academy_navdata_record(void);
bool_t ardrone_academy_navdata_screenshot(void);
bool_t ardrone_academy_navdata_emergency(void);
bool_t ardrone_academy_navdata_get_configuration(void);
bool_t ardrone_academy_navdata_get_takeoff_state(void);
bool_t ardrone_academy_navdata_get_record_ready(void);
bool_t ardrone_academy_navdata_get_camera_state(void);
bool_t ardrone_academy_navdata_get_usb_state(void);
bool_t ardrone_academy_navdata_get_emergency_state(void);
uint32_t ardrone_academy_navdata_get_remaining_usb_time(void);
bool_t ardrone_academy_navdata_check_usb_record_status(void);
bool_t ardrone_academy_navdata_check_takeoff_cancelled(void);
void ardrone_academy_navdata_set_wifi_record_codec(codec_type_t newCodec);
#endif //! _ARDRONE_ACADEMY_NAVDATA_H_
ARDroneLib/Soft/Lib/ardrone_tool/Navdata/ardrone_general_navdata.c
-456
Ver Arquivo
@@ -1,456 +0,0 @@
#include <stdio.h>
#include <sys/time.h>
#include <ardrone_api.h>
#include <ardrone_tool/ardrone_tool.h>
#include <ardrone_tool/ardrone_tool_configuration.h>
#include <ardrone_tool/Navdata/ardrone_navdata_client.h>
#include <ardrone_tool/Navdata/ardrone_general_navdata.h>
#include <utils/ardrone_gen_ids.h>
int configWasDone = 0;
static MULTICONFIG_STATE configState;
static NAVDATA_REQUEST_STATE navdataState;
static int appSwitch = 0;
static int usrSwitch = 0;
static int sesSwitch = 0;
static int navdataNeeded = 0;
static int droneSupportsMulticonfig = 0;
#define __MULTICONFIGURATION_MIN_VERSION_MAJOR__ (1U)
#define __MULTICONFIGURATION_MIN_VERSION_MINOR__ (6U)
#define __MULTICONFIGURATION_MIN_VERSION_REVISION__ (0U)
#define __MULTICONFIGURATION_MIN_VERSION__ ((__MULTICONFIGURATION_MIN_VERSION_MAJOR__ << 16) | (__MULTICONFIGURATION_MIN_VERSION_MINOR__ << 8) | (__MULTICONFIGURATION_MIN_VERSION_REVISION__))
static inline int versionSupportsMulticonfiguration (const char *currentString)
{
uint32_t currentMajor = 0, currentMinor = 0, currentRevision = 0;
uint32_t currentVersion;
sscanf (currentString, "%d.%d.%d", &currentMajor, &currentMinor, &currentRevision);
currentVersion = ((currentMajor << 16) | (currentMinor << 8) | (currentRevision));
return currentVersion >= __MULTICONFIGURATION_MIN_VERSION__;
}
#define _GENERAL_NAVDATA_DEBUG (0)
#define _GENERAL_NAVDATA_DEBUG_PREFIX "General Navdata : "
#if _GENERAL_NAVDATA_DEBUG || defined (DEBUG)
#define PRINTDBG(...) \
do \
{ \
printf ("[%d] %s", __LINE__, _GENERAL_NAVDATA_DEBUG_PREFIX); \
printf (__VA_ARGS__); \
printf ("\n"); \
} while (0)
#else
#define PRINTDBG(...)
#endif
static void sendInitConfigurations(void)
{
}
static void switchToSession(void)
{
ARDRONE_TOOL_CONFIGURATION_ADDEVENT (session_id, ses_id, NULL);
}
static void switchToUser(void)
{
ARDRONE_TOOL_CONFIGURATION_ADDEVENT (profile_id, usr_id, NULL);
}
static void switchToApplication(void)
{
ARDRONE_TOOL_CONFIGURATION_ADDEVENT (application_id , app_id, NULL);
}
void configurationCallback (int res)
{
PRINTDBG ("Config callback called with result %d", res);
if (0 != res)
{
PRINTDBG ("State : %d", configState);
switch (configState)
{
case MULTICONFIG_IN_PROGRESS_VERSION: // got config version
PRINTDBG ("Got config file");
configState = MULTICONFIG_GOT_DRONE_VERSION;
break;
case MULTICONFIG_IN_PROGRESS_LIST: // got multiconfig ids
PRINTDBG ("Got ids list");
configState = MULTICONFIG_GOT_IDS_LIST;
break;
case MULTICONFIG_IN_PROGRESS_IDS:
PRINTDBG ("Got current ids");
configState = MULTICONFIG_GOT_CURRENT_IDS;
break;
case MULTICONFIG_IN_PROGRESS_NAVDATA:
PRINTDBG ("Got navdatas");
configState = MULTICONFIG_GOT_NAVDATA;
break;
case MULTICONFIG_IDLE:
case MULTICONFIG_GOT_IDS_LIST:
case MULTICONFIG_GOT_DRONE_VERSION:
case MULTICONFIG_GOT_CURRENT_IDS:
case MULTICONFIG_GOT_NAVDATA:
case MULTICONFIG_NEEDED:
case MULTICONFIG_REQUEST_NAVDATA:
default:
break;
}
}
PRINTDBG ("End of config callback call");
}
void navdataCallback (int res)
{
PRINTDBG ("Navdata callback called with result %d", res);
if (0 != res)
{
PRINTDBG ("State : %d", navdataState);
switch (navdataState)
{
case NAVDATA_REQUEST_IN_PROGRESS: // Navdata request got acknowledged by the AR.Drone
navdataState = NAVDATA_REQUEST_DONE;
break;
case NAVDATA_REQUEST_IDLE:
case NAVDATA_REQUEST_NEEDED:
case NAVDATA_REQUEST_DONE:
default:
break;
}
}
PRINTDBG ("End of navdata callback call");
}
C_RESULT ardrone_general_navdata_init( void* data )
{
ARDRONE_TOOL_CONFIGURATION_ADDEVENT (session_id, "-all", NULL);
navdataState = NAVDATA_REQUEST_IDLE;
configState = MULTICONFIG_NEEDED;
appSwitch = 1;
usrSwitch = 1;
sesSwitch = 1;
navdataNeeded = 1;
return C_OK;
}
C_RESULT ardrone_general_navdata_process( const navdata_unpacked_t* const pnd )
{
navdata_mode_t current_navdata_state = NAVDATA_BOOTSTRAP;
/* Makes sure the navdata stream will be resumed if the drone is disconnected and reconnected.
* Allows changing the drone battery during debugging sessions. */
if( ardrone_get_mask_from_state(pnd->ardrone_state, ARDRONE_NAVDATA_BOOTSTRAP) )
{
current_navdata_state = NAVDATA_BOOTSTRAP;
}
else
{
current_navdata_state = (ardrone_get_mask_from_state(pnd->ardrone_state, ARDRONE_NAVDATA_DEMO_MASK)) ? NAVDATA_DEMO : NAVDATA_FULL ;
}
if (current_navdata_state == NAVDATA_BOOTSTRAP && configState == MULTICONFIG_IDLE && navdataState == NAVDATA_REQUEST_IDLE)
{
if ((0 != strncmp(ardrone_control_config.application_id, app_id, APPLI_NAME_SIZE)) ||
(0 != strncmp(ardrone_control_config.profile_id, usr_id, USER_NAME_SIZE)) ||
(0 != strncmp(ardrone_control_config.session_id, ses_id, SESSION_NAME_SIZE)))
{
configState = MULTICONFIG_NEEDED;
}
else
{
navdataState = NAVDATA_REQUEST_NEEDED;
}
}
/* Multiconfig settings */
int configIndex, userNeedInit, appNeedInit;
userNeedInit = 0; appNeedInit = 0;
switch (configState)
{
case MULTICONFIG_GOT_DRONE_VERSION:
PRINTDBG ("Checking drone version ...");
// Check if drone version is >= 1.6
if (versionSupportsMulticonfiguration (ardrone_control_config.num_version_soft))
{
PRINTDBG ("Drone supports multiconfig");
configState = MULTICONFIG_IN_PROGRESS_LIST;
ARDRONE_TOOL_CUSTOM_CONFIGURATION_GET (configurationCallback);
droneSupportsMulticonfig = 1;
}
else
{
PRINTDBG ("Drone does not support multiconfig");
// Drone does not support multiconfig ... don't call init functions because we don't want to mess up things in default config
configState = MULTICONFIG_REQUEST_NAVDATA;
}
break;
case MULTICONFIG_GOT_IDS_LIST:
// At this point, we're sure that the AR.Drone supports multiconfiguration, so we'll wheck if our ids exists, and send them.
PRINTDBG ("Got AR.Drone ID list. Switch->%d,%d,%d. ND->%d", sesSwitch, usrSwitch, appSwitch, navdataNeeded);
if (1 == sesSwitch)
{
switchToSession(); // Go to session ...
}
if (1 == appSwitch)
{
if (0 != strcmp(ardrone_control_config_default.application_id, app_id)) // Check for application only if we're not asking for the default one
{
appNeedInit = 1;
for (configIndex = 0; configIndex < available_configurations[CAT_APPLI].nb_configurations; configIndex++) // Check all existing app_ids
{
PRINTDBG ("Checking application %s (desc : %s)", available_configurations[CAT_APPLI].list[configIndex].id,
available_configurations[CAT_APPLI].list[configIndex].description);
if (0 == strcmp(available_configurations[CAT_APPLI].list[configIndex].id, app_id))
{
PRINTDBG ("Found our application ... should not init");
appNeedInit = 0;
break;
}
}
switchToApplication();
}
else
{
PRINTDBG ("We're requesting default application (%s), do nothing.", app_id);
}
}
if (1 == usrSwitch)
{
if (0 != strcmp(ardrone_control_config_default.profile_id, usr_id)) // Check for user only if we're not asking for the default one
{
userNeedInit = 1;
for (configIndex = 0; configIndex < available_configurations[CAT_USER].nb_configurations; configIndex++) // Check all existing user_ids
{
PRINTDBG ("Checking user %s (desc : %s)", available_configurations[CAT_USER].list[configIndex].id,
available_configurations[CAT_USER].list[configIndex].description);
if (0 == strcmp(available_configurations[CAT_USER].list[configIndex].id, usr_id))
{
PRINTDBG ("Found our user ... should not init");
userNeedInit = 0;
break;
}
}
switchToUser();
}
else
{
PRINTDBG ("We're requesting default user (%s), do nothing.", usr_id);
}
}
if (1 == appNeedInit)
{
// Send application defined default values
ardrone_tool_send_application_default();
PRINTDBG ("Creating app. profile on AR.Drone");
ARDRONE_TOOL_CONFIGURATION_ADDEVENT (application_desc, app_name, NULL);
}
if (1 == userNeedInit)
{
// Send user defined default values
ardrone_tool_send_user_default();
PRINTDBG ("Creating usr. profile on AR.Drone");
ARDRONE_TOOL_CONFIGURATION_ADDEVENT (profile_desc, usr_name, NULL);
}
if (1 == sesSwitch)
{
if (0 != strcmp(ardrone_control_config_default.session_id, ses_id)) // Send session info only if session is not the default one
{
ARDRONE_TOOL_CONFIGURATION_ADDEVENT (session_desc, ses_name, NULL);
// Send session specific default values
ardrone_tool_send_session_default();
}
else
{
PRINTDBG ("We're requesting default session (%s), do nothing.", ses_id);
}
}
configState = MULTICONFIG_IN_PROGRESS_IDS;
ARDRONE_TOOL_CONFIGURATION_GET (configurationCallback);
break;
case MULTICONFIG_GOT_CURRENT_IDS:
if (0 != strcmp(ardrone_control_config.session_id, ses_id) ||
0 != strcmp(ardrone_control_config.profile_id, usr_id) ||
0 != strcmp(ardrone_control_config.application_id, app_id))
{
configState = MULTICONFIG_GOT_DRONE_VERSION; // We failed at setting up the application ids ... restart (but assume that drone supports multiconfig as we already checked)
}
else if (1 == navdataNeeded)
{
configState = MULTICONFIG_REQUEST_NAVDATA;
}
else
{
configState = MULTICONFIG_IDLE;
}
break;
case MULTICONFIG_NEEDED:
PRINTDBG ("Need to check multiconfig ... request config file");
// Get config file for reset
configState = MULTICONFIG_IN_PROGRESS_VERSION;
ARDRONE_TOOL_CONFIGURATION_GET (configurationCallback);
break;
case MULTICONFIG_REQUEST_NAVDATA:
PRINTDBG ("Send application navdata demo/options");
// Send application navdata demo/options to start navdatas from AR.Drone
ARDRONE_TOOL_CONFIGURATION_ADDEVENT (navdata_demo, &ardrone_application_default_config.navdata_demo, NULL);
ARDRONE_TOOL_CONFIGURATION_ADDEVENT (navdata_options, &ardrone_application_default_config.navdata_options, configurationCallback);
configState = MULTICONFIG_IN_PROGRESS_NAVDATA;
break;
case MULTICONFIG_GOT_NAVDATA:
// Refresh configuration file
configState = MULTICONFIG_IDLE;
PRINTDBG ("Refreshing from MULTICONFIG FSM");
configWasDone = 1;
ARDRONE_TOOL_CONFIGURATION_GET (NULL);
break;
case MULTICONFIG_IDLE:
case MULTICONFIG_IN_PROGRESS_LIST:
case MULTICONFIG_IN_PROGRESS_VERSION:
case MULTICONFIG_IN_PROGRESS_IDS:
default:
break;
}
/* Navdata request settings */
switch (navdataState)
{
case NAVDATA_REQUEST_NEEDED:
PRINTDBG ("Resetting navdatas to %s", (0 == ardrone_application_default_config.navdata_demo) ? "full" : "demo");
navdataState = NAVDATA_REQUEST_IN_PROGRESS;
switchToSession(); // Resend session id when reconnecting.
sendInitConfigurations();
ARDRONE_TOOL_CONFIGURATION_ADDEVENT (navdata_demo, &ardrone_application_default_config.navdata_demo, NULL);
ARDRONE_TOOL_CONFIGURATION_ADDEVENT (navdata_options, &ardrone_application_default_config.navdata_options, navdataCallback);
break;
case NAVDATA_REQUEST_DONE:
// Refresh configuration file
PRINTDBG ("Refreshing from NAVDATA FSM");
navdataState = NAVDATA_REQUEST_IDLE;
ARDRONE_TOOL_CONFIGURATION_GET (NULL);
break;
case NAVDATA_REQUEST_IN_PROGRESS:
case NAVDATA_REQUEST_IDLE:
default:
break;
}
return C_OK;
}
C_RESULT ardrone_general_navdata_release( void )
{
return C_OK;
}
/* User switch/list functions */
/* User functions */
void ardrone_refresh_user_list(void)
{
if (1 == droneSupportsMulticonfig)
{
appSwitch = 0;
usrSwitch = 0;
sesSwitch = 0;
navdataNeeded = 0;
configState = MULTICONFIG_GOT_DRONE_VERSION;
}
}
void ardrone_switch_to_user(const char *new_user)
{
if (1 == droneSupportsMulticonfig)
{
ardrone_gen_usrid (new_user, usr_id, usr_name, USER_NAME_SIZE);
appSwitch = 0;
usrSwitch = 1;
sesSwitch = 0;
navdataNeeded = 0;
configState = MULTICONFIG_GOT_DRONE_VERSION;
}
}
void ardrone_switch_to_user_id(const char *new_user_id)
{
if (1 == droneSupportsMulticonfig)
{
// We assume that the userlist is up to date
int userExists = 0;
int configIndex;
for (configIndex = 0; configIndex < available_configurations[CAT_USER].nb_configurations; configIndex++) // Check all existing user_ids
{
if (0 == strcmp(available_configurations[CAT_USER].list[configIndex].id, new_user_id))
{
userExists = 1;
break;
}
}
if (1 == userExists)
{
strncpy (usr_id, new_user_id, MULTICONFIG_ID_SIZE);
appSwitch = 0;
usrSwitch = 0;
sesSwitch = 0;
navdataNeeded = 0;
configState = MULTICONFIG_GOT_DRONE_VERSION;
}
}
}
ardrone_users_t *ardrone_get_user_list(void)
{
if (0 == droneSupportsMulticonfig)
{
return NULL;
}
ardrone_users_t *retVal = vp_os_malloc (sizeof (ardrone_users_t));
if (NULL == retVal)
return NULL;
// Assume that userlist is up to date
int validUserCount = 0; // User whose descriptions start with a dot ('.') are hidden users that may not be shown to the application user (e.g. default user for each iPhone, or user specific to a control mode
int configIndex;
for (configIndex = 0; configIndex < available_configurations[CAT_USER].nb_configurations; configIndex++) // Check all existing user_ids
{
if ('.' != available_configurations[CAT_USER].list[configIndex].description[0]) // Not an hidden user
{
validUserCount++;
retVal->userList = vp_os_realloc (retVal->userList, validUserCount * sizeof (ardrone_user_t));
if (NULL == retVal->userList)
{
vp_os_free (retVal);
retVal = NULL;
return NULL;
}
strncpy (retVal->userList[validUserCount-1].ident, available_configurations[CAT_USER].list[configIndex].id, MULTICONFIG_ID_SIZE);
strncpy (retVal->userList[validUserCount-1].description, available_configurations[CAT_USER].list[configIndex].description, USER_NAME_SIZE);
}
}
retVal->userCount = validUserCount;
return retVal;
}
void ardrone_free_user_list (ardrone_users_t **users)
{
if (NULL != *users)
{
if (NULL != (*users)->userList)
{
vp_os_free ((*users)->userList);
(*users)->userList = NULL;
}
vp_os_free (*users);
*users = NULL;
}
}
ARDroneLib/Soft/Lib/ardrone_tool/Navdata/ardrone_general_navdata.h
-36
Ver Arquivo
@@ -1,36 +0,0 @@
#ifndef _ARDRONE_NAVDATA_GENERAL_H_
#define _ARDRONE_NAVDATA_GENERAL_H_
#include <ardrone_tool/Navdata/ardrone_navdata_client.h>
typedef enum _MULTICONFIG_STATE_
{
MULTICONFIG_IDLE,
MULTICONFIG_NEEDED,
MULTICONFIG_GOT_DRONE_VERSION,
MULTICONFIG_GOT_IDS_LIST,
MULTICONFIG_GOT_CURRENT_IDS,
MULTICONFIG_GOT_NAVDATA,
MULTICONFIG_IN_PROGRESS_VERSION,
MULTICONFIG_IN_PROGRESS_LIST,
MULTICONFIG_IN_PROGRESS_IDS,
MULTICONFIG_IN_PROGRESS_NAVDATA,
MULTICONFIG_REQUEST_NAVDATA,
} MULTICONFIG_STATE;
typedef enum _NAVDATA_REQUEST_STATE_
{
NAVDATA_REQUEST_IDLE,
NAVDATA_REQUEST_NEEDED,
NAVDATA_REQUEST_IN_PROGRESS,
NAVDATA_REQUEST_DONE,
} NAVDATA_REQUEST_STATE;
C_RESULT ardrone_general_navdata_init( void* data );
C_RESULT ardrone_general_navdata_process( const navdata_unpacked_t* const navdata );
C_RESULT ardrone_general_navdata_release( void );
extern int configWasDone;
#endif //! _ARDRONE_NAVDATA_GENERAL_H_
ARDroneLib/Soft/Lib/ardrone_tool/Navdata/ardrone_navdata_client.c
-256
Ver Arquivo
@@ -1,256 +0,0 @@
#include <errno.h>
#include <string.h>
#include <config.h>
#include <VP_Os/vp_os_print.h>
#include <VP_Com/vp_com.h>
#include <ardrone_api.h>
#include <ardrone_tool/ardrone_tool.h>
#include <ardrone_tool/Navdata/ardrone_navdata_client.h>
#include <ardrone_tool/Com/config_com.h>
#ifndef _WIN32
#include <sys/socket.h>
#include <sys/ioctl.h>
#include <netinet/in.h>
#include <netinet/tcp.h>
#include <unistd.h>
#endif
static bool_t navdata_thread_in_pause = TRUE;
static bool_t bContinue = TRUE;
static uint32_t num_retries = 0;
static vp_os_cond_t navdata_client_condition;
static vp_os_mutex_t navdata_client_mutex;
static vp_com_socket_t navdata_socket;
static Read navdata_read = NULL;
static Write navdata_write = NULL;
uint8_t navdata_buffer[NAVDATA_MAX_SIZE];
navdata_unpacked_t navdata_unpacked;
C_RESULT ardrone_navdata_client_init(void)
{
C_RESULT res;
COM_CONFIG_SOCKET_NAVDATA(&navdata_socket, VP_COM_CLIENT, NAVDATA_PORT, wifi_ardrone_ip);
navdata_socket.protocol = VP_COM_UDP;
navdata_socket.is_multicast = 0; // disable multicast for Navdata
navdata_socket.multicast_base_addr = MULTICAST_BASE_ADDR;
vp_os_mutex_init(&navdata_client_mutex);
vp_os_cond_init(&navdata_client_condition, &navdata_client_mutex);
res = C_OK;
return res;
}
C_RESULT ardrone_navdata_client_suspend(void)
{
vp_os_mutex_lock(&navdata_client_mutex);
navdata_thread_in_pause = TRUE;
vp_os_mutex_unlock(&navdata_client_mutex);
return C_OK;
}
C_RESULT ardrone_navdata_client_resume(void)
{
vp_os_mutex_lock(&navdata_client_mutex);
vp_os_cond_signal(&navdata_client_condition);
navdata_thread_in_pause = FALSE;
vp_os_mutex_unlock(&navdata_client_mutex);
return C_OK;
}
C_RESULT ardrone_navdata_open_server(void)
{
// Flag value :
// 1 -> Unicast
// 2 -> Multicast
int32_t flag = 1, len = sizeof(flag);
if( navdata_write != NULL )
{
if (navdata_socket.is_multicast == 1)
flag = 2;
navdata_write(&navdata_socket, (const uint8_t*) &flag, &len);
}
return C_OK;
}
DEFINE_THREAD_ROUTINE( navdata_update, nomParams )
{
C_RESULT res;
int32_t i, size;
uint32_t cks, navdata_cks, sequence = NAVDATA_SEQUENCE_DEFAULT-1;
struct timeval tv;
#ifdef _WIN32
int timeout_for_windows=1000/*milliseconds*/;
#endif
navdata_t* navdata = (navdata_t*) &navdata_buffer[0];
tv.tv_sec = 1/*second*/;
tv.tv_usec = 0;
res = C_OK;
if( VP_FAILED(vp_com_open(COM_NAVDATA(), &navdata_socket, &navdata_read, &navdata_write)) )
{
printf("VP_Com : Failed to open socket for navdata\n");
res = C_FAIL;
}
if( VP_SUCCEEDED(res) )
{
PRINT("Thread navdata_update in progress...\n");
#ifdef _WIN32
setsockopt((int32_t)navdata_socket.priv, SOL_SOCKET, SO_RCVTIMEO, (const char*)&timeout_for_windows, sizeof(timeout_for_windows));
/* Added by Stephane to force the drone start sending data. */
if(navdata_write)
{ int sizeinit = 5; navdata_write( (void*)&navdata_socket, (int8_t*)"Init", &sizeinit ); }
#else
setsockopt((int32_t)navdata_socket.priv, SOL_SOCKET, SO_RCVTIMEO, (const char*)&tv, sizeof(tv));
#endif
i = 0;
while( ardrone_navdata_handler_table[i].init != NULL )
{
// if init failed for an handler we set its process function to null
// We keep its release function for cleanup
if( VP_FAILED( ardrone_navdata_handler_table[i].init(ardrone_navdata_handler_table[i].data) ) )
ardrone_navdata_handler_table[i].process = NULL;
i ++;
}
navdata_thread_in_pause = FALSE;
while( VP_SUCCEEDED(res)
&& !ardrone_tool_exit()
&& bContinue )
{
if(navdata_thread_in_pause)
{
vp_os_mutex_lock(&navdata_client_mutex);
num_retries = NAVDATA_MAX_RETRIES + 1;
vp_os_cond_wait(&navdata_client_condition);
vp_os_mutex_unlock(&navdata_client_mutex);
}
if( navdata_read == NULL )
{
res = C_FAIL;
continue;
}
size = NAVDATA_MAX_SIZE;
navdata->header = 0; // Soft reset
res = navdata_read( (void*)&navdata_socket, &navdata_buffer[0], &size );
#ifdef _WIN32
if( size <= 0 )
#else
if( size == 0 )
#endif
{
// timeout
PRINT("Timeout when reading navdatas - resending a navdata request on port %i\n",NAVDATA_PORT);
/* Resend a request to the drone to get navdatas */
ardrone_navdata_open_server();
sequence = NAVDATA_SEQUENCE_DEFAULT-1;
num_retries++;
}
else
num_retries = 0;
if( VP_SUCCEEDED( res ) )
{
if( navdata->header == NAVDATA_HEADER )
{
if( ardrone_get_mask_from_state(navdata->ardrone_state, ARDRONE_COM_WATCHDOG_MASK) )
{
// reset sequence number because of com watchdog
// This code is mandatory because we can have a com watchdog without detecting it on mobile side :
// Reconnection is fast enough (less than one second)
sequence = NAVDATA_SEQUENCE_DEFAULT-1;
if( ardrone_get_mask_from_state(navdata->ardrone_state, ARDRONE_NAVDATA_BOOTSTRAP) == FALSE )
ardrone_tool_send_com_watchdog(); // acknowledge
}
if( navdata->sequence > sequence )
{
i = 0;
ardrone_navdata_unpack_all(&navdata_unpacked, navdata, &navdata_cks);
cks = ardrone_navdata_compute_cks( &navdata_buffer[0], size - sizeof(navdata_cks_t) );
if( cks == navdata_cks )
{
while( ardrone_navdata_handler_table[i].init != NULL )
{
if( ardrone_navdata_handler_table[i].process != NULL )
ardrone_navdata_handler_table[i].process( &navdata_unpacked );
i++;
}
}
else
{
PRINT("[Navdata] Checksum failed : %d (distant) / %d (local)\n", navdata_cks, cks);
}
}
else
{
PRINT("[Navdata] Sequence pb : %d (distant) / %d (local)\n", navdata->sequence, sequence);
}
sequence = navdata->sequence;
}
}
}
// Release resources alllocated by handlers
i = 0;
while( ardrone_navdata_handler_table[i].init != NULL )
{
ardrone_navdata_handler_table[i].release();
i ++;
}
}
vp_com_close(COM_NAVDATA(), &navdata_socket);
DEBUG_PRINT_SDK("Thread navdata_update ended\n");
return (THREAD_RET)res;
}
uint32_t ardrone_navdata_client_get_num_retries(void)
{
return num_retries;
}
C_RESULT ardrone_navdata_client_shutdown(void)
{
bContinue = FALSE;
vp_os_mutex_lock(&navdata_client_mutex);
vp_os_cond_signal(&navdata_client_condition);
vp_os_mutex_unlock(&navdata_client_mutex);
return C_OK;
}
ARDroneLib/Soft/Lib/ardrone_tool/Navdata/ardrone_navdata_client.h
-63
Ver Arquivo
@@ -1,63 +0,0 @@
#ifndef _ARDRONE_NAVDATA_CLIENT_H_
#define _ARDRONE_NAVDATA_CLIENT_H_
#include <VP_Os/vp_os_types.h>
#include <VP_Api/vp_api_thread_helper.h>
#include <ardrone_api.h>
#include <ardrone_tool/Control/ardrone_navdata_control.h>
#include <ardrone_tool/Navdata/ardrone_navdata_file.h>
#include <ardrone_tool/Navdata/ardrone_general_navdata.h>
#include <ardrone_tool/Navdata/ardrone_academy_navdata.h>
#include <ardrone_tool/Video/video_navdata_handler.h>
#include <config.h>
#define NAVDATA_MAX_RETRIES 5
// Facility to declare a set of navdata handler
// Handler to resume control thread is mandatory
#define BEGIN_NAVDATA_HANDLER_TABLE \
ardrone_navdata_handler_t ardrone_navdata_handler_table[] = { \
{ ardrone_navdata_control_init, ardrone_navdata_control_process, ardrone_navdata_control_release, NULL }, \
{ ardrone_general_navdata_init, ardrone_general_navdata_process, ardrone_general_navdata_release, NULL }, \
{ video_navdata_handler_init, video_navdata_handler_process, video_navdata_handler_release, NULL }, \
{ ardrone_academy_navdata_init, ardrone_academy_navdata_process, ardrone_academy_navdata_release, NULL },
#define END_NAVDATA_HANDLER_TABLE \
{ NULL, NULL, NULL, NULL } \
};
#define NAVDATA_HANDLER_TABLE_ENTRY( init, process, release, init_data_ptr ) \
{ (ardrone_navdata_handler_init_t)init, process, release, init_data_ptr },
typedef C_RESULT (*ardrone_navdata_handler_init_t)( void* data );
typedef C_RESULT (*ardrone_navdata_handler_process_t)( const navdata_unpacked_t* const navdata );
typedef C_RESULT (*ardrone_navdata_handler_release_t)( void );
typedef struct _ardrone_navdata_handler_t {
ardrone_navdata_handler_init_t init;
ardrone_navdata_handler_process_t process;
ardrone_navdata_handler_release_t release;
void* data; // Data used during initialization
} ardrone_navdata_handler_t;
typedef enum
{
NAVDATA_BOOTSTRAP = 0,
NAVDATA_DEMO,
NAVDATA_FULL
} navdata_mode_t;
extern ardrone_navdata_handler_t ardrone_navdata_handler_table[] WEAK;
uint32_t ardrone_navdata_client_get_num_retries(void);
C_RESULT ardrone_navdata_client_init(void);
C_RESULT ardrone_navdata_client_suspend(void);
C_RESULT ardrone_navdata_client_resume(void);
C_RESULT ardrone_navdata_client_shutdown(void);
C_RESULT ardrone_navdata_open_server(void);
PROTO_THREAD_ROUTINE( navdata_update , nomParams );
#endif // _ARDRONE_NAVDATA_H_
ARDroneLib/Soft/Lib/ardrone_tool/Navdata/ardrone_navdata_file.c
-619
Ver Arquivo
@@ -1,619 +0,0 @@
#include <time.h>
#ifndef _WIN32
#include <sys/time.h>
#else
#include <sys/timeb.h>
#include <Winsock2.h> // for timeval structure
int gettimeofday (struct timeval *tp, void *tz)
{
struct _timeb timebuffer;
_ftime (&timebuffer);
tp->tv_sec = (long)timebuffer.time;
tp->tv_usec = (long)timebuffer.millitm * 1000;
return 0;
}
#endif
#include <stdlib.h>
#include <VP_Os/vp_os_malloc.h>
#include <VP_Os/vp_os_print.h>
#include <ardrone_tool/ardrone_tool.h>
#include <ardrone_tool/Navdata/ardrone_navdata_file.h>
#include <ardrone_tool/UI/ardrone_input.h>
uint32_t num_picture_decoded = 0;
float32_t nd_iphone_gaz = 0.0;
float32_t nd_iphone_yaw = 0.0;
int32_t nd_iphone_flag = 0;
float32_t nd_iphone_phi = 0.0;
float32_t nd_iphone_theta = 0.0;
float32_t nd_iphone_magneto_psi_accuracy = 0.0;
float32_t nd_iphone_magneto_psi = 0.0;
// Public declaration of navdata_file allowing other handlers to write into
FILE* navdata_file = NULL;
// Private declaration of navdata_file
// Allow this handler to disable other handlers that write in navdata file
static FILE* navdata_file_private = NULL;
static ardrone_navdata_file_data *navdata_file_data = NULL;
static void ardrone_navdata_file_print_version( void )
{
unsigned int i;
fprintf(navdata_file,"VERSION 19c\n"); // TODO : CHANGE VERSION NUMBER EVERY TIME THE FILE STRUCTURE CHANGES
fprintf(navdata_file,
"Control_state [-]; ARDrone_state [-]; Time [s]; nd_seq_num [-]; \
AccX_raw [LSB]; AccY_raw [LSB]; AccZ_raw [LSB]; \
GyroX_raw [LSB]; GyroY_raw [LSB]; GyroZ_raw [LSB]; GyroZIMMU3000 [LSB]; GyroX_110_raw [LSB]; GyroY_110_raw [LSB];Battery_Voltage_raw [mV]; \
Alim_3V3 [LSB]; vrefEpson [LSB]; vrefIDG [LSB]; flag_echo_ini [LSB]; us_debut_echo [LSB]; us_fin_echo [LSB]; us_association_echo; us_distance_echo [LSB];\
us_courbe_temps [LSB]; us_courbe_valeur [LSB]; us_courbe_ref [LSB]; nombre_echo [LSB]; sum_echo[LSB]; alt_temp_raw [mm];\
gradient [LSB];\
UP [LSB]; UT [LSB]; Temperature_meas [0_1C]; Pression_meas [Pa];\
Prediction_US [mm];\
Somme_inno [mm];\
Offset_Pressure [Pa];\
Estimated_altitude [mm];\
Estimated_velocity [m/s];\
Estimated_Bias_PWM [m/s2];\
Estimated_Biais_Pressure [Pa];\
Estimated_cov_alt [m];\
Estimated_offset_US [m];\
Estimated_cov_PWM;\
Bool_effet_sol [SU];\
Bool_rejet_US [SU];\
Estimated_cov_Vz [m/s];\
mx [LSB]; my [LSB]; mz [LSB];\
mx [mG]; my [mG]; mz [mG];\
mx_rect [mG]; my_rect [mG]; mz_rect [mG];\
offsetx [mG]; offsety [mG]; offsetz [mG];\
heading_unwrapped [deg]; heading_gyro_unwrapped [deg]; heading_fusion_unwrapped [mdeg];\
magneto_calibration_ok [bool];\
magneto_state [SU];\
wind_speed [m/s]; wind_angle [deg];\
wind_estimator_state_x1 [SU]; wind_estimator_state_x2 [SU]; wind_estimator_state_x3 [SU]; wind_estimator_state_x4 [SU]; wind_estimator_state_x5 [SU]; wind_estimator_state_x6 [SU];\
Phi_wind_compensation [rad]; Theta_wind_compensation [rad];\
magneto_radius [mG];\
magneto_error_mean [mG]; magneto_error_var [mG2];\
magneto_debug1 [SU]; magneto_debug2 [SU]; magneto_debug3 [SU];\
Accs_temperature [K]; Gyro_temperature [LSB]; AccX_phys_filt [mg]; AccY_phys_filt [mg]; AccZ_phys_filt [mg]; \
GyroX_phys_filt [deg/s]; GyroY_phys_filt [deg/s]; GyroZ_phys_filt [deg/s]; GyroZ_immu3000 [deg/s];\
GyroX_offset [deg/s]; GyroY_offset [deg/s]; GyroZ_offset [deg/s]; \
Theta_acc [mdeg]; Phi_acc [mdeg];\
Theta_ref_embedded [mdeg]; Phi_ref_embedded [mdeg]; Psi_ref_embedded [mdeg]; Theta_ref_int [mdeg]; Phi_ref_int [mdeg]; \
Pitch_ref_embedded [mdeg]; Roll_ref_embedded [mdeg]; Yaw_ref_embedded [mdeg/s]; \
Theta_trim_embedded [mdeg]; Phi_trim_embedded [mdeg]; Yaw_trim_embedded [mdeg/s]; \
Pitch_rc_embedded [-]; Roll_rc_embedded [-]; Yaw_rc_embedded [-]; Gaz_rc_embedded [-]; Ag_rc_embedded [-]; User_Input [-]; \
PWM1 [PWM]; PWM2 [PWM]; PWM3 [PWM]; PWM4 [PWM];\
SAT_PWM1 [PWM]; SAT_PWM2 [PWM]; SAT_PWM3 [PWM]; SAT_PWM4 [PWM];\
Gaz_feed_forward [PWM]; Gaz_altitude [PWM];\
Altitude_integral [mm/s]; Vz_ref [mm/s];\
Altitude_prop [PWM]; Altitude_der [PWM]; \
u_pitch [PWM]; u_roll [PWM]; u_yaw [PWM]; yaw_u_I [PWM];\
u_pitch_planif [PWM]; u_roll_planif [PWM]; u_yaw_planif [PWM]; u_gaz_planif [PWM];\
Current_motor1 [mA]; Current_motor2 [mA]; Current_motor3 [mA]; Current_motor4 [mA];\
Altitude_vision [mm]; Altitude_vz [mm/s]; Altitude_ref_embedded [mm]; Altitude_raw [mm]; \
Observer AccZ [m/s2]; Observer altitude US [m]; Estimated Altitude[m]; Estimated Vz [m/s]; Estimated acc bias [m/s2];\
Observer state [-]; vb 1; vb 2; Observer flight state [-];\
Vision_tx_raw [-]; Vision_ty_raw [-]; Vision_tz_raw [-]; Vision_State [-]; Vision_defined [-];\
Vision_phi_trim[rad]; Vision_phi_ref_prop[rad]; Vision_theta_trim[rad]; Vision_theta_ref_prop[rad];\
Vx_body [mm/s]; Vy_body [mm/s]; Vz_body [mm/s];\
New_raw_picture [-]; T_capture; F_capture; P_capture; delta_Phi; delta_Theta; delta_Psi; Alt_capture [-]; time_capture [s];\
Demo_vbat [-]; Demo_theta [mdeg]; Demo_phi [mdeg]; Demo_psi [mdeg]; Demo_altitude [mm]; Demo_vx [mm/s]; Demo_vy [mm/s]; Demo_vz [mm/s];Demo_num_frames [-];\
Demo_detect_rot_m11 [-]; Demo_detect_rot_m12 [-]; Demo_detect_rot_m13 [-]; Demo_detect_rot_m21 [-]; Demo_detect_rot_m22 [-]; Demo_detect_rot_m23 [-];\
Demo_detect_rot_m31 [-]; Demo_detect_rot_m32 [-]; Demo_detect_rot_m33 [-]; Demo_detect_trans_v1 [-]; Demo_detect_trans_v2 [-]; Demo_detect_trans_v3 [-]; \
Demo_detect_tag_index [-]; Demo_camera_type [-]; Demo_drone_camera_rot_m11 [-]; Demo_drone_camera_rot_m12 [-]; Demo_drone_camera_rot_m13 [-];\
Demo_drone_camera_rot_m21 [-]; Demo_drone_camera_rot_m22 [-]; Demo_drone_camera_rot_m23 [-]; Demo_drone_camera_rot_m31 [-]; Demo_drone_camera_rot_m32 [-];\
Demo_drone_camera_rot_m33 [-]; Demo_drone_camera_trans_x [-]; Demo_drone_camera_trans_y [-]; Demo_drone_camera_trans_z [-];\
nd_iphone_flag [-]; nd_iphone_phi [-]; nd_iphone_theta [-]; nd_iphone_gaz [-]; nd_iphone_yaw [-];\
quant [-]; encoded_frame_size [bytes]; encoded_frame_number [-]; atcmd_ref_seq [-]; atcmd_mean_ref_gap [ms]; atcmd_var_ref_gap [SU]; atcmd_ref_quality[-]; out_bitrate[-]; desired_bitrate[-]; tcp_queue_level[-]; fifo_queue_level[-];\
hdvideo_frame_number [-];");
/* for(i = 0 ; i < DEFAULT_NB_TRACKERS_WIDTH*DEFAULT_NB_TRACKERS_HEIGHT ; i++)
fprintf(navdata_file, "Locked_%u; X_%u; Y_%u; ", i, i, i);
*/
fprintf(navdata_file, "Nb_detected; ");
for(i = 0 ; i < 4 ; i++)
fprintf(navdata_file, "Type_%u; Xd_%u; Yd_%u; W_%u; H_%u; D_%u; O_%u; ", i, i, i, i, i, i, i);
fprintf(navdata_file, "Perf_szo [ms]; Perf_corners [ms]; Perf_compute [ms]; Perf_tracking [ms]; Perf_trans [ms]; Perf_update [ms]; ");
for(i=0; i<NAVDATA_MAX_CUSTOM_TIME_SAVE; i++)
fprintf(navdata_file, "Perf_Custom_%u; ", i);
// tags after "flag_new_picture" will be written on the IHM side
fprintf(navdata_file, "Watchdog Control [-]; flag_new_picture [-]; Sample time [s]; ");
fprintf(navdata_file, "Vx_Ref_[mm/s]; Vy_Ref_[mm/s]; Theta_modele [rad]; Phi_modele [rad]; k_v_x [-]; k_v_y [-]; k_mode [-];UI Time [-]; Theta UI [-]; Phi UI [-];Psi UI [-];Psi_accuracy UI [-]; UI_Seq;");
#ifdef PC_USE_POLARIS
fprintf( navdata_file,
"POLARIS_X [mm]; POLARIS_Y [mm]; POLARIS_Z [mm]; \
POLARIS_QX [deg]; POLARIS_QY [deg]; POLARIS_QZ [deg];\
POLARIS_Q0 [deg]; Time s [s]; Time us [us]; ");
#endif
#ifdef USE_TABLE_PILOTAGE
fprintf( navdata_file, " Table_Pilotage_position [mdeg]; Table_Pilotage_vitesse [deg/s]; ");
#endif
if((navdata_file_data != NULL) && (navdata_file_data->print_header != NULL))
navdata_file_data->print_header(navdata_file);
}
struct tm *navdata_atm = NULL;
C_RESULT ardrone_navdata_file_init( void* data )
{
char filename[1024];
struct timeval tv;
time_t temptime;
navdata_file_data = (ardrone_navdata_file_data*)data;
gettimeofday(&tv,NULL);
temptime = (time_t)tv.tv_sec;
navdata_atm = localtime(&temptime);
strcpy(filename, root_dir);
strcat(filename, "/");
if((navdata_file_data != NULL) && (navdata_file_data->filename != NULL))
{
strcpy(filename, navdata_file_data->filename);
}
else
{
sprintf(filename, "%s/mesures_%04d%02d%02d_%02d%02d%02d.txt",
filename,
navdata_atm->tm_year+1900, navdata_atm->tm_mon+1, navdata_atm->tm_mday,
navdata_atm->tm_hour, navdata_atm->tm_min, navdata_atm->tm_sec);
}
// private for instance
navdata_file_private = fopen(filename, "wb");
return navdata_file_private != NULL ? C_OK : C_FAIL;
}
C_RESULT ardrone_navdata_file_process( const navdata_unpacked_t* const pnd )
{
uint32_t i;
char str[50];
int32_t* locked_ptr;
screen_point_t* point_ptr;
struct timeval time;
input_state_t *input_state = NULL;
gettimeofday(&time,NULL);
if( navdata_file_private == NULL )
return C_FAIL;
if( ardrone_get_mask_from_state(pnd->ardrone_state, ARDRONE_NAVDATA_BOOTSTRAP) )
return C_OK;
if( navdata_file == NULL )
{
navdata_file = navdata_file_private;
if( ardrone_get_mask_from_state(pnd->ardrone_state, ARDRONE_NAVDATA_DEMO_MASK) )
{
printf("Receiving navdata demo\n");
}
else
{
printf("Receiving all navdata\n");
}
ardrone_navdata_file_print_version();
}
// Handle the case where user asked for a new navdata file
if( navdata_file != navdata_file_private )
{
fclose(navdata_file);
navdata_file = navdata_file_private;
if( ardrone_get_mask_from_state(pnd->ardrone_state, ARDRONE_NAVDATA_DEMO_MASK) )
{
printf("Receiving navdata demo\n");
}
else
{
printf("Receiving all navdata\n");
}
ardrone_navdata_file_print_version();
}
vp_os_memset(&str[0], 0, sizeof(str));
input_state = ardrone_tool_input_get_state();
fprintf( navdata_file,"\n" );
fprintf( navdata_file, "%u; %u", (unsigned int) pnd->navdata_demo.ctrl_state, (unsigned int) pnd->ardrone_state );
sprintf( str, "%d.%06d", (int)((pnd->navdata_time.time & TSECMASK) >> TSECDEC), (int)(pnd->navdata_time.time & TUSECMASK) );
fprintf( navdata_file, ";%s", str );
fprintf( navdata_file, "; %u", (unsigned int) pnd->nd_seq);
fprintf( navdata_file, "; %04u; %04u; %04u; %04d; %04d; %04d; %04d; %04u; %04u; %04u; %04u; %04u; %04u; %04u; %04u; %04u; %04u; %04u; %04u; %04u; %04u; %04u; %04u; %04d; %04d",
(unsigned int) pnd->navdata_raw_measures.raw_accs[ACC_X],
(unsigned int) pnd->navdata_raw_measures.raw_accs[ACC_Y],
(unsigned int) pnd->navdata_raw_measures.raw_accs[ACC_Z],
(int) pnd->navdata_raw_measures.raw_gyros[GYRO_X],
(int) pnd->navdata_raw_measures.raw_gyros[GYRO_Y],
(int) pnd->navdata_raw_measures.raw_gyros[GYRO_Z],
(int) pnd->navdata_zimmu_3000.vzimmuLSB,
(unsigned int) pnd->navdata_raw_measures.raw_gyros_110[0],
(unsigned int) pnd->navdata_raw_measures.raw_gyros_110[1],
(unsigned int) pnd->navdata_raw_measures.vbat_raw,
(unsigned int) pnd->navdata_phys_measures.alim3V3,
(unsigned int) pnd->navdata_phys_measures.vrefEpson,
(unsigned int) pnd->navdata_phys_measures.vrefIDG,
(unsigned int) pnd->navdata_raw_measures.flag_echo_ini,
(unsigned int) pnd->navdata_raw_measures.us_debut_echo,
(unsigned int) pnd->navdata_raw_measures.us_fin_echo,
(unsigned int) pnd->navdata_raw_measures.us_association_echo,
(unsigned int) pnd->navdata_raw_measures.us_distance_echo,
(unsigned int) pnd->navdata_raw_measures.us_courbe_temps,
(unsigned int) pnd->navdata_raw_measures.us_courbe_valeur,
(unsigned int) pnd->navdata_raw_measures.us_courbe_ref,
(unsigned int) pnd->navdata_raw_measures.nb_echo,
(unsigned int) pnd->navdata_raw_measures.sum_echo,
(int) pnd->navdata_raw_measures.alt_temp_raw,
(int) pnd->navdata_raw_measures.gradient
);
fprintf( navdata_file, "; %10u; %10u; %10u; %10u",
(signed int) pnd->navdata_pressure_raw.up,
(signed int) pnd->navdata_pressure_raw.ut,
(signed int) pnd->navdata_pressure_raw.Temperature_meas,
(signed int) pnd->navdata_pressure_raw.Pression_meas );
fprintf( navdata_file, "; %6f; %6f; %6f",
pnd->navdata_kalman_pressure.prediction_US,
pnd->navdata_kalman_pressure.somme_inno,
pnd->navdata_kalman_pressure.offset_pressure);
fprintf( navdata_file, "; %6f; %6f; %6f; %6f; %6f; %6f; %6f; %6d; %6d; %6f",
pnd->navdata_kalman_pressure.est_z,
pnd->navdata_kalman_pressure.est_zdot,
pnd->navdata_kalman_pressure.est_bias_PWM,
pnd->navdata_kalman_pressure.est_biais_pression,
pnd->navdata_kalman_pressure.cov_alt,
pnd->navdata_kalman_pressure.offset_US,
pnd->navdata_kalman_pressure.cov_PWM ,
pnd->navdata_kalman_pressure.bool_effet_sol,
pnd->navdata_kalman_pressure.flag_rejet_US,
pnd->navdata_kalman_pressure.cov_vitesse);
fprintf( navdata_file, "; %5d; %5d; %5d; %6f; %6f; %6f; %6f; %6f; %6f",
(signed int) pnd->navdata_magneto.mx,
(signed int) pnd->navdata_magneto.my,
(signed int) pnd->navdata_magneto.mz,
pnd->navdata_magneto.magneto_raw.x,
pnd->navdata_magneto.magneto_raw.y,
pnd->navdata_magneto.magneto_raw.z,
pnd->navdata_magneto.magneto_rectified.x,
pnd->navdata_magneto.magneto_rectified.y,
pnd->navdata_magneto.magneto_rectified.z);
fprintf( navdata_file, "; %6f; %6f; %6f; %6f; %6f; %6f",
pnd->navdata_magneto.magneto_offset.x,
pnd->navdata_magneto.magneto_offset.y,
pnd->navdata_magneto.magneto_offset.z,
pnd->navdata_magneto.heading_unwrapped,
pnd->navdata_magneto.heading_gyro_unwrapped,
pnd->navdata_magneto.heading_fusion_unwrapped);
fprintf( navdata_file, "; %d; %d; %6f; %6f; %6f; %6f; %6f; %6f; %6f; %6f; %6f; %6f; %6f; %6f; %6f; %6f; %6f; %6f",
pnd->navdata_magneto.magneto_calibration_ok,
pnd->navdata_magneto.magneto_state,
pnd->navdata_wind_speed.wind_speed,
pnd->navdata_wind_speed.wind_angle,
pnd->navdata_wind_speed.state_x1,
pnd->navdata_wind_speed.state_x2,
pnd->navdata_wind_speed.state_x3,
pnd->navdata_wind_speed.state_x4,
pnd->navdata_wind_speed.state_x5,
pnd->navdata_wind_speed.state_x6,
pnd->navdata_wind_speed.wind_compensation_phi,
pnd->navdata_wind_speed.wind_compensation_theta,
pnd->navdata_magneto.magneto_radius,
pnd->navdata_magneto.error_mean,
pnd->navdata_magneto.error_var,
pnd->navdata_wind_speed.magneto_debug1,
pnd->navdata_wind_speed.magneto_debug2,
pnd->navdata_wind_speed.magneto_debug3);
fprintf( navdata_file, "; %f; %04u; % 5f; % 5f; % 5f; % 6f; % 6f; % 6f; %6f",
pnd->navdata_phys_measures.accs_temp,
(unsigned int)pnd->navdata_phys_measures.gyro_temp,
pnd->navdata_phys_measures.phys_accs[ACC_X],
pnd->navdata_phys_measures.phys_accs[ACC_Y],
pnd->navdata_phys_measures.phys_accs[ACC_Z],
pnd->navdata_phys_measures.phys_gyros[GYRO_X],
pnd->navdata_phys_measures.phys_gyros[GYRO_Y],
pnd->navdata_phys_measures.phys_gyros[GYRO_Z],
pnd->navdata_zimmu_3000.vzfind );
fprintf( navdata_file, "; % f; % f; % f",
pnd->navdata_gyros_offsets.offset_g[GYRO_X],
pnd->navdata_gyros_offsets.offset_g[GYRO_Y],
pnd->navdata_gyros_offsets.offset_g[GYRO_Z] );
fprintf( navdata_file, "; % f; % f",
pnd->navdata_euler_angles.theta_a,
pnd->navdata_euler_angles.phi_a);
fprintf( navdata_file, "; %04d; %04d; %04d; %04d; %04d; %06d; %06d; %06d",
(int) pnd->navdata_references.ref_theta,
(int) pnd->navdata_references.ref_phi,
(int) pnd->navdata_references.ref_psi,
(int) pnd->navdata_references.ref_theta_I,
(int) pnd->navdata_references.ref_phi_I,
(int) pnd->navdata_references.ref_pitch,
(int) pnd->navdata_references.ref_roll,
(int) pnd->navdata_references.ref_yaw );
fprintf( navdata_file, "; % 8.6f; % 8.6f; % 8.6f",
pnd->navdata_trims.euler_angles_trim_theta,
pnd->navdata_trims.euler_angles_trim_phi,
pnd->navdata_trims.angular_rates_trim_r );
fprintf( navdata_file, "; %04d; %04d; %04d; %04d; %04d; %04u",
(int) pnd->navdata_rc_references.rc_ref_pitch,
(int) pnd->navdata_rc_references.rc_ref_roll,
(int) pnd->navdata_rc_references.rc_ref_yaw,
(int) pnd->navdata_rc_references.rc_ref_gaz,
(int) pnd->navdata_rc_references.rc_ref_ag,
(unsigned int) input_state->user_input );
fprintf( navdata_file, "; %03u; %03u; %03u; %03u; %03u; %03u; %03u; %03u",
(unsigned int) pnd->navdata_pwm.motor1,
(unsigned int) pnd->navdata_pwm.motor2,
(unsigned int) pnd->navdata_pwm.motor3,
(unsigned int) pnd->navdata_pwm.motor4,
(unsigned int) pnd->navdata_pwm.sat_motor1,
(unsigned int) pnd->navdata_pwm.sat_motor2,
(unsigned int) pnd->navdata_pwm.sat_motor3,
(unsigned int) pnd->navdata_pwm.sat_motor4);
fprintf( navdata_file, "; %6f; %6f;%6f; %6f;%6f; %6f",
pnd->navdata_pwm.gaz_feed_forward,
pnd->navdata_pwm.gaz_altitude,
pnd->navdata_pwm.altitude_integral,
pnd->navdata_pwm.vz_ref,
pnd->navdata_pwm.altitude_prop,
pnd->navdata_pwm.altitude_der);
fprintf( navdata_file, "; %03d; %03d; %03d; %f",
(int) pnd->navdata_pwm.u_pitch,
(int) pnd->navdata_pwm.u_roll,
(int) pnd->navdata_pwm.u_yaw,
pnd->navdata_pwm.yaw_u_I);
fprintf( navdata_file, "; %03d; %03d; %03d; %f",
(int) pnd->navdata_pwm.u_pitch_planif,
(int) pnd->navdata_pwm.u_roll_planif,
(int) pnd->navdata_pwm.u_yaw_planif,
pnd->navdata_pwm.u_gaz_planif);
fprintf(navdata_file, "; %04d; %04d; %04d; %04d",
(int) pnd->navdata_pwm.current_motor1,
(int) pnd->navdata_pwm.current_motor2,
(int) pnd->navdata_pwm.current_motor3,
(int) pnd->navdata_pwm.current_motor4 );
fprintf( navdata_file, "; %04d; %f; %04d;%04u",
(int) pnd->navdata_altitude.altitude_vision,
pnd->navdata_altitude.altitude_vz,
(int) pnd->navdata_altitude.altitude_ref,
(unsigned int) pnd->navdata_altitude.altitude_raw );
fprintf( navdata_file, "; %f; %f; %f; %f; %f; %04u; %f; %f; %04u",
pnd->navdata_altitude.obs_accZ,
pnd->navdata_altitude.obs_alt,
pnd->navdata_altitude.obs_x.v[0],
pnd->navdata_altitude.obs_x.v[1],
pnd->navdata_altitude.obs_x.v[2],
pnd->navdata_altitude.obs_state,
pnd->navdata_altitude.est_vb.v[0],
pnd->navdata_altitude.est_vb.v[1],
pnd->navdata_altitude.est_state );
vp_os_memset(&str[0], 0, sizeof(str));
sprintf( str, "%d.%06d", (int)((pnd->navdata_vision.time_capture & TSECMASK) >> TSECDEC), (int)(pnd->navdata_vision.time_capture & TUSECMASK) );
fprintf( navdata_file, "; % 8.6f; % 8.6f; % 8.6f; %u; %u; % f;% f;% f;% f; % f; % f; % f; %u; % f; % f; % f; % f; % f; % f; % d; %s",
pnd->navdata_vision_raw.vision_tx_raw,
pnd->navdata_vision_raw.vision_ty_raw,
pnd->navdata_vision_raw.vision_tz_raw,
(unsigned int) pnd->navdata_vision.vision_state,
(unsigned int) pnd->vision_defined,
pnd->navdata_vision.vision_phi_trim,
pnd->navdata_vision.vision_phi_ref_prop,
pnd->navdata_vision.vision_theta_trim,
pnd->navdata_vision.vision_theta_ref_prop,
pnd->navdata_vision.body_v.x,
pnd->navdata_vision.body_v.y,
pnd->navdata_vision.body_v.z,
(unsigned int) pnd->navdata_vision.new_raw_picture,
pnd->navdata_vision.theta_capture,
pnd->navdata_vision.phi_capture,
pnd->navdata_vision.psi_capture,
pnd->navdata_vision.delta_phi,
pnd->navdata_vision.delta_theta,
pnd->navdata_vision.delta_psi,
(int)pnd->navdata_vision.altitude_capture,
str );
fprintf( navdata_file, "; %04u",
(unsigned int) pnd->navdata_demo.vbat_flying_percentage );
fprintf( navdata_file, "; % f; % f; % f",
pnd->navdata_demo.theta,
pnd->navdata_demo.phi,
pnd->navdata_demo.psi );
fprintf( navdata_file, "; %04d",
(int) pnd->navdata_demo.altitude );
fprintf( navdata_file, "; %f; %f; %f ",
pnd->navdata_demo.vx,
pnd->navdata_demo.vy,
pnd->navdata_demo.vz );
fprintf( navdata_file, "; %04u", (unsigned int) pnd->navdata_demo.num_frames );
fprintf( navdata_file, "; %f; %f; %f; %f; %f; %f; %f; %f; %f", pnd->navdata_demo.detection_camera_rot.m11,
pnd->navdata_demo.detection_camera_rot.m12,
pnd->navdata_demo.detection_camera_rot.m13,
pnd->navdata_demo.detection_camera_rot.m21,
pnd->navdata_demo.detection_camera_rot.m22,
pnd->navdata_demo.detection_camera_rot.m23,
pnd->navdata_demo.detection_camera_rot.m31,
pnd->navdata_demo.detection_camera_rot.m32,
pnd->navdata_demo.detection_camera_rot.m33);
fprintf( navdata_file, "; %f; %f; %f", pnd->navdata_demo.detection_camera_trans.x,
pnd->navdata_demo.detection_camera_trans.y,
pnd->navdata_demo.detection_camera_trans.z);
fprintf( navdata_file, "; %04u; %04u",
(unsigned int) pnd->navdata_demo.detection_tag_index,
(unsigned int) pnd->navdata_demo.detection_camera_type);
fprintf( navdata_file, "; %f; %f; %f; %f; %f; %f; %f; %f; %f", pnd->navdata_demo.drone_camera_rot.m11,
pnd->navdata_demo.drone_camera_rot.m12,
pnd->navdata_demo.drone_camera_rot.m13,
pnd->navdata_demo.drone_camera_rot.m21,
pnd->navdata_demo.drone_camera_rot.m22,
pnd->navdata_demo.drone_camera_rot.m23,
pnd->navdata_demo.drone_camera_rot.m31,
pnd->navdata_demo.drone_camera_rot.m32,
pnd->navdata_demo.drone_camera_rot.m33);
fprintf( navdata_file, "; %f; %f; %f", pnd->navdata_demo.drone_camera_trans.x,
pnd->navdata_demo.drone_camera_trans.y,
pnd->navdata_demo.drone_camera_trans.z);
fprintf( navdata_file, "; %d; %f; %f; %f; %f",
(int)nd_iphone_flag,
nd_iphone_phi,
nd_iphone_theta,
nd_iphone_gaz,
nd_iphone_yaw);
/* Store information regarding the live video stream and the associated rate control */
fprintf( navdata_file, "; %d; %d; %d; %d; %d; %f; %d; %d; %d; %d; %d",
pnd->navdata_video_stream.quant,
pnd->navdata_video_stream.frame_size,
pnd->navdata_video_stream.frame_number,
pnd->navdata_video_stream.atcmd_ref_seq,
pnd->navdata_video_stream.atcmd_mean_ref_gap,
pnd->navdata_video_stream.atcmd_var_ref_gap,
pnd->navdata_video_stream.atcmd_ref_quality,
pnd->navdata_video_stream.out_bitrate,
pnd->navdata_video_stream.desired_bitrate,
pnd->navdata_video_stream.tcp_queue_level,
pnd->navdata_video_stream.fifo_queue_level
);
/* Store information regarding the HD storage stream */
fprintf( navdata_file, "; %d",
pnd->navdata_hdvideo_stream.frame_number
);
locked_ptr = (int32_t*) &pnd->navdata_trackers_send.locked[0];
point_ptr = (screen_point_t*) &pnd->navdata_trackers_send.point[0];
/*
for(i = 0; i < DEFAULT_NB_TRACKERS_WIDTH*DEFAULT_NB_TRACKERS_HEIGHT; i++)
{
fprintf( navdata_file, "; %d; %u; %u",
(int) *locked_ptr++,
(unsigned int) point_ptr->x,
(unsigned int) point_ptr->y );
point_ptr++;
}
*/
fprintf( navdata_file, "; %u", (unsigned int) pnd->navdata_vision_detect.nb_detected );
for(i = 0 ; i < 4 ; i++)
{
fprintf( navdata_file, "; %u; %u; %u; %u; %u; %u; %f",
(unsigned int) pnd->navdata_vision_detect.type[i],
(unsigned int) pnd->navdata_vision_detect.xc[i],
(unsigned int) pnd->navdata_vision_detect.yc[i],
(unsigned int) pnd->navdata_vision_detect.width[i],
(unsigned int) pnd->navdata_vision_detect.height[i],
(unsigned int) pnd->navdata_vision_detect.dist[i],
pnd->navdata_vision_detect.orientation_angle[i]);
}
fprintf( navdata_file, "; %f; %f; %f; %f; %f; %f",
pnd->navdata_vision_perf.time_szo,
pnd->navdata_vision_perf.time_corners,
pnd->navdata_vision_perf.time_compute,
pnd->navdata_vision_perf.time_tracking,
pnd->navdata_vision_perf.time_trans,
pnd->navdata_vision_perf.time_update );
for(i = 0 ; i < NAVDATA_MAX_CUSTOM_TIME_SAVE ; i++)
{
fprintf( navdata_file, "; %f", pnd->navdata_vision_perf.time_custom[i]);
}
fprintf( navdata_file, "; %d", (int) pnd->navdata_watchdog.watchdog );
fprintf( navdata_file, "; %u", (unsigned int) num_picture_decoded );
sprintf( str, "%d.%06d", (int)time.tv_sec, (int)time.tv_usec);
fprintf( navdata_file, "; %s", str );
fprintf( navdata_file, ";%f;%f;%f;%f;%f;%f;%d;%f;%f;%f;%f;%f;%d",
(float) pnd->navdata_references.vx_ref,
(float) pnd->navdata_references.vy_ref,
(float) pnd->navdata_references.theta_mod,
(float) pnd->navdata_references.phi_mod,
(float) pnd->navdata_references.k_v_x,
(float) pnd->navdata_references.k_v_y,
(int) pnd->navdata_references.k_mode,
(float) pnd->navdata_references.ui_time,
(float) pnd->navdata_references.ui_theta,
(float) pnd->navdata_references.ui_phi,
(float) pnd->navdata_references.ui_psi,
(float) pnd->navdata_references.ui_psi_accuracy,
(int) pnd->navdata_references.ui_seq);
if((navdata_file_data != NULL) && (navdata_file_data->print != NULL))
navdata_file_data->print(navdata_file);
return C_OK;
}
C_RESULT ardrone_navdata_file_release( void )
{
if( navdata_file != NULL )
{
navdata_file = NULL;
fprintf(navdata_file_private,"\n");
fclose( navdata_file_private );
navdata_file_private = NULL;
navdata_file_data = NULL;
}
return C_OK;
}
ARDroneLib/Soft/Lib/ardrone_tool/Navdata/ardrone_navdata_file.h
-27
Ver Arquivo
@@ -1,27 +0,0 @@
#ifndef _ARDRONE_NAVDATA_FILE_H_
#define _ARDRONE_NAVDATA_FILE_H_
#include <stdio.h>
#include <ardrone_api.h>
#include <ardrone_tool/Navdata/ardrone_navdata_client.h>
extern FILE* navdata_file;
typedef void (*ardrone_navdata_custom_print_header)(FILE *navdata_file);
typedef void (*ardrone_navdata_custom_print)(FILE *navdata_file);
typedef struct _ardrone_navdata_file_data_
{
const char *filename;
ardrone_navdata_custom_print_header print_header;
ardrone_navdata_custom_print print;
} ardrone_navdata_file_data;
// For this handler, data is the path where the file will be created
// If data is NULL then the file is created in current directory
C_RESULT ardrone_navdata_file_init( void* data );
C_RESULT ardrone_navdata_file_process( const navdata_unpacked_t* const navdata );
C_RESULT ardrone_navdata_file_release( void );
#endif // _ARDRONE_NAVDATA_FILE_H_
ARDroneLib/Soft/Lib/ardrone_tool/Navdata/navdata.c
-151
Ver Arquivo
@@ -1,151 +0,0 @@
#include <VP_Os/vp_os_types.h>
#include <VP_Os/vp_os_malloc.h>
#include <VP_Os/vp_os_print.h>
#include <ardrone_api.h>
/* Uncomment to activate interesting printf's */
//#define DEBUG_NAVDATA_C
/********************************************************************
* @fn ardrone_navdata_compute_cks:
* @param nv Data to calculate the checksum.
* @param size Size of data calculate as follow : size-sizeof(navdata_cks_t).
* @return Retrieves the checksum from the navdata nv.
*******************************************************************/
uint32_t ardrone_navdata_compute_cks( uint8_t* nv, int32_t size )
{
int32_t i;
uint32_t cks;
uint32_t temp;
cks = 0;
for( i = 0; i < size; i++ )
{
temp = nv[i];
cks += temp;
}
return cks;
}
/********************************************************************
* @fn ardrone_navdata_search_option:
* @param navdata_options_ptr
* @param tag ID of the bloc to search for.
* @brief Jumps to a specified 'option' (block of navdata) inside
* a navdata packed buffer.
*******************************************************************/
navdata_option_t* ardrone_navdata_search_option( navdata_option_t* navdata_options_ptr, navdata_tag_t tag )
{
uint8_t* ptr;
while( navdata_options_ptr->tag != tag )
{
ptr = (uint8_t*) navdata_options_ptr;
ptr += navdata_options_ptr->size;
if (0 == navdata_options_ptr->size)
{
navdata_options_ptr = NULL;
break;
}
navdata_options_ptr = (navdata_option_t*) ptr;
}
return navdata_options_ptr;
}
/********************************************************************
* ardrone_navdata_unpack_all:
* @param navdata_unpacked navdata_unpacked in which to store the navdata.
* @param navdata One packet read from the port NAVDATA.
* @param Checksum of navdata
* @brief Disassembles a buffer of received navdata, and dispatches
* it inside 'navdata_unpacked' structure.
* @DESCRIPTION
*
*******************************************************************/
C_RESULT ardrone_navdata_unpack_all(navdata_unpacked_t* navdata_unpacked, navdata_t* navdata, uint32_t* cks)
{
C_RESULT res;
navdata_cks_t navdata_cks = { 0 };
navdata_option_t* navdata_option_ptr;
navdata_option_ptr = (navdata_option_t*) &navdata->options[0];
vp_os_memset( navdata_unpacked, 0, sizeof(*navdata_unpacked) );
navdata_unpacked->nd_seq = navdata->sequence;
navdata_unpacked->ardrone_state = navdata->ardrone_state;
navdata_unpacked->vision_defined = navdata->vision_defined;
res = C_OK;
#ifdef DEBUG_NAVDATA_C
if (navdata_unpacked->ardrone_state & ARDRONE_COMMAND_MASK) { printf("[ACK]"); }
printf("Received navdatas tags :");
#endif
while( navdata_option_ptr != NULL )
{
// Check if we have a valid option
if( navdata_option_ptr->size == 0 )
{
PRINT("One option (%d) is not a valid option because its size is zero\n", navdata_option_ptr->tag);
navdata_option_ptr = NULL;
res = C_FAIL;
}
else
{
if( navdata_option_ptr->tag <= NAVDATA_NUM_TAGS){
#ifdef DEBUG_NAVDATA_C
printf("[%d]",navdata_option_ptr->tag);
#endif
navdata_unpacked->last_navdata_refresh |= NAVDATA_OPTION_MASK(navdata_option_ptr->tag);
}
switch( navdata_option_ptr->tag )
{
#define NAVDATA_OPTION(STRUCTURE,NAME,TAG) \
case TAG: \
navdata_option_ptr = ardrone_navdata_unpack( navdata_option_ptr, navdata_unpacked->NAME); \
break;
#define NAVDATA_OPTION_DEMO(STRUCTURE,NAME,TAG) NAVDATA_OPTION(STRUCTURE,NAME,TAG)
#define NAVDATA_OPTION_CKS(STRUCTURE,NAME,TAG) {}
#include <navdata_keys.h>
case NAVDATA_CKS_TAG:
navdata_option_ptr = ardrone_navdata_unpack( navdata_option_ptr, navdata_cks );
*cks = navdata_cks.cks;
navdata_option_ptr = NULL; // End of structure
break;
default:
PRINT("Tag %d is an unknown navdata option tag\n", (int) navdata_option_ptr->tag);
navdata_option_ptr = (navdata_option_t *)(((uint32_t)navdata_option_ptr) + navdata_option_ptr->size);
break;
}
}
}
#ifdef DEBUG_NAVDATA_C
printf("\n");
#endif
return res;
}
ARDroneLib/Soft/Lib/ardrone_tool/UI/ardrone_input.c
-330
Ver Arquivo
@@ -1,330 +0,0 @@
#include <VP_Os/vp_os_print.h>
#include <VP_Os/vp_os_assert.h>
#include <ardrone_tool/UI/ardrone_input.h>
#include <ardrone_tool/ardrone_version.h>
static input_device_t* devices_list[MAX_NUM_DEVICES];
static input_state_t input_state = { 0 };
C_RESULT ardrone_tool_input_add( input_device_t* device )
{
C_RESULT res;
int32_t i;
VP_OS_ASSERT( device != NULL );
res = C_FAIL;
i = 0;
while( i < MAX_NUM_DEVICES && devices_list[i] != NULL ) i++;
if( i < MAX_NUM_DEVICES )
{
if( VP_SUCCEEDED(device->init()) )
{
devices_list[i] = device;
PRINT("Input device %s added\n", device->name);
res = C_OK;
}
else
{
PRINT("Input device %s init failed\n", device->name);
res = C_FAIL;
}
}
else
{
PRINT("Not enough memory to add input device %s\n", device->name);
}
return res;
}
static C_RESULT ardrone_tool_input_remove_i( uint32_t i )
{
C_RESULT res;
res = C_OK;
if( i < MAX_NUM_DEVICES )
{
if( devices_list[i] != NULL )
{
if( VP_SUCCEEDED(devices_list[i]->shutdown()) )
{
PRINT("Input device %s removed\n", devices_list[i]->name);
res = C_OK;
}
else
{
PRINT("Input device %s removed but an error occured during its shutdown\n", devices_list[i]->name);
res = C_FAIL;
}
devices_list[i] = NULL;
}
} else {
res = C_FAIL;
PRINT("Input device index out of range\n");
}
return res;
}
C_RESULT ardrone_tool_input_remove( input_device_t* device )
{
C_RESULT res;
uint32_t i;
VP_OS_ASSERT( device != NULL );
res = C_FAIL;
i = 0;
while( i < MAX_NUM_DEVICES && devices_list[i] != device ) i++;
if( i < MAX_NUM_DEVICES )
{
res = ardrone_tool_input_remove_i(i);
}
else
{
DEBUG_PRINT_SDK("Input %s not found while removing\n", device->name);
}
return res;
}
C_RESULT ardrone_tool_set_ui_pad_ab(int32_t value)
{
if( value )
input_state.user_input |= (1 << ARDRONE_UI_BIT_AB);
else
input_state.user_input &= ~(1 << ARDRONE_UI_BIT_AB);
return C_OK;
}
C_RESULT ardrone_tool_set_ui_pad_ag(int32_t value)
{
if( value )
input_state.user_input |= (1 << ARDRONE_UI_BIT_AG);
else
input_state.user_input &= ~(1 << ARDRONE_UI_BIT_AG);
return C_OK;
}
C_RESULT ardrone_tool_set_ui_pad_ad(int32_t value)
{
if( value )
input_state.user_input |= (1 << ARDRONE_UI_BIT_AD);
else
input_state.user_input &= ~(1 << ARDRONE_UI_BIT_AD);
return C_OK;
}
C_RESULT ardrone_tool_set_ui_pad_ah(int32_t value)
{
if( value )
input_state.user_input |= (1 << ARDRONE_UI_BIT_AH);
else
input_state.user_input &= ~(1 << ARDRONE_UI_BIT_AH);
return C_OK;
}
C_RESULT ardrone_tool_set_ui_pad_l1(int32_t value)
{
if( value )
input_state.user_input |= (1 << ARDRONE_UI_BIT_L1);
else
input_state.user_input &= ~(1 << ARDRONE_UI_BIT_L1);
return C_OK;
}
C_RESULT ardrone_tool_set_ui_pad_r1(int32_t value)
{
if( value )
input_state.user_input |= (1 << ARDRONE_UI_BIT_R1);
else
input_state.user_input &= ~(1 << ARDRONE_UI_BIT_R1);
return C_OK;
}
C_RESULT ardrone_tool_set_ui_pad_l2(int32_t value)
{
if( value )
input_state.user_input |= (1 << ARDRONE_UI_BIT_L2);
else
input_state.user_input &= ~(1 << ARDRONE_UI_BIT_L2);
return C_OK;
}
C_RESULT ardrone_tool_set_ui_pad_r2(int32_t value)
{
if( value )
input_state.user_input |= (1 << ARDRONE_UI_BIT_R2);
else
input_state.user_input &= ~(1 << ARDRONE_UI_BIT_R2);
return C_OK;
}
C_RESULT ardrone_tool_set_ui_pad_select(int32_t value)
{
if( value )
input_state.user_input |= (1 << ARDRONE_UI_BIT_SELECT);
else
input_state.user_input &= ~(1 << ARDRONE_UI_BIT_SELECT);
return C_OK;
}
C_RESULT ardrone_tool_set_ui_pad_start(int32_t value)
{
if( value )
input_state.user_input |= (1 << ARDRONE_UI_BIT_START);
else
input_state.user_input &= ~(1 << ARDRONE_UI_BIT_START);
return C_OK;
}
C_RESULT ardrone_tool_set_ui_pad_xy(int32_t x, int32_t y)
{
input_state.user_input &= ~(3 << ARDRONE_UI_BIT_X);
input_state.user_input &= ~(3 << ARDRONE_UI_BIT_Y);
input_state.user_input |= (x + 1) << ARDRONE_UI_BIT_X;
input_state.user_input |= (y + 1) << ARDRONE_UI_BIT_Y;
return C_OK;
}
C_RESULT ardrone_tool_set_ui_pad_phi_trim( int32_t phi_trim )
{
input_state.user_input &= ~(3 << ARDRONE_UI_BIT_TRIM_PHI);
input_state.user_input |= (phi_trim + 1) << ARDRONE_UI_BIT_TRIM_PHI;
return C_OK;
}
C_RESULT ardrone_tool_set_ui_pad_theta_trim( int32_t theta_trim )
{
input_state.user_input &= ~(3 << ARDRONE_UI_BIT_TRIM_THETA);
input_state.user_input |= (theta_trim + 1) << ARDRONE_UI_BIT_TRIM_THETA;
return C_OK;
}
C_RESULT ardrone_tool_set_ui_pad_yaw_trim( int32_t yaw_trim )
{
input_state.user_input &= ~(3 << ARDRONE_UI_BIT_TRIM_YAW);
input_state.user_input |= (yaw_trim + 1) << ARDRONE_UI_BIT_TRIM_YAW;
return C_OK;
}
C_RESULT ardrone_tool_set_progressive_cmd(int32_t flag, float32_t phi, float32_t theta, float32_t gaz, float32_t yaw, float32_t psi, float32_t psi_accuracy)
{
input_state.pcmd.flag = flag;
input_state.pcmd.phi = phi;
input_state.pcmd.theta = theta;
input_state.pcmd.gaz = gaz;
input_state.pcmd.yaw = yaw;
input_state.pcmd.psi = psi;
input_state.pcmd.psi_accuracy = psi_accuracy;
return C_OK;
}
C_RESULT ardrone_tool_input_init(void)
{
int32_t i;
i = 0;
while( i < MAX_NUM_DEVICES )
{
devices_list[i] = NULL;
i++;
}
return ardrone_tool_input_reset();
}
C_RESULT ardrone_tool_input_reset(void)
{
vp_os_memset(&input_state, 0, sizeof(input_state_t));
ardrone_tool_set_ui_pad_phi_trim(0);
ardrone_tool_set_ui_pad_yaw_trim(0);
ardrone_tool_set_ui_pad_theta_trim(0);
ardrone_tool_set_ui_pad_xy(0,0);
return C_OK;
}
C_RESULT ardrone_tool_input_start_reset(void)
{
ardrone_tool_set_ui_pad_start(0);
return C_OK;
}
C_RESULT ardrone_tool_input_update(void)
{
C_RESULT res;
uint32_t i;
res = C_OK;
i = 0;
while( VP_SUCCEEDED(res) && i < MAX_NUM_DEVICES )
{
if( devices_list[i] != NULL && VP_FAILED(devices_list[i]->update()) )
{
PRINT("Input device %s update failed... it'll be removed\n", devices_list[i]->name);
ardrone_tool_input_remove_i(i);
res = C_FAIL;
}
i++;
}
if(IS_ARDRONE1)
{
ardrone_at_set_progress_cmd(input_state.pcmd.flag, input_state.pcmd.phi, input_state.pcmd.theta, input_state.pcmd.gaz, input_state.pcmd.yaw);
}
else
{
ardrone_at_set_progress_cmd_with_magneto(input_state.pcmd.flag, input_state.pcmd.phi, input_state.pcmd.theta, input_state.pcmd.gaz, input_state.pcmd.yaw, input_state.pcmd.psi, input_state.pcmd.psi_accuracy);
}
ardrone_at_set_ui_pad_value( input_state.user_input );
return res;
}
C_RESULT ardrone_tool_input_shutdown(void)
{
uint32_t i;
i = 0;
while( i < MAX_NUM_DEVICES )
{
ardrone_tool_input_remove_i(i);
i++;
}
return C_OK;
}
input_state_t* ardrone_tool_input_get_state( void )
{
return &input_state;
}
ARDroneLib/Soft/Lib/ardrone_tool/UI/ardrone_input.h
-63
Ver Arquivo
@@ -1,63 +0,0 @@
#ifndef _ARDRONE_INPUT_H_
#define _ARDRONE_INPUT_H_
#include <VP_Os/vp_os_types.h>
#include <ardrone_tool/ardrone_tool.h>
typedef struct _input_device_t {
char name[MAX_NAME_LENGTH];
C_RESULT (*init)(void);
C_RESULT (*update)(void);
C_RESULT (*shutdown)(void);
} input_device_t;
typedef struct _input_state_pcmd_t_
{
int32_t flag;
float32_t phi;
float32_t theta;
float32_t gaz;
float32_t yaw;
float32_t psi;
float32_t psi_accuracy;
} input_state_pcmd_t;
typedef struct _input_state_t
{
uint32_t user_input;
input_state_pcmd_t pcmd;
} input_state_t;
// Input change handling
C_RESULT ardrone_tool_set_ui_pad_ag(int32_t value);
C_RESULT ardrone_tool_set_ui_pad_ab(int32_t value);
C_RESULT ardrone_tool_set_ui_pad_ad(int32_t value);
C_RESULT ardrone_tool_set_ui_pad_ah(int32_t value);
C_RESULT ardrone_tool_set_ui_pad_l1(int32_t value);
C_RESULT ardrone_tool_set_ui_pad_r1(int32_t value);
C_RESULT ardrone_tool_set_ui_pad_l2(int32_t value);
C_RESULT ardrone_tool_set_ui_pad_r2(int32_t value);
C_RESULT ardrone_tool_set_ui_pad_select(int32_t value);
C_RESULT ardrone_tool_set_ui_pad_start(int32_t value);
C_RESULT ardrone_tool_set_ui_pad_xy(int32_t x, int32_t y);
C_RESULT ardrone_tool_set_ui_pad_phi_trim( int32_t phi_trim );
C_RESULT ardrone_tool_set_ui_pad_theta_trim( int32_t theta_trim );
C_RESULT ardrone_tool_set_ui_pad_yaw_trim( int32_t yaw_trim );
C_RESULT ardrone_tool_set_progressive_cmd(int32_t flag, float32_t phi, float32_t theta, float32_t gaz, float32_t yaw, float32_t psi, float32_t psi_accuracy);
input_state_t* ardrone_tool_input_get_state( void );
// Input API
C_RESULT ardrone_tool_input_add( input_device_t* device );
C_RESULT ardrone_tool_input_remove( input_device_t* device );
C_RESULT ardrone_tool_input_init(void);
C_RESULT ardrone_tool_input_reset(void);
C_RESULT ardrone_tool_input_update(void);
C_RESULT ardrone_tool_input_shutdown(void);
C_RESULT ardrone_tool_input_start_reset(void);
#endif // _ARDRONE_INPUT_H_
ARDroneLib/Soft/Lib/ardrone_tool/Video/buffer_to_picture_stage.c
-142
Ver Arquivo
@@ -1,142 +0,0 @@
#include <VP_Os/vp_os_malloc.h>
#include <VP_Os/vp_os_print.h>
#include <config.h>
#include <ardrone_tool/Video/buffer_to_picture_stage.h>
static int32_t copy_input_to_buffer( uint8_t* buffer, int32_t input_size, int32_t max_size, buffer_to_picture_config_t *cfg )
{
int32_t size_to_copy;
size_to_copy = input_size;
if( size_to_copy > max_size )
size_to_copy = max_size;
vp_os_memcpy( buffer, cfg->input_ptr, size_to_copy );
cfg->cumulated_size += size_to_copy;
cfg->input_ptr += size_to_copy;
return size_to_copy;
}
C_RESULT buffer_to_picture_open(buffer_to_picture_config_t *cfg)
{
cfg->num_picture_decoded = 0;
return C_OK;
}
C_RESULT buffer_to_picture_transform(buffer_to_picture_config_t *cfg, vp_api_io_data_t *in, vp_api_io_data_t *out)
{
vp_os_mutex_lock(&out->lock);
if(out->status == VP_API_STATUS_INIT)
{
out->numBuffers = 1;
out->buffers = (int8_t**)(int8_t*) cfg->picture;
out->indexBuffer = 0;
out->lineSize = 0;
out->status = VP_API_STATUS_PROCESSING;
cfg->y_buf_ptr = cfg->picture->y_buf;
#ifdef USE_VIDEO_YUV
cfg->cr_buf_ptr = cfg->picture->cr_buf;
cfg->cb_buf_ptr = cfg->picture->cb_buf;
#endif
cfg->cumulated_size = 0;
cfg->input_ptr = NULL;
}
if( in->status == VP_API_STATUS_ENDED )
out->status = in->status;
if( out->status == VP_API_STATUS_PROCESSING )
cfg->input_ptr = (uint8_t*)in->buffers[in->indexBuffer];
if(out->status == VP_API_STATUS_PROCESSING || out->status == VP_API_STATUS_STILL_RUNNING)
{
int32_t copied_size, y_size, c_size = 0;
// If out->size == 1 it means picture is ready
out->size = 0;
out->status = VP_API_STATUS_PROCESSING;
y_size = cfg->y_blockline_size;
#ifdef USE_VIDEO_YUV
c_size = cfg->y_blockline_size / 4;
#endif
while(in->size > 0 && cfg->y_current_size != cfg->y_buffer_size)
{
if( in->size > 0 && cfg->cumulated_size < y_size )
{
copied_size = copy_input_to_buffer( cfg->y_buf_ptr, in->size, y_size - cfg->cumulated_size, cfg );
cfg->y_buf_ptr += copied_size;
in->size -= copied_size;
}
#ifdef USE_VIDEO_YUV
if( in->size > 0 && cfg->cumulated_size >= y_size && cfg->cumulated_size < y_size + c_size )
{
copied_size = copy_input_to_buffer( cfg->cb_buf_ptr, in->size, y_size + c_size - cfg->cumulated_size, cfg );
cfg->cb_buf_ptr += copied_size;
in->size -= copied_size;
}
if( in->size > 0 && cfg->cumulated_size >= y_size + c_size && cfg->cumulated_size < y_size + 2*c_size )
{
copied_size = copy_input_to_buffer( cfg->cr_buf_ptr, in->size, y_size + 2*c_size - cfg->cumulated_size, cfg );
cfg->cr_buf_ptr += copied_size;
in->size -= copied_size;
}
#endif
if( cfg->cumulated_size == y_size + 2*c_size )
{
cfg->cumulated_size = 0;
cfg->y_current_size += cfg->y_blockline_size;
}
}
// All buffers are full but there's still data
if( in->size > 0 )
out->status = VP_API_STATUS_STILL_RUNNING;
if( cfg->y_current_size == cfg->y_buffer_size )
{
// we got one picture (handle case 1)
out->size = 1;
cfg->num_picture_decoded++;
DEBUG_PRINT_SDK( "%d picture received\n", (int)cfg->num_picture_decoded );
cfg->y_current_size = 0;
cfg->y_buf_ptr = cfg->picture->y_buf;
#ifdef USE_VIDEO_YUV
cfg->cr_buf_ptr = cfg->picture->cr_buf;
cfg->cb_buf_ptr = cfg->picture->cb_buf;
#endif
}
}
vp_os_mutex_unlock(&out->lock);
return C_OK;
}
C_RESULT buffer_to_picture_close(buffer_to_picture_config_t *cfg)
{
return C_OK;
}
const vp_api_stage_funcs_t buffer_to_picture_funcs =
{
NULL,
(vp_api_stage_open_t)buffer_to_picture_open,
(vp_api_stage_transform_t)buffer_to_picture_transform,
(vp_api_stage_close_t)buffer_to_picture_close
};
ARDroneLib/Soft/Lib/ardrone_tool/Video/buffer_to_picture_stage.h
-35
Ver Arquivo
@@ -1,35 +0,0 @@
#ifndef _BUFFER_TO_PICTURE_H_
#define _BUFFER_TO_PICTURE_H_
#include <VP_Api/vp_api.h>
#include <VP_Api/vp_api_picture.h>
typedef struct _buffer_to_picture_config_t
{
vp_api_picture_t* picture;
int32_t y_buffer_size;
int32_t y_blockline_size;
int32_t y_current_size;
int32_t num_frames;
uint8_t* y_buf_ptr;
#ifdef USE_VIDEO_YUV
uint8_t* cr_buf_ptr;
uint8_t* cb_buf_ptr;
#endif
int32_t cumulated_size;
uint8_t* input_ptr;
int32_t num_picture_decoded;
} buffer_to_picture_config_t;
C_RESULT buffer_to_picture_open(buffer_to_picture_config_t *cfg);
C_RESULT buffer_to_picture_transform(buffer_to_picture_config_t *cfg, vp_api_io_data_t *in, vp_api_io_data_t *out);
C_RESULT buffer_to_picture_close(buffer_to_picture_config_t *cfg);
extern const vp_api_stage_funcs_t buffer_to_picture_funcs;
#endif // _BUFFER_TO_PICTURE_H_
ARDroneLib/Soft/Lib/ardrone_tool/Video/mjpeg_stage_decode.c
-98
Ver Arquivo
@@ -1,98 +0,0 @@
#include <config.h>
#include <VP_Os/vp_os_print.h>
#include <ardrone_tool/Video/mjpeg_stage_decode.h>
uint32_t mjpeg_stage_num_picture_decoded = 0;
const vp_api_stage_funcs_t mjpeg_decoding_funcs = {
(vp_api_stage_handle_msg_t) NULL,
(vp_api_stage_open_t) mjpeg_stage_decoding_open,
(vp_api_stage_transform_t) mjpeg_stage_decoding_transform,
(vp_api_stage_close_t) mjpeg_stage_decoding_close
};
C_RESULT mjpeg_stage_decoding_open(mjpeg_stage_decoding_config_t *cfg)
{
stream_new( &cfg->stream, OUTPUT_STREAM );
return mjpeg_init( &cfg->mjpeg, MJPEG_DECODE, cfg->picture->width, cfg->picture->height, cfg->picture->format );
}
C_RESULT mjpeg_stage_decoding_transform(mjpeg_stage_decoding_config_t *cfg, vp_api_io_data_t *in, vp_api_io_data_t *out)
{
bool_t got_image;
vp_os_mutex_lock( &out->lock );
if(out->status == VP_API_STATUS_INIT)
{
out->numBuffers = 1;
out->buffers = (int8_t**) cfg->picture;
out->indexBuffer = 0;
out->lineSize = 0;
out->status = VP_API_STATUS_PROCESSING;
}
if( in->status == VP_API_STATUS_ENDED ) {
out->status = in->status;
}
// Several cases must be handled in this stage
// 1st: Input buffer is too small to decode a complete picture
// 2nd: Input buffer is big enough to decode 1 frame
// 3rd: Input buffer is so big we can decode more than 1 frame
if( out->status == VP_API_STATUS_PROCESSING )
{
// Reinit stream with new data
stream_config( &cfg->stream, in->size, in->buffers[in->indexBuffer] );
}
if(out->status == VP_API_STATUS_PROCESSING || out->status == VP_API_STATUS_STILL_RUNNING)
{
// If out->size == 1 it means picture is ready
out->size = 0;
out->status = VP_API_STATUS_PROCESSING;
mjpeg_decode( &cfg->mjpeg, cfg->picture, &cfg->stream, &got_image );
if( got_image )
{
// we got one picture (handle case 1)
out->size = 1;
mjpeg_stage_num_picture_decoded = cfg->mjpeg.num_frames;
#ifndef USE_VIDEO_YUV
int32_t i;
for(i = 0; i < cfg->picture->width * cfg->picture->height / 4; i++ )
{
cfg->picture->cr_buf[i] = 0x80;
cfg->picture->cb_buf[i] = 0x80;
}
#endif
// handle case 2 & 3
if( FAILED(stream_is_empty( &cfg->stream )) )
{
// Some data are still in stream
// Next time we run this stage we don't want this data to be lost
// So flag it!
out->status = VP_API_STATUS_STILL_RUNNING;
}
}
}
vp_os_mutex_unlock( &out->lock );
return C_OK;
}
C_RESULT mjpeg_stage_decoding_close(mjpeg_stage_decoding_config_t *cfg)
{
stream_delete( &cfg->stream );
return mjpeg_release( &cfg->mjpeg );
}
ARDroneLib/Soft/Lib/ardrone_tool/Video/mjpeg_stage_decode.h
-24
Ver Arquivo
@@ -1,24 +0,0 @@
#ifndef _MJPEG_STAGE_DECODE_H_
#define _MJPEG_STAGE_DECODE_H_
#include <VP_Api/vp_api.h>
#include <MJPEG/mjpeg.h>
typedef struct _mjpeg_stage_decoding_config_t
{
stream_t stream;
mjpeg_t mjpeg;
vp_api_picture_t* picture;
uint32_t out_buffer_size;
} mjpeg_stage_decoding_config_t;
C_RESULT mjpeg_stage_decoding_open(mjpeg_stage_decoding_config_t *cfg);
C_RESULT mjpeg_stage_decoding_transform(mjpeg_stage_decoding_config_t *cfg, vp_api_io_data_t *in, vp_api_io_data_t *out);
C_RESULT mjpeg_stage_decoding_close(mjpeg_stage_decoding_config_t *cfg);
extern uint32_t mjpeg_stage_num_picture_decoded;
extern const vp_api_stage_funcs_t mjpeg_decoding_funcs;
#endif // _MJPEG_STAGE_DECODE_H_
ARDroneLib/Soft/Lib/ardrone_tool/Video/video_com_stage.c
-618
Ver Arquivo
@@ -1,618 +0,0 @@
#include <config.h>
#include <VP_Os/vp_os_print.h>
#include <VP_Os/vp_os_malloc.h>
#include <VP_Os/vp_os_delay.h>
#include <VP_Os/vp_os_assert.h>
#include <ardrone_tool/Video/video_com_stage.h>
#include <VP_Com/vp_com_socket.h>
#ifndef _WIN32
#include <sys/socket.h>
#include <sys/ioctl.h>
#include <netinet/in.h>
#include <netinet/tcp.h>
#include <unistd.h>
#include <sys/time.h>
#define SSOPTCAST_RW(x) x
#define SSOPTCAST_RO(x) x
#else
#define SSOPTCAST_RO(x) (const char*)x
#define SSOPTCAST_RW(x) (char*)x
typedef int socklen_t;
#endif
#define VIDEO_COM_DEBUG (0)
#if VIDEO_COM_DEBUG
#define PDBG(...) \
do \
{ \
PRINT ("V_COM_STAGE: %s:%d : ", __FUNCTION__, __LINE__); \
PRINT (__VA_ARGS__); \
PRINT ("\n"); \
} while (0)
#else
#define PDBG(...)
#endif
#define SOCKET_BUFFER_SIZE (1024*1024)
/**
* IPHONE DEBUG ZONE
*/
float DEBUG_bitrate = 0.0;
#define DEBUG_TIME_BITRATE_CALCULATION_MSEC 1000.0
unsigned long DEBUG_totalBytes = 0;
int DEBUG_isTcp = 0;
static vp_os_mutex_t registerMutex;
static video_com_config_t **registeredStages = NULL;
static uint32_t nbRegisteredStages = 0;
const vp_api_stage_funcs_t video_com_funcs = {
(vp_api_stage_handle_msg_t) NULL,
(vp_api_stage_open_t) video_com_stage_open,
(vp_api_stage_transform_t) video_com_stage_transform,
(vp_api_stage_close_t) video_com_stage_close
};
const vp_api_stage_funcs_t video_com_multisocket_funcs = {
(vp_api_stage_handle_msg_t) NULL,
(vp_api_stage_open_t) video_com_multisocket_stage_open,
(vp_api_stage_transform_t) video_com_multisocket_stage_transform,
(vp_api_stage_close_t) video_com_multisocket_stage_close
};
C_RESULT video_com_stage_register (video_com_config_t *cfg)
{
static int runOnce = 1;
if (1 == runOnce)
{
vp_os_mutex_init (&registerMutex);
runOnce = 0;
}
vp_os_mutex_lock (&registerMutex);
nbRegisteredStages++;
registeredStages = vp_os_realloc (registeredStages, nbRegisteredStages * sizeof (video_com_config_t *));
C_RESULT retVal = C_FAIL;
if (NULL != registeredStages)
{
registeredStages[nbRegisteredStages - 1] = cfg;
retVal = C_OK;
}
vp_os_mutex_unlock (&registerMutex);
return retVal;
}
C_RESULT video_com_stage_connect (video_com_config_t *cfg)
{
#ifdef _WIN32
int timeout_for_windows=1000; /* timeout in milliseconds */
int sizeinit;
#else
struct timeval tv;
// 1 second timeout
tv.tv_sec = 1;
tv.tv_usec = 0;
#endif
C_RESULT res = C_FAIL;
if (TRUE == cfg->connected)
{
PDBG ("Will close");
res = vp_com_close (cfg->com, &cfg->socket);
cfg->connected = FALSE;
if (VP_FAILED (res))
{
PDBG ("Close failed");
return res;
}
}
if( cfg->protocol == VP_COM_PROBE)
{
PRINT("\n\nPROBING\n");
cfg->socket.protocol = VP_COM_TCP;
res = vp_com_open(cfg->com, &cfg->socket, &cfg->read, &cfg->write);
if( VP_SUCCEEDED(res) )
{
PRINT("\n\nTCP\n");
vp_com_close (cfg->com, &cfg->socket);
cfg->protocol = VP_COM_TCP;
}
else
{
PRINT("\n\nUDP\n");
cfg->protocol = VP_COM_UDP;
}
}
if( cfg->protocol == VP_COM_UDP )
{
cfg->socket.protocol = VP_COM_UDP;
cfg->socket.is_multicast = 0; // disable multicast for video
cfg->socket.multicast_base_addr = MULTICAST_BASE_ADDR;
res = vp_com_open(cfg->com, &cfg->socket, &cfg->read, &cfg->write);
if( VP_SUCCEEDED(res) )
{
int numi= 1;
socklen_t numi1= sizeof(int);
#ifdef _WIN32
setsockopt((int32_t)cfg->socket.priv, SOL_SOCKET, SO_RCVTIMEO, SSOPTCAST_RO(&timeout_for_windows), sizeof(timeout_for_windows));
#else
setsockopt((int32_t)cfg->socket.priv, SOL_SOCKET, SO_RCVTIMEO, SSOPTCAST_RO(&tv), sizeof(tv));
#endif
// Increase buffer for receiving datas.
setsockopt( (int32_t)cfg->socket.priv, SOL_SOCKET, SO_DEBUG, SSOPTCAST_RO(&numi), sizeof(numi));
numi = SOCKET_BUFFER_SIZE;
setsockopt( (int32_t)cfg->socket.priv, SOL_SOCKET, SO_RCVBUF, SSOPTCAST_RO(&numi),numi1);
getsockopt( (int32_t)cfg->socket.priv, SOL_SOCKET, SO_RCVBUF, SSOPTCAST_RW(&numi),&numi1);
PDBG ("New buffer size : %d", numi);
numi1 = 0;
setsockopt( (int32_t)cfg->socket.priv, SOL_SOCKET, SO_DEBUG, SSOPTCAST_RO(&numi1), sizeof(numi1));
cfg->connected = TRUE;
}
}
else if( cfg->protocol == VP_COM_TCP )
{
PDBG ("Will open TCP");
res = vp_com_open(cfg->com, &cfg->socket, &cfg->read, &cfg->write);
if( VP_SUCCEEDED(res) )
{
int numi= 1;
socklen_t numi1= sizeof(int);
PDBG ("Success open");
vp_com_sockopt(cfg->com, &cfg->socket, cfg->sockopt);
#ifdef _WIN32
setsockopt((int32_t)cfg->socket.priv, SOL_SOCKET, SO_RCVTIMEO, SSOPTCAST_RO(&timeout_for_windows), sizeof(timeout_for_windows));
#else
setsockopt((int32_t)cfg->socket.priv, SOL_SOCKET, SO_RCVTIMEO, SSOPTCAST_RO(&tv), sizeof(tv));
#endif
// Increase buffer for receiving datas.
setsockopt( (int32_t)cfg->socket.priv, SOL_SOCKET, SO_DEBUG, SSOPTCAST_RO(&numi), sizeof(numi));
numi = SOCKET_BUFFER_SIZE;
setsockopt( (int32_t)cfg->socket.priv, SOL_SOCKET, SO_RCVBUF, SSOPTCAST_RO(&numi),numi1);
getsockopt( (int32_t)cfg->socket.priv, SOL_SOCKET, SO_RCVBUF, SSOPTCAST_RW(&numi),&numi1);
PDBG ("NEW buffer size : %d", numi);
numi1 = 0;
setsockopt( (int32_t)cfg->socket.priv, SOL_SOCKET, SO_DEBUG, SSOPTCAST_RO(&numi1), sizeof(numi1));
cfg->connected = TRUE;
}
}
#ifdef _WIN32
sizeinit = strlen("Init");
vp_com_write_socket(&cfg->socket,"Init",&sizeinit);
#endif
return res;
}
void video_com_stage_notify_timeout (void)
{
vp_os_mutex_lock (&registerMutex);
int index = 0;
for (index = 0; index < nbRegisteredStages; index ++)
{
registeredStages[index]->mustReconnect = 1;
}
vp_os_mutex_unlock (&registerMutex);
}
C_RESULT video_com_stage_open(video_com_config_t *cfg)
{
cfg->connected = FALSE;
cfg->mustReconnect = 0;
video_com_stage_register (cfg);
PDBG ("Will call connect");
return video_com_stage_connect (cfg);
}
C_RESULT video_com_multisocket_stage_open(video_com_multisocket_config_t *cfg)
{
int i;
C_RESULT res = C_OK;
int nb_failed_connections = 0;
for (i = 0; i < cfg->nb_sockets; i++)
{
video_com_stage_register (cfg->configs[i]);
cfg->configs[i]->mustReconnect = 0;
}
PRINT("Video multisocket : init %i sockets\n",cfg->nb_sockets);
for(i=0;i<cfg->nb_sockets;i++)
{
PRINT("Video multisocket : connecting socket %d on port %d %s\n",
i,
cfg->configs[i]->socket.port,
(cfg->configs[i]->protocol==VP_COM_TCP)?"TCP":"UDP");
cfg->configs[i]->connected = FALSE;
res = video_com_stage_connect(cfg->configs[i]);
if (!VP_SUCCEEDED(res)) {
PRINT(" - Connection failed\n");
nb_failed_connections++;
}
}
cfg->last_active_socket = -1;
if (nb_failed_connections==cfg->nb_sockets) { return C_FAIL; }
return C_OK;
}
C_RESULT video_com_stage_transform(video_com_config_t *cfg, vp_api_io_data_t *in, vp_api_io_data_t *out)
{
C_RESULT res;
vp_os_mutex_lock(&out->lock);
if (1 == cfg->mustReconnect)
{
PDBG ("Will call connect");
PRINT ("Reconnecting ... ");
res = video_com_stage_connect (cfg);
PRINT ("%s\n", (VP_FAILED (res) ? "FAIL" : "OK"));
cfg->mustReconnect = 0;
}
if(out->status == VP_API_STATUS_INIT)
{
out->numBuffers = 1;
out->size = 0;
out->buffers = (uint8_t **) vp_os_malloc (sizeof(uint8_t *)+cfg->buffer_size*sizeof(uint8_t));
out->buffers[0] = (uint8_t *)(out->buffers+1);
out->indexBuffer = 0;
// out->lineSize not used
out->status = VP_API_STATUS_PROCESSING;
}
if(out->status == VP_API_STATUS_PROCESSING && cfg->read != NULL)
{
bool_t nonBlock = (cfg->forceNonBlocking && *(cfg->forceNonBlocking)==TRUE) ? TRUE : FALSE;
out->size = cfg->buffer_size;
if (nonBlock)
{
cfg->socket.block = VP_COM_DONTWAIT;
}
res = cfg->read(&cfg->socket, out->buffers[0], &out->size);
if (! nonBlock && cfg->protocol == VP_COM_UDP && out->size == 0)
{
// Send "1" for Unicast
// Send "2" to enable Multicast
char flag = 1; int32_t len = sizeof(flag);
if ( cfg->socket.is_multicast == 1 )
{
flag = 2;
}
cfg->write(&cfg->socket, (uint8_t*) &flag, &len);
}
bool_t bContinue = TRUE;
if( VP_FAILED(res) )
{
PDBG ("%s [%d] : status set to error !\n", __FUNCTION__, __LINE__);
perror ("Video_com_stage");
cfg->mustReconnect = 1;
out->size = 0;
vp_os_mutex_unlock (&out->lock);
return C_OK;
}
if( out->size == 0)
{
if (nonBlock)
{
out->size = -1; // Signal next stage that we don't have data waiting
}
else
{
cfg->num_retries++;
}
bContinue = FALSE;
}
else
{
cfg->num_retries = 0;
}
cfg->socket.block = VP_COM_DONTWAIT;
int32_t readSize = cfg->buffer_size - out->size;
while (TRUE == bContinue)
{
res = cfg->read(&cfg->socket, &(out->buffers[0][out->size]), &readSize);
if( VP_FAILED(res) )
{
PDBG ("%s [%d] : status set to error !\n", __FUNCTION__, __LINE__);
perror ("Video_com_stage");
cfg->mustReconnect = 1;
out->size = 0;
vp_os_mutex_unlock (&out->lock);
return C_OK;
}
if (0 == readSize)
{
bContinue = FALSE;
}
out->size += readSize;
readSize = cfg->buffer_size - out->size;
}
cfg->socket.block = VP_COM_DEFAULT;
}
if (NULL != cfg->timeoutFunc && 0 != cfg->timeoutFuncAfterSec)
{
if (cfg->num_retries >= cfg->timeoutFuncAfterSec)
{
cfg->timeoutFunc ();
}
}
vp_os_mutex_unlock(&out->lock);
return C_OK;
}
C_RESULT video_com_multisocket_stage_transform(video_com_multisocket_config_t *cfg, vp_api_io_data_t *in, vp_api_io_data_t *out)
{
C_RESULT res;
fd_set rfds;
int retval;
int fs,maxfs;
struct timeval tv;
int i;
bool_t selectTimeout;
vp_os_mutex_lock(&out->lock);
out->size = 0;
for (i=0;i<cfg->nb_sockets;i++) {
if (1 == cfg->configs[i]->mustReconnect)
{
PDBG ("Will call connect");
PRINT ("Reconnecting ... ");
res = C_FAIL;
res = video_com_stage_connect (cfg->configs[i]);
PRINT ("%s\n", (VP_FAILED (res) ? "FAIL" : "OK"));
cfg->configs[i]->mustReconnect = 0;
}
}
if(out->status == VP_API_STATUS_INIT)
{
out->numBuffers = 1;
out->size = 0;
out->buffers = (uint8_t **) vp_os_malloc (sizeof(uint8_t *)+cfg->buffer_size*sizeof(uint8_t));
out->buffers[0] = (uint8_t *)(out->buffers+1);
out->indexBuffer = 0;
// out->lineSize not used
out->status = VP_API_STATUS_PROCESSING;
}
if(out->status == VP_API_STATUS_PROCESSING)
{
/* Check which socket has data to read */
tv.tv_sec = 1;
tv.tv_usec = 0;
if(cfg->forceNonBlocking && *(cfg->forceNonBlocking)==TRUE) { tv.tv_sec = 0; }
FD_ZERO(&rfds);
maxfs=0;
for (i=0;i<cfg->nb_sockets;i++) {
if(cfg->configs[i]->connected) {
fs = (int)cfg->configs[i]->socket.priv;
if (fs>maxfs) maxfs=fs;
FD_SET(fs,&rfds);
}
}
retval = select(maxfs+1, &rfds, NULL, NULL, &tv);
/* Read the socket which has available data */
selectTimeout = FALSE;
i = -1;
if (retval>0)
{
if (cfg->last_active_socket!=-1)
{
i=cfg->last_active_socket;
fs = (int)cfg->configs[i]->socket.priv;
if (cfg->configs[i]->read && FD_ISSET(fs, &rfds))
{
out->size = cfg->configs[i]->buffer_size;
}
else
{
i = -1;
}
}
if (-1 == i)
{
for (i=0;i<cfg->nb_sockets;i++)
{
fs = (int)cfg->configs[i]->socket.priv;
if (cfg->configs[i]->read && FD_ISSET(fs, &rfds)) {
DEBUG_PRINT_SDK("Video multisocket : found data on port %s %d\n",
(cfg->configs[i]->socket.protocol==VP_COM_TCP)?"TCP":"UDP",cfg->configs[i]->socket.port);
cfg->last_active_socket = i;
if (VP_COM_TCP == cfg->configs[i]->protocol)
{
DEBUG_isTcp = 1;
}
else
{
DEBUG_isTcp = 0;
}
out->size = cfg->configs[i]->buffer_size;
break;
}
}
if(i == cfg->nb_sockets)
{
PRINT("%s:%d BUG !!!!!", __FUNCTION__, __LINE__);
selectTimeout = TRUE;
}
}
}
else
{
DEBUG_PRINT_SDK("%s\n",(retval==0)?"timeout":"error");
selectTimeout = TRUE;
}
if (FALSE == selectTimeout)
{
// DEBUG_PRINT_SDK ("Will read on socket %d\n", i);
// Actual first time read
res = cfg->configs[i]->read(&cfg->configs[i]->socket, out->buffers[0], &out->size);
if (VP_FAILED (res))
{
PDBG ("%s [%d] : status set to error !\n", __FUNCTION__, __LINE__);
perror ("Video_com_stage");
cfg->configs[i]->mustReconnect = 1;
out->size = 0;
vp_os_mutex_unlock (&out->lock);
return C_OK;
}
// Loop read to empty the socket buffers if needed
cfg->configs[i]->socket.block = VP_COM_DONTWAIT;
bool_t bContinue = TRUE;
int32_t readSize = cfg->configs[i]->buffer_size - out->size;
while (TRUE == bContinue)
{
// DEBUG_PRINT_SDK ("Will read %d octets from socket %d\n", readSize, i);
res = cfg->configs[i]->read(&cfg->configs[i]->socket, &(out->buffers[0][out->size]), &readSize);
if (VP_FAILED (res))
{
PDBG ("%s [%d] : status set to error !\n", __FUNCTION__, __LINE__);
perror ("Video_com_stage");
cfg->configs[i]->mustReconnect = 1;
out->size = 0;
vp_os_mutex_unlock (&out->lock);
return C_OK;
}
if (0 == readSize)
{
bContinue = FALSE;
}
out->size += readSize;
readSize = cfg->configs[i]->buffer_size - out->size;
}
cfg->configs[i]->socket.block = VP_COM_DEFAULT;
}
/* Resend a connection packet on UDP sockets */
if(!(cfg->forceNonBlocking && *(cfg->forceNonBlocking)==TRUE))
{
if( /* select timed out */ retval==0 )
{
for (i=0;i<cfg->nb_sockets;i++)
{
if( cfg->configs[i]->protocol == VP_COM_UDP )
{
// Send "1" for Unicast
// Send "2" to enable Multicast
char flag = 1; int len = sizeof(flag);
if ( cfg->configs[i]->socket.is_multicast == 1 )
flag = 2;
DEBUG_PRINT_SDK("Video multisocket : sending connection byte on port %s %d\n",
(cfg->configs[i]->socket.protocol==VP_COM_TCP)?"TCP":"UDP",cfg->configs[i]->socket.port);
cfg->configs[i]->write(&cfg->configs[i]->socket, (uint8_t*) &flag, &len);
}
}
}
}
if( (TRUE == selectTimeout || out->size == 0) && (!cfg->forceNonBlocking || (*(cfg->forceNonBlocking) == FALSE)) )
{
cfg->num_retries++;
}
else
cfg->num_retries = 0;
if((selectTimeout == TRUE) && cfg->forceNonBlocking && *(cfg->forceNonBlocking)==TRUE)
{
//PRINT("Debug %s:%d\n",__FUNCTION__,__LINE__);
/* No data are available here, but some are available in the next stage */
/* out->size=0 would restart the pipeline */
out->size=-1;
}
}
vp_os_mutex_unlock(&out->lock);
if (out->size > 0)
{
DEBUG_totalBytes += out->size;
static struct timeval DEBUG_now = {0, 0}, DEBUG_prev = {0, 0};
gettimeofday (&DEBUG_now, NULL);
float DEBUG_timeDiff = ((DEBUG_now.tv_sec - DEBUG_prev.tv_sec) * 1000.0) + ((DEBUG_now.tv_usec - DEBUG_prev.tv_usec) / 1000.0);
if (DEBUG_TIME_BITRATE_CALCULATION_MSEC <= DEBUG_timeDiff)
{
DEBUG_prev.tv_sec = DEBUG_now.tv_sec;
DEBUG_prev.tv_usec = DEBUG_now.tv_usec;
float DEBUG_instantBitrate = 8.0 * (float)(DEBUG_totalBytes) / DEBUG_TIME_BITRATE_CALCULATION_MSEC;
DEBUG_totalBytes = 0;
DEBUG_bitrate = 0.8 * DEBUG_bitrate + 0.2 * DEBUG_instantBitrate;
}
}
return C_OK;
}
C_RESULT video_com_stage_close(video_com_config_t *cfg)
{
vp_com_close(cfg->com, &cfg->socket);
return C_OK;
}
C_RESULT video_com_multisocket_stage_close(video_com_multisocket_config_t *cfg)
{
int i;
for (i=0;i<cfg->nb_sockets;i++)
{
vp_com_close(cfg->configs[i]->com, &cfg->configs[i]->socket);
}
return C_OK;
}
ARDroneLib/Soft/Lib/ardrone_tool/Video/video_com_stage.h
-58
Ver Arquivo
@@ -1,58 +0,0 @@
#ifndef _VIDEO_COM_STAGE_H_
#define _VIDEO_COM_STAGE_H_
#include <VP_Api/vp_api.h>
#include <VP_Com/vp_com.h>
#define VIDEO_MAX_RETRIES 5
typedef struct _video_com_config_t
{
vp_com_t* com;
vp_com_socket_t socket;
VP_COM_SOCKET_OPTIONS sockopt;
uint32_t buffer_size;
// Private Datas
Read read;
Write write;
uint32_t num_retries;
VP_COM_SOCKET_PROTOCOL protocol;
bool_t connected;
bool_t mustReconnect;
bool_t *forceNonBlocking;
void (*timeoutFunc)(void);
uint32_t timeoutFuncAfterSec;
} video_com_config_t;
typedef struct _video_com_multisocket_config_t
{
uint32_t nb_sockets;
video_com_config_t ** configs;
uint32_t last_active_socket;
uint32_t num_retries;
uint32_t buffer_size;
bool_t * forceNonBlocking;
}video_com_multisocket_config_t;
extern const vp_api_stage_funcs_t video_com_funcs;
extern const vp_api_stage_funcs_t video_com_multisocket_funcs;
void video_com_stage_notify_timeout (void);
C_RESULT video_com_stage_open(video_com_config_t *cfg);
C_RESULT video_com_stage_transform(video_com_config_t *cfg, vp_api_io_data_t *in, vp_api_io_data_t *out);
C_RESULT video_com_stage_close(video_com_config_t *cfg);
C_RESULT video_com_multisocket_stage_open(video_com_multisocket_config_t *cfg);
C_RESULT video_com_multisocket_stage_transform(video_com_multisocket_config_t *cfg, vp_api_io_data_t *in, vp_api_io_data_t *out);
C_RESULT video_com_multisocket_stage_close(video_com_multisocket_config_t *cfg);
#endif // _VIDEO_COM_STAGE_H_
ARDroneLib/Soft/Lib/ardrone_tool/Video/video_encapsulation.c
-45
Ver Arquivo
@@ -1,45 +0,0 @@
#include <stdio.h>
#include <VP_Os/vp_os_types.h>
#include <video_encapsulation.h>
C_RESULT init_parrot_video_encapsulation_header(parrot_video_encapsulation_t * header)
{
header->signature[0]='P'; /* Parrot */
header->signature[1]='a';
header->signature[2]='V'; /* Video */
header->signature[3]='E'; /* Encapsulation */
header->version = PAVE_CURRENT_VERSION;
return C_OK;
}
int pave_is_same_frame(parrot_video_encapsulation_t * header1 , parrot_video_encapsulation_t * header2 )
{
int res;
if (!header1) { return 0; }
if (!header2) { return 0; }
res = ( (header1->stream_id == header2->stream_id) &&
(header1->frame_number == header2->frame_number ) );
return ( res );
}
void dumpPave (parrot_video_encapsulation_t *PaVE)
{
printf ("Signature : \"%c%c%c%c\" [0x%02x][0x%02x][0x%02x][0x%02x]\n", PaVE->signature[0], PaVE->signature[1],
PaVE->signature[2], PaVE->signature[3], PaVE->signature[0], PaVE->signature[1], PaVE->signature[2], PaVE->signature[3]);
printf ("Frame Type / Number : %s : %d : slice %d/%d\n",
(PaVE->frame_type == FRAME_TYPE_P_FRAME) ? "P-Frame" : ((PaVE->frame_type == FRAME_TYPE_I_FRAME) ? "I-Frame" : "IDR-Frame"),
PaVE->frame_number,
PaVE->slice_index+1,
PaVE->total_slices);
printf ("Codec : %s\n", (PaVE->video_codec == CODEC_MPEG4_VISUAL) ? "MP4" : ((PaVE->video_codec == CODEC_MPEG4_AVC) ? "H264" : "Unknown"));
printf ("StreamID : %d \n", PaVE->stream_id);
printf ("Encoded dims : %d x %d\n", PaVE->encoded_stream_width, PaVE->encoded_stream_height);
printf ("Display dims : %d x %d\n", PaVE->display_width, PaVE->display_height);
printf ("Header size : %d\n", PaVE->header_size);
printf ("Payload size : %d\n", PaVE->payload_size);
printf ("Control : 0x%08x\n", PaVE->control);
}
ARDroneLib/Soft/Lib/ardrone_tool/Video/video_navdata_handler.c
-134
Ver Arquivo
@@ -1,134 +0,0 @@
#include <ardrone_tool/Video/video_navdata_handler.h>
#include <ardrone_tool/Video/video_com_stage.h>
#include <ardrone_tool/ardrone_tool_configuration.h>
#ifdef _WIN32
#include <Windows.h>
#else
#include <sys/time.h>
#endif
#define TIMEOUT_SEC (1)
#define TIMEOUT_MS (1000*TIMEOUT_SEC)
uint32_t hdvideo_remaining_frames;
uint32_t hdvideo_remaining_kilobytes;
uint32_t hdvideo_maximum_kilobytes;
float hdvideo_fifo_fill_percentage; // range 0.0 - 100.0
float hdvideo_retrieving_progress;
static uint32_t hdvideo_frames_to_retreive;
C_RESULT video_navdata_handler_init( void* data )
{
hdvideo_remaining_frames = 0;
hdvideo_remaining_kilobytes = 0;
hdvideo_maximum_kilobytes = 0;
hdvideo_fifo_fill_percentage = 0.0;
hdvideo_retrieving_progress = -1.0;
hdvideo_frames_to_retreive = 0;
return C_OK;
}
#ifdef _WIN32
#define isTimeout(NOW,PREV) (((NOW-PREV) >= TIMEOUT_MS) ? 1 : 0)
#define getTime(TIME) (TIME) = GetTickCount()
#else
#define getTime(TIME) gettimeofday (&TIME, NULL)
static inline int isTimeout (struct timeval now, struct timeval prev)
{
struct timeval diff;
if (now.tv_usec >= prev.tv_usec)
{
diff.tv_usec = now.tv_usec - prev.tv_usec;
diff.tv_sec = now.tv_sec - prev.tv_sec;
}
else
{
diff.tv_usec = 1000000 - prev.tv_usec + now.tv_usec;
diff.tv_sec = now.tv_sec - prev.tv_sec - 1;
}
return ((diff.tv_sec >= TIMEOUT_SEC) ? 1 : 0);
}
#endif
C_RESULT video_navdata_handler_process( const navdata_unpacked_t* const navdata )
{
/* Compute time between two prevous navdata to detect a timeout
* (i.e. more than 1 sec between two navdatas)
*/
static int firstTime = 1;
#if _WIN32
static DWORD prev, now;
#else
static struct timeval prev, now;
#endif
getTime (now);
if (1 == firstTime)
{
firstTime = 0;
}
else
{
int flag = isTimeout (now, prev);
if (flag)
{
video_com_stage_notify_timeout ();
}
}
/* Video storage navdatas */
hdvideo_remaining_frames = navdata->navdata_hdvideo_stream.storage_fifo_nb_packets;
hdvideo_remaining_kilobytes = navdata->navdata_hdvideo_stream.storage_fifo_size;
hdvideo_maximum_kilobytes = ardrone_control_config.video_storage_space;
if (NAVDATA_HDVIDEO_STORAGE_FIFO_IS_FULL & navdata->navdata_hdvideo_stream.hdvideo_state)
{
// Socket is flagged as full
hdvideo_fifo_fill_percentage = 100.0;
}
else if ((0 == hdvideo_maximum_kilobytes) ||
(hdvideo_maximum_kilobytes < hdvideo_remaining_kilobytes))
{
// Unexpected result (we don't have the total size
// or the current size is greater than the total size)
hdvideo_fifo_fill_percentage = -1.0;
}
else
{
// Normal case
hdvideo_fifo_fill_percentage = (hdvideo_remaining_kilobytes * 100.0) / (hdvideo_maximum_kilobytes * 1.0);
}
// Process check
if (0 != hdvideo_frames_to_retreive &&
hdvideo_frames_to_retreive >= hdvideo_remaining_frames)
{
uint32_t _retreived_frames = hdvideo_frames_to_retreive - hdvideo_remaining_frames;
hdvideo_retrieving_progress = (float)(_retreived_frames) / (float)(hdvideo_frames_to_retreive);
}
else
{
hdvideo_retrieving_progress = -1.0;
}
getTime (prev);
return C_OK;
}
C_RESULT video_navdata_handler_release( void )
{
return C_OK;
}
void startRetreiving (void)
{
hdvideo_frames_to_retreive = hdvideo_remaining_frames;
}
void endRetreiving (void)
{
hdvideo_frames_to_retreive = 0;
}
ARDroneLib/Soft/Lib/ardrone_tool/Video/video_navdata_handler.h
-21
Ver Arquivo
@@ -1,21 +0,0 @@
#ifndef _VIDEO_NAVDATA_HANDLER_H_
#define _VIDEO_NAVDATA_HANDLER_H_
#include <ardrone_tool/Navdata/ardrone_navdata_client.h>
// Handler that reset video connexion when there is a timeout in navdata connexion
extern uint32_t hdvideo_remaining_frames;
extern uint32_t hdvideo_remaining_kilobytes;
extern uint32_t hdvideo_maximum_kilobytes;
extern float hdvideo_fifo_fill_percentage; // range 0.0 - 100.0
extern float hdvideo_retrieving_progress; // -1.0 when not retreiving, else range 0.0-1.0
C_RESULT video_navdata_handler_init( void* data );
C_RESULT video_navdata_handler_process( const navdata_unpacked_t* const navdata );
C_RESULT video_navdata_handler_release( void );
void startRetreiving (void);
void endRetreiving (void);
#endif // _VIDEO_NAVDATA_HANDLER_H_
ARDroneLib/Soft/Lib/ardrone_tool/Video/video_recorder_pipeline.c
-151
Ver Arquivo
@@ -1,151 +0,0 @@
//
// video_recorder_pipeline.c
// ARDroneLib
//
// Created by Nicolas Brulez on 14/10/11.
// Copyright 2011 Parrot. All rights reserved.
//
#include <VP_Api/vp_api.h>
#include <VP_Api/vp_api_error.h>
#include <ardrone_tool/ardrone_tool.h>
#include <ardrone_tool/Com/config_com.h>
#include <ardrone_tool/Video/video_com_stage.h>
#include <ardrone_tool/Video/video_stage_encoded_recorder.h>
#include <ardrone_tool/Video/video_recorder_pipeline.h>
#include <ardrone_tool/Video/video_stage_tcp.h>
#include <ardrone_tool/ardrone_version.h>
video_com_config_t record_icc;
static bool_t video_recorder_in_pause = TRUE;
static vp_os_cond_t video_recorder_condition;
static vp_os_mutex_t video_recorder_mutex;
static bool_t isInit = FALSE;
void video_recorder_init (void)
{
vp_os_mutex_init (&video_recorder_mutex);
vp_os_cond_init (&video_recorder_condition, &video_recorder_mutex);
isInit = TRUE;
}
void video_recorder_suspend_thread (void)
{
vp_os_mutex_lock (&video_recorder_mutex);
video_recorder_in_pause = TRUE;
vp_os_mutex_unlock (&video_recorder_mutex);
}
void video_recorder_resume_thread (void)
{
vp_os_mutex_lock (&video_recorder_mutex);
vp_os_cond_signal (&video_recorder_condition);
video_recorder_in_pause = FALSE;
vp_os_mutex_unlock (&video_recorder_mutex);
}
DEFINE_THREAD_ROUTINE(video_recorder, data)
{
if (1 >= ARDRONE_VERSION ()) // Run only for ARDrone 2 and upper
{
return (THREAD_RET)0;
}
C_RESULT res = C_OK;
vp_api_io_pipeline_t pipeline;
vp_api_io_data_t out;
vp_api_io_stage_t stages[3];
PIPELINE_HANDLE video_recorder_pipeline_handle;
video_recorder_thread_param_t *video_recorder_thread_param = (video_recorder_thread_param_t *)data;
video_stage_tcp_config_t tcpConf;
if(video_recorder_thread_param != NULL)
{
CHANGE_THREAD_PRIO(video_recorder, video_recorder_thread_param->priority);
video_stage_encoded_recorder_config.finish_callback = video_recorder_thread_param->finish_callback;
}
vp_os_memset (&record_icc, 0x0, sizeof (record_icc));
record_icc.com = COM_VIDEO ();
record_icc.buffer_size = (8*1024);
record_icc.protocol = VP_COM_TCP;
record_icc.forceNonBlocking = &tcpConf.tcpStageHasMoreData;
COM_CONFIG_SOCKET_VIDEO (&record_icc.socket, VP_COM_CLIENT, VIDEO_RECORDER_PORT, wifi_ardrone_ip);
record_icc.timeoutFunc = &video_stage_encoded_recorder_com_timeout;
record_icc.timeoutFuncAfterSec = 10;
vp_os_memset (&tcpConf, 0, sizeof (tcpConf));
tcpConf.maxPFramesPerIFrame = 30;
tcpConf.frameMeanSize = 160*1024;
tcpConf.latencyDrop = 0;
pipeline.nb_stages = 0;
pipeline.stages = &stages[0];
// Com
stages[pipeline.nb_stages].type = VP_API_INPUT_SOCKET;
stages[pipeline.nb_stages].cfg = (void *)&record_icc;
stages[pipeline.nb_stages++].funcs = video_com_funcs;
// TCP
stages[pipeline.nb_stages].type = VP_API_FILTER_DECODER;
stages[pipeline.nb_stages].cfg = (void *) &tcpConf;
stages[pipeline.nb_stages++].funcs = video_stage_tcp_funcs;
// Record
stages[pipeline.nb_stages].type = VP_API_FILTER_DECODER;
stages[pipeline.nb_stages].cfg = (void *) &video_stage_encoded_recorder_config;
stages[pipeline.nb_stages++].funcs = video_encoded_recorder_funcs;
while (FALSE == isInit)
{
printf ("Waiting for init\n");
}
if (! ardrone_tool_exit ())
{
PRINT ("Video recorder thread initialisation\n");
res = vp_api_open (&pipeline, &video_recorder_pipeline_handle);
if (SUCCEED (res))
{
int loop = SUCCESS;
out.status = VP_API_STATUS_PROCESSING;
while (! ardrone_tool_exit () && (SUCCESS == loop))
{
if (video_recorder_in_pause)
{
vp_os_mutex_lock (&video_recorder_mutex);
record_icc.num_retries = VIDEO_MAX_RETRIES;
vp_os_cond_wait (&video_recorder_condition);
vp_os_mutex_unlock (&video_recorder_mutex);
}
if (SUCCEED (vp_api_run (&pipeline, &out)))
{
if ((VP_API_STATUS_PROCESSING == out.status) ||
(VP_API_STATUS_STILL_RUNNING == out.status))
{
loop = SUCCESS;
}
else loop = -1;
}
else loop = -1;
}
vp_api_close (&pipeline, &video_recorder_pipeline_handle);
}
}
PRINT ("Video recorder thread ended\n");
return (THREAD_RET)res;
}
uint32_t video_recorder_get_num_retries(void) {
return record_icc.num_retries;
}
ARDroneLib/Soft/Lib/ardrone_tool/Video/video_recorder_pipeline.h
-40
Ver Arquivo
@@ -1,40 +0,0 @@
//
// video_recorder_pipeline.h
// ARDroneEngine
//
// Created by Nicolas Brulez on 14/10/11.
// Copyright 2011 Parrot. All rights reserved.
//
#ifndef _VIDEO_RECORDER_PIPELINE_H_
#define _VIDEO_RECORDER_PIPELINE_H_
#include <VP_Os/vp_os.h>
#include <VP_Os/vp_os_print.h>
#include <VP_Os/vp_os_types.h>
#include <VP_Os/vp_os_signal.h>
#include <VP_Os/vp_os_malloc.h>
#include <VP_Os/vp_os_delay.h>
#include <VP_Api/vp_api.h>
#include <VP_Api/vp_api_error.h>
#include <VP_Api/vp_api_stage.h>
#include <VP_Api/vp_api_picture.h>
#include <VP_Api/vp_api_thread_helper.h>
#include <ardrone_tool/Video/video_stage_encoded_recorder.h>
typedef struct _video_recorder_thread_param_t_
{
int32_t priority;
video_stage_encoded_recorder_callback finish_callback;
} video_recorder_thread_param_t;
PROTO_THREAD_ROUTINE (video_recorder, data);
void video_recorder_init(void);
void video_recorder_suspend_thread(void);
void video_recorder_resume_thread(void);
uint32_t video_recorder_get_num_retries(void);
#endif
ARDroneLib/Soft/Lib/ardrone_tool/Video/video_stage.c
-227
Ver Arquivo
@@ -1,227 +0,0 @@
/*
* video_stage.c
* Test
*
* Created by Frederic D'HAEYER on 22/02/10.
* Copyright 2010 Parrot SA. All rights reserved.
*
*/
#include <VP_Api/vp_api.h>
#include <VP_Api/vp_api_error.h>
#include <ardrone_tool/ardrone_tool.h>
#include <ardrone_tool/Com/config_com.h>
#include <ardrone_tool/Video/video_stage.h>
#include <ardrone_tool/Academy/academy_stage_recorder.h>
#include <ardrone_tool/Video/video_stage_encoded_recorder.h>
#include <ardrone_tool/ardrone_version.h>
#ifndef STREAM_WIDTH
#define STREAM_WIDTH 512
#endif
#ifndef STREAM_HEIGHT
#define STREAM_HEIGHT 512
#endif
#define NB_STAGES 5
extern char documents_dir[];
extern char resources_dir[];
PIPELINE_HANDLE video_pipeline_handle;
static bool_t video_stage_in_pause = TRUE;
static vp_os_cond_t video_stage_condition;
static vp_os_mutex_t video_stage_mutex;
static video_com_config_t icc_tcp;
static video_com_config_t icc_udp;
video_com_multisocket_config_t icc;
static video_com_config_t* icc_tab[2];
static video_stage_tcp_config_t tcpConf;
video_decoder_config_t vec;
void video_stage_init(void) {
vp_os_mutex_init(&video_stage_mutex);
vp_os_cond_init(&video_stage_condition, &video_stage_mutex);
}
void video_stage_suspend_thread(void) {
vp_os_mutex_lock(&video_stage_mutex);
video_stage_in_pause = TRUE;
vp_os_mutex_unlock(&video_stage_mutex);
}
void video_stage_resume_thread(void) {
vp_os_mutex_lock(&video_stage_mutex);
vp_os_cond_signal(&video_stage_condition);
video_stage_in_pause = FALSE;
vp_os_mutex_unlock(&video_stage_mutex);
}
DEFINE_THREAD_ROUTINE(video_stage, data) {
C_RESULT res;
vp_api_io_pipeline_t pipeline;
vp_api_io_data_t out;
vp_api_io_stage_t * stages;
video_stage_merge_slices_config_t merge_slices_cfg;
uint8_t i;
specific_parameters_t * params = (specific_parameters_t *)(data);
if (1 == params->needSetPriority)
{
CHANGE_THREAD_PRIO (video_stage, params->priority);
}
vp_os_memset(&icc_tcp, 0, sizeof ( icc_tcp));
vp_os_memset(&icc_udp, 0, sizeof ( icc_udp));
// Video Communication config
icc_tcp.com = COM_VIDEO();
icc_tcp.buffer_size = (1024*1024);
icc_tcp.protocol = VP_COM_TCP;
COM_CONFIG_SOCKET_VIDEO(&icc_tcp.socket, VP_COM_CLIENT, VIDEO_PORT, wifi_ardrone_ip);
// Video Communication config
icc_udp.com = COM_VIDEO();
icc_udp.buffer_size = (1024*1024);
icc_udp.protocol = VP_COM_UDP;
COM_CONFIG_SOCKET_VIDEO(&icc_udp.socket, VP_COM_CLIENT, VIDEO_PORT, wifi_ardrone_ip);
icc.nb_sockets = 2;
icc.configs = icc_tab;
icc.forceNonBlocking = &(tcpConf.tcpStageHasMoreData);
icc_tab[1] = &icc_tcp;
icc_tab[0] = &icc_udp;
icc.buffer_size = (1024*1024);
vp_os_memset(&vec, 0, sizeof ( vec));
stages = (vp_api_io_stage_t*) (vp_os_calloc(
NB_STAGES + params->pre_processing_stages_list->length + params->post_processing_stages_list->length,
sizeof (vp_api_io_stage_t)
));
vec.src_picture = params->in_pic;
vec.dst_picture = params->out_pic;
vp_os_memset(&tcpConf, 0, sizeof ( tcpConf));
tcpConf.maxPFramesPerIFrame = 30;
tcpConf.frameMeanSize = 160*1024;
tcpConf.tcpStageHasMoreData = FALSE;
tcpConf.latencyDrop = 1;
pipeline.nb_stages = 0;
pipeline.stages = &stages[0];
//ENCODED FRAME PROCESSING STAGES
stages[pipeline.nb_stages].type = VP_API_INPUT_SOCKET;
stages[pipeline.nb_stages].cfg = (void *) &icc;
stages[pipeline.nb_stages++].funcs = video_com_multisocket_funcs;
stages[pipeline.nb_stages].type = VP_API_FILTER_DECODER;
stages[pipeline.nb_stages].cfg = (void *) &tcpConf;
stages[pipeline.nb_stages++].funcs = video_stage_tcp_funcs;
// Record Encoded video
if(1 == ARDRONE_VERSION())
{
ardrone_academy_stage_recorder_config.dest.pipeline = video_pipeline_handle;
ardrone_academy_stage_recorder_config.dest.stage = pipeline.nb_stages;
stages[pipeline.nb_stages].type = VP_API_FILTER_DECODER;
stages[pipeline.nb_stages].cfg = (void*)&ardrone_academy_stage_recorder_config;
stages[pipeline.nb_stages++].funcs = ardrone_academy_stage_recorder_funcs;
}
else
{
// Nothing to do for AR.Drone 2 as we have a separated thread for recording
}
//PRE-DECODING STAGES ==> recording, ...
for(i=0;i<params->pre_processing_stages_list->length;i++){
stages[pipeline.nb_stages].type = params->pre_processing_stages_list->stages_list[i].type;
stages[pipeline.nb_stages].cfg = params->pre_processing_stages_list->stages_list[i].cfg;
stages[pipeline.nb_stages++].funcs = params->pre_processing_stages_list->stages_list[i].funcs;
}
stages[pipeline.nb_stages].type = VP_API_FILTER_DECODER;
stages[pipeline.nb_stages].cfg = (void *)&merge_slices_cfg;
stages[pipeline.nb_stages++].funcs = video_stage_merge_slices_funcs;
//DECODING STAGES
stages[pipeline.nb_stages].type = VP_API_FILTER_DECODER;
stages[pipeline.nb_stages].cfg = (void*) &vec;
stages[pipeline.nb_stages++].funcs = video_decoding_funcs;
//POST-DECODING STAGES ==> transformation, display, ...
for(i=0;i<params->post_processing_stages_list->length;i++){
stages[pipeline.nb_stages].type = params->post_processing_stages_list->stages_list[i].type;
stages[pipeline.nb_stages].cfg = params->post_processing_stages_list->stages_list[i].cfg;
stages[pipeline.nb_stages++].funcs = params->post_processing_stages_list->stages_list[i].funcs;
}
if (!ardrone_tool_exit()) {
PRINT("\nvideo stage thread initialisation\n\n");
res = vp_api_open(&pipeline, &video_pipeline_handle);
if (SUCCEED(res)) {
int loop = SUCCESS;
out.status = VP_API_STATUS_PROCESSING;
while (!ardrone_tool_exit() && (loop == SUCCESS)) {
if (video_stage_in_pause) {
vp_os_mutex_lock(&video_stage_mutex);
icc.num_retries = VIDEO_MAX_RETRIES;
vp_os_cond_wait(&video_stage_condition);
vp_os_mutex_unlock(&video_stage_mutex);
}
if (SUCCEED(vp_api_run(&pipeline, &out))) {
if ((out.status == VP_API_STATUS_PROCESSING || out.status == VP_API_STATUS_STILL_RUNNING)) {
loop = SUCCESS;
}
} else loop = -1; // Finish this thread
}
vp_os_free(params->pre_processing_stages_list->stages_list);
params->pre_processing_stages_list->stages_list = NULL;
vp_os_free(params->post_processing_stages_list->stages_list);
params->post_processing_stages_list->stages_list = NULL;
vp_os_free(params->pre_processing_stages_list);
params->pre_processing_stages_list = NULL;
vp_os_free(params->post_processing_stages_list);
params->post_processing_stages_list = NULL;
vp_os_free(params->in_pic);
params->in_pic = NULL;
vp_os_free(params->out_pic);
params->out_pic = NULL;
vp_os_free(params);
params = NULL;
vp_api_close(&pipeline, &video_pipeline_handle);
}
}
PRINT("\nvideo stage thread ended\n\n");
return (THREAD_RET) 0;
}
uint32_t video_stage_get_num_retries(void) {
return icc.num_retries;
}
ARDroneLib/Soft/Lib/ardrone_tool/Video/video_stage.h
-66
Ver Arquivo
@@ -1,66 +0,0 @@
/*
* video_stage.h
* Test
*
* Created by Frederic D'HAEYER on 22/02/10.
* Copyright 2010 Parrot SA. All rights reserved.
*
*/
#ifndef _VIDEO_STAGE_H_
#define _VIDEO_STAGE_H_
#include <VP_Os/vp_os.h>
#include <VP_Os/vp_os_print.h>
#include <VP_Os/vp_os_types.h>
#include <VP_Os/vp_os_signal.h>
#include <VP_Os/vp_os_malloc.h>
#include <VP_Os/vp_os_delay.h>
#include <VP_Api/vp_api.h>
#include <VP_Api/vp_api_error.h>
#include <VP_Api/vp_api_stage.h>
#include <VP_Api/vp_api_picture.h>
#include <VP_Api/vp_api_thread_helper.h>
#include <ardrone_tool/Video/video_com_stage.h>
#include <ardrone_tool/Video/video_stage_tcp.h>
#include <ardrone_tool/Video/video_stage_decoder.h>
#include <ardrone_tool/Video/video_stage_merge_slices.h>
#include <ardrone_tool/Video/video_stage_latency_estimation.h>
typedef struct _specific_stages_t_
{
vp_api_io_stage_t * stages_list;
uint8_t length;
} specific_stages_t;
typedef struct _specific_parameters_t_
{
specific_stages_t * pre_processing_stages_list;
specific_stages_t * post_processing_stages_list;
vp_api_picture_t * in_pic;
vp_api_picture_t * out_pic;
int needSetPriority;
int priority;
} specific_parameters_t;
extern video_decoder_config_t vec;
PROTO_THREAD_ROUTINE(video_stage, data);
static inline unsigned long RoundPower2(unsigned long x)
{
int rval=512;
// rval<<=1 Is A Prettier Way Of Writing rval*=2;
while(rval < x)
rval<<=1;
return rval;
}
void video_stage_init(void);
void video_stage_suspend_thread(void);
void video_stage_resume_thread(void);
uint32_t video_stage_get_num_retries(void);
#endif // _VIDEO_STAGE_H_
ARDroneLib/Soft/Lib/ardrone_tool/Video/video_stage_decoder.c
-387
Ver Arquivo
@@ -1,387 +0,0 @@
/*
* video_stage_decoder.c
* ARDroneLib
*
* Created by n.brulez on 02/08/11.
* Copyright 2011 Parrot SA. All rights reserved.
*
*/
#if defined (FFMPEG_SUPPORT) || defined (ITTIAM_SUPPORT)
#include <ardrone_tool/Video/video_stage_decoder.h>
#include <video_encapsulation.h>
#include <VP_Os/vp_os_malloc.h>
#include <VP_Os/vp_os_print.h>
#include <stdio.h>
#ifndef USE_ANDROID
# ifdef ITTIAM_SUPPORT
# define mp4h264_open ittiam_stage_decoding_open
# define mp4h264_transform ittiam_stage_decoding_transform
# define mp4h264_close ittiam_stage_decoding_close
static const int resetDecoderOnStreamChange = 1;
# else // FFMPEG
# define mp4h264_open ffmpeg_stage_decoding_open
# define mp4h264_transform ffmpeg_stage_decoding_transform
# define mp4h264_close ffmpeg_stage_decoding_close
static const int resetDecoderOnStreamChange = 0;
# endif
#else // ANDROID
typedef enum {
NEON_SUPPORT_UNKNOWN = 0,
NEON_SUPPORT_OK,
NEON_SUPPORT_FAIL,
} neon_status_t;
static neon_status_t neonStatus = NEON_SUPPORT_FAIL; // <<< FORCE FFMPEG <<<
static int resetDecoderOnStreamChange = 0; // NON CONST as it will be set at runtime
C_RESULT mp4h264_open (mp4h264_config_t *cfg);
C_RESULT mp4h264_transform (mp4h264_config_t *cfg, vp_api_io_data_t *in, vp_api_io_data_t *out);
C_RESULT mp4h264_close (mp4h264_config_t *cfg);
#endif
/**
* DEBUG ZONE
*/
#if defined(TARGET_OS_IPHONE) || defined (TARGET_OS_IPHONE_SIMULATOR)
#include <mach/mach_time.h>
#endif
float DEBUG_decodingTimeUsec = 0.0;
#define ENABLE_VIDEO_STAGE_DECODER_DEBUG (0)
const vp_api_stage_funcs_t video_decoding_funcs = {
(vp_api_stage_handle_msg_t) NULL,
(vp_api_stage_open_t) video_stage_decoder_open,
(vp_api_stage_transform_t) video_stage_decoder_transform,
(vp_api_stage_close_t) video_stage_decoder_close
};
#if ENABLE_VIDEO_STAGE_DECODER_DEBUG || defined (DEBUG)
#define PDBG(...) \
do \
{ \
printf ("VIDEO_STAGE_DECODER (%s@%d) : ", __FUNCTION__, __LINE__); \
printf (__VA_ARGS__); \
printf ("\n"); \
} while (0)
#else
#define PDBG(...)
#endif
#define ALLOC_CHECK(POINTER, SIZE) \
do \
{ \
POINTER = vp_os_calloc (SIZE, 1); \
if (NULL == POINTER) \
{ \
printf ("Unable to alloc %s\n", #POINTER); \
return C_FAIL; \
} \
} while (0)
#ifdef USE_LINUX
#include <unistd.h>
int video_stage_decoder_fakeLatency = 0; /* Simulate a slow decoder inside AR.Drone Navigation */
#endif
parrot_video_encapsulation_codecs_t video_stage_decoder_lastDetectedCodec = 0;
C_RESULT video_stage_decoder_open (video_decoder_config_t *cfg)
{
// Allocate internal datas
ALLOC_CHECK (cfg->vlibConf, sizeof (vlib_stage_decoding_config_t));
ALLOC_CHECK (cfg->vlibOut, sizeof (vp_api_io_data_t));
ALLOC_CHECK (cfg->mp4h264Conf, sizeof (mp4h264_config_t));
ALLOC_CHECK (cfg->mp4h264Out, sizeof (vp_api_io_data_t));
// Fill alloc'd structs with data from cfg
// --> MPEG4 / H264
cfg->mp4h264Conf->dst_picture.format = cfg->dst_picture->format;
// --> VLIB
cfg->vlibConf->width = cfg->dst_picture->width;
cfg->vlibConf->height = cfg->dst_picture->height;
cfg->vlibConf->picture = cfg->dst_picture;
cfg->vlibConf->luma_only = FALSE;
cfg->vlibConf->block_mode_enable = TRUE;
switch (cfg->dst_picture->format)
{
case PIX_FMT_RGB565:
cfg->bpp=2;
break;
case PIX_FMT_RGB24:
cfg->bpp=3;
break;
default:
cfg->bpp=0;
break;
}
vlib_stage_decoding_open (cfg->vlibConf);
mp4h264_open (cfg->mp4h264Conf);
return C_OK;
}
static inline bool_t havePaVE (uint8_t *buffer)
{
if (buffer [0] == 'P' &&
buffer [1] == 'a' &&
buffer [2] == 'V' &&
buffer [3] == 'E')
{
return TRUE;
}
else
{
return FALSE;
}
}
C_RESULT video_stage_decoder_transform (video_decoder_config_t *cfg, vp_api_io_data_t *in, vp_api_io_data_t *out)
{
C_RESULT retVal = C_OK;
bool_t useVlib = FALSE;
bool_t useMP4H264 = FALSE;
vp_api_io_data_t *outToCopy = NULL;
uint8_t *buffer = in->buffers[in->indexBuffer];
static int lastDecodedStreamID = -1;
#if defined(TARGET_OS_IPHONE) || defined (TARGET_OS_IPHONE_SIMULATOR)
uint64_t startTime = 0;
uint64_t stopTime = 0;
uint64_t elapsedNano = 0;
static mach_timebase_info_data_t sTimebaseInfo;
if (0 == sTimebaseInfo.denom)
{
mach_timebase_info (&sTimebaseInfo);
}
startTime = mach_absolute_time ();
#endif
// Check for PaVE
if (havePaVE (buffer))
{
parrot_video_encapsulation_t *PaVE = (parrot_video_encapsulation_t *)buffer;
video_stage_decoder_lastDetectedCodec = PaVE->video_codec;
if (lastDecodedStreamID!=-1 && lastDecodedStreamID!=PaVE->stream_id && 1 == resetDecoderOnStreamChange)
{
PRINT("Resetting the video decoder.\n");
video_stage_decoder_close(cfg);
video_stage_decoder_open(cfg);
}
lastDecodedStreamID=PaVE->stream_id;
if (CODEC_VLIB == PaVE->video_codec ||
CODEC_P264 == PaVE->video_codec)
{
useVlib = TRUE;
}
else if (CODEC_MPEG4_VISUAL == PaVE->video_codec ||
CODEC_MPEG4_AVC == PaVE->video_codec)
{
useMP4H264 = TRUE;
}
else
{
// Unknown codec
return C_FAIL;
}
}
else if ( ((*(uint32_t*)buffer) & 0xFFFE7C00 )== 0 ) /* true if a UVLC or P264 header is present */
{
/* Test bits 15 and 16 of the header to differenciate UVLC and P264 */
video_stage_decoder_lastDetectedCodec = ( ((*(uint32_t*)buffer) & 0x8000 )== 0x8000 ) ? CODEC_VLIB : CODEC_P264;
// No PaVE -> give to VLIB
useVlib = TRUE;
}
else
{
// Unknown codec
printf(" -- Critical error : unrecognized codec (first bytes : %x %x %x %x %x) --\n",
buffer[0],buffer[1],buffer[2],buffer[3],buffer[4]);
return C_FAIL;
}
if (useVlib)
{
cfg->vlibConf->num_picture_decoded = cfg->num_picture_decoded;
retVal = vlib_stage_decoding_transform (cfg->vlibConf, in, cfg->vlibOut);
if (C_FAIL == retVal)
{
return retVal;
}
cfg->num_picture_decoded = cfg->vlibConf->num_picture_decoded;
cfg->num_frames = cfg->vlibConf->controller.num_frames;
cfg->src_picture->height = cfg->vlibConf->controller.height;
cfg->src_picture->width = cfg->vlibConf->controller.width;
cfg->rowstride = cfg->dst_picture->width * cfg->bpp; // Size of the buffer we got for VLIB
cfg->vlibOut->size = cfg->rowstride * cfg->dst_picture->height;
outToCopy = cfg->vlibOut;
}
if (useMP4H264)
{
parrot_video_encapsulation_t *PaVE = (parrot_video_encapsulation_t *)buffer;
cfg->mp4h264Conf->num_picture_decoded = cfg->num_picture_decoded;
retVal = mp4h264_transform (cfg->mp4h264Conf, in, cfg->mp4h264Out);
if (C_FAIL == retVal)
{
return retVal;
}
cfg->num_picture_decoded = cfg->mp4h264Conf->num_picture_decoded;
cfg->num_frames = PaVE->frame_number;
cfg->dst_picture->height = cfg->mp4h264Conf->dst_picture.height;
cfg->dst_picture->width = cfg->mp4h264Conf->dst_picture.width;
cfg->src_picture->height = cfg->mp4h264Conf->src_picture.height;
cfg->src_picture->width = cfg->mp4h264Conf->src_picture.width;
cfg->rowstride = cfg->dst_picture->width * cfg->bpp; // Size of the actual picture, alloc'd by MPEG4 / H264 stage
outToCopy = cfg->mp4h264Out;
}
if (NULL == outToCopy)
{
retVal = C_FAIL;
}
else
{
out->numBuffers = outToCopy->numBuffers;
out->buffers = outToCopy->buffers;
out->indexBuffer = outToCopy->indexBuffer;
out->size = outToCopy->size;
out->lineSize = outToCopy->lineSize;
out->status = outToCopy->status;
retVal = C_OK;
}
#if defined(TARGET_OS_IPHONE) || defined (TARGET_OS_IPHONE_SIMULATOR)
stopTime = mach_absolute_time ();
elapsedNano = (stopTime - startTime) * sTimebaseInfo.numer / sTimebaseInfo.denom;
if (0.0 == DEBUG_decodingTimeUsec)
{
DEBUG_decodingTimeUsec = elapsedNano / 1000.0;
}
else
{
DEBUG_decodingTimeUsec = 0.9 * DEBUG_decodingTimeUsec + (0.1 * elapsedNano / 1000.0);
}
#endif
#ifdef USE_LINUX
usleep(video_stage_decoder_fakeLatency * 1000);
#endif
return retVal;
}
C_RESULT video_stage_decoder_close (video_decoder_config_t *cfg)
{
C_RESULT res, resVlib, resMp4h264;
resVlib = vlib_stage_decoding_close (cfg->vlibConf);
resMp4h264 = mp4h264_close (cfg->mp4h264Conf);
res = (C_OK == resVlib && C_OK == resMp4h264 ) ? C_OK : C_FAIL;
vp_os_free (cfg->vlibConf);
cfg->vlibConf = NULL;
vp_os_free (cfg->vlibOut);
cfg->vlibOut = NULL;
vp_os_free (cfg->mp4h264Conf);
cfg->mp4h264Conf = NULL;
vp_os_free (cfg->mp4h264Out);
cfg->mp4h264Out = NULL;
return res;
}
#ifdef USE_ANDROID
#define NEONCHECK_BUFFER_STRING_SIZE (512)
void checkNeonSupport ()
{
neon_status_t loc_neonStat = NEON_SUPPORT_FAIL;
resetDecoderOnStreamChange = 0;
FILE *cpuInfo = fopen ("/proc/cpuinfo", "r");
if (NULL == cpuInfo)
{
return;
}
char *neonCheckStrBuf = vp_os_malloc (NEONCHECK_BUFFER_STRING_SIZE * sizeof (char));
if (NULL == neonCheckStrBuf)
{
fclose (cpuInfo);
return;
}
while (NULL != fgets (neonCheckStrBuf, NEONCHECK_BUFFER_STRING_SIZE, cpuInfo))
{
char *supportTest = strstr (neonCheckStrBuf, "neon");
if (NULL != supportTest)
{
loc_neonStat = NEON_SUPPORT_OK;
resetDecoderOnStreamChange = 1;
break;
}
}
vp_os_free (neonCheckStrBuf);
neonCheckStrBuf = NULL;
fclose (cpuInfo);
neonStatus = loc_neonStat;
}
C_RESULT mp4h264_open (mp4h264_config_t *cfg)
{
#if ITTIAM_SUPPORT
if (NEON_SUPPORT_UNKNOWN == neonStatus)
checkNeonSupport ();
if (NEON_SUPPORT_OK == neonStatus)
return ittiam_stage_decoding_open ((ittiam_stage_decoding_config_t *)cfg);
else
#endif
return ffmpeg_stage_decoding_open (cfg);
}
C_RESULT mp4h264_transform (mp4h264_config_t *cfg, vp_api_io_data_t *in, vp_api_io_data_t *out)
{
#if ITTIAM_SUPPORT
if (NEON_SUPPORT_UNKNOWN == neonStatus)
checkNeonSupport ();
if (NEON_SUPPORT_OK == neonStatus)
return ittiam_stage_decoding_transform ((ittiam_stage_decoding_config_t *)cfg, in, out);
else
#endif
return ffmpeg_stage_decoding_transform (cfg, in, out);
}
C_RESULT mp4h264_close (mp4h264_config_t *cfg)
{
#if ITTIAM_SUPPORT
if (NEON_SUPPORT_UNKNOWN == neonStatus)
checkNeonSupport ();
if (NEON_SUPPORT_OK == neonStatus)
return ittiam_stage_decoding_close ((ittiam_stage_decoding_config_t *)cfg);
else
#endif
return ffmpeg_stage_decoding_close (cfg);
}
#endif
#endif //FFMPEG_SUPPORT || ITTIAM_SUPPORT
ARDroneLib/Soft/Lib/ardrone_tool/Video/video_stage_decoder.h
-59
Ver Arquivo
@@ -1,59 +0,0 @@
/*
* video_stage_decoder.h
* ARDroneLib
*
* Created by n.brulez on 02/08/11.
* Copyright 2011 Parrot SA. All rights reserved.
*
*/
#if defined (FFMPEG_SUPPORT) || defined (ITTIAM_SUPPORT)
#ifndef __VIDEO_STAGE_DECODER_H__
#define __VIDEO_STAGE_DECODER_H__
#ifdef FFMPEG_SUPPORT
# define mp4h264_config_t ffmpeg_stage_decoding_config_t
#elif defined (ITTIAM_SUPPORT)
# define mp4h264_config_t ittiam_stage_decoding_config_t
#else
# error Either FFMPEG_SUPPORT or ITTIAM_SUPPORT must be defined
#endif
#ifdef ITTIAM_SUPPORT
# include <ardrone_tool/Video/video_stage_ittiam_decoder.h>
#endif
#ifdef FFMPEG_SUPPORT
# include <ardrone_tool/Video/video_stage_ffmpeg_decoder.h>
#endif
#include <VLIB/Stages/vlib_stage_decode.h>
typedef struct _video_decoder_config_t
{
// Input data : dst_picture->format
// Output : all others
vp_api_picture_t *src_picture;
vp_api_picture_t *dst_picture;
uint32_t num_frames;
uint32_t num_picture_decoded;
uint32_t rowstride;
uint32_t bpp;
// Internal datas
bool_t vlibMustChangeFormat;
vlib_stage_decoding_config_t *vlibConf;
vp_api_io_data_t *vlibOut;
mp4h264_config_t *mp4h264Conf;
vp_api_io_data_t *mp4h264Out;
} video_decoder_config_t;
C_RESULT video_stage_decoder_open (video_decoder_config_t *cfg);
C_RESULT video_stage_decoder_transform (video_decoder_config_t *cfg, vp_api_io_data_t *in, vp_api_io_data_t *out);
C_RESULT video_stage_decoder_close (video_decoder_config_t *cfg);
extern const vp_api_stage_funcs_t video_decoding_funcs;
#endif // __VIDEO_STAGE_DECODER_H__
#endif // FFMPEG_SUPPORT
ARDroneLib/Soft/Lib/ardrone_tool/Video/video_stage_encoded_recorder.c
-322
Ver Arquivo
@@ -1,322 +0,0 @@
#include <time.h>
#ifndef _WIN32
#include <sys/time.h>
#else
#include <sys/timeb.h>
#include <Winsock2.h> // for timeval structure
#endif
#include <VP_Api/vp_api.h>
#include <VP_Os/vp_os_malloc.h>
#include <VP_Api/vp_api_picture.h>
#include <ardrone_tool/Academy/academy.h>
#include <config.h>
#include <ardrone_tool/Video/video_stage_encoded_recorder.h>
#include <utils/ardrone_date.h>
#include <ardrone_tool/Video/video_navdata_handler.h>
#define VIDEO_ENCODED_RECORDER_VERBOSE (1)
#if defined(DEBUG) || VIDEO_ENCODED_RECORDER_VERBOSE
#define __VER_DEBUG (1)
#else
#define __VER_DEBUG (0)
#endif
#if __VER_DEBUG
#define VER_PRINT(...) \
do \
{ \
printf ("Video encoded recorder [%s @ %d] : ", __FUNCTION__, __LINE__); \
printf (__VA_ARGS__); \
printf ("\n"); \
} while (0)
#else
#define VER_PRINT(...)
#endif
/* Uncomment to force all apps to generate .mov format instead of .mp4 for non-apple devices */
//#define FORCE_MOV_FORMAT
#if defined(TARGET_OS_IPHONE) || defined (TARGET_OS_IPHONE_SIMULATOR) || defined (FORCE_MOV_FORMAT)
#define MOVIE_FILE_EXTENSION "mov"
#define VIDEO_FORMAT (ARDRONE_VIDEO_MOV)
#else
#define MOVIE_FILE_EXTENSION "mp4"
#define VIDEO_FORMAT (ARDRONE_VIDEO_MP4)
#endif
#ifndef VIDEO_ENCODED_FILE_DEFAULT_PATH
#define VIDEO_ENCODED_FILE_DEFAULT_PATH flight_dir
extern char flight_dir[];
#endif
const vp_api_stage_funcs_t video_encoded_recorder_funcs = {
(vp_api_stage_handle_msg_t) video_stage_encoded_recorder_handle,
(vp_api_stage_open_t) video_stage_encoded_recorder_open,
(vp_api_stage_transform_t) video_stage_encoded_recorder_transform,
(vp_api_stage_close_t) video_stage_encoded_recorder_close
};
video_stage_encoded_recorder_config_t video_stage_encoded_recorder_config;
C_RESULT video_stage_encoded_recorder_finish (video_stage_encoded_recorder_config_t *cfg)
{
C_RESULT retVal = C_OK;
endRetreiving ();
cfg->lastStreamId = cfg->currentStreamId;
ardrone_video_error_t vError = ardrone_video_finish (&(cfg->video));
if (ARDRONE_VIDEO_SUCCEEDED (vError))
{
VER_PRINT ("Video %s successfully written", cfg->video_filename);
if(cfg->finish_callback != NULL)
cfg->finish_callback((const char *)cfg->video_filename, FALSE);
}
else
{
VER_PRINT ("Error while completing video");
retVal = C_FAIL;
}
// Erase filename
vp_os_memset (cfg->video_filename, 0x0, VIDEO_ENCODED_FILENAME_LENGTH);
cfg->startRec = VIDEO_ENCODED_STOPPED;
return retVal;
}
C_RESULT video_stage_encoded_recorder_handle (video_stage_encoded_recorder_config_t * cfg, PIPELINE_MSG msg_id, void *callback, void *param)
{
vp_os_mutex_lock (&cfg->videoMutex);
void (*recorder_callback)(video_stage_encoded_recorder_config_t*) = callback;
switch (msg_id)
{
case PIPELINE_MSG_START: // video start
if(cfg->startRec==VIDEO_ENCODED_STOPPED)
{
cfg->startRec = VIDEO_ENCODED_START_RECORD;
time_t t;
char date[ARDRONE_DATE_MAXSIZE];
ardrone_time2date(*((uint32_t*)param), ARDRONE_FILE_DATE_FORMAT, date);
char media_dirname[VIDEO_ENCODED_FILENAME_LENGTH];
struct stat statbuf;
snprintf(media_dirname, VIDEO_ENCODED_FILENAME_LENGTH, "%s/media_%s", VIDEO_ENCODED_FILE_DEFAULT_PATH, date);
if((stat(media_dirname, &statbuf) != 0) && (mkdir(media_dirname, 0777) >= 0))
VER_PRINT("Create local media directory %s if not exist\n", media_dirname);
t = time(NULL);
ardrone_time2date(t, ARDRONE_FILE_DATE_FORMAT, date);
snprintf(cfg->video_filename,
VIDEO_ENCODED_FILENAME_LENGTH,
"%s/video_%s.%s",
media_dirname,
date,
MOVIE_FILE_EXTENSION);
}
break;
case PIPELINE_MSG_STOP: // video stop
if (cfg->startRec==VIDEO_ENCODED_START_RECORD)
{
cfg->startRec=VIDEO_ENCODED_STOPPED;
}
else if (cfg->startRec==VIDEO_ENCODED_RECORDING)
{
cfg->startRec = VIDEO_ENCODED_WAITING_STREAM_END;
startRetreiving ();
}
break;
case PIPELINE_MSG_SUSPEND: // Force stop
if (cfg->startRec==VIDEO_ENCODED_START_RECORD)
{
cfg->startRec=VIDEO_ENCODED_STOPPED;
}
else if (cfg->startRec==VIDEO_ENCODED_RECORDING ||
cfg->startRec==VIDEO_ENCODED_WAITING_STREAM_END)
{
video_stage_encoded_recorder_finish (cfg);
}
break;
default:
break;
}
if (recorder_callback!=NULL) {
recorder_callback(cfg);
}
vp_os_mutex_unlock (&cfg->videoMutex);
return (VP_SUCCESS);
}
bool_t video_stage_encoded_recorder_state(void)
{
return (video_stage_encoded_recorder_config.startRec != VIDEO_ENCODED_STOPPED);
}
video_encoded_record_state video_stage_encoded_complete_recorder_state (void)
{
return video_stage_encoded_recorder_config.startRec;
}
void video_stage_encoded_recorder_enable(bool_t enable, uint32_t timestamp)
{
printf("Recording %s...\n", enable ? "started" : "stopped");
video_stage_encoded_recorder_handle (&video_stage_encoded_recorder_config, (enable ? PIPELINE_MSG_START : PIPELINE_MSG_STOP), NULL, &timestamp);
}
void video_stage_encoded_recorder_com_timeout (void)
{
if (VIDEO_ENCODED_WAITING_STREAM_END == video_stage_encoded_recorder_config.startRec)
{
video_stage_encoded_recorder_force_stop ();
}
}
void video_stage_encoded_recorder_force_stop (void)
{
printf ("Force recording stop\n");
video_stage_encoded_recorder_handle (&video_stage_encoded_recorder_config, PIPELINE_MSG_SUSPEND, NULL, NULL);
}
C_RESULT video_stage_encoded_recorder_open(video_stage_encoded_recorder_config_t *cfg)
{
if (0 == cfg->fps)
{
cfg->fps = 30;
}
cfg->startRec=VIDEO_ENCODED_STOPPED;
cfg->lastStreamId = UINT16_MAX;
cfg->currentStreamId = UINT16_MAX;
cfg->video = NULL;
vp_os_mutex_init (&cfg->videoMutex);
ardrone_video_remove_all (VIDEO_ENCODED_FILE_DEFAULT_PATH);
return C_OK;
}
int frameIsLastFrame (uint8_t *data)
{
int retVal = 0;
if (PAVE_CHECK (data))
{
parrot_video_encapsulation_t *PaVE = (parrot_video_encapsulation_t *)data;
if (PAVE_CTRL_LAST_FRAME_IN_STREAM & PaVE->control)
{
retVal = 1;
}
}
return retVal;
}
C_RESULT video_stage_encoded_recorder_transform(video_stage_encoded_recorder_config_t *cfg, vp_api_io_data_t *in, vp_api_io_data_t *out)
{
vp_os_mutex_lock (&out->lock);
vp_os_mutex_lock (&cfg->videoMutex);
// Copy in to out
if (NULL != in && NULL != out)
{
out->numBuffers = in->numBuffers;
out->indexBuffer = in->indexBuffer;
out->lineSize = in->lineSize;
out->size = in->size;
out->status = in->status;
out->buffers = in->buffers;
out->status = VP_API_STATUS_PROCESSING;
}
uint8_t *slice = in->buffers[in->indexBuffer];
parrot_video_encapsulation_t *PaVE = (parrot_video_encapsulation_t *)slice;
if (cfg->lastStreamId == PaVE->stream_id)
{
VER_PRINT ("Got a video slice from an old stream, skipping\n");
vp_os_mutex_unlock (&cfg->videoMutex);
vp_os_mutex_unlock (&out->lock);
return C_OK;
}
if(cfg->startRec == VIDEO_ENCODED_START_RECORD)
{
ardrone_video_error_t vError = ARDRONE_VIDEO_NO_ERROR;
cfg->video = ardrone_video_start (cfg->video_filename, cfg->fps, VIDEO_FORMAT, &vError);
if (ARDRONE_VIDEO_SUCCEEDED (vError) && NULL != cfg->video)
{
cfg->startRec=VIDEO_ENCODED_RECORDING;
cfg->currentStreamId = PaVE->stream_id;
}
else
{
VER_PRINT ("An error occured when creating video");
cfg->startRec=VIDEO_ENCODED_STOPPED;
}
}
if((cfg->startRec == VIDEO_ENCODED_RECORDING) ||
(cfg->startRec == VIDEO_ENCODED_WAITING_STREAM_END))
{
#if USE_LINUX
/* Quick and dirty debugging inside AR.Drone Navigation */
if (PAVE_CHECK(slice))
{
parrot_video_encapsulation_t * pave = (parrot_video_encapsulation_t*) slice;
printf("Recording frame %dx%d num %d \n\033[1A",pave->display_width,pave->display_height,pave->frame_number);
if (pave->control & PAVE_CTRL_LAST_FRAME_IN_STREAM) {
printf("\n\nEnd of stream PaVE received !\n\n");
}
}
#endif
ardrone_video_error_t vError = ardrone_video_addSlice (cfg->video, slice);
if (ARDRONE_VIDEO_FAILED (vError))
{
char errBuf [50] = {0};
ardrone_video_error_string (vError, errBuf, 50);
VER_PRINT ("Error while recording frame : %s", errBuf);
ardrone_video_cleanup (&(cfg->video));
cfg->startRec = VIDEO_ENCODED_STOPPED;
}
if (1 == frameIsLastFrame (slice))
{
VER_PRINT ("Received last frame for current stream, finishing video\n");
video_stage_encoded_recorder_finish (cfg);
}
}
vp_os_mutex_unlock (&cfg->videoMutex);
vp_os_mutex_unlock (&out->lock);
return C_OK;
}
C_RESULT video_stage_encoded_recorder_close(video_stage_encoded_recorder_config_t *cfg)
{
if (NULL != cfg->video)
{
ardrone_video_finish (&(cfg->video));
}
return C_OK;
}
ARDroneLib/Soft/Lib/ardrone_tool/Video/video_stage_encoded_recorder.h
-83
Ver Arquivo
@@ -1,83 +0,0 @@
#ifndef _VIDEO_STAGE_ENCODED_RECORDER_H_
#define _VIDEO_STAGE_ENCODED_RECORDER_H_
#include <stdio.h>
#include <VP_Api/vp_api.h>
#include <utils/ardrone_video_encapsuler.h>
#define VIDEO_ENCODED_FILENAME_LENGTH 1024
#ifndef _VIDEO_ENCODED_RECORD_STATE_ENUM_
#define _VIDEO_ENCODED_RECORD_STATE_ENUM_
typedef enum
{
VIDEO_ENCODED_STOPPED = 0,
VIDEO_ENCODED_RECORDING,
VIDEO_ENCODED_START_RECORD,
VIDEO_ENCODED_WAITING_STREAM_END,
} video_encoded_record_state;
#endif
typedef void (*video_stage_encoded_recorder_callback)(const char *mediaPath, bool_t addToQueue);
typedef struct _video_stage_encoded_recorder_config_t
{
char video_filename[VIDEO_ENCODED_FILENAME_LENGTH];
ardrone_video_t *video;
video_encoded_record_state startRec;
uint32_t fps;
vp_os_mutex_t videoMutex;
int stage;
uint16_t lastStreamId;
uint16_t currentStreamId;
video_stage_encoded_recorder_callback finish_callback;
#ifdef USE_ELINUX
/* Asynchronous recording thread */
bool_t use_asynchronous_mode;
THREAD_HANDLE thread_handle;
int thread_priority;
void * thread;
int32_t quota; /*! Quantity of video data we can store */
int com_pipe[2];
#endif
} video_stage_encoded_recorder_config_t;
/*
* Message passed in the communication pipe in asynchronous mode.
*/
#ifdef USE_ELINUX
typedef struct{
void * slice;
int sliceSize;
}video_stage_encoded_recorder_msg_t;
#endif
C_RESULT video_stage_encoded_recorder_handle (video_stage_encoded_recorder_config_t *cfg, PIPELINE_MSG msg_id, void *callback, void *param);
C_RESULT video_stage_encoded_recorder_open(video_stage_encoded_recorder_config_t *cfg);
C_RESULT video_stage_encoded_recorder_transform(video_stage_encoded_recorder_config_t *cfg, vp_api_io_data_t *in, vp_api_io_data_t *out);
C_RESULT video_stage_encoded_recorder_close(video_stage_encoded_recorder_config_t *cfg);
video_encoded_record_state
video_stage_encoded_complete_recorder_state (void);
void video_stage_encoded_recorder_enable (bool_t enable, uint32_t timestamp);
void video_stage_encoded_recorder_force_stop (void);
void video_stage_encoded_recorder_com_timeout (void);
bool_t video_stage_encoded_recorder_state (void);
C_RESULT video_stage_encoded_recorder_finish (video_stage_encoded_recorder_config_t *cfg);
int frameIsLastFrame (uint8_t *data);
extern const vp_api_stage_funcs_t video_encoded_recorder_funcs;
extern video_stage_encoded_recorder_config_t video_stage_encoded_recorder_config;
#ifdef USE_ELINUX
#define VIDEO_STAGE_ENCODED_RECORDER_STACK_SIZE (8192)
PROTO_THREAD_ROUTINE_STACK(video_stage_encoded_recorder,param,VIDEO_STAGE_ENCODED_RECORDER_STACK_SIZE);
#endif
#endif // _VIDEO_STAGE_ENCODED_RECORDER_H_
ARDroneLib/Soft/Lib/ardrone_tool/Video/video_stage_ffmpeg_decoder.c
-522
Ver Arquivo
@@ -1,522 +0,0 @@
/*
* video_stage_ffmpeg_decoder.c
* ARDroneLib
*
* Created by f.dhaeyer on 09/12/10.
* Copyright 2010 Parrot SA. All rights reserved.
*
*/
#ifdef FFMPEG_SUPPORT
#include <VP_Os/vp_os_malloc.h>
#include <VP_Os/vp_os_print.h>
#include <ardrone_tool/Video/video_stage_ffmpeg_decoder.h>
#include <Maths/time.h>
#include <math.h>
#include <sys/time.h>
#include <video_encapsulation.h>
#include <sys/types.h>
#include <sys/stat.h>
#include <fcntl.h>
/* CONFIGURATION */
/**
* Enable (1) to display FPS
*/
#define DISPLAY_FPS (0)
/**
* Display FPS after N frames
* - default : 20
*/
#define DISPLAY_FPS_FRAMES (60)
/**
* Enable (1) to display missed/dropped frames
*/
#define DISPLAY_DROPPED_FRAMES (0)
/**
* Enable (1) to allow decoding of non-PaVE MPEG4/h264 frames
* Disable (0) for compatibility with VLIB/P264 (won't try to decode if no PaVE is found)
*/
#define FAKE_PaVE (1)
/**
* Default codec for non-PaVE frames :
* (0) - h264
* (1) - MPEG4
*/
#define FAKE_PaVE_CODEC_ID (0)
/**
* Wait for I-Frame mode :
* - (0) : display each received frame
* - (1) : when a frame is lost, wait for next I-Frame
*/
#define WAIT_FOR_I_FRAME (1)
#define FFMPEG_DEBUG_ENABLE_ON_GLOBAL_DEBUG (0) ///< Auto enable debug while on debug mode
#define FFMPEG_DEBUG_ENABLE (0) ///< Force enable of debug for this file
/* END OF CONFIGURATION */
#if DISPLAY_FPS
#define NUM_SAMPLES DISPLAY_FPS_FRAMES
#endif
#if FAKE_PaVE_CODEC_ID == 0
#define FAKE_PaVE_CODEC CODEC_MPEG4_AVC
#elif FAKE_PaVE_CODEC_ID == 1
#define FAKE_PaVE_CODEC CODEC_MPEG4_VISUAL
#else
#warning FAKE_PaVE_CODEC_ID : unknown value -> falling back to MPEG4
#define FAKE_PaVE_CODEC CODEC_MPEG4_VISUAL
#endif
#if FFMPEG_DEBUG_ENABLE || (FFMPEG_DEBUG_ENABLE_ON_GLOBAL_DEBUG && defined (DEBUG))
#define __FFMPEG_DEBUG_ENABLED (1)
#define FFMPEG_LOG_LEVEL (AV_LOG_WARNING)
#else
#define __FFMPEG_DEBUG_ENABLED (0)
#define FFMPEG_LOG_LEVEL (AV_LOG_QUIET)
#endif
#if __FFMPEG_DEBUG_ENABLED
#define FFMPEG_DEBUG(...) \
do \
{ \
printf ("FFMPEG-DEBUG (%s @ %d) : ", __FUNCTION__, __LINE__); \
printf (__VA_ARGS__); \
printf ("\n"); \
} while (0)
#else
#define FFMPEG_DEBUG(...)
#endif
#if DISPLAY_DROPPED_FRAMES
int missed_frames = 0;
int dropped_frames = 0;
int previous_ok_frame = 0;
#endif
const vp_api_stage_funcs_t ffmpeg_decoding_funcs = {
(vp_api_stage_handle_msg_t) NULL,
(vp_api_stage_open_t) ffmpeg_stage_decoding_open,
(vp_api_stage_transform_t) ffmpeg_stage_decoding_transform,
(vp_api_stage_close_t) ffmpeg_stage_decoding_close
};
C_RESULT ffmpeg_stage_decoding_open(ffmpeg_stage_decoding_config_t *cfg)
{
cfg->num_picture_decoded = 0;
/* must be called before using avcodec lib */
avcodec_init();
/* register all the codecs */
avcodec_register_all();
av_log_set_level(FFMPEG_LOG_LEVEL);
cfg->pCodecMP4 = avcodec_find_decoder (CODEC_ID_MPEG4);
cfg->pCodecH264 = avcodec_find_decoder (CODEC_ID_H264);
if(NULL == cfg->pCodecMP4 || NULL == cfg->pCodecH264)
{
fprintf(stderr, "Unsupported codec!\n");
return C_FAIL; // Codec not found
}
cfg->pCodecCtxMP4 = avcodec_alloc_context();
cfg->pCodecCtxH264 = avcodec_alloc_context();
if (NULL == cfg->pCodecCtxMP4 || NULL == cfg->pCodecCtxH264)
{
fprintf(stderr, "Impossible to allocate codec context\n");
return C_FAIL; // Allocation failure
}
cfg->pCodecCtxMP4->pix_fmt = PIX_FMT_YUV420P;
cfg->pCodecCtxMP4->skip_frame = AVDISCARD_DEFAULT;
cfg->pCodecCtxMP4->error_concealment = FF_EC_GUESS_MVS | FF_EC_DEBLOCK;
cfg->pCodecCtxMP4->error_recognition = FF_ER_CAREFUL;
cfg->pCodecCtxMP4->skip_loop_filter = AVDISCARD_DEFAULT;
cfg->pCodecCtxMP4->workaround_bugs = FF_BUG_AUTODETECT;
cfg->pCodecCtxMP4->codec_type = AVMEDIA_TYPE_VIDEO;
cfg->pCodecCtxMP4->codec_id = CODEC_ID_MPEG4;
cfg->pCodecCtxMP4->skip_idct = AVDISCARD_DEFAULT;
cfg->pCodecCtxH264->pix_fmt = PIX_FMT_YUV420P;
cfg->pCodecCtxH264->skip_frame = AVDISCARD_DEFAULT;
cfg->pCodecCtxH264->error_concealment = FF_EC_GUESS_MVS | FF_EC_DEBLOCK;
cfg->pCodecCtxH264->error_recognition = FF_ER_CAREFUL;
cfg->pCodecCtxH264->skip_loop_filter = AVDISCARD_DEFAULT;
cfg->pCodecCtxH264->workaround_bugs = FF_BUG_AUTODETECT;
cfg->pCodecCtxH264->codec_type = AVMEDIA_TYPE_VIDEO;
cfg->pCodecCtxH264->codec_id = CODEC_ID_H264;
cfg->pCodecCtxH264->skip_idct = AVDISCARD_DEFAULT;
// Open codec
if(avcodec_open(cfg->pCodecCtxMP4, cfg->pCodecMP4) < 0)
{
fprintf (stderr, "Error opening MP4 codec\n");
return C_FAIL;
}
if(avcodec_open(cfg->pCodecCtxH264, cfg->pCodecH264) < 0)
{
fprintf (stderr, "Error opening h264 codec\n");
return C_FAIL;
}
cfg->pFrameOutput = avcodec_alloc_frame();
cfg->pFrame = avcodec_alloc_frame();
if (NULL == cfg->pFrameOutput || NULL == cfg->pFrame)
{
fprintf (stderr, "Unable to alloc frames");
return C_FAIL;
}
cfg->bufferArray = (uint8_t **)vp_os_malloc (sizeof (uint8_t *));
if (NULL == cfg->bufferArray)
{
fprintf (stderr, "Unable to alloc output buffer");
return C_FAIL;
}
cfg->buffer = NULL;
cfg->img_convert_ctx = NULL;
return C_OK;
}
void empty_av_log_callback (void *ptr, int level, const char *fmt, va_list vl)
{
// Empty callback so we can hide all ffmpeg av_log outputs
}
#if __FFMPEG_DEBUG_ENABLED
void ffmpeg_decoder_dumpPave (parrot_video_encapsulation_t *PaVE)
{
printf ("Signature : \"%c%c%c%c\" [0x%02x][0x%02x][0x%02x][0x%02x]\n", PaVE->signature[0], PaVE->signature[1],
PaVE->signature[2], PaVE->signature[3], PaVE->signature[0], PaVE->signature[1], PaVE->signature[2], PaVE->signature[3]);
printf ("Codec : %s\n", (PaVE->video_codec == CODEC_MPEG4_VISUAL) ? "MP4" : ((PaVE->video_codec == CODEC_MPEG4_AVC) ? "H264" : "Unknown"));
printf ("StreamID : %d \n", PaVE->stream_id);
printf ("Encoded dims : %d x %d\n", PaVE->encoded_stream_width, PaVE->encoded_stream_height);
printf ("Display dims : %d x %d\n", PaVE->display_width, PaVE->display_height);
printf ("Header size : %d (PaVE size : %u)\n", PaVE->header_size, sizeof (parrot_video_encapsulation_t));
printf ("Payload size : %d\n", PaVE->payload_size);
printf ("Frame Type / Number : %s : %d : slide %d/%d\n",
(PaVE->frame_type == FRAME_TYPE_P_FRAME) ? "P-Frame" : ((PaVE->frame_type == FRAME_TYPE_I_FRAME) ? "I-Frame" : "IDR-Frame"),
PaVE->frame_number,
PaVE->slice_index+1,
PaVE->total_slices);
}
#endif
static inline bool_t check_and_copy_PaVE (parrot_video_encapsulation_t *PaVE, vp_api_io_data_t *data, parrot_video_encapsulation_t *prevPaVE, bool_t *dimChanged)
{
parrot_video_encapsulation_t *localPaVE = (parrot_video_encapsulation_t *)data->buffers[data->indexBuffer];
if (localPaVE->signature[0] == 'P' &&
localPaVE->signature[1] == 'a' &&
localPaVE->signature[2] == 'V' &&
localPaVE->signature[3] == 'E')
{
//FFMPEG_DEBUG("Found a PaVE");
vp_os_memcpy (prevPaVE, PaVE, sizeof (parrot_video_encapsulation_t)); // Make a backup of previous PaVE so we can check if things have changed
vp_os_memcpy (PaVE, localPaVE, sizeof (parrot_video_encapsulation_t)); // Copy PaVE to our local one
#if __FFMPEG_DEBUG_ENABLED
printf ("------------------------------------\n");
printf ("PREV : ");
ffmpeg_decoder_dumpPave (prevPaVE);
printf ("CURR : ");
ffmpeg_decoder_dumpPave (PaVE);
printf ("------------------------------------\n");
#endif
if (prevPaVE->encoded_stream_width != PaVE->encoded_stream_width ||
prevPaVE->encoded_stream_height != PaVE->encoded_stream_height ||
prevPaVE->display_width != PaVE->display_width ||
prevPaVE->display_width != PaVE->display_width ||
prevPaVE->stream_id != PaVE->stream_id )
{
*dimChanged = TRUE;
}
else
{
*dimChanged = FALSE;
}
data->size = localPaVE->payload_size;
memmove(data->buffers[data->indexBuffer], &(data->buffers[data->indexBuffer])[localPaVE->header_size], data->size);
#if DISPLAY_DROPPED_FRAMES
missed_frames += PaVE->frame_number - prevPaVE->frame_number - 1;
#endif
return TRUE;
}
else
{
FFMPEG_DEBUG("No PaVE, signature was [%c][%c][%c][%c]",
localPaVE->signature[0],
localPaVE->signature[1],
localPaVE->signature[2],
localPaVE->signature[3]);
#if FAKE_PaVE
PaVE->encoded_stream_width = 640;
PaVE->encoded_stream_height = 368;
PaVE->display_width = 640;
PaVE->display_height = 360;
PaVE->video_codec = FAKE_PaVE_CODEC;
PaVE->frame_type = FRAME_TYPE_I_FRAME;
vp_os_memcpy (prevPaVE, PaVE, sizeof (parrot_video_encapsulation_t));
*dimChanged = FALSE;
return TRUE;
#else
return FALSE;
#endif
}
}
C_RESULT ffmpeg_stage_decoding_transform(ffmpeg_stage_decoding_config_t *cfg, vp_api_io_data_t *in, vp_api_io_data_t *out)
{
static const int sws_flags = SWS_FAST_BILINEAR;
AVCodecContext *pCodecCtxMP4 = cfg->pCodecCtxMP4;
AVCodecContext *pCodecCtxH264 = cfg->pCodecCtxH264;
AVFrame *pFrame = cfg->pFrame;
AVFrame *pFrameOutput = cfg->pFrameOutput;
static AVPacket packet;
int frameFinished = 0;
bool_t frameDimChanged = FALSE;
static parrot_video_encapsulation_t __attribute__ ((aligned (4))) PaVE;
static parrot_video_encapsulation_t __attribute__ ((aligned (4))) prevPaVE;
#if WAIT_FOR_I_FRAME
static bool_t waitForIFrame = TRUE;
#endif
#ifdef NUM_SAMPLES
static struct timeval start_time, start_time2;
static int numsamples = 0;
#endif
if (0 == in->size) // No frame
{
FFMPEG_DEBUG ("in->size is zero, don't do anything");
return C_OK;
}
vp_os_mutex_lock( &out->lock );
if(out->status == VP_API_STATUS_INIT) // Init only code
{
out->numBuffers = 1;
out->buffers = cfg->bufferArray;
out->buffers[0] = NULL;
out->indexBuffer = 0;
out->lineSize = 0;
av_init_packet(&packet);
#if __FFMPEG_DEBUG_ENABLED
#else
av_log_set_callback (&empty_av_log_callback);
#endif
}
if (! check_and_copy_PaVE(&PaVE, in, &prevPaVE, &frameDimChanged))
{
FFMPEG_DEBUG("Received a frame without PaVE informations");
vp_os_mutex_unlock( &out->lock );
return C_FAIL;
}
if ((out->status == VP_API_STATUS_INIT) || frameDimChanged) // Init and "new frame dimensions" code
{
pCodecCtxMP4->width = PaVE.encoded_stream_width;
pCodecCtxMP4->height = PaVE.encoded_stream_height;
pCodecCtxH264->width = PaVE.encoded_stream_width;
pCodecCtxH264->height = PaVE.encoded_stream_height;
cfg->src_picture.width = PaVE.display_width;
cfg->src_picture.height = PaVE.display_height;
cfg->src_picture.format = pCodecCtxH264->pix_fmt;
cfg->dst_picture.width = PaVE.display_width;
cfg->dst_picture.height = PaVE.display_height;
out->size = avpicture_get_size(cfg->dst_picture.format, cfg->dst_picture.width, cfg->dst_picture.height);
cfg->buffer = (uint8_t *)av_realloc(cfg->buffer, out->size * sizeof(uint8_t));
out->buffers[0] = cfg->buffer;
avpicture_fill((AVPicture *)pFrameOutput, (uint8_t*)out->buffers[out->indexBuffer], cfg->dst_picture.format,
cfg->dst_picture.width, cfg->dst_picture.height);
cfg->img_convert_ctx = sws_getCachedContext(cfg->img_convert_ctx, PaVE.display_width, PaVE.display_height,
pCodecCtxH264->pix_fmt, PaVE.display_width, PaVE.display_height,
cfg->dst_picture.format, sws_flags, NULL, NULL, NULL);
if (out->status == VP_API_STATUS_INIT)
{
#ifdef NUM_SAMPLES
gettimeofday(&start_time, NULL);
#endif
out->status = VP_API_STATUS_PROCESSING;
FFMPEG_DEBUG("End of init");
}
}
#if WAIT_FOR_I_FRAME
if ( (PaVE.frame_number != (prevPaVE.frame_number +1))
&&
( PaVE.frame_number != prevPaVE.frame_number || PaVE.slice_index != (prevPaVE.slice_index+1) ) )
{
FFMPEG_DEBUG ("Missed a frame :\nPrevious was %d of type %d\nNew is %d of type %d", prevPaVE.frame_number, prevPaVE.frame_type,
PaVE.frame_number, PaVE.frame_type);
waitForIFrame = TRUE;
}
#if DISPLAY_DROPPED_FRAMES
if (waitForIFrame && PaVE.frame_type == FRAME_TYPE_P_FRAME)
{
FFMPEG_DEBUG ("Dropped a P frame\n");
dropped_frames++;
}
#endif
if(out->status == VP_API_STATUS_PROCESSING && (!waitForIFrame || (PaVE.frame_type == FRAME_TYPE_IDR_FRAME) || (PaVE.frame_type == FRAME_TYPE_I_FRAME))) // Processing code
{
waitForIFrame = FALSE;
#else
if(out->status == VP_API_STATUS_PROCESSING) // Processing code
{
#endif
/* The 'check_and_copy_PaVE' function already removed the PaVE from the 'in' buffer */
packet.data = ((unsigned char*)in->buffers[in->indexBuffer]);
packet.size = in->size;
FFMPEG_DEBUG("Size : %d", packet.size);
#ifdef NUM_SAMPLES
struct timeval end_time;
static float32_t frame_decoded_time = 0;
gettimeofday(&start_time2, NULL);
#endif
// Decode video frame
if (PaVE.video_codec == CODEC_MPEG4_VISUAL)
{
avcodec_decode_video2 (pCodecCtxMP4, pFrame, &frameFinished, &packet);
}
else if (PaVE.video_codec == CODEC_MPEG4_AVC)
{
avcodec_decode_video2 (pCodecCtxH264, pFrame, &frameFinished, &packet);
}
// Did we get a video frame?
if(frameFinished)
{
pFrameOutput->data[0] = (uint8_t*)out->buffers[out->indexBuffer];
sws_scale(cfg->img_convert_ctx, (const uint8_t *const*)pFrame->data,
pFrame->linesize, 0,
PaVE.display_height,
pFrameOutput->data, pFrameOutput->linesize);
cfg->num_picture_decoded++;
#ifdef NUM_SAMPLES
gettimeofday(&end_time, NULL);
frame_decoded_time += ((end_time.tv_sec * 1000.0 + end_time.tv_usec / 1000.0) - (start_time2.tv_sec * 1000.0 + start_time2.tv_usec / 1000.0));
if(numsamples++ > NUM_SAMPLES)
{
float32_t value = ((end_time.tv_sec * 1000.0 + end_time.tv_usec / 1000.0) - (start_time.tv_sec * 1000.0 + start_time.tv_usec / 1000.0));
printf("Frames decoded in average %f fps, received and decoded in average %f fps\n", (1000.0 / (frame_decoded_time / (float32_t)NUM_SAMPLES)), 1000.0 / (value / (float32_t)NUM_SAMPLES));
gettimeofday(&start_time, NULL);
frame_decoded_time = 0;
numsamples = 0;
}
#endif
}
else
{
/* Skip frames are usually 7 bytes long
* and make FFMPEG return an error. It is however normal to get
* skip frames from the drone.
*/
if (7!=PaVE.payload_size)
printf ("Decoding failed for a %s\n", (PaVE.frame_type == FRAME_TYPE_P_FRAME) ? "P Frame" : "I Frame");
}
#if DISPLAY_DROPPED_FRAMES
if ((PaVE.frame_type == FRAME_TYPE_IDR_FRAME) || (PaVE.frame_type == FRAME_TYPE_I_FRAME))
{
if (previous_ok_frame != 0)
{
static int globalMiss = 0, globalDrop = 0, globalFrames = 0;
globalMiss += missed_frames;
globalDrop += dropped_frames;
int globalMissDrop = globalMiss + globalDrop;
int total_miss = missed_frames + dropped_frames;
int total_frames = PaVE.frame_number - previous_ok_frame;
globalFrames += total_frames;
float missPercent = (100.0 * missed_frames) / (1.0 * total_frames);
float dropPercent = (100.0 * dropped_frames) / (1.0 * total_frames);
float totalPercent = (100.0 * total_miss) / (1.0 * total_frames);
float missMean = (100.0 * globalMiss) / (1.0 * globalFrames);
float dropMean = (100.0 * globalDrop) / (1.0 * globalFrames);
float totalMean = (100.0 * globalMissDrop) / (1.0 * globalFrames);
printf ("LAST %4d F => M %4d (%4.1f%%) / D %4d (%4.1f%%) / T %4d (%4.1f%%) <=> ALL %4d F => M %4d (%4.1f%%) / D %4d (%4.1f%%) / T %4d (%4.1f%%)\n", total_frames, missed_frames, missPercent, dropped_frames, dropPercent, total_miss, totalPercent, globalFrames, globalMiss, missMean, globalDrop, dropMean, globalMissDrop, totalMean);
}
missed_frames = 0; dropped_frames = 0;
previous_ok_frame = PaVE.frame_number;
}
#endif
}
vp_os_mutex_unlock( &out->lock );
return C_OK;
}
#define FFMPEG_CHECK_AND_FREE(pointer, freeFunc) \
do \
{ \
if (NULL != pointer) \
{ \
freeFunc (pointer); \
pointer = NULL; \
} \
} while (0)
#define FFMPEG_CHECK_AND_FREE_WITH_CALL(pointer, func, freeFunc) \
do \
{ \
if (NULL != pointer) \
{ \
func (pointer); \
freeFunc (pointer); \
pointer = NULL; \
} \
} while (0)
C_RESULT ffmpeg_stage_decoding_close(ffmpeg_stage_decoding_config_t *cfg)
{
FFMPEG_CHECK_AND_FREE_WITH_CALL(cfg->pCodecCtxMP4, avcodec_close, av_free);
FFMPEG_CHECK_AND_FREE_WITH_CALL(cfg->pCodecCtxH264, avcodec_close, av_free);
FFMPEG_CHECK_AND_FREE(cfg->pFrame, av_free);
FFMPEG_CHECK_AND_FREE(cfg->pFrameOutput, av_free);
FFMPEG_CHECK_AND_FREE(cfg->bufferArray, vp_os_free);
FFMPEG_CHECK_AND_FREE(cfg->buffer, av_free);
FFMPEG_CHECK_AND_FREE(cfg->img_convert_ctx, sws_freeContext);
return C_OK;
}
#endif
ARDroneLib/Soft/Lib/ardrone_tool/Video/video_stage_ffmpeg_decoder.h
-52
Ver Arquivo
@@ -1,52 +0,0 @@
/*
* video_stage_ffmpeg_decoder.h
* ARDroneLib
*
* Created by f.dhaeyer on 09/12/10.
* Copyright 2010 Parrot SA. All rights reserved.
*
*/
#ifdef FFMPEG_SUPPORT
#ifndef _VIDEO_STAGE_FFMPEG_DECODER_H_
#define _VIDEO_STAGE_FFMPEG_DECODER_H_
#include <VP_Api/vp_api.h>
#include <libavcodec/avcodec.h>
#include <libavformat/avformat.h>
#include <libswscale/swscale.h>
typedef struct _ffmpeg_picture_
{
enum PixelFormat format;
uint32_t width;
uint32_t height;
} ffmpeg_picture;
typedef struct _ffmpeg_stage_decoding_config_t
{
ffmpeg_picture dst_picture;
ffmpeg_picture src_picture;
uint32_t num_picture_decoded;
/* Alloced data pointers are saved into cfg to avoid memory leaks */
AVCodec *pCodecMP4;
AVCodec *pCodecH264;
AVCodecContext *pCodecCtxMP4;
AVCodecContext *pCodecCtxH264;
AVFrame *pFrame;
AVFrame *pFrameOutput;
uint8_t **bufferArray; // out->buffers
uint8_t *buffer; // out->buffers[0]
struct SwsContext *img_convert_ctx;
} ffmpeg_stage_decoding_config_t;
C_RESULT ffmpeg_stage_decoding_open(ffmpeg_stage_decoding_config_t *cfg);
C_RESULT ffmpeg_stage_decoding_transform(ffmpeg_stage_decoding_config_t *cfg, vp_api_io_data_t *in, vp_api_io_data_t *out);
C_RESULT ffmpeg_stage_decoding_close(ffmpeg_stage_decoding_config_t *cfg);
extern const vp_api_stage_funcs_t ffmpeg_decoding_funcs;
#endif // _VIDEO_STAGE_FFMPEG_DECODER_H_
#endif // FFMPEG_SUPPORT
ARDroneLib/Soft/Lib/ardrone_tool/Video/video_stage_ffmpeg_recorder.c
-609
Ver Arquivo
@@ -1,609 +0,0 @@
/* CODE ADAPATED FOR ARDRONE NAVIGATION FROM :
*
* Libavformat API example: Output a media file in any supported
* libavformat format. The default codecs are used.
*
* Copyright (c) 2003 Fabrice Bellard
*
* Permission is hereby granted, free of charge, to any person obtaining a copy
* of this software and associated documentation files (the "Software"), to deal
* in the Software without restriction, including without limitation the rights
* to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
* copies of the Software, and to permit persons to whom the Software is
* furnished to do so, subject to the following conditions:
*
* The above copyright notice and this permission notice shall be included in
* all copies or substantial portions of the Software.
*
* THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
* IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
* FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL
* THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
* LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
* OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN
* THE SOFTWARE.
*/
#if defined (FFMPEG_SUPPORT) && defined (RECORD_FFMPEG_VIDEO)
#include <generated_custom.h>
#include <stdlib.h>
#include <stdio.h>
#include <string.h>
#include <math.h>
#include <libavformat/avformat.h>
#include <libswscale/swscale.h>
#include <libavfilter/avfilter.h>
#include <libavutil/avutil.h>
#include <libavcodec/avcodec.h>
#include <libavcodec/opt.h>
#include <unistd.h>
#include <sys/types.h>
#include <sys/stat.h>
#include <fcntl.h>
#include <time.h>
#ifndef _WIN32
#include <sys/time.h>
#else
#include <sys/timeb.h>
#include <Winsock2.h> // for timeval structure
int gettimeofday (struct timeval *tp, void *tz)
{
struct _timeb timebuffer;
_ftime (&timebuffer);
tp->tv_sec = (long)timebuffer.time;
tp->tv_usec = (long)timebuffer.millitm * 1000;
return 0;
}
#endif
#include <VP_Os/vp_os_malloc.h>
#include <VP_Api/vp_api_picture.h>
#include <config.h>
#include <ardrone_tool/Video/video_stage_ffmpeg_recorder.h>
#define VIDEO_FILE_DEFAULT_PATH root_dir
extern char root_dir[];
/*- LibAVFormat variables */
const char *filename;
AVOutputFormat *fmt;
AVFormatContext *oc;
AVStream *video_st;
double video_pts;
int i;
#define STREAM_BIT_RATE_KBITS 1600
static const int sws_flags = SWS_BICUBIC;
AVFrame *picture_to_encode=NULL, *tmp_picture=NULL;
uint8_t *video_outbuf=NULL;
int frame_count=0, video_outbuf_size=0;
const vp_api_stage_funcs_t video_ffmpeg_recorder_funcs = {
(vp_api_stage_handle_msg_t) video_stage_ffmpeg_recorder_handle,
(vp_api_stage_open_t) video_stage_ffmpeg_recorder_open,
(vp_api_stage_transform_t) video_stage_ffmpeg_recorder_transform,
(vp_api_stage_close_t) video_stage_ffmpeg_recorder_close
};
struct
{
int width,height;
char* buffer;
int frame_number;
}previous_frame;
/******************************************************************************************************************************************/
void create_video_file(const char*filename,int width,int height,int frame_rate, enum PixelFormat pix_fmt)
{
/* auto detect the output format from the name. default is mpeg. */
fmt = av_guess_format(NULL, filename, NULL);
if (!fmt) {
fprintf(stderr, "Could not deduce output format from file extension: using MPEG.\n");
fmt = av_guess_format("mpeg", NULL, NULL);
}
if (!fmt) {
fprintf(stderr, "Could not find suitable output format\n");
exit(1);
}
/* allocate the output media context */
oc = avformat_alloc_context();
if (!oc) {
fprintf(stderr, "Memory error\n");
exit(1);
}
oc->oformat = fmt;
snprintf(oc->filename, sizeof(oc->filename), "%s", filename);
/* add the audio and video streams using the default format codecs
and initialize the codecs */
video_st = NULL;
if (fmt->video_codec != CODEC_ID_NONE) {
video_st = add_video_stream(oc, fmt->video_codec,width,height,frame_rate, pix_fmt);
}
/* set the output parameters (must be done even if no
parameters). */
if (av_set_parameters(oc, NULL) < 0) {
fprintf(stderr, "Invalid output format parameters\n");
exit(1);
}
av_dump_format(oc, 0, filename, 1);
/* now that all the parameters are set, we can open the audio and
video codecs and allocate the necessary encode buffers */
if (video_st)
open_video(oc, video_st);
/* open the output file, if needed */
if (!(fmt->flags & AVFMT_NOFILE)) {
if (avio_open(&oc->pb, filename, URL_WRONLY) < 0) {
fprintf(stderr, "Could not open '%s'\n", filename);
exit(1);
}
}
/* write the stream header, if any */
av_write_header(oc);
}
void close_video_file()
{
/* write the trailer, if any. the trailer must be written
* before you close the CodecContexts open when you wrote the
* header; otherwise write_trailer may try to use memory that
* was freed on av_codec_close() */
av_write_trailer(oc);
/* close each codec */
if (video_st)
close_video(oc, video_st);
/* free the streams */
for(i = 0; i < oc->nb_streams; i++) {
av_freep(&oc->streams[i]->codec);
av_freep(&oc->streams[i]);
}
if (!(fmt->flags & AVFMT_NOFILE)) {
/* close the output file */
avio_close(oc->pb);
}
/* free the stream */
av_free(oc);
}
/**************************************************************/
/* video output */
/* add a video output stream */
AVStream *add_video_stream(AVFormatContext *oc, enum CodecID codec_id, int width, int height,int frame_rate, enum PixelFormat pix_fmt)
{
AVCodecContext *c;
AVStream *st;
st = av_new_stream(oc, 0);
if (!st) {
fprintf(stderr, "Could not alloc stream\n");
exit(1);
}
c = st->codec;
c->codec_id = codec_id;
c->codec_type = AVMEDIA_TYPE_VIDEO;
/* put sample parameters */
c->bit_rate = (STREAM_BIT_RATE_KBITS)*1000;
/* resolution must be a multiple of two */
c->width = width;
c->height = height;
/* time base: this is the fundamental unit of time (in seconds) in terms
of which frame timestamps are represented. for fixed-fps content,
timebase should be 1/framerate and timestamp increments should be
identically 1. */
c->time_base.den = frame_rate;
c->time_base.num = 1;
c->gop_size = 12; /* emit one intra frame every twelve frames at most */
c->pix_fmt = pix_fmt;
if (c->codec_id == CODEC_ID_MPEG2VIDEO) {
/* just for testing, we also add B frames */
c->max_b_frames = 2;
}
if (c->codec_id == CODEC_ID_MPEG1VIDEO){
/* Needed to avoid using macroblocks in which some coeffs overflow.
This does not happen with normal video, it just happens here as
the motion of the chroma plane does not match the luma plane. */
c->mb_decision=2;
}
// some formats want stream headers to be separate
if(oc->oformat->flags & AVFMT_GLOBALHEADER)
c->flags |= CODEC_FLAG_GLOBAL_HEADER;
return st;
}
AVFrame *alloc_picture(enum PixelFormat pix_fmt, int width, int height)
{
AVFrame *picture;
uint8_t *picture_buf;
int size;
picture = avcodec_alloc_frame();
if (!picture)
return NULL;
size = avpicture_get_size(pix_fmt, width, height);
picture_buf = av_malloc(size);
if (!picture_buf) {
av_free(picture);
return NULL;
}
avpicture_fill((AVPicture *)picture, picture_buf,
pix_fmt, width, height);
return picture;
}
void open_video(AVFormatContext *oc, AVStream *st)
{
AVCodec *codec;
AVCodecContext *c;
c = st->codec;
/* find the video encoder */
codec = avcodec_find_encoder(c->codec_id);
if (!codec) {
fprintf(stderr, "codec not found\n");
exit(1);
}
/* open the codec */
if (avcodec_open(c, codec) < 0) {
fprintf(stderr, "could not open codec\n");
exit(1);
}
video_outbuf = NULL;
if (!(oc->oformat->flags & AVFMT_RAWPICTURE)) {
/* allocate output buffer */
/* XXX: API change will be done */
/* buffers passed into lav* can be allocated any way you prefer,
as long as they're aligned enough for the architecture, and
they're freed appropriately (such as using av_free for buffers
allocated with av_malloc) */
video_outbuf_size = 200000;
video_outbuf = av_malloc(video_outbuf_size);
}
/* allocate the encoded raw picture */
picture_to_encode = avcodec_alloc_frame();
/* if the output format is not YUV420P, then a temporary YUV420P
picture is needed too. It is then converted to the required
output format */
tmp_picture = NULL;
if (c->pix_fmt != PIX_FMT_YUV420P) {
tmp_picture = alloc_picture(PIX_FMT_YUV420P, c->width, c->height);
if (!tmp_picture) {
fprintf(stderr, "Could not allocate temporary picture\n");
exit(1);
}
}
}
void write_video_frame(AVFormatContext *oc, AVStream *st)
{
int out_size, ret;
AVCodecContext *c;
static struct SwsContext *img_convert_ctx;
//printf("Here0 \n");
c = st->codec;
if (c->pix_fmt != PIX_FMT_YUV420P) {
/* as we only generate a YUV420P picture, we must convert it
to the codec pixel format if needed */
if (img_convert_ctx == NULL) {
#if (LIBSWSCALE_VERSION_INT<AV_VERSION_INT(0,12,0))
img_convert_ctx = sws_getContext(c->width, c->height,
PIX_FMT_YUV420P,
c->width, c->height,
c->pix_fmt,
sws_flags, NULL, NULL, NULL);
#else
img_convert_ctx = sws_alloc_context();
if (img_convert_ctx == NULL) {
fprintf(stderr, "Cannot initialize the conversion context\n");
exit(1);
}
/* see http://permalink.gmane.org/gmane.comp.video.ffmpeg.devel/118362 */
/* see http://ffmpeg-users.933282.n4.nabble.com/Documentation-for-sws-init-context-td2956723.html */
av_set_int(img_convert_ctx, "srcw", c->width);
av_set_int(img_convert_ctx, "srch", c->height);
av_set_int(img_convert_ctx, "dstw", c->width);
av_set_int(img_convert_ctx, "dsth", c->height);
av_set_int(img_convert_ctx, "src_format", PIX_FMT_YUV420P);
av_set_int(img_convert_ctx, "dst_format", c->pix_fmt);
av_set_int(img_convert_ctx, "param0", 0);
av_set_int(img_convert_ctx, "param1", 0);
av_set_int(img_convert_ctx, "flags", sws_flags);
sws_init_context(img_convert_ctx,NULL,NULL);
#endif
}
sws_scale(img_convert_ctx, (const uint8_t* const *)tmp_picture->data,
tmp_picture->linesize,
0, c->height, picture_to_encode->data, picture_to_encode->linesize);
} else {
}
if (oc->oformat->flags & AVFMT_RAWPICTURE) {
/* raw video case. The API will change slightly in the near
futur for that */
AVPacket pkt;
av_init_packet(&pkt);
pkt.flags |= AV_PKT_FLAG_KEY;
pkt.stream_index= st->index;
pkt.data= (uint8_t *)picture_to_encode;
pkt.size= sizeof(AVPicture);
ret = av_interleaved_write_frame(oc, &pkt);
} else {
/* encode the image */
out_size = avcodec_encode_video(c, video_outbuf, video_outbuf_size, picture_to_encode);
/* if zero size, it means the image was buffered */
if (out_size > 0) {
AVPacket pkt;
av_init_packet(&pkt);
if (c->coded_frame->pts != AV_NOPTS_VALUE)
pkt.pts= av_rescale_q(c->coded_frame->pts, c->time_base, st->time_base);
if(c->coded_frame->key_frame)
pkt.flags |= AV_PKT_FLAG_KEY;
pkt.stream_index= st->index;
pkt.data= video_outbuf;
pkt.size= out_size;
/* write the compressed frame in the media file */
ret = av_interleaved_write_frame(oc, &pkt);
} else {
ret = 0;
}
}
if (ret != 0) {
fprintf(stderr, "Error while writing video frame\n");
exit(1);
}
frame_count++;
}
void close_video(AVFormatContext *oc, AVStream *st)
{
avcodec_close(st->codec);
av_free(picture_to_encode);
picture_to_encode = NULL;
if (tmp_picture) {
av_free(tmp_picture->data[0]);
av_free(tmp_picture);
tmp_picture=NULL;
}
av_free(video_outbuf);
}
/******************************************************************************************************************************************/
C_RESULT
video_stage_ffmpeg_recorder_handle (video_stage_ffmpeg_recorder_config_t * cfg, PIPELINE_MSG msg_id, void *callback, void *param)
{
printf("FFMPEG recorder message handler.\n");
switch (msg_id)
{
case PIPELINE_MSG_START:
if(cfg->startRec==VIDEO_RECORD_STOP)
cfg->startRec=VIDEO_RECORD_HOLD;
else
cfg->startRec=VIDEO_RECORD_STOP;
break;
default:
break;
}
return (VP_SUCCESS);
}
/******************************************************************************************************************************************/
C_RESULT video_stage_ffmpeg_recorder_open(video_stage_ffmpeg_recorder_config_t *cfg)
{
//return C_OK;
previous_frame.width = previous_frame.frame_number = previous_frame.height = 0;
previous_frame.buffer = NULL;
cfg->flag_video_file_open = 0;
cfg->startRec=VIDEO_RECORD_STOP;
/* initialize libavcodec, and register all codecs and formats */
av_register_all();
return C_OK;
}
/******************************************************************************************************************************************/
C_RESULT video_stage_ffmpeg_recorder_transform(video_stage_ffmpeg_recorder_config_t *cfg, vp_api_io_data_t *in, vp_api_io_data_t *out)
{
time_t temptime;
struct timeval tv;
struct tm *atm;
int i;
int frame_size;
vp_os_mutex_lock( &out->lock );
vp_api_picture_t* picture = (vp_api_picture_t *) in->buffers;
gettimeofday(&tv,NULL);
temptime = (time_t)tv.tv_sec;
atm = localtime(&temptime); //atm = localtime(&tv.tv_sec);
if( out->status == VP_API_STATUS_INIT )
{
out->numBuffers = 1;
out->indexBuffer = 0;
out->lineSize = NULL;
out->status = VP_API_STATUS_PROCESSING;
}
if( in->status == VP_API_STATUS_ENDED )
out->status = in->status;
out->size = in->size;
out->buffers = in->buffers;
if( out->status == VP_API_STATUS_PROCESSING)
{
if(cfg->startRec==VIDEO_RECORD_HOLD)
{
/* Create a new video file */
if(strlen(cfg->video_filename) == 0)
sprintf(cfg->video_filename, "%s/video_%04d%02d%02d_%02d%02d%02d_w%i_h%i.mp4",
VIDEO_FILE_DEFAULT_PATH,
atm->tm_year+1900, atm->tm_mon+1, atm->tm_mday,
atm->tm_hour, atm->tm_min, atm->tm_sec,
picture->width,
picture->height);
create_video_file(cfg->video_filename, picture->width, picture->height, picture->framerate, picture->format);
cfg->flag_video_file_open = 1;
strcpy(cfg->video_filename, "");
if(cfg->numframes != NULL)
previous_frame.frame_number = *cfg->numframes;
cfg->startRec=VIDEO_RECORD_START;
}
if( out->size > 0 && cfg->startRec == VIDEO_RECORD_START)
{
frame_size = ( previous_frame.width * previous_frame.height ) * 3 / 2;
/* Send the previous frame to FFMPEG */
if (previous_frame.buffer != NULL)
{
/* Compute the number of frames to store to achieve 60 FPS
* This should be computed using the timestamp of the first frame
* to avoid error accumulation.
*/
int nb_frames_to_write = 1;
if(cfg->numframes != NULL)
{
nb_frames_to_write = *cfg->numframes - previous_frame.frame_number;
previous_frame.frame_number = *cfg->numframes;
}
if (picture_to_encode!=NULL)
{
picture_to_encode->data[0] = picture_to_encode->base[0] = picture->y_buf;
picture_to_encode->data[1] = picture_to_encode->base[1] = picture->cb_buf;
picture_to_encode->data[2] = picture_to_encode->base[2] = picture->cr_buf;
picture_to_encode->linesize[0] = picture->width;
picture_to_encode->linesize[1] = picture->width/2;
picture_to_encode->linesize[2] = picture->width/2;
}
printf("Storing %i frames\n",nb_frames_to_write);
for (i = 0 ; i < nb_frames_to_write ; i++)
{
write_video_frame(oc, video_st);
}
}
/* Create a buffer to hold the current frame */
if (previous_frame.buffer!=NULL && (previous_frame.width!=picture->width || previous_frame.height!=picture->height))
{
vp_os_free(previous_frame.buffer);
previous_frame.buffer=NULL;
}
if (previous_frame.buffer==NULL)
{
previous_frame.width = picture->width;
previous_frame.height = picture->height;
frame_size = ( previous_frame.width * previous_frame.height )*3/2;
printf("Allocating previous frame.\n");
previous_frame.buffer=vp_os_malloc( frame_size );
}
/* Copy the current frame in a buffer so it can be encoded at next stage call */
if (previous_frame.buffer!=NULL)
{
char * dest = previous_frame.buffer;
int size = picture->width*picture->height;
vp_os_memcpy(dest,picture->y_buf,size);
dest+=size;
size /= 4;
vp_os_memcpy(dest,picture->cb_buf,size);
dest+=size;
vp_os_memcpy(dest,picture->cr_buf,size);
}
}
else
{
if(cfg->flag_video_file_open && cfg->startRec == VIDEO_RECORD_STOP)
{
close_video_file();
cfg->flag_video_file_open=0;
}
}
}
vp_os_mutex_unlock( &out->lock );
return C_OK;
}
/******************************************************************************************************************************************/
C_RESULT video_stage_ffmpeg_recorder_close(video_stage_ffmpeg_recorder_config_t *cfg)
{
if(cfg->flag_video_file_open)
{
close_video_file();
cfg->flag_video_file_open=0;
}
return C_OK;
}
#endif
ARDroneLib/Soft/Lib/ardrone_tool/Video/video_stage_ffmpeg_recorder.h
-57
Ver Arquivo
@@ -1,57 +0,0 @@
#ifdef FFMPEG_SUPPORT
#ifndef _VIDEO_STAGE_FFMPEG_RECORDER_H_
#define _VIDEO_STAGE_FFMPEG_RECORDER_H_
/* From FFMPEG example */
#include <stdlib.h>
#include <stdio.h>
#include <string.h>
#include <math.h>
#include <libavformat/avformat.h>
#include <libswscale/swscale.h>
#include <libavfilter/avfilter.h>
#include <libavutil/avutil.h>
#include <libavcodec/avcodec.h>
#include <stdio.h>
#include <VP_Api/vp_api.h>
#include <ardrone_tool/Video/video_stage_recorder.h>
#ifndef _VIDEO_RECORD_STATE_ENUM_
#define _VIDEO_RECORD_STATE_ENUM_
#endif
typedef struct _video_stage_ffmpeg_recorder_config_t
{
// Public
char video_filename[1024];
uint32_t *numframes;
int stage;
// Private
video_record_state startRec;
int flag_video_file_open;
} video_stage_ffmpeg_recorder_config_t;
C_RESULT video_stage_ffmpeg_recorder_handle (video_stage_ffmpeg_recorder_config_t * cfg, PIPELINE_MSG msg_id, void *callback, void *param);
C_RESULT video_stage_ffmpeg_recorder_open(video_stage_ffmpeg_recorder_config_t *cfg);
C_RESULT video_stage_ffmpeg_recorder_transform(video_stage_ffmpeg_recorder_config_t *cfg, vp_api_io_data_t *in, vp_api_io_data_t *out);
C_RESULT video_stage_ffmpeg_recorder_close(video_stage_ffmpeg_recorder_config_t *cfg);
void create_video_file(const char*filename,int width,int height,int frame_rate, enum PixelFormat pix_fmt);
void close_video_file();
AVStream *add_video_stream(AVFormatContext *oc, enum CodecID codec_id,int width, int height, int frame_rate, enum PixelFormat pix_fmt);
AVFrame *alloc_picture(enum PixelFormat pix_fmt, int width, int height);
void open_video(AVFormatContext *oc, AVStream *st);
void write_video_frame(AVFormatContext *oc, AVStream *st);
void close_video(AVFormatContext *oc, AVStream *st);
extern const vp_api_stage_funcs_t video_ffmpeg_recorder_funcs;
#endif // _VIDEO_STAGE_RECORDER_H_
#endif // FFMPEG_SUPPORT
ARDroneLib/Soft/Lib/ardrone_tool/Video/video_stage_ittiam_decoder.c
-834
Ver Arquivo
@@ -1,834 +0,0 @@
//
// ittiam_stage_decode.c
// ARDroneEngine
//
// Created by Mykonos on 08/07/11.
// Copyright 2011 PARROT SA. All rights reserved.
//
#ifdef ITTIAM_SUPPORT
#include <VP_Os/vp_os_malloc.h>
#include <VP_Os/vp_os_print.h>
#include <ardrone_tool/Video/video_stage_ittiam_decoder.h>
#include <Maths/time.h>
#include <math.h>
#include <sys/time.h>
#include <video_encapsulation.h>
#include <unistd.h>
#include <fcntl.h>
#define H264_DEC
#define MPEG4_DEC
static parrot_video_encapsulation_t current_PaVE, previous_PaVE;
static uint32_t old_num_frame = 0;
#ifdef H264_DEC
/* H264 CONFIGURATION */
#define H264_MAX_LEVEL_SUPPORTED 42
#define H264_MAX_FRAME_WIDTH 1920
#define H264_MAX_FRAME_HEIGHT 1088
#define H264_MAX_REF_FRAMES 2
#define H264_MAX_REORDER_FRAMES 0
#define H264_MIN_FRAME_WIDTH 64
#define H264_MIN_FRAME_HEIGHT 64
iv_obj_t * H264_DECHDL = NULL;
it_mem_t * h264_ps_it_mem;
iv_mem_rec_t * h264_mem_rec;
ivd_video_decode_ip_t h264_video_decode_ip;
ivd_video_decode_op_t h264_video_decode_op;
ivd_get_display_frame_ip_t h264_get_display_frame_ip;
ivd_get_display_frame_op_t h264_get_display_frame_op;
#endif
#ifdef MPEG4_DEC
/* MPEG4 CONFIGURATION */
iv_obj_t * MPEG4_DECHDL = NULL;
it_mem_t * mpeg4_ps_it_mem;
iv_mem_rec_t * mpeg4_mem_rec;
ivd_video_decode_ip_t mpeg4_video_decode_ip;
ivd_video_decode_op_t mpeg4_video_decode_op;
ivd_get_display_frame_ip_t mpeg4_get_display_frame_ip;
ivd_get_display_frame_op_t mpeg4_get_display_frame_op;
#endif
/**
* Enable (1) to display FPS
*/
#define DISPLAY_FPS (0)
/**
* Display FPS after N frames
* - default : 20
*/
#define DISPLAY_FPS_FRAMES (20)
/**
* Enable (1) to display missed/dropped frames
*/
#define DISPLAY_DROPPED_FRAMES (0)
/**
* Enable (1) to allow decoding of non-PaVE MPEG4/h264 frames
* Disable (0) for compatibility with VLIB/P264 (won't try to decode if no PaVE is found)
*/
#define FAKE_PaVE (0)
/**
* Default codec for non-PaVE frames :
* (0) - h264
* (1) - MPEG4
*/
#define FAKE_PaVE_CODEC_ID (0)
/**
* Wait for I-Frame mode :
* - (0) : display each received frame
* - (1) : when a frame is lost, wait for next I-Frame
*/
#define WAIT_FOR_I_FRAME (1)
/**
* Use UDP/TCP connexion :
* - (0) UDP : we get one frame per buffer
* - (1) TCP : we get each frame in multiple buffers
*/
//#define VIDEO_USES_TCP (0) // NOT FUNCTIONNAL !
/* END OF CONFIGURATION */
#if DISPLAY_FPS
#define NUM_SAMPLES DISPLAY_FPS_FRAMES
#endif
#if FAKE_PaVE_CODEC_ID == 0
#define FAKE_PaVE_CODEC CODEC_MPEG4_AVC
#elif FAKE_PaVE_CODEC_ID == 1
#define FAKE_PaVE_CODEC CODEC_MPEG4_VISUAL
#else
#warning FAKE_PaVE_CODEC_ID : unknown value -> falling back to MPEG4
#define FAKE_PaVE_CODEC CODEC_MPEG4_VISUAL
#endif
#define ITTIAM_DEBUG_ENABLED (0)
#if ITTIAM_DEBUG_ENABLED
#define ITTIAM_DEBUG_PRINT(...) \
do{ \
printf ("ITTIAM-DEBUG (%s @ %d) : ", __FUNCTION__, __LINE__); \
printf (__VA_ARGS__); \
printf ("\n"); \
} while (0)
#else
#define ITTIAM_DEBUG_PRINT(...)
#endif
#if DISPLAY_DROPPED_FRAMES
int missed_frames = 0;
int dropped_frames = 0;
int previous_ok_frame = 0;
#endif
const vp_api_stage_funcs_t ittiam_decoding_funcs = {
(vp_api_stage_handle_msg_t) NULL,
(vp_api_stage_open_t) ittiam_stage_decoding_open,
(vp_api_stage_transform_t) ittiam_stage_decoding_transform,
(vp_api_stage_close_t) ittiam_stage_decoding_close
};
void ITTIAM_dumpPave(parrot_video_encapsulation_t *PaVE) {
printf("Codec : %s\n", (PaVE->video_codec == CODEC_MPEG4_VISUAL) ? "MP4" : ((PaVE->video_codec == CODEC_MPEG4_AVC) ? "H264" : "Unknown"));
printf("Header 1 : %d\n",PaVE->header1_size);
printf("Header 2 : %d\n",PaVE->header2_size);
printf("Encoded dims : %d x %d\n", PaVE->encoded_stream_width, PaVE->encoded_stream_height);
printf("Display dims : %d x %d\n", PaVE->display_width, PaVE->display_height);
printf("Header size : %d (PaVE size : %lu)\n", PaVE->header_size, sizeof (parrot_video_encapsulation_t));
printf("Payload size : %d\n", PaVE->payload_size);
printf("Frame Type / Number : %s : %d\n", (PaVE->frame_type == FRAME_TYPE_P_FRAME) ? "P-Frame" : ((PaVE->frame_type == FRAME_TYPE_I_FRAME) ? "I-Frame" : "IDR-Frame"), PaVE->frame_number);
}
static inline bool_t check_and_copy_PaVE(parrot_video_encapsulation_t *PaVE, vp_api_io_data_t *data, parrot_video_encapsulation_t *prevPaVE, bool_t *dimChanged) {
parrot_video_encapsulation_t *localPaVE = (parrot_video_encapsulation_t *) data->buffers[data->indexBuffer];
if (localPaVE->signature[0] == 'P' &&
localPaVE->signature[1] == 'a' &&
localPaVE->signature[2] == 'V' &&
localPaVE->signature[3] == 'E') {
//ITTIAM_DEBUG_PRINT("Found a PaVE");
vp_os_memcpy(prevPaVE, PaVE, sizeof (parrot_video_encapsulation_t)); // Make a backup of previous PaVE so we can check if things have changed
vp_os_memcpy(PaVE, localPaVE, sizeof (parrot_video_encapsulation_t)); // Copy PaVE to our local one
#if ITTIAM_DEBUG_ENABLED
//printf ("PREV : ");
//ITTIAM_dumpPave (prevPaVE);
printf("CURR : ");
ITTIAM_dumpPave(PaVE);
#endif
if (prevPaVE->encoded_stream_width != PaVE->encoded_stream_width ||
prevPaVE->encoded_stream_height != PaVE->encoded_stream_height ||
prevPaVE->display_width != PaVE->display_width ||
prevPaVE->display_width != PaVE->display_width) {
*dimChanged = TRUE;
} else {
*dimChanged = FALSE;
}
data->size = localPaVE->payload_size;
memmove(data->buffers[data->indexBuffer], &(data->buffers[data->indexBuffer])[localPaVE->header_size], data->size);
#if DISPLAY_DROPPED_FRAMES
missed_frames += PaVE->frame_number - prevPaVE->frame_number - 1;
#endif
return TRUE;
} else {
ITTIAM_DEBUG_PRINT("No PaVE, signature was [%c][%c][%c][%c]", localPaVE->signature[0]
, localPaVE->signature[1]
, localPaVE->signature[2]
, localPaVE->signature[3]);
#if FAKE_PaVE
PaVE->encoded_stream_width = 1280; //640;
PaVE->encoded_stream_height = 720; //368;
PaVE->display_width = 1280; //640;
PaVE->display_height = 720; //360;
PaVE->video_codec = FAKE_PaVE_CODEC;
PaVE->frame_type = FRAME_TYPE_I_FRAME;
vp_os_memcpy(prevPaVE, PaVE, sizeof (parrot_video_encapsulation_t));
*dimChanged = FALSE;
return TRUE;
#else
return FALSE;
#endif
}
}
C_RESULT ittiam_stage_decoding_open(ittiam_stage_decoding_config_t *cfg) {
ITTIAM_DEBUG_PRINT("ITTIAM OPEN");
return C_OK;
}
C_RESULT ittiam_stage_decoding_transform(ittiam_stage_decoding_config_t *cfg, vp_api_io_data_t *in, vp_api_io_data_t *out) {
bool_t is_frame_dim_changed = FALSE;
UWORD32 i;
#if WAIT_FOR_I_FRAME
static bool_t waitForIFrame = TRUE;
#endif
#ifdef NUM_SAMPLES
static struct timeval start_time, start_time2;
static int numsamples = 0;
#endif
check_and_copy_PaVE(&current_PaVE, in, &previous_PaVE, &is_frame_dim_changed);
if (out->status == VP_API_STATUS_INIT && (current_PaVE.video_codec == CODEC_MPEG4_AVC || current_PaVE.video_codec == CODEC_MPEG4_VISUAL)) {
if (current_PaVE.video_codec == CODEC_MPEG4_AVC) {
#ifdef H264_DEC
//////////////////// H264 INIT SECTION //////////////////////////////////////////////////////////////////////////////////////
/****************************************************************************/
/* H264 ====== Initialize the memory records
*****************************************************************************/
cfg->num_picture_decoded = old_num_frame;
ITTIAM_DEBUG_PRINT("ITTIAM INIT");
iv_num_mem_rec_ip_t h264_num_mem_rec_ip;
iv_num_mem_rec_op_t h264_num_mem_rec_op;
h264_num_mem_rec_ip.e_cmd = IV_CMD_GET_NUM_MEM_REC;
h264_num_mem_rec_ip.u4_size = sizeof (iv_num_mem_rec_ip_t);
h264_num_mem_rec_op.u4_size = sizeof (iv_num_mem_rec_op_t);
if (ih264d_cxa8_api_function(H264_DECHDL, (void*) (&h264_num_mem_rec_ip), (void*) (&h264_num_mem_rec_op)) == IV_SUCCESS) {
ITTIAM_DEBUG_PRINT("IV_CMD_GET_NUM_MEM_REC [ OK ]");
} else {
ITTIAM_DEBUG_PRINT("IV_CMD_GET_NUM_MEM_REC [ NOK ] with error %d", (UWORD32) h264_num_mem_rec_op.u4_error_code);
}
ITTIAM_DEBUG_PRINT("Number of records : %d", h264_num_mem_rec_op.u4_num_mem_rec);
/****************************************************************************/
/* H264 ====== Allocate the pointers
*****************************************************************************/
h264_ps_it_mem = (it_mem_t *) vp_os_malloc(sizeof (it_mem_t));
if (h264_ps_it_mem == NULL) {
ITTIAM_DEBUG_PRINT("\nAllocation failure\n");
return 0;
}
it_mem_init(h264_ps_it_mem);
h264_mem_rec = h264_ps_it_mem->alloc(h264_ps_it_mem, (h264_num_mem_rec_op.u4_num_mem_rec) * sizeof (iv_mem_rec_t));
if (h264_mem_rec == NULL) {
ITTIAM_DEBUG_PRINT("\nAllocation failure\n");
return 0;
}
/****************************************************************************/
/* H264 ====== Fill the memory with some information
*****************************************************************************/
printf("current_PaVE.encoded_stream_width = %d, current_PaVE.encoded_stream_height = %d\n", current_PaVE.encoded_stream_width, current_PaVE.encoded_stream_height);
ih264d_cxa8_fill_mem_rec_ip_t h264_fill_mem_rec_ip;
ih264d_cxa8_fill_mem_rec_op_t h264_fill_mem_rec_op;
h264_fill_mem_rec_ip.s_ivd_fill_mem_rec_ip_t.e_cmd = IV_CMD_FILL_NUM_MEM_REC;
h264_fill_mem_rec_ip.s_ivd_fill_mem_rec_ip_t.pv_mem_rec_location = h264_mem_rec;
h264_fill_mem_rec_ip.s_ivd_fill_mem_rec_ip_t.u4_max_frm_wd = current_PaVE.encoded_stream_width; //for example 640;
h264_fill_mem_rec_ip.s_ivd_fill_mem_rec_ip_t.u4_max_frm_ht = current_PaVE.encoded_stream_height; //for example 368;
h264_fill_mem_rec_ip.s_level = H264_MAX_LEVEL_SUPPORTED;
h264_fill_mem_rec_ip.s_num_ref_frames = H264_MAX_REF_FRAMES;
h264_fill_mem_rec_ip.s_num_reorder_frames = H264_MAX_REORDER_FRAMES;
h264_fill_mem_rec_ip.s_ivd_fill_mem_rec_ip_t.u4_size = sizeof (ih264d_cxa8_fill_mem_rec_ip_t);
h264_fill_mem_rec_op.s_ivd_fill_mem_rec_op_t.u4_size = sizeof (ih264d_cxa8_fill_mem_rec_op_t);
for (i = 0; i < h264_num_mem_rec_op.u4_num_mem_rec; i++)
h264_mem_rec[i].u4_size = sizeof (iv_mem_rec_t);
if (ih264d_cxa8_api_function(H264_DECHDL, (void*) (&h264_fill_mem_rec_ip), (void*) (&h264_fill_mem_rec_op)) == IV_SUCCESS) {
ITTIAM_DEBUG_PRINT("IV_CMD_FILL_NUM_MEM_REC [ OK ]");
} else {
ITTIAM_DEBUG_PRINT("IV_CMD_FILL_NUM_MEM_REC [ NOK ]");
}
//Do some allocation on the mem_rec pointer
iv_mem_rec_t * h264_temp_mem_rec = h264_mem_rec;
for (i = 0; i < h264_num_mem_rec_op.u4_num_mem_rec; i++) {
h264_temp_mem_rec->pv_base = h264_ps_it_mem->align_alloc(h264_ps_it_mem, h264_temp_mem_rec->u4_mem_size, h264_temp_mem_rec->u4_mem_alignment);
if (h264_temp_mem_rec->pv_base == NULL) {
ITTIAM_DEBUG_PRINT("\nAllocation failure\n");
}
h264_temp_mem_rec++;
}
/****************************************************************************/
/* H264 ====== Init the DECHDL
*****************************************************************************/
ih264d_cxa8_init_ip_t h264_init_ip;
ih264d_cxa8_init_op_t h264_init_op;
void *h264_fxns = &ih264d_cxa8_api_function;
iv_mem_rec_t *h264_mem_tab = (iv_mem_rec_t*) h264_mem_rec;
h264_init_ip.s_ivd_init_ip_t.e_cmd = IV_CMD_INIT;
h264_init_ip.s_ivd_init_ip_t.pv_mem_rec_location = h264_mem_tab;
h264_init_ip.s_ivd_init_ip_t.u4_frm_max_wd = current_PaVE.encoded_stream_width; //for example 640;
h264_init_ip.s_ivd_init_ip_t.u4_frm_max_ht = current_PaVE.encoded_stream_height; //for example 368;
h264_init_ip.s_level = H264_MAX_LEVEL_SUPPORTED;
h264_init_ip.s_num_ref_frames = H264_MAX_REF_FRAMES;
h264_init_ip.s_num_reorder_frames = H264_MAX_REORDER_FRAMES;
h264_init_ip.s_ivd_init_ip_t.u4_num_mem_rec = h264_num_mem_rec_op.u4_num_mem_rec;
h264_init_ip.s_ivd_init_ip_t.e_output_format = IV_RGB_565; //IV_YUV_420P;
h264_init_ip.s_ivd_init_ip_t.u4_size = sizeof (ih264d_cxa8_init_ip_t);
h264_init_op.s_ivd_init_op_t.u4_size = sizeof (ih264d_cxa8_init_op_t);
H264_DECHDL = (iv_obj_t*) h264_mem_tab[0].pv_base;
H264_DECHDL->pv_fxns = h264_fxns;
H264_DECHDL->u4_size = sizeof (iv_obj_t);
if (ih264d_cxa8_api_function(H264_DECHDL, (void *) &h264_init_ip, (void *) &h264_init_op) == IV_SUCCESS) {
ITTIAM_DEBUG_PRINT("IV_CMD_INIT [ OK ]");
} else {
ITTIAM_DEBUG_PRINT("IV_CMD_INIT [ NOK ]");
}
/****************************************************************************/
/* H264 ====== Set decoder config
*****************************************************************************/
ivd_ctl_set_config_ip_t h264_ctl_set_config_ip;
ivd_ctl_set_config_op_t h264_ctl_set_config_op;
h264_ctl_set_config_ip.u4_disp_wd = current_PaVE.encoded_stream_width; //for example 640;
h264_ctl_set_config_ip.e_frm_skip_mode = IVD_NO_SKIP;
h264_ctl_set_config_ip.e_frm_out_mode = IVD_DISPLAY_FRAME_OUT;
h264_ctl_set_config_ip.e_vid_dec_mode = IVD_DECODE_FRAME;
h264_ctl_set_config_ip.e_cmd = IVD_CMD_VIDEO_CTL;
h264_ctl_set_config_ip.e_sub_cmd = IVD_CMD_CTL_SETPARAMS;
h264_ctl_set_config_ip.u4_size = sizeof (ivd_ctl_set_config_ip_t);
h264_ctl_set_config_op.u4_size = sizeof (ivd_ctl_set_config_op_t);
if (ih264d_cxa8_api_function(H264_DECHDL, (void *) &h264_ctl_set_config_ip, (void *) &h264_ctl_set_config_op) == IV_SUCCESS) {
ITTIAM_DEBUG_PRINT("IVD_CMD_VIDEO_CTL => IVD_CMD_CTL_SETPARAMS [ OK ]");
} else {
ITTIAM_DEBUG_PRINT("IVD_CMD_VIDEO_CTL => IVD_CMD_CTL_SETPARAMS [ NOK ]");
}
/****************************************************************************/
/* H264 ====== Decode the in buffer INIT PART
*****************************************************************************/
h264_video_decode_ip.e_cmd = IVD_CMD_VIDEO_DECODE;
h264_video_decode_ip.u4_size = sizeof (ivd_video_decode_ip_t);
h264_video_decode_op.u4_size = sizeof (ivd_video_decode_op_t);
/****************************************************************************/
/* H264 ====== Display the buffer INIT PART
*****************************************************************************/
h264_get_display_frame_ip.e_cmd = IVD_CMD_GET_DISPLAY_FRAME;
h264_get_display_frame_ip.u4_size = sizeof (ivd_get_display_frame_ip_t);
h264_get_display_frame_op.u4_size = sizeof (ivd_get_display_frame_op_t);
/****************************************************************************/
/* H264 ====== Get the buffers information to re-use them
*****************************************************************************/
ivd_ctl_getbufinfo_ip_t h264_ctl_dec_ip;
ivd_ctl_getbufinfo_op_t h264_ctl_dec_op;
h264_ctl_dec_ip.e_cmd = IVD_CMD_VIDEO_CTL;
h264_ctl_dec_ip.e_sub_cmd = IVD_CMD_CTL_GETBUFINFO;
h264_ctl_dec_ip.u4_size = sizeof (ivd_ctl_getbufinfo_ip_t);
h264_ctl_dec_op.u4_size = sizeof (ivd_ctl_getbufinfo_op_t);
if (ih264d_cxa8_api_function(H264_DECHDL, (void *) &h264_ctl_dec_ip, (void *) &h264_ctl_dec_op) == IV_SUCCESS) {
ITTIAM_DEBUG_PRINT("IVD_CMD_VIDEO_CTL => IVD_CMD_CTL_GETBUFINFO [ OK ]");
} else {
ITTIAM_DEBUG_PRINT("IVD_CMD_VIDEO_CTL => IVD_CMD_CTL_GETBUFINFO [ NOK ]");
}
//Allocate the output buffer used to store decoded frame (RGB 565)
h264_get_display_frame_ip.s_out_buffer.u4_min_out_buf_size[0] = h264_ctl_dec_op.u4_min_out_buf_size[0];
h264_get_display_frame_ip.s_out_buffer.pu1_bufs[0] = h264_ps_it_mem->alloc(h264_ps_it_mem, h264_ctl_dec_op.u4_min_out_buf_size[0]);
h264_get_display_frame_ip.s_out_buffer.u4_num_bufs = h264_ctl_dec_op.u4_min_num_out_bufs;
ITTIAM_DEBUG_PRINT("min buf size = %d", h264_get_display_frame_ip.s_out_buffer.u4_min_out_buf_size[0]);
ITTIAM_DEBUG_PRINT("num out buf = %d", h264_get_display_frame_ip.s_out_buffer.u4_num_bufs);
ITTIAM_DEBUG_PRINT("@buffer = %p", h264_get_display_frame_ip.s_out_buffer.pu1_bufs[0]);
puts("******************************** ITTIAM H264 decoding init *********************************");
#endif
///////////////////////////// END H264 INIT SECTION /////////////////////////////////////////////////////////////////////////
//================================================================================================================================
} else if (current_PaVE.video_codec == CODEC_MPEG4_VISUAL) {
//////////////////// MPEG4 INIT SECTION //////////////////////////////////////////////////////////////////////////////////////
#ifdef MPEG4_DEC
/****************************************************************************/
/* MPEG4 ====== Initialize the memory records
*****************************************************************************/
cfg->num_picture_decoded = old_num_frame;
ITTIAM_DEBUG_PRINT("ITTIAM INIT");
iv_num_mem_rec_ip_t mpeg4_num_mem_rec_ip;
iv_num_mem_rec_op_t mpeg4_num_mem_rec_op;
mpeg4_num_mem_rec_ip.e_cmd = IV_CMD_GET_NUM_MEM_REC;
mpeg4_num_mem_rec_ip.u4_size = sizeof (iv_num_mem_rec_ip_t);
mpeg4_num_mem_rec_op.u4_size = sizeof (iv_num_mem_rec_op_t);
if (imp4d_cxa8_api_function(MPEG4_DECHDL, (void*) (&mpeg4_num_mem_rec_ip), (void*) (&mpeg4_num_mem_rec_op)) == IV_SUCCESS) {
ITTIAM_DEBUG_PRINT("IV_CMD_GET_NUM_MEM_REC [ OK ]");
} else {
ITTIAM_DEBUG_PRINT("IV_CMD_GET_NUM_MEM_REC [ NOK ] with error %d", (UWORD32) mpeg4_num_mem_rec_op.u4_error_code);
}
ITTIAM_DEBUG_PRINT("Number of records : %d", mpeg4_num_mem_rec_op.u4_num_mem_rec);
/****************************************************************************/
/* MPEG4 ====== Allocate the pointers
*****************************************************************************/
mpeg4_ps_it_mem = (it_mem_t *) vp_os_malloc(sizeof (it_mem_t));
if (mpeg4_ps_it_mem == NULL) {
ITTIAM_DEBUG_PRINT("\nAllocation failure\n");
return 0;
}
it_mem_init(mpeg4_ps_it_mem);
mpeg4_mem_rec = mpeg4_ps_it_mem->alloc(mpeg4_ps_it_mem, (mpeg4_num_mem_rec_op.u4_num_mem_rec) * sizeof (iv_mem_rec_t));
if (mpeg4_mem_rec == NULL) {
ITTIAM_DEBUG_PRINT("\nAllocation failure\n");
return 0;
}
/****************************************************************************/
/* MPEG4 ====== Fill the memory with some information
*****************************************************************************/
printf("current_PaVE.encoded_stream_width = %d, current_PaVE.encoded_stream_height = %d\n", current_PaVE.encoded_stream_width, current_PaVE.encoded_stream_height);
imp4d_cxa8_fill_mem_rec_ip_t mpeg4_fill_mem_rec_ip;
imp4d_cxa8_fill_mem_rec_op_t mpeg4_fill_mem_rec_op;
mpeg4_fill_mem_rec_ip.s_ivd_fill_mem_rec_ip_t.e_cmd = IV_CMD_FILL_NUM_MEM_REC;
mpeg4_fill_mem_rec_ip.s_ivd_fill_mem_rec_ip_t.pv_mem_rec_location = mpeg4_mem_rec;
mpeg4_fill_mem_rec_ip.s_ivd_fill_mem_rec_ip_t.u4_max_frm_wd = current_PaVE.encoded_stream_width; //for example 640;
mpeg4_fill_mem_rec_ip.s_ivd_fill_mem_rec_ip_t.u4_max_frm_ht = current_PaVE.encoded_stream_height; //for example 360;
mpeg4_fill_mem_rec_ip.s_ivd_fill_mem_rec_ip_t.u4_size = sizeof (imp4d_cxa8_fill_mem_rec_ip_t);
mpeg4_fill_mem_rec_op.s_ivd_fill_mem_rec_op_t.u4_size = sizeof (imp4d_cxa8_fill_mem_rec_op_t);
for (i = 0; i < mpeg4_num_mem_rec_op.u4_num_mem_rec; i++)
mpeg4_mem_rec[i].u4_size = sizeof (iv_mem_rec_t);
if (imp4d_cxa8_api_function(MPEG4_DECHDL, (void*) (&mpeg4_fill_mem_rec_ip), (void*) (&mpeg4_fill_mem_rec_op)) == IV_SUCCESS) {
ITTIAM_DEBUG_PRINT("IV_CMD_FILL_NUM_MEM_REC [ OK ]");
} else {
ITTIAM_DEBUG_PRINT("IV_CMD_FILL_NUM_MEM_REC [ NOK ]");
}
//Do some allocation on the mem_rec pointer
iv_mem_rec_t * mpeg4_temp_mem_rec = mpeg4_mem_rec;
for (i = 0; i < mpeg4_num_mem_rec_op.u4_num_mem_rec; i++) {
mpeg4_temp_mem_rec->pv_base = mpeg4_ps_it_mem->align_alloc(mpeg4_ps_it_mem, mpeg4_temp_mem_rec->u4_mem_size, mpeg4_temp_mem_rec->u4_mem_alignment);
if (mpeg4_temp_mem_rec->pv_base == NULL) {
ITTIAM_DEBUG_PRINT("\nAllocation failure\n");
}
mpeg4_temp_mem_rec++;
}
/****************************************************************************/
/* MPEG4 ====== Init the DECHDL
*****************************************************************************/
imp4d_cxa8_init_ip_t mpeg4_init_ip;
imp4d_cxa8_init_op_t mpeg4_init_op;
void *mpeg4_fxns = &imp4d_cxa8_api_function;
iv_mem_rec_t *mpeg4_mem_tab = (iv_mem_rec_t*) mpeg4_mem_rec;
mpeg4_init_ip.s_ivd_init_ip_t.e_cmd = IV_CMD_INIT;
mpeg4_init_ip.s_ivd_init_ip_t.pv_mem_rec_location = mpeg4_mem_tab;
mpeg4_init_ip.s_ivd_init_ip_t.u4_frm_max_wd = current_PaVE.encoded_stream_width; //for example 640;
mpeg4_init_ip.s_ivd_init_ip_t.u4_frm_max_ht = current_PaVE.encoded_stream_height; //for example 360;
mpeg4_init_ip.s_ivd_init_ip_t.u4_num_mem_rec = mpeg4_num_mem_rec_op.u4_num_mem_rec;
mpeg4_init_ip.s_ivd_init_ip_t.e_output_format = IV_RGB_565; //IV_YUV_420P;
mpeg4_init_ip.s_ivd_init_ip_t.u4_size = sizeof (imp4d_cxa8_init_ip_t);
mpeg4_init_op.s_ivd_init_op_t.u4_size = sizeof (imp4d_cxa8_init_op_t);
MPEG4_DECHDL = (iv_obj_t*) mpeg4_mem_tab[0].pv_base;
MPEG4_DECHDL->pv_fxns = mpeg4_fxns;
MPEG4_DECHDL->u4_size = sizeof (iv_obj_t);
if (imp4d_cxa8_api_function(MPEG4_DECHDL, (void *) &mpeg4_init_ip, (void *) &mpeg4_init_op) == IV_SUCCESS) {
ITTIAM_DEBUG_PRINT("IV_CMD_INIT [ OK ]");
} else {
ITTIAM_DEBUG_PRINT("IV_CMD_INIT [ NOK ]");
}
/****************************************************************************/
/* MPEG4 ====== Set decoder config
*****************************************************************************/
ivd_ctl_set_config_ip_t mpeg4_ctl_set_config_ip;
ivd_ctl_set_config_op_t mpeg4_ctl_set_config_op;
mpeg4_ctl_set_config_ip.u4_disp_wd = current_PaVE.encoded_stream_width; //for example 640;
mpeg4_ctl_set_config_ip.e_frm_skip_mode = IVD_NO_SKIP;
mpeg4_ctl_set_config_ip.e_frm_out_mode = IVD_DISPLAY_FRAME_OUT;
mpeg4_ctl_set_config_ip.e_vid_dec_mode = IVD_DECODE_FRAME;
mpeg4_ctl_set_config_ip.e_cmd = IVD_CMD_VIDEO_CTL;
mpeg4_ctl_set_config_ip.e_sub_cmd = IVD_CMD_CTL_SETPARAMS;
mpeg4_ctl_set_config_ip.u4_size = sizeof (ivd_ctl_set_config_ip_t);
mpeg4_ctl_set_config_op.u4_size = sizeof (ivd_ctl_set_config_op_t);
if (imp4d_cxa8_api_function(MPEG4_DECHDL, (void *) &mpeg4_ctl_set_config_ip, (void *) &mpeg4_ctl_set_config_op) == IV_SUCCESS) {
ITTIAM_DEBUG_PRINT("IVD_CMD_VIDEO_CTL => IVD_CMD_CTL_SETPARAMS [ OK ]");
} else {
ITTIAM_DEBUG_PRINT("IVD_CMD_VIDEO_CTL => IVD_CMD_CTL_SETPARAMS [ NOK ]");
}
/****************************************************************************/
/* MPEG4 ====== Decode the in buffer INIT PART
*****************************************************************************/
mpeg4_video_decode_ip.e_cmd = IVD_CMD_VIDEO_DECODE;
mpeg4_video_decode_ip.u4_size = sizeof (ivd_video_decode_ip_t);
mpeg4_video_decode_op.u4_size = sizeof (ivd_video_decode_op_t);
/****************************************************************************/
/* Display the buffer INIT PART
*****************************************************************************/
mpeg4_get_display_frame_ip.e_cmd = IVD_CMD_GET_DISPLAY_FRAME;
mpeg4_get_display_frame_ip.u4_size = sizeof (ivd_get_display_frame_ip_t);
mpeg4_get_display_frame_op.u4_size = sizeof (ivd_get_display_frame_op_t);
/****************************************************************************/
/* MPEG4 ====== Get the buffers information to re-use them
*****************************************************************************/
ivd_ctl_getbufinfo_ip_t mpeg4_ctl_dec_ip;
ivd_ctl_getbufinfo_op_t mpeg4_ctl_dec_op;
mpeg4_ctl_dec_ip.e_cmd = IVD_CMD_VIDEO_CTL;
mpeg4_ctl_dec_ip.e_sub_cmd = IVD_CMD_CTL_GETBUFINFO;
mpeg4_ctl_dec_ip.u4_size = sizeof (ivd_ctl_getbufinfo_ip_t);
mpeg4_ctl_dec_op.u4_size = sizeof (ivd_ctl_getbufinfo_op_t);
if (imp4d_cxa8_api_function(MPEG4_DECHDL, (void *) &mpeg4_ctl_dec_ip, (void *) &mpeg4_ctl_dec_op) == IV_SUCCESS) {
ITTIAM_DEBUG_PRINT("IVD_CMD_VIDEO_CTL => IVD_CMD_CTL_GETBUFINFO [ OK ]");
} else {
ITTIAM_DEBUG_PRINT("IVD_CMD_VIDEO_CTL => IVD_CMD_CTL_GETBUFINFO [ NOK ]");
}
//Allocate the output buffer used to store decoded frame (RGB 565)
mpeg4_get_display_frame_ip.s_out_buffer.u4_min_out_buf_size[0] = mpeg4_ctl_dec_op.u4_min_out_buf_size[0];
mpeg4_get_display_frame_ip.s_out_buffer.pu1_bufs[0] = mpeg4_ps_it_mem->alloc(mpeg4_ps_it_mem, mpeg4_ctl_dec_op.u4_min_out_buf_size[0]);
mpeg4_get_display_frame_ip.s_out_buffer.u4_num_bufs = mpeg4_ctl_dec_op.u4_min_num_out_bufs;
ITTIAM_DEBUG_PRINT("min buf size = %d", mpeg4_get_display_frame_ip.s_out_buffer.u4_min_out_buf_size[0]);
ITTIAM_DEBUG_PRINT("num out buf = %d", mpeg4_get_display_frame_ip.s_out_buffer.u4_num_bufs);
ITTIAM_DEBUG_PRINT("@buffer = %p", mpeg4_get_display_frame_ip.s_out_buffer.pu1_bufs[0]);
puts("******************************** ITTIAM MPEG4 decoding init ********************************");
#endif
}
///////////////////////////// END MPEG4 INIT SECTION /////////////////////////////////////////////////////////////////////////
vp_os_mutex_lock(&out->lock);
out->numBuffers = 1;
if(out->buffers == NULL)
out->buffers = (uint8_t **) vp_os_malloc(sizeof (uint8_t*));
out->buffers[0] = NULL;
out->indexBuffer = 0;
out->lineSize = 0;
cfg->src_picture.width = current_PaVE.display_width; //for example 640;
cfg->src_picture.height = current_PaVE.display_height; //for example 360;
cfg->dst_picture.format = PIX_FMT_RGB565;
cfg->dst_picture.width = current_PaVE.display_width; //for example 640;
cfg->dst_picture.height = current_PaVE.display_height; //for example 360;
//Adress of output ittiam pointer
if (current_PaVE.video_codec == CODEC_MPEG4_AVC) {
out->buffers[0] = (uint8_t*) h264_get_display_frame_ip.s_out_buffer.pu1_bufs[0];
out->size = h264_get_display_frame_ip.s_out_buffer.u4_min_out_buf_size[0];
} else if (current_PaVE.video_codec == CODEC_MPEG4_VISUAL) {
out->buffers[0] = (uint8_t*) mpeg4_get_display_frame_ip.s_out_buffer.pu1_bufs[0];
out->size = mpeg4_get_display_frame_ip.s_out_buffer.u4_min_out_buf_size[0];
}
out->status = VP_API_STATUS_PROCESSING;
vp_os_mutex_unlock(&out->lock);
#ifdef NUM_SAMPLES
gettimeofday(&start_time, NULL);
#endif
}
///////////////////////////////////// PROCESS SECTION //////////////////////////////////////////////////////////////////
#if WAIT_FOR_I_FRAME
if (current_PaVE.frame_number != (previous_PaVE.frame_number + 1)) {
waitForIFrame = TRUE;
}
#endif
if (waitForIFrame == FALSE || current_PaVE.frame_type == FRAME_TYPE_IDR_FRAME || current_PaVE.frame_type == FRAME_TYPE_I_FRAME) {
#ifdef NUM_SAMPLES
struct timeval end_time;
static float32_t frame_decoded_time = 0;
gettimeofday(&start_time2, NULL);
#endif
waitForIFrame = FALSE;
if (current_PaVE.video_codec == CODEC_MPEG4_AVC) {
#ifdef H264_DEC
/****************************************************************************/
/* Decode the in buffer EXEC PART
*****************************************************************************/
h264_video_decode_ip.u4_ts = cfg->num_picture_decoded;
h264_video_decode_ip.pv_stream_buffer = ((unsigned char*) in->buffers[in->indexBuffer]);
h264_video_decode_ip.u4_num_Bytes = in->size;
ITTIAM_DEBUG_PRINT("In size = %d", in->size);
if (ih264d_cxa8_api_function(H264_DECHDL, (void *) &h264_video_decode_ip, (void *) &h264_video_decode_op) == IV_SUCCESS) {
ITTIAM_DEBUG_PRINT("IVD_CMD_VIDEO_DECODE [ OK ]");
} else {
ITTIAM_DEBUG_PRINT("IVD_CMD_VIDEO_DECODE [ NOK ]");
}
/****************************************************************************/
/* Display the buffer EXEC PART
*****************************************************************************/
if (ih264d_cxa8_api_function(H264_DECHDL, (void *) &h264_get_display_frame_ip, (void *) &h264_get_display_frame_op) == IV_SUCCESS) {
ITTIAM_DEBUG_PRINT("IVD_CMD_GET_DISPLAY_FRAME [ OK ]");
if (h264_video_decode_op.u4_frame_decoded_flag == 1) {
cfg->num_picture_decoded++;
//Display the FPS
#ifdef NUM_SAMPLES
gettimeofday(&end_time, NULL);
frame_decoded_time += ((end_time.tv_sec * 1000.0 + end_time.tv_usec / 1000.0)
- (start_time2.tv_sec * 1000.0 + start_time2.tv_usec / 1000.0));
if (numsamples++ > NUM_SAMPLES) {
float32_t value = ((end_time.tv_sec * 1000.0 + end_time.tv_usec / 1000.0)
- (start_time.tv_sec * 1000.0 + start_time.tv_usec / 1000.0));
printf("Frames decoded in average %f fps, received and decoded in average %f fps\n",
(1000.0 / (frame_decoded_time / (float32_t) NUM_SAMPLES)),
1000.0 / (value / (float32_t) NUM_SAMPLES)
);
//printf("%f\n", (1000.0 / (frame_decoded_time / (float32_t)NUM_SAMPLES)));
gettimeofday(&start_time, NULL);
frame_decoded_time = 0;
numsamples = 0;
}
#endif
}
} else {
ITTIAM_DEBUG_PRINT("IVD_CMD_GET_DISPLAY_FRAME [ NOK ]");
}
#endif
} else if (current_PaVE.video_codec == CODEC_MPEG4_VISUAL) {
#ifdef MPEG4_DEC
/****************************************************************************/
/* Decode the in buffer EXEC PART
*****************************************************************************/
mpeg4_video_decode_ip.u4_ts = cfg->num_picture_decoded;
if(current_PaVE.frame_type == FRAME_TYPE_P_FRAME){
mpeg4_video_decode_ip.pv_stream_buffer = ((unsigned char*) in->buffers[in->indexBuffer]);
mpeg4_video_decode_ip.u4_num_Bytes = in->size;
} else {
mpeg4_video_decode_ip.pv_stream_buffer = ((unsigned char*) in->buffers[in->indexBuffer]);
mpeg4_video_decode_ip.u4_num_Bytes = in->size;
}
ITTIAM_DEBUG_PRINT("In size = %d", in->size);
if (imp4d_cxa8_api_function(MPEG4_DECHDL, (void *) &mpeg4_video_decode_ip, (void *) &mpeg4_video_decode_op) == IV_SUCCESS) {
ITTIAM_DEBUG_PRINT("IVD_CMD_VIDEO_DECODE [ OK ]");
} else {
ITTIAM_DEBUG_PRINT("IVD_CMD_VIDEO_DECODE [ NOK ] with error 0x%04X ==> %d", mpeg4_video_decode_op.u4_error_code, mpeg4_video_decode_op.u4_error_code);
}
/****************************************************************************/
/* Display the buffer EXEC PART
*****************************************************************************/
if (imp4d_cxa8_api_function(MPEG4_DECHDL, (void *) &mpeg4_get_display_frame_ip, (void *) &mpeg4_get_display_frame_op) == IV_SUCCESS) {
ITTIAM_DEBUG_PRINT("IVD_CMD_GET_DISPLAY_FRAME [ OK ]");
if (mpeg4_video_decode_op.u4_frame_decoded_flag == 1) {
cfg->num_picture_decoded++;
//Display the FPS
#ifdef NUM_SAMPLES
gettimeofday(&end_time, NULL);
frame_decoded_time += ((end_time.tv_sec * 1000.0 + end_time.tv_usec / 1000.0)
- (start_time2.tv_sec * 1000.0 + start_time2.tv_usec / 1000.0));
if (numsamples++ > NUM_SAMPLES) {
float32_t value = ((end_time.tv_sec * 1000.0 + end_time.tv_usec / 1000.0)
- (start_time.tv_sec * 1000.0 + start_time.tv_usec / 1000.0));
printf("Frames decoded in average %f fps, received and decoded in average %f fps\n",
(1000.0 / (frame_decoded_time / (float32_t) NUM_SAMPLES)),
1000.0 / (value / (float32_t) NUM_SAMPLES)
);
//printf("%f\n", (1000.0 / (frame_decoded_time / (float32_t)NUM_SAMPLES)));
gettimeofday(&start_time, NULL);
frame_decoded_time = 0;
numsamples = 0;
}
#endif
}
} else {
ITTIAM_DEBUG_PRINT("IVD_CMD_GET_DISPLAY_FRAME [ NOK ]");
}
#endif
}
} else {
return C_OK;
}
///////////////////////////////////// END PROCESS SECTION //////////////////////////////////////////////////////////////
ITTIAM_DEBUG_PRINT("ITTIAM DECODE");
return C_OK;
}
C_RESULT ittiam_stage_decoding_close(ittiam_stage_decoding_config_t *cfg) {
if (current_PaVE.video_codec == CODEC_MPEG4_AVC) {
//H264
/****************************************************************************/
/* H264 ====== Reset the memory records
*****************************************************************************/
ITTIAM_DEBUG_PRINT("ITTIAM RESET");
ivd_ctl_reset_ip_t h264_ctl_reset_ip;
ivd_ctl_reset_op_t h264_ctl_reset_op;
h264_ctl_reset_ip.e_cmd = IVD_CMD_VIDEO_CTL;
h264_ctl_reset_ip.e_sub_cmd = IVD_CMD_CTL_RESET;
h264_ctl_reset_ip.u4_size = sizeof (ivd_ctl_reset_ip_t);
h264_ctl_reset_op.u4_size = sizeof (ivd_ctl_reset_op_t);
if (ih264d_cxa8_api_function(H264_DECHDL, (void*) (&h264_ctl_reset_ip), (void*) (&h264_ctl_reset_op)) == IV_SUCCESS) {
ITTIAM_DEBUG_PRINT("IVD_CMD_CTL_RESET [ OK ]");
} else {
ITTIAM_DEBUG_PRINT("IVD_CMD_CTL_RESET [ NOK ] with error %d", (UWORD32) h264_ctl_reset_op.u4_error_code);
}
vp_os_free(h264_mem_rec);
h264_mem_rec = NULL;
vp_os_free(h264_ps_it_mem);
h264_ps_it_mem = NULL;
} else if (current_PaVE.video_codec == CODEC_MPEG4_VISUAL) {
//MPEG4
/****************************************************************************/
/* H264 ====== Reset the memory records
*****************************************************************************/
ITTIAM_DEBUG_PRINT("ITTIAM RESET");
ivd_ctl_reset_ip_t mpeg4_ctl_reset_ip;
ivd_ctl_reset_op_t mpeg4_ctl_reset_op;
mpeg4_ctl_reset_ip.e_cmd = IVD_CMD_VIDEO_CTL;
mpeg4_ctl_reset_ip.e_sub_cmd = IVD_CMD_CTL_RESET;
mpeg4_ctl_reset_ip.u4_size = sizeof (ivd_ctl_reset_ip_t);
mpeg4_ctl_reset_op.u4_size = sizeof (ivd_ctl_reset_op_t);
if (imp4d_cxa8_api_function(MPEG4_DECHDL, (void*) (&mpeg4_ctl_reset_ip), (void*) (&mpeg4_ctl_reset_op)) == IV_SUCCESS) {
ITTIAM_DEBUG_PRINT("IVD_CMD_CTL_RESET [ OK ]");
} else {
ITTIAM_DEBUG_PRINT("IVD_CMD_CTL_RESET [ NOK ] with error %d", (UWORD32) mpeg4_ctl_reset_op.u4_error_code);
}
vp_os_free(mpeg4_mem_rec);
mpeg4_mem_rec = NULL;
vp_os_free(mpeg4_ps_it_mem);
mpeg4_ps_it_mem = NULL;
}
old_num_frame = cfg->num_picture_decoded;
ITTIAM_DEBUG_PRINT("ITTIAM CLEAN");
return C_OK;
}
#endif
ARDroneLib/Soft/Lib/ardrone_tool/Video/video_stage_ittiam_decoder.h
-52
Ver Arquivo
@@ -1,52 +0,0 @@
//
// ittiam_stage_decode.h
// ARDroneEngine
//
// Created by Mykonos on 08/07/11.
// Copyright 2011 PARROT SA. All rights reserved.
//
#ifdef ITTIAM_SUPPORT
#ifndef ITTIAM_STAGE_DECODE_H_
#define ITTIAM_STAGE_DECODE_H_
#include <VP_Api/vp_api.h>
//ITTIAM Common inlcudes
#include <datatypedef.h>
#include <it_mem.h>
#include <it_memory.h>
// H264 include
#include <ih264d_cxa8.h>
//MPEG4 include
#include <imp4d_cxa8.h>
#include <VP_Api/vp_api_picture.h>
typedef struct _ittiam_picture_ {
enum PixelFormat format;
uint32_t width;
uint32_t height;
} ittiam_picture;
typedef struct _ittiam_stage_decoding_config_t {
ittiam_picture dst_picture;
ittiam_picture src_picture;
uint32_t num_picture_decoded;
} ittiam_stage_decoding_config_t;
C_RESULT ittiam_stage_decoding_open(ittiam_stage_decoding_config_t *cfg);
C_RESULT ittiam_stage_decoding_transform(ittiam_stage_decoding_config_t *cfg, vp_api_io_data_t *in, vp_api_io_data_t *out);
C_RESULT ittiam_stage_decoding_close(ittiam_stage_decoding_config_t *cfg);
extern const vp_api_stage_funcs_t ittiam_decoding_funcs;
#endif // _ITTIAM_STAGE_DECODE_H_
#endif
ARDroneLib/Soft/Lib/ardrone_tool/Video/video_stage_latency_estimation.c
-301
Ver Arquivo
@@ -1,301 +0,0 @@
#include <ardrone_tool/Video/video_stage_latency_estimation.h>
#include <ardrone_tool/Video/video_stage_decoder.h>
#include <VP_Os/vp_os_malloc.h>
#include <sys/time.h>
#include <inttypes.h>
#include <stdio.h>
/**
* IPHONE DEBUG AREA
*/
float DEBUG_latency = 0.0;
C_RESULT latency_estimation_stage_handle_message(void *cfg, PIPELINE_MSG msg_id, void *callback, void *param)
{
printf("Latency estimator message handler.\n");
return (C_OK);
}
C_RESULT
latency_estimation_stage_open( vp_stages_latency_estimation_config_t *cfg )
{
cfg->state = 0;
cfg->w = cfg->h = 0;
return C_OK;
}
#define N (10)
typedef struct __stat { float min; float avg; } stat_t;
static stat_t average(float f)
{
static float p[N] = {0};
static int idx =0;
int i;
float avg;
float fmin;
stat_t stat;
p[idx] = f;
idx = (idx+1)%N;
avg = 0.0f;
fmin = p[0];
for (i=0;i<N;i++) { avg+=p[i]; fmin = (p[i]<fmin) ? p[i]:fmin; }
avg = avg / (float)N;
stat.min=fmin;
stat.avg=avg;
return stat;
}
C_RESULT
latency_estimation_stage_transform( vp_stages_latency_estimation_config_t *cfg , vp_api_io_data_t *in, vp_api_io_data_t *out)
{
video_decoder_config_t * vec;
static int previous_state = 0;
static long int timestamp1 = 0 , timestamp2 = 0;
struct timeval time;
long int delta;
stat_t stat;
static char * red_buffer = NULL;
static char * green_buffer = NULL;
static char * blue_buffer = NULL;
int i,j;
int bufsize;
int m;
float n;
float r=0.0f,g=0.0f,b=0.0f;
vec = (video_decoder_config_t *)cfg->last_decoded_frame_info;
if(out->status == VP_API_STATUS_INIT) // Init only code
{
out->status = VP_API_STATUS_PROCESSING;
}
out->numBuffers = in->numBuffers;
out->buffers = in->buffers;
out->indexBuffer = in->indexBuffer;
out->size = in->size;
out->lineSize = in->lineSize;
if(out->status == VP_API_STATUS_PROCESSING)
{
/* Build three RGB buffers whose dimensions are the same than the decoded video stream */
if ( cfg->w != vec->dst_picture->width || cfg->h != vec->dst_picture->height)
{
cfg->w = vec->dst_picture->width;
cfg->h = vec->dst_picture->height;
if (red_buffer) { vp_os_sfree((void**)&red_buffer); }
if (green_buffer){ vp_os_sfree((void**)&green_buffer); }
if (blue_buffer) { vp_os_sfree((void**)&blue_buffer); }
switch (vec->dst_picture->format)
{
case PIX_FMT_RGB24:
bufsize = cfg->w * cfg->h * 3;
break;
case PIX_FMT_RGB565:
default:
bufsize = cfg->w * cfg->h * 2;
break;
}
red_buffer = vp_os_malloc( bufsize );
green_buffer = vp_os_malloc( bufsize );
blue_buffer = vp_os_malloc( bufsize );
if (red_buffer) { vp_os_memset( red_buffer , 0 , bufsize ); }
if (green_buffer) { vp_os_memset( green_buffer , 0 , bufsize ); }
if (blue_buffer) { vp_os_memset( blue_buffer , 0 , bufsize ); }
switch (vec->dst_picture->format)
{
case PIX_FMT_RGB24:
{
for (i=0;i<bufsize;i+= 3)
{
red_buffer[i] = 255;
green_buffer[i+1] = 255;
blue_buffer[i+2] = 255;
}
break;
}
case PIX_FMT_RGB565:
default:
{
for (i=0; i < bufsize; i+=2)
{
red_buffer[i+1] = 0xf8;
green_buffer[i+1] = 0x07;
green_buffer[i] = 0xe0;
blue_buffer[i] = 0x1f;
}
break;
}
}
}
if (cfg->state == LE_WAITING)
{
out->numBuffers = in->numBuffers;
out->indexBuffer = in->indexBuffer;
}
else
{
out->numBuffers = 1;
out->indexBuffer = 0;
}
/* Switch state depending on what is seen in the decoded video stream */
#define L (3)
/* Coordinates of the center pixel */
r=0.0f;
g=0.0f;
b=0.0f;
uint8_t redVal, greenVal, blueVal;
for (i=-L;i<=L;i++)
{
for (j=-L;j<=L;j++)
{
m = ( cfg->w / 2 +i) + ( cfg->w * (cfg->h/2 +j) );
switch (vec->dst_picture->format)
{
case PIX_FMT_RGB24:
redVal = in->buffers[in->indexBuffer][3*m];
greenVal = in->buffers[in->indexBuffer][3*m+1];
blueVal = in->buffers[in->indexBuffer][3*m+2];
r += (float)(redVal);
g += (float)(greenVal);
b += (float)(blueVal);
break;
case PIX_FMT_RGB565:
redVal = (in->buffers[in->indexBuffer][2*m+1] >> 3);
greenVal = ((in->buffers[in->indexBuffer][2*m+1] & 0x07) << 3) + (in->buffers[in->indexBuffer][2*m] >> 5);
blueVal = (in->buffers[in->indexBuffer][2*m] & 0x1f);
r += (float)(redVal);
g += (float)(greenVal >> 1);
b += (float)(blueVal);
break;
default:
r = g = b = 0.0;
break;
}
}
}
n=(float)((2*L+1)*(2*L+1));
r/=n;
g/=n;
b/=n;
#if 0
printf ("R %3.1f | G %3.1f | B %3.1f --- State : %d\n", r, g, b, cfg->state);
#endif
#define K (1.0f)
switch (cfg->state)
{
default:
case LE_DISABLED:
/* Nothing to do */
break;
case LE_WAITING: /* waiting to see GREEN */
if ( (g>(K*r)) && (g>(K*b)) ) { cfg->state = LE_START; }
break;
case LE_START: /* waiting to see red */
if ( (g>(K*r)) && (g>(K*b)) ) { cfg->state = LE_START; }
else if ( (r>(K*b)) && (r>(K*g)) ) { cfg->state = LE_COLOR2; }
else { cfg->state = LE_WAITING; printf("================> %f %f %f \n",r,g,b);}
break;
case LE_COLOR1: /* GREEN requested */
if ( (r>(K*b)) && (r>(K*g)) ) { cfg->state = LE_COLOR2; }
else if ( (b>(K*g)) && (b>(K*r)) ) { cfg->state = LE_COLOR1; }
else { cfg->state = LE_WAITING; printf("================> %f %f %f \n",r,g,b);}
break;
case LE_COLOR2: /* BLUE requested */
/* Display a blue picture */
if ( (b>(K*g)) && (b>(K*r)) ) { cfg->state = LE_COLOR1; }
else if ( (r>(K*b)) && (r>(K*g)) ) { cfg->state = LE_COLOR2; }
else { cfg->state = LE_WAITING; printf("================> %f %f %f \n",r,g,b);}
break;
}
switch (cfg->state)
{
default:
case LE_DISABLED:
/* Forward the video stream */
out->buffers = in->buffers;
break;
case LE_WAITING:
/* Display a gren picture announcing we are waiting for the user to
* put the drone camera in front of the displayed picture.
*/
out->buffers = (uint8_t**)&(green_buffer);
break;
case LE_START:
case LE_COLOR1:
/* Display a red picture */
out->buffers = (uint8_t**)&(red_buffer);
break;
case LE_COLOR2:
/* Display a blue picture */
out->buffers = (uint8_t**)&(blue_buffer);
break;
}
}
if (previous_state!=cfg->state && cfg->state>=LE_COLOR1 && previous_state>=LE_COLOR1)
{
gettimeofday(&time,NULL);
timestamp2 = time.tv_sec*1000 + time.tv_usec/1000;
delta = timestamp2 - timestamp1;
stat = average(delta);
if (timestamp1!=0 && delta>30 && delta<1000) /* test on delta to remove false measures */
{
printf("State : %d - Middle pixel value : %3.0f %3.0f %3.0f -- Time since last change : %d milliseconds ",
cfg->state,
r,g,b,
(int)delta);
printf("- avg : %4.1f ms - min : %4.f ms\n",stat.avg,stat.min );
DEBUG_latency = stat.avg;
}
timestamp1 = timestamp2;
}
previous_state = cfg->state;
return C_OK;
}
C_RESULT
latency_estimation_stage_close( vp_stages_latency_estimation_config_t *cfg )
{
return C_OK;
}
const vp_api_stage_funcs_t vp_stages_latency_estimation_funcs =
{
(vp_api_stage_handle_msg_t) latency_estimation_stage_handle_message,
(vp_api_stage_open_t)latency_estimation_stage_open,
(vp_api_stage_transform_t)latency_estimation_stage_transform,
(vp_api_stage_close_t)latency_estimation_stage_close
};
ARDroneLib/Soft/Lib/ardrone_tool/Video/video_stage_latency_estimation.h
-37
Ver Arquivo
@@ -1,37 +0,0 @@
/*
* video_stage_latency_estimation.h
*
* Created on: Sep 14, 2011
* Author: s_piskorsko
*/
#ifndef VIDEO_STAGE_LATENCY_ESTIMATION_H_
#define VIDEO_STAGE_LATENCY_ESTIMATION_H_
#include <ardrone_tool/Video/vlib_stage_decode.h>
typedef enum{
LE_DISABLED=0,
LE_WAITING,
LE_START,
LE_COLOR1,
LE_COLOR2,
}vp_stages_latency_estimation_state;
typedef struct {
vp_stages_latency_estimation_state state;
vp_stages_latency_estimation_state previous_state;
int w,h;
vlib_stage_decoding_config_t * last_decoded_frame_info;
}vp_stages_latency_estimation_config_t;
C_RESULT latency_estimation_stage_handle_message(void *cfg, PIPELINE_MSG msg_id, void *callback, void *param);
C_RESULT latency_estimation_stage_open( vp_stages_latency_estimation_config_t *cfg );
C_RESULT latency_estimation_stage_transform( vp_stages_latency_estimation_config_t *cfg , vp_api_io_data_t *in, vp_api_io_data_t *out);
C_RESULT latency_estimation_stage_close( vp_stages_latency_estimation_config_t *cfg );
extern const vp_api_stage_funcs_t vp_stages_latency_estimation_funcs;
#endif /* VIDEO_STAGE_LATENCY_ESTIMATION_H_ */
ARDroneLib/Soft/Lib/ardrone_tool/Video/video_stage_merge_slices.c
-292
Ver Arquivo
@@ -1,292 +0,0 @@
#include "video_stage_merge_slices.h"
#include <video_encapsulation.h>
#include <VP_Os/vp_os_malloc.h>
#include <VP_Os/vp_os_assert.h>
#include <stdio.h>
const vp_api_stage_funcs_t video_stage_merge_slices_funcs = {
(vp_api_stage_handle_msg_t) video_stage_merge_slices_handle,
(vp_api_stage_open_t) video_stage_merge_slices_open,
(vp_api_stage_transform_t) video_stage_merge_slices_transform,
(vp_api_stage_close_t) video_stage_merge_slices_close
};
/**
* IPHONE DEBUG AREA
*/
float DEBUG_nbSlices = 0.0;
float DEBUG_totalSlices = 0.0;
int DEBUG_missed = 0;
int DEBUG_prevNumber = 0;
/* CONFIGURATION */
#define VIDEO_SLICE_DEBUG_ENABLE_ON_GLOBAL_DEBUG (0) ///< Auto enable debug while on debug mode
#define VIDEO_SLICE_DEBUG_ENABLE (0) ///< Force enable of debug for this file
/* END OF CONFIGURATION */
#if VIDEO_SLICE_DEBUG_ENABLE || (VIDEO_SLICE_DEBUG_ENABLE_ON_GLOBAL_DEBUG && defined (DEBUG))
#define __VIDEO_SLICE_DEBUG_ENABLED (1)
#else
#define __VIDEO_SLICE_DEBUG_ENABLED (0)
#endif
#if __VIDEO_SLICE_DEBUG_ENABLED
#define VIDEO_SLICE_DEBUG(...) \
do \
{ \
printf ("VIDEO_SLICE-DEBUG (%s @ %d) : ", __FUNCTION__, __LINE__); \
printf (__VA_ARGS__); \
printf ("\n"); \
} while (0)
#else
#define VIDEO_SLICE_DEBUG(...)
#endif
C_RESULT video_stage_merge_slices_handle (video_stage_merge_slices_config_t * cfg, PIPELINE_MSG msg_id, void *callback, void *param)
{
return C_OK;
}
C_RESULT video_stage_merge_slices_open(video_stage_merge_slices_config_t *cfg)
{
int idx;
for (idx=0;idx<2;idx++)
{
cfg->bufs[idx].bufferPointer = vp_os_malloc (sizeof (int8_t *));
if (NULL == cfg->bufs[idx].bufferPointer)
{
printf ("Unable to allocate output buffer pointer\n");
return C_FAIL;
}
cfg->bufs[idx].accumulated_size =0;
cfg->bufs[idx].buffer = NULL;
cfg->bufs[idx].buffer_size = 0;
}
cfg->mergingBuffer = 0;
cfg->readyBuffer = 0;
return C_OK;
}
C_RESULT video_stage_merge_slices_reset(video_stage_merge_slices_config_t *cfg)
{
int idx;
for (idx=0;idx<2;idx++)
{
if (cfg->bufs[idx].buffer) { vp_os_sfree((void**)&(cfg->bufs[idx].buffer)); }
cfg->bufs[idx].accumulated_size =0;
cfg->bufs[idx].buffer_size = 0;
cfg->bufs[idx].nb_slices = 0;
}
cfg->mergingBuffer = 0;
cfg->readyBuffer = 0;
return C_OK;
}
C_RESULT video_stage_merge_slices_transform(video_stage_merge_slices_config_t *cfg, vp_api_io_data_t *in, vp_api_io_data_t *out)
{
bool_t switchBuffers;
bool_t dataReady = 0;
video_stage_merge_slices_buffer_t *buf;
int dataSize;
out->size = 0;
if(out->status == VP_API_STATUS_INIT)
{
/* By default, feed the next stage with our local output */
out->numBuffers = 1;
out->buffers = cfg->bufs[0].bufferPointer;
out->buffers[0] = cfg->bufs[0].buffer;
out->indexBuffer = 0;
out->lineSize = 0;
out->status = VP_API_STATUS_PROCESSING;
}
// check input
parrot_video_encapsulation_t *PaVE = (parrot_video_encapsulation_t *) in->buffers[in->indexBuffer];
parrot_video_encapsulation_t *mergingPaVE = (parrot_video_encapsulation_t *) cfg->bufs[cfg->mergingBuffer].buffer;
VIDEO_SLICE_DEBUG("PAVE frm %d sl %d/%d %s\n ",
PaVE->frame_number,
PaVE->slice_index+1,
PaVE->total_slices,
(PaVE->frame_type == FRAME_TYPE_I_FRAME || PaVE->frame_type == FRAME_TYPE_IDR_FRAME) ? "I-frm":"p-frm" );
if ( (PaVE==NULL) || (!PAVE_CHECK(PaVE)) || (PaVE->total_slices == 1) )
{
// No slices, just send the data to the next stage
/* Feed the next stage with the previous stage's output */
out->buffers = in->buffers;
out->indexBuffer = in->indexBuffer;
out->lineSize = in->lineSize;
out->numBuffers = in->numBuffers;
out->status = in->status;
out->size = in->size;
out->status = VP_API_STATUS_PROCESSING;
/* Get rid of previously accumulated data */
video_stage_merge_slices_reset(cfg);
/* IPHONE DEBUG : set slices miss to 0/1 (to avoid keeping old/fake slice datas) */
DEBUG_totalSlices = 1;
DEBUG_nbSlices = 0;
return C_OK;
}
/*
* Check if the incoming PaVE belongs to the same frame.
*/
switchBuffers = (mergingPaVE) /* At program startup this buffer is empty and switching makes no sense */
&& ( !(pave_is_same_frame(PaVE,mergingPaVE)) );
/* If not the same frame, start building a new one */
if (switchBuffers)
{
if (0 != DEBUG_prevNumber)
{
int DEBUG_tempMiss = (PaVE->frame_number - (DEBUG_prevNumber + 1));
if (0 < DEBUG_tempMiss)
{
DEBUG_missed += DEBUG_tempMiss;
}
}
DEBUG_prevNumber = PaVE->frame_number;
cfg->readyBuffer = cfg->mergingBuffer;
cfg->mergingBuffer = (cfg->mergingBuffer+1)%2;
cfg->bufs[cfg->mergingBuffer].accumulated_size = 0;
cfg->bufs[cfg->mergingBuffer].nb_slices = 0;
}
/* Get a pointer to the buffer to store data to */
buf = &cfg->bufs[cfg->mergingBuffer];
// Check destination buffer size
if (buf->buffer_size < (buf->accumulated_size + in->size))
{
buf->buffer_size = buf->accumulated_size + in->size;
buf->buffer = vp_os_realloc(buf->buffer, buf->buffer_size );
if(!buf->buffer) { return C_FAIL; }
}
/* Copy data */
if (PaVE->slice_index == 0 || buf->accumulated_size==0)
{
/* Copy the entire packet */
if (buf->buffer) { vp_os_memcpy(buf->buffer, PaVE, in->size); }
buf->accumulated_size = in->size;
buf->nb_slices++;
}
else
{
#define mymin(x,y) ( ((x)<(y))?(x):(y) )
/* Copy only the payload */
dataSize = mymin(PaVE->payload_size,in->size); /* safety if the PaVE is corrupted */
if (buf->buffer) { vp_os_memcpy(buf->buffer + buf->accumulated_size, in->buffers[in->indexBuffer] + PaVE->header_size, dataSize); }
buf->accumulated_size += dataSize;
buf->nb_slices++;
}
/* Select the buffer to send to the next stage */
if (PaVE->slice_index == PaVE->total_slices - 1)
{
/* The buffer we just used for merging is ready to be sent */
cfg->readyBuffer = cfg->mergingBuffer;
cfg->mergingBuffer = (cfg->mergingBuffer+1)%2;
cfg->bufs[cfg->mergingBuffer].accumulated_size = 0;
cfg->bufs[cfg->mergingBuffer].nb_slices = 0;
dataReady = 1;
}
else if (switchBuffers)
{
/* Send the previous buffer if it contains something */
dataReady = 1;
}
// If slice was the last one, continue to next stage
if (dataReady)
{
buf = &cfg->bufs[cfg->readyBuffer];
if (buf->buffer && buf->accumulated_size)
{
int totalSlices;
parrot_video_encapsulation_t *newPaVE = (parrot_video_encapsulation_t *) buf->buffer;
/* Save the old value of total slices */
totalSlices = newPaVE->total_slices;
newPaVE->payload_size = buf->accumulated_size - PaVE->header_size;
newPaVE->slice_index = 0;
newPaVE->total_slices = 1;
/* Feed the next stage with our local output */
out->size = buf->accumulated_size;
out->buffers = buf->bufferPointer;
out->buffers[0] = buf->buffer;
out->indexBuffer = 0;
out->numBuffers = 1;
out->lineSize = 0;
out->status = VP_API_STATUS_PROCESSING;
if(!PAVE_CHECK(buf->buffer)) { printf("%s:%d - No PaVE !\n",__FUNCTION__,__LINE__); assert(0==1); }
if (buf->nb_slices != totalSlices) { printf("Missing slices (%d)\n",totalSlices-buf->nb_slices); }
DEBUG_nbSlices = 0.9*DEBUG_nbSlices + 0.1*(totalSlices-buf->nb_slices);
DEBUG_totalSlices = 0.9*DEBUG_totalSlices + 0.1*totalSlices;
}
}
else
{
out->size = 0;
}
if (out->size)
{
PaVE = (parrot_video_encapsulation_t*) out->buffers[0];
VIDEO_SLICE_DEBUG("Switch %d - Sending - PAVE frm %d sl %d/%d\n ",
switchBuffers,
PaVE->frame_number,
PaVE->slice_index+1,
PaVE->total_slices );}
return C_OK;
}
C_RESULT video_stage_merge_slices_close(video_stage_merge_slices_config_t *cfg)
{
int idx;
video_stage_merge_slices_buffer_t *buf;
for (idx=0;idx<2;idx++)
{
buf = &cfg->bufs[idx];
if (buf->buffer) { vp_os_sfree((void**)&buf->buffer); }
if (buf->bufferPointer) { vp_os_sfree((void**)&buf->bufferPointer); }
}
return C_OK;
}
ARDroneLib/Soft/Lib/ardrone_tool/Video/video_stage_merge_slices.h
-30
Ver Arquivo
@@ -1,30 +0,0 @@
#ifndef _VIDEO_STAGE_MERGE_SLICES_H_
#define _VIDEO_STAGE_MERGE_SLICES_H_
#include <VP_Api/vp_api.h>
typedef struct {
uint32_t accumulated_size;
uint32_t buffer_size;
uint8_t *buffer;
uint8_t **bufferPointer;
int nb_slices;
}video_stage_merge_slices_buffer_t;
typedef struct _video_stage_merge_slices_config_t
{
int mergingBuffer;
int readyBuffer;
video_stage_merge_slices_buffer_t bufs[2];
}
video_stage_merge_slices_config_t;
C_RESULT video_stage_merge_slices_handle (video_stage_merge_slices_config_t * cfg, PIPELINE_MSG msg_id, void *callback, void *param);
C_RESULT video_stage_merge_slices_open(video_stage_merge_slices_config_t *cfg);
C_RESULT video_stage_merge_slices_transform(video_stage_merge_slices_config_t *cfg, vp_api_io_data_t *in, vp_api_io_data_t *out);
C_RESULT video_stage_merge_slices_close(video_stage_merge_slices_config_t *cfg);
extern const vp_api_stage_funcs_t video_stage_merge_slices_funcs;
#endif
ARDroneLib/Soft/Lib/ardrone_tool/Video/video_stage_recorder.c
-251
Ver Arquivo
@@ -1,251 +0,0 @@
#include <time.h>
#ifndef _WIN32
#include <sys/time.h>
#else
#include <sys/timeb.h>
#include <Winsock2.h> // for timeval structure
int gettimeofday (struct timeval *tp, void *tz)
{
struct _timeb timebuffer;
_ftime (&timebuffer);
tp->tv_sec = (long)timebuffer.time;
tp->tv_usec = (long)timebuffer.millitm * 1000;
return 0;
}
#endif
#include <VP_Os/vp_os_malloc.h>
#include <VP_Api/vp_api_picture.h>
#include <config.h>
#include <ardrone_tool/Video/video_stage_recorder.h>
//#define USE_FIXED_60FPS
#ifdef USE_VIDEO_YUV
#define VIDEO_FILE_EXTENSION "yuv"
#else
#define VIDEO_FILE_EXTENSION "y"
#endif
#ifndef VIDEO_FILE_DEFAULT_PATH
#ifdef USE_ELINUX
#define VIDEO_FILE_DEFAULT_PATH "/data/video/usb/"
#else
#define VIDEO_FILE_DEFAULT_PATH root_dir
extern char root_dir[];
#endif
#endif
#if defined (NAVDATA_VISION_INCLUDED) && defined (USE_ELINUX)
static int32_t picture_captured = 0;
extern void navdata_set_raw_picture(int32_t new_raw_picture);
#endif
const vp_api_stage_funcs_t video_recorder_funcs = {
(vp_api_stage_handle_msg_t) video_stage_recorder_handle,
(vp_api_stage_open_t) video_stage_recorder_open,
(vp_api_stage_transform_t) video_stage_recorder_transform,
(vp_api_stage_close_t) video_stage_recorder_close
};
C_RESULT
video_stage_recorder_handle (video_stage_recorder_config_t * cfg, PIPELINE_MSG msg_id, void *callback, void *param)
{
void (*recorder_callback)(video_stage_recorder_config_t*) = callback;
switch (msg_id)
{
case PIPELINE_MSG_START: //video
{
if(cfg->startRec==VIDEO_RECORD_STOP)
{
cfg->startRec=VIDEO_RECORD_HOLD;
}
else
{
cfg->startRec=VIDEO_RECORD_STOP;
}
}
break;
case PIPELINE_MSG_COMMAND: //picture
{
if(cfg->startRec==VIDEO_RECORD_STOP)
{
cfg->startRec=VIDEO_PICTURE_HOLD;
}
else
{
cfg->startRec=VIDEO_RECORD_STOP;
}
}
default:
{
break;
}
}
if (recorder_callback!=NULL) { recorder_callback(cfg); }
return (VP_SUCCESS);
}
C_RESULT video_stage_recorder_open(video_stage_recorder_config_t *cfg)
{
cfg->startRec=VIDEO_RECORD_STOP;
return C_OK;
}
C_RESULT video_stage_recorder_transform(video_stage_recorder_config_t *cfg, vp_api_io_data_t *in, vp_api_io_data_t *out)
{
#ifndef _WIN32
time_t temptime;
struct timeval tv;
#ifdef USE_FIXED_60FPS
static struct timeval old_tv;
static uint8_t* old_pic=NULL;
unsigned long delta_us=0;
int ratio=0;
int i=0;
#endif
vp_os_mutex_lock( &out->lock );
if( out->status == VP_API_STATUS_INIT )
{
out->numBuffers = 1;
out->indexBuffer = 0;
out->lineSize = NULL;
//out->buffers = (int8_t **) vp_os_malloc( sizeof(int8_t *) );
}
out->size = in->size;
out->status = in->status;
out->buffers = in->buffers;
if( in->status == VP_API_STATUS_ENDED ) {
out->status = in->status;
}
else if(in->status == VP_API_STATUS_STILL_RUNNING) {
out->status = VP_API_STATUS_PROCESSING;
}
else {
out->status = in->status;
}
gettimeofday(&tv,NULL);
vp_api_picture_t* picture = (vp_api_picture_t *) in->buffers;
if(cfg->startRec==VIDEO_RECORD_HOLD)
{
struct tm *atm;
temptime = (time_t)tv.tv_sec;
atm = localtime(&temptime); //atm = localtime(&tv.tv_sec);
printf("recording video\n");
if(strlen(cfg->video_filename) == 0)
sprintf(cfg->video_filename, "%s/video_%04d%02d%02d_%02d%02d%02d_w%i_h%i.%s",
VIDEO_FILE_DEFAULT_PATH,
atm->tm_year+1900, atm->tm_mon+1, atm->tm_mday,
atm->tm_hour, atm->tm_min, atm->tm_sec,
picture->width,picture->height,
VIDEO_FILE_EXTENSION);
cfg->fp = fopen(cfg->video_filename, "wb");
if (cfg->fp == NULL)
printf ("error open file %s\n", cfg->video_filename);
cfg->startRec=VIDEO_RECORD_START;
}
if(cfg->startRec==VIDEO_PICTURE_HOLD)
{
struct tm *atm;
temptime = (time_t)tv.tv_sec;
atm = localtime(&temptime); //atm = localtime(&tv.tv_sec);
printf("recording picture\n");
sprintf(cfg->video_filename, "%s/picture_%04d%02d%02d_%02d%02d%02d_w%i_h%i.%s",
VIDEO_FILE_DEFAULT_PATH,
atm->tm_year+1900, atm->tm_mon+1, atm->tm_mday,
atm->tm_hour, atm->tm_min, atm->tm_sec,
picture->width,picture->height,
VIDEO_FILE_EXTENSION);
cfg->fp = fopen(cfg->video_filename, "wb");
if (cfg->fp == NULL)
printf ("error open file %s\n", cfg->video_filename);
cfg->startRec=VIDEO_PICTURE_START;
}
if( cfg->fp != NULL && out->size > 0 && out->status == VP_API_STATUS_PROCESSING && (cfg->startRec==VIDEO_RECORD_START||cfg->startRec==VIDEO_PICTURE_START ))
{
#if defined (NAVDATA_VISION_INCLUDED) && defined (USE_ELINUX)
navdata_set_raw_picture(picture_captured++);
#endif
#ifdef USE_FIXED_60FPS
delta_us=(tv.tv_sec*1000000+tv.tv_usec)-(old_tv.tv_sec*1000000+old_tv.tv_usec);
ratio=delta_us/16666;
old_tv=tv;
for(i=0; i<ratio && old_pic; i++)
{
fwrite(old_pic, picture->width * picture->height, 1, cfg->fp);
#ifdef USE_VIDEO_YUV
fwrite(old_pic+(picture->width * picture->height), picture->width * picture->height >> 1, 1, cfg->fp);
#endif
}
if(old_pic==NULL)
{
old_pic=vp_os_malloc(picture->width * picture->height*3/2);
}
memcpy(old_pic, picture->y_buf, picture->width * picture->height);
memcpy(old_pic + picture->width * picture->height, picture->cb_buf, picture->width * picture->height >> 2);
memcpy(old_pic + picture->width * picture->height*5/4, picture->cr_buf, picture->width * picture->height >> 2);
#endif //USE_FIXED_60FPS
fwrite(picture->y_buf, picture->width * picture->height, 1, cfg->fp);
#ifdef USE_VIDEO_YUV
fwrite(picture->cb_buf, picture->width * picture->height >> 2, 1, cfg->fp);
fwrite(picture->cr_buf, picture->width * picture->height >> 2, 1, cfg->fp);
#endif
if(cfg->startRec==VIDEO_PICTURE_START )//if picture, we stop after one picture ( and if video we continue )
{
cfg->startRec=VIDEO_RECORD_STOP;
}
}
else
{
if(cfg->startRec==VIDEO_RECORD_STOP && cfg->fp !=NULL)
{
#ifdef USE_FIXED_60FPS
if(old_pic)
{
vp_os_free(old_pic);
old_pic=NULL;
}
#endif
fclose(cfg->fp);
cfg->fp=NULL;
}
}
vp_os_mutex_unlock( &out->lock );
#endif
return C_OK;
}
C_RESULT video_stage_recorder_close(video_stage_recorder_config_t *cfg)
{
if( cfg->fp != NULL )
fclose( cfg->fp );
return C_OK;
}
ARDroneLib/Soft/Lib/ardrone_tool/Video/video_stage_recorder.h
-36
Ver Arquivo
@@ -1,36 +0,0 @@
#ifndef _VIDEO_STAGE_RECORDER_H_
#define _VIDEO_STAGE_RECORDER_H_
#include <stdio.h>
#include <VP_Api/vp_api.h>
#define VIDEO_FILENAME_LENGTH 1024
#ifndef _VIDEO_RECORD_STATE_ENUM_
#define _VIDEO_RECORD_STATE_ENUM_
typedef enum
{
VIDEO_RECORD_HOLD, // Video recording is on hold, waiting for the start command. This is the default state.
VIDEO_RECORD_START, // Video recording has started.
VIDEO_PICTURE_START,
VIDEO_PICTURE_HOLD,
VIDEO_RECORD_STOP // Video recording has been stopped. Stage will end and restart.
} video_record_state;
#endif
typedef struct _video_stage_recorder_config_t
{
char video_filename[VIDEO_FILENAME_LENGTH];
FILE* fp;
video_record_state startRec;
int stage;
} video_stage_recorder_config_t;
C_RESULT video_stage_recorder_handle (video_stage_recorder_config_t * cfg, PIPELINE_MSG msg_id, void *callback, void *param);
C_RESULT video_stage_recorder_open(video_stage_recorder_config_t *cfg);
C_RESULT video_stage_recorder_transform(video_stage_recorder_config_t *cfg, vp_api_io_data_t *in, vp_api_io_data_t *out);
C_RESULT video_stage_recorder_close(video_stage_recorder_config_t *cfg);
extern const vp_api_stage_funcs_t video_recorder_funcs;
#endif // _VIDEO_STAGE_RECORDER_H_
ARDroneLib/Soft/Lib/ardrone_tool/Video/video_stage_tcp.c
-413
Ver Arquivo
@@ -1,413 +0,0 @@
//
// video_stage_tcp.c
// ARDroneEngine
//
// Created by nicolas on 15/07/11.
// Copyright 2011 Parrot. All rights reserved.
//
#include "video_stage_tcp.h"
#include <VP_Os/vp_os_malloc.h>
#include <VP_Os/vp_os_print.h>
#include <video_encapsulation.h>
#include <VP_Os/vp_os_assert.h>
/* CONFIGURATION */
#define VIDEO_TCP_DEBUG_ENABLE_ON_GLOBAL_DEBUG (0) ///< Auto enable debug while on debug mode
#define VIDEO_TCP_DEBUG_ENABLE (0) ///< Force enable of debug for this file
/* END OF CONFIGURATION */
#if VIDEO_TCP_DEBUG_ENABLE || (VIDEO_TCP_DEBUG_ENABLE_ON_GLOBAL_DEBUG && defined (DEBUG))
#define __VIDEO_TCP_DEBUG_ENABLED (1)
#else
#define __VIDEO_TCP_DEBUG_ENABLED (0)
#endif
#if __VIDEO_TCP_DEBUG_ENABLED
#define VIDEO_TCP_DEBUG(...) \
do \
{ \
printf ("VIDEO_TCP-DEBUG (%s @ %d) : ", __FUNCTION__, __LINE__); \
printf (__VA_ARGS__); \
printf ("\n"); \
} while (0)
#else
#define VIDEO_TCP_DEBUG(...)
#endif
void video_stage_tcp_dumpPave (parrot_video_encapsulation_t *PaVE)
{
printf ("Signature : \"%c%c%c%c\" [0x%02x][0x%02x][0x%02x][0x%02x]\n", PaVE->signature[0], PaVE->signature[1],
PaVE->signature[2], PaVE->signature[3], PaVE->signature[0], PaVE->signature[1], PaVE->signature[2], PaVE->signature[3]);
printf ("Frame Type / Number : %s : %d : slice %d/%d\n",
(PaVE->frame_type == FRAME_TYPE_P_FRAME) ? "P-Frame" : ((PaVE->frame_type == FRAME_TYPE_I_FRAME) ? "I-Frame" : "IDR-Frame"),
PaVE->frame_number,
PaVE->slice_index+1,
PaVE->total_slices);
printf ("Codec : %s\n", (PaVE->video_codec == CODEC_MPEG4_VISUAL) ? "MP4" : ((PaVE->video_codec == CODEC_MPEG4_AVC) ? "H264" : "Unknown"));
printf ("StreamID : %d \n", PaVE->stream_id);
printf ("Encoded dims : %d x %d\n", PaVE->encoded_stream_width, PaVE->encoded_stream_height);
printf ("Display dims : %d x %d\n", PaVE->display_width, PaVE->display_height);
printf ("Header size : %d\n", PaVE->header_size);
printf ("Payload size : %d\n", PaVE->payload_size);
}
#define BUFFER_MAX_SIZE (cfg->maxPFramesPerIFrame * cfg->frameMeanSize)
const vp_api_stage_funcs_t video_stage_tcp_funcs = {
(vp_api_stage_handle_msg_t) NULL,
(vp_api_stage_open_t) video_stage_tcp_open,
(vp_api_stage_transform_t) video_stage_tcp_transform,
(vp_api_stage_close_t) video_stage_tcp_close
};
static inline bool_t frameIsIFrame (uint8_t *framePointer)
{
parrot_video_encapsulation_t *PaVE = (parrot_video_encapsulation_t *)framePointer;
if ( (PaVE->frame_type == FRAME_TYPE_I_FRAME ||
PaVE->frame_type == FRAME_TYPE_IDR_FRAME) && (PaVE->slice_index == 0) )
{
return TRUE;
}
return FALSE;
}
static inline bool_t frameHasPaVE (uint8_t *framePointer)
{
parrot_video_encapsulation_t *PaVE = (parrot_video_encapsulation_t *)framePointer;
if (NULL != PaVE &&
PaVE->signature[0] == 'P' &&
PaVE->signature[1] == 'a' &&
PaVE->signature[2] == 'V' &&
PaVE->signature[3] == 'E')
{
return TRUE;
}
return FALSE;
}
#if __VIDEO_TCP_DEBUG_ENABLED
static void printBuffer(uint8_t * buf,int size)
{
const int columns=100;
int i;
char c;
if ( (!buf) || (size<0)) { return; }
printf("\n"); for (i=0;i<columns;i++) printf("="); printf("\n");
for (i=0;i<size;i++)
{
c=buf[i];
switch(c)
{
case 'P':
case 'a': /* ifrm */
case 'V':
case 'E': putchar(c); break;
#if 0
case 0 : putchar('0'); break; /* nal start */
case 1 : putchar('1'); break; /* nal start */
case 0x67 : putchar('S'); break; /* sps */
case 0x68 : putchar('P'); break; /* pps */
case 0x65 : putchar('p'); break; /* pfrm */
#endif
default : putchar('.');
}
if ((i%columns)==(columns-1)) { printf("\n"); }
}
printf("\n"); for (i=0;i<columns;i++) printf("="); printf("\n");
}
#endif
C_RESULT video_stage_tcp_open(video_stage_tcp_config_t *cfg)
{
cfg->globalBuffer = vp_os_malloc (BUFFER_MAX_SIZE * sizeof (int8_t));
if (NULL == cfg->globalBuffer)
{
printf ("Unable to allocate TCP Buffer for frame reconstitution\n");
return C_FAIL;
}
cfg->frameBuffer = vp_os_malloc (BUFFER_MAX_SIZE * sizeof (int8_t)); //(2 * cfg->frameMeanSize * sizeof (int8_t));
if (NULL == cfg->frameBuffer)
{
printf ("Unable to allocate output frame buffer\n");
return C_FAIL;
}
cfg->bufferPointer = vp_os_malloc (sizeof (int8_t *));
if (NULL == cfg->bufferPointer)
{
printf ("Unable to allocate output buffer pointer\n");
return C_FAIL;
}
cfg->currentSize = 0;
return C_OK;
}
C_RESULT video_stage_tcp_transform(video_stage_tcp_config_t *cfg, vp_api_io_data_t *in, vp_api_io_data_t *out)
{
uint8_t * buf = NULL;
bool_t directForward = FALSE;
#if __VIDEO_TCP_DEBUG_ENABLED
printBuffer(in->buffers[in->indexBuffer],in->size);
#endif
if(out->status == VP_API_STATUS_INIT) // Init only code
{
out->numBuffers = 1;
out->buffers = cfg->bufferPointer;
out->buffers[0] = cfg->frameBuffer;
out->indexBuffer = 0;
out->lineSize = 0;
out->status = VP_API_STATUS_PROCESSING;
}
out->size = 0;
/* Should not happen, but ... */
if(in->size < -1 )
{
printf ("Bad size ...\n");
VP_OS_ASSERT(0==1);
return C_OK;
}
if (out->status == VP_API_STATUS_PROCESSING)
{
cfg->tcpStageHasMoreData = FALSE;
}
if (in->size > 0 && out->status == VP_API_STATUS_PROCESSING)
{
/* Directly forward the packet if it exactly contains a whole frame
* This happens when this stage is used with a UDP stream and one frame per packet.
*/
if (cfg->currentSize == 0)
{
buf = (uint8_t*)(in->buffers[in->indexBuffer]);
if ( frameHasPaVE(buf) )
{
parrot_video_encapsulation_t *PaVE = (parrot_video_encapsulation_t *)(in->buffers[in->indexBuffer]);
if ( in->size == (PaVE->header_size + PaVE->payload_size ))
{
directForward = TRUE;
VIDEO_TCP_DEBUG("No luck UDP ! ");
}
}
/* true if a UVLC or P264 header is present */
else if ( ((*(uint32_t*)buf) & 0xFFFe7c00 )== 0 )
{
/* Checks if the first encountered packet looks like a UVLC/P.264 packet */
directForward = TRUE;
}
}
if (TRUE == directForward)
{
out->numBuffers = in->numBuffers;
out->buffers = in->buffers;
out->indexBuffer = in->indexBuffer;
out->lineSize = in->lineSize;
out->status = VP_API_STATUS_PROCESSING;
out->size = in->size;
return C_OK;
}
if (in->size + cfg->currentSize >= BUFFER_MAX_SIZE)
{
printf ("Got a too big buffer for mine : got %d, had %d, max %d\n", in->size, cfg->currentSize, BUFFER_MAX_SIZE);
cfg->currentSize = 0;
return C_OK;
}
// Copy input buffer
vp_os_memcpy(&(cfg->globalBuffer[cfg->currentSize]), in->buffers[in->indexBuffer], in->size);
cfg->currentSize += in->size;
}
if (out->status == VP_API_STATUS_PROCESSING ||
out->status == VP_API_STATUS_STILL_RUNNING ||
(cfg->tcpStageHasMoreData==TRUE)
)
{
int lastIFrameStart = -1; /* Index of the last complete I-frame inside the 'globalBuffer' */
// Align buffer to first complete PaVE
int index = 0;
int maxIndex = cfg->currentSize - sizeof (parrot_video_encapsulation_t);
// Find first
for (index = 0; index <= maxIndex; index ++)
{
if (cfg->globalBuffer[index] == 'P' &&
cfg->globalBuffer[index+1] == 'a' &&
cfg->globalBuffer[index+2] == 'V' &&
cfg->globalBuffer[index+3] == 'E')
{
break;
}
}
if ( (maxIndex+1) == index || 0 > maxIndex )
{
/* Keep data in the buffer in case a piece of PaVE is at the end */
VIDEO_TCP_DEBUG ("Not enough picture data (PaVE not present or incomplete) ...");
return C_OK; // No picture, let out->size to zero
}
// If needed, dump all datas before this PaVE (so we're sure that we have a PaVE at index 0)
if (index != 0)
{
VIDEO_TCP_DEBUG ("First PaVE was not a index 0 (was at index %d)", index);
maxIndex -= index;
cfg->currentSize -= index;
memmove (cfg->globalBuffer, &(cfg->globalBuffer[index]), cfg->currentSize);
}
if (1 == cfg->latencyDrop)
{
// Iterate through all frames to find last available I Frame
int fIndex = 0;
#if __VIDEO_TCP_DEBUG_ENABLED
int fDropped = 0;
int fTested = 0;
static int totalDrop = 0;
#endif
while (fIndex < maxIndex)
{
// Check that the frame has a correct pave
if (FALSE == frameHasPaVE(&cfg->globalBuffer[fIndex]))
{
/* Should not happen, but ... */
#if __VIDEO_TCP_DEBUG_ENABLED
printf ("No pave found at index %d (current size = %d) on next frame ...\n",fIndex,cfg->currentSize);
printf("Should not happen. Happy debugging !!!\n");
video_stage_tcp_dumpPave ((parrot_video_encapsulation_t*)&cfg->globalBuffer[fIndex]);
VP_OS_ASSERT(0==1);
#endif
break;
}
// Check if the frame is complete
parrot_video_encapsulation_t *PaVE = (parrot_video_encapsulation_t *)&cfg->globalBuffer[fIndex];
#if __VIDEO_TCP_DEBUG_ENABLED
printf(" -- PaVE %d at index %d of accumulation buffer ---\n",fTested,fIndex);
video_stage_tcp_dumpPave (PaVE);
#endif
int packetSize = PaVE->header_size + PaVE->payload_size;
if ((cfg->currentSize-fIndex) >= packetSize && frameIsIFrame(&cfg->globalBuffer[fIndex]))
{
VIDEO_TCP_DEBUG ("Jumping to I-frame\n");
lastIFrameStart = fIndex;
#if __VIDEO_TCP_DEBUG_ENABLED
fDropped = fTested;
#endif
}
fIndex += packetSize;
#if __VIDEO_TCP_DEBUG_ENABLED
fTested++;
#endif
}
if (0 < lastIFrameStart)
{
#if __VIDEO_TCP_DEBUG_ENABLED
totalDrop += fDropped;
VIDEO_TCP_DEBUG ("I-frame found - dumping %3d frames --> total : %5d\n", fDropped, totalDrop);
#endif
// Dump all frames before last I frame
cfg->currentSize -= lastIFrameStart;
memmove (cfg->globalBuffer, &(cfg->globalBuffer[lastIFrameStart]), cfg->currentSize);
}
}
// Copy frame to decoder if complete
bool_t frameIsComplete = FALSE;
parrot_video_encapsulation_t *framePaVE = (parrot_video_encapsulation_t *)cfg->globalBuffer;
int framePacketSize = framePaVE->header_size + framePaVE->payload_size;
if (cfg->currentSize >= framePacketSize)
{
frameIsComplete = TRUE;
/* Copy the frame from the accumulation buffer to the output buffer */
vp_os_memcpy (cfg->frameBuffer, cfg->globalBuffer, framePacketSize);
cfg->currentSize -= framePacketSize;
/* Delete the sent frame from the accumulation buffer */
memmove (cfg->globalBuffer, &(cfg->globalBuffer[framePacketSize]), cfg->currentSize);
out->size = framePacketSize;
}
/* Fill the output structure */
if (frameIsComplete)
{
out->numBuffers = 1;
out->buffers = cfg->bufferPointer;
out->buffers[0] = cfg->frameBuffer;
out->indexBuffer = 0;
out->lineSize = 0;
out->status = VP_API_STATUS_PROCESSING;
/* Check the next available frame .. */
parrot_video_encapsulation_t *framePaVE = (parrot_video_encapsulation_t *)cfg->globalBuffer;
if (frameHasPaVE(cfg->globalBuffer) && cfg->currentSize>=sizeof(*framePaVE))
{
framePacketSize = framePaVE->header_size + framePaVE->payload_size;
if (cfg->currentSize >= framePacketSize)
{
/* Do this to inform the socket stage that more frames are available for decoding.
* In this case, the socket should be run in a non-blocking mode.
* This way, the next decoded frame is either an old frame already present in
* the TCP stage buffer, or a newer I-frame which just arrived through the socket.
*/
out->status = VP_API_STATUS_PROCESSING;
cfg->tcpStageHasMoreData = TRUE;
}
}
}
/*
* Here, the accumulation buffer must have a PaVE, but might not contain a complete frame.
*/
if(frameIsComplete && FALSE == frameHasPaVE(out->buffers[out->indexBuffer]))
{
/* Don't decode frame if it has no PaVE */
VIDEO_TCP_DEBUG ("Frame don't have PaVE");
printf("Should not happen. Happy debugging !!!\n");
video_stage_tcp_dumpPave ((parrot_video_encapsulation_t*)out->buffers[out->indexBuffer]);
VP_OS_ASSERT(0==1);
out->size = 0;
}
}
return C_OK;
}
C_RESULT video_stage_tcp_close(video_stage_tcp_config_t *cfg)
{
vp_os_free (cfg->bufferPointer);
cfg->bufferPointer = NULL;
vp_os_free (cfg->globalBuffer);
cfg->globalBuffer = NULL;
vp_os_free (cfg->frameBuffer);
cfg->frameBuffer = NULL;
return C_OK;
}
ARDroneLib/Soft/Lib/ardrone_tool/Video/video_stage_tcp.h
-35
Ver Arquivo
@@ -1,35 +0,0 @@
//
// video_stage_tcp.h
// ARDroneLib
//
// Created by nicolas on 15/07/11.
// Copyright 2011 Parrot. All rights reserved.
//
#ifndef _VIDEO_STAGE_TCP_H_
#define _VIDEO_STAGE_TCP_H_
#include <VP_Api/vp_api.h>
#include <inttypes.h>
typedef struct _video_stage_tcp_config_t
{
int maxPFramesPerIFrame;
int frameMeanSize;
int latencyDrop;
int currentSize;
uint8_t **bufferPointer;
uint8_t *globalBuffer;
uint8_t *frameBuffer;
bool_t tcpStageHasMoreData;
} video_stage_tcp_config_t;
C_RESULT video_stage_tcp_open(video_stage_tcp_config_t *cfg);
C_RESULT video_stage_tcp_transform(video_stage_tcp_config_t *cfg, vp_api_io_data_t *in, vp_api_io_data_t *out);
C_RESULT video_stage_tcp_close(video_stage_tcp_config_t *cfg);
extern const vp_api_stage_funcs_t video_stage_tcp_funcs;
#endif // _VIDEO_STAGE_TCP_H_
ARDroneLib/Soft/Lib/ardrone_tool/Video/vlib_stage_decode.c
-85
Ver Arquivo
@@ -1,85 +0,0 @@
#include <config.h>
#include <VP_Os/vp_os_malloc.h>
#include <VP_Os/vp_os_print.h>
#include <ardrone_tool/Video/vlib_stage_decode.h>
const vp_api_stage_funcs_t vlib_decoding_funcs = {
(vp_api_stage_handle_msg_t) NULL,
(vp_api_stage_open_t) vlib_stage_decoding_open,
(vp_api_stage_transform_t) vlib_stage_decoding_transform,
(vp_api_stage_close_t) vlib_stage_decoding_close
};
C_RESULT vlib_stage_decoding_open(vlib_stage_decoding_config_t *cfg)
{
video_codec_open( &cfg->controller, UVLC_CODEC );
video_controller_set_motion_estimation( &cfg->controller, FALSE );
video_controller_set_format( &cfg->controller, ACQ_WIDTH, ACQ_HEIGHT );
vp_os_free( cfg->controller.in_stream.bytes );
cfg->controller.in_stream.bytes = NULL;
return C_OK;
}
C_RESULT vlib_stage_decoding_transform(vlib_stage_decoding_config_t *cfg, vp_api_io_data_t *in, vp_api_io_data_t *out)
{
bool_t got_image;
vp_os_mutex_lock( &out->lock );
if(out->status == VP_API_STATUS_INIT)
{
out->numBuffers = 1;
out->buffers = (int8_t**)&cfg->picture;
out->indexBuffer = 0;
out->lineSize = 0;
out->status = VP_API_STATUS_PROCESSING;
}
if( in->status == VP_API_STATUS_ENDED ) {
out->status = in->status;
}
if(out->status == VP_API_STATUS_PROCESSING )
{
// If out->size == 1 it means picture is ready
out->size = 0;
cfg->controller.in_stream.bytes = (uint32_t*)in->buffers[0];
cfg->controller.in_stream.used = in->size;
cfg->controller.in_stream.size = in->size;
cfg->controller.in_stream.index = 0;
cfg->controller.in_stream.length = 32;
cfg->controller.in_stream.code = 0;
got_image = FALSE;
video_decode_blockline( &cfg->controller, cfg->picture, &got_image );
if( got_image )
{
// we got one picture
out->size = 1;
#ifndef USE_VIDEO_YUV
int32_t i;
for(i = 0; i < cfg->picture->width * cfg->picture->height / 4; i++ )
{
cfg->picture->cr_buf[i] = 0x80;
cfg->picture->cb_buf[i] = 0x80;
}
#endif
}
}
vp_os_mutex_unlock( &out->lock );
return C_OK;
}
C_RESULT vlib_stage_decoding_close(vlib_stage_decoding_config_t *cfg)
{
return video_codec_close( &cfg->controller );
}

Alguns arquivos não foram exibidos porque demasiados arquivos foram alterados neste diff Mostrar Mais