rooty/LibrePilot
1
0
Fork 0
mirror of https://bitbucket.org/librepilot/librepilot.git synced 2024-11-30 08:24:11 +01:00
Code Issues Releases Activity

Merge remote-tracking branch 'origin/filnet/OP-1122_gcs_waypoint_editor_stabilization' into next

Browse Source
This commit is contained in:
Philippe Renon 2014-01-15 20:32:04 +01:00
commit dd32fc6612
75 changed files with 2677 additions and 1645 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

13
flight/libraries/sanitycheck.c
View File

@ -35,6 +35,7 @@
// UAVOs
#include <manualcontrolsettings.h>
#include <systemsettings.h>
#include <systemalarms.h>
#include <taskinfo.h>
/****************************
@ -151,9 +152,15 @@ int32_t configuration_check()
case MANUALCONTROLSETTINGS_FLIGHTMODEPOSITION_PATHPLANNER:
if (coptercontrol) {
severity = SYSTEMALARMS_ALARM_ERROR;
} else if (!PIOS_TASK_MONITOR_IsRunning(TASKINFO_RUNNING_PATHFOLLOWER)) {
// Revo supports PathPlanner
severity = SYSTEMALARMS_ALARM_ERROR;
} else {
// Revo supports PathPlanner and that must be OK or we are not sane
// PathPlan alarm is uninitialized if not running
// PathPlan alarm is warning or error if the flightplan is invalid
SystemAlarmsAlarmData alarms;
SystemAlarmsAlarmGet(&alarms);
if (alarms.PathPlan != SYSTEMALARMS_ALARM_OK) {
severity = SYSTEMALARMS_ALARM_ERROR;
}
}
break;
case MANUALCONTROLSETTINGS_FLIGHTMODEPOSITION_RETURNTOBASE:

1
flight/modules/FlightPlan/flightplan.c
View File

@ -31,7 +31,6 @@
#include <openpilot.h>
#include "flightplan.h"
#include "flightplanstatus.h"
#include "flightplancontrol.h"
#include "flightplansettings.h"

36
flight/modules/FlightPlan/inc/flightplan.h
View File

@ -1,36 +0,0 @@
/**
******************************************************************************
* @addtogroup OpenPilotModules OpenPilot Modules
* @{
* @addtogroup FlightPlan Flight Plan Module
* @brief Executes flight plan scripts in Python
* @{
*
* @file flightplan.c
* @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2010.
* @brief Executes flight plan scripts in Python
*
* @see The GNU Public License (GPL) Version 3
*
*****************************************************************************/
/*
* This program is free software; you can redistribute it and/or modify
* it under the terms of the GNU General Public License as published by
* the Free Software Foundation; either version 3 of the License, or
* (at your option) any later version.
*
* This program is distributed in the hope that it will be useful, but
* WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY
* or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License
* for more details.
*
* You should have received a copy of the GNU General Public License along
* with this program; if not, write to the Free Software Foundation, Inc.,
* 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA
*/
#ifndef FLIGHTPLAN_H
#define FLIGHTPLAN_H
int32_t FlightPlanInitialize();
#endif // FLIGHTPLAN_H

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

