DIY USB adapter for Fanatec ClubSport Shifter SQ v1.5 (Arduino Due)

fanatec_shifter.ino

#include <cstdint>

#include "Joystick.h"

const int16_t gate_r = 3340;
const int16_t gate_12 = 2690;
const int16_t gate_34 = 2000;
const int16_t gate_56 = 1230;
const int16_t gate_7 = 550;

const int16_t gate_r1357 = 4000;
const int16_t gate_246 = 0;

const int16_t gate_width = 200;
const int16_t gate_height_in = 500;
const int16_t gate_height_out = 1000;

const int16_t threshold_mode_lo = 500;
const int16_t threshold_mode_hi = 3500;

const int16_t threshold_seq_lo = 500;
const int16_t threshold_seq_hi = 2000;

const int16_t delay_seq = 50;

template <typename T>
class Schmitt {
 public:
  Schmitt(bool state, T lo, T hi) : state_(state), lo_(lo), hi_(hi) {}
  bool Observe(T input) {
    if (state_) {
      if (input <= lo_) {
        state_ = false;
      }
    } else {
      if (input >= hi_) {
        state_ = true;
      }
    }
    return state_;
  }
  bool state() const { return state_; }
  void set_state(bool state) { state_ = state; }
 private:
  bool state_;
  T lo_;
  T hi_;
};

class Debouncer {
 public:
  Debouncer(bool state, int16_t suppress_num) :
      state_(state),
      suppress_num_(suppress_num) {}

  bool Observe(bool input) {
    if (counter_ >= 0) {
      counter_--;
      return state_;
    }
    if (state_ != input) {
      counter_ = suppress_num_;
      state_ = input;
    }
    return state_;
  }

  bool state() const { return state_; }
  void set_state(bool state) { state_ = state; }
 private:
  bool state_;
  int16_t suppress_num_;
  int16_t counter_;
};

template <typename T>
class SchmittDebouncer {
 public:
  SchmittDebouncer(bool state, T lo, T hi, int16_t suppress_num) :
      schmitt_(state, lo, hi),
      debouncer_(state, suppress_num) {}
  bool Observe(T input) {
    return debouncer_.Observe(schmitt_.Observe(input));
  }
  bool state() const { return debouncer_.state(); }
  void set_state(bool state) {
    schmitt_.set_state(state);
    debouncer_.set_state(state);
  }
 private:
  Schmitt<T> schmitt_;
  Debouncer debouncer_;
};

class HDecoder {
 public:
  struct Gear {
    int16_t x_min;
    int16_t x_max;
    int16_t y_center;
    int16_t height_in;
    int16_t height_out;
  };
  HDecoder(int8_t n, const Gear* gears) : n_(n), gears_(gears), current_gear_(-1) {}
  
  void Reset() {
    current_gear_ = -1;
  }
  
  int8_t Observe(int16_t x, int16_t y) {
    if (current_gear_ == -1) {
      // neutral; check if we have moved into any gear
      for (int8_t i = 0; i < n_; i++) {
        const Gear& gear = gears_[i];
        if (gear.x_min <= x && x <= gear.x_max &&
            gear.y_center - gear.height_in <= y && y <= gear.y_center + gear.height_in) {
          current_gear_ = i;
          break;
        }
      }
    } else {
      // in gear; check if we have moved back to neutral
      const Gear& gear = gears_[current_gear_];
      if (!(gear.y_center - gear.height_out <= y && y <= gear.y_center + gear.height_out)) {
        current_gear_ = -1;
      }
    }
    return current_gear_;
  }
  
  int current_gear() const { return current_gear_; }
  
 private:
  int8_t n_;
  const Gear* gears_;  // not owned
  int8_t current_gear_;
};

