297 lines
7.9 KiB
Plaintext
297 lines
7.9 KiB
Plaintext
########################################
|
|
#
|
|
# FLIGHT-RELATED FUNCTIONS
|
|
#
|
|
########################################
|
|
include "vec2.bzz"
|
|
include "rrtstar.bzz"
|
|
|
|
TARGET_ALTITUDE = 15.0 # m.
|
|
UAVSTATE = "TURNEDOFF"
|
|
PICTURE_WAIT = 20 # steps
|
|
GOTO_MAXVEL = 2 # m/steps
|
|
GOTO_MAXDIST = 150 # m.
|
|
GOTODIST_TOL = 0.5 # m.
|
|
GOTOANG_TOL = 0.1 # rad.
|
|
cur_goal_l = 0
|
|
rc_State = 0
|
|
|
|
function uav_initswarm() {
|
|
s = swarm.create(1)
|
|
s.join()
|
|
}
|
|
|
|
function turnedoff() {
|
|
statef=turnedoff
|
|
UAVSTATE = "TURNEDOFF"
|
|
}
|
|
|
|
function idle() {
|
|
statef=idle
|
|
UAVSTATE = "IDLE"
|
|
}
|
|
|
|
function takeoff() {
|
|
UAVSTATE = "TAKEOFF"
|
|
statef=takeoff
|
|
homegps = {.lat=position.latitude, .long=position.longitude}
|
|
|
|
if( flight.status == 2 and position.altitude >= TARGET_ALTITUDE-TARGET_ALTITUDE/20.0) {
|
|
barrier_set(ROBOTS, action, land, -1)
|
|
barrier_ready()
|
|
} else {
|
|
log("Altitude: ", position.altitude)
|
|
neighbors.broadcast("cmd", 22)
|
|
uav_takeoff(TARGET_ALTITUDE)
|
|
}
|
|
}
|
|
|
|
function land() {
|
|
UAVSTATE = "LAND"
|
|
statef=land
|
|
|
|
neighbors.broadcast("cmd", 21)
|
|
uav_land()
|
|
|
|
if(flight.status != 2 and flight.status != 3) {
|
|
barrier_set(ROBOTS,turnedoff,land, 21)
|
|
barrier_ready()
|
|
}
|
|
}
|
|
|
|
function set_goto(transf) {
|
|
UAVSTATE = "GOTOGPS"
|
|
statef=function() {
|
|
gotoWPRRT(transf)
|
|
}
|
|
|
|
if(rc_goto.id==id){
|
|
if(s!=nil){
|
|
if(s.in())
|
|
s.leave()
|
|
}
|
|
} else {
|
|
neighbors.broadcast("cmd", 16)
|
|
neighbors.broadcast("gt", rc_goto.id+rc_goto.longitude+rc_goto.latitude)
|
|
}
|
|
}
|
|
|
|
ptime=0
|
|
function picture() {
|
|
statef=picture
|
|
UAVSTATE="PICTURE"
|
|
uav_setgimbal(0.0, 0.0, -90.0, 20.0)
|
|
if(ptime==PICTURE_WAIT/2) { # wait for the drone to stabilize
|
|
uav_takepicture()
|
|
} else if(ptime>=PICTURE_WAIT) { # wait for the picture
|
|
statef=action
|
|
ptime=0
|
|
}
|
|
ptime=ptime+1
|
|
}
|
|
|
|
#
|
|
# still requires to be tuned, replaning takes too much time..
|
|
# DS 23/11/2017
|
|
function gotoWPRRT(transf) {
|
|
rc_goal = vec_from_gps(rc_goto.latitude, rc_goto.longitude, 0)
|
|
m_pos = math.vec2.scale(vec_from_gps(homegps.lat, homegps.long, 0),-1)
|
|
print(" has to move ", math.vec2.length(rc_goal), "from ", m_pos.x, " ", m_pos.y)
|
|
|
|
if(math.vec2.length(rc_goal)>GOTO_MAXDIST*5)
|
|
log("Sorry this is too far.")
|
|
else {
|
|
if(Path==nil){
|
|
# create the map
|
|
if(map==nil) {
|
|
UAVinit_map(rc_goal)
|
|
homegps.lat = position.latitude
|
|
homegps.long = position.longitude
|
|
}
|
|
# add proximity sensor and neighbor obstacles to the map
|
|
while(Path==nil) {
|
|
updateMap(m_pos)
|
|
Path = UAVpathPlanner(rc_goal, m_pos)
|
|
}
|
|
cur_goal_l = 1
|
|
} else if(cur_goal_l <= size(Path)) {
|
|
var cur_goal_gps = getvec(Path, cur_goal_l) #x=latitude, y=longitude
|
|
var cur_goal_pt = vec_from_gps(cur_goal_gps.x, cur_goal_gps.y, 0)
|
|
print(" heading to [", cur_goal_l, "/", size(Path), "]", cur_goal_pt.x, cur_goal_pt.y)
|
|
if(math.vec2.length(cur_goal_pt) > GOTODIST_TOL) {
|
|
updateMap(m_pos)
|
|
if(check_path(Path, cur_goal_l, m_pos, 0)) {
|
|
uav_moveto(0.0, 0.0, rc_goto.altitude-position.altitude)
|
|
Path = nil
|
|
rc_goal = vec_from_gps(rc_goto.latitude, rc_goto.longitude,0)
|
|
while(Path == nil) {
|
|
updateMap(m_pos)
|
|
Path = UAVpathPlanner(rc_goal, m_pos)
|
|
}
|
|
cur_goal_l = 1
|
|
} else {
|
|
cur_goal_pt = math.vec2.scale(cur_goal_pt, GOTO_MAXVEL/math.vec2.length(cur_goal_pt))
|
|
uav_moveto(cur_goal_pt.x, cur_goal_pt.y, rc_goto.altitude-position.altitude)
|
|
}
|
|
}
|
|
else
|
|
cur_goal_l = cur_goal_l + 1
|
|
} else {
|
|
Path = nil
|
|
transf()
|
|
}
|
|
}
|
|
}
|
|
|
|
function gotoWP(transf) {
|
|
m_navigation = vec_from_gps(rc_goto.latitude, rc_goto.longitude)
|
|
print(" has to move ", math.vec2.length(m_navigation), math.vec2.angle(m_navigation))
|
|
|
|
if(math.vec2.length(m_navigation)>GOTO_MAXDIST)
|
|
log("Sorry this is too far.")
|
|
else if(math.vec2.length(m_navigation)>GOTO_MAXVEL) { # limit velocity
|
|
m_navigation = math.vec2.scale(m_navigation, GOTO_MAXVEL/math.vec2.length(m_navigation))
|
|
uav_moveto(m_navigation.x, m_navigation.y, rc_goto.altitude-position.altitude)
|
|
} else if(math.vec2.length(m_navigation) < GOTODIST_TOL and math.vec2.angle(m_navigation) < GOTOANG_TOL) # reached destination
|
|
transf()
|
|
else
|
|
uav_moveto(m_navigation.x, m_navigation.y, rc_goto.altitude-position.altitude)
|
|
}
|
|
|
|
function follow() {
|
|
if(size(targets)>0) {
|
|
UAVSTATE = "FOLLOW"
|
|
statef=follow
|
|
attractor=math.vec2.newp(0,0)
|
|
foreach(targets, function(id, tab) {
|
|
force=(0.05)*(tab.range)^4
|
|
attractor=math.vec2.add(attractor, math.vec2.newp(force, tab.bearing))
|
|
})
|
|
uav_moveto(attractor.x, attractor.y, 0.0)
|
|
} else {
|
|
log("No target in local table!")
|
|
#statef=idle
|
|
}
|
|
}
|
|
|
|
function uav_rccmd() {
|
|
if(flight.rc_cmd==22) {
|
|
log("cmd 22")
|
|
flight.rc_cmd=0
|
|
statef = takeoff
|
|
UAVSTATE = "TAKEOFF"
|
|
neighbors.broadcast("cmd", 22)
|
|
} else if(flight.rc_cmd==21) {
|
|
log("cmd 21")
|
|
log("To land")
|
|
flight.rc_cmd=0
|
|
statef = land
|
|
UAVSTATE = "LAND"
|
|
neighbors.broadcast("cmd", 21)
|
|
} else if(flight.rc_cmd==16) {
|
|
flight.rc_cmd=0
|
|
UAVSTATE = "GOTOGPS"
|
|
statef = goto
|
|
} else if(flight.rc_cmd==400) {
|
|
flight.rc_cmd=0
|
|
uav_arm()
|
|
neighbors.broadcast("cmd", 400)
|
|
} else if (flight.rc_cmd==401){
|
|
flight.rc_cmd=0
|
|
uav_disarm()
|
|
neighbors.broadcast("cmd", 401)
|
|
} else if (flight.rc_cmd==666){
|
|
flight.rc_cmd=0
|
|
stattab_send()
|
|
} else if (flight.rc_cmd==900){
|
|
flight.rc_cmd=0
|
|
rc_State = 0
|
|
neighbors.broadcast("cmd", 900)
|
|
} else if (flight.rc_cmd==901){
|
|
flight.rc_cmd=0
|
|
rc_State = 1
|
|
neighbors.broadcast("cmd", 901)
|
|
} else if (flight.rc_cmd==902){
|
|
flight.rc_cmd=0
|
|
rc_State = 2
|
|
neighbors.broadcast("cmd", 902)
|
|
} else if (flight.rc_cmd==903){
|
|
flight.rc_cmd=0
|
|
rc_State = 3
|
|
neighbors.broadcast("cmd", 903)
|
|
}
|
|
}
|
|
|
|
function uav_neicmd() {
|
|
neighbors.listen("cmd",
|
|
function(vid, value, rid) {
|
|
print("Got (", vid, ",", value, ") #", rid, "(", UAVSTATE, ")")
|
|
if(value==22 and UAVSTATE!="TAKEOFF" and UAVSTATE!="BARRIERWAIT") {
|
|
statef=takeoff
|
|
UAVSTATE = "TAKEOFF"
|
|
} else if(value==21 and UAVSTATE!="LAND" and UAVSTATE!="BARRIERWAIT") {
|
|
statef=land
|
|
UAVSTATE = "LAND"
|
|
} else if(value==400 and UAVSTATE=="TURNEDOFF") {
|
|
uav_arm()
|
|
} else if(value==401 and UAVSTATE=="TURNEDOFF"){
|
|
uav_disarm()
|
|
} else if(value==900){
|
|
rc_State = 0
|
|
} else if(value==901){
|
|
rc_State = 1
|
|
} else if(value==902){
|
|
rc_State = 2
|
|
} else if(value==903){
|
|
rc_State = 3
|
|
} else if(value==16 and UAVSTATE=="IDLE"){
|
|
# neighbors.listen("gt",function(vid, value, rid) {
|
|
# print("Got (", vid, ",", value, ") from robot #", rid)
|
|
# # if(gt.id == id) statef=goto
|
|
# })
|
|
}
|
|
})
|
|
}
|
|
|
|
function LimitAngle(angle){
|
|
if(angle>2*math.pi)
|
|
return angle-2*math.pi
|
|
else if (angle<0)
|
|
return angle+2*math.pi
|
|
else
|
|
return angle
|
|
}
|
|
|
|
function vec_from_gps(lat, lon, home_ref) {
|
|
d_lon = lon - position.longitude
|
|
d_lat = lat - position.latitude
|
|
if(home_ref == 1) {
|
|
d_lon = lon - homegps.long
|
|
d_lat = lat - homegps.lat
|
|
}
|
|
ned_x = d_lat/180*math.pi * 6371000.0;
|
|
ned_y = d_lon/180*math.pi * 6371000.0 * math.cos(lat/180*math.pi);
|
|
#Lgoal.range = math.sqrt(ned_x*ned_x+ned_y*ned_y);
|
|
#Lgoal.bearing = LimitAngle(math.atan(ned_y,ned_x));
|
|
return math.vec2.new(ned_x,ned_y)
|
|
}
|
|
|
|
function gps_from_vec(vec) {
|
|
Lgoal = {.latitude=0, .longitude=0}
|
|
Vrange = math.vec2.length(vec)
|
|
Vbearing = LimitAngle(math.atan(vec.y, vec.x))
|
|
# print("rb2gps: ",Vrange,Vbearing,position.latitude,position.longitude)
|
|
latR = position.latitude*math.pi/180.0;
|
|
lonR = position.longitude*math.pi/180.0;
|
|
target_lat = math.asin(math.sin(latR) * math.cos(Vrange/6371000.0) + math.cos(latR) * math.sin(Vrange/6371000.0) * math.cos(Vbearing));
|
|
target_lon = lonR + math.atan(math.sin(Vbearing) * math.sin(Vrange/6371000.0) * math.cos(latR), math.cos(Vrange/6371000.0) - math.sin(latR) * math.sin(target_lat));
|
|
Lgoal.latitude = target_lat*180.0/math.pi;
|
|
Lgoal.longitude = target_lon*180.0/math.pi;
|
|
#d_lat = (vec.x / 6371000.0)*180.0/math.pi;
|
|
#goal.latitude = d_lat + position.latitude;
|
|
#d_lon = (vec.y / (6371000.0 * math.cos(goal.latitude*math.pi/180.0)))*180.0/math.pi;
|
|
#goal.longitude = d_lon + position.longitude;
|
|
|
|
return Lgoal
|
|
}
|