cl_px4_mr Namespace Reference

Namespaces
namespace	anonymous_namespace{cb_wait_for_heading_stable.cpp}
namespace	anonymous_namespace{cp_kml_mission_loader.cpp}
namespace	anonymous_namespace{cp_tf_broadcaster.cpp}
namespace	anonymous_namespace{cp_tunnel_centering.cpp}
namespace	anonymous_namespace{pattern_generators.cpp}
namespace	geo_detail
namespace	pattern_detail

Classes
class	CbArmPX4
class	CbAscendToAltitude
class	CbChangeAltitude
class	CbConnectMicroRosAgent
class	CbDisarmPX4
class	CbFigureEight
class	CbFollowNedPath
class	CbFollowWaypoints
class	CbGoToLocation
class	CbGridPattern
class	CbHoldPosition
class	CbLand
class	CbLawnmower
class	CbLemniscate
class	CbLoadKmlMission
class	CbLoiter
class	CbObstacleGuard
class	CbOrbitLocation
class	CbPx4ClientBehaviorBase
class	CbPx4PathFollowerBase
class	CbReturnToHome
class	CbSineAltitudeCruise
class	CbSineWaveHorizontal
class	CbSineWaveVertical
class	CbSpiral
class	CbSpiralPattern
class	CbSpiralUp
class	CbSquareSpiral
class	CbTakeOff
class	CbVSSearch
class	CbWaitForHeadingStable
class	CbYawRotate
class	CbYawScan
class	ClPx4Mr
class	CpForwardObstacleGuard
class	CpGoalChecker
class	CpKmlMissionLoader
class	CpMicroRosAgent
class	CpOffboardKeepAlive
class	CpTfBroadcaster
class	CpTrajectorySetpoint
class	CpTunnelCentering
class	CpVehicleCommand
class	CpVehicleCommandAck
class	CpVehicleLocalPosition
class	CpVehicleStatus
struct	EvObstacleCleared
struct	EvObstacleTooClose
struct	FlightPatternAscendParams
struct	FlightPatternFigureEightParams
struct	FlightPatternGridPatternParams
struct	FlightPatternLawnmowerParams
struct	FlightPatternLoiterParams
struct	FlightPatternSineWaveHorizontalParams
struct	FlightPatternSineWaveVerticalParams
struct	FlightPatternSpiralParams
struct	FlightPatternSquareSpiralParams
struct	FlightPatternVSSearchParams
struct	ForwardObstacleGuardParams
struct	GeoPoint
struct	HeadingStableParams
struct	KmlLoadResult
class	MapProjection
struct	NedPoint
struct	PathFollowerParams
struct	SpiralUpParams
struct	TfBroadcasterParams
struct	TunnelCenteringParams

Enumerations
enum class	YawMode { TANGENT , FIXED , PER_VERTEX , HOLD_ENTRY }
enum class	Turn { RIGHT , LEFT }

Functions
float	wrapPi (float angle)
double	haversineDistance (double lat1, double lon1, double lat2, double lon2)
double	haversineDistance (const GeoPoint &a, const GeoPoint &b)
double	initialBearing (double lat1, double lon1, double lat2, double lon2)
float	nedDistance (const NedPoint &a, const NedPoint &b)
float	nedDistanceXY (const NedPoint &a, const NedPoint &b)
std::vector< float >	cumulativeLengths (const std::vector< NedPoint > &path)
float	polylineLength (const std::vector< NedPoint > &path)
NedPoint	sampleAtArcLength (const std::vector< NedPoint > &path, const std::vector< float > &cum, float s, size_t *segmentIndex=nullptr)
std::vector< NedPoint >	resamplePolyline (const std::vector< NedPoint > &path, float spacing)
void	levelRotationFromPx4Quaternion (const std::array< float, 4 > &q, std::array< float, 9 > &r)
float	turnSign (Turn t)
const char *	turnName (Turn t)
std::vector< NedPoint >	generateFlightPatternAscend (const FlightPatternAscendParams &p, const NedPoint &current)
std::vector< NedPoint >	generateFlightPatternLoiter (const FlightPatternLoiterParams &p, const NedPoint &current)
std::vector< NedPoint >	generateFlightPatternFigureEight (const FlightPatternFigureEightParams &p, const NedPoint &current)
float	flightPatternFigureEightLength (const FlightPatternFigureEightParams &p)
std::vector< NedPoint >	generateFlightPatternSineWaveVertical (const FlightPatternSineWaveVerticalParams &p, const NedPoint &current)
std::vector< NedPoint >	generateFlightPatternSineWaveHorizontal (const FlightPatternSineWaveHorizontalParams &p, const NedPoint &current)
std::vector< NedPoint >	generateFlightPatternSquareSpiral (const FlightPatternSquareSpiralParams &p, const NedPoint &current)
float	flightPatternSquareSpiralLength (const FlightPatternSquareSpiralParams &p)
std::vector< NedPoint >	generateFlightPatternSpiral (const FlightPatternSpiralParams &p, const NedPoint &current)
float	flightPatternSpiralLength (const FlightPatternSpiralParams &p)
std::vector< NedPoint >	generateFlightPatternLawnmower (const FlightPatternLawnmowerParams &p, const NedPoint &current)
float	flightPatternLawnmowerLength (const FlightPatternLawnmowerParams &p)
int	flightPatternLawnmowerLaneCount (const FlightPatternLawnmowerParams &p)
std::vector< NedPoint >	generateFlightPatternGridPattern (const FlightPatternGridPatternParams &p, const NedPoint &current)
float	flightPatternGridPatternLength (const FlightPatternGridPatternParams &p)
std::vector< NedPoint >	generateFlightPatternVSSearch (const FlightPatternVSSearchParams &p, const NedPoint &current)
float	flightPatternVSSearchLength (const FlightPatternVSSearchParams &p)
float	legHeading (const NedPoint &from, const NedPoint &to)

Variables
constexpr double	kEarthRadiusM = 6371000.0

Enumeration Type Documentation

Turn

enum class cl_px4_mr::Turn

strong

Enumerator
RIGHT
LEFT

