diff --git a/src/modules/mavlink/mavlink_mission.cpp b/src/modules/mavlink/mavlink_mission.cpp index 806743e4d1..89a37d1d1b 100644 --- a/src/modules/mavlink/mavlink_mission.cpp +++ b/src/modules/mavlink/mavlink_mission.cpp @@ -912,16 +912,14 @@ MavlinkMissionManager::parse_mavlink_mission_item(const mavlink_mission_item_t * case MAV_CMD_NAV_LOITER_UNLIM: mission_item->nav_cmd = NAV_CMD_LOITER_UNLIMITED; - mission_item->loiter_radius = fabsf(mavlink_mission_item->param3); - mission_item->loiter_direction = (mavlink_mission_item->param3 > 0) ? 1 : -1; /* 1 if positive CW, -1 if negative CCW */ + mission_item->loiter_radius = mavlink_mission_item->param3; mission_item->yaw = _wrap_pi(mavlink_mission_item->param4 * M_DEG_TO_RAD_F); break; case MAV_CMD_NAV_LOITER_TIME: mission_item->nav_cmd = NAV_CMD_LOITER_TIME_LIMIT; mission_item->time_inside = mavlink_mission_item->param1; - mission_item->loiter_radius = fabsf(mavlink_mission_item->param3); - mission_item->loiter_direction = (mavlink_mission_item->param3 > 0) ? 1 : -1; /* 1 if positive CW, -1 if negative CCW */ + mission_item->loiter_radius = mavlink_mission_item->param3; mission_item->loiter_exit_xtrack = (mavlink_mission_item->param4 > 0) ? true : false; break; @@ -940,8 +938,7 @@ MavlinkMissionManager::parse_mavlink_mission_item(const mavlink_mission_item_t * case MAV_CMD_NAV_LOITER_TO_ALT: mission_item->nav_cmd = NAV_CMD_LOITER_TO_ALT; mission_item->force_heading = (mavlink_mission_item->param1 > 0) ? true : false; - mission_item->loiter_radius = fabsf(mavlink_mission_item->param2); - mission_item->loiter_direction = (mavlink_mission_item->param2 > 0) ? 1 : -1; /* 1 if positive CW, -1 if negative CCW */ + mission_item->loiter_radius = mavlink_mission_item->param2; mission_item->loiter_exit_xtrack = (mavlink_mission_item->param4 > 0) ? true : false; break; @@ -1097,13 +1094,13 @@ MavlinkMissionManager::format_mavlink_mission_item(const struct mission_item_s * break; case NAV_CMD_LOITER_UNLIMITED: - mavlink_mission_item->param3 = mission_item->loiter_radius * (float)mission_item->loiter_direction; + mavlink_mission_item->param3 = mission_item->loiter_radius; mavlink_mission_item->param4 = mission_item->yaw * M_RAD_TO_DEG_F; break; case NAV_CMD_LOITER_TIME_LIMIT: mavlink_mission_item->param1 = mission_item->time_inside; - mavlink_mission_item->param3 = mission_item->loiter_radius * (float)mission_item->loiter_direction; + mavlink_mission_item->param3 = mission_item->loiter_radius; mavlink_mission_item->param4 = mission_item->loiter_exit_xtrack; break; @@ -1119,7 +1116,7 @@ MavlinkMissionManager::format_mavlink_mission_item(const struct mission_item_s * case NAV_CMD_LOITER_TO_ALT: mavlink_mission_item->param1 = mission_item->force_heading; - mavlink_mission_item->param2 = mission_item->loiter_radius * (float)mission_item->loiter_direction; + mavlink_mission_item->param2 = mission_item->loiter_radius; mavlink_mission_item->param4 = mission_item->loiter_exit_xtrack; break; diff --git a/src/modules/navigator/datalinkloss.cpp b/src/modules/navigator/datalinkloss.cpp index d4e6168478..14cfd66016 100644 --- a/src/modules/navigator/datalinkloss.cpp +++ b/src/modules/navigator/datalinkloss.cpp @@ -124,7 +124,6 @@ DataLinkLoss::set_dll_item() _mission_item.altitude = _param_commsholdalt.get(); _mission_item.yaw = NAN; _mission_item.loiter_radius = _navigator->get_loiter_radius(); - _mission_item.loiter_direction = 1; _mission_item.nav_cmd = NAV_CMD_LOITER_TIME_LIMIT; _mission_item.acceptance_radius = _navigator->get_acceptance_radius(); _mission_item.time_inside = _param_commsholdwaittime.get() < 0.0f ? 0.0f : _param_commsholdwaittime.get(); @@ -142,7 +141,6 @@ DataLinkLoss::set_dll_item() _mission_item.altitude = _param_airfieldhomealt.get(); _mission_item.yaw = NAN; _mission_item.loiter_radius = _navigator->get_loiter_radius(); - _mission_item.loiter_direction = 1; _mission_item.nav_cmd = NAV_CMD_LOITER_TIME_LIMIT; _mission_item.time_inside = _param_airfieldhomewaittime.get() < 0.0f ? 0.0f : _param_airfieldhomewaittime.get(); _mission_item.acceptance_radius = _navigator->get_acceptance_radius(); diff --git a/src/modules/navigator/enginefailure.cpp b/src/modules/navigator/enginefailure.cpp index 5133d9547d..495ec696b8 100644 --- a/src/modules/navigator/enginefailure.cpp +++ b/src/modules/navigator/enginefailure.cpp @@ -113,7 +113,6 @@ EngineFailure::set_ef_item() _mission_item.altitude = _navigator->get_home_position()->alt - 1000.0f; _mission_item.yaw = NAN; _mission_item.loiter_radius = _navigator->get_loiter_radius(); - _mission_item.loiter_direction = 1; _mission_item.nav_cmd = NAV_CMD_LOITER_UNLIMITED; _mission_item.acceptance_radius = _navigator->get_acceptance_radius(); _mission_item.pitch_min = 0.0f; diff --git a/src/modules/navigator/mission.cpp b/src/modules/navigator/mission.cpp index 681285acda..eedade0ffe 100644 --- a/src/modules/navigator/mission.cpp +++ b/src/modules/navigator/mission.cpp @@ -1008,7 +1008,6 @@ Mission::do_abort_landing() _mission_item.altitude = alt_sp; _mission_item.yaw = NAN; _mission_item.loiter_radius = _navigator->get_loiter_radius(); - _mission_item.loiter_direction = 1; _mission_item.nav_cmd = NAV_CMD_LOITER_UNLIMITED; _mission_item.acceptance_radius = _navigator->get_acceptance_radius(); _mission_item.autocontinue = false; diff --git a/src/modules/navigator/mission_block.cpp b/src/modules/navigator/mission_block.cpp index b1aa4c61a9..10f2acbb15 100644 --- a/src/modules/navigator/mission_block.cpp +++ b/src/modules/navigator/mission_block.cpp @@ -207,7 +207,7 @@ MissionBlock::is_mission_item_reached() * Therefore the item is marked as reached once the system reaches the loiter * radius (+ some margin). Time inside and turn count is handled elsewhere. */ - if (dist >= 0.0f && dist <= _navigator->get_acceptance_radius(_mission_item.loiter_radius * 1.2f) + if (dist >= 0.0f && dist <= _navigator->get_acceptance_radius(fabsf(_mission_item.loiter_radius) * 1.2f) && dist_z <= _navigator->get_altitude_acceptance_radius()) { _waypoint_position_reached = true; @@ -235,7 +235,7 @@ MissionBlock::is_mission_item_reached() _navigator->get_global_position()->alt, &dist_xy, &dist_z); - if (dist >= 0.0f && dist <= _navigator->get_acceptance_radius(_mission_item.loiter_radius * 1.2f) + if (dist >= 0.0f && dist <= _navigator->get_acceptance_radius(fabsf(_mission_item.loiter_radius) * 1.2f) && dist_z <= _navigator->get_altitude_acceptance_radius()) { // now set the loiter to the final altitude in the NAV_CMD_LOITER_TO_ALT mission item @@ -244,7 +244,7 @@ MissionBlock::is_mission_item_reached() } } else { - if (dist >= 0.0f && dist <= _navigator->get_acceptance_radius(_mission_item.loiter_radius * 1.2f) + if (dist >= 0.0f && dist <= _navigator->get_acceptance_radius(fabsf(_mission_item.loiter_radius) * 1.2f) && dist_z <= _navigator->get_altitude_acceptance_radius()) { _waypoint_position_reached = true; @@ -483,9 +483,9 @@ MissionBlock::mission_item_to_position_setpoint(const struct mission_item_s *ite sp->lon = item->lon; sp->alt = item->altitude_is_relative ? item->altitude + _navigator->get_home_position()->alt : item->altitude; sp->yaw = item->yaw; - sp->loiter_radius = (item->loiter_radius > NAV_EPSILON_POSITION) ? item->loiter_radius : + sp->loiter_radius = (fabsf(item->loiter_radius) > NAV_EPSILON_POSITION) ? fabsf(item->loiter_radius) : _navigator->get_loiter_radius(); - sp->loiter_direction = item->loiter_direction; + sp->loiter_direction = (item->loiter_radius > 0) ? 1 : -1; sp->pitch_min = item->pitch_min; sp->acceptance_radius = item->acceptance_radius; sp->disable_mc_yaw_control = false; @@ -573,7 +573,6 @@ MissionBlock::set_loiter_item(struct mission_item_s *item, float min_clearance) item->altitude_is_relative = false; item->yaw = NAN; item->loiter_radius = _navigator->get_loiter_radius(); - item->loiter_direction = 1; item->acceptance_radius = _navigator->get_acceptance_radius(); item->time_inside = 0.0f; item->pitch_min = 0.0f; @@ -609,7 +608,6 @@ MissionBlock::set_follow_target_item(struct mission_item_s *item, float min_clea item->altitude_is_relative = false; item->yaw = yaw; item->loiter_radius = _navigator->get_loiter_radius(); - item->loiter_direction = 1; item->acceptance_radius = _navigator->get_acceptance_radius(); item->time_inside = 0.0f; item->pitch_min = 0.0f; @@ -631,7 +629,6 @@ MissionBlock::set_takeoff_item(struct mission_item_s *item, float abs_altitude, item->yaw = NAN; item->loiter_radius = _navigator->get_loiter_radius(); - item->loiter_direction = 1; item->time_inside = 0.0f; item->pitch_min = min_pitch; item->autocontinue = false; @@ -673,7 +670,6 @@ MissionBlock::set_land_item(struct mission_item_s *item, bool at_current_locatio item->altitude = 0; item->altitude_is_relative = false; item->loiter_radius = _navigator->get_loiter_radius(); - item->loiter_direction = 1; item->acceptance_radius = _navigator->get_acceptance_radius(); item->time_inside = 0.0f; item->pitch_min = 0.0f; @@ -691,7 +687,6 @@ MissionBlock::set_current_position_item(struct mission_item_s *item) item->altitude = _navigator->get_global_position()->alt; item->yaw = NAN; item->loiter_radius = _navigator->get_loiter_radius(); - item->loiter_direction = 1; item->acceptance_radius = _navigator->get_acceptance_radius(); item->time_inside = 0.0f; item->pitch_min = 0.0f; @@ -709,7 +704,6 @@ MissionBlock::set_idle_item(struct mission_item_s *item) item->altitude = _navigator->get_home_position()->alt; item->yaw = NAN; item->loiter_radius = _navigator->get_loiter_radius(); - item->loiter_direction = 1; item->acceptance_radius = _navigator->get_acceptance_radius(); item->time_inside = 0.0f; item->pitch_min = 0.0f; diff --git a/src/modules/navigator/navigation.h b/src/modules/navigator/navigation.h index 22caea4486..30d9b7278c 100644 --- a/src/modules/navigator/navigation.h +++ b/src/modules/navigator/navigation.h @@ -109,7 +109,7 @@ struct mission_item_s { float pitch_min; /**< minimal pitch angle for fixed wing takeoff waypoints */ }; float acceptance_radius; /**< default radius in which the mission is accepted as reached in meters */ - float loiter_radius; /**< loiter radius in meters, 0 for a VTOL to hover */ + float loiter_radius; /**< loiter radius in meters, 0 for a VTOL to hover, negative for counter-clockwise */ float yaw; /**< in radians NED -PI..+PI, NAN means don't change yaw */ float ___lat_float; /**< padding */ float ___lon_float; /**< padding */ @@ -117,17 +117,18 @@ struct mission_item_s { }; float params[7]; /**< array to store mission command values for MAV_FRAME_MISSION ***/ }; - enum NAV_CMD nav_cmd; /**< navigation command */ + uint16_t nav_cmd; /**< navigation command */ int16_t do_jump_mission_index; /**< index where the do jump will go to */ uint16_t do_jump_repeat_count; /**< how many times do jump needs to be done */ uint16_t do_jump_current_count; /**< count how many times the jump has been done */ - uint16_t origin; /**< where the waypoint has been generated */ - bool loiter_exit_xtrack; /**< exit xtrack location: 0 for center of loiter wp, 1 for exit location */ - bool force_heading; /**< heading needs to be reached ***/ - bool altitude_is_relative; /**< true if altitude is relative from start point */ - bool autocontinue; /**< true if next waypoint should follow after this one */ - int8_t frame; /**< mission frame ***/ - int8_t loiter_direction; /**< 1: positive / clockwise, -1, negative. */ + struct { + uint16_t frame : 4, /**< mission frame ***/ + origin : 3, /**< how the mission item was generated */ + loiter_exit_xtrack : 1, /**< exit xtrack location: 0 for center of loiter wp, 1 for exit location */ + force_heading : 1, /**< heading needs to be reached ***/ + altitude_is_relative : 1, /**< true if altitude is relative from start point */ + autocontinue : 1; /**< true if next waypoint should follow after this one */ + }; }; #pragma pack(pop) #include diff --git a/src/modules/navigator/rcloss.cpp b/src/modules/navigator/rcloss.cpp index 2572788d6e..9714afc1f2 100644 --- a/src/modules/navigator/rcloss.cpp +++ b/src/modules/navigator/rcloss.cpp @@ -114,7 +114,6 @@ RCLoss::set_rcl_item() _mission_item.altitude_is_relative = false; _mission_item.yaw = NAN; _mission_item.loiter_radius = _navigator->get_loiter_radius(); - _mission_item.loiter_direction = 1; _mission_item.nav_cmd = NAV_CMD_LOITER_TIME_LIMIT; _mission_item.acceptance_radius = _navigator->get_acceptance_radius(); _mission_item.time_inside = _param_loitertime.get() < 0.0f ? 0.0f : _param_loitertime.get(); diff --git a/src/modules/navigator/rtl.cpp b/src/modules/navigator/rtl.cpp index b7a6499fb2..7c83687988 100644 --- a/src/modules/navigator/rtl.cpp +++ b/src/modules/navigator/rtl.cpp @@ -155,7 +155,6 @@ RTL::set_rtl_item() _mission_item.altitude = climb_alt; _mission_item.yaw = NAN; _mission_item.loiter_radius = _navigator->get_loiter_radius(); - _mission_item.loiter_direction = 1; _mission_item.nav_cmd = NAV_CMD_WAYPOINT; _mission_item.acceptance_radius = _navigator->get_acceptance_radius(); _mission_item.time_inside = 0.0f; @@ -197,7 +196,6 @@ RTL::set_rtl_item() } } _mission_item.loiter_radius = _navigator->get_loiter_radius(); - _mission_item.loiter_direction = 1; _mission_item.nav_cmd = NAV_CMD_WAYPOINT; _mission_item.acceptance_radius = _navigator->get_acceptance_radius(); _mission_item.time_inside = 0.0f; @@ -244,7 +242,6 @@ RTL::set_rtl_item() } _mission_item.loiter_radius = _navigator->get_loiter_radius(); - _mission_item.loiter_direction = 1; _mission_item.nav_cmd = NAV_CMD_WAYPOINT; _mission_item.acceptance_radius = _navigator->get_acceptance_radius(); _mission_item.time_inside = 0.0f; @@ -269,7 +266,6 @@ RTL::set_rtl_item() // don't change altitude _mission_item.yaw = _navigator->get_home_position()->yaw; _mission_item.loiter_radius = _navigator->get_loiter_radius(); - _mission_item.loiter_direction = 1; _mission_item.nav_cmd = autoland ? NAV_CMD_LOITER_TIME_LIMIT : NAV_CMD_LOITER_UNLIMITED; _mission_item.acceptance_radius = _navigator->get_acceptance_radius(); _mission_item.time_inside = _param_land_delay.get() < 0.0f ? 0.0f : _param_land_delay.get();