Movatterモバイル変換


[0]ホーム

URL:


Skip to content

Navigation Menu

Sign in
Appearance settings

Search code, repositories, users, issues, pull requests...

Provide feedback

We read every piece of feedback, and take your input very seriously.

Saved searches

Use saved searches to filter your results more quickly

Sign up
Appearance settings
/dunePublic

Control/UAV/Ardupilot: make loiter radius for GUIDED Goto a parameter#204

New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

By clicking “Sign up for GitHub”, you agree to ourterms of service andprivacy statement. We’ll occasionally send you account related emails.

Already on GitHub?Sign in to your account

Open
krisgry wants to merge1 commit intoLSTS:master
base:master
Choose a base branch
Loading
fromkrisgry:feature/lraduis_min
Open
Changes fromall commits
Commits
File filter

Filter by extension

Filter by extension

Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
Control/UAV/Ardupilot: make loiter radius for GUIDED Goto a parameter
This allows setting it as small as the airframe permits, without going into a loiter around the Goto
  • Loading branch information
@krisgry
krisgry committedAug 6, 2019
commit3492f4bb06acf8db31ccd6981a02f3feb70e50c4
17 changes: 15 additions & 2 deletionssrc/Control/UAV/Ardupilot/Task.cpp
View file
Open in desktop
Original file line numberDiff line numberDiff line change
Expand Up@@ -132,6 +132,8 @@ namespace Control
float alt;
//! LoiterHere (default) radius
float lradius;
//! Radius sent to AP during Goto, to come close
float lradius_min;
//! Loitering tolerance
int ltolerance;
//! Has Power Module
Expand DownExpand Up@@ -313,6 +315,11 @@ namespace Control
.units(Units::Meter)
.description("Loiter radius used in LoiterHere (idle)");

param("Minimum loiter radius", m_args.lradius_min)
.defaultValue("3")
.units(Units::Meter)
.description("Radius sent to AP during Goto, to come close");

param("Loitering tolerance", m_args.ltolerance)
.defaultValue("10")
.units(Units::Meter)
Expand DownExpand Up@@ -975,17 +982,23 @@ namespace Control
//! Setting loiter radius parameter
if (m_vehicle_type != VEHICLE_COPTER)
{
//Set WP_LOITER_RAD to the desired radius if we are loitering
//if not, set it small so we will come close to the WP (before AP will start a loiter)
m_desired_radius = path->lradius ? (path->flags & DesiredPath::FL_CCLOCKW ? (-1 * path->lradius) : (path->lradius)) : m_args.lradius_min;
mavlink_msg_param_set_pack(255, 0, &msg,
m_sysid, //! target_system System ID
0, //! target_component Component ID
"WP_LOITER_RAD", //! Parameter name
path->lradius ? (path->flags & DesiredPath::FL_CCLOCKW ? (-1 * path->lradius) : (path->lradius)) : 10, //! Parameter value
m_desired_radius, //! Parameter value
MAV_PARAM_TYPE_INT16); //! Parameter type

n = mavlink_msg_to_send_buffer(buf, &msg);
sendData(buf, n);
}
m_desired_radius = (uint16_t) path->lradius;
else
{
m_desired_radius = (uint16_t) path->lradius;
}

float alt = (path->end_z_units & IMC::Z_NONE) ? m_args.alt : (float)path->end_z;

Expand Down

[8]ページ先頭

©2009-2025 Movatter.jp