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

Merge remote-tracking branch 'origin/next' into amorale/OP-1149_gyro_accel_thermal_calibration

Browse Source 
Conflicts:
	ground/openpilotgcs/src/plugins/uavobjects/uavobjects.pro
This commit is contained in:
Alessio Morale 2014-01-23 01:02:13 +01:00
commit 02c7084196
155 changed files with 26171 additions and 22610 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

23
.commit-template Normal file
View File

@ -0,0 +1,23 @@
# *************** OpenPilot commits guidelines ***************
# Each commit needs to have a message like the following sample:
# OP-1150 UI for thermal calibration: Connect State machine to UI
#
# It needs to begin with a reference to one or more Jira tickets followed by a short description.
# If needed add a longer description in the following lines, after an empty line.
#
# Before committing, ensure your code is properly formatted using:
# make uncrustify_all
# You can format flight or ground code only using respectively
# uncrustify_flight or uncrustify_ground
#
# To automatically create a review, append the following smart commit messages:
# +review OPReview
#
# To append the commit to an existing review, use the following smart commit message:
# +review OPReview-NNN
# For example "+review OPReview-609"
#
# *NOTE* leave an empty line between the commit message and "smart commit command"
# Smart commits commands need to starts immediately at first column

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

7
CREDITS.txt
View File

@ -5,7 +5,9 @@ Pedro Assuncao
Fredrik Arvidsson
Werner Backes
Jose Barros
Mikael Blomqvist
Pete Boehl
Glenn Campigli
David Carlson
James Cotton
Steve Doll
@ -13,7 +15,9 @@ Piotr Esden-Tempski
Richard Flay
Peter Farnworth
Ed Faulkner
Andrew Finegan
Darren Furniss
Cliff Geerdes
Frederic Goddeeris
Daniel Godin
Anthony Gomez
@ -24,6 +28,7 @@ Peter Gunnarsson
Dean Hall
Joe Hlebasko
Andy Honecker
Patrick Huebner
Ryan Hunt
Mark James
Ricky King
@ -34,6 +39,7 @@ Alan Krum
Edouard Lafargue
Mike Labranche
Fredrik Larsson
Richard von Lehe
Pablo Lema
David Llama
Matt Lipski
@ -65,6 +71,7 @@ Troy Schultz
Dr. Erhard Siegl
Dusty Anne Smith
Mike Smith
Bertrand Songis
Alex Sowa
Pete Stapley
Vova Starikh

2
Makefile
View File

@ -859,6 +859,8 @@ help:
@$(ECHO)
@$(ECHO) " Here is a summary of the available targets:"
@$(ECHO)
@$(ECHO) " [Source tree preparation]"
@$(ECHO) " prepare - Install GIT commit message template"
@$(ECHO) " [Tool Installers]"
@$(ECHO) " arm_sdk_install - Install the GNU ARM gcc toolchain"
@$(ECHO) " qt_sdk_install - Install the QT development tools"

21
WHATSNEW.txt
View File

