General Software Documentation for P15230: Quadcopter Project

Alyssa Colyette

1

Table of Contents

Table of Contents Overall Navigation System

Global Pathing The Idea The Algorithm A* PoC and Conclusions (thus far)

Object Detection Implementation Pseudocode

Localization Wifi­router Pinging

Concept Results from PoC (thus far)

Belief Location From Executed Commands Facial Recognition Subroutine

Backup Facial Recognition Implementation Pseudocode

Flight Controller API Chip to Chip Communications Protocols Description

Simple Serial I2C Communications

Pi to Arduino Arduino Slave IMU Sensor Modules

PPM (and PWM) Communications Arduino to Flight Controller

LIDAR Communications

2

Overall Navigation System The goal of this project is to create a quadrotor that can navigate autonomously to known positions at which it will search for a face, and snap an image of that face. To accomplish this a master Raspberry Pi will initiate path planning as well as execute movement commands for the craft. In the class diagram below, the objects in yellow are ran on the master Pi. The objects in magenta are ran on a separate slave Pi communicating via serial UART. The other two objects are ran on two separate arduino nanos both communicating as slave devices to the master Pi over I2C communications.

1

Below is the flow diagram of the overall navigation system. The flow of the facial recognition capture system is detailed in a later section (Facial Recognition Subroutine). The explanation of each component is in further detail in the proceeding sections.

1 The numbers by each module refers to the instance relationship of each module sharing a connecting line.

3

Global Pathing

The Idea (focus on 2D coordinates) Takes advantage of prior info:

navigation space/ dimensions target destination location

Risks:

Map Resolution too high for RT computations Mitigations:

reduce resolution create grid for pocketed/local area

project destination point, create sub­destination

I. Generate grid of course space

Initialize a fully connected graph where nodes of uniformly distributed across the area of the navigation space. The edge costs are all the same r (selected resolution for the grid eg r=1m).

4

Diagram II: Area Mapping *The grid depicted in diagram II is a basic grid. The edges between nodes can be between all nodes in relative vicinity, but the edge cost would be larger (√(2r)m).

II. Recursively apply node pathing algorithm (A* search) until destination is reached. A. Apply A* search on current state (pos) from localization module

With a believed position the quadcopter should navigate through the nodes available edges towards its destination with some allowable error.

Diagram III: Path Planned After calculating path, quadcopter would start execute set of controls reflecting that path.

B. Poll for refreshed grid map flag from object detection module, if so update map and go back to part A. New observations can be made by the quadcopter from the object detection

model. In that case the map would be updated. Diagram IV shows detected obstacle that marks the area where 3 nodes are located forbidden for it detected a wide incoming obstacle.Since the depth of obstacle is not know yet, the map is not updated for any other positions.

5

Diagram IV: Path Planned AFTER Map Update

note: backtracking may be a problem with new calculation of shortest path after object detection…

Should always use same start, use current localization to determine location in path.

The Algorithm A* Search pseudo code 1. Mark current block 2. Assign as parent for all adj blocks 3. For each adj block, calculate G, H, and F

G­ distance between current position and start H­ (Manhattan) distance between current and end position F = G+H

4. Choose current block as min(F) block, repeat 1 if H>0 methods for optimizing:

H values can be precalculated. Not all G have to be recalculated...

This is Koenig’s proposed RT Adaptive A* pseudo code 2

S set of states of the search task, a set of states GOAL set of goal states, a set of states A( ) sets of actions, a set of actions for every state succ( ) successor function, a state for every state­action pair variables lookahead number of states to expand at most, an integer larger than zero movements number of actions to execute at most, an integer larger than zero

2Sven Koenig. “Real­Time Adaptive A*”. http://scholar.google.com/citations?view_op=view_citation&hl=en&user=tpoh43QAAAAJ&citation_for_view=tpoh43QAAAAJ:8k81kl­MbHgC

6

