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

implement MAV_CMD_NAV_LOITER_TO_ALT and general mission cleanup

Browse Source
This commit is contained in:
Daniel Agar
2016-07-12 23:36:05 +02:00
committed by Lorenz Meier
parent 7419151314
commit 59b4350aa0
8 changed files with 314 additions and 119 deletions
Download Patch File Download Diff File Expand all files Collapse all files
ROMFS/px4fmu_common/init.d/rc.fw_defaults
+3
View File
@@ -14,6 +14,9 @@ then
# FW uses L1 distance for acceptance radius
# set a smaller NAV_ACC_RAD for vertical acceptance distance
param set NAV_ACC_RAD 10
param set MIS_LTRMIN_ALT 25
param set MIS_TAKEOFF_ALT 25
param set PE_VELNE_NOISE 0.3
param set PE_VELD_NOISE 0.35
src/modules/commander/commander.cpp
+6 -6
View File
@@ -1064,23 +1064,23 @@ bool handle_command(struct vehicle_status_s *status_local, const struct safety_s
}
break;
case vehicle_command_s::VEHICLE_CMD_CUSTOM_0:
case vehicle_command_s::VEHICLE_CMD_CUSTOM_1:
case vehicle_command_s::VEHICLE_CMD_CUSTOM_2:
case vehicle_command_s::VEHICLE_CMD_PREFLIGHT_REBOOT_SHUTDOWN:
case vehicle_command_s::VEHICLE_CMD_PREFLIGHT_CALIBRATION:
case vehicle_command_s::VEHICLE_CMD_PREFLIGHT_SET_SENSOR_OFFSETS:
case vehicle_command_s::VEHICLE_CMD_PREFLIGHT_STORAGE:
case vehicle_command_s::VEHICLE_CMD_PREFLIGHT_UAVCAN:
case vehicle_command_s::VEHICLE_CMD_CUSTOM_0:
case vehicle_command_s::VEHICLE_CMD_CUSTOM_1:
case vehicle_command_s::VEHICLE_CMD_CUSTOM_2:
case vehicle_command_s::VEHICLE_CMD_PAYLOAD_PREPARE_DEPLOY:
case vehicle_command_s::VEHICLE_CMD_PAYLOAD_CONTROL_DEPLOY:
case vehicle_command_s::VEHICLE_CMD_DO_MOUNT_CONTROL:
case vehicle_command_s::VEHICLE_CMD_DO_MOUNT_CONTROL_QUAT:
case vehicle_command_s::VEHICLE_CMD_DO_MOUNT_CONFIGURE:
case vehicle_command_s::VEHICLE_CMD_DO_TRIGGER_CONTROL:
case vehicle_command_s::VEHICLE_CMD_DO_VTOL_TRANSITION:
case vehicle_command_s::VEHICLE_CMD_DO_DIGICAM_CONTROL:
case vehicle_command_s::VEHICLE_CMD_DO_MOUNT_CONFIGURE:
case vehicle_command_s::VEHICLE_CMD_DO_MOUNT_CONTROL:
case vehicle_command_s::VEHICLE_CMD_DO_SET_CAM_TRIGG_DIST:
case vehicle_command_s::VEHICLE_CMD_DO_MOUNT_CONTROL_QUAT:
case vehicle_command_s::VEHICLE_CMD_DO_CHANGE_SPEED:
case vehicle_command_s::VEHICLE_CMD_START_RX_PAIR:
/* ignore commands that handled in low prio loop */
src/modules/mavlink/mavlink_mission.cpp
+164 -61
View File
@@ -767,24 +767,83 @@ MavlinkMissionManager::handle_mission_clear_all(const mavlink_message_t *msg)
int
MavlinkMissionManager::parse_mavlink_mission_item(const mavlink_mission_item_t *mavlink_mission_item, struct mission_item_s *mission_item)
{
/* only support global waypoints for now */
switch (mavlink_mission_item->frame) {
case MAV_FRAME_GLOBAL:
if (mavlink_mission_item->frame == MAV_FRAME_GLOBAL ||
mavlink_mission_item->frame == MAV_FRAME_GLOBAL_RELATIVE_ALT) {
// only support global waypoints for now
mission_item->lat = (double)mavlink_mission_item->x;
mission_item->lon = (double)mavlink_mission_item->y;
mission_item->altitude = mavlink_mission_item->z;
mission_item->altitude_is_relative = false;
break;
case MAV_FRAME_GLOBAL_RELATIVE_ALT:
mission_item->lat = (double)mavlink_mission_item->x;
mission_item->lon = (double)mavlink_mission_item->y;
mission_item->altitude = mavlink_mission_item->z;
mission_item->altitude_is_relative = true;
break;
if (mavlink_mission_item->frame == MAV_FRAME_GLOBAL) {
mission_item->altitude_is_relative = false;
} else if (mavlink_mission_item->frame == MAV_FRAME_GLOBAL_RELATIVE_ALT) {
mission_item->altitude_is_relative = true;
}
mission_item->time_inside = 0.0f;
switch (mavlink_mission_item->command) {
case MAV_CMD_NAV_WAYPOINT:
mission_item->nav_cmd = NAV_CMD_WAYPOINT;
mission_item->time_inside = mavlink_mission_item->param1;
mission_item->acceptance_radius = mavlink_mission_item->param2;
mission_item->yaw = _wrap_pi(mavlink_mission_item->param4 * M_DEG_TO_RAD_F);
break;
case MAV_CMD_NAV_LOITER_UNLIM:
mission_item->nav_cmd = NAV_CMD_LOITER_UNLIMITED;
mission_item->loiter_radius = fabsf(mavlink_mission_item->param3);
mission_item->loiter_direction = (mavlink_mission_item->param3 > 0) ? 1 : -1; /* 1 if positive CW, -1 if negative CCW */
mission_item->yaw = _wrap_pi(mavlink_mission_item->param4 * M_DEG_TO_RAD_F);
break;
case MAV_CMD_NAV_LOITER_TIME:
mission_item->nav_cmd = NAV_CMD_LOITER_TIME_LIMIT;
mission_item->time_inside = mavlink_mission_item->param1;
mission_item->loiter_radius = fabsf(mavlink_mission_item->param3);
mission_item->loiter_direction = (mavlink_mission_item->param3 > 0) ? 1 : -1; /* 1 if positive CW, -1 if negative CCW */
mission_item->loiter_exit_xtrack = (mavlink_mission_item->param4 > 0) ? true : false;
break;
case MAV_CMD_NAV_LAND:
mission_item->nav_cmd = NAV_CMD_LAND;
// TODO: abort alt param1
mission_item->yaw = _wrap_pi(mavlink_mission_item->param4 * M_DEG_TO_RAD_F);
break;
case MAV_CMD_NAV_TAKEOFF:
mission_item->nav_cmd = NAV_CMD_TAKEOFF;
mission_item->pitch_min = mavlink_mission_item->param1;
mission_item->yaw = _wrap_pi(mavlink_mission_item->param4 * M_DEG_TO_RAD_F);
break;
case MAV_CMD_NAV_LOITER_TO_ALT:
mission_item->nav_cmd = NAV_CMD_LOITER_TO_ALT;
mission_item->force_heading = (mavlink_mission_item->param1 > 0) ? true : false;
mission_item->loiter_radius = fabsf(mavlink_mission_item->param2);
mission_item->loiter_direction = (mavlink_mission_item->param2 > 0) ? 1 : -1; /* 1 if positive CW, -1 if negative CCW */
mission_item->loiter_exit_xtrack = (mavlink_mission_item->param4 > 0) ? true : false;
break;
case MAV_CMD_NAV_VTOL_TAKEOFF:
case MAV_CMD_NAV_VTOL_LAND:
mission_item->nav_cmd = (NAV_CMD)mavlink_mission_item->command;
mission_item->yaw = _wrap_pi(mavlink_mission_item->param4 * M_DEG_TO_RAD_F);
break;
default:
mission_item->nav_cmd = NAV_CMD_INVALID;
return MAV_MISSION_UNSUPPORTED;
}
mission_item->frame = mavlink_mission_item->frame;
} else if (mavlink_mission_item->frame == MAV_FRAME_MISSION) {
// this is a mission item with no coordinates
case MAV_FRAME_MISSION:
// This is a mission item with no coordinate
mission_item->params[0] = mavlink_mission_item->param1;
mission_item->params[1] = mavlink_mission_item->param2;
mission_item->params[2] = mavlink_mission_item->param3;
@@ -792,50 +851,44 @@ MavlinkMissionManager::parse_mavlink_mission_item(const mavlink_mission_item_t *
mission_item->params[4] = mavlink_mission_item->x;
mission_item->params[5] = mavlink_mission_item->y;
mission_item->params[6] = mavlink_mission_item->z;
break;
default:
return MAV_MISSION_UNSUPPORTED_FRAME;
}
switch (mavlink_mission_item->command) {
case MAV_CMD_DO_JUMP:
mission_item->nav_cmd = NAV_CMD_DO_JUMP;
mission_item->do_jump_mission_index = mavlink_mission_item->param1;
mission_item->do_jump_current_count = 0;
mission_item->do_jump_repeat_count = mavlink_mission_item->param2;
break;
mission_item->frame = mavlink_mission_item->frame;
case MAV_CMD_DO_CHANGE_SPEED:
case MAV_CMD_DO_SET_SERVO:
case MAV_CMD_DO_DIGICAM_CONTROL:
case MAV_CMD_DO_MOUNT_CONFIGURE:
case MAV_CMD_DO_MOUNT_CONTROL:
case MAV_CMD_DO_SET_CAM_TRIGG_DIST:
case MAV_CMD_DO_VTOL_TRANSITION:
mission_item->nav_cmd = (NAV_CMD)mavlink_mission_item->command;
break;
switch (mavlink_mission_item->command) {
case MAV_CMD_NAV_TAKEOFF:
mission_item->pitch_min = mavlink_mission_item->param1;
break;
default:
mission_item->nav_cmd = NAV_CMD_INVALID;
return MAV_MISSION_UNSUPPORTED;
}
case MAV_CMD_DO_JUMP:
mission_item->do_jump_mission_index = mavlink_mission_item->param1;
mission_item->do_jump_current_count = 0;
mission_item->do_jump_repeat_count = mavlink_mission_item->param2;
break;
default:
mission_item->acceptance_radius = mavlink_mission_item->param2;
mission_item->time_inside = mavlink_mission_item->param1;
break;
}
mission_item->yaw = _wrap_pi(mavlink_mission_item->param4 * M_DEG_TO_RAD_F);
mission_item->loiter_radius = fabsf(mavlink_mission_item->param3);
mission_item->loiter_direction = (mavlink_mission_item->param3 > 0) ? 1 : -1; /* 1 if positive CW, -1 if negative CCW */
/* unsigned, so can't be negative, only check for overflow */
if (mavlink_mission_item->command >= NAV_CMD_INVALID) {
mission_item->nav_cmd = NAV_CMD_INVALID;
mission_item->frame = MAV_FRAME_MISSION;
} else {
mission_item->nav_cmd = (NAV_CMD)mavlink_mission_item->command;
return MAV_MISSION_UNSUPPORTED_FRAME;
}
mission_item->autocontinue = mavlink_mission_item->autocontinue;
// mission_item->index = mavlink_mission_item->seq;
mission_item->origin = ORIGIN_MAVLINK;
/* reset DO_JUMP count */
mission_item->do_jump_current_count = 0;
mission_item->origin = ORIGIN_MAVLINK;
return MAV_MISSION_ACCEPTED;
}
@@ -857,32 +910,82 @@ MavlinkMissionManager::format_mavlink_mission_item(const struct mission_item_s *
mavlink_mission_item->y = mission_item->params[5];
mavlink_mission_item->z = mission_item->params[6];
// default mappings for regular waypoints
switch (mavlink_mission_item->command) {
case NAV_CMD_DO_JUMP:
mavlink_mission_item->param1 = mission_item->do_jump_mission_index;
mavlink_mission_item->param2 = mission_item->do_jump_repeat_count;
break;
case NAV_CMD_DO_CHANGE_SPEED:
case NAV_CMD_DO_SET_SERVO:
case NAV_CMD_DO_DIGICAM_CONTROL:
case NAV_CMD_DO_MOUNT_CONFIGURE:
case NAV_CMD_DO_MOUNT_CONTROL:
case NAV_CMD_DO_SET_CAM_TRIGG_DIST:
case NAV_CMD_DO_VTOL_TRANSITION:
break;
default:
return ERROR;
}
} else {
mavlink_mission_item->param1 = 0.0f;
mavlink_mission_item->param2 = 0.0f;
mavlink_mission_item->param3 = 0.0f;
mavlink_mission_item->param4 = 0.0f;
mavlink_mission_item->x = (float)mission_item->lat;
mavlink_mission_item->y = (float)mission_item->lon;
mavlink_mission_item->z = mission_item->altitude;
mavlink_mission_item->param4 = mission_item->yaw * M_RAD_TO_DEG_F;
mavlink_mission_item->param3 = mission_item->loiter_radius * (float)mission_item->loiter_direction;
}
if (mission_item->altitude_is_relative) {
mavlink_mission_item->frame = MAV_FRAME_GLOBAL_RELATIVE_ALT;
} else {
mavlink_mission_item->frame = MAV_FRAME_GLOBAL;
}
// specific item handling
switch (mission_item->nav_cmd) {
case NAV_CMD_TAKEOFF:
case NAV_CMD_VTOL_TAKEOFF:
mavlink_mission_item->param1 = mission_item->pitch_min;
break;
switch (mission_item->nav_cmd) {
case NAV_CMD_WAYPOINT:
mavlink_mission_item->param1 = mission_item->time_inside;
mavlink_mission_item->param2 = mission_item->acceptance_radius;
mavlink_mission_item->param4 = mission_item->yaw * M_RAD_TO_DEG_F;
break;
case NAV_CMD_DO_JUMP:
mavlink_mission_item->param1 = mission_item->do_jump_mission_index;
mavlink_mission_item->param2 = mission_item->do_jump_repeat_count - mission_item->do_jump_current_count;
break;
case NAV_CMD_LOITER_UNLIMITED:
mavlink_mission_item->param3 = mission_item->loiter_radius * (float)mission_item->loiter_direction;
mavlink_mission_item->param4 = mission_item->yaw * M_RAD_TO_DEG_F;
break;
default:
mavlink_mission_item->param2 = mission_item->acceptance_radius;
mavlink_mission_item->param1 = mission_item->time_inside;
break;
case NAV_CMD_LOITER_TIME_LIMIT:
mavlink_mission_item->param1 = mission_item->time_inside;
mavlink_mission_item->param3 = mission_item->loiter_radius * (float)mission_item->loiter_direction;
mavlink_mission_item->param4 = mission_item->loiter_exit_xtrack;
break;
case NAV_CMD_LAND:
// TODO: param1 abort alt
mavlink_mission_item->param4 = mission_item->yaw * M_RAD_TO_DEG_F;
break;
case NAV_CMD_TAKEOFF:
mavlink_mission_item->param1 = mission_item->pitch_min;
mavlink_mission_item->param4 = mission_item->yaw * M_RAD_TO_DEG_F;
break;
case NAV_CMD_LOITER_TO_ALT:
mavlink_mission_item->param1 = mission_item->force_heading;
mavlink_mission_item->param2 = mission_item->loiter_radius * (float)mission_item->loiter_direction;
mavlink_mission_item->param4 = mission_item->loiter_exit_xtrack;
break;
case MAV_CMD_NAV_VTOL_TAKEOFF:
case MAV_CMD_NAV_VTOL_LAND:
mavlink_mission_item->param4 = mission_item->yaw * M_RAD_TO_DEG_F;
break;
default:
return ERROR;
}
}
return OK;
src/modules/navigator/mission.cpp
+26 -12
View File
@@ -348,7 +348,7 @@ Mission::set_mission_items()
/* make sure param is up to date */
updateParams();
/* reset the altitude foh logic, if altitude foh is enabled (param) a new foh element starts now */
/* reset the altitude foh (first order hold) logic, if altitude foh is enabled (param) a new foh element starts now */
altitude_sp_foh_reset();
struct position_setpoint_triplet_s *pos_sp_triplet = _navigator->get_position_setpoint_triplet();
@@ -453,6 +453,7 @@ Mission::set_mission_items()
/* do takeoff before going to setpoint if needed and not already in takeoff */
if (do_need_takeoff() && _work_item_type != WORK_ITEM_TYPE_TAKEOFF) {
new_work_item_type = WORK_ITEM_TYPE_TAKEOFF;
/* use current mission item as next position item */
@@ -482,8 +483,9 @@ Mission::set_mission_items()
&& _navigator->get_vstatus()->is_rotary_wing
&& !_navigator->get_land_detected()->landed
&& has_next_position_item) {
/* check if the vtol_takeoff command is on top of us */
if(do_need_move_to_takeoff()){
if (do_need_move_to_takeoff()){
new_work_item_type = WORK_ITEM_TYPE_TRANSITON_AFTER_TAKEOFF;
} else {
new_work_item_type = WORK_ITEM_TYPE_DEFAULT;
@@ -516,6 +518,7 @@ Mission::set_mission_items()
if (_mission_item.nav_cmd == NAV_CMD_VTOL_LAND
&& _work_item_type == WORK_ITEM_TYPE_DEFAULT
&& !_navigator->get_land_detected()->landed) {
new_work_item_type = WORK_ITEM_TYPE_MOVE_TO_LAND;
/* use current mission item as next position item */
memcpy(&mission_item_next_position, &_mission_item, sizeof(struct mission_item_s));
@@ -537,6 +540,7 @@ Mission::set_mission_items()
&& _work_item_type == WORK_ITEM_TYPE_MOVE_TO_LAND
&& !_navigator->get_vstatus()->is_rotary_wing
&& !_navigator->get_land_detected()->landed) {
_mission_item.nav_cmd = NAV_CMD_DO_VTOL_TRANSITION;
_mission_item.params[0] = vtol_vehicle_status_s::VEHICLE_VTOL_STATE_MC;
_mission_item.autocontinue = true;
@@ -547,6 +551,7 @@ Mission::set_mission_items()
if (do_need_move_to_land() &&
(_work_item_type == WORK_ITEM_TYPE_DEFAULT ||
_work_item_type == WORK_ITEM_TYPE_MOVE_TO_LAND_AFTER_TRANSITION)) {
new_work_item_type = WORK_ITEM_TYPE_MOVE_TO_LAND;
/* use current mission item as next position item */
@@ -572,6 +577,7 @@ Mission::set_mission_items()
/* we just moved to the landing waypoint, now descend */
if (_work_item_type == WORK_ITEM_TYPE_MOVE_TO_LAND
&& _navigator->get_vstatus()->is_rotary_wing) {
new_work_item_type = WORK_ITEM_TYPE_DEFAULT;
}
@@ -607,7 +613,7 @@ Mission::set_mission_items()
/*********************************** set setpoints and check next *********************************************/
/* set current position setpoint from mission item (is protected agains non-position items) */
/* set current position setpoint from mission item (is protected against non-position items) */
mission_item_to_position_setpoint(&_mission_item, &pos_sp_triplet->current);
/* issue command if ready (will do nothing for position mission items) */
@@ -617,7 +623,9 @@ Mission::set_mission_items()
_work_item_type = new_work_item_type;
/* require takeoff after landing or idle */
if (pos_sp_triplet->current.type == position_setpoint_s::SETPOINT_TYPE_LAND || pos_sp_triplet->current.type == position_setpoint_s::SETPOINT_TYPE_IDLE) {
if (pos_sp_triplet->current.type == position_setpoint_s::SETPOINT_TYPE_LAND
|| pos_sp_triplet->current.type == position_setpoint_s::SETPOINT_TYPE_IDLE) {
_need_takeoff = true;
}
@@ -647,6 +655,7 @@ Mission::set_mission_items()
/* Save the distance between the current sp and the previous one */
if (pos_sp_triplet->current.valid && pos_sp_triplet->previous.valid) {
_distance_current_previous = get_distance_to_next_waypoint(
pos_sp_triplet->current.lat,
pos_sp_triplet->current.lon,
@@ -661,6 +670,7 @@ bool
Mission::do_need_takeoff()
{
if (_navigator->get_vstatus()->is_rotary_wing) {
float takeoff_alt = calculate_takeoff_altitude(&_mission_item);
/* force takeoff if landed (additional protection) */
@@ -674,13 +684,12 @@ Mission::do_need_takeoff()
/* check if current mission item is one that requires takeoff before */
if (_need_takeoff && (
_mission_item.nav_cmd == NAV_CMD_TAKEOFF ||
_mission_item.nav_cmd == NAV_CMD_WAYPOINT ||
_mission_item.nav_cmd == NAV_CMD_VTOL_TAKEOFF ||
_mission_item.nav_cmd == NAV_CMD_LOITER_TIME_LIMIT ||
_mission_item.nav_cmd == NAV_CMD_LOITER_TURN_COUNT ||
_mission_item.nav_cmd == NAV_CMD_LOITER_UNLIMITED ||
_mission_item.nav_cmd == NAV_CMD_RETURN_TO_LAUNCH)) {
_mission_item.nav_cmd == NAV_CMD_LOITER_TIME_LIMIT ||
_mission_item.nav_cmd == NAV_CMD_TAKEOFF ||
_mission_item.nav_cmd == NAV_CMD_LOITER_TO_ALT ||
_mission_item.nav_cmd == NAV_CMD_VTOL_TAKEOFF)) {
_need_takeoff = false;
return true;
@@ -693,7 +702,8 @@ Mission::do_need_takeoff()
bool
Mission::do_need_move_to_land()
{
if (_navigator->get_vstatus()->is_rotary_wing && (_mission_item.nav_cmd == NAV_CMD_LAND || _mission_item.nav_cmd == NAV_CMD_VTOL_LAND)) {
if (_navigator->get_vstatus()->is_rotary_wing
&& (_mission_item.nav_cmd == NAV_CMD_LAND || _mission_item.nav_cmd == NAV_CMD_VTOL_LAND)) {
float d_current = get_distance_to_next_waypoint(_mission_item.lat, _mission_item.lon,
_navigator->get_global_position()->lat, _navigator->get_global_position()->lon);
@@ -757,7 +767,7 @@ Mission::calculate_takeoff_altitude(struct mission_item_s *mission_item)
/* calculate takeoff altitude */
float takeoff_alt = get_absolute_altitude_for_item(*mission_item);
/* takeoff to at least NAV_TAKEOFF_ALT above home/ground, even if first waypoint is lower */
/* takeoff to at least MIS_TAKEOFF_ALT above home/ground, even if first waypoint is lower */
if (_navigator->get_land_detected()->landed) {
takeoff_alt = fmaxf(takeoff_alt, _navigator->get_global_position()->alt + _param_takeoff_alt.get());
@@ -866,10 +876,14 @@ Mission::altitude_sp_foh_update()
}
/* Don't do FOH for non-missions, landing and takeoff waypoints, the ground may be near
* and the FW controller has a custom landing logic */
* and the FW controller has a custom landing logic
*
* NAV_CMD_LOITER_TO_ALT doesn't change altitude until reaching desired lat/lon
* */
if (_mission_item.nav_cmd == NAV_CMD_LAND
|| _mission_item.nav_cmd == NAV_CMD_VTOL_LAND
|| _mission_item.nav_cmd == NAV_CMD_TAKEOFF
|| _mission_item.nav_cmd == NAV_CMD_LOITER_TO_ALT
|| !_navigator->is_planned_mission()) {
return;
}
src/modules/navigator/mission_block.cpp
+97 -25
View File
@@ -75,6 +75,7 @@ MissionBlock::MissionBlock(Navigator *navigator, const char *name) :
_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),
@@ -98,13 +99,13 @@ MissionBlock::is_mission_item_reached()
case NAV_CMD_VTOL_LAND:
return _navigator->get_land_detected()->landed;
/* TODO: count turns */
/*_mission_item.nav_cmd == NAV_CMD_LOITER_TURN_COUNT ||*/
case NAV_CMD_IDLE: /* fall through */
case NAV_CMD_LOITER_UNLIMITED:
return false;
case NAV_CMD_DO_DIGICAM_CONTROL:
case NAV_CMD_DO_MOUNT_CONFIGURE:
case NAV_CMD_DO_MOUNT_CONTROL:
case NAV_CMD_DO_SET_CAM_TRIGG_DIST:
return true;
@@ -115,13 +116,14 @@ MissionBlock::is_mission_item_reached()
*/
if (hrt_absolute_time() - _action_start > 500000 &&
!_navigator->get_vstatus()->in_transition_mode) {
_action_start = 0;
return true;
} else {
return false;
}
case vehicle_command_s::VEHICLE_CMD_DO_CHANGE_SPEED:
case NAV_CMD_DO_CHANGE_SPEED:
// XXX not differentiating ground and airspeed yet
if (_mission_item.params[1] > 0.0f) {
_navigator->set_cruising_speed(_mission_item.params[1]);
@@ -152,14 +154,14 @@ MissionBlock::is_mission_item_reached()
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);
if ((_mission_item.nav_cmd == NAV_CMD_TAKEOFF || _mission_item.nav_cmd == NAV_CMD_VTOL_TAKEOFF)
&& _navigator->get_vstatus()->is_rotary_wing) {
@@ -176,8 +178,7 @@ MissionBlock::is_mission_item_reached()
}
} 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_TURN_COUNT)) {
_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.
@@ -186,7 +187,61 @@ MissionBlock::is_mission_item_reached()
*/
if (dist >= 0.0f && dist <= _navigator->get_acceptance_radius(_mission_item.loiter_radius * 1.2f)
&& dist_z <= _navigator->get_default_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)) {
// NAV_CMD_LOITER_TO_ALT only uses mission item altitude once it's in the loiter
// first check if the altitude setpoint is the mission setpoint
struct position_setpoint_s *curr_sp = &_navigator->get_position_setpoint_triplet()->current;
if (fabsf(curr_sp->alt - altitude_amsl) >= FLT_EPSILON) {
// check if the initial loiter has been accepted
dist = -1.0f;
dist_xy = -1.0f;
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);
if (dist >= 0.0f && dist <= _navigator->get_acceptance_radius(_mission_item.loiter_radius * 1.2f)
&& dist_z <= _navigator->get_default_acceptance_radius()) {
// now set the loiter to the final altitude in the NAV_CMD_LOITER_TO_ALT mission item
curr_sp->alt = altitude_amsl;
_navigator->set_position_setpoint_triplet_updated();
}
} else {
if (dist >= 0.0f && dist <= _navigator->get_acceptance_radius(_mission_item.loiter_radius * 1.2f)
&& dist_z <= _navigator->get_default_acceptance_radius()) {
_waypoint_position_reached = true;
// set required yaw from bearing to the next mission item
if (_mission_item.force_heading) {
struct position_setpoint_s next_sp = _navigator->get_position_setpoint_triplet()->next;
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);
_waypoint_yaw_reached = false;
} else {
_waypoint_yaw_reached = true;
}
}
}
}
} else {
/* for normal mission items used their acceptance radius */
@@ -213,8 +268,9 @@ MissionBlock::is_mission_item_reached()
if (_waypoint_position_reached && !_waypoint_yaw_reached) {
/* TODO: removed takeoff, why? */
if (_navigator->get_vstatus()->is_rotary_wing && PX4_ISFINITE(_mission_item.yaw)) {
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)) {
/* check yaw if defined only for rotary wing except takeoff */
float yaw_err = _wrap_pi(_mission_item.yaw - _navigator->get_global_position()->yaw);
@@ -242,18 +298,29 @@ MissionBlock::is_mission_item_reached()
if (_time_first_inside_orbit == 0) {
_time_first_inside_orbit = now;
// if (_mission_item.time_inside > 0.01f) {
// mavlink_log_critical(_mavlink_log_pub, "waypoint reached, wait for %.1fs",
// (double)_mission_item.time_inside);
// }
}
/* check if the MAV was long enough inside the waypoint orbit */
if (now - _time_first_inside_orbit >= (hrt_abstime)_mission_item.time_inside * 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)) {
// reset lat/lon of loiter waypoint so vehicle exits on a tangent
struct position_setpoint_s *curr_sp = &_navigator->get_position_setpoint_triplet()->current;
curr_sp->lat = _navigator->get_global_position()->lat;
curr_sp->lon = _navigator->get_global_position()->lon;
}
return true;
}
}
// all acceptance criteria must be met in the same iteration
_waypoint_position_reached = false;
_waypoint_yaw_reached = false;
return false;
}
@@ -298,7 +365,7 @@ MissionBlock::issue_command(const struct mission_item_s *item)
PX4_WARN("do_set_servo command");
// XXX: we should issue a vehicle command and handle this somewhere else
memset(&actuators, 0, sizeof(actuators));
// params[0] actuator number to be set 0..5 ( corresponds to AUX outputs 1..6
// params[0] actuator number to be set 0..5 (corresponds to AUX outputs 1..6)
// params[1] new value for selected actuator in ms 900...2000
actuators.control[(int)item->params[0]] = 1.0f / 2000 * -item->params[1];
actuators.timestamp = hrt_absolute_time();
@@ -332,8 +399,9 @@ MissionBlock::item_contains_position(const struct mission_item_s *item)
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_REPEAT_SERVO ||
item->nav_cmd == NAV_CMD_DO_DIGICAM_CONTROL ||
item->nav_cmd == NAV_CMD_DO_MOUNT_CONFIGURE ||
item->nav_cmd == NAV_CMD_DO_MOUNT_CONTROL ||
item->nav_cmd == NAV_CMD_DO_SET_CAM_TRIGG_DIST ||
item->nav_cmd == NAV_CMD_DO_VTOL_TRANSITION) {
return false;
@@ -349,6 +417,7 @@ MissionBlock::mission_item_to_position_setpoint(const struct mission_item_s *ite
if (item->nav_cmd == NAV_CMD_DO_VTOL_TRANSITION && PX4_ISFINITE(item->yaw)
&& item->params[0] >= vtol_vehicle_status_s::VEHICLE_VTOL_STATE_FW - 0.5f) {
sp->type = position_setpoint_s::SETPOINT_TYPE_POSITION;
waypoint_from_heading_and_distance(_navigator->get_global_position()->lat,
_navigator->get_global_position()->lon,
@@ -365,7 +434,6 @@ MissionBlock::mission_item_to_position_setpoint(const struct mission_item_s *ite
return;
}
sp->valid = true;
sp->lat = item->lat;
sp->lon = item->lon;
sp->alt = item->altitude_is_relative ? item->altitude + _navigator->get_home_position()->alt : item->altitude;
@@ -397,8 +465,12 @@ MissionBlock::mission_item_to_position_setpoint(const struct mission_item_s *ite
}
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());
// no break
case NAV_CMD_LOITER_TIME_LIMIT:
case NAV_CMD_LOITER_TURN_COUNT:
case NAV_CMD_LOITER_UNLIMITED:
sp->type = position_setpoint_s::SETPOINT_TYPE_LOITER;
if (_navigator->get_vstatus()->is_vtol && _param_vtol_wv_loiter.get()) {
@@ -410,6 +482,8 @@ MissionBlock::mission_item_to_position_setpoint(const struct mission_item_s *ite
sp->type = position_setpoint_s::SETPOINT_TYPE_POSITION;
break;
}
sp->valid = true;
}
void
@@ -472,7 +546,7 @@ MissionBlock::set_follow_target_item(struct mission_item_s *item, float min_clea
} else {
item->nav_cmd = NAV_CMD_FOLLOW_TARGET;
item->nav_cmd = NAV_CMD_DO_FOLLOW_REPOSITION;
/* use current target position */
@@ -605,5 +679,3 @@ MissionBlock::set_idle_item(struct mission_item_s *item)
item->autocontinue = true;
item->origin = ORIGIN_ONBOARD;
}
src/modules/navigator/mission_block.h
+1
View File
@@ -141,6 +141,7 @@ protected:
orb_advert_t _actuator_pub;
orb_advert_t _cmd_pub;
control::BlockParamFloat _param_loiter_min_alt;
control::BlockParamFloat _param_yaw_timeout;
control::BlockParamFloat _param_yaw_err;
control::BlockParamInt _param_vtol_wv_land;
src/modules/navigator/mission_feasibility_checker.cpp
+11 -8
View File
@@ -238,17 +238,18 @@ bool MissionFeasibilityChecker::checkMissionItemValidity(dm_item_t dm_current, s
if (missionitem.nav_cmd != NAV_CMD_IDLE &&
missionitem.nav_cmd != NAV_CMD_WAYPOINT &&
missionitem.nav_cmd != NAV_CMD_LOITER_UNLIMITED &&
/* not yet supported: missionitem.nav_cmd != NAV_CMD_LOITER_TURN_COUNT && */
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_VTOL_LAND &&
missionitem.nav_cmd != NAV_CMD_LOITER_TO_ALT &&
missionitem.nav_cmd != NAV_CMD_VTOL_TAKEOFF &&
missionitem.nav_cmd != NAV_CMD_PATHPLANNING &&
missionitem.nav_cmd != NAV_CMD_VTOL_LAND &&
missionitem.nav_cmd != NAV_CMD_DO_JUMP &&
missionitem.nav_cmd != NAV_CMD_DO_SET_SERVO &&
missionitem.nav_cmd != NAV_CMD_DO_CHANGE_SPEED &&
missionitem.nav_cmd != NAV_CMD_DO_DIGICAM_CONTROL &&
missionitem.nav_cmd != NAV_CMD_DO_MOUNT_CONFIGURE &&
missionitem.nav_cmd != NAV_CMD_DO_MOUNT_CONTROL &&
missionitem.nav_cmd != NAV_CMD_DO_SET_CAM_TRIGG_DIST &&
missionitem.nav_cmd != NAV_CMD_DO_VTOL_TRANSITION) {
@@ -407,13 +408,15 @@ MissionFeasibilityChecker::check_dist_1wp(dm_item_t dm_current, size_t nMissionI
bool
MissionFeasibilityChecker::isPositionCommand(unsigned cmd){
if( cmd == NAV_CMD_WAYPOINT ||
cmd == NAV_CMD_LOITER_TIME_LIMIT ||
cmd == NAV_CMD_LOITER_TURN_COUNT ||
if (cmd == NAV_CMD_WAYPOINT ||
cmd == NAV_CMD_LOITER_UNLIMITED ||
cmd == NAV_CMD_TAKEOFF ||
cmd == NAV_CMD_LOITER_TIME_LIMIT ||
cmd == NAV_CMD_LAND ||
cmd == NAV_CMD_PATHPLANNING) {
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;
src/modules/navigator/navigation.h
+6 -7
View File
@@ -54,21 +54,19 @@ enum NAV_CMD {
NAV_CMD_IDLE = 0,
NAV_CMD_WAYPOINT = 16,
NAV_CMD_LOITER_UNLIMITED = 17,
NAV_CMD_LOITER_TURN_COUNT = 18,
NAV_CMD_LOITER_TIME_LIMIT = 19,
NAV_CMD_RETURN_TO_LAUNCH = 20,
NAV_CMD_LAND = 21,
NAV_CMD_TAKEOFF = 22,
NAV_CMD_ROI = 80,
NAV_CMD_PATHPLANNING = 81,
NAV_CMD_LOITER_TO_ALT = 31,
NAV_CMD_DO_FOLLOW_REPOSITION = 33,
NAV_CMD_VTOL_TAKEOFF = 84,
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_REPEAT_SERVO=184,
NAV_CMD_FOLLOW_TARGET = 194, // temporary placeholder
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_DO_VTOL_TRANSITION=3000,
NAV_CMD_INVALID=UINT16_MAX /* ensure that casting a large number results in a specific error */
@@ -99,7 +97,8 @@ struct mission_item_s {
float yaw; /**< in radians NED -PI..+PI, NAN means don't change yaw */
float loiter_radius; /**< loiter radius in meters, 0 for a VTOL to hover */
int8_t loiter_direction; /**< 1: positive / clockwise, -1, negative. */
unsigned nav_cmd; /**< navigation command */
bool loiter_exit_xtrack; /**< exit xtrack location: 0 for center of loiter wp, 1 for exit location */
enum NAV_CMD nav_cmd; /**< navigation command */
float acceptance_radius; /**< default radius in which the mission is accepted as reached in meters */
float time_inside; /**< time that the MAV should stay inside the radius before advancing in seconds */
float pitch_min; /**< minimal pitch angle for fixed wing takeoff waypoints */