From 7ae3884944bc82cbdede9cf054062de732467c73 Mon Sep 17 00:00:00 2001 From: Nicolas de Palezieux Date: Thu, 4 Jan 2018 10:15:53 +0100 Subject: [PATCH] heading towards ROI: do not require setting the MIS_YAWMODE param to enable multicopters yawing towards ROIs ROI yaw control: configure yawing capability of mount in vmount parameters. If configured that mount has no yaw control, vehicle will yaw towards ROI, irrespective of MIS_YAWMODE. Vehicle will behave according to MIS_YAWMODE when there is no ROI. --- src/drivers/vmount/vmount_params.c | 16 +++- src/modules/navigator/mission.cpp | 110 +++++++++++++++---------- src/modules/navigator/mission.h | 6 +- src/modules/navigator/mission_params.c | 1 - 4 files changed, 83 insertions(+), 50 deletions(-) diff --git a/src/drivers/vmount/vmount_params.c b/src/drivers/vmount/vmount_params.c index 8ef37c4a28..8c97372cd3 100644 --- a/src/drivers/vmount/vmount_params.c +++ b/src/drivers/vmount/vmount_params.c @@ -224,4 +224,18 @@ PARAM_DEFINE_FLOAT(MNT_OFF_ROLL, 0.0f); * @decimal 1 * @group Mount */ -PARAM_DEFINE_FLOAT(MNT_OFF_YAW, 0.0f); \ No newline at end of file +PARAM_DEFINE_FLOAT(MNT_OFF_YAW, 0.0f); + +/** +* Enable yaw control of the mount. (Only affects multicopters and ROI mission items) +* +* If enabled, yaw commands will be sent to the mount and the vehicle will follow its heading mode as specified by MIS_YAWMODE. +* If disabled, the vehicle will yaw towards the ROI. +* +* @value 0 Disable +* @value 1 Enable +* @min 0 +* @max 1 +* @group Mount +*/ +PARAM_DEFINE_INT32(MNT_YAW_CTL, 0); diff --git a/src/modules/navigator/mission.cpp b/src/modules/navigator/mission.cpp index ed4e36846d..fe7145b39a 100644 --- a/src/modules/navigator/mission.cpp +++ b/src/modules/navigator/mission.cpp @@ -1,6 +1,6 @@ /**************************************************************************** * - * Copyright (c) 2013-2016 PX4 Development Team. All rights reserved. + * Copyright (c) 2013-2018 PX4 Development Team. All rights reserved. * * Redistribution and use in source and binary forms, with or without * modification, are permitted provided that the following conditions @@ -42,6 +42,7 @@ * @author Simon Wilks * @author Andreas Antener * @author Sander Smeets + * @author Lorenz Meier */ #include "mission.h" @@ -217,9 +218,10 @@ Mission::on_active() } /* see if we need to update the current yaw heading */ - if ((_param_yawmode.get() != MISSION_YAWMODE_NONE - && _param_yawmode.get() < MISSION_YAWMODE_MAX - && _mission_type != MISSION_TYPE_NONE) + 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) { heading_sp_update(); @@ -989,62 +991,80 @@ Mission::heading_sp_update() return; } - /* 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; + /* Calculate direction the vehicle should point to. */ - } else { - /* Calculate direction the vehicle should point to. */ - double point_from_latlon[2]; - double point_to_latlon[2]; + 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; + point_from_latlon[0] = _navigator->get_global_position()->lat; + point_from_latlon[1] = _navigator->get_global_position()->lon; - /* 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 if (_param_yawmode.get() == MISSION_YAWMODE_TO_ROI - && _navigator->get_vroi().mode == vehicle_roi_s::ROI_LOCATION) { - /* target location is ROI */ - point_to_latlon[0] = _navigator->get_vroi().lat; - point_to_latlon[1] = _navigator->get_vroi().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; - } + 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; + /* 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]); - /* 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; + _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 { - _mission_item.yaw = yaw; - pos_sp_triplet->current.yaw = _mission_item.yaw; + /* 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 { + _mission_item.yaw = yaw; + pos_sp_triplet->current.yaw = _mission_item.yaw; + } } } } diff --git a/src/modules/navigator/mission.h b/src/modules/navigator/mission.h index becbe3be15..e6493b869c 100644 --- a/src/modules/navigator/mission.h +++ b/src/modules/navigator/mission.h @@ -1,6 +1,6 @@ /**************************************************************************** * - * Copyright (c) 2013-2015 PX4 Development Team. All rights reserved. + * Copyright (c) 2013-2018 PX4 Development Team. All rights reserved. * * Redistribution and use in source and binary forms, with or without * modification, are permitted provided that the following conditions @@ -85,8 +85,7 @@ public: MISSION_YAWMODE_FRONT_TO_WAYPOINT = 1, MISSION_YAWMODE_FRONT_TO_HOME = 2, MISSION_YAWMODE_BACK_TO_HOME = 3, - MISSION_YAWMODE_TO_ROI = 4, - MISSION_YAWMODE_MAX = 5 + MISSION_YAWMODE_MAX = 4 }; bool set_current_offboard_mission_index(uint16_t index); @@ -232,6 +231,7 @@ private: control::BlockParamFloat _param_dist_between_wps; control::BlockParamInt _param_altmode; control::BlockParamInt _param_yawmode; + control::BlockParamInt _param_mnt_yaw_ctl; struct mission_s _offboard_mission {}; diff --git a/src/modules/navigator/mission_params.c b/src/modules/navigator/mission_params.c index e477b1011c..4892e33e3a 100644 --- a/src/modules/navigator/mission_params.c +++ b/src/modules/navigator/mission_params.c @@ -130,7 +130,6 @@ PARAM_DEFINE_INT32(MIS_ALTMODE, 1); * @value 1 Heading towards waypoint * @value 2 Heading towards home * @value 3 Heading away from home - * @value 4 Heading towards ROI * @group Mission */ PARAM_DEFINE_INT32(MIS_YAWMODE, 1);