Skip to content

Got manual 'soft' yaw reset (ignores translational coordinate system)… #3

New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Merged
merged 1 commit into from
Jan 28, 2018
Merged

Got manual 'soft' yaw reset (ignores translational coordinate system)… #3

Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
1 commit
Select commit
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
23 changes: 20 additions & 3 deletions OrientationReader.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -39,13 +39,18 @@ namespace je_nourish_fusion {
osvrClientGetInterface(ctx, orientation_paths["yawFast"].asCString(), &(m_orientations[2]));
osvrClientGetInterface(ctx, orientation_paths["yawAccurate"].asCString(), &(m_orientations[3]));
m_alpha = orientation_paths["alpha"].asDouble();
m_do_soft_reset = orientation_paths["softReset"].asBool();
if (orientation_paths["recenterButton"].isNull()) {
m_do_instant_reset = false;
}
else {
m_do_instant_reset = true;
osvrClientGetInterface(ctx, orientation_paths["recenterButton"].asCString(), &m_instant_reset_path);
m_ctx.log(OSVR_LogLevel::OSVR_LOGLEVEL_INFO, "Recenter button detection is enabled in OSVR-Fusion.");
if (m_do_soft_reset) {
m_ctx.log(OSVR_LogLevel::OSVR_LOGLEVEL_INFO, "Soft recenter is enabled in OSVR-Fusion.");
osvrRegisterButtonCallback(m_instant_reset_path, &(je_nourish_fusion::soft_reset_callback), this);
}
}

m_last_yaw = 0;
Expand All @@ -54,6 +59,18 @@ namespace je_nourish_fusion {
m_ctx.log(OSVR_LogLevel::OSVR_LOGLEVEL_INFO, "Initialized a complementary fusion filter.");
}

void soft_reset_callback(void* userdata, const OSVR_TimeValue *timestamp, const OSVR_ButtonReport *report) {
FilteredOrientationReader *reader = (FilteredOrientationReader*)userdata;
if (report->state == OSVR_BUTTON_PRESSED) {
reader->m_yaw_offset = reader->m_yaw_raw;
reader->m_ctx.log(OSVR_LogLevel::OSVR_LOGLEVEL_INFO, "Button was pressed.");
}
reader->m_ctx.log(OSVR_LogLevel::OSVR_LOGLEVEL_INFO, "Button callback received.");
//std::cout << "Got button report: button is " << (report->state ? "pressed" : "released") << std::endl << std::flush;

}


OSVR_ReturnCode CombinedOrientationReader::update(OSVR_OrientationState* orientation, OSVR_TimeValue* timeValue) {
OSVR_OrientationState orientation_x;
OSVR_OrientationState orientation_y;
Expand Down Expand Up @@ -143,13 +160,14 @@ namespace je_nourish_fusion {
// Is the angular velocity low?
// Was the reset button recently or currently pressed?
// If all conditions are true, then we probably detected a yaw reset, so make it snappy.
if (m_do_instant_reset && (button_time_diff < 0.5) &&
if (m_do_instant_reset && !m_do_soft_reset && (button_time_diff < 0.5) &&
((z_diff < -z_displacement_limit) || (z_diff > z_displacement_limit)) && ((dzdt_fast < z_angular_limit) && (dzdt_fast > -z_angular_limit))) {
z_out = z_accurate;
}
// If instantReset is not enabled or if the checks are not met, carry on with regular filter implementation.
else {
z_out = a*(z_fast)+(1 - a)*(z_accurate);
m_yaw_raw = z_accurate;
}

// Replace bogus results with accurate yaw. Happens sometimes on startup.
Expand All @@ -167,11 +185,10 @@ namespace je_nourish_fusion {
OSVR_Vec3 rpy;
osvrVec3SetX(&rpy, osvrVec3GetX(&rpy_x));
osvrVec3SetY(&rpy, osvrVec3GetY(&rpy_y));
osvrVec3SetZ(&rpy, z_out);
osvrVec3SetZ(&rpy, z_out - m_yaw_offset);

quaternionFromRPY(&rpy, orientation);

return OSVR_RETURN_SUCCESS;
}

}
6 changes: 5 additions & 1 deletion OrientationReader.h
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -32,14 +32,18 @@ namespace je_nourish_fusion {
public:
FilteredOrientationReader(OSVR_ClientContext ctx, Json::Value orientation_paths);
OSVR_ReturnCode update(OSVR_OrientationState* orientation, OSVR_TimeValue* timeValue);
double m_yaw_raw;
double m_yaw_offset;
osvr::clientkit::ClientContext m_ctx;
protected:
OSVR_ClientInterface m_orientations[4];
osvr::clientkit::ClientContext m_ctx;
bool m_do_instant_reset;
bool m_do_soft_reset;
OSVR_ClientInterface m_instant_reset_path;
OSVR_TimeValue m_reset_press_time;
double m_alpha;
double m_last_yaw;
};

void soft_reset_callback(void* userdata, const OSVR_TimeValue *timestamp, const OSVR_ButtonReport *report);
}