Definition at line 51 of file pattern_generators.hpp.

{
  RIGHT,
  LEFT
};

YawMode

enum class cl_px4_mr::YawMode

strong

Enumerator
TANGENT
FIXED
PER_VERTEX
HOLD_ENTRY

Definition at line 35 of file cb_px4_path_follower_base.hpp.

{
  TANGENT,     // along the current path segment
  FIXED,       // PathFollowerParams::fixedYaw
  PER_VERTEX,  // NedPoint::yaw of the segment start (NaN -> tangent)
  HOLD_ENTRY   // heading at entry
};

Function Documentation

cumulativeLengths()

std::vector< float > cl_px4_mr::cumulativeLengths ( const std::vector< NedPoint > & path )

inline

Definition at line 205 of file geo_utils.hpp.

{
  std::vector<float> cum;
  cum.reserve(path.size());
  double acc = 0.0;
  for (size_t i = 0; i < path.size(); ++i)
  {
    if (i > 0)
    {
      acc += static_cast<double>(nedDistance(path[i - 1], path[i]));
    }
    cum.push_back(static_cast<float>(acc));
  }
  return cum;
}

References nedDistance().

Referenced by cl_px4_mr::CbPx4PathFollowerBase::onEntry(), and polylineLength().

Here is the call graph for this function:

Here is the caller graph for this function:

flightPatternFigureEightLength()

float cl_px4_mr::flightPatternFigureEightLength

(

const FlightPatternFigureEightParams &

p

)

Definition at line 183 of file pattern_generators.cpp.

{
  // perimeter of the lemniscate of Bernoulli = 5.2441 a
  return 5.2441f * std::max(p.size, 0.5f) * std::max(p.count, 0);
}

References cl_px4_mr::FlightPatternFigureEightParams::count, and cl_px4_mr::FlightPatternFigureEightParams::size.

Referenced by cl_px4_mr::CbLemniscate::buildPath().

Here is the caller graph for this function:

flightPatternGridPatternLength()

float cl_px4_mr::flightPatternGridPatternLength

(

const FlightPatternGridPatternParams &

p

)

Definition at line 426 of file pattern_generators.cpp.

{
  FlightPatternLawnmowerParams second = p.base;
  second.laneLength = p.base.width;
  second.width = p.base.laneLength;
  return flightPatternLawnmowerLength(p.base) +
         (p.secondPass ? flightPatternLawnmowerLength(second) : 0.0f);
}

References cl_px4_mr::FlightPatternGridPatternParams::base, flightPatternLawnmowerLength(), cl_px4_mr::FlightPatternLawnmowerParams::laneLength, cl_px4_mr::FlightPatternGridPatternParams::secondPass, and cl_px4_mr::FlightPatternLawnmowerParams::width.

Referenced by cl_px4_mr::CbGridPattern::buildPath().

Here is the call graph for this function:

Here is the caller graph for this function:

flightPatternLawnmowerLaneCount()

int cl_px4_mr::flightPatternLawnmowerLaneCount

(

const FlightPatternLawnmowerParams &

p

)

Definition at line 363 of file pattern_generators.cpp.

{
  const float spacing = std::max(p.laneSpacing, 0.5f);
  return std::max(1, static_cast<int>(std::floor(std::max(p.width, 0.0f) / spacing + 1e-3f)) + 1);
}

References cl_px4_mr::FlightPatternLawnmowerParams::laneSpacing, and cl_px4_mr::FlightPatternLawnmowerParams::width.

Referenced by cl_px4_mr::CbLawnmower::buildPath(), flightPatternLawnmowerLength(), and generateFlightPatternLawnmower().

Here is the caller graph for this function:

flightPatternLawnmowerLength()

float cl_px4_mr::flightPatternLawnmowerLength

(

const FlightPatternLawnmowerParams &

p

)

Definition at line 369 of file pattern_generators.cpp.

{
  const int lanes = flightPatternLawnmowerLaneCount(p);
  return lanes * std::max(p.laneLength, 0.0f) + (lanes - 1) * std::max(p.laneSpacing, 0.5f);
}

References flightPatternLawnmowerLaneCount(), cl_px4_mr::FlightPatternLawnmowerParams::laneLength, and cl_px4_mr::FlightPatternLawnmowerParams::laneSpacing.

Referenced by cl_px4_mr::CbLawnmower::buildPath(), and flightPatternGridPatternLength().

Here is the call graph for this function:

Here is the caller graph for this function:

flightPatternSpiralLength()

float cl_px4_mr::flightPatternSpiralLength

(

const FlightPatternSpiralParams &

p

)

Definition at line 354 of file pattern_generators.cpp.

{
  // area between the radii divided by the track spacing
  const float rStart = std::max(p.startRadius, 0.0f);
  const float rEnd = std::max(p.endRadius, rStart);
  const float spacing = std::max(p.spacing, 0.5f);
  return static_cast<float>(M_PI) * (rEnd * rEnd - rStart * rStart) / spacing;
}

References cl_px4_mr::FlightPatternSpiralParams::endRadius, cl_px4_mr::FlightPatternSpiralParams::spacing, and cl_px4_mr::FlightPatternSpiralParams::startRadius.

Referenced by cl_px4_mr::CbSpiral::buildPath().

Here is the caller graph for this function:

flightPatternSquareSpiralLength()

float cl_px4_mr::flightPatternSquareSpiralLength

(

const FlightPatternSquareSpiralParams &

p

)

Definition at line 291 of file pattern_generators.cpp.

{
  const float spacing = std::max(p.spacing, 0.5f);
  float total = 0.0f;
  for (int i = 0; i < p.numLegs; ++i)
  {
    const float len = spacing * static_cast<float>(i / 2 + 1);
    if (p.maxLegLength > 0.0f && len > p.maxLegLength)
    {
      break;
    }
    total += len;
  }
  return total;
}

References cl_px4_mr::FlightPatternSquareSpiralParams::maxLegLength, cl_px4_mr::FlightPatternSquareSpiralParams::numLegs, and cl_px4_mr::FlightPatternSquareSpiralParams::spacing.

