diff --git a/flight/libraries/inc/packet_handler.h b/flight/libraries/inc/packet_handler.h deleted file mode 100644 index 3df4f8609..000000000 --- a/flight/libraries/inc/packet_handler.h +++ /dev/null @@ -1,111 +0,0 @@ -/** - ****************************************************************************** - * @addtogroup OpenPilotSystem OpenPilot System - * @{ - * @addtogroup OpenPilotLibraries OpenPilot System Libraries - * @{ - * - * @file packet_handler.h - * @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2012. - * @brief A packet handler for handeling radio packet transmission. - * @see The GNU Public License (GPL) Version 3 - * - *****************************************************************************/ -/* - * This program is free software; you can redistribute it and/or modify - * it under the terms of the GNU General Public License as published by - * the Free Software Foundation; either version 3 of the License, or - * (at your option) any later version. - * - * This program is distributed in the hope that it will be useful, but - * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY - * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License - * for more details. - * - * You should have received a copy of the GNU General Public License along - * with this program; if not, write to the Free Software Foundation, Inc., - * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA - */ - -#ifndef __PACKET_HANDLER_H__ -#define __PACKET_HANDLER_H__ - -#include -#include -#include -#include - -// Public defines / macros -#define PHPacketSize(p) ((uint8_t *)(p->data) + p->header.data_size - (uint8_t *)p) -#define PHPacketSizeECC(p) ((uint8_t *)(p->data) + p->header.data_size + RS_ECC_NPARITY - (uint8_t *)p) - -// Public types -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_DATA, // data packet (packet contains user data) - PACKET_TYPE_DUPLICATE_DATA, // a duplicate data packet - PACKET_TYPE_PPM, // PPM relay values - PACKET_TYPE_ACK, // Acknowlege the receipt of a packet - PACKET_TYPE_NACK, // Acknowlege the receipt of an uncorrectable packet -} PHPacketType; - -typedef struct { - uint32_t destination_id; - portTickType prev_tx_time; - uint16_t seq_num; - uint8_t type; - uint8_t data_size; -} 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) -typedef struct { - PHPacketHeader header; - uint8_t data[PH_MAX_DATA + RS_ECC_NPARITY]; -} PHPacket, *PHPacketHandle; - -#define PH_ACK_NACK_DATA_SIZE(p) ((uint8_t *)((p)->ecc) - (uint8_t *)(((PHPacketHandle)(p))->data)) -typedef struct { - PHPacketHeader header; - 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; - int16_t channels[PIOS_RFM22B_RCVR_MAX_CHANNELS]; - uint8_t ecc[RS_ECC_NPARITY]; -} PHPpmPacket, *PHPpmPacketHandle; - -#define PH_STATUS_DATA_SIZE(p) ((uint8_t *)((p)->ecc) - (uint8_t *)(((PHPacketHandle)(p))->data)) -typedef struct { - PHPacketHeader header; - uint32_t source_id; - uint8_t link_quality; - int8_t received_rssi; - uint8_t ecc[RS_ECC_NPARITY]; -} PHStatusPacket, *PHStatusPacketHandle; - -#define PH_CONNECTION_DATA_SIZE(p) ((uint8_t *)((p)->ecc) - (uint8_t *)(((PHPacketHandle)(p))->data)) -typedef struct { - PHPacketHeader header; - uint32_t source_id; - uint32_t min_frequency; - uint32_t max_frequency; - uint32_t channel_spacing; - portTickType status_rx_time; - OPLinkSettingsMainPortOptions main_port; - OPLinkSettingsFlexiPortOptions flexi_port; - OPLinkSettingsVCPPortOptions vcp_port; - OPLinkSettingsComSpeedOptions com_speed; - uint8_t ecc[RS_ECC_NPARITY]; -} PHConnectionPacket, *PHConnectionPacketHandle; - -#endif // __PACKET_HANDLER_H__ - -/** - * @} - * @} - */ diff --git a/flight/modules/OPLink/oplinkmod.c b/flight/modules/OPLink/oplinkmod.c index 9b2b068a2..2c976ac16 100644 --- a/flight/modules/OPLink/oplinkmod.c +++ b/flight/modules/OPLink/oplinkmod.c @@ -38,10 +38,12 @@ * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA */ +#include + +#include #include #include -#include #include #include diff --git a/flight/modules/RadioComBridge/RadioComBridge.c b/flight/modules/RadioComBridge/RadioComBridge.c index cbf3608ed..9724f6e3f 100644 --- a/flight/modules/RadioComBridge/RadioComBridge.c +++ b/flight/modules/RadioComBridge/RadioComBridge.c @@ -7,7 +7,7 @@ * @{ * * @file RadioComBridge.c - * @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2012. + * @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2012-2013. * @brief Bridges selected Com Port to the COM VCP emulated serial port * @see The GNU Public License (GPL) Version 3 * @@ -32,8 +32,6 @@ #include #include -#include -#include #include #include #include @@ -46,12 +44,6 @@ #include -// External functions -void PIOS_InitUartMainPort(); -void PIOS_InitUartFlexiPort(); -void PIOS_InitPPMMainPort(bool input); -void PIOS_InitPPMFlexiPort(bool input); - // **************** // Private constants @@ -75,12 +67,12 @@ typedef struct { xTaskHandle radioRxTaskHandle; // The UAVTalk connection on the com side. - UAVTalkConnection outUAVTalkCon; - UAVTalkConnection inUAVTalkCon; + UAVTalkConnection telemUAVTalkCon; + UAVTalkConnection radioUAVTalkCon; // Queue handles. - xQueueHandle gcsEventQueue; xQueueHandle uavtalkEventQueue; + xQueueHandle radioEventQueue; // Error statistics. uint32_t comTxErrors; @@ -91,9 +83,6 @@ typedef struct { // Should we parse UAVTalk? bool parseUAVTalk; - // We can only configure the hardware once. - bool configured; - // The current configured uart speed OPLinkSettingsComSpeedOptions comSpeed; } RadioComBridgeData; @@ -107,11 +96,9 @@ static void radioTxTask(void *parameters); static void radioRxTask(void *parameters); static int32_t UAVTalkSendHandler(uint8_t *buf, int32_t length); static int32_t RadioSendHandler(uint8_t *buf, int32_t length); -static void ProcessInputStream(UAVTalkConnection connectionHandle, uint8_t rxbyte); -static void queueEvent(xQueueHandle queue, void *obj, uint16_t instId, UAVObjEventType type); -static void configureComCallback(OPLinkSettingsRemoteMainPortOptions main_port, OPLinkSettingsRemoteFlexiPortOptions flexi_port, - OPLinkSettingsRemoteVCPPortOptions vcp_port, OPLinkSettingsComSpeedOptions com_speed); -static void updateSettings(OPLinkSettingsData *oplinkSettings); +static void ProcessTelemetryStream(UAVTalkConnection inConnectionHandle, UAVTalkConnection outConnectionHandle, uint8_t rxbyte); +static void ProcessRadioStream(UAVTalkConnection inConnectionHandle, UAVTalkConnection outConnectionHandle, uint8_t rxbyte); +static void objectPersistenceUpdatedCb(UAVObjEvent *objEv); // **************** // Private variables @@ -119,7 +106,7 @@ static void updateSettings(OPLinkSettingsData *oplinkSettings); static RadioComBridgeData *data; /** - * Start the module + * @brief Start the module * * @return -1 if initialisation failed, 0 on success */ @@ -130,26 +117,10 @@ static int32_t RadioComBridgeStart(void) OPLinkSettingsData oplinkSettings; OPLinkSettingsGet(&oplinkSettings); - // Set the baudrates, etc. - bool is_coordinator = PIOS_RFM22B_IsCoordinator(pios_rfm22b_id); - if (is_coordinator) { - // Set the frequency range. - PIOS_RFM22B_SetFrequencyRange(pios_rfm22b_id, oplinkSettings.MinFrequency, oplinkSettings.MaxFrequency, oplinkSettings.ChannelSpacing); - - // Set the com baud rates. - updateSettings(&oplinkSettings); - - // Reinitilize the modem. - PIOS_RFM22B_Reinit(pios_rfm22b_id); - - // We will not parse/send UAVTalk if any ports are configured as Serial (except for over the USB HID port). - data->parseUAVTalk = ((oplinkSettings.MainPort != OPLINKSETTINGS_MAINPORT_SERIAL) && - (oplinkSettings.FlexiPort != OPLINKSETTINGS_FLEXIPORT_SERIAL) && - (oplinkSettings.VCPPort != OPLINKSETTINGS_VCPPORT_SERIAL)); - } else { - // Configure the com port configuration callback on the remote modem. - PIOS_RFM22B_SetComConfigCallback(pios_rfm22b_id, &configureComCallback); - } + // We will not parse/send UAVTalk if any ports are configured as Serial (except for over the USB HID port). + data->parseUAVTalk = ((oplinkSettings.MainPort != OPLINKSETTINGS_MAINPORT_SERIAL) && + (oplinkSettings.FlexiPort != OPLINKSETTINGS_FLEXIPORT_SERIAL) && + (oplinkSettings.VCPPort != OPLINKSETTINGS_VCPPORT_SERIAL)); // Set the maximum radio RF power. switch (oplinkSettings.MaxRFPower) { @@ -182,8 +153,9 @@ static int32_t RadioComBridgeStart(void) break; } - // Set the initial frequency. - PIOS_RFM22B_SetInitialFrequency(pios_rfm22b_id, oplinkSettings.InitFrequency); + // Configure our UAVObjects for updates. + UAVObjConnectQueue(UAVObjGetByID(OPLINKSTATUS_OBJID), data->uavtalkEventQueue, EV_UPDATED | EV_UPDATED_MANUAL | EV_UPDATE_REQ); + UAVObjConnectQueue(UAVObjGetByID(OBJECTPERSISTENCE_OBJID), data->uavtalkEventQueue, EV_UPDATED | EV_UPDATED_MANUAL); // Start the primary tasks for receiving/sending UAVTalk packets from the GCS. xTaskCreate(telemetryTxTask, (signed char *)"telemetryTxTask", STACK_SIZE_BYTES, NULL, TASK_PRIORITY, &(data->telemetryTxTaskHandle)); @@ -205,7 +177,7 @@ static int32_t RadioComBridgeStart(void) } /** - * Initialise the module + * @brief Initialise the module * * @return -1 if initialisation failed on success */ @@ -221,12 +193,16 @@ static int32_t RadioComBridgeInitialize(void) OPLinkStatusInitialize(); ObjectPersistenceInitialize(); + // Configure the UAVObject callbacks + ObjectPersistenceConnectCallback(&objectPersistenceUpdatedCb); + // Initialise UAVTalk - data->outUAVTalkCon = UAVTalkInitialize(&UAVTalkSendHandler); - data->inUAVTalkCon = UAVTalkInitialize(&RadioSendHandler); + data->telemUAVTalkCon = UAVTalkInitialize(&UAVTalkSendHandler); + data->radioUAVTalkCon = UAVTalkInitialize(&RadioSendHandler); // Initialize the queues. data->uavtalkEventQueue = xQueueCreate(EVENT_QUEUE_SIZE, sizeof(UAVObjEvent)); + data->radioEventQueue = xQueueCreate(EVENT_QUEUE_SIZE, sizeof(UAVObjEvent)); // Configure our UAVObjects for updates. UAVObjConnectQueue(UAVObjGetByID(OPLINKSTATUS_OBJID), data->uavtalkEventQueue, EV_UPDATED | EV_UPDATED_MANUAL | EV_UPDATE_REQ); @@ -240,7 +216,6 @@ static int32_t RadioComBridgeInitialize(void) data->comTxRetries = 0; data->UAVTalkErrors = 0; data->parseUAVTalk = true; - data->configured = false; data->comSpeed = OPLINKSETTINGS_COMSPEED_9600; PIOS_COM_RADIO = PIOS_COM_RFM22B; @@ -249,7 +224,7 @@ static int32_t RadioComBridgeInitialize(void) MODULE_INITCALL(RadioComBridgeInitialize, RadioComBridgeStart); /** - * Telemetry transmit task, regular priority + * @brief Telemetry transmit task, regular priority * * @param[in] parameters The task parameters */ @@ -269,7 +244,7 @@ static void telemetryTxTask(__attribute__((unused)) void *parameters) uint32_t retries = 0; int32_t success = -1; while (retries < MAX_RETRIES && success == -1) { - success = UAVTalkSendObject(data->outUAVTalkCon, ev.obj, 0, 0, RETRY_TIMEOUT_MS) == 0; + success = UAVTalkSendObject(data->telemUAVTalkCon, ev.obj, 0, 0, RETRY_TIMEOUT_MS) == 0; if (!success) { ++retries; } @@ -280,7 +255,7 @@ static void telemetryTxTask(__attribute__((unused)) void *parameters) uint32_t retries = 0; int32_t success = -1; while (retries < MAX_RETRIES && success == -1) { - success = UAVTalkSendAck(data->outUAVTalkCon, ev.obj, ev.instId) == 0; + success = UAVTalkSendAck(data->telemUAVTalkCon, ev.obj, ev.instId) == 0; if (!success) { ++retries; } @@ -291,7 +266,7 @@ static void telemetryTxTask(__attribute__((unused)) void *parameters) uint32_t retries = 0; int32_t success = -1; while (retries < MAX_RETRIES && success == -1) { - success = UAVTalkSendNack(data->outUAVTalkCon, UAVObjGetID(ev.obj)) == 0; + success = UAVTalkSendNack(data->telemUAVTalkCon, UAVObjGetID(ev.obj)) == 0; if (!success) { ++retries; } @@ -303,7 +278,7 @@ static void telemetryTxTask(__attribute__((unused)) void *parameters) } /** - * Radio tx task. Receive data packets from the com port and send to the radio. + * @brief Radio tx task. Receive data packets from the com port and send to the radio. * * @param[in] parameters The task parameters */ @@ -315,23 +290,51 @@ static void radioTxTask(__attribute__((unused)) void *parameters) PIOS_WDG_UpdateFlag(PIOS_WDG_RADIOTX); #endif - // Wait until the com port is available. - if (data->parseUAVTalk || !PIOS_COM_TELEMETRY) { - vTaskDelay(5); - continue; - } + // Process the radio event queue, sending UAVObjects over the radio link as necessary. + UAVObjEvent ev; - // Read from the com port. - uint8_t serial_data[1]; - uint16_t bytes_to_process = PIOS_COM_ReceiveBuffer(PIOS_COM_TELEMETRY, serial_data, sizeof(serial_data), MAX_PORT_DELAY); - if (bytes_to_process > 0) { - PIOS_COM_SendBufferNonBlocking(PIOS_COM_RADIO, serial_data, bytes_to_process); + // Wait for queue message + if (xQueueReceive(data->radioEventQueue, &ev, MAX_PORT_DELAY) == pdTRUE) { + if ((ev.event == EV_UPDATED) || (ev.event == EV_UPDATE_REQ)) { + // Send update (with retries) + uint32_t retries = 0; + int32_t success = -1; + while (retries < MAX_RETRIES && success == -1) { + success = UAVTalkSendObject(data->radioUAVTalkCon, ev.obj, 0, 0, RETRY_TIMEOUT_MS) == 0; + if (!success) { + ++retries; + } + } + data->comTxRetries += retries; + } else if (ev.event == EV_SEND_ACK) { + // Send the ACK + uint32_t retries = 0; + int32_t success = -1; + while (retries < MAX_RETRIES && success == -1) { + success = UAVTalkSendAck(data->radioUAVTalkCon, ev.obj, ev.instId) == 0; + if (!success) { + ++retries; + } + } + data->comTxRetries += retries; + } else if (ev.event == EV_SEND_NACK) { + // Send the NACK + uint32_t retries = 0; + int32_t success = -1; + while (retries < MAX_RETRIES && success == -1) { + success = UAVTalkSendNack(data->radioUAVTalkCon, UAVObjGetID(ev.obj)) == 0; + if (!success) { + ++retries; + } + } + data->comTxRetries += retries; + } } } } /** - * Radio rx task. Receive data packets from the radio and pass them on. + * @brief Radio rx task. Receive data packets from the radio and pass them on. * * @param[in] parameters The task parameters */ @@ -345,22 +348,16 @@ static void radioRxTask(__attribute__((unused)) void *parameters) uint8_t serial_data[1]; uint16_t bytes_to_process = PIOS_COM_ReceiveBuffer(PIOS_COM_RADIO, serial_data, sizeof(serial_data), MAX_PORT_DELAY); if (bytes_to_process > 0) { - // Either pass the data through the UAVTalk parser, or just send it to the radio (if we're doing raw comms). - if (data->parseUAVTalk) { - for (uint8_t i = 0; i < bytes_to_process; i++) { - if (UAVTalkRelayInputStream(data->outUAVTalkCon, serial_data[i]) == UAVTALK_STATE_ERROR) { - data->UAVTalkErrors++; - } - } - } else if (PIOS_COM_TELEMETRY) { - PIOS_COM_SendBufferNonBlocking(PIOS_COM_TELEMETRY, serial_data, bytes_to_process); + // Pass the data through the UAVTalk parser. + for (uint8_t i = 0; i < bytes_to_process; i++) { + ProcessRadioStream(data->radioUAVTalkCon, data->telemUAVTalkCon, serial_data[i]); } } } } /** - * Receive telemetry from the USB/COM port. + * @brief Receive telemetry from the USB/COM port. * * @param[in] parameters The task parameters */ @@ -383,7 +380,7 @@ static void telemetryRxTask(__attribute__((unused)) void *parameters) uint16_t bytes_to_process = PIOS_COM_ReceiveBuffer(inputPort, serial_data, sizeof(serial_data), MAX_PORT_DELAY); if (bytes_to_process > 0) { for (uint8_t i = 0; i < bytes_to_process; i++) { - ProcessInputStream(data->inUAVTalkCon, serial_data[i]); + ProcessTelemetryStream(data->telemUAVTalkCon, data->radioUAVTalkCon, serial_data[i]); } } } else { @@ -393,7 +390,7 @@ static void telemetryRxTask(__attribute__((unused)) void *parameters) } /** - * Transmit data buffer to the com port. + * @brief Transmit data buffer to the com port. * * @param[in] buf Data buffer to send * @param[in] length Length of buffer @@ -439,272 +436,109 @@ static int32_t RadioSendHandler(uint8_t *buf, int32_t length) } /** - * Process a byte of data received + * @brief Process a byte of data received on the telemetry stream * - * @param[in] connectionHandle The UAVTalk connection handle + * @param[in] inConnectionHandle The UAVTalk connection handle on the telemetry port + * @param[in] outConnectionHandle The UAVTalk connection handle on the radio port. * @param[in] rxbyte The received byte. */ -static void ProcessInputStream(UAVTalkConnection connectionHandle, uint8_t rxbyte) +static void ProcessTelemetryStream(UAVTalkConnection inConnectionHandle, UAVTalkConnection outConnectionHandle, uint8_t rxbyte) { // Keep reading until we receive a completed packet. - UAVTalkRxState state = UAVTalkRelayInputStream(connectionHandle, rxbyte); - UAVTalkConnectionData *connection = (UAVTalkConnectionData *)(connectionHandle); - UAVTalkInputProcessor *iproc = &(connection->iproc); + UAVTalkRxState state = UAVTalkProcessInputStream(inConnectionHandle, rxbyte); - if (state == UAVTALK_STATE_COMPLETE) { - // Is this a local UAVObject? - // We only generate GcsReceiver ojects, we don't consume them. - if ((iproc->obj != NULL) && (iproc->objId != GCSRECEIVER_OBJID)) { - // We treat the ObjectPersistence object differently - if (iproc->objId == OBJECTPERSISTENCE_OBJID) { - // Unpack object, if the instance does not exist it will be created! - UAVObjUnpack(iproc->obj, iproc->instId, connection->rxBuffer); - - // Get the ObjectPersistence object. - ObjectPersistenceData obj_per; - ObjectPersistenceGet(&obj_per); - - // Is this concerning or setting object? - if (obj_per.ObjectID == OPLINKSETTINGS_OBJID) { - // Queue up the ACK. - queueEvent(data->uavtalkEventQueue, (void *)iproc->obj, iproc->instId, EV_SEND_ACK); - - // Is this a save, load, or delete? - bool success = true; - switch (obj_per.Operation) { - case OBJECTPERSISTENCE_OPERATION_LOAD: - { -#if defined(PIOS_INCLUDE_FLASH_LOGFS_SETTINGS) - // Load the settings. - void *obj = UAVObjGetByID(obj_per.ObjectID); - if (obj == 0) { - success = false; - } else { - // Load selected instance - success = (UAVObjLoad(obj, obj_per.InstanceID) == 0); - } -#endif - break; - } - case OBJECTPERSISTENCE_OPERATION_SAVE: - { -#if defined(PIOS_INCLUDE_FLASH_LOGFS_SETTINGS) - void *obj = UAVObjGetByID(obj_per.ObjectID); - if (obj == 0) { - success = false; - } else { - // Save selected instance - success = UAVObjSave(obj, obj_per.InstanceID) == 0; - } -#endif - break; - } - case OBJECTPERSISTENCE_OPERATION_DELETE: - { -#if defined(PIOS_INCLUDE_FLASH_LOGFS_SETTINGS) - void *obj = UAVObjGetByID(obj_per.ObjectID); - if (obj == 0) { - success = false; - } else { - // Save selected instance - success = UAVObjDelete(obj, obj_per.InstanceID) == 0; - } -#endif - break; - } - default: - break; - } - if (success == true) { - obj_per.Operation = OBJECTPERSISTENCE_OPERATION_COMPLETED; - ObjectPersistenceSet(&obj_per); - } - } - } else { - switch (iproc->type) { - case UAVTALK_TYPE_OBJ: - // Unpack object, if the instance does not exist it will be created! - UAVObjUnpack(iproc->obj, iproc->instId, connection->rxBuffer); - break; - case UAVTALK_TYPE_OBJ_REQ: - // Queue up an object send request. - queueEvent(data->uavtalkEventQueue, (void *)iproc->obj, iproc->instId, EV_UPDATE_REQ); - break; - case UAVTALK_TYPE_OBJ_ACK: - if (UAVObjUnpack(iproc->obj, iproc->instId, connection->rxBuffer) == 0) { - // Queue up an ACK - queueEvent(data->uavtalkEventQueue, (void *)iproc->obj, iproc->instId, EV_SEND_ACK); - } - break; - } - } - } - } else if (state == UAVTALK_STATE_ERROR) { + if (state == UAVTALK_STATE_ERROR) { data->UAVTalkErrors++; + } else if (state == UAVTALK_STATE_COMPLETE) { + UAVTalkRelayPacket(inConnectionHandle, outConnectionHandle); + } +} - // Send a NACK if required. - if ((iproc->obj) && (iproc->type == UAVTALK_TYPE_OBJ_ACK)) { - // Queue up a NACK - queueEvent(data->uavtalkEventQueue, iproc->obj, iproc->instId, EV_SEND_NACK); +/** + * @brief Process a byte of data received on the radio data stream. + * + * @param[in] inConnectionHandle The UAVTalk connection handle on the radio port. + * @param[in] outConnectionHandle The UAVTalk connection handle on the telemetry port. + * @param[in] rxbyte The received byte. + */ +static void ProcessRadioStream(UAVTalkConnection inConnectionHandle, UAVTalkConnection outConnectionHandle, uint8_t rxbyte) +{ + // Keep reading until we receive a completed packet. + UAVTalkRxState state = UAVTalkProcessInputStreamQuiet(inConnectionHandle, rxbyte); + + if (state == UAVTALK_STATE_ERROR) { + data->UAVTalkErrors++; + } else if (state == UAVTALK_STATE_COMPLETE) { + // We only want to unpack certain objects from the remote modem. + uint32_t objId = UAVTalkGetPacketObjId(inConnectionHandle); + if (objId != OPLINKSTATUS_OBJID) { + UAVTalkRelayPacket(inConnectionHandle, outConnectionHandle); } } } /** - * Queue and event into an event queue. - * - * @param[in] queue The event queue - * @param[in] obj The data pointer - * @param[in] type The event type + * @brief Callback that is called when the ObjectPersistence UAVObject is changed. + * @param[in] objEv The event that precipitated the callback. */ -static void queueEvent(xQueueHandle queue, void *obj, uint16_t instId, UAVObjEventType type) +static void objectPersistenceUpdatedCb(__attribute__((unused)) UAVObjEvent *objEv) { - UAVObjEvent ev; + // Get the ObjectPersistence object. + ObjectPersistenceData obj_per; - ev.obj = (UAVObjHandle)obj; - ev.instId = instId; - ev.event = type; - xQueueSend(queue, &ev, portMAX_DELAY); -} + ObjectPersistenceGet(&obj_per); -/** - * Configure the output port based on a configuration event from the remote coordinator. - * - * @param[in] com_port The com port to configure - * @param[in] com_speed The com port speed - */ -static void configureComCallback(OPLinkSettingsRemoteMainPortOptions main_port, OPLinkSettingsRemoteFlexiPortOptions flexi_port, - OPLinkSettingsRemoteVCPPortOptions vcp_port, OPLinkSettingsComSpeedOptions com_speed) -{ - // Update the com baud rate - data->comSpeed = com_speed; - - // Get the settings. - OPLinkSettingsData oplinkSettings; - OPLinkSettingsGet(&oplinkSettings); - - switch (main_port) { - case OPLINKSETTINGS_REMOTEMAINPORT_DISABLED: - oplinkSettings.MainPort = OPLINKSETTINGS_MAINPORT_DISABLED; - break; - case OPLINKSETTINGS_REMOTEMAINPORT_SERIAL: - oplinkSettings.MainPort = OPLINKSETTINGS_MAINPORT_SERIAL; - break; - case OPLINKSETTINGS_REMOTEMAINPORT_PPM: - oplinkSettings.MainPort = OPLINKSETTINGS_MAINPORT_PPM; - break; - } - - switch (flexi_port) { - case OPLINKSETTINGS_REMOTEFLEXIPORT_DISABLED: - oplinkSettings.FlexiPort = OPLINKSETTINGS_FLEXIPORT_DISABLED; - break; - case OPLINKSETTINGS_REMOTEFLEXIPORT_SERIAL: - oplinkSettings.FlexiPort = OPLINKSETTINGS_FLEXIPORT_SERIAL; - break; - case OPLINKSETTINGS_REMOTEFLEXIPORT_PPM: - oplinkSettings.FlexiPort = OPLINKSETTINGS_FLEXIPORT_PPM; - break; - } - - switch (vcp_port) { - case OPLINKSETTINGS_REMOTEVCPPORT_DISABLED: - oplinkSettings.VCPPort = OPLINKSETTINGS_VCPPORT_DISABLED; - break; - case OPLINKSETTINGS_REMOTEVCPPORT_SERIAL: - oplinkSettings.VCPPort = OPLINKSETTINGS_VCPPORT_SERIAL; - break; - } - - // We will not parse/send UAVTalk if any ports are configured as Serial (except for over the USB HID port). - data->parseUAVTalk = ((oplinkSettings.MainPort != OPLINKSETTINGS_MAINPORT_SERIAL) && - (oplinkSettings.FlexiPort != OPLINKSETTINGS_FLEXIPORT_SERIAL) && - (oplinkSettings.VCPPort != OPLINKSETTINGS_VCPPORT_SERIAL)); - - // Update the OPLinkSettings object. - OPLinkSettingsSet(&oplinkSettings); - - // Perform the update. - updateSettings(&oplinkSettings); -} - -/** - * Update the oplink settings. - */ -static void updateSettings(OPLinkSettingsData *oplinkSettings) -{ - // We can only configure the hardware once. - if (data->configured) { - return; - } - data->configured = true; - - // Configure the main port - bool is_coordinator = PIOS_RFM22B_IsCoordinator(pios_rfm22b_id); - switch (oplinkSettings->MainPort) { - case OPLINKSETTINGS_MAINPORT_TELEMETRY: - case OPLINKSETTINGS_MAINPORT_SERIAL: - /* Configure the main port for uart serial */ - PIOS_InitUartMainPort(); - PIOS_COM_TELEMETRY = PIOS_COM_TELEM_UART_MAIN; - break; - case OPLINKSETTINGS_MAINPORT_PPM: - PIOS_InitPPMMainPort(is_coordinator); - break; - case OPLINKSETTINGS_MAINPORT_DISABLED: - break; - } - - // Configure the flexi port - switch (oplinkSettings->FlexiPort) { - case OPLINKSETTINGS_FLEXIPORT_TELEMETRY: - case OPLINKSETTINGS_FLEXIPORT_SERIAL: - /* Configure the flexi port as uart serial */ - PIOS_InitUartFlexiPort(); - PIOS_COM_TELEMETRY = PIOS_COM_TELEM_UART_FLEXI; - break; - case OPLINKSETTINGS_FLEXIPORT_PPM: - PIOS_InitPPMFlexiPort(is_coordinator); - break; - case OPLINKSETTINGS_FLEXIPORT_DISABLED: - break; - } - - // Configure the USB VCP port - switch (oplinkSettings->VCPPort) { - case OPLINKSETTINGS_VCPPORT_SERIAL: - PIOS_COM_TELEMETRY = PIOS_COM_TELEM_USB_VCP; - break; - case OPLINKSETTINGS_VCPPORT_DISABLED: - break; - } - - // Update the com baud rate. - uint32_t comBaud = 9600; - switch (data->comSpeed) { - case OPLINKSETTINGS_COMSPEED_2400: - comBaud = 2400; - break; - case OPLINKSETTINGS_COMSPEED_4800: - comBaud = 4800; - break; - case OPLINKSETTINGS_COMSPEED_9600: - comBaud = 9600; - break; - case OPLINKSETTINGS_COMSPEED_19200: - comBaud = 19200; - break; - case OPLINKSETTINGS_COMSPEED_38400: - comBaud = 38400; - break; - case OPLINKSETTINGS_COMSPEED_57600: - comBaud = 57600; - break; - case OPLINKSETTINGS_COMSPEED_115200: - comBaud = 115200; - break; - } - if (PIOS_COM_TELEMETRY) { - PIOS_COM_ChangeBaud(PIOS_COM_TELEMETRY, comBaud); + // Is this concerning or setting object? + if (obj_per.ObjectID == OPLINKSETTINGS_OBJID) { + // Is this a save, load, or delete? + bool success = true; + switch (obj_per.Operation) { + case OBJECTPERSISTENCE_OPERATION_LOAD: + { +#if defined(PIOS_INCLUDE_FLASH_LOGFS_SETTINGS) + // Load the settings. + void *obj = UAVObjGetByID(obj_per.ObjectID); + if (obj == 0) { + success = false; + } else { + // Load selected instance + success = (UAVObjLoad(obj, obj_per.InstanceID) == 0); + } +#endif + break; + } + case OBJECTPERSISTENCE_OPERATION_SAVE: + { +#if defined(PIOS_INCLUDE_FLASH_LOGFS_SETTINGS) + void *obj = UAVObjGetByID(obj_per.ObjectID); + if (obj == 0) { + success = false; + } else { + // Save selected instance + success = UAVObjSave(obj, obj_per.InstanceID) == 0; + } +#endif + break; + } + case OBJECTPERSISTENCE_OPERATION_DELETE: + { +#if defined(PIOS_INCLUDE_FLASH_LOGFS_SETTINGS) + void *obj = UAVObjGetByID(obj_per.ObjectID); + if (obj == 0) { + success = false; + } else { + // Save selected instance + success = UAVObjDelete(obj, obj_per.InstanceID) == 0; + } +#endif + break; + } + default: + break; + } + if (success == true) { + obj_per.Operation = OBJECTPERSISTENCE_OPERATION_COMPLETED; + ObjectPersistenceSet(&obj_per); + } } } diff --git a/flight/modules/System/systemmod.c b/flight/modules/System/systemmod.c index 9ff5dd7e0..874331d68 100644 --- a/flight/modules/System/systemmod.c +++ b/flight/modules/System/systemmod.c @@ -54,6 +54,9 @@ #include #include #include +#if defined(PIOS_INCLUDE_RFM22B) +#include +#endif // Flight Libraries #include @@ -80,7 +83,7 @@ #if defined(PIOS_SYSTEM_STACK_SIZE) #define STACK_SIZE_BYTES PIOS_SYSTEM_STACK_SIZE #else -#define STACK_SIZE_BYTES 924 +#define STACK_SIZE_BYTES 1024 #endif #define TASK_PRIORITY (tskIDLE_PRIORITY + 1) @@ -249,6 +252,54 @@ static void systemTask(__attribute__((unused)) void *parameters) UAVObjEvent ev; int delayTime = SYSTEM_UPDATE_PERIOD_MS / portTICK_RATE_MS / (LED_BLINK_RATE_HZ * 2); +#if defined(PIOS_INCLUDE_RFM22B) + // Update the OPLinkStatus UAVO + OPLinkStatusData oplinkStatus; + OPLinkStatusGet(&oplinkStatus); + + // Get the other device stats. + PIOS_RFM2B_GetPairStats(pios_rfm22b_id, oplinkStatus.PairIDs, oplinkStatus.PairSignalStrengths, OPLINKSTATUS_PAIRIDS_NUMELEM); + + // Get the stats from the radio device + struct rfm22b_stats radio_stats; + PIOS_RFM22B_GetStats(pios_rfm22b_id, &radio_stats); + + // Update the OPLInk status + static bool first_time = true; + static uint16_t prev_tx_count = 0; + static uint16_t prev_rx_count = 0; + oplinkStatus.HeapRemaining = xPortGetFreeHeapSize(); + oplinkStatus.DeviceID = PIOS_RFM22B_DeviceID(pios_rfm22b_id); + oplinkStatus.RxGood = radio_stats.rx_good; + oplinkStatus.RxCorrected = radio_stats.rx_corrected; + oplinkStatus.RxErrors = radio_stats.rx_error; + oplinkStatus.RxMissed = radio_stats.rx_missed; + oplinkStatus.RxFailure = radio_stats.rx_failure; + oplinkStatus.TxDropped = radio_stats.tx_dropped; + oplinkStatus.TxResent = radio_stats.tx_resent; + oplinkStatus.TxFailure = radio_stats.tx_failure; + oplinkStatus.Resets = radio_stats.resets; + oplinkStatus.Timeouts = radio_stats.timeouts; + oplinkStatus.RSSI = radio_stats.rssi; + oplinkStatus.LinkQuality = radio_stats.link_quality; + if (first_time) { + first_time = false; + } else { + uint16_t tx_count = radio_stats.tx_byte_count; + uint16_t rx_count = radio_stats.rx_byte_count; + uint16_t tx_bytes = (tx_count < prev_tx_count) ? (0xffff - prev_tx_count + tx_count) : (tx_count - prev_tx_count); + uint16_t rx_bytes = (rx_count < prev_rx_count) ? (0xffff - prev_rx_count + rx_count) : (rx_count - prev_rx_count); + oplinkStatus.TXRate = (uint16_t)((float)(tx_bytes * 1000) / SYSTEM_UPDATE_PERIOD_MS); + oplinkStatus.RXRate = (uint16_t)((float)(rx_bytes * 1000) / SYSTEM_UPDATE_PERIOD_MS); + prev_tx_count = tx_count; + prev_rx_count = rx_count; + } + oplinkStatus.TXSeq = radio_stats.tx_seq; + oplinkStatus.RXSeq = radio_stats.rx_seq; + oplinkStatus.LinkState = radio_stats.link_state; + OPLinkStatusSet(&oplinkStatus); +#endif /* if defined(PIOS_INCLUDE_RFM22B) */ + if (xQueueReceive(objectPersistenceQueue, &ev, delayTime) == pdTRUE) { // If object persistence is updated call the callback objectUpdatedCb(&ev); diff --git a/flight/pios/common/pios_rfm22b.c b/flight/pios/common/pios_rfm22b.c index 242f17b21..94538607d 100644 --- a/flight/pios/common/pios_rfm22b.c +++ b/flight/pios/common/pios_rfm22b.c @@ -51,12 +51,8 @@ #ifdef PIOS_INCLUDE_RFM22B #include -#include -#if defined(PIOS_INCLUDE_GCSRCVR) -#include -#endif #include -#include +#include #include /* Local Defines */ @@ -66,32 +62,24 @@ #define EVENT_QUEUE_SIZE 5 #define RFM22B_DEFAULT_RX_DATARATE RFM22_datarate_9600 #define RFM22B_DEFAULT_TX_POWER RFM22_tx_pwr_txpow_0 -#define RFM22B_LINK_QUALITY_THRESHOLD 20 #define RFM22B_NOMINAL_CARRIER_FREQUENCY 430000000 -#define RFM22B_MAXIMUM_FREQUENCY 440000000 -#define RFM22B_DEFAULT_FREQUENCY 433000000 -#define RFM22B_FREQUENCY_HOP_STEP_SIZE 75000 - -// The maximum amount of time since the last message received to consider the connection broken. -#define DISCONNECT_TIMEOUT_MS 1000 // ms +#define RFM22B_LINK_QUALITY_THRESHOLD 20 +#define RFM22B_DEFAULT_NUM_CHANNELS 1 +#define RFM22B_DEFAULT_MIN_CHANNEL 0 +#define RFM22B_DEFAULT_MAX_CHANNEL 250 +#define RFM22B_DEFAULT_CHANNEL_SET 24 +#define RFM22B_DEFAULT_PACKET_PERIOD 15 // The maximum amount of time without activity before initiating a reset. #define PIOS_RFM22B_SUPERVISOR_TIMEOUT 100 // ms -// The time between updates for sending stats the radio link. -#define RADIOSTATS_UPDATE_PERIOD_MS 250 - -// The number of stats updates that a modem can miss before it's considered disconnected -#define MAX_RADIOSTATS_MISS_COUNT 3 - -// The time between PPM updates -#define PPM_UPDATE_PERIOD_MS 30 - // this is too adjust the RF module so that it is on frequency #define OSC_LOAD_CAP 0x7F // cap = 12.5pf .. default #define TX_PREAMBLE_NIBBLES 12 // 7 to 511 (number of nibbles) #define RX_PREAMBLE_NIBBLES 6 // 5 to 31 (number of nibbles) +#define SYNC_BYTES 4 +#define HEADER_BYTES 4 // the size of the rf modules internal FIFO buffers #define FIFO_SIZE 64 @@ -155,24 +143,19 @@ static const uint8_t OUT_FF[64] = { 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF }; +// The randomized channel list. +static const uint8_t channel_list[] = { 68, 34, 2, 184, 166, 94, 204, 18, 47, 118, 239, 176, 5, 213, 218, 186, 104, 160, 199, 209, 231, 197, 92, 191, 88, 129, 40, 19, 93, 200, 156, 14, 247, 182, 193, 194, 208, 210, 248, 76, 244, 48, 179, 105, 25, 74, 155, 203, 39, 97, 195, 81, 83, 180, 134, 172, 235, 132, 198, 119, 207, 154, 0, 61, 140, 171, 245, 26, 95, 3, 22, 62, 169, 55, 127, 144, 45, 33, 170, 91, 158, 167, 63, 201, 41, 21, 190, 51, 103, 49, 189, 205, 240, 89, 181, 149, 6, 157, 249, 230, 115, 72, 163, 17, 29, 99, 28, 117, 219, 73, 78, 53, 69, 216, 161, 124, 110, 242, 214, 145, 13, 11, 220, 113, 138, 58, 54, 162, 237, 37, 152, 187, 232, 77, 126, 85, 38, 238, 173, 23, 188, 100, 131, 226, 31, 9, 114, 106, 221, 42, 233, 139, 4, 241, 96, 211, 8, 98, 121, 147, 24, 217, 27, 87, 122, 125, 135, 148, 178, 71, 206, 57, 141, 35, 30, 246, 159, 16, 32, 15, 229, 20, 12, 223, 150, 101, 79, 56, 102, 111, 174, 236, 137, 143, 52, 225, 64, 224, 112, 168, 243, 130, 108, 202, 123, 146, 228, 75, 46, 153, 7, 192, 175, 151, 222, 59, 82, 90, 1, 65, 109, 44, 165, 84, 43, 36, 128, 196, 67, 80, 136, 86, 70, 234, 66, 185, 10, 164, 177, 116, 50, 107, 183, 215, 212, 60, 227, 133, 120, 142 }; /* Local function forwared declarations */ static void pios_rfm22_task(void *parameters); static bool pios_rfm22_readStatus(struct pios_rfm22b_dev *rfm22b_dev); -static void pios_rfm22_setDatarate(struct pios_rfm22b_dev *rfm22b_dev, enum rfm22b_datarate datarate, bool data_whitening); +static void pios_rfm22_setDatarate(struct pios_rfm22b_dev *rfm22b_dev); static void rfm22_rxFailure(struct pios_rfm22b_dev *rfm22b_dev); static void pios_rfm22_inject_event(struct pios_rfm22b_dev *rfm22b_dev, enum pios_radio_event event, bool inISR); static enum pios_radio_event rfm22_init(struct pios_rfm22b_dev *rfm22b_dev); -static enum pios_radio_event rfm22_receiveStatus(struct pios_rfm22b_dev *rfm22b_dev); -static enum pios_radio_event rfm22_receiveAck(struct pios_rfm22b_dev *rfm22b_dev); -static enum pios_radio_event rfm22_receiveNack(struct pios_rfm22b_dev *rfm22b_dev); -static enum pios_radio_event rfm22_sendAck(struct pios_rfm22b_dev *rfm22b_dev); -static enum pios_radio_event rfm22_sendNack(struct pios_rfm22b_dev *rfm22b_dev); -static enum pios_radio_event rfm22_requestConnection(struct pios_rfm22b_dev *rfm22b_dev); -static enum pios_radio_event rfm22_acceptConnection(struct pios_rfm22b_dev *rfm22b_dev); static enum pios_radio_event radio_setRxMode(struct pios_rfm22b_dev *rfm22b_dev); static enum pios_radio_event radio_rxData(struct pios_rfm22b_dev *rfm22b_dev); -static enum pios_radio_event radio_receivePacket(struct pios_rfm22b_dev *rfm22b_dev, PHPacketHandle p, uint16_t rx_len); +static enum pios_radio_event radio_receivePacket(struct pios_rfm22b_dev *rfm22b_dev, uint8_t *p, uint16_t rx_len); static enum pios_radio_event radio_txStart(struct pios_rfm22b_dev *rfm22b_dev); static enum pios_radio_event radio_txData(struct pios_rfm22b_dev *rfm22b_dev); static enum pios_radio_event rfm22_txFailure(struct pios_rfm22b_dev *rfm22b_dev); @@ -181,17 +164,19 @@ static void rfm22_process_event(struct pios_rfm22b_dev *rfm22b_dev, enum pios_ra static enum pios_radio_event rfm22_timeout(struct pios_rfm22b_dev *rfm22b_dev); static enum pios_radio_event rfm22_error(struct pios_rfm22b_dev *rfm22b_dev); static enum pios_radio_event rfm22_fatal_error(struct pios_rfm22b_dev *rfm22b_dev); -static void rfm22_sendStatus(struct pios_rfm22b_dev *rfm22b_dev); -static void rfm22_sendPPM(struct pios_rfm22b_dev *rfm22b_dev); static void rfm22b_add_rx_status(struct pios_rfm22b_dev *rfm22b_dev, enum pios_rfm22b_rx_packet_status status); -static void rfm22_setNominalCarrierFrequency(struct pios_rfm22b_dev *rfm22b_dev, uint32_t min_frequency, uint32_t max_frequency, uint32_t step_size); +static void rfm22_setNominalCarrierFrequency(struct pios_rfm22b_dev *rfm22b_dev, uint8_t init_chan); static bool rfm22_setFreqHopChannel(struct pios_rfm22b_dev *rfm22b_dev, uint8_t channel); +static void rfm22_updatePairStatus(struct pios_rfm22b_dev *radio_dev); static void rfm22_calculateLinkQuality(struct pios_rfm22b_dev *rfm22b_dev); static bool rfm22_isConnected(struct pios_rfm22b_dev *rfm22b_dev); -static void rfm22_setConnectionParameters(struct pios_rfm22b_dev *rfm22b_dev); +static bool rfm22_isCoordinator(struct pios_rfm22b_dev *rfm22b_dev); +static uint32_t rfm22_destinationID(struct pios_rfm22b_dev *rfm22b_dev); static bool rfm22_timeToSend(struct pios_rfm22b_dev *rfm22b_dev); +static void rfm22_synchronizeClock(struct pios_rfm22b_dev *rfm22b_dev); static portTickType rfm22_coordinatorTime(struct pios_rfm22b_dev *rfm22b_dev, portTickType ticks); -static uint8_t rfm22_calcChannel(struct pios_rfm22b_dev *rfm22b_dev); +static uint8_t rfm22_calcChannel(struct pios_rfm22b_dev *rfm22b_dev, uint8_t index); +static uint8_t rfm22_calcChannelFromClock(struct pios_rfm22b_dev *rfm22b_dev); static bool rfm22_changeChannel(struct pios_rfm22b_dev *rfm22b_dev); static void rfm22_clearLEDs(); @@ -212,110 +197,50 @@ static uint8_t rfm22_read(struct pios_rfm22b_dev *rfm22b_dev, uint8_t addr); /* The state transition table */ static const struct pios_rfm22b_transition rfm22b_transitions[RADIO_STATE_NUM_STATES] = { // Initialization thread - [RADIO_STATE_UNINITIALIZED] = { + [RADIO_STATE_UNINITIALIZED] = { .entry_fn = 0, - .next_state = { + .next_state = { [RADIO_EVENT_INITIALIZE] = RADIO_STATE_INITIALIZING, [RADIO_EVENT_ERROR] = RADIO_STATE_ERROR, }, }, - [RADIO_STATE_INITIALIZING] = { + [RADIO_STATE_INITIALIZING] = { .entry_fn = rfm22_init, - .next_state = { + .next_state = { [RADIO_EVENT_INITIALIZED] = RADIO_STATE_RX_MODE, [RADIO_EVENT_ERROR] = RADIO_STATE_ERROR, [RADIO_EVENT_INITIALIZE] = RADIO_STATE_INITIALIZING, [RADIO_EVENT_FATAL_ERROR] = RADIO_STATE_FATAL_ERROR, }, }, - [RADIO_STATE_REQUESTING_CONNECTION] = { - .entry_fn = rfm22_requestConnection, - .next_state = { - [RADIO_EVENT_TX_START] = RADIO_STATE_TX_START, - [RADIO_EVENT_TIMEOUT] = RADIO_STATE_TIMEOUT, - [RADIO_EVENT_ERROR] = RADIO_STATE_ERROR, - [RADIO_EVENT_INITIALIZE] = RADIO_STATE_INITIALIZING, - [RADIO_EVENT_FATAL_ERROR] = RADIO_STATE_FATAL_ERROR - }, - }, - [RADIO_STATE_ACCEPTING_CONNECTION] = { - .entry_fn = rfm22_acceptConnection, - .next_state = { - [RADIO_EVENT_DEFAULT] = RADIO_STATE_SENDING_ACK, - [RADIO_EVENT_TIMEOUT] = RADIO_STATE_TIMEOUT, - [RADIO_EVENT_ERROR] = RADIO_STATE_ERROR, - [RADIO_EVENT_INITIALIZE] = RADIO_STATE_INITIALIZING, - [RADIO_EVENT_FATAL_ERROR] = RADIO_STATE_FATAL_ERROR, - }, - }, - [RADIO_STATE_RX_MODE] = { + [RADIO_STATE_RX_MODE] = { .entry_fn = radio_setRxMode, - .next_state = { + .next_state = { [RADIO_EVENT_TX_START] = RADIO_STATE_TX_START, - [RADIO_EVENT_ACK_TIMEOUT] = RADIO_STATE_RECEIVING_NACK, [RADIO_EVENT_INT_RECEIVED] = RADIO_STATE_RX_DATA, - [RADIO_EVENT_FAILURE] = RADIO_STATE_RX_FAILURE, - [RADIO_EVENT_TIMEOUT] = RADIO_STATE_TIMEOUT, - [RADIO_EVENT_ERROR] = RADIO_STATE_ERROR, - [RADIO_EVENT_INITIALIZE] = RADIO_STATE_INITIALIZING, - [RADIO_EVENT_FATAL_ERROR] = RADIO_STATE_FATAL_ERROR, - }, - }, - [RADIO_STATE_RX_DATA] = { - .entry_fn = radio_rxData, - .next_state = { - [RADIO_EVENT_INT_RECEIVED] = RADIO_STATE_RX_DATA, - [RADIO_EVENT_TX_START] = RADIO_STATE_TX_START, - [RADIO_EVENT_ACK_TIMEOUT] = RADIO_STATE_RECEIVING_NACK, - [RADIO_EVENT_RX_COMPLETE] = RADIO_STATE_SENDING_ACK, - [RADIO_EVENT_RX_MODE] = RADIO_STATE_RX_MODE, - [RADIO_EVENT_STATUS_RECEIVED] = RADIO_STATE_RECEIVING_STATUS, - [RADIO_EVENT_CONNECTION_REQUESTED] = RADIO_STATE_ACCEPTING_CONNECTION, - [RADIO_EVENT_PACKET_ACKED] = RADIO_STATE_RECEIVING_ACK, - [RADIO_EVENT_PACKET_NACKED] = RADIO_STATE_RECEIVING_NACK, - [RADIO_EVENT_FAILURE] = RADIO_STATE_RX_FAILURE, - [RADIO_EVENT_TIMEOUT] = RADIO_STATE_TIMEOUT, - [RADIO_EVENT_ERROR] = RADIO_STATE_ERROR, - [RADIO_EVENT_INITIALIZE] = RADIO_STATE_INITIALIZING, - [RADIO_EVENT_FATAL_ERROR] = RADIO_STATE_FATAL_ERROR, - }, - }, - [RADIO_STATE_RECEIVING_ACK] = { - .entry_fn = rfm22_receiveAck, - .next_state = { - [RADIO_EVENT_TX_START] = RADIO_STATE_TX_START, - [RADIO_EVENT_RX_MODE] = RADIO_STATE_RX_MODE, - [RADIO_EVENT_TIMEOUT] = RADIO_STATE_TIMEOUT, - [RADIO_EVENT_ERROR] = RADIO_STATE_ERROR, - [RADIO_EVENT_INITIALIZE] = RADIO_STATE_INITIALIZING, - [RADIO_EVENT_FATAL_ERROR] = RADIO_STATE_FATAL_ERROR, - }, - }, - [RADIO_STATE_RECEIVING_NACK] = { - .entry_fn = rfm22_receiveNack, - .next_state = { - [RADIO_EVENT_TX_START] = RADIO_STATE_TX_START, - [RADIO_EVENT_TIMEOUT] = RADIO_STATE_TIMEOUT, - [RADIO_EVENT_ERROR] = RADIO_STATE_ERROR, - [RADIO_EVENT_INITIALIZE] = RADIO_STATE_INITIALIZING, - [RADIO_EVENT_FATAL_ERROR] = RADIO_STATE_FATAL_ERROR, - }, - }, - [RADIO_STATE_RECEIVING_STATUS] = { - .entry_fn = rfm22_receiveStatus, - .next_state = { - [RADIO_EVENT_RX_COMPLETE] = RADIO_STATE_TX_START, - [RADIO_EVENT_REQUEST_CONNECTION] = RADIO_STATE_REQUESTING_CONNECTION, - [RADIO_EVENT_TIMEOUT] = RADIO_STATE_TIMEOUT, + [RADIO_EVENT_TIMEOUT] = RADIO_STATE_TIMEOUT, [RADIO_EVENT_ERROR] = RADIO_STATE_ERROR, + [RADIO_EVENT_INITIALIZE] = RADIO_STATE_INITIALIZING, + [RADIO_EVENT_FATAL_ERROR] = RADIO_STATE_FATAL_ERROR, + }, + }, + [RADIO_STATE_RX_DATA] = { + .entry_fn = radio_rxData, + .next_state = { + [RADIO_EVENT_INT_RECEIVED] = RADIO_STATE_RX_DATA, + [RADIO_EVENT_TX_START] = RADIO_STATE_TX_START, + [RADIO_EVENT_RX_COMPLETE] = RADIO_STATE_TX_START, + [RADIO_EVENT_RX_MODE] = RADIO_STATE_RX_MODE, + [RADIO_EVENT_TIMEOUT] = RADIO_STATE_TIMEOUT, + [RADIO_EVENT_ERROR] = RADIO_STATE_ERROR, [RADIO_EVENT_INITIALIZE] = RADIO_STATE_INITIALIZING, [RADIO_EVENT_FATAL_ERROR] = RADIO_STATE_FATAL_ERROR, }, }, - [RADIO_STATE_TX_START] = { + [RADIO_STATE_TX_START] = { .entry_fn = radio_txStart, - .next_state = { + .next_state = { [RADIO_EVENT_INT_RECEIVED] = RADIO_STATE_TX_DATA, [RADIO_EVENT_RX_MODE] = RADIO_STATE_RX_MODE, [RADIO_EVENT_TIMEOUT] = RADIO_STATE_TIMEOUT, @@ -324,21 +249,20 @@ static const struct pios_rfm22b_transition rfm22b_transitions[RADIO_STATE_NUM_ST [RADIO_EVENT_FATAL_ERROR] = RADIO_STATE_FATAL_ERROR, }, }, - [RADIO_STATE_TX_DATA] = { + [RADIO_STATE_TX_DATA] = { .entry_fn = radio_txData, - .next_state = { + .next_state = { [RADIO_EVENT_INT_RECEIVED] = RADIO_STATE_TX_DATA, [RADIO_EVENT_RX_MODE] = RADIO_STATE_RX_MODE, - [RADIO_EVENT_FAILURE] = RADIO_STATE_TX_FAILURE, [RADIO_EVENT_TIMEOUT] = RADIO_STATE_TIMEOUT, [RADIO_EVENT_ERROR] = RADIO_STATE_ERROR, [RADIO_EVENT_INITIALIZE] = RADIO_STATE_INITIALIZING, [RADIO_EVENT_FATAL_ERROR] = RADIO_STATE_FATAL_ERROR, }, }, - [RADIO_STATE_TX_FAILURE] = { + [RADIO_STATE_TX_FAILURE] = { .entry_fn = rfm22_txFailure, - .next_state = { + .next_state = { [RADIO_EVENT_TX_START] = RADIO_STATE_TX_START, [RADIO_EVENT_TIMEOUT] = RADIO_STATE_TIMEOUT, [RADIO_EVENT_ERROR] = RADIO_STATE_ERROR, @@ -346,29 +270,9 @@ static const struct pios_rfm22b_transition rfm22b_transitions[RADIO_STATE_NUM_ST [RADIO_EVENT_FATAL_ERROR] = RADIO_STATE_FATAL_ERROR, }, }, - [RADIO_STATE_SENDING_ACK] = { - .entry_fn = rfm22_sendAck, - .next_state = { - [RADIO_EVENT_TX_START] = RADIO_STATE_TX_START, - [RADIO_EVENT_TIMEOUT] = RADIO_STATE_TIMEOUT, - [RADIO_EVENT_ERROR] = RADIO_STATE_ERROR, - [RADIO_EVENT_INITIALIZE] = RADIO_STATE_INITIALIZING, - [RADIO_EVENT_FATAL_ERROR] = RADIO_STATE_FATAL_ERROR, - }, - }, - [RADIO_STATE_SENDING_NACK] = { - .entry_fn = rfm22_sendNack, - .next_state = { - [RADIO_EVENT_TX_START] = RADIO_STATE_TX_START, - [RADIO_EVENT_TIMEOUT] = RADIO_STATE_TIMEOUT, - [RADIO_EVENT_ERROR] = RADIO_STATE_ERROR, - [RADIO_EVENT_INITIALIZE] = RADIO_STATE_INITIALIZING, - [RADIO_EVENT_FATAL_ERROR] = RADIO_STATE_FATAL_ERROR, - }, - }, - [RADIO_STATE_TIMEOUT] = { + [RADIO_STATE_TIMEOUT] = { .entry_fn = rfm22_timeout, - .next_state = { + .next_state = { [RADIO_EVENT_TX_START] = RADIO_STATE_TX_START, [RADIO_EVENT_RX_MODE] = RADIO_STATE_RX_MODE, [RADIO_EVENT_ERROR] = RADIO_STATE_ERROR, @@ -376,16 +280,16 @@ static const struct pios_rfm22b_transition rfm22b_transitions[RADIO_STATE_NUM_ST [RADIO_EVENT_FATAL_ERROR] = RADIO_STATE_FATAL_ERROR, }, }, - [RADIO_STATE_ERROR] = { + [RADIO_STATE_ERROR] = { .entry_fn = rfm22_error, - .next_state = { + .next_state = { [RADIO_EVENT_INITIALIZE] = RADIO_STATE_INITIALIZING, [RADIO_EVENT_FATAL_ERROR] = RADIO_STATE_FATAL_ERROR, }, }, - [RADIO_STATE_FATAL_ERROR] = { + [RADIO_STATE_FATAL_ERROR] = { .entry_fn = rfm22_fatal_error, - .next_state = {}, + .next_state = {}, }, }; @@ -451,12 +355,12 @@ int32_t PIOS_RFM22B_Init(uint32_t *rfm22b_id, uint32_t spi_id, uint32_t slave_nu rfm22b_dev->spi_id = spi_id; // Initialize our configuration parameters - rfm22b_dev->send_ppm = false; rfm22b_dev->datarate = RFM22B_DEFAULT_RX_DATARATE; rfm22b_dev->tx_power = RFM22B_DEFAULT_TX_POWER; + rfm22b_dev->coordinator = false; + rfm22b_dev->coordinatorID = 0; // Initialize the com callbacks. - rfm22b_dev->com_config_cb = NULL; rfm22b_dev->rx_in_cb = NULL; rfm22b_dev->tx_out_cb = NULL; @@ -476,18 +380,11 @@ int32_t PIOS_RFM22B_Init(uint32_t *rfm22b_id, uint32_t spi_id, uint32_t slave_nu rfm22b_dev->stats.rx_seq = 0; rfm22b_dev->stats.tx_failure = 0; - // Initialize the frequencies. - PIOS_RFM22B_SetInitialFrequency(*rfm22b_id, RFM22B_DEFAULT_FREQUENCY); - PIOS_RFM22B_SetFrequencyRange(*rfm22b_id, RFM22B_DEFAULT_FREQUENCY, RFM22B_DEFAULT_FREQUENCY, RFM22B_FREQUENCY_HOP_STEP_SIZE); - - // Initialize the bindings. - for (uint32_t i = 0; i < OPLINKSETTINGS_BINDINGS_NUMELEM; ++i) { - rfm22b_dev->bindings[i].pairID = 0; - } - rfm22b_dev->coordinator = false; + // Initialize the channels. + PIOS_RFM22B_SetChannelConfig(*rfm22b_id, RFM22B_DEFAULT_NUM_CHANNELS, RFM22B_DEFAULT_MIN_CHANNEL, RFM22B_DEFAULT_MAX_CHANNEL, RFM22B_DEFAULT_CHANNEL_SET, RFM22B_DEFAULT_PACKET_PERIOD, false); // Create the event queue - rfm22b_dev->eventQueue = xQueueCreate(EVENT_QUEUE_SIZE, sizeof(enum pios_radio_event)); + rfm22b_dev->eventQueue = xQueueCreate(EVENT_QUEUE_SIZE, sizeof(enum pios_radio_event)); // Bind the configuration to the device instance rfm22b_dev->cfg = *cfg; @@ -508,11 +405,6 @@ int32_t PIOS_RFM22B_Init(uint32_t *rfm22b_id, uint32_t spi_id, uint32_t slave_nu rfm22b_dev->deviceID = crcs[0] | crcs[1] << 8 | crcs[2] << 16 | crcs[3] << 24; DEBUG_PRINTF(2, "RF device ID: %x\n\r", rfm22b_dev->deviceID); -#if defined(PIOS_INCLUDE_GCSRCVR) - // Initialize the GCSReceive object - GCSReceiverInitialize(); -#endif - // Initialize the external interrupt. PIOS_EXTI_Init(cfg->exti_cfg); @@ -580,6 +472,16 @@ uint32_t PIOS_RFM22B_DeviceID(uint32_t rfm22b_id) return 0; } +/** + * Are we connected to the remote modem? + * + * @param[in] rfm22b_dev The device structure + */ +static bool rfm22_isConnected(struct pios_rfm22b_dev *rfm22b_dev) +{ + return (rfm22b_dev->stats.link_state == OPLINKSTATUS_LINKSTATE_CONNECTED) || (rfm22b_dev->stats.link_state == OPLINKSTATUS_LINKSTATE_CONNECTING); +} + /** * Returns true if the modem is configured as a coordinator. * @@ -591,7 +493,7 @@ bool PIOS_RFM22B_IsCoordinator(uint32_t rfm22b_id) struct pios_rfm22b_dev *rfm22b_dev = (struct pios_rfm22b_dev *)rfm22b_id; if (PIOS_RFM22B_Validate(rfm22b_dev)) { - return rfm22b_dev->coordinator; + return rfm22_isCoordinator(rfm22b_dev); } return false; } @@ -629,85 +531,68 @@ void PIOS_RFM22B_SetTxPower(uint32_t rfm22b_id, enum rfm22b_tx_power tx_pwr) } /** - * Sets the radio frequency range and initial frequency + * Sets the range and number of channels to use for the radio. + * The channels are 0 to 255 divided across the 430-440 MHz range. + * The number of channels configured will be spread across the selected channel range. + * The channel spacing is 10MHz / 250 = 40kHz * * @param[in] rfm22b_id The RFM22B device index. - * @param[in] min_freq The minimum frequency - * @param[in] max_freq The maximum frequency - * @param[in] step_size The channel step size + * @param[in] num_channels The number of channels to use for frequency hopping. + * @param[in] min_chan The minimum channel. + * @param[in] max_chan The maximum channel. + * @param[in] chan_set The "seed" for selecting a channel sequence. + * @param[in] packet_period The fixed time alloted to sending a single packet + * @param[in] oneway Only the coordinator can send packets if true. */ -void PIOS_RFM22B_SetFrequencyRange(uint32_t rfm22b_id, uint32_t min_freq, uint32_t max_freq, uint32_t step_size) +void PIOS_RFM22B_SetChannelConfig(uint32_t rfm22b_id, uint8_t num_chan, uint8_t min_chan, uint8_t max_chan, uint8_t chan_set, uint8_t packet_period, bool oneway) { struct pios_rfm22b_dev *rfm22b_dev = (struct pios_rfm22b_dev *)rfm22b_id; if (!PIOS_RFM22B_Validate(rfm22b_dev)) { return; } - rfm22b_dev->con_packet.min_frequency = min_freq; - rfm22b_dev->con_packet.max_frequency = max_freq; - rfm22b_dev->con_packet.channel_spacing = step_size; + rfm22b_dev->packet_period = packet_period; + rfm22b_dev->num_channels = num_chan; + rfm22b_dev->one_way_link = oneway; + + // Find the first N channels that meet the min/max criteria out of the random channel list. + uint8_t num_found = 0; + for (uint16_t i = 0; (i < RFM22B_NUM_CHANNELS) && (num_found < num_chan); ++i) { + uint8_t idx = (i + chan_set) % RFM22B_NUM_CHANNELS; + uint8_t chan = channel_list[idx]; + if ((chan >= min_chan) && (chan <= max_chan)) { + rfm22b_dev->channels[num_found++] = chan; + } + } } /** - * Sets the initial radio frequency range + * Set a modem to be a coordinator or not. * - * @param[in] rfm22b_id The RFM22B device index. - * @param[in] init_freq The initial frequency + * @param[in] rfm22b_id The RFM22B device index. + * @param[in] coordinator If true, this modem will be configured as a coordinator. */ -void PIOS_RFM22B_SetInitialFrequency(uint32_t rfm22b_id, uint32_t init_freq) +extern 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; + if (PIOS_RFM22B_Validate(rfm22b_dev)) { + rfm22b_dev->coordinator = coordinator; } - rfm22b_dev->init_frequency = init_freq; } /** - * Set the com port configuration callback (to receive com configuration over the air) + * Sets the device coordinator ID. * - * @param[in] rfm22b_id The rfm22b device. - * @param[in] cb A pointer to the callback function + * @param[in] rfm22b_id The RFM22B device index. + * @param[in] coord_id The coordinator ID. */ -void PIOS_RFM22B_SetComConfigCallback(uint32_t rfm22b_id, PIOS_RFM22B_ComConfigCallback cb) +void PIOS_RFM22B_SetCoordinatorID(uint32_t rfm22b_id, uint32_t coord_id) { struct pios_rfm22b_dev *rfm22b_dev = (struct pios_rfm22b_dev *)rfm22b_id; - if (!PIOS_RFM22B_Validate(rfm22b_dev)) { - return; - } - rfm22b_dev->com_config_cb = cb; -} - -/** - * Set the list of modems that this modem will bind with. - * - * @param[in] rfm22b_id The rfm22b device. - * @param[in] bindingPairIDs The array of binding IDs. - * @param[in] mainPortSettings The array of main com port configurations. - * @param[in] flexiPortSettings The array of flexi com port configurations. - * @param[in] vcpPortSettings The array of VCP com port configurations. - * @param[in] comSpeeds The array of com port speeds. - */ -void PIOS_RFM22B_SetBindings(uint32_t rfm22b_id, const uint32_t bindingPairIDs[], const uint8_t mainPortSettings[], - const uint8_t flexiPortSettings[], const uint8_t vcpPortSettings[], const uint8_t comSpeeds[]) -{ - struct pios_rfm22b_dev *rfm22b_dev = (struct pios_rfm22b_dev *)rfm22b_id; - - if (!PIOS_RFM22B_Validate(rfm22b_dev)) { - return; - } - - // This modem will be considered a coordinator if any bindings have been set. - rfm22b_dev->coordinator = false; - for (uint32_t i = 0; i < OPLINKSETTINGS_BINDINGS_NUMELEM; ++i) { - rfm22b_dev->bindings[i].pairID = bindingPairIDs[i]; - rfm22b_dev->bindings[i].main_port = mainPortSettings[i]; - rfm22b_dev->bindings[i].flexi_port = flexiPortSettings[i]; - rfm22b_dev->bindings[i].vcp_port = vcpPortSettings[i]; - rfm22b_dev->bindings[i].com_speed = comSpeeds[i]; - rfm22b_dev->coordinator |= (rfm22b_dev->bindings[i].pairID != 0); + if (PIOS_RFM22B_Validate(rfm22b_dev)) { + rfm22b_dev->coordinatorID = coord_id; } } @@ -728,17 +613,7 @@ void PIOS_RFM22B_GetStats(uint32_t rfm22b_id, struct rfm22b_stats *stats) // Calculate the current link quality rfm22_calculateLinkQuality(rfm22b_dev); - // We are connected if our destination ID is in the pair stats. - if (rfm22b_dev->destination_id != 0xffffffff) { - for (uint8_t i = 0; i < OPLINKSTATUS_PAIRIDS_NUMELEM; ++i) { - if ((rfm22b_dev->pair_stats[i].pairID == rfm22b_dev->destination_id) && - (rfm22b_dev->pair_stats[i].rssi > -127)) { - rfm22b_dev->stats.rssi = rfm22b_dev->pair_stats[i].rssi; - rfm22b_dev->stats.afc_correction = rfm22b_dev->pair_stats[i].afc_correction; - break; - } - } - } + // Return the stats. *stats = rfm22b_dev->stats; } @@ -780,7 +655,7 @@ bool PIOS_RFM22B_LinkStatus(uint32_t rfm22b_id) if (!PIOS_RFM22B_Validate(rfm22b_dev)) { return false; } - return rfm22_isConnected(rfm22b_dev) && (rfm22b_dev->stats.link_quality > RFM22B_LINK_QUALITY_THRESHOLD); + return rfm22b_dev->stats.link_quality > RFM22B_LINK_QUALITY_THRESHOLD; } /** @@ -790,7 +665,7 @@ bool PIOS_RFM22B_LinkStatus(uint32_t rfm22b_id) * @param[in] p The packet to receive into. * @return true if Rx mode was entered sucessfully. */ -bool PIOS_RFM22B_ReceivePacket(uint32_t rfm22b_id, PHPacketHandle p) +bool PIOS_RFM22B_ReceivePacket(uint32_t rfm22b_id, uint8_t *p) { struct pios_rfm22b_dev *rfm22b_dev = (struct pios_rfm22b_dev *)rfm22b_id; @@ -855,7 +730,7 @@ bool PIOS_RFM22B_ReceivePacket(uint32_t rfm22b_id, PHPacketHandle p) * @param[in] p The packet to transmit. * @return true if there if the packet was queued for transmission. */ -bool PIOS_RFM22B_TransmitPacket(uint32_t rfm22b_id, PHPacketHandle p) +bool PIOS_RFM22B_TransmitPacket(uint32_t rfm22b_id, uint8_t *p, uint8_t len) { struct pios_rfm22b_dev *rfm22b_dev = (struct pios_rfm22b_dev *)rfm22b_id; @@ -863,8 +738,9 @@ bool PIOS_RFM22B_TransmitPacket(uint32_t rfm22b_id, PHPacketHandle p) return false; } - rfm22b_dev->tx_packet = p; - rfm22b_dev->packet_start_ticks = xTaskGetTickCount(); + rfm22b_dev->tx_packet_handle = p; + rfm22b_dev->stats.tx_byte_count += len; + rfm22b_dev->packet_start_ticks = xTaskGetTickCount(); if (rfm22b_dev->packet_start_ticks == 0) { rfm22b_dev->packet_start_ticks = 1; } @@ -877,23 +753,23 @@ bool PIOS_RFM22B_TransmitPacket(uint32_t rfm22b_id, PHPacketHandle p) rfm22_write(rfm22b_dev, RFM22_interrupt_enable2, 0x00); // set the tx power + rfm22b_dev->tx_power = 0x7; rfm22_write(rfm22b_dev, RFM22_tx_power, RFM22_tx_pwr_lna_sw | rfm22b_dev->tx_power); // TUNE mode rfm22_write(rfm22b_dev, RFM22_op_and_func_ctrl1, RFM22_opfc1_pllon); // Queue the data up for sending - rfm22b_dev->tx_data_wr = PH_PACKET_SIZE(rfm22b_dev->tx_packet); + rfm22b_dev->tx_data_wr = len; RX_LED_OFF; // Set the destination address in the transmit header. - // The destination address is the first 4 bytes of the message. - uint8_t *tx_buffer = (uint8_t *)(rfm22b_dev->tx_packet); - rfm22_write(rfm22b_dev, RFM22_transmit_header0, tx_buffer[0]); - rfm22_write(rfm22b_dev, RFM22_transmit_header1, tx_buffer[1]); - rfm22_write(rfm22b_dev, RFM22_transmit_header2, tx_buffer[2]); - rfm22_write(rfm22b_dev, RFM22_transmit_header3, tx_buffer[3]); + uint32_t id = rfm22_destinationID(rfm22b_dev); + rfm22_write(rfm22b_dev, RFM22_transmit_header0, id & 0xff); + rfm22_write(rfm22b_dev, RFM22_transmit_header1, (id >> 8) & 0xff); + rfm22_write(rfm22b_dev, RFM22_transmit_header2, (id >> 16) & 0xff); + rfm22_write(rfm22b_dev, RFM22_transmit_header3, (id >> 24) & 0xff); // FIFO mode, GFSK modulation uint8_t fd_bit = rfm22_read(rfm22b_dev, RFM22_modulation_mode_control2) & RFM22_mmc2_fd; @@ -904,9 +780,10 @@ bool PIOS_RFM22B_TransmitPacket(uint32_t rfm22b_id, PHPacketHandle p) rfm22_write(rfm22b_dev, RFM22_op_and_func_ctrl2, 0x00); // Set the total number of data bytes we are going to transmit. - rfm22_write(rfm22b_dev, RFM22_transmit_packet_length, rfm22b_dev->tx_data_wr); + rfm22_write(rfm22b_dev, RFM22_transmit_packet_length, len); // Add some data to the chips TX FIFO before enabling the transmitter + uint8_t *tx_buffer = rfm22b_dev->tx_packet_handle; rfm22_assertCs(rfm22b_dev); PIOS_SPI_TransferByte(rfm22b_dev->spi_id, RFM22_fifo_access | 0x80); int bytes_to_write = (rfm22b_dev->tx_data_wr - rfm22b_dev->tx_data_rd); @@ -929,6 +806,10 @@ bool PIOS_RFM22B_TransmitPacket(uint32_t rfm22b_id, PHPacketHandle p) TX_LED_ON; +#ifdef PIOS_RFM22B_DEBUG_ON_TELEM + D1_LED_ON; +#endif + return true; } @@ -951,11 +832,10 @@ pios_rfm22b_int_result PIOS_RFM22B_ProcessTx(uint32_t rfm22b_id) return PIOS_RFM22B_INT_FAILURE; } - // TX FIFO almost empty, it needs filling up if (rfm22b_dev->status_regs.int_status_1.tx_fifo_almost_empty) { // Add data to the TX FIFO buffer - uint8_t *tx_buffer = (uint8_t *)(rfm22b_dev->tx_packet); + uint8_t *tx_buffer = rfm22b_dev->tx_packet_handle; uint16_t max_bytes = FIFO_SIZE - TX_FIFO_LO_WATERMARK - 1; rfm22_claimBus(rfm22b_dev); rfm22_assertCs(rfm22b_dev); @@ -971,7 +851,6 @@ pios_rfm22b_int_result PIOS_RFM22B_ProcessTx(uint32_t rfm22b_id) } else if (rfm22b_dev->status_regs.int_status_1.packet_sent_interrupt) { // Transition out of Tx mode. rfm22b_dev->rfm22b_state = RFM22B_STATE_TRANSITION; - return PIOS_RFM22B_TX_COMPLETE; } @@ -991,7 +870,7 @@ pios_rfm22b_int_result PIOS_RFM22B_ProcessRx(uint32_t rfm22b_id) if (!PIOS_RFM22B_Validate(rfm22b_dev)) { return PIOS_RFM22B_INT_FAILURE; } - uint8_t *rx_buffer = (uint8_t *)(rfm22b_dev->rx_packet_handle); + uint8_t *rx_buffer = rfm22b_dev->rx_packet_handle; pios_rfm22b_int_result ret = PIOS_RFM22B_INT_SUCCESS; // Read the device status registers @@ -1015,7 +894,14 @@ pios_rfm22b_int_result PIOS_RFM22B_ProcessRx(uint32_t rfm22b_id) // read the total length of the packet data uint32_t len = rfm22_read(rfm22b_dev, RFM22_received_packet_length); - // their must still be data in the RX FIFO we need to get + // The received packet is going to be larger than the receive buffer + if (len > rfm22b_dev->max_packet_len) { + rfm22_releaseBus(rfm22b_dev); + rfm22_rxFailure(rfm22b_dev); + return PIOS_RFM22B_INT_FAILURE; + } + + // there must still be data in the RX FIFO we need to get if (rfm22b_dev->rx_buffer_wr < len) { int32_t bytes_to_read = len - rfm22b_dev->rx_buffer_wr; // Fetch the data from the RX FIFO @@ -1026,6 +912,12 @@ pios_rfm22b_int_result PIOS_RFM22B_ProcessRx(uint32_t rfm22b_id) rfm22_deassertCs(rfm22b_dev); } + // Read the packet header (destination ID) + rfm22b_dev->rx_destination_id = rfm22_read(rfm22b_dev, RFM22_received_header0); + rfm22b_dev->rx_destination_id |= (rfm22_read(rfm22b_dev, RFM22_received_header1) << 8); + rfm22b_dev->rx_destination_id |= (rfm22_read(rfm22b_dev, RFM22_received_header2) << 16); + rfm22b_dev->rx_destination_id |= (rfm22_read(rfm22b_dev, RFM22_received_header3) << 24); + // Release the SPI bus. rfm22_releaseBus(rfm22b_dev); @@ -1038,6 +930,9 @@ pios_rfm22b_int_result PIOS_RFM22B_ProcessRx(uint32_t rfm22b_id) // Increment the total byte received count. rfm22b_dev->stats.rx_byte_count += rfm22b_dev->rx_buffer_wr; + // Update the pair status with this packet. + rfm22_updatePairStatus(rfm22b_dev); + // We're finished with Rx mode rfm22b_dev->rfm22b_state = RFM22B_STATE_TRANSITION; @@ -1059,6 +954,13 @@ pios_rfm22b_int_result PIOS_RFM22B_ProcessRx(uint32_t rfm22b_id) return PIOS_RFM22B_INT_FAILURE; } + // The received packet is going to be larger than the receive buffer + if ((rfm22b_dev->rx_buffer_wr + RX_FIFO_HI_WATERMARK) > rfm22b_dev->max_packet_len) { + rfm22_releaseBus(rfm22b_dev); + rfm22_rxFailure(rfm22b_dev); + return PIOS_RFM22B_INT_FAILURE; + } + // Fetch the data from the RX FIFO rfm22_assertCs(rfm22b_dev); PIOS_SPI_TransferByte(rfm22b_dev->spi_id, RFM22_fifo_access & 0x7F); @@ -1074,15 +976,19 @@ pios_rfm22b_int_result PIOS_RFM22B_ProcessRx(uint32_t rfm22b_id) } else if (rfm22b_dev->status_regs.int_status_2.valid_preamble_detected) { // Valid preamble detected RX_LED_ON; + + // Sync word detected #ifdef PIOS_RFM22B_DEBUG_ON_TELEM D2_LED_ON; #endif // PIOS_RFM22B_DEBUG_ON_TELEM + rfm22b_dev->packet_start_ticks = xTaskGetTickCount(); + if (rfm22b_dev->packet_start_ticks == 0) { + rfm22b_dev->packet_start_ticks = 1; + } // We detected the preamble, now wait for sync. rfm22b_dev->rfm22b_state = RFM22B_STATE_RX_WAIT_SYNC; } else if (rfm22b_dev->status_regs.int_status_2.sync_word_detected) { - // Sync word detected - // Claim the SPI bus. rfm22_claimBus(rfm22b_dev); @@ -1112,15 +1018,6 @@ pios_rfm22b_int_result PIOS_RFM22B_ProcessRx(uint32_t rfm22b_id) return PIOS_RFM22B_INT_FAILURE; } - // Set the packet start time if necessary. - if ((rfm22b_dev->packet_start_ticks == 0) && - ((rfm22b_dev->rfm22b_state == RFM22B_STATE_RX_WAIT_SYNC) || (rfm22b_dev->rfm22b_state == RFM22B_STATE_RX_WAIT_SYNC))) { - rfm22b_dev->packet_start_ticks = xTaskGetTickCount(); - if (rfm22b_dev->packet_start_ticks == 0) { - rfm22b_dev->packet_start_ticks = 1; - } - } - return ret; } @@ -1151,9 +1048,7 @@ static void pios_rfm22_task(void *parameters) if (!PIOS_RFM22B_Validate(rfm22b_dev)) { return; } - portTickType lastEventTicks = xTaskGetTickCount(); - portTickType lastStatusTicks = lastEventTicks; - portTickType lastPPMTicks = lastEventTicks; + portTickType lastEventTicks = xTaskGetTickCount(); while (1) { #if defined(PIOS_INCLUDE_WDG) && defined(PIOS_WDG_RFM22B) @@ -1191,41 +1086,13 @@ static void pios_rfm22_task(void *parameters) } // Change channels if necessary. - if (PIOS_RFM22B_InRxWait((uint32_t)rfm22b_dev)) { - rfm22_changeChannel(rfm22b_dev); - } + rfm22_changeChannel(rfm22b_dev); portTickType curTicks = xTaskGetTickCount(); - uint32_t last_rec_ms = (rfm22b_dev->rx_complete_ticks == 0) ? 0 : pios_rfm22_time_difference_ms(rfm22b_dev->rx_complete_ticks, curTicks); // Have we been sending / receiving this packet too long? if ((rfm22b_dev->packet_start_ticks > 0) && - (pios_rfm22_time_difference_ms(rfm22b_dev->packet_start_ticks, curTicks) > (rfm22b_dev->max_packet_time * 3))) { + (pios_rfm22_time_difference_ms(rfm22b_dev->packet_start_ticks, curTicks) > (rfm22b_dev->packet_period * 3))) { rfm22_process_event(rfm22b_dev, RADIO_EVENT_TIMEOUT); - } else if (last_rec_ms > DISCONNECT_TIMEOUT_MS) { - // Has it been too long since we received a packet - rfm22_process_event(rfm22b_dev, RADIO_EVENT_ERROR); - } else { - // Are we waiting for an ACK? - if (rfm22b_dev->prev_tx_packet) { - // Should we resend the packet? - if ((pios_rfm22_time_difference_ms(rfm22b_dev->tx_complete_ticks, curTicks) > rfm22b_dev->max_ack_delay) && - PIOS_RFM22B_InRxWait((uint32_t)rfm22b_dev)) { - rfm22b_dev->tx_complete_ticks = curTicks; - rfm22_process_event(rfm22b_dev, RADIO_EVENT_ACK_TIMEOUT); - } - } else { - // Queue up a PPM packet if it's time. - if (pios_rfm22_time_difference_ms(lastPPMTicks, curTicks) > PPM_UPDATE_PERIOD_MS) { - rfm22_sendPPM(rfm22b_dev); - lastPPMTicks = curTicks; - } - - // Queue up a status packet if it's time. - if (pios_rfm22_time_difference_ms(lastStatusTicks, curTicks) > RADIOSTATS_UPDATE_PERIOD_MS) { - rfm22_sendStatus(rfm22b_dev); - lastStatusTicks = curTicks; - } - } } // Send a packet if it's our time slice @@ -1237,7 +1104,19 @@ static void pios_rfm22_task(void *parameters) D4_LED_OFF; } #endif + + // Start transmitting a packet if it's time. if (time_to_send && PIOS_RFM22B_InRxWait((uint32_t)rfm22b_dev)) { + // If the on_sync_channel flag is set, it means that we were on the sync channel, but no packet was received, so transition to a non-connected state. + if (rfm22b_dev->on_sync_channel) { + rfm22b_dev->on_sync_channel = false; + if (rfm22b_dev->stats.link_state == OPLINKSTATUS_LINKSTATE_CONNECTED) { + rfm22b_dev->stats.link_state = OPLINKSTATUS_LINKSTATE_CONNECTING; + } else { + rfm22b_dev->stats.link_state = OPLINKSTATUS_LINKSTATE_DISCONNECTED; + } + } + rfm22_process_event(rfm22b_dev, RADIO_EVENT_TX_START); } } @@ -1371,28 +1250,23 @@ static enum pios_radio_event rfm22_init(struct pios_rfm22b_dev *rfm22b_dev) } // Initialize the state - rfm22b_dev->stats.link_state = OPLINKSTATUS_LINKSTATE_DISCONNECTED; - rfm22b_dev->destination_id = 0xffffffff; - rfm22b_dev->send_status = false; - rfm22b_dev->send_connection_request = false; - rfm22b_dev->send_ack_nack = false; - rfm22b_dev->resend_packet = false; + rfm22b_dev->stats.link_state = OPLINKSTATUS_LINKSTATE_DISCONNECTED; // Initialize the packets. - rfm22b_dev->rx_packet_len = 0; - rfm22b_dev->tx_packet = NULL; - rfm22b_dev->prev_tx_packet = NULL; - rfm22b_dev->data_packet.header.data_size = 0; + rfm22b_dev->rx_packet_len = 0; + rfm22b_dev->rx_destination_id = 0; + rfm22b_dev->tx_packet_handle = NULL; // Initialize the devide state - rfm22b_dev->rx_buffer_wr = 0; - rfm22b_dev->tx_data_rd = rfm22b_dev->tx_data_wr = 0; - rfm22b_dev->frequency_hop_channel = 0; - rfm22b_dev->afc_correction_Hz = 0; - rfm22b_dev->packet_start_ticks = 0; - rfm22b_dev->tx_complete_ticks = 0; - rfm22b_dev->rx_complete_ticks = 0; - rfm22b_dev->rfm22b_state = RFM22B_STATE_INITIALIZING; + rfm22b_dev->rx_buffer_wr = 0; + rfm22b_dev->tx_data_rd = rfm22b_dev->tx_data_wr = 0; + rfm22b_dev->channel = 0; + rfm22b_dev->channel_index = 0; + rfm22b_dev->afc_correction_Hz = 0; + rfm22b_dev->packet_start_ticks = 0; + rfm22b_dev->tx_complete_ticks = 0; + rfm22b_dev->rfm22b_state = RFM22B_STATE_INITIALIZING; + rfm22b_dev->on_sync_channel = false; // software reset the RF chip .. following procedure according to Si4x3x Errata (rev. B) rfm22_write_claim(rfm22b_dev, RFM22_op_and_func_ctrl1, RFM22_opfc1_swres); @@ -1522,13 +1396,14 @@ static enum pios_radio_event rfm22_init(struct pios_rfm22b_dev *rfm22b_dev) RFM22_header_cntl1_hdch_1 | RFM22_header_cntl1_hdch_2 | RFM22_header_cntl1_hdch_3); - // Check all bit of all bytes of the header - rfm22_write(rfm22b_dev, RFM22_header_enable0, 0xff); - rfm22_write(rfm22b_dev, RFM22_header_enable1, 0xff); - rfm22_write(rfm22b_dev, RFM22_header_enable2, 0xff); - rfm22_write(rfm22b_dev, RFM22_header_enable3, 0xff); - // Set the ID to be checked - uint32_t id = rfm22b_dev->deviceID; + // Check all bit of all bytes of the header, unless we're an unbound modem. + uint8_t header_mask = (rfm22_destinationID(rfm22b_dev) == 0xffffffff) ? 0 : 0xff; + rfm22_write(rfm22b_dev, RFM22_header_enable0, header_mask); + rfm22_write(rfm22b_dev, RFM22_header_enable1, header_mask); + rfm22_write(rfm22b_dev, RFM22_header_enable2, header_mask); + rfm22_write(rfm22b_dev, RFM22_header_enable3, header_mask); + // The destination ID and receive ID should be the same. + uint32_t id = rfm22_destinationID(rfm22b_dev); rfm22_write(rfm22b_dev, RFM22_check_header0, id & 0xff); rfm22_write(rfm22b_dev, RFM22_check_header1, (id >> 8) & 0xff); rfm22_write(rfm22b_dev, RFM22_check_header2, (id >> 16) & 0xff); @@ -1561,8 +1436,8 @@ static enum pios_radio_event rfm22_init(struct pios_rfm22b_dev *rfm22b_dev) rfm22_releaseBus(rfm22b_dev); // Initialize the frequency and datarate to te default. - rfm22_setNominalCarrierFrequency(rfm22b_dev, rfm22b_dev->init_frequency, rfm22b_dev->init_frequency, RFM22B_FREQUENCY_HOP_STEP_SIZE); - pios_rfm22_setDatarate(rfm22b_dev, RFM22B_DEFAULT_RX_DATARATE, true); + rfm22_setNominalCarrierFrequency(rfm22b_dev, 0); + pios_rfm22_setDatarate(rfm22b_dev); return RADIO_EVENT_INITIALIZED; } @@ -1580,15 +1455,19 @@ static enum pios_radio_event rfm22_init(struct pios_rfm22b_dev *rfm22b_dev) * @param[in] datarate The air datarate. * @param[in] data_whitening Is data whitening desired? */ -static void pios_rfm22_setDatarate(struct pios_rfm22b_dev *rfm22b_dev, enum rfm22b_datarate datarate, bool data_whitening) +static void pios_rfm22_setDatarate(struct pios_rfm22b_dev *rfm22b_dev) { - uint32_t datarate_bps = data_rate[datarate]; + enum rfm22b_datarate datarate = rfm22b_dev->datarate; + bool data_whitening = true; + uint32_t datarate_bps = data_rate[datarate]; - rfm22b_dev->max_packet_time = (uint16_t)((float)(PIOS_PH_MAX_PACKET * 8 * 1000) / (float)(datarate_bps) + 0.5f); + // Calculate the maximum packet length from the datarate. + float bytes_per_period = (float)datarate_bps * (float)(rfm22b_dev->packet_period) / 8000; - // Generate a pseudo-random number from 0-8 to add to the delay - uint8_t random = PIOS_CRC_updateByte(0, (uint8_t)(xTaskGetTickCount() & 0xff)) & 0x03; - rfm22b_dev->max_ack_delay = (uint16_t)((float)((sizeof(PHAckNackPacket) * 8 + TX_PREAMBLE_NIBBLES * 4) * 1000) / (float)(datarate_bps) + 0.5f) * 4 + 4 + random; + rfm22b_dev->max_packet_len = bytes_per_period - TX_PREAMBLE_NIBBLES / 2 - SYNC_BYTES - HEADER_BYTES - 5; + if (rfm22b_dev->max_packet_len > RFM22B_MAX_PACKET_LEN) { + rfm22b_dev->max_packet_len = RFM22B_MAX_PACKET_LEN; + } // Claim the SPI bus. rfm22_claimBus(rfm22b_dev); @@ -1651,16 +1530,17 @@ static void pios_rfm22_setDatarate(struct pios_rfm22b_dev *rfm22b_dev, enum rfm2 } /** - * Set the nominal carrier frequency and channel step size. + * Set the nominal carrier frequency, channel step size, and initial channel * * @param[in] rfm33b_dev The device structure pointer. - * @param[in] min_frequency The minimum frequenc to transmit on (in Hz). - * @param[in] max_frequency The maximum frequenc to transmit on (in Hz). - * @param[in] step_size The channel spacing (in Hz). + * @param[in] init_chan The initial channel to tune to. */ -static void rfm22_setNominalCarrierFrequency(struct pios_rfm22b_dev *rfm22b_dev, uint32_t min_frequency, uint32_t max_frequency, uint32_t step_size) +static void rfm22_setNominalCarrierFrequency(struct pios_rfm22b_dev *rfm22b_dev, uint8_t init_chan) { - uint32_t frequency_hz = min_frequency; + // Set the frequency channels to start at 430MHz + uint32_t frequency_hz = RFM22B_NOMINAL_CARRIER_FREQUENCY; + // The step size is 10MHz / 250 channels = 40khz, and the step size is specified in 10khz increments. + uint8_t freq_hop_step_size = 4; // holds the hbsel (1 or 2) uint8_t hbsel; @@ -1681,22 +1561,15 @@ static void rfm22_setNominalCarrierFrequency(struct pios_rfm22b_dev *rfm22b_dev, // Claim the SPI bus. rfm22_claimBus(rfm22b_dev); - // Calculate the number of frequency hopping channels. - rfm22b_dev->num_channels = (step_size == 0) ? 1 : (uint16_t)((max_frequency - min_frequency) / step_size); - - // initialize the frequency hopping step size (specified in 10khz increments). - uint32_t freq_hop_step_size = step_size / 10000; - if (freq_hop_step_size > 255) { - freq_hop_step_size = 255; - } - rfm22_write(rfm22b_dev, RFM22_frequency_hopping_step_size, (uint8_t)freq_hop_step_size); + // Setthe frequency hopping step size. + rfm22_write(rfm22b_dev, RFM22_frequency_hopping_step_size, freq_hop_step_size); // frequency hopping channel (0-255) - rfm22b_dev->frequency_step_size = 156.25f * hbsel; + rfm22b_dev->frequency_step_size = 156.25f * hbsel; // frequency hopping channel (0-255) - rfm22b_dev->frequency_hop_channel = 0; - rfm22_write(rfm22b_dev, RFM22_frequency_hopping_channel_select, 0); + rfm22b_dev->channel = init_chan; + rfm22_write(rfm22b_dev, RFM22_frequency_hopping_channel_select, init_chan); // no frequency offset rfm22_write(rfm22b_dev, RFM22_frequency_offset1, 0); @@ -1720,14 +1593,14 @@ static void rfm22_setNominalCarrierFrequency(struct pios_rfm22b_dev *rfm22b_dev, static bool rfm22_setFreqHopChannel(struct pios_rfm22b_dev *rfm22b_dev, uint8_t channel) { // set the frequency hopping channel - if (rfm22b_dev->frequency_hop_channel == channel) { + if (rfm22b_dev->channel == channel) { return false; } #ifdef PIOS_RFM22B_DEBUG_ON_TELEM D3_LED_TOGGLE; #endif // PIOS_RFM22B_DEBUG_ON_TELEM - rfm22b_dev->frequency_hop_channel = channel; - rfm22_write_claim(rfm22b_dev, RFM22_frequency_hopping_channel_select, channel); + rfm22b_dev->channel = channel; + rfm22_write(rfm22b_dev, RFM22_frequency_hopping_channel_select, channel); return true; } @@ -1777,10 +1650,6 @@ static void rfm22_rxFailure(struct pios_rfm22b_dev *rfm22b_dev) rfm22b_dev->rx_buffer_wr = 0; rfm22b_dev->packet_start_ticks = 0; rfm22b_dev->rfm22b_state = RFM22B_STATE_TRANSITION; - rfm22b_dev->rx_complete_ticks = xTaskGetTickCount(); - if (rfm22b_dev->rx_complete_ticks == 0) { - rfm22b_dev->rx_complete_ticks = 1; - } } @@ -1796,94 +1665,45 @@ static void rfm22_rxFailure(struct pios_rfm22b_dev *rfm22b_dev) */ static enum pios_radio_event radio_txStart(struct pios_rfm22b_dev *radio_dev) { - PHPacketHandle p = NULL; + uint8_t *p = radio_dev->rx_packet; + uint8_t len = 0; + uint8_t max_data_len = radio_dev->max_packet_len - RS_ECC_NPARITY; // Don't send if it's not our turn, or if we're receiving a packet. if (!rfm22_timeToSend(radio_dev) || !PIOS_RFM22B_InRxWait((uint32_t)radio_dev)) { return RADIO_EVENT_RX_MODE; } -#ifdef PIOS_PPM_RECEIVER - // Send a PPM packet? - if (radio_dev->send_ppm) { - p = (PHPacketHandle) & (radio_dev->ppm_packet); - radio_dev->send_ppm = false; - } -#endif - - // Send an ACK/NACK? - if (!p && radio_dev->send_ack_nack) { - p = (PHPacketHandle) & (radio_dev->ack_nack_packet); - radio_dev->send_ack_nack = false; - } - - // Resend a packet? - if (!p && radio_dev->resend_packet) { - p = radio_dev->prev_tx_packet; - radio_dev->prev_tx_packet = NULL; - radio_dev->resend_packet = false; - } - - // Don't send another packet if we're waiting for an ACK. - if (!p && radio_dev->prev_tx_packet) { + // Don't send anything if we're bound to a coordinator and not yet connected. + if (!rfm22_isCoordinator(radio_dev) && !rfm22_isConnected(radio_dev)) { return RADIO_EVENT_RX_MODE; } - // Send a connection request? - if (!p && radio_dev->send_connection_request) { - p = (PHPacketHandle) & (radio_dev->con_packet); - radio_dev->send_connection_request = false; - } - // Try to get some data to send - if (!p) { + if (radio_dev->tx_out_cb) { bool need_yield = false; - p = &radio_dev->data_packet; - p->header.type = PACKET_TYPE_DATA; - p->header.destination_id = radio_dev->destination_id; - if (radio_dev->tx_out_cb && (p->header.data_size == 0)) { - p->header.data_size = (radio_dev->tx_out_cb)(radio_dev->tx_out_context, p->data, PH_MAX_DATA, NULL, &need_yield); - } - - // Don't send any data until we're connected. - if (!rfm22_isConnected(radio_dev)) { - p->header.data_size = 0; - } - if (p->header.data_size == 0) { - p = NULL; - } + len = (radio_dev->tx_out_cb)(radio_dev->tx_out_context, p, max_data_len, NULL, &need_yield); } - // Send status? - if (!p && radio_dev->send_status) { - p = (PHPacketHandle) & (radio_dev->status_packet); - radio_dev->send_status = false; - } - - if (!p) { + // Always send a packet on the sync channel if this modem is a coordinator. + if ((len == 0) && ((radio_dev->channel_index != 0) || !rfm22_isCoordinator(radio_dev))) { return RADIO_EVENT_RX_MODE; } -#ifdef PIOS_RFM22B_DEBUG_ON_TELEM - D1_LED_ON; -#endif + // Increment the packet sequence number. + radio_dev->stats.tx_seq++; - // Add the packet sequence number. - p->header.seq_num = radio_dev->stats.tx_seq++; - - // Pass the time of the previous transmitted packet to use for synchronizing the clocks. - p->header.prev_tx_time = radio_dev->tx_complete_ticks; - - // Change the channel if necessary, but not when ACKing the connection request message. - if ((p->header.type != PACKET_TYPE_ACK) || (radio_dev->rx_packet.header.type != PACKET_TYPE_CON_REQUEST)) { - rfm22_changeChannel(radio_dev); - } + // Change the channel. + rfm22_changeChannel(radio_dev); // Add the error correcting code. - encode_data((unsigned char *)p, PHPacketSize(p), (unsigned char *)p); + if (len != 0) { + encode_data((unsigned char *)p, len, (unsigned char *)p); + } + len += RS_ECC_NPARITY; // Transmit the packet. - PIOS_RFM22B_TransmitPacket((uint32_t)radio_dev, p); + PIOS_RFM22B_TransmitPacket((uint32_t)radio_dev, p, len); return RADIO_EVENT_NUM_EVENTS; } @@ -1901,25 +1721,11 @@ static enum pios_radio_event radio_txData(struct pios_rfm22b_dev *radio_dev) // Is the transmition complete if (res == PIOS_RFM22B_TX_COMPLETE) { - radio_dev->stats.tx_byte_count += PH_PACKET_SIZE(radio_dev->tx_packet); - radio_dev->tx_complete_ticks = xTaskGetTickCount(); + radio_dev->tx_complete_ticks = xTaskGetTickCount(); // Is this an ACK? - bool is_ack = (radio_dev->tx_packet->header.type == PACKET_TYPE_ACK); ret_event = RADIO_EVENT_RX_MODE; - if (is_ack) { - // If this is an ACK for a connection request message we need to - // configure this modem from the connection request message. - if (radio_dev->rx_packet.header.type == PACKET_TYPE_CON_REQUEST) { - rfm22_setConnectionParameters(radio_dev); - } - } else if ((radio_dev->tx_packet->header.type != PACKET_TYPE_NACK) && - (radio_dev->tx_packet->header.type != PACKET_TYPE_PPM) && - (radio_dev->tx_packet->header.type != PACKET_TYPE_STATUS)) { - // We need to wait for an ACK if this packet it not an ACK, NACK, PPM, or status packet. - radio_dev->prev_tx_packet = radio_dev->tx_packet; - } - radio_dev->tx_packet = 0; + radio_dev->tx_packet_handle = 0; radio_dev->tx_data_wr = radio_dev->tx_data_rd = 0; // Start a new transaction radio_dev->packet_start_ticks = 0; @@ -1940,7 +1746,7 @@ static enum pios_radio_event radio_txData(struct pios_rfm22b_dev *radio_dev) */ static enum pios_radio_event radio_setRxMode(struct pios_rfm22b_dev *rfm22b_dev) { - if (!PIOS_RFM22B_ReceivePacket((uint32_t)rfm22b_dev, &(rfm22b_dev->rx_packet))) { + if (!PIOS_RFM22B_ReceivePacket((uint32_t)rfm22b_dev, rfm22b_dev->rx_packet)) { return RADIO_EVENT_NUM_EVENTS; } rfm22b_dev->packet_start_ticks = 0; @@ -1957,19 +1763,22 @@ static enum pios_radio_event radio_setRxMode(struct pios_rfm22b_dev *rfm22b_dev) * @param[in] rc_len The number of bytes received. * @return enum pios_radio_event The next event to inject */ -static enum pios_radio_event radio_receivePacket(struct pios_rfm22b_dev *radio_dev, PHPacketHandle p, uint16_t rx_len) +static enum pios_radio_event radio_receivePacket(struct pios_rfm22b_dev *radio_dev, uint8_t *p, uint16_t rx_len) { - portTickType curTicks = xTaskGetTickCount(); - + uint8_t data_len = rx_len - RS_ECC_NPARITY; // Attempt to correct any errors in the packet. - decode_data((unsigned char *)p, rx_len); - - bool good_packet = check_syndrome() == 0; + bool good_packet = true; bool corrected_packet = false; - // We have an error. Try to correct it. - if (!good_packet && (correct_errors_erasures((unsigned char *)p, rx_len, 0, 0) != 0)) { - // We corrected it - corrected_packet = true; + + if (data_len > 0) { + decode_data((unsigned char *)p, rx_len); + + good_packet = check_syndrome() == 0; + // We have an error. Try to correct it. + if (!good_packet && (correct_errors_erasures((unsigned char *)p, rx_len, 0, 0) != 0)) { + // We corrected it + corrected_packet = true; + } } // Set the packet status @@ -1985,101 +1794,22 @@ static enum pios_radio_event radio_receivePacket(struct pios_rfm22b_dev *radio_d enum pios_radio_event ret_event = RADIO_EVENT_RX_COMPLETE; if (good_packet || corrected_packet) { - switch (p->header.type) { - case PACKET_TYPE_STATUS: - ret_event = RADIO_EVENT_STATUS_RECEIVED; - break; - case PACKET_TYPE_CON_REQUEST: - ret_event = RADIO_EVENT_CONNECTION_REQUESTED; - break; - case PACKET_TYPE_DATA: - { - // Send the data to the com port - bool rx_need_yield; - if (radio_dev->rx_in_cb) { - (radio_dev->rx_in_cb)(radio_dev->rx_in_context, p->data, p->header.data_size, NULL, &rx_need_yield); - } - break; - } - case PACKET_TYPE_DUPLICATE_DATA: - break; - case PACKET_TYPE_ACK: - ret_event = RADIO_EVENT_PACKET_ACKED; - break; - case PACKET_TYPE_NACK: - ret_event = RADIO_EVENT_PACKET_NACKED; - break; - case PACKET_TYPE_PPM: - { -#if defined(PIOS_INCLUDE_GCSRCVR) || (defined(PIOS_INCLUDE_PPM_OUT) && defined(PIOS_PPM_OUTPUT)) || defined(PIOS_INCLUDE_RFM22B_RCVR) - PHPpmPacketHandle ppmp = (PHPpmPacketHandle)p; -#if defined(PIOS_INCLUDE_GCSRCVR) || (defined(PIOS_INCLUDE_PPM_OUT) && defined(PIOS_PPM_OUTPUT)) - bool ppm_output = false; -#endif -#endif -#if defined(PIOS_INCLUDE_RFM22B_RCVR) - ppm_output = true; - radio_dev->ppm_fresh = true; - for (uint8_t i = 0; i < PIOS_RFM22B_RCVR_MAX_CHANNELS; ++i) { - radio_dev->ppm_channel[i] = ppmp->channels[i]; - } -#endif -#if defined(PIOS_INCLUDE_PPM_OUT) && defined(PIOS_PPM_OUTPUT) - if (PIOS_PPM_OUTPUT) { - ppm_output = true; - for (uint8_t i = 0; i < PIOS_RFM22B_RCVR_MAX_CHANNELS; ++i) { - PIOS_PPM_OUT_Set(PIOS_PPM_OUTPUT, i, ppmp->channels[i]); - } - } -#endif -#if defined(PIOS_INCLUDE_GCSRCVR) - if (!ppm_output) { - GCSReceiverData gcsRcvr; - for (uint8_t i = 0; (i < PIOS_RFM22B_RCVR_MAX_CHANNELS) && (i < GCSRECEIVER_CHANNEL_NUMELEM); ++i) { - gcsRcvr.Channel[i] = ppmp->channels[i]; - } - GCSReceiverSet(&gcsRcvr); - } -#endif - break; - } - default: - break; + // Send the data to the com port + bool rx_need_yield; + if (radio_dev->rx_in_cb) { + (radio_dev->rx_in_cb)(radio_dev->rx_in_context, p, data_len, NULL, &rx_need_yield); } - uint16_t seq_num = radio_dev->rx_packet.header.seq_num; - if (rfm22_isConnected(radio_dev)) { - // Adjust the clock syncronization if this is the remote modem. - // The coordinator sends the time that the previous packet was finised transmitting, - // which should match the time that the last packet was received. - uint16_t prev_seq_num = radio_dev->stats.rx_seq; - if (seq_num == (prev_seq_num + 1)) { - portTickType local_rx_time = radio_dev->rx_complete_ticks; - portTickType remote_tx_time = radio_dev->rx_packet.header.prev_tx_time; - portTickType time_delta = remote_tx_time - local_rx_time; - portTickType time_delta_delta = (time_delta > radio_dev->time_delta) ? (time_delta - radio_dev->time_delta) : (radio_dev->time_delta - time_delta); - if (time_delta_delta <= 2) { - radio_dev->time_delta = remote_tx_time - local_rx_time; - } - } else if (seq_num > prev_seq_num) { - // Add any missed packets into the stats. - uint16_t missed_packets = seq_num - prev_seq_num - 1; - radio_dev->stats.rx_missed += missed_packets; - } + // We only synchronize the clock on packets from our coordinator on the sync channel. + if (!rfm22_isCoordinator(radio_dev) && (radio_dev->rx_destination_id == rfm22_destinationID(radio_dev)) && (radio_dev->channel_index == 0)) { + rfm22_synchronizeClock(radio_dev); + radio_dev->stats.link_state = OPLINKSTATUS_LINKSTATE_CONNECTED; + radio_dev->on_sync_channel = false; } - - // Update the sequence number - radio_dev->stats.rx_seq = seq_num; } else { ret_event = RADIO_EVENT_RX_COMPLETE; } - // Log the time that the packet was received. - radio_dev->rx_complete_ticks = curTicks; - if (radio_dev->rx_complete_ticks == 0) { - radio_dev->rx_complete_ticks = 1; - } - return ret_event; } @@ -2122,210 +1852,19 @@ static enum pios_radio_event radio_rxData(struct pios_rfm22b_dev *radio_dev) } /***************************************************************************** -* Packet Transmition Functions +* Link Statistics Functions *****************************************************************************/ /** - * Send a radio status message. - * - * @param[in] rfm22b_dev The device structure - */ -static void rfm22_sendStatus(struct pios_rfm22b_dev *rfm22b_dev) -{ - // Don't send if a status is already queued or if we're the coordinator and not connected. - if (rfm22b_dev->send_status || (rfm22b_dev->coordinator && !rfm22_isConnected(rfm22b_dev))) { - return; - } - - // Update the link quality metric. - rfm22_calculateLinkQuality(rfm22b_dev); - - // Queue the status message - if (rfm22_isConnected(rfm22b_dev)) { - rfm22b_dev->status_packet.header.destination_id = rfm22b_dev->destination_id; - } else if (rfm22b_dev->coordinator) { - return; - } else { - rfm22b_dev->status_packet.header.destination_id = 0xffffffff; // Broadcast - } - rfm22b_dev->status_packet.header.type = PACKET_TYPE_STATUS; - rfm22b_dev->status_packet.header.data_size = PH_STATUS_DATA_SIZE(&(rfm22b_dev->status_packet)); - rfm22b_dev->status_packet.source_id = rfm22b_dev->deviceID; - rfm22b_dev->status_packet.link_quality = rfm22b_dev->stats.link_quality; - rfm22b_dev->status_packet.received_rssi = rfm22b_dev->rssi_dBm; - rfm22b_dev->send_status = true; -} - -/** - * Send a PPM packet. - * - * @param[in] rfm22b_dev The device structure - */ -static void rfm22_sendPPM(__attribute__((unused)) struct pios_rfm22b_dev *rfm22b_dev) -{ -#ifdef PIOS_PPM_RECEIVER - // Only send PPM if we're connected - if (!rfm22_isConnected(rfm22b_dev)) { - return; - } - - // Just return if the PPM receiver is not configured. - if (PIOS_PPM_RECEIVER == 0) { - return; - } - - // See if we have any valid channels. - bool valid_input_detected = false; - for (uint8_t i = 0; i < PIOS_PPM_NUM_INPUTS; ++i) { - rfm22b_dev->ppm_packet.channels[i] = PIOS_RCVR_Read(PIOS_PPM_RECEIVER, i + 1); - if ((rfm22b_dev->ppm_packet.channels[i] != PIOS_RCVR_INVALID) && (rfm22b_dev->ppm_packet.channels[i] != PIOS_RCVR_TIMEOUT)) { - valid_input_detected = true; - } - } - - // Send the PPM packet if it's valid - if (valid_input_detected) { - rfm22b_dev->ppm_packet.header.destination_id = rfm22b_dev->destination_id; - rfm22b_dev->ppm_packet.header.type = PACKET_TYPE_PPM; - rfm22b_dev->ppm_packet.header.data_size = PH_PPM_DATA_SIZE(&(rfm22b_dev->ppm_packet)); - rfm22b_dev->send_ppm = true; - } -#endif /* ifdef PIOS_PPM_RECEIVER */ -} - -/** - * Send an ACK to a received packet. + * Update the modem pair status. * * @param[in] rfm22b_dev The device structure - * @return enum pios_radio_event The next event to inject */ -static enum pios_radio_event rfm22_sendAck(struct pios_rfm22b_dev *rfm22b_dev) +static void rfm22_updatePairStatus(struct pios_rfm22b_dev *radio_dev) { - // We don't ACK PPM or status packets. - if ((rfm22b_dev->rx_packet.header.type != PACKET_TYPE_PPM) && (rfm22b_dev->rx_packet.header.type != PACKET_TYPE_STATUS)) { - PHAckNackPacketHandle aph = (PHAckNackPacketHandle)(&(rfm22b_dev->ack_nack_packet)); - aph->header.destination_id = rfm22b_dev->destination_id; - aph->header.type = PACKET_TYPE_ACK; - aph->header.data_size = PH_ACK_NACK_DATA_SIZE(aph); - rfm22b_dev->send_ack_nack = true; - } - return RADIO_EVENT_TX_START; -} - -/** - * Send an NACK to a received packet. - * - * @param[in] rfm22b_dev The device structure - * @return enum pios_radio_event The next event to inject - */ -static enum pios_radio_event rfm22_sendNack(struct pios_rfm22b_dev *rfm22b_dev) -{ - PHAckNackPacketHandle aph = (PHAckNackPacketHandle)(&(rfm22b_dev->ack_nack_packet)); - - // We don't NACK PPM or status packets. - if ((rfm22b_dev->rx_packet.header.type != PACKET_TYPE_PPM) && (rfm22b_dev->rx_packet.header.type != PACKET_TYPE_STATUS)) { - aph->header.destination_id = rfm22b_dev->destination_id; - aph->header.type = PACKET_TYPE_NACK; - aph->header.data_size = PH_ACK_NACK_DATA_SIZE(aph); - rfm22b_dev->send_ack_nack = true; - } - return RADIO_EVENT_TX_START; -} - -/** - * Send a connection request message. - * - * @param[in] rfm22b_dev The device structure - * @return enum pios_radio_event The next event to inject - */ -static enum pios_radio_event rfm22_requestConnection(struct pios_rfm22b_dev *rfm22b_dev) -{ - PHConnectionPacketHandle cph = &(rfm22b_dev->con_packet); - - // Set our connection state to requesting connection. - rfm22b_dev->stats.link_state = OPLINKSTATUS_LINKSTATE_CONNECTING; - - // Fill in the connection request - rfm22b_dev->destination_id = rfm22b_dev->bindings[rfm22b_dev->cur_binding].pairID; - cph->header.destination_id = rfm22b_dev->destination_id; - cph->header.type = PACKET_TYPE_CON_REQUEST; - cph->header.data_size = PH_CONNECTION_DATA_SIZE(&(rfm22b_dev->con_packet)); - cph->source_id = rfm22b_dev->deviceID; - cph->status_rx_time = rfm22b_dev->rx_complete_ticks; - cph->main_port = rfm22b_dev->bindings[rfm22b_dev->cur_binding].main_port; - cph->flexi_port = rfm22b_dev->bindings[rfm22b_dev->cur_binding].flexi_port; - cph->vcp_port = rfm22b_dev->bindings[rfm22b_dev->cur_binding].vcp_port; - cph->com_speed = rfm22b_dev->bindings[rfm22b_dev->cur_binding].com_speed; - rfm22b_dev->send_connection_request = true; - rfm22b_dev->prev_tx_packet = NULL; - - return RADIO_EVENT_TX_START; -} - - -/***************************************************************************** -* Packet Receipt Functions -*****************************************************************************/ - -/** - * Receive an ACK. - * - * @param[in] rfm22b_dev The device structure - * @return enum pios_radio_event The next event to inject - */ -static enum pios_radio_event rfm22_receiveAck(struct pios_rfm22b_dev *rfm22b_dev) -{ - PHPacketHandle prev = rfm22b_dev->prev_tx_packet; - - // Clear the previous TX packet. - rfm22b_dev->prev_tx_packet = NULL; - - // Was this a connection request? - switch (prev->header.type) { - case PACKET_TYPE_CON_REQUEST: - rfm22_setConnectionParameters(rfm22b_dev); - break; - case PACKET_TYPE_DATA: - rfm22b_dev->data_packet.header.data_size = 0; - break; - } - - // Should we try to start another TX? - return RADIO_EVENT_TX_START; -} - -/** - * Receive an MACK. - * - * @param[in] rfm22b_dev The device structure - * @return enum pios_radio_event The next event to inject - */ -static enum pios_radio_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; - - // Increment the reset packet counter if we're connected. - if (rfm22_isConnected(rfm22b_dev)) { - rfm22b_add_rx_status(rfm22b_dev, RADIO_RESENT_TX_PACKET); - } - return RADIO_EVENT_TX_START; -} - -/** - * Receive a status packet - * - * @param[in] rfm22b_dev The device structure - * @return enum pios_radio_event The next event to inject - */ -static enum pios_radio_event rfm22_receiveStatus(struct pios_rfm22b_dev *radio_dev) -{ - PHStatusPacketHandle status = (PHStatusPacketHandle) & (radio_dev->rx_packet); - int8_t rssi = radio_dev->rssi_dBm; - int8_t afc = radio_dev->afc_correction_Hz; - uint32_t id = status->source_id; - enum pios_radio_event ret_event = RADIO_EVENT_RX_COMPLETE; + int8_t rssi = radio_dev->rssi_dBm; + int8_t afc = radio_dev->afc_correction_Hz; + uint32_t id = radio_dev->rx_destination_id; // Have we seen this device recently? bool found = false; @@ -2338,7 +1877,7 @@ static enum pios_radio_event rfm22_receiveStatus(struct pios_rfm22b_dev *radio_d } } - // If we have seen it, update the RSSI and reset the last contact couter + // If we have seen it, update the RSSI and reset the last contact counter if (found) { radio_dev->pair_stats[id_idx].rssi = rssi; radio_dev->pair_stats[id_idx].afc_correction = afc; @@ -2358,26 +1897,8 @@ static enum pios_radio_event rfm22_receiveStatus(struct pios_rfm22b_dev *radio_d radio_dev->pair_stats[min_idx].afc_correction = afc; radio_dev->pair_stats[min_idx].lastContact = 0; } - - // Send a connection request message if we're not connected, and this is a status message from a modem that we're bound to. - if (radio_dev->coordinator && !rfm22_isConnected(radio_dev)) { - for (uint8_t i = 0; i < OPLINKSETTINGS_BINDINGS_NUMELEM; ++i) { - if (radio_dev->bindings[i].pairID == id) { - radio_dev->cur_binding = i; - ret_event = RADIO_EVENT_REQUEST_CONNECTION; - break; - } - } - } - - return ret_event; } - -/***************************************************************************** -* Link Statistics Functions -*****************************************************************************/ - /** * Calculate the link quality from the packet receipt, tranmittion statistics. * @@ -2438,87 +1959,30 @@ static void rfm22b_add_rx_status(struct pios_rfm22b_dev *rfm22b_dev, enum pios_r *****************************************************************************/ /** - * Are we connected to the remote modem? + * Are we a coordinator modem? * * @param[in] rfm22b_dev The device structure */ -static bool rfm22_isConnected(struct pios_rfm22b_dev *rfm22b_dev) +static bool rfm22_isCoordinator(struct pios_rfm22b_dev *rfm22b_dev) { - return rfm22b_dev->stats.link_state == OPLINKSTATUS_LINKSTATE_CONNECTED; + return rfm22b_dev->coordinator; } /** - * Set the connection parameters from a connection request message. + * Returns the destination ID to send packets to. * - * @param[in] rfm22b_dev The device structure + * @param[in] rfm22b_id The RFM22B device index. + * @return The destination ID */ -static void rfm22_setConnectionParameters(struct pios_rfm22b_dev *rfm22b_dev) +uint32_t rfm22_destinationID(struct pios_rfm22b_dev *rfm22b_dev) { - PHConnectionPacketHandle cph = &(rfm22b_dev->con_packet); - - // Set our connection state to connected - rfm22b_dev->stats.link_state = OPLINKSTATUS_LINKSTATE_CONNECTED; - - // Call the com port configuration function - if (rfm22b_dev->com_config_cb) { - rfm22b_dev->com_config_cb(cph->main_port, cph->flexi_port, cph->vcp_port, cph->com_speed); + if (rfm22_isCoordinator(rfm22b_dev)) { + return rfm22b_dev->deviceID; + } else if (rfm22b_dev->coordinatorID) { + return rfm22b_dev->coordinatorID; + } else { + return 0xffffffff; } - - // Configure this modem min an max frequency. - rfm22_setNominalCarrierFrequency(rfm22b_dev, cph->min_frequency, cph->max_frequency, cph->channel_spacing); - - // Configure the modem datarate. - rfm22b_dev->datarate = RFM22_datarate_64000; - switch (cph->com_speed) { - case OPLINKSETTINGS_COMSPEED_2400: - rfm22b_dev->datarate = RFM22_datarate_8000; - break; - case OPLINKSETTINGS_COMSPEED_4800: - rfm22b_dev->datarate = RFM22_datarate_8000; - break; - case OPLINKSETTINGS_COMSPEED_9600: - rfm22b_dev->datarate = RFM22_datarate_16000; - break; - case OPLINKSETTINGS_COMSPEED_19200: - rfm22b_dev->datarate = RFM22_datarate_32000; - break; - case OPLINKSETTINGS_COMSPEED_38400: - rfm22b_dev->datarate = RFM22_datarate_57600; - break; - case OPLINKSETTINGS_COMSPEED_57600: - rfm22b_dev->datarate = RFM22_datarate_128000; - break; - case OPLINKSETTINGS_COMSPEED_115200: - rfm22b_dev->datarate = RFM22_datarate_192000; - break; - } - pios_rfm22_setDatarate(rfm22b_dev, rfm22b_dev->datarate, true); -} - -/** - * Accept a connection request. - * - * @param[in] rfm22b_dev The device structure - * @return enum pios_radio_event The next event to inject - */ -static enum pios_radio_event rfm22_acceptConnection(struct pios_rfm22b_dev *rfm22b_dev) -{ - // Copy the connection packet - PHConnectionPacketHandle cph = (PHConnectionPacketHandle)(&(rfm22b_dev->rx_packet)); - PHConnectionPacketHandle lcph = (PHConnectionPacketHandle)(&(rfm22b_dev->con_packet)); - - memcpy((uint8_t *)lcph, (uint8_t *)cph, PH_PACKET_SIZE((PHPacketHandle)cph)); - - // Set the destination ID to the source of the connection request message. - rfm22b_dev->destination_id = cph->source_id; - - // The remote modem sets the time delta between the two modems using the differene between the clock - // on the local modem when it sent the status packet and the time on the coordinator modem when it was received. - portTickType local_tx_time = rfm22b_dev->tx_complete_ticks; - portTickType remote_rx_time = cph->status_rx_time; - rfm22b_dev->time_delta = remote_rx_time - local_tx_time; - - return RADIO_EVENT_DEFAULT; } @@ -2526,6 +1990,23 @@ static enum pios_radio_event rfm22_acceptConnection(struct pios_rfm22b_dev *rfm2 * Frequency Hopping Functions *****************************************************************************/ +/** + * Synchronize the clock after a packet receive from our coordinator on the syncronization channel. + * This function should be called when a packet is received on the synchronization channel. + * + * @param[in] rfm22b_dev The device structure + */ +static void rfm22_synchronizeClock(struct pios_rfm22b_dev *rfm22b_dev) +{ + portTickType start_time = rfm22b_dev->packet_start_ticks; + + // This packet was transmitted on channel 0, calculate the time delta that will force us to transmit on channel 0 at the time this packet started. + uint16_t frequency_hop_cycle_time = rfm22b_dev->packet_period * rfm22b_dev->num_channels; + uint16_t time_delta = start_time % frequency_hop_cycle_time; + + rfm22b_dev->time_delta = frequency_hop_cycle_time - time_delta + 1; +} + /** * Return the extimated current clock ticks count on the coordinator modem. * This is the master clock used for all synchronization. @@ -2534,7 +2015,7 @@ static enum pios_radio_event rfm22_acceptConnection(struct pios_rfm22b_dev *rfm2 */ static portTickType rfm22_coordinatorTime(struct pios_rfm22b_dev *rfm22b_dev, portTickType ticks) { - if (rfm22b_dev->coordinator) { + if (rfm22_isCoordinator(rfm22b_dev)) { return ticks; } return ticks + rfm22b_dev->time_delta; @@ -2549,9 +2030,30 @@ static bool rfm22_timeToSend(struct pios_rfm22b_dev *rfm22b_dev) { portTickType time = rfm22_coordinatorTime(rfm22b_dev, xTaskGetTickCount()); - // Divide time into 8ms blocks. Coordinator sends in first 1 ms, and remote send in 4th. - // Channel changes occurs in the 7th. - return (rfm22b_dev->coordinator) ? ((time & 0x7) == 0) : ((time & 0x7) == 3); + if (!rfm22_isCoordinator(rfm22b_dev)) { + time += rfm22b_dev->packet_period - 1; + } else { + time -= 1; + } + return (time % (rfm22b_dev->packet_period * 2)) == 0; +} + +/** + * Calculate the nth channel index. + * + * @param[in] rfm22b_dev The device structure + * @param[in] index The channel index to calculate + */ +static uint8_t rfm22_calcChannel(struct pios_rfm22b_dev *rfm22b_dev, uint8_t index) +{ + // Make sure we don't index outside of the range. + uint8_t idx = index % rfm22b_dev->num_channels; + + rfm22b_dev->channel_index = idx; + if (idx == 0) { + rfm22b_dev->on_sync_channel = true; + } + return rfm22b_dev->channels[idx]; } /** @@ -2559,14 +2061,14 @@ static bool rfm22_timeToSend(struct pios_rfm22b_dev *rfm22b_dev) * * @param[in] rfm22b_dev The device structure */ -static uint8_t rfm22_calcChannel(struct pios_rfm22b_dev *rfm22b_dev) +static uint8_t rfm22_calcChannelFromClock(struct pios_rfm22b_dev *rfm22b_dev) { portTickType time = rfm22_coordinatorTime(rfm22b_dev, xTaskGetTickCount()); // Divide time into 8ms blocks. Coordinator sends in first 2 ms, and remote send in 5th and 6th ms. // Channel changes occur in the last 2 ms. - uint8_t n = (((time + 2) >> 3) & 0xff); + uint8_t n = (time / rfm22b_dev->packet_period) % rfm22b_dev->num_channels; - return PIOS_CRC_updateByte(0, n) % rfm22b_dev->num_channels; + return rfm22_calcChannel(rfm22b_dev, n); } /** @@ -2576,10 +2078,12 @@ static uint8_t rfm22_calcChannel(struct pios_rfm22b_dev *rfm22b_dev) */ static bool rfm22_changeChannel(struct pios_rfm22b_dev *rfm22b_dev) { - if (rfm22_isConnected(rfm22b_dev)) { - return rfm22_setFreqHopChannel(rfm22b_dev, rfm22_calcChannel(rfm22b_dev)); + // A disconnected non-coordinator modem should sit on the sync channel until connected. + if (!rfm22_isCoordinator(rfm22b_dev) && !rfm22_isConnected(rfm22b_dev)) { + return rfm22_setFreqHopChannel(rfm22b_dev, rfm22_calcChannel(rfm22b_dev, 0)); + } else { + return rfm22_setFreqHopChannel(rfm22b_dev, rfm22_calcChannelFromClock(rfm22b_dev)); } - return false; } @@ -2612,7 +2116,7 @@ static enum pios_radio_event rfm22_timeout(struct pios_rfm22b_dev *rfm22b_dev) rfm22b_dev->stats.timeouts++; rfm22b_dev->packet_start_ticks = 0; // Release the Tx packet if it's set. - if (rfm22b_dev->tx_packet != 0) { + if (rfm22b_dev->tx_packet_handle != 0) { rfm22b_dev->tx_data_rd = rfm22b_dev->tx_data_wr = 0; } rfm22b_dev->rfm22b_state = RFM22B_STATE_TRANSITION; diff --git a/flight/pios/common/pios_rfm22b_rcvr.c b/flight/pios/common/pios_rfm22b_rcvr.c index 81f43efa8..f0dbc3bc4 100644 --- a/flight/pios/common/pios_rfm22b_rcvr.c +++ b/flight/pios/common/pios_rfm22b_rcvr.c @@ -34,6 +34,8 @@ #include "pios_rfm22b_priv.h" +#include + /* Provide a RCVR driver */ static int32_t PIOS_RFM22B_RCVR_Get(uint32_t rcvr_id, uint8_t channel); static void PIOS_RFM22B_RCVR_Supervisor(uint32_t rcvr_id); diff --git a/flight/pios/inc/pios_rfm22b.h b/flight/pios/inc/pios_rfm22b.h index c6417cc80..b28f8ff6b 100644 --- a/flight/pios/inc/pios_rfm22b.h +++ b/flight/pios/inc/pios_rfm22b.h @@ -31,7 +31,7 @@ #ifndef PIOS_RFM22B_H #define PIOS_RFM22B_H -#include +#include #include /* Constant definitions */ @@ -104,28 +104,21 @@ struct rfm22b_stats { uint8_t link_state; }; -/* Callback function prototypes */ -typedef void (*PIOS_RFM22B_ComConfigCallback)(OPLinkSettingsRemoteMainPortOptions main_port, OPLinkSettingsRemoteFlexiPortOptions flexi_port, - OPLinkSettingsRemoteVCPPortOptions vcp_port, OPLinkSettingsComSpeedOptions com_speed); - /* Public Functions */ extern int32_t PIOS_RFM22B_Init(uint32_t *rfb22b_id, uint32_t spi_id, uint32_t slave_num, const struct pios_rfm22b_cfg *cfg); extern void PIOS_RFM22B_Reinit(uint32_t rfb22b_id); extern void PIOS_RFM22B_SetTxPower(uint32_t rfm22b_id, enum rfm22b_tx_power tx_pwr); -extern void PIOS_RFM22B_SetFrequencyRange(uint32_t rfm22b_id, uint32_t min_freq, uint32_t max_freq, uint32_t step_size); -extern void PIOS_RFM22B_SetInitialFrequency(uint32_t rfm22b_id, uint32_t init_freq); -extern void PIOS_RFM22B_SetDestinationId(uint32_t rfm22b_id, uint32_t dest_id); -extern void PIOS_RFM22B_SetComConfigCallback(uint32_t rfm22b_id, PIOS_RFM22B_ComConfigCallback cb); -extern void PIOS_RFM22B_SetBindings(uint32_t rfm22b_id, const uint32_t bindingPairIDs[], const uint8_t mainPortSettings[], - const uint8_t flexiPortSettings[], const uint8_t vcpPortSettings[], const uint8_t comSpeeds[]); +extern void PIOS_RFM22B_SetChannelConfig(uint32_t rfm22b_id, uint8_t num_chan, uint8_t min_chan, uint8_t max_chan, uint8_t chan_set, uint8_t packet_period, bool oneway); +extern void PIOS_RFM22B_SetCoordinator(uint32_t rfm22b_id, bool coordinator); +extern void PIOS_RFM22B_SetCoordinatorID(uint32_t rfm22b_id, uint32_t coord_id); extern uint32_t PIOS_RFM22B_DeviceID(uint32_t rfb22b_id); extern bool PIOS_RFM22B_IsCoordinator(uint32_t rfb22b_id); extern void PIOS_RFM22B_GetStats(uint32_t rfm22b_id, struct rfm22b_stats *stats); extern uint8_t PIOS_RFM2B_GetPairStats(uint32_t rfm22b_id, uint32_t *device_ids, int8_t *RSSIs, uint8_t max_pairs); extern bool PIOS_RFM22B_InRxWait(uint32_t rfb22b_id); extern bool PIOS_RFM22B_LinkStatus(uint32_t rfm22b_id); -extern bool PIOS_RFM22B_ReceivePacket(uint32_t rfm22b_id, PHPacketHandle p); -extern bool PIOS_RFM22B_TransmitPacket(uint32_t rfm22b_id, PHPacketHandle p); +extern bool PIOS_RFM22B_ReceivePacket(uint32_t rfm22b_id, uint8_t *p); +extern bool PIOS_RFM22B_TransmitPacket(uint32_t rfm22b_id, uint8_t *p, uint8_t len); extern pios_rfm22b_int_result PIOS_RFM22B_ProcessTx(uint32_t rfm22b_id); extern pios_rfm22b_int_result PIOS_RFM22B_ProcessRx(uint32_t rfm22b_id); diff --git a/flight/pios/inc/pios_rfm22b_priv.h b/flight/pios/inc/pios_rfm22b_priv.h index 8e274ef44..ad21e68a9 100644 --- a/flight/pios/inc/pios_rfm22b_priv.h +++ b/flight/pios/inc/pios_rfm22b_priv.h @@ -40,14 +40,14 @@ // ************************************ -#define RFM22_DEVICE_VERSION_V2 0x02 -#define RFM22_DEVICE_VERSION_A0 0x04 -#define RFM22_DEVICE_VERSION_B1 0x06 +#define RFM22B_MAX_PACKET_LEN 64 +#define RFM22B_NUM_CHANNELS 250 // ************************************ -#define RFM22_MIN_CARRIER_FREQUENCY_HZ 240000000ul -#define RFM22_MAX_CARRIER_FREQUENCY_HZ 930000000ul +#define RFM22_DEVICE_VERSION_V2 0x02 +#define RFM22_DEVICE_VERSION_A0 0x04 +#define RFM22_DEVICE_VERSION_B1 0x06 // ************************************ @@ -534,19 +534,12 @@ enum pios_rfm22b_dev_magic { enum pios_radio_state { RADIO_STATE_UNINITIALIZED, RADIO_STATE_INITIALIZING, - RADIO_STATE_REQUESTING_CONNECTION, - RADIO_STATE_ACCEPTING_CONNECTION, RADIO_STATE_RX_MODE, RADIO_STATE_RX_DATA, RADIO_STATE_RX_FAILURE, - RADIO_STATE_RECEIVING_STATUS, RADIO_STATE_TX_START, RADIO_STATE_TX_DATA, RADIO_STATE_TX_FAILURE, - RADIO_STATE_SENDING_ACK, - RADIO_STATE_SENDING_NACK, - RADIO_STATE_RECEIVING_ACK, - RADIO_STATE_RECEIVING_NACK, RADIO_STATE_TIMEOUT, RADIO_STATE_ERROR, RADIO_STATE_FATAL_ERROR, @@ -559,16 +552,9 @@ enum pios_radio_event { RADIO_EVENT_INT_RECEIVED, RADIO_EVENT_INITIALIZE, RADIO_EVENT_INITIALIZED, - RADIO_EVENT_REQUEST_CONNECTION, - RADIO_EVENT_CONNECTION_REQUESTED, - RADIO_EVENT_PACKET_ACKED, - RADIO_EVENT_PACKET_NACKED, - RADIO_EVENT_ACK_TIMEOUT, RADIO_EVENT_RX_MODE, RADIO_EVENT_RX_COMPLETE, - RADIO_EVENT_STATUS_RECEIVED, RADIO_EVENT_TX_START, - RADIO_EVENT_FAILURE, RADIO_EVENT_TIMEOUT, RADIO_EVENT_ERROR, RADIO_EVENT_FATAL_ERROR, @@ -603,14 +589,6 @@ typedef struct { uint8_t lastContact; } rfm22b_pair_stats; -typedef struct { - uint32_t pairID; - OPLinkSettingsRemoteMainPortOptions main_port; - OPLinkSettingsRemoteFlexiPortOptions flexi_port; - OPLinkSettingsRemoteVCPPortOptions vcp_port; - OPLinkSettingsComSpeedOptions com_speed; -} rfm22b_binding; - enum pios_rfm22b_chip_power_state { RFM22B_IDLE_STATE = 0x00, RFM22B_RX_STATE = 0x01, @@ -683,27 +661,22 @@ typedef struct { rfm22b_int_status_2 int_status_2; } rfm22b_device_status; - struct pios_rfm22b_dev { enum pios_rfm22b_dev_magic magic; struct pios_rfm22b_cfg cfg; // The SPI bus information - uint32_t spi_id; - uint32_t slave_num; + uint32_t spi_id; + uint32_t slave_num; + + // Should this modem ack as a coordinator. + bool coordinator; // The device ID - uint32_t deviceID; + uint32_t deviceID; - // The destination ID - uint32_t destination_id; - - // The list of bound radios. - rfm22b_binding bindings[OPLINKSETTINGS_BINDINGS_NUMELEM]; - uint8_t cur_binding; - - // Is this device a coordinator? - bool coordinator; + // The coodinator ID (0 if this modem is a coordinator). + uint32_t coordinatorID; // The task handle xTaskHandle taskHandle; @@ -714,9 +687,6 @@ struct pios_rfm22b_dev { // ISR pending semaphore xSemaphoreHandle isrPending; - // The com configuration callback - PIOS_RFM22B_ComConfigCallback com_config_cb; - // The COM callback functions. pios_com_callback rx_in_cb; uint32_t rx_in_context; @@ -749,77 +719,57 @@ struct pios_rfm22b_dev { struct rfm22b_stats stats; // Stats - uint16_t errors; + uint16_t errors; // RSSI in dBm - int8_t rssi_dBm; + int8_t rssi_dBm; // The tx data packet - PHPacket data_packet; + uint8_t tx_packet[RFM22B_MAX_PACKET_LEN]; // The current tx packet - PHPacketHandle tx_packet; - // The previous tx packet (waiting for an ACK) - PHPacketHandle prev_tx_packet; + uint8_t *tx_packet_handle; // The tx data read index - uint16_t tx_data_rd; + uint16_t tx_data_rd; // The tx data write index - uint16_t tx_data_wr; + uint16_t tx_data_wr; // The tx packet sequence number - uint16_t tx_seq; + uint16_t tx_seq; // The rx data packet - PHPacket rx_packet; + uint8_t rx_packet[RFM22B_MAX_PACKET_LEN]; // The rx data packet - PHPacketHandle rx_packet_handle; + uint8_t *rx_packet_handle; // The receive buffer write index - uint16_t rx_buffer_wr; + uint16_t rx_buffer_wr; // The receive buffer write index - uint16_t rx_packet_len; + uint16_t rx_packet_len; + // The id that the packet was received from + uint32_t rx_destination_id; + // The maximum packet length (including header, etc.) + uint8_t max_packet_len; + // The time alloted for transmission of a packet. + uint8_t packet_period; + // Do all packets originate from the coordinator modem? + bool one_way_link; - // 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; - bool send_ack_nack; - bool resend_packet; - - // The initial frequency - uint32_t init_frequency; + // The channel list + uint8_t channels[RFM22B_NUM_CHANNELS]; // The number of frequency hopping channels. - uint16_t num_channels; - + uint8_t num_channels; // The frequency hopping step size float frequency_step_size; // current frequency hop channel - uint8_t frequency_hop_channel; + uint8_t channel; + // current frequency hop channel index + uint8_t channel_index; // afc correction reading (in Hz) int8_t afc_correction_Hz; // The packet timers. portTickType packet_start_ticks; portTickType tx_complete_ticks; - portTickType rx_complete_ticks; portTickType time_delta; - - // The maximum time (ms) that it should take to transmit / receive a packet. - uint32_t max_packet_time; - - // The maximum time to wait for an ACK. - uint8_t max_ack_delay; + bool on_sync_channel; #ifdef PIOS_INCLUDE_RFM22B_RCVR // The PPM channel values diff --git a/flight/targets/boards/oplinkmini/firmware/inc/pios_config.h b/flight/targets/boards/oplinkmini/firmware/inc/pios_config.h index 992cb67f3..da5b7d039 100644 --- a/flight/targets/boards/oplinkmini/firmware/inc/pios_config.h +++ b/flight/targets/boards/oplinkmini/firmware/inc/pios_config.h @@ -118,7 +118,7 @@ #define PIOS_INCLUDE_RFM22B_COM /* #define PIOS_INCLUDE_RFM22B_RCVR */ #define PIOS_INCLUDE_PPM_OUT -/* #define PIOS_RFM22B_DEBUG_ON_TELEM */ +#define PIOS_RFM22B_DEBUG_ON_TELEM /* PIOS misc peripherals */ /* #define PIOS_INCLUDE_VIDEO */ diff --git a/flight/targets/boards/oplinkmini/firmware/pios_board.c b/flight/targets/boards/oplinkmini/firmware/pios_board.c index dbcf00416..ec865d3a6 100644 --- a/flight/targets/boards/oplinkmini/firmware/pios_board.c +++ b/flight/targets/boards/oplinkmini/firmware/pios_board.c @@ -75,6 +75,12 @@ uint8_t *pios_uart_tx_buffer; uintptr_t pios_uavo_settings_fs_id; +// Forward definitions +static void PIOS_InitUartMainPort(); +static void PIOS_InitUartFlexiPort(); +static void PIOS_InitPPMMainPort(bool input); +static void PIOS_InitPPMFlexiPort(bool input); + /** * PIOS_Board_Init() * initializes all the core subsystems on this specific hardware @@ -216,12 +222,14 @@ void PIOS_Board_Init(void) /* Initalize the RFM22B radio COM device. */ #if defined(PIOS_INCLUDE_RFM22B) { + // Configure the RFM22B device const struct pios_board_info *bdinfo = &pios_board_info_blob; const struct pios_rfm22b_cfg *rfm22b_cfg = PIOS_BOARD_HW_DEFS_GetRfm22Cfg(bdinfo->board_rev); if (PIOS_RFM22B_Init(&pios_rfm22b_id, PIOS_RFM22_SPI_PORT, rfm22b_cfg->slave_num, rfm22b_cfg)) { PIOS_Assert(0); } + // Configure the radio com interface uint8_t *rx_buffer = (uint8_t *)pvPortMalloc(PIOS_COM_RFM22B_RF_RX_BUF_LEN); uint8_t *tx_buffer = (uint8_t *)pvPortMalloc(PIOS_COM_RFM22B_RF_TX_BUF_LEN); PIOS_Assert(rx_buffer); @@ -231,10 +239,40 @@ void PIOS_Board_Init(void) tx_buffer, PIOS_COM_RFM22B_RF_TX_BUF_LEN)) { PIOS_Assert(0); } + uint32_t comBaud = 9600; + switch (oplinkSettings.ComSpeed) { + case OPLINKSETTINGS_COMSPEED_2400: + comBaud = 2400; + break; + case OPLINKSETTINGS_COMSPEED_4800: + comBaud = 4800; + break; + case OPLINKSETTINGS_COMSPEED_9600: + comBaud = 9600; + break; + case OPLINKSETTINGS_COMSPEED_19200: + comBaud = 19200; + break; + case OPLINKSETTINGS_COMSPEED_38400: + comBaud = 38400; + break; + case OPLINKSETTINGS_COMSPEED_57600: + comBaud = 57600; + break; + case OPLINKSETTINGS_COMSPEED_115200: + comBaud = 115200; + break; + } + PIOS_COM_ChangeBaud(pios_com_rfm22b_id, comBaud); - /* Set the RFM22B bindings. */ - PIOS_RFM22B_SetBindings(pios_rfm22b_id, oplinkSettings.Bindings, oplinkSettings.RemoteMainPort, - oplinkSettings.RemoteFlexiPort, oplinkSettings.RemoteVCPPort, oplinkSettings.ComSpeed); + // Set the radio configuration parameters. + PIOS_RFM22B_SetChannelConfig(pios_rfm22b_id, oplinkSettings.NumChannels, oplinkSettings.MinChannel, oplinkSettings.MaxChannel, oplinkSettings.ChannelSet, + oplinkSettings.PacketTime, (oplinkSettings.OneWayLink == OPLINKSETTINGS_ONEWAYLINK_TRUE)); + PIOS_RFM22B_SetCoordinator(pios_rfm22b_id, (oplinkSettings.Coordinator == OPLINKSETTINGS_COORDINATOR_TRUE)); + PIOS_RFM22B_SetCoordinatorID(pios_rfm22b_id, oplinkSettings.CoordID); + + // Reinitilize the modem to affect te changes. + PIOS_RFM22B_Reinit(pios_rfm22b_id); } #endif /* PIOS_INCLUDE_RFM22B */ @@ -242,6 +280,75 @@ void PIOS_Board_Init(void) pios_uart_rx_buffer = (uint8_t *)pvPortMalloc(PIOS_COM_TELEM_RX_BUF_LEN); pios_uart_tx_buffer = (uint8_t *)pvPortMalloc(PIOS_COM_TELEM_TX_BUF_LEN); + // Configure the main port + bool is_coordinator = PIOS_RFM22B_IsCoordinator(pios_rfm22b_id); + switch (oplinkSettings.MainPort) { + case OPLINKSETTINGS_MAINPORT_TELEMETRY: + case OPLINKSETTINGS_MAINPORT_SERIAL: + /* Configure the main port for uart serial */ + PIOS_InitUartMainPort(); + PIOS_COM_TELEMETRY = PIOS_COM_TELEM_UART_MAIN; + break; + case OPLINKSETTINGS_MAINPORT_PPM: + PIOS_InitPPMMainPort(is_coordinator); + break; + case OPLINKSETTINGS_MAINPORT_DISABLED: + break; + } + + // Configure the flexi port + switch (oplinkSettings.FlexiPort) { + case OPLINKSETTINGS_FLEXIPORT_TELEMETRY: + case OPLINKSETTINGS_FLEXIPORT_SERIAL: + /* Configure the flexi port as uart serial */ + PIOS_InitUartFlexiPort(); + PIOS_COM_TELEMETRY = PIOS_COM_TELEM_UART_FLEXI; + break; + case OPLINKSETTINGS_FLEXIPORT_PPM: + PIOS_InitPPMFlexiPort(is_coordinator); + break; + case OPLINKSETTINGS_FLEXIPORT_DISABLED: + break; + } + + // Configure the USB VCP port + switch (oplinkSettings.VCPPort) { + case OPLINKSETTINGS_VCPPORT_SERIAL: + PIOS_COM_TELEMETRY = PIOS_COM_TELEM_USB_VCP; + break; + case OPLINKSETTINGS_VCPPORT_DISABLED: + break; + } + + // Update the com baud rate. + uint32_t comBaud = 9600; + switch (oplinkSettings.ComSpeed) { + case OPLINKSETTINGS_COMSPEED_2400: + comBaud = 2400; + break; + case OPLINKSETTINGS_COMSPEED_4800: + comBaud = 4800; + break; + case OPLINKSETTINGS_COMSPEED_9600: + comBaud = 9600; + break; + case OPLINKSETTINGS_COMSPEED_19200: + comBaud = 19200; + break; + case OPLINKSETTINGS_COMSPEED_38400: + comBaud = 38400; + break; + case OPLINKSETTINGS_COMSPEED_57600: + comBaud = 57600; + break; + case OPLINKSETTINGS_COMSPEED_115200: + comBaud = 115200; + break; + } + if (PIOS_COM_TELEMETRY) { + PIOS_COM_ChangeBaud(PIOS_COM_TELEMETRY, comBaud); + } + /* Remap AFIO pin */ GPIO_PinRemapConfig(GPIO_Remap_SWJ_NoJTRST, ENABLE); @@ -251,7 +358,7 @@ void PIOS_Board_Init(void) PIOS_GPIO_Init(); } -void PIOS_InitUartMainPort() +static void PIOS_InitUartMainPort() { #ifndef PIOS_RFM22B_DEBUG_ON_TELEM uint32_t pios_usart1_id; @@ -268,7 +375,7 @@ void PIOS_InitUartMainPort() #endif } -void PIOS_InitUartFlexiPort() +static void PIOS_InitUartFlexiPort() { uint32_t pios_usart3_id; @@ -284,7 +391,7 @@ void PIOS_InitUartFlexiPort() } } -void PIOS_InitPPMMainPort(bool input) +static void PIOS_InitPPMMainPort(bool input) { #if defined(PIOS_INCLUDE_PPM) /* PPM input is configured on the coordinator modem and output on the remote modem. */ @@ -300,7 +407,7 @@ void PIOS_InitPPMMainPort(bool input) #endif /* PIOS_INCLUDE_PPM */ } -void PIOS_InitPPMFlexiPort(bool input) +static void PIOS_InitPPMFlexiPort(bool input) { #if defined(PIOS_INCLUDE_PPM) /* PPM input is configured on the coordinator modem and output on the remote modem. */ diff --git a/flight/targets/boards/revolution/firmware/UAVObjects.inc b/flight/targets/boards/revolution/firmware/UAVObjects.inc index 57c9f180e..a8f47f8ba 100644 --- a/flight/targets/boards/revolution/firmware/UAVObjects.inc +++ b/flight/targets/boards/revolution/firmware/UAVObjects.inc @@ -87,7 +87,10 @@ UAVOBJSRCFILENAMES += hwsettings UAVOBJSRCFILENAMES += receiveractivity UAVOBJSRCFILENAMES += cameradesired UAVOBJSRCFILENAMES += camerastabsettings -UAVOBJSRCFILENAMES += altitudeholdsettings +UAVOBJSRCFILENAMES += altitudeholdsettings +UAVOBJSRCFILENAMES += oplinksettings +UAVOBJSRCFILENAMES += oplinkstatus + UAVOBJSRCFILENAMES += altitudeholddesired UAVOBJSRCFILENAMES += waypoint UAVOBJSRCFILENAMES += waypointactive diff --git a/flight/targets/boards/revolution/firmware/inc/pios_config.h b/flight/targets/boards/revolution/firmware/inc/pios_config.h index ab74c31a0..7ef8dc475 100644 --- a/flight/targets/boards/revolution/firmware/inc/pios_config.h +++ b/flight/targets/boards/revolution/firmware/inc/pios_config.h @@ -118,7 +118,7 @@ #define PIOS_INCLUDE_RFM22B_COM #define PIOS_INCLUDE_RFM22B_RCVR /* #define PIOS_INCLUDE_PPM_OUT */ -/* #define PIOS_RFM22B_DEBUG_ON_TELEM */ +#define PIOS_RFM22B_DEBUG_ON_TELEM /* PIOS misc peripherals */ /* #define PIOS_INCLUDE_VIDEO */ diff --git a/flight/targets/boards/revolution/firmware/pios_board.c b/flight/targets/boards/revolution/firmware/pios_board.c index 229acb3b5..855d9a849 100644 --- a/flight/targets/boards/revolution/firmware/pios_board.c +++ b/flight/targets/boards/revolution/firmware/pios_board.c @@ -30,6 +30,8 @@ #include #include #include +#include +#include #include /* @@ -391,8 +393,9 @@ void PIOS_Board_Init(void) /* Initialize UAVObject libraries */ EventDispatcherInitialize(); UAVObjInitialize(); - HwSettingsInitialize(); + OPLinkStatusInitialize(); + /* Initialize the alarms library */ AlarmsInitialize(); @@ -704,25 +707,54 @@ void PIOS_Board_Init(void) /* Initalize the RFM22B radio COM device. */ #if defined(PIOS_INCLUDE_RFM22B) + /* Is the radio turned on? */ uint8_t hwsettings_radioport; HwSettingsRadioPortGet(&hwsettings_radioport); - uint8_t hwsettings_maxrfpower; - HwSettingsMaxRFPowerGet(&hwsettings_maxrfpower); - uint32_t hwsettings_deffreq; - HwSettingsDefaultFrequencyGet(&hwsettings_deffreq); switch (hwsettings_radioport) { case HWSETTINGS_RADIOPORT_DISABLED: break; case HWSETTINGS_RADIOPORT_TELEMETRY: { + // Initialize out status object. + OPLinkStatusData oplinkStatus; + OPLinkStatusGet(&oplinkStatus); + oplinkStatus.BoardType = bdinfo->board_type; + PIOS_BL_HELPER_FLASH_Read_Description(oplinkStatus.Description, OPLINKSTATUS_DESCRIPTION_NUMELEM); + PIOS_SYS_SerialNumberGetBinary(oplinkStatus.CPUSerial); + oplinkStatus.BoardRevision = bdinfo->board_rev; + OPLinkStatusSet(&oplinkStatus); + + /* Initialize the OPLinkSettings object. */ + OPLinkSettingsInitialize(); + + /* Fetch the OPinkSettings object. */ + OPLinkSettingsData oplinkSettings; + OPLinkSettingsGet(&oplinkSettings); + + /* Configure the RFM22B device. */ const struct pios_rfm22b_cfg *rfm22b_cfg = PIOS_BOARD_HW_DEFS_GetRfm22Cfg(bdinfo->board_rev); if (PIOS_RFM22B_Init(&pios_rfm22b_id, PIOS_RFM22_SPI_PORT, rfm22b_cfg->slave_num, rfm22b_cfg)) { PIOS_Assert(0); } - // Set the modem parameters and reinitilize the modem. - PIOS_RFM22B_SetInitialFrequency(pios_rfm22b_id, hwsettings_deffreq); - switch (hwsettings_maxrfpower) { + /* Configure the radio com interface */ + uint8_t *rx_buffer = (uint8_t *)pvPortMalloc(PIOS_COM_RFM22B_RF_RX_BUF_LEN); + uint8_t *tx_buffer = (uint8_t *)pvPortMalloc(PIOS_COM_RFM22B_RF_TX_BUF_LEN); + PIOS_Assert(rx_buffer); + PIOS_Assert(tx_buffer); + if (PIOS_COM_Init(&pios_com_telem_rf_id, &pios_rfm22b_com_driver, pios_rfm22b_id, + rx_buffer, PIOS_COM_RFM22B_RF_RX_BUF_LEN, + tx_buffer, PIOS_COM_RFM22B_RF_TX_BUF_LEN)) { + PIOS_Assert(0); + } + + /* Set the radio configuration parameters. */ + PIOS_RFM22B_SetChannelConfig(pios_rfm22b_id, oplinkSettings.NumChannels, oplinkSettings.MinChannel, oplinkSettings.MaxChannel, oplinkSettings.ChannelSet, + oplinkSettings.PacketTime, (oplinkSettings.OneWayLink == OPLINKSETTINGS_ONEWAYLINK_TRUE)); + PIOS_RFM22B_SetCoordinatorID(pios_rfm22b_id, oplinkSettings.CoordID); + + /* Set the modem Tx poer level */ + switch (oplinkSettings.MaxRFPower) { case OPLINKSETTINGS_MAXRFPOWER_125: PIOS_RFM22B_SetTxPower(pios_rfm22b_id, RFM22_tx_pwr_txpow_0); break; @@ -751,19 +783,10 @@ void PIOS_Board_Init(void) // do nothing break; } + + /* Reinitialize the modem. */ PIOS_RFM22B_Reinit(pios_rfm22b_id); -#ifdef PIOS_INCLUDE_RFM22B_COM - uint8_t *rx_buffer = (uint8_t *)pvPortMalloc(PIOS_COM_RFM22B_RF_RX_BUF_LEN); - uint8_t *tx_buffer = (uint8_t *)pvPortMalloc(PIOS_COM_RFM22B_RF_TX_BUF_LEN); - PIOS_Assert(rx_buffer); - PIOS_Assert(tx_buffer); - if (PIOS_COM_Init(&pios_com_telem_rf_id, &pios_rfm22b_com_driver, pios_rfm22b_id, - rx_buffer, PIOS_COM_RFM22B_RF_RX_BUF_LEN, - tx_buffer, PIOS_COM_RFM22B_RF_TX_BUF_LEN)) { - PIOS_Assert(0); - } -#endif #ifdef PIOS_INCLUDE_RFM22B_RCVR if (PIOS_RFM22B_RCVR_Init(pios_rfm22b_id) != 0) { PIOS_Assert(0); diff --git a/flight/targets/boards/revolution/pios_board.h b/flight/targets/boards/revolution/pios_board.h index 8bdebb671..369d0e26b 100644 --- a/flight/targets/boards/revolution/pios_board.h +++ b/flight/targets/boards/revolution/pios_board.h @@ -229,6 +229,7 @@ extern uint32_t pios_packet_handler; #define PIOS_RCVR_MAX_CHANNELS 12 #define PIOS_GCSRCVR_TIMEOUT_MS 100 #define PIOS_RFM22B_RCVR_TIMEOUT_MS 200 +#define PIOS_OPLINK_RCVR_TIMEOUT_MS 100 // ------------------------- // Receiver PPM input diff --git a/ground/openpilotgcs/src/plugins/config/configpipxtremewidget.cpp b/ground/openpilotgcs/src/plugins/config/configpipxtremewidget.cpp index 27f0603ac..c74357f22 100644 --- a/ground/openpilotgcs/src/plugins/config/configpipxtremewidget.cpp +++ b/ground/openpilotgcs/src/plugins/config/configpipxtremewidget.cpp @@ -63,11 +63,16 @@ ConfigPipXtremeWidget::ConfigPipXtremeWidget(QWidget *parent) : ConfigTaskWidget addUAVObjectToWidgetRelation("OPLinkSettings", "MainPort", m_oplink->MainPort); addUAVObjectToWidgetRelation("OPLinkSettings", "FlexiPort", m_oplink->FlexiPort); addUAVObjectToWidgetRelation("OPLinkSettings", "VCPPort", m_oplink->VCPPort); + addUAVObjectToWidgetRelation("OPLinkSettings", "ComSpeed", m_oplink->ComSpeed); addUAVObjectToWidgetRelation("OPLinkSettings", "MaxRFPower", m_oplink->MaxRFTxPower); - addUAVObjectToWidgetRelation("OPLinkSettings", "MinFrequency", m_oplink->MinimumFrequency); - addUAVObjectToWidgetRelation("OPLinkSettings", "MaxFrequency", m_oplink->MaximumFrequency); - addUAVObjectToWidgetRelation("OPLinkSettings", "InitFrequency", m_oplink->InitFrequency); - addUAVObjectToWidgetRelation("OPLinkSettings", "ChannelSpacing", m_oplink->StepSize); + addUAVObjectToWidgetRelation("OPLinkSettings", "NumChannels", m_oplink->NumChannels); + addUAVObjectToWidgetRelation("OPLinkSettings", "MinChannel", m_oplink->MinimumChannel); + addUAVObjectToWidgetRelation("OPLinkSettings", "MaxChannel", m_oplink->MaximumChannel); + addUAVObjectToWidgetRelation("OPLinkSettings", "ChannelSet", m_oplink->ChannelSet); + addUAVObjectToWidgetRelation("OPLinkSettings", "PacketTime", m_oplink->PacketLength); + addUAVObjectToWidgetRelation("OPLinkSettings", "CoordID", m_oplink->CoordID); + addUAVObjectToWidgetRelation("OPLinkSettings", "Coordinator", m_oplink->Coordinator); + addUAVObjectToWidgetRelation("OPLinkSettings", "OneWayLink", m_oplink->OneWayLink); addUAVObjectToWidgetRelation("OPLinkStatus", "DeviceID", m_oplink->DeviceID); addUAVObjectToWidgetRelation("OPLinkStatus", "RxGood", m_oplink->Good); @@ -89,82 +94,11 @@ ConfigPipXtremeWidget::ConfigPipXtremeWidget(QWidget *parent) : ConfigTaskWidget addUAVObjectToWidgetRelation("OPLinkStatus", "RXRate", m_oplink->RXRate); addUAVObjectToWidgetRelation("OPLinkStatus", "TXRate", m_oplink->TXRate); - signalMapperAddBinding = new QSignalMapper(this); - signalMapperRemBinding = new QSignalMapper(this); - connect(m_oplink->BindingAdd_1, SIGNAL(clicked()), signalMapperAddBinding, SLOT(map())); - signalMapperAddBinding->setMapping(m_oplink->BindingAdd_1, (QWidget *)(m_oplink->BindingID_1)); - connect(m_oplink->BindingRemove_1, SIGNAL(clicked()), signalMapperRemBinding, SLOT(map())); - signalMapperRemBinding->setMapping(m_oplink->BindingRemove_1, (QWidget *)(m_oplink->BindingID_1)); - addUAVObjectToWidgetRelation("OPLinkSettings", "Bindings", m_oplink->BindingID_1, "0"); - addUAVObjectToWidgetRelation("OPLinkSettings", "RemoteMainPort", m_oplink->RemoteMainPort_1, "0"); - addUAVObjectToWidgetRelation("OPLinkSettings", "RemoteFlexiPort", m_oplink->RemoteFlexiPort_1, "0"); - addUAVObjectToWidgetRelation("OPLinkSettings", "RemoteVCPPort", m_oplink->RemoteVCPPort_1, "0"); - addUAVObjectToWidgetRelation("OPLinkSettings", "ComSpeed", m_oplink->ComSpeed_1, "0"); - connect(m_oplink->BindingAdd_2, SIGNAL(clicked()), signalMapperAddBinding, SLOT(map())); - signalMapperAddBinding->setMapping(m_oplink->BindingAdd_2, (QWidget *)(m_oplink->BindingID_2)); - connect(m_oplink->BindingRemove_2, SIGNAL(clicked()), signalMapperRemBinding, SLOT(map())); - signalMapperRemBinding->setMapping(m_oplink->BindingRemove_2, (QWidget *)(m_oplink->BindingID_2)); - addUAVObjectToWidgetRelation("OPLinkSettings", "Bindings", m_oplink->BindingID_2, "1"); - addUAVObjectToWidgetRelation("OPLinkSettings", "RemoteMainPort", m_oplink->RemoteMainPort_2, "1"); - addUAVObjectToWidgetRelation("OPLinkSettings", "RemoteFlexiPort", m_oplink->RemoteFlexiPort_2, "1"); - addUAVObjectToWidgetRelation("OPLinkSettings", "RemoteVCPPort", m_oplink->RemoteVCPPort_2, "1"); - addUAVObjectToWidgetRelation("OPLinkSettings", "ComSpeed", m_oplink->ComSpeed_2, "1"); - connect(m_oplink->BindingAdd_3, SIGNAL(clicked()), signalMapperAddBinding, SLOT(map())); - signalMapperAddBinding->setMapping(m_oplink->BindingAdd_3, (QWidget *)(m_oplink->BindingID_3)); - connect(m_oplink->BindingRemove_3, SIGNAL(clicked()), signalMapperRemBinding, SLOT(map())); - signalMapperRemBinding->setMapping(m_oplink->BindingRemove_3, (QWidget *)(m_oplink->BindingID_3)); - addUAVObjectToWidgetRelation("OPLinkSettings", "Bindings", m_oplink->BindingID_3, "2"); - addUAVObjectToWidgetRelation("OPLinkSettings", "RemoteMainPort", m_oplink->RemoteMainPort_3, "2"); - addUAVObjectToWidgetRelation("OPLinkSettings", "RemoteFlexiPort", m_oplink->RemoteFlexiPort_3, "2"); - addUAVObjectToWidgetRelation("OPLinkSettings", "RemoteVCPPort", m_oplink->RemoteVCPPort_3, "2"); - addUAVObjectToWidgetRelation("OPLinkSettings", "ComSpeed", m_oplink->ComSpeed_3, "2"); - connect(m_oplink->BindingAdd_4, SIGNAL(clicked()), signalMapperAddBinding, SLOT(map())); - signalMapperAddBinding->setMapping(m_oplink->BindingAdd_4, (QWidget *)(m_oplink->BindingID_4)); - connect(m_oplink->BindingRemove_4, SIGNAL(clicked()), signalMapperRemBinding, SLOT(map())); - signalMapperRemBinding->setMapping(m_oplink->BindingRemove_4, (QWidget *)(m_oplink->BindingID_4)); - addUAVObjectToWidgetRelation("OPLinkSettings", "Bindings", m_oplink->BindingID_4, "3"); - addUAVObjectToWidgetRelation("OPLinkSettings", "RemoteMainPort", m_oplink->RemoteMainPort_4, "3"); - addUAVObjectToWidgetRelation("OPLinkSettings", "RemoteFlexiPort", m_oplink->RemoteFlexiPort_4, "3"); - addUAVObjectToWidgetRelation("OPLinkSettings", "RemoteVCPPort", m_oplink->RemoteVCPPort_4, "3"); - addUAVObjectToWidgetRelation("OPLinkSettings", "ComSpeed", m_oplink->ComSpeed_4, "3"); - connect(m_oplink->BindingAdd_5, SIGNAL(clicked()), signalMapperAddBinding, SLOT(map())); - signalMapperAddBinding->setMapping(m_oplink->BindingAdd_5, (QWidget *)(m_oplink->BindingID_5)); - connect(m_oplink->BindingRemove_5, SIGNAL(clicked()), signalMapperRemBinding, SLOT(map())); - signalMapperRemBinding->setMapping(m_oplink->BindingRemove_5, (QWidget *)(m_oplink->BindingID_5)); - addUAVObjectToWidgetRelation("OPLinkSettings", "Bindings", m_oplink->BindingID_5, "4"); - addUAVObjectToWidgetRelation("OPLinkSettings", "RemoteMainPort", m_oplink->RemoteMainPort_5, "4"); - addUAVObjectToWidgetRelation("OPLinkSettings", "RemoteFlexiPort", m_oplink->RemoteFlexiPort_5, "4"); - addUAVObjectToWidgetRelation("OPLinkSettings", "RemoteVCPPort", m_oplink->RemoteVCPPort_5, "4"); - addUAVObjectToWidgetRelation("OPLinkSettings", "ComSpeed", m_oplink->ComSpeed_5, "4"); - connect(m_oplink->BindingAdd_6, SIGNAL(clicked()), signalMapperAddBinding, SLOT(map())); - signalMapperAddBinding->setMapping(m_oplink->BindingAdd_6, (QWidget *)(m_oplink->BindingID_6)); - connect(m_oplink->BindingRemove_6, SIGNAL(clicked()), signalMapperRemBinding, SLOT(map())); - signalMapperRemBinding->setMapping(m_oplink->BindingRemove_6, (QWidget *)(m_oplink->BindingID_6)); - addUAVObjectToWidgetRelation("OPLinkSettings", "Bindings", m_oplink->BindingID_6, "5"); - addUAVObjectToWidgetRelation("OPLinkSettings", "RemoteMainPort", m_oplink->RemoteMainPort_6, "5"); - addUAVObjectToWidgetRelation("OPLinkSettings", "RemoteFlexiPort", m_oplink->RemoteFlexiPort_6, "5"); - addUAVObjectToWidgetRelation("OPLinkSettings", "RemoteVCPPort", m_oplink->RemoteVCPPort_6, "5"); - addUAVObjectToWidgetRelation("OPLinkSettings", "ComSpeed", m_oplink->ComSpeed_6, "5"); - connect(m_oplink->BindingAdd_7, SIGNAL(clicked()), signalMapperAddBinding, SLOT(map())); - signalMapperAddBinding->setMapping(m_oplink->BindingAdd_7, (QWidget *)(m_oplink->BindingID_7)); - connect(m_oplink->BindingRemove_7, SIGNAL(clicked()), signalMapperRemBinding, SLOT(map())); - signalMapperRemBinding->setMapping(m_oplink->BindingRemove_7, (QWidget *)(m_oplink->BindingID_7)); - addUAVObjectToWidgetRelation("OPLinkSettings", "Bindings", m_oplink->BindingID_7, "6"); - addUAVObjectToWidgetRelation("OPLinkSettings", "RemoteMainPort", m_oplink->RemoteMainPort_7, "6"); - addUAVObjectToWidgetRelation("OPLinkSettings", "RemoteFlexiPort", m_oplink->RemoteFlexiPort_7, "6"); - addUAVObjectToWidgetRelation("OPLinkSettings", "RemoteVCPPort", m_oplink->RemoteVCPPort_7, "6"); - addUAVObjectToWidgetRelation("OPLinkSettings", "ComSpeed", m_oplink->ComSpeed_7, "6"); - connect(m_oplink->BindingAdd_8, SIGNAL(clicked()), signalMapperAddBinding, SLOT(map())); - signalMapperAddBinding->setMapping(m_oplink->BindingAdd_8, (QWidget *)(m_oplink->BindingID_8)); - connect(m_oplink->BindingRemove_8, SIGNAL(clicked()), signalMapperRemBinding, SLOT(map())); - signalMapperRemBinding->setMapping(m_oplink->BindingRemove_8, (QWidget *)(m_oplink->BindingID_8)); - addUAVObjectToWidgetRelation("OPLinkSettings", "Bindings", m_oplink->BindingID_8, "7"); - addUAVObjectToWidgetRelation("OPLinkSettings", "RemoteMainPort", m_oplink->RemoteMainPort_8, "7"); - addUAVObjectToWidgetRelation("OPLinkSettings", "RemoteFlexiPort", m_oplink->RemoteFlexiPort_8, "7"); - addUAVObjectToWidgetRelation("OPLinkSettings", "RemoteVCPPort", m_oplink->RemoteVCPPort_8, "7"); - addUAVObjectToWidgetRelation("OPLinkSettings", "ComSpeed", m_oplink->ComSpeed_8, "7"); - connect(signalMapperAddBinding, SIGNAL(mapped(QWidget *)), this, SLOT(addBinding(QWidget *))); - connect(signalMapperRemBinding, SIGNAL(mapped(QWidget *)), this, SLOT(removeBinding(QWidget *))); + // Connect the bind buttons + connect(m_oplink->Bind1, SIGNAL(clicked()), this, SLOT(bind1())); + connect(m_oplink->Bind2, SIGNAL(clicked()), this, SLOT(bind2())); + connect(m_oplink->Bind3, SIGNAL(clicked()), this, SLOT(bind3())); + connect(m_oplink->Bind4, SIGNAL(clicked()), this, SLOT(bind3())); // Add scroll bar when necessary QScrollArea *scroll = new QScrollArea; @@ -179,10 +113,7 @@ ConfigPipXtremeWidget::ConfigPipXtremeWidget(QWidget *parent) : ConfigTaskWidget } ConfigPipXtremeWidget::~ConfigPipXtremeWidget() -{ - delete signalMapperAddBinding; - delete signalMapperRemBinding; -} +{} /*! \brief Called by updates to @OPLinkStatus @@ -200,19 +131,19 @@ void ConfigPipXtremeWidget::updateStatus(UAVObject *object) quint32 pairid1 = pairIdField->getValue(0).toUInt(); m_oplink->PairID1->setText(QString::number(pairid1, 16).toUpper()); m_oplink->PairID1->setEnabled(false); - m_oplink->PairSelect1->setEnabled(pairid1); + m_oplink->Bind1->setEnabled(pairid1); quint32 pairid2 = pairIdField->getValue(1).toUInt(); m_oplink->PairID2->setText(QString::number(pairIdField->getValue(1).toUInt(), 16).toUpper()); m_oplink->PairID2->setEnabled(false); - m_oplink->PairSelect2->setEnabled(pairid2); + m_oplink->Bind2->setEnabled(pairid2); quint32 pairid3 = pairIdField->getValue(2).toUInt(); m_oplink->PairID3->setText(QString::number(pairIdField->getValue(2).toUInt(), 16).toUpper()); m_oplink->PairID3->setEnabled(false); - m_oplink->PairSelect3->setEnabled(pairid3); + m_oplink->Bind3->setEnabled(pairid3); quint32 pairid4 = pairIdField->getValue(3).toUInt(); m_oplink->PairID4->setText(QString::number(pairIdField->getValue(3).toUInt(), 16).toUpper()); m_oplink->PairID4->setEnabled(false); - m_oplink->PairSelect4->setEnabled(pairid4); + m_oplink->Bind4->setEnabled(pairid4); } else { qDebug() << "PipXtremeGadgetWidget: Count not read PairID field."; } @@ -307,32 +238,36 @@ void ConfigPipXtremeWidget::disconnected() } } -void ConfigPipXtremeWidget::addBinding(QWidget *w) +void ConfigPipXtremeWidget::SetPairID(QLineEdit *pairIdWidget) { - if (QLineEdit * le = qobject_cast(w)) { - // Get the pair ID out of the selection widget - quint32 pairID = 0; - bool okay; - if (m_oplink->PairSelect1->isChecked()) { - pairID = m_oplink->PairID1->text().toUInt(&okay, 16); - } else if (m_oplink->PairSelect2->isChecked()) { - pairID = m_oplink->PairID2->text().toUInt(&okay, 16); - } else if (m_oplink->PairSelect3->isChecked()) { - pairID = m_oplink->PairID3->text().toUInt(&okay, 16); - } else if (m_oplink->PairSelect4->isChecked()) { - pairID = m_oplink->PairID4->text().toUInt(&okay, 16); - } + // Get the pair ID out of the selection widget + quint32 pairID = 0; + bool okay; - // Store the ID in the first open slot (or the last slot if all are full). - le->setText(QString::number(pairID, 16).toUpper()); - } + pairID = pairIdWidget->text().toUInt(&okay, 16); + + // Store the ID in the coord ID field. + m_oplink->CoordID->setText(QString::number(pairID, 16).toUpper()); } -void ConfigPipXtremeWidget::removeBinding(QWidget *w) +void ConfigPipXtremeWidget::bind1() { - if (QLineEdit * le = qobject_cast(w)) { - le->setText(QString::number(0, 16).toUpper()); - } + SetPairID(m_oplink->PairID1); +} + +void ConfigPipXtremeWidget::bind2() +{ + SetPairID(m_oplink->PairID1); +} + +void ConfigPipXtremeWidget::bind3() +{ + SetPairID(m_oplink->PairID1); +} + +void ConfigPipXtremeWidget::bind4() +{ + SetPairID(m_oplink->PairID1); } /** diff --git a/ground/openpilotgcs/src/plugins/config/configpipxtremewidget.h b/ground/openpilotgcs/src/plugins/config/configpipxtremewidget.h index 304ebc478..6966691ce 100644 --- a/ground/openpilotgcs/src/plugins/config/configpipxtremewidget.h +++ b/ground/openpilotgcs/src/plugins/config/configpipxtremewidget.h @@ -55,14 +55,14 @@ private: // Are the settings current? bool settingsUpdated; - // Signal mappers to add arguments to signals. - QSignalMapper *signalMapperAddBinding; - QSignalMapper *signalMapperRemBinding; + void SetPairID(QLineEdit *pairIdWidget); private slots: void disconnected(); - void addBinding(QWidget *w); - void removeBinding(QWidget *w); + void bind1(); + void bind2(); + void bind3(); + void bind4(); }; #endif // CONFIGTXPIDWIDGET_H diff --git a/ground/openpilotgcs/src/plugins/config/configrevohwwidget.cpp b/ground/openpilotgcs/src/plugins/config/configrevohwwidget.cpp index 22da2b6b8..5842ba2ce 100644 --- a/ground/openpilotgcs/src/plugins/config/configrevohwwidget.cpp +++ b/ground/openpilotgcs/src/plugins/config/configrevohwwidget.cpp @@ -66,8 +66,7 @@ ConfigRevoHWWidget::ConfigRevoHWWidget(QWidget *parent) : ConfigTaskWidget(paren addUAVObjectToWidgetRelation("HwSettings", "ComUsbBridgeSpeed", m_ui->cbMainComSpeed); addUAVObjectToWidgetRelation("HwSettings", "RadioPort", m_ui->cbModem); - addUAVObjectToWidgetRelation("HwSettings", "MaxRFPower", m_ui->cbTxPower); - addUAVObjectToWidgetRelation("HwSettings", "DefaultFrequency", m_ui->leInitFreq); + addUAVObjectToWidgetRelation("HwSettings", "TelemetrySpeed", m_ui->cbRadioSpeed); connect(m_ui->cchwHelp, SIGNAL(clicked()), this, SLOT(openHelp())); @@ -291,18 +290,12 @@ void ConfigRevoHWWidget::modemPortChanged(int index) if (m_ui->cbFlexi->currentIndex() == HwSettings::RM_FLEXIPORT_TELEMETRY) { m_ui->cbFlexi->setCurrentIndex(HwSettings::RM_FLEXIPORT_DISABLED); } - m_ui->lblTxPower->setVisible(true); - m_ui->cbTxPower->setVisible(true); - m_ui->lblInitFreq->setVisible(true); - m_ui->leInitFreq->setVisible(true); + m_ui->cbRadioSpeed->setVisible(true); if (!m_refreshing) { QMessageBox::warning(this, tr("Warning"), tr("Activating the Radio requires an antenna be attached or modem damage will occur.")); } } else { - m_ui->lblTxPower->setVisible(false); - m_ui->cbTxPower->setVisible(false); - m_ui->lblInitFreq->setVisible(false); - m_ui->leInitFreq->setVisible(false); + m_ui->cbRadioSpeed->setVisible(false); } } diff --git a/ground/openpilotgcs/src/plugins/config/configrevohwwidget.ui b/ground/openpilotgcs/src/plugins/config/configrevohwwidget.ui index a8b3f65dd..43b605d35 100644 --- a/ground/openpilotgcs/src/plugins/config/configrevohwwidget.ui +++ b/ground/openpilotgcs/src/plugins/config/configrevohwwidget.ui @@ -113,8 +113,8 @@ 0 0 - 810 - 665 + 818 + 673 @@ -122,77 +122,22 @@ 12 - - - - - - 0 - 0 - + + + + + Qt::Horizontal - - USB VCP Function + + QSizePolicy::Minimum - - Qt::AlignBottom|Qt::AlignHCenter + + + 120 + 10 + - - - - - - Speed - - - Qt::AlignBottom|Qt::AlignHCenter - - - - - - - false - - - - - - - - 0 - 0 - - - - Speed - - - Qt::AlignBottom|Qt::AlignHCenter - - - - - - - - - - - 0 - 0 - - - - USB HID Function - - - Qt::AlignBottom|Qt::AlignHCenter - - - - - + @@ -243,6 +188,29 @@ + + + + false + + + + + + + + 0 + 0 + + + + Speed + + + Qt::AlignBottom|Qt::AlignHCenter + + + @@ -293,9 +261,51 @@ + + + + Qt::Vertical + + + QSizePolicy::Fixed + + + + 20 + 120 + + + + + + + + + 0 + 0 + + + + USB VCP Function + + + Qt::AlignBottom|Qt::AlignHCenter + + + + + + + Speed + + + Qt::AlignBottom|Qt::AlignHCenter + + + @@ -342,6 +352,28 @@ + + + + + + + + 0 + 0 + + + + USB HID Function + + + Qt::AlignBottom|Qt::AlignHCenter + + + + + + @@ -379,9 +411,9 @@ - + - Max Tx Power (mW) + Speed Qt::AlignBottom|Qt::AlignHCenter @@ -389,24 +421,7 @@ - - - - - - Initial Frequency (Hz) - - - Qt::AlignBottom|Qt::AlignHCenter - - - - - - - - - + @@ -466,38 +481,6 @@ - - - - Qt::Vertical - - - QSizePolicy::Fixed - - - - 20 - 120 - - - - - - - - Qt::Horizontal - - - QSizePolicy::Minimum - - - - 120 - 10 - - - - @@ -594,6 +577,19 @@ + + + + Qt::Horizontal + + + + 120 + 10 + + + + diff --git a/ground/openpilotgcs/src/plugins/config/pipxtreme.ui b/ground/openpilotgcs/src/plugins/config/pipxtreme.ui index 8df5ea3e9..2a8daaa7f 100644 --- a/ground/openpilotgcs/src/plugins/config/pipxtreme.ui +++ b/ground/openpilotgcs/src/plugins/config/pipxtreme.ui @@ -23,10 +23,7 @@ QFrame::Raised - - - - + @@ -113,7 +110,24 @@ - + + + + + + Qt::Vertical + + + + 20 + 40 + + + + + + + QFrame::StyledPanel @@ -136,132 +150,16 @@ true + + Qt::LeftToRight + Status + + Qt::AlignLeading|Qt::AlignLeft|Qt::AlignVCenter + - - - - Firmware Ver. - - - Qt::AlignRight|Qt::AlignTrailing|Qt::AlignVCenter - - - - - - - - 0 - 0 - - - - - 16777215 - 16777215 - - - - - 75 - true - - - - QLineEdit { - border: none; - border-radius: 1px; - padding: 0 4px; - background: rgba(0, 0, 0, 16); - /* background: transparent; */ - /* selection-background-color: darkgray;*/ - } - - - false - - - true - - - - - - - Serial Number - - - Qt::AlignRight|Qt::AlignTrailing|Qt::AlignVCenter - - - - - - - - 0 - 0 - - - - - 0 - 0 - - - - - 16777215 - 16777215 - - - - - 75 - false - true - - - - false - - - The modems serial number - - - QLineEdit { - border: none; - border-radius: 1px; - padding: 0 4px; - background: rgba(0, 0, 0, 16); - /* background: transparent; */ - /* selection-background-color: darkgray;*/ - } - - - true - - - Qt::AlignLeading|Qt::AlignLeft|Qt::AlignVCenter - - - true - - - - - - - Device ID - - - Qt::AlignRight|Qt::AlignTrailing|Qt::AlignVCenter - - - @@ -366,6 +264,163 @@ + + + + Firmware Ver. + + + Qt::AlignRight|Qt::AlignTrailing|Qt::AlignVCenter + + + + + + + + 0 + 0 + + + + + 16777215 + 16777215 + + + + + 75 + true + + + + QLineEdit { + border: none; + border-radius: 1px; + padding: 0 4px; + background: rgba(0, 0, 0, 16); + /* background: transparent; */ + /* selection-background-color: darkgray;*/ + } + + + false + + + true + + + + + + + + 0 + 0 + + + + + 101 + 16777215 + + + + + 75 + true + + + + QLineEdit { + border: none; + border-radius: 1px; + padding: 0 4px; + background: rgba(0, 0, 0, 16); + /* background: transparent; */ + /* selection-background-color: darkgray;*/ + } + + + true + + + + + + + Serial Number + + + Qt::AlignRight|Qt::AlignTrailing|Qt::AlignVCenter + + + + + + + + 0 + 0 + + + + + 0 + 0 + + + + + 16777215 + 16777215 + + + + + 75 + false + true + + + + false + + + The modems serial number + + + QLineEdit { + border: none; + border-radius: 1px; + padding: 0 4px; + background: rgba(0, 0, 0, 16); + /* background: transparent; */ + /* selection-background-color: darkgray;*/ + } + + + true + + + Qt::AlignLeading|Qt::AlignLeft|Qt::AlignVCenter + + + true + + + + + + + Device ID + + + Qt::AlignRight|Qt::AlignTrailing|Qt::AlignVCenter + + + @@ -418,13 +473,13 @@ - - - - - 0 - 0 - + + + + + 0 + 0 + @@ -438,6 +493,9 @@ true + + The number of packets that were unable to be transmitted + QLineEdit { border: none; @@ -448,6 +506,9 @@ /* selection-background-color: darkgray;*/ } + + Qt::AlignLeading|Qt::AlignLeft|Qt::AlignVCenter + true @@ -922,47 +983,6 @@ - - - - - 0 - 0 - - - - - 101 - 16777215 - - - - - 75 - true - - - - The number of packets that were unable to be transmitted - - - QLineEdit { - border: none; - border-radius: 1px; - padding: 0 4px; - background: rgba(0, 0, 0, 16); - /* background: transparent; */ - /* selection-background-color: darkgray;*/ - } - - - Qt::AlignLeading|Qt::AlignLeft|Qt::AlignVCenter - - - true - - - @@ -1239,1186 +1259,6 @@ - - - - - 75 - true - - - - Remote Modems - - - - - - - - - - - - - - - - -100dB - - - - - - - -127 - - - 0 - - - 0 - - - false - - - %v dBm - - - - - - - -100dB - - - - - - - - 100 - 16777215 - - - - - - - - -127 - - - 0 - - - 0 - - - false - - - %v dBm - - - - - - - -100dB - - - - - - - -127 - - - 0 - - - -127 - - - false - - - %v dBm - - - - - - - - 100 - 16777215 - - - - - - - - -100dB - - - - - - - - 100 - 16777215 - - - - 12345678 - - - - - - - - - - - - - - -127 - - - 0 - - - 0 - - - false - - - %v dBm - - - - - - - - - - - - - - - 100 - 16777215 - - - - - - - - - - - - - - - - - - - - - - 75 - true - - - - Bindings - - - - - - Add - - - Qt::AlignCenter - - - - - - - Clear - - - Qt::AlignCenter - - - - - - - Device ID - - - Qt::AlignCenter - - - - - - - Main Port - - - Qt::AlignCenter - - - - - - - Flexi Port - - - Qt::AlignCenter - - - - - - - VCP Port - - - Qt::AlignCenter - - - - - - - COM Speed - - - Qt::AlignCenter - - - - - - - true - - - - - 0 - 0 - 757 - 244 - - - - - - - Add >> - - - - - - - Clear - - - - - - - - 0 - 0 - - - - - 101 - 16777215 - - - - - 75 - true - - - - QLineEdit { - border: none; - border-radius: 1px; - padding: 0 4px; - background: rgba(0, 0, 0, 16); - /* background: transparent; */ - /* selection-background-color: darkgray;*/ - } - - - false - - - Qt::AlignLeading|Qt::AlignLeft|Qt::AlignVCenter - - - true - - - 12345678 - - - - - - - - 16777215 - 16777215 - - - - Choose the function for the remote main port - - - - - - - - 16777215 - 16777215 - - - - Choose the function for the remote flexi port - - - - - - - - 16777215 - 16777215 - - - - Choose the function for the remote USB virtual com port - - - - - - - - 16777215 - 16777215 - - - - Set the telemetry port speed - - - - - - - Add >> - - - - - - - Clear - - - - - - - - 0 - 0 - - - - - 101 - 16777215 - - - - - 75 - true - - - - QLineEdit { - border: none; - border-radius: 1px; - padding: 0 4px; - background: rgba(0, 0, 0, 16); - /* background: transparent; */ - /* selection-background-color: darkgray;*/ - } - - - false - - - Qt::AlignLeading|Qt::AlignLeft|Qt::AlignVCenter - - - true - - - 12345678 - - - - - - - - 16777215 - 16777215 - - - - Choose the function for the remote main port - - - - - - - - 16777215 - 16777215 - - - - Choose the function for the remote flexi port - - - - - - - - 16777215 - 16777215 - - - - Choose the function for the remote USB virtual com port - - - - - - - - 16777215 - 16777215 - - - - Set the telemetry port speed - - - - - - - Add >> - - - - - - - Clear - - - - - - - - 0 - 0 - - - - - 101 - 16777215 - - - - - 75 - true - - - - QLineEdit { - border: none; - border-radius: 1px; - padding: 0 4px; - background: rgba(0, 0, 0, 16); - /* background: transparent; */ - /* selection-background-color: darkgray;*/ - } - - - false - - - Qt::AlignLeading|Qt::AlignLeft|Qt::AlignVCenter - - - true - - - 12345678 - - - - - - - - 16777215 - 16777215 - - - - Choose the function for the remote main port - - - - - - - - 16777215 - 16777215 - - - - Choose the function for the remote flexi port - - - - - - - - 16777215 - 16777215 - - - - Choose the function for the remote USB virtual com port - - - - - - - - 16777215 - 16777215 - - - - Set the telemetry port speed - - - - - - - Add >> - - - - - - - Clear - - - - - - - - 0 - 0 - - - - - 101 - 16777215 - - - - - 75 - true - - - - QLineEdit { - border: none; - border-radius: 1px; - padding: 0 4px; - background: rgba(0, 0, 0, 16); - /* background: transparent; */ - /* selection-background-color: darkgray;*/ - } - - - false - - - Qt::AlignLeading|Qt::AlignLeft|Qt::AlignVCenter - - - true - - - 12345678 - - - - - - - - 16777215 - 16777215 - - - - Choose the function for the remote main port - - - - - - - - 16777215 - 16777215 - - - - Choose the function for the remote flexi port - - - - - - - - 16777215 - 16777215 - - - - Choose the function for the remote USB virtual com port - - - - - - - - 16777215 - 16777215 - - - - Set the telemetry port speed - - - - - - - Add >> - - - - - - - Clear - - - - - - - - 0 - 0 - - - - - 101 - 16777215 - - - - - 75 - true - - - - QLineEdit { - border: none; - border-radius: 1px; - padding: 0 4px; - background: rgba(0, 0, 0, 16); - /* background: transparent; */ - /* selection-background-color: darkgray;*/ - } - - - false - - - Qt::AlignLeading|Qt::AlignLeft|Qt::AlignVCenter - - - true - - - 12345678 - - - - - - - - 16777215 - 16777215 - - - - Choose the function for the remote main port - - - - - - - - 16777215 - 16777215 - - - - Choose the function for the remote flexi port - - - - - - - - 16777215 - 16777215 - - - - Choose the function for the remote USB virtual com port - - - - - - - - 16777215 - 16777215 - - - - Set the telemetry port speed - - - - - - - Add >> - - - - - - - Clear - - - - - - - - 0 - 0 - - - - - 101 - 16777215 - - - - - 75 - true - - - - QLineEdit { - border: none; - border-radius: 1px; - padding: 0 4px; - background: rgba(0, 0, 0, 16); - /* background: transparent; */ - /* selection-background-color: darkgray;*/ - } - - - false - - - Qt::AlignLeading|Qt::AlignLeft|Qt::AlignVCenter - - - true - - - 12345678 - - - - - - - - 16777215 - 16777215 - - - - Choose the function for the remote main port - - - - - - - - 16777215 - 16777215 - - - - Choose the function for the remote flexi port - - - - - - - - 16777215 - 16777215 - - - - Choose the function for the remote USB virtual com port - - - - - - - - 16777215 - 16777215 - - - - Set the telemetry port speed - - - - - - - Add >> - - - - - - - Clear - - - - - - - - 0 - 0 - - - - - 101 - 16777215 - - - - - 75 - true - - - - QLineEdit { - border: none; - border-radius: 1px; - padding: 0 4px; - background: rgba(0, 0, 0, 16); - /* background: transparent; */ - /* selection-background-color: darkgray;*/ - } - - - false - - - Qt::AlignLeading|Qt::AlignLeft|Qt::AlignVCenter - - - true - - - 12345678 - - - - - - - - 16777215 - 16777215 - - - - Choose the function for the remote main port - - - - - - - - 16777215 - 16777215 - - - - Choose the function for the remote flexi port - - - - - - - - 16777215 - 16777215 - - - - Choose the function for the remote USB virtual com port - - - - - - - - 16777215 - 16777215 - - - - Set the telemetry port speed - - - - - - - Add >> - - - - - - - Clear - - - - - - - - 0 - 0 - - - - - 101 - 16777215 - - - - - 75 - true - - - - QLineEdit { - border: none; - border-radius: 1px; - padding: 0 4px; - background: rgba(0, 0, 0, 16); - /* background: transparent; */ - /* selection-background-color: darkgray;*/ - } - - - false - - - Qt::AlignLeading|Qt::AlignLeft|Qt::AlignVCenter - - - true - - - 12345678 - - - - - - - - 16777215 - 16777215 - - - - Choose the function for the remote main port - - - - - - - - 16777215 - 16777215 - - - - Choose the function for the remote flexi port - - - - - - - - 16777215 - 16777215 - - - - Choose the function for the remote USB virtual com port - - - - - - - - 16777215 - 16777215 - - - - Set the telemetry port speed - - - - - - - - - - @@ -2437,7 +1277,37 @@ Configuration - + + + + Max Chan + + + Qt::AlignRight|Qt::AlignTrailing|Qt::AlignVCenter + + + + + + + Packet Length + + + Qt::AlignRight|Qt::AlignTrailing|Qt::AlignVCenter + + + + + + + Num Chan + + + Qt::AlignRight|Qt::AlignTrailing|Qt::AlignVCenter + + + + Main Port @@ -2447,87 +1317,7 @@ - - - - - 16777215 - 16777215 - - - - Choose the function for the main port - - - - - - - Min Freq - - - Qt::AlignRight|Qt::AlignTrailing|Qt::AlignVCenter - - - - - - - - - - - - - - Flexi Port - - - Qt::AlignRight|Qt::AlignTrailing|Qt::AlignVCenter - - - - - - - - 16777215 - 16777215 - - - - Choose the function for the flexi port - - - - - - - Max Freq - - - Qt::AlignRight|Qt::AlignTrailing|Qt::AlignVCenter - - - - - - - - - - - - - - VCP Port - - - Qt::AlignRight|Qt::AlignTrailing|Qt::AlignVCenter - - - - + @@ -2540,24 +1330,114 @@ - - + + + + 250 + + + + + - Initial Freq + Flexi Port Qt::AlignRight|Qt::AlignTrailing|Qt::AlignVCenter - - - - + + + + + + + VCP Port + + + Qt::AlignRight|Qt::AlignTrailing|Qt::AlignVCenter - + + + + + 16777215 + 16777215 + + + + Choose the function for the main port + + + + + + + Com Speed + + + Qt::AlignRight|Qt::AlignTrailing|Qt::AlignVCenter + + + + + + + Min Chan + + + Qt::AlignRight|Qt::AlignTrailing|Qt::AlignVCenter + + + + + + + 249 + + + + + + + Channel Set + + + Qt::AlignRight|Qt::AlignTrailing|Qt::AlignVCenter + + + + + + + 249 + + + + + + + + 16777215 + 16777215 + + + + Choose the function for the flexi port + + + + + + + 249 + + + + Max Power @@ -2567,7 +1447,7 @@ - + @@ -2586,38 +1466,260 @@ - - - - Step Size + + + + - - Qt::AlignRight|Qt::AlignTrailing|Qt::AlignVCenter + + 10 + + + 50 + + + 15 - - - - + + + + Coordinator + + + + + + + One-Way Link - - - - Qt::Vertical + + + + + 75 + true + - - - 20 - 40 - + + Remote Modems - + + + + + + + + -100dB + + + + + + + -127 + + + 0 + + + 0 + + + false + + + %v dBm + + + + + + + -100dB + + + + + + + + 100 + 16777215 + + + + + + + + -127 + + + 0 + + + 0 + + + false + + + %v dBm + + + + + + + + 100 + 16777215 + + + + + + + + -127 + + + 0 + + + -127 + + + false + + + %v dBm + + + + + + + -100dB + + + + + + + -127 + + + 0 + + + 0 + + + false + + + %v dBm + + + + + + + -100dB + + + + + + + + 100 + 16777215 + + + + + + + + + 100 + 16777215 + + + + 12345678 + + + + + + + Bind + + + + + + + Bind + + + + + + + Bind + + + + + + + Bind + + + + + + + Qt::LeftToRight + + + Coord ID + + + Qt::AlignRight|Qt::AlignTrailing|Qt::AlignVCenter + + + + + + + + 100 + 16777215 + + + + 8 + + + + + + + + @@ -2625,17 +1727,12 @@ - PairSelect1 PairID1 - PairSelect2 PairID2 - PairSelect3 PairID3 - PairSelect4 PairID4 FirmwareVersion SerialNumber - MaxRFTxPower Apply Save diff --git a/ground/openpilotgcs/src/plugins/uavobjects/uavobjects.pro b/ground/openpilotgcs/src/plugins/uavobjects/uavobjects.pro index 43a772af5..c71d6ef34 100644 --- a/ground/openpilotgcs/src/plugins/uavobjects/uavobjects.pro +++ b/ground/openpilotgcs/src/plugins/uavobjects/uavobjects.pro @@ -101,6 +101,7 @@ HEADERS += $$UAVOBJECT_SYNTHETICS/accessorydesired.h \ $$UAVOBJECT_SYNTHETICS/poilocation.h \ $$UAVOBJECT_SYNTHETICS/oplinksettings.h \ $$UAVOBJECT_SYNTHETICS/oplinkstatus.h \ + $$UAVOBJECT_SYNTHETICS/oplinkreceiver.h \ $$UAVOBJECT_SYNTHETICS/osdsettings.h \ $$UAVOBJECT_SYNTHETICS/waypoint.h \ $$UAVOBJECT_SYNTHETICS/waypointactive.h \ @@ -185,6 +186,7 @@ SOURCES += $$UAVOBJECT_SYNTHETICS/accessorydesired.cpp \ $$UAVOBJECT_SYNTHETICS/poilocation.cpp \ $$UAVOBJECT_SYNTHETICS/oplinksettings.cpp \ $$UAVOBJECT_SYNTHETICS/oplinkstatus.cpp \ + $$UAVOBJECT_SYNTHETICS/oplinkreceiver.cpp \ $$UAVOBJECT_SYNTHETICS/osdsettings.cpp \ $$UAVOBJECT_SYNTHETICS/waypoint.cpp \ $$UAVOBJECT_SYNTHETICS/waypointactive.cpp \ diff --git a/shared/uavobjectdefinition/hwsettings.xml b/shared/uavobjectdefinition/hwsettings.xml index 5d0a29fec..61c24d4f1 100644 --- a/shared/uavobjectdefinition/hwsettings.xml +++ b/shared/uavobjectdefinition/hwsettings.xml @@ -17,8 +17,6 @@ - - diff --git a/shared/uavobjectdefinition/oplinksettings.xml b/shared/uavobjectdefinition/oplinksettings.xml index b78b8d4c9..e0969e9dd 100644 --- a/shared/uavobjectdefinition/oplinksettings.xml +++ b/shared/uavobjectdefinition/oplinksettings.xml @@ -1,19 +1,19 @@ OPLink configurations options. - - - - - + + + + - - - - + + + + + diff --git a/shared/uavobjectdefinition/oplinkstatus.xml b/shared/uavobjectdefinition/oplinkstatus.xml index 1e79630fe..81172bcf1 100644 --- a/shared/uavobjectdefinition/oplinkstatus.xml +++ b/shared/uavobjectdefinition/oplinkstatus.xml @@ -30,7 +30,7 @@ - +