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

Merged librepilot/librepilot:next into mindnever/librepilot:LP-580_Add_QMC5883L_driver

Browse Source
This commit is contained in:
Vladimir Zidar 2022-02-15 22:14:54 +00:00
commit f60b6bb630
265 changed files with 15574 additions and 2740 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

9
CREDITS.txt
View File

@ -1,4 +1,5 @@
Connor Abbott
Karl Alberts
Sergiy Anikeyev
David Ankers
Fredrik Arvidsson
@ -34,7 +35,7 @@ Andrew Finegan
Kevin Finisterre
Richard Flay
Darren Furniss
Oliver Gaste
Olivier Gasté
Cliff Geerdes
Frederic Goddeeris
Daniel Godin
@ -69,9 +70,11 @@ Fredrik Larsson
Xavier Lecluse
Richard Von Lehe
Pablo Lema
Julian Lilov
Matt Lipski
David Llama
Jasper Van Loenen
Fernando Lopez Jr.
Ákos Máté
Ben Matthews
Greg Matthews
@ -80,9 +83,12 @@ Alessio Morale
Gary Mortimer
Cathy Moss
Les Newell
Karsten Telling Nielsen
Jan Nijs
Ken Northup
Craig Nuttall
Bertrand Oresve
Pashalis Padeleris
Angus Peart
Pablo Francisco Pérez Hidalgo
John Pike
@ -130,4 +136,3 @@ Kendal Wells
David Willis
Dmitriy Zaitsev
Vladimir Zidar

180
flight/libraries/WorldMagModel.c
View File

@ -2,7 +2,7 @@
******************************************************************************
*
* @file WorldMagModel.c
* @author The LibrePilot Project, http://www.librepilot.org Copyright (C) 2015.
* @author The LibrePilot Project, http://www.librepilot.org Copyright (C) 2015-2019.
* The OpenPilot Team, http://www.openpilot.org Copyright (C) 2010.
* @brief Source file for the World Magnetic Model
* This is a port of code available from the US NOAA.
@ -61,96 +61,96 @@
// first column not used but it will be optimized out by compiler
static const float CoeffFile[91][6] = {
{ 0.0f, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f },
{ 1.0f, 0.0f, -29438.5f, 0.0f, 10.7f, 0.0f },
{ 1.0f, 1.0f, -1501.1f, 4796.2f, 17.9f, -26.8f },
{ 2.0f, 0.0f, -2445.3f, 0.0f, -8.6f, 0.0f },
{ 2.0f, 1.0f, 3012.5f, -2845.6f, -3.3f, -27.1f },
{ 2.0f, 2.0f, 1676.6f, -642.0f, 2.4f, -13.3f },
{ 3.0f, 0.0f, 1351.1f, 0.0f, 3.1f, 0.0f },
{ 3.0f, 1.0f, -2352.3f, -115.3f, -6.2f, 8.4f },
{ 3.0f, 2.0f, 1225.6f, 245.0f, -0.4f, -0.4f },
{ 3.0f, 3.0f, 581.9f, -538.3f, -10.4f, 2.3f },
{ 4.0f, 0.0f, 907.2f, 0.0f, -0.4f, 0.0f },
{ 4.0f, 1.0f, 813.7f, 283.4f, 0.8f, -0.6f },
{ 4.0f, 2.0f, 120.3f, -188.6f, -9.2f, 5.3f },
{ 4.0f, 3.0f, -335.0f, 180.9f, 4.0f, 3.0f },
{ 4.0f, 4.0f, 70.3f, -329.5f, -4.2f, -5.3f },
{ 5.0f, 0.0f, -232.6f, 0.0f, -0.2f, 0.0f },
{ 5.0f, 1.0f, 360.1f, 47.4f, 0.1f, 0.4f },
{ 5.0f, 2.0f, 192.4f, 196.9f, -1.4f, 1.6f },
{ 5.0f, 3.0f, -141.0f, -119.4f, 0.0f, -1.1f },
{ 5.0f, 4.0f, -157.4f, 16.1f, 1.3f, 3.3f },
{ 5.0f, 5.0f, 4.3f, 100.1f, 3.8f, 0.1f },
{ 6.0f, 0.0f, 69.5f, 0.0f, -0.5f, 0.0f },
{ 6.0f, 1.0f, 67.4f, -20.7f, -0.2f, 0.0f },
{ 6.0f, 2.0f, 72.8f, 33.2f, -0.6f, -2.2f },
{ 6.0f, 3.0f, -129.8f, 58.8f, 2.4f, -0.7f },
{ 6.0f, 4.0f, -29.0f, -66.5f, -1.1f, 0.1f },
{ 6.0f, 5.0f, 13.2f, 7.3f, 0.3f, 1.0f },
{ 6.0f, 6.0f, -70.9f, 62.5f, 1.5f, 1.3f },
{ 7.0f, 0.0f, 81.6f, 0.0f, 0.2f, 0.0f },
{ 7.0f, 1.0f, -76.1f, -54.1f, -0.2f, 0.7f },
{ 7.0f, 2.0f, -6.8f, -19.4f, -0.4f, 0.5f },
{ 7.0f, 3.0f, 51.9f, 5.6f, 1.3f, -0.2f },
{ 7.0f, 4.0f, 15.0f, 24.4f, 0.2f, -0.1f },
{ 7.0f, 5.0f, 9.3f, 3.3f, -0.4f, -0.7f },
{ 7.0f, 6.0f, -2.8f, -27.5f, -0.9f, 0.1f },
{ 7.0f, 7.0f, 6.7f, -2.3f, 0.3f, 0.1f },
{ 8.0f, 0.0f, 24.0f, 0.0f, 0.0f, 0.0f },
{ 8.0f, 1.0f, 8.6f, 10.2f, 0.1f, -0.3f },
{ 8.0f, 2.0f, -16.9f, -18.1f, -0.5f, 0.3f },
{ 8.0f, 3.0f, -3.2f, 13.2f, 0.5f, 0.3f },
{ 8.0f, 4.0f, -20.6f, -14.6f, -0.2f, 0.6f },
{ 8.0f, 5.0f, 13.3f, 16.2f, 0.4f, -0.1f },
{ 8.0f, 6.0f, 11.7f, 5.7f, 0.2f, -0.2f },
{ 8.0f, 7.0f, -16.0f, -9.1f, -0.4f, 0.3f },
{ 8.0f, 8.0f, -2.0f, 2.2f, 0.3f, 0.0f },
{ 9.0f, 0.0f, 5.4f, 0.0f, 0.0f, 0.0f },
{ 9.0f, 1.0f, 8.8f, -21.6f, -0.1f, -0.2f },
{ 9.0f, 2.0f, 3.1f, 10.8f, -0.1f, -0.1f },
{ 9.0f, 3.0f, -3.1f, 11.7f, 0.4f, -0.2f },
{ 9.0f, 4.0f, 0.6f, -6.8f, -0.5f, 0.1f },
{ 9.0f, 5.0f, -13.3f, -6.9f, -0.2f, 0.1f },
{ 9.0f, 6.0f, -0.1f, 7.8f, 0.1f, 0.0f },
{ 9.0f, 7.0f, 8.7f, 1.0f, 0.0f, -0.2f },
{ 9.0f, 8.0f, -9.1f, -3.9f, -0.2f, 0.4f },
{ 9.0f, 9.0f, -10.5f, 8.5f, -0.1f, 0.3f },
{ 10.0f, 0.0f, -1.9f, 0.0f, 0.0f, 0.0f },
{ 10.0f, 1.0f, -6.5f, 3.3f, 0.0f, 0.1f },
{ 10.0f, 2.0f, 0.2f, -0.3f, -0.1f, -0.1f },
{ 10.0f, 3.0f, 0.6f, 4.6f, 0.3f, 0.0f },
{ 10.0f, 4.0f, -0.6f, 4.4f, -0.1f, 0.0f },
{ 10.0f, 5.0f, 1.7f, -7.9f, -0.1f, -0.2f },
{ 10.0f, 6.0f, -0.7f, -0.6f, -0.1f, 0.1f },
{ 10.0f, 7.0f, 2.1f, -4.1f, 0.0f, -0.1f },
{ 10.0f, 8.0f, 2.3f, -2.8f, -0.2f, -0.2f },
{ 10.0f, 9.0f, -1.8f, -1.1f, -0.1f, 0.1f },
{ 10.0f, 10.0f, -3.6f, -8.7f, -0.2f, -0.1f },
{ 11.0f, 0.0f, 3.1f, 0.0f, 0.0f, 0.0f },
{ 11.0f, 1.0f, -1.5f, -0.1f, 0.0f, 0.0f },
{ 11.0f, 2.0f, -2.3f, 2.1f, -0.1f, 0.1f },
{ 11.0f, 3.0f, 2.1f, -0.7f, 0.1f, 0.0f },
{ 11.0f, 4.0f, -0.9f, -1.1f, 0.0f, 0.1f },
{ 11.0f, 5.0f, 0.6f, 0.7f, 0.0f, 0.0f },
{ 11.0f, 6.0f, -0.7f, -0.2f, 0.0f, 0.0f },
{ 11.0f, 7.0f, 0.2f, -2.1f, 0.0f, 0.1f },
{ 11.0f, 8.0f, 1.7f, -1.5f, 0.0f, 0.0f },
{ 11.0f, 9.0f, -0.2f, -2.5f, 0.0f, -0.1f },
{ 11.0f, 10.0f, 0.4f, -2.0f, -0.1f, 0.0f },
{ 1.0f, 0.0f, -29438.2f, 0.0f, 7.0f, 0.0f },
{ 1.0f, 1.0f, -1493.5f, 4796.3f, 9.0f, -30.2f },
{ 2.0f, 0.0f, -2444.5f, 0.0f, -11.0f, 0.0f },
{ 2.0f, 1.0f, 3014.7f, -2842.4f, -6.2f, -29.6f },
{ 2.0f, 2.0f, 1679.0f, -638.8f, 0.3f, -17.3f },
{ 3.0f, 0.0f, 1351.8f, 0.0f, 2.4f, 0.0f },
{ 3.0f, 1.0f, -2351.6f, -113.7f, -5.7f, 6.5f },
{ 3.0f, 2.0f, 1223.6f, 246.5f, 2.0f, -0.8f },
{ 3.0f, 3.0f, 582.3f, -537.4f, -11.0f, -2.0f },
{ 4.0f, 0.0f, 907.5f, 0.0f, -0.8f, 0.0f },
{ 4.0f, 1.0f, 814.8f, 283.3f, -0.9f, -0.4f },
{ 4.0f, 2.0f, 117.8f, -188.6f, -6.5f, 5.8f },
{ 4.0f, 3.0f, -335.6f, 180.7f, 5.2f, 3.8f },
{ 4.0f, 4.0f, 69.7f, -330.0f, -4.0f, -3.5f },
{ 5.0f, 0.0f, -232.9f, 0.0f, -0.3f, 0.0f },
{ 5.0f, 1.0f, 360.1f, 46.9f, 0.6f, 0.2f },
{ 5.0f, 2.0f, 191.7f, 196.5f, -0.8f, 2.3f },
{ 5.0f, 3.0f, -141.3f, -119.9f, 0.1f, -0.0f },
{ 5.0f, 4.0f, -157.2f, 16.0f, 1.2f, 3.3f },
{ 5.0f, 5.0f, 7.7f, 100.6f, 1.4f, -0.6f },
{ 6.0f, 0.0f, 69.4f, 0.0f, -0.8f, 0.0f },
{ 6.0f, 1.0f, 67.7f, -20.1f, -0.5f, 0.3f },
{ 6.0f, 2.0f, 72.3f, 32.8f, -0.1f, -1.5f },
{ 6.0f, 3.0f, -129.1f, 59.1f, 1.6f, -1.2f },
{ 6.0f, 4.0f, -28.4f, -67.1f, -1.6f, 0.4f },
{ 6.0f, 5.0f, 13.6f, 8.1f, 0.0f, 0.2f },
{ 6.0f, 6.0f, -70.3f, 61.9f, 1.2f, 1.3f },
{ 7.0f, 0.0f, 81.7f, 0.0f, -0.3f, 0.0f },
{ 7.0f, 1.0f, -75.9f, -54.3f, -0.2f, 0.6f },
{ 7.0f, 2.0f, -7.1f, -19.5f, -0.3f, 0.5f },
{ 7.0f, 3.0f, 52.2f, 6.0f, 0.9f, -0.8f },
{ 7.0f, 4.0f, 15.0f, 24.5f, 0.1f, -0.2f },
{ 7.0f, 5.0f, 9.1f, 3.5f, -0.6f, -1.1f },
{ 7.0f, 6.0f, -3.0f, -27.7f, -0.9f, 0.1f },
{ 7.0f, 7.0f, 5.9f, -2.9f, 0.7f, 0.2f },
{ 8.0f, 0.0f, 24.2f, 0.0f, -0.1f, 0.0f },
{ 8.0f, 1.0f, 8.9f, 10.1f, 0.2f, -0.4f },
{ 8.0f, 2.0f, -16.9f, -18.3f, -0.2f, 0.6f },
{ 8.0f, 3.0f, -3.1f, 13.3f, 0.5f, -0.1f },
{ 8.0f, 4.0f, -20.7f, -14.5f, -0.1f, 0.6f },
{ 8.0f, 5.0f, 13.3f, 16.2f, 0.4f, -0.2f },
{ 8.0f, 6.0f, 11.6f, 6.0f, 0.4f, -0.5f },
{ 8.0f, 7.0f, -16.3f, -9.2f, -0.1f, 0.5f },
{ 8.0f, 8.0f, -2.1f, 2.4f, 0.4f, 0.1f },
{ 9.0f, 0.0f, 5.5f, 0.0f, -0.1f, 0.0f },
{ 9.0f, 1.0f, 8.8f, -21.8f, -0.1f, -0.3f },
{ 9.0f, 2.0f, 3.0f, 10.7f, -0.0f, 0.1f },
{ 9.0f, 3.0f, -3.2f, 11.8f, 0.4f, -0.4f },
{ 9.0f, 4.0f, 0.6f, -6.8f, -0.4f, 0.3f },
{ 9.0f, 5.0f, -13.2f, -6.9f, 0.0f, 0.1f },
{ 9.0f, 6.0f, -0.1f, 7.9f, 0.3f, -0.0f },
{ 9.0f, 7.0f, 8.7f, 1.0f, 0.0f, -0.1f },
{ 9.0f, 8.0f, -9.1f, -3.9f, -0.0f, 0.5f },
{ 9.0f, 9.0f, -10.4f, 8.5f, -0.3f, 0.2f },
{ 10.0f, 0.0f, -2.0f, 0.0f, 0.0f, 0.0f },
{ 10.0f, 1.0f, -6.1f, 3.3f, -0.0f, 0.0f },
{ 10.0f, 2.0f, 0.2f, -0.4f, -0.1f, 0.1f },
{ 10.0f, 3.0f, 0.6f, 4.6f, 0.2f, -0.2f },
{ 10.0f, 4.0f, -0.5f, 4.4f, -0.1f, 0.1f },
{ 10.0f, 5.0f, 1.8f, -7.9f, -0.2f, -0.1f },
{ 10.0f, 6.0f, -0.7f, -0.6f, -0.0f, 0.1f },
{ 10.0f, 7.0f, 2.2f, -4.2f, -0.1f, -0.0f },
{ 10.0f, 8.0f, 2.4f, -2.9f, -0.2f, -0.1f },
{ 10.0f, 9.0f, -1.8f, -1.1f, -0.1f, 0.2f },
{ 10.0f, 10.0f, -3.6f, -8.8f, -0.0f, -0.0f },
{ 11.0f, 0.0f, 3.0f, 0.0f, -0.0f, 0.0f },
{ 11.0f, 1.0f, -1.4f, -0.0f, 0.0f, 0.0f },
{ 11.0f, 2.0f, -2.3f, 2.1f, -0.0f, 0.1f },
{ 11.0f, 3.0f, 2.1f, -0.6f, 0.0f, 0.0f },
{ 11.0f, 4.0f, -0.8f, -1.1f, -0.0f, 0.1f },
{ 11.0f, 5.0f, 0.6f, 0.7f, -0.1f, -0.0f },
{ 11.0f, 6.0f, -0.7f, -0.2f, 0.0f, -0.0f },
{ 11.0f, 7.0f, 0.1f, -2.1f, -0.0f, 0.1f },
{ 11.0f, 8.0f, 1.7f, -1.5f, -0.0f, -0.0f },
{ 11.0f, 9.0f, -0.2f, -2.6f, -0.1f, -0.1f },
{ 11.0f, 10.0f, 0.4f, -2.0f, -0.0f, -0.0f },
{ 11.0f, 11.0f, 3.5f, -2.3f, -0.1f, -0.1f },
{ 12.0f, 0.0f, -2.0f, 0.0f, 0.1f, 0.0f },
{ 12.0f, 1.0f, -0.3f, -1.0f, 0.0f, 0.0f },
{ 12.0f, 2.0f, 0.4f, 0.5f, 0.0f, 0.0f },
{ 12.0f, 3.0f, 1.3f, 1.8f, 0.1f, -0.1f },
{ 12.0f, 4.0f, -0.9f, -2.2f, -0.1f, 0.0f },
{ 12.0f, 5.0f, 0.9f, 0.3f, 0.0f, 0.0f },
{ 12.0f, 6.0f, 0.1f, 0.7f, 0.1f, 0.0f },
{ 12.0f, 7.0f, 0.5f, -0.1f, 0.0f, 0.0f },
{ 12.0f, 0.0f, -2.0f, 0.0f, 0.0f, 0.0f },
{ 12.0f, 1.0f, -0.1f, -1.0f, 0.0f, -0.0f },
{ 12.0f, 2.0f, 0.5f, 0.3f, -0.0f, 0.0f },
{ 12.0f, 3.0f, 1.2f, 1.8f, 0.0f, -0.1f },
{ 12.0f, 4.0f, -0.9f, -2.2f, -0.1f, 0.1f },
{ 12.0f, 5.0f, 0.9f, 0.3f, -0.0f, -0.0f },
{ 12.0f, 6.0f, 0.1f, 0.7f, 0.0f, 0.0f },
{ 12.0f, 7.0f, 0.6f, -0.1f, -0.0f, -0.0f },
{ 12.0f, 8.0f, -0.4f, 0.3f, 0.0f, 0.0f },
{ 12.0f, 9.0f, -0.4f, 0.2f, 0.0f, 0.0f },
{ 12.0f, 10.0f, 0.2f, -0.9f, 0.0f, 0.0f },
{ 12.0f, 11.0f, -0.9f, -0.2f, 0.0f, 0.0f },
{ 12.0f, 12.0f, 0.0f, 0.7f, 0.0f, 0.0f }
{ 12.0f, 9.0f, -0.5f, 0.2f, -0.0f, 0.0f },
{ 12.0f, 10.0f, 0.2f, -0.9f, -0.0f, -0.0f },
{ 12.0f, 11.0f, -0.9f, -0.2f, -0.0f, 0.0f },
{ 12.0f, 12.0f, -0.0f, 0.8f, -0.1f, -0.1f }
};
static WMMtype_Ellipsoid *Ellip = NULL;
@ -194,7 +194,7 @@ int WMM_Initialize()
// Really, Really needs to be read from a file - out of date in 2020 at latest
MagneticModel->EditionDate = 0.0f; /* OP change. Originally 5.7863328170559505e-307, truncates to 0.0f */
MagneticModel->epoch = 2015.0f;
sprintf(MagneticModel->ModelName, "WMM-2015");
sprintf(MagneticModel->ModelName, "WMM-2015v2");
return 0; // OK
}

2
flight/libraries/inc/insgps.h
View File

@ -58,7 +58,7 @@ void INSGPSInit();
void INSStatePrediction(const float gyro_data[3], const float accel_data[3], float dT);
void INSCovariancePrediction(float dT);
void INSCorrection(const float mag_data[3], const float Pos[3], const float Vel[3],
float BaroAlt, uint16_t SensorsUsed);
const float BaroAlt, uint16_t SensorsUsed);
void INSResetP(const float PDiag[13]);
void INSGetVariance(float PDiag[13]);
void INSSetState(const float pos[3], const float vel[3], const float q[4], const float gyro_bias[3], const float accel_bias[3]);

22
flight/libraries/insgps13state.c
View File