Referenced by cl_px4_mr::CbSquareSpiral::buildPath().

Here is the caller graph for this function:

flightPatternVSSearchLength()

float cl_px4_mr::flightPatternVSSearchLength

(

const FlightPatternVSSearchParams &

p

)

Definition at line 480 of file pattern_generators.cpp.

{
  return 9.0f * std::max(p.radius, 1.0f) * static_cast<float>(std::max(p.cycles, 1));
}

References cl_px4_mr::FlightPatternVSSearchParams::cycles, and cl_px4_mr::FlightPatternVSSearchParams::radius.

Referenced by cl_px4_mr::CbVSSearch::buildPath().

Here is the caller graph for this function:

generateFlightPatternAscend()

std::vector< NedPoint > cl_px4_mr::generateFlightPatternAscend	(	const FlightPatternAscendParams &	p,
		const NedPoint &	current )

Definition at line 33 of file pattern_generators.cpp.

{
  NedPoint target;
  target.x = pick(p.targetX, current.x);
  target.y = pick(p.targetY, current.y);
  target.z = -p.altitudeAgl;
 
  const bool atXy = std::hypot(target.x - current.x, target.y - current.y) <= p.tolerance;
  if (atXy && std::fabs(current.z - target.z) <= p.tolerance)
  {
    return {target};
  }
  NedPoint start = current;
  start.yaw = kNaN;
  return {start, target};
}

References cl_px4_mr::FlightPatternAscendParams::altitudeAgl, cl_px4_mr::FlightPatternAscendParams::targetX, cl_px4_mr::FlightPatternAscendParams::targetY, cl_px4_mr::FlightPatternAscendParams::tolerance, cl_px4_mr::NedPoint::x, cl_px4_mr::NedPoint::y, cl_px4_mr::NedPoint::yaw, and cl_px4_mr::NedPoint::z.

Referenced by cl_px4_mr::CbAscendToAltitude::buildPath().

Here is the caller graph for this function:

generateFlightPatternFigureEight()

std::vector< NedPoint > cl_px4_mr::generateFlightPatternFigureEight	(	const FlightPatternFigureEightParams &	p,
		const NedPoint &	current )

Definition at line 118 of file pattern_generators.cpp.

{
  const float cx = pick(p.centerX, current.x);
  const float cy = pick(p.centerY, current.y);
  const float z = altitudeToZ(p.altitudeAgl, current.z);
  const float a = std::max(p.size, 0.5f);
  const int n = std::max(p.pointsPerLoop, 16);
  const float sign = turnSign(p.direction);
 
  if (p.count <= 0)
  {
    NedPoint here = current;
    here.z = z;
    here.yaw = kNaN;
    return {here};
  }
 
  // lobe axis: given, else the entry heading, else north
  const float heading =
    std::isnan(p.heading) ? (std::isnan(current.yaw) ? 0.0f : current.yaw) : p.heading;
  const float ch = std::cos(heading);
  const float sh = std::sin(heading);
 
  // one loop of the lemniscate x = a cos t / (1 + sin^2 t), y = a sin t cos t / (1 + sin^2 t)
  std::vector<NedPoint> loop;
  loop.reserve(static_cast<size_t>(n));
  for (int k = 0; k < n; ++k)
  {
    const float t = sign * 2.0f * static_cast<float>(M_PI) * k / n;
    const float st = std::sin(t);
    const float ct = std::cos(t);
    const float denom = 1.0f + st * st;
    const float lx = a * ct / denom;
    const float ly = a * st * ct / denom;
    NedPoint v;
    v.x = cx + lx * ch - ly * sh;
    v.y = cy + lx * sh + ly * ch;
    v.z = z;
    v.yaw = kNaN;
    loop.push_back(v);
  }
 
  // enter at the sample nearest the vehicle
  int start = 0;
  float best = std::numeric_limits<float>::infinity();
  for (int k = 0; k < n; ++k)
  {
    const float d = std::hypot(loop[k].x - current.x, loop[k].y - current.y);
    if (d < best)
    {
      best = d;
      start = k;
    }
  }
 
  std::vector<NedPoint> path;
  path.reserve(static_cast<size_t>(p.count * n + 1));
  for (int k = 0; k <= p.count * n; ++k)
  {
    path.push_back(loop[static_cast<size_t>((start + k) % n)]);
  }
  return path;
}

References cl_px4_mr::FlightPatternFigureEightParams::altitudeAgl, cl_px4_mr::FlightPatternFigureEightParams::centerX, cl_px4_mr::FlightPatternFigureEightParams::centerY, cl_px4_mr::FlightPatternFigureEightParams::count, cl_px4_mr::FlightPatternFigureEightParams::direction, cl_px4_mr::FlightPatternFigureEightParams::heading, cl_px4_mr::FlightPatternFigureEightParams::pointsPerLoop, cl_px4_mr::FlightPatternFigureEightParams::size, turnSign(), cl_px4_mr::NedPoint::x, cl_px4_mr::NedPoint::y, cl_px4_mr::NedPoint::yaw, and cl_px4_mr::NedPoint::z.

Referenced by cl_px4_mr::CbLemniscate::buildPath().

Here is the call graph for this function:

Here is the caller graph for this function:

generateFlightPatternGridPattern()

std::vector< NedPoint > cl_px4_mr::generateFlightPatternGridPattern	(	const FlightPatternGridPatternParams &	p,
		const NedPoint &	current )

Definition at line 435 of file pattern_generators.cpp.

