In [1]:

    

using JSON
using JuMP
using NLopt
using Winston
using ThermalTraj


    

In [12]:

    

# Preamble on global variables
ENBLNET = true

#create connection to Python Autopilot
if ENBLNET
    client = connect(ip"127.0.0.1",5510)
    println("Connected")
end

# and immediately request the current position from Python
# Commands to send to Python Autopilot
channel = "REQUEST_DATA"

my_dict = ["CHANNEL"=> channel]
reqPosMsg = json(my_dict)

# placeholder for trajectory tape
nodes = 40
x = zeros(5,nodes);
u = zeros(nodes);

Re = 6378137

# Thermal at KSO tower
#<latitude>37.61633</latitude>
#<longitude>-122.38334</longitude>
#<strength-fps>13.49</strength-fps>
#<diameter-ft>4800.0</diameter-ft>
Tlat = 37.61633
Tlon = -122.38334

dt = 0.5;
t = linspace(0,dt*nodes,nodes);


    

    




Connected

In [13]:

    

# run behaviors
tapeMsg = ""

if ENBLNET
    #request state from python
    println("Requesting data from Python controller")
    print(client,reqPosMsg)
    #block on reply from python
    msgdata = readline(client)
else
    msgdata = "{\"LAT\":37.6173,\"LON\":-122.384,\"HEADING\":60.0,\"CHANNEL\":\"GLIDER_STATE\"}"
end
msg = JSON.parse(msgdata)
dlat = (msg["LAT"]*180.0/pi - Tlat)*pi/180.0
dlon = (msg["LON"]*180.0/pi - Tlon)*pi/180.0

x0 = zeros(5)
println("Heading received ", msg["HEADING"])
x0[3] = heading2XY(msg["HEADING"]*180.0/pi)
x0[1] = Re*dlon
x0[2] = Re*dlat


    

    




Requesting data from Python controller
Heading received 2.4371731281280518






    
Out[13]:





347.83605592059945

In [23]:

    

println("x0: ", x0)

#solve new opt problem
solveTraj!(x0,u,x)

#pack new json tape
tape = ["CHANNEL"=> "NEW_TAPE", "t"=> t, "u"=>-x[4,:], "x"=>x[1,:], "y"=>x[2,:]]
tapeMsg = json(tape)
#send tape
if ENBLNET
    print(client,tapeMsg)
else
    #println(tapeMsg)    
end

pp = floor(4*nodes/5)
rpx0 = x[:,pp]
println(rpx0)
if rpx0[3]<-pi
    rpx0[3] += 2pi
end
if rpx0[3]>pi
    rpx0[3] -= 2pi
end
println(rpx0)
x0 = rpx0
timegap = pp*dt;
println(timegap)
plot(x[1,:],x[2,:],"b--", x[1,1:2],x[2,1:2],"g^",x[1,pp:(pp+1)],x[2,pp:(pp+1)],"r^")


    

    




x0: [-43.3005838378174,-131.78265332548986,-0.9686262251127494,0.7160457207835792,0.747510757545461]
Constant forward body speed of [25.9, 0] [m/s]
8.804416759
[30.854630537801366,44.41013342709192,3.438496817013252,0.4704263444989438,0.6491552474472049]
[30.854630537801366,44.41013342709192,-2.8446884901663343,0.4704263444989438,0.6491552474472049]
16.0






    
Out[23]:

In [25]:

    

plot(x[5,:])


    

    
Out[25]:

In [16]:

    

t0 = time()


    

    
Out[16]:





1.416796002508714e9

In [17]:

    

time() - t0


    

    
Out[17]:





0.33635592460632324

In [12]:

    

plot(u)


    

    
Out[12]:

In [28]:

    

close(client)


    

In [6]:

    

print(client,reqPosMsg)
msgdata = readline(client)
msg = JSON.parse(msgdata)
#sqrt(msg["V_NORTH"]^2 + msg["V_EAST"]^2+msg["V_DOWN"]^2)


    

    
Out[6]:





Dict{String,Any} with 14 entries:
  "THETA"     => 0.03474651649594307
  "CHANNEL"   => "GLIDER_STATE"
  "CUR_INDEX" => 0
  "V_EAST"    => 3.720635977935791
  "HEADING"   => 3.0144662857055664
  "LON"       => -2.1359970329770834
  "PHI"       => 0.13293160498142242
  "ALT"       => 1737.2586860195868
  "V_NORTH"   => -29.78737716064453
  "VCAS"      => 16.22727578430176
  "LAT"       => 0.656504700350418
  "V_DOWN"    => -3.8699303535461427
  "BETA"      => 0.0
  "ALPHA"     => 0.029979677870869637

In [26]:

    

println(msgdata)


    

    




{"LON": -2.1360657482262657, "VCAS": 14.072186398315431, "BETA": 0.0, "V_DOWN": 0.35784817914962769, "THETA": 0.024337785318493843, "HEADING": 2.6521437168121338, "CHANNEL": "GLIDER_STATE", "PHI": -1.4803615044911567e-07, "V_NORTH": -23.019897216796878, "V_EAST": 12.262245712280274, "LAT": 0.65657683073480955, "ALPHA": 0.055184818804264069, "ALT": 1763.1131056348274}

In [7]:

    

print(client,reqPosMsg)
msgdata = readline(client)
msg = JSON.parse(msgdata)
dlat = (msg["LAT"]*180.0/pi - Tlat)*pi/180.0
dlon = (msg["LON"]*180.0/pi - Tlon)*pi/180.0
x0 = zeros(3)
x0[1] = Re*dlon
x0[2] = Re*dlat

println(x0[1],", ",x0[2])


    

    




-30.61754128565801, -153.78113282633785

In [ ]:

    

x0[3] = heading2XY(msg["HEADING"]*180.0/pi) #-(msg["heading"]-90.0)*pi/180.0
println(msg["HEADING"]*180.0/pi)
println(x0[3]*180.0/pi)


    

In [37]:

    

# test function from ThermalTraj
println(heading2XY(0.0))
println(heading2XY(45.0))
println(heading2XY(90.0))
println(heading2XY(180.0))
println(heading2XY(270.0))
println(heading2XY(271.0))
println(heading2XY(359.0))


    

    




1.5707963267948966
0.7853981633974483
-0.0
-1.5707963267948966
3.141592653589793
3.12413936106985
1.5882496193148399

In [ ]:

    

# Trying out some initial conditions
Re*(-122.384 - Tlon)*pi/180.0

# Initial conditions
x0 = [60., 
      60., 
      2./2.*3.14159265358979323]


solveTraj!(x0,u,x)
plot(x[1,:],x[2,:])