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

navigator fix code style

sbg
Daniel Agar 8 years ago
parent
be14c11589
commit
c280358e32
11 changed files with 408 additions and 310 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. 5
      Tools/files_to_check_code_style.sh
  2. 11
      src/modules/navigator/mission.cpp
  3. 2
      src/modules/navigator/mission.h
  4. 262
      src/modules/navigator/mission_block.cpp
  5. 2
      src/modules/navigator/mission_block.h
  6. 158
      src/modules/navigator/mission_feasibility_checker.cpp
  7. 18
      src/modules/navigator/mission_feasibility_checker.h
  8. 38
      src/modules/navigator/navigation.h
  9. 42
      src/modules/navigator/navigator.h
  10. 178
      src/modules/navigator/navigator_main.cpp
  11. 2
      src/modules/navigator/takeoff.cpp

5
Tools/files_to_check_code_style.sh
Unescape View File

@ -17,13 +17,12 @@ exec find src \ @@ -17,13 +17,12 @@ exec find src \
-path src/lib/mathlib -prune -o \
-path src/lib/matrix -prune -o \
-path src/drivers/bootloaders -o \
-path src/modules/uavcanesc -o \
-path src/modules/uavcannode -o \
-path src/modules/commander -prune -o \
-path src/modules/mavlink -prune -o \
-path src/modules/navigator -prune -o \
-path src/modules/sdlog2 -prune -o \
-path src/modules/systemlib/uthash -prune -o \
-path src/modules/uavcan -prune -o \
-path src/modules/uavcan/libuavcan -prune -o \
-path src/modules/uavcanesc -o \
-path src/modules/uavcannode -o \
-type f \( -name "*.c" -o -name "*.h" -o -name "*.cpp" -o -name "*.hpp" \) | grep $PATTERN

11
src/modules/navigator/mission.cpp
Unescape View File

