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

mission heading_sp_update: remove everthing except of MC and ROI logic.

sbg
Dennis Mannhart 7 years ago committed by Dennis Mannhart
parent
a3ca016c1d
commit
e61131cf9c
1 changed files with 64 additions and 101 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. 165
      src/modules/navigator/mission.cpp

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

@ -246,15 +246,23 @@ Mission::on_active() @@ -246,15 +246,23 @@ Mission::on_active()
}
/* see if we need to update the current yaw heading */
if (_navigator->get_vroi().mode == vehicle_roi_s::ROI_LOCATION
|| (_param_yawmode.get() != MISSION_YAWMODE_NONE
&& _param_yawmode.get() < MISSION_YAWMODE_MAX
&& _mission_type != MISSION_TYPE_NONE)
|| _navigator->get_vstatus()->is_vtol) {
if (!_param_mnt_yaw_ctl.get() && (_navigator->get_vstatus()->is_rotary_wing)
&& (_navigator->get_vroi().mode != vehicle_roi_s::ROI_NONE)
&& !(_mission_item.nav_cmd == NAV_CMD_TAKEOFF
|| _mission_item.nav_cmd == NAV_CMD_VTOL_TAKEOFF
|| _mission_item.nav_cmd == NAV_CMD_DO_VTOL_TRANSITION
|| _mission_item.nav_cmd == NAV_CMD_LAND
|| _mission_item.nav_cmd == NAV_CMD_VTOL_LAND
|| _work_item_type == WORK_ITEM_TYPE_ALIGN)) {
// Mount control is disabled If the vehicle is in ROI-mode, the vehicle
// needs to rotate such that ROI is in the field of view.
// ROI only makes sense for multicopters.
heading_sp_update();
}
// TODO: Add vtol heading update method if required.
// Question: Why does vtol ever have to update heading?
/* check if landing needs to be aborted */
if ((_mission_item.nav_cmd == NAV_CMD_LAND)
&& (_navigator->abort_landing())) {
@ -1170,112 +1178,67 @@ Mission::calculate_takeoff_altitude(struct mission_item_s *mission_item) @@ -1170,112 +1178,67 @@ Mission::calculate_takeoff_altitude(struct mission_item_s *mission_item)
void
Mission::heading_sp_update()
{
/* we don't want to be yawing during takeoff, landing or aligning for a transition */
if (_mission_item.nav_cmd == NAV_CMD_TAKEOFF
|| _mission_item.nav_cmd == NAV_CMD_VTOL_TAKEOFF
|| _mission_item.nav_cmd == NAV_CMD_DO_VTOL_TRANSITION
|| _mission_item.nav_cmd == NAV_CMD_LAND
|| _mission_item.nav_cmd == NAV_CMD_VTOL_LAND
|| _work_item_type == WORK_ITEM_TYPE_ALIGN) {
return;
}
struct position_setpoint_triplet_s *pos_sp_triplet = _navigator->get_position_setpoint_triplet();
/* Don't change setpoint if last and current waypoint are not valid */
if (!pos_sp_triplet->current.valid) {
return;
}
/* Calculate direction the vehicle should point to. */
double point_from_latlon[2];
double point_to_latlon[2];
struct position_setpoint_triplet_s *pos_sp_triplet =
_navigator->get_position_setpoint_triplet();
// Only update if current triplet is valid
if (pos_sp_triplet->current.valid) {
double point_from_latlon[2] = { _navigator->get_global_position()->lat,
_navigator->get_global_position()->lon
};
double point_to_latlon[2] = { _navigator->get_global_position()->lat,
_navigator->get_global_position()->lon
};
float yaw_offset = 0.0f;
// Depending on ROI-mode, update heading
switch (_navigator->get_vroi().mode) {
case vehicle_roi_s::ROI_LOCATION: {
// ROI is a fixed location. Vehicle needs to point towards that location
point_to_latlon[0] = _navigator->get_vroi().lat;
point_to_latlon[1] = _navigator->get_vroi().lon;
// No yaw offset required
yaw_offset = 0.0f;
break;
}
point_from_latlon[0] = _navigator->get_global_position()->lat;
point_from_latlon[1] = _navigator->get_global_position()->lon;
case vehicle_roi_s::ROI_WPNEXT: {
// ROI is current waypoint. Vehcile needs to point towards current waypoint
point_to_latlon[0] = pos_sp_triplet->current.lat;
point_to_latlon[1] = pos_sp_triplet->current.lon;
// Add the gimbal's yaw offset
yaw_offset = _navigator->get_vroi().yaw_offset;
break;
}
if (_navigator->get_vroi().mode == vehicle_roi_s::ROI_LOCATION && !_param_mnt_yaw_ctl.get()) {
point_to_latlon[0] = _navigator->get_vroi().lat;
point_to_latlon[1] = _navigator->get_vroi().lon;
case vehicle_roi_s::ROI_NONE:
case vehicle_roi_s::ROI_WPINDEX:
case vehicle_roi_s::ROI_TARGET:
case vehicle_roi_s::ROI_ENUM_END:
default: {
}
}
/* stop if positions are close together to prevent excessive yawing */
float d_current = get_distance_to_next_waypoint(
point_from_latlon[0], point_from_latlon[1],
point_to_latlon[0], point_to_latlon[1]);
// Get desired heading and update it.
// However, only update if distance to desired heading is
// larger than acceptance radius to prevent excessive yawing
float d_current = get_distance_to_next_waypoint(point_from_latlon[0],
point_from_latlon[1], point_to_latlon[0], point_to_latlon[1]);
if (d_current > _navigator->get_acceptance_radius()) {
float yaw = get_bearing_to_next_waypoint(
point_from_latlon[0], point_from_latlon[1],
point_to_latlon[0], point_to_latlon[1]);
float yaw = _wrap_pi(
get_bearing_to_next_waypoint(point_from_latlon[0],
point_from_latlon[1], point_to_latlon[0],
point_to_latlon[1]) + yaw_offset);
_mission_item.yaw = yaw;
pos_sp_triplet->current.yaw = _mission_item.yaw;
}
} else {
/* set yaw angle for the waypoint if a loiter time has been specified */
if (_waypoint_position_reached && get_time_inside(_mission_item) > FLT_EPSILON) {
// XXX: should actually be param4 from mission item
// at the moment it will just keep the heading it has
//_mission_item.yaw = _on_arrival_yaw;
//pos_sp_triplet->current.yaw = _mission_item.yaw;
} else {
/* target location is home */
if ((_param_yawmode.get() == MISSION_YAWMODE_FRONT_TO_HOME
|| _param_yawmode.get() == MISSION_YAWMODE_BACK_TO_HOME)
// need to be rotary wing for this but not in a transition
// in VTOL mode this will prevent updating yaw during FW flight
// (which would result in a wrong yaw setpoint spike during back transition)
&& _navigator->get_vstatus()->is_rotary_wing
&& !(_mission_item.nav_cmd == NAV_CMD_DO_VTOL_TRANSITION || _navigator->get_vstatus()->in_transition_mode)) {
point_to_latlon[0] = _navigator->get_home_position()->lat;
point_to_latlon[1] = _navigator->get_home_position()->lon;
} else {
/* target location is next (current) waypoint */
point_to_latlon[0] = pos_sp_triplet->current.lat;
point_to_latlon[1] = pos_sp_triplet->current.lon;
}
float d_current = get_distance_to_next_waypoint(
point_from_latlon[0], point_from_latlon[1],
point_to_latlon[0], point_to_latlon[1]);
/* stop if positions are close together to prevent excessive yawing */
if (d_current > _navigator->get_acceptance_radius()) {
float yaw = get_bearing_to_next_waypoint(
point_from_latlon[0],
point_from_latlon[1],
point_to_latlon[0],
point_to_latlon[1]);
/* always keep the back of the rotary wing pointing towards home */
if (_param_yawmode.get() == MISSION_YAWMODE_BACK_TO_HOME) {
_mission_item.yaw = wrap_pi(yaw + M_PI_F);
pos_sp_triplet->current.yaw = _mission_item.yaw;
} else if (_param_yawmode.get() == MISSION_YAWMODE_FRONT_TO_WAYPOINT
&& _navigator->get_vroi().mode == vehicle_roi_s::ROI_WPNEXT && !_param_mnt_yaw_ctl.get()) {
/* if yaw control for the mount is disabled and we have a valid ROI that points to the next
* waypoint, we add the gimbal's yaw offset to the vehicle's yaw */
yaw += _navigator->get_vroi().yaw_offset;
_mission_item.yaw = yaw;
pos_sp_triplet->current.yaw = _mission_item.yaw;
} else {
_mission_item.yaw = yaw;
pos_sp_triplet->current.yaw = _mission_item.yaw;
}
}
}
// we set yaw directly so we can run this in parallel to the FOH update
_navigator->set_position_setpoint_triplet_updated();
}
// we set yaw directly so we can run this in parallel to the FOH update
_navigator->set_position_setpoint_triplet_updated();
}
void

Write Preview
Loading…
Cancel
Save