forked from Archive/PX4-Autopilot
navigator: spaces/tabs fixed, old commented code removed
This commit is contained in:
Anton Babushkin
parent
6e5aafb3a7
commit
8e8798a522
|
|
@ -446,329 +446,7 @@ Navigator::status()
|
|||
warnx("Geofence not set");
|
||||
}
|
||||
}
|
||||
#if 0
|
||||
bool
|
||||
Navigator::start_none_on_ground()
|
||||
{
|
||||
reset_reached();
|
||||
|
||||
_pos_sp_triplet.previous.valid = false;
|
||||
_pos_sp_triplet.current.valid = false;
|
||||
_pos_sp_triplet.next.valid = false;
|
||||
|
||||
_update_triplet = true;
|
||||
return true;
|
||||
}
|
||||
|
||||
bool
|
||||
Navigator::start_none_in_air()
|
||||
{
|
||||
reset_reached();
|
||||
|
||||
_pos_sp_triplet.previous.valid = false;
|
||||
_pos_sp_triplet.current.valid = false;
|
||||
_pos_sp_triplet.next.valid = false;
|
||||
|
||||
_update_triplet = true;
|
||||
return true;
|
||||
}
|
||||
|
||||
bool
|
||||
Navigator::start_auto_on_ground()
|
||||
{
|
||||
reset_reached();
|
||||
|
||||
_pos_sp_triplet.previous.valid = false;
|
||||
_pos_sp_triplet.current.valid = true;
|
||||
_pos_sp_triplet.next.valid = false;
|
||||
|
||||
_pos_sp_triplet.current.type = SETPOINT_TYPE_IDLE;
|
||||
|
||||
_update_triplet = true;
|
||||
return true;
|
||||
}
|
||||
|
||||
bool
|
||||
Navigator::start_loiter()
|
||||
{
|
||||
/* if no existing item available, use current position */
|
||||
if (!(_pos_sp_triplet.current.valid && _waypoint_position_reached)) {
|
||||
|
||||
_pos_sp_triplet.current.lat = _global_pos.lat;
|
||||
_pos_sp_triplet.current.lon = _global_pos.lon;
|
||||
_pos_sp_triplet.current.yaw = NAN; // NAN means to use current yaw
|
||||
_pos_sp_triplet.current.alt = _global_pos.alt;
|
||||
}
|
||||
_pos_sp_triplet.current.type = SETPOINT_TYPE_LOITER;
|
||||
_pos_sp_triplet.current.loiter_radius = _parameters.loiter_radius;
|
||||
_pos_sp_triplet.current.loiter_direction = 1;
|
||||
_pos_sp_triplet.previous.valid = false;
|
||||
_pos_sp_triplet.current.valid = true;
|
||||
_pos_sp_triplet.next.valid = false;
|
||||
|
||||
mavlink_log_info(_mavlink_fd, "#audio: loiter at current altitude");
|
||||
|
||||
_update_triplet = true;
|
||||
return true;
|
||||
}
|
||||
|
||||
bool
|
||||
Navigator::start_mission()
|
||||
{
|
||||
/* start fresh */
|
||||
_pos_sp_triplet.previous.valid = false;
|
||||
_pos_sp_triplet.current.valid = false;
|
||||
_pos_sp_triplet.next.valid = false;
|
||||
|
||||
return set_mission_items();
|
||||
}
|
||||
|
||||
bool
|
||||
Navigator::advance_mission()
|
||||
{
|
||||
/* tell mission to move by one */
|
||||
_mission.move_to_next();
|
||||
|
||||
/* now try to set the new mission items, if it fails, it will dispatch loiter */
|
||||
return set_mission_items();
|
||||
}
|
||||
|
||||
bool
|
||||
Navigator::set_mission_items()
|
||||
{
|
||||
if (_pos_sp_triplet.current.valid) {
|
||||
memcpy(&_pos_sp_triplet.previous, &_pos_sp_triplet.current, sizeof(position_setpoint_s));
|
||||
_pos_sp_triplet.previous.valid = true;
|
||||
}
|
||||
|
||||
bool onboard;
|
||||
int index;
|
||||
|
||||
/* if we fail to set the current mission, continue to loiter */
|
||||
if (!_mission.get_current_mission_item(&_mission_item, &onboard, &index)) {
|
||||
|
||||
return false;
|
||||
}
|
||||
|
||||
/* if we got an RTL mission item, switch to RTL mode and give up */
|
||||
if (_mission_item.nav_cmd == NAV_CMD_RETURN_TO_LAUNCH) {
|
||||
return false;
|
||||
}
|
||||
|
||||
_mission_item_valid = true;
|
||||
|
||||
/* convert the current mission item and set it valid */
|
||||
mission_item_to_position_setpoint(&_mission_item, &_pos_sp_triplet.current);
|
||||
_pos_sp_triplet.current.valid = true;
|
||||
|
||||
|
||||
mission_item_s next_mission_item;
|
||||
|
||||
bool last_wp = false;
|
||||
/* now try to set the next mission item as well, if there is no more next
|
||||
* this means we're heading to the last waypoint */
|
||||
if (_mission.get_next_mission_item(&next_mission_item)) {
|
||||
/* convert the next mission item and set it valid */
|
||||
mission_item_to_position_setpoint(&next_mission_item, &_pos_sp_triplet.next);
|
||||
_pos_sp_triplet.next.valid = true;
|
||||
} else {
|
||||
last_wp = true;
|
||||
}
|
||||
|
||||
/* notify user about what happened */
|
||||
mavlink_log_info(_mavlink_fd, "#audio: heading to %s%swaypoint %d",
|
||||
(last_wp ? "last " : "" ), (onboard ? "onboard " : ""), index);
|
||||
|
||||
_update_triplet = true;
|
||||
|
||||
reset_reached();
|
||||
|
||||
return true;
|
||||
}
|
||||
|
||||
bool
|
||||
Navigator::start_rtl()
|
||||
{
|
||||
if (_rtl.get_current_rtl_item(&_global_pos, &_mission_item)) {
|
||||
|
||||
_mission_item_valid = true;
|
||||
|
||||
mission_item_to_position_setpoint(&_mission_item, &_pos_sp_triplet.current);
|
||||
_pos_sp_triplet.current.valid = true;
|
||||
|
||||
reset_reached();
|
||||
|
||||
_update_triplet = true;
|
||||
return true;
|
||||
}
|
||||
|
||||
/* if RTL doesn't work, fallback to loiter */
|
||||
return false;
|
||||
}
|
||||
|
||||
bool
|
||||
Navigator::advance_rtl()
|
||||
{
|
||||
/* tell mission to move by one */
|
||||
_rtl.move_to_next();
|
||||
|
||||
/* now try to set the new mission items, if it fails, it will dispatch loiter */
|
||||
if (_rtl.get_current_rtl_item(&_global_pos, &_mission_item)) {
|
||||
|
||||
_mission_item_valid = true;
|
||||
|
||||
mission_item_to_position_setpoint(&_mission_item, &_pos_sp_triplet.current);
|
||||
_pos_sp_triplet.current.valid = true;
|
||||
|
||||
reset_reached();
|
||||
|
||||
_update_triplet = true;
|
||||
return true;
|
||||
}
|
||||
|
||||
return false;
|
||||
}
|
||||
|
||||
bool
|
||||
Navigator::start_land()
|
||||
{
|
||||
/* TODO: verify/test */
|
||||
|
||||
reset_reached();
|
||||
|
||||
/* this state can be requested by commander even if no global position available,
|
||||
* in his case controller must perform landing without position control */
|
||||
|
||||
memcpy(&_pos_sp_triplet.previous, &_pos_sp_triplet.current, sizeof(position_setpoint_s));
|
||||
|
||||
_mission_item.lat = _global_pos.lat;
|
||||
_mission_item.lon = _global_pos.lon;
|
||||
_mission_item.altitude_is_relative = false;
|
||||
_mission_item.altitude = _global_pos.alt;
|
||||
_mission_item.yaw = NAN;
|
||||
_mission_item.loiter_radius = _parameters.loiter_radius;
|
||||
_mission_item.loiter_direction = 1;
|
||||
_mission_item.nav_cmd = NAV_CMD_LAND;
|
||||
_mission_item.acceptance_radius = _parameters.acceptance_radius;
|
||||
_mission_item.time_inside = 0.0f;
|
||||
_mission_item.pitch_min = 0.0f;
|
||||
_mission_item.autocontinue = true;
|
||||
_mission_item.origin = ORIGIN_ONBOARD;
|
||||
|
||||
_mission_item_valid = true;
|
||||
|
||||
mission_item_to_position_setpoint(&_mission_item, &_pos_sp_triplet.current);
|
||||
|
||||
_pos_sp_triplet.next.valid = false;
|
||||
|
||||
_update_triplet = true;
|
||||
return true;
|
||||
}
|
||||
bool
|
||||
Navigator::check_mission_item_reached()
|
||||
{
|
||||
/* only check if there is actually a mission item to check */
|
||||
if (!_mission_item_valid) {
|
||||
return false;
|
||||
}
|
||||
|
||||
if (_mission_item.nav_cmd == NAV_CMD_LAND) {
|
||||
return _vstatus.condition_landed;
|
||||
}
|
||||
|
||||
/* XXX TODO count turns */
|
||||
if ((_mission_item.nav_cmd == NAV_CMD_LOITER_TURN_COUNT ||
|
||||
_mission_item.nav_cmd == NAV_CMD_LOITER_UNLIMITED) &&
|
||||
_mission_item.loiter_radius > 0.01f) {
|
||||
|
||||
// return false;
|
||||
// }
|
||||
|
||||
uint64_t now = hrt_absolute_time();
|
||||
|
||||
if (!_waypoint_position_reached) {
|
||||
float acceptance_radius;
|
||||
|
||||
if (_mission_item.nav_cmd == NAV_CMD_WAYPOINT && _mission_item.acceptance_radius > 0.01f) {
|
||||
acceptance_radius = _mission_item.acceptance_radius;
|
||||
|
||||
} else {
|
||||
acceptance_radius = _parameters.acceptance_radius;
|
||||
}
|
||||
|
||||
if (_do_takeoff) {
|
||||
/* require only altitude for takeoff */
|
||||
if (_global_pos.alt > _pos_sp_triplet.current.alt - acceptance_radius) {
|
||||
_waypoint_position_reached = true;
|
||||
}
|
||||
} else {
|
||||
float dist = -1.0f;
|
||||
float dist_xy = -1.0f;
|
||||
float dist_z = -1.0f;
|
||||
|
||||
/* calculate AMSL altitude for this waypoint */
|
||||
float wp_alt_amsl = _mission_item.altitude;
|
||||
|
||||
if (_mission_item.altitude_is_relative)
|
||||
wp_alt_amsl += _home_pos.alt;
|
||||
|
||||
dist = get_distance_to_point_global_wgs84(_mission_item.lat, _mission_item.lon, wp_alt_amsl,
|
||||
(double)_global_pos.lat, (double)_global_pos.lon, _global_pos.alt,
|
||||
&dist_xy, &dist_z);
|
||||
|
||||
if (dist >= 0.0f && dist <= acceptance_radius) {
|
||||
_waypoint_position_reached = true;
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
if (_waypoint_position_reached && !_waypoint_yaw_reached) {
|
||||
|
||||
/* TODO: removed takeoff, why? */
|
||||
if (_vstatus.is_rotary_wing && isfinite(_mission_item.yaw)) {
|
||||
|
||||
/* check yaw if defined only for rotary wing except takeoff */
|
||||
float yaw_err = _wrap_pi(_mission_item.yaw - _global_pos.yaw);
|
||||
|
||||
if (fabsf(yaw_err) < 0.2f) { /* TODO: get rid of magic number */
|
||||
_waypoint_yaw_reached = true;
|
||||
}
|
||||
|
||||
} else {
|
||||
_waypoint_yaw_reached = true;
|
||||
}
|
||||
}
|
||||
|
||||
/* check if the current waypoint was reached */
|
||||
if (_waypoint_position_reached && _waypoint_yaw_reached) {
|
||||
|
||||
if (_time_first_inside_orbit == 0) {
|
||||
_time_first_inside_orbit = now;
|
||||
|
||||
if (_mission_item.time_inside > 0.01f) {
|
||||
mavlink_log_info(_mavlink_fd, "#audio: waypoint reached, wait for %.1fs",
|
||||
(double)_mission_item.time_inside);
|
||||
}
|
||||
}
|
||||
|
||||
/* check if the MAV was long enough inside the waypoint orbit */
|
||||
if ((now - _time_first_inside_orbit >= (uint64_t)_mission_item.time_inside * 1e6)
|
||||
|| _mission_item.nav_cmd == NAV_CMD_TAKEOFF) {
|
||||
return true;
|
||||
}
|
||||
}
|
||||
return false;
|
||||
}
|
||||
|
||||
void
|
||||
Navigator::reset_reached()
|
||||
{
|
||||
_time_first_inside_orbit = 0;
|
||||
_waypoint_position_reached = false;
|
||||
_waypoint_yaw_reached = false;
|
||||
|
||||
}
|
||||
#endif
|
||||
void
|
||||
Navigator::publish_position_setpoint_triplet()
|
||||
{
|
||||
|
|
|
|||
|
|
@ -79,7 +79,7 @@ RTL::on_inactive()
|
|||
|
||||
/* reset RTL state only if setpoint moved */
|
||||
if (!_navigator->get_can_loiter_at_sp()) {
|
||||
_rtl_state = RTL_STATE_NONE;
|
||||
_rtl_state = RTL_STATE_NONE;
|
||||
}
|
||||
}
|
||||
|
||||
|
|
@ -89,28 +89,28 @@ RTL::on_active(struct position_setpoint_triplet_s *pos_sp_triplet)
|
|||
bool updated = false;
|
||||
|
||||
if (_first_run) {
|
||||
_first_run = false;
|
||||
_first_run = false;
|
||||
|
||||
/* decide where to enter the RTL procedure when we switch into it */
|
||||
if (_rtl_state == RTL_STATE_NONE) {
|
||||
/* for safety reasons don't go into RTL if landed */
|
||||
if (_navigator->get_vstatus()->condition_landed) {
|
||||
_rtl_state = RTL_STATE_FINISHED;
|
||||
mavlink_log_info(_navigator->get_mavlink_fd(), "#audio: no RTL when landed");
|
||||
/* decide where to enter the RTL procedure when we switch into it */
|
||||
if (_rtl_state == RTL_STATE_NONE) {
|
||||
/* for safety reasons don't go into RTL if landed */
|
||||
if (_navigator->get_vstatus()->condition_landed) {
|
||||
_rtl_state = RTL_STATE_FINISHED;
|
||||
mavlink_log_info(_navigator->get_mavlink_fd(), "#audio: no RTL when landed");
|
||||
|
||||
/* if lower than return altitude, climb up first */
|
||||
} else if (_navigator->get_global_position()->alt < _navigator->get_home_position()->alt
|
||||
+ _param_return_alt.get()) {
|
||||
_rtl_state = RTL_STATE_CLIMB;
|
||||
/* if lower than return altitude, climb up first */
|
||||
} else if (_navigator->get_global_position()->alt < _navigator->get_home_position()->alt
|
||||
+ _param_return_alt.get()) {
|
||||
_rtl_state = RTL_STATE_CLIMB;
|
||||
|
||||
/* otherwise go straight to return */
|
||||
} else {
|
||||
/* set altitude setpoint to current altitude */
|
||||
_rtl_state = RTL_STATE_RETURN;
|
||||
_mission_item.altitude_is_relative = false;
|
||||
_mission_item.altitude = _navigator->get_global_position()->alt;
|
||||
}
|
||||
}
|
||||
/* otherwise go straight to return */
|
||||
} else {
|
||||
/* set altitude setpoint to current altitude */
|
||||
_rtl_state = RTL_STATE_RETURN;
|
||||
_mission_item.altitude_is_relative = false;
|
||||
_mission_item.altitude = _navigator->get_global_position()->alt;
|
||||
}
|
||||
}
|
||||
|
||||
set_rtl_item(pos_sp_triplet);
|
||||
|
||||
|
|
@ -132,15 +132,15 @@ RTL::set_rtl_item(position_setpoint_triplet_s *pos_sp_triplet)
|
|||
/* make sure we have the latest params */
|
||||
updateParams();
|
||||
|
||||
if (_rtl_state == RTL_STATE_FINISHED) {
|
||||
/* RTL finished, nothing to do */
|
||||
pos_sp_triplet->next.valid = false;
|
||||
mavlink_log_info(_navigator->get_mavlink_fd(), "#audio: RTL: completed");
|
||||
return;
|
||||
}
|
||||
if (_rtl_state == RTL_STATE_FINISHED) {
|
||||
/* RTL finished, nothing to do */
|
||||
pos_sp_triplet->next.valid = false;
|
||||
mavlink_log_info(_navigator->get_mavlink_fd(), "#audio: RTL: completed");
|
||||
return;
|
||||
}
|
||||
|
||||
set_previous_pos_setpoint(pos_sp_triplet);
|
||||
_navigator->set_can_loiter_at_sp(false);
|
||||
set_previous_pos_setpoint(pos_sp_triplet);
|
||||
_navigator->set_can_loiter_at_sp(false);
|
||||
|
||||
switch (_rtl_state) {
|
||||
case RTL_STATE_CLIMB: {
|
||||
|
|
@ -241,11 +241,11 @@ RTL::set_rtl_item(position_setpoint_triplet_s *pos_sp_triplet)
|
|||
break;
|
||||
}
|
||||
|
||||
/* convert mission item to current position setpoint and make it valid */
|
||||
mission_item_to_position_setpoint(&_mission_item, &pos_sp_triplet->current);
|
||||
reset_mission_item_reached();
|
||||
pos_sp_triplet->current.valid = true;
|
||||
pos_sp_triplet->next.valid = false;
|
||||
/* convert mission item to current position setpoint and make it valid */
|
||||
mission_item_to_position_setpoint(&_mission_item, &pos_sp_triplet->current);
|
||||
reset_mission_item_reached();
|
||||
pos_sp_triplet->current.valid = true;
|
||||
pos_sp_triplet->next.valid = false;
|
||||
}
|
||||
|
||||
void
|
||||
|
|
|
|||
Loading…
Reference in New Issue