Bottle_drop: Publish to onboard mission

1 files changed, 37 insertions, 18 deletions

diff --git a/src/modules/bottle_drop/bottle_drop.c b/src/modules/bottle_drop/bottle_drop.c
index bb12674af..8d3d6d599 100644
--- a/src/modules/bottle_drop/bottle_drop.c
+++ b/src/modules/bottle_drop/bottle_drop.c
@@ -61,6 +61,7 @@
 #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>
 
@@ -187,7 +188,7 @@ int bottle_drop_thread_main(int argc, char *argv[]) {
 		double lat; //degrees 1E7
 		double lon; //degrees 1E7
 		float alt; //m
-	} target_position, drop_position, flight_vector_s, flight_vector_e;
+	} target_position, drop_position;
 
 	target_position.lat =  47.385806;
 	target_position.lon =  8.589093;
@@ -238,6 +239,7 @@ int bottle_drop_thread_main(int argc, char *argv[]) {
 	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");
@@ -270,6 +272,22 @@ int bottle_drop_thread_main(int argc, char *argv[]) {
 	memset(&actuators, 0, sizeof(actuators));
 	orb_advert_t actuator_pub = orb_advertise(ORB_ID(actuator_controls_1), &actuators);
 
+	struct mission_s onboard_mission;
+	memset(&onboard_mission, 0, sizeof(onboard_mission));
+	onboard_mission.items = (struct mission_item_s*)malloc(sizeof(struct mission_item_s) * 2);
+	
+	struct mission_item_s *flight_vector_s = &onboard_mission.items[0];
+	struct mission_item_s *flight_vector_e = &onboard_mission.items[1];
+
+	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;
+
+	orb_advert_t onboard_mission_pub = orb_advertise(ORB_ID(onboard_mission), &onboard_mission);
+
 	struct pollfd fds[] = {
 		{ .fd = vehicle_attitude_sub, .events = POLLIN }
 	};
@@ -295,14 +313,6 @@ int bottle_drop_thread_main(int argc, char *argv[]) {
 				/* copy global position */
 				orb_copy(ORB_ID(vehicle_global_position), vehicle_global_position_sub, &globalpos);
 			}
-			//////////////////////////////////////////////////////////////////// DEBUGGING
-			globalpos.lat = 47.384486;
-			globalpos.lon =  8.588239;
-			globalpos.vx = 18.0f;
-			globalpos.vy = 0.0f;
-			globalpos.alt = 60.0f;
-			globalpos.yaw = M_PI_F/2.0f;
-
 
 			orb_check(parameter_update_sub, &updated);
 			if (updated){
@@ -344,7 +354,7 @@ int bottle_drop_thread_main(int argc, char *argv[]) {
 			 //warnx("Initialization complete\n"); //////////////////////////////////////////////////////////////////// DEBUGGING
 
 
-			if (drop_approval && !state_drop)
+			if (drop_approval && !state_drop && counter % 10 == 0)
 			{
 				//warnx("approval given\n"); //////////////////////////////////////////////////////////////////// DEBUGGING
 				// drop here
@@ -352,8 +362,6 @@ int bottle_drop_thread_main(int argc, char *argv[]) {
 				//drop = false;
 				//drop_start = hrt_absolute_time();
 
-				unsigned counter = 0;
-
 				// Compute the distance the bottle will travel after it is dropped in body frame coordinates --> x
 				while( h > 0.05f)
 				{
@@ -402,13 +410,12 @@ int bottle_drop_thread_main(int argc, char *argv[]) {
 
 
 				// 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.alt = 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.alt = height;
+				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
 				distance_real = get_distance_to_next_waypoint(globalpos.lat, globalpos.lon, drop_position.lat, drop_position.lon);
 				map_projection_project(globalpos.lat, globalpos.lon, &x_l, &y_l);
@@ -418,6 +425,18 @@ int bottle_drop_thread_main(int argc, char *argv[]) {
 				future_distance = 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
 
+
+				onboard_mission.count = 2;
+
+				if (state_run && !state_drop) {
+					onboard_mission.current_index = 0;
+				} else {
+					onboard_mission.current_index = -1;
+				}
+				
+
+				orb_publish(ORB_ID(onboard_mission), onboard_mission_pub, &onboard_mission);
+
 			}
 
 			if(distance_real < distance_open_door && drop_approval)
@@ -447,10 +466,10 @@ int bottle_drop_thread_main(int argc, char *argv[]) {
 				}
 			}
 
-
 			actuators.timestamp = hrt_absolute_time();
 			orb_publish(ORB_ID(actuator_controls_1), actuator_pub, &actuators);
 
+			counter++;
 		}
 	}