dRonin  adbada4
dRonin firmware
 All Data Structures Namespaces Files Functions Variables Typedefs Enumerations Enumerator Macros Groups Pages
pios_openlrs.c
Go to the documentation of this file.
1 
16 /*
17  * This program is free software; you can redistribute it and/or modify
18  * it under the terms of the GNU General Public License as published by
19  * the Free Software Foundation; either version 3 of the License, or
20  * (at your option) any later version.
21  *
22  * This program is distributed in the hope that it will be useful, but
23  * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY
24  * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License
25  * for more details.
26  *
27  * You should have received a copy of the GNU General Public License along
28  * with this program; if not, see <http://www.gnu.org/licenses/>
29  */
30 
31 #include "pios.h"
32 
33 #ifdef PIOS_INCLUDE_OPENLRS
34 
35 #include <pios_hal.h> /* for rcvr_group_map in tx path */
36 #include <pios_inlinedelay.h>
37 #include <pios_thread.h>
38 #include <pios_spi_priv.h>
39 #include <pios_openlrs_priv.h>
40 #include <pios_openlrs_rcvr_priv.h>
41 #include <taskmonitor.h>
42 #include <taskinfo.h>
43 
44 #include <misc_math.h>
45 
46 #include <pios_com_priv.h>
47 
48 #include "openlrs.h"
49 #include "flightstatus.h"
50 #include "flightbatterystate.h"
51 #include "manualcontrolsettings.h"
52 #include "openlrsstatus.h"
53 
54 #include "pios_rfm22b_regs.h"
55 
56 #define STACK_SIZE_BYTES 900
57 #define TASK_PRIORITY PIOS_THREAD_PRIO_HIGH
58 
59 #define RF22B_PWRSTATE_POWERDOWN 0x00
60 #define RF22B_PWRSTATE_READY (RFM22_opfc1_xton | RFM22_opfc1_pllon)
61 #define RF22B_PWRSTATE_RX (RFM22_opfc1_rxon | RFM22_opfc1_xton)
62 #define RF22B_PWRSTATE_TX (RFM22_opfc1_txon | RFM22_opfc1_xton)
63 
64 static OpenLRSStatusData openlrs_status;
65 
66 static pios_openlrs_t pios_openlrs_alloc();
67 static bool pios_openlrs_validate(pios_openlrs_t openlrs_dev);
68 static void pios_openlrs_rx_task(void *parameters);
69 static void pios_openlrs_tx_task(void *parameters);
70 static void pios_openlrs_do_hop(pios_openlrs_t openlrs_dev);
71 
72 static void rfm22_init(pios_openlrs_t openlrs_dev, uint8_t isbind);
73 static void rfm22_disable(pios_openlrs_t openlrs_dev);
74 static void rfm22_set_frequency(pios_openlrs_t openlrs_dev, uint32_t f);
75 static uint8_t rfm22_get_rssi(pios_openlrs_t openlrs_dev);
76 static void rfm22_tx_packet(pios_openlrs_t openlrs_dev,
77  void *pkt, uint8_t size);
78 static bool rfm22_rx_packet(pios_openlrs_t openlrs_dev,
79  void *whence, uint32_t size);
80 static int rfm22_rx_packet_variable(pios_openlrs_t openlrs_dev,
81  void *whence, uint32_t max_size);
82 static void rfm22_set_channel(pios_openlrs_t openlrs_dev, uint8_t ch);
83 static void rfm22_rx_reset(pios_openlrs_t openlrs_dev, bool pause_long);
84 static void rfm22_check_hang(pios_openlrs_t openlrs_dev);
85 
86 // SPI read/write functions
87 static void rfm22_assert_cs(pios_openlrs_t openlrs_dev);
88 static void rfm22_deassert_cs(pios_openlrs_t openlrs_dev);
89 static void rfm22_claim_bus(pios_openlrs_t openlrs_dev);
90 static void rfm22_release_bus(pios_openlrs_t openlrs_dev);
91 static void rfm22_write_claim(pios_openlrs_t openlrs_dev,
92  uint8_t addr, uint8_t data);
93 static void rfm22_write(pios_openlrs_t openlrs_dev, uint8_t addr,
94  uint8_t data);
95 static uint8_t rfm22_read_claim(pios_openlrs_t openlrs_dev,
96  uint8_t addr);
97 static uint8_t rfm22_read(pios_openlrs_t openlrs_dev,
98  uint8_t addr);
99 static void rfm22_get_it_status(pios_openlrs_t openlrs_dev);
100 static void rfm22_beacon_send(pios_openlrs_t openlrs_dev, bool static_tone);
101 
102 // Private constants
103 const struct rfm22_modem_regs {
104  uint32_t bps;
105  uint8_t r_1c, r_1d, r_1e, r_20, r_21, r_22, r_23, r_24, r_25, r_2a, r_6e, r_6f, r_70, r_71, r_72;
106 } modem_params[] = {
107  { 4800, 0x1a, 0x40, 0x0a, 0xa1, 0x20, 0x4e, 0xa5, 0x00, 0x1b, 0x1e, 0x27, 0x52, 0x2c, 0x23, 0x30 }, // 50000 0x00
108  { 9600, 0x05, 0x40, 0x0a, 0xa1, 0x20, 0x4e, 0xa5, 0x00, 0x20, 0x24, 0x4e, 0xa5, 0x2c, 0x23, 0x30 }, // 25000 0x00
109  { 19200, 0x06, 0x40, 0x0a, 0xd0, 0x00, 0x9d, 0x49, 0x00, 0x7b, 0x28, 0x9d, 0x49, 0x2c, 0x23, 0x30 }, // 25000 0x01
110  { 57600, 0x05, 0x40, 0x0a, 0x45, 0x01, 0xd7, 0xdc, 0x03, 0xb8, 0x1e, 0x0e, 0xbf, 0x00, 0x23, 0x2e },
111  { 125000, 0x8a, 0x40, 0x0a, 0x60, 0x01, 0x55, 0x55, 0x02, 0xad, 0x1e, 0x20, 0x00, 0x00, 0x23, 0xc8 },
112 };
113 
114 /* invariants
115  * 1D = 0x40 AFC enabled, gearshift = 000000
116  * 20 = 0x0a clock recovery oversampling rate = 0x0a, min value 0x08
117  * 71 = 0x23 modulation mode -- FIFO, GFSK
118  */
119 
120 DONT_BUILD_IF(OPENLRS_MODEM_PARAMS_4800 != 0, InvModemParams);
121 DONT_BUILD_IF(OPENLRS_MODEM_PARAMS_9600 != 1, InvModemParams2);
122 DONT_BUILD_IF(OPENLRS_MODEM_PARAMS_19200 != 2, InvModemParams3);
123 DONT_BUILD_IF(OPENLRS_MODEM_PARAMS_57600 != 3, InvModemParams4);
124 DONT_BUILD_IF(OPENLRS_MODEM_PARAMS_125000 != 4, InvModemParams5);
125 DONT_BUILD_IF(OPENLRS_MODEM_PARAMS_MAXOPTVAL != 4, InvModemParams6);
126 
127 #define OPENLRS_BIND_PARAMS (OPENLRS_MODEM_PARAMS_9600)
128 #define BINDING_POWER (OPENLRS_RF_POWER_50 - 1)
129  // not lowest since may fail to tx with amp
130 
131 static uint8_t count_set_bits(uint16_t x)
132 {
133  return __builtin_popcount(x);
134 }
135 
136 static bool wait_interrupt_until(pios_openlrs_t openlrs_dev, uint32_t expiry,
137  uint8_t *rssi)
138 {
139  bool taken = false;
140 
141  uint8_t our_rssi;
142 
143  do {
144  if (rssi) {
145  our_rssi = rfm22_get_rssi(openlrs_dev);
146  }
147 
148  taken = PIOS_Semaphore_Take(openlrs_dev->sema_isr, 1);
149  } while ((!taken) && (!PIOS_DELAY_GetuSExpired(expiry)));
150 
151  if (taken && rssi) {
152  *rssi = our_rssi;
153  }
154 
155  return taken;
156 }
157 
158 static bool wait_interrupt(pios_openlrs_t openlrs_dev, uint32_t delay)
159 {
160  uint32_t expiry = delay + PIOS_DELAY_GetuS();
161  return wait_interrupt_until(openlrs_dev, expiry, NULL);
162 }
163 
164 static void olrs_bind_rx(uintptr_t olrs_id, pios_com_callback rx_in_cb,
165  uintptr_t context)
166 {
167  pios_openlrs_t openlrs_dev = (pios_openlrs_t) olrs_id;
168 
169  PIOS_Assert(pios_openlrs_validate(openlrs_dev));
170 
171  openlrs_dev->rx_in_cb = rx_in_cb;
172  openlrs_dev->rx_in_context = context;
173 }
174 
175 static void olrs_bind_tx(uintptr_t olrs_id, pios_com_callback tx_out_cb,
176  uintptr_t context)
177 {
178  pios_openlrs_t openlrs_dev = (pios_openlrs_t) olrs_id;
179 
180  PIOS_Assert(pios_openlrs_validate(openlrs_dev));
181 
182  openlrs_dev->tx_out_cb = tx_out_cb;
183  openlrs_dev->tx_out_context = context;
184 }
185 
186 static bool olrs_available(uintptr_t olrs_id)
187 {
188  pios_openlrs_t openlrs_dev = (pios_openlrs_t) olrs_id;
189 
190  PIOS_Assert(pios_openlrs_validate(openlrs_dev));
191 
192  if (openlrs_dev->linkLossTimeMs) {
193  return false;
194  }
195 
196  if (!openlrs_dev->link_acquired) {
197  return false;
198  }
199 
200  return true;
201 }
202 
203 const struct pios_com_driver pios_openlrs_com_driver = {
204  .bind_tx_cb = olrs_bind_tx,
205  .bind_rx_cb = olrs_bind_rx,
206  .available = olrs_available,
207 };
208 
209 const uint32_t packet_rssi_time_us = 2700;
210 
211 static uint32_t min_freq(HwSharedRfBandOptions band)
212 {
213  switch (band) {
214  default:
215  case HWSHARED_RFBAND_433:
216  return MIN_RFM_FREQUENCY_433;
217  case HWSHARED_RFBAND_868:
218  return MIN_RFM_FREQUENCY_868;
219  case HWSHARED_RFBAND_915:
220  return MIN_RFM_FREQUENCY_915;
221  }
222 }
223 
224 static uint32_t max_freq(HwSharedRfBandOptions band)
225 {
226  switch (band) {
227  default:
228  case HWSHARED_RFBAND_433:
229  return MAX_RFM_FREQUENCY_433;
230  case HWSHARED_RFBAND_868:
231  return MAX_RFM_FREQUENCY_868;
232  case HWSHARED_RFBAND_915:
233  return MAX_RFM_FREQUENCY_915;
234  }
235 }
236 
237 static uint32_t def_carrier_freq(HwSharedRfBandOptions band)
238 {
239  switch (band) {
240  default:
241  case HWSHARED_RFBAND_433:
242  return DEFAULT_CARRIER_FREQUENCY_433;
243  case HWSHARED_RFBAND_868:
244  return DEFAULT_CARRIER_FREQUENCY_868;
245  case HWSHARED_RFBAND_915:
246  return DEFAULT_CARRIER_FREQUENCY_915;
247  }
248 }
249 
250 static uint32_t binding_freq(HwSharedRfBandOptions band)
251 {
252  switch (band) {
253  default:
254  case HWSHARED_RFBAND_433:
255  return BINDING_FREQUENCY_433;
256  case HWSHARED_RFBAND_868:
257  return BINDING_FREQUENCY_868;
258  case HWSHARED_RFBAND_915:
259  return BINDING_FREQUENCY_915;
260  }
261 }
262 
263 /*****************************************************************************
264  * OpenLRS data formatting utilities
265  *****************************************************************************/
266 
267 static uint8_t get_control_packet_size(struct bind_data *bd)
268 {
269  return openlrs_pktsizes[(bd->flags & 0x07)];
270 }
271 
272 // Sending a x byte packet on bps y takes about (emperical)
273 // usec = (x + 15) * 8200000 / baudrate
274 //
275 // There's 5 bytes of preamble, 4 bytes of header, 1 byte of length,
276 // 2 bytes of sync word, 2 bytes of CRC, yielding 14 bytes.
277 // 15 is used here, for a small margin.
278 //
279 // Similarly, 8 bits/byte, 1000000 microseconds/second + 2.5% for
280 // margin. The values chosen here have interoperability
281 // considerations, so leave them alone :D
282 #define BYTES_AT_BAUD_TO_USEC(bytes, bps, div) ((uint32_t)((bytes) + (div ? 20 : 15)) * 8 * 1025000L / (uint32_t)(bps))
283 
284 static uint32_t get_control_packet_time(struct bind_data *bd)
285 {
286  return (BYTES_AT_BAUD_TO_USEC(get_control_packet_size(bd),
287  modem_params[bd->modem_params].bps,
288  bd->flags & DIVERSITY_ENABLED) + 2000);
289 }
290 
291 static uint32_t get_nominal_packet_interval(struct bind_data *bd,
292  uint32_t *rx_slop)
293 {
294  uint32_t ret = get_control_packet_time(bd);
295 
296  uint32_t pre_slop = ret - 2000;
297 
298  if (bd->flags & TELEMETRY_MASK) {
299  ret += (BYTES_AT_BAUD_TO_USEC(TELEMETRY_PACKETSIZE, modem_params[bd->modem_params].bps, bd->flags&DIVERSITY_ENABLED) + 1000);
300  }
301 
302  // round up to ms
303  ret = ((ret + 999) / 1000) * 1000;
304 
305  if (rx_slop) {
306  /* The "rx slop" is calculated as half the time between the
307  * actual tx frame length, and all the other times-- padding,
308  * rx telemetry length, etc.
309  *
310  * It is when the rx should hop, relative to the end of the
311  * last known packet plus the interval, to maximize margins.
312  */
313  *rx_slop = (ret - pre_slop) / 2;
314  }
315 
316  /* RRRRRRRTTTT--RRRRRRRTTTT--missed!......
317  * ^ we want to hop here
318  */
319 
320  return ret;
321 }
322 
323 static int pack_channels(uint8_t config, int16_t *ppm, uint8_t *buf)
324 {
325  config &= 7;
326 
327  uint8_t *p = buf;
328 
329  /* Pack 4 channels into 5 bytes */
330  for (int i = 0; i <= (config/2); i++) {
331  *(p++) = ppm[0];
332  *(p++) = ppm[1];
333  *(p++) = ppm[2];
334  *(p++) = ppm[3];
335  *(p++) =
336  ((ppm[0] & 0x300) >> 8) |
337  ((ppm[1] & 0x300) >> 6) |
338  ((ppm[2] & 0x300) >> 4) |
339  ((ppm[3] & 0x300) >> 2);
340 
341  ppm += 4;
342  }
343 
344  /* "Switch" channel */
345  if (config & 1) {
346  uint8_t packed_sw = 0;
347 
348  for (int i = 0; i < 4; i++) {
349  packed_sw <<= 2;
350 
351  if (ppm[0] < (166 + 12)) {
352  /* First 1/6 of travel sw pos 0 */
353  packed_sw |= 0;
354  } else if (ppm[0] < (550 + 12)) {
355  /* A little more than the next third is
356  * sw pos 1. Why? So a center value /
357  * 3 pos switch has clear value.
358  */
359  packed_sw |= 1;
360  } else if (ppm[0] < (833 + 12)) {
361  /* Up to 1/6 + 2/3 is sw pos 2 */
362  packed_sw |= 2;
363  } else {
364  /* Leaving the last sixth for sw pos 3 */
365  packed_sw |= 3;
366  }
367  }
368 
369  *(p++) = packed_sw;
370  }
371 
372  return p - buf;
373 }
374 
376 static void txscale_channels(int16_t *chan, int16_t nominal_min, int16_t nominal_max)
377 {
378  if (nominal_min == nominal_max) {
379  /* Prevent divide by zero */
380  nominal_max++;
381  }
382 
383  for (uint32_t i = 0; i < OPENLRS_PPM_NUM_CHANNELS; i++) {
384  int16_t x = chan[i];
385 
386  /*
387  * We have 1024 counts. The middle 1000 should correspond to
388  * 1000..1999, the "normal range". The extra 12 are scaled by
389  * a factor of 16 (from original openlrsng) to make the
390  * overall range from -19% to 119%
391  *
392  * In other words, the "normal resolution" is 0.1%; the
393  * extended resolution is in 1.6% chunks.
394  */
395 
396  float scaled = x;
397  scaled -= nominal_min;
398  scaled /= (nominal_max - nominal_min);
399 
400  const float scale_edge = (12*16) / 1000.0f; /* .192 */
401 
402  if (scaled < -scale_edge) {
403  x = 0;
404  } else if (scaled < 0) {
405  /* [-scale_edge, 0) mapped to 0..12 */
406  x = ((scaled + scale_edge) / scale_edge) * 12;
407  } else if (scaled < 1) {
408  /* [0 .. 1) mapped to 12..1011 */
409  x = scaled * 1000 + 12;
410  } else if (scaled < (1 + scale_edge)) {
411  /* [1, 1 + scale_edge) mapped to 1011..1023 */
412  x = ((scaled - 1) / scale_edge) * 12 + 1011;
413  } else {
414  x = 1023;
415  }
416 
417  chan[i] = x;
418  }
419 }
420 
421 DONT_BUILD_IF(OPENLRS_PPM_NUM_CHANNELS < 20, NotEnoughChannelSpaceForAllConfigs);
422 
423 static void unpack_channels(uint8_t config, int16_t *ppm, uint8_t *p)
424 {
425  /* Look at least 3 significant bit of config.
426  * LSB picks whether 4 switch channels are around.
427  * other 2 bits are a number between 1 and 4 for number of
428  * 4 channel groups.
429  *
430  * Therefore there can be up to 20 channels.
431  */
432 
433  config &= 7;
434  /* Ordinary channels are 4ch packed strangely in 5 bytes */
435  for (int i = 0; i<=(config/2); i++) {
436  ppm[0] = ((p[4] << 8) & 0x300) | p[0];
437  ppm[1] = ((p[4] << 6) & 0x300) | p[1];
438  ppm[2] = ((p[4] << 4) & 0x300) | p[2];
439  ppm[3] = ((p[4] << 2) & 0x300) | p[3];
440  p += 5;
441  ppm += 4;
442  }
443 
444  if (config & 1) { // 4ch packed in 1 byte;
445  ppm[0] = ((p[0] >> 6) & 3) * 333 + 12;
446  ppm[1] = ((p[0] >> 4) & 3) * 333 + 12;
447  ppm[2] = ((p[0] >> 2) & 3) * 333 + 12;
448  ppm[3] = ((p[0] >> 0) & 3) * 333 + 12;
449  }
450 }
451 
453 static void rxscale_channels(int16_t control[])
454 {
455  for (uint32_t i = 0; i < OPENLRS_PPM_NUM_CHANNELS; i++) {
456  int16_t x = control[i];
457  int16_t ret;
458 
459  if (x < 12) {
460  ret = 808 + x * 16;
461  } else if (x < 1012) {
462  ret = x + 988;
463  } else if (x < 1024) {
464  ret = 2000 + (x - 1011) * 16;
465  } else {
466  ret = 2192;
467  }
468 
469  control[i] = ret;
470  }
471 }
472 
473 static int pios_openlrs_form_telemetry(pios_openlrs_t openlrs_dev,
474  uint8_t *tx_buf, uint8_t rssi, bool only_serial,
475  int data_len_adjust)
476 {
477  int len = 0;
478 
479  tx_buf[0] &= 0xc0; // Preserve top two sequence bits
480 
481  int max_data_len = TELEMETRY_PACKETSIZE - 1;
482 
483  if (openlrs_dev->bind_data.ext_flags & EXT_FLAG_FASTDATA) {
484  /* Fast data extension: Adjust the amount we're
485  * willing to send from nominal packet lengths.
486  */
487  max_data_len += data_len_adjust;
488 
489  if (max_data_len < 1) {
490  /* Never shrink smaller than a 2b frame */
491  max_data_len = 1;
492  } else if (max_data_len >= MAX_PACKET_SIZE) {
493  max_data_len = MAX_PACKET_SIZE - 1;
494  }
495 
496  if (data_len_adjust < 0) {
497  only_serial = true;
498  }
499  }
500 
501  // Check for data on serial link
502  uint8_t bytes = 0;
503  // Append data from the com interface if applicable.
504  if (openlrs_dev->tx_out_cb) {
505  // Try to get some data to send
506  bool need_yield = false;
507  bytes = (openlrs_dev->tx_out_cb)(openlrs_dev->tx_out_context,
508  &tx_buf[1], max_data_len, NULL, &need_yield);
509  }
510 
511  if (bytes > 0) {
512  if (bytes < (TELEMETRY_PACKETSIZE - 1)) {
513  tx_buf[0] |= (0x37 + bytes);
514  } else {
515  tx_buf[0] |= 0x3f;
516  }
517 
518  if (openlrs_dev->bind_data.ext_flags & EXT_FLAG_FASTDATA) {
519  /* Fast data extension: send only the portion of the
520  * data that we used. Length field is capped at 8.
521  */
522  len = bytes + 1;
523  } else {
524  for (int i = bytes+1; i < TELEMETRY_PACKETSIZE; i++) {
525  tx_buf[i] = 0;
526  }
527 
528  len = TELEMETRY_PACKETSIZE;
529  }
530  } else if (only_serial) {
531  return 0;
532  } else {
533  // tx_buf[0] lowest 6 bits left at 0
534  tx_buf[1] = rssi;
535  if (FlightBatteryStateHandle()) {
536  FlightBatteryStateData bat;
537  FlightBatteryStateGet(&bat);
538  // FrSky protocol normally uses 3.3V at
539  // 255 but divider from display can be
540  // set internally
541  if (bat.DetectedCellCount) {
542  /* Use detected cell count to
543  * maximize useful resolution
544  */
545  tx_buf[2] = (uint8_t) (bat.Voltage / bat.DetectedCellCount / 5.0f * 255);
546  } else {
547  tx_buf[2] = (uint8_t) (bat.Voltage / 25.0f * 255);
548  }
549  tx_buf[3] = (uint8_t) (bat.Current / 60.0f * 255);
550  } else {
551  tx_buf[2] = 0;
552  tx_buf[3] = 0;
553  }
554 
555  /*
556  * 4-5: This used to be AFCC information, which
557  * was never used. Deprecated.
558  */
559  tx_buf[4] = 0;
560  tx_buf[5] = 0;
561  tx_buf[6] = count_set_bits(openlrs_status.LinkQuality & 0x7fff);
562  tx_buf[7] = 0;
563  tx_buf[8] = 0;
564 
565  len = TELEMETRY_PACKETSIZE;
566  }
567 
568  return len;
569 }
570 
571 static uint32_t millis()
572 {
573  return PIOS_Thread_Systime();
574 }
575 
576 /*****************************************************************************
577  * OpenLRS hardware access
578  *****************************************************************************/
579 
580 static uint8_t beaconGetRSSI(pios_openlrs_t openlrs_dev)
581 {
582  uint16_t rssiSUM = 0;
583 
584  rfm22_set_frequency(openlrs_dev, openlrs_dev->beacon_frequency);
585 
586  PIOS_Thread_Sleep(1);
587  rssiSUM += rfm22_get_rssi(openlrs_dev);
588  PIOS_Thread_Sleep(1);
589  rssiSUM += rfm22_get_rssi(openlrs_dev);
590  PIOS_Thread_Sleep(1);
591  rssiSUM += rfm22_get_rssi(openlrs_dev);
592  PIOS_Thread_Sleep(1);
593  rssiSUM += rfm22_get_rssi(openlrs_dev);
594 
595  rfm22_set_frequency(openlrs_dev, openlrs_dev->bind_data.rf_frequency);
596 
597  return rssiSUM / 4;
598 }
599 
600 static bool binding_button_pushed(pios_openlrs_t openlrs_dev)
601 {
602  static bool button_history[3];
603 
604  if (!openlrs_dev->cfg.bind_button) {
605  return false;
606  }
607 
608  bool truth_value = GPIO_ReadInputDataBit(
609  openlrs_dev->cfg.bind_button->gpio,
610  openlrs_dev->cfg.bind_button->init.GPIO_Pin) !=
611  Bit_RESET;
612 
613  if (!openlrs_dev->cfg.bind_active_high) {
614  truth_value = !truth_value;
615  }
616 
617  button_history[2] = button_history[1];
618  button_history[1] = button_history[0];
619  button_history[0] = truth_value;
620 
621  /* Catch an edge, but also require 2 in a row. */
622  return button_history[0] && button_history[1] &&
623  (!button_history[2]);
624 }
625 
626 /*****************************************************************************
627  * High level OpenLRS functions
628  *****************************************************************************/
629 
630 static bool pios_openlrs_bind_transmit_step(pios_openlrs_t openlrs_dev)
631 {
632  if (openlrs_dev->bind_data.version != BINDING_VERSION) {
633  /* decline to send bind with invalid configuration */
634  return false;
635  }
636 
637  uint32_t start = millis();
638 
639  /* Select bind channel, bind power, and bind packet header */
640  rfm22_init(openlrs_dev, true);
641  PIOS_Thread_Sleep(5);
642 
643  rfm22_tx_packet(openlrs_dev,
644  &openlrs_dev->bind_data,
645  sizeof(openlrs_dev->bind_data));
646 
647  rfm22_rx_reset(openlrs_dev, false);
648 
649  DEBUG_PRINTF(2,"Waiting bind ack\r\n");
650 
651  while ((millis() - start) < 200) {
652 #if defined(PIOS_INCLUDE_WDG) && defined(PIOS_WDG_RFM22B)
653  // Update the watchdog timer
654  PIOS_WDG_UpdateFlag(PIOS_WDG_RFM22B);
655 #endif /* PIOS_WDG_RFM22B */
656 
657  bool have_interrupt = wait_interrupt(openlrs_dev, 15);
658 
659  if (have_interrupt) {
660  DEBUG_PRINTF(2,"Got pkt\r\n");
661 
662  uint8_t recv_byte;
663 
664  if (rfm22_rx_packet(openlrs_dev, &recv_byte, 1) &&
665  (recv_byte == 'B')) {
666  DEBUG_PRINTF(2, "Received bind ack\r\n");
667 
668  return true;
669  }
670 
671  rfm22_rx_reset(openlrs_dev, false);
672  }
673  }
674 
675  return false;
676 }
677 
678 
679 static void pios_openlrs_config_to_port_config(pios_openlrs_t openlrs_dev)
680 {
681  uintptr_t *target = NULL;
682 
683  OpenLRSData config;
684  OpenLRSGet(&config);
685 
686  if (config.role == OPENLRS_ROLE_DISABLED) {
687  return;
688  }
689 
690  switch (config.data_source) {
691  case OPENLRS_DATA_SOURCE_UAVOTELEMETRY:
692  if (config.role == OPENLRS_ROLE_TX) {
693 #ifdef PIOS_COM_RADIOBRIDGE
694  target = &PIOS_COM_RADIOBRIDGE;
695 #else
696  return;
697 #endif
698  } else {
699 #if defined(PIPXTREME) && defined(PIOS_COM_RADIOBRIDGE)
700  /* Only on pipxtreme, does "uavotelemetry" in
701  * an RX still mean we use the radio
702  * combridge. Otherwise we just hook our
703  * native telemetry up to the port.
704  */
705  target = &PIOS_COM_RADIOBRIDGE;
706 #else
707  target = &PIOS_COM_TELEM_SER;
708 #endif
709  }
710  break;
711  case OPENLRS_DATA_SOURCE_COMBRIDGE:
712  target = &PIOS_COM_BRIDGE;
713  break;
714  case OPENLRS_DATA_SOURCE_DISABLED:
715  default:
716  return;
717  break;
718  }
719 
720  uintptr_t com_id;
721 
722  /* TX Buffer must be big enough to store maximal UAVO */
723  if (PIOS_COM_Init(&com_id, &pios_openlrs_com_driver,
724  (uintptr_t)openlrs_dev, 128, 288)) {
725  PIOS_Assert(0);
726  }
727 
728  *target = com_id;
729 }
730 
731 static bool pios_openlrs_config_to_bind_data(pios_openlrs_t openlrs_dev)
732 {
733  OpenLRSData config;
734  OpenLRSGet(&config);
735 
736  bool inited = false;
737 
738  if ((config.version == BINDING_VERSION) &&
739  (config.rf_power > OPENLRS_RF_POWER_0)) {
740  openlrs_dev->bind_data.hdr = 'b';
741  openlrs_dev->bind_data.version = config.version;
742  openlrs_dev->bind_data.reserved[0] = 0;
743  openlrs_dev->bind_data.reserved[1] = 0;
744  openlrs_dev->bind_data.reserved[2] = 0;
745  openlrs_dev->bind_data.rf_frequency = config.rf_frequency;
746  openlrs_dev->bind_data.rf_magic = config.rf_magic;
747  openlrs_dev->bind_data.rf_power = config.rf_power - 1;
748  openlrs_dev->bind_data.rf_channel_spacing = config.rf_channel_spacing;
749 
750  if (config.modem_params <= OPENLRS_MODEM_PARAMS_MAXOPTVAL) {
751  openlrs_dev->bind_data.modem_params = config.modem_params;
752  }
753  openlrs_dev->bind_data.flags = config.flags;
754  openlrs_dev->bind_data.ext_flags = config.ext_flags;
755  for (uint32_t i = 0; i < OPENLRS_HOPCHANNEL_NUMELEM; i++)
756  openlrs_dev->bind_data.hopchannel[i] = config.hopchannel[i];
757  } else if (config.role == OPENLRS_ROLE_TX) {
758  inited = true;
759 
760  /* Create valid bind data */
761  openlrs_dev->bind_data.hdr = 'b';
762  openlrs_dev->bind_data.version = BINDING_VERSION;
763  openlrs_dev->bind_data.reserved[0] = 0;
764  openlrs_dev->bind_data.reserved[1] = 0;
765  openlrs_dev->bind_data.reserved[2] = 0;
766  openlrs_dev->bind_data.rf_frequency =
767  def_carrier_freq(openlrs_dev->band);
768  openlrs_dev->bind_data.rf_magic = randomize_int(0);
769  openlrs_dev->bind_data.rf_power = OPENLRS_RF_POWER_100 - 1;
770  openlrs_dev->bind_data.rf_channel_spacing = 1;
771  /* Unit is * 10KHz */
772  openlrs_dev->bind_data.modem_params =
773  OPENLRS_MODEM_PARAMS_57600;
774  openlrs_dev->bind_data.flags = 10; /* Telemetry + 8 channels */
775 
776  /* Use dR extensions and pass telem by default */
777  openlrs_dev->bind_data.ext_flags = EXT_FLAG_FASTDATA;
778 
779  for (uint32_t i = 0; i < OPENLRS_HOPCHANNEL_NUMELEM; i++) {
780  /* Generate 7 channels by default. */
781  if (i < 7) {
782  /* At spacing of 10KHz, this is from 435.250
783  * to 437.480.
784  *
785  * Also ensure channels are unique by
786  * getting 3 LSB from hop idx
787  */
788  openlrs_dev->bind_data.hopchannel[i] =
789  (randomize_int(27) << 3) + 25 + i;
790  } else {
791  openlrs_dev->bind_data.hopchannel[i] = 0;
792  }
793  }
794  }
795 
796  // Also copy beacon settings over to device config.
797  openlrs_dev->beacon_frequency = config.beacon_frequency;
798  openlrs_dev->beacon_delay = config.beacon_delay;
799  openlrs_dev->beacon_period = config.beacon_period;
800 
801  openlrs_dev->scale_min = config.tx_scale_min;
802  openlrs_dev->scale_max = config.tx_scale_max;
803 
804  openlrs_dev->tx_source = config.tx_source;
805  openlrs_dev->tx_startup_bind_duration = config.tx_startup_bind_duration;
806  openlrs_dev->tx_bind_button_duration = config.tx_bind_button_duration;
807 
808  return inited;
809 }
810 
811 static bool pios_openlrs_bind_data_to_config(pios_openlrs_t openlrs_dev)
812 {
813  if (openlrs_dev->bind_data.hdr == 'b') {
814 #if defined(PIOS_INCLUDE_DEBUG_CONSOLE)
815  if (2 <= DEBUG_LEVEL && pios_com_debug_id > 0) {
816  DEBUG_PRINTF(2, "Binding settings:\r\n");
817  PIOS_Thread_Sleep(10);
818  DEBUG_PRINTF(2, " version: %d\r\n", openlrs_dev->bind_data.version);
819  PIOS_Thread_Sleep(10);
820  DEBUG_PRINTF(2, " rf_frequency: %d\r\n", openlrs_dev->bind_data.rf_frequency);
821  PIOS_Thread_Sleep(10);
822  DEBUG_PRINTF(2, " rf_power: %d\r\n", openlrs_dev->bind_data.rf_power);
823  PIOS_Thread_Sleep(10);
824  DEBUG_PRINTF(2, " rf_channel_spacing: %d\r\n", openlrs_dev->bind_data.rf_channel_spacing);
825  PIOS_Thread_Sleep(10);
826  DEBUG_PRINTF(2, " modem_params: %d\r\n", openlrs_dev->bind_data.modem_params);
827  PIOS_Thread_Sleep(10);
828  DEBUG_PRINTF(2, " flags: %d\r\n", openlrs_dev->bind_data.flags);
829  PIOS_Thread_Sleep(10);
830 
831  for (uint32_t i = 0; i < MAXHOPS; i++) {
832  DEBUG_PRINTF(2, " hop channel: %d\r\n", openlrs_dev->bind_data.hopchannel[i]);
833  PIOS_Thread_Sleep(10);
834  }
835  }
836 #endif /* PIOS_INCLUDE_DEBUG_CONSOLE */
837 
838  if (openlrs_dev->bind_data.version == BINDING_VERSION) {
839  if (openlrs_dev->bind_data.rf_power >=
840  OPENLRS_RF_POWER_MAXOPTVAL) {
841  /* >= because offset by 1 */
842  return false;
843  }
844 
845  if (openlrs_dev->bind_data.modem_params >
846  OPENLRS_MODEM_PARAMS_MAXOPTVAL) {
847  return false;
848  }
849 
850  OpenLRSData binding;
851  OpenLRSGet(&binding);
852  binding.version = openlrs_dev->bind_data.version;
853  binding.rf_frequency = openlrs_dev->bind_data.rf_frequency;
854  binding.rf_magic = openlrs_dev->bind_data.rf_magic;
855  binding.rf_power = openlrs_dev->bind_data.rf_power + 1;
856  binding.rf_channel_spacing = openlrs_dev->bind_data.rf_channel_spacing;
857  binding.modem_params = openlrs_dev->bind_data.modem_params;
858  binding.flags = openlrs_dev->bind_data.flags;
859  binding.ext_flags = openlrs_dev->bind_data.ext_flags;
860  for (uint32_t i = 0; i < OPENLRS_HOPCHANNEL_NUMELEM; i++)
861  binding.hopchannel[i] = openlrs_dev->bind_data.hopchannel[i];
862  binding.beacon_frequency = openlrs_dev->beacon_frequency;
863  binding.beacon_delay = openlrs_dev->beacon_delay;
864  binding.beacon_period = openlrs_dev->beacon_period;
865 
866  if ((openlrs_dev->bind_data.ext_flags &
867  EXT_FLAG_FASTDATA) &&
868  (binding.data_source ==
869  OPENLRS_DATA_SOURCE_DISABLED)) {
870  /* If we're bound to someone with dRLRS
871  * extensions, assume we're passing telem.
872  */
873  binding.data_source =
874  OPENLRS_DATA_SOURCE_UAVOTELEMETRY;
875  }
876 
877  OpenLRSSet(&binding);
878  UAVObjSave(OpenLRSHandle(), 0);
879 
880  DEBUG_PRINTF(2, "Saved bind data\r\n");
881 #if defined(PIOS_LED_LINK)
882  PIOS_ANNUNC_Toggle(PIOS_LED_LINK);
883 #endif /* PIOS_LED_LINK */
884 
885  return true;
886  }
887  }
888 
889  return false;
890 }
891 
892 static bool pios_openlrs_bind_receive(pios_openlrs_t openlrs_dev,
893  uint32_t timeout)
894 {
895  uint32_t start = millis();
896  uint8_t txb;
897  rfm22_init(openlrs_dev, true);
898  rfm22_rx_reset(openlrs_dev, true);
899  DEBUG_PRINTF(2,"Waiting bind\r\n");
900 
901  uint32_t i = 0;
902 
903  while ((!timeout) || ((millis() - start) < timeout)) {
904  system_annunc_custom_string("o"); /* --- */
905  PIOS_Thread_Sleep(1);
906 #if defined(PIOS_INCLUDE_WDG) && defined(PIOS_WDG_RFM22B)
907  // Update the watchdog timer
908  PIOS_WDG_UpdateFlag(PIOS_WDG_RFM22B);
909 #endif /* PIOS_WDG_RFM22B */
910 
911  if (i++ % 100 == 0) {
912  DEBUG_PRINTF(2,"Waiting bind\r\n");
913  }
914  if (openlrs_dev->rf_mode == Received) {
915  DEBUG_PRINTF(2,"Got pkt\r\n");
916 
917  bool ok = rfm22_rx_packet(openlrs_dev,
918  &openlrs_dev->bind_data,
919  sizeof(openlrs_dev->bind_data));
920 
921  if (ok && pios_openlrs_bind_data_to_config(openlrs_dev)) {
922  DEBUG_PRINTF(2, "data good\r\n");
923 
924  /* Acknowledge binding */
925  txb = 'B';
926  rfm22_tx_packet(openlrs_dev, &txb, 1);
927  rfm22_tx_packet(openlrs_dev, &txb, 1);
928  rfm22_tx_packet(openlrs_dev, &txb, 1);
929 
930  /* Exit bind mode, program proper
931  * carrier frequency, etc. */
932  rfm22_init(openlrs_dev, false);
933 
934  return true;
935  }
936  }
937  }
938 
939  return false;
940 }
941 
942 #if defined(PIOS_INCLUDE_DEBUG_CONSOLE)
943 static void printVersion(uint16_t v)
944 {
945  char ver[8];
946  ver[0] = '0' + ((v >> 8) & 0x0f);
947  ver[1] = '.';
948  ver[2] = '0' + ((v >> 4) & 0x0f);
949  if (v & 0x0f) {
950  ver[3] = '.';
951  ver[4] = '0' + (v & 0x0f);
952  ver[5] = '\r';
953  ver[6] = '\n';
954  ver[7] = '\0';
955  } else {
956  ver[3] = '\r';
957  ver[4] = '\n';
958  ver[5] = '\0';
959  }
960  DEBUG_PRINTF(2, ver);
961 }
962 #endif
963 
964 static void pios_openlrs_setup(pios_openlrs_t openlrs_dev)
965 {
966  DEBUG_PRINTF(2,"OpenLRSng RX setup starting.\r\n");
967  PIOS_Thread_Sleep(5);
968 #if defined(PIOS_INCLUDE_DEBUG_CONSOLE)
969  printVersion(OPENLRSNG_VERSION);
970 #endif
971 
972  DEBUG_PRINTF(2,"Entering normal mode\r\n");
973 
974  // Count hopchannels as we need it later
975  openlrs_dev->hopcount = 0;
976  while ((openlrs_dev->hopcount < MAXHOPS) &&
977  (openlrs_dev->bind_data.hopchannel[openlrs_dev->hopcount] != 0)) {
978  openlrs_dev->hopcount++;
979  }
980 
981  // Configure for normal operation.
982  rfm22_init(openlrs_dev, false);
983 
984  pios_openlrs_do_hop(openlrs_dev);
985 
986  openlrs_dev->link_acquired = false;
987  openlrs_dev->lastPacketTimeUs = PIOS_DELAY_GetuS();
988 
989  DEBUG_PRINTF(2,"OpenLRSng RX setup complete\r\n");
990 }
991 
992 static void pios_openlrs_rx_update_rssi(pios_openlrs_t openlrs_dev,
993  uint8_t rssi, bool have_frame)
994 {
995  openlrs_status.LinkQuality <<= 1;
996  openlrs_status.LinkQuality |= !!have_frame;
997 
998  if (have_frame) {
999  openlrs_status.LastRSSI = rssi;
1000  } else {
1001  /*
1002  * If we weren't able to sample RSSI, don't just carry the
1003  * old value forward indefinitely.
1004  */
1005  openlrs_status.LastRSSI >>= 1;
1006  }
1007 }
1008 
1009 static bool pios_openlrs_rx_frame(pios_openlrs_t openlrs_dev, uint8_t rssi)
1010 {
1011  uint8_t *tx_buf = openlrs_dev->tx_buf;
1012  uint8_t rx_buf[MAX_PACKET_SIZE];
1013 
1014  int ctrl_size = get_control_packet_size(&openlrs_dev->bind_data);
1015 
1016  // Read the packet from RFM22b
1017  int size = rfm22_rx_packet_variable(openlrs_dev, rx_buf,
1018  MAX_PACKET_SIZE);
1019 
1020  if (size <= 0) {
1021  return false;
1022  }
1023 
1024  pios_openlrs_rx_update_rssi(openlrs_dev, rssi, true);
1025 
1026 #if defined(PIOS_LED_LINK)
1027  PIOS_ANNUNC_Toggle(PIOS_LED_LINK);
1028 #endif /* PIOS_LED_LINK */
1029 
1030  if ((rx_buf[0] & 0x3e) == 0x00) {
1031  // This flag indicates receiving control data
1032 
1033  if (size != ctrl_size) {
1034  // These size mismatches should never happen because
1035  // we are protected by a CRC, and shared magic hdr.
1036  return false;
1037  }
1038 
1039  unpack_channels(openlrs_dev->bind_data.flags,
1040  openlrs_dev->ppm, rx_buf + 1);
1041  rxscale_channels(openlrs_dev->ppm);
1042 
1043  // Call the control received callback if it's available.
1044  if (openlrs_dev->openlrs_rcvr_id) {
1045 #if defined(PIOS_INCLUDE_OPENLRS_RCVR)
1046  PIOS_OpenLRS_Rcvr_UpdateChannels(
1047  openlrs_dev->openlrs_rcvr_id,
1048  openlrs_dev->ppm);
1049 #endif
1050  }
1051  } else if ((rx_buf[0] & 0x3f) == 0x02) {
1052  /* This is a "placeholder frame" -- no control data,
1053  * no telemetry.
1054  */
1055 
1056  /* Still set acknowledge bit to match-- they think we have
1057  * any data, and won't be resending, so be ready to accept
1058  * anything new they send.
1059  */
1060  if (rx_buf[0] & 0x80) {
1061  openlrs_dev->tx_buf[0] |= 0x80;
1062  } else {
1063  openlrs_dev->tx_buf[0] &= ~0x80;
1064  }
1065  } else {
1066  // Not control data. Push into serial RX buffer.
1067  if (((rx_buf[0] & 0x38) == 0x38) &&
1068  ((rx_buf[0] ^ tx_buf[0]) & 0x80)) {
1069  // We got new data... (not retransmission)
1070  bool rx_need_yield;
1071  uint8_t data_len = (rx_buf[0] & 7) + 1;
1072 
1073  if ((data_len + 1) > size) {
1074  return false;
1075  }
1076 
1077  if (data_len < (size - 1)) {
1078  /* Extended / fast-data operation */
1079  data_len = size - 1;
1080  }
1081 
1082  if (openlrs_dev->rx_in_cb) {
1083  (openlrs_dev->rx_in_cb)(openlrs_dev->rx_in_context,
1084  &rx_buf[1], data_len,
1085  NULL, &rx_need_yield);
1086  }
1087  }
1088 
1089  /* Set acknowledge bit to match */
1090  if (rx_buf[0] & 0x80) {
1091  openlrs_dev->tx_buf[0] |= 0x80;
1092  } else {
1093  openlrs_dev->tx_buf[0] &= ~0x80;
1094  }
1095  }
1096 
1097  // When telemetry is enabled we ack packets and send info about FC back
1098  if (openlrs_dev->bind_data.flags & TELEMETRY_MASK) {
1099  if ((tx_buf[0] ^ rx_buf[0]) & 0x40) {
1100  // Last message was lost... resend, unless it
1101  // is just ordinary rx telemetry in which
1102  // case we can update it.
1103 
1104  if ((tx_buf[0] & 0x3F) == 0) {
1105  // Don't swap telem sequence bit though.
1106  openlrs_dev->telem_len =
1107  pios_openlrs_form_telemetry(
1108  openlrs_dev, tx_buf, rssi,
1109  false, ctrl_size - size);
1110  }
1111  } else {
1112  tx_buf[0] ^= 0x40; // Swap telem sequence
1113  openlrs_dev->telem_len = pios_openlrs_form_telemetry(
1114  openlrs_dev, tx_buf, rssi, false,
1115  ctrl_size - size);
1116  }
1117 
1118  if (openlrs_dev->telem_len == 0) {
1119  tx_buf[0] |= 2;
1120  openlrs_dev->telem_len = 1;
1121  }
1122 
1123  // This will block until sent
1124  rfm22_tx_packet(openlrs_dev, tx_buf, openlrs_dev->telem_len);
1125  }
1126 
1127  // Once a packet has been processed, flip back into receiving mode
1128  rfm22_rx_reset(openlrs_dev, false);
1129 
1130  return true;
1131 }
1132 
1133 static void pios_openlrs_do_hop(pios_openlrs_t openlrs_dev)
1134 {
1135  openlrs_dev->rf_channel++;
1136 
1137  if (openlrs_dev->rf_channel >= openlrs_dev->hopcount) {
1138  openlrs_dev->rf_channel = 0;
1139  }
1140 
1141  if (openlrs_dev->beacon_armed) {
1142  // Listen for RSSI on beacon channel briefly for
1143  // a sharp rising edge in RSSI to 'trigger' us
1144  uint8_t rssi = beaconGetRSSI(openlrs_dev) << 2;
1145 
1146  if ((openlrs_dev->beacon_rssi_avg + 80) < rssi) {
1147  openlrs_dev->nextBeaconTimeMs =
1148  millis() + 1000L;
1149  }
1150 
1151  openlrs_dev->beacon_rssi_avg =
1152  (openlrs_dev->beacon_rssi_avg * 3 + rssi) >> 2;
1153  }
1154 
1155  rfm22_set_channel(openlrs_dev, openlrs_dev->rf_channel);
1156 }
1157 
1158 static void pios_openlrs_rx_after_receive(pios_openlrs_t openlrs_dev,
1159  bool have_frame)
1160 {
1161  uint32_t timeUs, timeMs;
1162 
1163  /* We hop channels here, unless we're hopping slowly and
1164  * it's not time yet.
1165  */
1166  bool willhop = true;
1167 
1168 #if defined(PIOS_INCLUDE_WDG) && defined(PIOS_WDG_RFM22B)
1169  // Update the watchdog timer
1170  PIOS_WDG_UpdateFlag(PIOS_WDG_RFM22B);
1171 #endif /* PIOS_WDG_RFM22B */
1172 
1173  timeUs = PIOS_DELAY_GetuS();
1174  timeMs = millis();
1175 
1176  DEBUG_PRINTF(2,"Time: %d\r\n", timeUs);
1177 
1178  uint32_t interval = get_nominal_packet_interval(
1179  &openlrs_dev->bind_data, NULL);
1180 
1181  // For missing packets to properly trigger a well timed
1182  // channel hop, this method should be called at the
1183  // appropriate time after the packet was expected to trigger
1184  // this path.
1185  if (have_frame) {
1186  // Flag to indicate we ever got a frame
1187  openlrs_dev->link_acquired = true;
1188 
1189  openlrs_dev->beacon_armed = false;
1190  openlrs_dev->numberOfLostPackets = 0;
1191  openlrs_dev->nextBeaconTimeMs = 0;
1192 
1193  /* Could consider splitting difference with interval */
1194  openlrs_dev->lastPacketTimeUs = timeUs;
1195  openlrs_dev->linkLossTimeMs = 0;
1196  } else if (openlrs_dev->numberOfLostPackets < openlrs_dev->hopcount) {
1197  DEBUG_PRINTF(2,"OLRS WARN: Lost packet: %d\r\n",
1198  openlrs_dev->numberOfLostPackets);
1199  /* We lost packet, execute normally timed hop */
1200  openlrs_dev->numberOfLostPackets++;
1201  openlrs_dev->lastPacketTimeUs += interval;
1202  } else if ((PIOS_DELAY_GetuSSince(openlrs_dev->lastPacketTimeUs) <
1203  (interval * (openlrs_dev->hopcount + 1)))) {
1204 #if defined(PIOS_LED_LINK)
1205  PIOS_ANNUNC_Off(PIOS_LED_LINK);
1206 #endif /* PIOS_LED_LINK */
1207 
1208  /* Don't have link; dwell on this channel to give a chance
1209  * for resync.
1210  */
1211  willhop = false;
1212  } else {
1213  /* We don't have link and have been on this channel a
1214  * long time; time to do a hop. (Because the channel
1215  * may be blocked preventing resync).
1216  */
1217  DEBUG_PRINTF(2,"OLRS WARN: Trying to sync\r\n");
1218 
1219  // hop slowly to allow resync with TX
1220  openlrs_dev->lastPacketTimeUs = timeUs;
1221 
1222  if (!openlrs_dev->linkLossTimeMs) {
1223  openlrs_dev->linkLossTimeMs = timeMs;
1224  }
1225  }
1226 
1227  if (openlrs_dev->link_acquired && openlrs_dev->linkLossTimeMs) {
1228  if (!openlrs_dev->nextBeaconTimeMs) {
1229  openlrs_dev->nextBeaconTimeMs = timeMs +
1230  openlrs_dev->beacon_delay * 1000UL;
1231  } else if ((openlrs_dev->beacon_frequency) &&
1232  ((timeMs - openlrs_dev->nextBeaconTimeMs)
1233  < 0x80000000)) {
1234  // Indicate that the beacon is now active so we can
1235  // trigger extra ones by RF.
1236  openlrs_dev->beacon_armed = true;
1237 
1238  DEBUG_PRINTF(2,"Beacon time: %d\r\n", openlrs_dev->nextBeaconTimeMs);
1239  // Only beacon when vehicle is disarmed
1240  // (This should be reconsidered, because it means we
1241  // can't beacon when always armed, etc.)
1242  uint8_t armed;
1243  FlightStatusArmedGet(&armed);
1244  if (armed == FLIGHTSTATUS_ARMED_DISARMED) {
1245  rfm22_beacon_send(openlrs_dev, false);
1246  rfm22_init(openlrs_dev, false);
1247  rfm22_rx_reset(openlrs_dev, false);
1248  openlrs_dev->nextBeaconTimeMs = (timeMs + 1000UL * openlrs_dev->beacon_period) | 1; // avoid 0 in time
1249  }
1250  }
1251  }
1252 
1253  if (willhop) {
1254  pios_openlrs_do_hop(openlrs_dev);
1255  rfm22_rx_reset(openlrs_dev, false);
1256  }
1257 
1258  // Update UAVO
1259  OpenLRSStatusSet(&openlrs_status);
1260 }
1261 
1262 uint8_t PIOS_OpenLRS_RSSI_Get(void)
1263 {
1264  // Check object handle exists
1265  if (OpenLRSHandle() == NULL)
1266  return 0;
1267 
1268  uint8_t rssi_type;
1269  OpenLRSRSSI_TypeGet(&rssi_type);
1270 
1271  uint16_t LQ = count_set_bits(openlrs_status.LinkQuality & 0x7fff);
1272 
1273  switch (rssi_type) {
1274  case OPENLRS_RSSI_TYPE_COMBINED:
1275  if (LQ == 15) {
1276  return (openlrs_status.LastRSSI >> 1)+128;
1277  } else if (LQ == 0) {
1278  return 0;
1279  } else {
1280  return LQ * 9 +
1281  (openlrs_status.LastRSSI >> 5);
1282  }
1283  case OPENLRS_RSSI_TYPE_RSSI:
1284  return openlrs_status.LastRSSI;
1285  case OPENLRS_RSSI_TYPE_LINKQUALITY:
1286  return LQ << 4;
1287  default:
1288  return 0;
1289  }
1290 }
1291 
1292 /*****************************************************************************
1293  * control Code
1294  *****************************************************************************/
1295 
1302 void PIOS_OpenLRS_RegisterRcvr(pios_openlrs_t openlrs_dev,
1303  uintptr_t openlrs_rcvr_id)
1304 {
1305  PIOS_Assert(pios_openlrs_validate(openlrs_dev));
1306 
1307  openlrs_dev->openlrs_rcvr_id = openlrs_rcvr_id;
1308 }
1309 
1310 /*****************************************************************************
1311 * Task and device setup
1312 *****************************************************************************/
1313 
1322 int32_t PIOS_OpenLRS_Init(pios_openlrs_t *openlrs_id, pios_spi_t spi_id,
1323  uint32_t slave_num, const struct pios_openlrs_cfg *cfg,
1324  HwSharedRfBandOptions rf_band,
1325  HwSharedMaxRfPowerOptions rf_power)
1326 {
1327  PIOS_DEBUG_Assert(rfm22b_id);
1328  PIOS_DEBUG_Assert(cfg);
1329 
1330  // Allocate the device structure.
1331  pios_openlrs_t openlrs_dev = pios_openlrs_alloc();
1332  if (!openlrs_dev) {
1333  return -1;
1334  }
1335 
1336  // Store the SPI handle
1337  openlrs_dev->slave_num = slave_num;
1338  openlrs_dev->spi_id = spi_id;
1339 
1340  // and the rf limitations
1341  openlrs_dev->band = rf_band;
1342  openlrs_dev->max_power = rf_power;
1343 
1344  // Before initializing everything, make sure device found
1345  uint8_t device_type = rfm22_read(openlrs_dev, RFM22_DEVICE_TYPE) & RFM22_DT_MASK;
1346  if (device_type != 0x08)
1347  return -1;
1348 
1349  OpenLRSInitialize();
1350  OpenLRSStatusInitialize();
1351 
1352  // Bind the configuration to the device instance
1353  openlrs_dev->cfg = *cfg;
1354 
1355  // Convert UAVO configuration to device runtime config
1356  // XXX register for updates! change at runtime.
1357  pios_openlrs_config_to_port_config(openlrs_dev);
1358 
1359  if (pios_openlrs_config_to_bind_data(openlrs_dev)) {
1360  /* Persist / save config */
1361  pios_openlrs_bind_data_to_config(openlrs_dev);
1362  }
1363 
1364  if (cfg->bind_button) {
1365  GPIO_Init(cfg->bind_button->gpio,
1366  (GPIO_InitTypeDef *) &(cfg->bind_button->init));
1367  }
1368 
1369  *openlrs_id = openlrs_dev;
1370 
1371  return 0;
1372 }
1373 
1374 int32_t PIOS_OpenLRS_Start(pios_openlrs_t openlrs_dev)
1375 {
1376  if (openlrs_dev->max_power == HWSHARED_MAXRFPOWER_0) {
1377  /* Refuse to init if we're not allowed to transmit.
1378  *
1379  * TODO: This rejects even when we have bind data
1380  * that would result in us never transmitting...
1381  * But that seems like a corner case anyways.
1382  */
1383  return -1;
1384  }
1385 
1386  // Initialize the external interrupt.
1387  PIOS_EXTI_Init(openlrs_dev->cfg.exti_cfg);
1388 
1389  // Start the receiver task.
1390  OpenLRSroleOptions role;
1391 
1392  OpenLRSroleGet(&role);
1393 
1394  switch (role) {
1395  case OPENLRS_ROLE_RX:
1396  openlrs_dev->taskHandle =
1397  PIOS_Thread_Create(pios_openlrs_rx_task,
1398  "PIOS_OpenLRS_Task", STACK_SIZE_BYTES,
1399  (void *)openlrs_dev, TASK_PRIORITY);
1400  TaskMonitorAdd(TASKINFO_RUNNING_MODEM,
1401  openlrs_dev->taskHandle);
1402  break;
1403  case OPENLRS_ROLE_TX:
1404  openlrs_dev->taskHandle =
1405  PIOS_Thread_Create(pios_openlrs_tx_task,
1406  "PIOS_OpenLRS_Task", STACK_SIZE_BYTES,
1407  (void *)openlrs_dev, TASK_PRIORITY);
1408  TaskMonitorAdd(TASKINFO_RUNNING_MODEM,
1409  openlrs_dev->taskHandle);
1410  break;
1411  case OPENLRS_ROLE_DISABLED:
1412  /* handle putting radio hw into safe state. */
1413  rfm22_disable(openlrs_dev);
1414  break;
1415  }
1416 
1417  return 0;
1418 }
1419 
1420 static int pios_openlrs_form_control_frame(pios_openlrs_t openlrs_dev,
1421  uint8_t *tx_buf)
1422 {
1423  int16_t channels[OPENLRS_PPM_NUM_CHANNELS];
1424 
1425  uintptr_t rcvr = PIOS_HAL_GetReceiver(openlrs_dev->tx_source);
1426 
1427  for (int i = 0; i < OPENLRS_PPM_NUM_CHANNELS; i++) {
1428  channels[i] = PIOS_RCVR_Read(rcvr, i+1);
1429  }
1430 
1431  for (int i = 0; i < 4; i++) {
1432  /* At least the first 4 channels must be good. If not,
1433  * decline to send a control frame.
1434  */
1435  if (channels[i] < 0) {
1436  return 0;
1437  }
1438  }
1439 
1440  for (int i = 4; i < OPENLRS_PPM_NUM_CHANNELS; i++) {
1441  /* Subsequent channels, put them in the middle if they're
1442  * invalid
1443  */
1444  if (channels[i] < 0) {
1445  channels[i] = (openlrs_dev->scale_min +
1446  openlrs_dev->scale_max) / 2;
1447  }
1448  }
1449 
1450  txscale_channels(channels, openlrs_dev->scale_min, openlrs_dev->scale_max);
1451 
1452  int len = pack_channels(openlrs_dev->bind_data.flags,
1453  channels, tx_buf+1);
1454 
1455  /* Packed length + 1 byte */
1456  return len + 1;
1457 }
1458 
1459 static bool pios_openlrs_tx_receive_telemetry(pios_openlrs_t openlrs_dev)
1460 {
1461  uint8_t rx_buf[MAX_PACKET_SIZE];
1462 
1463  // Read the packet from RFM22b
1464  int len = rfm22_rx_packet_variable(openlrs_dev, rx_buf,
1465  MAX_PACKET_SIZE);
1466 
1467  if (len <= 0) {
1468  return false;
1469  }
1470 
1471  if (((rx_buf[0] & 0x3f) == 0) && (len == TELEMETRY_PACKETSIZE)) {
1472  /* Generic link telemetry. Grab the data, shove it
1473  * into status.
1474  */
1475 
1476  openlrs_status.LastRSSI = rx_buf[1];
1477  openlrs_status.LinkQuality = (1 << (rx_buf[6])) - 1; /* fudge */
1478 
1479  OpenLRSStatusSet(&openlrs_status);
1480  } else if (((rx_buf[0] & 0x38) == 0x38) &&
1481  ((rx_buf[0] ^ openlrs_dev->tx_buf[0]) & 0x40)) {
1482  bool rx_need_yield;
1483  uint8_t data_len = (rx_buf[0] & 7) + 1;
1484 
1485  if ((data_len + 1) > len) {
1486  return false;
1487  }
1488 
1489  if (data_len < (len - 1)) {
1490  /* Extended / fast-data operation */
1491  data_len = len - 1;
1492  }
1493 
1494  if (openlrs_dev->rx_in_cb) {
1495  (openlrs_dev->rx_in_cb)(openlrs_dev->rx_in_context,
1496  &rx_buf[1], data_len,
1497  NULL, &rx_need_yield);
1498  }
1499  }
1500 
1501 #if defined(PIOS_LED_LINK)
1502  PIOS_ANNUNC_Toggle(PIOS_LED_LINK);
1503 #endif /* PIOS_LED_LINK */
1504 
1505  /* Set acknowledge bit to match */
1506  if (rx_buf[0] & 0x40) {
1507  openlrs_dev->tx_buf[0] |= 0x40;
1508  } else {
1509  openlrs_dev->tx_buf[0] &= ~0x40;
1510  }
1511 
1512  openlrs_dev->tx_prev_rxtelem_hdr = rx_buf[0];
1513 
1514  return true;
1515 }
1516 
1517 static void pios_openlrs_tx_wait_and_transmit(pios_openlrs_t openlrs_dev,
1518  uint32_t interval, uint8_t *tx_buf, int len)
1519 {
1520  // Now we want to delay until the next frame is due.
1521  while (PIOS_DELAY_GetuSSince(openlrs_dev->lastPacketTimeUs)
1522  < interval) {
1523  PIOS_Thread_Sleep(1);
1524  }
1525 
1526  openlrs_dev->lastPacketTimeUs += interval;
1527 
1528  rfm22_tx_packet(openlrs_dev, tx_buf, len);
1529 }
1530 
1531 static void pios_openlrs_tx_frame(pios_openlrs_t openlrs_dev)
1532 {
1533  rfm22_rx_reset(openlrs_dev, false);
1534 
1535  uint32_t rx_slop;
1536  uint32_t interval =
1537  get_nominal_packet_interval(&openlrs_dev->bind_data, &rx_slop);
1538 
1539  pios_openlrs_do_hop(openlrs_dev);
1540 
1541  bool telem_uplink = false;
1542 
1543  openlrs_dev->tx_buf[0] &= 0xc0; // Preserve top two sequence bits
1544 
1545  /* Form a control frame. If we can't, we're allowed to telemeter
1546  * no matter what. Otherwise, we hold it in case telemetry
1547  * decided to not do anything.
1548  */
1549  uint8_t tx_buf[MAX_PACKET_SIZE];
1550 
1551  tx_buf[0] = openlrs_dev->tx_buf[0];
1552 
1553  int len = pios_openlrs_form_control_frame(openlrs_dev, tx_buf);
1554 
1555  if ((!len) || openlrs_dev->tx_ok_to_telemeter) {
1556  openlrs_dev->tx_ok_to_telemeter = false;
1557 
1558  if (openlrs_dev->telem_len &&
1559  ((openlrs_dev->tx_buf[0] ^
1560  openlrs_dev->tx_prev_rxtelem_hdr) & 0x80)) {
1561  /* We're resending the existing buffer. */
1562  telem_uplink = true;
1563  } else {
1564  int ctrl_size =
1565  get_control_packet_size(&openlrs_dev->bind_data);
1566 
1567  /* Try to form a new telemetry lump */
1568  openlrs_dev->telem_len =
1569  pios_openlrs_form_telemetry(openlrs_dev,
1570  openlrs_dev->tx_buf, 0, true,
1571  ctrl_size - TELEMETRY_PACKETSIZE +
1572  TELEMETRY_PACKETSIZE / 2);
1573 
1574  /* If we have 9 byte control frames, allow us to send
1575  * up to 9 + 4 = 13 bytes of data, robbing downlink of
1576  * 4 bytes.
1577  *
1578  * 8 + (9-8+4) = 13
1579  *
1580  * Only FASTDATA obeys this.
1581  */
1582 
1583  if (openlrs_dev->telem_len) {
1584  /* And if we do, flip the send seq */
1585  telem_uplink = true;
1586  openlrs_dev->tx_buf[0] ^= 0x80;
1587  }
1588  }
1589  }
1590 
1591  if (telem_uplink) {
1592  pios_openlrs_tx_wait_and_transmit(openlrs_dev, interval,
1593  openlrs_dev->tx_buf, openlrs_dev->telem_len);
1594  } else if (len > 0) {
1595  /* From the control buf, so we don't touch the telemetry
1596  * buffer-- which may need a retry, after all.
1597  */
1598  pios_openlrs_tx_wait_and_transmit(openlrs_dev, interval,
1599  tx_buf, len);
1600  } else {
1601  /* We have nothing to transmit, but want to keep sync.
1602  * This is a dR protocol extension.
1603  */
1604  tx_buf[0] |= 2;
1605 
1606  pios_openlrs_tx_wait_and_transmit(openlrs_dev, interval,
1607  tx_buf, 1);
1608  }
1609 
1610  if (!(openlrs_dev->bind_data.flags & TELEMETRY_MASK)) {
1611  return;
1612  }
1613 
1614  rfm22_rx_reset(openlrs_dev, false);
1615 
1616  /* We wait until the RX will be listening on the next channel. */
1617  uint32_t expiry = openlrs_dev->lastPacketTimeUs + interval +
1618  rx_slop / 2;
1619 
1620  uint8_t rssi;
1621  bool have_interrupt = wait_interrupt_until(openlrs_dev, expiry,
1622  &rssi);
1623 
1624  bool got_packet = false;
1625 
1626  if (have_interrupt) {
1627  if (pios_openlrs_tx_receive_telemetry(openlrs_dev)) {
1628  if (!telem_uplink) {
1629  /* It's OK to telemeter when we receive
1630  * a downlink telemetry message.. and we
1631  * didn't telemeter this time around.
1632  */
1633  openlrs_dev->tx_ok_to_telemeter = true;
1634  }
1635 
1636  got_packet = true;
1637  }
1638  }
1639 
1640  if (!got_packet) {
1641  /* TODO: link-beep functionality goes here */
1642  if (openlrs_dev->numberOfLostPackets < openlrs_dev->hopcount) {
1643  openlrs_dev->numberOfLostPackets++;
1644  } else {
1645 #if defined(PIOS_LED_LINK)
1646  /* Ensure link light is off */
1647  PIOS_ANNUNC_Off(PIOS_LED_LINK);
1648 #endif
1649  if (!openlrs_dev->linkLossTimeMs) {
1650  openlrs_dev->linkLossTimeMs =
1651  PIOS_Thread_Systime();
1652  }
1653  }
1654  } else {
1655  openlrs_dev->numberOfLostPackets = 0;
1656  openlrs_dev->linkLossTimeMs = 0;
1657  openlrs_dev->link_acquired = true;
1658  }
1659 }
1660 
1661 static void pios_openlrs_tx_task(void *parameters)
1662 {
1663  pios_openlrs_t openlrs_dev = (pios_openlrs_t)parameters;
1664 
1665  // Register the watchdog timer for the radio driver task
1666 #if defined(PIOS_INCLUDE_WDG) && defined(PIOS_WDG_RFM22B)
1667  PIOS_WDG_RegisterFlag(PIOS_WDG_RFM22B);
1668 #endif /* PIOS_WDG_RFM22B */
1669 
1670  PIOS_Assert(pios_openlrs_validate(openlrs_dev));
1671 
1672  pios_openlrs_setup(openlrs_dev);
1673 
1674  PIOS_Thread_Sleep(200);
1675 
1676  pios_openlrs_setup(openlrs_dev);
1677 
1678  int i = openlrs_dev->tx_startup_bind_duration * 5;
1679 
1680  while (1) {
1681  //rfm22_check_hang(openlrs_dev);
1682 
1683  if (binding_button_pushed(openlrs_dev)) {
1684  i = openlrs_dev->tx_bind_button_duration * 5;
1685  }
1686 
1687  if (i > 0) {
1688  /* Time intervals are approximate; 100ms / step */
1689  i--;
1690 
1691  system_annunc_custom_string("cq"); /* -.-. --.- */
1692 
1693  /* Binding maintains WDG */
1694  if (pios_openlrs_bind_transmit_step(openlrs_dev)) {
1695  /* Binding done once we bind one */
1696  i = 0;
1697  }
1698 
1699  continue;
1700  } else if (i == 0) {
1701  rfm22_init(openlrs_dev, false);
1702  i--;
1703  }
1704 
1705  /* Actual packet transmit maintains WDG */
1706  pios_openlrs_tx_frame(openlrs_dev);
1707  }
1708 }
1709 
1710 static void pios_openlrs_rx_step(pios_openlrs_t openlrs_dev)
1711 {
1712  rfm22_check_hang(openlrs_dev);
1713 
1714  uint8_t rssi = 0;
1715 
1716  uint32_t interval, slop;
1717 
1718  interval = get_nominal_packet_interval(&openlrs_dev->bind_data,
1719  &slop);
1720 
1721  /*
1722  * It's worth noting that for non-telemetry modes, we need
1723  * to stay very closely in sync because the TX is transmitting
1724  * most of the time. This means that we need to carefully
1725  * split that "dead" non-TX time, arriving to channels slightly
1726  * early, and leaving slightly late if we have not received a
1727  * frame.
1728  *
1729  * For telemetry modes, since "we" have a timeslot the
1730  * tolerances can be much more lax-- we can afford to listen
1731  * into what we believe to be our telemetry timeslot and thus
1732  * cope better if the TX is slower than we expect.
1733  */
1734 
1735  uint32_t expiry = openlrs_dev->lastPacketTimeUs + interval + slop;
1736 
1737  bool have_frame = false;
1738 
1739  if (wait_interrupt_until(openlrs_dev, expiry, &rssi)) {
1740  // Process incoming data
1741  have_frame = pios_openlrs_rx_frame(openlrs_dev, rssi);
1742  } else {
1743  // TODO: We could delay a little bit more if we looked
1744  // at the hardware and see it's midway through
1745  // receiving a valid frame.
1746 
1747  // We timed out because packet was missed
1748  DEBUG_PRINTF(3,
1749  "ISR Timeout. Missed packet: %d %d\r\n",
1750  expiry, interval);
1751  }
1752 
1753  if (!have_frame) {
1754  pios_openlrs_rx_update_rssi(openlrs_dev, rssi,
1755  false);
1756  }
1757 
1758  // Select next channel, update timers, etc.
1759  pios_openlrs_rx_after_receive(openlrs_dev, have_frame);
1760 
1761  DEBUG_PRINTF(3, "Processing time %d\r\n", PIOS_DELAY_GetuSSince(openlrs_dev->lastPacketTimeUs));
1762 }
1763 
1769 static void pios_openlrs_rx_task(void *parameters)
1770 {
1771  pios_openlrs_t openlrs_dev = (pios_openlrs_t)parameters;
1772 
1773  // Register the watchdog timer for the radio driver task
1774 #if defined(PIOS_INCLUDE_WDG) && defined(PIOS_WDG_RFM22B)
1775  PIOS_WDG_RegisterFlag(PIOS_WDG_RFM22B);
1776 #endif /* PIOS_WDG_RFM22B */
1777 
1778  PIOS_Assert(pios_openlrs_validate(openlrs_dev));
1779 
1780  pios_openlrs_setup(openlrs_dev);
1781 
1782  DEBUG_PRINTF(2, "Setup complete\r\n");
1783 
1784  while (1) {
1785  // check binding truth value-- but only if we've not
1786  // had link since powerup.
1787  if ((!openlrs_dev->link_acquired) &&
1788  binding_button_pushed(openlrs_dev)) {
1789  openlrs_dev->bind_data.version = 0;
1790  }
1791 
1792  while (openlrs_dev->bind_data.version != BINDING_VERSION) {
1793  pios_openlrs_bind_receive(openlrs_dev, 0);
1794  }
1795 
1796 #if defined(PIOS_INCLUDE_WDG) && defined(PIOS_WDG_RFM22B)
1797  // Update the watchdog timer
1798  PIOS_WDG_UpdateFlag(PIOS_WDG_RFM22B);
1799 #endif /* PIOS_WDG_RFM22B */
1800 
1801  pios_openlrs_rx_step(openlrs_dev);
1802  }
1803 }
1804 
1805 bool PIOS_OpenLRS_EXT_Int(pios_openlrs_t openlrs_dev)
1806 {
1807  PIOS_Assert(pios_openlrs_validate(openlrs_dev));
1808 
1809  if (openlrs_dev->rf_mode == Transmit) {
1810  openlrs_dev->rf_mode = Transmitted;
1811  } else if (openlrs_dev->rf_mode == Receive) {
1812  openlrs_dev->rf_mode = Received;
1813  }
1814 
1815  // Indicate to main task that an ISR occurred
1816  bool woken = false;
1817  PIOS_Semaphore_Give_FromISR(openlrs_dev->sema_isr, &woken);
1818 
1819  return woken;
1820 }
1821 
1825 static pios_openlrs_t pios_openlrs_alloc(void)
1826 {
1827  pios_openlrs_t openlrs_dev;
1828 
1829  openlrs_dev = (pios_openlrs_t)PIOS_malloc(sizeof(*openlrs_dev));
1830 
1831  if (!openlrs_dev) {
1832  return NULL;
1833  }
1834 
1835  *openlrs_dev = (struct pios_openlrs_dev) {
1836  .magic = PIOS_OPENLRS_DEV_MAGIC,
1837  };
1838 
1839  // Create the ISR signal
1840  openlrs_dev->sema_isr = PIOS_Semaphore_Create();
1841  if (!openlrs_dev->sema_isr) {
1842  PIOS_free(openlrs_dev);
1843  return NULL;
1844  }
1845 
1846  return openlrs_dev;
1847 }
1848 
1854 static bool pios_openlrs_validate(pios_openlrs_t openlrs_dev)
1855 {
1856  return openlrs_dev != NULL
1857  && openlrs_dev->magic == PIOS_OPENLRS_DEV_MAGIC;
1858 }
1859 
1860 static void rfm22_set_channel(pios_openlrs_t openlrs_dev, uint8_t ch)
1861 {
1862  /* XXX enforce maximum frequency here */
1863  DEBUG_PRINTF(3,"rfm22_set_channel %d\r\n", ch);
1864  uint8_t magicLSB = (openlrs_dev->bind_data.rf_magic & 0xff) ^ ch;
1865  rfm22_claim_bus(openlrs_dev);
1866  rfm22_write(openlrs_dev, RFM22_frequency_hopping_channel_select, openlrs_dev->bind_data.hopchannel[ch]);
1867  rfm22_write(openlrs_dev, RFM22_transmit_header3 + 3, magicLSB);
1868  rfm22_write(openlrs_dev, RFM22_check_header3 + 3, magicLSB);
1869  rfm22_release_bus(openlrs_dev);
1870 }
1871 
1872 static uint8_t rfm22_get_rssi(pios_openlrs_t openlrs_dev)
1873 {
1874  return rfm22_read_claim(openlrs_dev, 0x26);
1875 }
1876 
1877 static void rfm22_set_modem_regs(pios_openlrs_t openlrs_dev, const struct rfm22_modem_regs *r)
1878 {
1879  DEBUG_PRINTF(3,"rfm22_set_modem_regs\r\n");
1880  rfm22_write(openlrs_dev, RFM22_if_filter_bandwidth, r->r_1c);
1881  rfm22_write(openlrs_dev, RFM22_afc_loop_gearshift_override, r->r_1d);
1882  rfm22_write(openlrs_dev, RFM22_afc_timing_control, r->r_1e);
1883  rfm22_write(openlrs_dev, RFM22_clk_recovery_oversampling_ratio, r->r_20);
1884  rfm22_write(openlrs_dev, RFM22_clk_recovery_offset2, r->r_21);
1885  rfm22_write(openlrs_dev, RFM22_clk_recovery_offset1, r->r_22);
1886  rfm22_write(openlrs_dev, RFM22_clk_recovery_offset0, r->r_23);
1887  rfm22_write(openlrs_dev, RFM22_clk_recovery_timing_loop_gain1, r->r_24);
1888  rfm22_write(openlrs_dev, RFM22_clk_recovery_timing_loop_gain0, r->r_25);
1889  rfm22_write(openlrs_dev, RFM22_afc_limiter, r->r_2a);
1890  rfm22_write(openlrs_dev, RFM22_tx_data_rate1, r->r_6e);
1891  rfm22_write(openlrs_dev, RFM22_tx_data_rate0, r->r_6f);
1892  rfm22_write(openlrs_dev, RFM22_modulation_mode_control1, r->r_70);
1893  rfm22_write(openlrs_dev, RFM22_modulation_mode_control2, r->r_71);
1894  rfm22_write(openlrs_dev, RFM22_frequency_deviation, r->r_72);
1895 }
1896 
1897 static void rfm22_beacon_tone(pios_openlrs_t openlrs_dev, int16_t hz, int16_t len) //duration is now in half seconds.
1898 {
1899  DEBUG_PRINTF(2,"beacon_tone: %d %d\r\n", hz, len*2);
1900  uint32_t d = 500000000 / hz; // time low or high per cycle, ns
1901 
1902  d = PIOS_INLINEDELAY_NsToCycles(d);
1903 
1904 #if defined(PIOS_LED_LINK)
1905  PIOS_ANNUNC_On(PIOS_LED_LINK);
1906 #endif /* PIOS_LED_LINK */
1907 
1908  rfm22_claim_bus(openlrs_dev);
1909 
1910  GPIO_TypeDef *gpio = openlrs_dev->cfg.spi_cfg->mosi.gpio;
1911  uint16_t pin_source = openlrs_dev->cfg.spi_cfg->mosi.init.GPIO_Pin;
1912 
1913  /* XXX Use SPI for this. */
1914  /* XXX this is not device independent */
1915 
1916 #if defined(STM32F10X_MD)
1917  GPIO_InitTypeDef init = {
1918  .GPIO_Speed = GPIO_Speed_50MHz,
1919  .GPIO_Mode = GPIO_Mode_Out_PP,
1920  .GPIO_Pin = pin_source,
1921  };
1922 #else
1923  GPIO_InitTypeDef init = {
1924  .GPIO_Speed = GPIO_Speed_50MHz,
1925  .GPIO_Mode = GPIO_Mode_OUT,
1926  .GPIO_OType = GPIO_OType_PP,
1927  .GPIO_PuPd = GPIO_PuPd_UP,
1928  .GPIO_Pin = pin_source,
1929  };
1930 
1931  // Set MOSI to digital out for bit banging
1932  GPIO_PinAFConfig(gpio, pin_source, 0);
1933 #endif
1934 
1935  GPIO_Init(gpio, &init);
1936 
1937  uint32_t cycles = (len * hz) / 5;
1938 
1939  uint32_t whence = PIOS_INLINEDELAY_GetCycleCnt();
1940 
1941  for (int16_t i = 0; i < cycles / 4; i++) {
1942  GPIO_SetBits(gpio, pin_source);
1943  whence += d; PIOS_INLINEDELAY_TillCycleCnt(whence);
1944  GPIO_ResetBits(gpio, pin_source);
1945  whence += d; PIOS_INLINEDELAY_TillCycleCnt(whence);
1946  GPIO_SetBits(gpio, pin_source);
1947  whence += d; PIOS_INLINEDELAY_TillCycleCnt(whence);
1948  GPIO_ResetBits(gpio, pin_source);
1949  whence += d; PIOS_INLINEDELAY_TillCycleCnt(whence);
1950  GPIO_SetBits(gpio, pin_source);
1951  whence += d; PIOS_INLINEDELAY_TillCycleCnt(whence);
1952  GPIO_ResetBits(gpio, pin_source);
1953  whence += d; PIOS_INLINEDELAY_TillCycleCnt(whence);
1954  GPIO_SetBits(gpio, pin_source);
1955  whence += d; PIOS_INLINEDELAY_TillCycleCnt(whence);
1956  GPIO_ResetBits(gpio, pin_source);
1957  whence += d; PIOS_INLINEDELAY_TillCycleCnt(whence);
1958  }
1959 
1960  GPIO_Init(gpio, (GPIO_InitTypeDef *) &openlrs_dev->cfg.spi_cfg->mosi.init);
1961 
1962 #if !defined(STM32F10X_MD)
1963  GPIO_PinAFConfig(gpio, pin_source, openlrs_dev->cfg.spi_cfg->remap);
1964 #endif
1965 
1966  rfm22_release_bus(openlrs_dev);
1967 
1968 #if defined(PIOS_LED_LINK)
1969  PIOS_ANNUNC_Off(PIOS_LED_LINK);
1970 #endif /* PIOS_LED_LINK */
1971 }
1972 
1973 static void rfm22_disable(pios_openlrs_t openlrs_dev)
1974 {
1975  rfm22_claim_bus(openlrs_dev);
1976 
1977  /* disable interrupts, turn off power */
1978  rfm22_write(openlrs_dev, RFM22_interrupt_enable2, 0x00);
1979  rfm22_write(openlrs_dev, RFM22_op_and_func_ctrl1, RF22B_PWRSTATE_POWERDOWN);
1980 
1981  rfm22_get_it_status(openlrs_dev);
1982 
1983  rfm22_release_bus(openlrs_dev);
1984 }
1985 
1986 static void rfm22_beacon_send(pios_openlrs_t openlrs_dev, bool static_tone)
1987 {
1988  DEBUG_PRINTF(2,"rfm22_beacon_send\r\n");
1989  rfm22_claim_bus(openlrs_dev);
1990  rfm22_get_it_status(openlrs_dev);
1991  rfm22_write(openlrs_dev, 0x06, 0x00); // no wakeup up, lbd,
1992  rfm22_write(openlrs_dev, 0x07, RF22B_PWRSTATE_READY); // disable lbd, wakeup timer, use internal 32768,xton = 1; in ready mode
1993  rfm22_write(openlrs_dev, 0x09, 0x7f); // (default) c = 12.5p
1994  rfm22_write(openlrs_dev, 0x0a, 0x05);
1995  rfm22_write(openlrs_dev, 0x0b, 0x12); // gpio0 TX State
1996  rfm22_write(openlrs_dev, 0x0c, 0x15); // gpio1 RX State
1997  rfm22_write(openlrs_dev, 0x0d, 0xfd); // gpio 2 micro-controller clk output
1998  rfm22_write(openlrs_dev, 0x0e, 0x00); // gpio 0, 1,2 NO OTHER FUNCTION.
1999 
2000  rfm22_write(openlrs_dev, 0x70, 0x2C); // disable manchest
2001 
2002  rfm22_write(openlrs_dev, 0x30, 0x00); //disable packet handling
2003 
2004  rfm22_write(openlrs_dev, 0x79, 0); // start channel
2005 
2006  rfm22_write(openlrs_dev, 0x7a, 0x05); // 50khz step size (10khz x value) // no hopping
2007 
2008  rfm22_write(openlrs_dev, 0x71, 0x12); // trclk=[00] no clock, dtmod=[01] direct using SPI, fd8=0 eninv=0 modtyp=[10] FSK
2009  rfm22_write(openlrs_dev, 0x72, 0x02); // fd (frequency deviation) 2*625Hz == 1.25kHz
2010 
2011  rfm22_write(openlrs_dev, 0x73, 0x00);
2012  rfm22_write(openlrs_dev, 0x74, 0x00); // no offset
2013  rfm22_release_bus(openlrs_dev);
2014 
2015  rfm22_set_frequency(openlrs_dev, openlrs_dev->beacon_frequency);
2016 
2017  rfm22_write_claim(openlrs_dev, 0x6d, 0x07); // 7 set max power 100mW
2018 
2019  /* XXX WATCHDOG */
2020  PIOS_Thread_Sleep(10);
2021  rfm22_write_claim(openlrs_dev, 0x07, RF22B_PWRSTATE_TX); // to tx mode
2022  PIOS_Thread_Sleep(500); /* Half a second of silence overcomes squelch */
2023 
2024  if (static_tone) {
2025  rfm22_beacon_tone(openlrs_dev, 440, 20);
2026  } else {
2027  rfm22_beacon_tone(openlrs_dev, 784, 1);
2028 
2029  rfm22_write(openlrs_dev, 0x6d, 0x05); // 5 set mid power 25mW
2030  PIOS_Thread_Sleep(80);
2031  rfm22_beacon_tone(openlrs_dev, 699, 1);
2032 
2033  rfm22_write(openlrs_dev, 0x6d, 0x04); // 4 set mid power 13mW
2034  PIOS_Thread_Sleep(80);
2035  rfm22_beacon_tone(openlrs_dev, 659, 1);
2036 
2037  rfm22_write(openlrs_dev, 0x6d, 0x02); // 2 set min power 3mW
2038  PIOS_Thread_Sleep(80);
2039  rfm22_beacon_tone(openlrs_dev, 659,1);
2040 
2041  rfm22_write(openlrs_dev, 0x6d, 0x00); // 0 set min power 1.3mW
2042  PIOS_Thread_Sleep(30);
2043  rfm22_beacon_tone(openlrs_dev, 699, 3);
2044  }
2045 
2046  rfm22_write_claim(openlrs_dev, 0x07, RF22B_PWRSTATE_READY);
2047 }
2048 
2049 static void rfm22_tx_packet(pios_openlrs_t openlrs_dev, void *pkt,
2050  uint8_t size)
2051 {
2052  if (openlrs_dev->max_power == HWSHARED_MAXRFPOWER_0) {
2053  /* Refuse to transmit in this case. */
2054  return;
2055  }
2056 
2057  openlrs_dev->rf_mode = Transmit;
2058 
2059  rfm22_claim_bus(openlrs_dev);
2060 
2061  /* Clear the fifo */
2062  rfm22_write(openlrs_dev, RFM22_op_and_func_ctrl2, 0x03);
2063  rfm22_write(openlrs_dev, RFM22_op_and_func_ctrl2, 0x00);
2064 
2065  rfm22_write(openlrs_dev, RFM22_interrupt_enable1, RFM22_ie1_enpksent);
2066 
2067  rfm22_get_it_status(openlrs_dev);
2068 
2069  rfm22_write(openlrs_dev, RFM22_transmit_packet_length, size); // total tx size
2070 
2071  rfm22_assert_cs(openlrs_dev);
2072  PIOS_SPI_TransferByte(openlrs_dev->spi_id, RFM22_fifo_access | 0x80);
2073  PIOS_SPI_TransferBlock(openlrs_dev->spi_id, pkt, NULL, size);
2074  rfm22_deassert_cs(openlrs_dev);
2075 
2076  /* Ensure sema taken */
2077  PIOS_Semaphore_Take(openlrs_dev->sema_isr, 0);
2078  /* Initiate TX */
2079  rfm22_write(openlrs_dev, RFM22_op_and_func_ctrl1, RF22B_PWRSTATE_TX);
2080 
2081  rfm22_release_bus(openlrs_dev);
2082 
2083  bool ok = wait_interrupt(openlrs_dev, 60000);
2084 
2085  rfm22_claim_bus(openlrs_dev);
2086  rfm22_get_it_status(openlrs_dev);
2087  rfm22_release_bus(openlrs_dev);
2088 
2089 #if defined(PIOS_INCLUDE_WDG) && defined(PIOS_WDG_RFM22B)
2090  // Update the watchdog timer
2091  PIOS_WDG_UpdateFlag(PIOS_WDG_RFM22B);
2092 #endif /* PIOS_WDG_RFM22B */
2093 
2094  if (!ok) {
2095  DEBUG_PRINTF(2,"OLRS ERR: rfm22_tx_packet timeout\r\n");
2096  rfm22_init(openlrs_dev, false); // reset modem
2097  }
2098 }
2099 
2100 static void rfm22_rx_packet_sizeknown(pios_openlrs_t openlrs_dev,
2101  void *whence, uint32_t size)
2102 {
2103  /* Assumes bus claimed */
2104 
2105  rfm22_assert_cs(openlrs_dev);
2106  PIOS_SPI_TransferByte(openlrs_dev->spi_id, RFM22_fifo_access);
2107  PIOS_SPI_TransferBlock(openlrs_dev->spi_id, NULL,
2108  whence, size);
2109  rfm22_deassert_cs(openlrs_dev);
2110 }
2111 
2112 static bool rfm22_rx_packet(pios_openlrs_t openlrs_dev,
2113  void *whence, uint32_t size)
2114 {
2115  rfm22_claim_bus(openlrs_dev);
2116  uint8_t len = rfm22_read(openlrs_dev, RFM22_received_packet_length);
2117 
2118  bool ret = false;
2119 
2120  if (len == size) {
2121  rfm22_rx_packet_sizeknown(openlrs_dev, whence, size);
2122 
2123  ret = true;
2124  }
2125 
2126  rfm22_release_bus(openlrs_dev);
2127 
2128  return ret;
2129 }
2130 
2131 static int rfm22_rx_packet_variable(pios_openlrs_t openlrs_dev,
2132  void *whence, uint32_t max_size)
2133 {
2134  rfm22_claim_bus(openlrs_dev);
2135  uint8_t size = rfm22_read(openlrs_dev, RFM22_received_packet_length);
2136 
2137  int ret = -1;
2138 
2139  if ((size <= max_size) && (size > 0)) {
2140  rfm22_rx_packet_sizeknown(openlrs_dev, whence, size);
2141 
2142  ret = size;
2143  }
2144 
2145  rfm22_release_bus(openlrs_dev);
2146 
2147  return ret;
2148 }
2149 
2150 
2151 static void rfm22_rx_reset(pios_openlrs_t openlrs_dev, bool pause_long)
2152 {
2153  openlrs_dev->rf_mode = Receive;
2154 
2155  DEBUG_PRINTF(3,"rfm22_rx_reset\r\n");
2156 
2157  if (pause_long) {
2158  rfm22_claim_bus(openlrs_dev);
2159  rfm22_write(openlrs_dev, RFM22_op_and_func_ctrl1, RF22B_PWRSTATE_READY);
2160  rfm22_get_it_status(openlrs_dev);
2161  rfm22_release_bus(openlrs_dev);
2162  PIOS_Thread_Sleep(20);
2163  }
2164 
2165  rfm22_claim_bus(openlrs_dev);
2166 
2167  /* Clear the fifo */
2168  rfm22_write(openlrs_dev, RFM22_op_and_func_ctrl2, 0x03);
2169  rfm22_write(openlrs_dev, RFM22_op_and_func_ctrl2, 0x00);
2170 
2171  rfm22_get_it_status(openlrs_dev);
2172 
2173  /* Ensure sema taken */
2174  PIOS_Semaphore_Take(openlrs_dev->sema_isr, 0);
2175 
2176  rfm22_write(openlrs_dev, RFM22_interrupt_enable1, RFM22_ie1_enpkvalid);
2177  rfm22_write(openlrs_dev, RFM22_op_and_func_ctrl1, RF22B_PWRSTATE_RX); // to rx mode
2178 
2179  rfm22_release_bus(openlrs_dev);
2180 }
2181 
2182 DONT_BUILD_IF(HWSHARED_MAXRFPOWER_MAXOPTVAL != 8, InvRfPowerOptions);
2183 DONT_BUILD_IF(HWSHARED_MAXRFPOWER_0 != 0, InvRfPowerOptions2);
2184 DONT_BUILD_IF(HWSHARED_MAXRFPOWER_125 != 1, InvRfPowerOptions3);
2185 DONT_BUILD_IF(HWSHARED_MAXRFPOWER_100 != 8, InvRfPowerOptions4);
2186 
2187 static void rfm22_init(pios_openlrs_t openlrs_dev, uint8_t isbind)
2188 {
2189  DEBUG_PRINTF(2,"rfm22_init %d\r\n", isbind);
2190 
2191  if (!isbind) {
2192  DEBUG_PRINTF(2, "Binding settings:\r\n");
2193  PIOS_Thread_Sleep(10);
2194  DEBUG_PRINTF(2, " version: %d\r\n", openlrs_dev->bind_data.version);
2195  PIOS_Thread_Sleep(10);
2196  DEBUG_PRINTF(2, " rf_frequency: %d\r\n", openlrs_dev->bind_data.rf_frequency);
2197  PIOS_Thread_Sleep(10);
2198  DEBUG_PRINTF(2, " rf_power: %d\r\n", openlrs_dev->bind_data.rf_power);
2199  PIOS_Thread_Sleep(10);
2200  DEBUG_PRINTF(2, " rf_channel_spacing: %d\r\n", openlrs_dev->bind_data.rf_channel_spacing);
2201  PIOS_Thread_Sleep(10);
2202  DEBUG_PRINTF(2, " modem_params: %d\r\n", openlrs_dev->bind_data.modem_params);
2203  PIOS_Thread_Sleep(10);
2204  DEBUG_PRINTF(2, " flags: %d\r\n", openlrs_dev->bind_data.flags);
2205  PIOS_Thread_Sleep(10);
2206  }
2207 
2208 
2209  rfm22_write_claim(openlrs_dev, RFM22_op_and_func_ctrl1, RFM22_opfc1_swres);
2210 
2211  PIOS_Thread_Sleep(40);
2212 
2213  rfm22_claim_bus(openlrs_dev);
2214  rfm22_write(openlrs_dev, RFM22_interrupt_enable2, 0x00); // disable interrupts
2215  rfm22_write(openlrs_dev, RFM22_op_and_func_ctrl1, RF22B_PWRSTATE_READY); // disable lbd, wakeup timer, use internal 32768,xton = 1; in ready mode
2216  rfm22_write(openlrs_dev, RFM22_xtal_osc_load_cap, 0x7f); // c = 12.5p
2217  rfm22_write(openlrs_dev, RFM22_cpu_output_clk, 0x05);
2218  switch (openlrs_dev->cfg.gpio_direction) {
2219  case GPIO0_TX_GPIO1_RX:
2220  rfm22_write(openlrs_dev, RFM22_gpio0_config, RFM22_gpio0_config_txstate); // gpio0 TX State
2221  rfm22_write(openlrs_dev, RFM22_gpio1_config, RFM22_gpio1_config_rxstate); // gpio1 RX State
2222  break;
2223  case GPIO0_RX_GPIO1_TX:
2224  rfm22_write(openlrs_dev, RFM22_gpio0_config, RFM22_gpio0_config_rxstate); // gpio0 RX State
2225  rfm22_write(openlrs_dev, RFM22_gpio1_config, RFM22_gpio1_config_txstate); // gpio1 TX State
2226  break;
2227  }
2228  rfm22_write(openlrs_dev, RFM22_gpio2_config, 0xfd); // gpio 2 VDD
2229  rfm22_write(openlrs_dev, RFM22_io_port_config, RFM22_io_port_default); // gpio 0, 1,2 NO OTHER FUNCTION.
2230 
2231  if (isbind) {
2232  rfm22_set_modem_regs(openlrs_dev, &modem_params[OPENLRS_BIND_PARAMS]);
2233  } else {
2234  rfm22_set_modem_regs(openlrs_dev, &modem_params[openlrs_dev->bind_data.modem_params]);
2235  }
2236 
2237  // Packet settings
2238  rfm22_write(openlrs_dev, RFM22_data_access_control, 0x8c); // enable packet handler, msb first, enable crc,
2239  rfm22_write(openlrs_dev, RFM22_header_control1, 0x0f); // no broadcast, check header bytes 3,2,1,0
2240  rfm22_write(openlrs_dev, RFM22_header_control2, 0x42); // 4 byte header, 2 byte synch, variable pkt size
2241  rfm22_write(openlrs_dev, RFM22_preamble_length, (openlrs_dev->bind_data.flags & DIVERSITY_ENABLED) ? 0x14 : 0x0a); // 40 bit preamble, 80 with diversity
2242  rfm22_write(openlrs_dev, RFM22_preamble_detection_ctrl1, 0x2a); // preath = 5 (20bits), rssioff = 2
2243  rfm22_write(openlrs_dev, RFM22_sync_word3, 0x2d); // synchronize word 3
2244  rfm22_write(openlrs_dev, RFM22_sync_word2, 0xd4); // synchronize word 2
2245  rfm22_write(openlrs_dev, RFM22_sync_word1, 0x00); // synch word 1 (not used)
2246  rfm22_write(openlrs_dev, RFM22_sync_word0, 0x00); // synch word 0 (not used)
2247 
2248  uint32_t magic = isbind ? BIND_MAGIC : openlrs_dev->bind_data.rf_magic;
2249  for (uint8_t i = 0; i < 4; i++) {
2250  rfm22_write(openlrs_dev, RFM22_transmit_header3 + i, (magic >> 24) & 0xff); // tx header
2251  rfm22_write(openlrs_dev, RFM22_check_header3 + i, (magic >> 24) & 0xff); // rx header
2252  magic = magic << 8; // advance to next byte
2253  }
2254 
2255  rfm22_write(openlrs_dev, RFM22_header_enable3, 0xff); // all the bit to be checked
2256  rfm22_write(openlrs_dev, RFM22_header_enable2, 0xff); // all the bit to be checked
2257  rfm22_write(openlrs_dev, RFM22_header_enable1, 0xff); // all the bit to be checked
2258  rfm22_write(openlrs_dev, RFM22_header_enable0, 0xff); // all the bit to be checked
2259 
2260  if (isbind) {
2261  /* These rely on:
2262  * A) OpenLRS not initing if maxpower = 0.
2263  * B) The DONT_BUILD_IFs above being true and the MaxRfPower
2264  * being in sync with module capabilities.
2265  */
2266  rfm22_write(openlrs_dev, RFM22_tx_power,
2267  MIN(openlrs_dev->max_power - 1, BINDING_POWER));
2268  } else {
2269  rfm22_write(openlrs_dev, RFM22_tx_power,
2270  MIN(openlrs_dev->max_power - 1,
2271  openlrs_dev->bind_data.rf_power));
2272  }
2273 
2274  rfm22_write(openlrs_dev, RFM22_frequency_hopping_step_size, openlrs_dev->bind_data.rf_channel_spacing); // channel spacing
2275 
2276  rfm22_write(openlrs_dev, RFM22_frequency_offset1, 0x00);
2277  rfm22_write(openlrs_dev, RFM22_frequency_offset2, 0x00); // no offset
2278 
2279  rfm22_release_bus(openlrs_dev);
2280 
2281  rfm22_rx_reset(openlrs_dev, true);
2282 
2283  rfm22_set_frequency(openlrs_dev,
2284  isbind ? binding_freq(openlrs_dev->band) :
2285  openlrs_dev->bind_data.rf_frequency);
2286 }
2287 
2288 static void rfm22_set_frequency(pios_openlrs_t openlrs_dev, uint32_t f)
2289 {
2290  /* Protect ourselves from out-of-band frequencies. Ideally we'd latch
2291  * an error here and prevent tx, but this is good enough to protect
2292  * the hardware. */
2293  if ((f < min_freq(openlrs_dev->band)) ||
2294  (f > max_freq(openlrs_dev->band))) {
2295  f = def_carrier_freq(openlrs_dev->band);
2296  }
2297 
2298  DEBUG_PRINTF(3,"rfm22_set_frequency %d\r\n", f);
2299  uint16_t fb, fc, hbsel;
2300  if (f < 480000000) {
2301  hbsel = 0;
2302  fb = f / 10000000 - 24;
2303  fc = (f - (fb + 24) * 10000000) * 4 / 625;
2304  } else {
2305  hbsel = 1;
2306  fb = f / 20000000 - 24;
2307  fc = (f - (fb + 24) * 20000000) * 2 / 625;
2308  }
2309  rfm22_claim_bus(openlrs_dev);
2310  rfm22_write(openlrs_dev, RFM22_frequency_band_select, RFM22_fbs_sbse + (hbsel ? RFM22_fbs_hbsel : 0) + (fb & RFM22_fb_mask));
2311  rfm22_write(openlrs_dev, RFM22_nominal_carrier_frequency1, (fc >> 8));
2312  rfm22_write(openlrs_dev, RFM22_nominal_carrier_frequency0, (fc & 0xff));
2313  rfm22_write(openlrs_dev, RFM22_frequency_hopping_channel_select, 0);
2314  rfm22_release_bus(openlrs_dev);
2315 }
2316 
2317 /* Must have claimed bus first */
2318 static void rfm22_get_it_status(pios_openlrs_t openlrs_dev)
2319 {
2320  openlrs_dev->it_status1 = rfm22_read(openlrs_dev, RFM22_interrupt_status1);
2321  openlrs_dev->it_status2 = rfm22_read(openlrs_dev, RFM22_interrupt_status2);
2322 }
2323 
2324 static void rfm22_check_hang(pios_openlrs_t openlrs_dev)
2325 {
2326  /* Hack-- detect a locked module and reset */
2327  if (rfm22_read_claim(openlrs_dev, 0x0C) == 0) {
2328  DEBUG_PRINTF(2,"OLRS ERR: RX hang\r\n");
2329  rfm22_init(openlrs_dev, false);
2330  }
2331 }
2332 
2333 /*****************************************************************************
2334 * SPI Read/Write Functions
2335 *****************************************************************************/
2336 
2342 static void rfm22_assert_cs(pios_openlrs_t openlrs_dev)
2343 {
2344  PIOS_DELAY_WaituS(1);
2345  PIOS_SPI_RC_PinSet(openlrs_dev->spi_id,
2346  openlrs_dev->slave_num, 0);
2347 }
2348 
2354 static void rfm22_deassert_cs(pios_openlrs_t openlrs_dev)
2355 {
2356  PIOS_SPI_RC_PinSet(openlrs_dev->spi_id,
2357  openlrs_dev->slave_num, 1);
2358 }
2359 
2365 static void rfm22_claim_bus(pios_openlrs_t openlrs_dev)
2366 {
2367  PIOS_SPI_ClaimBus(openlrs_dev->spi_id);
2368 }
2369 
2375 static void rfm22_release_bus(pios_openlrs_t openlrs_dev)
2376 {
2377  PIOS_SPI_ReleaseBus(openlrs_dev->spi_id);
2378 }
2379 
2387 static void rfm22_write(pios_openlrs_t openlrs_dev, uint8_t addr,
2388  uint8_t data)
2389 {
2390  rfm22_assert_cs(openlrs_dev);
2391  uint8_t buf[2] = { addr | 0x80, data };
2392  PIOS_SPI_TransferBlock(openlrs_dev->spi_id, buf, NULL, sizeof(buf));
2393  rfm22_deassert_cs(openlrs_dev);
2394 }
2395 
2403 static uint8_t rfm22_read(pios_openlrs_t openlrs_dev, uint8_t addr)
2404 {
2405  uint8_t out[2] = { addr & 0x7F, 0xFF };
2406  uint8_t in[2];
2407 
2408  rfm22_assert_cs(openlrs_dev);
2409  PIOS_SPI_TransferBlock(openlrs_dev->spi_id, out, in, sizeof(out));
2410  rfm22_deassert_cs(openlrs_dev);
2411 
2412  return in[1];
2413 }
2421 static void rfm22_write_claim(pios_openlrs_t openlrs_dev,
2422  uint8_t addr, uint8_t data)
2423 {
2424  rfm22_claim_bus(openlrs_dev);
2425  rfm22_write(openlrs_dev, addr, data);
2426  rfm22_release_bus(openlrs_dev);
2427 }
2428 
2436 static uint8_t rfm22_read_claim(pios_openlrs_t openlrs_dev, uint8_t addr)
2437 {
2438  uint8_t ret;
2439 
2440  rfm22_claim_bus(openlrs_dev);
2441  ret = rfm22_read(openlrs_dev, addr);
2442  rfm22_release_bus(openlrs_dev);
2443 
2444  return ret;
2445 }
2446 
2447 #endif /* PIOS_INCLUDE_OPENLRS */
2448 
pios_inlinedelay.h
start
Definition: common.h:35
PIOS_Thread_Systime
uint32_t PIOS_Thread_Systime(void)
Definition: pios_thread.c:212
PIOS_DELAY_GetuS
uint32_t PIOS_DELAY_GetuS()
Query the Delay timer for the current uS.
Definition: pios_delay.c:173
pios.h
Main PiOS header to include all the compiled in PiOS options.
RFM22_header_control1
#define RFM22_header_control1
Definition: pios_rfm22b_regs.h:377
pios_hal.h
config
struct usb_configuration_desc config
Definition: pios_usb_desc_hid_cdc.c:55
RFM22_header_enable2
#define RFM22_header_enable2
Definition: pios_rfm22b_regs.h:427
RFM22_data_access_control
#define RFM22_data_access_control
Definition: pios_rfm22b_regs.h:356
pios_com_driver::bind_tx_cb
void(* bind_tx_cb)(uintptr_t id, pios_com_callback tx_out_cb, uintptr_t context)
Definition: pios_com.h:48
pios_spi_dev
Definition: pios_spi_priv.h:38
RFM22_sync_word1
#define RFM22_sync_word1
Definition: pios_rfm22b_regs.h:411
pios_spi_priv.h
SPI private definitions.
PIOS_SPI_RC_PinSet
int32_t PIOS_SPI_RC_PinSet(pios_spi_t spi_dev, uint32_t slave_id, bool pin_value)
pios_com_priv.h
COM private definitions.
PIOS_EXTI_Init
int32_t PIOS_EXTI_Init(const struct pios_exti_cfg *cfg)
RFM22_afc_limiter
#define RFM22_afc_limiter
Definition: pios_rfm22b_regs.h:347
RFM22_clk_recovery_timing_loop_gain0
#define RFM22_clk_recovery_timing_loop_gain0
Definition: pios_rfm22b_regs.h:339
PIOS_SPI_ClaimBus
int32_t PIOS_SPI_ClaimBus(pios_spi_t spi_dev)
RFM22_cpu_output_clk
#define RFM22_cpu_output_clk
Definition: pios_rfm22b_regs.h:138
pios_com_debug_id
uintptr_t pios_com_debug_id
Definition: pios_board.c:76
PIOS_INLINEDELAY_NsToCycles
static uint32_t PIOS_INLINEDELAY_NsToCycles(uint32_t ns)
Definition: pios_inlinedelay.h:49
misc_math.h
PIOS_WDG_RegisterFlag
bool PIOS_WDG_RegisterFlag(uint16_t flag_requested)
Register a module against the watchdog.
Definition: pios_wdg.c:86
STACK_SIZE_BYTES
#define STACK_SIZE_BYTES
Definition: actuator.c:62
RFM22_sync_word2
#define RFM22_sync_word2
Definition: pios_rfm22b_regs.h:410
PIOS_DEBUG_Assert
#define PIOS_DEBUG_Assert(test)
Definition: pios_debug.h:51
PIOS_DELAY_GetuSExpired
uint32_t PIOS_DELAY_GetuSExpired(uint32_t t)
Calculates whether a given time has passed.
Definition: pios_delay.c:193
PIOS_WDG_UpdateFlag
bool PIOS_WDG_UpdateFlag(uint16_t flag)
Function called by modules to indicate they are still running.
Definition: pios_wdg.c:102
PIOS_malloc
void * PIOS_malloc(size_t size)
Definition: pios_heap.c:125
PIOS_Semaphore_Give_FromISR
bool PIOS_Semaphore_Give_FromISR(struct pios_semaphore *sema, bool *woken)
Definition: pios_semaphore.c:231
pios_com_driver
Definition: pios_com.h:43
rssi
uint16_t rssi
Definition: msp_messages.h:98
inited
static bool inited
Definition: main.c:47
RFM22_tx_data_rate0
#define RFM22_tx_data_rate0
Definition: pios_rfm22b_regs.h:462
UAVObjSave
int32_t UAVObjSave(UAVObjHandle obj_handle, uint16_t instId)
Definition: uavobjectmanager.c:739
PIOS_OpenLRS_Rcvr_UpdateChannels
int32_t PIOS_OpenLRS_Rcvr_UpdateChannels(uintptr_t openlrs_rcvr_id, int16_t *channels)
RFM22_gpio0_config_txstate
#define RFM22_gpio0_config_txstate
Definition: pios_rfm22b_regs.h:172
PIOS_DELAY_GetuSSince
uint32_t PIOS_DELAY_GetuSSince(uint32_t t)
Calculate time in microseconds since a previous time.
Definition: pios_delay.c:183
RFM22_interrupt_status1
#define RFM22_interrupt_status1
Definition: pios_rfm22b_regs.h:76
DEBUG_LEVEL
#define DEBUG_LEVEL
Definition: pios_board.h:72
PIOS_ANNUNC_Off
void PIOS_ANNUNC_Off(uint32_t annunc_id)
DEBUG_PRINTF
#define DEBUG_PRINTF(level,...)
Definition: pios_board.h:39
RFM22_fbs_hbsel
#define RFM22_fbs_hbsel
Definition: pios_rfm22b_regs.h:498
TASK_PRIORITY
#define TASK_PRIORITY
Definition: actuator.c:65
RFM22_frequency_deviation
#define RFM22_frequency_deviation
Definition: pios_rfm22b_regs.h:491
taskmonitor.h
Task monitoring library.
bytes
uint8_t bytes[2]
Definition: storm32bgc.c:156
RFM22_clk_recovery_offset1
#define RFM22_clk_recovery_offset1
Definition: pios_rfm22b_regs.h:336
RFM22_io_port_config
#define RFM22_io_port_config
Definition: pios_rfm22b_regs.h:264
RFM22_frequency_hopping_channel_select
#define RFM22_frequency_hopping_channel_select
Definition: pios_rfm22b_regs.h:504
RFM22_header_enable0
#define RFM22_header_enable0
Definition: pios_rfm22b_regs.h:429
RFM22_tx_data_rate1
#define RFM22_tx_data_rate1
Definition: pios_rfm22b_regs.h:461
RFM22_tx_power
#define RFM22_tx_power
Definition: pios_rfm22b_regs.h:458
data
uint8_t data[XFER_BYTES_PER_PACKET]
Definition: bl_messages.h:129
RFM22_check_header3
#define RFM22_check_header3
Definition: pios_rfm22b_regs.h:421
cfg
static struct flyingpicmd_cfg_fa cfg
Definition: main.c:49
PIOS_COM_TELEM_SER
#define PIOS_COM_TELEM_SER
Definition: pios_hal.h:58
RFM22_gpio0_config_rxstate
#define RFM22_gpio0_config_rxstate
Definition: pios_rfm22b_regs.h:175
RFM22_frequency_offset2
#define RFM22_frequency_offset2
Definition: pios_rfm22b_regs.h:494
RFM22_sync_word3
#define RFM22_sync_word3
Definition: pios_rfm22b_regs.h:409
RFM22_preamble_detection_ctrl1
#define RFM22_preamble_detection_ctrl1
Definition: pios_rfm22b_regs.h:405
RFM22_transmit_header3
#define RFM22_transmit_header3
Definition: pios_rfm22b_regs.h:414
RFM22_opfc1_swres
#define RFM22_opfc1_swres
Definition: pios_rfm22b_regs.h:124
PIOS_Semaphore_Create
struct pios_semaphore * PIOS_Semaphore_Create(void)
Creates a binary semaphore.
Definition: pios_semaphore.c:150
PIOS_SPI_TransferByte
uint8_t PIOS_SPI_TransferByte(pios_spi_t spi_dev, uint8_t b)
RFM22_gpio0_config
#define RFM22_gpio0_config
Definition: pios_rfm22b_regs.h:153
RFM22_header_enable3
#define RFM22_header_enable3
Definition: pios_rfm22b_regs.h:426
RFM22_interrupt_enable2
#define RFM22_interrupt_enable2
Definition: pios_rfm22b_regs.h:106
PIOS_SPI_TransferBlock
int32_t PIOS_SPI_TransferBlock(pios_spi_t spi_dev, const uint8_t *send_buffer, uint8_t *receive_buffer, uint16_t len)
RFM22_nominal_carrier_frequency1
#define RFM22_nominal_carrier_frequency1
Definition: pios_rfm22b_regs.h:501
PIOS_Thread_Create
struct pios_thread * PIOS_Thread_Create(void(*fp)(void *), const char *namep, size_t stack_bytes, void *argp, enum pios_thread_prio_e prio)
Definition: pios_thread.c:89
randomize_int
uint32_t randomize_int(uint32_t interval)
Definition: misc_math.c:340
RFM22_modulation_mode_control2
#define RFM22_modulation_mode_control2
Definition: pios_rfm22b_regs.h:472
PIOS_ANNUNC_On
void PIOS_ANNUNC_On(uint32_t annunc_id)
RFM22_fb_mask
#define RFM22_fb_mask
Definition: pios_rfm22b_regs.h:497
TaskMonitorAdd
int32_t TaskMonitorAdd(TaskInfoRunningElem task, struct pios_thread *threadp)
Definition: taskmonitor.c:67
i
uint8_t i
Definition: msp_messages.h:97
RFM22_gpio1_config_rxstate
#define RFM22_gpio1_config_rxstate
Definition: pios_rfm22b_regs.h:212
RFM22_frequency_hopping_step_size
#define RFM22_frequency_hopping_step_size
Definition: pios_rfm22b_regs.h:505
RFM22_clk_recovery_timing_loop_gain1
#define RFM22_clk_recovery_timing_loop_gain1
Definition: pios_rfm22b_regs.h:338
RFM22_clk_recovery_offset0
#define RFM22_clk_recovery_offset0
Definition: pios_rfm22b_regs.h:337
RFM22_xtal_osc_load_cap
#define RFM22_xtal_osc_load_cap
Definition: pios_rfm22b_regs.h:134
d
uint8_t d
Definition: msp_messages.h:98
RFM22_fbs_sbse
#define RFM22_fbs_sbse
Definition: pios_rfm22b_regs.h:499
RFM22_frequency_offset1
#define RFM22_frequency_offset1
Definition: pios_rfm22b_regs.h:493
RFM22_ie1_enpkvalid
#define RFM22_ie1_enpkvalid
Definition: pios_rfm22b_regs.h:98
magic
uint32_t magic
Definition: pios_flashfs_logfs.c:72
PIOS_COM_BRIDGE
#define PIOS_COM_BRIDGE
Definition: pios_board.h:115
PIOS_INLINEDELAY_TillCycleCnt
static void PIOS_INLINEDELAY_TillCycleCnt(uint32_t whence)
Definition: pios_inlinedelay.h:74
RFM22_fifo_access
#define RFM22_fifo_access
Definition: pios_rfm22b_regs.h:516
PIOS_free
void PIOS_free(void *buf)
Definition: pios_heap.c:174
PIOS_Thread_Sleep
void PIOS_Thread_Sleep(uint32_t time_ms)
Definition: pios_thread.c:229
RFM22_op_and_func_ctrl1
#define RFM22_op_and_func_ctrl1
Definition: pios_rfm22b_regs.h:116
PIOS_ANNUNC_Toggle
void PIOS_ANNUNC_Toggle(uint32_t annunc_id)
PIOS_RCVR_Read
int32_t PIOS_RCVR_Read(uintptr_t rcvr_id, uint8_t channel)
RFM22_frequency_band_select
#define RFM22_frequency_band_select
Definition: pios_rfm22b_regs.h:496
PIOS_HAL_GetReceiver
uintptr_t PIOS_HAL_GetReceiver(int receiver_type)
PIOS_WDG_RFM22B
#define PIOS_WDG_RFM22B
Definition: pios_board.h:81
RFM22_if_filter_bandwidth
#define RFM22_if_filter_bandwidth
Definition: pios_rfm22b_regs.h:320
px_mkfw.f
tuple f
Definition: px_mkfw.py:81
RFM22_interrupt_enable1
#define RFM22_interrupt_enable1
Definition: pios_rfm22b_regs.h:96
PIOS_INLINEDELAY_GetCycleCnt
static uint32_t PIOS_INLINEDELAY_GetCycleCnt()
Definition: pios_inlinedelay.h:68
PIOS_COM_Init
int32_t PIOS_COM_Init(uintptr_t *com_id, const struct pios_com_driver *driver, uintptr_t lower_id, uint16_t rx_buffer_len, uint16_t tx_buffer_len)
MIN
#define MIN(a, b)
Definition: misc_math.h:41
RFM22_preamble_length
#define RFM22_preamble_length
Definition: pios_rfm22b_regs.h:403
RFM22_clk_recovery_oversampling_ratio
#define RFM22_clk_recovery_oversampling_ratio
Definition: pios_rfm22b_regs.h:334
PIOS_Semaphore_Take
bool PIOS_Semaphore_Take(struct pios_semaphore *sema, uint32_t timeout_ms)
Takes binary semaphore.
Definition: pios_semaphore.c:175
pios_rfm22b_regs.h
PIOS_COM_RADIOBRIDGE
#define PIOS_COM_RADIOBRIDGE
Definition: pios_board.h:168
DONT_BUILD_IF
#define DONT_BUILD_IF(COND, MSG)
Definition: morsel.c:206
RFM22_ie1_enpksent
#define RFM22_ie1_enpksent
Definition: pios_rfm22b_regs.h:99
RFM22_DT_MASK
#define RFM22_DT_MASK
Definition: pios_rfm22b_regs.h:59
pios_openlrs_rcvr_priv.h
RFM22_op_and_func_ctrl2
#define RFM22_op_and_func_ctrl2
Definition: pios_rfm22b_regs.h:126
RFM22_gpio1_config_txstate
#define RFM22_gpio1_config_txstate
Definition: pios_rfm22b_regs.h:209
RFM22_modulation_mode_control1
#define RFM22_modulation_mode_control1
Definition: pios_rfm22b_regs.h:464
PIOS_SPI_ReleaseBus
int32_t PIOS_SPI_ReleaseBus(pios_spi_t spi_dev)
system_annunc_custom_string
void system_annunc_custom_string(const char *string)
Definition: systemmod.c:417
RFM22_header_enable1
#define RFM22_header_enable1
Definition: pios_rfm22b_regs.h:428
RFM22_DEVICE_TYPE
#define RFM22_DEVICE_TYPE
Definition: pios_rfm22b_regs.h:58
RFM22_interrupt_status2
#define RFM22_interrupt_status2
Definition: pios_rfm22b_regs.h:86
RFM22_afc_timing_control
#define RFM22_afc_timing_control
Definition: pios_rfm22b_regs.h:331
PIOS_LED_LINK
#define PIOS_LED_LINK
Definition: pios_board.h:88
RFM22_header_control2
#define RFM22_header_control2
Definition: pios_rfm22b_regs.h:389
RFM22_gpio2_config
#define RFM22_gpio2_config
Definition: pios_rfm22b_regs.h:227
RFM22_clk_recovery_offset2
#define RFM22_clk_recovery_offset2
Definition: pios_rfm22b_regs.h:335
RFM22_transmit_packet_length
#define RFM22_transmit_packet_length
Definition: pios_rfm22b_regs.h:419
if
if(BaroAltitudeHandle()!=NULL)
Definition: UAVOFrSKYSensorHubBridge.c:363
p
uint8_t p
Definition: msp_messages.h:96
RFM22_sync_word0
#define RFM22_sync_word0
Definition: pios_rfm22b_regs.h:412
RFM22_nominal_carrier_frequency0
#define RFM22_nominal_carrier_frequency0
Definition: pios_rfm22b_regs.h:502
PIOS_Assert
#define PIOS_Assert(test)
Definition: pios_debug.h:52
RFM22_gpio1_config
#define RFM22_gpio1_config
Definition: pios_rfm22b_regs.h:190
RFM22_io_port_default
#define RFM22_io_port_default
Definition: pios_rfm22b_regs.h:272
RFM22_afc_loop_gearshift_override
#define RFM22_afc_loop_gearshift_override
Definition: pios_rfm22b_regs.h:325
PIOS_DELAY_WaituS
int32_t PIOS_DELAY_WaituS(uint32_t uS)
Definition: pios_delay.c:116
pios_com_callback
uint16_t(* pios_com_callback)(uintptr_t context, uint8_t *buf, uint16_t buf_len, uint16_t *headroom, bool *task_woken)
Definition: pios_com.h:41
pios_thread.h
pios_openlrs_priv.h
Implements an OpenLRS driver for the RFM22B.
RFM22_received_packet_length
#define RFM22_received_packet_length
Definition: pios_rfm22b_regs.h:436