rooty/LibrePilot
1
0
Fork 0
mirror of https://bitbucket.org/librepilot/librepilot.git synced 2025-02-26 15:54:15 +01:00
Code Issues Releases Activity

LP-437 - Add support for a secondary radio stream and associated AuxCom driver

Browse Source 
TODO: find out if there is still free space on current packet and store both streams content
This commit is contained in:
Alessio Morale 2016-10-27 23:08:49 +02:00
parent c2cf44f8d8
commit 2e5f3aa60c
4 changed files with 110 additions and 12 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

49
flight/pios/common/pios_rfm22b.c
View File

@ -420,6 +420,8 @@ int32_t PIOS_RFM22B_Init(uint32_t *rfm22b_id, uint32_t spi_id, uint32_t slave_nu
rfm22b_dev->rx_in_cb = NULL;
rfm22b_dev->tx_out_cb = NULL;
rfm22b_dev->aux_rx_in_cb = NULL;
rfm22b_dev->aux_tx_out_cb = NULL;
// Initialize the PPM callback.
rfm22b_dev->ppm_callback = NULL;
@ -1905,10 +1907,28 @@ static enum pios_radio_event radio_txStart(struct pios_rfm22b_dev *radio_dev)
}
// Append data from the com interface if applicable.
if (!radio_dev->ppm_only_mode && radio_dev->tx_out_cb) {
// Try to get some data to send
if (!radio_dev->ppm_only_mode) {
uint8_t newlen = 0;
bool need_yield = false;
len += (radio_dev->tx_out_cb)(radio_dev->tx_out_context, p + len, max_data_len - len, NULL, &need_yield);
uint8_t i = 0;
// Try to get some data to send
while (newlen == 0 && i < 2) {
radio_dev->last_stream_sent = (radio_dev->last_stream_sent + 1) % 2;
if (!radio_dev->last_stream_sent) {
if (radio_dev->tx_out_cb) {
newlen = (radio_dev->tx_out_cb)(radio_dev->tx_out_context, p + len + 1, max_data_len - len - 1, NULL, &need_yield);
}
} else {
if (radio_dev->aux_tx_out_cb) {
newlen = (radio_dev->aux_tx_out_cb)(radio_dev->aux_tx_out_context, p + len + 1, max_data_len - len - 1, NULL, &need_yield);
}
}
i++;
}
if (newlen) {
*(p + len) = radio_dev->last_stream_sent;
len += newlen + 1;
}
}
// Always send a packet if this modem is a coordinator.
@ -1990,9 +2010,10 @@ static enum pios_radio_event radio_setRxMode(struct pios_rfm22b_dev *rfm22b_dev)
*/
static enum pios_radio_event radio_receivePacket(struct pios_rfm22b_dev *radio_dev, uint8_t *p, uint16_t rx_len)
{
bool good_packet = true;
bool good_packet = true;
bool corrected_packet = false;
uint8_t data_len = rx_len;
uint8_t stream_num = 0;
uint8_t data_len = rx_len;
// We don't rsencode ppm only packets.
if (!radio_dev->ppm_only_mode) {
@ -2073,10 +2094,22 @@ static enum pios_radio_event radio_receivePacket(struct pios_rfm22b_dev *radio_d
if (good_packet || corrected_packet) {
// Send the data to the com port
bool rx_need_yield;
if (radio_dev->rx_in_cb && (data_len > 0) && !radio_dev->ppm_only_mode) {
(radio_dev->rx_in_cb)(radio_dev->rx_in_context, p, data_len, NULL, &rx_need_yield);
}
if ((data_len > 0) && !radio_dev->ppm_only_mode) {
stream_num = *p;
p++;
data_len--;
if (!stream_num) {
if (radio_dev->rx_in_cb) {
(radio_dev->rx_in_cb)(radio_dev->rx_in_context, p, data_len, NULL, &rx_need_yield);
}
} else {
if (radio_dev->aux_rx_in_cb) {
(radio_dev->aux_rx_in_cb)(radio_dev->aux_rx_in_context, p, data_len, NULL, &rx_need_yield);
}
}
}
/*
* If the packet is valid and destined for us we synchronize the clock.
*/

58
flight/pios/common/pios_rfm22b_com.c
View File

@ -40,6 +40,10 @@ static void PIOS_RFM22B_COM_RegisterRxCallback(uint32_t rfm22b_id, pios_com_call
static void PIOS_RFM22B_COM_RegisterTxCallback(uint32_t rfm22b_id, pios_com_callback tx_out_cb, uint32_t context);
static void PIOS_RFM22B_COM_TxStart(uint32_t rfm22b_id, uint16_t tx_bytes_avail);
static void PIOS_RFM22B_COM_RxStart(uint32_t rfm22b_id, uint16_t rx_bytes_avail);
static void PIOS_RFM22B_COM_RegisterAuxRxCallback(uint32_t rfm22b_id, pios_com_callback rx_in_cb, uint32_t context);
static void PIOS_RFM22B_COM_RegisterAuxTxCallback(uint32_t rfm22b_id, pios_com_callback tx_out_cb, uint32_t context);
static uint32_t PIOS_RFM22B_COM_Available(uint32_t rfm22b_com_id);
/* Local variables */
@ -52,6 +56,15 @@ const struct pios_com_driver pios_rfm22b_com_driver = {
.available = PIOS_RFM22B_COM_Available
};
/* Local variables */
const struct pios_com_driver pios_rfm22b_aux_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_RegisterAuxTxCallback,
.bind_rx_cb = PIOS_RFM22B_COM_RegisterAuxRxCallback,
.available = PIOS_RFM22B_COM_Available
};
/**
* Changes the baud rate of the RFM22B peripheral without re-initialising.
*
@ -129,6 +142,51 @@ static void PIOS_RFM22B_COM_RegisterTxCallback(uint32_t rfm22b_id, pios_com_call
rfm22b_dev->tx_out_cb = tx_out_cb;
}
/**
* 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_RegisterAuxRxCallback(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;
}
/*
* Order is important in these assignments since ISR uses _cb
* field to determine if it's ok to dereference _cb and _context
*/
rfm22b_dev->aux_rx_in_context = context;
rfm22b_dev->aux_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_RegisterAuxTxCallback(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;
}
/*
* Order is important in these assignments since ISR uses _cb
* field to determine if it's ok to dereference _cb and _context
*/
rfm22b_dev->aux_tx_out_context = context;
rfm22b_dev->aux_tx_out_cb = tx_out_cb;
}
/**
* See if the COM port is alive
*

7
flight/pios/inc/pios_rfm22b_com.h
View File

@ -28,12 +28,13 @@
* 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA
*/
#ifndef PIOS_RFM22B_H
#define PIOS_RFM22B_H
#ifndef PIOS_RFM22B_COM_H
#define PIOS_RFM22B_COM_H
extern const struct pios_com_driver pios_rfm22b_com_driver;
extern const struct pios_com_driver pios_rfm22b_aux_com_driver;
#endif /* PIOS_RFM22B_H */
#endif /* PIOS_RFM22B_COM_H */
/**
* @}

8
flight/pios/inc/pios_rfm22b_priv.h
View File

@ -206,12 +206,18 @@ struct pios_rfm22b_dev {
// ISR pending semaphore
xSemaphoreHandle isrPending;
// The COM callback functions.
// The main 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;
uint8_t last_stream_sent;
// The Aux COM callback functions.
pios_com_callback aux_rx_in_cb;
uint32_t aux_rx_in_context;
pios_com_callback aux_tx_out_cb;
uint32_t aux_tx_out_context;
// the transmit power to use for data transmissions
uint8_t tx_power;