?  search:
summary | log | graphiclog1 | graphiclog2 | commit | commitdiff | tree | refs | edit | fork
blame | history | raw | HEAD
Merged in f5soh/librepilot/laurent/LP-92_Feed_forward_remove (pull request #33)
[librepilot.git] / flight / pios / common / pios_rfm22b.c
blob35fe19ce13b5b0b567f1dea0b9d5ff44041daa8a
1 /**
2 ******************************************************************************
3 * @addtogroup PIOS PIOS Core hardware abstraction layer
4 * @{
5 * @addtogroup PIOS_RFM22B Radio Functions
6 * @brief PIOS interface for for the RFM22B radio
7 * @{
8 *
9 * @file pios_rfm22b.c
10 * @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2012.
11 * @brief Implements a driver the the RFM22B driver
12 * @see The GNU Public License (GPL) Version 3
13 *
14 *****************************************************************************/
15 /*
16 * This program is free software; you can redistribute it and/or modify
17 * it under the terms of the GNU General Public License as published by
18 * the Free Software Foundation; either version 3 of the License, or
19 * (at your option) any later version.
20 *
21 * This program is distributed in the hope that it will be useful, but
22 * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY
23 * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License
24 * for more details.
25 *
26 * You should have received a copy of the GNU General Public License along
27 * with this program; if not, write to the Free Software Foundation, Inc.,
28 * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA
29 */
30
31 // *****************************************************************
32 // RFM22B hardware layer
33 //
34 // This module uses the RFM22B's internal packet handling hardware to
35 // encapsulate our own packet data.
36 //
37 // The RFM22B internal hardware packet handler configuration is as follows:
38 //
39 // 6-byte (32-bit) preamble .. alternating 0's & 1's
40 // 4-byte (32-bit) sync
41 // 1-byte packet length (number of data bytes to follow)
42 // 0 to 255 user data bytes
43 // 4 byte ECC
44 //
45 // OR in PPM only mode:
46 //
47 // 6-byte (32-bit) preamble .. alternating 0's & 1's
48 // 4-byte (32-bit) sync
49 // 1-byte packet length (number of data bytes to follow)
50 // 1 byte PPM values LSB (bit 0)
51 // 8 bytes PPM values MSBs (bit 8:1)
52 // 1 byte CRC
53 //
54 // *****************************************************************
55
56 #include "pios.h"
57
58 #ifdef PIOS_INCLUDE_RFM22B
59
60 #include <pios_spi_priv.h>
61 #include <pios_rfm22b_priv.h>
62 #include <pios_ppm_out.h>
63 #include <ecc.h>
64 #include <sha1.h>
65
66 /* Local Defines */
67 #define STACK_SIZE_BYTES 200
68 #define TASK_PRIORITY (tskIDLE_PRIORITY + 4) // flight control relevant device driver (ppm link)
69 #define ISR_TIMEOUT 1 // ms
70 #define EVENT_QUEUE_SIZE 5
71 #define RFM22B_DEFAULT_RX_DATARATE RFM22_datarate_9600
72 #define RFM22B_DEFAULT_TX_POWER RFM22_tx_pwr_txpow_0
73 #define RFM22B_NOMINAL_CARRIER_FREQUENCY 430000000
74 #define RFM22B_LINK_QUALITY_THRESHOLD 20
75 #define RFM22B_DEFAULT_MIN_CHANNEL 0
76 #define RFM22B_DEFAULT_MAX_CHANNEL 250
77 #define RFM22B_PPM_ONLY_DATARATE RFM22_datarate_9600
78
79 // PPM encoding limits
80 #define RFM22B_PPM_MIN 1
81 #define RFM22B_PPM_MAX 511
82 #define RFM22B_PPM_INVALID 0
83 #define RFM22B_PPM_SCALE 2
84 #define RFM22B_PPM_MIN_US 990
85 #define RFM22B_PPM_MAX_US (RFM22B_PPM_MIN_US + (RFM22B_PPM_MAX - RFM22B_PPM_MIN) * RFM22B_PPM_SCALE)
86
87 // The maximum amount of time without activity before initiating a reset.
88 #define PIOS_RFM22B_SUPERVISOR_TIMEOUT 150 // ms
89
90 // this is too adjust the RF module so that it is on frequency
91 #define OSC_LOAD_CAP 0x7F // cap = 12.5pf .. default
92
93 #define TX_PREAMBLE_NIBBLES 12 // 7 to 511 (number of nibbles)
94 #define RX_PREAMBLE_NIBBLES 6 // 5 to 31 (number of nibbles)
95 #define SYNC_BYTES 4
96 #define HEADER_BYTES 4
97 #define LENGTH_BYTES 1
98
99 // the size of the rf modules internal FIFO buffers
100 #define FIFO_SIZE 64
101
102 #define TX_FIFO_HI_WATERMARK 62 // 0-63
103 #define TX_FIFO_LO_WATERMARK 32 // 0-63
104
105 #define RX_FIFO_HI_WATERMARK 32 // 0-63
106
107 // preamble byte (preceeds SYNC_BYTE's)
108 #define PREAMBLE_BYTE 0x55
109
110 // RF sync bytes (32-bit in all)
111 #define SYNC_BYTE_1 0x2D
112 #define SYNC_BYTE_2 0xD4
113 #define SYNC_BYTE_3 0x4B
114 #define SYNC_BYTE_4 0x59
115
116 #ifndef RX_LED_ON
117 #define RX_LED_ON
118 #define RX_LED_OFF
119 #define TX_LED_ON
120 #define TX_LED_OFF
121 #define LINK_LED_ON
122 #define LINK_LED_OFF
123 #define USB_LED_ON
124 #define USB_LED_OFF
125 #endif
126
127 #define CONNECTED_TIMEOUT (250 / portTICK_RATE_MS) /* ms */
128 #define MAX_CHANNELS 32
129
130 /* Local type definitions */
131
132 struct pios_rfm22b_transition {
133 enum pios_radio_event (*entry_fn)(struct pios_rfm22b_dev *rfm22b_dev);
134 enum pios_radio_state next_state[RADIO_EVENT_NUM_EVENTS];
135 };
136
137 // Must ensure these prefilled arrays match the define sizes
138 static const uint8_t FULL_PREAMBLE[FIFO_SIZE] = {
139 PREAMBLE_BYTE, PREAMBLE_BYTE, PREAMBLE_BYTE, PREAMBLE_BYTE, PREAMBLE_BYTE,
140 PREAMBLE_BYTE, PREAMBLE_BYTE, PREAMBLE_BYTE, PREAMBLE_BYTE, PREAMBLE_BYTE,
141 PREAMBLE_BYTE, PREAMBLE_BYTE, PREAMBLE_BYTE, PREAMBLE_BYTE, PREAMBLE_BYTE,
142 PREAMBLE_BYTE, PREAMBLE_BYTE, PREAMBLE_BYTE, PREAMBLE_BYTE, PREAMBLE_BYTE,
143 PREAMBLE_BYTE, PREAMBLE_BYTE, PREAMBLE_BYTE, PREAMBLE_BYTE, PREAMBLE_BYTE,
144 PREAMBLE_BYTE, PREAMBLE_BYTE, PREAMBLE_BYTE, PREAMBLE_BYTE, PREAMBLE_BYTE,
145 PREAMBLE_BYTE, PREAMBLE_BYTE, PREAMBLE_BYTE, PREAMBLE_BYTE, PREAMBLE_BYTE,
146 PREAMBLE_BYTE, PREAMBLE_BYTE, PREAMBLE_BYTE, PREAMBLE_BYTE, PREAMBLE_BYTE,
147 PREAMBLE_BYTE, PREAMBLE_BYTE, PREAMBLE_BYTE, PREAMBLE_BYTE, PREAMBLE_BYTE,
148 PREAMBLE_BYTE, PREAMBLE_BYTE, PREAMBLE_BYTE, PREAMBLE_BYTE, PREAMBLE_BYTE,
149 PREAMBLE_BYTE, PREAMBLE_BYTE, PREAMBLE_BYTE, PREAMBLE_BYTE, PREAMBLE_BYTE,
150 PREAMBLE_BYTE, PREAMBLE_BYTE, PREAMBLE_BYTE, PREAMBLE_BYTE, PREAMBLE_BYTE,
151 PREAMBLE_BYTE, PREAMBLE_BYTE, PREAMBLE_BYTE, PREAMBLE_BYTE
152 }; // 64 bytes
153
154 static const uint8_t HEADER[(TX_PREAMBLE_NIBBLES + 1) / 2 + 2] = {
155 PREAMBLE_BYTE, PREAMBLE_BYTE, PREAMBLE_BYTE, PREAMBLE_BYTE, PREAMBLE_BYTE, PREAMBLE_BYTE, SYNC_BYTE_1, SYNC_BYTE_2
156 };
157
158 static const uint8_t OUT_FF[64] = {
159 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF,
160 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF,
161 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF,
162 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF,
163 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF,
164 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF,
165 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF,
166 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF
167 };
168
169 /* Local function forwared declarations */
170 static void pios_rfm22_task(void *parameters);
171 static bool pios_rfm22_readStatus(struct pios_rfm22b_dev *rfm22b_dev);
172 static void pios_rfm22_setDatarate(struct pios_rfm22b_dev *rfm22b_dev);
173 static void rfm22_rxFailure(struct pios_rfm22b_dev *rfm22b_dev);
174 static void pios_rfm22_inject_event(struct pios_rfm22b_dev *rfm22b_dev, enum pios_radio_event event, bool inISR);
175 static enum pios_radio_event rfm22_init(struct pios_rfm22b_dev *rfm22b_dev);
176 static enum pios_radio_event radio_setRxMode(struct pios_rfm22b_dev *rfm22b_dev);
177 static enum pios_radio_event radio_rxData(struct pios_rfm22b_dev *rfm22b_dev);
178 static enum pios_radio_event radio_receivePacket(struct pios_rfm22b_dev *rfm22b_dev, uint8_t *p, uint16_t rx_len);
179 static enum pios_radio_event radio_txStart(struct pios_rfm22b_dev *rfm22b_dev);
180 static enum pios_radio_event radio_txData(struct pios_rfm22b_dev *rfm22b_dev);
181 static enum pios_radio_event rfm22_txFailure(struct pios_rfm22b_dev *rfm22b_dev);
182 static enum pios_radio_event rfm22_process_state_transition(struct pios_rfm22b_dev *rfm22b_dev, enum pios_radio_event event);
183 static void rfm22_process_event(struct pios_rfm22b_dev *rfm22b_dev, enum pios_radio_event event);
184 static enum pios_radio_event rfm22_timeout(struct pios_rfm22b_dev *rfm22b_dev);
185 static enum pios_radio_event rfm22_error(struct pios_rfm22b_dev *rfm22b_dev);
186 static enum pios_radio_event rfm22_fatal_error(struct pios_rfm22b_dev *rfm22b_dev);
187 static void rfm22b_add_rx_status(struct pios_rfm22b_dev *rfm22b_dev, enum pios_rfm22b_rx_packet_status status);
188 static void rfm22_setNominalCarrierFrequency(struct pios_rfm22b_dev *rfm22b_dev, uint8_t init_chan);
189 static bool rfm22_setFreqHopChannel(struct pios_rfm22b_dev *rfm22b_dev, uint8_t channel);
190 static void rfm22_updatePairStatus(struct pios_rfm22b_dev *radio_dev);
191 static void rfm22_calculateLinkQuality(struct pios_rfm22b_dev *rfm22b_dev);
192 static bool rfm22_isConnected(struct pios_rfm22b_dev *rfm22b_dev);
193 static bool rfm22_isCoordinator(struct pios_rfm22b_dev *rfm22b_dev);
194 static uint32_t rfm22_destinationID(struct pios_rfm22b_dev *rfm22b_dev);
195 static bool rfm22_timeToSend(struct pios_rfm22b_dev *rfm22b_dev);
196 static void rfm22_synchronizeClock(struct pios_rfm22b_dev *rfm22b_dev);
197 static portTickType rfm22_coordinatorTime(struct pios_rfm22b_dev *rfm22b_dev, portTickType ticks);
198 static uint8_t rfm22_calcChannel(struct pios_rfm22b_dev *rfm22b_dev, uint8_t index);
199 static uint8_t rfm22_calcChannelFromClock(struct pios_rfm22b_dev *rfm22b_dev);
200 static bool rfm22_changeChannel(struct pios_rfm22b_dev *rfm22b_dev);
201 static void rfm22_clearLEDs();
202
203 // Utility functions.
204 static uint32_t pios_rfm22_time_difference_ms(portTickType start_time, portTickType end_time);
205 static struct pios_rfm22b_dev *pios_rfm22_alloc(void);
206 static void rfm22_hmac_sha1(const uint8_t *data, size_t len, uint8_t key[SHA1_DIGEST_LENGTH],
207 uint8_t digest[SHA1_DIGEST_LENGTH]);
208 static bool rfm22_gen_channels(uint32_t coordid, enum rfm22b_datarate datarate, uint8_t min,
209 uint8_t max, uint8_t channels[MAX_CHANNELS], uint8_t *clen);
210
211 // SPI read/write functions
212 static void rfm22_assertCs(struct pios_rfm22b_dev *rfm22b_dev);
213 static void rfm22_deassertCs(struct pios_rfm22b_dev *rfm22b_dev);
214 static void rfm22_claimBus(struct pios_rfm22b_dev *rfm22b_dev);
215 static void rfm22_releaseBus(struct pios_rfm22b_dev *rfm22b_dev);
216 static void rfm22_write_claim(struct pios_rfm22b_dev *rfm22b_dev, uint8_t addr, uint8_t data);
217 static void rfm22_write(struct pios_rfm22b_dev *rfm22b_dev, uint8_t addr, uint8_t data);
218 static uint8_t rfm22_read(struct pios_rfm22b_dev *rfm22b_dev, uint8_t addr);
219
220
221 /* The state transition table */
222 static const struct pios_rfm22b_transition rfm22b_transitions[RADIO_STATE_NUM_STATES] = {
223 // Initialization thread
224 [RADIO_STATE_UNINITIALIZED] = {
225 .entry_fn = 0,
226 .next_state = {
227 [RADIO_EVENT_INITIALIZE] = RADIO_STATE_INITIALIZING,
228 [RADIO_EVENT_ERROR] = RADIO_STATE_ERROR,
229 },
230 },
231 [RADIO_STATE_INITIALIZING] = {
232 .entry_fn = rfm22_init,
233 .next_state = {
234 [RADIO_EVENT_INITIALIZED] = RADIO_STATE_RX_MODE,
235 [RADIO_EVENT_ERROR] = RADIO_STATE_ERROR,
236 [RADIO_EVENT_INITIALIZE] = RADIO_STATE_INITIALIZING,
237 [RADIO_EVENT_FATAL_ERROR] = RADIO_STATE_FATAL_ERROR,
238 },
239 },
240
241 [RADIO_STATE_RX_MODE] = {
242 .entry_fn = radio_setRxMode,
243 .next_state = {
244 [RADIO_EVENT_INT_RECEIVED] = RADIO_STATE_RX_DATA,
245 [RADIO_EVENT_TX_START] = RADIO_STATE_TX_START,
246 [RADIO_EVENT_RX_MODE] = RADIO_STATE_RX_MODE,
247 [RADIO_EVENT_TIMEOUT] = RADIO_STATE_TIMEOUT,
248 [RADIO_EVENT_ERROR] = RADIO_STATE_ERROR,
249 [RADIO_EVENT_INITIALIZE] = RADIO_STATE_INITIALIZING,
250 [RADIO_EVENT_FATAL_ERROR] = RADIO_STATE_FATAL_ERROR,
251 },
252 },
253 [RADIO_STATE_RX_DATA] = {
254 .entry_fn = radio_rxData,
255 .next_state = {
256 [RADIO_EVENT_INT_RECEIVED] = RADIO_STATE_RX_DATA,
257 [RADIO_EVENT_TX_START] = RADIO_STATE_TX_START,
258 [RADIO_EVENT_RX_COMPLETE] = RADIO_STATE_TX_START,
259 [RADIO_EVENT_RX_MODE] = RADIO_STATE_RX_MODE,
260 [RADIO_EVENT_TIMEOUT] = RADIO_STATE_TIMEOUT,
261 [RADIO_EVENT_ERROR] = RADIO_STATE_ERROR,
262 [RADIO_EVENT_INITIALIZE] = RADIO_STATE_INITIALIZING,
263 [RADIO_EVENT_FATAL_ERROR] = RADIO_STATE_FATAL_ERROR,
264 },
265 },
266 [RADIO_STATE_TX_START] = {
267 .entry_fn = radio_txStart,
268 .next_state = {
269 [RADIO_EVENT_INT_RECEIVED] = RADIO_STATE_TX_DATA,
270 [RADIO_EVENT_RX_MODE] = RADIO_STATE_RX_MODE,
271 [RADIO_EVENT_TIMEOUT] = RADIO_STATE_TIMEOUT,
272 [RADIO_EVENT_ERROR] = RADIO_STATE_ERROR,
273 [RADIO_EVENT_INITIALIZE] = RADIO_STATE_INITIALIZING,
274 [RADIO_EVENT_FATAL_ERROR] = RADIO_STATE_FATAL_ERROR,
275 },
276 },
277 [RADIO_STATE_TX_DATA] = {
278 .entry_fn = radio_txData,
279 .next_state = {
280 [RADIO_EVENT_INT_RECEIVED] = RADIO_STATE_TX_DATA,
281 [RADIO_EVENT_RX_MODE] = RADIO_STATE_RX_MODE,
282 [RADIO_EVENT_TIMEOUT] = RADIO_STATE_TIMEOUT,
283 [RADIO_EVENT_ERROR] = RADIO_STATE_ERROR,
284 [RADIO_EVENT_INITIALIZE] = RADIO_STATE_INITIALIZING,
285 [RADIO_EVENT_FATAL_ERROR] = RADIO_STATE_FATAL_ERROR,
286 },
287 },
288 [RADIO_STATE_TX_FAILURE] = {
289 .entry_fn = rfm22_txFailure,
290 .next_state = {
291 [RADIO_EVENT_TX_START] = RADIO_STATE_TX_START,
292 [RADIO_EVENT_TIMEOUT] = RADIO_STATE_TIMEOUT,
293 [RADIO_EVENT_ERROR] = RADIO_STATE_ERROR,
294 [RADIO_EVENT_INITIALIZE] = RADIO_STATE_INITIALIZING,
295 [RADIO_EVENT_FATAL_ERROR] = RADIO_STATE_FATAL_ERROR,
296 },
297 },
298 [RADIO_STATE_TIMEOUT] = {
299 .entry_fn = rfm22_timeout,
300 .next_state = {
301 [RADIO_EVENT_TX_START] = RADIO_STATE_TX_START,
302 [RADIO_EVENT_RX_MODE] = RADIO_STATE_RX_MODE,
303 [RADIO_EVENT_ERROR] = RADIO_STATE_ERROR,
304 [RADIO_EVENT_INITIALIZE] = RADIO_STATE_INITIALIZING,
305 [RADIO_EVENT_FATAL_ERROR] = RADIO_STATE_FATAL_ERROR,
306 },
307 },
308 [RADIO_STATE_ERROR] = {
309 .entry_fn = rfm22_error,
310 .next_state = {
311 [RADIO_EVENT_INITIALIZE] = RADIO_STATE_INITIALIZING,
312 [RADIO_EVENT_FATAL_ERROR] = RADIO_STATE_FATAL_ERROR,
313 },
314 },
315 [RADIO_STATE_FATAL_ERROR] = {
316 .entry_fn = rfm22_fatal_error,
317 .next_state = {},
318 },
319 };
320
321 // xtal 10 ppm, 434MHz
322 static const uint32_t data_rate[] = {
323 9600, // 96 kbps, 433 HMz, 30 khz freq dev
324 19200, // 19.2 kbps, 433 MHz, 45 khz freq dev
325 32000, // 32 kbps, 433 MHz, 45 khz freq dev
326 57600, // 57.6 kbps, 433 MHz, 45 khz freq dev
327 64000, // 64 kbps, 433 MHz, 45 khz freq dev
328 100000, // 100 kbps, 433 MHz, 60 khz freq dev
329 128000, // 128 kbps, 433 MHz, 90 khz freq dev
330 192000, // 192 kbps, 433 MHz, 128 khz freq dev
331 256000, // 256 kbps, 433 MHz, 150 khz freq dev
332 };
333
334 static const uint8_t channel_spacing[] = {
335 1, /* 9.6kbps */
336 2, /* 19.2kps */
337 2, /* 32kps */
338 2, /* 57.6kps */
339 2, /* 64kps */
340 3, /* 100kps */
341 4, /* 128kps */
342 4, /* 192kps */
343 4 /* 256kps */
344 };
345
346 static const uint8_t reg_1C[] = { 0x01, 0x05, 0x06, 0x95, 0x95, 0x81, 0x88, 0x8B, 0x8D }; // rfm22_if_filter_bandwidth
347
348 static const uint8_t reg_1D[] = { 0x40, 0x40, 0x40, 0x40, 0x40, 0x40, 0x40, 0x40, 0x40 }; // rfm22_afc_loop_gearshift_override
349 static const uint8_t reg_1E[] = { 0x0A, 0x0A, 0x0A, 0x0A, 0x0A, 0x0A, 0x0A, 0x0A, 0x02 }; // rfm22_afc_timing_control
350
351 static const uint8_t reg_1F[] = { 0x03, 0x03, 0x03, 0x03, 0x03, 0x03, 0x03, 0x03, 0x03 }; // rfm22_clk_recovery_gearshift_override
352 static const uint8_t reg_20[] = { 0xA1, 0xD0, 0x7D, 0x68, 0x5E, 0x78, 0x5E, 0x3F, 0x2F }; // rfm22_clk_recovery_oversampling_ratio
353 static const uint8_t reg_21[] = { 0x20, 0x00, 0x01, 0x01, 0x01, 0x01, 0x01, 0x02, 0x02 }; // rfm22_clk_recovery_offset2
354 static const uint8_t reg_22[] = { 0x4E, 0x9D, 0x06, 0x3A, 0x5D, 0x11, 0x5D, 0x0C, 0xBB }; // rfm22_clk_recovery_offset1
355 static const uint8_t reg_23[] = { 0xA5, 0x49, 0x25, 0x93, 0x86, 0x11, 0x86, 0x4A, 0x0D }; // rfm22_clk_recovery_offset0
356 static const uint8_t reg_24[] = { 0x00, 0x00, 0x01, 0x03, 0x03, 0x03, 0x03, 0x06, 0x07 }; // rfm22_clk_recovery_timing_loop_gain1
357 static const uint8_t reg_25[] = { 0x34, 0x88, 0x77, 0x29, 0xE2, 0x90, 0xE2, 0x1A, 0xFF }; // rfm22_clk_recovery_timing_loop_gain0
358
359 static const uint8_t reg_2A[] = { 0x1E, 0x24, 0x28, 0x3C, 0x3C, 0x50, 0x50, 0x50, 0x50 }; // rfm22_afc_limiter .. AFC_pull_in_range = �AFCLimiter[7:0] x (hbsel+1) x 625 Hz
360
361 static const uint8_t reg_58[] = { 0x80, 0x80, 0x80, 0x80, 0x80, 0xC0, 0xC0, 0xC0, 0xED }; // rfm22_cpcuu
362 static const uint8_t reg_69[] = { 0x60, 0x60, 0x60, 0x60, 0x60, 0x60, 0x60, 0x60, 0x60 }; // rfm22_agc_override1
363 static const uint8_t reg_6E[] = { 0x4E, 0x9D, 0x08, 0x0E, 0x10, 0x19, 0x20, 0x31, 0x41 }; // rfm22_tx_data_rate1
364 static const uint8_t reg_6F[] = { 0xA5, 0x49, 0x31, 0xBF, 0x62, 0x9A, 0xC5, 0x27, 0x89 }; // rfm22_tx_data_rate0
365
366 static const uint8_t reg_70[] = { 0x2C, 0x2C, 0x0C, 0x0C, 0x0C, 0x0C, 0x0C, 0x0C, 0x0C }; // rfm22_modulation_mode_control1
367 static const uint8_t reg_71[] = { 0x23, 0x23, 0x23, 0x23, 0x23, 0x23, 0x23, 0x23, 0x23 }; // rfm22_modulation_mode_control2
368
369 static const uint8_t reg_72[] = { 0x30, 0x48, 0x48, 0x48, 0x48, 0x60, 0x90, 0xCD, 0x0F }; // rfm22_frequency_deviation
370
371 static const uint8_t packet_time[] = { 80, 40, 25, 15, 13, 10, 8, 6, 5 };
372 static const uint8_t packet_time_ppm[] = { 26, 25, 25, 15, 13, 10, 8, 6, 5 };
373 static const uint8_t num_channels[] = { 32, 32, 32, 32, 32, 32, 32, 32, 32 };
374
375 static struct pios_rfm22b_dev *g_rfm22b_dev = NULL;
376
377
378 /*****************************************************************************
379 * External Interface Functions
380 *****************************************************************************/
381
382 /**
383 * Initialise an RFM22B device
384 *
385 * @param[out] rfm22b_id A pointer to store the device ID in.
386 * @param[in] spi_id The SPI bus index.
387 * @param[in] slave_num The SPI bus slave number.
388 * @param[in] cfg The device configuration.
389 */
390 int32_t PIOS_RFM22B_Init(uint32_t *rfm22b_id, uint32_t spi_id, uint32_t slave_num, const struct pios_rfm22b_cfg *cfg)
391 {
392 PIOS_DEBUG_Assert(rfm22b_id);
393 PIOS_DEBUG_Assert(cfg);
394
395 // Allocate the device structure.
396 struct pios_rfm22b_dev *rfm22b_dev = (struct pios_rfm22b_dev *)pios_rfm22_alloc();
397 if (!rfm22b_dev) {
398 return -1;
399 }
400 *rfm22b_id = (uint32_t)rfm22b_dev;
401 g_rfm22b_dev = rfm22b_dev;
402
403 // Store the SPI handle
404 rfm22b_dev->slave_num = slave_num;
405 rfm22b_dev->spi_id = spi_id;
406
407 // Initialize our configuration parameters
408 rfm22b_dev->datarate = RFM22B_DEFAULT_RX_DATARATE;
409 rfm22b_dev->tx_power = RFM22B_DEFAULT_TX_POWER;
410 rfm22b_dev->coordinator = false;
411 rfm22b_dev->coordinatorID = 0;
412
413 // Initialize the com callbacks.
414 rfm22b_dev->rx_in_cb = NULL;
415 rfm22b_dev->tx_out_cb = NULL;
416
417 // Initialzie the PPM callback.
418 rfm22b_dev->ppm_callback = NULL;
419
420 // Initialize the stats.
421 rfm22b_dev->stats.packets_per_sec = 0;
422 rfm22b_dev->stats.rx_good = 0;
423 rfm22b_dev->stats.rx_corrected = 0;
424 rfm22b_dev->stats.rx_error = 0;
425 rfm22b_dev->stats.rx_missed = 0;
426 rfm22b_dev->stats.tx_dropped = 0;
427 rfm22b_dev->stats.resets = 0;
428 rfm22b_dev->stats.timeouts = 0;
429 rfm22b_dev->stats.link_quality = 0;
430 rfm22b_dev->stats.rssi = 0;
431 rfm22b_dev->stats.tx_seq = 0;
432 rfm22b_dev->stats.rx_seq = 0;
433 rfm22b_dev->stats.tx_failure = 0;
434
435 // Initialize the channels.
436 PIOS_RFM22B_SetChannelConfig(*rfm22b_id, RFM22B_DEFAULT_RX_DATARATE, RFM22B_DEFAULT_MIN_CHANNEL,
437 RFM22B_DEFAULT_MAX_CHANNEL, false, false, false, false);
438
439 // Create the event queue
440 rfm22b_dev->eventQueue = xQueueCreate(EVENT_QUEUE_SIZE, sizeof(enum pios_radio_event));
441
442 // Bind the configuration to the device instance
443 rfm22b_dev->cfg = *cfg;
444
445 // Create a semaphore to know if an ISR needs responding to
446 vSemaphoreCreateBinary(rfm22b_dev->isrPending);
447
448 // Create our (hopefully) unique 32 bit id from the processor serial number.
449 uint8_t crcs[] = { 0, 0, 0, 0 };
450 {
451 char serial_no_str[33];
452 PIOS_SYS_SerialNumberGet(serial_no_str);
453 // Create a 32 bit value using 4 8 bit CRC values.
454 for (uint8_t i = 0; serial_no_str[i] != 0; ++i) {
455 crcs[i % 4] = PIOS_CRC_updateByte(crcs[i % 4], serial_no_str[i]);
456 }
457 }
458 rfm22b_dev->deviceID = crcs[0] | crcs[1] << 8 | crcs[2] << 16 | crcs[3] << 24;
459 DEBUG_PRINTF(2, "RF device ID: %x\n\r", rfm22b_dev->deviceID);
460
461 // Initialize the external interrupt.
462 PIOS_EXTI_Init(cfg->exti_cfg);
463
464 // Register the watchdog timer for the radio driver task
465 #if defined(PIOS_INCLUDE_WDG) && defined(PIOS_WDG_RFM22B)
466 PIOS_WDG_RegisterFlag(PIOS_WDG_RFM22B);
467 #endif /* PIOS_WDG_RFM22B */
468
469 // Initialize the ECC library.
470 initialize_ecc();
471
472 // Set the state to initializing.
473 rfm22b_dev->state = RADIO_STATE_UNINITIALIZED;
474
475 // Initialize the radio device.
476 pios_rfm22_inject_event(rfm22b_dev, RADIO_EVENT_INITIALIZE, false);
477
478 // Start the driver task. This task controls the radio state machine and removed all of the IO from the IRQ handler.
479 xTaskCreate(pios_rfm22_task, "PIOS_RFM22B_Task", STACK_SIZE_BYTES, (void *)rfm22b_dev, TASK_PRIORITY, &(rfm22b_dev->taskHandle));
480
481 return 0;
482 }
483
484 /**
485 * Re-initialize the modem after a configuration change.
486 *
487 * @param[in] rbm22b_id The RFM22B device ID.
488 */
489 void PIOS_RFM22B_Reinit(uint32_t rfm22b_id)
490 {
491 struct pios_rfm22b_dev *rfm22b_dev = (struct pios_rfm22b_dev *)rfm22b_id;
492
493 if (PIOS_RFM22B_Validate(rfm22b_dev)) {
494 pios_rfm22_inject_event(rfm22b_dev, RADIO_EVENT_INITIALIZE, false);
495 }
496 }
497
498 /**
499 * The RFM22B external interrupt routine.
500 */
501 bool PIOS_RFM22_EXT_Int(void)
502 {
503 if (!PIOS_RFM22B_Validate(g_rfm22b_dev)) {
504 return false;
505 }
506
507 // Inject an interrupt event into the state machine.
508 pios_rfm22_inject_event(g_rfm22b_dev, RADIO_EVENT_INT_RECEIVED, true);
509 return false;
510 }
511
512 /**
513 * Returns the unique device ID for the RFM22B device.
514 *
515 * @param[in] rfm22b_id The RFM22B device index.
516 * @return The unique device ID
517 */
518 uint32_t PIOS_RFM22B_DeviceID(uint32_t rfm22b_id)
519 {
520 struct pios_rfm22b_dev *rfm22b_dev = (struct pios_rfm22b_dev *)rfm22b_id;
521
522 if (PIOS_RFM22B_Validate(rfm22b_dev)) {
523 return rfm22b_dev->deviceID;
524 }
525 return 0;
526 }
527
528 /**
529 * Are we connected to the remote modem?
530 *
531 * @param[in] rfm22b_dev The device structure
532 */
533 static bool rfm22_isConnected(struct pios_rfm22b_dev *rfm22b_dev)
534 {
535 return (rfm22b_dev->stats.link_state == OPLINKSTATUS_LINKSTATE_CONNECTED) || (rfm22b_dev->stats.link_state == OPLINKSTATUS_LINKSTATE_CONNECTING);
536 }
537
538 /**
539 * Returns true if the modem is not actively sending or receiving a packet.
540 *
541 * @param[in] rfm22b_id The RFM22B device index.
542 * @return True if the modem is not actively sending or receiving a packet.
543 */
544 bool PIOS_RFM22B_InRxWait(uint32_t rfm22b_id)
545 {
546 struct pios_rfm22b_dev *rfm22b_dev = (struct pios_rfm22b_dev *)rfm22b_id;
547
548 if (PIOS_RFM22B_Validate(rfm22b_dev)) {
549 return (rfm22b_dev->rfm22b_state == RFM22B_STATE_RX_WAIT) || (rfm22b_dev->rfm22b_state == RFM22B_STATE_TRANSITION);
550 }
551 return false;
552 }
553
554 /**
555 * Sets the radio device transmit power.
556 *
557 * @param[in] rfm22b_id The RFM22B device index.
558 * @param[in] tx_pwr The transmit power.
559 */
560 void PIOS_RFM22B_SetTxPower(uint32_t rfm22b_id, enum rfm22b_tx_power tx_pwr)
561 {
562 struct pios_rfm22b_dev *rfm22b_dev = (struct pios_rfm22b_dev *)rfm22b_id;
563
564 if (!PIOS_RFM22B_Validate(rfm22b_dev)) {
565 return;
566 }
567 rfm22b_dev->tx_power = tx_pwr;
568 }
569
570 /**
571 * Sets the range and number of channels to use for the radio.
572 * The channels are 0 to 250 divided across the 430-440 MHz range.
573 * The number of channels configured will be spread across the selected channel range.
574 * The channel spacing is 10MHz / 250 = 40kHz
575 *
576 * @param[in] rfm22b_id The RFM22B device index.
577 * @param[in] datarate The desired datarate.
578 * @param[in] min_chan The minimum channel.
579 * @param[in] max_chan The maximum channel.
580 * @param[in] coordinator Is this modem an coordinator.
581 * @param[in] ppm_mode Should this modem send/receive ppm packets?
582 * @param[in] oneway Only the coordinator can send packets if true.
583 * @param[in] ppm_only Should this modem run in ppm only mode?
584 */
585 void PIOS_RFM22B_SetChannelConfig(uint32_t rfm22b_id, enum rfm22b_datarate datarate, uint8_t min_chan, uint8_t max_chan, bool coordinator, bool oneway, bool ppm_mode, bool ppm_only)
586 {
587 struct pios_rfm22b_dev *rfm22b_dev = (struct pios_rfm22b_dev *)rfm22b_id;
588
589 if (!PIOS_RFM22B_Validate(rfm22b_dev)) {
590 return;
591 }
592 ppm_mode = ppm_mode || ppm_only;
593 rfm22b_dev->coordinator = coordinator;
594 rfm22b_dev->ppm_send_mode = ppm_mode && coordinator;
595 rfm22b_dev->ppm_recv_mode = ppm_mode && !coordinator;
596 if (ppm_mode && (datarate <= RFM22B_PPM_ONLY_DATARATE)) {
597 ppm_only = true;
598 }
599 rfm22b_dev->ppm_only_mode = ppm_only;
600 if (ppm_only) {
601 rfm22b_dev->one_way_link = true;
602 datarate = RFM22B_PPM_ONLY_DATARATE;
603 rfm22b_dev->datarate = RFM22B_PPM_ONLY_DATARATE;
604 } else {
605 rfm22b_dev->one_way_link = oneway;
606 rfm22b_dev->datarate = datarate;
607 }
608 rfm22b_dev->packet_time = (ppm_mode ? packet_time_ppm[datarate] : packet_time[datarate]);
609
610 uint8_t num_found = 0;
611 rfm22_gen_channels(rfm22_destinationID(rfm22b_dev), datarate, min_chan, max_chan,
612 rfm22b_dev->channels, &num_found);
613
614 rfm22b_dev->num_channels = num_found;
615
616 // Calculate the maximum packet length from the datarate.
617 float bytes_per_period = (float)data_rate[datarate] * (float)(rfm22b_dev->packet_time - 2) / 9000;
618
619 rfm22b_dev->max_packet_len = bytes_per_period - TX_PREAMBLE_NIBBLES / 2 - SYNC_BYTES - HEADER_BYTES - LENGTH_BYTES;
620 if (rfm22b_dev->max_packet_len > RFM22B_MAX_PACKET_LEN) {
621 rfm22b_dev->max_packet_len = RFM22B_MAX_PACKET_LEN;
622 }
623 }
624
625 /**
626 * Set a modem to be a coordinator or not.
627 *
628 * @param[in] rfm22b_id The RFM22B device index.
629 * @param[in] coordinator If true, this modem will be configured as a coordinator.
630 */
631 extern void PIOS_RFM22B_SetCoordinator(uint32_t rfm22b_id, bool coordinator)
632 {
633 struct pios_rfm22b_dev *rfm22b_dev = (struct pios_rfm22b_dev *)rfm22b_id;
634
635 if (PIOS_RFM22B_Validate(rfm22b_dev)) {
636 rfm22b_dev->coordinator = coordinator;
637 }
638 }
639
640 /**
641 * Sets the device coordinator ID.
642 *
643 * @param[in] rfm22b_id The RFM22B device index.
644 * @param[in] coord_id The coordinator ID.
645 */
646 void PIOS_RFM22B_SetCoordinatorID(uint32_t rfm22b_id, uint32_t coord_id)
647 {
648 struct pios_rfm22b_dev *rfm22b_dev = (struct pios_rfm22b_dev *)rfm22b_id;
649
650 if (PIOS_RFM22B_Validate(rfm22b_dev)) {
651 rfm22b_dev->coordinatorID = coord_id;
652 }
653 }
654
655 /**
656 * Returns the device statistics RFM22B device.
657 *
658 * @param[in] rfm22b_id The RFM22B device index.
659 * @param[out] stats The stats are returned in this structure
660 */
661 void PIOS_RFM22B_GetStats(uint32_t rfm22b_id, struct rfm22b_stats *stats)
662 {
663 struct pios_rfm22b_dev *rfm22b_dev = (struct pios_rfm22b_dev *)rfm22b_id;
664
665 if (!PIOS_RFM22B_Validate(rfm22b_dev)) {
666 return;
667 }
668
669 // Calculate the current link quality
670 rfm22_calculateLinkQuality(rfm22b_dev);
671
672 // Return the stats.
673 *stats = rfm22b_dev->stats;
674 }
675
676 /**
677 * Get the stats of the oter radio devices that are in range.
678 *
679 * @param[out] device_ids A pointer to the array to store the device IDs.
680 * @param[out] RSSIs A pointer to the array to store the RSSI values in.
681 * @param[in] mx_pairs The length of the pdevice_ids and RSSIs arrays.
682 * @return The number of pair stats returned.
683 */
684 uint8_t PIOS_RFM2B_GetPairStats(uint32_t rfm22b_id, uint32_t *device_ids, int8_t *RSSIs, uint8_t max_pairs)
685 {
686 struct pios_rfm22b_dev *rfm22b_dev = (struct pios_rfm22b_dev *)rfm22b_id;
687
688 if (!PIOS_RFM22B_Validate(rfm22b_dev)) {
689 return 0;
690 }
691
692 uint8_t mp = (max_pairs >= OPLINKSTATUS_PAIRIDS_NUMELEM) ? max_pairs : OPLINKSTATUS_PAIRIDS_NUMELEM;
693 for (uint8_t i = 0; i < mp; ++i) {
694 device_ids[i] = rfm22b_dev->pair_stats[i].pairID;
695 RSSIs[i] = rfm22b_dev->pair_stats[i].rssi;
696 }
697
698 return mp;
699 }
700
701 /**
702 * Check the radio device for a valid connection
703 *
704 * @param[in] rfm22b_id The rfm22b device.
705 * @return true if there is a valid connection to paired radio, false otherwise.
706 */
707 bool PIOS_RFM22B_LinkStatus(uint32_t rfm22b_id)
708 {
709 struct pios_rfm22b_dev *rfm22b_dev = (struct pios_rfm22b_dev *)rfm22b_id;
710
711 if (!PIOS_RFM22B_Validate(rfm22b_dev)) {
712 return false;
713 }
714 return rfm22b_dev->stats.link_quality > RFM22B_LINK_QUALITY_THRESHOLD;
715 }
716
717 /**
718 * Put the RFM22B device into receive mode.
719 *
720 * @param[in] rfm22b_id The rfm22b device.
721 * @param[in] p The packet to receive into.
722 * @return true if Rx mode was entered sucessfully.
723 */
724 bool PIOS_RFM22B_ReceivePacket(uint32_t rfm22b_id, uint8_t *p)
725 {
726 struct pios_rfm22b_dev *rfm22b_dev = (struct pios_rfm22b_dev *)rfm22b_id;
727
728 if (!PIOS_RFM22B_Validate(rfm22b_dev)) {
729 return false;
730 }
731 rfm22b_dev->rx_packet_handle = p;
732
733 // Claim the SPI bus.
734 rfm22_claimBus(rfm22b_dev);
735
736 // disable interrupts
737 rfm22_write(rfm22b_dev, RFM22_interrupt_enable1, 0x00);
738 rfm22_write(rfm22b_dev, RFM22_interrupt_enable2, 0x00);
739
740 // Switch to TUNE mode
741 rfm22_write(rfm22b_dev, RFM22_op_and_func_ctrl1, RFM22_opfc1_pllon);
742
743 #ifdef PIOS_RFM22B_DEBUG_ON_TELEM
744 D2_LED_OFF;
745 #endif // PIOS_RFM22B_DEBUG_ON_TELEM
746 RX_LED_OFF;
747 TX_LED_OFF;
748
749 // empty the rx buffer
750 rfm22b_dev->rx_buffer_wr = 0;
751
752 // Clear the TX buffer.
753 rfm22b_dev->tx_data_rd = rfm22b_dev->tx_data_wr = 0;
754
755 // clear FIFOs
756 rfm22_write(rfm22b_dev, RFM22_op_and_func_ctrl2, RFM22_opfc2_ffclrrx | RFM22_opfc2_ffclrtx);
757 rfm22_write(rfm22b_dev, RFM22_op_and_func_ctrl2, 0x00);
758
759 // enable RX interrupts
760 rfm22_write(rfm22b_dev, RFM22_interrupt_enable1, RFM22_ie1_encrcerror | RFM22_ie1_enpkvalid |
761 RFM22_ie1_enrxffafull | RFM22_ie1_enfferr);
762 rfm22_write(rfm22b_dev, RFM22_interrupt_enable2, RFM22_ie2_enpreaval | RFM22_ie2_enswdet);
763
764 // enable the receiver
765 rfm22_write(rfm22b_dev, RFM22_op_and_func_ctrl1, RFM22_opfc1_pllon | RFM22_opfc1_rxon);
766
767 // Release the SPI bus.
768 rfm22_releaseBus(rfm22b_dev);
769
770 // Indicate that we're in RX wait mode.
771 rfm22b_dev->rfm22b_state = RFM22B_STATE_RX_WAIT;
772
773 return true;
774 }
775
776 /**
777 * Transmit a packet via the RFM22B device.
778 *
779 * @param[in] rfm22b_id The rfm22b device.
780 * @param[in] p The packet to transmit.
781 * @return true if there if the packet was queued for transmission.
782 */
783 bool PIOS_RFM22B_TransmitPacket(uint32_t rfm22b_id, uint8_t *p, uint8_t len)
784 {
785 struct pios_rfm22b_dev *rfm22b_dev = (struct pios_rfm22b_dev *)rfm22b_id;
786
787 if (!PIOS_RFM22B_Validate(rfm22b_dev)) {
788 return false;
789 }
790
791 rfm22b_dev->tx_packet_handle = p;
792 rfm22b_dev->stats.tx_byte_count += len;
793 rfm22b_dev->packet_start_ticks = xTaskGetTickCount();
794 if (rfm22b_dev->packet_start_ticks == 0) {
795 rfm22b_dev->packet_start_ticks = 1;
796 }
797
798 // Claim the SPI bus.
799 rfm22_claimBus(rfm22b_dev);
800
801 // Disable interrupts
802 rfm22_write(rfm22b_dev, RFM22_interrupt_enable1, 0x00);
803 rfm22_write(rfm22b_dev, RFM22_interrupt_enable2, 0x00);
804
805 // set the tx power
806 rfm22_write(rfm22b_dev, RFM22_tx_power, RFM22_tx_pwr_lna_sw | rfm22b_dev->tx_power);
807
808 // TUNE mode
809 rfm22_write(rfm22b_dev, RFM22_op_and_func_ctrl1, RFM22_opfc1_pllon);
810
811 // Queue the data up for sending
812 rfm22b_dev->tx_data_wr = len;
813
814 RX_LED_OFF;
815
816 // Set the destination address in the transmit header.
817 uint32_t id = rfm22_destinationID(rfm22b_dev);
818 rfm22_write(rfm22b_dev, RFM22_transmit_header0, id & 0xff);
819 rfm22_write(rfm22b_dev, RFM22_transmit_header1, (id >> 8) & 0xff);
820 rfm22_write(rfm22b_dev, RFM22_transmit_header2, (id >> 16) & 0xff);
821 rfm22_write(rfm22b_dev, RFM22_transmit_header3, (id >> 24) & 0xff);
822
823 // FIFO mode, GFSK modulation
824 uint8_t fd_bit = rfm22_read(rfm22b_dev, RFM22_modulation_mode_control2) & RFM22_mmc2_fd;
825 rfm22_write(rfm22b_dev, RFM22_modulation_mode_control2, fd_bit | RFM22_mmc2_dtmod_fifo | RFM22_mmc2_modtyp_gfsk);
826
827 // Clear the FIFOs.
828 rfm22_write(rfm22b_dev, RFM22_op_and_func_ctrl2, RFM22_opfc2_ffclrrx | RFM22_opfc2_ffclrtx);
829 rfm22_write(rfm22b_dev, RFM22_op_and_func_ctrl2, 0x00);
830
831 // Set the total number of data bytes we are going to transmit.
832 rfm22_write(rfm22b_dev, RFM22_transmit_packet_length, len);
833
834 // Add some data to the chips TX FIFO before enabling the transmitter
835 uint8_t *tx_buffer = rfm22b_dev->tx_packet_handle;
836 rfm22_assertCs(rfm22b_dev);
837 PIOS_SPI_TransferByte(rfm22b_dev->spi_id, RFM22_fifo_access | 0x80);
838 int bytes_to_write = (rfm22b_dev->tx_data_wr - rfm22b_dev->tx_data_rd);
839 bytes_to_write = (bytes_to_write > FIFO_SIZE) ? FIFO_SIZE : bytes_to_write;
840 PIOS_SPI_TransferBlock(rfm22b_dev->spi_id, &tx_buffer[rfm22b_dev->tx_data_rd], NULL, bytes_to_write, NULL);
841 rfm22b_dev->tx_data_rd += bytes_to_write;
842 rfm22_deassertCs(rfm22b_dev);
843
844 // Enable TX interrupts.
845 rfm22_write(rfm22b_dev, RFM22_interrupt_enable1, RFM22_ie1_enpksent | RFM22_ie1_entxffaem);
846
847 // Enable the transmitter.
848 rfm22_write(rfm22b_dev, RFM22_op_and_func_ctrl1, RFM22_opfc1_pllon | RFM22_opfc1_txon);
849
850 // Release the SPI bus.
851 rfm22_releaseBus(rfm22b_dev);
852
853 // We're in Tx mode.
854 rfm22b_dev->rfm22b_state = RFM22B_STATE_TX_MODE;
855
856 TX_LED_ON;
857
858 #ifdef PIOS_RFM22B_DEBUG_ON_TELEM
859 D1_LED_ON;
860 #endif
861
862 return true;
863 }
864
865 /**
866 * Process a Tx interrupt from the RFM22B device.
867 *
868 * @param[in] rfm22b_id The rfm22b device.
869 * @return PIOS_RFM22B_TX_COMPLETE on completed Tx, or PIOS_RFM22B_INT_SUCCESS/PIOS_RFM22B_INT_FAILURE.
870 */
871 pios_rfm22b_int_result PIOS_RFM22B_ProcessTx(uint32_t rfm22b_id)
872 {
873 struct pios_rfm22b_dev *rfm22b_dev = (struct pios_rfm22b_dev *)rfm22b_id;
874
875 if (!PIOS_RFM22B_Validate(rfm22b_dev)) {
876 return PIOS_RFM22B_INT_FAILURE;
877 }
878
879 // Read the device status registers
880 if (!pios_rfm22_readStatus(rfm22b_dev)) {
881 return PIOS_RFM22B_INT_FAILURE;
882 }
883
884 // TX FIFO almost empty, it needs filling up
885 if (rfm22b_dev->status_regs.int_status_1.tx_fifo_almost_empty) {
886 // Add data to the TX FIFO buffer
887 uint8_t *tx_buffer = rfm22b_dev->tx_packet_handle;
888 uint16_t max_bytes = FIFO_SIZE - TX_FIFO_LO_WATERMARK - 1;
889 rfm22_claimBus(rfm22b_dev);
890 rfm22_assertCs(rfm22b_dev);
891 PIOS_SPI_TransferByte(rfm22b_dev->spi_id, RFM22_fifo_access | 0x80);
892 int bytes_to_write = (rfm22b_dev->tx_data_wr - rfm22b_dev->tx_data_rd);
893 bytes_to_write = (bytes_to_write > max_bytes) ? max_bytes : bytes_to_write;
894 PIOS_SPI_TransferBlock(rfm22b_dev->spi_id, &tx_buffer[rfm22b_dev->tx_data_rd], NULL, bytes_to_write, NULL);
895 rfm22b_dev->tx_data_rd += bytes_to_write;
896 rfm22_deassertCs(rfm22b_dev);
897 rfm22_releaseBus(rfm22b_dev);
898
899 return PIOS_RFM22B_INT_SUCCESS;
900 } else if (rfm22b_dev->status_regs.int_status_1.packet_sent_interrupt) {
901 // Transition out of Tx mode.
902 rfm22b_dev->rfm22b_state = RFM22B_STATE_TRANSITION;
903 return PIOS_RFM22B_TX_COMPLETE;
904 }
905
906 return 0;
907 }
908
909 /**
910 * Process a Rx interrupt from the RFM22B device.
911 *
912 * @param[in] rfm22b_id The rfm22b device.
913 * @return PIOS_RFM22B_RX_COMPLETE on completed Rx, or PIOS_RFM22B_INT_SUCCESS/PIOS_RFM22B_INT_FAILURE.
914 */
915 pios_rfm22b_int_result PIOS_RFM22B_ProcessRx(uint32_t rfm22b_id)
916 {
917 struct pios_rfm22b_dev *rfm22b_dev = (struct pios_rfm22b_dev *)rfm22b_id;
918
919 if (!PIOS_RFM22B_Validate(rfm22b_dev)) {
920 return PIOS_RFM22B_INT_FAILURE;
921 }
922 uint8_t *rx_buffer = rfm22b_dev->rx_packet_handle;
923 pios_rfm22b_int_result ret = PIOS_RFM22B_INT_SUCCESS;
924
925 // Read the device status registers
926 if (!pios_rfm22_readStatus(rfm22b_dev)) {
927 rfm22_rxFailure(rfm22b_dev);
928 return PIOS_RFM22B_INT_FAILURE;
929 }
930
931 // FIFO under/over flow error. Restart RX mode.
932 if (rfm22b_dev->status_regs.int_status_1.fifo_underoverflow_error ||
933 rfm22b_dev->status_regs.int_status_1.crc_error) {
934 rfm22_rxFailure(rfm22b_dev);
935 return PIOS_RFM22B_INT_FAILURE;
936 }
937
938 // Valid packet received
939 if (rfm22b_dev->status_regs.int_status_1.valid_packet_received) {
940 // Claim the SPI bus.
941 rfm22_claimBus(rfm22b_dev);
942
943 // read the total length of the packet data
944 uint32_t len = rfm22_read(rfm22b_dev, RFM22_received_packet_length);
945
946 // The received packet is going to be larger than the receive buffer
947 if (len > rfm22b_dev->max_packet_len) {
948 rfm22_releaseBus(rfm22b_dev);
949 rfm22_rxFailure(rfm22b_dev);
950 return PIOS_RFM22B_INT_FAILURE;
951 }
952
953 // there must still be data in the RX FIFO we need to get
954 if (rfm22b_dev->rx_buffer_wr < len) {
955 int32_t bytes_to_read = len - rfm22b_dev->rx_buffer_wr;
956 // Fetch the data from the RX FIFO
957 rfm22_assertCs(rfm22b_dev);
958 PIOS_SPI_TransferByte(rfm22b_dev->spi_id, RFM22_fifo_access & 0x7F);
959 rfm22b_dev->rx_buffer_wr += (PIOS_SPI_TransferBlock(rfm22b_dev->spi_id, OUT_FF, (uint8_t *)&rx_buffer[rfm22b_dev->rx_buffer_wr],
960 bytes_to_read, NULL) == 0) ? bytes_to_read : 0;
961 rfm22_deassertCs(rfm22b_dev);
962 }
963
964 // Read the packet header (destination ID)
965 rfm22b_dev->rx_destination_id = rfm22_read(rfm22b_dev, RFM22_received_header0);
966 rfm22b_dev->rx_destination_id |= (rfm22_read(rfm22b_dev, RFM22_received_header1) << 8);
967 rfm22b_dev->rx_destination_id |= (rfm22_read(rfm22b_dev, RFM22_received_header2) << 16);
968 rfm22b_dev->rx_destination_id |= (rfm22_read(rfm22b_dev, RFM22_received_header3) << 24);
969
970 // Release the SPI bus.
971 rfm22_releaseBus(rfm22b_dev);
972
973 // Is there a length error?
974 if (rfm22b_dev->rx_buffer_wr != len) {
975 rfm22_rxFailure(rfm22b_dev);
976 return PIOS_RFM22B_INT_FAILURE;
977 }
978
979 // Increment the total byte received count.
980 rfm22b_dev->stats.rx_byte_count += rfm22b_dev->rx_buffer_wr;
981
982 // Update the pair status with this packet.
983 rfm22_updatePairStatus(rfm22b_dev);
984
985 // We're finished with Rx mode
986 rfm22b_dev->rfm22b_state = RFM22B_STATE_TRANSITION;
987
988 ret = PIOS_RFM22B_RX_COMPLETE;
989 } else if (rfm22b_dev->status_regs.int_status_1.rx_fifo_almost_full) {
990 // RX FIFO almost full, it needs emptying
991 // read data from the rf chips FIFO buffer
992
993 // Claim the SPI bus.
994 rfm22_claimBus(rfm22b_dev);
995
996 // Read the total length of the packet data
997 uint16_t len = rfm22_read(rfm22b_dev, RFM22_received_packet_length);
998
999 // The received packet is going to be larger than the specified length
1000 if ((rfm22b_dev->rx_buffer_wr + RX_FIFO_HI_WATERMARK) > len) {
1001 rfm22_releaseBus(rfm22b_dev);
1002 rfm22_rxFailure(rfm22b_dev);
1003 return PIOS_RFM22B_INT_FAILURE;
1004 }
1005
1006 // The received packet is going to be larger than the receive buffer
1007 if ((rfm22b_dev->rx_buffer_wr + RX_FIFO_HI_WATERMARK) > rfm22b_dev->max_packet_len) {
1008 rfm22_releaseBus(rfm22b_dev);
1009 rfm22_rxFailure(rfm22b_dev);
1010 return PIOS_RFM22B_INT_FAILURE;
1011 }
1012
1013 // Fetch the data from the RX FIFO
1014 rfm22_assertCs(rfm22b_dev);
1015 PIOS_SPI_TransferByte(rfm22b_dev->spi_id, RFM22_fifo_access & 0x7F);
1016 rfm22b_dev->rx_buffer_wr += (PIOS_SPI_TransferBlock(rfm22b_dev->spi_id, OUT_FF, (uint8_t *)&rx_buffer[rfm22b_dev->rx_buffer_wr],
1017 RX_FIFO_HI_WATERMARK, NULL) == 0) ? RX_FIFO_HI_WATERMARK : 0;
1018 rfm22_deassertCs(rfm22b_dev);
1019
1020 // Release the SPI bus.
1021 rfm22_releaseBus(rfm22b_dev);
1022
1023 // Make sure that we're in RX mode.
1024 rfm22b_dev->rfm22b_state = RFM22B_STATE_RX_MODE;
1025 } else if (rfm22b_dev->status_regs.int_status_2.valid_preamble_detected) {
1026 // Valid preamble detected
1027 RX_LED_ON;
1028
1029 // Sync word detected
1030 #ifdef PIOS_RFM22B_DEBUG_ON_TELEM
1031 D2_LED_ON;
1032 #endif // PIOS_RFM22B_DEBUG_ON_TELEM
1033 rfm22b_dev->packet_start_ticks = xTaskGetTickCount();
1034 if (rfm22b_dev->packet_start_ticks == 0) {
1035 rfm22b_dev->packet_start_ticks = 1;
1036 }
1037
1038 // We detected the preamble, now wait for sync.
1039 rfm22b_dev->rfm22b_state = RFM22B_STATE_RX_WAIT_SYNC;
1040 } else if (rfm22b_dev->status_regs.int_status_2.sync_word_detected) {
1041 // Claim the SPI bus.
1042 rfm22_claimBus(rfm22b_dev);
1043
1044 // read the 10-bit signed afc correction value
1045 // bits 9 to 2
1046 uint16_t afc_correction = (uint16_t)rfm22_read(rfm22b_dev, RFM22_afc_correction_read) << 8;
1047 // bits 1 & 0
1048 afc_correction |= (uint16_t)rfm22_read(rfm22b_dev, RFM22_ook_counter_value1) & 0x00c0;
1049 afc_correction >>= 6;
1050 // convert the afc value to Hz
1051 int32_t afc_corr = (int32_t)(rfm22b_dev->frequency_step_size * afc_correction + 0.5f);
1052 rfm22b_dev->afc_correction_Hz = (afc_corr < -127) ? -127 : ((afc_corr > 127) ? 127 : afc_corr);
1053
1054 // read rx signal strength .. 45 = -100dBm, 205 = -20dBm
1055 uint8_t rssi = rfm22_read(rfm22b_dev, RFM22_rssi);
1056 // convert to dBm
1057 rfm22b_dev->rssi_dBm = (int8_t)(rssi >> 1) - 122;
1058
1059 // Release the SPI bus.
1060 rfm22_releaseBus(rfm22b_dev);
1061
1062 // Indicate that we're in RX mode.
1063 rfm22b_dev->rfm22b_state = RFM22B_STATE_RX_MODE;
1064 } else if ((rfm22b_dev->rfm22b_state == RFM22B_STATE_RX_WAIT_SYNC) && !rfm22b_dev->status_regs.int_status_2.valid_preamble_detected) {
1065 // Waiting for the preamble timed out.
1066 rfm22_rxFailure(rfm22b_dev);
1067 return PIOS_RFM22B_INT_FAILURE;
1068 }
1069
1070 return ret;
1071 }
1072
1073 /**
1074 * Set the PPM packet received callback.
1075 *
1076 * @param[in] rfm22b_dev The RFM22B device ID.
1077 * @param[in] cb The callback function pointer.
1078 */
1079 void PIOS_RFM22B_SetPPMCallback(uint32_t rfm22b_id, PPMReceivedCallback cb)
1080 {
1081 struct pios_rfm22b_dev *rfm22b_dev = (struct pios_rfm22b_dev *)rfm22b_id;
1082
1083 if (!PIOS_RFM22B_Validate(rfm22b_dev)) {
1084 return;
1085 }
1086
1087 rfm22b_dev->ppm_callback = cb;
1088 }
1089
1090 /**
1091 * Set the PPM values to be transmitted.
1092 *
1093 * @param[in] rfm22b_dev The RFM22B device ID.
1094 * @param[in] channels The PPM channel values.
1095 */
1096 extern void PIOS_RFM22B_PPMSet(uint32_t rfm22b_id, int16_t *channels)
1097 {
1098 struct pios_rfm22b_dev *rfm22b_dev = (struct pios_rfm22b_dev *)rfm22b_id;
1099
1100 if (!PIOS_RFM22B_Validate(rfm22b_dev)) {
1101 return;
1102 }
1103
1104 for (uint8_t i = 0; i < RFM22B_PPM_NUM_CHANNELS; ++i) {
1105 rfm22b_dev->ppm[i] = channels[i];
1106 }
1107 }
1108
1109 /**
1110 * Fetch the PPM values that have been received.
1111 *
1112 * @param[in] rfm22b_dev The RFM22B device structure pointer.
1113 * @param[out] channels The PPM channel values.
1114 */
1115 extern void PIOS_RFM22B_PPMGet(uint32_t rfm22b_id, int16_t *channels)
1116 {
1117 struct pios_rfm22b_dev *rfm22b_dev = (struct pios_rfm22b_dev *)rfm22b_id;
1118
1119 if (!PIOS_RFM22B_Validate(rfm22b_dev)) {
1120 return;
1121 }
1122
1123 for (uint8_t i = 0; i < RFM22B_PPM_NUM_CHANNELS; ++i) {
1124 channels[i] = rfm22b_dev->ppm[i];
1125 }
1126 }
1127
1128 /**
1129 * Validate that the device structure is valid.
1130 *
1131 * @param[in] rfm22b_dev The RFM22B device structure pointer.
1132 */
1133 inline bool PIOS_RFM22B_Validate(struct pios_rfm22b_dev *rfm22b_dev)
1134 {
1135 return rfm22b_dev != NULL && rfm22b_dev->magic == PIOS_RFM22B_DEV_MAGIC;
1136 }
1137
1138
1139 /*****************************************************************************
1140 * The Device Control Thread
1141 *****************************************************************************/
1142
1143 /**
1144 * The task that controls the radio state machine.
1145 *
1146 * @param[in] paramters The task parameters.
1147 */
1148 static void pios_rfm22_task(void *parameters)
1149 {
1150 struct pios_rfm22b_dev *rfm22b_dev = (struct pios_rfm22b_dev *)parameters;
1151
1152 if (!PIOS_RFM22B_Validate(rfm22b_dev)) {
1153 return;
1154 }
1155 portTickType lastEventTicks = xTaskGetTickCount();
1156
1157 while (1) {
1158 #if defined(PIOS_INCLUDE_WDG) && defined(PIOS_WDG_RFM22B)
1159 // Update the watchdog timer
1160 PIOS_WDG_UpdateFlag(PIOS_WDG_RFM22B);
1161 #endif /* PIOS_WDG_RFM22B */
1162
1163 // Wait for a signal indicating an external interrupt or a pending send/receive request.
1164 if (xSemaphoreTake(rfm22b_dev->isrPending, ISR_TIMEOUT / portTICK_RATE_MS) == pdTRUE) {
1165 lastEventTicks = xTaskGetTickCount();
1166
1167 // Process events through the state machine.
1168 enum pios_radio_event event;
1169 while (xQueueReceive(rfm22b_dev->eventQueue, &event, 0) == pdTRUE) {
1170 if ((event == RADIO_EVENT_INT_RECEIVED) &&
1171 ((rfm22b_dev->state == RADIO_STATE_UNINITIALIZED) || (rfm22b_dev->state == RADIO_STATE_INITIALIZING))) {
1172 continue;
1173 }
1174 rfm22_process_event(rfm22b_dev, event);
1175 }
1176 } else {
1177 // Has it been too long since the last event?
1178 portTickType curTicks = xTaskGetTickCount();
1179 if (pios_rfm22_time_difference_ms(lastEventTicks, curTicks) > PIOS_RFM22B_SUPERVISOR_TIMEOUT) {
1180 // Clear the event queue.
1181 enum pios_radio_event event;
1182 while (xQueueReceive(rfm22b_dev->eventQueue, &event, 0) == pdTRUE) {
1183 // Do nothing;
1184 }
1185 lastEventTicks = xTaskGetTickCount();
1186
1187 // Transsition through an error event.
1188 rfm22_process_event(rfm22b_dev, RADIO_EVENT_ERROR);
1189 }
1190 }
1191
1192 // Change channels if necessary.
1193 if (rfm22_changeChannel(rfm22b_dev)) {
1194 rfm22_process_event(rfm22b_dev, RADIO_EVENT_RX_MODE);
1195 }
1196
1197 portTickType curTicks = xTaskGetTickCount();
1198 // Have we been sending / receiving this packet too long?
1199
1200 if ((rfm22b_dev->packet_start_ticks > 0) &&
1201 (pios_rfm22_time_difference_ms(rfm22b_dev->packet_start_ticks, curTicks) > (rfm22b_dev->packet_time * 3))) {
1202 rfm22_process_event(rfm22b_dev, RADIO_EVENT_TIMEOUT);
1203 }
1204
1205 // Start transmitting a packet if it's time.
1206 bool time_to_send = rfm22_timeToSend(rfm22b_dev);
1207 #ifdef PIOS_RFM22B_DEBUG_ON_TELEM
1208 if (time_to_send) {
1209 D4_LED_ON;
1210 } else {
1211 D4_LED_OFF;
1212 }
1213 #endif
1214 if (time_to_send && PIOS_RFM22B_InRxWait((uint32_t)rfm22b_dev)) {
1215 rfm22_process_event(rfm22b_dev, RADIO_EVENT_TX_START);
1216 }
1217 }
1218 }
1219
1220
1221 /*****************************************************************************
1222 * The State Machine Functions
1223 *****************************************************************************/
1224
1225 /**
1226 * Inject an event into the RFM22B state machine.
1227 *
1228 * @param[in] rfm22b_dev The device structure
1229 * @param[in] event The event to inject
1230 * @param[in] inISR Is this being called from an interrrup service routine?
1231 */
1232 static void pios_rfm22_inject_event(struct pios_rfm22b_dev *rfm22b_dev, enum pios_radio_event event, bool inISR)
1233 {
1234 if (inISR) {
1235 // Store the event.
1236 portBASE_TYPE pxHigherPriorityTaskWoken1;
1237 if (xQueueSendFromISR(rfm22b_dev->eventQueue, &event, &pxHigherPriorityTaskWoken1) != pdTRUE) {
1238 return;
1239 }
1240 // Signal the semaphore to wake up the handler thread.
1241 portBASE_TYPE pxHigherPriorityTaskWoken2;
1242 if (xSemaphoreGiveFromISR(rfm22b_dev->isrPending, &pxHigherPriorityTaskWoken2) != pdTRUE) {
1243 // Something went fairly seriously wrong
1244 rfm22b_dev->errors++;
1245 }
1246 portEND_SWITCHING_ISR((pxHigherPriorityTaskWoken1 == pdTRUE) || (pxHigherPriorityTaskWoken2 == pdTRUE));
1247 } else {
1248 // Store the event.
1249 if (xQueueSend(rfm22b_dev->eventQueue, &event, portMAX_DELAY) != pdTRUE) {
1250 return;
1251 }
1252 // Signal the semaphore to wake up the handler thread.
1253 if (xSemaphoreGive(rfm22b_dev->isrPending) != pdTRUE) {
1254 // Something went fairly seriously wrong
1255 rfm22b_dev->errors++;
1256 }
1257 }
1258 }
1259
1260 /**
1261 * Process the next state transition from the given event.
1262 *
1263 * @param[in] rfm22b_dev The device structure
1264 * @param[in] event The event to process
1265 * @return enum pios_radio_event The next event to inject
1266 */
1267 static enum pios_radio_event rfm22_process_state_transition(struct pios_rfm22b_dev *rfm22b_dev, enum pios_radio_event event)
1268 {
1269 // No event
1270 if (event >= RADIO_EVENT_NUM_EVENTS) {
1271 return RADIO_EVENT_NUM_EVENTS;
1272 }
1273
1274 // Don't transition if there is no transition defined
1275 enum pios_radio_state next_state = rfm22b_transitions[rfm22b_dev->state].next_state[event];
1276 if (!next_state) {
1277 return RADIO_EVENT_NUM_EVENTS;
1278 }
1279
1280 /*
1281 * Move to the next state
1282 *
1283 * This is done prior to calling the new state's entry function to
1284 * guarantee that the entry function never depends on the previous
1285 * state. This way, it cannot ever know what the previous state was.
1286 */
1287 rfm22b_dev->state = next_state;
1288
1289 /* Call the entry function (if any) for the next state. */
1290 if (rfm22b_transitions[rfm22b_dev->state].entry_fn) {
1291 return rfm22b_transitions[rfm22b_dev->state].entry_fn(rfm22b_dev);
1292 }
1293
1294 return RADIO_EVENT_NUM_EVENTS;
1295 }
1296
1297 /**
1298 * Process the given event through the state transition table.
1299 * This could cause a series of events and transitions to take place.
1300 *
1301 * @param[in] rfm22b_dev The device structure
1302 * @param[in] event The event to process
1303 */
1304 static void rfm22_process_event(struct pios_rfm22b_dev *rfm22b_dev, enum pios_radio_event event)
1305 {
1306 // Process all state transitions.
1307 while (event != RADIO_EVENT_NUM_EVENTS) {
1308 event = rfm22_process_state_transition(rfm22b_dev, event);
1309 }
1310 }
1311
1312
1313 /*****************************************************************************
1314 * The Device Initialization / Configuration Functions
1315 *****************************************************************************/
1316
1317 /**
1318 * Initialize (or re-initialize) the RFM22B radio device.
1319 *
1320 * @param[in] rfm22b_dev The device structure
1321 * @return enum pios_radio_event The next event to inject
1322 */
1323 static enum pios_radio_event rfm22_init(struct pios_rfm22b_dev *rfm22b_dev)
1324 {
1325 // Initialize the register values.
1326 rfm22b_dev->status_regs.int_status_1.raw = 0;
1327 rfm22b_dev->status_regs.int_status_2.raw = 0;
1328 rfm22b_dev->status_regs.device_status.raw = 0;
1329 rfm22b_dev->status_regs.ezmac_status.raw = 0;
1330
1331 // Clean the LEDs
1332 rfm22_clearLEDs();
1333
1334 // Initialize the detected device statistics.
1335 for (uint8_t i = 0; i < OPLINKSTATUS_PAIRIDS_NUMELEM; ++i) {
1336 rfm22b_dev->pair_stats[i].pairID = 0;
1337 rfm22b_dev->pair_stats[i].rssi = -127;
1338 rfm22b_dev->pair_stats[i].afc_correction = 0;
1339 rfm22b_dev->pair_stats[i].lastContact = 0;
1340 }
1341
1342 // Initlize the link stats.
1343 for (uint8_t i = 0; i < RFM22B_RX_PACKET_STATS_LEN; ++i) {
1344 rfm22b_dev->rx_packet_stats[i] = 0;
1345 }
1346
1347 // Initialize the state
1348 rfm22b_dev->stats.link_state = OPLINKSTATUS_LINKSTATE_ENABLED;
1349
1350 // Initialize the packets.
1351 rfm22b_dev->rx_packet_len = 0;
1352 rfm22b_dev->rx_destination_id = 0;
1353 rfm22b_dev->tx_packet_handle = NULL;
1354
1355 // Initialize the devide state
1356 rfm22b_dev->rx_buffer_wr = 0;
1357 rfm22b_dev->tx_data_rd = rfm22b_dev->tx_data_wr = 0;
1358 rfm22b_dev->channel = 0;
1359 rfm22b_dev->channel_index = 0;
1360 rfm22b_dev->afc_correction_Hz = 0;
1361 rfm22b_dev->packet_start_ticks = 0;
1362 rfm22b_dev->tx_complete_ticks = 0;
1363 rfm22b_dev->rfm22b_state = RFM22B_STATE_INITIALIZING;
1364 rfm22b_dev->last_contact = 0;
1365
1366 // software reset the RF chip .. following procedure according to Si4x3x Errata (rev. B)
1367 rfm22_write_claim(rfm22b_dev, RFM22_op_and_func_ctrl1, RFM22_opfc1_swres);
1368
1369 for (uint8_t i = 0; i < 50; ++i) {
1370 // read the status registers
1371 pios_rfm22_readStatus(rfm22b_dev);
1372
1373 // Is the chip ready?
1374 if (rfm22b_dev->status_regs.int_status_2.chip_ready) {
1375 break;
1376 }
1377
1378 // Wait 1ms if not.
1379 vTaskDelay(1 + (1 / (portTICK_RATE_MS + 1)));
1380 }
1381
1382 // ****************
1383
1384 // read status - clears interrupt
1385 pios_rfm22_readStatus(rfm22b_dev);
1386
1387 // Claim the SPI bus.
1388 rfm22_claimBus(rfm22b_dev);
1389
1390 // disable all interrupts
1391 rfm22_write(rfm22b_dev, RFM22_interrupt_enable1, 0x00);
1392 rfm22_write(rfm22b_dev, RFM22_interrupt_enable2, 0x00);
1393
1394 // read the RF chip ID bytes
1395
1396 // read the device type
1397 uint8_t device_type = rfm22_read(rfm22b_dev, RFM22_DEVICE_TYPE) & RFM22_DT_MASK;
1398 // read the device version
1399 uint8_t device_version = rfm22_read(rfm22b_dev, RFM22_DEVICE_VERSION) & RFM22_DV_MASK;
1400
1401 #if defined(RFM22_DEBUG)
1402 DEBUG_PRINTF(2, "rf device type: %d\n\r", device_type);
1403 DEBUG_PRINTF(2, "rf device version: %d\n\r", device_version);
1404 #endif
1405
1406 if (device_type != 0x08) {
1407 #if defined(RFM22_DEBUG)
1408 DEBUG_PRINTF(2, "rf device type: INCORRECT - should be 0x08\n\r");
1409 #endif
1410
1411 // incorrect RF module type
1412 return RADIO_EVENT_FATAL_ERROR;
1413 }
1414 if (device_version != RFM22_DEVICE_VERSION_B1) {
1415 #if defined(RFM22_DEBUG)
1416 DEBUG_PRINTF(2, "rf device version: INCORRECT\n\r");
1417 #endif
1418 // incorrect RF module version
1419 return RADIO_EVENT_FATAL_ERROR;
1420 }
1421
1422 // calibrate our RF module to be exactly on frequency .. different for every module
1423 rfm22_write(rfm22b_dev, RFM22_xtal_osc_load_cap, OSC_LOAD_CAP);
1424
1425 // disable Low Duty Cycle Mode
1426 rfm22_write(rfm22b_dev, RFM22_op_and_func_ctrl2, 0x00);
1427
1428 // 1MHz clock output
1429 rfm22_write(rfm22b_dev, RFM22_cpu_output_clk, RFM22_coc_1MHz);
1430
1431 // READY mode
1432 rfm22_write(rfm22b_dev, RFM22_op_and_func_ctrl1, RFM22_opfc1_xton);
1433
1434 // choose the 3 GPIO pin functions
1435 // GPIO port use default value
1436 rfm22_write(rfm22b_dev, RFM22_io_port_config, RFM22_io_port_default);
1437 if (rfm22b_dev->cfg.gpio_direction == GPIO0_TX_GPIO1_RX) {
1438 // GPIO0 = TX State (to control RF Switch)
1439 rfm22_write(rfm22b_dev, RFM22_gpio0_config, RFM22_gpio0_config_drv3 | RFM22_gpio0_config_txstate);
1440 // GPIO1 = RX State (to control RF Switch)
1441 rfm22_write(rfm22b_dev, RFM22_gpio1_config, RFM22_gpio1_config_drv3 | RFM22_gpio1_config_rxstate);
1442 } else {
1443 // GPIO0 = TX State (to control RF Switch)
1444 rfm22_write(rfm22b_dev, RFM22_gpio0_config, RFM22_gpio0_config_drv3 | RFM22_gpio0_config_rxstate);
1445 // GPIO1 = RX State (to control RF Switch)
1446 rfm22_write(rfm22b_dev, RFM22_gpio1_config, RFM22_gpio1_config_drv3 | RFM22_gpio1_config_txstate);
1447 }
1448 // GPIO2 = Clear Channel Assessment
1449 rfm22_write(rfm22b_dev, RFM22_gpio2_config, RFM22_gpio2_config_drv3 | RFM22_gpio2_config_cca);
1450
1451 // FIFO mode, GFSK modulation
1452 uint8_t fd_bit = rfm22_read(rfm22b_dev, RFM22_modulation_mode_control2) & RFM22_mmc2_fd;
1453 rfm22_write(rfm22b_dev, RFM22_modulation_mode_control2, RFM22_mmc2_trclk_clk_none | RFM22_mmc2_dtmod_fifo | fd_bit | RFM22_mmc2_modtyp_gfsk);
1454
1455 // setup to read the internal temperature sensor
1456
1457 // ADC used to sample the temperature sensor
1458 uint8_t adc_config = RFM22_ac_adcsel_temp_sensor | RFM22_ac_adcref_bg;
1459 rfm22_write(rfm22b_dev, RFM22_adc_config, adc_config);
1460
1461 // adc offset
1462 rfm22_write(rfm22b_dev, RFM22_adc_sensor_amp_offset, 0);
1463
1464 // temp sensor calibration .. �40C to +64C 0.5C resolution
1465 rfm22_write(rfm22b_dev, RFM22_temp_sensor_calib, RFM22_tsc_tsrange0 | RFM22_tsc_entsoffs);
1466
1467 // temp sensor offset
1468 rfm22_write(rfm22b_dev, RFM22_temp_value_offset, 0);
1469
1470 // start an ADC conversion
1471 rfm22_write(rfm22b_dev, RFM22_adc_config, adc_config | RFM22_ac_adcstartbusy);
1472
1473 // set the RSSI threshold interrupt to about -90dBm
1474 rfm22_write(rfm22b_dev, RFM22_rssi_threshold_clear_chan_indicator, (-90 + 122) * 2);
1475
1476 // enable the internal Tx & Rx packet handlers (without CRC)
1477 rfm22_write(rfm22b_dev, RFM22_data_access_control, RFM22_dac_enpacrx | RFM22_dac_enpactx);
1478
1479 // x-nibbles tx preamble
1480 rfm22_write(rfm22b_dev, RFM22_preamble_length, TX_PREAMBLE_NIBBLES);
1481 // x-nibbles rx preamble detection
1482 rfm22_write(rfm22b_dev, RFM22_preamble_detection_ctrl1, RX_PREAMBLE_NIBBLES << 3);
1483
1484 // Release the bus
1485 rfm22_releaseBus(rfm22b_dev);
1486
1487 // Yield the CPU.
1488 vTaskDelay(1 + (1 / (portTICK_RATE_MS + 1)));
1489
1490 // Claim the SPI bus.
1491 rfm22_claimBus(rfm22b_dev);
1492
1493 // header control - using a 4 by header with broadcast of 0xffffffff
1494 rfm22_write(rfm22b_dev, RFM22_header_control1,
1495 RFM22_header_cntl1_bcen_0 |
1496 RFM22_header_cntl1_bcen_1 |
1497 RFM22_header_cntl1_bcen_2 |
1498 RFM22_header_cntl1_bcen_3 |
1499 RFM22_header_cntl1_hdch_0 |
1500 RFM22_header_cntl1_hdch_1 |
1501 RFM22_header_cntl1_hdch_2 |
1502 RFM22_header_cntl1_hdch_3);
1503 // Check all bit of all bytes of the header, unless we're an unbound modem.
1504 uint8_t header_mask = (rfm22_destinationID(rfm22b_dev) == 0xffffffff) ? 0 : 0xff;
1505 rfm22_write(rfm22b_dev, RFM22_header_enable0, header_mask);
1506 rfm22_write(rfm22b_dev, RFM22_header_enable1, header_mask);
1507 rfm22_write(rfm22b_dev, RFM22_header_enable2, header_mask);
1508 rfm22_write(rfm22b_dev, RFM22_header_enable3, header_mask);
1509 // The destination ID and receive ID should be the same.
1510 uint32_t id = rfm22_destinationID(rfm22b_dev);
1511 rfm22_write(rfm22b_dev, RFM22_check_header0, id & 0xff);
1512 rfm22_write(rfm22b_dev, RFM22_check_header1, (id >> 8) & 0xff);
1513 rfm22_write(rfm22b_dev, RFM22_check_header2, (id >> 16) & 0xff);
1514 rfm22_write(rfm22b_dev, RFM22_check_header3, (id >> 24) & 0xff);
1515 // 4 header bytes, synchronization word length 3, 2, 1 & 0 used, packet length included in header.
1516 rfm22_write(rfm22b_dev, RFM22_header_control2,
1517 RFM22_header_cntl2_hdlen_3210 |
1518 RFM22_header_cntl2_synclen_3210 |
1519 ((TX_PREAMBLE_NIBBLES >> 8) & 0x01));
1520
1521 // sync word
1522 rfm22_write(rfm22b_dev, RFM22_sync_word3, SYNC_BYTE_1);
1523 rfm22_write(rfm22b_dev, RFM22_sync_word2, SYNC_BYTE_2);
1524 rfm22_write(rfm22b_dev, RFM22_sync_word1, SYNC_BYTE_3);
1525 rfm22_write(rfm22b_dev, RFM22_sync_word0, SYNC_BYTE_4);
1526
1527 // TX FIFO Almost Full Threshold (0 - 63)
1528 rfm22_write(rfm22b_dev, RFM22_tx_fifo_control1, TX_FIFO_HI_WATERMARK);
1529
1530 // TX FIFO Almost Empty Threshold (0 - 63)
1531 rfm22_write(rfm22b_dev, RFM22_tx_fifo_control2, TX_FIFO_LO_WATERMARK);
1532
1533 // RX FIFO Almost Full Threshold (0 - 63)
1534 rfm22_write(rfm22b_dev, RFM22_rx_fifo_control, RX_FIFO_HI_WATERMARK);
1535
1536 // Set the frequency calibration
1537 rfm22_write(rfm22b_dev, RFM22_xtal_osc_load_cap, rfm22b_dev->cfg.RFXtalCap);
1538
1539 // Release the bus
1540 rfm22_releaseBus(rfm22b_dev);
1541
1542 // Yield the CPU.
1543 vTaskDelay(1 + (1 / (portTICK_RATE_MS + 1)));
1544
1545 // Initialize the frequency and datarate to te default.
1546 rfm22_setNominalCarrierFrequency(rfm22b_dev, 0);
1547 pios_rfm22_setDatarate(rfm22b_dev);
1548
1549 return RADIO_EVENT_INITIALIZED;
1550 }
1551
1552 /**
1553 * Set the air datarate for the RFM22B device.
1554 *
1555 * Carson's rule:
1556 * The signal bandwidth is about 2(Delta-f + fm) ..
1557 *
1558 * Delta-f = frequency deviation
1559 * fm = maximum frequency of the signal
1560 *
1561 * @param[in] rfm33b_dev The device structure pointer.
1562 * @param[in] datarate The air datarate.
1563 * @param[in] data_whitening Is data whitening desired?
1564 */
1565 static void pios_rfm22_setDatarate(struct pios_rfm22b_dev *rfm22b_dev)
1566 {
1567 enum rfm22b_datarate datarate = rfm22b_dev->datarate;
1568 bool data_whitening = true;
1569
1570 // Claim the SPI bus.
1571 rfm22_claimBus(rfm22b_dev);
1572
1573 // rfm22_if_filter_bandwidth
1574 rfm22_write(rfm22b_dev, 0x1C, reg_1C[datarate]);
1575
1576 // rfm22_afc_loop_gearshift_override
1577 rfm22_write(rfm22b_dev, 0x1D, reg_1D[datarate]);
1578 // RFM22_afc_timing_control
1579 rfm22_write(rfm22b_dev, 0x1E, reg_1E[datarate]);
1580
1581 // RFM22_clk_recovery_gearshift_override
1582 rfm22_write(rfm22b_dev, 0x1F, reg_1F[datarate]);
1583 // rfm22_clk_recovery_oversampling_ratio
1584 rfm22_write(rfm22b_dev, 0x20, reg_20[datarate]);
1585 // rfm22_clk_recovery_offset2
1586 rfm22_write(rfm22b_dev, 0x21, reg_21[datarate]);
1587 // rfm22_clk_recovery_offset1
1588 rfm22_write(rfm22b_dev, 0x22, reg_22[datarate]);
1589 // rfm22_clk_recovery_offset0
1590 rfm22_write(rfm22b_dev, 0x23, reg_23[datarate]);
1591 // rfm22_clk_recovery_timing_loop_gain1
1592 rfm22_write(rfm22b_dev, 0x24, reg_24[datarate]);
1593 // rfm22_clk_recovery_timing_loop_gain0
1594 rfm22_write(rfm22b_dev, 0x25, reg_25[datarate]);
1595 // rfm22_agc_override1
1596 rfm22_write(rfm22b_dev, RFM22_agc_override1, reg_69[datarate]);
1597
1598 // rfm22_afc_limiter
1599 rfm22_write(rfm22b_dev, 0x2A, reg_2A[datarate]);
1600
1601 // rfm22_tx_data_rate1
1602 rfm22_write(rfm22b_dev, 0x6E, reg_6E[datarate]);
1603 // rfm22_tx_data_rate0
1604 rfm22_write(rfm22b_dev, 0x6F, reg_6F[datarate]);
1605
1606 if (!data_whitening) {
1607 // rfm22_modulation_mode_control1
1608 rfm22_write(rfm22b_dev, 0x70, reg_70[datarate] & ~RFM22_mmc1_enwhite);
1609 } else {
1610 // rfm22_modulation_mode_control1
1611 rfm22_write(rfm22b_dev, 0x70, reg_70[datarate] | RFM22_mmc1_enwhite);
1612 }
1613
1614 // rfm22_modulation_mode_control2
1615 rfm22_write(rfm22b_dev, 0x71, reg_71[datarate]);
1616
1617 // rfm22_frequency_deviation
1618 rfm22_write(rfm22b_dev, 0x72, reg_72[datarate]);
1619
1620 // rfm22_cpcuu
1621 rfm22_write(rfm22b_dev, 0x58, reg_58[datarate]);
1622
1623 rfm22_write(rfm22b_dev, RFM22_ook_counter_value1, 0x00);
1624 rfm22_write(rfm22b_dev, RFM22_ook_counter_value2, 0x00);
1625
1626 // Release the bus
1627 rfm22_releaseBus(rfm22b_dev);
1628 }
1629
1630 /**
1631 * Set the nominal carrier frequency, channel step size, and initial channel
1632 *
1633 * @param[in] rfm33b_dev The device structure pointer.
1634 * @param[in] init_chan The initial channel to tune to.
1635 */
1636 static void rfm22_setNominalCarrierFrequency(struct pios_rfm22b_dev *rfm22b_dev, uint8_t init_chan)
1637 {
1638 // Set the frequency channels to start at 430MHz
1639 uint32_t frequency_hz = RFM22B_NOMINAL_CARRIER_FREQUENCY;
1640 // The step size is 10MHz / 250 = 40khz, and the step size is specified in 10khz increments.
1641 uint8_t freq_hop_step_size = 4;
1642
1643 // holds the hbsel (1 or 2)
1644 uint8_t hbsel;
1645
1646 if (frequency_hz < 480000000) {
1647 hbsel = 0;
1648 } else {
1649 hbsel = 1;
1650 }
1651 float freq_mhz = (float)(frequency_hz) / 1000000.0f;
1652 float xtal_freq_khz = 30000.0f;
1653 float sfreq = freq_mhz / (10.0f * (xtal_freq_khz / 30000.0f) * (1 + hbsel));
1654 uint32_t fb = (uint32_t)sfreq - 24 + (64 + 32 * hbsel);
1655 uint32_t fc = (uint32_t)((sfreq - (uint32_t)sfreq) * 64000.0f);
1656 uint8_t fch = (fc >> 8) & 0xff;
1657 uint8_t fcl = fc & 0xff;
1658
1659 // Claim the SPI bus.
1660 rfm22_claimBus(rfm22b_dev);
1661
1662 // Setthe frequency hopping step size.
1663 rfm22_write(rfm22b_dev, RFM22_frequency_hopping_step_size, freq_hop_step_size);
1664
1665 // frequency hopping channel (0-255)
1666 rfm22b_dev->frequency_step_size = 156.25f * hbsel;
1667
1668 // frequency hopping channel (0-255)
1669 rfm22b_dev->channel = init_chan;
1670 rfm22_write(rfm22b_dev, RFM22_frequency_hopping_channel_select, init_chan);
1671
1672 // no frequency offset
1673 rfm22_write(rfm22b_dev, RFM22_frequency_offset1, 0);
1674 rfm22_write(rfm22b_dev, RFM22_frequency_offset2, 0);
1675
1676 // set the carrier frequency
1677 rfm22_write(rfm22b_dev, RFM22_frequency_band_select, fb & 0xff);
1678 rfm22_write(rfm22b_dev, RFM22_nominal_carrier_frequency1, fch);
1679 rfm22_write(rfm22b_dev, RFM22_nominal_carrier_frequency0, fcl);
1680
1681 // Release the bus
1682 rfm22_releaseBus(rfm22b_dev);
1683 }
1684
1685
1686 /**
1687 * Set the frequency hopping channel.
1688 *
1689 * @param[in] rfm33b_dev The device structure pointer.
1690 */
1691 static bool rfm22_setFreqHopChannel(struct pios_rfm22b_dev *rfm22b_dev, uint8_t channel)
1692 {
1693 // set the frequency hopping channel
1694 if (rfm22b_dev->channel == channel) {
1695 return false;
1696 }
1697 #ifdef PIOS_RFM22B_DEBUG_ON_TELEM
1698 D3_LED_TOGGLE;
1699 #endif // PIOS_RFM22B_DEBUG_ON_TELEM
1700 rfm22b_dev->channel = channel;
1701 rfm22_write_claim(rfm22b_dev, RFM22_frequency_hopping_channel_select, channel);
1702 return true;
1703 }
1704
1705 /**
1706 * Read the RFM22B interrupt and device status registers
1707 *
1708 * @param[in] rfm22b_dev The device structure
1709 */
1710 static bool pios_rfm22_readStatus(struct pios_rfm22b_dev *rfm22b_dev)
1711 {
1712 // 1. Read the interrupt statuses with burst read
1713 rfm22_claimBus(rfm22b_dev); // Set RC and the semaphore
1714 uint8_t write_buf[3] = { RFM22_interrupt_status1 &0x7f, 0xFF, 0xFF };
1715 uint8_t read_buf[3];
1716 rfm22_assertCs(rfm22b_dev);
1717 PIOS_SPI_TransferBlock(rfm22b_dev->spi_id, write_buf, read_buf, sizeof(write_buf), NULL);
1718 rfm22_deassertCs(rfm22b_dev);
1719 rfm22b_dev->status_regs.int_status_1.raw = read_buf[1];
1720 rfm22b_dev->status_regs.int_status_2.raw = read_buf[2];
1721
1722 // Device status
1723 rfm22b_dev->status_regs.device_status.raw = rfm22_read(rfm22b_dev, RFM22_device_status);
1724
1725 // EzMAC status
1726 rfm22b_dev->status_regs.ezmac_status.raw = rfm22_read(rfm22b_dev, RFM22_ezmac_status);
1727
1728 // Release the bus
1729 rfm22_releaseBus(rfm22b_dev);
1730
1731 // the RF module has gone and done a reset - we need to re-initialize the rf module
1732 if (rfm22b_dev->status_regs.int_status_2.poweron_reset) {
1733 return false;
1734 }
1735
1736 return true;
1737 }
1738
1739 /**
1740 * Recover from a failure in receiving a packet.
1741 *
1742 * @param[in] rfm22b_dev The device structure
1743 * @return enum pios_radio_event The next event to inject
1744 */
1745 static void rfm22_rxFailure(struct pios_rfm22b_dev *rfm22b_dev)
1746 {
1747 rfm22b_add_rx_status(rfm22b_dev, RADIO_FAILURE_RX_PACKET);
1748 rfm22b_dev->rx_buffer_wr = 0;
1749 rfm22b_dev->packet_start_ticks = 0;
1750 rfm22b_dev->rfm22b_state = RFM22B_STATE_TRANSITION;
1751 }
1752
1753
1754 /*****************************************************************************
1755 * Radio Transmit and Receive functions.
1756 *****************************************************************************/
1757
1758 /**
1759 * Start a transmit if possible
1760 *
1761 * @param[in] radio_dev The device structure
1762 * @return enum pios_radio_event The next event to inject
1763 */
1764 static enum pios_radio_event radio_txStart(struct pios_rfm22b_dev *radio_dev)
1765 {
1766 uint8_t *p = radio_dev->tx_packet;
1767 uint8_t len = 0;
1768 uint8_t max_data_len = radio_dev->max_packet_len - (radio_dev->ppm_only_mode ? 0 : RS_ECC_NPARITY);
1769
1770 // Don't send if it's not our turn, or if we're receiving a packet.
1771 if (!rfm22_timeToSend(radio_dev) || !PIOS_RFM22B_InRxWait((uint32_t)radio_dev)) {
1772 return RADIO_EVENT_RX_MODE;
1773 }
1774
1775 // Don't send anything if we're bound to a coordinator and not yet connected.
1776 if (!rfm22_isCoordinator(radio_dev) && !rfm22_isConnected(radio_dev)) {
1777 return RADIO_EVENT_RX_MODE;
1778 }
1779
1780 // Should we append PPM data to the packet?
1781 if (radio_dev->ppm_send_mode) {
1782 len = RFM22B_PPM_NUM_CHANNELS + (radio_dev->ppm_only_mode ? 2 : 1);
1783
1784 // Ensure we can fit the PPM data in the packet.
1785 if (max_data_len < len) {
1786 return RADIO_EVENT_RX_MODE;
1787 }
1788
1789 // The first byte stores the LSB of each channel
1790 p[0] = 0;
1791
1792 // Read the PPM input.
1793 for (uint8_t i = 0; i < RFM22B_PPM_NUM_CHANNELS; ++i) {
1794 int32_t val = radio_dev->ppm[i];
1795
1796 // Clamp and translate value, or transmit as reserved "invalid" constant
1797 if ((val == PIOS_RCVR_INVALID) || (val == PIOS_RCVR_TIMEOUT)) {
1798 val = RFM22B_PPM_INVALID;
1799 } else if (val > RFM22B_PPM_MAX_US) {
1800 val = RFM22B_PPM_MAX;
1801 } else if (val < RFM22B_PPM_MIN_US) {
1802 val = RFM22B_PPM_MIN;
1803 } else {
1804 val = (val - RFM22B_PPM_MIN_US) / RFM22B_PPM_SCALE + RFM22B_PPM_MIN;
1805 }
1806
1807 // Store LSB
1808 if (val & 1) {
1809 p[0] |= (1 << i);
1810 }
1811
1812 // Store upper 8 bits in array
1813 p[i + 1] = val >> 1;
1814 }
1815
1816 // The last byte is a CRC.
1817 if (radio_dev->ppm_only_mode) {
1818 uint8_t crc = 0;
1819 for (uint8_t i = 0; i < RFM22B_PPM_NUM_CHANNELS + 1; ++i) {
1820 crc = PIOS_CRC_updateByte(crc, p[i]);
1821 }
1822 p[RFM22B_PPM_NUM_CHANNELS + 1] = crc;
1823 }
1824 }
1825
1826 // Append data from the com interface if applicable.
1827 if (!radio_dev->ppm_only_mode && radio_dev->tx_out_cb) {
1828 // Try to get some data to send
1829 bool need_yield = false;
1830 len += (radio_dev->tx_out_cb)(radio_dev->tx_out_context, p + len, max_data_len - len, NULL, &need_yield);
1831 }
1832
1833 // Always send a packet if this modem is a coordinator.
1834 if ((len == 0) && !rfm22_isCoordinator(radio_dev)) {
1835 return RADIO_EVENT_RX_MODE;
1836 }
1837
1838 // Increment the packet sequence number.
1839 radio_dev->stats.tx_seq++;
1840
1841 // Add the error correcting code.
1842 if (!radio_dev->ppm_only_mode) {
1843 if (len != 0) {
1844 encode_data((unsigned char *)p, len, (unsigned char *)p);
1845 }
1846 len += RS_ECC_NPARITY;
1847 }
1848
1849 // Transmit the packet.
1850 PIOS_RFM22B_TransmitPacket((uint32_t)radio_dev, p, len);
1851
1852 return RADIO_EVENT_NUM_EVENTS;
1853 }
1854
1855 /**
1856 * Transmit packet data.
1857 *
1858 * @param[in] rfm22b_dev The device structure
1859 * @return enum pios_radio_event The next event to inject
1860 */
1861 static enum pios_radio_event radio_txData(struct pios_rfm22b_dev *radio_dev)
1862 {
1863 enum pios_radio_event ret_event = RADIO_EVENT_NUM_EVENTS;
1864 pios_rfm22b_int_result res = PIOS_RFM22B_ProcessTx((uint32_t)radio_dev);
1865
1866 // Is the transmition complete
1867 if (res == PIOS_RFM22B_TX_COMPLETE) {
1868 radio_dev->tx_complete_ticks = xTaskGetTickCount();
1869
1870 // Is this an ACK?
1871 ret_event = RADIO_EVENT_RX_MODE;
1872 radio_dev->tx_packet_handle = 0;
1873 radio_dev->tx_data_wr = radio_dev->tx_data_rd = 0;
1874 // Start a new transaction
1875 radio_dev->packet_start_ticks = 0;
1876
1877 #ifdef PIOS_RFM22B_DEBUG_ON_TELEM
1878 D1_LED_OFF;
1879 #endif
1880 }
1881
1882 return ret_event;
1883 }
1884
1885 /**
1886 * Switch the radio into receive mode.
1887 *
1888 * @param[in] rfm22b_dev The device structure
1889 * @return enum pios_radio_event The next event to inject
1890 */
1891 static enum pios_radio_event radio_setRxMode(struct pios_rfm22b_dev *rfm22b_dev)
1892 {
1893 if (!PIOS_RFM22B_ReceivePacket((uint32_t)rfm22b_dev, rfm22b_dev->rx_packet)) {
1894 return RADIO_EVENT_NUM_EVENTS;
1895 }
1896 rfm22b_dev->packet_start_ticks = 0;
1897
1898 // No event generated
1899 return RADIO_EVENT_NUM_EVENTS;
1900 }
1901
1902 /**
1903 * Complete the receipt of a packet.
1904 *
1905 * @param[in] radio_dev The device structure
1906 * @param[in] p The packet handle of the received packet.
1907 * @param[in] rc_len The number of bytes received.
1908 * @return enum pios_radio_event The next event to inject
1909 */
1910 static enum pios_radio_event radio_receivePacket(struct pios_rfm22b_dev *radio_dev, uint8_t *p, uint16_t rx_len)
1911 {
1912 bool good_packet = true;
1913 bool corrected_packet = false;
1914 uint8_t data_len = rx_len;
1915
1916 // We don't rsencode ppm only packets.
1917 if (!radio_dev->ppm_only_mode) {
1918 data_len -= RS_ECC_NPARITY;
1919
1920 // Attempt to correct any errors in the packet.
1921 if (data_len > 0) {
1922 decode_data((unsigned char *)p, rx_len);
1923 good_packet = check_syndrome() == 0;
1924
1925 // We have an error. Try to correct it.
1926 if (!good_packet && (correct_errors_erasures((unsigned char *)p, rx_len, 0, 0) != 0)) {
1927 // We corrected it
1928 corrected_packet = true;
1929 }
1930 }
1931 }
1932
1933 // Should we pull PPM data off of the head of the packet?
1934 if ((good_packet || corrected_packet) && radio_dev->ppm_recv_mode) {
1935 uint8_t ppm_len = RFM22B_PPM_NUM_CHANNELS + (radio_dev->ppm_only_mode ? 2 : 1);
1936
1937 // Ensure the packet it long enough
1938 if (data_len < ppm_len) {
1939 good_packet = false;
1940 }
1941
1942 // Verify the CRC if this is a PPM only packet.
1943 if ((good_packet || corrected_packet) && radio_dev->ppm_only_mode) {
1944 uint8_t crc = 0;
1945 for (uint8_t i = 0; i < RFM22B_PPM_NUM_CHANNELS + 1; ++i) {
1946 crc = PIOS_CRC_updateByte(crc, p[i]);
1947 }
1948 if (p[RFM22B_PPM_NUM_CHANNELS + 1] != crc) {
1949 good_packet = false;
1950 corrected_packet = false;
1951 }
1952 }
1953
1954 if (good_packet || corrected_packet) {
1955 for (uint8_t i = 0; i < RFM22B_PPM_NUM_CHANNELS; ++i) {
1956 // Calculate 9-bit value taking the LSB from byte 0
1957 uint32_t val = (p[i + 1] << 1) + ((p[0] >> i) & 1);
1958 // Is this a valid channel?
1959 if (val != RFM22B_PPM_INVALID) {
1960 radio_dev->ppm[i] = (uint16_t)(RFM22B_PPM_MIN_US + (val - RFM22B_PPM_MIN) * RFM22B_PPM_SCALE);
1961 } else {
1962 radio_dev->ppm[i] = PIOS_RCVR_INVALID;
1963 }
1964 }
1965
1966 p += RFM22B_PPM_NUM_CHANNELS + 1;
1967 data_len -= RFM22B_PPM_NUM_CHANNELS + 1;
1968
1969 // Call the PPM received callback if it's available.
1970 if (radio_dev->ppm_callback) {
1971 radio_dev->ppm_callback(radio_dev->ppm);
1972 }
1973 }
1974 }
1975
1976 // Set the packet status
1977 if (good_packet) {
1978 rfm22b_add_rx_status(radio_dev, RADIO_GOOD_RX_PACKET);
1979 } else if (corrected_packet) {
1980 // We corrected the error.
1981 rfm22b_add_rx_status(radio_dev, RADIO_CORRECTED_RX_PACKET);
1982 } else {
1983 // We couldn't correct the error, so drop the packet.
1984 rfm22b_add_rx_status(radio_dev, RADIO_ERROR_RX_PACKET);
1985 }
1986
1987 enum pios_radio_event ret_event = RADIO_EVENT_RX_COMPLETE;
1988 if (good_packet || corrected_packet) {
1989 // Send the data to the com port
1990 bool rx_need_yield;
1991 if (radio_dev->rx_in_cb && (data_len > 0) && !radio_dev->ppm_only_mode) {
1992 (radio_dev->rx_in_cb)(radio_dev->rx_in_context, p, data_len, NULL, &rx_need_yield);
1993 }
1994
1995 /*
1996 * If the packet is valid and destined for us we synchronize the clock.
1997 */
1998 if (!rfm22_isCoordinator(radio_dev) &&
1999 radio_dev->rx_destination_id == rfm22_destinationID(radio_dev)) {
2000 rfm22_synchronizeClock(radio_dev);
2001 radio_dev->stats.link_state = OPLINKSTATUS_LINKSTATE_CONNECTED;
2002 }
2003
2004 radio_dev->last_contact = xTaskGetTickCount();
2005 } else {
2006 ret_event = RADIO_EVENT_RX_COMPLETE;
2007 }
2008
2009 return ret_event;
2010 }
2011
2012 /**
2013 * Receive the packet data.
2014 *
2015 * @param[in] rfm22b_dev The device structure
2016 * @return enum pios_radio_event The next event to inject
2017 */
2018 static enum pios_radio_event radio_rxData(struct pios_rfm22b_dev *radio_dev)
2019 {
2020 enum pios_radio_event ret_event = RADIO_EVENT_NUM_EVENTS;
2021 pios_rfm22b_int_result res = PIOS_RFM22B_ProcessRx((uint32_t)radio_dev);
2022
2023 switch (res) {
2024 case PIOS_RFM22B_RX_COMPLETE:
2025
2026 // Receive the packet.
2027 ret_event = radio_receivePacket(radio_dev, radio_dev->rx_packet_handle, radio_dev->rx_buffer_wr);
2028 radio_dev->rx_buffer_wr = 0;
2029 #ifdef PIOS_RFM22B_DEBUG_ON_TELEM
2030 D2_LED_OFF;
2031 #endif
2032
2033 // Start a new transaction
2034 radio_dev->packet_start_ticks = 0;
2035 break;
2036
2037 case PIOS_RFM22B_INT_FAILURE:
2038
2039 ret_event = RADIO_EVENT_RX_MODE;
2040 break;
2041
2042 default:
2043 // do nothing.
2044 break;
2045 }
2046
2047 return ret_event;
2048 }
2049
2050 /*****************************************************************************
2051 * Link Statistics Functions
2052 *****************************************************************************/
2053
2054 /**
2055 * Update the modem pair status.
2056 *
2057 * @param[in] rfm22b_dev The device structure
2058 */
2059 static void rfm22_updatePairStatus(struct pios_rfm22b_dev *radio_dev)
2060 {
2061 int8_t rssi = radio_dev->rssi_dBm;
2062 int8_t afc = radio_dev->afc_correction_Hz;
2063 uint32_t id = radio_dev->rx_destination_id;
2064
2065 // Have we seen this device recently?
2066 bool found = false;
2067 uint8_t id_idx = 0;
2068
2069 for (; id_idx < OPLINKSTATUS_PAIRIDS_NUMELEM; ++id_idx) {
2070 if (radio_dev->pair_stats[id_idx].pairID == id) {
2071 found = true;
2072 break;
2073 }
2074 }
2075
2076 // If we have seen it, update the RSSI and reset the last contact counter
2077 if (found) {
2078 radio_dev->pair_stats[id_idx].rssi = rssi;
2079 radio_dev->pair_stats[id_idx].afc_correction = afc;
2080 radio_dev->pair_stats[id_idx].lastContact = 0;
2081 } else {
2082 // If we haven't seen it, find a slot to put it in.
2083 uint8_t min_idx = 0;
2084 int8_t min_rssi = radio_dev->pair_stats[0].rssi;
2085 for (id_idx = 1; id_idx < OPLINKSTATUS_PAIRIDS_NUMELEM; ++id_idx) {
2086 if (radio_dev->pair_stats[id_idx].rssi < min_rssi) {
2087 min_rssi = radio_dev->pair_stats[id_idx].rssi;
2088 min_idx = id_idx;
2089 }
2090 }
2091 radio_dev->pair_stats[min_idx].pairID = id;
2092 radio_dev->pair_stats[min_idx].rssi = rssi;
2093 radio_dev->pair_stats[min_idx].afc_correction = afc;
2094 radio_dev->pair_stats[min_idx].lastContact = 0;
2095 }
2096 }
2097
2098 /**
2099 * Calculate the link quality from the packet receipt, tranmittion statistics.
2100 *
2101 * @param[in] rfm22b_dev The device structure
2102 */
2103 static void rfm22_calculateLinkQuality(struct pios_rfm22b_dev *rfm22b_dev)
2104 {
2105 // Add the RX packet statistics
2106 rfm22b_dev->stats.rx_good = 0;
2107 rfm22b_dev->stats.rx_corrected = 0;
2108 rfm22b_dev->stats.rx_error = 0;
2109 rfm22b_dev->stats.rx_failure = 0;
2110 for (uint8_t i = 0; i < RFM22B_RX_PACKET_STATS_LEN; ++i) {
2111 uint32_t val = rfm22b_dev->rx_packet_stats[i];
2112 for (uint8_t j = 0; j < 16; ++j) {
2113 switch ((val >> (j * 2)) & 0x3) {
2114 case RADIO_GOOD_RX_PACKET:
2115 rfm22b_dev->stats.rx_good++;
2116 break;
2117 case RADIO_CORRECTED_RX_PACKET:
2118 rfm22b_dev->stats.rx_corrected++;
2119 break;
2120 case RADIO_ERROR_RX_PACKET:
2121 rfm22b_dev->stats.rx_error++;
2122 break;
2123 case RADIO_FAILURE_RX_PACKET:
2124 rfm22b_dev->stats.rx_failure++;
2125 break;
2126 }
2127 }
2128 }
2129
2130 // Calculate the link quality metric, which is related to the number of good packets in relation to the number of bad packets.
2131 // Note: This assumes that the number of packets sampled for the stats is 64.
2132 // Using this equation, error and resent packets are counted as -2, and corrected packets are counted as -1.
2133 // The range is 0 (all error or resent packets) to 128 (all good packets).
2134 rfm22b_dev->stats.link_quality = 64 + rfm22b_dev->stats.rx_good - rfm22b_dev->stats.rx_error - rfm22b_dev->stats.rx_failure;
2135 }
2136
2137 /**
2138 * Add a status value to the RX packet status array.
2139 *
2140 * @param[in] rfm22b_dev The device structure
2141 * @param[in] status The packet status value
2142 */
2143 static void rfm22b_add_rx_status(struct pios_rfm22b_dev *rfm22b_dev, enum pios_rfm22b_rx_packet_status status)
2144 {
2145 // Shift the status registers
2146 for (uint8_t i = RFM22B_RX_PACKET_STATS_LEN - 1; i > 0; --i) {
2147 rfm22b_dev->rx_packet_stats[i] = (rfm22b_dev->rx_packet_stats[i] << 2) | (rfm22b_dev->rx_packet_stats[i - 1] >> 30);
2148 }
2149 rfm22b_dev->rx_packet_stats[0] = (rfm22b_dev->rx_packet_stats[0] << 2) | status;
2150 }
2151
2152
2153 /*****************************************************************************
2154 * Connection Handling Functions
2155 *****************************************************************************/
2156
2157 /**
2158 * Are we a coordinator modem?
2159 *
2160 * @param[in] rfm22b_dev The device structure
2161 */
2162 static bool rfm22_isCoordinator(struct pios_rfm22b_dev *rfm22b_dev)
2163 {
2164 return rfm22b_dev->coordinator;
2165 }
2166
2167 /**
2168 * Returns the destination ID to send packets to.
2169 *
2170 * @param[in] rfm22b_id The RFM22B device index.
2171 * @return The destination ID
2172 */
2173 uint32_t rfm22_destinationID(struct pios_rfm22b_dev *rfm22b_dev)
2174 {
2175 if (rfm22_isCoordinator(rfm22b_dev)) {
2176 return rfm22b_dev->deviceID;
2177 } else if (rfm22b_dev->coordinatorID) {
2178 return rfm22b_dev->coordinatorID;
2179 } else {
2180 return 0xffffffff;
2181 }
2182 }
2183
2184
2185 /*****************************************************************************
2186 * Frequency Hopping Functions
2187 *****************************************************************************/
2188
2189 /**
2190 * Synchronize the clock after a packet receive from our coordinator on the syncronization channel.
2191 * This function should be called when a packet is received on the synchronization channel.
2192 *
2193 * @param[in] rfm22b_dev The device structure
2194 */
2195 static void rfm22_synchronizeClock(struct pios_rfm22b_dev *rfm22b_dev)
2196 {
2197 portTickType start_time = rfm22b_dev->packet_start_ticks;
2198
2199 // This packet was transmitted on channel 0, calculate the time delta that will force us to transmit on channel 0 at the time this packet started.
2200 uint16_t frequency_hop_cycle_time = rfm22b_dev->packet_time * rfm22b_dev->num_channels;
2201 uint16_t time_delta = start_time % frequency_hop_cycle_time;
2202
2203 // Calculate the adjustment for the preamble
2204 uint8_t offset = (uint8_t)ceil(35000.0F / data_rate[rfm22b_dev->datarate]);
2205
2206 rfm22b_dev->time_delta = frequency_hop_cycle_time - time_delta + offset +
2207 rfm22b_dev->packet_time * rfm22b_dev->channel_index;
2208 }
2209
2210 /**
2211 * Return the extimated current clock ticks count on the coordinator modem.
2212 * This is the master clock used for all synchronization.
2213 *
2214 * @param[in] rfm22b_dev The device structure
2215 */
2216 static portTickType rfm22_coordinatorTime(struct pios_rfm22b_dev *rfm22b_dev, portTickType ticks)
2217 {
2218 if (rfm22_isCoordinator(rfm22b_dev)) {
2219 return ticks;
2220 }
2221 return ticks + rfm22b_dev->time_delta;
2222 }
2223
2224 /**
2225 * Return true if this modem is in the send interval, which allows the modem to initiate a transmit.
2226 *
2227 * @param[in] rfm22b_dev The device structure
2228 */
2229 static bool rfm22_timeToSend(struct pios_rfm22b_dev *rfm22b_dev)
2230 {
2231 portTickType time = rfm22_coordinatorTime(rfm22b_dev, xTaskGetTickCount());
2232 bool is_coordinator = rfm22_isCoordinator(rfm22b_dev);
2233
2234 // If this is a one-way link, only the coordinator can send.
2235 uint8_t packet_period = rfm22b_dev->packet_time;
2236
2237 if (rfm22b_dev->one_way_link) {
2238 if (is_coordinator) {
2239 return ((time - 1) % (packet_period)) == 0;
2240 } else {
2241 return false;
2242 }
2243 }
2244
2245 if (!is_coordinator) {
2246 time += packet_period - 1;
2247 } else {
2248 time -= 1;
2249 }
2250 return (time % (packet_period * 2)) == 0;
2251 }
2252
2253 /**
2254 * Calculate the nth channel index.
2255 *
2256 * @param[in] rfm22b_dev The device structure
2257 * @param[in] index The channel index to calculate
2258 */
2259 static uint8_t rfm22_calcChannel(struct pios_rfm22b_dev *rfm22b_dev, uint8_t index)
2260 {
2261 // Make sure we don't index outside of the range.
2262 uint8_t idx = index % rfm22b_dev->num_channels;
2263
2264 // Are we switching to a new channel?
2265 if (idx != rfm22b_dev->channel_index) {
2266 if (!rfm22_isCoordinator(rfm22b_dev) &&
2267 pios_rfm22_time_difference_ms(rfm22b_dev->last_contact, xTaskGetTickCount()) >=
2268 CONNECTED_TIMEOUT) {
2269 // Set the link state to disconnected.
2270 if (rfm22b_dev->stats.link_state == OPLINKSTATUS_LINKSTATE_CONNECTED) {
2271 rfm22b_dev->stats.link_state = OPLINKSTATUS_LINKSTATE_DISCONNECTED;
2272 // Set the PPM outputs to INVALID
2273 for (uint8_t i = 0; i < RFM22B_PPM_NUM_CHANNELS; ++i) {
2274 rfm22b_dev->ppm[i] = PIOS_RCVR_INVALID;
2275 }
2276 }
2277
2278 // Stay on first channel.
2279 idx = 0;
2280 }
2281
2282 rfm22b_dev->channel_index = idx;
2283 }
2284
2285 return rfm22b_dev->channels[idx];
2286 }
2287
2288 /**
2289 * Calculate what the current channel shold be.
2290 *
2291 * @param[in] rfm22b_dev The device structure
2292 */
2293 static uint8_t rfm22_calcChannelFromClock(struct pios_rfm22b_dev *rfm22b_dev)
2294 {
2295 portTickType time = rfm22_coordinatorTime(rfm22b_dev, xTaskGetTickCount());
2296 // Divide time into 8ms blocks. Coordinator sends in first 2 ms, and remote send in 5th and 6th ms.
2297 // Channel changes occur in the last 2 ms.
2298 uint8_t n = (time / rfm22b_dev->packet_time) % rfm22b_dev->num_channels;
2299
2300 return rfm22_calcChannel(rfm22b_dev, n);
2301 }
2302
2303 /**
2304 * Change channels to the calculated current channel.
2305 *
2306 * @param[in] rfm22b_dev The device structure
2307 */
2308 static bool rfm22_changeChannel(struct pios_rfm22b_dev *rfm22b_dev)
2309 {
2310 // A disconnected non-coordinator modem should sit on the sync channel until connected.
2311 if (!rfm22_isCoordinator(rfm22b_dev) && !rfm22_isConnected(rfm22b_dev)) {
2312 return rfm22_setFreqHopChannel(rfm22b_dev, rfm22_calcChannel(rfm22b_dev, 0));
2313 } else {
2314 return rfm22_setFreqHopChannel(rfm22b_dev, rfm22_calcChannelFromClock(rfm22b_dev));
2315 }
2316 }
2317
2318
2319 /*****************************************************************************
2320 * Error Handling Functions
2321 *****************************************************************************/
2322
2323 /**
2324 * Recover from a transmit failure.
2325 *
2326 * @param[in] rfm22b_dev The device structure
2327 * @return enum pios_radio_event The next event to inject
2328 */
2329 static enum pios_radio_event rfm22_txFailure(struct pios_rfm22b_dev *rfm22b_dev)
2330 {
2331 rfm22b_dev->stats.tx_failure++;
2332 rfm22b_dev->packet_start_ticks = 0;
2333 rfm22b_dev->tx_data_wr = rfm22b_dev->tx_data_rd = 0;
2334 return RADIO_EVENT_TX_START;
2335 }
2336
2337 /**
2338 * Recover from a timeout event.
2339 *
2340 * @param[in] rfm22b_dev The device structure
2341 * @return enum pios_radio_event The next event to inject
2342 */
2343 static enum pios_radio_event rfm22_timeout(struct pios_rfm22b_dev *rfm22b_dev)
2344 {
2345 rfm22b_dev->stats.timeouts++;
2346 rfm22b_dev->packet_start_ticks = 0;
2347 // Release the Tx packet if it's set.
2348 if (rfm22b_dev->tx_packet_handle != 0) {
2349 rfm22b_dev->tx_data_rd = rfm22b_dev->tx_data_wr = 0;
2350 }
2351 rfm22b_dev->rfm22b_state = RFM22B_STATE_TRANSITION;
2352 rfm22b_dev->rx_buffer_wr = 0;
2353 TX_LED_OFF;
2354 RX_LED_OFF;
2355 #ifdef PIOS_RFM22B_DEBUG_ON_TELEM
2356 D1_LED_OFF;
2357 D2_LED_OFF;
2358 D3_LED_OFF;
2359 D4_LED_OFF;
2360 #endif
2361 return RADIO_EVENT_RX_MODE;
2362 }
2363
2364 /**
2365 * Recover from a severe error.
2366 *
2367 * @param[in] rfm22b_dev The device structure
2368 * @return enum pios_radio_event The next event to inject
2369 */
2370 static enum pios_radio_event rfm22_error(struct pios_rfm22b_dev *rfm22b_dev)
2371 {
2372 rfm22b_dev->stats.resets++;
2373 rfm22_clearLEDs();
2374 return RADIO_EVENT_INITIALIZE;
2375 }
2376
2377 /**
2378 * A fatal error has occured in the state machine.
2379 * this should not happen.
2380 *
2381 * @parem [in] rfm22b_dev The device structure
2382 * @return enum pios_radio_event The next event to inject
2383 */
2384 static enum pios_radio_event rfm22_fatal_error(__attribute__((unused)) struct pios_rfm22b_dev *rfm22b_dev)
2385 {
2386 // RF module error .. flash the LED's
2387 rfm22_clearLEDs();
2388 for (unsigned int j = 0; j < 16; j++) {
2389 USB_LED_ON;
2390 LINK_LED_ON;
2391 RX_LED_OFF;
2392 TX_LED_OFF;
2393
2394 PIOS_DELAY_WaitmS(200);
2395
2396 USB_LED_OFF;
2397 LINK_LED_OFF;
2398 RX_LED_ON;
2399 TX_LED_ON;
2400
2401 PIOS_DELAY_WaitmS(200);
2402 }
2403
2404 PIOS_DELAY_WaitmS(1000);
2405
2406 PIOS_Assert(0);
2407
2408 return RADIO_EVENT_FATAL_ERROR;
2409 }
2410
2411
2412 /*****************************************************************************
2413 * Utility Functions
2414 *****************************************************************************/
2415
2416 /**
2417 * Calculate the time difference between the start time and end time.
2418 * Times are in ticks. Also handles rollover.
2419 *
2420 * @param[in] start_time The start time in ticks.
2421 * @param[in] end_time The end time in ticks.
2422 */
2423 static uint32_t pios_rfm22_time_difference_ms(portTickType start_time, portTickType end_time)
2424 {
2425 if (end_time >= start_time) {
2426 return (end_time - start_time) * portTICK_RATE_MS;
2427 }
2428 // Rollover
2429 return ((portMAX_DELAY - start_time) + end_time) * portTICK_RATE_MS;
2430 }
2431
2432 /**
2433 * Allocate the device structure
2434 */
2435 #if defined(PIOS_INCLUDE_FREERTOS)
2436 static struct pios_rfm22b_dev *pios_rfm22_alloc(void)
2437 {
2438 struct pios_rfm22b_dev *rfm22b_dev;
2439
2440 rfm22b_dev = (struct pios_rfm22b_dev *)pios_malloc(sizeof(*rfm22b_dev));
2441 rfm22b_dev->spi_id = 0;
2442 if (!rfm22b_dev) {
2443 return NULL;
2444 }
2445
2446 rfm22b_dev->magic = PIOS_RFM22B_DEV_MAGIC;
2447 return rfm22b_dev;
2448 }
2449 #else
2450 static struct pios_rfm22b_dev pios_rfm22b_devs[PIOS_RFM22B_MAX_DEVS];
2451 static uint8_t pios_rfm22b_num_devs;
2452 static struct pios_rfm22b_dev *pios_rfm22_alloc(void)
2453 {
2454 struct pios_rfm22b_dev *rfm22b_dev;
2455
2456 if (pios_rfm22b_num_devs >= PIOS_RFM22B_MAX_DEVS) {
2457 return NULL;
2458 }
2459
2460 rfm22b_dev = &pios_rfm22b_devs[pios_rfm22b_num_devs++];
2461 rfm22b_dev->magic = PIOS_RFM22B_DEV_MAGIC;
2462
2463 return rfm22b_dev;
2464 }
2465 #endif /* if defined(PIOS_INCLUDE_FREERTOS) */
2466
2467 /**
2468 * Turn off all of the LEDs
2469 */
2470 static void rfm22_clearLEDs(void)
2471 {
2472 LINK_LED_OFF;
2473 RX_LED_OFF;
2474 TX_LED_OFF;
2475 #ifdef PIOS_RFM22B_DEBUG_ON_TELEM
2476 D1_LED_OFF;
2477 D2_LED_OFF;
2478 D3_LED_OFF;
2479 D4_LED_OFF;
2480 #endif
2481 }
2482
2483
2484 /*****************************************************************************
2485 * SPI Read/Write Functions
2486 *****************************************************************************/
2487
2488 /**
2489 * Assert the chip select line.
2490 *
2491 * @param[in] rfm22b_dev The RFM22B device.
2492 */
2493 static void rfm22_assertCs(struct pios_rfm22b_dev *rfm22b_dev)
2494 {
2495 PIOS_DELAY_WaituS(1);
2496 if (rfm22b_dev->spi_id != 0) {
2497 PIOS_SPI_RC_PinSet(rfm22b_dev->spi_id, rfm22b_dev->slave_num, 0);
2498 }
2499 }
2500
2501 /**
2502 * Deassert the chip select line.
2503 *
2504 * @param[in] rfm22b_dev The RFM22B device structure pointer.
2505 */
2506 static void rfm22_deassertCs(struct pios_rfm22b_dev *rfm22b_dev)
2507 {
2508 if (rfm22b_dev->spi_id != 0) {
2509 PIOS_SPI_RC_PinSet(rfm22b_dev->spi_id, rfm22b_dev->slave_num, 1);
2510 }
2511 }
2512
2513 /**
2514 * Claim the SPI bus.
2515 *
2516 * @param[in] rfm22b_dev The RFM22B device structure pointer.
2517 */
2518 static void rfm22_claimBus(struct pios_rfm22b_dev *rfm22b_dev)
2519 {
2520 if (rfm22b_dev->spi_id != 0) {
2521 PIOS_SPI_ClaimBus(rfm22b_dev->spi_id);
2522 }
2523 }
2524
2525 /**
2526 * Release the SPI bus.
2527 *
2528 * @param[in] rfm22b_dev The RFM22B device structure pointer.
2529 */
2530 static void rfm22_releaseBus(struct pios_rfm22b_dev *rfm22b_dev)
2531 {
2532 if (rfm22b_dev->spi_id != 0) {
2533 PIOS_SPI_ReleaseBus(rfm22b_dev->spi_id);
2534 }
2535 }
2536
2537 /**
2538 * Claim the semaphore and write a byte to a register
2539 *
2540 * @param[in] rfm22b_dev The RFM22B device.
2541 * @param[in] addr The address to write to
2542 * @param[in] data The datat to write to that address
2543 */
2544 static void rfm22_write_claim(struct pios_rfm22b_dev *rfm22b_dev, uint8_t addr, uint8_t data)
2545 {
2546 rfm22_claimBus(rfm22b_dev);
2547 rfm22_assertCs(rfm22b_dev);
2548 uint8_t buf[2] = { addr | 0x80, data };
2549 PIOS_SPI_TransferBlock(rfm22b_dev->spi_id, buf, NULL, sizeof(buf), NULL);
2550 rfm22_deassertCs(rfm22b_dev);
2551 rfm22_releaseBus(rfm22b_dev);
2552 }
2553
2554 /**
2555 * Write a byte to a register without claiming the semaphore
2556 *
2557 * @param[in] rfm22b_dev The RFM22B device.
2558 * @param[in] addr The address to write to
2559 * @param[in] data The datat to write to that address
2560 */
2561 static void rfm22_write(struct pios_rfm22b_dev *rfm22b_dev, uint8_t addr, uint8_t data)
2562 {
2563 rfm22_assertCs(rfm22b_dev);
2564 uint8_t buf[2] = { addr | 0x80, data };
2565 PIOS_SPI_TransferBlock(rfm22b_dev->spi_id, buf, NULL, sizeof(buf), NULL);
2566 rfm22_deassertCs(rfm22b_dev);
2567 }
2568
2569 /**
2570 * Read a byte from an RFM22b register without claiming the bus
2571 *
2572 * @param[in] rfm22b_dev The RFM22B device structure pointer.
2573 * @param[in] addr The address to read from
2574 * @return Returns the result of the register read
2575 */
2576 static uint8_t rfm22_read(struct pios_rfm22b_dev *rfm22b_dev, uint8_t addr)
2577 {
2578 uint8_t out[2] = { addr &0x7F, 0xFF };
2579 uint8_t in[2];
2580
2581 rfm22_assertCs(rfm22b_dev);
2582 PIOS_SPI_TransferBlock(rfm22b_dev->spi_id, out, in, sizeof(out), NULL);
2583 rfm22_deassertCs(rfm22b_dev);
2584 return in[1];
2585 }
2586
2587
2588 static void rfm22_hmac_sha1(const uint8_t *data, size_t len,
2589 uint8_t key[SHA1_DIGEST_LENGTH],
2590 uint8_t digest[SHA1_DIGEST_LENGTH])
2591 {
2592 uint8_t ipad[64] = { 0 };
2593 uint8_t opad[64] = { 0 };
2594 static SHA1_CTX *ctx;
2595
2596 ctx = pios_malloc(sizeof(SHA1_CTX));
2597
2598 memcpy(ipad, key, SHA1_DIGEST_LENGTH);
2599 memcpy(opad, key, SHA1_DIGEST_LENGTH);
2600
2601 for (int i = 0; i < 64; i++) {
2602 ipad[i] ^= 0x36;
2603 opad[i] ^= 0x5c;
2604 }
2605
2606 SHA1Init(ctx);
2607 SHA1Update(ctx, ipad, sizeof(ipad));
2608 SHA1Update(ctx, data, len);
2609 SHA1Final(digest, ctx);
2610
2611 SHA1Init(ctx);
2612 SHA1Update(ctx, opad, sizeof(opad));
2613 SHA1Update(ctx, digest, SHA1_DIGEST_LENGTH);
2614 SHA1Final(digest, ctx);
2615
2616 pios_free(ctx);
2617 }
2618
2619 static bool rfm22_gen_channels(uint32_t coordid, enum rfm22b_datarate rate, uint8_t min,
2620 uint8_t max, uint8_t channels[MAX_CHANNELS], uint8_t *clen)
2621 {
2622 uint32_t data = 0;
2623 uint8_t cpos = 0;
2624 uint8_t chan_range = (max / channel_spacing[rate] - min / channel_spacing[rate]) + 1;
2625 uint8_t key[SHA1_DIGEST_LENGTH] = { 0 };
2626 uint8_t digest[SHA1_DIGEST_LENGTH];
2627 uint8_t *all_channels;
2628
2629 all_channels = pios_malloc(RFM22B_NUM_CHANNELS);
2630
2631 memcpy(key, &coordid, sizeof(coordid));
2632
2633 for (int i = 0; i < chan_range; i++) {
2634 all_channels[i] = min / channel_spacing[rate] + i;
2635 }
2636
2637 int j = SHA1_DIGEST_LENGTH;
2638 for (int i = 0; i < chan_range && i < MAX_CHANNELS; i++) {
2639 uint8_t rnd;
2640 uint8_t r;
2641 uint8_t tmp;
2642
2643 if (j == SHA1_DIGEST_LENGTH) {
2644 rfm22_hmac_sha1((uint8_t *)&data, sizeof(data), key, digest);
2645 j = 0;
2646 data++;
2647 }
2648 rnd = digest[j];
2649 j++;
2650 r = rnd % (chan_range - i) + i;
2651 tmp = all_channels[i];
2652 all_channels[i] = all_channels[r];
2653 all_channels[r] = tmp;
2654 }
2655
2656 for (int i = 0; i < chan_range && cpos < MAX_CHANNELS; i++, cpos++) {
2657 channels[cpos] = all_channels[i] * channel_spacing[rate];
2658 }
2659
2660 *clen = cpos & 0xfe;
2661
2662 pios_free(all_channels);
2663
2664 return *clen > 0;
2665 }
2666
2667 #endif /* PIOS_INCLUDE_RFM22B */
2668
2669 /**
2670 * @}
2671 * @}
2672 */