fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-10-21 23:23:33 +08:00
Code Issues Packages Projects Releases Wiki Activity

commander, navigator: minor cleanup (refactoring), code style fixed

Browse Source
This commit is contained in:
Anton Babushkin
2014-01-26 11:52:33 +01:00
parent c7f0553938
commit 7d2efe9367
3 changed files with 243 additions and 158 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/modules/navigator/navigator_main.cpp
+212 -146
View File
@@ -166,12 +166,12 @@ private:
bool _mission_item_valid; /**< current mission item valid */
perf_counter_t _loop_perf; /**< loop performance counter */
Geofence _geofence;
bool _geofence_violation_warning_sent;
bool _fence_valid; /**< flag if fence is valid */
bool _inside_fence; /**< vehicle is inside fence */
bool _inside_fence; /**< vehicle is inside fence */
struct navigation_capabilities_s _nav_caps;
@@ -358,7 +358,7 @@ static const int ERROR = -1;
Navigator *g_navigator;
}
Navigator::Navigator() :
Navigator::Navigator() :
/* state machine transition table */
StateTable(&myTable[0][0], NAV_STATE_MAX, MAX_EVENT),
@@ -490,16 +490,20 @@ void
Navigator::offboard_mission_update(bool isrotaryWing)
{
struct mission_s offboard_mission;
if (orb_copy(ORB_ID(mission), _offboard_mission_sub, &offboard_mission) == OK) {
/* Check mission feasibility, for now do not handle the return value,
* however warnings are issued to the gcs via mavlink from inside the MissionFeasiblityChecker */
dm_item_t dm_current;
if (offboard_mission.dataman_id == 0) {
dm_current = DM_KEY_WAYPOINTS_OFFBOARD_0;
} else {
dm_current = DM_KEY_WAYPOINTS_OFFBOARD_1;
}
missionFeasiblityChecker.checkMissionFeasible(isrotaryWing, dm_current, (size_t)offboard_mission.count, _geofence);
_mission.set_offboard_dataman_id(offboard_mission.dataman_id);
@@ -516,6 +520,7 @@ void
Navigator::onboard_mission_update()
{
struct mission_s onboard_mission;
if (orb_copy(ORB_ID(mission), _onboard_mission_sub, &onboard_mission) == OK) {
_mission.set_current_onboard_mission_index(onboard_mission.current_index);
@@ -558,11 +563,13 @@ Navigator::task_main()
* else clear geofence data in datamanager
*/
struct stat buffer;
if( stat (GEOFENCE_FILENAME, &buffer) == 0 ) {
if (stat(GEOFENCE_FILENAME, &buffer) == 0) {
warnx("Try to load geofence.txt");
_geofence.loadFromFile(GEOFENCE_FILENAME);
} else {
if (_geofence.clearDm() > 0 )
if (_geofence.clearDm() > 0)
warnx("Geofence cleared");
else
warnx("Could not clear geofence");
@@ -578,7 +585,7 @@ Navigator::task_main()
_vstatus_sub = orb_subscribe(ORB_ID(vehicle_status));
_params_sub = orb_subscribe(ORB_ID(parameter_update));
_home_pos_sub = orb_subscribe(ORB_ID(home_position));
/* copy all topics first time */
vehicle_status_update();
parameters_update();
@@ -649,6 +656,7 @@ Navigator::task_main()
if (_vstatus.failsafe_state == FAILSAFE_STATE_NORMAL) {
if (_vstatus.main_state == MAIN_STATE_AUTO) {
bool stick_mode = false;
if (!_vstatus.rc_signal_lost) {
/* RC signal available, use control switches to set mode */
/* RETURN switch, overrides MISSION switch */
@@ -656,21 +664,27 @@ Navigator::task_main()
if (myState != NAV_STATE_READY || _rtl_state != RTL_STATE_LAND) {
dispatch(EVENT_RTL_REQUESTED);
}
stick_mode = true;
} else {
/* MISSION switch */
if (_vstatus.mission_switch == MISSION_SWITCH_LOITER) {
dispatch(EVENT_LOITER_REQUESTED);
stick_mode = true;
} else if (_vstatus.mission_switch == MISSION_SWITCH_MISSION) {
/* switch to mission only if available */
if (_mission.current_mission_available()) {
dispatch(EVENT_MISSION_REQUESTED);
} else {
dispatch(EVENT_LOITER_REQUESTED);
}
stick_mode = true;
}
if (!stick_mode && _vstatus.return_switch == RETURN_SWITCH_NORMAL && myState == NAV_STATE_RTL) {
/* RETURN switch is in normal mode, no MISSION switch mapped, interrupt if in RTL state */
dispatch(EVENT_LOITER_REQUESTED);
@@ -696,15 +710,18 @@ Navigator::task_main()
case NAV_STATE_MISSION:
if (_mission.current_mission_available()) {
dispatch(EVENT_MISSION_REQUESTED);
} else {
dispatch(EVENT_LOITER_REQUESTED);
}
break;
case NAV_STATE_RTL:
if (myState != NAV_STATE_READY || _rtl_state != RTL_STATE_LAND) {
dispatch(EVENT_RTL_REQUESTED);
}
break;
default:
@@ -717,12 +734,14 @@ Navigator::task_main()
if (myState == NAV_STATE_NONE) {
if (_mission.current_mission_available()) {
dispatch(EVENT_MISSION_REQUESTED);
} else {
dispatch(EVENT_LOITER_REQUESTED);
}
}
}
}
} else {
/* not in AUTO mode */
dispatch(EVENT_NONE_REQUESTED);
@@ -765,7 +784,7 @@ Navigator::task_main()
/* offboard mission updated */
if (fds[4].revents & POLLIN) {
offboard_mission_update(_vstatus.is_rotary_wing);
// XXX check if mission really changed
// XXX check if mission really changed
dispatch(EVENT_MISSION_CHANGED);
}
@@ -786,6 +805,7 @@ Navigator::task_main()
/* global position updated */
if (fds[1].revents & POLLIN) {
global_position_update();
/* only check if waypoint has been reached in MISSION and RTL modes */
if (myState == NAV_STATE_MISSION || myState == NAV_STATE_RTL) {
if (check_mission_item_reached()) {
@@ -794,15 +814,15 @@ Navigator::task_main()
}
/* Check geofence violation */
if(!_geofence.inside(&_global_pos)) {
if (!_geofence.inside(&_global_pos)) {
//xxx: publish geofence violation here (or change local flag depending on which app handles the flight termination)
/* Issue a warning about the geofence violation once */
if (!_geofence_violation_warning_sent)
{
if (!_geofence_violation_warning_sent) {
mavlink_log_critical(_mavlink_fd, "#audio: Geofence violation");
_geofence_violation_warning_sent = true;
}
} else {
/* Reset the _geofence_violation_warning_sent field */
_geofence_violation_warning_sent = false;
@@ -852,48 +872,55 @@ Navigator::start()
}
void
Navigator::status()
Navigator::status()
{
warnx("Global position is %svalid", _global_pos.valid ? "" : "in");
if (_global_pos.valid) {
warnx("Longitude %5.5f degrees, latitude %5.5f degrees", _global_pos.lon, _global_pos.lat);
warnx("Altitude %5.5f meters, altitude above home %5.5f meters",
(double)_global_pos.alt, (double)(_global_pos.alt - _home_pos.alt));
(double)_global_pos.alt, (double)(_global_pos.alt - _home_pos.alt));
warnx("Ground velocity in m/s, N %5.5f, E %5.5f, D %5.5f",
(double)_global_pos.vel_n, (double)_global_pos.vel_e, (double)_global_pos.vel_d);
(double)_global_pos.vel_n, (double)_global_pos.vel_e, (double)_global_pos.vel_d);
warnx("Compass heading in degrees %5.5f", (double)(_global_pos.yaw * M_RAD_TO_DEG_F));
}
if (_fence_valid) {
warnx("Geofence is valid");
// warnx("Vertex longitude latitude");
// for (unsigned i = 0; i < _fence.count; i++)
// warnx("%6u %9.5f %8.5f", i, (double)_fence.vertices[i].lon, (double)_fence.vertices[i].lat);
} else {
warnx("Geofence not set");
}
switch (myState) {
case NAV_STATE_NONE:
warnx("State: None");
break;
case NAV_STATE_LOITER:
warnx("State: Loiter");
break;
case NAV_STATE_MISSION:
warnx("State: Mission");
break;
case NAV_STATE_RTL:
warnx("State: RTL");
break;
default:
warnx("State: Unknown");
break;
case NAV_STATE_NONE:
warnx("State: None");
break;
case NAV_STATE_LOITER:
warnx("State: Loiter");
break;
case NAV_STATE_MISSION:
warnx("State: Mission");
break;
case NAV_STATE_RTL:
warnx("State: RTL");
break;
default:
warnx("State: Unknown");
break;
}
}
StateTable::Tran const Navigator::myTable[NAV_STATE_MAX][MAX_EVENT] = {
{
/* STATE_NONE */
{
/* STATE_NONE */
/* EVENT_NONE_REQUESTED */ {NO_ACTION, NAV_STATE_NONE},
/* EVENT_READY_REQUESTED */ {ACTION(&Navigator::start_ready), NAV_STATE_READY},
/* EVENT_LOITER_REQUESTED */ {ACTION(&Navigator::start_loiter), NAV_STATE_LOITER},
@@ -903,8 +930,8 @@ StateTable::Tran const Navigator::myTable[NAV_STATE_MAX][MAX_EVENT] = {
/* EVENT_MISSION_CHANGED */ {NO_ACTION, NAV_STATE_NONE},
/* EVENT_HOME_POSITION_CHANGED */ {NO_ACTION, NAV_STATE_NONE},
},
{
/* STATE_READY */
{
/* STATE_READY */
/* EVENT_NONE_REQUESTED */ {ACTION(&Navigator::start_none), NAV_STATE_NONE},
/* EVENT_READY_REQUESTED */ {NO_ACTION, NAV_STATE_READY},
/* EVENT_LOITER_REQUESTED */ {NO_ACTION, NAV_STATE_READY},
@@ -915,7 +942,7 @@ StateTable::Tran const Navigator::myTable[NAV_STATE_MAX][MAX_EVENT] = {
/* EVENT_HOME_POSITION_CHANGED */ {NO_ACTION, NAV_STATE_READY},
},
{
/* STATE_LOITER */
/* STATE_LOITER */
/* EVENT_NONE_REQUESTED */ {ACTION(&Navigator::start_none), NAV_STATE_NONE},
/* EVENT_READY_REQUESTED */ {NO_ACTION, NAV_STATE_LOITER},
/* EVENT_LOITER_REQUESTED */ {NO_ACTION, NAV_STATE_LOITER},
@@ -925,8 +952,8 @@ StateTable::Tran const Navigator::myTable[NAV_STATE_MAX][MAX_EVENT] = {
/* EVENT_MISSION_CHANGED */ {NO_ACTION, NAV_STATE_LOITER},
/* EVENT_HOME_POSITION_CHANGED */ {NO_ACTION, NAV_STATE_LOITER},
},
{
/* STATE_MISSION */
{
/* STATE_MISSION */
/* EVENT_NONE_REQUESTED */ {ACTION(&Navigator::start_none), NAV_STATE_NONE},
/* EVENT_READY_REQUESTED */ {ACTION(&Navigator::start_ready), NAV_STATE_READY},
/* EVENT_LOITER_REQUESTED */ {ACTION(&Navigator::start_loiter), NAV_STATE_LOITER},
@@ -936,8 +963,8 @@ StateTable::Tran const Navigator::myTable[NAV_STATE_MAX][MAX_EVENT] = {
/* EVENT_MISSION_CHANGED */ {ACTION(&Navigator::start_mission), NAV_STATE_MISSION},
/* EVENT_HOME_POSITION_CHANGED */ {NO_ACTION, NAV_STATE_MISSION},
},
{
/* STATE_RTL */
{
/* STATE_RTL */
/* EVENT_NONE_REQUESTED */ {ACTION(&Navigator::start_none), NAV_STATE_NONE},
/* EVENT_READY_REQUESTED */ {ACTION(&Navigator::start_ready), NAV_STATE_READY},
/* EVENT_LOITER_REQUESTED */ {ACTION(&Navigator::start_loiter), NAV_STATE_LOITER},
@@ -948,7 +975,7 @@ StateTable::Tran const Navigator::myTable[NAV_STATE_MAX][MAX_EVENT] = {
/* EVENT_HOME_POSITION_CHANGED */ {ACTION(&Navigator::start_rtl), NAV_STATE_RTL}, // TODO need to reset rtl_state
},
{
/* STATE_LAND */
/* STATE_LAND */
/* EVENT_NONE_REQUESTED */ {ACTION(&Navigator::start_none), NAV_STATE_NONE},
/* EVENT_READY_REQUESTED */ {ACTION(&Navigator::start_ready), NAV_STATE_READY},
/* EVENT_LOITER_REQUESTED */ {ACTION(&Navigator::start_loiter), NAV_STATE_LOITER},
@@ -1013,6 +1040,7 @@ Navigator::start_loiter()
if (_global_pos.alt < min_alt_amsl) {
_pos_sp_triplet.current.alt = min_alt_amsl;
mavlink_log_info(_mavlink_fd, "[navigator] loiter %.1fm higher", (double)(min_alt_amsl - _global_pos.alt));
} else {
_pos_sp_triplet.current.alt = _global_pos.alt;
mavlink_log_info(_mavlink_fd, "[navigator] loiter at current altitude");
@@ -1065,22 +1093,22 @@ Navigator::set_mission_item()
position_setpoint_from_mission_item(&_pos_sp_triplet.current, &_mission_item);
if (_mission_item.nav_cmd != NAV_CMD_RETURN_TO_LAUNCH &&
_mission_item.nav_cmd != NAV_CMD_LOITER_TIME_LIMIT &&
_mission_item.nav_cmd != NAV_CMD_LOITER_TURN_COUNT &&
_mission_item.nav_cmd != NAV_CMD_LOITER_UNLIMITED) {
_mission_item.nav_cmd != NAV_CMD_LOITER_TIME_LIMIT &&
_mission_item.nav_cmd != NAV_CMD_LOITER_TURN_COUNT &&
_mission_item.nav_cmd != NAV_CMD_LOITER_UNLIMITED) {
/* don't reset RTL state on RTL or LOITER items */
_rtl_state = RTL_STATE_NONE;
}
if (_vstatus.is_rotary_wing) {
if (_need_takeoff && (
_mission_item.nav_cmd == NAV_CMD_TAKEOFF ||
_mission_item.nav_cmd == NAV_CMD_WAYPOINT ||
_mission_item.nav_cmd == NAV_CMD_RETURN_TO_LAUNCH ||
_mission_item.nav_cmd == NAV_CMD_LOITER_TIME_LIMIT ||
_mission_item.nav_cmd == NAV_CMD_LOITER_TURN_COUNT ||
_mission_item.nav_cmd == NAV_CMD_LOITER_UNLIMITED
)) {
_mission_item.nav_cmd == NAV_CMD_TAKEOFF ||
_mission_item.nav_cmd == NAV_CMD_WAYPOINT ||
_mission_item.nav_cmd == NAV_CMD_RETURN_TO_LAUNCH ||
_mission_item.nav_cmd == NAV_CMD_LOITER_TIME_LIMIT ||
_mission_item.nav_cmd == NAV_CMD_LOITER_TURN_COUNT ||
_mission_item.nav_cmd == NAV_CMD_LOITER_UNLIMITED
)) {
/* do special TAKEOFF handling for VTOL */
_need_takeoff = false;
@@ -1108,6 +1136,7 @@ Navigator::set_mission_item()
_pos_sp_triplet.current.yaw = NAN;
_pos_sp_triplet.current.type = SETPOINT_TYPE_TAKEOFF;
}
} else if (_mission_item.nav_cmd == NAV_CMD_LAND) {
/* will need takeoff after landing */
_need_takeoff = true;
@@ -1116,13 +1145,16 @@ Navigator::set_mission_item()
if (_do_takeoff) {
mavlink_log_info(_mavlink_fd, "[navigator] takeoff to %.1fm AMSL", _pos_sp_triplet.current.alt);
} else {
if (onboard) {
mavlink_log_info(_mavlink_fd, "[navigator] heading to onboard WP %d", index);
} else {
mavlink_log_info(_mavlink_fd, "[navigator] heading to offboard WP %d", index);
}
}
} else {
/* since a mission is not advanced without WPs available, this is not supposed to happen */
_mission_item_valid = false;
@@ -1136,6 +1168,7 @@ Navigator::set_mission_item()
if (ret == OK) {
position_setpoint_from_mission_item(&_pos_sp_triplet.next, &item_next);
} else {
/* this will fail for the last WP */
_pos_sp_triplet.next.valid = false;
@@ -1149,17 +1182,21 @@ void
Navigator::start_rtl()
{
_do_takeoff = false;
if (_rtl_state == RTL_STATE_NONE) {
if (_global_pos.alt < _home_pos.alt + _parameters.rtl_alt) {
_rtl_state = RTL_STATE_CLIMB;
} else {
_rtl_state = RTL_STATE_RETURN;
if (_reset_loiter_pos) {
_mission_item.altitude_is_relative = false;
_mission_item.altitude = _global_pos.alt;
}
}
}
_reset_loiter_pos = true;
mavlink_log_info(_mavlink_fd, "[navigator] RTL started");
set_rtl_item();
@@ -1191,118 +1228,123 @@ Navigator::set_rtl_item()
{
switch (_rtl_state) {
case RTL_STATE_CLIMB: {
memcpy(&_pos_sp_triplet.previous, &_pos_sp_triplet.current, sizeof(position_setpoint_s));
memcpy(&_pos_sp_triplet.previous, &_pos_sp_triplet.current, sizeof(position_setpoint_s));
float climb_alt = _home_pos.alt + _parameters.rtl_alt;
if (_vstatus.condition_landed) {
climb_alt = fmaxf(climb_alt, _global_pos.alt + _parameters.rtl_alt);
float climb_alt = _home_pos.alt + _parameters.rtl_alt;
if (_vstatus.condition_landed) {
climb_alt = fmaxf(climb_alt, _global_pos.alt + _parameters.rtl_alt);
}
_mission_item_valid = true;
_mission_item.lat = _global_pos.lat;
_mission_item.lon = _global_pos.lon;
_mission_item.altitude_is_relative = false;
_mission_item.altitude = climb_alt;
_mission_item.yaw = NAN;
_mission_item.loiter_radius = _parameters.loiter_radius;
_mission_item.loiter_direction = 1;
_mission_item.nav_cmd = NAV_CMD_TAKEOFF;
_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;
position_setpoint_from_mission_item(&_pos_sp_triplet.current, &_mission_item);
_pos_sp_triplet.next.valid = false;
mavlink_log_info(_mavlink_fd, "[navigator] RTL: climb to %.1fm", climb_alt - _home_pos.alt);
break;
}
_mission_item_valid = true;
_mission_item.lat = _global_pos.lat;
_mission_item.lon = _global_pos.lon;
_mission_item.altitude_is_relative = false;
_mission_item.altitude = climb_alt;
_mission_item.yaw = NAN;
_mission_item.loiter_radius = _parameters.loiter_radius;
_mission_item.loiter_direction = 1;
_mission_item.nav_cmd = NAV_CMD_TAKEOFF;
_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;
position_setpoint_from_mission_item(&_pos_sp_triplet.current, &_mission_item);
_pos_sp_triplet.next.valid = false;
mavlink_log_info(_mavlink_fd, "[navigator] RTL: climb to %.1fm", climb_alt - _home_pos.alt);
break;
}
case RTL_STATE_RETURN: {
memcpy(&_pos_sp_triplet.previous, &_pos_sp_triplet.current, sizeof(position_setpoint_s));
memcpy(&_pos_sp_triplet.previous, &_pos_sp_triplet.current, sizeof(position_setpoint_s));
_mission_item_valid = true;
_mission_item_valid = true;
_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
_mission_item.loiter_radius = _parameters.loiter_radius;
_mission_item.loiter_direction = 1;
_mission_item.nav_cmd = NAV_CMD_WAYPOINT;
_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.lat = _home_pos.lat;
_mission_item.lon = _home_pos.lon;
// don't change altitude
_mission_item.yaw = NAN; // TODO set heading to home
_mission_item.loiter_radius = _parameters.loiter_radius;
_mission_item.loiter_direction = 1;
_mission_item.nav_cmd = NAV_CMD_WAYPOINT;
_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;
position_setpoint_from_mission_item(&_pos_sp_triplet.current, &_mission_item);
position_setpoint_from_mission_item(&_pos_sp_triplet.current, &_mission_item);
_pos_sp_triplet.next.valid = false;
_pos_sp_triplet.next.valid = false;
mavlink_log_info(_mavlink_fd, "[navigator] RTL: return");
break;
}
mavlink_log_info(_mavlink_fd, "[navigator] RTL: return");
break;
}
case RTL_STATE_DESCEND: {
memcpy(&_pos_sp_triplet.previous, &_pos_sp_triplet.current, sizeof(position_setpoint_s));
memcpy(&_pos_sp_triplet.previous, &_pos_sp_triplet.current, sizeof(position_setpoint_s));
_mission_item_valid = true;
_mission_item_valid = true;
_mission_item.lat = _home_pos.lat;
_mission_item.lon = _home_pos.lon;
_mission_item.altitude_is_relative = false;
_mission_item.altitude = _home_pos.alt + _parameters.land_alt;
_mission_item.yaw = NAN;
_mission_item.loiter_radius = _parameters.loiter_radius;
_mission_item.loiter_direction = 1;
_mission_item.nav_cmd = NAV_CMD_WAYPOINT;
_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.lat = _home_pos.lat;
_mission_item.lon = _home_pos.lon;
_mission_item.altitude_is_relative = false;
_mission_item.altitude = _home_pos.alt + _parameters.land_alt;
_mission_item.yaw = NAN;
_mission_item.loiter_radius = _parameters.loiter_radius;
_mission_item.loiter_direction = 1;
_mission_item.nav_cmd = NAV_CMD_WAYPOINT;
_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;
position_setpoint_from_mission_item(&_pos_sp_triplet.current, &_mission_item);
position_setpoint_from_mission_item(&_pos_sp_triplet.current, &_mission_item);
_pos_sp_triplet.next.valid = false;
_pos_sp_triplet.next.valid = false;
mavlink_log_info(_mavlink_fd, "[navigator] RTL: descend to %.1fm AMSL", _mission_item.altitude);
break;
}
mavlink_log_info(_mavlink_fd, "[navigator] RTL: descend to %.1fm AMSL", _mission_item.altitude);
break;
}
case RTL_STATE_LAND: {
memcpy(&_pos_sp_triplet.previous, &_pos_sp_triplet.current, sizeof(position_setpoint_s));
memcpy(&_pos_sp_triplet.previous, &_pos_sp_triplet.current, sizeof(position_setpoint_s));
_mission_item_valid = true;
_mission_item_valid = true;
_mission_item.lat = _home_pos.lat;
_mission_item.lon = _home_pos.lon;
_mission_item.altitude_is_relative = false;
_mission_item.altitude = _home_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.lat = _home_pos.lat;
_mission_item.lon = _home_pos.lon;
_mission_item.altitude_is_relative = false;
_mission_item.altitude = _home_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;
position_setpoint_from_mission_item(&_pos_sp_triplet.current, &_mission_item);
position_setpoint_from_mission_item(&_pos_sp_triplet.current, &_mission_item);
_pos_sp_triplet.next.valid = false;
_pos_sp_triplet.next.valid = false;
mavlink_log_info(_mavlink_fd, "[navigator] RTL: land");
break;
}
mavlink_log_info(_mavlink_fd, "[navigator] RTL: land");
break;
}
default: {
mavlink_log_critical(_mavlink_fd, "[navigator] error: unknown RTL state: %d", _rtl_state);
start_loiter();
break;
}
mavlink_log_critical(_mavlink_fd, "[navigator] error: unknown RTL state: %d", _rtl_state);
start_loiter();
break;
}
}
publish_position_setpoint_triplet();
@@ -1312,28 +1354,35 @@ void
Navigator::position_setpoint_from_mission_item(position_setpoint_s *sp, mission_item_s *item)
{
sp->valid = true;
if (item->nav_cmd == NAV_CMD_RETURN_TO_LAUNCH) {
/* set home position for RTL item */
sp->lat = _home_pos.lat;
sp->lon = _home_pos.lon;
sp->alt = _home_pos.alt + _parameters.rtl_alt;
} 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;
if (item->nav_cmd == NAV_CMD_TAKEOFF) {
sp->type = SETPOINT_TYPE_TAKEOFF;
} else if (item->nav_cmd == NAV_CMD_LAND) {
sp->type = SETPOINT_TYPE_LAND;
} else if (item->nav_cmd == NAV_CMD_LOITER_TIME_LIMIT ||
item->nav_cmd == NAV_CMD_LOITER_TURN_COUNT ||
item->nav_cmd == NAV_CMD_LOITER_UNLIMITED) {
item->nav_cmd == NAV_CMD_LOITER_TURN_COUNT ||
item->nav_cmd == NAV_CMD_LOITER_UNLIMITED) {
sp->type = SETPOINT_TYPE_LOITER;
} else {
sp->type = SETPOINT_TYPE_NORMAL;
}
@@ -1350,6 +1399,7 @@ Navigator::check_mission_item_reached()
if (_mission_item.nav_cmd == NAV_CMD_LAND) {
if (_vstatus.is_rotary_wing) {
return _vstatus.condition_landed;
} else {
/* For fw there is currently no landing detector:
* make sure control is not stopped when overshooting the landing waypoint */
@@ -1364,7 +1414,7 @@ Navigator::check_mission_item_reached()
_mission_item.loiter_radius > 0.01f) {
return false;
}
}
uint64_t now = hrt_absolute_time();
@@ -1384,18 +1434,20 @@ Navigator::check_mission_item_reached()
/* current relative or AMSL altitude depending on mission item altitude_is_relative flag */
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);
(double)_global_pos.lat, (double)_global_pos.lon, _global_pos.alt,
&dist_xy, &dist_z);
if (_do_takeoff) {
if (_global_pos.alt > wp_alt_amsl - acceptance_radius) {
/* require only altitude for takeoff */
_waypoint_position_reached = true;
}
} else {
if (dist >= 0.0f && dist <= acceptance_radius) {
_waypoint_position_reached = true;
@@ -1407,9 +1459,11 @@ Navigator::check_mission_item_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);
if (fabsf(yaw_err) < 0.05f) { /* XXX get rid of magic number */
_waypoint_yaw_reached = true;
}
} else {
_waypoint_yaw_reached = true;
}
@@ -1419,20 +1473,22 @@ Navigator::check_mission_item_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, "[navigator] waypoint reached, wait for %.1fs", _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) {
|| _mission_item.nav_cmd == NAV_CMD_TAKEOFF) {
_time_first_inside_orbit = 0;
_waypoint_yaw_reached = false;
_waypoint_position_reached = false;
return true;
}
}
return false;
}
@@ -1445,6 +1501,7 @@ Navigator::on_mission_item_reached()
/* takeoff completed */
_do_takeoff = false;
mavlink_log_info(_mavlink_fd, "[navigator] takeoff completed");
} else {
/* advance by one mission item */
_mission.move_to_next();
@@ -1452,23 +1509,28 @@ Navigator::on_mission_item_reached()
if (_mission.current_mission_available()) {
set_mission_item();
} else {
/* if no more mission items available then finish mission */
/* loiter at last waypoint */
_reset_loiter_pos = false;
mavlink_log_info(_mavlink_fd, "[navigator] mission completed");
if (_vstatus.condition_landed) {
dispatch(EVENT_READY_REQUESTED);
} else {
dispatch(EVENT_LOITER_REQUESTED);
}
}
} else {
/* RTL finished */
if (_rtl_state == RTL_STATE_LAND) {
/* landed at home position */
mavlink_log_info(_mavlink_fd, "[navigator] RTL completed, landed");
dispatch(EVENT_READY_REQUESTED);
} else {
/* next RTL step */
_rtl_state = (RTLState)(_rtl_state + 1);
@@ -1546,6 +1608,7 @@ Navigator::publish_control_mode()
default:
break;
}
break;
case FAILSAFE_STATE_RTL:
@@ -1583,6 +1646,7 @@ Navigator::publish_control_mode()
/* navigator has control, set control mode flags according to nav state*/
if (navigator_enabled) {
_control_mode.flag_control_manual_enabled = false;
if (myState == NAV_STATE_READY) {
/* disable all controllers, armed but idle */
_control_mode.flag_control_rates_enabled = false;
@@ -1591,6 +1655,7 @@ Navigator::publish_control_mode()
_control_mode.flag_control_velocity_enabled = false;
_control_mode.flag_control_altitude_enabled = false;
_control_mode.flag_control_climb_rate_enabled = false;
} else {
_control_mode.flag_control_rates_enabled = true;
_control_mode.flag_control_attitude_enabled = true;
@@ -1669,8 +1734,9 @@ int navigator_main(int argc, char *argv[])
} 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);
navigator::g_navigator->load_fence_from_file(GEOFENCE_FILENAME);
} else {
usage();