[ardrone] Auto startup and configure paparazzi

Added the possibility to auto startup paparazzi sdk or raw version. Automaticly configures the drone with the airframe file.

conf/Makefile.omap

@@ -78,35 +78,46 @@ elf: $(OBJDIR)/$(TARGET).elf
 # Program the device and start it.
 load upload program: $(OBJDIR)/$(TARGET).elf
 
-# If it is not the SDK version, then kill program.elf
-ifneq ($(BOARD_TYPE), sdk)
-	-echo "killall -9 program.elf" | telnet $(HOST)
-endif
-
 	# Kill the application
 	-echo "killall -9 $(TARGET).elf" | telnet $(HOST)
 
-		# Upload the modules and start the application
+	# Make the target dir and edit the config
 	-{ 												\
-	  echo "mkdir -p $(TARGET_DIR)";						\
+	  echo "mkdir -p $(TARGET_DIR)";				\
+	  echo "if grep -q \"start_paparazzi *= \" /data/config.ini; then sed -i 's/\(start_paparazzi *= *\).*/\\\1$(ARDRONE2_START_PAPARAZZI)/g' /data/config.ini; else echo \"start_paparazzi = $(ARDRONE2_START_PAPARAZZI)\" >> /data/config.ini; fi"; \
+      echo "if grep -q \"wifi_mode *= \" /data/config.ini; then sed -i 's/\(wifi_mode *= *\).*/\\\1$(ARDRONE2_WIFI_MODE)/g' /data/config.ini; else echo \"wifi_mode = $(ARDRONE2_WIFI_MODE)\" >> /data/config.ini; fi"; \
+      echo "if grep -q \"ssid_single_player *= \" /data/config.ini; then sed -i 's/\(ssid_single_player *= *\).*/\\\1$(ARDRONE2_SSID)/g' /data/config.ini; else echo \"ssid_single_player = $(ARDRONE2_SSID)\" >> /data/config.ini; fi"; \
+      echo "if grep -q \"static_ip_address_base *= \" /data/config.ini; then sed -i 's/\(static_ip_address_base *= *\).*/\\\1$(ARDRONE2_IP_ADDRESS_BASE)/g' /data/config.ini; else echo \"static_ip_address_base = $(ARDRONE2_IP_ADDRESS_BASE)\" >> /data/config.ini; fi"; \
+      echo "if grep -q \"static_ip_address_probe *= \" /data/config.ini; then sed -i 's/\(static_ip_address_probe *= *\).*/\\\1$(ARDRONE2_IP_ADDRESS_PROBE)/g' /data/config.ini; else echo \"static_ip_address_probe = $(ARDRONE2_IP_ADDRESS_PROBE)\" >> /data/config.ini; fi"; \
 	} | telnet $(HOST)
 
 	# Upload the drivers and new application
 	{																								\
 	  echo "binary";																				\
 	  echo "put $(PAPARAZZI_SRC)/sw/ext/ardrone2_drivers/cdc-acm.ko /$(SUB_DIR)/cdc-acm.ko";		\
+	  echo "put $(PAPARAZZI_SRC)/sw/ext/ardrone2_drivers/check_update.sh check_update.sh";		    \
+	  echo "put $(PAPARAZZI_SRC)/sw/ext/ardrone2_drivers/wifi_setup.sh wifi_setup.sh";		        \
 	  echo "put $(OBJDIR)/$(TARGET).elf /$(SUB_DIR)/$(TARGET).elf";									\
 	  echo "quit";																					\
 	} | ftp -n $(HOST)
 
 	# Upload the modules and start the application