@ -1,3 +1,20 @@
--- RELEASE-14.01-RC1 --- Cruising Ratt ---
This is the RC1 for the first 2014 software release.
This version still supports the CopterControl and CC3D.
It includes some major "under the hood" changes like migration
to Qt5.1 and QtQuick2 widgets, an overhaul of UAVTalk to improve
Telemetry and OPLink reliability.
Some additions in this release:
- "Rattitude" flight mode;
- Altitude Hold Reimplementation;
- Multiple PID banks;
- "Cruise Control"
the full list of features, improvements and bufixes shipping
in this release is accessible here:
http://progress.openpilot.org/browse/OP/fixforversion/10220
--- RELEASE-13.06.04 ---
This maintenance release includes the following fixes missing in (previously not released to public) RELEASE-13.06.03.
- Fixed issues with Google Maps;
@ -6,7 +23,8 @@ This maintenance release includes the following fixes missing in (previously not
JIRA issues addressed in this maintenance release:
OP-1044, OP-1070, OP-1072
Use the following link for a comprehensive list of issues addressed by this release
http://progress.openpilot.org/browse/OP-1070
http://progress.openpilot.org/issues/?filter=11060
--- RELEASE-13.06.03 ---
@ -296,6 +314,7 @@ GCS code changes:
- added OPLinkMini configuration page;
- hardware options are now dynamically enabled/disabled to allow supported configurations only;
- new artwork for all boards everywhere;
- optimised some 3D models;
- new About dialog showing version info and contributors list;
- fixed badly broken HiTL options dialog;

9
flight/libraries/math/sin_lookup.c
View File

@ -105,7 +105,14 @@ float sin_lookup_deg(float angle)
return 0;
}
int i_ang = ((int32_t)angle) % 360;
// <bug, was> int i_ang = ((int32_t)angle) % 360;
// 1073741760 is a multiple of 360 that is close to 0x3fffffff
// so angle can be a very large number of positive or negative rotations
// this is the fastest fix (no tests, one integer addition)
// and has the largest range since float mantissas are 23-4 bit
// we could halve the lookup table size
// we could interpolate for greater accuracy
int i_ang = ((int32_t)(angle + 0.5f) + (int32_t)1073741760) % 360;
if (i_ang >= 180) { // for 180 to 360 deg
return -sin_table[i_ang - 180];
} else { // for 0 to 179 deg

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:

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

@ -46,33 +46,34 @@
#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 +83,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 +97,107 @@ 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);
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

146
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"
@ -96,7 +96,7 @@ static void updateStabilizationDesired(ManualControlCommandData *cmd, ManualCont
static void updateLandDesired(ManualControlCommandData *cmd, bool changed);
static void altitudeHoldDesired(ManualControlCommandData *cmd, bool changed);
static void updatePathDesired(ManualControlCommandData *cmd, bool changed, bool home);
static void processFlightMode(ManualControlSettingsData *settings, float flightMode);
static void processFlightMode(ManualControlSettingsData *settings, float flightMode, ManualControlCommandData *cmd);
static void processArm(ManualControlCommandData *cmd, ManualControlSettingsData *settings, int8_t armSwitch);
static void setArmedIfChanged(uint8_t val);
static void configurationUpdatedCb(UAVObjEvent *ev);
@ -331,6 +331,7 @@ static void manualControlTask(__attribute__((unused)) void *parameters)
if (settings.FailsafeBehavior != MANUALCONTROLSETTINGS_FAILSAFEBEHAVIOR_NONE) {
FlightStatusGet(&flightStatus);
cmd.FlightModeSwitchPosition = (uint8_t)settings.FailsafeBehavior - 1;
flightStatus.FlightMode = settings.FlightModePosition[settings.FailsafeBehavior - 1];
FlightStatusSet(&flightStatus);
}
@ -446,7 +447,7 @@ static void manualControlTask(__attribute__((unused)) void *parameters)
}
}
processFlightMode(&settings, flightMode);
processFlightMode(&settings, flightMode, &cmd);
}
// Process arming outside conditional so system will disarm when disconnected
@ -668,8 +669,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 +691,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 +846,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);
@ -1197,7 +1199,7 @@ static void processArm(ManualControlCommandData *cmd, ManualControlSettingsData
* @param[in] settings The settings which indicate which position is which mode
* @param[in] flightMode the value of the switch position
*/
static void processFlightMode(ManualControlSettingsData *settings, float flightMode)
static void processFlightMode(ManualControlSettingsData *settings, float flightMode, ManualControlCommandData *cmd)
{
FlightStatusData flightStatus;
@ -1209,6 +1211,8 @@ static void processFlightMode(ManualControlSettingsData *settings, float flightM
pos = settings->FlightModeNumber - 1;
}
cmd->FlightModeSwitchPosition = pos;
uint8_t newMode = settings->FlightModePosition[pos];
if (flightStatus.FlightMode != newMode) {

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;

498
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"
@ -44,6 +48,7 @@
#include "airspeedstate.h"
#include "gyrostate.h"
#include "flightstatus.h"
#include "manualcontrolsettings.h"
#include "manualcontrol.h" // Just to get a macro
#include "taskinfo.h"
@ -65,14 +70,22 @@
#if defined(PIOS_STABILIZATION_STACK_SIZE)
#define STACK_SIZE_BYTES PIOS_STABILIZATION_STACK_SIZE
#else
#define STACK_SIZE_BYTES 724
#define STACK_SIZE_BYTES 840
#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 };
// number of flight mode switch positions
#define NUM_FMS_POSITIONS 6
// 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;
@ -86,14 +99,31 @@ float weak_leveling_kp = 0;
uint8_t weak_leveling_max = 0;
bool lowThrottleZeroIntegral;
bool lowThrottleZeroAxis[MAX_AXES];
float vbar_decay = 0.991f;
float vbar_decay = 0.991f;
struct pid pids[PID_MAX];
int cur_flight_mode = -1;
static uint8_t rattitude_anti_windup;
static float cruise_control_min_throttle;
static float cruise_control_max_throttle;
static uint8_t cruise_control_max_angle;
static float cruise_control_max_power_factor;
static float cruise_control_power_trim;
static int8_t cruise_control_inverted_power_switch;
static float cruise_control_neutral_thrust;
static uint8_t cruise_control_flight_mode_switch_pos_enable[NUM_FMS_POSITIONS];
// 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 +141,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);
@ -125,8 +162,15 @@ int32_t StabilizationStart()
*/
int32_t StabilizationInitialize()
{
// stop the compile if the number of switch positions changes, but has not been changed here
PIOS_STATIC_ASSERT(NUM_FMS_POSITIONS == sizeof(((ManualControlSettingsData *)0)->FlightModePosition) / sizeof((((ManualControlSettingsData *)0)->FlightModePosition)[0]));
// Initialize variables
StabilizationSettingsInitialize();
StabilizationBankInitialize();
StabilizationSettingsBank1Initialize();
StabilizationSettingsBank2Initialize();
StabilizationSettingsBank3Initialize();
ActuatorDesiredInitialize();
#ifdef DIAG_RATEDESIRED
RateDesiredInitialize();
@ -159,6 +203,8 @@ static void stabilizationTask(__attribute__((unused)) void *parameters)
AttitudeStateData attitudeState;
GyroStateData gyroStateData;
FlightStatusData flightStatus;
StabilizationBankData stabBank;
#ifdef REVOLUTION
AirspeedStateData airspeedState;
@ -175,7 +221,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 +234,18 @@ static void stabilizationTask(__attribute__((unused)) void *parameters)
StabilizationDesiredGet(&stabDesired);
AttitudeStateGet(&attitudeState);
GyroStateGet(&gyroStateData);
StabilizationBankGet(&stabBank);
#ifdef DIAG_RATEDESIRED
RateDesiredGet(&rateDesired);
#endif
uint8_t flight_mode_switch_position;
ManualControlCommandFlightModeSwitchPositionGet(&flight_mode_switch_position);
if (cur_flight_mode != flight_mode_switch_position) {
cur_flight_mode = flight_mode_switch_position;
SettingsBankUpdatedCb(NULL);
}
#ifdef REVOLUTION
float speedScaleFactor;
// Scale PID coefficients based on current airspeed estimation - needed for fixed wing planes
@ -263,7 +318,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 +343,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 +360,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 +368,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 +496,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 +511,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 +523,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 +544,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 +561,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 +570,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;
@ -427,6 +605,63 @@ static void stabilizationTask(__attribute__((unused)) void *parameters)
}
}
// modify throttle according to 1/cos(bank angle)
// to maintain same altitdue with changing bank angle
// but without manually adjusting throttle
// do it here and all the various flight modes (e.g. Altitude Hold) can use it
if (flight_mode_switch_position < NUM_FMS_POSITIONS
&& cruise_control_flight_mode_switch_pos_enable[flight_mode_switch_position] != (uint8_t)0
&& cruise_control_max_power_factor > 0.0001f) {
static uint8_t toggle;
static float factor;
float angle;
// get attitude state and calculate angle
// do it every 8th iteration to save CPU
if ((toggle++ & 7) == 0) {
// spherical right triangle
// 0 <= acosf() <= Pi
angle = RAD2DEG(acosf(cos_lookup_deg(attitudeState.Roll) * cos_lookup_deg(attitudeState.Pitch)));
// if past the cutoff angle (60 to 180 (180 means never))
if (angle > cruise_control_max_angle) {
// -1 reversed collective, 0 zero power, or 1 normal power
// these are all unboosted
factor = cruise_control_inverted_power_switch;
} else {
// avoid singularity
if (angle > 89.999f && angle < 90.001f) {
factor = cruise_control_max_power_factor;
} else {
factor = 1.0f / fabsf(cos_lookup_deg(angle));
if (factor > cruise_control_max_power_factor) {
factor = cruise_control_max_power_factor;
}
}
// factor in the power trim, no effect at 1.0, linear effect increases with factor
factor = (factor - 1.0f) * cruise_control_power_trim + 1.0f;
// if inverted and they want negative boost
if (angle > 90.0f && cruise_control_inverted_power_switch == (int8_t)-1) {
factor = -factor;
// as long as throttle is getting reversed
// we may as well do pitch and yaw for a complete "invert switch"
actuatorDesired.Pitch = -actuatorDesired.Pitch;
actuatorDesired.Yaw = -actuatorDesired.Yaw;
}
}
}
// also don't adjust throttle if <= 0, leaves neg alone and zero throttle stops motors
if (actuatorDesired.Throttle > cruise_control_min_throttle) {
// quad example factor of 2 at hover power of 40%: (0.4 - 0.0) * 2.0 + 0.0 = 0.8
// CP heli example factor of 2 at hover stick of 60%: (0.6 - 0.5) * 2.0 + 0.5 = 0.7
actuatorDesired.Throttle = (actuatorDesired.Throttle - cruise_control_neutral_thrust) * factor + cruise_control_neutral_thrust;
if (actuatorDesired.Throttle > cruise_control_max_throttle) {
actuatorDesired.Throttle = cruise_control_max_throttle;
} else if (actuatorDesired.Throttle < cruise_control_min_throttle) {
actuatorDesired.Throttle = cruise_control_min_throttle;
}
}
}
if (PARSE_FLIGHT_MODE(flightStatus.FlightMode) != FLIGHTMODE_MANUAL) {
ActuatorDesiredSet(&actuatorDesired);
} else {
@ -477,50 +712,189 @@ static void ZeroPids(void)
static float bound(float val, float range)
{
if (val < -range) {
val = -range;
return -range;
} else if (val > range) {
val = range;
return 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 (cur_flight_mode < 0 || cur_flight_mode >= NUM_FMS_POSITIONS) {
return;
}
switch (settings.FlightModeMap[cur_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);
@ -554,8 +928,26 @@ static void SettingsUpdatedCb(__attribute__((unused)) UAVObjEvent *ev)
// Compute time constant for vbar decay term based on a tau
vbar_decay = expf(-fakeDt / settings.VbarTau);
}
// force flight mode update
cur_flight_mode = -1;
// Rattitude flight mode anti-windup factor
rattitude_anti_windup = settings.RattitudeAntiWindup;
cruise_control_min_throttle = (float)settings.CruiseControlMinThrottle / 100.0f;
cruise_control_max_throttle = (float)settings.CruiseControlMaxThrottle / 100.0f;
cruise_control_max_angle = settings.CruiseControlMaxAngle;
cruise_control_max_power_factor = settings.CruiseControlMaxPowerFactor;
cruise_control_power_trim = settings.CruiseControlPowerTrim / 100.0f;
cruise_control_inverted_power_switch = settings.CruiseControlInvertedPowerSwitch;
cruise_control_neutral_thrust = (float)settings.CruiseControlNeutralThrust / 100.0f;
memcpy(
cruise_control_flight_mode_switch_pos_enable,
settings.CruiseControlFlightModeSwitchPosEnable,
sizeof(cruise_control_flight_mode_switch_pos_enable));
}
/**
* @}

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

4
flight/modules/System/systemmod.c
View File

@ -521,12 +521,14 @@ static void updateStats()
// Get stats and update
SystemStatsGet(&stats);
stats.FlightTime = xTaskGetTickCount() * portTICK_RATE_MS;
stats.FlightTime = xTaskGetTickCount() * portTICK_RATE_MS;
#if defined(ARCH_POSIX) || defined(ARCH_WIN32)
// POSIX port of FreeRTOS doesn't have xPortGetFreeHeapSize()
stats.SystemModStackRemaining = 128;
stats.HeapRemaining = 10240;
#else
stats.HeapRemaining = xPortGetFreeHeapSize();
stats.SystemModStackRemaining = uxTaskGetStackHighWaterMark(NULL) * 4;
#endif
// Get Irq stack status

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.

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

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

@ -250,7 +250,7 @@ static void PIOS_SBus_UpdateState(struct pios_sbus_state *state, uint8_t b)
state->received_data[state->byte_count - 1] = b;
state->byte_count++;
} else {
if (b == SBUS_EOF_BYTE) {
if (b == SBUS_EOF_BYTE || (b % SBUS_R7008SB_EOF_COUNTER_MASK) == SBUS_R7008SB_EOF_BYTE) {
/* full frame received */
uint8_t flags = state->received_data[SBUS_FRAME_LENGTH - 3];
if (flags & SBUS_FLAG_FL) {

24
flight/pios/inc/pios_sbus_priv.h
View File

@ -50,14 +50,24 @@
* 0x08 - failsafe flag,
* 0xf0 - reserved
* 1 byte - 0x00 (end of frame byte)
*
* The R7008SB receiver has four different end of frame bytes, which rotates in order:
* 00000100
* 00010100
* 00100100
* 00110100
*/
#define SBUS_FRAME_LENGTH (1 + 22 + 1 + 1)
#define SBUS_SOF_BYTE 0x0f
#define SBUS_EOF_BYTE 0x00
#define SBUS_FLAG_DC1 0x01
#define SBUS_FLAG_DC2 0x02
#define SBUS_FLAG_FL 0x04
#define SBUS_FLAG_FS 0x08
#define SBUS_FRAME_LENGTH (1 + 22 + 1 + 1)
#define SBUS_SOF_BYTE 0x0f
#define SBUS_EOF_BYTE 0x00
#define SBUS_FLAG_DC1 0x01
#define SBUS_FLAG_DC2 0x02
#define SBUS_FLAG_FL 0x04
#define SBUS_FLAG_FS 0x08
#define SBUS_R7008SB_EOF_COUNTER_MASK 0xCF
#define SBUS_R7008SB_EOF_BYTE 0x04
/*
* S.Bus protocol provides 16 proportional and 2 discrete channels.

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

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

@ -154,11 +154,15 @@
/* Task stack sizes */
#define PIOS_ACTUATOR_STACK_SIZE 1020
#define PIOS_MANUAL_STACK_SIZE 800
#define PIOS_MANUAL_STACK_SIZE 850
#ifdef DIAG_TASKS
#define PIOS_SYSTEM_STACK_SIZE 720
#else
#define PIOS_SYSTEM_STACK_SIZE 660
#define PIOS_STABILIZATION_STACK_SIZE 524
#define PIOS_TELEM_STACK_SIZE 800
#define PIOS_EVENTDISPATCHER_STACK_SIZE 130
#endif
#define PIOS_STABILIZATION_STACK_SIZE 790
#define PIOS_TELEM_STACK_SIZE 540
#define PIOS_EVENTDISPATCHER_STACK_SIZE 150
/* This can't be too high to stop eventdispatcher thread overflowing */
#define PIOS_EVENTDISAPTCHER_QUEUE 10

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

@ -24,7 +24,6 @@ UAVOBJSRCFILENAMES += accessorydesired
UAVOBJSRCFILENAMES += actuatorcommand
UAVOBJSRCFILENAMES += actuatordesired
UAVOBJSRCFILENAMES += actuatorsettings
UAVOBJSRCFILENAMES += altholdsmoothed
UAVOBJSRCFILENAMES += attitudesettings
UAVOBJSRCFILENAMES += attitudestate
UAVOBJSRCFILENAMES += gyrostate
@ -71,6 +70,7 @@ UAVOBJSRCFILENAMES += overosyncstats
UAVOBJSRCFILENAMES += overosyncsettings
UAVOBJSRCFILENAMES += pathaction
UAVOBJSRCFILENAMES += pathdesired
UAVOBJSRCFILENAMES += pathplan
UAVOBJSRCFILENAMES += pathstatus
UAVOBJSRCFILENAMES += positionstate
UAVOBJSRCFILENAMES += ratedesired
@ -83,6 +83,10 @@ UAVOBJSRCFILENAMES += revosettings
UAVOBJSRCFILENAMES += sonaraltitude
UAVOBJSRCFILENAMES += stabilizationdesired
UAVOBJSRCFILENAMES += stabilizationsettings
UAVOBJSRCFILENAMES += stabilizationsettingsbank1
UAVOBJSRCFILENAMES += stabilizationsettingsbank2
UAVOBJSRCFILENAMES += stabilizationsettingsbank3
UAVOBJSRCFILENAMES += stabilizationbank
UAVOBJSRCFILENAMES += systemalarms
UAVOBJSRCFILENAMES += systemsettings
UAVOBJSRCFILENAMES += systemstats
@ -100,6 +104,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
@ -29,7 +29,6 @@ UAVOBJSRCFILENAMES += accessorydesired
UAVOBJSRCFILENAMES += actuatorcommand
UAVOBJSRCFILENAMES += actuatordesired
UAVOBJSRCFILENAMES += actuatorsettings
UAVOBJSRCFILENAMES += altholdsmoothed
UAVOBJSRCFILENAMES += attitudesettings
UAVOBJSRCFILENAMES += attitudestate
UAVOBJSRCFILENAMES += gyrostate
@ -71,6 +70,7 @@ UAVOBJSRCFILENAMES += overosyncstats
UAVOBJSRCFILENAMES += overosyncsettings
UAVOBJSRCFILENAMES += pathaction
UAVOBJSRCFILENAMES += pathdesired
UAVOBJSRCFILENAMES += pathplan
UAVOBJSRCFILENAMES += pathstatus
UAVOBJSRCFILENAMES += positionstate
UAVOBJSRCFILENAMES += ratedesired
@ -83,6 +83,10 @@ UAVOBJSRCFILENAMES += revosettings
UAVOBJSRCFILENAMES += sonaraltitude
UAVOBJSRCFILENAMES += stabilizationdesired
UAVOBJSRCFILENAMES += stabilizationsettings
UAVOBJSRCFILENAMES += stabilizationsettingsbank1
UAVOBJSRCFILENAMES += stabilizationsettingsbank2
UAVOBJSRCFILENAMES += stabilizationsettingsbank3
UAVOBJSRCFILENAMES += stabilizationbank
UAVOBJSRCFILENAMES += systemalarms
UAVOBJSRCFILENAMES += systemsettings
UAVOBJSRCFILENAMES += systemstats
@ -98,6 +102,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;
}

146
ground/openpilotgcs/copydata.pro
View File

@ -5,6 +5,124 @@ TEMPLATE = subdirs
# Copy Qt runtime libraries into the build directory (to run or package)
equals(copydata, 1) {
GCS_LIBRARY_PATH
linux-* {
QT_LIBS = libQt5Core.so.5 \
libQt5Gui.so.5 \
libQt5Widgets.so.5 \
libQt5Network.so.5 \
libQt5OpenGL.so.5 \
libQt5Sql.so.5 \
libQt5Svg.so.5 \
libQt5Test.so.5 \
libQt5Xml.so.5 \
libQt5Declarative.so.5 \
libQt5XmlPatterns.so.5 \
libQt5Script.so.5 \
libQt5Concurrent.so.5 \
libQt5PrintSupport.so.5 \
libQt5SerialPort.so.5 \
libQt5Multimedia.so.5 \
libQt5MultimediaWidgets.so.5 \
libQt5Quick.so.5 \
libQt5Qml.so.5 \
libQt5V8.so.5 \
libQt5DBus.so.5 \
libQt5QuickParticles.so.5 \
libicui18n.so.51 \
libicuuc.so.51 \
libicudata.so.51
data_copy.commands += -@$(MKDIR) $$targetPath(\"$$GCS_QT_LIBRARY_PATH\") $$addNewline()
for(lib, QT_LIBS) {
data_copy.commands += $(COPY_FILE) $$targetPath(\"$$[QT_INSTALL_LIBS]/$$lib\") $$targetPath(\"$$GCS_QT_LIBRARY_PATH/$$lib\") $$addNewline()
}
# create Qt plugin directories
QT_PLUGIN_DIRS = iconengines \
imageformats \
platforms \
mediaservice \
sqldrivers
for(dir, QT_PLUGIN_DIRS) {
data_copy.commands += -@$(MKDIR) $$targetPath(\"$$GCS_QT_PLUGINS_PATH/$$dir\") $$addNewline()
}
QT_PLUGIN_LIBS = iconengines/libqsvgicon.so \
imageformats/libqgif.so \
imageformats/libqico.so \
imageformats/libqjpeg.so \
imageformats/libqmng.so \
imageformats/libqsvg.so \
imageformats/libqtiff.so \
platforms/libqxcb.so \
sqldrivers/libqsqlite.so
for(lib, QT_PLUGIN_LIBS) {
data_copy.commands += $(COPY_FILE) $$targetPath(\"$$[QT_INSTALL_PLUGINS]/$$lib\") $$targetPath(\"$$GCS_QT_PLUGINS_PATH/$$lib\") $$addNewline()
}
# create QtQuick2 plugin directories
QT_QUICK2_DIRS = QtQuick \
QtQuick.2 \
QtQuick/Layouts \
QtQuick/LocalStorage \
QtQuick/Particles.2 \
QtQuick/PrivateWidgets \
QtQuick/Window.2 \
QtQuick/XmlListModel
for(dir, QT_QUICK2_DIRS) {
data_copy.commands += -@$(MKDIR) $$targetPath(\"$$GCS_QT_QML_PATH/$$dir\") $$addNewline()
}
# Copy QtQuick2 complete directories
# These directories have a lot of files
# Easier to copy everything
QTQ_WHOLE_DIRS = QtQuick/Controls \
QtQuick/Dialogs
for(dir, QTQ_WHOLE_DIRS) {
data_copy.commands += $(COPY_DIR) $$targetPath(\"$$[QT_INSTALL_QML]/$$dir\") $$targetPath(\"$$GCS_QT_QML_PATH/$$dir\") $$addNewline()
}
# Remove the few unwanted libs after whole dir copy
QT_QUICK2_DELS = QtQuick/Controls/libqtquickcontrolsplugin.so \
QtQuick/Controls/Private/libqtquickcontrolsprivateplugin.so \
QtQuick/Dialogs/libdialogplugin.so
for(delfile, QT_QUICK2_DELS) {
data_copy.commands += $(DEL_FILE) $$targetPath(\"$$GCS_QT_QML_PATH/$${delfile}\") $$addNewline()
}
# Remaining QtQuick plugin libs
QT_QUICK2_DLLS = QtQuick.2/libqtquick2plugin.so \
QtQuick.2/plugins.qmltypes \
QtQuick.2/qmldir \
QtQuick/Layouts/libqquicklayoutsplugin.so \
QtQuick/Layouts/plugins.qmltypes \
QtQuick/Layouts/qmldir \
QtQuick/LocalStorage/libqmllocalstorageplugin.so \
QtQuick/LocalStorage/plugins.qmltypes \
QtQuick/LocalStorage/qmldir \
QtQuick/Particles.2/libparticlesplugin.so \
QtQuick/Particles.2/plugins.qmltypes \
QtQuick/Particles.2/qmldir \
QtQuick/PrivateWidgets/libwidgetsplugin.so \
QtQuick/PrivateWidgets/plugins.qmltypes \
QtQuick/PrivateWidgets/qmldir \
QtQuick/Window.2/libwindowplugin.so \
QtQuick/Window.2/plugins.qmltypes \
QtQuick/Window.2/qmldir \
QtQuick/XmlListModel/libqmlxmllistmodelplugin.so \
QtQuick/XmlListModel/plugins.qmltypes \
QtQuick/XmlListModel/qmldir
for(lib, QT_QUICK2_DLLS) {
data_copy.commands += $(COPY_FILE) $$targetPath(\"$$[QT_INSTALL_QML]/$$lib\") $$targetPath(\"$$GCS_QT_QML_PATH/$$lib\") $$addNewline()
}
data_copy.target = FORCE
QMAKE_EXTRA_TARGETS += data_copy
}
# Windows release only, no debug target DLLs ending with 'd'
# It is assumed that SDL.dll can be found in the same directory as mingw32-make.exe
win32 {
@ -146,11 +264,37 @@ equals(copydata, 1) {
ssleay32.dll \
libeay32.dll
for(dll, OPENSSL_DLLS) {
data_copy.commands += $(COPY_FILE) $$targetPath(\"$$(OPENSSL)/$$dll\") $$targetPath(\"$$GCS_APP_PATH/$$dll\") $$addNewline()
data_copy.commands += $(COPY_FILE) $$targetPath(\"$$(OPENSSL_DIR)/$$dll\") $$targetPath(\"$$GCS_APP_PATH/$$dll\") $$addNewline()
}
data_copy.target = FORCE
QMAKE_EXTRA_TARGETS += data_copy
}
macx{
#NOTE: debug dylib can be copied as they will be cleaned out with packaging scripts
#standard plugins directory (will copy just dylib, plugins.qmltypes and qmldir
QT_QUICK2_PLUGINS = QtQuick.2 QtQuick/Layouts QtQuick/LocalStorage QtQuick/Particles.2 QtQuick/PrivateWidgets QtQuick/Window.2 QtQuick/XmlListModel
#those directories will be fully copied to dest
QT_QUICK2_FULL_DIRS = QtQuick/Controls QtQuick/Dialogs
#create QtQuick dir (that will host all subdirs)
data_copy.commands += -@$(MKDIR) $$targetPath(\"$$GCS_QT_QML_PATH/QtQuick\") $$addNewline()
for(dir, QT_QUICK2_FULL_DIRS) {
#data_copy.commands += -@$(MKDIR) $$targetPath(\"$$GCS_QT_QML_PATH/$$dir\") $$addNewline()
data_copy.commands += $(COPY_DIR) $$targetPath(\"$$[QT_INSTALL_QML]/$$dir\") $$targetPath(\"$$GCS_QT_QML_PATH/$$dir\") $$addNewline()
}
for(lib, QT_QUICK2_PLUGINS) {
data_copy.commands += $(MKDIR) $$targetPath(\"$$GCS_QT_QML_PATH/$$lib\") $$addNewline()
data_copy.commands += $(COPY_FILE) $$targetPath(\"$$[QT_INSTALL_QML]/$$lib/\"*.dylib) $$targetPath(\"$$GCS_QT_QML_PATH/$$lib/\") $$addNewline()
data_copy.commands += $(COPY_FILE) $$targetPath(\"$$[QT_INSTALL_QML]/$$lib/plugins.qmltypes\") $$targetPath(\"$$GCS_QT_QML_PATH/$$lib/plugins.qmltypes\") $$addNewline()
data_copy.commands += $(COPY_FILE) $$targetPath(\"$$[QT_INSTALL_QML]/$$lib/qmldir\") $$targetPath(\"$$GCS_QT_QML_PATH/$$lib/qmldir\") $$addNewline()
}
data_copy.target = FORCE
QMAKE_EXTRA_TARGETS += data_copy
}
}

4
ground/openpilotgcs/openpilotgcs.pri
View File

@ -77,6 +77,7 @@ macx {
GCS_APP_TARGET = "OpenPilot GCS"
GCS_LIBRARY_PATH = $$GCS_APP_PATH/$${GCS_APP_TARGET}.app/Contents/Plugins
GCS_PLUGIN_PATH = $$GCS_LIBRARY_PATH
GCS_QT_QML_PATH = $$GCS_APP_PATH/$${GCS_APP_TARGET}.app/Contents/Imports
GCS_LIBEXEC_PATH = $$GCS_APP_PATH/$${GCS_APP_TARGET}.app/Contents/Resources
GCS_DATA_PATH = $$GCS_APP_PATH/$${GCS_APP_TARGET}.app/Contents/Resources
GCS_DATA_BASENAME = Resources
@ -89,6 +90,9 @@ macx {
} else {
GCS_APP_WRAPPER = openpilotgcs
GCS_APP_TARGET = openpilotgcs.bin
GCS_QT_LIBRARY_PATH = $$GCS_BUILD_TREE/$$GCS_LIBRARY_BASENAME/qt5
GCS_QT_PLUGINS_PATH = $$GCS_BUILD_TREE/$$GCS_LIBRARY_BASENAME/qt5/plugins
GCS_QT_QML_PATH = $$GCS_BUILD_TREE/$$GCS_LIBRARY_BASENAME/qt5/qml
}
GCS_LIBRARY_PATH = $$GCS_BUILD_TREE/$$GCS_LIBRARY_BASENAME/openpilotgcs
GCS_PLUGIN_PATH = $$GCS_LIBRARY_PATH/plugins

1
ground/openpilotgcs/openpilotgcs.pro
View File

@ -21,6 +21,7 @@ macx {
QMAKE_CC = /usr/bin/gcc
QMAKE_CXX = /usr/bin/g++
QMAKE_LINK = /usr/bin/g++
QMAKE_MACOSX_DEPLOYMENT_TARGET = 10.6
}
include(openpilotgcs.pri)

2
ground/openpilotgcs/share/openpilotgcs/pfd/default/PfdWorldView.qml
View File

@ -35,7 +35,7 @@ Item {
Rotation {
angle: -AttitudeState.Roll
origin.x : world.parent.width/2
origin.y : world.parent.height/2
origin.y : horizontCenter
}
]

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

4
ground/openpilotgcs/src/openpilotgcsplugin.pri
View File

@ -31,7 +31,7 @@ macx {
QMAKE_LFLAGS_SONAME = -Wl,-install_name,@executable_path/../Plugins/$${PROVIDER}/
} else:linux-* {
#do the rpath by hand since it's not possible to use ORIGIN in QMAKE_RPATHDIR
QMAKE_RPATHDIR += \$\$ORIGIN
QMAKE_RPATHDIR = \$\$ORIGIN
QMAKE_RPATHDIR += \$\$ORIGIN/..
QMAKE_RPATHDIR += \$\$ORIGIN/../..
GCS_PLUGIN_RPATH = $$join(QMAKE_RPATHDIR, ":")
@ -45,7 +45,7 @@ contains(QT_CONFIG, reduce_exports):CONFIG += hGCS_symbols
CONFIG += plugin plugin_with_soname
!macx {
target.path = /$$GCS_LIBRARY_BASENAME/opnepilotgcs/plugins/$$PROVIDER
target.path = /$$GCS_LIBRARY_BASENAME/openpilotgcs/plugins/$$PROVIDER
pluginspec.files += $${TARGET}.pluginspec
pluginspec.path = /$$GCS_LIBRARY_BASENAME/openpilotgcs/plugins/$$PROVIDER
INSTALLS += target pluginspec

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

@ -60,12 +60,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

92
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
@ -81,17 +79,23 @@ ConfigInputWidget::ConfigInputWidget(QWidget *parent) :
unsigned int indexRT = 0;
foreach(QString name, manualSettingsObj->getField("ChannelNumber")->getElementNames()) {
Q_ASSERT(index < ManualControlSettings::CHANNELGROUPS_NUMELEM);
inputChannelForm *inpForm = new inputChannelForm(this, index == 0);
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);
// The order of the following three binding calls is important. Since the values will be populated
// in reverse order of the binding order otherwise the 'Reversed' logic will floor the neutral value
// to the max value ( which is smaller than the neutral value when reversed )
addWidgetBinding("ManualControlSettings", "ChannelNeutral", inpForm->ui->channelNeutral, index);
addWidgetBinding("ManualControlSettings", "ChannelNeutral", inpForm->ui->neutralValue, index);
addWidgetBinding("ManualControlSettings", "ChannelMin", inpForm->ui->channelMin, index);
addWidgetBinding("ManualControlSettings", "ChannelMax", inpForm->ui->channelMax, index);
addWidget(inpForm->ui->channelNumberDropdown);
addWidget(inpForm->ui->channelRev);
addWidget(inpForm->ui->channelResponseTime);
addWidget(inpForm->ui->channelRev);
// Input filter response time fields supported for some channels only
switch (index) {
@ -101,7 +105,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 +121,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,32 +132,34 @@ 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("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()));
addWidget(ui->configurationWizard);
addWidget(ui->runCalibration);
autoLoadWidgets();
populateWidgets();
refreshWidgetsValues();
// Connect the help button
@ -1312,21 +1318,33 @@ void ConfigInputWidget::updatePositionSlider()
default:
case 6:
ui->fmsModePos6->setEnabled(true);
ui->cc_box_5->setEnabled(true);
ui->pidBankSs1_5->setEnabled(true);
// pass through
case 5:
ui->fmsModePos5->setEnabled(true);
ui->cc_box_4->setEnabled(true);
ui->pidBankSs1_4->setEnabled(true);
// pass through
case 4:
ui->fmsModePos4->setEnabled(true);
ui->cc_box_3->setEnabled(true);
ui->pidBankSs1_3->setEnabled(true);
// pass through
case 3:
ui->fmsModePos3->setEnabled(true);
ui->cc_box_2->setEnabled(true);
ui->pidBankSs1_2->setEnabled(true);
// pass through
case 2:
ui->fmsModePos2->setEnabled(true);
ui->cc_box_1->setEnabled(true);
ui->pidBankSs1_1->setEnabled(true);
// pass through
case 1:
ui->fmsModePos1->setEnabled(true);
ui->cc_box_0->setEnabled(true);
ui->pidBankSs1_0->setEnabled(true);
// pass through
case 0:
break;
@ -1335,21 +1353,33 @@ void ConfigInputWidget::updatePositionSlider()
switch (manualSettingsDataPriv.FlightModeNumber) {
case 0:
ui->fmsModePos1->setEnabled(false);
ui->cc_box_0->setEnabled(false);
ui->pidBankSs1_0->setEnabled(false);
// pass through
case 1:
ui->fmsModePos2->setEnabled(false);
ui->cc_box_1->setEnabled(false);
ui->pidBankSs1_1->setEnabled(false);
// pass through
case 2:
ui->fmsModePos3->setEnabled(false);
ui->cc_box_2->setEnabled(false);
ui->pidBankSs1_2->setEnabled(false);
// pass through
case 3:
ui->fmsModePos4->setEnabled(false);
ui->cc_box_3->setEnabled(false);
ui->pidBankSs1_3->setEnabled(false);
// pass through
case 4:
ui->fmsModePos5->setEnabled(false);
ui->cc_box_4->setEnabled(false);
ui->pidBankSs1_4->setEnabled(false);
// pass through
case 5:
ui->fmsModePos6->setEnabled(false);
ui->cc_box_5->setEnabled(false);
ui->pidBankSs1_5->setEnabled(false);
// pass through
case 6:
default:

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

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

@ -53,46 +53,45 @@ ConfigPipXtremeWidget::ConfigPipXtremeWidget(QWidget *parent) : ConfigTaskWidget
} else {
qDebug() << "Error: Object is unknown (OPLinkSettings).";
}
autoLoadWidgets();
Core::Internal::GeneralSettings *settings = pm->getObject<Core::Internal::GeneralSettings>();
if (!settings->useExpertMode()) {
m_oplink->Apply->setVisible(false);
}
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()));
@ -114,7 +113,7 @@ ConfigPipXtremeWidget::ConfigPipXtremeWidget(QWidget *parent) : ConfigTaskWidget
// Request and update of the setting object.
settingsUpdated = false;
autoLoadWidgets();
disableMouseWheelEvents();
}
@ -151,7 +150,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 +163,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 +215,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 +223,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

@ -247,12 +247,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();

60
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>();
@ -65,6 +92,10 @@ ConfigStabilizationWidget::ConfigStabilizationWidget(QWidget *parent) : ConfigTa
addWidget(ui->realTimeUpdates_8);
connect(ui->realTimeUpdates_12, SIGNAL(toggled(bool)), this, SLOT(realtimeUpdatesSlot(bool)));
addWidget(ui->realTimeUpdates_12);
connect(ui->realTimeUpdates_7, SIGNAL(toggled(bool)), this, SLOT(realtimeUpdatesSlot(bool)));
addWidget(ui->realTimeUpdates_7);
connect(ui->realTimeUpdates_9, SIGNAL(toggled(bool)), this, SLOT(realtimeUpdatesSlot(bool)));
addWidget(ui->realTimeUpdates_9);
connect(ui->checkBox_7, SIGNAL(toggled(bool)), this, SLOT(linkCheckBoxes(bool)));
addWidget(ui->checkBox_7);
@ -116,6 +147,8 @@ void ConfigStabilizationWidget::realtimeUpdatesSlot(bool value)
ui->realTimeUpdates_6->setChecked(value);
ui->realTimeUpdates_8->setChecked(value);
ui->realTimeUpdates_12->setChecked(value);
ui->realTimeUpdates_7->setChecked(value);
ui->realTimeUpdates_9->setChecked(value);
if (value && !realtimeUpdates->isActive()) {
realtimeUpdates->start(AUTOMATIC_UPDATE_RATE);
@ -205,6 +238,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 +264,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);

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

@ -6,15 +6,24 @@
<rect>
<x>0</x>
<y>0</y>
<width>880</width>
<height>672</height>
<width>796</width>
<height>828</height>
</rect>
</property>
<property name="windowTitle">
<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>
@ -110,21 +128,30 @@
<rect>
<x>0</x>
<y>0</y>
<width>850</width>
<height>572</height>
<width>766</width>
<height>745</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">
<property name="title">
<string/>
<string>Input Channel Configuration</string>
</property>
<layout class="QVBoxLayout" name="verticalLayout_6">
<property name="leftMargin">
@ -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">
@ -336,6 +381,12 @@
</item>
<item>
<widget class="QPushButton" name="configurationWizard">
<property name="minimumSize">
<size>
<width>210</width>
<height>0</height>
</size>
</property>
<property name="text">
<string>Start Configuration Wizard</string>
</property>
@ -418,7 +469,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>
@ -498,57 +558,91 @@
<rect>
<x>0</x>
<y>0</y>
<width>850</width>
<height>572</height>
<width>766</width>
<height>745</height>
</rect>
</property>
<layout class="QGridLayout" name="gridLayout_7" rowstretch="1,0,0">
<property name="margin">
<layout class="QGridLayout" name="gridLayout_7" rowstretch="1,0,0,0">
<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="3" column="0">
<spacer name="verticalSpacer_5">
<property name="orientation">
<enum>Qt::Vertical</enum>
</property>
<property name="sizeType">
<enum>QSizePolicy::Expanding</enum>
</property>
<property name="sizeHint" stdset="0">
<size>
<width>20</width>
<height>20</height>
</size>
</property>
</spacer>
</item>
<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>
<string>Stabilization Modes Configuration</string>
</property>
<layout class="QGridLayout" name="gridLayout_2" columnstretch="0,0,1,0,1,0,1,0">
<layout class="QGridLayout" name="gridLayout_2" columnstretch="0,0,0,0,0,0,0,0,0,0,0">
<property name="leftMargin">
<number>9</number>
</property>
<property name="horizontalSpacing">
<number>12</number>
<number>6</number>
</property>
<item row="2" column="1">
<spacer name="horizontalSpacer_11">
<item row="1" column="4">
<spacer name="horizontalSpacer_12">
<property name="orientation">
<enum>Qt::Horizontal</enum>
</property>
<property name="sizeType">
<enum>QSizePolicy::Minimum</enum>
<enum>QSizePolicy::Fixed</enum>
</property>
<property name="sizeHint" stdset="0">
<size>
<width>5</width>
<width>10</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>
<item row="1" column="8">
<spacer name="horizontalSpacer_13">
<property name="orientation">
<enum>Qt::Horizontal</enum>
</property>
</widget>
<property name="sizeType">
<enum>QSizePolicy::Fixed</enum>
</property>
<property name="sizeHint" stdset="0">
<size>
<width>10</width>
<height>20</height>
</size>
</property>
</spacer>
</item>
<item row="0" column="6">
<item row="0" column="9">
<widget class="QLabel" name="label_10">
<property name="minimumSize">
<size>
<width>0</width>
<width>120</width>
<height>20</height>
</size>
</property>
@ -573,78 +667,11 @@ margin:1px;</string>
</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">
<item row="0" column="7">
<widget class="QLabel" name="label_9">
<property name="minimumSize">
<size>
<width>0</width>
<width>120</width>
<height>20</height>
</size>
</property>
@ -669,11 +696,27 @@ margin:1px;</string>
</property>
</widget>
</item>
<item row="0" column="2">
<item row="1" column="10">
<spacer name="horizontalSpacer_11">
<property name="orientation">
<enum>Qt::Horizontal</enum>
</property>
<property name="sizeType">
<enum>QSizePolicy::Preferred</enum>
</property>
<property name="sizeHint" stdset="0">
<size>
<width>170</width>
<height>20</height>
</size>
</property>
</spacer>
</item>
<item row="0" column="3">
<widget class="QLabel" name="label_8">
<property name="minimumSize">
<size>
<width>0</width>
<width>120</width>
<height>20</height>
</size>
</property>
@ -698,77 +741,224 @@ margin:1px;</string>
</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">
<item row="1" column="1">
<widget class="QFrame" name="frame_3">
<property name="minimumSize">
<size>
<width>102</width>
<width>106</width>
<height>0</height>
</size>
</property>
<property name="focusPolicy">
<enum>Qt::StrongFocus</enum>
<property name="maximumSize">
<size>
<width>105</width>
<height>16777215</height>
</size>
</property>
<property name="frameShape">
<enum>QFrame::NoFrame</enum>
</property>
<property name="frameShadow">
<enum>QFrame::Raised</enum>
</property>
<layout class="QVBoxLayout" name="verticalLayout_13">
<item>
<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>
<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>
<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>
</layout>
</widget>
</item>
<item row="2" column="2">
<widget class="QComboBox" name="fmsSsPos2Roll">
<property name="focusPolicy">
<enum>Qt::StrongFocus</enum>
<item row="1" column="3">
<widget class="QFrame" name="frame_2">
<property name="frameShape">
<enum>QFrame::NoFrame</enum>
</property>
<property name="frameShadow">
<enum>QFrame::Raised</enum>
</property>
<layout class="QVBoxLayout" name="verticalLayout_10">
<property name="leftMargin">
<number>1</number>
</property>
<property name="topMargin">
<number>1</number>
</property>
<property name="rightMargin">
<number>1</number>
</property>
<property name="bottomMargin">
<number>1</number>
</property>
<item>
<widget class="QComboBox" name="fmsSsPos1Roll">
<property name="minimumSize">
<size>
<width>0</width>
<height>0</height>
</size>
</property>
<property name="focusPolicy">
<enum>Qt::StrongFocus</enum>
</property>
</widget>
</item>
<item>
<widget class="QComboBox" name="fmsSsPos2Roll">
<property name="focusPolicy">
<enum>Qt::StrongFocus</enum>
</property>
</widget>
</item>
<item>
<widget class="QComboBox" name="fmsSsPos3Roll">
<property name="focusPolicy">
<enum>Qt::StrongFocus</enum>
</property>
</widget>
</item>
</layout>
</widget>
</item>
<item row="1" column="7">
<widget class="QFrame" name="frame_5">
<property name="frameShape">
<enum>QFrame::NoFrame</enum>
</property>
<property name="frameShadow">
<enum>QFrame::Raised</enum>
</property>
<layout class="QVBoxLayout" name="verticalLayout_14">
<property name="leftMargin">
<number>1</number>
</property>
<property name="topMargin">
<number>1</number>
</property>
<property name="rightMargin">
<number>1</number>
</property>
<property name="bottomMargin">
<number>1</number>
</property>
<item>
<widget class="QComboBox" name="fmsSsPos1Pitch">
<property name="minimumSize">
<size>
<width>0</width>
<height>0</height>
</size>
</property>
<property name="focusPolicy">
<enum>Qt::StrongFocus</enum>
</property>
</widget>
</item>
<item>
<widget class="QComboBox" name="fmsSsPos2Pitch">
<property name="focusPolicy">
<enum>Qt::StrongFocus</enum>
</property>
</widget>
</item>
<item>
<widget class="QComboBox" name="fmsSsPos3Pitch">
<property name="focusPolicy">
<enum>Qt::StrongFocus</enum>
</property>
</widget>
</item>
</layout>
</widget>
</item>
<item row="1" column="9">
<widget class="QFrame" name="frame_6">
<property name="frameShape">
<enum>QFrame::NoFrame</enum>
</property>
<property name="frameShadow">
<enum>QFrame::Raised</enum>
</property>
<layout class="QVBoxLayout" name="verticalLayout_15">
<property name="leftMargin">
<number>1</number>
</property>
<property name="topMargin">
<number>1</number>
</property>
<property name="rightMargin">
<number>1</number>
</property>
<property name="bottomMargin">
<number>1</number>
</property>
<item>
<widget class="QComboBox" name="fmsSsPos1Yaw">
<property name="minimumSize">
<size>
<width>0</width>
<height>0</height>
</size>
</property>
<property name="focusPolicy">
<enum>Qt::StrongFocus</enum>
</property>
</widget>
</item>
<item>
<widget class="QComboBox" name="fmsSsPos2Yaw">
<property name="focusPolicy">
<enum>Qt::StrongFocus</enum>
</property>
</widget>
</item>
<item>
<widget class="QComboBox" name="fmsSsPos3Yaw">
<property name="focusPolicy">
<enum>Qt::StrongFocus</enum>
</property>
</widget>
</item>
</layout>
</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="sizeType">
<enum>QSizePolicy::Fixed</enum>
</property>
<property name="sizeHint" stdset="0">
<size>
<width>40</width>
<width>10</width>
<height>20</height>
</size>
</property>
@ -792,307 +982,92 @@ margin:1px;</string>
</size>
</property>
<property name="title">
<string>FlightMode Switch Positions</string>
<string>Flight Mode Switch Positions</string>
</property>
<layout class="QGridLayout" name="gridLayout_4" rowstretch="0,0,0" columnstretch="0,0,1,0,1,0">
<property name="margin">
<number>12</number>
<layout class="QGridLayout" name="gridLayout_4">
<property name="leftMargin">
<number>9</number>
</property>
<item row="0" column="0" rowspan="3">
<layout class="QVBoxLayout" name="verticalLayout_9" stretch="0,0,0,0,0,0">
<item>
<widget class="QLabel" name="label_pos1">
<property name="sizePolicy">
<sizepolicy hsizetype="Preferred" vsizetype="Maximum">
<horstretch>0</horstretch>
<verstretch>0</verstretch>
</sizepolicy>
</property>
<property name="minimumSize">
<size>
<width>0</width>
<height>16</height>
</size>
</property>
<property name="maximumSize">
<size>
<width>16777215</width>
<height>16</height>
</size>
</property>
<property name="text">
<string>Pos. 1</string>
</property>
</widget>
</item>
<item>
<widget class="QLabel" name="label_pos2">
<property name="sizePolicy">
<sizepolicy hsizetype="Preferred" vsizetype="Maximum">
<horstretch>0</horstretch>
<verstretch>0</verstretch>
</sizepolicy>
</property>
<property name="minimumSize">
<size>
<width>16</width>
<height>16</height>
</size>
</property>
<property name="maximumSize">
<size>
<width>16777215</width>
<height>16</height>
</size>
</property>
<property name="text">
<string>Pos. 2</string>
</property>
</widget>
</item>
<item>
<widget class="QLabel" name="label_pos3">
<property name="sizePolicy">
<sizepolicy hsizetype="Preferred" vsizetype="Maximum">
<horstretch>0</horstretch>
<verstretch>0</verstretch>
</sizepolicy>
</property>
<property name="minimumSize">
<size>
<width>0</width>
<height>16</height>
</size>
</property>
<property name="maximumSize">
<size>
<width>16777215</width>
<height>16</height>
</size>
</property>
<property name="text">
<string>Pos. 3</string>
</property>
</widget>
</item>
<item>
<widget class="QLabel" name="label_pos4">
<property name="sizePolicy">
<sizepolicy hsizetype="Preferred" vsizetype="Maximum">
<horstretch>0</horstretch>
<verstretch>0</verstretch>
</sizepolicy>
</property>
<property name="minimumSize">
<size>
<width>0</width>
<height>16</height>
</size>
</property>
<property name="maximumSize">
<size>
<width>16777215</width>
<height>16</height>
</size>
</property>
<property name="text">
<string>Pos. 4</string>
</property>
</widget>
</item>
<item>
<widget class="QLabel" name="label_pos5">
<property name="sizePolicy">
<sizepolicy hsizetype="Preferred" vsizetype="Maximum">
<horstretch>0</horstretch>
<verstretch>0</verstretch>
</sizepolicy>
</property>
<property name="minimumSize">
<size>
<width>0</width>
<height>16</height>
</size>
</property>
<property name="maximumSize">
<size>
<width>16777215</width>
<height>16</height>
</size>
</property>
<property name="text">
<string>Pos. 5</string>
</property>
</widget>
</item>
<item>
<widget class="QLabel" name="label_pos6">
<property name="sizePolicy">
<sizepolicy hsizetype="Preferred" vsizetype="Maximum">
<horstretch>0</horstretch>
<verstretch>0</verstretch>
</sizepolicy>
</property>
<property name="minimumSize">
<size>
<width>0</width>
<height>16</height>
</size>
</property>
<property name="maximumSize">
<size>
<width>16777215</width>
<height>16</height>
</size>
</property>
<property name="text">
<string>Pos. 6</string>
</property>
</widget>
</item>
</layout>
</item>
<item row="1" column="1">
<widget class="QSlider" name="fmsSlider">
<property name="enabled">
<bool>false</bool>
<property name="topMargin">
<number>9</number>
</property>
<property name="rightMargin">
<number>9</number>
</property>
<property name="bottomMargin">
<number>9</number>
</property>
<property name="horizontalSpacing">
<number>6</number>
</property>
<item row="0" column="6">
<widget class="QLabel" name="label_11">
<property name="minimumSize">
<size>
<width>80</width>
<height>20</height>
</size>
</property>
<property name="maximumSize">
<size>
<width>16777215</width>
<height>160</height>
</size>
</property>
<property name="focusPolicy">
<enum>Qt::StrongFocus</enum>
</property>
<property name="toolTip">
<string>This slider moves when you move the flight mode switch
on your remote. It shows currently active flight mode.
Setup the flight mode channel on the RC Input tab if you have not done so already.</string>
</property>
<property name="minimum">
<number>0</number>
</property>
<property name="maximum">
<number>5</number>
</property>
<property name="pageStep">
<number>10</number>
</property>
<property name="value">
<number>0</number>
</property>
<property name="sliderPosition">
<number>0</number>
</property>
<property name="orientation">
<enum>Qt::Vertical</enum>
</property>
<property name="invertedAppearance">
<bool>true</bool>
</property>
<property name="tickPosition">
<enum>QSlider::TicksBelow</enum>
</property>
<property name="tickInterval">
<number>1</number>
</property>
</widget>
</item>
<item row="0" column="2" rowspan="3">
<layout class="QVBoxLayout" name="verticalLayout_10">
<property name="spacing">
<number>3</number>
</property>
<item>
<widget class="QComboBox" name="fmsModePos1">
<property name="focusPolicy">
<enum>Qt::StrongFocus</enum>
</property>
<property name="toolTip">
<string>Select the stabilization mode on this position (manual/stabilized/auto)</string>
</property>
</widget>
</item>
<item>
<widget class="QComboBox" name="fmsModePos2">
<property name="focusPolicy">
<enum>Qt::StrongFocus</enum>
</property>
</widget>
</item>
<item>
<widget class="QComboBox" name="fmsModePos3">
<property name="focusPolicy">
<enum>Qt::StrongFocus</enum>
</property>
</widget>
</item>
<item>
<widget class="QComboBox" name="fmsModePos4">
<property name="enabled">
<bool>false</bool>
</property>
<property name="focusPolicy">
<enum>Qt::StrongFocus</enum>
</property>
<property name="toolTip">
<string>Select the stabilization mode on this position (manual/stabilized/auto)</string>
</property>
</widget>
</item>
<item>
<widget class="QComboBox" name="fmsModePos5">
<property name="enabled">
<bool>false</bool>
</property>
<property name="focusPolicy">
<enum>Qt::StrongFocus</enum>
</property>
</widget>
</item>
<item>
<widget class="QComboBox" name="fmsModePos6">
<property name="enabled">
<bool>false</bool>
</property>
<property name="focusPolicy">
<enum>Qt::StrongFocus</enum>
</property>
</widget>
</item>
</layout>
</item>
<item row="1" column="3">
<spacer name="horizontalSpacer_4">
<property name="orientation">
<enum>Qt::Horizontal</enum>
</property>
<property name="sizeHint" stdset="0">
<size>
<width>40</width>
<height>20</height>
</size>
</property>
</spacer>
<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="0" column="4" rowspan="3">
<layout class="QGridLayout" name="gridLayout_5">
<item row="0" column="0">
<layout class="QHBoxLayout" name="horizontalLayout_6">
<item>
<widget class="QLabel" name="label">
<property name="text">
<string>Number of flight modes:</string>
</property>
</widget>
</item>
<item>
<widget class="QSpinBox" name="fmsPosNum">
<property name="toolTip">
<string>Number of positions your FlightMode switch has.
<item row="0" column="10">
<widget class="QLabel" name="label_16">
<property name="minimumSize">
<size>
<width>132</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>Flight Mode Count</string>
</property>
<property name="alignment">
<set>Qt::AlignCenter</set>
</property>
</widget>
</item>
<item row="1" column="10">
<widget class="QSpinBox" name="fmsPosNum">
<property name="maximumSize">
<size>
<width>151</width>
<height>16777215</height>
</size>
</property>
<property name="toolTip">
<string>Number of positions your FlightMode switch has.
Default is 3.
@ -1100,102 +1075,1013 @@ It will be 2 or 3 for most of setups, but it also can be up to 6.
In that case you have to configure your radio mixers so the whole range
from min to max is split into N equal intervals, and you may set arbitrary
channel value for each flight mode.</string>
</property>
<property name="minimum">
<number>1</number>
</property>
<property name="maximum">
<number>6</number>
</property>
<property name="value">
<number>3</number>
</property>
</widget>
</item>
</layout>
</item>
<item row="1" column="0">
<widget class="QLabel" name="label_15">
<property name="font">
<font>
<weight>75</weight>
<bold>true</bold>
</font>
</property>
<property name="text">
<string>Avoid &quot;Manual&quot; for multirotors!</string>
</property>
<property name="wordWrap">
<bool>true</bool>
</property>
</widget>
</item>
</layout>
</property>
<property name="minimum">
<number>1</number>
</property>
<property name="maximum">
<number>6</number>
</property>
<property name="value">
<number>3</number>
</property>
</widget>
</item>
<item row="2" column="1">
<spacer name="verticalSpacer_4">
<property name="orientation">
<enum>Qt::Vertical</enum>
<item row="2" column="10">
<widget class="QLabel" name="label_15">
<property name="sizePolicy">
<sizepolicy hsizetype="Preferred" vsizetype="Preferred">
<horstretch>0</horstretch>
<verstretch>0</verstretch>
</sizepolicy>
</property>
<property name="sizeType">
<enum>QSizePolicy::Fixed</enum>
<property name="font">
<font>
<weight>75</weight>
<bold>true</bold>
</font>
</property>
<property name="sizeHint" stdset="0">
<size>
<width>20</width>
<height>0</height>
</size>
<property name="text">
<string>Avoid &quot;Manual&quot; for multirotors!</string>
</property>
</spacer>
<property name="alignment">
<set>Qt::AlignCenter</set>
</property>
<property name="wordWrap">
<bool>true</bool>
</property>
</widget>
</item>
<item row="0" column="1">
<spacer name="verticalSpacer_6">
<property name="orientation">
<enum>Qt::Vertical</enum>
</property>
<property name="sizeType">
<enum>QSizePolicy::Fixed</enum>
</property>
<property name="sizeHint" stdset="0">
<size>
<width>20</width>
<height>0</height>
</size>
</property>
</spacer>
</item>
<item row="1" column="5">
<item row="1" column="3">
<spacer name="horizontalSpacer_6">
<property name="orientation">
<enum>Qt::Horizontal</enum>
</property>
<property name="sizeType">
<enum>QSizePolicy::Fixed</enum>
</property>
<property name="sizeHint" stdset="0">
<size>
<width>40</width>
<width>10</width>
<height>20</height>
</size>
</property>
</spacer>
</item>
<item row="1" column="5">
<spacer name="horizontalSpacer_15">
<property name="orientation">
<enum>Qt::Horizontal</enum>
</property>
<property name="sizeType">
<enum>QSizePolicy::Fixed</enum>
</property>
<property name="sizeHint" stdset="0">
<size>
<width>10</width>
<height>20</height>
</size>
</property>
</spacer>
</item>
<item row="1" column="7">
<spacer name="horizontalSpacer_16">
<property name="orientation">
<enum>Qt::Horizontal</enum>
</property>
<property name="sizeType">
<enum>QSizePolicy::Fixed</enum>
</property>
<property name="sizeHint" stdset="0">
<size>
<width>10</width>
<height>20</height>
</size>
</property>
</spacer>
</item>
<item row="1" column="1">
<spacer name="horizontalSpacer_4">
<property name="orientation">
<enum>Qt::Horizontal</enum>
</property>
<property name="sizeType">
<enum>QSizePolicy::Fixed</enum>
</property>
<property name="sizeHint" stdset="0">
<size>
<width>10</width>
<height>20</height>
</size>
</property>
</spacer>
</item>
<item row="0" column="4">
<widget class="QLabel" name="label_12">
<property name="minimumSize">
<size>
<width>105</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>Cruise Control</string>
</property>
<property name="alignment">
<set>Qt::AlignCenter</set>
</property>
</widget>
</item>
<item row="0" column="8" rowspan="3">
<widget class="Line" name="line">
<property name="orientation">
<enum>Qt::Vertical</enum>
</property>
</widget>
</item>
<item row="0" column="2">
<widget class="QLabel" name="label_13">
<property name="minimumSize">
<size>
<width>138</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>Flight Mode</string>
</property>
<property name="alignment">
<set>Qt::AlignCenter</set>
</property>
</widget>
</item>
<item row="1" column="4" rowspan="2">
<widget class="QFrame" name="frame_4">
<property name="minimumSize">
<size>
<width>30</width>
<height>0</height>
</size>
</property>
<property name="maximumSize">
<size>
<width>16777215</width>
<height>16777215</height>
</size>
</property>
<layout class="QVBoxLayout" name="verticalLayout_4">
<property name="spacing">
<number>6</number>
</property>
<property name="leftMargin">
<number>1</number>
</property>
<property name="topMargin">
<number>1</number>
</property>
<property name="rightMargin">
<number>1</number>
</property>
<property name="bottomMargin">
<number>1</number>
</property>
<item alignment="Qt::AlignHCenter">
<widget class="QCheckBox" name="cc_box_0">
<property name="sizePolicy">
<sizepolicy hsizetype="Fixed" vsizetype="Minimum">
<horstretch>0</horstretch>
<verstretch>0</verstretch>
</sizepolicy>
</property>
<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="font">
<font>
<weight>75</weight>
<bold>true</bold>
</font>
</property>
<property name="toolTip">
<string>&lt;html&gt;&lt;head/&gt;&lt;body&gt;&lt;p&gt;Enabling this checkbox will enable Cruise Control for Flight Mode Switch position #1.&lt;/p&gt;&lt;/body&gt;&lt;/html&gt;</string>
</property>
<property name="layoutDirection">
<enum>Qt::RightToLeft</enum>
</property>
<property name="text">
<string/>
</property>
<property name="objrelation" stdset="0">
<stringlist>
<string>objname:StabilizationSettings</string>
<string>fieldname:CruiseControlFlightModeSwitchPosEnable</string>
<string>index:0</string>
<string>haslimits:no</string>
<string>scale:1</string>
<string>buttongroup:16</string>
</stringlist>
</property>
</widget>
</item>
<item alignment="Qt::AlignHCenter">
<widget class="QCheckBox" name="cc_box_1">
<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="font">
<font>
<weight>75</weight>
<bold>true</bold>
</font>
</property>
<property name="toolTip">
<string>&lt;html&gt;&lt;head/&gt;&lt;body&gt;&lt;p&gt;Enabling this checkbox will enable Cruise Control for Flight Mode Switch position #2.&lt;/p&gt;&lt;/body&gt;&lt;/html&gt;</string>
</property>
<property name="layoutDirection">
<enum>Qt::RightToLeft</enum>
</property>
<property name="text">
<string/>
</property>
<property name="objrelation" stdset="0">
<stringlist>
<string>objname:StabilizationSettings</string>
<string>fieldname:CruiseControlFlightModeSwitchPosEnable</string>
<string>index:1</string>
<string>haslimits:no</string>
<string>scale:1</string>
<string>buttongroup:16</string>
</stringlist>
</property>
</widget>
</item>
<item alignment="Qt::AlignHCenter">
<widget class="QCheckBox" name="cc_box_2">
<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="font">
<font>
<weight>75</weight>
<bold>true</bold>
</font>
</property>
<property name="toolTip">
<string>&lt;html&gt;&lt;head/&gt;&lt;body&gt;&lt;p&gt;Enabling this checkbox will enable Cruise Control for Flight Mode Switch position #3.&lt;/p&gt;&lt;/body&gt;&lt;/html&gt;</string>
</property>
<property name="layoutDirection">
<enum>Qt::RightToLeft</enum>
</property>
<property name="text">
<string/>
</property>
<property name="objrelation" stdset="0">
<stringlist>
<string>objname:StabilizationSettings</string>
<string>fieldname:CruiseControlFlightModeSwitchPosEnable</string>
<string>index:2</string>
<string>haslimits:no</string>
<string>scale:1</string>
<string>buttongroup:16</string>
</stringlist>
</property>
</widget>
</item>
<item alignment="Qt::AlignHCenter">
<widget class="QCheckBox" name="cc_box_3">
<property name="enabled">
<bool>false</bool>
</property>
<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="font">
<font>
<weight>75</weight>
<bold>true</bold>
</font>
</property>
<property name="toolTip">
<string>&lt;html&gt;&lt;head/&gt;&lt;body&gt;&lt;p&gt;Enabling this checkbox will enable Cruise Control for Flight Mode Switch position #4.&lt;/p&gt;&lt;/body&gt;&lt;/html&gt;</string>
</property>
<property name="layoutDirection">
<enum>Qt::RightToLeft</enum>
</property>
<property name="text">
<string/>
</property>
<property name="objrelation" stdset="0">
<stringlist>
<string>objname:StabilizationSettings</string>
<string>fieldname:CruiseControlFlightModeSwitchPosEnable</string>
<string>index:3</string>
<string>haslimits:no</string>
<string>scale:1</string>
<string>buttongroup:16</string>
</stringlist>
</property>
</widget>
</item>
<item alignment="Qt::AlignHCenter">
<widget class="QCheckBox" name="cc_box_4">
<property name="enabled">
<bool>false</bool>
</property>
<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="font">
<font>
<weight>75</weight>
<bold>true</bold>
</font>
</property>
<property name="toolTip">
<string>&lt;html&gt;&lt;head/&gt;&lt;body&gt;&lt;p&gt;Enabling this checkbox will enable Cruise Control for Flight Mode Switch position #5.&lt;/p&gt;&lt;/body&gt;&lt;/html&gt;</string>
</property>
<property name="layoutDirection">
<enum>Qt::RightToLeft</enum>
</property>
<property name="text">
<string/>
</property>
<property name="objrelation" stdset="0">
<stringlist>
<string>objname:StabilizationSettings</string>
<string>fieldname:CruiseControlFlightModeSwitchPosEnable</string>
<string>index:4</string>
<string>haslimits:no</string>
<string>scale:1</string>
<string>buttongroup:16</string>
</stringlist>
</property>
</widget>
</item>
<item alignment="Qt::AlignHCenter">
<widget class="QCheckBox" name="cc_box_5">
<property name="enabled">
<bool>false</bool>
</property>
<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="font">
<font>
<weight>75</weight>
<bold>true</bold>
</font>
</property>
<property name="toolTip">
<string>&lt;html&gt;&lt;head/&gt;&lt;body&gt;&lt;p&gt;Enabling this checkbox will enable Cruise Control for Flight Mode Switch position #6.&lt;/p&gt;&lt;/body&gt;&lt;/html&gt;</string>
</property>
<property name="layoutDirection">
<enum>Qt::RightToLeft</enum>
</property>
<property name="text">
<string/>
</property>
<property name="objrelation" stdset="0">
<stringlist>
<string>objname:StabilizationSettings</string>
<string>fieldname:CruiseControlFlightModeSwitchPosEnable</string>
<string>index:5</string>
<string>haslimits:no</string>
<string>scale:1</string>
<string>buttongroup:16</string>
</stringlist>
</property>
</widget>
</item>
</layout>
</widget>
</item>
<item row="1" column="0" rowspan="2">
<widget class="QFrame" name="frame">
<property name="minimumSize">
<size>
<width>90</width>
<height>0</height>
</size>
</property>
<property name="maximumSize">
<size>
<width>90</width>
<height>16777215</height>
</size>
</property>
<property name="frameShape">
<enum>QFrame::NoFrame</enum>
</property>
<property name="frameShadow">
<enum>QFrame::Raised</enum>
</property>
<layout class="QGridLayout" name="gridLayout_8">
<property name="leftMargin">
<number>1</number>
</property>
<property name="topMargin">
<number>1</number>
</property>
<property name="rightMargin">
<number>1</number>
</property>
<property name="bottomMargin">
<number>1</number>
</property>
<property name="horizontalSpacing">
<number>10</number>
</property>
<item row="4" column="1">
<widget class="QLabel" name="label_pos5">
<property name="sizePolicy">
<sizepolicy hsizetype="Preferred" vsizetype="Maximum">
<horstretch>0</horstretch>
<verstretch>0</verstretch>
</sizepolicy>
</property>
<property name="minimumSize">
<size>
<width>0</width>
<height>20</height>
</size>
</property>
<property name="maximumSize">
<size>
<width>16777215</width>
<height>16</height>
</size>
</property>
<property name="text">
<string>Pos. 5</string>
</property>
</widget>
</item>
<item row="2" column="1">
<widget class="QLabel" name="label_pos3">
<property name="sizePolicy">
<sizepolicy hsizetype="Preferred" vsizetype="Maximum">
<horstretch>0</horstretch>
<verstretch>0</verstretch>
</sizepolicy>
</property>
<property name="minimumSize">
<size>
<width>0</width>
<height>20</height>
</size>
</property>
<property name="maximumSize">
<size>
<width>16777215</width>
<height>16</height>
</size>
</property>
<property name="text">
<string>Pos. 3</string>
</property>
</widget>
</item>
<item row="3" column="1">
<widget class="QLabel" name="label_pos4">
<property name="sizePolicy">
<sizepolicy hsizetype="Preferred" vsizetype="Maximum">
<horstretch>0</horstretch>
<verstretch>0</verstretch>
</sizepolicy>
</property>
<property name="minimumSize">
<size>
<width>0</width>
<height>20</height>
</size>
</property>
<property name="maximumSize">
<size>
<width>16777215</width>
<height>16</height>
</size>
</property>
<property name="text">
<string>Pos. 4</string>
</property>
</widget>
</item>
<item row="0" column="1">
<widget class="QLabel" name="label_pos1">
<property name="sizePolicy">
<sizepolicy hsizetype="Preferred" vsizetype="Maximum">
<horstretch>0</horstretch>
<verstretch>0</verstretch>
</sizepolicy>
</property>
<property name="minimumSize">
<size>
<width>0</width>
<height>20</height>
</size>
</property>
<property name="maximumSize">
<size>
<width>16777215</width>
<height>16</height>
</size>
</property>
<property name="text">
<string>Pos. 1</string>
</property>
</widget>
</item>
<item row="1" column="1">
<widget class="QLabel" name="label_pos2">
<property name="sizePolicy">
<sizepolicy hsizetype="Preferred" vsizetype="Maximum">
<horstretch>0</horstretch>
<verstretch>0</verstretch>
</sizepolicy>
</property>
<property name="minimumSize">
<size>
<width>16</width>
<height>20</height>
</size>
</property>
<property name="maximumSize">
<size>
<width>16777215</width>
<height>16</height>
</size>
</property>
<property name="text">
<string>Pos. 2</string>
</property>
</widget>
</item>
<item row="5" column="1">
<widget class="QLabel" name="label_pos6">
<property name="sizePolicy">
<sizepolicy hsizetype="Preferred" vsizetype="Maximum">
<horstretch>0</horstretch>
<verstretch>0</verstretch>
</sizepolicy>
</property>
<property name="minimumSize">
<size>
<width>0</width>
<height>20</height>
</size>
</property>
<property name="maximumSize">
<size>
<width>16777215</width>
<height>16</height>
</size>
</property>
<property name="text">
<string>Pos. 6</string>
</property>
</widget>
</item>
<item row="0" column="0" rowspan="6">
<widget class="QSlider" name="fmsSlider">
<property name="enabled">
<bool>false</bool>
</property>
<property name="maximumSize">
<size>
<width>16777215</width>
<height>150</height>
</size>
</property>
<property name="focusPolicy">
<enum>Qt::StrongFocus</enum>
</property>
<property name="toolTip">
<string>This slider moves when you move the flight mode switch
on your remote. It shows currently active flight mode.
Setup the flight mode channel on the RC Input tab if you have not done so already.</string>
</property>
<property name="minimum">
<number>0</number>
</property>
<property name="maximum">
<number>5</number>
</property>
<property name="pageStep">
<number>10</number>
</property>
<property name="value">
<number>0</number>
</property>
<property name="sliderPosition">
<number>0</number>
</property>
<property name="orientation">
<enum>Qt::Vertical</enum>
</property>
<property name="invertedAppearance">
<bool>true</bool>
</property>
<property name="tickPosition">
<enum>QSlider::TicksBelow</enum>
</property>
<property name="tickInterval">
<number>1</number>
</property>
</widget>
</item>
</layout>
</widget>
</item>
<item row="1" column="2" rowspan="2">
<widget class="QFrame" name="frame">
<property name="minimumSize">
<size>
<width>120</width>
<height>0</height>
</size>
</property>
<property name="maximumSize">
<size>
<width>16777215</width>
<height>16777215</height>
</size>
</property>
<layout class="QGridLayout" name="gridLayout_6">
<property name="leftMargin">
<number>1</number>
</property>
<property name="topMargin">
<number>1</number>
</property>
<property name="rightMargin">
<number>1</number>
</property>
<property name="bottomMargin">
<number>1</number>
</property>
<item row="5" column="1">
<widget class="QComboBox" name="fmsModePos6">
<property name="enabled">
<bool>false</bool>
</property>
<property name="focusPolicy">
<enum>Qt::StrongFocus</enum>
</property>
<property name="toolTip">
<string>Select the stabilization mode on this position (manual/stabilized/auto)</string>
</property>
</widget>
</item>
<item row="0" column="1">
<widget class="QComboBox" name="fmsModePos1">
<property name="minimumSize">
<size>
<width>0</width>
<height>0</height>
</size>
</property>
<property name="maximumSize">
<size>
<width>16777215</width>
<height>16777215</height>
</size>
</property>
<property name="focusPolicy">
<enum>Qt::StrongFocus</enum>
</property>
<property name="toolTip">
<string>Select the stabilization mode on this position (manual/stabilized/auto)</string>
</property>
</widget>
</item>
<item row="4" column="1">
<widget class="QComboBox" name="fmsModePos5">
<property name="enabled">
<bool>false</bool>
</property>
<property name="focusPolicy">
<enum>Qt::StrongFocus</enum>
</property>
<property name="toolTip">
<string>Select the stabilization mode on this position (manual/stabilized/auto)</string>
</property>
</widget>
</item>
<item row="2" column="1">
<widget class="QComboBox" name="fmsModePos3">
<property name="focusPolicy">
<enum>Qt::StrongFocus</enum>
</property>
<property name="toolTip">
<string>Select the stabilization mode on this position (manual/stabilized/auto)</string>
</property>
</widget>
</item>
<item row="3" column="1">
<widget class="QComboBox" name="fmsModePos4">
<property name="enabled">
<bool>false</bool>
</property>
<property name="focusPolicy">
<enum>Qt::StrongFocus</enum>
</property>
<property name="toolTip">
<string>Select the stabilization mode on this position (manual/stabilized/auto)</string>
</property>
</widget>
</item>
<item row="1" column="1">
<widget class="QComboBox" name="fmsModePos2">
<property name="focusPolicy">
<enum>Qt::StrongFocus</enum>
</property>
<property name="toolTip">
<string>Select the stabilization mode on this position (manual/stabilized/auto)</string>
</property>
</widget>
</item>
</layout>
</widget>
</item>
<item row="1" column="6" rowspan="2">
<widget class="QFrame" name="frame_7">
<property name="minimumSize">
<size>
<width>75</width>
<height>0</height>
</size>
</property>
<property name="maximumSize">
<size>
<width>16777215</width>
<height>16777215</height>
</size>
</property>
<layout class="QVBoxLayout" name="verticalLayout_9">
<property name="leftMargin">
<number>1</number>
</property>
<property name="topMargin">
<number>1</number>
</property>
<property name="rightMargin">
<number>1</number>
</property>
<property name="bottomMargin">
<number>1</number>
</property>
<item>
<widget class="QComboBox" name="pidBankSs1_0">
<property name="minimumSize">
<size>
<width>75</width>
<height>0</height>
</size>
</property>
<property name="focusPolicy">
<enum>Qt::StrongFocus</enum>
</property>
<property name="toolTip">
<string>&lt;html&gt;&lt;head/&gt;&lt;body&gt;&lt;p&gt;Select which set of roll rates / max bank angles / PIDs you want active on this switch position.&lt;/p&gt;&lt;/body&gt;&lt;/html&gt;</string>
</property>
<property name="objrelation" stdset="0">
<stringlist>
<string>objname:StabilizationSettings</string>
<string>fieldname:FlightModeMap</string>
<string>index:0</string>
<string>haslimits:no</string>
<string>scale:1</string>
<string>buttongroup:16</string>
</stringlist>
</property>
</widget>
</item>
<item>
<widget class="QComboBox" name="pidBankSs1_1">
<property name="minimumSize">
<size>
<width>75</width>
<height>0</height>
</size>
</property>
<property name="focusPolicy">
<enum>Qt::StrongFocus</enum>
</property>
<property name="toolTip">
<string>&lt;html&gt;&lt;head/&gt;&lt;body&gt;&lt;p&gt;Select which set of roll rates / max bank angles / PIDs you want active on this switch position.&lt;/p&gt;&lt;/body&gt;&lt;/html&gt;</string>
</property>
<property name="objrelation" stdset="0">
<stringlist>
<string>objname:StabilizationSettings</string>
<string>fieldname:FlightModeMap</string>
<string>index:1</string>
<string>haslimits:no</string>
<string>scale:1</string>
<string>buttongroup:16</string>
</stringlist>
</property>
</widget>
</item>
<item>
<widget class="QComboBox" name="pidBankSs1_2">
<property name="minimumSize">
<size>
<width>75</width>
<height>0</height>
</size>
</property>
<property name="focusPolicy">
<enum>Qt::StrongFocus</enum>
</property>
<property name="toolTip">
<string>&lt;html&gt;&lt;head/&gt;&lt;body&gt;&lt;p&gt;Select which set of roll rates / max bank angles / PIDs you want active on this switch position.&lt;/p&gt;&lt;/body&gt;&lt;/html&gt;</string>
</property>
<property name="objrelation" stdset="0">
<stringlist>
<string>objname:StabilizationSettings</string>
<string>fieldname:FlightModeMap</string>
<string>index:2</string>
<string>haslimits:no</string>
<string>scale:1</string>
<string>buttongroup:16</string>
</stringlist>
</property>
</widget>
</item>
<item>
<widget class="QComboBox" name="pidBankSs1_3">
<property name="enabled">
<bool>false</bool>
</property>
<property name="minimumSize">
<size>
<width>75</width>
<height>0</height>
</size>
</property>
<property name="focusPolicy">
<enum>Qt::StrongFocus</enum>
</property>
<property name="toolTip">
<string>&lt;html&gt;&lt;head/&gt;&lt;body&gt;&lt;p&gt;Select which set of roll rates / max bank angles / PIDs you want active on this switch position.&lt;/p&gt;&lt;/body&gt;&lt;/html&gt;</string>
</property>
<property name="objrelation" stdset="0">
<stringlist>
<string>objname:StabilizationSettings</string>
<string>fieldname:FlightModeMap</string>
<string>index:3</string>
<string>haslimits:no</string>
<string>scale:1</string>
<string>buttongroup:16</string>
</stringlist>
</property>
</widget>
</item>
<item>
<widget class="QComboBox" name="pidBankSs1_4">
<property name="enabled">
<bool>false</bool>
</property>
<property name="minimumSize">
<size>
<width>75</width>
<height>0</height>
</size>
</property>
<property name="focusPolicy">
<enum>Qt::StrongFocus</enum>
</property>
<property name="toolTip">
<string>&lt;html&gt;&lt;head/&gt;&lt;body&gt;&lt;p&gt;Select which set of roll rates / max bank angles / PIDs you want active on this switch position.&lt;/p&gt;&lt;/body&gt;&lt;/html&gt;</string>
</property>
<property name="objrelation" stdset="0">
<stringlist>
<string>objname:StabilizationSettings</string>
<string>fieldname:FlightModeMap</string>
<string>index:4</string>
<string>haslimits:no</string>
<string>scale:1</string>
<string>buttongroup:16</string>
</stringlist>
</property>
</widget>
</item>
<item>
<widget class="QComboBox" name="pidBankSs1_5">
<property name="enabled">
<bool>false</bool>
</property>
<property name="minimumSize">
<size>
<width>75</width>
<height>0</height>
</size>
</property>
<property name="focusPolicy">
<enum>Qt::StrongFocus</enum>
</property>
<property name="toolTip">
<string>&lt;html&gt;&lt;head/&gt;&lt;body&gt;&lt;p&gt;Select which set of roll rates / max bank angles / PIDs you want active on this switch position.&lt;/p&gt;&lt;/body&gt;&lt;/html&gt;</string>
</property>
<property name="objrelation" stdset="0">
<stringlist>
<string>objname:StabilizationSettings</string>
<string>fieldname:FlightModeMap</string>
<string>index:5</string>
<string>haslimits:no</string>
<string>scale:1</string>
<string>buttongroup:16</string>
</stringlist>
</property>
</widget>
</item>
</layout>
</widget>
</item>
</layout>
</widget>
</item>
<item row="2" column="0">
<spacer name="verticalSpacer_5">
<property name="orientation">
<enum>Qt::Vertical</enum>
</property>
<property name="sizeType">
<enum>QSizePolicy::Expanding</enum>
</property>
<property name="sizeHint" stdset="0">
<size>
<width>20</width>
<height>20</height>
</size>
</property>
</spacer>
</item>
</layout>
</widget>
</widget>
@ -1207,7 +2093,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>
@ -1287,12 +2182,21 @@ channel value for each flight mode.</string>
<rect>
<x>0</x>
<y>0</y>
<width>850</width>
<height>572</height>
<width>766</width>
<height>745</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>
@ -1539,15 +2443,6 @@ Applies and Saves all settings to SD</string>
</layout>
</widget>
<tabstops>
<tabstop>fmsSsPos1Roll</tabstop>
<tabstop>fmsSsPos1Pitch</tabstop>
<tabstop>fmsSsPos1Yaw</tabstop>
<tabstop>fmsSsPos2Roll</tabstop>
<tabstop>fmsSsPos2Pitch</tabstop>
<tabstop>fmsSsPos2Yaw</tabstop>
<tabstop>fmsSsPos3Roll</tabstop>
<tabstop>fmsSsPos3Pitch</tabstop>
<tabstop>fmsSsPos3Yaw</tabstop>
<tabstop>tabWidget</tabstop>
<tabstop>deadband</tabstop>
<tabstop>graphicsView</tabstop>

62
ground/openpilotgcs/src/plugins/config/inputchannelform.cpp
View File

@ -4,9 +4,9 @@
#include "manualcontrolsettings.h"
#include "gcsreceiver.h"
inputChannelForm::inputChannelForm(QWidget *parent, bool showlegend) :
InputChannelForm::InputChannelForm(QWidget *parent, bool showlegend) :
ConfigTaskWidget(parent),
ui(new Ui::inputChannelForm)
ui(new Ui::InputChannelForm)
{
ui->setupUi(this);
@ -19,6 +19,7 @@ inputChannelForm::inputChannelForm(QWidget *parent, bool showlegend) :
layout()->removeWidget(ui->legend4);
layout()->removeWidget(ui->legend5);
layout()->removeWidget(ui->legend6);
layout()->removeWidget(ui->legend7);
delete ui->legend0;
delete ui->legend1;
delete ui->legend2;
@ -26,12 +27,14 @@ inputChannelForm::inputChannelForm(QWidget *parent, bool showlegend) :
delete ui->legend4;
delete ui->legend5;
delete ui->legend6;
delete ui->legend7;
}
connect(ui->channelMin, SIGNAL(valueChanged(int)), this, SLOT(minMaxUpdated()));
connect(ui->channelMax, SIGNAL(valueChanged(int)), this, SLOT(minMaxUpdated()));
connect(ui->neutralValue, SIGNAL(valueChanged(int)), this, SLOT(neutralUpdated()));
connect(ui->channelGroup, SIGNAL(currentIndexChanged(int)), this, SLOT(groupUpdated()));
connect(ui->channelNeutral, SIGNAL(valueChanged(int)), this, SLOT(neutralUpdated(int)));
connect(ui->channelRev, SIGNAL(toggled(bool)), this, SLOT(reversedUpdated()));
// This is awkward but since we want the UI to be a dropdown but the field is not an enum
// it breaks the UAUVObject widget relation of the task gadget. Running the data through
@ -43,17 +46,17 @@ inputChannelForm::inputChannelForm(QWidget *parent, bool showlegend) :
}
inputChannelForm::~inputChannelForm()
InputChannelForm::~InputChannelForm()
{
delete ui;
}
void inputChannelForm::setName(QString &name)
void InputChannelForm::setName(QString &name)
{
ui->channelName->setText(name);
QFontMetrics metrics(ui->channelName->font());
int width = metrics.width(name) + 5;
foreach(inputChannelForm * form, parent()->findChildren<inputChannelForm *>()) {
foreach(InputChannelForm * form, parent()->findChildren<InputChannelForm *>()) {
if (form == this) {
continue;
}
@ -69,7 +72,7 @@ void inputChannelForm::setName(QString &name)
/**
* Update the direction of the slider and boundaries
*/
void inputChannelForm::minMaxUpdated()
void InputChannelForm::minMaxUpdated()
{
bool reverse = ui->channelMin->value() > ui->channelMax->value();
@ -85,9 +88,44 @@ void inputChannelForm::minMaxUpdated()
ui->channelNeutral->setInvertedControls(reverse);
}
void inputChannelForm::neutralUpdated(int newval)
void InputChannelForm::neutralUpdated()
{
ui->neutral->setText(QString::number(newval));
int neutralValue = ui->neutralValue->value();
if (ui->channelRev->isChecked()) {
if (neutralValue > ui->channelMin->value()) {
ui->channelMin->setValue(neutralValue);
} else if (neutralValue < ui->channelMax->value()) {
ui->channelMax->setValue(neutralValue);
}
} else {
if (neutralValue < ui->channelMin->value()) {
ui->channelMin->setValue(neutralValue);
} else if (neutralValue > ui->channelMax->value()) {
ui->channelMax->setValue(neutralValue);
}
}
}
void InputChannelForm::reversedUpdated()
{
int value = ui->channelNeutral->value();
int min = ui->channelMin->value();
int max = ui->channelMax->value();
if (ui->channelRev->isChecked()) {
if (min < max) {
ui->channelMax->setValue(min);
ui->channelMin->setValue(max);
ui->channelNeutral->setValue(value);
}
} else {
if (min > max) {
ui->channelMax->setValue(min);
ui->channelMin->setValue(max);
ui->channelNeutral->setValue(value);
}
}
}
/**
@ -96,7 +134,7 @@ void inputChannelForm::neutralUpdated(int newval)
* I fully admit this is terrible practice to embed data within UI
* like this. Open to suggestions. JC 2011-09-07
*/
void inputChannelForm::groupUpdated()
void InputChannelForm::groupUpdated()
{
ui->channelNumberDropdown->clear();
ui->channelNumberDropdown->addItem("Disabled");
@ -140,7 +178,7 @@ void inputChannelForm::groupUpdated()
/**
* Update the dropdown from the hidden control
*/
void inputChannelForm::channelDropdownUpdated(int newval)
void InputChannelForm::channelDropdownUpdated(int newval)
{
ui->channelNumber->setValue(newval);
}
@ -148,7 +186,7 @@ void inputChannelForm::channelDropdownUpdated(int newval)
/**
* Update the hidden control from the dropdown
*/
void inputChannelForm::channelNumberUpdated(int newval)
void InputChannelForm::channelNumberUpdated(int newval)
{
ui->channelNumberDropdown->setCurrentIndex(newval);
}

13
ground/openpilotgcs/src/plugins/config/inputchannelform.h
View File

@ -4,26 +4,27 @@
#include <QWidget>
#include "configinputwidget.h"
namespace Ui {
class inputChannelForm;
class InputChannelForm;
}
class inputChannelForm : public ConfigTaskWidget {
class InputChannelForm : public ConfigTaskWidget {
Q_OBJECT
public:
explicit inputChannelForm(QWidget *parent = 0, bool showlegend = false);
~inputChannelForm();
explicit InputChannelForm(QWidget *parent = 0, bool showlegend = false);
~InputChannelForm();
friend class ConfigInputWidget;
void setName(QString &name);
private slots:
void minMaxUpdated();
void neutralUpdated(int);
void neutralUpdated();
void reversedUpdated();
void groupUpdated();
void channelDropdownUpdated(int);
void channelNumberUpdated(int);
private:
Ui::inputChannelForm *ui;
Ui::InputChannelForm *ui;
};
#endif // INPUTCHANNELFORM_H

545
ground/openpilotgcs/src/plugins/config/inputchannelform.ui
View File

@ -1,13 +1,13 @@
<?xml version="1.0" encoding="UTF-8"?>
<ui version="4.0">
<class>inputChannelForm</class>
<widget class="QWidget" name="inputChannelForm">
<class>InputChannelForm</class>
<widget class="QWidget" name="InputChannelForm">
<property name="geometry">
<rect>
<x>0</x>
<y>0</y>
<width>828</width>
<height>69</height>
<height>93</height>
</rect>
</property>
<property name="windowTitle">
@ -26,6 +26,169 @@
<property name="bottomMargin">
<number>6</number>
</property>
<item row="0" column="5" colspan="2">
<widget class="QLabel" name="legend4">
<property name="enabled">
<bool>true</bool>
</property>
<property name="sizePolicy">
<sizepolicy hsizetype="Expanding" vsizetype="Fixed">
<horstretch>0</horstretch>
<verstretch>0</verstretch>
</sizepolicy>
</property>
<property name="minimumSize">
<size>
<width>0</width>
<height>20</height>
</size>
</property>
<property name="font">
<font>
<weight>75</weight>
<italic>false</italic>
<bold>true</bold>
</font>
</property>
<property name="toolTip">
<string>Channel neutral</string>
</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;
margin:1px;
font:bold;</string>
</property>
<property name="frameShape">
<enum>QFrame::StyledPanel</enum>
</property>
<property name="text">
<string>Neutral</string>
</property>
<property name="alignment">
<set>Qt::AlignCenter</set>
</property>
</widget>
</item>
<item row="1" column="8">
<widget class="QSpinBox" name="channelMax">
<property name="minimumSize">
<size>
<width>0</width>
<height>25</height>
</size>
</property>
<property name="focusPolicy">
<enum>Qt::StrongFocus</enum>
</property>
<property name="buttonSymbols">
<enum>QAbstractSpinBox::UpDownArrows</enum>
</property>
<property name="maximum">
<number>9999</number>
</property>
<property name="value">
<number>1000</number>
</property>
</widget>
</item>
<item row="0" column="12">
<widget class="QLabel" name="legend7">
<property name="enabled">
<bool>true</bool>
</property>
<property name="sizePolicy">
<sizepolicy hsizetype="Minimum" vsizetype="Fixed">
<horstretch>0</horstretch>
<verstretch>0</verstretch>
</sizepolicy>
</property>
<property name="minimumSize">
<size>
<width>30</width>
<height>20</height>
</size>
</property>
<property name="font">
<font>
<weight>75</weight>
<italic>false</italic>
<bold>true</bold>
</font>
</property>
<property name="toolTip">
<string>Response time</string>
</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;
margin:1px;
font:bold;</string>
</property>
<property name="frameShape">
<enum>QFrame::StyledPanel</enum>
</property>
<property name="lineWidth">
<number>1</number>
</property>
<property name="text">
<string>RT</string>
</property>
<property name="alignment">
<set>Qt::AlignCenter</set>
</property>
</widget>
</item>
<item row="0" column="10" colspan="2">
<widget class="QLabel" name="legend6">
<property name="enabled">
<bool>true</bool>
</property>
<property name="sizePolicy">
<sizepolicy hsizetype="Minimum" vsizetype="Fixed">
<horstretch>0</horstretch>
<verstretch>0</verstretch>
</sizepolicy>
</property>
<property name="minimumSize">
<size>
<width>75</width>
<height>20</height>
</size>
</property>
<property name="font">
<font>
<weight>75</weight>
<italic>false</italic>
<bold>true</bold>
</font>
</property>
<property name="toolTip">
<string>Channel values are inverted</string>
</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;
margin:1px;
font:bold;</string>
</property>
<property name="frameShape">
<enum>QFrame::StyledPanel</enum>
</property>
<property name="lineWidth">
<number>1</number>
</property>
<property name="text">
<string>Reversed</string>
</property>
<property name="alignment">
<set>Qt::AlignCenter</set>
</property>
</widget>
</item>
<item row="0" column="0">
<widget class="QLabel" name="legend0">
<property name="enabled">
@ -50,6 +213,9 @@
<bold>true</bold>
</font>
</property>
<property name="toolTip">
<string>Channel function</string>
</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);
@ -92,6 +258,9 @@ font:bold;</string>
<bold>true</bold>
</font>
</property>
<property name="toolTip">
<string>Channel type</string>
</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);
@ -134,6 +303,9 @@ font:bold;</string>
<bold>true</bold>
</font>
</property>
<property name="toolTip">
<string>Channel number</string>
</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);
@ -176,6 +348,9 @@ font:bold;</string>
<bold>true</bold>
</font>
</property>
<property name="toolTip">
<string>Channel min</string>
</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);
@ -194,48 +369,6 @@ font:bold;</string>
</property>
</widget>
</item>
<item row="0" column="5">
<widget class="QLabel" name="legend4">
<property name="enabled">
<bool>true</bool>
</property>
<property name="sizePolicy">
<sizepolicy hsizetype="Expanding" vsizetype="Fixed">
<horstretch>0</horstretch>
<verstretch>0</verstretch>
</sizepolicy>
</property>
<property name="minimumSize">
<size>
<width>0</width>
<height>20</height>
</size>
</property>
<property name="font">
<font>
<weight>75</weight>
<italic>false</italic>
<bold>true</bold>
</font>
</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;
margin:1px;
font:bold;</string>
</property>
<property name="frameShape">
<enum>QFrame::StyledPanel</enum>
</property>
<property name="text">
<string>Neutral</string>
</property>
<property name="alignment">
<set>Qt::AlignCenter</set>
</property>
</widget>
</item>
<item row="1" column="0">
<widget class="QLabel" name="channelName">
<property name="sizePolicy">
@ -255,31 +388,6 @@ font:bold;</string>
</property>
</widget>
</item>
<item row="1" column="1">
<widget class="QComboBox" name="channelGroup">
<property name="sizePolicy">
<sizepolicy hsizetype="Maximum" vsizetype="Fixed">
<horstretch>6</horstretch>
<verstretch>0</verstretch>
</sizepolicy>
</property>
<property name="minimumSize">
<size>
<width>0</width>
<height>25</height>
</size>
</property>
<property name="maximumSize">
<size>
<width>100</width>
<height>16777215</height>
</size>
</property>
<property name="focusPolicy">
<enum>Qt::StrongFocus</enum>
</property>
</widget>
</item>
<item row="1" column="2">
<widget class="QComboBox" name="channelNumberDropdown">
<property name="sizePolicy">
@ -308,6 +416,31 @@ font:bold;</string>
</property>
</widget>
</item>
<item row="1" column="1">
<widget class="QComboBox" name="channelGroup">
<property name="sizePolicy">
<sizepolicy hsizetype="Maximum" vsizetype="Fixed">
<horstretch>6</horstretch>
<verstretch>0</verstretch>
</sizepolicy>
</property>
<property name="minimumSize">
<size>
<width>0</width>
<height>25</height>
</size>
</property>
<property name="maximumSize">
<size>
<width>100</width>
<height>16777215</height>
</size>
</property>
<property name="focusPolicy">
<enum>Qt::StrongFocus</enum>
</property>
</widget>
</item>
<item row="1" column="3">
<widget class="QSpinBox" name="channelMin">
<property name="minimumSize">
@ -320,7 +453,7 @@ font:bold;</string>
<enum>Qt::StrongFocus</enum>
</property>
<property name="buttonSymbols">
<enum>QAbstractSpinBox::NoButtons</enum>
<enum>QAbstractSpinBox::UpDownArrows</enum>
</property>
<property name="maximum">
<number>9999</number>
@ -346,6 +479,70 @@ font:bold;</string>
</property>
</spacer>
</item>
<item row="0" column="8">
<widget class="QLabel" name="legend5">
<property name="enabled">
<bool>true</bool>
</property>
<property name="sizePolicy">
<sizepolicy hsizetype="Minimum" vsizetype="Fixed">
<horstretch>0</horstretch>
<verstretch>0</verstretch>
</sizepolicy>
</property>
<property name="minimumSize">
<size>
<width>0</width>
<height>20</height>
</size>
</property>
<property name="font">
<font>
<weight>75</weight>
<italic>false</italic>
<bold>true</bold>
</font>
</property>
<property name="toolTip">
<string>Channel max</string>
</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;
margin:1px;
font:bold;</string>
</property>
<property name="frameShape">
<enum>QFrame::StyledPanel</enum>
</property>
<property name="lineWidth">
<number>1</number>
</property>
<property name="text">
<string>Max</string>
</property>
<property name="alignment">
<set>Qt::AlignCenter</set>
</property>
</widget>
</item>
<item row="1" column="7">
<spacer name="horizontalSpacer_2">
<property name="orientation">
<enum>Qt::Horizontal</enum>
</property>
<property name="sizeType">
<enum>QSizePolicy::Fixed</enum>
</property>
<property name="sizeHint" stdset="0">
<size>
<width>10</width>
<height>20</height>
</size>
</property>
</spacer>
</item>
<item row="1" column="5">
<widget class="QSlider" name="channelNeutral">
<property name="sizePolicy">
@ -368,112 +565,13 @@ font:bold;</string>
</property>
</widget>
</item>
<item row="1" column="6">
<spacer name="horizontalSpacer_2">
<property name="orientation">
<enum>Qt::Horizontal</enum>
</property>
<property name="sizeType">
<enum>QSizePolicy::Fixed</enum>
</property>
<property name="sizeHint" stdset="0">
<size>
<width>10</width>
<height>20</height>
</size>
</property>
</spacer>
</item>
<item row="1" column="7">
<widget class="QSpinBox" name="channelMax">
<property name="minimumSize">
<size>
<width>0</width>
<height>25</height>
</size>
</property>
<property name="focusPolicy">
<enum>Qt::StrongFocus</enum>
</property>
<property name="buttonSymbols">
<enum>QAbstractSpinBox::NoButtons</enum>
</property>
<property name="maximum">
<number>9999</number>
</property>
<property name="value">
<number>1000</number>
</property>
</widget>
</item>
<item row="0" column="7">
<widget class="QLabel" name="legend5">
<property name="enabled">
<bool>true</bool>
</property>
<property name="sizePolicy">
<sizepolicy hsizetype="Minimum" vsizetype="Fixed">
<horstretch>0</horstretch>
<verstretch>0</verstretch>
</sizepolicy>
</property>
<property name="minimumSize">
<size>
<width>0</width>
<height>20</height>
</size>
</property>
<property name="font">
<font>
<weight>75</weight>
<italic>false</italic>
<bold>true</bold>
</font>
</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;
margin:1px;
font:bold;</string>
</property>
<property name="frameShape">
<enum>QFrame::StyledPanel</enum>
</property>
<property name="lineWidth">
<number>1</number>
</property>
<property name="text">
<string>Max</string>
</property>
<property name="alignment">
<set>Qt::AlignCenter</set>
</property>
</widget>
</item>
<item row="1" column="9">
<widget class="QCheckBox" name="channelRev">
<property name="enabled">
<bool>false</bool>
</property>
<property name="minimumSize">
<size>
<width>0</width>
<height>20</height>
</size>
</property>
<property name="text">
<string>Rev</string>
</property>
</widget>
</item>
<item row="1" column="10">
<item row="1" column="12">
<widget class="QSpinBox" name="channelResponseTime">
<property name="enabled">
<bool>true</bool>
</property>
<property name="sizePolicy">
<sizepolicy hsizetype="Preferred" vsizetype="Fixed">
<sizepolicy hsizetype="Fixed" vsizetype="Fixed">
<horstretch>0</horstretch>
<verstretch>0</verstretch>
</sizepolicy>
@ -501,71 +599,72 @@ even lead to crash. Use with caution.</string>
<bool>true</bool>
</property>
<property name="buttonSymbols">
<enum>QAbstractSpinBox::NoButtons</enum>
<enum>QAbstractSpinBox::UpDownArrows</enum>
</property>
<property name="maximum">
<number>999</number>
</property>
</widget>
</item>
<item row="0" column="10">
<widget class="QLabel" name="legend6">
<property name="sizePolicy">
<sizepolicy hsizetype="Minimum" vsizetype="Fixed">
<horstretch>0</horstretch>
<verstretch>0</verstretch>
</sizepolicy>
</property>
<item row="1" column="10" colspan="2">
<widget class="QFrame" name="frame">
<property name="minimumSize">
<size>
<width>30</width>
<height>26</height>
</size>
</property>
<property name="font">
<font>
<weight>75</weight>
<italic>false</italic>
<bold>true</bold>
</font>
</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;
margin:5px;
font:bold;</string>
</property>
<property name="text">
<string>RT</string>
</property>
<property name="alignment">
<set>Qt::AlignCenter</set>
</property>
</widget>
</item>
<item row="1" column="8">
<widget class="QLabel" name="neutral">
<property name="sizePolicy">
<sizepolicy hsizetype="Preferred" vsizetype="Fixed">
<horstretch>0</horstretch>
<verstretch>0</verstretch>
</sizepolicy>
</property>
<property name="minimumSize">
<size>
<width>48</width>
<width>75</width>
<height>0</height>
</size>
</property>
<property name="maximumSize">
<property name="frameShape">
<enum>QFrame::NoFrame</enum>
</property>
<property name="frameShadow">
<enum>QFrame::Raised</enum>
</property>
<layout class="QGridLayout" name="gridLayout_2">
<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 row="0" column="0" alignment="Qt::AlignHCenter">
<widget class="QCheckBox" name="channelRev">
<property name="enabled">
<bool>true</bool>
</property>
<property name="minimumSize">
<size>
<width>0</width>
<height>20</height>
</size>
</property>
<property name="text">
<string/>
</property>
</widget>
</item>
</layout>
</widget>
</item>
<item row="1" column="6">
<widget class="QSpinBox" name="neutralValue">
<property name="minimumSize">
<size>
<width>45</width>
<height>16777215</height>
<width>0</width>
<height>25</height>
</size>
</property>
<property name="text">
<string/>
<property name="maximum">
<number>9999</number>
</property>
<property name="value">
<number>1000</number>
</property>
</widget>
</item>

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

File diff suppressed because it is too large Load Diff

3
ground/openpilotgcs/src/plugins/config/twostep.cpp
View File

@ -124,7 +124,7 @@ Vector3f twostep_bias_only(const Vector3f samples[],
const float noise)
{
// \tilde{H}
Vector3f centeredSamples[n_samples];
Vector3f *centeredSamples = new Vector3f[n_samples];
// z_k
float sampleDeltaMag[n_samples];
// eq 7 and 8 applied to samples
@ -170,6 +170,7 @@ Vector3f twostep_bias_only(const Vector3f samples[],
// Note that the negative has been done twice
estimate += neg_increment;
}
delete[] centeredSamples;
return estimate;
}

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

42
ground/openpilotgcs/src/plugins/ophid/src/ophid.cpp
View File

@ -205,6 +205,8 @@ qint64 RawHIDReadThread::getBytesAvailable()
return m_readBuffer.size();
}
// *********************************************************************************
RawHIDWriteThread::RawHIDWriteThread(RawHID *hid)
: m_hid(hid),
hiddev(&hid->dev),
@ -212,8 +214,6 @@ RawHIDWriteThread::RawHIDWriteThread(RawHID *hid)
m_running(true)
{}
// *********************************************************************************
RawHIDWriteThread::~RawHIDWriteThread()
{
m_running = false;
@ -227,29 +227,31 @@ void RawHIDWriteThread::run()
{
while (m_running) {
char buffer[WRITE_SIZE] = { 0 };
int size;
m_writeBufMtx.lock();
int size = qMin(WRITE_SIZE - 2, m_writeBuffer.size());
while (size <= 0) {
// wait on new data to write condition, the timeout
// enable the thread to shutdown properly
m_newDataToWrite.wait(&m_writeBufMtx, 200);
if (!m_running) {
return;
{
QMutexLocker lock(&m_writeBufMtx);
size = qMin(WRITE_SIZE - 2, m_writeBuffer.size());
while (size <= 0) {
// wait on new data to write condition, the timeout
// enable the thread to shutdown properly
m_newDataToWrite.wait(&m_writeBufMtx, 200);
if (!m_running) {
return;
}
size = m_writeBuffer.size();
}
size = m_writeBuffer.size();
// NOTE: data size is limited to 2 bytes less than the
// usb packet size (64 bytes for interrupt) to make room
// for the reportID and valid data length
size = qMin(WRITE_SIZE - 2, m_writeBuffer.size());
memcpy(&buffer[2], m_writeBuffer.constData(), size);
buffer[1] = size; // valid data length
buffer[0] = 2; // reportID
}
// NOTE: data size is limited to 2 bytes less than the
// usb packet size (64 bytes for interrupt) to make room
// for the reportID and valid data length
size = qMin(WRITE_SIZE - 2, m_writeBuffer.size());
memcpy(&buffer[2], m_writeBuffer.constData(), size);
buffer[1] = size; // valid data length
buffer[0] = 2; // reportID
m_writeBufMtx.unlock();
// must hold lock through the send to know how much was sent
int ret = hiddev->send(hidno, buffer, WRITE_SIZE, WRITE_TIMEOUT);

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)

15
ground/openpilotgcs/src/plugins/uavobjectbrowser/treeitem.cpp
View File

@ -42,11 +42,11 @@ HighLightManager::HighLightManager(long checkingInterval)
bool HighLightManager::add(TreeItem *itemToAdd)
{
// Lock to ensure thread safety
QMutexLocker locker(&m_listMutex);
QMutexLocker locker(&m_mutex);
// Check so that the item isn't already in the list
if (!m_itemsList.contains(itemToAdd)) {
m_itemsList.append(itemToAdd);
if (!m_items.contains(itemToAdd)) {
m_items.insert(itemToAdd);
return true;
}
return false;
@ -59,10 +59,10 @@ bool HighLightManager::add(TreeItem *itemToAdd)
bool HighLightManager::remove(TreeItem *itemToRemove)
{
// Lock to ensure thread safety
QMutexLocker locker(&m_listMutex);
QMutexLocker locker(&m_mutex);
// Remove item and return result
return m_itemsList.removeOne(itemToRemove);
return m_items.remove(itemToRemove);
}
/*
@ -74,10 +74,10 @@ bool HighLightManager::remove(TreeItem *itemToRemove)
void HighLightManager::checkItemsExpired()
{
// Lock to ensure thread safety
QMutexLocker locker(&m_listMutex);
QMutexLocker locker(&m_mutex);
// Get a mutable iterator for the list
QMutableLinkedListIterator<TreeItem *> iter(m_itemsList);
QMutableSetIterator<TreeItem *> iter(m_items);
// This is the timestamp to compare with
QTime now = QTime::currentTime();
@ -211,7 +211,6 @@ void TreeItem::setHighlight(bool highlight)
void TreeItem::removeHighlight()
{
m_highlight = false;
// update();
emit updateHighlight(this);
}

12
ground/openpilotgcs/src/plugins/uavobjectbrowser/treeitem.h
View File

@ -78,11 +78,11 @@ private:
// The timer checking highlight expiration.
QTimer m_expirationTimer;
// The list holding all items due to be updated.
QLinkedList<TreeItem *> m_itemsList;
// The collection holding all items due to be updated.
QSet<TreeItem *> m_items;
// Mutex to lock when accessing list.
QMutex m_listMutex;
// Mutex to lock when accessing collection.
QMutex m_mutex;
};
class TreeItem : public QObject {
@ -231,8 +231,8 @@ public:
QList<MetaObjectTreeItem *> getMetaObjectItems();
private:
QMap<quint32, DataObjectTreeItem *> m_objectTreeItemsPerObjectIds;
QMap<quint32, MetaObjectTreeItem *> m_metaObjectTreeItemsPerObjectIds;
QHash<quint32, DataObjectTreeItem *> m_objectTreeItemsPerObjectIds;
QHash<quint32, MetaObjectTreeItem *> m_metaObjectTreeItemsPerObjectIds;
};
class ObjectTreeItem : public TreeItem {

2
ground/openpilotgcs/src/plugins/uavobjectbrowser/uavobjectbrowserwidget.cpp
View File

@ -122,7 +122,7 @@ void UAVObjectBrowserWidget::useScientificNotation(bool scientific)
Q_ASSERT(objManager);
UAVObjectTreeModel *tmpModel = m_model;
m_model = new UAVObjectTreeModel(0, m_viewoptions->cbCategorized, scientific);
m_model = new UAVObjectTreeModel(0, m_viewoptions->cbCategorized->isChecked(), scientific);
m_model->setRecentlyUpdatedColor(m_recentlyUpdatedColor);
m_model->setManuallyChangedColor(m_manuallyChangedColor);
m_model->setRecentlyUpdatedTimeout(m_recentlyUpdatedTimeout);

12
ground/openpilotgcs/src/plugins/uavobjectbrowser/uavobjecttreemodel.cpp
View File

@ -33,7 +33,6 @@
#include "uavobjectfield.h"
#include "extensionsystem/pluginmanager.h"
#include <QColor>
// #include <QIcon>
#include <QtCore/QTimer>
#include <QtCore/QSignalMapper>
#include <QtCore/QDebug>
@ -41,6 +40,7 @@
UAVObjectTreeModel::UAVObjectTreeModel(QObject *parent, bool categorize, bool useScientificNotation) :
QAbstractItemModel(parent),
m_useScientificFloatNotation(useScientificNotation),
m_categorize(categorize),
m_recentlyUpdatedTimeout(500), // ms
m_recentlyUpdatedColor(QColor(255, 230, 230)),
m_manuallyChangedColor(QColor(230, 230, 255))
@ -54,7 +54,7 @@ UAVObjectTreeModel::UAVObjectTreeModel(QObject *parent, bool categorize, bool us
connect(objManager, SIGNAL(newInstance(UAVObject *)), this, SLOT(newObject(UAVObject *)));
TreeItem::setHighlightTime(m_recentlyUpdatedTimeout);
setupModelData(objManager, categorize);
setupModelData(objManager);
}
UAVObjectTreeModel::~UAVObjectTreeModel()
@ -63,7 +63,7 @@ UAVObjectTreeModel::~UAVObjectTreeModel()
delete m_rootItem;
}
void UAVObjectTreeModel::setupModelData(UAVObjectManager *objManager, bool categorize)
void UAVObjectTreeModel::setupModelData(UAVObjectManager *objManager)
{
// root
QList<QVariant> rootData;
@ -83,7 +83,7 @@ void UAVObjectTreeModel::setupModelData(UAVObjectManager *objManager, bool categ
QList< QList<UAVDataObject *> > objList = objManager->getDataObjects();
foreach(QList<UAVDataObject *> list, objList) {
foreach(UAVDataObject * obj, list) {
addDataObject(obj, categorize);
addDataObject(obj);
}
}
}
@ -97,13 +97,13 @@ void UAVObjectTreeModel::newObject(UAVObject *obj)
}
}
void UAVObjectTreeModel::addDataObject(UAVDataObject *obj, bool categorize)
void UAVObjectTreeModel::addDataObject(UAVDataObject *obj)
{
TopTreeItem *root = obj->isSettings() ? m_settingsTree : m_nonSettingsTree;
TreeItem *parent = root;
if (categorize && obj->getCategory() != 0 && !obj->getCategory().isEmpty()) {
if (m_categorize && obj->getCategory() != 0 && !obj->getCategory().isEmpty()) {
QStringList categoryPath = obj->getCategory().split('/');
parent = createCategoryItems(categoryPath, root);
}

5
ground/openpilotgcs/src/plugins/uavobjectbrowser/uavobjecttreemodel.h
View File

@ -92,9 +92,9 @@ private slots:
void updateHighlight(TreeItem *);
private:
void setupModelData(UAVObjectManager *objManager, bool categorize = true);
void setupModelData(UAVObjectManager *objManager);
QModelIndex index(TreeItem *item);
void addDataObject(UAVDataObject *obj, bool categorize = true);
void addDataObject(UAVDataObject *obj);
MetaObjectTreeItem *addMetaObject(UAVMetaObject *obj, TreeItem *parent);
void addArrayField(UAVObjectField *field, TreeItem *parent);
void addSingleField(int index, UAVObjectField *field, TreeItem *parent);
@ -115,6 +115,7 @@ private:
QColor m_manuallyChangedColor;
bool m_onlyHilightChangedValues;
bool m_useScientificFloatNotation;
bool m_categorize;
// Highlight manager to handle highlighting of tree items.
HighLightManager *m_highlightManager;

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