constexpr int8_t kGearNum = 8;
HDecoder::Gear g_gears[kGearNum] = {
  {
    .x_min = gate_12 - gate_width,
    .x_max = gate_12 + gate_width,
    .y_center = gate_r1357,
    .height_in = gate_height_in,
    .height_out = gate_height_out,
  },
  {
    .x_min = gate_12 - gate_width,
    .x_max = gate_12 + gate_width,
    .y_center = gate_246,
    .height_in = gate_height_in,
    .height_out = gate_height_out,
  },
  {
    .x_min = gate_34 - gate_width,
    .x_max = gate_34 + gate_width,
    .y_center = gate_r1357,
    .height_in = gate_height_in,
    .height_out = gate_height_out,
  },
  {
    .x_min = gate_34 - gate_width,
    .x_max = gate_34 + gate_width,
    .y_center = gate_246,
    .height_in = gate_height_in,
    .height_out = gate_height_out,
  },
  {
    .x_min = gate_56 - gate_width,
    .x_max = gate_56 + gate_width,
    .y_center = gate_r1357,
    .height_in = gate_height_in,
    .height_out = gate_height_out,
  },
  {
    .x_min = gate_56 - gate_width,
    .x_max = gate_56 + gate_width,
    .y_center = gate_246,
    .height_in = gate_height_in,
    .height_out = gate_height_out,
  },
  {
    .x_min = gate_7 - gate_width,
    .x_max = gate_7 + gate_width,
    .y_center = gate_r1357,
    .height_in = gate_height_in,
    .height_out = gate_height_out,
  },
  {
    .x_min = gate_r - gate_width,
    .x_max = gate_r + gate_width,
    .y_center = gate_r1357,
    .height_in = gate_height_in,
    .height_out = gate_height_out,
  },
};
constexpr int8_t kButtonNum = kGearNum + 3;

Schmitt<int16_t> g_mode_schmitt(false, threshold_mode_lo, threshold_mode_hi);
HDecoder g_h_decoder(kGearNum, g_gears);
SchmittDebouncer<int16_t> g_seq_x_debouncer(true, threshold_seq_lo, threshold_seq_hi, delay_seq);
SchmittDebouncer<int16_t> g_seq_y_debouncer(true, threshold_seq_lo, threshold_seq_hi, delay_seq);

bool g_current_mode = false;  // false => H, true => SEQ

Joystick_ g_joystick(
  JOYSTICK_DEFAULT_REPORT_ID,
  JOYSTICK_TYPE_GAMEPAD,
  kButtonNum, 0,         // Button Count, Hat Switch Count
  false, false, false,   // X, Y, Z
  false, false, false,   // Rx, Ry, Rz
  false, false,          // rudder, throttle
  false, false, false    // accelerator, brake, steering
  );

void setup() {
  Serial.begin(115200);
  g_joystick.begin();
  analogReadResolution(12);
}

void loop() {
  const int16_t x_raw = analogRead(A0);
  const int16_t y_raw = analogRead(A1);  
  const int16_t mode_raw = analogRead(A2);

  const bool mode = g_mode_schmitt.Observe(mode_raw);  
  g_joystick.setButton(0, mode);
  if (mode != g_current_mode) {
    // simply reset all state
    g_current_mode = mode;
    g_h_decoder.Reset();
    g_seq_x_debouncer.set_state(true);
    g_seq_y_debouncer.set_state(true);
    for (int i = 1; i < kButtonNum; i++) {
      g_joystick.setButton(i, 0);
    }
    return;
  }
  
  if (mode) {
    // SEQ
    // NOTE(summivox): switch signals are active-low
    const bool seq_x = !g_seq_x_debouncer.Observe(x_raw);
    const bool seq_y = !g_seq_y_debouncer.Observe(y_raw);
    g_joystick.setButton(kGearNum + 1, seq_x);
    g_joystick.setButton(kGearNum + 2, seq_y);
    Serial.println(seq_x ? kGearNum + 1 : seq_y ? kGearNum + 2 : kGearNum + 3);
  } else {
    // H
    const int8_t gear = g_h_decoder.Observe(x_raw, y_raw) + 1;
    for (int i = 1; i <= kGearNum; i++) {
      g_joystick.setButton(i, gear == i);
    }
    Serial.println(gear);
  }
}