{
  FlightPatternLawnmowerParams first = p.base;
  first.originIsCenter = true;  // the grid is always centred on the origin
  first.originX = pick(p.base.originX, current.x);
  first.originY = pick(p.base.originY, current.y);
  first.laneHeading = pick(p.base.laneHeading, std::isnan(current.yaw) ? 0.0f : current.yaw);
 
  std::vector<NedPoint> path = generateFlightPatternLawnmower(first, current);
  if (!p.secondPass || path.empty())
  {
    return path;
  }
 
  // second pass: rotated 90 degrees over the same rectangle; try the four
  // start corners (heading +-90, step side left/right) and take the one that
  // starts nearest the end of pass 1
  const NedPoint & joint = path.back();
  std::vector<NedPoint> best;
  float bestDist = std::numeric_limits<float>::max();
  for (int variant = 0; variant < 4; ++variant)
  {
    FlightPatternLawnmowerParams second = first;
    second.laneLength = first.width;
    second.width = first.laneLength;
    second.laneHeading =
      first.laneHeading + ((variant & 1) ? -1.0f : 1.0f) * static_cast<float>(M_PI) / 2.0f;
    second.firstTurn = (variant & 2) ? Turn::LEFT : Turn::RIGHT;
    std::vector<NedPoint> candidate = generateFlightPatternLawnmower(second, current);
    if (candidate.empty())
    {
      continue;
    }
    const float d = nedDistance(joint, candidate.front());
    if (d < bestDist)
    {
      bestDist = d;
      best = std::move(candidate);
    }
  }
  path.insert(path.end(), best.begin(), best.end());
  return path;
}

References cl_px4_mr::FlightPatternGridPatternParams::base, cl_px4_mr::FlightPatternLawnmowerParams::firstTurn, generateFlightPatternLawnmower(), cl_px4_mr::FlightPatternLawnmowerParams::laneHeading, cl_px4_mr::FlightPatternLawnmowerParams::laneLength, LEFT, nedDistance(), cl_px4_mr::FlightPatternLawnmowerParams::originIsCenter, cl_px4_mr::FlightPatternLawnmowerParams::originX, cl_px4_mr::FlightPatternLawnmowerParams::originY, RIGHT, cl_px4_mr::FlightPatternGridPatternParams::secondPass, cl_px4_mr::FlightPatternLawnmowerParams::width, cl_px4_mr::NedPoint::x, cl_px4_mr::NedPoint::y, and cl_px4_mr::NedPoint::yaw.

Referenced by cl_px4_mr::CbGridPattern::buildPath().

Here is the call graph for this function:

Here is the caller graph for this function:

generateFlightPatternLawnmower()

std::vector< NedPoint > cl_px4_mr::generateFlightPatternLawnmower	(	const FlightPatternLawnmowerParams &	p,
		const NedPoint &	current )

Definition at line 375 of file pattern_generators.cpp.

{
  const float z = altitudeToZ(p.altitudeAgl, current.z);
  const float heading = pick(p.laneHeading, std::isnan(current.yaw) ? 0.0f : current.yaw);
  const float ux = std::cos(heading);
  const float uy = std::sin(heading);
  // side normal: RIGHT of the heading in NED (x north, y east) is (-uy, ux)
  const float side = turnSign(p.firstTurn);
  const float nx = -uy * side;
  const float ny = ux * side;
 
  const int lanes = flightPatternLawnmowerLaneCount(p);
  const float spacing = std::max(p.laneSpacing, 0.5f);
  const float length = std::max(p.laneLength, 0.0f);
  const float coveredWidth = (lanes - 1) * spacing;
 
  float ox = pick(p.originX, current.x);
  float oy = pick(p.originY, current.y);
  if (p.originIsCenter)
  {
    ox -= 0.5f * length * ux + 0.5f * coveredWidth * nx;
    oy -= 0.5f * length * uy + 0.5f * coveredWidth * ny;
  }
 
  std::vector<NedPoint> path;
  path.reserve(static_cast<size_t>(2 * lanes));
  for (int i = 0; i < lanes; ++i)
  {
    NedPoint a;
    a.x = ox + i * spacing * nx;
    a.y = oy + i * spacing * ny;
    a.z = z;
    NedPoint b;
    b.x = a.x + length * ux;
    b.y = a.y + length * uy;
    b.z = z;
    if (i % 2 == 0)
    {
      path.push_back(a);
      path.push_back(b);
    }
    else
    {
      path.push_back(b);
      path.push_back(a);
    }
  }
  return path;
}

References cl_px4_mr::FlightPatternLawnmowerParams::altitudeAgl, cl_px4_mr::FlightPatternLawnmowerParams::firstTurn, flightPatternLawnmowerLaneCount(), cl_px4_mr::FlightPatternLawnmowerParams::laneHeading, cl_px4_mr::FlightPatternLawnmowerParams::laneLength, cl_px4_mr::FlightPatternLawnmowerParams::laneSpacing, cl_px4_mr::FlightPatternLawnmowerParams::originIsCenter, cl_px4_mr::FlightPatternLawnmowerParams::originX, cl_px4_mr::FlightPatternLawnmowerParams::originY, turnSign(), cl_px4_mr::NedPoint::x, cl_px4_mr::NedPoint::y, cl_px4_mr::NedPoint::yaw, and cl_px4_mr::NedPoint::z.

Referenced by cl_px4_mr::CbLawnmower::buildPath(), and generateFlightPatternGridPattern().

Here is the call graph for this function:

Here is the caller graph for this function:

generateFlightPatternLoiter()

std::vector< NedPoint > cl_px4_mr::generateFlightPatternLoiter	(	const FlightPatternLoiterParams &	p,
		const NedPoint &	current )

Definition at line 51 of file pattern_generators.cpp.

{
  const float cx = pick(p.centerX, current.x);
  const float cy = pick(p.centerY, current.y);
  const float z = altitudeToZ(p.altitudeAgl, current.z);
  const float r = std::max(p.radius, 0.5f);
  const int n = std::max(p.pointsPerCircle, 8);
  const float sign = turnSign(p.direction);
 
  if (p.count <= 0)
  {
    // nothing to fly: hold the entry point (the follower posts success at once)
    NedPoint here = current;
    here.z = z;
    here.yaw = kNaN;
    return {here};
  }
  const int count = p.count;
 
  // enter at the point of the circle nearest the vehicle; if at the centre
  // use the entry heading to pick a start
  const float dx = current.x - cx;
  const float dy = current.y - cy;
  float theta0 =
    std::hypot(dx, dy) > 0.1f ? std::atan2(dy, dx) : (std::isnan(current.yaw) ? 0.0f : current.yaw);
 
  std::vector<NedPoint> path;
  path.reserve(static_cast<size_t>(count * n + 1));
  for (int k = 0; k <= count * n; ++k)
  {
    const float theta = theta0 + sign * 2.0f * static_cast<float>(M_PI) * k / n;
    NedPoint v;
    v.x = cx + r * std::cos(theta);
    v.y = cy + r * std::sin(theta);
    v.z = z;
    v.yaw = p.faceCenter ? std::atan2(cy - v.y, cx - v.x) : kNaN;
    path.push_back(v);
  }
  return path;
}

