diff --git a/src/modules/navigator/datalinkloss.cpp b/src/modules/navigator/datalinkloss.cpp index 5511a15f45..6c910c9844 100644 --- a/src/modules/navigator/datalinkloss.cpp +++ b/src/modules/navigator/datalinkloss.cpp @@ -118,48 +118,51 @@ DataLinkLoss::set_dll_item() switch (_dll_state) { case DLL_STATE_FLYTOCOMMSHOLDWP: { - _mission_item.lat = (double)(_param_commsholdlat.get()) * 1.0e-7; - _mission_item.lon = (double)(_param_commsholdlon.get()) * 1.0e-7; - _mission_item.altitude_is_relative = false; - _mission_item.altitude = _param_commsholdalt.get(); - _mission_item.yaw = NAN; - _mission_item.loiter_radius = _navigator->get_loiter_radius(); - _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(); - _mission_item.autocontinue = true; - _mission_item.origin = ORIGIN_ONBOARD; + _mission_item.lat = (double)(_param_commsholdlat.get()) * 1.0e-7; + _mission_item.lon = (double)(_param_commsholdlon.get()) * 1.0e-7; + _mission_item.altitude_is_relative = false; + _mission_item.altitude = _param_commsholdalt.get(); + _mission_item.yaw = NAN; + _mission_item.loiter_radius = _navigator->get_loiter_radius(); + _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(); + _mission_item.autocontinue = true; + _mission_item.origin = ORIGIN_ONBOARD; + + _navigator->set_can_loiter_at_sp(true); + break; + } - _navigator->set_can_loiter_at_sp(true); - break; - } case DLL_STATE_FLYTOAIRFIELDHOMEWP: { - _mission_item.lat = (double)(_param_airfieldhomelat.get()) * 1.0e-7; - _mission_item.lon = (double)(_param_airfieldhomelon.get()) * 1.0e-7; - _mission_item.altitude_is_relative = false; - _mission_item.altitude = _param_airfieldhomealt.get(); - _mission_item.yaw = NAN; - _mission_item.loiter_radius = _navigator->get_loiter_radius(); - _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(); - _mission_item.autocontinue = true; - _mission_item.origin = ORIGIN_ONBOARD; + _mission_item.lat = (double)(_param_airfieldhomelat.get()) * 1.0e-7; + _mission_item.lon = (double)(_param_airfieldhomelon.get()) * 1.0e-7; + _mission_item.altitude_is_relative = false; + _mission_item.altitude = _param_airfieldhomealt.get(); + _mission_item.yaw = NAN; + _mission_item.loiter_radius = _navigator->get_loiter_radius(); + _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(); + _mission_item.autocontinue = true; + _mission_item.origin = ORIGIN_ONBOARD; + + _navigator->set_can_loiter_at_sp(true); + break; + } - _navigator->set_can_loiter_at_sp(true); - break; - } case DLL_STATE_TERMINATE: { - /* Request flight termination from the commander */ - _navigator->get_mission_result()->flight_termination = true; - _navigator->set_mission_result_updated(); - reset_mission_item_reached(); - warnx("not switched to manual: request flight termination"); - pos_sp_triplet->previous.valid = false; - pos_sp_triplet->current.valid = false; - pos_sp_triplet->next.valid = false; - break; - } + /* Request flight termination from the commander */ + _navigator->get_mission_result()->flight_termination = true; + _navigator->set_mission_result_updated(); + reset_mission_item_reached(); + warnx("not switched to manual: request flight termination"); + pos_sp_triplet->previous.valid = false; + pos_sp_triplet->current.valid = false; + pos_sp_triplet->next.valid = false; + break; + } + default: break; } @@ -178,21 +181,24 @@ DataLinkLoss::advance_dll() { switch (_dll_state) { case DLL_STATE_NONE: + /* Check the number of data link losses. If above home fly home directly */ /* if number of data link losses limit is not reached fly to comms hold wp */ if (_navigator->get_vstatus()->data_link_lost_counter > _param_numberdatalinklosses.get()) { warnx("%d data link losses, limit is %d, fly to airfield home", - _navigator->get_vstatus()->data_link_lost_counter, _param_numberdatalinklosses.get()); + _navigator->get_vstatus()->data_link_lost_counter, _param_numberdatalinklosses.get()); mavlink_log_critical(_navigator->get_mavlink_log_pub(), "too many DL losses, fly to airfield home"); _navigator->get_mission_result()->stay_in_failsafe = true; _navigator->set_mission_result_updated(); reset_mission_item_reached(); _dll_state = DLL_STATE_FLYTOAIRFIELDHOMEWP; + } else { if (!_param_skipcommshold.get()) { warnx("fly to comms hold, datalink loss counter: %d", _navigator->get_vstatus()->data_link_lost_counter); mavlink_log_critical(_navigator->get_mavlink_log_pub(), "fly to comms hold"); _dll_state = DLL_STATE_FLYTOCOMMSHOLDWP; + } else { /* comms hold wp not active, fly to airfield home directly */ warnx("Skipping comms hold wp. Flying directly to airfield home"); @@ -200,15 +206,18 @@ DataLinkLoss::advance_dll() _dll_state = DLL_STATE_FLYTOAIRFIELDHOMEWP; } } + break; + case DLL_STATE_FLYTOCOMMSHOLDWP: warnx("fly to airfield home"); - mavlink_log_critical(_navigator->get_mavlink_log_pub(), "fly to airfield home"); + mavlink_log_critical(_navigator->get_mavlink_log_pub(), "fly to airfield home"); _dll_state = DLL_STATE_FLYTOAIRFIELDHOMEWP; _navigator->get_mission_result()->stay_in_failsafe = true; _navigator->set_mission_result_updated(); reset_mission_item_reached(); break; + case DLL_STATE_FLYTOAIRFIELDHOMEWP: _dll_state = DLL_STATE_TERMINATE; warnx("time is up, state should have been changed manually by now"); @@ -217,6 +226,7 @@ DataLinkLoss::advance_dll() _navigator->set_mission_result_updated(); reset_mission_item_reached(); break; + case DLL_STATE_TERMINATE: warnx("dll end"); _dll_state = DLL_STATE_END; diff --git a/src/modules/navigator/enginefailure.cpp b/src/modules/navigator/enginefailure.cpp index 6f74149728..cf48edb405 100644 --- a/src/modules/navigator/enginefailure.cpp +++ b/src/modules/navigator/enginefailure.cpp @@ -31,11 +31,11 @@ * ****************************************************************************/ - /* @file enginefailure.cpp - * Helper class for a fixedwing engine failure mode - * - * @author Thomas Gubler - */ +/* @file enginefailure.cpp +* Helper class for a fixedwing engine failure mode +* +* @author Thomas Gubler +*/ #include #include #include @@ -104,23 +104,24 @@ EngineFailure::set_ef_item() switch (_ef_state) { case EF_STATE_LOITERDOWN: { - //XXX create mission item at ground (below?) here + //XXX create mission item at ground (below?) here - _mission_item.lat = _navigator->get_global_position()->lat; - _mission_item.lon = _navigator->get_global_position()->lon; - _mission_item.altitude_is_relative = false; - //XXX setting altitude to a very low value, evaluate other options - _mission_item.altitude = _navigator->get_home_position()->alt - 1000.0f; - _mission_item.yaw = NAN; - _mission_item.loiter_radius = _navigator->get_loiter_radius(); - _mission_item.nav_cmd = NAV_CMD_LOITER_UNLIMITED; - _mission_item.acceptance_radius = _navigator->get_acceptance_radius(); - _mission_item.autocontinue = true; - _mission_item.origin = ORIGIN_ONBOARD; + _mission_item.lat = _navigator->get_global_position()->lat; + _mission_item.lon = _navigator->get_global_position()->lon; + _mission_item.altitude_is_relative = false; + //XXX setting altitude to a very low value, evaluate other options + _mission_item.altitude = _navigator->get_home_position()->alt - 1000.0f; + _mission_item.yaw = NAN; + _mission_item.loiter_radius = _navigator->get_loiter_radius(); + _mission_item.nav_cmd = NAV_CMD_LOITER_UNLIMITED; + _mission_item.acceptance_radius = _navigator->get_acceptance_radius(); + _mission_item.autocontinue = true; + _mission_item.origin = ORIGIN_ONBOARD; + + _navigator->set_can_loiter_at_sp(true); + break; + } - _navigator->set_can_loiter_at_sp(true); - break; - } default: break; } @@ -142,6 +143,7 @@ EngineFailure::advance_ef() mavlink_log_emergency(_navigator->get_mavlink_log_pub(), "Engine failure. Loitering down"); _ef_state = EF_STATE_LOITERDOWN; break; + default: break; } diff --git a/src/modules/navigator/follow_target.cpp b/src/modules/navigator/follow_target.cpp index 0c80f9bc99..4c6a018866 100644 --- a/src/modules/navigator/follow_target.cpp +++ b/src/modules/navigator/follow_target.cpp @@ -60,10 +60,10 @@ FollowTarget::FollowTarget(Navigator *navigator, const char *name) : MissionBlock(navigator, name), _navigator(navigator), _param_min_alt(this, "NAV_MIN_FT_HT", false), - _param_tracking_dist(this,"NAV_FT_DST", false), - _param_tracking_side(this,"NAV_FT_FS", false), - _param_tracking_resp(this,"NAV_FT_RS", false), - _param_yaw_auto_max(this,"MC_YAWRAUTO_MAX", false), + _param_tracking_dist(this, "NAV_FT_DST", false), + _param_tracking_side(this, "NAV_FT_FS", false), + _param_tracking_resp(this, "NAV_FT_RS", false), + _param_yaw_auto_max(this, "MC_YAWRAUTO_MAX", false), _follow_target_state(SET_WAIT_FOR_TARGET_POSITION), _follow_target_position(FOLLOW_FROM_BEHIND), _follow_target_sub(-1), @@ -71,14 +71,16 @@ FollowTarget::FollowTarget(Navigator *navigator, const char *name) : _follow_offset(OFFSET_M), _target_updates(0), _last_update_time(0), - _current_target_motion({}), - _previous_target_motion({}), + _current_target_motion(), + _previous_target_motion(), _yaw_rate(0.0F), _responsiveness(0.0F), _yaw_auto_max(0.0F), _yaw_angle(0.0F) { updateParams(); + _current_target_motion = {}; + _previous_target_motion = {}; _current_vel.zero(); _step_vel.zero(); _est_target_vel.zero(); @@ -108,7 +110,7 @@ void FollowTarget::on_activation() _follow_target_position = _param_tracking_side.get(); - if((_follow_target_position > FOLLOW_FROM_LEFT) || (_follow_target_position < FOLLOW_FROM_RIGHT)) { + if ((_follow_target_position > FOLLOW_FROM_LEFT) || (_follow_target_position < FOLLOW_FROM_RIGHT)) { _follow_target_position = FOLLOW_FROM_BEHIND; } @@ -143,18 +145,20 @@ void FollowTarget::on_active() orb_copy(ORB_ID(follow_target), _follow_target_sub, &target_motion); - if(_current_target_motion.timestamp == 0) { + if (_current_target_motion.timestamp == 0) { _current_target_motion = target_motion; } _current_target_motion.timestamp = target_motion.timestamp; - _current_target_motion.lat = (_current_target_motion.lat*(double)_responsiveness) + target_motion.lat*(double)(1 - _responsiveness); - _current_target_motion.lon = (_current_target_motion.lon*(double)_responsiveness) + target_motion.lon*(double)(1 - _responsiveness); + _current_target_motion.lat = (_current_target_motion.lat * (double)_responsiveness) + target_motion.lat * (double)( + 1 - _responsiveness); + _current_target_motion.lon = (_current_target_motion.lon * (double)_responsiveness) + target_motion.lon * (double)( + 1 - _responsiveness); target_reported_velocity(0) = _current_target_motion.vx; target_reported_velocity(1) = _current_target_motion.vy; - } else if (((current_time - _current_target_motion.timestamp) / 1000 ) > TARGET_TIMEOUT_MS && target_velocity_valid()) { + } else if (((current_time - _current_target_motion.timestamp) / 1000) > TARGET_TIMEOUT_MS && target_velocity_valid()) { reset_target_validity(); } @@ -165,7 +169,8 @@ void FollowTarget::on_active() // get distance to target map_projection_init(&target_ref, _navigator->get_global_position()->lat, _navigator->get_global_position()->lon); - map_projection_project(&target_ref, _current_target_motion.lat, _current_target_motion.lon, &_target_distance(0), &_target_distance(1)); + map_projection_project(&target_ref, _current_target_motion.lat, _current_target_motion.lon, &_target_distance(0), + &_target_distance(1)); } @@ -187,7 +192,8 @@ void FollowTarget::on_active() // calculate distance the target has moved - map_projection_project(&target_ref, _current_target_motion.lat, _current_target_motion.lon, &(_target_position_delta(0)), &(_target_position_delta(1))); + map_projection_project(&target_ref, _current_target_motion.lat, _current_target_motion.lon, + &(_target_position_delta(0)), &(_target_position_delta(1))); // update the average velocity of the target based on the position @@ -195,15 +201,15 @@ void FollowTarget::on_active() // if the target is moving add an offset and rotation - if(_est_target_vel.length() > .5F) { - _target_position_offset = _rot_matrix*_est_target_vel.normalized()*_follow_offset; + if (_est_target_vel.length() > .5F) { + _target_position_offset = _rot_matrix * _est_target_vel.normalized() * _follow_offset; } // are we within the target acceptance radius? // give a buffer to exit/enter the radius to give the velocity controller // a chance to catch up - _radius_exited = ((_target_position_offset + _target_distance).length() > (float) TARGET_ACCEPTANCE_RADIUS_M * 1.5f); + _radius_exited = ((_target_position_offset + _target_distance).length() > (float) TARGET_ACCEPTANCE_RADIUS_M * 1.5f); _radius_entered = ((_target_position_offset + _target_distance).length() < (float) TARGET_ACCEPTANCE_RADIUS_M); // to keep the velocity increase/decrease smooth @@ -221,7 +227,7 @@ void FollowTarget::on_active() // if we are less than 1 meter from the target don't worry about trying to yaw // lock the yaw until we are at a distance that makes sense - if((_target_distance).length() > 1.0F) { + if ((_target_distance).length() > 1.0F) { // yaw rate smoothing @@ -229,15 +235,15 @@ void FollowTarget::on_active() // but seems to work ok for now since the yaw rate cannot be controlled directly in auto mode _yaw_angle = get_bearing_to_next_waypoint(_navigator->get_global_position()->lat, - _navigator->get_global_position()->lon, - _current_target_motion.lat, - _current_target_motion.lon); + _navigator->get_global_position()->lon, + _current_target_motion.lat, + _current_target_motion.lon); _yaw_rate = (_yaw_angle - _navigator->get_global_position()->yaw) / (dt_ms / 1000.0F); _yaw_rate = _wrap_pi(_yaw_rate); - _yaw_rate = math::constrain(_yaw_rate, -1.0F*_yaw_auto_max, _yaw_auto_max); + _yaw_rate = math::constrain(_yaw_rate, -1.0F * _yaw_auto_max, _yaw_auto_max); } else { _yaw_angle = _yaw_rate = NAN; @@ -257,12 +263,13 @@ void FollowTarget::on_active() // (double)_avg_cos_ratio, (double) _yaw_rate); } - if(target_position_valid()) { + if (target_position_valid()) { // get the target position using the calculated offset map_projection_init(&target_ref, _current_target_motion.lat, _current_target_motion.lon); - map_projection_reproject(&target_ref, _target_position_offset(0), _target_position_offset(1), &target_motion_with_offset.lat, &target_motion_with_offset.lon); + map_projection_reproject(&target_ref, _target_position_offset(0), _target_position_offset(1), + &target_motion_with_offset.lat, &target_motion_with_offset.lon); } // clamp yaw rate smoothing if we are with in @@ -282,12 +289,14 @@ void FollowTarget::on_active() if (_radius_entered == true) { _follow_target_state = TRACK_VELOCITY; + } else if (target_velocity_valid()) { set_follow_target_item(&_mission_item, _param_min_alt.get(), target_motion_with_offset, _yaw_angle); // keep the current velocity updated with the target velocity for when it's needed _current_vel = _est_target_vel; update_position_sp(true, true, _yaw_rate); + } else { _follow_target_state = SET_WAIT_FOR_TARGET_POSITION; } @@ -299,6 +308,7 @@ void FollowTarget::on_active() if (_radius_exited == true) { _follow_target_state = TRACK_POSITION; + } else if (target_velocity_valid()) { if ((current_time - _last_update_time) / 1000 >= _step_time_in_ms) { @@ -309,37 +319,40 @@ void FollowTarget::on_active() set_follow_target_item(&_mission_item, _param_min_alt.get(), target_motion_with_offset, _yaw_angle); update_position_sp(true, false, _yaw_rate); + } else { _follow_target_state = SET_WAIT_FOR_TARGET_POSITION; } break; } + case SET_WAIT_FOR_TARGET_POSITION: { - // Climb to the minimum altitude - // and wait until a position is received + // Climb to the minimum altitude + // and wait until a position is received - follow_target_s target = {}; + follow_target_s target = {}; - // for now set the target at the minimum height above the uav + // for now set the target at the minimum height above the uav - target.lat = _navigator->get_global_position()->lat; - target.lon = _navigator->get_global_position()->lon; - target.alt = 0.0F; + target.lat = _navigator->get_global_position()->lat; + target.lon = _navigator->get_global_position()->lon; + target.alt = 0.0F; - set_follow_target_item(&_mission_item, _param_min_alt.get(), target, _yaw_angle); + set_follow_target_item(&_mission_item, _param_min_alt.get(), target, _yaw_angle); - update_position_sp(false, false, _yaw_rate); + update_position_sp(false, false, _yaw_rate); + + _follow_target_state = WAIT_FOR_TARGET_POSITION; + } - _follow_target_state = WAIT_FOR_TARGET_POSITION; - } case WAIT_FOR_TARGET_POSITION: { - if (is_mission_item_reached() && target_velocity_valid()) { - _target_position_offset(0) = _follow_offset; - _follow_target_state = TRACK_POSITION; - } + if (is_mission_item_reached() && target_velocity_valid()) { + _target_position_offset(0) = _follow_offset; + _follow_target_state = TRACK_POSITION; + } break; } diff --git a/src/modules/navigator/follow_target.h b/src/modules/navigator/follow_target.h index cc6891a8ac..d38feafe2b 100644 --- a/src/modules/navigator/follow_target.h +++ b/src/modules/navigator/follow_target.h @@ -85,21 +85,30 @@ private: }; float _follow_position_matricies[4][9] = { - {1.0F, -1.0F, 0.0F, - 1.0F, 1.0F, 0.0F, - 0.0F, 0.0F, 1.0F}, // follow right + { + 1.0F, -1.0F, 0.0F, + 1.0F, 1.0F, 0.0F, + 0.0F, 0.0F, 1.0F + }, // follow right - {-1.0F, 0.0F, 0.0F, - 0.0F, -1.0F, 0.0F, - 0.0F, 0.0F, 1.0F}, // follow behind + { + -1.0F, 0.0F, 0.0F, + 0.0F, -1.0F, 0.0F, + 0.0F, 0.0F, 1.0F + }, // follow behind - {1.0F, 0.0F, 0.0F, - 0.0F, 1.0F, 0.0F, - 0.0F, 0.0F, 1.0F}, // follow front + { + 1.0F, 0.0F, 0.0F, + 0.0F, 1.0F, 0.0F, + 0.0F, 0.0F, 1.0F + }, // follow front - {1.0F, 1.0F, 0.0F, - -1.0F, 1.0F, 0.0F, - 0.0F, 0.0F, 1.0F}}; // follow left side + { + 1.0F, 1.0F, 0.0F, + -1.0F, 1.0F, 0.0F, + 0.0F, 0.0F, 1.0F + } + }; // follow left side Navigator *_navigator; @@ -137,14 +146,14 @@ private: // Mavlink defined motion reporting capabilities - enum { - POS = 0, - VEL = 1, - ACCEL = 2, - ATT_RATES = 3 - }; + enum { + POS = 0, + VEL = 1, + ACCEL = 2, + ATT_RATES = 3 + }; - math::Matrix<3,3> _rot_matrix; + math::Matrix<3, 3> _rot_matrix; void track_target_position(); void track_target_velocity(); bool target_velocity_valid(); diff --git a/src/modules/navigator/gpsfailure.cpp b/src/modules/navigator/gpsfailure.cpp index 0973d4c932..7ece880d1f 100644 --- a/src/modules/navigator/gpsfailure.cpp +++ b/src/modules/navigator/gpsfailure.cpp @@ -100,29 +100,32 @@ GpsFailure::on_active() switch (_gpsf_state) { case GPSF_STATE_LOITER: { - /* Position controller does not run in this mode: - * navigator has to publish an attitude setpoint */ - _navigator->get_att_sp()->roll_body = M_DEG_TO_RAD_F * _param_openlooploiter_roll.get(); - _navigator->get_att_sp()->pitch_body = M_DEG_TO_RAD_F * _param_openlooploiter_pitch.get(); - _navigator->get_att_sp()->thrust = _param_openlooploiter_thrust.get(); - _navigator->publish_att_sp(); + /* Position controller does not run in this mode: + * navigator has to publish an attitude setpoint */ + _navigator->get_att_sp()->roll_body = M_DEG_TO_RAD_F * _param_openlooploiter_roll.get(); + _navigator->get_att_sp()->pitch_body = M_DEG_TO_RAD_F * _param_openlooploiter_pitch.get(); + _navigator->get_att_sp()->thrust = _param_openlooploiter_thrust.get(); + _navigator->publish_att_sp(); - /* Measure time */ - hrt_abstime elapsed = hrt_elapsed_time(&_timestamp_activation); + /* Measure time */ + hrt_abstime elapsed = hrt_elapsed_time(&_timestamp_activation); - //warnx("open loop loiter, posctl enabled %u, elapsed %.1fs, thrust %.2f", - //_navigator->get_control_mode()->flag_control_position_enabled, elapsed * 1e-6, (double)_param_openlooploiter_thrust.get()); - if (elapsed > _param_loitertime.get() * 1e6f) { - /* no recovery, adavance the state machine */ - warnx("gps not recovered, switch to next state"); - advance_gpsf(); + //warnx("open loop loiter, posctl enabled %u, elapsed %.1fs, thrust %.2f", + //_navigator->get_control_mode()->flag_control_position_enabled, elapsed * 1e-6, (double)_param_openlooploiter_thrust.get()); + if (elapsed > _param_loitertime.get() * 1e6f) { + /* no recovery, adavance the state machine */ + warnx("gps not recovered, switch to next state"); + advance_gpsf(); + } + + break; } - break; - } + case GPSF_STATE_TERMINATE: set_gpsf_item(); advance_gpsf(); break; + default: break; } @@ -140,11 +143,12 @@ GpsFailure::set_gpsf_item() switch (_gpsf_state) { case GPSF_STATE_TERMINATE: { - /* Request flight termination from the commander */ - _navigator->get_mission_result()->flight_termination = true; - _navigator->set_mission_result_updated(); - warnx("gps fail: request flight termination"); - } + /* Request flight termination from the commander */ + _navigator->get_mission_result()->flight_termination = true; + _navigator->set_mission_result_updated(); + warnx("gps fail: request flight termination"); + } + default: break; } @@ -164,15 +168,18 @@ GpsFailure::advance_gpsf() warnx("gpsf loiter"); mavlink_log_critical(_navigator->get_mavlink_log_pub(), "GPS failed: open loop loiter"); break; + case GPSF_STATE_LOITER: _gpsf_state = GPSF_STATE_TERMINATE; warnx("gpsf terminate"); mavlink_log_emergency(_navigator->get_mavlink_log_pub(), "no gps recovery, termination"); warnx("mavlink sent"); break; + case GPSF_STATE_TERMINATE: warnx("gpsf end"); _gpsf_state = GPSF_STATE_END; + default: break; } diff --git a/src/modules/navigator/land.cpp b/src/modules/navigator/land.cpp index 0fc4ee7626..144cf27a0a 100644 --- a/src/modules/navigator/land.cpp +++ b/src/modules/navigator/land.cpp @@ -54,10 +54,10 @@ #include "navigator.h" Land::Land(Navigator *navigator, const char *name) : - MissionBlock(navigator, name) + MissionBlock(navigator, name) { - /* load initial params */ - updateParams(); + /* load initial params */ + updateParams(); } Land::~Land() @@ -72,34 +72,34 @@ Land::on_inactive() void Land::on_activation() { - /* set current mission item to Land */ - set_land_item(&_mission_item, true); - _navigator->get_mission_result()->reached = false; - _navigator->get_mission_result()->finished = false; - _navigator->set_mission_result_updated(); - reset_mission_item_reached(); + /* set current mission item to Land */ + set_land_item(&_mission_item, true); + _navigator->get_mission_result()->reached = false; + _navigator->get_mission_result()->finished = false; + _navigator->set_mission_result_updated(); + reset_mission_item_reached(); - /* convert mission item to current setpoint */ - struct position_setpoint_triplet_s *pos_sp_triplet = _navigator->get_position_setpoint_triplet(); - pos_sp_triplet->previous.valid = false; - mission_item_to_position_setpoint(&_mission_item, &pos_sp_triplet->current); - pos_sp_triplet->next.valid = false; + /* convert mission item to current setpoint */ + struct position_setpoint_triplet_s *pos_sp_triplet = _navigator->get_position_setpoint_triplet(); + pos_sp_triplet->previous.valid = false; + mission_item_to_position_setpoint(&_mission_item, &pos_sp_triplet->current); + pos_sp_triplet->next.valid = false; - _navigator->set_can_loiter_at_sp(false); + _navigator->set_can_loiter_at_sp(false); - _navigator->set_position_setpoint_triplet_updated(); + _navigator->set_position_setpoint_triplet_updated(); } void Land::on_active() { - if (is_mission_item_reached() && !_navigator->get_mission_result()->finished) { - _navigator->get_mission_result()->finished = true; - _navigator->set_mission_result_updated(); - set_idle_item(&_mission_item); + if (is_mission_item_reached() && !_navigator->get_mission_result()->finished) { + _navigator->get_mission_result()->finished = true; + _navigator->set_mission_result_updated(); + set_idle_item(&_mission_item); - struct position_setpoint_triplet_s *pos_sp_triplet = _navigator->get_position_setpoint_triplet(); - mission_item_to_position_setpoint(&_mission_item, &pos_sp_triplet->current); - _navigator->set_position_setpoint_triplet_updated(); - } + struct position_setpoint_triplet_s *pos_sp_triplet = _navigator->get_position_setpoint_triplet(); + mission_item_to_position_setpoint(&_mission_item, &pos_sp_triplet->current); + _navigator->set_position_setpoint_triplet_updated(); + } } diff --git a/src/modules/navigator/land.h b/src/modules/navigator/land.h index c97feb7046..ef17b29987 100644 --- a/src/modules/navigator/land.h +++ b/src/modules/navigator/land.h @@ -50,15 +50,15 @@ class Land : public MissionBlock { public: - Land(Navigator *navigator, const char *name); + Land(Navigator *navigator, const char *name); - ~Land(); + ~Land(); - virtual void on_inactive(); + virtual void on_inactive(); - virtual void on_activation(); + virtual void on_activation(); - virtual void on_active(); + virtual void on_active(); }; diff --git a/src/modules/navigator/loiter.cpp b/src/modules/navigator/loiter.cpp index 6ef7fd304d..d773b9bb74 100644 --- a/src/modules/navigator/loiter.cpp +++ b/src/modules/navigator/loiter.cpp @@ -80,6 +80,7 @@ Loiter::on_activation() { if (_navigator->get_reposition_triplet()->current.valid) { reposition(); + } else { set_loiter_position(); } @@ -103,8 +104,9 @@ Loiter::set_loiter_position() { // not setting loiter position until armed if (_navigator->get_vstatus()->arming_state != vehicle_status_s::ARMING_STATE_ARMED || - _loiter_pos_set) { + _loiter_pos_set) { return; + } else { _loiter_pos_set = true; } @@ -149,16 +151,17 @@ Loiter::reposition() // set yaw - float travel_dist = get_distance_to_next_waypoint(_navigator->get_global_position()->lat, _navigator->get_global_position()->lon, - pos_sp_triplet->current.lat, pos_sp_triplet->current.lon); + float travel_dist = get_distance_to_next_waypoint(_navigator->get_global_position()->lat, + _navigator->get_global_position()->lon, + pos_sp_triplet->current.lat, pos_sp_triplet->current.lon); if (travel_dist > 1.0f) { // calculate direction the vehicle should point to. pos_sp_triplet->current.yaw = get_bearing_to_next_waypoint( - _navigator->get_global_position()->lat, - _navigator->get_global_position()->lon, - pos_sp_triplet->current.lat, - pos_sp_triplet->current.lon); + _navigator->get_global_position()->lat, + _navigator->get_global_position()->lon, + pos_sp_triplet->current.lat, + pos_sp_triplet->current.lon); } _navigator->set_can_loiter_at_sp(pos_sp_triplet->current.type == position_setpoint_s::SETPOINT_TYPE_LOITER); diff --git a/src/modules/navigator/rcloss.cpp b/src/modules/navigator/rcloss.cpp index d911653491..89861d7b9f 100644 --- a/src/modules/navigator/rcloss.cpp +++ b/src/modules/navigator/rcloss.cpp @@ -108,31 +108,33 @@ RCLoss::set_rcl_item() switch (_rcl_state) { case RCL_STATE_LOITER: { - _mission_item.lat = _navigator->get_global_position()->lat; - _mission_item.lon = _navigator->get_global_position()->lon; - _mission_item.altitude = _navigator->get_global_position()->alt; - _mission_item.altitude_is_relative = false; - _mission_item.yaw = NAN; - _mission_item.loiter_radius = _navigator->get_loiter_radius(); - _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(); - _mission_item.autocontinue = true; - _mission_item.origin = ORIGIN_ONBOARD; + _mission_item.lat = _navigator->get_global_position()->lat; + _mission_item.lon = _navigator->get_global_position()->lon; + _mission_item.altitude = _navigator->get_global_position()->alt; + _mission_item.altitude_is_relative = false; + _mission_item.yaw = NAN; + _mission_item.loiter_radius = _navigator->get_loiter_radius(); + _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(); + _mission_item.autocontinue = true; + _mission_item.origin = ORIGIN_ONBOARD; + + _navigator->set_can_loiter_at_sp(true); + break; + } - _navigator->set_can_loiter_at_sp(true); - break; - } case RCL_STATE_TERMINATE: { - /* Request flight termination from the commander */ - _navigator->get_mission_result()->flight_termination = true; - _navigator->set_mission_result_updated(); - warnx("rc not recovered: request flight termination"); - pos_sp_triplet->previous.valid = false; - pos_sp_triplet->current.valid = false; - pos_sp_triplet->next.valid = false; - break; - } + /* Request flight termination from the commander */ + _navigator->get_mission_result()->flight_termination = true; + _navigator->set_mission_result_updated(); + warnx("rc not recovered: request flight termination"); + pos_sp_triplet->previous.valid = false; + pos_sp_triplet->current.valid = false; + pos_sp_triplet->next.valid = false; + break; + } + default: break; } @@ -155,6 +157,7 @@ RCLoss::advance_rcl() warnx("RC loss, OBC mode, loiter"); mavlink_log_critical(_navigator->get_mavlink_log_pub(), "rc loss, loitering"); _rcl_state = RCL_STATE_LOITER; + } else { warnx("RC loss, OBC mode, slip loiter, terminate"); mavlink_log_critical(_navigator->get_mavlink_log_pub(), "rc loss, terminating"); @@ -163,7 +166,9 @@ RCLoss::advance_rcl() _navigator->set_mission_result_updated(); reset_mission_item_reached(); } + break; + case RCL_STATE_LOITER: _rcl_state = RCL_STATE_TERMINATE; warnx("time is up, no RC regain, terminating"); @@ -172,10 +177,12 @@ RCLoss::advance_rcl() _navigator->set_mission_result_updated(); reset_mission_item_reached(); break; + case RCL_STATE_TERMINATE: warnx("rcl end"); _rcl_state = RCL_STATE_END; break; + default: break; } diff --git a/src/modules/navigator/rtl.cpp b/src/modules/navigator/rtl.cpp index cb234aacd1..ccbd3f4016 100644 --- a/src/modules/navigator/rtl.cpp +++ b/src/modules/navigator/rtl.cpp @@ -94,8 +94,9 @@ RTL::on_activation() mission_item_to_position_setpoint(&_mission_item, &pos_sp_triplet->current); /* check if we are pretty close to home already */ - float home_dist = get_distance_to_next_waypoint(_navigator->get_home_position()->lat, _navigator->get_home_position()->lon, - _navigator->get_global_position()->lat, _navigator->get_global_position()->lon); + float home_dist = get_distance_to_next_waypoint(_navigator->get_home_position()->lat, + _navigator->get_home_position()->lon, + _navigator->get_global_position()->lat, _navigator->get_global_position()->lon); /* decide where to enter the RTL procedure when we switch into it */ if (_rtl_state == RTL_STATE_NONE) { @@ -104,12 +105,15 @@ RTL::on_activation() _rtl_state = RTL_STATE_LANDED; mavlink_log_critical(_navigator->get_mavlink_log_pub(), "Already landed, not executing RTL"); - /* if lower than return altitude, climb up first */ - } else if (home_dist > _param_rtl_min_dist.get() && _navigator->get_global_position()->alt < _navigator->get_home_position()->alt + /* if lower than return altitude, climb up first */ + + } else if (home_dist > _param_rtl_min_dist.get() + && _navigator->get_global_position()->alt < _navigator->get_home_position()->alt + _param_return_alt.get()) { _rtl_state = RTL_STATE_CLIMB; - /* otherwise go straight to return */ + /* otherwise go straight to return */ + } else { /* set altitude setpoint to current altitude */ _rtl_state = RTL_STATE_RETURN; @@ -147,153 +151,157 @@ RTL::set_rtl_item() switch (_rtl_state) { case RTL_STATE_CLIMB: { - float climb_alt = _navigator->get_home_position()->alt + _param_return_alt.get(); + float climb_alt = _navigator->get_home_position()->alt + _param_return_alt.get(); - _mission_item.lat = _navigator->get_global_position()->lat; - _mission_item.lon = _navigator->get_global_position()->lon; - _mission_item.altitude_is_relative = false; - _mission_item.altitude = climb_alt; - _mission_item.yaw = NAN; - _mission_item.loiter_radius = _navigator->get_loiter_radius(); - _mission_item.nav_cmd = NAV_CMD_WAYPOINT; - _mission_item.acceptance_radius = _navigator->get_acceptance_radius(); - _mission_item.time_inside = 0.0f; - _mission_item.autocontinue = true; - _mission_item.origin = ORIGIN_ONBOARD; + _mission_item.lat = _navigator->get_global_position()->lat; + _mission_item.lon = _navigator->get_global_position()->lon; + _mission_item.altitude_is_relative = false; + _mission_item.altitude = climb_alt; + _mission_item.yaw = NAN; + _mission_item.loiter_radius = _navigator->get_loiter_radius(); + _mission_item.nav_cmd = NAV_CMD_WAYPOINT; + _mission_item.acceptance_radius = _navigator->get_acceptance_radius(); + _mission_item.time_inside = 0.0f; + _mission_item.autocontinue = true; + _mission_item.origin = ORIGIN_ONBOARD; - mavlink_log_info(_navigator->get_mavlink_log_pub(), "RTL: climb to %d m (%d m above home)", - (int)(climb_alt), - (int)(climb_alt - _navigator->get_home_position()->alt)); - break; - } + mavlink_log_info(_navigator->get_mavlink_log_pub(), "RTL: climb to %d m (%d m above home)", + (int)(climb_alt), + (int)(climb_alt - _navigator->get_home_position()->alt)); + break; + } case RTL_STATE_RETURN: { - _mission_item.lat = _navigator->get_home_position()->lat; - _mission_item.lon = _navigator->get_home_position()->lon; - // don't change altitude + _mission_item.lat = _navigator->get_home_position()->lat; + _mission_item.lon = _navigator->get_home_position()->lon; + // don't change altitude - // use home yaw if close to home - /* check if we are pretty close to home already */ - float home_dist = get_distance_to_next_waypoint(_navigator->get_home_position()->lat, _navigator->get_home_position()->lon, - _navigator->get_global_position()->lat, _navigator->get_global_position()->lon); + // use home yaw if close to home + /* check if we are pretty close to home already */ + float home_dist = get_distance_to_next_waypoint(_navigator->get_home_position()->lat, + _navigator->get_home_position()->lon, + _navigator->get_global_position()->lat, _navigator->get_global_position()->lon); - if (home_dist < _param_rtl_min_dist.get()) { - _mission_item.yaw = _navigator->get_home_position()->yaw; - - } else { - if (pos_sp_triplet->previous.valid) { - /* if previous setpoint is valid then use it to calculate heading to home */ - _mission_item.yaw = get_bearing_to_next_waypoint( - pos_sp_triplet->previous.lat, pos_sp_triplet->previous.lon, - _mission_item.lat, _mission_item.lon); + if (home_dist < _param_rtl_min_dist.get()) { + _mission_item.yaw = _navigator->get_home_position()->yaw; } else { - /* else use current position */ - _mission_item.yaw = get_bearing_to_next_waypoint( - _navigator->get_global_position()->lat, _navigator->get_global_position()->lon, - _mission_item.lat, _mission_item.lon); + if (pos_sp_triplet->previous.valid) { + /* if previous setpoint is valid then use it to calculate heading to home */ + _mission_item.yaw = get_bearing_to_next_waypoint( + pos_sp_triplet->previous.lat, pos_sp_triplet->previous.lon, + _mission_item.lat, _mission_item.lon); + + } else { + /* else use current position */ + _mission_item.yaw = get_bearing_to_next_waypoint( + _navigator->get_global_position()->lat, _navigator->get_global_position()->lon, + _mission_item.lat, _mission_item.lon); + } } + + _mission_item.loiter_radius = _navigator->get_loiter_radius(); + _mission_item.nav_cmd = NAV_CMD_WAYPOINT; + _mission_item.acceptance_radius = _navigator->get_acceptance_radius(); + _mission_item.time_inside = 0.0f; + _mission_item.autocontinue = true; + _mission_item.origin = ORIGIN_ONBOARD; + + mavlink_log_info(_navigator->get_mavlink_log_pub(), "RTL: return at %d m (%d m above home)", + (int)(_mission_item.altitude), + (int)(_mission_item.altitude - _navigator->get_home_position()->alt)); + + _rtl_start_lock = true; + break; } - _mission_item.loiter_radius = _navigator->get_loiter_radius(); - _mission_item.nav_cmd = NAV_CMD_WAYPOINT; - _mission_item.acceptance_radius = _navigator->get_acceptance_radius(); - _mission_item.time_inside = 0.0f; - _mission_item.autocontinue = true; - _mission_item.origin = ORIGIN_ONBOARD; - - mavlink_log_info(_navigator->get_mavlink_log_pub(), "RTL: return at %d m (%d m above home)", - (int)(_mission_item.altitude), - (int)(_mission_item.altitude - _navigator->get_home_position()->alt)); - - _rtl_start_lock = true; - break; - } case RTL_STATE_TRANSITION_TO_MC: { - _mission_item.nav_cmd = NAV_CMD_DO_VTOL_TRANSITION; - _mission_item.params[0] = vtol_vehicle_status_s::VEHICLE_VTOL_STATE_MC; - break; - } + _mission_item.nav_cmd = NAV_CMD_DO_VTOL_TRANSITION; + _mission_item.params[0] = vtol_vehicle_status_s::VEHICLE_VTOL_STATE_MC; + break; + } case RTL_STATE_DESCEND: { - _mission_item.lat = _navigator->get_home_position()->lat; - _mission_item.lon = _navigator->get_home_position()->lon; - _mission_item.altitude_is_relative = false; - _mission_item.altitude = _navigator->get_home_position()->alt + _param_descend_alt.get(); + _mission_item.lat = _navigator->get_home_position()->lat; + _mission_item.lon = _navigator->get_home_position()->lon; + _mission_item.altitude_is_relative = false; + _mission_item.altitude = _navigator->get_home_position()->alt + _param_descend_alt.get(); - // check if we are already lower - then we will just stay there - if (_mission_item.altitude > _navigator->get_global_position()->alt) { - _mission_item.altitude = _navigator->get_global_position()->alt; + // check if we are already lower - then we will just stay there + if (_mission_item.altitude > _navigator->get_global_position()->alt) { + _mission_item.altitude = _navigator->get_global_position()->alt; + } + + _mission_item.yaw = _navigator->get_home_position()->yaw; + + // except for vtol which might be still off here and should point towards this location + float d_current = get_distance_to_next_waypoint( + _navigator->get_global_position()->lat, _navigator->get_global_position()->lon, + _mission_item.lat, _mission_item.lon); + + if (_navigator->get_vstatus()->is_vtol && d_current > _navigator->get_acceptance_radius()) { + _mission_item.yaw = get_bearing_to_next_waypoint( + _navigator->get_global_position()->lat, _navigator->get_global_position()->lon, + _mission_item.lat, _mission_item.lon); + } + + _mission_item.loiter_radius = _navigator->get_loiter_radius(); + _mission_item.nav_cmd = NAV_CMD_WAYPOINT; + _mission_item.acceptance_radius = _navigator->get_acceptance_radius(); + _mission_item.time_inside = 0.0f; + _mission_item.autocontinue = false; + _mission_item.origin = ORIGIN_ONBOARD; + + /* disable previous setpoint to prevent drift */ + pos_sp_triplet->previous.valid = false; + + mavlink_log_info(_navigator->get_mavlink_log_pub(), "RTL: descend to %d m (%d m above home)", + (int)(_mission_item.altitude), + (int)(_mission_item.altitude - _navigator->get_home_position()->alt)); + break; } - _mission_item.yaw = _navigator->get_home_position()->yaw; - - // except for vtol which might be still off here and should point towards this location - float d_current = get_distance_to_next_waypoint( - _navigator->get_global_position()->lat, _navigator->get_global_position()->lon, - _mission_item.lat, _mission_item.lon); - - if (_navigator->get_vstatus()->is_vtol && d_current > _navigator->get_acceptance_radius()) { - _mission_item.yaw = get_bearing_to_next_waypoint( - _navigator->get_global_position()->lat, _navigator->get_global_position()->lon, - _mission_item.lat, _mission_item.lon); - } - - _mission_item.loiter_radius = _navigator->get_loiter_radius(); - _mission_item.nav_cmd = NAV_CMD_WAYPOINT; - _mission_item.acceptance_radius = _navigator->get_acceptance_radius(); - _mission_item.time_inside = 0.0f; - _mission_item.autocontinue = false; - _mission_item.origin = ORIGIN_ONBOARD; - - /* disable previous setpoint to prevent drift */ - pos_sp_triplet->previous.valid = false; - - mavlink_log_info(_navigator->get_mavlink_log_pub(), "RTL: descend to %d m (%d m above home)", - (int)(_mission_item.altitude), - (int)(_mission_item.altitude - _navigator->get_home_position()->alt)); - break; - } - case RTL_STATE_LOITER: { - bool autoland = _param_land_delay.get() > -DELAY_SIGMA; + bool autoland = _param_land_delay.get() > -DELAY_SIGMA; - _mission_item.lat = _navigator->get_home_position()->lat; - _mission_item.lon = _navigator->get_home_position()->lon; - // don't change altitude - _mission_item.yaw = _navigator->get_home_position()->yaw; - _mission_item.loiter_radius = _navigator->get_loiter_radius(); - _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(); - _mission_item.autocontinue = autoland; - _mission_item.origin = ORIGIN_ONBOARD; + _mission_item.lat = _navigator->get_home_position()->lat; + _mission_item.lon = _navigator->get_home_position()->lon; + // don't change altitude + _mission_item.yaw = _navigator->get_home_position()->yaw; + _mission_item.loiter_radius = _navigator->get_loiter_radius(); + _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(); + _mission_item.autocontinue = autoland; + _mission_item.origin = ORIGIN_ONBOARD; - _navigator->set_can_loiter_at_sp(true); + _navigator->set_can_loiter_at_sp(true); - if (autoland && (Navigator::get_time_inside(_mission_item) > FLT_EPSILON)) { - mavlink_log_info(_navigator->get_mavlink_log_pub(), "RTL: loiter %.1fs", (double)Navigator::get_time_inside(_mission_item)); + if (autoland && (Navigator::get_time_inside(_mission_item) > FLT_EPSILON)) { + mavlink_log_info(_navigator->get_mavlink_log_pub(), "RTL: loiter %.1fs", + (double)Navigator::get_time_inside(_mission_item)); - } else { - mavlink_log_info(_navigator->get_mavlink_log_pub(), "RTL: completed, loiter"); + } else { + mavlink_log_info(_navigator->get_mavlink_log_pub(), "RTL: completed, loiter"); + } + + break; } - break; - } case RTL_STATE_LAND: { - _mission_item.yaw = _navigator->get_home_position()->yaw; - set_land_item(&_mission_item, false); + _mission_item.yaw = _navigator->get_home_position()->yaw; + set_land_item(&_mission_item, false); - mavlink_log_info(_navigator->get_mavlink_log_pub(), "RTL: land at home"); - break; - } + mavlink_log_info(_navigator->get_mavlink_log_pub(), "RTL: land at home"); + break; + } case RTL_STATE_LANDED: { - set_idle_item(&_mission_item); + set_idle_item(&_mission_item); - mavlink_log_info(_navigator->get_mavlink_log_pub(), "RTL: completed, landed"); - break; - } + mavlink_log_info(_navigator->get_mavlink_log_pub(), "RTL: completed, landed"); + break; + } default: break; @@ -327,6 +335,7 @@ RTL::advance_rtl() if (_navigator->get_vstatus()->is_vtol && !_navigator->get_vstatus()->is_rotary_wing) { _rtl_state = RTL_STATE_TRANSITION_TO_MC; } + break; case RTL_STATE_TRANSITION_TO_MC: @@ -334,6 +343,7 @@ RTL::advance_rtl() break; case RTL_STATE_DESCEND: + /* only go to land if autoland is enabled */ if (_param_land_delay.get() < -DELAY_SIGMA || _param_land_delay.get() > DELAY_SIGMA) { _rtl_state = RTL_STATE_LOITER; @@ -341,6 +351,7 @@ RTL::advance_rtl() } else { _rtl_state = RTL_STATE_LAND; } + break; case RTL_STATE_LOITER: