dark0707
/
px4-firmware
forked from Archive/PX4-Autopilot
1
0
Fork 0
Code Pull Requests Activity

Merge pull request #660 from PX4/rtl_heading 

navigator: use bearing to home for RTL
This commit is contained in:
julianoes 2014-03-03 09:14:38 +01:00
parent 85247e3fdd f79aea89bc
commit 8b99062e66
1 changed files with 53 additions and 18 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

71
src/modules/navigator/navigator_main.cpp
View File

@ -321,6 +321,11 @@ private:
*/
bool onboard_mission_available(unsigned relative_index);
/**
* Reset all "reached" flags.
*/
void reset_reached();
/**
* Check if current mission item has been reached.
*/
@ -853,9 +858,6 @@ Navigator::task_main()
if (myState != prevState) {
mavlink_log_info(_mavlink_fd, "#audio: navigation state: %s", nav_states_str[myState]);
prevState = myState;
/* reset time counter on state changes */
_time_first_inside_orbit = 0;
}
perf_end(_loop_perf);
@ -1007,6 +1009,8 @@ StateTable::Tran const Navigator::myTable[NAV_STATE_MAX][MAX_EVENT] = {
void
Navigator::start_none()
{
reset_reached();
_pos_sp_triplet.previous.valid = false;
_pos_sp_triplet.current.valid = false;
_pos_sp_triplet.next.valid = false;
@ -1022,6 +1026,8 @@ Navigator::start_none()
void
Navigator::start_ready()
{
reset_reached();
_pos_sp_triplet.previous.valid = false;
_pos_sp_triplet.current.valid = true;
_pos_sp_triplet.next.valid = false;
@ -1044,6 +1050,8 @@ Navigator::start_ready()
void
Navigator::start_loiter()
{
reset_reached();
_do_takeoff = false;
/* set loiter position if needed */
@ -1089,6 +1097,8 @@ Navigator::start_mission()
void
Navigator::set_mission_item()
{
reset_reached();
/* copy current mission to previous item */
memcpy(&_pos_sp_triplet.previous, &_pos_sp_triplet.current, sizeof(position_setpoint_s));
@ -1102,9 +1112,6 @@ Navigator::set_mission_item()
ret = _mission.get_current_mission_item(&_mission_item, &onboard, &index);
if (ret == OK) {
/* reset time counter for new item */
_time_first_inside_orbit = 0;
_mission_item_valid = true;
position_setpoint_from_mission_item(&_pos_sp_triplet.current, &_mission_item);
@ -1222,6 +1229,8 @@ Navigator::start_rtl()
void
Navigator::start_land()
{
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 */
_do_takeoff = false;
@ -1253,6 +1262,8 @@ Navigator::start_land()
void
Navigator::start_land_home()
{
reset_reached();
/* land to home position, should be called when hovering above home, from RTL state */
_do_takeoff = false;
_reset_loiter_pos = true;
@ -1283,8 +1294,7 @@ Navigator::start_land_home()
void
Navigator::set_rtl_item()
{
/*reset time counter for new RTL item */
_time_first_inside_orbit = 0;
reset_reached();
switch (_rtl_state) {
case RTL_STATE_CLIMB: {
@ -1328,7 +1338,14 @@ Navigator::set_rtl_item()
_mission_item.lat = _home_pos.lat;
_mission_item.lon = _home_pos.lon;
// don't change altitude
_mission_item.yaw = NAN; // TODO set heading to home
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(_global_pos.lat, _global_pos.lon, _mission_item.lat, _mission_item.lon);
}
_mission_item.loiter_radius = _parameters.loiter_radius;
_mission_item.loiter_direction = 1;
_mission_item.nav_cmd = NAV_CMD_WAYPOINT;
@ -1417,17 +1434,28 @@ Navigator::position_setpoint_from_mission_item(position_setpoint_s *sp, mission_
sp->lon = _home_pos.lon;
sp->alt = _home_pos.alt + _parameters.rtl_alt;
if (_pos_sp_triplet.previous.valid) {
/* if previous setpoint is valid then use it to calculate heading to home */
sp->yaw = get_bearing_to_next_waypoint(_pos_sp_triplet.previous.lat, _pos_sp_triplet.previous.lon, sp->lat, sp->lon);
} else {
/* else use current position */
sp->yaw = get_bearing_to_next_waypoint(_global_pos.lat, _global_pos.lon, sp->lat, sp->lon);
}
sp->loiter_radius = _parameters.loiter_radius;
sp->loiter_direction = 1;
sp->pitch_min = 0.0f;
} else {
sp->lat = item->lat;
sp->lon = item->lon;
sp->alt = item->altitude_is_relative ? item->altitude + _home_pos.alt : item->altitude;
sp->yaw = item->yaw;
sp->loiter_radius = item->loiter_radius;
sp->loiter_direction = item->loiter_direction;
sp->pitch_min = item->pitch_min;
}
sp->yaw = item->yaw;
sp->loiter_radius = item->loiter_radius;
sp->loiter_direction = item->loiter_direction;
sp->pitch_min = item->pitch_min;
if (item->nav_cmd == NAV_CMD_TAKEOFF) {
sp->type = SETPOINT_TYPE_TAKEOFF;
@ -1504,7 +1532,7 @@ Navigator::check_mission_item_reached()
}
}
if (!_waypoint_yaw_reached) {
if (_waypoint_position_reached && !_waypoint_yaw_reached) {
if (_vstatus.is_rotary_wing && !_do_takeoff && 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);
@ -1531,9 +1559,7 @@ Navigator::check_mission_item_reached()
/* 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) {
_time_first_inside_orbit = 0;
_waypoint_yaw_reached = false;
_waypoint_position_reached = false;
reset_reached();
return true;
}
}
@ -1542,6 +1568,15 @@ Navigator::check_mission_item_reached()
}
void
Navigator::reset_reached()
{
_time_first_inside_orbit = 0;
_waypoint_position_reached = false;
_waypoint_yaw_reached = false;
}
void
Navigator::on_mission_item_reached()
{