References cl_px4_mr::FlightPatternLoiterParams::altitudeAgl, cl_px4_mr::FlightPatternLoiterParams::centerX, cl_px4_mr::FlightPatternLoiterParams::centerY, cl_px4_mr::FlightPatternLoiterParams::count, cl_px4_mr::FlightPatternLoiterParams::direction, cl_px4_mr::FlightPatternLoiterParams::faceCenter, cl_px4_mr::FlightPatternLoiterParams::pointsPerCircle, cl_px4_mr::FlightPatternLoiterParams::radius, turnSign(), cl_px4_mr::NedPoint::x, cl_px4_mr::NedPoint::y, cl_px4_mr::NedPoint::yaw, and cl_px4_mr::NedPoint::z.

Referenced by cl_px4_mr::CbLoiter::buildPath().

Here is the call graph for this function:

Here is the caller graph for this function:

generateFlightPatternSineWaveHorizontal()

std::vector< NedPoint > cl_px4_mr::generateFlightPatternSineWaveHorizontal	(	const FlightPatternSineWaveHorizontalParams &	p,
		const NedPoint &	current )

Definition at line 221 of file pattern_generators.cpp.

{
  const float endX = pick(p.endX, current.x);
  const float endY = pick(p.endY, current.y);
  const float z = altitudeToZ(p.altitudeAgl, current.z);
  const float lambda = std::max(p.wavelength, 0.1f);
  const float spacing = std::max(p.sampleSpacing, 0.1f);
  const Leg leg = legFrom(current, endX, endY);
 
  std::vector<NedPoint> path;
  if (leg.length <= 1e-3f)
  {
    path.push_back(NedPoint{endX, endY, z, kNaN});
    return path;
  }
 
  // left-of-track normal in NED (x north, y east): rotate u by -90 deg
  const float nx = leg.uy;
  const float ny = -leg.ux;
 
  const int n = static_cast<int>(std::floor(leg.length / spacing));
  path.reserve(static_cast<size_t>(n + 2));
  for (int k = 0; k < n; ++k)
  {
    const float s = k * spacing;
    const float offset = p.amplitude * std::sin(2.0f * static_cast<float>(M_PI) * s / lambda);
    NedPoint v;
    v.x = current.x + s * leg.ux + offset * nx;
    v.y = current.y + s * leg.uy + offset * ny;
    v.z = z;
    path.push_back(v);
  }
  path.push_back(NedPoint{endX, endY, z, kNaN});
  return path;
}

References cl_px4_mr::FlightPatternSineWaveHorizontalParams::altitudeAgl, cl_px4_mr::FlightPatternSineWaveHorizontalParams::amplitude, cl_px4_mr::FlightPatternSineWaveHorizontalParams::endX, cl_px4_mr::FlightPatternSineWaveHorizontalParams::endY, cl_px4_mr::FlightPatternSineWaveHorizontalParams::sampleSpacing, cl_px4_mr::FlightPatternSineWaveHorizontalParams::wavelength, cl_px4_mr::NedPoint::x, cl_px4_mr::NedPoint::y, and cl_px4_mr::NedPoint::z.

Referenced by cl_px4_mr::CbSineWaveHorizontal::buildPath().

Here is the caller graph for this function:

generateFlightPatternSineWaveVertical()

std::vector< NedPoint > cl_px4_mr::generateFlightPatternSineWaveVertical	(	const FlightPatternSineWaveVerticalParams &	p,
		const NedPoint &	current )

Definition at line 189 of file pattern_generators.cpp.

{
  const float endX = pick(p.endX, current.x);
  const float endY = pick(p.endY, current.y);
  const float zBase = altitudeToZ(p.baseAltitudeAgl, current.z);
  const float lambda = std::max(p.wavelength, 0.1f);
  const float spacing = std::max(p.sampleSpacing, 0.1f);
  const Leg leg = legFrom(current, endX, endY);
 
  std::vector<NedPoint> path;
  if (leg.length <= 1e-3f)
  {
    path.push_back(NedPoint{endX, endY, zBase, kNaN});
    return path;
  }
 
  const int n = static_cast<int>(std::floor(leg.length / spacing));
  path.reserve(static_cast<size_t>(n + 2));
  for (int k = 0; k < n; ++k)
  {
    const float s = k * spacing;
    NedPoint v;
    v.x = current.x + s * leg.ux;
    v.y = current.y + s * leg.uy;
    v.z = zBase - p.amplitude * std::sin(2.0f * static_cast<float>(M_PI) * s / lambda);
    path.push_back(v);
  }
  path.push_back(NedPoint{endX, endY, zBase, kNaN});
  return path;
}

References cl_px4_mr::FlightPatternSineWaveVerticalParams::amplitude, cl_px4_mr::FlightPatternSineWaveVerticalParams::baseAltitudeAgl, cl_px4_mr::FlightPatternSineWaveVerticalParams::endX, cl_px4_mr::FlightPatternSineWaveVerticalParams::endY, cl_px4_mr::FlightPatternSineWaveVerticalParams::sampleSpacing, cl_px4_mr::FlightPatternSineWaveVerticalParams::wavelength, cl_px4_mr::NedPoint::x, cl_px4_mr::NedPoint::y, and cl_px4_mr::NedPoint::z.

Referenced by cl_px4_mr::CbSineWaveVertical::buildPath().

Here is the caller graph for this function:

generateFlightPatternSpiral()

std::vector< NedPoint > cl_px4_mr::generateFlightPatternSpiral	(	const FlightPatternSpiralParams &	p,
		const NedPoint &	current )

Definition at line 307 of file pattern_generators.cpp.

