Work on HW abstraction for radio.

pull/4/head
CInsights 2018-03-29 23:23:12 +11:00
rodzic efc46631cf
commit d551174371
18 zmienionych plików z 299 dodań i 920 usunięć

Wyświetl plik

@ -11,7 +11,7 @@ const conf_t conf_flash_default = {
// Primary position transmission thread
.pos_pri = {
.thread_conf = {
.active = false,
.active = true,
.cycle = TIME_S2I(300),
.init_delay = TIME_S2I(5)
},
@ -59,7 +59,7 @@ const conf_t conf_flash_default = {
.active = true,
.cycle = CYCLE_CONTINUOUSLY,
.init_delay = TIME_S2I(5),
.packet_spacing = TIME_S2I(30)
.packet_spacing = TIME_S2I(/*3*/0)
},
.radio_conf = {
.pwr = 0x7F,

Wyświetl plik

@ -178,6 +178,7 @@ void usb_cmd_send_aprs_message(BaseSequentialStream *chp, int argc, char *argv[]
chprintf(chp, "Destination: %s\r\n", argv[0]);
chprintf(chp, "Message: %s\r\n", argv[1]);
/* TODO: Check for failure to get packet (NULL). */
packet_t packet = aprs_encode_message(conf_sram.rx.call,
conf_sram.rx.path,
argv[0], argv[1], false);

Wyświetl plik

@ -300,7 +300,7 @@ void pktOpenPWMChannelI(ICUDriver *myICU, eventflags_t evt) {
* Send the FIFO entry to the decoder thread.
*/
chFifoSendObjectI(myDemod->pwm_fifo_pool, myFIFO);
myFIFO->status |= EVT_PWM_FIFO_SENT;
/* myFIFO->status |= EVT_PWM_FIFO_SENT;*/
/*
* Start the ICU activity timer.

Wyświetl plik

@ -189,10 +189,6 @@
#define Si446x_FIFO_SEPARATE_SIZE 64
#define Si446x_FIFO_COMBINED_SIZE 129
#define AFSK_FEEDER_BUFFER_SIZE 3072
#define USE_DYNAMIC_AFSK_TX TRUE
#define USE_MIN_AFSK_TX TRUE
#define SI_AFSK_FIFO_FEEDER_WA_SIZE (AFSK_FEEDER_BUFFER_SIZE + 1024)
#define SI_AFSK_FIFO_MIN_FEEDER_WA_SIZE 1024
#define USE_DYNAMIC_FSK_TX FALSE
@ -206,36 +202,50 @@ typedef enum radioMode {
// Public methods
int16_t Si446x_getLastTemperature(void);
int16_t Si446x_getLastTemperature(radio_unit_t radio);
void Si446x_shutdown(radio_unit_t radio);
void Si446x_sendAFSK(packet_t pp);
void Si446x_send2FSK(packet_t pp);
void Si446x_disableReceive(void);
/*void Si446x_startPacketReception(uint32_t freq, uint16_t step,
uint8_t ch, uint8_t sq, void* cb);*/
void Si446x_disableReceive(radio_unit_t radio);
void Si446x_stopDecoder(void);
bool Si4464_resumeReceive(radio_unit_t radio);
bool Si446x_receiveNoLock(uint8_t chan, uint8_t rssi, mod_t mod);
bool Si4464_resumeReceive(radio_unit_t radio,
radio_freq_t rx_frequency,
channel_hz_t rx_step,
radio_ch_t rx_chan,
radio_squelch_t rx_rssi,
mod_t rx_mod);
bool Si446x_receiveNoLock(radio_unit_t radio,
radio_freq_t rx_frequency,
channel_hz_t rx_step,
radio_ch_t chan,
radio_squelch_t rssi,
mod_t mod);
void Si446x_lockRadio(radio_mode_t mode);
void Si446x_unlockRadio(radio_mode_t mode);
void Si446x_lockRadioByCamera(void);
void Si446x_unlockRadioByCamera(void);
void Si446x_conditional_init(void);
bool Si446x_setBandParameters(uint32_t freq,
uint16_t step,
radio_mode_t mode);
void Si446x_conditional_init(radio_unit_t radio);
bool Si446x_setBandParameters(radio_unit_t radio,
radio_freq_t freq,
channel_hz_t step);
/* Inline. */
static inline uint32_t Si446x_computeOperatingFrequency(uint32_t base_freq,
uint16_t step,
uint8_t chan) {
return base_freq + (step * chan);
}
/* TODO: Forward need for declaration. */
radio_freq_t pktComputeOperatingFrequency(radio_freq_t base_freq,
channel_hz_t step,
radio_ch_t chan);
static inline bool Si446x_isFrequencyInBand(uint32_t base_freq,
uint16_t step, uint8_t chan) {
uint32_t freq = Si446x_computeOperatingFrequency(base_freq, step, chan);
static inline bool Si446x_isFrequencyInBand(radio_unit_t radio,
radio_freq_t base_freq,
channel_hz_t step,
radio_ch_t chan) {
(void)radio;
radio_freq_t freq = pktComputeOperatingFrequency(base_freq,
step,
chan);
/* TODO: Lookup frequency band table for radio. */
return (Si446x_MIN_FREQ <= freq && freq < Si446x_MAX_FREQ);
}

Wyświetl plik

@ -75,22 +75,27 @@ THD_FUNCTION(pktRadioManager, arg) {
case PKT_RADIO_RX_OPEN: {
/* Create the packet management services. */
if(pktBufferManagerCreate(radio) == NULL)
/* TODO: Set an event on fail. */
if(pktBufferManagerCreate(radio) == NULL) {
pktAddEventFlags(handler, (EVT_PKT_BUFFER_MGR_FAIL));
break;
}
/* Create callback manager. */
if(pktCallbackManagerCreate(radio) == NULL)
/* TODO: Set an event on fail. */
if(pktCallbackManagerCreate(radio) == NULL) {
pktAddEventFlags(handler, (EVT_PKT_CBK_MGR_FAIL));
pktBufferManagerRelease(handler);
break;
}
/* Switch on modulation type. */
switch(task_object->type) {
case MOD_AFSK: {
/* Create the AFSK decoder (includes PWM, filters, etc.). */
AFSKDemodDriver *driver = pktCreateAFSKDecoder(handler);
handler->link_controller = driver;
chDbgCheck(driver != NULL);
/* TODO: Implement thread events or callback for results. */
/* If AFSK start failed send event but leave managers running. */
if(driver == NULL) {
pktAddEventFlags(handler, (EVT_AFSK_START_FAIL));
/* pktBufferManagerRelease(handler);
pktCallbackManagerRelease(handler);*/
break;
}
break;
@ -104,29 +109,34 @@ THD_FUNCTION(pktRadioManager, arg) {
} /* End switch on modulation type. */
/* Initialise the radio. */
Si446x_conditional_init();
Si446x_conditional_init(radio);
break;
} /* End case PKT_RADIO_OPEN. */
case PKT_RADIO_RX_START: {
/* The function switches on mod type so no need for switch here. */
switch(task_object->type) {
case MOD_AFSK: {
Si446x_lockRadio(RADIO_RX);
pktAcquireRadio(radio);
Si446x_setBandParameters(task_object->base_frequency,
task_object->step_hz,
RADIO_RX);
/* TODO: Move these 446x calls into abstracted LLD. */
Si446x_setBandParameters(radio,
task_object->base_frequency,
task_object->step_hz);
radio_ch_t chan = task_object->channel;
radio_squelch_t sq = task_object->squelch;
Si446x_receiveNoLock(radio,
task_object->base_frequency,
task_object->step_hz,
task_object->channel,
task_object->squelch,
MOD_AFSK);
/* TODO: If decoder is not running error out. */
Si446x_receiveNoLock(chan, sq, MOD_AFSK);
pktStartDecoder(handler->radio);
pktStartDecoder(radio);
handler->rx_active = true;
/* Allow transmit requests. */
Si446x_unlockRadio(RADIO_RX);
pktReleaseRadio(radio);
break;
} /* End case MOD_AFSK. */
@ -167,6 +177,8 @@ THD_FUNCTION(pktRadioManager, arg) {
pp->radio_chan = task_object->channel;
pp->radio_pwr = task_object->tx_power;
/* Give each send a sequence number. */
pp->tx_seq = ++handler->radio_tx_config.seq_num;
/* TODO: Don't use fixed RSSI. */
pp->cca_rssi = 0x4F;
@ -186,7 +198,7 @@ THD_FUNCTION(pktRadioManager, arg) {
thread_t *decoder = NULL;
switch(task_object->type) {
case MOD_AFSK: {
Si446x_disableReceive();
Si446x_disableReceive(radio);
esp = pktGetEventSource((AFSKDemodDriver *)handler->link_controller);
pktRegisterEventListener(esp, &el, USR_COMMAND_ACK, DEC_CLOSE_EXEC);
decoder = ((AFSKDemodDriver *)(handler->link_controller))->decoder_thd;
@ -477,7 +489,7 @@ void pktReleaseRadio(radio_unit_t radio) {
*
* @param[in] base_freq Radio base frequency.
* @param[in] step Radio channel step frequency.
* @param[in] chan Radio channel nu,ber.
* @param[in] chan Radio channel number.
*
* @api
*/
@ -491,6 +503,7 @@ radio_freq_t pktComputeOperatingFrequency(radio_freq_t base_freq,
* @brief Enable receive on a radio.
* @notes This is the API interface to the radio LLD.
* @notes Currently just map directly to 446x driver.
* @notes In future would implement a lookup and VMT to access radio methods.
*
* @param[in] radio radio unit ID.
*
@ -513,18 +526,30 @@ bool pktLLDsendPacket(packet_t pp, mod_t type) {
}
/**
* @brief Send a packet via radio.
* @brief Resume reception paused by transmit task.
* @notes This is the API interface to the radio LLD.
* @notes Currently just map directly to 446x driver.
* @notes In future would implement a lookup and VMT to access radio methods.
*
* @param[in] radio radio unit ID.
* @param[in] step Radio channel step frequency.
* @param[in] chan Radio channel number.
*
* @return status of the operation
* @retval true operation succeeded.
* retval false operation failed.
*
* @notapi
*/
bool pktLLDresumeReceive(radio_unit_t radio) {
return Si4464_resumeReceive(radio);
packet_svc_t *handler = pktGetServiceObject(radio);
chDbgAssert(handler != NULL, "invalid radio ID");
radio_freq_t freq = handler->radio_rx_config.base_frequency;
channel_hz_t step = handler->radio_rx_config.step_hz;
radio_ch_t chan = handler->radio_rx_config.channel;
radio_squelch_t rssi = handler->radio_rx_config.squelch;
mod_t mod = handler->radio_rx_config.type;
return Si4464_resumeReceive(radio, freq, step, chan, rssi, mod);
}
/** @} */

Wyświetl plik

@ -84,6 +84,7 @@ struct radioTask {
packet_t packet_out;
uint8_t tx_power;
uint32_t tx_speed;
uint8_t seq_num;
};
/*===========================================================================*/

Wyświetl plik

@ -50,7 +50,7 @@
bool pktServiceCreate(radio_unit_t radio) {
/*
* TODO: This should lookup radio and assign handler (RPKTDx).
* Get service object maps radio IDs to service objects
*/
packet_svc_t *handler = pktGetServiceObject(radio);
if(handler == NULL)
@ -66,9 +66,9 @@ bool pktServiceCreate(radio_unit_t radio) {
memset(&handler->radio_rx_config, 0, sizeof(radio_task_object_t));
memset(&handler->radio_tx_config, 0, sizeof(radio_task_object_t));
/* Set parameters. */
//handler->radio_rx_config.radio_id = radio;
/* Set flags and radio ID. */
handler->rx_active = false;
handler->radio_init = false;
handler->radio = radio;
/* Set service semaphore to idle state. */
@ -93,7 +93,7 @@ bool pktServiceCreate(radio_unit_t radio) {
*
*@return result of operation.
*@retval true service was released.
*@retval false service state is incorrect.
*@retval false service state is incorrect or invalid radio ID.
*
* @api
*/

Wyświetl plik

@ -79,6 +79,8 @@ typedef struct packetHandlerData {
radio_unit_t radio;
bool radio_init;
/**
* @brief Radio receiver operating parameters.
*/

Wyświetl plik

@ -66,7 +66,7 @@
#define EVT_PKT_FAILED_CB_THD EVENT_MASK(EVT_PRIORITY_BASE + 7)
#define EVT_PWM_NO_DATA EVENT_MASK(EVT_PRIORITY_BASE + 8)
#define EVT_PWM_FIFO_SENT EVENT_MASK(EVT_PRIORITY_BASE + 9)
#define EVT_AFSK_START_FAIL EVENT_MASK(EVT_PRIORITY_BASE + 9)
#define EVT_RADIO_CCA_OPEN EVENT_MASK(EVT_PRIORITY_BASE + 10)
#define EVT_PWM_QUEUE_FULL EVENT_MASK(EVT_PRIORITY_BASE + 11)
@ -91,9 +91,9 @@
#define EVT_AFSK_DECODE_DONE EVENT_MASK(EVT_PRIORITY_BASE + 27)
#define EVT_RADIO_CCA_SPIKE EVENT_MASK(EVT_PRIORITY_BASE + 28)
//#define EVT_SERIAL_PKT_OUT_END EVENT_MASK(EVT_PRIORITY_BASE + 29)
#define EVT_PKT_BUFFER_MGR_FAIL EVENT_MASK(EVT_PRIORITY_BASE + 29)
#define EVT_ICU_OUT_OF_RANGE EVENT_MASK(EVT_PRIORITY_BASE + 30)
//#define EVT_TBA EVENT_MASK(EVT_PRIORITY_BASE + 31)
#define EVT_PKT_CBK_MGR_FAIL EVENT_MASK(EVT_PRIORITY_BASE + 31)
/* Decoder thread events (sent from initiator to decoder). */

Wyświetl plik

@ -137,6 +137,9 @@ struct packet_s {
radio_pwr_t radio_pwr;
radio_squelch_t cca_rssi;
uint16_t preamble_size;
uint8_t tx_seq;
/* TODO: Set size of name with definition. */
char tx_thd_name[16];
/* Frame length without CRC. */
int frame_len;
@ -152,8 +155,8 @@ struct packet_s {
/* For I & S frames: 8 or 128 if known. 0 if unknown. */
int modulo;
/* Raw frame contents, without the CRC. */
unsigned char frame_data[AX25_MAX_PACKET_LEN+1];
/* Raw frame contents, without the CRC plus one byte if \0 appended. */
unsigned char frame_data[AX25_MAX_PACKET_LEN + 1];
/* Will get stomped on if above overflows. */
int magic2;

Wyświetl plik

@ -114,7 +114,9 @@ void pktWrite(uint8_t *buf, uint32_t len) {
chnWrite((BaseSequentialStream*)SERIAL_CFG_DEBUG_DRIVER, buf, len);
}
void pktConfigureRadioGPIO() {
void pktConfigureRadioGPIO(radio_unit_t radio) {
/* TODO: Implement hardware mapping. */
(void)radio;
// Configure Radio pins
palSetLineMode(LINE_SPI_SCK, PAL_MODE_ALTERNATE(6) | PAL_STM32_OSPEED_HIGHEST); // SCK
palSetLineMode(LINE_SPI_MISO, PAL_MODE_ALTERNATE(6) | PAL_STM32_OSPEED_HIGHEST); // MISO
@ -134,8 +136,9 @@ void pktConfigureRadioGPIO() {
chThdSleep(TIME_MS2I(10)); // Wait for transceiver to power up
}
void pktDeconfigureRadioGPIO() {
void pktDeconfigureRadioGPIO(radio_unit_t radio) {
/* TODO: Implement hardware mapping. */
(void)radio;
palSetLineMode(LINE_SPI_SCK, PAL_MODE_INPUT_PULLDOWN); // SCK
palSetLineMode(LINE_SPI_MISO, PAL_MODE_INPUT_PULLDOWN); // MISO
palSetLineMode(LINE_SPI_MOSI, PAL_MODE_INPUT_PULLDOWN); // MOSI

Wyświetl plik

@ -45,8 +45,8 @@
#define LINE_SPI_MISO PAL_LINE(GPIOB, 4U)
#define LINE_SPI_MOSI PAL_LINE(GPIOB, 5U)
#define Si446x_MIN_FREQ 144000000 /* Minimum allowed frequency in Hz */
#define Si446x_MAX_FREQ 148000000 /* Maximum allowed frequency in Hz */
#define Si446x_MIN_2M_FREQ 144000000 /* Minimum allowed frequency in Hz */
#define Si446x_MAX_2M_FREQ 148000000 /* Maximum allowed frequency in Hz */
#define Si446x_CLK STM32_HSECLK /* Oscillator frequency in Hz */
#define Si446x_CLK_OFFSET 22 /* Oscillator frequency drift in ppm */
#define Si446x_CLK_TCXO_EN true /* Set this true, if a TCXO is used, false for XTAL */
@ -121,8 +121,8 @@ extern "C" {
void dbgWrite(uint8_t level, uint8_t *buf, uint32_t len);
int dbgPrintf(uint8_t level, const char *format, ...);
void pktWrite(uint8_t *buf, uint32_t len);
void pktConfigureRadioGPIO(void);
void pktDeconfigureRadioGPIO(void);
void pktConfigureRadioGPIO(radio_unit_t radio);
void pktDeconfigureRadioGPIO(radio_unit_t radio);
#ifdef __cplusplus
}
#endif

Wyświetl plik

@ -14,6 +14,7 @@
#include "pi2c.h"
#include "si446x.h"
#include "pflash.h"
#include "pkttypes.h"
static dataPoint_t dataPoints[2];
static dataPoint_t* lastDataPoint;
@ -186,7 +187,7 @@ static void getSensors(dataPoint_t* tp)
#endif
// Measure various temperature sensors
tp->stm32_temp = stm32_get_temp();
tp->si446x_temp = Si446x_getLastTemperature();
tp->si446x_temp = Si446x_getLastTemperature(PKT_RADIO_1);
// Measure light intensity from OV5640
tp->light_intensity = OV5640_getLastLightIntensity() & 0xFFFF;

Wyświetl plik

@ -325,7 +325,7 @@ static void transmit_image_packet(const uint8_t *image, uint32_t image_len, thd_
if(i == packet_id) {
// Sync byte, CRC and FEC of SSDV not transmitted (because its not neccessary inside an APRS packet)
base91_encode(&pkt[6], pkt_base91, 174);
/* TODO: Check for failure to get packet (NULL). */
packet_t packet = aprs_encode_data_packet(conf->call, conf->path, 'I', pkt_base91);
transmitOnRadio(packet,
conf->radio_conf.freq,

Wyświetl plik

@ -55,7 +55,7 @@ THD_FUNCTION(logThread, arg)
// Encode Base91
uint8_t pkt_base91[BASE91LEN(sizeof(dataPoint_t))];
base91_encode((uint8_t*)log, pkt_base91, sizeof(dataPoint_t));
/* TODO: Check for failure to get packet (NULL). */
// Encode and transmit log packet
packet_t packet = aprs_encode_data_packet(conf->call, conf->path, 'L', pkt_base91); // Encode packet

Wyświetl plik

@ -41,6 +41,7 @@ THD_FUNCTION(posThread, arg)
TRACE_INFO("POS > Transmit position");
// Encode/Transmit position packet
/* TODO: Check for failure to get packet (NULL). */
packet_t packet = aprs_encode_position(conf->call, conf->path, conf->symbol, dataPoint);
transmitOnRadio(packet,
conf->radio_conf.freq,
@ -51,6 +52,7 @@ THD_FUNCTION(posThread, arg)
chThdSleep(TIME_S2I(5));
// Encode/Transmit APRSD packet
/* TODO: Check for failure to get packet (NULL). */
packet_t pp = aprs_encode_query_answer_aprsd(conf->call, conf->path, conf->call);
transmitOnRadio(pp,
conf->radio_conf.freq,
@ -70,6 +72,7 @@ THD_FUNCTION(posThread, arg)
for(uint8_t type=0; type<4; type++)
{
packet = aprs_encode_telemetry_configuration(conf->call, conf->path, type);
/* TODO: Check for failure to get packet (NULL). */
transmitOnRadio(packet,
conf->radio_conf.freq,
conf->radio_conf.step,

Wyświetl plik

@ -58,11 +58,11 @@ if(pktIsBufferValidAX25Frame(pkt_buff)) {
}
}
void start_rx_thread(radio_unit_t radio, uint32_t freq, uint16_t step,
void start_rx_thread(radio_unit_t radio, uint32_t base_freq, uint16_t step,
radio_ch_t chan, uint8_t rssi) {
if(freq == FREQ_APRS_DYNAMIC) {
freq = getAPRSRegionFrequency(); // Get transmission frequency by geofencing
if(base_freq == FREQ_APRS_DYNAMIC) {
base_freq = getAPRSRegionFrequency(); // Get transmission frequency by geofencing
/* If using geofence ignore channel and step for now. */
chan = 0;
step = 0;
@ -71,7 +71,7 @@ void start_rx_thread(radio_unit_t radio, uint32_t freq, uint16_t step,
/* Open packet radio service. */
msg_t omsg = pktOpenRadioReceive(radio,
MOD_AFSK,
freq,
base_freq,
step);
if(omsg != MSG_OK) {
@ -93,19 +93,23 @@ void start_rx_thread(radio_unit_t radio, uint32_t freq, uint16_t step,
/*
*
*/
bool transmitOnRadio(packet_t pp, radio_freq_t freq,
bool transmitOnRadio(packet_t pp, radio_freq_t base_freq,
channel_hz_t step, radio_ch_t chan,
radio_pwr_t pwr, mod_t mod) {
/* TODO: This should have the radio ID. For now just fix it. */
/* TODO: This should select a radio by frequency. For now just use 1. */
radio_unit_t radio = PKT_RADIO_1;
if(freq == FREQ_APRS_DYNAMIC) {
freq = getAPRSRegionFrequency(); // Get transmission frequency by geofencing
if(pktIsTransmitOpen(radio)) {
TRACE_WARN( "RAD > Transmit is not open on radio");
return false;
}
if(base_freq == FREQ_APRS_DYNAMIC) {
base_freq = getAPRSRegionFrequency(); // Get transmission frequency by geofencing
step = 0;
chan = 0;
}
if(freq == FREQ_APRS_RECEIVE) {
if(base_freq == FREQ_APRS_RECEIVE) {
/* TODO: Get current RX frequency (if valid) and use that. */
}
@ -115,14 +119,14 @@ bool transmitOnRadio(packet_t pp, radio_freq_t freq,
if(len) // Message length is not zero
{
/* Check frequency. */
if(!Si446x_isFrequencyInBand(freq, step, chan)) {
if(!Si446x_isFrequencyInBand(radio, base_freq, step, chan)) {
TRACE_ERROR("RAD > Transmit base frequency of %d.%03d MHz is invalid",
freq/1000000, (freq%1000000)/1000);
base_freq/1000000, (base_freq%1000000)/1000);
return false;
}
radio_freq_t op_freq = pktComputeOperatingFrequency(freq, step, chan);
radio_freq_t op_freq = pktComputeOperatingFrequency(base_freq, step, chan);
TRACE_INFO( "RAD > Transmit packet on %d.%03d MHz (ch %d),"
" Pwr %d, %s, %d byte",
op_freq/1000000, (op_freq%1000000)/1000,
@ -150,7 +154,7 @@ bool transmitOnRadio(packet_t pp, radio_freq_t freq,
rt.handler = handler;
rt.command = PKT_RADIO_TX_SEND;
rt.type = mod;
rt.base_frequency = freq;
rt.base_frequency = base_freq;
rt.step_hz = step;
rt.channel = chan;
rt.tx_power = pwr;
@ -171,7 +175,7 @@ bool transmitOnRadio(packet_t pp, radio_freq_t freq,
} else {
TRACE_ERROR("RAD > It is nonsense to transmit 0 bits, %d.%03d MHz, Pwr dBm, %s, %d byte",
freq/1000000, (freq%1000000)/1000, pwr,
base_freq/1000000, (base_freq%1000000)/1000, pwr,
getModulation(mod), len);
ax25_delete(pp);
}