From fa5f7a8fdd501a08da7e1068862316e23bbf1aff Mon Sep 17 00:00:00 2001
From: Brian Webb <webbbn@gmail.com>
Date: Sat, 3 Nov 2012 09:18:40 -0700
Subject: [PATCH] RFM22B: All packets are now ACKed, and added a formal
 connection request / accept that will be used or auto-configuring the remote
 modem.

---
 flight/Libraries/inc/packet_handler.h         |  21 +-
 flight/Modules/PipXtreme/pipxtrememod.c       |   9 +-
 .../Modules/RadioComBridge/RadioComBridge.c   |   8 +-
 flight/PiOS/Common/pios_rfm22b.c              | 434 ++++++++++++++----
 flight/PiOS/Common/pios_rfm22b_com.c          |  25 +-
 flight/PiOS/inc/pios_rfm22b.h                 |   4 +-
 flight/PiOS/inc/pios_rfm22b_priv.h            |  56 ++-
 flight/PipXtreme/System/pios_board.c          |   3 +
 8 files changed, 411 insertions(+), 149 deletions(-)

diff --git a/flight/Libraries/inc/packet_handler.h b/flight/Libraries/inc/packet_handler.h
index 789807a04..744a73b97 100644
--- a/flight/Libraries/inc/packet_handler.h
+++ b/flight/Libraries/inc/packet_handler.h
@@ -42,8 +42,6 @@ typedef enum {
 	PACKET_TYPE_NONE = 0,
 	PACKET_TYPE_STATUS,         // broadcasts status of this modem
 	PACKET_TYPE_CON_REQUEST,    // request a connection to another modem
-	PACKET_TYPE_CON_ACCEPT,     // accecpt a connection to another modem
-	PACKET_TYPE_CON_DECLINE,    // decline a connection to another modem
 	PACKET_TYPE_DATA,           // data packet (packet contains user data)
 	PACKET_TYPE_PPM,            // PPM relay values
 	PACKET_TYPE_ACK,
@@ -59,12 +57,19 @@ typedef struct {
 } PHPacketHeader;
 
 #define PH_MAX_DATA (PIOS_PH_MAX_PACKET - sizeof(PHPacketHeader) - RS_ECC_NPARITY)
-#define PH_PACKET_SIZE(p) (p->data + p->header.data_size - (uint8_t*)p + RS_ECC_NPARITY)
+#define PH_PACKET_SIZE(p) ((p)->data + (p)->header.data_size - (uint8_t*)(p) + RS_ECC_NPARITY)
 typedef struct {
 	PHPacketHeader header;
 	uint8_t data[PH_MAX_DATA + RS_ECC_NPARITY];
 } PHPacket, *PHPacketHandle;
 
+typedef struct {
+	PHPacketHeader header;
+	uint16_t seq_num;
+	bool ready_to_send;
+	uint8_t ecc[RS_ECC_NPARITY];
+} PHAckNackPacket, *PHAckNackPacketHandle;
+
 #define PH_PPM_DATA_SIZE(p) ((uint8_t*)((p)->ecc) - (uint8_t*)(((PHPacketHandle)(p))->data))
 typedef struct {
 	PHPacketHeader header;
@@ -83,11 +88,11 @@ typedef struct {
 #define PH_CONNECTION_DATA_SIZE(p) ((uint8_t*)((p)->ecc) - (uint8_t*)(((PHPacketHandle)(p))->data))
 typedef struct {
 	PHPacketHeader header;
-	uint32_t datarate;
-	uint32_t frequencyHz;
-	uint32_t minFrequencyHz;
-	uint32_t maxFrequencyHz;
-	uint8_t maxTxPower;
+	uint8_t datarate;
+	uint32_t frequency_hz;
+	uint32_t min_frequency;
+	uint32_t max_frequency;
+	uint8_t max_tx_power;
 	uint8_t ecc[RS_ECC_NPARITY];
 } PHConnectionPacket, *PHConnectionPacketHandle;
 
diff --git a/flight/Modules/PipXtreme/pipxtrememod.c b/flight/Modules/PipXtreme/pipxtrememod.c
index 2bbc7dc49..fe3a4fb32 100644
--- a/flight/Modules/PipXtreme/pipxtrememod.c
+++ b/flight/Modules/PipXtreme/pipxtrememod.c
@@ -192,16 +192,11 @@ static void systemTask(void *parameters)
 			prev_tx_count = tx_count;
 			prev_rx_count = rx_count;
 		}
-		if (radio_stats.connected)
-		{
-			oplinkStatus.LinkState = OPLINKSTATUS_LINKSTATE_CONNECTED;
+		oplinkStatus.LinkState = radio_stats.link_state;
+		if (radio_stats.link_state == OPLINKSTATUS_LINKSTATE_CONNECTED)
 			LINK_LED_ON;
-		}
 		else
-		{
-			oplinkStatus.LinkState = OPLINKSTATUS_LINKSTATE_DISCONNECTED;
 			LINK_LED_OFF;
-		}
 
 		// Update the object
 		OPLinkStatusSet(&oplinkStatus);
diff --git a/flight/Modules/RadioComBridge/RadioComBridge.c b/flight/Modules/RadioComBridge/RadioComBridge.c
index aff437525..ca3556e83 100644
--- a/flight/Modules/RadioComBridge/RadioComBridge.c
+++ b/flight/Modules/RadioComBridge/RadioComBridge.c
@@ -306,7 +306,7 @@ static int32_t UAVTalkSendHandler(uint8_t *buf, int32_t length)
 	uint32_t outputPort = PIOS_COM_TELEMETRY;
 #if defined(PIOS_INCLUDE_USB)
 	// Determine output port (USB takes priority over telemetry port)
-	if (PIOS_USB_CheckAvailable(0) && PIOS_COM_TELEM_USB)
+	if (PIOS_COM_TELEM_USB && PIOS_COM_Available(PIOS_COM_TELEM_USB))
 		outputPort = PIOS_COM_TELEM_USB;
 #endif /* PIOS_INCLUDE_USB */
 	if(outputPort)
@@ -325,10 +325,12 @@ static int32_t UAVTalkSendHandler(uint8_t *buf, int32_t length)
 static int32_t RadioSendHandler(uint8_t *buf, int32_t length)
 {
 	uint32_t outputPort = PIOS_COM_RADIO;
-	if(outputPort)
+	// Don't send any data unless the radio port is available.
+	if(outputPort && PIOS_COM_Available(outputPort))
 		return PIOS_COM_SendBuffer(outputPort, buf, length);
 	else
-		return -1;
+		// For some reason, if this function returns failure, it prevents saving settings.
+		return length;
 }
 
 static void ProcessInputStream(UAVTalkConnection connectionHandle, uint8_t rxbyte)
diff --git a/flight/PiOS/Common/pios_rfm22b.c b/flight/PiOS/Common/pios_rfm22b.c
index 5fb0dd571..bc5ecf9af 100644
--- a/flight/PiOS/Common/pios_rfm22b.c
+++ b/flight/PiOS/Common/pios_rfm22b.c
@@ -71,6 +71,9 @@
 // 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
 
@@ -159,7 +162,6 @@ static const uint8_t OUT_FF[64] = {0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,
 
 /* Local function forwared declarations */
 static void PIOS_RFM22B_Task(void *parameters);
-static void PIOS_RFM22B_InjectEvent(struct pios_rfm22b_dev *rfm22b_dev, enum pios_rfm22b_event event, bool inISR);
 static bool rfm22_readStatus(struct pios_rfm22b_dev *rfm22b_dev);
 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);
@@ -167,10 +169,14 @@ static enum pios_rfm22b_event rfm22_detectPreamble(struct pios_rfm22b_dev *rfm22
 static enum pios_rfm22b_event rfm22_detectSync(struct pios_rfm22b_dev *rfm22b_dev);
 static enum pios_rfm22b_event rfm22_rxData(struct pios_rfm22b_dev *rfm22b_dev);
 static enum pios_rfm22b_event rfm22_receiveStatus(struct pios_rfm22b_dev *rfm22b_dev);
+static enum pios_rfm22b_event rfm22_receiveAck(struct pios_rfm22b_dev *rfm22b_dev);
+static enum pios_rfm22b_event rfm22_receiveNack(struct pios_rfm22b_dev *rfm22b_dev);
+static enum pios_rfm22b_event rfm22_sendAck(struct pios_rfm22b_dev *rfm22b_dev);
+static enum pios_rfm22b_event rfm22_sendNack(struct pios_rfm22b_dev *rfm22b_dev);
 static enum pios_rfm22b_event rfm22_requestConnection(struct pios_rfm22b_dev *rfm22b_dev);
+static enum pios_rfm22b_event rfm22_initConnection(struct pios_rfm22b_dev *rfm22b_dev);
 static enum pios_rfm22b_event rfm22_acceptConnection(struct pios_rfm22b_dev *rfm22b_dev);
 static enum pios_rfm22b_event rfm22_connectionAccepted(struct pios_rfm22b_dev *rfm22b_dev);
-static enum pios_rfm22b_event rfm22_connectionDeclined(struct pios_rfm22b_dev *rfm22b_dev);
 static enum pios_rfm22b_event rfm22_txStart(struct pios_rfm22b_dev *rfm22b_dev);
 static enum pios_rfm22b_event rfm22_txData(struct pios_rfm22b_dev *rfm22b_dev);
 static enum pios_rfm22b_event rfm22_process_state_transition(struct pios_rfm22b_dev *rfm22b_dev, enum pios_rfm22b_event event);
@@ -196,6 +202,8 @@ static uint8_t rfm22_read_noclaim(uint8_t addr);
 
 /* Te 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 = {
@@ -206,27 +214,40 @@ const static struct pios_rfm22b_transition rfm22b_transitions[RFM22B_STATE_NUM_S
 	[RFM22B_STATE_INITIALIZING] = {
 		.entry_fn = rfm22_init,
 		.next_state = {
-			//[RFM22B_EVENT_INITIALIZED] = RFM22B_STATE_REQUESTING_CONNECTION,
-			[RFM22B_EVENT_INITIALIZED] = RFM22B_STATE_TX_START,
+			[RFM22B_EVENT_INITIALIZED] = RFM22B_STATE_INITIATING_CONNECTION,
+			[RFM22B_EVENT_INITIALIZE] = RFM22B_STATE_INITIALIZING,
 			[RFM22B_EVENT_ERROR] = RFM22B_STATE_ERROR,
 			[RFM22B_EVENT_FATAL_ERROR] = RFM22B_STATE_FATAL_ERROR,
 		},
 	},
+	[RFM22B_STATE_INITIATING_CONNECTION] = {
+		.entry_fn = rfm22_initConnection,
+		.next_state = {
+			[RFM22B_EVENT_REQUEST_CONNECTION] = RFM22B_STATE_REQUESTING_CONNECTION,
+			[RFM22B_EVENT_WAIT_FOR_CONNECTION] = 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_REQUESTING_CONNECTION] = {
 		.entry_fn = rfm22_requestConnection,
 		.next_state = {
-			[RFM22B_EVENT_REQUESTED_CONNECTION] = RFM22B_STATE_TX_START,
+			[RFM22B_EVENT_START_TRANSFER] = 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_RX_COMPLETE] = RFM22B_STATE_TX_START,
+			[RFM22B_EVENT_CONNECTION_ACCEPTED] = 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,
 		},
 	},
@@ -236,38 +257,35 @@ const static struct pios_rfm22b_transition rfm22b_transitions[RFM22B_STATE_NUM_S
 			[RFM22B_EVENT_RX_COMPLETE] = 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_CONNECTION_DECLINED] = {
-		.entry_fn = rfm22_connectionDeclined,
-		.next_state = {
-			[RFM22B_EVENT_RX_COMPLETE] = RFM22B_STATE_TX_START,
-			[RFM22B_EVENT_TIMEOUT] = RFM22B_STATE_TIMEOUT,
-			[RFM22B_EVENT_ERROR] = RFM22B_STATE_ERROR,
-			[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_SEND_PACKET] = RFM22B_STATE_TX_START,
-			[RFM22B_EVENT_TX_START] = RFM22B_STATE_TX_START,
+			[RFM22B_EVENT_START_TRANSFER] = RFM22B_STATE_TX_START,
+			[RFM22B_EVENT_PACKET_NACKED] = RFM22B_STATE_RECEIVING_NACK,
 			[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_INT_RECEIVED] = RFM22B_STATE_WAIT_PREAMBLE,
 			[RFM22B_EVENT_PREAMBLE_DETECTED] = RFM22B_STATE_WAIT_SYNC,
+			[RFM22B_EVENT_INT_RECEIVED] = RFM22B_STATE_WAIT_PREAMBLE,
 			[RFM22B_EVENT_SEND_PACKET] = RFM22B_STATE_TX_START,
-			[RFM22B_EVENT_TX_START] = RFM22B_STATE_TX_START,
+			[RFM22B_EVENT_START_TRANSFER] = RFM22B_STATE_TX_START,
+			[RFM22B_EVENT_PACKET_NACKED] = RFM22B_STATE_RECEIVING_NACK,
 			[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,
 		},
 	},
@@ -276,9 +294,9 @@ const static struct pios_rfm22b_transition rfm22b_transitions[RFM22B_STATE_NUM_S
 		.next_state = {
 			[RFM22B_EVENT_INT_RECEIVED] = RFM22B_STATE_WAIT_SYNC,
 			[RFM22B_EVENT_SYNC_DETECTED] = RFM22B_STATE_RX_DATA,
-			[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,
 		},
 	},
@@ -286,22 +304,46 @@ const static struct pios_rfm22b_transition rfm22b_transitions[RFM22B_STATE_NUM_S
 		.entry_fn = rfm22_rxData,
 		.next_state = {
 			[RFM22B_EVENT_INT_RECEIVED] = RFM22B_STATE_RX_DATA,
-			[RFM22B_EVENT_RX_COMPLETE] = RFM22B_STATE_TX_START,
+			[RFM22B_EVENT_RX_COMPLETE] = RFM22B_STATE_SENDING_ACK,
 			[RFM22B_EVENT_STATUS_RECEIVED] = RFM22B_STATE_RECEIVING_STATUS,
-			[RFM22B_EVENT_CONNECTION_REQUEST] = RFM22B_STATE_ACCEPTING_CONNECTION,
-			[RFM22B_EVENT_CONNECTION_ACCEPT] = RFM22B_STATE_CONNECTION_ACCEPTED,
-			[RFM22B_EVENT_CONNECTION_DECLINED] = RFM22B_STATE_CONNECTION_DECLINED,
+			[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_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_CONNECTION_ACCEPTED] = RFM22B_STATE_CONNECTION_ACCEPTED,
+			[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_START_TRANSFER] = 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_TX_START,
+			[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,
 		},
 	},
@@ -312,6 +354,7 @@ const static struct pios_rfm22b_transition rfm22b_transitions[RFM22B_STATE_NUM_S
 			[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,
 		},
 	},
@@ -319,18 +362,41 @@ const static struct pios_rfm22b_transition rfm22b_transitions[RFM22B_STATE_NUM_S
 		.entry_fn = rfm22_txData,
 		.next_state = {
 			[RFM22B_EVENT_INT_RECEIVED] = RFM22B_STATE_TX_DATA,
-			[RFM22B_EVENT_TX_COMPLETE] = RFM22B_STATE_RX_MODE,
+			[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_SENDING_ACK] = {
+		.entry_fn = rfm22_sendAck,
+		.next_state = {
+			[RFM22B_EVENT_START_TRANSFER] = RFM22B_STATE_TX_START,
+			[RFM22B_EVENT_INITIALIZE] = RFM22B_STATE_INITIALIZING,
+			[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_START_TRANSFER] = RFM22B_STATE_TX_START,
+			[RFM22B_EVENT_INITIALIZE] = RFM22B_STATE_INITIALIZING,
+			[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_START_TRANSFER] = RFM22B_STATE_TX_START,
 			[RFM22B_EVENT_INITIALIZE] = RFM22B_STATE_INITIALIZING,
 			[RFM22B_EVENT_ERROR] = RFM22B_STATE_ERROR,
+			[RFM22B_EVENT_INITIALIZE] = RFM22B_STATE_INITIALIZING,
 			[RFM22B_EVENT_FATAL_ERROR] = RFM22B_STATE_FATAL_ERROR,
 		},
 	},
@@ -339,14 +405,13 @@ const static struct pios_rfm22b_transition rfm22b_transitions[RFM22B_STATE_NUM_S
 		.next_state = {
 			[RFM22B_EVENT_INITIALIZE] = RFM22B_STATE_INITIALIZING,
 			[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_EVENT_ERROR] = RFM22B_STATE_ERROR,
-			[RFM22B_EVENT_FATAL_ERROR] = RFM22B_STATE_FATAL_ERROR,
 		},
 	},
 };
@@ -411,6 +476,12 @@ static const uint8_t ss_reg_70[] = {  0x24, 0x2D}; // rfm22_modulation_mode_cont
 static const uint8_t ss_reg_71[] = {  0x2B, 0x23}; // rfm22_modulation_mode_control2
 
 
+static inline uint32_t timeDifferenceMs(portTickType start_time, portTickType end_time) {
+	if(end_time > start_time)
+		return (end_time - start_time) * portTICK_RATE_MS;
+	return ((((portTICK_RATE_MS) -1) - 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);
@@ -498,12 +569,18 @@ int32_t PIOS_RFM22B_Init(uint32_t *rfm22b_id, uint32_t spi_id, uint32_t slave_nu
 	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.link_state = OPLINKSTATUS_LINKSTATE_DISCONNECTED;
 	rfm22b_dev->tx_power = RFM22B_DEFAULT_TX_POWER;
 	rfm22b_dev->destination_id = 0xffffffff;
+	rfm22b_dev->coordinator = false;
+	rfm22b_dev->send_status = false;
+	rfm22b_dev->send_ppm = false;
+	rfm22b_dev->send_connection_request = false;
 
 	// Bind the configuration to the device instance
 	rfm22b_dev->cfg = *cfg;
@@ -512,6 +589,7 @@ int32_t PIOS_RFM22B_Init(uint32_t *rfm22b_id, uint32_t spi_id, uint32_t slave_nu
 	// Initialize the packets.
 	rfm22b_dev->rx_packet_len = 0;
 	rfm22b_dev->tx_packet = NULL;
+	rfm22b_dev->prev_tx_packet = NULL;
 	rfm22b_dev->tx_seq = 0;
 
 	*rfm22b_id = (uint32_t)rfm22b_dev;
@@ -577,7 +655,7 @@ bool PIOS_RFM22_EXT_Int(void)
  * \param[in] event The event to inject
  * \param[in] inISR Is this being called from an interrrup service routine?
  */
-static void PIOS_RFM22B_InjectEvent(struct pios_rfm22b_dev *rfm22b_dev, enum pios_rfm22b_event event, bool inISR)
+void PIOS_RFM22B_InjectEvent(struct pios_rfm22b_dev *rfm22b_dev, enum pios_rfm22b_event event, bool inISR)
 {
 
 	// Store the event.
@@ -663,6 +741,22 @@ void PIOS_RFM22B_SetDestinationId(uint32_t rfm22b_id, uint32_t dest_id)
 	rfm22b_dev->pair_stats[0].lastContact = 0;
 }
 
+/**
+ * Configures the radio as a coordinator or not.
+ * \param[in] rfm22b_id The RFM22B device index.
+ * \param[in] coordinator Sets as coordinator if true.
+ */
+void PIOS_RFM22B_SetCoordinator(uint32_t rfm22b_id, bool coordinator)
+{
+	struct pios_rfm22b_dev *rfm22b_dev = (struct pios_rfm22b_dev *)rfm22b_id;
+	if (!PIOS_RFM22B_validate(rfm22b_dev))
+		return;
+	rfm22b_dev->coordinator = coordinator;
+
+	// Re-initialize the radio device.
+	PIOS_RFM22B_InjectEvent(rfm22b_dev, RFM22B_EVENT_INITIALIZE, false);
+}
+
 /**
  * Returns the device statistics RFM22B device.
  * \param[in] rfm22b_id The RFM22B device index.
@@ -677,7 +771,6 @@ void PIOS_RFM22B_GetStats(uint32_t rfm22b_id, struct rfm22b_stats *stats) {
 	rfm22_calculateLinkQuality(rfm22b_dev);
 
 	// We are connected if our destination ID is in the pair stats.
-	rfm22b_dev->stats.connected = false;
 	if (rfm22b_dev->destination_id != 0xffffffff)
 		for (uint8_t i = 0; i < OPLINKSTATUS_PAIRIDS_NUMELEM; ++i)
 		{
@@ -686,10 +779,10 @@ void PIOS_RFM22B_GetStats(uint32_t rfm22b_id, struct rfm22b_stats *stats) {
 			{
 				rfm22b_dev->stats.rssi = rfm22b_dev->pair_stats[i].rssi;
 				rfm22b_dev->stats.afc_correction = rfm22b_dev->pair_stats[i].afc_correction;
-				rfm22b_dev->stats.connected = true;
 				break;
 			}
 		}
+	rfm22b_dev->stats.tx_resent = rfm22b_dev->state;
 	*stats = rfm22b_dev->stats;
 }
 
@@ -752,7 +845,7 @@ 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 rfm22b_dev->stats.connected  && (rfm22b_dev->stats.link_quality > RFM22B_LINK_QUALITY_THRESHOLD);
+	return (rfm22b_dev->stats.link_state == OPLINKSTATUS_LINKSTATE_CONNECTED) && (rfm22b_dev->stats.link_quality > RFM22B_LINK_QUALITY_THRESHOLD);
 }
 
 /**
@@ -766,6 +859,7 @@ static void PIOS_RFM22B_Task(void *parameters)
 	portTickType lastEventTicks = xTaskGetTickCount();
 	portTickType lastStatusTicks = lastEventTicks;
 	portTickType lastPPMTicks = lastEventTicks;
+	portTickType lastConnectTicks = lastEventTicks;
 
 	while(1)
 	{
@@ -792,10 +886,7 @@ static void PIOS_RFM22B_Task(void *parameters)
 		{
 			// Has it been too long since the last event?
 			portTickType curTicks = xTaskGetTickCount();
-			if (curTicks < lastEventTicks)
-				lastEventTicks = curTicks;
-			portTickType ticksSinceEvent = curTicks - lastEventTicks;
-			if ((ticksSinceEvent / portTICK_RATE_MS) > PIOS_RFM22B_SUPERVISOR_TIMEOUT)
+			if (timeDifferenceMs(lastEventTicks, curTicks) > PIOS_RFM22B_SUPERVISOR_TIMEOUT)
 			{
  				// Transsition through an error event.
 				rfm22_process_event(rfm22b_dev, RFM22B_EVENT_ERROR);
@@ -806,40 +897,33 @@ static void PIOS_RFM22B_Task(void *parameters)
 					;
 				lastEventTicks = xTaskGetTickCount();
 			}
-			else if (rfm22b_dev->state == RFM22B_STATE_RX_MODE)
-				// Try to start a transmission
-				rfm22_process_event(rfm22b_dev, RFM22B_EVENT_TX_START);
 		}
 
-		// Have we locked up sending / receiving a packet?
-		if (rfm22b_dev->packet_start_ticks > 0)
-		{
-			portTickType cur_ticks = xTaskGetTickCount();
-
-			// Did the clock wrap around?
-			if (cur_ticks < rfm22b_dev->packet_start_ticks)
-				rfm22b_dev->packet_start_ticks = (cur_ticks > 0) ? cur_ticks : 1;
-
-			// Have we been sending this packet too long?
-			if (((cur_ticks - rfm22b_dev->packet_start_ticks) / portTICK_RATE_MS) > (rfm22b_dev->max_packet_time * 3))
-				rfm22_process_event(rfm22b_dev, RFM22B_EVENT_TIMEOUT);
-		}
-
-		// Queue up a ppm packet if it's time.
 		portTickType curTicks = xTaskGetTickCount();
-		// Rollover
-		if (curTicks < lastPPMTicks)
-			lastPPMTicks = curTicks;
-		if (((curTicks - lastPPMTicks) / portTICK_RATE_MS) > PPM_UPDATE_PERIOD_MS)
-			if (rfm22_sendPPM(rfm22b_dev))
+		// 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);
+
+		// Queue up a connection request packet if it's necessary
+		else if (rfm22b_dev->coordinator && (rfm22b_dev->stats.link_state != OPLINKSTATUS_LINKSTATE_CONNECTED) && (timeDifferenceMs(lastConnectTicks, curTicks) > CONNECT_ATTEMPT_PERIOD_MS))
+		{
+			lastConnectTicks = curTicks;
+			rfm22_process_event(rfm22b_dev, RFM22B_EVENT_INITIALIZE);
+		}
+		else
+		{
+			// Queue up a PPM packet if it's time.
+			if ((timeDifferenceMs(lastPPMTicks, curTicks) > PPM_UPDATE_PERIOD_MS) && rfm22_sendPPM(rfm22b_dev))
 				lastPPMTicks = curTicks;
-		// Rollover
-		if (curTicks < lastStatusTicks)
-			lastStatusTicks = curTicks;
-		// Queue up a status packet if it's time.
-		if (((curTicks - lastStatusTicks) / portTICK_RATE_MS) > RADIOSTATS_UPDATE_PERIOD_MS)
-			if (rfm22_sendStatus(rfm22b_dev))
+
+			// Queue up a status packet if it's time.
+			if ((timeDifferenceMs(lastStatusTicks, curTicks) > RADIOSTATS_UPDATE_PERIOD_MS) && rfm22_sendStatus(rfm22b_dev))
 				lastStatusTicks = curTicks;
+
+			// Can we kick of a transmit?
+			if (rfm22b_dev->prev_tx_packet && timeDifferenceMs(rfm22b_dev->tx_complete_ticks, curTicks) > rfm22b_dev->max_ack_delay)
+				rfm22_process_event(rfm22b_dev, RFM22B_EVENT_PACKET_NACKED);
+		}
 	}
 }
 
@@ -865,6 +949,7 @@ void RFM22_SetDatarate(uint32_t rfm22b_id, enum rfm22b_datarate datarate, bool d
 	uint32_t datarate_bps = data_rate[datarate];
 	rfm22b_dev->datarate = datarate;
 	rfm22b_dev->max_packet_time = (uint16_t)((float)(PIOS_PH_MAX_PACKET * 8 * 1000) / (float)(datarate_bps) + 0.5);
+	rfm22b_dev->max_ack_delay = 25;
 
 	// rfm22_if_filter_bandwidth
 	rfm22_write(0x1C, reg_1C[datarate]);
@@ -1206,14 +1291,45 @@ static enum pios_rfm22b_event rfm22_setRxMode(struct pios_rfm22b_dev *rfm22b_dev
 
 static enum pios_rfm22b_event rfm22_txStart(struct pios_rfm22b_dev *rfm22b_dev)
 {
-	// See if there's a packet on the packet queue.
-	PHPacketHandle p;
-	if (xQueueReceive(rfm22b_dev->packetQueue, &p, 0) != pdTRUE)
+	PHPacketHandle p = NULL;
+
+	// See if there's a packet ready to send.
+	if (rfm22b_dev->tx_packet)
+		p = rfm22b_dev->tx_packet;
+
+	// Are we waiting for an ACK?
+	else
 	{
-		// Clear the TX buffer.
-		rfm22b_dev->tx_data_rd = rfm22b_dev->tx_data_wr = 0;
-		return RFM22B_EVENT_RX_MODE;
+
+		// Don't send a packet if we're waiting for an ACK
+		if (rfm22b_dev->prev_tx_packet)
+			return RFM22B_EVENT_RX_MODE;
+
+		// Is there a packet in the queue to send?
+		if (xQueueReceive(rfm22b_dev->packetQueue, &p, 0) != pdTRUE)
+			p = NULL;
+
+		if (!p)
+		{
+
+			// Try to get some data to send
+			bool need_yield = false;
+			p = &rfm22b_dev->data_packet;
+			p->header.type = PACKET_TYPE_DATA;
+			p->header.destination_id = rfm22b_dev->destination_id;
+			p->header.data_size = (rfm22b_dev->tx_out_cb)(rfm22b_dev->tx_out_context, p->data,
+								      PH_MAX_DATA, NULL, &need_yield);
+			if (p->header.data_size == 0)
+				p = NULL;
+
+			// Don't send any data until we're connected.
+			else if (rfm22b_dev->stats.link_state != OPLINKSTATUS_LINKSTATE_CONNECTED)
+				return RFM22B_EVENT_RX_MODE;
+		}
+
 	}
+	if (!p)
+		return RFM22B_EVENT_RX_MODE;
 
 	// Add the error correcting code.
 	p->header.source_id = rfm22b_dev->deviceID;
@@ -1288,22 +1404,27 @@ static enum pios_rfm22b_event rfm22_txStart(struct pios_rfm22b_dev *rfm22b_dev)
 
 static bool rfm22_sendStatus(struct pios_rfm22b_dev *rfm22b_dev)
 {
-	PHPacketHandle sph = (PHPacketHandle)&(rfm22b_dev->status_packet);
+	// We don't send status if we're a coordinator
+	if (rfm22b_dev->coordinator)
+		return true;
 
 	// Update the link quality metric.
 	rfm22_calculateLinkQuality(rfm22b_dev);
 
 	// Queue the status message
-	rfm22b_dev->status_packet.header.destination_id = 0xffffffff; // Broadcast
+	PHPacketHandle sph = (PHPacketHandle)&(rfm22b_dev->status_packet);
+	if (rfm22b_dev->stats.link_state == OPLINKSTATUS_LINKSTATE_CONNECTED)
+		rfm22b_dev->status_packet.header.destination_id = rfm22b_dev->destination_id;
+	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.link_quality = rfm22b_dev->stats.link_quality;
 	rfm22b_dev->status_packet.received_rssi = rfm22b_dev->rssi_dBm;
+	//rfm22b_dev->send_status = true;
 	if (xQueueSend(rfm22b_dev->packetQueue, &sph, 0) != pdTRUE)
 		return false;
-
-	// Process a SEND_PACKT event.
-	rfm22_process_event(rfm22b_dev, RFM22B_EVENT_SEND_PACKET);
+	rfm22_process_event(rfm22b_dev, RFM22B_EVENT_START_TRANSFER);
 
 	return true;
 }
@@ -1444,7 +1565,7 @@ static enum pios_rfm22b_event rfm22_detectSync(struct pios_rfm22b_dev *rfm22b_de
 	else if (rfm22b_dev->int_status2 & !RFM22_is2_ipreaval)
 	{
 		// Waiting for sync timed out.
-		return RFM22B_EVENT_TX_START;
+		return RFM22B_EVENT_START_TRANSFER;
 	}
 
 	return RFM22B_EVENT_NUM_EVENTS;
@@ -1573,10 +1694,7 @@ static enum pios_rfm22b_event rfm22_rxData(struct pios_rfm22b_dev *rfm22b_dev)
 					ret_event = RFM22B_EVENT_STATUS_RECEIVED;
 					break;
 				case PACKET_TYPE_CON_REQUEST:
-					ret_event = RFM22B_EVENT_CONNECTION_REQUEST;
-					break;
-				case PACKET_TYPE_CON_ACCEPT:
-					ret_event = RFM22B_EVENT_CONNECTION_ACCEPT;
+					ret_event = RFM22B_EVENT_CONNECTION_REQUESTED;
 					break;
 				case PACKET_TYPE_DATA:
 				{
@@ -1585,6 +1703,12 @@ static enum pios_rfm22b_event rfm22_rxData(struct pios_rfm22b_dev *rfm22b_dev)
 					(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);
 					break;
 				}
+				case PACKET_TYPE_ACK:
+					ret_event = RFM22B_EVENT_PACKET_ACKED;
+					break;
+				case PACKET_TYPE_NACK:
+					ret_event = RFM22B_EVENT_PACKET_NACKED;
+					break;
 				default:
 					break;
 				}
@@ -1602,6 +1726,7 @@ static enum pios_rfm22b_event rfm22_rxData(struct pios_rfm22b_dev *rfm22b_dev)
 
 static enum pios_rfm22b_event rfm22_txData(struct pios_rfm22b_dev *rfm22b_dev)
 {
+	enum pios_rfm22b_event ret_event = RFM22B_EVENT_NUM_EVENTS;
 
 	// Read the device status registers
 	if (!rfm22_readStatus(rfm22b_dev))
@@ -1645,16 +1770,93 @@ static enum pios_rfm22b_event rfm22_txData(struct pios_rfm22b_dev *rfm22b_dev)
 	{
 		rfm22b_dev->stats.tx_count++;
 		rfm22b_dev->stats.tx_byte_count += PH_PACKET_SIZE(rfm22b_dev->tx_packet);
-		// Free the tx packet
-		PHReleaseTXPacket(pios_packet_handler, rfm22b_dev->tx_packet);
+		ret_event = (rfm22b_dev->tx_packet->header.type == PACKET_TYPE_ACK) ? RFM22B_EVENT_TX_START : RFM22B_EVENT_RX_MODE;
+
+		// Do we require an ACK?
+		if ((rfm22b_dev->tx_packet->header.type != PACKET_TYPE_ACK) && (rfm22b_dev->tx_packet->header.type != PACKET_TYPE_NACK))
+		{
+			rfm22b_dev->prev_tx_packet = rfm22b_dev->tx_packet;
+			rfm22b_dev->tx_complete_ticks = xTaskGetTickCount();
+		}
+		else
+			// Free the tx packet
+			PHReleaseTXPacket(pios_packet_handler, rfm22b_dev->tx_packet);
+
 		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;
-		return RFM22B_EVENT_TX_COMPLETE;
 	}
 
-	return RFM22B_EVENT_NUM_EVENTS;
+	return ret_event;
+}
+
+/**
+ * Send an ACK to a received packet.
+ * \param[in] rfm22b_dev  The device structure
+ */
+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->rx_packet.header.source_id;
+	aph->header.type = PACKET_TYPE_ACK;
+	aph->header.data_size = 0;
+	aph->seq_num = rfm22b_dev->rx_packet.header.seq_num;
+	//aph->ready_to_send = (uxQueueMessagesWaiting(rfm22b_dev->packetQueue) > 0);
+	aph->ready_to_send = false;
+	rfm22b_dev->tx_packet = (PHPacketHandle)aph;
+	return RFM22B_EVENT_START_TRANSFER;
+}
+
+/**
+ * Send an NACK to a received packet.
+ * \param[in] rfm22b_dev  The device structure
+ */
+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->rx_packet.header.source_id;
+	aph->header.type = PACKET_TYPE_NACK;
+	aph->header.data_size = 0;
+	aph->seq_num = rfm22b_dev->rx_packet.header.seq_num;
+	rfm22b_dev->tx_packet = (PHPacketHandle)aph;
+	return RFM22B_EVENT_START_TRANSFER;
+}
+
+/**
+ * Receive an ACK.
+ * \param[in] rfm22b_dev  The device structure
+ */
+static enum pios_rfm22b_event rfm22_receiveAck(struct pios_rfm22b_dev *rfm22b_dev)
+{
+	PHPacketHandle prev = rfm22b_dev->prev_tx_packet;
+	PHAckNackPacketHandle aph = (PHAckNackPacketHandle)&(rfm22b_dev->rx_packet);
+
+	// Clear the previous TX packet.
+	rfm22b_dev->prev_tx_packet = NULL;
+
+	// Was this a connection request?
+	if (prev->header.type == PACKET_TYPE_CON_REQUEST)
+		return RFM22B_EVENT_CONNECTION_ACCEPTED;
+
+	// Should we try to start another TX?
+	if (aph->ready_to_send)
+		return RFM22B_EVENT_RX_MODE;
+	else
+		return RFM22B_EVENT_TX_START;
+}
+
+/**
+ * Receive an MACK.
+ * \param[in] rfm22b_dev  The device structure
+ */
+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;
+	//rfm22b_dev->stats.tx_resent++;
+	return RFM22B_EVENT_START_TRANSFER;
 }
 
 /**
@@ -1713,24 +1915,70 @@ static enum pios_rfm22b_event rfm22_receiveStatus(struct pios_rfm22b_dev *rfm22b
 	return RFM22B_EVENT_RX_COMPLETE;
 }
 
+static enum pios_rfm22b_event rfm22_initConnection(struct pios_rfm22b_dev *rfm22b_dev)
+{
+	if (rfm22b_dev->coordinator)
+		return RFM22B_EVENT_REQUEST_CONNECTION;
+	else
+		return RFM22B_EVENT_WAIT_FOR_CONNECTION;
+}
+
 static enum pios_rfm22b_event rfm22_requestConnection(struct pios_rfm22b_dev *rfm22b_dev)
 {
-	// Generate and set a connection request message.
-	return RFM22B_EVENT_RX_COMPLETE;
+	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
+	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->datarate = rfm22b_dev->datarate;
+	cph->frequency_hz = rfm22b_dev->frequency_hz;
+	cph->min_frequency = rfm22b_dev->min_frequency;
+	cph->max_frequency = rfm22b_dev->max_frequency;
+	cph->max_tx_power = rfm22b_dev->tx_power;
+	rfm22b_dev->tx_packet = (PHPacketHandle)cph;
+
+	return RFM22B_EVENT_START_TRANSFER;
 }
 
 static enum pios_rfm22b_event rfm22_acceptConnection(struct pios_rfm22b_dev *rfm22b_dev)
 {
-	return RFM22B_EVENT_RX_COMPLETE;
+	// 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));
+
+	// Configure this modem from the connection request message.
+	PIOS_RFM22B_SetDestinationId((uint32_t)rfm22b_dev, cph->header.source_id);
+	RFM22_SetDatarate((uint32_t)rfm22b_dev, cph->datarate, true);
+	PIOS_RFM22B_SetTxPower((uint32_t)rfm22b_dev, cph->max_tx_power);
+	rfm22b_dev->min_frequency = cph->min_frequency;
+	rfm22b_dev->max_frequency = cph->max_frequency;
+	rfm22_setNominalCarrierFrequency(rfm22b_dev, cph->frequency_hz);
+
+	return RFM22B_EVENT_CONNECTION_ACCEPTED;
 }
 
 static enum pios_rfm22b_event rfm22_connectionAccepted(struct pios_rfm22b_dev *rfm22b_dev)
 {
-	return RFM22B_EVENT_RX_COMPLETE;
-}
+	PHConnectionPacketHandle cph = (PHConnectionPacketHandle)(&(rfm22b_dev->con_packet));
+
+	// Set our connection state to connected
+	rfm22b_dev->stats.link_state = OPLINKSTATUS_LINKSTATE_CONNECTED;
+
+	// Configure this modem from the connection request message.
+	RFM22_SetDatarate((uint32_t)rfm22b_dev, cph->datarate, true);
+	PIOS_RFM22B_SetTxPower((uint32_t)rfm22b_dev, cph->max_tx_power);
+	rfm22b_dev->min_frequency = cph->min_frequency;
+	rfm22b_dev->max_frequency = cph->max_frequency;
+	rfm22_setNominalCarrierFrequency(rfm22b_dev, cph->frequency_hz);
 
-static enum pios_rfm22b_event rfm22_connectionDeclined(struct pios_rfm22b_dev *rfm22b_dev)
-{
 	return RFM22B_EVENT_RX_COMPLETE;
 }
 
@@ -1997,7 +2245,7 @@ static enum pios_rfm22b_event rfm22_timeout(struct pios_rfm22b_dev *rfm22b_dev)
 		rfm22b_dev->tx_data_rd = rfm22b_dev->tx_data_wr = 0;
 	}
 	rfm22b_dev->rx_buffer_wr = 0;
-	return RFM22B_EVENT_TX_START;
+	return RFM22B_EVENT_START_TRANSFER;
 }
 
 static enum pios_rfm22b_event rfm22_error(struct pios_rfm22b_dev *rfm22b_dev)
diff --git a/flight/PiOS/Common/pios_rfm22b_com.c b/flight/PiOS/Common/pios_rfm22b_com.c
index 478376c17..2833c73f1 100644
--- a/flight/PiOS/Common/pios_rfm22b_com.c
+++ b/flight/PiOS/Common/pios_rfm22b_com.c
@@ -89,29 +89,8 @@ static void PIOS_RFM22B_COM_TxStart(uint32_t rfm22b_id, uint16_t tx_bytes_avail)
 	if (!PIOS_RFM22B_validate(rfm22b_dev))
 		return;
 
-	// Get a packet when we see the sync
-	PHPacketHandle p = PHGetTXPacket(pios_packet_handler);
-	if (!p)
-		return;
-
-	// Get some data to send
-	bool need_yield = false;
-	p->header.type = PACKET_TYPE_DATA;
-	p->header.destination_id = rfm22b_dev->destination_id;
-	p->header.data_size = (rfm22b_dev->tx_out_cb)(rfm22b_dev->tx_out_context, p->data,
-						      PH_MAX_DATA, NULL, &need_yield);
-	if (p->header.data_size == 0)
-	{
-		PHReleaseTXPacket(pios_packet_handler, p);
-		return;
-	}
-
-	// Send the data packet.
-	if (!PIOS_RFM22B_Send_Packet(rfm22b_id, p, 5))
-	{
-		rfm22b_dev->stats.tx_dropped++;
-		PHReleaseTXPacket(pios_packet_handler, p);
-	}
+	// Send a signal to the radio to start a transmit.
+	PIOS_RFM22B_InjectEvent(rfm22b_dev, RFM22B_EVENT_SEND_PACKET, false);
 }
 
 static void PIOS_RFM22B_COM_RegisterRxCallback(uint32_t rfm22b_id, pios_com_callback rx_in_cb, uint32_t context)
diff --git a/flight/PiOS/inc/pios_rfm22b.h b/flight/PiOS/inc/pios_rfm22b.h
index 51745dea9..20b617f54 100644
--- a/flight/PiOS/inc/pios_rfm22b.h
+++ b/flight/PiOS/inc/pios_rfm22b.h
@@ -83,12 +83,13 @@ struct rfm22b_stats {
 	uint8_t rx_error;
 	uint8_t rx_missed;
 	uint8_t tx_dropped;
+	uint8_t tx_resent;
 	uint8_t resets;
 	uint8_t timeouts;
 	uint8_t link_quality;
 	int8_t rssi;
 	int8_t afc_correction;
-	bool connected;
+	uint8_t link_state;
 };
 
 /* Public Functions */
