rooty/LibrePilot
1
0
Fork 0
mirror of https://bitbucket.org/librepilot/librepilot.git synced 2025-03-22 14:19:42 +01:00
Code Issues Releases Activity

Merge remote-tracking branch 'origin/next' into translations_next

Browse Source
This commit is contained in:
Laurent Lalanne 2014-01-16 19:18:44 +01:00 committed by f5soh
commit 8c4e45a893
124 changed files with 23749 additions and 21926 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

2
.gitignore vendored
View File

@ -10,6 +10,7 @@ GPATH
GRTAGS
GSYMS
GTAGS
core
# flight
/flight/*.pnproj
@ -56,6 +57,7 @@ openpilotgcs-build-desktop
/.cproject
/.project
/.metadata
/.settings
# Ignore Eclipse temp folder, git plugin based?
RemoteSystemsTempFiles

17
flight/libraries/sanitycheck.c
View File

@ -35,6 +35,7 @@
// UAVOs
#include <manualcontrolsettings.h>
#include <systemsettings.h>
#include <systemalarms.h>
#include <taskinfo.h>
/****************************
@ -113,9 +114,9 @@ int32_t configuration_check()
case MANUALCONTROLSETTINGS_FLIGHTMODEPOSITION_ALTITUDEVARIO:
if (coptercontrol) {
severity = SYSTEMALARMS_ALARM_ERROR;
} else if (!PIOS_TASK_MONITOR_IsRunning(TASKINFO_RUNNING_ALTITUDEHOLD)) { // Revo supports altitude hold
severity = SYSTEMALARMS_ALARM_ERROR;
}
// TODO: put check equivalent to TASK_MONITOR_IsRunning
// here as soon as available for delayed callbacks
break;
case MANUALCONTROLSETTINGS_FLIGHTMODEPOSITION_VELOCITYCONTROL:
if (coptercontrol) {
@ -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:

21
flight/modules/Altitude/revolution/altitude.c
View File

@ -40,22 +40,24 @@
#include "altitude.h"
#include "barosensor.h" // object that will be updated by the module
#include "revosettings.h"
#if defined(PIOS_INCLUDE_HCSR04)
#include "sonaraltitude.h" // object that will be updated by the module
#endif
#include "taskinfo.h"
// Private constants
#define STACK_SIZE_BYTES 500
#define STACK_SIZE_BYTES 550
#define TASK_PRIORITY (tskIDLE_PRIORITY + 1)
// Private types
// Private variables
static xTaskHandle taskHandle;
static RevoSettingsBaroTempCorrectionPolynomialData baroCorrection;
// Private functions
static void altitudeTask(void *parameters);
static void SettingsUpdatedCb(UAVObjEvent *ev);
/**
* Initialise the module, called on startup
@ -77,6 +79,9 @@ int32_t AltitudeStart()
int32_t AltitudeInitialize()
{
BaroSensorInitialize();
RevoSettingsInitialize();
RevoSettingsConnectCallback(&SettingsUpdatedCb);
#if defined(PIOS_INCLUDE_HCSR04)
SonarAltitudeInitialize();
#endif
@ -103,6 +108,8 @@ static void altitudeTask(__attribute__((unused)) void *parameters)
// Undef for normal operation
// #define PIOS_MS5611_SLOW_TEMP_RATE 20
RevoSettingsBaroTempCorrectionPolynomialGet(&baroCorrection);
#ifdef PIOS_MS5611_SLOW_TEMP_RATE
uint8_t temp_press_interleave_count = 1;
#endif
@ -154,12 +161,12 @@ static void altitudeTask(__attribute__((unused)) void *parameters)
vTaskDelay(PIOS_MS5611_GetDelay());
PIOS_MS5611_ReadADC();
temp = PIOS_MS5611_GetTemperature();
press = PIOS_MS5611_GetPressure();
float temp2 = temp * temp;
press = press - baroCorrection.a + temp * baroCorrection.b + temp2 * baroCorrection.c + temp * temp2 * baroCorrection.d;
float altitude = 44330.0f * (1.0f - powf(press / MS5611_P0, (1.0f / 5.255f)));
float altitude = 44330.0f * (1.0f - powf((press) / MS5611_P0, (1.0f / 5.255f)));
if (!isnan(altitude)) {
data.Altitude = altitude;
@ -171,6 +178,10 @@ static void altitudeTask(__attribute__((unused)) void *parameters)
}
}
static void SettingsUpdatedCb(__attribute__((unused)) UAVObjEvent *ev)
{
RevoSettingsBaroTempCorrectionPolynomialGet(&baroCorrection);
}
/**
* @}
* @}

417
flight/modules/AltitudeHold/altitudehold.c
View File

@ -46,33 +46,35 @@
#include <openpilot.h>
#include <math.h>
#include <pid.h>
#include <CoordinateConversions.h>
#include <altholdsmoothed.h>
#include <attitudestate.h>
#include <altitudeholdsettings.h>
#include <altitudeholddesired.h> // object that will be updated by the module
#include <barosensor.h>
#include <positionstate.h>
#include <altitudeholdstatus.h>
#include <flightstatus.h>
#include <stabilizationdesired.h>
#include <accelstate.h>
#include <taskinfo.h>
#include <pios_constants.h>
#include <velocitystate.h>
#include <positionstate.h>
// Private constants
#define MAX_QUEUE_SIZE 2
#define STACK_SIZE_BYTES 1024
#define TASK_PRIORITY (tskIDLE_PRIORITY + 1)
#define ACCEL_DOWNSAMPLE 4
#define TIMEOUT_TRESHOLD 200000
#define CALLBACK_PRIORITY CALLBACK_PRIORITY_LOW
#define CBTASK_PRIORITY CALLBACK_TASK_FLIGHTCONTROL
#define STACK_SIZE_BYTES 1024
#define DESIRED_UPDATE_RATE_MS 100 // milliseconds
// Private types
// Private variables
static xTaskHandle altitudeHoldTaskHandle;
static xQueueHandle queue;
static DelayedCallbackInfo *altitudeHoldCBInfo;
static AltitudeHoldSettingsData altitudeHoldSettings;
static struct pid pid0, pid1;
// Private functions
static void altitudeHoldTask(void *parameters);
static void altitudeHoldTask(void);
static void SettingsUpdatedCb(UAVObjEvent *ev);
/**
@ -82,8 +84,8 @@ static void SettingsUpdatedCb(UAVObjEvent *ev);
int32_t AltitudeHoldStart()
{
// Start main task
xTaskCreate(altitudeHoldTask, (signed char *)"AltitudeHold", STACK_SIZE_BYTES / 4, NULL, TASK_PRIORITY, &altitudeHoldTaskHandle);
PIOS_TASK_MONITOR_RegisterTask(TASKINFO_RUNNING_ALTITUDEHOLD, altitudeHoldTaskHandle);
SettingsUpdatedCb(NULL);
DelayedCallbackDispatch(altitudeHoldCBInfo);
return 0;
}
@ -96,319 +98,122 @@ int32_t AltitudeHoldInitialize()
{
AltitudeHoldSettingsInitialize();
AltitudeHoldDesiredInitialize();
AltHoldSmoothedInitialize();
AltitudeHoldStatusInitialize();
// Create object queue
queue = xQueueCreate(MAX_QUEUE_SIZE, sizeof(UAVObjEvent));
altitudeHoldCBInfo = DelayedCallbackCreate(&altitudeHoldTask, CALLBACK_PRIORITY, CBTASK_PRIORITY, STACK_SIZE_BYTES);
AltitudeHoldSettingsConnectCallback(&SettingsUpdatedCb);
return 0;
}
MODULE_INITCALL(AltitudeHoldInitialize, AltitudeHoldStart);
float tau;
float throttleIntegral;
float velocity;
float decay;
float velocity_decay;
bool running = false;
float error;
float switchThrottle;
float smoothed_altitude;
float starting_altitude;
/**
* Module thread, should not return.
*/
static void altitudeHoldTask(__attribute__((unused)) void *parameters)
static void altitudeHoldTask(void)
{
AltitudeHoldDesiredData altitudeHoldDesired;
StabilizationDesiredData stabilizationDesired;
static float startThrottle = 0.5f;
portTickType thisTime, lastUpdateTime;
UAVObjEvent ev;
// make sure we run only when we are supposed to run
FlightStatusData flightStatus;
// Force update of the settings
SettingsUpdatedCb(&ev);
// Failsafe handling
uint32_t lastAltitudeHoldDesiredUpdate = 0;
bool enterFailSafe = false;
// Listen for updates.
AltitudeHoldDesiredConnectQueue(queue);
BaroSensorConnectQueue(queue);
FlightStatusConnectQueue(queue);
AccelStateConnectQueue(queue);
bool altitudeHoldFlightMode = false;
BaroSensorAltitudeGet(&smoothed_altitude);
running = false;
enum init_state { WAITING_BARO, WAITIING_INIT, INITED } init = WAITING_BARO;
FlightStatusGet(&flightStatus);
switch (flightStatus.FlightMode) {
case FLIGHTSTATUS_FLIGHTMODE_ALTITUDEHOLD:
case FLIGHTSTATUS_FLIGHTMODE_ALTITUDEVARIO:
break;
default:
pid_zero(&pid0);
pid_zero(&pid1);
StabilizationDesiredThrottleGet(&startThrottle);
DelayedCallbackSchedule(altitudeHoldCBInfo, DESIRED_UPDATE_RATE_MS, CALLBACK_UPDATEMODE_SOONER);
return;
uint8_t flightMode;
FlightStatusFlightModeGet(&flightMode);
// initialize enable flag
altitudeHoldFlightMode = flightMode == FLIGHTSTATUS_FLIGHTMODE_ALTITUDEHOLD || flightMode == FLIGHTSTATUS_FLIGHTMODE_ALTITUDEVARIO;
// Main task loop
bool baro_updated = false;
while (1) {
enterFailSafe = PIOS_DELAY_DiffuS(lastAltitudeHoldDesiredUpdate) > TIMEOUT_TRESHOLD;
// Wait until the AttitudeRaw object is updated, if a timeout then go to failsafe
if (xQueueReceive(queue, &ev, 100 / portTICK_RATE_MS) != pdTRUE) {
if (!running) {
throttleIntegral = 0;
}
// Todo: Add alarm if it should be running
continue;
} else if (ev.obj == BaroSensorHandle()) {
baro_updated = true;
init = (init == WAITING_BARO) ? WAITIING_INIT : init;
} else if (ev.obj == FlightStatusHandle()) {
FlightStatusFlightModeGet(&flightMode);
altitudeHoldFlightMode = flightMode == FLIGHTSTATUS_FLIGHTMODE_ALTITUDEHOLD || flightMode == FLIGHTSTATUS_FLIGHTMODE_ALTITUDEVARIO;
if (altitudeHoldFlightMode && !running) {
// Copy the current throttle as a starting point for integral
StabilizationDesiredThrottleGet(&throttleIntegral);
switchThrottle = throttleIntegral;
error = 0;
velocity = 0;
running = true;
AltHoldSmoothedData altHold;
AltHoldSmoothedGet(&altHold);
starting_altitude = altHold.Altitude;
} else if (!altitudeHoldFlightMode) {
running = false;
lastAltitudeHoldDesiredUpdate = PIOS_DELAY_GetRaw();
}
} else if (ev.obj == AccelStateHandle()) {
static uint32_t timeval;
static float z[4] = { 0, 0, 0, 0 };
float z_new[4];
float P[4][4], K[4][2], x[2];
float G[4] = { 1.0e-15f, 1.0e-15f, 1.0e-3f, 1.0e-7f };
static float V[4][4] = {
{ 10.0f, 0.0f, 0.0f, 0.0f },
{ 0.0f, 100.0f, 0.0f, 0.0f },
{ 0.0f, 0.0f, 100.0f, 0.0f },
{ 0.0f, 0.0f, 0.0f, 1000.0f }
};
static uint32_t accel_downsample_count = 0;
static float accels_accum[3] = { 0.0f, 0.0f, 0.0f };
float dT;
static float S[2] = { 1.0f, 10.0f };
/* Somehow this always assigns to zero. Compiler bug? Race condition? */
S[0] = altitudeHoldSettings.PressureNoise;
S[1] = altitudeHoldSettings.AccelNoise;
G[2] = altitudeHoldSettings.AccelDrift;
AccelStateData accelState;
AccelStateGet(&accelState);
AttitudeStateData attitudeState;
AttitudeStateGet(&attitudeState);
BaroSensorData baro;
BaroSensorGet(&baro);
/* Downsample accels to stop this calculation consuming too much CPU */
accels_accum[0] += accelState.x;
accels_accum[1] += accelState.y;
accels_accum[2] += accelState.z;
accel_downsample_count++;
if (accel_downsample_count < ACCEL_DOWNSAMPLE) {
continue;
}
accel_downsample_count = 0;
accelState.x = accels_accum[0] / ACCEL_DOWNSAMPLE;
accelState.y = accels_accum[1] / ACCEL_DOWNSAMPLE;
accelState.z = accels_accum[2] / ACCEL_DOWNSAMPLE;
accels_accum[0] = accels_accum[1] = accels_accum[2] = 0;
thisTime = xTaskGetTickCount();
if (init == WAITIING_INIT) {
z[0] = baro.Altitude;
z[1] = 0;
z[2] = accelState.z;
z[3] = 0;
init = INITED;
} else if (init == WAITING_BARO) {
continue;
}
x[0] = baro.Altitude;
// rotate avg accels into earth frame and store it
if (1) {
float q[4], Rbe[3][3];
q[0] = attitudeState.q1;
q[1] = attitudeState.q2;
q[2] = attitudeState.q3;
q[3] = attitudeState.q4;
Quaternion2R(q, Rbe);
x[1] = -(Rbe[0][2] * accelState.x + Rbe[1][2] * accelState.y + Rbe[2][2] * accelState.z + 9.81f);
} else {
x[1] = -accelState.z + 9.81f;
}
dT = PIOS_DELAY_DiffuS(timeval) / 1.0e6f;
timeval = PIOS_DELAY_GetRaw();
P[0][0] = dT * (V[0][1] + dT * V[1][1]) + V[0][0] + G[0] + dT * V[1][0];
P[0][1] = dT * (V[0][2] + dT * V[1][2]) + V[0][1] + dT * V[1][1];
P[0][2] = V[0][2] + dT * V[1][2];
P[0][3] = V[0][3] + dT * V[1][3];
P[1][0] = dT * (V[1][1] + dT * V[2][1]) + V[1][0] + dT * V[2][0];
P[1][1] = dT * (V[1][2] + dT * V[2][2]) + V[1][1] + G[1] + dT * V[2][1];
P[1][2] = V[1][2] + dT * V[2][2];
P[1][3] = V[1][3] + dT * V[2][3];
P[2][0] = V[2][0] + dT * V[2][1];
P[2][1] = V[2][1] + dT * V[2][2];
P[2][2] = V[2][2] + G[2];
P[2][3] = V[2][3];
P[3][0] = V[3][0] + dT * V[3][1];
P[3][1] = V[3][1] + dT * V[3][2];
P[3][2] = V[3][2];
P[3][3] = V[3][3] + G[3];
if (baro_updated) {
K[0][0] = -(V[2][2] * S[0] + V[2][3] * S[0] + V[3][2] * S[0] + V[3][3] * S[0] + G[2] * S[0] + G[3] * S[0] + S[0] * S[1]) / (V[0][0] * G[2] + V[0][0] * G[3] + V[2][2] * G[0] + V[2][3] * G[0] + V[3][2] * G[0] + V[3][3] * G[0] + V[0][0] * S[1] + V[2][2] * S[0] + V[2][3] * S[0] + V[3][2] * S[0] + V[3][3] * S[0] + V[0][0] * V[2][2] - V[0][2] * V[2][0] + V[0][0] * V[2][3] + V[0][0] * V[3][2] - V[0][2] * V[3][0] - V[2][0] * V[0][3] + V[0][0] * V[3][3] - V[0][3] * V[3][0] + G[0] * G[2] + G[0] * G[3] + G[0] * S[1] + G[2] * S[0] + G[3] * S[0] + S[0] * S[1] + (dT * dT) * V[1][1] * V[2][2] - (dT * dT) * V[1][2] * V[2][1] + (dT * dT) * V[1][1] * V[2][3] + (dT * dT) * V[1][1] * V[3][2] - (dT * dT) * V[1][2] * V[3][1] - (dT * dT) * V[2][1] * V[1][3] + (dT * dT) * V[1][1] * V[3][3] - (dT * dT) * V[1][3] * V[3][1] + dT * V[0][1] * G[2] + dT * V[1][0] * G[2] + dT * V[0][1] * G[3] + dT * V[1][0] * G[3] + dT * V[0][1] * S[1] + dT * V[1][0] * S[1] + (dT * dT) * V[1][1] * G[2] + (dT * dT) * V[1][1] * G[3] + (dT * dT) * V[1][1] * S[1] + dT * V[0][1] * V[2][2] + dT * V[1][0] * V[2][2] - dT * V[0][2] * V[2][1] - dT * V[2][0] * V[1][2] + dT * V[0][1] * V[2][3] + dT * V[0][1] * V[3][2] + dT * V[1][0] * V[2][3] + dT * V[1][0] * V[3][2] - dT * V[0][2] * V[3][1] - dT * V[2][0] * V[1][3] - dT * V[0][3] * V[2][1] - dT * V[1][2] * V[3][0] + dT * V[0][1] * V[3][3] + dT * V[1][0] * V[3][3] - dT * V[0][3] * V[3][1] - dT * V[3][0] * V[1][3]) + 1.0f;
K[0][1] = ((V[0][2] + V[0][3]) * S[0] + dT * (V[1][2] + V[1][3]) * S[0]) / (V[0][0] * G[2] + V[0][0] * G[3] + V[2][2] * G[0] + V[2][3] * G[0] + V[3][2] * G[0] + V[3][3] * G[0] + V[0][0] * S[1] + V[2][2] * S[0] + V[2][3] * S[0] + V[3][2] * S[0] + V[3][3] * S[0] + V[0][0] * V[2][2] - V[0][2] * V[2][0] + V[0][0] * V[2][3] + V[0][0] * V[3][2] - V[0][2] * V[3][0] - V[2][0] * V[0][3] + V[0][0] * V[3][3] - V[0][3] * V[3][0] + G[0] * G[2] + G[0] * G[3] + G[0] * S[1] + G[2] * S[0] + G[3] * S[0] + S[0] * S[1] + (dT * dT) * V[1][1] * V[2][2] - (dT * dT) * V[1][2] * V[2][1] + (dT * dT) * V[1][1] * V[2][3] + (dT * dT) * V[1][1] * V[3][2] - (dT * dT) * V[1][2] * V[3][1] - (dT * dT) * V[2][1] * V[1][3] + (dT * dT) * V[1][1] * V[3][3] - (dT * dT) * V[1][3] * V[3][1] + dT * V[0][1] * G[2] + dT * V[1][0] * G[2] + dT * V[0][1] * G[3] + dT * V[1][0] * G[3] + dT * V[0][1] * S[1] + dT * V[1][0] * S[1] + (dT * dT) * V[1][1] * G[2] + (dT * dT) * V[1][1] * G[3] + (dT * dT) * V[1][1] * S[1] + dT * V[0][1] * V[2][2] + dT * V[1][0] * V[2][2] - dT * V[0][2] * V[2][1] - dT * V[2][0] * V[1][2] + dT * V[0][1] * V[2][3] + dT * V[0][1] * V[3][2] + dT * V[1][0] * V[2][3] + dT * V[1][0] * V[3][2] - dT * V[0][2] * V[3][1] - dT * V[2][0] * V[1][3] - dT * V[0][3] * V[2][1] - dT * V[1][2] * V[3][0] + dT * V[0][1] * V[3][3] + dT * V[1][0] * V[3][3] - dT * V[0][3] * V[3][1] - dT * V[3][0] * V[1][3]);
K[1][0] = (V[1][0] * G[2] + V[1][0] * G[3] + V[1][0] * S[1] + V[1][0] * V[2][2] - V[2][0] * V[1][2] + V[1][0] * V[2][3] + V[1][0] * V[3][2] - V[2][0] * V[1][3] - V[1][2] * V[3][0] + V[1][0] * V[3][3] - V[3][0] * V[1][3] + (dT * dT) * V[2][1] * V[3][2] - (dT * dT) * V[2][2] * V[3][1] + (dT * dT) * V[2][1] * V[3][3] - (dT * dT) * V[3][1] * V[2][3] + dT * V[1][1] * G[2] + dT * V[2][0] * G[2] + dT * V[1][1] * G[3] + dT * V[2][0] * G[3] + dT * V[1][1] * S[1] + dT * V[2][0] * S[1] + (dT * dT) * V[2][1] * G[2] + (dT * dT) * V[2][1] * G[3] + (dT * dT) * V[2][1] * S[1] + dT * V[1][1] * V[2][2] - dT * V[1][2] * V[2][1] + dT * V[1][1] * V[2][3] + dT * V[1][1] * V[3][2] + dT * V[2][0] * V[3][2] - dT * V[1][2] * V[3][1] - dT * V[2][1] * V[1][3] - dT * V[3][0] * V[2][2] + dT * V[1][1] * V[3][3] + dT * V[2][0] * V[3][3] - dT * V[3][0] * V[2][3] - dT * V[1][3] * V[3][1]) / (V[0][0] * G[2] + V[0][0] * G[3] + V[2][2] * G[0] + V[2][3] * G[0] + V[3][2] * G[0] + V[3][3] * G[0] + V[0][0] * S[1] + V[2][2] * S[0] + V[2][3] * S[0] + V[3][2] * S[0] + V[3][3] * S[0] + V[0][0] * V[2][2] - V[0][2] * V[2][0] + V[0][0] * V[2][3] + V[0][0] * V[3][2] - V[0][2] * V[3][0] - V[2][0] * V[0][3] + V[0][0] * V[3][3] - V[0][3] * V[3][0] + G[0] * G[2] + G[0] * G[3] + G[0] * S[1] + G[2] * S[0] + G[3] * S[0] + S[0] * S[1] + (dT * dT) * V[1][1] * V[2][2] - (dT * dT) * V[1][2] * V[2][1] + (dT * dT) * V[1][1] * V[2][3] + (dT * dT) * V[1][1] * V[3][2] - (dT * dT) * V[1][2] * V[3][1] - (dT * dT) * V[2][1] * V[1][3] + (dT * dT) * V[1][1] * V[3][3] - (dT * dT) * V[1][3] * V[3][1] + dT * V[0][1] * G[2] + dT * V[1][0] * G[2] + dT * V[0][1] * G[3] + dT * V[1][0] * G[3] + dT * V[0][1] * S[1] + dT * V[1][0] * S[1] + (dT * dT) * V[1][1] * G[2] + (dT * dT) * V[1][1] * G[3] + (dT * dT) * V[1][1] * S[1] + dT * V[0][1] * V[2][2] + dT * V[1][0] * V[2][2] - dT * V[0][2] * V[2][1] - dT * V[2][0] * V[1][2] + dT * V[0][1] * V[2][3] + dT * V[0][1] * V[3][2] + dT * V[1][0] * V[2][3] + dT * V[1][0] * V[3][2] - dT * V[0][2] * V[3][1] - dT * V[2][0] * V[1][3] - dT * V[0][3] * V[2][1] - dT * V[1][2] * V[3][0] + dT * V[0][1] * V[3][3] + dT * V[1][0] * V[3][3] - dT * V[0][3] * V[3][1] - dT * V[3][0] * V[1][3]);
K[1][1] = (V[1][2] * G[0] + V[1][3] * G[0] + V[1][2] * S[0] + V[1][3] * S[0] + V[0][0] * V[1][2] - V[1][0] * V[0][2] + V[0][0] * V[1][3] - V[1][0] * V[0][3] + (dT * dT) * V[0][1] * V[2][2] + (dT * dT) * V[1][0] * V[2][2] - (dT * dT) * V[0][2] * V[2][1] - (dT * dT) * V[2][0] * V[1][2] + (dT * dT) * V[0][1] * V[2][3] + (dT * dT) * V[1][0] * V[2][3] - (dT * dT) * V[2][0] * V[1][3] - (dT * dT) * V[0][3] * V[2][1] + (dT * dT * dT) * V[1][1] * V[2][2] - (dT * dT * dT) * V[1][2] * V[2][1] + (dT * dT * dT) * V[1][1] * V[2][3] - (dT * dT * dT) * V[2][1] * V[1][3] + dT * V[2][2] * G[0] + dT * V[2][3] * G[0] + dT * V[2][2] * S[0] + dT * V[2][3] * S[0] + dT * V[0][0] * V[2][2] + dT * V[0][1] * V[1][2] - dT * V[0][2] * V[1][1] - dT * V[0][2] * V[2][0] + dT * V[0][0] * V[2][3] + dT * V[0][1] * V[1][3] - dT * V[1][1] * V[0][3] - dT * V[2][0] * V[0][3]) / (V[0][0] * G[2] + V[0][0] * G[3] + V[2][2] * G[0] + V[2][3] * G[0] + V[3][2] * G[0] + V[3][3] * G[0] + V[0][0] * S[1] + V[2][2] * S[0] + V[2][3] * S[0] + V[3][2] * S[0] + V[3][3] * S[0] + V[0][0] * V[2][2] - V[0][2] * V[2][0] + V[0][0] * V[2][3] + V[0][0] * V[3][2] - V[0][2] * V[3][0] - V[2][0] * V[0][3] + V[0][0] * V[3][3] - V[0][3] * V[3][0] + G[0] * G[2] + G[0] * G[3] + G[0] * S[1] + G[2] * S[0] + G[3] * S[0] + S[0] * S[1] + (dT * dT) * V[1][1] * V[2][2] - (dT * dT) * V[1][2] * V[2][1] + (dT * dT) * V[1][1] * V[2][3] + (dT * dT) * V[1][1] * V[3][2] - (dT * dT) * V[1][2] * V[3][1] - (dT * dT) * V[2][1] * V[1][3] + (dT * dT) * V[1][1] * V[3][3] - (dT * dT) * V[1][3] * V[3][1] + dT * V[0][1] * G[2] + dT * V[1][0] * G[2] + dT * V[0][1] * G[3] + dT * V[1][0] * G[3] + dT * V[0][1] * S[1] + dT * V[1][0] * S[1] + (dT * dT) * V[1][1] * G[2] + (dT * dT) * V[1][1] * G[3] + (dT * dT) * V[1][1] * S[1] + dT * V[0][1] * V[2][2] + dT * V[1][0] * V[2][2] - dT * V[0][2] * V[2][1] - dT * V[2][0] * V[1][2] + dT * V[0][1] * V[2][3] + dT * V[0][1] * V[3][2] + dT * V[1][0] * V[2][3] + dT * V[1][0] * V[3][2] - dT * V[0][2] * V[3][1] - dT * V[2][0] * V[1][3] - dT * V[0][3] * V[2][1] - dT * V[1][2] * V[3][0] + dT * V[0][1] * V[3][3] + dT * V[1][0] * V[3][3] - dT * V[0][3] * V[3][1] - dT * V[3][0] * V[1][3]);
K[2][0] = (V[2][0] * G[3] - V[3][0] * G[2] + V[2][0] * S[1] + V[2][0] * V[3][2] - V[3][0] * V[2][2] + V[2][0] * V[3][3] - V[3][0] * V[2][3] + dT * V[2][1] * G[3] - dT * V[3][1] * G[2] + dT * V[2][1] * S[1] + dT * V[2][1] * V[3][2] - dT * V[2][2] * V[3][1] + dT * V[2][1] * V[3][3] - dT * V[3][1] * V[2][3]) / (V[0][0] * G[2] + V[0][0] * G[3] + V[2][2] * G[0] + V[2][3] * G[0] + V[3][2] * G[0] + V[3][3] * G[0] + V[0][0] * S[1] + V[2][2] * S[0] + V[2][3] * S[0] + V[3][2] * S[0] + V[3][3] * S[0] + V[0][0] * V[2][2] - V[0][2] * V[2][0] + V[0][0] * V[2][3] + V[0][0] * V[3][2] - V[0][2] * V[3][0] - V[2][0] * V[0][3] + V[0][0] * V[3][3] - V[0][3] * V[3][0] + G[0] * G[2] + G[0] * G[3] + G[0] * S[1] + G[2] * S[0] + G[3] * S[0] + S[0] * S[1] + (dT * dT) * V[1][1] * V[2][2] - (dT * dT) * V[1][2] * V[2][1] + (dT * dT) * V[1][1] * V[2][3] + (dT * dT) * V[1][1] * V[3][2] - (dT * dT) * V[1][2] * V[3][1] - (dT * dT) * V[2][1] * V[1][3] + (dT * dT) * V[1][1] * V[3][3] - (dT * dT) * V[1][3] * V[3][1] + dT * V[0][1] * G[2] + dT * V[1][0] * G[2] + dT * V[0][1] * G[3] + dT * V[1][0] * G[3] + dT * V[0][1] * S[1] + dT * V[1][0] * S[1] + (dT * dT) * V[1][1] * G[2] + (dT * dT) * V[1][1] * G[3] + (dT * dT) * V[1][1] * S[1] + dT * V[0][1] * V[2][2] + dT * V[1][0] * V[2][2] - dT * V[0][2] * V[2][1] - dT * V[2][0] * V[1][2] + dT * V[0][1] * V[2][3] + dT * V[0][1] * V[3][2] + dT * V[1][0] * V[2][3] + dT * V[1][0] * V[3][2] - dT * V[0][2] * V[3][1] - dT * V[2][0] * V[1][3] - dT * V[0][3] * V[2][1] - dT * V[1][2] * V[3][0] + dT * V[0][1] * V[3][3] + dT * V[1][0] * V[3][3] - dT * V[0][3] * V[3][1] - dT * V[3][0] * V[1][3]);
K[2][1] = (V[0][0] * G[2] + V[2][2] * G[0] + V[2][3] * G[0] + V[2][2] * S[0] + V[2][3] * S[0] + V[0][0] * V[2][2] - V[0][2] * V[2][0] + V[0][0] * V[2][3] - V[2][0] * V[0][3] + G[0] * G[2] + G[2] * S[0] + (dT * dT) * V[1][1] * V[2][2] - (dT * dT) * V[1][2] * V[2][1] + (dT * dT) * V[1][1] * V[2][3] - (dT * dT) * V[2][1] * V[1][3] + dT * V[0][1] * G[2] + dT * V[1][0] * G[2] + (dT * dT) * V[1][1] * G[2] + dT * V[0][1] * V[2][2] + dT * V[1][0] * V[2][2] - dT * V[0][2] * V[2][1] - dT * V[2][0] * V[1][2] + dT * V[0][1] * V[2][3] + dT * V[1][0] * V[2][3] - dT * V[2][0] * V[1][3] - dT * V[0][3] * V[2][1]) / (V[0][0] * G[2] + V[0][0] * G[3] + V[2][2] * G[0] + V[2][3] * G[0] + V[3][2] * G[0] + V[3][3] * G[0] + V[0][0] * S[1] + V[2][2] * S[0] + V[2][3] * S[0] + V[3][2] * S[0] + V[3][3] * S[0] + V[0][0] * V[2][2] - V[0][2] * V[2][0] + V[0][0] * V[2][3] + V[0][0] * V[3][2] - V[0][2] * V[3][0] - V[2][0] * V[0][3] + V[0][0] * V[3][3] - V[0][3] * V[3][0] + G[0] * G[2] + G[0] * G[3] + G[0] * S[1] + G[2] * S[0] + G[3] * S[0] + S[0] * S[1] + (dT * dT) * V[1][1] * V[2][2] - (dT * dT) * V[1][2] * V[2][1] + (dT * dT) * V[1][1] * V[2][3] + (dT * dT) * V[1][1] * V[3][2] - (dT * dT) * V[1][2] * V[3][1] - (dT * dT) * V[2][1] * V[1][3] + (dT * dT) * V[1][1] * V[3][3] - (dT * dT) * V[1][3] * V[3][1] + dT * V[0][1] * G[2] + dT * V[1][0] * G[2] + dT * V[0][1] * G[3] + dT * V[1][0] * G[3] + dT * V[0][1] * S[1] + dT * V[1][0] * S[1] + (dT * dT) * V[1][1] * G[2] + (dT * dT) * V[1][1] * G[3] + (dT * dT) * V[1][1] * S[1] + dT * V[0][1] * V[2][2] + dT * V[1][0] * V[2][2] - dT * V[0][2] * V[2][1] - dT * V[2][0] * V[1][2] + dT * V[0][1] * V[2][3] + dT * V[0][1] * V[3][2] + dT * V[1][0] * V[2][3] + dT * V[1][0] * V[3][2] - dT * V[0][2] * V[3][1] - dT * V[2][0] * V[1][3] - dT * V[0][3] * V[2][1] - dT * V[1][2] * V[3][0] + dT * V[0][1] * V[3][3] + dT * V[1][0] * V[3][3] - dT * V[0][3] * V[3][1] - dT * V[3][0] * V[1][3]);
K[3][0] = (-V[2][0] * G[3] + V[3][0] * G[2] + V[3][0] * S[1] - V[2][0] * V[3][2] + V[3][0] * V[2][2] - V[2][0] * V[3][3] + V[3][0] * V[2][3] - dT * V[2][1] * G[3] + dT * V[3][1] * G[2] + dT * V[3][1] * S[1] - dT * V[2][1] * V[3][2] + dT * V[2][2] * V[3][1] - dT * V[2][1] * V[3][3] + dT * V[3][1] * V[2][3]) / (V[0][0] * G[2] + V[0][0] * G[3] + V[2][2] * G[0] + V[2][3] * G[0] + V[3][2] * G[0] + V[3][3] * G[0] + V[0][0] * S[1] + V[2][2] * S[0] + V[2][3] * S[0] + V[3][2] * S[0] + V[3][3] * S[0] + V[0][0] * V[2][2] - V[0][2] * V[2][0] + V[0][0] * V[2][3] + V[0][0] * V[3][2] - V[0][2] * V[3][0] - V[2][0] * V[0][3] + V[0][0] * V[3][3] - V[0][3] * V[3][0] + G[0] * G[2] + G[0] * G[3] + G[0] * S[1] + G[2] * S[0] + G[3] * S[0] + S[0] * S[1] + (dT * dT) * V[1][1] * V[2][2] - (dT * dT) * V[1][2] * V[2][1] + (dT * dT) * V[1][1] * V[2][3] + (dT * dT) * V[1][1] * V[3][2] - (dT * dT) * V[1][2] * V[3][1] - (dT * dT) * V[2][1] * V[1][3] + (dT * dT) * V[1][1] * V[3][3] - (dT * dT) * V[1][3] * V[3][1] + dT * V[0][1] * G[2] + dT * V[1][0] * G[2] + dT * V[0][1] * G[3] + dT * V[1][0] * G[3] + dT * V[0][1] * S[1] + dT * V[1][0] * S[1] + (dT * dT) * V[1][1] * G[2] + (dT * dT) * V[1][1] * G[3] + (dT * dT) * V[1][1] * S[1] + dT * V[0][1] * V[2][2] + dT * V[1][0] * V[2][2] - dT * V[0][2] * V[2][1] - dT * V[2][0] * V[1][2] + dT * V[0][1] * V[2][3] + dT * V[0][1] * V[3][2] + dT * V[1][0] * V[2][3] + dT * V[1][0] * V[3][2] - dT * V[0][2] * V[3][1] - dT * V[2][0] * V[1][3] - dT * V[0][3] * V[2][1] - dT * V[1][2] * V[3][0] + dT * V[0][1] * V[3][3] + dT * V[1][0] * V[3][3] - dT * V[0][3] * V[3][1] - dT * V[3][0] * V[1][3]);
K[3][1] = (V[0][0] * G[3] + V[3][2] * G[0] + V[3][3] * G[0] + V[3][2] * S[0] + V[3][3] * S[0] + V[0][0] * V[3][2] - V[0][2] * V[3][0] + V[0][0] * V[3][3] - V[0][3] * V[3][0] + G[0] * G[3] + G[3] * S[0] + (dT * dT) * V[1][1] * V[3][2] - (dT * dT) * V[1][2] * V[3][1] + (dT * dT) * V[1][1] * V[3][3] - (dT * dT) * V[1][3] * V[3][1] + dT * V[0][1] * G[3] + dT * V[1][0] * G[3] + (dT * dT) * V[1][1] * G[3] + dT * V[0][1] * V[3][2] + dT * V[1][0] * V[3][2] - dT * V[0][2] * V[3][1] - dT * V[1][2] * V[3][0] + dT * V[0][1] * V[3][3] + dT * V[1][0] * V[3][3] - dT * V[0][3] * V[3][1] - dT * V[3][0] * V[1][3]) / (V[0][0] * G[2] + V[0][0] * G[3] + V[2][2] * G[0] + V[2][3] * G[0] + V[3][2] * G[0] + V[3][3] * G[0] + V[0][0] * S[1] + V[2][2] * S[0] + V[2][3] * S[0] + V[3][2] * S[0] + V[3][3] * S[0] + V[0][0] * V[2][2] - V[0][2] * V[2][0] + V[0][0] * V[2][3] + V[0][0] * V[3][2] - V[0][2] * V[3][0] - V[2][0] * V[0][3] + V[0][0] * V[3][3] - V[0][3] * V[3][0] + G[0] * G[2] + G[0] * G[3] + G[0] * S[1] + G[2] * S[0] + G[3] * S[0] + S[0] * S[1] + (dT * dT) * V[1][1] * V[2][2] - (dT * dT) * V[1][2] * V[2][1] + (dT * dT) * V[1][1] * V[2][3] + (dT * dT) * V[1][1] * V[3][2] - (dT * dT) * V[1][2] * V[3][1] - (dT * dT) * V[2][1] * V[1][3] + (dT * dT) * V[1][1] * V[3][3] - (dT * dT) * V[1][3] * V[3][1] + dT * V[0][1] * G[2] + dT * V[1][0] * G[2] + dT * V[0][1] * G[3] + dT * V[1][0] * G[3] + dT * V[0][1] * S[1] + dT * V[1][0] * S[1] + (dT * dT) * V[1][1] * G[2] + (dT * dT) * V[1][1] * G[3] + (dT * dT) * V[1][1] * S[1] + dT * V[0][1] * V[2][2] + dT * V[1][0] * V[2][2] - dT * V[0][2] * V[2][1] - dT * V[2][0] * V[1][2] + dT * V[0][1] * V[2][3] + dT * V[0][1] * V[3][2] + dT * V[1][0] * V[2][3] + dT * V[1][0] * V[3][2] - dT * V[0][2] * V[3][1] - dT * V[2][0] * V[1][3] - dT * V[0][3] * V[2][1] - dT * V[1][2] * V[3][0] + dT * V[0][1] * V[3][3] + dT * V[1][0] * V[3][3] - dT * V[0][3] * V[3][1] - dT * V[3][0] * V[1][3]);
z_new[0] = -K[0][0] * (dT * z[1] - x[0] + z[0]) + dT * z[1] - K[0][1] * (-x[1] + z[2] + z[3]) + z[0];
z_new[1] = -K[1][0] * (dT * z[1] - x[0] + z[0]) + dT * z[2] - K[1][1] * (-x[1] + z[2] + z[3]) + z[1];
z_new[2] = -K[2][0] * (dT * z[1] - x[0] + z[0]) - K[2][1] * (-x[1] + z[2] + z[3]) + z[2];
z_new[3] = -K[3][0] * (dT * z[1] - x[0] + z[0]) - K[3][1] * (-x[1] + z[2] + z[3]) + z[3];
memcpy(z, z_new, sizeof(z_new));
V[0][0] = -K[0][1] * P[2][0] - K[0][1] * P[3][0] - P[0][0] * (K[0][0] - 1.0f);
V[0][1] = -K[0][1] * P[2][1] - K[0][1] * P[3][2] - P[0][1] * (K[0][0] - 1.0f);
V[0][2] = -K[0][1] * P[2][2] - K[0][1] * P[3][2] - P[0][2] * (K[0][0] - 1.0f);
V[0][3] = -K[0][1] * P[2][3] - K[0][1] * P[3][3] - P[0][3] * (K[0][0] - 1.0f);
V[1][0] = P[1][0] - K[1][0] * P[0][0] - K[1][1] * P[2][0] - K[1][1] * P[3][0];
V[1][1] = P[1][1] - K[1][0] * P[0][1] - K[1][1] * P[2][1] - K[1][1] * P[3][2];
V[1][2] = P[1][2] - K[1][0] * P[0][2] - K[1][1] * P[2][2] - K[1][1] * P[3][2];
V[1][3] = P[1][3] - K[1][0] * P[0][3] - K[1][1] * P[2][3] - K[1][1] * P[3][3];
V[2][0] = -K[2][0] * P[0][0] - K[2][1] * P[3][0] - P[2][0] * (K[2][1] - 1.0f);
V[2][1] = -K[2][0] * P[0][1] - K[2][1] * P[3][2] - P[2][1] * (K[2][1] - 1.0f);
V[2][2] = -K[2][0] * P[0][2] - K[2][1] * P[3][2] - P[2][2] * (K[2][1] - 1.0f);
V[2][3] = -K[2][0] * P[0][3] - K[2][1] * P[3][3] - P[2][3] * (K[2][1] - 1.0f);
V[3][0] = -K[3][0] * P[0][0] - K[3][1] * P[2][0] - P[3][0] * (K[3][1] - 1.0f);
V[3][1] = -K[3][0] * P[0][1] - K[3][1] * P[2][1] - P[3][2] * (K[3][1] - 1.0f);
V[3][2] = -K[3][0] * P[0][2] - K[3][1] * P[2][2] - P[3][2] * (K[3][1] - 1.0f);
V[3][3] = -K[3][0] * P[0][3] - K[3][1] * P[2][3] - P[3][3] * (K[3][1] - 1.0f);
baro_updated = false;
} else {
K[0][0] = (V[0][2] + V[0][3] + dT * V[1][2] + dT * V[1][3]) / (V[2][2] + V[2][3] + V[3][2] + V[3][3] + G[2] + G[3] + S[1]);
K[1][0] = (V[1][2] + V[1][3] + dT * V[2][2] + dT * V[2][3]) / (V[2][2] + V[2][3] + V[3][2] + V[3][3] + G[2] + G[3] + S[1]);
K[2][0] = (V[2][2] + V[2][3] + G[2]) / (V[2][2] + V[2][3] + V[3][2] + V[3][3] + G[2] + G[3] + S[1]);
K[3][0] = (V[3][2] + V[3][3] + G[3]) / (V[2][2] + V[2][3] + V[3][2] + V[3][3] + G[2] + G[3] + S[1]);
z_new[0] = dT * z[1] - K[0][0] * (-x[1] + z[2] + z[3]) + z[0];
z_new[1] = dT * z[2] - K[1][0] * (-x[1] + z[2] + z[3]) + z[1];
z_new[2] = -K[2][0] * (-x[1] + z[2] + z[3]) + z[2];
z_new[3] = -K[3][0] * (-x[1] + z[2] + z[3]) + z[3];
memcpy(z, z_new, sizeof(z_new));
V[0][0] = P[0][0] - K[0][0] * P[2][0] - K[0][0] * P[3][0];
V[0][1] = P[0][1] - K[0][0] * P[2][1] - K[0][0] * P[3][2];
V[0][2] = P[0][2] - K[0][0] * P[2][2] - K[0][0] * P[3][2];
V[0][3] = P[0][3] - K[0][0] * P[2][3] - K[0][0] * P[3][3];
V[1][0] = P[1][0] - K[1][0] * P[2][0] - K[1][0] * P[3][0];
V[1][1] = P[1][1] - K[1][0] * P[2][1] - K[1][0] * P[3][2];
V[1][2] = P[1][2] - K[1][0] * P[2][2] - K[1][0] * P[3][2];
V[1][3] = P[1][3] - K[1][0] * P[2][3] - K[1][0] * P[3][3];
V[2][0] = -K[2][0] * P[3][0] - P[2][0] * (K[2][0] - 1.0f);
V[2][1] = -K[2][0] * P[3][2] - P[2][1] * (K[2][0] - 1.0f);
V[2][2] = -K[2][0] * P[3][2] - P[2][2] * (K[2][0] - 1.0f);
V[2][3] = -K[2][0] * P[3][3] - P[2][3] * (K[2][0] - 1.0f);
V[3][0] = -K[3][0] * P[2][0] - P[3][0] * (K[3][0] - 1.0f);
V[3][1] = -K[3][0] * P[2][1] - P[3][2] * (K[3][0] - 1.0f);
V[3][2] = -K[3][0] * P[2][2] - P[3][2] * (K[3][0] - 1.0f);
V[3][3] = -K[3][0] * P[2][3] - P[3][3] * (K[3][0] - 1.0f);
}
AltHoldSmoothedData altHold;
AltHoldSmoothedGet(&altHold);
altHold.Altitude = z[0];
altHold.Velocity = z[1];
altHold.Accel = z[2];
AltHoldSmoothedSet(&altHold);
AltHoldSmoothedGet(&altHold);
// Verify that we are in altitude hold mode
uint8_t armed;
FlightStatusArmedGet(&armed);
if (!altitudeHoldFlightMode || armed != FLIGHTSTATUS_ARMED_ARMED) {
running = false;
}
if (!running) {
lastAltitudeHoldDesiredUpdate = PIOS_DELAY_GetRaw();
continue;
}
// Compute the altitude error
error = (starting_altitude + altitudeHoldDesired.Altitude) - altHold.Altitude;
// Compute integral off altitude error
throttleIntegral += error * altitudeHoldSettings.Ki * dT;
// Only update stabilizationDesired less frequently
if ((thisTime - lastUpdateTime) < 20) {
continue;
}
lastUpdateTime = thisTime;
// Instead of explicit limit on integral you output limit feedback
StabilizationDesiredGet(&stabilizationDesired);
if (!enterFailSafe) {
stabilizationDesired.Throttle = error * altitudeHoldSettings.Kp + throttleIntegral -
altHold.Velocity * altitudeHoldSettings.Kd - altHold.Accel * altitudeHoldSettings.Ka;
if (stabilizationDesired.Throttle > 1) {
throttleIntegral -= (stabilizationDesired.Throttle - 1);
stabilizationDesired.Throttle = 1;
} else if (stabilizationDesired.Throttle < 0) {
throttleIntegral -= stabilizationDesired.Throttle;
stabilizationDesired.Throttle = 0;
}
} else {
// shutdown motors
stabilizationDesired.Throttle = -1;
}
stabilizationDesired.StabilizationMode.Roll = STABILIZATIONDESIRED_STABILIZATIONMODE_ATTITUDE;
stabilizationDesired.StabilizationMode.Pitch = STABILIZATIONDESIRED_STABILIZATIONMODE_ATTITUDE;
stabilizationDesired.StabilizationMode.Yaw = STABILIZATIONDESIRED_STABILIZATIONMODE_AXISLOCK;
stabilizationDesired.Roll = altitudeHoldDesired.Roll;
stabilizationDesired.Pitch = altitudeHoldDesired.Pitch;
stabilizationDesired.Yaw = altitudeHoldDesired.Yaw;
StabilizationDesiredSet(&stabilizationDesired);
} else if (ev.obj == AltitudeHoldDesiredHandle()) {
// reset the failsafe timer
lastAltitudeHoldDesiredUpdate = PIOS_DELAY_GetRaw();
AltitudeHoldDesiredGet(&altitudeHoldDesired);
}
break;
}
AltitudeHoldStatusData altitudeHoldStatus;
AltitudeHoldStatusGet(&altitudeHoldStatus);
// do the actual control loop(s)
AltitudeHoldDesiredData altitudeHoldDesired;
AltitudeHoldDesiredGet(&altitudeHoldDesired);
float positionStateDown;
PositionStateDownGet(&positionStateDown);
float velocityStateDown;
VelocityStateDownGet(&velocityStateDown);
switch (altitudeHoldDesired.ControlMode) {
case ALTITUDEHOLDDESIRED_CONTROLMODE_ALTITUDE:
// altitude control loop
altitudeHoldStatus.VelocityDesired = pid_apply_setpoint(&pid0, 1.0f, altitudeHoldDesired.SetPoint, positionStateDown, 1000.0f / DESIRED_UPDATE_RATE_MS);
break;
case ALTITUDEHOLDDESIRED_CONTROLMODE_VELOCITY:
altitudeHoldStatus.VelocityDesired = altitudeHoldDesired.SetPoint;
break;
default:
altitudeHoldStatus.VelocityDesired = 0;
break;
}
AltitudeHoldStatusSet(&altitudeHoldStatus);
float throttle;
switch (altitudeHoldDesired.ControlMode) {
case ALTITUDEHOLDDESIRED_CONTROLMODE_THROTTLE:
throttle = altitudeHoldDesired.SetPoint;
break;
default:
// velocity control loop
throttle = startThrottle - pid_apply_setpoint(&pid1, 1.0f, altitudeHoldStatus.VelocityDesired, velocityStateDown, 1000.0f / DESIRED_UPDATE_RATE_MS);
// compensate throttle for current attitude
// Rbe[2][2] in the rotation matrix is the rotated down component of a
// 0,0,1 vector
// it is used to scale the throttle, but only up to 4 times the regular
// throttle
AttitudeStateData attitudeState;
AttitudeStateGet(&attitudeState);
float Rbe[3][3];
Quaternion2R(&attitudeState.q1, Rbe);
if (fabsf(Rbe[2][2]) > 0.25f) {
throttle /= Rbe[2][2];
} else {
throttle /= 0.25f;
}
if (throttle >= 1.0f) {
throttle = 1.0f;
}
if (throttle <= 0.0f) {
throttle = 0.0f;
}
break;
}
StabilizationDesiredData stab;
StabilizationDesiredGet(&stab);
stab.Roll = altitudeHoldDesired.Roll;
stab.Pitch = altitudeHoldDesired.Pitch;
stab.Yaw = altitudeHoldDesired.Yaw;
stab.Throttle = throttle;
stab.StabilizationMode.Roll = STABILIZATIONDESIRED_STABILIZATIONMODE_ATTITUDE;
stab.StabilizationMode.Pitch = STABILIZATIONDESIRED_STABILIZATIONMODE_ATTITUDE;
stab.StabilizationMode.Yaw = STABILIZATIONDESIRED_STABILIZATIONMODE_AXISLOCK;
StabilizationDesiredSet(&stab);
DelayedCallbackSchedule(altitudeHoldCBInfo, DESIRED_UPDATE_RATE_MS, CALLBACK_UPDATEMODE_SOONER);
}
static void SettingsUpdatedCb(__attribute__((unused)) UAVObjEvent *ev)
{
AltitudeHoldSettingsGet(&altitudeHoldSettings);
pid_configure(&pid0, altitudeHoldSettings.AltitudePI.Kp, altitudeHoldSettings.AltitudePI.Ki, 0, altitudeHoldSettings.AltitudePI.Ilimit);
pid_zero(&pid0);
pid_configure(&pid1, altitudeHoldSettings.VelocityPI.Kp, altitudeHoldSettings.VelocityPI.Ki, 0, altitudeHoldSettings.VelocityPI.Ilimit);
pid_zero(&pid1);
}

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

137
flight/modules/ManualControl/manualcontrol.c
View File

@ -46,7 +46,7 @@
#include "manualcontrolcommand.h"
#include "positionstate.h"
#include "pathdesired.h"
#include "stabilizationsettings.h"
#include "stabilizationbank.h"
#include "stabilizationdesired.h"
#include "receiveractivity.h"
#include "systemsettings.h"
@ -668,8 +668,8 @@ static void updateStabilizationDesired(ManualControlCommandData *cmd, ManualCont
StabilizationDesiredGet(&stabilization);
StabilizationSettingsData stabSettings;
StabilizationSettingsGet(&stabSettings);
StabilizationBankData stabSettings;
StabilizationBankGet(&stabSettings);
uint8_t *stab_settings;
FlightStatusData flightStatus;
@ -690,46 +690,52 @@ static void updateStabilizationDesired(ManualControlCommandData *cmd, ManualCont
return;
}
// TOOD: Add assumption about order of stabilization desired and manual control stabilization mode fields having same order
stabilization.StabilizationMode.Roll = stab_settings[0];
stabilization.StabilizationMode.Pitch = stab_settings[1];
stabilization.StabilizationMode.Yaw = stab_settings[2];
stabilization.Roll =
(stab_settings[0] == STABILIZATIONDESIRED_STABILIZATIONMODE_NONE) ? cmd->Roll :
(stab_settings[0] == STABILIZATIONDESIRED_STABILIZATIONMODE_RATE) ? cmd->Roll * stabSettings.ManualRate.Roll :
(stab_settings[0] == STABILIZATIONDESIRED_STABILIZATIONMODE_WEAKLEVELING) ?
cmd->Roll * stabSettings.ManualRate.Roll :
(stab_settings[0] == STABILIZATIONDESIRED_STABILIZATIONMODE_WEAKLEVELING) ? cmd->Roll * stabSettings.ManualRate.Roll :
(stab_settings[0] == STABILIZATIONDESIRED_STABILIZATIONMODE_ATTITUDE) ? cmd->Roll * stabSettings.RollMax :
(stab_settings[0] == STABILIZATIONDESIRED_STABILIZATIONMODE_AXISLOCK) ? cmd->Roll * stabSettings.ManualRate.Roll :
(stab_settings[0] == STABILIZATIONDESIRED_STABILIZATIONMODE_VIRTUALBAR) ? cmd->Roll :
(stab_settings[0] == STABILIZATIONDESIRED_STABILIZATIONMODE_RELAYRATE) ?
cmd->Roll * stabSettings.ManualRate.Roll :
(stab_settings[0] == STABILIZATIONDESIRED_STABILIZATIONMODE_RELAYATTITUDE) ? cmd->Roll * stabSettings.RollMax : 0; // this is an invalid mode
;
(stab_settings[0] == STABILIZATIONDESIRED_STABILIZATIONMODE_RATTITUDE) ? cmd->Roll :
(stab_settings[0] == STABILIZATIONDESIRED_STABILIZATIONMODE_RELAYRATE) ? cmd->Roll * stabSettings.ManualRate.Roll :
(stab_settings[0] == STABILIZATIONDESIRED_STABILIZATIONMODE_RELAYATTITUDE) ? cmd->Roll * stabSettings.RollMax :
0; // this is an invalid mode
stabilization.Pitch =
(stab_settings[1] == STABILIZATIONDESIRED_STABILIZATIONMODE_NONE) ? cmd->Pitch :
(stab_settings[1] == STABILIZATIONDESIRED_STABILIZATIONMODE_RATE) ? cmd->Pitch * stabSettings.ManualRate.Pitch :
(stab_settings[1] == STABILIZATIONDESIRED_STABILIZATIONMODE_WEAKLEVELING) ?
cmd->Pitch * stabSettings.ManualRate.Pitch :
(stab_settings[1] == STABILIZATIONDESIRED_STABILIZATIONMODE_WEAKLEVELING) ? cmd->Pitch * stabSettings.ManualRate.Pitch :
(stab_settings[1] == STABILIZATIONDESIRED_STABILIZATIONMODE_ATTITUDE) ? cmd->Pitch * stabSettings.PitchMax :
(stab_settings[1] == STABILIZATIONDESIRED_STABILIZATIONMODE_AXISLOCK) ?
cmd->Pitch * stabSettings.ManualRate.Pitch :
(stab_settings[1] == STABILIZATIONDESIRED_STABILIZATIONMODE_AXISLOCK) ? cmd->Pitch * stabSettings.ManualRate.Pitch :
(stab_settings[1] == STABILIZATIONDESIRED_STABILIZATIONMODE_VIRTUALBAR) ? cmd->Pitch :
(stab_settings[1] == STABILIZATIONDESIRED_STABILIZATIONMODE_RELAYRATE) ?
cmd->Pitch * stabSettings.ManualRate.Pitch :
(stab_settings[1] == STABILIZATIONDESIRED_STABILIZATIONMODE_RELAYATTITUDE) ? cmd->Pitch * stabSettings.PitchMax : 0; // this is an invalid mode
(stab_settings[1] == STABILIZATIONDESIRED_STABILIZATIONMODE_RATTITUDE) ? cmd->Pitch :
(stab_settings[1] == STABILIZATIONDESIRED_STABILIZATIONMODE_RELAYRATE) ? cmd->Pitch * stabSettings.ManualRate.Pitch :
(stab_settings[1] == STABILIZATIONDESIRED_STABILIZATIONMODE_RELAYATTITUDE) ? cmd->Pitch * stabSettings.PitchMax :
0; // this is an invalid mode
stabilization.Yaw =
(stab_settings[2] == STABILIZATIONDESIRED_STABILIZATIONMODE_NONE) ? cmd->Yaw :
(stab_settings[2] == STABILIZATIONDESIRED_STABILIZATIONMODE_RATE) ? cmd->Yaw * stabSettings.ManualRate.Yaw :
(stab_settings[2] == STABILIZATIONDESIRED_STABILIZATIONMODE_WEAKLEVELING) ?
cmd->Yaw * stabSettings.ManualRate.Yaw :
(stab_settings[2] == STABILIZATIONDESIRED_STABILIZATIONMODE_ATTITUDE) ? cmd->Yaw * stabSettings.YawMax :
(stab_settings[2] == STABILIZATIONDESIRED_STABILIZATIONMODE_AXISLOCK) ? cmd->Yaw * stabSettings.ManualRate.Yaw :
(stab_settings[2] == STABILIZATIONDESIRED_STABILIZATIONMODE_VIRTUALBAR) ? cmd->Yaw :
(stab_settings[2] == STABILIZATIONDESIRED_STABILIZATIONMODE_RELAYRATE) ? cmd->Yaw * stabSettings.ManualRate.Yaw :
(stab_settings[2] == STABILIZATIONDESIRED_STABILIZATIONMODE_RELAYATTITUDE) ? cmd->Yaw * stabSettings.YawMax : 0; // this is an invalid mode
// TOOD: Add assumption about order of stabilization desired and manual control stabilization mode fields having same order
stabilization.StabilizationMode.Roll = stab_settings[0];
stabilization.StabilizationMode.Pitch = stab_settings[1];
// Other axes (yaw) cannot be Rattitude, so use Rate
// Should really do this for Attitude mode as well?
if (stab_settings[2] == STABILIZATIONDESIRED_STABILIZATIONMODE_RATTITUDE) {
stabilization.StabilizationMode.Yaw = STABILIZATIONDESIRED_STABILIZATIONMODE_RATE;
stabilization.Yaw = cmd->Yaw * stabSettings.ManualRate.Yaw;
} else {
stabilization.StabilizationMode.Yaw = stab_settings[2];
stabilization.Yaw =
(stab_settings[2] == STABILIZATIONDESIRED_STABILIZATIONMODE_NONE) ? cmd->Yaw :
(stab_settings[2] == STABILIZATIONDESIRED_STABILIZATIONMODE_RATE) ? cmd->Yaw * stabSettings.ManualRate.Yaw :
(stab_settings[2] == STABILIZATIONDESIRED_STABILIZATIONMODE_WEAKLEVELING) ? cmd->Yaw * stabSettings.ManualRate.Yaw :
(stab_settings[2] == STABILIZATIONDESIRED_STABILIZATIONMODE_ATTITUDE) ? cmd->Yaw * stabSettings.YawMax :
(stab_settings[2] == STABILIZATIONDESIRED_STABILIZATIONMODE_AXISLOCK) ? cmd->Yaw * stabSettings.ManualRate.Yaw :
(stab_settings[2] == STABILIZATIONDESIRED_STABILIZATIONMODE_VIRTUALBAR) ? cmd->Yaw :
(stab_settings[2] == STABILIZATIONDESIRED_STABILIZATIONMODE_RATTITUDE) ? cmd->Yaw :
(stab_settings[2] == STABILIZATIONDESIRED_STABILIZATIONMODE_RELAYRATE) ? cmd->Yaw * stabSettings.ManualRate.Yaw :
(stab_settings[2] == STABILIZATIONDESIRED_STABILIZATIONMODE_RELAYATTITUDE) ? cmd->Yaw * stabSettings.YawMax :
0; // this is an invalid mode
}
stabilization.Throttle = (cmd->Throttle < 0) ? -1 : cmd->Throttle;
StabilizationDesiredSet(&stabilization);
@ -839,65 +845,60 @@ static void updateLandDesired(__attribute__((unused)) ManualControlCommandData *
*/
static void altitudeHoldDesired(ManualControlCommandData *cmd, bool changed)
{
const float DEADBAND = 0.25f;
const float DEADBAND = 0.20f;
const float DEADBAND_HIGH = 1.0f / 2 + DEADBAND / 2;
const float DEADBAND_LOW = 1.0f / 2 - DEADBAND / 2;
// Stop updating AltitudeHoldDesired triggering a failsafe condition.
if (cmd->Throttle < 0) {
return;
}
// this is the max speed in m/s at the extents of throttle
uint8_t throttleRate;
float throttleRate;
uint8_t throttleExp;
static uint8_t flightMode;
static portTickType lastSysTimeAH;
static bool zeroed = false;
portTickType thisSysTime;
float dT;
static bool newaltitude = true;
FlightStatusFlightModeGet(&flightMode);
AltitudeHoldDesiredData altitudeHoldDesiredData;
AltitudeHoldDesiredGet(&altitudeHoldDesiredData);
if (flightMode == FLIGHTSTATUS_FLIGHTMODE_ALTITUDEHOLD) {
throttleExp = 128;
throttleRate = 0;
} else {
AltitudeHoldSettingsThrottleExpGet(&throttleExp);
AltitudeHoldSettingsThrottleRateGet(&throttleRate);
}
AltitudeHoldSettingsThrottleExpGet(&throttleExp);
AltitudeHoldSettingsThrottleRateGet(&throttleRate);
StabilizationSettingsData stabSettings;
StabilizationSettingsGet(&stabSettings);
StabilizationBankData stabSettings;
StabilizationBankGet(&stabSettings);
thisSysTime = xTaskGetTickCount();
dT = ((thisSysTime == lastSysTimeAH) ? 0.001f : (thisSysTime - lastSysTimeAH) * portTICK_RATE_MS * 0.001f);
lastSysTimeAH = thisSysTime;
PositionStateData posState;
PositionStateGet(&posState);
altitudeHoldDesiredData.Roll = cmd->Roll * stabSettings.RollMax;
altitudeHoldDesiredData.Pitch = cmd->Pitch * stabSettings.PitchMax;
altitudeHoldDesiredData.Yaw = cmd->Yaw * stabSettings.ManualRate.Yaw;
float currentDown;
PositionStateDownGet(&currentDown);
if (changed) {
// After not being in this mode for a while init at current height
altitudeHoldDesiredData.Altitude = 0;
zeroed = false;
} else if (cmd->Throttle > DEADBAND_HIGH && zeroed) {
newaltitude = true;
}
uint8_t cutOff;
AltitudeHoldSettingsCutThrottleWhenZeroGet(&cutOff);
if (cutOff && cmd->Throttle < 0) {
// Cut throttle if desired
altitudeHoldDesiredData.SetPoint = cmd->Throttle;
altitudeHoldDesiredData.ControlMode = ALTITUDEHOLDDESIRED_CONTROLMODE_THROTTLE;
newaltitude = true;
} else if (flightMode == FLIGHTSTATUS_FLIGHTMODE_ALTITUDEVARIO && cmd->Throttle > DEADBAND_HIGH) {
// being the two band symmetrical I can divide by DEADBAND_LOW to scale it to a value betweeon 0 and 1
// then apply an "exp" f(x,k) = (k*x*x*x + x*(256−k)) / 256
altitudeHoldDesiredData.Altitude += (throttleExp * powf((cmd->Throttle - DEADBAND_HIGH) / (DEADBAND_LOW), 3) + (256 - throttleExp)) / 256 * throttleRate * dT;
} else if (cmd->Throttle < DEADBAND_LOW && zeroed) {
altitudeHoldDesiredData.Altitude -= (throttleExp * powf((DEADBAND_LOW - (cmd->Throttle < 0 ? 0 : cmd->Throttle)) / DEADBAND_LOW, 3) + (256 - throttleExp)) / 256 * throttleRate * dT;
} else if (cmd->Throttle >= DEADBAND_LOW && cmd->Throttle <= DEADBAND_HIGH && (throttleRate != 0)) {
// Require the stick to enter the dead band before they can move height
// Vario is not "engaged" when throttleRate == 0
zeroed = true;
// then apply an "exp" f(x,k) = (k*x*x*x + (255-k)*x) / 255
altitudeHoldDesiredData.SetPoint = -((throttleExp * powf((cmd->Throttle - DEADBAND_HIGH) / (DEADBAND_LOW), 3) + (255 - throttleExp) * (cmd->Throttle - DEADBAND_HIGH) / DEADBAND_LOW) / 255 * throttleRate);
altitudeHoldDesiredData.ControlMode = ALTITUDEHOLDDESIRED_CONTROLMODE_VELOCITY;
newaltitude = true;
} else if (flightMode == FLIGHTSTATUS_FLIGHTMODE_ALTITUDEVARIO && cmd->Throttle < DEADBAND_LOW) {
altitudeHoldDesiredData.SetPoint = -(-(throttleExp * powf((DEADBAND_LOW - (cmd->Throttle < 0 ? 0 : cmd->Throttle)) / DEADBAND_LOW, 3) + (255 - throttleExp) * (DEADBAND_LOW - cmd->Throttle) / DEADBAND_LOW) / 255 * throttleRate);
altitudeHoldDesiredData.ControlMode = ALTITUDEHOLDDESIRED_CONTROLMODE_VELOCITY;
newaltitude = true;
} else if (newaltitude == true) {
altitudeHoldDesiredData.SetPoint = posState.Down;
altitudeHoldDesiredData.ControlMode = ALTITUDEHOLDDESIRED_CONTROLMODE_ALTITUDE;
newaltitude = false;
}
AltitudeHoldDesiredSet(&altitudeHoldDesiredData);

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;

409
flight/modules/Stabilization/stabilization.c
View File

@ -35,6 +35,10 @@
#include <pios_struct_helper.h>
#include "stabilization.h"
#include "stabilizationsettings.h"
#include "stabilizationbank.h"
#include "stabilizationsettingsbank1.h"
#include "stabilizationsettingsbank2.h"
#include "stabilizationsettingsbank3.h"
#include "actuatordesired.h"
#include "ratedesired.h"
#include "relaytuning.h"
@ -65,14 +69,19 @@
#if defined(PIOS_STABILIZATION_STACK_SIZE)
#define STACK_SIZE_BYTES PIOS_STABILIZATION_STACK_SIZE
#else
#define STACK_SIZE_BYTES 724
#define STACK_SIZE_BYTES 790
#endif
#define TASK_PRIORITY (tskIDLE_PRIORITY + 4)
#define FAILSAFE_TIMEOUT_MS 30
enum { PID_RATE_ROLL, PID_RATE_PITCH, PID_RATE_YAW, PID_ROLL, PID_PITCH, PID_YAW, PID_MAX };
// The PID_RATE_ROLL set is used by Rate mode and the rate portion of Attitude mode
// The PID_RATE set is used by the attitude portion of Attitude mode
// The PID_RATEA_ROLL set is used by Rattitude mode because it needs to maintain
// - two independant rate PIDs because it does rate and attitude simultaneously
enum { PID_RATE_ROLL, PID_RATE_PITCH, PID_RATE_YAW, PID_ROLL, PID_PITCH, PID_YAW, PID_RATEA_ROLL, PID_RATEA_PITCH, PID_RATEA_YAW, PID_MAX };
enum { RATE_P, RATE_I, RATE_D, RATE_LIMIT, RATE_OFFSET };
enum { ATT_P, ATT_I, ATT_LIMIT, ATT_OFFSET };
// Private variables
static xTaskHandle taskHandle;
@ -89,11 +98,20 @@ bool lowThrottleZeroAxis[MAX_AXES];
float vbar_decay = 0.991f;
struct pid pids[PID_MAX];
int flight_mode = -1;
static uint8_t rattitude_anti_windup;
// Private functions
static void stabilizationTask(void *parameters);
static float bound(float val, float range);
static void ZeroPids(void);
static void SettingsUpdatedCb(UAVObjEvent *ev);
static void BankUpdatedCb(UAVObjEvent *ev);
static void SettingsBankUpdatedCb(UAVObjEvent *ev);
static float stab_log2f(float x);
static float stab_powf(float x, uint8_t p);
/**
* Module initialization
@ -111,6 +129,13 @@ int32_t StabilizationStart()
StabilizationSettingsConnectCallback(SettingsUpdatedCb);
SettingsUpdatedCb(StabilizationSettingsHandle());
StabilizationBankConnectCallback(BankUpdatedCb);
StabilizationSettingsBank1ConnectCallback(SettingsBankUpdatedCb);
StabilizationSettingsBank2ConnectCallback(SettingsBankUpdatedCb);
StabilizationSettingsBank3ConnectCallback(SettingsBankUpdatedCb);
// Start main task
xTaskCreate(stabilizationTask, (signed char *)"Stabilization", STACK_SIZE_BYTES / 4, NULL, TASK_PRIORITY, &taskHandle);
PIOS_TASK_MONITOR_RegisterTask(TASKINFO_RUNNING_STABILIZATION, taskHandle);
@ -127,6 +152,10 @@ int32_t StabilizationInitialize()
{
// Initialize variables
StabilizationSettingsInitialize();
StabilizationBankInitialize();
StabilizationSettingsBank1Initialize();
StabilizationSettingsBank2Initialize();
StabilizationSettingsBank3Initialize();
ActuatorDesiredInitialize();
#ifdef DIAG_RATEDESIRED
RateDesiredInitialize();
@ -159,6 +188,8 @@ static void stabilizationTask(__attribute__((unused)) void *parameters)
AttitudeStateData attitudeState;
GyroStateData gyroStateData;
FlightStatusData flightStatus;
StabilizationBankData stabBank;
#ifdef REVOLUTION
AirspeedStateData airspeedState;
@ -175,7 +206,7 @@ static void stabilizationTask(__attribute__((unused)) void *parameters)
PIOS_WDG_UpdateFlag(PIOS_WDG_STABILIZATION);
#endif
// Wait until the AttitudeRaw object is updated, if a timeout then go to failsafe
// Wait until the Gyro object is updated, if a timeout then go to failsafe
if (xQueueReceive(queue, &ev, FAILSAFE_TIMEOUT_MS / portTICK_RATE_MS) != pdTRUE) {
AlarmsSet(SYSTEMALARMS_ALARM_STABILIZATION, SYSTEMALARMS_ALARM_WARNING);
continue;
@ -188,9 +219,16 @@ static void stabilizationTask(__attribute__((unused)) void *parameters)
StabilizationDesiredGet(&stabDesired);
AttitudeStateGet(&attitudeState);
GyroStateGet(&gyroStateData);
StabilizationBankGet(&stabBank);
#ifdef DIAG_RATEDESIRED
RateDesiredGet(&rateDesired);
#endif
if (flight_mode != flightStatus.FlightMode) {
flight_mode = flightStatus.FlightMode;
SettingsBankUpdatedCb(NULL);
}
#ifdef REVOLUTION
float speedScaleFactor;
// Scale PID coefficients based on current airspeed estimation - needed for fixed wing planes
@ -263,7 +301,7 @@ static void stabilizationTask(__attribute__((unused)) void *parameters)
gyro_filtered[1] = gyro_filtered[1] * gyro_alpha + gyroStateData.y * (1 - gyro_alpha);
gyro_filtered[2] = gyro_filtered[2] * gyro_alpha + gyroStateData.z * (1 - gyro_alpha);
float *attitudeDesiredAxis = &stabDesired.Roll;
float *stabDesiredAxis = &stabDesired.Roll;
float *actuatorDesiredAxis = &actuatorDesired.Roll;
float *rateDesiredAxis = &rateDesired.Roll;
@ -288,7 +326,7 @@ static void stabilizationTask(__attribute__((unused)) void *parameters)
// Store to rate desired variable for storing to UAVO
rateDesiredAxis[i] =
bound(attitudeDesiredAxis[i], cast_struct_to_array(settings.ManualRate, settings.ManualRate.Roll)[i]);
bound(stabDesiredAxis[i], cast_struct_to_array(stabBank.ManualRate, stabBank.ManualRate.Roll)[i]);
// Compute the inner loop
actuatorDesiredAxis[i] = pid_apply_setpoint(&pids[PID_RATE_ROLL + i], speedScaleFactor, rateDesiredAxis[i], gyro_filtered[i], dT);
@ -305,7 +343,7 @@ static void stabilizationTask(__attribute__((unused)) void *parameters)
// Compute the outer loop
rateDesiredAxis[i] = pid_apply(&pids[PID_ROLL + i], local_error[i], dT);
rateDesiredAxis[i] = bound(rateDesiredAxis[i],
cast_struct_to_array(settings.MaximumRate, settings.MaximumRate.Roll)[i]);
cast_struct_to_array(stabBank.MaximumRate, stabBank.MaximumRate.Roll)[i]);
// Compute the inner loop
actuatorDesiredAxis[i] = pid_apply_setpoint(&pids[PID_RATE_ROLL + i], speedScaleFactor, rateDesiredAxis[i], gyro_filtered[i], dT);
@ -313,10 +351,127 @@ static void stabilizationTask(__attribute__((unused)) void *parameters)
break;
case STABILIZATIONDESIRED_STABILIZATIONMODE_RATTITUDE:
// A parameterization from Attitude mode at center stick
// - to Rate mode at full stick
// This is done by parameterizing to use the rotation rate that Attitude mode
// - would use at center stick to using the rotation rate that Rate mode
// - would use at full stick in a weighted average sort of way.
{
if (reinit) {
pids[PID_ROLL + i].iAccumulator = 0;
pids[PID_RATE_ROLL + i].iAccumulator = 0;
pids[PID_RATEA_ROLL + i].iAccumulator = 0;
}
// Compute what Rate mode would give for this stick angle's rate
// Save Rate's rate in a temp for later merging with Attitude's rate
float rateDesiredAxisRate;
rateDesiredAxisRate = bound(stabDesiredAxis[i], 1.0f)
* cast_struct_to_array(stabBank.ManualRate, stabBank.ManualRate.Roll)[i];
// Compute what Attitude mode would give for this stick angle's rate
// stabDesired for this mode is [-1.0f,+1.0f]
// - multiply by Attitude mode max angle to get desired angle
// - subtract off the actual angle to get the angle error
// This is what local_error[] holds for Attitude mode
float attitude_error = stabDesiredAxis[i]
* cast_struct_to_array(stabBank.RollMax, stabBank.RollMax)[i]
- cast_struct_to_array(attitudeState.Roll, attitudeState.Roll)[i];
// Compute the outer loop just like Attitude mode does
float rateDesiredAxisAttitude;
rateDesiredAxisAttitude = pid_apply(&pids[PID_ROLL + i], attitude_error, dT);
rateDesiredAxisAttitude = bound(rateDesiredAxisAttitude,
cast_struct_to_array(stabBank.MaximumRate,
stabBank.MaximumRate.Roll)[i]);
// Compute the weighted average rate desired
// Using max() rather than sqrt() for cpu speed;
// - this makes the stick region into a square;
// - this is a feature!
// - hold a roll angle and add just pitch without the stick sensitivity changing
// magnitude = sqrt(stabDesired.Roll*stabDesired.Roll + stabDesired.Pitch*stabDesired.Pitch);
float magnitude;
magnitude = fmaxf(fabsf(stabDesired.Roll), fabsf(stabDesired.Pitch));
rateDesiredAxis[i] = (1.0f - magnitude) * rateDesiredAxisAttitude
+ magnitude * rateDesiredAxisRate;
// Compute the inner loop for both Rate mode and Attitude mode
// actuatorDesiredAxis[i] is the weighted average of the two PIDs from the two rates
actuatorDesiredAxis[i] =
(1.0f - magnitude) * pid_apply_setpoint(&pids[PID_RATEA_ROLL + i], speedScaleFactor, rateDesiredAxis[i], gyro_filtered[i], dT)
+ magnitude * pid_apply_setpoint(&pids[PID_RATE_ROLL + i], speedScaleFactor, rateDesiredAxis[i], gyro_filtered[i], dT);
actuatorDesiredAxis[i] = bound(actuatorDesiredAxis[i], 1.0f);
// settings.RattitudeAntiWindup controls the iAccumulator zeroing
// - so both iAccs don't wind up too far;
// - nor do both iAccs get held too close to zero at mid stick
// I suspect that there would be windup without it
// - since rate and attitude fight each other here
// - rate trying to pull it over the top and attitude trying to pull it back down
// Wind-up increases linearly with cycles for a fixed error.
// We must never increase the iAcc or we risk oscillation.
// Use the powf() function to make two anti-windup curves
// - one for zeroing rate close to center stick
// - the other for zeroing attitude close to max stick
// the bigger the dT the more anti windup needed
// the bigger the PID[].i the more anti windup needed
// more anti windup is achieved with a lower powf() power
// a doubling of e.g. PID[].i should cause the runtime anti windup factor
// to get twice as far away from 1.0 (towards zero)
// e.g. from .90 to .80
// magic numbers
// these generate the inverted parabola like curves that go through [0,1] and [1,0]
// the higher the power, the closer the curve is to a straight line
// from [0,1] to [1,1] to [1,0] and the less anti windup is applied
// the UAVO RattitudeAntiWindup varies from 0 to 31
// 0 turns off anti windup
// 1 gives very little anti-windup because the power given to powf() is 31
// 31 gives a lot of anti-windup because the power given to powf() is 1
// the 32.1 is what does this
// the 7.966 and 17.668 cancel the default PID value and dT given to log2f()
// if these are non-default, tweaking is thus done so the user doesn't have to readjust
// the default value of 10 for UAVO RattitudeAntiWindup gives a power of 22
// these calculations are for magnitude = 0.5, so 22 corresponds to the number of bits
// used in the mantissa of the float
// i.e. 1.0-(0.5^22) almost underflows
// This may only be useful for aircraft with large Ki values and limits
if (dT > 0.0f && rattitude_anti_windup > 0.0f) {
float factor;
// At magnitudes close to one, the Attitude accumulators gets zeroed
if (pids[PID_ROLL + i].i > 0.0f) {
factor = 1.0f - stab_powf(magnitude, ((uint8_t)(32.1f - 7.966f - stab_log2f(dT * pids[PID_ROLL + i].i))) - rattitude_anti_windup);
pids[PID_ROLL + i].iAccumulator *= factor;
}
if (pids[PID_RATEA_ROLL + i].i > 0.0f) {
factor = 1.0f - stab_powf(magnitude, ((uint8_t)(32.1f - 17.668f - stab_log2f(dT * pids[PID_RATEA_ROLL + i].i))) - rattitude_anti_windup);
pids[PID_RATEA_ROLL + i].iAccumulator *= factor;
}
// At magnitudes close to zero, the Rate accumulator gets zeroed
if (pids[PID_RATE_ROLL + i].i > 0.0f) {
factor = 1.0f - stab_powf(1.0f - magnitude, ((uint8_t)(32.1f - 17.668f - stab_log2f(dT * pids[PID_RATE_ROLL + i].i))) - rattitude_anti_windup);
pids[PID_RATE_ROLL + i].iAccumulator *= factor;
}
}
break;
}
case STABILIZATIONDESIRED_STABILIZATIONMODE_VIRTUALBAR:
// Store for debugging output
rateDesiredAxis[i] = attitudeDesiredAxis[i];
rateDesiredAxis[i] = stabDesiredAxis[i];
// Run a virtual flybar stabilization algorithm on this axis
stabilization_virtual_flybar(gyro_filtered[i], rateDesiredAxis[i], &actuatorDesiredAxis[i], dT, reinit, i, &settings);
@ -324,6 +479,12 @@ static void stabilizationTask(__attribute__((unused)) void *parameters)
break;
case STABILIZATIONDESIRED_STABILIZATIONMODE_WEAKLEVELING:
// FIXME: local_error[] is rate - attitude for Weak Leveling
// The only ramifications are:
// Weak Leveling Kp is off by a factor of 3 to 12 and may need a different default in GCS
// Changing Rate mode max rate currently requires a change to Kp
// That would be changed to Attitude mode max angle affecting Kp
// Also does not take dT into account
{
if (reinit) {
pids[PID_RATE_ROLL + i].iAccumulator = 0;
@ -333,7 +494,7 @@ static void stabilizationTask(__attribute__((unused)) void *parameters)
weak_leveling = bound(weak_leveling, weak_leveling_max);
// Compute desired rate as input biased towards leveling
rateDesiredAxis[i] = attitudeDesiredAxis[i] + weak_leveling;
rateDesiredAxis[i] = stabDesiredAxis[i] + weak_leveling;
actuatorDesiredAxis[i] = pid_apply_setpoint(&pids[PID_RATE_ROLL + i], speedScaleFactor, rateDesiredAxis[i], gyro_filtered[i], dT);
actuatorDesiredAxis[i] = bound(actuatorDesiredAxis[i], 1.0f);
@ -345,19 +506,19 @@ static void stabilizationTask(__attribute__((unused)) void *parameters)
pids[PID_RATE_ROLL + i].iAccumulator = 0;
}
if (fabsf(attitudeDesiredAxis[i]) > max_axislock_rate) {
if (fabsf(stabDesiredAxis[i]) > max_axislock_rate) {
// While getting strong commands act like rate mode
rateDesiredAxis[i] = attitudeDesiredAxis[i];
rateDesiredAxis[i] = stabDesiredAxis[i];
axis_lock_accum[i] = 0;
} else {
// For weaker commands or no command simply attitude lock (almost) on no gyro change
axis_lock_accum[i] += (attitudeDesiredAxis[i] - gyro_filtered[i]) * dT;
axis_lock_accum[i] += (stabDesiredAxis[i] - gyro_filtered[i]) * dT;
axis_lock_accum[i] = bound(axis_lock_accum[i], max_axis_lock);
rateDesiredAxis[i] = pid_apply(&pids[PID_ROLL + i], axis_lock_accum[i], dT);
}
rateDesiredAxis[i] = bound(rateDesiredAxis[i],
cast_struct_to_array(settings.ManualRate, settings.ManualRate.Roll)[i]);
cast_struct_to_array(stabBank.ManualRate, stabBank.ManualRate.Roll)[i]);
actuatorDesiredAxis[i] = pid_apply_setpoint(&pids[PID_RATE_ROLL + i], speedScaleFactor, rateDesiredAxis[i], gyro_filtered[i], dT);
actuatorDesiredAxis[i] = bound(actuatorDesiredAxis[i], 1.0f);
@ -366,8 +527,8 @@ static void stabilizationTask(__attribute__((unused)) void *parameters)
case STABILIZATIONDESIRED_STABILIZATIONMODE_RELAYRATE:
// Store to rate desired variable for storing to UAVO
rateDesiredAxis[i] = bound(attitudeDesiredAxis[i],
cast_struct_to_array(settings.ManualRate, settings.ManualRate.Roll)[i]);
rateDesiredAxis[i] = bound(stabDesiredAxis[i],
cast_struct_to_array(stabBank.ManualRate, stabBank.ManualRate.Roll)[i]);
// Run the relay controller which also estimates the oscillation parameters
stabilization_relay_rate(rateDesiredAxis[i] - gyro_filtered[i], &actuatorDesiredAxis[i], i, reinit);
@ -383,7 +544,7 @@ static void stabilizationTask(__attribute__((unused)) void *parameters)
// Compute the outer loop like attitude mode
rateDesiredAxis[i] = pid_apply(&pids[PID_ROLL + i], local_error[i], dT);
rateDesiredAxis[i] = bound(rateDesiredAxis[i],
cast_struct_to_array(settings.MaximumRate, settings.MaximumRate.Roll)[i]);
cast_struct_to_array(stabBank.MaximumRate, stabBank.MaximumRate.Roll)[i]);
// Run the relay controller which also estimates the oscillation parameters
stabilization_relay_rate(rateDesiredAxis[i] - gyro_filtered[i], &actuatorDesiredAxis[i], i, reinit);
@ -392,7 +553,7 @@ static void stabilizationTask(__attribute__((unused)) void *parameters)
break;
case STABILIZATIONDESIRED_STABILIZATIONMODE_NONE:
actuatorDesiredAxis[i] = bound(attitudeDesiredAxis[i], 1.0f);
actuatorDesiredAxis[i] = bound(stabDesiredAxis[i], 1.0f);
break;
default:
error = true;
@ -484,43 +645,182 @@ static float bound(float val, float range)
return val;
}
// x small (0.0 < x < .01) so interpolation of fractional part is reasonable
static float stab_log2f(float x)
{
union {
volatile float f;
volatile uint32_t i;
volatile unsigned char c[4];
} __attribute__((packed)) u1, u2;
u2.f = u1.f = x;
u1.i <<= 1;
u2.i &= 0xff800000;
// get and unbias the exponent, add in a linear interpolation of the fractional part
return (float)(u1.c[3] - 127) + (x / u2.f) - 1.0f;
}
// 0<=x<=1, 0<=p<=31
static float stab_powf(float x, uint8_t p)
{
float retval = 1.0f;
while (p) {
if (p & 1) {
retval *= x;
}
x *= x;
p >>= 1;
}
return retval;
}
static void SettingsBankUpdatedCb(__attribute__((unused)) UAVObjEvent *ev)
{
StabilizationBankData bank, oldBank;
StabilizationBankGet(&oldBank);
if (flight_mode < 0) {
return;
}
switch (cast_struct_to_array(settings.FlightModeMap, settings.FlightModeMap.Stabilized1)[flight_mode]) {
case 0:
StabilizationSettingsBank1Get((StabilizationSettingsBank1Data *)&bank);
break;
case 1:
StabilizationSettingsBank2Get((StabilizationSettingsBank2Data *)&bank);
break;
case 2:
StabilizationSettingsBank3Get((StabilizationSettingsBank3Data *)&bank);
break;
default:
memset(&bank, 0, sizeof(StabilizationBankDataPacked));
// return; //bank number is invalid. All we can do is ignore it.
}
// Need to do this to prevent an infinite loop
if (memcmp(&oldBank, &bank, sizeof(StabilizationBankDataPacked)) != 0) {
StabilizationBankSet(&bank);
}
}
static void BankUpdatedCb(__attribute__((unused)) UAVObjEvent *ev)
{
StabilizationBankData bank;
StabilizationBankGet(&bank);
// this code will be needed if any other modules alter stabilizationbank
/*
StabilizationBankData curBank;
if(flight_mode < 0) return;
switch(cast_struct_to_array(settings.FlightModeMap, settings.FlightModeMap.Stabilized1)[flight_mode])
{
case 0:
StabilizationSettingsBank1Get((StabilizationSettingsBank1Data *) &curBank);
if(memcmp(&curBank, &bank, sizeof(StabilizationBankDataPacked)) != 0)
{
StabilizationSettingsBank1Set((StabilizationSettingsBank1Data *) &bank);
}
break;
case 1:
StabilizationSettingsBank2Get((StabilizationSettingsBank2Data *) &curBank);
if(memcmp(&curBank, &bank, sizeof(StabilizationBankDataPacked)) != 0)
{
StabilizationSettingsBank2Set((StabilizationSettingsBank2Data *) &bank);
}
break;
case 2:
StabilizationSettingsBank3Get((StabilizationSettingsBank3Data *) &curBank);
if(memcmp(&curBank, &bank, sizeof(StabilizationBankDataPacked)) != 0)
{
StabilizationSettingsBank3Set((StabilizationSettingsBank3Data *) &bank);
}
break;
default:
return; //invalid bank number
}
*/
// Set the roll rate PID constants
pid_configure(&pids[PID_RATE_ROLL], bank.RollRatePID.Kp,
bank.RollRatePID.Ki,
bank.RollRatePID.Kd,
bank.RollRatePID.ILimit);
// Set the pitch rate PID constants
pid_configure(&pids[PID_RATE_PITCH], bank.PitchRatePID.Kp,
bank.PitchRatePID.Ki,
bank.PitchRatePID.Kd,
bank.PitchRatePID.ILimit);
// Set the yaw rate PID constants
pid_configure(&pids[PID_RATE_YAW], bank.YawRatePID.Kp,
bank.YawRatePID.Ki,
bank.YawRatePID.Kd,
bank.YawRatePID.ILimit);
// Set the roll attitude PI constants
pid_configure(&pids[PID_ROLL], bank.RollPI.Kp,
bank.RollPI.Ki,
0,
bank.RollPI.ILimit);
// Set the pitch attitude PI constants
pid_configure(&pids[PID_PITCH], bank.PitchPI.Kp,
bank.PitchPI.Ki,
0,
bank.PitchPI.ILimit);
// Set the yaw attitude PI constants
pid_configure(&pids[PID_YAW], bank.YawPI.Kp,
bank.YawPI.Ki,
0,
bank.YawPI.ILimit);
// Set the Rattitude roll rate PID constants
pid_configure(&pids[PID_RATEA_ROLL],
bank.RollRatePID.Kp,
bank.RollRatePID.Ki,
bank.RollRatePID.Kd,
bank.RollRatePID.ILimit);
// Set the Rattitude pitch rate PID constants
pid_configure(&pids[PID_RATEA_PITCH],
bank.PitchRatePID.Kp,
bank.PitchRatePID.Ki,
bank.PitchRatePID.Kd,
bank.PitchRatePID.ILimit);
// Set the Rattitude yaw rate PID constants
pid_configure(&pids[PID_RATEA_YAW],
bank.YawRatePID.Kp,
bank.YawRatePID.Ki,
bank.YawRatePID.Kd,
bank.YawRatePID.ILimit);
}
static void SettingsUpdatedCb(__attribute__((unused)) UAVObjEvent *ev)
{
StabilizationSettingsGet(&settings);
// Set the roll rate PID constants
pid_configure(&pids[PID_RATE_ROLL], settings.RollRatePID.Kp,
settings.RollRatePID.Ki,
pids[PID_RATE_ROLL].d = settings.RollRatePID.Kd,
pids[PID_RATE_ROLL].iLim = settings.RollRatePID.ILimit);
// Set the pitch rate PID constants
pid_configure(&pids[PID_RATE_PITCH], settings.PitchRatePID.Kp,
pids[PID_RATE_PITCH].i = settings.PitchRatePID.Ki,
pids[PID_RATE_PITCH].d = settings.PitchRatePID.Kd,
pids[PID_RATE_PITCH].iLim = settings.PitchRatePID.ILimit);
// Set the yaw rate PID constants
pid_configure(&pids[PID_RATE_YAW], settings.YawRatePID.Kp,
pids[PID_RATE_YAW].i = settings.YawRatePID.Ki,
pids[PID_RATE_YAW].d = settings.YawRatePID.Kd,
pids[PID_RATE_YAW].iLim = settings.YawRatePID.ILimit);
// Set the roll attitude PI constants
pid_configure(&pids[PID_ROLL], settings.RollPI.Kp,
settings.RollPI.Ki, 0,
pids[PID_ROLL].iLim = settings.RollPI.ILimit);
// Set the pitch attitude PI constants
pid_configure(&pids[PID_PITCH], settings.PitchPI.Kp,
pids[PID_PITCH].i = settings.PitchPI.Ki, 0,
settings.PitchPI.ILimit);
// Set the yaw attitude PI constants
pid_configure(&pids[PID_YAW], settings.YawPI.Kp,
settings.YawPI.Ki, 0,
settings.YawPI.ILimit);
// Set up the derivative term
pid_configure_derivative(settings.DerivativeCutoff, settings.DerivativeGamma);
@ -553,9 +853,14 @@ static void SettingsUpdatedCb(__attribute__((unused)) UAVObjEvent *ev)
}
// Compute time constant for vbar decay term based on a tau
vbar_decay = expf(-fakeDt / settings.VbarTau);
}
vbar_decay = expf(-fakeDt / settings.VbarTau);
// force flight mode update
flight_mode = -1;
// Rattitude flight mode anti-windup factor
rattitude_anti_windup = settings.RattitudeAntiWindup;
}
/**
* @}

7
flight/modules/StateEstimation/filterbaro.c
View File

@ -37,6 +37,7 @@
// Private constants
#define STACK_REQUIRED 64
#define INIT_CYCLES 100
// Private types
struct data {
@ -67,7 +68,7 @@ static int32_t init(stateFilter *self)
struct data *this = (struct data *)self->localdata;
this->baroOffset = 0.0f;
this->first_run = 100;
this->first_run = INIT_CYCLES;
RevoSettingsInitialize();
RevoSettingsBaroGPSOffsetCorrectionAlphaGet(&this->baroGPSOffsetCorrectionAlpha);
@ -82,8 +83,8 @@ static int32_t filter(stateFilter *self, stateEstimation *state)
if (this->first_run) {
// Initialize to current altitude reading at initial location
if (IS_SET(state->updated, SENSORUPDATES_baro)) {
this->baroOffset = (100.f - this->first_run) / 100.f * this->baroOffset + (this->first_run / 100.f) * state->baro[0];
this->baroAlt = this->baroOffset;
this->baroOffset = ((float)(INIT_CYCLES)-this->first_run) / (float)(INIT_CYCLES)*this->baroOffset + (this->first_run / (float)(INIT_CYCLES)) * state->baro[0];
this->baroAlt = 0;
this->first_run--;
UNSET_MASK(state->updated, SENSORUPDATES_baro);
}

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

116
flight/modules/TxPID/txpid.c
View File

@ -56,6 +56,10 @@
#include "accessorydesired.h"
#include "manualcontrolcommand.h"
#include "stabilizationsettings.h"
#include "stabilizationbank.h"
#include "stabilizationsettingsbank1.h"
#include "stabilizationsettingsbank2.h"
#include "stabilizationsettingsbank3.h"
#include "flightstatus.h"
#include "hwsettings.h"
@ -163,11 +167,29 @@ static void updatePIDs(UAVObjEvent *ev)
return;
}
StabilizationBankData bank;
switch (inst.BankNumber) {
case 0:
StabilizationSettingsBank1Get((StabilizationSettingsBank1Data *)&bank);
break;
case 1:
StabilizationSettingsBank2Get((StabilizationSettingsBank2Data *)&bank);
break;
case 2:
StabilizationSettingsBank2Get((StabilizationSettingsBank2Data *)&bank);
break;
default:
return;
}
StabilizationSettingsData stab;
StabilizationSettingsGet(&stab);
AccessoryDesiredData accessory;
uint8_t needsUpdate = 0;
uint8_t needsUpdateBank = 0;
uint8_t needsUpdateStab = 0;
// Loop through every enabled instance
for (uint8_t i = 0; i < TXPIDSETTINGS_PIDS_NUMELEM; i++) {
@ -192,107 +214,125 @@ static void updatePIDs(UAVObjEvent *ev)
switch (cast_struct_to_array(inst.PIDs, inst.PIDs.Instance1)[i]) {
case TXPIDSETTINGS_PIDS_ROLLRATEKP:
needsUpdate |= update(&stab.RollRatePID.Kp, value);
needsUpdateBank |= update(&bank.RollRatePID.Kp, value);
break;
case TXPIDSETTINGS_PIDS_ROLLRATEKI:
needsUpdate |= update(&stab.RollRatePID.Ki, value);
needsUpdateBank |= update(&bank.RollRatePID.Ki, value);
break;
case TXPIDSETTINGS_PIDS_ROLLRATEKD:
needsUpdate |= update(&stab.RollRatePID.Kd, value);
needsUpdateBank |= update(&bank.RollRatePID.Kd, value);
break;
case TXPIDSETTINGS_PIDS_ROLLRATEILIMIT:
needsUpdate |= update(&stab.RollRatePID.ILimit, value);
needsUpdateBank |= update(&bank.RollRatePID.ILimit, value);
break;
case TXPIDSETTINGS_PIDS_ROLLATTITUDEKP:
needsUpdate |= update(&stab.RollPI.Kp, value);
needsUpdateBank |= update(&bank.RollPI.Kp, value);
break;
case TXPIDSETTINGS_PIDS_ROLLATTITUDEKI:
needsUpdate |= update(&stab.RollPI.Ki, value);
needsUpdateBank |= update(&bank.RollPI.Ki, value);
break;
case TXPIDSETTINGS_PIDS_ROLLATTITUDEILIMIT:
needsUpdate |= update(&stab.RollPI.ILimit, value);
needsUpdateBank |= update(&bank.RollPI.ILimit, value);
break;
case TXPIDSETTINGS_PIDS_PITCHRATEKP:
needsUpdate |= update(&stab.PitchRatePID.Kp, value);
needsUpdateBank |= update(&bank.PitchRatePID.Kp, value);
break;
case TXPIDSETTINGS_PIDS_PITCHRATEKI:
needsUpdate |= update(&stab.PitchRatePID.Ki, value);
needsUpdateBank |= update(&bank.PitchRatePID.Ki, value);
break;
case TXPIDSETTINGS_PIDS_PITCHRATEKD:
needsUpdate |= update(&stab.PitchRatePID.Kd, value);
needsUpdateBank |= update(&bank.PitchRatePID.Kd, value);
break;
case TXPIDSETTINGS_PIDS_PITCHRATEILIMIT:
needsUpdate |= update(&stab.PitchRatePID.ILimit, value);
needsUpdateBank |= update(&bank.PitchRatePID.ILimit, value);
break;
case TXPIDSETTINGS_PIDS_PITCHATTITUDEKP:
needsUpdate |= update(&stab.PitchPI.Kp, value);
needsUpdateBank |= update(&bank.PitchPI.Kp, value);
break;
case TXPIDSETTINGS_PIDS_PITCHATTITUDEKI:
needsUpdate |= update(&stab.PitchPI.Ki, value);
needsUpdateBank |= update(&bank.PitchPI.Ki, value);
break;
case TXPIDSETTINGS_PIDS_PITCHATTITUDEILIMIT:
needsUpdate |= update(&stab.PitchPI.ILimit, value);
needsUpdateBank |= update(&bank.PitchPI.ILimit, value);
break;
case TXPIDSETTINGS_PIDS_ROLLPITCHRATEKP:
needsUpdate |= update(&stab.RollRatePID.Kp, value);
needsUpdate |= update(&stab.PitchRatePID.Kp, value);
needsUpdateBank |= update(&bank.RollRatePID.Kp, value);
needsUpdateBank |= update(&bank.PitchRatePID.Kp, value);
break;
case TXPIDSETTINGS_PIDS_ROLLPITCHRATEKI:
needsUpdate |= update(&stab.RollRatePID.Ki, value);
needsUpdate |= update(&stab.PitchRatePID.Ki, value);
needsUpdateBank |= update(&bank.RollRatePID.Ki, value);
needsUpdateBank |= update(&bank.PitchRatePID.Ki, value);
break;
case TXPIDSETTINGS_PIDS_ROLLPITCHRATEKD:
needsUpdate |= update(&stab.RollRatePID.Kd, value);
needsUpdate |= update(&stab.PitchRatePID.Kd, value);
needsUpdateBank |= update(&bank.RollRatePID.Kd, value);
needsUpdateBank |= update(&bank.PitchRatePID.Kd, value);
break;
case TXPIDSETTINGS_PIDS_ROLLPITCHRATEILIMIT:
needsUpdate |= update(&stab.RollRatePID.ILimit, value);
needsUpdate |= update(&stab.PitchRatePID.ILimit, value);
needsUpdateBank |= update(&bank.RollRatePID.ILimit, value);
needsUpdateBank |= update(&bank.PitchRatePID.ILimit, value);
break;
case TXPIDSETTINGS_PIDS_ROLLPITCHATTITUDEKP:
needsUpdate |= update(&stab.RollPI.Kp, value);
needsUpdate |= update(&stab.PitchPI.Kp, value);
needsUpdateBank |= update(&bank.RollPI.Kp, value);
needsUpdateBank |= update(&bank.PitchPI.Kp, value);
break;
case TXPIDSETTINGS_PIDS_ROLLPITCHATTITUDEKI:
needsUpdate |= update(&stab.RollPI.Ki, value);
needsUpdate |= update(&stab.PitchPI.Ki, value);
needsUpdateBank |= update(&bank.RollPI.Ki, value);
needsUpdateBank |= update(&bank.PitchPI.Ki, value);
break;
case TXPIDSETTINGS_PIDS_ROLLPITCHATTITUDEILIMIT:
needsUpdate |= update(&stab.RollPI.ILimit, value);
needsUpdate |= update(&stab.PitchPI.ILimit, value);
needsUpdateBank |= update(&bank.RollPI.ILimit, value);
needsUpdateBank |= update(&bank.PitchPI.ILimit, value);
break;
case TXPIDSETTINGS_PIDS_YAWRATEKP:
needsUpdate |= update(&stab.YawRatePID.Kp, value);
needsUpdateBank |= update(&bank.YawRatePID.Kp, value);
break;
case TXPIDSETTINGS_PIDS_YAWRATEKI:
needsUpdate |= update(&stab.YawRatePID.Ki, value);
needsUpdateBank |= update(&bank.YawRatePID.Ki, value);
break;
case TXPIDSETTINGS_PIDS_YAWRATEKD:
needsUpdate |= update(&stab.YawRatePID.Kd, value);
needsUpdateBank |= update(&bank.YawRatePID.Kd, value);
break;
case TXPIDSETTINGS_PIDS_YAWRATEILIMIT:
needsUpdate |= update(&stab.YawRatePID.ILimit, value);
needsUpdateBank |= update(&bank.YawRatePID.ILimit, value);
break;
case TXPIDSETTINGS_PIDS_YAWATTITUDEKP:
needsUpdate |= update(&stab.YawPI.Kp, value);
needsUpdateBank |= update(&bank.YawPI.Kp, value);
break;
case TXPIDSETTINGS_PIDS_YAWATTITUDEKI:
needsUpdate |= update(&stab.YawPI.Ki, value);
needsUpdateBank |= update(&bank.YawPI.Ki, value);
break;
case TXPIDSETTINGS_PIDS_YAWATTITUDEILIMIT:
needsUpdate |= update(&stab.YawPI.ILimit, value);
needsUpdateBank |= update(&bank.YawPI.ILimit, value);
break;
case TXPIDSETTINGS_PIDS_GYROTAU:
needsUpdate |= update(&stab.GyroTau, value);
needsUpdateStab |= update(&stab.GyroTau, value);
break;
default:
PIOS_Assert(0);
}
}
}
if (needsUpdate) {
if (needsUpdateStab) {
StabilizationSettingsSet(&stab);
}
if (needsUpdateBank) {
switch (inst.BankNumber) {
case 0:
StabilizationSettingsBank1Set((StabilizationSettingsBank1Data *)&bank);
break;
case 1:
StabilizationSettingsBank2Set((StabilizationSettingsBank2Data *)&bank);
break;
case 2:
StabilizationSettingsBank2Set((StabilizationSettingsBank2Data *)&bank);
break;
default:
return;
}
}
}
/**

5
flight/modules/VtolPathFollower/vtolpathfollower.c
View File

@ -65,6 +65,7 @@
#include "nedaccel.h"
#include "stabilizationdesired.h"
#include "stabilizationsettings.h"
#include "stabilizationbank.h"
#include "systemsettings.h"
#include "velocitydesired.h"
#include "velocitystate.h"
@ -575,7 +576,7 @@ static void updateVtolDesiredAttitude(bool yaw_attitude)
StabilizationDesiredData stabDesired;
AttitudeStateData attitudeState;
NedAccelData nedAccel;
StabilizationSettingsData stabSettings;
StabilizationBankData stabSettings;
SystemSettingsData systemSettings;
float northError;
@ -593,7 +594,7 @@ static void updateVtolDesiredAttitude(bool yaw_attitude)
StabilizationDesiredGet(&stabDesired);
VelocityDesiredGet(&velocityDesired);
AttitudeStateGet(&attitudeState);
StabilizationSettingsGet(&stabSettings);
StabilizationBankGet(&stabSettings);
NedAccelGet(&nedAccel);
float northVel = 0, eastVel = 0, downVel = 0;

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.

34
flight/pios/common/pios_ms5611.c
View File

@ -52,6 +52,9 @@ static int32_t lastConversionStart;
static int32_t PIOS_MS5611_Read(uint8_t address, uint8_t *buffer, uint8_t len);
static int32_t PIOS_MS5611_WriteCommand(uint8_t command);
// Second order temperature compensation. Temperature offset
static int64_t compensation_t2;
// Move into proper driver structure with cfg stored
static uint32_t oversampling;
static const struct pios_ms5611_cfg *dev_cfg;
@ -74,6 +77,9 @@ void PIOS_MS5611_Init(const struct pios_ms5611_cfg *cfg, int32_t i2c_device)
uint8_t data[2];
// reset temperature compensation values
compensation_t2 = 0;
/* Calibration parameters */
for (int i = 0; i < 6; i++) {
PIOS_MS5611_Read(MS5611_CALIB_ADDR + i * 2, data, 2);
@ -190,17 +196,34 @@ int32_t PIOS_MS5611_ReadADC(void)
} else {
int64_t Offset;
int64_t Sens;
// used for second order temperature compensation
int64_t Offset2 = 0;
int64_t Sens2 = 0;
/* Read the pressure conversion */
if (PIOS_MS5611_Read(MS5611_ADC_READ, Data, 3) != 0) {
return -1;
}
// check if temperature is less than 20°C
if (Temperature < 2000) {
Offset2 = 5 * (Temperature - 2000) >> 1;
Sens2 = Offset2 >> 1;
compensation_t2 = (deltaTemp * deltaTemp) >> 31;
// Apply the "Very low temperature compensation" when temp is less than -15°C
if (Temperature < -1500) {
int64_t tcorr = (Temperature + 1500) * (Temperature + 1500);
Offset2 += 7 * tcorr;
Sens2 += (11 * tcorr) >> 1;
}
} else {
compensation_t2 = 0;
Offset2 = 0;
Sens2 = 0;
}
RawPressure = ((Data[0] << 16) | (Data[1] << 8) | Data[2]);
Offset = (((int64_t)CalibData.C[1]) << 16) + ((((int64_t)CalibData.C[3]) * deltaTemp) >> 7);
Offset = (((int64_t)CalibData.C[1]) << 16) + ((((int64_t)CalibData.C[3]) * deltaTemp) >> 7) - Offset2;
Sens = ((int64_t)CalibData.C[0]) << 15;
Sens = Sens + ((((int64_t)CalibData.C[2]) * deltaTemp) >> 8);
Sens = Sens + ((((int64_t)CalibData.C[2]) * deltaTemp) >> 8) - Sens2;
Pressure = (((((int64_t)RawPressure) * Sens) >> 21) - Offset) >> 15;
}
return 0;
@ -211,7 +234,8 @@ int32_t PIOS_MS5611_ReadADC(void)
*/
float PIOS_MS5611_GetTemperature(void)
{
return ((float)Temperature) / 100.0f;
// Apply the second order low and very low temperature compensation offset
return ((float)(Temperature - compensation_t2)) / 100.0f;
}
/**

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

4
flight/targets/boards/coptercontrol/firmware/Makefile
View File

@ -84,6 +84,10 @@ ifndef TESTAPP
SRC += $(OPUAVSYNTHDIR)/systemsettings.c
SRC += $(OPUAVSYNTHDIR)/stabilizationdesired.c
SRC += $(OPUAVSYNTHDIR)/stabilizationsettings.c
SRC += $(OPUAVSYNTHDIR)/stabilizationsettingsbank1.c
SRC += $(OPUAVSYNTHDIR)/stabilizationsettingsbank2.c
SRC += $(OPUAVSYNTHDIR)/stabilizationsettingsbank3.c
SRC += $(OPUAVSYNTHDIR)/stabilizationbank.c
SRC += $(OPUAVSYNTHDIR)/actuatorcommand.c
SRC += $(OPUAVSYNTHDIR)/actuatordesired.c
SRC += $(OPUAVSYNTHDIR)/actuatorsettings.c

2
flight/targets/boards/coptercontrol/firmware/inc/pios_config.h
View File

@ -156,7 +156,7 @@
#define PIOS_ACTUATOR_STACK_SIZE 1020
#define PIOS_MANUAL_STACK_SIZE 800
#define PIOS_SYSTEM_STACK_SIZE 660
#define PIOS_STABILIZATION_STACK_SIZE 524
#define PIOS_STABILIZATION_STACK_SIZE 790
#define PIOS_TELEM_STACK_SIZE 800
#define PIOS_EVENTDISPATCHER_STACK_SIZE 130

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

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

@ -23,7 +23,6 @@ UAVOBJSRCFILENAMES += accessorydesired
UAVOBJSRCFILENAMES += actuatorcommand
UAVOBJSRCFILENAMES += actuatordesired
UAVOBJSRCFILENAMES += actuatorsettings
UAVOBJSRCFILENAMES += altholdsmoothed
UAVOBJSRCFILENAMES += attitudesettings
UAVOBJSRCFILENAMES += attitudestate
UAVOBJSRCFILENAMES += gyrostate
@ -70,6 +69,7 @@ UAVOBJSRCFILENAMES += overosyncstats
UAVOBJSRCFILENAMES += overosyncsettings
UAVOBJSRCFILENAMES += pathaction
UAVOBJSRCFILENAMES += pathdesired
UAVOBJSRCFILENAMES += pathplan
UAVOBJSRCFILENAMES += pathstatus
UAVOBJSRCFILENAMES += positionstate
UAVOBJSRCFILENAMES += ratedesired
@ -82,6 +82,10 @@ UAVOBJSRCFILENAMES += revosettings
UAVOBJSRCFILENAMES += sonaraltitude
UAVOBJSRCFILENAMES += stabilizationdesired
UAVOBJSRCFILENAMES += stabilizationsettings
UAVOBJSRCFILENAMES += stabilizationsettingsbank1
UAVOBJSRCFILENAMES += stabilizationsettingsbank2
UAVOBJSRCFILENAMES += stabilizationsettingsbank3
UAVOBJSRCFILENAMES += stabilizationbank
UAVOBJSRCFILENAMES += systemalarms
UAVOBJSRCFILENAMES += systemsettings
UAVOBJSRCFILENAMES += systemstats
@ -99,6 +103,7 @@ UAVOBJSRCFILENAMES += oplinksettings
UAVOBJSRCFILENAMES += oplinkstatus
UAVOBJSRCFILENAMES += altitudefiltersettings
UAVOBJSRCFILENAMES += altitudeholddesired
UAVOBJSRCFILENAMES += altitudeholdstatus
UAVOBJSRCFILENAMES += waypoint
UAVOBJSRCFILENAMES += waypointactive
UAVOBJSRCFILENAMES += poilocation

2
flight/targets/boards/revolution/firmware/pios_board.c
View File

@ -133,7 +133,7 @@ static const struct pios_hmc5883_cfg pios_hmc5883_cfg = {
#if defined(PIOS_INCLUDE_MS5611)
#include "pios_ms5611.h"
static const struct pios_ms5611_cfg pios_ms5611_cfg = {
.oversampling = MS5611_OSR_512,
.oversampling = MS5611_OSR_4096,
};
#endif /* PIOS_INCLUDE_MS5611 */

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

@ -4,7 +4,7 @@
# Makefile for OpenPilot UAVObject code
#
# The OpenPilot Team, http://www.openpilot.org, Copyright (C) 2011.
#
#
# 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
@ -28,7 +28,6 @@ UAVOBJSRCFILENAMES += accessorydesired
UAVOBJSRCFILENAMES += actuatorcommand
UAVOBJSRCFILENAMES += actuatordesired
UAVOBJSRCFILENAMES += actuatorsettings
UAVOBJSRCFILENAMES += altholdsmoothed
UAVOBJSRCFILENAMES += attitudesettings
UAVOBJSRCFILENAMES += attitudestate
UAVOBJSRCFILENAMES += gyrostate
@ -70,6 +69,7 @@ UAVOBJSRCFILENAMES += overosyncstats
UAVOBJSRCFILENAMES += overosyncsettings
UAVOBJSRCFILENAMES += pathaction
UAVOBJSRCFILENAMES += pathdesired
UAVOBJSRCFILENAMES += pathplan
UAVOBJSRCFILENAMES += pathstatus
UAVOBJSRCFILENAMES += positionstate
UAVOBJSRCFILENAMES += ratedesired
@ -82,6 +82,10 @@ UAVOBJSRCFILENAMES += revosettings
UAVOBJSRCFILENAMES += sonaraltitude
UAVOBJSRCFILENAMES += stabilizationdesired
UAVOBJSRCFILENAMES += stabilizationsettings
UAVOBJSRCFILENAMES += stabilizationsettingsbank1
UAVOBJSRCFILENAMES += stabilizationsettingsbank2
UAVOBJSRCFILENAMES += stabilizationsettingsbank3
UAVOBJSRCFILENAMES += stabilizationbank
UAVOBJSRCFILENAMES += systemalarms
UAVOBJSRCFILENAMES += systemsettings
UAVOBJSRCFILENAMES += systemstats
@ -97,6 +101,7 @@ UAVOBJSRCFILENAMES += camerastabsettings
UAVOBJSRCFILENAMES += altitudeholdsettings
UAVOBJSRCFILENAMES += altitudefiltersettings
UAVOBJSRCFILENAMES += altitudeholddesired
UAVOBJSRCFILENAMES += altitudeholdstatus
UAVOBJSRCFILENAMES += waypoint
UAVOBJSRCFILENAMES += waypointactive
UAVOBJSRCFILENAMES += poilocation

2
flight/targets/boards/revoproto/firmware/pios_board.c
View File

@ -129,7 +129,7 @@ static const struct pios_hmc5883_cfg pios_hmc5883_cfg = {
#if defined(PIOS_INCLUDE_MS5611)
#include "pios_ms5611.h"
static const struct pios_ms5611_cfg pios_ms5611_cfg = {
.oversampling = MS5611_OSR_512,
.oversampling = MS5611_OSR_4096,
};
#endif /* PIOS_INCLUDE_MS5611 */

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

@ -42,6 +42,7 @@ MODULES += FirmwareIAP
MODULES += StateEstimation
#MODULES += Sensors/simulated/Sensors
MODULES += Airspeed
MODULES += AltitudeHold
#MODULES += OveroSync
# Paths
@ -68,7 +69,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

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

@ -4,7 +4,7 @@
# Makefile for OpenPilot UAVObject code
#
# The OpenPilot Team, http://www.openpilot.org, Copyright (C) 2011.
#
#
# 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
@ -28,7 +28,6 @@ UAVOBJSRCFILENAMES += accessorydesired
UAVOBJSRCFILENAMES += actuatorcommand
UAVOBJSRCFILENAMES += actuatordesired
UAVOBJSRCFILENAMES += actuatorsettings
UAVOBJSRCFILENAMES += altholdsmoothed
UAVOBJSRCFILENAMES += attitudesettings
UAVOBJSRCFILENAMES += attitudestate
UAVOBJSRCFILENAMES += attitudesimulated
@ -73,6 +72,7 @@ UAVOBJSRCFILENAMES += objectpersistence
UAVOBJSRCFILENAMES += overosyncstats
UAVOBJSRCFILENAMES += pathaction
UAVOBJSRCFILENAMES += pathdesired
UAVOBJSRCFILENAMES += pathplan
UAVOBJSRCFILENAMES += pathstatus
UAVOBJSRCFILENAMES += positionstate
UAVOBJSRCFILENAMES += ratedesired
@ -82,6 +82,10 @@ UAVOBJSRCFILENAMES += relaytuningsettings
UAVOBJSRCFILENAMES += sonaraltitude
UAVOBJSRCFILENAMES += stabilizationdesired
UAVOBJSRCFILENAMES += stabilizationsettings
UAVOBJSRCFILENAMES += stabilizationsettingsbank1
UAVOBJSRCFILENAMES += stabilizationsettingsbank2
UAVOBJSRCFILENAMES += stabilizationsettingsbank3
UAVOBJSRCFILENAMES += stabilizationbank
UAVOBJSRCFILENAMES += systemalarms
UAVOBJSRCFILENAMES += systemsettings
UAVOBJSRCFILENAMES += systemstats
@ -98,10 +102,11 @@ UAVOBJSRCFILENAMES += hwsettings
UAVOBJSRCFILENAMES += receiveractivity
UAVOBJSRCFILENAMES += cameradesired
UAVOBJSRCFILENAMES += camerastabsettings
UAVOBJSRCFILENAMES += altitudeholdsettings
UAVOBJSRCFILENAMES += altitudefiltersettings
UAVOBJSRCFILENAMES += altitudeholdsettings
UAVOBJSRCFILENAMES += altitudefiltersettings
UAVOBJSRCFILENAMES += revosettings
UAVOBJSRCFILENAMES += altitudeholddesired
UAVOBJSRCFILENAMES += altitudeholdstatus
UAVOBJSRCFILENAMES += ekfconfiguration
UAVOBJSRCFILENAMES += ekfstatevariance

1
flight/uavobjects/eventdispatcher.c
View File

@ -104,6 +104,7 @@ int32_t EventDispatcherInitialize()
// Create callback
eventSchedulerCallback = DelayedCallbackCreate(&eventTask, CALLBACK_PRIORITY, TASK_PRIORITY, STACK_SIZE * 4);
DelayedCallbackDispatch(eventSchedulerCallback);
// Done
return 0;

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

22
ground/openpilotgcs/src/libs/utils/mytabbedstackwidget.cpp
View File

@ -30,6 +30,7 @@
#include <QHBoxLayout>
#include <QLabel>
#include <QtCore/QDebug>
#include <QScrollBar>
MyTabbedStackWidget::MyTabbedStackWidget(QWidget *parent, bool isVertical, bool iconAbove)
: QWidget(parent),
@ -37,8 +38,8 @@ MyTabbedStackWidget::MyTabbedStackWidget(QWidget *parent, bool isVertical, bool
m_iconAbove(iconAbove)
{
m_listWidget = new QListWidget();
m_listWidget->setObjectName("list");
m_stackWidget = new QStackedWidget();
m_stackWidget->setSizePolicy(QSizePolicy::Minimum, QSizePolicy::Expanding);
QBoxLayout *toplevelLayout;
if (m_vertical) {
@ -58,16 +59,22 @@ MyTabbedStackWidget::MyTabbedStackWidget(QWidget *parent, bool isVertical, bool
}
if (m_iconAbove && m_vertical) {
m_listWidget->setFixedWidth(80); // this should be computed instead
m_listWidget->setFixedWidth(LIST_VIEW_WIDTH); // this should be computed instead
m_listWidget->setWrapping(false);
}
toplevelLayout->setSpacing(0);
toplevelLayout->setContentsMargins(0, 0, 0, 0);
m_listWidget->setContentsMargins(0, 0, 0, 0);
m_listWidget->setSpacing(0);
m_listWidget->setViewMode(QListView::IconMode);
m_listWidget->setMovement(QListView::Static);
m_listWidget->setUniformItemSizes(true);
m_listWidget->setStyleSheet("#list {border: 0px; margin-left: 9px; margin-top: 9px; background-color: transparent; }");
m_stackWidget->setSizePolicy(QSizePolicy::Minimum, QSizePolicy::Expanding);
m_stackWidget->setContentsMargins(0, 0, 0, 0);
toplevelLayout->setSpacing(0);
toplevelLayout->setContentsMargins(0, 0, 0, 0);
setLayout(toplevelLayout);
connect(m_listWidget, SIGNAL(currentRowChanged(int)), this, SLOT(showWidget(int)), Qt::QueuedConnection);
@ -127,6 +134,13 @@ void MyTabbedStackWidget::showWidget(int index)
}
}
void MyTabbedStackWidget::resizeEvent(QResizeEvent *event)
{
QWidget::resizeEvent(event);
m_listWidget->setFixedWidth(m_listWidget->verticalScrollBar()->isVisible() ? LIST_VIEW_WIDTH + 20 : LIST_VIEW_WIDTH);
}
void MyTabbedStackWidget::insertCornerWidget(int index, QWidget *widget)
{
Q_UNUSED(index);

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

@ -76,6 +76,10 @@ private:
QStackedWidget *m_stackWidget;
bool m_vertical;
bool m_iconAbove;
static const int LIST_VIEW_WIDTH = 80;
protected:
void resizeEvent(QResizeEvent *event);
};
#endif // MYTABBEDSTACKWIDGET_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>

16
ground/openpilotgcs/src/plugins/config/config_cc_hw_widget.cpp
View File

@ -75,14 +75,14 @@ ConfigCCHWWidget::ConfigCCHWWidget(QWidget *parent) : ConfigTaskWidget(parent)
break;
}
addApplySaveButtons(m_telemetry->saveTelemetryToRAM, m_telemetry->saveTelemetryToSD);
addUAVObjectToWidgetRelation("HwSettings", "CC_FlexiPort", m_telemetry->cbFlexi);
addUAVObjectToWidgetRelation("HwSettings", "CC_MainPort", m_telemetry->cbTele);
addUAVObjectToWidgetRelation("HwSettings", "CC_RcvrPort", m_telemetry->cbRcvr);
addUAVObjectToWidgetRelation("HwSettings", "USB_HIDPort", m_telemetry->cbUsbHid);
addUAVObjectToWidgetRelation("HwSettings", "USB_VCPPort", m_telemetry->cbUsbVcp);
addUAVObjectToWidgetRelation("HwSettings", "TelemetrySpeed", m_telemetry->telemetrySpeed);
addUAVObjectToWidgetRelation("HwSettings", "GPSSpeed", m_telemetry->gpsSpeed);
addUAVObjectToWidgetRelation("HwSettings", "ComUsbBridgeSpeed", m_telemetry->comUsbBridgeSpeed);
addWidgetBinding("HwSettings", "CC_FlexiPort", m_telemetry->cbFlexi);
addWidgetBinding("HwSettings", "CC_MainPort", m_telemetry->cbTele);
addWidgetBinding("HwSettings", "CC_RcvrPort", m_telemetry->cbRcvr);
addWidgetBinding("HwSettings", "USB_HIDPort", m_telemetry->cbUsbHid);
addWidgetBinding("HwSettings", "USB_VCPPort", m_telemetry->cbUsbVcp);
addWidgetBinding("HwSettings", "TelemetrySpeed", m_telemetry->telemetrySpeed);
addWidgetBinding("HwSettings", "GPSSpeed", m_telemetry->gpsSpeed);
addWidgetBinding("HwSettings", "ComUsbBridgeSpeed", m_telemetry->comUsbBridgeSpeed);
connect(m_telemetry->cchwHelp, SIGNAL(clicked()), this, SLOT(openHelp()));
enableSaveButtons(false);
populateWidgets();

10
ground/openpilotgcs/src/plugins/config/configccattitudewidget.cpp
View File

@ -58,12 +58,12 @@ ConfigCCAttitudeWidget::ConfigCCAttitudeWidget(QWidget *parent) :
// Connect the help button
connect(ui->ccAttitudeHelp, SIGNAL(clicked()), this, SLOT(openHelp()));
addUAVObjectToWidgetRelation("AttitudeSettings", "ZeroDuringArming", ui->zeroGyroBiasOnArming);
addUAVObjectToWidgetRelation("AttitudeSettings", "AccelTau", ui->accelTauSpinbox);
addWidgetBinding("AttitudeSettings", "ZeroDuringArming", ui->zeroGyroBiasOnArming);
addWidgetBinding("AttitudeSettings", "AccelTau", ui->accelTauSpinbox);
addUAVObjectToWidgetRelation("AttitudeSettings", "BoardRotation", ui->rollBias, AttitudeSettings::BOARDROTATION_ROLL);
addUAVObjectToWidgetRelation("AttitudeSettings", "BoardRotation", ui->pitchBias, AttitudeSettings::BOARDROTATION_PITCH);
addUAVObjectToWidgetRelation("AttitudeSettings", "BoardRotation", ui->yawBias, AttitudeSettings::BOARDROTATION_YAW);
addWidgetBinding("AttitudeSettings", "BoardRotation", ui->rollBias, AttitudeSettings::BOARDROTATION_ROLL);
addWidgetBinding("AttitudeSettings", "BoardRotation", ui->pitchBias, AttitudeSettings::BOARDROTATION_PITCH);
addWidgetBinding("AttitudeSettings", "BoardRotation", ui->yawBias, AttitudeSettings::BOARDROTATION_YAW);
addWidget(ui->zeroBias);
refreshWidgetsValues();
}

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

60
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
@ -84,11 +82,11 @@ ConfigInputWidget::ConfigInputWidget(QWidget *parent) :
inputChannelForm *inpForm = new inputChannelForm(this, index == 0);
ui->channelSettings->layout()->addWidget(inpForm); // Add the row to the UI
inpForm->setName(name);
addUAVObjectToWidgetRelation("ManualControlSettings", "ChannelGroups", inpForm->ui->channelGroup, index);
addUAVObjectToWidgetRelation("ManualControlSettings", "ChannelNumber", inpForm->ui->channelNumber, index);
addUAVObjectToWidgetRelation("ManualControlSettings", "ChannelMin", inpForm->ui->channelMin, index);
addUAVObjectToWidgetRelation("ManualControlSettings", "ChannelNeutral", inpForm->ui->channelNeutral, index);
addUAVObjectToWidgetRelation("ManualControlSettings", "ChannelMax", inpForm->ui->channelMax, index);
addWidgetBinding("ManualControlSettings", "ChannelGroups", inpForm->ui->channelGroup, index);
addWidgetBinding("ManualControlSettings", "ChannelNumber", inpForm->ui->channelNumber, index);
addWidgetBinding("ManualControlSettings", "ChannelMin", inpForm->ui->channelMin, index);
addWidgetBinding("ManualControlSettings", "ChannelNeutral", inpForm->ui->channelNeutral, index);
addWidgetBinding("ManualControlSettings", "ChannelMax", inpForm->ui->channelMax, index);
addWidget(inpForm->ui->channelNumberDropdown);
addWidget(inpForm->ui->channelRev);
addWidget(inpForm->ui->channelResponseTime);
@ -101,7 +99,7 @@ ConfigInputWidget::ConfigInputWidget(QWidget *parent) :
case ManualControlSettings::CHANNELGROUPS_ACCESSORY0:
case ManualControlSettings::CHANNELGROUPS_ACCESSORY1:
case ManualControlSettings::CHANNELGROUPS_ACCESSORY2:
addUAVObjectToWidgetRelation("ManualControlSettings", "ResponseTime", inpForm->ui->channelResponseTime, indexRT);
addWidgetBinding("ManualControlSettings", "ResponseTime", inpForm->ui->channelResponseTime, indexRT);
++indexRT;
break;
case ManualControlSettings::CHANNELGROUPS_THROTTLE:
@ -117,7 +115,7 @@ ConfigInputWidget::ConfigInputWidget(QWidget *parent) :
++index;
}
addUAVObjectToWidgetRelation("ManualControlSettings", "Deadband", ui->deadband, 0, 0.01f);
addWidgetBinding("ManualControlSettings", "Deadband", ui->deadband, 0, 0.01f);
connect(ui->configurationWizard, SIGNAL(clicked()), this, SLOT(goToWizard()));
connect(ui->stackedWidget, SIGNAL(currentChanged(int)), this, SLOT(disableWizardButton(int)));
@ -128,26 +126,30 @@ ConfigInputWidget::ConfigInputWidget(QWidget *parent) :
connect(ui->wzBack, SIGNAL(clicked()), this, SLOT(wzBack()));
ui->stackedWidget->setCurrentIndex(0);
addUAVObjectToWidgetRelation("ManualControlSettings", "FlightModePosition", ui->fmsModePos1, 0, 1, true);
addUAVObjectToWidgetRelation("ManualControlSettings", "FlightModePosition", ui->fmsModePos2, 1, 1, true);
addUAVObjectToWidgetRelation("ManualControlSettings", "FlightModePosition", ui->fmsModePos3, 2, 1, true);
addUAVObjectToWidgetRelation("ManualControlSettings", "FlightModePosition", ui->fmsModePos4, 3, 1, true);
addUAVObjectToWidgetRelation("ManualControlSettings", "FlightModePosition", ui->fmsModePos5, 4, 1, true);
addUAVObjectToWidgetRelation("ManualControlSettings", "FlightModePosition", ui->fmsModePos6, 5, 1, true);
addUAVObjectToWidgetRelation("ManualControlSettings", "FlightModeNumber", ui->fmsPosNum);
addWidgetBinding("ManualControlSettings", "FlightModePosition", ui->fmsModePos1, 0, 1, true);
addWidgetBinding("ManualControlSettings", "FlightModePosition", ui->fmsModePos2, 1, 1, true);
addWidgetBinding("ManualControlSettings", "FlightModePosition", ui->fmsModePos3, 2, 1, true);
addWidgetBinding("ManualControlSettings", "FlightModePosition", ui->fmsModePos4, 3, 1, true);
addWidgetBinding("ManualControlSettings", "FlightModePosition", ui->fmsModePos5, 4, 1, true);
addWidgetBinding("ManualControlSettings", "FlightModePosition", ui->fmsModePos6, 5, 1, true);
addWidgetBinding("ManualControlSettings", "FlightModeNumber", ui->fmsPosNum);
addUAVObjectToWidgetRelation("ManualControlSettings", "Stabilization1Settings", ui->fmsSsPos1Roll, "Roll", 1, true);
addUAVObjectToWidgetRelation("ManualControlSettings", "Stabilization2Settings", ui->fmsSsPos2Roll, "Roll", 1, true);
addUAVObjectToWidgetRelation("ManualControlSettings", "Stabilization3Settings", ui->fmsSsPos3Roll, "Roll", 1, true);
addUAVObjectToWidgetRelation("ManualControlSettings", "Stabilization1Settings", ui->fmsSsPos1Pitch, "Pitch", 1, true);
addUAVObjectToWidgetRelation("ManualControlSettings", "Stabilization2Settings", ui->fmsSsPos2Pitch, "Pitch", 1, true);
addUAVObjectToWidgetRelation("ManualControlSettings", "Stabilization3Settings", ui->fmsSsPos3Pitch, "Pitch", 1, true);
addUAVObjectToWidgetRelation("ManualControlSettings", "Stabilization1Settings", ui->fmsSsPos1Yaw, "Yaw", 1, true);
addUAVObjectToWidgetRelation("ManualControlSettings", "Stabilization2Settings", ui->fmsSsPos2Yaw, "Yaw", 1, true);
addUAVObjectToWidgetRelation("ManualControlSettings", "Stabilization3Settings", ui->fmsSsPos3Yaw, "Yaw", 1, true);
addWidgetBinding("ManualControlSettings", "Stabilization1Settings", ui->fmsSsPos1Roll, "Roll", 1, true);
addWidgetBinding("ManualControlSettings", "Stabilization2Settings", ui->fmsSsPos2Roll, "Roll", 1, true);
addWidgetBinding("ManualControlSettings", "Stabilization3Settings", ui->fmsSsPos3Roll, "Roll", 1, true);
addWidgetBinding("ManualControlSettings", "Stabilization1Settings", ui->fmsSsPos1Pitch, "Pitch", 1, true);
addWidgetBinding("ManualControlSettings", "Stabilization2Settings", ui->fmsSsPos2Pitch, "Pitch", 1, true);
addWidgetBinding("ManualControlSettings", "Stabilization3Settings", ui->fmsSsPos3Pitch, "Pitch", 1, true);
addWidgetBinding("ManualControlSettings", "Stabilization1Settings", ui->fmsSsPos1Yaw, "Yaw", 1, true);
addWidgetBinding("ManualControlSettings", "Stabilization2Settings", ui->fmsSsPos2Yaw, "Yaw", 1, true);
addWidgetBinding("ManualControlSettings", "Stabilization3Settings", ui->fmsSsPos3Yaw, "Yaw", 1, true);
addUAVObjectToWidgetRelation("ManualControlSettings", "Arming", ui->armControl);
addUAVObjectToWidgetRelation("ManualControlSettings", "ArmedTimeout", ui->armTimeout, 0, 1000);
addWidgetBinding("StabilizationSettings", "FlightModeMap", ui->pidBankSs1, "Stabilized1", 1, true);
addWidgetBinding("StabilizationSettings", "FlightModeMap", ui->pidBankSs2, "Stabilized2", 1, true);
addWidgetBinding("StabilizationSettings", "FlightModeMap", ui->pidBankSs3, "Stabilized3", 1, true);
addWidgetBinding("ManualControlSettings", "Arming", ui->armControl);
addWidgetBinding("ManualControlSettings", "ArmedTimeout", ui->armTimeout, 0, 1000);
connect(ManualControlCommand::GetInstance(getObjectManager()), SIGNAL(objectUpdated(UAVObject *)), this, SLOT(moveFMSlider()));
connect(ManualControlSettings::GetInstance(getObjectManager()), SIGNAL(objectUpdated(UAVObject *)), this, SLOT(updatePositionSlider()));

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

