diff --git a/src/modules/navigator/mission.cpp b/src/modules/navigator/mission.cpp index 5c01017678..709e70f402 100644 --- a/src/modules/navigator/mission.cpp +++ b/src/modules/navigator/mission.cpp @@ -40,6 +40,7 @@ * @author Anton Babushkin * @author Ban Siesta * @author Simon Wilks + * @author Andreas Antener */ #include @@ -84,7 +85,8 @@ Mission::Mission(Navigator *navigator, const char *name) : _min_current_sp_distance_xy(FLT_MAX), _mission_item_previous_alt(NAN), _on_arrival_yaw(NAN), - _distance_current_previous(0.0f) + _distance_current_previous(0.0f), + _work_item_type(WORK_ITEM_TYPE_DEFAULT) { /* load initial params */ updateParams(); @@ -338,8 +340,12 @@ Mission::set_mission_items() /* the home dist check provides user feedback, so we initialize it to this */ bool user_feedback_done = false; + /* mission item that comes after current if available */ + struct mission_item_s mission_item_next; + bool has_next_item = false; + /* try setting onboard mission item */ - if (_param_onboard_enabled.get() && read_mission_item(true, true, &_mission_item)) { + if (_param_onboard_enabled.get() && prepare_mission_items(true, &_mission_item, &mission_item_next, &has_next_item)) { /* if mission type changed, notify */ if (_mission_type != MISSION_TYPE_ONBOARD) { mavlink_log_critical(_navigator->get_mavlink_fd(), "onboard mission now running"); @@ -348,7 +354,7 @@ Mission::set_mission_items() _mission_type = MISSION_TYPE_ONBOARD; /* try setting offboard mission item */ - } else if (read_mission_item(false, true, &_mission_item)) { + } else if (prepare_mission_items(false, &_mission_item, &mission_item_next, &has_next_item)) { /* if mission type changed, notify */ if (_mission_type != MISSION_TYPE_OFFBOARD) { mavlink_log_info(_navigator->get_mavlink_fd(), "offboard mission now running"); @@ -404,6 +410,12 @@ Mission::set_mission_items() return; } + if (!item_contains_position(&_mission_item)) { + // XXX: before issuing command, check if we need to align for transition first + issue_command(&_mission_item); + return; + } + if (pos_sp_triplet->current.valid) { _on_arrival_yaw = _mission_item.yaw; } @@ -494,10 +506,9 @@ Mission::set_mission_items() // TODO: report onboard mission item somehow if (_mission_item.autocontinue && _mission_item.time_inside <= 0.001f) { - /* try to read next mission item */ - struct mission_item_s mission_item_next; + /* try to process next mission item */ - if (read_mission_item(_mission_type == MISSION_TYPE_ONBOARD, false, &mission_item_next)) { + if (has_next_item) { /* got next mission item, update setpoint triplet */ mission_item_to_position_setpoint(&mission_item_next, &pos_sp_triplet->next); } else { @@ -650,14 +661,40 @@ Mission::altitude_sp_foh_reset() } bool -Mission::read_mission_item(bool onboard, bool is_current, struct mission_item_s *mission_item) +Mission::prepare_mission_items(bool onboard, struct mission_item_s *mission_item, + struct mission_item_s *next_mission_item, bool *has_next_item) +{ + bool first_res = false; + int offset = 1; + + if (read_mission_item(onboard, 0, mission_item)) { + + first_res = true; + + /* trying to find next position mission item */ + while(read_mission_item(onboard, offset, next_mission_item)) { + + if (item_contains_position(next_mission_item)) { + *has_next_item = true; + break; + } + + offset++; + } + } + + return first_res; +} + +bool +Mission::read_mission_item(bool onboard, int offset, struct mission_item_s *mission_item) { /* select onboard/offboard mission */ int *mission_index_ptr; dm_item_t dm_item; struct mission_s *mission = (onboard) ? &_onboard_mission : &_offboard_mission; - int mission_index_next = (onboard) ? _current_onboard_mission_index : _current_offboard_mission_index; + int index_to_read = (onboard) ? _current_onboard_mission_index : _current_offboard_mission_index; /* do not work on empty missions */ if (mission->count == 0) { @@ -665,18 +702,18 @@ Mission::read_mission_item(bool onboard, bool is_current, struct mission_item_s } /* move to next item if there is one */ - if (mission_index_next < ((int)mission->count - 1)) { - mission_index_next++; + if (index_to_read + offset < ((int)mission->count - 1)) { + index_to_read += offset; } + mission_index_ptr = &index_to_read; + if (onboard) { /* onboard mission */ - mission_index_ptr = is_current ? &_current_onboard_mission_index : &mission_index_next; dm_item = DM_KEY_WAYPOINTS_ONBOARD; } else { /* offboard mission */ - mission_index_ptr = is_current ? &_current_offboard_mission_index : &mission_index_next; dm_item = DM_KEY_WAYPOINTS_OFFBOARD(_offboard_mission.dataman_id); } @@ -713,7 +750,7 @@ Mission::read_mission_item(bool onboard, bool is_current, struct mission_item_s /* only raise the repeat count if this is for the current mission item * but not for the next mission item */ - if (is_current) { + if (offset == 0) { (mission_item_tmp.do_jump_current_count)++; /* save repeat count */ if (dm_write(dm_item, *mission_index_ptr, DM_PERSIST_POWER_ON_RESET, @@ -732,7 +769,7 @@ Mission::read_mission_item(bool onboard, bool is_current, struct mission_item_s *mission_index_ptr = mission_item_tmp.do_jump_mission_index; } else { - if (is_current) { + if (offset == 0) { mavlink_log_critical(_navigator->get_mavlink_fd(), "DO JUMP repetitions completed"); } diff --git a/src/modules/navigator/mission.h b/src/modules/navigator/mission.h index 971ba37162..036968f317 100644 --- a/src/modules/navigator/mission.h +++ b/src/modules/navigator/mission.h @@ -136,11 +136,14 @@ private: float get_absolute_altitude_for_item(struct mission_item_s &mission_item); + bool prepare_mission_items(bool onboard, struct mission_item_s *mission_item, + struct mission_item_s *next_mission_item, bool *has_next_item); + /** * Read current or next mission item from the dataman and watch out for DO_JUMPS * @return true if successful */ - bool read_mission_item(bool onboard, bool is_current, struct mission_item_s *mission_item); + bool read_mission_item(bool onboard, int offset, struct mission_item_s *mission_item); /** * Save current offboard mission state to dataman @@ -204,6 +207,15 @@ private: float _on_arrival_yaw; /**< holds the yaw value that should be applied when the current waypoint is reached */ float _distance_current_previous; /**< distance from previous to current sp in pos_sp_triplet, only use if current and previous are valid */ + + enum { + WORK_ITEM_TYPE_DEFAULT, + WORK_ITEM_TYPE_TAKEOFF, + WORK_ITEM_TYPE_LAND, + WORK_ITEM_TYPE_TRANSITION, + WORK_ITEM_TYPE_YAW + } _work_item_type; /**< current type of work to do, intermediate to complete mission item */ + }; #endif diff --git a/src/modules/navigator/mission_block.cpp b/src/modules/navigator/mission_block.cpp index 0830c05fdc..6020e087ac 100644 --- a/src/modules/navigator/mission_block.cpp +++ b/src/modules/navigator/mission_block.cpp @@ -67,6 +67,7 @@ MissionBlock::MissionBlock(Navigator *navigator, const char *name) : _waypoint_position_reached(false), _waypoint_yaw_reached(false), _time_first_inside_orbit(0), + _action_start(0), _actuators{}, _actuator_pub(nullptr), _cmd_pub(nullptr) @@ -82,6 +83,7 @@ MissionBlock::is_mission_item_reached() { /* handle non-navigation or indefinite waypoints */ switch (_mission_item.nav_cmd) { + // XXX: move handling to mission as well case NAV_CMD_DO_SET_SERVO: { memset(&actuators, 0, sizeof(actuators)); actuators.control[_mission_item.actuator_num] = 1.0f / 2000 * -_mission_item.actuator_value; @@ -105,18 +107,13 @@ MissionBlock::is_mission_item_reached() case vehicle_command_s::VEHICLE_CMD_DO_DIGICAM_CONTROL: /* fallthrough */ case vehicle_command_s::VEHICLE_CMD_DO_VTOL_TRANSITION: - { - /* forward the command to other processes */ - warnx("got instantaneous command, forwarding.\n"); - struct vehicle_command_s cmd = {}; - cmd.command = _mission_item.nav_cmd; - mission_item_to_vehicle_command(&_mission_item, &cmd); - if (_cmd_pub != nullptr) { - orb_publish(ORB_ID(vehicle_command), _cmd_pub, &cmd); + // XXX: we should wait on command ACK or status change instead + // currently we just wait so the command be processed + if (hrt_absolute_time() - _action_start > 1000) { + return true; + } else { - _cmd_pub = orb_advertise(ORB_ID(vehicle_command), &cmd); - } - return true; + return false; } default: @@ -238,6 +235,7 @@ MissionBlock::mission_item_to_vehicle_command(const struct mission_item_s *item, cmd->param5 = item->params[4]; cmd->param6 = item->params[5]; cmd->param7 = item->params[6]; + cmd->command = item->nav_cmd; cmd->target_system = _navigator->get_vstatus()->system_id; cmd->target_component = _navigator->get_vstatus()->component_id; @@ -246,9 +244,46 @@ MissionBlock::mission_item_to_vehicle_command(const struct mission_item_s *item, cmd->confirmation = false; } +void +MissionBlock::issue_command(const struct mission_item_s *item) +{ + if (item_contains_position(item)) { + return; + } + + warnx("forwarding command.\n"); + struct vehicle_command_s cmd = {}; + mission_item_to_vehicle_command(item, &cmd); + _action_start = hrt_absolute_time(); + + if (_cmd_pub != nullptr) { + orb_publish(ORB_ID(vehicle_command), _cmd_pub, &cmd); + + } else { + _cmd_pub = orb_advertise(ORB_ID(vehicle_command), &cmd); + } +} + +bool +MissionBlock::item_contains_position(const struct mission_item_s *item) +{ + // XXX: maybe extend that check onto item properties + if (item->nav_cmd == vehicle_command_s::VEHICLE_CMD_DO_DIGICAM_CONTROL || + item->nav_cmd == vehicle_command_s::VEHICLE_CMD_DO_VTOL_TRANSITION) { + return false; + } + + return true; +} + void MissionBlock::mission_item_to_position_setpoint(const struct mission_item_s *item, struct position_setpoint_s *sp) { + /* don't change the setpoint for non-position items */ + if (!item_contains_position(item)) { + return; + } + sp->valid = true; sp->lat = item->lat; sp->lon = item->lon; @@ -261,7 +296,7 @@ MissionBlock::mission_item_to_position_setpoint(const struct mission_item_s *ite sp->acceptance_radius = item->acceptance_radius; switch (item->nav_cmd) { - case NAV_CMD_DO_SET_SERVO: + case NAV_CMD_DO_SET_SERVO: // XXX: should be also return in the beginning for this? /* Set current position for loitering set point*/ sp->lat = _navigator->get_global_position()->lat; sp->lon = _navigator->get_global_position()->lon; diff --git a/src/modules/navigator/mission_block.h b/src/modules/navigator/mission_block.h index 79845c781b..3560e6967c 100644 --- a/src/modules/navigator/mission_block.h +++ b/src/modules/navigator/mission_block.h @@ -78,6 +78,8 @@ protected: */ void reset_mission_item_reached(); + bool item_contains_position(const struct mission_item_s *item); + /** * Convert a mission item to a position setpoint * @@ -116,10 +118,13 @@ protected: */ void mission_item_to_vehicle_command(const struct mission_item_s *item, struct vehicle_command_s *cmd); + void issue_command(const struct mission_item_s *item); + mission_item_s _mission_item; bool _waypoint_position_reached; bool _waypoint_yaw_reached; hrt_abstime _time_first_inside_orbit; + hrt_abstime _action_start; actuator_controls_s _actuators; orb_advert_t _actuator_pub;