ThreadCmd.java

/**
 *  Catroid: An on-device graphical programming language for Android devices
 *  Copyright (C) 2010  Catroid development team
 *  (<http://code.google.com/p/catroid/wiki/Credits>)
 *
 *  This program is free software: you can redistribute it and/or modify
 *  it under the terms of the GNU General Public License as published by
 *  the Free Software Foundation, either version 3 of the License, or
 *  (at your option) any later version.
 *
 *  This program is distributed in the hope that it will be useful,
 *  but WITHOUT ANY WARRANTY; without even the implied warranty of
 *  MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
 *  GNU General Public License for more details.
 *
 *  You should have received a copy of the GNU General Public License
 *  along with this program.  If not, see <http://www.gnu.org/licenses/>.
 */
package at.tugraz.ist.droned.dcf;


import java.util.Vector;
import java.util.List;

import at.tugraz.ist.droned.Drone;
import at.tugraz.ist.droned.DroneConsts;
import at.tugraz.ist.droned.dcf.command.Command;

public class ThreadCmd extends Thread {

	private WiFiConnection wifi = null;
	private Drone drone = null;
	
	private List<Command> cmdList;

	private int move_armed, old_move_armed;
	private float move_roll, move_pitch, move_altitude, move_yaw;

	private int uiBits = 290717696;

	public ThreadCmd(Drone drone) {
		this.drone = drone;
		wifi = drone.getWifi();
		cmdList = new Vector<Command>();
	}

	@Override
	public void run() {
		Vector<Command> removeList = new Vector<Command>();
		try {
			//Log.d(DroneConsts.DroneLogTag, "CMD Thread started");

			while (wifi.isRunning()) {
				for(Command c: cmdList) {
					removeList = sendCommand(c);
					
					Thread.sleep(wifi.INTERVAL);
				}
				removeCommands(removeList);
				removeList.removeAllElements();
			}
			//Log.d(DroneConsts.DroneLogTag, "CMD Thread stopped");

		} catch (Exception e) {
			wifi.running = false;
			//Log.e(DroneConsts.DroneLogTag, "Error in CMD Thread loop", e);
			if (wifi.socketCmd != null) {
				wifi.socketCmd.close();
			}
		}

	}	
	
	
	public void addCommand(Command c)
	{
		if (c.isUnique()) {
			for (Command lc: cmdList) {

    		if (c.getName().compareTo(lc.getName()) == 0) {
    			lc = c;
    			}
    		}
		}

		if (!isCommandInList(c)) {
			cmdList.add(c);
		}
	}
	
    public Boolean isCommandInList(Command c) {
    	for (Command lc: cmdList) {
    		if (lc == c) {
    			return true;
    		}
    		if (c.isUnique() && (c.getName().compareTo(lc.getName()) == 0)) {
    			return true;
    		}
    	}
		return false;
	}

	
	

	public Vector<Command> sendCommand(Command c)
	{
		int req_number = 0;
		int req_fullfilled = 0;
		Vector<Command> removeList = new Vector<Command>();
		
		if (c.getRepetitionType().contains(Command.REPETITION_UNTIL_FLYING)) {
			++req_number;
			if (drone.getFlyingState() == DroneConsts.FlyingState.FLYING) {
				req_fullfilled++;
			}
			

		}
		// TODO : requirements for Command.REPETITION_*
		
		if (c.getRepetitionType()!= Command.REPETITION_UNLIMITED && (
			c.getRepetitionType().contains(Command.REPETITION_ONCE) ||
			req_fullfilled == req_number)) {
			removeList.add(c);
		}
		
		try {
			wifi.sendAtCommand(c.getDroneCommand());
			
		}
		catch (Exception e) {
			wifi.running = false;
				//Log.e(DroneConsts.DroneLogTag, "Error in CMD Thread loop", e);				
			if (wifi.socketCmd != null) {
				wifi.socketCmd.close();
			}
		}
		return removeList;
		
	}
	
	public void removeCommands(Vector<Command> removeList)
	{
		cmdList.removeAll(removeList);
	}

	public void setAxisMove(float roll, float pitch, float altitude, float yaw) {
		int enabled = 0;

		if((roll >= -1) && (roll <= 1)
				&& (pitch >= -1) && (pitch <= 1)
				&& (altitude >= -1) && (altitude <= 1)
				&& (yaw >= -1) && (yaw <= 1))
		{
			if (roll == 0 && pitch == 0 && altitude == 0 && yaw == 0) {
				enabled = 0;
			} else {
				enabled = 1;
			}

			synchronized (this) {
				move_armed = enabled;
				move_roll = roll;
				move_pitch = pitch;
				move_altitude = altitude;
				move_yaw = yaw;
			}
		}
		else
		{
			//Log.e(DroneConsts.DroneLogTag, "ThreadCmd -> setAxisMove() got parameters out of bounds");
		}

	}

	public void setHovering(boolean hovering) {
		if (hovering) {
			uiBits = 290718208;
		} else {
			uiBits = 290717696;
		}
	}

	public String createTakeOffLandCmd() {
		return "AT*REF=#SEQ#," + Integer.toString(uiBits);
	}
/*
	public String createMoveCmd() {
		String cmd = "";
		synchronized (this) {
			cmd = "AT*PCMD=#SEQ#," + (move_armed) + "," + Float.floatToIntBits(move_roll) + ","
			        + Float.floatToIntBits(move_pitch) + "," + Float.floatToIntBits(move_altitude) + ","
			        + Float.floatToIntBits(move_yaw);
		}
		return cmd;
	}
	*/
}