125
ground/openpilotgcs/src/plugins/config/configpipxtremewidget.cpp
View File

@ -60,39 +60,39 @@ ConfigPipXtremeWidget::ConfigPipXtremeWidget(QWidget *parent) : ConfigTaskWidget
}
addApplySaveButtons(m_oplink->Apply, m_oplink->Save);
addUAVObjectToWidgetRelation("OPLinkSettings", "MainPort", m_oplink->MainPort);
addUAVObjectToWidgetRelation("OPLinkSettings", "FlexiPort", m_oplink->FlexiPort);
addUAVObjectToWidgetRelation("OPLinkSettings", "VCPPort", m_oplink->VCPPort);
addUAVObjectToWidgetRelation("OPLinkSettings", "ComSpeed", m_oplink->ComSpeed);
addUAVObjectToWidgetRelation("OPLinkSettings", "MaxRFPower", m_oplink->MaxRFTxPower);
addUAVObjectToWidgetRelation("OPLinkSettings", "MinChannel", m_oplink->MinimumChannel);
addUAVObjectToWidgetRelation("OPLinkSettings", "MaxChannel", m_oplink->MaximumChannel);
addUAVObjectToWidgetRelation("OPLinkSettings", "ChannelSet", m_oplink->ChannelSet);
addUAVObjectToWidgetRelation("OPLinkSettings", "CoordID", m_oplink->CoordID);
addUAVObjectToWidgetRelation("OPLinkSettings", "Coordinator", m_oplink->Coordinator);
addUAVObjectToWidgetRelation("OPLinkSettings", "OneWay", m_oplink->OneWayLink);
addUAVObjectToWidgetRelation("OPLinkSettings", "PPMOnly", m_oplink->PPMOnly);
addUAVObjectToWidgetRelation("OPLinkSettings", "PPM", m_oplink->PPM);
addWidgetBinding("OPLinkSettings", "MainPort", m_oplink->MainPort);
addWidgetBinding("OPLinkSettings", "FlexiPort", m_oplink->FlexiPort);
addWidgetBinding("OPLinkSettings", "VCPPort", m_oplink->VCPPort);
addWidgetBinding("OPLinkSettings", "ComSpeed", m_oplink->ComSpeed);
addWidgetBinding("OPLinkSettings", "MaxRFPower", m_oplink->MaxRFTxPower);
addWidgetBinding("OPLinkSettings", "MinChannel", m_oplink->MinimumChannel);
addWidgetBinding("OPLinkSettings", "MaxChannel", m_oplink->MaximumChannel);
addWidgetBinding("OPLinkSettings", "ChannelSet", m_oplink->ChannelSet);
addWidgetBinding("OPLinkSettings", "CoordID", m_oplink->CoordID);
addWidgetBinding("OPLinkSettings", "Coordinator", m_oplink->Coordinator);
addWidgetBinding("OPLinkSettings", "OneWay", m_oplink->OneWayLink);
addWidgetBinding("OPLinkSettings", "PPMOnly", m_oplink->PPMOnly);
addWidgetBinding("OPLinkSettings", "PPM", m_oplink->PPM);
addUAVObjectToWidgetRelation("OPLinkStatus", "DeviceID", m_oplink->DeviceID);
addUAVObjectToWidgetRelation("OPLinkStatus", "RxGood", m_oplink->Good);
addUAVObjectToWidgetRelation("OPLinkStatus", "RxCorrected", m_oplink->Corrected);
addUAVObjectToWidgetRelation("OPLinkStatus", "RxErrors", m_oplink->Errors);
addUAVObjectToWidgetRelation("OPLinkStatus", "RxMissed", m_oplink->Missed);
addUAVObjectToWidgetRelation("OPLinkStatus", "RxFailure", m_oplink->RxFailure);
addUAVObjectToWidgetRelation("OPLinkStatus", "UAVTalkErrors", m_oplink->UAVTalkErrors);
addUAVObjectToWidgetRelation("OPLinkStatus", "TxDropped", m_oplink->Dropped);
addUAVObjectToWidgetRelation("OPLinkStatus", "TxResent", m_oplink->Resent);
addUAVObjectToWidgetRelation("OPLinkStatus", "TxFailure", m_oplink->TxFailure);
addUAVObjectToWidgetRelation("OPLinkStatus", "Resets", m_oplink->Resets);
addUAVObjectToWidgetRelation("OPLinkStatus", "Timeouts", m_oplink->Timeouts);
addUAVObjectToWidgetRelation("OPLinkStatus", "RSSI", m_oplink->RSSI);
addUAVObjectToWidgetRelation("OPLinkStatus", "HeapRemaining", m_oplink->FreeHeap);
addUAVObjectToWidgetRelation("OPLinkStatus", "LinkQuality", m_oplink->LinkQuality);
addUAVObjectToWidgetRelation("OPLinkStatus", "RXSeq", m_oplink->RXSeq);
addUAVObjectToWidgetRelation("OPLinkStatus", "TXSeq", m_oplink->TXSeq);
addUAVObjectToWidgetRelation("OPLinkStatus", "RXRate", m_oplink->RXRate);
addUAVObjectToWidgetRelation("OPLinkStatus", "TXRate", m_oplink->TXRate);
addWidgetBinding("OPLinkStatus", "DeviceID", m_oplink->DeviceID);
addWidgetBinding("OPLinkStatus", "RxGood", m_oplink->Good);
addWidgetBinding("OPLinkStatus", "RxCorrected", m_oplink->Corrected);
addWidgetBinding("OPLinkStatus", "RxErrors", m_oplink->Errors);
addWidgetBinding("OPLinkStatus", "RxMissed", m_oplink->Missed);
addWidgetBinding("OPLinkStatus", "RxFailure", m_oplink->RxFailure);
addWidgetBinding("OPLinkStatus", "UAVTalkErrors", m_oplink->UAVTalkErrors);
addWidgetBinding("OPLinkStatus", "TxDropped", m_oplink->Dropped);
addWidgetBinding("OPLinkStatus", "TxResent", m_oplink->Resent);
addWidgetBinding("OPLinkStatus", "TxFailure", m_oplink->TxFailure);
addWidgetBinding("OPLinkStatus", "Resets", m_oplink->Resets);
addWidgetBinding("OPLinkStatus", "Timeouts", m_oplink->Timeouts);
addWidgetBinding("OPLinkStatus", "RSSI", m_oplink->RSSI);
addWidgetBinding("OPLinkStatus", "HeapRemaining", m_oplink->FreeHeap);
addWidgetBinding("OPLinkStatus", "LinkQuality", m_oplink->LinkQuality);
addWidgetBinding("OPLinkStatus", "RXSeq", m_oplink->RXSeq);
addWidgetBinding("OPLinkStatus", "TXSeq", m_oplink->TXSeq);
addWidgetBinding("OPLinkStatus", "RXRate", m_oplink->RXRate);
addWidgetBinding("OPLinkStatus", "TXRate", m_oplink->TXRate);
// Connect the bind buttons
connect(m_oplink->Bind1, SIGNAL(clicked()), this, SLOT(bind1()));
@ -151,7 +151,7 @@ void ConfigPipXtremeWidget::updateStatus(UAVObject *object)
m_oplink->PairID4->setEnabled(false);
m_oplink->Bind4->setEnabled(pairid4);
} else {
qDebug() << "PipXtremeGadgetWidget: Count not read PairID field.";
qDebug() << "ConfigPipXtremeWidget: Count not read PairID field.";
}
UAVObjectField *pairRssiField = object->getField("PairSignalStrengths");
if (pairRssiField) {
@ -164,38 +164,43 @@ void ConfigPipXtremeWidget::updateStatus(UAVObject *object)
m_oplink->PairSignalStrengthLabel3->setText(QString("%1dB").arg(pairRssiField->getValue(2).toInt()));
m_oplink->PairSignalStrengthLabel4->setText(QString("%1dB").arg(pairRssiField->getValue(3).toInt()));
} else {
qDebug() << "PipXtremeGadgetWidget: Count not read PairID field.";
qDebug() << "ConfigPipXtremeWidget: Count not read PairID field.";
}
// Update the Description field
// TODO use UAVObjectUtilManager::descriptionToStructure()
UAVObjectField *descField = object->getField("Description");
if (descField) {
/*
* This looks like a binary with a description at the end:
* 4 bytes: header: "OpFw".
* 4 bytes: GIT commit tag (short version of SHA1).
* 4 bytes: Unix timestamp of compile time.
* 2 bytes: target platform. Should follow same rule as BOARD_TYPE and BOARD_REVISION in board define files.
* 26 bytes: commit tag if it is there, otherwise branch name. '-dirty' may be added if needed. Zero-padded.
* 20 bytes: SHA1 sum of the firmware.
* 20 bytes: SHA1 sum of the uavo definitions.
* 20 bytes: free for now.
*/
char buf[OPLinkStatus::DESCRIPTION_NUMELEM];
for (unsigned int i = 0; i < 26; ++i) {
buf[i] = descField->getValue(i + 14).toChar().toLatin1();
if (descField->getValue(0) != QChar(255)) {
/*
* This looks like a binary with a description at the end:
* 4 bytes: header: "OpFw".
* 4 bytes: GIT commit tag (short version of SHA1).
* 4 bytes: Unix timestamp of compile time.
* 2 bytes: target platform. Should follow same rule as BOARD_TYPE and BOARD_REVISION in board define files.
* 26 bytes: commit tag if it is there, otherwise branch name. '-dirty' may be added if needed. Zero-padded.
* 20 bytes: SHA1 sum of the firmware.
* 20 bytes: SHA1 sum of the uavo definitions.
* 20 bytes: free for now.
*/
char buf[OPLinkStatus::DESCRIPTION_NUMELEM];
for (unsigned int i = 0; i < 26; ++i) {
buf[i] = descField->getValue(i + 14).toChar().toLatin1();
}
buf[26] = '\0';
QString descstr(buf);
quint32 gitDate = descField->getValue(11).toChar().toLatin1() & 0xFF;
for (int i = 1; i < 4; i++) {
gitDate = gitDate << 8;
gitDate += descField->getValue(11 - i).toChar().toLatin1() & 0xFF;
}
QString date = QDateTime::fromTime_t(gitDate).toUTC().toString("yyyy-MM-dd HH:mm");
m_oplink->FirmwareVersion->setText(descstr + " " + date);
} else {
m_oplink->FirmwareVersion->setText(tr("Unknown"));
}
buf[26] = '\0';
QString descstr(buf);
quint32 gitDate = descField->getValue(11).toChar().toLatin1() & 0xFF;
for (int i = 1; i < 4; i++) {
gitDate = gitDate << 8;
gitDate += descField->getValue(11 - i).toChar().toLatin1() & 0xFF;
}
QString date = QDateTime::fromTime_t(gitDate).toUTC().toString("yyyy-MM-dd HH:mm");
m_oplink->FirmwareVersion->setText(descstr + " " + date);
} else {
qDebug() << "PipXtremeGadgetWidget: Count not read Description field.";
qDebug() << "ConfigPipXtremeWidget: Failed to read Description field.";
}
// Update the serial number field
@ -211,7 +216,7 @@ void ConfigPipXtremeWidget::updateStatus(UAVObject *object)
buf[OPLinkStatus::CPUSERIAL_NUMELEM * 2] = '\0';
m_oplink->SerialNumber->setText(buf);
} else {
qDebug() << "PipXtremeGadgetWidget: Count not read Description field.";
qDebug() << "ConfigPipXtremeWidget: Failed to read CPUSerial field.";
}
// Update the link state
@ -219,7 +224,7 @@ void ConfigPipXtremeWidget::updateStatus(UAVObject *object)
if (linkField) {
m_oplink->LinkState->setText(linkField->getValue().toString());
} else {
qDebug() << "PipXtremeGadgetWidget: Count not read link state field.";
qDebug() << "ConfigPipXtremeWidget: Failed to read LinkState field.";
}
}

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>

