ROSBuzz_MISTLab/buzz_scripts/include/act/states.bzz

########################################
#
# FLIGHT-RELATED FUNCTIONS
#
########################################
include "utils/vec2.bzz"
include "act/barrier.bzz"
include "utils/conversions.bzz"

TARGET_ALTITUDE = 15.0 # m.
BVMSTATE = "TURNEDOFF"
PICTURE_WAIT = 20 # steps
GOTO_MAXVEL = 1.5 # m/steps
GOTO_MAXDIST = 150 # m.
GOTODIST_TOL = 0.5  # m.
GOTOANG_TOL = 0.1 # rad.
path_it = 0
rc_State = 0
pic_time = 0
g_it = 0

function turnedoff() {
    BVMSTATE = "TURNEDOFF"
}

function idle() {
    BVMSTATE = "IDLE"
}

function launch() {
	BVMSTATE = "LAUNCH"
	if(V_TYPE == 0 or V_TYPE == 1) {	# flying vehicle so LAND
		homegps = {.lat=pose.position.latitude, .long=pose.position.longitude}
		if( flight.status == 2 and pose.position.altitude >= TARGET_ALTITUDE-TARGET_ALTITUDE/20.0) {
			barrier_set(ROBOTS, AUTO_LAUNCH_STATE, "LAUNCH")
			barrier_ready()
		} else {
			log("Altitude: ", pose.position.altitude)
			neighbors.broadcast("cmd", 22)
			uav_takeoff(TARGET_ALTITUDE)
		}
	} else {
		barrier_set(ROBOTS, AUTO_LAUNCH_STATE, "LAUNCH")
		barrier_ready()
	}
}

function stop() {
    BVMSTATE = "STOP"
	if(V_TYPE == 0 or V_TYPE == 1) {	# flying vehicle so LAND
		neighbors.broadcast("cmd", 21)
		uav_land()
		if(flight.status != 2 and flight.status != 3) {
			barrier_set(ROBOTS,"TURNEDOFF","STOP")
			barrier_ready()
		}
	} else {
			barrier_set(ROBOTS,"TURNEDOFF","STOP")
			barrier_ready()
	}	
}

function take_picture() {
  BVMSTATE="PICTURE"
  uav_setgimbal(0.0, 0.0, -90.0, 20.0)
	if(pic_time==PICTURE_WAIT/2) { # wait for the drone to stabilize
	  uav_takepicture()
  } else if(pic_time>=PICTURE_WAIT) { # wait for the picture
  BVMSTATE="IDLE"
    pic_time=0 
  }
  pic_time=pic_time+1
}

function goto_gps(transf) {
	m_navigation = vec_from_gps(rc_goto.latitude, rc_goto.longitude, 0)
	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))
		goto_abs(m_navigation.x, m_navigation.y, rc_goto.altitude - pose.position.altitude)
	} else if(math.vec2.length(m_navigation) < GOTODIST_TOL and math.vec2.angle(m_navigation) < GOTOANG_TOL) # reached destination
		transf()
	else
		goto_abs(m_navigation.x, m_navigation.y, rc_goto.altitude - pose.position.altitude)
}

function follow() {
	if(size(targets)>0) {
		BVMSTATE = "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))
		})
		goto_abs(attractor.x, attractor.y, 0.0)
	} else {
		log("No target in local table!")
		BVMSTATE = "IDLE"
	}
}

function rc_cmd_listen() {
    if(flight.rc_cmd==22) {
		log("cmd 22")
		flight.rc_cmd=0
		BVMSTATE = "LAUNCH"		
		neighbors.broadcast("cmd", 22)
	} else if(flight.rc_cmd==21) {
		log("cmd 21")
		flight.rc_cmd=0
		BVMSTATE = "STOP"
		neighbors.broadcast("cmd", 21)
	} else if(flight.rc_cmd==16) {
		flight.rc_cmd=0
		BVMSTATE = "PATHPLAN"
	} 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 nei_cmd_listen() {
    neighbors.listen("cmd",
			function(vid, value, rid) {
				print("Got (", vid, ",", value, ") #", rid, "(", BVMSTATE, ")")
				if(value==22 and BVMSTATE!="BARRIERWAIT") {
		      		BVMSTATE = "LAUNCH"
				} else if(value==21 and BVMSTATE!="BARRIERWAIT") {
		      		BVMSTATE = "STOP"
				} else if(value==400 and BVMSTATE=="TURNEDOFF") {
					uav_arm()
				} else if(value==401 and BVMSTATE=="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 BVMSTATE=="IDLE"){
				#  neighbors.listen("gt",function(vid, value, rid) {
				#    print("Got (", vid, ",", value, ") from robot #", rid)
				#    # if(gt.id == id) statef=goto
				#  }) 
				}
    })
}