{
  const float cx = pick(p.centerX, current.x);
  const float cy = pick(p.centerY, current.y);
  const float z = altitudeToZ(p.altitudeAgl, current.z);
  const float spacing = std::max(p.spacing, 0.5f);
  const float a = spacing / (2.0f * static_cast<float>(M_PI));  // r = a * theta
  const float rStart = std::max(p.startRadius, 0.0f);
  const float rEnd = std::max(p.endRadius, rStart);
  const float sign = turnSign(p.direction);
  const float ds = std::max(p.sampleSpacing, 0.1f);
 
  // angular origin: bearing of the vehicle from the centre, else entry heading
  const float dx = current.x - cx;
  const float dy = current.y - cy;
  const float theta0 =
    std::hypot(dx, dy) > 0.1f ? std::atan2(dy, dx) : (std::isnan(current.yaw) ? 0.0f : current.yaw);
 
  const float thetaStart = rStart / a;
  const float thetaEnd = rEnd / a;
 
  std::vector<NedPoint> path;
  float theta = thetaStart;
  while (theta < thetaEnd)
  {
    const float r = a * theta;
    NedPoint v;
    v.x = cx + r * std::cos(theta0 + sign * theta);
    v.y = cy + r * std::sin(theta0 + sign * theta);
    v.z = z;
    path.push_back(v);
    theta += ds / std::max(r, a);  // constant arc-length steps
  }
  NedPoint last;
  last.x = cx + rEnd * std::cos(theta0 + sign * thetaEnd);
  last.y = cy + rEnd * std::sin(theta0 + sign * thetaEnd);
  last.z = z;
  path.push_back(last);
 
  if (p.inward)
  {
    std::reverse(path.begin(), path.end());
  }
  return path;
}

References cl_px4_mr::FlightPatternSpiralParams::altitudeAgl, cl_px4_mr::FlightPatternSpiralParams::centerX, cl_px4_mr::FlightPatternSpiralParams::centerY, cl_px4_mr::FlightPatternSpiralParams::direction, cl_px4_mr::FlightPatternSpiralParams::endRadius, cl_px4_mr::FlightPatternSpiralParams::inward, cl_px4_mr::FlightPatternSpiralParams::sampleSpacing, cl_px4_mr::FlightPatternSpiralParams::spacing, cl_px4_mr::FlightPatternSpiralParams::startRadius, turnSign(), cl_px4_mr::NedPoint::x, cl_px4_mr::NedPoint::y, cl_px4_mr::NedPoint::yaw, and cl_px4_mr::NedPoint::z.

Referenced by cl_px4_mr::CbSpiral::buildPath().

Here is the call graph for this function:

Here is the caller graph for this function:

generateFlightPatternSquareSpiral()

std::vector< NedPoint > cl_px4_mr::generateFlightPatternSquareSpiral	(	const FlightPatternSquareSpiralParams &	p,
		const NedPoint &	current )

Definition at line 258 of file pattern_generators.cpp.

{
  const float ox = pick(p.originX, current.x);
  const float oy = pick(p.originY, current.y);
  const float z = altitudeToZ(p.altitudeAgl, current.z);
  const float spacing = std::max(p.spacing, 0.5f);
  const float sign = turnSign(p.direction);
  float heading = pick(p.initialHeading, std::isnan(current.yaw) ? 0.0f : current.yaw);
 
  std::vector<NedPoint> path;
  path.reserve(static_cast<size_t>(std::max(p.numLegs, 0) + 1));
  NedPoint v;
  v.x = ox;
  v.y = oy;
  v.z = z;
  path.push_back(v);
 
  for (int i = 0; i < p.numLegs; ++i)
  {
    const float len = spacing * static_cast<float>(i / 2 + 1);
    if (p.maxLegLength > 0.0f && len > p.maxLegLength)
    {
      break;
    }
    v.x += len * std::cos(heading);
    v.y += len * std::sin(heading);
    path.push_back(v);
    heading += sign * static_cast<float>(M_PI) / 2.0f;
  }
  return path;
}

References cl_px4_mr::FlightPatternSquareSpiralParams::altitudeAgl, cl_px4_mr::FlightPatternSquareSpiralParams::direction, cl_px4_mr::FlightPatternSquareSpiralParams::initialHeading, cl_px4_mr::FlightPatternSquareSpiralParams::maxLegLength, cl_px4_mr::FlightPatternSquareSpiralParams::numLegs, cl_px4_mr::FlightPatternSquareSpiralParams::originX, cl_px4_mr::FlightPatternSquareSpiralParams::originY, cl_px4_mr::FlightPatternSquareSpiralParams::spacing, turnSign(), cl_px4_mr::NedPoint::x, cl_px4_mr::NedPoint::y, cl_px4_mr::NedPoint::yaw, and cl_px4_mr::NedPoint::z.

Referenced by cl_px4_mr::CbSquareSpiral::buildPath().

Here is the call graph for this function:

Here is the caller graph for this function:

generateFlightPatternVSSearch()

std::vector< NedPoint > cl_px4_mr::generateFlightPatternVSSearch	(	const FlightPatternVSSearchParams &	p,
		const NedPoint &	current )

Definition at line 485 of file pattern_generators.cpp.

{
  const float dx = pick(p.datumX, current.x);
  const float dy = pick(p.datumY, current.y);
  const float z = altitudeToZ(p.altitudeAgl, current.z);
  const float r = std::max(p.radius, 1.0f);
  const float sign = turnSign(p.direction);
  const float h0 = pick(p.initialHeading, std::isnan(current.yaw) ? 0.0f : current.yaw);
  const int cycles = std::max(p.cycles, 1);
 
  // leg headings in units of 120 degrees relative to the cycle's base heading:
  // legs 3->4 and 6->7 continue straight through the datum
  static const int kSteps[9] = {0, 1, 2, 2, 0, 1, 1, 2, 0};
  const float third = 2.0f * static_cast<float>(M_PI) / 3.0f;
 
  std::vector<NedPoint> path;
  path.reserve(static_cast<size_t>(9 * cycles + 1));
  NedPoint v;
  v.x = dx;
  v.y = dy;
  v.z = z;
  path.push_back(v);
 
  for (int c = 0; c < cycles; ++c)
  {
    const float base = h0 + sign * p.reorientation * static_cast<float>(c);
    for (int leg = 0; leg < 9; ++leg)
    {
      const float heading = base + sign * third * static_cast<float>(kSteps[leg]);
      v.x += r * std::cos(heading);
      v.y += r * std::sin(heading);
      path.push_back(v);
    }
    // snap back onto the datum to cancel accumulated float drift
    path.back().x = dx;
    path.back().y = dy;
  }
  return path;
}