24
ground/openpilotgcs/src/plugins/config/configrevohwwidget.cpp
View File

@ -49,21 +49,21 @@ ConfigRevoHWWidget::ConfigRevoHWWidget(QWidget *parent) : ConfigTaskWidget(paren
addApplySaveButtons(m_ui->saveTelemetryToRAM, m_ui->saveTelemetryToSD);
addUAVObjectToWidgetRelation("HwSettings", "RM_FlexiPort", m_ui->cbFlexi);
addUAVObjectToWidgetRelation("HwSettings", "RM_MainPort", m_ui->cbMain);
addUAVObjectToWidgetRelation("HwSettings", "RM_RcvrPort", m_ui->cbRcvr);
addWidgetBinding("HwSettings", "RM_FlexiPort", m_ui->cbFlexi);
addWidgetBinding("HwSettings", "RM_MainPort", m_ui->cbMain);
addWidgetBinding("HwSettings", "RM_RcvrPort", m_ui->cbRcvr);
addUAVObjectToWidgetRelation("HwSettings", "USB_HIDPort", m_ui->cbUSBHIDFunction);
addUAVObjectToWidgetRelation("HwSettings", "USB_VCPPort", m_ui->cbUSBVCPFunction);
addUAVObjectToWidgetRelation("HwSettings", "ComUsbBridgeSpeed", m_ui->cbUSBVCPSpeed);
addWidgetBinding("HwSettings", "USB_HIDPort", m_ui->cbUSBHIDFunction);
addWidgetBinding("HwSettings", "USB_VCPPort", m_ui->cbUSBVCPFunction);
addWidgetBinding("HwSettings", "ComUsbBridgeSpeed", m_ui->cbUSBVCPSpeed);
addUAVObjectToWidgetRelation("HwSettings", "TelemetrySpeed", m_ui->cbFlexiTelemSpeed);
addUAVObjectToWidgetRelation("HwSettings", "GPSSpeed", m_ui->cbFlexiGPSSpeed);
addUAVObjectToWidgetRelation("HwSettings", "ComUsbBridgeSpeed", m_ui->cbFlexiComSpeed);
addWidgetBinding("HwSettings", "TelemetrySpeed", m_ui->cbFlexiTelemSpeed);
addWidgetBinding("HwSettings", "GPSSpeed", m_ui->cbFlexiGPSSpeed);
addWidgetBinding("HwSettings", "ComUsbBridgeSpeed", m_ui->cbFlexiComSpeed);
addUAVObjectToWidgetRelation("HwSettings", "TelemetrySpeed", m_ui->cbMainTelemSpeed);
addUAVObjectToWidgetRelation("HwSettings", "GPSSpeed", m_ui->cbMainGPSSpeed);
addUAVObjectToWidgetRelation("HwSettings", "ComUsbBridgeSpeed", m_ui->cbMainComSpeed);
addWidgetBinding("HwSettings", "TelemetrySpeed", m_ui->cbMainTelemSpeed);
addWidgetBinding("HwSettings", "GPSSpeed", m_ui->cbMainGPSSpeed);
addWidgetBinding("HwSettings", "ComUsbBridgeSpeed", m_ui->cbMainComSpeed);
connect(m_ui->cchwHelp, SIGNAL(clicked()), this, SLOT(openHelp()));

10
ground/openpilotgcs/src/plugins/config/configrevowidget.cpp
View File

@ -226,12 +226,12 @@ ConfigRevoWidget::ConfigRevoWidget(QWidget *parent) :
connect(m_ui->hlClearButton, SIGNAL(clicked()), this, SLOT(clearHomeLocation()));
addUAVObjectToWidgetRelation("RevoSettings", "FusionAlgorithm", m_ui->FusionAlgorithm);
addWidgetBinding("RevoSettings", "FusionAlgorithm", m_ui->FusionAlgorithm);
addUAVObjectToWidgetRelation("AttitudeSettings", "BoardRotation", m_ui->rollRotation, AttitudeSettings::BOARDROTATION_ROLL);
addUAVObjectToWidgetRelation("AttitudeSettings", "BoardRotation", m_ui->pitchRotation, AttitudeSettings::BOARDROTATION_PITCH);
addUAVObjectToWidgetRelation("AttitudeSettings", "BoardRotation", m_ui->yawRotation, AttitudeSettings::BOARDROTATION_YAW);
addUAVObjectToWidgetRelation("AttitudeSettings", "AccelTau", m_ui->accelTau);
addWidgetBinding("AttitudeSettings", "BoardRotation", m_ui->rollRotation, AttitudeSettings::BOARDROTATION_ROLL);
addWidgetBinding("AttitudeSettings", "BoardRotation", m_ui->pitchRotation, AttitudeSettings::BOARDROTATION_PITCH);
addWidgetBinding("AttitudeSettings", "BoardRotation", m_ui->yawRotation, AttitudeSettings::BOARDROTATION_YAW);
addWidgetBinding("AttitudeSettings", "AccelTau", m_ui->accelTau);
populateWidgets();
refreshWidgetsValues();

54
ground/openpilotgcs/src/plugins/config/configstabilizationwidget.cpp
View File

@ -35,17 +35,44 @@
#include <QDesktopServices>
#include <QUrl>
#include <QList>
#include <QTabBar>
#include <QMessageBox>
#include <extensionsystem/pluginmanager.h>
#include <coreplugin/generalsettings.h>
#include "altitudeholdsettings.h"
#include "stabilizationsettings.h"
ConfigStabilizationWidget::ConfigStabilizationWidget(QWidget *parent) : ConfigTaskWidget(parent),
boardModel(0)
boardModel(0), m_pidBankCount(0), m_currentPIDBank(0)
{
ui = new Ui_StabilizationWidget();
ui->setupUi(this);
StabilizationSettings *stabSettings = qobject_cast<StabilizationSettings *>(getObject("StabilizationSettings"));
Q_ASSERT(stabSettings);
m_pidBankCount = stabSettings->getField("FlightModeMap")->getOptions().count();
// Set up fake tab widget stuff for pid banks support
m_pidTabBars.append(ui->basicPIDBankTabBar);
m_pidTabBars.append(ui->advancedPIDBankTabBar);
m_pidTabBars.append(ui->expertPIDBankTabBar);
foreach(QTabBar * tabBar, m_pidTabBars) {
for (int i = 0; i < m_pidBankCount; i++) {
tabBar->addTab(tr("PID Bank %1").arg(i + 1));
tabBar->setTabData(i, QString("StabilizationSettingsBank%1").arg(i + 1));
}
tabBar->setExpanding(false);
connect(tabBar, SIGNAL(currentChanged(int)), this, SLOT(pidBankChanged(int)));
}
for (int i = 0; i < m_pidBankCount; i++) {
if (i > 0) {
m_stabilizationObjectsString.append(",");
}
m_stabilizationObjectsString.append(m_pidTabBars.at(0)->tabData(i).toString());
}
ExtensionSystem::PluginManager *pm = ExtensionSystem::PluginManager::instance();
Core::Internal::GeneralSettings *settings = pm->getObject<Core::Internal::GeneralSettings>();
@ -205,6 +232,23 @@ void ConfigStabilizationWidget::onBoardConnected()
ui->AltitudeHold->setEnabled((boardModel & 0xff00) == 0x0900);
}
void ConfigStabilizationWidget::pidBankChanged(int index)
{
foreach(QTabBar * tabBar, m_pidTabBars) {
disconnect(tabBar, SIGNAL(currentChanged(int)), this, SLOT(pidBankChanged(int)));
tabBar->setCurrentIndex(index);
connect(tabBar, SIGNAL(currentChanged(int)), this, SLOT(pidBankChanged(int)));
}
for (int i = 0; i < m_pidTabBars.at(0)->count(); i++) {
setWidgetBindingObjectEnabled(m_pidTabBars.at(0)->tabData(i).toString(), false);
}
setWidgetBindingObjectEnabled(m_pidTabBars.at(0)->tabData(index).toString(), true);
m_currentPIDBank = index;
}
bool ConfigStabilizationWidget::shouldObjectBeSaved(UAVObject *object)
{
// AltitudeHoldSettings should only be saved for Revolution board to avoid error.
@ -214,3 +258,11 @@ bool ConfigStabilizationWidget::shouldObjectBeSaved(UAVObject *object)
return true;
}
}
QString ConfigStabilizationWidget::mapObjectName(const QString objectName)
{
if (objectName == "StabilizationSettingsBankX") {
return m_stabilizationObjectsString;
}
return ConfigTaskWidget::mapObjectName(objectName);
}

