Skip to content

PRGWhippet

nitinjsanket edited this page May 10, 2020 · 23 revisions

Hardware and Software Setup for the PRGWhippet

Table of Contents

Notes about Whippet dog

1. PRGWhippet Configuration

PRGWhippet comes in the following configurations (Click on the name for hardware and software setup).
Config Autopilot Software
PRGWhippetAlpha Ardupilot

2. What Is PRGWhippetAlpha?

PRGWhippetAlpha is a 100-160 size nano quadrotor built from scratch using the Ardupilot based flight controllers for indoor autonomy using on-board sensing and computation.

2.1. Bill Of Materials

Item Qty Price/Item (USD) Cost (USD) Sourcing Link(s)
T-Motor F20-II 3750 KV Motors 4 18 72 Link
Lumenier 35A 4In1 ESC 1 60 60 Link
Hobbypower PX4Flow v1.3.1 1 62 62 Link
TFMini Lidar Module 1 45 45 Link
Carbon Fiber 200mm x 300mm x 2mm 1 26 26 Link
Tattu 3S Battery 850mAh 1 14.5 14.5 Link
Spektrum DSMX Reciever 1 23 23 Link
Holybro Kakute F7 AIO Flight Controller 1 50 50 Link
Intel Up Core 1 169 169 Link
Pololu 5V 5A Regulator 1 15 15 Link
RayCorp 3030x4 Propeller 1 15 15 Link
Spektrum DXe Transmitter 1 63 63 Link

Note that the above table represents the Bill Of Materials for the best performance quadrotor which are tested, i.e., highest thrust to weight ratio, the best reliable components and best computer that can fit in the maximum size of 160 mm. Some alternate motor and ESC choices are given in the following tables.

Item Qty Price/Item (USD) Cost (USD) Sourcing Link(s) Propeller Pairing
T-Motor F20-II 3750 KV Motors 4 18 72 Link 3030x4
EMAX RS1306B 4000KV Motors 4 11 44 Link 3030x4
T-Motor F15 6000KV Motors 4 15 60 Link 2040x3 2540x3
Lumenier RX1306-7 4000KV Motor 4 15 60 Link 3030x4
Item Qty Price/Item (USD) Cost (USD) Sourcing Link(s)
DYS 4In1 BLHeli_S ESC 1 39 39 Link
Lumenier 35A 4In1 ESC 1 60 60 Link
T-Motor F45A BLHeli_32 4In1 ESC 1 64 64 Link

A cost effective motor + ESC combo will be to use EMAX RS1306B 4000KV Motors with a DYS 4In1 BLHeli_S ESC (noet that this ESC is not BLHeli_32). The smoothest performance will be obtained using any T-Motor motor with the T-Motor 45A ESC.

Also note that, EMAX Bullet Series of ESCs have some problems working with various autopilots and it is highly recommended to avoid them.

2.1.1. Details On Required Bolts, Nuts and Standoffs

Coming Soon.

2.1.2. Tools And Miscellaneous Items Required

2.2. Where Can I Get Water-jet Cutting And 3D Printing Done?

For University of Maryland, College Park students use Terrapin Works service and submit jobs through Papercut server. Others can use an online service such as this and this or use your own 3D printer/water-jet cutter.

2.3. Assembly Instructions

2.3.1. Wiring Instructions

Wiring for the PRGWhippetAlpha is divided into three steps, these three steps help diagnose the hardware problems early.

The first step is the bare minimum to fly a manual quadrotor. This is similar to a racing quadrotor. This step should help obtain a basic tune before trying out any autonomy functionality.

Once the aforementioned connections are made, please follow the instructions in Sections 2.4.1.1 to 2.4.1.3.

Now assuming you have a stable manually flying quadrotor, now it's time to get into some autonomy. First step is to hold altitude using a TFMini Lidar. The TFMini is connected to the KakuteF7AIO using a serial connection as shown below.

Now, go to Section 2.4.1.4 for software setup instructions.

Now, we have AltHold working perfectly, i.e., the vehicle can hold altitude on it's own using the feedback from the TFMini. Next, we want to hold position in full 3D as well. For this we will need to connect the PX4Flow sensor. In our experience, PX4Flow v1.3.1 from Hobbypower worked the best.