scurr current state of the agent, a state [USER] c current action costs, a float for every state­action pair [USER] h current (consistent) heuristics, a float for every state [USER] g g­values, a float for every state [A*] CLOSED closed list of A* (= all expanded states), a set of states [A*] s state that A* was about to expand when it terminated, a state [A*] procedure realtime adaptive astar(): 01 while (scurr 6∈ GOAL) do 02 lookahead := any desired integer greater than zero; 03 astar(); 04 if s = FAILURE then 05 return FAILURE; 06 for all s ∈ CLOSED do 07 h[s] := g[s] + h[s] − g[s]; 08 movements := any desired integer greater than zero; 09 while (scurr 6= s AND movements > 0) do 10 a := the action in A(scurr) on the cost­minimal trajectory from scurr to s; 11 scurr := succ(scurr, a); 12 movements := movements − 1; 13 for any desired number of times (including zero) do 14 increase any desired c[s, a] where s ∈ S and a ∈ A(s); 15 if any increased c[s, a] is on the cost­minimal trajectory from scurr to s then 16 break; 17 return SUCCESS; A* PoC and Conclusions (thus far)

7

The environment and the algorithm for A* search was coded in C++ and ran on the

Raspberry Pi. The basic testing performs two least resistant pathings. Both starting from one corner node (0,0) to the opposite corner node (85,85).

The first path is done with a 85x85 unit area. There are no obstacles therefore every node is linked to its neighbor within the discretized space. A path is delivered of a node transversal along the edges of the grid; from (0,0) to (0,85) to (85,85).

The second path is on the same grid where the edge between nodes (0,0) and (0, 1) (the first node delivered in the 1st path) removed. This forces the pathing to transverse the nodes along the x­direction first instead; from (0,0) to (85,0) to (85,85)

Both of these tests are ran on the Pi with a captured timing by the system clock as: real 0m0.075s, user 0m0.060s, sys 0m0.020s. Though, both paths are represented by a simple transversal, a new path doesn’t take long to calculate for two paths were calculated within 100ms with the .

TODO: Add diagonally accessible neighbors to get optimal path. Could update cost heurist to using only euclidean distances instead of manhattan distances. Build into Adaptive A* Search environment for Flight_Controller_API execution.

8

Object Detection

9

The object detect class relies on messaging from two different systems on different communications protocols. Object_Detect will have to handle some form of object tracking and/or thresholding to determine if a detected obstacle warrants a severed edge connection thus provoking a graph update to the Global Pathing grid. Error Checking:

SonarDetect should relay error in a sensor reading LidarDetect should relay error in an image captured (resolution etc)

Implementation Pseudocode LidarCheck = ⅕ sec /// pi processes ~ 5fps tested SonarCheck= 19­20ms; //takes 10us to initiate command pulse and return pulse as long as 18ms for ~3m detection range //main function main ()

for (;;) sonarPkt = getSonarPkt; for every sensor in pkt

edges +=convertSonarGlobal(pos,dist); if (time_elasped ==TimeLiDAR)

TimeLiDAR = time_elasped+LidarCheck;

10

edges += converLidarGlobal(length,dist) else

sleep(SonarCheck); //lessen load on CPU for edge in edges

removeEdge(edge);

//request and preliminary pkt parsing for SONAR getSonarPkt()

request sonarArduinopkt via i2c for every sensor in sonarArduinopkt

extract pos & distance if distance within resolution

add to sonarPkt return sonarPkt

//request and preliminary pkt parsing for LiDAR getLidarPkt()

request lidarPipkt via UART extract distance & width if within resolution

add to lidarPkt return lidarPkt

11

Localization

12

Wifi-router Pinging Alt: Obtain distance by Freq & Strength of Router

Concept CLI Call: “#iwlist wlan0 scan | egrep ‘Cell|Frequency|Quality|ESSID’ “

Frequency extracted for distance calculation parameters ESSID needed to ID router via MAC ADDRESS Quality extracted for distance calculation parameter

given as a percentage therefore conversion: give x/100 RSSI = x/100 * 60 dbm = RSSI ­92

To calculate distance measurement (meters)

calcdist(leveldBm, freqMHz) dist = 10 (27.55 ­ (20*log10(freqMHz)) + |leveldBm|)/20