7
ground/openpilotgcs/src/plugins/config/configstabilizationwidget.h
View File

@ -48,11 +48,17 @@ public:
private:
Ui_StabilizationWidget *ui;
QTimer *realtimeUpdates;
QList<QTabBar *> m_pidTabBars;
QString m_stabilizationObjectsString;
// Milliseconds between automatic 'Instant Updates'
static const int AUTOMATIC_UPDATE_RATE = 500;
int m_pidBankCount;
int boardModel;
int m_currentPIDBank;
protected:
QString mapObjectName(const QString objectName);
protected slots:
void refreshWidgetsValues(UAVObject *o = NULL);
@ -62,6 +68,7 @@ private slots:
void linkCheckBoxes(bool value);
void processLinkedWidgets(QWidget *);
void onBoardConnected();
void pidBankChanged(int index);
};
#endif // ConfigStabilizationWidget_H

32
ground/openpilotgcs/src/plugins/config/configtxpidwidget.cpp
View File

@ -51,26 +51,28 @@ ConfigTxPIDWidget::ConfigTxPIDWidget(QWidget *parent) : ConfigTaskWidget(parent)
connect(m_txpid->Apply, SIGNAL(clicked()), this, SLOT(applySettings()));
connect(m_txpid->Save, SIGNAL(clicked()), this, SLOT(saveSettings()));
addUAVObjectToWidgetRelation("TxPIDSettings", "PIDs", m_txpid->PID1, TxPIDSettings::PIDS_INSTANCE1);
addUAVObjectToWidgetRelation("TxPIDSettings", "PIDs", m_txpid->PID2, TxPIDSettings::PIDS_INSTANCE2);
addUAVObjectToWidgetRelation("TxPIDSettings", "PIDs", m_txpid->PID3, TxPIDSettings::PIDS_INSTANCE3);
addWidgetBinding("TxPIDSettings", "BankNumber", m_txpid->pidBank, 0, 1, true);
addUAVObjectToWidgetRelation("TxPIDSettings", "Inputs", m_txpid->Input1, TxPIDSettings::INPUTS_INSTANCE1);
addUAVObjectToWidgetRelation("TxPIDSettings", "Inputs", m_txpid->Input2, TxPIDSettings::INPUTS_INSTANCE2);
addUAVObjectToWidgetRelation("TxPIDSettings", "Inputs", m_txpid->Input3, TxPIDSettings::INPUTS_INSTANCE3);
addWidgetBinding("TxPIDSettings", "PIDs", m_txpid->PID1, TxPIDSettings::PIDS_INSTANCE1);
addWidgetBinding("TxPIDSettings", "PIDs", m_txpid->PID2, TxPIDSettings::PIDS_INSTANCE2);
addWidgetBinding("TxPIDSettings", "PIDs", m_txpid->PID3, TxPIDSettings::PIDS_INSTANCE3);
addUAVObjectToWidgetRelation("TxPIDSettings", "MinPID", m_txpid->MinPID1, TxPIDSettings::MINPID_INSTANCE1);
addUAVObjectToWidgetRelation("TxPIDSettings", "MinPID", m_txpid->MinPID2, TxPIDSettings::MINPID_INSTANCE2);
addUAVObjectToWidgetRelation("TxPIDSettings", "MinPID", m_txpid->MinPID3, TxPIDSettings::MINPID_INSTANCE3);
addWidgetBinding("TxPIDSettings", "Inputs", m_txpid->Input1, TxPIDSettings::INPUTS_INSTANCE1);
addWidgetBinding("TxPIDSettings", "Inputs", m_txpid->Input2, TxPIDSettings::INPUTS_INSTANCE2);
addWidgetBinding("TxPIDSettings", "Inputs", m_txpid->Input3, TxPIDSettings::INPUTS_INSTANCE3);
addUAVObjectToWidgetRelation("TxPIDSettings", "MaxPID", m_txpid->MaxPID1, TxPIDSettings::MAXPID_INSTANCE1);
addUAVObjectToWidgetRelation("TxPIDSettings", "MaxPID", m_txpid->MaxPID2, TxPIDSettings::MAXPID_INSTANCE2);
addUAVObjectToWidgetRelation("TxPIDSettings", "MaxPID", m_txpid->MaxPID3, TxPIDSettings::MAXPID_INSTANCE3);
addWidgetBinding("TxPIDSettings", "MinPID", m_txpid->MinPID1, TxPIDSettings::MINPID_INSTANCE1);
addWidgetBinding("TxPIDSettings", "MinPID", m_txpid->MinPID2, TxPIDSettings::MINPID_INSTANCE2);
addWidgetBinding("TxPIDSettings", "MinPID", m_txpid->MinPID3, TxPIDSettings::MINPID_INSTANCE3);
addUAVObjectToWidgetRelation("TxPIDSettings", "ThrottleRange", m_txpid->ThrottleMin, TxPIDSettings::THROTTLERANGE_MIN);
addUAVObjectToWidgetRelation("TxPIDSettings", "ThrottleRange", m_txpid->ThrottleMax, TxPIDSettings::THROTTLERANGE_MAX);
addWidgetBinding("TxPIDSettings", "MaxPID", m_txpid->MaxPID1, TxPIDSettings::MAXPID_INSTANCE1);
addWidgetBinding("TxPIDSettings", "MaxPID", m_txpid->MaxPID2, TxPIDSettings::MAXPID_INSTANCE2);
addWidgetBinding("TxPIDSettings", "MaxPID", m_txpid->MaxPID3, TxPIDSettings::MAXPID_INSTANCE3);
addUAVObjectToWidgetRelation("TxPIDSettings", "UpdateMode", m_txpid->UpdateMode);
addWidgetBinding("TxPIDSettings", "ThrottleRange", m_txpid->ThrottleMin, TxPIDSettings::THROTTLERANGE_MIN);
addWidgetBinding("TxPIDSettings", "ThrottleRange", m_txpid->ThrottleMax, TxPIDSettings::THROTTLERANGE_MAX);
addWidgetBinding("TxPIDSettings", "UpdateMode", m_txpid->UpdateMode);
addWidget(m_txpid->TxPIDEnable);

