이충섭

feat : print next target information

......@@ -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) {
offset--;
} else {
if (_mission_execution_mode == mission_result_s::MISSION_EXECUTION_MODE_REVERSE) {\
offset--;
}
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;
}
}
......
......@@ -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
......