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

OP-1122 uncrustified gcs and flight

Browse Source
This commit is contained in:
Philippe Renon 2014-01-14 22:46:01 +01:00
parent 1fca85784c
commit d43b220dc0
21 changed files with 248 additions and 250 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

13
flight/modules/PathPlanner/pathplanner.c
View File

@ -265,8 +265,9 @@ static uint8_t checkPathPlan()
uint16_t actionCount; uint16_t actionCount;
uint8_t pathCrc; uint8_t pathCrc;
PathPlanData pathPlan; PathPlanData pathPlan;
//WaypointData waypoint; // using global instead (?)
//PathActionData action; // using global instead (?) // WaypointData waypoint; // using global instead (?)
// PathActionData action; // using global instead (?)
PathPlanGet(&pathPlan); PathPlanGet(&pathPlan);
@ -275,15 +276,15 @@ static uint8_t checkPathPlan()
// an empty path plan is invalid // an empty path plan is invalid
return false; return false;
} }
actionCount = pathPlan.PathActionCount; actionCount = pathPlan.PathActionCount;
// check count consistency // check count consistency
if (waypointCount > UAVObjGetNumInstances(WaypointHandle())) { if (waypointCount > UAVObjGetNumInstances(WaypointHandle())) {
//PIOS_DEBUGLOG_Printf("PathPlan : waypoint count error!"); // PIOS_DEBUGLOG_Printf("PathPlan : waypoint count error!");
return false; return false;
} }
if (actionCount > UAVObjGetNumInstances(PathActionHandle())) { if (actionCount > UAVObjGetNumInstances(PathActionHandle())) {
//PIOS_DEBUGLOG_Printf("PathPlan : path action count error!"); // PIOS_DEBUGLOG_Printf("PathPlan : path action count error!");
return false; return false;
} }
@ -297,7 +298,7 @@ static uint8_t checkPathPlan()
} }
if (pathCrc != pathPlan.Crc) { if (pathCrc != pathPlan.Crc) {
// failed crc check // failed crc check
//PIOS_DEBUGLOG_Printf("PathPlan : bad CRC (%d / %d)!", pathCrc, pathPlan.Crc); // PIOS_DEBUGLOG_Printf("PathPlan : bad CRC (%d / %d)!", pathCrc, pathPlan.Crc);
return false; return false;
} }

56
flight/modules/RadioComBridge/RadioComBridge.c
View File

