1
0
mirror of https://bitbucket.org/librepilot/librepilot.git synced 2025-02-20 10:54:14 +01:00

Various minor changes to do with switch modem mode on the fly but mainly PPM Input decoder (with error/noise detection) is now working

git-svn-id: svn://svn.openpilot.org/OpenPilot/trunk@2992 ebee16cc-31ac-478f-84a7-5cbb03baadba
This commit is contained in:
pip 2011-03-06 18:52:18 +00:00 committed by pip
parent cc1a53417d
commit 88b2d360a3
12 changed files with 447 additions and 114 deletions

View File

@ -370,6 +370,10 @@ void apiconfig_processInputPacket(void *buf, uint16_t len)
// ******
ph_deinit();
stream_deinit();
ppm_deinit();
PIOS_COM_ChangeBaud(PIOS_COM_SERIAL, saved_settings.serial_baudrate);
switch (saved_settings.mode)

View File

@ -68,6 +68,7 @@ void ph_set_AES128_key(const void *key);
int ph_set_remote_serial_number(int connection_index, uint32_t sn);
void ph_set_remote_encryption(int connection_index, bool enabled, const void *key);
void ph_deinit(void);
void ph_init(uint32_t our_sn);
// *****************************************************************************

View File

@ -40,6 +40,5 @@
#define PIOS_INCLUDE_GPIO
#define PIOS_INCLUDE_EXTI
#define PIOS_INCLUDE_USB_HID
//#define PIOS_INCLUDE_PPM
#endif /* PIOS_CONFIG_H */

View File

@ -43,6 +43,11 @@
#define PIOS_USB_PRODUCT_ID 0x4117
#endif
//#ifdef PIOS_USB_PRODUCT_ID
//#undef PIOS_USB_PRODUCT_ID
//#endif
//#define PIOS_USB_PRODUCT_ID 0x415C // PipXtreme PID
#ifndef PIOS_USB_VERSION_ID
#define PIOS_USB_VERSION_ID 0x1201 /* v2.00 */
#endif
@ -76,4 +81,4 @@ extern int32_t PIOS_USB_CableConnected(void);
/**
* @}
* @}
*/
*/

View File

@ -32,6 +32,7 @@
void ppm_1ms_tick(void);
void ppm_process(void);
void ppm_deinit(void);
void ppm_init(uint32_t our_sn);
// *************************************************************

View File

