Handheld Camera Stabilizer

Introduction

This is a guide for creating a 3-axis handheld camera stabilization rig for a GoPro using a Digilent Zybo Zynq-7000 Development Board. This project was developed for CPE Real-Time Operating Systems class (CPE 439). The stabilizer uses three servos and an IMU to correct the motion of the user to keep the camera level.

Parts Required for Project

• Digilent Zybo Zynq-7000 Development Board
• Sparkfun IMU Breakout - MPU 9250
• 2 HiTec HS-5485HB Servos (buy 180 degree motion or program from 90 to 180 degrees)
• 1 HiTec HS-5685MH Servo (buy 180 degree motion or program from 90 to 180 degrees)
• 2 Standard Servo Brackets
• 1 Breadboard
• 15 male-to-male jumper wires
• 4 male-to-female jumper wires
• Hot Glue
• Grip or Handle
• 5 mm diameter wooden dowel
• GoPro or other camera and mounting hardware
• Power supply capable of outputting 5V.
• Access to 3D Printer

Step 1: Vivado Hardware Setup

Let's get started with creating the underlying block design for the project.

1. Open up Vivado 2016.2, click the "Create New Project" icon, and click "Next>".
2. Name your project and click "Next >".
3. Choose the RTL project and hit "Next >".
4. Type into the search bar xc7z010clg400-1 and then select the part and hit "Next >" and "Finish".

Step 2: Setting Up Block Design

Now we will start to generate the block design by adding and setting up the Zynq IP Block.

