60 lines
2.4 KiB
C++
60 lines
2.4 KiB
C++
#pragma once
|
|
|
|
#include <optional>
|
|
|
|
namespace fgc {
|
|
|
|
// Normalise an angle to [0, 360) / [-180, 180). Free functions because headings
|
|
// are compared and differenced in several places outside AxisMap.
|
|
double wrap360(double deg);
|
|
double wrap180(double deg);
|
|
|
|
// Affine map between degrees and absolute encoder counts for one axis, anchored
|
|
// on the *mechanical centre* found by homing:
|
|
//
|
|
// counts = center_count + (deg - deg_at_center) * counts_per_deg
|
|
// deg = deg_at_center + (counts - center_count) / counts_per_deg
|
|
//
|
|
// The firmware speaks only in encoder counts. `center_count` is the count the
|
|
// firmware assigns to the midpoint of the homed travel (it is told this value,
|
|
// see the CENTER command), so it is reproducible across power cycles - unlike the
|
|
// free-running encoder origin it replaced. `deg_at_center` is what that midpoint
|
|
// points at: a true compass heading for yaw (0 = north), an elevation above level
|
|
// for pitch (+ = up).
|
|
//
|
|
// min_deg/max_deg are DERIVED from the homed soft limits after each home, not
|
|
// configured - the firmware's limits are the only place they can be correct.
|
|
// counts_per_deg may be negative to flip the direction sense.
|
|
struct AxisMap {
|
|
double counts_per_deg = 1.0; // calibrate on the rig
|
|
long center_count = 500000; // count at the homed mechanical centre
|
|
double deg_at_center = 0.0; // heading/elevation there
|
|
double min_deg = -100000.0; // soft clamp applied by toCounts (degrees)
|
|
double max_deg = 100000.0;
|
|
|
|
// Degrees -> counts. Clamps `deg` to [min_deg, max_deg] and rounds.
|
|
long toCounts(double deg) const;
|
|
|
|
// Counts -> degrees. Returns deg_at_center if counts_per_deg is ~0
|
|
// (misconfigured), i.e. it degrades to "we are at the centre" rather than to
|
|
// an arbitrary zero.
|
|
double toDeg(long counts) const;
|
|
|
|
// Yaw only: the same reading as a compass heading in [0, 360).
|
|
double headingDeg(long counts) const;
|
|
|
|
// Yaw only: counts for a true heading, taking the short way round. Returns
|
|
// nullopt when the heading lies in the arc the gimbal cannot reach - the
|
|
// travel is under 360 deg, so there is always a dead sector, and pointing at
|
|
// the nearest limit instead would silently image the wrong direction.
|
|
std::optional<long> headingToCounts(double heading_deg) const;
|
|
};
|
|
|
|
// Per-axis maps for the two-axis gimbal.
|
|
struct Geometry {
|
|
AxisMap yaw;
|
|
AxisMap pitch;
|
|
};
|
|
|
|
} // namespace fgc
|