Close

Quadcopter Rassberry Pi 2 Autopilot

stephane-carronStephane Carron wrote 11/05/2015 at 21:41 • • 10 min read • Like

RPI2 configuration.


1.0 Image Installation

  1. sudo apt-get update

1.1 Basic RPI2 Settings

Run raspi-config

!Note: See THIS thread at Emlid.com if any issues like “AK8963: bad DEVICE ID” when overclocking,


1.1 RPI power source

We need to attach more power to the RPI2 USB, using a 5v 5A BEC. This will supply our 4G modem with enough power.
All onboard equipment are powered from the same battery source.

2.0 gStreamer-1.0 Installation


I recently manage to compile gstreamer v1.4. See main discussion thread Here

Version 1.4 are not required for this tutorial, so we just use v1.0 for the sake of convenience.

Type this line to install gstreamer1.0

  1. sudo apt-get install gstreamer1.0

Save attached(in bottom of this article) gstreamer.sh file to RPI or copy paste content bellow.
NOTE! Change the IP to match your GCS ip or dyndns. You may also try with different framerate and bitrate. Try to find a setting you are satisfied with.

  1. #!/bin/bash
  2. #320x240
  3. #360x288
  4. #640x480
  5. #704x576
  6.  
  7. WIDTH=640
  8. HEIGHT=480
  9. ip=GCS_IP
  10.  
  11. v4l2-ctl --set-fmt-video=width=$WIDTH,height=$HEIGHT,pixelformat=1
  12. v4l2-ctl -i 1
  13. gst-launch-1.0 -v \
  14.     rtpbin name=rtpbin v4l2src device=/dev/video0 \
  15. ! video/x-raw,width=$WIDTH,height=$HEIGHT,framerate=15/1 \
  16. ! queue \
  17. ! omxh264enc target-bitrate=500000 control-rate=1 \
  18. ! "video/x-h264,profile=high" \
  19. ! h264parse \
  20. ! queue max-size-bytes=10000000 \
  21. ! rtph264pay pt=96 config-interval=1 \
  22. ! rtpbin.send_rtp_sink_0 rtpbin.send_rtp_src_0 \
  23. ! udpsink port=5000 host=$ip ts-offset=0 name=vrtpsink rtpbin.send_rtcp_src_0 \
  24. ! udpsink port=5001 host=$ip sync=false async=false name=vrtcpsink udpsrc port=5000 name=vrtpsrc \
  25. ! rtpbin.recv_rtcp_sink_0



Remember to set permission to file, 

  1. chmod 755 gstreamer.sh



To run gstreamer script, type this;

  1. sudo sh gstreamer.sh
Client. Windows or unix.

Get the gstreamer installation package for windows HERE
Install the package and then navigate to the installation bin directory. e.g gstreamer\1.0\x86\bin
Then open windows command prompt in that directory. left shift + right click. "Open command line here".



