diff --git a/.gitignore b/.gitignore index 17b41c76c..a5af05849 100644 --- a/.gitignore +++ b/.gitignore @@ -20,6 +20,9 @@ /flight/OpenPilot/Build /flight/OpenPilot/Build.win32 +#flight/Project/OpenPilotOSX +flight/Project/OpenPilotOSX/build + # /flight/PipBee/ /flight/PipBee/Build diff --git a/flight/Libraries/inc/packet_handler.h b/flight/Libraries/inc/packet_handler.h index b0bd8315c..1e9f60807 100644 --- a/flight/Libraries/inc/packet_handler.h +++ b/flight/Libraries/inc/packet_handler.h @@ -106,7 +106,7 @@ void PHRegisterStatusHandler(PHInstHandle h, PHStatusHandler f); void PHRegisterPPMHandler(PHInstHandle h, PHPPMHandler f); uint32_t PHConnect(PHInstHandle h, uint32_t dest_id); PHPacketHandle PHGetRXPacket(PHInstHandle h); -void PHReleaseTXPacket(PHInstHandle h, PHPacketHandle p); +void PHReleaseRXPacket(PHInstHandle h, PHPacketHandle p); PHPacketHandle PHGetTXPacket(PHInstHandle h); void PHReleaseTXPacket(PHInstHandle h, PHPacketHandle p); uint8_t PHTransmitPacket(PHInstHandle h, PHPacketHandle p); diff --git a/flight/Libraries/packet_handler.c b/flight/Libraries/packet_handler.c index 59a9a187f..f4e8e28d7 100644 --- a/flight/Libraries/packet_handler.c +++ b/flight/Libraries/packet_handler.c @@ -169,16 +169,15 @@ PHPacketHandle PHGetTXPacket(PHInstHandle h) // Lock xSemaphoreTakeRecursive(data->lock, portMAX_DELAY); - PHPacketHandle p = data->tx_packets + data->tx_win_end; - // Is the window full? - uint8_t next_end = (data->tx_win_end + 1) % data->cfg.winSize; - if(next_end == data->tx_win_start) - { - xSemaphoreGiveRecursive(data->lock); - return NULL; - } - data->tx_win_end = next_end; + // Find a free packet. + PHPacketHandle p = NULL; + for (uint8_t i = 0; i < data->cfg.winSize; ++i) + if (data->tx_packets[i].header.type == PACKET_TYPE_NONE) + { + p = data->tx_packets + i; + break; + } // Release lock xSemaphoreGiveRecursive(data->lock); @@ -224,17 +223,15 @@ PHPacketHandle PHGetRXPacket(PHInstHandle h) // Lock xSemaphoreTakeRecursive(data->lock, portMAX_DELAY); - PHPacketHandle p = data->rx_packets + data->rx_win_end; - // Is the window full? - uint8_t next_end = (data->rx_win_end + 1) % data->cfg.winSize; - if(next_end == data->rx_win_start) - { - // Release lock - xSemaphoreGiveRecursive(data->lock); - return NULL; - } - data->rx_win_end = next_end; + // Find a free packet. + PHPacketHandle p = NULL; + for (uint8_t i = 0; i < data->cfg.winSize; ++i) + if (data->rx_packets[i].header.type == PACKET_TYPE_NONE) + { + p = data->rx_packets + i; + break; + } // Release lock xSemaphoreGiveRecursive(data->lock); diff --git a/flight/Modules/RadioComBridge/RadioComBridge.c b/flight/Modules/RadioComBridge/RadioComBridge.c index a321b4bfa..eb674cbdd 100644 --- a/flight/Modules/RadioComBridge/RadioComBridge.c +++ b/flight/Modules/RadioComBridge/RadioComBridge.c @@ -61,8 +61,9 @@ #define PACKET_QUEUE_SIZE 10 #define MAX_PORT_DELAY 200 #define EV_PACKET_RECEIVED 0x20 +#define EV_TRANSMIT_PACKET 0x30 #define EV_SEND_ACK 0x40 -#define EV_SEND_NACK 0x80 +#define EV_SEND_NACK 0x50 // **************** // Private types @@ -79,22 +80,36 @@ typedef struct { } PairStats; typedef struct { + uint32_t comPort; + UAVTalkConnection UAVTalkCon; + xQueueHandle sendQueue; + xQueueHandle recvQueue; + xQueueHandle gcsQueue; + uint16_t wdg; + bool checkHID; +} UAVTalkComTaskParams; + +typedef struct { + // The task handles. - xTaskHandle comUAVTalkTaskHandle; + xTaskHandle GCSUAVTalkRecvTaskHandle; + xTaskHandle UAVTalkRecvTaskHandle; xTaskHandle radioReceiveTaskHandle; xTaskHandle sendPacketTaskHandle; - xTaskHandle sendDataTaskHandle; + xTaskHandle UAVTalkSendTaskHandle; xTaskHandle radioStatusTaskHandle; xTaskHandle transparentCommTaskHandle; xTaskHandle ppmInputTaskHandle; // The UAVTalk connection on the com side. - UAVTalkConnection inUAVTalkCon; - UAVTalkConnection outUAVTalkCon; + UAVTalkConnection UAVTalkCon; + UAVTalkConnection GCSUAVTalkCon; // Queue handles. - xQueueHandle sendPacketQueue; - xQueueHandle objEventQueue; + xQueueHandle radioPacketQueue; + xQueueHandle gcsEventQueue; + xQueueHandle uavtalkEventQueue; + xQueueHandle ppmOutQueue; // Error statistics. uint32_t comTxErrors; @@ -122,6 +137,10 @@ typedef struct { // The RSSI of the last packet received. int8_t RSSI; + // Thread parameters. + UAVTalkComTaskParams uavtalk_params; + UAVTalkComTaskParams gcs_uavtalk_params; + } RadioComBridgeData; typedef struct { @@ -135,21 +154,24 @@ typedef struct { // **************** // Private functions -static void comUAVTalkTask(void *parameters); +static void UAVTalkRecvTask(void *parameters); static void radioReceiveTask(void *parameters); static void sendPacketTask(void *parameters); -static void sendDataTask(void *parameters); +static void UAVTalkSendTask(void *parameters); static void transparentCommTask(void * parameters); static void radioStatusTask(void *parameters); static void ppmInputTask(void *parameters); -static int32_t transmitData(uint8_t * data, int32_t length); +static int32_t UAVTalkSendHandler(uint8_t * data, int32_t length); +static int32_t GCSUAVTalkSendHandler(uint8_t * data, int32_t length); static int32_t transmitPacket(PHPacketHandle packet); static void receiveData(uint8_t *buf, uint8_t len, int8_t rssi, int8_t afc); +static void transmitData(uint32_t outputPort, uint8_t *buf, uint8_t len, bool checkHid); static void StatusHandler(PHStatusPacketHandle p, int8_t rssi, int8_t afc); static void PPMHandler(uint16_t *channels); static BufferedReadHandle BufferedReadInit(uint32_t com_port, uint16_t buffer_length); static bool BufferedRead(BufferedReadHandle h, uint8_t *value, uint32_t timeout_ms); static void BufferedReadSetCom(BufferedReadHandle h, uint32_t com_port); +static void queueEvent(xQueueHandle queue, void *obj, uint16_t instId, UAVObjEventType type); static void updateSettings(); // **************** @@ -165,23 +187,34 @@ static RadioComBridgeData *data; static int32_t RadioComBridgeStart(void) { if(data) { + // Start the primary tasks for receiving/sending UAVTalk packets from the GCS. + xTaskCreate(UAVTalkRecvTask, (signed char *)"GCSUAVTalkRecvTask", STACK_SIZE_BYTES, (void*)&(data->gcs_uavtalk_params), TASK_PRIORITY + 2, &(data->GCSUAVTalkRecvTaskHandle)); + xTaskCreate(UAVTalkSendTask, (signed char *)"GCSUAVTalkSendTask", STACK_SIZE_BYTES, (void*)&(data->gcs_uavtalk_params), TASK_PRIORITY+ 2, &(data->UAVTalkSendTaskHandle)); + + // If a UAVTalk (non-GCS) com port is set it implies that the com port is connected on the flight side. + // In this case we want to start another com thread on the HID port to talk to the GCS when connected. + if (PIOS_COM_UAVTALK) + { + xTaskCreate(UAVTalkRecvTask, (signed char *)"UAVTalkRecvTask", STACK_SIZE_BYTES, (void*)&(data->uavtalk_params), TASK_PRIORITY + 2, &(data->UAVTalkRecvTaskHandle)); + xTaskCreate(UAVTalkSendTask, (signed char *)"UAVTalkSendTask", STACK_SIZE_BYTES, (void*)&(data->uavtalk_params), TASK_PRIORITY+ 2, &(data->UAVTalkSendTaskHandle)); + } + // Start the tasks - xTaskCreate(comUAVTalkTask, (signed char *)"ComUAVTalk", STACK_SIZE_BYTES, NULL, TASK_PRIORITY + 2, &(data->comUAVTalkTaskHandle)); if(PIOS_COM_TRANS_COM) xTaskCreate(transparentCommTask, (signed char *)"transparentComm", STACK_SIZE_BYTES, NULL, TASK_PRIORITY + 2, &(data->transparentCommTaskHandle)); xTaskCreate(radioReceiveTask, (signed char *)"RadioReceive", STACK_SIZE_BYTES, NULL, TASK_PRIORITY+ 2, &(data->radioReceiveTaskHandle)); xTaskCreate(sendPacketTask, (signed char *)"SendPacketTask", STACK_SIZE_BYTES, NULL, TASK_PRIORITY + 2, &(data->sendPacketTaskHandle)); - xTaskCreate(sendDataTask, (signed char *)"SendDataTask", STACK_SIZE_BYTES, NULL, TASK_PRIORITY+ 2, &(data->sendDataTaskHandle)); xTaskCreate(radioStatusTask, (signed char *)"RadioStatus", STACK_SIZE_BYTES, NULL, TASK_PRIORITY, &(data->radioStatusTaskHandle)); if(PIOS_PPM_RECEIVER) xTaskCreate(ppmInputTask, (signed char *)"PPMInputTask", STACK_SIZE_BYTES, NULL, TASK_PRIORITY + 2, &(data->ppmInputTaskHandle)); #ifdef PIOS_INCLUDE_WDG - PIOS_WDG_RegisterFlag(PIOS_WDG_COMUAVTALK); + PIOS_WDG_RegisterFlag(PIOS_WDG_COMGCS); + if(PIOS_COM_UAVTALK) + PIOS_WDG_RegisterFlag(PIOS_WDG_COMUAVTALK); if(PIOS_COM_TRANS_COM) PIOS_WDG_RegisterFlag(PIOS_WDG_TRANSCOMM); PIOS_WDG_RegisterFlag(PIOS_WDG_RADIORECEIVE); //PIOS_WDG_RegisterFlag(PIOS_WDG_SENDPACKET); - //PIOS_WDG_RegisterFlag(PIOS_WDG_SENDDATA); if(PIOS_PPM_RECEIVER) PIOS_WDG_RegisterFlag(PIOS_WDG_PPMINPUT); #endif @@ -211,12 +244,21 @@ static int32_t RadioComBridgeInitialize(void) updateSettings(); // Initialise UAVTalk - data->inUAVTalkCon = UAVTalkInitialize(0); - data->outUAVTalkCon = UAVTalkInitialize(&transmitData); + data->GCSUAVTalkCon = UAVTalkInitialize(&GCSUAVTalkSendHandler); + if (PIOS_COM_UAVTALK) + data->UAVTalkCon = UAVTalkInitialize(&UAVTalkSendHandler); // Initialize the queues. - data->sendPacketQueue = xQueueCreate(PACKET_QUEUE_SIZE, sizeof(PHPacketHandle)); - data->objEventQueue = xQueueCreate(PACKET_QUEUE_SIZE, sizeof(UAVObjEvent)); + data->ppmOutQueue = 0; + data->radioPacketQueue = xQueueCreate(PACKET_QUEUE_SIZE, sizeof(UAVObjEvent)); + data->gcsEventQueue = xQueueCreate(PACKET_QUEUE_SIZE, sizeof(UAVObjEvent)); + if (PIOS_COM_UAVTALK) + data->uavtalkEventQueue = xQueueCreate(PACKET_QUEUE_SIZE, sizeof(UAVObjEvent)); + else + { + data->uavtalkEventQueue = 0; + data->ppmOutQueue = data->radioPacketQueue; + } // Initialize the statistics. data->radioTxErrors = 0; @@ -232,8 +274,8 @@ static int32_t RadioComBridgeInitialize(void) // Register the callbacks with the packet handler PHRegisterOutputStream(pios_packet_handler, transmitPacket); PHRegisterDataHandler(pios_packet_handler, receiveData); - PHRegisterStatusHandler(pios_packet_handler, StatusHandler); PHRegisterPPMHandler(pios_packet_handler, PPMHandler); + PHRegisterStatusHandler(pios_packet_handler, StatusHandler); // Initialize the packet send timeout data->send_timeout = 25; // ms @@ -255,9 +297,28 @@ static int32_t RadioComBridgeInitialize(void) PipXSettingsPairIDGet(&(data->pairStats[0].pairID)); // Configure our UAVObjects for updates. - UAVObjConnectQueue(UAVObjGetByID(PIPXSTATUS_OBJID), data->objEventQueue, EV_UPDATED | EV_UPDATED_MANUAL | EV_UPDATE_REQ); - UAVObjConnectQueue(UAVObjGetByID(GCSRECEIVER_OBJID), data->objEventQueue, EV_UPDATED | EV_UPDATED_MANUAL | EV_UPDATE_REQ); - UAVObjConnectQueue(UAVObjGetByID(OBJECTPERSISTENCE_OBJID), data->objEventQueue, EV_UPDATED | EV_UPDATED_MANUAL); + UAVObjConnectQueue(UAVObjGetByID(PIPXSTATUS_OBJID), data->gcsEventQueue, EV_UPDATED | EV_UPDATED_MANUAL | EV_UPDATE_REQ); + UAVObjConnectQueue(UAVObjGetByID(GCSRECEIVER_OBJID), data->uavtalkEventQueue ? data->uavtalkEventQueue : data->gcsEventQueue, EV_UPDATED | EV_UPDATED_MANUAL | EV_UPDATE_REQ); + UAVObjConnectQueue(UAVObjGetByID(OBJECTPERSISTENCE_OBJID), data->gcsEventQueue, EV_UPDATED | EV_UPDATED_MANUAL); + + // Initialize the UAVTalk comm parameters. + data->gcs_uavtalk_params.UAVTalkCon = data->GCSUAVTalkCon; + data->gcs_uavtalk_params.sendQueue = data->radioPacketQueue; + data->gcs_uavtalk_params.recvQueue = data->gcsEventQueue; + data->gcs_uavtalk_params.wdg = PIOS_WDG_COMGCS; + data->gcs_uavtalk_params.checkHID = true; + data->gcs_uavtalk_params.comPort = PIOS_COM_GCS; + if (PIOS_COM_UAVTALK) + { + data->gcs_uavtalk_params.sendQueue = data->uavtalkEventQueue; + data->uavtalk_params.UAVTalkCon = data->UAVTalkCon; + data->uavtalk_params.sendQueue = data->radioPacketQueue; + data->uavtalk_params.recvQueue = data->uavtalkEventQueue; + data->uavtalk_params.gcsQueue = data->gcsEventQueue; + data->uavtalk_params.wdg = PIOS_WDG_COMUAVTALK; + data->uavtalk_params.checkHID = false; + data->uavtalk_params.comPort = PIOS_COM_UAVTALK; + } return 0; } @@ -266,30 +327,32 @@ MODULE_INITCALL(RadioComBridgeInitialize, RadioComBridgeStart) /** * Reads UAVTalk messages froma com port and creates packets out of them. */ -static void comUAVTalkTask(void *parameters) +static void UAVTalkRecvTask(void *parameters) { + UAVTalkComTaskParams *params = (UAVTalkComTaskParams *)parameters; PHPacketHandle p = NULL; // Create the buffered reader. - BufferedReadHandle f = BufferedReadInit(PIOS_COM_UAVTALK, TEMP_BUFFER_SIZE); + BufferedReadHandle f = BufferedReadInit(params->comPort, TEMP_BUFFER_SIZE); while (1) { #ifdef PIOS_INCLUDE_WDG // Update the watchdog timer. - PIOS_WDG_UpdateFlag(PIOS_WDG_COMUAVTALK); + if (params->wdg) + PIOS_WDG_UpdateFlag(params->wdg); #endif /* PIOS_INCLUDE_WDG */ // Receive from USB HID if available, otherwise UAVTalk com if it's available. #if defined(PIOS_INCLUDE_USB) // Determine input port (USB takes priority over telemetry port) - if (PIOS_USB_CheckAvailable(0)) + if (params->checkHID && PIOS_USB_CheckAvailable(0)) BufferedReadSetCom(f, PIOS_COM_USB_HID); else #endif /* PIOS_INCLUDE_USB */ { - if (PIOS_COM_UAVTALK) - BufferedReadSetCom(f, PIOS_COM_UAVTALK); + if (params->comPort) + BufferedReadSetCom(f, params->comPort); else { vTaskDelay(5); @@ -307,7 +370,7 @@ static void comUAVTalkTask(void *parameters) { // Wait until we receive a sync. - UAVTalkRxState state = UAVTalkProcessInputStreamQuiet(data->inUAVTalkCon, rx_byte); + UAVTalkRxState state = UAVTalkProcessInputStreamQuiet(params->UAVTalkCon, rx_byte); if (state != UAVTALK_STATE_TYPE) continue; @@ -324,7 +387,6 @@ static void comUAVTalkTask(void *parameters) // Initialize the packet. p->header.destination_id = data->destination_id; p->header.source_id = PIOS_RFM22B_DeviceID(pios_rfm22b_id); - //p->header.type = PACKET_TYPE_ACKED_DATA; p->header.type = PACKET_TYPE_DATA; p->data[0] = rx_byte; p->header.data_size = 1; @@ -335,12 +397,19 @@ static void comUAVTalkTask(void *parameters) p->data[p->header.data_size++] = rx_byte; // Keep reading until we receive a completed packet. - UAVTalkRxState state = UAVTalkProcessInputStreamQuiet(data->inUAVTalkCon, rx_byte); - UAVTalkConnectionData *connection = (UAVTalkConnectionData*)(data->inUAVTalkCon); + UAVTalkRxState state = UAVTalkProcessInputStreamQuiet(params->UAVTalkCon, rx_byte); + UAVTalkConnectionData *connection = (UAVTalkConnectionData*)(params->UAVTalkCon); UAVTalkInputProcessor *iproc = &(connection->iproc); if (state == UAVTALK_STATE_COMPLETE) { + xQueueHandle sendQueue = params->sendQueue; +#if defined(PIOS_INCLUDE_USB) + if (params->gcsQueue) + if (PIOS_USB_CheckAvailable(0) && PIOS_COM_USB_HID) + sendQueue = params->gcsQueue; +#endif /* PIOS_INCLUDE_USB */ + // Is this a local UAVObject? // We only generate GcsReceiver ojects, we don't consume them. if ((iproc->obj != NULL) && (iproc->objId != GCSRECEIVER_OBJID)) @@ -359,11 +428,7 @@ static void comUAVTalkTask(void *parameters) if (obj_per.ObjectID == PIPXSETTINGS_OBJID) { // Queue up the ACK. - UAVObjEvent ev; - ev.obj = iproc->obj; - ev.instId = iproc->instId; - ev.event = EV_SEND_ACK; - xQueueSend(data->objEventQueue, &ev, MAX_PORT_DELAY); + queueEvent(params->recvQueue, (void*)iproc->obj, iproc->instId, EV_SEND_ACK); // Is this a save, load, or delete? bool success = true; @@ -390,6 +455,19 @@ static void comUAVTalkTask(void *parameters) int32_t ret = PIOS_EEPROM_Save((uint8_t*)&pipxSettings, sizeof(PipXSettingsData)); if (ret != 0) success = false; +#endif + break; + } + case OBJECTPERSISTENCE_OPERATION_DELETE: + { +#if defined(PIOS_INCLUDE_FLASH_EEPROM) + // Erase the settings. + PipXSettingsData pipxSettings; + uint8_t *ptr = (uint8_t*)&pipxSettings; + memset(ptr, 0, sizeof(PipXSettingsData)); + int32_t ret = PIOS_EEPROM_Save(ptr, sizeof(PipXSettingsData)); + if (ret != 0) + success = false; #endif break; } @@ -408,14 +486,11 @@ static void comUAVTalkTask(void *parameters) else { // Otherwise, queue the packet for transmission. - xQueueSend(data->sendPacketQueue, &p, MAX_PORT_DELAY); + queueEvent(sendQueue, (void*)p, 0, EV_TRANSMIT_PACKET); } } else { - UAVObjEvent ev; - ev.obj = iproc->obj; - ev.instId = 0; switch (iproc->type) { case UAVTALK_TYPE_OBJ: @@ -424,16 +499,12 @@ static void comUAVTalkTask(void *parameters) break; case UAVTALK_TYPE_OBJ_REQ: // Queue up an object send request. - ev.event = EV_UPDATE_REQ; - xQueueSend(data->objEventQueue, &ev, MAX_PORT_DELAY); + queueEvent(params->recvQueue, (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 - ev.event = EV_SEND_ACK; - xQueueSend(data->objEventQueue, &ev, MAX_PORT_DELAY); - } + queueEvent(params->recvQueue, (void*)iproc->obj, iproc->instId, EV_SEND_ACK); break; } @@ -444,22 +515,24 @@ static void comUAVTalkTask(void *parameters) else { // Queue the packet for transmission. - xQueueSend(data->sendPacketQueue, &p, MAX_PORT_DELAY); + queueEvent(sendQueue, (void*)p, 0, EV_TRANSMIT_PACKET); } p = NULL; } else if(state == UAVTALK_STATE_ERROR) { - DEBUG_PRINTF(1, "UAVTalk FAILED!\n\r"); + xQueueHandle sendQueue = params->sendQueue; +#if defined(PIOS_INCLUDE_USB) + if (params->gcsQueue) + if (PIOS_USB_CheckAvailable(0) && PIOS_COM_USB_HID) + sendQueue = params->gcsQueue; +#endif /* PIOS_INCLUDE_USB */ data->UAVTalkErrors++; // Send a NACK if required. if((iproc->obj) && (iproc->type == UAVTALK_TYPE_OBJ_ACK)) { // Queue up a NACK - UAVObjEvent ev; - ev.obj = iproc->obj; - ev.event = EV_SEND_NACK; - xQueueSend(data->objEventQueue, &ev, MAX_PORT_DELAY); + queueEvent(params->recvQueue, iproc->obj, iproc->instId, EV_SEND_NACK); // Release the packet and start over again. PHReleaseTXPacket(pios_packet_handler, p); @@ -467,7 +540,7 @@ static void comUAVTalkTask(void *parameters) else { // Transmit the packet anyway... - xQueueSend(data->sendPacketQueue, &p, MAX_PORT_DELAY); + queueEvent(sendQueue, (void*)p, 0, EV_TRANSMIT_PACKET); } p = NULL; } @@ -512,7 +585,7 @@ static void radioReceiveTask(void *parameters) UAVObjEvent ev; ev.obj = (UAVObjHandle)p; ev.event = EV_PACKET_RECEIVED; - xQueueSend(data->objEventQueue, &ev, portMAX_DELAY); + xQueueSend(data->gcsEventQueue, &ev, portMAX_DELAY); } else { data->packetErrors++; @@ -527,7 +600,7 @@ static void radioReceiveTask(void *parameters) */ static void sendPacketTask(void *parameters) { - PHPacketHandle p; + UAVObjEvent ev; // Loop forever while (1) { @@ -536,19 +609,21 @@ static void sendPacketTask(void *parameters) //PIOS_WDG_UpdateFlag(PIOS_WDG_SENDPACKET); #endif /* PIOS_INCLUDE_WDG */ // Wait for a packet on the queue. - if (xQueueReceive(data->sendPacketQueue, &p, MAX_PORT_DELAY) == pdTRUE) { + if (xQueueReceive(data->radioPacketQueue, &ev, MAX_PORT_DELAY) == pdTRUE) { + PHPacketHandle p = (PHPacketHandle)ev.obj; // Send the packet. if(!PHTransmitPacket(pios_packet_handler, p)) - PHReleaseTXPacket(pios_packet_handler, p); + PHReleaseRXPacket(pios_packet_handler, p); } } } /** - * Send packets to the radio. + * Send packets to the com port. */ -static void sendDataTask(void *parameters) +static void UAVTalkSendTask(void *parameters) { + UAVTalkComTaskParams *params = (UAVTalkComTaskParams *)parameters; UAVObjEvent ev; // Loop forever @@ -560,15 +635,16 @@ static void sendDataTask(void *parameters) //PIOS_WDG_UpdateFlag(PIOS_WDG_SENDDATA); #endif /* PIOS_INCLUDE_WDG */ // Wait for a packet on the queue. - if (xQueueReceive(data->objEventQueue, &ev, MAX_PORT_DELAY) == pdTRUE) { + if (xQueueReceive(params->recvQueue, &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->outUAVTalkCon, ev.obj, 0, 0, RETRY_TIMEOUT_MS); - ++retries; + success = UAVTalkSendObject(params->UAVTalkCon, ev.obj, 0, 0, RETRY_TIMEOUT_MS) == 0; + if (!success) + ++retries; } data->comTxRetries += retries; } @@ -578,8 +654,9 @@ static void sendDataTask(void *parameters) uint32_t retries = 0; int32_t success = -1; while (retries < MAX_RETRIES && success == -1) { - success = UAVTalkSendAck(data->outUAVTalkCon, ev.obj, ev.instId); - ++retries; + success = UAVTalkSendAck(params->UAVTalkCon, ev.obj, ev.instId) == 0; + if (!success) + ++retries; } data->comTxRetries += retries; } @@ -589,8 +666,9 @@ static void sendDataTask(void *parameters) uint32_t retries = 0; int32_t success = -1; while (retries < MAX_RETRIES && success == -1) { - success = UAVTalkSendNack(data->outUAVTalkCon, UAVObjGetID(ev.obj)); - ++retries; + success = UAVTalkSendNack(params->UAVTalkCon, UAVObjGetID(ev.obj)) == 0; + if (!success) + ++retries; } data->comTxRetries += retries; } @@ -599,6 +677,13 @@ static void sendDataTask(void *parameters) // Receive the packet. PHReceivePacket(pios_packet_handler, (PHPacketHandle)ev.obj, false); } + else if(ev.event == EV_TRANSMIT_PACKET) + { + // Transmit the packet. + PHPacketHandle p = (PHPacketHandle)ev.obj; + transmitData(params->comPort, p->data, p->header.data_size, params->checkHID); + PHReleaseTXPacket(pios_packet_handler, p); + } } } } @@ -643,7 +728,7 @@ static void transparentCommTask(void * parameters) } // Receive data from the com port - uint32_t cur_rx_bytes = PIOS_COM_ReceiveBuffer(PIOS_COM_TRANS_COM, p->data + p->header.data_size, + uint32_t cur_rx_bytes = PIOS_COM_ReceiveBuffer(PIOS_COM_TRANS_COM, p->data + p->header.data_size, PH_MAX_DATA - p->header.data_size, timeout); // Do we have an data to send? @@ -675,7 +760,7 @@ static void transparentCommTask(void * parameters) if (send_packet) { // Queue the packet for transmission. - xQueueSend(data->sendPacketQueue, &p, MAX_PORT_DELAY); + queueEvent(data->radioPacketQueue, (void*)p, 0, EV_TRANSMIT_PACKET); // Reset the timeout timeout = MAX_PORT_DELAY; @@ -767,7 +852,7 @@ static void radioStatusTask(void *parameters) status_packet.dropped = data->droppedPackets; status_packet.resets = PIOS_RFM22B_Resets(pios_rfm22b_id); PHPacketHandle sph = (PHPacketHandle)&status_packet; - xQueueSend(data->sendPacketQueue, &sph, MAX_PORT_DELAY); + queueEvent(data->radioPacketQueue, (void*)sph, 0, EV_TRANSMIT_PACKET); cntr = 0; } } @@ -792,22 +877,52 @@ static void ppmInputTask(void *parameters) PIOS_WDG_UpdateFlag(PIOS_WDG_PPMINPUT); #endif /* PIOS_INCLUDE_WDG */ - // Send the PPM packet + // Read the receiver. for (uint8_t i = 1; i <= PIOS_PPM_NUM_INPUTS; ++i) ppm_packet.channels[i - 1] = PIOS_RCVR_Read(PIOS_PPM_RECEIVER, i); - // Send the packet. - ppm_packet.header.destination_id = data->destination_id; - ppm_packet.header.source_id = PIOS_RFM22B_DeviceID(pios_rfm22b_id); - ppm_packet.header.type = PACKET_TYPE_PPM; - ppm_packet.header.data_size = PH_PPM_DATA_SIZE(&ppm_packet); - xQueueSend(data->sendPacketQueue, &pph, MAX_PORT_DELAY); + // Send the PPM packet + if (data->ppmOutQueue) + { + ppm_packet.header.destination_id = data->destination_id; + ppm_packet.header.source_id = PIOS_RFM22B_DeviceID(pios_rfm22b_id); + ppm_packet.header.type = PACKET_TYPE_PPM; + ppm_packet.header.data_size = PH_PPM_DATA_SIZE(&ppm_packet); + queueEvent(data->ppmOutQueue, (void*)pph, 0, EV_TRANSMIT_PACKET); + } + else + PPMHandler(ppm_packet.channels); // Delay until the next update period. vTaskDelay(PIOS_PPM_PACKET_UPDATE_PERIOD_MS / portTICK_RATE_MS); } } +/** + * Transmit data buffer to the com port. + * \param[in] params The comm parameters. + * \param[in] buf Data buffer to send + * \param[in] length Length of buffer + * \return -1 on failure + * \return number of bytes transmitted on success + */ +static int32_t UAVTalkSend(UAVTalkComTaskParams *params, uint8_t *buf, int32_t length) +{ + uint32_t outputPort = params->comPort; +#if defined(PIOS_INCLUDE_USB) + if (params->checkHID) + { + // Determine output port (USB takes priority over telemetry port) + if (PIOS_USB_CheckAvailable(0) && PIOS_COM_USB_HID) + outputPort = PIOS_COM_USB_HID; + } +#endif /* PIOS_INCLUDE_USB */ + if(outputPort) + return PIOS_COM_SendBuffer(outputPort, buf, length); + else + return -1; +} + /** * Transmit data buffer to the com port. * \param[in] buf Data buffer to send @@ -815,18 +930,21 @@ static void ppmInputTask(void *parameters) * \return -1 on failure * \return number of bytes transmitted on success */ -static int32_t transmitData(uint8_t *buf, int32_t length) +static int32_t UAVTalkSendHandler(uint8_t *buf, int32_t length) { - uint32_t outputPort = PIOS_COM_UAVTALK; -#if defined(PIOS_INCLUDE_USB) - // Determine output port (USB takes priority over telemetry port) - if (PIOS_USB_CheckAvailable(0) && PIOS_COM_USB_HID) - outputPort = PIOS_COM_USB_HID; -#endif /* PIOS_INCLUDE_USB */ - if(outputPort) - return PIOS_COM_SendBuffer(outputPort, buf, length); - else - return -1; + return UAVTalkSend(&(data->uavtalk_params), buf, length); +} + +/** + * Transmit data buffer to the com port. + * \param[in] buf Data buffer to send + * \param[in] length Length of buffer + * \return -1 on failure + * \return number of bytes transmitted on success + */ +static int32_t GCSUAVTalkSendHandler(uint8_t *buf, int32_t length) +{ + return UAVTalkSend(&(data->gcs_uavtalk_params), buf, length); } /** @@ -847,22 +965,15 @@ static int32_t transmitPacket(PHPacketHandle p) * \param[in] buf The received data buffer * \param[in] length Length of buffer */ -static void receiveData(uint8_t *buf, uint8_t len, int8_t rssi, int8_t afc) +static void transmitData(uint32_t outputPort, uint8_t *buf, uint8_t len, bool checkHid) { - data->RSSI = rssi; - - // Packet data should go to transparent com if it's configured, - // USB HID if it's connected, otherwise, UAVTalk com if it's configured. - uint32_t outputPort = PIOS_COM_TRANS_COM; - if (!outputPort) - { - outputPort = PIOS_COM_UAVTALK; #if defined(PIOS_INCLUDE_USB) + // See if USB is connected if requested. + if(checkHid) // Determine output port (USB takes priority over telemetry port) if (PIOS_USB_CheckAvailable(0) && PIOS_COM_USB_HID) outputPort = PIOS_COM_USB_HID; #endif /* PIOS_INCLUDE_USB */ - } if (!outputPort) return; @@ -872,6 +983,22 @@ static void receiveData(uint8_t *buf, uint8_t len, int8_t rssi, int8_t afc) data->comTxErrors++; } +/** + * Receive a packet + * \param[in] buf The received data buffer + * \param[in] length Length of buffer + */ +static void receiveData(uint8_t *buf, uint8_t len, int8_t rssi, int8_t afc) +{ + data->RSSI = rssi; + + // Packet data should go to transparent com if it's configured, + // USB HID if it's connected, otherwise, UAVTalk com if it's configured. + uint32_t outputPort = PIOS_COM_TRANS_COM ? PIOS_COM_TRANS_COM : PIOS_COM_UAVTALK; + bool checkHid = (PIOS_COM_TRANS_COM == 0); + transmitData(outputPort, buf, len, checkHid); +} + /** * Receive a status packet * \param[in] status The status structure @@ -987,6 +1114,21 @@ static void BufferedReadSetCom(BufferedReadHandle h, uint32_t com_port) h->com_port = com_port; } +/** + * 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 + */ +static void queueEvent(xQueueHandle queue, void *obj, uint16_t instId, UAVObjEventType type) +{ + UAVObjEvent ev; + ev.obj = (UAVObjHandle)obj; + ev.instId = instId; + ev.event = type; + xQueueSend(queue, &ev, portMAX_DELAY); +} + /** * Update the telemetry settings, called on startup. * FIXME: This should be in the TelemetrySettings object. But objects @@ -1003,7 +1145,6 @@ static void updateSettings() // Initialize the destination ID data->destination_id = pipxSettings.PairID ? pipxSettings.PairID : 0xffffffff; - DEBUG_PRINTF(2, "PairID: %x\n\r", data->destination_id); if (PIOS_COM_TELEMETRY) { switch (pipxSettings.TelemetrySpeed) { diff --git a/flight/PiOS/Boards/STM32103CB_PIPXTREME_Rev1.h b/flight/PiOS/Boards/STM32103CB_PIPXTREME_Rev1.h index 868bba722..71426d594 100755 --- a/flight/PiOS/Boards/STM32103CB_PIPXTREME_Rev1.h +++ b/flight/PiOS/Boards/STM32103CB_PIPXTREME_Rev1.h @@ -77,6 +77,7 @@ TIM4 | RC In 1 | Servo 3 | Servo 2 | Servo 1 #define PIOS_WDG_SENDDATA 0x0008 #define PIOS_WDG_TRANSCOMM 0x0010 #define PIOS_WDG_PPMINPUT 0x0020 +#define PIOS_WDG_COMGCS 0x0040 //------------------------ // TELEMETRY @@ -157,6 +158,7 @@ extern uint32_t pios_com_telemetry_id; extern uint32_t pios_com_flexi_id; extern uint32_t pios_com_vcp_id; extern uint32_t pios_com_uavtalk_com_id; +extern uint32_t pios_com_gcs_com_id; extern uint32_t pios_com_trans_com_id; extern uint32_t pios_com_debug_id; extern uint32_t pios_com_rfm22b_id; @@ -166,6 +168,7 @@ extern uint32_t pios_ppm_rcvr_id; #define PIOS_COM_FLEXI (pios_com_flexi_id) #define PIOS_COM_VCP (pios_com_vcp_id) #define PIOS_COM_UAVTALK (pios_com_uavtalk_com_id) +#define PIOS_COM_GCS (pios_com_gcs_com_id) #define PIOS_COM_TRANS_COM (pios_com_trans_com_id) #define PIOS_COM_DEBUG (pios_com_debug_id) #define PIOS_COM_RADIO (pios_com_rfm22b_id) diff --git a/flight/PipXtreme/Makefile b/flight/PipXtreme/Makefile index 9ef5e9842..63f2e7907 100644 --- a/flight/PipXtreme/Makefile +++ b/flight/PipXtreme/Makefile @@ -53,7 +53,7 @@ USE_GPS ?= NO USE_I2C ?= YES # Set to YES when using Code Sourcery toolchain -CODE_SOURCERY ?= YES +CODE_SOURCERY ?= NO # Remove command is different for Code Sourcery on Windows ifeq ($(CODE_SOURCERY), YES) diff --git a/flight/PipXtreme/System/pios_board.c b/flight/PipXtreme/System/pios_board.c index d1ab4bcfd..3a2dd0544 100644 --- a/flight/PipXtreme/System/pios_board.c +++ b/flight/PipXtreme/System/pios_board.c @@ -33,26 +33,27 @@ #include #include -#define PIOS_COM_SERIAL_RX_BUF_LEN 256 -#define PIOS_COM_SERIAL_TX_BUF_LEN 256 +#define PIOS_COM_SERIAL_RX_BUF_LEN 128 +#define PIOS_COM_SERIAL_TX_BUF_LEN 128 -#define PIOS_COM_FLEXI_RX_BUF_LEN 256 -#define PIOS_COM_FLEXI_TX_BUF_LEN 256 +#define PIOS_COM_FLEXI_RX_BUF_LEN 128 +#define PIOS_COM_FLEXI_TX_BUF_LEN 128 -#define PIOS_COM_TELEM_USB_RX_BUF_LEN 256 -#define PIOS_COM_TELEM_USB_TX_BUF_LEN 256 +#define PIOS_COM_TELEM_USB_RX_BUF_LEN 128 +#define PIOS_COM_TELEM_USB_TX_BUF_LEN 128 -#define PIOS_COM_VCP_USB_RX_BUF_LEN 256 -#define PIOS_COM_VCP_USB_TX_BUF_LEN 256 +#define PIOS_COM_VCP_USB_RX_BUF_LEN 128 +#define PIOS_COM_VCP_USB_TX_BUF_LEN 128 -#define PIOS_COM_RFM22B_RF_RX_BUF_LEN 256 -#define PIOS_COM_RFM22B_RF_TX_BUF_LEN 256 +#define PIOS_COM_RFM22B_RF_RX_BUF_LEN 128 +#define PIOS_COM_RFM22B_RF_TX_BUF_LEN 128 uint32_t pios_com_telem_usb_id = 0; uint32_t pios_com_telemetry_id; uint32_t pios_com_flexi_id; uint32_t pios_com_vcp_id; uint32_t pios_com_uavtalk_com_id = 0; +uint32_t pios_com_gcs_com_id = 0; uint32_t pios_com_trans_com_id = 0; uint32_t pios_com_debug_id = 0; uint32_t pios_com_rfm22b_id = 0; @@ -96,7 +97,7 @@ void PIOS_Board_Init(void) { PipXSettingsData pipxSettings; #if defined(PIOS_INCLUDE_FLASH_EEPROM) - PIOS_EEPROM_Init(&pios_eeprom_cfg); + PIOS_EEPROM_Init(&pios_eeprom_cfg); /* Read the settings from flash. */ /* NOTE: We probably need to save/restore the objID here incase the object changed but the size doesn't */ @@ -129,20 +130,17 @@ void PIOS_Board_Init(void) { /* Flags to determine if various USB interfaces are advertised */ - bool usb_hid_present = false; bool usb_cdc_present = false; #if defined(PIOS_INCLUDE_USB_CDC) if (PIOS_USB_DESC_HID_CDC_Init()) { PIOS_Assert(0); } - usb_hid_present = true; usb_cdc_present = true; #else if (PIOS_USB_DESC_HID_ONLY_Init()) { PIOS_Assert(0); } - usb_hid_present = true; #endif uint32_t pios_usb_id; @@ -172,7 +170,7 @@ void PIOS_Board_Init(void) { tx_buffer, PIOS_COM_VCP_USB_TX_BUF_LEN)) { PIOS_Assert(0); } - switch (pipxSettings.TelemetryConfig) + switch (pipxSettings.VCPConfig) { case PIPXSETTINGS_VCPCONFIG_SERIAL: pios_com_trans_com_id = pios_com_vcp_id; @@ -218,6 +216,7 @@ void PIOS_Board_Init(void) { { case PIPXSETTINGS_TELEMETRYCONFIG_SERIAL: case PIPXSETTINGS_TELEMETRYCONFIG_UAVTALK: + case PIPXSETTINGS_TELEMETRYCONFIG_GCS: case PIPXSETTINGS_TELEMETRYCONFIG_DEBUG: { uint32_t pios_usart1_id; @@ -241,6 +240,9 @@ void PIOS_Board_Init(void) { case PIPXSETTINGS_TELEMETRYCONFIG_UAVTALK: pios_com_uavtalk_com_id = pios_com_telemetry_id; break; + case PIPXSETTINGS_TELEMETRYCONFIG_GCS: + pios_com_gcs_com_id = pios_com_telemetry_id; + break; case PIPXSETTINGS_TELEMETRYCONFIG_DEBUG: pios_com_debug_id = pios_com_telemetry_id; break; @@ -256,6 +258,7 @@ void PIOS_Board_Init(void) { { case PIPXSETTINGS_FLEXICONFIG_SERIAL: case PIPXSETTINGS_FLEXICONFIG_UAVTALK: + case PIPXSETTINGS_FLEXICONFIG_GCS: case PIPXSETTINGS_FLEXICONFIG_DEBUG: { uint32_t pios_usart3_id; @@ -279,6 +282,9 @@ void PIOS_Board_Init(void) { case PIPXSETTINGS_FLEXICONFIG_UAVTALK: pios_com_uavtalk_com_id = pios_com_flexi_id; break; + case PIPXSETTINGS_FLEXICONFIG_GCS: + pios_com_gcs_com_id = pios_com_flexi_id; + break; case PIPXSETTINGS_FLEXICONFIG_DEBUG: pios_com_debug_id = pios_com_flexi_id; break; diff --git a/ground/openpilotgcs/src/plugins/config/airframe.ui b/ground/openpilotgcs/src/plugins/config/airframe.ui index 1e2e15b03..16b0172a3 100644 --- a/ground/openpilotgcs/src/plugins/config/airframe.ui +++ b/ground/openpilotgcs/src/plugins/config/airframe.ui @@ -2775,7 +2775,7 @@ p, li { white-space: pre-wrap; } <table border="0" style="-qt-table-type: root; margin-top:4px; margin-bottom:4px; margin-left:4px; margin-right:4px;"> <tr> <td style="border: none;"> -<p align="center" style=" margin-top:0px; margin-bottom:0px; margin-left:0px; margin-right:0px; -qt-block-indent:0; text-indent:0px;"><span style=" font-family:'Ubuntu'; font-size:14pt; font-weight:600; color:#ff0000;">SETTING UP FEED FORWARD IS DANGEROUS</span></p> +<p align="center" style=" margin-top:0px; margin-bottom:0px; margin-left:0px; margin-right:0px; -qt-block-indent:0; text-indent:0px;"><span style=" font-family:'Ubuntu'; font-size:14pt; font-weight:600; color:#ff0000;">SETTING UP FEED FORWARD NEEDS CAUTION</span></p> <p style="-qt-paragraph-type:empty; margin-top:0px; margin-bottom:0px; margin-left:0px; margin-right:0px; -qt-block-indent:0; text-indent:0px; font-family:'Ubuntu'; font-size:11pt;"></p> <p style=" margin-top:0px; margin-bottom:0px; margin-left:0px; margin-right:0px; -qt-block-indent:0; text-indent:0px;"><span style=" font-family:'Lucida Grande'; font-size:13pt;">Beware: Feed Forward Tuning will launch all engines around mid-throttle, you have been warned!</span></p> <p style=" margin-top:12px; margin-bottom:12px; margin-left:0px; margin-right:0px; -qt-block-indent:0; text-indent:0px;"><span style=" font-family:'Lucida Grande'; font-size:13pt;">Remove your props initially, and for fine-tuning, make sure your airframe is safely held in place. Wear glasses and protect your face and body.</span></p></td></tr></table></body></html> diff --git a/ground/openpilotgcs/src/plugins/config/configpipxtremewidget.cpp b/ground/openpilotgcs/src/plugins/config/configpipxtremewidget.cpp index fb1223607..7808b6dfa 100644 --- a/ground/openpilotgcs/src/plugins/config/configpipxtremewidget.cpp +++ b/ground/openpilotgcs/src/plugins/config/configpipxtremewidget.cpp @@ -71,6 +71,8 @@ ConfigPipXtremeWidget::ConfigPipXtremeWidget(QWidget *parent) : ConfigTaskWidget addUAVObjectToWidgetRelation("PipXStatus", "MinFrequency", m_pipx->MinFrequency); addUAVObjectToWidgetRelation("PipXStatus", "MaxFrequency", m_pipx->MaxFrequency); addUAVObjectToWidgetRelation("PipXStatus", "FrequencyStepSize", m_pipx->FrequencyStepSize); + addUAVObjectToWidgetRelation("PipXStatus", "FrequencyBand", m_pipx->FreqBand); + addUAVObjectToWidgetRelation("PipXStatus", "RSSI", m_pipx->RSSI); addUAVObjectToWidgetRelation("PipXStatus", "AFC", m_pipx->RxAFC); addUAVObjectToWidgetRelation("PipXStatus", "Retries", m_pipx->Retries); addUAVObjectToWidgetRelation("PipXStatus", "Errors", m_pipx->Errors); @@ -81,6 +83,7 @@ ConfigPipXtremeWidget::ConfigPipXtremeWidget(QWidget *parent) : ConfigTaskWidget addUAVObjectToWidgetRelation("PipXStatus", "TXRate", m_pipx->TXRate); // Connect to the pair ID radio buttons. + connect(m_pipx->PairSelectB, SIGNAL(toggled(bool)), this, SLOT(pairBToggled(bool))); connect(m_pipx->PairSelect1, SIGNAL(toggled(bool)), this, SLOT(pair1Toggled(bool))); connect(m_pipx->PairSelect2, SIGNAL(toggled(bool)), this, SLOT(pair2Toggled(bool))); connect(m_pipx->PairSelect3, SIGNAL(toggled(bool)), this, SLOT(pair3Toggled(bool))); @@ -89,6 +92,7 @@ ConfigPipXtremeWidget::ConfigPipXtremeWidget(QWidget *parent) : ConfigTaskWidget //Add scroll bar when necessary QScrollArea *scroll = new QScrollArea; scroll->setWidget(m_pipx->frame_3); + scroll->setWidgetResizable(true); m_pipx->verticalLayout_3->addWidget(scroll); // Request and update of the setting object. @@ -148,7 +152,7 @@ void ConfigPipXtremeWidget::updateStatus(UAVObject *object) PipXSettings *pipxSettings = PipXSettings::GetInstance(getObjectManager()); quint32 pairID = 0; if (pipxSettings) - pipxSettings->getPairID(); + pairID = pipxSettings->getPairID(); // Update the detected devices. UAVObjectField* pairIdField = object->getField("PairIDs"); @@ -245,6 +249,9 @@ void ConfigPipXtremeWidget::updateStatus(UAVObject *object) qDebug() << "PipXtremeGadgetWidget: Count not read DeviceID field."; } + // Update the PairID field + m_pipx->PairID->setText(QString::number(pairID, 16).toUpper()); + // Update the link state UAVObjectField* linkField = object->getField("LinkState"); if (linkField) { @@ -284,9 +291,16 @@ void ConfigPipXtremeWidget::pairIDToggled(bool checked, quint8 idx) if (pipxStatus && pipxSettings) { - quint32 pairID = pipxStatus->getPairIDs(idx); - if (pairID) - pipxSettings->setPairID(pairID); + if (idx == 4) + { + pipxSettings->setPairID(0); + } + else + { + quint32 pairID = pipxStatus->getPairIDs(idx); + if (pairID) + pipxSettings->setPairID(pairID); + } } } } @@ -311,6 +325,11 @@ void ConfigPipXtremeWidget::pair4Toggled(bool checked) pairIDToggled(checked, 3); } +void ConfigPipXtremeWidget::pairBToggled(bool checked) +{ + pairIDToggled(checked, 4); +} + /** @} @} diff --git a/ground/openpilotgcs/src/plugins/config/configpipxtremewidget.h b/ground/openpilotgcs/src/plugins/config/configpipxtremewidget.h index b702326e8..28db67b74 100644 --- a/ground/openpilotgcs/src/plugins/config/configpipxtremewidget.h +++ b/ground/openpilotgcs/src/plugins/config/configpipxtremewidget.h @@ -63,6 +63,7 @@ private slots: void pair2Toggled(bool checked); void pair3Toggled(bool checked); void pair4Toggled(bool checked); + void pairBToggled(bool checked); }; #endif // CONFIGTXPIDWIDGET_H diff --git a/ground/openpilotgcs/src/plugins/config/pipxtreme.ui b/ground/openpilotgcs/src/plugins/config/pipxtreme.ui index 557023855..9de1a7120 100644 --- a/ground/openpilotgcs/src/plugins/config/pipxtreme.ui +++ b/ground/openpilotgcs/src/plugins/config/pipxtreme.ui @@ -6,8 +6,8 @@ 0 0 - 840 - 862 + 834 + 772 @@ -63,9 +63,6 @@ - - 4 - @@ -84,7 +81,7 @@ 40 - 20 + 5 @@ -138,16 +135,56 @@ + + + + + + + + + + false + + + + 100 + 16777215 + + + + Broadcast + + + + + + + Broadcast Address + + + + - - + + + + + 100 + 16777215 + + + + 12345678 + + - + -127 @@ -166,17 +203,31 @@ - + + + + -100dB + + + + - - + + + + + 100 + 16777215 + + + - + -127 @@ -195,17 +246,31 @@ - + + + + -100dB + + + + - - + + + + + 100 + 16777215 + + + - + -127 @@ -224,17 +289,31 @@ - + + + + -100dB + + + + - - + + + + + 100 + 16777215 + + + - + -127 @@ -253,28 +332,7 @@ - - - - -100dB - - - - - - - -100dB - - - - - - - -100dB - - - - + -100dB @@ -288,7 +346,7 @@ - 400 + 430 0 @@ -305,15 +363,27 @@ - Firmware Version + Firmware Ver. Qt::AlignRight|Qt::AlignTrailing|Qt::AlignVCenter - + + + + 0 + 0 + + + + + 16777215 + 16777215 + + 75 @@ -324,7 +394,7 @@ QLineEdit { border: none; border-radius: 1px; - padding: 0 8px; + padding: 0 4px; background: rgba(0, 0, 0, 16); /* background: transparent; */ /* selection-background-color: darkgray;*/ @@ -348,7 +418,7 @@ - + @@ -356,6 +426,12 @@ 0 + + + 0 + 0 + + 16777215 @@ -379,7 +455,7 @@ QLineEdit { border: none; border-radius: 1px; - padding: 0 8px; + padding: 0 4px; background: rgba(0, 0, 0, 16); /* background: transparent; */ /* selection-background-color: darkgray;*/ @@ -396,6 +472,108 @@ + + + + Device ID + + + Qt::AlignRight|Qt::AlignTrailing|Qt::AlignVCenter + + + + + + + + 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 + + + + + + + Pair ID + + + Qt::AlignRight|Qt::AlignTrailing|Qt::AlignVCenter + + + + + + + + 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 + + + 90ABCDEF + + + @@ -414,9 +592,15 @@ 0 + + + 0 + 0 + + - 16777215 + 101 16777215 @@ -433,7 +617,7 @@ QLineEdit { border: none; border-radius: 1px; - padding: 0 8px; + padding: 0 4px; background: rgba(0, 0, 0, 16); /* background: transparent; */ /* selection-background-color: darkgray;*/ @@ -447,7 +631,7 @@ - + Max Frequency @@ -457,7 +641,7 @@ - + @@ -467,7 +651,7 @@ - 16777215 + 101 16777215 @@ -484,7 +668,7 @@ QLineEdit { border: none; border-radius: 1px; - padding: 0 8px; + padding: 0 4px; background: rgba(0, 0, 0, 16); /* background: transparent; */ /* selection-background-color: darkgray;*/ @@ -498,7 +682,7 @@ - + Freq. Step Size @@ -508,7 +692,7 @@ - + @@ -516,9 +700,15 @@ 0 + + + 0 + 0 + + - 16777215 + 101 16777215 @@ -535,7 +725,7 @@ QLineEdit { border: none; border-radius: 1px; - padding: 0 8px; + padding: 0 4px; background: rgba(0, 0, 0, 16); /* background: transparent; */ /* selection-background-color: darkgray;*/ @@ -549,7 +739,232 @@ + + + + Freq. Band + + + Qt::AlignRight|Qt::AlignTrailing|Qt::AlignVCenter + + + + + + + + 0 + 0 + + + + + 101 + 16777215 + + + + + 75 + true + + + + The current frequency band + + + 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 + + + + + + + RSSI + + + Qt::AlignRight|Qt::AlignTrailing|Qt::AlignVCenter + + + + + + + + 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 + + + + + + + Rx AFC + + + Qt::AlignRight|Qt::AlignTrailing|Qt::AlignVCenter + + + + + + + + 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 + + + + + + TX Rate (B/s) + + + Qt::AlignRight|Qt::AlignTrailing|Qt::AlignVCenter + + + + + + + + 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 + + + true + + + + + + + RX Rate (B/s) + + + Qt::AlignRight|Qt::AlignTrailing|Qt::AlignVCenter + + + + + + + + 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 + + + true + + + + Link State @@ -559,17 +974,23 @@ - + - + 0 0 + + + 0 + 0 + + - 16777215 + 101 16777215 @@ -586,7 +1007,7 @@ QLineEdit { border: none; border-radius: 1px; - padding: 0 8px; + padding: 0 3px; background: rgba(0, 0, 0, 16); /* background: transparent; */ /* selection-background-color: darkgray;*/ @@ -598,20 +1019,29 @@ true + + Disconnected + - - + + - Rx AFC + Errors Qt::AlignRight|Qt::AlignTrailing|Qt::AlignVCenter - - + + + + + 101 + 16777215 + + 75 @@ -622,12 +1052,15 @@ QLineEdit { border: none; border-radius: 1px; - padding: 0 8px; + padding: 0 4px; background: rgba(0, 0, 0, 16); /* background: transparent; */ /* selection-background-color: darkgray;*/ } + + false + true @@ -645,6 +1078,18 @@ + + + 0 + 0 + + + + + 101 + 16777215 + + 75 @@ -655,7 +1100,7 @@ QLineEdit { border: none; border-radius: 1px; - padding: 0 8px; + padding: 0 4px; background: rgba(0, 0, 0, 16); /* background: transparent; */ /* selection-background-color: darkgray;*/ @@ -669,8 +1114,24 @@ - - + + + + UAVTalk Errors + + + Qt::AlignRight|Qt::AlignTrailing|Qt::AlignVCenter + + + + + + + + 101 + 16777215 + + 75 @@ -681,7 +1142,7 @@ QLineEdit { border: none; border-radius: 1px; - padding: 0 8px; + padding: 0 4px; background: rgba(0, 0, 0, 16); /* background: transparent; */ /* selection-background-color: darkgray;*/ @@ -696,52 +1157,6 @@ - - - Errors - - - Qt::AlignRight|Qt::AlignTrailing|Qt::AlignVCenter - - - - - - - UAVTalk Errors - - - Qt::AlignRight|Qt::AlignTrailing|Qt::AlignVCenter - - - - - - - - 75 - true - - - - QLineEdit { - border: none; - border-radius: 1px; - padding: 0 8px; - background: rgba(0, 0, 0, 16); -/* background: transparent; */ -/* selection-background-color: darkgray;*/ -} - - - false - - - true - - - - Resets @@ -751,8 +1166,20 @@ - + + + + 0 + 0 + + + + + 101 + 16777215 + + 75 @@ -763,7 +1190,7 @@ QLineEdit { border: none; border-radius: 1px; - padding: 0 8px; + padding: 0 4px; background: rgba(0, 0, 0, 16); /* background: transparent; */ /* selection-background-color: darkgray;*/ @@ -777,7 +1204,7 @@ - + Dropped @@ -787,8 +1214,14 @@ - + + + + 101 + 16777215 + + 75 @@ -799,7 +1232,7 @@ QLineEdit { border: none; border-radius: 1px; - padding: 0 8px; + padding: 0 4px; background: rgba(0, 0, 0, 16); /* background: transparent; */ /* selection-background-color: darkgray;*/ @@ -813,114 +1246,6 @@ - - - - TX Rate (B/s) - - - Qt::AlignRight|Qt::AlignTrailing|Qt::AlignVCenter - - - - - - - - 75 - true - - - - QLineEdit { - border: none; - border-radius: 1px; - padding: 0 8px; - background: rgba(0, 0, 0, 16); -/* background: transparent; */ -/* selection-background-color: darkgray;*/ -} - - - false - - - true - - - - - - - RX Rate (B/s) - - - Qt::AlignRight|Qt::AlignTrailing|Qt::AlignVCenter - - - - - - - - 75 - true - - - - QLineEdit { - border: none; - border-radius: 1px; - padding: 0 8px; - background: rgba(0, 0, 0, 16); -/* background: transparent; */ -/* selection-background-color: darkgray;*/ -} - - - false - - - true - - - - - - - - 75 - true - - - - QLineEdit { - border: none; - border-radius: 1px; - padding: 0 8px; - background: rgba(0, 0, 0, 16); -/* background: transparent; */ -/* selection-background-color: darkgray;*/ -} - - - false - - - true - - - - - - - Device ID - - - Qt::AlignRight|Qt::AlignTrailing|Qt::AlignVCenter - - - @@ -929,6 +1254,12 @@ + + + 0 + 0 + + 75 diff --git a/ground/openpilotgcs/src/plugins/gcscontrol/Matlab/GCSControl.m b/ground/openpilotgcs/src/plugins/gcscontrol/Matlab/GCSControl.m new file mode 100644 index 000000000..39e89ffc1 --- /dev/null +++ b/ground/openpilotgcs/src/plugins/gcscontrol/Matlab/GCSControl.m @@ -0,0 +1,48 @@ +% GCSCONTROL +% This class allows the user to send 4-axis stick commands to OpenPilot +% GCS. +% +% Create class by +% control = GCSControl +% +% Open connection by +% control.connect('01.23.45.67', 89) +% where the first value is the IP address of the computer running GCS and +% the second value is the port on which GCS is listening. +% +% Send command by +% control.command(pitch, yaw, roll, throttle) +% where all variables are between [-1,1] +% +% Close connection by +% control.close() + +classdef GCSControl < handle + + properties + udpObj; + isConnected=false; + end + + methods + function obj=GCSControl() + obj.isConnected = false; + end + function obj=connect(obj,rhost,rport) + obj.udpObj = udp(rhost,rport); + fopen(obj.udpObj); + obj.isConnected = true; + end + function obj=command(obj,pitch,yaw,roll,throttle) + if(obj.isConnected) + fwrite(obj.udpObj,[42,pitch,yaw,roll,throttle,36],'double') + end + end + function obj=close(obj) + if(obj.isConnected) + fclose(obj.udpObj); + obj.isConnected = false; + end + end + end +end \ No newline at end of file diff --git a/ground/openpilotgcs/src/plugins/gcscontrol/gcscontrol.pro b/ground/openpilotgcs/src/plugins/gcscontrol/gcscontrol.pro index 426579e77..2e4695328 100644 --- a/ground/openpilotgcs/src/plugins/gcscontrol/gcscontrol.pro +++ b/ground/openpilotgcs/src/plugins/gcscontrol/gcscontrol.pro @@ -2,6 +2,7 @@ TEMPLATE = lib TARGET = GCSControl QT += svg QT += opengl +QT += network include(../../openpilotgcsplugin.pri) include(../../plugins/coreplugin/coreplugin.pri) diff --git a/ground/openpilotgcs/src/plugins/gcscontrol/gcscontrol.ui b/ground/openpilotgcs/src/plugins/gcscontrol/gcscontrol.ui index 024db71bb..0d2889785 100644 --- a/ground/openpilotgcs/src/plugins/gcscontrol/gcscontrol.ui +++ b/ground/openpilotgcs/src/plugins/gcscontrol/gcscontrol.ui @@ -41,8 +41,21 @@ + + + + false + + + UDP Control + + + + + true + Armed diff --git a/ground/openpilotgcs/src/plugins/gcscontrol/gcscontrolgadget.cpp b/ground/openpilotgcs/src/plugins/gcscontrol/gcscontrolgadget.cpp index 3c00987f7..b95082fb9 100644 --- a/ground/openpilotgcs/src/plugins/gcscontrol/gcscontrolgadget.cpp +++ b/ground/openpilotgcs/src/plugins/gcscontrol/gcscontrolgadget.cpp @@ -44,6 +44,10 @@ GCSControlGadget::GCSControlGadget(QString classId, GCSControlGadgetWidget *widg manualControlCommandUpdated(getManualControlCommand()); + control_sock = new QUdpSocket(this); + + connect(control_sock,SIGNAL(readyRead()),this,SLOT(readUDPCommand())); + joystickTime.start(); GCSControlPlugin *pl = dynamic_cast(plugin); connect(pl->sdlGamepad,SIGNAL(gamepads(quint8)),this,SLOT(gamepads(quint8))); @@ -67,6 +71,12 @@ void GCSControlGadget::loadConfiguration(IUAVGadgetConfiguration* config) yawChannel = ql.at(2); throttleChannel = ql.at(3); + // if(control_sock->isOpen()) + // control_sock->close(); + control_sock->bind(GCSControlConfig->getUDPControlHost(), GCSControlConfig->getUDPControlPort(),QUdpSocket::ShareAddress); + + + controlsMode = GCSControlConfig->getControlsMode(); int i; @@ -174,7 +184,8 @@ void GCSControlGadget::sticksChangedLocally(double leftX, double leftY, double r } //if we are not in local gcs control mode, ignore the joystick input - if (((GCSControlGadgetWidget *)m_widget)->getGCSControl()==false)return; + if (((GCSControlGadgetWidget *)m_widget)->getGCSControl()==false || ((GCSControlGadgetWidget *)m_widget)->getUDPControl()) + return; if((newThrottle != oldThrottle) || (newPitch != oldPitch) || (newYaw != oldYaw) || (newRoll != oldRoll)) { if (buttonRollControl==0)obj->getField("Roll")->setDouble(newRoll); @@ -191,6 +202,93 @@ void GCSControlGadget::gamepads(quint8 count) // sdlGamepad.setTickRate(JOYSTICK_UPDATE_RATE); } +void GCSControlGadget::readUDPCommand() +{ + double pitch, yaw, roll, throttle; + while (control_sock->hasPendingDatagrams()) { + QByteArray datagram; + datagram.resize(control_sock->pendingDatagramSize()); + control_sock->readDatagram(datagram.data(), datagram.size()); + QDataStream readData(datagram); + bool badPack = false; + int state = 0; + while(!readData.atEnd() && !badPack) + { + double buffer; + readData >> buffer; + switch(state) + { + case 0: + if(buffer == 42){ + state = 1; + }else{ + state = 0; + badPack = true; + } + break; + case 1: + pitch = buffer; + state = 2; + break; + case 2: + yaw = buffer; + state = 3; + break; + case 3: + roll = buffer; + state = 4; + break; + case 4: + throttle = buffer; + state = 5; + break; + case 5: + if(buffer != 36 || !readData.atEnd()) + badPack=true; + break; + } + + } + if(!badPack && ((GCSControlGadgetWidget *)m_widget)->getUDPControl()) + { + ManualControlCommand * obj = getManualControlCommand(); + bool update = false; + + if(pitch != obj->getField("Pitch")->getDouble()){ + obj->getField("Pitch")->setDouble(constrain(pitch)); + update = true; + } + if(yaw != obj->getField("Yaw")->getDouble()){ + obj->getField("Yaw")->setDouble(constrain(yaw)); + update = true; + } + if(roll != obj->getField("Roll")->getDouble()){ + obj->getField("Roll")->setDouble(constrain(roll)); + update = true; + } + if(throttle != obj->getField("Throttle")->getDouble()){ + obj->getField("Throttle")->setDouble(constrain(throttle)); + update = true; + } + if(update) + obj->updated(); + } + } + + qDebug() << "Pitch: " << pitch << " Yaw: " << yaw << " Roll: " << roll << " Throttle: " << throttle; + + +} + +double GCSControlGadget::constrain(double value) +{ + if(value < -1) + return -1; + if(value > 1) + return 1; + return value; +} + void GCSControlGadget::buttonState(ButtonNumber number, bool pressed) { int state; @@ -200,6 +298,7 @@ void GCSControlGadget::buttonState(ButtonNumber number, bool pressed) UAVObjectManager *objManager = pm->getObject(); UAVDataObject* obj = dynamic_cast( objManager->getObject(QString("ManualControlCommand")) ); bool currentCGSControl = ((GCSControlGadgetWidget *)m_widget)->getGCSControl(); + bool currentUDPControl = ((GCSControlGadgetWidget *)m_widget)->getUDPControl(); switch (buttonSettings[number].ActionID) { @@ -268,6 +367,11 @@ void GCSControlGadget::buttonState(ButtonNumber number, bool pressed) ((GCSControlGadgetWidget *)m_widget)->setGCSControl(!currentCGSControl); break; + case 3: //UDP Control + if(currentCGSControl) + ((GCSControlGadgetWidget *)m_widget)->setUDPControl(!currentUDPControl); + + break; } break; diff --git a/ground/openpilotgcs/src/plugins/gcscontrol/gcscontrolgadget.h b/ground/openpilotgcs/src/plugins/gcscontrol/gcscontrolgadget.h index 4d465b0cd..216e6d075 100644 --- a/ground/openpilotgcs/src/plugins/gcscontrol/gcscontrolgadget.h +++ b/ground/openpilotgcs/src/plugins/gcscontrol/gcscontrolgadget.h @@ -34,6 +34,9 @@ #include "sdlgamepad/sdlgamepad.h" #include #include "gcscontrolplugin.h" +#include +#include + namespace Core { class IUAVGadget; @@ -59,6 +62,7 @@ public: private: ManualControlCommand* getManualControlCommand(); + double constrain(double value); QTime joystickTime; QWidget *m_widget; QList m_context; @@ -72,6 +76,8 @@ private: double bound(double input); double wrap(double input); bool channelReverse[8]; + QUdpSocket *control_sock; + signals: void sticksChangedRemotely(double leftX, double leftY, double rightX, double rightY); @@ -79,6 +85,7 @@ signals: protected slots: void manualControlCommandUpdated(UAVObject *); void sticksChangedLocally(double leftX, double leftY, double rightX, double rightY); + void readUDPCommand(); // signals from joystick void gamepads(quint8 count); diff --git a/ground/openpilotgcs/src/plugins/gcscontrol/gcscontrolgadgetconfiguration.cpp b/ground/openpilotgcs/src/plugins/gcscontrol/gcscontrolgadgetconfiguration.cpp index 24a72e62a..c213088c6 100644 --- a/ground/openpilotgcs/src/plugins/gcscontrol/gcscontrolgadgetconfiguration.cpp +++ b/ground/openpilotgcs/src/plugins/gcscontrol/gcscontrolgadgetconfiguration.cpp @@ -54,6 +54,9 @@ GCSControlGadgetConfiguration::GCSControlGadgetConfiguration(QString classId, QS yawChannel = qSettings->value("yawChannel").toInt(); throttleChannel = qSettings->value("throttleChannel").toInt(); + udp_port = qSettings->value("controlPortUDP").toUInt(); + udp_host = QHostAddress(qSettings->value("controlHostUDP").toString()); + int i; for (i=0;i<8;i++) { @@ -66,6 +69,21 @@ GCSControlGadgetConfiguration::GCSControlGadgetConfiguration(QString classId, QS } +void GCSControlGadgetConfiguration::setUDPControlSettings(int port, QString host) +{ + udp_port = port; + udp_host = QHostAddress(host); +} + +int GCSControlGadgetConfiguration::getUDPControlPort() +{ + return udp_port; +} +QHostAddress GCSControlGadgetConfiguration::getUDPControlHost() +{ + return udp_host; +} + void GCSControlGadgetConfiguration::setRPYTchannels(int roll, int pitch, int yaw, int throttle) { rollChannel = roll; pitchChannel = pitch; @@ -102,6 +120,9 @@ IUAVGadgetConfiguration *GCSControlGadgetConfiguration::clone() m->yawChannel = yawChannel; m->throttleChannel = throttleChannel; + m->udp_host = udp_host; + m->udp_port = udp_port; + int i; for (i=0;i<8;i++) { @@ -126,6 +147,9 @@ void GCSControlGadgetConfiguration::saveConfig(QSettings* settings) const { settings->setValue("yawChannel", yawChannel); settings->setValue("throttleChannel", throttleChannel); + settings->setValue("controlPortUDP",QString::number(udp_port)); + settings->setValue("controlHostUDP",udp_host.toString()); + int i; for (i=0;i<8;i++) { diff --git a/ground/openpilotgcs/src/plugins/gcscontrol/gcscontrolgadgetconfiguration.h b/ground/openpilotgcs/src/plugins/gcscontrol/gcscontrolgadgetconfiguration.h index c3ca4c6d3..8f1a07359 100644 --- a/ground/openpilotgcs/src/plugins/gcscontrol/gcscontrolgadgetconfiguration.h +++ b/ground/openpilotgcs/src/plugins/gcscontrol/gcscontrolgadgetconfiguration.h @@ -29,6 +29,7 @@ #define GCSCONTROLGADGETCONFIGURATION_H #include +#include typedef struct{ int ActionID; @@ -36,6 +37,11 @@ typedef struct{ double Amount; }buttonSettingsStruct; +typedef struct{ + int port; + QHostAddress address; +}portSettingsStruct; + using namespace Core; @@ -49,6 +55,9 @@ class GCSControlGadgetConfiguration : public IUAVGadgetConfiguration void setControlsMode(int mode) { controlsMode = mode; } void setRPYTchannels(int roll, int pitch, int yaw, int throttle); + void setUDPControlSettings(int port, QString host); + int getUDPControlPort(); + QHostAddress getUDPControlHost(); int getControlsMode() { return controlsMode; } QList getChannelsMapping(); QList getChannelsReverse(); @@ -72,6 +81,8 @@ class GCSControlGadgetConfiguration : public IUAVGadgetConfiguration int throttleChannel; buttonSettingsStruct buttonSettings[8]; bool channelReverse[8]; + int udp_port; + QHostAddress udp_host; }; diff --git a/ground/openpilotgcs/src/plugins/gcscontrol/gcscontrolgadgetoptionspage.cpp b/ground/openpilotgcs/src/plugins/gcscontrol/gcscontrolgadgetoptionspage.cpp index ad0bf9a34..1a68c6be6 100644 --- a/ground/openpilotgcs/src/plugins/gcscontrol/gcscontrolgadgetoptionspage.cpp +++ b/ground/openpilotgcs/src/plugins/gcscontrol/gcscontrolgadgetoptionspage.cpp @@ -137,7 +137,7 @@ QWidget *GCSControlGadgetOptionsPage::createPage(QWidget *parent) options_page->buttonFunction4 << options_page->buttonFunction5 << options_page->buttonFunction6 << options_page->buttonFunction7; QStringList buttonOptions; - buttonOptions <<"-" << "Roll" << "Pitch" << "Yaw" << "Throttle" << "Armed" << "GCS Control" ; + buttonOptions <<"-" << "Roll" << "Pitch" << "Yaw" << "Throttle" << "Armed" << "GCS Control"; //added UDP control to action list foreach (QComboBox* qb, buttonFunctionList) { qb->addItems(buttonOptions); } @@ -187,6 +187,9 @@ QWidget *GCSControlGadgetOptionsPage::createPage(QWidget *parent) //updateButtonFunction(); + options_page->udp_host->setText(m_config->getUDPControlHost().toString()); + options_page->udp_port->setText(QString::number(m_config->getUDPControlPort())); + // Controls mode are from 1 to 4. if (m_config->getControlsMode()>0 && m_config->getControlsMode() < 5) @@ -262,6 +265,9 @@ void GCSControlGadgetOptionsPage::apply() } m_config->setRPYTchannels(roll,pitch,yaw,throttle); + m_config->setUDPControlSettings(options_page->udp_port->text().toInt(),options_page->udp_host->text()); + + int j; for (j=0;j<8;j++) { @@ -271,6 +277,7 @@ void GCSControlGadgetOptionsPage::apply() m_config->setChannelReverse(j,chRevList.at(j)->isChecked()); } + } void GCSControlGadgetOptionsPage::finish() @@ -369,7 +376,7 @@ void GCSControlGadgetOptionsPage::updateButtonAction(int controlID) if (buttonActionList.at(i)->currentText().compare("Toggles")==0) { disconnect(buttonFunctionList.at(i),SIGNAL(currentIndexChanged(int)),this,SLOT(updateButtonFunction())); - buttonOptions <<"-" << "Armed" << "GCS Control" ; + buttonOptions <<"-" << "Armed" << "GCS Control" << "UDP Control"; buttonFunctionList.at(i)->clear(); buttonFunctionList.at(i)->insertItems(-1,buttonOptions); diff --git a/ground/openpilotgcs/src/plugins/gcscontrol/gcscontrolgadgetoptionspage.ui b/ground/openpilotgcs/src/plugins/gcscontrol/gcscontrolgadgetoptionspage.ui index 260aa4e04..5191788a0 100644 --- a/ground/openpilotgcs/src/plugins/gcscontrol/gcscontrolgadgetoptionspage.ui +++ b/ground/openpilotgcs/src/plugins/gcscontrol/gcscontrolgadgetoptionspage.ui @@ -141,7 +141,7 @@ - + 0 @@ -1011,6 +1011,66 @@ + + + UDP Setup + + + + + 20 + 20 + 301 + 71 + + + + UDP Port Configuration + + + + + + Host: + + + + + + + + 120 + 16777215 + + + + 127.0.0.1 + + + + + + + Port: + + + + + + + + 50 + 16777215 + + + + 2323 + + + + + + diff --git a/ground/openpilotgcs/src/plugins/gcscontrol/gcscontrolgadgetwidget.cpp b/ground/openpilotgcs/src/plugins/gcscontrol/gcscontrolgadgetwidget.cpp index 6ee123ff6..09e25132d 100644 --- a/ground/openpilotgcs/src/plugins/gcscontrol/gcscontrolgadgetwidget.cpp +++ b/ground/openpilotgcs/src/plugins/gcscontrol/gcscontrolgadgetwidget.cpp @@ -34,6 +34,7 @@ #include #include + #include "uavobject.h" #include "uavobjectmanager.h" #include "manualcontrolcommand.h" @@ -64,9 +65,14 @@ GCSControlGadgetWidget::GCSControlGadgetWidget(QWidget *parent) : QLabel(parent) connect(m_gcscontrol->checkBoxArmed, SIGNAL(stateChanged(int)), this, SLOT(toggleArmed(int))); connect(m_gcscontrol->comboBoxFlightMode, SIGNAL(currentIndexChanged(int)), this, SLOT(selectFlightMode(int))); + connect(m_gcscontrol->checkBoxUDPControl, SIGNAL(stateChanged(int)),this,SLOT(toggleUDPControl(int))); //UDP control checkbox + // Connect object updated event from UAVObject to also update check boxes and dropdown connect(obj, SIGNAL(objectUpdated(UAVObject*)), this, SLOT(mccChanged(UAVObject*))); + + + leftX = 0; leftY = 0; rightX = 0; @@ -122,11 +128,14 @@ void GCSControlGadgetWidget::toggleControl(int state) UAVObject::SetGcsTelemetryAcked(mdata, false); UAVObject::SetGcsTelemetryUpdateMode(mdata, UAVObject::UPDATEMODE_ONCHANGE); mdata.gcsTelemetryUpdatePeriod = 100; + m_gcscontrol->checkBoxUDPControl->setEnabled(true); } else { mdata = mccInitialData; + toggleUDPControl(false); + m_gcscontrol->checkBoxUDPControl->setEnabled(false); } obj->setMetadata(mdata); } @@ -152,6 +161,16 @@ void GCSControlGadgetWidget::mccChanged(UAVObject * obj) m_gcscontrol->checkBoxArmed->setChecked(flightStatus->getField("Armed")->getValue() == "Armed"); } +void GCSControlGadgetWidget::toggleUDPControl(int state) +{ + if(state) + { + setUDPControl(true); + }else{ + setUDPControl(false); + } +} + /*! \brief Called when the flight mode drop down is changed and sets the ManualControlCommand->FlightMode accordingly */ @@ -168,11 +187,21 @@ void GCSControlGadgetWidget::selectFlightMode(int state) void GCSControlGadgetWidget::setGCSControl(bool newState) { m_gcscontrol->checkBoxGcsControl->setChecked(newState); -}; +} bool GCSControlGadgetWidget::getGCSControl(void) { return m_gcscontrol->checkBoxGcsControl->isChecked(); -}; +} + +void GCSControlGadgetWidget::setUDPControl(bool newState) +{ + m_gcscontrol->checkBoxUDPControl->setChecked(newState); +} + +bool GCSControlGadgetWidget::getUDPControl(void) +{ + return m_gcscontrol->checkBoxUDPControl->isChecked(); +} /** diff --git a/ground/openpilotgcs/src/plugins/gcscontrol/gcscontrolgadgetwidget.h b/ground/openpilotgcs/src/plugins/gcscontrol/gcscontrolgadgetwidget.h index 58c2ecb0f..5f4b3eba0 100644 --- a/ground/openpilotgcs/src/plugins/gcscontrol/gcscontrolgadgetwidget.h +++ b/ground/openpilotgcs/src/plugins/gcscontrol/gcscontrolgadgetwidget.h @@ -31,6 +31,8 @@ #include #include "manualcontrolcommand.h" +#define UDP_PORT 2323 + class Ui_GCSControl; class GCSControlGadgetWidget : public QLabel @@ -42,6 +44,8 @@ public: ~GCSControlGadgetWidget(); void setGCSControl(bool newState); bool getGCSControl(void); + void setUDPControl(bool newState); + bool getUDPControl(void); signals: void sticksChanged(double leftX, double leftY, double rightX, double rightY); @@ -59,6 +63,7 @@ protected slots: void toggleArmed(int state); void selectFlightMode(int state); void mccChanged(UAVObject *); + void toggleUDPControl(int state); private: Ui_GCSControl *m_gcscontrol; diff --git a/shared/uavobjectdefinition/gcsreceiver.xml b/shared/uavobjectdefinition/gcsreceiver.xml index cbc00c965..ee4fa696f 100644 --- a/shared/uavobjectdefinition/gcsreceiver.xml +++ b/shared/uavobjectdefinition/gcsreceiver.xml @@ -2,7 +2,7 @@ A receiver channel group carried over the telemetry link. - + diff --git a/shared/uavobjectdefinition/pipxsettings.xml b/shared/uavobjectdefinition/pipxsettings.xml index a8899ff47..52524aa22 100644 --- a/shared/uavobjectdefinition/pipxsettings.xml +++ b/shared/uavobjectdefinition/pipxsettings.xml @@ -2,9 +2,9 @@ PipXtreme configurations options. - + - +