References cl_px4_mr::FlightPatternVSSearchParams::altitudeAgl, cl_px4_mr::FlightPatternVSSearchParams::cycles, cl_px4_mr::FlightPatternVSSearchParams::datumX, cl_px4_mr::FlightPatternVSSearchParams::datumY, cl_px4_mr::FlightPatternVSSearchParams::direction, cl_px4_mr::FlightPatternVSSearchParams::initialHeading, cl_px4_mr::FlightPatternVSSearchParams::radius, cl_px4_mr::FlightPatternVSSearchParams::reorientation, turnSign(), cl_px4_mr::NedPoint::x, cl_px4_mr::NedPoint::y, cl_px4_mr::NedPoint::yaw, and cl_px4_mr::NedPoint::z.

Referenced by cl_px4_mr::CbVSSearch::buildPath().

Here is the call graph for this function:

Here is the caller graph for this function:

haversineDistance() [1/2]

double cl_px4_mr::haversineDistance ( const GeoPoint & a, const GeoPoint & b )

inline

Definition at line 169 of file geo_utils.hpp.

{
  return haversineDistance(a.lat, a.lon, b.lat, b.lon);
}

References haversineDistance(), cl_px4_mr::GeoPoint::lat, and cl_px4_mr::GeoPoint::lon.

Here is the call graph for this function:

haversineDistance() [2/2]

double cl_px4_mr::haversineDistance ( double lat1, double lon1, double lat2, double lon2 )

inline

Definition at line 156 of file geo_utils.hpp.

{
  const double p1 = geo_detail::radians(lat1);
  const double p2 = geo_detail::radians(lat2);
  const double dLat = p2 - p1;
  const double dLon = geo_detail::radians(lon2 - lon1);
 
  const double a = std::sin(dLat / 2.0) * std::sin(dLat / 2.0) +
                   std::sin(dLon / 2.0) * std::sin(dLon / 2.0) * std::cos(p1) * std::cos(p2);
  const double c = 2.0 * std::atan2(std::sqrt(a), std::sqrt(1.0 - a));
  return kEarthRadiusM * c;
}

References kEarthRadiusM, and cl_px4_mr::geo_detail::radians().

Referenced by haversineDistance().

Here is the call graph for this function:

Here is the caller graph for this function:

initialBearing()

double cl_px4_mr::initialBearing ( double lat1, double lon1, double lat2, double lon2 )

inline

Definition at line 176 of file geo_utils.hpp.

{
  const double p1 = geo_detail::radians(lat1);
  const double p2 = geo_detail::radians(lat2);
  const double dLon = geo_detail::radians(lon2 - lon1);
 
  const double y = std::sin(dLon) * std::cos(p2);
  const double x = std::cos(p1) * std::sin(p2) - std::sin(p1) * std::cos(p2) * std::cos(dLon);
  return std::atan2(y, x);
}

References cl_px4_mr::geo_detail::radians().

Here is the call graph for this function:

legHeading()

float cl_px4_mr::legHeading ( const NedPoint & from, const NedPoint & to )

inline

Definition at line 239 of file pattern_generators.hpp.

{
  return std::atan2(to.y - from.y, to.x - from.x);
}

References cl_px4_mr::NedPoint::x, and cl_px4_mr::NedPoint::y.

Referenced by cl_px4_mr::CbSineWaveHorizontal::buildPath().

Here is the caller graph for this function:

levelRotationFromPx4Quaternion()

void cl_px4_mr::levelRotationFromPx4Quaternion ( const std::array< float, 4 > & q, std::array< float, 9 > & r )

inline

Definition at line 30 of file level_frame.hpp.

{
  // px4_ros_com frame_transforms constants (tf2 order x, y, z, w)
  const tf2::Quaternion kNedEnuQ(0.70710678118654752, 0.70710678118654752, 0.0, 0.0);
  const tf2::Quaternion kFrdFluQ(1.0, 0.0, 0.0, 0.0);
  const tf2::Quaternion qNedFrd(q[1], q[2], q[3], q[0]);
  tf2::Quaternion qEnuFlu = kNedEnuQ * qNedFrd * kFrdFluQ;
  qEnuFlu.normalize();
  const tf2::Matrix3x3 m(qEnuFlu);
  const double yaw = std::atan2(m[1][0], m[0][0]);
  tf2::Matrix3x3 unyaw;
  unyaw.setRPY(0.0, 0.0, -yaw);
  const tf2::Matrix3x3 level = unyaw * m;
  for (int i = 0; i < 3; ++i)
  {
    for (int j = 0; j < 3; ++j)
    {
      r[i * 3 + j] = static_cast<float>(level[i][j]);
    }
  }
}

Referenced by cl_px4_mr::CpTunnelCentering::onCloud().

Here is the caller graph for this function:

nedDistance()

float cl_px4_mr::nedDistance ( const NedPoint & a, const NedPoint & b )

inline

Definition at line 188 of file geo_utils.hpp.

{
  const float dx = b.x - a.x;
  const float dy = b.y - a.y;
  const float dz = b.z - a.z;
  return std::sqrt(dx * dx + dy * dy + dz * dz);
}

References cl_px4_mr::NedPoint::x, cl_px4_mr::NedPoint::y, and cl_px4_mr::NedPoint::z.

Referenced by cumulativeLengths(), generateFlightPatternGridPattern(), cl_px4_mr::CbPx4PathFollowerBase::onEntry(), resamplePolyline(), cl_px4_mr::CbFollowNedPath::setPath(), and cl_px4_mr::CbPx4PathFollowerBase::update().