1. On the left hand panel, under IP Integrator, Click "Create Block Design" and then click "OK".
2. Right click in the "Diagram" tab and choose "Add IP...".
3. Type "ZYNQ7 Processing System" and click the selection.
4. Double click on the Zynq block that appears.
5. Click "Import XPS Settings" and import the provided "ZYBO_zynq_def.xml" file.
6. Go to "MIO Configuration" and select "Application Processor Unit" and the enable Timer 0 and Watchdog timers.
7. In the same tab, under "I/O Peripherals", select ENET 0 (and change dropdown menu to "MIO 16 .. 27", USB 0, SD 0, UART 1, I2C 0.
8. Under "GPIO", check GPIO MIO, ENET Reset, USB Reset, and I2C Reset.
9. Now navigate to "Clock Configuration". Select FCLK_CLK0 under PL Fabric Clocks. Then, Click "OK".

Step 3: Create Custom PWM IP Block

This IP block allows the board to send a PWM signal out to control the movement of the servos. The work was heavily based off the tutorial by Digitronix Nepal, found here. Logic was added to slow down the clock so the pulse was outputting at the correct rate. The block takes a number from 0 to 180 and converts it into a pulse from 750-2150 usec.

1. Now, under the Tools tab near the top left-hand corner, click "Create and Package IP..." and hit Next.
2. Then select "Create a new AXI4 peripheral" and hit Next.
3. Name your PWM IP block (we named it pwm_core) and click Next and then click Next on the next page as well.
4. Now click "Edit IP" and hit Finish. This will open up a new window to edit the pwm block.
5. In the "Sources" tab and under "Design Sources", expand 'pwm_core_v1_0' (replace pwm_core with your name) and open the file that becomes visible.
6. Copy and paste the code provided under 'pwm_core_v1_0_S00_AXI.v' in the zip file at the bottom of the project. Ctrl + Shift + R and replace 'pwm_core' with your name for the ip block.
7. Next open 'name_v1_0' and copy in the provided code in the file 'pwm_core_v1_0.v'. Ctrl + Shift + R and replace 'pwm_core' with name.
8. Now navigate to 'Package IP - name' tab and select "Customization Parameters".
9. In this tab there will be a yellow bar at the top that has linked text. Select this, and "Hidden Parameters" will show up in the box.
10. Now go to "Customization GUI" and right click on Pwm Counter Max select "Edit Parameter...".
11. Check the "Visible in Customization GUI" and "Specify Range" boxes.
12. Change the "Type:" drop down menu to Range of integers and set minimum to 0 and maximum to 65535 and check the "Show Range" box. Now click OK.
13. Drag Pwm Counter Max under the 'Page 0' Tree. Now go to "Review and Package" and click the "Re-Package IP" button.

Step 4: Add PWM IP Block to Design

We will be adding the IP block into the block design to allow the user access to the PWM IP block through the processor.

1. Right click in the diagram tab and click "IP Settings...". Navigate to the "Repository Manager" tab.
2. Click the green plus button and select it. Now find ip_repo in the File Manager and add that to the project. Then Hit Apply and then OK.
3. Right click in the diagram tab and click "Add IP...". Type in your PWM IP block name and select it.
4. There should be a green bar at the top of screen, first select "Run Connection Automation" and click OK. Then click "Run Block Automation" and click OK.
5. Double click on the PWM block and change Pwm Counter Max to 1024 from 128.
6. Hover your mouse pointer over PWM0 on the PWM block. There should be a small pencil that shows up when you do. Right click and select "Create Port..." and click OK when a window opens. This creates an external port for the signal to be passed to.
7. Repeat step 6 for PWM1 and PWM2 as well.
8. Find the small circular double arrow icon on the sidebar and click that. It will regenerate layout and your block design should look like the picture above.

Step 5: Configure HDL Wrapper and Set Up Constraints File

We are now going to generate the High Level Design for our Block Design and then map PWM0, PWM1, and PWM2 to Pmod pins on the Zybo board.

1. Go to the "Sources" tab. Right click your block design file under "Design Sources" and click "Create HDL Wrapper...". Select "Copy generated wrapper to allow user edits" and click OK. This generates the High Level Design for the Block Design we created.
2. The Pmod we will be outputting to is JE.
3. Under File, select "Add Sources..." and select "Add or create constraints" and click Next.
4. Click add files and select the included "ZYBO_Master.xdc" file. If you look in this file you will notice everything is uncommented except for six "set_property" lines under "##Pmod Header JE". You will notice that PWM0, PWM1, and PWM2 are the arguments for these lines. They map to Pin 1, Pin 2, and Pin 3 of the JE Pmod.

Step 6: Generating Bitstream

We need to generate the bitstream for the hardware design to export to the SDK before we move on.

1. Under "Program and Debug" on the sidebar, select "Generate Bitstream". This will run synthesis, then implementation, and then generate the bitstream for the design.
2. Correct any errors that pop up, but warnings generally can be ignored.
3. Go to File->Launch SDK and click OK. This will open up the Xilinx SDK.

Step 7: Setting Up Project in SDK

This part can be a little frustrating. When in doubt, make a new BSP and replace the old one. This saved us a bunch of debugging time.

1. Start by downloading the latest version of FreeRTOS here.
2. Extract everything from the download and import FreeRTOS into the SDK by Clicking File->Import, and under "General" click "Existing Projects Into Workspace" then click Next.
3. Go to "FreeRTOS/Demo/CORTEX_A9_Zynq_ZC702" within the FreeRTOS folder. Only import "RTOSDemo" from this location.
4. Now generate a Board Support Package (BSP) by clicking File->New Board Support Package.
5. Select "ps7_cortexa9_0" and check "lwip141" and click OK.
6. Right click on RTOSDemo blue folder and select "Project References".
7. Uncheck "RTOSDemo_bsp" and check the new BSP that we just created.

Step 8: FreeRTOS Code Modifications

The code we provide can be separated into 7 different files. main.c, iic_main_thread.c, xil_printfloat.c, xil_printfloat.h, IIC_funcs.c, IIC_funcs.h and iic_imu.h. The code in iic_main_thread.c has been adapted from Kris Winer's library, which can be found here. We mainly transformed his code to incorporate tasks and make it work with the Zybo board. We also added functions for calculating the correction of orientation of the camera. We have left in several print statements that are useful for debugging. Most of them are commented out but if you feel the need to you can uncomment them.

1. The easiest way to modify the main.c file is to replace the code with copied code from our included main.c file.
2. To create a new file, right click on the src folder under RTOSDemo and select C Source File. Name this file "iic_main_thread.c".
3. Copy code from the included "iic_main_thread.c" and paste it in your newly created file.
4. Repeat steps 2 and 3 with the remaining files.
5. requires a linking instruction in gcc. To add this to the build path you right click on RTOSDemo and select "C/C++ Build Settings".
6. A new window will open. Navigate to ARM v7 gcc linker->Libraries. Select the small add file in the top right corner and type in "m". This will include the math library in the project.
7. Build project with Ctrl + B to confirm everything works. Check the warnings that are generated but you may be able to ignore them.
8. There are a couple places that will needed modification, mainly the magnetic declination of your current location. We will explain how to change this in the calibration part of the tutorial.

Step 9: 3D Printing for Stabilizer

You need to 3D print a couple parts for this project. One can probably purchase parts that are of similar dimensions/sizes to our printed parts.

1. Use the files provided to print out the arm and holding bracket for the GoPro.
2. You need to add scaffolding to the .stl file.
3. Trim/clean parts of excess scaffolding once printed.
4. You can replace the wooden dowel with a 3D printed part if you desired to.

Step 10: Assembling the Parts

The are several parts to assembling the stabilizer. The purchased brackets come with 4 self-tapping screws and 4 bolts with nuts. Since there are 3 servos, one of the servo horns needs to be pre-tapped to allow 2 of the bolts to fit through.

1. Solder 8 pins onto the IMU breakout, 4 on each side.
2. The IMU is attached to the 3D printed holding bracket for the GoPro in the center of the bracket.
3. Orient the bracket so the servo mounting holes are on your left hand side. Place the IMU on the closest edge to you, with the pins hanging off the edge. Then, place the GoPro mount on top of the IMU, sticking the IMU and the mount in place on the bracket.
4. Attach an HS-5485HB to the servo bracket that is integrated into the 3D printed arm.
5. Screw the GoPro bracket into the arm attached servo, making sure that the servo is set so that it is in the middle of its motion range.
6. Next, attach the HS-5685MH servo to a servo bracket. Then tap the servo horn with one of the screws. Now attach the servo to the bottom of the last servo bracket.
7. Now attach the last servo to the the bracket that the HS-5685MH servo is screwed into. Then screw the arm into this servo, making sure that the arm is screwed on so it can move 90 degrees each way.
8. To finish the construction of the gimbal, add a small piece of the wooden dowel to connect between the GoPro bracket and the 3D printed arm. You have now assembled the stabilizer.
9. Lastly, you can add a handle connected to the bottom servo bracket.

Step 11: Connecting Zybo to Stabilizer

There are a couple things to be careful of when doing this. You want to make sure that the 5V from the power supply never goes into the Zybo board, as this would lead to problems with the board. Make sure to double check your jumpers to confirm no wires are being switched.

1. To attach the Zybo to the stabilizer you will need 15 male to male jumpers and 4 male to female jumpers.
2. First, connect two jumpers to your 5V power supply along the + and - rails of the breadboard. These will supply the power to the servos.
3. Then, connect 3 pairs of jumpers to the + and - rails of the breadboard. These will be the power for each of the servos.
4. Plug the other end of the + and - jumpers into each of the servos.
5. Connect a jumper between the - rail of the breadboard and one of the GND pins on the Zybo JE Pmod (See Step 5 picture). This will create a common ground between the Zybo board and the power supply.
6. Next connect a signal wire to pin 1, pin 2, and pin 3 of the JE Pmod. Pin 1 maps to the bottom servo, pin 2 maps to the servo on the end of the arm, and pin 3 maps to the middle servo.
7. Plug the 4 female wires into the GND, VDD, SDA and SCL pins of the IMU breakout. GND and VDD plug into the GND and 3V3 on the JF pins. Plug in SDA pin into pin 8 and SCL into pin 7 on the JF (See Step 5 picture).
8. Lastly, connect the computer to the board using a micro usb cable. This will allow uart communication and allow you to program the Zybo board.

Step 12: True North Correction

The calibration of the magnetometer in the IMU is important to the correct operation of the device.The magnetic declination, which corrects magnetic north to true north.

1. To correct the difference from magnetic and true north, you need to use a combination of two services, Google Maps and NOAA's magnetic field calculator.
2. Use Google Maps to find your latitude and longitude of your current location.
3. Take your current longitude and latitude and plug it into the magnetic field calculator.
4. What is returned is the magnetic declination. Plug in this calculation into the code on line 378 of "iic_main_thread.c". If your declination is east, then subtract from the yaw value, if west then add to the yaw value.

*photo was taken from Sparkfun's MPU 9250 hookup guide, found here.

Step 13: Running the Program

The moment you have been waiting for! The best part of the project is seeing it working. One problem we have noticed is that there is drift from the values reported from the IMU. A low-pass filter may help correct this drift, and fiddling with the magnetometer, acceleration and gyro calibrations will also help correct this drift.

1. First, build all in the SDK, this can be done by pressing Ctrl + B.
2. Make sure the power supply is on and set to 5V. Double check that all the wires are going to their correct places.
3. Then, to run the program, press the green triangle in the upper center of the taskbar.
4. When the program runs, the servos will all reset to their 0 positions, so be ready for the rig to move. Once the program initializes, the servos will then snap back to their 90 degree positions.
5. A magnetometer calibration function will run and directions will be printed out to the UART terminal, which you can connect to via a serial monitor such as 'putty' or the serial monitor provided in the SDK.
6. The calibration will have you move the device in a figure 8 for around 10 seconds. You can remove this step by commenting out line 273 of "iic_main_thread.c". If you comment it out you need to uncomment lines 323 - 325 "iic_main_thread.c". These values were initially gathered from the magnetometer calibration above and then plugged in as values.
7. After the calibration the stabilization code will initialize and the device will keep the camera stable.