TBTracker-ESP32/FSK4_Mod.ino at main · RoelKroes/TBTracker-ESP32 · 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
// MFSK Modulation

#include <RadioLib.h>


uint32_t fsk4_base = 0, fsk4_baseHz = 0;
uint32_t fsk4_shift = 0, fsk4_shiftHz = 0;
uint32_t fsk4_bitDuration = 0;
uint32_t fsk4_tones[4];
uint32_t fsk4_tonesHz[4];


#if RADIOLIB_VERSION >= 117571584 // Radiolib version must be >= 7.2.0
int16_t fsk4_setup(PhysicalLayer* phy, float base, uint32_t shift, uint16_t rate){
 // save configuration
  fsk4_baseHz = base;
  fsk4_shiftHz = shift;


  // calculate duration of 1 bit
  fsk4_bitDuration = (uint32_t)1000000/rate;

  // calculate module carrier frequency resolution
  uint32_t step = round(phy->freqStep);

  // check minimum shift value
  if(shift < step / 2) {
    return 0;
  }

  // round shift to multiples of frequency step size
  if(shift % step < (step / 2)) {
    fsk4_shift = shift / step;
  } else {
    fsk4_shift = (shift / step) + 1;
  }

  // Write resultant tones into arrays for quick lookup when modulating.
  fsk4_tones[0] = 0;
  fsk4_tones[1] = fsk4_shift;
  fsk4_tones[2] = fsk4_shift*2;
  fsk4_tones[3] = fsk4_shift*3;


  // calculate 24-bit frequency
  fsk4_base = (base * 1000000.0) / phy->freqStep;

  toSerialConsole("[FSK4] base: "); toSerialConsole(fsk4_base);toSerialConsole("\n");

  // configure for direct mode
  return(phy->startDirect());
}
#else // Backward compatible code for Radiolib < 7.2.0
int16_t fsk4_setup(PhysicalLayer* phy, float base, uint32_t shift, uint16_t rate){
 // save configuration
  fsk4_baseHz = base;
  fsk4_shiftHz = shift;


  // calculate duration of 1 bit
  fsk4_bitDuration = (uint32_t)1000000/rate;

  // calculate module carrier frequency resolution
  uint32_t step = round(phy->getFreqStep());

  // check minimum shift value
  if(shift < step / 2) {
    return 0;
  }

  // round shift to multiples of frequency step size
  if(shift % step < (step / 2)) {
    fsk4_shift = shift / step;
  } else {
    fsk4_shift = (shift / step) + 1;
  }

  // Write resultant tones into arrays for quick lookup when modulating.
  fsk4_tones[0] = 0;
  fsk4_tones[1] = fsk4_shift;
  fsk4_tones[2] = fsk4_shift*2;
  fsk4_tones[3] = fsk4_shift*3;


  // calculate 24-bit frequency
  fsk4_base = (base * 1000000.0) / phy->getFreqStep();

  toSerialConsole("[FSK4] base: ");toSerialConsole(fsk4_base);toSerialConsole("\n");

  // configure for direct mode
  return(phy->startDirect());
}
#endif

int16_t fsk4_transmitDirect(PhysicalLayer* phy, uint32_t freq) {
  return(phy->transmitDirect(freq));
}

void fsk4_tone(PhysicalLayer* phy, uint8_t i)
{
  uint32_t start = micros();
  fsk4_transmitDirect(phy, fsk4_base + fsk4_tones[i]);
  //delayMicroseconds(fsk4_bitDuration);
  while(micros() - start < fsk4_bitDuration)
  {
    yield();
  }
}


void fsk4_idle(PhysicalLayer* phy){
    fsk4_tone(phy, 0);
}

void fsk4_standby(PhysicalLayer* phy){
    phy->standby();
}

void fsk4_preamble(PhysicalLayer* phy, uint8_t len){
    int k;
    for (k=0; k<len; k++){
        fsk4_writebyte(phy, 0x1B);
    }
}

size_t fsk4_writebyte(PhysicalLayer* phy, uint8_t b){
    int k;
    // Send symbols MSB first.
    for (k=0;k<4;k++)
    {
        // Extract 4FSK symbol (2 bits)
        uint8_t symbol = (b & 0xC0) >> 6;
        // Modulate
        fsk4_tone(phy, symbol);
        // Shift to next symbol.
        b = b << 2;
    }

  return(1);
}

void fsk4_write(PhysicalLayer* phy, uint8_t* buff, size_t len){
  size_t n = 0;
  for(size_t i = 0; i < len; i++) {
    n += fsk4_writebyte(phy, buff[i]);
  }
  fsk4_standby(phy);
  return;
}