Connect the PX4Flow as shown below.

Now, go to Section 2.4.1.6 for software setup instructions.

2.3.2. Hardware Assembly

2.4. Software Setup

2.4.1. ArduPilot Setup

2.4.1.1. Flashing Firmware with Bootloader

Download the arducopter firmware with bootloader arducopter_with_bl.hex from here.

To flash the arducopter firmware with bootloader you need to download the Betaflight configurator chrome app from here. You'll need Google Chrome web browser for this step. If you don't have Google Chrome, please download it from here.

Now, follow the instructions from the offical Kakute F7 AIO Manual to install the drivers for your particular operating system. Though Windows is the hardest to get to work, we suggest using Windows since it had the most stable performance in our experience.

Now, press and hold the DFU button as shown below on the Kakute F7AIO while you plug in the USB to your computer. (If you don't see DFU in Betaflight, please follow this instruction.

Now, open the Betaflight configurator chrome app, it should say DFU on the top right corner as shown below. If this is not the case, follow the previous steps again carefully.

Now, select the correct board KakuteF7, check Full chip erase and click on Load Firmware [Local] on the bottom right corner as shown below (Image Credits: OscarLiang.com). Then select the arducopter_with_bl.hex file we downloaded before.

The click on Flash Firmware once the loading is complete. Wait for the firmware to complete flashing.

If you have done all the previous steps correctly, the flight controller should show a COM port on the top right. The COM port will have different names based on the operating system you are on. Now, proceed to the next step if everything worked else repeat previous steps.

2.4.1.2. Calibration with Arducopter

Download Mission Planner for Windows from here. Note that this is officially supported on Windows for now. However, many people have gotten it to work on Linux, more details about this can be found here.

Now, follow the steps here to calibrate your accelerometer.

Now, follow the steps here to calibrate your RC transmitter after binding with your reciever.

2.4.1.3. Motor Test and first flight

Now, test the motors following the instructions here, if the order is wrong, please verify the motor connections for your ESC (the connections shown in our diagram in Section 2.4.1.1 are for Lumenier 35A 4in1 ESC).

If any of the motors spin in the wrong direction, i.e., opposite to the one shown below, swap any two of the three wires for that particular motor which connect to the ESC.

Once all the motors are in the correct order and they spin in the correct direction, calibrate the ESCs by following the instructions here.

Now, change the following parameters so that you start off with a reasonably stable quadrotor and hopefully will not crash.

Parameter Value Notes
ATC_ANG_RLL_P 12.0 Stabilize Roll (Error to Rate)
ATC_ANG_PIT_P 12.0 Stabilize Pitch (Error to Rate)
ATC_ANG_YAW_P 6.0 Stabilize Yaw (Error to Rate)
ATC_RAT_RLL_P 0.145 Roll Rate Controller P Gain
ATC_RAT_RLL_I 0.145 Roll Rate Controller I Gain
ATC_RAT_RLL_D 0.003 Roll Rate Controller D Gain
ATC_RAT_PIT_P 0.145 Pitch Rate Controller P Gain
ATC_RAT_PIT_I 0.145 Pitch Rate Controller I Gain
ATC_RAT_PIT_D 0.003 Pitch Rate Controller D Gain
ATC_RAT_YAW_P 0.19 Yaw Rate Controller P Gain
ATC_RAT_YAW_I 0.019 Yaw Rate Controller I Gain
ATC_RAT_YAW_D 0 Yaw Rate Controller D Gain
ATC_RAT_RLL_FILT 20 Roll Rate Controller Filter Frequency
ATC_RAT_PIT_FILT 20 Pitch Rate Controller Filter Frequency
ATC_RAT_YAW_FILT 2 Yaw Rate Controller Filter Frequency
ATC_RAT_RLL_IMAX 0.5 Roll Rate Controller Filter I gain maximum
ATC_RAT_PIT_IMAX 0.5 Pitch Rate Controller Filter I gain maximum
ATC_RAT_YAW_IMAX 0.2 Yaw Rate Controller Filter I gain maximum
SCHED_LOOP_RATE 800 Main Control Loop Frequency
INS_GYRO_FILTER 60 Filter Cutoff Frequency for Gyroscopes
EK2_ENABLE 1 Enable EKF2 Computation for State Estimation
AHRS_EKF_TYPE 2 Use EKF2 state estimates for attitude and position control
MAG_ENABLE 0 Disable Magnetometer
EK2_MAG_CAL 2 Disable Magnetometer calibration on the fly
LOG_BITMASK 0 Disable Logging
COMPASS_USE 0 Disable Compass
COMPASS_USE2 0 Disable Compass 2
COMPASS_USE3 0 Disable Compass 3
ARMING_CHECK 0 Disable PreArm Checks
FS_CRASH_CHECK 0 Disable Crash Detection

By default, your quadrotor should follow the Mode 2 convention, i.e., left stick up/down controls throttle, left stick left/right controls Yaw, right stick up/down controls pitch and right stick left/right controls roll. If any of these are flipped, you can flip the channel in your RC Transmitter or by setting RCn_REVERSED flag to 1 in Mission Planner. By default RC1 is set to Roll, RC2 is set to Pitch, RC3 is set to Throttle and RC4 is set to Yaw. Note that RC1 to RC4 doesn't map to reciever 1 though 4 directly, in fact they are reversed in Mode 2, i.e., RC1 -> CH4, RC2 -> CH3, RC3 -> CH2 and RC4 -> CH1 (Mode details can be found here). In short, for default settings, CH1 to CH4 on the trasmitter should control Throttle, Roll, Pitch and Yaw respectively. Make sure that channel directions are correct and not flipped before you fly, i.e., stick up/right should increase and stick down/left should decrease the value. If for some reason you don't like Mode 2 or want custom RC Mappings you can follow the instructions here.

Now, go ahead and fly in your quadrotor in Stabilise mode an open area outdoor or in a netted area indoor. Make sure to fly atleast 1 m over the ground and 1 m under the ceiling to avoid any ground/ceiling effect. If you quadrotor has a lot of oscillation, tune the quadrotor in Acro mode (not stabilize) to dampen out the oscillations. DO THIS STEP BEFORE YOU PROCEED TO ANY AUTONOMY FUNCIONALITY.

If you see mid throttle oscillations and they don't dampen even after excessive tuning of PIDs and/or INS_GYRO_FILTER and/or ATC_RAT_RLL_FILT and/or ATC_RAT_RLL_FILT and/or ATC_RAT_PIT_FILT and/or ATC_RAT_YAW_FILT you might want to soft-mount your motors as described in this video.

2.4.1.4. AltHold Mode

Go back to Section 2.3.1 for the wiring diagram to connect the TFMini Lidar, which we'll use to hold altitude. Set the following parameters to start using TFMini (Use the search bar in Full Parameter List tab in CONFIG\TUNING tab in Mission Planner). Here we connected the TFMini to UART2, please adapt the SERIAL number if you changed the UART port.

Parameter Value Notes
SERIAL2_PROTOCOL 9 Rangefinder
SERIAL2_BAUD 115 115200
RNGFND1_TYPE or RNGFND_TYPE 20 Benewake TFMini
RNGFND1_MIN_CM or RNGFND_MIN_CM 30 Min. dist that rangefinder can reliably read in cm
RNGFND1_MAX_CM or RNGFND_MAX_CM 1000 (indoors) or 600 (outdoors) Max. dist that rangefinder can reliably read in cm
RNGFND1_GNDCLEAR or RNGFND_GNDCLEAR 10 Distance of sensor from ground when vehicle is landed

Restart your flight-controller by power cycling and disconnecting the USB cable. You should see the sonarrange values changing in Status tab of Mission Planner when you move the vehicle/TFMini.

Note that to have quick access to this parameter you can right click on the numbers in the quick access tab in Mission Planner and change the number or rows and columns as shown below.

The double click any icon and check the parameter you want to be displayed there as shown below.

Now, go to Initial Setup > Flight Modes and change one of the modes to AltHold as shown below.

Also, before you fly change a few EKF parameters which is used for state estimation shown below.

Parameter Value Notes
EK2_ALT_SOURCE 1
RNGFND1_MAX_CM or RNGFND_MAX_CM 1000 1000 for indoors and 600 for outdoors
RNGFND1_GNDCLEAR or RNGFND_GNDCLEAR 10 distance of sensor from ground when vehicle is landed
EK2_GPS_TYPE 3 Do not use GPS as we are indoors
EK2_RNG_USE_HGT 70 Use this percentage as range finder measurements as primary height source

Take off in Stabilize mode, once you are about 1 m high and reasonably stable, switch to AltHold mode when the throttle stick is at about 50% (while doing this for the first time, make sure you have enough headroom). Now the quadrotor should hold altitude requiring minimal adjustments in roll, pitch and yaw. You can adjust the ascent/descent rate by moving the throttle stick up/down respectively. If the AltHold mode is very sensitive to throttle mid stick you can increase the dead-band (the larger the value the mode play you have with throttle sitck when AltHold will be active without ascending/descending) by changing the value of THR_DZ in Mission Planner.

Once AltHold is works well, try the autotake off feature in AltHold mode as follows.

Start in AltHold mode and try to take off by moving throttle slowly to about 50% throttle, the quadrotor should slowly takeoff and hold altitude requiring minimal adjustments in roll, pitch and yaw. As again, you can adjust the ascent/descent rate by moving the throttle stick up/down respectively. You can land the quadrotor by moving the throttle stick slowly down.

If your gains are still not perfect, proceed to Section 2.4.1.5. for autotuning your gains. If everything works well, we are can proceed to holding position using the optical flow sensor coupled with the TFMini Lidar. Now, go to Section 2.3.1 for wiring instructions of the PX4Flow sensor.

2.4.1.5. (Optional) Fine-tune PIDs using Autotune

Coming Soon.

2.4.1.6. FlowHold Mode

Unplug the connector that connects your Kakute F7AIO to your PX4Flow sensor and follow the steps below.

The first step is to download a custom firmware which will force enable the PX4Flow to work on the Kakute F7AIO. This bug will probably be fixed on Ardupilot at some point and will not need this step. As of writing this article, you need this step. This fix was suggested in this blog post and comes from the fact that Kakute F7AIO is not defined in ardupilot/libraries/AP_HAL/AP_HAL_boards.h. Download the custom arducopter firmware v3.6.11 with this fix from here. However, you can refer to this blog post and do the necessary changes and finally compile the code following instructions here if you are feeling extra enthusiastic or want a different version.

Now, flash the firmware following these instructions. Note that you can do this directly with Mission Planner as the board already has a bootloader and you don't need to do this through the BetaFlight software again.

Now, we'll prepare our PX4Flow sensor to be used with the arducopter firmware.

  • The first step is to install QGroundControl (QGC) to flash firmware to the PX4Flow sensor. Download and install QGroundControl from here. Though QGC works on multiple operating systems, we found it to work best on Windows.
  • Plug in the USB to the PX4Flow sensor and open QGC, if it doesn't show up then you have to install additional drivers. Download and manually install the PX4Flow Windows Driver from here which may in turn require allowing installing unsigned drivers.
  • Now, QGC should be able to detect your PX4Flow sensor. Download and unzip the PX4Flow-KLT Firmware from here. Note that other firmwares do not work with arducopter stack.
  • Unplug the sensor, click on the Firmware tab and replug the sensor.
  • On the right hand side, click on the firmware version dropdown, and select Custom firmware file. Click OK. Then select the firmware downloaded above. QGC should now flash a firmware compatible with ArduPilot. QGC will now think that the sensor is a Pixhawk. Dont worry. Unplug the USB from the PX4Flow.

Now connect the cable between Kakute F7AIO and PX4Flow.

Remove the props.

Then, connect the USB to the Kakute F7AIO, don't forget to power everything from a battery first as the USB port doesn't provide enough power to power up everything.

If you see any errors you might have to calibrate the accelerometer again as given in Section 2.4.1.2. Also, note that the PX4Flow will not work if it is not getting altitude measurements from the TFMini Lidar.

Now, if you don't see any errors go to Status tab and look for opt_m_x, opt_m_y values as shown below, they should be non zero and should change if you move your vehicle (with the PX4Flow sensor attached) around.

Now, you are ready to fly in FlowHold mode except for two steps. Change the FLOW_ORIENT_YAW parameter to the angle your copter's horizontal axis makes with the X axis on the PX4Flow sensor, if you used our mounting direction, this value should be set to zero. Also, check for FLOW_ADDR, this should be set to 0 if you haven't modified your PX4Flow sensor by soldering the jumpers on the back. More details about th I2C address can be found here.

If you change either of FLOW_ORIENT_YAW or FLOW_ADDR, power cycle your entire system and also unplug and replug your USB to your flight controller. Finally ensure that you are seeing changing values of opt_m_x, opt_m_y in the Status tab before you proceed. This is super important!.

Now, change one of your flight modes in Config/Tuning > Flight Modes to FlowHold. It is highly recommended that you have one mode as Stabilize, other one as AltHold and preferably they are in this order Stabilize > AltHold > FlowHold as you don't want to accidently switch to FlowHold directly from Stabilize mode.

Unplug your quadrotor from the PC, place the quadrotor outdoor or in a netted area indoor. Arm it in Stabilize mode, fly upto a meter high and switch to AltHold mode, the quadrotor should be holding altitude which requires minor correction from the user for keeping it in place. Now, when the quadrotor is almost stable switch it to FlowHold mode, you should have a quadrotor which can hold position on its own.

Viola you just built a quadrotor which can perform position hold autonomously, all on its own without the need of an external motion capture system.

Note that Loiter and PosHold mode should ideally work in a similar way, but currently these modes have bugs and don't work as they should with Kakute F7AIO.

  • Write about FLOW_ENABLE

2.4.2. Up Board Setup

2.4.2.1. Ubuntu 16.04 Installation

The Intel Up Core board does NOT support legacy setting in BIOS. It ONLY has UEFI mode so do not use tools like rufus or PowerISO to create the Ubuntu Bootable disk. You can install Ubuntu with rufus but the bootloader (GRUB) won't be installed.

  • Download Ubuntu 16.04.6 iso file from the Ubuntu download page (link)
  • Create a USB disk (minimum 8GB space) using etcher tool which creates UEFI bootable disk. You can download etcher from here.
  • Install Ubuntu the regular way and everything should work.

Note: In BIOS settings, make sure you have OS type selected as Android/Linux, not Windows.

2.4.2.2. Update Ubuntu Kernel for Up Core

  1. After the reboot you need to add our repository:
sudo add-apt-repository ppa:ubilinux/up
  1. Update the repository list
sudo apt update
  1. Remove all the generic installed kernel
sudo apt-get autoremove --purge 'linux-.*generic'
  1. Install our kernel
sudo apt-get install linux-image-generic-hwe-16.04-upboard
  1. Install the updates (please make sure to not upgrade the system to Ubuntu 18.04)
sudo apt dist-upgrade -y
sudo reboot
  1. Reboot
sudo reboot
  1. After the reboot you can verify that the kernel is indeed installed by typing
$ uname -a
Linux upsquared-UP-APL01 4.15.0-37-generic #40~upboard03-Ubuntu SMP Wed Dec 12 16:21:24 UTC 2018 x86_64 x86_64 x86_64 GNU/Linux

2.4.2.3. Install the Ampak firmware for WiFi and Bluetooth

This firmware is available for UP Core (AP6214A chipset) and UP Core Plus (AP6355 chipset).

Just run the next command

sudo apt install firmware-ampak

Reboot your machine to get the firmware working:

sudo reboot

WiFi First at all, you may need to connect a WiFi antenna to your UP Core board [ANT1 connector]. Check the UP Core connectors in https://downloads.up-community.org/download/up-core-connectors-description/.

Scan your available WiFi networks:

sudo iwlist wlan0 scan

You will see all the WiFi interfaces in your area.

Bluetooth Check your Bluetooth devices in your area:

hcitool scan

For other Kernel level and Networking changes, please follow these instructions.

2.5. Autonomous Trajectory Following