@ -289,6 +289,7 @@ Mission::find_offboard_land_start() @@ -289,6 +289,7 @@ Mission::find_offboard_land_start()
for (size_t i = 0; i < _offboard_mission.count; i++) {
struct mission_item_s missionitem;
const ssize_t len = sizeof(missionitem);
if (dm_read(dm_current, i, &missionitem, len) != len) {
/* not supposed to happen unless the datamanager can't access the SD card, etc. */
return -1;
@ -860,8 +861,8 @@ Mission::do_need_takeoff() @@ -860,8 +861,8 @@ Mission::do_need_takeoff()
_need_takeoff = false;
} else if (_navigator->get_global_position()->alt <= takeoff_alt - _navigator->get_acceptance_radius()
&& (_mission_item.nav_cmd == NAV_CMD_TAKEOFF
|| _mission_item.nav_cmd == NAV_CMD_VTOL_TAKEOFF)) {
&& (_mission_item.nav_cmd == NAV_CMD_TAKEOFF
|| _mission_item.nav_cmd == NAV_CMD_VTOL_TAKEOFF)) {
/* if in-air but below takeoff height and we have a takeoff item */
_need_takeoff = true;
}
@ -1054,8 +1055,8 @@ Mission::altitude_sp_foh_update() @@ -1054,8 +1055,8 @@ Mission::altitude_sp_foh_update()
* or if the previous altitude isn't from a position or loiter setpoint
*/
if (!pos_sp_triplet->previous.valid || !pos_sp_triplet->current.valid || !PX4_ISFINITE(pos_sp_triplet->previous.alt)
|| !(pos_sp_triplet->previous.type == position_setpoint_s::SETPOINT_TYPE_POSITION ||
pos_sp_triplet->previous.type == position_setpoint_s::SETPOINT_TYPE_LOITER)) {
|| !(pos_sp_triplet->previous.type == position_setpoint_s::SETPOINT_TYPE_POSITION ||
pos_sp_triplet->previous.type == position_setpoint_s::SETPOINT_TYPE_LOITER)) {
return;
}
@ -1234,7 +1235,7 @@ Mission::read_mission_item(bool onboard, int offset, struct mission_item_s *miss @@ -1234,7 +1235,7 @@ Mission::read_mission_item(bool onboard, int offset, struct mission_item_s *miss
/* mission item index out of bounds - if they are equal, we just reached the end */
if (*mission_index_ptr != (int)mission->count) {
mavlink_log_critical(_navigator->get_mavlink_log_pub(), "[wpm] err: index: %d, max: %d", *mission_index_ptr,
(int)mission->count);
(int)mission->count);
}
return false;

2
src/modules/navigator/mission.h
Unescape View File

@ -181,7 +181,7 @@ private: @@ -181,7 +181,7 @@ private:
* @return true if current mission item available
*/
bool prepare_mission_items(bool onboard, struct mission_item_s *mission_item,
struct mission_item_s *next_position_mission_item, bool *has_next_position_item);
struct mission_item_s *next_position_mission_item, bool *has_next_position_item);
/**
* Read current (offset == 0) or a specific (offset > 0) mission item

262
src/modules/navigator/mission_block.cpp
Unescape View File

@ -66,22 +66,22 @@ orb_advert_t actuator_pub_fd; @@ -66,22 +66,22 @@ orb_advert_t actuator_pub_fd;
MissionBlock::MissionBlock(Navigator *navigator, const char *name) :
NavigatorMode(navigator, name),
_mission_item({0}),
_waypoint_position_reached(false),
_waypoint_yaw_reached(false),
_time_first_inside_orbit(0),
_action_start(0),
_time_wp_reached(0),
_actuators{},
_actuator_pub(nullptr),
_cmd_pub(nullptr),
_param_loiter_min_alt(this, "MIS_LTRMIN_ALT", false),
_param_yaw_timeout(this, "MIS_YAW_TMT", false),
_param_yaw_err(this, "MIS_YAW_ERR", false),
_param_vtol_wv_land(this, "VT_WV_LND_EN", false),
_param_vtol_wv_takeoff(this, "VT_WV_TKO_EN", false),
_param_vtol_wv_loiter(this, "VT_WV_LTR_EN", false),
_param_force_vtol(this, "NAV_FORCE_VT", false)
_mission_item( {0}),
_waypoint_position_reached(false),
_waypoint_yaw_reached(false),
_time_first_inside_orbit(0),
_action_start(0),
_time_wp_reached(0),
_actuators{},
_actuator_pub(nullptr),
_cmd_pub(nullptr),
_param_loiter_min_alt(this, "MIS_LTRMIN_ALT", false),
_param_yaw_timeout(this, "MIS_YAW_TMT", false),
_param_yaw_err(this, "MIS_YAW_ERR", false),
_param_vtol_wv_land(this, "VT_WV_LND_EN", false),
_param_vtol_wv_takeoff(this, "VT_WV_TKO_EN", false),
_param_vtol_wv_loiter(this, "VT_WV_LTR_EN", false),
_param_force_vtol(this, "NAV_FORCE_VT", false)
{
}
@ -94,80 +94,83 @@ MissionBlock::is_mission_item_reached() @@ -94,80 +94,83 @@ MissionBlock::is_mission_item_reached()
{
/* handle non-navigation or indefinite waypoints */
switch (_mission_item.nav_cmd) {
case NAV_CMD_DO_SET_SERVO:
return true;
case NAV_CMD_DO_SET_SERVO:
return true;
case NAV_CMD_LAND: /* fall through */
case NAV_CMD_VTOL_LAND:
return _navigator->get_land_detected()->landed;
case NAV_CMD_LAND: /* fall through */
case NAV_CMD_VTOL_LAND:
return _navigator->get_land_detected()->landed;
case NAV_CMD_IDLE: /* fall through */
case NAV_CMD_LOITER_UNLIMITED:
return false;
case NAV_CMD_IDLE: /* fall through */
case NAV_CMD_LOITER_UNLIMITED:
return false;
case NAV_CMD_DO_LAND_START:
case NAV_CMD_DO_DIGICAM_CONTROL:
case NAV_CMD_IMAGE_START_CAPTURE:
case NAV_CMD_IMAGE_STOP_CAPTURE:
case NAV_CMD_VIDEO_START_CAPTURE:
case NAV_CMD_VIDEO_STOP_CAPTURE:
case NAV_CMD_DO_MOUNT_CONFIGURE:
case NAV_CMD_DO_MOUNT_CONTROL:
case NAV_CMD_DO_SET_ROI:
case NAV_CMD_ROI:
case NAV_CMD_DO_SET_CAM_TRIGG_DIST:
case NAV_CMD_DO_LAND_START:
case NAV_CMD_DO_DIGICAM_CONTROL:
case NAV_CMD_IMAGE_START_CAPTURE:
case NAV_CMD_IMAGE_STOP_CAPTURE:
case NAV_CMD_VIDEO_START_CAPTURE:
case NAV_CMD_VIDEO_STOP_CAPTURE:
case NAV_CMD_DO_MOUNT_CONFIGURE:
case NAV_CMD_DO_MOUNT_CONTROL:
case NAV_CMD_DO_SET_ROI:
case NAV_CMD_ROI:
case NAV_CMD_DO_SET_CAM_TRIGG_DIST:
return true;
case NAV_CMD_DO_VTOL_TRANSITION:
/*
* We wait half a second to give the transition command time to propagate.
* Then monitor the transition status for completion.
*/
if (hrt_absolute_time() - _action_start > 500000 &&
!_navigator->get_vstatus()->in_transition_mode) {
_action_start = 0;
return true;
case NAV_CMD_DO_VTOL_TRANSITION:
/*
* We wait half a second to give the transition command time to propagate.
* Then monitor the transition status for completion.
*/
if (hrt_absolute_time() - _action_start > 500000 &&
!_navigator->get_vstatus()->in_transition_mode) {
_action_start = 0;
return true;
} else {
return false;
}
} else {
return false;
}
case NAV_CMD_DO_CHANGE_SPEED:
return true;
case NAV_CMD_DO_CHANGE_SPEED:
return true;
default:
/* do nothing, this is a 3D waypoint */
break;
default:
/* do nothing, this is a 3D waypoint */
break;
}
hrt_abstime now = hrt_absolute_time();
if ((_navigator->get_land_detected()->landed == false)
&& !_waypoint_position_reached) {
&& !_waypoint_position_reached) {
float dist = -1.0f;
float dist_xy = -1.0f;
float dist_z = -1.0f;
float altitude_amsl = _mission_item.altitude_is_relative
? _mission_item.altitude + _navigator->get_home_position()->alt
: _mission_item.altitude;
? _mission_item.altitude + _navigator->get_home_position()->alt
: _mission_item.altitude;
dist = get_distance_to_point_global_wgs84(_mission_item.lat, _mission_item.lon, altitude_amsl,
_navigator->get_global_position()->lat,
_navigator->get_global_position()->lon,
_navigator->get_global_position()->alt,
&dist_xy, &dist_z);
_navigator->get_global_position()->lat,
_navigator->get_global_position()->lon,
_navigator->get_global_position()->alt,
&dist_xy, &dist_z);
/* FW special case for NAV_CMD_WAYPOINT to achieve altitude via loiter */
if (!_navigator->get_vstatus()->is_rotary_wing &&
(_mission_item.nav_cmd == NAV_CMD_WAYPOINT)) {
(_mission_item.nav_cmd == NAV_CMD_WAYPOINT)) {
struct position_setpoint_s *curr_sp = &_navigator->get_position_setpoint_triplet()->current;
/* close to waypoint, but altitude error greater than twice acceptance */
if ((dist >= 0.0f)
&& (dist_z > 2 * _navigator->get_altitude_acceptance_radius())
&& (dist_xy < 2 * _navigator->get_loiter_radius())) {
&& (dist_z > 2 * _navigator->get_altitude_acceptance_radius())
&& (dist_xy < 2 * _navigator->get_loiter_radius())) {
/* SETPOINT_TYPE_POSITION -> SETPOINT_TYPE_LOITER */
if (curr_sp->type == position_setpoint_s::SETPOINT_TYPE_POSITION) {
@ -176,13 +179,14 @@ MissionBlock::is_mission_item_reached() @@ -176,13 +179,14 @@ MissionBlock::is_mission_item_reached()
curr_sp->loiter_direction = 1;
_navigator->set_position_setpoint_triplet_updated();
}
} else {
/* restore SETPOINT_TYPE_POSITION */
if (curr_sp->type == position_setpoint_s::SETPOINT_TYPE_LOITER) {
/* loiter acceptance criteria required to revert back to SETPOINT_TYPE_POSITION */
if ((dist >= 0.0f)
&& (dist_z < _navigator->get_loiter_radius())
&& (dist_xy <= _navigator->get_loiter_radius() * 1.2f)) {
&& (dist_z < _navigator->get_loiter_radius())
&& (dist_xy <= _navigator->get_loiter_radius() * 1.2f)) {
curr_sp->type = position_setpoint_s::SETPOINT_TYPE_POSITION;
_navigator->set_position_setpoint_triplet_updated();
@ -192,7 +196,7 @@ MissionBlock::is_mission_item_reached() @@ -192,7 +196,7 @@ MissionBlock::is_mission_item_reached()
}
if ((_mission_item.nav_cmd == NAV_CMD_TAKEOFF || _mission_item.nav_cmd == NAV_CMD_VTOL_TAKEOFF)
&& _navigator->get_vstatus()->is_rotary_wing) {
&& _navigator->get_vstatus()->is_rotary_wing) {
/* We want to avoid the edge case where the acceptance radius is bigger or equal than
* the altitude of the takeoff waypoint above home. Otherwise, we do not really follow
@ -211,18 +215,20 @@ MissionBlock::is_mission_item_reached() @@ -211,18 +215,20 @@ MissionBlock::is_mission_item_reached()
/* require only altitude for takeoff for multicopter */
if (_navigator->get_global_position()->alt >
altitude_amsl - altitude_acceptance_radius) {
altitude_amsl - altitude_acceptance_radius) {
_waypoint_position_reached = true;
}
} else if (_mission_item.nav_cmd == NAV_CMD_TAKEOFF) {
/* for takeoff mission items use the parameter for the takeoff acceptance radius */
if (dist >= 0.0f && dist <= _navigator->get_acceptance_radius()
&& dist_z <= _navigator->get_altitude_acceptance_radius()) {
&& dist_z <= _navigator->get_altitude_acceptance_radius()) {
_waypoint_position_reached = true;
}
} else if (!_navigator->get_vstatus()->is_rotary_wing &&
(_mission_item.nav_cmd == NAV_CMD_LOITER_UNLIMITED ||
_mission_item.nav_cmd == NAV_CMD_LOITER_TIME_LIMIT)) {
(_mission_item.nav_cmd == NAV_CMD_LOITER_UNLIMITED ||
_mission_item.nav_cmd == NAV_CMD_LOITER_TIME_LIMIT)) {
/* Loiter mission item on a non rotary wing: the aircraft is going to circle the
* coordinates with a radius equal to the loiter_radius field. It is not flying
* through the waypoint center.
@ -230,15 +236,16 @@ MissionBlock::is_mission_item_reached() @@ -230,15 +236,16 @@ MissionBlock::is_mission_item_reached()
* radius (+ some margin). Time inside and turn count is handled elsewhere.
*/
if (dist >= 0.0f && dist <= _navigator->get_acceptance_radius(fabsf(_mission_item.loiter_radius) * 1.2f)
&& dist_z <= _navigator->get_altitude_acceptance_radius()) {
&& dist_z <= _navigator->get_altitude_acceptance_radius()) {
_waypoint_position_reached = true;
} else {
_time_first_inside_orbit = 0;
}
} else if (!_navigator->get_vstatus()->is_rotary_wing &&
(_mission_item.nav_cmd == NAV_CMD_LOITER_TO_ALT)) {
(_mission_item.nav_cmd == NAV_CMD_LOITER_TO_ALT)) {
// NAV_CMD_LOITER_TO_ALT only uses mission item altitude once it's in the loiter
@ -252,13 +259,13 @@ MissionBlock::is_mission_item_reached() @@ -252,13 +259,13 @@ MissionBlock::is_mission_item_reached()
dist_z = -1.0f;
dist = get_distance_to_point_global_wgs84(_mission_item.lat, _mission_item.lon, curr_sp->alt,
_navigator->get_global_position()->lat,
_navigator->get_global_position()->lon,
_navigator->get_global_position()->alt,
&dist_xy, &dist_z);
_navigator->get_global_position()->lat,
_navigator->get_global_position()->lon,
_navigator->get_global_position()->alt,
&dist_xy, &dist_z);
if (dist >= 0.0f && dist <= _navigator->get_acceptance_radius(fabsf(_mission_item.loiter_radius) * 1.2f)
&& dist_z <= _navigator->get_altitude_acceptance_radius()) {
&& dist_z <= _navigator->get_altitude_acceptance_radius()) {
// now set the loiter to the final altitude in the NAV_CMD_LOITER_TO_ALT mission item
curr_sp->alt = altitude_amsl;
@ -267,7 +274,7 @@ MissionBlock::is_mission_item_reached() @@ -267,7 +274,7 @@ MissionBlock::is_mission_item_reached()
} else {
if (dist >= 0.0f && dist <= _navigator->get_acceptance_radius(fabsf(_mission_item.loiter_radius) * 1.2f)
&& dist_z <= _navigator->get_altitude_acceptance_radius()) {
&& dist_z <= _navigator->get_altitude_acceptance_radius()) {
_waypoint_position_reached = true;
@ -277,16 +284,18 @@ MissionBlock::is_mission_item_reached() @@ -277,16 +284,18 @@ MissionBlock::is_mission_item_reached()
if (next_sp.valid) {
_mission_item.yaw = get_bearing_to_next_waypoint(_navigator->get_global_position()->lat,
_navigator->get_global_position()->lon,
next_sp.lat, next_sp.lon);
_navigator->get_global_position()->lon,
next_sp.lat, next_sp.lon);
_waypoint_yaw_reached = false;
} else {
_waypoint_yaw_reached = true;
}
}
}
}
} else {
/* for normal mission items used their acceptance radius */
float mission_acceptance_radius = _navigator->get_acceptance_radius(_mission_item.acceptance_radius);
@ -297,7 +306,7 @@ MissionBlock::is_mission_item_reached() @@ -297,7 +306,7 @@ MissionBlock::is_mission_item_reached()
}
if (dist >= 0.0f && dist <= mission_acceptance_radius
&& dist_z <= _navigator->get_altitude_acceptance_radius()) {
&& dist_z <= _navigator->get_altitude_acceptance_radius()) {
_waypoint_position_reached = true;
}
}
@ -313,23 +322,23 @@ MissionBlock::is_mission_item_reached() @@ -313,23 +322,23 @@ MissionBlock::is_mission_item_reached()
if (_waypoint_position_reached && !_waypoint_yaw_reached) {
if ((_navigator->get_vstatus()->is_rotary_wing
|| (_mission_item.nav_cmd == NAV_CMD_LOITER_TO_ALT && _mission_item.force_heading))
&& PX4_ISFINITE(_mission_item.yaw)) {
|| (_mission_item.nav_cmd == NAV_CMD_LOITER_TO_ALT && _mission_item.force_heading))
&& PX4_ISFINITE(_mission_item.yaw)) {
/* check yaw if defined only for rotary wing except takeoff */
float yaw_err = _wrap_pi(_mission_item.yaw - _navigator->get_global_position()->yaw);
/* accept yaw if reached or if timeout is set in which case we ignore not forced headings */
if (fabsf(yaw_err) < math::radians(_param_yaw_err.get())
|| (_param_yaw_timeout.get() >= FLT_EPSILON && !_mission_item.force_heading)) {
|| (_param_yaw_timeout.get() >= FLT_EPSILON && !_mission_item.force_heading)) {
_waypoint_yaw_reached = true;
}
/* if heading needs to be reached, the timeout is enabled and we don't make it, abort mission */
if (!_waypoint_yaw_reached && _mission_item.force_heading &&
(_param_yaw_timeout.get() >= FLT_EPSILON) &&
(now - _time_wp_reached >= (hrt_abstime)_param_yaw_timeout.get() * 1e6f)) {
(_param_yaw_timeout.get() >= FLT_EPSILON) &&
(now - _time_wp_reached >= (hrt_abstime)_param_yaw_timeout.get() * 1e6f)) {
_navigator->set_mission_failure("unable to reach heading within timeout");
}
@ -348,12 +357,12 @@ MissionBlock::is_mission_item_reached() @@ -348,12 +357,12 @@ MissionBlock::is_mission_item_reached()
/* check if the MAV was long enough inside the waypoint orbit */
if ((Navigator::get_time_inside(_mission_item) < FLT_EPSILON) ||
(now - _time_first_inside_orbit >= (hrt_abstime)(Navigator::get_time_inside(_mission_item) * 1e6f))) {
(now - _time_first_inside_orbit >= (hrt_abstime)(Navigator::get_time_inside(_mission_item) * 1e6f))) {
// exit xtrack location
if (_mission_item.loiter_exit_xtrack &&
(_mission_item.nav_cmd == NAV_CMD_LOITER_TIME_LIMIT ||
_mission_item.nav_cmd == NAV_CMD_LOITER_TO_ALT)) {
(_mission_item.nav_cmd == NAV_CMD_LOITER_TIME_LIMIT ||
_mission_item.nav_cmd == NAV_CMD_LOITER_TO_ALT)) {
// reset lat/lon of loiter waypoint so vehicle exits on a tangent
struct position_setpoint_s *curr_sp = &_navigator->get_position_setpoint_triplet()->current;
@ -399,15 +408,16 @@ MissionBlock::mission_item_to_vehicle_command(const struct mission_item_s *item, @@ -399,15 +408,16 @@ MissionBlock::mission_item_to_vehicle_command(const struct mission_item_s *item,
// The camera commands are not processed on the autopilot but will be
// sent to the mavlink links to other components.
switch (item->nav_cmd) {
case NAV_CMD_IMAGE_START_CAPTURE:
case NAV_CMD_IMAGE_STOP_CAPTURE:
case NAV_CMD_VIDEO_START_CAPTURE:
case NAV_CMD_VIDEO_STOP_CAPTURE:
cmd->target_component = 100; // MAV_COMP_ID_CAMERA
break;
default:
cmd->target_component = _navigator->get_vstatus()->component_id;
break;
case NAV_CMD_IMAGE_START_CAPTURE:
case NAV_CMD_IMAGE_STOP_CAPTURE:
case NAV_CMD_VIDEO_START_CAPTURE:
case NAV_CMD_VIDEO_STOP_CAPTURE:
cmd->target_component = 100; // MAV_COMP_ID_CAMERA
break;
default:
cmd->target_component = _navigator->get_vstatus()->component_id;
break;
}
cmd->source_system = _navigator->get_vstatus()->system_id;
@ -463,20 +473,20 @@ MissionBlock::item_contains_position(const struct mission_item_s *item) @@ -463,20 +473,20 @@ MissionBlock::item_contains_position(const struct mission_item_s *item)
{
// XXX: maybe extend that check onto item properties
if (item->nav_cmd == NAV_CMD_DO_JUMP ||
item->nav_cmd == NAV_CMD_DO_CHANGE_SPEED ||
item->nav_cmd == NAV_CMD_DO_SET_SERVO ||
item->nav_cmd == NAV_CMD_DO_LAND_START ||
item->nav_cmd == NAV_CMD_DO_DIGICAM_CONTROL ||
item->nav_cmd == NAV_CMD_IMAGE_START_CAPTURE ||
item->nav_cmd == NAV_CMD_IMAGE_STOP_CAPTURE ||
item->nav_cmd == NAV_CMD_VIDEO_START_CAPTURE ||
item->nav_cmd == NAV_CMD_VIDEO_STOP_CAPTURE ||
item->nav_cmd == NAV_CMD_DO_MOUNT_CONFIGURE ||
item->nav_cmd == NAV_CMD_DO_MOUNT_CONTROL ||
item->nav_cmd == NAV_CMD_DO_SET_ROI ||
item->nav_cmd == NAV_CMD_ROI ||
item->nav_cmd == NAV_CMD_DO_SET_CAM_TRIGG_DIST ||
item->nav_cmd == NAV_CMD_DO_VTOL_TRANSITION) {
item->nav_cmd == NAV_CMD_DO_CHANGE_SPEED ||
item->nav_cmd == NAV_CMD_DO_SET_SERVO ||
item->nav_cmd == NAV_CMD_DO_LAND_START ||
item->nav_cmd == NAV_CMD_DO_DIGICAM_CONTROL ||
item->nav_cmd == NAV_CMD_IMAGE_START_CAPTURE ||
item->nav_cmd == NAV_CMD_IMAGE_STOP_CAPTURE ||
item->nav_cmd == NAV_CMD_VIDEO_START_CAPTURE ||
item->nav_cmd == NAV_CMD_VIDEO_STOP_CAPTURE ||
item->nav_cmd == NAV_CMD_DO_MOUNT_CONFIGURE ||
item->nav_cmd == NAV_CMD_DO_MOUNT_CONTROL ||
item->nav_cmd == NAV_CMD_DO_SET_ROI ||
item->nav_cmd == NAV_CMD_ROI ||
item->nav_cmd == NAV_CMD_DO_SET_CAM_TRIGG_DIST ||
item->nav_cmd == NAV_CMD_DO_VTOL_TRANSITION) {
return false;
}
@ -497,7 +507,7 @@ MissionBlock::mission_item_to_position_setpoint(const struct mission_item_s *ite @@ -497,7 +507,7 @@ MissionBlock::mission_item_to_position_setpoint(const struct mission_item_s *ite
sp->alt = item->altitude_is_relative ? item->altitude + _navigator->get_home_position()->alt : item->altitude;
sp->yaw = item->yaw;
sp->loiter_radius = (fabsf(item->loiter_radius) > NAV_EPSILON_POSITION) ? fabsf(item->loiter_radius) :
_navigator->get_loiter_radius();
_navigator->get_loiter_radius();
sp->loiter_direction = (item->loiter_radius > 0) ? 1 : -1;
sp->acceptance_radius = item->acceptance_radius;
sp->disable_mc_yaw_control = item->disable_mc_yaw;
@ -514,33 +524,40 @@ MissionBlock::mission_item_to_position_setpoint(const struct mission_item_s *ite @@ -514,33 +524,40 @@ MissionBlock::mission_item_to_position_setpoint(const struct mission_item_s *ite
// set pitch and ensure that the hold time is zero
sp->pitch_min = item->pitch_min;
// fall through
// fall through
case NAV_CMD_VTOL_TAKEOFF:
sp->type = position_setpoint_s::SETPOINT_TYPE_TAKEOFF;
if(_navigator->get_vstatus()->is_vtol && _param_vtol_wv_takeoff.get()){
if (_navigator->get_vstatus()->is_vtol && _param_vtol_wv_takeoff.get()) {
sp->disable_mc_yaw_control = true;
}
break;
case NAV_CMD_LAND:
case NAV_CMD_VTOL_LAND:
sp->type = position_setpoint_s::SETPOINT_TYPE_LAND;
if (_navigator->get_vstatus()->is_vtol && _param_vtol_wv_land.get()) {
sp->disable_mc_yaw_control = true;
}
break;
case NAV_CMD_LOITER_TO_ALT:
// initially use current altitude, and switch to mission item altitude once in loiter position
sp->alt = math::max(_navigator->get_global_position()->alt, _navigator->get_home_position()->alt + _param_loiter_min_alt.get());
sp->alt = math::max(_navigator->get_global_position()->alt,
_navigator->get_home_position()->alt + _param_loiter_min_alt.get());
// fall through
// fall through
case NAV_CMD_LOITER_TIME_LIMIT:
case NAV_CMD_LOITER_UNLIMITED:
sp->type = position_setpoint_s::SETPOINT_TYPE_LOITER;
if (_navigator->get_vstatus()->is_vtol && _param_vtol_wv_loiter.get()) {
sp->disable_mc_yaw_control = true;
}
break;
default:
@ -601,7 +618,8 @@ MissionBlock::set_loiter_item(struct mission_item_s *item, float min_clearance) @@ -601,7 +618,8 @@ MissionBlock::set_loiter_item(struct mission_item_s *item, float min_clearance)
}
void
MissionBlock::set_follow_target_item(struct mission_item_s *item, float min_clearance, follow_target_s & target, float yaw)
MissionBlock::set_follow_target_item(struct mission_item_s *item, float min_clearance, follow_target_s &target,
float yaw)
{
if (_navigator->get_land_detected()->landed) {
/* landed, don't takeoff, but switch to IDLE mode */
@ -619,6 +637,7 @@ MissionBlock::set_follow_target_item(struct mission_item_s *item, float min_clea @@ -619,6 +637,7 @@ MissionBlock::set_follow_target_item(struct mission_item_s *item, float min_clea
if (min_clearance > 8.0f) {
item->altitude += min_clearance;
} else {
item->altitude += 8.0f; // if min clearance is bad set it to 8.0 meters (well above the average height of a person)
}
@ -658,12 +677,14 @@ MissionBlock::set_land_item(struct mission_item_s *item, bool at_current_locatio @@ -658,12 +677,14 @@ MissionBlock::set_land_item(struct mission_item_s *item, bool at_current_locatio
/* VTOL transition to RW before landing */
if (_navigator->get_vstatus()->is_vtol && !_navigator->get_vstatus()->is_rotary_wing &&
_param_force_vtol.get() == 1) {
_param_force_vtol.get() == 1) {
struct vehicle_command_s cmd = {};
cmd.command = NAV_CMD_DO_VTOL_TRANSITION;
cmd.param1 = vtol_vehicle_status_s::VEHICLE_VTOL_STATE_MC;
if (_cmd_pub != nullptr) {
orb_publish(ORB_ID(vehicle_command), _cmd_pub, &cmd);
} else {
_cmd_pub = orb_advertise_queue(ORB_ID(vehicle_command), &cmd, vehicle_command_s::ORB_QUEUE_LENGTH);
}
@ -678,7 +699,8 @@ MissionBlock::set_land_item(struct mission_item_s *item, bool at_current_locatio @@ -678,7 +699,8 @@ MissionBlock::set_land_item(struct mission_item_s *item, bool at_current_locatio
item->lon = _navigator->get_global_position()->lon;
item->yaw = _navigator->get_global_position()->yaw;
/* use home position */
/* use home position */
} else {
item->lat = _navigator->get_home_position()->lat;
item->lon = _navigator->get_home_position()->lon;

2
src/modules/navigator/mission_block.h
Unescape View File

@ -129,7 +129,7 @@ protected: @@ -129,7 +129,7 @@ protected:
/**
* Set follow_target item
*/
void set_follow_target_item(struct mission_item_s *item, float min_clearance, follow_target_s & target, float yaw);
void set_follow_target_item(struct mission_item_s *item, float min_clearance, follow_target_s &target, float yaw);
void issue_command(const struct mission_item_s *item);

158
src/modules/navigator/mission_feasibility_checker.cpp
Unescape View File

@ -64,10 +64,10 @@ MissionFeasibilityChecker::MissionFeasibilityChecker() : @@ -64,10 +64,10 @@ MissionFeasibilityChecker::MissionFeasibilityChecker() :
bool MissionFeasibilityChecker::checkMissionFeasible(orb_advert_t *mavlink_log_pub, bool isRotarywing,
dm_item_t dm_current, size_t nMissionItems, Geofence &geofence,
float home_alt, bool home_valid, double curr_lat, double curr_lon, float max_waypoint_distance, bool &warning_issued,
float default_acceptance_rad,
bool condition_landed)
dm_item_t dm_current, size_t nMissionItems, Geofence &geofence,
float home_alt, bool home_valid, double curr_lat, double curr_lon, float max_waypoint_distance, bool &warning_issued,
float default_acceptance_rad,
bool condition_landed)
{
bool failed = false;
bool warned = false;
@ -81,8 +81,10 @@ bool MissionFeasibilityChecker::checkMissionFeasible(orb_advert_t *mavlink_log_p @@ -81,8 +81,10 @@ bool MissionFeasibilityChecker::checkMissionFeasible(orb_advert_t *mavlink_log_p
failed = true;
warned = true;
mavlink_log_info(_mavlink_log_pub, "Not yet ready for mission, no position lock.");
} else {
failed = failed || !check_dist_1wp(dm_current, nMissionItems, curr_lat, curr_lon, max_waypoint_distance, warning_issued);
failed = failed
|| !check_dist_1wp(dm_current, nMissionItems, curr_lat, curr_lon, max_waypoint_distance, warning_issued);
}
// check if all mission item commands are supported
@ -91,7 +93,9 @@ bool MissionFeasibilityChecker::checkMissionFeasible(orb_advert_t *mavlink_log_p @@ -91,7 +93,9 @@ bool MissionFeasibilityChecker::checkMissionFeasible(orb_advert_t *mavlink_log_p
failed = failed || !checkHomePositionAltitude(dm_current, nMissionItems, home_alt, home_valid, warned);
if (isRotarywing) {
failed = failed || !checkMissionFeasibleRotarywing(dm_current, nMissionItems, geofence, home_alt, home_valid, default_acceptance_rad);
failed = failed
|| !checkMissionFeasibleRotarywing(dm_current, nMissionItems, geofence, home_alt, home_valid, default_acceptance_rad);
} else {
failed = failed || !checkMissionFeasibleFixedwing(dm_current, nMissionItems, geofence, home_alt, home_valid);
}
@ -100,7 +104,7 @@ bool MissionFeasibilityChecker::checkMissionFeasible(orb_advert_t *mavlink_log_p @@ -100,7 +104,7 @@ bool MissionFeasibilityChecker::checkMissionFeasible(orb_advert_t *mavlink_log_p
}
bool MissionFeasibilityChecker::checkMissionFeasibleRotarywing(dm_item_t dm_current, size_t nMissionItems,
Geofence &geofence, float home_alt, bool home_valid, float default_acceptance_rad)
Geofence &geofence, float home_alt, bool home_valid, float default_acceptance_rad)
{
/* Check if all all waypoints are above the home altitude, only return false if bool throw_error = true */
for (size_t i = 0; i < nMissionItems; i++) {
@ -117,8 +121,8 @@ bool MissionFeasibilityChecker::checkMissionFeasibleRotarywing(dm_item_t dm_curr @@ -117,8 +121,8 @@ bool MissionFeasibilityChecker::checkMissionFeasibleRotarywing(dm_item_t dm_curr
// make sure that the altitude of the waypoint is at least one meter larger than the acceptance radius
// this makes sure that the takeoff waypoint is not reached before we are at least one meter in the air
float takeoff_alt = missionitem.altitude_is_relative
? missionitem.altitude
: missionitem.altitude - home_alt;
? missionitem.altitude
: missionitem.altitude - home_alt;
// check if we should use default acceptance radius
float acceptance_radius = default_acceptance_rad;
@ -137,7 +141,8 @@ bool MissionFeasibilityChecker::checkMissionFeasibleRotarywing(dm_item_t dm_curr @@ -137,7 +141,8 @@ bool MissionFeasibilityChecker::checkMissionFeasibleRotarywing(dm_item_t dm_curr
return true;
}
bool MissionFeasibilityChecker::checkMissionFeasibleFixedwing(dm_item_t dm_current, size_t nMissionItems, Geofence &geofence, float home_alt, bool home_valid)
bool MissionFeasibilityChecker::checkMissionFeasibleFixedwing(dm_item_t dm_current, size_t nMissionItems,
Geofence &geofence, float home_alt, bool home_valid)
{
/* Update fixed wing navigation capabilites */
updateNavigationCapabilities();
@ -149,7 +154,8 @@ bool MissionFeasibilityChecker::checkMissionFeasibleFixedwing(dm_item_t dm_curre @@ -149,7 +154,8 @@ bool MissionFeasibilityChecker::checkMissionFeasibleFixedwing(dm_item_t dm_curre
return resLanding;
}
bool MissionFeasibilityChecker::checkGeofence(dm_item_t dm_current, size_t nMissionItems, Geofence &geofence, float home_alt)
bool MissionFeasibilityChecker::checkGeofence(dm_item_t dm_current, size_t nMissionItems, Geofence &geofence,
float home_alt)
{
/* Check if all mission items are inside the geofence (if we have a valid geofence) */
if (geofence.valid()) {
@ -164,11 +170,11 @@ bool MissionFeasibilityChecker::checkGeofence(dm_item_t dm_current, size_t nMiss @@ -164,11 +170,11 @@ bool MissionFeasibilityChecker::checkGeofence(dm_item_t dm_current, size_t nMiss
// Geofence function checks against home altitude amsl
missionitem.altitude = missionitem.altitude_is_relative
? missionitem.altitude + home_alt
: missionitem.altitude;
? missionitem.altitude + home_alt
: missionitem.altitude;
if (MissionBlock::item_contains_position(&missionitem) &&
!geofence.inside(missionitem)) {
!geofence.inside(missionitem)) {
mavlink_log_critical(_mavlink_log_pub, "Geofence violation for waypoint %d", i + 1);
return false;
@ -180,7 +186,7 @@ bool MissionFeasibilityChecker::checkGeofence(dm_item_t dm_current, size_t nMiss @@ -180,7 +186,7 @@ bool MissionFeasibilityChecker::checkGeofence(dm_item_t dm_current, size_t nMiss
}
bool MissionFeasibilityChecker::checkHomePositionAltitude(dm_item_t dm_current, size_t nMissionItems,
float home_alt, bool home_valid, bool &warning_issued, bool throw_error)
float home_alt, bool home_valid, bool &warning_issued, bool throw_error)
{
/* Check if all waypoints are above the home altitude, only return false if bool throw_error = true */
for (size_t i = 0; i < nMissionItems; i++) {
@ -199,10 +205,11 @@ bool MissionFeasibilityChecker::checkHomePositionAltitude(dm_item_t dm_current, @@ -199,10 +205,11 @@ bool MissionFeasibilityChecker::checkHomePositionAltitude(dm_item_t dm_current,
warning_issued = true;
if (throw_error) {
mavlink_log_critical(_mavlink_log_pub, "Rejecting mission: No home pos, WP %d uses rel alt", i+1);
mavlink_log_critical(_mavlink_log_pub, "Rejecting mission: No home pos, WP %d uses rel alt", i + 1);
return false;
} else {
mavlink_log_critical(_mavlink_log_pub, "Warning: No home pos, WP %d uses rel alt", i+1);
mavlink_log_critical(_mavlink_log_pub, "Warning: No home pos, WP %d uses rel alt", i + 1);
return true;
}
}
@ -215,10 +222,11 @@ bool MissionFeasibilityChecker::checkHomePositionAltitude(dm_item_t dm_current, @@ -215,10 +222,11 @@ bool MissionFeasibilityChecker::checkHomePositionAltitude(dm_item_t dm_current,
warning_issued = true;
if (throw_error) {
mavlink_log_critical(_mavlink_log_pub, "Rejecting mission: Waypoint %d below home", i+1);
mavlink_log_critical(_mavlink_log_pub, "Rejecting mission: Waypoint %d below home", i + 1);
return false;
} else {
mavlink_log_critical(_mavlink_log_pub, "Warning: Waypoint %d below home", i+1);
mavlink_log_critical(_mavlink_log_pub, "Warning: Waypoint %d below home", i + 1);
return true;
}
}
@ -227,7 +235,9 @@ bool MissionFeasibilityChecker::checkHomePositionAltitude(dm_item_t dm_current, @@ -227,7 +235,9 @@ bool MissionFeasibilityChecker::checkHomePositionAltitude(dm_item_t dm_current,
return true;
}
bool MissionFeasibilityChecker::checkMissionItemValidity(dm_item_t dm_current, size_t nMissionItems, bool condition_landed) {
bool MissionFeasibilityChecker::checkMissionItemValidity(dm_item_t dm_current, size_t nMissionItems,
bool condition_landed)
{
// do not allow mission if we find unsupported item
for (size_t i = 0; i < nMissionItems; i++) {
struct mission_item_s missionitem;
@ -241,38 +251,39 @@ bool MissionFeasibilityChecker::checkMissionItemValidity(dm_item_t dm_current, s @@ -241,38 +251,39 @@ bool MissionFeasibilityChecker::checkMissionItemValidity(dm_item_t dm_current, s
// check if we find unsupported items and reject mission if so
if (missionitem.nav_cmd != NAV_CMD_IDLE &&
missionitem.nav_cmd != NAV_CMD_WAYPOINT &&
missionitem.nav_cmd != NAV_CMD_LOITER_UNLIMITED &&
missionitem.nav_cmd != NAV_CMD_LOITER_TIME_LIMIT &&
missionitem.nav_cmd != NAV_CMD_LAND &&
missionitem.nav_cmd != NAV_CMD_TAKEOFF &&
missionitem.nav_cmd != NAV_CMD_LOITER_TO_ALT &&
missionitem.nav_cmd != NAV_CMD_VTOL_TAKEOFF &&
missionitem.nav_cmd != NAV_CMD_VTOL_LAND &&
missionitem.nav_cmd != NAV_CMD_DO_JUMP &&
missionitem.nav_cmd != NAV_CMD_DO_CHANGE_SPEED &&
missionitem.nav_cmd != NAV_CMD_DO_SET_SERVO &&
missionitem.nav_cmd != NAV_CMD_DO_LAND_START &&
missionitem.nav_cmd != NAV_CMD_DO_DIGICAM_CONTROL &&
missionitem.nav_cmd != NAV_CMD_IMAGE_START_CAPTURE &&
missionitem.nav_cmd != NAV_CMD_IMAGE_STOP_CAPTURE &&
missionitem.nav_cmd != NAV_CMD_VIDEO_START_CAPTURE &&
missionitem.nav_cmd != NAV_CMD_VIDEO_STOP_CAPTURE &&
missionitem.nav_cmd != NAV_CMD_DO_MOUNT_CONFIGURE &&
missionitem.nav_cmd != NAV_CMD_DO_MOUNT_CONTROL &&
missionitem.nav_cmd != NAV_CMD_DO_SET_ROI &&
missionitem.nav_cmd != NAV_CMD_ROI &&
missionitem.nav_cmd != NAV_CMD_DO_SET_CAM_TRIGG_DIST &&
missionitem.nav_cmd != NAV_CMD_DO_VTOL_TRANSITION) {
mavlink_log_critical(_mavlink_log_pub, "Rejecting mission item %i: unsupported cmd: %d", (int)(i+1), (int)missionitem.nav_cmd);
missionitem.nav_cmd != NAV_CMD_WAYPOINT &&
missionitem.nav_cmd != NAV_CMD_LOITER_UNLIMITED &&
missionitem.nav_cmd != NAV_CMD_LOITER_TIME_LIMIT &&
missionitem.nav_cmd != NAV_CMD_LAND &&
missionitem.nav_cmd != NAV_CMD_TAKEOFF &&
missionitem.nav_cmd != NAV_CMD_LOITER_TO_ALT &&
missionitem.nav_cmd != NAV_CMD_VTOL_TAKEOFF &&
missionitem.nav_cmd != NAV_CMD_VTOL_LAND &&
missionitem.nav_cmd != NAV_CMD_DO_JUMP &&
missionitem.nav_cmd != NAV_CMD_DO_CHANGE_SPEED &&
missionitem.nav_cmd != NAV_CMD_DO_SET_SERVO &&
missionitem.nav_cmd != NAV_CMD_DO_LAND_START &&
missionitem.nav_cmd != NAV_CMD_DO_DIGICAM_CONTROL &&
missionitem.nav_cmd != NAV_CMD_IMAGE_START_CAPTURE &&
missionitem.nav_cmd != NAV_CMD_IMAGE_STOP_CAPTURE &&
missionitem.nav_cmd != NAV_CMD_VIDEO_START_CAPTURE &&
missionitem.nav_cmd != NAV_CMD_VIDEO_STOP_CAPTURE &&
missionitem.nav_cmd != NAV_CMD_DO_MOUNT_CONFIGURE &&
missionitem.nav_cmd != NAV_CMD_DO_MOUNT_CONTROL &&
missionitem.nav_cmd != NAV_CMD_DO_SET_ROI &&
missionitem.nav_cmd != NAV_CMD_ROI &&
missionitem.nav_cmd != NAV_CMD_DO_SET_CAM_TRIGG_DIST &&
missionitem.nav_cmd != NAV_CMD_DO_VTOL_TRANSITION) {
mavlink_log_critical(_mavlink_log_pub, "Rejecting mission item %i: unsupported cmd: %d", (int)(i + 1),
(int)missionitem.nav_cmd);
return false;
}
// check if the mission starts with a land command while the vehicle is landed
if (missionitem.nav_cmd == NAV_CMD_LAND &&
i == 0 &&
condition_landed) {
i == 0 &&
condition_landed) {
mavlink_log_critical(_mavlink_log_pub, "Rejecting mission that starts with LAND command while vehicle is landed.");
return false;
@ -280,6 +291,7 @@ bool MissionFeasibilityChecker::checkMissionItemValidity(dm_item_t dm_current, s @@ -280,6 +291,7 @@ bool MissionFeasibilityChecker::checkMissionItemValidity(dm_item_t dm_current, s
}
return true;
}
@ -291,6 +303,7 @@ bool MissionFeasibilityChecker::checkFixedWingLanding(dm_item_t dm_current, size @@ -291,6 +303,7 @@ bool MissionFeasibilityChecker::checkFixedWingLanding(dm_item_t dm_current, size
for (size_t i = 0; i < nMissionItems; i++) {
struct mission_item_s missionitem;
const ssize_t len = sizeof(missionitem);
if (dm_read(dm_current, i, &missionitem, len) != len) {
/* not supposed to happen unless the datamanager can't access the SD card, etc. */
return false;
@ -298,15 +311,19 @@ bool MissionFeasibilityChecker::checkFixedWingLanding(dm_item_t dm_current, size @@ -298,15 +311,19 @@ bool MissionFeasibilityChecker::checkFixedWingLanding(dm_item_t dm_current, size
if (missionitem.nav_cmd == NAV_CMD_LAND) {
struct mission_item_s missionitem_previous;
if (i != 0) {
if (dm_read(dm_current, i-1, &missionitem_previous, len) != len) {
if (dm_read(dm_current, i - 1, &missionitem_previous, len) != len) {
/* not supposed to happen unless the datamanager can't access the SD card, etc. */
return false;
}
float wp_distance = get_distance_to_next_waypoint(missionitem_previous.lat , missionitem_previous.lon, missionitem.lat, missionitem.lon);
float slope_alt_req = Landingslope::getLandingSlopeAbsoluteAltitude(wp_distance, missionitem.altitude, _fw_pos_ctrl_status.landing_horizontal_slope_displacement, _fw_pos_ctrl_status.landing_slope_angle_rad);
float wp_distance_req = Landingslope::getLandingSlopeWPDistance(missionitem_previous.altitude, missionitem.altitude, _fw_pos_ctrl_status.landing_horizontal_slope_displacement, _fw_pos_ctrl_status.landing_slope_angle_rad);
float wp_distance = get_distance_to_next_waypoint(missionitem_previous.lat , missionitem_previous.lon, missionitem.lat,
missionitem.lon);
float slope_alt_req = Landingslope::getLandingSlopeAbsoluteAltitude(wp_distance, missionitem.altitude,
_fw_pos_ctrl_status.landing_horizontal_slope_displacement, _fw_pos_ctrl_status.landing_slope_angle_rad);
float wp_distance_req = Landingslope::getLandingSlopeWPDistance(missionitem_previous.altitude, missionitem.altitude,
_fw_pos_ctrl_status.landing_horizontal_slope_displacement, _fw_pos_ctrl_status.landing_slope_angle_rad);
float delta_altitude = missionitem.altitude - missionitem_previous.altitude;
// warnx("wp_distance %.2f, delta_altitude %.2f, missionitem_previous.altitude %.2f, missionitem.altitude %.2f, slope_alt_req %.2f, wp_distance_req %.2f",
// wp_distance, delta_altitude, missionitem_previous.altitude, missionitem.altitude, slope_alt_req, wp_distance_req);
@ -320,25 +337,29 @@ bool MissionFeasibilityChecker::checkFixedWingLanding(dm_item_t dm_current, size @@ -320,25 +337,29 @@ bool MissionFeasibilityChecker::checkFixedWingLanding(dm_item_t dm_current, size
if (missionitem_previous.altitude <= slope_alt_req) {
/* Landing waypoint is at or below altitude of slope at the given waypoint distance: this is ok, aircraft will intersect the slope */
return true;
} else {
/* Landing waypoint is above altitude of slope at the given waypoint distance */
mavlink_log_critical(_mavlink_log_pub, "Landing: last waypoint too high/too close");
mavlink_log_critical(_mavlink_log_pub, "Move down to %.1fm or move further away by %.1fm",
(double)(slope_alt_req),
(double)(wp_distance_req - wp_distance));
(double)(slope_alt_req),
(double)(wp_distance_req - wp_distance));
return false;
}
} else {
/* Landing waypoint is above last waypoint */
mavlink_log_critical(_mavlink_log_pub, "Landing waypoint above last nav waypoint");
return false;
}
} else {
/* Last wp is in flare region */
//xxx give recommendations
mavlink_log_critical(_mavlink_log_pub, "Last waypoint too close to landing waypoint");
return false;
}
} else {
mavlink_log_critical(_mavlink_log_pub, "Invalid mission: starts with land waypoint");
return false;
@ -351,7 +372,8 @@ bool MissionFeasibilityChecker::checkFixedWingLanding(dm_item_t dm_current, size @@ -351,7 +372,8 @@ bool MissionFeasibilityChecker::checkFixedWingLanding(dm_item_t dm_current, size
}
bool
MissionFeasibilityChecker::check_dist_1wp(dm_item_t dm_current, size_t nMissionItems, double curr_lat, double curr_lon, float dist_first_wp, bool &warning_issued)
MissionFeasibilityChecker::check_dist_1wp(dm_item_t dm_current, size_t nMissionItems, double curr_lat, double curr_lon,
float dist_first_wp, bool &warning_issued)
{
/* check if first waypoint is not too far from home */
@ -361,9 +383,9 @@ MissionFeasibilityChecker::check_dist_1wp(dm_item_t dm_current, size_t nMissionI @@ -361,9 +383,9 @@ MissionFeasibilityChecker::check_dist_1wp(dm_item_t dm_current, size_t nMissionI
/* find first waypoint (with lat/lon) item in datamanager */
for (unsigned i = 0; i < nMissionItems; i++) {
if (dm_read(dm_current, i,
&mission_item, sizeof(mission_item_s)) == sizeof(mission_item_s)) {
&mission_item, sizeof(mission_item_s)) == sizeof(mission_item_s)) {
/* Check non navigation item */
if (mission_item.nav_cmd == NAV_CMD_DO_SET_SERVO){
if (mission_item.nav_cmd == NAV_CMD_DO_SET_SERVO) {
/* check actuator number */
if (mission_item.params[0] < 0 || mission_item.params[0] > 5) {
@ -371,6 +393,7 @@ MissionFeasibilityChecker::check_dist_1wp(dm_item_t dm_current, size_t nMissionI @@ -371,6 +393,7 @@ MissionFeasibilityChecker::check_dist_1wp(dm_item_t dm_current, size_t nMissionI
warning_issued = true;
return false;
}
/* check actuator value */
if (mission_item.params[1] < -2000 || mission_item.params[1] > 2000) {
mavlink_log_critical(_mavlink_log_pub, "Actuator value %d is out of bounds -2000..2000", (int)mission_item.params[1]);
@ -378,20 +401,23 @@ MissionFeasibilityChecker::check_dist_1wp(dm_item_t dm_current, size_t nMissionI @@ -378,20 +401,23 @@ MissionFeasibilityChecker::check_dist_1wp(dm_item_t dm_current, size_t nMissionI
return false;
}
}
/* check only items with valid lat/lon */
else if (isPositionCommand(mission_item.nav_cmd)) {
/* check distance from current position to item */
float dist_to_1wp = get_distance_to_next_waypoint(
mission_item.lat, mission_item.lon, curr_lat, curr_lon);
mission_item.lat, mission_item.lon, curr_lat, curr_lon);
if (dist_to_1wp < dist_first_wp) {
_dist_1wp_ok = true;
if (dist_to_1wp > ((dist_first_wp * 3) / 2)) {
/* allow at 2/3 distance, but warn */
mavlink_log_critical(_mavlink_log_pub, "Warning: First waypoint very far: %d m", (int)dist_to_1wp);
warning_issued = true;
}
return true;
} else {
@ -420,17 +446,19 @@ MissionFeasibilityChecker::check_dist_1wp(dm_item_t dm_current, size_t nMissionI @@ -420,17 +446,19 @@ MissionFeasibilityChecker::check_dist_1wp(dm_item_t dm_current, size_t nMissionI
}
bool
MissionFeasibilityChecker::isPositionCommand(unsigned cmd){
MissionFeasibilityChecker::isPositionCommand(unsigned cmd)
{
if (cmd == NAV_CMD_WAYPOINT ||
cmd == NAV_CMD_LOITER_UNLIMITED ||
cmd == NAV_CMD_LOITER_TIME_LIMIT ||
cmd == NAV_CMD_LAND ||
cmd == NAV_CMD_TAKEOFF ||
cmd == NAV_CMD_LOITER_TO_ALT ||
cmd == NAV_CMD_VTOL_TAKEOFF ||
cmd == NAV_CMD_VTOL_LAND) {
cmd == NAV_CMD_LOITER_UNLIMITED ||
cmd == NAV_CMD_LOITER_TIME_LIMIT ||
cmd == NAV_CMD_LAND ||
cmd == NAV_CMD_TAKEOFF ||
cmd == NAV_CMD_LOITER_TO_ALT ||
cmd == NAV_CMD_VTOL_TAKEOFF ||
cmd == NAV_CMD_VTOL_LAND) {
return true;
} else {
return false;

18
src/modules/navigator/mission_feasibility_checker.h
Unescape View File

@ -63,18 +63,22 @@ private: @@ -63,18 +63,22 @@ private:
/* Checks for all airframes */
bool checkGeofence(dm_item_t dm_current, size_t nMissionItems, Geofence &geofence, float home_alt);
bool checkHomePositionAltitude(dm_item_t dm_current, size_t nMissionItems, float home_alt, bool home_valid, bool &warning_issued, bool throw_error = false);
bool checkHomePositionAltitude(dm_item_t dm_current, size_t nMissionItems, float home_alt, bool home_valid,
bool &warning_issued, bool throw_error = false);
bool checkMissionItemValidity(dm_item_t dm_current, size_t nMissionItems, bool condition_landed);
bool check_dist_1wp(dm_item_t dm_current, size_t nMissionItems, double curr_lat, double curr_lon, float dist_first_wp, bool &warning_issued);
bool check_dist_1wp(dm_item_t dm_current, size_t nMissionItems, double curr_lat, double curr_lon, float dist_first_wp,
bool &warning_issued);
bool isPositionCommand(unsigned cmd);
/* Checks specific to fixedwing airframes */
bool checkMissionFeasibleFixedwing(dm_item_t dm_current, size_t nMissionItems, Geofence &geofence, float home_alt, bool home_valid);
bool checkMissionFeasibleFixedwing(dm_item_t dm_current, size_t nMissionItems, Geofence &geofence, float home_alt,
bool home_valid);
bool checkFixedWingLanding(dm_item_t dm_current, size_t nMissionItems);
void updateNavigationCapabilities();
/* Checks specific to rotarywing airframes */
bool checkMissionFeasibleRotarywing(dm_item_t dm_current, size_t nMissionItems, Geofence &geofence, float home_alt, bool home_valid, float default_acceptance_rad);
bool checkMissionFeasibleRotarywing(dm_item_t dm_current, size_t nMissionItems, Geofence &geofence, float home_alt,
bool home_valid, float default_acceptance_rad);
public:
MissionFeasibilityChecker();
@ -88,9 +92,9 @@ public: @@ -88,9 +92,9 @@ public:
* Returns true if mission is feasible and false otherwise
*/
bool checkMissionFeasible(orb_advert_t *mavlink_log_pub, bool isRotarywing, dm_item_t dm_current,
size_t nMissionItems, Geofence &geofence, float home_alt, bool home_valid,
double curr_lat, double curr_lon, float max_waypoint_distance, bool &warning_issued, float default_acceptance_rad,
bool condition_landed);
size_t nMissionItems, Geofence &geofence, float home_alt, bool home_valid,
double curr_lat, double curr_lon, float max_waypoint_distance, bool &warning_issued, float default_acceptance_rad,
bool condition_landed);
};

38
src/modules/navigator/navigation.h
Unescape View File

@ -69,19 +69,19 @@ enum NAV_CMD { @@ -69,19 +69,19 @@ enum NAV_CMD {
NAV_CMD_VTOL_LAND = 85,
NAV_CMD_DO_JUMP = 177,
NAV_CMD_DO_CHANGE_SPEED = 178,
NAV_CMD_DO_SET_SERVO=183,
NAV_CMD_DO_LAND_START=189,
NAV_CMD_DO_SET_ROI=201,
NAV_CMD_DO_DIGICAM_CONTROL=203,
NAV_CMD_DO_MOUNT_CONFIGURE=204,
NAV_CMD_DO_MOUNT_CONTROL=205,
NAV_CMD_DO_SET_CAM_TRIGG_DIST=206,
NAV_CMD_IMAGE_START_CAPTURE=2000,
NAV_CMD_IMAGE_STOP_CAPTURE=2001,
NAV_CMD_VIDEO_START_CAPTURE=2500,
NAV_CMD_VIDEO_STOP_CAPTURE=2501,
NAV_CMD_DO_VTOL_TRANSITION=3000,
NAV_CMD_INVALID=UINT16_MAX /* ensure that casting a large number results in a specific error */
NAV_CMD_DO_SET_SERVO = 183,
NAV_CMD_DO_LAND_START = 189,
NAV_CMD_DO_SET_ROI = 201,
NAV_CMD_DO_DIGICAM_CONTROL = 203,
NAV_CMD_DO_MOUNT_CONFIGURE = 204,
NAV_CMD_DO_MOUNT_CONTROL = 205,
NAV_CMD_DO_SET_CAM_TRIGG_DIST = 206,
NAV_CMD_IMAGE_START_CAPTURE = 2000,
NAV_CMD_IMAGE_STOP_CAPTURE = 2001,
NAV_CMD_VIDEO_START_CAPTURE = 2500,
NAV_CMD_VIDEO_STOP_CAPTURE = 2501,
NAV_CMD_DO_VTOL_TRANSITION = 3000,
NAV_CMD_INVALID = UINT16_MAX /* ensure that casting a large number results in a specific error */
};
enum ORIGIN {
@ -125,12 +125,12 @@ struct mission_item_s { @@ -125,12 +125,12 @@ struct mission_item_s {
uint16_t do_jump_current_count; /**< count how many times the jump has been done */
struct {
uint16_t frame : 4, /**< mission frame ***/
origin : 3, /**< how the mission item was generated */
loiter_exit_xtrack : 1, /**< exit xtrack location: 0 for center of loiter wp, 1 for exit location */
force_heading : 1, /**< heading needs to be reached ***/
altitude_is_relative : 1, /**< true if altitude is relative from start point */
autocontinue : 1, /**< true if next waypoint should follow after this one */
disable_mc_yaw : 1; /**< weathervane mode */
origin : 3, /**< how the mission item was generated */
loiter_exit_xtrack : 1, /**< exit xtrack location: 0 for center of loiter wp, 1 for exit location */
force_heading : 1, /**< heading needs to be reached ***/
altitude_is_relative : 1, /**< true if altitude is relative from start point */
autocontinue : 1, /**< true if next waypoint should follow after this one */
disable_mc_yaw : 1; /**< weathervane mode */
};
};
#pragma pack(pop)

42
src/modules/navigator/navigator.h
Unescape View File

@ -133,24 +133,24 @@ public: @@ -133,24 +133,24 @@ public:
/**
* Getters
*/
struct vehicle_status_s* get_vstatus() { return &_vstatus; }
struct vehicle_land_detected_s* get_land_detected() { return &_land_detected; }
struct vehicle_control_mode_s* get_control_mode() { return &_control_mode; }
struct vehicle_global_position_s* get_global_position() { return &_global_pos; }
struct vehicle_gps_position_s* get_gps_position() { return &_gps_pos; }
struct sensor_combined_s* get_sensor_combined() { return &_sensor_combined; }
struct home_position_s* get_home_position() { return &_home_pos; }
struct vehicle_status_s *get_vstatus() { return &_vstatus; }
struct vehicle_land_detected_s *get_land_detected() { return &_land_detected; }
struct vehicle_control_mode_s *get_control_mode() { return &_control_mode; }
struct vehicle_global_position_s *get_global_position() { return &_global_pos; }
struct vehicle_gps_position_s *get_gps_position() { return &_gps_pos; }
struct sensor_combined_s *get_sensor_combined() { return &_sensor_combined; }
struct home_position_s *get_home_position() { return &_home_pos; }
bool home_position_valid() { return (_home_pos.timestamp > 0); }
struct position_setpoint_triplet_s* get_position_setpoint_triplet() { return &_pos_sp_triplet; }
struct position_setpoint_triplet_s* get_reposition_triplet() { return &_reposition_triplet; }
struct position_setpoint_triplet_s* get_takeoff_triplet() { return &_takeoff_triplet; }
struct mission_result_s* get_mission_result() { return &_mission_result; }
struct geofence_result_s* get_geofence_result() { return &_geofence_result; }
struct vehicle_attitude_setpoint_s* get_att_sp() { return &_att_sp; }
struct position_setpoint_triplet_s *get_position_setpoint_triplet() { return &_pos_sp_triplet; }
struct position_setpoint_triplet_s *get_reposition_triplet() { return &_reposition_triplet; }
struct position_setpoint_triplet_s *get_takeoff_triplet() { return &_takeoff_triplet; }
struct mission_result_s *get_mission_result() { return &_mission_result; }
struct geofence_result_s *get_geofence_result() { return &_geofence_result; }
struct vehicle_attitude_setpoint_s *get_att_sp() { return &_att_sp; }
int get_onboard_mission_sub() { return _onboard_mission_sub; }
int get_offboard_mission_sub() { return _offboard_mission_sub; }
Geofence& get_geofence() { return _geofence; }
Geofence &get_geofence() { return _geofence; }
bool get_can_loiter_at_sp() { return _can_loiter_at_sp; }
float get_loiter_radius() { return _param_loiter_radius.get(); }
@ -189,7 +189,7 @@ public: @@ -189,7 +189,7 @@ public:
* For VTOL: sets cuising speed for current mode only (multirotor or fixed-wing).
*
*/
void set_cruising_speed(float speed=-1.0f);
void set_cruising_speed(float speed = -1.0f);
/**
* Reset cruising speed to default values
@ -208,7 +208,7 @@ public: @@ -208,7 +208,7 @@ public:
/**
* Set the target throttle
*/
void set_cruising_throttle(float throttle=-1.0f) { _mission_throttle = throttle; }
void set_cruising_throttle(float throttle = -1.0f) { _mission_throttle = throttle; }
/**
* Get the acceptance radius given the mission item preset radius
@ -228,7 +228,7 @@ public: @@ -228,7 +228,7 @@ public:
bool abort_landing();
static float get_time_inside(struct mission_item_s& item) { return (item.nav_cmd == NAV_CMD_TAKEOFF) ? 0.0f : item.time_inside; }
static float get_time_inside(struct mission_item_s &item) { return (item.nav_cmd == NAV_CMD_TAKEOFF) ? 0.0f : item.time_inside; }
private:
@ -310,7 +310,7 @@ private: @@ -310,7 +310,7 @@ private:
control::BlockParamFloat _param_acceptance_radius; /**< acceptance for takeoff */
control::BlockParamFloat _param_fw_alt_acceptance_radius; /**< acceptance radius for fixedwing altitude */
control::BlockParamFloat _param_mc_alt_acceptance_radius; /**< acceptance radius for multicopter altitude */
control::BlockParamFloat _param_cruising_speed_hover;
control::BlockParamFloat _param_cruising_speed_plane;
control::BlockParamFloat _param_cruising_throttle_plane;
@ -337,7 +337,7 @@ private: @@ -337,7 +337,7 @@ private:
/**
* Retrieve home position
*/
void home_position_update(bool force=false);
void home_position_update(bool force = false);
/**
* Retrieve fixed wing navigation capabilities
@ -393,7 +393,7 @@ private: @@ -393,7 +393,7 @@ private:
/* this class has ptr data members, so it should not be copied,
* consequently the copy constructors are private.
*/
Navigator(const Navigator&);
Navigator operator=(const Navigator&);
Navigator(const Navigator &);
Navigator operator=(const Navigator &);
};
#endif

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

@ -344,6 +344,7 @@ Navigator::task_main() @@ -344,6 +344,7 @@ Navigator::task_main()
global_pos_available_once = false;
PX4_WARN("global position timeout");
}
/* Let the loop run anyway, don't do `continue` here. */
} else if (pret < 0) {
@ -351,14 +352,17 @@ Navigator::task_main() @@ -351,14 +352,17 @@ Navigator::task_main()
PX4_ERR("nav: poll error %d, %d", pret, errno);
usleep(10000);
continue;
} else {
if (fds[0].revents & POLLIN) {
/* success, global pos is available */
global_position_update();
if (_geofence.getSource() == Geofence::GF_SOURCE_GLOBALPOS) {
have_geofence_position_data = true;
}
global_pos_available_once = true;
}
}
@ -369,8 +373,10 @@ Navigator::task_main() @@ -369,8 +373,10 @@ Navigator::task_main()
/* gps updated */
orb_check(_gps_pos_sub, &updated);
if (updated) {
gps_position_update();
if (_geofence.getSource() == Geofence::GF_SOURCE_GPS) {
have_geofence_position_data = true;
}
@ -378,12 +384,14 @@ Navigator::task_main() @@ -378,12 +384,14 @@ Navigator::task_main()
/* sensors combined updated */
orb_check(_sensor_combined_sub, &updated);
if (updated) {
sensor_combined_update();
}
/* parameters updated */
orb_check(_param_update_sub, &updated);
if (updated) {
params_update();
updateParams();
@ -391,35 +399,41 @@ Navigator::task_main() @@ -391,35 +399,41 @@ Navigator::task_main()
/* vehicle control mode updated */
orb_check(_control_mode_sub, &updated);
if (updated) {
vehicle_control_mode_update();
}
/* vehicle status updated */
orb_check(_vstatus_sub, &updated);
if (updated) {
vehicle_status_update();
}
/* vehicle land detected updated */
orb_check(_land_detected_sub, &updated);
if (updated) {
vehicle_land_detected_update();
}
/* navigation capabilities updated */
orb_check(_fw_pos_ctrl_status_sub, &updated);
if (updated) {
fw_pos_ctrl_status_update();
}
/* home position updated */
orb_check(_home_pos_sub, &updated);
if (updated) {
home_position_update();
}
orb_check(_vehicle_command_sub, &updated);
if (updated) {
vehicle_command_s cmd;
orb_copy(ORB_ID(vehicle_command), _vehicle_command_sub, &cmd);
@ -441,6 +455,7 @@ Navigator::task_main() @@ -441,6 +455,7 @@ Navigator::task_main()
// Go on and check which changes had been requested
if (PX4_ISFINITE(cmd.param4)) {
rep->current.yaw = cmd.param4;
} else {
rep->current.yaw = NAN;
}
@ -456,6 +471,7 @@ Navigator::task_main() @@ -456,6 +471,7 @@ Navigator::task_main()
if (PX4_ISFINITE(cmd.param7)) {
rep->current.alt = cmd.param7;
} else {
rep->current.alt = get_global_position()->alt;
}
@ -463,6 +479,7 @@ Navigator::task_main() @@ -463,6 +479,7 @@ Navigator::task_main()
rep->previous.valid = true;
rep->current.valid = true;
rep->next.valid = false;
} else if (cmd.command == vehicle_command_s::VEHICLE_CMD_NAV_TAKEOFF) {
struct position_setpoint_triplet_s *rep = get_takeoff_triplet();
@ -480,6 +497,7 @@ Navigator::task_main() @@ -480,6 +497,7 @@ Navigator::task_main()
if (PX4_ISFINITE(cmd.param5) && PX4_ISFINITE(cmd.param6)) {
rep->current.lat = (cmd.param5 < 1000) ? cmd.param5 : cmd.param5 / (double)1e7;
rep->current.lon = (cmd.param6 < 1000) ? cmd.param6 : cmd.param6 / (double)1e7;
} else {
// If one of them is non-finite, reset both
rep->current.lat = NAN;
@ -498,6 +516,7 @@ Navigator::task_main() @@ -498,6 +516,7 @@ Navigator::task_main()
* use MAV_CMD_MISSION_START to start the mission there
*/
unsigned land_start = _mission.find_offboard_land_start();
if (land_start != -1) {
vehicle_command_s vcmd = {};
vcmd.target_system = get_vstatus()->system_id;
@ -513,7 +532,7 @@ Navigator::task_main() @@ -513,7 +532,7 @@ Navigator::task_main()
} else if (cmd.command == vehicle_command_s::VEHICLE_CMD_MISSION_START) {
if (get_mission_result()->valid &&
PX4_ISFINITE(cmd.param1) && (cmd.param1 >= 0) && (cmd.param1 < _mission_result.seq_total)) {
PX4_ISFINITE(cmd.param1) && (cmd.param1 >= 0) && (cmd.param1 < _mission_result.seq_total)) {
_mission.set_current_offboard_mission_index(cmd.param1);
}
@ -542,15 +561,18 @@ Navigator::task_main() @@ -542,15 +561,18 @@ Navigator::task_main()
/* Check geofence violation */
static hrt_abstime last_geofence_check = 0;
if (have_geofence_position_data &&
(_geofence.getGeofenceAction() != geofence_result_s::GF_ACTION_NONE) &&
(hrt_elapsed_time(&last_geofence_check) > GEOFENCE_CHECK_INTERVAL)) {
(_geofence.getGeofenceAction() != geofence_result_s::GF_ACTION_NONE) &&
(hrt_elapsed_time(&last_geofence_check) > GEOFENCE_CHECK_INTERVAL)) {
bool inside = _geofence.inside(_global_pos, _gps_pos, _sensor_combined.baro_alt_meter, _home_pos, home_position_valid());
bool inside = _geofence.inside(_global_pos, _gps_pos, _sensor_combined.baro_alt_meter, _home_pos,
home_position_valid());
last_geofence_check = hrt_absolute_time();
have_geofence_position_data = false;
_geofence_result.geofence_action = _geofence.getGeofenceAction();
if (!inside) {
/* inform other apps via the mission result */
_geofence_result.geofence_violated = true;
@ -561,6 +583,7 @@ Navigator::task_main() @@ -561,6 +583,7 @@ Navigator::task_main()
mavlink_log_critical(&_mavlink_log_pub, "Geofence violation");
_geofence_violation_warning_sent = true;
}
} else {
/* inform other apps via the mission result */
_geofence_result.geofence_violated = false;
@ -572,62 +595,73 @@ Navigator::task_main() @@ -572,62 +595,73 @@ Navigator::task_main()
/* Do stuff according to navigation state set by commander */
switch (_vstatus.nav_state) {
case vehicle_status_s::NAVIGATION_STATE_AUTO_MISSION:
_pos_sp_triplet_published_invalid_once = false;
_navigation_mode = &_mission;
break;
case vehicle_status_s::NAVIGATION_STATE_AUTO_LOITER:
_pos_sp_triplet_published_invalid_once = false;
_navigation_mode = &_loiter;
break;
case vehicle_status_s::NAVIGATION_STATE_AUTO_RCRECOVER:
_pos_sp_triplet_published_invalid_once = false;
_navigation_mode = &_rcLoss;
break;
case vehicle_status_s::NAVIGATION_STATE_AUTO_RTL:
_pos_sp_triplet_published_invalid_once = false;
_navigation_mode = &_rtl;
break;
case vehicle_status_s::NAVIGATION_STATE_AUTO_TAKEOFF:
_pos_sp_triplet_published_invalid_once = false;
_navigation_mode = &_takeoff;
break;
case vehicle_status_s::NAVIGATION_STATE_AUTO_LAND:
_pos_sp_triplet_published_invalid_once = false;
_navigation_mode = &_land;
break;
case vehicle_status_s::NAVIGATION_STATE_DESCEND:
_pos_sp_triplet_published_invalid_once = false;
_navigation_mode = &_land;
break;
case vehicle_status_s::NAVIGATION_STATE_AUTO_RTGS:
_pos_sp_triplet_published_invalid_once = false;
_navigation_mode = &_dataLinkLoss;
break;
case vehicle_status_s::NAVIGATION_STATE_AUTO_LANDENGFAIL:
_pos_sp_triplet_published_invalid_once = false;
_navigation_mode = &_engineFailure;
break;
case vehicle_status_s::NAVIGATION_STATE_AUTO_LANDGPSFAIL:
_pos_sp_triplet_published_invalid_once = false;
_navigation_mode = &_gpsFailure;
break;
case vehicle_status_s::NAVIGATION_STATE_AUTO_FOLLOW_TARGET:
_pos_sp_triplet_published_invalid_once = false;
_navigation_mode = &_follow_target;
break;
case vehicle_status_s::NAVIGATION_STATE_MANUAL:
case vehicle_status_s::NAVIGATION_STATE_ACRO:
case vehicle_status_s::NAVIGATION_STATE_ALTCTL:
case vehicle_status_s::NAVIGATION_STATE_POSCTL:
case vehicle_status_s::NAVIGATION_STATE_TERMINATION:
case vehicle_status_s::NAVIGATION_STATE_OFFBOARD:
case vehicle_status_s::NAVIGATION_STATE_STAB:
default:
_pos_sp_triplet_published_invalid_once = false;
_navigation_mode = nullptr;
_can_loiter_at_sp = false;
break;
case vehicle_status_s::NAVIGATION_STATE_AUTO_MISSION:
_pos_sp_triplet_published_invalid_once = false;
_navigation_mode = &_mission;
break;
case vehicle_status_s::NAVIGATION_STATE_AUTO_LOITER:
_pos_sp_triplet_published_invalid_once = false;
_navigation_mode = &_loiter;
break;
case vehicle_status_s::NAVIGATION_STATE_AUTO_RCRECOVER:
_pos_sp_triplet_published_invalid_once = false;
_navigation_mode = &_rcLoss;
break;
case vehicle_status_s::NAVIGATION_STATE_AUTO_RTL:
_pos_sp_triplet_published_invalid_once = false;
_navigation_mode = &_rtl;
break;
case vehicle_status_s::NAVIGATION_STATE_AUTO_TAKEOFF:
_pos_sp_triplet_published_invalid_once = false;
_navigation_mode = &_takeoff;
break;
case vehicle_status_s::NAVIGATION_STATE_AUTO_LAND:
_pos_sp_triplet_published_invalid_once = false;
_navigation_mode = &_land;
break;
case vehicle_status_s::NAVIGATION_STATE_DESCEND:
_pos_sp_triplet_published_invalid_once = false;
_navigation_mode = &_land;
break;
case vehicle_status_s::NAVIGATION_STATE_AUTO_RTGS:
_pos_sp_triplet_published_invalid_once = false;
_navigation_mode = &_dataLinkLoss;
break;
case vehicle_status_s::NAVIGATION_STATE_AUTO_LANDENGFAIL:
_pos_sp_triplet_published_invalid_once = false;
_navigation_mode = &_engineFailure;
break;
case vehicle_status_s::NAVIGATION_STATE_AUTO_LANDGPSFAIL:
_pos_sp_triplet_published_invalid_once = false;
_navigation_mode = &_gpsFailure;
break;
case vehicle_status_s::NAVIGATION_STATE_AUTO_FOLLOW_TARGET:
_pos_sp_triplet_published_invalid_once = false;
_navigation_mode = &_follow_target;
break;
case vehicle_status_s::NAVIGATION_STATE_MANUAL:
case vehicle_status_s::NAVIGATION_STATE_ACRO:
case vehicle_status_s::NAVIGATION_STATE_ALTCTL:
case vehicle_status_s::NAVIGATION_STATE_POSCTL:
case vehicle_status_s::NAVIGATION_STATE_TERMINATION:
case vehicle_status_s::NAVIGATION_STATE_OFFBOARD:
case vehicle_status_s::NAVIGATION_STATE_STAB:
default:
_pos_sp_triplet_published_invalid_once = false;
_navigation_mode = nullptr;
_can_loiter_at_sp = false;
break;
}
/* iterate through navigation modes and set active/inactive for each */
@ -657,6 +691,7 @@ Navigator::task_main() @@ -657,6 +691,7 @@ Navigator::task_main()
perf_end(_loop_perf);
}
PX4_INFO("exiting");
_navigator_task = -1;
@ -670,11 +705,11 @@ Navigator::start() @@ -670,11 +705,11 @@ Navigator::start()
/* start the task */
_navigator_task = px4_task_spawn_cmd("navigator",
SCHED_DEFAULT,
SCHED_PRIORITY_DEFAULT + 5,
1600,
(px4_main_t)&Navigator::task_main_trampoline,
nullptr);
SCHED_DEFAULT,
SCHED_PRIORITY_DEFAULT + 5,
1600,
(px4_main_t)&Navigator::task_main_trampoline,
nullptr);
if (_navigator_task < 0) {
warn("task start failed");
@ -748,6 +783,7 @@ Navigator::get_altitude_acceptance_radius() @@ -748,6 +783,7 @@ Navigator::get_altitude_acceptance_radius()
{
if (!this->get_vstatus()->is_rotary_wing) {
return _param_fw_alt_acceptance_radius.get();
} else {
return _param_mc_alt_acceptance_radius.get();
}
@ -778,7 +814,8 @@ Navigator::get_cruising_speed() @@ -778,7 +814,8 @@ Navigator::get_cruising_speed()
}
void
Navigator::set_cruising_speed(float speed) {
Navigator::set_cruising_speed(float speed)
{
if (_vstatus.is_rotary_wing) {
_mission_cruising_speed_mc = speed;
@ -800,6 +837,7 @@ Navigator::get_cruising_throttle() @@ -800,6 +837,7 @@ Navigator::get_cruising_throttle()
/* Return the mission-requested cruise speed, or default FW_THR_CRUISE value */
if (_mission_throttle > FLT_EPSILON) {
return _mission_throttle;
} else {
return _param_cruising_throttle_plane.get();
}
@ -844,7 +882,7 @@ Navigator::abort_landing() @@ -844,7 +882,7 @@ Navigator::abort_landing()
if (hrt_elapsed_time(&_fw_pos_ctrl_status.timestamp) < 1000000) {
if (get_position_setpoint_triplet()->current.valid
&& get_position_setpoint_triplet()->current.type == position_setpoint_s::SETPOINT_TYPE_LAND) {
&& get_position_setpoint_triplet()->current.type == position_setpoint_s::SETPOINT_TYPE_LAND) {
should_abort = _fw_pos_ctrl_status.abort_landing;
}
@ -898,12 +936,16 @@ int navigator_main(int argc, char *argv[]) @@ -898,12 +936,16 @@ int navigator_main(int argc, char *argv[])
if (!strcmp(argv[1], "stop")) {
delete navigator::g_navigator;
navigator::g_navigator = nullptr;
} else if (!strcmp(argv[1], "status")) {
navigator::g_navigator->status();
} 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);
} else {
usage();
return 1;
@ -970,7 +1012,7 @@ Navigator::publish_att_sp() @@ -970,7 +1012,7 @@ Navigator::publish_att_sp()
}
void
Navigator::set_mission_failure(const char* reason)
Navigator::set_mission_failure(const char *reason)
{
if (!_mission_result.mission_failure) {
_mission_result.mission_failure = true;

2
src/modules/navigator/takeoff.cpp
Unescape View File

@ -80,6 +80,7 @@ void @@ -80,6 +80,7 @@ void
Takeoff::on_active()
{
struct position_setpoint_triplet_s *rep = _navigator->get_takeoff_triplet();
if (rep->current.valid) {
// reset the position
set_takeoff_position();
@ -115,6 +116,7 @@ Takeoff::set_takeoff_position() @@ -115,6 +116,7 @@ Takeoff::set_takeoff_position()
mavlink_log_critical(_navigator->get_mavlink_log_pub(),
"Using minimum takeoff altitude: %.2f m", (double)_param_min_alt.get());
}
} else {
// Use home + minimum clearance but only notify.
abs_altitude = min_abs_altitude;

Write Preview
Loading…
Cancel
Save