├── README.md ├── params.param ├── sololink.sdp └── target_land.py /README.md: -------------------------------------------------------------------------------- 1 | target-land 2 | =========== 3 | 4 | This demo allows a 3DR Solo to land on a colorful object using only the quadcopter, the controller, a gopro and a laptop. The CV runs offboard. 5 | 6 | [Demo Video](https://youtu.be/PURlcAu6MEk) 7 | 8 | ### Setup instructions: 9 | 10 | #### video: 11 | 12 | The SDP file defines a network stream. Opening this file with VLC or OpenCV's `cv2.VideoCapture()` should work. Before opening the network stream, we need to tell the controller to start streaming video. We do this by running `nc 10.1.1.1 5502` on the computer where we want to receive the video. Let that run in the background. TODO make the python script open this instead, so the user isn't bothered by this extra step. 13 | 14 | 15 | #### autopilot firmware: 16 | 17 | Clone [ardupilot](github.com/diydrones/ardupilot) and `cd` into ArduCopter. In the file called `APM_Config.h`, find the line that says `//#define PRECISION_LANDING ENABLED`. Uncomment this line. Now compile with `make px4-v2`. The build output is in this folder, and it's called `ArduCopter-v2.px4`. Now upload it to the Solo: 18 | 19 | - `scp ./ArduCopter-v2.px4 root@10.1.1.10:/firmware/ArduCopter-v2.px4` 20 | 21 | restart the Solo and wait a minute for the changes to take effect. If it doesn't work, try bumping the version number in the filename of `/firmware/ArduCopter-v2.px4` to be higher than the last firmware that was flashed (located in `./loaded`). 22 | 23 | #### parameters: 24 | 25 | Using you favorite GCS, load the parameters file. Using MAVProxy it looks like this: 26 | 27 | - `mavproxy.py --master=udpin:0.0.0.0:14550` 28 | - `param load ./params.param` 29 | 30 | Then do a compass calibration using the Solo app, not a traditional GCS. A traditional GCS will do an offboard compass calibration, we want to do an onboard calibration. 31 | 32 | #### colors: 33 | 34 | In `target_land.py` there are variables near the top that need to be set in order to detect the right color. They are in HSV. Change these depending on the object, camera, lighting, etc. Red objects might are not ideal, because their H value is sometimes very high and sometimes very low. 35 | 36 | #### run it: 37 | 38 | Make sure these dependencies are updated: 39 | `sudo pip2 install mavproxy dronekit pymavlink numpy --upgrade`. 40 | 41 | Also install OpenCV and put the right things in your path so that you can `import cv2` in Python2. 42 | 43 | run it: `./target_land.py` 44 | 45 | The script will connect to the video feed of the camera and then will connect to the vehicle over MAVLink. Whenever it sees a target that is big enough, it will send a LANDING_TARGET message. 46 | -------------------------------------------------------------------------------- /params.param: -------------------------------------------------------------------------------- 1 | ACCEL_Z_D 0.000000 2 | ACCEL_Z_FILT_HZ 20.000000 3 | ACCEL_Z_I 1.500000 4 | ACCEL_Z_IMAX 800.000000 5 | ACCEL_Z_P 0.750000 6 | 7 | ATC_ACCEL_P_MAX 72000.000000 8 | ATC_ACCEL_R_MAX 72000.000000 9 | ATC_ACCEL_Y_MAX 21000.000000 10 | ATC_RATE_FF_ENAB 1.000000 11 | ATC_SLEW_YAW 1000.000000 12 | 13 | BATT_AMP_OFFSET 0.000000 14 | BATT_AMP_PERVOLT 17.000000 15 | BATT_CAPACITY 5200.000000 16 | BATT_CURR_PIN 3.000000 17 | BATT_MONITOR 5.000000 18 | BATT_VOLT_MULT 10.100000 19 | BATT_VOLT_PIN 2.000000 20 | 21 | BRD_SER1_RTSCTS 1.000000 22 | 23 | COMPASS_ORIENT 38.000000 24 | COMPASS_ORIENT2 0.000000 25 | COMPASS_ORIENT3 0.000000 26 | COMPASS_PRIMARY 0.000000 27 | COMPASS_USE 1.000000 28 | COMPASS_USE2 1.000000 29 | COMPASS_USE3 0.000000 30 | 31 | EKF_ABIAS_PNOISE 0.000050 32 | EKF_ACC_PNOISE 0.250000 33 | EKF_ALT_NOISE 2.000000 34 | EKF_ALT_SOURCE 1.000000 35 | EKF_EAS_GATE 10.000000 36 | EKF_EAS_NOISE 1.400000 37 | EKF_FALLBACK 1.000000 38 | EKF_FLOW_DELAY 10.000000 39 | EKF_FLOW_GATE 3.000000 40 | EKF_FLOW_NOISE 0.250000 41 | EKF_GBIAS_PNOISE 0.000001 42 | EKF_GLITCH_ACCEL 100.000000 43 | EKF_GLITCH_RAD 25.000000 44 | EKF_GND_GRADIENT 2.000000 45 | EKF_GPS_TYPE 0.000000 46 | EKF_GYRO_PNOISE 0.015000 47 | EKF_HGT_GATE 10.000000 48 | EKF_MAGB_PNOISE 0.000600 49 | EKF_MAGE_PNOISE 0.000600 50 | EKF_MAG_CAL 3.000000 51 | EKF_MAG_GATE 3.000000 52 | EKF_MAG_NOISE 0.050000 53 | EKF_MAX_FLOW 2.500000 54 | EKF_POSNE_NOISE 0.500000 55 | EKF_POS_DELAY 200.000000 56 | EKF_POS_GATE 5.000000 57 | EKF_RNG_GATE 5.000000 58 | EKF_VELD_NOISE 0.700000 59 | EKF_VELNE_NOISE 0.500000 60 | EKF_VEL_DELAY 200.000000 61 | EKF_VEL_GATE 4.000000 62 | EKF_WIND_PNOISE 0.100000 63 | EKF_WIND_PSCALE 0.500000 64 | 65 | FENCE_ACTION 0.000000 66 | FENCE_ALT_MAX 117.348000 67 | FENCE_ENABLE 1.000000 68 | FENCE_MARGIN 0.000000 69 | FENCE_RADIUS 300.000000 70 | FENCE_TYPE 1.000000 71 | 72 | FS_BATT_ENABLE 2.000000 73 | FS_BATT_MAH 520.000000 74 | FS_BATT_VOLTAGE 14.000000 75 | FS_EKF_ACTION 2.000000 76 | FS_EKF_THRESH 0.800000 77 | FS_GCS_ENABLE 0.000000 78 | FS_THR_ENABLE 1.000000 79 | FS_THR_VALUE 910.000000 80 | 81 | GND_ABS_PRESS 102613.757812 82 | GND_TEMP 43.123463 83 | 84 | LAND_REPOSITION 1.000000 85 | LAND_SPEED 50.000000 86 | 87 | MOT_CURR_MAX 42.000000 88 | MOT_SPIN_ARMED 70.000000 89 | MOT_THR_MIX_MIN 0.150000 90 | MOT_THST_BAT_MAX 16.799999 91 | MOT_THST_BAT_MIN 12.000000 92 | MOT_THST_EXPO 0.800000 93 | MOT_THST_MAX 0.940000 94 | MOT_YAW_HEADROOM 200.000000 95 | 96 | PHLD_BRAKE_ANGLE 3000.000000 97 | PHLD_BRAKE_RATE 8.000000 98 | PILOT_ACCEL_Z 300.000000 99 | PILOT_THR_BHV 1.000000 100 | PILOT_THR_FILT 2.000000 101 | PILOT_TKOFF_ALT 75.000000 102 | PILOT_TKOFF_DZ 250.000000 103 | PILOT_VELZ_MAX 400.000000 104 | POS_XY_P 0.700000 105 | POS_Z_P 1.000000 106 | PSC_ACC_XY_FILT 2.000000 107 | 108 | RATE_PIT_D 0.008000 109 | RATE_PIT_FILT_HZ 40.000000 110 | RATE_PIT_I 0.168000 111 | RATE_PIT_IMAX 3000.000000 112 | RATE_PIT_P 0.168000 113 | RATE_RLL_D 0.007200 114 | RATE_RLL_FILT_HZ 40.000000 115 | RATE_RLL_I 0.116000 116 | RATE_RLL_IMAX 3000.000000 117 | RATE_RLL_P 0.116000 118 | RATE_YAW_D 0.000000 119 | RATE_YAW_FILT_HZ 7.600000 120 | RATE_YAW_I 0.066000 121 | RATE_YAW_IMAX 1000.000000 122 | RATE_YAW_P 0.660000 123 | 124 | PLAND_SPEED 100.000000 125 | PLAND_TYPE 1.000000 126 | 127 | THR_DZ 10.000000 128 | THR_MID 400.000000 129 | THR_MIN 130.000000 130 | TUNE 0.000000 131 | TUNE_HIGH 1000.000000 132 | TUNE_LOW 0.000000 133 | VEL_XY_FILT_HZ 5.000000 134 | VEL_XY_I 0.500000 135 | VEL_XY_IMAX 1000.000000 136 | VEL_XY_P 1.400000 137 | VEL_Z_P 5.000000 138 | WPNAV_ACCEL 340.000000 139 | WPNAV_ACCEL_Z 160.000000 140 | WPNAV_LOIT_JERK 3000.000000 141 | WPNAV_LOIT_MAXA 687.000000 142 | WPNAV_LOIT_MINA 206.000000 143 | WPNAV_LOIT_SPEED 1500.000000 144 | WPNAV_RADIUS 200.000000 145 | WPNAV_SPEED 1100.000000 146 | WPNAV_SPEED_DN 240.000000 147 | WPNAV_SPEED_UP 320.000000 148 | WP_YAW_BEHAVIOR 2.000000 149 | -------------------------------------------------------------------------------- /sololink.sdp: -------------------------------------------------------------------------------- 1 | c=IN IP4 10.1.1.1 2 | m=video 5600 RTP/AVP 96 3 | a=rtpmap:96 H264/90000 4 | t=0 0 5 | -------------------------------------------------------------------------------- /target_land.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python2 2 | import math 3 | 4 | import cv2 5 | import numpy as np 6 | from dronekit import VehicleMode, connect 7 | 8 | #color settings 9 | hue_lower = 55 10 | hue_upper = 185 11 | saturation_lower = 110 12 | saturation_upper = 170 13 | value_lower = 190 14 | value_upper = 250 15 | min_contour_area = 500 # the smallest number of pixels in a contour before it will register this as a target 16 | 17 | #camera 18 | horizontal_fov = 118.2 * math.pi/180 19 | vertical_fov = 69.5 * math.pi/180 20 | horizontal_resolution = 1280 21 | vertical_resolution = 720 22 | 23 | camera = cv2.VideoCapture("./sololink.sdp") 24 | 25 | vehicle = connect('udpin:0.0.0.0:14550',wait_ready=True) 26 | 27 | def send_land_message(x, y): 28 | msg = vehicle.message_factory.landing_target_encode( 29 | 0, # time_boot_ms (not used) 30 | 0, # target num 31 | 0, # frame 32 | (x-horizontal_resolution/2)*horizontal_fov/horizontal_resolution, 33 | (y-vertical_resolution/2)*vertical_fov/vertical_resolution, 34 | 0, # altitude. Not supported. 35 | 0,0) # size of target in radians 36 | vehicle.send_mavlink(msg) 37 | vehicle.flush() 38 | 39 | while(1): 40 | _,capture = camera.read() 41 | hsvcapture = cv2.cvtColor(capture,cv2.COLOR_BGR2HSV) 42 | inrangepixels = cv2.inRange(hsvcapture,np.array((hue_lower,saturation_lower,value_lower)),np.array((hue_upper,saturation_upper,value_upper)))#in opencv, HSV is 0-180,0-255,0-255 43 | tobecontourdetected = inrangepixels.copy() 44 | #TODO filter better. binary morphology would be a good start. 45 | contours,hierarchy = cv2.findContours(tobecontourdetected,cv2.RETR_LIST,cv2.CHAIN_APPROX_SIMPLE) 46 | 47 | contour_sizes=[] 48 | contour_centroids = [] 49 | for contour in contours: 50 | real_area = cv2.contourArea(contour) 51 | if real_area > min_contour_area: 52 | M = cv2.moments(contour) #moment is centroid 53 | cx,cy = int(M['m10']/M['m00']), int(M['m01']/M['m00']) 54 | cv2.circle(capture,(cx,cy),5,(0,0,255),-1) 55 | contour_sizes.append(real_area) 56 | contour_centroids.append((cx,cy)) 57 | 58 | #find biggest contour (by area) 59 | biggest_contour_index = 0 60 | for i in range(1,len(contour_sizes)): 61 | if contour_sizes[i] > contour_sizes[biggest_contour_index]: 62 | biggest_contour_index = i 63 | biggest_contour_centroid=None 64 | if len(contour_sizes)>0: 65 | biggest_contour_centroid=contour_centroids[biggest_contour_index] 66 | 67 | #if the biggest contour was found, color it blue and send the message 68 | if biggest_contour_centroid is not None: 69 | cv2.circle(capture,biggest_contour_centroid,5,(255,0,0),-1) 70 | x,y = biggest_contour_centroid 71 | send_land_message(x,y) 72 | 73 | cv2.imshow('capture',capture) 74 | cv2.imshow('inrangepixels',inrangepixels) 75 | 76 | if cv2.waitKey(1) == 27: 77 | break 78 | 79 | cv2.destroyAllWindows() 80 | camera.release() 81 | --------------------------------------------------------------------------------