Then type this syntax to start gstreamer to listen for any video stream at port 5000.
If your RPI are streaming, you should get a picture right away.

  1. gst-launch-1.0 -v udpsrc port="5000" caps="application/x-rtp,media=(string)video,
  2. clock-rate=(int)90000,encoding-name=(string)H264,payload=(int)96,ssrc=(uint)237526004,
  3. clock-base=(uint)1584170994,seqnum-base=(uint)42626" ! rtph264depay ! decodebin!
  4. autovideosink -v



    VLC
    If you don`t have the VLC installed, you can get it HERE
    Save the attached(In the Bottom of this article) vlc.sdp and run it.

3.0 UQMI 4G integration
    UQMI (Supports 4G LTE)  Read discussion thread HERE
    Tested and working on RPI 2.Install required dependencies:
    1. sudo apt-get update
    2. sudo apt-get upgrade
    3. sudo apt-get install usb-modeswitch
    4. sudo apt-get install cmake
    5. sudo apt-get install dh-autoreconf
    6. sudo apt-get install dnsutils

    DIR /home/pi*** installing json-c ***
    1. wget  https://s3.amazonaws.com/json-c_releases/releases/json-c-0.12.tar.gz
    2. sudo tar -xvf json-c-0.12.tar.gz
    3. cd json-c-0.12
    4. sed -i s/-Werror// Makefile.in   && ./configure --prefix=/usr --disable-static  && make -j1
    5. su
    6. make install
    7. cd ..
    *** Installing libubox ****
    1. git clone https://github.com/UAVmatrix/libubox.git libubox
    2. cd libubox
    3. cmake CMakeLists.txt -DBUILD_LUA=OFF
    4. make
    5. sudo make install
    6. mkdir -p /usr/include/libubox
    7. cp *.h /usr/include/libubox
    8. cp libubox.so /usr/lib
    9. cp libblobmsg_json.so /usr/lib
    10. ldconfig
    Then git, compile and install uqmi
    1. cd /home/pi
    2. sudo git clone git://nbd.name/uqmi.git
    3. cd uqmi
    4. sudo cmake CMakeLists.txt
    5. sudo make install

3.1 Autoconnection of LTE if loss of signal.  (UPDATE 13.05.2015)

      This section will explain autoconnection of LTE 4G if loss of signal. We need to create crontab job that checks every 15sec if RPI is online, and if not reconnect.
      We also needs to change the way APM streams UDP packets. Normally we just type UDP:GCS_IP:port, but this wont work after wwan0 ( LTE USB Modem) is re-iniziated. we need to stream our packets to localhost and then use udp_redirect script to forward packets to GCS.

      First we are going to create "checkifonline" script.
      1. sudo vi checkifonline.sh
      Then copy and paste code bellow.
      This script will check if our RPI is online and if not, re-connect our LTE modem
      Change the green fields to match your setting.
       
      1. #!/bin/bash
      2. #Create lock file
      3. LOCKFILE=/tmp/lock.txt
      4. if [ -e ${LOCKFILE} ] && kill -0 `cat ${LOCKFILE}`; then
      5.     echo "This script is already running"
      6. exit
      7. fi
      8.  
      9. # make sure the lockfile is removed when we exit and then claim it
      10. trap "rm -f ${LOCKFILE}; exit" INT TERM EXIT
      11. echo $$ > ${LOCKFILE}
      12. #let`s ping google.
      13. if ! wget -q --tries=10 --timeout=20 --spider http://google.com
      14. then
      15.             sleep 1
      16. #let`s ping google again to doublecheck that RPI is offline.
      17. if ! wget -q --tries=10 --timeout=20 --spider http://google.com
      18. then
      19.                     sudo killall -9 uqmi
      20.                     sudo killall -9 dhclient
      21.                     sudo uqmi -d /dev/cdc-wdm0 --set-device-operating-mode shutting_down
      22.                     sleep 30
      23.                     sudo uqmi -d /dev/cdc-wdm0 --stop-network 4294967295 --autoconnect
      24.                     sleep 2
      25.                     sudo uqmi -d /dev/cdc-wdm0 --network-register
      26.                     sleep 2
      27.                     sudo uqmi -s -d /dev/cdc-wdm0 --start-network YOUR_APN --keep-client-id wds --autoconnect &
      28.                     echo "Restartet Modem"
      29.                     sleep 8
      30.                     sudo dhclient -v wwan0
      31.                     sleep 5
      32. else
      33.                         echo "RPI Online"
      34. fi
      35. else
      36.         echo "RPI Online"
      37. fi
      38.  
      39. #remove LockFile
      40. rm -f ${LOCKFILE}
      41.  
      Remember to set permission
      1. sudo chmod 755 checkifonline.sh
      Let`s add this script to crontab
      1. sudo crontab -e
      2.  
      add these line to the bottom of the file
      1. * * * * * /home/pi/./checkifonline.sh &
      2. * * * * * sleep 15; /home/pi/./checkifonline.sh &
      3. * * * * * sleep 30; /home/pi/./checkifonline.sh &
      4. * * * * * sleep 45; /home/pi/./checkifonline.sh &
      This will run our checkifonline script every 15sec.
      next we need to create udp_redirect program.
      Make another script
       
      1. sudo vi udp_redirect.c
      Copy paste the code bellow.
      1. #include <stdio.h>
      2. #include <stdlib.h>
      3. #include <sys/socket.h>
      4. #include <netinet/in.h>
      5.  
      6. int main(int argc, char *argv[]) {
      7. if (argc!=3 && argc!=5) {
      8.         printf("Usage: %s our-ip our-port send-to-ip send-to-port\n",argv[0]);
      9.         printf("Usage: %s our-ip our-port             # echo mode\n",argv[0]);
      10. exit(1);
      11. }
      12.  
      13. int os=socket(PF_INET,SOCK_DGRAM,IPPROTO_IP);
      14.  
      15. struct sockaddr_in a;
      16.     a.sin_family=AF_INET;
      17.     a.sin_addr.s_addr=inet_addr(argv[1]); a.sin_port=htons(atoi(argv[2]));
      18. if(bind(os,(struct sockaddr *)&a,sizeof(a)) == -1) {
      19.         printf("Can't bind our address (%s:%s)\n", argv[1], argv[2]);
      20. exit(1); }
      21.  
      22. if(argc==5) { a.sin_addr.s_addr=inet_addr(argv[3]); a.sin_port=htons(atoi(argv[4])); }
      23.  
      24. struct sockaddr_in sa;
      25. struct sockaddr_in da; da.sin_addr.s_addr=0;
      26. while(1) {
      27. char buf[65535];
      28. int sn=sizeof(sa);
      29. int n=recvfrom(os,buf,sizeof(buf),0,(struct sockaddr *)&sa,&sn);
      30. if(n<=0) continue;
      31.  
      32. if(argc==3) { sendto(os,buf,n,0,(struct sockaddr *)&sa,sn);
      33. } else if(sa.sin_addr.s_addr==a.sin_addr.s_addr && sa.sin_port==a.sin_port) {
      34. if(da.sin_addr.s_addr) sendto(os,buf,n,0,(struct sockaddr *)&da,sizeof(da));
      35. } else {
      36.             sendto(os,buf,n,0,(struct sockaddr *)&a,sizeof(a));
      37.             da=sa;
      38. }
      39. }
      40. }
      Set permission to file
      1. sudo chmod 755 udp_redirect.c
      Then we compile the file we just created.
      1. sudo gcc -o udp_redirect udp_redirect.c
      Now you should have a file named udp_redirect in /home/pi 
      Finally we are going to make udp-send.sh script. This scripft will find GCS ip address and forward UDP trafic from 127.0.0.1( localhost) to your GCS.
      1. sudo vi udp-send.sh
      Copy paste the code bellow 
      1. #!/bin/bash
      2. ip=`dig +short YOUR_GCS_DNS`
      3. echo $ip
      4. /home/pi/udp_redirect 0.0.0.0 14550 $ip 14550 &
      Note! - Remeber to change the APM start line in ArduPilot.sh as described in section 4.0 From:
      1. sudo ArduPlane -A udp:$ip:14550
      To
      1. sudo ArduPlane -A udp:127.0.0.1:14550
       4.0 APM installation. 
       All documentation is provided at Emlid.com, i would recommend to see theirs instructions.
      After successfully installed APM,  make another script which just fires ArduPilot software.sudo vi Ardupilot.sh
      Copy paste code bellow. Change code to mach you airframe and port
      !NOTE, If you are going to use Autoconnect features as described in section 3.1, you need to change the APM start line to sudo ArduPlane -A udp:127.0.0.1:14550
      1. #!/bin/bash
      2. ip=`dig +short YOUR_DynDns_ADRESS`
      3. sudo ArduPlane -A udp:$ip:14550
      Set permission to file
      1. sudo chmod 755 ArduPilot.sh
      Now test that ArduPilot is working properly.
      1. sudo sh ArduPilot.sh
      5.0 RPI startup and usage of installed code.Now you should have two files located in /home/pi  ( gstreamer.sh, ArduPilot.sh)
      I have made a startup script which calls all the necessary programs in correct order.
      ( Also see attched runCode.sh in the bottom)sudo vi runCode.sh
      Copy paste code bellow.
      Change the yellow fields to mach yours. You will need to contact your operator to get correct APN name.
      Telenor, Norway using;   internet.public
      1. #!/bin/bash
      2. function CheckOnline {
      3. SERVICE='uqmi'
      4. if ps ax | grep -v grep | grep $SERVICE > /dev/null
      5. then
      6. echo "$SERVICE service running, everything is fine"
      7. wget -q --tries=10 --timeout=20 --spider http://google.com
      8. if [[ $? -eq 0 ]]; then
      9.    echo "Online, starting APM"
      10. ArduPilot
      11.    sleep 1
      12. else
      13.   echo "4G Offline"
      14. fi
      15.  
      16. else
      17.     echo "$SERVICE is not running"
      18.     echo "Check if RPI is online anyway"
      19. wget -q --tries=10 --timeout=20 --spider http://google.com
      20. if [[ $? -eq 0 ]]; then
      21.     echo "RPI is Online, starting APM"
      22. ArduPilot
      23.     sleep 1
      24. else
      25.     echo "RPI Offline"
      26.     uqmi
      27. fi
      28. fi
      29. }
      30. function gstreamer {
      31. pidof gst-launch-1.0 >/dev/null
      32. if [[ $? -ne 0 ]] ; then
      33.   sudo sh /home/pi/gstreamer.sh &
      34.   sleep 5
      35.   pidof gst-launch-1.0 >/dev/null
      36. if [[ $? -eq 0 ]] ; then
      37.      echo "gStremer Started"
      38. else
      39.    echo 'Could`t start gStreamer'
      40. fi
      41. else
      42. echo "gStremer already running"
      43. fi
      44. }
      45. function uqmi {
      46. sudo uqmi -d /dev/cdc-wdm0 --stop-network 4294967295 --autoconnect
      47. sleep 2
      48. if ! sudo uqmi -s -d /dev/cdc-wdm0 --get-data-status | grep '"connected"' > /dev/null; then
      49. sudo uqmi -d /dev/cdc-wdm0 --stop-network 4294967295 --autoconnect
      50. sleep 2
      51. sudo uqmi -d /dev/cdc-wdm0 --network-register
      52. echo network register
      53. sleep 3
      54. echo Connecting 4G
      55. sudo uqmi -s -d /dev/cdc-wdm0 --start-network YOUR_APN_NAME --keep-client-id wds --autoconnect &
      56. sleep 15
      57. if ! sudo uqmi -s -d /dev/cdc-wdm0 --get-data-status | grep '"connected"' > /dev/null; then
      58.    echo "Not Connected!"
      59. else
      60.    echo "Connected"
      61.    sudo dhclient -v wwan0
      62.    sleep 3
      63. CheckOnline
      64. fi
      65. else
      66.   echo "Already connected to internet."
      67.   echo "Check if gStreamer or APM is running, if not we start them."
      68. CheckOnline
      69. fi
      70. }
      71. function ArduPilot {
      72. pidof ArduPlane >/dev/null
      73. if [[ $? -ne 0 ]] ; then
      74.   nohup sudo sh /home/pi/ArduPilot.sh > /dev/null 2>&1 &
      75.   sleep 5
      76.   pidof ArduPlane >/dev/null
      77. if [[ $? -eq 0 ]] ; then
      78.    echo "APM started"
      79.    gstreamer
      80. else
      81.    echo 'could`t start ArduPilot'
      82. fi
      83. else
      84.   echo "APM already running"
      85.   echo "Starting gStreamer"
      86.   gstreamer
      87. fi
      88. }
      89. sleep 25
      90. CheckOnline
      91.  
      Set Permission
      1. chmod 755 runCode.sh

      This script is not the most glorious one ( made in a hurry) but it should give you an idea on how to set everything up.
      You are free to make a more robust version with further functionalities.

      5.1 Add runCode.sh to run a startupwe need to add runCode.sh to /etc/rc.local sudo vi /etc/rc.local
      add this line to the bottom before exit 0
      1. sudo /home/pi/./runCode.sh &
