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
commit
e61131cf9c
  1. 153
      src/modules/navigator/mission.cpp

153
src/modules/navigator/mission.cpp

@ -246,15 +246,23 @@ Mission::on_active()
} }
/* see if we need to update the current yaw heading */ /* see if we need to update the current yaw heading */
if (_navigator->get_vroi().mode == vehicle_roi_s::ROI_LOCATION if (!_param_mnt_yaw_ctl.get() && (_navigator->get_vstatus()->is_rotary_wing)
|| (_param_yawmode.get() != MISSION_YAWMODE_NONE && (_navigator->get_vroi().mode != vehicle_roi_s::ROI_NONE)
&& _param_yawmode.get() < MISSION_YAWMODE_MAX && !(_mission_item.nav_cmd == NAV_CMD_TAKEOFF
&& _mission_type != MISSION_TYPE_NONE) || _mission_item.nav_cmd == NAV_CMD_VTOL_TAKEOFF
|| _navigator->get_vstatus()->is_vtol) { || _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(); 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 */ /* check if landing needs to be aborted */
if ((_mission_item.nav_cmd == NAV_CMD_LAND) if ((_mission_item.nav_cmd == NAV_CMD_LAND)
&& (_navigator->abort_landing())) { && (_navigator->abort_landing())) {
@ -1170,112 +1178,67 @@ Mission::calculate_takeoff_altitude(struct mission_item_s *mission_item)
void void
Mission::heading_sp_update() Mission::heading_sp_update()
{ {
/* we don't want to be yawing during takeoff, landing or aligning for a transition */ struct position_setpoint_triplet_s *pos_sp_triplet =
if (_mission_item.nav_cmd == NAV_CMD_TAKEOFF _navigator->get_position_setpoint_triplet();
|| _mission_item.nav_cmd == NAV_CMD_VTOL_TAKEOFF
|| _mission_item.nav_cmd == NAV_CMD_DO_VTOL_TRANSITION // Only update if current triplet is valid
|| _mission_item.nav_cmd == NAV_CMD_LAND if (pos_sp_triplet->current.valid) {
|| _mission_item.nav_cmd == NAV_CMD_VTOL_LAND
|| _work_item_type == WORK_ITEM_TYPE_ALIGN) { double point_from_latlon[2] = { _navigator->get_global_position()->lat,
_navigator->get_global_position()->lon
return; };
} double point_to_latlon[2] = { _navigator->get_global_position()->lat,
_navigator->get_global_position()->lon
struct position_setpoint_triplet_s *pos_sp_triplet = _navigator->get_position_setpoint_triplet(); };
float yaw_offset = 0.0f;
/* Don't change setpoint if last and current waypoint are not valid */
if (!pos_sp_triplet->current.valid) { // Depending on ROI-mode, update heading
return; switch (_navigator->get_vroi().mode) {
} case vehicle_roi_s::ROI_LOCATION: {
// ROI is a fixed location. Vehicle needs to point towards that location
/* Calculate direction the vehicle should point to. */
double point_from_latlon[2];
double point_to_latlon[2];
point_from_latlon[0] = _navigator->get_global_position()->lat;
point_from_latlon[1] = _navigator->get_global_position()->lon;
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[0] = _navigator->get_vroi().lat;
point_to_latlon[1] = _navigator->get_vroi().lon; point_to_latlon[1] = _navigator->get_vroi().lon;
// No yaw offset required
/* stop if positions are close together to prevent excessive yawing */ yaw_offset = 0.0f;
float d_current = get_distance_to_next_waypoint( break;
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]);
_mission_item.yaw = yaw;
pos_sp_triplet->current.yaw = _mission_item.yaw;
} }
} else { case vehicle_roi_s::ROI_WPNEXT: {
/* set yaw angle for the waypoint if a loiter time has been specified */ // ROI is current waypoint. Vehcile needs to point towards current waypoint
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[0] = pos_sp_triplet->current.lat;
point_to_latlon[1] = pos_sp_triplet->current.lon; point_to_latlon[1] = pos_sp_triplet->current.lon;
// Add the gimbal's yaw offset
yaw_offset = _navigator->get_vroi().yaw_offset;
break;
} }
float d_current = get_distance_to_next_waypoint( case vehicle_roi_s::ROI_NONE:
point_from_latlon[0], point_from_latlon[1], case vehicle_roi_s::ROI_WPINDEX:
point_to_latlon[0], point_to_latlon[1]); case vehicle_roi_s::ROI_TARGET:
case vehicle_roi_s::ROI_ENUM_END:
default: {
}
}
/* stop if positions are close together to prevent excessive yawing */ // Get desired heading and update it.
if (d_current > _navigator->get_acceptance_radius()) { // However, only update if distance to desired heading is
float yaw = get_bearing_to_next_waypoint( // larger than acceptance radius to prevent excessive yawing
point_from_latlon[0], float d_current = get_distance_to_next_waypoint(point_from_latlon[0],
point_from_latlon[1], point_from_latlon[1], point_to_latlon[0], point_to_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 if (d_current > _navigator->get_acceptance_radius()) {
&& _navigator->get_vroi().mode == vehicle_roi_s::ROI_WPNEXT && !_param_mnt_yaw_ctl.get()) { float yaw = _wrap_pi(
/* if yaw control for the mount is disabled and we have a valid ROI that points to the next get_bearing_to_next_waypoint(point_from_latlon[0],
* waypoint, we add the gimbal's yaw offset to the vehicle's yaw */ point_from_latlon[1], point_to_latlon[0],
yaw += _navigator->get_vroi().yaw_offset; point_to_latlon[1]) + yaw_offset);
_mission_item.yaw = yaw;
pos_sp_triplet->current.yaw = _mission_item.yaw;
} else {
_mission_item.yaw = yaw; _mission_item.yaw = yaw;
pos_sp_triplet->current.yaw = _mission_item.yaw; pos_sp_triplet->current.yaw = _mission_item.yaw;
} }
}
}
}
// we set yaw directly so we can run this in parallel to the FOH update // we set yaw directly so we can run this in parallel to the FOH update
_navigator->set_position_setpoint_triplet_updated(); _navigator->set_position_setpoint_triplet_updated();
}
} }
void void

Loading…
Cancel
Save