117 lines
4.6 KiB
C++
117 lines
4.6 KiB
C++
#include <doctest/doctest.h>
|
|
|
|
#include "fgc/Geometry.h"
|
|
|
|
using namespace fgc;
|
|
|
|
// Fields in order: counts_per_deg, center_count, deg_at_center, min_deg, max_deg.
|
|
|
|
TEST_CASE("AxisMap converts degrees and counts about the anchored centre") {
|
|
AxisMap m{983.33, 500000, 0.0, -90.0, 90.0};
|
|
|
|
CHECK(m.toCounts(0.0) == 500000);
|
|
CHECK(m.toCounts(90.0) == 588500); // lround(500000 + 90*983.33)
|
|
CHECK(m.toDeg(500000) == doctest::Approx(0.0));
|
|
CHECK(m.toDeg(m.toCounts(45.0)) == doctest::Approx(45.0).epsilon(0.001));
|
|
}
|
|
|
|
TEST_CASE("a non-zero deg_at_center shifts the whole map") {
|
|
// The centre points at 137.4 deg true; everything else follows from that.
|
|
AxisMap m{-529.8, 500000, 137.4, 137.4 - 160.0, 137.4 + 160.0};
|
|
|
|
CHECK(m.toCounts(137.4) == 500000);
|
|
CHECK(m.toDeg(500000) == doctest::Approx(137.4));
|
|
// Negative counts_per_deg: a higher heading is a lower count.
|
|
CHECK(m.toCounts(147.4) < 500000);
|
|
CHECK(m.toDeg(m.toCounts(200.0)) == doctest::Approx(200.0).epsilon(0.001));
|
|
}
|
|
|
|
TEST_CASE("AxisMap clamps to the soft range on the way out") {
|
|
AxisMap m{10.0, 0, 0.0, -90.0, 90.0};
|
|
CHECK(m.toCounts(200.0) == 900); // clamped to +90
|
|
CHECK(m.toCounts(-200.0) == -900); // clamped to -90
|
|
CHECK(m.toCounts(45.0) == 450);
|
|
}
|
|
|
|
TEST_CASE("AxisMap supports a negative (flipped) direction") {
|
|
AxisMap m{-10.0, 1000, 0.0, -90.0, 90.0};
|
|
CHECK(m.toCounts(10.0) == 900);
|
|
CHECK(m.toDeg(900) == doctest::Approx(10.0));
|
|
}
|
|
|
|
TEST_CASE("AxisMap toDeg guards against a zero scale") {
|
|
// Degrades to "we are at the centre", not to an unrelated zero.
|
|
AxisMap m{0.0, 0, 42.0, -90.0, 90.0};
|
|
CHECK(m.toDeg(12345) == doctest::Approx(42.0));
|
|
}
|
|
|
|
TEST_CASE("wrap360 and wrap180 normalise at and across the boundaries") {
|
|
CHECK(wrap360(0.0) == doctest::Approx(0.0));
|
|
CHECK(wrap360(360.0) == doctest::Approx(0.0));
|
|
CHECK(wrap360(361.0) == doctest::Approx(1.0));
|
|
CHECK(wrap360(-1.0) == doctest::Approx(359.0));
|
|
CHECK(wrap360(-361.0) == doctest::Approx(359.0));
|
|
CHECK(wrap360(180.0) == doctest::Approx(180.0));
|
|
// A tiny negative must land at 0, never at exactly 360.
|
|
CHECK(wrap360(-1e-15) < 360.0);
|
|
|
|
CHECK(wrap180(0.0) == doctest::Approx(0.0));
|
|
CHECK(wrap180(179.0) == doctest::Approx(179.0));
|
|
CHECK(wrap180(180.0) == doctest::Approx(-180.0));
|
|
CHECK(wrap180(181.0) == doctest::Approx(-179.0));
|
|
CHECK(wrap180(-179.0) == doctest::Approx(-179.0));
|
|
CHECK(wrap180(359.0) == doctest::Approx(-1.0));
|
|
}
|
|
|
|
TEST_CASE("headingDeg reports a compass bearing regardless of the linear degree") {
|
|
// Centre at 350 deg true; +20 deg of travel crosses north.
|
|
AxisMap m{100.0, 500000, 350.0, 350.0 - 160.0, 350.0 + 160.0};
|
|
|
|
CHECK(m.headingDeg(500000) == doctest::Approx(350.0));
|
|
// Linear 370 deg is the compass bearing 10 deg.
|
|
CHECK(m.headingDeg(m.toCounts(370.0)) == doctest::Approx(10.0));
|
|
}
|
|
|
|
TEST_CASE("headingToCounts reaches across north") {
|
|
AxisMap m{100.0, 500000, 350.0, 350.0 - 160.0, 350.0 + 160.0};
|
|
|
|
// 10 deg true is 20 deg past the centre the short way, well inside travel.
|
|
auto c = m.headingToCounts(10.0);
|
|
REQUIRE(c.has_value());
|
|
CHECK(*c == 502000);
|
|
CHECK(m.headingDeg(*c) == doctest::Approx(10.0));
|
|
|
|
// And the other way, back past north.
|
|
auto d = m.headingToCounts(330.0);
|
|
REQUIRE(d.has_value());
|
|
CHECK(*d == 498000);
|
|
}
|
|
|
|
TEST_CASE("headingToCounts refuses the dead arc instead of clamping to a limit") {
|
|
// 320 deg of travel centred on 0 deg true: reachable is 200..360..160.
|
|
AxisMap m{100.0, 500000, 0.0, -160.0, 160.0};
|
|
|
|
CHECK(m.headingToCounts(90.0).has_value());
|
|
CHECK(m.headingToCounts(270.0).has_value()); // == -90, inside
|
|
CHECK(m.headingToCounts(160.0).has_value()); // exactly on the limit
|
|
CHECK_FALSE(m.headingToCounts(180.0).has_value()); // dead centre of the gap
|
|
CHECK_FALSE(m.headingToCounts(170.0).has_value());
|
|
CHECK_FALSE(m.headingToCounts(190.0).has_value());
|
|
|
|
// The point of returning nullopt: 180 must NOT come back as the 160 limit.
|
|
CHECK(m.headingToCounts(180.0) != m.headingToCounts(160.0));
|
|
}
|
|
|
|
TEST_CASE("headingToCounts refuses a degenerate scale") {
|
|
AxisMap m{0.0, 500000, 0.0, -160.0, 160.0};
|
|
CHECK_FALSE(m.headingToCounts(90.0).has_value());
|
|
}
|
|
|
|
TEST_CASE("encoderPitchDeg falls back to the fixed elevation without a pitch encoder") {
|
|
AxisMap m{8333.33, 500000, 5.0, -30.0, 30.0};
|
|
CHECK(encoderPitchDeg(m, 500000) == doctest::Approx(5.0));
|
|
CHECK(encoderPitchDeg(m, 500000 + 83333) == doctest::Approx(15.0).epsilon(1e-4));
|
|
// A yaw-only tower: not toDeg(0), which would read about -55 deg here.
|
|
CHECK(encoderPitchDeg(m, std::nullopt) == doctest::Approx(5.0));
|
|
}
|