path: root/src/modules/bottle_drop/bottle_drop.c

/****************************************************************************
 *
 *   Copyright (c) 2012, 2013 PX4 Development Team. All rights reserved.
 *
 * Redistribution and use in source and binary forms, with or without
 * modification, are permitted provided that the following conditions
 * are met:
 *
 * 1. Redistributions of source code must retain the above copyright
 *    notice, this list of conditions and the following disclaimer.
 * 2. Redistributions in binary form must reproduce the above copyright
 *    notice, this list of conditions and the following disclaimer in
 *    the documentation and/or other materials provided with the
 *    distribution.
 * 3. Neither the name PX4 nor the names of its contributors may be
 *    used to endorse or promote products derived from this software
 *    without specific prior written permission.
 *
 * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
 * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
 * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
 * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
 * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
 * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
 * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
 * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
 * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
 * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
 * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
 * POSSIBILITY OF SUCH DAMAGE.
 *
 ****************************************************************************/

/**
 * @file bottle_drop.c
 * bottle_drop application
 * 
 * @author Dominik Juchli <juchlid@ethz.ch>
 */

#include <nuttx/config.h>
#include <nuttx/sched.h>
#include <unistd.h>
#include <stdio.h>
#include <stdlib.h>
#include <poll.h>
#include <stdbool.h>
#include <fcntl.h>
#include <string.h>
#include <math.h>


#include <systemlib/systemlib.h>
#include <systemlib/err.h>
#include <systemlib/param/param.h>

#include <geo/geo.h>
#include <dataman/dataman.h>

#include <uORB/uORB.h>
#include <uORB/topics/actuator_controls.h>
#include <uORB/topics/vehicle_attitude.h>
#include <uORB/topics/vehicle_global_position.h>
#include <uORB/topics/parameter_update.h>
#include <uORB/topics/mission.h>

#include <drivers/drv_hrt.h>

PARAM_DEFINE_FLOAT(BD_HEIGHT, 60.0f);
PARAM_DEFINE_FLOAT(BD_GPROPERTIES, 0.03f);
PARAM_DEFINE_FLOAT(BD_TURNRADIUS, 70.0f);
PARAM_DEFINE_FLOAT(BD_PRECISION, 1.0f);
PARAM_DEFINE_INT32(BD_APPROVAL, 0);




static bool thread_should_exit = false;		/**< daemon exit flag */
static bool thread_running = false;		/**< daemon status flag */
static int daemon_task;				/**< Handle of daemon task / thread */

static bool drop = false;

/**
 * daemon management function.
 */
__EXPORT int bottle_drop_main(int argc, char *argv[]);

/**
 * Mainloop of daemon.
 */
int bottle_drop_thread_main(int argc, char *argv[]);

/**
 * Print the correct usage.
 */
static void usage(const char *reason);

static void
usage(const char *reason)
{
	if (reason)
		warnx("%s\n", reason);
	errx(1, "usage: daemon {start|stop|status} [-p <additional params>]\n\n");
}

/**
 * The daemon app only briefly exists to start
 * the background job. The stack size assigned in the
 * Makefile does only apply to this management task.
 * 
 * The actual stack size should be set in the call
 * to task_create().
 */
int bottle_drop_main(int argc, char *argv[])
{
	if (argc < 1)
		usage("missing command");

	if (!strcmp(argv[1], "start")) {

		if (thread_running) {
			warnx("daemon already running\n");
			/* this is not an error */
			exit(0);
		}

		thread_should_exit = false;
		daemon_task = task_spawn_cmd("bottle_drop",
					 SCHED_DEFAULT,
					 SCHED_PRIORITY_DEFAULT,
					 4096,
					 bottle_drop_thread_main,
					 (argv) ? (const char **)&argv[2] : (const char **)NULL);
		exit(0);
	}

	if (!strcmp(argv[1], "drop")) {
		drop = true;
		exit(0);
	}

	if (!strcmp(argv[1], "stop")) {
		thread_should_exit = true;
		exit(0);
	}

	if (!strcmp(argv[1], "status")) {
		if (thread_running) {
			warnx("\trunning\n");
		} else {
			warnx("\tnot started\n");
		}
		exit(0);
	}

	usage("unrecognized command");
	exit(1);
}

