Add tilt support to DFT processor

src/daemon/config.cpp

@@ -117,6 +117,15 @@ static int parse_conf(void *user, const char *c_section, const char *c_name, con
  if (section == "DFT" && name == "FreqMinMag")
  config->dft_freq_min_mag = std::stoi(value);
 
+ if (section == "DFT" && name == "TiltMinMag")
+ config->dft_tilt_min_mag = std::stoi(value);
+
+ if (section == "DFT" && name == "TiltDistance")
+ config->dft_tilt_distance = std::stof(value);
+
+ if (section == "DFT" && name == "TipDistance")
+ config->dft_tip_distance = std::stof(value);
+
  return 1;
 }
 

src/daemon/config.hpp

@@ -39,6 +39,9 @@ class Config {
  f32 dft_position_exp = -0.7;
  u16 dft_button_min_mag = 1000;
  u16 dft_freq_min_mag = 10000;
+ u16 dft_tilt_min_mag = 10000;
+ f32 dft_tilt_distance = 6;
+ f32 dft_tip_distance = 2;
 
  i16 vendor;
  i16 product;

src/daemon/dft.cpp

@@ -130,7 +130,7 @@ static f64 iptsd_dft_interpolate_frequency(const Context &ctx, const ipts::DftWi
 static void iptsd_dft_handle_position(Context &ctx, const ipts::DftWindow &dft,
  ipts::StylusData &stylus)
 {
- if (dft.rows <= 0) {
+ if (dft.rows <= 1) {
  iptsd_dft_lift(stylus);
  return;
  }
@@ -161,6 +161,47 @@ static void iptsd_dft_handle_position(Context &ctx, const ipts::DftWindow &dft,
  if (ctx.config.invert_y)
  y = 1 - y;
 
+ if (dft.x[1].magnitude > ctx.config.dft_tilt_min_mag &&
+ dft.y[1].magnitude > ctx.config.dft_tilt_min_mag) {
+
+ // calculate tilt angle from relative position of secondary transmitter
+
+ auto [pxt, xt] = iptsd_dft_interpolate_position(ctx, dft.x[1]);
+ auto [pyt, yt] = iptsd_dft_interpolate_position(ctx, dft.y[1]);
+
+ if (pxt && pyt) {
+
+ xt /= dft.dim.width - 1;
+ yt /= dft.dim.height - 1;
+
+ if (ctx.config.invert_x)
+ xt = 1 - xt;
+
+ if (ctx.config.invert_y)
+ yt = 1 - yt;
+
+ xt -= x;
+ yt -= y;
+
+ if (ctx.config.dft_tip_distance) {
+ // correct tip position using tilt data
+ auto r = ctx.config.dft_tip_distance / ctx.config.dft_tilt_distance;
+ x -= xt * r;
+ y -= yt * r;
+ }
+
+ xt *= ctx.config.width / (ctx.config.dft_tilt_distance * 10);
+ yt *= ctx.config.height / (ctx.config.dft_tilt_distance * 10);
+
+ auto azm = std::max(0., std::fmod(std::atan2(-yt, xt) / M_PI + 2, 2)) * 18000;
+ auto alt = (.5 - std::acos(std::min(1., std::hypot(xt, yt))) / M_PI) * 18000;
+ stylus.azimuth = gsl::narrow<u16>(std::round(azm));
+ stylus.altitude = gsl::narrow<u16>(std::round(alt));
+
+ }
+
+ }
+
  x = std::round(std::clamp(x, 0.0, 1.0) * IPTS_MAX_X);
  y = std::round(std::clamp(y, 0.0, 1.0) * IPTS_MAX_Y);