feat : print next target information

이충섭
Commit 67e0e540578a5063003fc825549cb9cbc171d144 67e0e540 1 parent 3a8b2bcd
Showing 2 changed files with 21 additions and 54 deletions
home/Firmware/src/modules/navigator/mission.cpp
home/Firmware/src/modules/navigator/mission.h
--- a/home/Firmware/src/modules/navigator/mission.cpp
View file @67e0e54
+++ b/home/Firmware/src/modules/navigator/mission.cpp
View file @67e0e54
@@ -47,7 +47,7 @@
 #include "mission.h"
 #include "navigator.h"
-#include "iostream"
+#include <iostream>
 #include <string.h>
 #include <drivers/drv_hrt.h>
 #include <dataman/dataman.h>
@@ -381,7 +381,6 @@ Mission::set_execution_mode(const uint8_t mode)
 		_mission_execution_mode = mode;
 	}
 }
-
 bool
 Mission::find_mission_land_start()
 {
@@ -621,11 +620,12 @@ Mission::set_mission_items()
 	struct mission_item_s mission_item_after_next_position;
 	bool has_next_position_item = false;
 	bool has_after_next_position_item = false;
-
 	work_item_type new_work_item_type = WORK_ITEM_TYPE_DEFAULT;
 	if (prepare_mission_items(&_mission_item, &mission_item_next_position, &has_next_position_item,
 				  &mission_item_after_next_position, &has_after_next_position_item)) {
+		print_next_mission(&mission_item_next_position);
+
 		/* if mission type changed, notify */
 		if (_mission_type != MISSION_TYPE_MISSION) {
 			mavlink_log_info(_navigator->get_mavlink_log_pub(),
@@ -633,7 +633,6 @@ Mission::set_mission_items()
 					 "Executing Mission");
 			user_feedback_done = true;
 		}
-
 		_mission_type = MISSION_TYPE_MISSION;
 	} else {
@@ -1031,30 +1030,6 @@ Mission::set_mission_items()
 					generate_waypoint_from_heading(&pos_sp_triplet->current, pos_sp_triplet->current.yaw);
 				}
-				/* don't advance mission after FW to MC command */
-				if (_mission_item.nav_cmd == NAV_CMD_DO_VTOL_TRANSITION
-				    && _work_item_type == WORK_ITEM_TYPE_DEFAULT
-				    && new_work_item_type == WORK_ITEM_TYPE_DEFAULT
-				    && _navigator->get_vstatus()->vehicle_type == vehicle_status_s::VEHICLE_TYPE_FIXED_WING
-				    && !_navigator->get_land_detected()->landed
-				    && pos_sp_triplet->current.valid) {
-
-					new_work_item_type = WORK_ITEM_TYPE_CMD_BEFORE_MOVE;
-				}
-
-				/* after FW to MC transition finish moving to the waypoint */
-				if (_work_item_type == WORK_ITEM_TYPE_CMD_BEFORE_MOVE &&
-				    new_work_item_type == WORK_ITEM_TYPE_DEFAULT
-				    && pos_sp_triplet->current.valid) {
-
-					new_work_item_type = WORK_ITEM_TYPE_DEFAULT;
-
-					_mission_item.nav_cmd = NAV_CMD_WAYPOINT;
-					copy_position_if_valid(&_mission_item, &pos_sp_triplet->current);
-					_mission_item.autocontinue = true;
-					_mission_item.time_inside = 0.0f;
-				}
-
 				// ignore certain commands in mission fast forward
 				if ((_mission_execution_mode == mission_result_s::MISSION_EXECUTION_MODE_FAST_FORWARD) &&
 				    (_mission_item.nav_cmd == NAV_CMD_DELAY)) {
@@ -1461,17 +1436,14 @@ Mission::prepare_mission_items(struct mission_item_s *mission_item,
 	if (_mission_execution_mode == mission_result_s::MISSION_EXECUTION_MODE_REVERSE) {
 		offset = -1;
 	}
-
 	if (read_mission_item(0, mission_item)) {
-
 		first_res = true;
-
 		/* trying to find next position mission item */
 		while (read_mission_item(offset, next_position_mission_item)) {
-			if (_mission_execution_mode == mission_result_s::MISSION_EXECUTION_MODE_REVERSE) {
+			if (_mission_execution_mode == mission_result_s::MISSION_EXECUTION_MODE_REVERSE) {\
-				offset--;
+			offset--;
-
+			}
-			} else {
+			else {
 				offset++;
 			}
@@ -1480,13 +1452,11 @@ Mission::prepare_mission_items(struct mission_item_s *mission_item,
 				break;
 			}
 		}
-
 		if (_mission_execution_mode != mission_result_s::MISSION_EXECUTION_MODE_REVERSE &&
 		    after_next_position_mission_item && has_after_next_position_item) {
 			/* trying to find next next position mission item */
 			while (read_mission_item(offset, after_next_position_mission_item)) {
 				offset++;
-
 				if (item_contains_position(*after_next_position_mission_item)) {
 					*has_after_next_position_item = true;
 					break;
@@ -1497,14 +1467,24 @@ Mission::prepare_mission_items(struct mission_item_s *mission_item,
 	return first_res;
 }
+bool
+Mission::print_next_mission(struct mission_item_s *mis)
+{
+	std::cout <<"----------------------target--------------------------"<<std::endl;
+	std::cout << "lat : "<<mis->lat <<std:: endl;
+	std::cout << "lon : "<<mis->lon <<std:: endl;
+	std::cout << "alt : "<<mis->altitude <<std:: endl;
+	//std::cout << "[whatever] "<<mis->timestamp <<std:: endl;
+	mission_index++;
+	return 0;
+}
 bool
 Mission::read_mission_item(int offset, struct mission_item_s *mission_item)
 {
 	/* select mission */
 	const int current_index = _current_mission_index;
 	int index_to_read = current_index + offset;
-
 	int *mission_index_ptr = (offset == 0) ? &_current_mission_index : &index_to_read;
 	const dm_item_t dm_item = (dm_item_t)_mission.dataman_id;
@@ -1584,7 +1564,6 @@ Mission::read_mission_item(int offset, struct mission_item_s *mission_item)
 			return true;
 		}
 	}
-
 	/* we have given up, we don't want to cycle forever */
 	mavlink_log_critical(_navigator->get_mavlink_log_pub(), "DO JUMP is cycling, giving up.");
 	return false;
@@ -1862,9 +1841,6 @@ bool Mission::position_setpoint_equal(const position_setpoint_s *p1, const posit
 void Mission::publish_navigator_mission_item()
 {
-	
-		for(int i=0;i<8;i++)
-{
 	navigator_mission_item_s navigator_mission_item{};
 	navigator_mission_item.instance_count = _navigator->mission_instance_count();
@@ -1889,13 +1865,5 @@ void Mission::publish_navigator_mission_item()
 	navigator_mission_item.vtol_back_transition = _mission_item.vtol_back_transition;
 	navigator_mission_item.timestamp = hrt_absolute_time();
-
 	_navigator_mission_item_pub.publish(navigator_mission_item);
-	
-	
-	std::cout<<navigator_mission_item.latitude<<std::endl;
-	navigator_mission_item.sequence_current++;
-	std::cout<<"************************"<<std::endl;
-}
-	
 }
--- a/home/Firmware/src/modules/navigator/mission.h
View file @67e0e54
+++ b/home/Firmware/src/modules/navigator/mission.h
View file @67e0e54
@@ -49,7 +49,7 @@
 #include "navigator_mode.h"
 #include <float.h>
-
+#include <iostream>
 #include <dataman/dataman.h>
 #include <drivers/drv_hrt.h>
 #include <px4_platform_common/module_params.h>
@@ -227,7 +227,7 @@ private:
 	 * Find and store the index of the landing sequence (DO_LAND_START)
 	 */
 	bool find_mission_land_start();
-
+	bool print_next_mission(struct mission_item_s *mis);
 	/**
 	 * Return the index of the closest mission item to the current global position.
 	 */
@@ -269,7 +269,7 @@ private:
 		MISSION_TYPE_NONE,
 		MISSION_TYPE_MISSION
 	} _mission_type{MISSION_TYPE_NONE};
-
+	int mission_index=-1;
 	bool _inited{false};
 	bool _home_inited{false};
 	bool _need_mission_reset{false};
@@ -281,7 +281,6 @@ private:
 		WORK_ITEM_TYPE_TAKEOFF,		/**< takeoff before moving to waypoint */
 		WORK_ITEM_TYPE_MOVE_TO_LAND,	/**< move to land waypoint before descent */
 		WORK_ITEM_TYPE_ALIGN,		/**< align for next waypoint */
-		WORK_ITEM_TYPE_CMD_BEFORE_MOVE,
 		WORK_ITEM_TYPE_TRANSITON_AFTER_TAKEOFF,
 		WORK_ITEM_TYPE_MOVE_TO_LAND_AFTER_TRANSITION,
 		WORK_ITEM_TYPE_PRECISION_LAND