The distance would be the calculated relative radial distance from the given known router location. TODO: Will need to be done for a total of 3+ routers to solve for coordinates using Trilateration. Get accurate measurements to compare to calculated measurements via trilateration.

13

Results from PoC (thus far) Distance/signal level does NOT update for multiple calls within the same run of the C program(refer to closeagain100.txt and faragain100.txt). When the program is manually ran multiple times, the values scanned to update upon variance and movement of RPI (refer to testDist.txt). In both cases ( manually and programmatically) the modem’s displacement was ~3ft (not measured for directionallity). In the table below are the max and min measurements retrieved from separate manual runs of the program. Only 10 calculations were done for each position.

position Min Measurement (m)

Max Measurement (m)

Var (m)

1 1.164695 1.337248 0.172553

2 0.670212 0.769509 0.099297

Without accurate real world measurements further analysis needs to be made. Though as a crude analysis, direct proportionality of the measurements and displacement was present. TODO: Measure the execution time for the program when abstracting data and calculating the distance for one router.

Belief Location From Executed Commands If Trilateration fails, or in addition to trilateration. The location the quadrotor believes it is in will be a prediction based on the commands given to the craft from a known start point. Using a Kalman filter, bayesian theory can be applied to predict the location from given commands as well as provide a (increasing) covariance relation that provides the uncertaintly of the crafts predicted location. This would become ever increasing until more observations are provided to the engine to increase its certainty.

14

Facial Recognition Subroutine

15

The facial detection module would activate on run from the main task manager. The TaskManager will have to become a listener for when the facial detection module would find a face. The algorithm for facial detection would be an implementation of the Viola­Jones method. This approach should detect faces within the range of profile and ¾ view.

Link to openCV library reference: http://docs.opencv.org/doc/user_guide/ug_traincascade.html TODO: find training set for classifier

Backup Facial Recognition Since the recognition software will run on the same RPi for LIDAR detection, using the

image processing intensive Viola­Jones method may prove too costly. As a backup plan a IR LED would be used as a beacon to detect face placement. Bayesian theory would be used to determine probability of a face within fram given that the beacon is detected.

Implementation Pseudocode // precon: copter is believed to be at coordinates of target // @brief checks 4 sides within vertical height of 1 ft // Facial Recognition detects up to ¾ view // Needs to communicate with Flight Controller command API (to Nano)

16

// Face_Found flag set in another thread (to IP Pi) searchForFace()

bool image_cap =false; while (!Face Found)

//searches vertical region of expected face location transverseFaceRegVertical(); for vertical_scan_time

if Face_Found send stop; image_cap= takePic()

//~~~~check other side orbit90(); for orbit_quart_scan

if Face_Found send stop; image_cap= takePic()

transverseFaceRegVertical(); for vertical_scan_time

if Face_Found send stop; image_cap= takePic()

//~~~~check other side (180) orbit90(); for orbit_quart_scan

if Face_Found send stop; image_cap= takePic()

transverseFaceRegVertical(); for vertical_scan_time

if Face_Found send stop; image_cap= takePic()

//~~~~check last side orbit90(); for orbit_quart_scan

if Face_Found send stop; image_cap= takePic()

transverseFaceRegVertical(); for vertical_scan_time

if Face_Found send stop; image_cap= takePic();

17

Face_Found = false; return image_cap;

Flight Controller API

The internal PID controller within the flight controller should be tuned for maximum stability to the point of being fairly rigid. It should account for user error (in this case computer error) if bad controls are given to the actuators. Without any given commands the quadrotor would give a static hover state. It should not be prone to tipping (flipping upside down) within the initial phases as the command library is built.

In the starting phase the Flight_Controller_API will have a set of ‘safe to exe at hover’ commands. This would be limited to at least the commands given in the class diagram for a static parameterized distance or degree. Each of this commands will derive the required

18

motor adjustments as well as package the command to be sent to the arduino controller via I2C to the motors.

