diff --git a/Tools/files_to_check_code_style.sh b/Tools/files_to_check_code_style.sh index be63c55e19..e49e3784ae 100755 --- a/Tools/files_to_check_code_style.sh +++ b/Tools/files_to_check_code_style.sh @@ -17,13 +17,12 @@ exec find src \ -path src/lib/mathlib -prune -o \ -path src/lib/matrix -prune -o \ -path src/drivers/bootloaders -o \ - -path src/modules/uavcanesc -o \ - -path src/modules/uavcannode -o \ -path src/modules/commander -prune -o \ -path src/modules/mavlink -prune -o \ - -path src/modules/navigator -prune -o \ -path src/modules/sdlog2 -prune -o \ -path src/modules/systemlib/uthash -prune -o \ -path src/modules/uavcan -prune -o \ -path src/modules/uavcan/libuavcan -prune -o \ + -path src/modules/uavcanesc -o \ + -path src/modules/uavcannode -o \ -type f \( -name "*.c" -o -name "*.h" -o -name "*.cpp" -o -name "*.hpp" \) | grep $PATTERN diff --git a/src/modules/navigator/mission.cpp b/src/modules/navigator/mission.cpp index cc9f8b2a2e..726066bce1 100644 --- a/src/modules/navigator/mission.cpp +++ b/src/modules/navigator/mission.cpp @@ -289,6 +289,7 @@ Mission::find_offboard_land_start() for (size_t i = 0; i < _offboard_mission.count; i++) { struct mission_item_s missionitem; const ssize_t len = sizeof(missionitem); + if (dm_read(dm_current, i, &missionitem, len) != len) { /* not supposed to happen unless the datamanager can't access the SD card, etc. */ return -1; @@ -860,8 +861,8 @@ Mission::do_need_takeoff() _need_takeoff = false; } else if (_navigator->get_global_position()->alt <= takeoff_alt - _navigator->get_acceptance_radius() - && (_mission_item.nav_cmd == NAV_CMD_TAKEOFF - || _mission_item.nav_cmd == NAV_CMD_VTOL_TAKEOFF)) { + && (_mission_item.nav_cmd == NAV_CMD_TAKEOFF + || _mission_item.nav_cmd == NAV_CMD_VTOL_TAKEOFF)) { /* if in-air but below takeoff height and we have a takeoff item */ _need_takeoff = true; } @@ -1054,8 +1055,8 @@ Mission::altitude_sp_foh_update() * or if the previous altitude isn't from a position or loiter setpoint */ if (!pos_sp_triplet->previous.valid || !pos_sp_triplet->current.valid || !PX4_ISFINITE(pos_sp_triplet->previous.alt) - || !(pos_sp_triplet->previous.type == position_setpoint_s::SETPOINT_TYPE_POSITION || - pos_sp_triplet->previous.type == position_setpoint_s::SETPOINT_TYPE_LOITER)) { + || !(pos_sp_triplet->previous.type == position_setpoint_s::SETPOINT_TYPE_POSITION || + pos_sp_triplet->previous.type == position_setpoint_s::SETPOINT_TYPE_LOITER)) { return; } @@ -1234,7 +1235,7 @@ Mission::read_mission_item(bool onboard, int offset, struct mission_item_s *miss /* mission item index out of bounds - if they are equal, we just reached the end */ if (*mission_index_ptr != (int)mission->count) { mavlink_log_critical(_navigator->get_mavlink_log_pub(), "[wpm] err: index: %d, max: %d", *mission_index_ptr, - (int)mission->count); + (int)mission->count); } return false; diff --git a/src/modules/navigator/mission.h b/src/modules/navigator/mission.h index ee34f8f07e..a688dfc6d4 100644 --- a/src/modules/navigator/mission.h +++ b/src/modules/navigator/mission.h @@ -181,7 +181,7 @@ private: * @return true if current mission item available */ bool prepare_mission_items(bool onboard, struct mission_item_s *mission_item, - struct mission_item_s *next_position_mission_item, bool *has_next_position_item); + struct mission_item_s *next_position_mission_item, bool *has_next_position_item); /** * Read current (offset == 0) or a specific (offset > 0) mission item diff --git a/src/modules/navigator/mission_block.cpp b/src/modules/navigator/mission_block.cpp index d7ea916fc0..46a0c0cec9 100644 --- a/src/modules/navigator/mission_block.cpp +++ b/src/modules/navigator/mission_block.cpp @@ -66,22 +66,22 @@ orb_advert_t actuator_pub_fd; MissionBlock::MissionBlock(Navigator *navigator, const char *name) : NavigatorMode(navigator, name), - _mission_item({0}), - _waypoint_position_reached(false), - _waypoint_yaw_reached(false), - _time_first_inside_orbit(0), - _action_start(0), - _time_wp_reached(0), - _actuators{}, - _actuator_pub(nullptr), - _cmd_pub(nullptr), - _param_loiter_min_alt(this, "MIS_LTRMIN_ALT", false), - _param_yaw_timeout(this, "MIS_YAW_TMT", false), - _param_yaw_err(this, "MIS_YAW_ERR", false), - _param_vtol_wv_land(this, "VT_WV_LND_EN", false), - _param_vtol_wv_takeoff(this, "VT_WV_TKO_EN", false), - _param_vtol_wv_loiter(this, "VT_WV_LTR_EN", false), - _param_force_vtol(this, "NAV_FORCE_VT", false) + _mission_item( {0}), + _waypoint_position_reached(false), + _waypoint_yaw_reached(false), + _time_first_inside_orbit(0), + _action_start(0), + _time_wp_reached(0), + _actuators{}, + _actuator_pub(nullptr), + _cmd_pub(nullptr), + _param_loiter_min_alt(this, "MIS_LTRMIN_ALT", false), + _param_yaw_timeout(this, "MIS_YAW_TMT", false), + _param_yaw_err(this, "MIS_YAW_ERR", false), + _param_vtol_wv_land(this, "VT_WV_LND_EN", false), + _param_vtol_wv_takeoff(this, "VT_WV_TKO_EN", false), + _param_vtol_wv_loiter(this, "VT_WV_LTR_EN", false), + _param_force_vtol(this, "NAV_FORCE_VT", false) { } @@ -94,80 +94,83 @@ MissionBlock::is_mission_item_reached() { /* handle non-navigation or indefinite waypoints */ switch (_mission_item.nav_cmd) { - case NAV_CMD_DO_SET_SERVO: + case NAV_CMD_DO_SET_SERVO: + return true; + + case NAV_CMD_LAND: /* fall through */ + case NAV_CMD_VTOL_LAND: + return _navigator->get_land_detected()->landed; + + case NAV_CMD_IDLE: /* fall through */ + case NAV_CMD_LOITER_UNLIMITED: + return false; + + case NAV_CMD_DO_LAND_START: + case NAV_CMD_DO_DIGICAM_CONTROL: + case NAV_CMD_IMAGE_START_CAPTURE: + case NAV_CMD_IMAGE_STOP_CAPTURE: + case NAV_CMD_VIDEO_START_CAPTURE: + case NAV_CMD_VIDEO_STOP_CAPTURE: + case NAV_CMD_DO_MOUNT_CONFIGURE: + case NAV_CMD_DO_MOUNT_CONTROL: + case NAV_CMD_DO_SET_ROI: + case NAV_CMD_ROI: + case NAV_CMD_DO_SET_CAM_TRIGG_DIST: + return true; + + case NAV_CMD_DO_VTOL_TRANSITION: + + /* + * We wait half a second to give the transition command time to propagate. + * Then monitor the transition status for completion. + */ + if (hrt_absolute_time() - _action_start > 500000 && + !_navigator->get_vstatus()->in_transition_mode) { + + _action_start = 0; return true; - case NAV_CMD_LAND: /* fall through */ - case NAV_CMD_VTOL_LAND: - return _navigator->get_land_detected()->landed; - - case NAV_CMD_IDLE: /* fall through */ - case NAV_CMD_LOITER_UNLIMITED: + } else { return false; + } - case NAV_CMD_DO_LAND_START: - case NAV_CMD_DO_DIGICAM_CONTROL: - case NAV_CMD_IMAGE_START_CAPTURE: - case NAV_CMD_IMAGE_STOP_CAPTURE: - case NAV_CMD_VIDEO_START_CAPTURE: - case NAV_CMD_VIDEO_STOP_CAPTURE: - case NAV_CMD_DO_MOUNT_CONFIGURE: - case NAV_CMD_DO_MOUNT_CONTROL: - case NAV_CMD_DO_SET_ROI: - case NAV_CMD_ROI: - case NAV_CMD_DO_SET_CAM_TRIGG_DIST: - return true; + case NAV_CMD_DO_CHANGE_SPEED: + return true; - case NAV_CMD_DO_VTOL_TRANSITION: - /* - * We wait half a second to give the transition command time to propagate. - * Then monitor the transition status for completion. - */ - if (hrt_absolute_time() - _action_start > 500000 && - !_navigator->get_vstatus()->in_transition_mode) { - - _action_start = 0; - return true; - } else { - return false; - } - - case NAV_CMD_DO_CHANGE_SPEED: - return true; - - default: - /* do nothing, this is a 3D waypoint */ - break; + default: + /* do nothing, this is a 3D waypoint */ + break; } hrt_abstime now = hrt_absolute_time(); if ((_navigator->get_land_detected()->landed == false) - && !_waypoint_position_reached) { + && !_waypoint_position_reached) { float dist = -1.0f; float dist_xy = -1.0f; float dist_z = -1.0f; float altitude_amsl = _mission_item.altitude_is_relative - ? _mission_item.altitude + _navigator->get_home_position()->alt - : _mission_item.altitude; + ? _mission_item.altitude + _navigator->get_home_position()->alt + : _mission_item.altitude; dist = get_distance_to_point_global_wgs84(_mission_item.lat, _mission_item.lon, altitude_amsl, - _navigator->get_global_position()->lat, - _navigator->get_global_position()->lon, - _navigator->get_global_position()->alt, - &dist_xy, &dist_z); + _navigator->get_global_position()->lat, + _navigator->get_global_position()->lon, + _navigator->get_global_position()->alt, + &dist_xy, &dist_z); /* FW special case for NAV_CMD_WAYPOINT to achieve altitude via loiter */ if (!_navigator->get_vstatus()->is_rotary_wing && - (_mission_item.nav_cmd == NAV_CMD_WAYPOINT)) { + (_mission_item.nav_cmd == NAV_CMD_WAYPOINT)) { struct position_setpoint_s *curr_sp = &_navigator->get_position_setpoint_triplet()->current; + /* close to waypoint, but altitude error greater than twice acceptance */ if ((dist >= 0.0f) - && (dist_z > 2 * _navigator->get_altitude_acceptance_radius()) - && (dist_xy < 2 * _navigator->get_loiter_radius())) { + && (dist_z > 2 * _navigator->get_altitude_acceptance_radius()) + && (dist_xy < 2 * _navigator->get_loiter_radius())) { /* SETPOINT_TYPE_POSITION -> SETPOINT_TYPE_LOITER */ if (curr_sp->type == position_setpoint_s::SETPOINT_TYPE_POSITION) { @@ -176,13 +179,14 @@ MissionBlock::is_mission_item_reached() curr_sp->loiter_direction = 1; _navigator->set_position_setpoint_triplet_updated(); } + } else { /* restore SETPOINT_TYPE_POSITION */ if (curr_sp->type == position_setpoint_s::SETPOINT_TYPE_LOITER) { /* loiter acceptance criteria required to revert back to SETPOINT_TYPE_POSITION */ if ((dist >= 0.0f) - && (dist_z < _navigator->get_loiter_radius()) - && (dist_xy <= _navigator->get_loiter_radius() * 1.2f)) { + && (dist_z < _navigator->get_loiter_radius()) + && (dist_xy <= _navigator->get_loiter_radius() * 1.2f)) { curr_sp->type = position_setpoint_s::SETPOINT_TYPE_POSITION; _navigator->set_position_setpoint_triplet_updated(); @@ -192,7 +196,7 @@ MissionBlock::is_mission_item_reached() } if ((_mission_item.nav_cmd == NAV_CMD_TAKEOFF || _mission_item.nav_cmd == NAV_CMD_VTOL_TAKEOFF) - && _navigator->get_vstatus()->is_rotary_wing) { + && _navigator->get_vstatus()->is_rotary_wing) { /* We want to avoid the edge case where the acceptance radius is bigger or equal than * the altitude of the takeoff waypoint above home. Otherwise, we do not really follow @@ -211,18 +215,20 @@ MissionBlock::is_mission_item_reached() /* require only altitude for takeoff for multicopter */ if (_navigator->get_global_position()->alt > - altitude_amsl - altitude_acceptance_radius) { + altitude_amsl - altitude_acceptance_radius) { _waypoint_position_reached = true; } + } else if (_mission_item.nav_cmd == NAV_CMD_TAKEOFF) { /* for takeoff mission items use the parameter for the takeoff acceptance radius */ if (dist >= 0.0f && dist <= _navigator->get_acceptance_radius() - && dist_z <= _navigator->get_altitude_acceptance_radius()) { + && dist_z <= _navigator->get_altitude_acceptance_radius()) { _waypoint_position_reached = true; } + } else if (!_navigator->get_vstatus()->is_rotary_wing && - (_mission_item.nav_cmd == NAV_CMD_LOITER_UNLIMITED || - _mission_item.nav_cmd == NAV_CMD_LOITER_TIME_LIMIT)) { + (_mission_item.nav_cmd == NAV_CMD_LOITER_UNLIMITED || + _mission_item.nav_cmd == NAV_CMD_LOITER_TIME_LIMIT)) { /* Loiter mission item on a non rotary wing: the aircraft is going to circle the * coordinates with a radius equal to the loiter_radius field. It is not flying * through the waypoint center. @@ -230,15 +236,16 @@ MissionBlock::is_mission_item_reached() * radius (+ some margin). Time inside and turn count is handled elsewhere. */ if (dist >= 0.0f && dist <= _navigator->get_acceptance_radius(fabsf(_mission_item.loiter_radius) * 1.2f) - && dist_z <= _navigator->get_altitude_acceptance_radius()) { + && dist_z <= _navigator->get_altitude_acceptance_radius()) { _waypoint_position_reached = true; + } else { _time_first_inside_orbit = 0; } } else if (!_navigator->get_vstatus()->is_rotary_wing && - (_mission_item.nav_cmd == NAV_CMD_LOITER_TO_ALT)) { + (_mission_item.nav_cmd == NAV_CMD_LOITER_TO_ALT)) { // NAV_CMD_LOITER_TO_ALT only uses mission item altitude once it's in the loiter @@ -252,13 +259,13 @@ MissionBlock::is_mission_item_reached() dist_z = -1.0f; dist = get_distance_to_point_global_wgs84(_mission_item.lat, _mission_item.lon, curr_sp->alt, - _navigator->get_global_position()->lat, - _navigator->get_global_position()->lon, - _navigator->get_global_position()->alt, - &dist_xy, &dist_z); + _navigator->get_global_position()->lat, + _navigator->get_global_position()->lon, + _navigator->get_global_position()->alt, + &dist_xy, &dist_z); if (dist >= 0.0f && dist <= _navigator->get_acceptance_radius(fabsf(_mission_item.loiter_radius) * 1.2f) - && dist_z <= _navigator->get_altitude_acceptance_radius()) { + && dist_z <= _navigator->get_altitude_acceptance_radius()) { // now set the loiter to the final altitude in the NAV_CMD_LOITER_TO_ALT mission item curr_sp->alt = altitude_amsl; @@ -267,7 +274,7 @@ MissionBlock::is_mission_item_reached() } else { if (dist >= 0.0f && dist <= _navigator->get_acceptance_radius(fabsf(_mission_item.loiter_radius) * 1.2f) - && dist_z <= _navigator->get_altitude_acceptance_radius()) { + && dist_z <= _navigator->get_altitude_acceptance_radius()) { _waypoint_position_reached = true; @@ -277,16 +284,18 @@ MissionBlock::is_mission_item_reached() if (next_sp.valid) { _mission_item.yaw = get_bearing_to_next_waypoint(_navigator->get_global_position()->lat, - _navigator->get_global_position()->lon, - next_sp.lat, next_sp.lon); + _navigator->get_global_position()->lon, + next_sp.lat, next_sp.lon); _waypoint_yaw_reached = false; + } else { _waypoint_yaw_reached = true; } } } } + } else { /* for normal mission items used their acceptance radius */ float mission_acceptance_radius = _navigator->get_acceptance_radius(_mission_item.acceptance_radius); @@ -297,7 +306,7 @@ MissionBlock::is_mission_item_reached() } if (dist >= 0.0f && dist <= mission_acceptance_radius - && dist_z <= _navigator->get_altitude_acceptance_radius()) { + && dist_z <= _navigator->get_altitude_acceptance_radius()) { _waypoint_position_reached = true; } } @@ -313,23 +322,23 @@ MissionBlock::is_mission_item_reached() if (_waypoint_position_reached && !_waypoint_yaw_reached) { if ((_navigator->get_vstatus()->is_rotary_wing - || (_mission_item.nav_cmd == NAV_CMD_LOITER_TO_ALT && _mission_item.force_heading)) - && PX4_ISFINITE(_mission_item.yaw)) { + || (_mission_item.nav_cmd == NAV_CMD_LOITER_TO_ALT && _mission_item.force_heading)) + && PX4_ISFINITE(_mission_item.yaw)) { /* check yaw if defined only for rotary wing except takeoff */ float yaw_err = _wrap_pi(_mission_item.yaw - _navigator->get_global_position()->yaw); /* accept yaw if reached or if timeout is set in which case we ignore not forced headings */ if (fabsf(yaw_err) < math::radians(_param_yaw_err.get()) - || (_param_yaw_timeout.get() >= FLT_EPSILON && !_mission_item.force_heading)) { + || (_param_yaw_timeout.get() >= FLT_EPSILON && !_mission_item.force_heading)) { _waypoint_yaw_reached = true; } /* if heading needs to be reached, the timeout is enabled and we don't make it, abort mission */ if (!_waypoint_yaw_reached && _mission_item.force_heading && - (_param_yaw_timeout.get() >= FLT_EPSILON) && - (now - _time_wp_reached >= (hrt_abstime)_param_yaw_timeout.get() * 1e6f)) { + (_param_yaw_timeout.get() >= FLT_EPSILON) && + (now - _time_wp_reached >= (hrt_abstime)_param_yaw_timeout.get() * 1e6f)) { _navigator->set_mission_failure("unable to reach heading within timeout"); } @@ -348,12 +357,12 @@ MissionBlock::is_mission_item_reached() /* check if the MAV was long enough inside the waypoint orbit */ if ((Navigator::get_time_inside(_mission_item) < FLT_EPSILON) || - (now - _time_first_inside_orbit >= (hrt_abstime)(Navigator::get_time_inside(_mission_item) * 1e6f))) { + (now - _time_first_inside_orbit >= (hrt_abstime)(Navigator::get_time_inside(_mission_item) * 1e6f))) { // exit xtrack location if (_mission_item.loiter_exit_xtrack && - (_mission_item.nav_cmd == NAV_CMD_LOITER_TIME_LIMIT || - _mission_item.nav_cmd == NAV_CMD_LOITER_TO_ALT)) { + (_mission_item.nav_cmd == NAV_CMD_LOITER_TIME_LIMIT || + _mission_item.nav_cmd == NAV_CMD_LOITER_TO_ALT)) { // reset lat/lon of loiter waypoint so vehicle exits on a tangent struct position_setpoint_s *curr_sp = &_navigator->get_position_setpoint_triplet()->current; @@ -399,15 +408,16 @@ MissionBlock::mission_item_to_vehicle_command(const struct mission_item_s *item, // The camera commands are not processed on the autopilot but will be // sent to the mavlink links to other components. switch (item->nav_cmd) { - case NAV_CMD_IMAGE_START_CAPTURE: - case NAV_CMD_IMAGE_STOP_CAPTURE: - case NAV_CMD_VIDEO_START_CAPTURE: - case NAV_CMD_VIDEO_STOP_CAPTURE: - cmd->target_component = 100; // MAV_COMP_ID_CAMERA - break; - default: - cmd->target_component = _navigator->get_vstatus()->component_id; - break; + case NAV_CMD_IMAGE_START_CAPTURE: + case NAV_CMD_IMAGE_STOP_CAPTURE: + case NAV_CMD_VIDEO_START_CAPTURE: + case NAV_CMD_VIDEO_STOP_CAPTURE: + cmd->target_component = 100; // MAV_COMP_ID_CAMERA + break; + + default: + cmd->target_component = _navigator->get_vstatus()->component_id; + break; } cmd->source_system = _navigator->get_vstatus()->system_id; @@ -463,20 +473,20 @@ MissionBlock::item_contains_position(const struct mission_item_s *item) { // XXX: maybe extend that check onto item properties if (item->nav_cmd == NAV_CMD_DO_JUMP || - item->nav_cmd == NAV_CMD_DO_CHANGE_SPEED || - item->nav_cmd == NAV_CMD_DO_SET_SERVO || - item->nav_cmd == NAV_CMD_DO_LAND_START || - item->nav_cmd == NAV_CMD_DO_DIGICAM_CONTROL || - item->nav_cmd == NAV_CMD_IMAGE_START_CAPTURE || - item->nav_cmd == NAV_CMD_IMAGE_STOP_CAPTURE || - item->nav_cmd == NAV_CMD_VIDEO_START_CAPTURE || - item->nav_cmd == NAV_CMD_VIDEO_STOP_CAPTURE || - item->nav_cmd == NAV_CMD_DO_MOUNT_CONFIGURE || - item->nav_cmd == NAV_CMD_DO_MOUNT_CONTROL || - item->nav_cmd == NAV_CMD_DO_SET_ROI || - item->nav_cmd == NAV_CMD_ROI || - item->nav_cmd == NAV_CMD_DO_SET_CAM_TRIGG_DIST || - item->nav_cmd == NAV_CMD_DO_VTOL_TRANSITION) { + item->nav_cmd == NAV_CMD_DO_CHANGE_SPEED || + item->nav_cmd == NAV_CMD_DO_SET_SERVO || + item->nav_cmd == NAV_CMD_DO_LAND_START || + item->nav_cmd == NAV_CMD_DO_DIGICAM_CONTROL || + item->nav_cmd == NAV_CMD_IMAGE_START_CAPTURE || + item->nav_cmd == NAV_CMD_IMAGE_STOP_CAPTURE || + item->nav_cmd == NAV_CMD_VIDEO_START_CAPTURE || + item->nav_cmd == NAV_CMD_VIDEO_STOP_CAPTURE || + item->nav_cmd == NAV_CMD_DO_MOUNT_CONFIGURE || + item->nav_cmd == NAV_CMD_DO_MOUNT_CONTROL || + item->nav_cmd == NAV_CMD_DO_SET_ROI || + item->nav_cmd == NAV_CMD_ROI || + item->nav_cmd == NAV_CMD_DO_SET_CAM_TRIGG_DIST || + item->nav_cmd == NAV_CMD_DO_VTOL_TRANSITION) { return false; } @@ -497,7 +507,7 @@ MissionBlock::mission_item_to_position_setpoint(const struct mission_item_s *ite sp->alt = item->altitude_is_relative ? item->altitude + _navigator->get_home_position()->alt : item->altitude; sp->yaw = item->yaw; sp->loiter_radius = (fabsf(item->loiter_radius) > NAV_EPSILON_POSITION) ? fabsf(item->loiter_radius) : - _navigator->get_loiter_radius(); + _navigator->get_loiter_radius(); sp->loiter_direction = (item->loiter_radius > 0) ? 1 : -1; sp->acceptance_radius = item->acceptance_radius; sp->disable_mc_yaw_control = item->disable_mc_yaw; @@ -514,33 +524,40 @@ MissionBlock::mission_item_to_position_setpoint(const struct mission_item_s *ite // set pitch and ensure that the hold time is zero sp->pitch_min = item->pitch_min; - // fall through + // fall through case NAV_CMD_VTOL_TAKEOFF: sp->type = position_setpoint_s::SETPOINT_TYPE_TAKEOFF; - if(_navigator->get_vstatus()->is_vtol && _param_vtol_wv_takeoff.get()){ + + if (_navigator->get_vstatus()->is_vtol && _param_vtol_wv_takeoff.get()) { sp->disable_mc_yaw_control = true; } + break; case NAV_CMD_LAND: case NAV_CMD_VTOL_LAND: sp->type = position_setpoint_s::SETPOINT_TYPE_LAND; + if (_navigator->get_vstatus()->is_vtol && _param_vtol_wv_land.get()) { sp->disable_mc_yaw_control = true; } + break; case NAV_CMD_LOITER_TO_ALT: // initially use current altitude, and switch to mission item altitude once in loiter position - sp->alt = math::max(_navigator->get_global_position()->alt, _navigator->get_home_position()->alt + _param_loiter_min_alt.get()); + sp->alt = math::max(_navigator->get_global_position()->alt, + _navigator->get_home_position()->alt + _param_loiter_min_alt.get()); - // fall through + // fall through case NAV_CMD_LOITER_TIME_LIMIT: case NAV_CMD_LOITER_UNLIMITED: sp->type = position_setpoint_s::SETPOINT_TYPE_LOITER; + if (_navigator->get_vstatus()->is_vtol && _param_vtol_wv_loiter.get()) { sp->disable_mc_yaw_control = true; } + break; default: @@ -601,7 +618,8 @@ MissionBlock::set_loiter_item(struct mission_item_s *item, float min_clearance) } void -MissionBlock::set_follow_target_item(struct mission_item_s *item, float min_clearance, follow_target_s & target, float yaw) +MissionBlock::set_follow_target_item(struct mission_item_s *item, float min_clearance, follow_target_s &target, + float yaw) { if (_navigator->get_land_detected()->landed) { /* landed, don't takeoff, but switch to IDLE mode */ @@ -619,6 +637,7 @@ MissionBlock::set_follow_target_item(struct mission_item_s *item, float min_clea if (min_clearance > 8.0f) { item->altitude += min_clearance; + } else { item->altitude += 8.0f; // if min clearance is bad set it to 8.0 meters (well above the average height of a person) } @@ -658,12 +677,14 @@ MissionBlock::set_land_item(struct mission_item_s *item, bool at_current_locatio /* VTOL transition to RW before landing */ if (_navigator->get_vstatus()->is_vtol && !_navigator->get_vstatus()->is_rotary_wing && - _param_force_vtol.get() == 1) { + _param_force_vtol.get() == 1) { struct vehicle_command_s cmd = {}; cmd.command = NAV_CMD_DO_VTOL_TRANSITION; cmd.param1 = vtol_vehicle_status_s::VEHICLE_VTOL_STATE_MC; + if (_cmd_pub != nullptr) { orb_publish(ORB_ID(vehicle_command), _cmd_pub, &cmd); + } else { _cmd_pub = orb_advertise_queue(ORB_ID(vehicle_command), &cmd, vehicle_command_s::ORB_QUEUE_LENGTH); } @@ -678,7 +699,8 @@ MissionBlock::set_land_item(struct mission_item_s *item, bool at_current_locatio item->lon = _navigator->get_global_position()->lon; item->yaw = _navigator->get_global_position()->yaw; - /* use home position */ + /* use home position */ + } else { item->lat = _navigator->get_home_position()->lat; item->lon = _navigator->get_home_position()->lon; diff --git a/src/modules/navigator/mission_block.h b/src/modules/navigator/mission_block.h index 25efa4737e..573d234819 100644 --- a/src/modules/navigator/mission_block.h +++ b/src/modules/navigator/mission_block.h @@ -129,7 +129,7 @@ protected: /** * Set follow_target item */ - void set_follow_target_item(struct mission_item_s *item, float min_clearance, follow_target_s & target, float yaw); + void set_follow_target_item(struct mission_item_s *item, float min_clearance, follow_target_s &target, float yaw); void issue_command(const struct mission_item_s *item); diff --git a/src/modules/navigator/mission_feasibility_checker.cpp b/src/modules/navigator/mission_feasibility_checker.cpp index 16998dec28..6233f533f0 100644 --- a/src/modules/navigator/mission_feasibility_checker.cpp +++ b/src/modules/navigator/mission_feasibility_checker.cpp @@ -64,10 +64,10 @@ MissionFeasibilityChecker::MissionFeasibilityChecker() : bool MissionFeasibilityChecker::checkMissionFeasible(orb_advert_t *mavlink_log_pub, bool isRotarywing, - dm_item_t dm_current, size_t nMissionItems, Geofence &geofence, - float home_alt, bool home_valid, double curr_lat, double curr_lon, float max_waypoint_distance, bool &warning_issued, - float default_acceptance_rad, - bool condition_landed) + dm_item_t dm_current, size_t nMissionItems, Geofence &geofence, + float home_alt, bool home_valid, double curr_lat, double curr_lon, float max_waypoint_distance, bool &warning_issued, + float default_acceptance_rad, + bool condition_landed) { bool failed = false; bool warned = false; @@ -81,8 +81,10 @@ bool MissionFeasibilityChecker::checkMissionFeasible(orb_advert_t *mavlink_log_p failed = true; warned = true; mavlink_log_info(_mavlink_log_pub, "Not yet ready for mission, no position lock."); + } else { - failed = failed || !check_dist_1wp(dm_current, nMissionItems, curr_lat, curr_lon, max_waypoint_distance, warning_issued); + failed = failed + || !check_dist_1wp(dm_current, nMissionItems, curr_lat, curr_lon, max_waypoint_distance, warning_issued); } // check if all mission item commands are supported @@ -91,7 +93,9 @@ bool MissionFeasibilityChecker::checkMissionFeasible(orb_advert_t *mavlink_log_p failed = failed || !checkHomePositionAltitude(dm_current, nMissionItems, home_alt, home_valid, warned); if (isRotarywing) { - failed = failed || !checkMissionFeasibleRotarywing(dm_current, nMissionItems, geofence, home_alt, home_valid, default_acceptance_rad); + failed = failed + || !checkMissionFeasibleRotarywing(dm_current, nMissionItems, geofence, home_alt, home_valid, default_acceptance_rad); + } else { failed = failed || !checkMissionFeasibleFixedwing(dm_current, nMissionItems, geofence, home_alt, home_valid); } @@ -100,7 +104,7 @@ bool MissionFeasibilityChecker::checkMissionFeasible(orb_advert_t *mavlink_log_p } bool MissionFeasibilityChecker::checkMissionFeasibleRotarywing(dm_item_t dm_current, size_t nMissionItems, - Geofence &geofence, float home_alt, bool home_valid, float default_acceptance_rad) + Geofence &geofence, float home_alt, bool home_valid, float default_acceptance_rad) { /* Check if all all waypoints are above the home altitude, only return false if bool throw_error = true */ for (size_t i = 0; i < nMissionItems; i++) { @@ -117,8 +121,8 @@ bool MissionFeasibilityChecker::checkMissionFeasibleRotarywing(dm_item_t dm_curr // make sure that the altitude of the waypoint is at least one meter larger than the acceptance radius // this makes sure that the takeoff waypoint is not reached before we are at least one meter in the air float takeoff_alt = missionitem.altitude_is_relative - ? missionitem.altitude - : missionitem.altitude - home_alt; + ? missionitem.altitude + : missionitem.altitude - home_alt; // check if we should use default acceptance radius float acceptance_radius = default_acceptance_rad; @@ -137,7 +141,8 @@ bool MissionFeasibilityChecker::checkMissionFeasibleRotarywing(dm_item_t dm_curr return true; } -bool MissionFeasibilityChecker::checkMissionFeasibleFixedwing(dm_item_t dm_current, size_t nMissionItems, Geofence &geofence, float home_alt, bool home_valid) +bool MissionFeasibilityChecker::checkMissionFeasibleFixedwing(dm_item_t dm_current, size_t nMissionItems, + Geofence &geofence, float home_alt, bool home_valid) { /* Update fixed wing navigation capabilites */ updateNavigationCapabilities(); @@ -149,7 +154,8 @@ bool MissionFeasibilityChecker::checkMissionFeasibleFixedwing(dm_item_t dm_curre return resLanding; } -bool MissionFeasibilityChecker::checkGeofence(dm_item_t dm_current, size_t nMissionItems, Geofence &geofence, float home_alt) +bool MissionFeasibilityChecker::checkGeofence(dm_item_t dm_current, size_t nMissionItems, Geofence &geofence, + float home_alt) { /* Check if all mission items are inside the geofence (if we have a valid geofence) */ if (geofence.valid()) { @@ -164,11 +170,11 @@ bool MissionFeasibilityChecker::checkGeofence(dm_item_t dm_current, size_t nMiss // Geofence function checks against home altitude amsl missionitem.altitude = missionitem.altitude_is_relative - ? missionitem.altitude + home_alt - : missionitem.altitude; + ? missionitem.altitude + home_alt + : missionitem.altitude; if (MissionBlock::item_contains_position(&missionitem) && - !geofence.inside(missionitem)) { + !geofence.inside(missionitem)) { mavlink_log_critical(_mavlink_log_pub, "Geofence violation for waypoint %d", i + 1); return false; @@ -180,7 +186,7 @@ bool MissionFeasibilityChecker::checkGeofence(dm_item_t dm_current, size_t nMiss } bool MissionFeasibilityChecker::checkHomePositionAltitude(dm_item_t dm_current, size_t nMissionItems, - float home_alt, bool home_valid, bool &warning_issued, bool throw_error) + float home_alt, bool home_valid, bool &warning_issued, bool throw_error) { /* Check if all waypoints are above the home altitude, only return false if bool throw_error = true */ for (size_t i = 0; i < nMissionItems; i++) { @@ -199,10 +205,11 @@ bool MissionFeasibilityChecker::checkHomePositionAltitude(dm_item_t dm_current, warning_issued = true; if (throw_error) { - mavlink_log_critical(_mavlink_log_pub, "Rejecting mission: No home pos, WP %d uses rel alt", i+1); + mavlink_log_critical(_mavlink_log_pub, "Rejecting mission: No home pos, WP %d uses rel alt", i + 1); return false; + } else { - mavlink_log_critical(_mavlink_log_pub, "Warning: No home pos, WP %d uses rel alt", i+1); + mavlink_log_critical(_mavlink_log_pub, "Warning: No home pos, WP %d uses rel alt", i + 1); return true; } } @@ -215,10 +222,11 @@ bool MissionFeasibilityChecker::checkHomePositionAltitude(dm_item_t dm_current, warning_issued = true; if (throw_error) { - mavlink_log_critical(_mavlink_log_pub, "Rejecting mission: Waypoint %d below home", i+1); + mavlink_log_critical(_mavlink_log_pub, "Rejecting mission: Waypoint %d below home", i + 1); return false; + } else { - mavlink_log_critical(_mavlink_log_pub, "Warning: Waypoint %d below home", i+1); + mavlink_log_critical(_mavlink_log_pub, "Warning: Waypoint %d below home", i + 1); return true; } } @@ -227,7 +235,9 @@ bool MissionFeasibilityChecker::checkHomePositionAltitude(dm_item_t dm_current, return true; } -bool MissionFeasibilityChecker::checkMissionItemValidity(dm_item_t dm_current, size_t nMissionItems, bool condition_landed) { +bool MissionFeasibilityChecker::checkMissionItemValidity(dm_item_t dm_current, size_t nMissionItems, + bool condition_landed) +{ // do not allow mission if we find unsupported item for (size_t i = 0; i < nMissionItems; i++) { struct mission_item_s missionitem; @@ -241,38 +251,39 @@ bool MissionFeasibilityChecker::checkMissionItemValidity(dm_item_t dm_current, s // check if we find unsupported items and reject mission if so if (missionitem.nav_cmd != NAV_CMD_IDLE && - missionitem.nav_cmd != NAV_CMD_WAYPOINT && - missionitem.nav_cmd != NAV_CMD_LOITER_UNLIMITED && - missionitem.nav_cmd != NAV_CMD_LOITER_TIME_LIMIT && - missionitem.nav_cmd != NAV_CMD_LAND && - missionitem.nav_cmd != NAV_CMD_TAKEOFF && - missionitem.nav_cmd != NAV_CMD_LOITER_TO_ALT && - missionitem.nav_cmd != NAV_CMD_VTOL_TAKEOFF && - missionitem.nav_cmd != NAV_CMD_VTOL_LAND && - missionitem.nav_cmd != NAV_CMD_DO_JUMP && - missionitem.nav_cmd != NAV_CMD_DO_CHANGE_SPEED && - missionitem.nav_cmd != NAV_CMD_DO_SET_SERVO && - missionitem.nav_cmd != NAV_CMD_DO_LAND_START && - missionitem.nav_cmd != NAV_CMD_DO_DIGICAM_CONTROL && - missionitem.nav_cmd != NAV_CMD_IMAGE_START_CAPTURE && - missionitem.nav_cmd != NAV_CMD_IMAGE_STOP_CAPTURE && - missionitem.nav_cmd != NAV_CMD_VIDEO_START_CAPTURE && - missionitem.nav_cmd != NAV_CMD_VIDEO_STOP_CAPTURE && - missionitem.nav_cmd != NAV_CMD_DO_MOUNT_CONFIGURE && - missionitem.nav_cmd != NAV_CMD_DO_MOUNT_CONTROL && - missionitem.nav_cmd != NAV_CMD_DO_SET_ROI && - missionitem.nav_cmd != NAV_CMD_ROI && - missionitem.nav_cmd != NAV_CMD_DO_SET_CAM_TRIGG_DIST && - missionitem.nav_cmd != NAV_CMD_DO_VTOL_TRANSITION) { + missionitem.nav_cmd != NAV_CMD_WAYPOINT && + missionitem.nav_cmd != NAV_CMD_LOITER_UNLIMITED && + missionitem.nav_cmd != NAV_CMD_LOITER_TIME_LIMIT && + missionitem.nav_cmd != NAV_CMD_LAND && + missionitem.nav_cmd != NAV_CMD_TAKEOFF && + missionitem.nav_cmd != NAV_CMD_LOITER_TO_ALT && + missionitem.nav_cmd != NAV_CMD_VTOL_TAKEOFF && + missionitem.nav_cmd != NAV_CMD_VTOL_LAND && + missionitem.nav_cmd != NAV_CMD_DO_JUMP && + missionitem.nav_cmd != NAV_CMD_DO_CHANGE_SPEED && + missionitem.nav_cmd != NAV_CMD_DO_SET_SERVO && + missionitem.nav_cmd != NAV_CMD_DO_LAND_START && + missionitem.nav_cmd != NAV_CMD_DO_DIGICAM_CONTROL && + missionitem.nav_cmd != NAV_CMD_IMAGE_START_CAPTURE && + missionitem.nav_cmd != NAV_CMD_IMAGE_STOP_CAPTURE && + missionitem.nav_cmd != NAV_CMD_VIDEO_START_CAPTURE && + missionitem.nav_cmd != NAV_CMD_VIDEO_STOP_CAPTURE && + missionitem.nav_cmd != NAV_CMD_DO_MOUNT_CONFIGURE && + missionitem.nav_cmd != NAV_CMD_DO_MOUNT_CONTROL && + missionitem.nav_cmd != NAV_CMD_DO_SET_ROI && + missionitem.nav_cmd != NAV_CMD_ROI && + missionitem.nav_cmd != NAV_CMD_DO_SET_CAM_TRIGG_DIST && + missionitem.nav_cmd != NAV_CMD_DO_VTOL_TRANSITION) { - mavlink_log_critical(_mavlink_log_pub, "Rejecting mission item %i: unsupported cmd: %d", (int)(i+1), (int)missionitem.nav_cmd); + mavlink_log_critical(_mavlink_log_pub, "Rejecting mission item %i: unsupported cmd: %d", (int)(i + 1), + (int)missionitem.nav_cmd); return false; } // check if the mission starts with a land command while the vehicle is landed if (missionitem.nav_cmd == NAV_CMD_LAND && - i == 0 && - condition_landed) { + i == 0 && + condition_landed) { mavlink_log_critical(_mavlink_log_pub, "Rejecting mission that starts with LAND command while vehicle is landed."); return false; @@ -280,6 +291,7 @@ bool MissionFeasibilityChecker::checkMissionItemValidity(dm_item_t dm_current, s } + return true; } @@ -291,6 +303,7 @@ bool MissionFeasibilityChecker::checkFixedWingLanding(dm_item_t dm_current, size for (size_t i = 0; i < nMissionItems; i++) { struct mission_item_s missionitem; const ssize_t len = sizeof(missionitem); + if (dm_read(dm_current, i, &missionitem, len) != len) { /* not supposed to happen unless the datamanager can't access the SD card, etc. */ return false; @@ -298,15 +311,19 @@ bool MissionFeasibilityChecker::checkFixedWingLanding(dm_item_t dm_current, size if (missionitem.nav_cmd == NAV_CMD_LAND) { struct mission_item_s missionitem_previous; + if (i != 0) { - if (dm_read(dm_current, i-1, &missionitem_previous, len) != len) { + if (dm_read(dm_current, i - 1, &missionitem_previous, len) != len) { /* not supposed to happen unless the datamanager can't access the SD card, etc. */ return false; } - float wp_distance = get_distance_to_next_waypoint(missionitem_previous.lat , missionitem_previous.lon, missionitem.lat, missionitem.lon); - float slope_alt_req = Landingslope::getLandingSlopeAbsoluteAltitude(wp_distance, missionitem.altitude, _fw_pos_ctrl_status.landing_horizontal_slope_displacement, _fw_pos_ctrl_status.landing_slope_angle_rad); - float wp_distance_req = Landingslope::getLandingSlopeWPDistance(missionitem_previous.altitude, missionitem.altitude, _fw_pos_ctrl_status.landing_horizontal_slope_displacement, _fw_pos_ctrl_status.landing_slope_angle_rad); + float wp_distance = get_distance_to_next_waypoint(missionitem_previous.lat , missionitem_previous.lon, missionitem.lat, + missionitem.lon); + float slope_alt_req = Landingslope::getLandingSlopeAbsoluteAltitude(wp_distance, missionitem.altitude, + _fw_pos_ctrl_status.landing_horizontal_slope_displacement, _fw_pos_ctrl_status.landing_slope_angle_rad); + float wp_distance_req = Landingslope::getLandingSlopeWPDistance(missionitem_previous.altitude, missionitem.altitude, + _fw_pos_ctrl_status.landing_horizontal_slope_displacement, _fw_pos_ctrl_status.landing_slope_angle_rad); float delta_altitude = missionitem.altitude - missionitem_previous.altitude; // warnx("wp_distance %.2f, delta_altitude %.2f, missionitem_previous.altitude %.2f, missionitem.altitude %.2f, slope_alt_req %.2f, wp_distance_req %.2f", // wp_distance, delta_altitude, missionitem_previous.altitude, missionitem.altitude, slope_alt_req, wp_distance_req); @@ -320,25 +337,29 @@ bool MissionFeasibilityChecker::checkFixedWingLanding(dm_item_t dm_current, size if (missionitem_previous.altitude <= slope_alt_req) { /* Landing waypoint is at or below altitude of slope at the given waypoint distance: this is ok, aircraft will intersect the slope */ return true; + } else { /* Landing waypoint is above altitude of slope at the given waypoint distance */ mavlink_log_critical(_mavlink_log_pub, "Landing: last waypoint too high/too close"); mavlink_log_critical(_mavlink_log_pub, "Move down to %.1fm or move further away by %.1fm", - (double)(slope_alt_req), - (double)(wp_distance_req - wp_distance)); + (double)(slope_alt_req), + (double)(wp_distance_req - wp_distance)); return false; } + } else { /* Landing waypoint is above last waypoint */ mavlink_log_critical(_mavlink_log_pub, "Landing waypoint above last nav waypoint"); return false; } + } else { /* Last wp is in flare region */ //xxx give recommendations mavlink_log_critical(_mavlink_log_pub, "Last waypoint too close to landing waypoint"); return false; } + } else { mavlink_log_critical(_mavlink_log_pub, "Invalid mission: starts with land waypoint"); return false; @@ -351,7 +372,8 @@ bool MissionFeasibilityChecker::checkFixedWingLanding(dm_item_t dm_current, size } bool -MissionFeasibilityChecker::check_dist_1wp(dm_item_t dm_current, size_t nMissionItems, double curr_lat, double curr_lon, float dist_first_wp, bool &warning_issued) +MissionFeasibilityChecker::check_dist_1wp(dm_item_t dm_current, size_t nMissionItems, double curr_lat, double curr_lon, + float dist_first_wp, bool &warning_issued) { /* check if first waypoint is not too far from home */ @@ -361,9 +383,9 @@ MissionFeasibilityChecker::check_dist_1wp(dm_item_t dm_current, size_t nMissionI /* find first waypoint (with lat/lon) item in datamanager */ for (unsigned i = 0; i < nMissionItems; i++) { if (dm_read(dm_current, i, - &mission_item, sizeof(mission_item_s)) == sizeof(mission_item_s)) { + &mission_item, sizeof(mission_item_s)) == sizeof(mission_item_s)) { /* Check non navigation item */ - if (mission_item.nav_cmd == NAV_CMD_DO_SET_SERVO){ + if (mission_item.nav_cmd == NAV_CMD_DO_SET_SERVO) { /* check actuator number */ if (mission_item.params[0] < 0 || mission_item.params[0] > 5) { @@ -371,6 +393,7 @@ MissionFeasibilityChecker::check_dist_1wp(dm_item_t dm_current, size_t nMissionI warning_issued = true; return false; } + /* check actuator value */ if (mission_item.params[1] < -2000 || mission_item.params[1] > 2000) { mavlink_log_critical(_mavlink_log_pub, "Actuator value %d is out of bounds -2000..2000", (int)mission_item.params[1]); @@ -378,20 +401,23 @@ MissionFeasibilityChecker::check_dist_1wp(dm_item_t dm_current, size_t nMissionI return false; } } + /* check only items with valid lat/lon */ else if (isPositionCommand(mission_item.nav_cmd)) { /* check distance from current position to item */ float dist_to_1wp = get_distance_to_next_waypoint( - mission_item.lat, mission_item.lon, curr_lat, curr_lon); + mission_item.lat, mission_item.lon, curr_lat, curr_lon); if (dist_to_1wp < dist_first_wp) { _dist_1wp_ok = true; + if (dist_to_1wp > ((dist_first_wp * 3) / 2)) { /* allow at 2/3 distance, but warn */ mavlink_log_critical(_mavlink_log_pub, "Warning: First waypoint very far: %d m", (int)dist_to_1wp); warning_issued = true; } + return true; } else { @@ -420,17 +446,19 @@ MissionFeasibilityChecker::check_dist_1wp(dm_item_t dm_current, size_t nMissionI } bool -MissionFeasibilityChecker::isPositionCommand(unsigned cmd){ +MissionFeasibilityChecker::isPositionCommand(unsigned cmd) +{ if (cmd == NAV_CMD_WAYPOINT || - cmd == NAV_CMD_LOITER_UNLIMITED || - cmd == NAV_CMD_LOITER_TIME_LIMIT || - cmd == NAV_CMD_LAND || - cmd == NAV_CMD_TAKEOFF || - cmd == NAV_CMD_LOITER_TO_ALT || - cmd == NAV_CMD_VTOL_TAKEOFF || - cmd == NAV_CMD_VTOL_LAND) { + cmd == NAV_CMD_LOITER_UNLIMITED || + cmd == NAV_CMD_LOITER_TIME_LIMIT || + cmd == NAV_CMD_LAND || + cmd == NAV_CMD_TAKEOFF || + cmd == NAV_CMD_LOITER_TO_ALT || + cmd == NAV_CMD_VTOL_TAKEOFF || + cmd == NAV_CMD_VTOL_LAND) { return true; + } else { return false; diff --git a/src/modules/navigator/mission_feasibility_checker.h b/src/modules/navigator/mission_feasibility_checker.h index a0df970aa2..71df42d5fa 100644 --- a/src/modules/navigator/mission_feasibility_checker.h +++ b/src/modules/navigator/mission_feasibility_checker.h @@ -63,18 +63,22 @@ private: /* Checks for all airframes */ bool checkGeofence(dm_item_t dm_current, size_t nMissionItems, Geofence &geofence, float home_alt); - bool checkHomePositionAltitude(dm_item_t dm_current, size_t nMissionItems, float home_alt, bool home_valid, bool &warning_issued, bool throw_error = false); + bool checkHomePositionAltitude(dm_item_t dm_current, size_t nMissionItems, float home_alt, bool home_valid, + bool &warning_issued, bool throw_error = false); bool checkMissionItemValidity(dm_item_t dm_current, size_t nMissionItems, bool condition_landed); - bool check_dist_1wp(dm_item_t dm_current, size_t nMissionItems, double curr_lat, double curr_lon, float dist_first_wp, bool &warning_issued); + bool check_dist_1wp(dm_item_t dm_current, size_t nMissionItems, double curr_lat, double curr_lon, float dist_first_wp, + bool &warning_issued); bool isPositionCommand(unsigned cmd); /* Checks specific to fixedwing airframes */ - bool checkMissionFeasibleFixedwing(dm_item_t dm_current, size_t nMissionItems, Geofence &geofence, float home_alt, bool home_valid); + bool checkMissionFeasibleFixedwing(dm_item_t dm_current, size_t nMissionItems, Geofence &geofence, float home_alt, + bool home_valid); bool checkFixedWingLanding(dm_item_t dm_current, size_t nMissionItems); void updateNavigationCapabilities(); /* Checks specific to rotarywing airframes */ - bool checkMissionFeasibleRotarywing(dm_item_t dm_current, size_t nMissionItems, Geofence &geofence, float home_alt, bool home_valid, float default_acceptance_rad); + bool checkMissionFeasibleRotarywing(dm_item_t dm_current, size_t nMissionItems, Geofence &geofence, float home_alt, + bool home_valid, float default_acceptance_rad); public: MissionFeasibilityChecker(); @@ -88,9 +92,9 @@ public: * Returns true if mission is feasible and false otherwise */ bool checkMissionFeasible(orb_advert_t *mavlink_log_pub, bool isRotarywing, dm_item_t dm_current, - size_t nMissionItems, Geofence &geofence, float home_alt, bool home_valid, - double curr_lat, double curr_lon, float max_waypoint_distance, bool &warning_issued, float default_acceptance_rad, - bool condition_landed); + size_t nMissionItems, Geofence &geofence, float home_alt, bool home_valid, + double curr_lat, double curr_lon, float max_waypoint_distance, bool &warning_issued, float default_acceptance_rad, + bool condition_landed); }; diff --git a/src/modules/navigator/navigation.h b/src/modules/navigator/navigation.h index 29a686f8f8..46a5a5832f 100644 --- a/src/modules/navigator/navigation.h +++ b/src/modules/navigator/navigation.h @@ -69,19 +69,19 @@ enum NAV_CMD { NAV_CMD_VTOL_LAND = 85, NAV_CMD_DO_JUMP = 177, NAV_CMD_DO_CHANGE_SPEED = 178, - NAV_CMD_DO_SET_SERVO=183, - NAV_CMD_DO_LAND_START=189, - NAV_CMD_DO_SET_ROI=201, - NAV_CMD_DO_DIGICAM_CONTROL=203, - NAV_CMD_DO_MOUNT_CONFIGURE=204, - NAV_CMD_DO_MOUNT_CONTROL=205, - NAV_CMD_DO_SET_CAM_TRIGG_DIST=206, - NAV_CMD_IMAGE_START_CAPTURE=2000, - NAV_CMD_IMAGE_STOP_CAPTURE=2001, - NAV_CMD_VIDEO_START_CAPTURE=2500, - NAV_CMD_VIDEO_STOP_CAPTURE=2501, - NAV_CMD_DO_VTOL_TRANSITION=3000, - NAV_CMD_INVALID=UINT16_MAX /* ensure that casting a large number results in a specific error */ + NAV_CMD_DO_SET_SERVO = 183, + NAV_CMD_DO_LAND_START = 189, + NAV_CMD_DO_SET_ROI = 201, + NAV_CMD_DO_DIGICAM_CONTROL = 203, + NAV_CMD_DO_MOUNT_CONFIGURE = 204, + NAV_CMD_DO_MOUNT_CONTROL = 205, + NAV_CMD_DO_SET_CAM_TRIGG_DIST = 206, + NAV_CMD_IMAGE_START_CAPTURE = 2000, + NAV_CMD_IMAGE_STOP_CAPTURE = 2001, + NAV_CMD_VIDEO_START_CAPTURE = 2500, + NAV_CMD_VIDEO_STOP_CAPTURE = 2501, + NAV_CMD_DO_VTOL_TRANSITION = 3000, + NAV_CMD_INVALID = UINT16_MAX /* ensure that casting a large number results in a specific error */ }; enum ORIGIN { @@ -125,12 +125,12 @@ struct mission_item_s { uint16_t do_jump_current_count; /**< count how many times the jump has been done */ 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 */ - disable_mc_yaw : 1; /**< weathervane mode */ + 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 */ + disable_mc_yaw : 1; /**< weathervane mode */ }; }; #pragma pack(pop) diff --git a/src/modules/navigator/navigator.h b/src/modules/navigator/navigator.h index 3979754abd..ffb1c6250a 100644 --- a/src/modules/navigator/navigator.h +++ b/src/modules/navigator/navigator.h @@ -133,24 +133,24 @@ public: /** * Getters */ - struct vehicle_status_s* get_vstatus() { return &_vstatus; } - struct vehicle_land_detected_s* get_land_detected() { return &_land_detected; } - struct vehicle_control_mode_s* get_control_mode() { return &_control_mode; } - struct vehicle_global_position_s* get_global_position() { return &_global_pos; } - struct vehicle_gps_position_s* get_gps_position() { return &_gps_pos; } - struct sensor_combined_s* get_sensor_combined() { return &_sensor_combined; } - struct home_position_s* get_home_position() { return &_home_pos; } + struct vehicle_status_s *get_vstatus() { return &_vstatus; } + struct vehicle_land_detected_s *get_land_detected() { return &_land_detected; } + struct vehicle_control_mode_s *get_control_mode() { return &_control_mode; } + struct vehicle_global_position_s *get_global_position() { return &_global_pos; } + struct vehicle_gps_position_s *get_gps_position() { return &_gps_pos; } + struct sensor_combined_s *get_sensor_combined() { return &_sensor_combined; } + struct home_position_s *get_home_position() { return &_home_pos; } bool home_position_valid() { return (_home_pos.timestamp > 0); } - struct position_setpoint_triplet_s* get_position_setpoint_triplet() { return &_pos_sp_triplet; } - struct position_setpoint_triplet_s* get_reposition_triplet() { return &_reposition_triplet; } - struct position_setpoint_triplet_s* get_takeoff_triplet() { return &_takeoff_triplet; } - struct mission_result_s* get_mission_result() { return &_mission_result; } - struct geofence_result_s* get_geofence_result() { return &_geofence_result; } - struct vehicle_attitude_setpoint_s* get_att_sp() { return &_att_sp; } + struct position_setpoint_triplet_s *get_position_setpoint_triplet() { return &_pos_sp_triplet; } + struct position_setpoint_triplet_s *get_reposition_triplet() { return &_reposition_triplet; } + struct position_setpoint_triplet_s *get_takeoff_triplet() { return &_takeoff_triplet; } + struct mission_result_s *get_mission_result() { return &_mission_result; } + struct geofence_result_s *get_geofence_result() { return &_geofence_result; } + struct vehicle_attitude_setpoint_s *get_att_sp() { return &_att_sp; } int get_onboard_mission_sub() { return _onboard_mission_sub; } int get_offboard_mission_sub() { return _offboard_mission_sub; } - Geofence& get_geofence() { return _geofence; } + Geofence &get_geofence() { return _geofence; } bool get_can_loiter_at_sp() { return _can_loiter_at_sp; } float get_loiter_radius() { return _param_loiter_radius.get(); } @@ -189,7 +189,7 @@ public: * For VTOL: sets cuising speed for current mode only (multirotor or fixed-wing). * */ - void set_cruising_speed(float speed=-1.0f); + void set_cruising_speed(float speed = -1.0f); /** * Reset cruising speed to default values @@ -208,7 +208,7 @@ public: /** * Set the target throttle */ - void set_cruising_throttle(float throttle=-1.0f) { _mission_throttle = throttle; } + void set_cruising_throttle(float throttle = -1.0f) { _mission_throttle = throttle; } /** * Get the acceptance radius given the mission item preset radius @@ -228,7 +228,7 @@ public: bool abort_landing(); - static float get_time_inside(struct mission_item_s& item) { return (item.nav_cmd == NAV_CMD_TAKEOFF) ? 0.0f : item.time_inside; } + static float get_time_inside(struct mission_item_s &item) { return (item.nav_cmd == NAV_CMD_TAKEOFF) ? 0.0f : item.time_inside; } private: @@ -310,7 +310,7 @@ private: control::BlockParamFloat _param_acceptance_radius; /**< acceptance for takeoff */ control::BlockParamFloat _param_fw_alt_acceptance_radius; /**< acceptance radius for fixedwing altitude */ control::BlockParamFloat _param_mc_alt_acceptance_radius; /**< acceptance radius for multicopter altitude */ - + control::BlockParamFloat _param_cruising_speed_hover; control::BlockParamFloat _param_cruising_speed_plane; control::BlockParamFloat _param_cruising_throttle_plane; @@ -337,7 +337,7 @@ private: /** * Retrieve home position */ - void home_position_update(bool force=false); + void home_position_update(bool force = false); /** * Retrieve fixed wing navigation capabilities @@ -393,7 +393,7 @@ private: /* this class has ptr data members, so it should not be copied, * consequently the copy constructors are private. */ - Navigator(const Navigator&); - Navigator operator=(const Navigator&); + Navigator(const Navigator &); + Navigator operator=(const Navigator &); }; #endif diff --git a/src/modules/navigator/navigator_main.cpp b/src/modules/navigator/navigator_main.cpp index 7e01d4c7df..6096c70983 100644 --- a/src/modules/navigator/navigator_main.cpp +++ b/src/modules/navigator/navigator_main.cpp @@ -344,6 +344,7 @@ Navigator::task_main() global_pos_available_once = false; PX4_WARN("global position timeout"); } + /* Let the loop run anyway, don't do `continue` here. */ } else if (pret < 0) { @@ -351,14 +352,17 @@ Navigator::task_main() PX4_ERR("nav: poll error %d, %d", pret, errno); usleep(10000); continue; + } else { if (fds[0].revents & POLLIN) { /* success, global pos is available */ global_position_update(); + if (_geofence.getSource() == Geofence::GF_SOURCE_GLOBALPOS) { have_geofence_position_data = true; } + global_pos_available_once = true; } } @@ -369,8 +373,10 @@ Navigator::task_main() /* gps updated */ orb_check(_gps_pos_sub, &updated); + if (updated) { gps_position_update(); + if (_geofence.getSource() == Geofence::GF_SOURCE_GPS) { have_geofence_position_data = true; } @@ -378,12 +384,14 @@ Navigator::task_main() /* sensors combined updated */ orb_check(_sensor_combined_sub, &updated); + if (updated) { sensor_combined_update(); } /* parameters updated */ orb_check(_param_update_sub, &updated); + if (updated) { params_update(); updateParams(); @@ -391,35 +399,41 @@ Navigator::task_main() /* vehicle control mode updated */ orb_check(_control_mode_sub, &updated); + if (updated) { vehicle_control_mode_update(); } /* vehicle status updated */ orb_check(_vstatus_sub, &updated); + if (updated) { vehicle_status_update(); } /* vehicle land detected updated */ orb_check(_land_detected_sub, &updated); + if (updated) { vehicle_land_detected_update(); } /* navigation capabilities updated */ orb_check(_fw_pos_ctrl_status_sub, &updated); + if (updated) { fw_pos_ctrl_status_update(); } /* home position updated */ orb_check(_home_pos_sub, &updated); + if (updated) { home_position_update(); } orb_check(_vehicle_command_sub, &updated); + if (updated) { vehicle_command_s cmd; orb_copy(ORB_ID(vehicle_command), _vehicle_command_sub, &cmd); @@ -441,6 +455,7 @@ Navigator::task_main() // Go on and check which changes had been requested if (PX4_ISFINITE(cmd.param4)) { rep->current.yaw = cmd.param4; + } else { rep->current.yaw = NAN; } @@ -456,6 +471,7 @@ Navigator::task_main() if (PX4_ISFINITE(cmd.param7)) { rep->current.alt = cmd.param7; + } else { rep->current.alt = get_global_position()->alt; } @@ -463,6 +479,7 @@ Navigator::task_main() rep->previous.valid = true; rep->current.valid = true; rep->next.valid = false; + } else if (cmd.command == vehicle_command_s::VEHICLE_CMD_NAV_TAKEOFF) { struct position_setpoint_triplet_s *rep = get_takeoff_triplet(); @@ -480,6 +497,7 @@ Navigator::task_main() if (PX4_ISFINITE(cmd.param5) && PX4_ISFINITE(cmd.param6)) { rep->current.lat = (cmd.param5 < 1000) ? cmd.param5 : cmd.param5 / (double)1e7; rep->current.lon = (cmd.param6 < 1000) ? cmd.param6 : cmd.param6 / (double)1e7; + } else { // If one of them is non-finite, reset both rep->current.lat = NAN; @@ -498,6 +516,7 @@ Navigator::task_main() * use MAV_CMD_MISSION_START to start the mission there */ unsigned land_start = _mission.find_offboard_land_start(); + if (land_start != -1) { vehicle_command_s vcmd = {}; vcmd.target_system = get_vstatus()->system_id; @@ -513,7 +532,7 @@ Navigator::task_main() } else if (cmd.command == vehicle_command_s::VEHICLE_CMD_MISSION_START) { if (get_mission_result()->valid && - PX4_ISFINITE(cmd.param1) && (cmd.param1 >= 0) && (cmd.param1 < _mission_result.seq_total)) { + PX4_ISFINITE(cmd.param1) && (cmd.param1 >= 0) && (cmd.param1 < _mission_result.seq_total)) { _mission.set_current_offboard_mission_index(cmd.param1); } @@ -542,15 +561,18 @@ Navigator::task_main() /* Check geofence violation */ static hrt_abstime last_geofence_check = 0; - if (have_geofence_position_data && - (_geofence.getGeofenceAction() != geofence_result_s::GF_ACTION_NONE) && - (hrt_elapsed_time(&last_geofence_check) > GEOFENCE_CHECK_INTERVAL)) { - bool inside = _geofence.inside(_global_pos, _gps_pos, _sensor_combined.baro_alt_meter, _home_pos, home_position_valid()); + if (have_geofence_position_data && + (_geofence.getGeofenceAction() != geofence_result_s::GF_ACTION_NONE) && + (hrt_elapsed_time(&last_geofence_check) > GEOFENCE_CHECK_INTERVAL)) { + + bool inside = _geofence.inside(_global_pos, _gps_pos, _sensor_combined.baro_alt_meter, _home_pos, + home_position_valid()); last_geofence_check = hrt_absolute_time(); have_geofence_position_data = false; _geofence_result.geofence_action = _geofence.getGeofenceAction(); + if (!inside) { /* inform other apps via the mission result */ _geofence_result.geofence_violated = true; @@ -561,6 +583,7 @@ Navigator::task_main() mavlink_log_critical(&_mavlink_log_pub, "Geofence violation"); _geofence_violation_warning_sent = true; } + } else { /* inform other apps via the mission result */ _geofence_result.geofence_violated = false; @@ -572,62 +595,73 @@ Navigator::task_main() /* Do stuff according to navigation state set by commander */ switch (_vstatus.nav_state) { - case vehicle_status_s::NAVIGATION_STATE_AUTO_MISSION: - _pos_sp_triplet_published_invalid_once = false; - _navigation_mode = &_mission; - break; - case vehicle_status_s::NAVIGATION_STATE_AUTO_LOITER: - _pos_sp_triplet_published_invalid_once = false; - _navigation_mode = &_loiter; - break; - case vehicle_status_s::NAVIGATION_STATE_AUTO_RCRECOVER: - _pos_sp_triplet_published_invalid_once = false; - _navigation_mode = &_rcLoss; - break; - case vehicle_status_s::NAVIGATION_STATE_AUTO_RTL: - _pos_sp_triplet_published_invalid_once = false; - _navigation_mode = &_rtl; - break; - case vehicle_status_s::NAVIGATION_STATE_AUTO_TAKEOFF: - _pos_sp_triplet_published_invalid_once = false; - _navigation_mode = &_takeoff; - break; - case vehicle_status_s::NAVIGATION_STATE_AUTO_LAND: - _pos_sp_triplet_published_invalid_once = false; - _navigation_mode = &_land; - break; - case vehicle_status_s::NAVIGATION_STATE_DESCEND: - _pos_sp_triplet_published_invalid_once = false; - _navigation_mode = &_land; - break; - case vehicle_status_s::NAVIGATION_STATE_AUTO_RTGS: - _pos_sp_triplet_published_invalid_once = false; - _navigation_mode = &_dataLinkLoss; - break; - case vehicle_status_s::NAVIGATION_STATE_AUTO_LANDENGFAIL: - _pos_sp_triplet_published_invalid_once = false; - _navigation_mode = &_engineFailure; - break; - case vehicle_status_s::NAVIGATION_STATE_AUTO_LANDGPSFAIL: - _pos_sp_triplet_published_invalid_once = false; - _navigation_mode = &_gpsFailure; - break; - case vehicle_status_s::NAVIGATION_STATE_AUTO_FOLLOW_TARGET: - _pos_sp_triplet_published_invalid_once = false; - _navigation_mode = &_follow_target; - break; - case vehicle_status_s::NAVIGATION_STATE_MANUAL: - case vehicle_status_s::NAVIGATION_STATE_ACRO: - case vehicle_status_s::NAVIGATION_STATE_ALTCTL: - case vehicle_status_s::NAVIGATION_STATE_POSCTL: - case vehicle_status_s::NAVIGATION_STATE_TERMINATION: - case vehicle_status_s::NAVIGATION_STATE_OFFBOARD: - case vehicle_status_s::NAVIGATION_STATE_STAB: - default: - _pos_sp_triplet_published_invalid_once = false; - _navigation_mode = nullptr; - _can_loiter_at_sp = false; - break; + case vehicle_status_s::NAVIGATION_STATE_AUTO_MISSION: + _pos_sp_triplet_published_invalid_once = false; + _navigation_mode = &_mission; + break; + + case vehicle_status_s::NAVIGATION_STATE_AUTO_LOITER: + _pos_sp_triplet_published_invalid_once = false; + _navigation_mode = &_loiter; + break; + + case vehicle_status_s::NAVIGATION_STATE_AUTO_RCRECOVER: + _pos_sp_triplet_published_invalid_once = false; + _navigation_mode = &_rcLoss; + break; + + case vehicle_status_s::NAVIGATION_STATE_AUTO_RTL: + _pos_sp_triplet_published_invalid_once = false; + _navigation_mode = &_rtl; + break; + + case vehicle_status_s::NAVIGATION_STATE_AUTO_TAKEOFF: + _pos_sp_triplet_published_invalid_once = false; + _navigation_mode = &_takeoff; + break; + + case vehicle_status_s::NAVIGATION_STATE_AUTO_LAND: + _pos_sp_triplet_published_invalid_once = false; + _navigation_mode = &_land; + break; + + case vehicle_status_s::NAVIGATION_STATE_DESCEND: + _pos_sp_triplet_published_invalid_once = false; + _navigation_mode = &_land; + break; + + case vehicle_status_s::NAVIGATION_STATE_AUTO_RTGS: + _pos_sp_triplet_published_invalid_once = false; + _navigation_mode = &_dataLinkLoss; + break; + + case vehicle_status_s::NAVIGATION_STATE_AUTO_LANDENGFAIL: + _pos_sp_triplet_published_invalid_once = false; + _navigation_mode = &_engineFailure; + break; + + case vehicle_status_s::NAVIGATION_STATE_AUTO_LANDGPSFAIL: + _pos_sp_triplet_published_invalid_once = false; + _navigation_mode = &_gpsFailure; + break; + + case vehicle_status_s::NAVIGATION_STATE_AUTO_FOLLOW_TARGET: + _pos_sp_triplet_published_invalid_once = false; + _navigation_mode = &_follow_target; + break; + + case vehicle_status_s::NAVIGATION_STATE_MANUAL: + case vehicle_status_s::NAVIGATION_STATE_ACRO: + case vehicle_status_s::NAVIGATION_STATE_ALTCTL: + case vehicle_status_s::NAVIGATION_STATE_POSCTL: + case vehicle_status_s::NAVIGATION_STATE_TERMINATION: + case vehicle_status_s::NAVIGATION_STATE_OFFBOARD: + case vehicle_status_s::NAVIGATION_STATE_STAB: + default: + _pos_sp_triplet_published_invalid_once = false; + _navigation_mode = nullptr; + _can_loiter_at_sp = false; + break; } /* iterate through navigation modes and set active/inactive for each */ @@ -657,6 +691,7 @@ Navigator::task_main() perf_end(_loop_perf); } + PX4_INFO("exiting"); _navigator_task = -1; @@ -670,11 +705,11 @@ Navigator::start() /* start the task */ _navigator_task = px4_task_spawn_cmd("navigator", - SCHED_DEFAULT, - SCHED_PRIORITY_DEFAULT + 5, - 1600, - (px4_main_t)&Navigator::task_main_trampoline, - nullptr); + SCHED_DEFAULT, + SCHED_PRIORITY_DEFAULT + 5, + 1600, + (px4_main_t)&Navigator::task_main_trampoline, + nullptr); if (_navigator_task < 0) { warn("task start failed"); @@ -748,6 +783,7 @@ Navigator::get_altitude_acceptance_radius() { if (!this->get_vstatus()->is_rotary_wing) { return _param_fw_alt_acceptance_radius.get(); + } else { return _param_mc_alt_acceptance_radius.get(); } @@ -778,7 +814,8 @@ Navigator::get_cruising_speed() } void -Navigator::set_cruising_speed(float speed) { +Navigator::set_cruising_speed(float speed) +{ if (_vstatus.is_rotary_wing) { _mission_cruising_speed_mc = speed; @@ -800,6 +837,7 @@ Navigator::get_cruising_throttle() /* Return the mission-requested cruise speed, or default FW_THR_CRUISE value */ if (_mission_throttle > FLT_EPSILON) { return _mission_throttle; + } else { return _param_cruising_throttle_plane.get(); } @@ -844,7 +882,7 @@ Navigator::abort_landing() if (hrt_elapsed_time(&_fw_pos_ctrl_status.timestamp) < 1000000) { if (get_position_setpoint_triplet()->current.valid - && get_position_setpoint_triplet()->current.type == position_setpoint_s::SETPOINT_TYPE_LAND) { + && get_position_setpoint_triplet()->current.type == position_setpoint_s::SETPOINT_TYPE_LAND) { should_abort = _fw_pos_ctrl_status.abort_landing; } @@ -898,12 +936,16 @@ int navigator_main(int argc, char *argv[]) if (!strcmp(argv[1], "stop")) { delete navigator::g_navigator; navigator::g_navigator = nullptr; + } else if (!strcmp(argv[1], "status")) { navigator::g_navigator->status(); + } else if (!strcmp(argv[1], "fence")) { navigator::g_navigator->add_fence_point(argc - 2, argv + 2); + } else if (!strcmp(argv[1], "fencefile")) { navigator::g_navigator->load_fence_from_file(GEOFENCE_FILENAME); + } else { usage(); return 1; @@ -970,7 +1012,7 @@ Navigator::publish_att_sp() } void -Navigator::set_mission_failure(const char* reason) +Navigator::set_mission_failure(const char *reason) { if (!_mission_result.mission_failure) { _mission_result.mission_failure = true; diff --git a/src/modules/navigator/takeoff.cpp b/src/modules/navigator/takeoff.cpp index f419705bdf..659b837542 100644 --- a/src/modules/navigator/takeoff.cpp +++ b/src/modules/navigator/takeoff.cpp @@ -80,6 +80,7 @@ void Takeoff::on_active() { struct position_setpoint_triplet_s *rep = _navigator->get_takeoff_triplet(); + if (rep->current.valid) { // reset the position set_takeoff_position(); @@ -115,6 +116,7 @@ Takeoff::set_takeoff_position() mavlink_log_critical(_navigator->get_mavlink_log_pub(), "Using minimum takeoff altitude: %.2f m", (double)_param_min_alt.get()); } + } else { // Use home + minimum clearance but only notify. abs_altitude = min_abs_altitude;