Add tilt support to DFT processor

Author: quo

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);