797
ground/openpilotgcs/src/plugins/config/input.ui
View File

@ -14,7 +14,16 @@
<string>Form</string>
</property>
<layout class="QVBoxLayout" name="verticalLayout">
<property name="margin">
<property name="leftMargin">
<number>12</number>
</property>
<property name="topMargin">
<number>12</number>
</property>
<property name="rightMargin">
<number>12</number>
</property>
<property name="bottomMargin">
<number>12</number>
</property>
<item>
@ -30,7 +39,16 @@
<property name="spacing">
<number>0</number>
</property>
<property name="margin">
<property name="leftMargin">
<number>0</number>
</property>
<property name="topMargin">
<number>0</number>
</property>
<property name="rightMargin">
<number>0</number>
</property>
<property name="bottomMargin">
<number>0</number>
</property>
<item>
@ -111,15 +129,24 @@
<x>0</x>
<y>0</y>
<width>850</width>
<height>572</height>
<height>589</height>
</rect>
</property>
<layout class="QGridLayout" name="gridLayout">
<property name="margin">
<property name="leftMargin">
<number>12</number>
</property>
<property name="topMargin">
<number>12</number>
</property>
<property name="rightMargin">
<number>12</number>
</property>
<property name="bottomMargin">
<number>12</number>
</property>
<property name="verticalSpacing">
<number>-1</number>
<number>6</number>
</property>
<item row="1" column="0">
<widget class="QGroupBox" name="groupBox_5">
@ -146,7 +173,16 @@
</property>
<widget class="QWidget" name="advancedPage">
<layout class="QVBoxLayout" name="verticalLayout_3">
<property name="margin">
<property name="leftMargin">
<number>12</number>
</property>
<property name="topMargin">
<number>12</number>
</property>
<property name="rightMargin">
<number>12</number>
</property>
<property name="bottomMargin">
<number>12</number>
</property>
<item>
@ -245,11 +281,20 @@
</widget>
<widget class="QWidget" name="wizard">
<layout class="QVBoxLayout" name="verticalLayout_5">
<property name="margin">
<property name="leftMargin">
<number>12</number>
</property>
<property name="topMargin">
<number>12</number>
</property>
<property name="rightMargin">
<number>12</number>
</property>
<property name="bottomMargin">
<number>12</number>
</property>
<item>
<widget class="QLabel" name="wzText">
<widget class="QLabel" name="wzText">
<property name="sizePolicy">
<sizepolicy hsizetype="Preferred" vsizetype="Minimum">
<horstretch>0</horstretch>
@ -260,28 +305,28 @@
<bool>true</bool>
</property>
</widget>
</item>
<item>
<layout class="QVBoxLayout" name="radioButtonsLayout"/>
</item>
<item>
<layout class="QGridLayout" name="checkBoxesLayout"/>
</item>
<item>
<widget class="QLabel" name="wzText2">
<property name="sizePolicy">
<sizepolicy hsizetype="Preferred" vsizetype="Minimum">
<horstretch>0</horstretch>
<verstretch>0</verstretch>
</sizepolicy>
</property>
<property name="wordWrap">
<bool>true</bool>
</property>
</widget>
</item>
</item>
<item>
<widget class="QGraphicsView" name="graphicsView"/>
<layout class="QVBoxLayout" name="radioButtonsLayout"/>
</item>
<item>
<layout class="QGridLayout" name="checkBoxesLayout"/>
</item>
<item>
<widget class="QLabel" name="wzText2">
<property name="sizePolicy">
<sizepolicy hsizetype="Preferred" vsizetype="Minimum">
<horstretch>0</horstretch>
<verstretch>0</verstretch>
</sizepolicy>
</property>
<property name="wordWrap">
<bool>true</bool>
</property>
</widget>
</item>
<item>
<widget class="QGraphicsView" name="graphicsView"/>
</item>
<item>
<layout class="QHBoxLayout" name="horizontalLayout_3">
@ -418,7 +463,16 @@
<string>Flight Mode Switch Settings</string>
</attribute>
<layout class="QVBoxLayout" name="verticalLayout_8">
<property name="margin">
<property name="leftMargin">
<number>0</number>
</property>
<property name="topMargin">
<number>0</number>
</property>
<property name="rightMargin">
<number>0</number>
</property>
<property name="bottomMargin">
<number>0</number>
</property>
<item>
@ -499,284 +553,22 @@
<x>0</x>
<y>0</y>
<width>850</width>
<height>572</height>
<height>589</height>
</rect>
</property>
<layout class="QGridLayout" name="gridLayout_7" rowstretch="1,0,0">
<property name="margin">
<property name="leftMargin">
<number>12</number>
</property>
<property name="topMargin">
<number>12</number>
</property>
<property name="rightMargin">
<number>12</number>
</property>
<property name="bottomMargin">
<number>12</number>
</property>
<item row="1" column="0">
<widget class="QGroupBox" name="groupBox">
<property name="styleSheet">
<string notr="true"/>
</property>
<property name="title">
<string>Configure each stabilization mode for each axis</string>
</property>
<layout class="QGridLayout" name="gridLayout_2" columnstretch="0,0,1,0,1,0,1,0">
<property name="leftMargin">
<number>9</number>
</property>
<property name="horizontalSpacing">
<number>12</number>
</property>
<item row="2" column="1">
<spacer name="horizontalSpacer_11">
<property name="orientation">
<enum>Qt::Horizontal</enum>
</property>
<property name="sizeType">
<enum>QSizePolicy::Minimum</enum>
</property>
<property name="sizeHint" stdset="0">
<size>
<width>5</width>
<height>20</height>
</size>
</property>
</spacer>
</item>
<item row="3" column="2">
<widget class="QComboBox" name="fmsSsPos3Roll">
<property name="focusPolicy">
<enum>Qt::StrongFocus</enum>
</property>
</widget>
</item>
<item row="0" column="6">
<widget class="QLabel" name="label_10">
<property name="minimumSize">
<size>
<width>0</width>
<height>20</height>
</size>
</property>
<property name="maximumSize">
<size>
<width>16777215</width>
<height>20</height>
</size>
</property>
<property name="styleSheet">
<string notr="true">background-color: qlineargradient(spread:reflect, x1:0.507, y1:0, x2:0.507, y2:0.772, stop:0.208955 rgba(74, 74, 74, 255), stop:0.78607 rgba(36, 36, 36, 255));
color: rgb(255, 255, 255);
border-radius: 5;
font: bold 12px;
margin:1px;</string>
</property>
<property name="text">
<string>Yaw</string>
</property>
<property name="alignment">
<set>Qt::AlignCenter</set>
</property>
</widget>
</item>
<item row="2" column="3">
<spacer name="horizontalSpacer_12">
<property name="orientation">
<enum>Qt::Horizontal</enum>
</property>
<property name="sizeHint" stdset="0">
<size>
<width>5</width>
<height>20</height>
</size>
</property>
</spacer>
</item>
<item row="1" column="0">
<widget class="QLabel" name="label_14">
<property name="text">
<string>Stabilized1</string>
</property>
<property name="alignment">
<set>Qt::AlignCenter</set>
</property>
</widget>
</item>
<item row="2" column="5">
<spacer name="horizontalSpacer_13">
<property name="orientation">
<enum>Qt::Horizontal</enum>
</property>
<property name="sizeHint" stdset="0">
<size>
<width>5</width>
<height>20</height>
</size>
</property>
</spacer>
</item>
<item row="3" column="6">
<widget class="QComboBox" name="fmsSsPos3Yaw">
<property name="focusPolicy">
<enum>Qt::StrongFocus</enum>
</property>
</widget>
</item>
<item row="2" column="0">
<widget class="QLabel" name="label_21">
<property name="text">
<string>Stabilized2</string>
</property>
<property name="alignment">
<set>Qt::AlignCenter</set>
</property>
</widget>
</item>
<item row="3" column="4">
<widget class="QComboBox" name="fmsSsPos3Pitch">
<property name="focusPolicy">
<enum>Qt::StrongFocus</enum>
</property>
</widget>
</item>
<item row="2" column="6">
<widget class="QComboBox" name="fmsSsPos2Yaw">
<property name="focusPolicy">
<enum>Qt::StrongFocus</enum>
</property>
</widget>
</item>
<item row="0" column="4">
<widget class="QLabel" name="label_9">
<property name="minimumSize">
<size>
<width>0</width>
<height>20</height>
</size>
</property>
<property name="maximumSize">
<size>
<width>16777215</width>
<height>20</height>
</size>
</property>
<property name="styleSheet">
<string notr="true">background-color: qlineargradient(spread:reflect, x1:0.507, y1:0, x2:0.507, y2:0.772, stop:0.208955 rgba(74, 74, 74, 255), stop:0.78607 rgba(36, 36, 36, 255));
color: rgb(255, 255, 255);
border-radius: 5;
font: bold 12px;
margin:1px;</string>
</property>
<property name="text">
<string>Pitch</string>
</property>
<property name="alignment">
<set>Qt::AlignCenter</set>
</property>
</widget>
</item>
<item row="0" column="2">
<widget class="QLabel" name="label_8">
<property name="minimumSize">
<size>
<width>0</width>
<height>20</height>
</size>
</property>
<property name="maximumSize">
<size>
<width>16777215</width>
<height>20</height>
</size>
</property>
<property name="styleSheet">
<string notr="true">background-color: qlineargradient(spread:reflect, x1:0.507, y1:0, x2:0.507, y2:0.772, stop:0.208955 rgba(74, 74, 74, 255), stop:0.78607 rgba(36, 36, 36, 255));
color: rgb(255, 255, 255);
border-radius: 5;
font: bold 12px;
margin:1px;</string>
</property>
<property name="text">
<string>Roll</string>
</property>
<property name="alignment">
<set>Qt::AlignCenter</set>
</property>
</widget>
</item>
<item row="2" column="4">
<widget class="QComboBox" name="fmsSsPos2Pitch">
<property name="focusPolicy">
<enum>Qt::StrongFocus</enum>
</property>
</widget>
</item>
<item row="1" column="6">
<widget class="QComboBox" name="fmsSsPos1Yaw">
<property name="minimumSize">
<size>
<width>102</width>
<height>0</height>
</size>
</property>
<property name="focusPolicy">
<enum>Qt::StrongFocus</enum>
</property>
</widget>
</item>
<item row="2" column="2">
<widget class="QComboBox" name="fmsSsPos2Roll">
<property name="focusPolicy">
<enum>Qt::StrongFocus</enum>
</property>
</widget>
</item>
<item row="1" column="2">
<widget class="QComboBox" name="fmsSsPos1Roll">
<property name="minimumSize">
<size>
<width>102</width>
<height>0</height>
</size>
</property>
<property name="focusPolicy">
<enum>Qt::StrongFocus</enum>
</property>
</widget>
</item>
<item row="1" column="4">
<widget class="QComboBox" name="fmsSsPos1Pitch">
<property name="minimumSize">
<size>
<width>102</width>
<height>0</height>
</size>
</property>
<property name="focusPolicy">
<enum>Qt::StrongFocus</enum>
</property>
</widget>
</item>
<item row="3" column="0">
<widget class="QLabel" name="label_22">
<property name="text">
<string>Stabilized3</string>
</property>
<property name="alignment">
<set>Qt::AlignCenter</set>
</property>
</widget>
</item>
<item row="2" column="7">
<spacer name="horizontalSpacer_14">
<property name="orientation">
<enum>Qt::Horizontal</enum>
</property>
<property name="sizeHint" stdset="0">
<size>
<width>40</width>
<height>20</height>
</size>
</property>
</spacer>
</item>
</layout>
</widget>
</item>
<item row="0" column="0">
<widget class="QGroupBox" name="groupBox_2">
<property name="minimumSize">
@ -795,7 +587,16 @@ margin:1px;</string>
<string>FlightMode Switch Positions</string>
</property>
<layout class="QGridLayout" name="gridLayout_4" rowstretch="0,0,0" columnstretch="0,0,1,0,1,0">
<property name="margin">
<property name="leftMargin">
<number>12</number>
</property>
<property name="topMargin">
<number>12</number>
</property>
<property name="rightMargin">
<number>12</number>
</property>
<property name="bottomMargin">
<number>12</number>
</property>
<item row="0" column="0" rowspan="3">
@ -1070,6 +871,9 @@ Setup the flight mode channel on the RC Input tab if you have not done so alread
<property name="orientation">
<enum>Qt::Horizontal</enum>
</property>
<property name="sizeType">
<enum>QSizePolicy::Expanding</enum>
</property>
<property name="sizeHint" stdset="0">
<size>
<width>40</width>
@ -1180,6 +984,367 @@ channel value for each flight mode.</string>
</layout>
</widget>
</item>
<item row="1" column="0">
<widget class="QGroupBox" name="groupBox">
<property name="styleSheet">
<string notr="true"/>
</property>
<property name="title">
<string>Stabilization Modes Configuration</string>
</property>
<layout class="QGridLayout" name="gridLayout_2" columnstretch="0,0,0,0,0,0,0,0,0,0">
<property name="leftMargin">
<number>9</number>
</property>
<property name="horizontalSpacing">
<number>12</number>
</property>
<item row="2" column="1">
<spacer name="horizontalSpacer_11">
<property name="orientation">
<enum>Qt::Horizontal</enum>
</property>
<property name="sizeType">
<enum>QSizePolicy::Minimum</enum>
</property>
<property name="sizeHint" stdset="0">
<size>
<width>5</width>
<height>20</height>
</size>
</property>
</spacer>
</item>
<item row="0" column="6">
<widget class="QLabel" name="label_10">
<property name="minimumSize">
<size>
<width>0</width>
<height>20</height>
</size>
</property>
<property name="maximumSize">
<size>
<width>16777215</width>
<height>20</height>
</size>
</property>
<property name="styleSheet">
<string notr="true">background-color: qlineargradient(spread:reflect, x1:0.507, y1:0, x2:0.507, y2:0.772, stop:0.208955 rgba(74, 74, 74, 255), stop:0.78607 rgba(36, 36, 36, 255));
color: rgb(255, 255, 255);
border-radius: 5;
font: bold 12px;
margin:1px;</string>
</property>
<property name="text">
<string>Yaw</string>
</property>
<property name="alignment">
<set>Qt::AlignCenter</set>
</property>
</widget>
</item>
<item row="2" column="3">
<spacer name="horizontalSpacer_12">
<property name="orientation">
<enum>Qt::Horizontal</enum>
</property>
<property name="sizeType">
<enum>QSizePolicy::MinimumExpanding</enum>
</property>
<property name="sizeHint" stdset="0">
<size>
<width>20</width>
<height>20</height>
</size>
</property>
</spacer>
</item>
<item row="3" column="2">
<widget class="QComboBox" name="fmsSsPos3Roll">
<property name="focusPolicy">
<enum>Qt::StrongFocus</enum>
</property>
</widget>
</item>
<item row="1" column="0">
<widget class="QLabel" name="label_14">
<property name="text">
<string>Stabilized 1</string>
</property>
<property name="alignment">
<set>Qt::AlignLeading|Qt::AlignLeft|Qt::AlignVCenter</set>
</property>
</widget>
</item>
<item row="2" column="5">
<spacer name="horizontalSpacer_13">
<property name="orientation">
<enum>Qt::Horizontal</enum>
</property>
<property name="sizeType">
<enum>QSizePolicy::MinimumExpanding</enum>
</property>
<property name="sizeHint" stdset="0">
<size>
<width>20</width>
<height>20</height>
</size>
</property>
</spacer>
</item>
<item row="2" column="0">
<widget class="QLabel" name="label_21">
<property name="text">
<string>Stabilized 2</string>
</property>
<property name="alignment">
<set>Qt::AlignLeading|Qt::AlignLeft|Qt::AlignVCenter</set>
</property>
</widget>
</item>
<item row="3" column="4">
<widget class="QComboBox" name="fmsSsPos3Pitch">
<property name="focusPolicy">
<enum>Qt::StrongFocus</enum>
</property>
</widget>
</item>
<item row="3" column="6">
<widget class="QComboBox" name="fmsSsPos3Yaw">
<property name="focusPolicy">
<enum>Qt::StrongFocus</enum>
</property>
</widget>
</item>
<item row="2" column="6">
<widget class="QComboBox" name="fmsSsPos2Yaw">
<property name="focusPolicy">
<enum>Qt::StrongFocus</enum>
</property>
</widget>
</item>
<item row="0" column="2">
<widget class="QLabel" name="label_8">
<property name="minimumSize">
<size>
<width>0</width>
<height>20</height>
</size>
</property>
<property name="maximumSize">
<size>
<width>16777215</width>
<height>20</height>
</size>
</property>
<property name="styleSheet">
<string notr="true">background-color: qlineargradient(spread:reflect, x1:0.507, y1:0, x2:0.507, y2:0.772, stop:0.208955 rgba(74, 74, 74, 255), stop:0.78607 rgba(36, 36, 36, 255));
color: rgb(255, 255, 255);
border-radius: 5;
font: bold 12px;
margin:1px;</string>
</property>
<property name="text">
<string>Roll</string>
</property>
<property name="alignment">
<set>Qt::AlignCenter</set>
</property>
</widget>
</item>
<item row="0" column="4">
<widget class="QLabel" name="label_9">
<property name="minimumSize">
<size>
<width>0</width>
<height>20</height>
</size>
</property>
<property name="maximumSize">
<size>
<width>16777215</width>
<height>20</height>
</size>
</property>
<property name="styleSheet">
<string notr="true">background-color: qlineargradient(spread:reflect, x1:0.507, y1:0, x2:0.507, y2:0.772, stop:0.208955 rgba(74, 74, 74, 255), stop:0.78607 rgba(36, 36, 36, 255));
color: rgb(255, 255, 255);
border-radius: 5;
font: bold 12px;
margin:1px;</string>
</property>
<property name="text">
<string>Pitch</string>
</property>
<property name="alignment">
<set>Qt::AlignCenter</set>
</property>
</widget>
</item>
<item row="2" column="4">
<widget class="QComboBox" name="fmsSsPos2Pitch">
<property name="focusPolicy">
<enum>Qt::StrongFocus</enum>
</property>
</widget>
</item>
<item row="1" column="6">
<widget class="QComboBox" name="fmsSsPos1Yaw">
<property name="minimumSize">
<size>
<width>102</width>
<height>0</height>
</size>
</property>
<property name="focusPolicy">
<enum>Qt::StrongFocus</enum>
</property>
</widget>
</item>
<item row="2" column="2">
<widget class="QComboBox" name="fmsSsPos2Roll">
<property name="focusPolicy">
<enum>Qt::StrongFocus</enum>
</property>
</widget>
</item>
<item row="1" column="2">
<widget class="QComboBox" name="fmsSsPos1Roll">
<property name="minimumSize">
<size>
<width>102</width>
<height>0</height>
</size>
</property>
<property name="focusPolicy">
<enum>Qt::StrongFocus</enum>
</property>
</widget>
</item>
<item row="3" column="0">
<widget class="QLabel" name="label_22">
<property name="text">
<string>Stabilized 3</string>
</property>
<property name="alignment">
<set>Qt::AlignLeading|Qt::AlignLeft|Qt::AlignVCenter</set>
</property>
</widget>
</item>
<item row="1" column="4">
<widget class="QComboBox" name="fmsSsPos1Pitch">
<property name="minimumSize">
<size>
<width>102</width>
<height>0</height>
</size>
</property>
<property name="focusPolicy">
<enum>Qt::StrongFocus</enum>
</property>
</widget>
</item>
<item row="3" column="8">
<widget class="QComboBox" name="pidBankSs3">
<property name="minimumSize">
<size>
<width>102</width>
<height>0</height>
</size>
</property>
<property name="focusPolicy">
<enum>Qt::StrongFocus</enum>
</property>
</widget>
</item>
<item row="2" column="8">
<widget class="QComboBox" name="pidBankSs2">
<property name="minimumSize">
<size>
<width>102</width>
<height>0</height>
</size>
</property>
<property name="focusPolicy">
<enum>Qt::StrongFocus</enum>
</property>
</widget>
</item>
<item row="0" column="9">
<spacer name="horizontalSpacer_14">
<property name="orientation">
<enum>Qt::Horizontal</enum>
</property>
<property name="sizeHint" stdset="0">
<size>
<width>40</width>
<height>20</height>
</size>
</property>
</spacer>
</item>
<item row="0" column="8">
<widget class="QLabel" name="label_11">
<property name="minimumSize">
<size>
<width>0</width>
<height>20</height>
</size>
</property>
<property name="maximumSize">
<size>
<width>16777215</width>
<height>20</height>
</size>
</property>
<property name="styleSheet">
<string notr="true">background-color: qlineargradient(spread:reflect, x1:0.507, y1:0, x2:0.507, y2:0.772, stop:0.208955 rgba(74, 74, 74, 255), stop:0.78607 rgba(36, 36, 36, 255));
color: rgb(255, 255, 255);
border-radius: 5;
font: bold 12px;
margin:1px;</string>
</property>
<property name="text">
<string>PID Bank</string>
</property>
<property name="alignment">
<set>Qt::AlignCenter</set>
</property>
</widget>
</item>
<item row="1" column="8">
<widget class="QComboBox" name="pidBankSs1">
<property name="minimumSize">
<size>
<width>102</width>
<height>0</height>
</size>
</property>
<property name="focusPolicy">
<enum>Qt::StrongFocus</enum>
</property>
</widget>
</item>
<item row="2" column="7">
<spacer name="horizontalSpacer_15">
<property name="orientation">
<enum>Qt::Horizontal</enum>
</property>
<property name="sizeType">
<enum>QSizePolicy::MinimumExpanding</enum>
</property>
<property name="sizeHint" stdset="0">
<size>
<width>20</width>
<height>20</height>
</size>
</property>
</spacer>
</item>
</layout>
</widget>
</item>
<item row="2" column="0">
<spacer name="verticalSpacer_5">
<property name="orientation">
@ -1207,7 +1372,16 @@ channel value for each flight mode.</string>
<string>Arming Settings</string>
</attribute>
<layout class="QVBoxLayout" name="verticalLayout_12">
<property name="margin">
<property name="leftMargin">
<number>0</number>
</property>
<property name="topMargin">
<number>0</number>
</property>
<property name="rightMargin">
<number>0</number>
</property>
<property name="bottomMargin">
<number>0</number>
</property>
<item>
@ -1288,11 +1462,20 @@ channel value for each flight mode.</string>
<x>0</x>
<y>0</y>
<width>850</width>
<height>572</height>
<height>589</height>
</rect>
</property>
<layout class="QVBoxLayout" name="verticalLayout_2">
<property name="margin">
<property name="leftMargin">
<number>12</number>
</property>
<property name="topMargin">
<number>12</number>
</property>
<property name="rightMargin">
<number>12</number>
</property>
<property name="bottomMargin">
<number>12</number>
</property>
<item>

