이충섭

feat : print next target information

...@@ -47,7 +47,7 @@ ...@@ -47,7 +47,7 @@
47 47
48 #include "mission.h" 48 #include "mission.h"
49 #include "navigator.h" 49 #include "navigator.h"
50 -#include "iostream" 50 +#include <iostream>
51 #include <string.h> 51 #include <string.h>
52 #include <drivers/drv_hrt.h> 52 #include <drivers/drv_hrt.h>
53 #include <dataman/dataman.h> 53 #include <dataman/dataman.h>
...@@ -381,7 +381,6 @@ Mission::set_execution_mode(const uint8_t mode) ...@@ -381,7 +381,6 @@ Mission::set_execution_mode(const uint8_t mode)
381 _mission_execution_mode = mode; 381 _mission_execution_mode = mode;
382 } 382 }
383 } 383 }
384 -
385 bool 384 bool
386 Mission::find_mission_land_start() 385 Mission::find_mission_land_start()
387 { 386 {
...@@ -621,11 +620,12 @@ Mission::set_mission_items() ...@@ -621,11 +620,12 @@ Mission::set_mission_items()
621 struct mission_item_s mission_item_after_next_position; 620 struct mission_item_s mission_item_after_next_position;
622 bool has_next_position_item = false; 621 bool has_next_position_item = false;
623 bool has_after_next_position_item = false; 622 bool has_after_next_position_item = false;
624 -
625 work_item_type new_work_item_type = WORK_ITEM_TYPE_DEFAULT; 623 work_item_type new_work_item_type = WORK_ITEM_TYPE_DEFAULT;
626 624
627 if (prepare_mission_items(&_mission_item, &mission_item_next_position, &has_next_position_item, 625 if (prepare_mission_items(&_mission_item, &mission_item_next_position, &has_next_position_item,
628 &mission_item_after_next_position, &has_after_next_position_item)) { 626 &mission_item_after_next_position, &has_after_next_position_item)) {
627 + print_next_mission(&mission_item_next_position);
628 +
629 /* if mission type changed, notify */ 629 /* if mission type changed, notify */
630 if (_mission_type != MISSION_TYPE_MISSION) { 630 if (_mission_type != MISSION_TYPE_MISSION) {
631 mavlink_log_info(_navigator->get_mavlink_log_pub(), 631 mavlink_log_info(_navigator->get_mavlink_log_pub(),
...@@ -633,7 +633,6 @@ Mission::set_mission_items() ...@@ -633,7 +633,6 @@ Mission::set_mission_items()
633 "Executing Mission"); 633 "Executing Mission");
634 user_feedback_done = true; 634 user_feedback_done = true;
635 } 635 }
636 -
637 _mission_type = MISSION_TYPE_MISSION; 636 _mission_type = MISSION_TYPE_MISSION;
638 637
639 } else { 638 } else {
...@@ -1031,30 +1030,6 @@ Mission::set_mission_items() ...@@ -1031,30 +1030,6 @@ Mission::set_mission_items()
1031 generate_waypoint_from_heading(&pos_sp_triplet->current, pos_sp_triplet->current.yaw); 1030 generate_waypoint_from_heading(&pos_sp_triplet->current, pos_sp_triplet->current.yaw);
1032 } 1031 }
1033 1032
1034 - /* don't advance mission after FW to MC command */
1035 - if (_mission_item.nav_cmd == NAV_CMD_DO_VTOL_TRANSITION
1036 - && _work_item_type == WORK_ITEM_TYPE_DEFAULT
1037 - && new_work_item_type == WORK_ITEM_TYPE_DEFAULT
1038 - && _navigator->get_vstatus()->vehicle_type == vehicle_status_s::VEHICLE_TYPE_FIXED_WING
1039 - && !_navigator->get_land_detected()->landed
1040 - && pos_sp_triplet->current.valid) {
1041 -
1042 - new_work_item_type = WORK_ITEM_TYPE_CMD_BEFORE_MOVE;
1043 - }
1044 -
1045 - /* after FW to MC transition finish moving to the waypoint */
1046 - if (_work_item_type == WORK_ITEM_TYPE_CMD_BEFORE_MOVE &&
1047 - new_work_item_type == WORK_ITEM_TYPE_DEFAULT
1048 - && pos_sp_triplet->current.valid) {
1049 -
1050 - new_work_item_type = WORK_ITEM_TYPE_DEFAULT;
1051 -
1052 - _mission_item.nav_cmd = NAV_CMD_WAYPOINT;
1053 - copy_position_if_valid(&_mission_item, &pos_sp_triplet->current);
1054 - _mission_item.autocontinue = true;
1055 - _mission_item.time_inside = 0.0f;
1056 - }
1057 -
1058 // ignore certain commands in mission fast forward 1033 // ignore certain commands in mission fast forward
1059 if ((_mission_execution_mode == mission_result_s::MISSION_EXECUTION_MODE_FAST_FORWARD) && 1034 if ((_mission_execution_mode == mission_result_s::MISSION_EXECUTION_MODE_FAST_FORWARD) &&
1060 (_mission_item.nav_cmd == NAV_CMD_DELAY)) { 1035 (_mission_item.nav_cmd == NAV_CMD_DELAY)) {
...@@ -1461,17 +1436,14 @@ Mission::prepare_mission_items(struct mission_item_s *mission_item, ...@@ -1461,17 +1436,14 @@ Mission::prepare_mission_items(struct mission_item_s *mission_item,
1461 if (_mission_execution_mode == mission_result_s::MISSION_EXECUTION_MODE_REVERSE) { 1436 if (_mission_execution_mode == mission_result_s::MISSION_EXECUTION_MODE_REVERSE) {
1462 offset = -1; 1437 offset = -1;
1463 } 1438 }
1464 -
1465 if (read_mission_item(0, mission_item)) { 1439 if (read_mission_item(0, mission_item)) {
1466 -
1467 first_res = true; 1440 first_res = true;
1468 -
1469 /* trying to find next position mission item */ 1441 /* trying to find next position mission item */
1470 while (read_mission_item(offset, next_position_mission_item)) { 1442 while (read_mission_item(offset, next_position_mission_item)) {
1471 - if (_mission_execution_mode == mission_result_s::MISSION_EXECUTION_MODE_REVERSE) { 1443 + if (_mission_execution_mode == mission_result_s::MISSION_EXECUTION_MODE_REVERSE) {\
1472 - offset--; 1444 + offset--;
1473 - 1445 + }
1474 - } else { 1446 + else {
1475 offset++; 1447 offset++;
1476 } 1448 }
1477 1449
...@@ -1480,13 +1452,11 @@ Mission::prepare_mission_items(struct mission_item_s *mission_item, ...@@ -1480,13 +1452,11 @@ Mission::prepare_mission_items(struct mission_item_s *mission_item,
1480 break; 1452 break;
1481 } 1453 }
1482 } 1454 }
1483 -
1484 if (_mission_execution_mode != mission_result_s::MISSION_EXECUTION_MODE_REVERSE && 1455 if (_mission_execution_mode != mission_result_s::MISSION_EXECUTION_MODE_REVERSE &&
1485 after_next_position_mission_item && has_after_next_position_item) { 1456 after_next_position_mission_item && has_after_next_position_item) {
1486 /* trying to find next next position mission item */ 1457 /* trying to find next next position mission item */
1487 while (read_mission_item(offset, after_next_position_mission_item)) { 1458 while (read_mission_item(offset, after_next_position_mission_item)) {
1488 offset++; 1459 offset++;
1489 -
1490 if (item_contains_position(*after_next_position_mission_item)) { 1460 if (item_contains_position(*after_next_position_mission_item)) {
1491 *has_after_next_position_item = true; 1461 *has_after_next_position_item = true;
1492 break; 1462 break;
...@@ -1497,14 +1467,24 @@ Mission::prepare_mission_items(struct mission_item_s *mission_item, ...@@ -1497,14 +1467,24 @@ Mission::prepare_mission_items(struct mission_item_s *mission_item,
1497 1467
1498 return first_res; 1468 return first_res;
1499 } 1469 }
1470 +bool
1471 +Mission::print_next_mission(struct mission_item_s *mis)
1472 +{
1500 1473
1474 + std::cout <<"----------------------target--------------------------"<<std::endl;
1475 + std::cout << "lat : "<<mis->lat <<std:: endl;
1476 + std::cout << "lon : "<<mis->lon <<std:: endl;
1477 + std::cout << "alt : "<<mis->altitude <<std:: endl;
1478 + //std::cout << "[whatever] "<<mis->timestamp <<std:: endl;
1479 + mission_index++;
1480 + return 0;
1481 +}
1501 bool 1482 bool
1502 Mission::read_mission_item(int offset, struct mission_item_s *mission_item) 1483 Mission::read_mission_item(int offset, struct mission_item_s *mission_item)
1503 { 1484 {
1504 /* select mission */ 1485 /* select mission */
1505 const int current_index = _current_mission_index; 1486 const int current_index = _current_mission_index;
1506 int index_to_read = current_index + offset; 1487 int index_to_read = current_index + offset;
1507 -
1508 int *mission_index_ptr = (offset == 0) ? &_current_mission_index : &index_to_read; 1488 int *mission_index_ptr = (offset == 0) ? &_current_mission_index : &index_to_read;
1509 const dm_item_t dm_item = (dm_item_t)_mission.dataman_id; 1489 const dm_item_t dm_item = (dm_item_t)_mission.dataman_id;
1510 1490
...@@ -1584,7 +1564,6 @@ Mission::read_mission_item(int offset, struct mission_item_s *mission_item) ...@@ -1584,7 +1564,6 @@ Mission::read_mission_item(int offset, struct mission_item_s *mission_item)
1584 return true; 1564 return true;
1585 } 1565 }
1586 } 1566 }
1587 -
1588 /* we have given up, we don't want to cycle forever */ 1567 /* we have given up, we don't want to cycle forever */
1589 mavlink_log_critical(_navigator->get_mavlink_log_pub(), "DO JUMP is cycling, giving up."); 1568 mavlink_log_critical(_navigator->get_mavlink_log_pub(), "DO JUMP is cycling, giving up.");
1590 return false; 1569 return false;
...@@ -1862,9 +1841,6 @@ bool Mission::position_setpoint_equal(const position_setpoint_s *p1, const posit ...@@ -1862,9 +1841,6 @@ bool Mission::position_setpoint_equal(const position_setpoint_s *p1, const posit
1862 1841
1863 void Mission::publish_navigator_mission_item() 1842 void Mission::publish_navigator_mission_item()
1864 { 1843 {
1865 -
1866 - for(int i=0;i<8;i++)
1867 -{
1868 navigator_mission_item_s navigator_mission_item{}; 1844 navigator_mission_item_s navigator_mission_item{};
1869 1845
1870 navigator_mission_item.instance_count = _navigator->mission_instance_count(); 1846 navigator_mission_item.instance_count = _navigator->mission_instance_count();
...@@ -1889,13 +1865,5 @@ void Mission::publish_navigator_mission_item() ...@@ -1889,13 +1865,5 @@ void Mission::publish_navigator_mission_item()
1889 navigator_mission_item.vtol_back_transition = _mission_item.vtol_back_transition; 1865 navigator_mission_item.vtol_back_transition = _mission_item.vtol_back_transition;
1890 1866
1891 navigator_mission_item.timestamp = hrt_absolute_time(); 1867 navigator_mission_item.timestamp = hrt_absolute_time();
1892 -
1893 _navigator_mission_item_pub.publish(navigator_mission_item); 1868 _navigator_mission_item_pub.publish(navigator_mission_item);
1894 -
1895 -
1896 - std::cout<<navigator_mission_item.latitude<<std::endl;
1897 - navigator_mission_item.sequence_current++;
1898 - std::cout<<"************************"<<std::endl;
1899 -}
1900 -
1901 } 1869 }
......
...@@ -49,7 +49,7 @@ ...@@ -49,7 +49,7 @@
49 #include "navigator_mode.h" 49 #include "navigator_mode.h"
50 50
51 #include <float.h> 51 #include <float.h>
52 - 52 +#include <iostream>
53 #include <dataman/dataman.h> 53 #include <dataman/dataman.h>
54 #include <drivers/drv_hrt.h> 54 #include <drivers/drv_hrt.h>
55 #include <px4_platform_common/module_params.h> 55 #include <px4_platform_common/module_params.h>
...@@ -227,7 +227,7 @@ private: ...@@ -227,7 +227,7 @@ private:
227 * Find and store the index of the landing sequence (DO_LAND_START) 227 * Find and store the index of the landing sequence (DO_LAND_START)
228 */ 228 */
229 bool find_mission_land_start(); 229 bool find_mission_land_start();
230 - 230 + bool print_next_mission(struct mission_item_s *mis);
231 /** 231 /**
232 * Return the index of the closest mission item to the current global position. 232 * Return the index of the closest mission item to the current global position.
233 */ 233 */
...@@ -269,7 +269,7 @@ private: ...@@ -269,7 +269,7 @@ private:
269 MISSION_TYPE_NONE, 269 MISSION_TYPE_NONE,
270 MISSION_TYPE_MISSION 270 MISSION_TYPE_MISSION
271 } _mission_type{MISSION_TYPE_NONE}; 271 } _mission_type{MISSION_TYPE_NONE};
272 - 272 + int mission_index=-1;
273 bool _inited{false}; 273 bool _inited{false};
274 bool _home_inited{false}; 274 bool _home_inited{false};
275 bool _need_mission_reset{false}; 275 bool _need_mission_reset{false};
...@@ -281,7 +281,6 @@ private: ...@@ -281,7 +281,6 @@ private:
281 WORK_ITEM_TYPE_TAKEOFF, /**< takeoff before moving to waypoint */ 281 WORK_ITEM_TYPE_TAKEOFF, /**< takeoff before moving to waypoint */
282 WORK_ITEM_TYPE_MOVE_TO_LAND, /**< move to land waypoint before descent */ 282 WORK_ITEM_TYPE_MOVE_TO_LAND, /**< move to land waypoint before descent */
283 WORK_ITEM_TYPE_ALIGN, /**< align for next waypoint */ 283 WORK_ITEM_TYPE_ALIGN, /**< align for next waypoint */
284 - WORK_ITEM_TYPE_CMD_BEFORE_MOVE,
285 WORK_ITEM_TYPE_TRANSITON_AFTER_TAKEOFF, 284 WORK_ITEM_TYPE_TRANSITON_AFTER_TAKEOFF,
286 WORK_ITEM_TYPE_MOVE_TO_LAND_AFTER_TRANSITION, 285 WORK_ITEM_TYPE_MOVE_TO_LAND_AFTER_TRANSITION,
287 WORK_ITEM_TYPE_PRECISION_LAND 286 WORK_ITEM_TYPE_PRECISION_LAND
......