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

heading towards ROI: do not require setting the MIS_YAWMODE param to enable multicopters yawing towards ROIs

Browse Source 
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.
This commit is contained in:
Nicolas de Palezieux
2018-01-22 15:56:42 +01:00
committed by Lorenz Meier
parent add3692357
commit 7ae3884944
4 changed files with 83 additions and 50 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/drivers/vmount/vmount_params.c
+15 -1
View File
@@ -224,4 +224,18 @@ PARAM_DEFINE_FLOAT(MNT_OFF_ROLL, 0.0f);
* @decimal 1 * @decimal 1
* @group Mount * @group Mount
*/ */
PARAM_DEFINE_FLOAT(MNT_OFF_YAW, 0.0f); 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);
src/modules/navigator/mission.cpp
+65 -45
View File
@@ -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 * Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions * modification, are permitted provided that the following conditions
@@ -42,6 +42,7 @@
* @author Simon Wilks <simon@uaventure.com> * @author Simon Wilks <simon@uaventure.com>
* @author Andreas Antener <andreas@uaventure.com> * @author Andreas Antener <andreas@uaventure.com>
* @author Sander Smeets <sander@droneslab.com> * @author Sander Smeets <sander@droneslab.com>
* @author Lorenz Meier <lorenz@px4.io>
*/ */
#include "mission.h" #include "mission.h"
@@ -217,9 +218,10 @@ Mission::on_active()
} }
/* see if we need to update the current yaw heading */ /* see if we need to update the current yaw heading */
if ((_param_yawmode.get() != MISSION_YAWMODE_NONE if (_navigator->get_vroi().mode == vehicle_roi_s::ROI_LOCATION
&& _param_yawmode.get() < MISSION_YAWMODE_MAX || (_param_yawmode.get() != MISSION_YAWMODE_NONE
&& _mission_type != MISSION_TYPE_NONE) && _param_yawmode.get() < MISSION_YAWMODE_MAX
&& _mission_type != MISSION_TYPE_NONE)
|| _navigator->get_vstatus()->is_vtol) { || _navigator->get_vstatus()->is_vtol) {
heading_sp_update(); heading_sp_update();
@@ -989,62 +991,80 @@ Mission::heading_sp_update()
return; return;
} }
/* set yaw angle for the waypoint if a loiter time has been specified */ /* Calculate direction the vehicle should point to. */
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 { double point_from_latlon[2];
/* Calculate direction the vehicle should point to. */ 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[0] = _navigator->get_global_position()->lat;
point_from_latlon[1] = _navigator->get_global_position()->lon; point_from_latlon[1] = _navigator->get_global_position()->lon;
/* target location is home */ if (_navigator->get_vroi().mode == vehicle_roi_s::ROI_LOCATION && !_param_mnt_yaw_ctl.get()) {
if ((_param_yawmode.get() == MISSION_YAWMODE_FRONT_TO_HOME || _param_yawmode.get() == MISSION_YAWMODE_BACK_TO_HOME) point_to_latlon[0] = _navigator->get_vroi().lat;
// need to be rotary wing for this but not in a transition point_to_latlon[1] = _navigator->get_vroi().lon;
// 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;
}
/* stop if positions are close together to prevent excessive yawing */
float d_current = get_distance_to_next_waypoint( float d_current = get_distance_to_next_waypoint(
point_from_latlon[0], point_from_latlon[1], point_from_latlon[0], point_from_latlon[1],
point_to_latlon[0], point_to_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()) { if (d_current > _navigator->get_acceptance_radius()) {
float yaw = get_bearing_to_next_waypoint( float yaw = get_bearing_to_next_waypoint(
point_from_latlon[0], point_from_latlon[1], point_from_latlon[0], 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 */ _mission_item.yaw = yaw;
if (_param_yawmode.get() == MISSION_YAWMODE_BACK_TO_HOME) { pos_sp_triplet->current.yaw = _mission_item.yaw;
_mission_item.yaw = _wrap_pi(yaw + M_PI_F); }
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 { } else {
_mission_item.yaw = yaw; /* target location is next (current) waypoint */
pos_sp_triplet->current.yaw = _mission_item.yaw; 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;
}
} }
} }
} }
src/modules/navigator/mission.h
+3 -3
View File
@@ -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 * Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions * modification, are permitted provided that the following conditions
@@ -85,8 +85,7 @@ public:
MISSION_YAWMODE_FRONT_TO_WAYPOINT = 1, MISSION_YAWMODE_FRONT_TO_WAYPOINT = 1,
MISSION_YAWMODE_FRONT_TO_HOME = 2, MISSION_YAWMODE_FRONT_TO_HOME = 2,
MISSION_YAWMODE_BACK_TO_HOME = 3, MISSION_YAWMODE_BACK_TO_HOME = 3,
MISSION_YAWMODE_TO_ROI = 4, MISSION_YAWMODE_MAX = 4
MISSION_YAWMODE_MAX = 5
}; };
bool set_current_offboard_mission_index(uint16_t index); bool set_current_offboard_mission_index(uint16_t index);
@@ -232,6 +231,7 @@ private:
control::BlockParamFloat _param_dist_between_wps; control::BlockParamFloat _param_dist_between_wps;
control::BlockParamInt _param_altmode; control::BlockParamInt _param_altmode;
control::BlockParamInt _param_yawmode; control::BlockParamInt _param_yawmode;
control::BlockParamInt _param_mnt_yaw_ctl;
struct mission_s _offboard_mission {}; struct mission_s _offboard_mission {};
src/modules/navigator/mission_params.c
-1
View File
@@ -130,7 +130,6 @@ PARAM_DEFINE_INT32(MIS_ALTMODE, 1);
* @value 1 Heading towards waypoint * @value 1 Heading towards waypoint
* @value 2 Heading towards home * @value 2 Heading towards home
* @value 3 Heading away from home * @value 3 Heading away from home
* @value 4 Heading towards ROI
* @group Mission * @group Mission
*/ */
PARAM_DEFINE_INT32(MIS_YAWMODE, 1); PARAM_DEFINE_INT32(MIS_YAWMODE, 1);