Skip to content

Commit

Permalink
fix(smartRTH): start frame and duration set correctly
Browse files Browse the repository at this point in the history
  • Loading branch information
vasarhelyi committed Oct 17, 2023
1 parent 4decc90 commit 90615b8
Show file tree
Hide file tree
Showing 2 changed files with 17 additions and 2 deletions.
11 changes: 11 additions & 0 deletions src/modules/sbstudio/api/types.py
Original file line number Diff line number Diff line change
Expand Up @@ -75,6 +75,17 @@ class SmartRTHPlan:
def empty(cls):
return cls(start_times=[], durations=[], inner_points=[])

@property
def duration(self) -> float:
"""Returns the overall duration of the smart RTH plan in seconds."""
if not self.start_times or not self.durations:
return 0

return max(
start + duration
for start, duration in zip(self.start_times, self.durations, strict=True)
)


@dataclass
class TransitionPlan:
Expand Down
8 changes: 6 additions & 2 deletions src/modules/sbstudio/plugin/operators/return_to_home.py
Original file line number Diff line number Diff line change
Expand Up @@ -148,10 +148,14 @@ def _run(self, storyboard, *, context) -> bool:
if not drones:
return False

use_smart_rth = self._should_use_smart_rth()
self.start_frame = max(self.start_frame, storyboard.frame_end) + (
1 if use_smart_rth else 0
)

with create_position_evaluator() as get_positions_of:
source = get_positions_of(drones, frame=self.start_frame)

use_smart_rth = self._should_use_smart_rth()
first_frame = storyboard.frame_start
_, target, _ = create_helper_formation_for_takeoff_and_landing(
drones,
Expand Down Expand Up @@ -187,7 +191,7 @@ def _run(self, storyboard, *, context) -> bool:
entry = storyboard.add_new_entry(
formation=create_formation("Smart return to home", source),
frame_start=self.start_frame,
duration=int(max(plan.durations) * fps),
duration=int((plan.duration + self.altitude / land_speed) * fps),
select=True,
context=context,
)
Expand Down

0 comments on commit 90615b8

Please sign in to comment.