@@ -97,6 +98,7 @@ extern void PIOS_RFM22B_SetFrequencyRange(uint32_t rfm22b_id, uint32_t min_frequ
 extern void PIOS_RFM22B_SetTxPower(uint32_t rfm22b_id, enum rfm22b_tx_power tx_pwr);
 extern void RFM22_SetDatarate(uint32_t rfm22b_id, enum rfm22b_datarate datarate, bool data_whitening);
 extern void PIOS_RFM22B_SetDestinationId(uint32_t rfm22b_id, uint32_t dest_id);
+extern void PIOS_RFM22B_SetCoordinator(uint32_t rfm22b_id, bool coordinator);
 extern uint32_t PIOS_RFM22B_DeviceID(uint32_t rfb22b_id);
 extern void PIOS_RFM22B_GetStats(uint32_t rfm22b_id, struct rfm22b_stats *stats);
 extern bool PIOS_RFM22B_Send_Packet(uint32_t rfm22b_id, PHPacketHandle p, uint32_t max_delay);
diff --git a/flight/PiOS/inc/pios_rfm22b_priv.h b/flight/PiOS/inc/pios_rfm22b_priv.h
index 9aeb86845..06f22a576 100644
--- a/flight/PiOS/inc/pios_rfm22b_priv.h
+++ b/flight/PiOS/inc/pios_rfm22b_priv.h
@@ -578,10 +578,11 @@ enum pios_rfm22b_state {
 	RFM22B_STATE_UNINITIALIZED,
 	RFM22B_STATE_INITIALIZING,
 	RFM22B_STATE_INITIALIZED,
+	RFM22B_STATE_INITIATING_CONNECTION,
+	RFM22B_STATE_WAIT_FOR_CONNECTION,
 	RFM22B_STATE_REQUESTING_CONNECTION,
 	RFM22B_STATE_ACCEPTING_CONNECTION,
 	RFM22B_STATE_CONNECTION_ACCEPTED,
-	RFM22B_STATE_CONNECTION_DECLINED,
 	RFM22B_STATE_RX_MODE,
 	RFM22B_STATE_WAIT_PREAMBLE,
 	RFM22B_STATE_WAIT_SYNC,
@@ -589,6 +590,10 @@ enum pios_rfm22b_state {
 	RFM22B_STATE_RECEIVING_STATUS,
 	RFM22B_STATE_TX_START,
 	RFM22B_STATE_TX_DATA,
+	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,
@@ -601,16 +606,18 @@ enum pios_rfm22b_event {
 	RFM22B_EVENT_INITIALIZE,
 	RFM22B_EVENT_INITIALIZED,
 	RFM22B_EVENT_REQUEST_CONNECTION,
-	RFM22B_EVENT_REQUESTED_CONNECTION,
-	RFM22B_EVENT_CONNECTION_REQUEST,
-	RFM22B_EVENT_CONNECTION_ACCEPT,
-	RFM22B_EVENT_CONNECTION_DECLINED,
+	RFM22B_EVENT_WAIT_FOR_CONNECTION,
+	RFM22B_EVENT_CONNECTION_REQUESTED,
+	RFM22B_EVENT_CONNECTION_ACCEPTED,
+	RFM22B_EVENT_PACKET_ACKED,
+	RFM22B_EVENT_PACKET_NACKED,
 	RFM22B_EVENT_RX_MODE,
 	RFM22B_EVENT_PREAMBLE_DETECTED,
 	RFM22B_EVENT_SYNC_DETECTED,
 	RFM22B_EVENT_RX_COMPLETE,
 	RFM22B_EVENT_STATUS_RECEIVED,
 	RFM22B_EVENT_SEND_PACKET,
+	RFM22B_EVENT_START_TRANSFER,
 	RFM22B_EVENT_TX_START,
 	RFM22B_EVENT_TX_STARTED,
 	RFM22B_EVENT_TX_COMPLETE,
@@ -649,6 +656,9 @@ struct pios_rfm22b_dev {
 	// The destination ID
 	uint32_t destination_id;
 
+	// Is this device a coordinator?
+	bool coordinator;
+
 	// The task handle
 	xTaskHandle taskHandle;
 
@@ -704,8 +714,12 @@ struct pios_rfm22b_dev {
 	// The packet queue handle
 	xQueueHandle packetQueue;
 
+	// 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
@@ -713,13 +727,32 @@ struct pios_rfm22b_dev {
 	// The tx packet sequence number
 	uint16_t tx_seq;
 
-	// The rx packet
+	// 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;
 
+	// The status packet
+	PHStatusPacket status_packet;
+
+	// The ACK/NACK packet
+	PHAckNackPacket ack_nack_packet;
+
+#ifdef PIOS_PPM_RECEIVER
+	// The PPM packet
+	PHPpmPacket ppm_packet;
+#endif
+
+	// The connection packet.
+	PHConnectionPacket con_packet;
+
+	// Send flags
+	bool send_status;
+	bool send_ppm;
+	bool send_connection_request;
+
 	// The minimum frequency
 	uint32_t min_frequency;
 	// The maximum frequency
@@ -735,17 +768,11 @@ struct pios_rfm22b_dev {
 	// afc correction reading (in Hz)
 	int8_t afc_correction_Hz;
 
-	// The status packet
-	PHStatusPacket status_packet;
-
-#ifdef PIOS_PPM_RECEIVER
-	// The PPM packet
-	PHPpmPacket ppm_packet;
-#endif
-
 	// The maximum time (ms) that it should take to transmit / receive a packet.
 	uint32_t max_packet_time;
 	portTickType packet_start_ticks;
+	portTickType tx_complete_ticks;
+	uint8_t max_ack_delay;
 };
 
 
@@ -753,6 +780,7 @@ struct pios_rfm22b_dev {
 
 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);
 
 
 // Global variable definitions
diff --git a/flight/PipXtreme/System/pios_board.c b/flight/PipXtreme/System/pios_board.c
index e69e0ed33..01cbedfab 100644
--- a/flight/PipXtreme/System/pios_board.c
+++ b/flight/PipXtreme/System/pios_board.c
@@ -246,6 +246,9 @@ void PIOS_Board_Init(void) {
 		}
 	}
 
+	/* Configure the RFM22B device as coordinator or not */
+	PIOS_RFM22B_SetCoordinator(pios_rfm22b_id, oplinkSettings.Coordinator == OPLINKSETTINGS_COORDINATOR_TRUE);
+
 	// Initialize the packet handler
 	PacketHandlerConfig pios_ph_cfg = {
 		.default_destination_id = 0xffffffff, // Broadcast