d/wmr: fix Reverb G2 controller orientation

We can't take the IMU values of the Reverb G2 controller as is.
We need to compensate for how the IMU chip is oriented on the
hardware.

Similar to the WMR HMD the WMR controllers' firmware configuration
provides us with the transformations necessary to adjust the
controller orientations. So apply them to fix the orientation
issues.

Signed-off-by: Linus Lüssing <linus.luessing@c0d3.blue>
This commit is contained in:
Linus Lüssing 2023-06-08 14:35:40 +02:00 committed by Jakob Bornecrantz
parent c95be33072
commit 57fb867514
2 changed files with 8 additions and 0 deletions

View file

@ -581,6 +581,8 @@ wmr_controller_base_init(struct wmr_controller_base *wcb,
return false;
}
wmr_config_precompute_transforms(&wcb->config.sensors, NULL);
/* Enable the status reports, IMU and control status reports */
const unsigned char wmr_controller_status_enable_cmd[64] = {0x06, 0x03, 0x01, 0x00, 0x02};
wmr_controller_send_bytes(wcb, wmr_controller_status_enable_cmd, sizeof(wmr_controller_status_enable_cmd));

View file

@ -8,6 +8,8 @@
* @author Jan Schmidt <jan@centricular.com>
* @ingroup drv_wmr
*/
#include "math/m_api.h"
#include "util/u_device.h"
#include "util/u_trace_marker.h"
#include "util/u_var.h"
@ -210,6 +212,8 @@ wmr_controller_hp_packet_parse(struct wmr_controller_hp *ctrl, const unsigned ch
acc[1] = read24(&p); // y
acc[2] = read24(&p); // z
vec3_from_wmr_controller_accel(acc, &last_input->imu.acc);
math_quat_rotate_vec3(&wcb->config.sensors.transforms.P_oxr_acc.orientation, &last_input->imu.acc,
&last_input->imu.acc);
U_LOG_IFL_T(wcb->log_level, "Accel [m/s^2] : %f",
sqrtf(last_input->imu.acc.x * last_input->imu.acc.x +
@ -224,6 +228,8 @@ wmr_controller_hp_packet_parse(struct wmr_controller_hp *ctrl, const unsigned ch
gyro[1] = read24(&p);
gyro[2] = read24(&p);
vec3_from_wmr_controller_gyro(gyro, &last_input->imu.gyro);
math_quat_rotate_vec3(&wcb->config.sensors.transforms.P_oxr_gyr.orientation, &last_input->imu.gyro,
&last_input->imu.gyro);
uint32_t prev_ticks = last_input->imu.timestamp_ticks & UINT32_C(0xFFFFFFFF);