# We need this for 2D vectors
# Make sure you pass the correct include path to "bzzc -I <path1:path2> ..."
#include "/home/ubuntu/buzz/src/include/vec2.bzz"
#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() {
	log("Altitude: ", position.altitude)
	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() {
}