@ -31,6 +31,7 @@
#include "openpilot.h"
#include "pathplan.h"
#include "flightstatus.h"
#include "airspeedstate.h"
#include "pathaction.h"
@ -40,23 +41,26 @@
#include "velocitystate.h"
#include "waypoint.h"
#include "waypointactive.h"
#include "taskinfo.h"
#include "manualcontrolsettings.h"
#include <pios_struct_helper.h>
#include "paths.h"
// Private constants
#define STACK_SIZE_BYTES 1024
#define TASK_PRIORITY (tskIDLE_PRIORITY + 1)
#define TASK_PRIORITY CALLBACK_TASK_NAVIGATION
#define MAX_QUEUE_SIZE 2
#define PATH_PLANNER_UPDATE_RATE_MS 20
#define PATH_PLANNER_UPDATE_RATE_MS 100 // can be slow, since we listen to status updates as well
// Private types
// Private functions
static void pathPlannerTask(void *parameters);
static void updatePathDesired(UAVObjEvent *ev);
static void pathPlannerTask();
static void commandUpdated(UAVObjEvent *ev);
static void statusUpdated(UAVObjEvent *ev);
static void updatePathDesired();
static void setWaypoint(uint16_t num);
static uint8_t checkPathPlan();
static uint8_t pathConditionCheck();
static uint8_t conditionNone();
static uint8_t conditionTimeOut();
@ -71,7 +75,8 @@ static uint8_t conditionImmediate();
// Private variables
static xTaskHandle taskHandle;
static DelayedCallbackInfo *pathPlannerHandle;
static DelayedCallbackInfo *pathDesiredUpdaterHandle;
static WaypointActiveData waypointActive;
static WaypointData waypoint;
static PathActionData pathAction;
@ -83,11 +88,14 @@ static bool pathplanner_active = false;
*/
int32_t PathPlannerStart()
{
taskHandle = NULL;
// when the active waypoint changes, update pathDesired
WaypointConnectCallback(commandUpdated);
WaypointActiveConnectCallback(commandUpdated);
PathActionConnectCallback(commandUpdated);
PathStatusConnectCallback(statusUpdated);
// Start VM thread
xTaskCreate(pathPlannerTask, (signed char *)"PathPlanner", STACK_SIZE_BYTES / 4, NULL, TASK_PRIORITY, &taskHandle);
PIOS_TASK_MONITOR_RegisterTask(TASKINFO_RUNNING_PATHPLANNER, taskHandle);
// Start main task callback
DelayedCallbackDispatch(pathPlannerHandle);
return 0;
}
@ -97,8 +105,7 @@ int32_t PathPlannerStart()
*/
int32_t PathPlannerInitialize()
{
taskHandle = NULL;
PathPlanInitialize();
PathActionInitialize();
PathStatusInitialize();
PathDesiredInitialize();
@ -108,6 +115,9 @@ int32_t PathPlannerInitialize()
WaypointInitialize();
WaypointActiveInitialize();
pathPlannerHandle = DelayedCallbackCreate(&pathPlannerTask, CALLBACK_PRIORITY_REGULAR, TASK_PRIORITY, STACK_SIZE_BYTES);
pathDesiredUpdaterHandle = DelayedCallbackCreate(&updatePathDesired, CALLBACK_PRIORITY_CRITICAL, TASK_PRIORITY, STACK_SIZE_BYTES);
return 0;
}
@ -116,129 +126,250 @@ MODULE_INITCALL(PathPlannerInitialize, PathPlannerStart);
/**
* Module task
*/
static void pathPlannerTask(__attribute__((unused)) void *parameters)
static void pathPlannerTask()
{
// when the active waypoint changes, update pathDesired
WaypointConnectCallback(updatePathDesired);
WaypointActiveConnectCallback(updatePathDesired);
PathActionConnectCallback(updatePathDesired);
DelayedCallbackSchedule(pathPlannerHandle, PATH_PLANNER_UPDATE_RATE_MS, CALLBACK_UPDATEMODE_SOONER);
bool endCondition = false;
// check path plan validity early to raise alarm
// even if not in guided mode
uint8_t validPathPlan = checkPathPlan();
FlightStatusData flightStatus;
FlightStatusGet(&flightStatus);
if (flightStatus.FlightMode != FLIGHTSTATUS_FLIGHTMODE_PATHPLANNER) {
pathplanner_active = false;
if (!validPathPlan) {
// unverified path plans are only a warning while we are not in pathplanner mode
// so it does not prevent arming. However manualcontrols safety check
// shall test for this warning when pathplan is on the flight mode selector
// thus a valid flight plan is a prerequirement for arming
AlarmsSet(SYSTEMALARMS_ALARM_PATHPLAN, SYSTEMALARMS_ALARM_WARNING);
} else {
AlarmsClear(SYSTEMALARMS_ALARM_PATHPLAN);
}
return;
}
PathDesiredData pathDesired;
PathDesiredGet(&pathDesired);
static uint8_t failsafeRTHset = 0;
if (!validPathPlan) {
// At this point the craft is in PathPlanner mode, so pilot has no manual control capability.
// Failsafe: behave as if in return to base mode
// what to do in this case is debatable, it might be better to just
// make a forced disarming but rth has a higher chance of survival when
// in flight.
pathplanner_active = false;
if (!failsafeRTHset) {
failsafeRTHset = 1;
// copy pasta: same calculation as in manualcontrol, set return to home coordinates
PositionStateData positionState;
PositionStateGet(&positionState);
ManualControlSettingsData settings;
ManualControlSettingsGet(&settings);
pathDesired.Start.North = 0;
pathDesired.Start.East = 0;
pathDesired.Start.Down = positionState.Down - settings.ReturnToHomeAltitudeOffset;
pathDesired.End.North = 0;
pathDesired.End.East = 0;
pathDesired.End.Down = positionState.Down - settings.ReturnToHomeAltitudeOffset;
pathDesired.StartingVelocity = 1;
pathDesired.EndingVelocity = 0;
pathDesired.Mode = PATHDESIRED_MODE_FLYENDPOINT;
PathDesiredSet(&pathDesired);
}
AlarmsSet(SYSTEMALARMS_ALARM_PATHPLAN, SYSTEMALARMS_ALARM_ERROR);
return;
}
failsafeRTHset = 0;
AlarmsClear(SYSTEMALARMS_ALARM_PATHPLAN);
WaypointActiveGet(&waypointActive);
if (pathplanner_active == false) {
pathplanner_active = true;
// This triggers callback to update variable
waypointActive.Index = 0;
WaypointActiveSet(&waypointActive);
return;
}
WaypointInstGet(waypointActive.Index, &waypoint);
PathActionInstGet(waypoint.Action, &pathAction);
PathStatusData pathStatus;
PathStatusGet(&pathStatus);
// Main thread loop
bool endCondition = false;
while (1) {
vTaskDelay(PATH_PLANNER_UPDATE_RATE_MS);
// delay next step until path follower has acknowledged the path mode
if (pathStatus.UID != pathDesired.UID) {
return;
}
FlightStatusGet(&flightStatus);
if (flightStatus.FlightMode != FLIGHTSTATUS_FLIGHTMODE_PATHPLANNER) {
pathplanner_active = false;
continue;
// negative destinations DISABLE this feature
if (pathStatus.Status == PATHSTATUS_STATUS_CRITICAL && waypointActive.Index != pathAction.ErrorDestination && pathAction.ErrorDestination >= 0) {
setWaypoint(pathAction.ErrorDestination);
return;
}
// check if condition has been met
endCondition = pathConditionCheck();
// decide what to do
switch (pathAction.Command) {
case PATHACTION_COMMAND_ONNOTCONDITIONNEXTWAYPOINT:
endCondition = !endCondition;
case PATHACTION_COMMAND_ONCONDITIONNEXTWAYPOINT:
if (endCondition) {
setWaypoint(waypointActive.Index + 1);
}
WaypointActiveGet(&waypointActive);
if (pathplanner_active == false) {
pathplanner_active = true;
// This triggers callback to update variable
waypointActive.Index = 0;
WaypointActiveSet(&waypointActive);
continue;
}
WaypointInstGet(waypointActive.Index, &waypoint);
PathActionInstGet(waypoint.Action, &pathAction);
PathStatusGet(&pathStatus);
PathDesiredGet(&pathDesired);
// delay next step until path follower has acknowledged the path mode
if (pathStatus.UID != pathDesired.UID) {
continue;
}
// negative destinations DISABLE this feature
if (pathStatus.Status == PATHSTATUS_STATUS_CRITICAL && waypointActive.Index != pathAction.ErrorDestination && pathAction.ErrorDestination >= 0) {
setWaypoint(pathAction.ErrorDestination);
continue;
}
// check if condition has been met
endCondition = pathConditionCheck();
// decide what to do
switch (pathAction.Command) {
case PATHACTION_COMMAND_ONNOTCONDITIONNEXTWAYPOINT:
endCondition = !endCondition;
case PATHACTION_COMMAND_ONCONDITIONNEXTWAYPOINT:
if (endCondition) {
setWaypoint(waypointActive.Index + 1);
}
break;
case PATHACTION_COMMAND_ONNOTCONDITIONJUMPWAYPOINT:
endCondition = !endCondition;
case PATHACTION_COMMAND_ONCONDITIONJUMPWAYPOINT:
if (endCondition) {
if (pathAction.JumpDestination < 0) {
// waypoint ids <0 code relative jumps
setWaypoint(waypointActive.Index - pathAction.JumpDestination);
} else {
setWaypoint(pathAction.JumpDestination);
}
}
break;
case PATHACTION_COMMAND_IFCONDITIONJUMPWAYPOINTELSENEXTWAYPOINT:
if (endCondition) {
if (pathAction.JumpDestination < 0) {
// waypoint ids <0 code relative jumps
setWaypoint(waypointActive.Index - pathAction.JumpDestination);
} else {
setWaypoint(pathAction.JumpDestination);
}
break;
case PATHACTION_COMMAND_ONNOTCONDITIONJUMPWAYPOINT:
endCondition = !endCondition;
case PATHACTION_COMMAND_ONCONDITIONJUMPWAYPOINT:
if (endCondition) {
if (pathAction.JumpDestination < 0) {
// waypoint ids <0 code relative jumps
setWaypoint(waypointActive.Index - pathAction.JumpDestination);
} else {
setWaypoint(waypointActive.Index + 1);
setWaypoint(pathAction.JumpDestination);
}
break;
}
break;
case PATHACTION_COMMAND_IFCONDITIONJUMPWAYPOINTELSENEXTWAYPOINT:
if (endCondition) {
if (pathAction.JumpDestination < 0) {
// waypoint ids <0 code relative jumps
setWaypoint(waypointActive.Index - pathAction.JumpDestination);
} else {
setWaypoint(pathAction.JumpDestination);
}
} else {
setWaypoint(waypointActive.Index + 1);
}
break;
}
}
// safety checks for path plan integrity
static uint8_t checkPathPlan()
{
uint16_t i;
uint16_t waypointCount;
uint16_t actionCount;
uint8_t pathCrc;
PathPlanData pathPlan;
// WaypointData waypoint; // using global instead (?)
// PathActionData action; // using global instead (?)
PathPlanGet(&pathPlan);
waypointCount = pathPlan.WaypointCount;
if (waypointCount == 0) {
// an empty path plan is invalid
return false;
}
actionCount = pathPlan.PathActionCount;
// check count consistency
if (waypointCount > UAVObjGetNumInstances(WaypointHandle())) {
// PIOS_DEBUGLOG_Printf("PathPlan : waypoint count error!");
return false;
}
if (actionCount > UAVObjGetNumInstances(PathActionHandle())) {
// PIOS_DEBUGLOG_Printf("PathPlan : path action count error!");
return false;
}
// check CRC
pathCrc = 0;
for (i = 0; i < waypointCount; i++) {
pathCrc = UAVObjUpdateCRC(WaypointHandle(), i, pathCrc);
}
for (i = 0; i < actionCount; i++) {
pathCrc = UAVObjUpdateCRC(PathActionHandle(), i, pathCrc);
}
if (pathCrc != pathPlan.Crc) {
// failed crc check
// PIOS_DEBUGLOG_Printf("PathPlan : bad CRC (%d / %d)!", pathCrc, pathPlan.Crc);
return false;
}
// waypoint consistency
for (i = 0; i < waypointCount; i++) {
WaypointInstGet(i, &waypoint);
if (waypoint.Action >= actionCount) {
// path action id is out of range
return false;
}
}
// path action consistency
for (i = 0; i < actionCount; i++) {
PathActionInstGet(i, &pathAction);
if (pathAction.ErrorDestination >= waypointCount) {
// waypoint id is out of range
return false;
}
if (pathAction.JumpDestination >= waypointCount) {
// waypoint id is out of range
return false;
}
}
// path plan passed checks
return true;
}
// callback function when status changed, issue execution of state machine
void commandUpdated(__attribute__((unused)) UAVObjEvent *ev)
{
DelayedCallbackDispatch(pathDesiredUpdaterHandle);
}
// callback function when waypoints changed in any way, update pathDesired
void updatePathDesired(__attribute__((unused)) UAVObjEvent *ev)
void statusUpdated(__attribute__((unused)) UAVObjEvent *ev)
{
DelayedCallbackDispatch(pathPlannerHandle);
}
// callback function when waypoints changed in any way, update pathDesired
void updatePathDesired()
{
// only ever touch pathDesired if pathplanner is enabled
if (!pathplanner_active) {
return;
}
// use local variables, dont use stack since this is huge and a callback,
// dont use the globals because we cant use mutexes here
static WaypointActiveData waypointActiveData;
static PathActionData pathActionData;
static WaypointData waypointData;
static PathDesiredData pathDesired;
PathDesiredData pathDesired;
// find out current waypoint
WaypointActiveGet(&waypointActiveData);
WaypointActiveGet(&waypointActive);
WaypointInstGet(waypointActiveData.Index, &waypointData);
PathActionInstGet(waypointData.Action, &pathActionData);
WaypointInstGet(waypointActive.Index, &waypoint);
PathActionInstGet(waypoint.Action, &pathAction);
pathDesired.End.North = waypointData.Position.North;
pathDesired.End.East = waypointData.Position.East;
pathDesired.End.Down = waypointData.Position.Down;
pathDesired.EndingVelocity = waypointData.Velocity;
pathDesired.Mode = pathActionData.Mode;
pathDesired.ModeParameters[0] = pathActionData.ModeParameters[0];
pathDesired.ModeParameters[1] = pathActionData.ModeParameters[1];
pathDesired.ModeParameters[2] = pathActionData.ModeParameters[2];
pathDesired.ModeParameters[3] = pathActionData.ModeParameters[3];
pathDesired.UID = waypointActiveData.Index;
pathDesired.End.North = waypoint.Position.North;
pathDesired.End.East = waypoint.Position.East;
pathDesired.End.Down = waypoint.Position.Down;
pathDesired.EndingVelocity = waypoint.Velocity;
pathDesired.Mode = pathAction.Mode;
pathDesired.ModeParameters[0] = pathAction.ModeParameters[0];
pathDesired.ModeParameters[1] = pathAction.ModeParameters[1];
pathDesired.ModeParameters[2] = pathAction.ModeParameters[2];
pathDesired.ModeParameters[3] = pathAction.ModeParameters[3];
pathDesired.UID = waypointActive.Index;
if (waypointActiveData.Index == 0) {
if (waypointActive.Index == 0) {
PositionStateData positionState;
PositionStateGet(&positionState);
// First waypoint has itself as start point (used to be home position but that proved dangerous when looping)
@ -266,8 +397,13 @@ void updatePathDesired(__attribute__((unused)) UAVObjEvent *ev)
// helper function to go to a specific waypoint
static void setWaypoint(uint16_t num)
{
// path plans wrap around
if (num >= UAVObjGetNumInstances(WaypointHandle())) {
PathPlanData pathPlan;
PathPlanGet(&pathPlan);
// here it is assumed that the path plan has been validated (waypoint count is consistent)
if (num >= pathPlan.WaypointCount) {
// path plans wrap around
num = 0;
}
@ -275,10 +411,10 @@ static void setWaypoint(uint16_t num)
WaypointActiveSet(&waypointActive);
}
// execute the apropriate condition and report result
// execute the appropriate condition and report result
static uint8_t pathConditionCheck()
{
// i thought about a lookup table, but a switch is saver considering there could be invalid EndCondition ID's
// i thought about a lookup table, but a switch is safer considering there could be invalid EndCondition ID's
switch (pathAction.EndCondition) {
case PATHACTION_ENDCONDITION_NONE:
return conditionNone();

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

@ -1,5 +1,6 @@
/**
******************************************************************************
* @addtogroup OpenPilotModules OpenPilot Modules
* @{
* @addtogroup RadioComBridgeModule Com Port to Radio Bridge Module
@ -36,6 +37,7 @@
#include <objectpersistence.h>
#include <oplinksettings.h>
#include <oplinkreceiver.h>
#include <radiocombridgestats.h>
#include <uavtalk_priv.h>
#include <pios_rfm22b.h>
#include <ecc.h>
@ -57,6 +59,7 @@
#define SERIAL_RX_BUF_LEN 100
#define PPM_INPUT_TIMEOUT 100
// ****************
// Private types
@ -81,10 +84,11 @@ typedef struct {
uint8_t serialRxBuf[SERIAL_RX_BUF_LEN];
// Error statistics.
uint32_t comTxErrors;
uint32_t comTxRetries;
uint32_t UAVTalkErrors;
uint32_t droppedPackets;
uint32_t telemetryTxRetries;
uint32_t radioTxRetries;
// Is this modem the coordinator
bool isCoordinator;
// Should we parse UAVTalk?
bool parseUAVTalk;
@ -107,6 +111,7 @@ static int32_t RadioSendHandler(uint8_t *buf, int32_t length);
static void ProcessTelemetryStream(UAVTalkConnection inConnectionHandle, UAVTalkConnection outConnectionHandle, uint8_t rxbyte);
static void ProcessRadioStream(UAVTalkConnection inConnectionHandle, UAVTalkConnection outConnectionHandle, uint8_t rxbyte);
static void objectPersistenceUpdatedCb(UAVObjEvent *objEv);
static void registerObject(UAVObjHandle obj);
// ****************
// Private variables
@ -124,12 +129,14 @@ static int32_t RadioComBridgeStart(void)
// Get the settings.
OPLinkSettingsData oplinkSettings;
OPLinkSettingsGet(&oplinkSettings);
bool is_coordinator = (oplinkSettings.Coordinator == OPLINKSETTINGS_COORDINATOR_TRUE);
// Check if this is the coordinator modem
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) {
@ -165,12 +172,16 @@ static int32_t RadioComBridgeStart(void)
// Configure our UAVObjects for updates.
UAVObjConnectQueue(UAVObjGetByID(OPLINKSTATUS_OBJID), data->uavtalkEventQueue, EV_UPDATED | EV_UPDATED_MANUAL | EV_UPDATE_REQ);
UAVObjConnectQueue(UAVObjGetByID(OBJECTPERSISTENCE_OBJID), data->uavtalkEventQueue, EV_UPDATED | EV_UPDATED_MANUAL);
if (is_coordinator) {
if (data->isCoordinator) {
UAVObjConnectQueue(UAVObjGetByID(OPLINKRECEIVER_OBJID), data->radioEventQueue, EV_UPDATED | EV_UPDATED_MANUAL | EV_UPDATE_REQ);
} else {
UAVObjConnectQueue(UAVObjGetByID(OPLINKRECEIVER_OBJID), data->uavtalkEventQueue, EV_UPDATED | EV_UPDATED_MANUAL | EV_UPDATE_REQ);
}
if (data->isCoordinator) {
registerObject(RadioComBridgeStatsHandle());
}
// Configure the UAVObject callbacks
ObjectPersistenceConnectCallback(&objectPersistenceUpdatedCb);
@ -223,27 +234,94 @@ static int32_t RadioComBridgeInitialize(void)
OPLinkStatusInitialize();
ObjectPersistenceInitialize();
OPLinkReceiverInitialize();
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->comTxErrors = 0;
data->comTxRetries = 0;
data->UAVTalkErrors = 0;
data->parseUAVTalk = true;
data->comSpeed = OPLINKSETTINGS_COMSPEED_9600;
PIOS_COM_RADIO = PIOS_COM_RFM22B;
data->telemetryTxRetries = 0;
data->radioTxRetries = 0;
data->parseUAVTalk = true;
data->comSpeed = OPLINKSETTINGS_COMSPEED_9600;
PIOS_COM_RADIO = PIOS_COM_RFM22B;
return 0;
}
MODULE_INITCALL(RadioComBridgeInitialize, RadioComBridgeStart);
// TODO this code (badly) duplicates code from telemetry.c
// This method should be used only for periodically updated objects.
// The register method defined in telemetry.c should be factored out in a shared location so it can be
// used from here...
static void registerObject(UAVObjHandle obj)
{
// Setup object for periodic updates
UAVObjEvent ev = {
.obj = obj,
.instId = UAVOBJ_ALL_INSTANCES,
.event = EV_UPDATED_PERIODIC,
};
// Get metadata
UAVObjMetadata metadata;
UAVObjGetMetadata(obj, &metadata);
EventPeriodicQueueCreate(&ev, data->uavtalkEventQueue, metadata.telemetryUpdatePeriod);
UAVObjConnectQueue(obj, data->uavtalkEventQueue, EV_UPDATED_PERIODIC | EV_UPDATED_MANUAL | EV_UPDATE_REQ);
}
/**
* Update telemetry statistics
*/
static void updateRadioComBridgeStats()
{
UAVTalkStats telemetryUAVTalkStats;
UAVTalkStats radioUAVTalkStats;
RadioComBridgeStatsData radioComBridgeStats;
// Get telemetry stats
UAVTalkGetStats(data->telemUAVTalkCon, &telemetryUAVTalkStats);
UAVTalkResetStats(data->telemUAVTalkCon);
// Get radio stats
UAVTalkGetStats(data->radioUAVTalkCon, &radioUAVTalkStats);
UAVTalkResetStats(data->radioUAVTalkCon);
// Get stats object data
RadioComBridgeStatsGet(&radioComBridgeStats);
// Update stats object
radioComBridgeStats.TelemetryTxBytes += telemetryUAVTalkStats.txBytes;
radioComBridgeStats.TelemetryTxFailures += telemetryUAVTalkStats.txErrors;
radioComBridgeStats.TelemetryTxRetries += data->telemetryTxRetries;
radioComBridgeStats.TelemetryRxBytes += telemetryUAVTalkStats.rxBytes;
radioComBridgeStats.TelemetryRxFailures += telemetryUAVTalkStats.rxErrors;
radioComBridgeStats.TelemetryRxSyncErrors += telemetryUAVTalkStats.rxSyncErrors;
radioComBridgeStats.TelemetryRxCrcErrors += telemetryUAVTalkStats.rxCrcErrors;
radioComBridgeStats.RadioTxBytes += radioUAVTalkStats.txBytes;
radioComBridgeStats.RadioTxFailures += radioUAVTalkStats.txErrors;
radioComBridgeStats.RadioTxRetries += data->radioTxRetries;
radioComBridgeStats.RadioRxBytes += radioUAVTalkStats.rxBytes;
radioComBridgeStats.RadioRxFailures += radioUAVTalkStats.rxErrors;
radioComBridgeStats.RadioRxSyncErrors += radioUAVTalkStats.rxSyncErrors;
radioComBridgeStats.RadioRxCrcErrors += radioUAVTalkStats.rxCrcErrors;
// Update stats object data
RadioComBridgeStatsSet(&radioComBridgeStats);
}
/**
* @brief Telemetry transmit task, regular priority
*
@ -260,18 +338,20 @@ static void telemetryTxTask(__attribute__((unused)) void *parameters)
#endif
// Wait for queue message
if (xQueueReceive(data->uavtalkEventQueue, &ev, MAX_PORT_DELAY) == pdTRUE) {
if ((ev.event == EV_UPDATED) || (ev.event == EV_UPDATE_REQ)) {
// Send update (with retries)
uint32_t retries = 0;
int32_t success = -1;
while (retries < MAX_RETRIES && success == -1) {
success = UAVTalkSendObject(data->telemUAVTalkCon, ev.obj, 0, 0, RETRY_TIMEOUT_MS) == 0;
if (!success) {
++retries;
}
}
data->comTxRetries += retries;
if (ev.obj == RadioComBridgeStatsHandle()) {
updateRadioComBridgeStats();
}
// Send update (with retries)
uint32_t retries = 0;
int32_t success = -1;
while (retries < MAX_RETRIES && success == -1) {
success = UAVTalkSendObject(data->telemUAVTalkCon, ev.obj, ev.instId, 0, RETRY_TIMEOUT_MS) == 0;
if (success == -1) {
++retries;
}
}
// Update stats
data->telemetryTxRetries += retries;
}
}
}
@ -299,12 +379,12 @@ static void radioTxTask(__attribute__((unused)) void *parameters)
uint32_t retries = 0;
int32_t success = -1;
while (retries < MAX_RETRIES && success == -1) {
success = UAVTalkSendObject(data->radioUAVTalkCon, ev.obj, 0, 0, RETRY_TIMEOUT_MS) == 0;
if (!success) {
success = UAVTalkSendObject(data->radioUAVTalkCon, ev.obj, ev.instId, 0, RETRY_TIMEOUT_MS) == 0;
if (success == -1) {
++retries;
}
}
data->comTxRetries += retries;
data->radioTxRetries += retries;
}
}
}
@ -333,6 +413,8 @@ static void radioRxTask(__attribute__((unused)) void *parameters)
}
} else if (PIOS_COM_TELEMETRY) {
// Send the data straight to the telemetry port.
// FIXME following call can fail (with -2 error code) if buffer is full
// it is the caller responsibility to retry in such cases...
PIOS_COM_SendBufferNonBlocking(PIOS_COM_TELEMETRY, serial_data, bytes_to_process);
}
}
@ -425,8 +507,10 @@ static void serialRxTask(__attribute__((unused)) void *parameters)
// Receive some data.
uint16_t bytes_to_process = PIOS_COM_ReceiveBuffer(inputPort, data->serialRxBuf, sizeof(data->serialRxBuf), MAX_PORT_DELAY);
// Send the data over the radio link.
if (bytes_to_process > 0) {
// Send the data over the radio link.
// FIXME following call can fail (with -2 error code) if buffer is full
// it is the caller responsibility to retry in such cases...
PIOS_COM_SendBufferNonBlocking(PIOS_COM_RADIO, data->serialRxBuf, bytes_to_process);
}
} else {
@ -445,6 +529,7 @@ static void serialRxTask(__attribute__((unused)) void *parameters)
*/
static int32_t UAVTalkSendHandler(uint8_t *buf, int32_t length)
{
int32_t ret;
uint32_t outputPort = data->parseUAVTalk ? PIOS_COM_TELEMETRY : 0;
#if defined(PIOS_INCLUDE_USB)
@ -454,10 +539,13 @@ static int32_t UAVTalkSendHandler(uint8_t *buf, int32_t length)
}
#endif /* PIOS_INCLUDE_USB */
if (outputPort) {
return PIOS_COM_SendBufferNonBlocking(outputPort, buf, length);
// FIXME following call can fail (with -2 error code) if buffer is full
// it is the caller responsibility to retry in such cases...
ret = PIOS_COM_SendBufferNonBlocking(outputPort, buf, length);
} else {
return -1;
ret = -1;
}
return ret;
}
/**
@ -477,10 +565,11 @@ static int32_t RadioSendHandler(uint8_t *buf, int32_t length)
// Don't send any data unless the radio port is available.
if (outputPort && PIOS_COM_Available(outputPort)) {
// FIXME following call can fail (with -2 error code) if buffer is full
// it is the caller responsibility to retry in such cases...
return PIOS_COM_SendBufferNonBlocking(outputPort, buf, length);
} else {
// For some reason, if this function returns failure, it prevents saving settings.
return length;
return -1;
}
}
@ -494,12 +583,40 @@ static int32_t RadioSendHandler(uint8_t *buf, int32_t length)
static void ProcessTelemetryStream(UAVTalkConnection inConnectionHandle, UAVTalkConnection outConnectionHandle, uint8_t rxbyte)
{
// Keep reading until we receive a completed packet.
UAVTalkRxState state = UAVTalkProcessInputStream(inConnectionHandle, rxbyte);
UAVTalkRxState state = UAVTalkProcessInputStreamQuiet(inConnectionHandle, rxbyte);
if (state == UAVTALK_STATE_ERROR) {
data->UAVTalkErrors++;
} else if (state == UAVTALK_STATE_COMPLETE) {
UAVTalkRelayPacket(inConnectionHandle, outConnectionHandle);
if (state == UAVTALK_STATE_COMPLETE) {
// We only want to unpack certain telemetry objects
uint32_t objId = UAVTalkGetPacketObjId(inConnectionHandle);
switch (objId) {
case OPLINKSTATUS_OBJID:
case OPLINKSETTINGS_OBJID:
case OPLINKRECEIVER_OBJID:
case MetaObjectId(OPLINKSTATUS_OBJID):
case MetaObjectId(OPLINKSETTINGS_OBJID):
case MetaObjectId(OPLINKRECEIVER_OBJID):
UAVTalkReceiveObject(inConnectionHandle);
break;
case OBJECTPERSISTENCE_OBJID:
case MetaObjectId(OBJECTPERSISTENCE_OBJID):
// receive object locally
// some objects will send back a response to telemetry
// FIXME:
// OPLM will ack or nack all objects requests and acked object sends
// Receiver will probably also ack / nack the same messages
// This has some consequences like :
// Second ack/nack will not match an open transaction or will apply to wrong transaction
// Question : how does GCS handle receiving the same object twice
// The OBJECTPERSISTENCE logic can be broken too if for example OPLM nacks and then REVO acks...
UAVTalkReceiveObject(inConnectionHandle);
// relay packet to remote modem
UAVTalkRelayPacket(inConnectionHandle, outConnectionHandle);
break;
default:
// all other packets are relayed to the remote modem
UAVTalkRelayPacket(inConnectionHandle, outConnectionHandle);
break;
}
}
}
@ -515,19 +632,30 @@ static void ProcessRadioStream(UAVTalkConnection inConnectionHandle, UAVTalkConn
// Keep reading until we receive a completed packet.
UAVTalkRxState state = UAVTalkProcessInputStreamQuiet(inConnectionHandle, rxbyte);
if (state == UAVTALK_STATE_ERROR) {
data->UAVTalkErrors++;
} else if (state == UAVTALK_STATE_COMPLETE) {
// We only want to unpack certain objects from the remote modem.
if (state == UAVTALK_STATE_COMPLETE) {
// We only want to unpack certain objects from the remote modem
// Similarly we only want to relay certain objects to the telemetry port
uint32_t objId = UAVTalkGetPacketObjId(inConnectionHandle);
switch (objId) {
case OPLINKSTATUS_OBJID:
case OPLINKSETTINGS_OBJID:
case MetaObjectId(OPLINKSTATUS_OBJID):
case MetaObjectId(OPLINKSETTINGS_OBJID):
// Ignore object...
// These objects are shadowed by the modem and are not transmitted to the telemetry port
// - OPLINKSTATUS_OBJID : ground station will receive the OPLM link status instead
// - OPLINKSETTINGS_OBJID : ground station will read and write the OPLM settings instead
break;
case OPLINKRECEIVER_OBJID:
case MetaObjectId(OPLINKRECEIVER_OBJID):
// Receive object locally
// These objects are received by the modem and are not transmitted to the telemetry port
// - OPLINKRECEIVER_OBJID : not sure why
// some objects will send back a response to the remote modem
UAVTalkReceiveObject(inConnectionHandle);
break;
default:
// all other packets are relayed to the telemetry port
UAVTalkRelayPacket(inConnectionHandle, outConnectionHandle);
break;
}
@ -545,7 +673,7 @@ static void objectPersistenceUpdatedCb(__attribute__((unused)) UAVObjEvent *objE
ObjectPersistenceGet(&obj_per);
// Is this concerning or setting object?
// Is this concerning our setting object?
if (obj_per.ObjectID == OPLINKSETTINGS_OBJID) {
// Is this a save, load, or delete?
bool success = false;

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

@ -158,6 +158,7 @@ int32_t TelemetryInitialize(void)
#endif
// Create periodic event that will be used to update the telemetry stats
// FIXME STATS_UPDATE_PERIOD_MS is 4000ms while FlighTelemetryStats update period is 5000ms...
txErrors = 0;
txRetries = 0;
UAVObjEvent ev;
@ -177,22 +178,10 @@ MODULE_INITCALL(TelemetryInitialize, TelemetryStart);
static void registerObject(UAVObjHandle obj)
{
if (UAVObjIsMetaobject(obj)) {
/* Only connect change notifications for meta objects. No periodic updates */
// Only connect change notifications for meta objects. No periodic updates
UAVObjConnectQueue(obj, priorityQueue, EV_MASK_ALL_UPDATES);
return;
} 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);
}
}
@ -208,9 +197,8 @@ static void updateObject(UAVObjHandle obj, int32_t eventType)
int32_t eventMask;
if (UAVObjIsMetaobject(obj)) {
/* This function updates the periodic updates for the object.
* Meta Objects cannot have periodic updates.
*/
// This function updates the periodic updates for the object.
// Meta Objects cannot have periodic updates.
PIOS_Assert(false);
return;
}
@ -298,7 +286,6 @@ static void processObjEvent(UAVObjEvent *ev)
{
UAVObjMetadata metadata;
UAVObjUpdateMode updateMode;
FlightTelemetryStatsData flightStats;
int32_t retries;
int32_t success;
@ -307,7 +294,6 @@ static void processObjEvent(UAVObjEvent *ev)
} else if (ev->obj == GCSTelemetryStatsHandle()) {
gcsTelemetryStatsUpdated();
} else {
FlightTelemetryStatsGet(&flightStats);
// Get object metadata
UAVObjGetMetadata(ev->obj, &metadata);
updateMode = UAVObjGetTelemetryUpdateMode(&metadata);
@ -315,32 +301,41 @@ static void processObjEvent(UAVObjEvent *ev)
// Act on event
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))) {
if ((ev->event == EV_UPDATED && (updateMode == UPDATEMODE_ONCHANGE || 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) {
success = UAVTalkSendObject(uavTalkCon, ev->obj, ev->instId, UAVObjGetTelemetryAcked(&metadata), REQ_TIMEOUT_MS); // call blocks until ack is received or timeout
++retries;
// call blocks until ack is received or timeout
success = UAVTalkSendObject(uavTalkCon, ev->obj, ev->instId, UAVObjGetTelemetryAcked(&metadata), REQ_TIMEOUT_MS);
if (success == -1) {
++retries;
}
}
// Update stats
txRetries += (retries - 1);
txRetries += retries;
if (success == -1) {
++txErrors;
}
} else if (ev->event == EV_UPDATE_REQ) {
// Request object update from GCS (with retries)
while (retries < MAX_RETRIES && success == -1) {
success = UAVTalkSendObjectRequest(uavTalkCon, ev->obj, ev->instId, REQ_TIMEOUT_MS); // call blocks until update is received or timeout
++retries;
// call blocks until update is received or timeout
success = UAVTalkSendObjectRequest(uavTalkCon, ev->obj, ev->instId, REQ_TIMEOUT_MS);
if (success == -1) {
++retries;
}
}
// Update stats
txRetries += (retries - 1);
txRetries += retries;
if (success == -1) {
++txErrors;
}
}
// If this is a metaobject then make necessary telemetry updates
if (UAVObjIsMetaobject(ev->obj)) {
updateObject(UAVObjGetLinkedObj(ev->obj), EV_NONE); // linked object will be the actual object the metadata are for
// linked object will be the actual object the metadata are for
updateObject(UAVObjGetLinkedObj(ev->obj), EV_NONE);
} else {
if (updateMode == UPDATEMODE_THROTTLED) {
// If this is UPDATEMODE_THROTTLED, the event mask changes on every event.
@ -351,7 +346,9 @@ static void processObjEvent(UAVObjEvent *ev)
// Log UAVObject if necessary
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))) {
if ((ev->event == EV_UPDATED && (updateMode == UPDATEMODE_ONCHANGE || 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++) {
@ -484,12 +481,18 @@ static int32_t transmitData(uint8_t *data, int32_t length)
static int32_t setUpdatePeriod(UAVObjHandle obj, int32_t updatePeriodMs)
{
UAVObjEvent ev;
int32_t ret;
// Add object for periodic updates
// Add or update object for periodic updates
ev.obj = obj;
ev.instId = UAVOBJ_ALL_INSTANCES;
ev.event = EV_UPDATED_PERIODIC;
return EventPeriodicQueueUpdate(&ev, queue, updatePeriodMs);
ret = EventPeriodicQueueUpdate(&ev, queue, updatePeriodMs);
if (ret == -1) {
ret = EventPeriodicQueueCreate(&ev, queue, updatePeriodMs);
}
return ret;
}
/**
@ -502,12 +505,18 @@ static int32_t setUpdatePeriod(UAVObjHandle obj, int32_t updatePeriodMs)
static int32_t setLoggingPeriod(UAVObjHandle obj, int32_t updatePeriodMs)
{
UAVObjEvent ev;
int32_t ret;
// Add object for periodic updates
// Add or update object for periodic updates
ev.obj = obj;
ev.instId = UAVOBJ_ALL_INSTANCES;
ev.event = EV_LOGGING_PERIODIC;
return EventPeriodicQueueUpdate(&ev, queue, updatePeriodMs);
ret = EventPeriodicQueueUpdate(&ev, queue, updatePeriodMs);
if (ret == -1) {
ret = EventPeriodicQueueCreate(&ev, queue, updatePeriodMs);
}
return ret;
}
/**
@ -553,25 +562,33 @@ static void updateTelemetryStats()
// Update stats object
if (flightStats.Status == FLIGHTTELEMETRYSTATS_STATUS_CONNECTED) {
flightStats.RxDataRate = (float)utalkStats.rxBytes / ((float)STATS_UPDATE_PERIOD_MS / 1000.0f);
flightStats.TxDataRate = (float)utalkStats.txBytes / ((float)STATS_UPDATE_PERIOD_MS / 1000.0f);
flightStats.RxFailures += utalkStats.rxErrors;
flightStats.TxFailures += txErrors;
flightStats.TxRetries += txRetries;
txErrors = 0;
txRetries = 0;
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.RxSyncErrors += utalkStats.rxSyncErrors;
flightStats.RxCrcErrors += utalkStats.rxCrcErrors;
} else {
flightStats.RxDataRate = 0;
flightStats.TxDataRate = 0;
flightStats.RxFailures = 0;
flightStats.TxFailures = 0;
flightStats.TxRetries = 0;
txErrors = 0;
txRetries = 0;
flightStats.TxDataRate = 0;
flightStats.TxBytes = 0;
flightStats.TxFailures = 0;
flightStats.TxRetries = 0;
flightStats.RxDataRate = 0;
flightStats.RxBytes = 0;
flightStats.RxFailures = 0;
flightStats.RxSyncErrors = 0;
flightStats.RxCrcErrors = 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;
}

22
flight/pios/common/pios_crc.c
View File

@ -64,6 +64,17 @@ static const uint16_t CRC_Table16[] = { // HDLC polynomial
0x7bc7, 0x6a4e, 0x58d5, 0x495c, 0x3de3, 0x2c6a, 0x1ef1, 0x0f78
};
/**
* Generated by pycrc v0.7.5, http://www.tty1.net/pycrc/
* using the configuration:
* Width = 8
* Poly = 0x07
* XorIn = 0x00
* ReflectIn = False
* XorOut = 0x00
* ReflectOut = False
* Algorithm = table-driven
*/
static const uint32_t CRC_Table32[] = {
0x00000000, 0x04c11db7, 0x09823b6e, 0x0d4326d9, 0x130476dc, 0x17c56b6b, 0x1a864db2, 0x1e475005,
0x2608edb8, 0x22c9f00f, 0x2f8ad6d6, 0x2b4bcb61, 0x350c9b64, 0x31cd86d3, 0x3c8ea00a, 0x384fbdbd,
@ -101,17 +112,6 @@ static const uint32_t CRC_Table32[] = {
/**
* Update the crc value with new data.
*
* Generated by pycrc v0.7.5, http://www.tty1.net/pycrc/
* using the configuration:
* Width = 8
* Poly = 0x07
* XorIn = 0x00
* ReflectIn = False
* XorOut = 0x00
* ReflectOut = False
* Algorithm = table-driven
*
* \param crc The current crc value.
* \param data Pointer to a buffer of \a data_len bytes.
* \param length Number of bytes in the \a data buffer.

2
flight/pios/common/pios_rfm22b.c
View File

@ -1221,7 +1221,7 @@ static void pios_rfm22_inject_event(struct pios_rfm22b_dev *rfm22b_dev, enum pio
// Something went fairly seriously wrong
rfm22b_dev->errors++;
}
portEND_SWITCHING_ISR((pxHigherPriorityTaskWoken2 == pdTRUE) || (pxHigherPriorityTaskWoken2 == pdTRUE));
portEND_SWITCHING_ISR((pxHigherPriorityTaskWoken1 == pdTRUE) || (pxHigherPriorityTaskWoken2 == pdTRUE));
} else {
// Store the event.
if (xQueueSend(rfm22b_dev->eventQueue, &event, portMAX_DELAY) != pdTRUE) {

1
flight/targets/boards/oplinkmini/firmware/Makefile
View File

@ -53,6 +53,7 @@ ifndef TESTAPP
SRC += $(OPUAVSYNTHDIR)/oplinksettings.c
SRC += $(OPUAVSYNTHDIR)/objectpersistence.c
SRC += $(OPUAVSYNTHDIR)/oplinkreceiver.c
SRC += $(OPUAVSYNTHDIR)/radiocombridgestats.c
else
## Test Code
SRC += $(OPTESTS)/test_common.c

1
flight/targets/boards/revolution/firmware/UAVObjects.inc
View File

@ -70,6 +70,7 @@ UAVOBJSRCFILENAMES += overosyncstats
UAVOBJSRCFILENAMES += overosyncsettings
UAVOBJSRCFILENAMES += pathaction
UAVOBJSRCFILENAMES += pathdesired
UAVOBJSRCFILENAMES += pathplan
UAVOBJSRCFILENAMES += pathstatus
UAVOBJSRCFILENAMES += positionstate
UAVOBJSRCFILENAMES += ratedesired

1
flight/targets/boards/revoproto/firmware/UAVObjects.inc
View File

@ -70,6 +70,7 @@ UAVOBJSRCFILENAMES += overosyncstats
UAVOBJSRCFILENAMES += overosyncsettings
UAVOBJSRCFILENAMES += pathaction
UAVOBJSRCFILENAMES += pathdesired
UAVOBJSRCFILENAMES += pathplan
UAVOBJSRCFILENAMES += pathstatus
UAVOBJSRCFILENAMES += positionstate
UAVOBJSRCFILENAMES += ratedesired

2
flight/targets/boards/simposix/firmware/Makefile
View File

@ -68,7 +68,7 @@ UAVOBJSYNTHDIR = $(OUTDIR)/../uavobject-synthetics/flight
# optional component libraries
include $(PIOS)/common/libraries/FreeRTOS/library.mk
include $(FLIGHTLIB)/PyMite/pymite.mk
#include $(FLIGHTLIB)/PyMite/pymite.mk
# List C source files here. (C dependencies are automatically generated.)
# use file-extension c for "c-only"-files

1
flight/targets/boards/simposix/firmware/UAVObjects.inc
View File

@ -73,6 +73,7 @@ UAVOBJSRCFILENAMES += objectpersistence
UAVOBJSRCFILENAMES += overosyncstats
UAVOBJSRCFILENAMES += pathaction
UAVOBJSRCFILENAMES += pathdesired
UAVOBJSRCFILENAMES += pathplan
UAVOBJSRCFILENAMES += pathstatus
UAVOBJSRCFILENAMES += positionstate
UAVOBJSRCFILENAMES += ratedesired

3
flight/uavobjects/inc/uavobjectmanager.h
View File

@ -49,6 +49,8 @@
typedef void *UAVObjHandle;
#define MetaObjectId(id) ((id) + 1)
/**
* Object update mode, used by multiple modules (e.g. telemetry and logger)
*/
@ -158,6 +160,7 @@ bool UAVObjIsMetaobject(UAVObjHandle obj);
bool UAVObjIsSettings(UAVObjHandle obj);
int32_t UAVObjUnpack(UAVObjHandle obj_handle, uint16_t instId, const uint8_t *dataIn);
int32_t UAVObjPack(UAVObjHandle obj_handle, uint16_t instId, uint8_t *dataOut);
uint8_t UAVObjUpdateCRC(UAVObjHandle obj_handle, uint16_t instId, uint8_t crc);
int32_t UAVObjSave(UAVObjHandle obj_handle, uint16_t instId);
int32_t UAVObjLoad(UAVObjHandle obj_handle, uint16_t instId);
int32_t UAVObjDelete(UAVObjHandle obj_handle, uint16_t instId);

90
flight/uavobjects/uavobjectmanager.c
View File

@ -167,22 +167,19 @@ struct UAVOMulti {
#define MetaObjectPtr(obj) ((struct UAVODataMeta *)&((obj)->metaObj))
#define MetaDataPtr(obj) ((UAVObjMetadata *)&((obj)->instance0))
#define LinkedMetaDataPtr(obj) ((UAVObjMetadata *)&((obj)->metaObj.instance0))
#define MetaObjectId(id) ((id) + 1)
/** all information about instances are dependant on object type **/
#define ObjSingleInstanceDataOffset(obj) ((void *)(&(((struct UAVOSingle *)obj)->instance0)))
#define InstanceDataOffset(inst) ((void *)&(((struct UAVOMultiInst *)inst)->instance))
#define InstanceData(instance) (void *)instance
#define InstanceData(instance) ((void *)instance)
// Private functions
static int32_t sendEvent(struct UAVOBase *obj, uint16_t instId,
UAVObjEventType event);
static int32_t sendEvent(struct UAVOBase *obj, uint16_t instId, UAVObjEventType event);
static InstanceHandle createInstance(struct UAVOData *obj, uint16_t instId);
static InstanceHandle getInstance(struct UAVOData *obj, uint16_t instId);
static int32_t connectObj(UAVObjHandle obj_handle, xQueueHandle queue,
UAVObjEventCallback cb, uint8_t eventMask);
static int32_t disconnectObj(UAVObjHandle obj_handle, xQueueHandle queue,
UAVObjEventCallback cb);
static int32_t connectObj(UAVObjHandle obj_handle, xQueueHandle queue, UAVObjEventCallback cb, uint8_t eventMask);
static int32_t disconnectObj(UAVObjHandle obj_handle, xQueueHandle queue, UAVObjEventCallback cb);
static void instanceAutoUpdated(UAVObjHandle obj_handle, uint16_t instId);
// Private variables
static xSemaphoreHandle mutex;
@ -390,8 +387,8 @@ UAVObjHandle UAVObjRegister(uint32_t id,
}
// fire events for outer object and its embedded meta object
UAVObjInstanceUpdated((UAVObjHandle)uavo_data, 0);
UAVObjInstanceUpdated((UAVObjHandle) & (uavo_data->metaObj), 0);
instanceAutoUpdated((UAVObjHandle)uavo_data, 0);
instanceAutoUpdated((UAVObjHandle) & (uavo_data->metaObj), 0);
unlock_exit:
xSemaphoreGiveRecursive(mutex);
@ -615,8 +612,7 @@ bool UAVObjIsSettings(UAVObjHandle obj_handle)
* \param[in] dataIn The byte array
* \return 0 if success or -1 if failure
*/
int32_t UAVObjUnpack(UAVObjHandle obj_handle, uint16_t instId,
const uint8_t *dataIn)
int32_t UAVObjUnpack(UAVObjHandle obj_handle, uint16_t instId, const uint8_t *dataIn)
{
PIOS_Assert(obj_handle);
@ -704,6 +700,45 @@ unlock_exit:
return rc;
}
/**
* Update a CRC with an object data
* \param[in] obj The object handle
* \param[in] instId The instance ID
* \param[in] crc The crc to update
* \return the updated crc
*/
uint8_t UAVObjUpdateCRC(UAVObjHandle obj_handle, uint16_t instId, uint8_t crc)
{
PIOS_Assert(obj_handle);
// Lock
xSemaphoreTakeRecursive(mutex, portMAX_DELAY);
if (UAVObjIsMetaobject(obj_handle)) {
if (instId != 0) {
goto unlock_exit;
}
// TODO
} else {
struct UAVOData *obj;
InstanceHandle instEntry;
// Cast handle to object
obj = (struct UAVOData *)obj_handle;
// Get the instance
instEntry = getInstance(obj, instId);
if (instEntry == NULL) {
goto unlock_exit;
}
// Update crc
crc = PIOS_CRC_updateCRC(crc, (uint8_t *)InstanceData(instEntry), (int32_t)obj->instance_size);
}
unlock_exit:
xSemaphoreGiveRecursive(mutex);
return crc;
}
/**
* Actually write the object's data to the logfile
@ -1582,8 +1617,8 @@ void UAVObjRequestUpdate(UAVObjHandle obj_handle)
}
/**
* Request an update of the object's data from the GCS. The call will not wait for the response, a EV_UPDATED event
* will be generated as soon as the object is updated.
* Request an update of the object's data from the GCS.
* The call will not wait for the response, a EV_UPDATED event will be generated as soon as the object is updated.
* \param[in] obj The object handle
* \param[in] instId Object instance ID to update
*/
@ -1596,7 +1631,7 @@ void UAVObjRequestInstanceUpdate(UAVObjHandle obj_handle, uint16_t instId)
}
/**
* Send the object's data to the GCS (triggers a EV_UPDATED_MANUAL event on this object).
* Trigger a EV_UPDATED_MANUAL event for an object.
* \param[in] obj The object handle
*/
void UAVObjUpdated(UAVObjHandle obj_handle)
@ -1605,7 +1640,7 @@ void UAVObjUpdated(UAVObjHandle obj_handle)
}
/**
* Send the object's data to the GCS (triggers a EV_UPDATED_MANUAL event on this object).
* Trigger a EV_UPDATED_MANUAL event for an object instance.
* \param[in] obj The object handle
* \param[in] instId The object instance ID
*/
@ -1618,6 +1653,19 @@ void UAVObjInstanceUpdated(UAVObjHandle obj_handle, uint16_t instId)
}
/**
* Trigger a EV_UPDATED event for an object instance.
* \param[in] obj The object handle
* \param[in] instId The object instance ID
*/
static void instanceAutoUpdated(UAVObjHandle obj_handle, uint16_t instId)
{
PIOS_Assert(obj_handle);
xSemaphoreTakeRecursive(mutex, portMAX_DELAY);
sendEvent((struct UAVOBase *)obj_handle, instId, EV_UPDATED);
xSemaphoreGiveRecursive(mutex);
}
/*
* Log the object's data (triggers a EV_LOGGING_MANUAL event on this object).
* \param[in] obj The object handle
*/
@ -1664,8 +1712,7 @@ xSemaphoreGiveRecursive(mutex);
/**
* Send a triggered event to all event queues registered on the object.
*/
static int32_t sendEvent(struct UAVOBase *obj, uint16_t instId,
UAVObjEventType triggered_event)
static int32_t sendEvent(struct UAVOBase *obj, uint16_t instId, UAVObjEventType triggered_event)
{
/* Set up the message that will be sent to all registered listeners */
UAVObjEvent msg = {
@ -1678,14 +1725,13 @@ static int32_t sendEvent(struct UAVOBase *obj, uint16_t instId,
struct ObjectEventEntry *event;
LL_FOREACH(obj->next_event, event) {
if (event->eventMask == 0
|| (event->eventMask & triggered_event) != 0) {
if (event->eventMask == 0 || (event->eventMask & triggered_event) != 0) {
// Send to queue if a valid queue is registered
if (event->queue) {
// will not block
if (xQueueSend(event->queue, &msg, 0) != pdTRUE) {
stats.lastQueueErrorID = UAVObjGetID(obj);
++stats.eventQueueErrors;
stats.lastQueueErrorID = UAVObjGetID(obj);
}
}
@ -1745,7 +1791,7 @@ static InstanceHandle createInstance(struct UAVOData *obj, uint16_t instId)
((struct UAVOMulti *)obj)->num_instances++;
// Fire event
UAVObjInstanceUpdated((UAVObjHandle)obj, instId);
instanceAutoUpdated((UAVObjHandle)obj, instId);
// Done
return InstanceDataOffset(instEntry);

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

@ -34,13 +34,16 @@ typedef int32_t (*UAVTalkOutputStream)(uint8_t *data, int32_t length);
typedef struct {
uint32_t txBytes;
uint32_t rxBytes;
uint32_t txObjectBytes;
uint32_t rxObjectBytes;
uint32_t rxObjects;
uint32_t txObjects;
uint32_t txErrors;
uint32_t rxBytes;
uint32_t rxObjectBytes;
uint32_t rxObjects;
uint32_t rxErrors;
uint32_t rxSyncErrors;
uint32_t rxCrcErrors;
} UAVTalkStats;
typedef void *UAVTalkConnection;
@ -54,9 +57,6 @@ UAVTalkOutputStream UAVTalkGetOutputStream(UAVTalkConnection connection);
int32_t UAVTalkSendObject(UAVTalkConnection connection, UAVObjHandle obj, uint16_t instId, uint8_t acked, int32_t timeoutMs);
int32_t UAVTalkSendObjectTimestamped(UAVTalkConnection connectionHandle, UAVObjHandle obj, uint16_t instId, uint8_t acked, int32_t timeoutMs);
int32_t UAVTalkSendObjectRequest(UAVTalkConnection connection, UAVObjHandle obj, uint16_t instId, int32_t timeoutMs);
int32_t UAVTalkSendAck(UAVTalkConnection connectionHandle, UAVObjHandle obj, uint16_t instId);
int32_t UAVTalkSendNack(UAVTalkConnection connectionHandle, uint32_t objId);
int32_t UAVTalkSendBuf(UAVTalkConnection connectionHandle, uint8_t *buf, uint16_t len);
UAVTalkRxState UAVTalkProcessInputStream(UAVTalkConnection connection, uint8_t rxbyte);
UAVTalkRxState UAVTalkProcessInputStreamQuiet(UAVTalkConnection connection, uint8_t rxbyte);
UAVTalkRxState UAVTalkRelayPacket(UAVTalkConnection inConnectionHandle, UAVTalkConnection outConnectionHandle);

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

@ -32,38 +32,26 @@
#include "uavobjectsinit.h"
// Private types and constants
typedef struct {
uint8_t sync;
uint8_t type;
uint16_t size;
uint32_t objId;
} uavtalk_min_header;
#define UAVTALK_MIN_HEADER_LENGTH sizeof(uavtalk_min_header)
typedef struct {
uint8_t sync;
uint8_t type;
uint16_t size;
uint32_t objId;
uint16_t instId;
uint16_t timestamp;
} uavtalk_max_header;
#define UAVTALK_MAX_HEADER_LENGTH sizeof(uavtalk_max_header)
// min header : sync(1), type (1), size(2), object ID(4), instance ID(2)
#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_CHECKSUM_LENGTH 1
typedef uint8_t uavtalk_checksum;
#define UAVTALK_CHECKSUM_LENGTH sizeof(uavtalk_checksum)
#define UAVTALK_MAX_PAYLOAD_LENGTH (UAVOBJECTS_LARGEST + 1)
#define UAVTALK_MIN_PACKET_LENGTH UAVTALK_MAX_HEADER_LENGTH + UAVTALK_CHECKSUM_LENGTH
#define UAVTALK_MAX_PACKET_LENGTH UAVTALK_MIN_PACKET_LENGTH + UAVTALK_MAX_PAYLOAD_LENGTH
typedef struct {
UAVObjHandle obj;
uint8_t type;
uint16_t packet_size;
uint32_t objId;
uint16_t instId;
uint32_t length;
uint8_t instanceLength;
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;
@ -78,19 +66,18 @@ typedef struct {
xSemaphoreHandle lock;
xSemaphoreHandle transLock;
xSemaphoreHandle respSema;
UAVObjHandle respObj;
uint8_t respType;
uint32_t respObjId;
uint16_t respInstId;
UAVTalkStats stats;
UAVTalkInputProcessor iproc;
uint8_t *rxBuffer;
uint32_t txSize;
uint8_t *txBuffer;
} UAVTalkConnectionData;
#define UAVTALK_CANARI 0xCA
#define UAVTALK_WAITFOREVER -1
#define UAVTALK_NOWAIT 0
#define UAVTALK_SYNC_VAL 0x3C
#define UAVTALK_TYPE_MASK 0x78
#define UAVTALK_TYPE_VER 0x20
#define UAVTALK_TIMESTAMPED 0x80

612
flight/uavtalk/uavtalk.c
View File

@ -32,14 +32,27 @@
#include "openpilot.h"
#include "uavtalk_priv.h"
// #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__); }
#endif
#ifndef UAVT_DEBUGLOG_PRINTF
#define UAVT_DEBUGLOG_PRINTF(...)
#endif
#ifndef UAVT_DEBUGLOG_CPRINTF
#define UAVT_DEBUGLOG_CPRINTF(objId, ...)
#endif
// Private functions
static int32_t objectTransaction(UAVTalkConnectionData *connection, UAVObjHandle objectId, uint16_t instId, uint8_t type, int32_t timeout);
static int32_t sendObject(UAVTalkConnectionData *connection, UAVObjHandle obj, uint16_t instId, uint8_t type);
static int32_t sendSingleObject(UAVTalkConnectionData *connection, UAVObjHandle obj, uint16_t instId, uint8_t type);
static int32_t sendNack(UAVTalkConnectionData *connection, uint32_t objId);
static int32_t receiveObject(UAVTalkConnectionData *connection, uint8_t type, uint32_t objId, uint16_t instId, uint8_t *data, int32_t length);
static void updateAck(UAVTalkConnectionData *connection, UAVObjHandle obj, uint16_t instId);
static int32_t objectTransaction(UAVTalkConnectionData *connection, uint8_t type, UAVObjHandle obj, uint16_t instId, int32_t timeout);
static int32_t sendObject(UAVTalkConnectionData *connection, uint8_t type, uint32_t objId, uint16_t instId, UAVObjHandle obj);
static int32_t sendSingleObject(UAVTalkConnectionData *connection, uint8_t type, uint32_t objId, uint16_t instId, UAVObjHandle obj);
static int32_t receiveObject(UAVTalkConnectionData *connection, uint8_t type, uint32_t objId, uint16_t instId, uint8_t *data);
static void updateAck(UAVTalkConnectionData *connection, uint8_t type, uint32_t objId, uint16_t instId);
/**
* Initialize the UAVTalk library
@ -152,13 +165,15 @@ void UAVTalkAddStats(UAVTalkConnection connectionHandle, UAVTalkStats *statsOut)
// Copy stats
statsOut->txBytes += connection->stats.txBytes;
statsOut->rxBytes += connection->stats.rxBytes;
statsOut->txObjectBytes += connection->stats.txObjectBytes;
statsOut->rxObjectBytes += connection->stats.rxObjectBytes;
statsOut->txObjects += connection->stats.txObjects;
statsOut->txErrors += connection->stats.txErrors;
statsOut->rxBytes += connection->stats.rxBytes;
statsOut->rxObjectBytes += connection->stats.rxObjectBytes;
statsOut->rxObjects += connection->stats.rxObjects;
statsOut->txErrors += connection->stats.txErrors;
statsOut->txErrors += connection->stats.txErrors;
statsOut->rxErrors += connection->stats.rxErrors;
statsOut->rxSyncErrors += connection->stats.rxSyncErrors;
statsOut->rxCrcErrors += connection->stats.rxCrcErrors;
// Release lock
xSemaphoreGiveRecursive(connection->lock);
@ -197,7 +212,6 @@ void UAVTalkGetLastTimestamp(UAVTalkConnection connectionHandle, uint16_t *times
*timestamp = iproc->timestamp;
}
/**
* Request an update for the specified object, on success the object data would have been
* updated by the GCS.
@ -213,7 +227,8 @@ int32_t UAVTalkSendObjectRequest(UAVTalkConnection connectionHandle, UAVObjHandl
UAVTalkConnectionData *connection;
CHECKCONHANDLE(connectionHandle, connection, return -1);
return objectTransaction(connection, obj, instId, UAVTALK_TYPE_OBJ_REQ, timeout);
return objectTransaction(connection, UAVTALK_TYPE_OBJ_REQ, obj, instId, timeout);
}
/**
@ -231,11 +246,12 @@ int32_t UAVTalkSendObject(UAVTalkConnection connectionHandle, UAVObjHandle obj,
UAVTalkConnectionData *connection;
CHECKCONHANDLE(connectionHandle, connection, return -1);
// Send object
if (acked == 1) {
return objectTransaction(connection, obj, instId, UAVTALK_TYPE_OBJ_ACK, timeoutMs);
return objectTransaction(connection, UAVTALK_TYPE_OBJ_ACK, obj, instId, timeoutMs);
} else {
return objectTransaction(connection, obj, instId, UAVTALK_TYPE_OBJ, timeoutMs);
return objectTransaction(connection, UAVTALK_TYPE_OBJ, obj, instId, timeoutMs);
}
}
@ -254,29 +270,32 @@ int32_t UAVTalkSendObjectTimestamped(UAVTalkConnection connectionHandle, UAVObjH
UAVTalkConnectionData *connection;
CHECKCONHANDLE(connectionHandle, connection, return -1);
// Send object
if (acked == 1) {
return objectTransaction(connection, obj, instId, UAVTALK_TYPE_OBJ_ACK_TS, timeoutMs);
return objectTransaction(connection, UAVTALK_TYPE_OBJ_ACK_TS, obj, instId, timeoutMs);
} else {
return objectTransaction(connection, obj, instId, UAVTALK_TYPE_OBJ_TS, timeoutMs);
return objectTransaction(connection, UAVTALK_TYPE_OBJ_TS, obj, instId, timeoutMs);
}
}
/**
* Execute the requested transaction on an object.
* \param[in] connection UAVTalkConnection to be used
* \param[in] obj Object
* \param[in] instId The instance ID of UAVOBJ_ALL_INSTANCES for all instances.
* \param[in] type Transaction type
* UAVTALK_TYPE_OBJ: send object,
* UAVTALK_TYPE_OBJ_REQ: request object update
* UAVTALK_TYPE_OBJ_ACK: send object with an ack
* \param[in] obj Object
* \param[in] instId The instance ID of UAVOBJ_ALL_INSTANCES for all instances.
* \param[in] timeoutMs Time to wait for the ack, when zero it will return immediately
* \return 0 Success
* \return -1 Failure
*/
static int32_t objectTransaction(UAVTalkConnectionData *connection, UAVObjHandle obj, uint16_t instId, uint8_t type, int32_t timeoutMs)
static int32_t objectTransaction(UAVTalkConnectionData *connection, uint8_t type, UAVObjHandle obj, uint16_t instId, int32_t timeoutMs)
{
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) {
@ -284,33 +303,38 @@ static int32_t objectTransaction(UAVTalkConnectionData *connection, UAVObjHandle
xSemaphoreTakeRecursive(connection->transLock, portMAX_DELAY);
// Send object
xSemaphoreTakeRecursive(connection->lock, portMAX_DELAY);
connection->respObj = obj;
// expected response type
connection->respType = (type == UAVTALK_TYPE_OBJ_REQ) ? UAVTALK_TYPE_OBJ : UAVTALK_TYPE_ACK;
connection->respObjId = UAVObjGetID(obj);
connection->respInstId = instId;
sendObject(connection, obj, instId, type);
ret = sendObject(connection, type, UAVObjGetID(obj), instId, obj);
xSemaphoreGiveRecursive(connection->lock);
// Wait for response (or timeout)
respReceived = xSemaphoreTake(connection->respSema, timeoutMs / portTICK_RATE_MS);
// Wait for response (or timeout) if sending the object succeeded
respReceived = pdFALSE;
if (ret == 0) {
respReceived = xSemaphoreTake(connection->respSema, timeoutMs / portTICK_RATE_MS);
}
// Check if a response was received
if (respReceived == pdFALSE) {
if (respReceived == pdTRUE) {
// We are done successfully
xSemaphoreGiveRecursive(connection->transLock);
ret = 0;
} else {
// Cancel transaction
xSemaphoreTakeRecursive(connection->lock, portMAX_DELAY);
xSemaphoreTake(connection->respSema, 0); // non blocking call to make sure the value is reset to zero (binary sema)
connection->respObj = 0;
// non blocking call to make sure the value is reset to zero (binary sema)
xSemaphoreTake(connection->respSema, 0);
connection->respObjId = 0;
xSemaphoreGiveRecursive(connection->lock);
xSemaphoreGiveRecursive(connection->transLock);
return -1;
} else {
xSemaphoreGiveRecursive(connection->transLock);
return 0;
}
} else if (type == UAVTALK_TYPE_OBJ || type == UAVTALK_TYPE_OBJ_TS) {
xSemaphoreTakeRecursive(connection->lock, portMAX_DELAY);
sendObject(connection, obj, instId, type);
ret = sendObject(connection, type, UAVObjGetID(obj), instId, obj);
xSemaphoreGiveRecursive(connection->lock);
return 0;
} else {
return -1;
}
return ret;
}
/**
@ -326,6 +350,7 @@ UAVTalkRxState UAVTalkProcessInputStreamQuiet(UAVTalkConnection connectionHandle
CHECKCONHANDLE(connectionHandle, connection, return -1);
UAVTalkInputProcessor *iproc = &connection->iproc;
++connection->stats.rxBytes;
if (iproc->state == UAVTALK_STATE_ERROR || iproc->state == UAVTALK_STATE_COMPLETE) {
@ -333,12 +358,16 @@ UAVTalkRxState UAVTalkProcessInputStreamQuiet(UAVTalkConnection connectionHandle
}
if (iproc->rxPacketLength < 0xffff) {
iproc->rxPacketLength++; // update packet byte count
// update packet byte count
iproc->rxPacketLength++;
}
// Receive state machine
switch (iproc->state) {
case UAVTALK_STATE_SYNC:
if (rxbyte != UAVTALK_SYNC_VAL) {
connection->stats.rxSyncErrors++;
break;
}
@ -346,26 +375,27 @@ UAVTalkRxState UAVTalkProcessInputStreamQuiet(UAVTalkConnection connectionHandle
iproc->cs = PIOS_CRC_updateByte(0, rxbyte);
iproc->rxPacketLength = 1;
iproc->rxCount = 0;
iproc->state = UAVTALK_STATE_TYPE;
iproc->type = 0;
iproc->state = UAVTALK_STATE_TYPE;
break;
case UAVTALK_STATE_TYPE:
// update the CRC
iproc->cs = PIOS_CRC_updateByte(iproc->cs, rxbyte);
if ((rxbyte & UAVTALK_TYPE_MASK) != UAVTALK_TYPE_VER) {
iproc->state = UAVTALK_STATE_ERROR;
connection->stats.rxErrors++;
iproc->state = UAVTALK_STATE_SYNC;
break;
}
iproc->type = rxbyte;
// update the CRC
iproc->cs = PIOS_CRC_updateByte(iproc->cs, rxbyte);
iproc->type = rxbyte;
iproc->packet_size = 0;
iproc->state = UAVTALK_STATE_SIZE;
iproc->rxCount = 0;
break;
case UAVTALK_STATE_SIZE:
@ -378,17 +408,18 @@ UAVTalkRxState UAVTalkProcessInputStreamQuiet(UAVTalkConnection connectionHandle
iproc->rxCount++;
break;
}
iproc->packet_size += rxbyte << 8;
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
if (iproc->packet_size < UAVTALK_MIN_HEADER_LENGTH || iproc->packet_size > UAVTALK_MAX_HEADER_LENGTH + UAVTALK_MAX_PAYLOAD_LENGTH) {
// incorrect packet size
connection->stats.rxErrors++;
iproc->state = UAVTALK_STATE_ERROR;
break;
}
iproc->rxCount = 0;
iproc->objId = 0;
iproc->state = UAVTALK_STATE_OBJID;
iproc->objId = 0;
iproc->state = UAVTALK_STATE_OBJID;
break;
case UAVTALK_STATE_OBJID:
@ -397,55 +428,60 @@ UAVTalkRxState UAVTalkProcessInputStreamQuiet(UAVTalkConnection connectionHandle
iproc->cs = PIOS_CRC_updateByte(iproc->cs, rxbyte);
iproc->objId += rxbyte << (8 * (iproc->rxCount++));
if (iproc->rxCount < 4) {
break;
}
iproc->rxCount = 0;
// Search for object.
iproc->obj = UAVObjGetByID(iproc->objId);
iproc->instId = 0;
iproc->state = UAVTALK_STATE_INSTID;
break;
case UAVTALK_STATE_INSTID:
// update the CRC
iproc->cs = PIOS_CRC_updateByte(iproc->cs, rxbyte);
iproc->instId += rxbyte << (8 * (iproc->rxCount++));
if (iproc->rxCount < 2) {
break;
}
iproc->rxCount = 0;
UAVObjHandle obj = UAVObjGetByID(iproc->objId);
// Determine data length
if (iproc->type == UAVTALK_TYPE_OBJ_REQ || iproc->type == UAVTALK_TYPE_ACK || iproc->type == UAVTALK_TYPE_NACK) {
iproc->length = 0;
iproc->instanceLength = 0;
iproc->timestampLength = 0;
} else {
if (iproc->obj) {
iproc->length = UAVObjGetNumBytes(iproc->obj);
iproc->instanceLength = (UAVObjIsSingleInstance(iproc->obj) ? 0 : 2);
} else {
// We don't know if it's a multi-instance object, so just assume it's 0.
iproc->instanceLength = 0;
iproc->length = iproc->packet_size - iproc->rxPacketLength;
}
iproc->timestampLength = (iproc->type & UAVTALK_TIMESTAMPED) ? 2 : 0;
if (obj) {
iproc->length = UAVObjGetNumBytes(obj);
} else {
iproc->length = iproc->packet_size - iproc->rxPacketLength - iproc->timestampLength;
}
}
// Check length and determine next state
// Check length
if (iproc->length >= UAVTALK_MAX_PAYLOAD_LENGTH) {
// packet error - exceeded payload max length
connection->stats.rxErrors++;
iproc->state = UAVTALK_STATE_ERROR;
break;
}
// Check the lengths match
if ((iproc->rxPacketLength + iproc->instanceLength + iproc->timestampLength + iproc->length) != iproc->packet_size) { // packet error - mismatched packet size
if ((iproc->rxPacketLength + iproc->timestampLength + iproc->length) != iproc->packet_size) {
// packet error - mismatched packet size
connection->stats.rxErrors++;
iproc->state = UAVTALK_STATE_ERROR;
break;
}
iproc->instId = 0;
if (iproc->type == UAVTALK_TYPE_NACK) {
// If this is a NACK, we skip to Checksum
iproc->state = UAVTALK_STATE_CS;
}
// Check if this is a single instance object (i.e. if the instance ID field is coming next)
else if ((iproc->obj != 0) && !UAVObjIsSingleInstance(iproc->obj)) {
iproc->state = UAVTALK_STATE_INSTID;
}
// Check if this is a single instance and has a timestamp in it
else if ((iproc->obj != 0) && (iproc->type & UAVTALK_TIMESTAMPED)) {
// Determine next state
if (iproc->type & UAVTALK_TIMESTAMPED) {
// If there is a timestamp get it
iproc->timestamp = 0;
iproc->state = UAVTALK_STATE_TIMESTAMP;
} else {
@ -456,47 +492,17 @@ UAVTalkRxState UAVTalkProcessInputStreamQuiet(UAVTalkConnection connectionHandle
iproc->state = UAVTALK_STATE_CS;
}
}
iproc->rxCount = 0;
break;
case UAVTALK_STATE_INSTID:
// update the CRC
iproc->cs = PIOS_CRC_updateByte(iproc->cs, rxbyte);
iproc->instId += rxbyte << (8 * (iproc->rxCount++));
if (iproc->rxCount < 2) {
break;
}
iproc->rxCount = 0;
// If there is a timestamp, get it
if ((iproc->length > 0) && (iproc->type & UAVTALK_TIMESTAMPED)) {
iproc->timestamp = 0;
iproc->state = UAVTALK_STATE_TIMESTAMP;
}
// If there is a payload get it, otherwise receive checksum
else if (iproc->length > 0) {
iproc->state = UAVTALK_STATE_DATA;
} else {
iproc->state = UAVTALK_STATE_CS;
}
break;
case UAVTALK_STATE_TIMESTAMP:
// update the CRC
iproc->cs = PIOS_CRC_updateByte(iproc->cs, rxbyte);
iproc->timestamp += rxbyte << (8 * (iproc->rxCount++));
if (iproc->rxCount < 2) {
break;
}
iproc->rxCount = 0;
// If there is a payload get it, otherwise receive checksum
@ -516,35 +522,40 @@ UAVTalkRxState UAVTalkProcessInputStreamQuiet(UAVTalkConnection connectionHandle
if (iproc->rxCount < iproc->length) {
break;
}
iproc->rxCount = 0;
iproc->state = UAVTALK_STATE_CS;
iproc->rxCount = 0;
break;
case UAVTALK_STATE_CS:
// the CRC byte
if (rxbyte != iproc->cs) { // packet error - faulty CRC
// Check the CRC byte
if (rxbyte != iproc->cs) {
// packet error - faulty CRC
UAVT_DEBUGLOG_PRINTF("BAD CRC");
connection->stats.rxCrcErrors++;
connection->stats.rxErrors++;
iproc->state = UAVTALK_STATE_ERROR;
break;
}
if (iproc->rxPacketLength != (iproc->packet_size + 1)) { // packet error - mismatched packet size
if (iproc->rxPacketLength != (iproc->packet_size + UAVTALK_CHECKSUM_LENGTH)) {
// packet error - mismatched packet size
connection->stats.rxErrors++;
iproc->state = UAVTALK_STATE_ERROR;
break;
}
connection->stats.rxObjectBytes += iproc->length;
connection->stats.rxObjects++;
connection->stats.rxObjectBytes += iproc->length;
iproc->state = UAVTALK_STATE_COMPLETE;
break;
default:
connection->stats.rxErrors++;
iproc->state = UAVTALK_STATE_ERROR;
break;
}
// Done
@ -587,6 +598,8 @@ UAVTalkRxState UAVTalkRelayPacket(UAVTalkConnection inConnectionHandle, UAVTalkC
// The input packet must be completely parsed.
if (inIproc->state != UAVTALK_STATE_COMPLETE) {
inConnection->stats.rxErrors++;
return -1;
}
@ -594,66 +607,66 @@ UAVTalkRxState UAVTalkRelayPacket(UAVTalkConnection inConnectionHandle, UAVTalkC
CHECKCONHANDLE(outConnectionHandle, outConnection, return -1);
if (!outConnection->outStream) {
outConnection->stats.txErrors++;
return -1;
}
// Lock
xSemaphoreTakeRecursive(outConnection->lock, portMAX_DELAY);
// Setup type and object id fields
outConnection->txBuffer[0] = UAVTALK_SYNC_VAL; // sync byte
outConnection->txBuffer[0] = UAVTALK_SYNC_VAL;
// Setup type
outConnection->txBuffer[1] = inIproc->type;
// data length inserted here below
int32_t dataOffset = 8;
if (inIproc->objId != 0) {
outConnection->txBuffer[4] = (uint8_t)(inIproc->objId & 0xFF);
outConnection->txBuffer[5] = (uint8_t)((inIproc->objId >> 8) & 0xFF);
outConnection->txBuffer[6] = (uint8_t)((inIproc->objId >> 16) & 0xFF);
outConnection->txBuffer[7] = (uint8_t)((inIproc->objId >> 24) & 0xFF);
// Setup instance ID if one is required
if (inIproc->instanceLength > 0) {
outConnection->txBuffer[8] = (uint8_t)(inIproc->instId & 0xFF);
outConnection->txBuffer[9] = (uint8_t)((inIproc->instId >> 8) & 0xFF);
dataOffset = 10;
}
} else {
dataOffset = 4;
}
// next 2 bytes are reserved for data length (inserted here later)
// Setup object ID
outConnection->txBuffer[4] = (uint8_t)(inIproc->objId & 0xFF);
outConnection->txBuffer[5] = (uint8_t)((inIproc->objId >> 8) & 0xFF);
outConnection->txBuffer[6] = (uint8_t)((inIproc->objId >> 16) & 0xFF);
outConnection->txBuffer[7] = (uint8_t)((inIproc->objId >> 24) & 0xFF);
// Setup instance ID
outConnection->txBuffer[8] = (uint8_t)(inIproc->instId & 0xFF);
outConnection->txBuffer[9] = (uint8_t)((inIproc->instId >> 8) & 0xFF);
int32_t headerLength = 10;
// Add timestamp when the transaction type is appropriate
if (inIproc->type & UAVTALK_TIMESTAMPED) {
portTickType time = xTaskGetTickCount();
outConnection->txBuffer[dataOffset] = (uint8_t)(time & 0xFF);
outConnection->txBuffer[dataOffset + 1] = (uint8_t)((time >> 8) & 0xFF);
dataOffset += 2;
outConnection->txBuffer[10] = (uint8_t)(time & 0xFF);
outConnection->txBuffer[11] = (uint8_t)((time >> 8) & 0xFF);
headerLength += 2;
}
// Copy data (if any)
memcpy(&outConnection->txBuffer[dataOffset], inConnection->rxBuffer, inIproc->length);
if (inIproc->length > 0) {
memcpy(&outConnection->txBuffer[headerLength], inConnection->rxBuffer, inIproc->length);
}
// Store the packet length
outConnection->txBuffer[2] = (uint8_t)((dataOffset + inIproc->length) & 0xFF);
outConnection->txBuffer[3] = (uint8_t)(((dataOffset + inIproc->length) >> 8) & 0xFF);
outConnection->txBuffer[2] = (uint8_t)((headerLength + inIproc->length) & 0xFF);
outConnection->txBuffer[3] = (uint8_t)(((headerLength + inIproc->length) >> 8) & 0xFF);
// Copy the checksum
outConnection->txBuffer[dataOffset + inIproc->length] = inIproc->cs;
outConnection->txBuffer[headerLength + inIproc->length] = inIproc->cs;
// Send the buffer.
int32_t rc = (*outConnection->outStream)(outConnection->txBuffer, inIproc->rxPacketLength);
int32_t rc = (*outConnection->outStream)(outConnection->txBuffer, headerLength + inIproc->length + UAVTALK_CHECKSUM_LENGTH);
// Update stats
outConnection->stats.txBytes += rc;
outConnection->stats.txBytes += (rc > 0) ? rc : 0;
// evaluate return value before releasing the lock
UAVTalkRxState rxState = 0;
if (rc != (int32_t)(headerLength + inIproc->length + UAVTALK_CHECKSUM_LENGTH)) {
outConnection->stats.txErrors++;
rxState = -1;
}
// Release lock
xSemaphoreGiveRecursive(outConnection->lock);
// Done
if (rc != inIproc->rxPacketLength) {
return -1;
}
return 0;
return rxState;
}
/**
@ -667,66 +680,18 @@ int32_t UAVTalkReceiveObject(UAVTalkConnection connectionHandle)
UAVTalkConnectionData *connection;
CHECKCONHANDLE(connectionHandle, connection, return -1);
UAVTalkInputProcessor *iproc = &connection->iproc;
if (iproc->state != UAVTALK_STATE_COMPLETE) {
return -1;
}
receiveObject(connection, iproc->type, iproc->objId, iproc->instId, connection->rxBuffer, iproc->length);
return 0;
}
/**
* Send a ACK through the telemetry link.
* \param[in] connectionHandle UAVTalkConnection to be used
* \param[in] objId Object ID to send a NACK for
* \return 0 Success
* \return -1 Failure
*/
int32_t UAVTalkSendAck(UAVTalkConnection connectionHandle, UAVObjHandle obj, uint16_t instId)
{
UAVTalkConnectionData *connection;
CHECKCONHANDLE(connectionHandle, connection, return -1);
// Lock
xSemaphoreTakeRecursive(connection->lock, portMAX_DELAY);
int32_t ret = sendObject(connection, obj, instId, UAVTALK_TYPE_ACK);
// Release lock
xSemaphoreGiveRecursive(connection->lock);
return ret;
}
/**
* Send a NACK through the telemetry link.
* \param[in] connectionHandle UAVTalkConnection to be used
* \param[in] objId Object ID to send a NACK for
* \return 0 Success
* \return -1 Failure
*/
int32_t UAVTalkSendNack(UAVTalkConnection connectionHandle, uint32_t objId)
{
UAVTalkConnectionData *connection;
CHECKCONHANDLE(connectionHandle, connection, return -1);
// Lock
xSemaphoreTakeRecursive(connection->lock, portMAX_DELAY);
int32_t ret = sendNack(connection, objId);
// Release lock
xSemaphoreGiveRecursive(connection->lock);
return ret;
return receiveObject(connection, iproc->type, iproc->objId, iproc->instId, connection->rxBuffer);
}
/**
* Get the object ID of the current packet.
* \param[in] connectionHandle UAVTalkConnection to be used
* \param[in] objId Object ID to send a NACK for
* \return The object ID, or 0 on error.
*/
uint32_t UAVTalkGetPacketObjId(UAVTalkConnection connectionHandle)
@ -734,11 +699,19 @@ uint32_t UAVTalkGetPacketObjId(UAVTalkConnection connectionHandle)
UAVTalkConnectionData *connection;
CHECKCONHANDLE(connectionHandle, connection, return 0);
return connection->iproc.objId;
}
/**
* Receive an object. This function process objects received through the telemetry stream.
*
* Parser errors are considered as transmission errors and are not NACKed.
* Some senders (GCS) can timeout and retry if the message is not answered by an ack or nack.
*
* Object handling errors are considered as application errors and are NACked.
* In that case we want to nack as there is no point in the sender retrying to send invalid objects.
*
* \param[in] connection UAVTalkConnection to be used
* \param[in] type Type of received message (UAVTALK_TYPE_OBJ, UAVTALK_TYPE_OBJ_REQ, UAVTALK_TYPE_OBJ_ACK, UAVTALK_TYPE_ACK, UAVTALK_TYPE_NACK)
* \param[in] objId ID of the object to work on
@ -748,12 +721,7 @@ 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,
__attribute__((unused)) int32_t length)
static int32_t receiveObject(UAVTalkConnectionData *connection, uint8_t type, uint32_t objId, uint16_t instId, uint8_t *data)
{
UAVObjHandle obj;
int32_t ret = 0;
@ -761,33 +729,25 @@ static int32_t receiveObject(UAVTalkConnectionData *connection,
// Lock
xSemaphoreTakeRecursive(connection->lock, portMAX_DELAY);
// Get the handle to the Object. Will be zero
// if object does not exist.
// Get the handle to the object. Will be null if object does not exist.
// Warning :
// Here we ask for instance ID 0 without taking into account the provided instId
// The provided instId will be used later when packing, unpacking, etc...
// TODO the above should be fixed as it is cumbersome and error prone
obj = UAVObjGetByID(objId);
// Process message type
switch (type) {
case UAVTALK_TYPE_OBJ:
case UAVTALK_TYPE_OBJ_TS:
// All instances, not allowed for OBJ messages
if (obj && (instId != UAVOBJ_ALL_INSTANCES)) {
// Unpack object, if the instance does not exist it will be created!
UAVObjUnpack(obj, instId, data);
// Check if an ack is pending
updateAck(connection, obj, instId);
} else {
ret = -1;
}
break;
case UAVTALK_TYPE_OBJ_ACK:
case UAVTALK_TYPE_OBJ_ACK_TS:
// All instances, not allowed for OBJ_ACK messages
// All instances not allowed for OBJ messages
if (obj && (instId != UAVOBJ_ALL_INSTANCES)) {
// Unpack object, if the instance does not exist it will be created!
if (UAVObjUnpack(obj, instId, data) == 0) {
// Transmit ACK
sendObject(connection, obj, instId, UAVTALK_TYPE_ACK);
// Check if this object acks a pending OBJ_REQ message
// any OBJ message can ack a pending OBJ_REQ message
// even one that was not sent in response to the OBJ_REQ message
updateAck(connection, type, objId, instId);
} else {
ret = -1;
}
@ -795,26 +755,68 @@ static int32_t receiveObject(UAVTalkConnectionData *connection,
ret = -1;
}
break;
case UAVTALK_TYPE_OBJ_REQ:
// Send requested object if message is of type OBJ_REQ
if (obj == 0) {
sendNack(connection, objId);
case UAVTALK_TYPE_OBJ_ACK:
case UAVTALK_TYPE_OBJ_ACK_TS:
UAVT_DEBUGLOG_CPRINTF(objId, "OBJ_ACK %X %d", objId, instId);
// All instances not allowed for OBJ_ACK messages
if (obj && (instId != UAVOBJ_ALL_INSTANCES)) {
// Unpack object, if the instance does not exist it will be created!
if (UAVObjUnpack(obj, instId, data) == 0) {
UAVT_DEBUGLOG_CPRINTF(objId, "OBJ ACK %X %d", objId, instId);
// Object updated or created, transmit ACK
sendObject(connection, UAVTALK_TYPE_ACK, objId, instId, NULL);
} else {
ret = -1;
}
} else {
sendObject(connection, obj, instId, UAVTALK_TYPE_OBJ);
ret = -1;
}
if (ret == -1) {
// failed to update object, transmit NACK
UAVT_DEBUGLOG_PRINTF("OBJ NACK %X %d", objId, instId);
sendObject(connection, UAVTALK_TYPE_NACK, objId, instId, NULL);
}
break;
case UAVTALK_TYPE_OBJ_REQ:
// Check if requested object exists
UAVT_DEBUGLOG_CPRINTF(objId, "REQ %X %d", objId, instId);
if (obj) {
// Object found, transmit it
// The sent object will ack the object request on the receiver side
ret = sendObject(connection, UAVTALK_TYPE_OBJ, objId, instId, obj);
} else {
ret = -1;
}
if (ret == -1) {
// failed to send object, transmit NACK
UAVT_DEBUGLOG_PRINTF("REQ NACK %X %d", objId, instId);
sendObject(connection, UAVTALK_TYPE_NACK, objId, instId, NULL);
}
break;
case UAVTALK_TYPE_NACK:
// Do nothing on flight side, let it time out.
// TODO:
// The transaction takes the result code of the "semaphore taking operation" into account to determine success.
// If we give that semaphore in time, its "success" (ack received)
// If we do not give that semaphore before the timeout it will return failure.
// What would have to be done here is give the semaphore, but set a flag (for example connection->respFail=true)
// that indicates failure and then above where it checks for the result code, have it behave as if it failed
// if the explicit failure is set.
break;
case UAVTALK_TYPE_ACK:
// All instances, not allowed for ACK messages
// All instances not allowed for ACK messages
if (obj && (instId != UAVOBJ_ALL_INSTANCES)) {
// Check if an ack is pending
updateAck(connection, obj, instId);
// Check if an ACK is pending
updateAck(connection, type, objId, instId);
} else {
ret = -1;
}
break;
default:
ret = -1;
}
@ -832,30 +834,40 @@ static int32_t receiveObject(UAVTalkConnectionData *connection,
* \param[in] obj Object
* \param[in] instId The instance ID of UAVOBJ_ALL_INSTANCES for all instances.
*/
static void updateAck(UAVTalkConnectionData *connection, UAVObjHandle obj, uint16_t instId)
static void updateAck(UAVTalkConnectionData *connection, uint8_t type, uint32_t objId, uint16_t instId)
{
if (connection->respObj == obj && (connection->respInstId == instId || connection->respInstId == UAVOBJ_ALL_INSTANCES)) {
xSemaphoreGive(connection->respSema);
connection->respObj = 0;
if ((connection->respObjId == objId) && (connection->respType == type)) {
if ((connection->respInstId == UAVOBJ_ALL_INSTANCES) && (instId == 0)) {
// last instance received, complete transaction
xSemaphoreGive(connection->respSema);
connection->respObjId = 0;
} else if (connection->respInstId == instId) {
xSemaphoreGive(connection->respSema);
connection->respObjId = 0;
}
}
}
/**
* Send an object through the telemetry link.
* \param[in] connection UAVTalkConnection to be used
* \param[in] obj Object handle to send
* \param[in] instId The instance ID or UAVOBJ_ALL_INSTANCES for all instances
* \param[in] type Transaction type
* \param[in] objId The object ID
* \param[in] instId The instance ID or UAVOBJ_ALL_INSTANCES for all instances
* \param[in] obj Object handle to send (null when type is NACK)
* \return 0 Success
* \return -1 Failure
*/
static int32_t sendObject(UAVTalkConnectionData *connection, UAVObjHandle obj, uint16_t instId, uint8_t type)
static int32_t sendObject(UAVTalkConnectionData *connection, uint8_t type, uint32_t objId, uint16_t instId, UAVObjHandle obj)
{
uint32_t numInst;
uint32_t n;
uint32_t ret = -1;
// Important note : obj can be null (when type is NACK for example) so protect all obj dereferences.
// If all instances are requested and this is a single instance object, force instance ID to zero
if (instId == UAVOBJ_ALL_INSTANCES && UAVObjIsSingleInstance(obj)) {
if ((obj != NULL) && (instId == UAVOBJ_ALL_INSTANCES) && UAVObjIsSingleInstance(obj)) {
instId = 0;
}
@ -864,153 +876,117 @@ static int32_t sendObject(UAVTalkConnectionData *connection, UAVObjHandle obj, u
if (instId == UAVOBJ_ALL_INSTANCES) {
// Get number of instances
numInst = UAVObjGetNumInstances(obj);
// Send all instances
// 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;
for (n = 0; n < numInst; ++n) {
sendSingleObject(connection, obj, n, type);
if (sendSingleObject(connection, type, objId, numInst - n - 1, obj) == -1) {
ret = -1;
break;
}
}
return 0;
} else {
return sendSingleObject(connection, obj, instId, type);
ret = sendSingleObject(connection, type, objId, instId, obj);
}
} else if (type == UAVTALK_TYPE_OBJ_REQ) {
return sendSingleObject(connection, obj, instId, UAVTALK_TYPE_OBJ_REQ);
} else if (type == UAVTALK_TYPE_ACK) {
ret = sendSingleObject(connection, type, objId, instId, obj);
} else if (type == UAVTALK_TYPE_ACK || type == UAVTALK_TYPE_NACK) {
if (instId != UAVOBJ_ALL_INSTANCES) {
return sendSingleObject(connection, obj, instId, UAVTALK_TYPE_ACK);
} else {
return -1;
ret = sendSingleObject(connection, type, objId, instId, obj);
}
} else {
return -1;
}
return ret;
}
/**
* Send an object through the telemetry link.
* \param[in] connection UAVTalkConnection to be used
* \param[in] obj Object handle to send
* \param[in] instId The instance ID (can NOT be UAVOBJ_ALL_INSTANCES, use sendObject() instead)
* \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)
* \param[in] obj Object handle to send (null when type is NACK)
* \return 0 Success
* \return -1 Failure
*/
static int32_t sendSingleObject(UAVTalkConnectionData *connection, UAVObjHandle obj, uint16_t instId, uint8_t type)
static int32_t sendSingleObject(UAVTalkConnectionData *connection, uint8_t type, uint32_t objId, uint16_t instId, UAVObjHandle obj)
{
int32_t length;
int32_t dataOffset;
uint32_t objId;
// IMPORTANT : obj can be null (when type is NACK for example)
if (!connection->outStream) {
connection->stats.txErrors++;
return -1;
}
// Setup type and object id fields
objId = UAVObjGetID(obj);
connection->txBuffer[0] = UAVTALK_SYNC_VAL; // sync byte
// Setup sync byte
connection->txBuffer[0] = UAVTALK_SYNC_VAL;
// Setup type
connection->txBuffer[1] = type;
// data length inserted here below
// next 2 bytes are reserved for data length (inserted here later)
// Setup object ID
connection->txBuffer[4] = (uint8_t)(objId & 0xFF);
connection->txBuffer[5] = (uint8_t)((objId >> 8) & 0xFF);
connection->txBuffer[6] = (uint8_t)((objId >> 16) & 0xFF);
connection->txBuffer[7] = (uint8_t)((objId >> 24) & 0xFF);
// Setup instance ID if one is required
if (UAVObjIsSingleInstance(obj)) {
dataOffset = 8;
} else {
connection->txBuffer[8] = (uint8_t)(instId & 0xFF);
connection->txBuffer[9] = (uint8_t)((instId >> 8) & 0xFF);
dataOffset = 10;
}
// Setup instance ID
connection->txBuffer[8] = (uint8_t)(instId & 0xFF);
connection->txBuffer[9] = (uint8_t)((instId >> 8) & 0xFF);
int32_t headerLength = 10;
// Add timestamp when the transaction type is appropriate
if (type & UAVTALK_TIMESTAMPED) {
portTickType time = xTaskGetTickCount();
connection->txBuffer[dataOffset] = (uint8_t)(time & 0xFF);
connection->txBuffer[dataOffset + 1] = (uint8_t)((time >> 8) & 0xFF);
dataOffset += 2;
connection->txBuffer[10] = (uint8_t)(time & 0xFF);
connection->txBuffer[11] = (uint8_t)((time >> 8) & 0xFF);
headerLength += 2;
}
// Determine data length
if (type == UAVTALK_TYPE_OBJ_REQ || type == UAVTALK_TYPE_ACK) {
int32_t length;
if (type == UAVTALK_TYPE_OBJ_REQ || type == UAVTALK_TYPE_ACK || type == UAVTALK_TYPE_NACK) {
length = 0;
} else {
length = UAVObjGetNumBytes(obj);
}
// Check length
if (length >= UAVTALK_MAX_PAYLOAD_LENGTH) {
if (length > UAVOBJECTS_LARGEST) {
connection->stats.txErrors++;
return -1;
}
// Copy data (if any)
if (length > 0) {
if (UAVObjPack(obj, instId, &connection->txBuffer[dataOffset]) < 0) {
if (UAVObjPack(obj, instId, &connection->txBuffer[headerLength]) == -1) {
connection->stats.txErrors++;
return -1;
}
}
// Store the packet length
connection->txBuffer[2] = (uint8_t)((dataOffset + length) & 0xFF);
connection->txBuffer[3] = (uint8_t)(((dataOffset + length) >> 8) & 0xFF);
connection->txBuffer[2] = (uint8_t)((headerLength + length) & 0xFF);
connection->txBuffer[3] = (uint8_t)(((headerLength + length) >> 8) & 0xFF);
// Calculate checksum
connection->txBuffer[dataOffset + length] = PIOS_CRC_updateCRC(0, connection->txBuffer, dataOffset + length);
// Calculate and store checksum
connection->txBuffer[headerLength + length] = PIOS_CRC_updateCRC(0, connection->txBuffer, headerLength + length);
uint16_t tx_msg_len = dataOffset + length + UAVTALK_CHECKSUM_LENGTH;
// Send object
uint16_t tx_msg_len = headerLength + length + UAVTALK_CHECKSUM_LENGTH;
int32_t rc = (*connection->outStream)(connection->txBuffer, tx_msg_len);
// Update stats
if (rc == tx_msg_len) {
// Update stats
++connection->stats.txObjects;
connection->stats.txBytes += tx_msg_len;
connection->stats.txObjectBytes += length;
}
// Done
return 0;
}
/**
* Send a NACK through the telemetry link.
* \param[in] connection UAVTalkConnection to be used
* \param[in] objId Object ID to send a NACK for
* \return 0 Success
* \return -1 Failure
*/
static int32_t sendNack(UAVTalkConnectionData *connection, uint32_t objId)
{
int32_t dataOffset;
if (!connection->outStream) {
connection->stats.txBytes += tx_msg_len;
} else {
connection->stats.txErrors++;
// TDOD rc == -1 connection not open, -2 buffer full should retry
connection->stats.txBytes += (rc > 0) ? rc : 0;
return -1;
}
connection->txBuffer[0] = UAVTALK_SYNC_VAL; // sync byte
connection->txBuffer[1] = UAVTALK_TYPE_NACK;
// data length inserted here below
connection->txBuffer[4] = (uint8_t)(objId & 0xFF);
connection->txBuffer[5] = (uint8_t)((objId >> 8) & 0xFF);
connection->txBuffer[6] = (uint8_t)((objId >> 16) & 0xFF);
connection->txBuffer[7] = (uint8_t)((objId >> 24) & 0xFF);
dataOffset = 8;
// Store the packet length
connection->txBuffer[2] = (uint8_t)((dataOffset) & 0xFF);
connection->txBuffer[3] = (uint8_t)(((dataOffset) >> 8) & 0xFF);
// Calculate checksum
connection->txBuffer[dataOffset] = PIOS_CRC_updateCRC(0, connection->txBuffer, dataOffset);
uint16_t tx_msg_len = dataOffset + UAVTALK_CHECKSUM_LENGTH;
int32_t rc = (*connection->outStream)(connection->txBuffer, tx_msg_len);
if (rc == tx_msg_len) {
// Update stats
connection->stats.txBytes += tx_msg_len;
}
// Done
return 0;
}

5
ground/openpilotgcs/src/libs/opmapcontrol/src/mapwidget/waypointitem.cpp
View File

@ -292,26 +292,21 @@ void WayPointItem::setRelativeCoord(distBearingAltitude value)
void WayPointItem::SetCoord(const internals::PointLatLng &value)
{
qDebug() << "1 SetCoord(" << value.Lat() << "," << value.Lng() << ")" << "OLD:" << Coord().Lat() << "," << Coord().Lng();
if (qAbs(Coord().Lat() - value.Lat()) < 0.0001 && qAbs(Coord().Lng() - value.Lng()) < 0.0001) {
qDebug() << "2 SetCoord nothing changed returning";
return;
}
qDebug() << "3 setCoord there were changes";
coord = value;
distBearingAltitude back = relativeCoord;
if (myHome) {
map->Projection()->offSetFromLatLngs(myHome->Coord(), Coord(), back.distance, back.bearing);
}
if (qAbs(back.bearing - relativeCoord.bearing) > 0.01 || qAbs(back.distance - relativeCoord.distance) > 0.1) {
qDebug() << "4 setCoord-relative coordinates where also updated";
relativeCoord = back;
}
emit WPValuesChanged(this);
RefreshPos();
RefreshToolTip();
this->update();
qDebug() << "5 setCoord EXIT";
}
void WayPointItem::SetDescription(const QString &value)
{

74
ground/openpilotgcs/src/libs/utils/crc.cpp Normal file
View File

@ -0,0 +1,74 @@
/**
******************************************************************************
*
* @file crc.cpp
* @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2013.
* Parts by Nokia Corporation (qt-info@nokia.com) Copyright (C) 2009.
* @addtogroup GCSPlugins GCS Plugins
* @{
* @addtogroup CorePlugin Core Plugin
* @{
* @brief The Core GCS plugin
*****************************************************************************/
/*
* This program is free software; you can redistribute it and/or modify
* it under the terms of the GNU General Public License as published by
* the Free Software Foundation; either version 3 of the License, or
* (at your option) any later version.
*
* This program is distributed in the hope that it will be useful, but
* WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY
* or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License
* for more details.
*
* You should have received a copy of the GNU General Public License along
* with this program; if not, write to the Free Software Foundation, Inc.,
* 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA
*/
#include "crc.h"
using namespace Utils;
/*
* Generated by pycrc v0.7.5, http://www.tty1.net/pycrc/
* using the configuration:
* Width = 8
* Poly = 0x07
* XorIn = 0x00
* ReflectIn = False
* XorOut = 0x00
* ReflectOut = False
* Algorithm = table-driven
*/
const quint8 crc_table[256] = {
0x00, 0x07, 0x0e, 0x09, 0x1c, 0x1b, 0x12, 0x15, 0x38, 0x3f, 0x36, 0x31, 0x24, 0x23, 0x2a, 0x2d,
0x70, 0x77, 0x7e, 0x79, 0x6c, 0x6b, 0x62, 0x65, 0x48, 0x4f, 0x46, 0x41, 0x54, 0x53, 0x5a, 0x5d,
0xe0, 0xe7, 0xee, 0xe9, 0xfc, 0xfb, 0xf2, 0xf5, 0xd8, 0xdf, 0xd6, 0xd1, 0xc4, 0xc3, 0xca, 0xcd,
0x90, 0x97, 0x9e, 0x99, 0x8c, 0x8b, 0x82, 0x85, 0xa8, 0xaf, 0xa6, 0xa1, 0xb4, 0xb3, 0xba, 0xbd,
0xc7, 0xc0, 0xc9, 0xce, 0xdb, 0xdc, 0xd5, 0xd2, 0xff, 0xf8, 0xf1, 0xf6, 0xe3, 0xe4, 0xed, 0xea,
0xb7, 0xb0, 0xb9, 0xbe, 0xab, 0xac, 0xa5, 0xa2, 0x8f, 0x88, 0x81, 0x86, 0x93, 0x94, 0x9d, 0x9a,
0x27, 0x20, 0x29, 0x2e, 0x3b, 0x3c, 0x35, 0x32, 0x1f, 0x18, 0x11, 0x16, 0x03, 0x04, 0x0d, 0x0a,
0x57, 0x50, 0x59, 0x5e, 0x4b, 0x4c, 0x45, 0x42, 0x6f, 0x68, 0x61, 0x66, 0x73, 0x74, 0x7d, 0x7a,
0x89, 0x8e, 0x87, 0x80, 0x95, 0x92, 0x9b, 0x9c, 0xb1, 0xb6, 0xbf, 0xb8, 0xad, 0xaa, 0xa3, 0xa4,
0xf9, 0xfe, 0xf7, 0xf0, 0xe5, 0xe2, 0xeb, 0xec, 0xc1, 0xc6, 0xcf, 0xc8, 0xdd, 0xda, 0xd3, 0xd4,
0x69, 0x6e, 0x67, 0x60, 0x75, 0x72, 0x7b, 0x7c, 0x51, 0x56, 0x5f, 0x58, 0x4d, 0x4a, 0x43, 0x44,
0x19, 0x1e, 0x17, 0x10, 0x05, 0x02, 0x0b, 0x0c, 0x21, 0x26, 0x2f, 0x28, 0x3d, 0x3a, 0x33, 0x34,
0x4e, 0x49, 0x40, 0x47, 0x52, 0x55, 0x5c, 0x5b, 0x76, 0x71, 0x78, 0x7f, 0x6a, 0x6d, 0x64, 0x63,
0x3e, 0x39, 0x30, 0x37, 0x22, 0x25, 0x2c, 0x2b, 0x06, 0x01, 0x08, 0x0f, 0x1a, 0x1d, 0x14, 0x13,
0xae, 0xa9, 0xa0, 0xa7, 0xb2, 0xb5, 0xbc, 0xbb, 0x96, 0x91, 0x98, 0x9f, 0x8a, 0x8d, 0x84, 0x83,
0xde, 0xd9, 0xd0, 0xd7, 0xc2, 0xc5, 0xcc, 0xcb, 0xe6, 0xe1, 0xe8, 0xef, 0xfa, 0xfd, 0xf4, 0xf3
};
quint8 Crc::updateCRC(quint8 crc, const quint8 data)
{
return crc_table[crc ^ data];
}
quint8 Crc::updateCRC(quint8 crc, const quint8 *data, qint32 length)
{
while (length--) {
crc = crc_table[crc ^ *data++];
}
return crc;
}

58
ground/openpilotgcs/src/libs/utils/crc.h Normal file
View File

@ -0,0 +1,58 @@
/**
******************************************************************************
*
* @file crc.h
* @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2013.
* Parts by Nokia Corporation (qt-info@nokia.com) Copyright (C) 2009.
* @addtogroup GCSPlugins GCS Plugins
* @{
* @addtogroup CorePlugin Core Plugin
* @{
* @brief The Core GCS plugin
*****************************************************************************/
/*
* This program is free software; you can redistribute it and/or modify
* it under the terms of the GNU General Public License as published by
* the Free Software Foundation; either version 3 of the License, or
* (at your option) any later version.
*
* This program is distributed in the hope that it will be useful, but
* WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY
* or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License
* for more details.
*
* You should have received a copy of the GNU General Public License along
* with this program; if not, write to the Free Software Foundation, Inc.,
* 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA
*/
#ifndef CRC_H
#define CRC_H
#include "utils_global.h"
namespace Utils {
class QTCREATOR_UTILS_EXPORT Crc {
public:
/**
* Update the crc value with new data.
*
* \param crc The current crc value.
* \param data data to update the crc with.
* \return The updated crc value.
*/
static quint8 updateCRC(quint8 crc, const quint8 data);
/**
* Update the crc value with new data.
*
* \param crc The current crc value.
* \param data Pointer to a buffer of \a data_len bytes.
* \param length Number of bytes in the \a data buffer.
* \return The updated crc value.
*/
static quint8 updateCRC(quint8 crc, const quint8 *data, qint32 length);
};
} // namespace Utils
#endif // CRC_H

6
ground/openpilotgcs/src/libs/utils/utils.pro
View File

@ -55,7 +55,8 @@ SOURCES += reloadpromptutils.cpp \
cachedsvgitem.cpp \
svgimageprovider.cpp \
hostosinfo.cpp \
logfile.cpp
logfile.cpp \
crc.cpp
SOURCES += xmlconfig.cpp
@ -113,7 +114,8 @@ HEADERS += utils_global.h \
cachedsvgitem.h \
svgimageprovider.h \
hostosinfo.h \
logfile.h
logfile.h \
crc.h
HEADERS += xmlconfig.h

1
ground/openpilotgcs/src/plugins/config/cfg_vehicletypes/configcustomwidget.h
View File

@ -33,7 +33,6 @@
#include "extensionsystem/pluginmanager.h"
#include "uavobjectmanager.h"
#include "uavobject.h"
#include "uavtalk/telemetrymanager.h"
#include <QWidget>
#include <QList>

2
ground/openpilotgcs/src/plugins/config/cfg_vehicletypes/configfixedwingwidget.h
View File

@ -33,7 +33,7 @@
#include "extensionsystem/pluginmanager.h"
#include "uavobjectmanager.h"
#include "uavobject.h"
#include "uavtalk/telemetrymanager.h"
#include <QWidget>
#include <QList>
#include <QItemDelegate>

2
ground/openpilotgcs/src/plugins/config/cfg_vehicletypes/configgroundvehiclewidget.h
View File

@ -33,7 +33,7 @@
#include "extensionsystem/pluginmanager.h"
#include "uavobjectmanager.h"
#include "uavobject.h"
#include "uavtalk/telemetrymanager.h"
#include <QWidget>
#include <QList>
#include <QItemDelegate>

1
ground/openpilotgcs/src/plugins/config/cfg_vehicletypes/configmultirotorwidget.h
View File

@ -33,7 +33,6 @@
#include "extensionsystem/pluginmanager.h"
#include "uavobjectmanager.h"
#include "uavobject.h"
#include "uavtalk/telemetrymanager.h"
#include <QtCore/QList>
#include <QWidget>

2
ground/openpilotgcs/src/plugins/config/configgadgetwidget.cpp
View File

@ -42,6 +42,8 @@
#include "defaulthwsettingswidget.h"
#include "uavobjectutilmanager.h"
#include <uavtalk/telemetrymanager.h>
#include <QDebug>
#include <QStringList>
#include <QWidget>

8
ground/openpilotgcs/src/plugins/config/configgadgetwidget.h
View File

@ -27,18 +27,18 @@
#ifndef CONFIGGADGETWIDGET_H
#define CONFIGGADGETWIDGET_H
#include "uavtalk/telemetrymanager.h"
#include "extensionsystem/pluginmanager.h"
#include "uavobjectmanager.h"
#include "uavobject.h"
#include "objectpersistence.h"
#include "utils/pathutils.h"
#include "utils/mytabbedstackwidget.h"
#include "../uavobjectwidgetutils/configtaskwidget.h"
#include <QWidget>
#include <QList>
#include <QTextBrowser>
#include "utils/pathutils.h"
#include <QMessageBox>
#include "utils/mytabbedstackwidget.h"
#include "../uavobjectwidgetutils/configtaskwidget.h"
class ConfigGadgetWidget : public QWidget {
Q_OBJECT

6
ground/openpilotgcs/src/plugins/config/configinputwidget.cpp
View File

@ -27,7 +27,8 @@
#include "configinputwidget.h"
#include "uavtalk/telemetrymanager.h"
#include <extensionsystem/pluginmanager.h>
#include <coreplugin/generalsettings.h>
#include <QDebug>
#include <QStringList>
@ -41,9 +42,6 @@
#include <utils/stylehelper.h>
#include <QMessageBox>
#include <extensionsystem/pluginmanager.h>
#include <coreplugin/generalsettings.h>
#define ACCESS_MIN_MOVE -3
#define ACCESS_MAX_MOVE 3
#define STICK_MIN_MOVE -8

17
ground/openpilotgcs/src/plugins/config/configoutputwidget.cpp
View File

@ -29,7 +29,14 @@
#include "outputchannelform.h"
#include "configvehicletypewidget.h"
#include "uavtalk/telemetrymanager.h"
#include "mixersettings.h"
#include "actuatorcommand.h"
#include "actuatorsettings.h"
#include "systemalarms.h"
#include "systemsettings.h"
#include "uavsettingsimportexport/uavsettingsimportexportfactory.h"
#include <extensionsystem/pluginmanager.h>
#include <coreplugin/generalsettings.h>
#include <QDebug>
#include <QStringList>
@ -40,14 +47,6 @@
#include <QMessageBox>
#include <QDesktopServices>
#include <QUrl>
#include "mixersettings.h"
#include "actuatorcommand.h"
#include "actuatorsettings.h"
#include "systemalarms.h"
#include "systemsettings.h"
#include "uavsettingsimportexport/uavsettingsimportexportfactory.h"
#include <extensionsystem/pluginmanager.h>
#include <coreplugin/generalsettings.h>
ConfigOutputWidget::ConfigOutputWidget(QWidget *parent) : ConfigTaskWidget(parent), wasItMe(false)
{

1
ground/openpilotgcs/src/plugins/config/configplugin.h
View File

@ -31,7 +31,6 @@
#include <coreplugin/icore.h>
#include <coreplugin/coreconstants.h>
#include <coreplugin/actionmanager/actionmanager.h>
#include "uavtalk/telemetrymanager.h"
#include "objectpersistence.h"
#include <QMessageBox>

6
ground/openpilotgcs/src/plugins/coreplugin/mainwindow.cpp
View File

@ -188,7 +188,8 @@ MainWindow::MainWindow() :
MainWindow::~MainWindow()
{
if (m_connectionManager) { // Pip
if (m_connectionManager) {
// Pip
m_connectionManager->disconnectDevice();
m_connectionManager->suspendPolling();
}
@ -351,6 +352,9 @@ void MainWindow::closeEvent(QCloseEvent *event)
saveSettings(m_settings);
m_uavGadgetInstanceManager->saveSettings(m_settings);
}
qApp->closeAllWindows();
event->accept();
}

11
ground/openpilotgcs/src/plugins/hitl/isimulator.h
View File

@ -1,12 +1,6 @@
#ifndef ISIMULATOR_H
#define ISIMULATOR_H
#include <QObject>
#include <QUdpSocket>
#include <QTimer>
#include <math.h>
#include "uavtalk/telemetrymanager.h"
#include "uavobjectmanager.h"
#include "actuatordesired.h"
#include "altitudestate.h"
@ -15,6 +9,11 @@
#include "positionstate.h"
#include "gcstelemetrystats.h"
#include <QObject>
#include <QUdpSocket>
#include <QTimer>
#include <math.h>
class Simulator : public QObject {
Q_OBJECT
public:

8
ground/openpilotgcs/src/plugins/hitl/simulator.cpp
View File

@ -27,11 +27,13 @@
#include "simulator.h"
#include "extensionsystem/pluginmanager.h"
#include "coreplugin/icore.h"
#include "coreplugin/threadmanager.h"
#include "hitlnoisegeneration.h"
#include <extensionsystem/pluginmanager.h>
#include <coreplugin/icore.h>
#include <coreplugin/threadmanager.h>
#include <uavtalk/telemetrymanager.h>
volatile bool Simulator::isStarted = false;
const float Simulator::GEE = 9.81;

14
ground/openpilotgcs/src/plugins/hitl/simulator.h
View File

@ -28,14 +28,7 @@
#ifndef ISIMULATOR_H
#define ISIMULATOR_H
#include <QObject>
#include <QUdpSocket>
#include <QTimer>
#include <QProcess>
#include <qmath.h>
#include "qscopedpointer.h"
#include "uavtalk/telemetrymanager.h"
#include "uavobjectmanager.h"
#include "accelstate.h"
@ -61,6 +54,13 @@
#include "utils/coordinateconversions.h"
#include <QObject>
#include <QUdpSocket>
#include <QTime>
#include <QTimer>
#include <QProcess>
#include <qmath.h>
/**
* just imagine this was a class without methods and all public properties
*/

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

@ -29,15 +29,17 @@
#include "notificationitem.h"
#include "notifypluginoptionspage.h"
#include "notifylogging.h"
#include <extensionsystem/pluginmanager.h>
#include <coreplugin/icore.h>
#include <uavtalk/telemetrymanager.h>
#include <iostream>
#include <QDebug>
#include <QtPlugin>
#include <QStringList>
#include <extensionsystem/pluginmanager.h>
#include <iostream>
static const QString VERSION = "1.0.0";
// #define DEBUG_NOTIFIES
@ -125,7 +127,8 @@ void SoundNotifyPlugin::readConfig(QSettings *settings, UAVConfigInfo * /* confi
void SoundNotifyPlugin::onTelemetryManagerAdded(QObject *obj)
{
telMngr = qobject_cast<TelemetryManager *>(obj);
TelemetryManager *telMngr = qobject_cast<TelemetryManager *>(obj);
if (telMngr) {
connect(telMngr, SIGNAL(disconnected()), this, SLOT(onAutopilotDisconnect()));
}

2
ground/openpilotgcs/src/plugins/notify/notifyplugin.h
View File

@ -29,7 +29,6 @@
#include <extensionsystem/iplugin.h>
#include <coreplugin/iconfigurableplugin.h>
#include "uavtalk/telemetrymanager.h"
#include "uavobjectmanager.h"
#include "uavobject.h"
#include "notificationitem.h"
@ -111,7 +110,6 @@ private:
PhononObject phonon;
NotifyPluginOptionsPage *mop;
TelemetryManager *telMngr;
QMediaPlaylist *playlist;
};

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

@ -28,6 +28,7 @@
#include "flightdatamodel.h"
#include <QMessageBox>
#include <QDomDocument>
flightDataModel::flightDataModel(QObject *parent) : QAbstractTableModel(parent)
{}
@ -72,329 +73,275 @@ 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();
return true;
b = true;
break;
case LATPOSITION:
row->latPosition = value.toDouble();
return true;
b = true;
break;
case LNGPOSITION:
row->lngPosition = value.toDouble();
return true;
b = true;
break;
case DISRELATIVE:
row->disRelative = value.toDouble();
return true;
b = true;
break;
case BEARELATIVE:
row->beaRelative = value.toDouble();
return true;
b = true;
break;
case ALTITUDERELATIVE:
row->altitudeRelative = value.toFloat();
return true;
b = true;
break;
case ISRELATIVE:
row->isRelative = value.toBool();
return true;
b = true;
break;
case ALTITUDE:
row->altitude = value.toDouble();
return true;
b = true;
break;
case VELOCITY:
row->velocity = value.toFloat();
return true;
b = true;
break;
case MODE:
row->mode = value.toInt();
return true;
b = true;
break;
case MODE_PARAMS0:
row->mode_params[0] = value.toFloat();
return true;
b = true;
break;
case MODE_PARAMS1:
row->mode_params[1] = value.toFloat();
return true;
b = true;
break;
case MODE_PARAMS2:
row->mode_params[2] = value.toFloat();
return true;
b = true;
break;
case MODE_PARAMS3:
row->mode_params[3] = value.toFloat();
return true;
b = true;
break;
case CONDITION:
row->condition = value.toInt();
return true;
b = true;
break;
case CONDITION_PARAMS0:
row->condition_params[0] = value.toFloat();
return true;
b = true;
break;
case CONDITION_PARAMS1:
row->condition_params[1] = value.toFloat();
return true;
b = true;
break;
case CONDITION_PARAMS2:
row->condition_params[2] = value.toFloat();
return true;
b = true;
break;
case CONDITION_PARAMS3:
row->condition_params[3] = value.toFloat();
return true;
b = true;
break;
case COMMAND:
row->command = value.toInt();
b = true;
break;
case JUMPDESTINATION:
row->jumpdestination = value.toInt();
return true;
b = true;
break;
case ERRORDESTINATION:
row->errordestination = value.toInt();
return true;
b = true;
break;
case LOCKED:
row->locked = value.toBool();
return true;
b = true;
break;
default:
return false;
b = false;
break;
}
return false;
return b;
}
QVariant flightDataModel::getColumnByIndex(const pathPlanData *row, const int index) const
{
QVariant value;
switch (index) {
case WPDESCRITPTION:
return row->wpDescritption;
value = row->wpDescritption;
break;
case LATPOSITION:
return row->latPosition;
value = row->latPosition;
break;
case LNGPOSITION:
return row->lngPosition;
value = row->lngPosition;
break;
case DISRELATIVE:
return row->disRelative;
value = row->disRelative;
break;
case BEARELATIVE:
return row->beaRelative;
value = row->beaRelative;
break;
case ALTITUDERELATIVE:
return row->altitudeRelative;
value = row->altitudeRelative;
break;
case ISRELATIVE:
return row->isRelative;
value = row->isRelative;
break;
case ALTITUDE:
return row->altitude;
value = row->altitude;
break;
case VELOCITY:
return row->velocity;
value = row->velocity;
break;
case MODE:
return row->mode;
value = row->mode;
break;
case MODE_PARAMS0:
return row->mode_params[0];
value = row->mode_params[0];
break;
case MODE_PARAMS1:
return row->mode_params[1];
value = row->mode_params[1];
break;
case MODE_PARAMS2:
return row->mode_params[2];
value = row->mode_params[2];
break;
case MODE_PARAMS3:
return row->mode_params[3];
value = row->mode_params[3];
break;
case CONDITION:
return row->condition;
value = row->condition;
break;
case CONDITION_PARAMS0:
return row->condition_params[0];
value = row->condition_params[0];
break;
case CONDITION_PARAMS1:
return row->condition_params[1];
value = row->condition_params[1];
break;
case CONDITION_PARAMS2:
return row->condition_params[2];
value = row->condition_params[2];
break;
case CONDITION_PARAMS3:
return row->condition_params[3];
value = row->condition_params[3];
break;
case COMMAND:
return row->command;
value = row->command;
break;
case JUMPDESTINATION:
return row->jumpdestination;
value = row->jumpdestination;
break;
case ERRORDESTINATION:
return row->errordestination;
value = row->errordestination;
break;
case LOCKED:
return row->locked;
value = row->locked;
break;
}
return value;
}
QVariant flightDataModel::headerData(int section, Qt::Orientation orientation, int role) const
{
QVariant value;
if (role == Qt::DisplayRole) {
if (orientation == Qt::Vertical) {
return QString::number(section + 1);
value = QString::number(section + 1);
} else if (orientation == Qt::Horizontal) {
switch (section) {
case WPDESCRITPTION:
return QString("Description");
value = QString("Description");
break;
case LATPOSITION:
return QString("Latitude");
value = QString("Latitude");
break;
case LNGPOSITION:
return QString("Longitude");
value = QString("Longitude");
break;
case DISRELATIVE:
return QString("Distance to home");
value = QString("Distance to home");
break;
case BEARELATIVE:
return QString("Bearing from home");
value = QString("Bearing from home");
break;
case ALTITUDERELATIVE:
return QString("Altitude above home");
value = QString("Altitude above home");
break;
case ISRELATIVE:
return QString("Relative to home");
value = QString("Relative to home");
break;
case ALTITUDE:
return QString("Altitude");
value = QString("Altitude");
break;
case VELOCITY:
return QString("Velocity");
value = QString("Velocity");
break;
case MODE:
return QString("Mode");
value = QString("Mode");
break;
case MODE_PARAMS0:
return QString("Mode parameter 0");
value = QString("Mode parameter 0");
break;
case MODE_PARAMS1:
return QString("Mode parameter 1");
value = QString("Mode parameter 1");
break;
case MODE_PARAMS2:
return QString("Mode parameter 2");
value = QString("Mode parameter 2");
break;
case MODE_PARAMS3:
return QString("Mode parameter 3");
value = QString("Mode parameter 3");
break;
case CONDITION:
return QString("Condition");
value = QString("Condition");
break;
case CONDITION_PARAMS0:
return QString("Condition parameter 0");
value = QString("Condition parameter 0");
break;
case CONDITION_PARAMS1:
return QString("Condition parameter 1");
value = QString("Condition parameter 1");
break;
case CONDITION_PARAMS2:
return QString("Condition parameter 2");
value = QString("Condition parameter 2");
break;
case CONDITION_PARAMS3:
return QString("Condition parameter 3");
value = QString("Condition parameter 3");
break;
case COMMAND:
return QString("Command");
value = QString("Command");
break;
case JUMPDESTINATION:
return QString("Jump Destination");
value = QString("Jump Destination");
break;
case ERRORDESTINATION:
return QString("Error Destination");
value = QString("Error Destination");
break;
case LOCKED:
return QString("Locked");
value = QString("Locked");
break;
default:
return QString();
value = QString();
break;
}
}
} else {
return QAbstractTableModel::headerData(section, orientation, role);
value = QAbstractTableModel::headerData(section, orientation, role);
}
return value;
}
bool flightDataModel::setData(const QModelIndex &index, const QVariant &value, int role)
{
if (role == Qt::EditRole) {
@ -465,11 +412,12 @@ bool flightDataModel::insertRows(int row, int count, const QModelIndex & /*paren
dataStorage.insert(row, data);
}
endInsertRows();
return true;
}
bool flightDataModel::removeRows(int row, int count, const QModelIndex & /*parent*/)
{
if (row < 0) {
if (row < 0 || count <= 0) {
return false;
}
beginRemoveRows(QModelIndex(), row, row + count - 1);
@ -478,6 +426,7 @@ bool flightDataModel::removeRows(int row, int count, const QModelIndex & /*paren
dataStorage.removeAt(row);
}
endRemoveRows();
return true;
}
bool flightDataModel::writeToFile(QString fileName)
@ -617,6 +566,7 @@ bool flightDataModel::writeToFile(QString fileName)
file.close();
return true;
}
void flightDataModel::readFromFile(QString fileName)
{
// TODO warning message
@ -657,54 +607,56 @@ void flightDataModel::readFromFile(QString fileName)
while (!fieldNode.isNull()) {
QDomElement field = fieldNode.toElement();
if (field.tagName() == "field") {
if (field.attribute("name") == "altitude") {
data->altitude = field.attribute("value").toDouble();
} else if (field.attribute("name") == "description") {
data->wpDescritption = field.attribute("value");
} else if (field.attribute("name") == "latitude") {
data->latPosition = field.attribute("value").toDouble();
} else if (field.attribute("name") == "longitude") {
data->lngPosition = field.attribute("value").toDouble();
} else if (field.attribute("name") == "distance_to_home") {
data->disRelative = field.attribute("value").toDouble();
} else if (field.attribute("name") == "bearing_from_home") {
data->beaRelative = field.attribute("value").toDouble();
} else if (field.attribute("name") == "altitude_above_home") {
data->altitudeRelative = field.attribute("value").toFloat();
} else if (field.attribute("name") == "is_relative_to_home") {
data->isRelative = field.attribute("value").toInt();
} else if (field.attribute("name") == "altitude") {
data->altitude = field.attribute("value").toDouble();
} else if (field.attribute("name") == "velocity") {
data->velocity = field.attribute("value").toFloat();
} else if (field.attribute("name") == "mode") {
data->mode = field.attribute("value").toInt();
} else if (field.attribute("name") == "mode_param0") {
data->mode_params[0] = field.attribute("value").toFloat();
} else if (field.attribute("name") == "mode_param1") {
data->mode_params[1] = field.attribute("value").toFloat();
} else if (field.attribute("name") == "mode_param2") {
data->mode_params[2] = field.attribute("value").toFloat();
} else if (field.attribute("name") == "mode_param3") {
data->mode_params[3] = field.attribute("value").toFloat();
} else if (field.attribute("name") == "condition") {
data->condition = field.attribute("value").toDouble();
} else if (field.attribute("name") == "condition_param0") {
data->condition_params[0] = field.attribute("value").toFloat();
} else if (field.attribute("name") == "condition_param1") {
data->condition_params[1] = field.attribute("value").toFloat();
} else if (field.attribute("name") == "condition_param2") {
data->condition_params[2] = field.attribute("value").toFloat();
} else if (field.attribute("name") == "condition_param3") {
data->condition_params[3] = field.attribute("value").toFloat();
} else if (field.attribute("name") == "command") {
data->command = field.attribute("value").toInt();
} else if (field.attribute("name") == "jumpdestination") {
data->jumpdestination = field.attribute("value").toInt();
} else if (field.attribute("name") == "errordestination") {
data->errordestination = field.attribute("value").toInt();
} else if (field.attribute("name") == "is_locked") {
data->locked = field.attribute("value").toInt();
QString name = field.attribute("name");
QString value = field.attribute("value");
if (name == "altitude") {
data->altitude = value.toDouble();
} else if (name == "description") {
data->wpDescritption = value;
} else if (name == "latitude") {
data->latPosition = value.toDouble();
} else if (name == "longitude") {
data->lngPosition = value.toDouble();
} else if (name == "distance_to_home") {
data->disRelative = value.toDouble();
} else if (name == "bearing_from_home") {
data->beaRelative = value.toDouble();
} else if (name == "altitude_above_home") {
data->altitudeRelative = value.toFloat();
} else if (name == "is_relative_to_home") {
data->isRelative = value.toInt();
} else if (name == "altitude") {
data->altitude = value.toDouble();
} else if (name == "velocity") {
data->velocity = value.toFloat();
} else if (name == "mode") {
data->mode = value.toInt();
} else if (name == "mode_param0") {
data->mode_params[0] = value.toFloat();
} else if (name == "mode_param1") {
data->mode_params[1] = value.toFloat();
} else if (name == "mode_param2") {
data->mode_params[2] = value.toFloat();
} else if (name == "mode_param3") {
data->mode_params[3] = value.toFloat();
} else if (name == "condition") {
data->condition = value.toDouble();
} else if (name == "condition_param0") {
data->condition_params[0] = value.toFloat();
} else if (name == "condition_param1") {
data->condition_params[1] = value.toFloat();
} else if (name == "condition_param2") {
data->condition_params[2] = value.toFloat();
} else if (name == "condition_param3") {
data->condition_params[3] = value.toFloat();
} else if (name == "command") {
data->command = value.toInt();
} else if (name == "jumpdestination") {
data->jumpdestination = value.toInt();
} else if (name == "errordestination") {
data->errordestination = value.toInt();
} else if (name == "is_locked") {
data->locked = value.toInt();
}
}
fieldNode = fieldNode.nextSibling();

5
ground/openpilotgcs/src/plugins/opmap/modelmapproxy.cpp
View File

@ -345,7 +345,10 @@ void modelMapProxy::createWayPoint(internals::PointLatLng coord)
index = model->index(model->rowCount() - 1, flightDataModel::ERRORDESTINATION, QModelIndex());
model->setData(index, 1, Qt::EditRole);
}
void modelMapProxy::deleteAll()
{
model->removeRows(0, model->rowCount(), QModelIndex());
if (model->rowCount() > 0) {
model->removeRows(0, model->rowCount(), QModelIndex());
}
}

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

@ -26,222 +26,455 @@
*/
#include "modeluavoproxy.h"
#include "extensionsystem/pluginmanager.h"
#include <math.h>
modelUavoProxy::modelUavoProxy(QObject *parent, flightDataModel *model) : QObject(parent), myModel(model)
ModelUavoProxy::ModelUavoProxy(QObject *parent, flightDataModel *model) : QObject(parent), myModel(model)
{
ExtensionSystem::PluginManager *pm = ExtensionSystem::PluginManager::instance();
Q_ASSERT(pm != NULL);
objManager = pm->getObject<UAVObjectManager>();
Q_ASSERT(objManager != NULL);
waypointObj = Waypoint::GetInstance(objManager);
Q_ASSERT(waypointObj != NULL);
pathactionObj = PathAction::GetInstance(objManager);
Q_ASSERT(pathactionObj != NULL);
}
void modelUavoProxy::modelToObjects()
{
PathAction *act = NULL;
Waypoint *wp = NULL;
QModelIndex index;
double distance;
double bearing;
double altitude;
int lastaction = -1;
for (int x = 0; x < myModel->rowCount(); ++x) {
int instances = objManager->getNumInstances(waypointObj->getObjID());
if (x > instances - 1) {
wp = new Waypoint;
wp->initialize(x, wp->getMetaObject());
objManager->registerObject(wp);
objMngr = pm->getObject<UAVObjectManager>();
Q_ASSERT(objMngr != NULL);
completionCountdown = 0;
successCountdown = 0;
}
void ModelUavoProxy::sendPathPlan()
{
modelToObjects();
PathPlan *pathPlan = PathPlan::GetInstance(objMngr, 0);
connect(pathPlan, SIGNAL(transactionCompleted(UAVObject *, bool)),
this, SLOT(pathPlanElementSent(UAVObject *, bool)), Qt::UniqueConnection);
Waypoint *waypoint = Waypoint::GetInstance(objMngr, 0);
connect(waypoint, SIGNAL(transactionCompleted(UAVObject *, bool)),
this, SLOT(pathPlanElementSent(UAVObject *, bool)), Qt::UniqueConnection);
PathAction *action = PathAction::GetInstance(objMngr, 0);
connect(action, SIGNAL(transactionCompleted(UAVObject *, bool)),
this, SLOT(pathPlanElementSent(UAVObject *, bool)), Qt::UniqueConnection);
// we will start 3 update all
completionCountdown = 3;
successCountdown = completionCountdown;
pathPlan->updated();
waypoint->updatedAll();
action->updatedAll();
}
void ModelUavoProxy::pathPlanElementSent(UAVObject *obj, bool success)
{
obj->disconnect(this);
completionCountdown--;
successCountdown -= success ? 1 : 0;
if (completionCountdown == 0) {
qDebug() << "ModelUavoProxy::pathPlanSent - completed" << (successCountdown == 0);
if (successCountdown == 0) {
QMessageBox::information(NULL, tr("Path Plan Upload Successful"), tr("Path plan upload was successful."));
} else {
wp = Waypoint::GetInstance(objManager, x);
QMessageBox::critical(NULL, tr("Path Plan Upload Failed"), tr("Failed to upload the path plan !"));
}
act = new PathAction;
Q_ASSERT(act);
Q_ASSERT(wp);
Waypoint::DataFields waypoint = wp->getData();
PathAction::DataFields action = act->getData();
///Waypoint object data
index = myModel->index(x, flightDataModel::DISRELATIVE);
distance = myModel->data(index).toDouble();
index = myModel->index(x, flightDataModel::BEARELATIVE);
bearing = myModel->data(index).toDouble();
index = myModel->index(x, flightDataModel::ALTITUDERELATIVE);
altitude = myModel->data(index).toFloat();
index = myModel->index(x, flightDataModel::VELOCITY);
waypoint.Velocity = myModel->data(index).toFloat();
waypoint.Position[Waypoint::POSITION_NORTH] = distance * cos(bearing / 180 * M_PI);
waypoint.Position[Waypoint::POSITION_EAST] = distance * sin(bearing / 180 * M_PI);
waypoint.Position[Waypoint::POSITION_DOWN] = (-1.0f) * altitude;
///PathAction object data
index = myModel->index(x, flightDataModel::MODE);
action.Mode = myModel->data(index).toInt();
index = myModel->index(x, flightDataModel::MODE_PARAMS0);
action.ModeParameters[0] = myModel->data(index).toFloat();
index = myModel->index(x, flightDataModel::MODE_PARAMS1);
action.ModeParameters[1] = myModel->data(index).toFloat();
index = myModel->index(x, flightDataModel::MODE_PARAMS2);
action.ModeParameters[2] = myModel->data(index).toFloat();
index = myModel->index(x, flightDataModel::MODE_PARAMS3);
action.ModeParameters[3] = myModel->data(index).toFloat();
index = myModel->index(x, flightDataModel::CONDITION);
action.EndCondition = myModel->data(index).toInt();
index = myModel->index(x, flightDataModel::CONDITION_PARAMS0);
action.ConditionParameters[0] = myModel->data(index).toFloat();
index = myModel->index(x, flightDataModel::CONDITION_PARAMS1);
action.ConditionParameters[1] = myModel->data(index).toFloat();
index = myModel->index(x, flightDataModel::CONDITION_PARAMS2);
action.ConditionParameters[2] = myModel->data(index).toFloat();
index = myModel->index(x, flightDataModel::CONDITION_PARAMS3);
action.ConditionParameters[3] = myModel->data(index).toFloat();
index = myModel->index(x, flightDataModel::COMMAND);
action.Command = myModel->data(index).toInt();
index = myModel->index(x, flightDataModel::JUMPDESTINATION);
action.JumpDestination = myModel->data(index).toInt() - 1;
index = myModel->index(x, flightDataModel::ERRORDESTINATION);
action.ErrorDestination = myModel->data(index).toInt() - 1;
int actionNumber = addAction(act, action, lastaction);
if (actionNumber > lastaction) {
lastaction = actionNumber;
}
waypoint.Action = actionNumber;
wp->setData(waypoint);
wp->updated();
}
}
void modelUavoProxy::objectsToModel()
void ModelUavoProxy::receivePathPlan()
{
Waypoint *wp;
Waypoint::DataFields wpfields;
PathAction *action;
QModelIndex index;
double distance;
double bearing;
PathPlan *pathPlan = PathPlan::GetInstance(objMngr, 0);
PathAction::DataFields actionfields;
connect(pathPlan, SIGNAL(transactionCompleted(UAVObject *, bool)), this, SLOT(pathPlanElementReceived(UAVObject *, bool)));
myModel->removeRows(0, myModel->rowCount());
for (int x = 0; x < objManager->getNumInstances(waypointObj->getObjID()); ++x) {
wp = Waypoint::GetInstance(objManager, x);
Q_ASSERT(wp);
if (!wp) {
continue;
Waypoint *waypoint = Waypoint::GetInstance(objMngr, 0);
connect(waypoint, SIGNAL(transactionCompleted(UAVObject *, bool)), this, SLOT(pathPlanElementReceived(UAVObject *, bool)));
PathAction *action = PathAction::GetInstance(objMngr, 0);
connect(action, SIGNAL(transactionCompleted(UAVObject *, bool)), this, SLOT(pathPlanElementReceived(UAVObject *, bool)));
// we will start 3 update requests
completionCountdown = 3;
successCountdown = completionCountdown;
pathPlan->requestUpdate();
waypoint->requestUpdateAll();
action->requestUpdateAll();
}
void ModelUavoProxy::pathPlanElementReceived(UAVObject *obj, bool success)
{
obj->disconnect(this);
completionCountdown--;
successCountdown -= success ? 1 : 0;
if (completionCountdown == 0) {
qDebug() << "ModelUavoProxy::pathPlanReceived - completed" << (successCountdown == 0);
if (successCountdown == 0) {
if (objectsToModel()) {
QMessageBox::information(NULL, tr("Path Plan Download Successful"), tr("Path plan download was successful."));
}
} else {
QMessageBox::critical(NULL, tr("Path Plan Download Failed"), tr("Failed to download the path plan !"));
}
wpfields = wp->getData();
myModel->insertRow(x);
index = myModel->index(x, flightDataModel::VELOCITY);
myModel->setData(index, wpfields.Velocity);
distance = sqrt(wpfields.Position[Waypoint::POSITION_NORTH] * wpfields.Position[Waypoint::POSITION_NORTH] +
wpfields.Position[Waypoint::POSITION_EAST] * wpfields.Position[Waypoint::POSITION_EAST]);
bearing = atan2(wpfields.Position[Waypoint::POSITION_EAST], wpfields.Position[Waypoint::POSITION_NORTH]) * 180 / M_PI;
if (bearing != bearing) {
bearing = 0;
}
index = myModel->index(x, flightDataModel::DISRELATIVE);
myModel->setData(index, distance);
index = myModel->index(x, flightDataModel::BEARELATIVE);
myModel->setData(index, bearing);
index = myModel->index(x, flightDataModel::ALTITUDERELATIVE);
myModel->setData(index, (-1.0f) * wpfields.Position[Waypoint::POSITION_DOWN]);
action = PathAction::GetInstance(objManager, wpfields.Action);
Q_ASSERT(action);
if (!action) {
continue;
}
actionfields = action->getData();
index = myModel->index(x, flightDataModel::ISRELATIVE);
myModel->setData(index, true);
index = myModel->index(x, flightDataModel::COMMAND);
myModel->setData(index, actionfields.Command);
index = myModel->index(x, flightDataModel::CONDITION_PARAMS0);
myModel->setData(index, actionfields.ConditionParameters[0]);
index = myModel->index(x, flightDataModel::CONDITION_PARAMS1);
myModel->setData(index, actionfields.ConditionParameters[1]);
index = myModel->index(x, flightDataModel::CONDITION_PARAMS2);
myModel->setData(index, actionfields.ConditionParameters[2]);
index = myModel->index(x, flightDataModel::CONDITION_PARAMS3);
myModel->setData(index, actionfields.ConditionParameters[3]);
index = myModel->index(x, flightDataModel::CONDITION);
myModel->setData(index, actionfields.EndCondition);
index = myModel->index(x, flightDataModel::ERRORDESTINATION);
myModel->setData(index, actionfields.ErrorDestination + 1);
index = myModel->index(x, flightDataModel::JUMPDESTINATION);
myModel->setData(index, actionfields.JumpDestination + 1);
index = myModel->index(x, flightDataModel::MODE);
myModel->setData(index, actionfields.Mode);
index = myModel->index(x, flightDataModel::MODE_PARAMS0);
myModel->setData(index, actionfields.ModeParameters[0]);
index = myModel->index(x, flightDataModel::MODE_PARAMS1);
myModel->setData(index, actionfields.ModeParameters[1]);
index = myModel->index(x, flightDataModel::MODE_PARAMS2);
myModel->setData(index, actionfields.ModeParameters[2]);
index = myModel->index(x, flightDataModel::MODE_PARAMS3);
myModel->setData(index, actionfields.ModeParameters[3]);
}
}
int modelUavoProxy::addAction(PathAction *actionObj, PathAction::DataFields actionFields, int lastaction)
{
// check if a similar action already exhists
int instances = objManager->getNumInstances(pathactionObj->getObjID());
for (int x = 0; x < lastaction + 1; ++x) {
PathAction *action = PathAction::GetInstance(objManager, x);
// update waypoint and path actions UAV objects
//
// waypoints are unique and each waypoint has an entry in the UAV waypoint list
//
// a path action can be referenced by multiple waypoints
// waypoints reference path action by their index in the UAV path action list
// the compression of path actions happens here.
// (compression consists in keeping only one instance of similar path actions)
//
// the UAV waypoint list and path action list are probably not empty, so we try to reuse existing instances
bool ModelUavoProxy::modelToObjects()
{
qDebug() << "ModelUAVProxy::modelToObjects";
// track number of path actions
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
// this object does not need to be deleted as it will either be added to the managed list or deleted later
PathAction *action = new PathAction;
// get model data
PathAction::DataFields actionData = action->getData();
modelToPathAction(i, actionData);
// see if that path action has already been added in this run
PathAction *foundAction = findPathAction(actionData, actionCount);
// TODO this test needs a consistency check as it is unsafe.
// if the find method is buggy and returns false positives then the flight plan sent to the uav is broken!
// the find method should do a "binary" compare instead of a field by field compare
// if a field is added everywhere and not in the compare method then you can start having false positives
if (!foundAction) {
// create or reuse an action instance
action = createPathAction(actionCount, action);
actionCount++;
// update UAVObject
action->setData(actionData);
} else {
action->deleteLater();
action = foundAction;
qDebug() << "ModelUAVProxy::modelToObjects - found action instance :" << action->getInstID();
}
// Waypoints
// create or reuse a waypoint instance
Waypoint *waypoint = createWaypoint(i, NULL);
Q_ASSERT(waypoint);
// get model data
Waypoint::DataFields waypointData = waypoint->getData();
modelToWaypoint(i, waypointData);
// connect waypoint to path action
waypointData.Action = action->getInstID();
// update UAVObject
waypoint->setData(waypointData);
}
// Put "safe" values in unused waypoint and path action objects
if (waypointCount < objMngr->getNumInstances(Waypoint::OBJID)) {
for (int i = waypointCount; i < objMngr->getNumInstances(Waypoint::OBJID); ++i) {
// TODO
}
}
if (actionCount < objMngr->getNumInstances(PathAction::OBJID)) {
for (int i = actionCount; i < objMngr->getNumInstances(PathAction::OBJID); ++i) {
// TODO
}
}
// Update PathPlan
PathPlan *pathPlan = PathPlan::GetInstance(objMngr);
PathPlan::DataFields pathPlanData = pathPlan->getData();
pathPlanData.WaypointCount = waypointCount;
pathPlanData.PathActionCount = actionCount;
pathPlanData.Crc = computePathPlanCrc(waypointCount, actionCount);
pathPlan->setData(pathPlanData);
return true;
}
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;
waypoint = Waypoint::GetInstance(objMngr, index);
if (newWaypoint) {
newWaypoint->deleteLater();
}
} else if (index < count + 1) {
// create "next" object
qDebug() << "ModelUAVProxy::createWaypoint - created waypoint instance :" << index;
// TODO is there a limit to the number of wp?
waypoint = newWaypoint ? newWaypoint : new Waypoint;
waypoint->initialize(index, waypoint->getMetaObject());
objMngr->registerObject(waypoint);
} else {
// we can only create the "next" object
// TODO fail in a clean way :(
}
return waypoint;
}
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;
action = PathAction::GetInstance(objMngr, index);
if (newAction) {
newAction->deleteLater();
}
} else if (index < count + 1) {
// create "next" object
qDebug() << "ModelUAVProxy::createPathAction - created action instance :" << index;
// TODO is there a limit to the number of path actions?
action = newAction ? newAction : new PathAction;
action->initialize(index, action->getMetaObject());
objMngr->registerObject(action);
} 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)
{
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);
if (!action) {
continue;
}
PathAction::DataFields fields = action->getData();
if (fields.Command == actionFields.Command
&& fields.ConditionParameters[0] == actionFields.ConditionParameters[0]
&& fields.ConditionParameters[1] == actionFields.ConditionParameters[1]
&& fields.ConditionParameters[2] == actionFields.ConditionParameters[2]
&& fields.EndCondition == actionFields.EndCondition
&& fields.ErrorDestination == actionFields.ErrorDestination
&& fields.JumpDestination == actionFields.JumpDestination
&& fields.Mode == actionFields.Mode
&& fields.ModeParameters[0] == actionFields.ModeParameters[0]
&& fields.ModeParameters[1] == actionFields.ModeParameters[1]
&& fields.ModeParameters[2] == actionFields.ModeParameters[2]) {
qDebug() << "ModelUAVProxy:" << "found similar action instance:" << x;
actionObj->deleteLater();
return x;
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]) {
return action;
}
}
// if we get here it means no similar action was found, we have to create it
if (instances < lastaction + 2) {
actionObj->initialize(instances, actionObj->getMetaObject());
objManager->registerObject(actionObj);
actionObj->setData(actionFields);
actionObj->updated();
qDebug() << "ModelUAVProxy:" << "created new action instance:" << instances;
return lastaction + 1;
} else {
PathAction *action = PathAction::GetInstance(objManager, lastaction + 1);
Q_ASSERT(action);
action->setData(actionFields);
action->updated();
actionObj->deleteLater();
qDebug() << "ModelUAVProxy:" << "reused action instance:" << lastaction + 1;
return lastaction + 1;
}
return -1; // error we should never get here
return NULL;
}
bool ModelUavoProxy::objectsToModel()
{
// build model from uav objects
// the list of objects can end with "garbage" instances due to previous flightpath
// they need to be ignored
PathPlan *pathPlan = PathPlan::GetInstance(objMngr);
PathPlan::DataFields pathPlanData = pathPlan->getData();
int waypointCount = pathPlanData.WaypointCount;
int actionCount = pathPlanData.PathActionCount;
// consistency check
if (waypointCount > objMngr->getNumInstances(Waypoint::OBJID)) {
QMessageBox::critical(NULL, tr("Path Plan Download Failed"), tr("Path plan way point count error !"));
return false;
}
if (actionCount > objMngr->getNumInstances(PathAction::OBJID)) {
QMessageBox::critical(NULL, tr("Path Plan Download Failed"), tr("Path plan path action count error !"));
return false;
}
if (pathPlanData.Crc != computePathPlanCrc(waypointCount, actionCount)) {
QMessageBox::critical(NULL, tr("Path Plan Upload Failed"), tr("Path plan CRC error !"));
return false;
}
int rowCount = myModel->rowCount();
if (waypointCount < rowCount) {
myModel->removeRows(waypointCount, rowCount - waypointCount);
} else if (waypointCount > rowCount) {
myModel->insertRows(rowCount, waypointCount - rowCount);
}
for (int i = 0; i < waypointCount; ++i) {
Waypoint *waypoint = Waypoint::GetInstance(objMngr, i);
Q_ASSERT(waypoint);
if (!waypoint) {
continue;
}
Waypoint::DataFields waypointData = waypoint->getData();
waypointToModel(i, waypointData);
PathAction *action = PathAction::GetInstance(objMngr, waypoint->getAction());
Q_ASSERT(action);
if (!action) {
continue;
}
PathAction::DataFields actionData = action->getData();
pathActionToModel(i, actionData);
}
return true;
}
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();
index = myModel->index(i, flightDataModel::ALTITUDERELATIVE);
altitude = myModel->data(index).toFloat();
index = myModel->index(i, flightDataModel::VELOCITY);
velocity = myModel->data(index).toFloat();
data.Position[Waypoint::POSITION_NORTH] = distance * cos(bearing / 180 * M_PI);
data.Position[Waypoint::POSITION_EAST] = distance * sin(bearing / 180 * M_PI);
data.Position[Waypoint::POSITION_DOWN] = -altitude;
data.Velocity = velocity;
}
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]);
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];
QModelIndex index = myModel->index(i, flightDataModel::VELOCITY);
myModel->setData(index, data.Velocity);
index = myModel->index(i, flightDataModel::DISRELATIVE);
myModel->setData(index, distance);
index = myModel->index(i, flightDataModel::BEARELATIVE);
myModel->setData(index, bearing);
index = myModel->index(i, flightDataModel::ALTITUDERELATIVE);
myModel->setData(index, altitude);
}
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.ModeParameters[0] = myModel->data(index).toFloat();
index = myModel->index(i, flightDataModel::MODE_PARAMS1);
data.ModeParameters[1] = myModel->data(index).toFloat();
index = myModel->index(i, flightDataModel::MODE_PARAMS2);
data.ModeParameters[2] = myModel->data(index).toFloat();
index = myModel->index(i, flightDataModel::MODE_PARAMS3);
data.ModeParameters[3] = myModel->data(index).toFloat();
index = myModel->index(i, flightDataModel::CONDITION);
data.EndCondition = myModel->data(index).toInt();
index = myModel->index(i, flightDataModel::CONDITION_PARAMS0);
data.ConditionParameters[0] = myModel->data(index).toFloat();
index = myModel->index(i, flightDataModel::CONDITION_PARAMS1);
data.ConditionParameters[1] = myModel->data(index).toFloat();
index = myModel->index(i, flightDataModel::CONDITION_PARAMS2);
data.ConditionParameters[2] = myModel->data(index).toFloat();
index = myModel->index(i, flightDataModel::CONDITION_PARAMS3);
data.ConditionParameters[3] = myModel->data(index).toFloat();
index = myModel->index(i, flightDataModel::COMMAND);
data.Command = myModel->data(index).toInt();
index = myModel->index(i, flightDataModel::JUMPDESTINATION);
data.JumpDestination = myModel->data(index).toInt() - 1;
index = myModel->index(i, flightDataModel::ERRORDESTINATION);
data.ErrorDestination = myModel->data(index).toInt() - 1;
}
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);
myModel->setData(index, data.Command);
index = myModel->index(i, flightDataModel::CONDITION_PARAMS0);
myModel->setData(index, data.ConditionParameters[0]);
index = myModel->index(i, flightDataModel::CONDITION_PARAMS1);
myModel->setData(index, data.ConditionParameters[1]);
index = myModel->index(i, flightDataModel::CONDITION_PARAMS2);
myModel->setData(index, data.ConditionParameters[2]);
index = myModel->index(i, flightDataModel::CONDITION_PARAMS3);
myModel->setData(index, data.ConditionParameters[3]);
index = myModel->index(i, flightDataModel::CONDITION);
myModel->setData(index, data.EndCondition);
index = myModel->index(i, flightDataModel::ERRORDESTINATION);
myModel->setData(index, data.ErrorDestination + 1);
index = myModel->index(i, flightDataModel::JUMPDESTINATION);
myModel->setData(index, data.JumpDestination + 1);
index = myModel->index(i, flightDataModel::MODE);
myModel->setData(index, data.Mode);
index = myModel->index(i, flightDataModel::MODE_PARAMS0);
myModel->setData(index, data.ModeParameters[0]);
index = myModel->index(i, flightDataModel::MODE_PARAMS1);
myModel->setData(index, data.ModeParameters[1]);
index = myModel->index(i, flightDataModel::MODE_PARAMS2);
myModel->setData(index, data.ModeParameters[2]);
index = myModel->index(i, flightDataModel::MODE_PARAMS3);
myModel->setData(index, data.ModeParameters[3]);
}
quint8 ModelUavoProxy::computePathPlanCrc(int waypointCount, int actionCount)
{
quint8 crc = 0;
for (int i = 0; i < waypointCount; ++i) {
Waypoint *waypoint = Waypoint::GetInstance(objMngr, i);
crc = waypoint->updateCRC(crc);
}
for (int i = 0; i < actionCount; ++i) {
PathAction *action = PathAction::GetInstance(objMngr, i);
crc = action->updateCRC(crc);
}
return crc;
}

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

@ -27,24 +27,50 @@
#ifndef MODELUAVOPROXY_H
#define MODELUAVOPROXY_H
#include <QObject>
#include "flightdatamodel.h"
#include "pathplan.h"
#include "pathaction.h"
#include "waypoint.h"
class modelUavoProxy : public QObject {
#include <QObject>
class ModelUavoProxy : public QObject {
Q_OBJECT
public:
explicit modelUavoProxy(QObject *parent, flightDataModel *model);
int addAction(PathAction *actionObj, PathAction::DataFields actionFields, int lastaction);
explicit ModelUavoProxy(QObject *parent, flightDataModel *model);
public slots:
void modelToObjects();
void objectsToModel();
void sendPathPlan();
void receivePathPlan();
private:
UAVObjectManager *objManager;
Waypoint *waypointObj;
PathAction *pathactionObj;
UAVObjectManager *objMngr;
flightDataModel *myModel;
uint completionCountdown;
uint successCountdown;
bool modelToObjects();
bool objectsToModel();
Waypoint *createWaypoint(int index, Waypoint *newWaypoint);
PathAction *createPathAction(int index, PathAction *newAction);
PathAction *findPathAction(const PathAction::DataFields & actionFields, int actionCount);
void modelToWaypoint(int i, Waypoint::DataFields &data);
void modelToPathAction(int i, PathAction::DataFields &data);
void waypointToModel(int i, Waypoint::DataFields &data);
void pathActionToModel(int i, PathAction::DataFields &data);
quint8 computePathPlanCrc(int waypointCount, int actionCount);
private slots:
void pathPlanElementSent(UAVObject *, bool success);
void pathPlanElementReceived(UAVObject *, bool success);
};
#endif // MODELUAVOPROXY_H

2
ground/openpilotgcs/src/plugins/opmap/opmap_edit_waypoint_dialog.cpp
View File

@ -176,7 +176,7 @@ void opmap_edit_waypoint_dialog::setupConditionWidgets()
ui->dsb_condParam2->setVisible(false);
ui->dsb_condParam3->setVisible(false);
ui->dsb_condParam4->setVisible(false);
ui->condParam1->setText("Timeout(ms)");
ui->condParam1->setText("Timeout(s)");
break;
case MapDataDelegate::ENDCONDITION_DISTANCETOTARGET:
ui->condParam1->setVisible(true);

15
ground/openpilotgcs/src/plugins/opmap/opmapgadgetwidget.cpp
View File

@ -225,9 +225,11 @@ OPMapGadgetWidget::OPMapGadgetWidget(QWidget *parent) : QWidget(parent)
mapProxy = new modelMapProxy(this, m_map, model, selectionModel);
table->setModel(model, selectionModel);
waypoint_edit_dialog = new opmap_edit_waypoint_dialog(this, model, selectionModel);
UAVProxy = new modelUavoProxy(this, model);
connect(table, SIGNAL(sendPathPlanToUAV()), UAVProxy, SLOT(modelToObjects()));
connect(table, SIGNAL(receivePathPlanFromUAV()), UAVProxy, SLOT(objectsToModel()));
UAVProxy = new ModelUavoProxy(this, model);
// sending and receiving is asynchronous
// TODO : buttons should be disabled while a send or receive is in progress
connect(table, SIGNAL(sendPathPlanToUAV()), UAVProxy, SLOT(sendPathPlan()));
connect(table, SIGNAL(receivePathPlanFromUAV()), UAVProxy, SLOT(receivePathPlan()));
#endif
magicWayPoint = m_map->magicWPCreate();
magicWayPoint->setVisible(false);
@ -500,7 +502,8 @@ void OPMapGadgetWidget::contextMenuEvent(QContextMenuEvent *event)
contextMenu.addAction(wayPointEditorAct);
contextMenu.addAction(addWayPointActFromContextMenu);
if (m_mouse_waypoint) { // we have a waypoint under the mouse
if (m_mouse_waypoint) {
// we have a waypoint under the mouse
contextMenu.addAction(editWayPointAct);
lockWayPointAct->setChecked(waypoint_locked);
@ -1868,8 +1871,12 @@ void OPMapGadgetWidget::onUAVTrailDistanceActGroup_triggered(QAction *action)
void OPMapGadgetWidget::onOpenWayPointEditorAct_triggered()
{
// open dialog
table->show();
// bring dialog to the front in case it was already open and hidden away
table->raise();
}
void OPMapGadgetWidget::onAddWayPointAct_triggeredFromContextMenu()
{
onAddWayPointAct_triggered(m_context_menu_lat_lon);

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

@ -233,7 +233,7 @@ private:
QPointer<opmap_edit_waypoint_dialog> waypoint_edit_dialog;
QStandardItemModel wayPoint_treeView_model;
mapcontrol::WayPointItem *m_mouse_waypoint;
QPointer<modelUavoProxy> UAVProxy;
QPointer<ModelUavoProxy> UAVProxy;
QMutex m_map_mutex;
bool m_telemetry_connected;
QAction *closeAct1;

1
ground/openpilotgcs/src/plugins/opmap/widgetdelegates.cpp
View File

@ -76,7 +76,6 @@ void MapDataDelegate::setEditorData(QWidget *editor,
int value = index.model()->data(index, Qt::EditRole).toInt();
QComboBox *comboBox = static_cast<QComboBox *>(editor);
int x = comboBox->findData(value);
qDebug() << "VALUE=" << x;
comboBox->setCurrentIndex(x);
} else {
QItemDelegate::setEditorData(editor, index);

1
ground/openpilotgcs/src/plugins/scope/scopegadgetwidget.cpp
View File

@ -30,7 +30,6 @@
#include "scopegadgetwidget.h"
#include "utils/stylehelper.h"
#include "uavtalk/telemetrymanager.h"
#include "extensionsystem/pluginmanager.h"
#include "uavobjectmanager.h"
#include "uavobject.h"

1
ground/openpilotgcs/src/plugins/setupwizard/pages/autoupdatepage.h
View File

@ -31,7 +31,6 @@
#include <coreplugin/icore.h>
#include <coreplugin/connectionmanager.h>
#include "setupwizard.h"
#include "uavtalk/telemetrymanager.h"
#include "abstractwizardpage.h"
#include "uploader/enums.h"

4
ground/openpilotgcs/src/plugins/systemhealth/systemhealthgadgetwidget.cpp
View File

@ -25,11 +25,13 @@
* 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA
*/
#include "systemalarms.h"
#include "systemhealthgadgetwidget.h"
#include "utils/stylehelper.h"
#include "extensionsystem/pluginmanager.h"
#include "uavobjectmanager.h"
#include "systemalarms.h"
#include <uavtalk/telemetrymanager.h>
#include <QDebug>
#include <QWhatsThis>

2
ground/openpilotgcs/src/plugins/systemhealth/systemhealthgadgetwidget.h
View File

@ -30,7 +30,7 @@
#include "systemhealthgadgetconfiguration.h"
#include "uavobject.h"
#include "uavtalk/telemetrymanager.h"
#include <QGraphicsView>
#include <QtSvg/QSvgRenderer>
#include <QtSvg/QGraphicsSvgItem>

5
ground/openpilotgcs/src/plugins/telemetry/monitorgadgetfactory.cpp
View File

@ -25,14 +25,15 @@
* 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA
*/
#include "monitorgadgetfactory.h"
#include "uavtalk/telemetrymanager.h"
#include "extensionsystem/pluginmanager.h"
#include "monitorgadgetconfiguration.h"
#include "monitorgadget.h"
#include "monitorgadgetoptionspage.h"
#include <extensionsystem/pluginmanager.h>
#include <coreplugin/connectionmanager.h>
#include <coreplugin/icore.h>
#include <uavtalk/telemetrymanager.h>
MonitorGadgetFactory::MonitorGadgetFactory(QObject *parent) :
IUAVGadgetFactory(QString("TelemetryMonitorGadget"), tr("Telemetry Monitor"), parent)

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

@ -26,9 +26,14 @@
* 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA
*/
#include "uavobject.h"
#include <utils/crc.h>
#include <QtEndian>
#include <QDebug>
using namespace Utils;
// Constants
#define UAVOBJ_ACCESS_SHIFT 0
#define UAVOBJ_GCS_ACCESS_SHIFT 1
@ -50,11 +55,13 @@
*/
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->mutex = new QMutex(QMutex::Recursive);
this->name = name;
this->data = 0;
this->numBytes = 0;
this->mutex = new QMutex(QMutex::Recursive);
}
/**
@ -163,7 +170,6 @@ void UAVObject::setCategory(const QString & category)
this->category = category;
}
/**
* Get the total number of bytes of the object's data
*/
@ -180,6 +186,17 @@ void UAVObject::requestUpdate()
emit updateRequested(this);
}
/**
* Request that all instances of this object are updated with the latest values from the autopilot
* Must be called on instance zero
*/
void UAVObject::requestUpdateAll()
{
if (instID == 0) {
emit updateRequested(this, true);
}
}
/**
* Signal that the object has been updated
*/
@ -189,6 +206,19 @@ void UAVObject::updated()
emit objectUpdated(this);
}
/**
* Signal that all instance of the object have been updated
* Must be called on instance zero
*/
void UAVObject::updatedAll()
{
if (instID == 0) {
emit objectUpdatedManual(this, true);
// TODO call objectUpdated() for all instances?
// emit objectUpdated(this);
}
}
/**
* Lock mutex of this object
*/
@ -256,7 +286,8 @@ 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.";
qWarning() << "UAVObject::getField Non existant field" << name << "requested."
<< "This indicates a bug. Make sure you also have null checking for non-debug code.";
return NULL;
}
@ -295,6 +326,21 @@ qint32 UAVObject::unpack(const quint8 *dataIn)
return numBytes;
}
/**
* Update a CRC with the object data
* @returns The updated CRC
*/
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, data, numBytes);
return crc;
}
/**
* Save the object data to the file.
* The file will be created in the current directory
@ -437,6 +483,7 @@ QString UAVObject::toString()
QString sout;
sout.append(toStringBrief());
sout.append('\n');
sout.append(toStringData());
return sout;
}
@ -448,12 +495,13 @@ QString UAVObject::toStringBrief()
{
QString sout;
sout.append(QString("%1 (ID: %2, InstID: %3, NumBytes: %4, SInst: %5)\n")
// object Id is converted to uppercase hexadecimal
sout.append(QString("%1 (ID: %2-%3, %4 bytes, %5)")
.arg(getName())
.arg(getObjID())
.arg(getObjID(), 1, 16).toUpper()
.arg(getInstID())
.arg(getNumBytes())
.arg(isSingleInstance()));
.arg(isSingleInstance() ? "single" : "multiple"));
return sout;
}

7
ground/openpilotgcs/src/plugins/uavobjects/uavobject.h
View File

@ -109,6 +109,7 @@ public:
quint32 getNumBytes();
qint32 pack(quint8 *dataOut);
qint32 unpack(const quint8 *dataIn);
quint8 updateCRC(quint8 crc = 0);
bool save();
bool save(QFile & file);
bool load();
@ -146,15 +147,17 @@ public:
public slots:
void requestUpdate();
void requestUpdateAll();
void updated();
void updatedAll();
signals:
void objectUpdated(UAVObject *obj);
void objectUpdatedAuto(UAVObject *obj);
void objectUpdatedManual(UAVObject *obj);
void objectUpdatedManual(UAVObject *obj, bool all = false);
void objectUpdatedPeriodic(UAVObject *obj);
void objectUnpacked(UAVObject *obj);
void updateRequested(UAVObject *obj);
void updateRequested(UAVObject *obj, bool all = false);
void transactionCompleted(UAVObject *obj, bool success);
void newInstance(UAVObject *obj);

18
ground/openpilotgcs/src/plugins/uavobjects/uavobjects.pro
View File

@ -1,10 +1,13 @@
TEMPLATE = lib
TARGET = UAVObjects
DEFINES += UAVOBJECTS_LIBRARY
include(../../openpilotgcsplugin.pri)
include(uavobjects_dependencies.pri)
HEADERS += uavobjects_global.h \
HEADERS += \
uavobjects_global.h \
uavobject.h \
uavmetaobject.h \
uavobjectmanager.h \
@ -14,7 +17,8 @@ HEADERS += uavobjects_global.h \
uavobjectsplugin.h \
uavobjecthelper.h
SOURCES += uavobject.cpp \
SOURCES += \
uavobject.cpp \
uavmetaobject.cpp \
uavobjectmanager.cpp \
uavdataobject.cpp \
@ -25,7 +29,8 @@ SOURCES += uavobject.cpp \
OTHER_FILES += UAVObjects.pluginspec
# Add in all of the synthetic/generated uavobject files
HEADERS += $$UAVOBJECT_SYNTHETICS/accessorydesired.h \
HEADERS += \
$$UAVOBJECT_SYNTHETICS/accessorydesired.h \
$$UAVOBJECT_SYNTHETICS/barosensor.h \
$$UAVOBJECT_SYNTHETICS/airspeedsensor.h \
$$UAVOBJECT_SYNTHETICS/airspeedsettings.h \
@ -76,6 +81,7 @@ HEADERS += $$UAVOBJECT_SYNTHETICS/accessorydesired.h \
$$UAVOBJECT_SYNTHETICS/gpssettings.h \
$$UAVOBJECT_SYNTHETICS/pathaction.h \
$$UAVOBJECT_SYNTHETICS/pathdesired.h \
$$UAVOBJECT_SYNTHETICS/pathplan.h \
$$UAVOBJECT_SYNTHETICS/pathstatus.h \
$$UAVOBJECT_SYNTHETICS/gpsvelocitysensor.h \
$$UAVOBJECT_SYNTHETICS/positionstate.h \
@ -115,12 +121,14 @@ HEADERS += $$UAVOBJECT_SYNTHETICS/accessorydesired.h \
$$UAVOBJECT_SYNTHETICS/oplinksettings.h \
$$UAVOBJECT_SYNTHETICS/oplinkstatus.h \
$$UAVOBJECT_SYNTHETICS/oplinkreceiver.h \
$$UAVOBJECT_SYNTHETICS/radiocombridgestats.h \
$$UAVOBJECT_SYNTHETICS/osdsettings.h \
$$UAVOBJECT_SYNTHETICS/waypoint.h \
$$UAVOBJECT_SYNTHETICS/waypointactive.h \
$$UAVOBJECT_SYNTHETICS/mpu6000settings.h
SOURCES += $$UAVOBJECT_SYNTHETICS/accessorydesired.cpp \
SOURCES += \
$$UAVOBJECT_SYNTHETICS/accessorydesired.cpp \
$$UAVOBJECT_SYNTHETICS/barosensor.cpp \
$$UAVOBJECT_SYNTHETICS/airspeedsensor.cpp \
$$UAVOBJECT_SYNTHETICS/airspeedsettings.cpp \
@ -171,6 +179,7 @@ SOURCES += $$UAVOBJECT_SYNTHETICS/accessorydesired.cpp \
$$UAVOBJECT_SYNTHETICS/gpssettings.cpp \
$$UAVOBJECT_SYNTHETICS/pathaction.cpp \
$$UAVOBJECT_SYNTHETICS/pathdesired.cpp \
$$UAVOBJECT_SYNTHETICS/pathplan.cpp \
$$UAVOBJECT_SYNTHETICS/pathstatus.cpp \
$$UAVOBJECT_SYNTHETICS/gpsvelocitysensor.cpp \
$$UAVOBJECT_SYNTHETICS/positionstate.cpp \
@ -211,6 +220,7 @@ SOURCES += $$UAVOBJECT_SYNTHETICS/accessorydesired.cpp \
$$UAVOBJECT_SYNTHETICS/oplinksettings.cpp \
$$UAVOBJECT_SYNTHETICS/oplinkstatus.cpp \
$$UAVOBJECT_SYNTHETICS/oplinkreceiver.cpp \
$$UAVOBJECT_SYNTHETICS/radiocombridgestats.cpp \
$$UAVOBJECT_SYNTHETICS/osdsettings.cpp \
$$UAVOBJECT_SYNTHETICS/waypoint.cpp \
$$UAVOBJECT_SYNTHETICS/waypointactive.cpp \

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

@ -256,11 +256,10 @@ int UAVObjectUtilManager::getBoardModel()
{
FirmwareIAPObj::DataFields firmwareIapData = getFirmwareIap();
qDebug() << "Board type=" << firmwareIapData.BoardType;
qDebug() << "Board revision=" << firmwareIapData.BoardRevision;
int ret = firmwareIapData.BoardType << 8;
ret = ret + firmwareIapData.BoardRevision;
qDebug() << "Board info=" << ret;
return ret;
}

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

@ -25,9 +25,12 @@
* 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA
*/
#include "configtaskwidget.h"
#include <uavtalk/telemetrymanager.h>
#include "uavsettingsimportexport/uavsettingsimportexportfactory.h"
#include <QWidget>
#include <QLineEdit>
#include "uavsettingsimportexport/uavsettingsimportexportfactory.h"
ConfigTaskWidget::ConfigTaskWidget(QWidget *parent) : QWidget(parent), m_isConnected(false), m_isWidgetUpdatesAllowed(true),
m_saveButton(NULL), m_isDirty(false), m_outOfLimitsStyle("background-color: rgb(255, 0, 0);"), m_realtimeUpdateTimer(NULL)
@ -269,7 +272,7 @@ void ConfigTaskWidget::forceConnectedState() // dynamic widgets don't recieve th
void ConfigTaskWidget::onAutopilotConnect()
{
if (m_objectUtilManager) {
m_currentBoardId = m_objectUtilManager->getBoardModel(); // TODO REMEMBER TO ADD THIS TO FORCE CONNECTED FUNC ON CC3D_RELEASE
m_currentBoardId = m_objectUtilManager->getBoardModel();
}
invalidateObjects();
m_isConnected = true;
@ -1039,6 +1042,7 @@ void ConfigTaskWidget::updateEnableControls()
TelemetryManager *telMngr = m_pluginManager->getObject<TelemetryManager>();
Q_ASSERT(telMngr);
enableControls(telMngr->isConnected());
}

1
ground/openpilotgcs/src/plugins/uavobjectwidgetutils/smartsavebutton.h
View File

@ -27,7 +27,6 @@
#ifndef SMARTSAVEBUTTON_H
#define SMARTSAVEBUTTON_H
#include "uavtalk/telemetrymanager.h"
#include "extensionsystem/pluginmanager.h"
#include "uavobjectmanager.h"
#include "uavobject.h"

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

@ -36,28 +36,34 @@
/**
* Constructor
*/
Telemetry::Telemetry(UAVTalk *utalk, UAVObjectManager *objMngr)
Telemetry::Telemetry(UAVTalk *utalk, UAVObjectManager *objMngr) : objMngr(objMngr), utalk(utalk)
{
this->utalk = utalk;
this->objMngr = objMngr;
mutex = new QMutex(QMutex::Recursive);
// Process all objects in the list
// Register all objects in the list
QList< QList<UAVObject *> > objs = objMngr->getObjects();
for (int objidx = 0; objidx < objs.length(); ++objidx) {
registerObject(objs[objidx][0]); // we only need to register one instance per object type
// we only need to register one instance per object type
registerObject(objs[objidx][0]);
}
// Listen to new object creations
connect(objMngr, SIGNAL(newObject(UAVObject *)), this, SLOT(newObject(UAVObject *)));
connect(objMngr, SIGNAL(newInstance(UAVObject *)), this, SLOT(newInstance(UAVObject *)));
// Listen to transaction completions
// TODO should send a status (SUCCESS, FAILED, TIMEOUT)
connect(utalk, SIGNAL(transactionCompleted(UAVObject *, bool)), this, SLOT(transactionCompleted(UAVObject *, bool)));
// Get GCS stats object
gcsStatsObj = GCSTelemetryStats::GetInstance(objMngr);
// Setup and start the periodic timer
timeToNextUpdateMs = 0;
updateTimer = new QTimer(this);
connect(updateTimer, SIGNAL(timeout()), this, SLOT(processPeriodicUpdates()));
updateTimer->start(1000);
// Setup and start the stats timer
txErrors = 0;
txRetries = 0;
@ -65,9 +71,7 @@ Telemetry::Telemetry(UAVTalk *utalk, UAVObjectManager *objMngr)
Telemetry::~Telemetry()
{
for (QMap<quint32, ObjectTransactionInfo *>::iterator itr = transMap.begin(); itr != transMap.end(); ++itr) {
delete itr.value();
}
closeAllTransactions();
}
/**
@ -122,26 +126,36 @@ void Telemetry::setUpdatePeriod(UAVObject *obj, qint32 periodMs)
*/
void Telemetry::connectToObjectInstances(UAVObject *obj, quint32 eventMask)
{
// TODO why connect systematically to all instances?
// It is probably not needed to connect to always connect to all instances.
QList<UAVObject *> objs = objMngr->getObjectInstances(obj->getObjID());
for (int n = 0; n < objs.length(); ++n) {
// Disconnect all
objs[n]->disconnect(this);
// Connect only the selected events
if ((eventMask & EV_UNPACKED) != 0) {
connect(objs[n], SIGNAL(objectUnpacked(UAVObject *)), this, SLOT(objectUnpacked(UAVObject *)));
}
if ((eventMask & EV_UPDATED) != 0) {
connect(objs[n], SIGNAL(objectUpdatedAuto(UAVObject *)), this, SLOT(objectUpdatedAuto(UAVObject *)));
}
if ((eventMask & EV_UPDATED_MANUAL) != 0) {
connect(objs[n], SIGNAL(objectUpdatedManual(UAVObject *)), this, SLOT(objectUpdatedManual(UAVObject *)));
}
if ((eventMask & EV_UPDATED_PERIODIC) != 0) {
connect(objs[n], SIGNAL(objectUpdatedPeriodic(UAVObject *)), this, SLOT(objectUpdatedPeriodic(UAVObject *)));
}
if ((eventMask & EV_UPDATE_REQ) != 0) {
connect(objs[n], SIGNAL(updateRequested(UAVObject *)), this, SLOT(updateRequested(UAVObject *)));
}
connectToObject(objs[n], eventMask);
}
}
/**
* Connect to all instances of an object depending on the event mask specified
*/
void Telemetry::connectToObject(UAVObject *obj, quint32 eventMask)
{
// Disconnect all
obj->disconnect(this);
// Connect only the selected events
if ((eventMask & EV_UNPACKED) != 0) {
connect(obj, SIGNAL(objectUnpacked(UAVObject *)), this, SLOT(objectUnpacked(UAVObject *)));
}
if ((eventMask & EV_UPDATED) != 0) {
connect(obj, SIGNAL(objectUpdatedAuto(UAVObject *)), this, SLOT(objectUpdatedAuto(UAVObject *)));
}
if ((eventMask & EV_UPDATED_MANUAL) != 0) {
connect(obj, SIGNAL(objectUpdatedManual(UAVObject *, bool)), this, SLOT(objectUpdatedManual(UAVObject *, bool)));
}
if ((eventMask & EV_UPDATED_PERIODIC) != 0) {
connect(obj, SIGNAL(objectUpdatedPeriodic(UAVObject *)), this, SLOT(objectUpdatedPeriodic(UAVObject *)));
}
if ((eventMask & EV_UPDATE_REQ) != 0) {
connect(obj, SIGNAL(updateRequested(UAVObject *, bool)), this, SLOT(updateRequested(UAVObject *, bool)));
}
}
@ -160,19 +174,21 @@ void Telemetry::updateObject(UAVObject *obj, quint32 eventType)
if (updateMode == UAVObject::UPDATEMODE_PERIODIC) {
// Set update period
setUpdatePeriod(obj, metadata.gcsTelemetryUpdatePeriod);
// Connect signals for all instances
// Connect signals
eventMask = EV_UPDATED_MANUAL | EV_UPDATE_REQ | EV_UPDATED_PERIODIC;
if (dynamic_cast<UAVMetaObject *>(obj) != NULL) {
eventMask |= EV_UNPACKED; // we also need to act on remote updates (unpack events)
// we also need to act on remote updates (unpack events)
eventMask |= EV_UNPACKED;
}
connectToObjectInstances(obj, eventMask);
} else if (updateMode == UAVObject::UPDATEMODE_ONCHANGE) {
// Set update period
setUpdatePeriod(obj, 0);
// Connect signals for all instances
// Connect signals
eventMask = EV_UPDATED | EV_UPDATED_MANUAL | EV_UPDATE_REQ;
if (dynamic_cast<UAVMetaObject *>(obj) != NULL) {
eventMask |= EV_UNPACKED; // we also need to act on remote updates (unpack events)
// we also need to act on remote updates (unpack events)
eventMask |= EV_UNPACKED;
}
connectToObjectInstances(obj, eventMask);
} else if (updateMode == UAVObject::UPDATEMODE_THROTTLED) {
@ -182,24 +198,26 @@ void Telemetry::updateObject(UAVObject *obj, quint32 eventType)
if (eventType == EV_NONE) {
setUpdatePeriod(obj, metadata.gcsTelemetryUpdatePeriod);
}
// Connect signals for all instances
// Connect signals
eventMask = EV_UPDATED | EV_UPDATED_MANUAL | EV_UPDATE_REQ | EV_UPDATED_PERIODIC;
} else {
// Otherwise, we just received an object update, so switch to periodic for the timeout period to prevent more updates
// Connect signals for all instances
// Connect signals
eventMask = EV_UPDATED | EV_UPDATED_MANUAL | EV_UPDATE_REQ;
}
if (dynamic_cast<UAVMetaObject *>(obj) != NULL) {
eventMask |= EV_UNPACKED; // we also need to act on remote updates (unpack events)
// we also need to act on remote updates (unpack events)
eventMask |= EV_UNPACKED;
}
connectToObjectInstances(obj, eventMask);
} else if (updateMode == UAVObject::UPDATEMODE_MANUAL) {
// Set update period
setUpdatePeriod(obj, 0);
// Connect signals for all instances
// Connect signals
eventMask = EV_UPDATED_MANUAL | EV_UPDATE_REQ;
if (dynamic_cast<UAVMetaObject *>(obj) != NULL) {
eventMask |= EV_UNPACKED; // we also need to act on remote updates (unpack events)
// we also need to act on remote updates (unpack events)
eventMask |= EV_UNPACKED;
}
connectToObjectInstances(obj, eventMask);
}
@ -211,21 +229,27 @@ void Telemetry::updateObject(UAVObject *obj, quint32 eventType)
void Telemetry::transactionCompleted(UAVObject *obj, bool success)
{
// Lookup the transaction in the transaction map.
quint32 objId = obj->getObjID();
ObjectTransactionInfo *transInfo = findTransaction(obj);
if (transInfo) {
if (success) {
#ifdef VERBOSE_TELEMETRY
qDebug() << "Telemetry - transaction successful for object" << obj->toStringBrief();
#endif
} else {
qWarning() << "Telemetry - !!! transaction failed for object" << obj->toStringBrief();
}
QMap<quint32, ObjectTransactionInfo *>::iterator itr = transMap.find(objId);
if (itr != transMap.end()) {
ObjectTransactionInfo *transInfo = itr.value();
// Remove this transaction as it's complete.
transInfo->timer->stop();
transMap.remove(objId);
delete transInfo;
closeTransaction(transInfo);
// Send signal
obj->emitTransactionCompleted(success);
// Process new object updates from queue
processObjectQueue();
} else {
qDebug() << "Error: received a transaction completed when did not expect it.";
qWarning() << "Telemetry - Error: received a transaction completed when did not expect it for" << obj->toStringBrief();
}
}
@ -234,25 +258,33 @@ void Telemetry::transactionCompleted(UAVObject *obj, bool success)
*/
void Telemetry::transactionTimeout(ObjectTransactionInfo *transInfo)
{
transInfo->timer->stop();
// Check if more retries are pending
if (transInfo->retriesRemaining > 0) {
--transInfo->retriesRemaining;
processObjectTransaction(transInfo);
#ifdef VERBOSE_TELEMETRY
qDebug().nospace() << "Telemetry - transaction timed out for object " << transInfo->obj->toStringBrief() << ", retrying...";
#endif
++txRetries;
--transInfo->retriesRemaining;
// Retry the transaction
processObjectTransaction(transInfo);
} else {
// Stop the timer.
transInfo->timer->stop();
qWarning().nospace() << "Telemetry - !!! transaction timed out for object " << transInfo->obj->toStringBrief();
++txErrors;
// Terminate transaction
utalk->cancelTransaction(transInfo->obj);
// Send signal
transInfo->obj->emitTransactionCompleted(false);
// Remove this transaction as it's complete.
transMap.remove(transInfo->obj->getObjID());
delete transInfo;
UAVObject *obj = transInfo->obj;
closeTransaction(transInfo);
// Send signal
obj->emitTransactionCompleted(false);
// Process new object updates from queue
processObjectQueue();
++txErrors;
}
}
@ -262,18 +294,32 @@ void Telemetry::transactionTimeout(ObjectTransactionInfo *transInfo)
void Telemetry::processObjectTransaction(ObjectTransactionInfo *transInfo)
{
// Initiate transaction
bool sent = false;
if (transInfo->objRequest) {
utalk->sendObjectRequest(transInfo->obj, transInfo->allInstances);
#ifdef VERBOSE_TELEMETRY
qDebug().nospace() << "Telemetry - sending request for object " << transInfo->obj->toStringBrief() << ", " << (transInfo->allInstances ? "all" : "single") << " " << (transInfo->acked ? "acked" : "");
#endif
sent = utalk->sendObjectRequest(transInfo->obj, transInfo->allInstances);
} else {
utalk->sendObject(transInfo->obj, transInfo->acked, transInfo->allInstances);
#ifdef VERBOSE_TELEMETRY
qDebug().nospace() << "Telemetry - sending object " << transInfo->obj->toStringBrief() << ", " << (transInfo->allInstances ? "all" : "single") << " " << (transInfo->acked ? "acked" : "");
#endif
sent = utalk->sendObject(transInfo->obj, transInfo->acked, transInfo->allInstances);
}
// Start timer if a response is expected
// Check if a response is needed now or will arrive asynchronously
if (transInfo->objRequest || transInfo->acked) {
transInfo->timer->start(REQ_TIMEOUT_MS);
if (sent) {
// Start timer if a response is expected
transInfo->timer->start(REQ_TIMEOUT_MS);
} 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);
}
} else {
// Otherwise, remove this transaction as it's complete.
transMap.remove(transInfo->obj->getObjID());
delete transInfo;
// not transacted, so just close the transaction with no notification of completion
closeTransaction(transInfo);
}
}
@ -293,14 +339,15 @@ void Telemetry::processObjectUpdates(UAVObject *obj, EventMask event, bool allIn
objPriorityQueue.enqueue(objInfo);
} else {
++txErrors;
qWarning().nospace() << "Telemetry - !!! priority event queue is full, event lost " << obj->toStringBrief();
obj->emitTransactionCompleted(false);
qDebug() << tr("Telemetry: priority event queue is full, event lost (%1)").arg(obj->getName());
}
} else {
if (objQueue.length() < MAX_QUEUE_SIZE) {
objQueue.enqueue(objInfo);
} else {
++txErrors;
qWarning().nospace() << "Telemetry - !!! event queue is full, event lost " << obj->toStringBrief();
obj->emitTransactionCompleted(false);
}
}
@ -330,7 +377,9 @@ void Telemetry::processObjectQueue()
GCSTelemetryStats::DataFields gcsStats = gcsStatsObj->getData();
if (gcsStats.Status != GCSTelemetryStats::STATUS_CONNECTED) {
objQueue.clear();
if (objInfo.obj->getObjID() != GCSTelemetryStats::OBJID && objInfo.obj->getObjID() != OPLinkSettings::OBJID && objInfo.obj->getObjID() != ObjectPersistence::OBJID) {
if ((objInfo.obj->getObjID() != GCSTelemetryStats::OBJID) &&
(objInfo.obj->getObjID() != OPLinkSettings::OBJID) &&
(objInfo.obj->getObjID() != ObjectPersistence::OBJID)) {
objInfo.obj->emitTransactionCompleted(false);
return;
}
@ -340,9 +389,15 @@ void Telemetry::processObjectQueue()
UAVObject::Metadata metadata = objInfo.obj->getMetadata();
UAVObject::UpdateMode updateMode = UAVObject::GetGcsTelemetryUpdateMode(metadata);
if ((objInfo.event != EV_UNPACKED) && ((objInfo.event != EV_UPDATED_PERIODIC) || (updateMode != UAVObject::UPDATEMODE_THROTTLED))) {
QMap<quint32, ObjectTransactionInfo *>::iterator itr = transMap.find(objInfo.obj->getObjID());
if (itr != transMap.end()) {
qDebug() << "!!!!!! Making request for an object: " << objInfo.obj->getName() << " for which a request is already in progress!!!!!!";
// Check if a transaction for that object already exists
// It is allowed to have multiple transaction on the same object ID provided that the instance IDs are different
// If an "all instances" transaction is running, then it is not allowed to start another transaction with same object ID
// If a single instance transaction is running, then starting an "all instance" transaction is not allowed
// 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);
return;
}
UAVObject::Metadata metadata = objInfo.obj->getMetadata();
ObjectTransactionInfo *transInfo = new ObjectTransactionInfo(this);
@ -357,7 +412,7 @@ void Telemetry::processObjectQueue()
}
transInfo->telem = this;
// Insert the transaction into the transaction map.
transMap.insert(objInfo.obj->getObjID(), transInfo);
openTransaction(transInfo);
processObjectTransaction(transInfo);
}
@ -396,6 +451,7 @@ void Telemetry::processPeriodicUpdates()
qint32 elapsedMs = 0;
QTime time;
qint32 offset;
bool allInstances;
for (int n = 0; n < objList.length(); ++n) {
objinfo = &objList[n];
// If object is configured for periodic updates
@ -408,8 +464,9 @@ void Telemetry::processPeriodicUpdates()
objinfo->timeToNextUpdateMs = objinfo->updatePeriodMs - offset;
// Send object
time.start();
processObjectUpdates(objinfo->obj, EV_UPDATED_PERIODIC, true, false);
elapsedMs = time.elapsed();
allInstances = !objinfo->obj->isSingleInstance();
processObjectUpdates(objinfo->obj, EV_UPDATED_PERIODIC, allInstances, false);
elapsedMs = time.elapsed();
// Update timeToNextUpdateMs with the elapsed delay of sending the object;
timeToNextUpdateMs += elapsedMs;
}
@ -443,15 +500,18 @@ Telemetry::TelemetryStats Telemetry::getStats()
TelemetryStats stats;
stats.txBytes = utalkStats.txBytes;
stats.rxBytes = utalkStats.rxBytes;
stats.txObjectBytes = utalkStats.txObjectBytes;
stats.rxObjectBytes = utalkStats.rxObjectBytes;
stats.rxObjects = utalkStats.rxObjects;
stats.txObjects = utalkStats.txObjects;
stats.txErrors = utalkStats.txErrors + txErrors;
stats.rxErrors = utalkStats.rxErrors;
stats.txRetries = txRetries;
stats.rxBytes = utalkStats.rxBytes;
stats.rxObjectBytes = utalkStats.rxObjectBytes;
stats.rxObjects = utalkStats.rxObjects;
stats.rxErrors = utalkStats.rxErrors;
stats.rxSyncErrors = utalkStats.rxSyncErrors;
stats.rxCrcErrors = utalkStats.rxCrcErrors;
// Done
return stats;
}
@ -472,11 +532,13 @@ void Telemetry::objectUpdatedAuto(UAVObject *obj)
processObjectUpdates(obj, EV_UPDATED, false, true);
}
void Telemetry::objectUpdatedManual(UAVObject *obj)
void Telemetry::objectUpdatedManual(UAVObject *obj, bool all)
{
QMutexLocker locker(mutex);
processObjectUpdates(obj, EV_UPDATED_MANUAL, false, true);
bool allInstances = obj->isSingleInstance() ? false : all;
processObjectUpdates(obj, EV_UPDATED_MANUAL, allInstances, true);
}
void Telemetry::objectUpdatedPeriodic(UAVObject *obj)
@ -493,11 +555,13 @@ void Telemetry::objectUnpacked(UAVObject *obj)
processObjectUpdates(obj, EV_UNPACKED, false, true);
}
void Telemetry::updateRequested(UAVObject *obj)
void Telemetry::updateRequested(UAVObject *obj, bool all)
{
QMutexLocker locker(mutex);
processObjectUpdates(obj, EV_UPDATE_REQ, false, true);
bool allInstances = obj->isSingleInstance() ? false : all;
processObjectUpdates(obj, EV_UPDATE_REQ, allInstances, true);
}
void Telemetry::newObject(UAVObject *obj)
@ -514,6 +578,67 @@ void Telemetry::newInstance(UAVObject *obj)
registerObject(obj);
}
ObjectTransactionInfo *Telemetry::findTransaction(UAVObject *obj)
{
quint32 objId = obj->getObjID();
quint16 instId = obj->getInstID();
// Lookup the transaction in the transaction map
QMap<quint32, ObjectTransactionInfo *> *objTransactions = transMap.value(objId);
if (objTransactions != NULL) {
ObjectTransactionInfo *trans = objTransactions->value(instId);
if (trans == NULL) {
// see if there is an ALL_INSTANCES transaction
trans = objTransactions->value(UAVTalk::ALL_INSTANCES);
}
return trans;
}
return NULL;
}
void Telemetry::openTransaction(ObjectTransactionInfo *trans)
{
quint32 objId = trans->obj->getObjID();
quint16 instId = trans->allInstances ? UAVTalk::ALL_INSTANCES : trans->obj->getInstID();
QMap<quint32, ObjectTransactionInfo *> *objTransactions = transMap.value(objId);
if (objTransactions == NULL) {
objTransactions = new QMap<quint32, ObjectTransactionInfo *>();
transMap.insert(objId, objTransactions);
}
objTransactions->insert(instId, trans);
}
void Telemetry::closeTransaction(ObjectTransactionInfo *trans)
{
quint32 objId = trans->obj->getObjID();
quint16 instId = trans->allInstances ? UAVTalk::ALL_INSTANCES : trans->obj->getInstID();
QMap<quint32, ObjectTransactionInfo *> *objTransactions = transMap.value(objId);
if (objTransactions != NULL) {
objTransactions->remove(instId);
// Keep the map even if it is empty
// There are at most 100 different object IDs...
}
delete trans;
}
void Telemetry::closeAllTransactions()
{
foreach(quint32 objId, transMap.keys()) {
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;
}
transMap.remove(objId);
delete objTransactions;
}
}
ObjectTransactionInfo::ObjectTransactionInfo(QObject *parent) : QObject(parent)
{
obj = 0;
@ -524,7 +649,7 @@ ObjectTransactionInfo::ObjectTransactionInfo(QObject *parent) : QObject(parent)
telem = 0;
// Setup transaction timer
timer = new QTimer(this);
timer->stop();
timer->setSingleShot(true);
connect(timer, SIGNAL(timeout()), this, SLOT(timeout()));
}

27
ground/openpilotgcs/src/plugins/uavtalk/telemetry.h
View File

@ -60,14 +60,17 @@ class Telemetry : public QObject {
public:
typedef struct {
quint32 txBytes;
quint32 rxBytes;
quint32 txObjectBytes;
quint32 rxObjectBytes;
quint32 rxObjects;
quint32 txObjects;
quint32 txErrors;
quint32 rxErrors;
quint32 txRetries;
quint32 rxBytes;
quint32 rxObjectBytes;
quint32 rxObjects;
quint32 rxErrors;
quint32 rxSyncErrors;
quint32 rxCrcErrors;
} TelemetryStats;
Telemetry(UAVTalk *utalk, UAVObjectManager *objMngr);
@ -95,8 +98,8 @@ private:
EV_UNPACKED = 0x01, /** Object data updated by unpacking */
EV_UPDATED = 0x02, /** Object data updated by changing the data structure */
EV_UPDATED_MANUAL = 0x04, /** Object update event manually generated */
EV_UPDATED_PERIODIC = 0x8, /** Object update event generated by timer */
EV_UPDATE_REQ = 0x010 /** Request to update object data */
EV_UPDATED_PERIODIC = 0x08, /** Object update event generated by timer */
EV_UPDATE_REQ = 0x10 /** Request to update object data */
} EventMask;
typedef struct {
@ -118,7 +121,7 @@ private:
QList<ObjectTimeInfo> objList;
QQueue<ObjectQueueInfo> objQueue;
QQueue<ObjectQueueInfo> objPriorityQueue;
QMap<quint32, ObjectTransactionInfo *>transMap;
QMap<quint32, QMap<quint32, ObjectTransactionInfo *> *> transMap;
QMutex *mutex;
QTimer *updateTimer;
QTimer *statsTimer;
@ -131,17 +134,23 @@ private:
void addObject(UAVObject *obj);
void setUpdatePeriod(UAVObject *obj, qint32 periodMs);
void connectToObjectInstances(UAVObject *obj, quint32 eventMask);
void connectToObject(UAVObject *obj, quint32 eventMask);
void updateObject(UAVObject *obj, quint32 eventMask);
void processObjectUpdates(UAVObject *obj, EventMask event, bool allInstances, bool priority);
void processObjectTransaction(ObjectTransactionInfo *transInfo);
void processObjectQueue();
ObjectTransactionInfo *findTransaction(UAVObject *obj);
void openTransaction(ObjectTransactionInfo *trans);
void closeTransaction(ObjectTransactionInfo *trans);
void closeAllTransactions();
private slots:
void objectUpdatedAuto(UAVObject *obj);
void objectUpdatedManual(UAVObject *obj);
void objectUpdatedManual(UAVObject *obj, bool all = false);
void objectUpdatedPeriodic(UAVObject *obj);
void objectUnpacked(UAVObject *obj);
void updateRequested(UAVObject *obj);
void updateRequested(UAVObject *obj, bool all = false);
void newObject(UAVObject *obj);
void newInstance(UAVObject *obj);
void processPeriodicUpdates();

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

@ -30,8 +30,7 @@
#include <coreplugin/icore.h>
#include <coreplugin/threadmanager.h>
TelemetryManager::TelemetryManager() :
autopilotConnected(false)
TelemetryManager::TelemetryManager() : autopilotConnected(false)
{
moveToThread(Core::ICore::instance()->threadManager()->getRealTimeThread());
// Get UAVObjectManager instance
@ -59,9 +58,31 @@ void TelemetryManager::start(QIODevice *dev)
void TelemetryManager::onStart()
{
utalk = new UAVTalk(device, objMngr);
utalk = new UAVTalk(device, objMngr);
if (false) {
// UAVTalk must be thread safe and for that:
// 1- all public methods must lock a mutex
// 2- the reader thread must lock that mutex too
// The reader thread locks the mutex once a packet is read and decoded.
// It is assumed that the UAVObjectManager is thread safe
// Create the reader and move it to the reader thread
IODeviceReader *reader = new IODeviceReader(utalk);
reader->moveToThread(&readerThread);
// The reader will be deleted (later) when the thread finishes
connect(&readerThread, &QThread::finished, reader, &QObject::deleteLater);
// Connect IO device to reader
connect(device, SIGNAL(readyRead()), reader, SLOT(read()));
// start the reader thread
readerThread.start();
} else {
// Connect IO device to reader
connect(device, SIGNAL(readyRead()), utalk, SLOT(processInputStream()));
}
telemetry = new Telemetry(utalk, objMngr);
telemetryMon = new TelemetryMonitor(objMngr, telemetry);
connect(telemetryMon, SIGNAL(connected()), this, SLOT(onConnect()));
connect(telemetryMon, SIGNAL(disconnected()), this, SLOT(onDisconnect()));
connect(telemetryMon, SIGNAL(telemetryUpdated(double, double)), this, SLOT(onTelemetryUpdate(double, double)));
@ -70,6 +91,11 @@ void TelemetryManager::onStart()
void TelemetryManager::stop()
{
emit myStop();
if (false) {
readerThread.quit();
readerThread.wait();
}
}
void TelemetryManager::onStop()
@ -97,3 +123,11 @@ void TelemetryManager::onTelemetryUpdate(double txRate, double rxRate)
{
emit telemetryUpdated(txRate, rxRate);
}
IODeviceReader::IODeviceReader(UAVTalk *uavTalk) : uavTalk(uavTalk)
{}
void IODeviceReader::read()
{
uavTalk->processInputStream();
}

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

@ -68,6 +68,19 @@ private:
TelemetryMonitor *telemetryMon;
QIODevice *device;
bool autopilotConnected;
QThread readerThread;
};
class IODeviceReader : public QObject {
Q_OBJECT
public:
IODeviceReader(UAVTalk *uavTalk);
UAVTalk *uavTalk;
public slots:
void read();
};
#endif // TELEMETRYMANAGER_H

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

@ -179,6 +179,7 @@ void TelemetryMonitor::firmwareIAPUpdated(UAVObject *obj)
QMutexLocker locker(mutex);
if (firmwareIAPObj->getBoardType() != 0) {
disconnect(firmwareIAPObj);
emit connected();
}
}
@ -198,11 +199,16 @@ void TelemetryMonitor::processStatsUpdates()
tel->resetStats();
// Update stats object
gcsStats.RxDataRate = (float)telStats.rxBytes / ((float)statsTimer->interval() / 1000.0);
gcsStats.TxDataRate = (float)telStats.txBytes / ((float)statsTimer->interval() / 1000.0);
gcsStats.RxFailures += telStats.rxErrors;
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.RxSyncErrors += telStats.rxSyncErrors;
gcsStats.RxCrcErrors += telStats.rxCrcErrors;
// Check for a connection timeout
bool connectionTimeout;

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

@ -25,56 +25,37 @@
* 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA
*/
#include "uavtalk.h"
#include <QtEndian>
#include <QDebug>
#include <extensionsystem/pluginmanager.h>
#include <coreplugin/generalsettings.h>
// #define UAVTALK_DEBUG
#ifdef UAVTALK_DEBUG
#define UAVTALK_QXTLOG_DEBUG(args ...)
#else // UAVTALK_DEBUG
#define UAVTALK_QXTLOG_DEBUG(args ...)
#endif // UAVTALK_DEBUG
#include <utils/crc.h>
#include <QtEndian>
#include <QDebug>
#include <QEventLoop>
#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)
#endif
#ifndef VERBOSE_FILTER
#define VERBOSE_FILTER(objId)
#endif
#define SYNC_VAL 0x3C
const quint8 UAVTalk::crc_table[256] = {
0x00, 0x07, 0x0e, 0x09, 0x1c, 0x1b, 0x12, 0x15, 0x38, 0x3f, 0x36, 0x31, 0x24, 0x23, 0x2a, 0x2d,
0x70, 0x77, 0x7e, 0x79, 0x6c, 0x6b, 0x62, 0x65, 0x48, 0x4f, 0x46, 0x41, 0x54, 0x53, 0x5a, 0x5d,
0xe0, 0xe7, 0xee, 0xe9, 0xfc, 0xfb, 0xf2, 0xf5, 0xd8, 0xdf, 0xd6, 0xd1, 0xc4, 0xc3, 0xca, 0xcd,
0x90, 0x97, 0x9e, 0x99, 0x8c, 0x8b, 0x82, 0x85, 0xa8, 0xaf, 0xa6, 0xa1, 0xb4, 0xb3, 0xba, 0xbd,
0xc7, 0xc0, 0xc9, 0xce, 0xdb, 0xdc, 0xd5, 0xd2, 0xff, 0xf8, 0xf1, 0xf6, 0xe3, 0xe4, 0xed, 0xea,
0xb7, 0xb0, 0xb9, 0xbe, 0xab, 0xac, 0xa5, 0xa2, 0x8f, 0x88, 0x81, 0x86, 0x93, 0x94, 0x9d, 0x9a,
0x27, 0x20, 0x29, 0x2e, 0x3b, 0x3c, 0x35, 0x32, 0x1f, 0x18, 0x11, 0x16, 0x03, 0x04, 0x0d, 0x0a,
0x57, 0x50, 0x59, 0x5e, 0x4b, 0x4c, 0x45, 0x42, 0x6f, 0x68, 0x61, 0x66, 0x73, 0x74, 0x7d, 0x7a,
0x89, 0x8e, 0x87, 0x80, 0x95, 0x92, 0x9b, 0x9c, 0xb1, 0xb6, 0xbf, 0xb8, 0xad, 0xaa, 0xa3, 0xa4,
0xf9, 0xfe, 0xf7, 0xf0, 0xe5, 0xe2, 0xeb, 0xec, 0xc1, 0xc6, 0xcf, 0xc8, 0xdd, 0xda, 0xd3, 0xd4,
0x69, 0x6e, 0x67, 0x60, 0x75, 0x72, 0x7b, 0x7c, 0x51, 0x56, 0x5f, 0x58, 0x4d, 0x4a, 0x43, 0x44,
0x19, 0x1e, 0x17, 0x10, 0x05, 0x02, 0x0b, 0x0c, 0x21, 0x26, 0x2f, 0x28, 0x3d, 0x3a, 0x33, 0x34,
0x4e, 0x49, 0x40, 0x47, 0x52, 0x55, 0x5c, 0x5b, 0x76, 0x71, 0x78, 0x7f, 0x6a, 0x6d, 0x64, 0x63,
0x3e, 0x39, 0x30, 0x37, 0x22, 0x25, 0x2c, 0x2b, 0x06, 0x01, 0x08, 0x0f, 0x1a, 0x1d, 0x14, 0x13,
0xae, 0xa9, 0xa0, 0xa7, 0xb2, 0xb5, 0xbc, 0xbb, 0x96, 0x91, 0x98, 0x9f, 0x8a, 0x8d, 0x84, 0x83,
0xde, 0xd9, 0xd0, 0xd7, 0xc2, 0xc5, 0xcc, 0xcb, 0xe6, 0xe1, 0xe8, 0xef, 0xfa, 0xfd, 0xf4, 0xf3
};
using namespace Utils;
/**
* Constructor
*/
UAVTalk::UAVTalk(QIODevice *iodev, UAVObjectManager *objMngr)
UAVTalk::UAVTalk(QIODevice *iodev, UAVObjectManager *objMngr) : io(iodev), objMngr(objMngr), mutex(QMutex::Recursive)
{
io = iodev;
this->objMngr = objMngr;
rxState = STATE_SYNC;
rxPacketLength = 0;
mutex = new QMutex(QMutex::Recursive);
memset(&stats, 0, sizeof(ComStats));
connect(io, SIGNAL(readyRead()), this, SLOT(processInputStream()));
ExtensionSystem::PluginManager *pm = ExtensionSystem::PluginManager::instance();
Core::Internal::GeneralSettings *settings = pm->getObject<Core::Internal::GeneralSettings>();
useUDPMirror = settings->useUDPMirror();
@ -91,18 +72,18 @@ UAVTalk::UAVTalk(QIODevice *iodev, UAVObjectManager *objMngr)
UAVTalk::~UAVTalk()
{
// According to Qt, it is not necessary to disconnect upon
// object deletion.
// disconnect(io, SIGNAL(readyRead()), this, SLOT(processInputStream()));
}
// According to Qt, it is not necessary to disconnect upon object deletion.
// disconnect(io, SIGNAL(readyRead()), worker, SLOT(processInputStream()));
closeAllTransactions();
}
/**
* Reset the statistics counters
*/
void UAVTalk::resetStats()
{
QMutexLocker locker(mutex);
QMutexLocker locker(&mutex);
memset(&stats, 0, sizeof(ComStats));
}
@ -112,26 +93,11 @@ void UAVTalk::resetStats()
*/
UAVTalk::ComStats UAVTalk::getStats()
{
QMutexLocker locker(mutex);
QMutexLocker locker(&mutex);
return stats;
}
/**
* Called each time there are data in the input buffer
*/
void UAVTalk::processInputStream()
{
quint8 tmp;
if (io && io->isReadable()) {
while (io->bytesAvailable() > 0) {
io->read((char *)&tmp, 1);
processInputByte(tmp);
}
}
}
void UAVTalk::dummyUDPRead()
{
QUdpSocket *socket = qobject_cast<QUdpSocket *>(sender());
@ -143,20 +109,6 @@ void UAVTalk::dummyUDPRead()
}
}
/**
* Request an update for the specified object, on success the object data would have been
* updated by the GCS.
* \param[in] obj Object to update
* \param[in] allInstances If set true then all instances will be updated
* \return Success (true), Failure (false)
*/
bool UAVTalk::sendObjectRequest(UAVObject *obj, bool allInstances)
{
QMutexLocker locker(mutex);
return objectTransaction(obj, TYPE_OBJ_REQ, allInstances);
}
/**
* Send the specified object through the telemetry link.
* \param[in] obj Object to send
@ -166,13 +118,44 @@ bool UAVTalk::sendObjectRequest(UAVObject *obj, bool allInstances)
*/
bool UAVTalk::sendObject(UAVObject *obj, bool acked, bool allInstances)
{
QMutexLocker locker(mutex);
QMutexLocker locker(&mutex);
if (acked) {
return objectTransaction(obj, TYPE_OBJ_ACK, allInstances);
} else {
return objectTransaction(obj, TYPE_OBJ, allInstances);
quint16 instId = 0;
if (allInstances) {
instId = ALL_INSTANCES;
} else if (obj) {
instId = obj->getInstID();
}
bool success = false;
if (acked) {
success = objectTransaction(TYPE_OBJ_ACK, obj->getObjID(), instId, obj);
} else {
success = objectTransaction(TYPE_OBJ, obj->getObjID(), instId, obj);
}
return success;
}
/**
* Request an update for the specified object, on success the object data would have been
* updated by the GCS.
* \param[in] obj Object to update
* \param[in] allInstances If set true then all instances will be updated
* \return Success (true), Failure (false)
*/
bool UAVTalk::sendObjectRequest(UAVObject *obj, bool allInstances)
{
QMutexLocker locker(&mutex);
quint16 instId = 0;
if (allInstances) {
instId = ALL_INSTANCES;
} else if (obj) {
instId = obj->getInstID();
}
return objectTransaction(TYPE_OBJ_REQ, obj->getObjID(), instId, obj);
}
/**
@ -180,16 +163,14 @@ bool UAVTalk::sendObject(UAVObject *obj, bool acked, bool allInstances)
*/
void UAVTalk::cancelTransaction(UAVObject *obj)
{
QMutexLocker locker(mutex);
quint32 objId = obj->getObjID();
QMutexLocker locker(&mutex);
if (io.isNull()) {
return;
}
QMap<quint32, Transaction *>::iterator itr = transMap.find(objId);
if (itr != transMap.end()) {
transMap.remove(objId);
delete itr.value();
Transaction *trans = findTransaction(obj->getObjID(), obj->getInstID());
if (trans != NULL) {
closeTransaction(trans);
}
}
@ -203,26 +184,60 @@ void UAVTalk::cancelTransaction(UAVObject *obj)
* \param[in] allInstances If set true then all instances will be updated
* \return Success (true), Failure (false)
*/
bool UAVTalk::objectTransaction(UAVObject *obj, quint8 type, bool allInstances)
bool UAVTalk::objectTransaction(quint8 type, quint32 objId, quint16 instId, UAVObject *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) {
if (transmitObject(obj, type, allInstances)) {
Transaction *trans = new Transaction();
trans->obj = obj;
trans->allInstances = allInstances;
transMap.insert(obj->getObjID(), trans);
if (transmitObject(type, objId, instId, obj)) {
openTransaction(type, objId, instId);
return true;
} else {
return false;
}
} else if (type == TYPE_OBJ) {
return transmitObject(obj, TYPE_OBJ, allInstances);
return transmitObject(type, objId, instId, obj);
} else {
return false;
}
}
/**
* Called each time there are data in the input buffer
*/
void UAVTalk::processInputStream()
{
quint8 tmp;
if (io && io->isReadable()) {
while (io->bytesAvailable() > 0) {
int ret = io->read((char *)&tmp, 1);
if (ret != -1) {
processInputByte(tmp);
} else {
// TODOD
}
if (rxState == STATE_COMPLETE) {
mutex.lock();
if (receiveObject(rxType, rxObjId, rxInstId, rxBuffer, rxLength)) {
stats.rxObjectBytes += rxLength;
stats.rxObjects++;
} else {
// TODO...
}
mutex.unlock();
if (useUDPMirror) {
// it is safe to do this outside of the above critical section as the rxDataArray is
// accessed from this thread only
udpSocketTx->writeDatagram(rxDataArray, QHostAddress::LocalHost, udpSocketRx->localPort());
}
}
}
}
}
/**
* Process an byte from the telemetry stream.
* \param[in] rxbyte Received byte
@ -230,10 +245,19 @@ bool UAVTalk::objectTransaction(UAVObject *obj, quint8 type, bool allInstances)
*/
bool UAVTalk::processInputByte(quint8 rxbyte)
{
if (rxState == STATE_COMPLETE || rxState == STATE_ERROR) {
rxState = STATE_SYNC;
if (useUDPMirror) {
rxDataArray.clear();
}
}
// Update stats
stats.rxBytes++;
rxPacketLength++; // update packet byte count
// update packet byte count
rxPacketLength++;
if (useUDPMirror) {
rxDataArray.append(rxbyte);
@ -244,32 +268,31 @@ bool UAVTalk::processInputByte(quint8 rxbyte)
case STATE_SYNC:
if (rxbyte != SYNC_VAL) {
UAVTALK_QXTLOG_DEBUG("UAVTalk: Sync->Sync (" + QString::number(rxbyte) + " " + QString("0x%1").arg(rxbyte, 2, 16) + ")");
// continue until sync byte is matched
stats.rxSyncErrors++;
break;
}
// Initialize and update CRC
rxCS = updateCRC(0, rxbyte);
rxCS = Crc::updateCRC(0, rxbyte);
rxPacketLength = 1;
if (useUDPMirror) {
rxDataArray.clear();
rxDataArray.append(rxbyte);
}
// case local byte counter, don't forget to zero it after use.
rxCount = 0;
rxState = STATE_TYPE;
UAVTALK_QXTLOG_DEBUG("UAVTalk: Sync->Type");
break;
case STATE_TYPE:
// Update CRC
rxCS = updateCRC(rxCS, rxbyte);
rxCS = Crc::updateCRC(rxCS, rxbyte);
if ((rxbyte & TYPE_MASK) != TYPE_VER) {
rxState = STATE_SYNC;
UAVTALK_QXTLOG_DEBUG("UAVTalk: Type->Sync");
qWarning() << "UAVTalk - error : bad type";
stats.rxErrors++;
rxState = STATE_ERROR;
break;
}
@ -278,149 +301,124 @@ bool UAVTalk::processInputByte(quint8 rxbyte)
packetSize = 0;
rxState = STATE_SIZE;
UAVTALK_QXTLOG_DEBUG("UAVTalk: Type->Size");
rxCount = 0;
break;
case STATE_SIZE:
// Update CRC
rxCS = updateCRC(rxCS, rxbyte);
rxCS = Crc::updateCRC(rxCS, rxbyte);
if (rxCount == 0) {
packetSize += rxbyte;
rxCount++;
UAVTALK_QXTLOG_DEBUG("UAVTalk: Size->Size");
break;
}
packetSize += (quint32)rxbyte << 8;
rxCount = 0;
if (packetSize < MIN_HEADER_LENGTH || packetSize > MAX_HEADER_LENGTH + MAX_PAYLOAD_LENGTH) { // incorrect packet size
rxState = STATE_SYNC;
UAVTALK_QXTLOG_DEBUG("UAVTalk: Size->Sync");
if (packetSize < HEADER_LENGTH || packetSize > HEADER_LENGTH + MAX_PAYLOAD_LENGTH) {
// incorrect packet size
qWarning() << "UAVTalk - error : incorrect packet size";
stats.rxErrors++;
rxState = STATE_ERROR;
break;
}
rxCount = 0;
rxState = STATE_OBJID;
UAVTALK_QXTLOG_DEBUG("UAVTalk: Size->ObjID");
break;
case STATE_OBJID:
// Update CRC
rxCS = updateCRC(rxCS, rxbyte);
rxCS = Crc::updateCRC(rxCS, rxbyte);
rxTmpBuffer[rxCount++] = rxbyte;
if (rxCount < 4) {
UAVTALK_QXTLOG_DEBUG("UAVTalk: ObjID->ObjID");
break;
}
rxCount = 0;
rxObjId = (qint32)qFromLittleEndian<quint32>(rxTmpBuffer);
// Message always contain an instance ID
rxInstId = 0;
rxState = STATE_INSTID;
break;
case STATE_INSTID:
// Update CRC
rxCS = Crc::updateCRC(rxCS, rxbyte);
rxTmpBuffer[rxCount++] = rxbyte;
if (rxCount < 2) {
break;
}
rxCount = 0;
rxInstId = (qint16)qFromLittleEndian<quint16>(rxTmpBuffer);
// Search for object, if not found reset state machine
rxObjId = (qint32)qFromLittleEndian<quint32>(rxTmpBuffer);
{
UAVObject *rxObj = objMngr->getObject(rxObjId);
if (rxObj == NULL && rxType != TYPE_OBJ_REQ) {
qWarning() << "UAVTalk - error : unknown object" << rxObjId;
stats.rxErrors++;
rxState = STATE_SYNC;
UAVTALK_QXTLOG_DEBUG("UAVTalk: ObjID->Sync (badtype)");
rxState = STATE_ERROR;
break;
}
// Determine data length
if (rxType == TYPE_OBJ_REQ || rxType == TYPE_ACK || rxType == TYPE_NACK) {
rxLength = 0;
rxInstanceLength = 0;
} else {
rxLength = rxObj->getNumBytes();
rxInstanceLength = (rxObj->isSingleInstance() ? 0 : 2);
if (rxObj) {
rxLength = rxObj->getNumBytes();
} else {
rxLength = packetSize - rxPacketLength;
}
}
// Check length and determine next state
if (rxLength >= MAX_PAYLOAD_LENGTH) {
// packet error - exceeded payload max length
qWarning() << "UAVTalk - error : exceeded payload max length" << rxObjId;
stats.rxErrors++;
rxState = STATE_SYNC;
UAVTALK_QXTLOG_DEBUG("UAVTalk: ObjID->Sync (oversize)");
rxState = STATE_ERROR;
break;
}
// Check the lengths match
if ((rxPacketLength + rxInstanceLength + rxLength) != packetSize) { // packet error - mismatched packet size
if ((rxPacketLength + rxLength) != packetSize) {
// packet error - mismatched packet size
qWarning() << "UAVTalk - error : mismatched packet size" << rxObjId;
stats.rxErrors++;
rxState = STATE_SYNC;
UAVTALK_QXTLOG_DEBUG("UAVTalk: ObjID->Sync (length mismatch)");
rxState = STATE_ERROR;
break;
}
// Check if this is a single instance object (i.e. if the instance ID field is coming next)
if (rxObj == NULL) {
// This is a non-existing object, just skip to checksum
// and we'll send a NACK next.
rxState = STATE_CS;
UAVTALK_QXTLOG_DEBUG("UAVTalk: ObjID->CSum (no obj)");
rxInstId = 0;
rxCount = 0;
} else if (rxObj->isSingleInstance()) {
// If there is a payload get it, otherwise receive checksum
if (rxLength > 0) {
rxState = STATE_DATA;
UAVTALK_QXTLOG_DEBUG("UAVTalk: ObjID->Data (needs data)");
} else {
rxState = STATE_CS;
UAVTALK_QXTLOG_DEBUG("UAVTalk: ObjID->Checksum");
}
rxInstId = 0;
rxCount = 0;
} else {
rxState = STATE_INSTID;
UAVTALK_QXTLOG_DEBUG("UAVTalk: ObjID->InstID");
rxCount = 0;
}
}
break;
case STATE_INSTID:
// Update CRC
rxCS = updateCRC(rxCS, rxbyte);
rxTmpBuffer[rxCount++] = rxbyte;
if (rxCount < 2) {
UAVTALK_QXTLOG_DEBUG("UAVTalk: InstID->InstID");
break;
}
rxInstId = (qint16)qFromLittleEndian<quint16>(rxTmpBuffer);
rxCount = 0;
// If there is a payload get it, otherwise receive checksum
if (rxLength > 0) {
rxState = STATE_DATA;
UAVTALK_QXTLOG_DEBUG("UAVTalk: InstID->Data");
} else {
rxState = STATE_CS;
UAVTALK_QXTLOG_DEBUG("UAVTalk: InstID->CSum");
}
break;
case STATE_DATA:
// Update CRC
rxCS = updateCRC(rxCS, rxbyte);
rxCS = Crc::updateCRC(rxCS, rxbyte);
rxBuffer[rxCount++] = rxbyte;
if (rxCount < rxLength) {
UAVTALK_QXTLOG_DEBUG("UAVTalk: Data->Data");
break;
}
rxCount = 0;
rxState = STATE_CS;
UAVTALK_QXTLOG_DEBUG("UAVTalk: Data->CSum");
rxCount = 0;
break;
case STATE_CS:
@ -428,37 +426,30 @@ bool UAVTalk::processInputByte(quint8 rxbyte)
// The CRC byte
rxCSPacket = rxbyte;
if (rxCS != rxCSPacket) { // packet error - faulty CRC
stats.rxErrors++;
rxState = STATE_SYNC;
UAVTALK_QXTLOG_DEBUG("UAVTalk: CSum->Sync (badcrc)");
if (rxCS != rxCSPacket) {
// packet error - faulty CRC
qWarning() << "UAVTalk - error : failed CRC check" << rxObjId;
stats.rxCrcErrors++;
rxState = STATE_ERROR;
break;
}
if (rxPacketLength != packetSize + 1) { // packet error - mismatched packet size
if (rxPacketLength != packetSize + CHECKSUM_LENGTH) {
// packet error - mismatched packet size
qWarning() << "UAVTalk - error : mismatched packet size" << rxObjId;
stats.rxErrors++;
rxState = STATE_SYNC;
UAVTALK_QXTLOG_DEBUG("UAVTalk: CSum->Sync (length mismatch)");
rxState = STATE_ERROR;
break;
}
mutex->lock();
receiveObject(rxType, rxObjId, rxInstId, rxBuffer, rxLength);
if (useUDPMirror) {
udpSocketTx->writeDatagram(rxDataArray, QHostAddress::LocalHost, udpSocketRx->localPort());
}
stats.rxObjectBytes += rxLength;
stats.rxObjects++;
mutex->unlock();
rxState = STATE_SYNC;
UAVTALK_QXTLOG_DEBUG("UAVTalk: CSum->Sync (OK)");
rxState = STATE_COMPLETE;
break;
default:
rxState = STATE_SYNC;
stats.rxErrors++;
UAVTALK_QXTLOG_DEBUG("UAVTalk: \?\?\?->Sync"); // Use the escape character for '?' so that the tripgraph isn't triggered.
qWarning() << "UAVTalk - error : bad state";
rxState = STATE_ERROR;
break;
}
// Done
@ -467,6 +458,13 @@ bool UAVTalk::processInputByte(quint8 rxbyte)
/**
* Receive an object. This function process objects received through the telemetry stream.
*
* Parser errors are considered as transmission errors and are not NACKed.
* Some senders (GCS) can timeout and retry if the message is not answered by an ack or nack.
*
* Object handling errors are considered as application errors and are NACked.
* In that case we want to nack as there is no point in the sender retrying to send invalid objects.
*
* \param[in] type Type of received message (TYPE_OBJ, TYPE_OBJ_REQ, TYPE_OBJ_ACK, TYPE_ACK, TYPE_NACK)
* \param[in] obj Handle of the received object
* \param[in] instId The instance ID of UAVOBJ_ALL_INSTANCES for all instances.
@ -477,6 +475,7 @@ bool UAVTalk::processInputByte(quint8 rxbyte)
bool UAVTalk::receiveObject(quint8 type, quint32 objId, quint16 instId, quint8 *data, qint32 length)
{
Q_UNUSED(length);
UAVObject *obj = NULL;
bool error = false;
bool allInstances = (instId == ALL_INSTANCES);
@ -488,9 +487,14 @@ bool UAVTalk::receiveObject(quint8 type, quint32 objId, quint16 instId, quint8 *
if (!allInstances) {
// Get object and update its data
obj = updateObject(objId, instId, data);
// Check if an ack is pending
#ifdef VERBOSE_UAVTALK
VERBOSE_FILTER(objId) qDebug() << "UAVTalk - received object" << objId << instId << (obj != NULL ? obj->toStringBrief() : "<null object>");
#endif
if (obj != NULL) {
updateAck(obj);
// Check if this object acks a pending OBJ_REQ message
// any OBJ message can ack a pending OBJ_REQ message
// even one that was not sent in response to the OBJ_REQ message
updateAck(type, objId, instId, obj);
} else {
error = true;
}
@ -498,67 +502,94 @@ bool UAVTalk::receiveObject(quint8 type, quint32 objId, quint16 instId, quint8 *
error = true;
}
break;
case TYPE_OBJ_ACK:
// All instances, not allowed for OBJ_ACK messages
if (!allInstances) {
// Get object and update its data
obj = updateObject(objId, instId, data);
// Transmit ACK
#ifdef VERBOSE_UAVTALK
VERBOSE_FILTER(objId) qDebug() << "UAVTalk - received object (acked)" << objId << instId << (obj != NULL ? obj->toStringBrief() : "<null object>");
#endif
if (obj != NULL) {
transmitObject(obj, TYPE_ACK, false);
// Object updated or created, transmit ACK
error = !transmitObject(TYPE_ACK, objId, instId, obj);
} else {
error = true;
}
} else {
error = true;
}
if (error) {
// failed to update object, transmit NACK
transmitObject(TYPE_NACK, objId, instId, NULL);
}
break;
case TYPE_OBJ_REQ:
// Get object, if all instances are requested get instance 0 of the object
// Check if requested object exists
if (allInstances) {
// All instances, so get instance zero
obj = objMngr->getObject(objId);
} else {
obj = objMngr->getObject(objId, instId);
}
// If object was found transmit it
#ifdef VERBOSE_UAVTALK
VERBOSE_FILTER(objId) qDebug() << "UAVTalk - received object request" << objId << instId << (obj != NULL ? obj->toStringBrief() : "<null object>");
#endif
if (obj != NULL) {
transmitObject(obj, TYPE_OBJ, allInstances);
// Object found, transmit it
// The sent object will ack the object request on the receiver side
error = !transmitObject(TYPE_OBJ, objId, instId, obj);
} else {
// Object was not found, transmit a NACK with the
// objId which was not found.
transmitNack(objId);
error = true;
}
break;
case TYPE_NACK:
// All instances, not allowed for NACK messages
if (!allInstances) {
// Get object
obj = objMngr->getObject(objId, instId);
// Check if object exists:
if (obj != NULL) {
updateNack(obj);
} else {
error = true;
}
if (error) {
// failed to send object, transmit NACK
transmitObject(TYPE_NACK, objId, instId, NULL);
}
break;
case TYPE_ACK:
// All instances, not allowed for ACK messages
if (!allInstances) {
// Get object
obj = objMngr->getObject(objId, instId);
// Check if an ack is pending
#ifdef VERBOSE_UAVTALK
VERBOSE_FILTER(objId) qDebug() << "UAVTalk - received ack" << objId << instId << (obj != NULL ? obj->toStringBrief() : "<null object>");
#endif
if (obj != NULL) {
updateAck(obj);
// Check if an ACK is pending
updateAck(type, objId, instId, obj);
} else {
error = true;
}
}
break;
case TYPE_NACK:
// All instances, not allowed for NACK messages
if (!allInstances) {
// Get object
obj = objMngr->getObject(objId, instId);
#ifdef VERBOSE_UAVTALK
VERBOSE_FILTER(objId) qDebug() << "UAVTalk - received nack" << objId << instId << (obj != NULL ? obj->toStringBrief() : "<null object>");
#endif
if (obj != NULL) {
// Check if a NACK is pending
updateNack(objId, instId, obj);
} else {
error = true;
}
}
break;
default:
error = true;
}
if (error) {
qWarning() << "UAVTalk - !!! error receiving" << typeToString(type) << objId << instId << (obj != NULL ? obj->toStringBrief() : "<null object>");
}
// Done
return !error;
}
@ -576,22 +607,24 @@ UAVObject *UAVTalk::updateObject(quint32 objId, quint16 instId, quint8 *data)
// If the instance does not exist create it
if (obj == NULL) {
// Get the object type
UAVObject *tobj = objMngr->getObject(objId);
if (tobj == NULL) {
UAVObject *typeObj = objMngr->getObject(objId);
if (typeObj == NULL) {
qWarning() << "UAVTalk - failed to get object, object ID :" << objId;
return NULL;
}
// Make sure this is a data object
UAVDataObject *dobj = dynamic_cast<UAVDataObject *>(tobj);
if (dobj == NULL) {
UAVDataObject *dataObj = dynamic_cast<UAVDataObject *>(typeObj);
if (dataObj == NULL) {
return NULL;
}
// Create a new instance, unpack and register
UAVDataObject *instobj = dobj->clone(instId);
if (!objMngr->registerObject(instobj)) {
UAVDataObject *instObj = dataObj->clone(instId);
if (!objMngr->registerObject(instObj)) {
qWarning() << "UAVTalk - failed to register object " << instObj->toStringBrief();
return NULL;
}
instobj->unpack(data);
return instobj;
instObj->unpack(data);
return instObj;
} else {
// Unpack data into object instance
obj->unpack(data);
@ -602,151 +635,129 @@ UAVObject *UAVTalk::updateObject(quint32 objId, quint16 instId, quint8 *data)
/**
* Check if a transaction is pending and if yes complete it.
*/
void UAVTalk::updateNack(UAVObject *obj)
void UAVTalk::updateAck(quint8 type, quint32 objId, quint16 instId, UAVObject *obj)
{
Q_ASSERT(obj);
if (!obj) {
return;
}
quint32 objId = obj->getObjID();
QMap<quint32, Transaction *>::iterator itr = transMap.find(objId);
if (itr != transMap.end() && (itr.value()->obj->getInstID() == obj->getInstID() || itr.value()->allInstances)) {
transMap.remove(objId);
delete itr.value();
emit transactionCompleted(obj, false);
Transaction *trans = findTransaction(objId, instId);
if (trans && trans->respType == type) {
if (trans->respInstId == ALL_INSTANCES) {
if (instId == 0) {
// last instance received, complete transaction
closeTransaction(trans);
emit transactionCompleted(obj, true);
} else {
// TODO extend timeout?
}
} else {
closeTransaction(trans);
emit transactionCompleted(obj, true);
}
}
}
/**
* Check if a transaction is pending and if yes complete it.
*/
void UAVTalk::updateAck(UAVObject *obj)
void UAVTalk::updateNack(quint32 objId, quint16 instId, UAVObject *obj)
{
quint32 objId = obj->getObjID();
QMap<quint32, Transaction *>::iterator itr = transMap.find(objId);
if (itr != transMap.end() && (itr.value()->obj->getInstID() == obj->getInstID() || itr.value()->allInstances)) {
transMap.remove(objId);
delete itr.value();
emit transactionCompleted(obj, true);
Q_ASSERT(obj);
if (!obj) {
return;
}
Transaction *trans = findTransaction(objId, instId);
if (trans) {
closeTransaction(trans);
emit transactionCompleted(obj, false);
}
}
/**
* Send an object through the telemetry link.
* \param[in] obj Object to send
* \param[in] type Transaction type
* \param[in] allInstances True is all instances of the object are to be sent
* \param[in] objId Object ID to send
* \param[in] instId Instance ID to send
* \param[in] obj Object to send (null when type is NACK)
* \return Success (true), Failure (false)
*/
bool UAVTalk::transmitObject(UAVObject *obj, quint8 type, bool allInstances)
bool UAVTalk::transmitObject(quint8 type, quint32 objId, quint16 instId, UAVObject *obj)
{
// Important note : obj can be null (when type is NACK for example) so protect all obj dereferences.
// If all instances are requested on a single instance object it is an error
if (allInstances && obj->isSingleInstance()) {
allInstances = false;
if ((obj != NULL) && (instId == ALL_INSTANCES) && obj->isSingleInstance()) {
instId = 0;
}
bool allInstances = (instId == ALL_INSTANCES);
#ifdef VERBOSE_UAVTALK
VERBOSE_FILTER(objId) qDebug() << "UAVTalk - transmitting" << typeToString(type) << objId << instId << (obj != NULL ? obj->toStringBrief() : "<null object>");
#endif
// Process message type
bool ret = false;
if (type == TYPE_OBJ || type == TYPE_OBJ_ACK) {
if (allInstances) {
// Get number of instances
quint32 numInst = objMngr->getNumInstances(obj->getObjID());
// Send all instances
for (quint32 instId = 0; instId < numInst; ++instId) {
UAVObject *inst = objMngr->getObject(obj->getObjID(), instId);
transmitSingleObject(inst, type, false);
// 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 = true;
quint32 numInst = objMngr->getNumInstances(objId);
for (quint32 n = 0; n < numInst; ++n) {
quint32 i = numInst - n - 1;
UAVObject *o = objMngr->getObject(objId, i);
if (!transmitSingleObject(type, objId, i, o)) {
ret = false;
break;
}
}
return true;
} else {
return transmitSingleObject(obj, type, false);
ret = transmitSingleObject(type, objId, instId, obj);
}
} else if (type == TYPE_OBJ_REQ) {
return transmitSingleObject(obj, TYPE_OBJ_REQ, allInstances);
} else if (type == TYPE_ACK) {
ret = transmitSingleObject(TYPE_OBJ_REQ, objId, instId, NULL);
} else if (type == TYPE_ACK || type == TYPE_NACK) {
if (!allInstances) {
return transmitSingleObject(obj, TYPE_ACK, false);
} else {
return false;
ret = transmitSingleObject(type, objId, instId, NULL);
}
} else {
return false;
}
}
/**
* Transmit a NACK through the telemetry link.
* \param[in] objId the ObjectID we rejected
*/
bool UAVTalk::transmitNack(quint32 objId)
{
int dataOffset = 8;
txBuffer[0] = SYNC_VAL;
txBuffer[1] = TYPE_NACK;
qToLittleEndian<quint32>(objId, &txBuffer[4]);
// Calculate checksum
txBuffer[dataOffset] = updateCRC(0, txBuffer, dataOffset);
qToLittleEndian<quint16>(dataOffset, &txBuffer[2]);
// Send buffer, check that the transmit backlog does not grow above limit
if (io && io->isWritable() && io->bytesToWrite() < TX_BUFFER_SIZE) {
io->write((const char *)txBuffer, dataOffset + CHECKSUM_LENGTH);
if (useUDPMirror) {
udpSocketRx->writeDatagram((const char *)txBuffer, dataOffset + CHECKSUM_LENGTH, QHostAddress::LocalHost, udpSocketTx->localPort());
}
} else {
++stats.txErrors;
return false;
}
// Update stats
stats.txBytes += 8 + CHECKSUM_LENGTH;
#ifdef VERBOSE_UAVTALK
if (!ret) {
VERBOSE_FILTER(objId) qDebug() << "UAVTalk - failed to transmit" << typeToString(type) << objId << instId << (obj != NULL ? obj->toStringBrief() : "<null object>");
}
#endif
// Done
return true;
return ret;
}
/**
* Send an object through the telemetry link.
* \param[in] obj Object handle to send
* \param[in] type Transaction type
* \param[in] objId Object ID to send
* \param[in] instId Instance ID to send
* \param[in] obj Object to send (null when type is NACK)
* \return Success (true), Failure (false)
*/
bool UAVTalk::transmitSingleObject(UAVObject *obj, quint8 type, bool allInstances)
bool UAVTalk::transmitSingleObject(quint8 type, quint32 objId, quint16 instId, UAVObject *obj)
{
qint32 length;
qint32 dataOffset;
quint32 objId;
quint16 instId;
quint16 allInstId = ALL_INSTANCES;
// Setup type and object id fields
objId = obj->getObjID();
// IMPORTANT : obj can be null (when type is NACK for example)
// Setup sync byte
txBuffer[0] = SYNC_VAL;
// Setup type
txBuffer[1] = type;
// next 2 bytes are reserved for data length (inserted here later)
// Setup object ID
qToLittleEndian<quint32>(objId, &txBuffer[4]);
// Setup instance ID if one is required
if (obj->isSingleInstance()) {
dataOffset = 8;
} else {
// Check if all instances are requested
if (allInstances) {
qToLittleEndian<quint16>(allInstId, &txBuffer[8]);
} else {
instId = obj->getInstID();
qToLittleEndian<quint16>(instId, &txBuffer[8]);
}
dataOffset = 10;
}
// Setup instance ID
qToLittleEndian<quint16>(instId, &txBuffer[8]);
// Determine data length
if (type == TYPE_OBJ_REQ || type == TYPE_ACK) {
if (type == TYPE_OBJ_REQ || type == TYPE_ACK || type == TYPE_NACK) {
length = 0;
} else {
length = obj->getNumBytes();
@ -754,67 +765,138 @@ bool UAVTalk::transmitSingleObject(UAVObject *obj, quint8 type, bool allInstance
// Check length
if (length >= MAX_PAYLOAD_LENGTH) {
qWarning() << "UAVTalk - error transmitting : object exceeds max payload length" << obj->toStringBrief();
++stats.txErrors;
return false;
}
// Copy data (if any)
if (length > 0) {
if (!obj->pack(&txBuffer[dataOffset])) {
if (!obj->pack(&txBuffer[HEADER_LENGTH])) {
qWarning() << "UAVTalk - error transmitting : failed to pack object" << obj->toStringBrief();
++stats.txErrors;
return false;
}
}
qToLittleEndian<quint16>(dataOffset + length, &txBuffer[2]);
// Store the packet length
qToLittleEndian<quint16>(HEADER_LENGTH + length, &txBuffer[2]);
// Calculate checksum
txBuffer[dataOffset + length] = updateCRC(0, txBuffer, dataOffset + length);
txBuffer[HEADER_LENGTH + length] = Crc::updateCRC(0, txBuffer, HEADER_LENGTH + length);
// Send buffer, check that the transmit backlog does not grow above limit
if (!io.isNull() && io->isWritable() && io->bytesToWrite() < TX_BUFFER_SIZE) {
io->write((const char *)txBuffer, dataOffset + length + CHECKSUM_LENGTH);
if (useUDPMirror) {
udpSocketRx->writeDatagram((const char *)txBuffer, dataOffset + length + CHECKSUM_LENGTH, QHostAddress::LocalHost, udpSocketTx->localPort());
if (!io.isNull() && io->isWritable()) {
if (io->bytesToWrite() < TX_BUFFER_SIZE) {
io->write((const char *)txBuffer, HEADER_LENGTH + length + CHECKSUM_LENGTH);
if (useUDPMirror) {
udpSocketRx->writeDatagram((const char *)txBuffer, HEADER_LENGTH + length + CHECKSUM_LENGTH, QHostAddress::LocalHost, udpSocketTx->localPort());
}
} else {
qWarning() << "UAVTalk - error transmitting : io device full";
++stats.txErrors;
return false;
}
} else {
qWarning() << "UAVTalk - error transmitting : io device not writable";
++stats.txErrors;
return false;
}
// Update stats
++stats.txObjects;
stats.txBytes += dataOffset + length + CHECKSUM_LENGTH;
stats.txObjectBytes += length;
stats.txBytes += HEADER_LENGTH + length + CHECKSUM_LENGTH;
// Done
return true;
}
/**
* Update the crc value with new data.
*
* Generated by pycrc v0.7.5, http://www.tty1.net/pycrc/
* using the configuration:
* Width = 8
* Poly = 0x07
* XorIn = 0x00
* ReflectIn = False
* XorOut = 0x00
* ReflectOut = False
* Algorithm = table-driven
*
* \param crc The current crc value.
* \param data Pointer to a buffer of \a data_len bytes.
* \param length Number of bytes in the \a data buffer.
* \return The updated crc value.
*/
quint8 UAVTalk::updateCRC(quint8 crc, const quint8 data)
UAVTalk::Transaction *UAVTalk::findTransaction(quint32 objId, quint16 instId)
{
return crc_table[crc ^ data];
}
quint8 UAVTalk::updateCRC(quint8 crc, const quint8 *data, qint32 length)
{
while (length--) {
crc = crc_table[crc ^ *data++];
// Lookup the transaction in the transaction map
QMap<quint32, Transaction *> *objTransactions = transMap.value(objId);
if (objTransactions != NULL) {
Transaction *trans = objTransactions->value(instId);
if (trans == NULL) {
// see if there is an ALL_INSTANCES transaction
trans = objTransactions->value(ALL_INSTANCES);
}
return trans;
}
return crc;
return NULL;
}
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->respInstId = instId;
QMap<quint32, Transaction *> *objTransactions = transMap.value(trans->respObjId);
if (objTransactions == NULL) {
objTransactions = new QMap<quint32, Transaction *>();
transMap.insert(trans->respObjId, objTransactions);
}
objTransactions->insert(trans->respInstId, trans);
}
void UAVTalk::closeTransaction(Transaction *trans)
{
QMap<quint32, Transaction *> *objTransactions = transMap.value(trans->respObjId);
if (objTransactions != NULL) {
objTransactions->remove(trans->respInstId);
// Keep the map even if it is empty
// There are at most 100 different object IDs...
delete trans;
}
}
void UAVTalk::closeAllTransactions()
{
foreach(quint32 objId, transMap.keys()) {
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;
}
transMap.remove(objId);
delete objTransactions;
}
}
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;
}
return "<error>";
}

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

@ -27,101 +27,120 @@
#ifndef UAVTALK_H
#define UAVTALK_H
#include "uavobjectmanager.h"
#include "uavtalk_global.h"
#include <QtCore>
#include <QIODevice>
#include <QMutex>
#include <QMutexLocker>
#include <QMap>
#include <QSemaphore>
#include "uavobjectmanager.h"
#include "uavtalk_global.h"
#include <QThread>
#include <QtNetwork/QUdpSocket>
class UAVTALK_EXPORT UAVTalk : public QObject {
Q_OBJECT
friend class IODeviceReader;
public:
static const quint16 ALL_INSTANCES = 0xFFFF;
typedef struct {
quint32 txBytes;
quint32 rxBytes;
quint32 txObjectBytes;
quint32 rxObjectBytes;
quint32 rxObjects;
quint32 txObjects;
quint32 txErrors;
quint32 rxBytes;
quint32 rxObjectBytes;
quint32 rxObjects;
quint32 rxErrors;
quint32 rxSyncErrors;
quint32 rxCrcErrors;
} ComStats;
UAVTalk(QIODevice *iodev, UAVObjectManager *objMngr);
~UAVTalk();
ComStats getStats();
void resetStats();
bool sendObject(UAVObject *obj, bool acked, bool allInstances);
bool sendObjectRequest(UAVObject *obj, bool allInstances);
void cancelTransaction(UAVObject *obj);
ComStats getStats();
void resetStats();
signals:
void transactionCompleted(UAVObject *obj, bool success);
private slots:
void processInputStream(void);
void processInputStream();
void dummyUDPRead();
private:
typedef struct {
UAVObject *obj;
bool allInstances;
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);
static const int MIN_HEADER_LENGTH = 8; // sync(1), type (1), size(2), object ID(4)
static const int MAX_HEADER_LENGTH = 10; // sync(1), type (1), size(2), object ID (4), instance ID(2, not used in single objects)
static const int CHECKSUM_LENGTH = 1;
// header : sync(1), type (1), size(2), object ID(4), instance ID(2)
static const int HEADER_LENGTH = 10;
static const int MAX_PAYLOAD_LENGTH = 256;
static const int MAX_PACKET_LENGTH = (MAX_HEADER_LENGTH + MAX_PAYLOAD_LENGTH + CHECKSUM_LENGTH);
static const int CHECKSUM_LENGTH = 1;
static const quint16 ALL_INSTANCES = 0xFFFF;
static const quint16 OBJID_NOTFOUND = 0x0000;
static const int MAX_PACKET_LENGTH = (HEADER_LENGTH + MAX_PAYLOAD_LENGTH + CHECKSUM_LENGTH);
static const int TX_BUFFER_SIZE = 2 * 1024;
static const quint8 crc_table[256];
// Types
typedef enum { STATE_SYNC, STATE_TYPE, STATE_SIZE, STATE_OBJID, STATE_INSTID, STATE_DATA, STATE_CS } RxStateType;
typedef enum {
STATE_SYNC, STATE_TYPE, STATE_SIZE, STATE_OBJID, STATE_INSTID, STATE_DATA, STATE_CS, STATE_COMPLETE, STATE_ERROR
} RxStateType;
// Variables
QPointer<QIODevice> io;
UAVObjectManager *objMngr;
QMutex *mutex;
QMap<quint32, Transaction *> transMap;
ComStats stats;
QMutex mutex;
QMap<quint32, QMap<quint32, Transaction *> *> transMap;
quint8 rxBuffer[MAX_PACKET_LENGTH];
quint8 txBuffer[MAX_PACKET_LENGTH];
// Variables used by the receive state machine
// state machine variables
qint32 rxCount;
qint32 packetSize;
RxStateType rxState;
// data variables
quint8 rxTmpBuffer[4];
quint8 rxType;
quint32 rxObjId;
quint16 rxInstId;
quint16 rxLength;
quint16 rxPacketLength;
quint8 rxInstanceLength;
quint8 rxCSPacket, rxCS;
qint32 rxCount;
qint32 packetSize;
RxStateType rxState;
ComStats stats;
quint8 rxCSPacket;
quint8 rxCS;
bool useUDPMirror;
QUdpSocket *udpSocketTx;
@ -129,17 +148,21 @@ private:
QByteArray rxDataArray;
// Methods
bool objectTransaction(UAVObject *obj, quint8 type, bool allInstances);
bool objectTransaction(quint8 type, quint32 objId, quint16 instId, UAVObject *obj);
bool processInputByte(quint8 rxbyte);
bool receiveObject(quint8 type, quint32 objId, quint16 instId, quint8 *data, qint32 length);
UAVObject *updateObject(quint32 objId, quint16 instId, quint8 *data);
void updateAck(UAVObject *obj);
void updateNack(UAVObject *obj);
bool transmitNack(quint32 objId);
bool transmitObject(UAVObject *obj, quint8 type, bool allInstances);
bool transmitSingleObject(UAVObject *obj, quint8 type, bool allInstances);
quint8 updateCRC(quint8 crc, const quint8 data);
quint8 updateCRC(quint8 crc, const quint8 *data, qint32 length);
void updateAck(quint8 type, quint32 objId, quint16 instId, UAVObject *obj);
void updateNack(quint32 objId, quint16 instId, UAVObject *obj);
bool transmitObject(quint8 type, quint32 objId, quint16 instId, UAVObject *obj);
bool transmitSingleObject(quint8 type, quint32 objId, quint16 instId, UAVObject *obj);
Transaction *findTransaction(quint32 objId, quint16 instId);
void openTransaction(quint8 type, quint32 objId, quint16 instId);
void closeTransaction(Transaction *trans);
void closeAllTransactions();
const char *typeToString(quint8 type);
};
#endif // UAVTALK_H

13
ground/openpilotgcs/src/plugins/uavtalk/uavtalk.pro
View File

@ -3,22 +3,27 @@ TARGET = UAVTalk
QT += network
DEFINES += UAVTALK_LIBRARY
#DEFINES += VERBOSE_TELEMETRY
#DEFINES += VERBOSE_UAVTALK
include(../../openpilotgcsplugin.pri)
include(uavtalk_dependencies.pri)
HEADERS += uavtalk.h \
HEADERS += \
uavtalk.h \
uavtalkplugin.h \
telemetrymonitor.h \
telemetrymanager.h \
uavtalk_global.h \
telemetry.h
SOURCES += uavtalk.cpp \
SOURCES += \
uavtalk.cpp \
uavtalkplugin.cpp \
telemetrymonitor.cpp \
telemetrymanager.cpp \
telemetry.cpp
DEFINES += UAVTALK_LIBRARY
OTHER_FILES += UAVTalk.pluginspec

10
ground/openpilotgcs/src/plugins/uploader/runningdevicewidget.h
View File

@ -30,17 +30,17 @@
#include "ui_runningdevicewidget.h"
#include <QWidget>
#include <QErrorMessage>
#include <QtSvg/QGraphicsSvgItem>
#include <QtSvg/QSvgRenderer>
#include "uavtalk/telemetrymanager.h"
#include "extensionsystem/pluginmanager.h"
#include "uavobjectmanager.h"
#include "uavobject.h"
#include "uavobjectutilmanager.h"
#include "uploader_global.h"
#include <QWidget>
#include <QErrorMessage>
#include <QtSvg/QGraphicsSvgItem>
#include <QtSvg/QSvgRenderer>
class UPLOADER_EXPORT RunningDeviceWidget : public QWidget {
Q_OBJECT
public:

7
ground/openpilotgcs/src/plugins/uploader/uploadergadgetwidget.cpp
View File

@ -26,10 +26,13 @@
*/
#include "uploadergadgetwidget.h"
#include "version_info/version_info.h"
#include <coreplugin/coreconstants.h>
#include <QDebug>
#include "flightstatus.h"
#include <coreplugin/coreconstants.h>
#include <uavtalk/telemetrymanager.h>
#include <QDebug>
#define DFU_DEBUG true
const int UploaderGadgetWidget::AUTOUPDATE_CLOSE_TIMEOUT = 7000;

1
ground/openpilotgcs/src/plugins/uploader/uploadergadgetwidget.h
View File

@ -36,7 +36,6 @@
#include <QtSerialPort/QSerialPort>
#include <QtSerialPort/QSerialPortInfo>
#include "uavtalk/telemetrymanager.h"
#include "extensionsystem/pluginmanager.h"
#include "uavobjectmanager.h"
#include "uavobject.h"

12
shared/uavobjectdefinition/flighttelemetrystats.xml
View File

@ -1,12 +1,20 @@
<xml>
<object name="FlightTelemetryStats" singleinstance="true" settings="false" category="System">
<description>Maintains the telemetry statistics from the OpenPilot flight computer.</description>
<field name="Status" units="" type="enum" elements="1" options="Disconnected,HandshakeReq,HandshakeAck,Connected"/>
<field name="TxDataRate" units="bytes/sec" type="float" elements="1"/>
<field name="RxDataRate" units="bytes/sec" type="float" elements="1"/>
<field name="TxBytes" units="bytes" type="uint32" elements="1"/>
<field name="TxFailures" units="count" type="uint32" elements="1"/>
<field name="RxFailures" units="count" type="uint32" elements="1"/>
<field name="TxRetries" units="count" type="uint32" elements="1"/>
<field name="RxDataRate" units="bytes/sec" type="float" elements="1"/>
<field name="RxBytes" units="bytes" type="uint32" elements="1"/>
<field name="RxFailures" units="count" type="uint32" elements="1"/>
<field name="RxSyncErrors" units="count" type="uint32" elements="1"/>
<field name="RxCrcErrors" units="count" type="uint32" elements="1"/>
<access gcs="readwrite" flight="readwrite"/>
<telemetrygcs acked="false" updatemode="manual" period="0"/>
<telemetryflight acked="false" updatemode="periodic" period="5000"/>

10
shared/uavobjectdefinition/gcstelemetrystats.xml
View File

@ -1,12 +1,18 @@
<xml>
<object name="GCSTelemetryStats" singleinstance="true" settings="false" category="System">
<description>The telemetry statistics from the ground computer</description>
<field name="Status" units="" type="enum" elements="1" options="Disconnected,HandshakeReq,HandshakeAck,Connected"/>
<field name="TxDataRate" units="bytes/sec" type="float" elements="1"/>
<field name="RxDataRate" units="bytes/sec" type="float" elements="1"/>
<field name="TxBytes" units="bytes" type="uint32" elements="1"/>
<field name="TxFailures" units="count" type="uint32" elements="1"/>
<field name="RxFailures" units="count" type="uint32" elements="1"/>
<field name="TxRetries" units="count" type="uint32" elements="1"/>
<field name="RxDataRate" units="bytes/sec" type="float" elements="1"/>
<field name="RxBytes" units="bytes" type="uint32" elements="1"/>
<field name="RxFailures" units="count" type="uint32" elements="1"/>
<field name="RxSyncErrors" units="count" type="uint32" elements="1"/>
<field name="RxCrcErrors" units="count" type="uint32" elements="1"/>
<access gcs="readwrite" flight="readwrite"/>
<telemetrygcs acked="false" updatemode="periodic" period="5000"/>
<telemetryflight acked="false" updatemode="manual" period="0"/>

2
shared/uavobjectdefinition/oplinkreceiver.xml
View File

@ -1,5 +1,5 @@
<xml>
<object name="OPLinkReceiver" singleinstance="true" settings="false">
<object name="OPLinkReceiver" singleinstance="true" settings="false" category="System">
<description>A receiver channel group carried over the OPLink radio.</description>
<field name="Channel" units="us" type="int16" elements="8"/>
<access gcs="readonly" flight="readwrite"/>

36
shared/uavobjectdefinition/pathaction.xml
View File

@ -1,30 +1,24 @@
<xml>
<object name="PathAction" singleinstance="false" settings="false" category="Navigation">
<description>A waypoint command the pathplanner is to use at a certain waypoint</description>
<field name="Mode" units="" type="enum" elements="1" options="FlyEndpoint,FlyVector,FlyCircleRight,FlyCircleLeft,
DriveEndpoint,DriveVector,DriveCircleLeft,DriveCircleRight,
FixedAttitude,
SetAccessory,
DisarmAlarm" default="Endpoint" />
<field name="ModeParameters" units="" type="float" elements="4" default="0"/>
<field name="EndCondition" units="" type="enum" elements="1" options="None,TimeOut,
DistanceToTarget,LegRemaining,BelowError,
AboveAltitude,AboveSpeed,
PointingTowardsNext,
PythonScript,
Immediate" default="None" />
<field name="ConditionParameters" units="" type="float" elements="4" default="0"/>
<field name="Command" units="" type="enum" elements="1" options="OnConditionNextWaypoint,OnNotConditionNextWaypoint,
OnConditionJumpWaypoint,OnNotConditionJumpWaypoint,
IfConditionJumpWaypointElseNextWaypoint" default="OnConditionNextWaypoint" />
<field name="JumpDestination" units="waypoint" type="int16" elements="1" default="0"/>
<field name="ErrorDestination" units="waypoint" type="int16" elements="1" default="0"/>
<field name="Mode" units="" type="enum" elements="1" options="FlyEndpoint,FlyVector,FlyCircleRight,FlyCircleLeft,
DriveEndpoint,DriveVector,DriveCircleLeft,DriveCircleRight,
FixedAttitude, SetAccessory, DisarmAlarm" default="Endpoint" />
<field name="ModeParameters" units="" type="float" elements="4" default="0"/>
<field name="EndCondition" units="" type="enum" elements="1" options="None,TimeOut,
DistanceToTarget,LegRemaining,BelowError,AboveAltitude,AboveSpeed,PointingTowardsNext,
PythonScript,Immediate" default="None" />
<field name="ConditionParameters" units="" type="float" elements="4" default="0"/>
<field name="Command" units="" type="enum" elements="1" options="OnConditionNextWaypoint,OnNotConditionNextWaypoint,
OnConditionJumpWaypoint,OnNotConditionJumpWaypoint,
IfConditionJumpWaypointElseNextWaypoint" default="OnConditionNextWaypoint" />
<field name="JumpDestination" units="waypoint" type="int16" elements="1" default="0"/>
<field name="ErrorDestination" units="waypoint" type="int16" elements="1" default="0"/>
<access gcs="readwrite" flight="readwrite"/>
<telemetrygcs acked="true" updatemode="manual" period="0"/>
<telemetryflight acked="false" updatemode="periodic" period="4000"/>
<logging updatemode="manual" period="0"/>
<telemetryflight acked="true" updatemode="manual" period="0"/>
<logging updatemode="manual" period="0"/>
</object>
</xml>

14
shared/uavobjectdefinition/pathplan.xml Normal file
View File

@ -0,0 +1,14 @@
<xml>
<object name="PathPlan" singleinstance="true" settings="false" category="Navigation">
<description>Flight plan informations</description>
<field name="WaypointCount" units="" type="uint16" elements="1" default="0" />
<field name="PathActionCount" units="" type="uint16" elements="1" default="0" />
<field name="Crc" units="" type="uint8" elements="1" default="0" />
<access gcs="readwrite" flight="readonly"/>
<telemetrygcs acked="true" updatemode="manual" period="0"/>
<telemetryflight acked="true" updatemode="manual" period="0"/>
<logging updatemode="manual" period="0"/>
</object>
</xml>

26
shared/uavobjectdefinition/radiocombridgestats.xml Normal file
View File

@ -0,0 +1,26 @@
<xml>
<object name="RadioComBridgeStats" singleinstance="true" settings="false" category="System">
<description>Maintains the telemetry statistics from the OPLM RadioComBridge.</description>
<field name="TelemetryTxBytes" units="bytes" type="uint32" elements="1"/>
<field name="TelemetryTxFailures" units="count" type="uint32" elements="1"/>
<field name="TelemetryTxRetries" units="count" type="uint32" elements="1"/>
<field name="TelemetryRxBytes" units="bytes" type="uint32" elements="1"/>
<field name="TelemetryRxFailures" units="count" type="uint32" elements="1"/>
<field name="TelemetryRxSyncErrors" units="count" type="uint32" elements="1"/>
<field name="TelemetryRxCrcErrors" units="count" type="uint32" elements="1"/>
<field name="RadioTxBytes" units="bytes" type="uint32" elements="1"/>
<field name="RadioTxFailures" units="count" type="uint32" elements="1"/>
<field name="RadioTxRetries" units="count" type="uint32" elements="1"/>
<field name="RadioRxBytes" units="bytes" type="uint32" elements="1"/>
<field name="RadioRxFailures" units="count" type="uint32" elements="1"/>
<field name="RadioRxSyncErrors" units="count" type="uint32" elements="1"/>
<field name="RadioRxCrcErrors" units="count" type="uint32" elements="1"/>
<access gcs="readonly" flight="readwrite"/>
<telemetrygcs acked="false" updatemode="manual" period="0"/>
<telemetryflight acked="false" updatemode="periodic" period="5000"/>
<logging updatemode="manual" period="0"/>
</object>
</xml>

1
shared/uavobjectdefinition/systemalarms.xml
View File

@ -16,6 +16,7 @@
<elementname>Sensors</elementname>
<elementname>Stabilization</elementname>
<elementname>Guidance</elementname>
<elementname>PathPlan</elementname>
<elementname>Battery</elementname>
<elementname>FlightTime</elementname>
<elementname>I2C</elementname>

10
shared/uavobjectdefinition/waypoint.xml
View File

@ -1,12 +1,14 @@
<xml>
<object name="Waypoint" singleinstance="false" settings="false" category="Navigation">
<description>A waypoint the aircraft can try and hit. Used by the @ref PathPlanner module</description>
<field name="Position" units="m" type="float" elementnames="North, East, Down"/>
<field name="Velocity" units="m/s" type="float" elements="1"/>
<field name="Action" units="" type="uint8" elements="1" />
<field name="Velocity" units="m/s" type="float" elements="1"/>
<field name="Action" units="" type="uint8" elements="1" />
<access gcs="readwrite" flight="readwrite"/>
<telemetrygcs acked="true" updatemode="manual" period="0"/>
<telemetryflight acked="false" updatemode="periodic" period="4000"/>
<logging updatemode="manual" period="0"/>
<telemetryflight acked="true" updatemode="manual" period="0"/>
<logging updatemode="manual" period="0"/>
</object>
</xml>