diff --git a/flight/modules/PathPlanner/pathplanner.c b/flight/modules/PathPlanner/pathplanner.c index 93e664b85..6c548b6b7 100644 --- a/flight/modules/PathPlanner/pathplanner.c +++ b/flight/modules/PathPlanner/pathplanner.c @@ -265,8 +265,9 @@ static uint8_t checkPathPlan() uint16_t actionCount; uint8_t pathCrc; PathPlanData pathPlan; - //WaypointData waypoint; // using global instead (?) - //PathActionData action; // using global instead (?) + + // WaypointData waypoint; // using global instead (?) + // PathActionData action; // using global instead (?) PathPlanGet(&pathPlan); @@ -275,15 +276,15 @@ static uint8_t checkPathPlan() // an empty path plan is invalid return false; } - actionCount = pathPlan.PathActionCount; + actionCount = pathPlan.PathActionCount; // check count consistency if (waypointCount > UAVObjGetNumInstances(WaypointHandle())) { - //PIOS_DEBUGLOG_Printf("PathPlan : waypoint count error!"); + // PIOS_DEBUGLOG_Printf("PathPlan : waypoint count error!"); return false; } if (actionCount > UAVObjGetNumInstances(PathActionHandle())) { - //PIOS_DEBUGLOG_Printf("PathPlan : path action count error!"); + // PIOS_DEBUGLOG_Printf("PathPlan : path action count error!"); return false; } @@ -297,7 +298,7 @@ static uint8_t checkPathPlan() } if (pathCrc != pathPlan.Crc) { // 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; } diff --git a/flight/modules/RadioComBridge/RadioComBridge.c b/flight/modules/RadioComBridge/RadioComBridge.c index f913ab672..22341a771 100644 --- a/flight/modules/RadioComBridge/RadioComBridge.c +++ b/flight/modules/RadioComBridge/RadioComBridge.c @@ -1,5 +1,6 @@ /** ****************************************************************************** + * @addtogroup OpenPilotModules OpenPilot Modules * @{ * @addtogroup RadioComBridgeModule Com Port to Radio Bridge Module @@ -133,9 +134,9 @@ static int32_t RadioComBridgeStart(void) 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). - data->parseUAVTalk = ((oplinkSettings.MainPort != OPLINKSETTINGS_MAINPORT_SERIAL) && - (oplinkSettings.FlexiPort != OPLINKSETTINGS_FLEXIPORT_SERIAL) && - (oplinkSettings.VCPPort != OPLINKSETTINGS_VCPPORT_SERIAL)); + data->parseUAVTalk = ((oplinkSettings.MainPort != OPLINKSETTINGS_MAINPORT_SERIAL) && + (oplinkSettings.FlexiPort != OPLINKSETTINGS_FLEXIPORT_SERIAL) && + (oplinkSettings.VCPPort != OPLINKSETTINGS_VCPPORT_SERIAL)); // Set the maximum radio RF power. switch (oplinkSettings.MaxRFPower) { @@ -236,20 +237,20 @@ static int32_t RadioComBridgeInitialize(void) RadioComBridgeStatsInitialize(); // Initialise UAVTalk - data->telemUAVTalkCon = UAVTalkInitialize(&UAVTalkSendHandler); - data->radioUAVTalkCon = UAVTalkInitialize(&RadioSendHandler); + data->telemUAVTalkCon = UAVTalkInitialize(&UAVTalkSendHandler); + data->radioUAVTalkCon = UAVTalkInitialize(&RadioSendHandler); // Initialize the queues. - data->uavtalkEventQueue = xQueueCreate(EVENT_QUEUE_SIZE, sizeof(UAVObjEvent)); - data->radioEventQueue = xQueueCreate(EVENT_QUEUE_SIZE, sizeof(UAVObjEvent)); + data->uavtalkEventQueue = xQueueCreate(EVENT_QUEUE_SIZE, sizeof(UAVObjEvent)); + data->radioEventQueue = xQueueCreate(EVENT_QUEUE_SIZE, sizeof(UAVObjEvent)); // Initialize the statistics. - data->telemetryTxRetries = 0; - data->radioTxRetries = 0; + data->telemetryTxRetries = 0; + data->radioTxRetries = 0; - data->parseUAVTalk = true; - data->comSpeed = OPLINKSETTINGS_COMSPEED_9600; - PIOS_COM_RADIO = PIOS_COM_RFM22B; + data->parseUAVTalk = true; + data->comSpeed = OPLINKSETTINGS_COMSPEED_9600; + PIOS_COM_RADIO = PIOS_COM_RFM22B; return 0; } @@ -264,13 +265,14 @@ static void registerObject(UAVObjHandle obj) { // Setup object for periodic updates UAVObjEvent ev = { - .obj = obj, - .instId = UAVOBJ_ALL_INSTANCES, - .event = EV_UPDATED_PERIODIC, + .obj = obj, + .instId = UAVOBJ_ALL_INSTANCES, + .event = EV_UPDATED_PERIODIC, }; // Get metadata UAVObjMetadata metadata; + UAVObjGetMetadata(obj, &metadata); EventPeriodicQueueCreate(&ev, data->uavtalkEventQueue, metadata.telemetryUpdatePeriod); @@ -298,23 +300,23 @@ static void updateRadioComBridgeStats() RadioComBridgeStatsGet(&radioComBridgeStats); // Update stats object - radioComBridgeStats.TelemetryTxBytes += telemetryUAVTalkStats.txBytes; - radioComBridgeStats.TelemetryTxFailures += telemetryUAVTalkStats.txErrors; - radioComBridgeStats.TelemetryTxRetries += data->telemetryTxRetries; + radioComBridgeStats.TelemetryTxBytes += telemetryUAVTalkStats.txBytes; + radioComBridgeStats.TelemetryTxFailures += telemetryUAVTalkStats.txErrors; + radioComBridgeStats.TelemetryTxRetries += data->telemetryTxRetries; - radioComBridgeStats.TelemetryRxBytes += telemetryUAVTalkStats.rxBytes; - radioComBridgeStats.TelemetryRxFailures += telemetryUAVTalkStats.rxErrors; + radioComBridgeStats.TelemetryRxBytes += telemetryUAVTalkStats.rxBytes; + radioComBridgeStats.TelemetryRxFailures += telemetryUAVTalkStats.rxErrors; radioComBridgeStats.TelemetryRxSyncErrors += telemetryUAVTalkStats.rxSyncErrors; - radioComBridgeStats.TelemetryRxCrcErrors += telemetryUAVTalkStats.rxCrcErrors; + radioComBridgeStats.TelemetryRxCrcErrors += telemetryUAVTalkStats.rxCrcErrors; - radioComBridgeStats.RadioTxBytes += radioUAVTalkStats.txBytes; - radioComBridgeStats.RadioTxFailures += radioUAVTalkStats.txErrors; - radioComBridgeStats.RadioTxRetries += data->radioTxRetries; + radioComBridgeStats.RadioTxBytes += radioUAVTalkStats.txBytes; + radioComBridgeStats.RadioTxFailures += radioUAVTalkStats.txErrors; + radioComBridgeStats.RadioTxRetries += data->radioTxRetries; - radioComBridgeStats.RadioRxBytes += radioUAVTalkStats.rxBytes; - radioComBridgeStats.RadioRxFailures += radioUAVTalkStats.rxErrors; + radioComBridgeStats.RadioRxBytes += radioUAVTalkStats.rxBytes; + radioComBridgeStats.RadioRxFailures += radioUAVTalkStats.rxErrors; radioComBridgeStats.RadioRxSyncErrors += radioUAVTalkStats.rxSyncErrors; - radioComBridgeStats.RadioRxCrcErrors += radioUAVTalkStats.rxCrcErrors; + radioComBridgeStats.RadioRxCrcErrors += radioUAVTalkStats.rxCrcErrors; // Update stats object data RadioComBridgeStatsSet(&radioComBridgeStats); diff --git a/flight/modules/Telemetry/telemetry.c b/flight/modules/Telemetry/telemetry.c index 443112773..ef1ab9ea4 100644 --- a/flight/modules/Telemetry/telemetry.c +++ b/flight/modules/Telemetry/telemetry.c @@ -182,16 +182,6 @@ static void registerObject(UAVObjHandle obj) UAVObjConnectQueue(obj, priorityQueue, EV_MASK_ALL_UPDATES); } else { // 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); } } @@ -312,8 +302,8 @@ static void processObjEvent(UAVObjEvent *ev) retries = 0; success = -1; if ((ev->event == EV_UPDATED && (updateMode == UPDATEMODE_ONCHANGE || updateMode == UPDATEMODE_THROTTLED)) - || ev->event == EV_UPDATED_MANUAL - || (ev->event == EV_UPDATED_PERIODIC && updateMode != UPDATEMODE_THROTTLED)) { + || ev->event == EV_UPDATED_MANUAL + || (ev->event == EV_UPDATED_PERIODIC && updateMode != UPDATEMODE_THROTTLED)) { // Send update to GCS (with retries) while (retries < MAX_RETRIES && success == -1) { // call blocks until ack is received or timeout @@ -357,8 +347,8 @@ static void processObjEvent(UAVObjEvent *ev) if (ev->obj) { updateMode = UAVObjGetLoggingUpdateMode(&metadata); if ((ev->event == EV_UPDATED && (updateMode == UPDATEMODE_ONCHANGE || updateMode == UPDATEMODE_THROTTLED)) - || ev->event == EV_LOGGING_MANUAL - || (ev->event == EV_LOGGING_PERIODIC && updateMode != UPDATEMODE_THROTTLED)) { + || ev->event == EV_LOGGING_MANUAL + || (ev->event == EV_LOGGING_PERIODIC && updateMode != UPDATEMODE_THROTTLED)) { if (ev->instId == UAVOBJ_ALL_INSTANCES) { success = UAVObjGetNumInstances(ev->obj); for (retries = 0; retries < success; retries++) { @@ -572,33 +562,33 @@ static void updateTelemetryStats() // Update stats object if (flightStats.Status == FLIGHTTELEMETRYSTATS_STATUS_CONNECTED) { - flightStats.TxDataRate = (float)utalkStats.txBytes / ((float)STATS_UPDATE_PERIOD_MS / 1000.0f); - flightStats.TxBytes += utalkStats.txBytes; - flightStats.TxFailures += txErrors; - flightStats.TxRetries += txRetries; + flightStats.TxDataRate = (float)utalkStats.txBytes / ((float)STATS_UPDATE_PERIOD_MS / 1000.0f); + flightStats.TxBytes += utalkStats.txBytes; + flightStats.TxFailures += txErrors; + flightStats.TxRetries += txRetries; - flightStats.RxDataRate = (float)utalkStats.rxBytes / ((float)STATS_UPDATE_PERIOD_MS / 1000.0f); - flightStats.RxBytes += utalkStats.rxBytes; - flightStats.RxFailures += utalkStats.rxErrors; + flightStats.RxDataRate = (float)utalkStats.rxBytes / ((float)STATS_UPDATE_PERIOD_MS / 1000.0f); + flightStats.RxBytes += utalkStats.rxBytes; + flightStats.RxFailures += utalkStats.rxErrors; flightStats.RxSyncErrors += utalkStats.rxSyncErrors; - flightStats.RxCrcErrors += utalkStats.rxCrcErrors; + flightStats.RxCrcErrors += utalkStats.rxCrcErrors; } else { - flightStats.TxDataRate = 0; - flightStats.TxBytes = 0; - flightStats.TxFailures = 0; - flightStats.TxRetries = 0; + flightStats.TxDataRate = 0; + flightStats.TxBytes = 0; + flightStats.TxFailures = 0; + flightStats.TxRetries = 0; - flightStats.RxDataRate = 0; - flightStats.RxBytes = 0; - flightStats.RxFailures = 0; + flightStats.RxDataRate = 0; + flightStats.RxBytes = 0; + flightStats.RxFailures = 0; flightStats.RxSyncErrors = 0; - flightStats.RxCrcErrors = 0; + flightStats.RxCrcErrors = 0; } - txErrors = 0; + txErrors = 0; txRetries = 0; // Check for connection timeout - timeNow = xTaskGetTickCount() * portTICK_RATE_MS; + timeNow = xTaskGetTickCount() * portTICK_RATE_MS; if (utalkStats.rxObjects > 0) { timeOfLastObjectUpdate = timeNow; } diff --git a/flight/uavobjects/uavobjectmanager.c b/flight/uavobjects/uavobjectmanager.c index bf5d52a80..f5077c982 100644 --- a/flight/uavobjects/uavobjectmanager.c +++ b/flight/uavobjects/uavobjectmanager.c @@ -732,7 +732,7 @@ uint8_t UAVObjUpdateCRC(UAVObjHandle obj_handle, uint16_t instId, uint8_t crc) goto unlock_exit; } // 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: diff --git a/flight/uavtalk/inc/uavtalk.h b/flight/uavtalk/inc/uavtalk.h index 9a346be3a..e54231735 100644 --- a/flight/uavtalk/inc/uavtalk.h +++ b/flight/uavtalk/inc/uavtalk.h @@ -44,7 +44,6 @@ typedef struct { uint32_t rxErrors; uint32_t rxSyncErrors; uint32_t rxCrcErrors; - } UAVTalkStats; typedef void *UAVTalkConnection; diff --git a/flight/uavtalk/inc/uavtalk_priv.h b/flight/uavtalk/inc/uavtalk_priv.h index c7cab8d9e..73354845a 100644 --- a/flight/uavtalk/inc/uavtalk_priv.h +++ b/flight/uavtalk/inc/uavtalk_priv.h @@ -34,10 +34,10 @@ // Private types and constants // 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) -#define UAVTALK_MAX_HEADER_LENGTH 12 +#define UAVTALK_MAX_HEADER_LENGTH 12 #define UAVTALK_CHECKSUM_LENGTH 1 @@ -47,11 +47,11 @@ #define UAVTALK_MAX_PACKET_LENGTH UAVTALK_MIN_PACKET_LENGTH + UAVTALK_MAX_PAYLOAD_LENGTH typedef struct { - uint8_t type; - uint16_t packet_size; - uint32_t objId; - uint16_t instId; - uint32_t length; + uint8_t type; + uint16_t packet_size; + uint32_t objId; + uint16_t instId; + uint32_t length; uint8_t timestampLength; uint8_t cs; uint16_t timestamp; diff --git a/flight/uavtalk/uavtalk.c b/flight/uavtalk/uavtalk.c index f4909aca7..0f9dd3d0e 100644 --- a/flight/uavtalk/uavtalk.c +++ b/flight/uavtalk/uavtalk.c @@ -32,13 +32,13 @@ #include "openpilot.h" #include "uavtalk_priv.h" -//#define UAV_DEBUGLOG 1 +// #define UAV_DEBUGLOG 1 #if defined UAV_DEBUGLOG && defined FLASH_FREERTOS #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 -//#include "flighttelemetrystats.h" -//#define UAVT_DEBUGLOG_CPRINTF(objId, ...) if (objId == FLIGHTTELEMETRYSTATS_OBJID) { UAVT_DEBUGLOG_PRINTF(__VA_ARGS__); } +// #include "flighttelemetrystats.h" +// #define UAVT_DEBUGLOG_CPRINTF(objId, ...) if (objId == FLIGHTTELEMETRYSTATS_OBJID) { UAVT_DEBUGLOG_PRINTF(__VA_ARGS__); } #endif #ifndef 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 ret = -1; + // 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) { // 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); // expected response type connection->respType = (type == UAVTALK_TYPE_OBJ_REQ) ? UAVTALK_TYPE_OBJ : UAVTALK_TYPE_ACK; - connection->respObjId = UAVObjGetID(obj); + connection->respObjId = UAVObjGetID(obj); connection->respInstId = instId; ret = sendObject(connection, type, UAVObjGetID(obj), instId, obj); xSemaphoreGiveRecursive(connection->lock); @@ -376,8 +377,8 @@ UAVTalkRxState UAVTalkProcessInputStreamQuiet(UAVTalkConnection connectionHandle iproc->rxPacketLength = 1; iproc->rxCount = 0; - iproc->type = 0; - iproc->state = UAVTALK_STATE_TYPE; + iproc->type = 0; + iproc->state = UAVTALK_STATE_TYPE; break; case UAVTALK_STATE_TYPE: @@ -389,9 +390,9 @@ UAVTalkRxState UAVTalkProcessInputStreamQuiet(UAVTalkConnection connectionHandle } // 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->state = UAVTALK_STATE_SIZE; @@ -408,7 +409,7 @@ UAVTalkRxState UAVTalkProcessInputStreamQuiet(UAVTalkConnection connectionHandle break; } 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) { // incorrect packet size @@ -417,8 +418,8 @@ UAVTalkRxState UAVTalkProcessInputStreamQuiet(UAVTalkConnection connectionHandle break; } - iproc->objId = 0; - iproc->state = UAVTALK_STATE_OBJID; + iproc->objId = 0; + iproc->state = UAVTALK_STATE_OBJID; break; case UAVTALK_STATE_OBJID: @@ -432,8 +433,8 @@ UAVTalkRxState UAVTalkProcessInputStreamQuiet(UAVTalkConnection connectionHandle } iproc->rxCount = 0; - iproc->instId = 0; - iproc->state = UAVTALK_STATE_INSTID; + iproc->instId = 0; + iproc->state = UAVTALK_STATE_INSTID; break; case UAVTALK_STATE_INSTID: @@ -482,7 +483,7 @@ UAVTalkRxState UAVTalkProcessInputStreamQuiet(UAVTalkConnection connectionHandle if (iproc->type & UAVTALK_TIMESTAMPED) { // If there is a timestamp get it iproc->timestamp = 0; - iproc->state = UAVTALK_STATE_TIMESTAMP; + iproc->state = UAVTALK_STATE_TIMESTAMP; } else { // If there is a payload get it, otherwise receive checksum if (iproc->length > 0) { @@ -598,6 +599,7 @@ UAVTalkRxState UAVTalkRelayPacket(UAVTalkConnection inConnectionHandle, UAVTalkC // The input packet must be completely parsed. if (inIproc->state != UAVTALK_STATE_COMPLETE) { inConnection->stats.rxErrors++; + return -1; } @@ -606,6 +608,7 @@ UAVTalkRxState UAVTalkRelayPacket(UAVTalkConnection inConnectionHandle, UAVTalkC if (!outConnection->outStream) { outConnection->stats.txErrors++; + return -1; } @@ -654,7 +657,7 @@ UAVTalkRxState UAVTalkRelayPacket(UAVTalkConnection inConnectionHandle, UAVTalkC // evaluate return value before releasing the lock 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++; rxState = -1; } @@ -718,7 +721,8 @@ uint32_t UAVTalkGetPacketObjId(UAVTalkConnection connectionHandle) * \return 0 Success * \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; 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 // even one that was not sent in response to the OBJ_REQ message updateAck(connection, type, objId, instId); - } - else { + } else { ret = -1; } } else { @@ -838,8 +841,7 @@ static void updateAck(UAVTalkConnectionData *connection, uint8_t type, uint32_t // last instance received, complete transaction xSemaphoreGive(connection->respSema); connection->respObjId = 0; - } - else if (connection->respInstId == instId) { + } else if (connection->respInstId == instId) { xSemaphoreGive(connection->respSema); connection->respObjId = 0; } @@ -876,7 +878,7 @@ static int32_t sendObject(UAVTalkConnectionData *connection, uint8_t type, uint3 numInst = UAVObjGetNumInstances(obj); // 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) - ret = 0; + ret = 0; for (n = 0; n < numInst; ++n) { if (sendSingleObject(connection, type, objId, numInst - n - 1, obj) == -1) { ret = -1; @@ -903,7 +905,7 @@ static int32_t sendObject(UAVTalkConnectionData *connection, uint8_t type, uint3 * \param[in] type Transaction type * \param[in] objId The object ID * \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) * \return 0 Success * \return -1 Failure @@ -978,8 +980,7 @@ static int32_t sendSingleObject(UAVTalkConnectionData *connection, uint8_t type, ++connection->stats.txObjects; connection->stats.txObjectBytes += length; connection->stats.txBytes += tx_msg_len; - } - else { + } else { connection->stats.txErrors++; // TDOD rc == -1 connection not open, -2 buffer full should retry connection->stats.txBytes += (rc > 0) ? rc : 0; diff --git a/ground/openpilotgcs/src/libs/utils/crc.h b/ground/openpilotgcs/src/libs/utils/crc.h index 2d9fc15a1..92c007a59 100644 --- a/ground/openpilotgcs/src/libs/utils/crc.h +++ b/ground/openpilotgcs/src/libs/utils/crc.h @@ -32,9 +32,7 @@ #include "utils_global.h" namespace Utils { - class QTCREATOR_UTILS_EXPORT Crc { - public: /** * Update the crc value with new data. @@ -54,9 +52,7 @@ public: * \return The updated crc value. */ static quint8 updateCRC(quint8 crc, const quint8 *data, qint32 length); - }; - } // namespace Utils #endif // CRC_H diff --git a/ground/openpilotgcs/src/plugins/notify/notifyplugin.cpp b/ground/openpilotgcs/src/plugins/notify/notifyplugin.cpp index 898e22802..4eee0438e 100644 --- a/ground/openpilotgcs/src/plugins/notify/notifyplugin.cpp +++ b/ground/openpilotgcs/src/plugins/notify/notifyplugin.cpp @@ -128,6 +128,7 @@ void SoundNotifyPlugin::readConfig(QSettings *settings, UAVConfigInfo * /* confi void SoundNotifyPlugin::onTelemetryManagerAdded(QObject *obj) { TelemetryManager *telMngr = qobject_cast(obj); + if (telMngr) { connect(telMngr, SIGNAL(disconnected()), this, SLOT(onAutopilotDisconnect())); } diff --git a/ground/openpilotgcs/src/plugins/opmap/flightdatamodel.cpp b/ground/openpilotgcs/src/plugins/opmap/flightdatamodel.cpp index ad269d626..674f4e092 100644 --- a/ground/openpilotgcs/src/plugins/opmap/flightdatamodel.cpp +++ b/ground/openpilotgcs/src/plugins/opmap/flightdatamodel.cpp @@ -30,8 +30,7 @@ #include flightDataModel::flightDataModel(QObject *parent) : QAbstractTableModel(parent) -{ -} +{} 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 b; + switch (index) { case WPDESCRITPTION: 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 value; + switch (index) { case 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 value; + if (role == Qt::DisplayRole) { if (orientation == Qt::Vertical) { value = QString::number(section + 1); @@ -605,7 +607,7 @@ void flightDataModel::readFromFile(QString fileName) while (!fieldNode.isNull()) { QDomElement field = fieldNode.toElement(); if (field.tagName() == "field") { - QString name = field.attribute("name"); + QString name = field.attribute("name"); QString value = field.attribute("value"); if (name == "altitude") { data->altitude = value.toDouble(); diff --git a/ground/openpilotgcs/src/plugins/opmap/modeluavoproxy.cpp b/ground/openpilotgcs/src/plugins/opmap/modeluavoproxy.cpp index 8b31c1d65..e328bf0db 100644 --- a/ground/openpilotgcs/src/plugins/opmap/modeluavoproxy.cpp +++ b/ground/openpilotgcs/src/plugins/opmap/modeluavoproxy.cpp @@ -32,13 +32,14 @@ ModelUavoProxy::ModelUavoProxy(QObject *parent, flightDataModel *model) : QObject(parent), myModel(model) { ExtensionSystem::PluginManager *pm = ExtensionSystem::PluginManager::instance(); + Q_ASSERT(pm != NULL); - objMngr = pm->getObject(); + objMngr = pm->getObject(); Q_ASSERT(objMngr != NULL); completionCountdown = 0; - successCountdown = 0; + successCountdown = 0; } void ModelUavoProxy::sendPathPlan() @@ -59,7 +60,7 @@ void ModelUavoProxy::sendPathPlan() // we will start 3 update all completionCountdown = 3; - successCountdown = completionCountdown; + successCountdown = completionCountdown; pathPlan->updated(); waypoint->updatedAll(); @@ -77,8 +78,7 @@ void ModelUavoProxy::pathPlanElementSent(UAVObject *obj, bool success) qDebug() << "ModelUavoProxy::pathPlanSent - completed" << (successCountdown == 0); if (successCountdown == 0) { 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 !")); } } @@ -87,6 +87,7 @@ void ModelUavoProxy::pathPlanElementSent(UAVObject *obj, bool success) void ModelUavoProxy::receivePathPlan() { PathPlan *pathPlan = PathPlan::GetInstance(objMngr, 0); + connect(pathPlan, SIGNAL(transactionCompleted(UAVObject *, bool)), this, SLOT(pathPlanElementReceived(UAVObject *, bool))); Waypoint *waypoint = Waypoint::GetInstance(objMngr, 0); @@ -97,7 +98,7 @@ void ModelUavoProxy::receivePathPlan() // we will start 3 update requests completionCountdown = 3; - successCountdown = completionCountdown; + successCountdown = completionCountdown; pathPlan->requestUpdate(); waypoint->requestUpdateAll(); @@ -117,8 +118,7 @@ void ModelUavoProxy::pathPlanElementReceived(UAVObject *obj, bool success) if (objectsToModel()) { 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 !")); } } @@ -139,12 +139,11 @@ bool ModelUavoProxy::modelToObjects() qDebug() << "ModelUAVProxy::modelToObjects"; // track number of path actions - int actionCount = 0; + int actionCount = 0; // iterate over waypoints int waypointCount = myModel->rowCount(); for (int i = 0; i < waypointCount; ++i) { - // Path Actions // create action to use as a search criteria @@ -168,8 +167,7 @@ bool ModelUavoProxy::modelToObjects() // update UAVObject action->setData(actionData); - } - else { + } else { action->deleteLater(); action = foundAction; qDebug() << "ModelUAVProxy::modelToObjects - found action instance :" << action->getInstID(); @@ -208,7 +206,7 @@ bool ModelUavoProxy::modelToObjects() PathPlan *pathPlan = PathPlan::GetInstance(objMngr); PathPlan::DataFields pathPlanData = pathPlan->getData(); - pathPlanData.WaypointCount = waypointCount; + pathPlanData.WaypointCount = waypointCount; pathPlanData.PathActionCount = actionCount; pathPlanData.Crc = computePathPlanCrc(waypointCount, actionCount); @@ -217,9 +215,11 @@ bool ModelUavoProxy::modelToObjects() return true; } -Waypoint *ModelUavoProxy::createWaypoint(int index, Waypoint *newWaypoint) { +Waypoint *ModelUavoProxy::createWaypoint(int index, Waypoint *newWaypoint) +{ Waypoint *waypoint = NULL; int count = objMngr->getNumInstances(Waypoint::OBJID); + if (index < count) { // reuse object 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->initialize(index, waypoint->getMetaObject()); objMngr->registerObject(waypoint); - } - else { + } else { // we can only create the "next" object // TODO fail in a clean way :( } return waypoint; } -PathAction *ModelUavoProxy::createPathAction(int index, PathAction *newAction) { +PathAction *ModelUavoProxy::createPathAction(int index, PathAction *newAction) +{ PathAction *action = NULL; int count = objMngr->getNumInstances(PathAction::OBJID); + if (index < count) { // reuse object 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->initialize(index, action->getMetaObject()); objMngr->registerObject(action); - } - else { + } else { // we can only create the "next" object // TODO fail in a clean way :( } 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 count = actionCount <= instancesCount ? actionCount : instancesCount; + for (int i = 0; i < count; ++i) { PathAction *action = PathAction::GetInstance(objMngr, i); Q_ASSERT(action); @@ -278,15 +280,15 @@ PathAction *ModelUavoProxy::findPathAction(const PathAction::DataFields &actionD } PathAction::DataFields fields = action->getData(); if (fields.Command == actionData.Command - && fields.ConditionParameters[0] == actionData.ConditionParameters[0] - && fields.ConditionParameters[1] == actionData.ConditionParameters[1] - && fields.ConditionParameters[2] == actionData.ConditionParameters[2] - && fields.EndCondition == actionData.EndCondition - && fields.ErrorDestination == actionData.ErrorDestination - && fields.JumpDestination == actionData.JumpDestination && fields.Mode == actionData.Mode - && fields.ModeParameters[0] == actionData.ModeParameters[0] - && fields.ModeParameters[1] == actionData.ModeParameters[1] - && fields.ModeParameters[2] == actionData.ModeParameters[2]) { + && fields.ConditionParameters[0] == actionData.ConditionParameters[0] + && fields.ConditionParameters[1] == actionData.ConditionParameters[1] + && fields.ConditionParameters[2] == actionData.ConditionParameters[2] + && fields.EndCondition == actionData.EndCondition + && fields.ErrorDestination == actionData.ErrorDestination + && fields.JumpDestination == actionData.JumpDestination && fields.Mode == actionData.Mode + && fields.ModeParameters[0] == actionData.ModeParameters[0] + && fields.ModeParameters[1] == actionData.ModeParameters[1] + && fields.ModeParameters[2] == actionData.ModeParameters[2]) { return action; } } @@ -302,8 +304,8 @@ bool ModelUavoProxy::objectsToModel() PathPlan *pathPlan = PathPlan::GetInstance(objMngr); PathPlan::DataFields pathPlanData = pathPlan->getData(); - int waypointCount = pathPlanData.WaypointCount; - int actionCount = pathPlanData.PathActionCount; + int waypointCount = pathPlanData.WaypointCount; + int actionCount = pathPlanData.PathActionCount; // consistency check if (waypointCount > objMngr->getNumInstances(Waypoint::OBJID)) { @@ -322,8 +324,7 @@ bool ModelUavoProxy::objectsToModel() int rowCount = myModel->rowCount(); if (waypointCount < rowCount) { myModel->removeRows(waypointCount, rowCount - waypointCount); - } - else if (waypointCount > rowCount) { + } else if (waypointCount > rowCount) { myModel->insertRows(rowCount, waypointCount - rowCount); } @@ -349,10 +350,12 @@ bool ModelUavoProxy::objectsToModel() return true; } -void ModelUavoProxy::modelToWaypoint(int i, Waypoint::DataFields &data) { +void ModelUavoProxy::modelToWaypoint(int i, Waypoint::DataFields &data) +{ double distance, bearing, altitude, velocity; QModelIndex index = myModel->index(i, flightDataModel::DISRELATIVE); + distance = myModel->data(index).toDouble(); index = myModel->index(i, flightDataModel::BEARELATIVE); bearing = myModel->data(index).toDouble(); @@ -367,59 +370,64 @@ void ModelUavoProxy::modelToWaypoint(int i, Waypoint::DataFields &data) { 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] + - 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; + if (bearing != bearing) { bearing = 0; } - double altitude = -data.Position[Waypoint::POSITION_DOWN]; + double altitude = -data.Position[Waypoint::POSITION_DOWN]; QModelIndex index = myModel->index(i, flightDataModel::VELOCITY); myModel->setData(index, data.Velocity); - index = myModel->index(i, flightDataModel::DISRELATIVE); + index = myModel->index(i, flightDataModel::DISRELATIVE); myModel->setData(index, distance); - index = myModel->index(i, flightDataModel::BEARELATIVE); + index = myModel->index(i, flightDataModel::BEARELATIVE); myModel->setData(index, bearing); - index = myModel->index(i, flightDataModel::ALTITUDERELATIVE); + index = myModel->index(i, flightDataModel::ALTITUDERELATIVE); 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); - 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(); - index = myModel->index(i, flightDataModel::MODE_PARAMS1); + index = myModel->index(i, flightDataModel::MODE_PARAMS1); 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(); - index = myModel->index(i, flightDataModel::MODE_PARAMS3); + index = myModel->index(i, flightDataModel::MODE_PARAMS3); 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(); - index = myModel->index(i, flightDataModel::CONDITION_PARAMS0); + index = myModel->index(i, flightDataModel::CONDITION_PARAMS0); 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(); - index = myModel->index(i, flightDataModel::CONDITION_PARAMS2); + index = myModel->index(i, flightDataModel::CONDITION_PARAMS2); 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(); - index = myModel->index(i, flightDataModel::COMMAND); + index = myModel->index(i, flightDataModel::COMMAND); 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; - index = myModel->index(i, flightDataModel::ERRORDESTINATION); + index = myModel->index(i, flightDataModel::ERRORDESTINATION); 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); + myModel->setData(index, true); index = myModel->index(i, flightDataModel::COMMAND); @@ -456,16 +464,17 @@ void ModelUavoProxy::pathActionToModel(int i, PathAction::DataFields &data) { myModel->setData(index, data.ModeParameters[3]); } -quint8 ModelUavoProxy::computePathPlanCrc(int waypointCount, int actionCount) { +quint8 ModelUavoProxy::computePathPlanCrc(int waypointCount, int actionCount) +{ quint8 crc = 0; + for (int i = 0; i < waypointCount; ++i) { - Waypoint* waypoint = Waypoint::GetInstance(objMngr, i); + Waypoint *waypoint = Waypoint::GetInstance(objMngr, i); crc = waypoint->updateCRC(crc); } for (int i = 0; i < actionCount; ++i) { - PathAction* action = PathAction::GetInstance(objMngr, i); + PathAction *action = PathAction::GetInstance(objMngr, i); crc = action->updateCRC(crc); } return crc; } - diff --git a/ground/openpilotgcs/src/plugins/opmap/modeluavoproxy.h b/ground/openpilotgcs/src/plugins/opmap/modeluavoproxy.h index d62d67746..e1f26262f 100644 --- a/ground/openpilotgcs/src/plugins/opmap/modeluavoproxy.h +++ b/ground/openpilotgcs/src/plugins/opmap/modeluavoproxy.h @@ -58,7 +58,7 @@ private: Waypoint *createWaypoint(int index, Waypoint *newWaypoint); 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 modelToPathAction(int i, PathAction::DataFields &data); diff --git a/ground/openpilotgcs/src/plugins/uavobjects/uavobject.cpp b/ground/openpilotgcs/src/plugins/uavobjects/uavobject.cpp index 275a9708b..90a42be9e 100644 --- a/ground/openpilotgcs/src/plugins/uavobjects/uavobject.cpp +++ b/ground/openpilotgcs/src/plugins/uavobjects/uavobject.cpp @@ -55,13 +55,13 @@ using namespace Utils; */ UAVObject::UAVObject(quint32 objID, bool isSingleInst, const QString & name) { - this->objID = objID; - this->instID = 0; + this->objID = objID; + this->instID = 0; this->isSingleInst = isSingleInst; - this->name = name; - this->data = 0; - this->numBytes = 0; - this->mutex = new QMutex(QMutex::Recursive); + this->name = name; + this->data = 0; + this->numBytes = 0; + this->mutex = new QMutex(QMutex::Recursive); } /** @@ -215,7 +215,7 @@ void UAVObject::updatedAll() if (instID == 0) { emit objectUpdatedManual(this, true); // 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 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; } @@ -334,8 +334,8 @@ quint8 UAVObject::updateCRC(quint8 crc) { QMutexLocker locker(mutex); - //crc = Crc::updateCRC(crc, (quint8 *) &objID, sizeof(objID)); - //crc = Crc::updateCRC(crc, (quint8 *) &instID, sizeof(instID)); + // crc = Crc::updateCRC(crc, (quint8 *) &objID, sizeof(objID)); + // crc = Crc::updateCRC(crc, (quint8 *) &instID, sizeof(instID)); crc = Crc::updateCRC(crc, data, numBytes); return crc; diff --git a/ground/openpilotgcs/src/plugins/uavobjectutil/uavobjectutilmanager.cpp b/ground/openpilotgcs/src/plugins/uavobjectutil/uavobjectutilmanager.cpp index 31435fca6..5b3b402fe 100644 --- a/ground/openpilotgcs/src/plugins/uavobjectutil/uavobjectutilmanager.cpp +++ b/ground/openpilotgcs/src/plugins/uavobjectutil/uavobjectutilmanager.cpp @@ -257,6 +257,7 @@ int UAVObjectUtilManager::getBoardModel() FirmwareIAPObj::DataFields firmwareIapData = getFirmwareIap(); int ret = firmwareIapData.BoardType << 8; + ret = ret + firmwareIapData.BoardRevision; return ret; diff --git a/ground/openpilotgcs/src/plugins/uavobjectwidgetutils/configtaskwidget.cpp b/ground/openpilotgcs/src/plugins/uavobjectwidgetutils/configtaskwidget.cpp index b9317114c..70aac4163 100644 --- a/ground/openpilotgcs/src/plugins/uavobjectwidgetutils/configtaskwidget.cpp +++ b/ground/openpilotgcs/src/plugins/uavobjectwidgetutils/configtaskwidget.cpp @@ -1230,6 +1230,7 @@ void ConfigTaskWidget::loadWidgetLimits(QWidget *widget, UAVObjectField *field, void ConfigTaskWidget::updateEnableControls() { TelemetryManager *telMngr = pm->getObject(); + Q_ASSERT(telMngr); enableControls(telMngr->isConnected()); diff --git a/ground/openpilotgcs/src/plugins/uavtalk/telemetry.cpp b/ground/openpilotgcs/src/plugins/uavtalk/telemetry.cpp index 6ce88acbe..059d988b1 100644 --- a/ground/openpilotgcs/src/plugins/uavtalk/telemetry.cpp +++ b/ground/openpilotgcs/src/plugins/uavtalk/telemetry.cpp @@ -230,6 +230,7 @@ void Telemetry::transactionCompleted(UAVObject *obj, bool success) { // Lookup the transaction in the transaction map. ObjectTransactionInfo *transInfo = findTransaction(obj); + if (transInfo) { if (success) { #ifdef VERBOSE_TELEMETRY @@ -264,17 +265,17 @@ void Telemetry::transactionTimeout(ObjectTransactionInfo *transInfo) #endif ++txRetries; --transInfo->retriesRemaining; - + // Retry the transaction processObjectTransaction(transInfo); } else { qWarning().nospace() << "Telemetry - !!! transaction timed out for object " << transInfo->obj->toStringBrief(); ++txErrors; - + // Terminate transaction utalk->cancelTransaction(transInfo->obj); - + // Remove this transaction as it's complete. UAVObject *obj = transInfo->obj; closeTransaction(transInfo); @@ -294,6 +295,7 @@ void Telemetry::processObjectTransaction(ObjectTransactionInfo *transInfo) { // Initiate transaction bool sent = false; + if (transInfo->objRequest) { #ifdef VERBOSE_TELEMETRY 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) { // Start timer if a response is expected transInfo->timer->start(REQ_TIMEOUT_MS); - } - else { + } else { // 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 - //transactionCompleted(transInfo->obj, false); + // transactionCompleted(transInfo->obj, false); } - } - else { + } else { // not transacted, so just close the transaction with no notification of completion closeTransaction(transInfo); } @@ -378,8 +378,8 @@ void Telemetry::processObjectQueue() if (gcsStats.Status != GCSTelemetryStats::STATUS_CONNECTED) { objQueue.clear(); if ((objInfo.obj->getObjID() != GCSTelemetryStats::OBJID) && - (objInfo.obj->getObjID() != OPLinkSettings::OBJID) && - (objInfo.obj->getObjID() != ObjectPersistence::OBJID)) { + (objInfo.obj->getObjID() != OPLinkSettings::OBJID) && + (objInfo.obj->getObjID() != ObjectPersistence::OBJID)) { objInfo.obj->emitTransactionCompleted(false); return; } @@ -396,7 +396,7 @@ void Telemetry::processObjectQueue() // TODO make the above logic a reality... if (findTransaction(objInfo.obj)) { 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; } UAVObject::Metadata metadata = objInfo.obj->getMetadata(); @@ -466,7 +466,7 @@ void Telemetry::processPeriodicUpdates() time.start(); allInstances = !objinfo->obj->isSingleInstance(); processObjectUpdates(objinfo->obj, EV_UPDATED_PERIODIC, allInstances, false); - elapsedMs = time.elapsed(); + elapsedMs = time.elapsed(); // Update timeToNextUpdateMs with the elapsed delay of sending the object; timeToNextUpdateMs += elapsedMs; } @@ -537,6 +537,7 @@ void Telemetry::objectUpdatedManual(UAVObject *obj, bool all) QMutexLocker locker(mutex); bool allInstances = obj->isSingleInstance() ? false : all; + processObjectUpdates(obj, EV_UPDATED_MANUAL, allInstances, true); } @@ -559,6 +560,7 @@ void Telemetry::updateRequested(UAVObject *obj, bool all) QMutexLocker locker(mutex); bool allInstances = obj->isSingleInstance() ? false : all; + processObjectUpdates(obj, EV_UPDATE_REQ, allInstances, true); } @@ -578,7 +580,7 @@ void Telemetry::newInstance(UAVObject *obj) ObjectTransactionInfo *Telemetry::findTransaction(UAVObject *obj) { - quint32 objId = obj->getObjID(); + quint32 objId = obj->getObjID(); quint16 instId = obj->getInstID(); // Lookup the transaction in the transaction map @@ -596,7 +598,7 @@ ObjectTransactionInfo *Telemetry::findTransaction(UAVObject *obj) 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(); QMap *objTransactions = transMap.value(objId); @@ -609,7 +611,7 @@ void Telemetry::openTransaction(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(); QMap *objTransactions = transMap.value(objId); @@ -627,6 +629,7 @@ void Telemetry::closeAllTransactions() QMap *objTransactions = transMap.value(objId); foreach(quint32 instId, objTransactions->keys()) { ObjectTransactionInfo *trans = objTransactions->value(instId); + qWarning() << "Telemetry - closing active transaction for object" << trans->obj->toStringBrief(); objTransactions->remove(instId); delete trans; diff --git a/ground/openpilotgcs/src/plugins/uavtalk/telemetrymanager.cpp b/ground/openpilotgcs/src/plugins/uavtalk/telemetrymanager.cpp index ad3b3db16..d0865337c 100644 --- a/ground/openpilotgcs/src/plugins/uavtalk/telemetrymanager.cpp +++ b/ground/openpilotgcs/src/plugins/uavtalk/telemetrymanager.cpp @@ -43,8 +43,7 @@ TelemetryManager::TelemetryManager() : autopilotConnected(false) } TelemetryManager::~TelemetryManager() -{ -} +{} bool TelemetryManager::isConnected() { @@ -76,8 +75,7 @@ void TelemetryManager::onStart() connect(device, SIGNAL(readyRead()), reader, SLOT(read())); // start the reader thread readerThread.start(); - } - else { + } else { // Connect IO device to reader connect(device, SIGNAL(readyRead()), utalk, SLOT(processInputStream())); } @@ -127,8 +125,7 @@ void TelemetryManager::onTelemetryUpdate(double txRate, double rxRate) } IODeviceReader::IODeviceReader(UAVTalk *uavTalk) : uavTalk(uavTalk) -{ -} +{} void IODeviceReader::read() { diff --git a/ground/openpilotgcs/src/plugins/uavtalk/telemetrymanager.h b/ground/openpilotgcs/src/plugins/uavtalk/telemetrymanager.h index fd7745c68..0e32fcf55 100644 --- a/ground/openpilotgcs/src/plugins/uavtalk/telemetrymanager.h +++ b/ground/openpilotgcs/src/plugins/uavtalk/telemetrymanager.h @@ -72,8 +72,7 @@ private: }; -class IODeviceReader : public QObject -{ +class IODeviceReader : public QObject { Q_OBJECT public: IODeviceReader(UAVTalk *uavTalk); diff --git a/ground/openpilotgcs/src/plugins/uavtalk/telemetrymonitor.cpp b/ground/openpilotgcs/src/plugins/uavtalk/telemetrymonitor.cpp index f0068c6cf..ad1148654 100644 --- a/ground/openpilotgcs/src/plugins/uavtalk/telemetrymonitor.cpp +++ b/ground/openpilotgcs/src/plugins/uavtalk/telemetrymonitor.cpp @@ -199,16 +199,16 @@ void TelemetryMonitor::processStatsUpdates() tel->resetStats(); // Update stats object - gcsStats.TxDataRate = (float)telStats.txBytes / ((float)statsTimer->interval() / 1000.0); - gcsStats.TxBytes += telStats.txBytes; - gcsStats.TxFailures += telStats.txErrors; - gcsStats.TxRetries += telStats.txRetries; + gcsStats.TxDataRate = (float)telStats.txBytes / ((float)statsTimer->interval() / 1000.0); + gcsStats.TxBytes += telStats.txBytes; + gcsStats.TxFailures += telStats.txErrors; + gcsStats.TxRetries += telStats.txRetries; - gcsStats.RxDataRate = (float)telStats.rxBytes / ((float)statsTimer->interval() / 1000.0); - gcsStats.RxBytes += telStats.rxBytes; - gcsStats.RxFailures += telStats.rxErrors; + gcsStats.RxDataRate = (float)telStats.rxBytes / ((float)statsTimer->interval() / 1000.0); + gcsStats.RxBytes += telStats.rxBytes; + gcsStats.RxFailures += telStats.rxErrors; gcsStats.RxSyncErrors += telStats.rxSyncErrors; - gcsStats.RxCrcErrors += telStats.rxCrcErrors; + gcsStats.RxCrcErrors += telStats.rxCrcErrors; // Check for a connection timeout bool connectionTimeout; diff --git a/ground/openpilotgcs/src/plugins/uavtalk/uavtalk.cpp b/ground/openpilotgcs/src/plugins/uavtalk/uavtalk.cpp index 1c7a33322..08ac75935 100644 --- a/ground/openpilotgcs/src/plugins/uavtalk/uavtalk.cpp +++ b/ground/openpilotgcs/src/plugins/uavtalk/uavtalk.cpp @@ -35,8 +35,8 @@ #ifdef VERBOSE_UAVTALK // uncomment and adapt the following lines to filter verbose logging to include specific object(s) only -//#include "flighttelemetrystats.h" -//#define VERBOSE_FILTER(objId) if (objId == FlightTelemetryStats::OBJID) +// #include "flighttelemetrystats.h" +// #define VERBOSE_FILTER(objId) if (objId == FlightTelemetryStats::OBJID) #endif #ifndef VERBOSE_FILTER #define VERBOSE_FILTER(objId) @@ -124,9 +124,8 @@ bool UAVTalk::sendObject(UAVObject *obj, bool acked, bool allInstances) if (allInstances) { instId = ALL_INSTANCES; - } - else if (obj) { - instId = obj->getInstID(); + } else if (obj) { + instId = obj->getInstID(); } bool success = false; if (acked) { @@ -153,9 +152,8 @@ bool UAVTalk::sendObjectRequest(UAVObject *obj, bool allInstances) if (allInstances) { instId = ALL_INSTANCES; - } - else if (obj) { - instId = obj->getInstID(); + } else if (obj) { + instId = obj->getInstID(); } 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) { - Q_ASSERT(obj); + Q_ASSERT(obj); // Send object depending on if a response is needed // transactions of TYPE_OBJ_REQ are acked by the response if (type == TYPE_OBJ_ACK || type == TYPE_OBJ_REQ) { @@ -217,8 +215,7 @@ void UAVTalk::processInputStream() int ret = io->read((char *)&tmp, 1); if (ret != -1) { processInputByte(tmp); - } - else { + } else { // TODOD } if (rxState == STATE_COMPLETE) { @@ -226,8 +223,7 @@ void UAVTalk::processInputStream() if (receiveObject(rxType, rxObjId, rxInstId, rxBuffer, rxLength)) { stats.rxObjectBytes += rxLength; stats.rxObjects++; - } - else { + } else { // TODO... } mutex.unlock(); @@ -237,7 +233,6 @@ void UAVTalk::processInputStream() // accessed from this thread only udpSocketTx->writeDatagram(rxDataArray, QHostAddress::LocalHost, udpSocketRx->localPort()); } - } } } @@ -319,7 +314,7 @@ bool UAVTalk::processInputByte(quint8 rxbyte) break; } packetSize += (quint32)rxbyte << 8; - rxCount = 0; + rxCount = 0; if (packetSize < HEADER_LENGTH || packetSize > HEADER_LENGTH + MAX_PAYLOAD_LENGTH) { @@ -342,13 +337,13 @@ bool UAVTalk::processInputByte(quint8 rxbyte) if (rxCount < 4) { break; } - rxCount = 0; + rxCount = 0; - rxObjId = (qint32)qFromLittleEndian(rxTmpBuffer); + rxObjId = (qint32)qFromLittleEndian(rxTmpBuffer); // Message always contain an instance ID rxInstId = 0; - rxState = STATE_INSTID; + rxState = STATE_INSTID; break; case STATE_INSTID: @@ -402,7 +397,6 @@ bool UAVTalk::processInputByte(quint8 rxbyte) rxState = STATE_ERROR; break; } - } // 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->respInstId == ALL_INSTANCES) { if (instId == 0) { - // last instance received, complete transaction + // last instance received, complete transaction closeTransaction(trans); emit transactionCompleted(obj, true); + } else { + // TODO extend timeout? } - else { - // TODO extend timeout? - } - } - else { + } else { closeTransaction(trans); emit transactionCompleted(obj, true); } @@ -713,7 +705,7 @@ bool UAVTalk::transmitObject(quint8 type, quint32 objId, quint16 instId, UAVObje ret = true; quint32 numInst = objMngr->getNumInstances(objId); for (quint32 n = 0; n < numInst; ++n) { - quint32 i = numInst - n - 1; + quint32 i = numInst - n - 1; UAVObject *o = objMngr->getObject(objId, i); if (!transmitSingleObject(type, objId, i, o)) { ret = false; @@ -800,13 +792,11 @@ bool UAVTalk::transmitSingleObject(quint8 type, quint32 objId, quint16 instId, U if (useUDPMirror) { udpSocketRx->writeDatagram((const char *)txBuffer, HEADER_LENGTH + length + CHECKSUM_LENGTH, QHostAddress::LocalHost, udpSocketTx->localPort()); } - } - else { + } else { qWarning() << "UAVTalk - error transmitting : io device full"; ++stats.txErrors; return false; } - } else { qWarning() << "UAVTalk - error transmitting : io device not writable"; ++stats.txErrors; @@ -840,8 +830,9 @@ UAVTalk::Transaction *UAVTalk::findTransaction(quint32 objId, quint16 instId) void UAVTalk::openTransaction(quint8 type, quint32 objId, quint16 instId) { 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; QMap *objTransactions = transMap.value(trans->respObjId); @@ -869,6 +860,7 @@ void UAVTalk::closeAllTransactions() QMap *objTransactions = transMap.value(objId); foreach(quint32 instId, objTransactions->keys()) { Transaction *trans = objTransactions->value(instId); + qWarning() << "UAVTalk - closing active transaction for object" << trans->respObjId; objTransactions->remove(instId); delete trans; @@ -878,29 +870,33 @@ void UAVTalk::closeAllTransactions() } } -const char* UAVTalk::typeToString(quint8 type) +const char *UAVTalk::typeToString(quint8 type) { switch (type) { case TYPE_OBJ: return "object"; + break; case TYPE_OBJ_ACK: return "object (acked)"; + break; case TYPE_OBJ_REQ: return "object request"; + break; case TYPE_ACK: return "ack"; + break; case TYPE_NACK: return "nack"; - break; + break; } return ""; } diff --git a/ground/openpilotgcs/src/plugins/uavtalk/uavtalk.h b/ground/openpilotgcs/src/plugins/uavtalk/uavtalk.h index fe5922096..4d27399f9 100644 --- a/ground/openpilotgcs/src/plugins/uavtalk/uavtalk.h +++ b/ground/openpilotgcs/src/plugins/uavtalk/uavtalk.h @@ -44,7 +44,7 @@ class UAVTALK_EXPORT UAVTalk : public QObject { friend class IODeviceReader; public: - static const quint16 ALL_INSTANCES = 0xFFFF; + static const quint16 ALL_INSTANCES = 0xFFFF; typedef struct { quint32 txBytes; @@ -80,22 +80,22 @@ private slots: private: typedef struct { - quint8 respType; + quint8 respType; quint32 respObjId; quint16 respInstId; } Transaction; // Constants - static const int TYPE_MASK = 0xF8; - static const int TYPE_VER = 0x20; - static const int TYPE_OBJ = (TYPE_VER | 0x00); - static const int TYPE_OBJ_REQ = (TYPE_VER | 0x01); - static const int TYPE_OBJ_ACK = (TYPE_VER | 0x02); - static const int TYPE_ACK = (TYPE_VER | 0x03); - static const int TYPE_NACK = (TYPE_VER | 0x04); + static const int TYPE_MASK = 0xF8; + static const int TYPE_VER = 0x20; + static const int TYPE_OBJ = (TYPE_VER | 0x00); + static const int TYPE_OBJ_REQ = (TYPE_VER | 0x01); + static const int TYPE_OBJ_ACK = (TYPE_VER | 0x02); + static const int TYPE_ACK = (TYPE_VER | 0x03); + static const int TYPE_NACK = (TYPE_VER | 0x04); // 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;