diff --git a/examples/parking_controller.jl b/examples/parking_controller.jl index e1e5e05..c139092 100644 --- a/examples/parking_controller.jl +++ b/examples/parking_controller.jl @@ -81,7 +81,7 @@ function navigate(pc::ParkingController, azimuth, elevation; limit=50.0) end y = sin(phi_set - phi) * cos(beta_set) x = cos(beta) * sin(beta_set) - sin(beta) * cos(beta_set) * cos(phi_set - phi) - pc.chi_set = atan(-y, x) + pc.chi_set = atan(y, x) end """ diff --git a/examples/parking_wind_dir.jl b/examples/parking_wind_dir.jl index 83827ed..e722cda 100644 --- a/examples/parking_wind_dir.jl +++ b/examples/parking_wind_dir.jl @@ -91,13 +91,13 @@ function sim_parking(integrator) time = i * dt steering = 0.0 if i > 100 - heading = calc_heading(kps4; neg_azimuth=true, one_point=false) if i == 100 - pc.last_heading = heading + pc.last_heading = sys_state.heading end elevation = sys_state.elevation - chi_set = -navigate(pc, sys_state.azimuth, elevation) - steering, ndi_gain, psi_dot, psi_dot_set = calc_steering(pc, heading, chi_set; elevation, v_app = sys_state.v_app) + chi_set = navigate(pc, sys_state.azimuth, elevation) + steering, ndi_gain, psi_dot, psi_dot_set = calc_steering(pc, sys_state.heading, chi_set; + elevation, v_app = sys_state.v_app) set_depower_steering(kps4.kcu, MIN_DEPOWER, steering) end SET_STEERING[i] = steering