Reflecta/ReflectaHeartbeat.cpp at master · sysrun/Reflecta · 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
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
120
121
122
123
124
125
126
127
128
129
130
131
132
133
134
135
136
137
138
139
140
141
142
143
144
145
146
147
148
149
150
151
152
153
154
155
156
157
158
/*
ReflectaHeartbeat.cpp - Library for sending timed heartbeats of data over Reflecta to a host
*/

#include "Reflecta.h"

using namespace reflecta;

namespace reflectaHeartbeat
{
  const byte heartbeatStackMax = 128;
  int heartbeatStackTop = -1;
  int8_t heartbeatStack[heartbeatStackMax + 1];

  void push(int8_t b)
  {
    if (heartbeatStackTop == heartbeatStackMax)
    {
      reflectaFrames::sendEvent(Error, StackOverflow);
    }
    else
    {
      heartbeatStack[++heartbeatStackTop] = b;
    }
  }

  void push16(int16_t w)
  {
    if (heartbeatStackTop > heartbeatStackMax - 2)
    {
      reflectaFrames::sendEvent(Error, StackOverflow);
    }
    else
    {
      heartbeatStack[++heartbeatStackTop] = (w >> 8);
      heartbeatStack[++heartbeatStackTop] = (w & 0xFF);
    }
  }

  void pushf(float f)
  {
    if (heartbeatStackTop > heartbeatStackMax - 4)
    {
      reflectaFrames::sendEvent(Error, StackOverflow);
    }
    else
    {
      byte* pb = (byte*)&f;
      heartbeatStack[++heartbeatStackTop] = pb[3];
      heartbeatStack[++heartbeatStackTop] = pb[2];
      heartbeatStack[++heartbeatStackTop] = pb[1];
      heartbeatStack[++heartbeatStackTop] = pb[0];
    }
  }

  int8_t pop()
  {
    if (heartbeatStackTop == -1)
    {
      reflectaFrames::sendEvent(Error, StackUnderflow);
      return -1;
    }
    else
    {
      return heartbeatStack[heartbeatStackTop--];
    }
  }


  const int MaxFunctions = 16;

  // Function table of all functions to call in heartbeat
  bool (*heartbeatFunctions[MaxFunctions])();

  // Has the function finished for this heartbeat
  bool functionComplete[MaxFunctions];

  int functionsTop = 0;
  void bind(bool (*function)()) {
    heartbeatFunctions[functionsTop++] = function;
  };

  uint32_t microsBetweenFrames = 100000;

  void setFrameRate() {
    int16_t framesPerSecond = reflectaFunctions::pop16();
    microsBetweenFrames = 1000000 / framesPerSecond;
  };

  // Number of times loop is called and we're still waiting for one of our bound data
  // collection functions to finish
  uint16_t collectingLoops = 0;

  // Number of times loop is called and we've finished collecting data but the timeout
  // for next heartbeat has not yet expired
  uint16_t idleLoops = 0;

  void sendHeartbeat() {

      push16(idleLoops);
      push16(collectingLoops);
      push(Heartbeat);

      byte heartbeatSize = heartbeatStackTop + 1;
      byte frame[heartbeatSize];

      for (int i = 0; i < heartbeatSize; i++)
      {
        frame[i] = pop();
      }

      reflectaFrames::sendFrame(frame, heartbeatSize);
  };

  bool finished = false;
  uint32_t nextHeartbeat = 0;
  void loop() {

    if (!finished) {
      collectingLoops++;
      finished = true;
      for (int i = 0; i < functionsTop; i++) {
        if (!functionComplete[i]) {
          functionComplete[i] = heartbeatFunctions[i]();
          if (!functionComplete[i]) finished = false;
        }
      }
    }

    if (finished) {

      unsigned long currentTime = micros();
      if (currentTime >= nextHeartbeat) {

        sendHeartbeat();

        // Set the micros delay for when the next heartbeat should be sent
        nextHeartbeat = currentTime + microsBetweenFrames;

        // Zero out the heartbeat state
        collectingLoops = 0;
        idleLoops = 0;

        for (int i = 0; i < MaxFunctions; i++) {
          functionComplete[i] = false;
          finished = false;
        }

      } else {
        idleLoops++;
      };
    }
  };

  void setup() {
    reflectaFunctions::bind("hart1", setFrameRate);
  }
};