@@ -37,6 +37,7 @@ def __init__(self, waypoint: location.Location, acceptance_radius: float) -> Non
37
37
# Add your own
38
38
self .has_sent_landing_command = False
39
39
self .send_landing_command = False
40
+ self .has_reached_waypoint = False
40
41
self .min_bounds = - 60
41
42
self .max_bounds = 60
42
43
@@ -83,9 +84,13 @@ def run(
83
84
and self .waypoint .location_y >= self .min_bounds
84
85
and self .waypoint .location_y <= self .max_bounds
85
86
):
86
- proximity = (self .waypoint .location_x - report .position .location_x ) ** 2 + (
87
- self .waypoint .location_y - report .position .location_y
88
- ) ** 2
87
+ if not self .has_reached_waypoint :
88
+ proximity = (self .waypoint .location_x - report .position .location_x ) ** 2 + (
89
+ self .waypoint .location_y - report .position .location_y
90
+ ) ** 2
91
+ if proximity < self .acceptance_radius ** 2 :
92
+ self .has_reached_waypoint = True
93
+
89
94
new_pad = location .Location (0 , 0 )
90
95
# checking if the drone is halted
91
96
if report .status == drone_status .DroneStatus .HALTED :
@@ -104,11 +109,19 @@ def run(
104
109
smallest_dist = new_dist
105
110
new_pad = landing_pads
106
111
107
- command = commands .Command .create_set_relative_destination_command (
108
- new_pad .location_x - self .waypoint .location_x ,
109
- new_pad .location_y - self .waypoint .location_y ,
110
- )
111
- self .send_landing_command = True
112
+ # if the landing pad is at the waypoint
113
+ if new_pad == self .waypoint :
114
+ command = commands .Command .create_set_relative_destination_command (
115
+ self .waypoint .location_x - report .position .location_x ,
116
+ self .waypoint .location_y - report .position .location_y ,
117
+ )
118
+ self .send_landing_command = True
119
+ else :
120
+ command = commands .Command .create_set_relative_destination_command (
121
+ new_pad .location_x - report .position .location_x ,
122
+ new_pad .location_y - report .position .location_y ,
123
+ )
124
+ self .send_landing_command = True
112
125
113
126
# setting relative destination to designated waypoint
114
127
elif proximity > self .acceptance_radius ** 2 and not self .has_sent_landing_command :
0 commit comments