zrzk
/
px4_autopilot
1
0
Fork 0
Code Issues Pull Requests Projects Releases Wiki Activity
Browse Source

navigator: spaces/tabs fixed, old commented code removed

sbg
Anton Babushkin 11 years ago
parent
6e5aafb3a7
commit
8e8798a522
2 changed files with 36 additions and 358 deletions
Whitespace
Show all changes Ignore whitespace when comparing lines Ignore changes in amount of whitespace Ignore changes in whitespace at EOL
Split View
Diff Options
Show Stats Download Patch File Download Diff File
  1. 322
      src/modules/navigator/navigator_main.cpp
  2. 72
      src/modules/navigator/rtl.cpp

322
src/modules/navigator/navigator_main.cpp
Unescape View File

@ -446,329 +446,7 @@ Navigator::status() @@ -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()
{

72
src/modules/navigator/rtl.cpp
Unescape View File

@ -79,7 +79,7 @@ RTL::on_inactive() @@ -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) @@ -89,28 +89,28 @@ RTL::on_active(struct position_setpoint_triplet_s *pos_sp_triplet)
bool updated = false;
if (_first_run) {
_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");
/* 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;
}
}
_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");
/* 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;
}
}
set_rtl_item(pos_sp_triplet);
@ -132,15 +132,15 @@ RTL::set_rtl_item(position_setpoint_triplet_s *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) @@ -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

Write Preview
Loading…
Cancel
Save