--------------------------------
This script is not the most glorious one ( made in a hurry) but it should give you an idea on how to set everything up.
You are free to make a more robust version with further functionalities.

        5.1 Add runCode.sh to run a startupwe need to add runCode.sh to /etc/rc.local sudo vi /etc/rc.local
        add this line to the bottom before exit 0
        1. sudo /home/pi/./runCode.sh &
        Reboot RPI and verify that 4G, APM code, gStreamer is running.
        At this stage you should be able to connect 4G cellular network and stream telemetry from Navio to Mission planner and 
        simultaneously have video stream provided by gstreamer to your GCS.

        Now you can connect your gamepad and select Joystick tab in mission planner. Set desired function to each of the buttons and joysticks.
        This is how i have configured my Gamepad. 

        FailsafeFew thing to remember before flight.
        My failsafe function is set as bellow;
        • GCS failsafe in Mission planner.  When flying while using telemetry on the GCS, the autopilot can be programmed to trigger into failsafe mode if it loses telemetry. In the event that the autopilot stops receiving MAVlink (telemetry protocol) heartbeat messages for more than 20 sec, the GCS failsafe (FS_GCS_ENABL, 0=Disabled, 1=Enabled) will trigger the autopilot to go into long failsafe and change the flight mode to RTL
        • Set Long failsafe in mission planner. The action to take on a long (20 second) failsafe event in AUTO, GUIDED or LOITER modes. A long failsafe event in stabilization modes will always cause an RTL (ReturnToLaunch). In AUTO modes you can choose whether it will RTL or continue with the mission. If FS_LONG_ACTN is 0 then it will continue with the mission, if it is 1 then it will enter RTL mode. Note that if FS_SHORT_ACTN is 1, then the aircraft will enter CIRCLE mode after 1.5 seconds of failsafe, and will always enter RTL after 20 seconds of failsafe, regardless of the FS_LONG_ACTN setting.
        Read all about failsafe from Ardupilot 


        Happy and SAFE Flying! smiley
Like

Discussions

Going up?

About Us Contact Hackaday.io Give Feedback Terms of Use Privacy Policy Hackaday API

© 2024 Hackaday

Yes, delete it Cancel