A-Team_Digital_Solutions/android_kernel_motorola_sm6375
Watch
2
0
Fork
You've already forked android_kernel_motorola_sm6375
 0
mirror of https://github.com/BobTheBlinker/android_kernel_motorola_sm6375.git synced 2026-10-07 04:12:04 -04:00
Code Issues Projects Releases Packages Wiki Activity

Merge "HID: qvr: Correct axes orientation"

Browse source
This commit is contained in:
qctecmdr 2020-02-21 19:28:05 -08:00 • committed by Gerrit - the friendly Code Review server
commit f8abd34ab6
1 changed files with 54 additions and 15 deletions
Download patch file Download diff file Expand all files Collapse all files

69
drivers/hid/hid-qvr.c
View file

@ -46,7 +46,8 @@
#include "hid-qvr.h"
#include "hid-trace.h"
#define WAIT_EVENT_INT_TOUT 1000
#define TIME_OUT_START_STOP_MS 500
#define TIME_OUT_READ_WRITE_MS 20
#define QVR_START_IMU _IO('q', 1)
#define QVR_STOP_IMU _IO('q', 2)
@ -105,6 +106,7 @@ struct qvr_external_sensor {
static DECLARE_WAIT_QUEUE_HEAD(wq);
static struct qvr_external_sensor qvr_external_sensor;
static uint8_t DEBUG_ORIENTATION;
static int read_calibration_len(void)
{
@ -127,7 +129,7 @@ static int read_calibration_len(void)
ret = wait_event_interruptible_timeout(wq,
sensor->calib_data_len != -1,
msecs_to_jiffies(WAIT_EVENT_INT_TOUT));
msecs_to_jiffies(TIME_OUT_READ_WRITE_MS));
if (ret == 0) {
kfree(hid_buf);
return -ETIME;
@ -172,7 +174,7 @@ static uint8_t *read_calibration_data(void)
HID_REQ_SET_REPORT);
ret = wait_event_interruptible_timeout(wq,
sensor->calib_data_recv == 1,
msecs_to_jiffies(WAIT_EVENT_INT_TOUT));
msecs_to_jiffies(TIME_OUT_READ_WRITE_MS));
if (ret == 0) {
pr_err("%s:get calibration data timeout\n", __func__);
kfree(hid_buf);
@ -220,7 +222,7 @@ static int control_imu_stream(bool status)
HID_FEATURE_REPORT,
HID_REQ_SET_REPORT);
ret = wait_event_interruptible_timeout(wq, sensor->ext_ack == 1,
msecs_to_jiffies(WAIT_EVENT_INT_TOUT));
msecs_to_jiffies(TIME_OUT_START_STOP_MS));
if (ret && status) {
pr_debug("qvr: falling back - start IMU stream failed\n");
hid_buf[0] = QVR_HID_REPORT_ID_CAL;
@ -237,7 +239,6 @@ static int control_imu_stream(bool status)
return -ETIME;
}
static int qvr_send_package_wrap(u8 *message, int msize, struct hid_device *hid)
{
struct qvr_external_sensor *sensor = &qvr_external_sensor;
@ -254,7 +255,32 @@ static int qvr_send_package_wrap(u8 *message, int msize, struct hid_device *hid)
memcpy((void *)&imuData, (void *)message,
sizeof(struct external_imu_format));
if (!sensor->ts_base) {
if (imuData.gNumerator == 1 && imuData.aNumerator == 1)
DEBUG_ORIENTATION = 1;
else
DEBUG_ORIENTATION = 0;
pr_debug("qvr msize = %d reportID=%d padding=%d\n"
"qvr version=%d numImu=%d nspip=%d pSize=%d\n"
"qvr imuID=%d sampleID=%d temp=%d\n",
msize, imuData.reportID, imuData.padding,
imuData.version, imuData.numIMUs,
imuData.numSamplesPerImuPacket,
imuData.totalPayloadSize, imuData.imuID,
imuData.sampleID, imuData.temperature);
pr_debug("qvr gts0=%llu num=%d denom=%d\n"
"qvr gx0=%d gy0=%d gz0=%d\n",
imuData.gts0, imuData.gNumerator, imuData.gDenominator,
imuData.gx0, imuData.gy0, imuData.gz0);
pr_debug("qvr ats0=%llu num=%d denom=%d\n"
"qvr ax0=%d ay0=%d az0=%d\n",
imuData.ats0, imuData.aNumerator, imuData.aDenominator,
imuData.ax0, imuData.ay0, imuData.az0);
pr_debug("qvr mts0=%llu num=%d denom=%d\n"
"mx0=%d my0=%d mz0=%d\n",
imuData.mts0, imuData.mNumerator, imuData.mDenominator,
imuData.mx0, imuData.my0, imuData.mz0);
}
if (!sensor->ts_base)
sensor->ts_base = ktime_to_ns(ktime_get_boottime());
if (!sensor->ts_offset)
@ -276,15 +302,28 @@ static int qvr_send_package_wrap(u8 *message, int msize, struct hid_device *hid)
else
data->mts = data->ats;
data->gts = data->ats;
data->ax = -imuData.ax0;
data->ay = imuData.ay0;
data->az = -imuData.az0;
data->gx = -imuData.gx0;
data->gy = imuData.gy0;
data->gz = -imuData.gz0;
data->mx = -imuData.mx0;
data->my = imuData.my0;
data->mz = -imuData.mz0;
if (DEBUG_ORIENTATION == 1) {
data->ax = -imuData.ax0;
data->ay = imuData.ay0;
data->az = -imuData.az0;
data->gx = -imuData.gx0;
data->gy = imuData.gy0;
data->gz = -imuData.gz0;
data->mx = -imuData.my0;
data->my = -imuData.mx0;
data->mz = -imuData.mz0;
} else {
data->ax = -imuData.ay0;
data->ay = -imuData.ax0;
data->az = -imuData.az0;
data->gx = -imuData.gy0;
data->gy = -imuData.gx0;
data->gz = -imuData.gz0;
data->mx = -imuData.my0;
data->my = -imuData.mx0;
data->mz = -imuData.mz0;
}
trace_qvr_recv_sensor("gyro", data->gts, data->gx, data->gy, data->gz);
trace_qvr_recv_sensor("accel", data->ats, data->ax, data->ay, data->az);