######################################################################################################
## File created on 31-03-2016 to be used in creating waypoint mission file for Pixhawk FC			##
## By <NAME> fir ISS-Drone																	##
## Sample file taken from http://qgroundcontrol.org/mavlink/waypoint_protocol#waypoint_file_format	##
## MAV_CMD_NAV_WAYPOINT																				##
######################################################################################################
from os.path import abspath
import pdb

def createFile(map,altitudeRel, altitudeAbs):
	version = '110'									# version of firmware currently in use
	missionFile = abspath("mission.txt")
	#missionFile = 'C:\\Users\\naiti\\Desktop\\mission.txt'	# name of mission file and path which will be sent to pixhawk
	f = open(missionFile,"w")						#open a file in write mode
	f.write('QGC\tWPL\t%s\n' % version)				# write first line for waypoint file
	listLen = len(map)							# getting length of waypoints gather in list
	
	# Loop for all point which will be print in each line
	for i in range(listLen):
		#pdb.set_trace()
		mapi = map[i]				#take 1st string from List input
		coma = mapi.find(',')		# Find separator , between lat and long
		totlen = len(mapi)-1
		lat = float(mapi[1:coma])	# lat is between 1st and coma position
		long = float(mapi[coma+1:totlen])	# long is between coma+1 and last-1 position
		if i == 0:																		# First line should be start position hence param 1 should be 1 (arm)
			f.write('%d\t1\t0\t16\t0.14999999999999994\t0\t0\t0\t%.17f\t%.17f\t%d\t1\n' % (i, lat, long, altitudeAbs))
		elif i == 1:
			f.write('%d\t0\t0\t16\t0.14999999999999994\t0\t0\t0\t%.17f\t%.17f\t%d\t1\n' % (i,lat, long, (altitudeAbs+altitudeRel)))		# Takeoff
		else:
			f.write('%d\t0\t0\t16\t0.14999999999999994\t0\t0\t0\t%.17f\t%.17f\t%d\t1\n' % (i,lat, long, altitudeRel))					# Fly
	f.close()										# close the file after write completes

if __name__ == "__main__":
	try:
		map =["(123.12222,234.2323232)","(23423.231212,1213.1213432)","(54545.23434234,23423.23432424)"]
		altitudeRel = 10
		altitudeAbs = 30
		createFile(map,altitudeRel, altitudeAbs)
	except KeyboardInterrupt:
		pass

######################################################################################################
## File created on 31-03-2016 to be used in creating waypoint mission file for Pixhawk FC			##
## By <NAME> fir ISS-Drone																	##
## Sample file taken from http://qgroundcontrol.org/mavlink/waypoint_protocol#waypoint_file_format	##
## MAV_CMD_NAV_WAYPOINT																				##
######################################################################################################
from os.path import abspath
import pdb

def createFile(map,altitudeRel, altitudeAbs):
	version = '110'									# version of firmware currently in use
	missionFile = abspath("mission.txt")
	#missionFile = 'C:\\Users\\naiti\\Desktop\\mission.txt'	# name of mission file and path which will be sent to pixhawk
	f = open(missionFile,"w")						#open a file in write mode
	f.write('QGC\tWPL\t%s\n' % version)				# write first line for waypoint file
	listLen = len(map)							# getting length of waypoints gather in list
	
	# Loop for all point which will be print in each line
	for i in range(listLen):
		#pdb.set_trace()
		mapi = map[i]				#take 1st string from List input
		coma = mapi.find(',')		# Find separator , between lat and long
		totlen = len(mapi)-1
		lat = float(mapi[1:coma])	# lat is between 1st and coma position
		long = float(mapi[coma+1:totlen])	# long is between coma+1 and last-1 position
		if i == 0:																		# First line should be start position hence param 1 should be 1 (arm)
			f.write('%d\t1\t0\t16\t0.14999999999999994\t0\t0\t0\t%.17f\t%.17f\t%d\t1\n' % (i, lat, long, altitudeAbs))
		elif i == 1:
			f.write('%d\t0\t0\t16\t0.14999999999999994\t0\t0\t0\t%.17f\t%.17f\t%d\t1\n' % (i,lat, long, (altitudeAbs+altitudeRel)))		# Takeoff
		else:
			f.write('%d\t0\t0\t16\t0.14999999999999994\t0\t0\t0\t%.17f\t%.17f\t%d\t1\n' % (i,lat, long, altitudeRel))					# Fly
	f.close()										# close the file after write completes

if __name__ == "__main__":
	try:
		map =["(123.12222,234.2323232)","(23423.231212,1213.1213432)","(54545.23434234,23423.23432424)"]
		altitudeRel = 10
		altitudeAbs = 30
		createFile(map,altitudeRel, altitudeAbs)
	except KeyboardInterrupt:
		pass

###################################################################################################### ## File created on 31-03-2016 to be used in creating waypoint mission file for Pixhawk FC			## ## By <NAME> fir ISS-Drone																	## ## Sample file taken from http://qgroundcontrol.org/mavlink/waypoint_protocol#waypoint_file_format	## ## MAV_CMD_NAV_WAYPOINT																				## ###################################################################################################### # version of firmware currently in use #missionFile = 'C:\\Users\\naiti\\Desktop\\mission.txt'	# name of mission file and path which will be sent to pixhawk #open a file in write mode # write first line for waypoint file # getting length of waypoints gather in list # Loop for all point which will be print in each line #pdb.set_trace() #take 1st string from List input # Find separator , between lat and long # lat is between 1st and coma position # long is between coma+1 and last-1 position # First line should be start position hence param 1 should be 1 (arm) # Takeoff # Fly # close the file after write completes