@ -576,7 +576,7 @@ enum { RX_SCAN_SPECTRUM = 0,
// ************************************
typedef int16_t ( *t_rfm22_TxDataByteCallback ) (void);
typedef bool ( *t_rfm22_RxDataByteCallback ) (uint8_t b);
typedef bool ( *t_rfm22_RxDataCallback ) (void *data, uint8_t len);
// ************************************
@ -631,7 +631,7 @@ void rfm22_1ms_tick(void);
void rfm22_process(void);
void rfm22_TxDataByte_SetCallback(t_rfm22_TxDataByteCallback new_function);
void rfm22_RxDataByte_SetCallback(t_rfm22_RxDataByteCallback new_function);
void rfm22_RxData_SetCallback(t_rfm22_RxDataCallback new_function);
int rfm22_init_scan_spectrum(uint32_t min_frequency_hz, uint32_t max_frequency_hz);
int rfm22_init_tx_stream(uint32_t min_frequency_hz, uint32_t max_frequency_hz);

View File

@ -32,6 +32,7 @@
void stream_1ms_tick(void);
void stream_process(void);
void stream_deinit(void);
void stream_init(uint32_t our_sn);
// *************************************************************

View File

@ -532,6 +532,10 @@ int main()
RF_CS_ENABLE;
RF_CS_HIGH;
// PPM OUT low
PPM_OUT_ENABLE;
PPM_OUT_LOW;
// pin high
SPARE1_ENABLE;
SPARE1_HIGH;

View File

@ -358,11 +358,11 @@ int16_t ph_TxDataByteCallback(void)
}
// *************************************************************
// we are being given a received byte
// we are being given a block of received bytes
//
// return TRUE to continue current packet receive, otherwise return FALSe to halt current packet reception
// return TRUE to continue current packet receive, otherwise return FALSE to halt current packet reception
bool ph_RxDataByteCallback(uint8_t b)
bool ph_RxDataCallback(void *data, uint8_t len)
{
return true;
}
@ -1627,86 +1627,98 @@ void ph_process(void)
// *****************************************************************************
void ph_init(uint32_t our_sn)
void ph_disconnectAll(void)
{
our_serial_number = our_sn; // remember our own serial number
for (int i = 0; i < PH_MAX_CONNECTIONS; i++)
{
random32 = updateCRC32(random32, 0xff);
fast_ping = false;
t_connection *conn = &connection[i];
for (int i = 0; i < PH_MAX_CONNECTIONS; i++)
{
random32 = updateCRC32(random32, 0xff);
conn->serial_number = 0;
t_connection *conn = &connection[i];
conn->tx_sequence = 0;
conn->tx_sequence_data_size = 0;
conn->serial_number = 0;
conn->rx_sequence = 0;
conn->tx_sequence = 0;
conn->tx_sequence_data_size = 0;
fifoBuf_init(&conn->tx_fifo_buffer, conn->tx_buffer, PH_FIFO_BUFFER_SIZE);
fifoBuf_init(&conn->rx_fifo_buffer, conn->rx_buffer, PH_FIFO_BUFFER_SIZE);
conn->rx_sequence = 0;
conn->link_state = LINK_DISCONNECTED;
fifoBuf_init(&conn->tx_fifo_buffer, conn->tx_buffer, PH_FIFO_BUFFER_SIZE);
fifoBuf_init(&conn->rx_fifo_buffer, conn->rx_buffer, PH_FIFO_BUFFER_SIZE);
conn->tx_packet_timer = 0;
conn->link_state = LINK_DISCONNECTED;
conn->tx_retry_time_slots = 5;
conn->tx_retry_time_slot_len = 40;
conn->tx_retry_time = conn->tx_retry_time_slot_len * 4 + (random32 % conn->tx_retry_time_slots) * conn->tx_retry_time_slot_len * 4;
conn->tx_retry_counter = 0;
conn->tx_packet_timer = 0;
conn->data_speed_timer = 0;
conn->tx_data_speed_count = 0;
conn->tx_data_speed = 0;
conn->rx_data_speed_count = 0;
conn->rx_data_speed = 0;
conn->tx_retry_time_slots = 5;
conn->tx_retry_time_slot_len = 40;
conn->tx_retry_time = conn->tx_retry_time_slot_len * 4 + (random32 % conn->tx_retry_time_slots) * conn->tx_retry_time_slot_len * 4;
conn->tx_retry_counter = 0;
conn->ping_time = 8000 + (random32 % 100) * 10;
conn->fast_ping_time = 600 + (random32 % 50) * 10;
conn->pinging = false;
conn->data_speed_timer = 0;
conn->tx_data_speed_count = 0;
conn->tx_data_speed = 0;
conn->rx_data_speed_count = 0;
conn->rx_data_speed = 0;
conn->rx_not_ready_mode = false;
conn->ping_time = 8000 + (random32 % 100) * 10;
conn->fast_ping_time = 600 + (random32 % 50) * 10;
conn->pinging = false;
conn->ready_to_send_timer = -1;
conn->rx_not_ready_mode = false;
conn->not_ready_timer = -1;
conn->ready_to_send_timer = -1;
conn->send_encrypted = false;
conn->not_ready_timer = -1;
conn->send_encrypted = false;
conn->rx_rssi_dBm = -200;
conn->rx_afc_Hz = 0;
}
// set the AES encryption key using the default AES key
ph_set_AES128_key(default_aes_key);
// try too randomise the tx AES CBC bytes
for (uint32_t j = 0, k = 0; j < 123 + (random32 & 1023); j++)
{
random32 = updateCRC32(random32, 0xff);
enc_cbc[k] ^= random32 >> 3;
if (++k >= sizeof(enc_cbc)) k = 0;
}
// ******
rfm22_init_normal(saved_settings.min_frequency_Hz, saved_settings.max_frequency_Hz, rfm22_freqHopSize());
rfm22_TxDataByte_SetCallback(ph_TxDataByteCallback);
rfm22_RxDataByte_SetCallback(ph_RxDataByteCallback);
rfm22_setFreqCalibration(saved_settings.rf_xtal_cap);
ph_setNominalCarrierFrequency(saved_settings.frequency_Hz);
ph_setDatarate(saved_settings.max_rf_bandwidth);
ph_setTxPower(saved_settings.max_tx_power);
ph_set_remote_encryption(0, saved_settings.aes_enable, (const void *)saved_settings.aes_key);
ph_set_remote_serial_number(0, saved_settings.destination_id);
// ******
conn->rx_rssi_dBm = -200;
conn->rx_afc_Hz = 0;
}
}
// *****************************************************************************
void ph_deinit(void)
{
ph_disconnectAll();
}
void ph_init(uint32_t our_sn)
{
our_serial_number = our_sn; // remember our own serial number
fast_ping = false;
ph_disconnectAll();
// set the AES encryption key using the default AES key
ph_set_AES128_key(default_aes_key);
// try too randomise the tx AES CBC bytes
for (uint32_t j = 0, k = 0; j < 123 + (random32 & 1023); j++)
{
random32 = updateCRC32(random32, 0xff);
enc_cbc[k] ^= random32 >> 3;
if (++k >= sizeof(enc_cbc)) k = 0;
}
// ******
rfm22_init_normal(saved_settings.min_frequency_Hz, saved_settings.max_frequency_Hz, rfm22_freqHopSize());
rfm22_TxDataByte_SetCallback(ph_TxDataByteCallback);
rfm22_RxData_SetCallback(ph_RxDataCallback);
rfm22_setFreqCalibration(saved_settings.rf_xtal_cap);
ph_setNominalCarrierFrequency(saved_settings.frequency_Hz);
ph_setDatarate(saved_settings.max_rf_bandwidth);
ph_setTxPower(saved_settings.max_tx_power);
ph_set_remote_encryption(0, saved_settings.aes_enable, (const void *)saved_settings.aes_key);
ph_set_remote_serial_number(0, saved_settings.destination_id);
// ******
}
// *****************************************************************************