int bottle_drop_thread_main(int argc, char *argv[]) {

	warnx("starting\n");

	bool updated = false;

	float height;		// height at which the normal should be dropped NED
	float z_0;		// ground properties
	float turn_radius;	// turn radius of the UAV
	float precision;	// Expected precision of the UAV
	bool drop_approval;	// if approval is given = true, otherwise = false

	thread_running = true;

	/* XXX TODO: create, publish and read in wind speed topic */
	struct wind_speed_s {
		float vx; // m/s
		float vy; // m/s
		float altitude; // m
	} wind_speed;

	wind_speed.vx = 4.2f;
	wind_speed.vy = 0.0f;
	wind_speed.altitude = 62.0f;


	/* XXX TODO: create, publish and read in target position in NED*/
	struct position_s {
		double lat; //degrees 1E7
		double lon; //degrees 1E7
		float alt; //m
	} target_position, drop_position;

	target_position.lat =  47.385806;
	target_position.lon =  8.589093;
	target_position.alt = 0.0f;


	// constant
	float g = 9.81f;               		// constant of gravity [m/s^2]
	float m = 0.5f;                		// mass of bottle [kg]
	float rho = 1.2f;              		// air density [kg/m^3]
	float A = (powf(0.063f, 2.0f)/4.0f*M_PI_F);     // Bottle cross section [m^2]
	float dt = 0.01f;             		// step size [s]
	float dt2 = 0.05f;			// step size 2 [s]

	// Has to be estimated by experiment
	float cd = 0.86f;              	// Drag coefficient for a cylinder with a d/l ratio of 1/3 []
	float t_signal = 0.084f;	// Time span between sending the signal and the bottle top reaching level height with the bottom of the plane [s]
	float t_door = 0.7f;		// The time the system needs to open the door + safety, is also the time the palyload needs to safely escape the shaft [s]


	// Definition
	float h_0;						// height over target
	float az;                 				// acceleration in z direction[m/s^2]
	float vz; 						// velocity in z direction [m/s]
	float z; 						// fallen distance [m]
	float h; 						// height over target [m]
	float ax;						// acceleration in x direction [m/s^2]
	float vx;						// ground speed in x direction [m/s]
	float x;					        // traveled distance in x direction [m]
	float vw;                 				// wind speed [m/s]
	float vrx;						// relative velocity in x direction [m/s]
	float v;						// relative speed vector [m/s]
	float Fd;						// Drag force [N]
	float Fdx;						// Drag force in x direction [N]
	float Fdz;						// Drag force in z direction [N]
	float vr; 						// absolute wind speed [m/s]
	float x_drop, y_drop;					// coordinates of the drop point in reference to the target (projection of NED)
	float x_t,y_t;						// coordinates of the target in reference to the target x_t = 0, y_t = 0 (projection of NED)
	float x_l,y_l;						// local position in projected coordinates
	float x_f,y_f;						// to-be position of the UAV after dt2 seconds in projected coordinates
	double x_f_NED, y_f_NED;				// to-be position of the UAV after dt2 seconds in NED
	float distance_open_door;				// The distance the UAV travels during its doors open [m]
	float distance_real = 0;				// The distance between the UAVs position and the drop point [m]
	float future_distance = 0;				// The distance between the UAVs to-be position and the drop point [m]

	// states
	bool state_door = false;				// Doors are closed = false, open = true
	bool state_drop = false;				// Drop occurred = true, Drop din't occur = false
	bool state_run = false;					// A drop was attempted = true, the drop is still in progress = false

	unsigned counter = 0;

	param_t param_height = param_find("BD_HEIGHT");
	param_t param_gproperties = param_find("BD_GPROPERTIES");
	param_t param_turn_radius = param_find("BD_TURNRADIUS");
	param_t param_precision = param_find("BD_PRECISION");
	param_t param_approval = param_find("BD_APPROVAL");


	param_get(param_approval, &drop_approval);
	param_get(param_precision, &precision);
	param_get(param_turn_radius, &turn_radius);
	param_get(param_height, &height);
	param_get(param_gproperties, &z_0);


	struct vehicle_attitude_s att;
	memset(&att, 0, sizeof(att));
	int vehicle_attitude_sub = orb_subscribe(ORB_ID(vehicle_attitude));
	orb_set_interval(vehicle_attitude_sub, 20);

	struct vehicle_global_position_s globalpos;
	memset(&globalpos, 0, sizeof(globalpos));
	int vehicle_global_position_sub = orb_subscribe(ORB_ID(vehicle_global_position));

	struct parameter_update_s update;
	memset(&update, 0, sizeof(update));
	int parameter_update_sub = orb_subscribe(ORB_ID(parameter_update));

	struct actuator_controls_s actuators;
	memset(&actuators, 0, sizeof(actuators));
	orb_advert_t actuator_pub = orb_advertise(ORB_ID(actuator_controls_1), &actuators);
	
	struct mission_item_s flight_vector_s;
	struct mission_item_s flight_vector_e;

	flight_vector_s.nav_cmd = NAV_CMD_WAYPOINT;
	flight_vector_s.radius = 50; // TODO: make parameter
	flight_vector_s.autocontinue = true;
	flight_vector_e.nav_cmd = NAV_CMD_WAYPOINT;
	flight_vector_e.radius = 50; // TODO: make parameter
	flight_vector_e.autocontinue = true;

	struct mission_s onboard_mission;
	memset(&onboard_mission, 0, sizeof(onboard_mission));

	orb_advert_t onboard_mission_pub = orb_advertise(ORB_ID(onboard_mission), &onboard_mission);

	struct pollfd fds[] = {
		{ .fd = vehicle_attitude_sub, .events = POLLIN }
	};

	double latitude;
	double longitude;


	while (!thread_should_exit) {

//		warnx("in while!\n");
		// values from -1 to 1

		int ret = poll(fds, 1, 500);

		if (ret < 0) {
			/* poll error, count it in perf */
			warnx("poll error");

		} else if (ret > 0) {

			orb_copy(ORB_ID(vehicle_attitude), vehicle_attitude_sub, &att);

			orb_check(vehicle_global_position_sub, &updated);
			if (updated){
				/* copy global position */
				orb_copy(ORB_ID(vehicle_global_position), vehicle_global_position_sub, &globalpos);

				latitude = (double)globalpos.lat / 1e7;
				longitude = (double)globalpos.lon / 1e7;
			}

			orb_check(parameter_update_sub, &updated);
			if (updated){
				/* copy global position */
				orb_copy(ORB_ID(parameter_update), parameter_update_sub, &update);

				/* update all parameters */
			param_get(param_height, &height);
			param_get(param_gproperties, &z_0);
			param_get(param_turn_radius, &turn_radius);
			param_get(param_approval, &drop_approval);
			param_get(param_precision, &precision);


			}

			// Initialization
			
			
			vr = sqrtf(powf(wind_speed.vx,2) + powf(wind_speed.vy,2)); // absolute wind speed [m/s]
			distance_open_door = fabsf(t_door * globalpos.vx);


			 //warnx("absolut wind speed = %.4f", vr); //////////////////////////////////////////////////////////////////// DEBUGGING


			 //warnx("Initialization complete\n"); //////////////////////////////////////////////////////////////////// DEBUGGING


			if (drop_approval && !state_drop)
			{
				//warnx("approval given\n"); //////////////////////////////////////////////////////////////////// DEBUGGING
				// drop here
				//open_now = true;
				//drop = false;
				//drop_start = hrt_absolute_time();


				if (counter % 50 == 0) {

					az = g;                 				// acceleration in z direction[m/s^2]
					vz = 0; 						// velocity in z direction [m/s]
					z = 0; 							// fallen distance [m]
					h_0 = globalpos.alt - target_position.alt; 		// height over target at start[m]
					h = h_0;						// height over target [m]
					ax = 0;							// acceleration in x direction [m/s^2]
					vx = sqrtf(powf(globalpos.vx,2.0f) + powf(globalpos.vy,2.0f));					// ground speed in x direction [m/s]
					x = 0;							// traveled distance in x direction [m]
					vw = 0;                 				// wind speed [m/s]
					vrx = 0;						// relative velocity in x direction [m/s]
					v = sqrtf(powf(globalpos.vx,2.0f) + powf(globalpos.vy,2.0f));					// relative speed vector [m/s]
					Fd = 0;							// Drag force [N]
					Fdx = 0;						// Drag force in x direction [N]
					Fdz = 0;						// Drag force in z direction [N]


					// Compute the distance the bottle will travel after it is dropped in body frame coordinates --> x
					while( h > 0.05f)
					{
						// z-direction
						vz = vz + az*dt;
						z = z + vz*dt;
						h = h_0 - z;

						// x-direction
						vw = vr*logf(h/z_0)/logf(wind_speed.altitude/z_0);
						vx = vx + ax*dt;
						x = x + vx*dt;
						vrx = vx + vw;

						//Drag force
						v = sqrtf(powf(vz,2.0f) + powf(vrx,2.0f));
						Fd = 0.5f*rho*A*cd*powf(v,2.0f);
						Fdx = Fd*vrx/v;
						Fdz = Fd*vz/v;

						//acceleration
						az = g - Fdz/m;
						ax = -Fdx/m;
					}
					// Compute Drop point
					x = sqrtf(powf(globalpos.vx,2.0f) + powf(globalpos.vy,2.0f))*t_signal + x;
					map_projection_init(target_position.lat, target_position.lon);
				}

				map_projection_project(target_position.lat, target_position.lon, &x_t, &y_t);
				if( vr < 0.001f)		// if there is no wind, an arbitrarily direction is chosen
				{
					vr = 1;
					wind_speed.vx = 1;
					wind_speed.vy = 0;
				}
				x_drop = x_t + x*wind_speed.vx/vr;
				y_drop = y_t + x*wind_speed.vy/vr;
				map_projection_reproject(x_drop, y_drop, &drop_position.lat, &drop_position.lon);
				drop_position.alt = height;
				//warnx("drop point: lat = %.7f , lon = %.7f", drop_position.lat, drop_position.lon); //////////////////////////////////////////////////////////////////// DEBUGGING



				// Compute flight vector
				map_projection_reproject(x_drop + 2*turn_radius*wind_speed.vx/vr, y_drop + 2*turn_radius*wind_speed.vy/vr, &(flight_vector_s.lat), &(flight_vector_s.lon));
				flight_vector_s.altitude = height;
				map_projection_reproject(x_drop - turn_radius*wind_speed.vx/vr, y_drop - turn_radius*wind_speed.vy/vr, &flight_vector_e.lat, &flight_vector_e.lon);
				flight_vector_e.altitude = height;
				//warnx("Flight vector: starting point = %.7f  %.7f , end point = %.7f  %.7f", flight_vector_s.lat,flight_vector_s.lon,flight_vector_e.lat,flight_vector_e.lon); //////////////////////////////////////////////////////////////////// DEBUGGING

				// Drop Cancellation if terms are not met

				// warnx("latitude:%.2f", latitude);
				// warnx("longitude:%.2f", longitude);
				// warnx("drop_position.lat:%.2f", drop_position.lat);
				// warnx("drop_position.lon:%.2f", drop_position.lon);

				distance_real = fabsf(get_distance_to_next_waypoint(latitude, longitude, drop_position.lat, drop_position.lon));
				map_projection_project(latitude, longitude, &x_l, &y_l);
				x_f = x_l + globalpos.vx*dt2;
				y_f = y_l + globalpos.vy*dt2;
				map_projection_reproject(x_f, y_f, &x_f_NED, &y_f_NED);
				future_distance = fabsf(get_distance_to_next_waypoint(x_f_NED, y_f_NED, drop_position.lat, drop_position.lon));
				//warnx("position to-be: = %.7f  %.7f" ,x_f_NED, y_f_NED ); //////////////////////////////////////////////////////////////////// DEBUGGING

				// if (counter % 10 ==0) {
				// 	warnx("x: %.4f", x);
				// 	warnx("drop_position.lat: %.4f, drop_position.lon: %.4f", drop_position.lat, drop_position.lon);
				// 	warnx("latitude %.4f, longitude: %.4f", latitude, longitude);
				// 	warnx("future_distance: %.2f, precision: %.2f", future_distance, precision);
				// }

				/* Save WPs in datamanager */
				const size_t len = sizeof(struct mission_item_s);

				if (dm_write(DM_KEY_WAYPOINTS_ONBOARD, 0, DM_PERSIST_IN_FLIGHT_RESET, &flight_vector_s, len) != len) {
					warnx("ERROR: could not save onboard WP");
				}

				if (dm_write(DM_KEY_WAYPOINTS_ONBOARD, 1, DM_PERSIST_IN_FLIGHT_RESET, &flight_vector_e, len) != len) {
					warnx("ERROR: could not save onboard WP");
				}

				onboard_mission.count = 2;

				if (state_run && !state_drop) {
					onboard_mission.current_index = 0;
				} else {
					onboard_mission.current_index = -1;
				}
				
				if (counter % 10 ==0)
					warnx("Distance real: %.2f, distance_open_door: %.2f, angle to wind: %.2f", distance_real, distance_open_door, fabsf(_wrap_pi(globalpos.yaw-atan2f(wind_speed.vy,wind_speed.vx)+M_PI_F)));

				orb_publish(ORB_ID(onboard_mission), onboard_mission_pub, &onboard_mission);

			}

			if(isfinite(distance_real) && distance_real < distance_open_door && drop_approval)
			{
				actuators.control[0] = -1.0f;	// open door
				actuators.control[1] = 1.0f;
				state_door = true;
				warnx("open doors");
			}
			else
			{		// closed door and locked survival kit
				actuators.control[0] = 0.5f;
				actuators.control[1] = -0.5f;
				actuators.control[2] = -0.5f;
				state_door = false;
			}
			if(isfinite(distance_real) && distance_real < precision && distance_real < future_distance && state_door)  // Drop only if the distance between drop point and actual position is getting larger again
			{
				if(fabsf(_wrap_pi(globalpos.yaw-atan2f(wind_speed.vy,wind_speed.vx)+M_PI_F)) < 10.0f/180.0f*M_PI_F) // if flight trajectory deviates more than 10 degrees from calculated path, it will no drop
				{
					actuators.control[2] = 0.5f;
					state_drop = true;
					state_run = true;
					warnx("dropping now");
				}
				else
				{
					state_run = true;
				}
			}

			actuators.timestamp = hrt_absolute_time();
			orb_publish(ORB_ID(actuator_controls_1), actuator_pub, &actuators);

			counter++;
		}
	}

	warnx("exiting.\n");

	thread_running = false;


	return 0;
}