37341
ground/openpilotgcs/src/plugins/config/stabilization.ui
View File

File diff suppressed because it is too large Load Diff

47
ground/openpilotgcs/src/plugins/config/txpid.ui
View File

@ -17,7 +17,16 @@
<property name="spacing">
<number>6</number>
</property>
<property name="margin">
<property name="leftMargin">
<number>12</number>
</property>
<property name="topMargin">
<number>12</number>
</property>
<property name="rightMargin">
<number>12</number>
</property>
<property name="bottomMargin">
<number>12</number>
</property>
<item>
@ -33,7 +42,16 @@
<property name="spacing">
<number>6</number>
</property>
<property name="margin">
<property name="leftMargin">
<number>0</number>
</property>
<property name="topMargin">
<number>0</number>
</property>
<property name="rightMargin">
<number>0</number>
</property>
<property name="bottomMargin">
<number>0</number>
</property>
<item>
@ -113,15 +131,24 @@
<rect>
<x>0</x>
<y>0</y>
<width>745</width>
<height>469</height>
<width>742</width>
<height>450</height>
</rect>
</property>
<layout class="QVBoxLayout" name="verticalLayout_2">
<property name="spacing">
<number>6</number>
</property>
<property name="margin">
<property name="leftMargin">
<number>12</number>
</property>
<property name="topMargin">
<number>12</number>
</property>
<property name="rightMargin">
<number>12</number>
</property>
<property name="bottomMargin">
<number>12</number>
</property>
<item>
@ -661,6 +688,16 @@ margin:1px;</string>
</property>
</spacer>
</item>
<item row="0" column="0">
<widget class="QLabel" name="label_6">
<property name="text">
<string>PID Bank</string>
</property>
</widget>
</item>
<item row="0" column="1">
<widget class="QComboBox" name="pidBank"/>
</item>
</layout>
</widget>
</item>

1
ground/openpilotgcs/src/plugins/coreplugin/coreplugin.pro
View File

@ -129,7 +129,6 @@ HEADERS += mainwindow.h \
uavgadgetdecorator.h \
workspacesettings.h \
uavconfiginfo.h \
authorsdialog.h \
iconfigurableplugin.h \
aboutdialog.h

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
*/

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

@ -318,10 +318,10 @@ void XplaneSimulator::processUpdate(const QByteArray & dataBuf)
out.velEast = velX;
out.velDown = -velZ;
// Update gyroscope sensor data
out.rollRate = rollRate_rad;
out.pitchRate = pitchRate_rad;
out.yawRate = yawRate_rad;
// Update gyroscope sensor data - convert from rad/s to deg/s
out.rollRate = rollRate_rad * (180.0 / M_PI);
out.pitchRate = pitchRate_rad * (180.0 / M_PI);
out.yawRate = yawRate_rad * (180.0 / M_PI);
// Update accelerometer sensor data
out.accX = accX;

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

30
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,16 +29,17 @@ 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 \
$$UAVOBJECT_SYNTHETICS/airspeedstate.h \
$$UAVOBJECT_SYNTHETICS/attitudestate.h \
$$UAVOBJECT_SYNTHETICS/attitudesimulated.h \
$$UAVOBJECT_SYNTHETICS/altholdsmoothed.h \
$$UAVOBJECT_SYNTHETICS/altitudeholddesired.h \
$$UAVOBJECT_SYNTHETICS/altitudeholdsettings.h \
$$UAVOBJECT_SYNTHETICS/altitudeholdstatus.h \
$$UAVOBJECT_SYNTHETICS/altitudefiltersettings.h \
$$UAVOBJECT_SYNTHETICS/debuglogsettings.h \
$$UAVOBJECT_SYNTHETICS/debuglogcontrol.h \
@ -60,6 +65,10 @@ HEADERS += $$UAVOBJECT_SYNTHETICS/accessorydesired.h \
$$UAVOBJECT_SYNTHETICS/overosyncsettings.h \
$$UAVOBJECT_SYNTHETICS/systemsettings.h \
$$UAVOBJECT_SYNTHETICS/stabilizationsettings.h \
$$UAVOBJECT_SYNTHETICS/stabilizationsettingsbank1.h \
$$UAVOBJECT_SYNTHETICS/stabilizationsettingsbank2.h \
$$UAVOBJECT_SYNTHETICS/stabilizationsettingsbank3.h \
$$UAVOBJECT_SYNTHETICS/stabilizationbank.h \
$$UAVOBJECT_SYNTHETICS/manualcontrolsettings.h \
$$UAVOBJECT_SYNTHETICS/manualcontrolcommand.h \
$$UAVOBJECT_SYNTHETICS/stabilizationdesired.h \
@ -72,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 \
@ -111,21 +121,23 @@ 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 \
$$UAVOBJECT_SYNTHETICS/airspeedstate.cpp \
$$UAVOBJECT_SYNTHETICS/attitudestate.cpp \
$$UAVOBJECT_SYNTHETICS/attitudesimulated.cpp \
$$UAVOBJECT_SYNTHETICS/altholdsmoothed.cpp \
$$UAVOBJECT_SYNTHETICS/altitudeholddesired.cpp \
$$UAVOBJECT_SYNTHETICS/altitudeholdsettings.cpp \
$$UAVOBJECT_SYNTHETICS/altitudeholdstatus.cpp \
$$UAVOBJECT_SYNTHETICS/debuglogsettings.cpp \
$$UAVOBJECT_SYNTHETICS/debuglogcontrol.cpp \
$$UAVOBJECT_SYNTHETICS/debuglogstatus.cpp \
@ -151,6 +163,10 @@ SOURCES += $$UAVOBJECT_SYNTHETICS/accessorydesired.cpp \
$$UAVOBJECT_SYNTHETICS/overosyncsettings.cpp \
$$UAVOBJECT_SYNTHETICS/systemsettings.cpp \
$$UAVOBJECT_SYNTHETICS/stabilizationsettings.cpp \
$$UAVOBJECT_SYNTHETICS/stabilizationsettingsbank1.cpp \
$$UAVOBJECT_SYNTHETICS/stabilizationsettingsbank2.cpp \
$$UAVOBJECT_SYNTHETICS/stabilizationsettingsbank3.cpp \
$$UAVOBJECT_SYNTHETICS/stabilizationbank.cpp \
$$UAVOBJECT_SYNTHETICS/manualcontrolsettings.cpp \
$$UAVOBJECT_SYNTHETICS/manualcontrolcommand.cpp \
$$UAVOBJECT_SYNTHETICS/stabilizationdesired.cpp \
@ -163,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 \
@ -203,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 \

15
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;
}
@ -301,6 +300,14 @@ QByteArray UAVObjectUtilManager::getBoardDescription()
return ret;
}
QString UAVObjectUtilManager::getBoardDescriptionString()
{
QByteArray arr = getBoardDescription();
int index = arr.indexOf(255);
return QString((index == -1) ? arr : arr.left(index));
}
// ******************************
// HomeLocation
@ -474,5 +481,3 @@ bool UAVObjectUtilManager::descriptionToStructure(QByteArray desc, deviceDescrip
}
return false;
}
// ******************************

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

@ -61,6 +61,7 @@ public:
QByteArray getBoardCPUSerial();
quint32 getFirmwareCRC();
QByteArray getBoardDescription();
QString getBoardDescriptionString();
deviceDescriptorStruct getBoardDescriptionStruct();
static bool descriptionToStructure(QByteArray desc, deviceDescriptorStruct & struc);
UAVObjectManager *getObjectManager();

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