-	-{ 												\
+	-{ 															\
-	  echo "cd $(TARGET_DIR)";						\
+	  echo "mv /data/video/check_update.sh /bin/";				\
-	  echo "insmod cdc-acm.ko";						\
+	  echo "mv /data/video/wifi_setup.sh /bin/";				\
-	  echo "chmod 777 $(TARGET).elf";				\
+	  echo "chmod 777 /bin/check_update.sh"         			\
-	  echo "./$(TARGET).elf > /dev/null 2>&1 &";	\
+	  echo "chmod 777 /bin/wifi_setup.sh"          			 	\
+	  echo "insmod $(TARGET_DIR)/cdc-acm.ko";					\
+	  echo "chmod 777 $(TARGET_DIR)/$(TARGET).elf";				\
+	  echo "$(TARGET_DIR)/$(TARGET).elf > /dev/null 2>&1 &";	\
 	} | telnet $(HOST)
 	
+ifeq ($(ARDRONE2_REBOOT),1)
+	-{ 										\
+	  echo "reboot";						\
+	} | telnet $(HOST)
+endif
+
 
 # Link: create ELF output file from object files.
 .SECONDARY : $(OBJDIR)/$(TARGET).elf

conf/boards/ardrone2_raw.makefile

@@ -15,11 +15,17 @@ $(TARGET).ARCHDIR = $(ARCH)
 
 # -----------------------------------------------------------------------
 USER=foobar
-HOST=192.168.1.1
+HOST?=192.168.1.1
 SUB_DIR=raw
 FTP_DIR=/data/video
 TARGET_DIR=$(FTP_DIR)/$(SUB_DIR)
 # -----------------------------------------------------------------------
+ARDRONE2_START_PAPARAZZI ?= 0
+ARDRONE2_WIFI_MODE ?= 0
+ARDRONE2_SSID ?= ardrone2_paparazzi
+ARDRONE2_IP_ADDRESS_BASE ?= 192.168.1.
+ARDRONE2_IP_ADDRESS_PROBE ?= 1
+# -----------------------------------------------------------------------
 
 # The GPS sensor is connected trough USB so we have to define the device
 GPS_PORT           ?= UART1

conf/boards/ardrone2_sdk.makefile

@@ -20,6 +20,12 @@ SUB_DIR=sdk
 FTP_DIR=/data/video
 TARGET_DIR=$(FTP_DIR)/$(SUB_DIR)
 # -----------------------------------------------------------------------
+ARDRONE2_START_PAPARAZZI ?= 0
+ARDRONE2_WIFI_MODE ?= 0
+ARDRONE2_SSID ?= ardrone2_paparazzi
+ARDRONE2_IP_ADDRESS_BASE ?= 192.168.1.
+ARDRONE2_IP_ADDRESS_PROBE ?= 1
+# -----------------------------------------------------------------------
 
 # The GPS sensor is connected trough USB so we have to define the device
 GPS_PORT         ?= UART1

sw/airborne/boards/ardrone/electrical_raw.c

@@ -68,10 +68,11 @@ static struct {
 int fd;
 
 void electrical_init(void) {
+  // First we try to kill the program.elf if it is running (done here because initializes first)
+  system("killall -9 program.elf");
 
+  // Initialize 12c device for power
   fd = open( "/dev/i2c-1", O_RDWR );
-
-
   if ( ioctl( fd, I2C_SLAVE_FORCE, 0x4a) < 0 ) {
     fprintf( stderr, "Failed to set slave address: %m\n" );
   }

sw/ext/ardrone2_drivers/check_update.sh

@@ -0,0 +1,96 @@
+#!/bin/sh
+
+UPDATE_DIR=/update
+UPDATE_PATH=$UPDATE_DIR/ardrone2_update.plf
+VERSION_PATH=$UPDATE_DIR/version.txt
+ERR_PATH=$UPDATE_DIR/err.log
+
+echo "Check if need to start paparazzi ..."
+START_PAPARAZZI=`grep start_paparazzi /data/config.ini | awk -F "=" '{ gsub(/ */,"",$2); print $2}'`
+case $START_PAPARAZZI in
+1)
+    START_PAPARAZZI=raw
+    ;;
+2)
+    START_PAPARAZZI=sdk
+    ;;
+*)
+    START_PAPARAZZI=no
+    ;;
+esac
+echo "START_PAPARAZZI=$START_PAPARAZZI"
+
+echo "Copy version.txt file in ftp directory"
+cp /firmware/version.txt $VERSION_PATH
+
+PELF_ARGS=$(cat /tmp/.program.elf.arguments | tr '\n' ' ')
+
+echo "Check if update is necessary ..."
+if [ -e $UPDATE_PATH ] ; then
+	VERSION=`cat $VERSION_PATH`
+	
+	if [ -e $ERR_PATH ] ; then
+		CHECK_ERR=`cat $ERR_PATH`
+		if [ "$CHECK_ERR" = "NEED_TO_FLASH" ] ; then
+			CHECK_PLF=`/bin/checkplf $UPDATE_PATH $VERSION`
+			if [ "$CHECK_PLF" = "NEED_TO_FLASH" ] ; then
+				echo "ERR=FLASH_KO" > $ERR_PATH
+			else
+				/bin/checkplf $UPDATE_PATH $VERSION > $ERR_PATH	
+			fi
+		else
+			/bin/checkplf $UPDATE_PATH $VERSION > $ERR_PATH	
+		fi
+	else
+		/bin/checkplf $UPDATE_PATH $VERSION > $ERR_PATH
+	fi
+	
+	CHECK_ERR=`cat $ERR_PATH`
+	if [ "$CHECK_ERR" = "NEED_TO_FLASH" ] ; then 
+    	echo "File $UPDATE_PATH exists... Start updating..."
+ 		pinst_trigger
+		echo "Rebooting..."
+    	reboot
+    else
+    	if [ "$CHECK_ERR" = "VERSION_OK" ] ; then
+ 	    	echo "Version OK"
+    	elif [ "$CHECK_ERR" = "ERR=FLASH_KO" ] ; then
+ 	    	echo "Error during Updating... Removing..."
+    	else
+ 	    	echo "File $UPDATE_PATH not valid... Removing..."
+    	fi
+# Cleaning update directory
+    	rm -Rf $UPDATE_DIR/*
+			cp /firmware/version.txt $VERSION_PATH
+		echo "Start Drone software..."
+		inetd
+		
+		# Check what to start
+		if [ "$START_PAPARAZZI" = "raw" ] ; then
+			(/data/video/raw/ap.elf; gpio 181 -d ho 1) &
+		elif [ "$START_PAPARAZZI" = "sdk" ] ; then
+			(/data/video/sdk/ap.elf; gpio 181 -d ho 1) &
+		else
+			(/bin/program.elf ${PELF_ARGS}; gpio 181 -d ho 1) &
+		fi
+	fi
+else
+    echo "File $UPDATE_PATH doesn't exists... Start Drone software..."
+# Cleaning update directory
+		rm -Rf $UPDATE_DIR/*
+		cp /firmware/version.txt $VERSION_PATH
+
+    inetd
+    # Check what to start
+    if [ "$START_PAPARAZZI" = "raw" ] ; then
+        (/bin/program.elf ${PELF_ARGS}; gpio 181 -d ho 1) &
+        sleep 10
+	(/data/video/raw/ap.elf; gpio 181 -d ho 1) &
+    elif [ "$START_PAPARAZZI" = "sdk" ] ; then
+        (/bin/program.elf ${PELF_ARGS}; gpio 181 -d ho 1) &
+        sleep 10
+	(/data/video/sdk/ap.elf; gpio 181 -d ho 1) &
+    else
+	(/bin/program.elf ${PELF_ARGS}; gpio 181 -d ho 1) &
+    fi
+fi

sw/ext/ardrone2_drivers/wifi_setup.sh

@@ -0,0 +1,201 @@
+#!/bin/sh
+#
+# Script to see if an IP adress is already used or not
+#
+# Getting SSID from config.ini file.
+
+#initializing random generator
+cat /data/random_mac.txt > /dev/urandom
+/bin/random_mac > /data/random_mac.txt
+
+#echo 2 > /proc/cpu/alignment
+
+export NETIF=ath0
+export WORKAREA="/lib/firmware"
+export ATH_PLATFORM="parrot-omap-sdio"
+export ATH_MODULE_ARGS="ifname=$NETIF"
+
+# Switch Wifi mode depending on value into /data/config.ini
+#
+
+WIFI_MODE=`grep wifi_mode /data/config.ini | awk -F "=" '{ gsub(/ */,"",$2); print $2}'` 
+
+case $WIFI_MODE in
+0)
+    WIFI_MODE=master
+    ;;
+1)
+    WIFI_MODE=ad-hoc
+    ;;
+2)
+    WIFI_MODE=managed
+    ;;
+*)
+    WIFI_MODE=master
+    ;;
+esac
+
+
+if [ -s /factory/mac_address.txt ]
+then
+MAC_ADDR=`cat /factory/mac_address.txt`
+else
+MAC_ADDR=`cat /data/random_mac.txt`
+fi
+
+loadAR6000.sh -i $NETIF --setmac $MAC_ADDR
+
+# Waiting 2s for the wifi chip to be ready
+sleep 2
+
+AR6K_PID=`ps_procps -A -T -c | grep AR6K | awk '{print $1}'`
+SDIO_PID=`ps_procps -A -T -c | grep ksdioirqd | awk '{print $1}'`
+
+#Changing wifi priority
+chrt -p -r 25 $SDIO_PID
+chrt -p -r 24 $AR6K_PID
+
+
+# Disabling powersaving
+wmiconfig -i $NETIF --power maxperf
+# Disabling 802.11n aggregation
+wmiconfig -i $NETIF --allow_aggr 0 0
+# enabling WMM
+wmiconfig -i $NETIF --setwmm 1
+
+i=0
+
+while [ ! -n "$SSID" ] && [ $i -lt 10 ]
+do
+i=`expr $i + 1`
+SSID=`grep ssid_single_player /data/config.ini | awk -F "=" '{print $2}'`
+
+# Removing leading and trailing spaces
+SSID=`echo $SSID`
+sleep 1
+done
+
+if [ -n "$SSID" ]
+then
+echo "SSID=$SSID"
+else
+#default SSID.
+SSID=ardrone2_wifi
+echo "SSID=\"$SSID\""
+fi
+
+RANDOM_CHAN=auto
+
+
+echo "Creating $WIFI_MODE Network $SSID"
+
+iwconfig $NETIF mode $WIFI_MODE
+iwconfig $NETIF essid "$SSID"
+
+if [ "$WIFI_MODE" != "managed" ]
+then
+# Allowing ACS to select only channels 1 & 6
+wmiconfig -i $NETIF --acsdisablehichannels 1
+iwconfig $NETIF channel $RANDOM_CHAN
+iwconfig $NETIF rate auto
+iwconfig $NETIF commit
+else
+# The Wifi connection freezes when in managed mode if there is no keepalive
+wmiconfig -i ath0 --setkeepalive 1
+fi
+
+wmiconfig -i $NETIF --dtim 1
+wmiconfig -i $NETIF --commit
+
+OK=0
+BASE_ADRESS=`grep static_ip_address_base /data/config.ini | awk -F "=" '{print $2}'`
+PROBE=`grep static_ip_address_probe /data/config.ini | awk -F "=" '{print $2}'`
+
+# Removing leading and trailing spaces
+BASE_ADRESS=`echo $BASE_ADRESS`
+PROBE=`echo $PROBE`
+
+# Default base address
+if [ -n "$BASE_ADRESS" ]
+then
+echo "BASE_ADRESS=$BASE_ADRESS"
+else
+#default BASE_ADDRESS.
+BASE_ADRESS=192.168.1.
+echo "BASE_ADRESS=\"$BASE_ADRESS\""
+fi
+
+# Default probe
+if [ -n "$PROBE" ]
+then
+echo "PROBE=$PROBE"
+else
+#default PROBE.
+PROBE=1
+echo "PROBE=\"$PROBE\""
+fi
+
+while [ $OK -eq 0 ]
+do
+#configuring interface.
+ifconfig $NETIF $BASE_ADRESS$PROBE
+arping -I $NETIF -q -f -D -w 2 $BASE_ADRESS$PROBE
+
+if [ $? -eq 1 ]
+then
+	if [ -s /data/old_adress.txt ]
+	then
+		# Testing previously given adress.
+		PROBE=`cat /data/old_adress.txt`
+	else
+		#generating random odd IP address
+		PROBE=`/bin/random_ip`
+	fi
+	/bin/random_ip > /data/old_adress.txt
+else
+	echo $PROBE > /data/old_adress.txt
+	OK=1
+fi
+
+done
+
+#Configuring DHCP server.
+echo "Using address $BASE_ADRESS$PROBE"
+echo "start $BASE_ADRESS`expr $PROBE + 1`" > /tmp/udhcpd.conf
+echo "end $BASE_ADRESS`expr $PROBE + 4`" >> /tmp/udhcpd.conf
+echo "interface $NETIF" >> /tmp/udhcpd.conf
+echo "decline_time 1" >> /tmp/udhcpd.conf
+echo "conflict_time 1" >> /tmp/udhcpd.conf
+echo "opt router  $BASE_ADRESS$PROBE" >> /tmp/udhcpd.conf
+echo "opt subnet 255.255.255.0" >> /tmp/udhcpd.conf
+echo "opt lease  1200" >> /tmp/udhcpd.conf
+
+/bin/pairing_setup.sh
+
+# Saving random info for initialization at next reboot
+echo $MAC_ADDR `date` `/bin/random_mac` > /dev/urandom
+/bin/random_mac > /data/random_mac.txt
+
+
+
+telnetd -l /bin/sh
+
+# Check if not booting in master mode
+if [ "$WIFI_MODE" != "managed" ]
+then
+	udhcpd /tmp/udhcpd.conf
+fi
+
+# Adding route for multicast-packet
+route add -net 224.0.0.0 netmask 240.0.0.0 dev $NETIF
+
+# Allow reconnection to dirty TCP ports
+echo "1" > /proc/sys/net/ipv4/tcp_tw_reuse 
+echo "1" > /proc/sys/net/ipv4/tcp_tw_recycle 
+
+# Starting the daemon which responds to the AUTH messages from FreeFlight if not in master mode
+if [ "$WIFI_MODE" != "managed" ]
+then
+	/bin/parrotauthdaemon
+fi
+