mirror of
https://github.com/UberGuidoZ/Flipper.git
synced 2024-12-23 15:00:13 +00:00
96 lines
2.8 KiB
C++
96 lines
2.8 KiB
C++
/*
|
|
* Copyright 2019 Google Inc. All Rights Reserved.
|
|
*
|
|
* Licensed under the Apache License, Version 2.0 (the "License");
|
|
* you may not use this file except in compliance with the License.
|
|
* You may obtain a copy of the License at
|
|
*
|
|
* http://www.apache.org/licenses/LICENSE-2.0
|
|
*
|
|
* Unless required by applicable law or agreed to in writing, software
|
|
* distributed under the License is distributed on an "AS IS" BASIS,
|
|
* WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
|
|
* See the License for the specific language governing permissions and
|
|
* limitations under the License.
|
|
*/
|
|
#include "orientation_tracker.h"
|
|
|
|
#include "sensors/pose_prediction.h"
|
|
#include "util/logging.h"
|
|
#include "util/vector.h"
|
|
#include "util/vectorutils.h"
|
|
|
|
namespace cardboard {
|
|
|
|
OrientationTracker::OrientationTracker(const long sampling_period_ns)
|
|
: sampling_period_ns_(sampling_period_ns)
|
|
, calibration_(Vector3::Zero())
|
|
, is_tracking_(false)
|
|
, sensor_fusion_(new SensorFusionEkf())
|
|
, latest_gyroscope_data_({ 0, 0, Vector3::Zero() })
|
|
{
|
|
sensor_fusion_->SetBiasEstimationEnabled(/*kGyroBiasEstimationEnabled*/ true);
|
|
}
|
|
|
|
void OrientationTracker::SetCalibration(const Vector3& calibration) {
|
|
calibration_ = calibration;
|
|
}
|
|
|
|
void OrientationTracker::Pause()
|
|
{
|
|
if (!is_tracking_) {
|
|
return;
|
|
}
|
|
|
|
// Create a gyro event with zero velocity. This effectively stops the prediction.
|
|
GyroscopeData event = latest_gyroscope_data_;
|
|
event.data = Vector3::Zero();
|
|
|
|
OnGyroscopeData(event);
|
|
|
|
is_tracking_ = false;
|
|
}
|
|
|
|
void OrientationTracker::Resume() { is_tracking_ = true; }
|
|
|
|
Vector4 OrientationTracker::GetPose(int64_t timestamp_ns) const
|
|
{
|
|
Rotation predicted_rotation;
|
|
const PoseState pose_state = sensor_fusion_->GetLatestPoseState();
|
|
if (sensor_fusion_->IsFullyInitialized()) {
|
|
predicted_rotation = pose_state.sensor_from_start_rotation;
|
|
} else {
|
|
CARDBOARD_LOGI("Tracker not fully initialized yet. Using pose prediction only.");
|
|
predicted_rotation = pose_prediction::PredictPose(timestamp_ns, pose_state);
|
|
}
|
|
|
|
return (-predicted_rotation).GetQuaternion();
|
|
}
|
|
|
|
void OrientationTracker::OnAccelerometerData(const AccelerometerData& event)
|
|
{
|
|
if (!is_tracking_) {
|
|
return;
|
|
}
|
|
sensor_fusion_->ProcessAccelerometerSample(event);
|
|
}
|
|
|
|
Vector4 OrientationTracker::OnGyroscopeData(const GyroscopeData& event)
|
|
{
|
|
if (!is_tracking_) {
|
|
return Vector4();
|
|
}
|
|
|
|
const GyroscopeData data = { .system_timestamp = event.system_timestamp,
|
|
.sensor_timestamp_ns = event.sensor_timestamp_ns,
|
|
.data = event.data - calibration_ };
|
|
|
|
latest_gyroscope_data_ = data;
|
|
|
|
sensor_fusion_->ProcessGyroscopeSample(data);
|
|
|
|
return OrientationTracker::GetPose(data.sensor_timestamp_ns + sampling_period_ns_);
|
|
}
|
|
|
|
} // namespace cardboard
|