@ -1,5 +1,6 @@
/** /**
****************************************************************************** ******************************************************************************
* @addtogroup OpenPilotModules OpenPilot Modules * @addtogroup OpenPilotModules OpenPilot Modules
* @{ * @{
* @addtogroup RadioComBridgeModule Com Port to Radio Bridge Module * @addtogroup RadioComBridgeModule Com Port to Radio Bridge Module
@ -133,9 +134,9 @@ static int32_t RadioComBridgeStart(void)
data->isCoordinator = (oplinkSettings.Coordinator == OPLINKSETTINGS_COORDINATOR_TRUE); data->isCoordinator = (oplinkSettings.Coordinator == OPLINKSETTINGS_COORDINATOR_TRUE);
// We will not parse/send UAVTalk if any ports are configured as Serial (except for over the USB HID port). // 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) && data->parseUAVTalk = ((oplinkSettings.MainPort != OPLINKSETTINGS_MAINPORT_SERIAL) &&
(oplinkSettings.FlexiPort != OPLINKSETTINGS_FLEXIPORT_SERIAL) && (oplinkSettings.FlexiPort != OPLINKSETTINGS_FLEXIPORT_SERIAL) &&
(oplinkSettings.VCPPort != OPLINKSETTINGS_VCPPORT_SERIAL)); (oplinkSettings.VCPPort != OPLINKSETTINGS_VCPPORT_SERIAL));
// Set the maximum radio RF power. // Set the maximum radio RF power.
switch (oplinkSettings.MaxRFPower) { switch (oplinkSettings.MaxRFPower) {
@ -236,20 +237,20 @@ static int32_t RadioComBridgeInitialize(void)
RadioComBridgeStatsInitialize(); RadioComBridgeStatsInitialize();
// Initialise UAVTalk // Initialise UAVTalk
data->telemUAVTalkCon = UAVTalkInitialize(&UAVTalkSendHandler); data->telemUAVTalkCon = UAVTalkInitialize(&UAVTalkSendHandler);
data->radioUAVTalkCon = UAVTalkInitialize(&RadioSendHandler); data->radioUAVTalkCon = UAVTalkInitialize(&RadioSendHandler);
// Initialize the queues. // Initialize the queues.
data->uavtalkEventQueue = xQueueCreate(EVENT_QUEUE_SIZE, sizeof(UAVObjEvent)); data->uavtalkEventQueue = xQueueCreate(EVENT_QUEUE_SIZE, sizeof(UAVObjEvent));
data->radioEventQueue = xQueueCreate(EVENT_QUEUE_SIZE, sizeof(UAVObjEvent)); data->radioEventQueue = xQueueCreate(EVENT_QUEUE_SIZE, sizeof(UAVObjEvent));
// Initialize the statistics. // Initialize the statistics.
data->telemetryTxRetries = 0; data->telemetryTxRetries = 0;
data->radioTxRetries = 0; data->radioTxRetries = 0;
data->parseUAVTalk = true; data->parseUAVTalk = true;
data->comSpeed = OPLINKSETTINGS_COMSPEED_9600; data->comSpeed = OPLINKSETTINGS_COMSPEED_9600;
PIOS_COM_RADIO = PIOS_COM_RFM22B; PIOS_COM_RADIO = PIOS_COM_RFM22B;
return 0; return 0;
} }
@ -264,13 +265,14 @@ static void registerObject(UAVObjHandle obj)
{ {
// Setup object for periodic updates // Setup object for periodic updates
UAVObjEvent ev = { UAVObjEvent ev = {
.obj = obj, .obj = obj,
.instId = UAVOBJ_ALL_INSTANCES, .instId = UAVOBJ_ALL_INSTANCES,
.event = EV_UPDATED_PERIODIC, .event = EV_UPDATED_PERIODIC,
}; };
// Get metadata // Get metadata
UAVObjMetadata metadata; UAVObjMetadata metadata;
UAVObjGetMetadata(obj, &metadata); UAVObjGetMetadata(obj, &metadata);
EventPeriodicQueueCreate(&ev, data->uavtalkEventQueue, metadata.telemetryUpdatePeriod); EventPeriodicQueueCreate(&ev, data->uavtalkEventQueue, metadata.telemetryUpdatePeriod);
@ -298,23 +300,23 @@ static void updateRadioComBridgeStats()
RadioComBridgeStatsGet(&radioComBridgeStats); RadioComBridgeStatsGet(&radioComBridgeStats);
// Update stats object // Update stats object
radioComBridgeStats.TelemetryTxBytes += telemetryUAVTalkStats.txBytes; radioComBridgeStats.TelemetryTxBytes += telemetryUAVTalkStats.txBytes;
radioComBridgeStats.TelemetryTxFailures += telemetryUAVTalkStats.txErrors; radioComBridgeStats.TelemetryTxFailures += telemetryUAVTalkStats.txErrors;
radioComBridgeStats.TelemetryTxRetries += data->telemetryTxRetries; radioComBridgeStats.TelemetryTxRetries += data->telemetryTxRetries;
radioComBridgeStats.TelemetryRxBytes += telemetryUAVTalkStats.rxBytes; radioComBridgeStats.TelemetryRxBytes += telemetryUAVTalkStats.rxBytes;
radioComBridgeStats.TelemetryRxFailures += telemetryUAVTalkStats.rxErrors; radioComBridgeStats.TelemetryRxFailures += telemetryUAVTalkStats.rxErrors;
radioComBridgeStats.TelemetryRxSyncErrors += telemetryUAVTalkStats.rxSyncErrors; radioComBridgeStats.TelemetryRxSyncErrors += telemetryUAVTalkStats.rxSyncErrors;
radioComBridgeStats.TelemetryRxCrcErrors += telemetryUAVTalkStats.rxCrcErrors; radioComBridgeStats.TelemetryRxCrcErrors += telemetryUAVTalkStats.rxCrcErrors;
radioComBridgeStats.RadioTxBytes += radioUAVTalkStats.txBytes; radioComBridgeStats.RadioTxBytes += radioUAVTalkStats.txBytes;
radioComBridgeStats.RadioTxFailures += radioUAVTalkStats.txErrors; radioComBridgeStats.RadioTxFailures += radioUAVTalkStats.txErrors;
radioComBridgeStats.RadioTxRetries += data->radioTxRetries; radioComBridgeStats.RadioTxRetries += data->radioTxRetries;
radioComBridgeStats.RadioRxBytes += radioUAVTalkStats.rxBytes; radioComBridgeStats.RadioRxBytes += radioUAVTalkStats.rxBytes;
radioComBridgeStats.RadioRxFailures += radioUAVTalkStats.rxErrors; radioComBridgeStats.RadioRxFailures += radioUAVTalkStats.rxErrors;
radioComBridgeStats.RadioRxSyncErrors += radioUAVTalkStats.rxSyncErrors; radioComBridgeStats.RadioRxSyncErrors += radioUAVTalkStats.rxSyncErrors;
radioComBridgeStats.RadioRxCrcErrors += radioUAVTalkStats.rxCrcErrors; radioComBridgeStats.RadioRxCrcErrors += radioUAVTalkStats.rxCrcErrors;
// Update stats object data // Update stats object data
RadioComBridgeStatsSet(&radioComBridgeStats); RadioComBridgeStatsSet(&radioComBridgeStats);

54
flight/modules/Telemetry/telemetry.c
View File

@ -182,16 +182,6 @@ static void registerObject(UAVObjHandle obj)
UAVObjConnectQueue(obj, priorityQueue, EV_MASK_ALL_UPDATES); UAVObjConnectQueue(obj, priorityQueue, EV_MASK_ALL_UPDATES);
} else { } else {
// Setup object for periodic updates // Setup object for periodic updates
// UAVObjEvent ev = {
// .obj = obj,
// .instId = UAVOBJ_ALL_INSTANCES,
// .event = EV_UPDATED_PERIODIC,
// };
// EventPeriodicQueueCreate(&ev, queue, 0);
// ev.event = EV_LOGGING_PERIODIC;
// EventPeriodicQueueCreate(&ev, queue, 0);
// Setup object for telemetry updates
updateObject(obj, EV_NONE); updateObject(obj, EV_NONE);
} }
} }
@ -312,8 +302,8 @@ static void processObjEvent(UAVObjEvent *ev)
retries = 0; retries = 0;
success = -1; success = -1;
if ((ev->event == EV_UPDATED && (updateMode == UPDATEMODE_ONCHANGE || updateMode == UPDATEMODE_THROTTLED)) if ((ev->event == EV_UPDATED && (updateMode == UPDATEMODE_ONCHANGE || updateMode == UPDATEMODE_THROTTLED))
|| ev->event == EV_UPDATED_MANUAL || ev->event == EV_UPDATED_MANUAL
|| (ev->event == EV_UPDATED_PERIODIC && updateMode != UPDATEMODE_THROTTLED)) { || (ev->event == EV_UPDATED_PERIODIC && updateMode != UPDATEMODE_THROTTLED)) {
// Send update to GCS (with retries) // Send update to GCS (with retries)
while (retries < MAX_RETRIES && success == -1) { while (retries < MAX_RETRIES && success == -1) {
// call blocks until ack is received or timeout // call blocks until ack is received or timeout
@ -357,8 +347,8 @@ static void processObjEvent(UAVObjEvent *ev)
if (ev->obj) { if (ev->obj) {
updateMode = UAVObjGetLoggingUpdateMode(&metadata); updateMode = UAVObjGetLoggingUpdateMode(&metadata);
if ((ev->event == EV_UPDATED && (updateMode == UPDATEMODE_ONCHANGE || updateMode == UPDATEMODE_THROTTLED)) if ((ev->event == EV_UPDATED && (updateMode == UPDATEMODE_ONCHANGE || updateMode == UPDATEMODE_THROTTLED))
|| ev->event == EV_LOGGING_MANUAL || ev->event == EV_LOGGING_MANUAL
|| (ev->event == EV_LOGGING_PERIODIC && updateMode != UPDATEMODE_THROTTLED)) { || (ev->event == EV_LOGGING_PERIODIC && updateMode != UPDATEMODE_THROTTLED)) {
if (ev->instId == UAVOBJ_ALL_INSTANCES) { if (ev->instId == UAVOBJ_ALL_INSTANCES) {
success = UAVObjGetNumInstances(ev->obj); success = UAVObjGetNumInstances(ev->obj);
for (retries = 0; retries < success; retries++) { for (retries = 0; retries < success; retries++) {
@ -572,33 +562,33 @@ static void updateTelemetryStats()
// Update stats object // Update stats object
if (flightStats.Status == FLIGHTTELEMETRYSTATS_STATUS_CONNECTED) { if (flightStats.Status == FLIGHTTELEMETRYSTATS_STATUS_CONNECTED) {
flightStats.TxDataRate = (float)utalkStats.txBytes / ((float)STATS_UPDATE_PERIOD_MS / 1000.0f); flightStats.TxDataRate = (float)utalkStats.txBytes / ((float)STATS_UPDATE_PERIOD_MS / 1000.0f);
flightStats.TxBytes += utalkStats.txBytes; flightStats.TxBytes += utalkStats.txBytes;
flightStats.TxFailures += txErrors; flightStats.TxFailures += txErrors;
flightStats.TxRetries += txRetries; flightStats.TxRetries += txRetries;
flightStats.RxDataRate = (float)utalkStats.rxBytes / ((float)STATS_UPDATE_PERIOD_MS / 1000.0f); flightStats.RxDataRate = (float)utalkStats.rxBytes / ((float)STATS_UPDATE_PERIOD_MS / 1000.0f);
flightStats.RxBytes += utalkStats.rxBytes; flightStats.RxBytes += utalkStats.rxBytes;
flightStats.RxFailures += utalkStats.rxErrors; flightStats.RxFailures += utalkStats.rxErrors;
flightStats.RxSyncErrors += utalkStats.rxSyncErrors; flightStats.RxSyncErrors += utalkStats.rxSyncErrors;
flightStats.RxCrcErrors += utalkStats.rxCrcErrors; flightStats.RxCrcErrors += utalkStats.rxCrcErrors;
} else { } else {
flightStats.TxDataRate = 0; flightStats.TxDataRate = 0;
flightStats.TxBytes = 0; flightStats.TxBytes = 0;
flightStats.TxFailures = 0; flightStats.TxFailures = 0;
flightStats.TxRetries = 0; flightStats.TxRetries = 0;
flightStats.RxDataRate = 0; flightStats.RxDataRate = 0;
flightStats.RxBytes = 0; flightStats.RxBytes = 0;
flightStats.RxFailures = 0; flightStats.RxFailures = 0;
flightStats.RxSyncErrors = 0; flightStats.RxSyncErrors = 0;
flightStats.RxCrcErrors = 0; flightStats.RxCrcErrors = 0;
} }
txErrors = 0; txErrors = 0;
txRetries = 0; txRetries = 0;
// Check for connection timeout // Check for connection timeout
timeNow = xTaskGetTickCount() * portTICK_RATE_MS; timeNow = xTaskGetTickCount() * portTICK_RATE_MS;
if (utalkStats.rxObjects > 0) { if (utalkStats.rxObjects > 0) {
timeOfLastObjectUpdate = timeNow; timeOfLastObjectUpdate = timeNow;
} }

2
flight/uavobjects/uavobjectmanager.c
View File

@ -732,7 +732,7 @@ uint8_t UAVObjUpdateCRC(UAVObjHandle obj_handle, uint16_t instId, uint8_t crc)
goto unlock_exit; goto unlock_exit;
} }
// Update crc // Update crc
crc = PIOS_CRC_updateCRC(crc, (uint8_t *) InstanceData(instEntry), (int32_t) obj->instance_size); crc = PIOS_CRC_updateCRC(crc, (uint8_t *)InstanceData(instEntry), (int32_t)obj->instance_size);
} }
unlock_exit: unlock_exit:

1
flight/uavtalk/inc/uavtalk.h
View File

@ -44,7 +44,6 @@ typedef struct {
uint32_t rxErrors; uint32_t rxErrors;
uint32_t rxSyncErrors; uint32_t rxSyncErrors;
uint32_t rxCrcErrors; uint32_t rxCrcErrors;
} UAVTalkStats; } UAVTalkStats;
typedef void *UAVTalkConnection; typedef void *UAVTalkConnection;

14
flight/uavtalk/inc/uavtalk_priv.h
View File

@ -34,10 +34,10 @@
// Private types and constants // Private types and constants
// min header : sync(1), type (1), size(2), object ID(4), instance ID(2) // min header : sync(1), type (1), size(2), object ID(4), instance ID(2)
#define UAVTALK_MIN_HEADER_LENGTH 10 #define UAVTALK_MIN_HEADER_LENGTH 10
// max header : sync(1), type (1), size(2), object ID(4), instance ID(2), timestamp(2) // max header : sync(1), type (1), size(2), object ID(4), instance ID(2), timestamp(2)
#define UAVTALK_MAX_HEADER_LENGTH 12 #define UAVTALK_MAX_HEADER_LENGTH 12
#define UAVTALK_CHECKSUM_LENGTH 1 #define UAVTALK_CHECKSUM_LENGTH 1
@ -47,11 +47,11 @@
#define UAVTALK_MAX_PACKET_LENGTH UAVTALK_MIN_PACKET_LENGTH + UAVTALK_MAX_PAYLOAD_LENGTH #define UAVTALK_MAX_PACKET_LENGTH UAVTALK_MIN_PACKET_LENGTH + UAVTALK_MAX_PAYLOAD_LENGTH
typedef struct { typedef struct {
uint8_t type; uint8_t type;
uint16_t packet_size; uint16_t packet_size;
uint32_t objId; uint32_t objId;
uint16_t instId; uint16_t instId;
uint32_t length; uint32_t length;
uint8_t timestampLength; uint8_t timestampLength;
uint8_t cs; uint8_t cs;
uint16_t timestamp; uint16_t timestamp;

49
flight/uavtalk/uavtalk.c
View File

@ -32,13 +32,13 @@
#include "openpilot.h" #include "openpilot.h"
#include "uavtalk_priv.h" #include "uavtalk_priv.h"
//#define UAV_DEBUGLOG 1 // #define UAV_DEBUGLOG 1
#if defined UAV_DEBUGLOG && defined FLASH_FREERTOS #if defined UAV_DEBUGLOG && defined FLASH_FREERTOS
#define UAVT_DEBUGLOG_PRINTF(...) PIOS_DEBUGLOG_Printf(__VA_ARGS__) #define UAVT_DEBUGLOG_PRINTF(...) PIOS_DEBUGLOG_Printf(__VA_ARGS__)
// uncomment and adapt the following lines to filter verbose logging to include specific object(s) only // uncomment and adapt the following lines to filter verbose logging to include specific object(s) only
//#include "flighttelemetrystats.h" // #include "flighttelemetrystats.h"
//#define UAVT_DEBUGLOG_CPRINTF(objId, ...) if (objId == FLIGHTTELEMETRYSTATS_OBJID) { UAVT_DEBUGLOG_PRINTF(__VA_ARGS__); } // #define UAVT_DEBUGLOG_CPRINTF(objId, ...) if (objId == FLIGHTTELEMETRYSTATS_OBJID) { UAVT_DEBUGLOG_PRINTF(__VA_ARGS__); }
#endif #endif
#ifndef UAVT_DEBUGLOG_PRINTF #ifndef UAVT_DEBUGLOG_PRINTF
#define UAVT_DEBUGLOG_PRINTF(...) #define UAVT_DEBUGLOG_PRINTF(...)
@ -296,6 +296,7 @@ static int32_t objectTransaction(UAVTalkConnectionData *connection, uint8_t type
{ {
int32_t respReceived; int32_t respReceived;
int32_t ret = -1; int32_t ret = -1;
// Send object depending on if a response is needed // Send object depending on if a response is needed
if (type == UAVTALK_TYPE_OBJ_ACK || type == UAVTALK_TYPE_OBJ_ACK_TS || type == UAVTALK_TYPE_OBJ_REQ) { if (type == UAVTALK_TYPE_OBJ_ACK || type == UAVTALK_TYPE_OBJ_ACK_TS || type == UAVTALK_TYPE_OBJ_REQ) {
// Get transaction lock (will block if a transaction is pending) // Get transaction lock (will block if a transaction is pending)
@ -304,7 +305,7 @@ static int32_t objectTransaction(UAVTalkConnectionData *connection, uint8_t type
xSemaphoreTakeRecursive(connection->lock, portMAX_DELAY); xSemaphoreTakeRecursive(connection->lock, portMAX_DELAY);
// expected response type // expected response type
connection->respType = (type == UAVTALK_TYPE_OBJ_REQ) ? UAVTALK_TYPE_OBJ : UAVTALK_TYPE_ACK; connection->respType = (type == UAVTALK_TYPE_OBJ_REQ) ? UAVTALK_TYPE_OBJ : UAVTALK_TYPE_ACK;
connection->respObjId = UAVObjGetID(obj); connection->respObjId = UAVObjGetID(obj);
connection->respInstId = instId; connection->respInstId = instId;
ret = sendObject(connection, type, UAVObjGetID(obj), instId, obj); ret = sendObject(connection, type, UAVObjGetID(obj), instId, obj);
xSemaphoreGiveRecursive(connection->lock); xSemaphoreGiveRecursive(connection->lock);
@ -376,8 +377,8 @@ UAVTalkRxState UAVTalkProcessInputStreamQuiet(UAVTalkConnection connectionHandle
iproc->rxPacketLength = 1; iproc->rxPacketLength = 1;
iproc->rxCount = 0; iproc->rxCount = 0;
iproc->type = 0; iproc->type = 0;
iproc->state = UAVTALK_STATE_TYPE; iproc->state = UAVTALK_STATE_TYPE;
break; break;
case UAVTALK_STATE_TYPE: case UAVTALK_STATE_TYPE:
@ -389,9 +390,9 @@ UAVTalkRxState UAVTalkProcessInputStreamQuiet(UAVTalkConnection connectionHandle
} }
// update the CRC // update the CRC
iproc->cs = PIOS_CRC_updateByte(iproc->cs, rxbyte); iproc->cs = PIOS_CRC_updateByte(iproc->cs, rxbyte);
iproc->type = rxbyte; iproc->type = rxbyte;
iproc->packet_size = 0; iproc->packet_size = 0;
iproc->state = UAVTALK_STATE_SIZE; iproc->state = UAVTALK_STATE_SIZE;
@ -408,7 +409,7 @@ UAVTalkRxState UAVTalkProcessInputStreamQuiet(UAVTalkConnection connectionHandle
break; break;
} }
iproc->packet_size += rxbyte << 8; iproc->packet_size += rxbyte << 8;
iproc->rxCount = 0; iproc->rxCount = 0;
if (iproc->packet_size < UAVTALK_MIN_HEADER_LENGTH || iproc->packet_size > UAVTALK_MAX_HEADER_LENGTH + UAVTALK_MAX_PAYLOAD_LENGTH) { if (iproc->packet_size < UAVTALK_MIN_HEADER_LENGTH || iproc->packet_size > UAVTALK_MAX_HEADER_LENGTH + UAVTALK_MAX_PAYLOAD_LENGTH) {
// incorrect packet size // incorrect packet size
@ -417,8 +418,8 @@ UAVTalkRxState UAVTalkProcessInputStreamQuiet(UAVTalkConnection connectionHandle
break; break;
} }
iproc->objId = 0; iproc->objId = 0;
iproc->state = UAVTALK_STATE_OBJID; iproc->state = UAVTALK_STATE_OBJID;
break; break;
case UAVTALK_STATE_OBJID: case UAVTALK_STATE_OBJID:
@ -432,8 +433,8 @@ UAVTalkRxState UAVTalkProcessInputStreamQuiet(UAVTalkConnection connectionHandle
} }
iproc->rxCount = 0; iproc->rxCount = 0;
iproc->instId = 0; iproc->instId = 0;
iproc->state = UAVTALK_STATE_INSTID; iproc->state = UAVTALK_STATE_INSTID;
break; break;
case UAVTALK_STATE_INSTID: case UAVTALK_STATE_INSTID:
@ -482,7 +483,7 @@ UAVTalkRxState UAVTalkProcessInputStreamQuiet(UAVTalkConnection connectionHandle
if (iproc->type & UAVTALK_TIMESTAMPED) { if (iproc->type & UAVTALK_TIMESTAMPED) {
// If there is a timestamp get it // If there is a timestamp get it
iproc->timestamp = 0; iproc->timestamp = 0;
iproc->state = UAVTALK_STATE_TIMESTAMP; iproc->state = UAVTALK_STATE_TIMESTAMP;
} else { } else {
// If there is a payload get it, otherwise receive checksum // If there is a payload get it, otherwise receive checksum
if (iproc->length > 0) { if (iproc->length > 0) {
@ -598,6 +599,7 @@ UAVTalkRxState UAVTalkRelayPacket(UAVTalkConnection inConnectionHandle, UAVTalkC
// The input packet must be completely parsed. // The input packet must be completely parsed.
if (inIproc->state != UAVTALK_STATE_COMPLETE) { if (inIproc->state != UAVTALK_STATE_COMPLETE) {
inConnection->stats.rxErrors++; inConnection->stats.rxErrors++;
return -1; return -1;
} }
@ -606,6 +608,7 @@ UAVTalkRxState UAVTalkRelayPacket(UAVTalkConnection inConnectionHandle, UAVTalkC
if (!outConnection->outStream) { if (!outConnection->outStream) {
outConnection->stats.txErrors++; outConnection->stats.txErrors++;
return -1; return -1;
} }
@ -654,7 +657,7 @@ UAVTalkRxState UAVTalkRelayPacket(UAVTalkConnection inConnectionHandle, UAVTalkC
// evaluate return value before releasing the lock // evaluate return value before releasing the lock
UAVTalkRxState rxState = 0; UAVTalkRxState rxState = 0;
if (rc != (int32_t) (headerLength + inIproc->length + UAVTALK_CHECKSUM_LENGTH)) { if (rc != (int32_t)(headerLength + inIproc->length + UAVTALK_CHECKSUM_LENGTH)) {
outConnection->stats.txErrors++; outConnection->stats.txErrors++;
rxState = -1; rxState = -1;
} }
@ -718,7 +721,8 @@ uint32_t UAVTalkGetPacketObjId(UAVTalkConnection connectionHandle)
* \return 0 Success * \return 0 Success
* \return -1 Failure * \return -1 Failure
*/ */
static int32_t receiveObject(UAVTalkConnectionData *connection, uint8_t type, uint32_t objId, uint16_t instId, uint8_t *data) { static int32_t receiveObject(UAVTalkConnectionData *connection, uint8_t type, uint32_t objId, uint16_t instId, uint8_t *data)
{
UAVObjHandle obj; UAVObjHandle obj;
int32_t ret = 0; int32_t ret = 0;
@ -744,8 +748,7 @@ static int32_t receiveObject(UAVTalkConnectionData *connection, uint8_t type, ui
// any OBJ message can ack a pending OBJ_REQ message // any OBJ message can ack a pending OBJ_REQ message
// even one that was not sent in response to the OBJ_REQ message // even one that was not sent in response to the OBJ_REQ message
updateAck(connection, type, objId, instId); updateAck(connection, type, objId, instId);
} } else {
else {
ret = -1; ret = -1;
} }
} else { } else {
@ -838,8 +841,7 @@ static void updateAck(UAVTalkConnectionData *connection, uint8_t type, uint32_t
// last instance received, complete transaction // last instance received, complete transaction
xSemaphoreGive(connection->respSema); xSemaphoreGive(connection->respSema);
connection->respObjId = 0; connection->respObjId = 0;
} } else if (connection->respInstId == instId) {
else if (connection->respInstId == instId) {
xSemaphoreGive(connection->respSema); xSemaphoreGive(connection->respSema);
connection->respObjId = 0; connection->respObjId = 0;
} }
@ -876,7 +878,7 @@ static int32_t sendObject(UAVTalkConnectionData *connection, uint8_t type, uint3
numInst = UAVObjGetNumInstances(obj); numInst = UAVObjGetNumInstances(obj);
// Send all instances in reverse order // Send all instances in reverse order
// This allows the receiver to detect when the last object has been received (i.e. when instance 0 is received) // This allows the receiver to detect when the last object has been received (i.e. when instance 0 is received)
ret = 0; ret = 0;
for (n = 0; n < numInst; ++n) { for (n = 0; n < numInst; ++n) {
if (sendSingleObject(connection, type, objId, numInst - n - 1, obj) == -1) { if (sendSingleObject(connection, type, objId, numInst - n - 1, obj) == -1) {
ret = -1; ret = -1;
@ -903,7 +905,7 @@ static int32_t sendObject(UAVTalkConnectionData *connection, uint8_t type, uint3
* \param[in] type Transaction type * \param[in] type Transaction type
* \param[in] objId The object ID * \param[in] objId The object ID
* \param[in] instId The instance ID (can NOT be UAVOBJ_ALL_INSTANCES, use * \param[in] instId The instance ID (can NOT be UAVOBJ_ALL_INSTANCES, use
() instead) () instead)
* \param[in] obj Object handle to send (null when type is NACK) * \param[in] obj Object handle to send (null when type is NACK)
* \return 0 Success * \return 0 Success
* \return -1 Failure * \return -1 Failure
@ -978,8 +980,7 @@ static int32_t sendSingleObject(UAVTalkConnectionData *connection, uint8_t type,
++connection->stats.txObjects; ++connection->stats.txObjects;
connection->stats.txObjectBytes += length; connection->stats.txObjectBytes += length;
connection->stats.txBytes += tx_msg_len; connection->stats.txBytes += tx_msg_len;
} } else {
else {
connection->stats.txErrors++; connection->stats.txErrors++;
// TDOD rc == -1 connection not open, -2 buffer full should retry // TDOD rc == -1 connection not open, -2 buffer full should retry
connection->stats.txBytes += (rc > 0) ? rc : 0; connection->stats.txBytes += (rc > 0) ? rc : 0;

4
ground/openpilotgcs/src/libs/utils/crc.h
View File

@ -32,9 +32,7 @@
#include "utils_global.h" #include "utils_global.h"
namespace Utils { namespace Utils {
class QTCREATOR_UTILS_EXPORT Crc { class QTCREATOR_UTILS_EXPORT Crc {
public: public:
/** /**
* Update the crc value with new data. * Update the crc value with new data.
@ -54,9 +52,7 @@ public:
* \return The updated crc value. * \return The updated crc value.
*/ */
static quint8 updateCRC(quint8 crc, const quint8 *data, qint32 length); static quint8 updateCRC(quint8 crc, const quint8 *data, qint32 length);
}; };
} // namespace Utils } // namespace Utils
#endif // CRC_H #endif // CRC_H

1
ground/openpilotgcs/src/plugins/notify/notifyplugin.cpp
View File

@ -128,6 +128,7 @@ void SoundNotifyPlugin::readConfig(QSettings *settings, UAVConfigInfo * /* confi
void SoundNotifyPlugin::onTelemetryManagerAdded(QObject *obj) void SoundNotifyPlugin::onTelemetryManagerAdded(QObject *obj)
{ {
TelemetryManager *telMngr = qobject_cast<TelemetryManager *>(obj); TelemetryManager *telMngr = qobject_cast<TelemetryManager *>(obj);
if (telMngr) { if (telMngr) {
connect(telMngr, SIGNAL(disconnected()), this, SLOT(onAutopilotDisconnect())); connect(telMngr, SIGNAL(disconnected()), this, SLOT(onAutopilotDisconnect()));
} }

8
ground/openpilotgcs/src/plugins/opmap/flightdatamodel.cpp
View File

@ -30,8 +30,7 @@
#include <QDomDocument> #include <QDomDocument>
flightDataModel::flightDataModel(QObject *parent) : QAbstractTableModel(parent) flightDataModel::flightDataModel(QObject *parent) : QAbstractTableModel(parent)
{ {}
}
int flightDataModel::rowCount(const QModelIndex & /*parent*/) const int flightDataModel::rowCount(const QModelIndex & /*parent*/) const
{ {
@ -75,6 +74,7 @@ QVariant flightDataModel::data(const QModelIndex &index, int role) const
bool flightDataModel::setColumnByIndex(pathPlanData *row, const int index, const QVariant value) bool flightDataModel::setColumnByIndex(pathPlanData *row, const int index, const QVariant value)
{ {
bool b; bool b;
switch (index) { switch (index) {
case WPDESCRITPTION: case WPDESCRITPTION:
row->wpDescritption = value.toString(); row->wpDescritption = value.toString();
@ -178,6 +178,7 @@ bool flightDataModel::setColumnByIndex(pathPlanData *row, const int index, const
QVariant flightDataModel::getColumnByIndex(const pathPlanData *row, const int index) const QVariant flightDataModel::getColumnByIndex(const pathPlanData *row, const int index) const
{ {
QVariant value; QVariant value;
switch (index) { switch (index) {
case WPDESCRITPTION: case WPDESCRITPTION:
value = row->wpDescritption; value = row->wpDescritption;
@ -255,6 +256,7 @@ QVariant flightDataModel::getColumnByIndex(const pathPlanData *row, const int in
QVariant flightDataModel::headerData(int section, Qt::Orientation orientation, int role) const QVariant flightDataModel::headerData(int section, Qt::Orientation orientation, int role) const
{ {
QVariant value; QVariant value;
if (role == Qt::DisplayRole) { if (role == Qt::DisplayRole) {
if (orientation == Qt::Vertical) { if (orientation == Qt::Vertical) {
value = QString::number(section + 1); value = QString::number(section + 1);
@ -605,7 +607,7 @@ void flightDataModel::readFromFile(QString fileName)
while (!fieldNode.isNull()) { while (!fieldNode.isNull()) {
QDomElement field = fieldNode.toElement(); QDomElement field = fieldNode.toElement();
if (field.tagName() == "field") { if (field.tagName() == "field") {
QString name = field.attribute("name"); QString name = field.attribute("name");
QString value = field.attribute("value"); QString value = field.attribute("value");
if (name == "altitude") { if (name == "altitude") {
data->altitude = value.toDouble(); data->altitude = value.toDouble();

129
ground/openpilotgcs/src/plugins/opmap/modeluavoproxy.cpp
View File

@ -32,13 +32,14 @@
ModelUavoProxy::ModelUavoProxy(QObject *parent, flightDataModel *model) : QObject(parent), myModel(model) ModelUavoProxy::ModelUavoProxy(QObject *parent, flightDataModel *model) : QObject(parent), myModel(model)
{ {
ExtensionSystem::PluginManager *pm = ExtensionSystem::PluginManager::instance(); ExtensionSystem::PluginManager *pm = ExtensionSystem::PluginManager::instance();
Q_ASSERT(pm != NULL); Q_ASSERT(pm != NULL);
objMngr = pm->getObject<UAVObjectManager>(); objMngr = pm->getObject<UAVObjectManager>();
Q_ASSERT(objMngr != NULL); Q_ASSERT(objMngr != NULL);
completionCountdown = 0; completionCountdown = 0;
successCountdown = 0; successCountdown = 0;
} }
void ModelUavoProxy::sendPathPlan() void ModelUavoProxy::sendPathPlan()
@ -59,7 +60,7 @@ void ModelUavoProxy::sendPathPlan()
// we will start 3 update all // we will start 3 update all
completionCountdown = 3; completionCountdown = 3;
successCountdown = completionCountdown; successCountdown = completionCountdown;
pathPlan->updated(); pathPlan->updated();
waypoint->updatedAll(); waypoint->updatedAll();
@ -77,8 +78,7 @@ void ModelUavoProxy::pathPlanElementSent(UAVObject *obj, bool success)
qDebug() << "ModelUavoProxy::pathPlanSent - completed" << (successCountdown == 0); qDebug() << "ModelUavoProxy::pathPlanSent - completed" << (successCountdown == 0);
if (successCountdown == 0) { if (successCountdown == 0) {
QMessageBox::information(NULL, tr("Path Plan Upload Successful"), tr("Path plan upload was successful.")); QMessageBox::information(NULL, tr("Path Plan Upload Successful"), tr("Path plan upload was successful."));
} } else {
else {
QMessageBox::critical(NULL, tr("Path Plan Upload Failed"), tr("Failed to upload the path plan !")); QMessageBox::critical(NULL, tr("Path Plan Upload Failed"), tr("Failed to upload the path plan !"));
} }
} }
@ -87,6 +87,7 @@ void ModelUavoProxy::pathPlanElementSent(UAVObject *obj, bool success)
void ModelUavoProxy::receivePathPlan() void ModelUavoProxy::receivePathPlan()
{ {
PathPlan *pathPlan = PathPlan::GetInstance(objMngr, 0); PathPlan *pathPlan = PathPlan::GetInstance(objMngr, 0);
connect(pathPlan, SIGNAL(transactionCompleted(UAVObject *, bool)), this, SLOT(pathPlanElementReceived(UAVObject *, bool))); connect(pathPlan, SIGNAL(transactionCompleted(UAVObject *, bool)), this, SLOT(pathPlanElementReceived(UAVObject *, bool)));
Waypoint *waypoint = Waypoint::GetInstance(objMngr, 0); Waypoint *waypoint = Waypoint::GetInstance(objMngr, 0);
@ -97,7 +98,7 @@ void ModelUavoProxy::receivePathPlan()
// we will start 3 update requests // we will start 3 update requests
completionCountdown = 3; completionCountdown = 3;
successCountdown = completionCountdown; successCountdown = completionCountdown;
pathPlan->requestUpdate(); pathPlan->requestUpdate();
waypoint->requestUpdateAll(); waypoint->requestUpdateAll();
@ -117,8 +118,7 @@ void ModelUavoProxy::pathPlanElementReceived(UAVObject *obj, bool success)
if (objectsToModel()) { if (objectsToModel()) {
QMessageBox::information(NULL, tr("Path Plan Download Successful"), tr("Path plan download was successful.")); QMessageBox::information(NULL, tr("Path Plan Download Successful"), tr("Path plan download was successful."));
} }
} } else {
else {
QMessageBox::critical(NULL, tr("Path Plan Download Failed"), tr("Failed to download the path plan !")); QMessageBox::critical(NULL, tr("Path Plan Download Failed"), tr("Failed to download the path plan !"));
} }
} }
@ -139,12 +139,11 @@ bool ModelUavoProxy::modelToObjects()
qDebug() << "ModelUAVProxy::modelToObjects"; qDebug() << "ModelUAVProxy::modelToObjects";
// track number of path actions // track number of path actions
int actionCount = 0; int actionCount = 0;
// iterate over waypoints // iterate over waypoints
int waypointCount = myModel->rowCount(); int waypointCount = myModel->rowCount();
for (int i = 0; i < waypointCount; ++i) { for (int i = 0; i < waypointCount; ++i) {
// Path Actions // Path Actions
// create action to use as a search criteria // create action to use as a search criteria
@ -168,8 +167,7 @@ bool ModelUavoProxy::modelToObjects()
// update UAVObject // update UAVObject
action->setData(actionData); action->setData(actionData);
} } else {
else {
action->deleteLater(); action->deleteLater();
action = foundAction; action = foundAction;
qDebug() << "ModelUAVProxy::modelToObjects - found action instance :" << action->getInstID(); qDebug() << "ModelUAVProxy::modelToObjects - found action instance :" << action->getInstID();
@ -208,7 +206,7 @@ bool ModelUavoProxy::modelToObjects()
PathPlan *pathPlan = PathPlan::GetInstance(objMngr); PathPlan *pathPlan = PathPlan::GetInstance(objMngr);
PathPlan::DataFields pathPlanData = pathPlan->getData(); PathPlan::DataFields pathPlanData = pathPlan->getData();
pathPlanData.WaypointCount = waypointCount; pathPlanData.WaypointCount = waypointCount;
pathPlanData.PathActionCount = actionCount; pathPlanData.PathActionCount = actionCount;
pathPlanData.Crc = computePathPlanCrc(waypointCount, actionCount); pathPlanData.Crc = computePathPlanCrc(waypointCount, actionCount);
@ -217,9 +215,11 @@ bool ModelUavoProxy::modelToObjects()
return true; return true;
} }
Waypoint *ModelUavoProxy::createWaypoint(int index, Waypoint *newWaypoint) { Waypoint *ModelUavoProxy::createWaypoint(int index, Waypoint *newWaypoint)
{
Waypoint *waypoint = NULL; Waypoint *waypoint = NULL;
int count = objMngr->getNumInstances(Waypoint::OBJID); int count = objMngr->getNumInstances(Waypoint::OBJID);
if (index < count) { if (index < count) {
// reuse object // reuse object
qDebug() << "ModelUAVProxy::createWaypoint - reused waypoint instance :" << index << "/" << count; qDebug() << "ModelUAVProxy::createWaypoint - reused waypoint instance :" << index << "/" << count;
@ -234,17 +234,18 @@ Waypoint *ModelUavoProxy::createWaypoint(int index, Waypoint *newWaypoint) {
waypoint = newWaypoint ? newWaypoint : new Waypoint; waypoint = newWaypoint ? newWaypoint : new Waypoint;
waypoint->initialize(index, waypoint->getMetaObject()); waypoint->initialize(index, waypoint->getMetaObject());
objMngr->registerObject(waypoint); objMngr->registerObject(waypoint);
} } else {
else {
// we can only create the "next" object // we can only create the "next" object
// TODO fail in a clean way :( // TODO fail in a clean way :(
} }
return waypoint; return waypoint;
} }
PathAction *ModelUavoProxy::createPathAction(int index, PathAction *newAction) { PathAction *ModelUavoProxy::createPathAction(int index, PathAction *newAction)
{
PathAction *action = NULL; PathAction *action = NULL;
int count = objMngr->getNumInstances(PathAction::OBJID); int count = objMngr->getNumInstances(PathAction::OBJID);
if (index < count) { if (index < count) {
// reuse object // reuse object
qDebug() << "ModelUAVProxy::createPathAction - reused action instance :" << index << "/" << count; qDebug() << "ModelUAVProxy::createPathAction - reused action instance :" << index << "/" << count;
@ -259,17 +260,18 @@ PathAction *ModelUavoProxy::createPathAction(int index, PathAction *newAction) {
action = newAction ? newAction : new PathAction; action = newAction ? newAction : new PathAction;
action->initialize(index, action->getMetaObject()); action->initialize(index, action->getMetaObject());
objMngr->registerObject(action); objMngr->registerObject(action);
} } else {
else {
// we can only create the "next" object // we can only create the "next" object
// TODO fail in a clean way :( // TODO fail in a clean way :(
} }
return action; return action;
} }
PathAction *ModelUavoProxy::findPathAction(const PathAction::DataFields &actionData, int actionCount) { PathAction *ModelUavoProxy::findPathAction(const PathAction::DataFields &actionData, int actionCount)
{
int instancesCount = objMngr->getNumInstances(PathAction::OBJID); int instancesCount = objMngr->getNumInstances(PathAction::OBJID);
int count = actionCount <= instancesCount ? actionCount : instancesCount; int count = actionCount <= instancesCount ? actionCount : instancesCount;
for (int i = 0; i < count; ++i) { for (int i = 0; i < count; ++i) {
PathAction *action = PathAction::GetInstance(objMngr, i); PathAction *action = PathAction::GetInstance(objMngr, i);
Q_ASSERT(action); Q_ASSERT(action);
@ -278,15 +280,15 @@ PathAction *ModelUavoProxy::findPathAction(const PathAction::DataFields &actionD
} }
PathAction::DataFields fields = action->getData(); PathAction::DataFields fields = action->getData();
if (fields.Command == actionData.Command if (fields.Command == actionData.Command
&& fields.ConditionParameters[0] == actionData.ConditionParameters[0] && fields.ConditionParameters[0] == actionData.ConditionParameters[0]
&& fields.ConditionParameters[1] == actionData.ConditionParameters[1] && fields.ConditionParameters[1] == actionData.ConditionParameters[1]
&& fields.ConditionParameters[2] == actionData.ConditionParameters[2] && fields.ConditionParameters[2] == actionData.ConditionParameters[2]
&& fields.EndCondition == actionData.EndCondition && fields.EndCondition == actionData.EndCondition
&& fields.ErrorDestination == actionData.ErrorDestination && fields.ErrorDestination == actionData.ErrorDestination
&& fields.JumpDestination == actionData.JumpDestination && fields.Mode == actionData.Mode && fields.JumpDestination == actionData.JumpDestination && fields.Mode == actionData.Mode
&& fields.ModeParameters[0] == actionData.ModeParameters[0] && fields.ModeParameters[0] == actionData.ModeParameters[0]
&& fields.ModeParameters[1] == actionData.ModeParameters[1] && fields.ModeParameters[1] == actionData.ModeParameters[1]
&& fields.ModeParameters[2] == actionData.ModeParameters[2]) { && fields.ModeParameters[2] == actionData.ModeParameters[2]) {
return action; return action;
} }
} }
@ -302,8 +304,8 @@ bool ModelUavoProxy::objectsToModel()
PathPlan *pathPlan = PathPlan::GetInstance(objMngr); PathPlan *pathPlan = PathPlan::GetInstance(objMngr);
PathPlan::DataFields pathPlanData = pathPlan->getData(); PathPlan::DataFields pathPlanData = pathPlan->getData();
int waypointCount = pathPlanData.WaypointCount; int waypointCount = pathPlanData.WaypointCount;
int actionCount = pathPlanData.PathActionCount; int actionCount = pathPlanData.PathActionCount;
// consistency check // consistency check
if (waypointCount > objMngr->getNumInstances(Waypoint::OBJID)) { if (waypointCount > objMngr->getNumInstances(Waypoint::OBJID)) {
@ -322,8 +324,7 @@ bool ModelUavoProxy::objectsToModel()
int rowCount = myModel->rowCount(); int rowCount = myModel->rowCount();
if (waypointCount < rowCount) { if (waypointCount < rowCount) {
myModel->removeRows(waypointCount, rowCount - waypointCount); myModel->removeRows(waypointCount, rowCount - waypointCount);
} } else if (waypointCount > rowCount) {
else if (waypointCount > rowCount) {
myModel->insertRows(rowCount, waypointCount - rowCount); myModel->insertRows(rowCount, waypointCount - rowCount);
} }
@ -349,10 +350,12 @@ bool ModelUavoProxy::objectsToModel()
return true; return true;
} }
void ModelUavoProxy::modelToWaypoint(int i, Waypoint::DataFields &data) { void ModelUavoProxy::modelToWaypoint(int i, Waypoint::DataFields &data)
{
double distance, bearing, altitude, velocity; double distance, bearing, altitude, velocity;
QModelIndex index = myModel->index(i, flightDataModel::DISRELATIVE); QModelIndex index = myModel->index(i, flightDataModel::DISRELATIVE);
distance = myModel->data(index).toDouble(); distance = myModel->data(index).toDouble();
index = myModel->index(i, flightDataModel::BEARELATIVE); index = myModel->index(i, flightDataModel::BEARELATIVE);
bearing = myModel->data(index).toDouble(); bearing = myModel->data(index).toDouble();
@ -367,59 +370,64 @@ void ModelUavoProxy::modelToWaypoint(int i, Waypoint::DataFields &data) {
data.Velocity = velocity; data.Velocity = velocity;
} }
void ModelUavoProxy::waypointToModel(int i, Waypoint::DataFields &data) { void ModelUavoProxy::waypointToModel(int i, Waypoint::DataFields &data)
{
double distance = sqrt(data.Position[Waypoint::POSITION_NORTH] * data.Position[Waypoint::POSITION_NORTH] + double distance = sqrt(data.Position[Waypoint::POSITION_NORTH] * data.Position[Waypoint::POSITION_NORTH] +
data.Position[Waypoint::POSITION_EAST] * data.Position[Waypoint::POSITION_EAST]); data.Position[Waypoint::POSITION_EAST] * data.Position[Waypoint::POSITION_EAST]);
double bearing = atan2(data.Position[Waypoint::POSITION_EAST], data.Position[Waypoint::POSITION_NORTH]) * 180 / M_PI; double bearing = atan2(data.Position[Waypoint::POSITION_EAST], data.Position[Waypoint::POSITION_NORTH]) * 180 / M_PI;
if (bearing != bearing) { if (bearing != bearing) {
bearing = 0; bearing = 0;
} }
double altitude = -data.Position[Waypoint::POSITION_DOWN]; double altitude = -data.Position[Waypoint::POSITION_DOWN];
QModelIndex index = myModel->index(i, flightDataModel::VELOCITY); QModelIndex index = myModel->index(i, flightDataModel::VELOCITY);
myModel->setData(index, data.Velocity); myModel->setData(index, data.Velocity);
index = myModel->index(i, flightDataModel::DISRELATIVE); index = myModel->index(i, flightDataModel::DISRELATIVE);
myModel->setData(index, distance); myModel->setData(index, distance);
index = myModel->index(i, flightDataModel::BEARELATIVE); index = myModel->index(i, flightDataModel::BEARELATIVE);
myModel->setData(index, bearing); myModel->setData(index, bearing);
index = myModel->index(i, flightDataModel::ALTITUDERELATIVE); index = myModel->index(i, flightDataModel::ALTITUDERELATIVE);
myModel->setData(index, altitude); myModel->setData(index, altitude);
} }
void ModelUavoProxy::modelToPathAction(int i, PathAction::DataFields &data) { void ModelUavoProxy::modelToPathAction(int i, PathAction::DataFields &data)
{
QModelIndex index = myModel->index(i, flightDataModel::MODE); QModelIndex index = myModel->index(i, flightDataModel::MODE);
data.Mode = myModel->data(index).toInt();
index = myModel->index(i, flightDataModel::MODE_PARAMS0); data.Mode = myModel->data(index).toInt();
index = myModel->index(i, flightDataModel::MODE_PARAMS0);
data.ModeParameters[0] = myModel->data(index).toFloat(); data.ModeParameters[0] = myModel->data(index).toFloat();
index = myModel->index(i, flightDataModel::MODE_PARAMS1); index = myModel->index(i, flightDataModel::MODE_PARAMS1);
data.ModeParameters[1] = myModel->data(index).toFloat(); data.ModeParameters[1] = myModel->data(index).toFloat();
index = myModel->index(i, flightDataModel::MODE_PARAMS2); index = myModel->index(i, flightDataModel::MODE_PARAMS2);
data.ModeParameters[2] = myModel->data(index).toFloat(); data.ModeParameters[2] = myModel->data(index).toFloat();
index = myModel->index(i, flightDataModel::MODE_PARAMS3); index = myModel->index(i, flightDataModel::MODE_PARAMS3);
data.ModeParameters[3] = myModel->data(index).toFloat(); data.ModeParameters[3] = myModel->data(index).toFloat();
index = myModel->index(i, flightDataModel::CONDITION); index = myModel->index(i, flightDataModel::CONDITION);
data.EndCondition = myModel->data(index).toInt(); data.EndCondition = myModel->data(index).toInt();
index = myModel->index(i, flightDataModel::CONDITION_PARAMS0); index = myModel->index(i, flightDataModel::CONDITION_PARAMS0);
data.ConditionParameters[0] = myModel->data(index).toFloat(); data.ConditionParameters[0] = myModel->data(index).toFloat();
index = myModel->index(i, flightDataModel::CONDITION_PARAMS1); index = myModel->index(i, flightDataModel::CONDITION_PARAMS1);
data.ConditionParameters[1] = myModel->data(index).toFloat(); data.ConditionParameters[1] = myModel->data(index).toFloat();
index = myModel->index(i, flightDataModel::CONDITION_PARAMS2); index = myModel->index(i, flightDataModel::CONDITION_PARAMS2);
data.ConditionParameters[2] = myModel->data(index).toFloat(); data.ConditionParameters[2] = myModel->data(index).toFloat();
index = myModel->index(i, flightDataModel::CONDITION_PARAMS3); index = myModel->index(i, flightDataModel::CONDITION_PARAMS3);
data.ConditionParameters[3] = myModel->data(index).toFloat(); data.ConditionParameters[3] = myModel->data(index).toFloat();
index = myModel->index(i, flightDataModel::COMMAND); index = myModel->index(i, flightDataModel::COMMAND);
data.Command = myModel->data(index).toInt(); data.Command = myModel->data(index).toInt();
index = myModel->index(i, flightDataModel::JUMPDESTINATION); index = myModel->index(i, flightDataModel::JUMPDESTINATION);
data.JumpDestination = myModel->data(index).toInt() - 1; data.JumpDestination = myModel->data(index).toInt() - 1;
index = myModel->index(i, flightDataModel::ERRORDESTINATION); index = myModel->index(i, flightDataModel::ERRORDESTINATION);
data.ErrorDestination = myModel->data(index).toInt() - 1; data.ErrorDestination = myModel->data(index).toInt() - 1;
} }
void ModelUavoProxy::pathActionToModel(int i, PathAction::DataFields &data) { void ModelUavoProxy::pathActionToModel(int i, PathAction::DataFields &data)
{
QModelIndex index = myModel->index(i, flightDataModel::ISRELATIVE); QModelIndex index = myModel->index(i, flightDataModel::ISRELATIVE);
myModel->setData(index, true); myModel->setData(index, true);
index = myModel->index(i, flightDataModel::COMMAND); index = myModel->index(i, flightDataModel::COMMAND);
@ -456,16 +464,17 @@ void ModelUavoProxy::pathActionToModel(int i, PathAction::DataFields &data) {
myModel->setData(index, data.ModeParameters[3]); myModel->setData(index, data.ModeParameters[3]);
} }
quint8 ModelUavoProxy::computePathPlanCrc(int waypointCount, int actionCount) { quint8 ModelUavoProxy::computePathPlanCrc(int waypointCount, int actionCount)
{
quint8 crc = 0; quint8 crc = 0;
for (int i = 0; i < waypointCount; ++i) { for (int i = 0; i < waypointCount; ++i) {
Waypoint* waypoint = Waypoint::GetInstance(objMngr, i); Waypoint *waypoint = Waypoint::GetInstance(objMngr, i);
crc = waypoint->updateCRC(crc); crc = waypoint->updateCRC(crc);
} }
for (int i = 0; i < actionCount; ++i) { for (int i = 0; i < actionCount; ++i) {
PathAction* action = PathAction::GetInstance(objMngr, i); PathAction *action = PathAction::GetInstance(objMngr, i);
crc = action->updateCRC(crc); crc = action->updateCRC(crc);
} }
return crc; return crc;
} }

2
ground/openpilotgcs/src/plugins/opmap/modeluavoproxy.h
View File

@ -58,7 +58,7 @@ private:
Waypoint *createWaypoint(int index, Waypoint *newWaypoint); Waypoint *createWaypoint(int index, Waypoint *newWaypoint);
PathAction *createPathAction(int index, PathAction *newAction); PathAction *createPathAction(int index, PathAction *newAction);
PathAction *findPathAction(const PathAction::DataFields& actionFields, int actionCount); PathAction *findPathAction(const PathAction::DataFields & actionFields, int actionCount);
void modelToWaypoint(int i, Waypoint::DataFields &data); void modelToWaypoint(int i, Waypoint::DataFields &data);
void modelToPathAction(int i, PathAction::DataFields &data); void modelToPathAction(int i, PathAction::DataFields &data);

20
ground/openpilotgcs/src/plugins/uavobjects/uavobject.cpp
View File

@ -55,13 +55,13 @@ using namespace Utils;
*/ */
UAVObject::UAVObject(quint32 objID, bool isSingleInst, const QString & name) UAVObject::UAVObject(quint32 objID, bool isSingleInst, const QString & name)
{ {
this->objID = objID; this->objID = objID;
this->instID = 0; this->instID = 0;
this->isSingleInst = isSingleInst; this->isSingleInst = isSingleInst;
this->name = name; this->name = name;
this->data = 0; this->data = 0;
this->numBytes = 0; this->numBytes = 0;
this->mutex = new QMutex(QMutex::Recursive); this->mutex = new QMutex(QMutex::Recursive);
} }
/** /**
@ -215,7 +215,7 @@ void UAVObject::updatedAll()
if (instID == 0) { if (instID == 0) {
emit objectUpdatedManual(this, true); emit objectUpdatedManual(this, true);
// TODO call objectUpdated() for all instances? // TODO call objectUpdated() for all instances?
//emit objectUpdated(this); // emit objectUpdated(this);
} }
} }
@ -287,7 +287,7 @@ UAVObjectField *UAVObject::getField(const QString & name)
} }
// If this point is reached then the field was not found // If this point is reached then the field was not found
qWarning() << "UAVObject::getField Non existant field" << name << "requested." qWarning() << "UAVObject::getField Non existant field" << name << "requested."
<< "This indicates a bug. Make sure you also have null checking for non-debug code."; << "This indicates a bug. Make sure you also have null checking for non-debug code.";
return NULL; return NULL;
} }
@ -334,8 +334,8 @@ quint8 UAVObject::updateCRC(quint8 crc)
{ {
QMutexLocker locker(mutex); QMutexLocker locker(mutex);
//crc = Crc::updateCRC(crc, (quint8 *) &objID, sizeof(objID)); // crc = Crc::updateCRC(crc, (quint8 *) &objID, sizeof(objID));
//crc = Crc::updateCRC(crc, (quint8 *) &instID, sizeof(instID)); // crc = Crc::updateCRC(crc, (quint8 *) &instID, sizeof(instID));
crc = Crc::updateCRC(crc, data, numBytes); crc = Crc::updateCRC(crc, data, numBytes);
return crc; return crc;

1
ground/openpilotgcs/src/plugins/uavobjectutil/uavobjectutilmanager.cpp
View File

@ -257,6 +257,7 @@ int UAVObjectUtilManager::getBoardModel()
FirmwareIAPObj::DataFields firmwareIapData = getFirmwareIap(); FirmwareIAPObj::DataFields firmwareIapData = getFirmwareIap();
int ret = firmwareIapData.BoardType << 8; int ret = firmwareIapData.BoardType << 8;
ret = ret + firmwareIapData.BoardRevision; ret = ret + firmwareIapData.BoardRevision;
return ret; return ret;

1
ground/openpilotgcs/src/plugins/uavobjectwidgetutils/configtaskwidget.cpp
View File

@ -1230,6 +1230,7 @@ void ConfigTaskWidget::loadWidgetLimits(QWidget *widget, UAVObjectField *field,
void ConfigTaskWidget::updateEnableControls() void ConfigTaskWidget::updateEnableControls()
{ {
TelemetryManager *telMngr = pm->getObject<TelemetryManager>(); TelemetryManager *telMngr = pm->getObject<TelemetryManager>();
Q_ASSERT(telMngr); Q_ASSERT(telMngr);
enableControls(telMngr->isConnected()); enableControls(telMngr->isConnected());

33
ground/openpilotgcs/src/plugins/uavtalk/telemetry.cpp
View File

@ -230,6 +230,7 @@ void Telemetry::transactionCompleted(UAVObject *obj, bool success)
{ {
// Lookup the transaction in the transaction map. // Lookup the transaction in the transaction map.
ObjectTransactionInfo *transInfo = findTransaction(obj); ObjectTransactionInfo *transInfo = findTransaction(obj);
if (transInfo) { if (transInfo) {
if (success) { if (success) {
#ifdef VERBOSE_TELEMETRY #ifdef VERBOSE_TELEMETRY
@ -264,17 +265,17 @@ void Telemetry::transactionTimeout(ObjectTransactionInfo *transInfo)
#endif #endif
++txRetries; ++txRetries;
--transInfo->retriesRemaining; --transInfo->retriesRemaining;
// Retry the transaction // Retry the transaction
processObjectTransaction(transInfo); processObjectTransaction(transInfo);
} else { } else {
qWarning().nospace() << "Telemetry - !!! transaction timed out for object " << transInfo->obj->toStringBrief(); qWarning().nospace() << "Telemetry - !!! transaction timed out for object " << transInfo->obj->toStringBrief();
++txErrors; ++txErrors;
// Terminate transaction // Terminate transaction
utalk->cancelTransaction(transInfo->obj); utalk->cancelTransaction(transInfo->obj);
// Remove this transaction as it's complete. // Remove this transaction as it's complete.
UAVObject *obj = transInfo->obj; UAVObject *obj = transInfo->obj;
closeTransaction(transInfo); closeTransaction(transInfo);
@ -294,6 +295,7 @@ void Telemetry::processObjectTransaction(ObjectTransactionInfo *transInfo)
{ {
// Initiate transaction // Initiate transaction
bool sent = false; bool sent = false;
if (transInfo->objRequest) { if (transInfo->objRequest) {
#ifdef VERBOSE_TELEMETRY #ifdef VERBOSE_TELEMETRY
qDebug().nospace() << "Telemetry - sending request for object " << transInfo->obj->toStringBrief() << ", " << (transInfo->allInstances ? "all" : "single") << " " << (transInfo->acked ? "acked" : ""); qDebug().nospace() << "Telemetry - sending request for object " << transInfo->obj->toStringBrief() << ", " << (transInfo->allInstances ? "all" : "single") << " " << (transInfo->acked ? "acked" : "");
@ -310,14 +312,12 @@ void Telemetry::processObjectTransaction(ObjectTransactionInfo *transInfo)
if (sent) { if (sent) {
// Start timer if a response is expected // Start timer if a response is expected
transInfo->timer->start(REQ_TIMEOUT_MS); transInfo->timer->start(REQ_TIMEOUT_MS);
} } else {
else {
// message was not sent, the transaction will not complete and will timeout // message was not sent, the transaction will not complete and will timeout
// there is no need to wait to close the transaction and notify of completion failure // there is no need to wait to close the transaction and notify of completion failure
//transactionCompleted(transInfo->obj, false); // transactionCompleted(transInfo->obj, false);
} }
} } else {
else {
// not transacted, so just close the transaction with no notification of completion // not transacted, so just close the transaction with no notification of completion
closeTransaction(transInfo); closeTransaction(transInfo);
} }
@ -378,8 +378,8 @@ void Telemetry::processObjectQueue()
if (gcsStats.Status != GCSTelemetryStats::STATUS_CONNECTED) { if (gcsStats.Status != GCSTelemetryStats::STATUS_CONNECTED) {
objQueue.clear(); objQueue.clear();
if ((objInfo.obj->getObjID() != GCSTelemetryStats::OBJID) && if ((objInfo.obj->getObjID() != GCSTelemetryStats::OBJID) &&
(objInfo.obj->getObjID() != OPLinkSettings::OBJID) && (objInfo.obj->getObjID() != OPLinkSettings::OBJID) &&
(objInfo.obj->getObjID() != ObjectPersistence::OBJID)) { (objInfo.obj->getObjID() != ObjectPersistence::OBJID)) {
objInfo.obj->emitTransactionCompleted(false); objInfo.obj->emitTransactionCompleted(false);
return; return;
} }
@ -396,7 +396,7 @@ void Telemetry::processObjectQueue()
// TODO make the above logic a reality... // TODO make the above logic a reality...
if (findTransaction(objInfo.obj)) { if (findTransaction(objInfo.obj)) {
qWarning().nospace() << "Telemetry - !!! Making request for an object " << objInfo.obj->toStringBrief() << " for which a request is already in progress"; qWarning().nospace() << "Telemetry - !!! Making request for an object " << objInfo.obj->toStringBrief() << " for which a request is already in progress";
//objInfo.obj->emitTransactionCompleted(false); // objInfo.obj->emitTransactionCompleted(false);
return; return;
} }
UAVObject::Metadata metadata = objInfo.obj->getMetadata(); UAVObject::Metadata metadata = objInfo.obj->getMetadata();
@ -466,7 +466,7 @@ void Telemetry::processPeriodicUpdates()
time.start(); time.start();
allInstances = !objinfo->obj->isSingleInstance(); allInstances = !objinfo->obj->isSingleInstance();
processObjectUpdates(objinfo->obj, EV_UPDATED_PERIODIC, allInstances, false); processObjectUpdates(objinfo->obj, EV_UPDATED_PERIODIC, allInstances, false);
elapsedMs = time.elapsed(); elapsedMs = time.elapsed();
// Update timeToNextUpdateMs with the elapsed delay of sending the object; // Update timeToNextUpdateMs with the elapsed delay of sending the object;
timeToNextUpdateMs += elapsedMs; timeToNextUpdateMs += elapsedMs;
} }
@ -537,6 +537,7 @@ void Telemetry::objectUpdatedManual(UAVObject *obj, bool all)
QMutexLocker locker(mutex); QMutexLocker locker(mutex);
bool allInstances = obj->isSingleInstance() ? false : all; bool allInstances = obj->isSingleInstance() ? false : all;
processObjectUpdates(obj, EV_UPDATED_MANUAL, allInstances, true); processObjectUpdates(obj, EV_UPDATED_MANUAL, allInstances, true);
} }
@ -559,6 +560,7 @@ void Telemetry::updateRequested(UAVObject *obj, bool all)
QMutexLocker locker(mutex); QMutexLocker locker(mutex);
bool allInstances = obj->isSingleInstance() ? false : all; bool allInstances = obj->isSingleInstance() ? false : all;
processObjectUpdates(obj, EV_UPDATE_REQ, allInstances, true); processObjectUpdates(obj, EV_UPDATE_REQ, allInstances, true);
} }
@ -578,7 +580,7 @@ void Telemetry::newInstance(UAVObject *obj)
ObjectTransactionInfo *Telemetry::findTransaction(UAVObject *obj) ObjectTransactionInfo *Telemetry::findTransaction(UAVObject *obj)
{ {
quint32 objId = obj->getObjID(); quint32 objId = obj->getObjID();
quint16 instId = obj->getInstID(); quint16 instId = obj->getInstID();
// Lookup the transaction in the transaction map // Lookup the transaction in the transaction map
@ -596,7 +598,7 @@ ObjectTransactionInfo *Telemetry::findTransaction(UAVObject *obj)
void Telemetry::openTransaction(ObjectTransactionInfo *trans) void Telemetry::openTransaction(ObjectTransactionInfo *trans)
{ {
quint32 objId = trans->obj->getObjID(); quint32 objId = trans->obj->getObjID();
quint16 instId = trans->allInstances ? UAVTalk::ALL_INSTANCES : trans->obj->getInstID(); quint16 instId = trans->allInstances ? UAVTalk::ALL_INSTANCES : trans->obj->getInstID();
QMap<quint32, ObjectTransactionInfo *> *objTransactions = transMap.value(objId); QMap<quint32, ObjectTransactionInfo *> *objTransactions = transMap.value(objId);
@ -609,7 +611,7 @@ void Telemetry::openTransaction(ObjectTransactionInfo *trans)
void Telemetry::closeTransaction(ObjectTransactionInfo *trans) void Telemetry::closeTransaction(ObjectTransactionInfo *trans)
{ {
quint32 objId = trans->obj->getObjID(); quint32 objId = trans->obj->getObjID();
quint16 instId = trans->allInstances ? UAVTalk::ALL_INSTANCES : trans->obj->getInstID(); quint16 instId = trans->allInstances ? UAVTalk::ALL_INSTANCES : trans->obj->getInstID();
QMap<quint32, ObjectTransactionInfo *> *objTransactions = transMap.value(objId); QMap<quint32, ObjectTransactionInfo *> *objTransactions = transMap.value(objId);
@ -627,6 +629,7 @@ void Telemetry::closeAllTransactions()
QMap<quint32, ObjectTransactionInfo *> *objTransactions = transMap.value(objId); QMap<quint32, ObjectTransactionInfo *> *objTransactions = transMap.value(objId);
foreach(quint32 instId, objTransactions->keys()) { foreach(quint32 instId, objTransactions->keys()) {
ObjectTransactionInfo *trans = objTransactions->value(instId); ObjectTransactionInfo *trans = objTransactions->value(instId);
qWarning() << "Telemetry - closing active transaction for object" << trans->obj->toStringBrief(); qWarning() << "Telemetry - closing active transaction for object" << trans->obj->toStringBrief();
objTransactions->remove(instId); objTransactions->remove(instId);
delete trans; delete trans;

9
ground/openpilotgcs/src/plugins/uavtalk/telemetrymanager.cpp
View File

@ -43,8 +43,7 @@ TelemetryManager::TelemetryManager() : autopilotConnected(false)
} }
TelemetryManager::~TelemetryManager() TelemetryManager::~TelemetryManager()
{ {}
}
bool TelemetryManager::isConnected() bool TelemetryManager::isConnected()
{ {
@ -76,8 +75,7 @@ void TelemetryManager::onStart()
connect(device, SIGNAL(readyRead()), reader, SLOT(read())); connect(device, SIGNAL(readyRead()), reader, SLOT(read()));
// start the reader thread // start the reader thread
readerThread.start(); readerThread.start();
} } else {
else {
// Connect IO device to reader // Connect IO device to reader
connect(device, SIGNAL(readyRead()), utalk, SLOT(processInputStream())); connect(device, SIGNAL(readyRead()), utalk, SLOT(processInputStream()));
} }
@ -127,8 +125,7 @@ void TelemetryManager::onTelemetryUpdate(double txRate, double rxRate)
} }
IODeviceReader::IODeviceReader(UAVTalk *uavTalk) : uavTalk(uavTalk) IODeviceReader::IODeviceReader(UAVTalk *uavTalk) : uavTalk(uavTalk)
{ {}
}
void IODeviceReader::read() void IODeviceReader::read()
{ {

3
ground/openpilotgcs/src/plugins/uavtalk/telemetrymanager.h
View File

@ -72,8 +72,7 @@ private:
}; };
class IODeviceReader : public QObject class IODeviceReader : public QObject {
{
Q_OBJECT Q_OBJECT
public: public:
IODeviceReader(UAVTalk *uavTalk); IODeviceReader(UAVTalk *uavTalk);

16
ground/openpilotgcs/src/plugins/uavtalk/telemetrymonitor.cpp
View File

@ -199,16 +199,16 @@ void TelemetryMonitor::processStatsUpdates()
tel->resetStats(); tel->resetStats();
// Update stats object // Update stats object
gcsStats.TxDataRate = (float)telStats.txBytes / ((float)statsTimer->interval() / 1000.0); gcsStats.TxDataRate = (float)telStats.txBytes / ((float)statsTimer->interval() / 1000.0);
gcsStats.TxBytes += telStats.txBytes; gcsStats.TxBytes += telStats.txBytes;
gcsStats.TxFailures += telStats.txErrors; gcsStats.TxFailures += telStats.txErrors;
gcsStats.TxRetries += telStats.txRetries; gcsStats.TxRetries += telStats.txRetries;
gcsStats.RxDataRate = (float)telStats.rxBytes / ((float)statsTimer->interval() / 1000.0); gcsStats.RxDataRate = (float)telStats.rxBytes / ((float)statsTimer->interval() / 1000.0);
gcsStats.RxBytes += telStats.rxBytes; gcsStats.RxBytes += telStats.rxBytes;
gcsStats.RxFailures += telStats.rxErrors; gcsStats.RxFailures += telStats.rxErrors;
gcsStats.RxSyncErrors += telStats.rxSyncErrors; gcsStats.RxSyncErrors += telStats.rxSyncErrors;
gcsStats.RxCrcErrors += telStats.rxCrcErrors; gcsStats.RxCrcErrors += telStats.rxCrcErrors;
// Check for a connection timeout // Check for a connection timeout
bool connectionTimeout; bool connectionTimeout;

62
ground/openpilotgcs/src/plugins/uavtalk/uavtalk.cpp
View File

@ -35,8 +35,8 @@
#ifdef VERBOSE_UAVTALK #ifdef VERBOSE_UAVTALK
// uncomment and adapt the following lines to filter verbose logging to include specific object(s) only // uncomment and adapt the following lines to filter verbose logging to include specific object(s) only
//#include "flighttelemetrystats.h" // #include "flighttelemetrystats.h"
//#define VERBOSE_FILTER(objId) if (objId == FlightTelemetryStats::OBJID) // #define VERBOSE_FILTER(objId) if (objId == FlightTelemetryStats::OBJID)
#endif #endif
#ifndef VERBOSE_FILTER #ifndef VERBOSE_FILTER
#define VERBOSE_FILTER(objId) #define VERBOSE_FILTER(objId)
@ -124,9 +124,8 @@ bool UAVTalk::sendObject(UAVObject *obj, bool acked, bool allInstances)
if (allInstances) { if (allInstances) {
instId = ALL_INSTANCES; instId = ALL_INSTANCES;
} } else if (obj) {
else if (obj) { instId = obj->getInstID();
instId = obj->getInstID();
} }
bool success = false; bool success = false;
if (acked) { if (acked) {
@ -153,9 +152,8 @@ bool UAVTalk::sendObjectRequest(UAVObject *obj, bool allInstances)
if (allInstances) { if (allInstances) {
instId = ALL_INSTANCES; instId = ALL_INSTANCES;
} } else if (obj) {
else if (obj) { instId = obj->getInstID();
instId = obj->getInstID();
} }
return objectTransaction(TYPE_OBJ_REQ, obj->getObjID(), instId, obj); return objectTransaction(TYPE_OBJ_REQ, obj->getObjID(), instId, obj);
} }
@ -188,7 +186,7 @@ void UAVTalk::cancelTransaction(UAVObject *obj)
*/ */
bool UAVTalk::objectTransaction(quint8 type, quint32 objId, quint16 instId, UAVObject *obj) bool UAVTalk::objectTransaction(quint8 type, quint32 objId, quint16 instId, UAVObject *obj)
{ {
Q_ASSERT(obj); Q_ASSERT(obj);
// Send object depending on if a response is needed // Send object depending on if a response is needed
// transactions of TYPE_OBJ_REQ are acked by the response // transactions of TYPE_OBJ_REQ are acked by the response
if (type == TYPE_OBJ_ACK || type == TYPE_OBJ_REQ) { if (type == TYPE_OBJ_ACK || type == TYPE_OBJ_REQ) {
@ -217,8 +215,7 @@ void UAVTalk::processInputStream()
int ret = io->read((char *)&tmp, 1); int ret = io->read((char *)&tmp, 1);
if (ret != -1) { if (ret != -1) {
processInputByte(tmp); processInputByte(tmp);
} } else {
else {
// TODOD // TODOD
} }
if (rxState == STATE_COMPLETE) { if (rxState == STATE_COMPLETE) {
@ -226,8 +223,7 @@ void UAVTalk::processInputStream()
if (receiveObject(rxType, rxObjId, rxInstId, rxBuffer, rxLength)) { if (receiveObject(rxType, rxObjId, rxInstId, rxBuffer, rxLength)) {
stats.rxObjectBytes += rxLength; stats.rxObjectBytes += rxLength;
stats.rxObjects++; stats.rxObjects++;
} } else {
else {
// TODO... // TODO...
} }
mutex.unlock(); mutex.unlock();
@ -237,7 +233,6 @@ void UAVTalk::processInputStream()
// accessed from this thread only // accessed from this thread only
udpSocketTx->writeDatagram(rxDataArray, QHostAddress::LocalHost, udpSocketRx->localPort()); udpSocketTx->writeDatagram(rxDataArray, QHostAddress::LocalHost, udpSocketRx->localPort());
} }
} }
} }
} }
@ -319,7 +314,7 @@ bool UAVTalk::processInputByte(quint8 rxbyte)
break; break;
} }
packetSize += (quint32)rxbyte << 8; packetSize += (quint32)rxbyte << 8;
rxCount = 0; rxCount = 0;
if (packetSize < HEADER_LENGTH || packetSize > HEADER_LENGTH + MAX_PAYLOAD_LENGTH) { if (packetSize < HEADER_LENGTH || packetSize > HEADER_LENGTH + MAX_PAYLOAD_LENGTH) {
@ -342,13 +337,13 @@ bool UAVTalk::processInputByte(quint8 rxbyte)
if (rxCount < 4) { if (rxCount < 4) {
break; break;
} }
rxCount = 0; rxCount = 0;
rxObjId = (qint32)qFromLittleEndian<quint32>(rxTmpBuffer); rxObjId = (qint32)qFromLittleEndian<quint32>(rxTmpBuffer);
// Message always contain an instance ID // Message always contain an instance ID
rxInstId = 0; rxInstId = 0;
rxState = STATE_INSTID; rxState = STATE_INSTID;
break; break;
case STATE_INSTID: case STATE_INSTID:
@ -402,7 +397,6 @@ bool UAVTalk::processInputByte(quint8 rxbyte)
rxState = STATE_ERROR; rxState = STATE_ERROR;
break; break;
} }
} }
// If there is a payload get it, otherwise receive checksum // If there is a payload get it, otherwise receive checksum
@ -651,15 +645,13 @@ void UAVTalk::updateAck(quint8 type, quint32 objId, quint16 instId, UAVObject *o
if (trans && trans->respType == type) { if (trans && trans->respType == type) {
if (trans->respInstId == ALL_INSTANCES) { if (trans->respInstId == ALL_INSTANCES) {
if (instId == 0) { if (instId == 0) {
// last instance received, complete transaction // last instance received, complete transaction
closeTransaction(trans); closeTransaction(trans);
emit transactionCompleted(obj, true); emit transactionCompleted(obj, true);
} else {
// TODO extend timeout?
} }
else { } else {
// TODO extend timeout?
}
}
else {
closeTransaction(trans); closeTransaction(trans);
emit transactionCompleted(obj, true); emit transactionCompleted(obj, true);
} }
@ -713,7 +705,7 @@ bool UAVTalk::transmitObject(quint8 type, quint32 objId, quint16 instId, UAVObje
ret = true; ret = true;
quint32 numInst = objMngr->getNumInstances(objId); quint32 numInst = objMngr->getNumInstances(objId);
for (quint32 n = 0; n < numInst; ++n) { for (quint32 n = 0; n < numInst; ++n) {
quint32 i = numInst - n - 1; quint32 i = numInst - n - 1;
UAVObject *o = objMngr->getObject(objId, i); UAVObject *o = objMngr->getObject(objId, i);
if (!transmitSingleObject(type, objId, i, o)) { if (!transmitSingleObject(type, objId, i, o)) {
ret = false; ret = false;
@ -800,13 +792,11 @@ bool UAVTalk::transmitSingleObject(quint8 type, quint32 objId, quint16 instId, U
if (useUDPMirror) { if (useUDPMirror) {
udpSocketRx->writeDatagram((const char *)txBuffer, HEADER_LENGTH + length + CHECKSUM_LENGTH, QHostAddress::LocalHost, udpSocketTx->localPort()); udpSocketRx->writeDatagram((const char *)txBuffer, HEADER_LENGTH + length + CHECKSUM_LENGTH, QHostAddress::LocalHost, udpSocketTx->localPort());
} }
} } else {
else {
qWarning() << "UAVTalk - error transmitting : io device full"; qWarning() << "UAVTalk - error transmitting : io device full";
++stats.txErrors; ++stats.txErrors;
return false; return false;
} }
} else { } else {
qWarning() << "UAVTalk - error transmitting : io device not writable"; qWarning() << "UAVTalk - error transmitting : io device not writable";
++stats.txErrors; ++stats.txErrors;
@ -840,8 +830,9 @@ UAVTalk::Transaction *UAVTalk::findTransaction(quint32 objId, quint16 instId)
void UAVTalk::openTransaction(quint8 type, quint32 objId, quint16 instId) void UAVTalk::openTransaction(quint8 type, quint32 objId, quint16 instId)
{ {
Transaction *trans = new Transaction(); Transaction *trans = new Transaction();
trans->respType = (type == TYPE_OBJ_REQ) ? TYPE_OBJ : TYPE_ACK;
trans->respObjId = objId; trans->respType = (type == TYPE_OBJ_REQ) ? TYPE_OBJ : TYPE_ACK;
trans->respObjId = objId;
trans->respInstId = instId; trans->respInstId = instId;
QMap<quint32, Transaction *> *objTransactions = transMap.value(trans->respObjId); QMap<quint32, Transaction *> *objTransactions = transMap.value(trans->respObjId);
@ -869,6 +860,7 @@ void UAVTalk::closeAllTransactions()
QMap<quint32, Transaction *> *objTransactions = transMap.value(objId); QMap<quint32, Transaction *> *objTransactions = transMap.value(objId);
foreach(quint32 instId, objTransactions->keys()) { foreach(quint32 instId, objTransactions->keys()) {
Transaction *trans = objTransactions->value(instId); Transaction *trans = objTransactions->value(instId);
qWarning() << "UAVTalk - closing active transaction for object" << trans->respObjId; qWarning() << "UAVTalk - closing active transaction for object" << trans->respObjId;
objTransactions->remove(instId); objTransactions->remove(instId);
delete trans; delete trans;
@ -878,29 +870,33 @@ void UAVTalk::closeAllTransactions()
} }
} }
const char* UAVTalk::typeToString(quint8 type) const char *UAVTalk::typeToString(quint8 type)
{ {
switch (type) { switch (type) {
case TYPE_OBJ: case TYPE_OBJ:
return "object"; return "object";
break; break;
case TYPE_OBJ_ACK: case TYPE_OBJ_ACK:
return "object (acked)"; return "object (acked)";
break; break;
case TYPE_OBJ_REQ: case TYPE_OBJ_REQ:
return "object request"; return "object request";
break; break;
case TYPE_ACK: case TYPE_ACK:
return "ack"; return "ack";
break; break;
case TYPE_NACK: case TYPE_NACK:
return "nack"; return "nack";
break;
break;
} }
return "<error>"; return "<error>";
} }

20
ground/openpilotgcs/src/plugins/uavtalk/uavtalk.h
View File

@ -44,7 +44,7 @@ class UAVTALK_EXPORT UAVTalk : public QObject {
friend class IODeviceReader; friend class IODeviceReader;
public: public:
static const quint16 ALL_INSTANCES = 0xFFFF; static const quint16 ALL_INSTANCES = 0xFFFF;
typedef struct { typedef struct {
quint32 txBytes; quint32 txBytes;
@ -80,22 +80,22 @@ private slots:
private: private:
typedef struct { typedef struct {
quint8 respType; quint8 respType;
quint32 respObjId; quint32 respObjId;
quint16 respInstId; quint16 respInstId;
} Transaction; } Transaction;
// Constants // Constants
static const int TYPE_MASK = 0xF8; static const int TYPE_MASK = 0xF8;
static const int TYPE_VER = 0x20; static const int TYPE_VER = 0x20;
static const int TYPE_OBJ = (TYPE_VER | 0x00); static const int TYPE_OBJ = (TYPE_VER | 0x00);
static const int TYPE_OBJ_REQ = (TYPE_VER | 0x01); static const int TYPE_OBJ_REQ = (TYPE_VER | 0x01);
static const int TYPE_OBJ_ACK = (TYPE_VER | 0x02); static const int TYPE_OBJ_ACK = (TYPE_VER | 0x02);
static const int TYPE_ACK = (TYPE_VER | 0x03); static const int TYPE_ACK = (TYPE_VER | 0x03);
static const int TYPE_NACK = (TYPE_VER | 0x04); static const int TYPE_NACK = (TYPE_VER | 0x04);
// header : sync(1), type (1), size(2), object ID(4), instance ID(2) // header : sync(1), type (1), size(2), object ID(4), instance ID(2)
static const int HEADER_LENGTH = 10; static const int HEADER_LENGTH = 10;
static const int MAX_PAYLOAD_LENGTH = 256; static const int MAX_PAYLOAD_LENGTH = 256;