@ -25,222 +25,186 @@
* 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA
*/
#include "configtaskwidget.h"
#include <QWidget>
#include <QLineEdit>
#include <uavtalk/telemetrymanager.h>
#include "uavsettingsimportexport/uavsettingsimportexportfactory.h"
/**
* Constructor
*/
ConfigTaskWidget::ConfigTaskWidget(QWidget *parent) : QWidget(parent), isConnected(false), allowWidgetUpdates(true), smartsave(NULL), dirty(false), outOfLimitsStyle("background-color: rgb(255, 0, 0);"), timeOut(NULL)
#include <QWidget>
#include <QLineEdit>
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)
{
pm = ExtensionSystem::PluginManager::instance();
objManager = pm->getObject<UAVObjectManager>();
TelemetryManager *telMngr = pm->getObject<TelemetryManager>();
utilMngr = pm->getObject<UAVObjectUtilManager>();
m_pluginManager = ExtensionSystem::PluginManager::instance();
TelemetryManager *telMngr = m_pluginManager->getObject<TelemetryManager>();
m_objectUtilManager = m_pluginManager->getObject<UAVObjectUtilManager>();
connect(telMngr, SIGNAL(connected()), this, SLOT(onAutopilotConnect()), Qt::UniqueConnection);
connect(telMngr, SIGNAL(disconnected()), this, SLOT(onAutopilotDisconnect()), Qt::UniqueConnection);
connect(telMngr, SIGNAL(connected()), this, SIGNAL(autoPilotConnected()), Qt::UniqueConnection);
connect(telMngr, SIGNAL(disconnected()), this, SIGNAL(autoPilotDisconnected()), Qt::UniqueConnection);
UAVSettingsImportExportFactory *importexportplugin = pm->getObject<UAVSettingsImportExportFactory>();
UAVSettingsImportExportFactory *importexportplugin = m_pluginManager->getObject<UAVSettingsImportExportFactory>();
connect(importexportplugin, SIGNAL(importAboutToBegin()), this, SLOT(invalidateObjects()));
}
/**
* Add a widget to the dirty detection pool
* @param widget to add to the detection pool
*/
void ConfigTaskWidget::addWidget(QWidget *widget)
{
addUAVObjectToWidgetRelation("", "", widget);
addWidgetBinding("", "", widget);
}
/**
* Add an object to the management system
* @param objectName name of the object to add to the management system
*/
void ConfigTaskWidget::addUAVObject(QString objectName, QList<int> *reloadGroups)
{
addUAVObjectToWidgetRelation(objectName, "", NULL, 0, 1, false, reloadGroups);
addWidgetBinding(objectName, "", NULL, 0, 1, false, reloadGroups);
}
void ConfigTaskWidget::addUAVObject(UAVObject *objectName, QList<int> *reloadGroups)
{
QString objstr;
if (objectName) {
objstr = objectName->getName();
}
addUAVObject(objstr, reloadGroups);
}
/**
* Add an UAVObject field to widget relation to the management system
* @param object name of the object to add
* @param field name of the field to add
* @param widget pointer to the widget whitch will display/define the field value
* @param index index of the field element to add to this relation
*/
void ConfigTaskWidget::addUAVObjectToWidgetRelation(QString object, QString field, QWidget *widget, QString index)
{
UAVObject *obj = NULL;
UAVObjectField *_field = NULL;
obj = objManager->getObject(QString(object));
Q_ASSERT(obj);
_field = obj->getField(QString(field));
Q_ASSERT(_field);
addUAVObjectToWidgetRelation(object, field, widget, _field->getElementNames().indexOf(index));
addUAVObject(objectName ? objectName->getName() : QString(""), reloadGroups);
}
void ConfigTaskWidget::addUAVObjectToWidgetRelation(UAVObject *obj, UAVObjectField *field, QWidget *widget, QString index)
int ConfigTaskWidget::fieldIndexFromElementName(QString objectName, QString fieldName, QString elementName)
{
QString objstr;
QString fieldstr;
if (elementName.isEmpty() || objectName.isEmpty()) {
return 0;
}
if (obj) {
objstr = obj->getName();
}
if (field) {
fieldstr = field->getName();
}
addUAVObjectToWidgetRelation(objstr, fieldstr, widget, index);
}
/**
* Add a UAVObject field to widget relation to the management system
* @param object name of the object to add
* @param field name of the field to add
* @param widget pointer to the widget whitch will display/define the field value
* @param element name of the element of the field element to add to this relation
* @param scale scale value of this relation
* @param isLimited bool to signal if this widget contents is limited in value
* @param defaultReloadGroups default and reload groups this relation belongs to
* @param instID instance ID of the object used on this relation
*/
void ConfigTaskWidget::addUAVObjectToWidgetRelation(QString object, QString field, QWidget *widget, QString element, double scale, bool isLimited, QList<int> *defaultReloadGroups, quint32 instID)
{
UAVObject *obj = objManager->getObject(QString(object), instID);
QString singleObjectName = mapObjectName(objectName).split(",").at(0);
UAVObject *object = getObject(singleObjectName);
Q_ASSERT(object);
Q_ASSERT(obj);
UAVObjectField *_field;
int index = 0;
if (!field.isEmpty() && obj) {
_field = obj->getField(QString(field));
if (!element.isEmpty()) {
index = _field->getElementNames().indexOf(QString(element));
}
}
addUAVObjectToWidgetRelation(object, field, widget, index, scale, isLimited, defaultReloadGroups, instID);
UAVObjectField *field = object->getField(fieldName);
Q_ASSERT(field);
return field->getElementNames().indexOf(elementName);
}
void ConfigTaskWidget::addUAVObjectToWidgetRelation(UAVObject *obj, UAVObjectField *field, QWidget *widget, QString element, double scale, bool isLimited, QList<int> *defaultReloadGroups, quint32 instID)
void ConfigTaskWidget::addWidgetBinding(QString objectName, QString fieldName, QWidget *widget, QString elementName)
{
QString objstr;
QString fieldstr;
if (obj) {
objstr = obj->getName();
}
if (field) {
fieldstr = field->getName();
}
addUAVObjectToWidgetRelation(objstr, fieldstr, widget, element, scale, isLimited, defaultReloadGroups, instID);
}
void ConfigTaskWidget::addUAVObjectToWidgetRelation(UAVObject *obj, UAVObjectField *field, QWidget *widget, int index, double scale, bool isLimited, QList<int> *defaultReloadGroups, quint32 instID)
{
QString objstr;
QString fieldstr;
if (obj) {
objstr = obj->getName();
}
if (field) {
fieldstr = field->getName();
}
addUAVObjectToWidgetRelation(objstr, fieldstr, widget, index, scale, isLimited, defaultReloadGroups, instID);
addWidgetBinding(objectName, fieldName, widget, fieldIndexFromElementName(objectName, fieldName, elementName));
}
/**
* Add an UAVObject field to widget relation to the management system
* @param object name of the object to add
* @param field name of the field to add
* @param widget pointer to the widget whitch will display/define the field value
* @param index index of the element of the field to add to this relation
* @param scale scale value of this relation
* @param isLimited bool to signal if this widget contents is limited in value
* @param defaultReloadGroups default and reload groups this relation belongs to
* @param instID instance ID of the object used on this relation
*/
void ConfigTaskWidget::addUAVObjectToWidgetRelation(QString object, QString field, QWidget *widget, int index, double scale, bool isLimited, QList<int> *defaultReloadGroups, quint32 instID)
void ConfigTaskWidget::addWidgetBinding(UAVObject *object, UAVObjectField *field, QWidget *widget, QString elementName)
{
if (addShadowWidget(object, field, widget, index, scale, isLimited, defaultReloadGroups, instID)) {
addWidgetBinding(object ? object->getName() : QString(""), field ? field->getName() : QString(""), widget, elementName);
}
void ConfigTaskWidget::addWidgetBinding(QString objectName, QString fieldName, QWidget *widget, QString elementName, double scale,
bool isLimited, QList<int> *reloadGroupIDs, quint32 instID)
{
addWidgetBinding(objectName, fieldName, widget, fieldIndexFromElementName(objectName, fieldName, elementName),
scale, isLimited, reloadGroupIDs, instID);
}
void ConfigTaskWidget::addWidgetBinding(UAVObject *object, UAVObjectField *field, QWidget *widget, QString elementName, double scale,
bool isLimited, QList<int> *reloadGroupIDs, quint32 instID)
{
addWidgetBinding(object ? object->getName() : QString(""), field ? field->getName() : QString(""), widget, elementName, scale,
isLimited, reloadGroupIDs, instID);
}
void ConfigTaskWidget::addWidgetBinding(UAVObject *object, UAVObjectField *field, QWidget *widget, int index, double scale,
bool isLimited, QList<int> *reloadGroupIDs, quint32 instID)
{
addWidgetBinding(object ? object->getName() : QString(""), field ? field->getName() : QString(""), widget, index, scale,
isLimited, reloadGroupIDs, instID);
}
void ConfigTaskWidget::addWidgetBinding(QString objectName, QString fieldName, QWidget *widget, int index, double scale,
bool isLimited, QList<int> *reloadGroupIDs, quint32 instID)
{
QString mappedObjectName = mapObjectName(objectName);
// If object name is comma separated list of objects, call one time per objectName
foreach(QString singleObjectName, mappedObjectName.split(",")) {
doAddWidgetBinding(singleObjectName, fieldName, widget, index, scale, isLimited, reloadGroupIDs, instID);
}
}
void ConfigTaskWidget::doAddWidgetBinding(QString objectName, QString fieldName, QWidget *widget, int index, double scale,
bool isLimited, QList<int> *reloadGroupIDs, quint32 instID)
{
if (addShadowWidgetBinding(objectName, fieldName, widget, index, scale, isLimited, reloadGroupIDs, instID)) {
return;
}
UAVObject *obj = NULL;
UAVObjectField *_field = NULL;
if (!object.isEmpty()) {
obj = objManager->getObject(QString(object), instID);
Q_ASSERT(obj);
objectUpdates.insert(obj, true);
connect(obj, SIGNAL(objectUpdated(UAVObject *)), this, SLOT(objectUpdated(UAVObject *)));
connect(obj, SIGNAL(objectUpdated(UAVObject *)), this, SLOT(refreshWidgetsValues(UAVObject *)), Qt::UniqueConnection);
UAVObject *object = NULL;
UAVObjectField *field = NULL;
if (!objectName.isEmpty()) {
object = getObject(QString(objectName), instID);
Q_ASSERT(object);
m_updatedObjects.insert(object, true);
connect(object, SIGNAL(objectUpdated(UAVObject *)), this, SLOT(objectUpdated(UAVObject *)));
connect(object, SIGNAL(objectUpdated(UAVObject *)), this, SLOT(refreshWidgetsValues(UAVObject *)), Qt::UniqueConnection);
}
if (!field.isEmpty() && obj) {
_field = obj->getField(QString(field));
if (!fieldName.isEmpty() && object) {
field = object->getField(QString(fieldName));
}
objectToWidget *ow = new objectToWidget();
ow->field = _field;
ow->object = obj;
ow->widget = widget;
ow->index = index;
ow->scale = scale;
ow->isLimited = isLimited;
objOfInterest.append(ow);
if (obj) {
if (smartsave) {
smartsave->addObject((UAVDataObject *)obj);
WidgetBinding *binding = new WidgetBinding(widget, object, field, index, scale, isLimited);
// Only the first binding per widget can be enabled.
binding->setIsEnabled(m_widgetBindingsPerWidget.count(widget) == 0);
m_widgetBindingsPerWidget.insert(widget, binding);
if (object) {
m_widgetBindingsPerObject.insert(object, binding);
if (m_saveButton) {
m_saveButton->addObject((UAVDataObject *)object);
}
}
if (widget == NULL) {
if (defaultReloadGroups && obj) {
foreach(int i, *defaultReloadGroups) {
if (this->defaultReloadGroups.contains(i)) {
this->defaultReloadGroups.value(i)->append(ow);
} else {
this->defaultReloadGroups.insert(i, new QList<objectToWidget *>());
this->defaultReloadGroups.value(i)->append(ow);
}
if (!widget) {
if (reloadGroupIDs && object) {
foreach(int groupId, *reloadGroupIDs) {
m_reloadGroups.insert(groupId, binding);
}
}
} else {
connectWidgetUpdatesToSlot(widget, SLOT(widgetsContentsChanged()));
if (defaultReloadGroups) {
addWidgetToDefaultReloadGroups(widget, defaultReloadGroups);
if (reloadGroupIDs) {
addWidgetToReloadGroups(widget, reloadGroupIDs);
}
if (binding->isEnabled()) {
loadWidgetLimits(widget, field, index, isLimited, scale);
}
shadowsList.insert(widget, ow);
loadWidgetLimits(widget, _field, index, isLimited, scale);
}
}
/**
* destructor
*/
void ConfigTaskWidget::setWidgetBindingObjectEnabled(QString objectName, bool enabled)
{
UAVObject *object = getObject(objectName);
Q_ASSERT(object);
bool dirtyBack = isDirty();
foreach(WidgetBinding * binding, m_widgetBindingsPerObject.values(object)) {
binding->setIsEnabled(enabled);
if (enabled) {
if (binding->value().isValid() && !binding->value().isNull()) {
setWidgetFromVariant(binding->widget(), binding->value(), binding->scale());
} else {
setWidgetFromField(binding->widget(), binding->field(), binding->index(), binding->scale(), binding->isLimited());
}
}
}
setDirty(dirtyBack);
}
ConfigTaskWidget::~ConfigTaskWidget()
{
if (smartsave) {
delete smartsave;
if (m_saveButton) {
delete m_saveButton;
}
foreach(QList<objectToWidget *> *pointer, defaultReloadGroups.values()) {
if (pointer) {
delete pointer;
QSet<WidgetBinding *> deleteSet = m_widgetBindingsPerWidget.values().toSet();
foreach(WidgetBinding * binding, deleteSet) {
if (binding) {
delete binding;
}
}
foreach(objectToWidget * oTw, objOfInterest) {
if (oTw) {
delete oTw;
}
}
if (timeOut) {
delete timeOut;
timeOut = NULL;
if (m_realtimeUpdateTimer) {
delete m_realtimeUpdateTimer;
m_realtimeUpdateTimer = NULL;
}
}
@ -254,10 +218,6 @@ void ConfigTaskWidget::saveObjectToSD(UAVObject *obj)
utilMngr->saveObjectToSD(obj);
}
/**
* Util function to get a pointer to the object manager
* @return pointer to the UAVObjectManager
*/
UAVObjectManager *ConfigTaskWidget::getObjectManager()
{
ExtensionSystem::PluginManager *pm = ExtensionSystem::PluginManager::instance();
@ -266,11 +226,7 @@ UAVObjectManager *ConfigTaskWidget::getObjectManager()
Q_ASSERT(objMngr);
return objMngr;
}
/**
* Utility function which calculates the Mean value of a list of values
* @param list list of double values
* @returns Mean value of the list of parameter values
*/
double ConfigTaskWidget::listMean(QList<double> list)
{
double accum = 0;
@ -281,11 +237,6 @@ double ConfigTaskWidget::listMean(QList<double> list)
return accum / list.size();
}
/**
* Utility function which calculates the Variance value of a list of values
* @param list list of double values
* @returns Variance of the list of parameter values
*/
double ConfigTaskWidget::listVar(QList<double> list)
{
double mean_accum = 0;
@ -305,155 +256,123 @@ double ConfigTaskWidget::listVar(QList<double> list)
return var_accum / (list.size() - 1);
}
// ************************************
// telemetry start/stop connect/disconnect signals
void ConfigTaskWidget::onAutopilotDisconnect()
{
isConnected = false;
m_isConnected = false;
enableControls(false);
invalidateObjects();
}
void ConfigTaskWidget::forceConnectedState() // dynamic widgets don't recieve the connected signal. This should be called instead.
{
isConnected = true;
m_isConnected = true;
setDirty(false);
}
void ConfigTaskWidget::onAutopilotConnect()
{
if (utilMngr) {
currentBoard = utilMngr->getBoardModel(); // TODO REMEMBER TO ADD THIS TO FORCE CONNECTED FUNC ON CC3D_RELEASE
if (m_objectUtilManager) {
m_currentBoardId = m_objectUtilManager->getBoardModel();
}
invalidateObjects();
isConnected = true;
foreach(objectToWidget * ow, objOfInterest) {
loadWidgetLimits(ow->widget, ow->field, ow->index, ow->isLimited, ow->scale);
m_isConnected = true;
foreach(WidgetBinding * binding, m_widgetBindingsPerObject) {
if (!binding->isEnabled()) {
continue;
}
loadWidgetLimits(binding->widget(), binding->field(), binding->index(), binding->isLimited(), binding->scale());
}
setDirty(false);
enableControls(true);
refreshWidgetsValues();
}
/**
* SLOT Function used to populate the widgets with the initial values
* Overwrite this if you need to change the default behavior
*/
void ConfigTaskWidget::populateWidgets()
{
bool dirtyBack = dirty;
bool dirtyBack = isDirty();
emit populateWidgetsRequested();
foreach(objectToWidget * ow, objOfInterest) {
if (ow->object == NULL || ow->field == NULL || ow->widget == NULL) {
// do nothing
} else {
setWidgetFromField(ow->widget, ow->field, ow->index, ow->scale, ow->isLimited);
foreach(WidgetBinding * binding, m_widgetBindingsPerObject) {
if (binding->isEnabled() && binding->object() != NULL && binding->field() != NULL && binding->widget() != NULL) {
setWidgetFromField(binding->widget(), binding->field(), binding->index(), binding->scale(), binding->isLimited());
}
}
setDirty(dirtyBack);
}
/**
* SLOT function used to refresh the widgets contents of the widgets with relation to
* object field added to the framework pool
* Overwrite this if you need to change the default behavior
*/
void ConfigTaskWidget::refreshWidgetsValues(UAVObject *obj)
{
if (!allowWidgetUpdates) {
if (!m_isWidgetUpdatesAllowed) {
return;
}
bool dirtyBack = dirty;
bool dirtyBack = isDirty();
emit refreshWidgetsValuesRequested();
foreach(objectToWidget * ow, objOfInterest) {
if (ow->object == NULL || ow->field == NULL || ow->widget == NULL) {
// do nothing
} else {
if (ow->object == obj || obj == NULL) {
setWidgetFromField(ow->widget, ow->field, ow->index, ow->scale, ow->isLimited);
}
foreach(WidgetBinding * binding, m_widgetBindingsPerObject.values(obj)) {
if (binding->isEnabled() && binding->field() != NULL && binding->widget() != NULL) {
setWidgetFromField(binding->widget(), binding->field(), binding->index(), binding->scale(), binding->isLimited());
}
}
setDirty(dirtyBack);
}
/**
* SLOT function used to update the uavobject fields from widgets with relation to
* object field added to the framework pool
* Overwrite this if you need to change the default behavior
*/
void ConfigTaskWidget::updateObjectsFromWidgets()
{
emit updateObjectsFromWidgetsRequested();
foreach(objectToWidget * ow, objOfInterest) {
if (ow->object == NULL || ow->field == NULL) {} else {
setFieldFromWidget(ow->widget, ow->field, ow->index, ow->scale);
foreach(WidgetBinding * binding, m_widgetBindingsPerObject) {
if (binding->object() != NULL && binding->field() != NULL) {
binding->updateObjectFieldFromValue();
}
}
}
/**
* SLOT function used handle help button presses
* Overwrite this if you need to change the default behavior
*/
void ConfigTaskWidget::helpButtonPressed()
{
QString url = helpButtonList.value((QPushButton *)sender(), QString());
QString url = m_helpButtons.value((QPushButton *)sender(), QString());
if (!url.isEmpty()) {
QDesktopServices::openUrl(QUrl(url, QUrl::StrictMode));
}
}
/**
* Add update and save buttons to the form
* multiple buttons can be added for the same function
* @param update pointer to the update button
* @param save pointer to the save button
*/
void ConfigTaskWidget::addApplySaveButtons(QPushButton *update, QPushButton *save)
{
if (!smartsave) {
smartsave = new smartSaveButton(this);
connect(smartsave, SIGNAL(preProcessOperations()), this, SLOT(updateObjectsFromWidgets()));
connect(smartsave, SIGNAL(saveSuccessfull()), this, SLOT(clearDirty()));
connect(smartsave, SIGNAL(beginOp()), this, SLOT(disableObjUpdates()));
connect(smartsave, SIGNAL(endOp()), this, SLOT(enableObjUpdates()));
if (!m_saveButton) {
m_saveButton = new SmartSaveButton(this);
connect(m_saveButton, SIGNAL(preProcessOperations()), this, SLOT(updateObjectsFromWidgets()));
connect(m_saveButton, SIGNAL(saveSuccessfull()), this, SLOT(clearDirty()));
connect(m_saveButton, SIGNAL(beginOp()), this, SLOT(disableObjectUpdates()));
connect(m_saveButton, SIGNAL(endOp()), this, SLOT(enableObjectUpdates()));
}
if (update && save) {
smartsave->addButtons(save, update);
m_saveButton->addButtons(save, update);
} else if (update) {
smartsave->addApplyButton(update);
m_saveButton->addApplyButton(update);
} else if (save) {
smartsave->addSaveButton(save);
m_saveButton->addSaveButton(save);
}
if (objOfInterest.count() > 0) {
foreach(objectToWidget * oTw, objOfInterest) {
smartsave->addObject((UAVDataObject *)oTw->object);
}
foreach(WidgetBinding * binding, m_widgetBindingsPerWidget) {
m_saveButton->addObject((UAVDataObject *)binding->object());
}
updateEnableControls();
}
/**
* SLOT function used the enable or disable the SAVE, UPLOAD and RELOAD buttons
* @param enable set to true to enable the buttons or false to disable them
* @param field name of the field to add
*/
void ConfigTaskWidget::enableControls(bool enable)
{
if (smartsave) {
smartsave->enableControls(enable);
if (m_saveButton) {
m_saveButton->enableControls(enable);
}
foreach(QPushButton * button, reloadButtonList) {
foreach(QPushButton * button, m_reloadButtons) {
button->setEnabled(enable);
}
foreach(objectToWidget * ow, objOfInterest) {
if (ow->widget) {
ow->widget->setEnabled(enable);
foreach(shadow * sh, ow->shadowsList) {
sh->widget->setEnabled(enable);
foreach(WidgetBinding * binding, m_widgetBindingsPerObject) {
if (binding->isEnabled() && binding->widget()) {
binding->widget()->setEnabled(enable);
foreach(ShadowWidgetBinding * shadow, binding->shadows()) {
shadow->widget()->setEnabled(enable);
}
}
}
@ -465,233 +384,187 @@ bool ConfigTaskWidget::shouldObjectBeSaved(UAVObject *object)
return true;
}
/**
* SLOT function called when on of the widgets contents added to the framework changes
*/
void ConfigTaskWidget::forceShadowUpdates()
{
foreach(objectToWidget * oTw, objOfInterest) {
foreach(shadow * sh, oTw->shadowsList) {
disconnectWidgetUpdatesToSlot((QWidget *)sh->widget, SLOT(widgetsContentsChanged()));
checkWidgetsLimits(sh->widget, oTw->field, oTw->index, sh->isLimited, getVariantFromWidget(oTw->widget, oTw->scale, oTw->getUnits()), sh->scale);
setWidgetFromVariant(sh->widget, getVariantFromWidget(oTw->widget, oTw->scale, oTw->getUnits()), sh->scale);
emit widgetContentsChanged((QWidget *)sh->widget);
connectWidgetUpdatesToSlot((QWidget *)sh->widget, SLOT(widgetsContentsChanged()));
foreach(WidgetBinding * binding, m_widgetBindingsPerObject) {
if (!binding->isEnabled()) {
continue;
}
QVariant widgetValue = getVariantFromWidget(binding->widget(), binding->scale(), binding->units());
foreach(ShadowWidgetBinding * shadow, binding->shadows()) {
disconnectWidgetUpdatesToSlot(shadow->widget(), SLOT(widgetsContentsChanged()));
checkWidgetsLimits(shadow->widget(), binding->field(), binding->index(), shadow->isLimited(), widgetValue, shadow->scale());
setWidgetFromVariant(shadow->widget(), widgetValue, shadow->scale());
emit widgetContentsChanged(shadow->widget());
connectWidgetUpdatesToSlot(shadow->widget(), SLOT(widgetsContentsChanged()));
}
}
setDirty(true);
}
/**
* SLOT function called when one of the widgets contents added to the framework changes
*/
void ConfigTaskWidget::widgetsContentsChanged()
{
emit widgetContentsChanged((QWidget *)sender());
QWidget *emitter = ((QWidget *)sender());
emit widgetContentsChanged(emitter);
double scale;
objectToWidget *oTw = shadowsList.value((QWidget *)sender(), NULL);
QVariant value;
if (oTw) {
if (oTw->widget == (QWidget *)sender()) {
scale = oTw->scale;
checkWidgetsLimits((QWidget *)sender(), oTw->field, oTw->index, oTw->isLimited, getVariantFromWidget((QWidget *)sender(),
oTw->scale, oTw->getUnits()), oTw->scale);
} else {
foreach(shadow * sh, oTw->shadowsList) {
if (sh->widget == (QWidget *)sender()) {
scale = sh->scale;
checkWidgetsLimits((QWidget *)sender(), oTw->field, oTw->index, sh->isLimited, getVariantFromWidget((QWidget *)sender(),
scale, oTw->getUnits()), scale);
foreach(WidgetBinding * binding, m_widgetBindingsPerWidget.values(emitter)) {
if (binding && binding->isEnabled()) {
if (binding->widget() == emitter) {
scale = binding->scale();
checkWidgetsLimits(emitter, binding->field(), binding->index(), binding->isLimited(),
getVariantFromWidget(emitter, scale, binding->units()), scale);
} else {
foreach(ShadowWidgetBinding * shadow, binding->shadows()) {
if (shadow->widget() == emitter) {
scale = shadow->scale();
checkWidgetsLimits(emitter, binding->field(), binding->index(), shadow->isLimited(),
getVariantFromWidget(emitter, scale, binding->units()), scale);
}
}
}
value = getVariantFromWidget(emitter, scale, binding->units());
binding->setValue(value);
if (binding->widget() != emitter) {
disconnectWidgetUpdatesToSlot(binding->widget(), SLOT(widgetsContentsChanged()));
checkWidgetsLimits(binding->widget(), binding->field(), binding->index(), binding->isLimited(),
value, binding->scale());
setWidgetFromVariant(binding->widget(), value, binding->scale());
emit widgetContentsChanged(binding->widget());
connectWidgetUpdatesToSlot(binding->widget(), SLOT(widgetsContentsChanged()));
}
foreach(ShadowWidgetBinding * shadow, binding->shadows()) {
if (shadow->widget() != emitter) {
disconnectWidgetUpdatesToSlot(shadow->widget(), SLOT(widgetsContentsChanged()));
checkWidgetsLimits(shadow->widget(), binding->field(), binding->index(), shadow->isLimited(),
value, shadow->scale());
setWidgetFromVariant(shadow->widget(), value, shadow->scale());
emit widgetContentsChanged(shadow->widget());
connectWidgetUpdatesToSlot(shadow->widget(), SLOT(widgetsContentsChanged()));
}
}
}
if (oTw->widget != (QWidget *)sender()) {
disconnectWidgetUpdatesToSlot((QWidget *)oTw->widget, SLOT(widgetsContentsChanged()));
checkWidgetsLimits(oTw->widget, oTw->field, oTw->index, oTw->isLimited, getVariantFromWidget((QWidget *)sender(), scale, oTw->getUnits()), oTw->scale);
setWidgetFromVariant(oTw->widget, getVariantFromWidget((QWidget *)sender(), scale, oTw->getUnits()), oTw->scale);
emit widgetContentsChanged((QWidget *)oTw->widget);
connectWidgetUpdatesToSlot((QWidget *)oTw->widget, SLOT(widgetsContentsChanged()));
}
foreach(shadow * sh, oTw->shadowsList) {
if (sh->widget != (QWidget *)sender()) {
disconnectWidgetUpdatesToSlot((QWidget *)sh->widget, SLOT(widgetsContentsChanged()));
checkWidgetsLimits(sh->widget, oTw->field, oTw->index, sh->isLimited, getVariantFromWidget((QWidget *)sender(), scale, oTw->getUnits()), sh->scale);
setWidgetFromVariant(sh->widget, getVariantFromWidget((QWidget *)sender(), scale, oTw->getUnits()), sh->scale);
emit widgetContentsChanged((QWidget *)sh->widget);
connectWidgetUpdatesToSlot((QWidget *)sh->widget, SLOT(widgetsContentsChanged()));
}
}
}
if (smartsave) {
smartsave->resetIcons();
if (m_saveButton) {
m_saveButton->resetIcons();
}
setDirty(true);
}
/**
* SLOT function used clear the forms dirty status flag
*/
void ConfigTaskWidget::clearDirty()
{
setDirty(false);
}
/**
* Sets the form's dirty status flag
* @param value
*/
void ConfigTaskWidget::setDirty(bool value)
{
dirty = value;
m_isDirty = value;
}
/**
* Checks if the form is dirty (unsaved changes)
* @return true if the form has unsaved changes
*/
bool ConfigTaskWidget::isDirty()
{
if (isConnected) {
return dirty;
if (m_isConnected) {
return m_isDirty;
} else {
return false;
}
}
/**
* SLOT function used to disable widget contents changes when related object field changes
*/
void ConfigTaskWidget::disableObjUpdates()
void ConfigTaskWidget::disableObjectUpdates()
{
allowWidgetUpdates = false;
foreach(objectToWidget * obj, objOfInterest) {
if (obj->object) {
disconnect(obj->object, SIGNAL(objectUpdated(UAVObject *)), this, SLOT(refreshWidgetsValues(UAVObject *)));
m_isWidgetUpdatesAllowed = false;
foreach(WidgetBinding * binding, m_widgetBindingsPerWidget) {
if (binding->object()) {
disconnect(binding->object(), SIGNAL(objectUpdated(UAVObject *)), this, SLOT(refreshWidgetsValues(UAVObject *)));
}
}
}
/**
* SLOT function used to enable widget contents changes when related object field changes
*/
void ConfigTaskWidget::enableObjUpdates()
void ConfigTaskWidget::enableObjectUpdates()
{
allowWidgetUpdates = true;
foreach(objectToWidget * obj, objOfInterest) {
if (obj->object) {
connect(obj->object, SIGNAL(objectUpdated(UAVObject *)), this, SLOT(refreshWidgetsValues(UAVObject *)), Qt::UniqueConnection);
m_isWidgetUpdatesAllowed = true;
foreach(WidgetBinding * binding, m_widgetBindingsPerWidget) {
if (binding->object()) {
connect(binding->object(), SIGNAL(objectUpdated(UAVObject *)), this, SLOT(refreshWidgetsValues(UAVObject *)), Qt::UniqueConnection);
}
}
}
/**
* Called when an uav object is updated
* @param obj pointer to the object whitch has just been updated
*/
void ConfigTaskWidget::objectUpdated(UAVObject *obj)
void ConfigTaskWidget::objectUpdated(UAVObject *object)
{
objectUpdates[obj] = true;
m_updatedObjects[object] = true;
}
/**
* Checks if all objects added to the pool have already been updated
* @return true if all objects added to the pool have already been updated
*/
bool ConfigTaskWidget::allObjectsUpdated()
{
qDebug() << "ConfigTaskWidge:allObjectsUpdated called";
bool ret = true;
foreach(UAVObject * obj, objectUpdates.keys()) {
ret = ret & objectUpdates[obj];
bool result = true;
foreach(UAVObject * object, m_updatedObjects.keys()) {
result = result & m_updatedObjects[object];
}
qDebug() << "Returned:" << ret;
return ret;
return result;
}
/**
* Adds a new help button
* @param button pointer to the help button
* @param url url to open in the browser when the help button is pressed
*/
void ConfigTaskWidget::addHelpButton(QPushButton *button, QString url)
{
helpButtonList.insert(button, url);
m_helpButtons.insert(button, url);
connect(button, SIGNAL(clicked()), this, SLOT(helpButtonPressed()));
}
/**
* Invalidates all the uav objects "is updated" flag
*/
void ConfigTaskWidget::invalidateObjects()
{
foreach(UAVObject * obj, objectUpdates.keys()) {
objectUpdates[obj] = false;
foreach(UAVObject * obj, m_updatedObjects.keys()) {
m_updatedObjects[obj] = false;
}
}
/**
* SLOT call this to apply changes to uav objects
*/
void ConfigTaskWidget::apply()
{
if (smartsave) {
smartsave->apply();
if (m_saveButton) {
m_saveButton->apply();
}
}
/**
* SLOT call this to save changes to uav objects
*/
void ConfigTaskWidget::save()
{
if (smartsave) {
smartsave->save();
if (m_saveButton) {
m_saveButton->save();
}
}
/**
* Adds a new shadow widget
* shadow widgets are widgets whitch have a relation to an object already present on the framework pool i.e. already added trough addUAVObjectToWidgetRelation
* This function doesn't have to be used directly, addUAVObjectToWidgetRelation will call it if a previous relation exhists.
* @return returns false if the shadow widget relation failed to be added (no previous relation exhisted)
*/
bool ConfigTaskWidget::addShadowWidget(QString object, QString field, QWidget *widget, int index, double scale, bool isLimited,
QList<int> *defaultReloadGroups, quint32 instID)
bool ConfigTaskWidget::addShadowWidgetBinding(QString objectName, QString fieldName, QWidget *widget, int index, double scale, bool isLimited,
QList<int> *defaultReloadGroups, quint32 instID)
{
foreach(objectToWidget * oTw, objOfInterest) {
if (!oTw->object || !oTw->widget || !oTw->field) {
foreach(WidgetBinding * binding, m_widgetBindingsPerWidget) {
if (!binding->object() || !binding->widget() || !binding->field()) {
continue;
}
if (oTw->object->getName() == object && oTw->field->getName() == field && oTw->index == index && oTw->object->getInstID() == instID) {
shadow *sh = NULL;
// prefer anything else to QLabel
if (qobject_cast<QLabel *>(oTw->widget) && !qobject_cast<QLabel *>(widget)) {
sh = new shadow;
sh->isLimited = oTw->isLimited;
sh->scale = oTw->scale;
sh->widget = oTw->widget;
oTw->isLimited = isLimited;
oTw->scale = scale;
oTw->widget = widget;
}
// prefer QDoubleSpinBox to anything else
else if (!qobject_cast<QDoubleSpinBox *>(oTw->widget) && qobject_cast<QDoubleSpinBox *>(widget)) {
sh = new shadow;
sh->isLimited = oTw->isLimited;
sh->scale = oTw->scale;
sh->widget = oTw->widget;
oTw->isLimited = isLimited;
oTw->scale = scale;
oTw->widget = widget;
} else {
sh = new shadow;
sh->isLimited = isLimited;
sh->scale = scale;
sh->widget = widget;
}
shadowsList.insert(widget, oTw);
oTw->shadowsList.append(sh);
if (binding->matches(objectName, fieldName, index, instID)) {
binding->addShadow(widget, scale, isLimited);
m_widgetBindingsPerWidget.insert(widget, binding);
connectWidgetUpdatesToSlot(widget, SLOT(widgetsContentsChanged()));
if (defaultReloadGroups) {
addWidgetToDefaultReloadGroups(widget, defaultReloadGroups);
addWidgetToReloadGroups(widget, defaultReloadGroups);
}
if (!binding->isEnabled()) {
loadWidgetLimits(widget, binding->field(), binding->index(), isLimited, scale);
}
loadWidgetLimits(widget, oTw->field, oTw->index, isLimited, scale);
return true;
}
}
return false;
}
/**
* Auto loads widgets based on the Dynamic property named "objrelation"
* Check the wiki for more information
*/
void ConfigTaskWidget::autoLoadWidgets()
{
QPushButton *saveButtonWidget = NULL;
@ -701,7 +574,7 @@ void ConfigTaskWidget::autoLoadWidgets()
QVariant info = widget->property("objrelation");
if (info.isValid()) {
uiRelationAutomation uiRelation;
bindingStruct uiRelation;
uiRelation.buttonType = none;
uiRelation.scale = 1;
uiRelation.element = QString();
@ -788,119 +661,105 @@ void ConfigTaskWidget::autoLoadWidgets()
} else {
QWidget *wid = qobject_cast<QWidget *>(widget);
if (wid) {
addUAVObjectToWidgetRelation(uiRelation.objname, uiRelation.fieldname, wid, uiRelation.element, uiRelation.scale, uiRelation.haslimits, &uiRelation.buttonGroup);
addWidgetBinding(uiRelation.objname, uiRelation.fieldname, wid, uiRelation.element, uiRelation.scale, uiRelation.haslimits, &uiRelation.buttonGroup);
}
}
}
}
refreshWidgetsValues();
forceShadowUpdates();
foreach(objectToWidget * ow, objOfInterest) {
if (ow->widget) {
qDebug() << "Master:" << ow->widget->objectName();
}
foreach(shadow * sh, ow->shadowsList) {
if (sh->widget) {
qDebug() << "Child" << sh->widget->objectName();
}
}
}
}
/**
* Adds a widget to a list of default/reload groups
* default/reload groups are groups of widgets to be set with default or reloaded (values from persistent memory) when a defined button is pressed
* @param widget pointer to the widget to be added to the groups
* @param groups list of the groups on which to add the widget
*/
void ConfigTaskWidget::addWidgetToDefaultReloadGroups(QWidget *widget, QList<int> *groups)
{
foreach(objectToWidget * oTw, objOfInterest) {
bool addOTW = false;
if (oTw->widget == widget) {
addOTW = true;
/*
foreach(WidgetBinding * binding, m_widgetBindingsPerObject) {
if (binding->widget()) {
qDebug() << "Binding :" << binding->widget()->objectName();
qDebug() << " Object :" << binding->object()->getName();
qDebug() << " Field :" << binding->field()->getName();
qDebug() << " Scale :" << binding->scale();
qDebug() << " Enabled:" << binding->isEnabled();
}
foreach(ShadowWidgetBinding * shadow, binding->shadows()) {
if (shadow->widget()) {
qDebug() << " Shadow:" << shadow->widget()->objectName();
qDebug() << " Scale :" << shadow->scale();
}
}
}
*/
}
void ConfigTaskWidget::addWidgetToReloadGroups(QWidget *widget, QList<int> *reloadGroupIDs)
{
foreach(WidgetBinding * binding, m_widgetBindingsPerWidget) {
bool addBinding = false;
if (binding->widget() == widget) {
addBinding = true;
} else {
foreach(shadow * sh, oTw->shadowsList) {
if (sh->widget == widget) {
addOTW = true;
foreach(ShadowWidgetBinding * shadow, binding->shadows()) {
if (shadow->widget() == widget) {
addBinding = true;
}
}
}
if (addOTW) {
foreach(int i, *groups) {
if (defaultReloadGroups.contains(i)) {
defaultReloadGroups.value(i)->append(oTw);
} else {
defaultReloadGroups.insert(i, new QList<objectToWidget *>());
defaultReloadGroups.value(i)->append(oTw);
}
if (addBinding) {
foreach(int groupID, *reloadGroupIDs) {
m_reloadGroups.insert(groupID, binding);
}
}
}
}
/**
* Adds a button to a default group
* @param button pointer to the default button
* @param buttongroup number of the group
*/
void ConfigTaskWidget::addDefaultButton(QPushButton *button, int buttonGroup)
{
button->setProperty("group", buttonGroup);
connect(button, SIGNAL(clicked()), this, SLOT(defaultButtonClicked()));
}
/**
* Adds a button to a reload group
* @param button pointer to the reload button
* @param buttongroup number of the group
*/
void ConfigTaskWidget::addReloadButton(QPushButton *button, int buttonGroup)
{
button->setProperty("group", buttonGroup);
reloadButtonList.append(button);
m_reloadButtons.append(button);
connect(button, SIGNAL(clicked()), this, SLOT(reloadButtonClicked()));
}
/**
* Called when a default button is clicked
*/
void ConfigTaskWidget::defaultButtonClicked()
{
int group = sender()->property("group").toInt();
emit defaultRequested(group);
int groupID = sender()->property("group").toInt();
emit defaultRequested(groupID);
QList<objectToWidget *> *list = defaultReloadGroups.value(group);
foreach(objectToWidget * oTw, *list) {
if (!oTw->object || !oTw->field) {
QList<WidgetBinding *> bindings = m_reloadGroups.values(groupID);
foreach(WidgetBinding * binding, bindings) {
if (!binding->isEnabled() || !binding->object() || !binding->field()) {
continue;
}
UAVDataObject *temp = ((UAVDataObject *)oTw->object)->dirtyClone();
setWidgetFromField(oTw->widget, temp->getField(oTw->field->getName()), oTw->index, oTw->scale, oTw->isLimited);
UAVDataObject *temp = ((UAVDataObject *)binding->object())->dirtyClone();
setWidgetFromField(binding->widget(), temp->getField(binding->field()->getName()), binding->index(), binding->scale(), binding->isLimited());
}
}
/**
* Called when a reload button is clicked
*/
void ConfigTaskWidget::reloadButtonClicked()
{
if (timeOut) {
if (m_realtimeUpdateTimer) {
return;
}
int group = sender()->property("group").toInt();
QList<objectToWidget *> *list = defaultReloadGroups.value(group, NULL);
if (!list) {
int groupID = sender()->property("group").toInt();
QList<WidgetBinding *> bindings = m_reloadGroups.values(groupID);
if (!bindings.isEmpty()) {
return;
}
ObjectPersistence *objper = dynamic_cast<ObjectPersistence *>(getObjectManager()->getObject(ObjectPersistence::NAME));
timeOut = new QTimer(this);
m_realtimeUpdateTimer = new QTimer(this);
QEventLoop *eventLoop = new QEventLoop(this);
connect(timeOut, SIGNAL(timeout()), eventLoop, SLOT(quit()));
connect(m_realtimeUpdateTimer, SIGNAL(timeout()), eventLoop, SLOT(quit()));
connect(objper, SIGNAL(objectUpdated(UAVObject *)), eventLoop, SLOT(quit()));
QList<temphelper> temp;
foreach(objectToWidget * oTw, *list) {
if (oTw->object != NULL) {
temphelper value;
value.objid = oTw->object->getObjID();
value.objinstid = oTw->object->getInstID();
QList<objectComparator> temp;
foreach(WidgetBinding * binding, bindings) {
if (binding->isEnabled() && binding->object() != NULL) {
objectComparator value;
value.objid = binding->object()->getObjID();
value.objinstid = binding->object()->getInstID();
if (temp.contains(value)) {
continue;
} else {
@ -909,62 +768,57 @@ void ConfigTaskWidget::reloadButtonClicked()
ObjectPersistence::DataFields data;
data.Operation = ObjectPersistence::OPERATION_LOAD;
data.Selection = ObjectPersistence::SELECTION_SINGLEOBJECT;
data.ObjectID = oTw->object->getObjID();
data.InstanceID = oTw->object->getInstID();
data.ObjectID = binding->object()->getObjID();
data.InstanceID = binding->object()->getInstID();
objper->setData(data);
objper->updated();
timeOut->start(500);
m_realtimeUpdateTimer->start(500);
eventLoop->exec();
if (timeOut->isActive()) {
oTw->object->requestUpdate();
if (oTw->widget) {
setWidgetFromField(oTw->widget, oTw->field, oTw->index, oTw->scale, oTw->isLimited);
if (m_realtimeUpdateTimer->isActive()) {
binding->object()->requestUpdate();
if (binding->widget()) {
setWidgetFromField(binding->widget(), binding->field(), binding->index(), binding->scale(), binding->isLimited());
}
}
timeOut->stop();
m_realtimeUpdateTimer->stop();
}
}
if (eventLoop) {
delete eventLoop;
eventLoop = NULL;
}
if (timeOut) {
delete timeOut;
timeOut = NULL;
if (m_realtimeUpdateTimer) {
delete m_realtimeUpdateTimer;
m_realtimeUpdateTimer = NULL;
}
}
/**
* Connects widgets "contents changed" signals to a slot
*/
void ConfigTaskWidget::connectWidgetUpdatesToSlot(QWidget *widget, const char *function)
{
if (!widget) {
return;
}
if (QComboBox * cb = qobject_cast<QComboBox *>(widget)) {
connect(cb, SIGNAL(currentIndexChanged(int)), this, function);
connect(cb, SIGNAL(currentIndexChanged(int)), this, function, Qt::UniqueConnection);
} else if (QSlider * cb = qobject_cast<QSlider *>(widget)) {
connect(cb, SIGNAL(valueChanged(int)), this, function);
connect(cb, SIGNAL(valueChanged(int)), this, function, Qt::UniqueConnection);
} else if (MixerCurveWidget * cb = qobject_cast<MixerCurveWidget *>(widget)) {
connect(cb, SIGNAL(curveUpdated()), this, function);
connect(cb, SIGNAL(curveUpdated()), this, function, Qt::UniqueConnection);
} else if (QTableWidget * cb = qobject_cast<QTableWidget *>(widget)) {
connect(cb, SIGNAL(cellChanged(int, int)), this, function);
connect(cb, SIGNAL(cellChanged(int, int)), this, function, Qt::UniqueConnection);
} else if (QSpinBox * cb = qobject_cast<QSpinBox *>(widget)) {
connect(cb, SIGNAL(valueChanged(int)), this, function);
connect(cb, SIGNAL(valueChanged(int)), this, function, Qt::UniqueConnection);
} else if (QDoubleSpinBox * cb = qobject_cast<QDoubleSpinBox *>(widget)) {
connect(cb, SIGNAL(valueChanged(double)), this, function);
connect(cb, SIGNAL(valueChanged(double)), this, function, Qt::UniqueConnection);
} else if (QCheckBox * cb = qobject_cast<QCheckBox *>(widget)) {
connect(cb, SIGNAL(stateChanged(int)), this, function);
connect(cb, SIGNAL(stateChanged(int)), this, function, Qt::UniqueConnection);
} else if (QPushButton * cb = qobject_cast<QPushButton *>(widget)) {
connect(cb, SIGNAL(clicked()), this, function);
connect(cb, SIGNAL(clicked()), this, function, Qt::UniqueConnection);
} else {
qDebug() << __FUNCTION__ << "widget to uavobject relation not implemented" << widget->metaObject()->className();
qDebug() << __FUNCTION__ << "widget binding not implemented" << widget->metaObject()->className();
}
}
/**
* Disconnects widgets "contents changed" signals to a slot
*/
void ConfigTaskWidget::disconnectWidgetUpdatesToSlot(QWidget *widget, const char *function)
{
if (!widget) {
@ -987,38 +841,10 @@ void ConfigTaskWidget::disconnectWidgetUpdatesToSlot(QWidget *widget, const char
} else if (QPushButton * cb = qobject_cast<QPushButton *>(widget)) {
disconnect(cb, SIGNAL(clicked()), this, function);
} else {
qDebug() << __FUNCTION__ << "widget to uavobject relation not implemented" << widget->metaObject()->className();
qDebug() << __FUNCTION__ << "widget binding not implemented" << widget->metaObject()->className();
}
}
/**
* Sets a widget value from an UAVObject field
* @param widget pointer for the widget to set
* @param field pointer to the UAVObject field to use
* @param index index of the element to use
* @param scale scale to be used on the assignement
* @return returns true if the assignement was successfull
*/
bool ConfigTaskWidget::setFieldFromWidget(QWidget *widget, UAVObjectField *field, int index, double scale)
{
if (!widget || !field) {
return false;
}
QVariant ret = getVariantFromWidget(widget, scale, field->getUnits());
if (ret.isValid()) {
field->setValue(ret, index);
return true;
}
{
qDebug() << __FUNCTION__ << "widget to uavobject relation not implemented" << widget->metaObject()->className();
return false;
}
}
/**
* Gets a variant from a widget
* @param widget pointer to the widget from where to get the value
* @param scale scale to be used on the assignement
* @return returns the value of the widget times the scale
*/
QVariant ConfigTaskWidget::getVariantFromWidget(QWidget *widget, double scale, QString units)
{
if (QComboBox * cb = qobject_cast<QComboBox *>(widget)) {
@ -1043,14 +869,7 @@ QVariant ConfigTaskWidget::getVariantFromWidget(QWidget *widget, double scale, Q
return QVariant();
}
}
/**
* Sets a widget from a variant
* @param widget pointer for the widget to set
* @param value value to be used on the assignement
* @param scale scale to be used on the assignement
* @param units the units for the value
* @return returns true if the assignement was successfull
*/
bool ConfigTaskWidget::setWidgetFromVariant(QWidget *widget, QVariant value, double scale, QString units)
{
if (QComboBox * cb = qobject_cast<QComboBox *>(widget)) {
@ -1092,27 +911,11 @@ bool ConfigTaskWidget::setWidgetFromVariant(QWidget *widget, QVariant value, dou
}
}
/**
* Sets a widget from a variant
* @param widget pointer for the widget to set
* @param value value to be used on the assignement
* @param scale scale to be used on the assignement
* @return returns true if the assignement was successfull
*/
bool ConfigTaskWidget::setWidgetFromVariant(QWidget *widget, QVariant value, double scale)
{
return setWidgetFromVariant(widget, value, scale, QString(""));
}
/**
* Sets a widget from a UAVObject field
* @param widget pointer to the widget to set
* @param field pointer to the field from where to get the value from
* @param index index of the element to use
* @param scale scale to be used on the assignement
* @param hasLimits set to true if you want to limit the values (check wiki)
* @return returns true if the assignement was successfull
*/
bool ConfigTaskWidget::setWidgetFromField(QWidget *widget, UAVObjectField *field, int index, double scale, bool hasLimits)
{
if (!widget || !field) {
@ -1123,26 +926,27 @@ bool ConfigTaskWidget::setWidgetFromField(QWidget *widget, UAVObjectField *field
loadWidgetLimits(cb, field, index, hasLimits, scale);
}
}
QVariant var = field->getValue(index);
checkWidgetsLimits(widget, field, index, hasLimits, var, scale);
bool ret = setWidgetFromVariant(widget, var, scale, field->getUnits());
if (ret) {
QVariant value = field->getValue(index);
checkWidgetsLimits(widget, field, index, hasLimits, value, scale);
bool result = setWidgetFromVariant(widget, value, scale, field->getUnits());
if (result) {
return true;
} else {
qDebug() << __FUNCTION__ << "widget to uavobject relation not implemented" << widget->metaObject()->className();
return false;
}
}
void ConfigTaskWidget::checkWidgetsLimits(QWidget *widget, UAVObjectField *field, int index, bool hasLimits, QVariant value, double scale)
{
if (!hasLimits) {
return;
}
if (!field->isWithinLimits(value, index, currentBoard)) {
if (!field->isWithinLimits(value, index, m_currentBoardId)) {
if (!widget->property("styleBackup").isValid()) {
widget->setProperty("styleBackup", widget->styleSheet());
}
widget->setStyleSheet(outOfLimitsStyle);
widget->setStyleSheet(m_outOfLimitsStyle);
widget->setProperty("wasOverLimits", (bool)true);
if (QComboBox * cb = qobject_cast<QComboBox *>(widget)) {
if (cb->findText(value.toString()) == -1) {
@ -1189,7 +993,7 @@ void ConfigTaskWidget::loadWidgetLimits(QWidget *widget, UAVObjectField *field,
QStringList option = field->getOptions();
if (hasLimits) {
foreach(QString str, option) {
if (field->isWithinLimits(str, index, currentBoard)) {
if (field->isWithinLimits(str, index, m_currentBoardId)) {
cb->addItem(str);
}
}
@ -1201,33 +1005,44 @@ void ConfigTaskWidget::loadWidgetLimits(QWidget *widget, UAVObjectField *field,
return;
} else if (QDoubleSpinBox * cb = qobject_cast<QDoubleSpinBox *>(widget)) {
if (field->getMaxLimit(index).isValid()) {
cb->setMaximum((double)(field->getMaxLimit(index, currentBoard).toDouble() / scale));
cb->setMaximum((double)(field->getMaxLimit(index, m_currentBoardId).toDouble() / scale));
}
if (field->getMinLimit(index, currentBoard).isValid()) {
cb->setMinimum((double)(field->getMinLimit(index, currentBoard).toDouble() / scale));
if (field->getMinLimit(index, m_currentBoardId).isValid()) {
cb->setMinimum((double)(field->getMinLimit(index, m_currentBoardId).toDouble() / scale));
}
} else if (QSpinBox * cb = qobject_cast<QSpinBox *>(widget)) {
if (field->getMaxLimit(index, currentBoard).isValid()) {
cb->setMaximum((int)qRound(field->getMaxLimit(index, currentBoard).toDouble() / scale));
if (field->getMaxLimit(index, m_currentBoardId).isValid()) {
cb->setMaximum((int)qRound(field->getMaxLimit(index, m_currentBoardId).toDouble() / scale));
}
if (field->getMinLimit(index, currentBoard).isValid()) {
cb->setMinimum((int)qRound(field->getMinLimit(index, currentBoard).toDouble() / scale));
if (field->getMinLimit(index, m_currentBoardId).isValid()) {
cb->setMinimum((int)qRound(field->getMinLimit(index, m_currentBoardId).toDouble() / scale));
}
} else if (QSlider * cb = qobject_cast<QSlider *>(widget)) {
if (field->getMaxLimit(index, currentBoard).isValid()) {
cb->setMaximum((int)qRound(field->getMaxLimit(index, currentBoard).toDouble() / scale));
if (field->getMaxLimit(index, m_currentBoardId).isValid()) {
cb->setMaximum((int)qRound(field->getMaxLimit(index, m_currentBoardId).toDouble() / scale));
}
if (field->getMinLimit(index, currentBoard).isValid()) {
cb->setMinimum((int)(field->getMinLimit(index, currentBoard).toDouble() / scale));
if (field->getMinLimit(index, m_currentBoardId).isValid()) {
cb->setMinimum((int)(field->getMinLimit(index, m_currentBoardId).toDouble() / scale));
}
}
}
UAVObject *ConfigTaskWidget::getObject(const QString name, quint32 instId)
{
return m_pluginManager->getObject<UAVObjectManager>()->getObject(name, instId);
}
QString ConfigTaskWidget::mapObjectName(const QString objectName)
{
return objectName;
}
void ConfigTaskWidget::updateEnableControls()
{
TelemetryManager *telMngr = pm->getObject<TelemetryManager>();
TelemetryManager *telMngr = m_pluginManager->getObject<TelemetryManager>();
Q_ASSERT(telMngr);
enableControls(telMngr->isConnected());
}
@ -1260,7 +1075,126 @@ bool ConfigTaskWidget::eventFilter(QObject *obj, QEvent *evt)
}
return QWidget::eventFilter(obj, evt);
}
/**
@}
@}
*/
WidgetBinding::WidgetBinding(QWidget *widget, UAVObject *object, UAVObjectField *field, int index, double scale, bool isLimited) :
ShadowWidgetBinding(widget, scale, isLimited), m_isEnabled(true)
{
m_object = object;
m_field = field;
m_index = index;
}
WidgetBinding::~WidgetBinding()
{}
QString WidgetBinding::units() const
{
if (m_field) {
return m_field->getUnits();
}
return QString("");
}
UAVObject *WidgetBinding::object() const
{
return m_object;
}
UAVObjectField *WidgetBinding::field() const
{
return m_field;
}
int WidgetBinding::index() const
{
return m_index;
}
QList<ShadowWidgetBinding *> WidgetBinding::shadows() const
{
return m_shadows;
}
void WidgetBinding::addShadow(QWidget *widget, double scale, bool isLimited)
{
ShadowWidgetBinding *shadow = NULL;
// Prefer anything else to QLabel and prefer QDoubleSpinBox to anything else
if ((qobject_cast<QLabel *>(m_widget) && !qobject_cast<QLabel *>(widget)) ||
(!qobject_cast<QDoubleSpinBox *>(m_widget) && qobject_cast<QDoubleSpinBox *>(widget))) {
shadow = new ShadowWidgetBinding(m_widget, m_scale, m_isLimited);
m_isLimited = isLimited;
m_scale = scale;
m_widget = widget;
} else {
shadow = new ShadowWidgetBinding(widget, scale, isLimited);
}
m_shadows.append(shadow);
}
bool WidgetBinding::matches(QString objectName, QString fieldName, int index, quint32 instanceId)
{
if (m_object && m_field) {
return m_object->getName() == objectName && m_object->getInstID() == instanceId &&
m_field->getName() == fieldName && m_index == index;
} else {
return false;
}
}
bool WidgetBinding::isEnabled() const
{
return m_isEnabled;
}
void WidgetBinding::setIsEnabled(bool isEnabled)
{
m_isEnabled = isEnabled;
}
QVariant WidgetBinding::value() const
{
return m_value;
}
void WidgetBinding::setValue(const QVariant &value)
{
m_value = value;
/*
if (m_object && m_field) {
qDebug() << "WidgetBinding" << m_object->getName() << ":" << m_field->getName() << "value =" << value.toString();
}
*/
}
void WidgetBinding::updateObjectFieldFromValue()
{
if (m_value.isValid()) {
m_field->setValue(m_value, m_index);
}
}
ShadowWidgetBinding::ShadowWidgetBinding(QWidget *widget, double scale, bool isLimited)
{
m_widget = widget;
m_scale = scale;
m_isLimited = isLimited;
}
ShadowWidgetBinding::~ShadowWidgetBinding()
{}
QWidget *ShadowWidgetBinding::widget() const
{
return m_widget;
}
double ShadowWidgetBinding::scale() const
{
return m_scale;
}
bool ShadowWidgetBinding::isLimited() const
{
return m_isLimited;
}

199
ground/openpilotgcs/src/plugins/uavobjectwidgetutils/configtaskwidget.h
View File

@ -27,7 +27,6 @@
#ifndef CONFIGTASKWIDGET_H
#define CONFIGTASKWIDGET_H
#include "extensionsystem/pluginmanager.h"
#include "uavobjectmanager.h"
#include "uavobject.h"
@ -48,53 +47,57 @@
#include <QUrl>
#include <QEvent>
class ShadowWidgetBinding : public QObject {
Q_OBJECT
public:
ShadowWidgetBinding(QWidget *widget, double scale, bool isLimited);
~ShadowWidgetBinding();
QWidget *widget() const;
double scale() const;
bool isLimited() const;
protected:
QWidget *m_widget;
double m_scale;
bool m_isLimited;
};
class WidgetBinding : public ShadowWidgetBinding {
Q_OBJECT
public:
WidgetBinding(QWidget *widget, UAVObject *object, UAVObjectField *field, int index, double scale, bool isLimited);
~WidgetBinding();
QString units() const;
UAVObject *object() const;
UAVObjectField *field() const;
int index() const;
QList<ShadowWidgetBinding *> shadows() const;
void addShadow(QWidget *widget, double scale, bool isLimited);
bool matches(QString objectName, QString fieldName, int index, quint32 instanceId);
bool isEnabled() const;
void setIsEnabled(bool isEnabled);
QVariant value() const;
void setValue(const QVariant &value);
void updateObjectFieldFromValue();
private:
UAVObject *m_object;
UAVObjectField *m_field;
int m_index;
bool m_isEnabled;
QList<ShadowWidgetBinding *> m_shadows;
QVariant m_value;
};
class UAVOBJECTWIDGETUTILS_EXPORT ConfigTaskWidget : public QWidget {
Q_OBJECT
public:
struct shadow {
QWidget *widget;
double scale;
bool isLimited;
};
struct objectToWidget {
UAVObject *object;
UAVObjectField *field;
QWidget *widget;
int index;
double scale;
bool isLimited;
QList<shadow *> shadowsList;
QString getUnits() const
{
if (field) {
return field->getUnits();
}
return QString("");
}
};
struct temphelper {
quint32 objid;
quint32 objinstid;
bool operator==(const temphelper & lhs)
{
return lhs.objid == this->objid && lhs.objinstid == this->objinstid;
}
};
enum buttonTypeEnum { none, save_button, apply_button, reload_button, default_button, help_button };
struct uiRelationAutomation {
QString objname;
QString fieldname;
QString element;
QString url;
buttonTypeEnum buttonType;
QList<int> buttonGroup;
double scale;
bool haslimits;
};
ConfigTaskWidget(QWidget *parent = 0);
virtual ~ConfigTaskWidget();
@ -111,25 +114,27 @@ public:
void addWidget(QWidget *widget);
void addUAVObjectToWidgetRelation(QString object, QString field, QWidget *widget, int index = 0, double scale = 1, bool isLimited = false, QList<int> *defaultReloadGroups = 0, quint32 instID = 0);
void addUAVObjectToWidgetRelation(UAVObject *obj, UAVObjectField *field, QWidget *widget, int index = 0, double scale = 1, bool isLimited = false, QList<int> *defaultReloadGroups = 0, quint32 instID = 0);
void addWidgetBinding(QString objectName, QString fieldName, QWidget *widget, int index = 0, double scale = 1,
bool isLimited = false, QList<int> *reloadGroupIDs = 0, quint32 instID = 0);
void addWidgetBinding(UAVObject *object, UAVObjectField *field, QWidget *widget, int index = 0, double scale = 1,
bool isLimited = false, QList<int> *reloadGroupIDs = 0, quint32 instID = 0);
void addUAVObjectToWidgetRelation(QString object, QString field, QWidget *widget, QString element, double scale, bool isLimited = false, QList<int> *defaultReloadGroups = 0, quint32 instID = 0);
void addUAVObjectToWidgetRelation(UAVObject *obj, UAVObjectField *field, QWidget *widget, QString element, double scale, bool isLimited = false, QList<int> *defaultReloadGroups = 0, quint32 instID = 0);
void addWidgetBinding(QString objectName, QString fieldName, QWidget *widget, QString elementName, double scale,
bool isLimited = false, QList<int> *reloadGroupIDs = 0, quint32 instID = 0);
void addWidgetBinding(UAVObject *object, UAVObjectField *field, QWidget *widget, QString elementName, double scale,
bool isLimited = false, QList<int> *reloadGroupIDs = 0, quint32 instID = 0);
void addUAVObjectToWidgetRelation(QString object, QString field, QWidget *widget, QString index);
void addUAVObjectToWidgetRelation(UAVObject *obj, UAVObjectField *field, QWidget *widget, QString index);
void addWidgetBinding(QString objectName, QString fieldName, QWidget *widget, QString elementName);
void addWidgetBinding(UAVObject *object, UAVObjectField *field, QWidget *widget, QString elementName);
// BUTTONS//
void addApplySaveButtons(QPushButton *update, QPushButton *save);
void addReloadButton(QPushButton *button, int buttonGroup);
void addDefaultButton(QPushButton *button, int buttonGroup);
//////////
void addWidgetToDefaultReloadGroups(QWidget *widget, QList<int> *groups);
void addWidgetToReloadGroups(QWidget *widget, QList<int> *reloadGroupIDs);
bool addShadowWidget(QWidget *masterWidget, QWidget *shadowWidget, double shadowScale = 1, bool shadowIsLimited = false);
bool addShadowWidget(QString object, QString field, QWidget *widget, int index = 0, double scale = 1, bool isLimited = false, QList<int> *defaultReloadGroups = NULL, quint32 instID = 0);
bool addShadowWidgetBinding(QString objectName, QString fieldName, QWidget *widget, int index = 0, double scale = 1,
bool isLimited = false, QList<int> *m_reloadGroups = NULL, quint32 instID = 0);
void autoLoadWidgets();
@ -139,18 +144,21 @@ public:
bool allObjectsUpdated();
void setOutOfLimitsStyle(QString style)
{
outOfLimitsStyle = style;
m_outOfLimitsStyle = style;
}
void addHelpButton(QPushButton *button, QString url);
void forceShadowUpdates();
void forceConnectedState();
virtual bool shouldObjectBeSaved(UAVObject *object);
public slots:
void onAutopilotDisconnect();
void onAutopilotConnect();
void invalidateObjects();
void apply();
void save();
void setWidgetBindingObjectEnabled(QString objectName, bool enabled);
signals:
// fired when a widgets contents changes
void widgetContentsChanged(QWidget *widget);
@ -165,40 +173,72 @@ signals:
// fired when the autopilot disconnects
void autoPilotDisconnected();
void defaultRequested(int group);
private slots:
void objectUpdated(UAVObject *);
void objectUpdated(UAVObject *object);
void defaultButtonClicked();
void reloadButtonClicked();
private:
int currentBoard;
bool isConnected;
bool allowWidgetUpdates;
QStringList objectsList;
QList <objectToWidget *> objOfInterest;
ExtensionSystem::PluginManager *pm;
UAVObjectManager *objManager;
UAVObjectUtilManager *utilMngr;
smartSaveButton *smartsave;
QMap<UAVObject *, bool> objectUpdates;
QMap<int, QList<objectToWidget *> *> defaultReloadGroups;
QMap<QWidget *, objectToWidget *> shadowsList;
QMap<QPushButton *, QString> helpButtonList;
QList<QPushButton *> reloadButtonList;
bool dirty;
bool setFieldFromWidget(QWidget *widget, UAVObjectField *field, int index, double scale);
struct objectComparator {
quint32 objid;
quint32 objinstid;
bool operator==(const objectComparator & lhs)
{
return lhs.objid == this->objid && lhs.objinstid == this->objinstid;
}
};
enum buttonTypeEnum { none, save_button, apply_button, reload_button, default_button, help_button };
struct bindingStruct {
QString objname;
QString fieldname;
QString element;
QString url;
buttonTypeEnum buttonType;
QList<int> buttonGroup;
double scale;
bool haslimits;
};
int m_currentBoardId;
bool m_isConnected;
bool m_isWidgetUpdatesAllowed;
QStringList m_objects;
QMultiHash<int, WidgetBinding *> m_reloadGroups;
QMultiHash<QWidget *, WidgetBinding *> m_widgetBindingsPerWidget;
QMultiHash<UAVObject *, WidgetBinding *> m_widgetBindingsPerObject;
ExtensionSystem::PluginManager *m_pluginManager;
UAVObjectUtilManager *m_objectUtilManager;
SmartSaveButton *m_saveButton;
QHash<UAVObject *, bool> m_updatedObjects;
QHash<QPushButton *, QString> m_helpButtons;
QList<QPushButton *> m_reloadButtons;
bool m_isDirty;
QString m_outOfLimitsStyle;
QTimer *m_realtimeUpdateTimer;
bool setWidgetFromField(QWidget *widget, UAVObjectField *field, int index, double scale, bool hasLimits);
QVariant getVariantFromWidget(QWidget *widget, double scale, QString units);
QVariant getVariantFromWidget(QWidget *widget, double scale, const QString units);
bool setWidgetFromVariant(QWidget *widget, QVariant value, double scale, QString units);
bool setWidgetFromVariant(QWidget *widget, QVariant value, double scale);
void connectWidgetUpdatesToSlot(QWidget *widget, const char *function);
void disconnectWidgetUpdatesToSlot(QWidget *widget, const char *function);
void loadWidgetLimits(QWidget *widget, UAVObjectField *field, int index, bool hasLimits, double sclale);
QString outOfLimitsStyle;
QTimer *timeOut;
int fieldIndexFromElementName(QString objectName, QString fieldName, QString elementName);
void doAddWidgetBinding(QString objectName, QString fieldName, QWidget *widget, int index = 0, double scale = 1,
bool isLimited = false, QList<int> *reloadGroupIDs = 0, quint32 instID = 0);
protected slots:
virtual void disableObjUpdates();
virtual void enableObjUpdates();
virtual void disableObjectUpdates();
virtual void enableObjectUpdates();
virtual void clearDirty();
virtual void widgetsContentsChanged();
virtual void populateWidgets();
@ -208,7 +248,8 @@ protected slots:
protected:
virtual void enableControls(bool enable);
virtual QString mapObjectName(const QString objectName);
virtual UAVObject *getObject(const QString name, quint32 instId = 0);
void checkWidgetsLimits(QWidget *widget, UAVObjectField *field, int index, bool hasLimits, QVariant value, double scale);
void updateEnableControls();
};

36
ground/openpilotgcs/src/plugins/uavobjectwidgetutils/smartsavebutton.cpp
View File

@ -27,27 +27,27 @@
#include "smartsavebutton.h"
#include "configtaskwidget.h"
smartSaveButton::smartSaveButton(ConfigTaskWidget *configTaskWidget) : configWidget(configTaskWidget)
SmartSaveButton::SmartSaveButton(ConfigTaskWidget *configTaskWidget) : configWidget(configTaskWidget)
{}
void smartSaveButton::addButtons(QPushButton *save, QPushButton *apply)
void SmartSaveButton::addButtons(QPushButton *save, QPushButton *apply)
{
buttonList.insert(save, save_button);
buttonList.insert(apply, apply_button);
connect(save, SIGNAL(clicked()), this, SLOT(processClick()));
connect(apply, SIGNAL(clicked()), this, SLOT(processClick()));
}
void smartSaveButton::addApplyButton(QPushButton *apply)
void SmartSaveButton::addApplyButton(QPushButton *apply)
{
buttonList.insert(apply, apply_button);
connect(apply, SIGNAL(clicked()), this, SLOT(processClick()));
}
void smartSaveButton::addSaveButton(QPushButton *save)
void SmartSaveButton::addSaveButton(QPushButton *save)
{
buttonList.insert(save, save_button);
connect(save, SIGNAL(clicked()), this, SLOT(processClick()));
}
void smartSaveButton::processClick()
void SmartSaveButton::processClick()
{
emit beginOp();
bool save = false;
@ -62,7 +62,7 @@ void smartSaveButton::processClick()
processOperation(button, save);
}
void smartSaveButton::processOperation(QPushButton *button, bool save)
void SmartSaveButton::processOperation(QPushButton *button, bool save)
{
emit preProcessOperations();
@ -157,33 +157,33 @@ void smartSaveButton::processOperation(QPushButton *button, bool save)
emit endOp();
}
void smartSaveButton::setObjects(QList<UAVDataObject *> list)
void SmartSaveButton::setObjects(QList<UAVDataObject *> list)
{
objects = list;
}
void smartSaveButton::addObject(UAVDataObject *obj)
void SmartSaveButton::addObject(UAVDataObject *obj)
{
Q_ASSERT(obj);
if (!objects.contains(obj)) {
objects.append(obj);
}
}
void smartSaveButton::removeObject(UAVDataObject *obj)
void SmartSaveButton::removeObject(UAVDataObject *obj)
{
if (objects.contains(obj)) {
objects.removeAll(obj);
}
}
void smartSaveButton::removeAllObjects()
void SmartSaveButton::removeAllObjects()
{
objects.clear();
}
void smartSaveButton::clearObjects()
void SmartSaveButton::clearObjects()
{
objects.clear();
}
void smartSaveButton::transaction_finished(UAVObject *obj, bool result)
void SmartSaveButton::transaction_finished(UAVObject *obj, bool result)
{
if (current_object == obj) {
up_result = result;
@ -191,32 +191,32 @@ void smartSaveButton::transaction_finished(UAVObject *obj, bool result)
}
}
void smartSaveButton::saving_finished(int id, bool result)
void SmartSaveButton::saving_finished(int id, bool result)
{
if (id == current_objectID) {
if (id == (int)current_objectID) {
sv_result = result;
loop.quit();
}
}
void smartSaveButton::enableControls(bool value)
void SmartSaveButton::enableControls(bool value)
{
foreach(QPushButton * button, buttonList.keys())
button->setEnabled(value);
}
void smartSaveButton::resetIcons()
void SmartSaveButton::resetIcons()
{
foreach(QPushButton * button, buttonList.keys())
button->setIcon(QIcon());
}
void smartSaveButton::apply()
void SmartSaveButton::apply()
{
processOperation(NULL, false);
}
void smartSaveButton::save()
void SmartSaveButton::save()
{
processOperation(NULL, true);
}

5
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"
@ -40,13 +39,13 @@
class ConfigTaskWidget;
class smartSaveButton : public QObject {
class SmartSaveButton : public QObject {
enum buttonTypeEnum { save_button, apply_button };
public:
Q_OBJECT
public:
smartSaveButton(ConfigTaskWidget *configTaskWidget);
SmartSaveButton(ConfigTaskWidget *configTaskWidget);
void addButtons(QPushButton *save, QPushButton *apply);
void setObjects(QList<UAVDataObject *>);
void addObject(UAVDataObject *);

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;

785
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,38 +426,30 @@ bool UAVTalk::processInputByte(quint8 rxbyte)
// The CRC byte
rxCSPacket = rxbyte;
if (rxCS != rxCSPacket) { // packet error - faulty CRC
stats.rxErrors++;
rxState = STATE_SYNC;
qDebug() << "******** CRC ERROR *********";
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
@ -468,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.
@ -478,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);
@ -489,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;
}
@ -499,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;
}
@ -577,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);
@ -603,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();
@ -755,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

37
ground/openpilotgcs/src/plugins/uploader/devicewidget.cpp
View File

@ -41,8 +41,7 @@ DeviceWidget::DeviceWidget(QWidget *parent) :
connect(myDevice->updateButton, SIGNAL(clicked()), this, SLOT(uploadFirmware()));
connect(myDevice->pbLoad, SIGNAL(clicked()), this, SLOT(loadFirmware()));
connect(myDevice->confirmCheckBox, SIGNAL(stateChanged(int)), this, SLOT(confirmCB(int)));
QPixmap pix = QPixmap(QString(":uploader/images/view-refresh.svg"));
myDevice->statusIcon->setPixmap(pix);
myDevice->statusIcon->setPixmap(QPixmap(":uploader/images/view-refresh.svg"));
myDevice->lblCertified->setText("");
}
@ -79,12 +78,12 @@ void DeviceWidget::populate()
{
int id = m_dfu->devices[deviceID].ID;
myDevice->lbldevID->setText(QString("Device ID: ") + QString::number(id, 16));
myDevice->lbldevID->setText(tr("Device ID: ") + QString::number(id, 16));
// DeviceID tells us what sort of HW we have detected:
// display a nice icon:
myDevice->gVDevice->scene()->clear();
myDevice->lblDevName->setText(deviceDescriptorStruct::idToBoardName(id));
myDevice->lblHWRev->setText(QString(tr("HW Revision: ")) + QString::number(id & 0x00FF, 16));
myDevice->lblHWRev->setText(tr("HW Revision: ") + QString::number(id & 0x00FF, 16));
switch (id) {
case 0x0101:
@ -115,25 +114,23 @@ void DeviceWidget::populate()
bool r = m_dfu->devices[deviceID].Readable;
bool w = m_dfu->devices[deviceID].Writable;
myDevice->lblAccess->setText(QString("Flash access: ") + QString(r ? "R" : "-") + QString(w ? "W" : "-"));
myDevice->lblMaxCode->setText(QString("Max code size: ") + QString::number(m_dfu->devices[deviceID].SizeOfCode));
myDevice->lblAccess->setText(tr("Flash access: ") + QString(r ? "R" : "-") + QString(w ? "W" : "-"));
myDevice->lblMaxCode->setText(tr("Max code size: ") + QString::number(m_dfu->devices[deviceID].SizeOfCode));
myDevice->lblCRC->setText(QString::number(m_dfu->devices[deviceID].FW_CRC));
myDevice->lblBLVer->setText(QString("BL version: ") + QString::number(m_dfu->devices[deviceID].BL_Version));
myDevice->lblBLVer->setText(tr("BL version: ") + QString::number(m_dfu->devices[deviceID].BL_Version));
int size = ((OP_DFU::device)m_dfu->devices[deviceID]).SizeOfDesc;
m_dfu->enterDFU(deviceID);
QByteArray desc = m_dfu->DownloadDescriptionAsBA(size);
if (!populateBoardStructuredDescription(desc)) {
// TODO
// desc was not a structured description
QString str = m_dfu->DownloadDescription(size);
myDevice->lblDescription->setText(QString("Firmware custom description: ") + str.left(str.indexOf(QChar(255))));
QPixmap pix = QPixmap(QString(":uploader/images/warning.svg"));
myDevice->lblCertified->setPixmap(pix);
myDevice->lblDescription->setText((!str.isEmpty()) ? str : tr("Unknown"));
myDevice->lblCertified->setPixmap(QPixmap(":uploader/images/warning.svg"));
myDevice->lblCertified->setToolTip(tr("Custom Firmware Build"));
myDevice->lblBuildDate->setText("Warning: development firmware");
myDevice->lblGitTag->setText("");
myDevice->lblBrdName->setText("");
myDevice->lblBuildDate->setText(tr("Unknown"));
myDevice->lblGitTag->setText(tr("Unknown"));
myDevice->lblBrdName->setText(tr("Unknown"));
}
status("Ready...", STATUSICON_INFO);
@ -179,13 +176,11 @@ bool DeviceWidget::populateBoardStructuredDescription(QByteArray desc)
myDevice->lblBuildDate->setText(onBoardDescription.gitDate.insert(4, "-").insert(7, "-"));
if (onBoardDescription.gitTag.startsWith("RELEASE", Qt::CaseSensitive)) {
myDevice->lblDescription->setText(onBoardDescription.gitTag);
QPixmap pix = QPixmap(QString(":uploader/images/application-certificate.svg"));
myDevice->lblCertified->setPixmap(pix);
myDevice->lblCertified->setPixmap(QPixmap(":uploader/images/application-certificate.svg"));
myDevice->lblCertified->setToolTip(tr("Tagged officially released firmware build"));
} else {
myDevice->lblDescription->setText(onBoardDescription.gitTag);
QPixmap pix = QPixmap(QString(":uploader/images/warning.svg"));
myDevice->lblCertified->setPixmap(pix);
myDevice->lblCertified->setPixmap(QPixmap(":uploader/images/warning.svg"));
myDevice->lblCertified->setToolTip(tr("Untagged or custom firmware build"));
}
@ -205,14 +200,12 @@ bool DeviceWidget::populateLoadedStructuredDescription(QByteArray desc)
if (LoadedDescription.gitTag.startsWith("RELEASE", Qt::CaseSensitive)) {
myDevice->lblDescritpionL->setText(LoadedDescription.gitTag);
myDevice->description->setText(LoadedDescription.gitTag);
QPixmap pix = QPixmap(QString(":uploader/images/application-certificate.svg"));
myDevice->lblCertifiedL->setPixmap(pix);
myDevice->lblCertifiedL->setPixmap(QPixmap(":uploader/images/application-certificate.svg"));
myDevice->lblCertifiedL->setToolTip(tr("Tagged officially released firmware build"));
} else {
myDevice->lblDescritpionL->setText(LoadedDescription.gitTag);
myDevice->description->setText(LoadedDescription.gitTag);
QPixmap pix = QPixmap(QString(":uploader/images/warning.svg"));
myDevice->lblCertifiedL->setPixmap(pix);
myDevice->lblCertifiedL->setPixmap(QPixmap(":uploader/images/warning.svg"));
myDevice->lblCertifiedL->setToolTip(tr("Untagged or custom firmware build"));
}
myDevice->lblBrdNameL->setText(deviceDescriptorStruct::idToBoardName(LoadedDescription.boardType << 8 | LoadedDescription.boardRevision));

6
ground/openpilotgcs/src/plugins/uploader/op_dfu.cpp
View File

@ -342,8 +342,9 @@ QString DFUObject::DownloadDescription(int const & numberOfChars)
QByteArray arr;
StartDownloadT(&arr, numberOfChars, OP_DFU::Descript);
QString str(arr);
return str;
int index = arr.indexOf(255);
return QString((index == -1) ? arr : arr.left(index));
}
QByteArray DFUObject::DownloadDescriptionAsBA(int const & numberOfChars)
@ -351,6 +352,7 @@ QByteArray DFUObject::DownloadDescriptionAsBA(int const & numberOfChars)
QByteArray arr;
StartDownloadT(&arr, numberOfChars, OP_DFU::Descript);
return arr;
}

24
ground/openpilotgcs/src/plugins/uploader/runningdevicewidget.cpp
View File

@ -104,24 +104,22 @@ void RunningDeviceWidget::populate()
deviceDescriptorStruct devDesc;
if (UAVObjectUtilManager::descriptionToStructure(description, devDesc)) {
if (devDesc.gitTag.startsWith("RELEASE", Qt::CaseSensitive)) {
myDevice->lblFWTag->setText(QString("Firmware tag: ") + devDesc.gitTag);
QPixmap pix = QPixmap(QString(":uploader/images/application-certificate.svg"));
myDevice->lblCertified->setPixmap(pix);
myDevice->lblFWTag->setText(tr("Firmware tag: ") + devDesc.gitTag);
myDevice->lblCertified->setPixmap(QPixmap(":uploader/images/application-certificate.svg"));
myDevice->lblCertified->setToolTip(tr("Tagged officially released firmware build"));
} else {
myDevice->lblFWTag->setText(QString("Firmware tag: ") + devDesc.gitTag);
QPixmap pix = QPixmap(QString(":uploader/images/warning.svg"));
myDevice->lblCertified->setPixmap(pix);
myDevice->lblFWTag->setText(tr("Firmware tag: ") + devDesc.gitTag);
myDevice->lblCertified->setPixmap(QPixmap(":uploader/images/warning.svg"));
myDevice->lblCertified->setToolTip(tr("Untagged or custom firmware build"));
}
myDevice->lblGitCommitTag->setText("Git commit hash: " + devDesc.gitHash);
myDevice->lblFWDate->setText(QString("Firmware date: ") + devDesc.gitDate.insert(4, "-").insert(7, "-"));
myDevice->lblGitCommitTag->setText(tr("Git commit hash: ") + devDesc.gitHash);
myDevice->lblFWDate->setText(tr("Firmware date: ") + devDesc.gitDate.insert(4, "-").insert(7, "-"));
} else {
myDevice->lblFWTag->setText(QString("Firmware tag: ") + QString(description).left(QString(description).indexOf(QChar(255))));
myDevice->lblGitCommitTag->setText("Git commit tag: Unknown");
myDevice->lblFWDate->setText(QString("Firmware date: Unknown"));
QPixmap pix = QPixmap(QString(":uploader/images/warning.svg"));
myDevice->lblCertified->setPixmap(pix);
QString desc = utilMngr->getBoardDescriptionString();
myDevice->lblFWTag->setText(tr("Firmware tag: ") + (!desc.isEmpty() ? desc : tr("Unknown")));
myDevice->lblCertified->setPixmap(QPixmap(":uploader/images/warning.svg"));
myDevice->lblCertified->setToolTip(tr("Custom Firmware Build"));
myDevice->lblGitCommitTag->setText(tr("Git commit hash: ") + tr("Unknown"));
myDevice->lblFWDate->setText(tr("Firmware date: ") + tr("Unknown"));
}
}

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:

Some files were not shown because too many files have changed in this diff Show More