View File

@ -23,7 +23,7 @@
* 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA
*/
//#include <string.h> // memmove
#include <string.h> // memmove
#include "main.h"
#include "rfm22b.h"
@ -34,20 +34,268 @@
#define PPM_DEBUG
#endif
// *************************************************************
#define PPM_OUT_SYNC_PULSE_US 12000 // microseconds
#define PPM_IN_MIN_SYNC_PULSE_US 7000 // microseconds .. Pip's 6-chan TX goes down to 8.8ms
#define PPM_IN_MAX_SYNC_PULSE_US 16000 // microseconds .. Pip's 6-chan TX goes up to 14.4ms
#define PPM_IN_MIN_CHANNEL_PULSE_US 750 // microseconds
#define PPM_IN_MAX_CHANNEL_PULSE_US 2400 // microseconds
// *************************************************************
volatile bool ppm_initialising = true;
volatile uint32_t ppm_In_PrevFrames = 0;
volatile uint32_t ppm_In_LastValidFrameTimer = 0;
volatile uint32_t ppm_In_Frames = 0;
volatile uint32_t ppm_In_SyncPulseWidth = 0;
volatile uint32_t ppm_In_LastFrameTime = 0;
volatile uint8_t ppm_In_NoisyChannelCounter = 0;
volatile int8_t ppm_In_ChannelsDetected = 0;
volatile int8_t ppm_In_ChannelPulseIndex = -1;
volatile uint32_t ppm_In_PreviousValue = 0;
volatile uint32_t ppm_In_CurrentValue = 0;
volatile uint32_t ppm_In_ChannelPulseWidthNew[PIOS_PPM_IN_MAX_INPUTS];
volatile uint32_t ppm_In_ChannelPulseWidth[PIOS_PPM_IN_MAX_INPUTS];
// *************************************************************
// Initialise the ppm
void ppm_In_Init(void)
{
TIM_ICInitTypeDef TIM_ICInitStructure;
// disable the timer
TIM_Cmd(PIOS_PPM_IN_TIM, DISABLE);
ppm_In_PrevFrames = 0;
ppm_In_NoisyChannelCounter = 0;
ppm_In_LastValidFrameTimer = 0;
ppm_In_Frames = 0;
ppm_In_SyncPulseWidth = 0;
ppm_In_LastFrameTime = 0;
ppm_In_ChannelsDetected = 0;
ppm_In_ChannelPulseIndex = -1;
ppm_In_PreviousValue = 0;
ppm_In_CurrentValue = 0;
for (int i = 0; i < PIOS_PPM_IN_MAX_INPUTS; i++)
{
ppm_In_ChannelPulseWidthNew[i] = 0;
ppm_In_ChannelPulseWidth[i] = 0;
}
// Setup RCC
PIOS_PPM_IN_TIMER_RCC_FUNC;
// Enable timer interrupts
NVIC_InitTypeDef NVIC_InitStructure;
NVIC_InitStructure.NVIC_IRQChannelPreemptionPriority = PIOS_IRQ_PRIO_MID;
NVIC_InitStructure.NVIC_IRQChannelSubPriority = 0;
NVIC_InitStructure.NVIC_IRQChannelCmd = ENABLE;
NVIC_InitStructure.NVIC_IRQChannel = PIOS_PPM_IN_TIM_IRQ;
NVIC_Init(&NVIC_InitStructure);
// Init PPM IN pin
GPIO_InitTypeDef GPIO_InitStructure;
GPIO_StructInit(&GPIO_InitStructure);
GPIO_InitStructure.GPIO_Mode = PPM_IN_MODE;
GPIO_InitStructure.GPIO_Speed = GPIO_Speed_2MHz;
GPIO_InitStructure.GPIO_Pin = PPM_IN_PIN;
GPIO_Init(PPM_IN_PORT, &GPIO_InitStructure);
// remap the pin to switch it to timer mode
// GPIO_PinRemapConfig(GPIO_PartialRemap1_TIM2, ENABLE);
GPIO_PinRemapConfig(GPIO_PartialRemap2_TIM2, ENABLE);
// GPIO_PinRemapConfig(GPIO_FullRemap_TIM2, ENABLE);
// Configure timer for input capture
TIM_ICInitStructure.TIM_ICPolarity = TIM_ICPolarity_Rising;
TIM_ICInitStructure.TIM_ICSelection = TIM_ICSelection_DirectTI;
TIM_ICInitStructure.TIM_ICPrescaler = TIM_ICPSC_DIV1;
TIM_ICInitStructure.TIM_ICFilter = 0x0;
TIM_ICInitStructure.TIM_Channel = PIOS_PPM_IN_TIM_CHANNEL;
TIM_ICInit(PIOS_PPM_IN_TIM_PORT, &TIM_ICInitStructure);
// Configure timer clocks
TIM_TimeBaseInitTypeDef TIM_TimeBaseStructure;
TIM_TimeBaseStructInit(&TIM_TimeBaseStructure);
TIM_TimeBaseStructure.TIM_Period = 0xFFFF;
TIM_TimeBaseStructure.TIM_Prescaler = (PIOS_MASTER_CLOCK / 1000000) - 1;
TIM_TimeBaseStructure.TIM_ClockDivision = TIM_CKD_DIV1;
TIM_TimeBaseStructure.TIM_CounterMode = TIM_CounterMode_Up;
TIM_InternalClockConfig(PIOS_PPM_IN_TIM_PORT);
TIM_TimeBaseInit(PIOS_PPM_IN_TIM_PORT, &TIM_TimeBaseStructure);
// Enable the Capture Compare Interrupt Request
TIM_ITConfig(PIOS_PPM_IN_TIM_PORT, PIOS_PPM_IN_TIM_CCR, ENABLE);
// Enable timer
TIM_Cmd(PIOS_PPM_IN_TIM, ENABLE);
// Setup local variable which stays in this scope
// Doing this here and using a local variable saves doing it in the ISR
TIM_ICInitStructure.TIM_ICSelection = TIM_ICSelection_DirectTI;
TIM_ICInitStructure.TIM_ICPrescaler = TIM_ICPSC_DIV1;
TIM_ICInitStructure.TIM_ICFilter = 0x0;
}
// TIMER capture/compare interrupt
void PIOS_PPM_IN_CC_IRQ_FUNC(void)
{
uint32_t pulse_width_us; // new pulse width in microseconds
if (booting || ppm_initialising)
{ // just clear the interrupt
if (TIM_GetITStatus(PIOS_PPM_IN_TIM_PORT, PIOS_PPM_IN_TIM_CCR) == SET)
{
TIM_ClearITPendingBit(PIOS_PPM_IN_TIM_PORT, PIOS_PPM_IN_TIM_CCR);
PIOS_PPM_IN_TIM_GETCAP_FUNC(PIOS_PPM_IN_TIM_PORT);
}
TIM_ClearITPendingBit(PIOS_PPM_IN_TIM_PORT, PIOS_PPM_IN_TIM_CCR);
return;
}
// Do this as it's more efficient
if (TIM_GetITStatus(PIOS_PPM_IN_TIM_PORT, PIOS_PPM_IN_TIM_CCR) == SET)
{
ppm_In_PreviousValue = ppm_In_CurrentValue;
ppm_In_CurrentValue = PIOS_PPM_IN_TIM_GETCAP_FUNC(PIOS_PPM_IN_TIM_PORT);
}
// Clear TIMER Capture compare interrupt pending bit
TIM_ClearITPendingBit(PIOS_PPM_IN_TIM_PORT, PIOS_PPM_IN_TIM_CCR);
// Capture computation
if (ppm_In_CurrentValue > ppm_In_PreviousValue)
pulse_width_us = (ppm_In_CurrentValue - ppm_In_PreviousValue);
else
pulse_width_us = ((0xFFFF - ppm_In_PreviousValue) + ppm_In_CurrentValue);
// ********
#ifdef PPM_DEBUG
// DEBUG_PRINTF("ppm_in: %uus\r\n", pulse_width_us);
#endif
if (pulse_width_us >= PPM_IN_MIN_SYNC_PULSE_US)
{ // SYNC pulse
if (pulse_width_us <= PPM_IN_MAX_SYNC_PULSE_US)
{ // SYNC pulse width is within accepted tolerance
if (ppm_In_ChannelPulseIndex > 0)
{ // we found some channel PWM's in the PPM stream
if (ppm_In_ChannelsDetected > 0 && ppm_In_ChannelPulseIndex == ppm_In_ChannelsDetected)
{ // detected same number of channels as in previous PPM frame .. save the new channel PWM values
// if (ppm_In_NoisyChannelCounter <= 2) // only update channels if the channels are fairly noise free
for (int i = 0; i < PIOS_PPM_IN_MAX_INPUTS; i++)
ppm_In_ChannelPulseWidth[i] = ppm_In_ChannelPulseWidthNew[i];
}
ppm_In_ChannelsDetected = ppm_In_ChannelPulseIndex; // the number of channels we found in this frame
ppm_In_LastValidFrameTimer = 0; // reset timer
ppm_In_Frames++; // update frame counter
}
ppm_In_NoisyChannelCounter = 0; // reset noisy channel detector
ppm_In_ChannelPulseIndex = 0; // start of PPM frame
ppm_In_LastFrameTime = 0; // reset timer
}
ppm_In_SyncPulseWidth = pulse_width_us; // remember the length of this SYNC pulse
}
else
if (ppm_In_SyncPulseWidth > 0 && ppm_In_ChannelPulseIndex >= 0)
{ // CHANNEL pulse
if (pulse_width_us >= PPM_IN_MIN_CHANNEL_PULSE_US && pulse_width_us <= PPM_IN_MAX_CHANNEL_PULSE_US)
{ // this new channel pulse is within the accepted tolerance range
if (ppm_In_ChannelPulseIndex < PIOS_PPM_IN_MAX_INPUTS)
{
int32_t difference = (int32_t)pulse_width_us - ppm_In_ChannelPulseWidthNew[ppm_In_ChannelPulseIndex];
if (abs(difference) >= 300)
ppm_In_NoisyChannelCounter++; // possibly a noisy channel - or an RC switch was moved
ppm_In_ChannelPulseWidthNew[ppm_In_ChannelPulseIndex] = pulse_width_us; // save it
}
if (ppm_In_ChannelPulseIndex < 127)
ppm_In_ChannelPulseIndex++; // next channel
ppm_In_LastFrameTime = 0; // reset timer
}
else
{ // bad/noisy channel pulse .. reset state to wait for next SYNC pulse
ppm_In_Frames = 0;
ppm_In_ChannelPulseIndex = -1;
}
}
// ********
}
void ppm_In_Supervisor(void)
{ // this gets called once every millisecond by an interrupt
if (booting || ppm_initialising)
return;
if (ppm_In_LastValidFrameTimer < 0xffffffff)
ppm_In_LastValidFrameTimer++;
if (ppm_In_LastFrameTime < 0xffffffff)
ppm_In_LastFrameTime++;
if (ppm_In_LastFrameTime > ((PPM_IN_MAX_SYNC_PULSE_US * 2) / 1000) && ppm_In_SyncPulseWidth > 0)
{ // no PPM frames detected for a while .. reset PPM state
ppm_In_SyncPulseWidth = 0;
ppm_In_ChannelsDetected = 0;
ppm_In_ChannelPulseIndex = -1;
ppm_In_NoisyChannelCounter = 0;
ppm_In_Frames = 0;
}
}
int32_t ppm_In_GetChannelPulseWidth(uint8_t channel)
{
if (booting || ppm_initialising)
return -1;
// Return error if channel not available
if (channel >= PIOS_PPM_IN_MAX_INPUTS || channel >= ppm_In_ChannelsDetected)
return -2;
if (ppm_In_LastValidFrameTimer > (PPM_IN_MAX_SYNC_PULSE_US * 4) / 1000)
return 0; // to long since last valid PPM frame
return ppm_In_ChannelPulseWidth[channel]; // return channel pulse width
}
// *************************************************************
// can be called from an interrupt if you wish
// call this once every ms
void ppm_1ms_tick(void)
{
if (booting) return;
if (booting || ppm_initialising)
return;
if (saved_settings.mode == MODE_PPM_TX)
{
ppm_In_Supervisor();
return;
}
else
if (saved_settings.mode == MODE_PPM_RX)
{
return;
}
}
@ -62,11 +310,11 @@ int16_t ppm_TxDataByteCallback(void)
}
// *************************************************************
// we are being given a received byte
// we are being given a block of received bytes
//
// return TRUE to continue current packet receive, otherwise return FALSe to halt current packet reception
// return TRUE to continue current packet receive, otherwise return FALSE to halt current packet reception
bool ppm_RxDataByteCallback(uint8_t b)
bool ppm_RxDataCallback(void *data, uint8_t len)
{
return true;
}
@ -76,33 +324,69 @@ bool ppm_RxDataByteCallback(uint8_t b)
void ppm_process(void)
{
if (booting) return;
if (booting || ppm_initialising)
return;
if (saved_settings.mode == MODE_PPM_TX)
{
if (ppm_In_Frames > 0 && ppm_In_Frames != ppm_In_PrevFrames)
{ // we have a new PPM frame
ppm_In_PrevFrames = ppm_In_Frames;
#ifdef PPM_DEBUG
DEBUG_PRINTF("\r\n");
DEBUG_PRINTF("ppm_in: sync %u\r\n", ppm_In_SyncPulseWidth);
#endif
for (int i = 0; i < PIOS_PPM_IN_MAX_INPUTS && i < ppm_In_ChannelsDetected; i++)
{
// int32_t pwm = ppm_In_GetChannelPulseWidth(i);
#ifdef PPM_DEBUG
DEBUG_PRINTF("ppm_in: %u %u %4u\r\n", ppm_In_PrevFrames, i, ppm_In_GetChannelPulseWidth(i));
#endif
}
}
return;
}
else
if (saved_settings.mode == MODE_PPM_RX)
{
return;
}
}
// *************************************************************
void ppm_deinit(void)
{
// disable the PPM timer
TIM_Cmd(PIOS_PPM_IN_TIM, DISABLE);
}
void ppm_init(uint32_t our_sn)
{
ppm_initialising = true;
#if defined(PPM_DEBUG)
DEBUG_PRINTF("\r\nPPM init\r\n");
#endif
if (saved_settings.mode == MODE_PPM_TX)
{
ppm_In_Init();
rfm22_init_tx_stream(saved_settings.min_frequency_Hz, saved_settings.max_frequency_Hz);
}
else
if (saved_settings.mode == MODE_PPM_RX)
{
rfm22_init_rx_stream(saved_settings.min_frequency_Hz, saved_settings.max_frequency_Hz);
}
rfm22_TxDataByte_SetCallback(ppm_TxDataByteCallback);
rfm22_RxDataByte_SetCallback(ppm_RxDataByteCallback);
rfm22_RxData_SetCallback(ppm_RxDataCallback);
rfm22_setFreqCalibration(saved_settings.rf_xtal_cap);
rfm22_setNominalCarrierFrequency(saved_settings.frequency_Hz);
@ -110,6 +394,8 @@ void ppm_init(uint32_t our_sn)
rfm22_setTxPower(saved_settings.max_tx_power);
rfm22_setTxStream(); // TEST ONLY
ppm_initialising = false;
}
// *************************************************************

