rooty/LibrePilot
1
0
Fork 0
mirror of https://bitbucket.org/librepilot/librepilot.git synced 2025-02-26 15: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;
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;
}

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

@ -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);

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

@ -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;
}

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;
}
// 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:

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

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

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

@ -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;

49
flight/uavtalk/uavtalk.c
View File

@ -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;

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

@ -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

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)
{
TelemetryManager *telMngr = qobject_cast<TelemetryManager *>(obj);
if (telMngr) {
connect(telMngr, SIGNAL(disconnected()), this, SLOT(onAutopilotDisconnect()));
}

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

@ -30,8 +30,7 @@
#include <QDomDocument>
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();

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

@ -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<UAVObjectManager>();
objMngr = pm->getObject<UAVObjectManager>();
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;
}

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

@ -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);

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)
{
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;

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

@ -257,6 +257,7 @@ int UAVObjectUtilManager::getBoardModel()
FirmwareIAPObj::DataFields firmwareIapData = getFirmwareIap();
int ret = firmwareIapData.BoardType << 8;
ret = ret + firmwareIapData.BoardRevision;
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()
{
TelemetryManager *telMngr = pm->getObject<TelemetryManager>();
Q_ASSERT(telMngr);
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.
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<quint32, ObjectTransactionInfo *> *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<quint32, ObjectTransactionInfo *> *objTransactions = transMap.value(objId);
@ -627,6 +629,7 @@ void Telemetry::closeAllTransactions()
QMap<quint32, ObjectTransactionInfo *> *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;

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

@ -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()
{

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
public:
IODeviceReader(UAVTalk *uavTalk);

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

@ -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;

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

@ -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<quint32>(rxTmpBuffer);
rxObjId = (qint32)qFromLittleEndian<quint32>(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<quint32, Transaction *> *objTransactions = transMap.value(trans->respObjId);
@ -869,6 +860,7 @@ void UAVTalk::closeAllTransactions()
QMap<quint32, Transaction *> *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 "<error>";
}

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

@ -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;