ROSBuzz_MISTLab/script/testalone.bzz

# We need this for 2D vectors
# Make sure you pass the correct include path to "bzzc -I <path1:path2> ..."
include "vec2.bzz"
####################################################################################################
# Updater related
# This should be here for the updater to work, changing position of code will crash the updater
####################################################################################################
updated="update_ack"
update_no=0
function updated_neigh(){
neighbors.broadcast(updated, update_no)
}

TARGET_ALTITUDE = 5.0
 
# Executed once at init time.
function init() {
}

# Executed at each time step.
function step() {

	if(flight.rc_cmd==22) {
		flight.rc_cmd=0
		uav_takeoff(TARGET_ALTITUDE)		
	} else if(flight.rc_cmd==21) {
		flight.rc_cmd=0
		uav_land()
	} else if(flight.rc_cmd==16) {
		flight.rc_cmd=0
		uav_goto()
	}

# test moveto cmd
#if( flight.status == 2 and position.altitude >= TARGET_ALTITUDE-TARGET_ALTITUDE/20.0)
#	uav_moveto(0.5, 0.5)
}
 
# Executed once when the robot (or the simulator) is reset.
function reset() {
}
 
# Executed once at the end of experiment.
function destroy() {
}