Here is the caller graph for this function:

nedDistanceXY()

float cl_px4_mr::nedDistanceXY ( const NedPoint & a, const NedPoint & b )

inline

Definition at line 196 of file geo_utils.hpp.

{
  const float dx = b.x - a.x;
  const float dy = b.y - a.y;
  return std::sqrt(dx * dx + dy * dy);
}

References cl_px4_mr::NedPoint::x, and cl_px4_mr::NedPoint::y.

Referenced by cl_px4_mr::CbPx4PathFollowerBase::update().

Here is the caller graph for this function:

polylineLength()

float cl_px4_mr::polylineLength ( const std::vector< NedPoint > & path )

inline

Definition at line 221 of file geo_utils.hpp.

{
  if (path.empty())
  {
    return 0.0f;
  }
  return cumulativeLengths(path).back();
}

References cumulativeLengths().

Here is the call graph for this function:

resamplePolyline()

std::vector< NedPoint > cl_px4_mr::resamplePolyline ( const std::vector< NedPoint > & path, float spacing )

inline

Definition at line 271 of file geo_utils.hpp.

{
  std::vector<NedPoint> out;
  if (path.empty() || spacing <= 0.0f)
  {
    return path;
  }
  out.push_back(path.front());
  for (size_t i = 1; i < path.size(); ++i)
  {
    const NedPoint & a = path[i - 1];
    const NedPoint & b = path[i];
    const float len = nedDistance(a, b);
    const int n = std::max(1, static_cast<int>(std::ceil(len / spacing)));
    for (int k = 1; k < n; ++k)
    {
      const float t = static_cast<float>(k) / static_cast<float>(n);
      NedPoint p;
      p.x = a.x + t * (b.x - a.x);
      p.y = a.y + t * (b.y - a.y);
      p.z = a.z + t * (b.z - a.z);
      out.push_back(p);
    }
    out.push_back(b);
  }
  return out;
}

References nedDistance(), cl_px4_mr::NedPoint::x, cl_px4_mr::NedPoint::y, and cl_px4_mr::NedPoint::z.

Here is the call graph for this function:

sampleAtArcLength()

NedPoint cl_px4_mr::sampleAtArcLength ( const std::vector< NedPoint > & path, const std::vector< float > & cum, float s, size_t * segmentIndex = nullptr )

inline

Definition at line 233 of file geo_utils.hpp.

{
  if (path.empty())
  {
    return NedPoint{};
  }
  if (path.size() == 1 || s <= 0.0f)
  {
    if (segmentIndex) *segmentIndex = 0;
    return path.front();
  }
  if (s >= cum.back())
  {
    if (segmentIndex) *segmentIndex = path.size() - 2;
    return path.back();
  }
 
  // first vertex whose cumulative length exceeds s
  auto it = std::upper_bound(cum.begin(), cum.end(), s);
  size_t i1 = static_cast<size_t>(it - cum.begin());
  size_t i0 = i1 - 1;
  const float segLen = cum[i1] - cum[i0];
  const float t = segLen > 0.0f ? (s - cum[i0]) / segLen : 0.0f;
 
  if (segmentIndex) *segmentIndex = i0;
 
  NedPoint p;
  p.x = path[i0].x + t * (path[i1].x - path[i0].x);
  p.y = path[i0].y + t * (path[i1].y - path[i0].y);
  p.z = path[i0].z + t * (path[i1].z - path[i0].z);
  p.yaw = path[i0].yaw;  // yaw is per-vertex, not interpolated
  return p;
}

References cl_px4_mr::NedPoint::x, cl_px4_mr::NedPoint::y, cl_px4_mr::NedPoint::yaw, and cl_px4_mr::NedPoint::z.

Referenced by cl_px4_mr::CbPx4PathFollowerBase::commandFor().

Here is the caller graph for this function:

turnName()

const char * cl_px4_mr::turnName ( Turn t )

inline

Definition at line 58 of file pattern_generators.hpp.

{ return t == Turn::RIGHT ? "RIGHT" : "LEFT"; }

References RIGHT.

Referenced by cl_px4_mr::CbLemniscate::buildPath(), cl_px4_mr::CbLoiter::buildPath(), cl_px4_mr::CbSpiral::buildPath(), cl_px4_mr::CbSquareSpiral::buildPath(), and cl_px4_mr::CbVSSearch::buildPath().

Here is the caller graph for this function:

turnSign()

float cl_px4_mr::turnSign ( Turn t )

inline

Definition at line 57 of file pattern_generators.hpp.

{ return t == Turn::RIGHT ? 1.0f : -1.0f; }

References RIGHT.

Referenced by generateFlightPatternFigureEight(), generateFlightPatternLawnmower(), generateFlightPatternLoiter(), generateFlightPatternSpiral(), generateFlightPatternSquareSpiral(), and generateFlightPatternVSSearch().

Here is the caller graph for this function:

wrapPi()

float cl_px4_mr::wrapPi ( float angle )

inline

Definition at line 25 of file angle_utils.hpp.

{
  angle = std::fmod(angle + static_cast<float>(M_PI), 2.0f * static_cast<float>(M_PI));
  if (angle < 0.0f)
  {
    angle += 2.0f * static_cast<float>(M_PI);
  }
  return angle - static_cast<float>(M_PI);
}

Referenced by cl_px4_mr::CbPx4PathFollowerBase::applyCentering(), cl_px4_mr::CbPx4PathFollowerBase::commandFor(), cl_px4_mr::CbSineAltitudeCruise::onEntry(), cl_px4_mr::CbWaitForHeadingStable::onEntry(), cl_px4_mr::CbYawScan::onEntry(), and cl_px4_mr::CbYawScan::update().

Here is the caller graph for this function:

Variable Documentation

kEarthRadiusM

double cl_px4_mr::kEarthRadiusM = 6371000.0

constexpr

Definition at line 59 of file geo_utils.hpp.

Referenced by haversineDistance(), cl_px4_mr::MapProjection::project(), and cl_px4_mr::MapProjection::reproject().