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

Merge branch 'corvuscorax/OP-947_stateestimator-module' into next

Browse Source 
Thanks everyone for helping getting this together, especially Alessio
and DLite :-)
This commit is contained in:
Corvus Corax 2013-06-25 22:35:22 +02:00
commit 0c43346292
187 changed files with 4191 additions and 2226 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

92
flight/libraries/CoordinateConversions.c
View File

@ -35,27 +35,27 @@
#define MIN_ALLOWABLE_MAGNITUDE 1e-30f
// ****** convert Lat,Lon,Alt to ECEF ************
void LLA2ECEF(float LLA[3], float ECEF[3])
void LLA2ECEF(float LLA[3], double ECEF[3])
{
const float a = 6378137.0f; // Equatorial Radius
const float e = 8.1819190842622e-2f; // Eccentricity
float sinLat, sinLon, cosLat, cosLon;
float N;
const double a = 6378137.0d; // Equatorial Radius
const double e = 8.1819190842622e-2d; // Eccentricity
double sinLat, sinLon, cosLat, cosLon;
double N;
sinLat = sinf(DEG2RAD(LLA[0]));
sinLon = sinf(DEG2RAD(LLA[1]));
cosLat = cosf(DEG2RAD(LLA[0]));
cosLon = cosf(DEG2RAD(LLA[1]));
sinLat = sin(DEG2RAD(LLA[0]));
sinLon = sin(DEG2RAD(LLA[1]));
cosLat = cos(DEG2RAD(LLA[0]));
cosLon = cos(DEG2RAD(LLA[1]));
N = a / sqrtf(1.0f - e * e * sinLat * sinLat); // prime vertical radius of curvature
N = a / sqrt(1.0d - e * e * sinLat * sinLat); // prime vertical radius of curvature
ECEF[0] = (N + LLA[2]) * cosLat * cosLon;
ECEF[1] = (N + LLA[2]) * cosLat * sinLon;
ECEF[2] = ((1 - e * e) * N + LLA[2]) * sinLat;
ECEF[0] = (N + (double)LLA[2]) * cosLat * cosLon;
ECEF[1] = (N + (double)LLA[2]) * cosLat * sinLon;
ECEF[2] = ((1 - e * e) * N + (double)LLA[2]) * sinLat;
}
// ****** convert ECEF to Lat,Lon,Alt (ITERATIVE!) *********
uint16_t ECEF2LLA(float ECEF[3], float LLA[3])
uint16_t ECEF2LLA(double ECEF[3], float LLA[3])
{
/**
* LLA parameter is used to prime the iteration.
@ -66,48 +66,48 @@ uint16_t ECEF2LLA(float ECEF[3], float LLA[3])
* Suggestion: [0,0,0]
**/
const float a = 6378137.0f; // Equatorial Radius
const float e = 8.1819190842622e-2f; // Eccentricity
float x = ECEF[0], y = ECEF[1], z = ECEF[2];
float Lat, N, NplusH, delta, esLat;
const double a = 6378137.0f; // Equatorial Radius
const double e = 8.1819190842622e-2f; // Eccentricity
double x = ECEF[0], y = ECEF[1], z = ECEF[2];
double Lat, N, NplusH, delta, esLat;
uint16_t iter;
#define MAX_ITER 10 // should not take more than 5 for valid coordinates
#define ACCURACY 1.0e-11f // used to be e-14, but we don't need sub micrometer exact calculations
#define ACCURACY 1.0e-11d // used to be e-14, but we don't need sub micrometer exact calculations
LLA[1] = RAD2DEG(atan2f(y, x));
Lat = DEG2RAD(LLA[0]);
esLat = e * sinf(Lat);
N = a / sqrtf(1 - esLat * esLat);
NplusH = N + LLA[2];
LLA[1] = (float)RAD2DEG_D(atan2(y, x));
Lat = DEG2RAD_D((double)LLA[0]);
esLat = e * sin(Lat);
N = a / sqrt(1 - esLat * esLat);
NplusH = N + (double)LLA[2];
delta = 1;
iter = 0;
while (((delta > ACCURACY) || (delta < -ACCURACY))
&& (iter < MAX_ITER)) {
delta = Lat - atanf(z / (sqrtf(x * x + y * y) * (1 - (N * e * e / NplusH))));
delta = Lat - atan(z / (sqrt(x * x + y * y) * (1 - (N * e * e / NplusH))));
Lat = Lat - delta;
esLat = e * sinf(Lat);
N = a / sqrtf(1 - esLat * esLat);
NplusH = sqrtf(x * x + y * y) / cosf(Lat);
esLat = e * sin(Lat);
N = a / sqrt(1 - esLat * esLat);
NplusH = sqrt(x * x + y * y) / cos(Lat);
iter += 1;
}
LLA[0] = RAD2DEG(Lat);
LLA[0] = RAD2DEG_D(Lat);
LLA[2] = NplusH - N;
return iter < MAX_ITER;
}
// ****** find ECEF to NED rotation matrix ********
void RneFromLLA(float LLA[3], float Rne[3][3])
void RneFromLLA(float LLA[3], double Rne[3][3])
{
float sinLat, sinLon, cosLat, cosLon;
double sinLat, sinLon, cosLat, cosLon;
sinLat = (float)sinf(DEG2RAD(LLA[0]));
sinLon = (float)sinf(DEG2RAD(LLA[1]));
cosLat = (float)cosf(DEG2RAD(LLA[0]));
cosLon = (float)cosf(DEG2RAD(LLA[1]));
sinLat = sin(DEG2RAD(LLA[0]));
sinLon = sin(DEG2RAD(LLA[1]));
cosLat = cos(DEG2RAD(LLA[0]));
cosLon = cos(DEG2RAD(LLA[1]));
Rne[0][0] = -sinLat * cosLon;
Rne[0][1] = -sinLat * sinLon;
@ -188,16 +188,16 @@ void Quaternion2R(float q[4], float Rbe[3][3])
}
// ****** Express LLA in a local NED Base Frame ********
void LLA2Base(float LLA[3], float BaseECEF[3], float Rne[3][3], float NED[3])
void LLA2Base(float LLA[3], double BaseECEF[3], double Rne[3][3], float NED[3])
{
float ECEF[3];
float diff[3];
double ECEF[3];
double diff[3];
LLA2ECEF(LLA, ECEF);
diff[0] = (float)(ECEF[0] - BaseECEF[0]);
diff[1] = (float)(ECEF[1] - BaseECEF[1]);
diff[2] = (float)(ECEF[2] - BaseECEF[2]);
diff[0] = (ECEF[0] - BaseECEF[0]);
diff[1] = (ECEF[1] - BaseECEF[1]);
diff[2] = (ECEF[2] - BaseECEF[2]);
NED[0] = Rne[0][0] * diff[0] + Rne[0][1] * diff[1] + Rne[0][2] * diff[2];
NED[1] = Rne[1][0] * diff[0] + Rne[1][1] * diff[1] + Rne[1][2] * diff[2];
@ -205,13 +205,13 @@ void LLA2Base(float LLA[3], float BaseECEF[3], float Rne[3][3], float NED[3])
}
// ****** Express ECEF in a local NED Base Frame ********
void ECEF2Base(float ECEF[3], float BaseECEF[3], float Rne[3][3], float NED[3])
void ECEF2Base(double ECEF[3], double BaseECEF[3], double Rne[3][3], float NED[3])
{
float diff[3];
double diff[3];
diff[0] = (float)(ECEF[0] - BaseECEF[0]);
diff[1] = (float)(ECEF[1] - BaseECEF[1]);
diff[2] = (float)(ECEF[2] - BaseECEF[2]);
diff[0] = (ECEF[0] - BaseECEF[0]);
diff[1] = (ECEF[1] - BaseECEF[1]);
diff[2] = (ECEF[2] - BaseECEF[2]);
NED[0] = Rne[0][0] * diff[0] + Rne[0][1] * diff[1] + Rne[0][2] * diff[2];
NED[1] = Rne[1][0] * diff[0] + Rne[1][1] * diff[1] + Rne[1][2] * diff[2];

10
flight/libraries/inc/CoordinateConversions.h
View File

@ -31,12 +31,12 @@
#define COORDINATECONVERSIONS_H_
// ****** convert Lat,Lon,Alt to ECEF ************
void LLA2ECEF(float LLA[3], float ECEF[3]);
void LLA2ECEF(float LLA[3], double ECEF[3]);
// ****** convert ECEF to Lat,Lon,Alt (ITERATIVE!) *********
uint16_t ECEF2LLA(float ECEF[3], float LLA[3]);
uint16_t ECEF2LLA(double ECEF[3], float LLA[3]);
void RneFromLLA(float LLA[3], float Rne[3][3]);
void RneFromLLA(float LLA[3], double Rne[3][3]);
// ****** find rotation matrix from rotation vector
void Rv2Rot(float Rv[3], float R[3][3]);
@ -51,10 +51,10 @@ void RPY2Quaternion(const float rpy[3], float q[4]);
void Quaternion2R(float q[4], float Rbe[3][3]);
// ****** Express LLA in a local NED Base Frame ********
void LLA2Base(float LLA[3], float BaseECEF[3], float Rne[3][3], float NED[3]);
void LLA2Base(float LLA[3], double BaseECEF[3], double Rne[3][3], float NED[3]);
// ****** Express ECEF in a local NED Base Frame ********
void ECEF2Base(float ECEF[3], float BaseECEF[3], float Rne[3][3], float NED[3]);
void ECEF2Base(double ECEF[3], double BaseECEF[3], double Rne[3][3], float NED[3]);
// ****** convert Rotation Matrix to Quaternion ********
// ****** if R converts from e to b, q is rotation from e to b ****

15
flight/libraries/insgps13state.c
View File

@ -44,7 +44,7 @@
void CovariancePrediction(float F[NUMX][NUMX], float G[NUMX][NUMW],
float Q[NUMW], float dT, float P[NUMX][NUMX]);
void SerialUpdate(float H[NUMV][NUMX], float R[NUMV], float Z[NUMV],
float Y[NUMV], float P[NUMX][NUMX], float X[NUMX], float K[NUMX][NUMV],
float Y[NUMV], float P[NUMX][NUMX], float X[NUMX],
uint16_t SensorsUsed);
void RungeKutta(float X[NUMX], float U[NUMU], float dT);
void StateEq(float X[NUMX], float U[NUMU], float Xdot[NUMX]);
@ -97,7 +97,6 @@ static struct EKFData {
// input noise and measurement noise variances
float Q[NUMW];
float R[NUMV];
float K[NUMX][NUMV]; // feedback gain matrix
} ekf;
// Global variables
@ -129,7 +128,6 @@ void INSGPSInit() // pretty much just a place holder for now
for (int j = 0; j < NUMV; j++) {
ekf.H[j][i] = 0.0f;
ekf.K[i][j] = 0.0f;
}
ekf.X[i] = 0.0f;
@ -402,7 +400,7 @@ void INSCorrection(float mag_data[3], float Pos[3], float Vel[3],
// EKF correction step
LinearizeH(ekf.X, ekf.Be, ekf.H);
MeasurementEq(ekf.X, ekf.Be, Y);
SerialUpdate(ekf.H, ekf.R, Z, Y, ekf.P, ekf.X, ekf.K, SensorsUsed);
SerialUpdate(ekf.H, ekf.R, Z, Y, ekf.P, ekf.X, SensorsUsed);
qmag = sqrtf(ekf.X[6] * ekf.X[6] + ekf.X[7] * ekf.X[7] + ekf.X[8] * ekf.X[8] + ekf.X[9] * ekf.X[9]);
ekf.X[6] /= qmag;
ekf.X[7] /= qmag;
@ -515,11 +513,12 @@ void CovariancePrediction(float F[NUMX][NUMX], float G[NUMX][NUMW],
// ************************************************
void SerialUpdate(float H[NUMV][NUMX], float R[NUMV], float Z[NUMV],
float Y[NUMV], float P[NUMX][NUMX], float X[NUMX], float K[NUMX][NUMV],
float Y[NUMV], float P[NUMX][NUMX], float X[NUMX],
uint16_t SensorsUsed)
{
float HP[NUMX], HPHR, Error;
uint8_t i, j, k, m;
float Km[NUMX];
for (m = 0; m < NUMV; m++) {
if (SensorsUsed & (0x01 << m)) { // use this sensor for update
@ -535,18 +534,18 @@ void SerialUpdate(float H[NUMV][NUMX], float R[NUMV], float Z[NUMV],
}
for (k = 0; k < NUMX; k++) {
K[k][m] = HP[k] / HPHR; // find K = HP/HPHR
Km[k] = HP[k] / HPHR; // find K = HP/HPHR
}
for (i = 0; i < NUMX; i++) { // Find P(m)= P(m-1) + K*HP
for (j = i; j < NUMX; j++) {
P[i][j] = P[j][i] =
P[i][j] - K[i][m] * HP[j];
P[i][j] - Km[i] * HP[j];
}
}
Error = Z[m] - Y[m];
for (i = 0; i < NUMX; i++) { // Find X(m)= X(m-1) + K*Error
X[i] = X[i] + K[i][m] * Error;
X[i] = X[i] + Km[i] * Error;
}
}
}

20
flight/modules/Airspeed/revolution/gps_airspeed.c
View File

@ -32,8 +32,8 @@
#include "openpilot.h"
#include "gps_airspeed.h"
#include "gpsvelocity.h"
#include "attitudeactual.h"
#include "gpsvelocitysensor.h"
#include "attitudestate.h"
#include "CoordinateConversions.h"
#include <pios_math.h>
@ -66,15 +66,15 @@ void gps_airspeedInitialize()
gps = (struct GPSGlobals *)pvPortMalloc(sizeof(struct GPSGlobals));
// GPS airspeed calculation variables
GPSVelocityData gpsVelData;
GPSVelocityGet(&gpsVelData);
GPSVelocitySensorData gpsVelData;
GPSVelocitySensorGet(&gpsVelData);
gps->gpsVelOld_N = gpsVelData.North;
gps->gpsVelOld_E = gpsVelData.East;
gps->gpsVelOld_D = gpsVelData.Down;
AttitudeActualData attData;
AttitudeActualGet(&attData);
AttitudeStateData attData;
AttitudeStateGet(&attData);
float Rbe[3][3];
float q[4] = { attData.q1, attData.q2, attData.q3, attData.q4 };
@ -101,8 +101,8 @@ void gps_airspeedGet(float *v_air_GPS)
float Rbe[3][3];
{ // Scoping to save memory. We really just need Rbe.
AttitudeActualData attData;
AttitudeActualGet(&attData);
AttitudeStateData attData;
AttitudeStateGet(&attData);
float q[4] = { attData.q1, attData.q2, attData.q3, attData.q4 };
@ -115,8 +115,8 @@ void gps_airspeedGet(float *v_air_GPS)
// If there's more than a 5 degree difference between two fuselage measurements, then we have sufficient delta to continue.
if (fabsf(cosDiff) < cosf(DEG2RAD(5.0f))) {
GPSVelocityData gpsVelData;
GPSVelocityGet(&gpsVelData);
GPSVelocitySensorData gpsVelData;
GPSVelocitySensorGet(&gpsVelData);
// Calculate the norm^2 of the difference between the two GPS vectors
float normDiffGPS2 = powf(gpsVelData.North - gps->gpsVelOld_N, 2.0f) + powf(gpsVelData.East - gps->gpsVelOld_E, 2.0f) + powf(gpsVelData.Down - gps->gpsVelOld_D, 2.0f);

14
flight/modules/Altitude/altitude.c
View File

@ -3,7 +3,7 @@
* @addtogroup OpenPilotModules OpenPilot Modules
* @{
* @addtogroup AltitudeModule Altitude Module
* @brief Communicate with BMP085 and update @ref BaroAltitude "BaroAltitude UAV Object"
* @brief Communicate with BMP085 and update @ref BaroSensor "BaroSensor UAV Object"
* @{
*
* @file altitude.c
@ -30,9 +30,9 @@
*/
/**
* Output object: BaroAltitude
* Output object: BaroSensor
*
* This module will periodically update the value of the BaroAltitude object.
* This module will periodically update the value of the BaroSensor object.
*
*/
@ -41,7 +41,7 @@
#include "hwsettings.h"
#include "altitude.h"
#if defined(PIOS_INCLUDE_BMP085)
#include "baroaltitude.h" // object that will be updated by the module
#include "barosensor.h" // object that will be updated by the module
#endif
#if defined(PIOS_INCLUDE_HCSR04)
#include "sonaraltitude.h" // object that will be updated by the module
@ -80,7 +80,7 @@ int32_t AltitudeStart()
{
if (altitudeEnabled) {
#if defined(PIOS_INCLUDE_BMP085)
BaroAltitudeInitialize();
BaroSensorInitialize();
#endif
#if defined(PIOS_INCLUDE_HCSR04)
SonarAltitudeInitialize();
@ -139,7 +139,7 @@ static void altitudeTask(__attribute__((unused)) void *parameters)
}
#endif
#if defined(PIOS_INCLUDE_BMP085)
BaroAltitudeData data;
BaroSensorData data;
PIOS_BMP085_Init();
#endif
@ -206,7 +206,7 @@ static void altitudeTask(__attribute__((unused)) void *parameters)
data.Altitude = 44330.0 * (1.0 - powf((data.Pressure / (BMP085_P0 / 1000.0)), (1.0 / 5.255)));
// Update the AltitudeActual UAVObject
BaroAltitudeSet(&data);
BaroSensorSet(&data);
}
#endif /* if defined(PIOS_INCLUDE_BMP085) */

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

@ -3,7 +3,7 @@
* @addtogroup OpenPilotModules OpenPilot Modules
* @{
* @addtogroup AltitudeModule Altitude Module
* @brief Communicate with BMP085 and update @ref BaroAltitude "BaroAltitude UAV Object"
* @brief Communicate with BMP085 and update @ref BaroSensor "BaroSensor UAV Object"
* @{
*
* @file altitude.c
@ -30,16 +30,16 @@
*/
/**
* Output object: BaroAltitude
* Output object: BaroSensor
*
* This module will periodically update the value of the BaroAltitude object.
* This module will periodically update the value of the BaroSensor object.
*
*/
#include <openpilot.h>
#include "altitude.h"
#include "baroaltitude.h" // object that will be updated by the module
#include "barosensor.h" // object that will be updated by the module
#if defined(PIOS_INCLUDE_HCSR04)
#include "sonaraltitude.h" // object that will be updated by the module
#endif
@ -76,7 +76,7 @@ int32_t AltitudeStart()
*/
int32_t AltitudeInitialize()
{
BaroAltitudeInitialize();
BaroSensorInitialize();
#if defined(PIOS_INCLUDE_HCSR04)
SonarAltitudeInitialize();
#endif
@ -88,7 +88,7 @@ MODULE_INITCALL(AltitudeInitialize, AltitudeStart);
*/
static void altitudeTask(__attribute__((unused)) void *parameters)
{
BaroAltitudeData data;
BaroSensorData data;
#if defined(PIOS_INCLUDE_HCSR04)
SonarAltitudeData sonardata;
@ -165,8 +165,8 @@ static void altitudeTask(__attribute__((unused)) void *parameters)
data.Altitude = altitude;
data.Temperature = temp;
data.Pressure = press;
// Update the AltitudeActual UAVObject
BaroAltitudeSet(&data);
// Update the BasoSensor UAVObject
BaroSensorSet(&data);
}
}
}

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

@ -28,7 +28,7 @@
/**
* Input object: ActiveWaypoint
* Input object: PositionActual
* Input object: PositionState
* Input object: ManualControlCommand
* Output object: AttitudeDesired
*
@ -48,14 +48,14 @@
#include <math.h>
#include <CoordinateConversions.h>
#include <altholdsmoothed.h>
#include <attitudeactual.h>
#include <attitudestate.h>
#include <altitudeholdsettings.h>
#include <altitudeholddesired.h> // object that will be updated by the module
#include <baroaltitude.h>
#include <positionactual.h>
#include <barosensor.h>
#include <positionstate.h>
#include <flightstatus.h>
#include <stabilizationdesired.h>
#include <accels.h>
#include <accelstate.h>
#include <taskinfo.h>
#include <pios_constants.h>
// Private constants
@ -136,11 +136,11 @@ static void altitudeHoldTask(__attribute__((unused)) void *parameters)
bool enterFailSafe = false;
// Listen for updates.
AltitudeHoldDesiredConnectQueue(queue);
BaroAltitudeConnectQueue(queue);
BaroSensorConnectQueue(queue);
FlightStatusConnectQueue(queue);
AccelsConnectQueue(queue);
AccelStateConnectQueue(queue);
bool altitudeHoldFlightMode = false;
BaroAltitudeAltitudeGet(&smoothed_altitude);
BaroSensorAltitudeGet(&smoothed_altitude);
running = false;
enum init_state { WAITING_BARO, WAITIING_INIT, INITED } init = WAITING_BARO;
@ -160,7 +160,7 @@ static void altitudeHoldTask(__attribute__((unused)) void *parameters)
// Todo: Add alarm if it should be running
continue;
} else if (ev.obj == BaroAltitudeHandle()) {
} else if (ev.obj == BaroSensorHandle()) {
baro_updated = true;
init = (init == WAITING_BARO) ? WAITIING_INIT : init;
@ -182,7 +182,7 @@ static void altitudeHoldTask(__attribute__((unused)) void *parameters)
running = false;
lastAltitudeHoldDesiredUpdate = PIOS_DELAY_GetRaw();
}
} else if (ev.obj == AccelsHandle()) {
} else if (ev.obj == AccelStateHandle()) {
static uint32_t timeval;
static float z[4] = { 0, 0, 0, 0 };
@ -205,17 +205,17 @@ static void altitudeHoldTask(__attribute__((unused)) void *parameters)
S[1] = altitudeHoldSettings.AccelNoise;
G[2] = altitudeHoldSettings.AccelDrift;
AccelsData accels;
AccelsGet(&accels);
AttitudeActualData attitudeActual;
AttitudeActualGet(&attitudeActual);
BaroAltitudeData baro;
BaroAltitudeGet(&baro);
AccelStateData accelState;
AccelStateGet(&accelState);
AttitudeStateData attitudeState;
AttitudeStateGet(&attitudeState);
BaroSensorData baro;
BaroSensorGet(&baro);
/* Downsample accels to stop this calculation consuming too much CPU */
accels_accum[0] += accels.x;
accels_accum[1] += accels.y;
accels_accum[2] += accels.z;
accels_accum[0] += accelState.x;
accels_accum[1] += accelState.y;
accels_accum[2] += accelState.z;
accel_downsample_count++;
if (accel_downsample_count < ACCEL_DOWNSAMPLE) {
@ -223,9 +223,9 @@ static void altitudeHoldTask(__attribute__((unused)) void *parameters)
}
accel_downsample_count = 0;
accels.x = accels_accum[0] / ACCEL_DOWNSAMPLE;
accels.y = accels_accum[1] / ACCEL_DOWNSAMPLE;
accels.z = accels_accum[2] / ACCEL_DOWNSAMPLE;
accelState.x = accels_accum[0] / ACCEL_DOWNSAMPLE;
accelState.y = accels_accum[1] / ACCEL_DOWNSAMPLE;
accelState.z = accels_accum[2] / ACCEL_DOWNSAMPLE;
accels_accum[0] = accels_accum[1] = accels_accum[2] = 0;
thisTime = xTaskGetTickCount();
@ -233,7 +233,7 @@ static void altitudeHoldTask(__attribute__((unused)) void *parameters)
if (init == WAITIING_INIT) {
z[0] = baro.Altitude;
z[1] = 0;
z[2] = accels.z;
z[2] = accelState.z;
z[3] = 0;
init = INITED;
} else if (init == WAITING_BARO) {
@ -244,14 +244,14 @@ static void altitudeHoldTask(__attribute__((unused)) void *parameters)
// rotate avg accels into earth frame and store it
if (1) {
float q[4], Rbe[3][3];
q[0] = attitudeActual.q1;
q[1] = attitudeActual.q2;
q[2] = attitudeActual.q3;
q[3] = attitudeActual.q4;
q[0] = attitudeState.q1;
q[1] = attitudeState.q2;
q[2] = attitudeState.q3;
q[3] = attitudeState.q4;
Quaternion2R(q, Rbe);
x[1] = -(Rbe[0][2] * accels.x + Rbe[1][2] * accels.y + Rbe[2][2] * accels.z + 9.81f);
x[1] = -(Rbe[0][2] * accelState.x + Rbe[1][2] * accelState.y + Rbe[2][2] * accelState.z + 9.81f);
} else {
x[1] = -accels.z + 9.81f;
x[1] = -accelState.z + 9.81f;
}
dT = PIOS_DELAY_DiffuS(timeval) / 1.0e6f;

106
flight/modules/Attitude/attitude.c
View File

@ -4,7 +4,7 @@
* @{
* @addtogroup Attitude Copter Control Attitude Estimation
* @brief Acquires sensor data and computes attitude estimate
* Specifically updates the the @ref AttitudeActual "AttitudeActual" and @ref AttitudeRaw "AttitudeRaw" settings objects
* Specifically updates the the @ref AttitudeState "AttitudeState" and @ref AttitudeRaw "AttitudeRaw" settings objects
* @{
*
* @file attitude.c
@ -32,7 +32,7 @@
/**
* Input objects: None, takes sensor data via pios
* Output objects: @ref AttitudeRaw @ref AttitudeActual
* Output objects: @ref AttitudeRaw @ref AttitudeState
*
* This module computes an attitude estimate from the sensor data
*
@ -53,9 +53,9 @@
#include <pios_board_info.h>
#include "attitude.h"
#include "gyros.h"
#include "accels.h"
#include "attitudeactual.h"
#include "gyrostate.h"
#include "accelstate.h"
#include "attitudestate.h"
#include "attitudesettings.h"
#include "flightstatus.h"
#include "manualcontrolcommand.h"
@ -83,9 +83,9 @@ static void AttitudeTask(void *parameters);
static float gyro_correct_int[3] = { 0, 0, 0 };
static xQueueHandle gyro_queue;
static int32_t updateSensors(AccelsData *, GyrosData *);
static int32_t updateSensorsCC3D(AccelsData *accelsData, GyrosData *gyrosData);
static void updateAttitude(AccelsData *, GyrosData *);
static int32_t updateSensors(AccelStateData *, GyroStateData *);
static int32_t updateSensorsCC3D(AccelStateData *accelStateData, GyroStateData *gyrosData);
static void updateAttitude(AccelStateData *, GyroStateData *);
static void settingsUpdatedCb(UAVObjEvent *objEv);
static float accelKi = 0;
@ -134,19 +134,19 @@ int32_t AttitudeStart(void)
*/
int32_t AttitudeInitialize(void)
{
AttitudeActualInitialize();
AttitudeStateInitialize();
AttitudeSettingsInitialize();
AccelsInitialize();
GyrosInitialize();
AccelStateInitialize();
GyroStateInitialize();
// Initialize quaternion
AttitudeActualData attitude;
AttitudeActualGet(&attitude);
AttitudeStateData attitude;
AttitudeStateGet(&attitude);
attitude.q1 = 1;
attitude.q2 = 0;
attitude.q3 = 0;
attitude.q4 = 0;
AttitudeActualSet(&attitude);
AttitudeStateSet(&attitude);
// Cannot trust the values to init right above if BL runs
gyro_correct_int[0] = 0;
@ -248,14 +248,14 @@ static void AttitudeTask(__attribute__((unused)) void *parameters)
PIOS_WDG_UpdateFlag(PIOS_WDG_ATTITUDE);
AccelsData accels;
GyrosData gyros;
AccelStateData accelState;
GyroStateData gyros;
int32_t retval = 0;
if (cc3d) {
retval = updateSensorsCC3D(&accels, &gyros);
retval = updateSensorsCC3D(&accelState, &gyros);
} else {
retval = updateSensors(&accels, &gyros);
retval = updateSensors(&accelState, &gyros);
}
// Only update attitude when sensor data is good
@ -263,8 +263,8 @@ static void AttitudeTask(__attribute__((unused)) void *parameters)
AlarmsSet(SYSTEMALARMS_ALARM_ATTITUDE, SYSTEMALARMS_ALARM_ERROR);
} else {
// Do not update attitude data in simulation mode
if (!AttitudeActualReadOnly()) {
updateAttitude(&accels, &gyros);
if (!AttitudeStateReadOnly()) {
updateAttitude(&accelState, &gyros);
}
AlarmsClear(SYSTEMALARMS_ALARM_ATTITUDE);
@ -279,7 +279,7 @@ float gyros_passed[3];
* @param[in] attitudeRaw Populate the UAVO instead of saving right here
* @return 0 if successfull, -1 if not
*/
static int32_t updateSensors(AccelsData *accels, GyrosData *gyros)
static int32_t updateSensors(AccelStateData *accelState, GyroStateData *gyros)
{
struct pios_adxl345_data accel_data;
float gyro[4];
@ -291,7 +291,7 @@ static int32_t updateSensors(AccelsData *accels, GyrosData *gyros)
}
// Do not read raw sensor data in simulation mode
if (GyrosReadOnly() || AccelsReadOnly()) {
if (GyroStateReadOnly() || AccelStateReadOnly()) {
return 0;
}
@ -317,7 +317,7 @@ static int32_t updateSensors(AccelsData *accels, GyrosData *gyros)
y += -accel_data.y;
z += -accel_data.z;
} while ((i < 32) && (samples_remaining > 0));
gyros->temperature = samples_remaining;
// gyros->temperature = samples_remaining;
float accel[3] = { (float)x / i, (float)y / i, (float)z / i };
@ -325,17 +325,17 @@ static int32_t updateSensors(AccelsData *accels, GyrosData *gyros)
// TODO: rotate sensors too so stabilization is well behaved
float vec_out[3];
rot_mult(R, accel, vec_out);
accels->x = vec_out[0];
accels->y = vec_out[1];
accels->z = vec_out[2];
accelState->x = vec_out[0];
accelState->y = vec_out[1];
accelState->z = vec_out[2];
rot_mult(R, &gyros->x, vec_out);
gyros->x = vec_out[0];
gyros->y = vec_out[1];
gyros->z = vec_out[2];
} else {
accels->x = accel[0];
accels->y = accel[1];
accels->z = accel[2];
accelState->x = accel[0];
accelState->y = accel[1];
accelState->z = accel[2];
}
if (trim_requested) {
@ -349,17 +349,17 @@ static int32_t updateSensors(AccelsData *accels, GyrosData *gyros)
if ((armed == FLIGHTSTATUS_ARMED_ARMED) && (throttle > 0.0f)) {
trim_samples++;
// Store the digitally scaled version since that is what we use for bias
trim_accels[0] += accels->x;
trim_accels[1] += accels->y;
trim_accels[2] += accels->z;
trim_accels[0] += accelState->x;
trim_accels[1] += accelState->y;
trim_accels[2] += accelState->z;
}
}
}
// Scale accels and correct bias
accels->x = (accels->x - accelbias[0]) * ACCEL_SCALE;
accels->y = (accels->y - accelbias[1]) * ACCEL_SCALE;
accels->z = (accels->z - accelbias[2]) * ACCEL_SCALE;
accelState->x = (accelState->x - accelbias[0]) * ACCEL_SCALE;
accelState->y = (accelState->y - accelbias[1]) * ACCEL_SCALE;
accelState->z = (accelState->z - accelbias[2]) * ACCEL_SCALE;
if (bias_correct_gyro) {
// Applying integral component here so it can be seen on the gyros and correct bias
@ -376,8 +376,8 @@ static int32_t updateSensors(AccelsData *accels, GyrosData *gyros)
// and make it average zero (weakly)
gyro_correct_int[2] += -gyros->z * yawBiasRate;
GyrosSet(gyros);
AccelsSet(accels);
GyroStateSet(gyros);
AccelStateSet(accelState);
return 0;
}
@ -388,7 +388,7 @@ static int32_t updateSensors(AccelsData *accels, GyrosData *gyros)
* @return 0 if successfull, -1 if not
*/
struct pios_mpu6000_data mpu6000_data;
static int32_t updateSensorsCC3D(AccelsData *accelsData, GyrosData *gyrosData)
static int32_t updateSensorsCC3D(AccelStateData *accelStateData, GyroStateData *gyrosData)
{
float accels[3], gyros[3];
@ -400,7 +400,7 @@ static int32_t updateSensorsCC3D(AccelsData *accelsData, GyrosData *gyrosData)
return -1; // Error, no data
}
// Do not read raw sensor data in simulation mode
if (GyrosReadOnly() || AccelsReadOnly()) {
if (GyroStateReadOnly() || AccelStateReadOnly()) {
return 0;
}
@ -412,8 +412,8 @@ static int32_t updateSensorsCC3D(AccelsData *accelsData, GyrosData *gyrosData)
accels[1] = mpu6000_data.accel_y * PIOS_MPU6000_GetAccelScale();
accels[2] = mpu6000_data.accel_z * PIOS_MPU6000_GetAccelScale();
gyrosData->temperature = 35.0f + ((float)mpu6000_data.temperature + 512.0f) / 340.0f;
accelsData->temperature = 35.0f + ((float)mpu6000_data.temperature + 512.0f) / 340.0f;
// gyrosData->temperature = 35.0f + ((float)mpu6000_data.temperature + 512.0f) / 340.0f;
// accelsData->temperature = 35.0f + ((float)mpu6000_data.temperature + 512.0f) / 340.0f;
#endif /* if defined(PIOS_INCLUDE_MPU6000) */
if (rotate) {
@ -429,9 +429,9 @@ static int32_t updateSensorsCC3D(AccelsData *accelsData, GyrosData *gyrosData)
gyros[2] = vec_out[2];
}
accelsData->x = accels[0] - accelbias[0] * ACCEL_SCALE; // Applying arbitrary scale here to match CC v1
accelsData->y = accels[1] - accelbias[1] * ACCEL_SCALE;
accelsData->z = accels[2] - accelbias[2] * ACCEL_SCALE;
accelStateData->x = accels[0] - accelbias[0] * ACCEL_SCALE; // Applying arbitrary scale here to match CC v1
accelStateData->y = accels[1] - accelbias[1] * ACCEL_SCALE;
accelStateData->z = accels[2] - accelbias[2] * ACCEL_SCALE;
gyrosData->x = gyros[0];
gyrosData->y = gyros[1];
@ -452,8 +452,8 @@ static int32_t updateSensorsCC3D(AccelsData *accelsData, GyrosData *gyrosData)
// and make it average zero (weakly)
gyro_correct_int[2] += -gyrosData->z * yawBiasRate;
GyrosSet(gyrosData);
AccelsSet(accelsData);
GyroStateSet(gyrosData);
AccelStateSet(accelStateData);
return 0;
}
@ -471,7 +471,7 @@ static inline void apply_accel_filter(const float *raw, float *filtered)
}
}
static void updateAttitude(AccelsData *accelsData, GyrosData *gyrosData)
static void updateAttitude(AccelStateData *accelStateData, GyroStateData *gyrosData)
{
float dT;
portTickType thisSysTime = xTaskGetTickCount();
@ -482,7 +482,7 @@ static void updateAttitude(AccelsData *accelsData, GyrosData *gyrosData)
// Bad practice to assume structure order, but saves memory
float *gyros = &gyrosData->x;
float *accels = &accelsData->x;
float *accels = &accelStateData->x;
float grot[3];
float accel_err[3];
@ -573,15 +573,15 @@ static void updateAttitude(AccelsData *accelsData, GyrosData *gyrosData)
q[3] = q[3] / qmag;
}
AttitudeActualData attitudeActual;
AttitudeActualGet(&attitudeActual);
AttitudeStateData attitudeState;
AttitudeStateGet(&attitudeState);
quat_copy(q, &attitudeActual.q1);
quat_copy(q, &attitudeState.q1);
// Convert into eueler degrees (makes assumptions about RPY order)
Quaternion2RPY(&attitudeActual.q1, &attitudeActual.Roll);
Quaternion2RPY(&attitudeState.q1, &attitudeState.Roll);
AttitudeActualSet(&attitudeActual);
AttitudeStateSet(&attitudeState);
}
static void settingsUpdatedCb(__attribute__((unused)) UAVObjEvent *objEv)

536
flight/modules/Attitude/revolution/attitude.c
View File

@ -4,7 +4,7 @@
* @{
* @addtogroup Attitude Copter Control Attitude Estimation
* @brief Acquires sensor data and computes attitude estimate
* Specifically updates the the @ref AttitudeActual "AttitudeActual" and @ref AttitudeRaw "AttitudeRaw" settings objects
* Specifically updates the the @ref AttitudeState "AttitudeState" and @ref AttitudeRaw "AttitudeRaw" settings objects
* @{
*
* @file attitude.c
@ -32,7 +32,7 @@
/**
* Input objects: None, takes sensor data via pios
* Output objects: @ref AttitudeRaw @ref AttitudeActual
* Output objects: @ref AttitudeRaw @ref AttitudeState
*
* This module computes an attitude estimate from the sensor data
*
@ -51,25 +51,27 @@
#include <openpilot.h>
#include "attitude.h"
#include "accels.h"
#include "accelsensor.h"
#include "accelstate.h"
#include "airspeedsensor.h"
#include "airspeedactual.h"
#include "attitudeactual.h"
#include "airspeedstate.h"
#include "attitudestate.h"
#include "attitudesettings.h"
#include "baroaltitude.h"
#include "barosensor.h"
#include "flightstatus.h"
#include "gpsposition.h"
#include "gpsvelocity.h"
#include "gyros.h"
#include "gyrosbias.h"
#include "gpspositionsensor.h"
#include "gpsvelocitysensor.h"
#include "gyrostate.h"
#include "gyrosensor.h"
#include "homelocation.h"
#include "magnetometer.h"
#include "positionactual.h"
#include "magsensor.h"
#include "magstate.h"
#include "positionstate.h"
#include "ekfconfiguration.h"
#include "ekfstatevariance.h"
#include "revocalibration.h"
#include "revosettings.h"
#include "velocityactual.h"
#include "velocitystate.h"
#include "taskinfo.h"
#include "CoordinateConversions.h"
@ -130,7 +132,9 @@ static int32_t updateAttitudeComplementary(bool first_run);
static int32_t updateAttitudeINSGPS(bool first_run, bool outdoor_mode);
static void settingsUpdatedCb(UAVObjEvent *objEv);
static int32_t getNED(GPSPositionData *gpsPosition, float *NED);
static int32_t getNED(GPSPositionSensorData *gpsPosition, float *NED);
static void magOffsetEstimation(MagSensorData *mag);
// check for invalid values
static inline bool invalid(float data)
@ -168,12 +172,21 @@ static inline bool invalid_var(float data)
*/
int32_t AttitudeInitialize(void)
{
AttitudeActualInitialize();
AirspeedActualInitialize();
GyroSensorInitialize();
GyroStateInitialize();
AccelSensorInitialize();
AccelStateInitialize();
MagSensorInitialize();
MagStateInitialize();
AirspeedSensorInitialize();
AirspeedStateInitialize();
BaroSensorInitialize();
GPSPositionSensorInitialize();
GPSVelocitySensorInitialize();
AttitudeSettingsInitialize();
PositionActualInitialize();
VelocityActualInitialize();
AttitudeStateInitialize();
PositionStateInitialize();
VelocityStateInitialize();
RevoSettingsInitialize();
RevoCalibrationInitialize();
EKFConfigurationInitialize();
@ -184,21 +197,13 @@ int32_t AttitudeInitialize(void)
HomeLocationInitialize();
// Initialize quaternion
AttitudeActualData attitude;
AttitudeActualGet(&attitude);
AttitudeStateData attitude;
AttitudeStateGet(&attitude);
attitude.q1 = 1.0f;
attitude.q2 = 0.0f;
attitude.q3 = 0.0f;
attitude.q4 = 0.0f;
AttitudeActualSet(&attitude);
// Cannot trust the values to init right above if BL runs
GyrosBiasData gyrosBias;
GyrosBiasGet(&gyrosBias);
gyrosBias.x = 0.0f;
gyrosBias.y = 0.0f;
gyrosBias.z = 0.0f;
GyrosBiasSet(&gyrosBias);
AttitudeStateSet(&attitude);
AttitudeSettingsConnectCallback(&settingsUpdatedCb);
RevoSettingsConnectCallback(&settingsUpdatedCb);
@ -232,13 +237,13 @@ int32_t AttitudeStart(void)
PIOS_WDG_RegisterFlag(PIOS_WDG_ATTITUDE);
#endif
GyrosConnectQueue(gyroQueue);
AccelsConnectQueue(accelQueue);
MagnetometerConnectQueue(magQueue);
GyroSensorConnectQueue(gyroQueue);
AccelSensorConnectQueue(accelQueue);
MagSensorConnectQueue(magQueue);
AirspeedSensorConnectQueue(airspeedQueue);
BaroAltitudeConnectQueue(baroQueue);
GPSPositionConnectQueue(gpsQueue);
GPSVelocityConnectQueue(gpsVelQueue);
BaroSensorConnectQueue(baroQueue);
GPSPositionSensorConnectQueue(gpsQueue);
GPSVelocitySensorConnectQueue(gpsVelQueue);
return 0;
}
@ -273,10 +278,10 @@ static void AttitudeTask(__attribute__((unused)) void *parameters)
case REVOSETTINGS_FUSIONALGORITHM_COMPLEMENTARY:
ret_val = updateAttitudeComplementary(first_run);
break;
case REVOSETTINGS_FUSIONALGORITHM_INSOUTDOOR:
case REVOSETTINGS_FUSIONALGORITHM_INS13OUTDOOR:
ret_val = updateAttitudeINSGPS(first_run, true);
break;
case REVOSETTINGS_FUSIONALGORITHM_INSINDOOR:
case REVOSETTINGS_FUSIONALGORITHM_INS13INDOOR:
ret_val = updateAttitudeINSGPS(first_run, false);
break;
default:
@ -315,11 +320,13 @@ float mag_err[3];
static int32_t updateAttitudeComplementary(bool first_run)
{
UAVObjEvent ev;
GyrosData gyrosData;
AccelsData accelsData;
GyroSensorData gyroSensorData;
GyroStateData gyroStateData;
AccelSensorData accelSensorData;
static int32_t timeval;
float dT;
static uint8_t init = 0;
static float gyro_bias[3] = { 0, 0, 0 };
static bool magCalibrated = true;
static uint32_t initStartupTime = 0;
@ -328,13 +335,20 @@ static int32_t updateAttitudeComplementary(bool first_run)
xQueueReceive(accelQueue, &ev, 1 / portTICK_RATE_MS) != pdTRUE) {
// When one of these is updated so should the other
// Do not set attitude timeout warnings in simulation mode
if (!AttitudeActualReadOnly()) {
if (!AttitudeStateReadOnly()) {
AlarmsSet(SYSTEMALARMS_ALARM_ATTITUDE, SYSTEMALARMS_ALARM_WARNING);
return -1;
}
}
AccelsGet(&accelsData);
AccelSensorGet(&accelSensorData);
// TODO: put in separate filter
AccelStateData accelState;
accelState.x = accelSensorData.x;
accelState.y = accelSensorData.y;
accelState.z = accelSensorData.z;
AccelStateSet(&accelState);
// During initialization and
if (first_run) {
@ -343,10 +357,10 @@ static int32_t updateAttitudeComplementary(bool first_run)
if (xQueueReceive(magQueue, &ev, 0 / portTICK_RATE_MS) != pdTRUE) {
return -1;
}
MagnetometerData magData;
MagnetometerGet(&magData);
MagSensorData magData;
MagSensorGet(&magData);
#else
MagnetometerData magData;
MagSensorData magData;
magData.x = 100.0f;
magData.y = 0.0f;
magData.z = 0.0f;
@ -360,35 +374,35 @@ static int32_t updateAttitudeComplementary(bool first_run)
magData.y = 0.0f;
magData.z = 0.0f;
}
AttitudeActualData attitudeActual;
AttitudeActualGet(&attitudeActual);
AttitudeStateData attitudeState;
AttitudeStateGet(&attitudeState);
init = 0;
// Set initial attitude. Use accels to determine roll and pitch, rotate magnetic measurement accordingly,
// so pseudo "north" vector can be estimated even if the board is not level
attitudeActual.Roll = atan2f(-accelsData.y, -accelsData.z);
float zn = cosf(attitudeActual.Roll) * magData.z + sinf(attitudeActual.Roll) * magData.y;
float yn = cosf(attitudeActual.Roll) * magData.y - sinf(attitudeActual.Roll) * magData.z;
attitudeState.Roll = atan2f(-accelSensorData.y, -accelSensorData.z);
float zn = cosf(attitudeState.Roll) * magData.z + sinf(attitudeState.Roll) * magData.y;
float yn = cosf(attitudeState.Roll) * magData.y - sinf(attitudeState.Roll) * magData.z;
// rotate accels z vector according to roll
float azn = cosf(attitudeActual.Roll) * accelsData.z + sinf(attitudeActual.Roll) * accelsData.y;
attitudeActual.Pitch = atan2f(accelsData.x, -azn);
float azn = cosf(attitudeState.Roll) * accelSensorData.z + sinf(attitudeState.Roll) * accelSensorData.y;
attitudeState.Pitch = atan2f(accelSensorData.x, -azn);
float xn = cosf(attitudeActual.Pitch) * magData.x + sinf(attitudeActual.Pitch) * zn;
float xn = cosf(attitudeState.Pitch) * magData.x + sinf(attitudeState.Pitch) * zn;
attitudeActual.Yaw = atan2f(-yn, xn);
attitudeState.Yaw = atan2f(-yn, xn);
// TODO: This is still a hack
// Put this in a proper generic function in CoordinateConversion.c
// should take 4 vectors: g (0,0,-9.81), accels, Be (or 1,0,0 if no home loc) and magnetometers (or 1,0,0 if no mags)
// should calculate the rotation in 3d space using proper cross product math
// SUBTODO: formulate the math required
attitudeActual.Roll = RAD2DEG(attitudeActual.Roll);
attitudeActual.Pitch = RAD2DEG(attitudeActual.Pitch);
attitudeActual.Yaw = RAD2DEG(attitudeActual.Yaw);
attitudeState.Roll = RAD2DEG(attitudeState.Roll);
attitudeState.Pitch = RAD2DEG(attitudeState.Pitch);
attitudeState.Yaw = RAD2DEG(attitudeState.Yaw);
RPY2Quaternion(&attitudeActual.Roll, &attitudeActual.q1);
AttitudeActualSet(&attitudeActual);
RPY2Quaternion(&attitudeState.Roll, &attitudeState.q1);
AttitudeStateSet(&attitudeState);
timeval = PIOS_DELAY_GetRaw();
@ -401,12 +415,9 @@ static int32_t updateAttitudeComplementary(bool first_run)
// Zero gyro bias
// This is really needed after updating calibration settings.
GyrosBiasData zeroGyrosBias;
GyrosBiasGet(&zeroGyrosBias);
zeroGyrosBias.x = 0;
zeroGyrosBias.y = 0;
zeroGyrosBias.z = 0;
GyrosBiasSet(&zeroGyrosBias);
gyro_bias[0] = 0.0f;
gyro_bias[1] = 0.0f;
gyro_bias[2] = 0.0f;
return 0;
}
@ -439,7 +450,10 @@ static int32_t updateAttitudeComplementary(bool first_run)
init = 1;
}
GyrosGet(&gyrosData);
GyroSensorGet(&gyroSensorData);
gyroStateData.x = gyroSensorData.x;
gyroStateData.y = gyroSensorData.y;
gyroStateData.z = gyroSensorData.z;
// Compute the dT using the cpu clock
dT = PIOS_DELAY_DiffuS(timeval) / 1000000.0f;
@ -447,17 +461,17 @@ static int32_t updateAttitudeComplementary(bool first_run)
float q[4];
AttitudeActualData attitudeActual;
AttitudeActualGet(&attitudeActual);
AttitudeStateData attitudeState;
AttitudeStateGet(&attitudeState);
float grot[3];
float accel_err[3];
// Get the current attitude estimate
quat_copy(&attitudeActual.q1, q);
quat_copy(&attitudeState.q1, q);
// Apply smoothing to accel values, to reduce vibration noise before main calculations.
apply_accel_filter((const float *)&accelsData.x, accels_filtered);
apply_accel_filter((const float *)&accelSensorData.x, accels_filtered);
// Rotate gravity to body frame and cross with accels
grot[0] = -(2.0f * (q[1] * q[3] - q[0] * q[2]));
@ -490,10 +504,20 @@ static int32_t updateAttitudeComplementary(bool first_run)
// Rotate gravity to body frame and cross with accels
float brot[3];
float Rbe[3][3];
MagnetometerData mag;
MagSensorData mag;
Quaternion2R(q, Rbe);
MagnetometerGet(&mag);
MagSensorGet(&mag);
// TODO: separate filter!
if (revoCalibration.MagBiasNullingRate > 0) {
magOffsetEstimation(&mag);
}
MagStateData mags;
mags.x = mag.x;
mags.y = mag.y;
mags.z = mag.z;
MagStateSet(&mags);
// If the mag is producing bad data don't use it (normally bad calibration)
if (!isnan(mag.x) && !isinf(mag.x) && !isnan(mag.y) && !isinf(mag.y) && !isnan(mag.z) && !isinf(mag.z)) {
@ -521,32 +545,30 @@ static int32_t updateAttitudeComplementary(bool first_run)
}
// Accumulate integral of error. Scale here so that units are (deg/s) but Ki has units of s
GyrosBiasData gyrosBias;
GyrosBiasGet(&gyrosBias);
if (revoCalibration.BiasCorrectedRaw == REVOCALIBRATION_BIASCORRECTEDRAW_FALSE) {
// if the raw values are not adjusted, we need to adjust here.
gyrosData.x -= gyrosBias.x;
gyrosData.y -= gyrosBias.y;
gyrosData.z -= gyrosBias.z;
}
// Correct rates based on integral coefficient
gyroStateData.x -= gyro_bias[0];
gyroStateData.y -= gyro_bias[1];
gyroStateData.z -= gyro_bias[2];
gyrosBias.x -= accel_err[0] * attitudeSettings.AccelKi - gyrosData.x * rollPitchBiasRate;
gyrosBias.y -= accel_err[1] * attitudeSettings.AccelKi - gyrosData.y * rollPitchBiasRate;
gyrosBias.z -= -mag_err[2] * attitudeSettings.MagKi - gyrosData.z * rollPitchBiasRate;
GyrosBiasSet(&gyrosBias);
gyro_bias[0] -= accel_err[0] * attitudeSettings.AccelKi - (gyroStateData.x) * rollPitchBiasRate;
gyro_bias[1] -= accel_err[1] * attitudeSettings.AccelKi - (gyroStateData.y) * rollPitchBiasRate;
gyro_bias[2] -= -mag_err[2] * attitudeSettings.MagKi - (gyroStateData.z) * rollPitchBiasRate;
// Correct rates based on error, integral component dealt with in updateSensors
gyrosData.x += accel_err[0] * attitudeSettings.AccelKp / dT;
gyrosData.y += accel_err[1] * attitudeSettings.AccelKp / dT;
gyrosData.z += accel_err[2] * attitudeSettings.AccelKp / dT + mag_err[2] * attitudeSettings.MagKp / dT;
// save gyroscope state
GyroStateSet(&gyroStateData);
// Correct rates based on proportional coefficient
gyroStateData.x += accel_err[0] * attitudeSettings.AccelKp / dT;
gyroStateData.y += accel_err[1] * attitudeSettings.AccelKp / dT;
gyroStateData.z += accel_err[2] * attitudeSettings.AccelKp / dT + mag_err[2] * attitudeSettings.MagKp / dT;
// Work out time derivative from INSAlgo writeup
// Also accounts for the fact that gyros are in deg/s
float qdot[4];
qdot[0] = DEG2RAD(-q[1] * gyrosData.x - q[2] * gyrosData.y - q[3] * gyrosData.z) * dT / 2;
qdot[1] = DEG2RAD(q[0] * gyrosData.x - q[3] * gyrosData.y + q[2] * gyrosData.z) * dT / 2;
qdot[2] = DEG2RAD(q[3] * gyrosData.x + q[0] * gyrosData.y - q[1] * gyrosData.z) * dT / 2;
qdot[3] = DEG2RAD(-q[2] * gyrosData.x + q[1] * gyrosData.y + q[0] * gyrosData.z) * dT / 2;
qdot[0] = DEG2RAD(-q[1] * gyroStateData.x - q[2] * gyroStateData.y - q[3] * gyroStateData.z) * dT / 2;
qdot[1] = DEG2RAD(q[0] * gyroStateData.x - q[3] * gyroStateData.y + q[2] * gyroStateData.z) * dT / 2;
qdot[2] = DEG2RAD(q[3] * gyroStateData.x + q[0] * gyroStateData.y - q[1] * gyroStateData.z) * dT / 2;
qdot[3] = DEG2RAD(-q[2] * gyroStateData.x + q[1] * gyroStateData.y + q[0] * gyroStateData.z) * dT / 2;
// Take a time step
q[0] = q[0] + qdot[0];
@ -577,41 +599,41 @@ static int32_t updateAttitudeComplementary(bool first_run)
q[3] = 0.0f;
}
quat_copy(q, &attitudeActual.q1);
quat_copy(q, &attitudeState.q1);
// Convert into eueler degrees (makes assumptions about RPY order)
Quaternion2RPY(&attitudeActual.q1, &attitudeActual.Roll);
Quaternion2RPY(&attitudeState.q1, &attitudeState.Roll);
AttitudeActualSet(&attitudeActual);
AttitudeStateSet(&attitudeState);
// Flush these queues for avoid errors
xQueueReceive(baroQueue, &ev, 0);
if (xQueueReceive(gpsQueue, &ev, 0) == pdTRUE && homeLocation.Set == HOMELOCATION_SET_TRUE) {
float NED[3];
// Transform the GPS position into NED coordinates
GPSPositionData gpsPosition;
GPSPositionGet(&gpsPosition);
GPSPositionSensorData gpsPosition;
GPSPositionSensorGet(&gpsPosition);
getNED(&gpsPosition, NED);
PositionActualData positionActual;
PositionActualGet(&positionActual);
positionActual.North = NED[0];
positionActual.East = NED[1];
positionActual.Down = NED[2];
PositionActualSet(&positionActual);
PositionStateData positionState;
PositionStateGet(&positionState);
positionState.North = NED[0];
positionState.East = NED[1];
positionState.Down = NED[2];
PositionStateSet(&positionState);
}
if (xQueueReceive(gpsVelQueue, &ev, 0) == pdTRUE) {
// Transform the GPS position into NED coordinates
GPSVelocityData gpsVelocity;
GPSVelocityGet(&gpsVelocity);
GPSVelocitySensorData gpsVelocity;
GPSVelocitySensorGet(&gpsVelocity);
VelocityActualData velocityActual;
VelocityActualGet(&velocityActual);
velocityActual.North = gpsVelocity.North;
velocityActual.East = gpsVelocity.East;
velocityActual.Down = gpsVelocity.Down;
VelocityActualSet(&velocityActual);
VelocityStateData velocityState;
VelocityStateGet(&velocityState);
velocityState.North = gpsVelocity.North;
velocityState.East = gpsVelocity.East;
velocityState.Down = gpsVelocity.Down;
VelocityStateSet(&velocityState);
}
if (xQueueReceive(airspeedQueue, &ev, 0) == pdTRUE) {
@ -619,17 +641,17 @@ static int32_t updateAttitudeComplementary(bool first_run)
AirspeedSensorData airspeedSensor;
AirspeedSensorGet(&airspeedSensor);
AirspeedActualData airspeed;
AirspeedActualGet(&airspeed);
AirspeedStateData airspeed;
AirspeedStateGet(&airspeed);
PositionActualData positionActual;
PositionActualGet(&positionActual);
PositionStateData positionState;
PositionStateGet(&positionState);
if (airspeedSensor.SensorConnected == AIRSPEEDSENSOR_SENSORCONNECTED_TRUE) {
// we have airspeed available
airspeed.CalibratedAirspeed = airspeedSensor.CalibratedAirspeed;
airspeed.TrueAirspeed = airspeed.CalibratedAirspeed * IAS2TAS(homeLocation.Altitude - positionActual.Down);
AirspeedActualSet(&airspeed);
airspeed.TrueAirspeed = airspeed.CalibratedAirspeed * IAS2TAS(homeLocation.Altitude - positionState.Down);
AirspeedStateSet(&airspeed);
}
}
@ -659,14 +681,13 @@ int32_t init_stage = 0;
static int32_t updateAttitudeINSGPS(bool first_run, bool outdoor_mode)
{
UAVObjEvent ev;
GyrosData gyrosData;
AccelsData accelsData;
MagnetometerData magData;
GyroSensorData gyroSensorData;
AccelSensorData accelSensorData;
MagStateData magData;
AirspeedSensorData airspeedData;
BaroAltitudeData baroData;
GPSPositionData gpsData;
GPSVelocityData gpsVelData;
GyrosBiasData gyrosBias;
BaroSensorData baroData;
GPSPositionSensorData gpsData;
GPSVelocitySensorData gpsVelData;
static bool mag_updated = false;
static bool baro_updated;
@ -693,7 +714,7 @@ static int32_t updateAttitudeINSGPS(bool first_run, bool outdoor_mode)
if ((xQueueReceive(gyroQueue, &ev, FAILSAFE_TIMEOUT_MS / portTICK_RATE_MS) != pdTRUE) ||
(xQueueReceive(accelQueue, &ev, 1 / portTICK_RATE_MS) != pdTRUE)) {
// Do not set attitude timeout warnings in simulation mode
if (!AttitudeActualReadOnly()) {
if (!AttitudeStateReadOnly()) {
AlarmsSet(SYSTEMALARMS_ALARM_ATTITUDE, SYSTEMALARMS_ALARM_WARNING);
return -1;
}
@ -727,47 +748,61 @@ static int32_t updateAttitudeINSGPS(bool first_run, bool outdoor_mode)
airspeed_updated |= xQueueReceive(airspeedQueue, &ev, 0 / portTICK_RATE_MS) == pdTRUE;
// Check if we are running simulation
if (!GPSPositionReadOnly()) {
if (!GPSPositionSensorReadOnly()) {
gps_updated |= (xQueueReceive(gpsQueue, &ev, 0 / portTICK_RATE_MS) == pdTRUE) && outdoor_mode;
} else {
gps_updated |= pdTRUE && outdoor_mode;
}
if (!GPSVelocityReadOnly()) {
if (!GPSVelocitySensorReadOnly()) {
gps_vel_updated |= (xQueueReceive(gpsVelQueue, &ev, 0 / portTICK_RATE_MS) == pdTRUE) && outdoor_mode;
} else {
gps_vel_updated |= pdTRUE && outdoor_mode;
}
// Get most recent data
GyrosGet(&gyrosData);
AccelsGet(&accelsData);
MagnetometerGet(&magData);
BaroAltitudeGet(&baroData);
GyroSensorGet(&gyroSensorData);
AccelSensorGet(&accelSensorData);
// TODO: separate filter!
if (mag_updated) {
MagSensorData mags;
MagSensorGet(&mags);
if (revoCalibration.MagBiasNullingRate > 0) {
magOffsetEstimation(&mags);
}
magData.x = mags.x;
magData.y = mags.y;
magData.z = mags.z;
MagStateSet(&magData);
} else {
MagStateGet(&magData);
}
BaroSensorGet(&baroData);
AirspeedSensorGet(&airspeedData);
GPSPositionGet(&gpsData);
GPSVelocityGet(&gpsVelData);
GyrosBiasGet(&gyrosBias);
GPSPositionSensorGet(&gpsData);
GPSVelocitySensorGet(&gpsVelData);
// TODO: put in separate filter
AccelStateData accelState;
accelState.x = accelSensorData.x;
accelState.y = accelSensorData.y;
accelState.z = accelSensorData.z;
AccelStateSet(&accelState);
value_error = false;
// safety checks
if (invalid(gyrosData.x) ||
invalid(gyrosData.y) ||
invalid(gyrosData.z) ||
invalid(accelsData.x) ||
invalid(accelsData.y) ||
invalid(accelsData.z)) {
if (invalid(gyroSensorData.x) ||
invalid(gyroSensorData.y) ||
invalid(gyroSensorData.z) ||
invalid(accelSensorData.x) ||
invalid(accelSensorData.y) ||
invalid(accelSensorData.z)) {
// cannot run process update, raise error!
AlarmsSet(SYSTEMALARMS_ALARM_ATTITUDE, SYSTEMALARMS_ALARM_ERROR);
return 0;
}
if (invalid(gyrosBias.x) ||
invalid(gyrosBias.y) ||
invalid(gyrosBias.z)) {
gyrosBias.x = 0.0f;
gyrosBias.y = 0.0f;
gyrosBias.z = 0.0f;
}
if (invalid(magData.x) ||
invalid(magData.y) ||
@ -882,8 +917,8 @@ static int32_t updateAttitudeINSGPS(bool first_run, bool outdoor_mode)
float pos[3] = { 0.0f, 0.0f, 0.0f };
if (outdoor_mode) {
GPSPositionData gpsPosition;
GPSPositionGet(&gpsPosition);
GPSPositionSensorData gpsPosition;
GPSPositionSensorGet(&gpsPosition);
// Transform the GPS position into NED coordinates
getNED(&gpsPosition, pos);
@ -896,39 +931,39 @@ static int32_t updateAttitudeINSGPS(bool first_run, bool outdoor_mode)
pos[2] = -(baroData.Altitude + baroOffset);
}
xQueueReceive(magQueue, &ev, 100 / portTICK_RATE_MS);
MagnetometerGet(&magData);
// xQueueReceive(magQueue, &ev, 100 / portTICK_RATE_MS);
// MagSensorGet(&magData);
AttitudeActualData attitudeActual;
AttitudeActualGet(&attitudeActual);
AttitudeStateData attitudeState;
AttitudeStateGet(&attitudeState);
// Set initial attitude. Use accels to determine roll and pitch, rotate magnetic measurement accordingly,
// so pseudo "north" vector can be estimated even if the board is not level
attitudeActual.Roll = atan2f(-accelsData.y, -accelsData.z);
float zn = cosf(attitudeActual.Roll) * magData.z + sinf(attitudeActual.Roll) * magData.y;
float yn = cosf(attitudeActual.Roll) * magData.y - sinf(attitudeActual.Roll) * magData.z;
attitudeState.Roll = atan2f(-accelSensorData.y, -accelSensorData.z);
float zn = cosf(attitudeState.Roll) * magData.z + sinf(attitudeState.Roll) * magData.y;
float yn = cosf(attitudeState.Roll) * magData.y - sinf(attitudeState.Roll) * magData.z;
// rotate accels z vector according to roll
float azn = cosf(attitudeActual.Roll) * accelsData.z + sinf(attitudeActual.Roll) * accelsData.y;
attitudeActual.Pitch = atan2f(accelsData.x, -azn);
float azn = cosf(attitudeState.Roll) * accelSensorData.z + sinf(attitudeState.Roll) * accelSensorData.y;
attitudeState.Pitch = atan2f(accelSensorData.x, -azn);
float xn = cosf(attitudeActual.Pitch) * magData.x + sinf(attitudeActual.Pitch) * zn;
float xn = cosf(attitudeState.Pitch) * magData.x + sinf(attitudeState.Pitch) * zn;
attitudeActual.Yaw = atan2f(-yn, xn);
attitudeState.Yaw = atan2f(-yn, xn);
// TODO: This is still a hack
// Put this in a proper generic function in CoordinateConversion.c
// should take 4 vectors: g (0,0,-9.81), accels, Be (or 1,0,0 if no home loc) and magnetometers (or 1,0,0 if no mags)
// should calculate the rotation in 3d space using proper cross product math
// SUBTODO: formulate the math required
attitudeActual.Roll = RAD2DEG(attitudeActual.Roll);
attitudeActual.Pitch = RAD2DEG(attitudeActual.Pitch);
attitudeActual.Yaw = RAD2DEG(attitudeActual.Yaw);
attitudeState.Roll = RAD2DEG(attitudeState.Roll);
attitudeState.Pitch = RAD2DEG(attitudeState.Pitch);
attitudeState.Yaw = RAD2DEG(attitudeState.Yaw);
RPY2Quaternion(&attitudeActual.Roll, &attitudeActual.q1);
AttitudeActualSet(&attitudeActual);
RPY2Quaternion(&attitudeState.Roll, &attitudeState.q1);
AttitudeStateSet(&attitudeState);
float q[4] = { attitudeActual.q1, attitudeActual.q2, attitudeActual.q3, attitudeActual.q4 };
float q[4] = { attitudeState.q1, attitudeState.q2, attitudeState.q3, attitudeState.q4 };
INSSetState(pos, zeros, q, zeros, zeros);
INSResetP(ekfConfiguration.P);
@ -937,22 +972,17 @@ static int32_t updateAttitudeINSGPS(bool first_run, bool outdoor_mode)
// Because the sensor module remove the bias we need to add it
// back in here so that the INS algorithm can track it correctly
float gyros[3] = { DEG2RAD(gyrosData.x), DEG2RAD(gyrosData.y), DEG2RAD(gyrosData.z) };
if (revoCalibration.BiasCorrectedRaw == REVOCALIBRATION_BIASCORRECTEDRAW_TRUE) {
gyros[0] += DEG2RAD(gyrosBias.x);
gyros[1] += DEG2RAD(gyrosBias.y);
gyros[2] += DEG2RAD(gyrosBias.z);
}
INSStatePrediction(gyros, &accelsData.x, dT);
float gyros[3] = { DEG2RAD(gyroSensorData.x), DEG2RAD(gyroSensorData.y), DEG2RAD(gyroSensorData.z) };
INSStatePrediction(gyros, &accelSensorData.x, dT);
AttitudeActualData attitude;
AttitudeActualGet(&attitude);
AttitudeStateData attitude;
AttitudeStateGet(&attitude);
attitude.q1 = Nav.q[0];
attitude.q2 = Nav.q[1];
attitude.q3 = Nav.q[2];
attitude.q4 = Nav.q[3];
Quaternion2RPY(&attitude.q1, &attitude.Roll);
AttitudeActualSet(&attitude);
AttitudeStateSet(&attitude);
}
init_stage++;
@ -969,25 +999,20 @@ static int32_t updateAttitudeINSGPS(bool first_run, bool outdoor_mode)
// Because the sensor module remove the bias we need to add it
// back in here so that the INS algorithm can track it correctly
float gyros[3] = { DEG2RAD(gyrosData.x), DEG2RAD(gyrosData.y), DEG2RAD(gyrosData.z) };
if (revoCalibration.BiasCorrectedRaw == REVOCALIBRATION_BIASCORRECTEDRAW_TRUE) {
gyros[0] += DEG2RAD(gyrosBias.x);
gyros[1] += DEG2RAD(gyrosBias.y);
gyros[2] += DEG2RAD(gyrosBias.z);
}
float gyros[3] = { DEG2RAD(gyroSensorData.x), DEG2RAD(gyroSensorData.y), DEG2RAD(gyroSensorData.z) };
// Advance the state estimate
INSStatePrediction(gyros, &accelsData.x, dT);
INSStatePrediction(gyros, &accelSensorData.x, dT);
// Copy the attitude into the UAVO
AttitudeActualData attitude;
AttitudeActualGet(&attitude);
AttitudeStateData attitude;
AttitudeStateGet(&attitude);
attitude.q1 = Nav.q[0];
attitude.q2 = Nav.q[1];
attitude.q3 = Nav.q[2];
attitude.q4 = Nav.q[3];
Quaternion2RPY(&attitude.q1, &attitude.Roll);
AttitudeActualSet(&attitude);
AttitudeStateSet(&attitude);
// Advance the covariance estimate
INSCovariancePrediction(dT);
@ -1049,12 +1074,12 @@ static int32_t updateAttitudeINSGPS(bool first_run, bool outdoor_mode)
if (airspeed_updated) {
// we have airspeed available
AirspeedActualData airspeed;
AirspeedActualGet(&airspeed);
AirspeedStateData airspeed;
AirspeedStateGet(&airspeed);
airspeed.CalibratedAirspeed = airspeedData.CalibratedAirspeed;
airspeed.TrueAirspeed = airspeed.CalibratedAirspeed * IAS2TAS(homeLocation.Altitude - Nav.Pos[2]);
AirspeedActualSet(&airspeed);
AirspeedStateSet(&airspeed);
if (!gps_vel_updated && !gps_updated) {
// feed airspeed into EKF, treat wind as 1e2 variance
@ -1083,24 +1108,25 @@ static int32_t updateAttitudeINSGPS(bool first_run, bool outdoor_mode)
}
// Copy the position and velocity into the UAVO
PositionActualData positionActual;
PositionActualGet(&positionActual);
positionActual.North = Nav.Pos[0];
positionActual.East = Nav.Pos[1];
positionActual.Down = Nav.Pos[2];
PositionActualSet(&positionActual);
PositionStateData positionState;
PositionStateGet(&positionState);
positionState.North = Nav.Pos[0];
positionState.East = Nav.Pos[1];
positionState.Down = Nav.Pos[2];
PositionStateSet(&positionState);
VelocityActualData velocityActual;
VelocityActualGet(&velocityActual);
velocityActual.North = Nav.Vel[0];
velocityActual.East = Nav.Vel[1];
velocityActual.Down = Nav.Vel[2];
VelocityActualSet(&velocityActual);
VelocityStateData velocityState;
VelocityStateGet(&velocityState);
velocityState.North = Nav.Vel[0];
velocityState.East = Nav.Vel[1];
velocityState.Down = Nav.Vel[2];
VelocityStateSet(&velocityState);
gyrosBias.x = RAD2DEG(Nav.gyro_bias[0]);
gyrosBias.y = RAD2DEG(Nav.gyro_bias[1]);
gyrosBias.z = RAD2DEG(Nav.gyro_bias[2]);
GyrosBiasSet(&gyrosBias);
GyroStateData gyroState;
gyroState.x = RAD2DEG(gyros[0] - RAD2DEG(Nav.gyro_bias[0]));
gyroState.y = RAD2DEG(gyros[1] - RAD2DEG(Nav.gyro_bias[1]));
gyroState.z = RAD2DEG(gyros[2] - RAD2DEG(Nav.gyro_bias[2]));
GyroStateSet(&gyroState);
EKFStateVarianceData vardata;
EKFStateVarianceGet(&vardata);
@ -1120,7 +1146,7 @@ static int32_t updateAttitudeINSGPS(bool first_run, bool outdoor_mode)
* @returns 0 for success, -1 for failure
*/
float T[3];
static int32_t getNED(GPSPositionData *gpsPosition, float *NED)
static int32_t getNED(GPSPositionSensorData *gpsPosition, float *NED)
{
float dL[3] = { DEG2RAD((gpsPosition->Latitude - homeLocation.Latitude) / 10.0e6f),
DEG2RAD((gpsPosition->Longitude - homeLocation.Longitude) / 10.0e6f),
@ -1198,7 +1224,7 @@ static void settingsUpdatedCb(UAVObjEvent *ev)
T[1] = cosf(lat) * (alt + 6.378137E6f);
T[2] = -1.0f;
// TODO: convert positionActual to new reference frame and gracefully update EKF state!
// TODO: convert positionState to new reference frame and gracefully update EKF state!
// needed for long range flights where the reference coordinate is adjusted in flight
}
if (ev == NULL || ev->obj == AttitudeSettingsHandle()) {
@ -1215,6 +1241,104 @@ static void settingsUpdatedCb(UAVObjEvent *ev)
}
}
}
/**
* Perform an update of the @ref MagBias based on
* Magmeter Offset Cancellation: Theory and Implementation,
* revisited William Premerlani, October 14, 2011
*/
static void magOffsetEstimation(MagSensorData *mag)
{
#if 0
// Constants, to possibly go into a UAVO
static const float MIN_NORM_DIFFERENCE = 50;
static float B2[3] = { 0, 0, 0 };
MagBiasData magBias;
MagBiasGet(&magBias);
// Remove the current estimate of the bias
mag->x -= magBias.x;
mag->y -= magBias.y;
mag->z -= magBias.z;
// First call
if (B2[0] == 0 && B2[1] == 0 && B2[2] == 0) {
B2[0] = mag->x;
B2[1] = mag->y;
B2[2] = mag->z;
return;
}
float B1[3] = { mag->x, mag->y, mag->z };
float norm_diff = sqrtf(powf(B2[0] - B1[0], 2) + powf(B2[1] - B1[1], 2) + powf(B2[2] - B1[2], 2));
if (norm_diff > MIN_NORM_DIFFERENCE) {
float norm_b1 = sqrtf(B1[0] * B1[0] + B1[1] * B1[1] + B1[2] * B1[2]);
float norm_b2 = sqrtf(B2[0] * B2[0] + B2[1] * B2[1] + B2[2] * B2[2]);
float scale = cal.MagBiasNullingRate * (norm_b2 - norm_b1) / norm_diff;
float b_error[3] = { (B2[0] - B1[0]) * scale, (B2[1] - B1[1]) * scale, (B2[2] - B1[2]) * scale };
magBias.x += b_error[0];
magBias.y += b_error[1];
magBias.z += b_error[2];
MagBiasSet(&magBias);
// Store this value to compare against next update
B2[0] = B1[0]; B2[1] = B1[1]; B2[2] = B1[2];
}
#else // if 0
static float magBias[3] = { 0 };
// Remove the current estimate of the bias
mag->x -= magBias[0];
mag->y -= magBias[1];
mag->z -= magBias[2];
AttitudeStateData attitude;
AttitudeStateGet(&attitude);
const float Rxy = sqrtf(homeLocation.Be[0] * homeLocation.Be[0] + homeLocation.Be[1] * homeLocation.Be[1]);
const float Rz = homeLocation.Be[2];
const float rate = revoCalibration.MagBiasNullingRate;
float Rot[3][3];
float B_e[3];
float xy[2];
float delta[3];
// Get the rotation matrix
Quaternion2R(&attitude.q1, Rot);
// Rotate the mag into the NED frame
B_e[0] = Rot[0][0] * mag->x + Rot[1][0] * mag->y + Rot[2][0] * mag->z;
B_e[1] = Rot[0][1] * mag->x + Rot[1][1] * mag->y + Rot[2][1] * mag->z;
B_e[2] = Rot[0][2] * mag->x + Rot[1][2] * mag->y + Rot[2][2] * mag->z;
float cy = cosf(DEG2RAD(attitude.Yaw));
float sy = sinf(DEG2RAD(attitude.Yaw));
xy[0] = cy * B_e[0] + sy * B_e[1];
xy[1] = -sy * B_e[0] + cy * B_e[1];
float xy_norm = sqrtf(xy[0] * xy[0] + xy[1] * xy[1]);
delta[0] = -rate * (xy[0] / xy_norm * Rxy - xy[0]);
delta[1] = -rate * (xy[1] / xy_norm * Rxy - xy[1]);
delta[2] = -rate * (Rz - B_e[2]);
if (!isnan(delta[0]) && !isinf(delta[0]) &&
!isnan(delta[1]) && !isinf(delta[1]) &&
!isnan(delta[2]) && !isinf(delta[2])) {
magBias[0] += delta[0];
magBias[1] += delta[1];
magBias[2] += delta[2];
}
#endif // if 0
}
/**
* @}
* @}

18
flight/modules/CameraStab/camerastab.c
View File

@ -48,7 +48,7 @@
#include "openpilot.h"
#include "accessorydesired.h"
#include "attitudeactual.h"
#include "attitudestate.h"
#include "camerastabsettings.h"
#include "cameradesired.h"
#include "hwsettings.h"
@ -119,12 +119,12 @@ int32_t CameraStabInitialize(void)
memset(csd, 0, sizeof(struct CameraStab_data));
csd->lastSysTime = xTaskGetTickCount();
AttitudeActualInitialize();
AttitudeStateInitialize();
CameraStabSettingsInitialize();
CameraDesiredInitialize();
UAVObjEvent ev = {
.obj = AttitudeActualHandle(),
.obj = AttitudeStateHandle(),
.instId = 0,
.event = 0,
};
@ -146,7 +146,7 @@ MODULE_INITCALL(CameraStabInitialize, CameraStabStart);
static void attitudeUpdated(UAVObjEvent *ev)
{
if (ev->obj != AttitudeActualHandle()) {
if (ev->obj != AttitudeStateHandle()) {
return;
}
@ -195,13 +195,13 @@ static void attitudeUpdated(UAVObjEvent *ev)
switch (i) {
case CAMERASTABSETTINGS_INPUT_ROLL:
AttitudeActualRollGet(&attitude);
AttitudeStateRollGet(&attitude);
break;
case CAMERASTABSETTINGS_INPUT_PITCH:
AttitudeActualPitchGet(&attitude);
AttitudeStatePitchGet(&attitude);
break;
case CAMERASTABSETTINGS_INPUT_YAW:
AttitudeActualYawGet(&attitude);
AttitudeStateYawGet(&attitude);
break;
default:
PIOS_Assert(0);
@ -297,7 +297,7 @@ void applyFeedForward(uint8_t index, float dT_millis, float *attitude, CameraSta
case CAMERASTABSETTINGS_GIMBALTYPE_YAWROLLPITCH:
if (index == CAMERASTABSETTINGS_INPUT_ROLL) {
float pitch;
AttitudeActualPitchGet(&pitch);
AttitudeStatePitchGet(&pitch);
gimbalTypeCorrection = (cameraStab->OutputRange[CAMERASTABSETTINGS_OUTPUTRANGE_PITCH] - fabsf(pitch))
/ cameraStab->OutputRange[CAMERASTABSETTINGS_OUTPUTRANGE_PITCH];
}
@ -305,7 +305,7 @@ void applyFeedForward(uint8_t index, float dT_millis, float *attitude, CameraSta
case CAMERASTABSETTINGS_GIMBALTYPE_YAWPITCHROLL:
if (index == CAMERASTABSETTINGS_INPUT_PITCH) {
float roll;
AttitudeActualRollGet(&roll);
AttitudeStateRollGet(&roll);
gimbalTypeCorrection = (cameraStab->OutputRange[CAMERASTABSETTINGS_OUTPUTRANGE_ROLL] - fabsf(roll))
/ cameraStab->OutputRange[CAMERASTABSETTINGS_OUTPUTRANGE_ROLL];
}

2
flight/modules/Extensions/MagBaro/inc/magbaro.h
View File

@ -3,7 +3,7 @@
* @addtogroup OpenPilotModules OpenPilot Modules
* @{
* @addtogroup AltitudeModule Altitude Module
* @brief Communicate with BMP085 and update @ref AltitudeActual "AltitudeActual UAV Object"
* @brief Communicate with BMP085 and update @ref BaroSensor "BaroSensor UAV Object"
* @{
*
* @file altitude.h

24
flight/modules/Extensions/MagBaro/magbaro.c
View File

@ -3,7 +3,7 @@
* @addtogroup OpenPilotModules OpenPilot Modules
* @{
* @addtogroup AltitudeModule Altitude Module
* @brief Communicate with BMP085 and update @ref BaroAltitude "BaroAltitude UAV Object"
* @brief Communicate with BMP085 and update @ref BaroSensor "BaroSensor UAV Object"
* @{
*
* @file altitude.c
@ -30,9 +30,9 @@
*/
/**
* Output object: BaroAltitude
* Output object: BaroSensor
*
* This module will periodically update the value of the BaroAltitude object.
* This module will periodically update the value of the BaroSensor object.
*
*/
@ -40,8 +40,8 @@
#include "hwsettings.h"
#include "magbaro.h"
#include "baroaltitude.h" // object that will be updated by the module
#include "magnetometer.h"
#include "barosensor.h" // object that will be updated by the module
#include "magsensor.h"
#include "taskinfo.h"
// Private constants
@ -109,11 +109,11 @@ int32_t MagBaroInitialize()
if (magbaroEnabled) {
#if defined(PIOS_INCLUDE_HMC5883)
MagnetometerInitialize();
MagSensorInitialize();
#endif
#if defined(PIOS_INCLUDE_BMP085)
BaroAltitudeInitialize();
BaroSensorInitialize();
// init down-sampling data
alt_ds_temp = 0;
@ -144,12 +144,12 @@ static void magbaroTask(__attribute__((unused)) void *parameters)
portTickType lastSysTime;
#if defined(PIOS_INCLUDE_BMP085)
BaroAltitudeData data;
BaroSensorData data;
PIOS_BMP085_Init();
#endif
#if defined(PIOS_INCLUDE_HMC5883)
MagnetometerData mag;
MagSensorData mag;
PIOS_HMC5883_Init(&pios_hmc5883_cfg);
uint32_t mag_update_time = PIOS_DELAY_GetRaw();
#endif
@ -192,8 +192,8 @@ static void magbaroTask(__attribute__((unused)) void *parameters)
// Compute the current altitude (all pressures in kPa)
data.Altitude = 44330.0f * (1.0f - powf((data.Pressure / (BMP085_P0 / 1000.0f)), (1.0f / 5.255f)));
// Update the AltitudeActual UAVObject
BaroAltitudeSet(&data);
// Update the BaroSensor UAVObject
BaroSensorSet(&data);
}
#endif /* if defined(PIOS_INCLUDE_BMP085) */
@ -207,7 +207,7 @@ static void magbaroTask(__attribute__((unused)) void *parameters)
mag.x = mags[0];
mag.y = mags[1];
mag.z = mags[2];
MagnetometerSet(&mag);
MagSensorSet(&mag);
mag_update_time = PIOS_DELAY_GetRaw();
}
#endif

117
flight/modules/FixedWingPathFollower/fixedwingpathfollower.c
View File

@ -28,7 +28,7 @@
/**
* Input object: ActiveWaypoint
* Input object: PositionActual
* Input object: PositionState
* Input object: ManualControlCommand
* Output object: AttitudeDesired
*
@ -45,17 +45,14 @@
#include <openpilot.h>
#include "accels.h"
#include "hwsettings.h"
#include "attitudeactual.h"
#include "attitudestate.h"
#include "pathdesired.h" // object that will be updated by the module
#include "positionactual.h"
#include "positionstate.h"
#include "manualcontrol.h"
#include "flightstatus.h"
#include "pathstatus.h"
#include "airspeedactual.h"
#include "gpsvelocity.h"
#include "gpsposition.h"
#include "airspeedstate.h"
#include "fixedwingpathfollowersettings.h"
#include "fixedwingpathfollowerstatus.h"
#include "homelocation.h"
@ -63,7 +60,7 @@
#include "stabilizationsettings.h"
#include "systemsettings.h"
#include "velocitydesired.h"
#include "velocityactual.h"
#include "velocitystate.h"
#include "taskinfo.h"
#include "paths.h"
@ -87,7 +84,7 @@ static void SettingsUpdatedCb(UAVObjEvent *ev);
static void updatePathVelocity();
static uint8_t updateFixedDesiredAttitude();
static void updateFixedAttitude();
static void airspeedActualUpdatedCb(UAVObjEvent *ev);
static void airspeedStateUpdatedCb(UAVObjEvent *ev);
static float bound(float val, float min, float max);
/**
@ -121,7 +118,7 @@ int32_t FixedWingPathFollowerInitialize()
PathDesiredInitialize();
PathStatusInitialize();
VelocityDesiredInitialize();
AirspeedActualInitialize();
AirspeedStateInitialize();
} else {
followerEnabled = false;
}
@ -139,7 +136,7 @@ static float powerIntegral = 0;
static float airspeedErrorInt = 0;
// correct speed by measured airspeed
static float indicatedAirspeedActualBias = 0;
static float indicatedAirspeedStateBias = 0;
/**
* Module thread, should not return.
@ -151,7 +148,7 @@ static void pathfollowerTask(__attribute__((unused)) void *parameters)
portTickType lastUpdateTime;
AirspeedActualConnectCallback(airspeedActualUpdatedCb);
AirspeedStateConnectCallback(airspeedStateUpdatedCb);
FixedWingPathFollowerSettingsConnectCallback(SettingsUpdatedCb);
PathDesiredConnectCallback(SettingsUpdatedCb);
@ -247,21 +244,21 @@ static void pathfollowerTask(__attribute__((unused)) void *parameters)
/**
* Compute desired velocity from the current position and path
*
* Takes in @ref PositionActual and compares it to @ref PathDesired
* Takes in @ref PositionState and compares it to @ref PathDesired
* and computes @ref VelocityDesired
*/
static void updatePathVelocity()
{
PositionActualData positionActual;
PositionStateData positionState;
PositionActualGet(&positionActual);
VelocityActualData velocityActual;
VelocityActualGet(&velocityActual);
PositionStateGet(&positionState);
VelocityStateData velocityState;
VelocityStateGet(&velocityState);
// look ahead fixedwingpathfollowerSettings.HeadingFeedForward seconds
float cur[3] = { positionActual.North + (velocityActual.North * fixedwingpathfollowerSettings.HeadingFeedForward),
positionActual.East + (velocityActual.East * fixedwingpathfollowerSettings.HeadingFeedForward),
positionActual.Down + (velocityActual.Down * fixedwingpathfollowerSettings.HeadingFeedForward) };
float cur[3] = { positionState.North + (velocityState.North * fixedwingpathfollowerSettings.HeadingFeedForward),
positionState.East + (velocityState.East * fixedwingpathfollowerSettings.HeadingFeedForward),
positionState.Down + (velocityState.Down * fixedwingpathfollowerSettings.HeadingFeedForward) };
struct path_status progress;
path_progress(pathDesired.Start, pathDesired.End, cur, &progress, pathDesired.Mode);
@ -307,11 +304,11 @@ static void updatePathVelocity()
// turn steep unless there is enough space complete the turn before crossing the flightpath
// in this case the plane effectively needs to be turned around
// indicators:
// difference between correction_direction and velocityactual >90 degrees and
// difference between path_direction and velocityactual >90 degrees ( 4th sector, facing away from eerything )
// difference between correction_direction and velocitystate >90 degrees and
// difference between path_direction and velocitystate >90 degrees ( 4th sector, facing away from eerything )
// fix: ignore correction, steer in path direction until the situation has become better (condition doesn't apply anymore)
float angle1 = RAD2DEG(atan2f(progress.path_direction[1], progress.path_direction[0]) - atan2f(velocityActual.East, velocityActual.North));
float angle2 = RAD2DEG(atan2f(progress.correction_direction[1], progress.correction_direction[0]) - atan2f(velocityActual.East, velocityActual.North));
float angle1 = RAD2DEG(atan2f(progress.path_direction[1], progress.path_direction[0]) - atan2f(velocityState.East, velocityState.North));
float angle2 = RAD2DEG(atan2f(progress.correction_direction[1], progress.correction_direction[0]) - atan2f(velocityState.East, velocityState.North));
if (angle1 < -180.0f) {
angle1 += 360.0f;
}
@ -337,7 +334,7 @@ static void updatePathVelocity()
velocityDesired.North *= groundspeed / l;
velocityDesired.East *= groundspeed / l;
float downError = altitudeSetpoint - positionActual.Down;
float downError = altitudeSetpoint - positionState.Down;
velocityDesired.Down = downError * fixedwingpathfollowerSettings.VerticalPosP;
// update pathstatus
@ -370,9 +367,9 @@ static void updateFixedAttitude(float *attitude)
/**
* Compute desired attitude from the desired velocity
*
* Takes in @ref NedActual which has the acceleration in the
* Takes in @ref NedState which has the acceleration in the
* NED frame as the feedback term and then compares the
* @ref VelocityActual against the @ref VelocityDesired
* @ref VelocityState against the @ref VelocityDesired
*/
static uint8_t updateFixedDesiredAttitude()
{
@ -381,17 +378,16 @@ static uint8_t updateFixedDesiredAttitude()
float dT = fixedwingpathfollowerSettings.UpdatePeriod / 1000.0f; // Convert from [ms] to [s]
VelocityDesiredData velocityDesired;
VelocityActualData velocityActual;
VelocityStateData velocityState;
StabilizationDesiredData stabDesired;
AttitudeActualData attitudeActual;
AccelsData accels;
AttitudeStateData attitudeState;
StabilizationSettingsData stabSettings;
FixedWingPathFollowerStatusData fixedwingpathfollowerStatus;
AirspeedActualData airspeedActual;
AirspeedStateData airspeedState;
float groundspeedActual;
float groundspeedState;
float groundspeedDesired;
float indicatedAirspeedActual;
float indicatedAirspeedState;
float indicatedAirspeedDesired;
float airspeedError;
@ -406,13 +402,12 @@ static uint8_t updateFixedDesiredAttitude()
FixedWingPathFollowerStatusGet(&fixedwingpathfollowerStatus);
VelocityActualGet(&velocityActual);
VelocityStateGet(&velocityState);
StabilizationDesiredGet(&stabDesired);
VelocityDesiredGet(&velocityDesired);
AttitudeActualGet(&attitudeActual);
AccelsGet(&accels);
AttitudeStateGet(&attitudeState);
StabilizationSettingsGet(&stabSettings);
AirspeedActualGet(&airspeedActual);
AirspeedStateGet(&airspeedState);
/**
@ -420,59 +415,59 @@ static uint8_t updateFixedDesiredAttitude()
*/
// Current ground speed
groundspeedActual = sqrtf(velocityActual.East * velocityActual.East + velocityActual.North * velocityActual.North);
// note that airspeedActualBias is ( calibratedAirspeed - groundSpeed ) at the time of measurement,
groundspeedState = sqrtf(velocityState.East * velocityState.East + velocityState.North * velocityState.North);
// note that airspeedStateBias is ( calibratedAirspeed - groundSpeed ) at the time of measurement,
// but thanks to accelerometers, groundspeed reacts faster to changes in direction
// than airspeed and gps sensors alone
indicatedAirspeedActual = groundspeedActual + indicatedAirspeedActualBias;
indicatedAirspeedState = groundspeedState + indicatedAirspeedStateBias;
// Desired ground speed
groundspeedDesired = sqrtf(velocityDesired.North * velocityDesired.North + velocityDesired.East * velocityDesired.East);
indicatedAirspeedDesired = bound(groundspeedDesired + indicatedAirspeedActualBias,
indicatedAirspeedDesired = bound(groundspeedDesired + indicatedAirspeedStateBias,
fixedwingpathfollowerSettings.BestClimbRateSpeed,
fixedwingpathfollowerSettings.CruiseSpeed);
// Airspeed error
airspeedError = indicatedAirspeedDesired - indicatedAirspeedActual;
airspeedError = indicatedAirspeedDesired - indicatedAirspeedState;
// Vertical speed error
descentspeedDesired = bound(
velocityDesired.Down,
-fixedwingpathfollowerSettings.VerticalVelMax,
fixedwingpathfollowerSettings.VerticalVelMax);
descentspeedError = descentspeedDesired - velocityActual.Down;
descentspeedError = descentspeedDesired - velocityState.Down;
// Error condition: wind speed is higher than maximum allowed speed. We are forced backwards!
fixedwingpathfollowerStatus.Errors[FIXEDWINGPATHFOLLOWERSTATUS_ERRORS_WIND] = 0;
if (groundspeedDesired - indicatedAirspeedActualBias <= 0) {
if (groundspeedDesired - indicatedAirspeedStateBias <= 0) {
fixedwingpathfollowerStatus.Errors[FIXEDWINGPATHFOLLOWERSTATUS_ERRORS_WIND] = 1;
result = 0;
}
// Error condition: plane too slow or too fast
fixedwingpathfollowerStatus.Errors[FIXEDWINGPATHFOLLOWERSTATUS_ERRORS_HIGHSPEED] = 0;
fixedwingpathfollowerStatus.Errors[FIXEDWINGPATHFOLLOWERSTATUS_ERRORS_LOWSPEED] = 0;
if (indicatedAirspeedActual > fixedwingpathfollowerSettings.AirSpeedMax) {
if (indicatedAirspeedState > fixedwingpathfollowerSettings.AirSpeedMax) {
fixedwingpathfollowerStatus.Errors[FIXEDWINGPATHFOLLOWERSTATUS_ERRORS_OVERSPEED] = 1;
result = 0;
}
if (indicatedAirspeedActual > fixedwingpathfollowerSettings.CruiseSpeed * 1.2f) {
if (indicatedAirspeedState > fixedwingpathfollowerSettings.CruiseSpeed * 1.2f) {
fixedwingpathfollowerStatus.Errors[FIXEDWINGPATHFOLLOWERSTATUS_ERRORS_HIGHSPEED] = 1;
result = 0;
}
if (indicatedAirspeedActual < fixedwingpathfollowerSettings.BestClimbRateSpeed * 0.8f && 1) { // The next three && 1 are placeholders for UAVOs representing LANDING and TAKEOFF
if (indicatedAirspeedState < fixedwingpathfollowerSettings.BestClimbRateSpeed * 0.8f && 1) { // The next three && 1 are placeholders for UAVOs representing LANDING and TAKEOFF
fixedwingpathfollowerStatus.Errors[FIXEDWINGPATHFOLLOWERSTATUS_ERRORS_LOWSPEED] = 1;
result = 0;
}
if (indicatedAirspeedActual < fixedwingpathfollowerSettings.StallSpeedClean && 1 && 1) { // Where the && 1 represents the UAVO that will control whether the airplane is prepped for landing or not
if (indicatedAirspeedState < fixedwingpathfollowerSettings.StallSpeedClean && 1 && 1) { // Where the && 1 represents the UAVO that will control whether the airplane is prepped for landing or not
fixedwingpathfollowerStatus.Errors[FIXEDWINGPATHFOLLOWERSTATUS_ERRORS_STALLSPEED] = 1;
result = 0;
}
if (indicatedAirspeedActual < fixedwingpathfollowerSettings.StallSpeedDirty && 1) {
if (indicatedAirspeedState < fixedwingpathfollowerSettings.StallSpeedDirty && 1) {
fixedwingpathfollowerStatus.Errors[FIXEDWINGPATHFOLLOWERSTATUS_ERRORS_STALLSPEED] = 1;
result = 0;
}
if (indicatedAirspeedActual < 1e-6f) {
if (indicatedAirspeedState < 1e-6f) {
// prevent division by zero, abort without controlling anything. This guidance mode is not suited for takeoff or touchdown, or handling stationary planes
// also we cannot handle planes flying backwards, lets just wait until the nose drops
fixedwingpathfollowerStatus.Errors[FIXEDWINGPATHFOLLOWERSTATUS_ERRORS_LOWSPEED] = 1;
@ -515,7 +510,7 @@ static uint8_t updateFixedDesiredAttitude()
// Error condition: plane cannot hold altitude at current speed.
fixedwingpathfollowerStatus.Errors[FIXEDWINGPATHFOLLOWERSTATUS_ERRORS_LOWPOWER] = 0;
if (powerCommand >= fixedwingpathfollowerSettings.ThrottleLimit[FIXEDWINGPATHFOLLOWERSETTINGS_THROTTLELIMIT_MAX] && // throttle at maximum
velocityActual.Down > 0 && // we ARE going down
velocityState.Down > 0 && // we ARE going down
descentspeedDesired < 0 && // we WANT to go up
airspeedError > 0) { // we are too slow already
fixedwingpathfollowerStatus.Errors[FIXEDWINGPATHFOLLOWERSTATUS_ERRORS_LOWPOWER] = 1;
@ -524,7 +519,7 @@ static uint8_t updateFixedDesiredAttitude()
// Error condition: plane keeps climbing despite minimum throttle (opposite of above)
fixedwingpathfollowerStatus.Errors[FIXEDWINGPATHFOLLOWERSTATUS_ERRORS_HIGHPOWER] = 0;
if (powerCommand >= fixedwingpathfollowerSettings.ThrottleLimit[FIXEDWINGPATHFOLLOWERSETTINGS_THROTTLELIMIT_MIN] && // throttle at minimum
velocityActual.Down < 0 && // we ARE going up
velocityState.Down < 0 && // we ARE going up
descentspeedDesired > 0 && // we WANT to go down
airspeedError < 0) { // we are too fast already
fixedwingpathfollowerStatus.Errors[FIXEDWINGPATHFOLLOWERSTATUS_ERRORS_HIGHPOWER] = 1;
@ -565,7 +560,7 @@ static uint8_t updateFixedDesiredAttitude()
// Error condition: high speed dive
fixedwingpathfollowerStatus.Errors[FIXEDWINGPATHFOLLOWERSTATUS_ERRORS_PITCHCONTROL] = 0;
if (pitchCommand >= fixedwingpathfollowerSettings.PitchLimit[FIXEDWINGPATHFOLLOWERSETTINGS_PITCHLIMIT_MAX] && // pitch demand is full up
velocityActual.Down > 0 && // we ARE going down
velocityState.Down > 0 && // we ARE going down
descentspeedDesired < 0 && // we WANT to go up
airspeedError < 0) { // we are too fast already
fixedwingpathfollowerStatus.Errors[FIXEDWINGPATHFOLLOWERSTATUS_ERRORS_PITCHCONTROL] = 1;
@ -576,7 +571,7 @@ static uint8_t updateFixedDesiredAttitude()
* Compute desired roll command
*/
if (groundspeedDesired > 1e-6f) {
bearingError = RAD2DEG(atan2f(velocityDesired.East, velocityDesired.North) - atan2f(velocityActual.East, velocityActual.North));
bearingError = RAD2DEG(atan2f(velocityDesired.East, velocityDesired.North) - atan2f(velocityState.East, velocityState.North));
} else {
// if we are not supposed to move, run in a circle
bearingError = -90.0f;
@ -645,17 +640,17 @@ static void SettingsUpdatedCb(__attribute__((unused)) UAVObjEvent *ev)
PathDesiredGet(&pathDesired);
}
static void airspeedActualUpdatedCb(__attribute__((unused)) UAVObjEvent *ev)
static void airspeedStateUpdatedCb(__attribute__((unused)) UAVObjEvent *ev)
{
AirspeedActualData airspeedActual;
VelocityActualData velocityActual;
AirspeedStateData airspeedState;
VelocityStateData velocityState;
AirspeedActualGet(&airspeedActual);
VelocityActualGet(&velocityActual);
float groundspeed = sqrtf(velocityActual.East * velocityActual.East + velocityActual.North * velocityActual.North);
AirspeedStateGet(&airspeedState);
VelocityStateGet(&velocityState);
float groundspeed = sqrtf(velocityState.East * velocityState.East + velocityState.North * velocityState.North);
indicatedAirspeedActualBias = airspeedActual.CalibratedAirspeed - groundspeed;
indicatedAirspeedStateBias = airspeedState.CalibratedAirspeed - groundspeed;
// note - we do fly by Indicated Airspeed (== calibrated airspeed)
// however since airspeed is updated less often than groundspeed, we use sudden changes to groundspeed to offset the airspeed by the same measurement.
}

79
flight/modules/GPS/GPS.c
View File

@ -32,11 +32,12 @@
#include <openpilot.h>
#include "gpsposition.h"
#include "gpspositionsensor.h"
#include "homelocation.h"
#include "positionsensor.h"
#include "gpstime.h"
#include "gpssatellites.h"
#include "gpsvelocity.h"
#include "gpsvelocitysensor.h"
#include "systemsettings.h"
#include "taskinfo.h"
#include "hwsettings.h"
@ -56,9 +57,12 @@ static void gpsTask(void *parameters);
static void updateSettings();
#ifdef PIOS_GPS_SETS_HOMELOCATION
static void setHomeLocation(GPSPositionData *gpsData);
static void setHomeLocation(GPSPositionSensorData *gpsData);
static float GravityAccel(float latitude, float longitude, float altitude);
#endif
#ifdef PIOS_GPS_SETS_POSITIONSENSOR
static void setPositionSensor(GPSPositionSensorData *gpsData);
#endif
// ****************
// Private constants
@ -68,7 +72,7 @@ static float GravityAccel(float latitude, float longitude, float altitude);
#ifdef PIOS_GPS_SETS_HOMELOCATION
// Unfortunately need a good size stack for the WMM calculation
#define STACK_SIZE_BYTES 850
#define STACK_SIZE_BYTES 1024
#else
#if defined(PIOS_GPS_MINIMAL)
#define STACK_SIZE_BYTES 500
@ -145,23 +149,27 @@ int32_t GPSInitialize(void)
// These objects MUST be initialized for Revolution
// because the rest of the system expects to just
// attach to their queues
GPSPositionInitialize();
GPSVelocityInitialize();
GPSPositionSensorInitialize();
GPSVelocitySensorInitialize();
GPSTimeInitialize();
GPSSatellitesInitialize();
HomeLocationInitialize();
PositionSensorInitialize();
updateSettings();
#else
if (gpsPort && gpsEnabled) {
GPSPositionInitialize();
GPSVelocityInitialize();
GPSPositionSensorInitialize();
GPSVelocitySensorInitialize();
#if !defined(PIOS_GPS_MINIMAL)
GPSTimeInitialize();
GPSSatellitesInitialize();
#endif
#ifdef PIOS_GPS_SETS_HOMELOCATION
#if (defined(PIOS_GPS_SETS_HOMELOCATION) || defined(PIOS_GPS_SETS_POSITIONSENSOR))
HomeLocationInitialize();
#endif
#ifdef PIOS_GPS_SETS_POSITIONSENSOR
PositionSensorInitialize();
#endif
updateSettings();
}
@ -200,7 +208,7 @@ static void gpsTask(__attribute__((unused)) void *parameters)
portTickType xDelay = 100 / portTICK_RATE_MS;
uint32_t timeNowMs = xTaskGetTickCount() * portTICK_RATE_MS;
GPSPositionData gpsposition;
GPSPositionSensorData gpspositionsensor;
uint8_t gpsProtocol;
SystemSettingsGPSDataProtocolGet(&gpsProtocol);
@ -208,7 +216,7 @@ static void gpsTask(__attribute__((unused)) void *parameters)
timeOfLastUpdateMs = timeNowMs;
timeOfLastCommandMs = timeNowMs;
GPSPositionGet(&gpsposition);
GPSPositionSensorGet(&gpspositionsensor);
// Loop forever
while (1) {
uint8_t c;
@ -219,12 +227,12 @@ static void gpsTask(__attribute__((unused)) void *parameters)
switch (gpsProtocol) {
#if defined(PIOS_INCLUDE_GPS_NMEA_PARSER)
case SYSTEMSETTINGS_GPSDATAPROTOCOL_NMEA:
res = parse_nmea_stream(c, gps_rx_buffer, &gpsposition, &gpsRxStats);
res = parse_nmea_stream(c, gps_rx_buffer, &gpspositionsensor, &gpsRxStats);
break;
#endif
#if defined(PIOS_INCLUDE_GPS_UBX_PARSER)
case SYSTEMSETTINGS_GPSDATAPROTOCOL_UBX:
res = parse_ubx_stream(c, gps_rx_buffer, &gpsposition, &gpsRxStats);
res = parse_ubx_stream(c, gps_rx_buffer, &gpspositionsensor, &gpsRxStats);
break;
#endif
default:
@ -244,25 +252,30 @@ static void gpsTask(__attribute__((unused)) void *parameters)
if ((timeNowMs - timeOfLastUpdateMs) >= GPS_TIMEOUT_MS) {
// we have not received any valid GPS sentences for a while.
// either the GPS is not plugged in or a hardware problem or the GPS has locked up.
uint8_t status = GPSPOSITION_STATUS_NOGPS;
GPSPositionStatusSet(&status);
uint8_t status = GPSPOSITIONSENSOR_STATUS_NOGPS;
GPSPositionSensorStatusSet(&status);
AlarmsSet(SYSTEMALARMS_ALARM_GPS, SYSTEMALARMS_ALARM_ERROR);
} else {
// we appear to be receiving GPS sentences OK, we've had an update
// criteria for GPS-OK taken from this post...
// http://forums.openpilot.org/topic/1523-professors-insgps-in-svn/page__view__findpost__p__5220
if ((gpsposition.PDOP < 3.5f) && (gpsposition.Satellites >= 7) &&
(gpsposition.Status == GPSPOSITION_STATUS_FIX3D)) {
if ((gpspositionsensor.PDOP < 3.5f) && (gpspositionsensor.Satellites >= 7) &&
(gpspositionsensor.Status == GPSPOSITIONSENSOR_STATUS_FIX3D) &&
(gpspositionsensor.Latitude != 0 || gpspositionsensor.Longitude != 0)) {
AlarmsClear(SYSTEMALARMS_ALARM_GPS);
#ifdef PIOS_GPS_SETS_HOMELOCATION
HomeLocationData home;
HomeLocationGet(&home);
if (home.Set == HOMELOCATION_SET_FALSE) {
setHomeLocation(&gpsposition);
setHomeLocation(&gpspositionsensor);
}
#endif
} else if (gpsposition.Status == GPSPOSITION_STATUS_FIX3D) {
#ifdef PIOS_GPS_SETS_POSITIONSENSOR
setPositionSensor(&gpspositionsensor);
#endif
} else if ((gpspositionsensor.Status == GPSPOSITIONSENSOR_STATUS_FIX3D) &&
(gpspositionsensor.Latitude != 0 || gpspositionsensor.Longitude != 0)) {
AlarmsSet(SYSTEMALARMS_ALARM_GPS, SYSTEMALARMS_ALARM_WARNING);
} else {
AlarmsSet(SYSTEMALARMS_ALARM_GPS, SYSTEMALARMS_ALARM_CRITICAL);
@ -290,7 +303,7 @@ static float GravityAccel(float latitude, __attribute__((unused)) float longitud
// ****************
static void setHomeLocation(GPSPositionData *gpsData)
static void setHomeLocation(GPSPositionSensorData *gpsData)
{
HomeLocationData home;
@ -323,6 +336,32 @@ static void setHomeLocation(GPSPositionData *gpsData)
}
#endif /* ifdef PIOS_GPS_SETS_HOMELOCATION */
#ifdef PIOS_GPS_SETS_POSITIONSENSOR
static void setPositionSensor(GPSPositionSensorData *gpsData)
{
HomeLocationData home;
HomeLocationGet(&home);
PositionSensorData pos;
PositionSensorGet(&pos);
double ECEF[3];
double Rne[3][3];
{
float LLA[3] = { (home.Latitude) / 10e6f, (home.Longitude) / 10e6f, (home.Altitude) };
LLA2ECEF(LLA, ECEF);
RneFromLLA(LLA, Rne);
}
{ float LLA[3] = { (gpsData->Latitude) / 10e6d, (gpsData->Longitude) / 10e6d, gpsData->Altitude + gpsData->GeoidSeparation };
float NED[3];
LLA2Base(LLA, ECEF, Rne, NED);
pos.North = NED[0];
pos.East = NED[1];
pos.Down = NED[2]; }
PositionSensorSet(&pos);
}
#endif /* ifdef PIOS_GPS_SETS_POSITIONSENSOR */
/**
* Update the GPS settings, called on startup.
* FIXME: This should be in the GPSSettings object. But objects have

60
flight/modules/GPS/NMEA.c
View File

@ -33,7 +33,7 @@
#if defined(PIOS_INCLUDE_GPS_NMEA_PARSER)
#include "gpsposition.h"
#include "gpspositionsensor.h"
#include "NMEA.h"
#include "gpstime.h"
#include "gpssatellites.h"
@ -65,16 +65,16 @@
struct nmea_parser {
const char *prefix;
bool (*handler)(GPSPositionData *GpsData, bool *gpsDataUpdated, char *param[], uint8_t nbParam);
bool (*handler)(GPSPositionSensorData *GpsData, bool *gpsDataUpdated, char *param[], uint8_t nbParam);
};
static bool nmeaProcessGPGGA(GPSPositionData *GpsData, bool *gpsDataUpdated, char *param[], uint8_t nbParam);
static bool nmeaProcessGPRMC(GPSPositionData *GpsData, bool *gpsDataUpdated, char *param[], uint8_t nbParam);
static bool nmeaProcessGPVTG(GPSPositionData *GpsData, bool *gpsDataUpdated, char *param[], uint8_t nbParam);
static bool nmeaProcessGPGSA(GPSPositionData *GpsData, bool *gpsDataUpdated, char *param[], uint8_t nbParam);
static bool nmeaProcessGPGGA(GPSPositionSensorData *GpsData, bool *gpsDataUpdated, char *param[], uint8_t nbParam);
static bool nmeaProcessGPRMC(GPSPositionSensorData *GpsData, bool *gpsDataUpdated, char *param[], uint8_t nbParam);
static bool nmeaProcessGPVTG(GPSPositionSensorData *GpsData, bool *gpsDataUpdated, char *param[], uint8_t nbParam);
static bool nmeaProcessGPGSA(GPSPositionSensorData *GpsData, bool *gpsDataUpdated, char *param[], uint8_t nbParam);
#if !defined(PIOS_GPS_MINIMAL)
static bool nmeaProcessGPZDA(GPSPositionData *GpsData, bool *gpsDataUpdated, char *param[], uint8_t nbParam);
static bool nmeaProcessGPGSV(GPSPositionData *GpsData, bool *gpsDataUpdated, char *param[], uint8_t nbParam);
static bool nmeaProcessGPZDA(GPSPositionSensorData *GpsData, bool *gpsDataUpdated, char *param[], uint8_t nbParam);
static bool nmeaProcessGPGSV(GPSPositionSensorData *GpsData, bool *gpsDataUpdated, char *param[], uint8_t nbParam);
#endif // PIOS_GPS_MINIMAL
static const struct nmea_parser nmea_parsers[] = {
@ -106,7 +106,7 @@ static const struct nmea_parser nmea_parsers[] = {
#endif // PIOS_GPS_MINIMAL
};
int parse_nmea_stream(uint8_t c, char *gps_rx_buffer, GPSPositionData *GpsData, struct GPS_RX_STATS *gpsRxStats)
int parse_nmea_stream(uint8_t c, char *gps_rx_buffer, GPSPositionSensorData *GpsData, struct GPS_RX_STATS *gpsRxStats)
{
static uint8_t rx_count = 0;
static bool start_flag = false;
@ -351,12 +351,12 @@ static bool NMEA_latlon_to_fixed_point(int32_t *latlon, char *nmea_latlon, bool
/**
* Parses a complete NMEA sentence and updates the GPSPosition UAVObject
* Parses a complete NMEA sentence and updates the GPSPositionSensor UAVObject
* \param[in] An NMEA sentence with a valid checksum
* \return true if the sentence was successfully parsed
* \return false if any errors were encountered with the parsing
*/
bool NMEA_update_position(char *nmea_sentence, GPSPositionData *GpsData)
bool NMEA_update_position(char *nmea_sentence, GPSPositionSensorData *GpsData)
{
char *p = nmea_sentence;
char *params[MAX_NB_PARAMS];
@ -412,7 +412,7 @@ bool NMEA_update_position(char *nmea_sentence, GPSPositionData *GpsData)
#endif
// Send the message to the parser and get it update the GpsData
// Information from various different NMEA messages are temporarily
// cumulated in the GpsData structure. An actual GPSPosition update
// cumulated in the GpsData structure. An actual GPSPositionSensor update
// is triggered by GGA messages only. This message type sets the
// gpsDataUpdated flag to request this.
bool gpsDataUpdated = false;
@ -420,8 +420,8 @@ bool NMEA_update_position(char *nmea_sentence, GPSPositionData *GpsData)
if (!parser->handler(GpsData, &gpsDataUpdated, params, nbParams)) {
// Parse failed
DEBUG_MSG("PARSE FAILED (\"%s\")\n", params[0]);
if (gpsDataUpdated && (GpsData->Status == GPSPOSITION_STATUS_NOFIX)) {
GPSPositionSet(GpsData);
if (gpsDataUpdated && (GpsData->Status == GPSPOSITIONSENSOR_STATUS_NOFIX)) {
GPSPositionSensorSet(GpsData);
}
return false;
}
@ -432,7 +432,7 @@ bool NMEA_update_position(char *nmea_sentence, GPSPositionData *GpsData)
#ifdef DEBUG_MGSID_IN
DEBUG_MSG("U");
#endif
GPSPositionSet(GpsData);
GPSPositionSensorSet(GpsData);
}
#ifdef DEBUG_MGSID_IN
@ -444,10 +444,10 @@ bool NMEA_update_position(char *nmea_sentence, GPSPositionData *GpsData)
/**
* Parse an NMEA GPGGA sentence and update the given UAVObject
* \param[in] A pointer to a GPSPosition UAVObject to be updated.
* \param[in] A pointer to a GPSPositionSensor UAVObject to be updated.
* \param[in] An NMEA sentence with a valid checksum
*/
static bool nmeaProcessGPGGA(GPSPositionData *GpsData, bool *gpsDataUpdated, char *param[], uint8_t nbParam)
static bool nmeaProcessGPGGA(GPSPositionSensorData *GpsData, bool *gpsDataUpdated, char *param[], uint8_t nbParam)
{
if (nbParam != 15) {
return false;
@ -470,7 +470,7 @@ static bool nmeaProcessGPGGA(GPSPositionData *GpsData, bool *gpsDataUpdated, cha
// do this first to make sure we get this information, even if later checks exit
// this function early
if (param[6][0] == '0') {
GpsData->Status = GPSPOSITION_STATUS_NOFIX; // treat invalid fix as NOFIX
GpsData->Status = GPSPOSITIONSENSOR_STATUS_NOFIX; // treat invalid fix as NOFIX
}
// get latitude [DDMM.mmmmm] [N|S]
@ -497,10 +497,10 @@ static bool nmeaProcessGPGGA(GPSPositionData *GpsData, bool *gpsDataUpdated, cha
/**
* Parse an NMEA GPRMC sentence and update the given UAVObject
* \param[in] A pointer to a GPSPosition UAVObject to be updated.
* \param[in] A pointer to a GPSPositionSensor UAVObject to be updated.
* \param[in] An NMEA sentence with a valid checksum
*/
static bool nmeaProcessGPRMC(GPSPositionData *GpsData, bool *gpsDataUpdated, char *param[], uint8_t nbParam)
static bool nmeaProcessGPRMC(GPSPositionSensorData *GpsData, bool *gpsDataUpdated, char *param[], uint8_t nbParam)
{
if (nbParam != 13) {
return false;
@ -565,10 +565,10 @@ static bool nmeaProcessGPRMC(GPSPositionData *GpsData, bool *gpsDataUpdated, cha
/**
* Parse an NMEA GPVTG sentence and update the given UAVObject
* \param[in] A pointer to a GPSPosition UAVObject to be updated.
* \param[in] A pointer to a GPSPositionSensor UAVObject to be updated.
* \param[in] An NMEA sentence with a valid checksum
*/
static bool nmeaProcessGPVTG(GPSPositionData *GpsData, bool *gpsDataUpdated, char *param[], uint8_t nbParam)
static bool nmeaProcessGPVTG(GPSPositionSensorData *GpsData, bool *gpsDataUpdated, char *param[], uint8_t nbParam)
{
if (nbParam != 9 && nbParam != 10 /*GTOP GPS seems to gemnerate an extra parameter...*/) {
return false;
@ -590,10 +590,10 @@ static bool nmeaProcessGPVTG(GPSPositionData *GpsData, bool *gpsDataUpdated, cha
#if !defined(PIOS_GPS_MINIMAL)
/**
* Parse an NMEA GPZDA sentence and update the @ref GPSTime object
* \param[in] A pointer to a GPSPosition UAVObject to be updated (unused).
* \param[in] A pointer to a GPSPositionSensor UAVObject to be updated (unused).
* \param[in] An NMEA sentence with a valid checksum
*/
static bool nmeaProcessGPZDA(__attribute__((unused)) GPSPositionData *GpsData, bool *gpsDataUpdated, char *param[], uint8_t nbParam)
static bool nmeaProcessGPZDA(__attribute__((unused)) GPSPositionSensorData *GpsData, bool *gpsDataUpdated, char *param[], uint8_t nbParam)
{
if (nbParam != 7) {
return false;
@ -633,7 +633,7 @@ static uint8_t gsv_processed_mask;
static uint16_t gsv_incomplete_error;
static uint16_t gsv_duplicate_error;
static bool nmeaProcessGPGSV(__attribute__((unused)) GPSPositionData *GpsData, bool *gpsDataUpdated, char *param[], uint8_t nbParam)
static bool nmeaProcessGPGSV(__attribute__((unused)) GPSPositionSensorData *GpsData, bool *gpsDataUpdated, char *param[], uint8_t nbParam)
{
if (nbParam < 4) {
return false;
@ -717,10 +717,10 @@ static bool nmeaProcessGPGSV(__attribute__((unused)) GPSPositionData *GpsData, b
/**
* Parse an NMEA GPGSA sentence and update the given UAVObject
* \param[in] A pointer to a GPSPosition UAVObject to be updated.
* \param[in] A pointer to a GPSPositionSensor UAVObject to be updated.
* \param[in] An NMEA sentence with a valid checksum
*/
static bool nmeaProcessGPGSA(GPSPositionData *GpsData, bool *gpsDataUpdated, char *param[], uint8_t nbParam)
static bool nmeaProcessGPGSA(GPSPositionSensorData *GpsData, bool *gpsDataUpdated, char *param[], uint8_t nbParam)
{
if (nbParam != 18) {
return false;
@ -737,13 +737,13 @@ static bool nmeaProcessGPGSA(GPSPositionData *GpsData, bool *gpsDataUpdated, cha
switch (atoi(param[2])) {
case 1:
GpsData->Status = GPSPOSITION_STATUS_NOFIX;
GpsData->Status = GPSPOSITIONSENSOR_STATUS_NOFIX;
break;
case 2:
GpsData->Status = GPSPOSITION_STATUS_FIX2D;
GpsData->Status = GPSPOSITIONSENSOR_STATUS_FIX2D;
break;
case 3:
GpsData->Status = GPSPOSITION_STATUS_FIX3D;
GpsData->Status = GPSPOSITIONSENSOR_STATUS_FIX3D;
break;
default:
/* Unhandled */

32
flight/modules/GPS/UBX.c
View File

@ -38,7 +38,7 @@
// parse incoming character stream for messages in UBX binary format
int parse_ubx_stream(uint8_t c, char *gps_rx_buffer, GPSPositionData *GpsData, struct GPS_RX_STATS *gpsRxStats)
int parse_ubx_stream(uint8_t c, char *gps_rx_buffer, GPSPositionSensorData *GpsData, struct GPS_RX_STATS *gpsRxStats)
{
enum proto_states {
START,
@ -193,10 +193,10 @@ bool checksum_ubx_message(struct UBXPacket *ubx)
}
}
void parse_ubx_nav_posllh(struct UBX_NAV_POSLLH *posllh, GPSPositionData *GpsPosition)
void parse_ubx_nav_posllh(struct UBX_NAV_POSLLH *posllh, GPSPositionSensorData *GpsPosition)
{
if (check_msgtracker(posllh->iTOW, POSLLH_RECEIVED)) {
if (GpsPosition->Status != GPSPOSITION_STATUS_NOFIX) {
if (GpsPosition->Status != GPSPOSITIONSENSOR_STATUS_NOFIX) {
GpsPosition->Altitude = (float)posllh->hMSL * 0.001f;
GpsPosition->GeoidSeparation = (float)(posllh->height - posllh->hMSL) * 0.001f;
GpsPosition->Latitude = posllh->lat;
@ -205,7 +205,7 @@ void parse_ubx_nav_posllh(struct UBX_NAV_POSLLH *posllh, GPSPositionData *GpsPos
}
}
void parse_ubx_nav_sol(struct UBX_NAV_SOL *sol, GPSPositionData *GpsPosition)
void parse_ubx_nav_sol(struct UBX_NAV_SOL *sol, GPSPositionSensorData *GpsPosition)
{
if (check_msgtracker(sol->iTOW, SOL_RECEIVED)) {
GpsPosition->Satellites = sol->numSV;
@ -213,20 +213,20 @@ void parse_ubx_nav_sol(struct UBX_NAV_SOL *sol, GPSPositionData *GpsPosition)
if (sol->flags & STATUS_FLAGS_GPSFIX_OK) {
switch (sol->gpsFix) {
case STATUS_GPSFIX_2DFIX:
GpsPosition->Status = GPSPOSITION_STATUS_FIX2D;
GpsPosition->Status = GPSPOSITIONSENSOR_STATUS_FIX2D;
break;
case STATUS_GPSFIX_3DFIX:
GpsPosition->Status = GPSPOSITION_STATUS_FIX3D;
GpsPosition->Status = GPSPOSITIONSENSOR_STATUS_FIX3D;
break;
default: GpsPosition->Status = GPSPOSITION_STATUS_NOFIX;
default: GpsPosition->Status = GPSPOSITIONSENSOR_STATUS_NOFIX;
}
} else { // fix is not valid so we make sure to treat is as NOFIX
GpsPosition->Status = GPSPOSITION_STATUS_NOFIX;
GpsPosition->Status = GPSPOSITIONSENSOR_STATUS_NOFIX;
}
}
}
void parse_ubx_nav_dop(struct UBX_NAV_DOP *dop, GPSPositionData *GpsPosition)
void parse_ubx_nav_dop(struct UBX_NAV_DOP *dop, GPSPositionSensorData *GpsPosition)
{
if (check_msgtracker(dop->iTOW, DOP_RECEIVED)) {
GpsPosition->HDOP = (float)dop->hDOP * 0.01f;
@ -235,16 +235,16 @@ void parse_ubx_nav_dop(struct UBX_NAV_DOP *dop, GPSPositionData *GpsPosition)
}
}
void parse_ubx_nav_velned(struct UBX_NAV_VELNED *velned, GPSPositionData *GpsPosition)
void parse_ubx_nav_velned(struct UBX_NAV_VELNED *velned, GPSPositionSensorData *GpsPosition)
{
GPSVelocityData GpsVelocity;
GPSVelocitySensorData GpsVelocity;
if (check_msgtracker(velned->iTOW, VELNED_RECEIVED)) {
if (GpsPosition->Status != GPSPOSITION_STATUS_NOFIX) {
if (GpsPosition->Status != GPSPOSITIONSENSOR_STATUS_NOFIX) {
GpsVelocity.North = (float)velned->velN / 100.0f;
GpsVelocity.East = (float)velned->velE / 100.0f;
GpsVelocity.Down = (float)velned->velD / 100.0f;
GPSVelocitySet(&GpsVelocity);
GPSVelocitySensorSet(&GpsVelocity);
GpsPosition->Groundspeed = (float)velned->gSpeed * 0.01f;
GpsPosition->Heading = (float)velned->heading * 1.0e-5f;
}
@ -302,7 +302,7 @@ void parse_ubx_nav_svinfo(struct UBX_NAV_SVINFO *svinfo)
// UBX message parser
// returns UAVObjectID if a UAVObject structure is ready for further processing
uint32_t parse_ubx_message(struct UBXPacket *ubx, GPSPositionData *GpsPosition)
uint32_t parse_ubx_message(struct UBXPacket *ubx, GPSPositionSensorData *GpsPosition)
{
uint32_t id = 0;
@ -333,9 +333,9 @@ uint32_t parse_ubx_message(struct UBXPacket *ubx, GPSPositionData *GpsPosition)
break;
}
if (msgtracker.msg_received == ALL_RECEIVED) {
GPSPositionSet(GpsPosition);
GPSPositionSensorSet(GpsPosition);
msgtracker.msg_received = NONE_RECEIVED;
id = GPSPOSITION_OBJID;
id = GPSPOSITIONSENSOR_OBJID;
}
return id;
}

4
flight/modules/GPS/inc/GPS.h
View File

@ -34,9 +34,9 @@
#ifndef GPS_H
#define GPS_H
#include "gpsvelocity.h"
#include "gpsvelocitysensor.h"
#include "gpssatellites.h"
#include "gpsposition.h"
#include "gpspositionsensor.h"
#include "gpstime.h"
#define NO_PARSER -3 // no parser available

4
flight/modules/GPS/inc/NMEA.h
View File

@ -37,8 +37,8 @@
#define NMEA_MAX_PACKET_LENGTH 96 // 82 max NMEA msg size plus 12 margin (because some vendors add custom crap) plus CR plus Linefeed
extern bool NMEA_update_position(char *nmea_sentence, GPSPositionData *GpsData);
extern bool NMEA_update_position(char *nmea_sentence, GPSPositionSensorData *GpsData);
extern bool NMEA_checksum(char *nmea_sentence);
extern int parse_nmea_stream(uint8_t, char *, GPSPositionData *, struct GPS_RX_STATS *);
extern int parse_nmea_stream(uint8_t, char *, GPSPositionSensorData *, struct GPS_RX_STATS *);
#endif /* NMEA_H */

6
flight/modules/GPS/inc/UBX.h
View File

@ -31,7 +31,7 @@
#ifndef UBX_H
#define UBX_H
#include "openpilot.h"
#include "gpsposition.h"
#include "gpspositionsensor.h"
#include "GPS.h"
@ -218,7 +218,7 @@ struct UBXPacket {
};
bool checksum_ubx_message(struct UBXPacket *);
uint32_t parse_ubx_message(struct UBXPacket *, GPSPositionData *);
int parse_ubx_stream(uint8_t, char *, GPSPositionData *, struct GPS_RX_STATS *);
uint32_t parse_ubx_message(struct UBXPacket *, GPSPositionSensorData *);
int parse_ubx_stream(uint8_t, char *, GPSPositionSensorData *, struct GPS_RX_STATS *);
#endif /* UBX_H */

16
flight/modules/MK/MKSerial/MKSerial.c
View File

@ -31,8 +31,8 @@
#include "openpilot.h"
#include "MkSerial.h"
#include "attitudeactual.h" // object that will be updated by the module
#include "positionactual.h"
#include "attitudestate.h" // object that will be updated by the module
#include "positionstate.h"
#include "flightbatterystate.h"
//
@ -411,7 +411,7 @@ static uint16_t VersionMsg_GetVersion(const MkMsg_t *msg)
static void DoConnectedToFC(void)
{
AttitudeActualData attitudeData;
AttitudeStateData attitudeData;
MkMsg_t msg;
DEBUG_MSG("FC\n\r");
@ -434,7 +434,7 @@ static void DoConnectedToFC(void)
attitudeData.Pitch = -(float)nick / 10;
attitudeData.Roll = -(float)roll / 10;
AttitudeActualSet(&attitudeData);
AttitudeStateSet(&attitudeData);
} else {
DEBUG_MSG("TO\n\r");
break;
@ -446,8 +446,8 @@ static void DoConnectedToNC(void)
{
MkMsg_t msg;
GpsPosition_t pos;
AttitudeActualData attitudeData;
PositionActualData positionData;
AttitudeStateData attitudeData;
PositionStateData positionData;
FlightBatteryStateData flightBatteryData;
#ifdef GENERATE_BATTERY_INFO
@ -478,7 +478,7 @@ static void DoConnectedToNC(void)
#endif
attitudeData.Pitch = -Par2Int8(&msg, OSD_MSG_NICK_IDX);
attitudeData.Roll = -Par2Int8(&msg, OSD_MSG_ROLL_IDX);
AttitudeActualSet(&attitudeData);
AttitudeStateSet(&attitudeData);
positionData.Longitude = pos.longitute;
positionData.Latitude = pos.latitude;
@ -491,7 +491,7 @@ static void DoConnectedToNC(void)
} else {
positionData.Status = POSITIONACTUAL_STATUS_FIX3D;
}
PositionActualSet(&positionData);
PositionStateSet(&positionData);
#if GENERATE_BATTERY_INFO
if (++battStateCnt > 2) {

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

@ -38,14 +38,13 @@
#include "accessorydesired.h"
#include "actuatordesired.h"
#include "altitudeholddesired.h"
#include "baroaltitude.h"
#include "flighttelemetrystats.h"
#include "flightstatus.h"
#include "sanitycheck.h"
#include "manualcontrol.h"
#include "manualcontrolsettings.h"
#include "manualcontrolcommand.h"
#include "positionactual.h"
#include "positionstate.h"
#include "pathdesired.h"
#include "stabilizationsettings.h"
#include "stabilizationdesired.h"
@ -723,34 +722,34 @@ static void updatePathDesired(__attribute__((unused)) ManualControlCommandData *
if (home && changed) {
// Simple Return To Base mode - keep altitude the same, fly to home position
PositionActualData positionActual;
PositionActualGet(&positionActual);
PositionStateData positionState;
PositionStateGet(&positionState);
PathDesiredData pathDesired;
PathDesiredGet(&pathDesired);
pathDesired.Start[PATHDESIRED_START_NORTH] = 0;
pathDesired.Start[PATHDESIRED_START_EAST] = 0;
pathDesired.Start[PATHDESIRED_START_DOWN] = positionActual.Down - 10;
pathDesired.Start[PATHDESIRED_START_DOWN] = positionState.Down - 10;
pathDesired.End[PATHDESIRED_END_NORTH] = 0;
pathDesired.End[PATHDESIRED_END_EAST] = 0;
pathDesired.End[PATHDESIRED_END_DOWN] = positionActual.Down - 10;
pathDesired.End[PATHDESIRED_END_DOWN] = positionState.Down - 10;
pathDesired.StartingVelocity = 1;
pathDesired.EndingVelocity = 0;
pathDesired.Mode = PATHDESIRED_MODE_FLYENDPOINT;
PathDesiredSet(&pathDesired);
} else if (changed) {
// After not being in this mode for a while init at current height
PositionActualData positionActual;
PositionActualGet(&positionActual);
PositionStateData positionState;
PositionStateGet(&positionState);
PathDesiredData pathDesired;
PathDesiredGet(&pathDesired);
pathDesired.Start[PATHDESIRED_START_NORTH] = positionActual.North;
pathDesired.Start[PATHDESIRED_START_EAST] = positionActual.East;
pathDesired.Start[PATHDESIRED_START_DOWN] = positionActual.Down;
pathDesired.End[PATHDESIRED_END_NORTH] = positionActual.North;
pathDesired.End[PATHDESIRED_END_EAST] = positionActual.East;
pathDesired.End[PATHDESIRED_END_DOWN] = positionActual.Down;
pathDesired.Start[PATHDESIRED_START_NORTH] = positionState.North;
pathDesired.Start[PATHDESIRED_START_EAST] = positionState.East;
pathDesired.Start[PATHDESIRED_START_DOWN] = positionState.Down;
pathDesired.End[PATHDESIRED_END_NORTH] = positionState.North;
pathDesired.End[PATHDESIRED_END_EAST] = positionState.East;
pathDesired.End[PATHDESIRED_END_DOWN] = positionState.Down;
pathDesired.StartingVelocity = 1;
pathDesired.EndingVelocity = 0;
pathDesired.Mode = PATHDESIRED_MODE_FLYENDPOINT;
@ -779,25 +778,25 @@ static void updateLandDesired(__attribute__((unused)) ManualControlCommandData *
lastSysTime = thisSysTime;
*/
PositionActualData positionActual;
PositionStateData positionState;
PositionActualGet(&positionActual);
PositionStateGet(&positionState);
PathDesiredData pathDesired;
PathDesiredGet(&pathDesired);
if (changed) {
// After not being in this mode for a while init at current height
pathDesired.Start[PATHDESIRED_START_NORTH] = positionActual.North;
pathDesired.Start[PATHDESIRED_START_EAST] = positionActual.East;
pathDesired.Start[PATHDESIRED_START_DOWN] = positionActual.Down;
pathDesired.End[PATHDESIRED_END_NORTH] = positionActual.North;
pathDesired.End[PATHDESIRED_END_EAST] = positionActual.East;
pathDesired.End[PATHDESIRED_END_DOWN] = positionActual.Down;
pathDesired.Start[PATHDESIRED_START_NORTH] = positionState.North;
pathDesired.Start[PATHDESIRED_START_EAST] = positionState.East;
pathDesired.Start[PATHDESIRED_START_DOWN] = positionState.Down;
pathDesired.End[PATHDESIRED_END_NORTH] = positionState.North;
pathDesired.End[PATHDESIRED_END_EAST] = positionState.East;
pathDesired.End[PATHDESIRED_END_DOWN] = positionState.Down;
pathDesired.StartingVelocity = 1;
pathDesired.EndingVelocity = 0;
pathDesired.Mode = PATHDESIRED_MODE_FLYENDPOINT;
}
pathDesired.End[PATHDESIRED_END_DOWN] = positionActual.Down + 5;
pathDesired.End[PATHDESIRED_END_DOWN] = positionState.Down + 5;
PathDesiredSet(&pathDesired);
}
@ -852,7 +851,7 @@ static void altitudeHoldDesired(ManualControlCommandData *cmd, bool changed)
altitudeHoldDesiredData.Yaw = cmd->Yaw * stabSettings.ManualRate[STABILIZATIONSETTINGS_MANUALRATE_YAW];
float currentDown;
PositionActualDownGet(&currentDown);
PositionStateDownGet(&currentDown);
if (changed) {
// After not being in this mode for a while init at current height
altitudeHoldDesiredData.Altitude = 0;

34
flight/modules/Osd/OsdEtStd/OsdEtStd.c
View File

@ -32,9 +32,9 @@
#include "openpilot.h"
#include "flightbatterystate.h"
#include "gpsposition.h"
#include "attitudeactual.h"
#include "baroaltitude.h"
#include "gpspositionsensor.h"
#include "attitudestate.h"
#include "barosensor.h"
//
// Configuration
@ -168,7 +168,7 @@ static void SetCourse(uint16_t dir)
WriteToMsg16(OSDMSG_HOME_IDX, dir);
}
static void SetBaroAltitude(int16_t altitudeMeter)
static void SetBaroSensor(int16_t altitudeMeter)
{
// calculated formula
// ET OSD uses first update as zeropoint and then +- from that
@ -209,12 +209,12 @@ static void FlightBatteryStateUpdatedCb(__attribute__((unused)) UAVObjEvent *ev)
newBattData = TRUE;
}
static void GPSPositionUpdatedCb(__attribute__((unused)) UAVObjEvent *ev)
static void GPSPositionSensorUpdatedCb(__attribute__((unused)) UAVObjEvent *ev)
{
newPosData = TRUE;
}
static void BaroAltitudeUpdatedCb(__attribute__((unused)) UAVObjEvent *ev)
static void BaroSensorUpdatedCb(__attribute__((unused)) UAVObjEvent *ev)
{
newBaroData = TRUE;
}
@ -440,17 +440,17 @@ static void Run(void)
}
if (newPosData) {
GPSPositionData positionData;
AttitudeActualData attitudeActualData;
GPSPositionSensorData positionData;
AttitudeStateData attitudeStateData;
GPSPositionGet(&positionData);
AttitudeActualGet(&attitudeActualData);
GPSPositionSensorGet(&positionData);
AttitudeStateGet(&attitudeStateData);
// DEBUG_MSG("%5d Pos: #stat=%d #sats=%d alt=%d\n\r", cnt,
// positionData.Status, positionData.Satellites, (uint32_t)positionData.Altitude);
// GPS Status
if (positionData.Status == GPSPOSITION_STATUS_FIX3D) {
if (positionData.Status == GPSPOSITIONSENSOR_STATUS_FIX3D) {
msg[OSDMSG_GPS_STAT] = OSDMSG_GPS_STAT_FIX;
} else {
msg[OSDMSG_GPS_STAT] = OSDMSG_GPS_STAT_NOFIX;
@ -462,17 +462,17 @@ static void Run(void)
SetCoord(OSDMSG_LON_IDX, positionData.Longitude);
SetAltitude(positionData.Altitude);
SetNbSats(positionData.Satellites);
SetCourse(attitudeActualData.Yaw);
SetCourse(attitudeStateData.Yaw);
newPosData = FALSE;
} else {
msg[OSDMSG_GPS_STAT] &= ~OSDMSG_GPS_STAT_HB_FLAG;
}
if (newBaroData) {
BaroAltitudeData baroData;
BaroSensorData baroData;
BaroAltitudeGet(&baroData);
SetBaroAltitude(baroData.Altitude);
BaroSensorGet(&baroData);
SetBaroSensor(baroData.Altitude);
newBaroData = FALSE;
}
@ -543,9 +543,9 @@ static void onTimer(UAVObjEvent *ev)
*/
int32_t OsdEtStdInitialize(void)
{
GPSPositionConnectCallback(GPSPositionUpdatedCb);
GPSPositionSensorConnectCallback(GPSPositionSensorUpdatedCb);
FlightBatteryStateConnectCallback(FlightBatteryStateUpdatedCb);
BaroAltitudeConnectCallback(BaroAltitudeUpdatedCb);
BaroSensorConnectCallback(BaroSensorUpdatedCb);
memset(&ev, 0, sizeof(UAVObjEvent));
EventPeriodicCallbackCreate(&ev, onTimer, 100 / portTICK_RATE_MS);

28
flight/modules/Osd/osdgen/osdgen.c
View File

@ -34,13 +34,13 @@
#include "osdgen.h"
#include "attitudeactual.h"
#include "gpsposition.h"
#include "attitudestate.h"
#include "gpspositionsensor.h"
#include "homelocation.h"
#include "gpstime.h"
#include "gpssatellites.h"
#include "osdsettings.h"
#include "baroaltitude.h"
#include "barosensor.h"
#include "taskinfo.h"
#include "flightstatus.h"
@ -2036,8 +2036,8 @@ void calcHomeArrow(int16_t m_yaw)
HomeLocationData home;
HomeLocationGet(&home);
GPSPositionData gpsData;
GPSPositionGet(&gpsData);
GPSPositionSensorData gpsData;
GPSPositionSensorGet(&gpsData);
/** http://www.movable-type.co.uk/scripts/latlong.html **/
float lat1, lat2, lon1, lon2, a, c, d, x, y, brng, u2g;
@ -2137,14 +2137,14 @@ void updateGraphics()
OsdSettingsData OsdSettings;
OsdSettingsGet(&OsdSettings);
AttitudeActualData attitude;
AttitudeActualGet(&attitude);
GPSPositionData gpsData;
GPSPositionGet(&gpsData);
AttitudeStateData attitude;
AttitudeStateGet(&attitude);
GPSPositionSensorData gpsData;
GPSPositionSensorGet(&gpsData);
HomeLocationData home;
HomeLocationGet(&home);
BaroAltitudeData baro;
BaroAltitudeGet(&baro);
BaroSensorData baro;
BaroSensorGet(&baro);
FlightStatusData status;
FlightStatusGet(&status);
@ -2419,9 +2419,9 @@ int32_t osdgenStart(void)
*/
int32_t osdgenInitialize(void)
{
AttitudeActualInitialize();
AttitudeStateInitialize();
#ifdef PIOS_INCLUDE_GPS
GPSPositionInitialize();
GPSPositionSensorInitialize();
#if !defined(PIOS_GPS_MINIMAL)
GPSTimeInitialize();
GPSSatellitesInitialize();
@ -2431,7 +2431,7 @@ int32_t osdgenInitialize(void)
#endif
#endif
OsdSettingsInitialize();
BaroAltitudeInitialize();
BaroSensorInitialize();
FlightStatusInitialize();
return 0;

16
flight/modules/Osd/osdinput/osdinput.c
View File

@ -34,7 +34,7 @@
#include "osdinput.h"
#include "attitudeactual.h"
#include "attitudestate.h"
#include "taskinfo.h"
#include "flightstatus.h"
@ -86,11 +86,11 @@ int32_t osdinputStart(void)
*/
int32_t osdinputInitialize(void)
{
AttitudeActualInitialize();
AttitudeStateInitialize();
FlightStatusInitialize();
// Initialize quaternion
AttitudeActualData attitude;
AttitudeActualGet(&attitude);
AttitudeStateData attitude;
AttitudeStateGet(&attitude);
attitude.q1 = 1;
attitude.q2 = 0;
attitude.q3 = 0;
@ -98,7 +98,7 @@ int32_t osdinputInitialize(void)
attitude.Roll = 0;
attitude.Pitch = 0;
attitude.Yaw = 0;
AttitudeActualSet(&attitude);
AttitudeStateSet(&attitude);
oposdPort = PIOS_COM_OSD;
@ -148,8 +148,8 @@ static void osdinputTask(__attribute__((unused)) void *parameters)
}
if (rx_count == 11) {
if (oposd_rx_buffer[1] == OSD_PKT_TYPE_ATT) {
AttitudeActualData attitude;
AttitudeActualGet(&attitude);
AttitudeStateData attitude;
AttitudeStateGet(&attitude);
attitude.q1 = 1;
attitude.q2 = 0;
attitude.q3 = 0;
@ -157,7 +157,7 @@ static void osdinputTask(__attribute__((unused)) void *parameters)
attitude.Roll = (float)((int16_t)(oposd_rx_buffer[3] | oposd_rx_buffer[4] << 8)) / 10.0f;
attitude.Pitch = (float)((int16_t)(oposd_rx_buffer[5] | oposd_rx_buffer[6] << 8)) / 10.0f;
attitude.Yaw = (float)((int16_t)(oposd_rx_buffer[7] | oposd_rx_buffer[8] << 8)) / 10.0f;
AttitudeActualSet(&attitude);
AttitudeStateSet(&attitude);
} else if (oposd_rx_buffer[1] == OSD_PKT_TYPE_MODE) {
FlightStatusData status;
FlightStatusGet(&status);

20
flight/modules/Osd/osdoutput/osdoutput.c
View File

@ -35,11 +35,11 @@
#endif
#if POSITIONACTUAL_SUPPORTED
#include "positionactual.h"
#include "positionstate.h"
#endif
#include "systemalarms.h"
#include "attitudeactual.h"
#include "attitudestate.h"
#include "hwsettings.h"
#include "flightstatus.h"
@ -117,7 +117,7 @@ struct osd_hk_msg {
static struct osd_hk_msg osd_hk_msg_buf;
static volatile bool newPositionActualData = false;
static volatile bool newPositionStateData = false;
static volatile bool newBattData = false;
static volatile bool newAttitudeData = false;
static volatile bool newAlarmData = false;
@ -155,15 +155,15 @@ static void send_update(__attribute__((unused)) UAVObjEvent *ev)
case OSD_HK_PKT_TYPE_ATT:
msg->t = OSD_HK_PKT_TYPE_ATT;
float roll;
AttitudeActualRollGet(&roll);
AttitudeStateRollGet(&roll);
blob->att.roll = (int16_t)(roll * 10);
float pitch;
AttitudeActualPitchGet(&pitch);
AttitudeStatePitchGet(&pitch);
blob->att.pitch = (int16_t)(pitch * 10);
float yaw;
AttitudeActualYawGet(&yaw);
AttitudeStateYawGet(&yaw);
blob->att.yaw = (int16_t)(yaw * 10);
break;
case OSD_HK_PKT_TYPE_MODE:
@ -217,9 +217,9 @@ static void send_update(__attribute__((unused)) UAVObjEvent *ev)
#endif
#if POSITIONACTUAL_SUPPORTED
if (newPositionActualData) {
PositionActualData position;
PositionActualGet(&position);
if (newPositionStateData) {
PositionStateData position;
PositionStateGet(&position);
/* compute 3D distance */
float d = sqrt(
@ -234,7 +234,7 @@ static void send_update(__attribute__((unused)) UAVObjEvent *ev)
(home & 0xFF00 >> 8) |
(home & 0x00FF << 8));
newPositionActualData = false;
newPositionStateData = false;
}
#else
// blob->misc.home = 0;

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

@ -32,12 +32,12 @@
#include "openpilot.h"
#include "flightstatus.h"
#include "airspeedactual.h"
#include "airspeedstate.h"
#include "pathaction.h"
#include "pathdesired.h"
#include "pathstatus.h"
#include "positionactual.h"
#include "velocityactual.h"
#include "positionstate.h"
#include "velocitystate.h"
#include "waypoint.h"
#include "waypointactive.h"
#include "taskinfo.h"
@ -102,9 +102,9 @@ int32_t PathPlannerInitialize()
PathActionInitialize();
PathStatusInitialize();
PathDesiredInitialize();
PositionActualInitialize();
AirspeedActualInitialize();
VelocityActualInitialize();
PositionStateInitialize();
AirspeedStateInitialize();
VelocityStateInitialize();
WaypointInitialize();
WaypointActiveInitialize();
@ -239,16 +239,16 @@ void updatePathDesired(__attribute__((unused)) UAVObjEvent *ev)
pathDesired.UID = waypointActiveData.Index;
if (waypointActiveData.Index == 0) {
PositionActualData positionActual;
PositionActualGet(&positionActual);
PositionStateData positionState;
PositionStateGet(&positionState);
// First waypoint has itself as start point (used to be home position but that proved dangerous when looping)
/*pathDesired.Start[PATHDESIRED_START_NORTH] = waypoint.Position[WAYPOINT_POSITION_NORTH];
pathDesired.Start[PATHDESIRED_START_EAST] = waypoint.Position[WAYPOINT_POSITION_EAST];
pathDesired.Start[PATHDESIRED_START_DOWN] = waypoint.Position[WAYPOINT_POSITION_DOWN];*/
pathDesired.Start[PATHDESIRED_START_NORTH] = positionActual.North;
pathDesired.Start[PATHDESIRED_START_EAST] = positionActual.East;
pathDesired.Start[PATHDESIRED_START_DOWN] = positionActual.Down;
pathDesired.Start[PATHDESIRED_START_NORTH] = positionState.North;
pathDesired.Start[PATHDESIRED_START_EAST] = positionState.East;
pathDesired.Start[PATHDESIRED_START_DOWN] = positionState.Down;
pathDesired.StartingVelocity = pathDesired.EndingVelocity;
} else {
// Get previous waypoint as start point
@ -365,16 +365,16 @@ static uint8_t conditionTimeOut()
static uint8_t conditionDistanceToTarget()
{
float distance;
PositionActualData positionActual;
PositionStateData positionState;
PositionActualGet(&positionActual);
PositionStateGet(&positionState);
if (pathAction.ConditionParameters[1] > 0.5f) {
distance = sqrtf(powf(waypoint.Position[0] - positionActual.North, 2)
+ powf(waypoint.Position[1] - positionActual.East, 2)
+ powf(waypoint.Position[1] - positionActual.Down, 2));
distance = sqrtf(powf(waypoint.Position[0] - positionState.North, 2)
+ powf(waypoint.Position[1] - positionState.East, 2)
+ powf(waypoint.Position[1] - positionState.Down, 2));
} else {
distance = sqrtf(powf(waypoint.Position[0] - positionActual.North, 2)
+ powf(waypoint.Position[1] - positionActual.East, 2));
distance = sqrtf(powf(waypoint.Position[0] - positionState.North, 2)
+ powf(waypoint.Position[1] - positionState.East, 2));
}
if (distance <= pathAction.ConditionParameters[0]) {
@ -392,12 +392,12 @@ static uint8_t conditionDistanceToTarget()
static uint8_t conditionLegRemaining()
{
PathDesiredData pathDesired;
PositionActualData positionActual;
PositionStateData positionState;
PathDesiredGet(&pathDesired);
PositionActualGet(&positionActual);
PositionStateGet(&positionState);
float cur[3] = { positionActual.North, positionActual.East, positionActual.Down };
float cur[3] = { positionState.North, positionState.East, positionState.Down };
struct path_status progress;
path_progress(pathDesired.Start, pathDesired.End, cur, &progress, pathDesired.Mode);
@ -415,12 +415,12 @@ static uint8_t conditionLegRemaining()
static uint8_t conditionBelowError()
{
PathDesiredData pathDesired;
PositionActualData positionActual;
PositionStateData positionState;
PathDesiredGet(&pathDesired);
PositionActualGet(&positionActual);
PositionStateGet(&positionState);
float cur[3] = { positionActual.North, positionActual.East, positionActual.Down };
float cur[3] = { positionState.North, positionState.East, positionState.Down };
struct path_status progress;
path_progress(pathDesired.Start, pathDesired.End, cur, &progress, pathDesired.Mode);
@ -438,11 +438,11 @@ static uint8_t conditionBelowError()
*/
static uint8_t conditionAboveAltitude()
{
PositionActualData positionActual;
PositionStateData positionState;
PositionActualGet(&positionActual);
PositionStateGet(&positionState);
if (positionActual.Down <= pathAction.ConditionParameters[0]) {
if (positionState.Down <= pathAction.ConditionParameters[0]) {
return true;
}
return false;
@ -456,15 +456,15 @@ static uint8_t conditionAboveAltitude()
*/
static uint8_t conditionAboveSpeed()
{
VelocityActualData velocityActual;
VelocityStateData velocityState;
VelocityActualGet(&velocityActual);
float velocity = sqrtf(velocityActual.North * velocityActual.North + velocityActual.East * velocityActual.East + velocityActual.Down * velocityActual.Down);
VelocityStateGet(&velocityState);
float velocity = sqrtf(velocityState.North * velocityState.North + velocityState.East * velocityState.East + velocityState.Down * velocityState.Down);
// use airspeed if requested and available
if (pathAction.ConditionParameters[1] > 0.5f) {
AirspeedActualData airspeed;
AirspeedActualGet(&airspeed);
AirspeedStateData airspeed;
AirspeedStateGet(&airspeed);
velocity = airspeed.CalibratedAirspeed;
}
@ -494,8 +494,8 @@ static uint8_t conditionPointingTowardsNext()
float angle1 = atan2f((nextWaypoint.Position[0] - waypoint.Position[0]), (nextWaypoint.Position[1] - waypoint.Position[1]));
VelocityActualData velocity;
VelocityActualGet(&velocity);
VelocityStateData velocity;
VelocityStateGet(&velocity);
float angle2 = atan2f(velocity.North, velocity.East);
// calculate the absolute angular difference

192
flight/modules/Sensors/sensors.c
View File

@ -4,7 +4,7 @@
* @{
* @addtogroup Sensors
* @brief Acquires sensor data
* Specifically updates the the @ref Gyros, @ref Accels, and @ref Magnetometer objects
* Specifically updates the the @ref GyroSensor, @ref AccelSensor, and @ref MagSensor objects
* @{
*
* @file sensors.c
@ -32,7 +32,7 @@
/**
* Input objects: None, takes sensor data via pios
* Output objects: @ref Gyros @ref Accels @ref Magnetometer
* Output objects: @ref GyroSensor @ref AccelSensor @ref MagSensor
*
* The module executes in its own thread.
*
@ -49,12 +49,10 @@
#include <openpilot.h>
#include <homelocation.h>
#include <magnetometer.h>
#include <magbias.h>
#include <accels.h>
#include <gyros.h>
#include <gyrosbias.h>
#include <attitudeactual.h>
#include <magsensor.h>
#include <accelsensor.h>
#include <gyrosensor.h>
#include <attitudestate.h>
#include <attitudesettings.h>
#include <revocalibration.h>
#include <flightstatus.h>
@ -75,14 +73,13 @@
// Private functions
static void SensorsTask(void *parameters);
static void settingsUpdatedCb(UAVObjEvent *objEv);
static void magOffsetEstimation(MagnetometerData *mag);
// static void magOffsetEstimation(MagSensorData *mag);
// Private variables
static xTaskHandle sensorsTaskHandle;
RevoCalibrationData cal;
// These values are initialized by settings but can be updated by the attitude algorithm
static bool bias_correct_gyro = true;
static float mag_bias[3] = { 0, 0, 0 };
static float mag_scale[3] = { 0, 0, 0 };
@ -111,11 +108,9 @@ static int8_t rotate = 0;
*/
int32_t SensorsInitialize(void)
{
GyrosInitialize();
GyrosBiasInitialize();
AccelsInitialize();
MagnetometerInitialize();
MagBiasInitialize();
GyroSensorInitialize();
AccelSensorInitialize();
MagSensorInitialize();
RevoCalibrationInitialize();
AttitudeSettingsInitialize();
@ -244,8 +239,8 @@ static void SensorsTask(__attribute__((unused)) void *parameters)
accel_samples = 0;
gyro_samples = 0;
AccelsData accelsData;
GyrosData gyrosData;
AccelSensorData accelSensorData;
GyroSensorData gyroSensorData;
switch (bdinfo->board_rev) {
case 0x01: // L3GD20 + BMA180 board
@ -281,7 +276,7 @@ static void SensorsTask(__attribute__((unused)) void *parameters)
accel_scaling = PIOS_BMA180_GetScale();
// Get temp from last reading
accelsData.temperature = 25.0f + ((float)accel.temperature - 2.0f) / 2.0f;
accelSensorData.temperature = 25.0f + ((float)accel.temperature - 2.0f) / 2.0f;
}
#endif /* if defined(PIOS_INCLUDE_BMA180) */
#if defined(PIOS_INCLUDE_L3GD20)
@ -303,7 +298,7 @@ static void SensorsTask(__attribute__((unused)) void *parameters)
gyro_scaling = PIOS_L3GD20_GetScale();
// Get temp from last reading
gyrosData.temperature = gyro.temperature;
gyroSensorData.temperature = gyro.temperature;
}
#endif /* if defined(PIOS_INCLUDE_L3GD20) */
break;
@ -336,8 +331,8 @@ static void SensorsTask(__attribute__((unused)) void *parameters)
gyro_scaling = PIOS_MPU6000_GetScale();
accel_scaling = PIOS_MPU6000_GetAccelScale();
gyrosData.temperature = 35.0f + ((float)mpu6000_data.temperature + 512.0f) / 340.0f;
accelsData.temperature = 35.0f + ((float)mpu6000_data.temperature + 512.0f) / 340.0f;
gyroSensorData.temperature = 35.0f + ((float)mpu6000_data.temperature + 512.0f) / 340.0f;
accelSensorData.temperature = 35.0f + ((float)mpu6000_data.temperature + 512.0f) / 340.0f;
}
#endif /* PIOS_INCLUDE_MPU6000 */
break;
@ -354,15 +349,15 @@ static void SensorsTask(__attribute__((unused)) void *parameters)
accels[2] * accel_scaling * accel_scale[2] - accel_bias[2] };
if (rotate) {
rot_mult(R, accels_out, accels);
accelsData.x = accels[0];
accelsData.y = accels[1];
accelsData.z = accels[2];
accelSensorData.x = accels[0];
accelSensorData.y = accels[1];
accelSensorData.z = accels[2];
} else {
accelsData.x = accels_out[0];
accelsData.y = accels_out[1];
accelsData.z = accels_out[2];
accelSensorData.x = accels_out[0];
accelSensorData.y = accels_out[1];
accelSensorData.z = accels_out[2];
}
AccelsSet(&accelsData);
AccelSensorSet(&accelSensorData);
// Scale the gyros
float gyros[3] = { (float)gyro_accum[0] / gyro_samples,
@ -373,30 +368,22 @@ static void SensorsTask(__attribute__((unused)) void *parameters)
gyros[2] * gyro_scaling * gyro_scale[2] - gyro_staticbias[2] };
if (rotate) {
rot_mult(R, gyros_out, gyros);
gyrosData.x = gyros[0];
gyrosData.y = gyros[1];
gyrosData.z = gyros[2];
gyroSensorData.x = gyros[0];
gyroSensorData.y = gyros[1];
gyroSensorData.z = gyros[2];
} else {
gyrosData.x = gyros_out[0];
gyrosData.y = gyros_out[1];
gyrosData.z = gyros_out[2];
gyroSensorData.x = gyros_out[0];
gyroSensorData.y = gyros_out[1];
gyroSensorData.z = gyros_out[2];
}
if (bias_correct_gyro) {
// Apply bias correction to the gyros from the state estimator
GyrosBiasData gyrosBias;
GyrosBiasGet(&gyrosBias);
gyrosData.x -= gyrosBias.x;
gyrosData.y -= gyrosBias.y;
gyrosData.z -= gyrosBias.z;
}
GyrosSet(&gyrosData);
GyroSensorSet(&gyroSensorData);
// Because most crafts wont get enough information from gravity to zero yaw gyro, we try
// and make it average zero (weakly)
#if defined(PIOS_INCLUDE_HMC5883)
MagnetometerData mag;
MagSensorData mag;
if (PIOS_HMC5883_NewDataAvailable() || PIOS_DELAY_DiffuS(mag_update_time) > 150000) {
int16_t values[3];
PIOS_HMC5883_ReadMag(values);
@ -415,12 +402,7 @@ static void SensorsTask(__attribute__((unused)) void *parameters)
mag.z = mags[2];
}
// Correct for mag bias and update if the rate is non zero
if (cal.MagBiasNullingRate > 0) {
magOffsetEstimation(&mag);
}
MagnetometerSet(&mag);
MagSensorSet(&mag);
mag_update_time = PIOS_DELAY_GetRaw();
}
#endif /* if defined(PIOS_INCLUDE_HMC5883) */
@ -433,107 +415,6 @@ static void SensorsTask(__attribute__((unused)) void *parameters)
}
}
/**
* Perform an update of the @ref MagBias based on
* Magnetometer Offset Cancellation: Theory and Implementation,
* revisited William Premerlani, October 14, 2011
*/
static void magOffsetEstimation(MagnetometerData *mag)
{
#if 0
// Constants, to possibly go into a UAVO
static const float MIN_NORM_DIFFERENCE = 50;
static float B2[3] = { 0, 0, 0 };
MagBiasData magBias;
MagBiasGet(&magBias);
// Remove the current estimate of the bias
mag->x -= magBias.x;
mag->y -= magBias.y;
mag->z -= magBias.z;
// First call
if (B2[0] == 0 && B2[1] == 0 && B2[2] == 0) {
B2[0] = mag->x;
B2[1] = mag->y;
B2[2] = mag->z;
return;
}
float B1[3] = { mag->x, mag->y, mag->z };
float norm_diff = sqrtf(powf(B2[0] - B1[0], 2) + powf(B2[1] - B1[1], 2) + powf(B2[2] - B1[2], 2));
if (norm_diff > MIN_NORM_DIFFERENCE) {
float norm_b1 = sqrtf(B1[0] * B1[0] + B1[1] * B1[1] + B1[2] * B1[2]);
float norm_b2 = sqrtf(B2[0] * B2[0] + B2[1] * B2[1] + B2[2] * B2[2]);
float scale = cal.MagBiasNullingRate * (norm_b2 - norm_b1) / norm_diff;
float b_error[3] = { (B2[0] - B1[0]) * scale, (B2[1] - B1[1]) * scale, (B2[2] - B1[2]) * scale };
magBias.x += b_error[0];
magBias.y += b_error[1];
magBias.z += b_error[2];
MagBiasSet(&magBias);
// Store this value to compare against next update
B2[0] = B1[0]; B2[1] = B1[1]; B2[2] = B1[2];
}
#else /* if 0 */
MagBiasData magBias;
MagBiasGet(&magBias);
// Remove the current estimate of the bias
mag->x -= magBias.x;
mag->y -= magBias.y;
mag->z -= magBias.z;
HomeLocationData homeLocation;
HomeLocationGet(&homeLocation);
AttitudeActualData attitude;
AttitudeActualGet(&attitude);
const float Rxy = sqrtf(homeLocation.Be[0] * homeLocation.Be[0] + homeLocation.Be[1] * homeLocation.Be[1]);
const float Rz = homeLocation.Be[2];
const float rate = cal.MagBiasNullingRate;
float Rot[3][3];
float B_e[3];
float xy[2];
float delta[3];
// Get the rotation matrix
Quaternion2R(&attitude.q1, R);
// Rotate the mag into the NED frame
B_e[0] = Rot[0][0] * mag->x + Rot[1][0] * mag->y + Rot[2][0] * mag->z;
B_e[1] = Rot[0][1] * mag->x + Rot[1][1] * mag->y + Rot[2][1] * mag->z;
B_e[2] = Rot[0][2] * mag->x + Rot[1][2] * mag->y + Rot[2][2] * mag->z;
float cy = cosf(DEG2RAD(attitude.Yaw));
float sy = sinf(DEG2RAD(attitude.Yaw));
xy[0] = cy * B_e[0] + sy * B_e[1];
xy[1] = -sy * B_e[0] + cy * B_e[1];
float xy_norm = sqrtf(xy[0] * xy[0] + xy[1] * xy[1]);
delta[0] = -rate * (xy[0] / xy_norm * Rxy - xy[0]);
delta[1] = -rate * (xy[1] / xy_norm * Rxy - xy[1]);
delta[2] = -rate * (Rz - B_e[2]);
if (!isnan(delta[0]) && !isinf(delta[0]) &&
!isnan(delta[1]) && !isinf(delta[1]) &&
!isnan(delta[2]) && !isinf(delta[2])) {
magBias.x += delta[0];
magBias.y += delta[1];
magBias.z += delta[2];
MagBiasSet(&magBias);
}
#endif /* if 0 */
}
/**
* Locally cache some variables from the AtttitudeSettings object
*/
@ -560,18 +441,9 @@ static void settingsUpdatedCb(__attribute__((unused)) UAVObjEvent *objEv)
gyro_scale[1] = cal.gyro_scale[REVOCALIBRATION_GYRO_SCALE_Y];
gyro_scale[2] = cal.gyro_scale[REVOCALIBRATION_GYRO_SCALE_Z];
// Zero out any adaptive tracking
MagBiasData magBias;
MagBiasGet(&magBias);
magBias.x = 0;
magBias.y = 0;
magBias.z = 0;
MagBiasSet(&magBias);
AttitudeSettingsData attitudeSettings;
AttitudeSettingsGet(&attitudeSettings);
bias_correct_gyro = (cal.BiasCorrectedRaw == REVOCALIBRATION_BIASCORRECTEDRAW_TRUE);
// Indicates not to expend cycles on rotation
if (attitudeSettings.BoardRotation[0] == 0 && attitudeSettings.BoardRotation[1] == 0 &&

397
flight/modules/Sensors/simulated/sensors.c → flight/modules/Sensors/simulated/Sensors/sensors.c
View File

@ -4,7 +4,7 @@
* @{
* @addtogroup Sensors
* @brief Acquires sensor data
* Specifically updates the the @ref Gyros, @ref Accels, and @ref Magnetometer objects
* Specifically updates the the @ref GyroSensor, @ref AccelSensor, and @ref MagSensor objects
* @{
*
* @file sensors.c
@ -32,7 +32,7 @@
/**
* Input objects: None, takes sensor data via pios
* Output objects: @ref Gyros @ref Accels @ref Magnetometer
* Output objects: @ref GyroSensor @ref AccelSensor @ref MagSensor
*
* The module executes in its own thread.
*
@ -48,22 +48,21 @@
#include <openpilot.h>
#include "attitude.h"
#include "accels.h"
#include "attitudestate.h"
#include "accelsensor.h"
#include "actuatordesired.h"
#include "attitudeactual.h"
#include "attitudestate.h"
#include "attitudesimulated.h"
#include "attitudesettings.h"
#include "rawairspeed.h"
#include "baroaltitude.h"
#include "gyros.h"
#include "gyrosbias.h"
#include "airspeedsensor.h"
#include "barosensor.h"
#include "magsensor.h"
#include "gyrosensor.h"
#include "flightstatus.h"
#include "gpsposition.h"
#include "gpsvelocity.h"
#include "gpspositionsensor.h"
#include "gpsvelocitysensor.h"
#include "homelocation.h"
#include "magnetometer.h"
#include "magbias.h"
// #include "sensor.h"
#include "ratedesired.h"
#include "revocalibration.h"
#include "systemsettings.h"
@ -90,8 +89,6 @@ static void simulateModelAgnostic();
static void simulateModelQuadcopter();
static void simulateModelAirplane();
static void magOffsetEstimation(MagnetometerData *mag);
static float accel_bias[3];
static float rand_gauss();
@ -109,16 +106,15 @@ int32_t SensorsInitialize(void)
accel_bias[1] = rand_gauss() / 10;
accel_bias[2] = rand_gauss() / 10;
AccelsInitialize();
AccelSensorInitialize();
AttitudeSimulatedInitialize();
BaroAltitudeInitialize();
BaroSensorInitialize();
AirspeedSensorInitialize();
GyrosInitialize();
GyrosBiasInitialize();
GPSPositionInitialize();
GPSVelocityInitialize();
MagnetometerInitialize();
MagBiasInitialize();
GyroSensorInitialize();
// GyrosBiasInitialize();
GPSPositionSensorInitialize();
GPSVelocitySensorInitialize();
MagSensorInitialize();
RevoCalibrationInitialize();
return 0;
@ -218,47 +214,49 @@ static void SensorsTask(__attribute__((unused)) void *parameters)
static void simulateConstant()
{
AccelsData accelsData; // Skip get as we set all the fields
AccelSensorData accelSensorData; // Skip get as we set all the fields
accelsData.x = 0;
accelsData.y = 0;
accelsData.z = -GRAV;
accelsData.temperature = 0;
AccelsSet(&accelsData);
accelSensorData.x = 0;
accelSensorData.y = 0;
accelSensorData.z = -GRAV;
accelSensorData.temperature = 0;
AccelSensorSet(&accelSensorData);
GyrosData gyrosData; // Skip get as we set all the fields
gyrosData.x = 0;
gyrosData.y = 0;
gyrosData.z = 0;
GyroSensorData gyroSensorData; // Skip get as we set all the fields
gyroSensorData.x = 0;
gyroSensorData.y = 0;
gyroSensorData.z = 0;
/* TODO
// Apply bias correction to the gyros
GyrosBiasData gyrosBias;
GyrosBiasGet(&gyrosBias);
gyrosData.x += gyrosBias.x;
gyrosData.y += gyrosBias.y;
gyrosData.z += gyrosBias.z;
gyroSensorData.x += gyrosBias.x;
gyroSensorData.y += gyrosBias.y;
gyroSensorData.z += gyrosBias.z;
*/
GyrosSet(&gyrosData);
GyroSensorSet(&gyroSensorData);
BaroAltitudeData baroAltitude;
BaroAltitudeGet(&baroAltitude);
baroAltitude.Altitude = 1;
BaroAltitudeSet(&baroAltitude);
BaroSensorData baroSensor;
BaroSensorGet(&baroSensor);
baroSensor.Altitude = 1;
BaroSensorSet(&baroSensor);
GPSPositionData gpsPosition;
GPSPositionGet(&gpsPosition);
GPSPositionSensorData gpsPosition;
GPSPositionSensorGet(&gpsPosition);
gpsPosition.Latitude = 0;
gpsPosition.Longitude = 0;
gpsPosition.Altitude = 0;
GPSPositionSet(&gpsPosition);
GPSPositionSensorSet(&gpsPosition);
// Because most crafts wont get enough information from gravity to zero yaw gyro, we try
// and make it average zero (weakly)
MagnetometerData mag;
MagSensorData mag;
mag.x = 400;
mag.y = 0;
mag.z = 800;
MagnetometerSet(&mag);
MagSensorSet(&mag);
}
static void simulateModelAgnostic()
@ -267,58 +265,60 @@ static void simulateModelAgnostic()
float q[4];
// Simulate accels based on current attitude
AttitudeActualData attitudeActual;
AttitudeStateData attitudeState;
AttitudeActualGet(&attitudeActual);
q[0] = attitudeActual.q1;
q[1] = attitudeActual.q2;
q[2] = attitudeActual.q3;
q[3] = attitudeActual.q4;
AttitudeStateGet(&attitudeState);
q[0] = attitudeState.q1;
q[1] = attitudeState.q2;
q[2] = attitudeState.q3;
q[3] = attitudeState.q4;
Quaternion2R(q, Rbe);
AccelsData accelsData; // Skip get as we set all the fields
accelsData.x = -GRAV * Rbe[0][2];
accelsData.y = -GRAV * Rbe[1][2];
accelsData.z = -GRAV * Rbe[2][2];
accelsData.temperature = 30;
AccelsSet(&accelsData);
AccelSensorData accelSensorData; // Skip get as we set all the fields
accelSensorData.x = -GRAV * Rbe[0][2];
accelSensorData.y = -GRAV * Rbe[1][2];
accelSensorData.z = -GRAV * Rbe[2][2];
accelSensorData.temperature = 30;
AccelSensorSet(&accelSensorData);
RateDesiredData rateDesired;
RateDesiredGet(&rateDesired);
GyrosData gyrosData; // Skip get as we set all the fields
gyrosData.x = rateDesired.Roll + rand_gauss();
gyrosData.y = rateDesired.Pitch + rand_gauss();
gyrosData.z = rateDesired.Yaw + rand_gauss();
GyroSensorData gyroSensorData; // Skip get as we set all the fields
gyroSensorData.x = rateDesired.Roll + rand_gauss();
gyroSensorData.y = rateDesired.Pitch + rand_gauss();
gyroSensorData.z = rateDesired.Yaw + rand_gauss();
/* TODO
// Apply bias correction to the gyros
GyrosBiasData gyrosBias;
GyrosBiasGet(&gyrosBias);
gyrosData.x += gyrosBias.x;
gyrosData.y += gyrosBias.y;
gyrosData.z += gyrosBias.z;
gyroSensorData.x += gyrosBias.x;
gyroSensorData.y += gyrosBias.y;
gyroSensorData.z += gyrosBias.z;
*/
GyrosSet(&gyrosData);
GyroSensorSet(&gyroSensorData);
BaroAltitudeData baroAltitude;
BaroAltitudeGet(&baroAltitude);
baroAltitude.Altitude = 1;
BaroAltitudeSet(&baroAltitude);
BaroSensorData baroSensor;
BaroSensorGet(&baroSensor);
baroSensor.Altitude = 1;
BaroSensorSet(&baroSensor);
GPSPositionData gpsPosition;
GPSPositionGet(&gpsPosition);
GPSPositionSensorData gpsPosition;
GPSPositionSensorGet(&gpsPosition);
gpsPosition.Latitude = 0;
gpsPosition.Longitude = 0;
gpsPosition.Altitude = 0;
GPSPositionSet(&gpsPosition);
GPSPositionSensorSet(&gpsPosition);
// Because most crafts wont get enough information from gravity to zero yaw gyro, we try
// and make it average zero (weakly)
MagnetometerData mag;
MagSensorData mag;
mag.x = 400;
mag.y = 0;
mag.z = 800;
MagnetometerSet(&mag);
MagSensorSet(&mag);
}
float thrustToDegs = 50;
@ -369,10 +369,10 @@ static void simulateModelQuadcopter()
// rpy[1] = control_scaling * actuatorDesired.Pitch * (1 - ACTUATOR_ALPHA) + rpy[1] * ACTUATOR_ALPHA;
// rpy[2] = control_scaling * actuatorDesired.Yaw * (1 - ACTUATOR_ALPHA) + rpy[2] * ACTUATOR_ALPHA;
//
// GyrosData gyrosData; // Skip get as we set all the fields
// gyrosData.x = rpy[0] * 180 / M_PI + rand_gauss();
// gyrosData.y = rpy[1] * 180 / M_PI + rand_gauss();
// gyrosData.z = rpy[2] * 180 / M_PI + rand_gauss();
// GyrosData gyroSensorData; // Skip get as we set all the fields
// gyroSensorData.x = rpy[0] * 180 / M_PI + rand_gauss();
// gyroSensorData.y = rpy[1] * 180 / M_PI + rand_gauss();
// gyroSensorData.z = rpy[2] * 180 / M_PI + rand_gauss();
RateDesiredData rateDesired;
RateDesiredGet(&rateDesired);
@ -381,11 +381,11 @@ static void simulateModelQuadcopter()
rpy[1] = (flightStatus.Armed == FLIGHTSTATUS_ARMED_ARMED) * rateDesired.Pitch * (1 - ACTUATOR_ALPHA) + rpy[1] * ACTUATOR_ALPHA;
rpy[2] = (flightStatus.Armed == FLIGHTSTATUS_ARMED_ARMED) * rateDesired.Yaw * (1 - ACTUATOR_ALPHA) + rpy[2] * ACTUATOR_ALPHA;
GyrosData gyrosData; // Skip get as we set all the fields
gyrosData.x = rpy[0] + rand_gauss();
gyrosData.y = rpy[1] + rand_gauss();
gyrosData.z = rpy[2] + rand_gauss();
GyrosSet(&gyrosData);
GyroSensorData gyroSensorData; // Skip get as we set all the fields
gyroSensorData.x = rpy[0] + rand_gauss();
gyroSensorData.y = rpy[1] + rand_gauss();
gyroSensorData.z = rpy[2] + rand_gauss();
GyroSensorSet(&gyroSensorData);
// Predict the attitude forward in time
float qdot[4];
@ -407,13 +407,13 @@ static void simulateModelQuadcopter()
q[3] = q[3] / qmag;
if (overideAttitude) {
AttitudeActualData attitudeActual;
AttitudeActualGet(&attitudeActual);
attitudeActual.q1 = q[0];
attitudeActual.q2 = q[1];
attitudeActual.q3 = q[2];
attitudeActual.q4 = q[3];
AttitudeActualSet(&attitudeActual);
AttitudeStateData attitudeState;
AttitudeStateGet(&attitudeState);
attitudeState.q1 = q[0];
attitudeState.q2 = q[1];
attitudeState.q3 = q[2];
attitudeState.q4 = q[3];
AttitudeStateSet(&attitudeState);
}
static float wind[3] = { 0, 0, 0 };
@ -454,12 +454,12 @@ static void simulateModelQuadcopter()
ned_accel[2] -= 9.81;
// Transform the accels back in to body frame
AccelsData accelsData; // Skip get as we set all the fields
accelsData.x = ned_accel[0] * Rbe[0][0] + ned_accel[1] * Rbe[0][1] + ned_accel[2] * Rbe[0][2] + accel_bias[0];
accelsData.y = ned_accel[0] * Rbe[1][0] + ned_accel[1] * Rbe[1][1] + ned_accel[2] * Rbe[1][2] + accel_bias[1];
accelsData.z = ned_accel[0] * Rbe[2][0] + ned_accel[1] * Rbe[2][1] + ned_accel[2] * Rbe[2][2] + accel_bias[2];
accelsData.temperature = 30;
AccelsSet(&accelsData);
AccelSensorData accelSensorData; // Skip get as we set all the fields
accelSensorData.x = ned_accel[0] * Rbe[0][0] + ned_accel[1] * Rbe[0][1] + ned_accel[2] * Rbe[0][2] + accel_bias[0];
accelSensorData.y = ned_accel[0] * Rbe[1][0] + ned_accel[1] * Rbe[1][1] + ned_accel[2] * Rbe[1][2] + accel_bias[1];
accelSensorData.z = ned_accel[0] * Rbe[2][0] + ned_accel[1] * Rbe[2][1] + ned_accel[2] * Rbe[2][2] + accel_bias[2];
accelSensorData.temperature = 30;
AccelSensorSet(&accelSensorData);
if (baro_offset == 0) {
// Hacky initialization
@ -471,10 +471,10 @@ static void simulateModelQuadcopter()
// Update baro periodically
static uint32_t last_baro_time = 0;
if (PIOS_DELAY_DiffuS(last_baro_time) / 1.0e6 > BARO_PERIOD) {
BaroAltitudeData baroAltitude;
BaroAltitudeGet(&baroAltitude);
baroAltitude.Altitude = -pos[2] + baro_offset;
BaroAltitudeSet(&baroAltitude);
BaroSensorData baroSensor;
BaroSensorGet(&baroSensor);
baroSensor.Altitude = -pos[2] + baro_offset;
BaroSensorSet(&baroSensor);
last_baro_time = PIOS_DELAY_GetRaw();
}
@ -500,8 +500,8 @@ static void simulateModelQuadcopter()
gps_drift[1] = gps_drift[1] * 0.95 + rand_gauss() / 10.0;
gps_drift[2] = gps_drift[2] * 0.95 + rand_gauss() / 10.0;
GPSPositionData gpsPosition;
GPSPositionGet(&gpsPosition);
GPSPositionSensorData gpsPosition;
GPSPositionSensorGet(&gpsPosition);
gpsPosition.Latitude = homeLocation.Latitude + ((pos[0] + gps_drift[0]) / T[0] * 10.0e6);
gpsPosition.Longitude = homeLocation.Longitude + ((pos[1] + gps_drift[1]) / T[1] * 10.0e6);
gpsPosition.Altitude = homeLocation.Altitude + ((pos[2] + gps_drift[2]) / T[2]);
@ -509,34 +509,31 @@ static void simulateModelQuadcopter()
gpsPosition.Heading = 180 / M_PI * atan2(vel[1] + gps_vel_drift[1], vel[0] + gps_vel_drift[0]);
gpsPosition.Satellites = 7;
gpsPosition.PDOP = 1;
GPSPositionSet(&gpsPosition);
GPSPositionSensorSet(&gpsPosition);
last_gps_time = PIOS_DELAY_GetRaw();
}
// Update GPS Velocity measurements
static uint32_t last_gps_vel_time = 1000; // Delay by a millisecond
if (PIOS_DELAY_DiffuS(last_gps_vel_time) / 1.0e6 > GPS_PERIOD) {
GPSVelocityData gpsVelocity;
GPSVelocityGet(&gpsVelocity);
GPSVelocitySensorData gpsVelocity;
GPSVelocitySensorGet(&gpsVelocity);
gpsVelocity.North = vel[0] + gps_vel_drift[0];
gpsVelocity.East = vel[1] + gps_vel_drift[1];
gpsVelocity.Down = vel[2] + gps_vel_drift[2];
GPSVelocitySet(&gpsVelocity);
GPSVelocitySensorSet(&gpsVelocity);
last_gps_vel_time = PIOS_DELAY_GetRaw();
}
// Update mag periodically
static uint32_t last_mag_time = 0;
if (PIOS_DELAY_DiffuS(last_mag_time) / 1.0e6 > MAG_PERIOD) {
MagnetometerData mag;
MagSensorData mag;
mag.x = homeLocation.Be[0] * Rbe[0][0] + homeLocation.Be[1] * Rbe[0][1] + homeLocation.Be[2] * Rbe[0][2];
mag.y = homeLocation.Be[0] * Rbe[1][0] + homeLocation.Be[1] * Rbe[1][1] + homeLocation.Be[2] * Rbe[1][2];
mag.z = homeLocation.Be[0] * Rbe[2][0] + homeLocation.Be[1] * Rbe[2][1] + homeLocation.Be[2] * Rbe[2][2];
// Run the offset compensation algorithm from the firmware
magOffsetEstimation(&mag);
MagnetometerSet(&mag);
MagSensorSet(&mag);
last_mag_time = PIOS_DELAY_GetRaw();
}
@ -615,20 +612,20 @@ static void simulateModelAirplane()
// rpy[1] = control_scaling * actuatorDesired.Pitch * (1 - ACTUATOR_ALPHA) + rpy[1] * ACTUATOR_ALPHA;
// rpy[2] = control_scaling * actuatorDesired.Yaw * (1 - ACTUATOR_ALPHA) + rpy[2] * ACTUATOR_ALPHA;
//
// GyrosData gyrosData; // Skip get as we set all the fields
// gyrosData.x = rpy[0] * 180 / M_PI + rand_gauss();
// gyrosData.y = rpy[1] * 180 / M_PI + rand_gauss();
// gyrosData.z = rpy[2] * 180 / M_PI + rand_gauss();
// GyrosData gyroSensorData; // Skip get as we set all the fields
// gyroSensorData.x = rpy[0] * 180 / M_PI + rand_gauss();
// gyroSensorData.y = rpy[1] * 180 / M_PI + rand_gauss();
// gyroSensorData.z = rpy[2] * 180 / M_PI + rand_gauss();
/**** 1. Update attitude ****/
RateDesiredData rateDesired;
RateDesiredGet(&rateDesired);
// Need to get roll angle for easy cross coupling
AttitudeActualData attitudeActual;
AttitudeActualGet(&attitudeActual);
double roll = attitudeActual.Roll;
double pitch = attitudeActual.Pitch;
AttitudeStateData attitudeState;
AttitudeStateGet(&attitudeState);
double roll = attitudeState.Roll;
double pitch = attitudeState.Pitch;
rpy[0] = (flightStatus.Armed == FLIGHTSTATUS_ARMED_ARMED) * rateDesired.Roll * (1 - ACTUATOR_ALPHA) + rpy[0] * ACTUATOR_ALPHA;
rpy[1] = (flightStatus.Armed == FLIGHTSTATUS_ARMED_ARMED) * rateDesired.Pitch * (1 - ACTUATOR_ALPHA) + rpy[1] * ACTUATOR_ALPHA;
@ -636,11 +633,11 @@ static void simulateModelAirplane()
rpy[2] += roll * ROLL_HEADING_COUPLING;
GyrosData gyrosData; // Skip get as we set all the fields
gyrosData.x = rpy[0] + rand_gauss();
gyrosData.y = rpy[1] + rand_gauss();
gyrosData.z = rpy[2] + rand_gauss();
GyrosSet(&gyrosData);
GyroSensorData gyroSensorData; // Skip get as we set all the fields
gyroSensorData.x = rpy[0] + rand_gauss();
gyroSensorData.y = rpy[1] + rand_gauss();
gyroSensorData.z = rpy[2] + rand_gauss();
GyroSensorSet(&gyroSensorData);
// Predict the attitude forward in time
float qdot[4];
@ -662,13 +659,13 @@ static void simulateModelAirplane()
q[3] = q[3] / qmag;
if (overideAttitude) {
AttitudeActualData attitudeActual;
AttitudeActualGet(&attitudeActual);
attitudeActual.q1 = q[0];
attitudeActual.q2 = q[1];
attitudeActual.q3 = q[2];
attitudeActual.q4 = q[3];
AttitudeActualSet(&attitudeActual);
AttitudeStateData attitudeState;
AttitudeStateGet(&attitudeState);
attitudeState.q1 = q[0];
attitudeState.q2 = q[1];
attitudeState.q3 = q[2];
attitudeState.q4 = q[3];
AttitudeStateSet(&attitudeState);
}
/**** 2. Update position based on velocity ****/
@ -730,12 +727,12 @@ static void simulateModelAirplane()
ned_accel[2] -= GRAV;
// Transform the accels back in to body frame
AccelsData accelsData; // Skip get as we set all the fields
accelsData.x = ned_accel[0] * Rbe[0][0] + ned_accel[1] * Rbe[0][1] + ned_accel[2] * Rbe[0][2] + accel_bias[0];
accelsData.y = ned_accel[0] * Rbe[1][0] + ned_accel[1] * Rbe[1][1] + ned_accel[2] * Rbe[1][2] + accel_bias[1];
accelsData.z = ned_accel[0] * Rbe[2][0] + ned_accel[1] * Rbe[2][1] + ned_accel[2] * Rbe[2][2] + accel_bias[2];
accelsData.temperature = 30;
AccelsSet(&accelsData);
AccelSensorData accelSensorData; // Skip get as we set all the fields
accelSensorData.x = ned_accel[0] * Rbe[0][0] + ned_accel[1] * Rbe[0][1] + ned_accel[2] * Rbe[0][2] + accel_bias[0];
accelSensorData.y = ned_accel[0] * Rbe[1][0] + ned_accel[1] * Rbe[1][1] + ned_accel[2] * Rbe[1][2] + accel_bias[1];
accelSensorData.z = ned_accel[0] * Rbe[2][0] + ned_accel[1] * Rbe[2][1] + ned_accel[2] * Rbe[2][2] + accel_bias[2];
accelSensorData.temperature = 30;
AccelSensorSet(&accelSensorData);
if (baro_offset == 0) {
// Hacky initialization
@ -747,10 +744,10 @@ static void simulateModelAirplane()
// Update baro periodically
static uint32_t last_baro_time = 0;
if (PIOS_DELAY_DiffuS(last_baro_time) / 1.0e6 > BARO_PERIOD) {
BaroAltitudeData baroAltitude;
BaroAltitudeGet(&baroAltitude);
baroAltitude.Altitude = -pos[2] + baro_offset;
BaroAltitudeSet(&baroAltitude);
BaroSensorData baroSensor;
BaroSensorGet(&baroSensor);
baroSensor.Altitude = -pos[2] + baro_offset;
BaroSensorSet(&baroSensor);
last_baro_time = PIOS_DELAY_GetRaw();
}
@ -786,8 +783,8 @@ static void simulateModelAirplane()
gps_drift[1] = gps_drift[1] * 0.95 + rand_gauss() / 10.0;
gps_drift[2] = gps_drift[2] * 0.95 + rand_gauss() / 10.0;
GPSPositionData gpsPosition;
GPSPositionGet(&gpsPosition);
GPSPositionSensorData gpsPosition;
GPSPositionSensorGet(&gpsPosition);
gpsPosition.Latitude = homeLocation.Latitude + ((pos[0] + gps_drift[0]) / T[0] * 10.0e6);
gpsPosition.Longitude = homeLocation.Longitude + ((pos[1] + gps_drift[1]) / T[1] * 10.0e6);
gpsPosition.Altitude = homeLocation.Altitude + ((pos[2] + gps_drift[2]) / T[2]);
@ -795,31 +792,30 @@ static void simulateModelAirplane()
gpsPosition.Heading = 180 / M_PI * atan2(vel[1] + gps_vel_drift[1], vel[0] + gps_vel_drift[0]);
gpsPosition.Satellites = 7;
gpsPosition.PDOP = 1;
GPSPositionSet(&gpsPosition);
GPSPositionSensorSet(&gpsPosition);
last_gps_time = PIOS_DELAY_GetRaw();
}
// Update GPS Velocity measurements
static uint32_t last_gps_vel_time = 1000; // Delay by a millisecond
if (PIOS_DELAY_DiffuS(last_gps_vel_time) / 1.0e6 > GPS_PERIOD) {
GPSVelocityData gpsVelocity;
GPSVelocityGet(&gpsVelocity);
GPSVelocitySensorData gpsVelocity;
GPSVelocitySensorGet(&gpsVelocity);
gpsVelocity.North = vel[0] + gps_vel_drift[0];
gpsVelocity.East = vel[1] + gps_vel_drift[1];
gpsVelocity.Down = vel[2] + gps_vel_drift[2];
GPSVelocitySet(&gpsVelocity);
GPSVelocitySensorSet(&gpsVelocity);
last_gps_vel_time = PIOS_DELAY_GetRaw();
}
// Update mag periodically
static uint32_t last_mag_time = 0;
if (PIOS_DELAY_DiffuS(last_mag_time) / 1.0e6 > MAG_PERIOD) {
MagnetometerData mag;
MagSensorData mag;
mag.x = 100 + homeLocation.Be[0] * Rbe[0][0] + homeLocation.Be[1] * Rbe[0][1] + homeLocation.Be[2] * Rbe[0][2];
mag.y = 100 + homeLocation.Be[0] * Rbe[1][0] + homeLocation.Be[1] * Rbe[1][1] + homeLocation.Be[2] * Rbe[1][2];
mag.z = 100 + homeLocation.Be[0] * Rbe[2][0] + homeLocation.Be[1] * Rbe[2][1] + homeLocation.Be[2] * Rbe[2][2];
magOffsetEstimation(&mag);
MagnetometerSet(&mag);
MagSensorSet(&mag);
last_mag_time = PIOS_DELAY_GetRaw();
}
@ -857,105 +853,6 @@ static float rand_gauss(void)
}
}
/**
* Perform an update of the @ref MagBias based on
* Magnetometer Offset Cancellation: Theory and Implementation,
* revisited William Premerlani, October 14, 2011
*/
static void magOffsetEstimation(MagnetometerData *mag)
{
#if 0
RevoCalibrationData cal;
RevoCalibrationGet(&cal);
// Constants, to possibly go into a UAVO
static const float MIN_NORM_DIFFERENCE = 50;
static float B2[3] = { 0, 0, 0 };
MagBiasData magBias;
MagBiasGet(&magBias);
// Remove the current estimate of the bias
mag->x -= magBias.x;
mag->y -= magBias.y;
mag->z -= magBias.z;
// First call
if (B2[0] == 0 && B2[1] == 0 && B2[2] == 0) {
B2[0] = mag->x;
B2[1] = mag->y;
B2[2] = mag->z;
return;
}
float B1[3] = { mag->x, mag->y, mag->z };
float norm_diff = sqrtf(powf(B2[0] - B1[0], 2) + powf(B2[1] - B1[1], 2) + powf(B2[2] - B1[2], 2));
if (norm_diff > MIN_NORM_DIFFERENCE) {
float norm_b1 = sqrtf(B1[0] * B1[0] + B1[1] * B1[1] + B1[2] * B1[2]);
float norm_b2 = sqrtf(B2[0] * B2[0] + B2[1] * B2[1] + B2[2] * B2[2]);
float scale = cal.MagBiasNullingRate * (norm_b2 - norm_b1) / norm_diff;
float b_error[3] = { (B2[0] - B1[0]) * scale, (B2[1] - B1[1]) * scale, (B2[2] - B1[2]) * scale };
magBias.x += b_error[0];
magBias.y += b_error[1];
magBias.z += b_error[2];
MagBiasSet(&magBias);
// Store this value to compare against next update
B2[0] = B1[0]; B2[1] = B1[1]; B2[2] = B1[2];
}
#else /* if 0 */
HomeLocationData homeLocation;
HomeLocationGet(&homeLocation);
AttitudeActualData attitude;
AttitudeActualGet(&attitude);
MagBiasData magBias;
MagBiasGet(&magBias);
// Remove the current estimate of the bias
mag->x -= magBias.x;
mag->y -= magBias.y;
mag->z -= magBias.z;
const float Rxy = sqrtf(homeLocation.Be[0] * homeLocation.Be[0] + homeLocation.Be[1] * homeLocation.Be[1]);
const float Rz = homeLocation.Be[2];
const float rate = 0.01;
float R[3][3];
float B_e[3];
float xy[2];
float delta[3];
// Get the rotation matrix
Quaternion2R(&attitude.q1, R);
// Rotate the mag into the NED frame
B_e[0] = R[0][0] * mag->x + R[1][0] * mag->y + R[2][0] * mag->z;
B_e[1] = R[0][1] * mag->x + R[1][1] * mag->y + R[2][1] * mag->z;
B_e[2] = R[0][2] * mag->x + R[1][2] * mag->y + R[2][2] * mag->z;
float cy = cosf(attitude.Yaw * M_PI / 180.0f);
float sy = sinf(attitude.Yaw * M_PI / 180.0f);
xy[0] = cy * B_e[0] + sy * B_e[1];
xy[1] = -sy * B_e[0] + cy * B_e[1];
float xy_norm = sqrtf(xy[0] * xy[0] + xy[1] * xy[1]);
delta[0] = -rate * (xy[0] / xy_norm * Rxy - xy[0]);
delta[1] = -rate * (xy[1] / xy_norm * Rxy - xy[1]);
delta[2] = -rate * (Rz - B_e[2]);
magBias.x += delta[0];
magBias.y += delta[1];
magBias.z += delta[2];
MagBiasSet(&magBias);
#endif /* if 0 */
}
/**
* @}

2
flight/modules/Stabilization/inc/relay_tuning.h
View File

@ -5,7 +5,7 @@
* @addtogroup StabilizationModule Stabilization Module
* @brief Relay tuning controller
* @note This object updates the @ref ActuatorDesired "Actuator Desired" based on the
* PID loops on the @ref AttitudeDesired "Attitude Desired" and @ref AttitudeActual "Attitude Actual"
* PID loops on the @ref AttitudeDesired "Attitude Desired" and @ref AttitudeState "Attitude State"
* @{
*
* @file relay_tuning.h

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

@ -5,7 +5,7 @@
* @addtogroup StabilizationModule Stabilization Module
* @brief Stabilization PID loops in an airframe type independent manner
* @note This object updates the @ref ActuatorDesired "Actuator Desired" based on the
* PID loops on the @ref AttitudeDesired "Attitude Desired" and @ref AttitudeActual "Attitude Actual"
* PID loops on the @ref AttitudeDesired "Attitude Desired" and @ref AttitudeState "Attitude State"
* @{
*
* @file stabilization.h

2
flight/modules/Stabilization/relay_tuning.c
View File

@ -5,7 +5,7 @@
* @addtogroup StabilizationModule Stabilization Module
* @brief Relay tuning controller
* @note This object updates the @ref ActuatorDesired "Actuator Desired" based on the
* PID loops on the @ref AttitudeDesired "Attitude Desired" and @ref AttitudeActual "Attitude Actual"
* PID loops on the @ref AttitudeDesired "Attitude Desired" and @ref AttitudeState "Attitude State"
* @{
*
* @file stabilization.c

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

@ -5,7 +5,7 @@
* @addtogroup StabilizationModule Stabilization Module
* @brief Stabilization PID loops in an airframe type independent manner
* @note This object updates the @ref ActuatorDesired "Actuator Desired" based on the
* PID loops on the @ref AttitudeDesired "Attitude Desired" and @ref AttitudeActual "Attitude Actual"
* PID loops on the @ref AttitudeDesired "Attitude Desired" and @ref AttitudeState "Attitude State"
* @{
*
* @file stabilization.c
@ -40,8 +40,8 @@
#include "relaytuning.h"
#include "relaytuningsettings.h"
#include "stabilizationdesired.h"
#include "attitudeactual.h"
#include "gyros.h"
#include "attitudestate.h"
#include "gyrostate.h"
#include "flightstatus.h"
#include "manualcontrol.h" // Just to get a macro
#include "taskinfo.h"
@ -104,8 +104,8 @@ int32_t StabilizationStart()
queue = xQueueCreate(MAX_QUEUE_SIZE, sizeof(UAVObjEvent));
// Listen for updates.
// AttitudeActualConnectQueue(queue);
GyrosConnectQueue(queue);
// AttitudeStateConnectQueue(queue);
GyroStateConnectQueue(queue);
StabilizationSettingsConnectCallback(SettingsUpdatedCb);
SettingsUpdatedCb(StabilizationSettingsHandle());
@ -152,8 +152,8 @@ static void stabilizationTask(__attribute__((unused)) void *parameters)
ActuatorDesiredData actuatorDesired;
StabilizationDesiredData stabDesired;
RateDesiredData rateDesired;
AttitudeActualData attitudeActual;
GyrosData gyrosData;
AttitudeStateData attitudeState;
GyroStateData gyroStateData;
FlightStatusData flightStatus;
SettingsUpdatedCb((UAVObjEvent *)NULL);
@ -178,8 +178,8 @@ static void stabilizationTask(__attribute__((unused)) void *parameters)
FlightStatusGet(&flightStatus);
StabilizationDesiredGet(&stabDesired);
AttitudeActualGet(&attitudeActual);
GyrosGet(&gyrosData);
AttitudeStateGet(&attitudeState);
GyroStateGet(&gyroStateData);
#ifdef DIAG_RATEDESIRED
RateDesiredGet(&rateDesired);
#endif
@ -195,32 +195,32 @@ static void stabilizationTask(__attribute__((unused)) void *parameters)
if (stabDesired.StabilizationMode[STABILIZATIONDESIRED_STABILIZATIONMODE_ROLL] == STABILIZATIONDESIRED_STABILIZATIONMODE_ATTITUDE) {
rpy_desired[0] = stabDesired.Roll;
} else {
rpy_desired[0] = attitudeActual.Roll;
rpy_desired[0] = attitudeState.Roll;
}
if (stabDesired.StabilizationMode[STABILIZATIONDESIRED_STABILIZATIONMODE_PITCH] == STABILIZATIONDESIRED_STABILIZATIONMODE_ATTITUDE) {
rpy_desired[1] = stabDesired.Pitch;
} else {
rpy_desired[1] = attitudeActual.Pitch;
rpy_desired[1] = attitudeState.Pitch;
}
if (stabDesired.StabilizationMode[STABILIZATIONDESIRED_STABILIZATIONMODE_YAW] == STABILIZATIONDESIRED_STABILIZATIONMODE_ATTITUDE) {
rpy_desired[2] = stabDesired.Yaw;
} else {
rpy_desired[2] = attitudeActual.Yaw;
rpy_desired[2] = attitudeState.Yaw;
}
RPY2Quaternion(rpy_desired, q_desired);
quat_inverse(q_desired);
quat_mult(q_desired, &attitudeActual.q1, q_error);
quat_mult(q_desired, &attitudeState.q1, q_error);
quat_inverse(q_error);
Quaternion2RPY(q_error, local_error);
#else /* if defined(PIOS_QUATERNION_STABILIZATION) */
// Simpler algorithm for CC, less memory
float local_error[3] = { stabDesired.Roll - attitudeActual.Roll,
stabDesired.Pitch - attitudeActual.Pitch,
stabDesired.Yaw - attitudeActual.Yaw };
float local_error[3] = { stabDesired.Roll - attitudeState.Roll,
stabDesired.Pitch - attitudeState.Pitch,
stabDesired.Yaw - attitudeState.Yaw };
// find shortest way
float modulo = fmodf(local_error[2] + 180.0f, 360.0f);
if (modulo < 0) {
@ -231,9 +231,9 @@ static void stabilizationTask(__attribute__((unused)) void *parameters)
#endif /* if defined(PIOS_QUATERNION_STABILIZATION) */
float gyro_filtered[3];
gyro_filtered[0] = gyro_filtered[0] * gyro_alpha + gyrosData.x * (1 - gyro_alpha);
gyro_filtered[1] = gyro_filtered[1] * gyro_alpha + gyrosData.y * (1 - gyro_alpha);
gyro_filtered[2] = gyro_filtered[2] * gyro_alpha + gyrosData.z * (1 - gyro_alpha);
gyro_filtered[0] = gyro_filtered[0] * gyro_alpha + gyroStateData.x * (1 - gyro_alpha);
gyro_filtered[1] = gyro_filtered[1] * gyro_alpha + gyroStateData.y * (1 - gyro_alpha);
gyro_filtered[2] = gyro_filtered[2] * gyro_alpha + gyroStateData.z * (1 - gyro_alpha);
float *attitudeDesiredAxis = &stabDesired.Roll;
float *actuatorDesiredAxis = &actuatorDesired.Roll;

92
flight/modules/StateEstimation/filterair.c Normal file
View File

@ -0,0 +1,92 @@
/**
******************************************************************************
* @addtogroup OpenPilotModules OpenPilot Modules
* @{
* @addtogroup State Estimation
* @brief Acquires sensor data and computes state estimate
* @{
*
* @file filterair.c
* @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2013.
* @brief Airspeed filter, calculates true airspeed based on indicated
* airspeed and uncorrected barometric altitude
* NOTE: This Sensor uses UNCORRECTED barometric altitude for
* correction -- run before barometric bias correction!
*
* @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
*/
#include "inc/stateestimation.h"
// Private constants
#define STACK_REQUIRED 64
// simple IAS to TAS aproximation - 2% increase per 1000ft
// since we do not have flowing air temperature information
#define IAS2TAS(alt) (1.0f + (0.02f * (alt) / 304.8f))
// Private types
struct data {
float altitude;
};
// Private functions
static int32_t init(stateFilter *self);
static int32_t filter(stateFilter *self, stateEstimation *state);
int32_t filterAirInitialize(stateFilter *handle)
{
handle->init = &init;
handle->filter = &filter;
handle->localdata = pvPortMalloc(sizeof(struct data));
return STACK_REQUIRED;
}
static int32_t init(stateFilter *self)
{
struct data *this = (struct data *)self->localdata;
this->altitude = 0.0f;
return 0;
}
static int32_t filter(stateFilter *self, stateEstimation *state)
{
struct data *this = (struct data *)self->localdata;
// take static pressure altitude estimation for
if (IS_SET(state->updated, SENSORUPDATES_baro)) {
this->altitude = state->baro[0];
}
// calculate true airspeed estimation
if (IS_SET(state->updated, SENSORUPDATES_airspeed)) {
state->airspeed[1] = state->airspeed[0] * IAS2TAS(this->altitude);
}
return 0;
}
/**
* @}
* @}
*/

110
flight/modules/StateEstimation/filterbaro.c Normal file
View File

@ -0,0 +1,110 @@
/**
******************************************************************************
* @addtogroup OpenPilotModules OpenPilot Modules
* @{
* @addtogroup State Estimation
* @brief Acquires sensor data and computes state estimate
* @{
*
* @file filterbaro.c
* @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2013.
* @brief Barometric altitude filter, calculates altitude offset based on
* GPS altitude offset if available
*
* @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
*/
#include "inc/stateestimation.h"
// Private constants
#define STACK_REQUIRED 64
// low pass filter configuration to calculate offset
// of barometric altitude sensor
// reasoning: updates at: 10 Hz, tau= 300 s settle time
// exp(-(1/f) / tau ) ~=~ 0.9997
#define BARO_OFFSET_LOWPASS_ALPHA 0.9997f
// Private types
struct data {
float baroOffset;
float baroAlt;
bool first_run;
};
// Private variables
// Private functions
static int32_t init(stateFilter *self);
static int32_t filter(stateFilter *self, stateEstimation *state);
int32_t filterBaroInitialize(stateFilter *handle)
{
handle->init = &init;
handle->filter = &filter;
handle->localdata = pvPortMalloc(sizeof(struct data));
return STACK_REQUIRED;
}
static int32_t init(stateFilter *self)
{
struct data *this = (struct data *)self->localdata;
this->baroOffset = 0.0f;
this->first_run = 1;
return 0;
}
static int32_t filter(stateFilter *self, stateEstimation *state)
{
struct data *this = (struct data *)self->localdata;
if (this->first_run) {
// Initialize to current altitude reading at initial location
if (IS_SET(state->updated, SENSORUPDATES_baro)) {
this->first_run = 0;
this->baroOffset = state->baro[0];
this->baroAlt = state->baro[0];
}
} else {
// Track barometric altitude offset with a low pass filter
// based on GPS altitude if available
if (IS_SET(state->updated, SENSORUPDATES_pos)) {
this->baroOffset = BARO_OFFSET_LOWPASS_ALPHA * this->baroOffset +
(1.0f - BARO_OFFSET_LOWPASS_ALPHA)
* (this->baroAlt + state->pos[2]);
}
// calculate bias corrected altitude
if (IS_SET(state->updated, SENSORUPDATES_baro)) {
this->baroAlt = state->baro[0];
state->baro[0] -= this->baroOffset;
}
}
return 0;
}
/**
* @}
* @}
*/

459
flight/modules/StateEstimation/filtercf.c Normal file
View File

@ -0,0 +1,459 @@
/**
******************************************************************************
* @addtogroup OpenPilotModules OpenPilot Modules
* @{
* @addtogroup State Estimation
* @brief Acquires sensor data and computes state estimate
* @{
*
* @file filtercf.c
* @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2013.
* @brief Complementary filter to calculate Attitude from Accels and Gyros
* and optionally magnetometers:
* WARNING: Will drift if the mean acceleration force doesn't point
* to ground, unsafe on fixed wing, since position hold is
* implemented as continuous circular flying!
*
* @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
*/
#include "inc/stateestimation.h"
#include <attitudesettings.h>
#include <attitudestate.h>
#include <flightstatus.h>
#include <homelocation.h>
#include <revocalibration.h>
#include <CoordinateConversions.h>
// Private constants
#define STACK_REQUIRED 512
// Private types
struct data {
AttitudeSettingsData attitudeSettings;
HomeLocationData homeLocation;
bool first_run;
bool useMag;
float currentAccel[3];
float currentMag[3];
float gyroBias[3];
bool accelUpdated;
bool magUpdated;
float accel_alpha;
bool accel_filter_enabled;
float rollPitchBiasRate;
int32_t timeval;
int32_t starttime;
uint8_t init;
bool magCalibrated;
};
// Private variables
bool initialized = 0;
static FlightStatusData flightStatus;
// Private functions
static int32_t initwithmag(stateFilter *self);
static int32_t initwithoutmag(stateFilter *self);
static int32_t maininit(stateFilter *self);
static int32_t filter(stateFilter *self, stateEstimation *state);
static int32_t complementaryFilter(struct data *this, float gyro[3], float accel[3], float mag[3], float attitude[4]);
static void flightStatusUpdatedCb(UAVObjEvent *ev);
static void globalInit(void);
static void globalInit(void)
{
if (!initialized) {
initialized = 1;
FlightStatusInitialize();
HomeLocationInitialize();
RevoCalibrationInitialize();
FlightStatusConnectCallback(&flightStatusUpdatedCb);
flightStatusUpdatedCb(NULL);
}
}
int32_t filterCFInitialize(stateFilter *handle)
{
globalInit();
handle->init = &initwithoutmag;
handle->filter = &filter;
handle->localdata = pvPortMalloc(sizeof(struct data));
return STACK_REQUIRED;
}
int32_t filterCFMInitialize(stateFilter *handle)
{
globalInit();
handle->init = &initwithmag;
handle->filter = &filter;
handle->localdata = pvPortMalloc(sizeof(struct data));
return STACK_REQUIRED;
}
static int32_t initwithmag(stateFilter *self)
{
struct data *this = (struct data *)self->localdata;
this->useMag = 1;
return maininit(self);
}
static int32_t initwithoutmag(stateFilter *self)
{
struct data *this = (struct data *)self->localdata;
this->useMag = 0;
return maininit(self);
}
static int32_t maininit(stateFilter *self)
{
struct data *this = (struct data *)self->localdata;
this->first_run = 1;
this->accelUpdated = 0;
this->magCalibrated = true;
AttitudeSettingsGet(&this->attitudeSettings);
HomeLocationGet(&this->homeLocation);
const float fakeDt = 0.0025f;
if (this->attitudeSettings.AccelTau < 0.0001f) {
this->accel_alpha = 0; // not trusting this to resolve to 0
this->accel_filter_enabled = false;
} else {
this->accel_alpha = expf(-fakeDt / this->attitudeSettings.AccelTau);
this->accel_filter_enabled = true;
}
// reset gyro Bias
this->gyroBias[0] = 0.0f;
this->gyroBias[1] = 0.0f;
this->gyroBias[2] = 0.0f;
return 0;
}
/**
* Collect all required state variables, then run complementary filter
*/
static int32_t filter(stateFilter *self, stateEstimation *state)
{
struct data *this = (struct data *)self->localdata;
int32_t result = 0;
if (IS_SET(state->updated, SENSORUPDATES_mag)) {
this->magUpdated = 1;
this->currentMag[0] = state->mag[0];
this->currentMag[1] = state->mag[1];
this->currentMag[2] = state->mag[2];
}
if (IS_SET(state->updated, SENSORUPDATES_accel)) {
this->accelUpdated = 1;
this->currentAccel[0] = state->accel[0];
this->currentAccel[1] = state->accel[1];
this->currentAccel[2] = state->accel[2];
}
if (IS_SET(state->updated, SENSORUPDATES_gyro)) {
if (this->accelUpdated) {
float attitude[4];
result = complementaryFilter(this, state->gyro, this->currentAccel, this->currentMag, attitude);
if (!result) {
state->attitude[0] = attitude[0];
state->attitude[1] = attitude[1];
state->attitude[2] = attitude[2];
state->attitude[3] = attitude[3];
state->updated |= SENSORUPDATES_attitude;
}
this->accelUpdated = 0;
this->magUpdated = 0;
}
}
return result;
}
static inline void apply_accel_filter(const struct data *this, const float *raw, float *filtered)
{
if (this->accel_filter_enabled) {
filtered[0] = filtered[0] * this->accel_alpha + raw[0] * (1 - this->accel_alpha);
filtered[1] = filtered[1] * this->accel_alpha + raw[1] * (1 - this->accel_alpha);
filtered[2] = filtered[2] * this->accel_alpha + raw[2] * (1 - this->accel_alpha);
} else {
filtered[0] = raw[0];
filtered[1] = raw[1];
filtered[2] = raw[2];
}
}
static int32_t complementaryFilter(struct data *this, float gyro[3], float accel[3], float mag[3], float attitude[4])
{
float dT;
// During initialization and
if (this->first_run) {
#if defined(PIOS_INCLUDE_HMC5883)
// wait until mags have been updated
if (!this->magUpdated) {
return 1;
}
#else
mag[0] = 100.0f;
mag[1] = 0.0f;
mag[2] = 0.0f;
#endif
float magBias[3];
RevoCalibrationmag_biasGet(magBias);
// don't trust Mag for initial orientation if it has not been calibrated
if (magBias[0] < 1e-6f && magBias[1] < 1e-6f && magBias[2] < 1e-6f) {
this->magCalibrated = false;
mag[0] = 100.0f;
mag[1] = 0.0f;
mag[2] = 0.0f;
}
AttitudeStateData attitudeState; // base on previous state
AttitudeStateGet(&attitudeState);
this->init = 0;
// Set initial attitude. Use accels to determine roll and pitch, rotate magnetic measurement accordingly,
// so pseudo "north" vector can be estimated even if the board is not level
attitudeState.Roll = atan2f(-accel[1], -accel[2]);
float zn = cosf(attitudeState.Roll) * mag[2] + sinf(attitudeState.Roll) * mag[1];
float yn = cosf(attitudeState.Roll) * mag[1] - sinf(attitudeState.Roll) * mag[2];
// rotate accels z vector according to roll
float azn = cosf(attitudeState.Roll) * accel[2] + sinf(attitudeState.Roll) * accel[1];
attitudeState.Pitch = atan2f(accel[0], -azn);
float xn = cosf(attitudeState.Pitch) * mag[0] + sinf(attitudeState.Pitch) * zn;
attitudeState.Yaw = atan2f(-yn, xn);
// TODO: This is still a hack
// Put this in a proper generic function in CoordinateConversion.c
// should take 4 vectors: g (0,0,-9.81), accels, Be (or 1,0,0 if no home loc) and magnetometers (or 1,0,0 if no mags)
// should calculate the rotation in 3d space using proper cross product math
// SUBTODO: formulate the math required
attitudeState.Roll = RAD2DEG(attitudeState.Roll);
attitudeState.Pitch = RAD2DEG(attitudeState.Pitch);
attitudeState.Yaw = RAD2DEG(attitudeState.Yaw);
RPY2Quaternion(&attitudeState.Roll, attitude);
this->first_run = 0;
this->timeval = PIOS_DELAY_GetRaw();
this->starttime = this->timeval;
return 0; // must return zero on initial initialization, so attitude will init with a valid quaternion
}
if (this->init == 0 && PIOS_DELAY_DiffuS(this->starttime) < 4000000) {
// wait 4 seconds for the user to get his hands off in case the board was just powered
this->timeval = PIOS_DELAY_GetRaw();
return 1;
} else if (this->init == 0 && PIOS_DELAY_DiffuS(this->starttime) < 10000000) {
// For first 7 seconds use accels to get gyro bias
this->attitudeSettings.AccelKp = 1.0f;
this->attitudeSettings.AccelKi = 0.0f;
this->attitudeSettings.YawBiasRate = 0.23f;
this->accel_filter_enabled = false;
this->rollPitchBiasRate = 0.01f;
this->attitudeSettings.MagKp = this->magCalibrated ? 1.0f : 0.0f;
} else if ((this->attitudeSettings.ZeroDuringArming == ATTITUDESETTINGS_ZERODURINGARMING_TRUE) && (flightStatus.Armed == FLIGHTSTATUS_ARMED_ARMING)) {
this->attitudeSettings.AccelKp = 1.0f;
this->attitudeSettings.AccelKi = 0.0f;
this->attitudeSettings.YawBiasRate = 0.23f;
this->accel_filter_enabled = false;
this->rollPitchBiasRate = 0.01f;
this->attitudeSettings.MagKp = this->magCalibrated ? 1.0f : 0.0f;
this->init = 0;
} else if (this->init == 0) {
// Reload settings (all the rates)
AttitudeSettingsGet(&this->attitudeSettings);
this->rollPitchBiasRate = 0.0f;
if (this->accel_alpha > 0.0f) {
this->accel_filter_enabled = true;
}
this->init = 1;
}
// Compute the dT using the cpu clock
dT = PIOS_DELAY_DiffuS(this->timeval) / 1000000.0f;
this->timeval = PIOS_DELAY_GetRaw();
if (dT < 0.001f) { // safe bounds
dT = 0.001f;
}
AttitudeStateData attitudeState; // base on previous state
AttitudeStateGet(&attitudeState);
// Get the current attitude estimate
quat_copy(&attitudeState.q1, attitude);
float accels_filtered[3];
// Apply smoothing to accel values, to reduce vibration noise before main calculations.
apply_accel_filter(this, accel, accels_filtered);
// Rotate gravity to body frame and cross with accels
float grot[3];
grot[0] = -(2.0f * (attitude[1] * attitude[3] - attitude[0] * attitude[2]));
grot[1] = -(2.0f * (attitude[2] * attitude[3] + attitude[0] * attitude[1]));
grot[2] = -(attitude[0] * attitude[0] - attitude[1] * attitude[1] - attitude[2] * attitude[2] + attitude[3] * attitude[3]);
float grot_filtered[3];
float accel_err[3];
apply_accel_filter(this, grot, grot_filtered);
CrossProduct((const float *)accels_filtered, (const float *)grot_filtered, accel_err);
// Account for accel magnitude
float accel_mag = sqrtf(accels_filtered[0] * accels_filtered[0] + accels_filtered[1] * accels_filtered[1] + accels_filtered[2] * accels_filtered[2]);
if (accel_mag < 1.0e-3f) {
return 2; // safety feature copied from CC
}
// Account for filtered gravity vector magnitude
float grot_mag;
if (this->accel_filter_enabled) {
grot_mag = sqrtf(grot_filtered[0] * grot_filtered[0] + grot_filtered[1] * grot_filtered[1] + grot_filtered[2] * grot_filtered[2]);
} else {
grot_mag = 1.0f;
}
if (grot_mag < 1.0e-3f) {
return 2;
}
accel_err[0] /= (accel_mag * grot_mag);
accel_err[1] /= (accel_mag * grot_mag);
accel_err[2] /= (accel_mag * grot_mag);
float mag_err[3] = { 0.0f };
if (this->magUpdated && this->useMag) {
// Rotate gravity to body frame and cross with accels
float brot[3];
float Rbe[3][3];
Quaternion2R(attitude, Rbe);
rot_mult(Rbe, this->homeLocation.Be, brot);
float mag_len = sqrtf(mag[0] * mag[0] + mag[1] * mag[1] + mag[2] * mag[2]);
mag[0] /= mag_len;
mag[1] /= mag_len;
mag[2] /= mag_len;
float bmag = sqrtf(brot[0] * brot[0] + brot[1] * brot[1] + brot[2] * brot[2]);
brot[0] /= bmag;
brot[1] /= bmag;
brot[2] /= bmag;
// Only compute if neither vector is null
if (bmag < 1.0f || mag_len < 1.0f) {
mag_err[0] = mag_err[1] = mag_err[2] = 0.0f;
} else {
CrossProduct((const float *)mag, (const float *)brot, mag_err);
}
} else {
mag_err[0] = mag_err[1] = mag_err[2] = 0.0f;
}
// Correct rates based on integral coefficient
gyro[0] -= this->gyroBias[0];
gyro[1] -= this->gyroBias[1];
gyro[2] -= this->gyroBias[2];
// 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) {
this->gyroBias[2] -= -mag_err[2] * this->attitudeSettings.MagKi - gyro[2] * this->rollPitchBiasRate;
} else {
this->gyroBias[2] -= -gyro[2] * this->rollPitchBiasRate;
}
float gyrotmp[3] = { gyro[0], gyro[1], gyro[2] };
// 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) {
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;
}
// Work out time derivative from INSAlgo writeup
// Also accounts for the fact that gyros are in deg/s
float qdot[4];
qdot[0] = DEG2RAD(-attitude[1] * gyrotmp[0] - attitude[2] * gyrotmp[1] - attitude[3] * gyrotmp[2]) * dT / 2;
qdot[1] = DEG2RAD(attitude[0] * gyrotmp[0] - attitude[3] * gyrotmp[1] + attitude[2] * gyrotmp[2]) * dT / 2;
qdot[2] = DEG2RAD(attitude[3] * gyrotmp[0] + attitude[0] * gyrotmp[1] - attitude[1] * gyrotmp[2]) * dT / 2;
qdot[3] = DEG2RAD(-attitude[2] * gyrotmp[0] + attitude[1] * gyrotmp[1] + attitude[0] * gyrotmp[2]) * dT / 2;
// Take a time step
attitude[0] = attitude[0] + qdot[0];
attitude[1] = attitude[1] + qdot[1];
attitude[2] = attitude[2] + qdot[2];
attitude[3] = attitude[3] + qdot[3];
if (attitude[0] < 0.0f) {
attitude[0] = -attitude[0];
attitude[1] = -attitude[1];
attitude[2] = -attitude[2];
attitude[3] = -attitude[3];
}
// Renomalize
float qmag = sqrtf(attitude[0] * attitude[0] + attitude[1] * attitude[1] + attitude[2] * attitude[2] + attitude[3] * attitude[3]);
attitude[0] = attitude[0] / qmag;
attitude[1] = attitude[1] / qmag;
attitude[2] = attitude[2] / qmag;
attitude[3] = attitude[3] / qmag;
// If quaternion has become inappropriately short or is nan reinit.
// THIS SHOULD NEVER ACTUALLY HAPPEN
if ((fabsf(qmag) < 1.0e-3f) || isnan(qmag)) {
this->first_run = 1;
return 2;
}
if (this->init) {
return 0;
} else {
return 2; // return "critical" for now, so users can see the zeroing period, switch to more graceful notification later
}
}
static void flightStatusUpdatedCb(__attribute__((unused)) UAVObjEvent *ev)
{
FlightStatusGet(&flightStatus);
}
/**
* @}
* @}
*/

460
flight/modules/StateEstimation/filterekf.c Normal file
View File

@ -0,0 +1,460 @@
/**
******************************************************************************
* @addtogroup OpenPilotModules OpenPilot Modules
* @{
* @addtogroup State Estimation
* @brief Acquires sensor data and computes state estimate
* @{
*
* @file filterekf.c
* @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2013.
* @brief Extended Kalman Filter. Calculates complete system state except
* accelerometer drift.
*
* @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
*/
#include "inc/stateestimation.h"
#include <ekfconfiguration.h>
#include <ekfstatevariance.h>
#include <attitudestate.h>
#include <homelocation.h>
#include <insgps.h>
#include <CoordinateConversions.h>
// Private constants
#define STACK_REQUIRED 2048
#define DT_ALPHA 1e-3f
#define DT_INIT (1.0f / 666.0f) // initialize with 666 Hz (default sensor update rate on revo)
#define IMPORT_SENSOR_IF_UPDATED(shortname, num) \
if (IS_SET(state->updated, SENSORUPDATES_##shortname)) { \
uint8_t t; \
for (t = 0; t < num; t++) { \
this->work.shortname[t] = state->shortname[t]; \
} \
}
// Private types
struct data {
EKFConfigurationData ekfConfiguration;
HomeLocationData homeLocation;
bool usePos;
int32_t init_stage;
stateEstimation work;
uint32_t ins_last_time;
bool inited;
float dTa;
};
// Private variables
static bool initialized = 0;
// Private functions
static int32_t init13i(stateFilter *self);
static int32_t init13(stateFilter *self);
static int32_t maininit(stateFilter *self);
static int32_t filter(stateFilter *self, stateEstimation *state);
static inline bool invalid_var(float data);
static void globalInit(void);
static void globalInit(void)
{
if (!initialized) {
initialized = 1;
EKFConfigurationInitialize();
EKFStateVarianceInitialize();
HomeLocationInitialize();
}
}
int32_t filterEKF13iInitialize(stateFilter *handle)
{
globalInit();
handle->init = &init13i;
handle->filter = &filter;
handle->localdata = pvPortMalloc(sizeof(struct data));
return STACK_REQUIRED;
}
int32_t filterEKF13Initialize(stateFilter *handle)
{
globalInit();
handle->init = &init13;
handle->filter = &filter;
handle->localdata = pvPortMalloc(sizeof(struct data));
return STACK_REQUIRED;
}
// XXX
// TODO: Until the 16 state EKF is implemented, run 13 state, so compilation runs through
// XXX
int32_t filterEKF16iInitialize(stateFilter *handle)
{
globalInit();
handle->init = &init13i;
handle->filter = &filter;
handle->localdata = pvPortMalloc(sizeof(struct data));
return STACK_REQUIRED;
}
int32_t filterEKF16Initialize(stateFilter *handle)
{
globalInit();
handle->init = &init13;
handle->filter = &filter;
handle->localdata = pvPortMalloc(sizeof(struct data));
return STACK_REQUIRED;
}
static int32_t init13i(stateFilter *self)
{
struct data *this = (struct data *)self->localdata;
this->usePos = 0;
return maininit(self);
}
static int32_t init13(stateFilter *self)
{
struct data *this = (struct data *)self->localdata;
this->usePos = 1;
return maininit(self);
}
static int32_t maininit(stateFilter *self)
{
struct data *this = (struct data *)self->localdata;
this->inited = false;
this->init_stage = 0;
this->work.updated = 0;
this->ins_last_time = PIOS_DELAY_GetRaw();
this->dTa = DT_INIT;
EKFConfigurationGet(&this->ekfConfiguration);
int t;
// plausibility check
for (t = 0; t < EKFCONFIGURATION_P_NUMELEM; t++) {
if (invalid_var(this->ekfConfiguration.P[t])) {
return 2;
}
}
for (t = 0; t < EKFCONFIGURATION_Q_NUMELEM; t++) {
if (invalid_var(this->ekfConfiguration.Q[t])) {
return 2;
}
}
for (t = 0; t < EKFCONFIGURATION_R_NUMELEM; t++) {
if (invalid_var(this->ekfConfiguration.R[t])) {
return 2;
}
}
HomeLocationGet(&this->homeLocation);
// Don't require HomeLocation.Set to be true but at least require a mag configuration (allows easily
// switching between indoor and outdoor mode with Set = false)
if ((this->homeLocation.Be[0] * this->homeLocation.Be[0] + this->homeLocation.Be[1] * this->homeLocation.Be[1] + this->homeLocation.Be[2] * this->homeLocation.Be[2] < 1e-5f)) {
return 2;
}
return 0;
}
/**
* Collect all required state variables, then run complementary filter
*/
static int32_t filter(stateFilter *self, stateEstimation *state)
{
struct data *this = (struct data *)self->localdata;
const float zeros[3] = { 0.0f, 0.0f, 0.0f };
// Perform the update
float dT;
uint16_t sensors = 0;
this->work.updated |= state->updated;
// Get most recent data
IMPORT_SENSOR_IF_UPDATED(gyro, 3);
IMPORT_SENSOR_IF_UPDATED(accel, 3);
IMPORT_SENSOR_IF_UPDATED(mag, 3);
IMPORT_SENSOR_IF_UPDATED(baro, 1);
IMPORT_SENSOR_IF_UPDATED(pos, 3);
IMPORT_SENSOR_IF_UPDATED(vel, 3);
IMPORT_SENSOR_IF_UPDATED(airspeed, 2);
// check whether mandatory updates are present accels must have been supplied already,
// and gyros must be supplied just now for a prediction step to take place
// ("gyros last" rule for multi object synchronization)
if (!(IS_SET(this->work.updated, SENSORUPDATES_accel) && IS_SET(state->updated, SENSORUPDATES_gyro))) {
UNSET_MASK(state->updated, SENSORUPDATES_pos);
UNSET_MASK(state->updated, SENSORUPDATES_vel);
UNSET_MASK(state->updated, SENSORUPDATES_attitude);
UNSET_MASK(state->updated, SENSORUPDATES_gyro);
return 0;
}
dT = PIOS_DELAY_DiffuS(this->ins_last_time) / 1.0e6f;
this->ins_last_time = PIOS_DELAY_GetRaw();
// This should only happen at start up or at mode switches
if (dT > 0.01f) {
dT = 0.01f;
} else if (dT <= 0.001f) {
dT = 0.001f;
}
this->dTa = this->dTa * (1.0f - DT_ALPHA) + dT * DT_ALPHA; // low pass for average dT, compensate timing jitter from scheduler
if (!this->inited && IS_SET(this->work.updated, SENSORUPDATES_mag) && IS_SET(this->work.updated, SENSORUPDATES_baro) && IS_SET(this->work.updated, SENSORUPDATES_pos)) {
// Don't initialize until all sensors are read
if (this->init_stage == 0) {
// Reset the INS algorithm
INSGPSInit();
// variance is measured in mGaus, but internally the EKF works with a normalized vector. Scale down by Be^2
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[EKFCONFIGURATION_R_MAGX] / Be2,
this->ekfConfiguration.R[EKFCONFIGURATION_R_MAGY] / Be2,
this->ekfConfiguration.R[EKFCONFIGURATION_R_MAGZ] / Be2 }
);
INSSetAccelVar((float[3]) { this->ekfConfiguration.Q[EKFCONFIGURATION_Q_ACCELX],
this->ekfConfiguration.Q[EKFCONFIGURATION_Q_ACCELY],
this->ekfConfiguration.Q[EKFCONFIGURATION_Q_ACCELZ] }
);
INSSetGyroVar((float[3]) { this->ekfConfiguration.Q[EKFCONFIGURATION_Q_GYROX],
this->ekfConfiguration.Q[EKFCONFIGURATION_Q_GYROY],
this->ekfConfiguration.Q[EKFCONFIGURATION_Q_GYROZ] }
);
INSSetGyroBiasVar((float[3]) { this->ekfConfiguration.Q[EKFCONFIGURATION_Q_GYRODRIFTX],
this->ekfConfiguration.Q[EKFCONFIGURATION_Q_GYRODRIFTY],
this->ekfConfiguration.Q[EKFCONFIGURATION_Q_GYRODRIFTZ] }
);
INSSetBaroVar(this->ekfConfiguration.R[EKFCONFIGURATION_R_BAROZ]);
// Initialize the gyro bias
float gyro_bias[3] = { 0.0f, 0.0f, 0.0f };
INSSetGyroBias(gyro_bias);
AttitudeStateData attitudeState;
AttitudeStateGet(&attitudeState);
// Set initial attitude. Use accels to determine roll and pitch, rotate magnetic measurement accordingly,
// so pseudo "north" vector can be estimated even if the board is not level
attitudeState.Roll = atan2f(-this->work.accel[1], -this->work.accel[2]);
float zn = cosf(attitudeState.Roll) * this->work.mag[2] + sinf(attitudeState.Roll) * this->work.mag[1];
float yn = cosf(attitudeState.Roll) * this->work.mag[1] - sinf(attitudeState.Roll) * this->work.mag[2];
// rotate accels z vector according to roll
float azn = cosf(attitudeState.Roll) * this->work.accel[2] + sinf(attitudeState.Roll) * this->work.accel[1];
attitudeState.Pitch = atan2f(this->work.accel[0], -azn);
float xn = cosf(attitudeState.Pitch) * this->work.mag[0] + sinf(attitudeState.Pitch) * zn;
attitudeState.Yaw = atan2f(-yn, xn);
// TODO: This is still a hack
// Put this in a proper generic function in CoordinateConversion.c
// should take 4 vectors: g (0,0,-9.81), accels, Be (or 1,0,0 if no home loc) and magnetometers (or 1,0,0 if no mags)
// should calculate the rotation in 3d space using proper cross product math
// SUBTODO: formulate the math required
attitudeState.Roll = RAD2DEG(attitudeState.Roll);
attitudeState.Pitch = RAD2DEG(attitudeState.Pitch);
attitudeState.Yaw = RAD2DEG(attitudeState.Yaw);
RPY2Quaternion(&attitudeState.Roll, this->work.attitude);
INSSetState(this->work.pos, (float *)zeros, this->work.attitude, (float *)zeros, (float *)zeros);
INSResetP(this->ekfConfiguration.P);
} else {
// Run prediction a bit before any corrections
float gyros[3] = { DEG2RAD(this->work.gyro[0]), DEG2RAD(this->work.gyro[1]), DEG2RAD(this->work.gyro[2]) };
INSStatePrediction(gyros, this->work.accel, this->dTa);
// Copy the attitude into the state
// NOTE: updating gyr correctly is valid, because this code is reached only when SENSORUPDATES_gyro is already true
state->attitude[0] = Nav.q[0];
state->attitude[1] = Nav.q[1];
state->attitude[2] = Nav.q[2];
state->attitude[3] = Nav.q[3];
state->gyro[0] -= RAD2DEG(Nav.gyro_bias[0]);
state->gyro[1] -= RAD2DEG(Nav.gyro_bias[1]);
state->gyro[2] -= RAD2DEG(Nav.gyro_bias[2]);
state->pos[0] = Nav.Pos[0];
state->pos[1] = Nav.Pos[1];
state->pos[2] = Nav.Pos[2];
state->vel[0] = Nav.Vel[0];
state->vel[1] = Nav.Vel[1];
state->vel[2] = Nav.Vel[2];
state->updated |= SENSORUPDATES_attitude | SENSORUPDATES_pos | SENSORUPDATES_vel;
}
this->init_stage++;
if (this->init_stage > 10) {
this->inited = true;
}
return 0;
}
if (!this->inited) {
return 1;
}
float gyros[3] = { DEG2RAD(this->work.gyro[0]), DEG2RAD(this->work.gyro[1]), DEG2RAD(this->work.gyro[2]) };
// Advance the state estimate
INSStatePrediction(gyros, this->work.accel, this->dTa);
// Copy the attitude into the state
// NOTE: updating gyr correctly is valid, because this code is reached only when SENSORUPDATES_gyro is already true
state->attitude[0] = Nav.q[0];
state->attitude[1] = Nav.q[1];
state->attitude[2] = Nav.q[2];
state->attitude[3] = Nav.q[3];
state->gyro[0] -= RAD2DEG(Nav.gyro_bias[0]);
state->gyro[1] -= RAD2DEG(Nav.gyro_bias[1]);
state->gyro[2] -= RAD2DEG(Nav.gyro_bias[2]);
state->pos[0] = Nav.Pos[0];
state->pos[1] = Nav.Pos[1];
state->pos[2] = Nav.Pos[2];
state->vel[0] = Nav.Vel[0];
state->vel[1] = Nav.Vel[1];
state->vel[2] = Nav.Vel[2];
state->updated |= SENSORUPDATES_attitude | SENSORUPDATES_pos | SENSORUPDATES_vel;
// Advance the covariance estimate
INSCovariancePrediction(this->dTa);
if (IS_SET(this->work.updated, SENSORUPDATES_mag)) {
sensors |= MAG_SENSORS;
}
if (IS_SET(this->work.updated, SENSORUPDATES_baro)) {
sensors |= BARO_SENSOR;
}
INSSetMagNorth(this->homeLocation.Be);
if (!this->usePos) {
// position and velocity variance used in indoor mode
INSSetPosVelVar((float[3]) { this->ekfConfiguration.FakeR[EKFCONFIGURATION_FAKER_FAKEGPSPOSINDOOR],
this->ekfConfiguration.FakeR[EKFCONFIGURATION_FAKER_FAKEGPSPOSINDOOR],
this->ekfConfiguration.FakeR[EKFCONFIGURATION_FAKER_FAKEGPSPOSINDOOR] },
(float[3]) { this->ekfConfiguration.FakeR[EKFCONFIGURATION_FAKER_FAKEGPSVELINDOOR],
this->ekfConfiguration.FakeR[EKFCONFIGURATION_FAKER_FAKEGPSVELINDOOR],
this->ekfConfiguration.FakeR[EKFCONFIGURATION_FAKER_FAKEGPSVELINDOOR] }
);
} else {
// position and velocity variance used in outdoor mode
INSSetPosVelVar((float[3]) { this->ekfConfiguration.R[EKFCONFIGURATION_R_GPSPOSNORTH],
this->ekfConfiguration.R[EKFCONFIGURATION_R_GPSPOSEAST],
this->ekfConfiguration.R[EKFCONFIGURATION_R_GPSPOSDOWN] },
(float[3]) { this->ekfConfiguration.R[EKFCONFIGURATION_R_GPSVELNORTH],
this->ekfConfiguration.R[EKFCONFIGURATION_R_GPSVELEAST],
this->ekfConfiguration.R[EKFCONFIGURATION_R_GPSVELDOWN] }
);
}
if (IS_SET(this->work.updated, SENSORUPDATES_pos)) {
sensors |= POS_SENSORS;
}
if (IS_SET(this->work.updated, SENSORUPDATES_vel)) {
sensors |= HORIZ_SENSORS | VERT_SENSORS;
}
if (IS_SET(this->work.updated, SENSORUPDATES_airspeed) && ((!IS_SET(this->work.updated, SENSORUPDATES_vel) && !IS_SET(this->work.updated, SENSORUPDATES_pos)) | !this->usePos)) {
// HACK: feed airspeed into EKF as velocity, treat wind as 1e2 variance
sensors |= HORIZ_SENSORS | VERT_SENSORS;
INSSetPosVelVar((float[3]) { this->ekfConfiguration.FakeR[EKFCONFIGURATION_FAKER_FAKEGPSPOSINDOOR],
this->ekfConfiguration.FakeR[EKFCONFIGURATION_FAKER_FAKEGPSPOSINDOOR],
this->ekfConfiguration.FakeR[EKFCONFIGURATION_FAKER_FAKEGPSPOSINDOOR] },
(float[3]) { this->ekfConfiguration.FakeR[EKFCONFIGURATION_FAKER_FAKEGPSVELAIRSPEED],
this->ekfConfiguration.FakeR[EKFCONFIGURATION_FAKER_FAKEGPSVELAIRSPEED],
this->ekfConfiguration.FakeR[EKFCONFIGURATION_FAKER_FAKEGPSVELAIRSPEED] }
);
// rotate airspeed vector into NED frame - airspeed is measured in X axis only
float R[3][3];
Quaternion2R(Nav.q, R);
float vtas[3] = { this->work.airspeed[1], 0.0f, 0.0f };
rot_mult(R, vtas, this->work.vel);
}
/*
* TODO: Need to add a general sanity check for all the inputs to make sure their kosher
* although probably should occur within INS itself
*/
if (sensors) {
INSCorrection(this->work.mag, this->work.pos, this->work.vel, this->work.baro[0], sensors);
}
EKFStateVarianceData vardata;
EKFStateVarianceGet(&vardata);
INSGetP(vardata.P);
EKFStateVarianceSet(&vardata);
int t;
for (t = 0; t < EKFSTATEVARIANCE_P_NUMELEM; t++) {
if (!IS_REAL(vardata.P[t]) || vardata.P[t] <= 0.0f) {
INSResetP(this->ekfConfiguration.P);
this->init_stage = -1;
break;
}
}
// all sensor data has been used, reset!
this->work.updated = 0;
if (this->init_stage < 0) {
return 2;
} else {
return 0;
}
}
// check for invalid variance values
static inline bool invalid_var(float data)
{
if (isnan(data) || isinf(data)) {
return true;
}
if (data < 1e-15f) { // var should not be close to zero. And not negative either.
return true;
}
return false;
}
/**
* @}
* @}
*/

192
flight/modules/StateEstimation/filtermag.c Normal file
View File

@ -0,0 +1,192 @@
/**
******************************************************************************
* @addtogroup OpenPilotModules OpenPilot Modules
* @{
* @addtogroup State Estimation
* @brief Acquires sensor data and computes state estimate
* @{
*
* @file filtermag.c
* @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2013.
* @brief Magnetometer drift compensation, uses previous cycles
* AttitudeState for estimation
*
* @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
*/
#include "inc/stateestimation.h"
#include <attitudestate.h>
#include <revocalibration.h>
#include <homelocation.h>
#include <CoordinateConversions.h>
// Private constants
//
#define STACK_REQUIRED 256
// Private types
struct data {
HomeLocationData homeLocation;
RevoCalibrationData revoCalibration;
float magBias[3];
};
// Private variables
// Private functions
static int32_t init(stateFilter *self);
static int32_t filter(stateFilter *self, stateEstimation *state);
static void magOffsetEstimation(struct data *this, float mag[3]);
int32_t filterMagInitialize(stateFilter *handle)
{
handle->init = &init;
handle->filter = &filter;
handle->localdata = pvPortMalloc(sizeof(struct data));
HomeLocationInitialize();
return STACK_REQUIRED;
}
static int32_t init(stateFilter *self)
{
struct data *this = (struct data *)self->localdata;
this->magBias[0] = this->magBias[1] = this->magBias[2] = 0.0f;
HomeLocationGet(&this->homeLocation);
RevoCalibrationGet(&this->revoCalibration);
return 0;
}
static int32_t filter(stateFilter *self, stateEstimation *state)
{
struct data *this = (struct data *)self->localdata;
if (IS_SET(state->updated, SENSORUPDATES_mag)) {
if (this->revoCalibration.MagBiasNullingRate > 0) {
magOffsetEstimation(this, state->mag);
}
}
return 0;
}
/**
* Perform an update of the @ref MagBias based on
* Magmeter Offset Cancellation: Theory and Implementation,
* revisited William Premerlani, October 14, 2011
*/
static void magOffsetEstimation(struct data *this, float mag[3])
{
#if 0
// Constants, to possibly go into a UAVO
static const float MIN_NORM_DIFFERENCE = 50;
static float B2[3] = { 0, 0, 0 };
MagBiasData magBias;
MagBiasGet(&magBias);
// Remove the current estimate of the bias
mag->x -= magBias.x;
mag->y -= magBias.y;
mag->z -= magBias.z;
// First call
if (B2[0] == 0 && B2[1] == 0 && B2[2] == 0) {
B2[0] = mag->x;
B2[1] = mag->y;
B2[2] = mag->z;
return;
}
float B1[3] = { mag->x, mag->y, mag->z };
float norm_diff = sqrtf(powf(B2[0] - B1[0], 2) + powf(B2[1] - B1[1], 2) + powf(B2[2] - B1[2], 2));
if (norm_diff > MIN_NORM_DIFFERENCE) {
float norm_b1 = sqrtf(B1[0] * B1[0] + B1[1] * B1[1] + B1[2] * B1[2]);
float norm_b2 = sqrtf(B2[0] * B2[0] + B2[1] * B2[1] + B2[2] * B2[2]);
float scale = cal.MagBiasNullingRate * (norm_b2 - norm_b1) / norm_diff;
float b_error[3] = { (B2[0] - B1[0]) * scale, (B2[1] - B1[1]) * scale, (B2[2] - B1[2]) * scale };
magBias.x += b_error[0];
magBias.y += b_error[1];
magBias.z += b_error[2];
MagBiasSet(&magBias);
// Store this value to compare against next update
B2[0] = B1[0]; B2[1] = B1[1]; B2[2] = B1[2];
}
#else // if 0
const float Rxy = sqrtf(this->homeLocation.Be[0] * this->homeLocation.Be[0] + this->homeLocation.Be[1] * this->homeLocation.Be[1]);
const float Rz = this->homeLocation.Be[2];
const float rate = this->revoCalibration.MagBiasNullingRate;
float Rot[3][3];
float B_e[3];
float xy[2];
float delta[3];
AttitudeStateData attitude;
AttitudeStateGet(&attitude);
// Get the rotation matrix
Quaternion2R(&attitude.q1, Rot);
// Rotate the mag into the NED frame
B_e[0] = Rot[0][0] * mag[0] + Rot[1][0] * mag[1] + Rot[2][0] * mag[2];
B_e[1] = Rot[0][1] * mag[0] + Rot[1][1] * mag[1] + Rot[2][1] * mag[2];
B_e[2] = Rot[0][2] * mag[0] + Rot[1][2] * mag[1] + Rot[2][2] * mag[2];
float cy = cosf(DEG2RAD(attitude.Yaw));
float sy = sinf(DEG2RAD(attitude.Yaw));
xy[0] = cy * B_e[0] + sy * B_e[1];
xy[1] = -sy * B_e[0] + cy * B_e[1];
float xy_norm = sqrtf(xy[0] * xy[0] + xy[1] * xy[1]);
delta[0] = -rate * (xy[0] / xy_norm * Rxy - xy[0]);
delta[1] = -rate * (xy[1] / xy_norm * Rxy - xy[1]);
delta[2] = -rate * (Rz - B_e[2]);
if (!isnan(delta[0]) && !isinf(delta[0]) &&
!isnan(delta[1]) && !isinf(delta[1]) &&
!isnan(delta[2]) && !isinf(delta[2])) {
this->magBias[0] += delta[0];
this->magBias[1] += delta[1];
this->magBias[2] += delta[2];
}
// Add bias to state estimation
mag[0] += this->magBias[0];
mag[1] += this->magBias[1];
mag[2] += this->magBias[2];
#endif // if 0
}
/**
* @}
* @}
*/

79
flight/modules/StateEstimation/filterstationary.c Normal file
View File

@ -0,0 +1,79 @@
/**
******************************************************************************
* @addtogroup OpenPilotModules OpenPilot Modules
* @{
* @addtogroup State Estimation
* @brief Acquires sensor data and computes state estimate
* @{
*
* @file filterstationary.c
* @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2013.
* @brief Provides "fake" stationary state data for indoor mode
*
* @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
*/
#include "inc/stateestimation.h"
// Private constants
#define STACK_REQUIRED 64
// Private types
// Private variables
// Private functions
static int32_t init(stateFilter *self);
static int32_t filter(stateFilter *self, stateEstimation *state);
int32_t filterStationaryInitialize(stateFilter *handle)
{
handle->init = &init;
handle->filter = &filter;
handle->localdata = NULL;
return STACK_REQUIRED;
}
static int32_t init(__attribute__((unused)) stateFilter *self)
{
return 0;
}
static int32_t filter(__attribute__((unused)) stateFilter *self, stateEstimation *state)
{
state->pos[0] = 0.0f;
state->pos[1] = 0.0f;
state->pos[2] = 0.0f;
state->updated |= SENSORUPDATES_pos;
state->vel[0] = 0.0f;
state->vel[1] = 0.0f;
state->vel[2] = 0.0f;
state->updated |= SENSORUPDATES_vel;
return 0;
}
/**
* @}
* @}
*/

76
flight/modules/StateEstimation/inc/stateestimation.h Normal file
View File

@ -0,0 +1,76 @@
/**
******************************************************************************
* @addtogroup OpenPilotModules OpenPilot Modules
* @{
* @addtogroup StateSetimation Module
* @{
*
* @file stateestimation.h
* @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2011.
* @brief Acquires sensor data and fuses it into attitude estimate for CC
*
* @see The GNU Public License (GPL) Version 3
*
*****************************************************************************/
/*
* This program is free software; you can redistribute it and/or modify
* it under the terms of the GNU General Public License as published by
* the Free Software Foundation; either version 3 of the License, or
* (at your option) any later version.
*
* This program is distributed in the hope that it will be useful, but
* WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY
* or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License
* for more details.
*
* You should have received a copy of the GNU General Public License along
* with this program; if not, write to the Free Software Foundation, Inc.,
* 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA
*/
#ifndef STATEESTIMATION_H
#define STATEESTIMATION_H
#include <openpilot.h>
typedef enum {
SENSORUPDATES_gyro = 1 << 0,
SENSORUPDATES_accel = 1 << 1,
SENSORUPDATES_mag = 1 << 2,
SENSORUPDATES_attitude = 1 << 3,
SENSORUPDATES_pos = 1 << 4,
SENSORUPDATES_vel = 1 << 5,
SENSORUPDATES_airspeed = 1 << 6,
SENSORUPDATES_baro = 1 << 7,
} sensorUpdates;
typedef struct {
float gyro[3];
float accel[3];
float mag[3];
float attitude[4];
float pos[3];
float vel[3];
float airspeed[2];
float baro[1];
sensorUpdates updated;
} stateEstimation;
typedef struct stateFilterStruct {
int32_t (*init)(struct stateFilterStruct *self);
int32_t (*filter)(struct stateFilterStruct *self, stateEstimation *state);
void *localdata;
} stateFilter;
int32_t filterMagInitialize(stateFilter *handle);
int32_t filterBaroInitialize(stateFilter *handle);
int32_t filterAirInitialize(stateFilter *handle);
int32_t filterStationaryInitialize(stateFilter *handle);
int32_t filterCFInitialize(stateFilter *handle);
int32_t filterCFMInitialize(stateFilter *handle);
int32_t filterEKF13iInitialize(stateFilter *handle);
int32_t filterEKF13Initialize(stateFilter *handle);
int32_t filterEKF16iInitialize(stateFilter *handle);
int32_t filterEKF16Initialize(stateFilter *handle);
#endif // STATEESTIMATION_H

501
flight/modules/StateEstimation/stateestimation.c Normal file
View File

@ -0,0 +1,501 @@
/**
******************************************************************************
* @addtogroup OpenPilotModules OpenPilot Modules
* @{
* @addtogroup State Estimation
* @brief Acquires sensor data and computes state estimate
* @{
*
* @file stateestimation.c
* @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2013.
* @brief Module to handle all comms to the AHRS on a periodic basis.
*
* @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
*/
#include "inc/stateestimation.h"
#include <gyrosensor.h>
#include <accelsensor.h>
#include <magsensor.h>
#include <barosensor.h>
#include <airspeedsensor.h>
#include <positionsensor.h>
#include <gpsvelocitysensor.h>
#include <gyrostate.h>
#include <accelstate.h>
#include <magstate.h>
#include <airspeedstate.h>
#include <attitudestate.h>
#include <positionstate.h>
#include <velocitystate.h>
#include "revosettings.h"
#include "flightstatus.h"
#include "CoordinateConversions.h"
// Private constants
#define STACK_SIZE_BYTES 256
#define CALLBACK_PRIORITY CALLBACK_PRIORITY_REGULAR
#define TASK_PRIORITY CALLBACK_TASK_FLIGHTCONTROL
#define TIMEOUT_MS 10
// local macros, ONLY to be used in the middle of StateEstimationCb in section RUNSTATE_LOAD after the update of states updated!
#define FETCH_SENSOR_FROM_UAVOBJECT_CHECK_AND_LOAD_TO_STATE_3_DIMENSIONS(sensorname, shortname, a1, a2, a3) \
if (IS_SET(states.updated, SENSORUPDATES_##shortname)) { \
sensorname##Data s; \
sensorname##Get(&s); \
if (IS_REAL(s.a1) && IS_REAL(s.a2) && IS_REAL(s.a3)) { \
states.shortname[0] = s.a1; \
states.shortname[1] = s.a2; \
states.shortname[2] = s.a3; \
} \
else { \
UNSET_MASK(states.updated, SENSORUPDATES_##shortname); \
} \
}
#define FETCH_SENSOR_FROM_UAVOBJECT_CHECK_AND_LOAD_TO_STATE_1_DIMENSION_WITH_CUSTOM_EXTRA_CHECK(sensorname, shortname, a1, EXTRACHECK) \
if (IS_SET(states.updated, SENSORUPDATES_##shortname)) { \
sensorname##Data s; \
sensorname##Get(&s); \
if (IS_REAL(s.a1) && EXTRACHECK) { \
states.shortname[0] = s.a1; \
} \
else { \
UNSET_MASK(states.updated, SENSORUPDATES_##shortname); \
} \
}
// local macros, ONLY to be used in the middle of StateEstimationCb in section RUNSTATE_SAVE before the check of alarms!
#define EXPORT_STATE_TO_UAVOBJECT_IF_UPDATED_3_DIMENSIONS(statename, shortname, a1, a2, a3) \
if (IS_SET(states.updated, SENSORUPDATES_##shortname)) { \
statename##Data s; \
statename##Get(&s); \
s.a1 = states.shortname[0]; \
s.a2 = states.shortname[1]; \
s.a3 = states.shortname[2]; \
statename##Set(&s); \
}
#define EXPORT_STATE_TO_UAVOBJECT_IF_UPDATED_2_DIMENSIONS(statename, shortname, a1, a2) \
if (IS_SET(states.updated, SENSORUPDATES_##shortname)) { \
statename##Data s; \
statename##Get(&s); \
s.a1 = states.shortname[0]; \
s.a2 = states.shortname[1]; \
statename##Set(&s); \
}
// Private types
struct filterPipelineStruct;
typedef const struct filterPipelineStruct {
const stateFilter *filter;
const struct filterPipelineStruct *next;
} filterPipeline;
// Private variables
static DelayedCallbackInfo *stateEstimationCallback;
static volatile RevoSettingsData revoSettings;
static volatile sensorUpdates updatedSensors;
static int32_t fusionAlgorithm = -1;
static filterPipeline *filterChain = NULL;
// different filters available to state estimation
static stateFilter magFilter;
static stateFilter baroFilter;
static stateFilter airFilter;
static stateFilter stationaryFilter;
static stateFilter cfFilter;
static stateFilter cfmFilter;
static stateFilter ekf13iFilter;
static stateFilter ekf13Filter;
// preconfigured filter chains selectable via revoSettings.FusionAlgorithm
static filterPipeline *cfQueue = &(filterPipeline) {
.filter = &magFilter,
.next = &(filterPipeline) {
.filter = &airFilter,
.next = &(filterPipeline) {
.filter = &baroFilter,
.next = &(filterPipeline) {
.filter = &cfFilter,
.next = NULL,
},
}
}
};
static const filterPipeline *cfmQueue = &(filterPipeline) {
.filter = &magFilter,
.next = &(filterPipeline) {
.filter = &airFilter,
.next = &(filterPipeline) {
.filter = &baroFilter,
.next = &(filterPipeline) {
.filter = &cfmFilter,
.next = NULL,
}
}
}
};
static const filterPipeline *ekf13iQueue = &(filterPipeline) {
.filter = &magFilter,
.next = &(filterPipeline) {
.filter = &airFilter,
.next = &(filterPipeline) {
.filter = &baroFilter,
.next = &(filterPipeline) {
.filter = &stationaryFilter,
.next = &(filterPipeline) {
.filter = &ekf13iFilter,
.next = NULL,
}
}
}
}
};
static const filterPipeline *ekf13Queue = &(filterPipeline) {
.filter = &magFilter,
.next = &(filterPipeline) {
.filter = &airFilter,
.next = &(filterPipeline) {
.filter = &baroFilter,
.next = &(filterPipeline) {
.filter = &ekf13Filter,
.next = NULL,
}
}
}
};
// Private functions
static void settingsUpdatedCb(UAVObjEvent *objEv);
static void sensorUpdatedCb(UAVObjEvent *objEv);
static void StateEstimationCb(void);
static inline int32_t maxint32_t(int32_t a, int32_t b)
{
if (a > b) {
return a;
}
return b;
}
/**
* Initialise the module. Called before the start function
* \returns 0 on success or -1 if initialisation failed
*/
int32_t StateEstimationInitialize(void)
{
RevoSettingsInitialize();
GyroSensorInitialize();
MagSensorInitialize();
BaroSensorInitialize();
AirspeedSensorInitialize();
PositionSensorInitialize();
GPSVelocitySensorInitialize();
GyroStateInitialize();
AccelStateInitialize();
MagStateInitialize();
AirspeedStateInitialize();
PositionStateInitialize();
VelocityStateInitialize();
RevoSettingsConnectCallback(&settingsUpdatedCb);
GyroSensorConnectCallback(&sensorUpdatedCb);
AccelSensorConnectCallback(&sensorUpdatedCb);
MagSensorConnectCallback(&sensorUpdatedCb);
BaroSensorConnectCallback(&sensorUpdatedCb);
AirspeedSensorConnectCallback(&sensorUpdatedCb);
PositionSensorConnectCallback(&sensorUpdatedCb);
GPSVelocitySensorConnectCallback(&sensorUpdatedCb);
uint32_t stack_required = STACK_SIZE_BYTES;
// Initialize Filters
stack_required = maxint32_t(stack_required, filterMagInitialize(&magFilter));
stack_required = maxint32_t(stack_required, filterBaroInitialize(&baroFilter));
stack_required = maxint32_t(stack_required, filterAirInitialize(&airFilter));
stack_required = maxint32_t(stack_required, filterStationaryInitialize(&stationaryFilter));
stack_required = maxint32_t(stack_required, filterCFInitialize(&cfFilter));
stack_required = maxint32_t(stack_required, filterCFMInitialize(&cfmFilter));
stack_required = maxint32_t(stack_required, filterEKF13iInitialize(&ekf13iFilter));
stack_required = maxint32_t(stack_required, filterEKF13Initialize(&ekf13Filter));
stateEstimationCallback = DelayedCallbackCreate(&StateEstimationCb, CALLBACK_PRIORITY, TASK_PRIORITY, stack_required);
return 0;
}
/**
* Start the task. Expects all objects to be initialized by this point.
* \returns 0 on success or -1 if initialisation failed
*/
int32_t StateEstimationStart(void)
{
RevoSettingsConnectCallback(&settingsUpdatedCb);
// Force settings update to make sure rotation loaded
settingsUpdatedCb(NULL);
return 0;
}
MODULE_INITCALL(StateEstimationInitialize, StateEstimationStart);
/**
* Module callback
*/
static void StateEstimationCb(void)
{
static enum { RUNSTATE_LOAD = 0, RUNSTATE_FILTER = 1, RUNSTATE_SAVE = 2 } runState = RUNSTATE_LOAD;
static int8_t alarm = 0;
static int8_t lastAlarm = -1;
static uint16_t alarmcounter = 0;
static filterPipeline *current;
static stateEstimation states;
static uint32_t last_time;
static uint16_t bootDelay = 64;
// after system startup, first few sensor readings might be messed up, delay until everything has settled
if (bootDelay) {
bootDelay--;
DelayedCallbackSchedule(stateEstimationCallback, TIMEOUT_MS, CALLBACK_UPDATEMODE_SOONER);
return;
}
switch (runState) {
case RUNSTATE_LOAD:
alarm = 0;
// set alarm to warning if called through timeout
if (updatedSensors == 0) {
if (PIOS_DELAY_DiffuS(last_time) > 1000 * TIMEOUT_MS) {
alarm = 1;
}
} else {
last_time = PIOS_DELAY_GetRaw();
}
// check if a new filter chain should be initialized
if (fusionAlgorithm != revoSettings.FusionAlgorithm) {
FlightStatusData fs;
FlightStatusGet(&fs);
if (fs.Armed == FLIGHTSTATUS_ARMED_DISARMED || fusionAlgorithm == -1) {
const filterPipeline *newFilterChain;
switch (revoSettings.FusionAlgorithm) {
case REVOSETTINGS_FUSIONALGORITHM_COMPLEMENTARY:
newFilterChain = cfQueue;
break;
case REVOSETTINGS_FUSIONALGORITHM_COMPLEMENTARYMAG:
newFilterChain = cfmQueue;
break;
case REVOSETTINGS_FUSIONALGORITHM_INS13INDOOR:
newFilterChain = ekf13iQueue;
break;
case REVOSETTINGS_FUSIONALGORITHM_INS13OUTDOOR:
newFilterChain = ekf13Queue;
break;
default:
newFilterChain = NULL;
}
// initialize filters in chain
current = (filterPipeline *)newFilterChain;
bool error = 0;
while (current != NULL) {
int32_t result = current->filter->init((stateFilter *)current->filter);
if (result != 0) {
error = 1;
break;
}
current = current->next;
}
if (error) {
AlarmsSet(SYSTEMALARMS_ALARM_ATTITUDE, SYSTEMALARMS_ALARM_ERROR);
return;
} else {
// set new fusion algortithm
filterChain = (filterPipeline *)newFilterChain;
fusionAlgorithm = revoSettings.FusionAlgorithm;
}
}
}
// read updated sensor UAVObjects and set initial state
states.updated = updatedSensors;
updatedSensors = 0;
// fetch sensors, check values, and load into state struct
FETCH_SENSOR_FROM_UAVOBJECT_CHECK_AND_LOAD_TO_STATE_3_DIMENSIONS(GyroSensor, gyro, x, y, z);
FETCH_SENSOR_FROM_UAVOBJECT_CHECK_AND_LOAD_TO_STATE_3_DIMENSIONS(AccelSensor, accel, x, y, z);
FETCH_SENSOR_FROM_UAVOBJECT_CHECK_AND_LOAD_TO_STATE_3_DIMENSIONS(MagSensor, mag, x, y, z);
FETCH_SENSOR_FROM_UAVOBJECT_CHECK_AND_LOAD_TO_STATE_3_DIMENSIONS(PositionSensor, pos, North, East, Down);
FETCH_SENSOR_FROM_UAVOBJECT_CHECK_AND_LOAD_TO_STATE_3_DIMENSIONS(GPSVelocitySensor, vel, North, East, Down);
FETCH_SENSOR_FROM_UAVOBJECT_CHECK_AND_LOAD_TO_STATE_1_DIMENSION_WITH_CUSTOM_EXTRA_CHECK(BaroSensor, baro, Altitude, true);
FETCH_SENSOR_FROM_UAVOBJECT_CHECK_AND_LOAD_TO_STATE_1_DIMENSION_WITH_CUSTOM_EXTRA_CHECK(AirspeedSensor, airspeed, CalibratedAirspeed, s.SensorConnected == AIRSPEEDSENSOR_SENSORCONNECTED_TRUE);
states.airspeed[1] = 0.0f; // sensor does not provide true airspeed, needs to be calculated by filter, set to zero for now
// at this point sensor state is stored in "states" with some rudimentary filtering applied
// apply all filters in the current filter chain
current = (filterPipeline *)filterChain;
// we are not done, re-dispatch self execution
runState = RUNSTATE_FILTER;
DelayedCallbackDispatch(stateEstimationCallback);
break;
case RUNSTATE_FILTER:
if (current != NULL) {
int32_t result = current->filter->filter((stateFilter *)current->filter, &states);
if (result > alarm) {
alarm = result;
}
current = current->next;
}
// we are not done, re-dispatch self execution
if (!current) {
runState = RUNSTATE_SAVE;
}
DelayedCallbackDispatch(stateEstimationCallback);
break;
case RUNSTATE_SAVE:
// the final output of filters is saved in state variables
EXPORT_STATE_TO_UAVOBJECT_IF_UPDATED_3_DIMENSIONS(GyroState, gyro, x, y, z);
EXPORT_STATE_TO_UAVOBJECT_IF_UPDATED_3_DIMENSIONS(AccelState, accel, x, y, z);
EXPORT_STATE_TO_UAVOBJECT_IF_UPDATED_3_DIMENSIONS(MagState, mag, x, y, z);
EXPORT_STATE_TO_UAVOBJECT_IF_UPDATED_3_DIMENSIONS(PositionState, pos, North, East, Down);
EXPORT_STATE_TO_UAVOBJECT_IF_UPDATED_3_DIMENSIONS(VelocityState, vel, North, East, Down);
EXPORT_STATE_TO_UAVOBJECT_IF_UPDATED_2_DIMENSIONS(AirspeedState, airspeed, CalibratedAirspeed, TrueAirspeed);
// attitude nees manual conversion from quaternion to euler
if (IS_SET(states.updated, SENSORUPDATES_attitude)) { \
AttitudeStateData s;
AttitudeStateGet(&s);
s.q1 = states.attitude[0];
s.q2 = states.attitude[1];
s.q3 = states.attitude[2];
s.q4 = states.attitude[3];
Quaternion2RPY(&s.q1, &s.Roll);
AttitudeStateSet(&s);
}
// throttle alarms, raise alarm flags immediately
// but require system to run for a while before decreasing
// to prevent alarm flapping
if (alarm >= lastAlarm) {
lastAlarm = alarm;
alarmcounter = 0;
} else {
if (alarmcounter < 100) {
alarmcounter++;
} else {
lastAlarm = alarm;
alarmcounter = 0;
}
}
// clear alarms if everything is alright, then schedule callback execution after timeout
if (lastAlarm == 1) {
AlarmsSet(SYSTEMALARMS_ALARM_ATTITUDE, SYSTEMALARMS_ALARM_WARNING);
} else if (lastAlarm == 2) {
AlarmsSet(SYSTEMALARMS_ALARM_ATTITUDE, SYSTEMALARMS_ALARM_ERROR);
} else if (lastAlarm >= 3) {
AlarmsSet(SYSTEMALARMS_ALARM_ATTITUDE, SYSTEMALARMS_ALARM_CRITICAL);
} else {
AlarmsClear(SYSTEMALARMS_ALARM_ATTITUDE);
}
// we are done, re-schedule next self execution
runState = RUNSTATE_LOAD;
if (updatedSensors) {
DelayedCallbackDispatch(stateEstimationCallback);
} else {
DelayedCallbackSchedule(stateEstimationCallback, TIMEOUT_MS, CALLBACK_UPDATEMODE_SOONER);
}
break;
}
}
/**
* Callback for eventdispatcher when RevoSettings has been updated
*/
static void settingsUpdatedCb(__attribute__((unused)) UAVObjEvent *ev)
{
RevoSettingsGet((RevoSettingsData *)&revoSettings);
}
/**
* Callback for eventdispatcher when any sensor UAVObject has been updated
* updates the list of "recently updated UAVObjects" and dispatches the state estimator callback
*/
static void sensorUpdatedCb(UAVObjEvent *ev)
{
if (!ev) {
return;
}
if (ev->obj == GyroSensorHandle()) {
updatedSensors |= SENSORUPDATES_gyro;
}
if (ev->obj == AccelSensorHandle()) {
updatedSensors |= SENSORUPDATES_accel;
}
if (ev->obj == MagSensorHandle()) {
updatedSensors |= SENSORUPDATES_mag;
}
if (ev->obj == PositionSensorHandle()) {
updatedSensors |= SENSORUPDATES_pos;
}
if (ev->obj == GPSVelocitySensorHandle()) {
updatedSensors |= SENSORUPDATES_vel;
}
if (ev->obj == BaroSensorHandle()) {
updatedSensors |= SENSORUPDATES_baro;
}
if (ev->obj == AirspeedSensorHandle()) {
updatedSensors |= SENSORUPDATES_airspeed;
}
DelayedCallbackDispatch(stateEstimationCallback);
}
/**
* @}
* @}
*/

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

@ -3,7 +3,7 @@
*
* @file vtolpathfollower.c
* @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2012.
* @brief This module compared @ref PositionActual to @ref PathDesired
* @brief This module compared @ref PositionState to @ref PathDesired
* and sets @ref Stabilization. It only does this when the FlightMode field
* of @ref FlightStatus is PathPlanner or RTH.
*
@ -29,11 +29,11 @@
/**
* Input object: FlightStatus
* Input object: PathDesired
* Input object: PositionActual
* Input object: PositionState
* Output object: StabilizationDesired
*
* This module will periodically update the value of the @ref StabilizationDesired object based on
* @ref PathDesired and @PositionActual when the Flight Mode selected in @FlightStatus is supported
* @ref PathDesired and @PositionState when the Flight Mode selected in @FlightStatus is supported
* by this module. Otherwise another module (e.g. @ref ManualControlCommand) is expected to be
* writing to @ref StabilizationDesired.
*
@ -50,16 +50,16 @@
#include "vtolpathfollower.h"
#include "accels.h"
#include "attitudeactual.h"
#include "accelstate.h"
#include "attitudestate.h"
#include "hwsettings.h"
#include "pathdesired.h" // object that will be updated by the module
#include "positionactual.h"
#include "positionstate.h"
#include "manualcontrol.h"
#include "flightstatus.h"
#include "pathstatus.h"
#include "gpsvelocity.h"
#include "gpsposition.h"
#include "gpsvelocitysensor.h"
#include "gpspositionsensor.h"
#include "homelocation.h"
#include "vtolpathfollowersettings.h"
#include "nedaccel.h"
@ -67,7 +67,7 @@
#include "stabilizationsettings.h"
#include "systemsettings.h"
#include "velocitydesired.h"
#include "velocityactual.h"
#include "velocitystate.h"
#include "taskinfo.h"
#include "paths.h"
@ -286,8 +286,8 @@ static void updatePOIBearing()
PathDesiredData pathDesired;
PathDesiredGet(&pathDesired);
PositionActualData positionActual;
PositionActualGet(&positionActual);
PositionStateData positionState;
PositionStateGet(&positionState);
CameraDesiredData cameraDesired;
CameraDesiredGet(&cameraDesired);
StabilizationDesiredData stabDesired;
@ -299,9 +299,9 @@ static void updatePOIBearing()
float yaw = 0;
/*float elevation = 0;*/
dLoc[0] = positionActual.North - poi.North;
dLoc[1] = positionActual.East - poi.East;
dLoc[2] = positionActual.Down - poi.Down;
dLoc[0] = positionState.North - poi.North;
dLoc[1] = positionState.East - poi.East;
dLoc[2] = positionState.Down - poi.Down;
if (dLoc[1] < 0) {
yaw = RAD2DEG(atan2f(dLoc[1], dLoc[0])) + 180.0f;
@ -364,7 +364,7 @@ static void updatePOIBearing()
/**
* Compute desired velocity from the current position and path
*
* Takes in @ref PositionActual and compares it to @ref PathDesired
* Takes in @ref PositionState and compares it to @ref PathDesired
* and computes @ref VelocityDesired
*/
static void updatePathVelocity()
@ -375,11 +375,11 @@ static void updatePathVelocity()
PathDesiredData pathDesired;
PathDesiredGet(&pathDesired);
PositionActualData positionActual;
PositionActualGet(&positionActual);
PositionStateData positionState;
PositionStateGet(&positionState);
float cur[3] =
{ positionActual.North, positionActual.East, positionActual.Down };
{ positionState.North, positionState.East, positionState.Down };
struct path_status progress;
path_progress(pathDesired.Start, pathDesired.End, cur, &progress, pathDesired.Mode);
@ -429,7 +429,7 @@ static void updatePathVelocity()
float altitudeSetpoint = pathDesired.Start[2] + (pathDesired.End[2] - pathDesired.Start[2]) * bound(progress.fractional_progress, 0, 1);
float downError = altitudeSetpoint - positionActual.Down;
float downError = altitudeSetpoint - positionState.Down;
downPosIntegral = bound(downPosIntegral + downError * dT * vtolpathfollowerSettings.VerticalPosPI[VTOLPATHFOLLOWERSETTINGS_VERTICALPOSPI_KI],
-vtolpathfollowerSettings.VerticalPosPI[VTOLPATHFOLLOWERSETTINGS_VERTICALPOSPI_ILIMIT],
vtolpathfollowerSettings.VerticalPosPI[VTOLPATHFOLLOWERSETTINGS_VERTICALPOSPI_ILIMIT]);
@ -446,7 +446,7 @@ static void updatePathVelocity()
/**
* Compute desired velocity from the current position
*
* Takes in @ref PositionActual and compares it to @ref PositionDesired
* Takes in @ref PositionState and compares it to @ref PositionDesired
* and computes @ref VelocityDesired
*/
void updateEndpointVelocity()
@ -457,10 +457,10 @@ void updateEndpointVelocity()
PathDesiredGet(&pathDesired);
PositionActualData positionActual;
PositionStateData positionState;
VelocityDesiredData velocityDesired;
PositionActualGet(&positionActual);
PositionStateGet(&positionState);
VelocityDesiredGet(&velocityDesired);
float northError;
@ -473,16 +473,16 @@ void updateEndpointVelocity()
float northPos = 0, eastPos = 0, downPos = 0;
switch (vtolpathfollowerSettings.PositionSource) {
case VTOLPATHFOLLOWERSETTINGS_POSITIONSOURCE_EKF:
northPos = positionActual.North;
eastPos = positionActual.East;
downPos = positionActual.Down;
northPos = positionState.North;
eastPos = positionState.East;
downPos = positionState.Down;
break;
case VTOLPATHFOLLOWERSETTINGS_POSITIONSOURCE_GPSPOS:
{
// this used to work with the NEDposition UAVObject
// however this UAVObject has been removed
GPSPositionData gpsPosition;
GPSPositionGet(&gpsPosition);
GPSPositionSensorData gpsPosition;
GPSPositionSensorGet(&gpsPosition);
HomeLocationData homeLocation;
HomeLocationGet(&homeLocation);
float lat = DEG2RAD(homeLocation.Latitude / 10.0e6f);
@ -559,18 +559,18 @@ static void updateFixedAttitude(float *attitude)
/**
* Compute desired attitude from the desired velocity
*
* Takes in @ref NedActual which has the acceleration in the
* Takes in @ref NedState which has the acceleration in the
* NED frame as the feedback term and then compares the
* @ref VelocityActual against the @ref VelocityDesired
* @ref VelocityState against the @ref VelocityDesired
*/
static void updateVtolDesiredAttitude(bool yaw_attitude)
{
float dT = vtolpathfollowerSettings.UpdatePeriod / 1000.0f;
VelocityDesiredData velocityDesired;
VelocityActualData velocityActual;
VelocityStateData velocityState;
StabilizationDesiredData stabDesired;
AttitudeActualData attitudeActual;
AttitudeStateData attitudeState;
NedAccelData nedAccel;
StabilizationSettingsData stabSettings;
SystemSettingsData systemSettings;
@ -585,25 +585,25 @@ static void updateVtolDesiredAttitude(bool yaw_attitude)
float downCommand;
SystemSettingsGet(&systemSettings);
VelocityActualGet(&velocityActual);
VelocityStateGet(&velocityState);
VelocityDesiredGet(&velocityDesired);
StabilizationDesiredGet(&stabDesired);
VelocityDesiredGet(&velocityDesired);
AttitudeActualGet(&attitudeActual);
AttitudeStateGet(&attitudeState);
StabilizationSettingsGet(&stabSettings);
NedAccelGet(&nedAccel);
float northVel = 0, eastVel = 0, downVel = 0;
switch (vtolpathfollowerSettings.VelocitySource) {
case VTOLPATHFOLLOWERSETTINGS_VELOCITYSOURCE_EKF:
northVel = velocityActual.North;
eastVel = velocityActual.East;
downVel = velocityActual.Down;
northVel = velocityState.North;
eastVel = velocityState.East;
downVel = velocityState.Down;
break;
case VTOLPATHFOLLOWERSETTINGS_VELOCITYSOURCE_NEDVEL:
{
GPSVelocityData gpsVelocity;
GPSVelocityGet(&gpsVelocity);
GPSVelocitySensorData gpsVelocity;
GPSVelocitySensorGet(&gpsVelocity);
northVel = gpsVelocity.North;
eastVel = gpsVelocity.East;
downVel = gpsVelocity.Down;
@ -611,11 +611,11 @@ static void updateVtolDesiredAttitude(bool yaw_attitude)
break;
case VTOLPATHFOLLOWERSETTINGS_VELOCITYSOURCE_GPSPOS:
{
GPSPositionData gpsPosition;
GPSPositionGet(&gpsPosition);
GPSPositionSensorData gpsPosition;
GPSPositionSensorGet(&gpsPosition);
northVel = gpsPosition.Groundspeed * cosf(DEG2RAD(gpsPosition.Heading));
eastVel = gpsPosition.Groundspeed * sinf(DEG2RAD(gpsPosition.Heading));
downVel = velocityActual.Down;
downVel = velocityState.Down;
}
break;
default:
@ -659,11 +659,11 @@ static void updateVtolDesiredAttitude(bool yaw_attitude)
// Project the north and east command signals into the pitch and roll based on yaw. For this to behave well the
// craft should move similarly for 5 deg roll versus 5 deg pitch
stabDesired.Pitch = bound(-northCommand * cosf(DEG2RAD(attitudeActual.Yaw)) +
-eastCommand * sinf(DEG2RAD(attitudeActual.Yaw)),
stabDesired.Pitch = bound(-northCommand * cosf(DEG2RAD(attitudeState.Yaw)) +
-eastCommand * sinf(DEG2RAD(attitudeState.Yaw)),
-vtolpathfollowerSettings.MaxRollPitch, vtolpathfollowerSettings.MaxRollPitch);
stabDesired.Roll = bound(-northCommand * sinf(DEG2RAD(attitudeActual.Yaw)) +
eastCommand * cosf(DEG2RAD(attitudeActual.Yaw)),
stabDesired.Roll = bound(-northCommand * sinf(DEG2RAD(attitudeState.Yaw)) +
eastCommand * cosf(DEG2RAD(attitudeState.Yaw)),
-vtolpathfollowerSettings.MaxRollPitch, vtolpathfollowerSettings.MaxRollPitch);
if (vtolpathfollowerSettings.ThrottleControl == VTOLPATHFOLLOWERSETTINGS_THROTTLECONTROL_FALSE) {
@ -695,20 +695,20 @@ static void updateNedAccel()
float accel_ned[3];
// Collect downsampled attitude data
AccelsData accels;
AccelStateData accelState;
AccelsGet(&accels);
accel[0] = accels.x;
accel[1] = accels.y;
accel[2] = accels.z;
AccelStateGet(&accelState);
accel[0] = accelState.x;
accel[1] = accelState.y;
accel[2] = accelState.z;
// rotate avg accels into earth frame and store it
AttitudeActualData attitudeActual;
AttitudeActualGet(&attitudeActual);
q[0] = attitudeActual.q1;
q[1] = attitudeActual.q2;
q[2] = attitudeActual.q3;
q[3] = attitudeActual.q4;
AttitudeStateData attitudeState;
AttitudeStateGet(&attitudeState);
q[0] = attitudeState.q1;
q[1] = attitudeState.q2;
q[2] = attitudeState.q3;
q[3] = attitudeState.q4;
Quaternion2R(q, Rbe);
for (uint8_t i = 0; i < 3; i++) {
accel_ned[i] = 0;
@ -757,13 +757,13 @@ static void accessoryUpdated(UAVObjEvent *ev)
if (poiLearn.Input != POILEARNSETTINGS_INPUT_NONE) {
if (AccessoryDesiredInstGet(poiLearn.Input - POILEARNSETTINGS_INPUT_ACCESSORY0, &accessory) == 0) {
if (accessory.AccessoryVal < -0.5f) {
PositionActualData positionActual;
PositionActualGet(&positionActual);
PositionStateData positionState;
PositionStateGet(&positionState);
PoiLocationData poi;
PoiLocationGet(&poi);
poi.North = positionActual.North;
poi.East = positionActual.East;
poi.Down = positionActual.Down;
poi.North = positionState.North;
poi.East = positionState.East;
poi.Down = positionState.Down;
PoiLocationSet(&poi);
}
}

28
flight/pios/inc/pios_math.h
View File

@ -45,12 +45,40 @@
#define M_LNPI_F 1.14472988584940017414342735135f /* ln(pi) */
#define M_EULER_F 0.57721566490153286060651209008f /* Euler constant */
#define M_E_D 2.71828182845904523536028747135d /* e */
#define M_LOG2E_D 1.44269504088896340735992468100d /* log_2 (e) */
#define M_LOG10E_D 0.43429448190325182765112891892d /* log_10 (e) */
#define M_SQRT2_D 1.41421356237309504880168872421d /* sqrt(2) */
#define M_SQRT1_2_D 0.70710678118654752440084436210d /* sqrt(1/2) */
#define M_SQRT3_D 1.73205080756887729352744634151d /* sqrt(3) */
#define M_PI_D 3.14159265358979323846264338328d /* pi */
#define M_PI_2_D 1.57079632679489661923132169164d /* pi/2 */
#define M_PI_4_D 0.78539816339744830961566084582d /* pi/4 */
#define M_SQRTPI_D 1.77245385090551602729816748334d /* sqrt(pi) */
#define M_2_SQRTPI_D 1.12837916709551257389615890312d /* 2/sqrt(pi) */
#define M_1_PI_D 0.31830988618379067153776752675d /* 1/pi */
#define M_2_PI_D 0.63661977236758134307553505349d /* 2/pi */
#define M_LN10_D 2.30258509299404568401799145468d /* ln(10) */
#define M_LN2_D 0.69314718055994530941723212146d /* ln(2) */
#define M_LNPI_D 1.14472988584940017414342735135d /* ln(pi) */
#define M_EULER_D 0.57721566490153286060651209008d /* Euler constant */
// Conversion macro
#define RAD2DEG(rad) ((rad) * (180.0f / M_PI_F))
#define DEG2RAD(deg) ((deg) * (M_PI_F / 180.0f))
#define RAD2DEG_D(rad) ((rad) * (180.0d / M_PI_D))
#define DEG2RAD_D(deg) ((deg) * (M_PI_D / 180.0d))
// Useful math macros
#define MAX(a, b) ((a) > (b) ? (a) : (b))
#define MIN(a, b) ((a) < (b) ? (a) : (b))
#define IS_REAL(f) (!isnan(f) && !isinf(f))
// Bitfield access
#define IS_SET(field, mask) (((field) & (mask)) == (mask))
#define SET_MASK(field, mask) (field) |= (mask)
#define UNSET_MASK(field, mask) (field) &= ~(mask)
#endif // PIOS_MATH_H

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

@ -87,9 +87,9 @@ ifndef TESTAPP
SRC += $(OPUAVSYNTHDIR)/actuatorcommand.c
SRC += $(OPUAVSYNTHDIR)/actuatordesired.c
SRC += $(OPUAVSYNTHDIR)/actuatorsettings.c
SRC += $(OPUAVSYNTHDIR)/accels.c
SRC += $(OPUAVSYNTHDIR)/gyros.c
SRC += $(OPUAVSYNTHDIR)/attitudeactual.c
SRC += $(OPUAVSYNTHDIR)/accelstate.c
SRC += $(OPUAVSYNTHDIR)/gyrostate.c
SRC += $(OPUAVSYNTHDIR)/attitudestate.c
SRC += $(OPUAVSYNTHDIR)/manualcontrolcommand.c
SRC += $(OPUAVSYNTHDIR)/i2cstats.c
SRC += $(OPUAVSYNTHDIR)/watchdogstatus.c
@ -99,8 +99,8 @@ ifndef TESTAPP
SRC += $(OPUAVSYNTHDIR)/attitudesettings.c
SRC += $(OPUAVSYNTHDIR)/camerastabsettings.c
SRC += $(OPUAVSYNTHDIR)/cameradesired.c
SRC += $(OPUAVSYNTHDIR)/gpsposition.c
SRC += $(OPUAVSYNTHDIR)/gpsvelocity.c
SRC += $(OPUAVSYNTHDIR)/gpspositionsensor.c
SRC += $(OPUAVSYNTHDIR)/gpsvelocitysensor.c
SRC += $(OPUAVSYNTHDIR)/hwsettings.c
SRC += $(OPUAVSYNTHDIR)/gcsreceiver.c
SRC += $(OPUAVSYNTHDIR)/receiveractivity.c
@ -109,9 +109,9 @@ ifndef TESTAPP
SRC += $(OPUAVSYNTHDIR)/taskinfo.c
SRC += $(OPUAVSYNTHDIR)/mixerstatus.c
SRC += $(OPUAVSYNTHDIR)/ratedesired.c
SRC += $(OPUAVSYNTHDIR)/baroaltitude.c
SRC += $(OPUAVSYNTHDIR)/barosensor.c
SRC += $(OPUAVSYNTHDIR)/txpidsettings.c
SRC += $(OPUAVSYNTHDIR)/airspeedactual.c
SRC += $(OPUAVSYNTHDIR)/airspeedstate.c
SRC += $(OPUAVSYNTHDIR)/mpu6000settings.c
else
## Test Code

10
flight/targets/boards/osd/firmware/Makefile
View File

@ -79,7 +79,7 @@ ifndef TESTAPP
SRC += $(OPUAVSYNTHDIR)/systemstats.c
SRC += $(OPUAVSYNTHDIR)/systemalarms.c
SRC += $(OPUAVSYNTHDIR)/systemsettings.c
SRC += $(OPUAVSYNTHDIR)/attitudeactual.c
SRC += $(OPUAVSYNTHDIR)/attitudestate.c
SRC += $(OPUAVSYNTHDIR)/i2cstats.c
SRC += $(OPUAVSYNTHDIR)/watchdogstatus.c
SRC += $(OPUAVSYNTHDIR)/firmwareiapobj.c
@ -87,13 +87,13 @@ ifndef TESTAPP
SRC += $(OPUAVSYNTHDIR)/taskinfo.c
SRC += $(OPUAVSYNTHDIR)/mixerstatus.c
SRC += $(OPUAVSYNTHDIR)/homelocation.c
SRC += $(OPUAVSYNTHDIR)/gpsposition.c
SRC += $(OPUAVSYNTHDIR)/gpspositionsensor.c
SRC += $(OPUAVSYNTHDIR)/gpssatellites.c
SRC += $(OPUAVSYNTHDIR)/gpsvelocity.c
SRC += $(OPUAVSYNTHDIR)/gpsvelocitysensor.c
SRC += $(OPUAVSYNTHDIR)/gpstime.c
SRC += $(OPUAVSYNTHDIR)/osdsettings.c
SRC += $(OPUAVSYNTHDIR)/baroaltitude.c
SRC += $(OPUAVSYNTHDIR)/magnetometer.c
SRC += $(OPUAVSYNTHDIR)/barosensor.c
SRC += $(OPUAVSYNTHDIR)/magsensor.c
else
## Test Code
SRC += $(OPTESTS)/test_common.c

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

@ -29,7 +29,8 @@ USE_DSP_LIB ?= NO
# List of mandatory modules to include
MODULES += Sensors
MODULES += Attitude/revolution
#MODULES += Attitude/revolution
MODULES += StateEstimation # use instead of Attitude
MODULES += Altitude/revolution
MODULES += Airspeed/revolution
MODULES += AltitudeHold

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

@ -25,16 +25,17 @@ UAVOBJSRCFILENAMES += actuatordesired
UAVOBJSRCFILENAMES += actuatorsettings
UAVOBJSRCFILENAMES += altholdsmoothed
UAVOBJSRCFILENAMES += attitudesettings
UAVOBJSRCFILENAMES += attitudeactual
UAVOBJSRCFILENAMES += gyros
UAVOBJSRCFILENAMES += gyrosbias
UAVOBJSRCFILENAMES += accels
UAVOBJSRCFILENAMES += magnetometer
UAVOBJSRCFILENAMES += magbias
UAVOBJSRCFILENAMES += baroaltitude
UAVOBJSRCFILENAMES += attitudestate
UAVOBJSRCFILENAMES += gyrostate
UAVOBJSRCFILENAMES += gyrosensor
UAVOBJSRCFILENAMES += accelstate
UAVOBJSRCFILENAMES += accelsensor
UAVOBJSRCFILENAMES += magsensor
UAVOBJSRCFILENAMES += magstate
UAVOBJSRCFILENAMES += barosensor
UAVOBJSRCFILENAMES += airspeedsensor
UAVOBJSRCFILENAMES += airspeedsettings
UAVOBJSRCFILENAMES += airspeedactual
UAVOBJSRCFILENAMES += airspeedstate
UAVOBJSRCFILENAMES += flightbatterysettings
UAVOBJSRCFILENAMES += firmwareiapobj
UAVOBJSRCFILENAMES += flightbatterystate
@ -44,10 +45,10 @@ UAVOBJSRCFILENAMES += flightplanstatus
UAVOBJSRCFILENAMES += flighttelemetrystats
UAVOBJSRCFILENAMES += gcstelemetrystats
UAVOBJSRCFILENAMES += gcsreceiver
UAVOBJSRCFILENAMES += gpsposition
UAVOBJSRCFILENAMES += gpspositionsensor
UAVOBJSRCFILENAMES += gpssatellites
UAVOBJSRCFILENAMES += gpstime
UAVOBJSRCFILENAMES += gpsvelocity
UAVOBJSRCFILENAMES += gpsvelocitysensor
UAVOBJSRCFILENAMES += fixedwingpathfollowersettings
UAVOBJSRCFILENAMES += fixedwingpathfollowerstatus
UAVOBJSRCFILENAMES += vtolpathfollowersettings
@ -64,7 +65,8 @@ UAVOBJSRCFILENAMES += overosyncsettings
UAVOBJSRCFILENAMES += pathaction
UAVOBJSRCFILENAMES += pathdesired
UAVOBJSRCFILENAMES += pathstatus
UAVOBJSRCFILENAMES += positionactual
UAVOBJSRCFILENAMES += positionsensor
UAVOBJSRCFILENAMES += positionstate
UAVOBJSRCFILENAMES += ratedesired
UAVOBJSRCFILENAMES += relaytuning
UAVOBJSRCFILENAMES += relaytuningsettings
@ -79,7 +81,7 @@ UAVOBJSRCFILENAMES += systemalarms
UAVOBJSRCFILENAMES += systemsettings
UAVOBJSRCFILENAMES += systemstats
UAVOBJSRCFILENAMES += taskinfo
UAVOBJSRCFILENAMES += velocityactual
UAVOBJSRCFILENAMES += velocitystate
UAVOBJSRCFILENAMES += velocitydesired
UAVOBJSRCFILENAMES += watchdogstatus
UAVOBJSRCFILENAMES += flightstatus

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

@ -137,6 +137,7 @@
#define PIOS_INCLUDE_GPS_NMEA_PARSER
#define PIOS_INCLUDE_GPS_UBX_PARSER
#define PIOS_GPS_SETS_HOMELOCATION
#define PIOS_GPS_SETS_POSITIONSENSOR
/* Stabilization options */
/* #define PIOS_QUATERNION_STABILIZATION */

22
flight/targets/boards/revolution/firmware/pios_board_sim.c
View File

@ -31,12 +31,11 @@
#include <pios_gcsrcvr_priv.h>
#include <uavobjectsinit.h>
#include <accels.h>
#include <baroaltitude.h>
#include <gpsposition.h>
#include <gyros.h>
#include <gyrosbias.h>
#include <magnetometer.h>
#include <accelssensor.h>
#include <barosensor.h>
#include <gpspositionsensor.h>
#include <gyrosensor.h>
#include <magsensor.h>
#include <manualcontrolsettings.h>
void Stack_Change() {}
@ -137,12 +136,11 @@ void PIOS_Board_Init(void)
UAVObjInitialize();
UAVObjectsInitializeAll();
AccelsInitialize();
BaroAltitudeInitialize();
MagnetometerInitialize();
GPSPositionInitialize();
GyrosInitialize();
GyrosBiasInitialize();
AccelSensorInitialize();
BaroSensorInitialize();
MagSensorInitialize();
GPSPositionSensorInitialize();
GyroSensorInitialize();
/* Initialize the alarms library */
AlarmsInitialize();

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

@ -30,6 +30,7 @@ USE_DSP_LIB ?= NO
# List of mandatory modules to include
MODULES += Sensors
MODULES += Attitude/revolution
#MODULES += StateEstimation # use instead of Attitude
MODULES += Altitude/revolution
MODULES += Airspeed/revolution
MODULES += AltitudeHold

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

@ -30,16 +30,17 @@ UAVOBJSRCFILENAMES += actuatordesired
UAVOBJSRCFILENAMES += actuatorsettings
UAVOBJSRCFILENAMES += altholdsmoothed
UAVOBJSRCFILENAMES += attitudesettings
UAVOBJSRCFILENAMES += attitudeactual
UAVOBJSRCFILENAMES += gyros
UAVOBJSRCFILENAMES += gyrosbias
UAVOBJSRCFILENAMES += accels
UAVOBJSRCFILENAMES += magnetometer
UAVOBJSRCFILENAMES += magbias
UAVOBJSRCFILENAMES += baroaltitude
UAVOBJSRCFILENAMES += attitudestate
UAVOBJSRCFILENAMES += gyrostate
UAVOBJSRCFILENAMES += gyrosensor
UAVOBJSRCFILENAMES += accelstate
UAVOBJSRCFILENAMES += accelsensor
UAVOBJSRCFILENAMES += magstate
UAVOBJSRCFILENAMES += magsensor
UAVOBJSRCFILENAMES += barosensor
UAVOBJSRCFILENAMES += airspeedsensor
UAVOBJSRCFILENAMES += airspeedsettings
UAVOBJSRCFILENAMES += airspeedactual
UAVOBJSRCFILENAMES += airspeedstate
UAVOBJSRCFILENAMES += flightbatterysettings
UAVOBJSRCFILENAMES += firmwareiapobj
UAVOBJSRCFILENAMES += flightbatterystate
@ -49,10 +50,10 @@ UAVOBJSRCFILENAMES += flightplanstatus
UAVOBJSRCFILENAMES += flighttelemetrystats
UAVOBJSRCFILENAMES += gcstelemetrystats
UAVOBJSRCFILENAMES += gcsreceiver
UAVOBJSRCFILENAMES += gpsposition
UAVOBJSRCFILENAMES += gpspositionsensor
UAVOBJSRCFILENAMES += gpssatellites
UAVOBJSRCFILENAMES += gpstime
UAVOBJSRCFILENAMES += gpsvelocity
UAVOBJSRCFILENAMES += gpsvelocitysensor
UAVOBJSRCFILENAMES += fixedwingpathfollowersettings
UAVOBJSRCFILENAMES += fixedwingpathfollowerstatus
UAVOBJSRCFILENAMES += vtolpathfollowersettings
@ -69,7 +70,8 @@ UAVOBJSRCFILENAMES += overosyncsettings
UAVOBJSRCFILENAMES += pathaction
UAVOBJSRCFILENAMES += pathdesired
UAVOBJSRCFILENAMES += pathstatus
UAVOBJSRCFILENAMES += positionactual
UAVOBJSRCFILENAMES += positionsensor
UAVOBJSRCFILENAMES += positionstate
UAVOBJSRCFILENAMES += ratedesired
UAVOBJSRCFILENAMES += relaytuning
UAVOBJSRCFILENAMES += relaytuningsettings
@ -84,7 +86,7 @@ UAVOBJSRCFILENAMES += systemalarms
UAVOBJSRCFILENAMES += systemsettings
UAVOBJSRCFILENAMES += systemstats
UAVOBJSRCFILENAMES += taskinfo
UAVOBJSRCFILENAMES += velocityactual
UAVOBJSRCFILENAMES += velocitystate
UAVOBJSRCFILENAMES += velocitydesired
UAVOBJSRCFILENAMES += watchdogstatus
UAVOBJSRCFILENAMES += flightstatus

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

@ -137,6 +137,7 @@
#define PIOS_INCLUDE_GPS_NMEA_PARSER
#define PIOS_INCLUDE_GPS_UBX_PARSER
#define PIOS_GPS_SETS_HOMELOCATION
#define PIOS_GPS_SETS_POSITIONSENSOR
/* Stabilization options */
/* #define PIOS_QUATERNION_STABILIZATION */

23
flight/targets/boards/revoproto/firmware/pios_board_sim.c
View File

@ -31,12 +31,11 @@
#include <pios_gcsrcvr_priv.h>
#include <uavobjectsinit.h>
#include <accels.h>
#include <baroaltitude.h>
#include <gpsposition.h>
#include <gyros.h>
#include <gyrosbias.h>
#include <magnetometer.h>
#include <accelsensor.h>
#include <barosensor.h>
#include <gpspositionsensor.h>
#include <gyrosensor.h>
#include <magsensor.h>
#include <manualcontrolsettings.h>
void Stack_Change() {}
@ -137,12 +136,12 @@ void PIOS_Board_Init(void)
UAVObjInitialize();
UAVObjectsInitializeAll();
AccelsInitialize();
BaroAltitudeInitialize();
MagnetometerInitialize();
GPSPositionInitialize();
GyrosInitialize();
GyrosBiasInitialize();
AccelSensorInitialize();
BaroSensorInitialize();
MagSensorInitialize();
GPSPositionSensorInitialize();
GyroStatInitialize();
GyroSensorInitialize();
/* Initialize the alarms library */
AlarmsInitialize();

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

@ -38,6 +38,8 @@ MODULES += VtolPathFollower
MODULES += CameraStab
MODULES += Telemetry
MODULES += FirmwareIAP
MODULES += StateEstimation
MODULES += Sensors/simulated/Sensors
#MODULES += OveroSync
# Paths

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

@ -30,15 +30,18 @@ UAVOBJSRCFILENAMES += actuatordesired
UAVOBJSRCFILENAMES += actuatorsettings
UAVOBJSRCFILENAMES += altholdsmoothed
UAVOBJSRCFILENAMES += attitudesettings
UAVOBJSRCFILENAMES += attitudeactual
UAVOBJSRCFILENAMES += gyros
UAVOBJSRCFILENAMES += gyrosbias
UAVOBJSRCFILENAMES += accels
UAVOBJSRCFILENAMES += magnetometer
UAVOBJSRCFILENAMES += baroaltitude
UAVOBJSRCFILENAMES += attitudestate
UAVOBJSRCFILENAMES += attitudesimulated
UAVOBJSRCFILENAMES += gyrostate
UAVOBJSRCFILENAMES += gyrosensor
UAVOBJSRCFILENAMES += accelstate
UAVOBJSRCFILENAMES += accelsensor
UAVOBJSRCFILENAMES += magsensor
UAVOBJSRCFILENAMES += magstate
UAVOBJSRCFILENAMES += barosensor
UAVOBJSRCFILENAMES += airspeedsensor
UAVOBJSRCFILENAMES += airspeedsettings
UAVOBJSRCFILENAMES += airspeedactual
UAVOBJSRCFILENAMES += airspeedstate
UAVOBJSRCFILENAMES += flightbatterysettings
UAVOBJSRCFILENAMES += firmwareiapobj
UAVOBJSRCFILENAMES += flightbatterystate
@ -47,10 +50,10 @@ UAVOBJSRCFILENAMES += flightplansettings
UAVOBJSRCFILENAMES += flightplanstatus
UAVOBJSRCFILENAMES += flighttelemetrystats
UAVOBJSRCFILENAMES += gcstelemetrystats
UAVOBJSRCFILENAMES += gpsposition
UAVOBJSRCFILENAMES += gpspositionsensor
UAVOBJSRCFILENAMES += gpssatellites
UAVOBJSRCFILENAMES += gpstime
UAVOBJSRCFILENAMES += gpsvelocity
UAVOBJSRCFILENAMES += gpsvelocitysensor
UAVOBJSRCFILENAMES += vtolpathfollowersettings
UAVOBJSRCFILENAMES += fixedwingpathfollowersettings
UAVOBJSRCFILENAMES += fixedwingpathfollowerstatus
@ -66,7 +69,8 @@ UAVOBJSRCFILENAMES += overosyncstats
UAVOBJSRCFILENAMES += pathaction
UAVOBJSRCFILENAMES += pathdesired
UAVOBJSRCFILENAMES += pathstatus
UAVOBJSRCFILENAMES += positionactual
UAVOBJSRCFILENAMES += positionsensor
UAVOBJSRCFILENAMES += positionstate
UAVOBJSRCFILENAMES += ratedesired
UAVOBJSRCFILENAMES += revocalibration
UAVOBJSRCFILENAMES += relaytuning
@ -78,7 +82,7 @@ UAVOBJSRCFILENAMES += systemalarms
UAVOBJSRCFILENAMES += systemsettings
UAVOBJSRCFILENAMES += systemstats
UAVOBJSRCFILENAMES += taskinfo
UAVOBJSRCFILENAMES += velocityactual
UAVOBJSRCFILENAMES += velocitystate
UAVOBJSRCFILENAMES += velocitydesired
UAVOBJSRCFILENAMES += watchdogstatus
UAVOBJSRCFILENAMES += waypoint
@ -91,7 +95,10 @@ UAVOBJSRCFILENAMES += receiveractivity
UAVOBJSRCFILENAMES += cameradesired
UAVOBJSRCFILENAMES += camerastabsettings
UAVOBJSRCFILENAMES += altitudeholdsettings
UAVOBJSRCFILENAMES += revosettings
UAVOBJSRCFILENAMES += altitudeholddesired
UAVOBJSRCFILENAMES += ekfconfiguration
UAVOBJSRCFILENAMES += ekfstatevariance
UAVOBJSRC = $(foreach UAVOBJSRCFILE,$(UAVOBJSRCFILENAMES),$(UAVOBJSYNTHDIR)/$(UAVOBJSRCFILE).c )
UAVOBJDEFINE = $(foreach UAVOBJSRCFILE,$(UAVOBJSRCFILENAMES),-DUAVOBJ_INIT_$(UAVOBJSRCFILE) )

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

@ -45,6 +45,7 @@
#define PIOS_INCLUDE_IRQ
#define PIOS_INCLUDE_LED
#define PIOS_INCLUDE_SDCARD
#define PIOS_USE_SETTINGS_ON_SDCARD
// #define PIOS_INCLUDE_IAP
#define PIOS_INCLUDE_SERVO
#define PIOS_INCLUDE_SPI

198
ground/openpilotgcs/share/openpilotgcs/default_configurations/Developer.xml
View File

@ -49,7 +49,7 @@
<Current>
<arr_1>
<CurrentLanguage>default</CurrentLanguage>
<DataObject>Accels</DataObject>
<DataObject>AccelState</DataObject>
<ExpireTimeout>0</ExpireTimeout>
<Mute>false</Mute>
<ObjectField></ObjectField>
@ -99,19 +99,19 @@
<dialNeedleID2>needle</dialNeedleID2>
<dialNeedleID3>needle3</dialNeedleID3>
<font>Lucida Grande,13,-1,5,50,0,0,0,0,0</font>
<needle1DataObject>AttitudeActual</needle1DataObject>
<needle1DataObject>AttitudeState</needle1DataObject>
<needle1Factor>-1</needle1Factor>
<needle1MaxValue>360</needle1MaxValue>
<needle1MinValue>0</needle1MinValue>
<needle1Move>Rotate</needle1Move>
<needle1ObjectField>Roll</needle1ObjectField>
<needle2DataObject>AttitudeActual</needle2DataObject>
<needle2DataObject>AttitudeState</needle2DataObject>
<needle2Factor>75</needle2Factor>
<needle2MaxValue>20</needle2MaxValue>
<needle2MinValue>0</needle2MinValue>
<needle2Move>Vertical</needle2Move>
<needle2ObjectField>Pitch</needle2ObjectField>
<needle3DataObject>AttitudeActual</needle3DataObject>
<needle3DataObject>AttitudeState</needle3DataObject>
<needle3Factor>-1</needle3Factor>
<needle3MaxValue>360</needle3MaxValue>
<needle3MinValue>0</needle3MinValue>
@ -134,19 +134,19 @@
<dialNeedleID2>needle2</dialNeedleID2>
<dialNeedleID3>needle3</dialNeedleID3>
<font>Lucida Grande,13,-1,5,50,0,0,0,0,0</font>
<needle1DataObject>BaroAltitude</needle1DataObject>
<needle1DataObject>BaroSensor</needle1DataObject>
<needle1Factor>1</needle1Factor>
<needle1MaxValue>10</needle1MaxValue>
<needle1MinValue>0</needle1MinValue>
<needle1Move>Rotate</needle1Move>
<needle1ObjectField>Altitude</needle1ObjectField>
<needle2DataObject>BaroAltitude</needle2DataObject>
<needle2DataObject>BaroSensor</needle2DataObject>
<needle2Factor>1</needle2Factor>
<needle2MaxValue>100</needle2MaxValue>
<needle2MinValue>0</needle2MinValue>
<needle2Move>Rotate</needle2Move>
<needle2ObjectField>Altitude</needle2ObjectField>
<needle3DataObject>BaroAltitude</needle3DataObject>
<needle3DataObject>BaroSensor</needle3DataObject>
<needle3Factor>1</needle3Factor>
<needle3MaxValue>1000</needle3MaxValue>
<needle3MinValue>0</needle3MinValue>
@ -169,19 +169,19 @@
<dialNeedleID2></dialNeedleID2>
<dialNeedleID3></dialNeedleID3>
<font>Lucida Grande,13,-1,5,50,0,0,0,0,0</font>
<needle1DataObject>BaroAltitude</needle1DataObject>
<needle1DataObject>BaroSensor</needle1DataObject>
<needle1Factor>10</needle1Factor>
<needle1MaxValue>1120</needle1MaxValue>
<needle1MinValue>1000</needle1MinValue>
<needle1Move>Rotate</needle1Move>
<needle1ObjectField>Pressure</needle1ObjectField>
<needle2DataObject>BaroAltitude</needle2DataObject>
<needle2DataObject>BaroSensor</needle2DataObject>
<needle2Factor>1</needle2Factor>
<needle2MaxValue>100</needle2MaxValue>
<needle2MinValue>0</needle2MinValue>
<needle2Move>Rotate</needle2Move>
<needle2ObjectField>Altitude</needle2ObjectField>
<needle3DataObject>BaroAltitude</needle3DataObject>
<needle3DataObject>BaroSensor</needle3DataObject>
<needle3Factor>1</needle3Factor>
<needle3MaxValue>1000</needle3MaxValue>
<needle3MinValue>0</needle3MinValue>
@ -204,19 +204,19 @@
<dialNeedleID2></dialNeedleID2>
<dialNeedleID3></dialNeedleID3>
<font>Lucida Grande,13,-1,5,50,0,0,0,0,0</font>
<needle1DataObject>VelocityActual</needle1DataObject>
<needle1DataObject>VelocityState</needle1DataObject>
<needle1Factor>0.01</needle1Factor>
<needle1MaxValue>12</needle1MaxValue>
<needle1MinValue>-12</needle1MinValue>
<needle1Move>Rotate</needle1Move>
<needle1ObjectField>Down</needle1ObjectField>
<needle2DataObject>BaroAltitude</needle2DataObject>
<needle2DataObject>BaroSensor</needle2DataObject>
<needle2Factor>1</needle2Factor>
<needle2MaxValue>100</needle2MaxValue>
<needle2MinValue>0</needle2MinValue>
<needle2Move>Rotate</needle2Move>
<needle2ObjectField>Altitude</needle2ObjectField>
<needle3DataObject>BaroAltitude</needle3DataObject>
<needle3DataObject>BaroSensor</needle3DataObject>
<needle3Factor>1</needle3Factor>
<needle3MaxValue>1000</needle3MaxValue>
<needle3MinValue>0</needle3MinValue>
@ -239,19 +239,19 @@
<dialNeedleID2></dialNeedleID2>
<dialNeedleID3></dialNeedleID3>
<font>Lucida Grande,13,-1,5,50,0,0,0,0,0</font>
<needle1DataObject>AttitudeActual</needle1DataObject>
<needle1DataObject>AttitudeState</needle1DataObject>
<needle1Factor>-1</needle1Factor>
<needle1MaxValue>360</needle1MaxValue>
<needle1MinValue>0</needle1MinValue>
<needle1Move>Rotate</needle1Move>
<needle1ObjectField>Yaw</needle1ObjectField>
<needle2DataObject>BaroAltitude</needle2DataObject>
<needle2DataObject>BaroSensor</needle2DataObject>
<needle2Factor>1</needle2Factor>
<needle2MaxValue>100</needle2MaxValue>
<needle2MinValue>0</needle2MinValue>
<needle2Move>Rotate</needle2Move>
<needle2ObjectField>Altitude</needle2ObjectField>
<needle3DataObject>BaroAltitude</needle3DataObject>
<needle3DataObject>BaroSensor</needle3DataObject>
<needle3Factor>1</needle3Factor>
<needle3MaxValue>1000</needle3MaxValue>
<needle3MinValue>0</needle3MinValue>
@ -274,19 +274,19 @@
<dialNeedleID2>needle</dialNeedleID2>
<dialNeedleID3>needle3</dialNeedleID3>
<font>Lucida Grande,13,-1,5,50,0,0,0,0,0</font>
<needle1DataObject>AttitudeActual</needle1DataObject>
<needle1DataObject>AttitudeState</needle1DataObject>
<needle1Factor>-1</needle1Factor>
<needle1MaxValue>360</needle1MaxValue>
<needle1MinValue>0</needle1MinValue>
<needle1Move>Rotate</needle1Move>
<needle1ObjectField>Roll</needle1ObjectField>
<needle2DataObject>AttitudeActual</needle2DataObject>
<needle2DataObject>AttitudeState</needle2DataObject>
<needle2Factor>75</needle2Factor>
<needle2MaxValue>20</needle2MaxValue>
<needle2MinValue>0</needle2MinValue>
<needle2Move>Vertical</needle2Move>
<needle2ObjectField>Pitch</needle2ObjectField>
<needle3DataObject>AttitudeActual</needle3DataObject>
<needle3DataObject>AttitudeState</needle3DataObject>
<needle3Factor>-1</needle3Factor>
<needle3MaxValue>360</needle3MaxValue>
<needle3MinValue>0</needle3MinValue>
@ -309,19 +309,19 @@
<dialNeedleID2>needle2</dialNeedleID2>
<dialNeedleID3>needle3</dialNeedleID3>
<font>Lucida Grande,13,-1,5,50,0,0,0,0,0</font>
<needle1DataObject>BaroAltitude</needle1DataObject>
<needle1DataObject>BaroSensor</needle1DataObject>
<needle1Factor>1</needle1Factor>
<needle1MaxValue>10</needle1MaxValue>
<needle1MinValue>0</needle1MinValue>
<needle1Move>Rotate</needle1Move>
<needle1ObjectField>Altitude</needle1ObjectField>
<needle2DataObject>BaroAltitude</needle2DataObject>
<needle2DataObject>BaroSensor</needle2DataObject>
<needle2Factor>1</needle2Factor>
<needle2MaxValue>100</needle2MaxValue>
<needle2MinValue>0</needle2MinValue>
<needle2Move>Rotate</needle2Move>
<needle2ObjectField>Altitude</needle2ObjectField>
<needle3DataObject>BaroAltitude</needle3DataObject>
<needle3DataObject>BaroSensor</needle3DataObject>
<needle3Factor>1</needle3Factor>
<needle3MaxValue>1000</needle3MaxValue>
<needle3MinValue>0</needle3MinValue>
@ -344,19 +344,19 @@
<dialNeedleID2></dialNeedleID2>
<dialNeedleID3></dialNeedleID3>
<font>Lucida Grande,13,-1,5,50,0,0,0,0,0</font>
<needle1DataObject>BaroAltitude</needle1DataObject>
<needle1DataObject>BaroSensor</needle1DataObject>
<needle1Factor>10</needle1Factor>
<needle1MaxValue>1120</needle1MaxValue>
<needle1MinValue>1000</needle1MinValue>
<needle1Move>Rotate</needle1Move>
<needle1ObjectField>Pressure</needle1ObjectField>
<needle2DataObject>BaroAltitude</needle2DataObject>
<needle2DataObject>BaroSensor</needle2DataObject>
<needle2Factor>1</needle2Factor>
<needle2MaxValue>100</needle2MaxValue>
<needle2MinValue>0</needle2MinValue>
<needle2Move>Rotate</needle2Move>
<needle2ObjectField>Altitude</needle2ObjectField>
<needle3DataObject>BaroAltitude</needle3DataObject>
<needle3DataObject>BaroSensor</needle3DataObject>
<needle3Factor>1</needle3Factor>
<needle3MaxValue>1000</needle3MaxValue>
<needle3MinValue>0</needle3MinValue>
@ -379,19 +379,19 @@
<dialNeedleID2></dialNeedleID2>
<dialNeedleID3></dialNeedleID3>
<font>Lucida Grande,13,-1,5,50,0,0,0,0,0</font>
<needle1DataObject>VelocityActual</needle1DataObject>
<needle1DataObject>VelocityState</needle1DataObject>
<needle1Factor>0.01</needle1Factor>
<needle1MaxValue>11.2</needle1MaxValue>
<needle1MinValue>-11.2</needle1MinValue>
<needle1Move>Rotate</needle1Move>
<needle1ObjectField>Down</needle1ObjectField>
<needle2DataObject>BaroAltitude</needle2DataObject>
<needle2DataObject>BaroSensor</needle2DataObject>
<needle2Factor>1</needle2Factor>
<needle2MaxValue>100</needle2MaxValue>
<needle2MinValue>0</needle2MinValue>
<needle2Move>Rotate</needle2Move>
<needle2ObjectField>Altitude</needle2ObjectField>
<needle3DataObject>BaroAltitude</needle3DataObject>
<needle3DataObject>BaroSensor</needle3DataObject>
<needle3Factor>1</needle3Factor>
<needle3MaxValue>1000</needle3MaxValue>
<needle3MinValue>0</needle3MinValue>
@ -414,19 +414,19 @@
<dialNeedleID2></dialNeedleID2>
<dialNeedleID3></dialNeedleID3>
<font>Lucida Grande,13,-1,5,50,0,0,0,0,0</font>
<needle1DataObject>AttitudeActual</needle1DataObject>
<needle1DataObject>AttitudeState</needle1DataObject>
<needle1Factor>-1</needle1Factor>
<needle1MaxValue>360</needle1MaxValue>
<needle1MinValue>0</needle1MinValue>
<needle1Move>Rotate</needle1Move>
<needle1ObjectField>Yaw</needle1ObjectField>
<needle2DataObject>BaroAltitude</needle2DataObject>
<needle2DataObject>BaroSensor</needle2DataObject>
<needle2Factor>1</needle2Factor>
<needle2MaxValue>100</needle2MaxValue>
<needle2MinValue>0</needle2MinValue>
<needle2Move>Rotate</needle2Move>
<needle2ObjectField>Altitude</needle2ObjectField>
<needle3DataObject>BaroAltitude</needle3DataObject>
<needle3DataObject>BaroSensor</needle3DataObject>
<needle3Factor>1</needle3Factor>
<needle3MaxValue>1000</needle3MaxValue>
<needle3MinValue>0</needle3MinValue>
@ -449,19 +449,19 @@
<dialNeedleID2></dialNeedleID2>
<dialNeedleID3></dialNeedleID3>
<font>Lucida Grande,13,-1,5,50,0,0,0,0,0</font>
<needle1DataObject>GPSPosition</needle1DataObject>
<needle1DataObject>GPSPositionSensor</needle1DataObject>
<needle1Factor>3.6</needle1Factor>
<needle1MaxValue>120</needle1MaxValue>
<needle1MinValue>0</needle1MinValue>
<needle1Move>Rotate</needle1Move>
<needle1ObjectField>Groundspeed</needle1ObjectField>
<needle2DataObject>BaroAltitude</needle2DataObject>
<needle2DataObject>BaroSensor</needle2DataObject>
<needle2Factor>1</needle2Factor>
<needle2MaxValue>100</needle2MaxValue>
<needle2MinValue>0</needle2MinValue>
<needle2Move>Rotate</needle2Move>
<needle2ObjectField>Altitude</needle2ObjectField>
<needle3DataObject>BaroAltitude</needle3DataObject>
<needle3DataObject>BaroSensor</needle3DataObject>
<needle3Factor>1</needle3Factor>
<needle3MaxValue>1000</needle3MaxValue>
<needle3MinValue>0</needle3MinValue>
@ -484,19 +484,19 @@
<dialNeedleID2>needle2</dialNeedleID2>
<dialNeedleID3>needle3</dialNeedleID3>
<font>Lucida Grande,13,-1,5,50,0,0,0,0,0</font>
<needle1DataObject>BaroAltitude</needle1DataObject>
<needle1DataObject>BaroSensor</needle1DataObject>
<needle1Factor>1</needle1Factor>
<needle1MaxValue>120</needle1MaxValue>
<needle1MinValue>0</needle1MinValue>
<needle1Move>Rotate</needle1Move>
<needle1ObjectField>Temperature</needle1ObjectField>
<needle2DataObject>BaroAltitude</needle2DataObject>
<needle2DataObject>BaroSensor</needle2DataObject>
<needle2Factor>1</needle2Factor>
<needle2MaxValue>100</needle2MaxValue>
<needle2MinValue>0</needle2MinValue>
<needle2Move>Rotate</needle2Move>
<needle2ObjectField>Altitude</needle2ObjectField>
<needle3DataObject>BaroAltitude</needle3DataObject>
<needle3DataObject>BaroSensor</needle3DataObject>
<needle3Factor>1</needle3Factor>
<needle3MaxValue>1000</needle3MaxValue>
<needle3MinValue>0</needle3MinValue>
@ -519,19 +519,19 @@
<dialNeedleID2>needle2</dialNeedleID2>
<dialNeedleID3>needle2</dialNeedleID3>
<font>Lucida Grande,13,-1,5,50,0,0,0,0,0</font>
<needle1DataObject>AttitudeActual</needle1DataObject>
<needle1DataObject>AttitudeState</needle1DataObject>
<needle1Factor>-1</needle1Factor>
<needle1MaxValue>360</needle1MaxValue>
<needle1MinValue>0</needle1MinValue>
<needle1Move>Rotate</needle1Move>
<needle1ObjectField>Roll</needle1ObjectField>
<needle2DataObject>Accels</needle2DataObject>
<needle2DataObject>AccelState</needle2DataObject>
<needle2Factor>1</needle2Factor>
<needle2MaxValue>20</needle2MaxValue>
<needle2MinValue>-20</needle2MinValue>
<needle2Move>Horizontal</needle2Move>
<needle2ObjectField>x</needle2ObjectField>
<needle3DataObject>Accels</needle3DataObject>
<needle3DataObject>AccelState</needle3DataObject>
<needle3Factor>-1</needle3Factor>
<needle3MaxValue>360</needle3MaxValue>
<needle3MinValue>0</needle3MinValue>
@ -554,19 +554,19 @@
<dialNeedleID2></dialNeedleID2>
<dialNeedleID3></dialNeedleID3>
<font>Lucida Grande,13,-1,5,50,0,0,0,0,0</font>
<needle1DataObject>GPSPosition</needle1DataObject>
<needle1DataObject>GPSPositionSensor</needle1DataObject>
<needle1Factor>3.6</needle1Factor>
<needle1MaxValue>120</needle1MaxValue>
<needle1MinValue>0</needle1MinValue>
<needle1Move>Rotate</needle1Move>
<needle1ObjectField>Groundspeed</needle1ObjectField>
<needle2DataObject>BaroAltitude</needle2DataObject>
<needle2DataObject>BaroSensor</needle2DataObject>
<needle2Factor>1</needle2Factor>
<needle2MaxValue>100</needle2MaxValue>
<needle2MinValue>0</needle2MinValue>
<needle2Move>Rotate</needle2Move>
<needle2ObjectField>Altitude</needle2ObjectField>
<needle3DataObject>BaroAltitude</needle3DataObject>
<needle3DataObject>BaroSensor</needle3DataObject>
<needle3Factor>1</needle3Factor>
<needle3MaxValue>1000</needle3MaxValue>
<needle3MinValue>0</needle3MinValue>
@ -589,19 +589,19 @@
<dialNeedleID2>needle</dialNeedleID2>
<dialNeedleID3>needle3</dialNeedleID3>
<font>Lucida Grande,13,-1,5,50,0,0,0,0,0</font>
<needle1DataObject>AttitudeActual</needle1DataObject>
<needle1DataObject>AttitudeState</needle1DataObject>
<needle1Factor>-1</needle1Factor>
<needle1MaxValue>360</needle1MaxValue>
<needle1MinValue>0</needle1MinValue>
<needle1Move>Rotate</needle1Move>
<needle1ObjectField>Roll</needle1ObjectField>
<needle2DataObject>AttitudeActual</needle2DataObject>
<needle2DataObject>AttitudeState</needle2DataObject>
<needle2Factor>75</needle2Factor>
<needle2MaxValue>20</needle2MaxValue>
<needle2MinValue>0</needle2MinValue>
<needle2Move>Vertical</needle2Move>
<needle2ObjectField>Pitch</needle2ObjectField>
<needle3DataObject>AttitudeActual</needle3DataObject>
<needle3DataObject>AttitudeState</needle3DataObject>
<needle3Factor>-1</needle3Factor>
<needle3MaxValue>360</needle3MaxValue>
<needle3MinValue>0</needle3MinValue>
@ -624,19 +624,19 @@
<dialNeedleID2>needle2</dialNeedleID2>
<dialNeedleID3>needle3</dialNeedleID3>
<font>Lucida Grande,13,-1,5,50,0,0,0,0,0</font>
<needle1DataObject>BaroAltitude</needle1DataObject>
<needle1DataObject>BaroSensor</needle1DataObject>
<needle1Factor>1</needle1Factor>
<needle1MaxValue>10</needle1MaxValue>
<needle1MinValue>0</needle1MinValue>
<needle1Move>Rotate</needle1Move>
<needle1ObjectField>Altitude</needle1ObjectField>
<needle2DataObject>BaroAltitude</needle2DataObject>
<needle2DataObject>BaroSensor</needle2DataObject>
<needle2Factor>1</needle2Factor>
<needle2MaxValue>100</needle2MaxValue>
<needle2MinValue>0</needle2MinValue>
<needle2Move>Rotate</needle2Move>
<needle2ObjectField>Altitude</needle2ObjectField>
<needle3DataObject>BaroAltitude</needle3DataObject>
<needle3DataObject>BaroSensor</needle3DataObject>
<needle3Factor>1</needle3Factor>
<needle3MaxValue>1000</needle3MaxValue>
<needle3MinValue>0</needle3MinValue>
@ -659,19 +659,19 @@
<dialNeedleID2></dialNeedleID2>
<dialNeedleID3></dialNeedleID3>
<font>Lucida Grande,13,-1,5,50,0,0,0,0,0</font>
<needle1DataObject>BaroAltitude</needle1DataObject>
<needle1DataObject>BaroSensor</needle1DataObject>
<needle1Factor>10</needle1Factor>
<needle1MaxValue>1120</needle1MaxValue>
<needle1MinValue>1000</needle1MinValue>
<needle1Move>Rotate</needle1Move>
<needle1ObjectField>Pressure</needle1ObjectField>
<needle2DataObject>BaroAltitude</needle2DataObject>
<needle2DataObject>BaroSensor</needle2DataObject>
<needle2Factor>1</needle2Factor>
<needle2MaxValue>100</needle2MaxValue>
<needle2MinValue>0</needle2MinValue>
<needle2Move>Rotate</needle2Move>
<needle2ObjectField>Altitude</needle2ObjectField>
<needle3DataObject>BaroAltitude</needle3DataObject>
<needle3DataObject>BaroSensor</needle3DataObject>
<needle3Factor>1</needle3Factor>
<needle3MaxValue>1000</needle3MaxValue>
<needle3MinValue>0</needle3MinValue>
@ -694,19 +694,19 @@
<dialNeedleID2></dialNeedleID2>
<dialNeedleID3></dialNeedleID3>
<font>Lucida Grande,13,-1,5,50,0,0,0,0,0</font>
<needle1DataObject>VelocityActual</needle1DataObject>
<needle1DataObject>VelocityState</needle1DataObject>
<needle1Factor>0.01</needle1Factor>
<needle1MaxValue>12</needle1MaxValue>
<needle1MinValue>-12</needle1MinValue>
<needle1Move>Rotate</needle1Move>
<needle1ObjectField>Down</needle1ObjectField>
<needle2DataObject>BaroAltitude</needle2DataObject>
<needle2DataObject>BaroSensor</needle2DataObject>
<needle2Factor>1</needle2Factor>
<needle2MaxValue>100</needle2MaxValue>
<needle2MinValue>0</needle2MinValue>
<needle2Move>Rotate</needle2Move>
<needle2ObjectField>Altitude</needle2ObjectField>
<needle3DataObject>BaroAltitude</needle3DataObject>
<needle3DataObject>BaroSensor</needle3DataObject>
<needle3Factor>1</needle3Factor>
<needle3MaxValue>1000</needle3MaxValue>
<needle3MinValue>0</needle3MinValue>
@ -729,19 +729,19 @@
<dialNeedleID2></dialNeedleID2>
<dialNeedleID3></dialNeedleID3>
<font>Lucida Grande,13,-1,5,50,0,0,0,0,0</font>
<needle1DataObject>AttitudeActual</needle1DataObject>
<needle1DataObject>AttitudeState</needle1DataObject>
<needle1Factor>-1</needle1Factor>
<needle1MaxValue>360</needle1MaxValue>
<needle1MinValue>0</needle1MinValue>
<needle1Move>Rotate</needle1Move>
<needle1ObjectField>Yaw</needle1ObjectField>
<needle2DataObject>BaroAltitude</needle2DataObject>
<needle2DataObject>BaroSensor</needle2DataObject>
<needle2Factor>1</needle2Factor>
<needle2MaxValue>100</needle2MaxValue>
<needle2MinValue>0</needle2MinValue>
<needle2Move>Rotate</needle2Move>
<needle2ObjectField>Altitude</needle2ObjectField>
<needle3DataObject>BaroAltitude</needle3DataObject>
<needle3DataObject>BaroSensor</needle3DataObject>
<needle3Factor>1</needle3Factor>
<needle3MaxValue>1000</needle3MaxValue>
<needle3MinValue>0</needle3MinValue>
@ -764,19 +764,19 @@
<dialNeedleID2></dialNeedleID2>
<dialNeedleID3></dialNeedleID3>
<font>Lucida Grande,13,-1,5,50,0,0,0,0,0</font>
<needle1DataObject>GPSPosition</needle1DataObject>
<needle1DataObject>GPSPositionSensor</needle1DataObject>
<needle1Factor>3.6</needle1Factor>
<needle1MaxValue>120</needle1MaxValue>
<needle1MinValue>0</needle1MinValue>
<needle1Move>Rotate</needle1Move>
<needle1ObjectField>Groundspeed</needle1ObjectField>
<needle2DataObject>BaroAltitude</needle2DataObject>
<needle2DataObject>BaroSensor</needle2DataObject>
<needle2Factor>1</needle2Factor>
<needle2MaxValue>100</needle2MaxValue>
<needle2MinValue>0</needle2MinValue>
<needle2Move>Rotate</needle2Move>
<needle2ObjectField>Altitude</needle2ObjectField>
<needle3DataObject>BaroAltitude</needle3DataObject>
<needle3DataObject>BaroSensor</needle3DataObject>
<needle3Factor>1</needle3Factor>
<needle3MaxValue>1000</needle3MaxValue>
<needle3MinValue>0</needle3MinValue>
@ -799,19 +799,19 @@
<dialNeedleID2>needle2</dialNeedleID2>
<dialNeedleID3>needle3</dialNeedleID3>
<font>Lucida Grande,13,-1,5,50,0,0,0,0,0</font>
<needle1DataObject>BaroAltitude</needle1DataObject>
<needle1DataObject>BaroSensor</needle1DataObject>
<needle1Factor>1</needle1Factor>
<needle1MaxValue>120</needle1MaxValue>
<needle1MinValue>0</needle1MinValue>
<needle1Move>Rotate</needle1Move>
<needle1ObjectField>Temperature</needle1ObjectField>
<needle2DataObject>BaroAltitude</needle2DataObject>
<needle2DataObject>BaroSensor</needle2DataObject>
<needle2Factor>1</needle2Factor>
<needle2MaxValue>100</needle2MaxValue>
<needle2MinValue>0</needle2MinValue>
<needle2Move>Rotate</needle2Move>
<needle2ObjectField>Altitude</needle2ObjectField>
<needle3DataObject>BaroAltitude</needle3DataObject>
<needle3DataObject>BaroSensor</needle3DataObject>
<needle3Factor>1</needle3Factor>
<needle3MaxValue>1000</needle3MaxValue>
<needle3MinValue>0</needle3MinValue>
@ -840,13 +840,13 @@
<needle1MinValue>1000</needle1MinValue>
<needle1Move>Rotate</needle1Move>
<needle1ObjectField>Channel-3</needle1ObjectField>
<needle2DataObject>BaroAltitude</needle2DataObject>
<needle2DataObject>BaroSensor</needle2DataObject>
<needle2Factor>1</needle2Factor>
<needle2MaxValue>100</needle2MaxValue>
<needle2MinValue>0</needle2MinValue>
<needle2Move>Rotate</needle2Move>
<needle2ObjectField>Altitude</needle2ObjectField>
<needle3DataObject>BaroAltitude</needle3DataObject>
<needle3DataObject>BaroSensor</needle3DataObject>
<needle3Factor>1</needle3Factor>
<needle3MaxValue>1000</needle3MaxValue>
<needle3MinValue>0</needle3MinValue>
@ -869,19 +869,19 @@
<dialNeedleID2>needle2</dialNeedleID2>
<dialNeedleID3>needle3</dialNeedleID3>
<font>Lucida Grande,13,-1,5,50,0,0,0,0,0</font>
<needle1DataObject>BaroAltitude</needle1DataObject>
<needle1DataObject>BaroSensor</needle1DataObject>
<needle1Factor>1</needle1Factor>
<needle1MaxValue>120</needle1MaxValue>
<needle1MinValue>0</needle1MinValue>
<needle1Move>Rotate</needle1Move>
<needle1ObjectField>Temperature</needle1ObjectField>
<needle2DataObject>BaroAltitude</needle2DataObject>
<needle2DataObject>BaroSensor</needle2DataObject>
<needle2Factor>1</needle2Factor>
<needle2MaxValue>100</needle2MaxValue>
<needle2MinValue>0</needle2MinValue>
<needle2Move>Rotate</needle2Move>
<needle2ObjectField>Altitude</needle2ObjectField>
<needle3DataObject>BaroAltitude</needle3DataObject>
<needle3DataObject>BaroSensor</needle3DataObject>
<needle3Factor>1</needle3Factor>
<needle3MaxValue>1000</needle3MaxValue>
<needle3MinValue>0</needle3MinValue>
@ -1022,7 +1022,7 @@
<attActCalc>false</attActCalc>
<attActHW>false</attActHW>
<attActSim>true</attActSim>
<attActual>true</attActual>
<attState>true</attState>
<attRaw>true</attRaw>
<attRawRate>20</attRawRate>
<binPath></binPath>
@ -1064,7 +1064,7 @@
<minValue>-11</minValue>
<redMax>11</redMax>
<redMin>-11</redMin>
<sourceDataObject>Accels</sourceDataObject>
<sourceDataObject>AccelState</sourceDataObject>
<sourceObjectField>x</sourceObjectField>
<useOpenGLFlag>false</useOpenGLFlag>
<yellowMax>-5</yellowMax>
@ -1087,7 +1087,7 @@
<minValue>-11</minValue>
<redMax>11</redMax>
<redMin>-11</redMin>
<sourceDataObject>Accels</sourceDataObject>
<sourceDataObject>AccelState</sourceDataObject>
<sourceObjectField>y</sourceObjectField>
<useOpenGLFlag>false</useOpenGLFlag>
<yellowMax>-5</yellowMax>
@ -1110,7 +1110,7 @@
<minValue>-11</minValue>
<redMax>11</redMax>
<redMin>-11</redMin>
<sourceDataObject>Accels</sourceDataObject>
<sourceDataObject>AccelState</sourceDataObject>
<sourceObjectField>z</sourceObjectField>
<useOpenGLFlag>false</useOpenGLFlag>
<yellowMax>-5</yellowMax>
@ -1202,7 +1202,7 @@
<minValue>0</minValue>
<redMax>0</redMax>
<redMin>0</redMin>
<sourceDataObject>GPSPosition</sourceDataObject>
<sourceDataObject>GPSPositionSensor</sourceDataObject>
<sourceObjectField>Satellites</sourceObjectField>
<useOpenGLFlag>false</useOpenGLFlag>
<yellowMax>0</yellowMax>
@ -1225,7 +1225,7 @@
<minValue>0</minValue>
<redMax>33</redMax>
<redMin>0</redMin>
<sourceDataObject>GPSPosition</sourceDataObject>
<sourceDataObject>GPSPositionSensor</sourceDataObject>
<sourceObjectField>Status</sourceObjectField>
<useOpenGLFlag>false</useOpenGLFlag>
<yellowMax>66</yellowMax>
@ -1301,7 +1301,7 @@
<yellowMin>-0.8</yellowMin>
</data>
</Pitch>
<PitchActual>
<PitchState>
<configInfo>
<locked>false</locked>
<version>0.0.0</version>
@ -1317,13 +1317,13 @@
<minValue>-90</minValue>
<redMax>1</redMax>
<redMin>0</redMin>
<sourceDataObject>AttitudeActual</sourceDataObject>
<sourceDataObject>AttitudeState</sourceDataObject>
<sourceObjectField>Pitch</sourceObjectField>
<useOpenGLFlag>false</useOpenGLFlag>
<yellowMax>0.9</yellowMax>
<yellowMin>0.1</yellowMin>
</data>
</PitchActual>
</PitchState>
<Roll__PCT__20Desired>
<configInfo>
<locked>false</locked>
@ -1847,7 +1847,7 @@
<color>4294901760</color>
<mathFunction>None</mathFunction>
<uavField>x</uavField>
<uavObject>Accels</uavObject>
<uavObject>AccelState</uavObject>
<yMaximum>0</yMaximum>
<yMeanSamples>1</yMeanSamples>
<yMinimum>6.92920863607354e-310</yMinimum>
@ -1857,7 +1857,7 @@
<color>4283782655</color>
<mathFunction>None</mathFunction>
<uavField>y</uavField>
<uavObject>Accels</uavObject>
<uavObject>AccelState</uavObject>
<yMaximum>1.78017180972965e-306</yMaximum>
<yMeanSamples>1</yMeanSamples>
<yMinimum>9.34609790258712e-307</yMinimum>
@ -1867,7 +1867,7 @@
<color>4283804160</color>
<mathFunction>None</mathFunction>
<uavField>z</uavField>
<uavObject>Accels</uavObject>
<uavObject>AccelState</uavObject>
<yMaximum>2.6501977682312e-318</yMaximum>
<yMeanSamples>1</yMeanSamples>
<yMinimum>6.92920723246466e-310</yMinimum>
@ -1949,7 +1949,7 @@
<color>4283760895</color>
<mathFunction>None</mathFunction>
<uavField>Roll</uavField>
<uavObject>AttitudeActual</uavObject>
<uavObject>AttitudeState</uavObject>
<yMaximum>6.92921152826347e-310</yMaximum>
<yMeanSamples>1</yMeanSamples>
<yMinimum>6.92921152826031e-310</yMinimum>
@ -1959,7 +1959,7 @@
<color>4278233600</color>
<mathFunction>None</mathFunction>
<uavField>Yaw</uavField>
<uavObject>AttitudeActual</uavObject>
<uavObject>AttitudeState</uavObject>
<yMaximum>0</yMaximum>
<yMeanSamples>1</yMeanSamples>
<yMinimum>0</yMinimum>
@ -1969,7 +1969,7 @@
<color>4294901760</color>
<mathFunction>None</mathFunction>
<uavField>Pitch</uavField>
<uavObject>AttitudeActual</uavObject>
<uavObject>AttitudeState</uavObject>
<yMaximum>0</yMaximum>
<yMeanSamples>1</yMeanSamples>
<yMinimum>0</yMinimum>
@ -1995,7 +1995,7 @@
<color>4278190080</color>
<mathFunction>None</mathFunction>
<uavField>Pressure</uavField>
<uavObject>BaroAltitude</uavObject>
<uavObject>BaroSensor</uavObject>
<yMaximum>1.72723371102043e-77</yMaximum>
<yMeanSamples>1</yMeanSamples>
<yMinimum>3.86203233966055e-312</yMinimum>
@ -2102,7 +2102,7 @@
<refreshInterval>200</refreshInterval>
</data>
</Inputs>
<Raw__PCT__20Accels>
<Raw__PCT__20AccelState>
<configInfo>
<locked>false</locked>
<version>0.0.0</version>
@ -2117,7 +2117,7 @@
<color>4294901760</color>
<mathFunction>None</mathFunction>
<uavField>x</uavField>
<uavObject>Accels</uavObject>
<uavObject>AccelState</uavObject>
<yMaximum>0</yMaximum>
<yMeanSamples>1</yMeanSamples>
<yMinimum>0</yMinimum>
@ -2127,7 +2127,7 @@
<color>4283782655</color>
<mathFunction>None</mathFunction>
<uavField>y</uavField>
<uavObject>Accels</uavObject>
<uavObject>AccelState</uavObject>
<yMaximum>0</yMaximum>
<yMeanSamples>1</yMeanSamples>
<yMinimum>0</yMinimum>
@ -2137,7 +2137,7 @@
<color>4283804160</color>
<mathFunction>None</mathFunction>
<uavField>z</uavField>
<uavObject>Accels</uavObject>
<uavObject>AccelState</uavObject>
<yMaximum>0</yMaximum>
<yMeanSamples>1</yMeanSamples>
<yMinimum>0</yMinimum>
@ -2147,8 +2147,8 @@
<plotType>1</plotType>
<refreshInterval>500</refreshInterval>
</data>
</Raw__PCT__20Accels>
<Raw__PCT__20Gyros>
</Raw__PCT__20AccelState>
<Raw__PCT__20gyroState>
<configInfo>
<locked>false</locked>
<version>0.0.0</version>
@ -2163,7 +2163,7 @@
<color>4283804160</color>
<mathFunction>None</mathFunction>
<uavField>z</uavField>
<uavObject>Gyros</uavObject>
<uavObject>gyroState</uavObject>
<yMaximum>1.02360527502876e-306</yMaximum>
<yMeanSamples>1</yMeanSamples>
<yMinimum>6.92921152846347e-310</yMinimum>
@ -2173,7 +2173,7 @@
<color>4283782655</color>
<mathFunction>None</mathFunction>
<uavField>y</uavField>
<uavObject>Gyros</uavObject>
<uavObject>gyroState</uavObject>
<yMaximum>0</yMaximum>
<yMeanSamples>1</yMeanSamples>
<yMinimum>0</yMinimum>
@ -2183,7 +2183,7 @@
<color>4294901760</color>
<mathFunction>None</mathFunction>
<uavField>x</uavField>
<uavObject>Gyros</uavObject>
<uavObject>gyroState</uavObject>
<yMaximum>0</yMaximum>
<yMeanSamples>1</yMeanSamples>
<yMinimum>0</yMinimum>
@ -2193,7 +2193,7 @@
<plotType>1</plotType>
<refreshInterval>500</refreshInterval>
</data>
</Raw__PCT__20Gyros>
</Raw__PCT__20gyroState>
<Raw__PCT__20magnetometers>
<configInfo>
<locked>false</locked>
@ -2209,7 +2209,7 @@
<color>4294901760</color>
<mathFunction>None</mathFunction>
<uavField>x</uavField>
<uavObject>Magnetometer</uavObject>
<uavObject>MagState</uavObject>
<yMaximum>6.92916505601691e-310</yMaximum>
<yMeanSamples>1</yMeanSamples>
<yMinimum>3.86203233966055e-312</yMinimum>
@ -2219,7 +2219,7 @@
<color>4283782655</color>
<mathFunction>None</mathFunction>
<uavField>y</uavField>
<uavObject>Magnetometer</uavObject>
<uavObject>MagState</uavObject>
<yMaximum>1.72723371101889e-77</yMaximum>
<yMeanSamples>1</yMeanSamples>
<yMinimum>-1</yMinimum>
@ -2229,7 +2229,7 @@
<color>4283804160</color>
<mathFunction>None</mathFunction>
<uavField>z</uavField>
<uavObject>Magnetometer</uavObject>
<uavObject>MagState</uavObject>
<yMaximum>1.72723371102028e-77</yMaximum>
<yMeanSamples>1</yMeanSamples>
<yMinimum>7.21478569792807e-313</yMinimum>
@ -2670,7 +2670,7 @@
<side1>
<classId>ScopeGadget</classId>
<gadget>
<activeConfiguration>Raw Gyros</activeConfiguration>
<activeConfiguration>Raw gyroState</activeConfiguration>
</gadget>
<type>uavGadget</type>
</side1>

200
ground/openpilotgcs/share/openpilotgcs/default_configurations/OpenPilotGCS.xml
View File

@ -118,19 +118,19 @@
<dialNeedleID2>needle</dialNeedleID2>
<dialNeedleID3>needle3</dialNeedleID3>
<font>MS Shell Dlg 2,8.25,-1,5,50,0,0,0,0,0</font>
<needle1DataObject>AttitudeActual</needle1DataObject>
<needle1DataObject>AttitudeState</needle1DataObject>
<needle1Factor>-1</needle1Factor>
<needle1MaxValue>360</needle1MaxValue>
<needle1MinValue>0</needle1MinValue>
<needle1Move>Rotate</needle1Move>
<needle1ObjectField>Roll</needle1ObjectField>
<needle2DataObject>AttitudeActual</needle2DataObject>
<needle2DataObject>AttitudeState</needle2DataObject>
<needle2Factor>75</needle2Factor>
<needle2MaxValue>20</needle2MaxValue>
<needle2MinValue>0</needle2MinValue>
<needle2Move>Vertical</needle2Move>
<needle2ObjectField>Pitch</needle2ObjectField>
<needle3DataObject>AttitudeActual</needle3DataObject>
<needle3DataObject>AttitudeState</needle3DataObject>
<needle3Factor>-1</needle3Factor>
<needle3MaxValue>360</needle3MaxValue>
<needle3MinValue>0</needle3MinValue>
@ -153,19 +153,19 @@
<dialNeedleID2>needle2</dialNeedleID2>
<dialNeedleID3>needle3</dialNeedleID3>
<font>MS Shell Dlg 2,8.25,-1,5,50,0,0,0,0,0</font>
<needle1DataObject>BaroAltitude</needle1DataObject>
<needle1DataObject>BaroSensor</needle1DataObject>
<needle1Factor>1</needle1Factor>
<needle1MaxValue>10</needle1MaxValue>
<needle1MinValue>0</needle1MinValue>
<needle1Move>Rotate</needle1Move>
<needle1ObjectField>Altitude</needle1ObjectField>
<needle2DataObject>BaroAltitude</needle2DataObject>
<needle2DataObject>BaroSensor</needle2DataObject>
<needle2Factor>1</needle2Factor>
<needle2MaxValue>100</needle2MaxValue>
<needle2MinValue>0</needle2MinValue>
<needle2Move>Rotate</needle2Move>
<needle2ObjectField>Altitude</needle2ObjectField>
<needle3DataObject>BaroAltitude</needle3DataObject>
<needle3DataObject>BaroSensor</needle3DataObject>
<needle3Factor>1</needle3Factor>
<needle3MaxValue>1000</needle3MaxValue>
<needle3MinValue>0</needle3MinValue>
@ -185,19 +185,19 @@
<dialFile>%%DATAPATH%%dials/default/barometer.svg</dialFile>
<dialNeedleID1>needle</dialNeedleID1>
<font>MS Shell Dlg 2,8.25,-1,5,50,0,0,0,0,0</font>
<needle1DataObject>BaroAltitude</needle1DataObject>
<needle1DataObject>BaroSensor</needle1DataObject>
<needle1Factor>10</needle1Factor>
<needle1MaxValue>1120</needle1MaxValue>
<needle1MinValue>1000</needle1MinValue>
<needle1Move>Rotate</needle1Move>
<needle1ObjectField>Pressure</needle1ObjectField>
<needle2DataObject>BaroAltitude</needle2DataObject>
<needle2DataObject>BaroSensor</needle2DataObject>
<needle2Factor>1</needle2Factor>
<needle2MaxValue>100</needle2MaxValue>
<needle2MinValue>0</needle2MinValue>
<needle2Move>Rotate</needle2Move>
<needle2ObjectField>Altitude</needle2ObjectField>
<needle3DataObject>BaroAltitude</needle3DataObject>
<needle3DataObject>BaroSensor</needle3DataObject>
<needle3Factor>1</needle3Factor>
<needle3MaxValue>1000</needle3MaxValue>
<needle3MinValue>0</needle3MinValue>
@ -217,19 +217,19 @@
<dialFile>%%DATAPATH%%dials/default/vsi.svg</dialFile>
<dialNeedleID1>needle</dialNeedleID1>
<font>MS Shell Dlg 2,8.25,-1,5,50,0,0,0,0,0</font>
<needle1DataObject>VelocityActual</needle1DataObject>
<needle1DataObject>VelocityState</needle1DataObject>
<needle1Factor>0.01</needle1Factor>
<needle1MaxValue>12</needle1MaxValue>
<needle1MinValue>-12</needle1MinValue>
<needle1Move>Rotate</needle1Move>
<needle1ObjectField>Down</needle1ObjectField>
<needle2DataObject>BaroAltitude</needle2DataObject>
<needle2DataObject>BaroSensor</needle2DataObject>
<needle2Factor>1</needle2Factor>
<needle2MaxValue>100</needle2MaxValue>
<needle2MinValue>0</needle2MinValue>
<needle2Move>Rotate</needle2Move>
<needle2ObjectField>Altitude</needle2ObjectField>
<needle3DataObject>BaroAltitude</needle3DataObject>
<needle3DataObject>BaroSensor</needle3DataObject>
<needle3Factor>1</needle3Factor>
<needle3MaxValue>1000</needle3MaxValue>
<needle3MinValue>0</needle3MinValue>
@ -250,19 +250,19 @@
<dialForegroundID>foreground</dialForegroundID>
<dialNeedleID1>needle</dialNeedleID1>
<font>MS Shell Dlg 2,8.25,-1,5,50,0,0,0,0,0</font>
<needle1DataObject>AttitudeActual</needle1DataObject>
<needle1DataObject>AttitudeState</needle1DataObject>
<needle1Factor>-1</needle1Factor>
<needle1MaxValue>360</needle1MaxValue>
<needle1MinValue>0</needle1MinValue>
<needle1Move>Rotate</needle1Move>
<needle1ObjectField>Yaw</needle1ObjectField>
<needle2DataObject>BaroAltitude</needle2DataObject>
<needle2DataObject>BaroSensor</needle2DataObject>
<needle2Factor>1</needle2Factor>
<needle2MaxValue>100</needle2MaxValue>
<needle2MinValue>0</needle2MinValue>
<needle2Move>Rotate</needle2Move>
<needle2ObjectField>Altitude</needle2ObjectField>
<needle3DataObject>BaroAltitude</needle3DataObject>
<needle3DataObject>BaroSensor</needle3DataObject>
<needle3Factor>1</needle3Factor>
<needle3MaxValue>1000</needle3MaxValue>
<needle3MinValue>0</needle3MinValue>
@ -285,19 +285,19 @@
<dialNeedleID2>needle</dialNeedleID2>
<dialNeedleID3>needle3</dialNeedleID3>
<font>MS Shell Dlg 2,8.25,-1,5,50,0,0,0,0,0</font>
<needle1DataObject>AttitudeActual</needle1DataObject>
<needle1DataObject>AttitudeState</needle1DataObject>
<needle1Factor>-1</needle1Factor>
<needle1MaxValue>360</needle1MaxValue>
<needle1MinValue>0</needle1MinValue>
<needle1Move>Rotate</needle1Move>
<needle1ObjectField>Roll</needle1ObjectField>
<needle2DataObject>AttitudeActual</needle2DataObject>
<needle2DataObject>AttitudeState</needle2DataObject>
<needle2Factor>75</needle2Factor>
<needle2MaxValue>20</needle2MaxValue>
<needle2MinValue>0</needle2MinValue>
<needle2Move>Vertical</needle2Move>
<needle2ObjectField>Pitch</needle2ObjectField>
<needle3DataObject>AttitudeActual</needle3DataObject>
<needle3DataObject>AttitudeState</needle3DataObject>
<needle3Factor>-1</needle3Factor>
<needle3MaxValue>360</needle3MaxValue>
<needle3MinValue>0</needle3MinValue>
@ -320,19 +320,19 @@
<dialNeedleID2>needle2</dialNeedleID2>
<dialNeedleID3>needle3</dialNeedleID3>
<font>MS Shell Dlg 2,8.25,-1,5,50,0,0,0,0,0</font>
<needle1DataObject>BaroAltitude</needle1DataObject>
<needle1DataObject>BaroSensor</needle1DataObject>
<needle1Factor>1</needle1Factor>
<needle1MaxValue>10</needle1MaxValue>
<needle1MinValue>0</needle1MinValue>
<needle1Move>Rotate</needle1Move>
<needle1ObjectField>Altitude</needle1ObjectField>
<needle2DataObject>BaroAltitude</needle2DataObject>
<needle2DataObject>BaroSensor</needle2DataObject>
<needle2Factor>1</needle2Factor>
<needle2MaxValue>100</needle2MaxValue>
<needle2MinValue>0</needle2MinValue>
<needle2Move>Rotate</needle2Move>
<needle2ObjectField>Altitude</needle2ObjectField>
<needle3DataObject>BaroAltitude</needle3DataObject>
<needle3DataObject>BaroSensor</needle3DataObject>
<needle3Factor>1</needle3Factor>
<needle3MaxValue>1000</needle3MaxValue>
<needle3MinValue>0</needle3MinValue>
@ -353,19 +353,19 @@
<dialForegroundID>foreground</dialForegroundID>
<dialNeedleID1>needle</dialNeedleID1>
<font>MS Shell Dlg 2,8.25,-1,5,50,0,0,0,0,0</font>
<needle1DataObject>BaroAltitude</needle1DataObject>
<needle1DataObject>BaroSensor</needle1DataObject>
<needle1Factor>10</needle1Factor>
<needle1MaxValue>1120</needle1MaxValue>
<needle1MinValue>1000</needle1MinValue>
<needle1Move>Rotate</needle1Move>
<needle1ObjectField>Pressure</needle1ObjectField>
<needle2DataObject>BaroAltitude</needle2DataObject>
<needle2DataObject>BaroSensor</needle2DataObject>
<needle2Factor>1</needle2Factor>
<needle2MaxValue>100</needle2MaxValue>
<needle2MinValue>0</needle2MinValue>
<needle2Move>Rotate</needle2Move>
<needle2ObjectField>Altitude</needle2ObjectField>
<needle3DataObject>BaroAltitude</needle3DataObject>
<needle3DataObject>BaroSensor</needle3DataObject>
<needle3Factor>1</needle3Factor>
<needle3MaxValue>1000</needle3MaxValue>
<needle3MinValue>0</needle3MinValue>
@ -386,19 +386,19 @@
<dialForegroundID>foreground</dialForegroundID>
<dialNeedleID1>needle</dialNeedleID1>
<font>MS Shell Dlg 2,8.25,-1,5,50,0,0,0,0,0</font>
<needle1DataObject>VelocityActual</needle1DataObject>
<needle1DataObject>VelocityState</needle1DataObject>
<needle1Factor>0.01</needle1Factor>
<needle1MaxValue>11.2</needle1MaxValue>
<needle1MinValue>-11.2</needle1MinValue>
<needle1Move>Rotate</needle1Move>
<needle1ObjectField>Down</needle1ObjectField>
<needle2DataObject>BaroAltitude</needle2DataObject>
<needle2DataObject>BaroSensor</needle2DataObject>
<needle2Factor>1</needle2Factor>
<needle2MaxValue>100</needle2MaxValue>
<needle2MinValue>0</needle2MinValue>
<needle2Move>Rotate</needle2Move>
<needle2ObjectField>Altitude</needle2ObjectField>
<needle3DataObject>BaroAltitude</needle3DataObject>
<needle3DataObject>BaroSensor</needle3DataObject>
<needle3Factor>1</needle3Factor>
<needle3MaxValue>1000</needle3MaxValue>
<needle3MinValue>0</needle3MinValue>
@ -419,19 +419,19 @@
<dialForegroundID>foreground</dialForegroundID>
<dialNeedleID1>needle</dialNeedleID1>
<font>MS Shell Dlg 2,8.25,-1,5,50,0,0,0,0,0</font>
<needle1DataObject>AttitudeActual</needle1DataObject>
<needle1DataObject>AttitudeState</needle1DataObject>
<needle1Factor>-1</needle1Factor>
<needle1MaxValue>360</needle1MaxValue>
<needle1MinValue>0</needle1MinValue>
<needle1Move>Rotate</needle1Move>
<needle1ObjectField>Yaw</needle1ObjectField>
<needle2DataObject>BaroAltitude</needle2DataObject>
<needle2DataObject>BaroSensor</needle2DataObject>
<needle2Factor>1</needle2Factor>
<needle2MaxValue>100</needle2MaxValue>
<needle2MinValue>0</needle2MinValue>
<needle2Move>Rotate</needle2Move>
<needle2ObjectField>Altitude</needle2ObjectField>
<needle3DataObject>BaroAltitude</needle3DataObject>
<needle3DataObject>BaroSensor</needle3DataObject>
<needle3Factor>1</needle3Factor>
<needle3MaxValue>1000</needle3MaxValue>
<needle3MinValue>0</needle3MinValue>
@ -452,19 +452,19 @@
<dialForegroundID>foreground</dialForegroundID>
<dialNeedleID1>needle</dialNeedleID1>
<font>MS Shell Dlg 2,8.25,-1,5,50,0,0,0,0,0</font>
<needle1DataObject>GPSPosition</needle1DataObject>
<needle1DataObject>GPSPositionSensor</needle1DataObject>
<needle1Factor>3.6</needle1Factor>
<needle1MaxValue>120</needle1MaxValue>
<needle1MinValue>0</needle1MinValue>
<needle1Move>Rotate</needle1Move>
<needle1ObjectField>Groundspeed</needle1ObjectField>
<needle2DataObject>BaroAltitude</needle2DataObject>
<needle2DataObject>BaroSensor</needle2DataObject>
<needle2Factor>1</needle2Factor>
<needle2MaxValue>100</needle2MaxValue>
<needle2MinValue>0</needle2MinValue>
<needle2Move>Rotate</needle2Move>
<needle2ObjectField>Altitude</needle2ObjectField>
<needle3DataObject>BaroAltitude</needle3DataObject>
<needle3DataObject>BaroSensor</needle3DataObject>
<needle3Factor>1</needle3Factor>
<needle3MaxValue>1000</needle3MaxValue>
<needle3MinValue>0</needle3MinValue>
@ -487,19 +487,19 @@
<dialNeedleID2>needle2</dialNeedleID2>
<dialNeedleID3>needle3</dialNeedleID3>
<font>MS Shell Dlg 2,8.25,-1,5,50,0,0,0,0,0</font>
<needle1DataObject>BaroAltitude</needle1DataObject>
<needle1DataObject>BaroSensor</needle1DataObject>
<needle1Factor>1</needle1Factor>
<needle1MaxValue>120</needle1MaxValue>
<needle1MinValue>0</needle1MinValue>
<needle1Move>Rotate</needle1Move>
<needle1ObjectField>Temperature</needle1ObjectField>
<needle2DataObject>BaroAltitude</needle2DataObject>
<needle2DataObject>BaroSensor</needle2DataObject>
<needle2Factor>1</needle2Factor>
<needle2MaxValue>100</needle2MaxValue>
<needle2MinValue>0</needle2MinValue>
<needle2Move>Rotate</needle2Move>
<needle2ObjectField>Altitude</needle2ObjectField>
<needle3DataObject>BaroAltitude</needle3DataObject>
<needle3DataObject>BaroSensor</needle3DataObject>
<needle3Factor>1</needle3Factor>
<needle3MaxValue>1000</needle3MaxValue>
<needle3MinValue>0</needle3MinValue>
@ -522,19 +522,19 @@
<dialNeedleID2>needle2</dialNeedleID2>
<dialNeedleID3>needle2</dialNeedleID3>
<font>MS Shell Dlg 2,8.25,-1,5,50,0,0,0,0,0</font>
<needle1DataObject>AttitudeActual</needle1DataObject>
<needle1DataObject>AttitudeState</needle1DataObject>
<needle1Factor>-1</needle1Factor>
<needle1MaxValue>360</needle1MaxValue>
<needle1MinValue>0</needle1MinValue>
<needle1Move>Rotate</needle1Move>
<needle1ObjectField>Roll</needle1ObjectField>
<needle2DataObject>Accels</needle2DataObject>
<needle2DataObject>AccelState</needle2DataObject>
<needle2Factor>1</needle2Factor>
<needle2MaxValue>20</needle2MaxValue>
<needle2MinValue>-20</needle2MinValue>
<needle2Move>Horizontal</needle2Move>
<needle2ObjectField>x</needle2ObjectField>
<needle3DataObject>Accels</needle3DataObject>
<needle3DataObject>AccelState</needle3DataObject>
<needle3Factor>-1</needle3Factor>
<needle3MaxValue>360</needle3MaxValue>
<needle3MinValue>0</needle3MinValue>
@ -554,19 +554,19 @@
<dialFile>%%DATAPATH%%dials/default/speed.svg</dialFile>
<dialNeedleID1>needle</dialNeedleID1>
<font>MS Shell Dlg 2,8.25,-1,5,50,0,0,0,0,0</font>
<needle1DataObject>GPSPosition</needle1DataObject>
<needle1DataObject>GPSPositionSensor</needle1DataObject>
<needle1Factor>3.6</needle1Factor>
<needle1MaxValue>120</needle1MaxValue>
<needle1MinValue>0</needle1MinValue>
<needle1Move>Rotate</needle1Move>
<needle1ObjectField>Groundspeed</needle1ObjectField>
<needle2DataObject>BaroAltitude</needle2DataObject>
<needle2DataObject>BaroSensor</needle2DataObject>
<needle2Factor>1</needle2Factor>
<needle2MaxValue>100</needle2MaxValue>
<needle2MinValue>0</needle2MinValue>
<needle2Move>Rotate</needle2Move>
<needle2ObjectField>Altitude</needle2ObjectField>
<needle3DataObject>BaroAltitude</needle3DataObject>
<needle3DataObject>BaroSensor</needle3DataObject>
<needle3Factor>1</needle3Factor>
<needle3MaxValue>1000</needle3MaxValue>
<needle3MinValue>0</needle3MinValue>
@ -589,19 +589,19 @@
<dialNeedleID2>needle</dialNeedleID2>
<dialNeedleID3>needle3</dialNeedleID3>
<font>MS Shell Dlg 2,8.25,-1,5,50,0,0,0,0,0</font>
<needle1DataObject>AttitudeActual</needle1DataObject>
<needle1DataObject>AttitudeState</needle1DataObject>
<needle1Factor>-1</needle1Factor>
<needle1MaxValue>360</needle1MaxValue>
<needle1MinValue>0</needle1MinValue>
<needle1Move>Rotate</needle1Move>
<needle1ObjectField>Roll</needle1ObjectField>
<needle2DataObject>AttitudeActual</needle2DataObject>
<needle2DataObject>AttitudeState</needle2DataObject>
<needle2Factor>75</needle2Factor>
<needle2MaxValue>20</needle2MaxValue>
<needle2MinValue>0</needle2MinValue>
<needle2Move>Vertical</needle2Move>
<needle2ObjectField>Pitch</needle2ObjectField>
<needle3DataObject>AttitudeActual</needle3DataObject>
<needle3DataObject>AttitudeState</needle3DataObject>
<needle3Factor>-1</needle3Factor>
<needle3MaxValue>360</needle3MaxValue>
<needle3MinValue>0</needle3MinValue>
@ -624,19 +624,19 @@
<dialNeedleID2>needle2</dialNeedleID2>
<dialNeedleID3>needle3</dialNeedleID3>
<font>MS Shell Dlg 2,8.25,-1,5,50,0,0,0,0,0</font>
<needle1DataObject>BaroAltitude</needle1DataObject>
<needle1DataObject>BaroSensor</needle1DataObject>
<needle1Factor>1</needle1Factor>
<needle1MaxValue>10</needle1MaxValue>
<needle1MinValue>0</needle1MinValue>
<needle1Move>Rotate</needle1Move>
<needle1ObjectField>Altitude</needle1ObjectField>
<needle2DataObject>BaroAltitude</needle2DataObject>
<needle2DataObject>BaroSensor</needle2DataObject>
<needle2Factor>1</needle2Factor>
<needle2MaxValue>100</needle2MaxValue>
<needle2MinValue>0</needle2MinValue>
<needle2Move>Rotate</needle2Move>
<needle2ObjectField>Altitude</needle2ObjectField>
<needle3DataObject>BaroAltitude</needle3DataObject>
<needle3DataObject>BaroSensor</needle3DataObject>
<needle3Factor>1</needle3Factor>
<needle3MaxValue>1000</needle3MaxValue>
<needle3MinValue>0</needle3MinValue>
@ -656,19 +656,19 @@
<dialFile>%%DATAPATH%%dials/hi-contrast/barometer.svg</dialFile>
<dialNeedleID1>needle</dialNeedleID1>
<font>MS Shell Dlg 2,8.25,-1,5,50,0,0,0,0,0</font>
<needle1DataObject>BaroAltitude</needle1DataObject>
<needle1DataObject>BaroSensor</needle1DataObject>
<needle1Factor>10</needle1Factor>
<needle1MaxValue>1120</needle1MaxValue>
<needle1MinValue>1000</needle1MinValue>
<needle1Move>Rotate</needle1Move>
<needle1ObjectField>Pressure</needle1ObjectField>
<needle2DataObject>BaroAltitude</needle2DataObject>
<needle2DataObject>BaroSensor</needle2DataObject>
<needle2Factor>1</needle2Factor>
<needle2MaxValue>100</needle2MaxValue>
<needle2MinValue>0</needle2MinValue>
<needle2Move>Rotate</needle2Move>
<needle2ObjectField>Altitude</needle2ObjectField>
<needle3DataObject>BaroAltitude</needle3DataObject>
<needle3DataObject>BaroSensor</needle3DataObject>
<needle3Factor>1</needle3Factor>
<needle3MaxValue>1000</needle3MaxValue>
<needle3MinValue>0</needle3MinValue>
@ -688,19 +688,19 @@
<dialFile>%%DATAPATH%%dials/hi-contrast/vsi.svg</dialFile>
<dialNeedleID1>needle</dialNeedleID1>
<font>MS Shell Dlg 2,8.25,-1,5,50,0,0,0,0,0</font>
<needle1DataObject>VelocityActual</needle1DataObject>
<needle1DataObject>VelocityState</needle1DataObject>
<needle1Factor>0.01</needle1Factor>
<needle1MaxValue>12</needle1MaxValue>
<needle1MinValue>-12</needle1MinValue>
<needle1Move>Rotate</needle1Move>
<needle1ObjectField>Down</needle1ObjectField>
<needle2DataObject>BaroAltitude</needle2DataObject>
<needle2DataObject>BaroSensor</needle2DataObject>
<needle2Factor>1</needle2Factor>
<needle2MaxValue>100</needle2MaxValue>
<needle2MinValue>0</needle2MinValue>
<needle2Move>Rotate</needle2Move>
<needle2ObjectField>Altitude</needle2ObjectField>
<needle3DataObject>BaroAltitude</needle3DataObject>
<needle3DataObject>BaroSensor</needle3DataObject>
<needle3Factor>1</needle3Factor>
<needle3MaxValue>1000</needle3MaxValue>
<needle3MinValue>0</needle3MinValue>
@ -721,19 +721,19 @@
<dialForegroundID>foreground</dialForegroundID>
<dialNeedleID1>needle</dialNeedleID1>
<font>MS Shell Dlg 2,8.25,-1,5,50,0,0,0,0,0</font>
<needle1DataObject>AttitudeActual</needle1DataObject>
<needle1DataObject>AttitudeState</needle1DataObject>
<needle1Factor>-1</needle1Factor>
<needle1MaxValue>360</needle1MaxValue>
<needle1MinValue>0</needle1MinValue>
<needle1Move>Rotate</needle1Move>
<needle1ObjectField>Yaw</needle1ObjectField>
<needle2DataObject>BaroAltitude</needle2DataObject>
<needle2DataObject>BaroSensor</needle2DataObject>
<needle2Factor>1</needle2Factor>
<needle2MaxValue>100</needle2MaxValue>
<needle2MinValue>0</needle2MinValue>
<needle2Move>Rotate</needle2Move>
<needle2ObjectField>Altitude</needle2ObjectField>
<needle3DataObject>BaroAltitude</needle3DataObject>
<needle3DataObject>BaroSensor</needle3DataObject>
<needle3Factor>1</needle3Factor>
<needle3MaxValue>1000</needle3MaxValue>
<needle3MinValue>0</needle3MinValue>
@ -753,19 +753,19 @@
<dialFile>%%DATAPATH%%dials/hi-contrast/speed.svg</dialFile>
<dialNeedleID1>needle</dialNeedleID1>
<font>MS Shell Dlg 2,8.25,-1,5,50,0,0,0,0,0</font>
<needle1DataObject>GPSPosition</needle1DataObject>
<needle1DataObject>GPSPositionSensor</needle1DataObject>
<needle1Factor>3.6</needle1Factor>
<needle1MaxValue>120</needle1MaxValue>
<needle1MinValue>0</needle1MinValue>
<needle1Move>Rotate</needle1Move>
<needle1ObjectField>Groundspeed</needle1ObjectField>
<needle2DataObject>BaroAltitude</needle2DataObject>
<needle2DataObject>BaroSensor</needle2DataObject>
<needle2Factor>1</needle2Factor>
<needle2MaxValue>100</needle2MaxValue>
<needle2MinValue>0</needle2MinValue>
<needle2Move>Rotate</needle2Move>
<needle2ObjectField>Altitude</needle2ObjectField>
<needle3DataObject>BaroAltitude</needle3DataObject>
<needle3DataObject>BaroSensor</needle3DataObject>
<needle3Factor>1</needle3Factor>
<needle3MaxValue>1000</needle3MaxValue>
<needle3MinValue>0</needle3MinValue>
@ -787,19 +787,19 @@
<dialNeedleID2>needle2</dialNeedleID2>
<dialNeedleID3>needle3</dialNeedleID3>
<font>MS Shell Dlg 2,8.25,-1,5,50,0,0,0,0,0</font>
<needle1DataObject>BaroAltitude</needle1DataObject>
<needle1DataObject>BaroSensor</needle1DataObject>
<needle1Factor>1</needle1Factor>
<needle1MaxValue>120</needle1MaxValue>
<needle1MinValue>0</needle1MinValue>
<needle1Move>Rotate</needle1Move>
<needle1ObjectField>Temperature</needle1ObjectField>
<needle2DataObject>BaroAltitude</needle2DataObject>
<needle2DataObject>BaroSensor</needle2DataObject>
<needle2Factor>1</needle2Factor>
<needle2MaxValue>100</needle2MaxValue>
<needle2MinValue>0</needle2MinValue>
<needle2Move>Rotate</needle2Move>
<needle2ObjectField>Altitude</needle2ObjectField>
<needle3DataObject>BaroAltitude</needle3DataObject>
<needle3DataObject>BaroSensor</needle3DataObject>
<needle3Factor>1</needle3Factor>
<needle3MaxValue>1000</needle3MaxValue>
<needle3MinValue>0</needle3MinValue>
@ -827,13 +827,13 @@
<needle1MinValue>1000</needle1MinValue>
<needle1Move>Rotate</needle1Move>
<needle1ObjectField>Channel-3</needle1ObjectField>
<needle2DataObject>BaroAltitude</needle2DataObject>
<needle2DataObject>BaroSensor</needle2DataObject>
<needle2Factor>1</needle2Factor>
<needle2MaxValue>100</needle2MaxValue>
<needle2MinValue>0</needle2MinValue>
<needle2Move>Rotate</needle2Move>
<needle2ObjectField>Altitude</needle2ObjectField>
<needle3DataObject>BaroAltitude</needle3DataObject>
<needle3DataObject>BaroSensor</needle3DataObject>
<needle3Factor>1</needle3Factor>
<needle3MaxValue>1000</needle3MaxValue>
<needle3MinValue>0</needle3MinValue>
@ -855,19 +855,19 @@
<dialNeedleID2>needle2</dialNeedleID2>
<dialNeedleID3>needle3</dialNeedleID3>
<font>MS Shell Dlg 2,8.25,-1,5,50,0,0,0,0,0</font>
<needle1DataObject>BaroAltitude</needle1DataObject>
<needle1DataObject>BaroSensor</needle1DataObject>
<needle1Factor>1</needle1Factor>
<needle1MaxValue>120</needle1MaxValue>
<needle1MinValue>0</needle1MinValue>
<needle1Move>Rotate</needle1Move>
<needle1ObjectField>Temperature</needle1ObjectField>
<needle2DataObject>BaroAltitude</needle2DataObject>
<needle2DataObject>BaroSensor</needle2DataObject>
<needle2Factor>1</needle2Factor>
<needle2MaxValue>100</needle2MaxValue>
<needle2MinValue>0</needle2MinValue>
<needle2Move>Rotate</needle2Move>
<needle2ObjectField>Altitude</needle2ObjectField>
<needle3DataObject>BaroAltitude</needle3DataObject>
<needle3DataObject>BaroSensor</needle3DataObject>
<needle3Factor>1</needle3Factor>
<needle3MaxValue>1000</needle3MaxValue>
<needle3MinValue>0</needle3MinValue>
@ -968,7 +968,7 @@
<attActCalc>false</attActCalc>
<attActHW>false</attActHW>
<attActSim>true</attActSim>
<attActualEnabled>true</attActualEnabled>
<attStateEnabled>true</attStateEnabled>
<attRawEnabled>true</attRawEnabled>
<attRawRate>20</attRawRate>
<baroAltRate>50</baroAltRate>
@ -1004,7 +1004,7 @@
<attActCalc>false</attActCalc>
<attActHW>false</attActHW>
<attActSim>true</attActSim>
<attActualEnabled>true</attActualEnabled>
<attStateEnabled>true</attStateEnabled>
<attRawEnabled>true</attRawEnabled>
<attRawRate>20</attRawRate>
<baroAltRate>50</baroAltRate>
@ -1040,7 +1040,7 @@
<attActCalc>false</attActCalc>
<attActHW>false</attActHW>
<attActSim>true</attActSim>
<attActualEnabled>true</attActualEnabled>
<attStateEnabled>true</attStateEnabled>
<attRawEnabled>true</attRawEnabled>
<attRawRate>20</attRawRate>
<baroAltRate>50</baroAltRate>
@ -1084,7 +1084,7 @@
<minValue>-11</minValue>
<redMax>11</redMax>
<redMin>-11</redMin>
<sourceDataObject>Accels</sourceDataObject>
<sourceDataObject>AccelState</sourceDataObject>
<sourceObjectField>x</sourceObjectField>
<useOpenGLFlag>false</useOpenGLFlag>
<yellowMax>-5</yellowMax>
@ -1107,7 +1107,7 @@
<minValue>-11</minValue>
<redMax>11</redMax>
<redMin>-11</redMin>
<sourceDataObject>Accels</sourceDataObject>
<sourceDataObject>AccelState</sourceDataObject>
<sourceObjectField>y</sourceObjectField>
<useOpenGLFlag>false</useOpenGLFlag>
<yellowMax>-5</yellowMax>
@ -1130,7 +1130,7 @@
<minValue>-11</minValue>
<redMax>11</redMax>
<redMin>-11</redMin>
<sourceDataObject>Accels</sourceDataObject>
<sourceDataObject>AccelState</sourceDataObject>
<sourceObjectField>z</sourceObjectField>
<useOpenGLFlag>false</useOpenGLFlag>
<yellowMax>-5</yellowMax>
@ -1222,7 +1222,7 @@
<minValue>0</minValue>
<redMax>0</redMax>
<redMin>0</redMin>
<sourceDataObject>GPSPosition</sourceDataObject>
<sourceDataObject>GPSPositionSensor</sourceDataObject>
<sourceObjectField>Satellites</sourceObjectField>
<useOpenGLFlag>false</useOpenGLFlag>
<yellowMax>0</yellowMax>
@ -1245,7 +1245,7 @@
<minValue>0</minValue>
<redMax>33</redMax>
<redMin>0</redMin>
<sourceDataObject>GPSPosition</sourceDataObject>
<sourceDataObject>GPSPositionSensor</sourceDataObject>
<sourceObjectField>Status</sourceObjectField>
<useOpenGLFlag>false</useOpenGLFlag>
<yellowMax>66</yellowMax>
@ -1321,7 +1321,7 @@
<yellowMin>-0.8</yellowMin>
</data>
</Pitch>
<PitchActual>
<PitchState>
<configInfo>
<locked>false</locked>
<version>0.0.0</version>
@ -1337,13 +1337,13 @@
<minValue>-90</minValue>
<redMax>1</redMax>
<redMin>0</redMin>
<sourceDataObject>AttitudeActual</sourceDataObject>
<sourceDataObject>AttitudeState</sourceDataObject>
<sourceObjectField>Pitch</sourceObjectField>
<useOpenGLFlag>false</useOpenGLFlag>
<yellowMax>0.9</yellowMax>
<yellowMin>0.1</yellowMin>
</data>
</PitchActual>
</PitchState>
<Roll__PCT__20Desired>
<configInfo>
<locked>false</locked>
@ -1888,7 +1888,7 @@
<color>4294901760</color>
<mathFunction>None</mathFunction>
<uavField>x</uavField>
<uavObject>Accels</uavObject>
<uavObject>AccelState</uavObject>
<yMaximum>0</yMaximum>
<yMeanSamples>1</yMeanSamples>
<yMinimum>0</yMinimum>
@ -1898,7 +1898,7 @@
<color>4283782655</color>
<mathFunction>None</mathFunction>
<uavField>y</uavField>
<uavObject>Accels</uavObject>
<uavObject>AccelState</uavObject>
<yMaximum>0</yMaximum>
<yMeanSamples>1</yMeanSamples>
<yMinimum>0</yMinimum>
@ -1908,7 +1908,7 @@
<color>4283804160</color>
<mathFunction>None</mathFunction>
<uavField>z</uavField>
<uavObject>Accels</uavObject>
<uavObject>AccelState</uavObject>
<yMaximum>0</yMaximum>
<yMeanSamples>1</yMeanSamples>
<yMinimum>0</yMinimum>
@ -1988,7 +1988,7 @@
<color>4283760895</color>
<mathFunction>None</mathFunction>
<uavField>Roll</uavField>
<uavObject>AttitudeActual</uavObject>
<uavObject>AttitudeState</uavObject>
<yMaximum>0</yMaximum>
<yMeanSamples>1</yMeanSamples>
<yMinimum>0</yMinimum>
@ -1998,7 +1998,7 @@
<color>4278233600</color>
<mathFunction>None</mathFunction>
<uavField>Yaw</uavField>
<uavObject>AttitudeActual</uavObject>
<uavObject>AttitudeState</uavObject>
<yMaximum>0</yMaximum>
<yMeanSamples>1</yMeanSamples>
<yMinimum>0</yMinimum>
@ -2008,7 +2008,7 @@
<color>4294901760</color>
<mathFunction>None</mathFunction>
<uavField>Pitch</uavField>
<uavObject>AttitudeActual</uavObject>
<uavObject>AttitudeState</uavObject>
<yMaximum>0</yMaximum>
<yMeanSamples>1</yMeanSamples>
<yMinimum>0</yMinimum>
@ -2033,7 +2033,7 @@
<color>4278190080</color>
<mathFunction>None</mathFunction>
<uavField>Pressure</uavField>
<uavObject>BaroAltitude</uavObject>
<uavObject>BaroSensor</uavObject>
<yMaximum>0</yMaximum>
<yMeanSamples>1</yMeanSamples>
<yMinimum>0</yMinimum>
@ -2139,7 +2139,7 @@
<refreshInterval>200</refreshInterval>
</data>
</Inputs>
<Raw__PCT__20Accels>
<Raw__PCT__20AccelState>
<configInfo>
<locked>false</locked>
<version>0.0.0</version>
@ -2153,7 +2153,7 @@
<color>4294901760</color>
<mathFunction>None</mathFunction>
<uavField>x</uavField>
<uavObject>Accels</uavObject>
<uavObject>AccelState</uavObject>
<yMaximum>0</yMaximum>
<yMeanSamples>1</yMeanSamples>
<yMinimum>0</yMinimum>
@ -2163,7 +2163,7 @@
<color>4283782655</color>
<mathFunction>None</mathFunction>
<uavField>y</uavField>
<uavObject>Accels</uavObject>
<uavObject>AccelState</uavObject>
<yMaximum>0</yMaximum>
<yMeanSamples>1</yMeanSamples>
<yMinimum>0</yMinimum>
@ -2173,7 +2173,7 @@
<color>4283804160</color>
<mathFunction>None</mathFunction>
<uavField>z</uavField>
<uavObject>Accels</uavObject>
<uavObject>AccelState</uavObject>
<yMaximum>0</yMaximum>
<yMeanSamples>1</yMeanSamples>
<yMinimum>0</yMinimum>
@ -2183,8 +2183,8 @@
<plotType>1</plotType>
<refreshInterval>500</refreshInterval>
</data>
</Raw__PCT__20Accels>
<Raw__PCT__20Gyros>
</Raw__PCT__20AccelState>
<Raw__PCT__20GyroState>
<configInfo>
<locked>false</locked>
<version>0.0.0</version>
@ -2198,7 +2198,7 @@
<color>4283804160</color>
<mathFunction>None</mathFunction>
<uavField>z</uavField>
<uavObject>Gyros</uavObject>
<uavObject>GyroState</uavObject>
<yMaximum>0</yMaximum>
<yMeanSamples>1</yMeanSamples>
<yMinimum>0</yMinimum>
@ -2208,7 +2208,7 @@
<color>4283782655</color>
<mathFunction>None</mathFunction>
<uavField>y</uavField>
<uavObject>Gyros</uavObject>
<uavObject>GyroState</uavObject>
<yMaximum>0</yMaximum>
<yMeanSamples>1</yMeanSamples>
<yMinimum>0</yMinimum>
@ -2218,7 +2218,7 @@
<color>4294901760</color>
<mathFunction>None</mathFunction>
<uavField>x</uavField>
<uavObject>Gyros</uavObject>
<uavObject>GyroState</uavObject>
<yMaximum>0</yMaximum>
<yMeanSamples>1</yMeanSamples>
<yMinimum>0</yMinimum>
@ -2228,7 +2228,7 @@
<plotType>1</plotType>
<refreshInterval>500</refreshInterval>
</data>
</Raw__PCT__20Gyros>
</Raw__PCT__20GyroState>
<Raw__PCT__20magnetometers>
<configInfo>
<locked>false</locked>
@ -2243,7 +2243,7 @@
<color>4294901760</color>
<mathFunction>None</mathFunction>
<uavField>x</uavField>
<uavObject>Magnetometer</uavObject>
<uavObject>MagState</uavObject>
<yMaximum>0</yMaximum>
<yMeanSamples>1</yMeanSamples>
<yMinimum>0</yMinimum>
@ -2253,7 +2253,7 @@
<color>4283782655</color>
<mathFunction>None</mathFunction>
<uavField>y</uavField>
<uavObject>Magnetometer</uavObject>
<uavObject>MagState</uavObject>
<yMaximum>0</yMaximum>
<yMeanSamples>1</yMeanSamples>
<yMinimum>0</yMinimum>
@ -2263,7 +2263,7 @@
<color>4283804160</color>
<mathFunction>None</mathFunction>
<uavField>z</uavField>
<uavObject>Magnetometer</uavObject>
<uavObject>MagState</uavObject>
<yMaximum>0</yMaximum>
<yMeanSamples>1</yMeanSamples>
<yMinimum>0</yMinimum>
@ -2654,7 +2654,7 @@
<side1>
<classId>ScopeGadget</classId>
<gadget>
<activeConfiguration>Raw Gyros</activeConfiguration>
<activeConfiguration>Raw GyroState</activeConfiguration>
</gadget>
<type>uavGadget</type>
</side1>

200
ground/openpilotgcs/share/openpilotgcs/default_configurations/OpenPilotGCS_wide.xml
View File

@ -115,19 +115,19 @@
<dialNeedleID2>needle</dialNeedleID2>
<dialNeedleID3>needle3</dialNeedleID3>
<font>MS Shell Dlg 2,8.25,-1,5,50,0,0,0,0,0</font>
<needle1DataObject>AttitudeActual</needle1DataObject>
<needle1DataObject>AttitudeState</needle1DataObject>
<needle1Factor>-1</needle1Factor>
<needle1MaxValue>360</needle1MaxValue>
<needle1MinValue>0</needle1MinValue>
<needle1Move>Rotate</needle1Move>
<needle1ObjectField>Roll</needle1ObjectField>
<needle2DataObject>AttitudeActual</needle2DataObject>
<needle2DataObject>AttitudeState</needle2DataObject>
<needle2Factor>75</needle2Factor>
<needle2MaxValue>20</needle2MaxValue>
<needle2MinValue>0</needle2MinValue>
<needle2Move>Vertical</needle2Move>
<needle2ObjectField>Pitch</needle2ObjectField>
<needle3DataObject>AttitudeActual</needle3DataObject>
<needle3DataObject>AttitudeState</needle3DataObject>
<needle3Factor>-1</needle3Factor>
<needle3MaxValue>360</needle3MaxValue>
<needle3MinValue>0</needle3MinValue>
@ -150,19 +150,19 @@
<dialNeedleID2>needle2</dialNeedleID2>
<dialNeedleID3>needle3</dialNeedleID3>
<font>MS Shell Dlg 2,8.25,-1,5,50,0,0,0,0,0</font>
<needle1DataObject>BaroAltitude</needle1DataObject>
<needle1DataObject>BaroSensor</needle1DataObject>
<needle1Factor>1</needle1Factor>
<needle1MaxValue>10</needle1MaxValue>
<needle1MinValue>0</needle1MinValue>
<needle1Move>Rotate</needle1Move>
<needle1ObjectField>Altitude</needle1ObjectField>
<needle2DataObject>BaroAltitude</needle2DataObject>
<needle2DataObject>BaroSensor</needle2DataObject>
<needle2Factor>1</needle2Factor>
<needle2MaxValue>100</needle2MaxValue>
<needle2MinValue>0</needle2MinValue>
<needle2Move>Rotate</needle2Move>
<needle2ObjectField>Altitude</needle2ObjectField>
<needle3DataObject>BaroAltitude</needle3DataObject>
<needle3DataObject>BaroSensor</needle3DataObject>
<needle3Factor>1</needle3Factor>
<needle3MaxValue>1000</needle3MaxValue>
<needle3MinValue>0</needle3MinValue>
@ -182,19 +182,19 @@
<dialFile>%%DATAPATH%%dials/default/barometer.svg</dialFile>
<dialNeedleID1>needle</dialNeedleID1>
<font>MS Shell Dlg 2,8.25,-1,5,50,0,0,0,0,0</font>
<needle1DataObject>BaroAltitude</needle1DataObject>
<needle1DataObject>BaroSensor</needle1DataObject>
<needle1Factor>10</needle1Factor>
<needle1MaxValue>1120</needle1MaxValue>
<needle1MinValue>1000</needle1MinValue>
<needle1Move>Rotate</needle1Move>
<needle1ObjectField>Pressure</needle1ObjectField>
<needle2DataObject>BaroAltitude</needle2DataObject>
<needle2DataObject>BaroSensor</needle2DataObject>
<needle2Factor>1</needle2Factor>
<needle2MaxValue>100</needle2MaxValue>
<needle2MinValue>0</needle2MinValue>
<needle2Move>Rotate</needle2Move>
<needle2ObjectField>Altitude</needle2ObjectField>
<needle3DataObject>BaroAltitude</needle3DataObject>
<needle3DataObject>BaroSensor</needle3DataObject>
<needle3Factor>1</needle3Factor>
<needle3MaxValue>1000</needle3MaxValue>
<needle3MinValue>0</needle3MinValue>
@ -214,19 +214,19 @@
<dialFile>%%DATAPATH%%dials/default/vsi.svg</dialFile>
<dialNeedleID1>needle</dialNeedleID1>
<font>MS Shell Dlg 2,8.25,-1,5,50,0,0,0,0,0</font>
<needle1DataObject>VelocityActual</needle1DataObject>
<needle1DataObject>VelocityState</needle1DataObject>
<needle1Factor>0.01</needle1Factor>
<needle1MaxValue>12</needle1MaxValue>
<needle1MinValue>-12</needle1MinValue>
<needle1Move>Rotate</needle1Move>
<needle1ObjectField>Down</needle1ObjectField>
<needle2DataObject>BaroAltitude</needle2DataObject>
<needle2DataObject>BaroSensor</needle2DataObject>
<needle2Factor>1</needle2Factor>
<needle2MaxValue>100</needle2MaxValue>
<needle2MinValue>0</needle2MinValue>
<needle2Move>Rotate</needle2Move>
<needle2ObjectField>Altitude</needle2ObjectField>
<needle3DataObject>BaroAltitude</needle3DataObject>
<needle3DataObject>BaroSensor</needle3DataObject>
<needle3Factor>1</needle3Factor>
<needle3MaxValue>1000</needle3MaxValue>
<needle3MinValue>0</needle3MinValue>
@ -247,19 +247,19 @@
<dialForegroundID>foreground</dialForegroundID>
<dialNeedleID1>needle</dialNeedleID1>
<font>MS Shell Dlg 2,8.25,-1,5,50,0,0,0,0,0</font>
<needle1DataObject>AttitudeActual</needle1DataObject>
<needle1DataObject>AttitudeState</needle1DataObject>
<needle1Factor>-1</needle1Factor>
<needle1MaxValue>360</needle1MaxValue>
<needle1MinValue>0</needle1MinValue>
<needle1Move>Rotate</needle1Move>
<needle1ObjectField>Yaw</needle1ObjectField>
<needle2DataObject>BaroAltitude</needle2DataObject>
<needle2DataObject>BaroSensor</needle2DataObject>
<needle2Factor>1</needle2Factor>
<needle2MaxValue>100</needle2MaxValue>
<needle2MinValue>0</needle2MinValue>
<needle2Move>Rotate</needle2Move>
<needle2ObjectField>Altitude</needle2ObjectField>
<needle3DataObject>BaroAltitude</needle3DataObject>
<needle3DataObject>BaroSensor</needle3DataObject>
<needle3Factor>1</needle3Factor>
<needle3MaxValue>1000</needle3MaxValue>
<needle3MinValue>0</needle3MinValue>
@ -282,19 +282,19 @@
<dialNeedleID2>needle</dialNeedleID2>
<dialNeedleID3>needle3</dialNeedleID3>
<font>MS Shell Dlg 2,8.25,-1,5,50,0,0,0,0,0</font>
<needle1DataObject>AttitudeActual</needle1DataObject>
<needle1DataObject>AttitudeState</needle1DataObject>
<needle1Factor>-1</needle1Factor>
<needle1MaxValue>360</needle1MaxValue>
<needle1MinValue>0</needle1MinValue>
<needle1Move>Rotate</needle1Move>
<needle1ObjectField>Roll</needle1ObjectField>
<needle2DataObject>AttitudeActual</needle2DataObject>
<needle2DataObject>AttitudeState</needle2DataObject>
<needle2Factor>75</needle2Factor>
<needle2MaxValue>20</needle2MaxValue>
<needle2MinValue>0</needle2MinValue>
<needle2Move>Vertical</needle2Move>
<needle2ObjectField>Pitch</needle2ObjectField>
<needle3DataObject>AttitudeActual</needle3DataObject>
<needle3DataObject>AttitudeState</needle3DataObject>
<needle3Factor>-1</needle3Factor>
<needle3MaxValue>360</needle3MaxValue>
<needle3MinValue>0</needle3MinValue>
@ -317,19 +317,19 @@
<dialNeedleID2>needle2</dialNeedleID2>
<dialNeedleID3>needle3</dialNeedleID3>
<font>MS Shell Dlg 2,8.25,-1,5,50,0,0,0,0,0</font>
<needle1DataObject>BaroAltitude</needle1DataObject>
<needle1DataObject>BaroSensor</needle1DataObject>
<needle1Factor>1</needle1Factor>
<needle1MaxValue>10</needle1MaxValue>
<needle1MinValue>0</needle1MinValue>
<needle1Move>Rotate</needle1Move>
<needle1ObjectField>Altitude</needle1ObjectField>
<needle2DataObject>BaroAltitude</needle2DataObject>
<needle2DataObject>BaroSensor</needle2DataObject>
<needle2Factor>1</needle2Factor>
<needle2MaxValue>100</needle2MaxValue>
<needle2MinValue>0</needle2MinValue>
<needle2Move>Rotate</needle2Move>
<needle2ObjectField>Altitude</needle2ObjectField>
<needle3DataObject>BaroAltitude</needle3DataObject>
<needle3DataObject>BaroSensor</needle3DataObject>
<needle3Factor>1</needle3Factor>
<needle3MaxValue>1000</needle3MaxValue>
<needle3MinValue>0</needle3MinValue>
@ -350,19 +350,19 @@
<dialForegroundID>foreground</dialForegroundID>
<dialNeedleID1>needle</dialNeedleID1>
<font>MS Shell Dlg 2,8.25,-1,5,50,0,0,0,0,0</font>
<needle1DataObject>BaroAltitude</needle1DataObject>
<needle1DataObject>BaroSensor</needle1DataObject>
<needle1Factor>10</needle1Factor>
<needle1MaxValue>1120</needle1MaxValue>
<needle1MinValue>1000</needle1MinValue>
<needle1Move>Rotate</needle1Move>
<needle1ObjectField>Pressure</needle1ObjectField>
<needle2DataObject>BaroAltitude</needle2DataObject>
<needle2DataObject>BaroSensor</needle2DataObject>
<needle2Factor>1</needle2Factor>
<needle2MaxValue>100</needle2MaxValue>
<needle2MinValue>0</needle2MinValue>
<needle2Move>Rotate</needle2Move>
<needle2ObjectField>Altitude</needle2ObjectField>
<needle3DataObject>BaroAltitude</needle3DataObject>
<needle3DataObject>BaroSensor</needle3DataObject>
<needle3Factor>1</needle3Factor>
<needle3MaxValue>1000</needle3MaxValue>
<needle3MinValue>0</needle3MinValue>
@ -383,19 +383,19 @@
<dialForegroundID>foreground</dialForegroundID>
<dialNeedleID1>needle</dialNeedleID1>
<font>MS Shell Dlg 2,8.25,-1,5,50,0,0,0,0,0</font>
<needle1DataObject>VelocityActual</needle1DataObject>
<needle1DataObject>VelocityState</needle1DataObject>
<needle1Factor>0.01</needle1Factor>
<needle1MaxValue>11.2</needle1MaxValue>
<needle1MinValue>-11.2</needle1MinValue>
<needle1Move>Rotate</needle1Move>
<needle1ObjectField>Down</needle1ObjectField>
<needle2DataObject>BaroAltitude</needle2DataObject>
<needle2DataObject>BaroSensor</needle2DataObject>
<needle2Factor>1</needle2Factor>
<needle2MaxValue>100</needle2MaxValue>
<needle2MinValue>0</needle2MinValue>
<needle2Move>Rotate</needle2Move>
<needle2ObjectField>Altitude</needle2ObjectField>
<needle3DataObject>BaroAltitude</needle3DataObject>
<needle3DataObject>BaroSensor</needle3DataObject>
<needle3Factor>1</needle3Factor>
<needle3MaxValue>1000</needle3MaxValue>
<needle3MinValue>0</needle3MinValue>
@ -416,19 +416,19 @@
<dialForegroundID>foreground</dialForegroundID>
<dialNeedleID1>needle</dialNeedleID1>
<font>MS Shell Dlg 2,8.25,-1,5,50,0,0,0,0,0</font>
<needle1DataObject>AttitudeActual</needle1DataObject>
<needle1DataObject>AttitudeState</needle1DataObject>
<needle1Factor>-1</needle1Factor>
<needle1MaxValue>360</needle1MaxValue>
<needle1MinValue>0</needle1MinValue>
<needle1Move>Rotate</needle1Move>
<needle1ObjectField>Yaw</needle1ObjectField>
<needle2DataObject>BaroAltitude</needle2DataObject>
<needle2DataObject>BaroSensor</needle2DataObject>
<needle2Factor>1</needle2Factor>
<needle2MaxValue>100</needle2MaxValue>
<needle2MinValue>0</needle2MinValue>
<needle2Move>Rotate</needle2Move>
<needle2ObjectField>Altitude</needle2ObjectField>
<needle3DataObject>BaroAltitude</needle3DataObject>
<needle3DataObject>BaroSensor</needle3DataObject>
<needle3Factor>1</needle3Factor>
<needle3MaxValue>1000</needle3MaxValue>
<needle3MinValue>0</needle3MinValue>
@ -449,19 +449,19 @@
<dialForegroundID>foreground</dialForegroundID>
<dialNeedleID1>needle</dialNeedleID1>
<font>MS Shell Dlg 2,8.25,-1,5,50,0,0,0,0,0</font>
<needle1DataObject>GPSPosition</needle1DataObject>
<needle1DataObject>GPSPositionSensor</needle1DataObject>
<needle1Factor>3.6</needle1Factor>
<needle1MaxValue>120</needle1MaxValue>
<needle1MinValue>0</needle1MinValue>
<needle1Move>Rotate</needle1Move>
<needle1ObjectField>Groundspeed</needle1ObjectField>
<needle2DataObject>BaroAltitude</needle2DataObject>
<needle2DataObject>BaroSensor</needle2DataObject>
<needle2Factor>1</needle2Factor>
<needle2MaxValue>100</needle2MaxValue>
<needle2MinValue>0</needle2MinValue>
<needle2Move>Rotate</needle2Move>
<needle2ObjectField>Altitude</needle2ObjectField>
<needle3DataObject>BaroAltitude</needle3DataObject>
<needle3DataObject>BaroSensor</needle3DataObject>
<needle3Factor>1</needle3Factor>
<needle3MaxValue>1000</needle3MaxValue>
<needle3MinValue>0</needle3MinValue>
@ -484,19 +484,19 @@
<dialNeedleID2>needle2</dialNeedleID2>
<dialNeedleID3>needle3</dialNeedleID3>
<font>MS Shell Dlg 2,8.25,-1,5,50,0,0,0,0,0</font>
<needle1DataObject>BaroAltitude</needle1DataObject>
<needle1DataObject>BaroSensor</needle1DataObject>
<needle1Factor>1</needle1Factor>
<needle1MaxValue>120</needle1MaxValue>
<needle1MinValue>0</needle1MinValue>
<needle1Move>Rotate</needle1Move>
<needle1ObjectField>Temperature</needle1ObjectField>
<needle2DataObject>BaroAltitude</needle2DataObject>
<needle2DataObject>BaroSensor</needle2DataObject>
<needle2Factor>1</needle2Factor>
<needle2MaxValue>100</needle2MaxValue>
<needle2MinValue>0</needle2MinValue>
<needle2Move>Rotate</needle2Move>
<needle2ObjectField>Altitude</needle2ObjectField>
<needle3DataObject>BaroAltitude</needle3DataObject>
<needle3DataObject>BaroSensor</needle3DataObject>
<needle3Factor>1</needle3Factor>
<needle3MaxValue>1000</needle3MaxValue>
<needle3MinValue>0</needle3MinValue>
@ -519,19 +519,19 @@
<dialNeedleID2>needle2</dialNeedleID2>
<dialNeedleID3>needle2</dialNeedleID3>
<font>MS Shell Dlg 2,8.25,-1,5,50,0,0,0,0,0</font>
<needle1DataObject>AttitudeActual</needle1DataObject>
<needle1DataObject>AttitudeState</needle1DataObject>
<needle1Factor>-1</needle1Factor>
<needle1MaxValue>360</needle1MaxValue>
<needle1MinValue>0</needle1MinValue>
<needle1Move>Rotate</needle1Move>
<needle1ObjectField>Roll</needle1ObjectField>
<needle2DataObject>Accels</needle2DataObject>
<needle2DataObject>AccelState</needle2DataObject>
<needle2Factor>1</needle2Factor>
<needle2MaxValue>20</needle2MaxValue>
<needle2MinValue>-20</needle2MinValue>
<needle2Move>Horizontal</needle2Move>
<needle2ObjectField>x</needle2ObjectField>
<needle3DataObject>Accels</needle3DataObject>
<needle3DataObject>AccelState</needle3DataObject>
<needle3Factor>-1</needle3Factor>
<needle3MaxValue>360</needle3MaxValue>
<needle3MinValue>0</needle3MinValue>
@ -551,19 +551,19 @@
<dialFile>%%DATAPATH%%dials/default/speed.svg</dialFile>
<dialNeedleID1>needle</dialNeedleID1>
<font>MS Shell Dlg 2,8.25,-1,5,50,0,0,0,0,0</font>
<needle1DataObject>GPSPosition</needle1DataObject>
<needle1DataObject>GPSPositionSensor</needle1DataObject>
<needle1Factor>3.6</needle1Factor>
<needle1MaxValue>120</needle1MaxValue>
<needle1MinValue>0</needle1MinValue>
<needle1Move>Rotate</needle1Move>
<needle1ObjectField>Groundspeed</needle1ObjectField>
<needle2DataObject>BaroAltitude</needle2DataObject>
<needle2DataObject>BaroSensor</needle2DataObject>
<needle2Factor>1</needle2Factor>
<needle2MaxValue>100</needle2MaxValue>
<needle2MinValue>0</needle2MinValue>
<needle2Move>Rotate</needle2Move>
<needle2ObjectField>Altitude</needle2ObjectField>
<needle3DataObject>BaroAltitude</needle3DataObject>
<needle3DataObject>BaroSensor</needle3DataObject>
<needle3Factor>1</needle3Factor>
<needle3MaxValue>1000</needle3MaxValue>
<needle3MinValue>0</needle3MinValue>
@ -586,19 +586,19 @@
<dialNeedleID2>needle</dialNeedleID2>
<dialNeedleID3>needle3</dialNeedleID3>
<font>MS Shell Dlg 2,8.25,-1,5,50,0,0,0,0,0</font>
<needle1DataObject>AttitudeActual</needle1DataObject>
<needle1DataObject>AttitudeState</needle1DataObject>
<needle1Factor>-1</needle1Factor>
<needle1MaxValue>360</needle1MaxValue>
<needle1MinValue>0</needle1MinValue>
<needle1Move>Rotate</needle1Move>
<needle1ObjectField>Roll</needle1ObjectField>
<needle2DataObject>AttitudeActual</needle2DataObject>
<needle2DataObject>AttitudeState</needle2DataObject>
<needle2Factor>75</needle2Factor>
<needle2MaxValue>20</needle2MaxValue>
<needle2MinValue>0</needle2MinValue>
<needle2Move>Vertical</needle2Move>
<needle2ObjectField>Pitch</needle2ObjectField>
<needle3DataObject>AttitudeActual</needle3DataObject>
<needle3DataObject>AttitudeState</needle3DataObject>
<needle3Factor>-1</needle3Factor>
<needle3MaxValue>360</needle3MaxValue>
<needle3MinValue>0</needle3MinValue>
@ -621,19 +621,19 @@
<dialNeedleID2>needle2</dialNeedleID2>
<dialNeedleID3>needle3</dialNeedleID3>
<font>MS Shell Dlg 2,8.25,-1,5,50,0,0,0,0,0</font>
<needle1DataObject>BaroAltitude</needle1DataObject>
<needle1DataObject>BaroSensor</needle1DataObject>
<needle1Factor>1</needle1Factor>
<needle1MaxValue>10</needle1MaxValue>
<needle1MinValue>0</needle1MinValue>
<needle1Move>Rotate</needle1Move>
<needle1ObjectField>Altitude</needle1ObjectField>
<needle2DataObject>BaroAltitude</needle2DataObject>
<needle2DataObject>BaroSensor</needle2DataObject>
<needle2Factor>1</needle2Factor>
<needle2MaxValue>100</needle2MaxValue>
<needle2MinValue>0</needle2MinValue>
<needle2Move>Rotate</needle2Move>
<needle2ObjectField>Altitude</needle2ObjectField>
<needle3DataObject>BaroAltitude</needle3DataObject>
<needle3DataObject>BaroSensor</needle3DataObject>
<needle3Factor>1</needle3Factor>
<needle3MaxValue>1000</needle3MaxValue>
<needle3MinValue>0</needle3MinValue>
@ -653,19 +653,19 @@
<dialFile>%%DATAPATH%%dials/hi-contrast/barometer.svg</dialFile>
<dialNeedleID1>needle</dialNeedleID1>
<font>MS Shell Dlg 2,8.25,-1,5,50,0,0,0,0,0</font>
<needle1DataObject>BaroAltitude</needle1DataObject>
<needle1DataObject>BaroSensor</needle1DataObject>
<needle1Factor>10</needle1Factor>
<needle1MaxValue>1120</needle1MaxValue>
<needle1MinValue>1000</needle1MinValue>
<needle1Move>Rotate</needle1Move>
<needle1ObjectField>Pressure</needle1ObjectField>
<needle2DataObject>BaroAltitude</needle2DataObject>
<needle2DataObject>BaroSensor</needle2DataObject>
<needle2Factor>1</needle2Factor>
<needle2MaxValue>100</needle2MaxValue>
<needle2MinValue>0</needle2MinValue>
<needle2Move>Rotate</needle2Move>
<needle2ObjectField>Altitude</needle2ObjectField>
<needle3DataObject>BaroAltitude</needle3DataObject>
<needle3DataObject>BaroSensor</needle3DataObject>
<needle3Factor>1</needle3Factor>
<needle3MaxValue>1000</needle3MaxValue>
<needle3MinValue>0</needle3MinValue>
@ -685,19 +685,19 @@
<dialFile>%%DATAPATH%%dials/hi-contrast/vsi.svg</dialFile>
<dialNeedleID1>needle</dialNeedleID1>
<font>MS Shell Dlg 2,8.25,-1,5,50,0,0,0,0,0</font>
<needle1DataObject>VelocityActual</needle1DataObject>
<needle1DataObject>VelocityState</needle1DataObject>
<needle1Factor>0.01</needle1Factor>
<needle1MaxValue>12</needle1MaxValue>
<needle1MinValue>-12</needle1MinValue>
<needle1Move>Rotate</needle1Move>
<needle1ObjectField>Down</needle1ObjectField>
<needle2DataObject>BaroAltitude</needle2DataObject>
<needle2DataObject>BaroSensor</needle2DataObject>
<needle2Factor>1</needle2Factor>
<needle2MaxValue>100</needle2MaxValue>
<needle2MinValue>0</needle2MinValue>
<needle2Move>Rotate</needle2Move>
<needle2ObjectField>Altitude</needle2ObjectField>
<needle3DataObject>BaroAltitude</needle3DataObject>
<needle3DataObject>BaroSensor</needle3DataObject>
<needle3Factor>1</needle3Factor>
<needle3MaxValue>1000</needle3MaxValue>
<needle3MinValue>0</needle3MinValue>
@ -718,19 +718,19 @@
<dialForegroundID>foreground</dialForegroundID>
<dialNeedleID1>needle</dialNeedleID1>
<font>MS Shell Dlg 2,8.25,-1,5,50,0,0,0,0,0</font>
<needle1DataObject>AttitudeActual</needle1DataObject>
<needle1DataObject>AttitudeState</needle1DataObject>
<needle1Factor>-1</needle1Factor>
<needle1MaxValue>360</needle1MaxValue>
<needle1MinValue>0</needle1MinValue>
<needle1Move>Rotate</needle1Move>
<needle1ObjectField>Yaw</needle1ObjectField>
<needle2DataObject>BaroAltitude</needle2DataObject>
<needle2DataObject>BaroSensor</needle2DataObject>
<needle2Factor>1</needle2Factor>
<needle2MaxValue>100</needle2MaxValue>
<needle2MinValue>0</needle2MinValue>
<needle2Move>Rotate</needle2Move>
<needle2ObjectField>Altitude</needle2ObjectField>
<needle3DataObject>BaroAltitude</needle3DataObject>
<needle3DataObject>BaroSensor</needle3DataObject>
<needle3Factor>1</needle3Factor>
<needle3MaxValue>1000</needle3MaxValue>
<needle3MinValue>0</needle3MinValue>
@ -750,19 +750,19 @@
<dialFile>%%DATAPATH%%dials/hi-contrast/speed.svg</dialFile>
<dialNeedleID1>needle</dialNeedleID1>
<font>MS Shell Dlg 2,8.25,-1,5,50,0,0,0,0,0</font>
<needle1DataObject>GPSPosition</needle1DataObject>
<needle1DataObject>GPSPositionSensor</needle1DataObject>
<needle1Factor>3.6</needle1Factor>
<needle1MaxValue>120</needle1MaxValue>
<needle1MinValue>0</needle1MinValue>
<needle1Move>Rotate</needle1Move>
<needle1ObjectField>Groundspeed</needle1ObjectField>
<needle2DataObject>BaroAltitude</needle2DataObject>
<needle2DataObject>BaroSensor</needle2DataObject>
<needle2Factor>1</needle2Factor>
<needle2MaxValue>100</needle2MaxValue>
<needle2MinValue>0</needle2MinValue>
<needle2Move>Rotate</needle2Move>
<needle2ObjectField>Altitude</needle2ObjectField>
<needle3DataObject>BaroAltitude</needle3DataObject>
<needle3DataObject>BaroSensor</needle3DataObject>
<needle3Factor>1</needle3Factor>
<needle3MaxValue>1000</needle3MaxValue>
<needle3MinValue>0</needle3MinValue>
@ -784,19 +784,19 @@
<dialNeedleID2>needle2</dialNeedleID2>
<dialNeedleID3>needle3</dialNeedleID3>
<font>MS Shell Dlg 2,8.25,-1,5,50,0,0,0,0,0</font>
<needle1DataObject>BaroAltitude</needle1DataObject>
<needle1DataObject>BaroSensor</needle1DataObject>
<needle1Factor>1</needle1Factor>
<needle1MaxValue>120</needle1MaxValue>
<needle1MinValue>0</needle1MinValue>
<needle1Move>Rotate</needle1Move>
<needle1ObjectField>Temperature</needle1ObjectField>
<needle2DataObject>BaroAltitude</needle2DataObject>
<needle2DataObject>BaroSensor</needle2DataObject>
<needle2Factor>1</needle2Factor>
<needle2MaxValue>100</needle2MaxValue>
<needle2MinValue>0</needle2MinValue>
<needle2Move>Rotate</needle2Move>
<needle2ObjectField>Altitude</needle2ObjectField>
<needle3DataObject>BaroAltitude</needle3DataObject>
<needle3DataObject>BaroSensor</needle3DataObject>
<needle3Factor>1</needle3Factor>
<needle3MaxValue>1000</needle3MaxValue>
<needle3MinValue>0</needle3MinValue>
@ -824,13 +824,13 @@
<needle1MinValue>1000</needle1MinValue>
<needle1Move>Rotate</needle1Move>
<needle1ObjectField>Channel-3</needle1ObjectField>
<needle2DataObject>BaroAltitude</needle2DataObject>
<needle2DataObject>BaroSensor</needle2DataObject>
<needle2Factor>1</needle2Factor>
<needle2MaxValue>100</needle2MaxValue>
<needle2MinValue>0</needle2MinValue>
<needle2Move>Rotate</needle2Move>
<needle2ObjectField>Altitude</needle2ObjectField>
<needle3DataObject>BaroAltitude</needle3DataObject>
<needle3DataObject>BaroSensor</needle3DataObject>
<needle3Factor>1</needle3Factor>
<needle3MaxValue>1000</needle3MaxValue>
<needle3MinValue>0</needle3MinValue>
@ -852,19 +852,19 @@
<dialNeedleID2>needle2</dialNeedleID2>
<dialNeedleID3>needle3</dialNeedleID3>
<font>MS Shell Dlg 2,8.25,-1,5,50,0,0,0,0,0</font>
<needle1DataObject>BaroAltitude</needle1DataObject>
<needle1DataObject>BaroSensor</needle1DataObject>
<needle1Factor>1</needle1Factor>
<needle1MaxValue>120</needle1MaxValue>
<needle1MinValue>0</needle1MinValue>
<needle1Move>Rotate</needle1Move>
<needle1ObjectField>Temperature</needle1ObjectField>
<needle2DataObject>BaroAltitude</needle2DataObject>
<needle2DataObject>BaroSensor</needle2DataObject>
<needle2Factor>1</needle2Factor>
<needle2MaxValue>100</needle2MaxValue>
<needle2MinValue>0</needle2MinValue>
<needle2Move>Rotate</needle2Move>
<needle2ObjectField>Altitude</needle2ObjectField>
<needle3DataObject>BaroAltitude</needle3DataObject>
<needle3DataObject>BaroSensor</needle3DataObject>
<needle3Factor>1</needle3Factor>
<needle3MaxValue>1000</needle3MaxValue>
<needle3MinValue>0</needle3MinValue>
@ -965,7 +965,7 @@
<attActCalc>false</attActCalc>
<attActHW>false</attActHW>
<attActSim>true</attActSim>
<attActualEnabled>true</attActualEnabled>
<attStateEnabled>true</attStateEnabled>
<attRawEnabled>true</attRawEnabled>
<attRawRate>20</attRawRate>
<baroAltRate>50</baroAltRate>
@ -1001,7 +1001,7 @@
<attActCalc>false</attActCalc>
<attActHW>false</attActHW>
<attActSim>true</attActSim>
<attActualEnabled>true</attActualEnabled>
<attStateEnabled>true</attStateEnabled>
<attRawEnabled>true</attRawEnabled>
<attRawRate>20</attRawRate>
<baroAltRate>50</baroAltRate>
@ -1037,7 +1037,7 @@
<attActCalc>false</attActCalc>
<attActHW>false</attActHW>
<attActSim>true</attActSim>
<attActualEnabled>true</attActualEnabled>
<attStateEnabled>true</attStateEnabled>
<attRawEnabled>true</attRawEnabled>
<attRawRate>20</attRawRate>
<baroAltRate>50</baroAltRate>
@ -1081,7 +1081,7 @@
<minValue>-11</minValue>
<redMax>11</redMax>
<redMin>-11</redMin>
<sourceDataObject>Accels</sourceDataObject>
<sourceDataObject>AccelState</sourceDataObject>
<sourceObjectField>x</sourceObjectField>
<useOpenGLFlag>false</useOpenGLFlag>
<yellowMax>-5</yellowMax>
@ -1104,7 +1104,7 @@
<minValue>-11</minValue>
<redMax>11</redMax>
<redMin>-11</redMin>
<sourceDataObject>Accels</sourceDataObject>
<sourceDataObject>AccelState</sourceDataObject>
<sourceObjectField>y</sourceObjectField>
<useOpenGLFlag>false</useOpenGLFlag>
<yellowMax>-5</yellowMax>
@ -1127,7 +1127,7 @@
<minValue>-11</minValue>
<redMax>11</redMax>
<redMin>-11</redMin>
<sourceDataObject>Accels</sourceDataObject>
<sourceDataObject>AccelState</sourceDataObject>
<sourceObjectField>z</sourceObjectField>
<useOpenGLFlag>false</useOpenGLFlag>
<yellowMax>-5</yellowMax>
@ -1219,7 +1219,7 @@
<minValue>0</minValue>
<redMax>0</redMax>
<redMin>0</redMin>
<sourceDataObject>GPSPosition</sourceDataObject>
<sourceDataObject>GPSPositionSensor</sourceDataObject>
<sourceObjectField>Satellites</sourceObjectField>
<useOpenGLFlag>false</useOpenGLFlag>
<yellowMax>0</yellowMax>
@ -1242,7 +1242,7 @@
<minValue>0</minValue>
<redMax>33</redMax>
<redMin>0</redMin>
<sourceDataObject>GPSPosition</sourceDataObject>
<sourceDataObject>GPSPositionSensor</sourceDataObject>
<sourceObjectField>Status</sourceObjectField>
<useOpenGLFlag>false</useOpenGLFlag>
<yellowMax>66</yellowMax>
@ -1318,7 +1318,7 @@
<yellowMin>-0.8</yellowMin>
</data>
</Pitch>
<PitchActual>
<PitchState>
<configInfo>
<locked>false</locked>
<version>0.0.0</version>
@ -1334,13 +1334,13 @@
<minValue>-90</minValue>
<redMax>1</redMax>
<redMin>0</redMin>
<sourceDataObject>AttitudeActual</sourceDataObject>
<sourceDataObject>AttitudeState</sourceDataObject>
<sourceObjectField>Pitch</sourceObjectField>
<useOpenGLFlag>false</useOpenGLFlag>
<yellowMax>0.9</yellowMax>
<yellowMin>0.1</yellowMin>
</data>
</PitchActual>
</PitchState>
<Roll__PCT__20Desired>
<configInfo>
<locked>false</locked>
@ -1863,7 +1863,7 @@
<color>4294901760</color>
<mathFunction>None</mathFunction>
<uavField>x</uavField>
<uavObject>Accels</uavObject>
<uavObject>AccelState</uavObject>
<yMaximum>0</yMaximum>
<yMeanSamples>1</yMeanSamples>
<yMinimum>0</yMinimum>
@ -1873,7 +1873,7 @@
<color>4283782655</color>
<mathFunction>None</mathFunction>
<uavField>y</uavField>
<uavObject>Accels</uavObject>
<uavObject>AccelState</uavObject>
<yMaximum>0</yMaximum>
<yMeanSamples>1</yMeanSamples>
<yMinimum>0</yMinimum>
@ -1883,7 +1883,7 @@
<color>4283804160</color>
<mathFunction>None</mathFunction>
<uavField>z</uavField>
<uavObject>Accels</uavObject>
<uavObject>AccelState</uavObject>
<yMaximum>0</yMaximum>
<yMeanSamples>1</yMeanSamples>
<yMinimum>0</yMinimum>
@ -1963,7 +1963,7 @@
<color>4283760895</color>
<mathFunction>None</mathFunction>
<uavField>Roll</uavField>
<uavObject>AttitudeActual</uavObject>
<uavObject>AttitudeState</uavObject>
<yMaximum>0</yMaximum>
<yMeanSamples>1</yMeanSamples>
<yMinimum>0</yMinimum>
@ -1973,7 +1973,7 @@
<color>4278233600</color>
<mathFunction>None</mathFunction>
<uavField>Yaw</uavField>
<uavObject>AttitudeActual</uavObject>
<uavObject>AttitudeState</uavObject>
<yMaximum>0</yMaximum>
<yMeanSamples>1</yMeanSamples>
<yMinimum>0</yMinimum>
@ -1983,7 +1983,7 @@
<color>4294901760</color>
<mathFunction>None</mathFunction>
<uavField>Pitch</uavField>
<uavObject>AttitudeActual</uavObject>
<uavObject>AttitudeState</uavObject>
<yMaximum>0</yMaximum>
<yMeanSamples>1</yMeanSamples>
<yMinimum>0</yMinimum>
@ -2008,7 +2008,7 @@
<color>4278190080</color>
<mathFunction>None</mathFunction>
<uavField>Pressure</uavField>
<uavObject>BaroAltitude</uavObject>
<uavObject>BaroSensor</uavObject>
<yMaximum>0</yMaximum>
<yMeanSamples>1</yMeanSamples>
<yMinimum>0</yMinimum>
@ -2114,7 +2114,7 @@
<refreshInterval>200</refreshInterval>
</data>
</Inputs>
<Raw__PCT__20Accels>
<Raw__PCT__20AccelState>
<configInfo>
<locked>false</locked>
<version>0.0.0</version>
@ -2128,7 +2128,7 @@
<color>4294901760</color>
<mathFunction>None</mathFunction>
<uavField>x</uavField>
<uavObject>Accels</uavObject>
<uavObject>AccelState</uavObject>
<yMaximum>0</yMaximum>
<yMeanSamples>1</yMeanSamples>
<yMinimum>0</yMinimum>
@ -2138,7 +2138,7 @@
<color>4283782655</color>
<mathFunction>None</mathFunction>
<uavField>y</uavField>
<uavObject>Accels</uavObject>
<uavObject>AccelState</uavObject>
<yMaximum>0</yMaximum>
<yMeanSamples>1</yMeanSamples>
<yMinimum>0</yMinimum>
@ -2148,7 +2148,7 @@
<color>4283804160</color>
<mathFunction>None</mathFunction>
<uavField>z</uavField>
<uavObject>Accels</uavObject>
<uavObject>AccelState</uavObject>
<yMaximum>0</yMaximum>
<yMeanSamples>1</yMeanSamples>
<yMinimum>0</yMinimum>
@ -2158,8 +2158,8 @@
<plotType>1</plotType>
<refreshInterval>500</refreshInterval>
</data>
</Raw__PCT__20Accels>
<Raw__PCT__20Gyros>
</Raw__PCT__20AccelState>
<Raw__PCT__20GyroState>
<configInfo>
<locked>false</locked>
<version>0.0.0</version>
@ -2173,7 +2173,7 @@
<color>4283804160</color>
<mathFunction>None</mathFunction>
<uavField>z</uavField>
<uavObject>Gyros</uavObject>
<uavObject>GyroState</uavObject>
<yMaximum>0</yMaximum>
<yMeanSamples>1</yMeanSamples>
<yMinimum>0</yMinimum>
@ -2183,7 +2183,7 @@
<color>4283782655</color>
<mathFunction>None</mathFunction>
<uavField>y</uavField>
<uavObject>Gyros</uavObject>
<uavObject>GyroState</uavObject>
<yMaximum>0</yMaximum>
<yMeanSamples>1</yMeanSamples>
<yMinimum>0</yMinimum>
@ -2193,7 +2193,7 @@
<color>4294901760</color>
<mathFunction>None</mathFunction>
<uavField>x</uavField>
<uavObject>Gyros</uavObject>
<uavObject>GyroState</uavObject>
<yMaximum>0</yMaximum>
<yMeanSamples>1</yMeanSamples>
<yMinimum>0</yMinimum>
@ -2203,7 +2203,7 @@
<plotType>1</plotType>
<refreshInterval>500</refreshInterval>
</data>
</Raw__PCT__20Gyros>
</Raw__PCT__20GyroState>
<Raw__PCT__20magnetometers>
<configInfo>
<locked>false</locked>
@ -2218,7 +2218,7 @@
<color>4294901760</color>
<mathFunction>None</mathFunction>
<uavField>x</uavField>
<uavObject>Magnetometer</uavObject>
<uavObject>MagState</uavObject>
<yMaximum>0</yMaximum>
<yMeanSamples>1</yMeanSamples>
<yMinimum>0</yMinimum>
@ -2228,7 +2228,7 @@
<color>4283782655</color>
<mathFunction>None</mathFunction>
<uavField>y</uavField>
<uavObject>Magnetometer</uavObject>
<uavObject>MagState</uavObject>
<yMaximum>0</yMaximum>
<yMeanSamples>1</yMeanSamples>
<yMinimum>0</yMinimum>
@ -2238,7 +2238,7 @@
<color>4283804160</color>
<mathFunction>None</mathFunction>
<uavField>z</uavField>
<uavObject>Magnetometer</uavObject>
<uavObject>MagState</uavObject>
<yMaximum>0</yMaximum>
<yMeanSamples>1</yMeanSamples>
<yMinimum>0</yMinimum>
@ -2641,7 +2641,7 @@
<side1>
<classId>ScopeGadget</classId>
<gadget>
<activeConfiguration>Raw Gyros</activeConfiguration>
<activeConfiguration>Raw GyroState</activeConfiguration>
</gadget>
<type>uavGadget</type>
</side1>

6
ground/openpilotgcs/share/openpilotgcs/pfd/default/AltitudeScale.qml
View File

@ -24,10 +24,10 @@ Item {
anchors.verticalCenter: parent.verticalCenter
// The altitude scale represents 30 meters,
// move it in 0..5m range
anchors.verticalCenterOffset: -height/30 * (PositionActual.Down-Math.floor(PositionActual.Down/5*qmlWidget.altitudeFactor)*5)
anchors.verticalCenterOffset: -height/30 * (PositionState.Down-Math.floor(PositionState.Down/5*qmlWidget.altitudeFactor)*5)
anchors.left: parent.left
property int topNumber: 15-Math.floor(PositionActual.Down/5*qmlWidget.altitudeFactor)*5
property int topNumber: 15-Math.floor(PositionState.Down/5*qmlWidget.altitudeFactor)*5
// Altitude numbers
Column {
@ -68,7 +68,7 @@ Item {
Text {
id: altitude_text
text: Math.floor(-PositionActual.Down * qmlWidget.altitudeFactor).toFixed()
text: Math.floor(-PositionState.Down * qmlWidget.altitudeFactor).toFixed()
color: "white"
font {
family: "Arial"

4
ground/openpilotgcs/share/openpilotgcs/pfd/default/Compass.qml
View File

@ -19,8 +19,8 @@ Item {
//split compass band to 8 parts to ensure it doesn't exceed the max texture size
Row {
anchors.centerIn: parent
//the band is 540 degrees wide, AttitudeActual.Yaw is converted to -180..180 range
anchors.horizontalCenterOffset: -1*((AttitudeActual.Yaw+180+720) % 360 - 180)/540*width
//the band is 540 degrees wide, AttitudeState.Yaw is converted to -180..180 range
anchors.horizontalCenterOffset: -1*((AttitudeState.Yaw+180+720) % 360 - 180)/540*width
Repeater {
model: 5

4
ground/openpilotgcs/share/openpilotgcs/pfd/default/Pfd.qml
View File

@ -34,7 +34,7 @@ Rectangle {
anchors.centerIn: parent
//rotate it around the center of scene
transform: Rotation {
angle: -AttitudeActual.Roll
angle: -AttitudeState.Roll
origin.x : sceneItem.width/2 - x
origin.y : sceneItem.height/2 - y
}
@ -54,7 +54,7 @@ Rectangle {
sceneSize: background.sceneSize
smooth: true
property real sideSlip: Accels.y
property real sideSlip: AccelState.y
//smooth side slip changes, a low pass filter replacement
//accels are updated once per second
Behavior on sideSlip {

4
ground/openpilotgcs/share/openpilotgcs/pfd/default/PfdIndicators.qml
View File

@ -31,12 +31,12 @@ Item {
Text {
id: gps_text
text: "GPS: " + GPSPosition.Satellites + "\nPDP: " + Math.round(GPSPosition.PDOP*10)/10
text: "GPS: " + GPSPositionSensor.Satellites + "\nPDP: " + Math.round(GPSPositionSensor.PDOP*10)/10
color: "white"
font.family: "Arial"
font.pixelSize: telemetry_status.height * 0.75
visible: GPSPosition.Satellites > 0
visible: GPSPositionSensor.Satellites > 0
property variant scaledBounds: svgRenderer.scaledElementBounds("pfd.svg", "gps-txt")
x: Math.floor(scaledBounds.x * sceneItem.width)

12
ground/openpilotgcs/share/openpilotgcs/pfd/default/PfdTerrainView.qml
View File

@ -7,14 +7,14 @@ OsgEarth {
sceneFile: qmlWidget.earthFile
fieldOfView: 90
yaw: AttitudeActual.Yaw
pitch: AttitudeActual.Pitch
roll: AttitudeActual.Roll
yaw: AttitudeState.Yaw
pitch: AttitudeState.Pitch
roll: AttitudeState.Roll
latitude: qmlWidget.actualPositionUsed ?
GPSPosition.Latitude/10000000.0 : qmlWidget.latitude
GPSPositionSensor.Latitude/10000000.0 : qmlWidget.latitude
longitude: qmlWidget.actualPositionUsed ?
GPSPosition.Longitude/10000000.0 : qmlWidget.longitude
GPSPositionSensor.Longitude/10000000.0 : qmlWidget.longitude
altitude: qmlWidget.actualPositionUsed ?
GPSPosition.Altitude : qmlWidget.altitude
GPSPositionSensor.Altitude : qmlWidget.altitude
}

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

@ -23,10 +23,10 @@ Item {
id: pitchTranslate
x: Math.round((world.parent.width - world.width)/2)
y: Math.round((world.parent.height - world.height)/2 +
AttitudeActual.Pitch*world.parent.height/94)
AttitudeState.Pitch*world.parent.height/94)
},
Rotation {
angle: -AttitudeActual.Roll
angle: -AttitudeState.Roll
origin.x : world.parent.width/2
origin.y : world.parent.height/2
}

4
ground/openpilotgcs/share/openpilotgcs/pfd/default/SpeedScale.qml
View File

@ -3,8 +3,8 @@ import Qt 4.7
Item {
id: sceneItem
property variant sceneSize
property real groundSpeed : qmlWidget.speedFactor * Math.sqrt(Math.pow(VelocityActual.North,2)+
Math.pow(VelocityActual.East,2))
property real groundSpeed : qmlWidget.speedFactor * Math.sqrt(Math.pow(VelocityState.North,2)+
Math.pow(VelocityState.East,2))
SvgElementImage {
id: speed_bg

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

@ -20,7 +20,7 @@ Item {
sceneSize: sceneItem.sceneSize
//the scale in 1000 ft/min with height == 5200 ft/min
height: (-VelocityActual.Down*3.28*60/1000)*(vsi_scale.height/5.2)
height: (-VelocityState.Down*3.28*60/1000)*(vsi_scale.height/5.2)
anchors.bottom: parent.verticalCenter

4
ground/openpilotgcs/share/openpilotgcs/translations/openpilotgcs_de.ts
View File

@ -1767,7 +1767,7 @@ p, li { white-space: pre-wrap; }
</message>
<message>
<location/>
<source>AttitudeActual</source>
<source>AttitudeState</source>
<translation type="unfinished"></translation>
</message>
<message>
@ -1807,7 +1807,7 @@ p, li { white-space: pre-wrap; }
</message>
<message>
<location/>
<source>GPSPosition</source>
<source>GPSPositionSensor</source>
<translation type="unfinished"></translation>
</message>
<message>

4
ground/openpilotgcs/share/openpilotgcs/translations/openpilotgcs_es.ts
View File

@ -1768,7 +1768,7 @@ p, li { white-space: pre-wrap; }
</message>
<message>
<location/>
<source>AttitudeActual</source>
<source>AttitudeState</source>
<translation type="unfinished"></translation>
</message>
<message>
@ -1808,7 +1808,7 @@ p, li { white-space: pre-wrap; }
</message>
<message>
<location/>
<source>GPSPosition</source>
<source>GPSPositionSensor</source>
<translation type="unfinished"></translation>
</message>
<message>

4
ground/openpilotgcs/share/openpilotgcs/translations/openpilotgcs_fr.ts
View File

@ -1789,7 +1789,7 @@ p, li { white-space: pre-wrap; }
</message>
<message>
<location/>
<source>AttitudeActual</source>
<source>AttitudeState</source>
<translation type="unfinished"></translation>
</message>
<message>
@ -1829,7 +1829,7 @@ p, li { white-space: pre-wrap; }
</message>
<message>
<location/>
<source>GPSPosition</source>
<source>GPSPositionSensor</source>
<translation type="unfinished"></translation>
</message>
<message>

4
ground/openpilotgcs/share/openpilotgcs/translations/openpilotgcs_ru.ts
View File

@ -1721,7 +1721,7 @@ Sat SNR is displayed above (in dBHz)</source>
</message>
<message>
<location/>
<source>AttitudeActual</source>
<source>AttitudeState</source>
<translation type="unfinished"></translation>
</message>
<message>
@ -1761,7 +1761,7 @@ Sat SNR is displayed above (in dBHz)</source>
</message>
<message>
<location/>
<source>GPSPosition</source>
<source>GPSPositionSensor</source>
<translation type="unfinished"></translation>
</message>
<message>

4
ground/openpilotgcs/share/openpilotgcs/translations/openpilotgcs_zh_CN.ts
View File

@ -2607,7 +2607,7 @@ Sat SNR is displayed above (in dBHz)</source>
</message>
<message>
<location/>
<source>AttitudeActual</source>
<source>AttitudeState</source>
<translation type="unfinished"></translation>
</message>
<message>
@ -2647,7 +2647,7 @@ Sat SNR is displayed above (in dBHz)</source>
</message>
<message>
<location/>
<source>GPSPosition</source>
<source>GPSPositionSensor</source>
<translation type="unfinished"></translation>
</message>
<message>

4
ground/openpilotgcs/src/plugins/antennatrack/telemetryparser.cpp
View File

@ -39,12 +39,12 @@ TelemetryParser::TelemetryParser(QObject *parent) : GPSParser(parent)
{
ExtensionSystem::PluginManager *pm = ExtensionSystem::PluginManager::instance();
UAVObjectManager *objManager = pm->getObject<UAVObjectManager>();
UAVDataObject *gpsObj = dynamic_cast<UAVDataObject *>(objManager->getObject("GPSPosition"));
UAVDataObject *gpsObj = dynamic_cast<UAVDataObject *>(objManager->getObject("GPSPositionSensor"));
if (gpsObj != NULL) {
connect(gpsObj, SIGNAL(objectUpdated(UAVObject *)), this, SLOT(updateGPS(UAVObject *)));
} else {
qDebug() << "Error: Object is unknown (GPSPosition).";
qDebug() << "Error: Object is unknown (GPSPositionSensor).";
}
gpsObj = dynamic_cast<UAVDataObject *>(objManager->getObject("HomeLocation"));

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

@ -33,8 +33,8 @@
#include <QDebug>
#include <QDesktopServices>
#include <QUrl>
#include "accels.h"
#include "gyros.h"
#include "accelstate.h"
#include "gyrostate.h"
#include <extensionsystem/pluginmanager.h>
#include <coreplugin/generalsettings.h>
@ -80,26 +80,26 @@ void ConfigCCAttitudeWidget::sensorsUpdated(UAVObject *obj)
return;
}
Accels *accels = Accels::GetInstance(getObjectManager());
Gyros *gyros = Gyros::GetInstance(getObjectManager());
AccelState *accelState = AccelState::GetInstance(getObjectManager());
GyroState *gyroState = GyroState::GetInstance(getObjectManager());
// Accumulate samples until we have _at least_ NUM_SENSOR_UPDATES samples
// for both gyros and accels.
// Note that, at present, we stash the samples and then compute the bias
// at the end, even though the mean could be accumulated as we go.
// In future, a better algorithm could be used.
if (obj->getObjID() == Accels::OBJID) {
if (obj->getObjID() == AccelState::OBJID) {
accelUpdates++;
Accels::DataFields accelsData = accels->getData();
x_accum.append(accelsData.x);
y_accum.append(accelsData.y);
z_accum.append(accelsData.z);
} else if (obj->getObjID() == Gyros::OBJID) {
AccelState::DataFields accelStateData = accelState->getData();
x_accum.append(accelStateData.x);
y_accum.append(accelStateData.y);
z_accum.append(accelStateData.z);
} else if (obj->getObjID() == GyroState::OBJID) {
gyroUpdates++;
Gyros::DataFields gyrosData = gyros->getData();
x_gyro_accum.append(gyrosData.x);
y_gyro_accum.append(gyrosData.y);
z_gyro_accum.append(gyrosData.z);
GyroState::DataFields gyroStateData = gyroState->getData();
x_gyro_accum.append(gyroStateData.x);
y_gyro_accum.append(gyroStateData.y);
z_gyro_accum.append(gyroStateData.z);
}
// update the progress indicator
@ -118,8 +118,8 @@ void ConfigCCAttitudeWidget::sensorsUpdated(UAVObject *obj)
float x_gyro_bias = listMean(x_gyro_accum) * 100.0f;
float y_gyro_bias = listMean(y_gyro_accum) * 100.0f;
float z_gyro_bias = listMean(z_gyro_accum) * 100.0f;
accels->setMetadata(initialAccelsMdata);
gyros->setMetadata(initialGyrosMdata);
accelState->setMetadata(initialAccelStateMdata);
gyroState->setMetadata(initialGyroStateMdata);
AttitudeSettings::DataFields attitudeSettingsData = AttitudeSettings::GetInstance(getObjectManager())->getData();
// We offset the gyro bias by current bias to help precision
@ -140,15 +140,15 @@ void ConfigCCAttitudeWidget::sensorsUpdated(UAVObject *obj)
void ConfigCCAttitudeWidget::timeout()
{
UAVDataObject *obj = Accels::GetInstance(getObjectManager());
UAVDataObject *obj = AccelState::GetInstance(getObjectManager());
disconnect(obj, SIGNAL(objectUpdated(UAVObject *)), this, SLOT(sensorsUpdated(UAVObject *)));
disconnect(&timer, SIGNAL(timeout()), this, SLOT(timeout()));
Accels *accels = Accels::GetInstance(getObjectManager());
Gyros *gyros = Gyros::GetInstance(getObjectManager());
accels->setMetadata(initialAccelsMdata);
gyros->setMetadata(initialGyrosMdata);
AccelState *accelState = AccelState::GetInstance(getObjectManager());
GyroState *gyroState = GyroState::GetInstance(getObjectManager());
accelState->setMetadata(initialAccelStateMdata);
gyroState->setMetadata(initialGyroStateMdata);
QMessageBox msgBox;
msgBox.setText(tr("Calibration timed out before receiving required updates."));
@ -182,28 +182,28 @@ void ConfigCCAttitudeWidget::startAccelCalibration()
AttitudeSettings::GetInstance(getObjectManager())->setData(attitudeSettingsData);
// Set up to receive updates
UAVDataObject *accels = Accels::GetInstance(getObjectManager());
UAVDataObject *gyros = Gyros::GetInstance(getObjectManager());
connect(accels, SIGNAL(objectUpdated(UAVObject *)), this, SLOT(sensorsUpdated(UAVObject *)));
connect(gyros, SIGNAL(objectUpdated(UAVObject *)), this, SLOT(sensorsUpdated(UAVObject *)));
UAVDataObject *accelState = AccelState::GetInstance(getObjectManager());
UAVDataObject *gyroState = GyroState::GetInstance(getObjectManager());
connect(accelState, SIGNAL(objectUpdated(UAVObject *)), this, SLOT(sensorsUpdated(UAVObject *)));
connect(gyroState, SIGNAL(objectUpdated(UAVObject *)), this, SLOT(sensorsUpdated(UAVObject *)));
// Speed up updates
initialAccelsMdata = accels->getMetadata();
UAVObject::Metadata accelsMdata = initialAccelsMdata;
UAVObject::SetFlightTelemetryUpdateMode(accelsMdata, UAVObject::UPDATEMODE_PERIODIC);
accelsMdata.flightTelemetryUpdatePeriod = 30; // ms
accels->setMetadata(accelsMdata);
initialAccelStateMdata = accelState->getMetadata();
UAVObject::Metadata accelStateMdata = initialAccelStateMdata;
UAVObject::SetFlightTelemetryUpdateMode(accelStateMdata, UAVObject::UPDATEMODE_PERIODIC);
accelStateMdata.flightTelemetryUpdatePeriod = 30; // ms
accelState->setMetadata(accelStateMdata);
initialGyrosMdata = gyros->getMetadata();
UAVObject::Metadata gyrosMdata = initialGyrosMdata;
UAVObject::SetFlightTelemetryUpdateMode(gyrosMdata, UAVObject::UPDATEMODE_PERIODIC);
gyrosMdata.flightTelemetryUpdatePeriod = 30; // ms
gyros->setMetadata(gyrosMdata);
initialGyroStateMdata = gyroState->getMetadata();
UAVObject::Metadata gyroStateMdata = initialGyroStateMdata;
UAVObject::SetFlightTelemetryUpdateMode(gyroStateMdata, UAVObject::UPDATEMODE_PERIODIC);
gyroStateMdata.flightTelemetryUpdatePeriod = 30; // ms
gyroState->setMetadata(gyroStateMdata);
// Set up timeout timer
timer.setSingleShot(true);
timer.start(5000 + (NUM_SENSOR_UPDATES * qMax(accelsMdata.flightTelemetryUpdatePeriod,
gyrosMdata.flightTelemetryUpdatePeriod)));
timer.start(5000 + (NUM_SENSOR_UPDATES * qMax(accelStateMdata.flightTelemetryUpdatePeriod,
gyroStateMdata.flightTelemetryUpdatePeriod)));
connect(&timer, SIGNAL(timeout()), this, SLOT(timeout()));
}

4
ground/openpilotgcs/src/plugins/config/configccattitudewidget.h
View File

@ -56,8 +56,8 @@ private slots:
private:
Ui_ccattitude *ui;
QTimer timer;
UAVObject::Metadata initialAccelsMdata;
UAVObject::Metadata initialGyrosMdata;
UAVObject::Metadata initialAccelStateMdata;
UAVObject::Metadata initialGyroStateMdata;
int accelUpdates;
int gyroUpdates;

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

@ -44,9 +44,9 @@
#include <ekfconfiguration.h>
#include <revocalibration.h>
#include <homelocation.h>
#include <accels.h>
#include <gyros.h>
#include <magnetometer.h>
#include <accelstate.h>
#include <gyrostate.h>
#include <magstate.h>
#define GRAVITY 9.81f
#include "assertions.h"
@ -298,26 +298,26 @@ void ConfigRevoWidget::doStartAccelGyroBiasCalibration()
UAVObject::Metadata mdata;
/* Need to get as many accel updates as possible */
Accels *accels = Accels::GetInstance(getObjectManager());
Q_ASSERT(accels);
initialAccelsMdata = accels->getMetadata();
mdata = initialAccelsMdata;
AccelState *accelState = AccelState::GetInstance(getObjectManager());
Q_ASSERT(accelState);
initialAccelStateMdata = accelState->getMetadata();
mdata = initialAccelStateMdata;
UAVObject::SetFlightTelemetryUpdateMode(mdata, UAVObject::UPDATEMODE_PERIODIC);
mdata.flightTelemetryUpdatePeriod = 100;
accels->setMetadata(mdata);
accelState->setMetadata(mdata);
Gyros *gyros = Gyros::GetInstance(getObjectManager());
Q_ASSERT(gyros);
initialGyrosMdata = gyros->getMetadata();
mdata = initialGyrosMdata;
GyroState *gyroState = GyroState::GetInstance(getObjectManager());
Q_ASSERT(gyroState);
initialGyroStateMdata = gyroState->getMetadata();
mdata = initialGyroStateMdata;
UAVObject::SetFlightTelemetryUpdateMode(mdata, UAVObject::UPDATEMODE_PERIODIC);
mdata.flightTelemetryUpdatePeriod = 100;
gyros->setMetadata(mdata);
gyroState->setMetadata(mdata);
// Now connect to the accels and mag updates, gather for 100 samples
collectingData = true;
connect(accels, SIGNAL(objectUpdated(UAVObject *)), this, SLOT(doGetAccelGyroBiasData(UAVObject *)));
connect(gyros, SIGNAL(objectUpdated(UAVObject *)), this, SLOT(doGetAccelGyroBiasData(UAVObject *)));
connect(accelState, SIGNAL(objectUpdated(UAVObject *)), this, SLOT(doGetAccelGyroBiasData(UAVObject *)));
connect(gyroState, SIGNAL(objectUpdated(UAVObject *)), this, SLOT(doGetAccelGyroBiasData(UAVObject *)));
}
/**
@ -330,26 +330,26 @@ void ConfigRevoWidget::doGetAccelGyroBiasData(UAVObject *obj)
Q_UNUSED(lock);
switch (obj->getObjID()) {
case Accels::OBJID:
case AccelState::OBJID:
{
Accels *accels = Accels::GetInstance(getObjectManager());
Q_ASSERT(accels);
Accels::DataFields accelsData = accels->getData();
AccelState *accelState = AccelState::GetInstance(getObjectManager());
Q_ASSERT(accelState);
AccelState::DataFields accelStateData = accelState->getData();
accel_accum_x.append(accelsData.x);
accel_accum_y.append(accelsData.y);
accel_accum_z.append(accelsData.z);
accel_accum_x.append(accelStateData.x);
accel_accum_y.append(accelStateData.y);
accel_accum_z.append(accelStateData.z);
break;
}
case Gyros::OBJID:
case GyroState::OBJID:
{
Gyros *gyros = Gyros::GetInstance(getObjectManager());
Q_ASSERT(gyros);
Gyros::DataFields gyrosData = gyros->getData();
GyroState *gyroState = GyroState::GetInstance(getObjectManager());
Q_ASSERT(gyroState);
GyroState::DataFields gyroStateData = gyroState->getData();
gyro_accum_x.append(gyrosData.x);
gyro_accum_y.append(gyrosData.y);
gyro_accum_z.append(gyrosData.z);
gyro_accum_x.append(gyroStateData.x);
gyro_accum_y.append(gyroStateData.y);
gyro_accum_z.append(gyroStateData.z);
break;
}
default:
@ -366,11 +366,11 @@ void ConfigRevoWidget::doGetAccelGyroBiasData(UAVObject *obj)
collectingData == true) {
collectingData = false;
Accels *accels = Accels::GetInstance(getObjectManager());
Gyros *gyros = Gyros::GetInstance(getObjectManager());
AccelState *accelState = AccelState::GetInstance(getObjectManager());
GyroState *gyroState = GyroState::GetInstance(getObjectManager());
disconnect(accels, SIGNAL(objectUpdated(UAVObject *)), this, SLOT(doGetAccelGyroBiasData(UAVObject *)));
disconnect(gyros, SIGNAL(objectUpdated(UAVObject *)), this, SLOT(doGetAccelGyroBiasData(UAVObject *)));
disconnect(accelState, SIGNAL(objectUpdated(UAVObject *)), this, SLOT(doGetAccelGyroBiasData(UAVObject *)));
disconnect(gyroState, SIGNAL(objectUpdated(UAVObject *)), this, SLOT(doGetAccelGyroBiasData(UAVObject *)));
m_ui->accelBiasStart->setEnabled(true);
@ -397,8 +397,8 @@ void ConfigRevoWidget::doGetAccelGyroBiasData(UAVObject *obj)
attitudeSettings->setData(attitudeSettingsData);
attitudeSettings->updated();
accels->setMetadata(initialAccelsMdata);
gyros->setMetadata(initialGyrosMdata);
accelState->setMetadata(initialAccelStateMdata);
gyroState->setMetadata(initialGyroStateMdata);
// Recall saved board rotation
recallBoardRotation();
@ -575,21 +575,21 @@ void ConfigRevoWidget::doStartSixPointCalibration()
#ifdef SIX_POINT_CAL_ACCEL
/* Need to get as many accel updates as possible */
Accels *accels = Accels::GetInstance(getObjectManager());
Q_ASSERT(accels);
AccelState *accelState = AccelState::GetInstance(getObjectManager());
Q_ASSERT(accelState);
initialAccelsMdata = accels->getMetadata();
mdata = initialAccelsMdata;
initialAccelStateMdata = accelState->getMetadata();
mdata = initialAccelStateMdata;
UAVObject::SetFlightTelemetryUpdateMode(mdata, UAVObject::UPDATEMODE_PERIODIC);
mdata.flightTelemetryUpdatePeriod = 100;
accels->setMetadata(mdata);
accelState->setMetadata(mdata);
#endif
/* Need to get as many mag updates as possible */
Magnetometer *mag = Magnetometer::GetInstance(getObjectManager());
MagState *mag = MagState::GetInstance(getObjectManager());
Q_ASSERT(mag);
initialMagMdata = mag->getMetadata();
mdata = initialMagMdata;
initialMagStateMdata = mag->getMetadata();
mdata = initialMagStateMdata;
UAVObject::SetFlightTelemetryUpdateMode(mdata, UAVObject::UPDATEMODE_PERIODIC);
mdata.flightTelemetryUpdatePeriod = 100;
mag->setMetadata(mdata);
@ -623,12 +623,12 @@ void ConfigRevoWidget::savePositionData()
collectingData = true;
Accels *accels = Accels::GetInstance(getObjectManager());
Q_ASSERT(accels);
Magnetometer *mag = Magnetometer::GetInstance(getObjectManager());
AccelState *accelState = AccelState::GetInstance(getObjectManager());
Q_ASSERT(accelState);
MagState *mag = MagState::GetInstance(getObjectManager());
Q_ASSERT(mag);
connect(accels, SIGNAL(objectUpdated(UAVObject *)), this, SLOT(doGetSixPointCalibrationMeasurement(UAVObject *)));
connect(accelState, SIGNAL(objectUpdated(UAVObject *)), this, SLOT(doGetSixPointCalibrationMeasurement(UAVObject *)));
connect(mag, SIGNAL(objectUpdated(UAVObject *)), this, SLOT(doGetSixPointCalibrationMeasurement(UAVObject *)));
m_ui->sixPointCalibInstructions->append("Hold...");
@ -645,19 +645,19 @@ void ConfigRevoWidget::doGetSixPointCalibrationMeasurement(UAVObject *obj)
// This is necessary to prevent a race condition on disconnect signal and another update
if (collectingData == true) {
if (obj->getObjID() == Accels::OBJID) {
if (obj->getObjID() == AccelState::OBJID) {
#ifdef SIX_POINT_CAL_ACCEL
Accels *accels = Accels::GetInstance(getObjectManager());
Q_ASSERT(accels);
Accels::DataFields accelsData = accels->getData();
accel_accum_x.append(accelsData.x);
accel_accum_y.append(accelsData.y);
accel_accum_z.append(accelsData.z);
AccelState *accelState = AccelState::GetInstance(getObjectManager());
Q_ASSERT(accelState);
AccelState::DataFields accelStateData = accelState->getData();
accel_accum_x.append(accelStateData.x);
accel_accum_y.append(accelStateData.y);
accel_accum_z.append(accelStateData.z);
#endif
} else if (obj->getObjID() == Magnetometer::OBJID) {
Magnetometer *mag = Magnetometer::GetInstance(getObjectManager());
} else if (obj->getObjID() == MagState::OBJID) {
MagState *mag = MagState::GetInstance(getObjectManager());
Q_ASSERT(mag);
Magnetometer::DataFields magData = mag->getData();
MagState::DataFields magData = mag->getData();
mag_accum_x.append(magData.x);
mag_accum_y.append(magData.y);
mag_accum_z.append(magData.z);
@ -677,16 +677,16 @@ void ConfigRevoWidget::doGetSixPointCalibrationMeasurement(UAVObject *obj)
#ifdef SIX_POINT_CAL_ACCEL
// Store the mean for this position for the accel
Accels *accels = Accels::GetInstance(getObjectManager());
Q_ASSERT(accels);
disconnect(accels, SIGNAL(objectUpdated(UAVObject *)), this, SLOT(doGetSixPointCalibrationMeasurement(UAVObject *)));
AccelState *accelState = AccelState::GetInstance(getObjectManager());
Q_ASSERT(accelState);
disconnect(accelState, SIGNAL(objectUpdated(UAVObject *)), this, SLOT(doGetSixPointCalibrationMeasurement(UAVObject *)));
accel_data_x[position] = listMean(accel_accum_x);
accel_data_y[position] = listMean(accel_accum_y);
accel_data_z[position] = listMean(accel_accum_z);
#endif
// Store the mean for this position for the mag
Magnetometer *mag = Magnetometer::GetInstance(getObjectManager());
MagState *mag = MagState::GetInstance(getObjectManager());
Q_ASSERT(mag);
disconnect(mag, SIGNAL(objectUpdated(UAVObject *)), this, SLOT(doGetSixPointCalibrationMeasurement(UAVObject *)));
mag_data_x[position] = listMean(mag_accum_x);
@ -721,9 +721,9 @@ void ConfigRevoWidget::doGetSixPointCalibrationMeasurement(UAVObject *obj)
/* Cleanup original settings */
#ifdef SIX_POINT_CAL_ACCEL
accels->setMetadata(initialAccelsMdata);
accelState->setMetadata(initialAccelStateMdata);
#endif
mag->setMetadata(initialMagMdata);
mag->setMetadata(initialMagStateMdata);
// Recall saved board rotation
recallBoardRotation();
@ -925,36 +925,36 @@ void ConfigRevoWidget::doStartNoiseMeasurement()
mag_accum_z.clear();
/* Need to get as many accel, mag and gyro updates as possible */
Accels *accels = Accels::GetInstance(getObjectManager());
Q_ASSERT(accels);
Gyros *gyros = Gyros::GetInstance(getObjectManager());
Q_ASSERT(gyros);
Magnetometer *mag = Magnetometer::GetInstance(getObjectManager());
AccelState *accelState = AccelState::GetInstance(getObjectManager());
Q_ASSERT(accelState);
GyroState *gyroState = GyroState::GetInstance(getObjectManager());
Q_ASSERT(gyroState);
MagState *mag = MagState::GetInstance(getObjectManager());
Q_ASSERT(mag);
UAVObject::Metadata mdata;
initialAccelsMdata = accels->getMetadata();
mdata = initialAccelsMdata;
initialAccelStateMdata = accelState->getMetadata();
mdata = initialAccelStateMdata;
UAVObject::SetFlightTelemetryUpdateMode(mdata, UAVObject::UPDATEMODE_PERIODIC);
mdata.flightTelemetryUpdatePeriod = 100;
accels->setMetadata(mdata);
accelState->setMetadata(mdata);
initialGyrosMdata = gyros->getMetadata();
mdata = initialGyrosMdata;
initialGyroStateMdata = gyroState->getMetadata();
mdata = initialGyroStateMdata;
UAVObject::SetFlightTelemetryUpdateMode(mdata, UAVObject::UPDATEMODE_PERIODIC);
mdata.flightTelemetryUpdatePeriod = 100;
gyros->setMetadata(mdata);
gyroState->setMetadata(mdata);
initialMagMdata = mag->getMetadata();
mdata = initialMagMdata;
initialMagStateMdata = mag->getMetadata();
mdata = initialMagStateMdata;
UAVObject::SetFlightTelemetryUpdateMode(mdata, UAVObject::UPDATEMODE_PERIODIC);
mdata.flightTelemetryUpdatePeriod = 100;
mag->setMetadata(mdata);
/* Connect for updates */
connect(accels, SIGNAL(objectUpdated(UAVObject *)), this, SLOT(doGetNoiseSample(UAVObject *)));
connect(gyros, SIGNAL(objectUpdated(UAVObject *)), this, SLOT(doGetNoiseSample(UAVObject *)));
connect(accelState, SIGNAL(objectUpdated(UAVObject *)), this, SLOT(doGetNoiseSample(UAVObject *)));
connect(gyroState, SIGNAL(objectUpdated(UAVObject *)), this, SLOT(doGetNoiseSample(UAVObject *)));
connect(mag, SIGNAL(objectUpdated(UAVObject *)), this, SLOT(doGetNoiseSample(UAVObject *)));
}
@ -971,31 +971,31 @@ void ConfigRevoWidget::doGetNoiseSample(UAVObject *obj)
Q_ASSERT(obj);
switch (obj->getObjID()) {
case Gyros::OBJID:
case GyroState::OBJID:
{
Gyros *gyros = Gyros::GetInstance(getObjectManager());
Q_ASSERT(gyros);
Gyros::DataFields gyroData = gyros->getData();
GyroState *gyroState = GyroState::GetInstance(getObjectManager());
Q_ASSERT(gyroState);
GyroState::DataFields gyroData = gyroState->getData();
gyro_accum_x.append(gyroData.x);
gyro_accum_y.append(gyroData.y);
gyro_accum_z.append(gyroData.z);
break;
}
case Accels::OBJID:
case AccelState::OBJID:
{
Accels *accels = Accels::GetInstance(getObjectManager());
Q_ASSERT(accels);
Accels::DataFields accelsData = accels->getData();
accel_accum_x.append(accelsData.x);
accel_accum_y.append(accelsData.y);
accel_accum_z.append(accelsData.z);
AccelState *accelState = AccelState::GetInstance(getObjectManager());
Q_ASSERT(accelState);
AccelState::DataFields accelStateData = accelState->getData();
accel_accum_x.append(accelStateData.x);
accel_accum_y.append(accelStateData.y);
accel_accum_z.append(accelStateData.z);
break;
}
case Magnetometer::OBJID:
case MagState::OBJID:
{
Magnetometer *mags = Magnetometer::GetInstance(getObjectManager());
MagState *mags = MagState::GetInstance(getObjectManager());
Q_ASSERT(mags);
Magnetometer::DataFields magData = mags->getData();
MagState::DataFields magData = mags->getData();
mag_accum_x.append(magData.x);
mag_accum_y.append(magData.y);
mag_accum_z.append(magData.z);
@ -1018,11 +1018,11 @@ void ConfigRevoWidget::doGetNoiseSample(UAVObject *obj)
gyro_accum_x.length() >= NOISE_SAMPLES &&
accel_accum_x.length() >= NOISE_SAMPLES) {
// No need to for more updates
Magnetometer *mags = Magnetometer::GetInstance(getObjectManager());
Accels *accels = Accels::GetInstance(getObjectManager());
Gyros *gyros = Gyros::GetInstance(getObjectManager());
disconnect(accels, SIGNAL(objectUpdated(UAVObject *)), this, SLOT(doGetNoiseSample(UAVObject *)));
disconnect(gyros, SIGNAL(objectUpdated(UAVObject *)), this, SLOT(doGetNoiseSample(UAVObject *)));
MagState *mags = MagState::GetInstance(getObjectManager());
AccelState *accelState = AccelState::GetInstance(getObjectManager());
GyroState *gyroState = GyroState::GetInstance(getObjectManager());
disconnect(accelState, SIGNAL(objectUpdated(UAVObject *)), this, SLOT(doGetNoiseSample(UAVObject *)));
disconnect(gyroState, SIGNAL(objectUpdated(UAVObject *)), this, SLOT(doGetNoiseSample(UAVObject *)));
disconnect(mags, SIGNAL(objectUpdated(UAVObject *)), this, SLOT(doGetNoiseSample(UAVObject *)));
EKFConfiguration *ekfConfiguration = EKFConfiguration::GetInstance(getObjectManager());

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

@ -90,10 +90,10 @@ private:
double accel_data_x[6], accel_data_y[6], accel_data_z[6];
double mag_data_x[6], mag_data_y[6], mag_data_z[6];
UAVObject::Metadata initialAccelsMdata;
UAVObject::Metadata initialGyrosMdata;
UAVObject::Metadata initialMagMdata;
UAVObject::Metadata initialBaroMdata;
UAVObject::Metadata initialAccelStateMdata;
UAVObject::Metadata initialGyroStateMdata;
UAVObject::Metadata initialMagStateMdata;
UAVObject::Metadata initialBaroSensorMdata;
float initialMagCorrectionRate;
int position;

4
ground/openpilotgcs/src/plugins/gpsdisplay/telemetryparser.cpp
View File

@ -39,12 +39,12 @@ TelemetryParser::TelemetryParser(QObject *parent) : GPSParser(parent)
{
ExtensionSystem::PluginManager *pm = ExtensionSystem::PluginManager::instance();
UAVObjectManager *objManager = pm->getObject<UAVObjectManager>();
UAVDataObject *gpsObj = dynamic_cast<UAVDataObject *>(objManager->getObject("GPSPosition"));
UAVDataObject *gpsObj = dynamic_cast<UAVDataObject *>(objManager->getObject("GPSPositionSensor"));
if (gpsObj != NULL) {
connect(gpsObj, SIGNAL(objectUpdated(UAVObject *)), this, SLOT(updateGPS(UAVObject *)));
} else {
qDebug() << "Error: Object is unknown (GPSPosition).";
qDebug() << "Error: Object is unknown (GPSPositionSensor).";
}
gpsObj = dynamic_cast<UAVDataObject *>(objManager->getObject("GPSTime"));

24
ground/openpilotgcs/src/plugins/hitl/fgsimulator.cpp
View File

@ -268,12 +268,12 @@ void FGSimulator::processUpdate(const QByteArray & inp)
float temperature = fields[19].toFloat();
// Get pressure (kpa)
float pressure = fields[20].toFloat() * INHG2KPA;
// Get VelocityActual Down (cm/s)
float velocityActualDown = -fields[21].toFloat() * FPS2CMPS;
// Get VelocityActual East (cm/s)
float velocityActualEast = fields[22].toFloat() * FPS2CMPS;
// Get VelocityActual Down (cm/s)
float velocityActualNorth = fields[23].toFloat() * FPS2CMPS;
// Get VelocityState Down (cm/s)
float velocityStateDown = -fields[21].toFloat() * FPS2CMPS;
// Get VelocityState East (cm/s)
float velocityStateEast = fields[22].toFloat() * FPS2CMPS;
// Get VelocityState Down (cm/s)
float velocityStateNorth = fields[23].toFloat() * FPS2CMPS;
// Get UDP packets received by FG
int n = fields[24].toInt();
@ -307,11 +307,11 @@ void FGSimulator::processUpdate(const QByteArray & inp)
out.calibratedAirspeed = airspeed;
// Update BaroAltitude object
// Update BaroSensor object
out.temperature = temperature;
out.pressure = pressure;
// Update attActual object
// Update attState object
out.roll = roll; // roll;
out.pitch = pitch; // pitch
out.heading = yaw; // yaw
@ -320,10 +320,10 @@ void FGSimulator::processUpdate(const QByteArray & inp)
out.dstE = NED[1];
out.dstD = NED[2];
// Update VelocityActual.{North,East,Down}
out.velNorth = velocityActualNorth;
out.velEast = velocityActualEast;
out.velDown = velocityActualDown;
// Update VelocityState.{North,East,Down}
out.velNorth = velocityStateNorth;
out.velEast = velocityStateEast;
out.velDown = velocityStateDown;
// Update gyroscope sensor data
out.rollRate = rollRate;

22
ground/openpilotgcs/src/plugins/hitl/hitlconfiguration.cpp
View File

@ -47,7 +47,7 @@ HITLConfiguration::HITLConfiguration(QString classId, QSettings *qSettings, QObj
settings.attRawEnabled = false;
settings.attRawRate = 20;
settings.attActualEnabled = true;
settings.attStateEnabled = true;
settings.attActHW = false;
settings.attActSim = true;
settings.attActCalc = false;
@ -63,8 +63,8 @@ HITLConfiguration::HITLConfiguration(QString classId, QSettings *qSettings, QObj
settings.manualControlEnabled = false;
settings.minOutputPeriod = 100;
settings.airspeedActualEnabled = false;
settings.airspeedActualRate = 100;
settings.airspeedStateEnabled = false;
settings.airspeedStateRate = 100;
// if a saved configuration exists load it, and overwrite defaults
@ -89,12 +89,12 @@ HITLConfiguration::HITLConfiguration(QString classId, QSettings *qSettings, QObj
settings.attRawEnabled = qSettings->value("attRawEnabled").toBool();
settings.attRawRate = qSettings->value("attRawRate").toInt();
settings.attActualEnabled = qSettings->value("attActualEnabled").toBool();
settings.attStateEnabled = qSettings->value("attStateEnabled").toBool();
settings.attActHW = qSettings->value("attActHW").toBool();
settings.attActSim = qSettings->value("attActSim").toBool();
settings.attActCalc = qSettings->value("attActCalc").toBool();
settings.baroAltitudeEnabled = qSettings->value("baroAltitudeEnabled").toBool();
settings.baroSensorEnabled = qSettings->value("baroSensorEnabled").toBool();
settings.baroAltRate = qSettings->value("baroAltRate").toInt();
settings.gpsPositionEnabled = qSettings->value("gpsPositionEnabled").toBool();
@ -106,8 +106,8 @@ HITLConfiguration::HITLConfiguration(QString classId, QSettings *qSettings, QObj
settings.inputCommand = qSettings->value("inputCommand").toBool();
settings.minOutputPeriod = qSettings->value("minOutputPeriod").toInt();
settings.airspeedActualEnabled = qSettings->value("airspeedActualEnabled").toBool();
settings.airspeedActualRate = qSettings->value("airspeedActualRate").toInt();
settings.airspeedStateEnabled = qSettings->value("airspeedStateEnabled").toBool();
settings.airspeedStateRate = qSettings->value("airspeedStateRate").toInt();
}
}
@ -144,11 +144,11 @@ void HITLConfiguration::saveConfig(QSettings *qSettings) const
qSettings->setValue("attRawEnabled", settings.attRawEnabled);
qSettings->setValue("attRawRate", settings.attRawRate);
qSettings->setValue("attActualEnabled", settings.attActualEnabled);
qSettings->setValue("attStateEnabled", settings.attStateEnabled);
qSettings->setValue("attActHW", settings.attActHW);
qSettings->setValue("attActSim", settings.attActSim);
qSettings->setValue("attActCalc", settings.attActCalc);
qSettings->setValue("baroAltitudeEnabled", settings.baroAltitudeEnabled);
qSettings->setValue("baroSensorEnabled", settings.baroSensorEnabled);
qSettings->setValue("baroAltRate", settings.baroAltRate);
qSettings->setValue("gpsPositionEnabled", settings.gpsPositionEnabled);
qSettings->setValue("gpsPosRate", settings.gpsPosRate);
@ -157,6 +157,6 @@ void HITLConfiguration::saveConfig(QSettings *qSettings) const
qSettings->setValue("inputCommand", settings.inputCommand);
qSettings->setValue("minOutputPeriod", settings.minOutputPeriod);
qSettings->setValue("airspeedActualEnabled", settings.airspeedActualEnabled);
qSettings->setValue("airspeedActualRate", settings.airspeedActualRate);
qSettings->setValue("airspeedStateEnabled", settings.airspeedStateEnabled);
qSettings->setValue("airspeedStateRate", settings.airspeedStateRate);
}

26
ground/openpilotgcs/src/plugins/hitl/hitlnoisegeneration.cpp
View File

@ -44,9 +44,9 @@ Noise HitlNoiseGeneration::getNoise()
Noise HitlNoiseGeneration::generateNoise()
{
noise.accelData.x = 0;
noise.accelData.y = 0;
noise.accelData.z = 0;
noise.accelStateData.x = 0;
noise.accelStateData.y = 0;
noise.accelStateData.z = 0;
noise.gpsPosData.Latitude = 0;
noise.gpsPosData.Longitude = 0;
@ -60,19 +60,19 @@ Noise HitlNoiseGeneration::generateNoise()
noise.baroAltData.Altitude = 0;
noise.attActualData.Roll = 0;
noise.attActualData.Pitch = 0;
noise.attActualData.Yaw = 0;
noise.attStateData.Roll = 0;
noise.attStateData.Pitch = 0;
noise.attStateData.Yaw = 0;
noise.gyroData.x = 0;
noise.gyroData.y = 0;
noise.gyroData.z = 0;
noise.gyroStateData.x = 0;
noise.gyroStateData.y = 0;
noise.gyroStateData.z = 0;
noise.accelData.x = 0;
noise.accelData.y = 0;
noise.accelData.z = 0;
noise.accelStateData.x = 0;
noise.accelStateData.y = 0;
noise.accelStateData.z = 0;
noise.airspeedActual.CalibratedAirspeed = 0;
noise.airspeedState.CalibratedAirspeed = 0;
return noise;
}

18
ground/openpilotgcs/src/plugins/hitl/hitlnoisegeneration.h
View File

@ -37,16 +37,16 @@
#include <coreplugin/threadmanager.h>
struct Noise {
Accels::DataFields accelData;
AttitudeActual::DataFields attActualData;
BaroAltitude::DataFields baroAltData;
AirspeedActual::DataFields airspeedActual;
GPSPosition::DataFields gpsPosData;
GPSVelocity::DataFields gpsVelData;
Gyros::DataFields gyroData;
AccelState::DataFields accelStateData;
AttitudeState::DataFields attStateData;
BaroSensor::DataFields baroAltData;
AirspeedState::DataFields airspeedState;
GPSPositionSensor::DataFields gpsPosData;
GPSVelocitySensor::DataFields gpsVelData;
GyroState::DataFields gyroStateData;
HomeLocation::DataFields homeData;
PositionActual::DataFields positionActualData;
VelocityActual::DataFields velocityActualData;
PositionState::DataFields positionStateData;
VelocityState::DataFields velocityStateData;
};
class HitlNoiseGeneration {

18
ground/openpilotgcs/src/plugins/hitl/hitloptionspage.cpp
View File

@ -92,16 +92,16 @@ QWidget *HITLOptionsPage::createPage(QWidget *parent)
m_optionsPage->groundTruthCheckbox->setChecked(config->Settings().groundTruthEnabled);
m_optionsPage->gpsPositionCheckbox->setChecked(config->Settings().gpsPositionEnabled);
m_optionsPage->attActualCheckbox->setChecked(config->Settings().attActualEnabled);
m_optionsPage->attStateCheckbox->setChecked(config->Settings().attStateEnabled);
m_optionsPage->attRawCheckbox->setChecked(config->Settings().attRawEnabled);
m_optionsPage->attRawRateSpinbox->setValue(config->Settings().attRawRate);
m_optionsPage->gpsPosRateSpinbox->setValue(config->Settings().gpsPosRate);
m_optionsPage->groundTruthRateSpinbox->setValue(config->Settings().groundTruthRate);
// m_optionsPage->attActualRate->setValue(config->Settings().attActualRate);
// m_optionsPage->attStateRate->setValue(config->Settings().attStateRate);
m_optionsPage->baroAltitudeCheckbox->setChecked(config->Settings().baroAltitudeEnabled);
m_optionsPage->baroAltitudeCheckbox->setChecked(config->Settings().baroSensorEnabled);
m_optionsPage->baroAltRateSpinbox->setValue(config->Settings().baroAltRate);
m_optionsPage->minOutputPeriodSpinbox->setValue(config->Settings().minOutputPeriod);
@ -110,8 +110,8 @@ QWidget *HITLOptionsPage::createPage(QWidget *parent)
m_optionsPage->attActCalc->setChecked(config->Settings().attActCalc);
m_optionsPage->attActSim->setChecked(config->Settings().attActSim);
m_optionsPage->airspeedActualCheckbox->setChecked(config->Settings().airspeedActualEnabled);
m_optionsPage->airspeedRateSpinbox->setValue(config->Settings().airspeedActualRate);
m_optionsPage->airspeedStateCheckbox->setChecked(config->Settings().airspeedStateEnabled);
m_optionsPage->airspeedRateSpinbox->setValue(config->Settings().airspeedStateRate);
return optionsPageWidget;
}
@ -139,7 +139,7 @@ void HITLOptionsPage::apply()
settings.attRawEnabled = m_optionsPage->attRawCheckbox->isChecked();
settings.attRawRate = m_optionsPage->attRawRateSpinbox->value();
settings.attActualEnabled = m_optionsPage->attActualCheckbox->isChecked();
settings.attStateEnabled = m_optionsPage->attStateCheckbox->isChecked();
settings.gpsPositionEnabled = m_optionsPage->gpsPositionCheckbox->isChecked();
settings.gpsPosRate = m_optionsPage->gpsPosRateSpinbox->value();
@ -147,7 +147,7 @@ void HITLOptionsPage::apply()
settings.groundTruthEnabled = m_optionsPage->groundTruthCheckbox->isChecked();
settings.groundTruthRate = m_optionsPage->groundTruthRateSpinbox->value();
settings.baroAltitudeEnabled = m_optionsPage->baroAltitudeCheckbox->isChecked();
settings.baroSensorEnabled = m_optionsPage->baroAltitudeCheckbox->isChecked();
settings.baroAltRate = m_optionsPage->baroAltRateSpinbox->value();
settings.minOutputPeriod = m_optionsPage->minOutputPeriodSpinbox->value();
@ -159,8 +159,8 @@ void HITLOptionsPage::apply()
settings.attActSim = m_optionsPage->attActSim->isChecked();
settings.attActCalc = m_optionsPage->attActCalc->isChecked();
settings.airspeedActualEnabled = m_optionsPage->airspeedActualCheckbox->isChecked();
settings.airspeedActualRate = m_optionsPage->airspeedRateSpinbox->value();
settings.airspeedStateEnabled = m_optionsPage->airspeedStateCheckbox->isChecked();
settings.airspeedStateRate = m_optionsPage->airspeedRateSpinbox->value();
// Write settings to file
config->setSimulatorSettings(settings);

8
ground/openpilotgcs/src/plugins/hitl/hitloptionspage.ui
View File

@ -510,12 +510,12 @@
</widget>
</item>
<item>
<widget class="QGroupBox" name="attActualCheckbox">
<widget class="QGroupBox" name="attStateCheckbox">
<property name="enabled">
<bool>true</bool>
</property>
<property name="title">
<string>AttitudeActual</string>
<string>AttitudeState</string>
</property>
<property name="flat">
<bool>true</bool>
@ -760,7 +760,7 @@
<number>0</number>
</property>
<item>
<widget class="QGroupBox" name="airspeedActualCheckbox">
<widget class="QGroupBox" name="airspeedStateCheckbox">
<property name="minimumSize">
<size>
<width>0</width>
@ -768,7 +768,7 @@
</size>
</property>
<property name="title">
<string>AirspeedActual</string>
<string>AirspeedState</string>
</property>
<property name="flat">
<bool>true</bool>

6
ground/openpilotgcs/src/plugins/hitl/il2simulator.cpp
View File

@ -262,20 +262,20 @@ void IL2Simulator::processUpdate(const QByteArray & inp)
out.dstE = current.X;
out.dstD = -current.Z;
// Update BaroAltitude object
// Update BaroSensor object
out.altitude = current.Z;
out.agl = current.Z;
out.temperature = airParameters.groundTemp + (current.Z * airParameters.tempLapseRate) - 273.0;
out.pressure = airPressureFromAltitude(current.Z, airParameters, gravity); // kpa
// Update attActual object
// Update attState object
out.roll = current.roll; // roll;
out.pitch = current.pitch; // pitch
out.heading = current.azimuth; // yaw
// Update VelocityActual.{North,East,Down}
// Update VelocityState.{North,East,Down}
out.velNorth = current.dY;
out.velEast = current.dX;
out.velDown = -current.dZ;

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

@ -9,10 +9,10 @@
#include "uavtalk/telemetrymanager.h"
#include "uavobjectmanager.h"
#include "actuatordesired.h"
#include "altitudeactual.h"
#include "attitudeactual.h"
#include "velocityactual.h"
#include "positionactual.h"
#include "altitudestate.h"
#include "attitudestate.h"
#include "velocitystate.h"
#include "positionstate.h"
#include "gcstelemetrystats.h"
class Simulator : public QObject {
@ -48,10 +48,10 @@ private:
QUdpSocket *inSocket;
QUdpSocket *outSocket;
ActuatorDesired *actDesired;
AltitudeActual *altActual;
VelocityActual *velActual;
AttitudeActual *attActual;
PositionActual *posActual;
AltitudeState *altState;
VelocityState *velState;
AttitudeState *attState;
PositionState *posState;
GCSTelemetryStats *telStats;
QHostAddress fgHost;
int inPort;

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

@ -70,7 +70,7 @@ Simulator::Simulator(const SimulatorSettings & params) :
attRawTime = currentTime;
baroAltTime = currentTime;
battTime = currentTime;
airspeedActualTime = currentTime;
airspeedStateTime = currentTime;
// Define standard atmospheric constants
airParameters.univGasConstant = 8.31447; // [J/(mol·K)]
@ -147,17 +147,17 @@ void Simulator::onStart()
gcsReceiver = GCSReceiver::GetInstance(objManager);
flightStatus = FlightStatus::GetInstance(objManager);
posHome = HomeLocation::GetInstance(objManager);
velActual = VelocityActual::GetInstance(objManager);
posActual = PositionActual::GetInstance(objManager);
baroAlt = BaroAltitude::GetInstance(objManager);
velState = VelocityState::GetInstance(objManager);
posState = PositionState::GetInstance(objManager);
baroAlt = BaroSensor::GetInstance(objManager);
flightBatt = FlightBatteryState::GetInstance(objManager);
airspeedActual = AirspeedActual::GetInstance(objManager);
attActual = AttitudeActual::GetInstance(objManager);
airspeedState = AirspeedState::GetInstance(objManager);
attState = AttitudeState::GetInstance(objManager);
attSettings = AttitudeSettings::GetInstance(objManager);
accels = Accels::GetInstance(objManager);
gyros = Gyros::GetInstance(objManager);
gpsPos = GPSPosition::GetInstance(objManager);
gpsVel = GPSVelocity::GetInstance(objManager);
accelState = AccelState::GetInstance(objManager);
gyroState = GyroState::GetInstance(objManager);
gpsPos = GPSPositionSensor::GetInstance(objManager);
gpsVel = GPSVelocitySensor::GetInstance(objManager);
telStats = GCSTelemetryStats::GetInstance(objManager);
groundTruth = GroundTruth::GetInstance(objManager);
@ -257,30 +257,30 @@ void Simulator::setupObjects()
}
if (settings.groundTruthEnabled) {
setupOutputObject(posActual, settings.groundTruthRate);
setupOutputObject(velActual, settings.groundTruthRate);
setupOutputObject(posState, settings.groundTruthRate);
setupOutputObject(velState, settings.groundTruthRate);
}
if (settings.attRawEnabled) {
setupOutputObject(accels, settings.attRawRate);
setupOutputObject(gyros, settings.attRawRate);
setupOutputObject(accelState, settings.attRawRate);
setupOutputObject(gyroState, settings.attRawRate);
}
if (settings.attActualEnabled && settings.attActHW) {
setupOutputObject(accels, settings.attRawRate);
setupOutputObject(gyros, settings.attRawRate);
if (settings.attStateEnabled && settings.attActHW) {
setupOutputObject(accelState, settings.attRawRate);
setupOutputObject(gyroState, settings.attRawRate);
}
if (settings.attActualEnabled && !settings.attActHW) {
setupOutputObject(attActual, 20); // Hardcoded? Bleh.
if (settings.attStateEnabled && !settings.attActHW) {
setupOutputObject(attState, 20); // Hardcoded? Bleh.
} else {
setupWatchedObject(attActual, 100); // Hardcoded? Bleh.
setupWatchedObject(attState, 100); // Hardcoded? Bleh.
}
if (settings.airspeedActualEnabled) {
setupOutputObject(airspeedActual, settings.airspeedActualRate);
if (settings.airspeedStateEnabled) {
setupOutputObject(airspeedState, settings.airspeedStateRate);
}
if (settings.baroAltitudeEnabled) {
if (settings.baroSensorEnabled) {
setupOutputObject(baroAlt, settings.baroAltRate);
setupOutputObject(flightBatt, settings.baroAltRate);
}
@ -462,31 +462,31 @@ void Simulator::updateUAVOs(Output2Hardware out)
groundTruth->setData(groundTruthData);
/*******************************/
// Update attActual object
AttitudeActual::DataFields attActualData;
attActualData = attActual->getData();
// Update attState object
AttitudeState::DataFields attStateData;
attStateData = attState->getData();
if (settings.attActHW) {
// do nothing
/*****************************************/
} else if (settings.attActSim) {
// take all data from simulator
attActualData.Roll = out.roll + noise.attActualData.Roll; // roll;
attActualData.Pitch = out.pitch + noise.attActualData.Pitch; // pitch
attActualData.Yaw = out.heading + noise.attActualData.Yaw; // Yaw
attStateData.Roll = out.roll + noise.attStateData.Roll; // roll;
attStateData.Pitch = out.pitch + noise.attStateData.Pitch; // pitch
attStateData.Yaw = out.heading + noise.attStateData.Yaw; // Yaw
float rpy[3];
float quat[4];
rpy[0] = attActualData.Roll;
rpy[1] = attActualData.Pitch;
rpy[2] = attActualData.Yaw;
rpy[0] = attStateData.Roll;
rpy[1] = attStateData.Pitch;
rpy[2] = attStateData.Yaw;
Utils::CoordinateConversions().RPY2Quaternion(rpy, quat);
attActualData.q1 = quat[0];
attActualData.q2 = quat[1];
attActualData.q3 = quat[2];
attActualData.q4 = quat[3];
attStateData.q1 = quat[0];
attStateData.q2 = quat[1];
attStateData.q3 = quat[2];
attStateData.q4 = quat[3];
// Set UAVO
attActual->setData(attActualData);
attState->setData(attStateData);
/*****************************************/
} else if (settings.attActCalc) {
// calculate RPY with code from Attitude module
@ -598,16 +598,16 @@ void Simulator::updateUAVOs(Output2Hardware out)
rpy2[0] = RAD2DEG * atan2f(R23, R33);
}
attActualData.Roll = rpy2[0];
attActualData.Pitch = rpy2[1];
attActualData.Yaw = rpy2[2];
attActualData.q1 = q[0];
attActualData.q2 = q[1];
attActualData.q3 = q[2];
attActualData.q4 = q[3];
attStateData.Roll = rpy2[0];
attStateData.Pitch = rpy2[1];
attStateData.Yaw = rpy2[2];
attStateData.q1 = q[0];
attStateData.q2 = q[1];
attStateData.q3 = q[2];
attStateData.q4 = q[3];
// Set UAVO
attActual->setData(attActualData);
attState->setData(attStateData);
/*****************************************/
}
@ -633,8 +633,8 @@ void Simulator::updateUAVOs(Output2Hardware out)
if (gpsPosTime.msecsTo(currentTime) >= settings.gpsPosRate) {
qDebug() << " GPS time:" << gpsPosTime << ", currentTime: " << currentTime << ", difference: " << gpsPosTime.msecsTo(currentTime);
// Update GPS Position objects
GPSPosition::DataFields gpsPosData;
memset(&gpsPosData, 0, sizeof(GPSPosition::DataFields));
GPSPositionSensor::DataFields gpsPosData;
memset(&gpsPosData, 0, sizeof(GPSPositionSensor::DataFields));
gpsPosData.Altitude = out.altitude + noise.gpsPosData.Altitude;
gpsPosData.Heading = out.heading + noise.gpsPosData.Heading;
gpsPosData.Groundspeed = out.groundspeed + noise.gpsPosData.Groundspeed;
@ -644,13 +644,13 @@ void Simulator::updateUAVOs(Output2Hardware out)
gpsPosData.PDOP = 3.0;
gpsPosData.VDOP = gpsPosData.PDOP * 1.5;
gpsPosData.Satellites = 10;
gpsPosData.Status = GPSPosition::STATUS_FIX3D;
gpsPosData.Status = GPSPositionSensor::STATUS_FIX3D;
gpsPos->setData(gpsPosData);
// Update GPS Velocity.{North,East,Down}
GPSVelocity::DataFields gpsVelData;
memset(&gpsVelData, 0, sizeof(GPSVelocity::DataFields));
GPSVelocitySensor::DataFields gpsVelData;
memset(&gpsVelData, 0, sizeof(GPSVelocitySensor::DataFields));
gpsVelData.North = out.velNorth + noise.gpsVelData.North;
gpsVelData.East = out.velEast + noise.gpsVelData.East;
gpsVelData.Down = out.velDown + noise.gpsVelData.Down;
@ -662,23 +662,23 @@ void Simulator::updateUAVOs(Output2Hardware out)
}
/*******************************/
// Update VelocityActual.{North,East,Down}
// Update VelocityState.{North,East,Down}
if (settings.groundTruthEnabled) {
if (groundTruthTime.msecsTo(currentTime) >= settings.groundTruthRate) {
VelocityActual::DataFields velocityActualData;
memset(&velocityActualData, 0, sizeof(VelocityActual::DataFields));
velocityActualData.North = out.velNorth + noise.velocityActualData.North;
velocityActualData.East = out.velEast + noise.velocityActualData.East;
velocityActualData.Down = out.velDown + noise.velocityActualData.Down;
velActual->setData(velocityActualData);
VelocityState::DataFields velocityStateData;
memset(&velocityStateData, 0, sizeof(VelocityState::DataFields));
velocityStateData.North = out.velNorth + noise.velocityStateData.North;
velocityStateData.East = out.velEast + noise.velocityStateData.East;
velocityStateData.Down = out.velDown + noise.velocityStateData.Down;
velState->setData(velocityStateData);
// Update PositionActual.{Nort,East,Down}
PositionActual::DataFields positionActualData;
memset(&positionActualData, 0, sizeof(PositionActual::DataFields));
positionActualData.North = (out.dstN - initN) + noise.positionActualData.North;
positionActualData.East = (out.dstE - initE) + noise.positionActualData.East;
positionActualData.Down = (out.dstD /*-initD*/) + noise.positionActualData.Down;
posActual->setData(positionActualData);
// Update PositionState.{Nort,East,Down}
PositionState::DataFields positionStateData;
memset(&positionStateData, 0, sizeof(PositionState::DataFields));
positionStateData.North = (out.dstN - initN) + noise.positionStateData.North;
positionStateData.East = (out.dstE - initE) + noise.positionStateData.East;
positionStateData.Down = (out.dstD /*-initD*/) + noise.positionStateData.Down;
posState->setData(positionStateData);
groundTruthTime = groundTruthTime.addMSecs(settings.groundTruthRate);
}
@ -707,11 +707,11 @@ void Simulator::updateUAVOs(Output2Hardware out)
// }
/*******************************/
// Update BaroAltitude object
if (settings.baroAltitudeEnabled) {
// Update BaroSensor object
if (settings.baroSensorEnabled) {
if (baroAltTime.msecsTo(currentTime) >= settings.baroAltRate) {
BaroAltitude::DataFields baroAltData;
memset(&baroAltData, 0, sizeof(BaroAltitude::DataFields));
BaroSensor::DataFields baroAltData;
memset(&baroAltData, 0, sizeof(BaroSensor::DataFields));
baroAltData.Altitude = out.altitude + noise.baroAltData.Altitude;
baroAltData.Temperature = out.temperature + noise.baroAltData.Temperature;
baroAltData.Pressure = out.pressure + noise.baroAltData.Pressure;
@ -723,7 +723,7 @@ void Simulator::updateUAVOs(Output2Hardware out)
/*******************************/
// Update FlightBatteryState object
if (settings.baroAltitudeEnabled) {
if (settings.baroSensorEnabled) {
if (battTime.msecsTo(currentTime) >= settings.baroAltRate) {
FlightBatteryState::DataFields batteryData;
memset(&batteryData, 0, sizeof(FlightBatteryState::DataFields));
@ -737,18 +737,18 @@ void Simulator::updateUAVOs(Output2Hardware out)
}
/*******************************/
// Update AirspeedActual object
if (settings.airspeedActualEnabled) {
if (airspeedActualTime.msecsTo(currentTime) >= settings.airspeedActualRate) {
AirspeedActual::DataFields airspeedActualData;
memset(&airspeedActualData, 0, sizeof(AirspeedActual::DataFields));
airspeedActualData.CalibratedAirspeed = out.calibratedAirspeed + noise.airspeedActual.CalibratedAirspeed;
airspeedActualData.TrueAirspeed = out.trueAirspeed + noise.airspeedActual.TrueAirspeed;
// airspeedActualData.alpha=out.angleOfAttack; // to be implemented
// airspeedActualData.beta=out.angleOfSlip;
airspeedActual->setData(airspeedActualData);
// Update AirspeedState object
if (settings.airspeedStateEnabled) {
if (airspeedStateTime.msecsTo(currentTime) >= settings.airspeedStateRate) {
AirspeedState::DataFields airspeedStateData;
memset(&airspeedStateData, 0, sizeof(AirspeedState::DataFields));
airspeedStateData.CalibratedAirspeed = out.calibratedAirspeed + noise.airspeedState.CalibratedAirspeed;
airspeedStateData.TrueAirspeed = out.trueAirspeed + noise.airspeedState.TrueAirspeed;
// airspeedStateData.alpha=out.angleOfAttack; // to be implemented
// airspeedStateData.beta=out.angleOfSlip;
airspeedState->setData(airspeedStateData);
airspeedActualTime = airspeedActualTime.addMSecs(settings.airspeedActualRate);
airspeedStateTime = airspeedStateTime.addMSecs(settings.airspeedStateRate);
}
}
@ -757,20 +757,20 @@ void Simulator::updateUAVOs(Output2Hardware out)
if (settings.attRawEnabled) {
if (attRawTime.msecsTo(currentTime) >= settings.attRawRate) {
// Update gyroscope sensor data
Gyros::DataFields gyroData;
memset(&gyroData, 0, sizeof(Gyros::DataFields));
gyroData.x = out.rollRate + noise.gyroData.x;
gyroData.y = out.pitchRate + noise.gyroData.y;
gyroData.z = out.yawRate + noise.gyroData.z;
gyros->setData(gyroData);
GyroState::DataFields gyroStateData;
memset(&gyroStateData, 0, sizeof(GyroState::DataFields));
gyroStateData.x = out.rollRate + noise.gyroStateData.x;
gyroStateData.y = out.pitchRate + noise.gyroStateData.y;
gyroStateData.z = out.yawRate + noise.gyroStateData.z;
gyroState->setData(gyroStateData);
// Update accelerometer sensor data
Accels::DataFields accelData;
memset(&accelData, 0, sizeof(Accels::DataFields));
accelData.x = out.accX + noise.accelData.x;
accelData.y = out.accY + noise.accelData.y;
accelData.z = out.accZ + noise.accelData.z;
accels->setData(accelData);
AccelState::DataFields accelStateData;
memset(&accelStateData, 0, sizeof(AccelState::DataFields));
accelStateData.x = out.accX + noise.accelStateData.x;
accelStateData.y = out.accY + noise.accelStateData.y;
accelStateData.z = out.accZ + noise.accelStateData.z;
accelState->setData(accelStateData);
attRawTime = attRawTime.addMSecs(settings.attRawRate);
}

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

@ -38,26 +38,26 @@
#include "uavtalk/telemetrymanager.h"
#include "uavobjectmanager.h"
#include "accels.h"
#include "accelstate.h"
#include "actuatorcommand.h"
#include "actuatordesired.h"
#include "airspeedactual.h"
#include "attitudeactual.h"
#include "airspeedstate.h"
#include "attitudestate.h"
#include "attitudesettings.h"
#include "baroaltitude.h"
#include "barosensor.h"
#include "flightbatterystate.h"
#include "flightstatus.h"
#include "gcsreceiver.h"
#include "gcstelemetrystats.h"
#include "gpsposition.h"
#include "gpsvelocity.h"
#include "gpspositionsensor.h"
#include "gpsvelocitysensor.h"
#include "groundtruth.h"
#include "gyros.h"
#include "gyrostate.h"
#include "homelocation.h"
#include "manualcontrolcommand.h"
#include "positionactual.h"
#include "positionstate.h"
#include "sonaraltitude.h"
#include "velocityactual.h"
#include "velocitystate.h"
#include "utils/coordinateconversions.h"
@ -130,12 +130,12 @@ typedef struct _CONNECTION {
bool attRawEnabled;
quint8 attRawRate;
bool attActualEnabled;
bool attStateEnabled;
bool attActHW;
bool attActSim;
bool attActCalc;
bool baroAltitudeEnabled;
bool baroSensorEnabled;
quint16 baroAltRate;
bool groundTruthEnabled;
@ -149,8 +149,8 @@ typedef struct _CONNECTION {
bool manualControlEnabled;
quint16 minOutputPeriod;
bool airspeedActualEnabled;
quint16 airspeedActualRate;
bool airspeedStateEnabled;
quint16 airspeedStateRate;
} SimulatorSettings;
@ -321,17 +321,17 @@ protected:
ManualControlCommand *manCtrlCommand;
FlightStatus *flightStatus;
FlightBatteryState *flightBatt;
BaroAltitude *baroAlt;
AirspeedActual *airspeedActual;
AttitudeActual *attActual;
BaroSensor *baroAlt;
AirspeedState *airspeedState;
AttitudeState *attState;
AttitudeSettings *attSettings;
VelocityActual *velActual;
GPSPosition *gpsPos;
GPSVelocity *gpsVel;
PositionActual *posActual;
VelocityState *velState;
GPSPositionSensor *gpsPos;
GPSVelocitySensor *gpsVel;
PositionState *posState;
HomeLocation *posHome;
Accels *accels;
Gyros *gyros;
AccelState *accelState;
GyroState *gyroState;
GCSTelemetryStats *telStats;
GCSReceiver *gcsReceiver;
GroundTruth *groundTruth;
@ -361,7 +361,7 @@ private:
QTime baroAltTime;
QTime battTime;
QTime gcsRcvrTime;
QTime airspeedActualTime;
QTime airspeedStateTime;
QString name;
QString simulatorId;

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

@ -300,11 +300,11 @@ void XplaneSimulator::processUpdate(const QByteArray & dataBuf)
out.calibratedAirspeed = airspeed_keas * 1.15 * 1.6089 / 3.6; // Convert from [kts] to [m/s]
// Update BaroAltitude object
// Update BaroSensor object
out.temperature = temperature;
out.pressure = pressure;
// Update attActual object
// Update attState object
out.roll = roll; // roll;
out.pitch = pitch; // pitch
out.heading = heading; // yaw
@ -314,7 +314,7 @@ void XplaneSimulator::processUpdate(const QByteArray & dataBuf)
out.dstE = dstX;
out.dstD = -dstZ;
// Update VelocityActual.{North,East,Down}
// Update VelocityState.{North,East,Down}
out.velNorth = velY;
out.velEast = velX;
out.velDown = -velZ;
@ -332,9 +332,9 @@ void XplaneSimulator::processUpdate(const QByteArray & dataBuf)
updateUAVOs(out);
}
// issue manual update
// attActual->updated();
// altActual->updated();
// posActual->updated();
// attState->updated();
// altState->updated();
// posState->updated();
}

22
ground/openpilotgcs/src/plugins/magicwaypoint/magicwaypointgadgetwidget.cpp
View File

@ -48,11 +48,11 @@ MagicWaypointGadgetWidget::MagicWaypointGadgetWidget(QWidget *parent) : QLabel(p
// Connect object updated event from UAVObject to also update check boxes
connect(getPathDesired(), SIGNAL(objectUpdated(UAVObject *)), this, SLOT(positionObjectChanged(UAVObject *)));
connect(getPositionActual(), SIGNAL(objectUpdated(UAVObject *)), this, SLOT(positionObjectChanged(UAVObject *)));
connect(getPositionState(), SIGNAL(objectUpdated(UAVObject *)), this, SLOT(positionObjectChanged(UAVObject *)));
// Connect updates from the position widget to this widget
connect(m_magicwaypoint->widgetPosition, SIGNAL(positionClicked(double, double)), this, SLOT(positionSelected(double, double)));
connect(this, SIGNAL(positionActualObjectChanged(double, double)), m_magicwaypoint->widgetPosition, SLOT(updateActualIndicator(double, double)));
connect(this, SIGNAL(positionStateObjectChanged(double, double)), m_magicwaypoint->widgetPosition, SLOT(updateActualIndicator(double, double)));
connect(this, SIGNAL(positionDesiredObjectChanged(double, double)), m_magicwaypoint->widgetPosition, SLOT(updateDesiredIndicator(double, double)));
// Catch changes in scale for visualization
@ -81,13 +81,13 @@ PathDesired *MagicWaypointGadgetWidget::getPathDesired()
}
/*!
\brief Returns the @ref PositionActual UAVObject
\brief Returns the @ref PositionState UAVObject
*/
PositionActual *MagicWaypointGadgetWidget::getPositionActual()
PositionState *MagicWaypointGadgetWidget::getPositionState()
{
ExtensionSystem::PluginManager *pm = ExtensionSystem::PluginManager::instance();
UAVObjectManager *objManager = pm->getObject<UAVObjectManager>();
PositionActual *obj = PositionActual::GetInstance(objManager);
PositionState *obj = PositionState::GetInstance(objManager);
Q_ASSERT(obj != NULL);
return obj;
@ -100,19 +100,19 @@ void MagicWaypointGadgetWidget::scaleChanged(int scale)
{
Q_UNUSED(scale);
pathDesiredChanged(getPathDesired());
positionActualChanged(getPositionActual());
positionStateChanged(getPositionState());
}
/**
* Emit a position changed signal when @ref PositionActual object is changed
* Emit a position changed signal when @ref PositionState object is changed
*/
void MagicWaypointGadgetWidget::positionActualChanged(UAVObject *)
void MagicWaypointGadgetWidget::positionStateChanged(UAVObject *)
{
PositionActual::DataFields positionActual = getPositionActual()->getData();
PositionState::DataFields positionState = getPositionState()->getData();
double scale = m_magicwaypoint->horizontalSliderScale->value();
emit positionActualObjectChanged(positionActual.North / scale,
positionActual.East / scale);
emit positionStateObjectChanged(positionState.North / scale,
positionState.East / scale);
}
/**

8
ground/openpilotgcs/src/plugins/magicwaypoint/magicwaypointgadgetwidget.h
View File

@ -30,7 +30,7 @@
#include <QtGui/QLabel>
#include "pathdesired.h"
#include "positionactual.h"
#include "positionstate.h"
class Ui_MagicWaypoint;
@ -42,18 +42,18 @@ public:
~MagicWaypointGadgetWidget();
signals:
void positionActualObjectChanged(double north, double east);
void positionStateObjectChanged(double north, double east);
void positionDesiredObjectChanged(double north, double east);
protected slots:
void scaleChanged(int scale);
void positionActualChanged(UAVObject *);
void positionStateChanged(UAVObject *);
void pathDesiredChanged(UAVObject *);
void positionSelected(double north, double east);
private:
PathDesired *getPathDesired();
PositionActual *getPositionActual();
PositionState *getPositionState();
Ui_MagicWaypoint *m_magicwaypoint;
};

4
ground/openpilotgcs/src/plugins/modelview/modelviewgadgetwidget.cpp
View File

@ -57,7 +57,7 @@ ModelViewGadgetWidget::ModelViewGadgetWidget(QWidget *parent)
// Get required UAVObjects
ExtensionSystem::PluginManager *pm = ExtensionSystem::PluginManager::instance();
UAVObjectManager *objManager = pm->getObject<UAVObjectManager>();
attActual = AttitudeActual::GetInstance(objManager);
attState = AttitudeState::GetInstance(objManager);
connect(&m_MotionTimer, SIGNAL(timeout()), this, SLOT(updateAttitude()));
}
@ -311,7 +311,7 @@ void ModelViewGadgetWidget::keyPressEvent(QKeyEvent *e) // switch between camera
//////////////////////////////////////////////////////////////////////
void ModelViewGadgetWidget::updateAttitude()
{
AttitudeActual::DataFields data = attActual->getData(); // get attitude data
AttitudeState::DataFields data = attState->getData(); // get attitude data
GLC_StructOccurence *rootObject = m_World.rootOccurence(); // get the full 3D model
double x = data.q3;
double y = data.q2;

4
ground/openpilotgcs/src/plugins/modelview/modelviewgadgetwidget.h
View File

@ -39,7 +39,7 @@
#include "glc_exception.h"
#include "uavobjectmanager.h"
#include "attitudeactual.h"
#include "attitudestate.h"
class ModelViewGadgetWidget : public QGLWidget {
@ -89,7 +89,7 @@ private:
QString bgFilename;
bool vboEnable;
AttitudeActual *attActual;
AttitudeState *attState;
};
#endif /* MODELVIEWGADGETWIDGET_H_ */

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

@ -48,14 +48,14 @@
#include "uavtalk/telemetrymanager.h"
#include "uavobject.h"
#include "positionactual.h"
#include "positionstate.h"
#include "homelocation.h"
#include "gpsposition.h"
#include "gyros.h"
#include "attitudeactual.h"
#include "positionactual.h"
#include "velocityactual.h"
#include "airspeedactual.h"
#include "gpspositionsensor.h"
#include "gyrostate.h"
#include "attitudestate.h"
#include "positionstate.h"
#include "velocitystate.h"
#include "airspeedstate.h"
#define allow_manual_home_location_move
@ -579,10 +579,10 @@ void OPMapGadgetWidget::updatePosition()
// *************
// get the current GPS position and heading
GPSPosition *gpsPositionObj = GPSPosition::GetInstance(obm);
GPSPositionSensor *gpsPositionObj = GPSPositionSensor::GetInstance(obm);
Q_ASSERT(gpsPositionObj);
GPSPosition::DataFields gpsPositionData = gpsPositionObj->getData();
GPSPositionSensor::DataFields gpsPositionData = gpsPositionObj->getData();
gps_heading = gpsPositionData.Heading;
gps_latitude = gpsPositionData.Latitude;
@ -593,36 +593,36 @@ void OPMapGadgetWidget::updatePosition()
// **********************
// get the current position and heading estimates
AttitudeActual *attitudeActualObj = AttitudeActual::GetInstance(obm);
PositionActual *positionActualObj = PositionActual::GetInstance(obm);
VelocityActual *velocityActualObj = VelocityActual::GetInstance(obm);
AirspeedActual *airspeedActualObj = AirspeedActual::GetInstance(obm);
AttitudeState *attitudeStateObj = AttitudeState::GetInstance(obm);
PositionState *positionStateObj = PositionState::GetInstance(obm);
VelocityState *velocityStateObj = VelocityState::GetInstance(obm);
AirspeedState *airspeedStateObj = AirspeedState::GetInstance(obm);
Gyros *gyrosObj = Gyros::GetInstance(obm);
GyroState *gyroStateObj = GyroState::GetInstance(obm);
Q_ASSERT(attitudeActualObj);
Q_ASSERT(positionActualObj);
Q_ASSERT(velocityActualObj);
Q_ASSERT(airspeedActualObj);
Q_ASSERT(gyrosObj);
Q_ASSERT(attitudeStateObj);
Q_ASSERT(positionStateObj);
Q_ASSERT(velocityStateObj);
Q_ASSERT(airspeedStateObj);
Q_ASSERT(gyroStateObj);
AttitudeActual::DataFields attitudeActualData = attitudeActualObj->getData();
PositionActual::DataFields positionActualData = positionActualObj->getData();
VelocityActual::DataFields velocityActualData = velocityActualObj->getData();
AirspeedActual::DataFields airspeedActualData = airspeedActualObj->getData();
AttitudeState::DataFields attitudeStateData = attitudeStateObj->getData();
PositionState::DataFields positionStateData = positionStateObj->getData();
VelocityState::DataFields velocityStateData = velocityStateObj->getData();
AirspeedState::DataFields airspeedStateData = airspeedStateObj->getData();
Gyros::DataFields gyrosData = gyrosObj->getData();
GyroState::DataFields gyroStateData = gyroStateObj->getData();
double NED[3] = { positionActualData.North, positionActualData.East, positionActualData.Down };
double vNED[3] = { velocityActualData.North, velocityActualData.East, velocityActualData.Down };
double NED[3] = { positionStateData.North, positionStateData.East, positionStateData.Down };
double vNED[3] = { velocityStateData.North, velocityStateData.East, velocityStateData.Down };
// Set the position and heading estimates in the painter module
m_map->UAV->SetNED(NED);
m_map->UAV->SetCAS(airspeedActualData.CalibratedAirspeed);
m_map->UAV->SetCAS(airspeedStateData.CalibratedAirspeed);
m_map->UAV->SetGroundspeed(vNED, m_maxUpdateRate);
// Convert angular velocities into a rotationg rate around the world-frame yaw axis. This is found by simply taking the dot product of the angular Euler-rate matrix with the angular rates.
float psiRate_dps = 0 * gyrosData.z + sin(attitudeActualData.Roll * deg_to_rad) / cos(attitudeActualData.Pitch * deg_to_rad) * gyrosData.y + cos(attitudeActualData.Roll * deg_to_rad) / cos(attitudeActualData.Pitch * deg_to_rad) * gyrosData.z;
float psiRate_dps = 0 * gyroStateData.z + sin(attitudeStateData.Roll * deg_to_rad) / cos(attitudeStateData.Pitch * deg_to_rad) * gyroStateData.y + cos(attitudeStateData.Roll * deg_to_rad) / cos(attitudeStateData.Pitch * deg_to_rad) * gyroStateData.z;
// Set the angular rate in the painter module
m_map->UAV->SetYawRate(psiRate_dps); // Not correct, but I'm being lazy right now.
@ -2170,14 +2170,14 @@ bool OPMapGadgetWidget::getUAVPosition(double &latitude, double &longitude, doub
Q_ASSERT(obm != NULL);
PositionActual *positionActual = PositionActual::GetInstance(obm);
Q_ASSERT(positionActual != NULL);
PositionActual::DataFields positionActualData = positionActual->getData();
if (positionActualData.North == 0 && positionActualData.East == 0 && positionActualData.Down == 0) {
GPSPosition *gpsPositionObj = GPSPosition::GetInstance(obm);
PositionState *positionState = PositionState::GetInstance(obm);
Q_ASSERT(positionState != NULL);
PositionState::DataFields positionStateData = positionState->getData();
if (positionStateData.North == 0 && positionStateData.East == 0 && positionStateData.Down == 0) {
GPSPositionSensor *gpsPositionObj = GPSPositionSensor::GetInstance(obm);
Q_ASSERT(gpsPositionObj);
GPSPosition::DataFields gpsPositionData = gpsPositionObj->getData();
GPSPositionSensor::DataFields gpsPositionData = gpsPositionObj->getData();
latitude = gpsPositionData.Latitude / 1.0e7;
longitude = gpsPositionData.Longitude / 1.0e7;
altitude = gpsPositionData.Altitude;
@ -2191,9 +2191,9 @@ bool OPMapGadgetWidget::getUAVPosition(double &latitude, double &longitude, doub
homeLLA[1] = homeLocationData.Longitude / 1.0e7;
homeLLA[2] = homeLocationData.Altitude;
NED[0] = positionActualData.North;
NED[1] = positionActualData.East;
NED[2] = positionActualData.Down;
NED[0] = positionStateData.North;
NED[1] = positionStateData.East;
NED[2] = positionStateData.Down;
Utils::CoordinateConversions().NED2LLA_HomeLLA(homeLLA, NED, LLA);
@ -2229,7 +2229,7 @@ double OPMapGadgetWidget::getUAV_Yaw()
return 0;
}
UAVObject *obj = dynamic_cast<UAVDataObject *>(obm->getObject(QString("AttitudeActual")));
UAVObject *obj = dynamic_cast<UAVDataObject *>(obm->getObject(QString("AttitudeState")));
double yaw = obj->getField(QString("Yaw"))->getDouble();
if (yaw != yaw) {
@ -2245,7 +2245,7 @@ double OPMapGadgetWidget::getUAV_Yaw()
return yaw;
}
bool OPMapGadgetWidget::getGPSPosition(double &latitude, double &longitude, double &altitude)
bool OPMapGadgetWidget::getGPSPositionSensor(double &latitude, double &longitude, double &altitude)
{
double LLA[3];
@ -2253,7 +2253,7 @@ bool OPMapGadgetWidget::getGPSPosition(double &latitude, double &longitude, doub
return false;
}
if (obum->getGPSPosition(LLA) < 0) {
if (obum->getGPSPositionSensor(LLA) < 0) {
return false; // error
}
latitude = LLA[0];

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

@ -113,7 +113,7 @@ public:
void setMaxUpdateRate(int update_rate);
void setHomePosition(QPointF pos);
void setOverlayOpacity(qreal value);
bool getGPSPosition(double &latitude, double &longitude, double &altitude);
bool getGPSPositionSensor(double &latitude, double &longitude, double &altitude);
signals:
void defaultLocationAndZoomChanged(double lng, double lat, double zoom);
void overlayOpacityChanged(qreal);

4
ground/openpilotgcs/src/plugins/osgearthview/osgearthviewwidget.cpp
View File

@ -93,9 +93,9 @@ using namespace osgEarth::Annotation;
#include "utils/homelocationutil.h"
#include "utils/worldmagmodel.h"
#include "utils/coordinateconversions.h"
#include "attitudeactual.h"
#include "attitudestate.h"
#include "homelocation.h"
#include "positionactual.h"
#include "positionstate.h"
using namespace Utils;

26
ground/openpilotgcs/src/plugins/osgearthview/osgviewerwidget.cpp
View File

@ -95,10 +95,10 @@ using namespace osgEarth::Annotation;
#include "utils/homelocationutil.h"
#include "utils/worldmagmodel.h"
#include "utils/coordinateconversions.h"
#include "attitudeactual.h"
#include "gpsposition.h"
#include "attitudestate.h"
#include "gpspositionsensor.h"
#include "homelocation.h"
#include "positionactual.h"
#include "positionstate.h"
#include "systemsettings.h"
using namespace Utils;
@ -256,12 +256,12 @@ void OsgViewerWidget::paintEvent(QPaintEvent *event)
ExtensionSystem::PluginManager *pm = ExtensionSystem::PluginManager::instance();
UAVObjectManager *objMngr = pm->getObject<UAVObjectManager>();
PositionActual *positionActualObj = PositionActual::GetInstance(objMngr);
PositionActual::DataFields positionActual = positionActualObj->getData();
double NED[3] = { positionActual.North, positionActual.East, positionActual.Down };
PositionState *positionStateObj = PositionState::GetInstance(objMngr);
PositionState::DataFields positionState = positionStateObj->getData();
double NED[3] = { positionState.North, positionState.East, positionState.Down };
bool positionActualUpdate = true;
if (positionActualUpdate) {
bool positionStateUpdate = true;
if (positionStateUpdate) {
HomeLocation *homeLocationObj = HomeLocation::GetInstance(objMngr);
HomeLocation::DataFields homeLocation = homeLocationObj->getData();
double homeLLA[3] = { homeLocation.Latitude / 10.0e6, homeLocation.Longitude / 10.0e6, homeLocation.Altitude };
@ -270,15 +270,15 @@ void OsgViewerWidget::paintEvent(QPaintEvent *event)
CoordinateConversions().NED2LLA_HomeLLA(homeLLA, NED, LLA);
uavPos->getLocator()->setPosition(osg::Vec3d(LLA[1], LLA[0], LLA[2])); // Note this takes longtitude first
} else {
GPSPosition *gpsPosObj = GPSPosition::GetInstance(objMngr);
GPSPosition::DataFields gpsPos = gpsPosObj->getData();
GPSPositionSensor *gpsPosObj = GPSPositionSensor::GetInstance(objMngr);
GPSPositionSensor::DataFields gpsPos = gpsPosObj->getData();
uavPos->getLocator()->setPosition(osg::Vec3d(gpsPos.Longitude / 10.0e6, gpsPos.Latitude / 10.0e6, gpsPos.Altitude));
}
// Set the attitude (reverse the attitude)
AttitudeActual *attitudeActualObj = AttitudeActual::GetInstance(objMngr);
AttitudeActual::DataFields attitudeActual = attitudeActualObj->getData();
osg::Quat quat(attitudeActual.q2, attitudeActual.q3, attitudeActual.q4, attitudeActual.q1);
AttitudeState *attitudeStateObj = AttitudeState::GetInstance(objMngr);
AttitudeState::DataFields attitudeState = attitudeStateObj->getData();
osg::Quat quat(attitudeState.q2, attitudeState.q3, attitudeState.q4, attitudeState.q1);
// Have to rotate the axes from OP NED frame to OSG frame (X east, Y north, Z down)
double angle;

16
ground/openpilotgcs/src/plugins/pfdqml/pfdqmlgadgetwidget.cpp
View File

@ -52,14 +52,14 @@ PfdQmlGadgetWidget::PfdQmlGadgetWidget(QWidget *parent) :
// setViewport(new QGLWidget(QGLFormat(QGL::SampleBuffers)));
QStringList objectsToExport;
objectsToExport << "VelocityActual" <<
"PositionActual" <<
"AttitudeActual" <<
"Accels" <<
objectsToExport << "VelocityState" <<
"PositionState" <<
"AttitudeState" <<
"AccelState" <<
"VelocityDesired" <<
"PositionDesired" <<
"AttitudeHoldDesired" <<
"GPSPosition" <<
"PathDesired" <<
"AltitudeHoldDesired" <<
"GPSPositionSensor" <<
"GCSTelemetryStats" <<
"FlightBatteryState";
@ -174,7 +174,7 @@ void PfdQmlGadgetWidget::setOpenGLEnabled(bool arg)
}
}
// Switch between PositionActual UAVObject position
// Switch between PositionState UAVObject position
// and pre-defined latitude/longitude/altitude properties
void PfdQmlGadgetWidget::setActualPositionUsed(bool arg)
{

8
ground/openpilotgcs/src/plugins/qmlview/qmlviewgadgetwidget.cpp
View File

@ -49,10 +49,10 @@ QmlViewGadgetWidget::QmlViewGadgetWidget(QWidget *parent) :
setResizeMode(SizeRootObjectToView);
QStringList objectsToExport;
objectsToExport << "VelocityActual" <<
"PositionActual" <<
"AttitudeActual" <<
"GPSPosition" <<
objectsToExport << "VelocityState" <<
"PositionState" <<
"AttitudeState" <<
"GPSPositionSensor" <<
"GCSTelemetryStats" <<
"FlightBatteryState";

32
ground/openpilotgcs/src/plugins/setupwizard/biascalibrationutil.cpp
View File

@ -29,8 +29,8 @@
#include "extensionsystem/pluginmanager.h"
#include "uavobjectmanager.h"
#include "attitudesettings.h"
#include "accels.h"
#include "gyros.h"
#include "accelstate.h"
#include "gyrostate.h"
#include "qdebug.h"
#include "revocalibration.h"
@ -74,12 +74,12 @@ void BiasCalibrationUtil::gyroMeasurementsUpdated(UAVObject *obj)
UAVObjectManager *uavObjectManager = pm->getObject<UAVObjectManager>();
Q_ASSERT(uavObjectManager);
Gyros *gyros = Gyros::GetInstance(uavObjectManager);
Gyros::DataFields gyrosData = gyros->getData();
GyroState *gyroState = GyroState::GetInstance(uavObjectManager);
GyroState::DataFields gyroStateData = gyroState->getData();
m_gyroX += gyrosData.x;
m_gyroY += gyrosData.y;
m_gyroZ += gyrosData.z;
m_gyroX += gyroStateData.x;
m_gyroY += gyroStateData.y;
m_gyroZ += gyroStateData.z;
m_receivedGyroUpdates++;
emit progress(m_receivedGyroUpdates + m_receivedAccelUpdates, m_gyroMeasurementCount + m_accelMeasurementCount);
@ -98,12 +98,12 @@ void BiasCalibrationUtil::accelMeasurementsUpdated(UAVObject *obj)
UAVObjectManager *uavObjectManager = pm->getObject<UAVObjectManager>();
Q_ASSERT(uavObjectManager);
Accels *accels = Accels::GetInstance(uavObjectManager);
Accels::DataFields accelsData = accels->getData();
AccelState *accelState = AccelState::GetInstance(uavObjectManager);
AccelState::DataFields AccelStateData = accelState->getData();
m_accelerometerX += accelsData.x;
m_accelerometerY += accelsData.y;
m_accelerometerZ += accelsData.z;
m_accelerometerX += AccelStateData.x;
m_accelerometerY += AccelStateData.y;
m_accelerometerZ += AccelStateData.z;
m_receivedAccelUpdates++;
emit progress(m_receivedGyroUpdates + m_receivedAccelUpdates, m_gyroMeasurementCount + m_accelMeasurementCount);
@ -166,7 +166,7 @@ void BiasCalibrationUtil::startMeasurement()
AttitudeSettings::GetInstance(uavObjectManager)->setData(attitudeSettingsData);
}
// Set up to receive updates for accels
UAVDataObject *uavObject = Accels::GetInstance(uavObjectManager);
UAVDataObject *uavObject = AccelState::GetInstance(uavObjectManager);
connect(uavObject, SIGNAL(objectUpdated(UAVObject *)), this, SLOT(accelMeasurementsUpdated(UAVObject *)));
// Set update period for accels
@ -179,7 +179,7 @@ void BiasCalibrationUtil::startMeasurement()
uavObject->setMetadata(newMetaData);
}
// Set up to receive updates from gyros
uavObject = Gyros::GetInstance(uavObjectManager);
uavObject = GyroState::GetInstance(uavObjectManager);
connect(uavObject, SIGNAL(objectUpdated(UAVObject *)), this, SLOT(gyroMeasurementsUpdated(UAVObject *)));
// Set update period for gyros
@ -207,12 +207,12 @@ void BiasCalibrationUtil::stopMeasurement()
Q_ASSERT(uavObjectManager);
// Stop listening for updates from accels
UAVDataObject *uavObject = Accels::GetInstance(uavObjectManager);
UAVDataObject *uavObject = AccelState::GetInstance(uavObjectManager);
disconnect(uavObject, SIGNAL(objectUpdated(UAVObject *)), this, SLOT(accelMeasurementsUpdated(UAVObject *)));
uavObject->setMetadata(m_previousAccelMetaData);
// Stop listening for updates from gyros
uavObject = Gyros::GetInstance(uavObjectManager);
uavObject = GyroState::GetInstance(uavObjectManager);
disconnect(uavObject, SIGNAL(objectUpdated(UAVObject *)), this, SLOT(gyroMeasurementsUpdated(UAVObject *)));
uavObject->setMetadata(m_previousGyroMetaData);

40
ground/openpilotgcs/src/plugins/uavobjects/OPPlots.m
View File

@ -5,19 +5,19 @@ function OPPlots()
%load('specificfilename')
TimeVA = [VelocityActual.timestamp]/1000;
VA = [[VelocityActual.North]
[VelocityActual.East]
[VelocityActual.Down]]/100;
TimeVA = [VelocityState.timestamp]/1000;
VA = [[VelocityState.North]
[VelocityState.East]
[VelocityState.Down]]/100;
TimeGPSPos = [GPSPosition.timestamp]/1000;
Vgps=[[GPSPosition.Groundspeed].*cos([GPSPosition.Heading]*pi/180)
[GPSPosition.Groundspeed].*sin([GPSPosition.Heading]*pi/180)];
TimeGPSPos = [GPSPositionSensor.timestamp]/1000;
Vgps=[[GPSPositionSensor.Groundspeed].*cos([GPSPositionSensor.Heading]*pi/180)
[GPSPositionSensor.Groundspeed].*sin([GPSPositionSensor.Heading]*pi/180)];
figure(1);
plot(TimeVA,VA(1,:),TimeVA,VA(2,:),TimeGPSPos,Vgps(1,:),TimeGPSPos,Vgps(2,:));
s1='Velocity Actual North';
s2='Velocity Actual East';
s1='Velocity State North';
s2='Velocity State East';
s3='GPS Velocity North';
s4='GPS Velocity East';
legend(s1,s2,s3,s4);
@ -25,23 +25,23 @@ function OPPlots()
ylabel('Velocity (m/s)');
TimePA = [PositionActual.timestamp]/1000;
PA = [[PositionActual.North]
[PositionActual.East]
[PositionActual.Down]]/100;
TimePA = [PositionState.timestamp]/1000;
PA = [[PositionState.North]
[PositionState.East]
[PositionState.Down]]/100;
TimeGPSPos = [GPSPosition.timestamp]/1000;
LLA=[[GPSPosition.Latitude]*1e-7;
[GPSPosition.Longitude]*1e-7;
[GPSPosition.Altitude]+[GPSPosition.GeoidSeparation]];
TimeGPSPos = [GPSPositionSensor.timestamp]/1000;
LLA=[[GPSPositionSensor.Latitude]*1e-7;
[GPSPositionSensor.Longitude]*1e-7;
[GPSPositionSensor.Altitude]+[GPSPositionSensor.GeoidSeparation]];
BaseECEF = LLA2ECEF([HomeLocation.Latitude*1e-7, HomeLocation.Longitude*1e-7, HomeLocation.Altitude]');
Rne = RneFromLLA([HomeLocation.Latitude*1e-7, HomeLocation.Longitude*1e-7, HomeLocation.Altitude]');
GPSPos=LLA2Base(LLA,BaseECEF,Rne);
figure(2);
plot(TimePA,PA(1,:),TimePA,PA(2,:),TimeGPSPos,GPSPos(1,:),TimeGPSPos,GPSPos(2,:));
s1='Position Actual North';
s2='Position Actual East';
s1='Position State North';
s2='Position State East';
s3='GPS Position North';
s4='GPS Position East';
legend(s1,s2,s3,s4);
@ -50,7 +50,7 @@ function OPPlots()
figure(3);
plot3(PA(2,:),PA(1,:),PA(3,:),GPSPos(2,:),GPSPos(1,:),GPSPos(3,:));
s1='Pos Actual';
s1='Pos State';
s2='GPS Pos';
legend(s1,s2);
xlabel('East (m)');

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