Team2890/allwpilib
4
0
Fork 0
mirror of https://github.com/wpilibsuite/allwpilib synced 2026-06-20 00:51:42 +00:00
Code Issues Packages Projects Releases Wiki Activity

[wpimath] Rework odometry APIs to improve feature parity (#4645)

Browse Source 
Co-authored-by: Ryan Blue <ryanzblue@gmail.com>
This commit is contained in:
Jordan McMichael
2022-11-18 23:42:00 -05:00
committed by GitHub
parent e2d49181da
commit 902e8686d3
53 changed files with 266 additions and 157 deletions
Download Patch File Download Diff File Expand all files Collapse all files

13
wpimath/src/main/native/cpp/estimator/DifferentialDrivePoseEstimator.cpp
View File

@@ -12,7 +12,8 @@
using namespace frc;
DifferentialDrivePoseEstimator::DifferentialDrivePoseEstimator(
const Rotation2d& gyroAngle, const Pose2d& initialPose,
const Rotation2d& gyroAngle, units::meter_t leftDistance,
units::meter_t rightDistance, const Pose2d& initialPose,
const wpi::array<double, 5>& stateStdDevs,
const wpi::array<double, 3>& localMeasurementStdDevs,
const wpi::array<double, 3>& visionMeasurmentStdDevs,
@@ -41,7 +42,7 @@ DifferentialDrivePoseEstimator::DifferentialDrivePoseEstimator(
m_gyroOffset = initialPose.Rotation() - gyroAngle;
m_previousAngle = initialPose.Rotation();
m_observer.SetXhat(FillStateVector(initialPose, 0_m, 0_m));
m_observer.SetXhat(FillStateVector(initialPose, leftDistance, rightDistance));
}
void DifferentialDrivePoseEstimator::SetVisionMeasurementStdDevs(
@@ -50,13 +51,15 @@ void DifferentialDrivePoseEstimator::SetVisionMeasurementStdDevs(
m_visionContR = frc::MakeCovMatrix(visionMeasurmentStdDevs);
}
void DifferentialDrivePoseEstimator::ResetPosition(
const Pose2d& pose, const Rotation2d& gyroAngle) {
void DifferentialDrivePoseEstimator::ResetPosition(const Rotation2d& gyroAngle,
units::meter_t leftDistance,
units::meter_t rightDistance,
const Pose2d& pose) {
// Reset state estimate and error covariance
m_observer.Reset();
m_poseBuffer.Clear();
m_observer.SetXhat(FillStateVector(pose, 0_m, 0_m));
m_observer.SetXhat(FillStateVector(pose, leftDistance, rightDistance));
m_prevTime = -1_s;

8
wpimath/src/main/native/cpp/estimator/MecanumDrivePoseEstimator.cpp
View File

@@ -12,8 +12,8 @@
using namespace frc;
frc::MecanumDrivePoseEstimator::MecanumDrivePoseEstimator(
const Rotation2d& gyroAngle, const Pose2d& initialPose,
const MecanumDriveWheelPositions& wheelPositions,
const Rotation2d& gyroAngle,
const MecanumDriveWheelPositions& wheelPositions, const Pose2d& initialPose,
MecanumDriveKinematics kinematics,
const wpi::array<double, 7>& stateStdDevs,
const wpi::array<double, 5>& localMeasurementStdDevs,
@@ -67,8 +67,8 @@ void frc::MecanumDrivePoseEstimator::SetVisionMeasurementStdDevs(
}
void frc::MecanumDrivePoseEstimator::ResetPosition(
const Pose2d& pose, const Rotation2d& gyroAngle,
const MecanumDriveWheelPositions& wheelPositions) {
const Rotation2d& gyroAngle,
const MecanumDriveWheelPositions& wheelPositions, const Pose2d& pose) {
// Reset state estimate and error covariance
m_observer.Reset();
m_poseBuffer.Clear();

7
wpimath/src/main/native/cpp/kinematics/DifferentialDriveOdometry.cpp
View File

@@ -9,8 +9,11 @@
using namespace frc;
DifferentialDriveOdometry::DifferentialDriveOdometry(
const Rotation2d& gyroAngle, const Pose2d& initialPose)
: m_pose(initialPose) {
const Rotation2d& gyroAngle, units::meter_t leftDistance,
units::meter_t rightDistance, const Pose2d& initialPose)
: m_pose(initialPose),
m_prevLeftDistance(leftDistance),
m_prevRightDistance(rightDistance) {
m_previousAngle = m_pose.Rotation();
m_gyroOffset = m_pose.Rotation() - gyroAngle;
wpi::math::MathSharedStore::ReportUsage(

4
wpimath/src/main/native/cpp/kinematics/MecanumDriveOdometry.cpp
View File

@@ -10,7 +10,7 @@ using namespace frc;
MecanumDriveOdometry::MecanumDriveOdometry(
MecanumDriveKinematics kinematics, const Rotation2d& gyroAngle,
const MecanumDriveWheelPositions wheelPositions, const Pose2d& initialPose)
const MecanumDriveWheelPositions& wheelPositions, const Pose2d& initialPose)
: m_kinematics(kinematics),
m_pose(initialPose),
m_previousWheelPositions(wheelPositions) {
@@ -22,7 +22,7 @@ MecanumDriveOdometry::MecanumDriveOdometry(
const Pose2d& MecanumDriveOdometry::Update(
const Rotation2d& gyroAngle,
const MecanumDriveWheelPositions wheelPositions) {
const MecanumDriveWheelPositions& wheelPositions) {
auto angle = gyroAngle + m_gyroOffset;
MecanumDriveWheelPositions wheelDeltas{