As for actually controlling the motors, the quadrotor system expects potentiometer inputs read via an RC channel for each motor (refer to Philip’s RC Protocol Document). If the Arduino program functions as an interface for ranging these channel inputs to send directly to the flight controller, then the Flight_Controller_API must act as an automatic RC controller. It will account for controller toggle adjustments for executing the set number of moves. RISKS: May require an outer loop PID controller for executed directions. Many control systems for a quadrotor require two PD controllers to guide inertial and attitude overshoot.

19

Chip to Chip Communications Protocols Description

Simple Serial The (SR04) sonar sensors use a simple asynchronous master­slave implementation. A 10us pulsed ‘trigger’ is sent to each sensor and the returned pulse ‘echo’ed determines the distance measured by the sensor. Since the four sensors aren’t addressed, a trigger and echo pin is dedicated to each module.

/** Sonar 1 / Left */ #define TrigPin1 12; #define EchoPin1 11;

/** Sonar 2 / Right*/ #define TrigPin2 8; #define EchoPin2 10;

/** Sonar 3 / Rear */ #define TrigPin3 7; #define EchoPin3 9;

/** Sonar 4 / Height */ #define TrigPin1 13; #define EchoPin1 6;

The requests for all sensors accommodate a sampling frequency of 12.5 Hz (including time padding) if each one is accessed in isolation. This would also reduce sonar interference between each sensor.

max wait after pulse: 18ms Max serial triggering: ~18.1ms per sensor Max period: 18.1ms x 4 = 72.4ms ­> ~13.8 Hz

I2C Communications

Pi to Arduino There are two slave Arduino microcontrollers connected to the master Pi microprocessor. For accessing the I2C module on each device, the Pi uses the I2C Linux user space driver within a C++ program and Arduino uses its Wire library to become a software enabled I2C slave device. The slave addresses are reconfigurable in software for both Arduino slave devices. The actual addresses used are shown below.

Sonar Arduino Address = 0x04 Fight Arduino Address = 0x08

20

The packet for the Flight Arduino used contains a header (the command’s generic description) and values for the 4 RC channels (throttle, yaw,pitch,roll). The sensor Arduino packet is larger in order to provide all sensor readings within a certain resolution in one request from the master Pi.

typedef struct

uint8_t header; //Command to the microcontroller

uint8_t throttle; //speed

uint8_t yaw; //left & right

uint8_t pitch; //forward and back

uint8_t roll; //

ReqPkt;

typedef enum STOP, FORWARD, BACK, LEFT, RIGHT, UP, DOWN, ORBIT flight_cmd;

typedef struct

uint16_t header; //Command to the microcontroller

uint16_t sonar1;

uint16_t sonar2;

uint16_t sonar3;

uint16_t sonar4;

uint16_t heading; //heading calculated by float on Compass

uint16_t altitude; //altitude is calculated as float on Barometer

SonarReqPkt;

typedef enum ALL, SONAR1, SONAR2, SONAR3, SONAR4, COMPASS, BAROMETER sensor_cmd;

The actual values of the enumeration would not matter for it’s up to each microcontroller to interpret the command.

Pi to IMU Slave Sensor Modules The sensor Arduino also has its own slave devices to maintain. From the gy­80 sensor package the modules in use:

HMC5883L 3 Axis Compass Address = 0x1E BMP085 Barometer and Temperature Address=0x77

The Pi is responsible for performing all calculations for both sensors in order to obtain the altitude ( barometer) and heading (compass), both are 32 bit float numbers when sent to the Pi.

21

PPM (and PWM) Communications

Arduino to Flight Controller The flight Arduino is responsible for generating the PWM propeller motor guidance signals (previously dictated by the RC controller) into the flight controller. This is done via an encoded PPM stream into one channel of the flight controller. Based on timing allocated, the decoder within the flight controller should read each as separate PWM signals for each propeller. For more information refer to The RC Protocol . 3

LIDAR Communications For the object detection with the LIDAR and sonar, absolute location needs to be known.

3 File located on http://edge.rit.edu/edge/P15230/public/Detailed%20Design under Sub­System Design Prototyping,Engineering Analysis, Simulation

22