def update_wps(self, wp_list):
if Settings.enable_autopilot:
if len(wp_list) < 2:
return
with self.wp_update_in_progress:
if LosSettings.enable_los:
if self.los_thread.isAlive():
self.los_restart_event.set()
self.los_stop_event.set()
self.los_thread.join()
self.los_stop_event.clear()
self.los_restart_event.clear()
self.los_controller.update_wps(wp_list)
self.los_controller.update_pos(self.cur_pos_msg)
self.los_thread = threading.Thread(
target=self.los_controller.loop, daemon=True)
self.los_thread.start()
self.los_start_event.set()
return
thread = self.stop_autopilot()
thread.join()
self.send_autopilot_msg(ap.Command(
ap.CommandOptions.CLEAR_WPS))
self.send_autopilot_msg(ap.AddWaypoints(wp_list))
self.switch_ap_mode(ap.GuidanceModeOptions.PATH_FOLLOWING)
# self.send_autopilot_msg(ap.Setpoint(0, ap.Dofs.YAW, False))
# if len(wp_list[0]) > 3:
# self.send_autopilot_msg(ap.TrackingSpeed(wp_list[0][3]))
# else:
self.send_autopilot_msg(
ap.TrackingSpeed(CollisionSettings.tracking_speed))
def stop_autopilot(self):
if Settings.enable_autopilot:
logger.info('Stopping ROV')
if self.cur_pos_msg is None:
return
if self.guidance_mode is not ap.GuidanceModeOptions.STATION_KEEPING:
if self.guidance_mode is ap.GuidanceModeOptions.PATH_FOLLOWING:
# self.switch_ap_mode(ap.GuidanceModeOptions.PATH_FOLLOWING))
self.send_autopilot_msg(ap.TrackingSpeed(0))
if self.guidance_mode is ap.GuidanceModeOptions.CRUISE_MODE:
self.send_autopilot_msg(ap.CruiseSpeed(0))
counter = 0
n_set = 1
max = 20
surge = np.ones(max)
surge[0] = self.cur_pos_msg.v_surge
acc = 1
while abs(np.sum(surge) / n_set) > 0.02:
# print(np.sum(v_surge)/n_set, acc)
self.ap_pos_received.clear()
self.ap_pos_received.wait(0.1)
counter += 1
if counter == max:
counter = 0
surge[counter] = self.cur_pos_msg.v_surge
acc = abs((surge[counter] - surge[counter - 1]) / 0.1)
if n_set < max:
n_set += 1
self.switch_ap_mode(ap.GuidanceModeOptions.STATION_KEEPING)
# self.send_autopilot_msg(ap.Setpoint(0, ap.Dofs.SURGE, True))
# self.send_autopilot_msg(ap.Setpoint(0, ap.Dofs.SWAY, True))
# self.send_autopilot_msg(ap.Setpoint(self.cur_pos_msg.yaw, ap.Dofs.YAW, True))
self.send_autopilot_msg(
ap.VerticalPos(ap.VerticalPosOptions.ALTITUDE,
self.cur_pos_msg.alt))
else:
return