View File

@ -48,7 +48,7 @@
#include "rfm22b.h"
#if defined(PIOS_COM_DEBUG)
#define RFM22_DEBUG
// #define RFM22_DEBUG
// #define RFM22_INT_TIMEOUT_DEBUG
#endif
@ -272,6 +272,11 @@ volatile uint8_t *tx_data_addr; // the address of the data we send in the t
volatile uint16_t tx_data_rd; // the tx data read index
volatile uint16_t tx_data_wr; // the tx data write index
//volatile uint8_t tx_fifo[FIFO_SIZE]; //
volatile uint8_t rx_fifo[FIFO_SIZE]; //
volatile uint8_t rx_fifo_wr; //
int lookup_index;
int ss_lookup_index;
@ -287,8 +292,8 @@ uint16_t timeout_ms = 20000; //
uint16_t timeout_sync_ms = 3; //
uint16_t timeout_data_ms = 20; //
t_rfm22_TxDataByteCallback tx_data_byte_callback_function;
t_rfm22_RxDataByteCallback rx_data_byte_callback_function;
t_rfm22_TxDataByteCallback tx_data_byte_callback_function = NULL;
t_rfm22_RxDataCallback rx_data_callback_function = NULL;
// ************************************
// SPI read/write
@ -1084,22 +1089,27 @@ void rfm22_processRxInt(void)
// fetch the rx'ed data from the rf chips RX FIFO
rfm22_startBurstRead(RFM22_fifo_access);
for (register uint16_t i = RX_FIFO_HI_WATERMARK; i > 0; i--)
{
register uint8_t b = rfm22_burstRead(); // read a byte from the rf modules RX FIFO buffer
if (rx_data_byte_callback_function)
{
rx_data_byte_callback_function(b);
}
if (wr < sizeof(rx_buffer))
rx_buffer[wr++] = b; // save the byte into our rx buffer
}
rx_fifo_wr = 0;
for (register uint8_t i = RX_FIFO_HI_WATERMARK; i > 0; i--)
rx_fifo[rx_fifo_wr++] = rfm22_burstRead(); // read a byte from the rf modules RX FIFO buffer
rfm22_endBurstRead();
uint16_t i = rx_fifo_wr;
if (wr + i > sizeof(rx_buffer)) i = sizeof(rx_buffer) - wr;
memcpy((void *)(rx_buffer + wr), (void *)rx_fifo, i); // save the new bytes into our rx buffer
wr += i;
rx_buffer_wr = wr;
if (rx_data_callback_function)
{ // pass the new data onto whoever wanted it
if (!rx_data_callback_function((void *)rx_fifo, rx_fifo_wr))
{
rfm22_setRxMode(RX_WAIT_PREAMBLE_MODE, false);
return;
}
}
#if defined(RFM22_DEBUG) && !defined(RFM22_EXT_INT_USE)
// DEBUG_PRINTF(" r_data_%u/%u", rx_buffer_wr, len);
// debug_outputted = true;
@ -1161,22 +1171,28 @@ void rfm22_processRxInt(void)
if (wr < len)
{ // their must still be data in the RX FIFO we need to get
// fetch the rx'ed data from the rf chips RX FIFO
rfm22_startBurstRead(RFM22_fifo_access);
while (wr < len)
{
register uint8_t b = rfm22_burstRead();
if (rx_data_byte_callback_function)
{
rx_data_byte_callback_function(b);
}
if (wr >= sizeof(rx_buffer)) break;
rx_buffer[wr++] = b;
}
rx_fifo_wr = 0;
for (register uint8_t i = len - wr; i > 0; i--)
rx_fifo[rx_fifo_wr++] = rfm22_burstRead(); // read a byte from the rf modules RX FIFO buffer
rfm22_endBurstRead();
uint16_t i = rx_fifo_wr;
if (wr + i > sizeof(rx_buffer)) i = sizeof(rx_buffer) - wr;
memcpy((void *)(rx_buffer + wr), (void *)rx_fifo, i); // save the new bytes into our rx buffer
wr += i;
rx_buffer_wr = wr;
if (rx_data_callback_function)
{ // pass the new data onto whoever wanted it
if (!rx_data_callback_function((void *)rx_fifo, rx_fifo_wr))
{
// rfm22_setRxMode(RX_WAIT_PREAMBLE_MODE, false);
// return;
}
}
}
rfm22_setRxMode(RX_WAIT_PREAMBLE_MODE, false); // reset the receiver
@ -1968,9 +1984,9 @@ void rfm22_TxDataByte_SetCallback(t_rfm22_TxDataByteCallback new_function)
tx_data_byte_callback_function = new_function;
}
void rfm22_RxDataByte_SetCallback(t_rfm22_RxDataByteCallback new_function)
void rfm22_RxData_SetCallback(t_rfm22_RxDataCallback new_function)
{
rx_data_byte_callback_function = new_function;
rx_data_callback_function = new_function;
}
// ************************************
@ -2055,7 +2071,7 @@ int rfm22_resetModule(uint8_t mode, uint32_t min_frequency_hz, uint32_t max_freq
rssi_dBm = -200;
tx_data_byte_callback_function = NULL;
rx_data_byte_callback_function = NULL;
rx_data_callback_function = NULL;
rx_buffer_current = 0;
rx_buffer_wr = 0;

View File

@ -65,11 +65,11 @@ int16_t stream_TxDataByteCallback(void)
}
// *************************************************************
// we are being given a received byte
// we are being given a block of received bytes
//
// return TRUE to continue current packet receive, otherwise return FALSe to halt current packet reception
// return TRUE to continue current packet receive, otherwise return FALSE to halt current packet reception
bool stream_RxDataByteCallback(uint8_t b)
bool stream_RxDataCallback(void *data, uint8_t len)
{
return true;
}
@ -92,6 +92,10 @@ void stream_process(void)
// *************************************************************
void stream_deinit(void)
{
}
void stream_init(uint32_t our_sn)
{
#if defined(STREAM_DEBUG)
@ -105,7 +109,7 @@ void stream_init(uint32_t our_sn)
rfm22_init_rx_stream(saved_settings.min_frequency_Hz, saved_settings.max_frequency_Hz);
rfm22_TxDataByte_SetCallback(stream_TxDataByteCallback);
rfm22_RxDataByte_SetCallback(stream_RxDataByteCallback);
rfm22_RxData_SetCallback(stream_RxDataCallback);
rfm22_setFreqCalibration(saved_settings.rf_xtal_cap);
rfm22_setNominalCarrierFrequency(saved_settings.frequency_Hz);