From afb8a20b6f56082c852673110eb423cfe68d5902 Mon Sep 17 00:00:00 2001 From: "Richard Flay (Hyper)" Date: Sun, 14 Apr 2013 11:52:56 +0930 Subject: [PATCH 1/9] OP-913: tweaked some UAVO related struct alignments to attempt to ensure that UAVO data fields end up being 4 byte aligned on the heap. Also fixed a memset issue in the initialisation code for multi-instance UAVOs . +review OPReview --- .../targets/UAVObjects/inc/uavobjectmanager.h | 2 +- flight/targets/UAVObjects/uavobjectmanager.c | 17 ++++++++--------- 2 files changed, 9 insertions(+), 10 deletions(-) diff --git a/flight/targets/UAVObjects/inc/uavobjectmanager.h b/flight/targets/UAVObjects/inc/uavobjectmanager.h index 9383e2e84..53183f497 100644 --- a/flight/targets/UAVObjects/inc/uavobjectmanager.h +++ b/flight/targets/UAVObjects/inc/uavobjectmanager.h @@ -75,7 +75,7 @@ typedef enum { * 6-7 gcsTelemetryUpdateMode Update mode used by the GCS (UAVObjUpdateMode) */ typedef struct { - uint8_t flags; /** Defines flags for update and logging modes and whether an update should be ACK'd (bits defined above) */ + uint8_t flags; /** Defines flags for update and logging modes and whether an update should be ACK'd (bits defined above) */ uint16_t telemetryUpdatePeriod; /** Update period used by the telemetry module (only if telemetry mode is PERIODIC) */ uint16_t gcsTelemetryUpdatePeriod; /** Update period used by the GCS (only if telemetry mode is PERIODIC) */ uint16_t loggingUpdatePeriod; /** Update period used by the logging module (only if logging mode is PERIODIC) */ diff --git a/flight/targets/UAVObjects/uavobjectmanager.c b/flight/targets/UAVObjects/uavobjectmanager.c index 1f07e0c9c..2af6a7a05 100644 --- a/flight/targets/UAVObjects/uavobjectmanager.c +++ b/flight/targets/UAVObjects/uavobjectmanager.c @@ -60,10 +60,10 @@ for (struct UAVOData ** _uavo_slot = __start__uavo_handles; \ typedef void* InstanceHandle; struct ObjectEventEntry { + struct ObjectEventEntry * next; xQueueHandle queue; UAVObjEventCallback cb; uint8_t eventMask; - struct ObjectEventEntry * next; }; /* @@ -91,7 +91,6 @@ struct UAVOBase { bool isSingle : 1; bool isSettings : 1; } flags; - } __attribute__((packed)); /* Augmented type for Meta UAVO */ @@ -110,7 +109,7 @@ struct UAVOData { */ struct UAVOMeta metaObj; uint16_t instance_size; -} __attribute__((packed)); +} __attribute__((packed, aligned(4))); /* Augmented type for Single Instance Data UAVO */ struct UAVOSingle { @@ -136,9 +135,8 @@ struct UAVOMultiInst { /* Augmented type for Multi Instance Data UAVO */ struct UAVOMulti { struct UAVOData uavo; - uint16_t num_instances; - struct UAVOMultiInst instance0; + struct UAVOMultiInst instance0 __attribute__((aligned(4))); /* * Additional space will be malloc'd here to hold the * the data for instance 0. @@ -296,8 +294,8 @@ static struct UAVOData * UAVObjAllocMulti(uint32_t num_bytes) /* Set up the type-specific part of the UAVO */ uavo_multi->num_instances = 1; - /* Clear the instance data carried in the UAVO */ - memset (&(uavo_multi->instance0), 0, num_bytes); + /* Clear the multi instance data carried in the UAVO */ + memset (&(uavo_multi->instance0), 0, sizeof(struct UAVOMultiInst) + num_bytes); /* Give back the generic UAVO part */ return (&(uavo_multi->uavo)); @@ -1864,10 +1862,11 @@ static InstanceHandle createInstance(struct UAVOData * obj, uint16_t instId) } /* Create the actual instance */ - instEntry = (struct UAVOMultiInst *) pvPortMalloc(sizeof(struct UAVOMultiInst)+obj->instance_size); + uint32_t size = sizeof(struct UAVOMultiInst) + obj->instance_size; + instEntry = (struct UAVOMultiInst *) pvPortMalloc(size); if (!instEntry) return NULL; - memset(InstanceDataOffset(instEntry), 0, obj->instance_size); + memset(instEntry, 0, size); LL_APPEND(( (struct UAVOMulti*)obj )->instance0.next, instEntry); ( (struct UAVOMulti*)obj )->num_instances++; From 146e082e74c34ff17ebdcc72da740956f13aeac4 Mon Sep 17 00:00:00 2001 From: "Richard Flay (Hyper)" Date: Tue, 23 Apr 2013 22:32:38 +0930 Subject: [PATCH 2/9] OP-913: ensured that UAVO data structs in .bss/.data sections and on the stack have 4 byte alignment. Also changed linker options to sort common and sort sections by alignment, which reduces the amount of fill/padding in the .bss and .data sections. +review OPReview-444 --- flight/PiOS/inc/pios_debug.h | 6 ++++++ flight/targets/UAVObjects/inc/uavobjecttemplate.h | 12 ++++++++++-- flight/targets/UAVObjects/uavobjecttemplate.c | 5 +++++ make/common-defs.mk | 2 +- 4 files changed, 22 insertions(+), 3 deletions(-) diff --git a/flight/PiOS/inc/pios_debug.h b/flight/PiOS/inc/pios_debug.h index 4bc1e0802..6cd151e30 100644 --- a/flight/PiOS/inc/pios_debug.h +++ b/flight/PiOS/inc/pios_debug.h @@ -68,6 +68,12 @@ void PIOS_DEBUG_Panic(const char *msg); #define PIOS_Assert(test) if (!(test)) while (1); #endif +/* Static (compile-time) assertion for use in a function. + If test evaluates to 0 (ie fails) at compile time then compilation will + fail with the error: "size of unnamed array is negative" */ +#define PIOS_STATIC_ASSERT(test) ((void)sizeof(int[1 - 2*!(test)])) + + #endif /* PIOS_DEBUG_H */ /** diff --git a/flight/targets/UAVObjects/inc/uavobjecttemplate.h b/flight/targets/UAVObjects/inc/uavobjecttemplate.h index 8aee9ded0..5734f5960 100644 --- a/flight/targets/UAVObjects/inc/uavobjecttemplate.h +++ b/flight/targets/UAVObjects/inc/uavobjecttemplate.h @@ -51,11 +51,19 @@ int32_t $(NAME)Initialize(); UAVObjHandle $(NAME)Handle(); void $(NAME)SetDefaults(UAVObjHandle obj, uint16_t instId); -// Object data +// Packed Object data (unaligned). +// Should only be used where 4 byte alignment can be guaranteed +// (eg a single instance on the heap) typedef struct { $(DATAFIELDS) -} __attribute__((packed)) $(NAME)Data; +} __attribute__((packed)) $(NAME)DataPacked; +// Packed Object data. +// Alignment is forced to 4 bytes so as to avoid the potential for CPU usage faults +// on Cortex M4F during load/store of float UAVO fields +typedef $(NAME)DataPacked __attribute__((aligned(4))) $(NAME)Data; + + // Typesafe Object access functions /** * @function $(NAME)Get(dataOut) diff --git a/flight/targets/UAVObjects/uavobjecttemplate.c b/flight/targets/UAVObjects/uavobjecttemplate.c index 808faf496..271cb5c3a 100644 --- a/flight/targets/UAVObjects/uavobjecttemplate.c +++ b/flight/targets/UAVObjects/uavobjecttemplate.c @@ -49,6 +49,11 @@ static UAVObjHandle handle __attribute__((section("_uavo_handles"))); */ int32_t $(NAME)Initialize(void) { + // Compile time assertion that the $(NAME)DataPacked and $(NAME)Data structs + // have the same size (though instances of $(NAME)Data + // should be placed in memory by the linker/compiler on a 4 byte alignment). + PIOS_STATIC_ASSERT(sizeof($(NAME)DataPacked) == sizeof($(NAME)Data)); + // Don't set the handle to null if already registered if(UAVObjGetByID($(NAMEUC)_OBJID) != NULL) return -2; diff --git a/make/common-defs.mk b/make/common-defs.mk index 97ee3a818..25b8f9bc1 100644 --- a/make/common-defs.mk +++ b/make/common-defs.mk @@ -153,7 +153,7 @@ ASFLAGS += -Wa,-adhlns=$(addprefix $(OUTDIR)/, $(notdir $(addsuffix .lst, $(base # -Map: create map file # --cref: add cross reference to map file LDFLAGS += -nostartfiles -LDFLAGS += -Wl,--warn-common,--fatal-warnings,--gc-sections +LDFLAGS += -Wl,--warn-common,--fatal-warnings,--sort-common,--sort-section=alignment,--gc-sections LDFLAGS += -Wl,-Map=$(OUTDIR)/$(TARGET).map,--cref LDFLAGS += $(patsubst %,-L%,$(EXTRA_LIBDIRS)) LDFLAGS += $(patsubst %,-l%,$(EXTRA_LIBS)) From b3de04d09f5c65d36fec799db1f68a782cbefff9 Mon Sep 17 00:00:00 2001 From: Philippe Vanhaesendonck Date: Wed, 24 Apr 2013 22:39:39 +0200 Subject: [PATCH 3/9] Handle Mach-o object format for UAVOs Jira OP-921 --- flight/targets/UAVObjects/uavobjectmanager.c | 25 ++++++++++++++++++- flight/targets/UAVObjects/uavobjecttemplate.c | 4 +++ make/templates/firmwareinfotemplate.c | 4 +++ 3 files changed, 32 insertions(+), 1 deletion(-) diff --git a/flight/targets/UAVObjects/uavobjectmanager.c b/flight/targets/UAVObjects/uavobjectmanager.c index 1f07e0c9c..13ba7b1ff 100644 --- a/flight/targets/UAVObjects/uavobjectmanager.c +++ b/flight/targets/UAVObjects/uavobjectmanager.c @@ -34,6 +34,10 @@ #include "openpilot.h" #include "pios_struct_helper.h" +#if (defined(__MACH__) && defined(__APPLE__)) +#include +#endif + // Constants // Private types @@ -41,9 +45,21 @@ // Macros #define SET_BITS(var, shift, value, mask) var = (var & ~(mask << shift)) | (value << shift); -/* Table of UAVO handles registered at compile time */ +// Mach-o: dummy segment to calculate ASLR offset in sim_osx +#if (defined(__MACH__) && defined(__APPLE__)) +static long _aslr_offset __attribute__((section("__DATA,_aslr"))); +#endif + +/* Table of UAVO handles */ +#if (defined(__MACH__) && defined(__APPLE__)) +/* Mach-o format */ +static struct UAVOData ** __start__uavo_handles; +static struct UAVOData ** __stop__uavo_handles; +#else +/* ELF format: automagically defined at compile time */ extern struct UAVOData * __start__uavo_handles[] __attribute__((weak)); extern struct UAVOData * __stop__uavo_handles[] __attribute__((weak)); +#endif #define UAVO_LIST_ITERATE(_item) \ for (struct UAVOData ** _uavo_slot = __start__uavo_handles; \ @@ -199,6 +215,13 @@ int32_t UAVObjInitialize() // Initialize variables memset(&stats, 0, sizeof(UAVObjStats)); + /* Initialize _uavo_handles start/stop pointers */ + #if (defined(__MACH__) && defined(__APPLE__)) + uint64_t aslr_offset = (uint64_t) & _aslr_offset - getsectbyname("__DATA","_aslr")->addr; + __start__uavo_handles = (struct UAVOData **) (getsectbyname("__DATA","_uavo_handles")->addr + aslr_offset); + __stop__uavo_handles = (struct UAVOData **) ((uint64_t)__start__uavo_handles + getsectbyname("__DATA","_uavo_handles")->size); + #endif + // Initialize the uavo handle table memset(__start__uavo_handles, 0, (uintptr_t)__stop__uavo_handles - (uintptr_t)__start__uavo_handles); diff --git a/flight/targets/UAVObjects/uavobjecttemplate.c b/flight/targets/UAVObjects/uavobjecttemplate.c index 808faf496..9af13641b 100644 --- a/flight/targets/UAVObjects/uavobjecttemplate.c +++ b/flight/targets/UAVObjects/uavobjecttemplate.c @@ -40,7 +40,11 @@ #include "$(NAMELC).h" // Private variables +#if (defined(__MACH__) && defined(__APPLE__)) +static UAVObjHandle handle __attribute__((section("__DATA,_uavo_handles"))); +#else static UAVObjHandle handle __attribute__((section("_uavo_handles"))); +#endif /** * Initialize object. diff --git a/make/templates/firmwareinfotemplate.c b/make/templates/firmwareinfotemplate.c index 6e4fbf8b3..e686ac2b4 100644 --- a/make/templates/firmwareinfotemplate.c +++ b/make/templates/firmwareinfotemplate.c @@ -55,7 +55,11 @@ struct __attribute__((packed)) fw_version_info { uint8_t pad[20]; }; +#if (defined(__MACH__) && defined(__APPLE__)) +const struct fw_version_info fw_version_blob __attribute__((used)) __attribute__((__section__("__TEXT,.fw_version_blob"))) = { +#else const struct fw_version_info fw_version_blob __attribute__((used)) __attribute__((__section__(".fw_version_blob"))) = { +#endif .magic = { 'O','p','F','w' }, .commit_hash_prefix = 0x${HASH8}, .timestamp = ${UNIXTIME}, From 77b99cdafab8d4c0840ed40fcd8f922fd7d77f13 Mon Sep 17 00:00:00 2001 From: Brian Webb Date: Sat, 13 Apr 2013 04:10:40 +0100 Subject: [PATCH 4/9] OP-914 - Converted PIOS_RFM22B_Validate back into a normal function from an inline. --- flight/pios/common/pios_rfm22b.c | 627 +++++++++++++---------------- flight/pios/inc/pios_rfm22b_priv.h | 44 -- 2 files changed, 279 insertions(+), 392 deletions(-) diff --git a/flight/pios/common/pios_rfm22b.c b/flight/pios/common/pios_rfm22b.c index a523e7162..eca884a1a 100644 --- a/flight/pios/common/pios_rfm22b.c +++ b/flight/pios/common/pios_rfm22b.c @@ -79,9 +79,6 @@ // The maximum amount of time without activity before initiating a reset. #define PIOS_RFM22B_SUPERVISOR_TIMEOUT 100 // ms -// The time between connection attempts when not connected -#define CONNECT_ATTEMPT_PERIOD_MS 250 // ms - // The time between updates for sending stats the radio link. #define RADIOSTATS_UPDATE_PERIOD_MS 250 @@ -93,10 +90,6 @@ // this is too adjust the RF module so that it is on frequency #define OSC_LOAD_CAP 0x7F // cap = 12.5pf .. default -#define OSC_LOAD_CAP_1 0x7D // board 1 -#define OSC_LOAD_CAP_2 0x7B // board 2 -#define OSC_LOAD_CAP_3 0x7E // board 3 -#define OSC_LOAD_CAP_4 0x7F // board 4 // ************************************ @@ -176,6 +169,7 @@ static const uint8_t OUT_FF[64] = { static void PIOS_RFM22B_Task(void *parameters); static bool rfm22_readStatus(struct pios_rfm22b_dev *rfm22b_dev); static void rfm22_setDatarate(struct pios_rfm22b_dev * rfm22b_dev, enum rfm22b_datarate datarate, bool data_whitening); +static void pios_rfm22_inject_event(struct pios_rfm22b_dev *rfm22b_dev, enum pios_rfm22b_event event, bool inISR); static enum pios_rfm22b_event rfm22_init(struct pios_rfm22b_dev *rfm22b_dev); static enum pios_rfm22b_event rfm22_setRxMode(struct pios_rfm22b_dev *rfm22b_dev); static enum pios_rfm22b_event rfm22_detectPreamble(struct pios_rfm22b_dev *rfm22b_dev); @@ -222,7 +216,7 @@ static void rfm22_write(struct pios_rfm22b_dev *rfm22b_dev, uint8_t addr, uint8_ static uint8_t rfm22_read(struct pios_rfm22b_dev *rfm22b_dev, uint8_t addr); static uint8_t rfm22_read_noclaim(struct pios_rfm22b_dev *rfm22b_dev, uint8_t addr); -/* Te state transition table */ +/* The state transition table */ const static struct pios_rfm22b_transition rfm22b_transitions[RFM22B_STATE_NUM_STATES] = { // Initialization thread @@ -438,7 +432,6 @@ const static struct pios_rfm22b_transition rfm22b_transitions[RFM22B_STATE_NUM_S }; // xtal 10 ppm, 434MHz -#define LOOKUP_SIZE 15 static const uint32_t data_rate[] = { 500, 1000, 2000, 4000, 8000, 9600, 16000, 19200, 24000, 32000, 57600, 64000, 128000, 192000, 256000}; static const uint8_t modulation_index[] = { 16, 8, 4, 2, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1}; @@ -607,7 +600,7 @@ int32_t PIOS_RFM22B_Init(uint32_t *rfm22b_id, uint32_t spi_id, uint32_t slave_nu rfm22b_dev->state = RFM22B_STATE_UNINITIALIZED; // Initialize the radio device. - PIOS_RFM22B_InjectEvent(rfm22b_dev, RFM22B_EVENT_INITIALIZE, false); + pios_rfm22_inject_event(rfm22b_dev, RFM22B_EVENT_INITIALIZE, false); // Start the driver task. This task controls the radio state machine and removed all of the IO from the IRQ handler. xTaskCreate(PIOS_RFM22B_Task, (signed char *)"PIOS_RFM22B_Task", STACK_SIZE_BYTES, (void*)rfm22b_dev, TASK_PRIORITY, &(rfm22b_dev->taskHandle)); @@ -622,7 +615,7 @@ void PIOS_RFM22B_Reinit(uint32_t rfm22b_id) { struct pios_rfm22b_dev *rfm22b_dev = (struct pios_rfm22b_dev *)rfm22b_id; if (PIOS_RFM22B_validate(rfm22b_dev)) - PIOS_RFM22B_InjectEvent(rfm22b_dev, RFM22B_EVENT_INITIALIZE, false); + pios_rfm22_inject_event(rfm22b_dev, RFM22B_EVENT_INITIALIZE, false); } /** @@ -634,41 +627,10 @@ bool PIOS_RFM22_EXT_Int(void) return false; // Inject an interrupt event into the state machine. - PIOS_RFM22B_InjectEvent(g_rfm22b_dev, RFM22B_EVENT_INT_RECEIVED, true); + pios_rfm22_inject_event(g_rfm22b_dev, RFM22B_EVENT_INT_RECEIVED, true); return false; } -/** - * Inject an event into the RFM22B state machine. - * \param[in] rfm22b_dev The device structure - * \param[in] event The event to inject - * \param[in] inISR Is this being called from an interrrup service routine? - */ -void PIOS_RFM22B_InjectEvent(struct pios_rfm22b_dev *rfm22b_dev, enum pios_rfm22b_event event, bool inISR) -{ - - // Store the event. - if (xQueueSend(rfm22b_dev->eventQueue, &event, portMAX_DELAY) != pdTRUE) - return; - - // Signal the semaphore to wake up the handler thread. - if (inISR) { - portBASE_TYPE pxHigherPriorityTaskWoken; - if (xSemaphoreGiveFromISR(rfm22b_dev->isrPending, &pxHigherPriorityTaskWoken) != pdTRUE) { - // Something went fairly seriously wrong - rfm22b_dev->errors++; - } - portEND_SWITCHING_ISR(pxHigherPriorityTaskWoken); - } - else - { - if (xSemaphoreGive(rfm22b_dev->isrPending) != pdTRUE) { - // Something went fairly seriously wrong - rfm22b_dev->errors++; - } - } -} - /** * Returns the unique device ID for the RFM22B device. * \param[in] rfm22b_id The RFM22B device index. @@ -846,40 +808,6 @@ bool PIOS_RFM22B_LinkStatus(uint32_t rfm22b_id) return (rfm22_isConnected(rfm22b_dev) && (rfm22b_dev->stats.link_quality > RFM22B_LINK_QUALITY_THRESHOLD)); } -/** - * Send a PPM packet with the given channel values. - * \param[in] rfm22b_id The rfm22b device. - * \param[in] channels The channel values. - * \param[in] nchannels The number of channels. - * Returns true if there is a valid connection to paired radio, false otherwise. - */ -void PIOS_RFM22B_SendPPM(uint32_t rfm22b_id, const uint16_t *channels, uint8_t nchannels) -{ -#ifdef PIOS_PPM_RECEIVER - struct pios_rfm22b_dev *rfm22b_dev = (struct pios_rfm22b_dev *)rfm22b_id; - if (!PIOS_RFM22B_validate(rfm22b_dev)) { - return; - } - - // Only send PPM if we're connected - if (!rfm22_isConnected(rfm22b_dev)) { - return; - } - - // See if we have any valid channels. - uint8_t nchan = (nchannels <= PIOS_PPM_NUM_INPUTS) ? nchannels : PIOS_PPM_NUM_INPUTS; - for (uint8_t i = 0; i < nchan; ++i) { - rfm22b_dev->ppm_packet.channels[i] = channels[i]; - } - - // Send the PPM packet. - rfm22b_dev->ppm_packet.header.destination_id = rfm22b_dev->destination_id; - rfm22b_dev->ppm_packet.header.type = PACKET_TYPE_PPM; - rfm22b_dev->ppm_packet.header.data_size = PH_PPM_DATA_SIZE(&(rfm22b_dev->ppm_packet)); - rfm22b_dev->send_ppm = true; -#endif -} - /** * The task that controls the radio state machine. */ @@ -996,31 +924,284 @@ static void PIOS_RFM22B_Task(void *parameters) } } -// ************************************ -// radio datarate about 19200 Baud -// radio frequency deviation 45kHz -// radio receiver bandwidth 67kHz. -// -// Carson's rule: -// The signal bandwidth is about 2(Delta-f + fm) .. -// -// Delta-f = frequency deviation -// fm = maximum frequency of the signal -// -// This gives 2(45 + 9.6) = 109.2kHz. - -static void rfm22_setDatarate(struct pios_rfm22b_dev * rfm22b_dev, enum rfm22b_datarate datarate, bool data_whitening) +/** + * Initialize (or re-initialize) the RFM22B radio device. + * + * \param[in] rfm22b_dev The device structure + */ +static enum pios_rfm22b_event rfm22_init(struct pios_rfm22b_dev *rfm22b_dev) { - uint32_t datarate_bps = data_rate[datarate]; - rfm22b_dev->max_packet_time = (uint16_t)((float)(PIOS_PH_MAX_PACKET * 8 * 1000) / (float)(datarate_bps) + 0.5f); - if (rfm22_isConnected(rfm22b_dev)) + + // Initialize the register values. + rfm22b_dev->device_status = 0; + rfm22b_dev->int_status1 = 0; + rfm22b_dev->int_status2 = 0; + rfm22b_dev->ezmac_status = 0; + + // Clean the LEDs + rfm22_clearLEDs(); + + // Initialize the detected device statistics. + for (uint8_t i = 0; i < OPLINKSTATUS_PAIRIDS_NUMELEM; ++i) { + rfm22b_dev->pair_stats[i].pairID = 0; + rfm22b_dev->pair_stats[i].rssi = -127; + rfm22b_dev->pair_stats[i].afc_correction = 0; + rfm22b_dev->pair_stats[i].lastContact = 0; + } + + // Initlize the link stats. + for (uint8_t i = 0; i < RFM22B_RX_PACKET_STATS_LEN; ++i) + rfm22b_dev->rx_packet_stats[i] = 0; + + // Initialize the state + rfm22b_dev->stats.link_state = OPLINKSTATUS_LINKSTATE_DISCONNECTED; + rfm22b_dev->destination_id = 0xffffffff; + rfm22b_dev->time_to_send = false; + rfm22b_dev->time_to_send_offset = 0; + rfm22b_dev->send_status = false; + rfm22b_dev->send_connection_request = false; + + // Initialize the packets. + rfm22b_dev->rx_packet_len = 0; + rfm22b_dev->tx_packet = NULL; + rfm22b_dev->prev_tx_packet = NULL; + rfm22b_dev->data_packet.header.data_size = 0; + rfm22b_dev->in_rx_mode = false; + + // Initialize the devide state + rfm22b_dev->device_status = rfm22b_dev->int_status1 = rfm22b_dev->int_status2 = rfm22b_dev->ezmac_status = 0; + rfm22b_dev->rx_buffer_wr = 0; + rfm22b_dev->tx_data_rd = rfm22b_dev->tx_data_wr = 0; + rfm22b_dev->frequency_hop_channel = 0; + rfm22b_dev->afc_correction_Hz = 0; + rfm22b_dev->packet_start_ticks = 0; + rfm22b_dev->tx_complete_ticks = 0; + rfm22b_dev->rx_complete_ticks = 0; + + // software reset the RF chip .. following procedure according to Si4x3x Errata (rev. B) + rfm22_write(rfm22b_dev, RFM22_op_and_func_ctrl1, RFM22_opfc1_swres); + + for (int i = 50; i > 0; i--) { + // read the status registers + rfm22b_dev->int_status1 = rfm22_read(rfm22b_dev, RFM22_interrupt_status1); + rfm22b_dev->int_status2 = rfm22_read(rfm22b_dev, RFM22_interrupt_status2); + if (rfm22b_dev->int_status2 & RFM22_is2_ichiprdy) break; + + // wait 1ms + PIOS_DELAY_WaitmS(1); + } + + // **************** + + // read status - clears interrupt + rfm22b_dev->device_status = rfm22_read(rfm22b_dev, RFM22_device_status); + rfm22b_dev->int_status1 = rfm22_read(rfm22b_dev, RFM22_interrupt_status1); + rfm22b_dev->int_status2 = rfm22_read(rfm22b_dev, RFM22_interrupt_status2); + rfm22b_dev->ezmac_status = rfm22_read(rfm22b_dev, RFM22_ezmac_status); + + // disable all interrupts + rfm22_write(rfm22b_dev, RFM22_interrupt_enable1, 0x00); + rfm22_write(rfm22b_dev, RFM22_interrupt_enable2, 0x00); + + // read the RF chip ID bytes + + // read the device type + uint8_t device_type = rfm22_read(rfm22b_dev, RFM22_DEVICE_TYPE) & RFM22_DT_MASK; + // read the device version + uint8_t device_version = rfm22_read(rfm22b_dev, RFM22_DEVICE_VERSION) & RFM22_DV_MASK; + +#if defined(RFM22_DEBUG) + DEBUG_PRINTF(2, "rf device type: %d\n\r", device_type); + DEBUG_PRINTF(2, "rf device version: %d\n\r", device_version); +#endif + + if (device_type != 0x08) { - // Generate a pseudo-random number from 0-8 to add to the delay - uint8_t random = PIOS_CRC_updateByte(0, (uint8_t)(xTaskGetTickCount() & 0xff)) & 0x03; - rfm22b_dev->max_ack_delay = (uint16_t)((float)((sizeof(PHAckNackPacket) * 8 + TX_PREAMBLE_NIBBLES * 4) * 1000) / (float)(datarate_bps) + 0.5f) * 4 + 4 + random; +#if defined(RFM22_DEBUG) + DEBUG_PRINTF(2, "rf device type: INCORRECT - should be 0x08\n\r"); +#endif + // incorrect RF module type + return RFM22B_EVENT_FATAL_ERROR; + } + if (device_version != RFM22_DEVICE_VERSION_B1) + { +#if defined(RFM22_DEBUG) + DEBUG_PRINTF(2, "rf device version: INCORRECT\n\r"); +#endif + // incorrect RF module version + return RFM22B_EVENT_FATAL_ERROR; + } + + // calibrate our RF module to be exactly on frequency .. different for every module + rfm22_write(rfm22b_dev, RFM22_xtal_osc_load_cap, OSC_LOAD_CAP); + + // disable Low Duty Cycle Mode + rfm22_write(rfm22b_dev, RFM22_op_and_func_ctrl2, 0x00); + + // 1MHz clock output + rfm22_write(rfm22b_dev, RFM22_cpu_output_clk, RFM22_coc_1MHz); + + // READY mode + rfm22_write(rfm22b_dev, RFM22_op_and_func_ctrl1, RFM22_opfc1_xton); + + // choose the 3 GPIO pin functions + // GPIO port use default value + rfm22_write(rfm22b_dev, RFM22_io_port_config, RFM22_io_port_default); + if (rfm22b_dev->cfg.gpio_direction == GPIO0_TX_GPIO1_RX) { + // GPIO0 = TX State (to control RF Switch) + rfm22_write(rfm22b_dev, RFM22_gpio0_config, RFM22_gpio0_config_drv3 | RFM22_gpio0_config_txstate); + // GPIO1 = RX State (to control RF Switch) + rfm22_write(rfm22b_dev, RFM22_gpio1_config, RFM22_gpio1_config_drv3 | RFM22_gpio1_config_rxstate); + } else { + // GPIO0 = TX State (to control RF Switch) + rfm22_write(rfm22b_dev, RFM22_gpio0_config, RFM22_gpio0_config_drv3 | RFM22_gpio0_config_rxstate); + // GPIO1 = RX State (to control RF Switch) + rfm22_write(rfm22b_dev, RFM22_gpio1_config, RFM22_gpio1_config_drv3 | RFM22_gpio1_config_txstate); + } + // GPIO2 = Clear Channel Assessment + rfm22_write(rfm22b_dev, RFM22_gpio2_config, RFM22_gpio2_config_drv3 | RFM22_gpio2_config_cca); + + // FIFO mode, GFSK modulation + uint8_t fd_bit = rfm22_read(rfm22b_dev, RFM22_modulation_mode_control2) & RFM22_mmc2_fd; + rfm22_write(rfm22b_dev, RFM22_modulation_mode_control2, RFM22_mmc2_trclk_clk_none | RFM22_mmc2_dtmod_fifo | fd_bit | RFM22_mmc2_modtyp_gfsk); + + // setup to read the internal temperature sensor + + // ADC used to sample the temperature sensor + uint8_t adc_config = RFM22_ac_adcsel_temp_sensor | RFM22_ac_adcref_bg; + rfm22_write(rfm22b_dev, RFM22_adc_config, adc_config); + + // adc offset + rfm22_write(rfm22b_dev, RFM22_adc_sensor_amp_offset, 0); + + // temp sensor calibration .. �40C to +64C 0.5C resolution + rfm22_write(rfm22b_dev, RFM22_temp_sensor_calib, RFM22_tsc_tsrange0 | RFM22_tsc_entsoffs); + + // temp sensor offset + rfm22_write(rfm22b_dev, RFM22_temp_value_offset, 0); + + // start an ADC conversion + rfm22_write(rfm22b_dev, RFM22_adc_config, adc_config | RFM22_ac_adcstartbusy); + + // set the RSSI threshold interrupt to about -90dBm + rfm22_write(rfm22b_dev, RFM22_rssi_threshold_clear_chan_indicator, (-90 + 122) * 2); + + // enable the internal Tx & Rx packet handlers (without CRC) + rfm22_write(rfm22b_dev, RFM22_data_access_control, RFM22_dac_enpacrx | RFM22_dac_enpactx); + + // x-nibbles tx preamble + rfm22_write(rfm22b_dev, RFM22_preamble_length, TX_PREAMBLE_NIBBLES); + // x-nibbles rx preamble detection + rfm22_write(rfm22b_dev, RFM22_preamble_detection_ctrl1, RX_PREAMBLE_NIBBLES << 3); + + // header control - using a 4 by header with broadcast of 0xffffffff + rfm22_write(rfm22b_dev, RFM22_header_control1, + RFM22_header_cntl1_bcen_0 | + RFM22_header_cntl1_bcen_1 | + RFM22_header_cntl1_bcen_2 | + RFM22_header_cntl1_bcen_3 | + RFM22_header_cntl1_hdch_0 | + RFM22_header_cntl1_hdch_1 | + RFM22_header_cntl1_hdch_2 | + RFM22_header_cntl1_hdch_3); + // Check all bit of all bytes of the header + rfm22_write(rfm22b_dev, RFM22_header_enable0, 0xff); + rfm22_write(rfm22b_dev, RFM22_header_enable1, 0xff); + rfm22_write(rfm22b_dev, RFM22_header_enable2, 0xff); + rfm22_write(rfm22b_dev, RFM22_header_enable3, 0xff); + // Set the ID to be checked + uint32_t id = rfm22b_dev->deviceID; + rfm22_write(rfm22b_dev, RFM22_check_header0, id & 0xff); + rfm22_write(rfm22b_dev, RFM22_check_header1, (id >> 8) & 0xff); + rfm22_write(rfm22b_dev, RFM22_check_header2, (id >> 16) & 0xff); + rfm22_write(rfm22b_dev, RFM22_check_header3, (id >> 24) & 0xff); + // 4 header bytes, synchronization word length 3, 2, 1 & 0 used, packet length included in header. + rfm22_write(rfm22b_dev, RFM22_header_control2, + RFM22_header_cntl2_hdlen_3210 | + RFM22_header_cntl2_synclen_3210 | + ((TX_PREAMBLE_NIBBLES >> 8) & 0x01)); + + // sync word + rfm22_write(rfm22b_dev, RFM22_sync_word3, SYNC_BYTE_1); + rfm22_write(rfm22b_dev, RFM22_sync_word2, SYNC_BYTE_2); + rfm22_write(rfm22b_dev, RFM22_sync_word1, SYNC_BYTE_3); + rfm22_write(rfm22b_dev, RFM22_sync_word0, SYNC_BYTE_4); + + // set the tx power + rfm22_write(rfm22b_dev, RFM22_tx_power, RFM22_tx_pwr_lna_sw | rfm22b_dev->tx_power); + + // TX FIFO Almost Full Threshold (0 - 63) + rfm22_write(rfm22b_dev, RFM22_tx_fifo_control1, TX_FIFO_HI_WATERMARK); + + // TX FIFO Almost Empty Threshold (0 - 63) + rfm22_write(rfm22b_dev, RFM22_tx_fifo_control2, TX_FIFO_LO_WATERMARK); + + // RX FIFO Almost Full Threshold (0 - 63) + rfm22_write(rfm22b_dev, RFM22_rx_fifo_control, RX_FIFO_HI_WATERMARK); + + // Set the frequency calibration + rfm22_write(rfm22b_dev, RFM22_xtal_osc_load_cap, rfm22b_dev->cfg.RFXtalCap); + + // Initialize the frequency and datarate to te default. + rfm22_setNominalCarrierFrequency(rfm22b_dev, rfm22b_dev->init_frequency, rfm22b_dev->init_frequency, RFM22B_FREQUENCY_HOP_STEP_SIZE); + rfm22_setDatarate(rfm22b_dev, RFM22B_DEFAULT_RX_DATARATE, true); + + return RFM22B_EVENT_INITIALIZED; +} + +/** + * Inject an event into the RFM22B state machine. + * + * \param[in] rfm22b_dev The device structure + * \param[in] event The event to inject + * \param[in] inISR Is this being called from an interrrup service routine? + */ +static void pios_rfm22_inject_event(struct pios_rfm22b_dev *rfm22b_dev, enum pios_rfm22b_event event, bool inISR) +{ + + // Store the event. + if (xQueueSend(rfm22b_dev->eventQueue, &event, portMAX_DELAY) != pdTRUE) + return; + + // Signal the semaphore to wake up the handler thread. + if (inISR) { + portBASE_TYPE pxHigherPriorityTaskWoken; + if (xSemaphoreGiveFromISR(rfm22b_dev->isrPending, &pxHigherPriorityTaskWoken) != pdTRUE) { + // Something went fairly seriously wrong + rfm22b_dev->errors++; + } + portEND_SWITCHING_ISR(pxHigherPriorityTaskWoken); } else - rfm22b_dev->max_ack_delay = CONNECT_ATTEMPT_PERIOD_MS; + { + if (xSemaphoreGive(rfm22b_dev->isrPending) != pdTRUE) { + // Something went fairly seriously wrong + rfm22b_dev->errors++; + } + } +} + +/** + * Set the air datarate for the RFM22B device. + * + * Carson's rule: + * The signal bandwidth is about 2(Delta-f + fm) .. + * + * Delta-f = frequency deviation + * fm = maximum frequency of the signal + * + * \param[in] rfm33b_dev The device structure pointer. + * \param[in] datarate The air datarate. + * \param[in] data_whitening Is data whitening desired? + */ +static void rfm22_setDatarate(struct pios_rfm22b_dev *rfm22b_dev, enum rfm22b_datarate datarate, bool data_whitening) +{ + uint32_t datarate_bps = data_rate[datarate]; + rfm22b_dev->max_packet_time = (uint16_t)((float)(PIOS_PH_MAX_PACKET * 8 * 1000) / (float)(datarate_bps) + 0.5); + + // Generate a pseudo-random number from 0-8 to add to the delay + uint8_t random = PIOS_CRC_updateByte(0, (uint8_t)(xTaskGetTickCount() & 0xff)) & 0x03; + rfm22b_dev->max_ack_delay = (uint16_t)((float)((sizeof(PHAckNackPacket) * 8 + TX_PREAMBLE_NIBBLES * 4) * 1000) / (float)(datarate_bps) + 0.5) * 4 + 4 + random; // rfm22_if_filter_bandwidth rfm22_write(rfm22b_dev, 0x1C, reg_1C[datarate]); @@ -1122,20 +1303,6 @@ static void rfm22_write(struct pios_rfm22b_dev *rfm22b_dev, uint8_t addr, uint8_ rfm22_releaseBus(rfm22b_dev); } -/** - * Write a byte to a register without claiming the bus. Also - * toggle the NSS line - * @param[in] addr The address of the RFM22b register to write - * @param[in] data The data to write to that register - static void rfm22_write_noclaim(struct pios_rfm22b_dev *rfm22b_dev, uint8_t addr, uint8_t data) - { - uint8_t buf[2] = {addr | 0x80, data}; - rfm22_assertCs(rfm22b_dev); - PIOS_SPI_TransferBlock(rfm22b_dev->spi_id, buf, NULL, sizeof(buf), NULL); - rfm22_deassertCs(rfm22b_dev); - } -*/ - /** * Read a byte from an RFM22b register @@ -2068,9 +2235,6 @@ static enum pios_rfm22b_event rfm22_receiveAck(struct pios_rfm22b_dev *rfm22b_de rfm22b_dev->time_delta += remote_rx_time - local_tx_time; } - // Reset the resend count - rfm22b_dev->cur_resent_count = 0; - // Should we try to start another TX? if (rfm22b_dev->rx_packet.header.type == PACKET_TYPE_ACK) { rfm22b_dev->time_to_send_offset = curTicks; @@ -2240,239 +2404,6 @@ static enum pios_rfm22b_event rfm22_acceptConnection(struct pios_rfm22b_dev *rfm return RFM22B_EVENT_DEFAULT; } -// ************************************ -// Initialise this hardware layer module and the rf module - -static enum pios_rfm22b_event rfm22_init(struct pios_rfm22b_dev *rfm22b_dev) -{ - - // Initialize the register values. - rfm22b_dev->device_status = 0; - rfm22b_dev->int_status1 = 0; - rfm22b_dev->int_status2 = 0; - rfm22b_dev->ezmac_status = 0; - - // Clean the LEDs - rfm22_clearLEDs(); - - // Initialize the detected device statistics. - for (uint8_t i = 0; i < OPLINKSTATUS_PAIRIDS_NUMELEM; ++i) { - rfm22b_dev->pair_stats[i].pairID = 0; - rfm22b_dev->pair_stats[i].rssi = -127; - rfm22b_dev->pair_stats[i].afc_correction = 0; - rfm22b_dev->pair_stats[i].lastContact = 0; - } - - // Initlize the link stats. - for (uint8_t i = 0; i < RFM22B_RX_PACKET_STATS_LEN; ++i) - rfm22b_dev->rx_packet_stats[i] = 0; - - // Initialize the state - rfm22b_dev->stats.link_state = OPLINKSTATUS_LINKSTATE_DISCONNECTED; - rfm22b_dev->destination_id = 0xffffffff; - rfm22b_dev->time_to_send = false; - rfm22b_dev->time_to_send_offset = 0; - rfm22b_dev->send_status = false; - rfm22b_dev->send_connection_request = false; - rfm22b_dev->cur_resent_count = 0; - - // Initialize the packets. - rfm22b_dev->rx_packet_len = 0; - rfm22b_dev->tx_packet = NULL; - rfm22b_dev->prev_tx_packet = NULL; - rfm22b_dev->data_packet.header.data_size = 0; - rfm22b_dev->in_rx_mode = false; - - // Initialize the devide state - rfm22b_dev->device_status = rfm22b_dev->int_status1 = rfm22b_dev->int_status2 = rfm22b_dev->ezmac_status = 0; - rfm22b_dev->rx_buffer_wr = 0; - rfm22b_dev->tx_data_rd = rfm22b_dev->tx_data_wr = 0; - rfm22b_dev->frequency_hop_channel = 0; - rfm22b_dev->afc_correction_Hz = 0; - rfm22b_dev->packet_start_ticks = 0; - rfm22b_dev->tx_complete_ticks = 0; - rfm22b_dev->rx_complete_ticks = 0; - - // software reset the RF chip .. following procedure according to Si4x3x Errata (rev. B) - rfm22_write(rfm22b_dev, RFM22_op_and_func_ctrl1, RFM22_opfc1_swres); - - for (int i = 50; i > 0; i--) { - // read the status registers - rfm22b_dev->int_status1 = rfm22_read(rfm22b_dev, RFM22_interrupt_status1); - rfm22b_dev->int_status2 = rfm22_read(rfm22b_dev, RFM22_interrupt_status2); - if (rfm22b_dev->int_status2 & RFM22_is2_ichiprdy) break; - - // wait 1ms - PIOS_DELAY_WaitmS(1); - } - - // **************** - - // read status - clears interrupt - rfm22b_dev->device_status = rfm22_read(rfm22b_dev, RFM22_device_status); - rfm22b_dev->int_status1 = rfm22_read(rfm22b_dev, RFM22_interrupt_status1); - rfm22b_dev->int_status2 = rfm22_read(rfm22b_dev, RFM22_interrupt_status2); - rfm22b_dev->ezmac_status = rfm22_read(rfm22b_dev, RFM22_ezmac_status); - - // disable all interrupts - rfm22_write(rfm22b_dev, RFM22_interrupt_enable1, 0x00); - rfm22_write(rfm22b_dev, RFM22_interrupt_enable2, 0x00); - - // read the RF chip ID bytes - - // read the device type - uint8_t device_type = rfm22_read(rfm22b_dev, RFM22_DEVICE_TYPE) & RFM22_DT_MASK; - // read the device version - uint8_t device_version = rfm22_read(rfm22b_dev, RFM22_DEVICE_VERSION) & RFM22_DV_MASK; - -#if defined(RFM22_DEBUG) - DEBUG_PRINTF(2, "rf device type: %d\n\r", device_type); - DEBUG_PRINTF(2, "rf device version: %d\n\r", device_version); -#endif - - if (device_type != 0x08) - { -#if defined(RFM22_DEBUG) - DEBUG_PRINTF(2, "rf device type: INCORRECT - should be 0x08\n\r"); -#endif - // incorrect RF module type - return RFM22B_EVENT_FATAL_ERROR; - } - if (device_version != RFM22_DEVICE_VERSION_B1) - { -#if defined(RFM22_DEBUG) - DEBUG_PRINTF(2, "rf device version: INCORRECT\n\r"); -#endif - // incorrect RF module version - return RFM22B_EVENT_FATAL_ERROR; - } - - // calibrate our RF module to be exactly on frequency .. different for every module - rfm22_write(rfm22b_dev, RFM22_xtal_osc_load_cap, OSC_LOAD_CAP); - - // disable Low Duty Cycle Mode - rfm22_write(rfm22b_dev, RFM22_op_and_func_ctrl2, 0x00); - - // 1MHz clock output - rfm22_write(rfm22b_dev, RFM22_cpu_output_clk, RFM22_coc_1MHz); - - // READY mode - rfm22_write(rfm22b_dev, RFM22_op_and_func_ctrl1, RFM22_opfc1_xton); - - // choose the 3 GPIO pin functions - // GPIO port use default value - rfm22_write(rfm22b_dev, RFM22_io_port_config, RFM22_io_port_default); - if (rfm22b_dev->cfg.gpio_direction == GPIO0_TX_GPIO1_RX) { - // GPIO0 = TX State (to control RF Switch) - rfm22_write(rfm22b_dev, RFM22_gpio0_config, RFM22_gpio0_config_drv3 | RFM22_gpio0_config_txstate); - // GPIO1 = RX State (to control RF Switch) - rfm22_write(rfm22b_dev, RFM22_gpio1_config, RFM22_gpio1_config_drv3 | RFM22_gpio1_config_rxstate); - } else { - // GPIO0 = TX State (to control RF Switch) - rfm22_write(rfm22b_dev, RFM22_gpio0_config, RFM22_gpio0_config_drv3 | RFM22_gpio0_config_rxstate); - // GPIO1 = RX State (to control RF Switch) - rfm22_write(rfm22b_dev, RFM22_gpio1_config, RFM22_gpio1_config_drv3 | RFM22_gpio1_config_txstate); - } - // GPIO2 = Clear Channel Assessment - rfm22_write(rfm22b_dev, RFM22_gpio2_config, RFM22_gpio2_config_drv3 | RFM22_gpio2_config_cca); - - // FIFO mode, GFSK modulation - uint8_t fd_bit = rfm22_read(rfm22b_dev, RFM22_modulation_mode_control2) & RFM22_mmc2_fd; - rfm22_write(rfm22b_dev, RFM22_modulation_mode_control2, RFM22_mmc2_trclk_clk_none | RFM22_mmc2_dtmod_fifo | fd_bit | RFM22_mmc2_modtyp_gfsk); - - // setup to read the internal temperature sensor - - // ADC used to sample the temperature sensor - uint8_t adc_config = RFM22_ac_adcsel_temp_sensor | RFM22_ac_adcref_bg; - rfm22_write(rfm22b_dev, RFM22_adc_config, adc_config); - - // adc offset - rfm22_write(rfm22b_dev, RFM22_adc_sensor_amp_offset, 0); - - // temp sensor calibration .. �40C to +64C 0.5C resolution - rfm22_write(rfm22b_dev, RFM22_temp_sensor_calib, RFM22_tsc_tsrange0 | RFM22_tsc_entsoffs); - - // temp sensor offset - rfm22_write(rfm22b_dev, RFM22_temp_value_offset, 0); - - // start an ADC conversion - rfm22_write(rfm22b_dev, RFM22_adc_config, adc_config | RFM22_ac_adcstartbusy); - - // set the RSSI threshold interrupt to about -90dBm - rfm22_write(rfm22b_dev, RFM22_rssi_threshold_clear_chan_indicator, (-90 + 122) * 2); - - // enable the internal Tx & Rx packet handlers (without CRC) - rfm22_write(rfm22b_dev, RFM22_data_access_control, RFM22_dac_enpacrx | RFM22_dac_enpactx); - - // x-nibbles tx preamble - rfm22_write(rfm22b_dev, RFM22_preamble_length, TX_PREAMBLE_NIBBLES); - // x-nibbles rx preamble detection - rfm22_write(rfm22b_dev, RFM22_preamble_detection_ctrl1, RX_PREAMBLE_NIBBLES << 3); - -#ifdef PIOS_RFM22_NO_HEADER - // header control - we are not using the header - rfm22_write(rfm22b_dev, RFM22_header_control1, RFM22_header_cntl1_bcen_none | RFM22_header_cntl1_hdch_none); - - // no header bytes, synchronization word length 3, 2, 1 & 0 used, packet length included in header. - rfm22_write(rfm22b_dev, RFM22_header_control2, RFM22_header_cntl2_hdlen_none | - RFM22_header_cntl2_synclen_3210 | ((TX_PREAMBLE_NIBBLES >> 8) & 0x01)); -#else - // header control - using a 4 by header with broadcast of 0xffffffff - rfm22_write(rfm22b_dev, RFM22_header_control1, - RFM22_header_cntl1_bcen_0 | - RFM22_header_cntl1_bcen_1 | - RFM22_header_cntl1_bcen_2 | - RFM22_header_cntl1_bcen_3 | - RFM22_header_cntl1_hdch_0 | - RFM22_header_cntl1_hdch_1 | - RFM22_header_cntl1_hdch_2 | - RFM22_header_cntl1_hdch_3); - // Check all bit of all bytes of the header - rfm22_write(rfm22b_dev, RFM22_header_enable0, 0xff); - rfm22_write(rfm22b_dev, RFM22_header_enable1, 0xff); - rfm22_write(rfm22b_dev, RFM22_header_enable2, 0xff); - rfm22_write(rfm22b_dev, RFM22_header_enable3, 0xff); - // Set the ID to be checked - uint32_t id = rfm22b_dev->deviceID; - rfm22_write(rfm22b_dev, RFM22_check_header0, id & 0xff); - rfm22_write(rfm22b_dev, RFM22_check_header1, (id >> 8) & 0xff); - rfm22_write(rfm22b_dev, RFM22_check_header2, (id >> 16) & 0xff); - rfm22_write(rfm22b_dev, RFM22_check_header3, (id >> 24) & 0xff); - // 4 header bytes, synchronization word length 3, 2, 1 & 0 used, packet length included in header. - rfm22_write(rfm22b_dev, RFM22_header_control2, - RFM22_header_cntl2_hdlen_3210 | - RFM22_header_cntl2_synclen_3210 | - ((TX_PREAMBLE_NIBBLES >> 8) & 0x01)); -#endif - - // sync word - rfm22_write(rfm22b_dev, RFM22_sync_word3, SYNC_BYTE_1); - rfm22_write(rfm22b_dev, RFM22_sync_word2, SYNC_BYTE_2); - rfm22_write(rfm22b_dev, RFM22_sync_word1, SYNC_BYTE_3); - rfm22_write(rfm22b_dev, RFM22_sync_word0, SYNC_BYTE_4); - - // set the tx power - rfm22_write(rfm22b_dev, RFM22_tx_power, RFM22_tx_pwr_lna_sw | rfm22b_dev->tx_power); - - // TX FIFO Almost Full Threshold (0 - 63) - rfm22_write(rfm22b_dev, RFM22_tx_fifo_control1, TX_FIFO_HI_WATERMARK); - - // TX FIFO Almost Empty Threshold (0 - 63) - rfm22_write(rfm22b_dev, RFM22_tx_fifo_control2, TX_FIFO_LO_WATERMARK); - - // RX FIFO Almost Full Threshold (0 - 63) - rfm22_write(rfm22b_dev, RFM22_rx_fifo_control, RX_FIFO_HI_WATERMARK); - - // Set the frequency calibration - rfm22_write(rfm22b_dev, RFM22_xtal_osc_load_cap, rfm22b_dev->cfg.RFXtalCap); - - // Initialize the frequency and datarate to te default. - rfm22_setNominalCarrierFrequency(rfm22b_dev, rfm22b_dev->init_frequency, rfm22b_dev->init_frequency, RFM22B_FREQUENCY_HOP_STEP_SIZE); - rfm22_setDatarate(rfm22b_dev, RFM22B_DEFAULT_RX_DATARATE, true); - - return RFM22B_EVENT_INITIALIZED; -} - static void rfm22_clearLEDs() { LINK_LED_OFF; RX_LED_OFF; diff --git a/flight/pios/inc/pios_rfm22b_priv.h b/flight/pios/inc/pios_rfm22b_priv.h index 85d76c8a5..57594c3fe 100644 --- a/flight/pios/inc/pios_rfm22b_priv.h +++ b/flight/pios/inc/pios_rfm22b_priv.h @@ -443,37 +443,11 @@ #define RFM22_received_packet_length 0x4B // R #define RFM22_adc8_control 0x4F // R/W -/* - #define RFM22_analog_test_bus 0x50 // R/W - #define RFM22_digital_test_bus 0x51 // R/W - #define RFM22_tx_ramp_control 0x52 // R/W - #define RFM22_pll_tune_time 0x53 // R/W - #define RFM22_calibration_control 0x55 // R/W - - #define RFM22_modem_test 0x56 // R/W - - #define RFM22_chargepump_test 0x57 // R/W - #define RFM22_chargepump_current_trimming_override 0x58 // R/W - - #define RFM22_divider_current_trimming 0x59 // R/W - - #define RFM22_vco_current_trimming 0x5A // R/W - #define RFM22_vco_calibration_override 0x5B // R/W - - #define RFM22_synthersizer_test 0x5C // R/W - - #define RFM22_block_enable_override1 0x5D // R/W - #define RFM22_block_enable_override2 0x5E // R/W - #define RFM22_block_enable_override3 0x5F // R/W -*/ #define RFM22_channel_filter_coeff_addr 0x60 // R/W #define RFM22_ch_fil_coeff_ad_inv_pre_th_mask 0xF0 // #define RFM22_ch_fil_coeff_ad_chfiladd_mask 0x0F // Channel Filter Coefficient Look-up Table Address. The address for channel filter coefficients used in the RX path. - -//#define RFM22_channel_filter_coeff_value 0x61 // R/W - #define RFM22_xtal_osc_por_ctrl 0x62 // R/W #define RFM22_xtal_osc_por_ctrl_pwst_mask 0xE0 // Internal Power States of the Chip. #define RFM22_xtal_osc_por_ctrl_clkhyst 0x10 // Clock Hysteresis Setting. @@ -481,27 +455,13 @@ #define RFM22_xtal_osc_por_ctrl_enamp2x 0x04 // 2 Times Higher Amplification Enable. #define RFM22_xtal_osc_por_ctrl_bufovr 0x02 // Output Buffer Enable Override. #define RFM22_xtal_osc_por_ctrl_enbuf 0x01 // Output Buffer Enable. -/* - #define RFM22_rc_osc_coarse_calbration_override 0x63 // R/W - #define RFM22_rc_osc_fine_calbration_override 0x64 // R/W - #define RFM22_ldo_control_override 0x65 // R/W - #define RFM22_ldo_level_setting 0x66 // R/W - - #define RFM22_deltasigma_adc_tuning1 0x67 // R/W - #define RFM22_deltasigma_adc_tuning2 0x68 // R/W -*/ #define RFM22_agc_override1 0x69 // R/W #define RFM22_agc_ovr1_sgi 0x40 // AGC Loop, Set Gain Increase. If set to 0 then gain increasing will not be allowed. If set to 1 then gain increasing is allowed, default is 0. #define RFM22_agc_ovr1_agcen 0x20 // Automatic Gain Control Enable. When this bit is set then the result of the control can be read out from bits [4:0], otherwise the gain can be controlled manually by writing into bits [4:0]. #define RFM22_agc_ovr1_lnagain 0x10 // LNA Gain Select. 0 = min gain = 5dB, 1 = max gain = 25 dB. #define RFM22_agc_ovr1_pga_mask 0x0F // PGA Gain Override Value. -//#define RFM22_agc_override2 0x6A // R/W - -//#define RFM22_gfsk_fir_coeff_addr 0x6B // R/W -//#define RFM22_gfsk_fir_coeff_value 0x6C // R/W - #define RFM22_tx_power 0x6D // R/W #define RFM22_tx_pwr_lna_sw 0x08 // LNA Switch Controller. If set, lna_sw control from the digital will go high during TX modes, and low during other times. If reset, the digital control signal is low at all times. @@ -761,8 +721,6 @@ struct pios_rfm22b_dev { // The offset between our clock and the global send clock uint8_t time_to_send_offset; - // The number of times that the current packet has been resent. - uint8_t cur_resent_count; // The initial frequency uint32_t init_frequency; @@ -773,8 +731,6 @@ struct pios_rfm22b_dev { float frequency_step_size; // current frequency hop channel uint8_t frequency_hop_channel; - // the frequency hop step size - uint8_t frequency_hop_step_size_reg; // afc correction reading (in Hz) int8_t afc_correction_Hz; From a9079bfb7e7cfe2d1d79a5ee2e58a3eef76beff4 Mon Sep 17 00:00:00 2001 From: Brian Webb Date: Sat, 13 Apr 2013 17:43:50 +0100 Subject: [PATCH 5/9] OP-914: Additional cleanup of the RFM22B driver code. Updated to code to the current coding standard. Added Doxygen documentation. Rearranged functions into functional groups to simplify reading the code. --- flight/pios/common/pios_rfm22b.c | 4074 +++++++++++++------------ flight/pios/common/pios_rfm22b_com.c | 140 +- flight/pios/common/pios_rfm22b_rcvr.c | 120 +- flight/pios/inc/pios_rfm22b.h | 126 +- flight/pios/inc/pios_rfm22b_priv.h | 342 ++- 5 files changed, 2578 insertions(+), 2224 deletions(-) diff --git a/flight/pios/common/pios_rfm22b.c b/flight/pios/common/pios_rfm22b.c index eca884a1a..b938798f4 100644 --- a/flight/pios/common/pios_rfm22b.c +++ b/flight/pios/common/pios_rfm22b.c @@ -89,27 +89,27 @@ #define PPM_UPDATE_PERIOD_MS 20 // this is too adjust the RF module so that it is on frequency -#define OSC_LOAD_CAP 0x7F // cap = 12.5pf .. default +#define OSC_LOAD_CAP 0x7F // cap = 12.5pf .. default -// ************************************ - -#define TX_PREAMBLE_NIBBLES 12 // 7 to 511 (number of nibbles) -#define RX_PREAMBLE_NIBBLES 6 // 5 to 31 (number of nibbles) +#define TX_PREAMBLE_NIBBLES 12 // 7 to 511 (number of nibbles) +#define RX_PREAMBLE_NIBBLES 6 // 5 to 31 (number of nibbles) // the size of the rf modules internal FIFO buffers -#define FIFO_SIZE 64 +#define FIFO_SIZE 64 -#define TX_FIFO_HI_WATERMARK 62 // 0-63 -#define TX_FIFO_LO_WATERMARK 32 // 0-63 +#define TX_FIFO_HI_WATERMARK 62 // 0-63 +#define TX_FIFO_LO_WATERMARK 32 // 0-63 -#define RX_FIFO_HI_WATERMARK 32 // 0-63 +#define RX_FIFO_HI_WATERMARK 32 // 0-63 -#define PREAMBLE_BYTE 0x55 // preamble byte (preceeds SYNC_BYTE's) +// preamble byte (preceeds SYNC_BYTE's) +#define PREAMBLE_BYTE 0x55 -#define SYNC_BYTE_1 0x2D // RF sync bytes (32-bit in all) -#define SYNC_BYTE_2 0xD4 // -#define SYNC_BYTE_3 0x4B // -#define SYNC_BYTE_4 0x59 // +// RF sync bytes (32-bit in all) +#define SYNC_BYTE_1 0x2D +#define SYNC_BYTE_2 0xD4 +#define SYNC_BYTE_3 0x4B +#define SYNC_BYTE_4 0x59 #ifndef RX_LED_ON #define RX_LED_ON @@ -122,51 +122,41 @@ #define USB_LED_OFF #endif -// ************************************ -// Normal data streaming -// GFSK modulation -// no manchester encoding -// data whitening -// FIFO mode -// 5-nibble rx preamble length detection -// 10-nibble tx preamble length -// AFC enabled - /* Local type definitions */ struct pios_rfm22b_transition { - enum pios_rfm22b_event (*entry_fn) (struct pios_rfm22b_dev *rfm22b_dev); - enum pios_rfm22b_state next_state[RFM22B_EVENT_NUM_EVENTS]; + enum pios_rfm22b_event (*entry_fn) (struct pios_rfm22b_dev *rfm22b_dev); + enum pios_rfm22b_state next_state[RFM22B_EVENT_NUM_EVENTS]; }; // Must ensure these prefilled arrays match the define sizes static const uint8_t FULL_PREAMBLE[FIFO_SIZE] = { - PREAMBLE_BYTE,PREAMBLE_BYTE,PREAMBLE_BYTE,PREAMBLE_BYTE,PREAMBLE_BYTE, - PREAMBLE_BYTE,PREAMBLE_BYTE,PREAMBLE_BYTE,PREAMBLE_BYTE,PREAMBLE_BYTE, - PREAMBLE_BYTE,PREAMBLE_BYTE,PREAMBLE_BYTE,PREAMBLE_BYTE,PREAMBLE_BYTE, - PREAMBLE_BYTE,PREAMBLE_BYTE,PREAMBLE_BYTE,PREAMBLE_BYTE,PREAMBLE_BYTE, - PREAMBLE_BYTE,PREAMBLE_BYTE,PREAMBLE_BYTE,PREAMBLE_BYTE,PREAMBLE_BYTE, - PREAMBLE_BYTE,PREAMBLE_BYTE,PREAMBLE_BYTE,PREAMBLE_BYTE,PREAMBLE_BYTE, - PREAMBLE_BYTE,PREAMBLE_BYTE,PREAMBLE_BYTE,PREAMBLE_BYTE,PREAMBLE_BYTE, - PREAMBLE_BYTE,PREAMBLE_BYTE,PREAMBLE_BYTE,PREAMBLE_BYTE,PREAMBLE_BYTE, - PREAMBLE_BYTE,PREAMBLE_BYTE,PREAMBLE_BYTE,PREAMBLE_BYTE,PREAMBLE_BYTE, - PREAMBLE_BYTE,PREAMBLE_BYTE,PREAMBLE_BYTE,PREAMBLE_BYTE,PREAMBLE_BYTE, - PREAMBLE_BYTE,PREAMBLE_BYTE,PREAMBLE_BYTE,PREAMBLE_BYTE,PREAMBLE_BYTE, - PREAMBLE_BYTE,PREAMBLE_BYTE,PREAMBLE_BYTE,PREAMBLE_BYTE,PREAMBLE_BYTE, - PREAMBLE_BYTE,PREAMBLE_BYTE,PREAMBLE_BYTE,PREAMBLE_BYTE}; // 64 bytes + PREAMBLE_BYTE,PREAMBLE_BYTE,PREAMBLE_BYTE,PREAMBLE_BYTE,PREAMBLE_BYTE, + PREAMBLE_BYTE,PREAMBLE_BYTE,PREAMBLE_BYTE,PREAMBLE_BYTE,PREAMBLE_BYTE, + PREAMBLE_BYTE,PREAMBLE_BYTE,PREAMBLE_BYTE,PREAMBLE_BYTE,PREAMBLE_BYTE, + PREAMBLE_BYTE,PREAMBLE_BYTE,PREAMBLE_BYTE,PREAMBLE_BYTE,PREAMBLE_BYTE, + PREAMBLE_BYTE,PREAMBLE_BYTE,PREAMBLE_BYTE,PREAMBLE_BYTE,PREAMBLE_BYTE, + PREAMBLE_BYTE,PREAMBLE_BYTE,PREAMBLE_BYTE,PREAMBLE_BYTE,PREAMBLE_BYTE, + PREAMBLE_BYTE,PREAMBLE_BYTE,PREAMBLE_BYTE,PREAMBLE_BYTE,PREAMBLE_BYTE, + PREAMBLE_BYTE,PREAMBLE_BYTE,PREAMBLE_BYTE,PREAMBLE_BYTE,PREAMBLE_BYTE, + PREAMBLE_BYTE,PREAMBLE_BYTE,PREAMBLE_BYTE,PREAMBLE_BYTE,PREAMBLE_BYTE, + PREAMBLE_BYTE,PREAMBLE_BYTE,PREAMBLE_BYTE,PREAMBLE_BYTE,PREAMBLE_BYTE, + PREAMBLE_BYTE,PREAMBLE_BYTE,PREAMBLE_BYTE,PREAMBLE_BYTE,PREAMBLE_BYTE, + PREAMBLE_BYTE,PREAMBLE_BYTE,PREAMBLE_BYTE,PREAMBLE_BYTE,PREAMBLE_BYTE, + PREAMBLE_BYTE,PREAMBLE_BYTE,PREAMBLE_BYTE,PREAMBLE_BYTE}; // 64 bytes static const uint8_t HEADER[(TX_PREAMBLE_NIBBLES + 1)/2 + 2] = {PREAMBLE_BYTE, PREAMBLE_BYTE, PREAMBLE_BYTE, PREAMBLE_BYTE,PREAMBLE_BYTE, PREAMBLE_BYTE, SYNC_BYTE_1, SYNC_BYTE_2}; static const uint8_t OUT_FF[64] = { - 0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF, - 0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF, - 0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF, - 0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF, - 0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF, - 0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF, - 0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF, - 0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF}; + 0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF, + 0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF, + 0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF, + 0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF, + 0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF, + 0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF, + 0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF, + 0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF}; /* Local function forwared declarations */ -static void PIOS_RFM22B_Task(void *parameters); +static void pios_rfm22_task(void *parameters); static bool rfm22_readStatus(struct pios_rfm22b_dev *rfm22b_dev); static void rfm22_setDatarate(struct pios_rfm22b_dev * rfm22b_dev, enum rfm22b_datarate datarate, bool data_whitening); static void pios_rfm22_inject_event(struct pios_rfm22b_dev *rfm22b_dev, enum pios_rfm22b_event event, bool inISR); @@ -207,6 +197,10 @@ static uint8_t rfm22_calcChannel(struct pios_rfm22b_dev *rfm22b_dev); static bool rfm22_changeChannel(struct pios_rfm22b_dev *rfm22b_dev); static void rfm22_clearLEDs(); +// Utility functions. +static uint32_t pios_rfm22_time_difference_ms(portTickType start_time, portTickType end_time); +static struct pios_rfm22b_dev *pios_rfm22_alloc(void); + // SPI read/write functions static void rfm22_assertCs(struct pios_rfm22b_dev *rfm22b_dev); static void rfm22_deassertCs(struct pios_rfm22b_dev *rfm22b_dev); @@ -216,219 +210,220 @@ static void rfm22_write(struct pios_rfm22b_dev *rfm22b_dev, uint8_t addr, uint8_ static uint8_t rfm22_read(struct pios_rfm22b_dev *rfm22b_dev, uint8_t addr); static uint8_t rfm22_read_noclaim(struct pios_rfm22b_dev *rfm22b_dev, uint8_t addr); + /* The state transition table */ const static struct pios_rfm22b_transition rfm22b_transitions[RFM22B_STATE_NUM_STATES] = { - // Initialization thread - [RFM22B_STATE_UNINITIALIZED] = { - .entry_fn = 0, - .next_state = { - [RFM22B_EVENT_INITIALIZE] = RFM22B_STATE_INITIALIZING, - [RFM22B_EVENT_ERROR] = RFM22B_STATE_ERROR, - }, - }, - [RFM22B_STATE_INITIALIZING] = { - .entry_fn = rfm22_init, - .next_state = { - [RFM22B_EVENT_INITIALIZED] = RFM22B_STATE_RX_MODE, - [RFM22B_EVENT_ERROR] = RFM22B_STATE_ERROR, - [RFM22B_EVENT_INITIALIZE] = RFM22B_STATE_INITIALIZING, - [RFM22B_EVENT_FATAL_ERROR] = RFM22B_STATE_FATAL_ERROR, - }, - }, - [RFM22B_STATE_REQUESTING_CONNECTION] = { - .entry_fn = rfm22_requestConnection, - .next_state = { - [RFM22B_EVENT_TX_START] = RFM22B_STATE_TX_START, - [RFM22B_EVENT_TIMEOUT] = RFM22B_STATE_TIMEOUT, - [RFM22B_EVENT_ERROR] = RFM22B_STATE_ERROR, - [RFM22B_EVENT_INITIALIZE] = RFM22B_STATE_INITIALIZING, - [RFM22B_EVENT_FATAL_ERROR] = RFM22B_STATE_FATAL_ERROR - }, - }, - [RFM22B_STATE_ACCEPTING_CONNECTION] = { - .entry_fn = rfm22_acceptConnection, - .next_state = { - [RFM22B_EVENT_DEFAULT] = RFM22B_STATE_SENDING_ACK, - [RFM22B_EVENT_TIMEOUT] = RFM22B_STATE_TIMEOUT, - [RFM22B_EVENT_ERROR] = RFM22B_STATE_ERROR, - [RFM22B_EVENT_INITIALIZE] = RFM22B_STATE_INITIALIZING, - [RFM22B_EVENT_FATAL_ERROR] = RFM22B_STATE_FATAL_ERROR, - }, - }, + // Initialization thread + [RFM22B_STATE_UNINITIALIZED] = { + .entry_fn = 0, + .next_state = { + [RFM22B_EVENT_INITIALIZE] = RFM22B_STATE_INITIALIZING, + [RFM22B_EVENT_ERROR] = RFM22B_STATE_ERROR, + }, + }, + [RFM22B_STATE_INITIALIZING] = { + .entry_fn = rfm22_init, + .next_state = { + [RFM22B_EVENT_INITIALIZED] = RFM22B_STATE_RX_MODE, + [RFM22B_EVENT_ERROR] = RFM22B_STATE_ERROR, + [RFM22B_EVENT_INITIALIZE] = RFM22B_STATE_INITIALIZING, + [RFM22B_EVENT_FATAL_ERROR] = RFM22B_STATE_FATAL_ERROR, + }, + }, + [RFM22B_STATE_REQUESTING_CONNECTION] = { + .entry_fn = rfm22_requestConnection, + .next_state = { + [RFM22B_EVENT_TX_START] = RFM22B_STATE_TX_START, + [RFM22B_EVENT_TIMEOUT] = RFM22B_STATE_TIMEOUT, + [RFM22B_EVENT_ERROR] = RFM22B_STATE_ERROR, + [RFM22B_EVENT_INITIALIZE] = RFM22B_STATE_INITIALIZING, + [RFM22B_EVENT_FATAL_ERROR] = RFM22B_STATE_FATAL_ERROR + }, + }, + [RFM22B_STATE_ACCEPTING_CONNECTION] = { + .entry_fn = rfm22_acceptConnection, + .next_state = { + [RFM22B_EVENT_DEFAULT] = RFM22B_STATE_SENDING_ACK, + [RFM22B_EVENT_TIMEOUT] = RFM22B_STATE_TIMEOUT, + [RFM22B_EVENT_ERROR] = RFM22B_STATE_ERROR, + [RFM22B_EVENT_INITIALIZE] = RFM22B_STATE_INITIALIZING, + [RFM22B_EVENT_FATAL_ERROR] = RFM22B_STATE_FATAL_ERROR, + }, + }, - [RFM22B_STATE_RX_MODE] = { - .entry_fn = rfm22_setRxMode, - .next_state = { - [RFM22B_EVENT_INT_RECEIVED] = RFM22B_STATE_WAIT_PREAMBLE, - [RFM22B_EVENT_TX_START] = RFM22B_STATE_TX_START, - [RFM22B_EVENT_ACK_TIMEOUT] = RFM22B_STATE_RECEIVING_NACK, - [RFM22B_EVENT_FAILURE] = RFM22B_STATE_RX_FAILURE, - [RFM22B_EVENT_TIMEOUT] = RFM22B_STATE_TIMEOUT, - [RFM22B_EVENT_ERROR] = RFM22B_STATE_ERROR, - [RFM22B_EVENT_INITIALIZE] = RFM22B_STATE_INITIALIZING, - [RFM22B_EVENT_FATAL_ERROR] = RFM22B_STATE_FATAL_ERROR, - }, - }, - [RFM22B_STATE_WAIT_PREAMBLE] = { - .entry_fn = rfm22_detectPreamble, - .next_state = { - [RFM22B_EVENT_PREAMBLE_DETECTED] = RFM22B_STATE_WAIT_SYNC, - [RFM22B_EVENT_TX_START] = RFM22B_STATE_TX_START, - [RFM22B_EVENT_ACK_TIMEOUT] = RFM22B_STATE_RECEIVING_NACK, - [RFM22B_EVENT_INT_RECEIVED] = RFM22B_STATE_WAIT_PREAMBLE, - [RFM22B_EVENT_FAILURE] = RFM22B_STATE_RX_FAILURE, - [RFM22B_EVENT_TIMEOUT] = RFM22B_STATE_TIMEOUT, - [RFM22B_EVENT_ERROR] = RFM22B_STATE_ERROR, - [RFM22B_EVENT_INITIALIZE] = RFM22B_STATE_INITIALIZING, - [RFM22B_EVENT_FATAL_ERROR] = RFM22B_STATE_FATAL_ERROR, - }, - }, - [RFM22B_STATE_WAIT_SYNC] = { - .entry_fn = rfm22_detectSync, - .next_state = { - [RFM22B_EVENT_INT_RECEIVED] = RFM22B_STATE_WAIT_SYNC, - [RFM22B_EVENT_SYNC_DETECTED] = RFM22B_STATE_RX_DATA, - [RFM22B_EVENT_FAILURE] = RFM22B_STATE_RX_FAILURE, - [RFM22B_EVENT_TIMEOUT] = RFM22B_STATE_TIMEOUT, - [RFM22B_EVENT_ERROR] = RFM22B_STATE_ERROR, - [RFM22B_EVENT_INITIALIZE] = RFM22B_STATE_INITIALIZING, - [RFM22B_EVENT_FATAL_ERROR] = RFM22B_STATE_FATAL_ERROR, - }, - }, - [RFM22B_STATE_RX_DATA] = { - .entry_fn = rfm22_rxData, - .next_state = { - [RFM22B_EVENT_INT_RECEIVED] = RFM22B_STATE_RX_DATA, - [RFM22B_EVENT_RX_COMPLETE] = RFM22B_STATE_SENDING_ACK, - [RFM22B_EVENT_RX_MODE] = RFM22B_STATE_RX_MODE, - [RFM22B_EVENT_RX_ERROR] = RFM22B_STATE_SENDING_NACK, - [RFM22B_EVENT_STATUS_RECEIVED] = RFM22B_STATE_RECEIVING_STATUS, - [RFM22B_EVENT_CONNECTION_REQUESTED] = RFM22B_STATE_ACCEPTING_CONNECTION, - [RFM22B_EVENT_PACKET_ACKED] = RFM22B_STATE_RECEIVING_ACK, - [RFM22B_EVENT_PACKET_NACKED] = RFM22B_STATE_RECEIVING_NACK, - [RFM22B_EVENT_FAILURE] = RFM22B_STATE_RX_FAILURE, - [RFM22B_EVENT_TIMEOUT] = RFM22B_STATE_TIMEOUT, - [RFM22B_EVENT_ERROR] = RFM22B_STATE_ERROR, - [RFM22B_EVENT_INITIALIZE] = RFM22B_STATE_INITIALIZING, - [RFM22B_EVENT_FATAL_ERROR] = RFM22B_STATE_FATAL_ERROR, - }, - }, - [RFM22B_STATE_RX_FAILURE] = { - .entry_fn = rfm22_rxFailure, - .next_state = { - [RFM22B_EVENT_RX_MODE] = RFM22B_STATE_RX_MODE, - [RFM22B_EVENT_TIMEOUT] = RFM22B_STATE_TIMEOUT, - [RFM22B_EVENT_ERROR] = RFM22B_STATE_ERROR, - [RFM22B_EVENT_INITIALIZE] = RFM22B_STATE_INITIALIZING, - [RFM22B_EVENT_FATAL_ERROR] = RFM22B_STATE_FATAL_ERROR, - }, - }, - [RFM22B_STATE_RECEIVING_ACK] = { - .entry_fn = rfm22_receiveAck, - .next_state = { - [RFM22B_EVENT_TX_START] = RFM22B_STATE_TX_START, - [RFM22B_EVENT_RX_MODE] = RFM22B_STATE_RX_MODE, - [RFM22B_EVENT_TIMEOUT] = RFM22B_STATE_TIMEOUT, - [RFM22B_EVENT_ERROR] = RFM22B_STATE_ERROR, - [RFM22B_EVENT_INITIALIZE] = RFM22B_STATE_INITIALIZING, - [RFM22B_EVENT_FATAL_ERROR] = RFM22B_STATE_FATAL_ERROR, - }, - }, - [RFM22B_STATE_RECEIVING_NACK] = { - .entry_fn = rfm22_receiveNack, - .next_state = { - [RFM22B_EVENT_TX_START] = RFM22B_STATE_TX_START, - [RFM22B_EVENT_TIMEOUT] = RFM22B_STATE_TIMEOUT, - [RFM22B_EVENT_ERROR] = RFM22B_STATE_ERROR, - [RFM22B_EVENT_INITIALIZE] = RFM22B_STATE_INITIALIZING, - [RFM22B_EVENT_FATAL_ERROR] = RFM22B_STATE_FATAL_ERROR, - }, - }, - [RFM22B_STATE_RECEIVING_STATUS] = { - .entry_fn = rfm22_receiveStatus, - .next_state = { - [RFM22B_EVENT_RX_COMPLETE] = RFM22B_STATE_SENDING_ACK, - [RFM22B_EVENT_TIMEOUT] = RFM22B_STATE_TIMEOUT, - [RFM22B_EVENT_ERROR] = RFM22B_STATE_ERROR, - [RFM22B_EVENT_INITIALIZE] = RFM22B_STATE_INITIALIZING, - [RFM22B_EVENT_FATAL_ERROR] = RFM22B_STATE_FATAL_ERROR, - }, - }, - [RFM22B_STATE_TX_START] = { - .entry_fn = rfm22_txStart, - .next_state = { - [RFM22B_EVENT_INT_RECEIVED] = RFM22B_STATE_TX_DATA, - [RFM22B_EVENT_RX_MODE] = RFM22B_STATE_RX_MODE, - [RFM22B_EVENT_TIMEOUT] = RFM22B_STATE_TIMEOUT, - [RFM22B_EVENT_ERROR] = RFM22B_STATE_ERROR, - [RFM22B_EVENT_INITIALIZE] = RFM22B_STATE_INITIALIZING, - [RFM22B_EVENT_FATAL_ERROR] = RFM22B_STATE_FATAL_ERROR, - }, - }, - [RFM22B_STATE_TX_DATA] = { - .entry_fn = rfm22_txData, - .next_state = { - [RFM22B_EVENT_INT_RECEIVED] = RFM22B_STATE_TX_DATA, - [RFM22B_EVENT_REQUEST_CONNECTION] = RFM22B_STATE_REQUESTING_CONNECTION, - [RFM22B_EVENT_RX_MODE] = RFM22B_STATE_RX_MODE, - [RFM22B_EVENT_FAILURE] = RFM22B_STATE_TX_FAILURE, - [RFM22B_EVENT_TIMEOUT] = RFM22B_STATE_TIMEOUT, - [RFM22B_EVENT_ERROR] = RFM22B_STATE_ERROR, - [RFM22B_EVENT_INITIALIZE] = RFM22B_STATE_INITIALIZING, - [RFM22B_EVENT_FATAL_ERROR] = RFM22B_STATE_FATAL_ERROR, - }, - }, - [RFM22B_STATE_TX_FAILURE] = { - .entry_fn = rfm22_txFailure, - .next_state = { - [RFM22B_EVENT_TX_START] = RFM22B_STATE_TX_START, - [RFM22B_EVENT_TIMEOUT] = RFM22B_STATE_TIMEOUT, - [RFM22B_EVENT_ERROR] = RFM22B_STATE_ERROR, - [RFM22B_EVENT_INITIALIZE] = RFM22B_STATE_INITIALIZING, - [RFM22B_EVENT_FATAL_ERROR] = RFM22B_STATE_FATAL_ERROR, - }, - }, - [RFM22B_STATE_SENDING_ACK] = { - .entry_fn = rfm22_sendAck, - .next_state = { - [RFM22B_EVENT_TX_START] = RFM22B_STATE_TX_START, - [RFM22B_EVENT_ERROR] = RFM22B_STATE_ERROR, - [RFM22B_EVENT_INITIALIZE] = RFM22B_STATE_INITIALIZING, - [RFM22B_EVENT_FATAL_ERROR] = RFM22B_STATE_FATAL_ERROR, - }, - }, - [RFM22B_STATE_SENDING_NACK] = { - .entry_fn = rfm22_sendNack, - .next_state = { - [RFM22B_EVENT_TX_START] = RFM22B_STATE_TX_START, - [RFM22B_EVENT_ERROR] = RFM22B_STATE_ERROR, - [RFM22B_EVENT_INITIALIZE] = RFM22B_STATE_INITIALIZING, - [RFM22B_EVENT_FATAL_ERROR] = RFM22B_STATE_FATAL_ERROR, - }, - }, - [RFM22B_STATE_TIMEOUT] = { - .entry_fn = rfm22_timeout, - .next_state = { - [RFM22B_EVENT_TX_START] = RFM22B_STATE_TX_START, - [RFM22B_EVENT_ERROR] = RFM22B_STATE_ERROR, - [RFM22B_EVENT_INITIALIZE] = RFM22B_STATE_INITIALIZING, - [RFM22B_EVENT_FATAL_ERROR] = RFM22B_STATE_FATAL_ERROR, - }, - }, - [RFM22B_STATE_ERROR] = { - .entry_fn = rfm22_error, - .next_state = { - [RFM22B_EVENT_ERROR] = RFM22B_STATE_ERROR, - [RFM22B_EVENT_INITIALIZE] = RFM22B_STATE_INITIALIZING, - [RFM22B_EVENT_FATAL_ERROR] = RFM22B_STATE_FATAL_ERROR, - }, - }, - [RFM22B_STATE_FATAL_ERROR] = { - .entry_fn = rfm22_fatal_error, - .next_state = { - }, - }, + [RFM22B_STATE_RX_MODE] = { + .entry_fn = rfm22_setRxMode, + .next_state = { + [RFM22B_EVENT_INT_RECEIVED] = RFM22B_STATE_WAIT_PREAMBLE, + [RFM22B_EVENT_TX_START] = RFM22B_STATE_TX_START, + [RFM22B_EVENT_ACK_TIMEOUT] = RFM22B_STATE_RECEIVING_NACK, + [RFM22B_EVENT_FAILURE] = RFM22B_STATE_RX_FAILURE, + [RFM22B_EVENT_TIMEOUT] = RFM22B_STATE_TIMEOUT, + [RFM22B_EVENT_ERROR] = RFM22B_STATE_ERROR, + [RFM22B_EVENT_INITIALIZE] = RFM22B_STATE_INITIALIZING, + [RFM22B_EVENT_FATAL_ERROR] = RFM22B_STATE_FATAL_ERROR, + }, + }, + [RFM22B_STATE_WAIT_PREAMBLE] = { + .entry_fn = rfm22_detectPreamble, + .next_state = { + [RFM22B_EVENT_PREAMBLE_DETECTED] = RFM22B_STATE_WAIT_SYNC, + [RFM22B_EVENT_TX_START] = RFM22B_STATE_TX_START, + [RFM22B_EVENT_ACK_TIMEOUT] = RFM22B_STATE_RECEIVING_NACK, + [RFM22B_EVENT_INT_RECEIVED] = RFM22B_STATE_WAIT_PREAMBLE, + [RFM22B_EVENT_FAILURE] = RFM22B_STATE_RX_FAILURE, + [RFM22B_EVENT_TIMEOUT] = RFM22B_STATE_TIMEOUT, + [RFM22B_EVENT_ERROR] = RFM22B_STATE_ERROR, + [RFM22B_EVENT_INITIALIZE] = RFM22B_STATE_INITIALIZING, + [RFM22B_EVENT_FATAL_ERROR] = RFM22B_STATE_FATAL_ERROR, + }, + }, + [RFM22B_STATE_WAIT_SYNC] = { + .entry_fn = rfm22_detectSync, + .next_state = { + [RFM22B_EVENT_INT_RECEIVED] = RFM22B_STATE_WAIT_SYNC, + [RFM22B_EVENT_SYNC_DETECTED] = RFM22B_STATE_RX_DATA, + [RFM22B_EVENT_FAILURE] = RFM22B_STATE_RX_FAILURE, + [RFM22B_EVENT_TIMEOUT] = RFM22B_STATE_TIMEOUT, + [RFM22B_EVENT_ERROR] = RFM22B_STATE_ERROR, + [RFM22B_EVENT_INITIALIZE] = RFM22B_STATE_INITIALIZING, + [RFM22B_EVENT_FATAL_ERROR] = RFM22B_STATE_FATAL_ERROR, + }, + }, + [RFM22B_STATE_RX_DATA] = { + .entry_fn = rfm22_rxData, + .next_state = { + [RFM22B_EVENT_INT_RECEIVED] = RFM22B_STATE_RX_DATA, + [RFM22B_EVENT_RX_COMPLETE] = RFM22B_STATE_SENDING_ACK, + [RFM22B_EVENT_RX_MODE] = RFM22B_STATE_RX_MODE, + [RFM22B_EVENT_RX_ERROR] = RFM22B_STATE_SENDING_NACK, + [RFM22B_EVENT_STATUS_RECEIVED] = RFM22B_STATE_RECEIVING_STATUS, + [RFM22B_EVENT_CONNECTION_REQUESTED] = RFM22B_STATE_ACCEPTING_CONNECTION, + [RFM22B_EVENT_PACKET_ACKED] = RFM22B_STATE_RECEIVING_ACK, + [RFM22B_EVENT_PACKET_NACKED] = RFM22B_STATE_RECEIVING_NACK, + [RFM22B_EVENT_FAILURE] = RFM22B_STATE_RX_FAILURE, + [RFM22B_EVENT_TIMEOUT] = RFM22B_STATE_TIMEOUT, + [RFM22B_EVENT_ERROR] = RFM22B_STATE_ERROR, + [RFM22B_EVENT_INITIALIZE] = RFM22B_STATE_INITIALIZING, + [RFM22B_EVENT_FATAL_ERROR] = RFM22B_STATE_FATAL_ERROR, + }, + }, + [RFM22B_STATE_RX_FAILURE] = { + .entry_fn = rfm22_rxFailure, + .next_state = { + [RFM22B_EVENT_RX_MODE] = RFM22B_STATE_RX_MODE, + [RFM22B_EVENT_TIMEOUT] = RFM22B_STATE_TIMEOUT, + [RFM22B_EVENT_ERROR] = RFM22B_STATE_ERROR, + [RFM22B_EVENT_INITIALIZE] = RFM22B_STATE_INITIALIZING, + [RFM22B_EVENT_FATAL_ERROR] = RFM22B_STATE_FATAL_ERROR, + }, + }, + [RFM22B_STATE_RECEIVING_ACK] = { + .entry_fn = rfm22_receiveAck, + .next_state = { + [RFM22B_EVENT_TX_START] = RFM22B_STATE_TX_START, + [RFM22B_EVENT_RX_MODE] = RFM22B_STATE_RX_MODE, + [RFM22B_EVENT_TIMEOUT] = RFM22B_STATE_TIMEOUT, + [RFM22B_EVENT_ERROR] = RFM22B_STATE_ERROR, + [RFM22B_EVENT_INITIALIZE] = RFM22B_STATE_INITIALIZING, + [RFM22B_EVENT_FATAL_ERROR] = RFM22B_STATE_FATAL_ERROR, + }, + }, + [RFM22B_STATE_RECEIVING_NACK] = { + .entry_fn = rfm22_receiveNack, + .next_state = { + [RFM22B_EVENT_TX_START] = RFM22B_STATE_TX_START, + [RFM22B_EVENT_TIMEOUT] = RFM22B_STATE_TIMEOUT, + [RFM22B_EVENT_ERROR] = RFM22B_STATE_ERROR, + [RFM22B_EVENT_INITIALIZE] = RFM22B_STATE_INITIALIZING, + [RFM22B_EVENT_FATAL_ERROR] = RFM22B_STATE_FATAL_ERROR, + }, + }, + [RFM22B_STATE_RECEIVING_STATUS] = { + .entry_fn = rfm22_receiveStatus, + .next_state = { + [RFM22B_EVENT_RX_COMPLETE] = RFM22B_STATE_SENDING_ACK, + [RFM22B_EVENT_TIMEOUT] = RFM22B_STATE_TIMEOUT, + [RFM22B_EVENT_ERROR] = RFM22B_STATE_ERROR, + [RFM22B_EVENT_INITIALIZE] = RFM22B_STATE_INITIALIZING, + [RFM22B_EVENT_FATAL_ERROR] = RFM22B_STATE_FATAL_ERROR, + }, + }, + [RFM22B_STATE_TX_START] = { + .entry_fn = rfm22_txStart, + .next_state = { + [RFM22B_EVENT_INT_RECEIVED] = RFM22B_STATE_TX_DATA, + [RFM22B_EVENT_RX_MODE] = RFM22B_STATE_RX_MODE, + [RFM22B_EVENT_TIMEOUT] = RFM22B_STATE_TIMEOUT, + [RFM22B_EVENT_ERROR] = RFM22B_STATE_ERROR, + [RFM22B_EVENT_INITIALIZE] = RFM22B_STATE_INITIALIZING, + [RFM22B_EVENT_FATAL_ERROR] = RFM22B_STATE_FATAL_ERROR, + }, + }, + [RFM22B_STATE_TX_DATA] = { + .entry_fn = rfm22_txData, + .next_state = { + [RFM22B_EVENT_INT_RECEIVED] = RFM22B_STATE_TX_DATA, + [RFM22B_EVENT_REQUEST_CONNECTION] = RFM22B_STATE_REQUESTING_CONNECTION, + [RFM22B_EVENT_RX_MODE] = RFM22B_STATE_RX_MODE, + [RFM22B_EVENT_FAILURE] = RFM22B_STATE_TX_FAILURE, + [RFM22B_EVENT_TIMEOUT] = RFM22B_STATE_TIMEOUT, + [RFM22B_EVENT_ERROR] = RFM22B_STATE_ERROR, + [RFM22B_EVENT_INITIALIZE] = RFM22B_STATE_INITIALIZING, + [RFM22B_EVENT_FATAL_ERROR] = RFM22B_STATE_FATAL_ERROR, + }, + }, + [RFM22B_STATE_TX_FAILURE] = { + .entry_fn = rfm22_txFailure, + .next_state = { + [RFM22B_EVENT_TX_START] = RFM22B_STATE_TX_START, + [RFM22B_EVENT_TIMEOUT] = RFM22B_STATE_TIMEOUT, + [RFM22B_EVENT_ERROR] = RFM22B_STATE_ERROR, + [RFM22B_EVENT_INITIALIZE] = RFM22B_STATE_INITIALIZING, + [RFM22B_EVENT_FATAL_ERROR] = RFM22B_STATE_FATAL_ERROR, + }, + }, + [RFM22B_STATE_SENDING_ACK] = { + .entry_fn = rfm22_sendAck, + .next_state = { + [RFM22B_EVENT_TX_START] = RFM22B_STATE_TX_START, + [RFM22B_EVENT_ERROR] = RFM22B_STATE_ERROR, + [RFM22B_EVENT_INITIALIZE] = RFM22B_STATE_INITIALIZING, + [RFM22B_EVENT_FATAL_ERROR] = RFM22B_STATE_FATAL_ERROR, + }, + }, + [RFM22B_STATE_SENDING_NACK] = { + .entry_fn = rfm22_sendNack, + .next_state = { + [RFM22B_EVENT_TX_START] = RFM22B_STATE_TX_START, + [RFM22B_EVENT_ERROR] = RFM22B_STATE_ERROR, + [RFM22B_EVENT_INITIALIZE] = RFM22B_STATE_INITIALIZING, + [RFM22B_EVENT_FATAL_ERROR] = RFM22B_STATE_FATAL_ERROR, + }, + }, + [RFM22B_STATE_TIMEOUT] = { + .entry_fn = rfm22_timeout, + .next_state = { + [RFM22B_EVENT_TX_START] = RFM22B_STATE_TX_START, + [RFM22B_EVENT_ERROR] = RFM22B_STATE_ERROR, + [RFM22B_EVENT_INITIALIZE] = RFM22B_STATE_INITIALIZING, + [RFM22B_EVENT_FATAL_ERROR] = RFM22B_STATE_FATAL_ERROR, + }, + }, + [RFM22B_STATE_ERROR] = { + .entry_fn = rfm22_error, + .next_state = { + [RFM22B_EVENT_ERROR] = RFM22B_STATE_ERROR, + [RFM22B_EVENT_INITIALIZE] = RFM22B_STATE_INITIALIZING, + [RFM22B_EVENT_FATAL_ERROR] = RFM22B_STATE_FATAL_ERROR, + }, + }, + [RFM22B_STATE_FATAL_ERROR] = { + .entry_fn = rfm22_fatal_error, + .next_state = { + }, + }, }; // xtal 10 ppm, 434MHz @@ -460,162 +455,133 @@ static const uint8_t reg_71[] = { 0x23, 0x23, 0x23, 0x23, 0x static const uint8_t reg_72[] = { 0x06, 0x06, 0x06, 0x06, 0x06, 0x08, 0x0D, 0x0F, 0x13, 0x1A, 0x2E, 0x33, 0x66, 0x9A, 0xCD}; // rfm22_frequency_deviation - -static inline uint32_t timeDifferenceMs(portTickType start_time, portTickType end_time) -{ - if(end_time >= start_time) - return (end_time - start_time) * portTICK_RATE_MS; - // Rollover - return ((portMAX_DELAY - start_time) + end_time) * portTICK_RATE_MS; -} - -bool PIOS_RFM22B_validate(struct pios_rfm22b_dev * rfm22b_dev) -{ - return (rfm22b_dev != NULL && rfm22b_dev->magic == PIOS_RFM22B_DEV_MAGIC); -} - -#if defined(PIOS_INCLUDE_FREERTOS) -static struct pios_rfm22b_dev * PIOS_RFM22B_alloc(void) -{ - struct pios_rfm22b_dev * rfm22b_dev; - - rfm22b_dev = (struct pios_rfm22b_dev *)pvPortMalloc(sizeof(*rfm22b_dev)); - rfm22b_dev->spi_id = 0; - if (!rfm22b_dev) return(NULL); - - rfm22b_dev->magic = PIOS_RFM22B_DEV_MAGIC; - return(rfm22b_dev); -} -#else -static struct pios_rfm22b_dev pios_rfm22b_devs[PIOS_RFM22B_MAX_DEVS]; -static uint8_t pios_rfm22b_num_devs; -static struct pios_rfm22b_dev * PIOS_RFM22B_alloc(void) -{ - struct pios_rfm22b_dev * rfm22b_dev; - - if (pios_rfm22b_num_devs >= PIOS_RFM22B_MAX_DEVS) - return NULL; - - rfm22b_dev = &pios_rfm22b_devs[pios_rfm22b_num_devs++]; - rfm22b_dev->magic = PIOS_RFM22B_DEV_MAGIC; - - return (rfm22b_dev); -} -#endif - static struct pios_rfm22b_dev * g_rfm22b_dev = NULL; + +/***************************************************************************** + * External Interface Functions + *****************************************************************************/ + /** * Initialise an RFM22B device + * + * @param[out] rfm22b_id A pointer to store the device ID in. + * @param[in] spi_id The SPI bus index. + * @param[in] slave_num The SPI bus slave number. + * @param[in] cfg The device configuration. */ int32_t PIOS_RFM22B_Init(uint32_t *rfm22b_id, uint32_t spi_id, uint32_t slave_num, const struct pios_rfm22b_cfg *cfg) { - PIOS_DEBUG_Assert(rfm22b_id); - PIOS_DEBUG_Assert(cfg); + PIOS_DEBUG_Assert(rfm22b_id); + PIOS_DEBUG_Assert(cfg); - // Allocate the device structure. - struct pios_rfm22b_dev * rfm22b_dev = (struct pios_rfm22b_dev *) PIOS_RFM22B_alloc(); - if (!rfm22b_dev) - return(-1); - *rfm22b_id = (uint32_t)rfm22b_dev; - g_rfm22b_dev = rfm22b_dev; + // Allocate the device structure. + struct pios_rfm22b_dev *rfm22b_dev = (struct pios_rfm22b_dev *)pios_rfm22_alloc(); + if (!rfm22b_dev) { + return(-1); + } + *rfm22b_id = (uint32_t)rfm22b_dev; + g_rfm22b_dev = rfm22b_dev; - // Store the SPI handle - rfm22b_dev->slave_num = slave_num; - rfm22b_dev->spi_id = spi_id; + // Store the SPI handle + rfm22b_dev->slave_num = slave_num; + rfm22b_dev->spi_id = spi_id; - // Initialize our configuration parameters - rfm22b_dev->send_ppm = false; - rfm22b_dev->datarate = RFM22B_DEFAULT_RX_DATARATE; - rfm22b_dev->tx_power = RFM22B_DEFAULT_TX_POWER; + // Initialize our configuration parameters + rfm22b_dev->send_ppm = false; + rfm22b_dev->datarate = RFM22B_DEFAULT_RX_DATARATE; + rfm22b_dev->tx_power = RFM22B_DEFAULT_TX_POWER; - // Initialize the com callbacks. - rfm22b_dev->com_config_cb = NULL; - rfm22b_dev->rx_in_cb = NULL; - rfm22b_dev->tx_out_cb = NULL; + // Initialize the com callbacks. + rfm22b_dev->com_config_cb = NULL; + rfm22b_dev->rx_in_cb = NULL; + rfm22b_dev->tx_out_cb = NULL; - // Initialize the stats. - rfm22b_dev->stats.packets_per_sec = 0; - rfm22b_dev->stats.rx_good = 0; - rfm22b_dev->stats.rx_corrected = 0; - rfm22b_dev->stats.rx_error = 0; - rfm22b_dev->stats.rx_missed = 0; - rfm22b_dev->stats.tx_dropped = 0; - rfm22b_dev->stats.tx_resent = 0; - rfm22b_dev->stats.resets = 0; - rfm22b_dev->stats.timeouts = 0; - rfm22b_dev->stats.link_quality = 0; - rfm22b_dev->stats.rssi = 0; - rfm22b_dev->stats.tx_seq = 0; - rfm22b_dev->stats.rx_seq = 0; + // Initialize the stats. + rfm22b_dev->stats.packets_per_sec = 0; + rfm22b_dev->stats.rx_good = 0; + rfm22b_dev->stats.rx_corrected = 0; + rfm22b_dev->stats.rx_error = 0; + rfm22b_dev->stats.rx_missed = 0; + rfm22b_dev->stats.tx_dropped = 0; + rfm22b_dev->stats.tx_resent = 0; + rfm22b_dev->stats.resets = 0; + rfm22b_dev->stats.timeouts = 0; + rfm22b_dev->stats.link_quality = 0; + rfm22b_dev->stats.rssi = 0; + rfm22b_dev->stats.tx_seq = 0; + rfm22b_dev->stats.rx_seq = 0; - // Initialize the frequencies. - PIOS_RFM22B_SetInitialFrequency(*rfm22b_id, RFM22B_DEFAULT_FREQUENCY); - PIOS_RFM22B_SetFrequencyRange(*rfm22b_id, RFM22B_DEFAULT_FREQUENCY, RFM22B_DEFAULT_FREQUENCY, RFM22B_FREQUENCY_HOP_STEP_SIZE); + // Initialize the frequencies. + PIOS_RFM22B_SetInitialFrequency(*rfm22b_id, RFM22B_DEFAULT_FREQUENCY); + PIOS_RFM22B_SetFrequencyRange(*rfm22b_id, RFM22B_DEFAULT_FREQUENCY, RFM22B_DEFAULT_FREQUENCY, RFM22B_FREQUENCY_HOP_STEP_SIZE); - // Initialize the bindings. - for (uint32_t i = 0; i < OPLINKSETTINGS_BINDINGS_NUMELEM; ++i) { - rfm22b_dev->bindings[i].pairID = 0; - } - rfm22b_dev->coordinator = false; + // Initialize the bindings. + for (uint32_t i = 0; i < OPLINKSETTINGS_BINDINGS_NUMELEM; ++i) { + rfm22b_dev->bindings[i].pairID = 0; + } + rfm22b_dev->coordinator = false; - // Create the event queue - rfm22b_dev->eventQueue = xQueueCreate(EVENT_QUEUE_SIZE, sizeof(enum pios_rfm22b_event)); + // Create the event queue + rfm22b_dev->eventQueue = xQueueCreate(EVENT_QUEUE_SIZE, sizeof(enum pios_rfm22b_event)); - // Bind the configuration to the device instance - rfm22b_dev->cfg = *cfg; + // Bind the configuration to the device instance + rfm22b_dev->cfg = *cfg; - // Create a semaphore to know if an ISR needs responding to - vSemaphoreCreateBinary( rfm22b_dev->isrPending ); + // Create a semaphore to know if an ISR needs responding to + vSemaphoreCreateBinary( rfm22b_dev->isrPending ); - // Create our (hopefully) unique 32 bit id from the processor serial number. - uint8_t crcs[] = { 0, 0, 0, 0 }; - { - char serial_no_str[33]; - PIOS_SYS_SerialNumberGet(serial_no_str); - // Create a 32 bit value using 4 8 bit CRC values. - for (uint8_t i = 0; serial_no_str[i] != 0; ++i) - crcs[i % 4] = PIOS_CRC_updateByte(crcs[i % 4], serial_no_str[i]); - } - rfm22b_dev->deviceID = crcs[0] | crcs[1] << 8 | crcs[2] << 16 | crcs[3] << 24; - DEBUG_PRINTF(2, "RF device ID: %x\n\r", rfm22b_dev->deviceID); + // Create our (hopefully) unique 32 bit id from the processor serial number. + uint8_t crcs[] = { 0, 0, 0, 0 }; + { + char serial_no_str[33]; + PIOS_SYS_SerialNumberGet(serial_no_str); + // Create a 32 bit value using 4 8 bit CRC values. + for (uint8_t i = 0; serial_no_str[i] != 0; ++i) + crcs[i % 4] = PIOS_CRC_updateByte(crcs[i % 4], serial_no_str[i]); + } + rfm22b_dev->deviceID = crcs[0] | crcs[1] << 8 | crcs[2] << 16 | crcs[3] << 24; + DEBUG_PRINTF(2, "RF device ID: %x\n\r", rfm22b_dev->deviceID); #if defined(PIOS_INCLUDE_GCSRCVR) - // Initialize the GCSReceive object - GCSReceiverInitialize(); + // Initialize the GCSReceive object + GCSReceiverInitialize(); #endif - // Initialize the external interrupt. - PIOS_EXTI_Init(cfg->exti_cfg); + // Initialize the external interrupt. + PIOS_EXTI_Init(cfg->exti_cfg); - // Register the watchdog timer for the radio driver task + // Register the watchdog timer for the radio driver task #ifdef PIOS_WDG_RFM22B - PIOS_WDG_RegisterFlag(PIOS_WDG_RFM22B); + PIOS_WDG_RegisterFlag(PIOS_WDG_RFM22B); #endif /* PIOS_WDG_RFM22B */ - // Initialize the ECC library. - initialize_ecc(); + // Initialize the ECC library. + initialize_ecc(); - // Set the state to initializing. - rfm22b_dev->state = RFM22B_STATE_UNINITIALIZED; + // Set the state to initializing. + rfm22b_dev->state = RFM22B_STATE_UNINITIALIZED; - // Initialize the radio device. - pios_rfm22_inject_event(rfm22b_dev, RFM22B_EVENT_INITIALIZE, false); + // Initialize the radio device. + pios_rfm22_inject_event(rfm22b_dev, RFM22B_EVENT_INITIALIZE, false); - // Start the driver task. This task controls the radio state machine and removed all of the IO from the IRQ handler. - xTaskCreate(PIOS_RFM22B_Task, (signed char *)"PIOS_RFM22B_Task", STACK_SIZE_BYTES, (void*)rfm22b_dev, TASK_PRIORITY, &(rfm22b_dev->taskHandle)); + // Start the driver task. This task controls the radio state machine and removed all of the IO from the IRQ handler. + xTaskCreate(pios_rfm22_task, (signed char *)"PIOS_RFM22B_Task", STACK_SIZE_BYTES, (void*)rfm22b_dev, TASK_PRIORITY, &(rfm22b_dev->taskHandle)); - return(0); + return(0); } /** * Re-initialize the modem after a configuration change. + * + * @param[in] rbm22b_id The RFM22B device ID. */ void PIOS_RFM22B_Reinit(uint32_t rfm22b_id) { - struct pios_rfm22b_dev *rfm22b_dev = (struct pios_rfm22b_dev *)rfm22b_id; - if (PIOS_RFM22B_validate(rfm22b_dev)) - pios_rfm22_inject_event(rfm22b_dev, RFM22B_EVENT_INITIALIZE, false); + struct pios_rfm22b_dev *rfm22b_dev = (struct pios_rfm22b_dev *)rfm22b_id; + if (PIOS_RFM22B_Validate(rfm22b_dev)) { + pios_rfm22_inject_event(rfm22b_dev, RFM22B_EVENT_INITIALIZE, false); + } } /** @@ -623,562 +589,640 @@ void PIOS_RFM22B_Reinit(uint32_t rfm22b_id) */ bool PIOS_RFM22_EXT_Int(void) { - if (!PIOS_RFM22B_validate(g_rfm22b_dev)) - return false; - - // Inject an interrupt event into the state machine. - pios_rfm22_inject_event(g_rfm22b_dev, RFM22B_EVENT_INT_RECEIVED, true); + if (!PIOS_RFM22B_Validate(g_rfm22b_dev)) { return false; + } + + // Inject an interrupt event into the state machine. + pios_rfm22_inject_event(g_rfm22b_dev, RFM22B_EVENT_INT_RECEIVED, true); + return false; } /** * Returns the unique device ID for the RFM22B device. - * \param[in] rfm22b_id The RFM22B device index. - * \return The unique device ID + * + * @param[in] rfm22b_id The RFM22B device index. + * @return The unique device ID */ uint32_t PIOS_RFM22B_DeviceID(uint32_t rfm22b_id) { - struct pios_rfm22b_dev *rfm22b_dev = (struct pios_rfm22b_dev *)rfm22b_id; - if (PIOS_RFM22B_validate(rfm22b_dev)) { - return rfm22b_dev->deviceID; - } else { - return 0; - } + struct pios_rfm22b_dev *rfm22b_dev = (struct pios_rfm22b_dev *)rfm22b_id; + if (PIOS_RFM22B_Validate(rfm22b_dev)) { + return rfm22b_dev->deviceID; + } + return 0; } /** * Returns true if the modem is configured as a coordinator. - * \param[in] rfm22b_id The RFM22B device index. - * \return True if the modem is configured as a coordinator. + * + * @param[in] rfm22b_id The RFM22B device index. + * @return True if the modem is configured as a coordinator. */ bool PIOS_RFM22B_IsCoordinator(uint32_t rfm22b_id) { - struct pios_rfm22b_dev *rfm22b_dev = (struct pios_rfm22b_dev *)rfm22b_id; - if (PIOS_RFM22B_validate(rfm22b_dev)) { - return rfm22b_dev->coordinator; - } else { - return false; - } + struct pios_rfm22b_dev *rfm22b_dev = (struct pios_rfm22b_dev *)rfm22b_id; + if (PIOS_RFM22B_Validate(rfm22b_dev)) { + return rfm22b_dev->coordinator; + } + return false; } /** * Sets the radio device transmit power. - * \param[in] rfm22b_id The RFM22B device index. - * \param[in] tx_pwr The transmit power. + * + * @param[in] rfm22b_id The RFM22B device index. + * @param[in] tx_pwr The transmit power. */ void PIOS_RFM22B_SetTxPower(uint32_t rfm22b_id, enum rfm22b_tx_power tx_pwr) { - struct pios_rfm22b_dev *rfm22b_dev = (struct pios_rfm22b_dev *)rfm22b_id; - if (!PIOS_RFM22B_validate(rfm22b_dev)) { - return; - } - rfm22b_dev->tx_power = tx_pwr; + struct pios_rfm22b_dev *rfm22b_dev = (struct pios_rfm22b_dev *)rfm22b_id; + if (!PIOS_RFM22B_Validate(rfm22b_dev)) { + return; + } + rfm22b_dev->tx_power = tx_pwr; } /** * Sets the radio frequency range and initial frequency - * \param[in] rfm22b_id The RFM22B device index. - * \param[in] min_freq The minimum frequency - * \param[in] max_freq The maximum frequency - * \param[in] step_size The channel step size + * + * @param[in] rfm22b_id The RFM22B device index. + * @param[in] min_freq The minimum frequency + * @param[in] max_freq The maximum frequency + * @param[in] step_size The channel step size */ void PIOS_RFM22B_SetFrequencyRange(uint32_t rfm22b_id, uint32_t min_freq, uint32_t max_freq, uint32_t step_size) { - struct pios_rfm22b_dev *rfm22b_dev = (struct pios_rfm22b_dev *)rfm22b_id; - if (!PIOS_RFM22B_validate(rfm22b_dev)) { - return; - } - rfm22b_dev->con_packet.min_frequency = min_freq; - rfm22b_dev->con_packet.max_frequency = max_freq; - rfm22b_dev->con_packet.channel_spacing = step_size; + struct pios_rfm22b_dev *rfm22b_dev = (struct pios_rfm22b_dev *)rfm22b_id; + if (!PIOS_RFM22B_Validate(rfm22b_dev)) { + return; + } + rfm22b_dev->con_packet.min_frequency = min_freq; + rfm22b_dev->con_packet.max_frequency = max_freq; + rfm22b_dev->con_packet.channel_spacing = step_size; } /** * Sets the initial radio frequency range - * \param[in] rfm22b_id The RFM22B device index. - * \param[in] init_freq The initial frequency + * + * @param[in] rfm22b_id The RFM22B device index. + * @param[in] init_freq The initial frequency */ void PIOS_RFM22B_SetInitialFrequency(uint32_t rfm22b_id, uint32_t init_freq) { - struct pios_rfm22b_dev *rfm22b_dev = (struct pios_rfm22b_dev *)rfm22b_id; - if (!PIOS_RFM22B_validate(rfm22b_dev)) { - return; - } - rfm22b_dev->init_frequency = init_freq; + struct pios_rfm22b_dev *rfm22b_dev = (struct pios_rfm22b_dev *)rfm22b_id; + if (!PIOS_RFM22B_Validate(rfm22b_dev)) { + return; + } + rfm22b_dev->init_frequency = init_freq; } /** * Set the com port configuration callback (to receive com configuration over the air) - * \param[in] rfm22b_id The rfm22b device. - * \param[in] cb A pointer to the callback function + * + * @param[in] rfm22b_id The rfm22b device. + * @param[in] cb A pointer to the callback function */ void PIOS_RFM22B_SetComConfigCallback(uint32_t rfm22b_id, PIOS_RFM22B_ComConfigCallback cb) { - struct pios_rfm22b_dev *rfm22b_dev = (struct pios_rfm22b_dev *)rfm22b_id; - if(!PIOS_RFM22B_validate(rfm22b_dev)) { - return; - } - rfm22b_dev->com_config_cb = cb; + struct pios_rfm22b_dev *rfm22b_dev = (struct pios_rfm22b_dev *)rfm22b_id; + if(!PIOS_RFM22B_Validate(rfm22b_dev)) { + return; + } + rfm22b_dev->com_config_cb = cb; } /** * Set the list of modems that this modem will bind with. - * \param[in] rfm22b_id The rfm22b device. - * \param[in] bindings The array of bindings. + * + * @param[in] rfm22b_id The rfm22b device. + * @param[in] bindingPairIDs The array of binding IDs. + * @param[in] mainPortSettings The array of main com port configurations. + * @param[in] flexiPortSettings The array of flexi com port configurations. + * @param[in] vcpPortSettings The array of VCP com port configurations. + * @param[in] comSpeeds The array of com port speeds. */ void PIOS_RFM22B_SetBindings(uint32_t rfm22b_id, const uint32_t bindingPairIDs[], const uint8_t mainPortSettings[], const uint8_t flexiPortSettings[], const uint8_t vcpPortSettings[], const uint8_t comSpeeds[]) { - struct pios_rfm22b_dev *rfm22b_dev = (struct pios_rfm22b_dev *)rfm22b_id; - if(!PIOS_RFM22B_validate(rfm22b_dev)) - return; - // This modem will be considered a coordinator if any bindings have been set. - rfm22b_dev->coordinator = false; - for (uint32_t i = 0; i < OPLINKSETTINGS_BINDINGS_NUMELEM; ++i) { - rfm22b_dev->bindings[i].pairID = bindingPairIDs[i]; - rfm22b_dev->bindings[i].main_port = mainPortSettings[i]; - rfm22b_dev->bindings[i].flexi_port = flexiPortSettings[i]; - rfm22b_dev->bindings[i].vcp_port = vcpPortSettings[i]; - rfm22b_dev->bindings[i].com_speed = comSpeeds[i]; - rfm22b_dev->coordinator |= (rfm22b_dev->bindings[i].pairID != 0); - } + struct pios_rfm22b_dev *rfm22b_dev = (struct pios_rfm22b_dev *)rfm22b_id; + if(!PIOS_RFM22B_Validate(rfm22b_dev)) { + return; + } + + // This modem will be considered a coordinator if any bindings have been set. + rfm22b_dev->coordinator = false; + for (uint32_t i = 0; i < OPLINKSETTINGS_BINDINGS_NUMELEM; ++i) { + rfm22b_dev->bindings[i].pairID = bindingPairIDs[i]; + rfm22b_dev->bindings[i].main_port = mainPortSettings[i]; + rfm22b_dev->bindings[i].flexi_port = flexiPortSettings[i]; + rfm22b_dev->bindings[i].vcp_port = vcpPortSettings[i]; + rfm22b_dev->bindings[i].com_speed = comSpeeds[i]; + rfm22b_dev->coordinator |= (rfm22b_dev->bindings[i].pairID != 0); + } } /** * Returns the device statistics RFM22B device. - * \param[in] rfm22b_id The RFM22B device index. - * \param[out] stats The stats are returned in this structure + * + * @param[in] rfm22b_id The RFM22B device index. + * @param[out] stats The stats are returned in this structure */ void PIOS_RFM22B_GetStats(uint32_t rfm22b_id, struct rfm22b_stats *stats) { - struct pios_rfm22b_dev *rfm22b_dev = (struct pios_rfm22b_dev *)rfm22b_id; - if(!PIOS_RFM22B_validate(rfm22b_dev)) - return; + struct pios_rfm22b_dev *rfm22b_dev = (struct pios_rfm22b_dev *)rfm22b_id; + if(!PIOS_RFM22B_Validate(rfm22b_dev)) { + return; + } - // Calculate the current link quality - rfm22_calculateLinkQuality(rfm22b_dev); + // Calculate the current link quality + rfm22_calculateLinkQuality(rfm22b_dev); - // We are connected if our destination ID is in the pair stats. - if (rfm22b_dev->destination_id != 0xffffffff) - for (uint8_t i = 0; i < OPLINKSTATUS_PAIRIDS_NUMELEM; ++i) - { - if ((rfm22b_dev->pair_stats[i].pairID == rfm22b_dev->destination_id) && - (rfm22b_dev->pair_stats[i].rssi > -127)) - { - rfm22b_dev->stats.rssi = rfm22b_dev->pair_stats[i].rssi; - rfm22b_dev->stats.afc_correction = rfm22b_dev->pair_stats[i].afc_correction; - break; - } - } - *stats = rfm22b_dev->stats; + // We are connected if our destination ID is in the pair stats. + if (rfm22b_dev->destination_id != 0xffffffff) { + for (uint8_t i = 0; i < OPLINKSTATUS_PAIRIDS_NUMELEM; ++i) { + if ((rfm22b_dev->pair_stats[i].pairID == rfm22b_dev->destination_id) && + (rfm22b_dev->pair_stats[i].rssi > -127)) { + rfm22b_dev->stats.rssi = rfm22b_dev->pair_stats[i].rssi; + rfm22b_dev->stats.afc_correction = rfm22b_dev->pair_stats[i].afc_correction; + break; + } + } + } + *stats = rfm22b_dev->stats; } /** * Get the stats of the oter radio devices that are in range. - * \param[out] device_ids A pointer to the array to store the device IDs. - * \param[out] RSSIs A pointer to the array to store the RSSI values in. - * \param[in] mx_pairs The length of the pdevice_ids and RSSIs arrays. - * \return The number of pair stats returned. + * + * @param[out] device_ids A pointer to the array to store the device IDs. + * @param[out] RSSIs A pointer to the array to store the RSSI values in. + * @param[in] mx_pairs The length of the pdevice_ids and RSSIs arrays. + * @return The number of pair stats returned. */ uint8_t PIOS_RFM2B_GetPairStats(uint32_t rfm22b_id, uint32_t *device_ids, int8_t *RSSIs, uint8_t max_pairs) { - struct pios_rfm22b_dev *rfm22b_dev = (struct pios_rfm22b_dev *)rfm22b_id; - if (!PIOS_RFM22B_validate(rfm22b_dev)) - return 0; + struct pios_rfm22b_dev *rfm22b_dev = (struct pios_rfm22b_dev *)rfm22b_id; + if (!PIOS_RFM22B_Validate(rfm22b_dev)) { + return 0; + } - uint8_t mp = (max_pairs >= OPLINKSTATUS_PAIRIDS_NUMELEM) ? max_pairs : OPLINKSTATUS_PAIRIDS_NUMELEM; - for (uint8_t i = 0; i < mp; ++i) - { - device_ids[i] = rfm22b_dev->pair_stats[i].pairID; - RSSIs[i] = rfm22b_dev->pair_stats[i].rssi; - } + uint8_t mp = (max_pairs >= OPLINKSTATUS_PAIRIDS_NUMELEM) ? max_pairs : OPLINKSTATUS_PAIRIDS_NUMELEM; + for (uint8_t i = 0; i < mp; ++i) { + device_ids[i] = rfm22b_dev->pair_stats[i].pairID; + RSSIs[i] = rfm22b_dev->pair_stats[i].rssi; + } - return mp; + return mp; } /** * Check the radio device for a valid connection - * \param[in] rfm22b_id The rfm22b device. - * Returns true if there is a valid connection to paired radio, false otherwise. + * + * @param[in] rfm22b_id The rfm22b device. + * @return true if there is a valid connection to paired radio, false otherwise. */ bool PIOS_RFM22B_LinkStatus(uint32_t rfm22b_id) { - struct pios_rfm22b_dev *rfm22b_dev = (struct pios_rfm22b_dev *)rfm22b_id; - if(!PIOS_RFM22B_validate(rfm22b_dev)) - return false; - return (rfm22_isConnected(rfm22b_dev) && (rfm22b_dev->stats.link_quality > RFM22B_LINK_QUALITY_THRESHOLD)); + struct pios_rfm22b_dev *rfm22b_dev = (struct pios_rfm22b_dev *)rfm22b_id; + if(!PIOS_RFM22B_Validate(rfm22b_dev)) { + return false; + } + return (rfm22_isConnected(rfm22b_dev) && (rfm22b_dev->stats.link_quality > RFM22B_LINK_QUALITY_THRESHOLD)); } + +/***************************************************************************** + * The Device Control Thread + *****************************************************************************/ + /** * The task that controls the radio state machine. + * + * @param[in] paramters The task parameters. */ -static void PIOS_RFM22B_Task(void *parameters) +static void pios_rfm22_task(void *parameters) { - struct pios_rfm22b_dev *rfm22b_dev = (struct pios_rfm22b_dev *)parameters; - if (!PIOS_RFM22B_validate(rfm22b_dev)) - return; - portTickType lastEventTicks = xTaskGetTickCount(); - portTickType lastStatusTicks = lastEventTicks; - portTickType lastPPMTicks = lastEventTicks; + struct pios_rfm22b_dev *rfm22b_dev = (struct pios_rfm22b_dev *)parameters; + if (!PIOS_RFM22B_Validate(rfm22b_dev)) { + return; + } + portTickType lastEventTicks = xTaskGetTickCount(); + portTickType lastStatusTicks = lastEventTicks; + portTickType lastPPMTicks = lastEventTicks; - while(1) - { + while(1) { #ifdef PIOS_WDG_RFM22B - // Update the watchdog timer - PIOS_WDG_UpdateFlag(PIOS_WDG_RFM22B); + // Update the watchdog timer + PIOS_WDG_UpdateFlag(PIOS_WDG_RFM22B); #endif /* PIOS_WDG_RFM22B */ - // Wait for a signal indicating an external interrupt or a pending send/receive request. - if (xSemaphoreTake(rfm22b_dev->isrPending, ISR_TIMEOUT / portTICK_RATE_MS) == pdTRUE) { - lastEventTicks = xTaskGetTickCount(); + // Wait for a signal indicating an external interrupt or a pending send/receive request. + if (xSemaphoreTake(rfm22b_dev->isrPending, ISR_TIMEOUT / portTICK_RATE_MS) == pdTRUE) { + lastEventTicks = xTaskGetTickCount(); - // Process events through the state machine. - enum pios_rfm22b_event event; - while (xQueueReceive(rfm22b_dev->eventQueue, &event, 0) == pdTRUE) - { - if ((event == RFM22B_EVENT_INT_RECEIVED) && - ((rfm22b_dev->state == RFM22B_STATE_UNINITIALIZED) || (rfm22b_dev->state == RFM22B_STATE_INITIALIZING))) - continue; - rfm22_process_event(rfm22b_dev, event); - } - } - else - { - // Has it been too long since the last event? - portTickType curTicks = xTaskGetTickCount(); - if (timeDifferenceMs(lastEventTicks, curTicks) > PIOS_RFM22B_SUPERVISOR_TIMEOUT) - { - // Transsition through an error event. - rfm22_process_event(rfm22b_dev, RFM22B_EVENT_ERROR); + // Process events through the state machine. + enum pios_rfm22b_event event; + while (xQueueReceive(rfm22b_dev->eventQueue, &event, 0) == pdTRUE) { + if ((event == RFM22B_EVENT_INT_RECEIVED) && + ((rfm22b_dev->state == RFM22B_STATE_UNINITIALIZED) || (rfm22b_dev->state == RFM22B_STATE_INITIALIZING))) + continue; + rfm22_process_event(rfm22b_dev, event); + } + } else { + // Has it been too long since the last event? + portTickType curTicks = xTaskGetTickCount(); + if (pios_rfm22_time_difference_ms(lastEventTicks, curTicks) > PIOS_RFM22B_SUPERVISOR_TIMEOUT) { + // Transsition through an error event. + rfm22_process_event(rfm22b_dev, RFM22B_EVENT_ERROR); - // Clear the event queue. - enum pios_rfm22b_event event; - while (xQueueReceive(rfm22b_dev->eventQueue, &event, 0) == pdTRUE) - ; - lastEventTicks = xTaskGetTickCount(); - } - } + // Clear the event queue. + enum pios_rfm22b_event event; + while (xQueueReceive(rfm22b_dev->eventQueue, &event, 0) == pdTRUE) { + // Do nothing; + } + lastEventTicks = xTaskGetTickCount(); + } + } - // Change channels if necessary. - if ((rfm22b_dev->state == RFM22B_STATE_RX_MODE) || (rfm22b_dev->state == RFM22B_STATE_WAIT_PREAMBLE)) { - rfm22_changeChannel(rfm22b_dev); - } + // Change channels if necessary. + if ((rfm22b_dev->state == RFM22B_STATE_RX_MODE) || (rfm22b_dev->state == RFM22B_STATE_WAIT_PREAMBLE)) { + rfm22_changeChannel(rfm22b_dev); + } - portTickType curTicks = xTaskGetTickCount(); - uint32_t last_rec_ms = (rfm22b_dev->rx_complete_ticks == 0) ? 0 : timeDifferenceMs(rfm22b_dev->rx_complete_ticks, curTicks); - // Have we been sending this packet too long? - if ((rfm22b_dev->packet_start_ticks > 0) && (timeDifferenceMs(rfm22b_dev->packet_start_ticks, curTicks) > (rfm22b_dev->max_packet_time * 3))) { - rfm22_process_event(rfm22b_dev, RFM22B_EVENT_TIMEOUT); + portTickType curTicks = xTaskGetTickCount(); + uint32_t last_rec_ms = (rfm22b_dev->rx_complete_ticks == 0) ? 0 : pios_rfm22_time_difference_ms(rfm22b_dev->rx_complete_ticks, curTicks); + // Have we been sending this packet too long? + if ((rfm22b_dev->packet_start_ticks > 0) && (pios_rfm22_time_difference_ms(rfm22b_dev->packet_start_ticks, curTicks) > (rfm22b_dev->max_packet_time * 3))) { + rfm22_process_event(rfm22b_dev, RFM22B_EVENT_TIMEOUT); - // Has it been too long since we received a packet - } else if (last_rec_ms > DISCONNECT_TIMEOUT_MS) { - rfm22_process_event(rfm22b_dev, RFM22B_EVENT_ERROR); - } else { + // Has it been too long since we received a packet + } else if (last_rec_ms > DISCONNECT_TIMEOUT_MS) { + rfm22_process_event(rfm22b_dev, RFM22B_EVENT_ERROR); + } else { - // Are we waiting for an ACK? - if (rfm22b_dev->prev_tx_packet) - { + // Are we waiting for an ACK? + if (rfm22b_dev->prev_tx_packet) { - // Should we resend the packet? - if (timeDifferenceMs(rfm22b_dev->tx_complete_ticks, curTicks) > rfm22b_dev->max_ack_delay) - { - rfm22b_dev->tx_complete_ticks = curTicks; - rfm22_process_event(rfm22b_dev, RFM22B_EVENT_ACK_TIMEOUT); - } - } - else - { + // Should we resend the packet? + if (pios_rfm22_time_difference_ms(rfm22b_dev->tx_complete_ticks, curTicks) > rfm22b_dev->max_ack_delay) { + rfm22b_dev->tx_complete_ticks = curTicks; + rfm22_process_event(rfm22b_dev, RFM22B_EVENT_ACK_TIMEOUT); + } - // Queue up a PPM packet if it's time. - if (timeDifferenceMs(lastPPMTicks, curTicks) > PPM_UPDATE_PERIOD_MS) - { - rfm22_sendPPM(rfm22b_dev); - lastPPMTicks = curTicks; - } + } else { - // Queue up a status packet if it's time. - if ((timeDifferenceMs(lastStatusTicks, curTicks) > RADIOSTATS_UPDATE_PERIOD_MS) || (last_rec_ms > rfm22b_dev->max_packet_time * 4)) - { - rfm22_sendStatus(rfm22b_dev); - lastStatusTicks = curTicks; - } - } + // Queue up a PPM packet if it's time. + if (pios_rfm22_time_difference_ms(lastPPMTicks, curTicks) > PPM_UPDATE_PERIOD_MS) { + rfm22_sendPPM(rfm22b_dev); + lastPPMTicks = curTicks; + } - } + // Queue up a status packet if it's time. + if ((pios_rfm22_time_difference_ms(lastStatusTicks, curTicks) > RADIOSTATS_UPDATE_PERIOD_MS) || (last_rec_ms > rfm22b_dev->max_packet_time * 4)) { + rfm22_sendStatus(rfm22b_dev); + lastStatusTicks = curTicks; + } + } - // Send a packet if it's our time slice - rfm22b_dev->time_to_send = (((curTicks - rfm22b_dev->time_to_send_offset) & 0x6) == 0); + } + + // Send a packet if it's our time slice + rfm22b_dev->time_to_send = (((curTicks - rfm22b_dev->time_to_send_offset) & 0x6) == 0); #if defined(PIOS_RFM22B_DEBUG_ON_TELEM) || defined(PIOS_RFM22B_DEBUG_ON_RCVR) - if (rfm22b_dev->time_to_send) { - D4_LED_ON; - } else { - D4_LED_OFF; - } - if (rfm22_inChannelBuffer(rfm22b_dev)) { - D3_LED_ON; - } else { - D3_LED_OFF; - } + if (rfm22b_dev->time_to_send) { + D4_LED_ON; + } else { + D4_LED_OFF; + } + if (rfm22_inChannelBuffer(rfm22b_dev)) { + D3_LED_ON; + } else { + D3_LED_OFF; + } #endif - if (rfm22b_dev->time_to_send) - rfm22_process_event(rfm22b_dev, RFM22B_EVENT_TX_START); - } + if (rfm22b_dev->time_to_send) { + rfm22_process_event(rfm22b_dev, RFM22B_EVENT_TX_START); + } + } } -/** - * Initialize (or re-initialize) the RFM22B radio device. - * - * \param[in] rfm22b_dev The device structure - */ -static enum pios_rfm22b_event rfm22_init(struct pios_rfm22b_dev *rfm22b_dev) -{ - // Initialize the register values. - rfm22b_dev->device_status = 0; - rfm22b_dev->int_status1 = 0; - rfm22b_dev->int_status2 = 0; - rfm22b_dev->ezmac_status = 0; - - // Clean the LEDs - rfm22_clearLEDs(); - - // Initialize the detected device statistics. - for (uint8_t i = 0; i < OPLINKSTATUS_PAIRIDS_NUMELEM; ++i) { - rfm22b_dev->pair_stats[i].pairID = 0; - rfm22b_dev->pair_stats[i].rssi = -127; - rfm22b_dev->pair_stats[i].afc_correction = 0; - rfm22b_dev->pair_stats[i].lastContact = 0; - } - - // Initlize the link stats. - for (uint8_t i = 0; i < RFM22B_RX_PACKET_STATS_LEN; ++i) - rfm22b_dev->rx_packet_stats[i] = 0; - - // Initialize the state - rfm22b_dev->stats.link_state = OPLINKSTATUS_LINKSTATE_DISCONNECTED; - rfm22b_dev->destination_id = 0xffffffff; - rfm22b_dev->time_to_send = false; - rfm22b_dev->time_to_send_offset = 0; - rfm22b_dev->send_status = false; - rfm22b_dev->send_connection_request = false; - - // Initialize the packets. - rfm22b_dev->rx_packet_len = 0; - rfm22b_dev->tx_packet = NULL; - rfm22b_dev->prev_tx_packet = NULL; - rfm22b_dev->data_packet.header.data_size = 0; - rfm22b_dev->in_rx_mode = false; - - // Initialize the devide state - rfm22b_dev->device_status = rfm22b_dev->int_status1 = rfm22b_dev->int_status2 = rfm22b_dev->ezmac_status = 0; - rfm22b_dev->rx_buffer_wr = 0; - rfm22b_dev->tx_data_rd = rfm22b_dev->tx_data_wr = 0; - rfm22b_dev->frequency_hop_channel = 0; - rfm22b_dev->afc_correction_Hz = 0; - rfm22b_dev->packet_start_ticks = 0; - rfm22b_dev->tx_complete_ticks = 0; - rfm22b_dev->rx_complete_ticks = 0; - - // software reset the RF chip .. following procedure according to Si4x3x Errata (rev. B) - rfm22_write(rfm22b_dev, RFM22_op_and_func_ctrl1, RFM22_opfc1_swres); - - for (int i = 50; i > 0; i--) { - // read the status registers - rfm22b_dev->int_status1 = rfm22_read(rfm22b_dev, RFM22_interrupt_status1); - rfm22b_dev->int_status2 = rfm22_read(rfm22b_dev, RFM22_interrupt_status2); - if (rfm22b_dev->int_status2 & RFM22_is2_ichiprdy) break; - - // wait 1ms - PIOS_DELAY_WaitmS(1); - } - - // **************** - - // read status - clears interrupt - rfm22b_dev->device_status = rfm22_read(rfm22b_dev, RFM22_device_status); - rfm22b_dev->int_status1 = rfm22_read(rfm22b_dev, RFM22_interrupt_status1); - rfm22b_dev->int_status2 = rfm22_read(rfm22b_dev, RFM22_interrupt_status2); - rfm22b_dev->ezmac_status = rfm22_read(rfm22b_dev, RFM22_ezmac_status); - - // disable all interrupts - rfm22_write(rfm22b_dev, RFM22_interrupt_enable1, 0x00); - rfm22_write(rfm22b_dev, RFM22_interrupt_enable2, 0x00); - - // read the RF chip ID bytes - - // read the device type - uint8_t device_type = rfm22_read(rfm22b_dev, RFM22_DEVICE_TYPE) & RFM22_DT_MASK; - // read the device version - uint8_t device_version = rfm22_read(rfm22b_dev, RFM22_DEVICE_VERSION) & RFM22_DV_MASK; - -#if defined(RFM22_DEBUG) - DEBUG_PRINTF(2, "rf device type: %d\n\r", device_type); - DEBUG_PRINTF(2, "rf device version: %d\n\r", device_version); -#endif - - if (device_type != 0x08) - { -#if defined(RFM22_DEBUG) - DEBUG_PRINTF(2, "rf device type: INCORRECT - should be 0x08\n\r"); -#endif - // incorrect RF module type - return RFM22B_EVENT_FATAL_ERROR; - } - if (device_version != RFM22_DEVICE_VERSION_B1) - { -#if defined(RFM22_DEBUG) - DEBUG_PRINTF(2, "rf device version: INCORRECT\n\r"); -#endif - // incorrect RF module version - return RFM22B_EVENT_FATAL_ERROR; - } - - // calibrate our RF module to be exactly on frequency .. different for every module - rfm22_write(rfm22b_dev, RFM22_xtal_osc_load_cap, OSC_LOAD_CAP); - - // disable Low Duty Cycle Mode - rfm22_write(rfm22b_dev, RFM22_op_and_func_ctrl2, 0x00); - - // 1MHz clock output - rfm22_write(rfm22b_dev, RFM22_cpu_output_clk, RFM22_coc_1MHz); - - // READY mode - rfm22_write(rfm22b_dev, RFM22_op_and_func_ctrl1, RFM22_opfc1_xton); - - // choose the 3 GPIO pin functions - // GPIO port use default value - rfm22_write(rfm22b_dev, RFM22_io_port_config, RFM22_io_port_default); - if (rfm22b_dev->cfg.gpio_direction == GPIO0_TX_GPIO1_RX) { - // GPIO0 = TX State (to control RF Switch) - rfm22_write(rfm22b_dev, RFM22_gpio0_config, RFM22_gpio0_config_drv3 | RFM22_gpio0_config_txstate); - // GPIO1 = RX State (to control RF Switch) - rfm22_write(rfm22b_dev, RFM22_gpio1_config, RFM22_gpio1_config_drv3 | RFM22_gpio1_config_rxstate); - } else { - // GPIO0 = TX State (to control RF Switch) - rfm22_write(rfm22b_dev, RFM22_gpio0_config, RFM22_gpio0_config_drv3 | RFM22_gpio0_config_rxstate); - // GPIO1 = RX State (to control RF Switch) - rfm22_write(rfm22b_dev, RFM22_gpio1_config, RFM22_gpio1_config_drv3 | RFM22_gpio1_config_txstate); - } - // GPIO2 = Clear Channel Assessment - rfm22_write(rfm22b_dev, RFM22_gpio2_config, RFM22_gpio2_config_drv3 | RFM22_gpio2_config_cca); - - // FIFO mode, GFSK modulation - uint8_t fd_bit = rfm22_read(rfm22b_dev, RFM22_modulation_mode_control2) & RFM22_mmc2_fd; - rfm22_write(rfm22b_dev, RFM22_modulation_mode_control2, RFM22_mmc2_trclk_clk_none | RFM22_mmc2_dtmod_fifo | fd_bit | RFM22_mmc2_modtyp_gfsk); - - // setup to read the internal temperature sensor - - // ADC used to sample the temperature sensor - uint8_t adc_config = RFM22_ac_adcsel_temp_sensor | RFM22_ac_adcref_bg; - rfm22_write(rfm22b_dev, RFM22_adc_config, adc_config); - - // adc offset - rfm22_write(rfm22b_dev, RFM22_adc_sensor_amp_offset, 0); - - // temp sensor calibration .. �40C to +64C 0.5C resolution - rfm22_write(rfm22b_dev, RFM22_temp_sensor_calib, RFM22_tsc_tsrange0 | RFM22_tsc_entsoffs); - - // temp sensor offset - rfm22_write(rfm22b_dev, RFM22_temp_value_offset, 0); - - // start an ADC conversion - rfm22_write(rfm22b_dev, RFM22_adc_config, adc_config | RFM22_ac_adcstartbusy); - - // set the RSSI threshold interrupt to about -90dBm - rfm22_write(rfm22b_dev, RFM22_rssi_threshold_clear_chan_indicator, (-90 + 122) * 2); - - // enable the internal Tx & Rx packet handlers (without CRC) - rfm22_write(rfm22b_dev, RFM22_data_access_control, RFM22_dac_enpacrx | RFM22_dac_enpactx); - - // x-nibbles tx preamble - rfm22_write(rfm22b_dev, RFM22_preamble_length, TX_PREAMBLE_NIBBLES); - // x-nibbles rx preamble detection - rfm22_write(rfm22b_dev, RFM22_preamble_detection_ctrl1, RX_PREAMBLE_NIBBLES << 3); - - // header control - using a 4 by header with broadcast of 0xffffffff - rfm22_write(rfm22b_dev, RFM22_header_control1, - RFM22_header_cntl1_bcen_0 | - RFM22_header_cntl1_bcen_1 | - RFM22_header_cntl1_bcen_2 | - RFM22_header_cntl1_bcen_3 | - RFM22_header_cntl1_hdch_0 | - RFM22_header_cntl1_hdch_1 | - RFM22_header_cntl1_hdch_2 | - RFM22_header_cntl1_hdch_3); - // Check all bit of all bytes of the header - rfm22_write(rfm22b_dev, RFM22_header_enable0, 0xff); - rfm22_write(rfm22b_dev, RFM22_header_enable1, 0xff); - rfm22_write(rfm22b_dev, RFM22_header_enable2, 0xff); - rfm22_write(rfm22b_dev, RFM22_header_enable3, 0xff); - // Set the ID to be checked - uint32_t id = rfm22b_dev->deviceID; - rfm22_write(rfm22b_dev, RFM22_check_header0, id & 0xff); - rfm22_write(rfm22b_dev, RFM22_check_header1, (id >> 8) & 0xff); - rfm22_write(rfm22b_dev, RFM22_check_header2, (id >> 16) & 0xff); - rfm22_write(rfm22b_dev, RFM22_check_header3, (id >> 24) & 0xff); - // 4 header bytes, synchronization word length 3, 2, 1 & 0 used, packet length included in header. - rfm22_write(rfm22b_dev, RFM22_header_control2, - RFM22_header_cntl2_hdlen_3210 | - RFM22_header_cntl2_synclen_3210 | - ((TX_PREAMBLE_NIBBLES >> 8) & 0x01)); - - // sync word - rfm22_write(rfm22b_dev, RFM22_sync_word3, SYNC_BYTE_1); - rfm22_write(rfm22b_dev, RFM22_sync_word2, SYNC_BYTE_2); - rfm22_write(rfm22b_dev, RFM22_sync_word1, SYNC_BYTE_3); - rfm22_write(rfm22b_dev, RFM22_sync_word0, SYNC_BYTE_4); - - // set the tx power - rfm22_write(rfm22b_dev, RFM22_tx_power, RFM22_tx_pwr_lna_sw | rfm22b_dev->tx_power); - - // TX FIFO Almost Full Threshold (0 - 63) - rfm22_write(rfm22b_dev, RFM22_tx_fifo_control1, TX_FIFO_HI_WATERMARK); - - // TX FIFO Almost Empty Threshold (0 - 63) - rfm22_write(rfm22b_dev, RFM22_tx_fifo_control2, TX_FIFO_LO_WATERMARK); - - // RX FIFO Almost Full Threshold (0 - 63) - rfm22_write(rfm22b_dev, RFM22_rx_fifo_control, RX_FIFO_HI_WATERMARK); - - // Set the frequency calibration - rfm22_write(rfm22b_dev, RFM22_xtal_osc_load_cap, rfm22b_dev->cfg.RFXtalCap); - - // Initialize the frequency and datarate to te default. - rfm22_setNominalCarrierFrequency(rfm22b_dev, rfm22b_dev->init_frequency, rfm22b_dev->init_frequency, RFM22B_FREQUENCY_HOP_STEP_SIZE); - rfm22_setDatarate(rfm22b_dev, RFM22B_DEFAULT_RX_DATARATE, true); - - return RFM22B_EVENT_INITIALIZED; -} +/***************************************************************************** + * The State Machine Functions + *****************************************************************************/ /** * Inject an event into the RFM22B state machine. * - * \param[in] rfm22b_dev The device structure - * \param[in] event The event to inject - * \param[in] inISR Is this being called from an interrrup service routine? + * @param[in] rfm22b_dev The device structure + * @param[in] event The event to inject + * @param[in] inISR Is this being called from an interrrup service routine? */ static void pios_rfm22_inject_event(struct pios_rfm22b_dev *rfm22b_dev, enum pios_rfm22b_event event, bool inISR) { - // Store the event. - if (xQueueSend(rfm22b_dev->eventQueue, &event, portMAX_DELAY) != pdTRUE) - return; + // Store the event. + if (xQueueSend(rfm22b_dev->eventQueue, &event, portMAX_DELAY) != pdTRUE) + return; - // Signal the semaphore to wake up the handler thread. - if (inISR) { - portBASE_TYPE pxHigherPriorityTaskWoken; - if (xSemaphoreGiveFromISR(rfm22b_dev->isrPending, &pxHigherPriorityTaskWoken) != pdTRUE) { - // Something went fairly seriously wrong - rfm22b_dev->errors++; - } - portEND_SWITCHING_ISR(pxHigherPriorityTaskWoken); - } - else - { - if (xSemaphoreGive(rfm22b_dev->isrPending) != pdTRUE) { - // Something went fairly seriously wrong - rfm22b_dev->errors++; - } - } + // Signal the semaphore to wake up the handler thread. + if (inISR) { + portBASE_TYPE pxHigherPriorityTaskWoken; + if (xSemaphoreGiveFromISR(rfm22b_dev->isrPending, &pxHigherPriorityTaskWoken) != pdTRUE) { + // Something went fairly seriously wrong + rfm22b_dev->errors++; + } + portEND_SWITCHING_ISR(pxHigherPriorityTaskWoken); + } else { + if (xSemaphoreGive(rfm22b_dev->isrPending) != pdTRUE) { + // Something went fairly seriously wrong + rfm22b_dev->errors++; + } + } +} + +/** + * Process the next state transition from the given event. + * + * @param[in] rfm22b_dev The device structure + * @param[in] event The event to process + * @return enum pios_rfm22b_event The next event to inject + */ +static enum pios_rfm22b_event rfm22_process_state_transition(struct pios_rfm22b_dev *rfm22b_dev, enum pios_rfm22b_event event) +{ + + // No event + if (event == RFM22B_EVENT_NUM_EVENTS) { + return RFM22B_EVENT_NUM_EVENTS; + } + + // Don't transition if there is no transition defined + enum pios_rfm22b_state next_state = rfm22b_transitions[rfm22b_dev->state].next_state[event]; + if (!next_state) { + return RFM22B_EVENT_NUM_EVENTS; + } + + /* + * Move to the next state + * + * This is done prior to calling the new state's entry function to + * guarantee that the entry function never depends on the previous + * state. This way, it cannot ever know what the previous state was. + */ + rfm22b_dev->state = next_state; + + /* Call the entry function (if any) for the next state. */ + if (rfm22b_transitions[rfm22b_dev->state].entry_fn) { + return rfm22b_transitions[rfm22b_dev->state].entry_fn(rfm22b_dev); + } + + return RFM22B_EVENT_NUM_EVENTS; +} + +/** + * Process the given event through the state transition table. + * This could cause a series of events and transitions to take place. + * + * @param[in] rfm22b_dev The device structure + * @param[in] event The event to process + */ +static void rfm22_process_event(struct pios_rfm22b_dev *rfm22b_dev, enum pios_rfm22b_event event) +{ + // Process all state transitions. + while(event != RFM22B_EVENT_NUM_EVENTS) { + event = rfm22_process_state_transition(rfm22b_dev, event); + } +} + + +/***************************************************************************** + * The Device Initialization / Configuration Functions + *****************************************************************************/ + +/** + * Initialize (or re-initialize) the RFM22B radio device. + * + * @param[in] rfm22b_dev The device structure + * @return enum pios_rfm22b_event The next event to inject + */ +static enum pios_rfm22b_event rfm22_init(struct pios_rfm22b_dev *rfm22b_dev) +{ + + // Initialize the register values. + rfm22b_dev->device_status = 0; + rfm22b_dev->int_status1 = 0; + rfm22b_dev->int_status2 = 0; + rfm22b_dev->ezmac_status = 0; + + // Clean the LEDs + rfm22_clearLEDs(); + + // Initialize the detected device statistics. + for (uint8_t i = 0; i < OPLINKSTATUS_PAIRIDS_NUMELEM; ++i) { + rfm22b_dev->pair_stats[i].pairID = 0; + rfm22b_dev->pair_stats[i].rssi = -127; + rfm22b_dev->pair_stats[i].afc_correction = 0; + rfm22b_dev->pair_stats[i].lastContact = 0; + } + + // Initlize the link stats. + for (uint8_t i = 0; i < RFM22B_RX_PACKET_STATS_LEN; ++i) { + rfm22b_dev->rx_packet_stats[i] = 0; + } + + // Initialize the state + rfm22b_dev->stats.link_state = OPLINKSTATUS_LINKSTATE_DISCONNECTED; + rfm22b_dev->destination_id = 0xffffffff; + rfm22b_dev->time_to_send = false; + rfm22b_dev->time_to_send_offset = 0; + rfm22b_dev->send_status = false; + rfm22b_dev->send_connection_request = false; + + // Initialize the packets. + rfm22b_dev->rx_packet_len = 0; + rfm22b_dev->tx_packet = NULL; + rfm22b_dev->prev_tx_packet = NULL; + rfm22b_dev->data_packet.header.data_size = 0; + rfm22b_dev->in_rx_mode = false; + + // Initialize the devide state + rfm22b_dev->device_status = rfm22b_dev->int_status1 = rfm22b_dev->int_status2 = rfm22b_dev->ezmac_status = 0; + rfm22b_dev->rx_buffer_wr = 0; + rfm22b_dev->tx_data_rd = rfm22b_dev->tx_data_wr = 0; + rfm22b_dev->frequency_hop_channel = 0; + rfm22b_dev->afc_correction_Hz = 0; + rfm22b_dev->packet_start_ticks = 0; + rfm22b_dev->tx_complete_ticks = 0; + rfm22b_dev->rx_complete_ticks = 0; + + // software reset the RF chip .. following procedure according to Si4x3x Errata (rev. B) + rfm22_write(rfm22b_dev, RFM22_op_and_func_ctrl1, RFM22_opfc1_swres); + + for (int i = 50; i > 0; i--) { + // read the status registers + rfm22b_dev->int_status1 = rfm22_read(rfm22b_dev, RFM22_interrupt_status1); + rfm22b_dev->int_status2 = rfm22_read(rfm22b_dev, RFM22_interrupt_status2); + if (rfm22b_dev->int_status2 & RFM22_is2_ichiprdy) break; + + // wait 1ms + PIOS_DELAY_WaitmS(1); + } + + // **************** + + // read status - clears interrupt + rfm22b_dev->device_status = rfm22_read(rfm22b_dev, RFM22_device_status); + rfm22b_dev->int_status1 = rfm22_read(rfm22b_dev, RFM22_interrupt_status1); + rfm22b_dev->int_status2 = rfm22_read(rfm22b_dev, RFM22_interrupt_status2); + rfm22b_dev->ezmac_status = rfm22_read(rfm22b_dev, RFM22_ezmac_status); + + // disable all interrupts + rfm22_write(rfm22b_dev, RFM22_interrupt_enable1, 0x00); + rfm22_write(rfm22b_dev, RFM22_interrupt_enable2, 0x00); + + // read the RF chip ID bytes + + // read the device type + uint8_t device_type = rfm22_read(rfm22b_dev, RFM22_DEVICE_TYPE) & RFM22_DT_MASK; + // read the device version + uint8_t device_version = rfm22_read(rfm22b_dev, RFM22_DEVICE_VERSION) & RFM22_DV_MASK; + +#if defined(RFM22_DEBUG) + DEBUG_PRINTF(2, "rf device type: %d\n\r", device_type); + DEBUG_PRINTF(2, "rf device version: %d\n\r", device_version); +#endif + + if (device_type != 0x08) { +#if defined(RFM22_DEBUG) + DEBUG_PRINTF(2, "rf device type: INCORRECT - should be 0x08\n\r"); +#endif + + // incorrect RF module type + return RFM22B_EVENT_FATAL_ERROR; + } + if (device_version != RFM22_DEVICE_VERSION_B1) { +#if defined(RFM22_DEBUG) + DEBUG_PRINTF(2, "rf device version: INCORRECT\n\r"); +#endif + // incorrect RF module version + return RFM22B_EVENT_FATAL_ERROR; + } + + // calibrate our RF module to be exactly on frequency .. different for every module + rfm22_write(rfm22b_dev, RFM22_xtal_osc_load_cap, OSC_LOAD_CAP); + + // disable Low Duty Cycle Mode + rfm22_write(rfm22b_dev, RFM22_op_and_func_ctrl2, 0x00); + + // 1MHz clock output + rfm22_write(rfm22b_dev, RFM22_cpu_output_clk, RFM22_coc_1MHz); + + // READY mode + rfm22_write(rfm22b_dev, RFM22_op_and_func_ctrl1, RFM22_opfc1_xton); + + // choose the 3 GPIO pin functions + // GPIO port use default value + rfm22_write(rfm22b_dev, RFM22_io_port_config, RFM22_io_port_default); + if (rfm22b_dev->cfg.gpio_direction == GPIO0_TX_GPIO1_RX) { + // GPIO0 = TX State (to control RF Switch) + rfm22_write(rfm22b_dev, RFM22_gpio0_config, RFM22_gpio0_config_drv3 | RFM22_gpio0_config_txstate); + // GPIO1 = RX State (to control RF Switch) + rfm22_write(rfm22b_dev, RFM22_gpio1_config, RFM22_gpio1_config_drv3 | RFM22_gpio1_config_rxstate); + } else { + // GPIO0 = TX State (to control RF Switch) + rfm22_write(rfm22b_dev, RFM22_gpio0_config, RFM22_gpio0_config_drv3 | RFM22_gpio0_config_rxstate); + // GPIO1 = RX State (to control RF Switch) + rfm22_write(rfm22b_dev, RFM22_gpio1_config, RFM22_gpio1_config_drv3 | RFM22_gpio1_config_txstate); + } + // GPIO2 = Clear Channel Assessment + rfm22_write(rfm22b_dev, RFM22_gpio2_config, RFM22_gpio2_config_drv3 | RFM22_gpio2_config_cca); + + // FIFO mode, GFSK modulation + uint8_t fd_bit = rfm22_read(rfm22b_dev, RFM22_modulation_mode_control2) & RFM22_mmc2_fd; + rfm22_write(rfm22b_dev, RFM22_modulation_mode_control2, RFM22_mmc2_trclk_clk_none | RFM22_mmc2_dtmod_fifo | fd_bit | RFM22_mmc2_modtyp_gfsk); + + // setup to read the internal temperature sensor + + // ADC used to sample the temperature sensor + uint8_t adc_config = RFM22_ac_adcsel_temp_sensor | RFM22_ac_adcref_bg; + rfm22_write(rfm22b_dev, RFM22_adc_config, adc_config); + + // adc offset + rfm22_write(rfm22b_dev, RFM22_adc_sensor_amp_offset, 0); + + // temp sensor calibration .. �40C to +64C 0.5C resolution + rfm22_write(rfm22b_dev, RFM22_temp_sensor_calib, RFM22_tsc_tsrange0 | RFM22_tsc_entsoffs); + + // temp sensor offset + rfm22_write(rfm22b_dev, RFM22_temp_value_offset, 0); + + // start an ADC conversion + rfm22_write(rfm22b_dev, RFM22_adc_config, adc_config | RFM22_ac_adcstartbusy); + + // set the RSSI threshold interrupt to about -90dBm + rfm22_write(rfm22b_dev, RFM22_rssi_threshold_clear_chan_indicator, (-90 + 122) * 2); + + // enable the internal Tx & Rx packet handlers (without CRC) + rfm22_write(rfm22b_dev, RFM22_data_access_control, RFM22_dac_enpacrx | RFM22_dac_enpactx); + + // x-nibbles tx preamble + rfm22_write(rfm22b_dev, RFM22_preamble_length, TX_PREAMBLE_NIBBLES); + // x-nibbles rx preamble detection + rfm22_write(rfm22b_dev, RFM22_preamble_detection_ctrl1, RX_PREAMBLE_NIBBLES << 3); + + // header control - using a 4 by header with broadcast of 0xffffffff + rfm22_write(rfm22b_dev, RFM22_header_control1, + RFM22_header_cntl1_bcen_0 | + RFM22_header_cntl1_bcen_1 | + RFM22_header_cntl1_bcen_2 | + RFM22_header_cntl1_bcen_3 | + RFM22_header_cntl1_hdch_0 | + RFM22_header_cntl1_hdch_1 | + RFM22_header_cntl1_hdch_2 | + RFM22_header_cntl1_hdch_3); + // Check all bit of all bytes of the header + rfm22_write(rfm22b_dev, RFM22_header_enable0, 0xff); + rfm22_write(rfm22b_dev, RFM22_header_enable1, 0xff); + rfm22_write(rfm22b_dev, RFM22_header_enable2, 0xff); + rfm22_write(rfm22b_dev, RFM22_header_enable3, 0xff); + // Set the ID to be checked + uint32_t id = rfm22b_dev->deviceID; + rfm22_write(rfm22b_dev, RFM22_check_header0, id & 0xff); + rfm22_write(rfm22b_dev, RFM22_check_header1, (id >> 8) & 0xff); + rfm22_write(rfm22b_dev, RFM22_check_header2, (id >> 16) & 0xff); + rfm22_write(rfm22b_dev, RFM22_check_header3, (id >> 24) & 0xff); + // 4 header bytes, synchronization word length 3, 2, 1 & 0 used, packet length included in header. + rfm22_write(rfm22b_dev, RFM22_header_control2, + RFM22_header_cntl2_hdlen_3210 | + RFM22_header_cntl2_synclen_3210 | + ((TX_PREAMBLE_NIBBLES >> 8) & 0x01)); + + // sync word + rfm22_write(rfm22b_dev, RFM22_sync_word3, SYNC_BYTE_1); + rfm22_write(rfm22b_dev, RFM22_sync_word2, SYNC_BYTE_2); + rfm22_write(rfm22b_dev, RFM22_sync_word1, SYNC_BYTE_3); + rfm22_write(rfm22b_dev, RFM22_sync_word0, SYNC_BYTE_4); + + // set the tx power + rfm22_write(rfm22b_dev, RFM22_tx_power, RFM22_tx_pwr_lna_sw | rfm22b_dev->tx_power); + + // TX FIFO Almost Full Threshold (0 - 63) + rfm22_write(rfm22b_dev, RFM22_tx_fifo_control1, TX_FIFO_HI_WATERMARK); + + // TX FIFO Almost Empty Threshold (0 - 63) + rfm22_write(rfm22b_dev, RFM22_tx_fifo_control2, TX_FIFO_LO_WATERMARK); + + // RX FIFO Almost Full Threshold (0 - 63) + rfm22_write(rfm22b_dev, RFM22_rx_fifo_control, RX_FIFO_HI_WATERMARK); + + // Set the frequency calibration + rfm22_write(rfm22b_dev, RFM22_xtal_osc_load_cap, rfm22b_dev->cfg.RFXtalCap); + + // Initialize the frequency and datarate to te default. + rfm22_setNominalCarrierFrequency(rfm22b_dev, rfm22b_dev->init_frequency, rfm22b_dev->init_frequency, RFM22B_FREQUENCY_HOP_STEP_SIZE); + rfm22_setDatarate(rfm22b_dev, RFM22B_DEFAULT_RX_DATARATE, true); + + return RFM22B_EVENT_INITIALIZED; } /** @@ -1190,1295 +1234,1535 @@ static void pios_rfm22_inject_event(struct pios_rfm22b_dev *rfm22b_dev, enum pio * Delta-f = frequency deviation * fm = maximum frequency of the signal * - * \param[in] rfm33b_dev The device structure pointer. - * \param[in] datarate The air datarate. - * \param[in] data_whitening Is data whitening desired? + * @param[in] rfm33b_dev The device structure pointer. + * @param[in] datarate The air datarate. + * @param[in] data_whitening Is data whitening desired? */ static void rfm22_setDatarate(struct pios_rfm22b_dev *rfm22b_dev, enum rfm22b_datarate datarate, bool data_whitening) { - uint32_t datarate_bps = data_rate[datarate]; - rfm22b_dev->max_packet_time = (uint16_t)((float)(PIOS_PH_MAX_PACKET * 8 * 1000) / (float)(datarate_bps) + 0.5); + uint32_t datarate_bps = data_rate[datarate]; + rfm22b_dev->max_packet_time = (uint16_t)((float)(PIOS_PH_MAX_PACKET * 8 * 1000) / (float)(datarate_bps) + 0.5); - // Generate a pseudo-random number from 0-8 to add to the delay - uint8_t random = PIOS_CRC_updateByte(0, (uint8_t)(xTaskGetTickCount() & 0xff)) & 0x03; - rfm22b_dev->max_ack_delay = (uint16_t)((float)((sizeof(PHAckNackPacket) * 8 + TX_PREAMBLE_NIBBLES * 4) * 1000) / (float)(datarate_bps) + 0.5) * 4 + 4 + random; + // Generate a pseudo-random number from 0-8 to add to the delay + uint8_t random = PIOS_CRC_updateByte(0, (uint8_t)(xTaskGetTickCount() & 0xff)) & 0x03; + rfm22b_dev->max_ack_delay = (uint16_t)((float)((sizeof(PHAckNackPacket) * 8 + TX_PREAMBLE_NIBBLES * 4) * 1000) / (float)(datarate_bps) + 0.5) * 4 + 4 + random; - // rfm22_if_filter_bandwidth - rfm22_write(rfm22b_dev, 0x1C, reg_1C[datarate]); + // rfm22_if_filter_bandwidth + rfm22_write(rfm22b_dev, 0x1C, reg_1C[datarate]); - // rfm22_afc_loop_gearshift_override - rfm22_write(rfm22b_dev, 0x1D, reg_1D[datarate]); - // RFM22_afc_timing_control - rfm22_write(rfm22b_dev, 0x1E, reg_1E[datarate]); + // rfm22_afc_loop_gearshift_override + rfm22_write(rfm22b_dev, 0x1D, reg_1D[datarate]); + // RFM22_afc_timing_control + rfm22_write(rfm22b_dev, 0x1E, reg_1E[datarate]); - // RFM22_clk_recovery_gearshift_override - rfm22_write(rfm22b_dev, 0x1F, reg_1F[datarate]); - // rfm22_clk_recovery_oversampling_ratio - rfm22_write(rfm22b_dev, 0x20, reg_20[datarate]); - // rfm22_clk_recovery_offset2 - rfm22_write(rfm22b_dev, 0x21, reg_21[datarate]); - // rfm22_clk_recovery_offset1 - rfm22_write(rfm22b_dev, 0x22, reg_22[datarate]); - // rfm22_clk_recovery_offset0 - rfm22_write(rfm22b_dev, 0x23, reg_23[datarate]); - // rfm22_clk_recovery_timing_loop_gain1 - rfm22_write(rfm22b_dev, 0x24, reg_24[datarate]); - // rfm22_clk_recovery_timing_loop_gain0 - rfm22_write(rfm22b_dev, 0x25, reg_25[datarate]); - // rfm22_agc_override1 - rfm22_write(rfm22b_dev, RFM22_agc_override1, reg_69[datarate]); + // RFM22_clk_recovery_gearshift_override + rfm22_write(rfm22b_dev, 0x1F, reg_1F[datarate]); + // rfm22_clk_recovery_oversampling_ratio + rfm22_write(rfm22b_dev, 0x20, reg_20[datarate]); + // rfm22_clk_recovery_offset2 + rfm22_write(rfm22b_dev, 0x21, reg_21[datarate]); + // rfm22_clk_recovery_offset1 + rfm22_write(rfm22b_dev, 0x22, reg_22[datarate]); + // rfm22_clk_recovery_offset0 + rfm22_write(rfm22b_dev, 0x23, reg_23[datarate]); + // rfm22_clk_recovery_timing_loop_gain1 + rfm22_write(rfm22b_dev, 0x24, reg_24[datarate]); + // rfm22_clk_recovery_timing_loop_gain0 + rfm22_write(rfm22b_dev, 0x25, reg_25[datarate]); + // rfm22_agc_override1 + rfm22_write(rfm22b_dev, RFM22_agc_override1, reg_69[datarate]); - // rfm22_afc_limiter - rfm22_write(rfm22b_dev, 0x2A, reg_2A[datarate]); + // rfm22_afc_limiter + rfm22_write(rfm22b_dev, 0x2A, reg_2A[datarate]); - // rfm22_tx_data_rate1 - rfm22_write(rfm22b_dev, 0x6E, reg_6E[datarate]); - // rfm22_tx_data_rate0 - rfm22_write(rfm22b_dev, 0x6F, reg_6F[datarate]); + // rfm22_tx_data_rate1 + rfm22_write(rfm22b_dev, 0x6E, reg_6E[datarate]); + // rfm22_tx_data_rate0 + rfm22_write(rfm22b_dev, 0x6F, reg_6F[datarate]); - if (!data_whitening) - // rfm22_modulation_mode_control1 - rfm22_write(rfm22b_dev, 0x70, reg_70[datarate] & ~RFM22_mmc1_enwhite); - else - // rfm22_modulation_mode_control1 - rfm22_write(rfm22b_dev, 0x70, reg_70[datarate] | RFM22_mmc1_enwhite); + if (!data_whitening) { + // rfm22_modulation_mode_control1 + rfm22_write(rfm22b_dev, 0x70, reg_70[datarate] & ~RFM22_mmc1_enwhite); + } else { + // rfm22_modulation_mode_control1 + rfm22_write(rfm22b_dev, 0x70, reg_70[datarate] | RFM22_mmc1_enwhite); + } - // rfm22_modulation_mode_control2 - rfm22_write(rfm22b_dev, 0x71, reg_71[datarate]); + // rfm22_modulation_mode_control2 + rfm22_write(rfm22b_dev, 0x71, reg_71[datarate]); - // rfm22_frequency_deviation - rfm22_write(rfm22b_dev, 0x72, reg_72[datarate]); + // rfm22_frequency_deviation + rfm22_write(rfm22b_dev, 0x72, reg_72[datarate]); - // rfm22_cpcuu - rfm22_write(rfm22b_dev, 0x58, reg_58[datarate]); + // rfm22_cpcuu + rfm22_write(rfm22b_dev, 0x58, reg_58[datarate]); - rfm22_write(rfm22b_dev, RFM22_ook_counter_value1, 0x00); - rfm22_write(rfm22b_dev, RFM22_ook_counter_value2, 0x00); -} - -// ************************************ -// SPI read/write - -//! Assert the CS line -static void rfm22_assertCs(struct pios_rfm22b_dev *rfm22b_dev) -{ - PIOS_DELAY_WaituS(1); - if(rfm22b_dev->spi_id != 0) - PIOS_SPI_RC_PinSet(rfm22b_dev->spi_id, rfm22b_dev->slave_num, 0); -} - -//! Deassert the CS line -static void rfm22_deassertCs(struct pios_rfm22b_dev *rfm22b_dev) -{ - if(rfm22b_dev->spi_id != 0) - PIOS_SPI_RC_PinSet(rfm22b_dev->spi_id, rfm22b_dev->slave_num, 1); -} - -//! Claim the SPI bus semaphore -static void rfm22_claimBus(struct pios_rfm22b_dev *rfm22b_dev) -{ - if(rfm22b_dev->spi_id != 0) - PIOS_SPI_ClaimBus(rfm22b_dev->spi_id); -} - -//! Release the SPI bus semaphore -static void rfm22_releaseBus(struct pios_rfm22b_dev *rfm22b_dev) -{ - if(rfm22b_dev->spi_id != 0) - PIOS_SPI_ReleaseBus(rfm22b_dev->spi_id); + rfm22_write(rfm22b_dev, RFM22_ook_counter_value1, 0x00); + rfm22_write(rfm22b_dev, RFM22_ook_counter_value2, 0x00); } /** - * Claim the semaphore and write a byte to a register - * @param[in] addr The address to write to - * @param[in] data The datat to write to that address + * Set the nominal carrier frequency and channel step size. + * + * @param[in] rfm33b_dev The device structure pointer. + * @param[in] min_frequency The minimum frequenc to transmit on (in Hz). + * @param[in] max_frequency The maximum frequenc to transmit on (in Hz). + * @param[in] step_size The channel spacing (in Hz). */ -static void rfm22_write(struct pios_rfm22b_dev *rfm22b_dev, uint8_t addr, uint8_t data) -{ - rfm22_claimBus(rfm22b_dev); - rfm22_assertCs(rfm22b_dev); - uint8_t buf[2] = {addr | 0x80, data}; - PIOS_SPI_TransferBlock(rfm22b_dev->spi_id, buf, NULL, sizeof(buf), NULL); - rfm22_deassertCs(rfm22b_dev); - rfm22_releaseBus(rfm22b_dev); -} - -/** - - * Read a byte from an RFM22b register - * @param[in] addr The address to read from - * @return Returns the result of the register read - */ -static uint8_t rfm22_read(struct pios_rfm22b_dev *rfm22b_dev, uint8_t addr) -{ - uint8_t in[2]; - uint8_t out[2] = {addr & 0x7f, 0xFF}; - rfm22_claimBus(rfm22b_dev); - rfm22_assertCs(rfm22b_dev); - PIOS_SPI_TransferBlock(rfm22b_dev->spi_id, out, in, sizeof(out), NULL); - rfm22_deassertCs(rfm22b_dev); - rfm22_releaseBus(rfm22b_dev); - return in[1]; -} - -/** - * Read a byte from an RFM22b register without claiming the bus - * @param[in] addr The address to read from - * @return Returns the result of the register read - */ -static uint8_t rfm22_read_noclaim(struct pios_rfm22b_dev *rfm22b_dev, uint8_t addr) -{ - uint8_t out[2] = {addr & 0x7F, 0xFF}; - uint8_t in[2]; - rfm22_assertCs(rfm22b_dev); - PIOS_SPI_TransferBlock(rfm22b_dev->spi_id, out, in, sizeof(out), NULL); - rfm22_deassertCs(rfm22b_dev); - return in[1]; -} - -// ************************************ - -static enum pios_rfm22b_event rfm22_process_state_transition(struct pios_rfm22b_dev *rfm22b_dev, enum pios_rfm22b_event event) -{ - - // No event - if (event == RFM22B_EVENT_NUM_EVENTS) - return RFM22B_EVENT_NUM_EVENTS; - - // Don't transition if there is no transition defined - enum pios_rfm22b_state next_state = rfm22b_transitions[rfm22b_dev->state].next_state[event]; - if (!next_state) - return RFM22B_EVENT_NUM_EVENTS; - - /* - * Move to the next state - * - * This is done prior to calling the new state's entry function to - * guarantee that the entry function never depends on the previous - * state. This way, it cannot ever know what the previous state was. - */ - enum pios_rfm22b_state prev_state = rfm22b_dev->state; - if (prev_state) ; - - rfm22b_dev->state = next_state; - - /* Call the entry function (if any) for the next state. */ - if (rfm22b_transitions[rfm22b_dev->state].entry_fn) - return rfm22b_transitions[rfm22b_dev->state].entry_fn(rfm22b_dev); - - return RFM22B_EVENT_NUM_EVENTS; -} - -static void rfm22_process_event(struct pios_rfm22b_dev *rfm22b_dev, enum pios_rfm22b_event event) -{ - // Process all state transitions. - while(event != RFM22B_EVENT_NUM_EVENTS) - event = rfm22_process_state_transition(rfm22b_dev, event); -} - -// ************************************ - static void rfm22_setNominalCarrierFrequency(struct pios_rfm22b_dev *rfm22b_dev, uint32_t min_frequency, uint32_t max_frequency, uint32_t step_size) { - uint32_t frequency_hz = min_frequency; + uint32_t frequency_hz = min_frequency; - // holds the hbsel (1 or 2) - uint8_t hbsel; - if (frequency_hz < 480000000) - hbsel = 0; - else - hbsel = 1; - float freq_mhz = (float)(frequency_hz) / 1000000.0; - float xtal_freq_khz = 30000; - float sfreq = freq_mhz / (10.0 * (xtal_freq_khz / 30000.0) * (1 + hbsel)); - uint32_t fb = (uint32_t)sfreq - 24 + (64 + 32 * hbsel); - uint32_t fc = (uint32_t)((sfreq - (uint32_t)sfreq) * 64000.0); - uint8_t fch = (fc >> 8) & 0xff; - uint8_t fcl = fc & 0xff; + // holds the hbsel (1 or 2) + uint8_t hbsel; + if (frequency_hz < 480000000) { + hbsel = 0; + } else { + hbsel = 1; + } + float freq_mhz = (float)(frequency_hz) / 1000000.0; + float xtal_freq_khz = 30000; + float sfreq = freq_mhz / (10.0 * (xtal_freq_khz / 30000.0) * (1 + hbsel)); + uint32_t fb = (uint32_t)sfreq - 24 + (64 + 32 * hbsel); + uint32_t fc = (uint32_t)((sfreq - (uint32_t)sfreq) * 64000.0); + uint8_t fch = (fc >> 8) & 0xff; + uint8_t fcl = fc & 0xff; - // Calculate the number of frequency hopping channels. - rfm22b_dev->num_channels = (step_size == 0) ? 1 : (uint16_t)((max_frequency - min_frequency) / step_size); + // Calculate the number of frequency hopping channels. + rfm22b_dev->num_channels = (step_size == 0) ? 1 : (uint16_t)((max_frequency - min_frequency) / step_size); - // initialize the frequency hopping step size (specified in 10khz increments). - uint32_t freq_hop_step_size = step_size / 10000; - if (freq_hop_step_size > 255) { - freq_hop_step_size = 255; - } - rfm22_write(rfm22b_dev, RFM22_frequency_hopping_step_size, (uint8_t)freq_hop_step_size); + // initialize the frequency hopping step size (specified in 10khz increments). + uint32_t freq_hop_step_size = step_size / 10000; + if (freq_hop_step_size > 255) { + freq_hop_step_size = 255; + } + rfm22_write(rfm22b_dev, RFM22_frequency_hopping_step_size, (uint8_t)freq_hop_step_size); - // frequency hopping channel (0-255) - rfm22b_dev->frequency_step_size = 156.25f * hbsel; + // frequency hopping channel (0-255) + rfm22b_dev->frequency_step_size = 156.25f * hbsel; - // frequency hopping channel (0-255) - rfm22b_dev->frequency_hop_channel = 0; - rfm22_write(rfm22b_dev, RFM22_frequency_hopping_channel_select, 0); + // frequency hopping channel (0-255) + rfm22b_dev->frequency_hop_channel = 0; + rfm22_write(rfm22b_dev, RFM22_frequency_hopping_channel_select, 0); - // no frequency offset - rfm22_write(rfm22b_dev, RFM22_frequency_offset1, 0); - rfm22_write(rfm22b_dev, RFM22_frequency_offset2, 0); + // no frequency offset + rfm22_write(rfm22b_dev, RFM22_frequency_offset1, 0); + rfm22_write(rfm22b_dev, RFM22_frequency_offset2, 0); - // set the carrier frequency - rfm22_write(rfm22b_dev, RFM22_frequency_band_select, fb & 0xff); - rfm22_write(rfm22b_dev, RFM22_nominal_carrier_frequency1, fch); - rfm22_write(rfm22b_dev, RFM22_nominal_carrier_frequency0, fcl); + // set the carrier frequency + rfm22_write(rfm22b_dev, RFM22_frequency_band_select, fb & 0xff); + rfm22_write(rfm22b_dev, RFM22_nominal_carrier_frequency1, fch); + rfm22_write(rfm22b_dev, RFM22_nominal_carrier_frequency0, fcl); } + +/** + * Set the frequency hopping channel. + * + * @param[in] rfm33b_dev The device structure pointer. + */ static bool rfm22_setFreqHopChannel(struct pios_rfm22b_dev *rfm22b_dev, uint8_t channel) { - // set the frequency hopping channel - if (rfm22b_dev->frequency_hop_channel == channel) - return false; - rfm22b_dev->frequency_hop_channel = channel; - rfm22_write(rfm22b_dev, RFM22_frequency_hopping_channel_select, channel); - return true; + // set the frequency hopping channel + if (rfm22b_dev->frequency_hop_channel == channel) { + return false; + } + rfm22b_dev->frequency_hop_channel = channel; + rfm22_write(rfm22b_dev, RFM22_frequency_hopping_channel_select, channel); + return true; } -static void rfm22_calculateLinkQuality(struct pios_rfm22b_dev *rfm22b_dev) -{ - // Add the RX packet statistics - rfm22b_dev->stats.rx_good = 0; - rfm22b_dev->stats.rx_corrected = 0; - rfm22b_dev->stats.rx_error = 0; - rfm22b_dev->stats.tx_resent = 0; - for (uint8_t i = 0; i < RFM22B_RX_PACKET_STATS_LEN; ++i) - { - uint32_t val = rfm22b_dev->rx_packet_stats[i]; - for (uint8_t j = 0; j < 16; ++j) - { - switch ((val >> (j * 2)) & 0x3) - { - case RFM22B_GOOD_RX_PACKET: - rfm22b_dev->stats.rx_good++; - break; - case RFM22B_CORRECTED_RX_PACKET: - rfm22b_dev->stats.rx_corrected++; - break; - case RFM22B_ERROR_RX_PACKET: - rfm22b_dev->stats.rx_error++; - break; - case RFM22B_RESENT_TX_PACKET: - rfm22b_dev->stats.tx_resent++; - break; - } - } - } - // Calculate the link quality metric, which is related to the number of good packets in relation to the number of bad packets. - // Note: This assumes that the number of packets sampled for the stats is 64. - // Using this equation, error and resent packets are counted as -2, and corrected packets are counted as -1. - // The range is 0 (all error or resent packets) to 128 (all good packets). - rfm22b_dev->stats.link_quality = 64 + rfm22b_dev->stats.rx_good - rfm22b_dev->stats.rx_error - rfm22b_dev->stats.tx_resent; -} - -// ************************************ - -static enum pios_rfm22b_event rfm22_setRxMode(struct pios_rfm22b_dev *rfm22b_dev) -{ - // Are we already in Rx mode? - if (rfm22b_dev->in_rx_mode) - return RFM22B_EVENT_NUM_EVENTS; - rfm22b_dev->packet_start_ticks = 0; -#if defined(PIOS_RFM22B_DEBUG_ON_TELEM) || defined(PIOS_RFM22B_DEBUG_ON_RCVR) - D2_LED_ON; -#endif // PIOS_RFM22B_DEBUG_ON_TELEM - - // disable interrupts - rfm22_write(rfm22b_dev, RFM22_interrupt_enable1, 0x00); - rfm22_write(rfm22b_dev, RFM22_interrupt_enable2, 0x00); - - // Switch to TUNE mode - rfm22_write(rfm22b_dev, RFM22_op_and_func_ctrl1, RFM22_opfc1_pllon); - - RX_LED_OFF; - TX_LED_OFF; - - // empty the rx buffer - rfm22b_dev->rx_buffer_wr = 0; - - // Clear the TX buffer. - rfm22b_dev->tx_data_rd = rfm22b_dev->tx_data_wr = 0; - - // clear FIFOs - rfm22_write(rfm22b_dev, RFM22_op_and_func_ctrl2, RFM22_opfc2_ffclrrx | RFM22_opfc2_ffclrtx); - rfm22_write(rfm22b_dev, RFM22_op_and_func_ctrl2, 0x00); - - // enable RX interrupts - rfm22_write(rfm22b_dev, RFM22_interrupt_enable1, RFM22_ie1_encrcerror | RFM22_ie1_enpkvalid | - RFM22_ie1_enrxffafull | RFM22_ie1_enfferr); - rfm22_write(rfm22b_dev, RFM22_interrupt_enable2, RFM22_ie2_enpreainval | RFM22_ie2_enpreaval | - RFM22_ie2_enswdet); - - // enable the receiver - rfm22_write(rfm22b_dev, RFM22_op_and_func_ctrl1, RFM22_opfc1_pllon | RFM22_opfc1_rxon); - - // Indicate that we're in RX mode. - rfm22b_dev->in_rx_mode = true; - - // No event generated - return RFM22B_EVENT_NUM_EVENTS; -} - -// ************************************ - -static bool rfm22_ready_to_send(struct pios_rfm22b_dev *rfm22b_dev) -{ - // Is there a status of PPM packet ready to send? - if (rfm22b_dev->prev_tx_packet || rfm22b_dev->send_ppm || rfm22b_dev->send_status) - return true; - - // Are we not connected yet? - if (!rfm22_isConnected(rfm22b_dev)) - return true; - - // Is there some data ready to sent? - PHPacketHandle dp = &rfm22b_dev->data_packet; - if (dp->header.data_size > 0) - return true; - bool need_yield = false; - if (rfm22b_dev->tx_out_cb) - dp->header.data_size = (rfm22b_dev->tx_out_cb)(rfm22b_dev->tx_out_context, dp->data, PH_MAX_DATA, NULL, &need_yield); - if (dp->header.data_size > 0) - return true; - - return false; -} - -static bool rfm22_isConnected(struct pios_rfm22b_dev *rfm22b_dev) -{ - return (rfm22b_dev->stats.link_state == OPLINKSTATUS_LINKSTATE_CONNECTED); -} - -static enum pios_rfm22b_event rfm22_txStart(struct pios_rfm22b_dev *rfm22b_dev) -{ - PHPacketHandle p = NULL; - - // Don't send if it's not our turn. - if (!rfm22b_dev->time_to_send || (rfm22_inChannelBuffer(rfm22b_dev) && rfm22_isConnected(rfm22b_dev))) - return RFM22B_EVENT_RX_MODE; - - // See if there's a packet ready to send. - if (rfm22b_dev->tx_packet) - p = rfm22b_dev->tx_packet; - - else { - - // Don't send a packet if we're waiting for an ACK - if (rfm22b_dev->prev_tx_packet) - return RFM22B_EVENT_RX_MODE; - - // Send a connection request? - if (!p && rfm22b_dev->send_connection_request) { - p = (PHPacketHandle)&(rfm22b_dev->con_packet); - rfm22b_dev->send_connection_request = false; - } - -#ifdef PIOS_PPM_RECEIVER - // Send a PPM packet? - if (!p && rfm22b_dev->send_ppm) { - p = (PHPacketHandle)&(rfm22b_dev->ppm_packet); - rfm22b_dev->send_ppm = false; - } -#endif - - // Send status? - if (!p && rfm22b_dev->send_status) { - p = (PHPacketHandle)&(rfm22b_dev->status_packet); - rfm22b_dev->send_status = false; - } - - // Try to get some data to send - if (!p) { - bool need_yield = false; - p = &rfm22b_dev->data_packet; - p->header.type = PACKET_TYPE_DATA; - p->header.destination_id = rfm22b_dev->destination_id; - if (rfm22b_dev->tx_out_cb && (p->header.data_size == 0)) - p->header.data_size = (rfm22b_dev->tx_out_cb)(rfm22b_dev->tx_out_context, p->data, PH_MAX_DATA, NULL, &need_yield); - - // Don't send any data until we're connected. - if (!rfm22_isConnected(rfm22b_dev)) - p->header.data_size = 0; - if (p->header.data_size == 0) - p = NULL; - } - - if (p) - p->header.seq_num = rfm22b_dev->stats.tx_seq++; - } - if (!p) - return RFM22B_EVENT_RX_MODE; - - // We're transitioning out of Rx mode. - rfm22b_dev->in_rx_mode = false; - -#if defined(PIOS_RFM22B_DEBUG_ON_TELEM) || defined(PIOS_RFM22B_DEBUG_ON_RCVR) - D1_LED_ON; - D2_LED_OFF; -#endif - - // Change the channel if necessary. - if (((p->header.type != PACKET_TYPE_ACK) && (p->header.type != PACKET_TYPE_ACK_RTS)) || - (rfm22b_dev->rx_packet.header.type != PACKET_TYPE_CON_REQUEST)) - rfm22_changeChannel(rfm22b_dev); - - // Add the error correcting code. - encode_data((unsigned char*)p, PHPacketSize(p), (unsigned char*)p); - - rfm22b_dev->tx_packet = p; - rfm22b_dev->packet_start_ticks = xTaskGetTickCount(); - if (rfm22b_dev->packet_start_ticks == 0) - rfm22b_dev->packet_start_ticks = 1; - - // disable interrupts - rfm22_write(rfm22b_dev, RFM22_interrupt_enable1, 0x00); - rfm22_write(rfm22b_dev, RFM22_interrupt_enable2, 0x00); - - // TUNE mode - rfm22_write(rfm22b_dev, RFM22_op_and_func_ctrl1, RFM22_opfc1_pllon); - - // Queue the data up for sending - rfm22b_dev->tx_data_wr = PH_PACKET_SIZE(rfm22b_dev->tx_packet); - - RX_LED_OFF; - - // Set the destination address in the transmit header. - // The destination address is the first 4 bytes of the message. - uint8_t *tx_buffer = (uint8_t*)(rfm22b_dev->tx_packet); - rfm22_write(rfm22b_dev, RFM22_transmit_header0, tx_buffer[0]); - rfm22_write(rfm22b_dev, RFM22_transmit_header1, tx_buffer[1]); - rfm22_write(rfm22b_dev, RFM22_transmit_header2, tx_buffer[2]); - rfm22_write(rfm22b_dev, RFM22_transmit_header3, tx_buffer[3]); - - // FIFO mode, GFSK modulation - uint8_t fd_bit = rfm22_read(rfm22b_dev, RFM22_modulation_mode_control2) & RFM22_mmc2_fd; - rfm22_write(rfm22b_dev, RFM22_modulation_mode_control2, fd_bit | RFM22_mmc2_dtmod_fifo | - RFM22_mmc2_modtyp_gfsk); - - // clear FIFOs - rfm22_write(rfm22b_dev, RFM22_op_and_func_ctrl2, RFM22_opfc2_ffclrrx | RFM22_opfc2_ffclrtx); - rfm22_write(rfm22b_dev, RFM22_op_and_func_ctrl2, 0x00); - - // ******************* - // add some data to the chips TX FIFO before enabling the transmitter - - // set the total number of data bytes we are going to transmit - rfm22_write(rfm22b_dev, RFM22_transmit_packet_length, rfm22b_dev->tx_data_wr); - - // add some data - rfm22_claimBus(rfm22b_dev); - rfm22_assertCs(rfm22b_dev); - PIOS_SPI_TransferByte(rfm22b_dev->spi_id, RFM22_fifo_access | 0x80); - int bytes_to_write = (rfm22b_dev->tx_data_wr - rfm22b_dev->tx_data_rd); - bytes_to_write = (bytes_to_write > FIFO_SIZE) ? FIFO_SIZE: bytes_to_write; - PIOS_SPI_TransferBlock(rfm22b_dev->spi_id, &tx_buffer[rfm22b_dev->tx_data_rd], NULL, bytes_to_write, NULL); - rfm22b_dev->tx_data_rd += bytes_to_write; - rfm22_deassertCs(rfm22b_dev); - rfm22_releaseBus(rfm22b_dev); - - // enable TX interrupts - rfm22_write(rfm22b_dev, RFM22_interrupt_enable1, RFM22_ie1_enpksent | RFM22_ie1_entxffaem); - - // enable the transmitter - rfm22_write(rfm22b_dev, RFM22_op_and_func_ctrl1, RFM22_opfc1_pllon | RFM22_opfc1_txon); - - TX_LED_ON; - - return RFM22B_EVENT_NUM_EVENTS; -} - -static void rfm22_sendStatus(struct pios_rfm22b_dev *rfm22b_dev) -{ - // The coordinator doesn't send status. - if (rfm22b_dev->coordinator) - return; - - // Update the link quality metric. - rfm22_calculateLinkQuality(rfm22b_dev); - - // Queue the status message - if (rfm22_isConnected(rfm22b_dev)) - rfm22b_dev->status_packet.header.destination_id = rfm22b_dev->destination_id; - else if (rfm22b_dev->coordinator) - return; - else - rfm22b_dev->status_packet.header.destination_id = 0xffffffff; // Broadcast - rfm22b_dev->status_packet.header.type = PACKET_TYPE_STATUS; - rfm22b_dev->status_packet.header.data_size = PH_STATUS_DATA_SIZE(&(rfm22b_dev->status_packet)); - rfm22b_dev->status_packet.source_id = rfm22b_dev->deviceID; - rfm22b_dev->status_packet.link_quality = rfm22b_dev->stats.link_quality; - rfm22b_dev->status_packet.received_rssi = rfm22b_dev->rssi_dBm; - rfm22b_dev->send_status = true; - - return; -} - -static void rfm22_sendPPM(struct pios_rfm22b_dev *rfm22b_dev) -{ -#ifdef PIOS_PPM_RECEIVER - // Only send PPM if we're connected - if (!rfm22_isConnected(rfm22b_dev)) { - return; - } - - // Just return if the PPM receiver is not configured. - if (PIOS_PPM_RECEIVER == 0) { - return; - } - - // See if we have any valid channels. - bool valid_input_detected = false; - for (uint8_t i = 1; i <= PIOS_PPM_NUM_INPUTS; ++i) - { - rfm22b_dev->ppm_packet.channels[i - 1] = PIOS_RCVR_Read(PIOS_PPM_RECEIVER, i); - if(rfm22b_dev->ppm_packet.channels[i - 1] != PIOS_RCVR_TIMEOUT) - valid_input_detected = true; - } - - // Send the PPM packet if it's valid - if (valid_input_detected) - { - rfm22b_dev->ppm_packet.header.destination_id = rfm22b_dev->destination_id; - rfm22b_dev->ppm_packet.header.type = PACKET_TYPE_PPM; - rfm22b_dev->ppm_packet.header.data_size = PH_PPM_DATA_SIZE(&(rfm22b_dev->ppm_packet)); - rfm22b_dev->send_ppm = true; - } -#endif -} +/***************************************************************************** + * Radio Transmit and Receive functions. + *****************************************************************************/ /** * Read the RFM22B interrupt and device status registers - * \param[in] rfm22b_dev The device structure + * + * @param[in] rfm22b_dev The device structure */ static bool rfm22_readStatus(struct pios_rfm22b_dev *rfm22b_dev) { - // 1. Read the interrupt statuses with burst read - rfm22_claimBus(rfm22b_dev); // Set RC and the semaphore - uint8_t write_buf[3] = {RFM22_interrupt_status1 & 0x7f, 0xFF, 0xFF}; - uint8_t read_buf[3]; - rfm22_assertCs(rfm22b_dev); - PIOS_SPI_TransferBlock(rfm22b_dev->spi_id, write_buf, read_buf, sizeof(write_buf), NULL); - rfm22_deassertCs(rfm22b_dev); - rfm22b_dev->int_status1 = read_buf[1]; - rfm22b_dev->int_status2 = read_buf[2]; + // 1. Read the interrupt statuses with burst read + rfm22_claimBus(rfm22b_dev); // Set RC and the semaphore + uint8_t write_buf[3] = {RFM22_interrupt_status1 & 0x7f, 0xFF, 0xFF}; + uint8_t read_buf[3]; + rfm22_assertCs(rfm22b_dev); + PIOS_SPI_TransferBlock(rfm22b_dev->spi_id, write_buf, read_buf, sizeof(write_buf), NULL); + rfm22_deassertCs(rfm22b_dev); + rfm22b_dev->int_status1 = read_buf[1]; + rfm22b_dev->int_status2 = read_buf[2]; - // Device status - rfm22b_dev->device_status = rfm22_read_noclaim(rfm22b_dev, RFM22_device_status); + // Device status + rfm22b_dev->device_status = rfm22_read_noclaim(rfm22b_dev, RFM22_device_status); - // EzMAC status - rfm22b_dev->ezmac_status = rfm22_read_noclaim(rfm22b_dev, RFM22_ezmac_status); + // EzMAC status + rfm22b_dev->ezmac_status = rfm22_read_noclaim(rfm22b_dev, RFM22_ezmac_status); - // Release the bus - rfm22_releaseBus(rfm22b_dev); + // Release the bus + rfm22_releaseBus(rfm22b_dev); - // the RF module has gone and done a reset - we need to re-initialize the rf module - if (rfm22b_dev->int_status2 & RFM22_is2_ipor) - return false; + // the RF module has gone and done a reset - we need to re-initialize the rf module + if (rfm22b_dev->int_status2 & RFM22_is2_ipor) { + return false; + } - return true; + return true; } /** - * Add a status value to the RX packet status array. - * \param[in] rfm22b_dev The device structure - * \param[in] status The packet status value + * Switch the radio into receive mode. + * + * @param[in] rfm22b_dev The device structure + * @return enum pios_rfm22b_event The next event to inject */ -static void rfm22b_add_rx_status(struct pios_rfm22b_dev *rfm22b_dev, enum pios_rfm22b_rx_packet_status status) +static enum pios_rfm22b_event rfm22_setRxMode(struct pios_rfm22b_dev *rfm22b_dev) { - // Shift the status registers - for (uint8_t i = RFM22B_RX_PACKET_STATS_LEN - 1; i > 0; --i) - { - rfm22b_dev->rx_packet_stats[i] = (rfm22b_dev->rx_packet_stats[i] << 2) | (rfm22b_dev->rx_packet_stats[i - 1] >> 30); - } - rfm22b_dev->rx_packet_stats[0] = (rfm22b_dev->rx_packet_stats[0] << 2) | status; + // Are we already in Rx mode? + if (rfm22b_dev->in_rx_mode) { + return RFM22B_EVENT_NUM_EVENTS; + } + rfm22b_dev->packet_start_ticks = 0; +#if defined(PIOS_RFM22B_DEBUG_ON_TELEM) || defined(PIOS_RFM22B_DEBUG_ON_RCVR) + D2_LED_ON; +#endif // PIOS_RFM22B_DEBUG_ON_TELEM + + // disable interrupts + rfm22_write(rfm22b_dev, RFM22_interrupt_enable1, 0x00); + rfm22_write(rfm22b_dev, RFM22_interrupt_enable2, 0x00); + + // Switch to TUNE mode + rfm22_write(rfm22b_dev, RFM22_op_and_func_ctrl1, RFM22_opfc1_pllon); + + RX_LED_OFF; + TX_LED_OFF; + + // empty the rx buffer + rfm22b_dev->rx_buffer_wr = 0; + + // Clear the TX buffer. + rfm22b_dev->tx_data_rd = rfm22b_dev->tx_data_wr = 0; + + // clear FIFOs + rfm22_write(rfm22b_dev, RFM22_op_and_func_ctrl2, RFM22_opfc2_ffclrrx | RFM22_opfc2_ffclrtx); + rfm22_write(rfm22b_dev, RFM22_op_and_func_ctrl2, 0x00); + + // enable RX interrupts + rfm22_write(rfm22b_dev, RFM22_interrupt_enable1, RFM22_ie1_encrcerror | RFM22_ie1_enpkvalid | + RFM22_ie1_enrxffafull | RFM22_ie1_enfferr); + rfm22_write(rfm22b_dev, RFM22_interrupt_enable2, RFM22_ie2_enpreainval | RFM22_ie2_enpreaval | + RFM22_ie2_enswdet); + + // enable the receiver + rfm22_write(rfm22b_dev, RFM22_op_and_func_ctrl1, RFM22_opfc1_pllon | RFM22_opfc1_rxon); + + // Indicate that we're in RX mode. + rfm22b_dev->in_rx_mode = true; + + // No event generated + return RFM22B_EVENT_NUM_EVENTS; } +/** + * Detect the preamble + * + * @param[in] rfm22b_dev The device structure + * @return enum pios_rfm22b_event The next event to inject + */ static enum pios_rfm22b_event rfm22_detectPreamble(struct pios_rfm22b_dev *rfm22b_dev) { - // Read the device status registers - if (!rfm22_readStatus(rfm22b_dev)) - return RFM22B_EVENT_FAILURE; + // Read the device status registers + if (!rfm22_readStatus(rfm22b_dev)) + return RFM22B_EVENT_FAILURE; - // Valid preamble detected - if (rfm22b_dev->int_status2 & RFM22_is2_ipreaval) - { - rfm22b_dev->packet_start_ticks = xTaskGetTickCount(); - if (rfm22b_dev->packet_start_ticks == 0) - rfm22b_dev->packet_start_ticks = 1; - RX_LED_ON; - return RFM22B_EVENT_PREAMBLE_DETECTED; - } + // Valid preamble detected + if (rfm22b_dev->int_status2 & RFM22_is2_ipreaval) { + rfm22b_dev->packet_start_ticks = xTaskGetTickCount(); + if (rfm22b_dev->packet_start_ticks == 0) + rfm22b_dev->packet_start_ticks = 1; + RX_LED_ON; + return RFM22B_EVENT_PREAMBLE_DETECTED; + } - return RFM22B_EVENT_NUM_EVENTS; + return RFM22B_EVENT_NUM_EVENTS; } +/** + * Detect the sync + * + * @param[in] rfm22b_dev The device structure + * @return enum pios_rfm22b_event The next event to inject + */ static enum pios_rfm22b_event rfm22_detectSync(struct pios_rfm22b_dev *rfm22b_dev) { - // Read the device status registers - if (!rfm22_readStatus(rfm22b_dev)) - return RFM22B_EVENT_FAILURE; + // Read the device status registers + if (!rfm22_readStatus(rfm22b_dev)) + return RFM22B_EVENT_FAILURE; - // Sync word detected - if (rfm22b_dev->int_status2 & RFM22_is2_iswdet) - { - RX_LED_ON; + // Sync word detected + if (rfm22b_dev->int_status2 & RFM22_is2_iswdet) { + RX_LED_ON; - // read the 10-bit signed afc correction value - // bits 9 to 2 - uint16_t afc_correction = (uint16_t)rfm22_read(rfm22b_dev, RFM22_afc_correction_read) << 8; - // bits 1 & 0 - afc_correction |= (uint16_t)rfm22_read(rfm22b_dev, RFM22_ook_counter_value1) & 0x00c0; - afc_correction >>= 6; - // convert the afc value to Hz - int32_t afc_corr = (int32_t)(rfm22b_dev->frequency_step_size * afc_correction + 0.5f); - rfm22b_dev->afc_correction_Hz = (afc_corr < -127) ? -127 : ((afc_corr > 127) ? 127 : afc_corr); + // read the 10-bit signed afc correction value + // bits 9 to 2 + uint16_t afc_correction = (uint16_t)rfm22_read(rfm22b_dev, RFM22_afc_correction_read) << 8; + // bits 1 & 0 + afc_correction |= (uint16_t)rfm22_read(rfm22b_dev, RFM22_ook_counter_value1) & 0x00c0; + afc_correction >>= 6; + // convert the afc value to Hz + int32_t afc_corr = (int32_t)(rfm22b_dev->frequency_step_size * afc_correction + 0.5f); + rfm22b_dev->afc_correction_Hz = (afc_corr < -127) ? -127 : ((afc_corr > 127) ? 127 : afc_corr); - // read rx signal strength .. 45 = -100dBm, 205 = -20dBm - uint8_t rssi = rfm22_read(rfm22b_dev, RFM22_rssi); - // convert to dBm - rfm22b_dev->rssi_dBm = (int8_t)(rssi >> 1) - 122; + // read rx signal strength .. 45 = -100dBm, 205 = -20dBm + uint8_t rssi = rfm22_read(rfm22b_dev, RFM22_rssi); + // convert to dBm + rfm22b_dev->rssi_dBm = (int8_t)(rssi >> 1) - 122; - return RFM22B_EVENT_SYNC_DETECTED; - } - else if (rfm22b_dev->int_status2 & !RFM22_is2_ipreaval) - // Waiting for sync timed out. - return RFM22B_EVENT_FAILURE; + return RFM22B_EVENT_SYNC_DETECTED; + } else if (rfm22b_dev->int_status2 & !RFM22_is2_ipreaval) { + // Waiting for sync timed out. + return RFM22B_EVENT_FAILURE; + } - return RFM22B_EVENT_NUM_EVENTS; -} - -static bool rfm22_receivePacket(struct pios_rfm22b_dev *rfm22b_dev, PHPacketHandle p, uint16_t rx_len) -{ - - // Attempt to correct any errors in the packet. - decode_data((unsigned char*)p, rx_len); - - bool good_packet = check_syndrome() == 0; - bool corrected_packet = false; - // We have an error. Try to correct it. - if(!good_packet && (correct_errors_erasures((unsigned char*)p, rx_len, 0, 0) != 0)) - // We corrected it - corrected_packet = true; - - // Add any missed packets into the stats. - bool ack_nack_packet = ((p->header.type == PACKET_TYPE_ACK) || (p->header.type == PACKET_TYPE_ACK_RTS) || (p->header.type == PACKET_TYPE_NACK)); - if (!ack_nack_packet && (good_packet || corrected_packet)) - { - uint16_t seq_num = p->header.seq_num; - if (rfm22_isConnected(rfm22b_dev)) { - static bool first_time = true; - uint16_t missed_packets = 0; - if (first_time) - first_time = false; - else - { - uint16_t prev_seq_num = rfm22b_dev->stats.rx_seq; - if (seq_num > prev_seq_num) - missed_packets = seq_num - prev_seq_num - 1; - else if((seq_num == prev_seq_num) && (p->header.type == PACKET_TYPE_DATA)) - p->header.type = PACKET_TYPE_DUPLICATE_DATA; - } - rfm22b_dev->stats.rx_missed += missed_packets; - } - rfm22b_dev->stats.rx_seq = seq_num; - } - - // Set the packet status - if (good_packet) - rfm22b_add_rx_status(rfm22b_dev, RFM22B_GOOD_RX_PACKET); - else if(corrected_packet) - // We corrected the error. - rfm22b_add_rx_status(rfm22b_dev, RFM22B_CORRECTED_RX_PACKET); - else - // We couldn't correct the error, so drop the packet. - rfm22b_add_rx_status(rfm22b_dev, RFM22B_ERROR_RX_PACKET); - - return (good_packet || corrected_packet); + return RFM22B_EVENT_NUM_EVENTS; } +/** + * Receive the packet data. + * + * @param[in] rfm22b_dev The device structure + * @return enum pios_rfm22b_event The next event to inject + */ static enum pios_rfm22b_event rfm22_rxData(struct pios_rfm22b_dev *rfm22b_dev) { - // Swap in the next packet buffer if required. - uint8_t *rx_buffer = (uint8_t*)&(rfm22b_dev->rx_packet); + // Swap in the next packet buffer if required. + uint8_t *rx_buffer = (uint8_t*)&(rfm22b_dev->rx_packet); - // Read the device status registers - if (!rfm22_readStatus(rfm22b_dev)) - return RFM22B_EVENT_FAILURE; + // Read the device status registers + if (!rfm22_readStatus(rfm22b_dev)) { + return RFM22B_EVENT_FAILURE; + } - // FIFO under/over flow error. Restart RX mode. - if (rfm22b_dev->int_status1 & RFM22_is1_ifferr) - return RFM22B_EVENT_FAILURE; + // FIFO under/over flow error. Restart RX mode. + if (rfm22b_dev->int_status1 & RFM22_is1_ifferr) { + return RFM22B_EVENT_FAILURE; + } - // RX FIFO almost full, it needs emptying - if (rfm22b_dev->int_status1 & RFM22_is1_irxffafull) - { - // read data from the rf chips FIFO buffer - // read the total length of the packet data - uint16_t len = rfm22_read(rfm22b_dev, RFM22_received_packet_length); + // RX FIFO almost full, it needs emptying + if (rfm22b_dev->int_status1 & RFM22_is1_irxffafull) { + // read data from the rf chips FIFO buffer + // read the total length of the packet data + uint16_t len = rfm22_read(rfm22b_dev, RFM22_received_packet_length); - // The received packet is going to be larger than the specified length - if ((rfm22b_dev->rx_buffer_wr + RX_FIFO_HI_WATERMARK) > len) - return RFM22B_EVENT_FAILURE; + // The received packet is going to be larger than the specified length + if ((rfm22b_dev->rx_buffer_wr + RX_FIFO_HI_WATERMARK) > len) { + return RFM22B_EVENT_FAILURE; + } - // Another packet length error. - if (((rfm22b_dev->rx_buffer_wr + RX_FIFO_HI_WATERMARK) >= len) && !(rfm22b_dev->int_status1 & RFM22_is1_ipkvalid)) - return RFM22B_EVENT_FAILURE; + // Another packet length error. + if (((rfm22b_dev->rx_buffer_wr + RX_FIFO_HI_WATERMARK) >= len) && !(rfm22b_dev->int_status1 & RFM22_is1_ipkvalid)) { + return RFM22B_EVENT_FAILURE; + } - // Fetch the data from the RX FIFO - rfm22_claimBus(rfm22b_dev); - rfm22_assertCs(rfm22b_dev); - PIOS_SPI_TransferByte(rfm22b_dev->spi_id,RFM22_fifo_access & 0x7F); - rfm22b_dev->rx_buffer_wr += (PIOS_SPI_TransferBlock(rfm22b_dev->spi_id ,OUT_FF, (uint8_t *)&rx_buffer[rfm22b_dev->rx_buffer_wr], RX_FIFO_HI_WATERMARK, NULL) == 0) ? RX_FIFO_HI_WATERMARK : 0; - rfm22_deassertCs(rfm22b_dev); - rfm22_releaseBus(rfm22b_dev); - } + // Fetch the data from the RX FIFO + rfm22_claimBus(rfm22b_dev); + rfm22_assertCs(rfm22b_dev); + PIOS_SPI_TransferByte(rfm22b_dev->spi_id,RFM22_fifo_access & 0x7F); + rfm22b_dev->rx_buffer_wr += (PIOS_SPI_TransferBlock(rfm22b_dev->spi_id ,OUT_FF, (uint8_t *)&rx_buffer[rfm22b_dev->rx_buffer_wr], RX_FIFO_HI_WATERMARK, NULL) == 0) ? RX_FIFO_HI_WATERMARK : 0; + rfm22_deassertCs(rfm22b_dev); + rfm22_releaseBus(rfm22b_dev); + } - // CRC error .. discard the received data - if (rfm22b_dev->int_status1 & RFM22_is1_icrerror) - return RFM22B_EVENT_FAILURE; + // CRC error .. discard the received data + if (rfm22b_dev->int_status1 & RFM22_is1_icrerror) { + return RFM22B_EVENT_FAILURE; + } - // Valid packet received - if (rfm22b_dev->int_status1 & RFM22_is1_ipkvalid) - { + // Valid packet received + if (rfm22b_dev->int_status1 & RFM22_is1_ipkvalid) { - // read the total length of the packet data - uint32_t len = rfm22_read(rfm22b_dev, RFM22_received_packet_length); + // read the total length of the packet data + uint32_t len = rfm22_read(rfm22b_dev, RFM22_received_packet_length); - // their must still be data in the RX FIFO we need to get - if (rfm22b_dev->rx_buffer_wr < len) - { - int32_t bytes_to_read = len - rfm22b_dev->rx_buffer_wr; - // Fetch the data from the RX FIFO - rfm22_claimBus(rfm22b_dev); - rfm22_assertCs(rfm22b_dev); - PIOS_SPI_TransferByte(rfm22b_dev->spi_id,RFM22_fifo_access & 0x7F); - rfm22b_dev->rx_buffer_wr += (PIOS_SPI_TransferBlock(rfm22b_dev->spi_id,OUT_FF, (uint8_t *)&rx_buffer[rfm22b_dev->rx_buffer_wr], bytes_to_read, NULL) == 0) ? bytes_to_read : 0; - rfm22_deassertCs(rfm22b_dev); - rfm22_releaseBus(rfm22b_dev); - } + // their must still be data in the RX FIFO we need to get + if (rfm22b_dev->rx_buffer_wr < len) { + int32_t bytes_to_read = len - rfm22b_dev->rx_buffer_wr; + // Fetch the data from the RX FIFO + rfm22_claimBus(rfm22b_dev); + rfm22_assertCs(rfm22b_dev); + PIOS_SPI_TransferByte(rfm22b_dev->spi_id,RFM22_fifo_access & 0x7F); + rfm22b_dev->rx_buffer_wr += (PIOS_SPI_TransferBlock(rfm22b_dev->spi_id,OUT_FF, (uint8_t *)&rx_buffer[rfm22b_dev->rx_buffer_wr], bytes_to_read, NULL) == 0) ? bytes_to_read : 0; + rfm22_deassertCs(rfm22b_dev); + rfm22_releaseBus(rfm22b_dev); + } - if (rfm22b_dev->rx_buffer_wr != len) - return RFM22B_EVENT_FAILURE; + if (rfm22b_dev->rx_buffer_wr != len) { + return RFM22B_EVENT_FAILURE; + } - // we have a valid received packet - enum pios_rfm22b_event ret_event = RFM22B_EVENT_RX_COMPLETE; - if (rfm22b_dev->rx_buffer_wr > 0) - { - rfm22b_dev->stats.rx_byte_count += rfm22b_dev->rx_buffer_wr; - // Check the packet for errors. - if (rfm22_receivePacket(rfm22b_dev, &(rfm22b_dev->rx_packet), rfm22b_dev->rx_buffer_wr)) - { - switch (rfm22b_dev->rx_packet.header.type) - { - case PACKET_TYPE_STATUS: - ret_event = RFM22B_EVENT_STATUS_RECEIVED; - break; - case PACKET_TYPE_CON_REQUEST: - ret_event = RFM22B_EVENT_CONNECTION_REQUESTED; - break; - case PACKET_TYPE_DATA: - { - // Send the data to the com port - bool rx_need_yield; - if (rfm22b_dev->rx_in_cb) - (rfm22b_dev->rx_in_cb)(rfm22b_dev->rx_in_context, rfm22b_dev->rx_packet.data, rfm22b_dev->rx_packet.header.data_size, NULL, &rx_need_yield); + // we have a valid received packet + enum pios_rfm22b_event ret_event = RFM22B_EVENT_RX_COMPLETE; + if (rfm22b_dev->rx_buffer_wr > 0) { + rfm22b_dev->stats.rx_byte_count += rfm22b_dev->rx_buffer_wr; + // Check the packet for errors. + if (rfm22_receivePacket(rfm22b_dev, &(rfm22b_dev->rx_packet), rfm22b_dev->rx_buffer_wr)) { + switch (rfm22b_dev->rx_packet.header.type) { + case PACKET_TYPE_STATUS: + ret_event = RFM22B_EVENT_STATUS_RECEIVED; + break; + case PACKET_TYPE_CON_REQUEST: + ret_event = RFM22B_EVENT_CONNECTION_REQUESTED; + break; + case PACKET_TYPE_DATA: + { + // Send the data to the com port + bool rx_need_yield; + if (rfm22b_dev->rx_in_cb) + (rfm22b_dev->rx_in_cb)(rfm22b_dev->rx_in_context, rfm22b_dev->rx_packet.data, rfm22b_dev->rx_packet.header.data_size, NULL, &rx_need_yield); #ifdef RFM22B_TEST_DROPPED_PACKETS - // Inject radnom missed ACKs - { - static uint8_t crc = 0; - static uint8_t cntr = 0; - crc = PIOS_CRC_updateByte(crc, cntr++); - if ((crc & 0x7) == 0) - ret_event = RFM22B_EVENT_RX_MODE; - } + // Inject radnom missed ACKs + { + static uint8_t crc = 0; + static uint8_t cntr = 0; + crc = PIOS_CRC_updateByte(crc, cntr++); + if ((crc & 0x7) == 0) + ret_event = RFM22B_EVENT_RX_MODE; + } #endif - break; - } - case PACKET_TYPE_DUPLICATE_DATA: - break; - case PACKET_TYPE_ACK: - case PACKET_TYPE_ACK_RTS: - ret_event = RFM22B_EVENT_PACKET_ACKED; - break; - case PACKET_TYPE_NACK: - ret_event = RFM22B_EVENT_PACKET_NACKED; - break; - case PACKET_TYPE_PPM: - { + break; + } + case PACKET_TYPE_DUPLICATE_DATA: + break; + case PACKET_TYPE_ACK: + case PACKET_TYPE_ACK_RTS: + ret_event = RFM22B_EVENT_PACKET_ACKED; + break; + case PACKET_TYPE_NACK: + ret_event = RFM22B_EVENT_PACKET_NACKED; + break; + case PACKET_TYPE_PPM: + { #if defined(PIOS_INCLUDE_GCSRCVR) || (defined(PIOS_INCLUDE_PPM_OUT) && defined(PIOS_PPM_OUTPUT)) || defined(PIOS_INCLUDE_RFM22B_RCVR) - PHPpmPacketHandle ppmp = (PHPpmPacketHandle)&(rfm22b_dev->rx_packet); + PHPpmPacketHandle ppmp = (PHPpmPacketHandle)&(rfm22b_dev->rx_packet); #if defined(PIOS_INCLUDE_GCSRCVR) || (defined(PIOS_INCLUDE_PPM_OUT) && defined(PIOS_PPM_OUTPUT)) - bool ppm_output = false; + bool ppm_output = false; #endif #endif #if defined(PIOS_INCLUDE_RFM22B_RCVR) - ppm_output = true; - for (uint8_t i = 0; i < PIOS_RFM22B_RCVR_MAX_CHANNELS; ++i) { - rfm22b_dev->ppm_channel[i] = ppmp->channels[i]; - } + ppm_output = true; + for (uint8_t i = 0; i < PIOS_RFM22B_RCVR_MAX_CHANNELS; ++i) { + rfm22b_dev->ppm_channel[i] = ppmp->channels[i]; + } #endif #if defined(PIOS_INCLUDE_PPM_OUT) && defined(PIOS_PPM_OUTPUT) - if (PIOS_PPM_OUTPUT) { - ppm_output = true; - for (uint8_t i = 0; i < PIOS_RFM22B_RCVR_MAX_CHANNELS; ++i) { - PIOS_PPM_OUT_Set(PIOS_PPM_OUTPUT, i, ppmp->channels[i]); - } - } + if (PIOS_PPM_OUTPUT) { + ppm_output = true; + for (uint8_t i = 0; i < PIOS_RFM22B_RCVR_MAX_CHANNELS; ++i) { + PIOS_PPM_OUT_Set(PIOS_PPM_OUTPUT, i, ppmp->channels[i]); + } + } #endif #if defined(PIOS_INCLUDE_GCSRCVR) - if (!ppm_output) { - GCSReceiverData gcsRcvr; - for (uint8_t i = 0; (i < PIOS_RFM22B_RCVR_MAX_CHANNELS) && (i < GCSRECEIVER_CHANNEL_NUMELEM); ++i) { - gcsRcvr.Channel[i] = ppmp->channels[i]; - } - GCSReceiverSet(&gcsRcvr); - } + if (!ppm_output) { + GCSReceiverData gcsRcvr; + for (uint8_t i = 0; (i < PIOS_RFM22B_RCVR_MAX_CHANNELS) && (i < GCSRECEIVER_CHANNEL_NUMELEM); ++i) { + gcsRcvr.Channel[i] = ppmp->channels[i]; + } + GCSReceiverSet(&gcsRcvr); + } #endif - break; - } - default: - break; - } + break; + } + default: + break; + } - } - else - ret_event = RFM22B_EVENT_RX_ERROR; - rfm22b_dev->rx_buffer_wr = 0; - rfm22b_dev->rx_complete_ticks = xTaskGetTickCount(); - if (rfm22b_dev->rx_complete_ticks == 0) - rfm22b_dev->rx_complete_ticks = 1; + } + else { + ret_event = RFM22B_EVENT_RX_ERROR; + } + rfm22b_dev->rx_buffer_wr = 0; + rfm22b_dev->rx_complete_ticks = xTaskGetTickCount(); + if (rfm22b_dev->rx_complete_ticks == 0) + rfm22b_dev->rx_complete_ticks = 1; #if defined(PIOS_RFM22B_DEBUG_ON_TELEM) || defined(PIOS_RFM22B_DEBUG_ON_RCVR) - D2_LED_OFF; + D2_LED_OFF; #endif - } + } - // We're finished with Rx mode - rfm22b_dev->in_rx_mode = false; + // We're finished with Rx mode + rfm22b_dev->in_rx_mode = false; - // Start a new transaction - rfm22b_dev->packet_start_ticks = 0; - return ret_event; - } + // Start a new transaction + rfm22b_dev->packet_start_ticks = 0; + return ret_event; + } - return RFM22B_EVENT_NUM_EVENTS; + return RFM22B_EVENT_NUM_EVENTS; } -static enum pios_rfm22b_event rfm22_rxFailure(struct pios_rfm22b_dev *rfm22b_dev) +/** + * Complete the receipt of a packet. + * + * @param[in] rfm22b_dev The device structure + * @param[in] p The packet handle of the received packet. + * @param[in] rc_len The number of bytes received. + */ +static bool rfm22_receivePacket(struct pios_rfm22b_dev *rfm22b_dev, PHPacketHandle p, uint16_t rx_len) { - rfm22b_dev->stats.rx_failure++; - rfm22b_dev->rx_buffer_wr = 0; - rfm22b_dev->rx_complete_ticks = xTaskGetTickCount(); - rfm22b_dev->in_rx_mode = false; - if (rfm22b_dev->rx_complete_ticks == 0) - rfm22b_dev->rx_complete_ticks = 1; - return RFM22B_EVENT_RX_MODE; + + // Attempt to correct any errors in the packet. + decode_data((unsigned char*)p, rx_len); + + bool good_packet = check_syndrome() == 0; + bool corrected_packet = false; + // We have an error. Try to correct it. + if(!good_packet && (correct_errors_erasures((unsigned char*)p, rx_len, 0, 0) != 0)) { + // We corrected it + corrected_packet = true; + } + + // Add any missed packets into the stats. + bool ack_nack_packet = ((p->header.type == PACKET_TYPE_ACK) || (p->header.type == PACKET_TYPE_ACK_RTS) || (p->header.type == PACKET_TYPE_NACK)); + if (!ack_nack_packet && (good_packet || corrected_packet)) { + uint16_t seq_num = p->header.seq_num; + if (rfm22_isConnected(rfm22b_dev)) { + static bool first_time = true; + uint16_t missed_packets = 0; + if (first_time) { + first_time = false; + } else { + uint16_t prev_seq_num = rfm22b_dev->stats.rx_seq; + if (seq_num > prev_seq_num) + missed_packets = seq_num - prev_seq_num - 1; + else if((seq_num == prev_seq_num) && (p->header.type == PACKET_TYPE_DATA)) + p->header.type = PACKET_TYPE_DUPLICATE_DATA; + } + rfm22b_dev->stats.rx_missed += missed_packets; + } + rfm22b_dev->stats.rx_seq = seq_num; + } + + // Set the packet status + if (good_packet) { + rfm22b_add_rx_status(rfm22b_dev, RFM22B_GOOD_RX_PACKET); + } else if(corrected_packet) { + // We corrected the error. + rfm22b_add_rx_status(rfm22b_dev, RFM22B_CORRECTED_RX_PACKET); + } else { + // We couldn't correct the error, so drop the packet. + rfm22b_add_rx_status(rfm22b_dev, RFM22B_ERROR_RX_PACKET); + } + + return (good_packet || corrected_packet); } +/** + * Start a transmit if possible + * + * @param[in] rfm22b_dev The device structure + * @return enum pios_rfm22b_event The next event to inject + */ +static enum pios_rfm22b_event rfm22_txStart(struct pios_rfm22b_dev *rfm22b_dev) +{ + PHPacketHandle p = NULL; + + // Don't send if it's not our turn. + if (!rfm22b_dev->time_to_send || (rfm22_inChannelBuffer(rfm22b_dev) && rfm22_isConnected(rfm22b_dev))) { + return RFM22B_EVENT_RX_MODE; + } + + // See if there's a packet ready to send. + if (rfm22b_dev->tx_packet) { + p = rfm22b_dev->tx_packet; + + } else { + + // Don't send a packet if we're waiting for an ACK + if (rfm22b_dev->prev_tx_packet) { + return RFM22B_EVENT_RX_MODE; + } + + // Send a connection request? + if (!p && rfm22b_dev->send_connection_request) { + p = (PHPacketHandle)&(rfm22b_dev->con_packet); + rfm22b_dev->send_connection_request = false; + } + +#ifdef PIOS_PPM_RECEIVER + // Send a PPM packet? + if (!p && rfm22b_dev->send_ppm) { + p = (PHPacketHandle)&(rfm22b_dev->ppm_packet); + rfm22b_dev->send_ppm = false; + } +#endif + + // Send status? + if (!p && rfm22b_dev->send_status) { + p = (PHPacketHandle)&(rfm22b_dev->status_packet); + rfm22b_dev->send_status = false; + } + + // Try to get some data to send + if (!p) { + bool need_yield = false; + p = &rfm22b_dev->data_packet; + p->header.type = PACKET_TYPE_DATA; + p->header.destination_id = rfm22b_dev->destination_id; + if (rfm22b_dev->tx_out_cb && (p->header.data_size == 0)) { + p->header.data_size = (rfm22b_dev->tx_out_cb)(rfm22b_dev->tx_out_context, p->data, PH_MAX_DATA, NULL, &need_yield); + } + + // Don't send any data until we're connected. + if (!rfm22_isConnected(rfm22b_dev)) { + p->header.data_size = 0; + } + if (p->header.data_size == 0) { + p = NULL; + } + } + + if (p) { + p->header.seq_num = rfm22b_dev->stats.tx_seq++; + } + } + if (!p) { + return RFM22B_EVENT_RX_MODE; + } + + // We're transitioning out of Rx mode. + rfm22b_dev->in_rx_mode = false; + +#if defined(PIOS_RFM22B_DEBUG_ON_TELEM) || defined(PIOS_RFM22B_DEBUG_ON_RCVR) + D1_LED_ON; + D2_LED_OFF; +#endif + + // Change the channel if necessary. + if (((p->header.type != PACKET_TYPE_ACK) && (p->header.type != PACKET_TYPE_ACK_RTS)) || + (rfm22b_dev->rx_packet.header.type != PACKET_TYPE_CON_REQUEST)) { + rfm22_changeChannel(rfm22b_dev); + } + + // Add the error correcting code. + encode_data((unsigned char*)p, PHPacketSize(p), (unsigned char*)p); + + rfm22b_dev->tx_packet = p; + rfm22b_dev->packet_start_ticks = xTaskGetTickCount(); + if (rfm22b_dev->packet_start_ticks == 0) { + rfm22b_dev->packet_start_ticks = 1; + } + + // disable interrupts + rfm22_write(rfm22b_dev, RFM22_interrupt_enable1, 0x00); + rfm22_write(rfm22b_dev, RFM22_interrupt_enable2, 0x00); + + // TUNE mode + rfm22_write(rfm22b_dev, RFM22_op_and_func_ctrl1, RFM22_opfc1_pllon); + + // Queue the data up for sending + rfm22b_dev->tx_data_wr = PH_PACKET_SIZE(rfm22b_dev->tx_packet); + + RX_LED_OFF; + + // Set the destination address in the transmit header. + // The destination address is the first 4 bytes of the message. + uint8_t *tx_buffer = (uint8_t*)(rfm22b_dev->tx_packet); + rfm22_write(rfm22b_dev, RFM22_transmit_header0, tx_buffer[0]); + rfm22_write(rfm22b_dev, RFM22_transmit_header1, tx_buffer[1]); + rfm22_write(rfm22b_dev, RFM22_transmit_header2, tx_buffer[2]); + rfm22_write(rfm22b_dev, RFM22_transmit_header3, tx_buffer[3]); + + // FIFO mode, GFSK modulation + uint8_t fd_bit = rfm22_read(rfm22b_dev, RFM22_modulation_mode_control2) & RFM22_mmc2_fd; + rfm22_write(rfm22b_dev, RFM22_modulation_mode_control2, fd_bit | RFM22_mmc2_dtmod_fifo | + RFM22_mmc2_modtyp_gfsk); + + // clear FIFOs + rfm22_write(rfm22b_dev, RFM22_op_and_func_ctrl2, RFM22_opfc2_ffclrrx | RFM22_opfc2_ffclrtx); + rfm22_write(rfm22b_dev, RFM22_op_and_func_ctrl2, 0x00); + + // ******************* + // add some data to the chips TX FIFO before enabling the transmitter + + // set the total number of data bytes we are going to transmit + rfm22_write(rfm22b_dev, RFM22_transmit_packet_length, rfm22b_dev->tx_data_wr); + + // add some data + rfm22_claimBus(rfm22b_dev); + rfm22_assertCs(rfm22b_dev); + PIOS_SPI_TransferByte(rfm22b_dev->spi_id, RFM22_fifo_access | 0x80); + int bytes_to_write = (rfm22b_dev->tx_data_wr - rfm22b_dev->tx_data_rd); + bytes_to_write = (bytes_to_write > FIFO_SIZE) ? FIFO_SIZE: bytes_to_write; + PIOS_SPI_TransferBlock(rfm22b_dev->spi_id, &tx_buffer[rfm22b_dev->tx_data_rd], NULL, bytes_to_write, NULL); + rfm22b_dev->tx_data_rd += bytes_to_write; + rfm22_deassertCs(rfm22b_dev); + rfm22_releaseBus(rfm22b_dev); + + // enable TX interrupts + rfm22_write(rfm22b_dev, RFM22_interrupt_enable1, RFM22_ie1_enpksent | RFM22_ie1_entxffaem); + + // enable the transmitter + rfm22_write(rfm22b_dev, RFM22_op_and_func_ctrl1, RFM22_opfc1_pllon | RFM22_opfc1_txon); + + TX_LED_ON; + + return RFM22B_EVENT_NUM_EVENTS; +} + +/** + * Receive packet data. + * + * @param[in] rfm22b_dev The device structure + * @return enum pios_rfm22b_event The next event to inject + */ static enum pios_rfm22b_event rfm22_txData(struct pios_rfm22b_dev *rfm22b_dev) { - enum pios_rfm22b_event ret_event = RFM22B_EVENT_NUM_EVENTS; + enum pios_rfm22b_event ret_event = RFM22B_EVENT_NUM_EVENTS; - // Read the device status registers - if (!rfm22_readStatus(rfm22b_dev)) - return RFM22B_EVENT_FAILURE; + // Read the device status registers + if (!rfm22_readStatus(rfm22b_dev)) { + return RFM22B_EVENT_FAILURE; + } - // TX FIFO almost empty, it needs filling up - if (rfm22b_dev->int_status1 & RFM22_is1_ixtffaem) - { - // top-up the rf chips TX FIFO buffer - uint8_t *tx_buffer = (uint8_t*)(rfm22b_dev->tx_packet); - uint16_t max_bytes = FIFO_SIZE - TX_FIFO_LO_WATERMARK - 1; - rfm22_claimBus(rfm22b_dev); - rfm22_assertCs(rfm22b_dev); - PIOS_SPI_TransferByte(rfm22b_dev->spi_id, RFM22_fifo_access | 0x80); - int bytes_to_write = (rfm22b_dev->tx_data_wr - rfm22b_dev->tx_data_rd); - bytes_to_write = (bytes_to_write > max_bytes) ? max_bytes: bytes_to_write; - PIOS_SPI_TransferBlock(rfm22b_dev->spi_id, &tx_buffer[rfm22b_dev->tx_data_rd], NULL, bytes_to_write, NULL); - rfm22b_dev->tx_data_rd += bytes_to_write; - rfm22_deassertCs(rfm22b_dev); - rfm22_releaseBus(rfm22b_dev); - } + // TX FIFO almost empty, it needs filling up + if (rfm22b_dev->int_status1 & RFM22_is1_ixtffaem) { + // top-up the rf chips TX FIFO buffer + uint8_t *tx_buffer = (uint8_t*)(rfm22b_dev->tx_packet); + uint16_t max_bytes = FIFO_SIZE - TX_FIFO_LO_WATERMARK - 1; + rfm22_claimBus(rfm22b_dev); + rfm22_assertCs(rfm22b_dev); + PIOS_SPI_TransferByte(rfm22b_dev->spi_id, RFM22_fifo_access | 0x80); + int bytes_to_write = (rfm22b_dev->tx_data_wr - rfm22b_dev->tx_data_rd); + bytes_to_write = (bytes_to_write > max_bytes) ? max_bytes: bytes_to_write; + PIOS_SPI_TransferBlock(rfm22b_dev->spi_id, &tx_buffer[rfm22b_dev->tx_data_rd], NULL, bytes_to_write, NULL); + rfm22b_dev->tx_data_rd += bytes_to_write; + rfm22_deassertCs(rfm22b_dev); + rfm22_releaseBus(rfm22b_dev); - // Packet has been sent - else if (rfm22b_dev->int_status1 & RFM22_is1_ipksent) - { - portTickType curTicks = xTaskGetTickCount(); - rfm22b_dev->stats.tx_byte_count += PH_PACKET_SIZE(rfm22b_dev->tx_packet); + // Packet has been sent + } else if (rfm22b_dev->int_status1 & RFM22_is1_ipksent) { + portTickType curTicks = xTaskGetTickCount(); + rfm22b_dev->stats.tx_byte_count += PH_PACKET_SIZE(rfm22b_dev->tx_packet); - // Is this an ACK? - bool is_ack = ((rfm22b_dev->tx_packet->header.type == PACKET_TYPE_ACK) || (rfm22b_dev->tx_packet->header.type == PACKET_TYPE_ACK_RTS)); - ret_event = RFM22B_EVENT_RX_MODE; - if (is_ack) { + // Is this an ACK? + bool is_ack = ((rfm22b_dev->tx_packet->header.type == PACKET_TYPE_ACK) || (rfm22b_dev->tx_packet->header.type == PACKET_TYPE_ACK_RTS)); + ret_event = RFM22B_EVENT_RX_MODE; + if (is_ack) { - // If this is an ACK for a connection request message we need to - // configure this modem from the connection request message. - if (rfm22b_dev->rx_packet.header.type == PACKET_TYPE_CON_REQUEST) { + // If this is an ACK for a connection request message we need to + // configure this modem from the connection request message. + if (rfm22b_dev->rx_packet.header.type == PACKET_TYPE_CON_REQUEST) { - rfm22_setConnectionParameters(rfm22b_dev); + rfm22_setConnectionParameters(rfm22b_dev); - } else if (rfm22b_dev->coordinator && !rfm22_isConnected(rfm22b_dev) && (rfm22b_dev->rx_packet.header.type == PACKET_TYPE_STATUS)) { + } else if (rfm22b_dev->coordinator && !rfm22_isConnected(rfm22b_dev) && (rfm22b_dev->rx_packet.header.type == PACKET_TYPE_STATUS)) { - // Send a connection request message if we're not connected, and this is a status message from a modem that we're bound to. - PHStatusPacketHandle status = (PHStatusPacketHandle)&(rfm22b_dev->rx_packet); - uint32_t source_id = status->source_id; - for (uint8_t i = 0; OPLINKSETTINGS_BINDINGS_NUMELEM; ++i) { - if (rfm22b_dev->bindings[i].pairID == source_id) { - rfm22b_dev->cur_binding = i; - ret_event = RFM22B_EVENT_REQUEST_CONNECTION; - break; - } - } - } + // Send a connection request message if we're not connected, and this is a status message from a modem that we're bound to. + PHStatusPacketHandle status = (PHStatusPacketHandle)&(rfm22b_dev->rx_packet); + uint32_t source_id = status->source_id; + for (uint8_t i = 0; OPLINKSETTINGS_BINDINGS_NUMELEM; ++i) { + if (rfm22b_dev->bindings[i].pairID == source_id) { + rfm22b_dev->cur_binding = i; + ret_event = RFM22B_EVENT_REQUEST_CONNECTION; + break; + } + } + } - // Change the channel - // On the remote side, we initialize the time delta when we finish sending the ACK for the connection request message. - if (rfm22b_dev->rx_packet.header.type == PACKET_TYPE_CON_REQUEST) { - rfm22b_dev->time_delta = portMAX_DELAY - curTicks; - } + // Change the channel + // On the remote side, we initialize the time delta when we finish sending the ACK for the connection request message. + if (rfm22b_dev->rx_packet.header.type == PACKET_TYPE_CON_REQUEST) { + rfm22b_dev->time_delta = portMAX_DELAY - curTicks; + } - } else if (rfm22b_dev->tx_packet->header.type != PACKET_TYPE_NACK) { + } else if (rfm22b_dev->tx_packet->header.type != PACKET_TYPE_NACK) { - // We need to wait for an ACK if this packet it not an ACK or NACK. - rfm22b_dev->prev_tx_packet = rfm22b_dev->tx_packet; - rfm22b_dev->tx_complete_ticks = xTaskGetTickCount(); - } - // Set the Tx period - if (rfm22b_dev->tx_packet->header.type == PACKET_TYPE_ACK) - rfm22b_dev->time_to_send_offset = curTicks + 0x4; - else if (rfm22b_dev->tx_packet->header.type == PACKET_TYPE_ACK_RTS) - rfm22b_dev->time_to_send_offset = curTicks; - rfm22b_dev->tx_packet = 0; - rfm22b_dev->tx_data_wr = rfm22b_dev->tx_data_rd = 0; - // Start a new transaction - rfm22b_dev->packet_start_ticks = 0; + // We need to wait for an ACK if this packet it not an ACK or NACK. + rfm22b_dev->prev_tx_packet = rfm22b_dev->tx_packet; + rfm22b_dev->tx_complete_ticks = xTaskGetTickCount(); + } + // Set the Tx period + if (rfm22b_dev->tx_packet->header.type == PACKET_TYPE_ACK) { + rfm22b_dev->time_to_send_offset = curTicks + 0x4; + } else if (rfm22b_dev->tx_packet->header.type == PACKET_TYPE_ACK_RTS) { + rfm22b_dev->time_to_send_offset = curTicks; + } + rfm22b_dev->tx_packet = 0; + rfm22b_dev->tx_data_wr = rfm22b_dev->tx_data_rd = 0; + // Start a new transaction + rfm22b_dev->packet_start_ticks = 0; #if defined(PIOS_RFM22B_DEBUG_ON_TELEM) || defined(PIOS_RFM22B_DEBUG_ON_RCVR) - D1_LED_OFF; + D1_LED_OFF; #endif - } + } - return ret_event; + return ret_event; } -static enum pios_rfm22b_event rfm22_txFailure(struct pios_rfm22b_dev *rfm22b_dev) + +/***************************************************************************** + * Packet Transmition Functions + *****************************************************************************/ + +/** + * Send a radio status message. + * + * @param[in] rfm22b_dev The device structure + */ +static void rfm22_sendStatus(struct pios_rfm22b_dev *rfm22b_dev) { - rfm22b_dev->stats.tx_failure++; - rfm22b_dev->tx_data_wr = rfm22b_dev->tx_data_rd = 0; - return RFM22B_EVENT_TX_START; + // The coordinator doesn't send status. + if (rfm22b_dev->coordinator) { + return; + } + + // Update the link quality metric. + rfm22_calculateLinkQuality(rfm22b_dev); + + // Queue the status message + if (rfm22_isConnected(rfm22b_dev)) { + rfm22b_dev->status_packet.header.destination_id = rfm22b_dev->destination_id; + } else if (rfm22b_dev->coordinator) { + return; + } else { + rfm22b_dev->status_packet.header.destination_id = 0xffffffff; // Broadcast + } + rfm22b_dev->status_packet.header.type = PACKET_TYPE_STATUS; + rfm22b_dev->status_packet.header.data_size = PH_STATUS_DATA_SIZE(&(rfm22b_dev->status_packet)); + rfm22b_dev->status_packet.source_id = rfm22b_dev->deviceID; + rfm22b_dev->status_packet.link_quality = rfm22b_dev->stats.link_quality; + rfm22b_dev->status_packet.received_rssi = rfm22b_dev->rssi_dBm; + rfm22b_dev->send_status = true; + + return; +} + +/** + * Send a PPM packet. + * + * @param[in] rfm22b_dev The device structure + */ +static void rfm22_sendPPM(struct pios_rfm22b_dev *rfm22b_dev) +{ +#ifdef PIOS_PPM_RECEIVER + // Only send PPM if we're connected + if (!rfm22_isConnected(rfm22b_dev)) { + return; + } + + // Just return if the PPM receiver is not configured. + if (PIOS_PPM_RECEIVER == 0) { + return; + } + + // See if we have any valid channels. + bool valid_input_detected = false; + for (uint8_t i = 1; i <= PIOS_PPM_NUM_INPUTS; ++i) { + rfm22b_dev->ppm_packet.channels[i - 1] = PIOS_RCVR_Read(PIOS_PPM_RECEIVER, i); + if(rfm22b_dev->ppm_packet.channels[i - 1] != PIOS_RCVR_TIMEOUT) + valid_input_detected = true; + } + + // Send the PPM packet if it's valid + if (valid_input_detected) { + rfm22b_dev->ppm_packet.header.destination_id = rfm22b_dev->destination_id; + rfm22b_dev->ppm_packet.header.type = PACKET_TYPE_PPM; + rfm22b_dev->ppm_packet.header.data_size = PH_PPM_DATA_SIZE(&(rfm22b_dev->ppm_packet)); + rfm22b_dev->send_ppm = true; + } +#endif } /** * Send an ACK to a received packet. - * \param[in] rfm22b_dev The device structure + * + * @param[in] rfm22b_dev The device structure + * @return enum pios_rfm22b_event The next event to inject */ static enum pios_rfm22b_event rfm22_sendAck(struct pios_rfm22b_dev *rfm22b_dev) { - PHAckNackPacketHandle aph = (PHAckNackPacketHandle)(&(rfm22b_dev->ack_nack_packet)); - aph->header.destination_id = rfm22b_dev->destination_id; - aph->header.type = rfm22_ready_to_send(rfm22b_dev) ? PACKET_TYPE_ACK_RTS : PACKET_TYPE_ACK; - aph->header.data_size = PH_ACK_NACK_DATA_SIZE(aph); - aph->header.seq_num = rfm22b_dev->rx_packet.header.seq_num; - aph->packet_recv_time = rfm22_coordinatorTime(rfm22b_dev, rfm22b_dev->rx_complete_ticks); - rfm22b_dev->tx_packet = (PHPacketHandle)aph; - rfm22b_dev->time_to_send = true; - return RFM22B_EVENT_TX_START; + PHAckNackPacketHandle aph = (PHAckNackPacketHandle)(&(rfm22b_dev->ack_nack_packet)); + aph->header.destination_id = rfm22b_dev->destination_id; + aph->header.type = rfm22_ready_to_send(rfm22b_dev) ? PACKET_TYPE_ACK_RTS : PACKET_TYPE_ACK; + aph->header.data_size = PH_ACK_NACK_DATA_SIZE(aph); + aph->header.seq_num = rfm22b_dev->rx_packet.header.seq_num; + aph->packet_recv_time = rfm22_coordinatorTime(rfm22b_dev, rfm22b_dev->rx_complete_ticks); + rfm22b_dev->tx_packet = (PHPacketHandle)aph; + rfm22b_dev->time_to_send = true; + return RFM22B_EVENT_TX_START; } /** * Send an NACK to a received packet. - * \param[in] rfm22b_dev The device structure + * + * @param[in] rfm22b_dev The device structure + * @return enum pios_rfm22b_event The next event to inject */ static enum pios_rfm22b_event rfm22_sendNack(struct pios_rfm22b_dev *rfm22b_dev) { - PHAckNackPacketHandle aph = (PHAckNackPacketHandle)(&(rfm22b_dev->ack_nack_packet)); - aph->header.destination_id = rfm22b_dev->destination_id; - aph->header.type = PACKET_TYPE_NACK; - aph->header.data_size = PH_ACK_NACK_DATA_SIZE(aph); - aph->header.seq_num = rfm22b_dev->rx_packet.header.seq_num; - rfm22b_dev->tx_packet = (PHPacketHandle)aph; - rfm22b_dev->time_to_send = true; - return RFM22B_EVENT_TX_START; + PHAckNackPacketHandle aph = (PHAckNackPacketHandle)(&(rfm22b_dev->ack_nack_packet)); + aph->header.destination_id = rfm22b_dev->destination_id; + aph->header.type = PACKET_TYPE_NACK; + aph->header.data_size = PH_ACK_NACK_DATA_SIZE(aph); + aph->header.seq_num = rfm22b_dev->rx_packet.header.seq_num; + rfm22b_dev->tx_packet = (PHPacketHandle)aph; + rfm22b_dev->time_to_send = true; + return RFM22B_EVENT_TX_START; } +/** + * Send a connection request message. + * + * @param[in] rfm22b_dev The device structure + * @return enum pios_rfm22b_event The next event to inject + */ +static enum pios_rfm22b_event rfm22_requestConnection(struct pios_rfm22b_dev *rfm22b_dev) +{ + PHConnectionPacketHandle cph = &(rfm22b_dev->con_packet); + + // Set our connection state to requesting connection. + rfm22b_dev->stats.link_state = OPLINKSTATUS_LINKSTATE_CONNECTING; + + // Fill in the connection request + rfm22b_dev->destination_id = rfm22b_dev->bindings[rfm22b_dev->cur_binding].pairID; + cph->header.destination_id = rfm22b_dev->destination_id; + cph->header.type = PACKET_TYPE_CON_REQUEST; + cph->header.data_size = PH_CONNECTION_DATA_SIZE(&(rfm22b_dev->con_packet)); + cph->source_id = rfm22b_dev->deviceID; + cph->main_port = rfm22b_dev->bindings[rfm22b_dev->cur_binding].main_port; + cph->flexi_port = rfm22b_dev->bindings[rfm22b_dev->cur_binding].flexi_port; + cph->vcp_port = rfm22b_dev->bindings[rfm22b_dev->cur_binding].vcp_port; + cph->com_speed = rfm22b_dev->bindings[rfm22b_dev->cur_binding].com_speed; + rfm22b_dev->time_to_send = true; + rfm22b_dev->send_connection_request = true; + rfm22b_dev->prev_tx_packet = NULL; + + return RFM22B_EVENT_TX_START; +} + + +/***************************************************************************** + * Packet Receipt Functions + *****************************************************************************/ + /** * Receive an ACK. - * \param[in] rfm22b_dev The device structure + * + * @param[in] rfm22b_dev The device structure + * @return enum pios_rfm22b_event The next event to inject */ static enum pios_rfm22b_event rfm22_receiveAck(struct pios_rfm22b_dev *rfm22b_dev) { - PHPacketHandle prev = rfm22b_dev->prev_tx_packet; - portTickType curTicks = xTaskGetTickCount(); + PHPacketHandle prev = rfm22b_dev->prev_tx_packet; + portTickType curTicks = xTaskGetTickCount(); - // Clear the previous TX packet. - rfm22b_dev->prev_tx_packet = NULL; + // Clear the previous TX packet. + rfm22b_dev->prev_tx_packet = NULL; - // Was this a connection request? - switch (prev->header.type) { - case PACKET_TYPE_CON_REQUEST: - rfm22_setConnectionParameters(rfm22b_dev); - break; - case PACKET_TYPE_DATA: - rfm22b_dev->data_packet.header.data_size = 0; - break; - } + // Was this a connection request? + switch (prev->header.type) { + case PACKET_TYPE_CON_REQUEST: + rfm22_setConnectionParameters(rfm22b_dev); + break; + case PACKET_TYPE_DATA: + rfm22b_dev->data_packet.header.data_size = 0; + break; + } - // On the coordinator side, we initialize the time delta when we receive the ACK for the connection request message. - if (prev->header.type == PACKET_TYPE_CON_REQUEST) { - rfm22b_dev->time_delta = portMAX_DELAY - rfm22b_dev->rx_complete_ticks; - } else if (!rfm22b_dev->coordinator) { - PHAckNackPacketHandle aph = (PHAckNackPacketHandle)(&(rfm22b_dev->rx_packet)); - portTickType local_tx_time = rfm22_coordinatorTime(rfm22b_dev, rfm22b_dev->tx_complete_ticks); - portTickType remote_rx_time = aph->packet_recv_time; - // Adjust the time delta based on the difference between our estimated time offset and the coordinator offset. - // This is not working yet - rfm22b_dev->time_delta += remote_rx_time - local_tx_time; - } + // On the coordinator side, we initialize the time delta when we receive the ACK for the connection request message. + if (prev->header.type == PACKET_TYPE_CON_REQUEST) { + rfm22b_dev->time_delta = portMAX_DELAY - rfm22b_dev->rx_complete_ticks; + } else if (!rfm22b_dev->coordinator) { + PHAckNackPacketHandle aph = (PHAckNackPacketHandle)(&(rfm22b_dev->rx_packet)); + portTickType local_tx_time = rfm22_coordinatorTime(rfm22b_dev, rfm22b_dev->tx_complete_ticks); + portTickType remote_rx_time = aph->packet_recv_time; + // Adjust the time delta based on the difference between our estimated time offset and the coordinator offset. + // This is not working yet + rfm22b_dev->time_delta += remote_rx_time - local_tx_time; + } - // Should we try to start another TX? - if (rfm22b_dev->rx_packet.header.type == PACKET_TYPE_ACK) { - rfm22b_dev->time_to_send_offset = curTicks; - rfm22b_dev->time_to_send = true; - return RFM22B_EVENT_TX_START; - } else { - rfm22b_dev->time_to_send_offset = curTicks + 0x4; - return RFM22B_EVENT_RX_MODE; - } + // Should we try to start another TX? + if (rfm22b_dev->rx_packet.header.type == PACKET_TYPE_ACK) { + rfm22b_dev->time_to_send_offset = curTicks; + rfm22b_dev->time_to_send = true; + return RFM22B_EVENT_TX_START; + } else { + rfm22b_dev->time_to_send_offset = curTicks + 0x4; + return RFM22B_EVENT_RX_MODE; + } } /** * Receive an MACK. - * \param[in] rfm22b_dev The device structure + * + * @param[in] rfm22b_dev The device structure + * @return enum pios_rfm22b_event The next event to inject */ static enum pios_rfm22b_event rfm22_receiveNack(struct pios_rfm22b_dev *rfm22b_dev) { - // Resend the previous TX packet. - rfm22b_dev->tx_packet = rfm22b_dev->prev_tx_packet; - rfm22b_dev->prev_tx_packet = NULL; + // Resend the previous TX packet. + rfm22b_dev->tx_packet = rfm22b_dev->prev_tx_packet; + rfm22b_dev->prev_tx_packet = NULL; - // Increment the reset packet counter if we're connected. - if (rfm22_isConnected(rfm22b_dev)) { - rfm22b_add_rx_status(rfm22b_dev, RFM22B_RESENT_TX_PACKET); - } - rfm22b_dev->time_to_send = true; - return RFM22B_EVENT_TX_START; + // Increment the reset packet counter if we're connected. + if (rfm22_isConnected(rfm22b_dev)) { + rfm22b_add_rx_status(rfm22b_dev, RFM22B_RESENT_TX_PACKET); + } + rfm22b_dev->time_to_send = true; + return RFM22B_EVENT_TX_START; } /** * Receive a status packet - * \param[in] rfm22b_dev The device structure + * + * @param[in] rfm22b_dev The device structure + * @return enum pios_rfm22b_event The next event to inject */ static enum pios_rfm22b_event rfm22_receiveStatus(struct pios_rfm22b_dev *rfm22b_dev) { - PHStatusPacketHandle status = (PHStatusPacketHandle)&(rfm22b_dev->rx_packet); - int8_t rssi = rfm22b_dev->rssi_dBm; - int8_t afc = rfm22b_dev->afc_correction_Hz; - uint32_t id = status->source_id; + PHStatusPacketHandle status = (PHStatusPacketHandle)&(rfm22b_dev->rx_packet); + int8_t rssi = rfm22b_dev->rssi_dBm; + int8_t afc = rfm22b_dev->afc_correction_Hz; + uint32_t id = status->source_id; - // Have we seen this device recently? - bool found = false; - uint8_t id_idx = 0; - for ( ; id_idx < OPLINKSTATUS_PAIRIDS_NUMELEM; ++id_idx) - if(rfm22b_dev->pair_stats[id_idx].pairID == id) - { - found = true; - break; - } + // Have we seen this device recently? + bool found = false; + uint8_t id_idx = 0; + for ( ; id_idx < OPLINKSTATUS_PAIRIDS_NUMELEM; ++id_idx) { + if(rfm22b_dev->pair_stats[id_idx].pairID == id) { + found = true; + break; + } + } - // If we have seen it, update the RSSI and reset the last contact couter - if(found) - { - rfm22b_dev->pair_stats[id_idx].rssi = rssi; - rfm22b_dev->pair_stats[id_idx].afc_correction = afc; - rfm22b_dev->pair_stats[id_idx].lastContact = 0; - } + // If we have seen it, update the RSSI and reset the last contact couter + if(found) { + rfm22b_dev->pair_stats[id_idx].rssi = rssi; + rfm22b_dev->pair_stats[id_idx].afc_correction = afc; + rfm22b_dev->pair_stats[id_idx].lastContact = 0; - // If we haven't seen it, find a slot to put it in. - else - { - uint8_t min_idx = 0; - int8_t min_rssi = rfm22b_dev->pair_stats[0].rssi; - for (id_idx = 1; id_idx < OPLINKSTATUS_PAIRIDS_NUMELEM; ++id_idx) - { - if(rfm22b_dev->pair_stats[id_idx].rssi < min_rssi) - { - min_rssi = rfm22b_dev->pair_stats[id_idx].rssi; - min_idx = id_idx; - } - } - rfm22b_dev->pair_stats[min_idx].pairID = id; - rfm22b_dev->pair_stats[min_idx].rssi = rssi; - rfm22b_dev->pair_stats[min_idx].afc_correction = afc; - rfm22b_dev->pair_stats[min_idx].lastContact = 0; - } + // If we haven't seen it, find a slot to put it in. + } else { + uint8_t min_idx = 0; + int8_t min_rssi = rfm22b_dev->pair_stats[0].rssi; + for (id_idx = 1; id_idx < OPLINKSTATUS_PAIRIDS_NUMELEM; ++id_idx) { + if(rfm22b_dev->pair_stats[id_idx].rssi < min_rssi) { + min_rssi = rfm22b_dev->pair_stats[id_idx].rssi; + min_idx = id_idx; + } + } + rfm22b_dev->pair_stats[min_idx].pairID = id; + rfm22b_dev->pair_stats[min_idx].rssi = rssi; + rfm22b_dev->pair_stats[min_idx].afc_correction = afc; + rfm22b_dev->pair_stats[min_idx].lastContact = 0; + } - return RFM22B_EVENT_RX_COMPLETE; + return RFM22B_EVENT_RX_COMPLETE; } -static enum pios_rfm22b_event rfm22_requestConnection(struct pios_rfm22b_dev *rfm22b_dev) + +/***************************************************************************** + * Link Statistics Functions + *****************************************************************************/ + +/** + * Calculate the link quality from the packet receipt, tranmittion statistics. + * + * @param[in] rfm22b_dev The device structure + */ +static void rfm22_calculateLinkQuality(struct pios_rfm22b_dev *rfm22b_dev) { - PHConnectionPacketHandle cph = &(rfm22b_dev->con_packet); + // Add the RX packet statistics + rfm22b_dev->stats.rx_good = 0; + rfm22b_dev->stats.rx_corrected = 0; + rfm22b_dev->stats.rx_error = 0; + rfm22b_dev->stats.tx_resent = 0; + for (uint8_t i = 0; i < RFM22B_RX_PACKET_STATS_LEN; ++i) { + uint32_t val = rfm22b_dev->rx_packet_stats[i]; + for (uint8_t j = 0; j < 16; ++j) { + switch ((val >> (j * 2)) & 0x3) { + case RFM22B_GOOD_RX_PACKET: + rfm22b_dev->stats.rx_good++; + break; + case RFM22B_CORRECTED_RX_PACKET: + rfm22b_dev->stats.rx_corrected++; + break; + case RFM22B_ERROR_RX_PACKET: + rfm22b_dev->stats.rx_error++; + break; + case RFM22B_RESENT_TX_PACKET: + rfm22b_dev->stats.tx_resent++; + break; + } + } + } - // Set our connection state to requesting connection. - rfm22b_dev->stats.link_state = OPLINKSTATUS_LINKSTATE_CONNECTING; - - // Fill in the connection request - rfm22b_dev->destination_id = rfm22b_dev->bindings[rfm22b_dev->cur_binding].pairID; - cph->header.destination_id = rfm22b_dev->destination_id; - cph->header.type = PACKET_TYPE_CON_REQUEST; - cph->header.data_size = PH_CONNECTION_DATA_SIZE(&(rfm22b_dev->con_packet)); - cph->source_id = rfm22b_dev->deviceID; - cph->main_port = rfm22b_dev->bindings[rfm22b_dev->cur_binding].main_port; - cph->flexi_port = rfm22b_dev->bindings[rfm22b_dev->cur_binding].flexi_port; - cph->vcp_port = rfm22b_dev->bindings[rfm22b_dev->cur_binding].vcp_port; - cph->com_speed = rfm22b_dev->bindings[rfm22b_dev->cur_binding].com_speed; - rfm22b_dev->time_to_send = true; - rfm22b_dev->send_connection_request = true; - rfm22b_dev->prev_tx_packet = NULL; - - return RFM22B_EVENT_TX_START; + // Calculate the link quality metric, which is related to the number of good packets in relation to the number of bad packets. + // Note: This assumes that the number of packets sampled for the stats is 64. + // Using this equation, error and resent packets are counted as -2, and corrected packets are counted as -1. + // The range is 0 (all error or resent packets) to 128 (all good packets). + rfm22b_dev->stats.link_quality = 64 + rfm22b_dev->stats.rx_good - rfm22b_dev->stats.rx_error - rfm22b_dev->stats.tx_resent; } +/** + * Add a status value to the RX packet status array. + * + * @param[in] rfm22b_dev The device structure + * @param[in] status The packet status value + */ +static void rfm22b_add_rx_status(struct pios_rfm22b_dev *rfm22b_dev, enum pios_rfm22b_rx_packet_status status) +{ + // Shift the status registers + for (uint8_t i = RFM22B_RX_PACKET_STATS_LEN - 1; i > 0; --i) { + rfm22b_dev->rx_packet_stats[i] = (rfm22b_dev->rx_packet_stats[i] << 2) | (rfm22b_dev->rx_packet_stats[i - 1] >> 30); + } + rfm22b_dev->rx_packet_stats[0] = (rfm22b_dev->rx_packet_stats[0] << 2) | status; +} + +/** + * Is it this modem's turn to send? + * + * @param[in] rfm22b_dev The device structure + */ +static bool rfm22_ready_to_send(struct pios_rfm22b_dev *rfm22b_dev) +{ + // Is there a status of PPM packet ready to send? + if (rfm22b_dev->prev_tx_packet || rfm22b_dev->send_ppm || rfm22b_dev->send_status) { + return true; + } + + // Are we not connected yet? + if (!rfm22_isConnected(rfm22b_dev)) { + return true; + } + + // Is there some data ready to sent? + PHPacketHandle dp = &rfm22b_dev->data_packet; + if (dp->header.data_size > 0) { + return true; + } + bool need_yield = false; + if (rfm22b_dev->tx_out_cb) { + dp->header.data_size = (rfm22b_dev->tx_out_cb)(rfm22b_dev->tx_out_context, dp->data, PH_MAX_DATA, NULL, &need_yield); + } + if (dp->header.data_size > 0) { + return true; + } + + return false; +} + + +/***************************************************************************** + * Connection Handling Functions + *****************************************************************************/ + +/** + * Are we connected to the remote modem? + * + * @param[in] rfm22b_dev The device structure + */ +static bool rfm22_isConnected(struct pios_rfm22b_dev *rfm22b_dev) +{ + return (rfm22b_dev->stats.link_state == OPLINKSTATUS_LINKSTATE_CONNECTED); +} + +/** + * Set the connection parameters from a connection request message. + * + * @param[in] rfm22b_dev The device structure + */ static void rfm22_setConnectionParameters(struct pios_rfm22b_dev *rfm22b_dev) { - PHConnectionPacketHandle cph = &(rfm22b_dev->con_packet); + PHConnectionPacketHandle cph = &(rfm22b_dev->con_packet); - // Set our connection state to connected - rfm22b_dev->stats.link_state = OPLINKSTATUS_LINKSTATE_CONNECTED; + // Set our connection state to connected + rfm22b_dev->stats.link_state = OPLINKSTATUS_LINKSTATE_CONNECTED; - // Call the com port configuration function - if (rfm22b_dev->com_config_cb) - rfm22b_dev->com_config_cb(cph->main_port, cph->flexi_port, cph->vcp_port, cph->com_speed, - cph->min_frequency, cph->max_frequency, cph->channel_spacing); - - // Configure this modem from the connection request message. - rfm22_setNominalCarrierFrequency(rfm22b_dev, cph->min_frequency, cph->max_frequency, cph->channel_spacing); - rfm22_setDatarate(rfm22b_dev, rfm22b_dev->datarate, true); -} - -static portTickType rfm22_coordinatorTime(struct pios_rfm22b_dev *rfm22b_dev, portTickType ticks) -{ - return ticks + rfm22b_dev->time_delta; -} - -static bool rfm22_inChannelBuffer(struct pios_rfm22b_dev *rfm22b_dev) -{ - portTickType time = rfm22_coordinatorTime(rfm22b_dev, xTaskGetTickCount()); - uint8_t window = (uint8_t)(time & 0x7e); - return ((window == 0x7f) || (window == 0)); -} - -static uint8_t rfm22_calcChannel(struct pios_rfm22b_dev *rfm22b_dev) -{ - portTickType time = rfm22_coordinatorTime(rfm22b_dev, xTaskGetTickCount()); - // We change channels every 128 ms. - uint16_t n = (time >> 7) & 0xffff; - // The channel is calculated using the 16 bit CRC as the pseudo random number generator. - n = PIOS_CRC16_updateByte(n, 0); - float num_channels = rfm22b_dev->num_channels; - return (uint8_t)(num_channels * (float)n / (float)0xffff); -} - -static bool rfm22_changeChannel(struct pios_rfm22b_dev *rfm22b_dev) -{ - if (rfm22_isConnected(rfm22b_dev)) { - return rfm22_setFreqHopChannel(rfm22b_dev, rfm22_calcChannel(rfm22b_dev)); - } - return false; + // Call the com port configuration function + if (rfm22b_dev->com_config_cb) { + rfm22b_dev->com_config_cb(cph->main_port, cph->flexi_port, cph->vcp_port, cph->com_speed, + cph->min_frequency, cph->max_frequency, cph->channel_spacing); + } + + // Configure this modem from the connection request message. + rfm22_setNominalCarrierFrequency(rfm22b_dev, cph->min_frequency, cph->max_frequency, cph->channel_spacing); + rfm22_setDatarate(rfm22b_dev, rfm22b_dev->datarate, true); } +/** + * Accept a connection request. + * + * @param[in] rfm22b_dev The device structure + * @return enum pios_rfm22b_event The next event to inject + */ static enum pios_rfm22b_event rfm22_acceptConnection(struct pios_rfm22b_dev *rfm22b_dev) { - // Set our connection state to connected - rfm22b_dev->stats.link_state = OPLINKSTATUS_LINKSTATE_CONNECTED; + // Set our connection state to connected + rfm22b_dev->stats.link_state = OPLINKSTATUS_LINKSTATE_CONNECTED; - // Copy the connection packet - PHConnectionPacketHandle cph = (PHConnectionPacketHandle)(&(rfm22b_dev->rx_packet)); - PHConnectionPacketHandle lcph = (PHConnectionPacketHandle)(&(rfm22b_dev->con_packet)); - memcpy((uint8_t*)lcph, (uint8_t*)cph, PH_PACKET_SIZE((PHPacketHandle)cph)); + // Copy the connection packet + PHConnectionPacketHandle cph = (PHConnectionPacketHandle)(&(rfm22b_dev->rx_packet)); + PHConnectionPacketHandle lcph = (PHConnectionPacketHandle)(&(rfm22b_dev->con_packet)); + memcpy((uint8_t*)lcph, (uint8_t*)cph, PH_PACKET_SIZE((PHPacketHandle)cph)); - // Set the destination ID to the source of the connection request message. - rfm22b_dev->destination_id = cph->source_id; + // Set the destination ID to the source of the connection request message. + rfm22b_dev->destination_id = cph->source_id; - return RFM22B_EVENT_DEFAULT; + return RFM22B_EVENT_DEFAULT; } -static void rfm22_clearLEDs() { - LINK_LED_OFF; - RX_LED_OFF; - TX_LED_OFF; -#if defined(PIOS_RFM22B_DEBUG_ON_TELEM) || defined(PIOS_RFM22B_DEBUG_ON_RCVR) - D1_LED_OFF; - D2_LED_OFF; - D3_LED_OFF; - D4_LED_OFF; -#endif + +/***************************************************************************** + * Frequency Hopping Functions + *****************************************************************************/ + +/** + * There needs to be a buffer in time around a channel change in which we delay starting a new packet transmit. + * This function returns true of we are in that range of time. + * + * @param[in] rfm22b_dev The device structure + * @return True if we're near a channel change time. + */ +static bool rfm22_inChannelBuffer(struct pios_rfm22b_dev *rfm22b_dev) +{ + portTickType time = rfm22_coordinatorTime(rfm22b_dev, xTaskGetTickCount()); + uint8_t window = (uint8_t)(time & 0x7e); + return ((window == 0x7f) || (window == 0)); } +/** + * Return the extimated current clock ticks count on the coordinator modem. + * This is the master clock used for all synchronization. + * + * @param[in] rfm22b_dev The device structure + */ +static portTickType rfm22_coordinatorTime(struct pios_rfm22b_dev *rfm22b_dev, portTickType ticks) +{ + return ticks + rfm22b_dev->time_delta; +} + +/** + * Calculate what the current channel shold be. + * + * @param[in] rfm22b_dev The device structure + */ +static uint8_t rfm22_calcChannel(struct pios_rfm22b_dev *rfm22b_dev) +{ + portTickType time = rfm22_coordinatorTime(rfm22b_dev, xTaskGetTickCount()); + // We change channels every 128 ms. + uint16_t n = (time >> 7) & 0xffff; + // The channel is calculated using the 16 bit CRC as the pseudo random number generator. + n = PIOS_CRC16_updateByte(n, 0); + float num_channels = rfm22b_dev->num_channels; + return (uint8_t)(num_channels * (float)n / (float)0xffff); +} + +/** + * Change channels to the calculated current channel. + * + * @param[in] rfm22b_dev The device structure + */ +static bool rfm22_changeChannel(struct pios_rfm22b_dev *rfm22b_dev) +{ + if (rfm22_isConnected(rfm22b_dev)) { + return rfm22_setFreqHopChannel(rfm22b_dev, rfm22_calcChannel(rfm22b_dev)); + } + return false; +} + + +/***************************************************************************** + * Error Handling Functions + *****************************************************************************/ + +/** + * Recover from a failure in receiving a packet. + * + * @param[in] rfm22b_dev The device structure + * @return enum pios_rfm22b_event The next event to inject + */ +static enum pios_rfm22b_event rfm22_rxFailure(struct pios_rfm22b_dev *rfm22b_dev) +{ + rfm22b_dev->stats.rx_failure++; + rfm22b_dev->rx_buffer_wr = 0; + rfm22b_dev->rx_complete_ticks = xTaskGetTickCount(); + rfm22b_dev->in_rx_mode = false; + if (rfm22b_dev->rx_complete_ticks == 0) { + rfm22b_dev->rx_complete_ticks = 1; + } + return RFM22B_EVENT_RX_MODE; +} + +/** + * Recover from a transmit failure. + * + * @param[in] rfm22b_dev The device structure + * @return enum pios_rfm22b_event The next event to inject + */ +static enum pios_rfm22b_event rfm22_txFailure(struct pios_rfm22b_dev *rfm22b_dev) +{ + rfm22b_dev->stats.tx_failure++; + rfm22b_dev->tx_data_wr = rfm22b_dev->tx_data_rd = 0; + return RFM22B_EVENT_TX_START; +} + +/** + * Recover from a timeout event. + * + * @param[in] rfm22b_dev The device structure + * @return enum pios_rfm22b_event The next event to inject + */ static enum pios_rfm22b_event rfm22_timeout(struct pios_rfm22b_dev *rfm22b_dev) { - rfm22b_dev->stats.timeouts++; - rfm22b_dev->packet_start_ticks = 0; - // Release the Tx packet if it's set. - if (rfm22b_dev->tx_packet != 0) - { - rfm22b_dev->tx_data_rd = rfm22b_dev->tx_data_wr = 0; - } - rfm22b_dev->rx_buffer_wr = 0; - TX_LED_OFF; - RX_LED_OFF; + rfm22b_dev->stats.timeouts++; + rfm22b_dev->packet_start_ticks = 0; + // Release the Tx packet if it's set. + if (rfm22b_dev->tx_packet != 0) { + rfm22b_dev->tx_data_rd = rfm22b_dev->tx_data_wr = 0; + } + rfm22b_dev->rx_buffer_wr = 0; + TX_LED_OFF; + RX_LED_OFF; #if defined(PIOS_RFM22B_DEBUG_ON_TELEM) || defined(PIOS_RFM22B_DEBUG_ON_RCVR) - D1_LED_OFF; - D2_LED_OFF; - D3_LED_OFF; - D4_LED_OFF; + D1_LED_OFF; + D2_LED_OFF; + D3_LED_OFF; + D4_LED_OFF; #endif - return RFM22B_EVENT_TX_START; + return RFM22B_EVENT_TX_START; } +/** + * Recover from a severe error. + * + * @param[in] rfm22b_dev The device structure + * @return enum pios_rfm22b_event The next event to inject + */ static enum pios_rfm22b_event rfm22_error(struct pios_rfm22b_dev *rfm22b_dev) { - rfm22b_dev->stats.resets++; - rfm22_clearLEDs(); - return RFM22B_EVENT_INITIALIZE; + rfm22b_dev->stats.resets++; + rfm22_clearLEDs(); + return RFM22B_EVENT_INITIALIZE; } /** * A fatal error has occured in the state machine. * this should not happen. - * \parem [in] rfm22b_dev The device structure - * \return enum pios_rfm22b_event The next event to inject + * + * @parem [in] rfm22b_dev The device structure + * @return enum pios_rfm22b_event The next event to inject */ static enum pios_rfm22b_event rfm22_fatal_error(struct pios_rfm22b_dev *rfm22b_dev) { - // RF module error .. flash the LED's - rfm22_clearLEDs(); - for(unsigned int j = 0; j < 16; j++) - { - USB_LED_ON; - LINK_LED_ON; - RX_LED_OFF; - TX_LED_OFF; + // RF module error .. flash the LED's + rfm22_clearLEDs(); + for(unsigned int j = 0; j < 16; j++) { + USB_LED_ON; + LINK_LED_ON; + RX_LED_OFF; + TX_LED_OFF; - PIOS_DELAY_WaitmS(200); + PIOS_DELAY_WaitmS(200); - USB_LED_OFF; - LINK_LED_OFF; - RX_LED_ON; - TX_LED_ON; + USB_LED_OFF; + LINK_LED_OFF; + RX_LED_ON; + TX_LED_ON; - PIOS_DELAY_WaitmS(200); - } + PIOS_DELAY_WaitmS(200); + } - PIOS_DELAY_WaitmS(1000); + PIOS_DELAY_WaitmS(1000); - PIOS_Assert(0); + PIOS_Assert(0); - return RFM22B_EVENT_FATAL_ERROR; + return RFM22B_EVENT_FATAL_ERROR; } -// ************************************ + +/***************************************************************************** + * Utility Functions + *****************************************************************************/ + +/** + * Calculate the time difference between the start time and end time. + * Times are in ticks. Also handles rollover. + * + * @param[in] start_time The start time in ticks. + * @param[in] end_time The end time in ticks. + */ +static uint32_t pios_rfm22_time_difference_ms(portTickType start_time, portTickType end_time) +{ + if(end_time >= start_time) { + return (end_time - start_time) * portTICK_RATE_MS; + } + // Rollover + return ((portMAX_DELAY - start_time) + end_time) * portTICK_RATE_MS; +} + +/** + * Allocate the device structure + */ +#if defined(PIOS_INCLUDE_FREERTOS) +static struct pios_rfm22b_dev *pios_rfm22_alloc(void) +{ + struct pios_rfm22b_dev * rfm22b_dev; + + rfm22b_dev = (struct pios_rfm22b_dev *)pvPortMalloc(sizeof(*rfm22b_dev)); + rfm22b_dev->spi_id = 0; + if (!rfm22b_dev) { + return NULL; + } + + rfm22b_dev->magic = PIOS_RFM22B_DEV_MAGIC; + return(rfm22b_dev); +} +#else +static struct pios_rfm22b_dev pios_rfm22b_devs[PIOS_RFM22B_MAX_DEVS]; +static uint8_t pios_rfm22b_num_devs; +static struct pios_rfm22b_dev *pios_rfm22_alloc(void) +{ + struct pios_rfm22b_dev * rfm22b_dev; + + if (pios_rfm22b_num_devs >= PIOS_RFM22B_MAX_DEVS) { + return NULL; + } + + rfm22b_dev = &pios_rfm22b_devs[pios_rfm22b_num_devs++]; + rfm22b_dev->magic = PIOS_RFM22B_DEV_MAGIC; + + return (rfm22b_dev); +} +#endif + +/** + * Turn off all of the LEDs + */ +static void rfm22_clearLEDs(void) { + LINK_LED_OFF; + RX_LED_OFF; + TX_LED_OFF; +#if defined(PIOS_RFM22B_DEBUG_ON_TELEM) || defined(PIOS_RFM22B_DEBUG_ON_RCVR) + D1_LED_OFF; + D2_LED_OFF; + D3_LED_OFF; + D4_LED_OFF; +#endif +} + + +/***************************************************************************** + * SPI Read/Write Functions + *****************************************************************************/ + +/** + * Assert the chip select line. + * + * @param[in] rfm22b_dev The RFM22B device. + */ +static void rfm22_assertCs(struct pios_rfm22b_dev *rfm22b_dev) +{ + PIOS_DELAY_WaituS(1); + if(rfm22b_dev->spi_id != 0) { + PIOS_SPI_RC_PinSet(rfm22b_dev->spi_id, rfm22b_dev->slave_num, 0); + } +} + +/** + * Deassert the chip select line. + * + * @param[in] rfm22b_dev The RFM22B device structure pointer. + */ +static void rfm22_deassertCs(struct pios_rfm22b_dev *rfm22b_dev) +{ + if(rfm22b_dev->spi_id != 0) { + PIOS_SPI_RC_PinSet(rfm22b_dev->spi_id, rfm22b_dev->slave_num, 1); + } +} + +/** + * Claim the SPI bus. + * + * @param[in] rfm22b_dev The RFM22B device structure pointer. + */ +static void rfm22_claimBus(struct pios_rfm22b_dev *rfm22b_dev) +{ + if(rfm22b_dev->spi_id != 0) { + PIOS_SPI_ClaimBus(rfm22b_dev->spi_id); + } +} + +/** + * Release the SPI bus. + * + * @param[in] rfm22b_dev The RFM22B device structure pointer. + */ +static void rfm22_releaseBus(struct pios_rfm22b_dev *rfm22b_dev) +{ + if(rfm22b_dev->spi_id != 0) { + PIOS_SPI_ReleaseBus(rfm22b_dev->spi_id); + } +} + +/** + * Claim the semaphore and write a byte to a register + * + * @param[in] rfm22b_dev The RFM22B device. + * @param[in] addr The address to write to + * @param[in] data The datat to write to that address + */ +static void rfm22_write(struct pios_rfm22b_dev *rfm22b_dev, uint8_t addr, uint8_t data) +{ + rfm22_claimBus(rfm22b_dev); + rfm22_assertCs(rfm22b_dev); + uint8_t buf[2] = {addr | 0x80, data}; + PIOS_SPI_TransferBlock(rfm22b_dev->spi_id, buf, NULL, sizeof(buf), NULL); + rfm22_deassertCs(rfm22b_dev); + rfm22_releaseBus(rfm22b_dev); +} + +/** + * Read a byte from an RFM22b register + * + * @param[in] rfm22b_dev The RFM22B device structure pointer. + * @param[in] addr The address to read from + * @return Returns the result of the register read + */ +static uint8_t rfm22_read(struct pios_rfm22b_dev *rfm22b_dev, uint8_t addr) +{ + uint8_t in[2]; + uint8_t out[2] = {addr & 0x7f, 0xFF}; + rfm22_claimBus(rfm22b_dev); + rfm22_assertCs(rfm22b_dev); + PIOS_SPI_TransferBlock(rfm22b_dev->spi_id, out, in, sizeof(out), NULL); + rfm22_deassertCs(rfm22b_dev); + rfm22_releaseBus(rfm22b_dev); + return in[1]; +} + +/** + * Read a byte from an RFM22b register without claiming the bus + * + * @param[in] rfm22b_dev The RFM22B device structure pointer. + * @param[in] addr The address to read from + * @return Returns the result of the register read + */ +static uint8_t rfm22_read_noclaim(struct pios_rfm22b_dev *rfm22b_dev, uint8_t addr) +{ + uint8_t out[2] = {addr & 0x7F, 0xFF}; + uint8_t in[2]; + rfm22_assertCs(rfm22b_dev); + PIOS_SPI_TransferBlock(rfm22b_dev->spi_id, out, in, sizeof(out), NULL); + rfm22_deassertCs(rfm22b_dev); + return in[1]; +} #endif /* PIOS_INCLUDE_RFM22B */ diff --git a/flight/pios/common/pios_rfm22b_com.c b/flight/pios/common/pios_rfm22b_com.c index 89364777c..11081b11d 100644 --- a/flight/pios/common/pios_rfm22b_com.c +++ b/flight/pios/common/pios_rfm22b_com.c @@ -44,87 +44,123 @@ static bool PIOS_RFM22B_COM_Available(uint32_t rfm22b_com_id); /* Local variables */ const struct pios_com_driver pios_rfm22b_com_driver = { - .set_baud = PIOS_RFM22B_COM_ChangeBaud, - .tx_start = PIOS_RFM22B_COM_TxStart, - .rx_start = PIOS_RFM22B_COM_RxStart, - .bind_tx_cb = PIOS_RFM22B_COM_RegisterTxCallback, - .bind_rx_cb = PIOS_RFM22B_COM_RegisterRxCallback, - .available = PIOS_RFM22B_COM_Available + .set_baud = PIOS_RFM22B_COM_ChangeBaud, + .tx_start = PIOS_RFM22B_COM_TxStart, + .rx_start = PIOS_RFM22B_COM_RxStart, + .bind_tx_cb = PIOS_RFM22B_COM_RegisterTxCallback, + .bind_rx_cb = PIOS_RFM22B_COM_RegisterRxCallback, + .available = PIOS_RFM22B_COM_Available }; /** * Changes the baud rate of the RFM22B peripheral without re-initialising. - * \param[in] rfm22b_id RFM22B name (GPS, TELEM, AUX) - * \param[in] baud Requested baud rate + * + * @param[in] rfm22b_id RFM22B name (GPS, TELEM, AUX) + * @param[in] baud Requested baud rate */ static void PIOS_RFM22B_COM_ChangeBaud(uint32_t rfm22b_id, uint32_t baud) { - struct pios_rfm22b_dev *rfm22b_dev = (struct pios_rfm22b_dev *)rfm22b_id; - if (!PIOS_RFM22B_validate(rfm22b_dev)) - return; - // Set the RF data rate on the modem to ~2X the selected buad rate because the modem is half duplex. - enum rfm22b_datarate datarate = RFM22_datarate_64000; - if (baud <= 1024) - datarate = RFM22_datarate_500; - else if (baud <= 2048) - datarate = RFM22_datarate_1000; - else if (baud <= 4096) - datarate = RFM22_datarate_8000; - else if (baud <= 9600) - datarate = RFM22_datarate_16000; - else if (baud <= 19200) - datarate = RFM22_datarate_32000; - else if (baud <= 38400) - datarate = RFM22_datarate_57600; - else if (baud <= 57600) - datarate = RFM22_datarate_128000; - else if (baud <= 115200) - datarate = RFM22_datarate_192000; - rfm22b_dev->datarate = datarate; + struct pios_rfm22b_dev *rfm22b_dev = (struct pios_rfm22b_dev *)rfm22b_id; + if (!PIOS_RFM22B_Validate(rfm22b_dev)) { + return; + } + // Set the RF data rate on the modem to ~2X the selected buad rate because the modem is half duplex. + enum rfm22b_datarate datarate = RFM22_datarate_64000; + if (baud <= 1024) + datarate = RFM22_datarate_500; + else if (baud <= 2048) + datarate = RFM22_datarate_1000; + else if (baud <= 4096) + datarate = RFM22_datarate_8000; + else if (baud <= 9600) + datarate = RFM22_datarate_16000; + else if (baud <= 19200) + datarate = RFM22_datarate_32000; + else if (baud <= 38400) + datarate = RFM22_datarate_57600; + else if (baud <= 57600) + datarate = RFM22_datarate_128000; + else if (baud <= 115200) + datarate = RFM22_datarate_192000; + rfm22b_dev->datarate = datarate; } +/** + * Start a receive from the COM device + * + * @param[in] rfm22b_dev The device ID. + * @param[in] rx_bytes_available The number of bytes available to receive + */ static void PIOS_RFM22B_COM_RxStart(uint32_t rfm22b_id, uint16_t rx_bytes_avail) { } +/** + * Start a transmit from the COM device + * + * @param[in] rfm22b_dev The device ID. + * @param[in] tx_bytes_available The number of bytes available to transmit + */ static void PIOS_RFM22B_COM_TxStart(uint32_t rfm22b_id, uint16_t tx_bytes_avail) { - struct pios_rfm22b_dev *rfm22b_dev = (struct pios_rfm22b_dev *)rfm22b_id; - if (!PIOS_RFM22B_validate(rfm22b_dev)) - return; + struct pios_rfm22b_dev *rfm22b_dev = (struct pios_rfm22b_dev *)rfm22b_id; + if (!PIOS_RFM22B_Validate(rfm22b_dev)) { + return; + } } +/** + * Register the callback to pass received data to + * + * @param[in] rfm22b_dev The device ID. + * @param[in] rx_in_cb The Rx callback function. + * @param[in] context The callback context. + */ static void PIOS_RFM22B_COM_RegisterRxCallback(uint32_t rfm22b_id, pios_com_callback rx_in_cb, uint32_t context) { - struct pios_rfm22b_dev *rfm22b_dev = (struct pios_rfm22b_dev *)rfm22b_id; - if (!PIOS_RFM22B_validate(rfm22b_dev)) - return; + struct pios_rfm22b_dev *rfm22b_dev = (struct pios_rfm22b_dev *)rfm22b_id; + if (!PIOS_RFM22B_Validate(rfm22b_dev)) + return; - /* - * Order is important in these assignments since ISR uses _cb - * field to determine if it's ok to dereference _cb and _context - */ - rfm22b_dev->rx_in_context = context; - rfm22b_dev->rx_in_cb = rx_in_cb; + /* + * Order is important in these assignments since ISR uses _cb + * field to determine if it's ok to dereference _cb and _context + */ + rfm22b_dev->rx_in_context = context; + rfm22b_dev->rx_in_cb = rx_in_cb; } +/** + * Register the callback to get data from. + * + * @param[in] rfm22b_dev The device ID. + * @param[in] rx_in_cb The Tx callback function. + * @param[in] context The callback context. + */ static void PIOS_RFM22B_COM_RegisterTxCallback(uint32_t rfm22b_id, pios_com_callback tx_out_cb, uint32_t context) { - struct pios_rfm22b_dev *rfm22b_dev = (struct pios_rfm22b_dev *)rfm22b_id; - if (!PIOS_RFM22B_validate(rfm22b_dev)) - return; + struct pios_rfm22b_dev *rfm22b_dev = (struct pios_rfm22b_dev *)rfm22b_id; + if (!PIOS_RFM22B_Validate(rfm22b_dev)) { + return; + } - /* - * Order is important in these assignments since ISR uses _cb - * field to determine if it's ok to dereference _cb and _context - */ - rfm22b_dev->tx_out_context = context; - rfm22b_dev->tx_out_cb = tx_out_cb; + /* + * Order is important in these assignments since ISR uses _cb + * field to determine if it's ok to dereference _cb and _context + */ + rfm22b_dev->tx_out_context = context; + rfm22b_dev->tx_out_cb = tx_out_cb; } +/** + * See if the COM port is alive + * + * @param[in] rfm22b_dev The device ID. + * @return True of the device is available. + */ static bool PIOS_RFM22B_COM_Available(uint32_t rfm22b_id) { - return PIOS_RFM22B_LinkStatus(rfm22b_id); + return PIOS_RFM22B_LinkStatus(rfm22b_id); } #endif /* PIOS_INCLUDE_RFM22B_COM */ diff --git a/flight/pios/common/pios_rfm22b_rcvr.c b/flight/pios/common/pios_rfm22b_rcvr.c index 889615cb4..faa21b604 100644 --- a/flight/pios/common/pios_rfm22b_rcvr.c +++ b/flight/pios/common/pios_rfm22b_rcvr.c @@ -1,17 +1,17 @@ /** - ****************************************************************************** - * @addtogroup PIOS PIOS Core hardware abstraction layer - * @{ - * @addtogroup PIOS_RFM22B_RCVR RFM22B Receiver Input Functions - * @brief Code to output the PPM signal from the RFM22B - * @{ - * - * @file pios_rfm22b_rcvr.c - * @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2012. - * @brief Implements a receiver interface to the RFM22B device - * @see The GNU Public License (GPL) Version 3 - * - *****************************************************************************/ +****************************************************************************** +* @addtogroup PIOS PIOS Core hardware abstraction layer +* @{ +* @addtogroup PIOS_RFM22B_RCVR RFM22B Receiver Input Functions +* @brief Code to output the PPM signal from the RFM22B +* @{ +* +* @file pios_rfm22b_rcvr.c +* @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2012. +* @brief Implements a receiver interface to the RFM22B device +* @see The GNU Public License (GPL) Version 3 +* +*****************************************************************************/ /* * This program is free software; you can redistribute it and/or modify * it under the terms of the GNU General Public License as published by @@ -39,60 +39,86 @@ static int32_t PIOS_RFM22B_RCVR_Get(uint32_t rcvr_id, uint8_t channel); static void PIOS_RFM22B_RCVR_Supervisor(uint32_t rcvr_id); const struct pios_rcvr_driver pios_rfm22b_rcvr_driver = { - .read = PIOS_RFM22B_RCVR_Get, + .read = PIOS_RFM22B_RCVR_Get, }; +/** + * Initialize the receiver. + * + * @param[in] rfm22b_dev The receiver ID. + * @return < 0 on failure. + */ int32_t PIOS_RFM22B_RCVR_Init(uint32_t rcvr_id) { - struct pios_rfm22b_dev *rfm22b_dev = (struct pios_rfm22b_dev *)rcvr_id; - if (!PIOS_RFM22B_validate(rfm22b_dev)) - return -1; + struct pios_rfm22b_dev *rfm22b_dev = (struct pios_rfm22b_dev *)rcvr_id; + if (!PIOS_RFM22B_Validate(rfm22b_dev)) { + return -1; + } - // Initialize - for (uint8_t i = 0; i < PIOS_RFM22B_RCVR_MAX_CHANNELS; ++i) - rfm22b_dev->ppm_channel[i] = PIOS_RCVR_TIMEOUT; - rfm22b_dev->ppm_supv_timer = 0; + // Initialize + for (uint8_t i = 0; i < PIOS_RFM22B_RCVR_MAX_CHANNELS; ++i) { + rfm22b_dev->ppm_channel[i] = PIOS_RCVR_TIMEOUT; + } + rfm22b_dev->ppm_supv_timer = 0; - // Register the failsafe timer callback. - if (!PIOS_RTC_RegisterTickCallback(PIOS_RFM22B_RCVR_Supervisor, rcvr_id)) - PIOS_DEBUG_Assert(0); + // Register the failsafe timer callback. + if (!PIOS_RTC_RegisterTickCallback(PIOS_RFM22B_RCVR_Supervisor, rcvr_id)) { + PIOS_DEBUG_Assert(0); + } - return 0; + return 0; } +/** + * Get a channel from the receiver. + * + * @param[in] rcvr_id The receiver ID. + * @return The channel value, or -1 on failure. + */ static int32_t PIOS_RFM22B_RCVR_Get(uint32_t rcvr_id, uint8_t channel) { - struct pios_rfm22b_dev *rfm22b_dev = (struct pios_rfm22b_dev *)rcvr_id; - if (!PIOS_RFM22B_validate(rfm22b_dev)) - return -1; + struct pios_rfm22b_dev *rfm22b_dev = (struct pios_rfm22b_dev *)rcvr_id; + if (!PIOS_RFM22B_Validate(rfm22b_dev)) { + return -1; + } - if (channel >= GCSRECEIVER_CHANNEL_NUMELEM) - /* channel is out of range */ - return -1; + if (channel >= GCSRECEIVER_CHANNEL_NUMELEM) { + /* channel is out of range */ + return -1; + } - return rfm22b_dev->ppm_channel[channel]; + return rfm22b_dev->ppm_channel[channel]; } +/** + * The supervisor function that ensures that the data is current. + * + * @param[in] rcvr_id The receiver ID. + */ static void PIOS_RFM22B_RCVR_Supervisor(uint32_t rcvr_id) { - struct pios_rfm22b_dev *rfm22b_dev = (struct pios_rfm22b_dev *)rcvr_id; - if (!PIOS_RFM22B_validate(rfm22b_dev)) - return; + struct pios_rfm22b_dev *rfm22b_dev = (struct pios_rfm22b_dev *)rcvr_id; + if (!PIOS_RFM22B_Validate(rfm22b_dev)) { + return; + } - // RTC runs at 625Hz. - if (++(rfm22b_dev->ppm_supv_timer) < (PIOS_RFM22B_RCVR_TIMEOUT_MS * 1000 / 625)) - return; - rfm22b_dev->ppm_supv_timer = 0; + // RTC runs at 625Hz. + if (++(rfm22b_dev->ppm_supv_timer) < (PIOS_RFM22B_RCVR_TIMEOUT_MS * 1000 / 625)) { + return; + } + rfm22b_dev->ppm_supv_timer = 0; - // Have we received fresh values since the last update? - if (!rfm22b_dev->ppm_fresh) - for (uint8_t i = 0; i < PIOS_RFM22B_RCVR_MAX_CHANNELS; ++i) - rfm22b_dev->ppm_channel[i] = 0; - rfm22b_dev->ppm_fresh = false; + // Have we received fresh values since the last update? + if (!rfm22b_dev->ppm_fresh) { + for (uint8_t i = 0; i < PIOS_RFM22B_RCVR_MAX_CHANNELS; ++i) { + rfm22b_dev->ppm_channel[i] = 0; + } + } + rfm22b_dev->ppm_fresh = false; } #endif /* PIOS_INCLUDE_RFM22B_RCVR */ /** - * @} - * @} - */ + * @} + * @} + */ diff --git a/flight/pios/inc/pios_rfm22b.h b/flight/pios/inc/pios_rfm22b.h index 88f7bff17..5c0514a8b 100644 --- a/flight/pios/inc/pios_rfm22b.h +++ b/flight/pios/inc/pios_rfm22b.h @@ -1,17 +1,17 @@ /** - ****************************************************************************** - * @addtogroup PIOS PIOS Core hardware abstraction layer - * @{ - * @addtogroup PIOS_RFM22B Radio Functions - * @brief PIOS interface for RFM22B Radio - * @{ - * - * @file pios_rfm22b.h - * @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2012. - * @brief RFM22B functions header. - * @see The GNU Public License (GPL) Version 3 - * - *****************************************************************************/ +****************************************************************************** +* @addtogroup PIOS PIOS Core hardware abstraction layer +* @{ +* @addtogroup PIOS_RFM22B Radio Functions +* @brief PIOS interface for RFM22B Radio +* @{ +* +* @file pios_rfm22b.h +* @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2012. +* @brief RFM22B functions header. +* @see The GNU Public License (GPL) Version 3 +* +*****************************************************************************/ /* * This program is free software; you can redistribute it and/or modify * it under the terms of the GNU General Public License as published by @@ -39,62 +39,62 @@ enum gpio_direction {GPIO0_TX_GPIO1_RX, GPIO0_RX_GPIO1_TX}; /* Global Types */ struct pios_rfm22b_cfg { - const struct pios_spi_cfg * spi_cfg; /* Pointer to SPI interface configuration */ - const struct pios_exti_cfg * exti_cfg; /* Pointer to the EXTI configuration */ - uint8_t RFXtalCap; - uint8_t slave_num; - enum gpio_direction gpio_direction; + const struct pios_spi_cfg * spi_cfg; /* Pointer to SPI interface configuration */ + const struct pios_exti_cfg * exti_cfg; /* Pointer to the EXTI configuration */ + uint8_t RFXtalCap; + uint8_t slave_num; + enum gpio_direction gpio_direction; }; enum rfm22b_tx_power { - RFM22_tx_pwr_txpow_0 = 0x00, // +1dBm .. 1.25mW - RFM22_tx_pwr_txpow_1 = 0x01, // +2dBm .. 1.6mW - RFM22_tx_pwr_txpow_2 = 0x02, // +5dBm .. 3.16mW - RFM22_tx_pwr_txpow_3 = 0x03, // +8dBm .. 6.3mW - RFM22_tx_pwr_txpow_4 = 0x04, // +11dBm .. 12.6mW - RFM22_tx_pwr_txpow_5 = 0x05, // +14dBm .. 25mW - RFM22_tx_pwr_txpow_6 = 0x06, // +17dBm .. 50mW - RFM22_tx_pwr_txpow_7 = 0x07 // +20dBm .. 100mW + RFM22_tx_pwr_txpow_0 = 0x00, // +1dBm .. 1.25mW + RFM22_tx_pwr_txpow_1 = 0x01, // +2dBm .. 1.6mW + RFM22_tx_pwr_txpow_2 = 0x02, // +5dBm .. 3.16mW + RFM22_tx_pwr_txpow_3 = 0x03, // +8dBm .. 6.3mW + RFM22_tx_pwr_txpow_4 = 0x04, // +11dBm .. 12.6mW + RFM22_tx_pwr_txpow_5 = 0x05, // +14dBm .. 25mW + RFM22_tx_pwr_txpow_6 = 0x06, // +17dBm .. 50mW + RFM22_tx_pwr_txpow_7 = 0x07 // +20dBm .. 100mW }; enum rfm22b_datarate { - RFM22_datarate_500 = 0, - RFM22_datarate_1000 = 1, - RFM22_datarate_2000 = 2, - RFM22_datarate_4000 = 3, - RFM22_datarate_8000 = 4, - RFM22_datarate_9600 = 5, - RFM22_datarate_16000 = 6, - RFM22_datarate_19200 = 7, - RFM22_datarate_24000 = 8, - RFM22_datarate_32000 = 9, - RFM22_datarate_57600 = 10, - RFM22_datarate_64000 = 11, - RFM22_datarate_128000 = 12, - RFM22_datarate_192000 = 13, - RFM22_datarate_256000 = 14, + RFM22_datarate_500 = 0, + RFM22_datarate_1000 = 1, + RFM22_datarate_2000 = 2, + RFM22_datarate_4000 = 3, + RFM22_datarate_8000 = 4, + RFM22_datarate_9600 = 5, + RFM22_datarate_16000 = 6, + RFM22_datarate_19200 = 7, + RFM22_datarate_24000 = 8, + RFM22_datarate_32000 = 9, + RFM22_datarate_57600 = 10, + RFM22_datarate_64000 = 11, + RFM22_datarate_128000 = 12, + RFM22_datarate_192000 = 13, + RFM22_datarate_256000 = 14, }; struct rfm22b_stats { - uint16_t packets_per_sec; - uint16_t tx_byte_count; - uint16_t rx_byte_count; - uint16_t tx_seq; - uint16_t rx_seq; - uint8_t rx_good; - uint8_t rx_corrected; - uint8_t rx_error; - uint8_t rx_missed; - uint8_t rx_failure; - uint8_t tx_dropped; - uint8_t tx_resent; - uint8_t tx_failure; - uint8_t resets; - uint8_t timeouts; - uint8_t link_quality; - int8_t rssi; - int8_t afc_correction; - uint8_t link_state; + uint16_t packets_per_sec; + uint16_t tx_byte_count; + uint16_t rx_byte_count; + uint16_t tx_seq; + uint16_t rx_seq; + uint8_t rx_good; + uint8_t rx_corrected; + uint8_t rx_error; + uint8_t rx_missed; + uint8_t rx_failure; + uint8_t tx_dropped; + uint8_t tx_resent; + uint8_t tx_failure; + uint8_t resets; + uint8_t timeouts; + uint8_t link_quality; + int8_t rssi; + int8_t afc_correction; + uint8_t link_state; }; /* Callback function prototypes */ @@ -124,6 +124,6 @@ extern const struct pios_com_driver pios_rfm22b_com_driver; #endif /* PIOS_RFM22B_H */ /** - * @} - * @} - */ + * @} + * @} + */ diff --git a/flight/pios/inc/pios_rfm22b_priv.h b/flight/pios/inc/pios_rfm22b_priv.h index 57594c3fe..9b64d97ca 100644 --- a/flight/pios/inc/pios_rfm22b_priv.h +++ b/flight/pios/inc/pios_rfm22b_priv.h @@ -528,229 +528,229 @@ typedef int16_t (*t_rfm22_TxDataByteCallback) (void); typedef bool (*t_rfm22_RxDataCallback) (void *data, uint8_t len); enum pios_rfm22b_dev_magic { - PIOS_RFM22B_DEV_MAGIC = 0x68e971b6, + PIOS_RFM22B_DEV_MAGIC = 0x68e971b6, }; enum pios_rfm22b_state { - RFM22B_STATE_UNINITIALIZED, - RFM22B_STATE_INITIALIZING, - RFM22B_STATE_REQUESTING_CONNECTION, - RFM22B_STATE_ACCEPTING_CONNECTION, - RFM22B_STATE_RX_MODE, - RFM22B_STATE_WAIT_PREAMBLE, - RFM22B_STATE_WAIT_SYNC, - RFM22B_STATE_RX_DATA, - RFM22B_STATE_RX_FAILURE, - RFM22B_STATE_RECEIVING_STATUS, - RFM22B_STATE_TX_START, - RFM22B_STATE_TX_DATA, - RFM22B_STATE_TX_FAILURE, - RFM22B_STATE_SENDING_ACK, - RFM22B_STATE_SENDING_NACK, - RFM22B_STATE_RECEIVING_ACK, - RFM22B_STATE_RECEIVING_NACK, - RFM22B_STATE_TIMEOUT, - RFM22B_STATE_ERROR, - RFM22B_STATE_FATAL_ERROR, + RFM22B_STATE_UNINITIALIZED, + RFM22B_STATE_INITIALIZING, + RFM22B_STATE_REQUESTING_CONNECTION, + RFM22B_STATE_ACCEPTING_CONNECTION, + RFM22B_STATE_RX_MODE, + RFM22B_STATE_WAIT_PREAMBLE, + RFM22B_STATE_WAIT_SYNC, + RFM22B_STATE_RX_DATA, + RFM22B_STATE_RX_FAILURE, + RFM22B_STATE_RECEIVING_STATUS, + RFM22B_STATE_TX_START, + RFM22B_STATE_TX_DATA, + RFM22B_STATE_TX_FAILURE, + RFM22B_STATE_SENDING_ACK, + RFM22B_STATE_SENDING_NACK, + RFM22B_STATE_RECEIVING_ACK, + RFM22B_STATE_RECEIVING_NACK, + RFM22B_STATE_TIMEOUT, + RFM22B_STATE_ERROR, + RFM22B_STATE_FATAL_ERROR, - RFM22B_STATE_NUM_STATES // Must be last + RFM22B_STATE_NUM_STATES // Must be last }; enum pios_rfm22b_event { - RFM22B_EVENT_DEFAULT, - RFM22B_EVENT_INT_RECEIVED, - RFM22B_EVENT_INITIALIZE, - RFM22B_EVENT_INITIALIZED, - RFM22B_EVENT_REQUEST_CONNECTION, - RFM22B_EVENT_CONNECTION_REQUESTED, - RFM22B_EVENT_PACKET_ACKED, - RFM22B_EVENT_PACKET_NACKED, - RFM22B_EVENT_ACK_TIMEOUT, - RFM22B_EVENT_RX_MODE, - RFM22B_EVENT_PREAMBLE_DETECTED, - RFM22B_EVENT_SYNC_DETECTED, - RFM22B_EVENT_RX_COMPLETE, - RFM22B_EVENT_RX_ERROR, - RFM22B_EVENT_STATUS_RECEIVED, - RFM22B_EVENT_TX_START, - RFM22B_EVENT_FAILURE, - RFM22B_EVENT_TIMEOUT, - RFM22B_EVENT_ERROR, - RFM22B_EVENT_FATAL_ERROR, + RFM22B_EVENT_DEFAULT, + RFM22B_EVENT_INT_RECEIVED, + RFM22B_EVENT_INITIALIZE, + RFM22B_EVENT_INITIALIZED, + RFM22B_EVENT_REQUEST_CONNECTION, + RFM22B_EVENT_CONNECTION_REQUESTED, + RFM22B_EVENT_PACKET_ACKED, + RFM22B_EVENT_PACKET_NACKED, + RFM22B_EVENT_ACK_TIMEOUT, + RFM22B_EVENT_RX_MODE, + RFM22B_EVENT_PREAMBLE_DETECTED, + RFM22B_EVENT_SYNC_DETECTED, + RFM22B_EVENT_RX_COMPLETE, + RFM22B_EVENT_RX_ERROR, + RFM22B_EVENT_STATUS_RECEIVED, + RFM22B_EVENT_TX_START, + RFM22B_EVENT_FAILURE, + RFM22B_EVENT_TIMEOUT, + RFM22B_EVENT_ERROR, + RFM22B_EVENT_FATAL_ERROR, - RFM22B_EVENT_NUM_EVENTS // Must be last + RFM22B_EVENT_NUM_EVENTS // Must be last }; #define RFM22B_RX_PACKET_STATS_LEN 4 enum pios_rfm22b_rx_packet_status { - RFM22B_GOOD_RX_PACKET = 0x00, - RFM22B_CORRECTED_RX_PACKET = 0x01, - RFM22B_ERROR_RX_PACKET = 0x2, - RFM22B_RESENT_TX_PACKET = 0x3 + RFM22B_GOOD_RX_PACKET = 0x00, + RFM22B_CORRECTED_RX_PACKET = 0x01, + RFM22B_ERROR_RX_PACKET = 0x2, + RFM22B_RESENT_TX_PACKET = 0x3 }; typedef struct { - uint32_t pairID; - int8_t rssi; - int8_t afc_correction; - uint8_t lastContact; + uint32_t pairID; + int8_t rssi; + int8_t afc_correction; + uint8_t lastContact; } rfm22b_pair_stats; typedef struct { - uint32_t pairID; - OPLinkSettingsRemoteMainPortOptions main_port; - OPLinkSettingsRemoteFlexiPortOptions flexi_port; - OPLinkSettingsRemoteVCPPortOptions vcp_port; - OPLinkSettingsComSpeedOptions com_speed; + uint32_t pairID; + OPLinkSettingsRemoteMainPortOptions main_port; + OPLinkSettingsRemoteFlexiPortOptions flexi_port; + OPLinkSettingsRemoteVCPPortOptions vcp_port; + OPLinkSettingsComSpeedOptions com_speed; } rfm22b_binding; struct pios_rfm22b_dev { - enum pios_rfm22b_dev_magic magic; - struct pios_rfm22b_cfg cfg; + enum pios_rfm22b_dev_magic magic; + struct pios_rfm22b_cfg cfg; - // The SPI bus information - uint32_t spi_id; - uint32_t slave_num; + // The SPI bus information + uint32_t spi_id; + uint32_t slave_num; - // The device ID - uint32_t deviceID; + // The device ID + uint32_t deviceID; - // The destination ID - uint32_t destination_id; + // The destination ID + uint32_t destination_id; - // The list of bound radios. - rfm22b_binding bindings[OPLINKSETTINGS_BINDINGS_NUMELEM]; - uint8_t cur_binding; + // The list of bound radios. + rfm22b_binding bindings[OPLINKSETTINGS_BINDINGS_NUMELEM]; + uint8_t cur_binding; - // Is this device a coordinator? - bool coordinator; + // Is this device a coordinator? + bool coordinator; - // The task handle - xTaskHandle taskHandle; + // The task handle + xTaskHandle taskHandle; - // The potential paired statistics - rfm22b_pair_stats pair_stats[OPLINKSTATUS_PAIRIDS_NUMELEM]; + // The potential paired statistics + rfm22b_pair_stats pair_stats[OPLINKSTATUS_PAIRIDS_NUMELEM]; - // ISR pending semaphore - xSemaphoreHandle isrPending; + // ISR pending semaphore + xSemaphoreHandle isrPending; - // The com configuration callback - PIOS_RFM22B_ComConfigCallback com_config_cb; + // The com configuration callback + PIOS_RFM22B_ComConfigCallback com_config_cb; - // The COM callback functions. - pios_com_callback rx_in_cb; - uint32_t rx_in_context; - pios_com_callback tx_out_cb; - uint32_t tx_out_context; + // The COM callback functions. + pios_com_callback rx_in_cb; + uint32_t rx_in_context; + pios_com_callback tx_out_cb; + uint32_t tx_out_context; - // the transmit power to use for data transmissions - uint8_t tx_power; + // the transmit power to use for data transmissions + uint8_t tx_power; - // The RF datarate lookup index. - uint8_t datarate; + // The RF datarate lookup index. + uint8_t datarate; - // The state machine state and the current event - enum pios_rfm22b_state state; + // The state machine state and the current event + enum pios_rfm22b_state state; - // The event queue handle - xQueueHandle eventQueue; + // The event queue handle + xQueueHandle eventQueue; - // device status register - uint8_t device_status; - // interrupt status register 1 - uint8_t int_status1; - // interrupt status register 2 - uint8_t int_status2; - // ezmac status register - uint8_t ezmac_status; + // device status register + uint8_t device_status; + // interrupt status register 1 + uint8_t int_status1; + // interrupt status register 2 + uint8_t int_status2; + // ezmac status register + uint8_t ezmac_status; - // The error statistics counters - uint16_t prev_rx_seq_num; - uint32_t rx_packet_stats[RFM22B_RX_PACKET_STATS_LEN]; + // The error statistics counters + uint16_t prev_rx_seq_num; + uint32_t rx_packet_stats[RFM22B_RX_PACKET_STATS_LEN]; - // The packet statistics - struct rfm22b_stats stats; + // The packet statistics + struct rfm22b_stats stats; - // Stats - uint16_t errors; + // Stats + uint16_t errors; - // RSSI in dBm - int8_t rssi_dBm; + // RSSI in dBm + int8_t rssi_dBm; - // The tx data packet - PHPacket data_packet; - // The current tx packet - PHPacketHandle tx_packet; - // The previous tx packet (waiting for an ACK) - PHPacketHandle prev_tx_packet; - // The tx data read index - uint16_t tx_data_rd; - // The tx data write index - uint16_t tx_data_wr; - // The tx packet sequence number - uint16_t tx_seq; + // The tx data packet + PHPacket data_packet; + // The current tx packet + PHPacketHandle tx_packet; + // The previous tx packet (waiting for an ACK) + PHPacketHandle prev_tx_packet; + // The tx data read index + uint16_t tx_data_rd; + // The tx data write index + uint16_t tx_data_wr; + // The tx packet sequence number + uint16_t tx_seq; - // The rx data packet - PHPacket rx_packet; - // The receive buffer write index - uint16_t rx_buffer_wr; - // The receive buffer write index - uint16_t rx_packet_len; - // Is the modem currently in Rx mode? - bool in_rx_mode; + // The rx data packet + PHPacket rx_packet; + // The receive buffer write index + uint16_t rx_buffer_wr; + // The receive buffer write index + uint16_t rx_packet_len; + // Is the modem currently in Rx mode? + bool in_rx_mode; - // The status packet - PHStatusPacket status_packet; + // The status packet + PHStatusPacket status_packet; - // The ACK/NACK packet - PHAckNackPacket ack_nack_packet; + // The ACK/NACK packet + PHAckNackPacket ack_nack_packet; #ifdef PIOS_PPM_RECEIVER - // The PPM packet - PHPpmPacket ppm_packet; + // The PPM packet + PHPpmPacket ppm_packet; #endif - // The connection packet. - PHConnectionPacket con_packet; + // The connection packet. + PHConnectionPacket con_packet; - // Send flags - bool send_status; - bool send_ppm; - bool send_connection_request; - bool time_to_send; + // Send flags + bool send_status; + bool send_ppm; + bool send_connection_request; + bool time_to_send; - // The offset between our clock and the global send clock - uint8_t time_to_send_offset; + // The offset between our clock and the global send clock + uint8_t time_to_send_offset; - // The initial frequency - uint32_t init_frequency; - // The number of frequency hopping channels. - uint16_t num_channels; + // The initial frequency + uint32_t init_frequency; + // The number of frequency hopping channels. + uint16_t num_channels; - // The frequency hopping step size - float frequency_step_size; - // current frequency hop channel - uint8_t frequency_hop_channel; - // afc correction reading (in Hz) - int8_t afc_correction_Hz; + // The frequency hopping step size + float frequency_step_size; + // current frequency hop channel + uint8_t frequency_hop_channel; + // afc correction reading (in Hz) + int8_t afc_correction_Hz; - // The packet timers. - portTickType packet_start_ticks; - portTickType tx_complete_ticks; - portTickType rx_complete_ticks; - portTickType time_delta; + // The packet timers. + portTickType packet_start_ticks; + portTickType tx_complete_ticks; + portTickType rx_complete_ticks; + portTickType time_delta; - // The maximum time (ms) that it should take to transmit / receive a packet. - uint32_t max_packet_time; + // The maximum time (ms) that it should take to transmit / receive a packet. + uint32_t max_packet_time; - // The maximum time to wait for an ACK. - uint8_t max_ack_delay; + // The maximum time to wait for an ACK. + uint8_t max_ack_delay; #ifdef PIOS_INCLUDE_RFM22B_RCVR - // The PPM channel values - uint16_t ppm_channel[PIOS_RFM22B_RCVR_MAX_CHANNELS]; - uint8_t ppm_supv_timer; - bool ppm_fresh; + // The PPM channel values + uint16_t ppm_channel[PIOS_RFM22B_RCVR_MAX_CHANNELS]; + uint8_t ppm_supv_timer; + bool ppm_fresh; #endif }; @@ -758,9 +758,17 @@ struct pios_rfm22b_dev { // External function definitions bool PIOS_RFM22_EXT_Int(void); -bool PIOS_RFM22B_validate(struct pios_rfm22b_dev * rfm22b_dev); void PIOS_RFM22B_InjectEvent(struct pios_rfm22b_dev *rfm22b_dev, enum pios_rfm22b_event event, bool inISR); +/** + * Validate that the device structure is valid. + * + * @param[in] rfm22b_dev The RFM22B device structure pointer. + */ +inline bool PIOS_RFM22B_Validate(struct pios_rfm22b_dev *rfm22b_dev) +{ + return (rfm22b_dev != NULL && rfm22b_dev->magic == PIOS_RFM22B_DEV_MAGIC); +} // Global variable definitions From 9bb3b5870cb739e67920121d2afac0667bd5702f Mon Sep 17 00:00:00 2001 From: Brian Webb Date: Sat, 13 Apr 2013 17:52:48 +0100 Subject: [PATCH 6/9] OP-914: Reformatted the RadioComBridge module. --- .../modules/RadioComBridge/RadioComBridge.c | 527 +++++++++--------- 1 file changed, 267 insertions(+), 260 deletions(-) diff --git a/flight/modules/RadioComBridge/RadioComBridge.c b/flight/modules/RadioComBridge/RadioComBridge.c index c3263276d..ccb6199ef 100644 --- a/flight/modules/RadioComBridge/RadioComBridge.c +++ b/flight/modules/RadioComBridge/RadioComBridge.c @@ -1,17 +1,17 @@ /** - ****************************************************************************** - * @addtogroup OpenPilotModules OpenPilot Modules - * @{ - * @addtogroup RadioComBridgeModule Com Port to Radio Bridge Module - * @brief Bridge Com and Radio ports - * @{ - * - * @file RadioComBridge.c - * @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2012. - * @brief Bridges selected Com Port to the COM VCP emulated serial port - * @see The GNU Public License (GPL) Version 3 - * - *****************************************************************************/ +****************************************************************************** +* @addtogroup OpenPilotModules OpenPilot Modules +* @{ +* @addtogroup RadioComBridgeModule Com Port to Radio Bridge Module +* @brief Bridge Com and Radio ports +* @{ +* +* @file RadioComBridge.c +* @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2012. +* @brief Bridges selected Com Port to the COM VCP emulated serial port +* @see The GNU Public License (GPL) Version 3 +* +*****************************************************************************/ /* * This program is free software; you can redistribute it and/or modify * it under the terms of the GNU General Public License as published by @@ -69,30 +69,30 @@ void PIOS_InitPPMFlexiPort(bool input); typedef struct { - // The task handles. - xTaskHandle telemetryTxTaskHandle; - xTaskHandle radioRxTaskHandle; - xTaskHandle radioTxTaskHandle; + // The task handles. + xTaskHandle telemetryTxTaskHandle; + xTaskHandle radioRxTaskHandle; + xTaskHandle radioTxTaskHandle; - // The UAVTalk connection on the com side. - UAVTalkConnection outUAVTalkCon; - UAVTalkConnection inUAVTalkCon; + // The UAVTalk connection on the com side. + UAVTalkConnection outUAVTalkCon; + UAVTalkConnection inUAVTalkCon; - // Queue handles. - xQueueHandle gcsEventQueue; - xQueueHandle uavtalkEventQueue; + // Queue handles. + xQueueHandle gcsEventQueue; + xQueueHandle uavtalkEventQueue; - // Error statistics. - uint32_t comTxErrors; - uint32_t comTxRetries; - uint32_t UAVTalkErrors; - uint32_t droppedPackets; + // Error statistics. + uint32_t comTxErrors; + uint32_t comTxRetries; + uint32_t UAVTalkErrors; + uint32_t droppedPackets; - // Should we parse UAVTalk? - bool parseUAVTalk; + // Should we parse UAVTalk? + bool parseUAVTalk; - // The current configured uart speed - OPLinkSettingsComSpeedOptions comSpeed; + // The current configured uart speed + OPLinkSettingsComSpeedOptions comSpeed; } RadioComBridgeData; @@ -118,8 +118,8 @@ static RadioComBridgeData *data; /** * Start the module - * \return -1 if initialisation failed - * \return 0 on success + * + * @return -1 if initialisation failed, 0 on success */ static int32_t RadioComBridgeStart(void) { @@ -137,8 +137,7 @@ static int32_t RadioComBridgeStart(void) if (is_coordinator) { // Set the maximum radio RF power. - switch (oplinkSettings.MaxRFPower) - { + switch (oplinkSettings.MaxRFPower) { case OPLINKSETTINGS_MAXRFPOWER_125: PIOS_RFM22B_SetTxPower(pios_rfm22b_id, RFM22_tx_pwr_txpow_0); break; @@ -198,175 +197,186 @@ static int32_t RadioComBridgeStart(void) /** * Initialise the module - * \return -1 if initialisation failed - * \return 0 on success + * + * @return -1 if initialisation failed on success */ static int32_t RadioComBridgeInitialize(void) { - // allocate and initialize the static data storage only if module is enabled - data = (RadioComBridgeData *)pvPortMalloc(sizeof(RadioComBridgeData)); - if (!data) - return -1; + // allocate and initialize the static data storage only if module is enabled + data = (RadioComBridgeData *)pvPortMalloc(sizeof(RadioComBridgeData)); + if (!data) { + return -1; + } - // Initialize the UAVObjects that we use - OPLinkStatusInitialize(); - ObjectPersistenceInitialize(); + // Initialize the UAVObjects that we use + OPLinkStatusInitialize(); + ObjectPersistenceInitialize(); - // Initialise UAVTalk - data->outUAVTalkCon = UAVTalkInitialize(&UAVTalkSendHandler); - data->inUAVTalkCon = UAVTalkInitialize(&RadioSendHandler); + // Initialise UAVTalk + data->outUAVTalkCon = UAVTalkInitialize(&UAVTalkSendHandler); + data->inUAVTalkCon = UAVTalkInitialize(&RadioSendHandler); - // Initialize the queues. - data->uavtalkEventQueue = xQueueCreate(EVENT_QUEUE_SIZE, sizeof(UAVObjEvent)); + // Initialize the queues. + data->uavtalkEventQueue = xQueueCreate(EVENT_QUEUE_SIZE, sizeof(UAVObjEvent)); - // Configure our UAVObjects for updates. - UAVObjConnectQueue(UAVObjGetByID(OPLINKSTATUS_OBJID), data->uavtalkEventQueue, EV_UPDATED | EV_UPDATED_MANUAL | EV_UPDATE_REQ); - UAVObjConnectQueue(UAVObjGetByID(OBJECTPERSISTENCE_OBJID), data->uavtalkEventQueue, EV_UPDATED | EV_UPDATED_MANUAL); + // Configure our UAVObjects for updates. + UAVObjConnectQueue(UAVObjGetByID(OPLINKSTATUS_OBJID), data->uavtalkEventQueue, EV_UPDATED | EV_UPDATED_MANUAL | EV_UPDATE_REQ); + UAVObjConnectQueue(UAVObjGetByID(OBJECTPERSISTENCE_OBJID), data->uavtalkEventQueue, EV_UPDATED | EV_UPDATED_MANUAL); #if defined(PIOS_INCLUDE_RFM22B_GCSRECEIVER) - UAVObjConnectQueue(UAVObjGetByID(GCSRECEIVER_OBJID), data->uavtalkEventQueue, EV_UPDATED | EV_UPDATED_MANUAL | EV_UPDATE_REQ); + UAVObjConnectQueue(UAVObjGetByID(GCSRECEIVER_OBJID), data->uavtalkEventQueue, EV_UPDATED | EV_UPDATED_MANUAL | EV_UPDATE_REQ); #endif - // Initialize the statistics. - data->comTxErrors = 0; - data->comTxRetries = 0; - data->UAVTalkErrors = 0; - data->parseUAVTalk = false; - data->comSpeed = OPLINKSETTINGS_COMSPEED_9600; - PIOS_COM_RADIO = PIOS_COM_RFM22B; + // Initialize the statistics. + data->comTxErrors = 0; + data->comTxRetries = 0; + data->UAVTalkErrors = 0; + data->parseUAVTalk = false; + data->comSpeed = OPLINKSETTINGS_COMSPEED_9600; + PIOS_COM_RADIO = PIOS_COM_RFM22B; - return 0; + return 0; } MODULE_INITCALL(RadioComBridgeInitialize, RadioComBridgeStart) /** * Telemetry transmit task, regular priority + * + * @param[in] parameters The task parameters */ static void telemetryTxTask(void *parameters) { - UAVObjEvent ev; + UAVObjEvent ev; - // Loop forever - while (1) { + // Loop forever + while (1) { #ifdef PIOS_INCLUDE_WDG - PIOS_WDG_UpdateFlag(PIOS_WDG_TELEMETRY); + PIOS_WDG_UpdateFlag(PIOS_WDG_TELEMETRY); #endif - // Wait for queue message - if (xQueueReceive(data->uavtalkEventQueue, &ev, MAX_PORT_DELAY) == pdTRUE) { - if ((ev.event == EV_UPDATED) || (ev.event == EV_UPDATE_REQ)) - { - // Send update (with retries) - uint32_t retries = 0; - int32_t success = -1; - while (retries < MAX_RETRIES && success == -1) { - success = UAVTalkSendObject(data->outUAVTalkCon, ev.obj, 0, 0, RETRY_TIMEOUT_MS) == 0; - if (!success) - ++retries; - } - data->comTxRetries += retries; - } - else if(ev.event == EV_SEND_ACK) - { - // Send the ACK - uint32_t retries = 0; - int32_t success = -1; - while (retries < MAX_RETRIES && success == -1) { - success = UAVTalkSendAck(data->outUAVTalkCon, ev.obj, ev.instId) == 0; - if (!success) - ++retries; - } - data->comTxRetries += retries; - } - else if(ev.event == EV_SEND_NACK) - { - // Send the NACK - uint32_t retries = 0; - int32_t success = -1; - while (retries < MAX_RETRIES && success == -1) { - success = UAVTalkSendNack(data->outUAVTalkCon, UAVObjGetID(ev.obj)) == 0; - if (!success) - ++retries; - } - data->comTxRetries += retries; - } - } - } + // Wait for queue message + if (xQueueReceive(data->uavtalkEventQueue, &ev, MAX_PORT_DELAY) == pdTRUE) { + if ((ev.event == EV_UPDATED) || (ev.event == EV_UPDATE_REQ)) { + // Send update (with retries) + uint32_t retries = 0; + int32_t success = -1; + while (retries < MAX_RETRIES && success == -1) { + success = UAVTalkSendObject(data->outUAVTalkCon, ev.obj, 0, 0, RETRY_TIMEOUT_MS) == 0; + if (!success) + ++retries; + } + data->comTxRetries += retries; + } else if(ev.event == EV_SEND_ACK) { + // Send the ACK + uint32_t retries = 0; + int32_t success = -1; + while (retries < MAX_RETRIES && success == -1) { + success = UAVTalkSendAck(data->outUAVTalkCon, ev.obj, ev.instId) == 0; + if (!success) + ++retries; + } + data->comTxRetries += retries; + } else if(ev.event == EV_SEND_NACK) { + // Send the NACK + uint32_t retries = 0; + int32_t success = -1; + while (retries < MAX_RETRIES && success == -1) { + success = UAVTalkSendNack(data->outUAVTalkCon, UAVObjGetID(ev.obj)) == 0; + if (!success) + ++retries; + } + data->comTxRetries += retries; + } + } + } } /** * Radio rx task. Receive data packets from the radio and pass them on. + * + * @param[in] parameters The task parameters */ static void radioRxTask(void *parameters) { - // Task loop - while (1) { + // Task loop + while (1) { #ifdef PIOS_INCLUDE_WDG - PIOS_WDG_UpdateFlag(PIOS_WDG_RADIORX); + PIOS_WDG_UpdateFlag(PIOS_WDG_RADIORX); #endif - uint8_t serial_data[1]; - uint16_t bytes_to_process = PIOS_COM_ReceiveBuffer(PIOS_COM_RADIO, serial_data, sizeof(serial_data), MAX_PORT_DELAY); - if (bytes_to_process > 0) - for (uint8_t i = 0; i < bytes_to_process; i++) - if (UAVTalkRelayInputStream(data->outUAVTalkCon, serial_data[i]) == UAVTALK_STATE_ERROR) - data->UAVTalkErrors++; - } + uint8_t serial_data[1]; + uint16_t bytes_to_process = PIOS_COM_ReceiveBuffer(PIOS_COM_RADIO, serial_data, sizeof(serial_data), MAX_PORT_DELAY); + if (bytes_to_process > 0) { + for (uint8_t i = 0; i < bytes_to_process; i++) { + if (UAVTalkRelayInputStream(data->outUAVTalkCon, serial_data[i]) == UAVTALK_STATE_ERROR) { + data->UAVTalkErrors++; + } + } + } + } } /** * Radio rx task. Receive data from a com port and pass it on to the radio. + * + * @param[in] parameters The task parameters */ static void radioTxTask(void *parameters) { - // Task loop - while (1) { - uint32_t inputPort = PIOS_COM_TELEMETRY; + // Task loop + while (1) { + uint32_t inputPort = PIOS_COM_TELEMETRY; #ifdef PIOS_INCLUDE_WDG - PIOS_WDG_UpdateFlag(PIOS_WDG_RADIOTX); + PIOS_WDG_UpdateFlag(PIOS_WDG_RADIOTX); #endif #if defined(PIOS_INCLUDE_USB) - // Determine output port (USB takes priority over telemetry port) - if (PIOS_USB_CheckAvailable(0) && PIOS_COM_TELEM_USB_HID) - inputPort = PIOS_COM_TELEM_USB_HID; + // Determine output port (USB takes priority over telemetry port) + if (PIOS_USB_CheckAvailable(0) && PIOS_COM_TELEM_USB_HID) { + inputPort = PIOS_COM_TELEM_USB_HID; + } #endif /* PIOS_INCLUDE_USB */ - if(inputPort) { - uint8_t serial_data[1]; - uint16_t bytes_to_process = PIOS_COM_ReceiveBuffer(inputPort, serial_data, sizeof(serial_data), MAX_PORT_DELAY); - if (bytes_to_process > 0) { - for (uint8_t i = 0; i < bytes_to_process; i++) - ProcessInputStream(data->inUAVTalkCon, serial_data[i]); - } - } else - vTaskDelay(5); - } + if(inputPort) { + uint8_t serial_data[1]; + uint16_t bytes_to_process = PIOS_COM_ReceiveBuffer(inputPort, serial_data, sizeof(serial_data), MAX_PORT_DELAY); + if (bytes_to_process > 0) { + for (uint8_t i = 0; i < bytes_to_process; i++) + ProcessInputStream(data->inUAVTalkCon, serial_data[i]); + } + } else { + vTaskDelay(5); + } + } } /** * Transmit data buffer to the com port. - * \param[in] buf Data buffer to send - * \param[in] length Length of buffer - * \return -1 on failure - * \return number of bytes transmitted on success + * + * @param[in] buf Data buffer to send + * @param[in] length Length of buffer + * @return -1 on failure + * @return number of bytes transmitted on success */ static int32_t UAVTalkSendHandler(uint8_t *buf, int32_t length) { - uint32_t outputPort = PIOS_COM_TELEMETRY; + uint32_t outputPort = PIOS_COM_TELEMETRY; #if defined(PIOS_INCLUDE_USB) - // Determine output port (USB takes priority over telemetry port) - if (PIOS_COM_TELEM_USB_HID && PIOS_COM_Available(PIOS_COM_TELEM_USB_HID)) - outputPort = PIOS_COM_TELEM_USB_HID; + // Determine output port (USB takes priority over telemetry port) + if (PIOS_COM_TELEM_USB_HID && PIOS_COM_Available(PIOS_COM_TELEM_USB_HID)) { + outputPort = PIOS_COM_TELEM_USB_HID; + } #endif /* PIOS_INCLUDE_USB */ - if(outputPort) - return PIOS_COM_SendBufferNonBlocking(outputPort, buf, length); - else - return -1; + if(outputPort) { + return PIOS_COM_SendBufferNonBlocking(outputPort, buf, length); + } else { + return -1; + } } /** * Transmit data buffer to the com port. - * \param[in] buf Data buffer to send - * \param[in] length Length of buffer - * \return -1 on failure - * \return number of bytes transmitted on success + * + * @param[in] buf Data buffer to send + * @param[in] length Length of buffer + * @return -1 on failure + * @return number of bytes transmitted on success */ static int32_t RadioSendHandler(uint8_t *buf, int32_t length) { @@ -380,136 +390,136 @@ static int32_t RadioSendHandler(uint8_t *buf, int32_t length) } } +/** + * Process a byte of data received + * + * @param[in] connectionHandle The UAVTalk connection handle + * @param[in] rxbyte The received byte. + */ static void ProcessInputStream(UAVTalkConnection connectionHandle, uint8_t rxbyte) { - // Keep reading until we receive a completed packet. - UAVTalkRxState state = UAVTalkRelayInputStream(connectionHandle, rxbyte); - UAVTalkConnectionData *connection = (UAVTalkConnectionData*)(connectionHandle); - UAVTalkInputProcessor *iproc = &(connection->iproc); + // Keep reading until we receive a completed packet. + UAVTalkRxState state = UAVTalkRelayInputStream(connectionHandle, rxbyte); + UAVTalkConnectionData *connection = (UAVTalkConnectionData*)(connectionHandle); + UAVTalkInputProcessor *iproc = &(connection->iproc); - if (state == UAVTALK_STATE_COMPLETE) - { - // Is this a local UAVObject? - // We only generate GcsReceiver ojects, we don't consume them. - if ((iproc->obj != NULL) && (iproc->objId != GCSRECEIVER_OBJID)) - { - // We treat the ObjectPersistence object differently - if(iproc->objId == OBJECTPERSISTENCE_OBJID) - { - // Unpack object, if the instance does not exist it will be created! - UAVObjUnpack(iproc->obj, iproc->instId, connection->rxBuffer); + if (state == UAVTALK_STATE_COMPLETE) { + // Is this a local UAVObject? + // We only generate GcsReceiver ojects, we don't consume them. + if ((iproc->obj != NULL) && (iproc->objId != GCSRECEIVER_OBJID)) { + // We treat the ObjectPersistence object differently + if(iproc->objId == OBJECTPERSISTENCE_OBJID) { + // Unpack object, if the instance does not exist it will be created! + UAVObjUnpack(iproc->obj, iproc->instId, connection->rxBuffer); - // Get the ObjectPersistence object. - ObjectPersistenceData obj_per; - ObjectPersistenceGet(&obj_per); + // Get the ObjectPersistence object. + ObjectPersistenceData obj_per; + ObjectPersistenceGet(&obj_per); - // Is this concerning or setting object? - if (obj_per.ObjectID == OPLINKSETTINGS_OBJID) - { - // Queue up the ACK. - queueEvent(data->uavtalkEventQueue, (void*)iproc->obj, iproc->instId, EV_SEND_ACK); + // Is this concerning or setting object? + if (obj_per.ObjectID == OPLINKSETTINGS_OBJID) { + // Queue up the ACK. + queueEvent(data->uavtalkEventQueue, (void*)iproc->obj, iproc->instId, EV_SEND_ACK); - // Is this a save, load, or delete? - bool success = true; - switch (obj_per.Operation) - { - case OBJECTPERSISTENCE_OPERATION_LOAD: - { + // Is this a save, load, or delete? + bool success = true; + switch (obj_per.Operation) { + case OBJECTPERSISTENCE_OPERATION_LOAD: + { #if defined(PIOS_INCLUDE_FLASH_EEPROM) - // Load the settings. - OPLinkSettingsData oplinkSettings; - if (PIOS_EEPROM_Load((uint8_t*)&oplinkSettings, sizeof(OPLinkSettingsData)) == 0) - OPLinkSettingsSet(&oplinkSettings); - else - success = false; + // Load the settings. + OPLinkSettingsData oplinkSettings; + if (PIOS_EEPROM_Load((uint8_t*)&oplinkSettings, sizeof(OPLinkSettingsData)) == 0) + OPLinkSettingsSet(&oplinkSettings); + else + success = false; #endif - break; - } - case OBJECTPERSISTENCE_OPERATION_SAVE: - { + break; + } + case OBJECTPERSISTENCE_OPERATION_SAVE: + { #if defined(PIOS_INCLUDE_FLASH_EEPROM) - // Save the settings. - OPLinkSettingsData oplinkSettings; - OPLinkSettingsGet(&oplinkSettings); - int32_t ret = PIOS_EEPROM_Save((uint8_t*)&oplinkSettings, sizeof(OPLinkSettingsData)); - if (ret != 0) - success = false; + // Save the settings. + OPLinkSettingsData oplinkSettings; + OPLinkSettingsGet(&oplinkSettings); + int32_t ret = PIOS_EEPROM_Save((uint8_t*)&oplinkSettings, sizeof(OPLinkSettingsData)); + if (ret != 0) + success = false; #endif - break; - } - case OBJECTPERSISTENCE_OPERATION_DELETE: - { + break; + } + case OBJECTPERSISTENCE_OPERATION_DELETE: + { #if defined(PIOS_INCLUDE_FLASH_EEPROM) - // Erase the settings. - OPLinkSettingsData oplinkSettings; - uint8_t *ptr = (uint8_t*)&oplinkSettings; - memset(ptr, 0, sizeof(OPLinkSettingsData)); - int32_t ret = PIOS_EEPROM_Save(ptr, sizeof(OPLinkSettingsData)); - if (ret != 0) - success = false; + // Erase the settings. + OPLinkSettingsData oplinkSettings; + uint8_t *ptr = (uint8_t*)&oplinkSettings; + memset(ptr, 0, sizeof(OPLinkSettingsData)); + int32_t ret = PIOS_EEPROM_Save(ptr, sizeof(OPLinkSettingsData)); + if (ret != 0) + success = false; #endif - break; - } - default: - break; - } - if (success == true) - { - obj_per.Operation = OBJECTPERSISTENCE_OPERATION_COMPLETED; - ObjectPersistenceSet(&obj_per); - } - } - } - else - { - switch (iproc->type) - { - case UAVTALK_TYPE_OBJ: - // Unpack object, if the instance does not exist it will be created! - UAVObjUnpack(iproc->obj, iproc->instId, connection->rxBuffer); - break; - case UAVTALK_TYPE_OBJ_REQ: - // Queue up an object send request. - queueEvent(data->uavtalkEventQueue, (void*)iproc->obj, iproc->instId, EV_UPDATE_REQ); - break; - case UAVTALK_TYPE_OBJ_ACK: - if (UAVObjUnpack(iproc->obj, iproc->instId, connection->rxBuffer) == 0) - // Queue up an ACK - queueEvent(data->uavtalkEventQueue, (void*)iproc->obj, iproc->instId, EV_SEND_ACK); - break; - } - } - } + break; + } + default: + break; + } + if (success == true) { + obj_per.Operation = OBJECTPERSISTENCE_OPERATION_COMPLETED; + ObjectPersistenceSet(&obj_per); + } + } + } else { + switch (iproc->type) { + case UAVTALK_TYPE_OBJ: + // Unpack object, if the instance does not exist it will be created! + UAVObjUnpack(iproc->obj, iproc->instId, connection->rxBuffer); + break; + case UAVTALK_TYPE_OBJ_REQ: + // Queue up an object send request. + queueEvent(data->uavtalkEventQueue, (void*)iproc->obj, iproc->instId, EV_UPDATE_REQ); + break; + case UAVTALK_TYPE_OBJ_ACK: + if (UAVObjUnpack(iproc->obj, iproc->instId, connection->rxBuffer) == 0) + // Queue up an ACK + queueEvent(data->uavtalkEventQueue, (void*)iproc->obj, iproc->instId, EV_SEND_ACK); + break; + } + } + } - } else if(state == UAVTALK_STATE_ERROR) { - data->UAVTalkErrors++; + } else if(state == UAVTALK_STATE_ERROR) { + data->UAVTalkErrors++; - // Send a NACK if required. - if((iproc->obj) && (iproc->type == UAVTALK_TYPE_OBJ_ACK)) - // Queue up a NACK - queueEvent(data->uavtalkEventQueue, iproc->obj, iproc->instId, EV_SEND_NACK); - } + // Send a NACK if required. + if((iproc->obj) && (iproc->type == UAVTALK_TYPE_OBJ_ACK)) { + // Queue up a NACK + queueEvent(data->uavtalkEventQueue, iproc->obj, iproc->instId, EV_SEND_NACK); + } + } } /** * Queue and event into an event queue. - * \param[in] queue The event queue - * \param[in] obj The data pointer - * \param[in] type The event type + * + * @param[in] queue The event queue + * @param[in] obj The data pointer + * @param[in] type The event type */ static void queueEvent(xQueueHandle queue, void *obj, uint16_t instId, UAVObjEventType type) { - UAVObjEvent ev; - ev.obj = (UAVObjHandle)obj; - ev.instId = instId; - ev.event = type; - xQueueSend(queue, &ev, portMAX_DELAY); + UAVObjEvent ev; + ev.obj = (UAVObjHandle)obj; + ev.instId = instId; + ev.event = type; + xQueueSend(queue, &ev, portMAX_DELAY); } /** * Configure the output port based on a configuration event from the remote coordinator. - * \param[in] com_port The com port to configure - * \param[in] com_speed The com port speed + * + * @param[in] com_port The com port to configure + * @param[in] com_speed The com port speed */ static void configureComCallback(OPLinkSettingsRemoteMainPortOptions main_port, OPLinkSettingsRemoteFlexiPortOptions flexi_port, OPLinkSettingsRemoteVCPPortOptions vcp_port, OPLinkSettingsComSpeedOptions com_speed, @@ -582,8 +592,7 @@ static void updateSettings() // Configure the main port bool is_coordinator = PIOS_RFM22B_IsCoordinator(pios_rfm22b_id); - switch (oplinkSettings.MainPort) - { + switch (oplinkSettings.MainPort) { case OPLINKSETTINGS_MAINPORT_TELEMETRY: data->parseUAVTalk = true; case OPLINKSETTINGS_MAINPORT_SERIAL: @@ -599,8 +608,7 @@ static void updateSettings() } // Configure the flexi port - switch (oplinkSettings.FlexiPort) - { + switch (oplinkSettings.FlexiPort) { case OPLINKSETTINGS_FLEXIPORT_TELEMETRY: data->parseUAVTalk = true; case OPLINKSETTINGS_FLEXIPORT_SERIAL: @@ -616,8 +624,7 @@ static void updateSettings() } // Configure the USB VCP port - switch (oplinkSettings.VCPPort) - { + switch (oplinkSettings.VCPPort) { case OPLINKSETTINGS_VCPPORT_SERIAL: PIOS_COM_TELEMETRY = PIOS_COM_TELEM_USB_VCP; break; From bb6986ea042b2bd3377c5591a1cd762ead882ba4 Mon Sep 17 00:00:00 2001 From: Brian Webb Date: Tue, 16 Apr 2013 14:56:21 +0100 Subject: [PATCH 7/9] OP-914 - Converted PIOS_RFM22B_Validate back into a normal function from an inline. --- flight/pios/common/pios_rfm22b.c | 10 ++++++++++ flight/pios/inc/pios_rfm22b_priv.h | 12 +----------- 2 files changed, 11 insertions(+), 11 deletions(-) diff --git a/flight/pios/common/pios_rfm22b.c b/flight/pios/common/pios_rfm22b.c index b938798f4..30be8fbd5 100644 --- a/flight/pios/common/pios_rfm22b.c +++ b/flight/pios/common/pios_rfm22b.c @@ -790,6 +790,16 @@ bool PIOS_RFM22B_LinkStatus(uint32_t rfm22b_id) return (rfm22_isConnected(rfm22b_dev) && (rfm22b_dev->stats.link_quality > RFM22B_LINK_QUALITY_THRESHOLD)); } +/** + * Validate that the device structure is valid. + * + * @param[in] rfm22b_dev The RFM22B device structure pointer. + */ +inline bool PIOS_RFM22B_Validate(struct pios_rfm22b_dev *rfm22b_dev) +{ + return (rfm22b_dev != NULL && rfm22b_dev->magic == PIOS_RFM22B_DEV_MAGIC); +} + /***************************************************************************** * The Device Control Thread diff --git a/flight/pios/inc/pios_rfm22b_priv.h b/flight/pios/inc/pios_rfm22b_priv.h index 9b64d97ca..584c354d5 100644 --- a/flight/pios/inc/pios_rfm22b_priv.h +++ b/flight/pios/inc/pios_rfm22b_priv.h @@ -758,17 +758,7 @@ struct pios_rfm22b_dev { // External function definitions bool PIOS_RFM22_EXT_Int(void); -void PIOS_RFM22B_InjectEvent(struct pios_rfm22b_dev *rfm22b_dev, enum pios_rfm22b_event event, bool inISR); - -/** - * Validate that the device structure is valid. - * - * @param[in] rfm22b_dev The RFM22B device structure pointer. - */ -inline bool PIOS_RFM22B_Validate(struct pios_rfm22b_dev *rfm22b_dev) -{ - return (rfm22b_dev != NULL && rfm22b_dev->magic == PIOS_RFM22B_DEV_MAGIC); -} +bool PIOS_RFM22B_Validate(struct pios_rfm22b_dev *rfm22b_dev); // Global variable definitions From 31bf1f4d271b1b6ba94490afd46b464084c92efe Mon Sep 17 00:00:00 2001 From: David Ankers Date: Sat, 27 Apr 2013 12:19:50 +1000 Subject: [PATCH 8/9] Just for testing, might need to be backed out, easier to put it here for now so it gets dragged in with test builds. --- shared/uavobjectdefinition/stabilizationsettings.xml | 8 ++++---- 1 file changed, 4 insertions(+), 4 deletions(-) diff --git a/shared/uavobjectdefinition/stabilizationsettings.xml b/shared/uavobjectdefinition/stabilizationsettings.xml index 0be267cf6..bc1ea12e8 100644 --- a/shared/uavobjectdefinition/stabilizationsettings.xml +++ b/shared/uavobjectdefinition/stabilizationsettings.xml @@ -1,10 +1,10 @@ PID settings used by the Stabilization module to combine the @ref AttitudeActual and @ref AttitudeDesired to compute @ref ActuatorDesired - - - - + + + + From 5fa40ac65a25179f707fe113ef51bdea85a58baa Mon Sep 17 00:00:00 2001 From: David Ankers Date: Sat, 27 Apr 2013 17:20:36 +1000 Subject: [PATCH 9/9] Changed yaw back --- shared/uavobjectdefinition/stabilizationsettings.xml | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/shared/uavobjectdefinition/stabilizationsettings.xml b/shared/uavobjectdefinition/stabilizationsettings.xml index bc1ea12e8..74dbeebf9 100644 --- a/shared/uavobjectdefinition/stabilizationsettings.xml +++ b/shared/uavobjectdefinition/stabilizationsettings.xml @@ -4,7 +4,7 @@ - +