|
From: | Jorn Anke |
Subject: | Re: [Paparazzi-devel] Interest for a new survey pattern? (Zamboni-survey) |
Date: | Tue, 6 Nov 2012 19:37:08 +0100 |
Thanks for your interest Mitchell,
The pseudo-code I wrote for prototyping is copy-pasted below. It will run in BASIC-256, which is the reason for the graphic commands, (line, circle, etc). I will port the code to C, then see if I can modify one of the existing surveying routines. Cheers, Jorn # This is a routine for surveying of a rectangle-(like) area defined by 3 waypoints, A, B and C. The survey-pattern consists of oval-shaped # laps, starting from one side of the rectangle at P1, ending at P7 just "south og P1. The next laps will iterate toward the oposite side of # the rectangle (WPT_C), until the whole area is covered. This survey-pattern is sometimes called a "Zamboni-pattern", named after # the pattern resurfacing machines follows at ice-hockey rinks. The idea behind this survay-pattern is that it will provide the greatest # turn-radies possible, and thereby as smooth as possible flight characteristics. # # INPUT: 3 waypoints, A, B, and C. D is found by assuming line A-D is parallell to line B-C # LineSpace: Spacing between the flight lines # # # # FLIGHTPATTERN # # A ---------------------------- B # ( | P3 P4 | ) # R_ad ( | P2 P5 | ) R_bc # | P1 Center | ) # | P7 <--------------- P6 | # | | # |------------/---------------- | LineSpacing # D------------/---------------- C # # Length # # will be positioned on the edge lines A-D and B-C # For first round: # - P1: midpoint of line A-D # - P2: circlecenter to turn about, mid between P1 and P3 # - P3: startpoint of flightline from A-D to B-C, nearest point A # - P4: endpoint of flightline from A-D to B-C, nearest point B # - P5: circlecenter to turn about, mid between P4 and P6 # - P6: startpoint of flightline from B-C to A-D, one "linespacing" south of midpoint line B-C # - P7: endpoint of flightline from B-C to A-D, one "linespacing" south of midpoint line A-D # SAMPLE DATA # (Swap names between a, b, c, d to change the pattern or flight direction). ax=0 ay=50 bx=100 by=250 cx=200 cy=200 dx=100 dy=0 LineSpace=20 # COMPUTE SOME VALUES FOR LATER USE # delta distances betwen edge lines dx_ab=bx-ax dy_ab=by-ay dx_bc=cx-bx dy_bc=cy-by # length and width of the area to be surveyed L=sqrt(dx_ab^2 + dy_ab^2) W=sqrt(dx_bc^2 + dy_bc^2) # delta distances betwen adjecent flightlines dx_Linespace = dx_bc * LineSpace / W dy_Linespace = dy_bc * LineSpace / W # number of laps to fly (Ceil finds the integer greater then the float computed in the _expression_, eg. 9.1 => 10 NumRounds = ceil((W/2) / LineSpace) # (actually only the half of the last round will be flown, since the last line will be outside of the rectangle). # number of flight lines NumLines = NumRounds * 2 - 1 # circleradius for turning, by line A-D and B-C circleradius_ad = LineSpace * (NumRounds - 1) * 0.5 circleradius_bc = LineSpace * NumRounds * 0.5 # turn-direction (going from vector/line A-B to point C) direction = ((bx-ax)*(cy-ay) - (by-ay)*(cx-ax)) # direction > 0 : go right, direction < 0 : go left (or the other way around in paparazzi) # compute centerpoint of field center_x=(ax+cx)/2 center_y=(ay+cy)/2 # COMPUTE WAYPOINTS (FIRST ROUND) # P1 (mid line a-d) P1_x = center_x - dx_ab / 2 P1_y = center_y - dy_ab / 2 # P3 (on line a-d, near d) P3_x = P1_x - dx_Linespace * (NumRounds - 1) P3_y = P1_y - dy_Linespace * (NumRounds - 1)
# P2 (circlecenter, mid between P1 and P3) P2_x = (P1_x + P3_x) / 2 P2_y = (P1_y + P3_y) / 2 # P4 (on line b-c, near b) P4_x = center_x + dx_ab / 2 - dx_Linespace * (NumRounds - 1) P4_y = center_y + dy_ab / 2 - dy_Linespace * (NumRounds - 1) # P6 (on line b-c, nearest c) P6_x = center_x + dx_ab / 2 + dx_Linespace P6_y = center_y + dy_ab / 2 + dy_Linespace # P5 (on line b-c, mid between P4 and P6) P5_x = (P4_x + P6_x) / 2 P5_y = (P4_y + P6_y) / 2 # P7 (on line a-d, one flightline south of P1 ) P7_x = P1_x + dx_Linespace P7_y = P1_y + dy_Linespace # compute the total number of waypoints. (Not used for now). NumberOfWaypoints = NumRounds * 7 - 3 # The last 3 wpt's on the last round will not be flown # PRINT TO ILLUSTRATE # initially calculated wpts, round 1 (the circles are printed first not to hide the lines) clg color green circle (P2_x, P2_y, circleradius_ad) circle (P5_x, P5_y, circleradius_bc) color blue circle(P1_x, P1_y, 4) # mark enter-point first round line (P3_x, P3_y, P4_x, P4_y) circle(P3_x, P3_y, 4) line (P4_x, P4_y, P5_x, P5_y) circle(P4_x, P4_y, 4) line (P6_x, P6_y, P7_x, P7_y) circle(P6_x, P6_y, 4) circle(P7_x, P7_y, 4) # mark exit-point first round # the rectangle between the given WPTs color red line (ax, ay, bx, by) line (bx, by,cx, cy) line (cx, cy, dx, dy) line (dx, dy, ax, ay) color purple # compute the waypoints for the remaining rounds, each waypoint is moved one flightline "south" for each round For i = 1 to NumRounds - 1 NextP1x = P1_x + dx_Linespace * i NextP1y = P1_y + dy_Linespace * i NextP2x = P2_x + dx_Linespace * i NextP2y = P2_y + dy_Linespace * i NextP3x = P3_x + dx_Linespace * i NextP3y = P3_y + dy_Linespace * i NextP4x = P4_x + dx_Linespace * i NextP4y = P4_y + dy_Linespace * i if i < NumRounds-1 then # the last 3 waypoints will not be used NextP5x = P5_x + dx_Linespace * i NextP5y = P5_y + dy_Linespace * i NextP6x = P6_x + dx_Linespace * i NextP6y = P6_y + dy_Linespace * i NextP7x = P7_x + dx_Linespace * i NextP7y = P7_y + dy_Linespace * i else # mark exit-point Circle(NextP4x,NextP4y,4) endif # printing for illustrating, (only lines, not circles) line(NextP1x, NextP1y, NextP3x, NextP3y) line(NextP3x, NextP3y, NextP4x, NextP4y) line(NextP4x, NextP4y, NextP6x, NextP6y) line(NextP6x, NextP6y, NextP7x, NextP7y) Next i # Print computed walues for testing print "Space between flightlines: " + LineSpace print "Length of field: " + L print "Width pf field: " + W print "Number of rounds: " + NumRounds Print "Number of flightlines: " + NumLines print "Circleradius by line A-D: " + circleradius_ad print "Circleradius by line B-C: " + circleradius_bc if direction > 0 then print "Turn direction: " + direction + " => turn right" else print "Turn direction: " + direction + " => turn left" endif |
[Prev in Thread] | Current Thread | [Next in Thread] |