SnipsControllers/src/activity.cpp at main · thePunderWoman/SnipsControllers · GitHub

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
#include "activity.h"

#include <cstdio>
#include <cstdlib>

namespace {

struct SourceName {
  uint8_t bit;
  const char *name;
};

constexpr SourceName kSourceNames[] = {
    {ActivitySources::kButton, "button"},
    {ActivitySources::kStick, "stick"},
    {ActivitySources::kTrigger, "trigger"},
    {ActivitySources::kMotion, "motion"},
};

}  // namespace

void describeActivitySources(uint8_t mask, char *out, size_t outCapacity) {
  if (outCapacity == 0) return;
  out[0] = '\0';
  if (mask == 0) {
    std::snprintf(out, outCapacity, "none");
    return;
  }
  size_t used = 0;
  for (const SourceName &source : kSourceNames) {
    if ((mask & source.bit) == 0) continue;
    const int written = std::snprintf(out + used, outCapacity - used, "%s%s",
                                      used == 0 ? "" : ", ", source.name);
    if (written < 0 || used + written >= outCapacity) return;  // truncated
    used += written;
  }
}

bool isTriggerCalibrated(const CalibrationData &data) {
  const CalibrationData defaults;
  return data.triggerMin != defaults.triggerMin ||
         data.triggerMax != defaults.triggerMax;
}

bool isStickCalibrated(const CalibrationData &data) {
  const CalibrationData defaults;
  return data.stickXMin != defaults.stickXMin ||
         data.stickXMax != defaults.stickXMax ||
         data.stickXCenter != defaults.stickXCenter ||
         data.stickYMin != defaults.stickYMin ||
         data.stickYMax != defaults.stickYMax ||
         data.stickYCenter != defaults.stickYCenter;
}

void ActivityDetector::setCalibration(const CalibrationData &data) {
  stickCalibrated_ = isStickCalibrated(data);
  triggerCalibrated_ = isTriggerCalibrated(data);
  hasStickReference_ = false;
}

bool ActivityDetector::stickActive(const ActivityInputs &inputs) {
  if (stickCalibrated_) {
    return std::abs(inputs.stickXPercent) >= kHeldPercent ||
           std::abs(inputs.stickYPercent) >= kHeldPercent;
  }
  // The first reading just sets where the stick rests.
  if (!hasStickReference_ ||
      std::abs(inputs.rawStickX - stickReferenceX_) >=
          kUncalibratedMoveCounts ||
      std::abs(inputs.rawStickY - stickReferenceY_) >=
          kUncalibratedMoveCounts) {
    const bool moved = hasStickReference_;
    hasStickReference_ = true;
    stickReferenceX_ = inputs.rawStickX;
    stickReferenceY_ = inputs.rawStickY;
    return moved;
  }
  return false;
}

uint8_t ActivityDetector::update(const ActivityInputs &inputs) {
  uint8_t active = 0;
  if (inputs.anyButtonHeld) active |= ActivitySources::kButton;
  if (stickActive(inputs)) active |= ActivitySources::kStick;
  if (triggerCalibrated_ && inputs.triggerPercent >= kHeldPercent) {
    active |= ActivitySources::kTrigger;
  }
  if (inputs.motion) active |= ActivitySources::kMotion;

  newlyActive_ = active & ~previousActive_;
  previousActive_ = active;
  return active;
}