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

navigator fix code style

This commit is contained in:
Daniel Agar 2017-01-04 17:27:13 -05:00
parent be14c11589
commit c280358e32
11 changed files with 410 additions and 312 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

5
Tools/files_to_check_code_style.sh
View File

@ -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
View File

@ -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()
_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()
* 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
/* 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
View File

@ -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

266
src/modules/navigator/mission_block.cpp
View File

@ -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()
{
/* handle non-navigation or indefinite waypoints */
switch (_mission_item.nav_cmd) {
case NAV_CMD_DO_SET_SERVO:
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_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:
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_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:
} else {
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:
return true;
case NAV_CMD_DO_CHANGE_SPEED:
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;
}
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()
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()
}
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()
/* 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()
* 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()
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()
} 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()
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()
}
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()
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()
/* 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,
// 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)
{
// 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
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
// 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)
}
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
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
/* 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
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
View File

@ -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);

156
src/modules/navigator/mission_feasibility_checker.cpp
View File

@ -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
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
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
}
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
// 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
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
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
// 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
}
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,
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,
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,
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
// 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) {
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);
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
}
return true;
}
@ -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
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
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
}
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
/* 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
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
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
}
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
View File

@ -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:
* 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
View File

@ -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 {
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
View File

@ -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:
* 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:
/**
* 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:
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:
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:
/**
* 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:
/* 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

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

@ -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()
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()
/* 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()
/* 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()
/* 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()
// 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()
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()
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()
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()
* 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()
} 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()
/* 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)) {
bool inside = _geofence.inside(_global_pos, _gps_pos, _sensor_combined.baro_alt_meter, _home_pos, home_position_valid());
if (have_geofence_position_data &&
(_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());
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()
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()
/* 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()
perf_end(_loop_perf);
}
PX4_INFO("exiting");
_navigator_task = -1;
@ -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()
{
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()
}
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()
/* 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()
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[])
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()
}
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
View File

@ -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()
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;