@ -92,7 +92,6 @@ static struct EKFData {
float H[NUMV][NUMX];
// local magnetic unit vector in NED frame
float Be[3];
float BeScaleFactor;
// covariance matrix and state vector
float P[NUMX][NUMX];
float X[NUMX];
@ -281,26 +280,25 @@ void INSSetGyroBiasVar(const float gyro_bias_var[3])
ekf.Q[8] = gyro_bias_var[2];
}
// must be called AFTER SetMagNorth
void INSSetMagVar(const float mag_var[3])
void INSSetMagVar(const float scaled_mag_var[3])
{
ekf.R[6] = mag_var[0] * ekf.BeScaleFactor;
ekf.R[7] = mag_var[1] * ekf.BeScaleFactor;
ekf.R[8] = mag_var[2] * ekf.BeScaleFactor;
ekf.R[6] = scaled_mag_var[0];
ekf.R[7] = scaled_mag_var[1];
ekf.R[8] = scaled_mag_var[2];
}
void INSSetBaroVar(float baro_var)
void INSSetBaroVar(const float baro_var)
{
ekf.R[9] = baro_var;
}
void INSSetMagNorth(const float B[3])
{
ekf.BeScaleFactor = invsqrtf(B[0] * B[0] + B[1] * B[1] + B[2] * B[2]);
float invmag = invsqrtf(B[0] * B[0] + B[1] * B[1] + B[2] * B[2]);
ekf.Be[0] = B[0] * ekf.BeScaleFactor;
ekf.Be[1] = B[1] * ekf.BeScaleFactor;
ekf.Be[2] = B[2] * ekf.BeScaleFactor;
ekf.Be[0] = B[0] * invmag;
ekf.Be[1] = B[1] * invmag;
ekf.Be[2] = B[2] * invmag;
}
void INSStatePrediction(const float gyro_data[3], const float accel_data[3], float dT)
@ -402,8 +400,6 @@ void INSCorrection(const float mag_data[3], const float Pos[3], const float Vel[
Z[3] = Vel[0];
Z[4] = Vel[1];
Z[5] = Vel[2];
// magnetometer data in any units (use unit vector) and in body frame
if (SensorsUsed & MAG_SENSORS) {
// magnetometer data in any units (use unit vector) and in body frame

12
flight/libraries/math/pid.c
View File

@ -219,7 +219,17 @@ float pid2_apply(
// pid->I = (pid->u0 - pid->va) / pid->vb - pid->kp * (pid->beta * r - y);
// this is dangerous, if pid->I starts nonzero with very low or zero kI, then
// it will never settle!
pid->I = 0.0f;
// pid->I = 0.0f;
// whether ki is zero or non-zero, when doing 'reconfigure' we start with setpoint==actual
pid->I = (pid->u0 - pid->va) / pid->vb;
// if ki is non-zero
if (pid->bi > 5e-7f) {
// then the output can wind up/down to get rid of a difference between setpoint and actual
// so we can generate the full bumpless transfer here
// else leave off the part that can vary due to difference between setpoint and actual
// so that it is repeatable (for tuning purposes at least)
pid->I -= pid->kp * (pid->beta * r - y);
}
}
// compute proportional part

3
flight/libraries/sanitycheck.c
View File

@ -77,6 +77,9 @@ int32_t configuration_check()
RevoSettingsFusionAlgorithmGet(&revoFusion);
bool navCapableFusion;
switch (revoFusion) {
case REVOSETTINGS_FUSIONALGORITHM_COMPLEMENTARYGPSOUTDOOR:
navCapableFusion = (GetCurrentFrameType() == FRAME_TYPE_FIXED_WING) ? true : false;
break;
case REVOSETTINGS_FUSIONALGORITHM_COMPLEMENTARYMAGGPSOUTDOOR:
case REVOSETTINGS_FUSIONALGORITHM_GPSNAVIGATIONINS13:
case REVOSETTINGS_FUSIONALGORITHM_GPSNAVIGATIONINS13CF:

2
flight/modules/AutoTune/autotune.c
View File

@ -835,7 +835,7 @@ static void UpdateStabilizationDesired(bool doingIdent)
} else {
stabDesired.StabilizationMode.Roll = STABILIZATIONDESIRED_STABILIZATIONMODE_ATTITUDE;
stabDesired.StabilizationMode.Pitch = STABILIZATIONDESIRED_STABILIZATIONMODE_ATTITUDE;
stabDesired.StabilizationMode.Yaw = STABILIZATIONDESIRED_STABILIZATIONMODE_RATE;
stabDesired.StabilizationMode.Yaw = STABILIZATIONDESIRED_STABILIZATIONMODE_AXISLOCK;
}
if (systemIdentSettings.ThrustControl == SYSTEMIDENTSETTINGS_THRUSTCONTROL_ALTITUDEVARIO) {

41
flight/modules/FirmwareIAP/firmwareiap.c
View File

@ -2,7 +2,8 @@
******************************************************************************
*
* @file firmwareiap.c
* @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2010.
* @author The LibrePilot Project, http://www.librepilot.org Copyright (C) 2018.
* The OpenPilot Team, http://www.openpilot.org Copyright (C) 2010.
* @brief In Application Programming module to support firmware upgrades by
* providing a means to enter the bootloader.
*
@ -47,7 +48,7 @@
#define IAP_STATE_STEP_2 2
#define IAP_STATE_RESETTING 3
#define RESET_DELAY 500 /* delay between sending reset ot INS */
#define RESET_DELAY 500
#define TICKS2MS(t) ((t) / portTICK_RATE_MS)
#define MS2TICKS(m) ((m) * portTICK_RATE_MS)
@ -96,13 +97,15 @@ int32_t FirmwareIAPInitialize()
FirmwareIAPObjData data;
FirmwareIAPObjGet(&data);
data.BoardType = bdinfo->board_type;
PIOS_BL_HELPER_FLASH_Read_Description(data.Description, FIRMWAREIAPOBJ_DESCRIPTION_NUMELEM);
PIOS_SYS_SerialNumberGetBinary(data.CPUSerial);
data.BoardRevision = bdinfo->board_rev;
if (data.BoardType == 0) {
data.BoardRevision = bdinfo->board_rev;
data.BoardType = bdinfo->board_type;
}
data.BootloaderRevision = bdinfo->bl_rev;
data.ArmReset = 0;
data.crc = 0;
data.crc = PIOS_BL_HELPER_CRC_Memory_Calc();
FirmwareIAPObjSet(&data);
if (bdinfo->magic == PIOS_BOARD_INFO_BLOB_MAGIC) {
FirmwareIAPObjConnectCallback(&FirmwareIAPCallback);
@ -126,7 +129,6 @@ int32_t FirmwareIAPStart()
static uint8_t iap_state = IAP_STATE_READY;
static void FirmwareIAPCallback(UAVObjEvent *ev)
{
const struct pios_board_info *bdinfo = &pios_board_info_blob;
static uint32_t last_time = 0;
uint32_t this_time;
uint32_t delta;
@ -136,7 +138,6 @@ static void FirmwareIAPCallback(UAVObjEvent *ev)
}
FirmwareIAPObjData data;
FirmwareIAPObjGet(&data);
if (ev->obj == FirmwareIAPObjHandle()) {
// Get the input object data
@ -144,18 +145,6 @@ static void FirmwareIAPCallback(UAVObjEvent *ev)
this_time = get_time();
delta = this_time - last_time;
last_time = this_time;
if ((data.BoardType == bdinfo->board_type) && (data.crc != PIOS_BL_HELPER_CRC_Memory_Calc())) {
PIOS_BL_HELPER_FLASH_Read_Description(data.Description, FIRMWAREIAPOBJ_DESCRIPTION_NUMELEM);
PIOS_SYS_SerialNumberGetBinary(data.CPUSerial);
data.BoardRevision = bdinfo->board_rev;
data.BootloaderRevision = bdinfo->bl_rev;
data.crc = PIOS_BL_HELPER_CRC_Memory_Calc();
FirmwareIAPObjSet(&data);
}
if ((data.ArmReset == 1) && (iap_state != IAP_STATE_RESETTING)) {
data.ArmReset = 0;
FirmwareIAPObjSet(&data);
}
switch (iap_state) {
case IAP_STATE_READY:
if (data.Command == IAP_CMD_STEP_1) {
@ -238,7 +227,7 @@ static uint32_t get_time(void)
}
/**
* Executed by event dispatcher callback to reset INS before resetting OP
* Executed by event dispatcher callback to reset Board
*/
static void resetTask(__attribute__((unused)) UAVObjEvent *ev)
{
@ -250,18 +239,8 @@ static void resetTask(__attribute__((unused)) UAVObjEvent *ev)
PIOS_LED_Toggle(PIOS_LED_ALARM);
#endif /* PIOS_LED_ALARM */
FirmwareIAPObjData data;
FirmwareIAPObjGet(&data);
if ((portTickType)(xTaskGetTickCount() - lastResetSysTime) > RESET_DELAY / portTICK_RATE_MS) {
lastResetSysTime = xTaskGetTickCount();
data.BoardType = 0xFF;
data.ArmReset = 1;
data.crc = reset_count; /* Must change a value for this to get to INS */
FirmwareIAPObjSet(&data);
++reset_count;
if (reset_count > 3) {
PIOS_SYS_Reset();
}
PIOS_SYS_Reset();
}
}

5
flight/modules/FirmwareIAP/inc/firmwareiap.h
View File

@ -1,9 +1,10 @@
/**
******************************************************************************
*
* @file firmwareiap.c
* @file firmwareiap.h
* @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2010.
* @brief Example module to be used as a template for actual modules.
* @brief In Application Programming module to support firmware upgrades by
* providing a means to enter the bootloader.
*
* @see The GNU Public License (GPL) Version 3
*

4
flight/modules/GPS/ubx_autoconfig.c
View File

@ -243,7 +243,9 @@ void gps_ubx_reset_sensor_type()
ubxSensorType = GPSPOSITIONSENSOR_SENSORTYPE_UNKNOWN;
GPSPositionSensorSensorTypeSet(&ubxSensorType);
// make the sensor type / autobaud code time out immediately to send the request immediately
status->lastStepTimestampRaw += 0x8000000UL;
if (status) {
status->lastStepTimestampRaw += 0x8000000UL;
}
}
--mutex;
}

10
flight/modules/PathFollower/inc/vtolautotakeofffsm.h
View File

@ -3,13 +3,13 @@
* @addtogroup LibrePilotModules LibrePilot Modules
* @{
* @addtogroup PathFollower FSM
* @brief Executes landing sequence via an FSM
* @brief Executes auto takeoff sequence via an FSM
* @{
*
* @file vtollandfsm.h
* @author The LibrePilot Project, http://www.librepilot.org Copyright (C) 2016.
* @file vtolautotakeofffsm.h
* @author The LibrePilot Project, http://www.librepilot.org Copyright (C) 2016-2018
* The OpenPilot Team, http://www.openpilot.org Copyright (C) 2015.
* @brief Executes FSM for landing sequence
* @brief Executes FSM for auto takeoff sequence
*
* @see The GNU Public License (GPL) Version 3
*
@ -144,7 +144,7 @@ protected:
int32_t runAlways();
void updateVtolAutoTakeoffFSMStatus();
void assessAltitude(void);
float assessAltitude(void);
void setStateTimeout(int32_t count);

5
flight/modules/PathFollower/inc/vtollandfsm.h
View File

@ -7,7 +7,8 @@
* @{
*
* @file vtollandfsm.h
* @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2015.
* @author The LibrePilot Project, http://www.librepilot.org Copyright (C) 2018
* The OpenPilot Team, http://www.openpilot.org Copyright (C) 2015.
* @brief Executes FSM for landing sequence
*
* @see The GNU Public License (GPL) Version 3
@ -133,7 +134,7 @@ protected:
int32_t runState();
int32_t runAlways();
void updateVtolLandFSMStatus();
void assessAltitude(void);
float assessAltitude(void);
void setStateTimeout(int32_t count);

2
flight/modules/PathFollower/pathfollower.cpp
View File

@ -477,7 +477,7 @@ static void updateFixedAttitude(float *attitude)
stabDesired.Thrust = attitude[3];
stabDesired.StabilizationMode.Roll = STABILIZATIONDESIRED_STABILIZATIONMODE_ATTITUDE;
stabDesired.StabilizationMode.Pitch = STABILIZATIONDESIRED_STABILIZATIONMODE_ATTITUDE;
stabDesired.StabilizationMode.Yaw = STABILIZATIONDESIRED_STABILIZATIONMODE_RATE;
stabDesired.StabilizationMode.Yaw = STABILIZATIONDESIRED_STABILIZATIONMODE_AXISLOCK;
stabDesired.StabilizationMode.Thrust = STABILIZATIONDESIRED_STABILIZATIONMODE_MANUAL;
StabilizationDesiredSet(&stabDesired);
}

18
flight/modules/PathFollower/vtolautotakeoffcontroller.cpp
View File

@ -1,10 +1,10 @@
/*
******************************************************************************
*
* @file vtollandcontroller.cpp
* @author The LibrePilot Project, http://www.librepilot.org Copyright (C) 2016.
* @file vtolautotakeoffcontroller.cpp
* @author The LibrePilot Project, http://www.librepilot.org Copyright (C) 2016-2018
* The OpenPilot Team, http://www.openpilot.org Copyright (C) 2015.
* @brief Vtol landing controller loop
* @brief Vtol auto takeoff controller loop
* @see The GNU Public License (GPL) Version 3
* @addtogroup LibrePilot LibrePilotModules Modules PathFollower Navigation
*
@ -132,11 +132,11 @@ void VtolAutoTakeoffController::ObjectiveUpdated(void)
if (mOverride) {
// override pathDesired from PathPlanner with current position,
// as we deliberately don't care about the location of the waypoints on the map
float velocity_down;
float autotakeoff_velocity;
float autotakeoff_height;
PositionStateData positionState;
PositionStateGet(&positionState);
FlightModeSettingsAutoTakeOffVelocityGet(&velocity_down);
FlightModeSettingsAutoTakeOffVelocityGet(&autotakeoff_velocity);
FlightModeSettingsAutoTakeOffHeightGet(&autotakeoff_height);
autotakeoff_height = fabsf(autotakeoff_height);
if (autotakeoff_height < AUTOTAKEOFF_TO_INCREMENTAL_HEIGHT_MIN) {
@ -144,7 +144,7 @@ void VtolAutoTakeoffController::ObjectiveUpdated(void)
} else if (autotakeoff_height > AUTOTAKEOFF_TO_INCREMENTAL_HEIGHT_MAX) {
autotakeoff_height = AUTOTAKEOFF_TO_INCREMENTAL_HEIGHT_MAX;
}
controlDown.UpdateVelocitySetpoint(velocity_down);
controlDown.UpdateVelocitySetpoint(-autotakeoff_velocity);
controlNE.UpdateVelocitySetpoint(0.0f, 0.0f);
controlNE.UpdatePositionSetpoint(positionState.North, positionState.East);
@ -320,9 +320,9 @@ void VtolAutoTakeoffController::UpdateAutoPilot()
// 1. Arming must be done whilst in the AutoTakeOff flight mode
// 2. If the AutoTakeoff flight mode is selected and already armed, requires disarming first
// 3. Wait for armed state
// 4. Once the user increases the throttle position to above 50%, then and only then initiate auto-takeoff.
// 5. Whilst the throttle is < 50% before takeoff, all stick inputs are being ignored.
// 6. If during the autotakeoff sequence, at any stage, if the throttle stick position reduces to less than 10%, landing is initiated.
// 4. Once the user increases the throttle position to above 30%, then and only then initiate auto-takeoff.
// 5. Whilst the throttle is < 30% before takeoff, all stick inputs are being ignored.
// 6. If during the autotakeoff sequence, at any stage, the throttle stick position reduces to less than 10%, landing is initiated.
switch (autotakeoffState) {
case STATUSVTOLAUTOTAKEOFF_CONTROLSTATE_REQUIREUNARMEDFIRST:

39
flight/modules/PathFollower/vtolautotakeofffsm.cpp
View File

@ -2,7 +2,8 @@
******************************************************************************
*
* @file vtolautotakeofffsm.cpp
* @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2015.
* @author The LibrePilot Project, http://www.librepilot.org Copyright (C) 2018
* The OpenPilot Team, http://www.openpilot.org Copyright (C) 2015.
* @brief This autotakeoff state machine is a helper state machine to the
* VtolAutoTakeoffController.
* @see The GNU Public License (GPL) Version 3
@ -60,11 +61,11 @@ extern "C" {
// Private constants
#define TIMER_COUNT_PER_SECOND (1000 / vtolPathFollowerSettings->UpdatePeriod)
#define TIMEOUT_SLOWSTART (2 * TIMER_COUNT_PER_SECOND)
#define TIMEOUT_THRUSTUP (1 * TIMER_COUNT_PER_SECOND)
#define TIMEOUT_THRUSTDOWN (5 * TIMER_COUNT_PER_SECOND)
#define AUTOTAKEOFFING_SLOWDOWN_HEIGHT -5.0f
#define TIMER_COUNT_PER_SECOND (1000 / vtolPathFollowerSettings->UpdatePeriod)
#define TIMEOUT_SLOWSTART (2 * TIMER_COUNT_PER_SECOND)
#define TIMEOUT_THRUSTUP (1 * TIMER_COUNT_PER_SECOND)
#define TIMEOUT_THRUSTDOWN (5 * TIMER_COUNT_PER_SECOND)
#define AUTOTAKEOFF_SLOWDOWN_HEIGHT -5.0f
VtolAutoTakeoffFSM::PathFollowerFSM_AutoTakeoffStateHandler_T VtolAutoTakeoffFSM::sAutoTakeoffStateTable[AUTOTAKEOFF_STATE_SIZE] = {
[AUTOTAKEOFF_STATE_INACTIVE] = { .setup = &VtolAutoTakeoffFSM::setup_inactive, .run = 0 },
@ -246,14 +247,8 @@ void VtolAutoTakeoffFSM::setState(StatusVtolAutoTakeoffStateOptions newState, St
mAutoTakeoffData->currentState = newState;
if (newState != STATUSVTOLAUTOTAKEOFF_STATE_INACTIVE) {
PositionStateData positionState;
PositionStateGet(&positionState);
float takeOffDown = 0.0f;
if (mAutoTakeoffData->takeOffLocation.Status == TAKEOFFLOCATION_STATUS_VALID) {
takeOffDown = mAutoTakeoffData->takeOffLocation.Down;
}
mAutoTakeoffData->fsmAutoTakeoffStatus.AltitudeAtState[newState] = positionState.Down - takeOffDown;
assessAltitude();
float altitudeAboveTakeoff = assessAltitude();
mAutoTakeoffData->fsmAutoTakeoffStatus.AltitudeAtState[newState] = altitudeAboveTakeoff;
}
// Restart state timer counter
@ -266,6 +261,7 @@ void VtolAutoTakeoffFSM::setState(StatusVtolAutoTakeoffStateOptions newState, St
(this->*sAutoTakeoffStateTable[mAutoTakeoffData->currentState].setup)();
}
// Update StatusVtolAutoTakeoff UAVObject with new status
updateVtolAutoTakeoffFSMStatus();
}
@ -276,6 +272,7 @@ void VtolAutoTakeoffFSM::setStateTimeout(int32_t count)
mAutoTakeoffData->stateTimeoutCount = count;
}
// Update StatusVtolAutoTakeoff UAVObject
void VtolAutoTakeoffFSM::updateVtolAutoTakeoffFSMStatus()
{
mAutoTakeoffData->fsmAutoTakeoffStatus.State = mAutoTakeoffData->currentState;
@ -288,7 +285,7 @@ void VtolAutoTakeoffFSM::updateVtolAutoTakeoffFSMStatus()
}
void VtolAutoTakeoffFSM::assessAltitude(void)
float VtolAutoTakeoffFSM::assessAltitude(void)
{
float positionDown;
@ -298,11 +295,13 @@ void VtolAutoTakeoffFSM::assessAltitude(void)
takeOffDown = mAutoTakeoffData->takeOffLocation.Down;
}
float positionDownRelativeToTakeoff = positionDown - takeOffDown;
if (positionDownRelativeToTakeoff < AUTOTAKEOFFING_SLOWDOWN_HEIGHT) {
if (positionDownRelativeToTakeoff < AUTOTAKEOFF_SLOWDOWN_HEIGHT) {
mAutoTakeoffData->flLowAltitude = false;
} else {
mAutoTakeoffData->flLowAltitude = true;
}
// Return the altitude above takeoff, which is the negation of positionDownRelativeToTakeoff
return -positionDownRelativeToTakeoff;
}
// Action the required state from plans.c
@ -347,10 +346,10 @@ void VtolAutoTakeoffFSM::setup_checkstate(void)
// Assumptions that do not need to be checked if flight mode is AUTOTAKEOFF
// 1. Already armed
// 2. Not in flight. This was checked in plans.c
// 3. User has placed throttle position to more than 50% to allow autotakeoff
// 3. User has placed throttle position to more than 30% to allow autotakeoff
// If pathplanner, we need additional checks
// E.g. if inflight, this mode is just positon hol
// E.g. if inflight, this mode is just position hold
StabilizationDesiredData stabDesired;
StabilizationDesiredGet(&stabDesired);
@ -407,15 +406,13 @@ void VtolAutoTakeoffFSM::run_slowstart(__attribute__((unused)) uint8_t flTimeout
}
}
// STATE: THRUSTUP spools up motors to vtol min over 5 seconds for effect.
// STATE: THRUSTUP spools up motors to vtol min over 1 second for effect.
// PID loops may be cumulating I terms but that problem needs to be solved
#define THRUSTUP_FINAL_THRUST_AS_RATIO_OF_VTOLMAX 0.8f
void VtolAutoTakeoffFSM::setup_thrustup(void)
{
setStateTimeout(TIMEOUT_THRUSTUP);
mAutoTakeoffData->flZeroStabiHorizontal = false;
StabilizationDesiredData stabDesired;
StabilizationDesiredGet(&stabDesired);
mAutoTakeoffData->sum2 = THRUSTUP_FINAL_THRUST_AS_RATIO_OF_VTOLMAX * vtolPathFollowerSettings->ThrustLimits.Max;
mAutoTakeoffData->sum1 = (mAutoTakeoffData->sum2 - mAutoTakeoffData->boundThrustMax) / (float)TIMEOUT_THRUSTUP;
mAutoTakeoffData->boundThrustMin = vtolPathFollowerSettings->ThrustLimits.Min;

2
flight/modules/PathFollower/vtolbrakecontroller.cpp
View File

@ -289,7 +289,7 @@ int8_t VtolBrakeController::UpdateStabilizationDesired(void)
ManualControlCommandData manualControl;
ManualControlCommandGet(&manualControl);
stabDesired.StabilizationMode.Yaw = STABILIZATIONDESIRED_STABILIZATIONMODE_RATE;
stabDesired.StabilizationMode.Yaw = STABILIZATIONDESIRED_STABILIZATIONMODE_AXISLOCK;
stabDesired.Yaw = stabSettings.MaximumRate.Yaw * manualControl.Yaw;
// default thrust mode to cruise control

6
flight/modules/PathFollower/vtolflycontroller.cpp
View File

@ -274,7 +274,9 @@ int8_t VtolFlyController::UpdateStabilizationDesired(bool yaw_attitude, float ya
}
#endif // if 0
if (yaw_attitude) {
// Yaw Attitude will be disabled without velocity requested.
// PositionHold, AutoTakeoff or AutoCruise still using manual Yaw.
if (yaw_attitude && ((fabsf(pathDesired->StartingVelocity) > 0.0f) && (fabsf(pathDesired->EndingVelocity) > 0.0f))) {
stabDesired.StabilizationMode.Yaw = STABILIZATIONDESIRED_STABILIZATIONMODE_ATTITUDE;
stabDesired.Yaw = yaw_direction;
} else {
@ -341,7 +343,7 @@ void VtolFlyController::UpdateDesiredAttitudeEmergencyFallback()
stabDesired.StabilizationMode.Roll = STABILIZATIONDESIRED_STABILIZATIONMODE_ATTITUDE;
stabDesired.StabilizationMode.Pitch = STABILIZATIONDESIRED_STABILIZATIONMODE_ATTITUDE;
stabDesired.StabilizationMode.Yaw = STABILIZATIONDESIRED_STABILIZATIONMODE_RATE;
stabDesired.StabilizationMode.Yaw = STABILIZATIONDESIRED_STABILIZATIONMODE_AXISLOCK;
stabDesired.StabilizationMode.Thrust = STABILIZATIONDESIRED_STABILIZATIONMODE_CRUISECONTROL;
StabilizationDesiredSet(&stabDesired);
}

17
flight/modules/PathFollower/vtollandfsm.cpp
View File

@ -2,7 +2,8 @@
******************************************************************************
*
* @file vtollandfsm.cpp
* @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2015.
* @author The LibrePilot Project, http://www.librepilot.org Copyright (C) 2018
* The OpenPilot Team, http://www.openpilot.org Copyright (C) 2015.
* @brief This landing state machine is a helper state machine to the
* VtolLandController.
* @see The GNU Public License (GPL) Version 3
@ -275,14 +276,8 @@ void VtolLandFSM::setState(StatusVtolLandStateOptions newState, StatusVtolLandSt
mLandData->currentState = newState;
if (newState != STATUSVTOLLAND_STATE_INACTIVE) {
PositionStateData positionState;
PositionStateGet(&positionState);
float takeOffDown = 0.0f;
if (mLandData->takeOffLocation.Status == TAKEOFFLOCATION_STATUS_VALID) {
takeOffDown = mLandData->takeOffLocation.Down;
}
mLandData->fsmLandStatus.AltitudeAtState[newState] = positionState.Down - takeOffDown;
assessAltitude();
float altitudeAboveTakeoff = assessAltitude();
mLandData->fsmLandStatus.AltitudeAtState[newState] = altitudeAboveTakeoff;
}
// Restart state timer counter
@ -332,7 +327,7 @@ float VtolLandFSM::BoundVelocityDown(float velocity_down)
}
}
void VtolLandFSM::assessAltitude(void)
float VtolLandFSM::assessAltitude(void)
{
float positionDown;
@ -347,6 +342,8 @@ void VtolLandFSM::assessAltitude(void)
} else {
mLandData->flLowAltitude = true;
}
// Return the altitude above takeoff, which is the negation of positionDownRelativeToTakeoff
return -positionDownRelativeToTakeoff;
}

2
flight/modules/PathFollower/vtolvelocitycontroller.cpp
View File

@ -180,7 +180,7 @@ int8_t VtolVelocityController::UpdateStabilizationDesired(__attribute__((unused)
ManualControlCommandData manualControl;
ManualControlCommandGet(&manualControl);
stabDesired.StabilizationMode.Yaw = STABILIZATIONDESIRED_STABILIZATIONMODE_RATE;
stabDesired.StabilizationMode.Yaw = STABILIZATIONDESIRED_STABILIZATIONMODE_AXISLOCK;
stabDesired.Yaw = stabSettings.MaximumRate.Yaw * manualControl.Yaw;
// default thrust mode to altvario

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

@ -222,7 +222,8 @@ static void pathPlannerTask()
}
// the transition from pathplanner to another flightmode back to pathplanner
// triggers a reset back to 0 index in the waypoint list
// triggers a reset back either to 0 index in the waypoint list,
// or to current index in the waypoint list, depending on FlightModeChangeRestartsPathPlan setting
if (pathplanner_active == false) {
pathplanner_active = true;
@ -230,8 +231,10 @@ static void pathPlannerTask()
FlightModeSettingsFlightModeChangeRestartsPathPlanGet(&restart);
if (restart == FLIGHTMODESETTINGS_FLIGHTMODECHANGERESTARTSPATHPLAN_TRUE) {
setWaypoint(0);
return;
} else {
setWaypoint(waypointActive.Index);
}
return;
}
WaypointInstGet(waypointActive.Index, &waypoint);
@ -601,8 +604,7 @@ static uint8_t conditionBelowError()
/**
* the AboveAltitude measures the flight altitude relative to home position
* returns true if above critical altitude
* WARNING! Altitudes are always negative (down coordinate)
* Parameter 0: altitude in meters (negative!)
* Parameter 0: altitude in meters
*/
static uint8_t conditionAboveAltitude()
{
@ -610,7 +612,7 @@ static uint8_t conditionAboveAltitude()
PositionStateGet(&positionState);
if (positionState.Down <= pathAction.ConditionParameters[0]) {
if (-positionState.Down >= pathAction.ConditionParameters[0]) {
return true;
}
return false;

1
flight/modules/Stabilization/inc/stabilization.h
View File

@ -63,6 +63,7 @@ typedef struct {
struct pid innerPids[3], outerPids[3];
// TPS [Roll,Pitch,Yaw][P,I,D]
bool thrust_pid_scaling_enabled[3][3];
float feedForward_alpha[3];
} StabilizationData;

28
flight/modules/Stabilization/outerloop.c
View File

@ -38,6 +38,7 @@
#include <ratedesired.h>
#include <stabilizationdesired.h>
#include <attitudestate.h>
#include <gyrostate.h>
#include <stabilizationstatus.h>
#include <flightstatus.h>
#include <manualcontrolcommand.h>
@ -63,6 +64,7 @@ static DelayedCallbackInfo *callbackHandle;
static AttitudeStateData attitude;
static uint8_t previous_mode[AXES] = { 255, 255, 255, 255 };
static float gyro_filtered[3] = { 0, 0, 0 };
static PiOSDeltatimeConfig timeval;
static bool pitchMin = false;
static bool pitchMax = false;
@ -71,6 +73,7 @@ static bool rollMax = false;
// Private functions
static void stabilizationOuterloopTask();
static void GyroStateUpdatedCb(__attribute__((unused)) UAVObjEvent *ev);
static void AttitudeStateUpdatedCb(__attribute__((unused)) UAVObjEvent *ev);
void stabilizationOuterloopInit()
@ -78,6 +81,7 @@ void stabilizationOuterloopInit()
RateDesiredInitialize();
StabilizationDesiredInitialize();
AttitudeStateInitialize();
GyroStateInitialize();
StabilizationStatusInitialize();
FlightStatusInitialize();
ManualControlCommandInitialize();
@ -85,6 +89,7 @@ void stabilizationOuterloopInit()
PIOS_DELTATIME_Init(&timeval, UPDATE_EXPECTED, UPDATE_MIN, UPDATE_MAX, UPDATE_ALPHA);
callbackHandle = PIOS_CALLBACKSCHEDULER_Create(&stabilizationOuterloopTask, CALLBACK_PRIORITY, CBTASK_PRIORITY, CALLBACKINFO_RUNNING_STABILIZATION0, STACK_SIZE_BYTES);
GyroStateConnectCallback(GyroStateUpdatedCb);
AttitudeStateConnectCallback(AttitudeStateUpdatedCb);
}
@ -146,8 +151,8 @@ static void stabilizationOuterloopTask()
float local_error[3];
{
#if defined(PIOS_QUATERNION_STABILIZATION)
if (stabSettings.settings.ForceRollPitchDuringYawTransition == STABILIZATIONSETTINGS_FORCEROLLPITCHDURINGYAWTRANSITION_FALSE) {
// Quaternion calculation of error in each axis. Uses more memory.
float rpy_desired[3];
float q_desired[4];
@ -173,8 +178,10 @@ static void stabilizationOuterloopTask()
quat_mult(q_desired, &attitudeState.q1, q_error);
quat_inverse(q_error);
Quaternion2RPY(q_error, local_error);
} else {
#else /* if defined(PIOS_QUATERNION_STABILIZATION) */
{
#endif /* if defined(PIOS_QUATERNION_STABILIZATION) */
// Simpler algorithm for CC, less memory
local_error[0] = stabilizationDesiredAxis[0] - attitudeState.Roll;
local_error[1] = stabilizationDesiredAxis[1] - attitudeState.Pitch;
@ -187,9 +194,12 @@ static void stabilizationOuterloopTask()
} else {
local_error[2] = modulo - 180.0f;
}
#endif /* if defined(PIOS_QUATERNION_STABILIZATION) */
}
// Feed forward: Assume things always get worse before they get better
local_error[0] = local_error[0] - (gyro_filtered[0] * stabSettings.stabBank.AttitudeFeedForward.Roll);
local_error[1] = local_error[1] - (gyro_filtered[1] * stabSettings.stabBank.AttitudeFeedForward.Pitch);
local_error[2] = local_error[2] - (gyro_filtered[2] * stabSettings.stabBank.AttitudeFeedForward.Yaw);
for (t = STABILIZATIONSTATUS_OUTERLOOP_ROLL; t < STABILIZATIONSTATUS_OUTERLOOP_THRUST; t++) {
reinit = (StabilizationStatusOuterLoopToArray(enabled)[t] != previous_mode[t]);
@ -380,6 +390,18 @@ static void AttitudeStateUpdatedCb(__attribute__((unused)) UAVObjEvent *ev)
#endif
}
static void GyroStateUpdatedCb(__attribute__((unused)) UAVObjEvent *ev)
{
GyroStateData gyroState;
GyroStateGet(&gyroState);
gyro_filtered[0] = gyro_filtered[0] * stabSettings.feedForward_alpha[0] + gyroState.x * (1 - stabSettings.feedForward_alpha[0]);
gyro_filtered[1] = gyro_filtered[1] * stabSettings.feedForward_alpha[1] + gyroState.y * (1 - stabSettings.feedForward_alpha[1]);
gyro_filtered[2] = gyro_filtered[2] * stabSettings.feedForward_alpha[2] + gyroState.z * (1 - stabSettings.feedForward_alpha[2]);
}
/**
* @}
* @}

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

@ -364,6 +364,21 @@ static void BankUpdatedCb(__attribute__((unused)) UAVObjEvent *ev)
stabSettings.acroInsanityFactors[0] = (float)(stabSettings.stabBank.AcroInsanityFactor.Roll) * 0.01f;
stabSettings.acroInsanityFactors[1] = (float)(stabSettings.stabBank.AcroInsanityFactor.Pitch) * 0.01f;
stabSettings.acroInsanityFactors[2] = (float)(stabSettings.stabBank.AcroInsanityFactor.Yaw) * 0.01f;
// The dT has some jitter iteration to iteration that we don't want to
// make thie result unpredictable. Still, it's nicer to specify the constant
// based on a time (in ms) rather than a fixed multiplier. The error between
// update rates on OP (~300 Hz) and CC (~475 Hz) is negligible for this
// calculation
const float fakeDt = 0.0025f;
for (int t = 0; t < STABILIZATIONBANK_ATTITUDEFEEDFORWARD_NUMELEM; t++) {
float tau = StabilizationBankAttitudeFeedForwardToArray(stabSettings.stabBank.AttitudeFeedForward)[t] * 0.1f;
if (tau < 0.0001f) {
stabSettings.feedForward_alpha[t] = 0.0f; // not trusting this to resolve to 0
} else {
stabSettings.feedForward_alpha[t] = expf(-fakeDt / tau);
}
}
}

68
flight/modules/StateEstimation/filtercf.c
View File

@ -64,13 +64,16 @@ struct data {
HomeLocationData homeLocation;
bool first_run;
bool useMag;
bool useVelHeading;
float currentAccel[3];
float currentMag[3];
float currentVel[3];
float accels_filtered[3];
float grot_filtered[3];
float gyroBias[3];
bool accelUpdated;
bool magUpdated;
bool velUpdated;
float accel_alpha;
bool accel_filter_enabled;
float rollPitchBiasRate;
@ -90,9 +93,10 @@ static FlightStatusData flightStatus;
static int32_t initwithmag(stateFilter *self);
static int32_t initwithoutmag(stateFilter *self);
static int32_t initwithvelheading(stateFilter *self);
static int32_t maininit(stateFilter *self);
static filterResult filter(stateFilter *self, stateEstimation *state);
static filterResult complementaryFilter(struct data *this, float gyro[3], float accel[3], float mag[3], float attitude[4]);
static filterResult complementaryFilter(struct data *this, float gyro[3], float accel[3], float mag[3], float velheading[2], float attitude[4]);
static void flightStatusUpdatedCb(UAVObjEvent *ev);
@ -118,6 +122,15 @@ int32_t filterCFInitialize(stateFilter *handle)
return STACK_REQUIRED;
}
int32_t filterCFHInitialize(stateFilter *handle)
{
globalInit();
handle->init = &initwithvelheading;
handle->filter = &filter;
handle->localdata = pios_malloc(sizeof(struct data));
return STACK_REQUIRED;
}
int32_t filterCFMInitialize(stateFilter *handle)
{
globalInit();
@ -132,6 +145,16 @@ static int32_t initwithmag(stateFilter *self)
struct data *this = (struct data *)self->localdata;
this->useMag = 1;
this->useVelHeading = 0;
return maininit(self);
}
static int32_t initwithvelheading(stateFilter *self)
{
struct data *this = (struct data *)self->localdata;
this->useMag = 0;
this->useVelHeading = 1;
return maininit(self);
}
@ -140,6 +163,7 @@ static int32_t initwithoutmag(stateFilter *self)
struct data *this = (struct data *)self->localdata;
this->useMag = 0;
this->useVelHeading = 0;
return maininit(self);
}
@ -184,6 +208,12 @@ static filterResult filter(stateFilter *self, stateEstimation *state)
this->currentMag[1] = state->mag[1];
this->currentMag[2] = state->mag[2];
}
if (IS_SET(state->updated, SENSORUPDATES_vel)) {
this->velUpdated = 1;
this->currentVel[0] = state->vel[0];
this->currentVel[1] = state->vel[1];
this->currentVel[2] = state->vel[2];
}
if (IS_SET(state->updated, SENSORUPDATES_accel)) {
this->accelUpdated = 1;
this->currentAccel[0] = state->accel[0];
@ -193,7 +223,7 @@ static filterResult filter(stateFilter *self, stateEstimation *state)
if (IS_SET(state->updated, SENSORUPDATES_gyro)) {
if (this->accelUpdated) {
float attitude[4];
result = complementaryFilter(this, state->gyro, this->currentAccel, this->currentMag, attitude);
result = complementaryFilter(this, state->gyro, this->currentAccel, this->currentMag, this->currentVel, attitude);
if (result == FILTERRESULT_OK) {
state->attitude[0] = attitude[0];
state->attitude[1] = attitude[1];
@ -203,6 +233,7 @@ static filterResult filter(stateFilter *self, stateEstimation *state)
}
this->accelUpdated = 0;
this->magUpdated = 0;
this->velUpdated = 0;
}
}
return result;
@ -222,7 +253,7 @@ static inline void apply_accel_filter(const struct data *this, const float *raw,
}
}
static filterResult complementaryFilter(struct data *this, float gyro[3], float accel[3], float mag[3], float attitude[4])
static filterResult complementaryFilter(struct data *this, float gyro[3], float accel[3], float mag[3], float vel[3], float attitude[4])
{
float dT;
@ -390,7 +421,7 @@ static filterResult complementaryFilter(struct data *this, float gyro[3], float
accel_err[2] /= (accel_mag * grot_mag);
float mag_err[3] = { 0.0f };
if (this->magUpdated && this->useMag) {
if (this->magUpdated && this->useMag && !this->useVelHeading) {
// Rotate gravity to body frame and cross with accels
float brot[3];
float Rbe[3][3];
@ -415,6 +446,31 @@ static filterResult complementaryFilter(struct data *this, float gyro[3], float
} else {
CrossProduct((const float *)mag, (const float *)brot, mag_err);
}
} else if (this->velUpdated && this->useVelHeading) {
float speed = sqrtf(vel[0] * vel[0] + vel[1] * vel[1]);
if (speed > 3.0f) { // ~11kmh
float rpy[3];
Quaternion2RPY(attitude, rpy);
float heading = RAD2DEG(atan2f(-vel[1], -vel[0])) + 180.0f;
// find shortest way
float modulo = fmodf((heading - rpy[2]) + 180.0f, 360.0f);
if (modulo < 0) {
modulo += 180.0f;
} else {
modulo -= 180.0f;
}
/* From dRonin: The 0.008 is chosen to put things in a somewhat similar scale
* to the cross product. A 45 degree heading error will become
* a 7 deg/sec correction, so it neither takes forever to
* correct nor does a second of bad heading data screw us up
* too badly.
*/
mag_err[2] = modulo * 0.008f;
} else {
mag_err[0] = mag_err[1] = mag_err[2] = 0.0f;
}
} else {
mag_err[0] = mag_err[1] = mag_err[2] = 0.0f;
}
@ -427,7 +483,7 @@ static filterResult complementaryFilter(struct data *this, float gyro[3], float
// Accumulate integral of error. Scale here so that units are (deg/s) but Ki has units of s
this->gyroBias[0] -= accel_err[0] * this->attitudeSettings.AccelKi - gyro[0] * this->rollPitchBiasRate;
this->gyroBias[1] -= accel_err[1] * this->attitudeSettings.AccelKi - gyro[1] * this->rollPitchBiasRate;
if (this->useMag) {
if (this->useMag || this->useVelHeading) {
this->gyroBias[2] -= mag_err[2] * this->attitudeSettings.MagKi - gyro[2] * this->rollPitchBiasRate;
} else {
this->gyroBias[2] -= -gyro[2] * this->rollPitchBiasRate;
@ -437,7 +493,7 @@ static filterResult complementaryFilter(struct data *this, float gyro[3], float
// Correct rates based on proportional coefficient
gyrotmp[0] += accel_err[0] * this->attitudeSettings.AccelKp / dT;
gyrotmp[1] += accel_err[1] * this->attitudeSettings.AccelKp / dT;
if (this->useMag) {
if (this->useMag || this->useVelHeading) {
gyrotmp[2] += accel_err[2] * this->attitudeSettings.AccelKp / dT + mag_err[2] * this->attitudeSettings.MagKp / dT;
} else {
gyrotmp[2] += accel_err[2] * this->attitudeSettings.AccelKp / dT;

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

@ -232,9 +232,10 @@ static filterResult filter(stateFilter *self, stateEstimation *state)
// Reset the INS algorithm
INSGPSInit();
// variance is measured in mGaus, but internally the EKF works with a normalized vector. Scale down by Be^2
INSSetMagVar((float[3]) { this->ekfConfiguration.R.MagX,
this->ekfConfiguration.R.MagY,
this->ekfConfiguration.R.MagZ }
float Be2 = this->homeLocation.Be[0] * this->homeLocation.Be[0] + this->homeLocation.Be[1] * this->homeLocation.Be[1] + this->homeLocation.Be[2] * this->homeLocation.Be[2];
INSSetMagVar((float[3]) { this->ekfConfiguration.R.MagX / Be2,
this->ekfConfiguration.R.MagY / Be2,
this->ekfConfiguration.R.MagZ / Be2 }
);
INSSetAccelVar((float[3]) { this->ekfConfiguration.Q.AccelX,
this->ekfConfiguration.Q.AccelY,

1
flight/modules/StateEstimation/inc/stateestimation.h
View File

@ -95,6 +95,7 @@ int32_t filterAirInitialize(stateFilter *handle);
int32_t filterStationaryInitialize(stateFilter *handle);
int32_t filterLLAInitialize(stateFilter *handle);
int32_t filterCFInitialize(stateFilter *handle);
int32_t filterCFHInitialize(stateFilter *handle);
int32_t filterCFMInitialize(stateFilter *handle);
int32_t filterEKF13iInitialize(stateFilter *handle);
int32_t filterEKF13Initialize(stateFilter *handle);

23
flight/modules/StateEstimation/stateestimation.c
View File

@ -154,6 +154,7 @@ static stateFilter airFilter;
static stateFilter stationaryFilter;
static stateFilter llaFilter;
static stateFilter cfFilter;
static stateFilter cfhFilter;
static stateFilter cfmFilter;
static stateFilter ekf13iFilter;
static stateFilter ekf13Filter;
@ -184,6 +185,22 @@ static const filterPipeline *cfQueue = &(filterPipeline) {
}
}
};
static const filterPipeline *cfgpsQueue = &(filterPipeline) {
.filter = &airFilter,
.next = &(filterPipeline) {
.filter = &llaFilter,
.next = &(filterPipeline) {
.filter = &baroiFilter,
.next = &(filterPipeline) {
.filter = &altitudeFilter,
.next = &(filterPipeline) {
.filter = &cfhFilter,
.next = NULL,
}
}
}
}
};
static const filterPipeline *cfmiQueue = &(filterPipeline) {
.filter = &magFilter,
.next = &(filterPipeline) {
@ -366,6 +383,7 @@ int32_t StateEstimationInitialize(void)
stack_required = maxint32_t(stack_required, filterStationaryInitialize(&stationaryFilter));
stack_required = maxint32_t(stack_required, filterLLAInitialize(&llaFilter));
stack_required = maxint32_t(stack_required, filterCFInitialize(&cfFilter));
stack_required = maxint32_t(stack_required, filterCFHInitialize(&cfhFilter));
stack_required = maxint32_t(stack_required, filterCFMInitialize(&cfmFilter));
stack_required = maxint32_t(stack_required, filterEKF13iInitialize(&ekf13iFilter));
stack_required = maxint32_t(stack_required, filterEKF13Initialize(&ekf13Filter));
@ -443,6 +461,11 @@ static void StateEstimationCb(void)
// reinit Mag alarm
AlarmsSet(SYSTEMALARMS_ALARM_MAGNETOMETER, SYSTEMALARMS_ALARM_UNINITIALISED);
break;
case REVOSETTINGS_FUSIONALGORITHM_COMPLEMENTARYGPSOUTDOOR:
newFilterChain = cfgpsQueue;
// reinit Mag alarm
AlarmsSet(SYSTEMALARMS_ALARM_MAGNETOMETER, SYSTEMALARMS_ALARM_UNINITIALISED);
break;
case REVOSETTINGS_FUSIONALGORITHM_COMPLEMENTARYMAG:
newFilterChain = cfmiQueue;
break;

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

@ -131,6 +131,7 @@ static uint8_t i2c_error_activity[PIOS_I2C_ERROR_COUNT_NUMELEM];
#ifdef PIOS_INCLUDE_RFM22B
static uint8_t previousRFXtalCap;
static uint8_t protocol;
static void oplinkSettingsUpdatedCb(UAVObjEvent *ev);
#endif
@ -239,6 +240,8 @@ static void systemTask(__attribute__((unused)) void *parameters)
// Initialize previousRFXtalCap used by callback
OPLinkSettingsRFXtalCapGet(&previousRFXtalCap);
OPLinkSettingsConnectCallback(oplinkSettingsUpdatedCb);
// Get protocol
OPLinkSettingsProtocolGet(&protocol);
#endif
#ifdef DIAG_TASKS
@ -332,7 +335,7 @@ static void systemTask(__attribute__((unused)) void *parameters)
oplinkStatus.RXSeq = radio_stats.rx_seq;
oplinkStatus.LinkState = radio_stats.link_state;
} else {
} else if (protocol != OPLINKSETTINGS_PROTOCOL_OPENLRS) {
oplinkStatus.LinkState = OPLINKSTATUS_LINKSTATE_DISABLED;
}
OPLinkStatusSet(&oplinkStatus);

31
ground/gcs/src/plugins/uavobjectbrowser/fieldtreeitem.cpp → flight/modules/Temperature/inc/temperature.h
View File

@ -1,13 +1,17 @@
/**
******************************************************************************
* @addtogroup OpenPilotModules OpenPilot Modules
* @{
* @addtogroup TemperatureModule Temperature Module
* @{
*
* @file temperature.h
* @author The LibrePilot Project, http://www.librepilot.org Copyright (C) 2019.
* The OpenPilot Team, http://www.openpilot.org Copyright (C) 2010.
* @brief Module to read the temperature periodically from NTC or LM35 sensors.
*
* @see The GNU Public License (GPL) Version 3
*
* @file fieldtreeitem.cpp
* @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2010.
* @addtogroup GCSPlugins GCS Plugins
* @{
* @addtogroup UAVObjectBrowserPlugin UAVObject Browser Plugin
* @{
* @brief The UAVObject Browser gadget plugin
*****************************************************************************/
/*
* This program is free software; you can redistribute it and/or modify
@ -24,5 +28,16 @@
* with this program; if not, write to the Free Software Foundation, Inc.,
* 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA
*/
#ifndef TEMPERATURE_H
#define TEMPERATURE_H
#include "fieldtreeitem.h"
#include "openpilot.h"
int32_t TemperatureInitialize(void);
#endif // TEMPERATURE_H
/**
* @}
* @}
*/

175
flight/modules/Temperature/temperature.c Normal file
View File

@ -0,0 +1,175 @@
/**
******************************************************************************
* @addtogroup OpenPilotModules OpenPilot Modules
* @{
* @addtogroup TemperatureModule Temperature Module
* @brief Measures the temperature from external sensor.
* Updates the TemperatureState object
* @{
*
* @file temperature.c
* @author The LibrePilot Project, http://www.librepilot.org Copyright (C) 2019.
* The OpenPilot Team, http://www.openpilot.org Copyright (C) 2010.
* @brief Module to read the temperature periodically from NTC or LM35 sensors.
*
* @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
*/
/**
* Output object: TemperatureState
*
* This module will periodically generate information on the temperature state.
*
* UAVObjects are automatically generated by the UAVObjectGenerator from
* the object definition XML file.
*
* Modules have no API, all communication to other modules is done through UAVObjects.
* However modules may use the API exposed by shared libraries.
* See the OpenPilot wiki for more details.
* http://www.openpilot.org/OpenPilot_Application_Architecture
*
*/
#include "openpilot.h"
#include "temperaturestate.h"
#include "temperaturesettings.h"
#include "hwsettings.h"
//
// Configuration
//
#define SAMPLE_PERIOD_MS 500
#ifndef PIOS_ADC_NTCTEMPERATURE_PIN
#define PIOS_ADC_NTCTEMPERATURE_PIN -1
#endif
#ifndef PIOS_ADC_LM35TEMPERATURE_PIN
#define PIOS_ADC_LM35TEMPERATURE_PIN -1
#endif
static int8_t ntcTemperatureADCPin = PIOS_ADC_NTCTEMPERATURE_PIN; // ADC pin for temperature (ntc)
static int8_t lm35TemperatureADCPin = PIOS_ADC_LM35TEMPERATURE_PIN; // ADC pin for temperature (lm35)
// Private functions
static void onTimer(UAVObjEvent *ev);
/**
* Initialise the module, called on startup
* \returns 0 on success or -1 if initialisation failed
*/
int32_t TemperatureInitialize(void)
{
bool temperatureEnabled;
HwSettingsOptionalModulesData optionalModules;
HwSettingsOptionalModulesGet(&optionalModules);
#ifdef MODULE_TEMPERATURE_BUILTIN
temperatureEnabled = true;
optionalModules.Temperature = HWSETTINGS_OPTIONALMODULES_ENABLED;
HwSettingsOptionalModulesSet(&optionalModules);
#else
if (optionalModules.Temperature == HWSETTINGS_OPTIONALMODULES_ENABLED) {
temperatureEnabled = true;
} else {
temperatureEnabled = false;
}
#endif
uint8_t adcRouting[HWSETTINGS_ADCROUTING_NUMELEM];
HwSettingsADCRoutingArrayGet(adcRouting);
// Determine if the temperature sensors are routed to ADC pins
for (int i = 0; i < HWSETTINGS_ADCROUTING_NUMELEM; i++) {
if (adcRouting[i] == HWSETTINGS_ADCROUTING_NTCTEMPERATURE) {
ntcTemperatureADCPin = i;
}
if (adcRouting[i] == HWSETTINGS_ADCROUTING_LM35TEMPERATURE) {
lm35TemperatureADCPin = i;
}
}
// Don't enable module if no ADC pins are routed to the sensors
if (ntcTemperatureADCPin < 0 && lm35TemperatureADCPin < 0) {
temperatureEnabled = false;
}
// Start module
if (temperatureEnabled) {
TemperatureStateInitialize();
static UAVObjEvent ev;
memset(&ev, 0, sizeof(UAVObjEvent));
EventPeriodicCallbackCreate(&ev, onTimer, SAMPLE_PERIOD_MS / portTICK_RATE_MS);
}
return 0;
}
MODULE_INITCALL(TemperatureInitialize, 0);
static void onTimer(__attribute__((unused)) UAVObjEvent *ev)
{
static TemperatureSettingsData temperatureSettings;
static TemperatureStateData temperatureData;
TemperatureSettingsGet(&temperatureSettings);
TemperatureStateGet(&temperatureData);
#ifdef PIOS_INCLUDE_ADC
// calculate the temperature (ntc)
if (ntcTemperatureADCPin >= 0) {
float ntc_resistance = 0;
if (temperatureSettings.NtcVoltageDivider == TEMPERATURESETTINGS_NTCVOLTAGEDIVIDER_NTCTOGROUND) {
ntc_resistance = temperatureSettings.NtcSerialResistor / ((temperatureSettings.NtcVoltageReference / PIOS_ADC_PinGetVolt(ntcTemperatureADCPin)) - 1);
} else if (temperatureSettings.NtcVoltageDivider == TEMPERATURESETTINGS_NTCVOLTAGEDIVIDER_NTCTOVREF) {
ntc_resistance = temperatureSettings.NtcSerialResistor * ((temperatureSettings.NtcVoltageReference / PIOS_ADC_PinGetVolt(ntcTemperatureADCPin)) - 1);
}
float steinhart = ntc_resistance / temperatureSettings.NtcNominalValue;
steinhart = logf(steinhart);
steinhart /= temperatureSettings.NtcBetaCoeff;
steinhart += 1.0f / (temperatureSettings.NtcNominalTemperature + 273.15f);
steinhart = 1.0f / steinhart;
temperatureData.Temperature1 = steinhart - 273.15f;
} else {
temperatureData.Temperature1 = 0;
}
// calculate the temperature (lm35)
if (lm35TemperatureADCPin >= 0) {
temperatureData.Temperature2 = (PIOS_ADC_PinGetVolt(lm35TemperatureADCPin) / temperatureSettings.Lm35Sensitivity);
} else {
temperatureData.Temperature2 = 0;
}
#else /* ifdef PIOS_INCLUDE_ADC */
temperatureData.Temperature1 = 0;
temperatureData.Temperature2 = 0;
#endif /* PIOS_INCLUDE_ADC */
TemperatureStateSet(&temperatureData);
}
/**
* @}
*/

3
flight/modules/UAVOHottBridge/inc/uavohottbridge.h
View File

@ -176,7 +176,8 @@ struct telemetrydata {
FlightBatteryStateData Battery;
FlightStatusData FlightStatus;
GPSPositionSensorData GPS;
GPSTimeData GPStime;
AirspeedStateData Airspeed;
GPSTimeData GPStime;
GyroSensorData Gyro;
HomeLocationData Home;
PositionStateData Position;

61
flight/modules/UAVOHottBridge/uavohottbridge.c
View File

@ -45,6 +45,7 @@
#include "gyrosensor.h"
#include "gpspositionsensor.h"
#include "gpstime.h"
#include "airspeedstate.h"
#include "homelocation.h"
#include "positionstate.h"
#include "systemalarms.h"
@ -451,6 +452,25 @@ uint16_t build_GAM_message(struct hott_gam_message *msg)
msg->current = scale_float2uword(current, 10, 0);
msg->capacity = scale_float2uword(energy, 0.1f, 0);
// simulate individual cell voltage
uint8_t cell_voltage = (telestate->Battery.Voltage > 0) ? scale_float2uint8(telestate->Battery.Voltage / telestate->Battery.NbCells, 50, 0) : 0;
msg->cell1 = (telestate->Battery.NbCells >= 1) ? cell_voltage : 0;
msg->cell2 = (telestate->Battery.NbCells >= 2) ? cell_voltage : 0;
msg->cell3 = (telestate->Battery.NbCells >= 3) ? cell_voltage : 0;
msg->cell4 = (telestate->Battery.NbCells >= 4) ? cell_voltage : 0;
msg->cell5 = (telestate->Battery.NbCells >= 5) ? cell_voltage : 0;
msg->cell6 = (telestate->Battery.NbCells >= 6) ? cell_voltage : 0;
msg->min_cell_volt = cell_voltage;
msg->min_cell_volt_num = telestate->Battery.NbCells;
// apply main voltage to batt1 voltage
msg->batt1_voltage = msg->voltage;
// AirSpeed
float airspeed = (telestate->Airspeed.TrueAirspeed > 0) ? telestate->Airspeed.TrueAirspeed : 0;
msg->speed = scale_float2uword(airspeed, MS_TO_KMH, 0);
// pressure kPa to 0.1Bar
msg->pressure = scale_float2uint8(telestate->Baro.Pressure, 0.1f, 0);
@ -492,9 +512,26 @@ uint16_t build_EAM_message(struct hott_eam_message *msg)
float voltage = (telestate->Battery.Voltage > 0) ? telestate->Battery.Voltage : 0;
float current = (telestate->Battery.Current > 0) ? telestate->Battery.Current : 0;
float energy = (telestate->Battery.ConsumedEnergy > 0) ? telestate->Battery.ConsumedEnergy : 0;
msg->voltage = scale_float2uword(voltage, 10, 0);
msg->current = scale_float2uword(current, 10, 0);
msg->capacity = scale_float2uword(energy, 0.1f, 0);
msg->voltage = scale_float2uword(voltage, 10, 0);
msg->current = scale_float2uword(current, 10, 0);
msg->capacity = scale_float2uword(energy, 0.1f, 0);
// simulate individual cell voltage
uint8_t cell_voltage = (telestate->Battery.Voltage > 0) ? scale_float2uint8(telestate->Battery.Voltage / telestate->Battery.NbCells, 50, 0) : 0;
msg->cell1_H = (telestate->Battery.NbCells >= 1) ? cell_voltage : 0;
msg->cell2_H = (telestate->Battery.NbCells >= 2) ? cell_voltage : 0;
msg->cell3_H = (telestate->Battery.NbCells >= 3) ? cell_voltage : 0;
msg->cell4_H = (telestate->Battery.NbCells >= 4) ? cell_voltage : 0;
msg->cell5_H = (telestate->Battery.NbCells >= 5) ? cell_voltage : 0;
msg->cell6_H = (telestate->Battery.NbCells >= 6) ? cell_voltage : 0;
msg->cell7_H = (telestate->Battery.NbCells >= 7) ? cell_voltage : 0;
// apply main voltage to batt1 voltage
msg->batt1_voltage = msg->voltage;
// AirSpeed
float airspeed = (telestate->Airspeed.TrueAirspeed > 0) ? telestate->Airspeed.TrueAirspeed : 0;
msg->speed = scale_float2uword(airspeed, MS_TO_KMH, 0);
// temperatures
msg->temperature1 = scale_float2uint8(telestate->Gyro.temperature, 1, OFFSET_TEMPERATURE);
@ -597,6 +634,9 @@ void update_telemetrydata()
if (GPSPositionSensorHandle() != NULL) {
GPSPositionSensorGet(&telestate->GPS);
}
if (AirspeedStateHandle() != NULL) {
AirspeedStateGet(&telestate->Airspeed);
}
if (GPSTimeHandle() != NULL) {
GPSTimeGet(&telestate->GPStime);
}
@ -646,8 +686,9 @@ void update_telemetrydata()
// calculate altitude relative to start position
telestate->altitude = -telestate->Position.Down;
// check and set min/max values when armed.
if (telestate->FlightStatus.Armed == FLIGHTSTATUS_ARMED_ARMED) {
// check and set min/max values when armed
// and without receiver input for standalone board used as sensor
if ((telestate->FlightStatus.Armed == FLIGHTSTATUS_ARMED_ARMED) || ((telestate->SysAlarms.Alarm.Attitude == SYSTEMALARMS_ALARM_OK) && (telestate->SysAlarms.Alarm.Receiver != SYSTEMALARMS_ALARM_OK))) {
if (telestate->min_altitude > telestate->altitude) {
telestate->min_altitude = telestate->altitude;
}
@ -774,10 +815,12 @@ void update_telemetrydata()
*/
uint8_t generate_warning()
{
bool gps_ok = (telestate->SysAlarms.Alarm.GPS == SYSTEMALARMS_ALARM_OK);
// set warning tone with hardcoded priority
if ((telestate->Settings.Warning.MinSpeed == HOTTBRIDGESETTINGS_WARNING_ENABLED) &&
(telestate->Settings.Limit.MinSpeed > telestate->GPS.Groundspeed * MS_TO_KMH)) {
return HOTT_TONE_A; // maximum speed
(telestate->Settings.Limit.MinSpeed > telestate->GPS.Groundspeed * MS_TO_KMH) && gps_ok) {
return HOTT_TONE_A; // minimum speed
}
if ((telestate->Settings.Warning.NegDifference2 == HOTTBRIDGESETTINGS_WARNING_ENABLED) &&
(telestate->Settings.Limit.NegDifference2 > telestate->climbrate3s)) {
@ -788,7 +831,7 @@ uint8_t generate_warning()
return HOTT_TONE_C; // sink rate 1s
}
if ((telestate->Settings.Warning.MaxDistance == HOTTBRIDGESETTINGS_WARNING_ENABLED) &&
(telestate->Settings.Limit.MaxDistance < telestate->homedistance)) {
(telestate->Settings.Limit.MaxDistance < telestate->homedistance) && gps_ok) {
return HOTT_TONE_D; // maximum distance
}
if ((telestate->Settings.Warning.MinSensor1Temp == HOTTBRIDGESETTINGS_WARNING_ENABLED) &&
@ -808,7 +851,7 @@ uint8_t generate_warning()
return HOTT_TONE_I; // maximum temperature sensor 2
}
if ((telestate->Settings.Warning.MaxSpeed == HOTTBRIDGESETTINGS_WARNING_ENABLED) &&
(telestate->Settings.Limit.MaxSpeed < telestate->GPS.Groundspeed * MS_TO_KMH)) {
(telestate->Settings.Limit.MaxSpeed < telestate->GPS.Groundspeed * MS_TO_KMH) && gps_ok) {
return HOTT_TONE_L; // maximum speed
}
if ((telestate->Settings.Warning.PosDifference2 == HOTTBRIDGESETTINGS_WARNING_ENABLED) &&

7
flight/modules/UAVOMSPBridge/UAVOMSPBridge.c
View File

@ -43,7 +43,7 @@
#include "gpspositionsensor.h"
#include "manualcontrolcommand.h"
#include "manualcontrolsettings.h"
#include "oplinkreceiver.h"
#include "oplinkstatus.h"
#include "accessorydesired.h"
#include "attitudestate.h"
#include "airspeedstate.h"
@ -479,9 +479,10 @@ static void msp_send_analog(struct msp_bridge *m)
ManualControlSettingsChannelGroupsGet(&channelGroups);
#ifdef PIOS_INCLUDE_OPLINKRCVR
if (channelGroups.Throttle == MANUALCONTROLSETTINGS_CHANNELGROUPS_OPLINK) {
if ((channelGroups.Throttle == MANUALCONTROLSETTINGS_CHANNELGROUPS_OPLINK) ||
(channelGroups.Throttle == MANUALCONTROLSETTINGS_CHANNELGROUPS_OPENLRS)) {
int8_t rssi;
OPLinkReceiverRSSIGet(&rssi);
OPLinkStatusRSSIGet(&rssi);
// MSP values have no units, and OSD rssi display requires calibration anyway, so we will translate OPLINK_LOW_RSSI to OPLINK_HIGH_RSSI -> 0-1023
if (rssi < OPLINK_LOW_RSSI) {

7
flight/modules/UAVOMavlinkBridge/UAVOMavlinkBridge.c
View File

@ -52,7 +52,7 @@
#include "taskinfo.h"
#include "mavlink.h"
#include "hwsettings.h"
#include "oplinkreceiver.h"
#include "oplinkstatus.h"
#include "receiverstatus.h"
#include "manualcontrolsettings.h"
@ -248,9 +248,10 @@ static void mavlink_send_rc_channels()
ManualControlSettingsChannelGroupsGet(&channelGroups);
#ifdef PIOS_INCLUDE_OPLINKRCVR
if (channelGroups.Throttle == MANUALCONTROLSETTINGS_CHANNELGROUPS_OPLINK) {
if ((channelGroups.Throttle == MANUALCONTROLSETTINGS_CHANNELGROUPS_OPLINK) ||
(channelGroups.Throttle == MANUALCONTROLSETTINGS_CHANNELGROUPS_OPENLRS)) {
int8_t rssi;
OPLinkReceiverRSSIGet(&rssi);
OPLinkStatusRSSIGet(&rssi);
if (rssi < OPLINK_LOW_RSSI) {
rssi = OPLINK_LOW_RSSI;

122
flight/pios/common/pios_openlrs.c
View File

@ -465,6 +465,57 @@ static void tx_packet(struct pios_openlrs_dev *openlrs_dev, uint8_t *pkt, uint8_
}
}
#ifdef OPENLRS_SIMPLE_BEACON_TONE
// Basic tone
static void beacon_simpletone(struct pios_openlrs_dev *openlrs_dev, uint8_t tone, int16_t len __attribute__((unused)))
{
DEBUG_PRINTF(2, "beacon_morse: %d\r\n", len);
#if defined(PIOS_LED_LINK)
PIOS_LED_On(PIOS_LED_LINK);
#endif /* PIOS_LED_LINK */
rfm22_claimBus(openlrs_dev);
GPIO_TypeDef *gpio = openlrs_dev->cfg.spi_cfg->mosi.gpio;
uint16_t pin_source = openlrs_dev->cfg.spi_cfg->mosi.init.GPIO_Pin;
uint8_t remap = openlrs_dev->cfg.spi_cfg->remap;
GPIO_InitTypeDef init = {
.GPIO_Speed = GPIO_Speed_50MHz,
.GPIO_Mode = GPIO_Mode_OUT,
.GPIO_OType = GPIO_OType_PP,
.GPIO_PuPd = GPIO_PuPd_UP
};
init.GPIO_Pin = pin_source;
// Set MOSI to digital out for bit banging
GPIO_PinAFConfig(gpio, pin_source, 0);
GPIO_Init(gpio, &init);
int16_t cycles = (len * 50) / tone;
for (int16_t i = 0; i < cycles; i++) {
GPIO_SetBits(gpio, pin_source);
PIOS_Thread_Sleep(tone);
GPIO_ResetBits(gpio, pin_source);
PIOS_Thread_Sleep(tone);
}
GPIO_Init(gpio, (GPIO_InitTypeDef *)&openlrs_dev->cfg.spi_cfg->mosi.init);
GPIO_PinAFConfig(gpio, pin_source, remap);
rfm22_releaseBus(openlrs_dev);
#if defined(PIOS_LED_LINK)
PIOS_LED_Off(PIOS_LED_LINK);
#endif /* PIOS_LED_LINK */
#if defined(PIOS_INCLUDE_WDG) && defined(PIOS_WDG_RFM22B)
// Update the watchdog timer
PIOS_WDG_UpdateFlag(PIOS_WDG_RFM22B);
#endif /* PIOS_WDG_RFM22B */
}
#else /* OPENLRS_SIMPLE_BEACON_TONE */
static void beacon_tone(struct pios_openlrs_dev *openlrs_dev, int16_t hz, int16_t len __attribute__((unused))) // duration is now in half seconds.
{
DEBUG_PRINTF(2, "beacon_tone: %d %d\r\n", hz, len * 2);
@ -481,7 +532,7 @@ static void beacon_tone(struct pios_openlrs_dev *openlrs_dev, int16_t hz, int16_
rfm22_claimBus(openlrs_dev);
// This need fixed for F1
#ifdef GPIO_Mode_OUT
#ifdef GPIO_MODE_OUT
GPIO_TypeDef *gpio = openlrs_dev->cfg.spi_cfg->mosi.gpio;
uint16_t pin_source = openlrs_dev->cfg.spi_cfg->mosi.init.GPIO_Pin;
uint8_t remap = openlrs_dev->cfg.spi_cfg->remap;
@ -515,7 +566,7 @@ static void beacon_tone(struct pios_openlrs_dev *openlrs_dev, int16_t hz, int16_
GPIO_Init(gpio, (GPIO_InitTypeDef *)&openlrs_dev->cfg.spi_cfg->mosi.init);
GPIO_PinAFConfig(gpio, pin_source, remap);
#endif /* ifdef GPIO_Mode_OUT */
#endif /* ifdef GPIO_MODE_OUT */
rfm22_releaseBus(openlrs_dev);
#if defined(PIOS_LED_LINK)
@ -528,6 +579,7 @@ static void beacon_tone(struct pios_openlrs_dev *openlrs_dev, int16_t hz, int16_
#endif /* PIOS_WDG_RFM22B */
}
#endif /* OPENLRS_SIMPLE_BEACON_TONE */
static uint8_t beaconGetRSSI(struct pios_openlrs_dev *openlrs_dev)
{
@ -588,9 +640,40 @@ static void beacon_send(struct pios_openlrs_dev *openlrs_dev, bool static_tone)
if (static_tone) {
uint8_t i = 0;
while (i++ < 20) {
#ifdef OPENLRS_SIMPLE_BEACON_TONE
beacon_simpletone(openlrs_dev, 2, 3);
#else
beacon_tone(openlrs_dev, 440, 1);
#endif
}
} else {
#ifdef OPENLRS_SIMPLE_BEACON_TONE
for (uint32_t tone = 1; tone < 4; tone++) {
if (tone == 1) {
rfm22_write(openlrs_dev, 0x6d, 0x05); // 5 set min power 25mW
} else {
rfm22_write(openlrs_dev, 0x6d, 0x00); // 0 set min power 1.3mW
}
// L - --- - -
beacon_simpletone(openlrs_dev, tone, 1);
PIOS_Thread_Sleep(100);
beacon_simpletone(openlrs_dev, tone, 3);
PIOS_Thread_Sleep(100);
beacon_simpletone(openlrs_dev, tone, 1);
PIOS_Thread_Sleep(100);
beacon_simpletone(openlrs_dev, tone, 1);
PIOS_Thread_Sleep(600);
// P - --- --- -
beacon_simpletone(openlrs_dev, tone, 1);
PIOS_Thread_Sleep(100);
beacon_simpletone(openlrs_dev, tone, 3);
PIOS_Thread_Sleep(100);
beacon_simpletone(openlrs_dev, tone, 3);
PIOS_Thread_Sleep(100);
beacon_simpletone(openlrs_dev, tone, 1);
PIOS_Thread_Sleep(2000);
}
#else /* ifdef OPENLRS_SIMPLE_BEACON_TONE */
// close encounters tune
// G, A, F, F(lower octave), C
// octave 3: 392 440 349 175 261
@ -612,6 +695,7 @@ static void beacon_send(struct pios_openlrs_dev *openlrs_dev, bool static_tone)
rfm22_write(openlrs_dev, 0x6d, 0x00); // 0 set min power 1.3mW
PIOS_Thread_Sleep(10);
beacon_tone(openlrs_dev, 261, 2);
#endif /* #ifdef OPENLRS_SIMPLE_BEACON_TONE */
}
rfm22_write_claim(openlrs_dev, 0x07, RF22B_PWRSTATE_READY);
}
@ -829,9 +913,6 @@ static void pios_openlrs_rx_loop(struct pios_openlrs_dev *openlrs_dev)
OPLinkStatusData oplink_status;
OPLinkStatusGet(&oplink_status);
// Update the RSSI
oplink_status.RSSI = openlrs_dev->rssi;
timeUs = PIOS_DELAY_GetuS();
timeMs = PIOS_Thread_Systime();
@ -860,8 +941,8 @@ static void pios_openlrs_rx_loop(struct pios_openlrs_dev *openlrs_dev)
openlrs_dev->lastPacketTimeUs = timeUs;
openlrs_dev->numberOfLostPackets = 0;
oplink_status.LinkQuality <<= 1;
oplink_status.LinkQuality |= 1;
openlrs_dev->link_quality <<= 1;
openlrs_dev->link_quality |= 1;
if ((openlrs_dev->rx_buf[0] & 0x3e) == 0x00) {
// This flag indicates receiving PPM data
@ -928,7 +1009,7 @@ static void pios_openlrs_rx_loop(struct pios_openlrs_dev *openlrs_dev)
}
tx_buf[4] = (openlrs_dev->lastAFCCvalue >> 8);
tx_buf[5] = openlrs_dev->lastAFCCvalue & 0xff;
tx_buf[6] = countSetBits(oplink_status.LinkQuality & 0x7fff);
tx_buf[6] = countSetBits(openlrs_dev->link_quality & 0x7fff);
}
}
@ -949,7 +1030,7 @@ static void pios_openlrs_rx_loop(struct pios_openlrs_dev *openlrs_dev)
if ((openlrs_dev->numberOfLostPackets < openlrs_dev->hopcount) && (PIOS_DELAY_GetuSSince(openlrs_dev->lastPacketTimeUs) > (getInterval(&openlrs_dev->bind_data) + packet_timeout_us))) {
DEBUG_PRINTF(2, "OLRS WARN: Lost packet: %d\r\n", openlrs_dev->numberOfLostPackets);
// we lost packet, hop to next channel
oplink_status.LinkQuality <<= 1;
openlrs_dev->link_quality <<= 1;
openlrs_dev->willhop = 1;
if (openlrs_dev->numberOfLostPackets == 0) {
openlrs_dev->linkLossTimeMs = timeMs;
@ -961,7 +1042,7 @@ static void pios_openlrs_rx_loop(struct pios_openlrs_dev *openlrs_dev)
} else if ((openlrs_dev->numberOfLostPackets >= openlrs_dev->hopcount) && (PIOS_DELAY_GetuSSince(openlrs_dev->lastPacketTimeUs) > (getInterval(&openlrs_dev->bind_data) * openlrs_dev->hopcount))) {
DEBUG_PRINTF(2, "ORLS WARN: Trying to resync\r\n");
// hop slowly to allow resync with TX
oplink_status.LinkQuality = 0;
openlrs_dev->link_quality = 0;
openlrs_dev->willhop = 1;
openlrs_dev->lastPacketTimeUs = timeUs;
}
@ -977,7 +1058,7 @@ static void pios_openlrs_rx_loop(struct pios_openlrs_dev *openlrs_dev)
DEBUG_PRINTF(2, "Failsafe activated: %d %d\r\n", timeMs, openlrs_dev->linkLossTimeMs);
oplink_status.LinkState = OPLINKSTATUS_LINKSTATE_DISCONNECTED;
// failsafeApply();
openlrs_dev->nextBeaconTimeMs = (timeMs + 1000UL * openlrs_dev->beacon_period) | 1; // beacon activating...
openlrs_dev->nextBeaconTimeMs = (timeMs + (1000UL * openlrs_dev->beacon_delay)) | 1; // beacon activating...
}
if ((openlrs_dev->beacon_frequency) && (openlrs_dev->nextBeaconTimeMs) &&
@ -993,7 +1074,7 @@ static void pios_openlrs_rx_loop(struct pios_openlrs_dev *openlrs_dev)
beacon_send(openlrs_dev, false); // play cool tune
init_rfm(openlrs_dev, 0); // go back to normal RX
rx_reset(openlrs_dev);
openlrs_dev->nextBeaconTimeMs = (timeMs + 1000UL * openlrs_dev->beacon_period) | 1; // avoid 0 in time
openlrs_dev->nextBeaconTimeMs = (timeMs + (1000UL * openlrs_dev->beacon_period)) | 1; // avoid 0 in time
}
}
}
@ -1029,6 +1110,19 @@ static void pios_openlrs_rx_loop(struct pios_openlrs_dev *openlrs_dev)
openlrs_dev->willhop = 0;
}
if (oplink_status.LinkState > OPLINKSTATUS_LINKSTATE_DISCONNECTED) {
// Convert raw Rssi to dBm
oplink_status.RSSI = (int8_t)(openlrs_dev->rssi >> 1) - 122;
// Count number of bits set in link_quality
uint8_t linkquality_bits = countSetBits(openlrs_dev->link_quality & 0x7fff);
// Translate link quality to 0 - 128 range
oplink_status.LinkQuality = (linkquality_bits + 1) * 8;
} else {
oplink_status.LinkQuality = 0;
oplink_status.RSSI = -127;
}
// Update UAVO
OPLinkStatusSet(&oplink_status);
}
@ -1276,13 +1370,13 @@ static void pios_openlrs_task(void *parameters)
}
} else {
// We timed out because packet was missed
DEBUG_PRINTF(3, "ISR Timeout. Missed packet: %d %d %d\r\n", delay, getInterval(&openlrs_dev->bind_data), time_since_packet_us);
// DEBUG_PRINTF(3, "ISR Timeout. Missed packet: %d %d %d\r\n", delay_ms, getInterval(&openlrs_dev->bind_data), time_since_packet_us);
pios_openlrs_rx_loop(openlrs_dev);
}
rssi_sampled = true;
} else {
// DEBUG_PRINTF(3, "ISR %d %d %d\r\n", delay, getInterval(&openlrs_dev->bind_data), time_since_packet_us);
// DEBUG_PRINTF(3, "ISR %d %d %d\r\n", delay_ms, getInterval(&openlrs_dev->bind_data), time_since_packet_us);
// Process incoming data
pios_openlrs_rx_loop(openlrs_dev);

36
flight/pios/common/pios_openlrs_rcvr.c
View File

@ -36,6 +36,7 @@
#include <uavobjectmanager.h>
#include <oplinkreceiver.h>
#include <oplinkstatus.h>
#include <pios_openlrs_priv.h>
#include <pios_openlrs_rcvr_priv.h>
@ -45,9 +46,11 @@
static int32_t PIOS_OpenLRS_Rcvr_Get(uint32_t rcvr_id, uint8_t channel);
static void PIOS_OpenLRS_Rcvr_Supervisor(uint32_t ppm_id);
static void PIOS_OpenLRS_Rcvr_ppm_callback(uint32_t openlrs_rcvr_id, const int16_t *channels);
static uint8_t PIOS_OpenLRSRCVR_Quality_Get(uint32_t openlrs_rcvr_id);
const struct pios_rcvr_driver pios_openlrs_rcvr_driver = {
.read = PIOS_OpenLRS_Rcvr_Get,
.read = PIOS_OpenLRS_Rcvr_Get,
.get_quality = PIOS_OpenLRSRCVR_Quality_Get
};
/* Local Variables */
@ -58,6 +61,8 @@ enum pios_openlrs_rcvr_dev_magic {
struct pios_openlrs_rcvr_dev {
enum pios_openlrs_rcvr_dev_magic magic;
int16_t channels[OPENLRS_PPM_NUM_CHANNELS];
int8_t RSSI;
uint8_t LinkQuality;
uint8_t supv_timer;
bool fresh;
};
@ -134,6 +139,15 @@ static void PIOS_OpenLRS_Rcvr_ppm_callback(uint32_t openlrs_rcvr_id, const int16
openlrs_rcvr_dev->channels[i] = channels[i];
}
// Update the RSSI and quality fields.
int8_t rssi;
OPLinkStatusRSSIGet(&rssi);
openlrs_rcvr_dev->RSSI = rssi;
uint16_t quality;
OPLinkStatusLinkQualityGet(&quality);
// Link quality is 0-128, so scale it down to 0-100
openlrs_rcvr_dev->LinkQuality = quality * 100 / 128;
openlrs_rcvr_update_uavo(openlrs_rcvr_dev);
// let supervisor know we have new data
@ -191,6 +205,8 @@ static void PIOS_OpenLRS_Rcvr_Supervisor(uint32_t openlrs_rcvr_id)
i++) {
openlrs_rcvr_dev->channels[i] = PIOS_RCVR_TIMEOUT;
}
openlrs_rcvr_dev->RSSI = -127;
openlrs_rcvr_dev->LinkQuality = 0;
}
openlrs_rcvr_dev->fresh = false;
@ -211,9 +227,27 @@ static void openlrs_rcvr_update_uavo(struct pios_openlrs_rcvr_dev *rcvr_dev)
for (int i = OPENLRS_PPM_NUM_CHANNELS - 1; i < OPLINKRECEIVER_CHANNEL_NUMELEM; i++) {
rcvr.Channel[i] = PIOS_RCVR_INVALID;
}
rcvr.RSSI = rcvr_dev->RSSI;
rcvr.LinkQuality = rcvr_dev->LinkQuality;
OPLinkReceiverSet(&rcvr);
}
static uint8_t PIOS_OpenLRSRCVR_Quality_Get(uint32_t openlrs_rcvr_id)
{
/* Recover our device context */
struct pios_openlrs_rcvr_dev *openlrs_rcvr_dev = (struct pios_openlrs_rcvr_dev *)openlrs_rcvr_id;
if (!PIOS_OpenLRS_Rcvr_Validate(openlrs_rcvr_dev)) {
/* Invalid device specified */
return 0;
}
return openlrs_rcvr_dev->LinkQuality;
}
#endif /* PIOS_INCLUDE_OPENLRS */
/**

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

@ -232,6 +232,8 @@ static void PIOS_oplinkrcvr_Supervisor(uint32_t oplinkrcvr_id)
for (int32_t i = 0; i < OPLINKRECEIVER_CHANNEL_NUMELEM; i++) {
oplinkrcvr_dev->oplinkreceiverdata.Channel[i] = PIOS_RCVR_TIMEOUT;
}
oplinkrcvr_dev->oplinkreceiverdata.RSSI = -127;
oplinkrcvr_dev->oplinkreceiverdata.LinkQuality = 0;
}
oplinkrcvr_dev->Fresh = false;

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

@ -339,14 +339,26 @@ static const uint32_t data_rate[] = {
static const uint8_t channel_spacing[] = {
1, /* 9.6kbps */
2, /* 19.2kps */
2, /* 32kps */
2, /* 57.6kps */
2, /* 64kps */
3, /* 100kps */
4, /* 128kps */
4, /* 192kps */
4, /* 256kps */
2, /* 19.2kbps */
2, /* 32kbps */
2, /* 57.6kbps */
2, /* 64kbps */
3, /* 100kbps */
4, /* 128kbps */
4, /* 192kbps */
5, /* 256kbps */
};
static const uint8_t channel_limits[] = {
1, /* 9.6kbps */
1, /* 19.2kbps */
1, /* 32kbps */
1, /* 57.6kbps */
1, /* 64kbps */
1, /* 100kbps */
2, /* 128kbps */
2, /* 192kbps */
2, /* 256kbps */
};
static const uint8_t reg_1C[] = { 0x01, 0x05, 0x06, 0x95, 0x95, 0x81, 0x88, 0x8B, 0x8D }; // rfm22_if_filter_bandwidth
@ -2669,9 +2681,16 @@ static void rfm22_hmac_sha1(const uint8_t *data, size_t len,
static bool rfm22_gen_channels(uint32_t coordid, enum rfm22b_datarate rate, uint8_t min,
uint8_t max, uint8_t channels[MAX_CHANNELS], uint8_t *clen)
{
// Define first and last channel to be used within min/max values
// according to the frequency deviation, without up/down overflow.
uint8_t chan_min_limit = min + channel_limits[rate];
uint8_t chan_max_limit = max - channel_limits[rate];
// Define how many channels we can use according to the spacing.
uint8_t chan_count = ((chan_max_limit - chan_min_limit) / channel_spacing[rate]) + 1;
uint32_t data = 0;
uint8_t cpos = 0;
uint8_t chan_range = (max / channel_spacing[rate] - min / channel_spacing[rate]) + 1;
uint8_t key[SHA1_DIGEST_LENGTH] = { 0 };
uint8_t digest[SHA1_DIGEST_LENGTH];
uint8_t *all_channels;
@ -2680,12 +2699,16 @@ static bool rfm22_gen_channels(uint32_t coordid, enum rfm22b_datarate rate, uint
memcpy(key, &coordid, sizeof(coordid));
for (int i = 0; i < chan_range; i++) {
all_channels[i] = min / channel_spacing[rate] + i;
// Fill all_channels[] with usable channels
for (int i = 0; i < chan_count; i++) {
all_channels[i] = chan_min_limit + (i * channel_spacing[rate]);
}
// DEBUG_PRINTF(3, "\r\nChannel Min: %d Max:%d - Spacing: %d Limits: %d\r\n", min, max, channel_spacing[rate], channel_limits[rate]);
// DEBUG_PRINTF(3, "Result: Channel count: %d - Usable channels from ch%d to ch%d\r\n", chan_count, all_channels[0], all_channels[chan_count - 1]);
int j = SHA1_DIGEST_LENGTH;
for (int i = 0; i < chan_range && i < MAX_CHANNELS; i++) {
for (int i = 0; i < chan_count && i < MAX_CHANNELS; i++) {
uint8_t rnd;
uint8_t r;
uint8_t tmp;
@ -2697,14 +2720,16 @@ static bool rfm22_gen_channels(uint32_t coordid, enum rfm22b_datarate rate, uint
}
rnd = digest[j];
j++;
r = rnd % (chan_range - i) + i;
r = rnd % (chan_count - i) + i;
tmp = all_channels[i];
all_channels[i] = all_channels[r];
all_channels[r] = tmp;
}
for (int i = 0; i < chan_range && cpos < MAX_CHANNELS; i++, cpos++) {
channels[cpos] = all_channels[i] * channel_spacing[rate];
// DEBUG_PRINTF(3, "Final channel list:");
for (int i = 0; i < chan_count && cpos < MAX_CHANNELS; i++, cpos++) {
channels[cpos] = all_channels[i];
// DEBUG_PRINTF(3, " %d ", all_channels[i]);
}
*clen = cpos & 0xfe;

6
flight/pios/common/pios_servo.c
View File

@ -72,7 +72,7 @@ static uint32_t pios_dshot_t0h_raw;
static uint32_t pios_dshot_t1h_raw;
static uint32_t pios_dshot_t_raw;
static bool pios_servo_enabled = true;
static bool pios_servo_enabled = false;
static uint32_t pios_servo_active = 0; // No active outputs by default
#define PIOS_SERVO_TIMER_CLOCK 1000000
@ -111,6 +111,10 @@ extern void PIOS_Servo_Disable()
* if using inverted setup */
for (uint8_t i = 0; (i < servo_cfg->num_channels); i++) {
if (!(pios_servo_active & (1L << i))) { // This output is not active
continue;
}
const struct pios_tim_channel *chan = &servo_cfg->channels[i];
GPIO_InitTypeDef init = chan->pin.init;

3
flight/pios/inc/pios_openlrs_priv.h
View File

@ -183,7 +183,8 @@ struct pios_openlrs_dev {
uint8_t rx_buf[64];
uint8_t tx_buf[9];
int8_t rssi;
uint8_t rssi;
uint16_t link_quality;
// Variables from OpenLRS for radio control
uint8_t hopcount;

2
flight/pios/stm32f4xx/libraries/STM32_USB_OTG_Driver/src/usb_dcd_int.c
View File

@ -681,7 +681,7 @@ static uint32_t DCD_WriteEmptyTxFifo(USB_OTG_CORE_HANDLE *pdev, uint32_t epnum)
}
// --- start fix
uint32_t fifoemptymsk;
if (len < ep->maxpacket)
if (len < ep->maxpacket || ep->xfer_count==0)
{
// FIFO empty
fifoemptymsk = 0x1 << epnum;

42
flight/pios/stm32f4xx/pios_usb.c
View File

@ -38,7 +38,7 @@
#include <pios_helpers.h>
/* Rx/Tx status */
static uint8_t transfer_possible = 0;
static volatile uint8_t transfer_possible = 0;
#ifdef PIOS_INCLUDE_FREERTOS
static void(*disconnection_cb_list[3]) (void);
@ -148,6 +148,9 @@ out_fail:
*/
int32_t PIOS_USB_ChangeConnectionState(bool connected)
{
#ifdef PIOS_INCLUDE_FREERTOS
static volatile uint8_t lastStatus = 2; // 2 is "no last status"
#endif
// In all cases: re-initialise USB HID driver
if (connected) {
transfer_possible = 1;
@ -168,6 +171,12 @@ int32_t PIOS_USB_ChangeConnectionState(bool connected)
#ifdef PIOS_INCLUDE_FREERTOS
raiseConnectionStateCallback(connected);
if (lastStatus != transfer_possible) {
if (lastStatus == 1) {
raiseDisconnectionCallbacks();
}
lastStatus = transfer_possible;
}
#endif
return 0;
@ -187,28 +196,19 @@ bool PIOS_USB_CheckAvailable(__attribute__((unused)) uint32_t id)
return false;
}
usb_found = ((usb_dev->cfg->vsense.gpio->IDR & usb_dev->cfg->vsense.init.GPIO_Pin) != 0) ^ usb_dev->cfg->vsense_active_low;
// Please note that checks of transfer_possible and the reconnection handling is
// suppressed for non freertos mode (aka bootloader) as this is causing problems detecting connection and
// broken communications.
#ifdef PIOS_INCLUDE_FREERTOS
static bool lastStatus = false;
bool status = usb_found != 0 && transfer_possible ? 1 : 0;
bool reconnect = false;
if (xSemaphoreTakeFromISR(usb_dev->statusCheckSemaphore, NULL) == pdTRUE) {
reconnect = (lastStatus && !status);
lastStatus = status;
xSemaphoreGiveFromISR(usb_dev->statusCheckSemaphore, NULL);
}
if (reconnect) {
raiseDisconnectionCallbacks();
}
return status;
return transfer_possible;
}
#else
return usb_found;
/**
* This function returns wether a USB cable (5V pin) has been detected
* \return true: cable connected
* \return false: cable not detected (no cable or cable with no power)
*/
bool PIOS_USB_CableConnected(__attribute__((unused)) uint8_t id)
{
struct pios_usb_dev *usb_dev = (struct pios_usb_dev *)pios_usb_id;
#endif
return ((usb_dev->cfg->vsense.gpio->IDR & usb_dev->cfg->vsense.init.GPIO_Pin) != 0) ^ usb_dev->cfg->vsense_active_low;
}
/*

7
flight/pios/stm32f4xx/pios_usb_cdc.c
View File

@ -191,7 +191,7 @@ int32_t PIOS_USB_CDC_Init(uint32_t *usbcdc_id, const struct pios_usb_cdc_cfg *cf
pios_usb_cdc_id = (uint32_t)usb_cdc_dev;
/* Tx is not active yet */
/* Tx and Rx are not active yet */
usb_cdc_dev->tx_active = false;
/* Clear stats */
@ -607,6 +607,11 @@ static void PIOS_USB_CDC_DATA_IF_Init(uint32_t usb_cdc_id)
PIOS_USB_CDC_DATA_EP_OUT_Callback,
(uint32_t)usb_cdc_dev);
usb_cdc_dev->usb_data_if_enabled = true;
usb_cdc_dev->tx_active = false;
/* Activate rx prophylactically */
PIOS_USBHOOK_EndpointRx(usb_cdc_dev->cfg->data_rx_ep,
usb_cdc_dev->rx_packet_buffer,
sizeof(usb_cdc_dev->rx_packet_buffer));
}
static void PIOS_USB_CDC_DATA_IF_DeInit(uint32_t usb_cdc_id)

25
flight/pios/stm32f4xx/pios_usbhook.c
View File

@ -280,7 +280,9 @@ static USBD_DEVICE device_callbacks = {
static void PIOS_USBHOOK_USR_Init(void)
{
PIOS_USB_ChangeConnectionState(false);
reconnect();
// reconnect dev logically on init (previously a call to reconnect())
DCD_DevDisconnect(&pios_usb_otg_core_handle);
DCD_DevConnect(&pios_usb_otg_core_handle);
}
static void PIOS_USBHOOK_USR_DeviceReset(__attribute__((unused)) uint8_t speed)
@ -491,9 +493,24 @@ static USBD_Class_cb_TypeDef class_callbacks = {
static void reconnect(void)
{
/* Force a physical disconnect/reconnect */
DCD_DevDisconnect(&pios_usb_otg_core_handle);
DCD_DevConnect(&pios_usb_otg_core_handle);
static volatile bool in_reconnect = false;
/* Force a complete device reset. This can trigger a call to reconnect() so prevent recursion */
if (!in_reconnect) {
in_reconnect = true; // save since volatile and STM32F4 is single core
// disable USB device
DCD_DevDisconnect(&pios_usb_otg_core_handle);
USBD_DeInit(&pios_usb_otg_core_handle);
USB_OTG_StopDevice(&pios_usb_otg_core_handle);
// enable USB device
USBD_Init(&pios_usb_otg_core_handle,
USB_OTG_FS_CORE_ID,
&device_callbacks,
&class_callbacks,
&user_callbacks);
in_reconnect = false;
}
}
#endif /* PIOS_INCLUDE_USB */

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

@ -161,7 +161,6 @@
#define IDLE_COUNTS_PER_SEC_AT_NO_LOAD 1995998
/* Alarm Thresholds */
#define HEAP_LIMIT_WARNING 220
#define HEAP_LIMIT_CRITICAL 40
#define IRQSTACK_LIMIT_WARNING 100
#define IRQSTACK_LIMIT_CRITICAL 60
@ -175,10 +174,13 @@
#define PIOS_STABILIZATION_STACK_SIZE 400
#ifdef DIAG_TASKS
#define PIOS_SYSTEM_STACK_SIZE 760
#define PIOS_SYSTEM_STACK_SIZE 880
#define HEAP_LIMIT_WARNING 150
#else
#define PIOS_SYSTEM_STACK_SIZE 660
#define PIOS_SYSTEM_STACK_SIZE 700
#define HEAP_LIMIT_WARNING 220
#endif
#define PIOS_TELEM_RX_STACK_SIZE 410
#define PIOS_TELEM_TX_STACK_SIZE 560
#define PIOS_EVENTDISPATCHER_STACK_SIZE 95

2
flight/targets/boards/discoveryf4bare/bootloader/main.c
View File

@ -82,7 +82,7 @@ int main()
// is 2.7 volts
check_bor();
USB_connected = PIOS_USB_CheckAvailable(0);
USB_connected = PIOS_USB_CableConnected(0);
if (PIOS_IAP_CheckRequest() == true) {
PIOS_DELAY_WaitmS(1000);

5
flight/targets/boards/discoveryf4bare/firmware/UAVObjects.inc
View File

@ -106,6 +106,11 @@ UAVOBJSRCFILENAMES += velocitydesired
UAVOBJSRCFILENAMES += watchdogstatus
UAVOBJSRCFILENAMES += flightstatus
UAVOBJSRCFILENAMES += hwsettings
UAVOBJSRCFILENAMES += hwdiscoveryf4baresettings
UAVOBJSRCFILENAMES += hwpikoblxsettings
UAVOBJSRCFILENAMES += hwspracingf3evosettings
UAVOBJSRCFILENAMES += hwspracingf3settings
UAVOBJSRCFILENAMES += hwtinyfishsettings
UAVOBJSRCFILENAMES += receiveractivity
UAVOBJSRCFILENAMES += receiverstatus
UAVOBJSRCFILENAMES += cameradesired

30
flight/targets/boards/discoveryf4bare/firmware/pios_board.c
View File

@ -33,6 +33,8 @@
#include <taskinfo.h>
#include <pios_ws2811.h>
#include "hwdiscoveryf4baresettings.h"
#include "firmwareiapobj.h"
#ifdef PIOS_INCLUDE_INSTRUMENTATION
#include <pios_instrumentation.h>
@ -58,6 +60,19 @@ uintptr_t pios_user_fs_id;
uint32_t pios_ws2811_id;
#endif
static HwDiscoveryF4BareSettingsData boardHwSettings;
static void hwDiscoveryF4BareSettingsUpdatedCb(__attribute__((unused)) UAVObjEvent *ev)
{
HwDiscoveryF4BareSettingsData currentBoardHwSettings;
HwDiscoveryF4BareSettingsGet(&currentBoardHwSettings);
if (memcmp(&currentBoardHwSettings, &boardHwSettings, sizeof(HwDiscoveryF4BareSettingsData)) != 0) {
ExtendedAlarmsSet(SYSTEMALARMS_ALARM_BOOTFAULT, SYSTEMALARMS_ALARM_CRITICAL, SYSTEMALARMS_EXTENDEDALARMSTATUS_REBOOTREQUIRED, 0);
}
}
static const PIOS_BOARD_IO_UART_Function flexiio_function_map[] = {
[HWSETTINGS_RM_RCVRPORT_PPMTELEMETRY] = PIOS_BOARD_IO_UART_TELEMETRY,
[HWSETTINGS_RM_RCVRPORT_PPMDEBUGCONSOLE] = PIOS_BOARD_IO_UART_DEBUGCONSOLE,
@ -189,6 +204,21 @@ void PIOS_Board_Init(void)
UAVObjInitialize();
SETTINGS_INITIALISE_ALL;
HwDiscoveryF4BareSettingsConnectCallback(hwDiscoveryF4BareSettingsUpdatedCb);
HwDiscoveryF4BareSettingsGet(&boardHwSettings);
if (boardHwSettings.BoardType != 0) {
FirmwareIAPObjInitialize();
FirmwareIAPObjData iap;
FirmwareIAPObjGet(&iap);
iap.BoardType = boardHwSettings.BoardType;
iap.BoardRevision = boardHwSettings.BoardRevision;
FirmwareIAPObjSet(&iap);
}
/* Initialize the alarms library */
AlarmsInitialize();

2
flight/targets/boards/osd/bootloader/main.c
View File

@ -76,7 +76,7 @@ int main()
PIOS_Board_Init();
PIOS_IAP_Init();
USB_connected = PIOS_USB_CheckAvailable(0);
USB_connected = PIOS_USB_CableConnected(0);
if (PIOS_IAP_CheckRequest() == true) {
PIOS_DELAY_WaitmS(1000);

2
flight/targets/boards/revolution/bootloader/main.c
View File

@ -82,7 +82,7 @@ int main()
// is 2.7 volts
check_bor();
USB_connected = PIOS_USB_CheckAvailable(0);
USB_connected = PIOS_USB_CableConnected(0);
if (PIOS_IAP_CheckRequest() == true) {
PIOS_DELAY_WaitmS(1000);

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

@ -54,6 +54,7 @@ MODULES += Telemetry
MODULES += Notify
OPTMODULES += AutoTune
OPTMODULES += Temperature
OPTMODULES += ComUsbBridge
OPTMODULES += UAVOHottBridge
OPTMODULES += UAVOMSPBridge

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

@ -122,6 +122,8 @@ UAVOBJSRCFILENAMES += waypointactive
UAVOBJSRCFILENAMES += poilocation
UAVOBJSRCFILENAMES += poilearnsettings
UAVOBJSRCFILENAMES += mpugyroaccelsettings
UAVOBJSRCFILENAMES += temperaturesettings
UAVOBJSRCFILENAMES += temperaturestate
UAVOBJSRCFILENAMES += txpidsettings
UAVOBJSRCFILENAMES += txpidstatus
UAVOBJSRCFILENAMES += hottbridgesettings

1
flight/targets/boards/revolution/firmware/inc/pios_config.h
View File

@ -139,6 +139,7 @@
#define PIOS_INCLUDE_RFM22B
#define PIOS_INCLUDE_RFM22B_COM
#define PIOS_INCLUDE_OPENLRS
/* #define OPENLRS_SIMPLE_BEACON_TONE */
/* #define PIOS_INCLUDE_PPM_OUT */
/* #define PIOS_RFM22B_DEBUG_ON_TELEM */

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

@ -62,17 +62,19 @@ static const PIOS_BOARD_IO_UART_Function flexiio_function_map[] = {
[HWSETTINGS_RM_RCVRPORT_PPMTELEMETRY] = PIOS_BOARD_IO_UART_TELEMETRY,
[HWSETTINGS_RM_RCVRPORT_PPMDEBUGCONSOLE] = PIOS_BOARD_IO_UART_DEBUGCONSOLE,
[HWSETTINGS_RM_RCVRPORT_PPMCOMBRIDGE] = PIOS_BOARD_IO_UART_COMBRIDGE,
[HWSETTINGS_RM_RCVRPORT_PPMMSP] = PIOS_BOARD_IO_UART_MSP,
[HWSETTINGS_RM_RCVRPORT_PPMMAVLINK] = PIOS_BOARD_IO_UART_MAVLINK,
[HWSETTINGS_RM_RCVRPORT_PPMGPS] = PIOS_BOARD_IO_UART_GPS,
[HWSETTINGS_RM_RCVRPORT_TELEMETRY] = PIOS_BOARD_IO_UART_TELEMETRY,
[HWSETTINGS_RM_RCVRPORT_DEBUGCONSOLE] = PIOS_BOARD_IO_UART_DEBUGCONSOLE,
[HWSETTINGS_RM_RCVRPORT_COMBRIDGE] = PIOS_BOARD_IO_UART_COMBRIDGE,
[HWSETTINGS_RM_RCVRPORT_PPMMSP] = PIOS_BOARD_IO_UART_MSP,
[HWSETTINGS_RM_RCVRPORT_PPMMAVLINK] = PIOS_BOARD_IO_UART_MAVLINK,
[HWSETTINGS_RM_RCVRPORT_PPMGPS] = PIOS_BOARD_IO_UART_GPS,
[HWSETTINGS_RM_RCVRPORT_PPMHOTTTELEMETRY] = PIOS_BOARD_IO_UART_HOTT_BRIDGE,
[HWSETTINGS_RM_RCVRPORT_TELEMETRY] = PIOS_BOARD_IO_UART_TELEMETRY,
[HWSETTINGS_RM_RCVRPORT_DEBUGCONSOLE] = PIOS_BOARD_IO_UART_DEBUGCONSOLE,
[HWSETTINGS_RM_RCVRPORT_COMBRIDGE] = PIOS_BOARD_IO_UART_COMBRIDGE,
[HWSETTINGS_RM_RCVRPORT_MSP] = PIOS_BOARD_IO_UART_MSP,
[HWSETTINGS_RM_RCVRPORT_MAVLINK] = PIOS_BOARD_IO_UART_MAVLINK,
[HWSETTINGS_RM_RCVRPORT_MAVLINK] = PIOS_BOARD_IO_UART_MAVLINK,
[HWSETTINGS_RM_RCVRPORT_GPS] = PIOS_BOARD_IO_UART_GPS,
[HWSETTINGS_RM_RCVRPORT_IBUS] = PIOS_BOARD_IO_UART_IBUS,
[HWSETTINGS_RM_RCVRPORT_FRSKYSENSORHUB] = PIOS_BOARD_IO_UART_FRSKY_SENSORHUB,
[HWSETTINGS_RM_RCVRPORT_FRSKYSENSORHUB] = PIOS_BOARD_IO_UART_FRSKY_SENSORHUB,
[HWSETTINGS_RM_RCVRPORT_HOTTTELEMETRY] = PIOS_BOARD_IO_UART_HOTT_BRIDGE,
};
static const PIOS_BOARD_IO_UART_Function main_function_map[] = {
@ -86,6 +88,7 @@ static const PIOS_BOARD_IO_UART_Function main_function_map[] = {
[HWSETTINGS_RM_MAINPORT_MSP] = PIOS_BOARD_IO_UART_MSP,
[HWSETTINGS_RM_MAINPORT_MAVLINK] = PIOS_BOARD_IO_UART_MAVLINK,
[HWSETTINGS_RM_MAINPORT_FRSKYSENSORHUB] = PIOS_BOARD_IO_UART_FRSKY_SENSORHUB,
[HWSETTINGS_RM_MAINPORT_HOTTTELEMETRY] = PIOS_BOARD_IO_UART_HOTT_BRIDGE,
};
static const PIOS_BOARD_IO_UART_Function flexi_function_map[] = {
@ -103,6 +106,7 @@ static const PIOS_BOARD_IO_UART_Function flexi_function_map[] = {
[HWSETTINGS_RM_FLEXIPORT_MSP] = PIOS_BOARD_IO_UART_MSP,
[HWSETTINGS_RM_FLEXIPORT_MAVLINK] = PIOS_BOARD_IO_UART_MAVLINK,
[HWSETTINGS_RM_FLEXIPORT_FRSKYSENSORHUB] = PIOS_BOARD_IO_UART_FRSKY_SENSORHUB,
[HWSETTINGS_RM_FLEXIPORT_HOTTTELEMETRY] = PIOS_BOARD_IO_UART_HOTT_BRIDGE,
};
/**

2
flight/targets/boards/revolution/pios_board.h
View File

@ -303,7 +303,7 @@ extern uint32_t pios_packet_handler;
{ GPIOA, GPIO_Pin_3, ADC_Channel_3, false }, /* Servo pin 3 */ \
{ GPIOA, GPIO_Pin_2, ADC_Channel_2, false }, /* Servo pin 4 */ \
{ GPIOA, GPIO_Pin_1, ADC_Channel_1, false }, /* Servo pin 5 */ \
{ GPIOA, GPIO_Pin_0, ADC_Channel_9, false }, /* Servo pin 6 */ \
{ GPIOA, GPIO_Pin_0, ADC_Channel_0, false }, /* Servo pin 6 */ \
{ NULL, 0, ADC_Channel_Vrefint, false }, /* Voltage reference */ \
{ NULL, 0, ADC_Channel_TempSensor, false }, /* Temperature sensor */ \
}

2
flight/targets/boards/revonano/bootloader/main.c
View File

@ -82,7 +82,7 @@ int main()
// is 2.7 volts
check_bor();
USB_connected = PIOS_USB_CheckAvailable(0);
USB_connected = PIOS_USB_CableConnected(0);
if (PIOS_IAP_CheckRequest() == true) {
PIOS_DELAY_WaitmS(1000);

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

@ -53,10 +53,12 @@ MODULES += Telemetry
MODULES += Notify
OPTMODULES += AutoTune
OPTMODULES += Temperature
OPTMODULES += ComUsbBridge
OPTMODULES += UAVOMSPBridge
OPTMODULES += UAVOMavlinkBridge
OPTMODULES += UAVOFrSKYSensorHubBridge
OPTMODULES += UAVOHottBridge
SRC += $(FLIGHTLIB)/notification.c

4
flight/targets/boards/revonano/firmware/UAVObjects.inc
View File

@ -122,8 +122,12 @@ UAVOBJSRCFILENAMES += waypointactive
UAVOBJSRCFILENAMES += poilocation
UAVOBJSRCFILENAMES += poilearnsettings
UAVOBJSRCFILENAMES += mpugyroaccelsettings
UAVOBJSRCFILENAMES += temperaturesettings
UAVOBJSRCFILENAMES += temperaturestate
UAVOBJSRCFILENAMES += txpidsettings
UAVOBJSRCFILENAMES += txpidstatus
UAVOBJSRCFILENAMES += hottbridgesettings
UAVOBJSRCFILENAMES += hottbridgestatus
UAVOBJSRCFILENAMES += takeofflocation
UAVOBJSRCFILENAMES += perfcounter
UAVOBJSRCFILENAMES += systemidentsettings

1
flight/targets/boards/revonano/firmware/inc/pios_config.h
View File

@ -123,6 +123,7 @@
#define PIOS_INCLUDE_LED
#define PIOS_INCLUDE_IAP
#define PIOS_INCLUDE_SERVO
#define PIOS_INCLUDE_HOTT_BRIDGE
/* #define PIOS_INCLUDE_I2C_ESC */
/* #define PIOS_INCLUDE_OVERO */
/* #define PIOS_OVERO_SPI */

11
flight/targets/boards/revonano/firmware/pios_board.c
View File

@ -86,6 +86,7 @@ static const PIOS_BOARD_IO_UART_Function flexi_function_map[] = {
[HWSETTINGS_RM_FLEXIPORT_MSP] = PIOS_BOARD_IO_UART_MSP,
[HWSETTINGS_RM_FLEXIPORT_MAVLINK] = PIOS_BOARD_IO_UART_MAVLINK,
[HWSETTINGS_RM_FLEXIPORT_FRSKYSENSORHUB] = PIOS_BOARD_IO_UART_FRSKY_SENSORHUB,
[HWSETTINGS_RM_FLEXIPORT_HOTTTELEMETRY] = PIOS_BOARD_IO_UART_HOTT_BRIDGE,
};
static const PIOS_BOARD_IO_UART_Function main_function_map[] = {
@ -98,6 +99,7 @@ static const PIOS_BOARD_IO_UART_Function main_function_map[] = {
[HWSETTINGS_RM_MAINPORT_OSDHK] = PIOS_BOARD_IO_UART_OSDHK,
[HWSETTINGS_RM_MAINPORT_MSP] = PIOS_BOARD_IO_UART_MSP,
[HWSETTINGS_RM_MAINPORT_FRSKYSENSORHUB] = PIOS_BOARD_IO_UART_FRSKY_SENSORHUB,
[HWSETTINGS_RM_MAINPORT_HOTTTELEMETRY] = PIOS_BOARD_IO_UART_HOTT_BRIDGE,
};
void PIOS_Board_Init(void)
@ -207,6 +209,15 @@ void PIOS_Board_Init(void)
PIOS_BOARD_IO_Configure_UART(&pios_usart_flexi_cfg, flexi_function_map[hwsettings_flexiport]);
}
#if defined(PIOS_INCLUDE_I2C)
if (hwsettings_flexiport == HWSETTINGS_RM_FLEXIPORT_I2C) {
if (PIOS_I2C_Init(&pios_i2c_flexiport_adapter_id, &pios_i2c_flexiport_adapter_cfg)) {
PIOS_Assert(0);
}
}
#endif
/* Configure MainPort */
uint8_t hwsettings_mainport;
HwSettingsRM_MainPortGet(&hwsettings_mainport);

2
flight/targets/boards/revoproto/bootloader/main.c
View File

@ -82,7 +82,7 @@ int main()
// is 2.7 volts
check_bor();
USB_connected = PIOS_USB_CheckAvailable(0);
USB_connected = PIOS_USB_CableConnected(0);
if (PIOS_IAP_CheckRequest() == true) {
PIOS_DELAY_WaitmS(1000);

2
flight/targets/boards/sparky2/bootloader/main.c
View File

@ -83,7 +83,7 @@ int main()
// is 2.7 volts
check_bor();
USB_connected = PIOS_USB_CheckAvailable(0);
USB_connected = PIOS_USB_CableConnected(0);
if (PIOS_IAP_CheckRequest() == true) {
PIOS_DELAY_WaitmS(1000);

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

@ -54,7 +54,9 @@ MODULES += Telemetry
MODULES += Notify
OPTMODULES += AutoTune
OPTMODULES += Temperature
OPTMODULES += ComUsbBridge
OPTMODULES += UAVOHottBridge
OPTMODULES += UAVOMSPBridge
OPTMODULES += UAVOMavlinkBridge
OPTMODULES += UAVOFrSKYSensorHubBridge

4
flight/targets/boards/sparky2/firmware/UAVObjects.inc
View File

@ -122,8 +122,12 @@ UAVOBJSRCFILENAMES += waypointactive
UAVOBJSRCFILENAMES += poilocation
UAVOBJSRCFILENAMES += poilearnsettings
UAVOBJSRCFILENAMES += mpugyroaccelsettings
UAVOBJSRCFILENAMES += temperaturesettings
UAVOBJSRCFILENAMES += temperaturestate
UAVOBJSRCFILENAMES += txpidsettings
UAVOBJSRCFILENAMES += txpidstatus
UAVOBJSRCFILENAMES += hottbridgesettings
UAVOBJSRCFILENAMES += hottbridgestatus
UAVOBJSRCFILENAMES += takeofflocation
UAVOBJSRCFILENAMES += perfcounter
UAVOBJSRCFILENAMES += systemidentsettings

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

@ -124,6 +124,7 @@
#define PIOS_INCLUDE_LED
#define PIOS_INCLUDE_IAP
#define PIOS_INCLUDE_SERVO
#define PIOS_INCLUDE_HOTT_BRIDGE
/* #define PIOS_INCLUDE_I2C_ESC */
/* #define PIOS_INCLUDE_OVERO */
/* #define PIOS_OVERO_SPI */
@ -142,6 +143,7 @@
#define PIOS_INCLUDE_RFM22B
#define PIOS_INCLUDE_RFM22B_COM
#define PIOS_INCLUDE_OPENLRS
/* #define OPENLRS_SIMPLE_BEACON_TONE */
/* #define PIOS_INCLUDE_PPM_OUT */
/* #define PIOS_RFM22B_DEBUG_ON_TELEM */

1
flight/targets/boards/sparky2/firmware/pios_board.c
View File

@ -97,6 +97,7 @@ static const PIOS_BOARD_IO_UART_Function flexi_function_map[] = {
[HWSETTINGS_SPK2_FLEXIPORT_MSP] = PIOS_BOARD_IO_UART_MSP,
[HWSETTINGS_SPK2_FLEXIPORT_MAVLINK] = PIOS_BOARD_IO_UART_MAVLINK,
[HWSETTINGS_SPK2_FLEXIPORT_FRSKYSENSORHUB] = PIOS_BOARD_IO_UART_FRSKY_SENSORHUB,
[HWSETTINGS_SPK2_FLEXIPORT_HOTTTELEMETRY] = PIOS_BOARD_IO_UART_HOTT_BRIDGE,
};
/**

2
flight/targets/boards/sparky2/pios_board.h
View File

@ -306,7 +306,7 @@ extern uint32_t pios_packet_handler;
{ GPIOA, GPIO_Pin_3, ADC_Channel_3, false }, /* Servo pin 3 */ \
{ GPIOA, GPIO_Pin_2, ADC_Channel_2, false }, /* Servo pin 4 */ \
{ GPIOA, GPIO_Pin_1, ADC_Channel_1, false }, /* Servo pin 5 */ \
{ GPIOA, GPIO_Pin_0, ADC_Channel_9, false }, /* Servo pin 6 */ \
{ GPIOA, GPIO_Pin_0, ADC_Channel_0, false }, /* Servo pin 6 */ \
{ NULL, 0, ADC_Channel_Vrefint, false }, /* Voltage reference */ \
{ NULL, 0, ADC_Channel_TempSensor, false }, /* Temperature sensor */ \
}

1
flight/uavobjects/inc/uavobject.h.template
View File

@ -40,6 +40,7 @@
#ifndef $(NAMEUC)_H
#define $(NAMEUC)_H
#include <stdbool.h>
$(INCLUDE)
/* Object constants */
#define $(NAMEUC)_OBJID $(OBJIDHEX)
#define $(NAMEUC)_ISSINGLEINST $(ISSINGLEINST)

4
ground/gcs/src/libs/gstreamer/copydata.pro
View File

@ -38,7 +38,7 @@ win32:gstreamer {
libgstautodetect.dll \
libgstavi.dll \
libgstdeinterlace.dll \
libgstdirectsoundsink.dll \
libgstdirectsound.dll \
libgstimagefreeze.dll \
libgstjpeg.dll \
libgstrawparse.dll \
@ -53,7 +53,7 @@ win32:gstreamer {
libgstaudiovisualizers.dll \
libgstautoconvert.dll \
libgstcompositor.dll \
libgstd3dvideosink.dll \
libgstd3d.dll \
libgstdebugutilsbad.dll \
libgstdirectsoundsrc.dll \
libgstopengl.dll \

2
ground/gcs/src/libs/opmapcontrol/src/core/providerstrings.cpp
View File

@ -41,7 +41,7 @@ ProviderStrings::ProviderStrings()
{
// Google version strings
VersionGoogleMap = "m@301";
QString version = "713";
QString version = "823";
QString envVersion = qgetenv("GCS_GOOGLE_SAT_VERSION").constData();
VersionGoogleSatellite = (envVersion.toInt() > version.toInt()) ? envVersion : version;
VersionGoogleLabels = "h@301";

2
ground/gcs/src/libs/opmapcontrol/src/core/urlfactory.cpp
View File

@ -214,7 +214,7 @@ QString UrlFactory::MakeImageUrl(const MapType::Types &type, const Point &pos, c
break;
case MapType::GoogleSatellite:
{
QString server = "khm";
QString server = "khms";
QString request = "kh";
QString sec1 = ""; // after &x=...
QString sec2 = ""; // after &zoom=...

13
ground/gcs/src/libs/opmapcontrol/src/mapwidget/homeitem.cpp
View File

@ -73,7 +73,12 @@ QRectF HomeItem::boundingRect() const
if (pic.width() > localsafearea * 2 && !toggleRefresh) {
return QRectF(-pic.width() / 2, -pic.height() / 2, pic.width(), pic.height());
} else {
return QRectF(-localsafearea, -localsafearea, localsafearea * 2, localsafearea * 2);
// FIXME: LP-573 For some reason when boundingRect is defined normally,
// return QRectF(-localsafearea, -localsafearea, localsafearea * 2, localsafearea * 2);
// the previous safearea circle and Home (both at previous/smaller size) still drawed
// into the corner of boudingRect area when zoom in.
// Current workaround is done expanding x100 the boundingRect returned.
return QRectF(-localsafearea * 100, -localsafearea * 100, localsafearea * 200, localsafearea * 200);
}
}
@ -98,6 +103,12 @@ void HomeItem::RefreshPos()
toggleRefresh = false;
}
void HomeItem::SetHomePic(QString HomePic)
{
pic.load(":/markers/images/" + HomePic);
pic = pic.scaled(30, 30, Qt::IgnoreAspectRatio);
}
void HomeItem::setOpacitySlot(qreal opacity)
{
setOpacity(opacity);

1
ground/gcs/src/libs/opmapcontrol/src/mapwidget/homeitem.h
View File

@ -102,6 +102,7 @@ protected:
QPainterPath shape() const;
public slots:
void RefreshPos();
void SetHomePic(QString HomePic);
void setOpacitySlot(qreal opacity);
signals:
void homePositionChanged(internals::PointLatLng coord, float);

BIN
ground/gcs/src/libs/opmapcontrol/src/mapwidget/images/airplane.png
View File

Binary file not shown.
Side by Side Swipe Overlay

Before

Width:  |  Height:  |  Size: 1.7 KiB

After

Width:  |  Height:  |  Size: 1.7 KiB

BIN
ground/gcs/src/libs/opmapcontrol/src/mapwidget/images/airplanepip.png
View File

Binary file not shown.
Side by Side Swipe Overlay

Before

Width:  |  Height:  |  Size: 13 KiB

After

Width:  |  Height:  |  Size: 3.2 KiB

80
ground/gcs/src/libs/opmapcontrol/src/mapwidget/images/home2_not_set.svg Normal file
View File

@ -0,0 +1,80 @@
<?xml version="1.0" encoding="UTF-8" standalone="no"?>
<!-- Created with Inkscape (http://www.inkscape.org/) -->
<svg
xmlns:dc="http://purl.org/dc/elements/1.1/"
xmlns:cc="http://creativecommons.org/ns#"
xmlns:rdf="http://www.w3.org/1999/02/22-rdf-syntax-ns#"
xmlns:svg="http://www.w3.org/2000/svg"
xmlns="http://www.w3.org/2000/svg"
xmlns:sodipodi="http://sodipodi.sourceforge.net/DTD/sodipodi-0.dtd"
xmlns:inkscape="http://www.inkscape.org/namespaces/inkscape"
width="463.89285"
height="438.87704"
id="svg2"
sodipodi:version="0.32"
inkscape:version="0.46"
version="1.0"
sodipodi:docname="dynaHouse.svg"
inkscape:output_extension="org.inkscape.output.svg.inkscape">
<defs
id="defs4">
<inkscape:perspective
sodipodi:type="inkscape:persp3d"
inkscape:vp_x="0 : 526.18109 : 1"
inkscape:vp_y="0 : 1000 : 0"
inkscape:vp_z="744.09448 : 526.18109 : 1"
inkscape:persp3d-origin="372.04724 : 350.78739 : 1"
id="perspective10" />
</defs>
<sodipodi:namedview
id="base"
pagecolor="#ffffff"
bordercolor="#666666"
borderopacity="1.0"
gridtolerance="10000"
guidetolerance="10"
objecttolerance="10"
inkscape:pageopacity="0.0"
inkscape:pageshadow="2"
inkscape:zoom="1.4098083"
inkscape:cx="231.94643"
inkscape:cy="219.43852"
inkscape:document-units="px"
inkscape:current-layer="layer1"
showgrid="false"
inkscape:window-width="1006"
inkscape:window-height="954"
inkscape:window-x="660"
inkscape:window-y="43" />
<metadata
id="metadata7">
<rdf:RDF>
<cc:Work
rdf:about="">
<dc:format>image/svg+xml</dc:format>
<dc:type
rdf:resource="http://purl.org/dc/dcmitype/StillImage" />
</cc:Work>
</rdf:RDF>
</metadata>
<g
inkscape:label="Layer 1"
inkscape:groupmode="layer"
id="layer1"
transform="translate(-42.339286,-276.34171)">
<path
style="opacity:1;fill:#000000;fill-opacity:1;stroke:none;stroke-width:0.40000001;stroke-miterlimit:4;stroke-dasharray:none;stroke-opacity:1"
d="M 437.14509,499.4375 L 437.14509,499.4375 z M 437.14509,499.4375 L 274.33259,355.25 L 111.42634,499.5 L 111.42634,705.625 C 111.42634,710.94838 115.72796,715.21875 121.05134,715.21875 L 222.86384,715.21875 L 222.86384,624.84375 C 222.86384,619.52035 227.13421,615.21875 232.45759,615.21875 L 316.11384,615.21875 C 321.43719,615.21875 325.70759,619.52037 325.70759,624.84375 L 325.70759,715.21875 L 427.55134,715.21875 C 432.87471,715.21875 437.14511,710.9484 437.14509,705.625 L 437.14509,499.4375 z M 111.42634,499.5 L 111.42634,499.5 z"
id="rect2391"
sodipodi:nodetypes="cccccccccccccccccc" />
<path
style="opacity:1;fill:#000000;fill-opacity:1;stroke:none;stroke-width:0.40000001;stroke-miterlimit:4;stroke-dasharray:none;stroke-opacity:1"
d="M 273.3878,276.34171 L 42.339286,480.92527 L 66.677596,508.38266 L 274.33298,324.49848 L 481.9411,508.38266 L 506.23215,480.92527 L 275.2309,276.34171 L 274.33298,277.3814 L 273.3878,276.34171 z"
id="path2399" />
<path
style="fill:#000000;fill-opacity:1;stroke:none;stroke-width:0.40000001;stroke-miterlimit:4;stroke-opacity:1"
d="M 111.42634,305.79074 L 169.99777,305.79074 L 169.48739,340.48183 L 111.42634,392.9336 L 111.42634,305.79074 z"
id="rect2404"
sodipodi:nodetypes="ccccc" />
</g>
</svg>
Side by Side

After

Width:  |  Height:  |  Size: 3.4 KiB

93
ground/gcs/src/libs/opmapcontrol/src/mapwidget/images/home2_set.svg Normal file
View File

@ -0,0 +1,93 @@
<?xml version="1.0" encoding="UTF-8" standalone="no"?>
<!-- Created with Inkscape (http://www.inkscape.org/) -->
<svg
xmlns:dc="http://purl.org/dc/elements/1.1/"
xmlns:cc="http://creativecommons.org/ns#"
xmlns:rdf="http://www.w3.org/1999/02/22-rdf-syntax-ns#"
xmlns:svg="http://www.w3.org/2000/svg"
xmlns="http://www.w3.org/2000/svg"
xmlns:sodipodi="http://sodipodi.sourceforge.net/DTD/sodipodi-0.dtd"
xmlns:inkscape="http://www.inkscape.org/namespaces/inkscape"
width="463.89285"
height="438.87704"
id="svg2"
sodipodi:version="0.32"
inkscape:version="0.91 r13725"
version="1.0"
sodipodi:docname="home2_set.svg"
inkscape:output_extension="org.inkscape.output.svg.inkscape">
<defs
id="defs4">
<inkscape:perspective
sodipodi:type="inkscape:persp3d"
inkscape:vp_x="0 : 526.18109 : 1"
inkscape:vp_y="0 : 1000 : 0"
inkscape:vp_z="744.09448 : 526.18109 : 1"
inkscape:persp3d-origin="372.04724 : 350.78739 : 1"
id="perspective10" />
</defs>
<sodipodi:namedview
id="base"
pagecolor="#ffffff"
bordercolor="#666666"
borderopacity="1.0"
gridtolerance="10000"
guidetolerance="10"
objecttolerance="10"
inkscape:pageopacity="0.0"
inkscape:pageshadow="2"
inkscape:zoom="1.2804681"
inkscape:cx="231.94643"
inkscape:cy="219.43852"
inkscape:document-units="px"
inkscape:current-layer="layer1"
showgrid="false"
inkscape:window-width="1920"
inkscape:window-height="1016"
inkscape:window-x="0"
inkscape:window-y="27"
inkscape:window-maximized="1"
inkscape:snap-bbox="true"
inkscape:bbox-nodes="true"
inkscape:snap-bbox-edge-midpoints="true" />
<metadata
id="metadata7">
<rdf:RDF>
<cc:Work
rdf:about="">
<dc:format>image/svg+xml</dc:format>
<dc:type
rdf:resource="http://purl.org/dc/dcmitype/StillImage" />
<dc:title />
</cc:Work>
</rdf:RDF>
</metadata>
<g
inkscape:label="Layer 1"
inkscape:groupmode="layer"
id="layer1"
transform="translate(-42.339286,-276.34171)">
<rect
style="color:#000000;display:inline;overflow:visible;visibility:visible;opacity:1;fill:#00ff00;fill-opacity:1;fill-rule:nonzero;stroke:none;stroke-width:1.86600006;stroke-linecap:round;stroke-linejoin:round;stroke-miterlimit:4;stroke-dasharray:none;stroke-dashoffset:0;stroke-opacity:1;marker:none;enable-background:accumulate"
id="rect3345"
width="129.64009"
height="110.11597"
x="209.46567"
y="605.10278" />
<path
style="opacity:1;fill:#000000;fill-opacity:1;stroke:none;stroke-width:0.40000001;stroke-miterlimit:4;stroke-dasharray:none;stroke-opacity:1"
d="M 437.14509,499.4375 L 437.14509,499.4375 z M 437.14509,499.4375 L 274.33259,355.25 L 111.42634,499.5 L 111.42634,705.625 C 111.42634,710.94838 115.72796,715.21875 121.05134,715.21875 L 222.86384,715.21875 L 222.86384,624.84375 C 222.86384,619.52035 227.13421,615.21875 232.45759,615.21875 L 316.11384,615.21875 C 321.43719,615.21875 325.70759,619.52037 325.70759,624.84375 L 325.70759,715.21875 L 427.55134,715.21875 C 432.87471,715.21875 437.14511,710.9484 437.14509,705.625 L 437.14509,499.4375 z M 111.42634,499.5 L 111.42634,499.5 z"
id="rect2391"
sodipodi:nodetypes="cccccccccccccccccc" />
<path
style="opacity:1;fill:#000000;fill-opacity:1;stroke:none;stroke-width:0.40000001;stroke-miterlimit:4;stroke-dasharray:none;stroke-opacity:1"
d="M 273.3878,276.34171 L 42.339286,480.92527 L 66.677596,508.38266 L 274.33298,324.49848 L 481.9411,508.38266 L 506.23215,480.92527 L 275.2309,276.34171 L 274.33298,277.3814 L 273.3878,276.34171 z"
id="path2399" />
<path
style="fill:#000000;fill-opacity:1;stroke:none;stroke-width:0.40000001;stroke-miterlimit:4;stroke-opacity:1"
d="M 111.42634,305.79074 L 169.99777,305.79074 L 169.48739,340.48183 L 111.42634,392.9336 L 111.42634,305.79074 z"
id="rect2404"
sodipodi:nodetypes="ccccc" />
</g>
</svg>
Side by Side

After

Width:  |  Height:  |  Size: 4.0 KiB

BIN
ground/gcs/src/libs/opmapcontrol/src/mapwidget/images/marker.png
View File

Binary file not shown.
Side by Side

Before

Width:  |  Height:  |  Size: 858 B

BIN
ground/gcs/src/libs/opmapcontrol/src/mapwidget/images/waypoint_marker3.png
View File

Binary file not shown.
Side by Side

Before

Width:  |  Height:  |  Size: 1.6 KiB

0
ground/gcs/src/plugins/opmap/images/waypoint_marker3.png → ground/gcs/src/libs/opmapcontrol/src/mapwidget/images/wp_marker_green.png
View File

Side by Side Swipe Overlay

Before

Width:  |  Height:  |  Size: 1.7 KiB

After

Width:  |  Height:  |  Size: 1.7 KiB

0
ground/gcs/src/libs/opmapcontrol/src/mapwidget/images/waypoint_marker2.png → ground/gcs/src/libs/opmapcontrol/src/mapwidget/images/wp_marker_orange.png
View File

Side by Side Swipe Overlay

Before

Width:  |  Height:  |  Size: 4.3 KiB

After

Width:  |  Height:  |  Size: 4.3 KiB

0
ground/gcs/src/libs/opmapcontrol/src/mapwidget/images/waypoint_marker1.png → ground/gcs/src/libs/opmapcontrol/src/mapwidget/images/wp_marker_red.png
View File

Side by Side Swipe Overlay

Before

Width:  |  Height:  |  Size: 4.3 KiB

After

Width:  |  Height:  |  Size: 4.3 KiB

22
ground/gcs/src/libs/opmapcontrol/src/mapwidget/mapresources.qrc
View File

@ -1,25 +1,23 @@
<RCC>
<qresource prefix="/markers">
<file>images/bigMarkerGreen.png</file>
<file>images/marker.png</file>
<file>images/wp_marker_green.png</file>
<file>images/wp_marker_orange.png</file>
<file>images/wp_marker_red.png</file>
<file>images/compas.svg</file>
<file>images/airplane.svg</file>
<file>images/home2.svg</file>
<file>images/home2_not_set.svg</file>
<file>images/home2_set.svg</file>
<file>images/nav.svg</file>
<file>images/home.png</file>
<file>images/home.svg</file>
<file>images/nav.svg</file>
<file>images/home2.svg</file>
<file>images/airplanepip.png</file>
<file>images/EasystarBlue.png</file>
<file>images/mapquad.png</file>
<file>images/dragons1.jpg</file>
<file>images/dragons2.jpeg</file>
<file>images/waypoint_marker1.png</file>
<file>images/waypoint_marker2.png</file>
<file>images/waypoint_marker3.png</file>
</qresource>
<qresource prefix="/uavs">
<file>images/airplanepip.png</file>
<file>images/EasystarBlue.png</file>
<file>images/mapquad.png</file>
<file>images/airplanepip.png</file>
<file>images/airplane.png</file>
<file>images/EasystarBlue.png</file>
</qresource>
</RCC>

14
ground/gcs/src/libs/opmapcontrol/src/mapwidget/opmapwidget.cpp
View File

@ -107,6 +107,12 @@ void OPMapWidget::SetUavPic(QString UAVPic)
GPS->SetUavPic(UAVPic);
}
}
void OPMapWidget::SetHomePic(QString HomePic)
{
if (Home != 0) {
Home->SetHomePic(HomePic);
}
}
WayPointLine *OPMapWidget::WPLineCreate(WayPointItem *from, WayPointItem *to, QColor color, bool dashed, int width)
{
@ -334,11 +340,9 @@ WayPointItem *OPMapWidget::WPInsert(internals::PointLatLng const & coord, int co
WayPointItem *OPMapWidget::WPInsert(internals::PointLatLng const & coord, int const & altitude, QString const & description, const int &position)
{
internals::PointLatLng mcoord;
bool reloc = false;
if (mcoord == internals::PointLatLng(0, 0)) {
if (coord == internals::PointLatLng(0, 0)) {
mcoord = CurrentPosition();
reloc = true;
} else {
mcoord = coord;
}
@ -347,9 +351,7 @@ WayPointItem *OPMapWidget::WPInsert(internals::PointLatLng const & coord, int co
ConnectWP(item);
item->setParentItem(map);
emit WPInserted(position, item);
if (reloc) {
emit WPValuesChanged(item);
}
emit WPValuesChanged(item);
setOverlayOpacity(overlayOpacity);
return item;
}

1
ground/gcs/src/libs/opmapcontrol/src/mapwidget/opmapwidget.h
View File

@ -521,6 +521,7 @@ public:
}
void SetShowDiagnostics(bool const & value);
void SetUavPic(QString UAVPic);
void SetHomePic(QString HomePic);
WayPointLine *WPLineCreate(WayPointItem *from, WayPointItem *to, QColor color, bool dashed = false, int width = -1);
WayPointLine *WPLineCreate(HomeItem *from, WayPointItem *to, QColor color, bool dashed = false, int width = -1);
WayPointCircle *WPCircleCreate(WayPointItem *center, WayPointItem *radius, bool clockwise, QColor color, bool dashed = false, int width = -1);

26
ground/gcs/src/libs/opmapcontrol/src/mapwidget/waypointitem.cpp
View File

@ -29,13 +29,15 @@
#include "homeitem.h"
#include <QGraphicsSceneMouseEvent>
#define COORDINATES_THRESHOLD 0.000002 // ~21cm
namespace mapcontrol {
WayPointItem::WayPointItem(const internals::PointLatLng &coord, int const & altitude, MapGraphicItem *map, wptype type) : coord(coord), reached(false), description(""), shownumber(true), isDragging(false), altitude(altitude), map(map), myType(type)
{
text = 0;
numberI = 0;
isMagic = false;
picture.load(QString::fromUtf8(":/markers/images/marker.png"));
picture.load(":/markers/images/wp_marker_red.png");
number = WayPointItem::snumber;
++WayPointItem::snumber;
this->setFlag(QGraphicsItem::ItemIsMovable, true);
@ -64,7 +66,7 @@ WayPointItem::WayPointItem(MapGraphicItem *map, bool magicwaypoint) : reached(fa
myType = relative;
if (magicwaypoint) {
isMagic = true;
picture.load(QString::fromUtf8(":/opmap/images/waypoint_marker3.png"));
picture.load(":/markers/images/wp_marker_green.png");
number = -1;
} else {
isMagic = false;
@ -95,7 +97,7 @@ WayPointItem::WayPointItem(const internals::PointLatLng &coord, int const & alti
text = 0;
numberI = 0;
isMagic = false;
picture.load(QString::fromUtf8(":/markers/images/marker.png"));
picture.load(":/markers/images/wp_marker_red.png");
number = WayPointItem::snumber;
++WayPointItem::snumber;
this->setFlag(QGraphicsItem::ItemIsMovable, true);
@ -128,7 +130,7 @@ WayPointItem::WayPointItem(const distBearingAltitude &relativeCoordenate, const
text = 0;
numberI = 0;
isMagic = false;
picture.load(QString::fromUtf8(":/markers/images/marker.png"));
picture.load(":/markers/images/wp_marker_red.png");
number = WayPointItem::snumber;
++WayPointItem::snumber;
this->setFlag(QGraphicsItem::ItemIsMovable, true);
@ -259,7 +261,9 @@ void WayPointItem::setRelativeCoord(distBearingAltitude value)
void WayPointItem::SetCoord(const internals::PointLatLng &value)
{
if (qAbs(Coord().Lat() - value.Lat()) < 0.0001 && qAbs(Coord().Lng() - value.Lng()) < 0.0001) {
// If no changes from previous coordinates, return.
if ((qAbs(Coord().Lat() - value.Lat()) < COORDINATES_THRESHOLD) &&
(qAbs(Coord().Lng() - value.Lng()) < COORDINATES_THRESHOLD)) {
return;
}
coord = value;
@ -301,16 +305,16 @@ void WayPointItem::SetReached(const bool &value)
reached = value;
emit WPValuesChanged(this);
if (value) {
picture.load(QString::fromUtf8(":/markers/images/bigMarkerGreen.png"));
picture.load(":/markers/images/bigMarkerGreen.png");
} else {
if (!isMagic) {
if ((this->flags() & QGraphicsItem::ItemIsMovable) == QGraphicsItem::ItemIsMovable) {
picture.load(QString::fromUtf8(":/markers/images/marker.png"));
picture.load(":/markers/images/wp_marker_red.png");
} else {
picture.load(QString::fromUtf8(":/markers/images/waypoint_marker2.png"));
picture.load(":/markers/images/wp_marker_orange.png");
}
} else {
picture.load(QString::fromUtf8(":/opmap/images/waypoint_marker3.png"));
picture.load(":/markers/images/wp_marker_green.png");
}
}
this->update();
@ -436,9 +440,9 @@ void WayPointItem::setFlag(QGraphicsItem::GraphicsItemFlag flag, bool enabled)
return;
} else if (flag == QGraphicsItem::ItemIsMovable) {
if (enabled) {
picture.load(QString::fromUtf8(":/markers/images/marker.png"));
picture.load(":/markers/images/wp_marker_red.png");
} else {
picture.load(QString::fromUtf8(":/markers/images/waypoint_marker2.png"));
picture.load(":/markers/images/wp_marker_orange.png");
}
}
QGraphicsItem::setFlag(flag, enabled);

46
ground/gcs/src/libs/osgearth/osgQtQuick/OSGGeoTransformNode.cpp
View File

@ -117,32 +117,30 @@ public:
} else {
qWarning() << "OSGGeoTransformNode::updatePosition - scene node is not valid";
}
// TODO factorize this logic to utility (same logic is found elsewhere)
osgEarth::GeoPoint geoPoint;
if (mapNode) {
geoPoint = osgQtQuick::toGeoPoint(mapNode->getTerrain()->getSRS(), position);
} else {
qWarning() << "OSGGeoTransformNode::onChildNodeChanged - no map node";
geoPoint = osgQtQuick::toGeoPoint(position);
osgEarth::GeoPoint geoPoint = createGeoPoint(position, mapNode);
if (clampToTerrain) {
// get "size" of model
// TODO this should be done once only...
#if 0
osg::ComputeBoundsVisitor cbv;
transform->accept(cbv);
const osg::BoundingBox & bbox = cbv.getBoundingBox();
offset = bbox.radius();
#else
const osg::BoundingSphere & boundingSphere = transform->getBound();
offset = boundingSphere.radius();
#endif
// clamp model to terrain if needed
intoTerrain = clampGeoPoint(geoPoint, offset, mapNode);
if (intoTerrain) {
qDebug() << "OSGGeoTransformNode::updateNode - into terrain" << offset;
}
} else if (clampToTerrain) {
qWarning() << "OSGGeoTransformNode::onChildNodeChanged - cannot clamp without map node";
}
transform->setPosition(geoPoint);
}
if (clampToTerrain && mapNode) {
// get "size" of model
// TODO this should be done once only...
osg::ComputeBoundsVisitor cbv;
transform->accept(cbv);
const osg::BoundingBox & bbox = cbv.getBoundingBox();
offset = bbox.radius();
// qDebug() << "OSGGeoTransformNode::updateNode - offset" << offset;
// clamp model to terrain if needed
intoTerrain = clampGeoPoint(geoPoint, offset, mapNode);
} else if (clampToTerrain) {
qWarning() << "OSGGeoTransformNode::onChildNodeChanged - cannot clamp without map node";
}
transform->setPosition(geoPoint);
}
private slots:

24
ground/gcs/src/libs/osgearth/osgQtQuick/ga/OSGGeoTransformManipulator.cpp
View File

@ -155,27 +155,21 @@ public:
if (sceneNode && sceneNode->node()) {
mapNode = osgEarth::MapNode::findMapNode(sceneNode->node());
if (!mapNode) {
qWarning() << "OSGGeoTransformManipulator::updatePosition - manipulator node does not contain a map node";
}
} else {
qWarning() << "OSGGeoTransformManipulator::updatePosition - scene node is null";
}
osgEarth::GeoPoint geoPoint;
if (mapNode) {
geoPoint = osgQtQuick::toGeoPoint(mapNode->getTerrain()->getSRS(), position);
osgEarth::GeoPoint geoPoint = osgQtQuick::createGeoPoint(position, mapNode);
if (clampToTerrain) {
// clamp model to terrain if needed
intoTerrain = osgQtQuick::clampGeoPoint(geoPoint, 0, mapNode);
} else if (clampToTerrain) {
qWarning() << "OSGGeoTransformManipulator::updatePosition - cannot clamp without map node";
}
geoPoint.createLocalToWorld(cameraPosition);
} else {
geoPoint = osgQtQuick::toGeoPoint(position);
qWarning() << "OSGGeoTransformManipulator::updatePosition - scene node does not contain a map node";
}
if (clampToTerrain && mapNode) {
// clamp model to terrain if needed
intoTerrain = osgQtQuick::clampGeoPoint(geoPoint, 0, mapNode);
} else if (clampToTerrain) {
qWarning() << "OSGGeoTransformManipulator::updatePosition - cannot clamp without map node";
}
geoPoint.createLocalToWorld(cameraPosition);
}
void updateAttitude()

31
ground/gcs/src/libs/osgearth/utils/utility.cpp
View File

@ -69,9 +69,11 @@
#include "osgQtQuick/ga/OSGEarthManipulator.hpp"
#include "osgQtQuick/ga/OSGGeoTransformManipulator.hpp"
#include <osgEarth/Version>
#include <osgEarth/Capabilities>
#include <osgEarth/MapNode>
#include <osgEarth/SpatialReference>
#include <osgEarth/Terrain>
#include <osgEarth/ElevationQuery>
#endif // USE_OSGEARTH
@ -454,18 +456,20 @@ QString getUsageString(osgViewer::CompositeViewer *viewer)
}
#ifdef USE_OSGEARTH
osgEarth::GeoPoint toGeoPoint(const osgEarth::SpatialReference *srs, const QVector3D &position)
osgEarth::GeoPoint createGeoPoint(const QVector3D &position, osgEarth::MapNode *mapNode)
{
const osgEarth::SpatialReference *srs = NULL;
if (mapNode) {
srs = mapNode->getTerrain()->getSRS();
} else {
qWarning() << "Utility::createGeoPoint - null map node";
srs = osgEarth::SpatialReference::get("wgs84");
}
osgEarth::GeoPoint geoPoint(srs, position.x(), position.y(), position.z(), osgEarth::ALTMODE_ABSOLUTE);
return geoPoint;
}
osgEarth::GeoPoint toGeoPoint(const QVector3D &position)
{
return toGeoPoint(osgEarth::SpatialReference::get("wgs84"), position);
}
bool clampGeoPoint(osgEarth::GeoPoint &geoPoint, float offset, osgEarth::MapNode *mapNode)
{
if (!mapNode) {
@ -477,9 +481,18 @@ bool clampGeoPoint(osgEarth::GeoPoint &geoPoint, float offset, osgEarth::MapNode
osgEarth::ElevationQuery eq(mapNode->getMap());
// qDebug() << "Utility::clampGeoPoint - SRS :" << QString::fromStdString(mapNode->getMap()->getSRS()->getName());
bool clamped = false;
double elevation;
if (eq.getElevation(geoPoint, elevation, 0.0)) {
bool gotElevation;
#if OSGEARTH_VERSION_LESS_THAN(2, 9, 0)
gotElevation = eq.getElevation(geoPoint, elevation, 0.0);
#else
const double resolution = 0.0;
double actualResolution;
elevation = eq.getElevation(geoPoint, resolution, &actualResolution);
gotElevation = (elevation != NO_DATA_VALUE);
#endif
bool clamped = false;
if (gotElevation) {
clamped = ((geoPoint.z() - offset) < elevation);
if (clamped) {
// qDebug() << "Utility::clampGeoPoint - clamping" << geoPoint.z() - offset << "/" << elevation;

3
ground/gcs/src/libs/osgearth/utils/utility.h
View File

@ -145,8 +145,7 @@ QString getUsageString(osgViewer::Viewer *viewer);
QString getUsageString(osgViewer::CompositeViewer *viewer);
#ifdef USE_OSGEARTH
osgEarth::GeoPoint toGeoPoint(const QVector3D &position);
osgEarth::GeoPoint toGeoPoint(const osgEarth::SpatialReference *srs, const QVector3D &position);
osgEarth::GeoPoint createGeoPoint(const QVector3D &position, osgEarth::MapNode *mapNode);
bool clampGeoPoint(osgEarth::GeoPoint &geoPoint, float offset, osgEarth::MapNode *mapNode);
void capabilitiesInfo(const osgEarth::Capabilities & caps);
#endif

346
ground/gcs/src/libs/utils/logfile.cpp
View File

@ -2,7 +2,7 @@
******************************************************************************
*
* @file logfile.cpp
* @author The LibrePilot Project, http://www.librepilot.org Copyright (C) 2017.
* @author The LibrePilot Project, http://www.librepilot.org Copyright (C) 2017-2018.
* The OpenPilot Team, http://www.openpilot.org Copyright (C) 2010.
* @see The GNU Public License (GPL) Version 3
*
@ -26,6 +26,9 @@
#include <QDebug>
#include <QtGlobal>
#include <QDataStream>
#define TIMESTAMP_SIZE_BYTES 4
LogFile::LogFile(QObject *parent) : QIODevice(parent),
m_timer(this),
@ -34,9 +37,12 @@ LogFile::LogFile(QObject *parent) : QIODevice(parent),
m_lastPlayed(0),
m_timeOffset(0),
m_playbackSpeed(1.0),
paused(false),
m_replayState(STOPPED),
m_useProvidedTimeStamp(false),
m_providedTimeStamp(0)
m_providedTimeStamp(0),
m_beginTimeStamp(0),
m_endTimeStamp(0),
m_timerTick(0)
{
connect(&m_timer, &QTimer::timeout, this, &LogFile::timerFired);
}
@ -137,36 +143,61 @@ qint64 LogFile::bytesAvailable() const
return len;
}
/**
timerFired()
This function is called at a 10 ms interval to fill the replay buffers.
*/
void LogFile::timerFired()
{
if (m_file.bytesAvailable() > 4) {
if (m_replayState != PLAYING) {
return;
}
m_timerTick++;
if (m_file.bytesAvailable() > TIMESTAMP_SIZE_BYTES) {
int time;
time = m_myTime.elapsed();
// TODO: going back in time will be a problem
while ((m_lastPlayed + ((double)(time - m_timeOffset) * m_playbackSpeed) > m_nextTimeStamp)) {
/*
This code generates an advancing playback window. All samples that fit the window
are replayed. The window is about the size of the timer interval: 10 ms.
Description of used variables:
time : real-time interval since start of playback (in ms) - now()
m_timeOffset : real-time interval since start of playback (in ms) - when timerFired() was previously run
m_nextTimeStamp : read log until this log timestamp has been reached (in ms)
m_lastPlayed : log referenced timestamp advanced to during previous cycle (in ms)
m_playbackSpeed : 0.1 .. 1.0 .. 10 replay speedup factor
*/
while (m_nextTimeStamp < (m_lastPlayed + (double)(time - m_timeOffset) * m_playbackSpeed)) {
// advance the replay window for the next time period
m_lastPlayed += ((double)(time - m_timeOffset) * m_playbackSpeed);
// read data size
qint64 dataSize;
if (m_file.bytesAvailable() < (qint64)sizeof(dataSize)) {
qDebug() << "LogFile - end of log file reached";
stopReplay();
qDebug() << "LogFile replay - end of log file reached";
resetReplay();
return;
}
m_file.read((char *)&dataSize, sizeof(dataSize));
// check size consistency
if (dataSize < 1 || dataSize > (1024 * 1024)) {
qWarning() << "LogFile - corrupted log file! Unlikely packet size:" << dataSize;
qWarning() << "LogFile replay - corrupted log file! Unlikely packet size:" << dataSize;
stopReplay();
return;
}
// read data
if (m_file.bytesAvailable() < dataSize) {
qDebug() << "LogFile - end of log file reached";
stopReplay();
qDebug() << "LogFile replay - end of log file reached";
resetReplay();
return;
}
QByteArray data = m_file.read(dataSize);
@ -178,10 +209,14 @@ void LogFile::timerFired()
emit readyRead();
// rate-limit slider bar position updates to 10 updates per second
if (m_timerTick % 10 == 0) {
emit playbackPositionChanged(m_nextTimeStamp);
}
// read next timestamp
if (m_file.bytesAvailable() < (qint64)sizeof(m_nextTimeStamp)) {
qDebug() << "LogFile - end of log file reached";
stopReplay();
qDebug() << "LogFile replay - end of log file reached";
resetReplay();
return;
}
m_previousTimeStamp = m_nextTimeStamp;
@ -190,17 +225,17 @@ void LogFile::timerFired()
// some validity checks
if ((m_nextTimeStamp < m_previousTimeStamp) // logfile goes back in time
|| ((m_nextTimeStamp - m_previousTimeStamp) > 60 * 60 * 1000)) { // gap of more than 60 minutes
qWarning() << "LogFile - corrupted log file! Unlikely timestamp:" << m_nextTimeStamp << "after" << m_previousTimeStamp;
qWarning() << "LogFile replay - corrupted log file! Unlikely timestamp:" << m_nextTimeStamp << "after" << m_previousTimeStamp;
stopReplay();
return;
}
m_timeOffset = time;
time = m_myTime.elapsed();
time = m_myTime.elapsed(); // number of milliseconds since start of playback
}
} else {
qDebug() << "LogFile - end of log file reached";
stopReplay();
qDebug() << "LogFile replay - end of log file reached";
resetReplay();
}
}
@ -209,8 +244,26 @@ bool LogFile::isPlaying() const
return m_file.isOpen() && m_timer.isActive();
}
/**
* FUNCTION: startReplay()
*
* Starts replaying a newly opened logfile.
* Starts a timer: m_timer
*
* This function and the stopReplay() function should only ever be called from the same thread.
* This is required for correct control of the timer.
*
*/
bool LogFile::startReplay()
{
// Walk through logfile and create timestamp index
// Don't start replay if there was a problem indexing the logfile.
if (!buildIndex()) {
return false;
}
m_timerTick = 0;
if (!m_file.isOpen() || m_timer.isActive()) {
return false;
}
@ -221,7 +274,9 @@ bool LogFile::startReplay()
m_lastPlayed = 0;
m_previousTimeStamp = 0;
m_nextTimeStamp = 0;
m_mutex.lock();
m_dataBuffer.clear();
m_mutex.unlock();
// read next timestamp
if (m_file.bytesAvailable() < (qint64)sizeof(m_nextTimeStamp)) {
@ -232,25 +287,125 @@ bool LogFile::startReplay()
m_timer.setInterval(10);
m_timer.start();
paused = false;
m_replayState = PLAYING;
emit replayStarted();
return true;
}
/**
* FUNCTION: stopReplay()
*
* Stops replaying the logfile.
* Stops the timer: m_timer
*
* This function and the startReplay() function should only ever be called from the same thread.
* This is a requirement to be able to control the timer.
*
*/
bool LogFile::stopReplay()
{
if (!m_file.isOpen() || !(m_timer.isActive() || paused)) {
if (!m_file.isOpen()) {
return false;
}
if (m_timer.isActive()) {
m_timer.stop();
}
qDebug() << "LogFile - stopReplay";
m_timer.stop();
paused = false;
m_replayState = STOPPED;
emit replayFinished();
return true;
}
/**
* FUNCTION: resetReplay()
*
* Stops replaying the logfile.
* Stops the timer: m_timer
* Resets playback position to the start of the logfile
* through the emission of a replayCompleted signal.
*
*/
bool LogFile::resetReplay()
{
if (!m_file.isOpen()) {
return false;
}
if (m_timer.isActive()) {
m_timer.stop();
}
qDebug() << "LogFile - resetReplay";
m_replayState = STOPPED;
emit replayCompleted();
return true;
}
/**
* SLOT: resumeReplay()
*
* Resumes replay from the given position.
* If no position is given, resumes from the last position
*
*/
bool LogFile::resumeReplay(quint32 desiredPosition)
{
if (m_timer.isActive()) {
return false;
}
qDebug() << "LogFile - resumeReplay";
// Clear the playout buffer:
m_mutex.lock();
m_dataBuffer.clear();
m_mutex.unlock();
m_file.seek(0);
/* Skip through the logfile until we reach the desired position.
Looking for the next log timestamp after the desired position
has the advantage that it skips over parts of the log
where data might be missing.
*/
for (int i = 0; i < m_timeStamps.size(); i++) {
if (m_timeStamps.at(i) >= desiredPosition) {
int bytesToSkip = m_timeStampPositions.at(i);
bool seek_ok = m_file.seek(bytesToSkip);
if (!seek_ok) {
qWarning() << "LogFile resumeReplay - an error occurred while seeking through the logfile.";
}
m_lastPlayed = m_timeStamps.at(i);
break;
}
}
m_file.read((char *)&m_nextTimeStamp, sizeof(m_nextTimeStamp));
// Real-time timestamps don't not need to match the log timestamps.
// However the delta between real-time variables "m_timeOffset" and "m_myTime" is important.
// This delta determines the number of log entries replayed per cycle.
// Set the real-time interval to 0 to start with:
m_myTime.restart();
m_timeOffset = 0;
m_replayState = PLAYING;
m_timer.start();
// Notify UI that playback has resumed
emit replayStarted();
return true;
}
/**
* SLOT: pauseReplay()
*
* Pauses replay while storing the current playback position
*
*/
bool LogFile::pauseReplay()
{
if (!m_timer.isActive()) {
@ -258,24 +413,149 @@ bool LogFile::pauseReplay()
}
qDebug() << "LogFile - pauseReplay";
m_timer.stop();
paused = true;
// hack to notify UI that replay paused
emit replayStarted();
m_replayState = PAUSED;
return true;
}
bool LogFile::resumeReplay()
/**
* SLOT: pauseReplayAndResetPosition()
*
* Pauses replay and resets the playback position to the start of the logfile
*
*/
bool LogFile::pauseReplayAndResetPosition()
{
if (m_timer.isActive()) {
if (!m_file.isOpen() || !m_timer.isActive()) {
return false;
}
qDebug() << "LogFile - resumeReplay";
m_timeOffset = m_myTime.elapsed();
m_timer.start();
paused = false;
qDebug() << "LogFile - pauseReplayAndResetPosition";
m_timer.stop();
m_replayState = STOPPED;
m_timeOffset = 0;
m_lastPlayed = m_timeStamps.at(0);
m_previousTimeStamp = 0;
m_nextTimeStamp = 0;
return true;
}
/**
* FUNCTION: getReplayState()
*
* Returns the current replay status.
*
*/
ReplayState LogFile::getReplayState()
{
return m_replayState;
}
/**
* FUNCTION: buildIndex()
*
* Walk through the opened logfile and stores the first and last position timestamps.
* Also builds an index for quickly skipping to a specific position in the logfile.
*
* Returns true when indexing has completed successfully.
* Returns false when a problem was encountered.
*
*/
bool LogFile::buildIndex()
{
quint32 timeStamp;
qint64 totalSize;
qint64 readPointer = 0;
quint64 index = 0;
int bytesRead = 0;
qDebug() << "LogFile - buildIndex";
// Ensure empty vectors:
m_timeStampPositions.clear();
m_timeStamps.clear();
QByteArray arr = m_file.readAll();
totalSize = arr.size();
QDataStream dataStream(&arr, QIODevice::ReadOnly);
// set the first timestamp
if (totalSize - readPointer >= TIMESTAMP_SIZE_BYTES) {
bytesRead = dataStream.readRawData((char *)&timeStamp, TIMESTAMP_SIZE_BYTES);
if (bytesRead != TIMESTAMP_SIZE_BYTES) {
qWarning() << "LogFile buildIndex - read first timeStamp: readRawData returned unexpected number of bytes:" << bytesRead << "at position" << readPointer << "\n";
return false;
}
m_timeStamps.append(timeStamp);
m_timeStampPositions.append(readPointer);
readPointer += TIMESTAMP_SIZE_BYTES;
index++;
m_beginTimeStamp = timeStamp;
m_endTimeStamp = timeStamp;
}
while (true) {
qint64 dataSize;
// Check if there are enough bytes remaining for a correct "dataSize" field
if (totalSize - readPointer < (qint64)sizeof(dataSize)) {
qWarning() << "LogFile buildIndex - logfile corrupted! Unexpected end of file";
return false;
}
// Read the dataSize field and check for I/O errors
bytesRead = dataStream.readRawData((char *)&dataSize, sizeof(dataSize));
if (bytesRead != sizeof(dataSize)) {
qWarning() << "LogFile buildIndex - read dataSize: readRawData returned unexpected number of bytes:" << bytesRead << "at position" << readPointer << "\n";
return false;
}
readPointer += sizeof(dataSize);
if (dataSize < 1 || dataSize > (1024 * 1024)) {
qWarning() << "LogFile buildIndex - logfile corrupted! Unlikely packet size: " << dataSize << "\n";
return false;
}
// Check if there are enough bytes remaining
if (totalSize - readPointer < dataSize) {
qWarning() << "LogFile buildIndex - logfile corrupted! Unexpected end of file";
return false;
}
// skip reading the data (we don't need it at this point)
readPointer += dataStream.skipRawData(dataSize);
// read the next timestamp
if (totalSize - readPointer >= TIMESTAMP_SIZE_BYTES) {
bytesRead = dataStream.readRawData((char *)&timeStamp, TIMESTAMP_SIZE_BYTES);
if (bytesRead != TIMESTAMP_SIZE_BYTES) {
qWarning() << "LogFile buildIndex - read timeStamp, readRawData returned unexpected number of bytes:" << bytesRead << "at position" << readPointer << "\n";
return false;
}
// some validity checks
if (timeStamp < m_endTimeStamp // logfile goes back in time
|| (timeStamp - m_endTimeStamp) > (60 * 60 * 1000)) { // gap of more than 60 minutes)
qWarning() << "LogFile buildIndex - logfile corrupted! Unlikely timestamp " << timeStamp << " after " << m_endTimeStamp;
return false;
}
m_timeStamps.append(timeStamp);
m_timeStampPositions.append(readPointer);
readPointer += TIMESTAMP_SIZE_BYTES;
index++;
m_endTimeStamp = timeStamp;
} else {
// Break without error (we expect to end at this location when we are at the end of the logfile)
break;
}
}
emit timesChanged(m_beginTimeStamp, m_endTimeStamp);
// reset the read pointer to the start of the file
m_file.seek(0);
// hack to notify UI that replay resumed
emit replayStarted();
return true;
}

32
ground/gcs/src/libs/utils/logfile.h
View File

@ -2,7 +2,7 @@
******************************************************************************
*
* @file logfile.h
* @author The LibrePilot Project, http://www.librepilot.org Copyright (C) 2017.
* @author The LibrePilot Project, http://www.librepilot.org Copyright (C) 2017-2018.
* The OpenPilot Team, http://www.openpilot.org Copyright (C) 2010.
* @see The GNU Public License (GPL) Version 3
*
@ -32,8 +32,10 @@
#include <QTimer>
#include <QMutexLocker>
#include <QDebug>
#include <QBuffer>
#include <QFile>
#include <QVector>
typedef enum { PLAYING, PAUSED, STOPPED } ReplayState;
class QTCREATOR_UTILS_EXPORT LogFile : public QIODevice {
Q_OBJECT
@ -75,6 +77,8 @@ public:
m_providedTimeStamp = providedTimestamp;
}
ReplayState getReplayState();
public slots:
void setReplaySpeed(double val)
{
@ -83,24 +87,28 @@ public slots:
};
bool startReplay();
bool stopReplay();
bool resumeReplay(quint32);
bool pauseReplay();
bool resumeReplay();
bool pauseReplayAndResetPosition();
protected slots:
void timerFired();
signals:
void readReady();
void replayStarted();
void replayFinished();
void replayFinished(); // Emitted on error during replay or when logfile disconnected
void replayCompleted(); // Emitted at the end of normal logfile playback
void playbackPositionChanged(quint32);
void timesChanged(quint32, quint32);
protected:
QByteArray m_dataBuffer;
QTimer m_timer;
QTime m_myTime;
QFile m_file;
qint32 m_previousTimeStamp;
qint32 m_nextTimeStamp;
quint32 m_previousTimeStamp;
quint32 m_nextTimeStamp;
double m_lastPlayed;
// QMutex wants to be mutable
// http://stackoverflow.com/questions/25521570/can-mutex-locking-function-be-marked-as-const
@ -108,11 +116,19 @@ protected:
int m_timeOffset;
double m_playbackSpeed;
bool paused;
ReplayState m_replayState;
private:
bool m_useProvidedTimeStamp;
qint32 m_providedTimeStamp;
quint32 m_beginTimeStamp;
quint32 m_endTimeStamp;
quint32 m_timerTick;
QVector<quint32> m_timeStamps;
QVector<qint64> m_timeStampPositions;
bool buildIndex();
bool resetReplay();
};
#endif // LOGFILE_H

189
ground/gcs/src/libs/utils/worldmagmodel.cpp
View File

@ -2,14 +2,15 @@
******************************************************************************
*
* @file worldmagmodel.cpp
* @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2010.
* @author The LibrePilot Project, http://www.librepilot.org Copyright (C) 2019.
* The OpenPilot Team, http://www.openpilot.org Copyright (C) 2010.
* @brief Utilities to find the location of openpilot GCS files:
* - Plugins Share directory path
*
* @brief Source file for the World Magnetic Model
* This is a port of code available from the US NOAA.
*
* The hard coded coefficients should be valid until 2015.
* The hard coded coefficients should be valid until 2020.
*
* Updated coeffs from ..
* http://www.ngdc.noaa.gov/geomag/WMM/wmm_ddownload.shtml
@ -55,96 +56,96 @@
// updated coeffs available from http://www.ngdc.noaa.gov/geomag/WMM/wmm_ddownload.shtml
const double CoeffFile[91][6] = {
{ 0, 0, 0, 0, 0, 0 },
{ 1, 0, -29496.6, 0.0, 11.6, 0.0 },
{ 1, 1, -1586.3, 4944.4, 16.5, -25.9 },
{ 2, 0, -2396.6, 0.0, -12.1, 0.0 },
{ 2, 1, 3026.1, -2707.7, -4.4, -22.5 },
{ 2, 2, 1668.6, -576.1, 1.9, -11.8 },
{ 3, 0, 1340.1, 0.0, 0.4, 0.0 },
{ 3, 1, -2326.2, -160.2, -4.1, 7.3 },
{ 3, 2, 1231.9, 251.9, -2.9, -3.9 },
{ 3, 3, 634.0, -536.6, -7.7, -2.6 },
{ 4, 0, 912.6, 0.0, -1.8, 0.0 },
{ 4, 1, 808.9, 286.4, 2.3, 1.1 },
{ 4, 2, 166.7, -211.2, -8.7, 2.7 },
{ 4, 3, -357.1, 164.3, 4.6, 3.9 },
{ 4, 4, 89.4, -309.1, -2.1, -0.8 },
{ 5, 0, -230.9, 0.0, -1.0, 0.0 },
{ 5, 1, 357.2, 44.6, 0.6, 0.4 },
{ 5, 2, 200.3, 188.9, -1.8, 1.8 },
{ 5, 3, -141.1, -118.2, -1.0, 1.2 },
{ 5, 4, -163.0, 0.0, 0.9, 4.0 },
{ 5, 5, -7.8, 100.9, 1.0, -0.6 },
{ 6, 0, 72.8, 0.0, -0.2, 0.0 },
{ 6, 1, 68.6, -20.8, -0.2, -0.2 },
{ 6, 2, 76.0, 44.1, -0.1, -2.1 },
{ 6, 3, -141.4, 61.5, 2.0, -0.4 },
{ 6, 4, -22.8, -66.3, -1.7, -0.6 },
{ 6, 5, 13.2, 3.1, -0.3, 0.5 },
{ 6, 6, -77.9, 55.0, 1.7, 0.9 },
{ 7, 0, 80.5, 0.0, 0.1, 0.0 },
{ 7, 1, -75.1, -57.9, -0.1, 0.7 },
{ 7, 2, -4.7, -21.1, -0.6, 0.3 },
{ 7, 3, 45.3, 6.5, 1.3, -0.1 },
{ 7, 4, 13.9, 24.9, 0.4, -0.1 },
{ 7, 5, 10.4, 7.0, 0.3, -0.8 },
{ 7, 6, 1.7, -27.7, -0.7, -0.3 },
{ 7, 7, 4.9, -3.3, 0.6, 0.3 },
{ 8, 0, 24.4, 0.0, -0.1, 0.0 },
{ 8, 1, 8.1, 11.0, 0.1, -0.1 },
{ 8, 2, -14.5, -20.0, -0.6, 0.2 },
{ 8, 3, -5.6, 11.9, 0.2, 0.4 },
{ 8, 4, -19.3, -17.4, -0.2, 0.4 },
{ 8, 5, 11.5, 16.7, 0.3, 0.1 },
{ 8, 6, 10.9, 7.0, 0.3, -0.1 },
{ 8, 7, -14.1, -10.8, -0.6, 0.4 },
{ 8, 8, -3.7, 1.7, 0.2, 0.3 },
{ 9, 0, 5.4, 0.0, 0.0, 0.0 },
{ 9, 1, 9.4, -20.5, -0.1, 0.0 },
{ 9, 2, 3.4, 11.5, 0.0, -0.2 },
{ 9, 3, -5.2, 12.8, 0.3, 0.0 },
{ 9, 4, 3.1, -7.2, -0.4, -0.1 },
{ 9, 5, -12.4, -7.4, -0.3, 0.1 },
{ 9, 6, -0.7, 8.0, 0.1, 0.0 },
{ 9, 7, 8.4, 2.1, -0.1, -0.2 },
{ 9, 8, -8.5, -6.1, -0.4, 0.3 },
{ 9, 9, -10.1, 7.0, -0.2, 0.2 },
{ 1, 0, -29438.2, 0.0, 7.0, 0.0 },
{ 1, 1, -1493.5, 4796.3, 9.0, -30.2 },
{ 2, 0, -2444.5, 0.0, -11.0, 0.0 },
{ 2, 1, 3014.7, -2842.4, -6.2, -29.6 },
{ 2, 2, 1679.0, -638.8, 0.3, -17.3 },
{ 3, 0, 1351.8, 0.0, 2.4, 0.0 },
{ 3, 1, -2351.6, -113.7, -5.7, 6.5 },
{ 3, 2, 1223.6, 246.5, 2.0, -0.8 },
{ 3, 3, 582.3, -537.4, -11.0, -2.0 },
{ 4, 0, 907.5, 0.0, -0.8, 0.0 },
{ 4, 1, 814.8, 283.3, -0.9, -0.4 },
{ 4, 2, 117.8, -188.6, -6.5, 5.8 },
{ 4, 3, -335.6, 180.7, 5.2, 3.8 },
{ 4, 4, 69.7, -330.0, -4.0, -3.5 },
{ 5, 0, -232.9, 0.0, -0.3, 0.0 },
{ 5, 1, 360.1, 46.9, 0.6, 0.2 },
{ 5, 2, 191.7, 196.5, -0.8, 2.3 },
{ 5, 3, -141.3, -119.9, 0.1, -0.0 },
{ 5, 4, -157.2, 16.0, 1.2, 3.3 },
{ 5, 5, 7.7, 100.6, 1.4, -0.6 },
{ 6, 0, 69.4, 0.0, -0.8, 0.0 },
{ 6, 1, 67.7, -20.1, -0.5, 0.3 },
{ 6, 2, 72.3, 32.8, -0.1, -1.5 },
{ 6, 3, -129.1, 59.1, 1.6, -1.2 },
{ 6, 4, -28.4, -67.1, -1.6, 0.4 },
{ 6, 5, 13.6, 8.1, 0.0, 0.2 },
{ 6, 6, -70.3, 61.9, 1.2, 1.3 },
{ 7, 0, 81.7, 0.0, -0.3, 0.0 },
{ 7, 1, -75.9, -54.3, -0.2, 0.6 },
{ 7, 2, -7.1, -19.5, -0.3, 0.5 },
{ 7, 3, 52.2, 6.0, 0.9, -0.8 },
{ 7, 4, 15.0, 24.5, 0.1, -0.2 },
{ 7, 5, 9.1, 3.5, -0.6, -1.1 },
{ 7, 6, -3.0, -27.7, -0.9, 0.1 },
{ 7, 7, 5.9, -2.9, 0.7, 0.2 },
{ 8, 0, 24.2, 0.0, -0.1, 0.0 },
{ 8, 1, 8.9, 10.1, 0.2, -0.4 },
{ 8, 2, -16.9, -18.3, -0.2, 0.6 },
{ 8, 3, -3.1, 13.3, 0.5, -0.1 },
{ 8, 4, -20.7, -14.5, -0.1, 0.6 },
{ 8, 5, 13.3, 16.2, 0.4, -0.2 },
{ 8, 6, 11.6, 6.0, 0.4, -0.5 },
{ 8, 7, -16.3, -9.2, -0.1, 0.5 },
{ 8, 8, -2.1, 2.4, 0.4, 0.1 },
{ 9, 0, 5.5, 0.0, -0.1, 0.0 },
{ 9, 1, 8.8, -21.8, -0.1, -0.3 },
{ 9, 2, 3.0, 10.7, -0.0, 0.1 },
{ 9, 3, -3.2, 11.8, 0.4, -0.4 },
{ 9, 4, 0.6, -6.8, -0.4, 0.3 },
{ 9, 5, -13.2, -6.9, 0.0, 0.1 },
{ 9, 6, -0.1, 7.9, 0.3, -0.0 },
{ 9, 7, 8.7, 1.0, 0.0, -0.1 },
{ 9, 8, -9.1, -3.9, -0.0, 0.5 },
{ 9, 9, -10.4, 8.5, -0.3, 0.2 },
{ 10, 0, -2.0, 0.0, 0.0, 0.0 },
{ 10, 1, -6.3, 2.8, 0.0, 0.1 },
{ 10, 2, 0.9, -0.1, -0.1, -0.1 },
{ 10, 3, -1.1, 4.7, 0.2, 0.0 },
{ 10, 4, -0.2, 4.4, 0.0, -0.1 },
{ 10, 5, 2.5, -7.2, -0.1, -0.1 },
{ 10, 6, -0.3, -1.0, -0.2, 0.0 },
{ 10, 7, 2.2, -3.9, 0.0, -0.1 },
{ 10, 8, 3.1, -2.0, -0.1, -0.2 },
{ 10, 9, -1.0, -2.0, -0.2, 0.0 },
{ 10, 10, -2.8, -8.3, -0.2, -0.1 },
{ 11, 0, 3.0, 0.0, 0.0, 0.0 },
{ 11, 1, -1.5, 0.2, 0.0, 0.0 },
{ 11, 2, -2.1, 1.7, 0.0, 0.1 },
{ 11, 3, 1.7, -0.6, 0.1, 0.0 },
{ 11, 4, -0.5, -1.8, 0.0, 0.1 },
{ 11, 5, 0.5, 0.9, 0.0, 0.0 },
{ 11, 6, -0.8, -0.4, 0.0, 0.1 },
{ 11, 7, 0.4, -2.5, 0.0, 0.0 },
{ 11, 8, 1.8, -1.3, 0.0, -0.1 },
{ 11, 9, 0.1, -2.1, 0.0, -0.1 },
{ 11, 10, 0.7, -1.9, -0.1, 0.0 },
{ 11, 11, 3.8, -1.8, 0.0, -0.1 },
{ 12, 0, -2.2, 0.0, 0.0, 0.0 },
{ 12, 1, -0.2, -0.9, 0.0, 0.0 },
{ 12, 2, 0.3, 0.3, 0.1, 0.0 },
{ 12, 3, 1.0, 2.1, 0.1, 0.0 },
{ 12, 4, -0.6, -2.5, -0.1, 0.0 },
{ 12, 5, 0.9, 0.5, 0.0, 0.0 },
{ 12, 6, -0.1, 0.6, 0.0, 0.1 },
{ 12, 7, 0.5, 0.0, 0.0, 0.0 },
{ 12, 8, -0.4, 0.1, 0.0, 0.0 },
{ 12, 9, -0.4, 0.3, 0.0, 0.0 },
{ 12, 10, 0.2, -0.9, 0.0, 0.0 },
{ 12, 11, -0.8, -0.2, -0.1, 0.0 },
{ 12, 12, 0.0, 0.9, 0.1, 0.0 }
{ 10, 1, -6.1, 3.3, -0.0, 0.0 },
{ 10, 2, 0.2, -0.4, -0.1, 0.1 },
{ 10, 3, 0.6, 4.6, 0.2, -0.2 },
{ 10, 4, -0.5, 4.4, -0.1, 0.1 },
{ 10, 5, 1.8, -7.9, -0.2, -0.1 },
{ 10, 6, -0.7, -0.6, -0.0, 0.1 },
{ 10, 7, 2.2, -4.2, -0.1, -0.0 },
{ 10, 8, 2.4, -2.9, -0.2, -0.1 },
{ 10, 9, -1.8, -1.1, -0.1, 0.2 },
{ 10, 10, -3.6, -8.8, -0.0, -0.0 },
{ 11, 0, 3.0, 0.0, -0.0, 0.0 },
{ 11, 1, -1.4, -0.0, 0.0, 0.0 },
{ 11, 2, -2.3, 2.1, -0.0, 0.1 },
{ 11, 3, 2.1, -0.6, 0.0, 0.0 },
{ 11, 4, -0.8, -1.1, -0.0, 0.1 },
{ 11, 5, 0.6, 0.7, -0.1, -0.0 },
{ 11, 6, -0.7, -0.2, 0.0, -0.0 },
{ 11, 7, 0.1, -2.1, -0.0, 0.1 },
{ 11, 8, 1.7, -1.5, -0.0, -0.0 },
{ 11, 9, -0.2, -2.6, -0.1, -0.1 },
{ 11, 10, 0.4, -2.0, -0.0, -0.0 },
{ 11, 11, 3.5, -2.3, -0.1, -0.1 },
{ 12, 0, -2.0, 0.0, 0.0, 0.0 },
{ 12, 1, -0.1, -1.0, 0.0, -0.0 },
{ 12, 2, 0.5, 0.3, -0.0, 0.0 },
{ 12, 3, 1.2, 1.8, 0.0, -0.1 },
{ 12, 4, -0.9, -2.2, -0.1, 0.1 },
{ 12, 5, 0.9, 0.3, -0.0, -0.0 },
{ 12, 6, 0.1, 0.7, 0.0, 0.0 },
{ 12, 7, 0.6, -0.1, -0.0, -0.0 },
{ 12, 8, -0.4, 0.3, 0.0, 0.0 },
{ 12, 9, -0.5, 0.2, -0.0, 0.0 },
{ 12, 10, 0.2, -0.9, -0.0, -0.0 },
{ 12, 11, -0.9, -0.2, -0.0, 0.0 },
{ 12, 12, -0.0, 0.8, -0.1, -0.1 }
};
namespace Utils {
@ -232,10 +233,10 @@ void WorldMagModel::Initialize()
MagneticModel.nMaxSecVar = WMM_MAX_SECULAR_VARIATION_MODEL_DEGREES;
MagneticModel.SecularVariationUsed = 0;
// Really, Really needs to be read from a file - out of date in 2015 at latest
// Really, Really needs to be read from a file - out of date in 2020 at latest
MagneticModel.EditionDate = 5.7863328170559505e-307;
MagneticModel.epoch = 2010.0;
sprintf(MagneticModel.ModelName, "WMM-2010");
MagneticModel.epoch = 2015.0;
sprintf(MagneticModel.ModelName, "WMM-2015v2");
}

10
ground/gcs/src/plugins/config/calibration/gyrobiascalibrationmodel.cpp
View File

@ -69,9 +69,13 @@ void GyroBiasCalibrationModel::start()
// Disable gyro bias correction while calibrating
attitudeSettingsData.BiasCorrectGyro = AttitudeSettings::BIASCORRECTGYRO_FALSE;
// Zero board rotation
attitudeSettingsData.BoardRotation[AttitudeSettings::BOARDROTATION_YAW] = 0;
attitudeSettingsData.BoardRotation[AttitudeSettings::BOARDROTATION_ROLL] = 0;
attitudeSettingsData.BoardRotation[AttitudeSettings::BOARDROTATION_PITCH] = 0;
attitudeSettingsData.BoardRotation[AttitudeSettings::BOARDROTATION_YAW] = 0;
attitudeSettingsData.BoardRotation[AttitudeSettings::BOARDROTATION_ROLL] = 0;
attitudeSettingsData.BoardRotation[AttitudeSettings::BOARDROTATION_PITCH] = 0;
// Zero board level trim
attitudeSettingsData.BoardLevelTrim[AttitudeSettings::BOARDLEVELTRIM_ROLL] = 0;
attitudeSettingsData.BoardLevelTrim[AttitudeSettings::BOARDLEVELTRIM_PITCH] = 0;
attitudeSettings->setData(attitudeSettingsData);
UAVObject::Metadata gyroStateMetadata = gyroState->getMetadata();

7
ground/gcs/src/plugins/config/cfg_vehicletypes/configgroundvehiclewidget.cpp
View File

@ -150,6 +150,7 @@ void ConfigGroundVehicleWidget::setupUI(QString frameType)
initMixerCurves(frameType);
if (frameType == "GroundVehicleBoat" || frameType == "Boat (Turnable)") {
setComboCurrentIndex(m_aircraft->groundVehicleType, m_aircraft->groundVehicleType->findText("Boat (Turnable)"));
// Boat
m_vehicleImg->setElementId("boat");
@ -185,11 +186,13 @@ void ConfigGroundVehicleWidget::setupUI(QString frameType)
bool isBoat = frameType.contains("Boat");
if (isBoat) {
setComboCurrentIndex(m_aircraft->groundVehicleType, m_aircraft->groundVehicleType->findText("Boat (Differential)"));
// Boat differential
m_vehicleImg->setElementId("boat_diff");
m_aircraft->gvSteering1Label->setText("First rudder");
m_aircraft->gvSteering2Label->setText("Second rudder");
} else {
setComboCurrentIndex(m_aircraft->groundVehicleType, m_aircraft->groundVehicleType->findText("Tank (Differential)"));
// Tank
m_vehicleImg->setElementId("tank");
m_aircraft->gvSteering1Label->setText("Front steering");
@ -227,6 +230,7 @@ void ConfigGroundVehicleWidget::setupUI(QString frameType)
m_aircraft->groundVehicleThrottle2->initLinearCurve(5, 0.8, 0.0);
}
} else if (frameType == "GroundVehicleMotorcycle" || frameType == "Motorcycle") {
setComboCurrentIndex(m_aircraft->groundVehicleType, m_aircraft->groundVehicleType->findText("Motorcycle"));
// Motorcycle
m_vehicleImg->setElementId("motorbike");
@ -257,7 +261,8 @@ void ConfigGroundVehicleWidget::setupUI(QString frameType)
m_aircraft->groundVehicleThrottle2->initLinearCurve(5, 1.0, 0.0);
m_aircraft->groundVehicleThrottle1->initLinearCurve(5, 1.0, 0.0);
}
} else {
} else if (frameType == "GroundVehicleCar" || frameType == "Car (Turnable)") {
setComboCurrentIndex(m_aircraft->groundVehicleType, m_aircraft->groundVehicleType->findText("Car (Turnable)"));
// Car
m_vehicleImg->setElementId("car");

8
ground/gcs/src/plugins/config/configgadgetwidget.cpp
View File

@ -98,6 +98,7 @@ ConfigGadgetWidget::ConfigGadgetWidget(QWidget *parent) : QWidget(parent)
widget = new ConfigInputWidget(this);
static_cast<ConfigTaskWidget *>(widget)->bind();
stackWidget->insertTab(ConfigGadgetWidget::Input, widget, *icon, QString("Input"));
QWidget *inputWidget = widget;
icon = new QIcon();
icon->addFile(":/configgadget/images/output_normal.png", QSize(), QIcon::Normal, QIcon::Off);
@ -105,6 +106,7 @@ ConfigGadgetWidget::ConfigGadgetWidget(QWidget *parent) : QWidget(parent)
widget = new ConfigOutputWidget(this);
static_cast<ConfigTaskWidget *>(widget)->bind();
stackWidget->insertTab(ConfigGadgetWidget::Output, widget, *icon, QString("Output"));
QWidget *outputWidget = widget;
icon = new QIcon();
icon->addFile(":/configgadget/images/ins_normal.png", QSize(), QIcon::Normal, QIcon::Off);
@ -165,6 +167,12 @@ ConfigGadgetWidget::ConfigGadgetWidget(QWidget *parent) : QWidget(parent)
onOPLinkConnect();
}
// Connect output tab and input tab
// Input tab do not start calibration if Output tab is not safe
// Output tab uses the signal from Input tab and freeze all output UI while calibrating inputs
connect(outputWidget, SIGNAL(outputConfigSafeChanged(bool)), inputWidget, SLOT(setOutputConfigSafe(bool)));
connect(inputWidget, SIGNAL(inputCalibrationStateChanged(bool)), outputWidget, SLOT(setInputCalibrationState(bool)));
help = 0;
connect(stackWidget, SIGNAL(currentAboutToShow(int, bool *)), this, SLOT(tabAboutToChange(int, bool *)));
}

65
ground/gcs/src/plugins/config/configinputwidget.cpp
View File

@ -497,6 +497,16 @@ void ConfigInputWidget::enableControls(bool enable)
} else {
// Hide configAlarmStatus when disconnected
ui->configAlarmStatus->setVisible(false);
if (wizardStep != wizardNone) {
// Close input wizard
wzCancel();
}
if (ui->runCalibration->isChecked()) {
// Close manual calibration
ui->runCalibration->setChecked(false);
ui->runCalibration->setText(tr("Start Manual Calibration"));
emit inputCalibrationStateChanged(false);
}
}
}
@ -509,6 +519,12 @@ void ConfigInputWidget::resizeEvent(QResizeEvent *event)
void ConfigInputWidget::goToWizard()
{
if (!outputConfigIsSafe) {
QMessageBox::warning(this, tr("Warning"), tr("There is something wrong in <b>Output</b> tab."
"<p>Please fix the issue before starting the Transmitter wizard.</p>"), QMessageBox::Ok);
return;
}
QMessageBox msgBox;
msgBox.setText(tr("Arming Settings are now set to 'Always Disarmed' for your safety."));
@ -519,6 +535,9 @@ void ConfigInputWidget::goToWizard()
msgBox.setDefaultButton(QMessageBox::Ok);
msgBox.exec();
// Tell Output tab we freeze actuators soon
emit inputCalibrationStateChanged(true);
// Set correct tab visible before starting wizard.
if (ui->tabWidget->currentIndex() != 0) {
ui->tabWidget->setCurrentIndex(0);
@ -587,6 +606,9 @@ void ConfigInputWidget::wzCancel()
flightModeSettingsObj->setData(memento.flightModeSettingsData);
actuatorSettingsObj->setData(memento.actuatorSettingsData);
systemSettingsObj->setData(memento.systemSettingsData);
// Tell Output tab the calibration is ended
emit inputCalibrationStateChanged(false);
}
void ConfigInputWidget::registerControlActivity()
@ -682,6 +704,9 @@ void ConfigInputWidget::wzNext()
// move to Arming Settings tab
ui->stackedWidget->setCurrentIndex(0);
ui->tabWidget->setCurrentIndex(3);
// Tell Output tab the calibration is ended
emit inputCalibrationStateChanged(false);
break;
default:
Q_ASSERT(0);
@ -1872,7 +1897,7 @@ void ConfigInputWidget::updateConfigAlarmStatus()
switch (systemAlarms.ExtendedAlarmStatus[SystemAlarms::EXTENDEDALARMSTATUS_SYSTEMCONFIGURATION]) {
case SystemAlarms::EXTENDEDALARMSTATUS_FLIGHTMODE:
message = tr("Config error");
tooltipMessage = tr("There is something wrong with your config,\nusually a Thrust mode or Assisted mode not supported.\n\n"
tooltipMessage = tr("There is something wrong in the current config,\nusually a Thrust mode or Assisted mode not supported.\n\n"
"Tip: Reduce the Flight Mode Count to find the culprit.");
bgColor = "red";
}
@ -1916,6 +1941,19 @@ void ConfigInputWidget::updateCalibration()
void ConfigInputWidget::simpleCalibration(bool enable)
{
if (!isConnected()) {
return;
}
if (!outputConfigIsSafe) {
if (enable) {
QMessageBox::warning(this, tr("Warning"), tr("There is something wrong in <b>Output</b> tab."
"<p>Please fix the issue before starting the Manual Calibration.</p>"), QMessageBox::Ok);
ui->runCalibration->setChecked(false);
}
return;
}
if (enable) {
ui->configurationWizard->setEnabled(false);
ui->applyButton->setEnabled(false);
@ -1932,6 +1970,9 @@ void ConfigInputWidget::simpleCalibration(bool enable)
msgBox.setDefaultButton(QMessageBox::Ok);
msgBox.exec();
// Tell Output tab we freeze actuators soon
emit inputCalibrationStateChanged(true);
manualCommandData = manualCommandObj->getData();
manualSettingsData = manualSettingsObj->getData();
@ -1999,6 +2040,9 @@ void ConfigInputWidget::simpleCalibration(bool enable)
ui->saveButton->setEnabled(true);
ui->runCalibration->setText(tr("Start Manual Calibration"));
// Tell Output tab the calibration is ended
emit inputCalibrationStateChanged(false);
disconnect(manualCommandObj, SIGNAL(objectUnpacked(UAVObject *)), this, SLOT(updateCalibration()));
}
}
@ -2082,7 +2126,7 @@ void ConfigInputWidget::resetActuatorSettings()
QString mixerType;
// Clear all output data : Min, max, neutral at same value
// 1000 for motors and 1500 for all others (Reversable motor included)
// min value for motors and neutral for all others (Reversable motor, servo)
for (unsigned int output = 0; output < ActuatorSettings::CHANNELMAX_NUMELEM; output++) {
QString mixerNumType = QString("Mixer%1Type").arg(output + 1);
UAVObjectField *field = mixer->getField(mixerNumType);
@ -2092,13 +2136,13 @@ void ConfigInputWidget::resetActuatorSettings()
mixerType = field->getValue().toString();
}
if ((mixerType == "Motor") || (mixerType == "Disabled")) {
actuatorSettingsData.ChannelMax[output] = 1000;
actuatorSettingsData.ChannelMin[output] = 1000;
actuatorSettingsData.ChannelNeutral[output] = 1000;
// Apply current min setting to neutral/max values
actuatorSettingsData.ChannelMax[output] = actuatorSettingsData.ChannelMin[output];
actuatorSettingsData.ChannelNeutral[output] = actuatorSettingsData.ChannelMin[output];
} else {
actuatorSettingsData.ChannelMax[output] = 1500;
actuatorSettingsData.ChannelMin[output] = 1500;
actuatorSettingsData.ChannelNeutral[output] = 1500;
// Apply current neutral setting to min/max values
actuatorSettingsData.ChannelMax[output] = actuatorSettingsData.ChannelNeutral[output];
actuatorSettingsData.ChannelMin[output] = actuatorSettingsData.ChannelNeutral[output];
}
UAVObjectUpdaterHelper updateHelper;
actuatorSettingsObj->setData(actuatorSettingsData, false);
@ -2189,3 +2233,8 @@ void ConfigInputWidget::enableControlsChanged(bool enabled)
ui->failsafeBatteryWarningFlightModeCb->setEnabled(enabled && batteryModuleEnabled);
ui->failsafeBatteryCriticalFlightModeCb->setEnabled(enabled && batteryModuleEnabled);
}
void ConfigInputWidget::setOutputConfigSafe(bool status)
{
outputConfigIsSafe = status;
}

7
ground/gcs/src/plugins/config/configinputwidget.h
View File

@ -72,10 +72,17 @@ public:
void enableControls(bool enable);
bool shouldObjectBeSaved(UAVObject *object);
public slots:
void setOutputConfigSafe(bool status);
signals:
void inputCalibrationStateChanged(bool newState);
private:
bool throttleError;
bool growing;
bool reverse[ManualControlSettings::CHANNELNEUTRAL_NUMELEM];
bool outputConfigIsSafe;
txMovements currentMovement;
int movePos;
void setTxMovement(txMovements movement);

290
ground/gcs/src/plugins/config/configoutputwidget.cpp
View File

@ -51,8 +51,23 @@
#include <QTextEdit>
#include <QMessageBox>
#define MAXOUTPUT_VALUE 2500
#define MINOUTPUT_VALUE 500
// Motor settings
#define DSHOT_MAXOUTPUT_RANGE 2000
#define DSHOT_MINTOUTPUT_RANGE 0
#define PWMSYNC_MAXOUTPUT_RANGE 1900
#define DEFAULT_MAXOUTPUT_RANGE 2000
#define DEFAULT_MINOUTPUT_RANGE 900
#define DEFAULT_MINOUTPUT_VALUE 1000
#define REVMOTOR_NEUTRAL_TARGET_VALUE 1500
#define REVMOTOR_NEUTRAL_DIFF_VALUE 200
#define MOTOR_NEUTRAL_DIFF_VALUE 300
// Servo settings
#define SERVO_MAXOUTPUT_RANGE 2500
#define SERVO_MINOUTPUT_RANGE 500
#define SERVO_MAXOUTPUT_VALUE 2000
#define SERVO_MINOUTPUT_VALUE 1000
#define SERVO_NEUTRAL_VALUE 1500
ConfigOutputWidget::ConfigOutputWidget(QWidget *parent) : ConfigTaskWidget(parent)
{
@ -64,7 +79,8 @@ ConfigOutputWidget::ConfigOutputWidget(QWidget *parent) : ConfigTaskWidget(paren
addAutoBindings();
m_ui->gvFrame->setVisible(false);
m_ui->boardWarningFrame->setVisible(false);
m_ui->configWarningFrame->setVisible(false);
ExtensionSystem::PluginManager *pm = ExtensionSystem::PluginManager::instance();
UAVSettingsImportExportFactory *importexportplugin = pm->getObject<UAVSettingsImportExportFactory>();
@ -128,7 +144,10 @@ ConfigOutputWidget::ConfigOutputWidget(QWidget *parent) : ConfigTaskWidget(paren
}
SystemAlarms *systemAlarmsObj = SystemAlarms::GetInstance(getObjectManager());
connect(systemAlarmsObj, SIGNAL(objectUpdated(UAVObject *)), this, SLOT(updateWarnings(UAVObject *)));
connect(systemAlarmsObj, SIGNAL(objectUpdated(UAVObject *)), this, SLOT(updateBoardWarnings(UAVObject *)));
inputCalibrationStarted = false;
channelTestsStarted = false;
// TODO why do we do that ?
disconnect(this, SLOT(refreshWidgetsValues(UAVObject *)));
@ -138,7 +157,7 @@ ConfigOutputWidget::~ConfigOutputWidget()
{
SystemAlarms *systemAlarmsObj = SystemAlarms::GetInstance(getObjectManager());
disconnect(systemAlarmsObj, SIGNAL(objectUpdated(UAVObject *)), this, SLOT(updateWarnings(UAVObject *)));
disconnect(systemAlarmsObj, SIGNAL(objectUpdated(UAVObject *)), this, SLOT(updateBoardWarnings(UAVObject *)));
foreach(OutputBankControls controls, m_banks) {
disconnect(controls.modeCombo(), SIGNAL(currentIndexChanged(int)), this, SLOT(onBankTypeChange()));
}
@ -150,6 +169,7 @@ void ConfigOutputWidget::enableControls(bool enable)
if (!enable) {
m_ui->channelOutTest->setChecked(false);
channelTestsStarted = false;
}
m_ui->channelOutTest->setEnabled(enable);
}
@ -203,6 +223,17 @@ void ConfigOutputWidget::runChannelTests(bool state)
}
}
channelTestsStarted = state;
// Emit signal to be received by Input tab
emit outputConfigSafeChanged(!state);
m_ui->spinningArmed->setEnabled(!state);
m_ui->alwaysStabilizedSwitch->setEnabled((m_ui->spinningArmed->isChecked()) && !state);
m_ui->alwayStabilizedLabel1->setEnabled((m_ui->spinningArmed->isChecked()) && !state);
m_ui->alwayStabilizedLabel2->setEnabled((m_ui->spinningArmed->isChecked()) && !state);
setBanksEnabled(!state);
ActuatorCommand *obj = ActuatorCommand::GetInstance(getObjectManager());
UAVObject::Metadata mdata = obj->getMetadata();
if (state) {
@ -390,6 +421,9 @@ void ConfigOutputWidget::refreshWidgetsValuesImpl(UAVObject *obj)
}
}
// Store how many banks are active according to the board
activeBanksCount = bankLabels.count();
int i = 0;
foreach(QString banklabel, bankLabels) {
OutputBankControls controls = m_banks.at(i);
@ -399,10 +433,11 @@ void ConfigOutputWidget::refreshWidgetsValuesImpl(UAVObject *obj)
if (index == -1) {
controls.rateCombo()->addItem(tr("%1 Hz").arg(actuatorSettingsData.BankUpdateFreq[i]), actuatorSettingsData.BankUpdateFreq[i]);
}
bool isPWM = (controls.modeCombo()->currentIndex() == ActuatorSettings::BANKMODE_PWM);
controls.rateCombo()->setCurrentIndex(index);
controls.rateCombo()->setEnabled(controls.modeCombo()->currentIndex() == ActuatorSettings::BANKMODE_PWM);
controls.rateCombo()->setEnabled(!inputCalibrationStarted && !channelTestsStarted && isPWM);
setColor(controls.rateCombo(), controls.color());
controls.modeCombo()->setEnabled(true);
controls.modeCombo()->setEnabled(!inputCalibrationStarted && !channelTestsStarted);
setColor(controls.modeCombo(), controls.color());
i++;
}
@ -430,6 +465,7 @@ void ConfigOutputWidget::refreshWidgetsValuesImpl(UAVObject *obj)
}
updateSpinStabilizeCheckComboBoxes();
checkOutputConfig();
}
/**
@ -483,9 +519,9 @@ void ConfigOutputWidget::updateObjectsFromWidgetsImpl()
void ConfigOutputWidget::updateSpinStabilizeCheckComboBoxes()
{
m_ui->alwayStabilizedLabel1->setEnabled(m_ui->spinningArmed->isChecked());
m_ui->alwayStabilizedLabel2->setEnabled(m_ui->spinningArmed->isChecked());
m_ui->alwaysStabilizedSwitch->setEnabled(m_ui->spinningArmed->isChecked());
m_ui->alwayStabilizedLabel1->setEnabled((m_ui->spinningArmed->isChecked()) && (m_ui->spinningArmed->isEnabled()));
m_ui->alwayStabilizedLabel2->setEnabled((m_ui->spinningArmed->isChecked()) && (m_ui->spinningArmed->isEnabled()));
m_ui->alwaysStabilizedSwitch->setEnabled((m_ui->spinningArmed->isChecked()) && (m_ui->spinningArmed->isEnabled()));
if (!m_ui->spinningArmed->isChecked()) {
m_ui->alwaysStabilizedSwitch->setCurrentIndex(FlightModeSettings::ALWAYSSTABILIZEWHENARMEDSWITCH_DISABLED);
@ -506,23 +542,118 @@ void ConfigOutputWidget::updateAlwaysStabilizeStatus()
void ConfigOutputWidget::setChannelLimits(OutputChannelForm *channelForm, OutputBankControls *bankControls)
{
// Set UI limits according to the bankmode and destination
switch (bankControls->modeCombo()->currentIndex()) {
case ActuatorSettings::BANKMODE_DSHOT:
channelForm->setLimits(0, 0, 0, 2000);
// 0 - 2000 UI limits, DShot min value is fixed to zero
if (channelForm->isServoOutput()) {
// Driving a servo using DShot doest not make sense so break
break;
}
channelForm->setLimits(DSHOT_MINTOUTPUT_RANGE, DSHOT_MINTOUTPUT_RANGE, DSHOT_MINTOUTPUT_RANGE, DSHOT_MAXOUTPUT_RANGE);
channelForm->setRange(DSHOT_MINTOUTPUT_RANGE, DSHOT_MAXOUTPUT_RANGE);
channelForm->setNeutral(DSHOT_MINTOUTPUT_RANGE);
break;
// case ActuatorSettings::BANKMODE_BRUSHED:
// channelForm->setLimits(0, 0, 0, 100); // 0 to 100%
// break;
case ActuatorSettings::BANKMODE_PWMSYNC:
// 900 - 1900 UI limits
// Default values 1000 - 1900
channelForm->setLimits(DEFAULT_MINOUTPUT_RANGE, PWMSYNC_MAXOUTPUT_RANGE, DEFAULT_MINOUTPUT_RANGE, PWMSYNC_MAXOUTPUT_RANGE);
channelForm->setRange(DEFAULT_MINOUTPUT_VALUE, PWMSYNC_MAXOUTPUT_RANGE);
channelForm->setNeutral(DEFAULT_MINOUTPUT_VALUE);
if (channelForm->isServoOutput()) {
// Servo: Some of them can handle PWMSync, 500 - 1900 UI limits
// Default values 1000 - 1900 + neutral 1500
channelForm->setRange(SERVO_MINOUTPUT_VALUE, PWMSYNC_MAXOUTPUT_RANGE);
channelForm->setNeutral(SERVO_NEUTRAL_VALUE);
}
break;
case ActuatorSettings::BANKMODE_PWM:
if (channelForm->isServoOutput()) {
// Servo: 500 - 2500 UI limits
// Default values 1000 - 2000 + neutral 1500
channelForm->setLimits(SERVO_MINOUTPUT_RANGE, SERVO_MAXOUTPUT_RANGE, SERVO_MINOUTPUT_RANGE, SERVO_MAXOUTPUT_RANGE);
channelForm->setRange(SERVO_MINOUTPUT_VALUE, SERVO_MAXOUTPUT_VALUE);
channelForm->setNeutral(SERVO_NEUTRAL_VALUE);
break;
}
// PWM motor outputs fall to default
case ActuatorSettings::BANKMODE_ONESHOT125:
case ActuatorSettings::BANKMODE_ONESHOT42:
case ActuatorSettings::BANKMODE_MULTISHOT:
if (channelForm->isServoOutput()) {
// Driving a servo using this mode does not make sense so break
break;
}
default:
channelForm->setLimits(MINOUTPUT_VALUE, MAXOUTPUT_VALUE, MINOUTPUT_VALUE, MAXOUTPUT_VALUE);
// Motors 900 - 2000 UI limits
// Default values 1000 - 2000, neutral set to min
// This settings are used for PWM, OneShot125, OneShot42 and MultiShot
channelForm->setLimits(DEFAULT_MINOUTPUT_RANGE, DEFAULT_MAXOUTPUT_RANGE, DEFAULT_MINOUTPUT_RANGE, DEFAULT_MAXOUTPUT_RANGE);
channelForm->setRange(DEFAULT_MINOUTPUT_VALUE, DEFAULT_MAXOUTPUT_RANGE);
channelForm->setNeutral(DEFAULT_MINOUTPUT_VALUE);
break;
}
}
ConfigOutputWidget::ChannelConfigWarning ConfigOutputWidget::checkChannelConfig(OutputChannelForm *channelForm, OutputBankControls *bankControls)
{
ChannelConfigWarning warning = None;
int currentNeutralValue = channelForm->neutralValue();
// Check if RevMotor has neutral value around center
if (channelForm->isReversibleMotorOutput()) {
warning = IsReversibleMotorCheckNeutral;
int neutralDiff = qAbs(REVMOTOR_NEUTRAL_TARGET_VALUE - currentNeutralValue);
if (neutralDiff < REVMOTOR_NEUTRAL_DIFF_VALUE) {
// Reset warning
warning = None;
}
}
// Check if NormalMotor neutral is not too high
if (channelForm->isNormalMotorOutput()) {
warning = IsNormalMotorCheckNeutral;
int neutralDiff = currentNeutralValue - DEFAULT_MINOUTPUT_VALUE;
if (neutralDiff < MOTOR_NEUTRAL_DIFF_VALUE) {
// Reset warning
warning = None;
}
}
switch (bankControls->modeCombo()->currentIndex()) {
case ActuatorSettings::BANKMODE_DSHOT:
if (channelForm->isServoOutput()) {
// Driving a servo using DShot doest not make sense
warning = CannotDriveServo;
} else if (channelForm->isReversibleMotorOutput()) {
// Bi-directional DShot not yet supported
warning = BiDirectionalDShotNotSupported;
}
break;
case ActuatorSettings::BANKMODE_PWMSYNC:
case ActuatorSettings::BANKMODE_PWM:
break;
case ActuatorSettings::BANKMODE_ONESHOT125:
case ActuatorSettings::BANKMODE_ONESHOT42:
case ActuatorSettings::BANKMODE_MULTISHOT:
if (channelForm->isServoOutput()) {
warning = CannotDriveServo;
// Driving a servo using this mode does not make sense so break
}
default:
break;
}
return warning;
}
void ConfigOutputWidget::onBankTypeChange()
{
QComboBox *bankModeCombo = qobject_cast<QComboBox *>(sender());
ChannelConfigWarning new_warning = None;
if (bankModeCombo != NULL) {
int bankNumber = 1;
QList<OutputChannelForm *> outputChannelForms = findChildren<OutputChannelForm *>();
@ -534,6 +665,10 @@ void ConfigOutputWidget::onBankTypeChange()
foreach(OutputChannelForm * outputChannelForm, outputChannelForms) {
if (outputChannelForm->bank().toInt() == bankNumber) {
setChannelLimits(outputChannelForm, &controls);
ChannelConfigWarning warning = checkChannelConfig(outputChannelForm, &controls);
if (warning > None) {
new_warning = warning;
}
}
}
break;
@ -542,14 +677,46 @@ void ConfigOutputWidget::onBankTypeChange()
bankNumber++;
}
}
updateChannelConfigWarning(new_warning);
}
bool ConfigOutputWidget::checkOutputConfig()
{
ChannelConfigWarning new_warning = None;
int bankNumber = 1;
QList<OutputChannelForm *> outputChannelForms = findChildren<OutputChannelForm *>();
foreach(OutputBankControls controls, m_banks) {
foreach(OutputChannelForm * outputChannelForm, outputChannelForms) {
if (!outputChannelForm->isDisabledOutput() && (outputChannelForm->bank().toInt() == bankNumber)) {
ChannelConfigWarning warning = checkChannelConfig(outputChannelForm, &controls);
if (warning > None) {
new_warning = warning;
}
}
}
bankNumber++;
}
updateChannelConfigWarning(new_warning);
// Emit signal to be received by Input tab
emit outputConfigSafeChanged(new_warning == None);
return new_warning == None;
}
void ConfigOutputWidget::stopTests()
{
m_ui->channelOutTest->setChecked(false);
channelTestsStarted = false;
}
void ConfigOutputWidget::updateWarnings(UAVObject *)
void ConfigOutputWidget::updateBoardWarnings(UAVObject *)
{
SystemAlarms *systemAlarmsObj = SystemAlarms::GetInstance(getObjectManager());
SystemAlarms::DataFields systemAlarms = systemAlarmsObj->getData();
@ -557,23 +724,96 @@ void ConfigOutputWidget::updateWarnings(UAVObject *)
if (systemAlarms.Alarm[SystemAlarms::ALARM_SYSTEMCONFIGURATION] > SystemAlarms::ALARM_WARNING) {
switch (systemAlarms.ExtendedAlarmStatus[SystemAlarms::EXTENDEDALARMSTATUS_SYSTEMCONFIGURATION]) {
case SystemAlarms::EXTENDEDALARMSTATUS_UNSUPPORTEDCONFIG_ONESHOT:
setWarning(tr("OneShot and PWMSync output only works with Receiver Port settings marked with '+OneShot'<br>"
"When using Receiver Port setting 'PPM_PIN8+OneShot' "
"<b><font color='%1'>Bank %2</font></b> must be set to PWM")
.arg(m_banks.at(3).color().name()).arg(m_banks.at(3).label()->text()));
setBoardWarning(tr("OneShot and PWMSync output only works with Receiver Port settings marked with '+OneShot'<br>"
"When using Receiver Port setting 'PPM_PIN8+OneShot' "
"<b><font color='%1'>Bank %2</font></b> must be set to PWM")
.arg(m_banks.at(3).color().name()).arg(m_banks.at(3).label()->text()));
return;
}
}
setWarning(NULL);
setBoardWarning(NULL);
}
void ConfigOutputWidget::setWarning(QString message)
void ConfigOutputWidget::updateChannelConfigWarning(ChannelConfigWarning warning)
{
m_ui->gvFrame->setVisible(!message.isNull());
m_ui->picWarning->setPixmap(message.isNull() ? QPixmap() : QPixmap(":/configgadget/images/error.svg"));
m_ui->txtWarning->setText(message);
QString warning_str;
if (warning == BiDirectionalDShotNotSupported) {
// TODO: Implement bi-directional DShot
warning_str = "There is <b>one reversible motor</b> using DShot is configured.<br>"
"Bi-directional DShot is currently not supported. Please use PWM, OneShotXXX or MultiShot.";
}
if (warning == IsNormalMotorCheckNeutral) {
warning_str = "There is at least one pretty <b>high neutral value</b> set in your configuration.<br>"
"Make sure all ESCs are calibrated and no mechanical stress in all motors.";
}
if (warning == IsReversibleMotorCheckNeutral) {
warning_str = "A least one <b>reversible motor</b> is configured.<br>"
"Make sure a appropriate neutral value is set before saving and applying power to the vehicule.";
}
if (warning == CannotDriveServo) {
warning_str = "One bank cannot drive a <b>servo output</b>!<br>"
"You must use PWM for this bank or move the servo output to another compatible bank.";
}
setConfigWarning(warning_str);
}
void ConfigOutputWidget::setBanksEnabled(bool state)
{
// Disable/Enable banks
for (int i = 0; i < m_banks.count(); i++) {
OutputBankControls controls = m_banks.at(i);
if (i < activeBanksCount) {
controls.modeCombo()->setEnabled(state);
controls.rateCombo()->setEnabled(state);
} else {
controls.modeCombo()->setEnabled(false);
controls.rateCombo()->setEnabled(false);
}
}
}
void ConfigOutputWidget::setBoardWarning(QString message)
{
m_ui->boardWarningFrame->setVisible(!message.isNull());
m_ui->boardWarningPic->setPixmap(message.isNull() ? QPixmap() : QPixmap(":/configgadget/images/error.svg"));
m_ui->boardWarningTxt->setText(message);
}
void ConfigOutputWidget::setConfigWarning(QString message)
{
m_ui->configWarningFrame->setVisible(!message.isNull());
m_ui->configWarningPic->setPixmap(message.isNull() ? QPixmap() : QPixmap(":/configgadget/images/error.svg"));
m_ui->configWarningTxt->setText(message);
}
void ConfigOutputWidget::setInputCalibrationState(bool started)
{
inputCalibrationStarted = started;
// Disable UI when a input calibration is started
// so user cannot manipulate settings.
enableControls(!started);
setBanksEnabled(!started);
// Disable ASWA
m_ui->spinningArmed->setEnabled(!started);
m_ui->alwaysStabilizedSwitch->setEnabled((m_ui->spinningArmed->isChecked()) && !started);
m_ui->alwayStabilizedLabel1->setEnabled((m_ui->spinningArmed->isChecked()) && !started);
m_ui->alwayStabilizedLabel2->setEnabled((m_ui->spinningArmed->isChecked()) && !started);
// Disable every channel form when needed
for (unsigned int i = 0; i < ActuatorCommand::CHANNEL_NUMELEM; i++) {
OutputChannelForm *form = getOutputChannelForm(i);
form->ui->actuatorRev->setChecked(false);
form->ui->actuatorLink->setChecked(false);
form->setChannelRangeEnabled(!started);
form->setControlsEnabled(!started);
}
}
OutputBankControls::OutputBankControls(MixerSettings *mixer, QLabel *label, QColor color, QComboBox *rateCombo, QComboBox *modeCombo) :
m_mixer(mixer), m_label(label), m_color(color), m_rateCombo(rateCombo), m_modeCombo(modeCombo)

20
ground/gcs/src/plugins/config/configoutputwidget.h
View File

@ -85,9 +85,16 @@ public:
ConfigOutputWidget(QWidget *parent = 0);
~ConfigOutputWidget();
public slots:
void setInputCalibrationState(bool state);
signals:
void outputConfigSafeChanged(bool newStatus);
protected:
void enableControls(bool enable);
void setWarning(QString message);
void setBoardWarning(QString message);
void setConfigWarning(QString message);
virtual void refreshWidgetsValuesImpl(UAVObject *obj);
virtual void updateObjectsFromWidgetsImpl();
@ -98,16 +105,25 @@ private:
int m_mccDataRate;
UAVObject::Metadata m_accInitialData;
QList<OutputBankControls> m_banks;
int activeBanksCount;
void setBanksEnabled(bool state);
bool inputCalibrationStarted;
bool channelTestsStarted;
OutputChannelForm *getOutputChannelForm(const int index) const;
void updateChannelInSlider(QSlider *slider, QLabel *min, QLabel *max, QCheckBox *rev, int value);
void assignOutputChannel(UAVDataObject *obj, QString &str);
void setColor(QWidget *widget, const QColor color);
void sendAllChannelTests();
enum ChannelConfigWarning { None, CannotDriveServo, IsNormalMotorCheckNeutral, IsReversibleMotorCheckNeutral, BiDirectionalDShotNotSupported };
void setChannelLimits(OutputChannelForm *channelForm, OutputBankControls *bankControls);
ChannelConfigWarning checkChannelConfig(OutputChannelForm *channelForm, OutputBankControls *bankControls);
bool checkOutputConfig();
void updateChannelConfigWarning(ChannelConfigWarning warning);
private slots:
void updateWarnings(UAVObject *);
void updateBoardWarnings(UAVObject *);
void updateSpinStabilizeCheckComboBoxes();
void updateAlwaysStabilizeStatus();
void stopTests();

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