From bb79ba66fa30af002e548bf89a36b610e914e3e6 Mon Sep 17 00:00:00 2001 From: peabody124 Date: Sat, 21 Aug 2010 08:35:44 +0000 Subject: [PATCH] OP-119 AHRS: Converted the big data structures for the WMM calculation to be allocated on the stack, which forced a dependency on FreeRTOS :-( but keeps things on the heap. Also changed the HomeLocation object to include a Set field. When this is false, OP will try and update it whenever it gets a 3D fix. If it is saved to disk with this field as true, then it won't need to get a lock to send the magnetic flux vector to the AHRS. git-svn-id: svn://svn.openpilot.org/OpenPilot/trunk@1346 ebee16cc-31ac-478f-84a7-5cbb03baadba --- flight/Libraries/WorldMagModel.c | 123 ++++++++++-------- flight/Libraries/inc/WMMInternal.h | 2 +- flight/OpenPilot/Modules/GPS/GPS.c | 21 +-- .../OpenPilot/UAVObjects/inc/homelocation.h | 6 +- .../OpenPilotOSX.xcodeproj/project.pbxproj | 6 +- .../src/plugins/uavobjects/homelocation.cpp | 6 + ground/src/plugins/uavobjects/homelocation.h | 6 +- ground/src/plugins/uavobjects/homelocation.py | 14 +- .../uavobjectdefinition/homelocation.xml | 25 ++-- 9 files changed, 122 insertions(+), 87 deletions(-) diff --git a/flight/Libraries/WorldMagModel.c b/flight/Libraries/WorldMagModel.c index 6b5b71112..cfbd2780e 100644 --- a/flight/Libraries/WorldMagModel.c +++ b/flight/Libraries/WorldMagModel.c @@ -32,6 +32,9 @@ * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA */ +// I don't want this dependency, but currently using pvPortMalloc +#include "openpilot.h" + #include #include #include @@ -41,8 +44,8 @@ #include "WorldMagModel.h" #include "WMMInternal.h" -static WMMtype_Ellipsoid Ellip; -static WMMtype_MagneticModel MagneticModel; +static WMMtype_Ellipsoid * Ellip; +static WMMtype_MagneticModel * MagneticModel; /************************************************************************************** * Example use - very simple - only two exposed functions @@ -58,24 +61,24 @@ static WMMtype_MagneticModel MagneticModel; int WMM_Initialize() // Sets default values for WMM subroutines. // UPDATES : Ellip and MagneticModel -{ +{ // Sets WGS-84 parameters - Ellip.a = 6378.137; // semi-major axis of the ellipsoid in km - Ellip.b = 6356.7523142; // semi-minor axis of the ellipsoid in km - Ellip.fla = 1/298.257223563; // flattening - Ellip.eps = sqrt(1- (Ellip.b*Ellip.b)/(Ellip.a*Ellip.a )); // first eccentricity - Ellip.epssq = (Ellip.eps*Ellip.eps); // first eccentricity squared - Ellip.re = 6371.2; // Earth's radius in km + Ellip->a = 6378.137; // semi-major axis of the ellipsoid in km + Ellip->b = 6356.7523142; // semi-minor axis of the ellipsoid in km + Ellip->fla = 1/298.257223563; // flattening + Ellip->eps = sqrt(1- (Ellip->b*Ellip->b)/(Ellip->a*Ellip->a )); // first eccentricity + Ellip->epssq = (Ellip->eps*Ellip->eps); // first eccentricity squared + Ellip->re = 6371.2; // Earth's radius in km // Sets Magnetic Model parameters - MagneticModel.nMax = WMM_MAX_MODEL_DEGREES; - MagneticModel.nMaxSecVar = WMM_MAX_SECULAR_VARIATION_MODEL_DEGREES; - MagneticModel.SecularVariationUsed = 0; + MagneticModel->nMax = WMM_MAX_MODEL_DEGREES; + MagneticModel->nMaxSecVar = WMM_MAX_SECULAR_VARIATION_MODEL_DEGREES; + MagneticModel->SecularVariationUsed = 0; // Really, Really needs to be read from a file - out of date in 2015 at latest - MagneticModel.EditionDate = 5.7863328170559505e-307; - MagneticModel.epoch = 2010.0; - sprintf(MagneticModel.ModelName, "WMM-2010"); + MagneticModel->EditionDate = 5.7863328170559505e-307; + MagneticModel->epoch = 2010.0; + sprintf(MagneticModel->ModelName, "WMM-2010"); WMM_Set_Coeff_Array(); return 0; } @@ -84,15 +87,20 @@ void WMM_GetMagVector(float Lat, float Lon, float AltEllipsoid, uint16_t Month, { char Error_Message[255]; + Ellip = (WMMtype_Ellipsoid *) pvPortMalloc(sizeof(WMMtype_Ellipsoid)); + MagneticModel = (WMMtype_MagneticModel *) pvPortMalloc(sizeof(WMMtype_MagneticModel)); + WMMtype_CoordSpherical CoordSpherical; WMMtype_CoordGeodetic CoordGeodetic; WMMtype_Date Date; WMMtype_GeoMagneticElements GeoMagneticElements; + WMM_Initialize(); + CoordGeodetic.lambda = Lon; CoordGeodetic.phi = Lat; CoordGeodetic.HeightAboveEllipsoid = AltEllipsoid; - WMM_GeodeticToSpherical(Ellip, CoordGeodetic, &CoordSpherical); /*Convert from geodeitic to Spherical Equations: 17-18, WMM Technical report*/ + WMM_GeodeticToSpherical(&CoordGeodetic, &CoordSpherical); /*Convert from geodeitic to Spherical Equations: 17-18, WMM Technical report*/ Date.Month=Month; Date.Day=Day; @@ -101,6 +109,9 @@ void WMM_GetMagVector(float Lat, float Lon, float AltEllipsoid, uint16_t Month, WMM_TimelyModifyMagneticModel(Date); WMM_Geomag(&CoordSpherical, &CoordGeodetic, &GeoMagneticElements); /* Computes the geoMagnetic field elements and their time change*/ + vPortFree(Ellip); + vPortFree(MagneticModel); + B[0]=GeoMagneticElements.X; B[1]=GeoMagneticElements.Y; B[2]=GeoMagneticElements.Z; @@ -136,8 +147,8 @@ uint16_t WMM_Geomag( WMMtype_CoordSpherical * CoordSpherical, WMMtype_CoordGeode WMMtype_SphericalHarmonicVariables SphVariables; WMMtype_MagneticResults MagneticResultsSph, MagneticResultsGeo, MagneticResultsSphVar, MagneticResultsGeoVar; - WMM_ComputeSphericalHarmonicVariables( CoordSpherical, MagneticModel.nMax, &SphVariables); /* Compute Spherical Harmonic variables */ - WMM_AssociatedLegendreFunction( CoordSpherical, MagneticModel.nMax, &LegendreFunction); /* Compute ALF */ + WMM_ComputeSphericalHarmonicVariables( CoordSpherical, MagneticModel->nMax, &SphVariables); /* Compute Spherical Harmonic variables */ + WMM_AssociatedLegendreFunction( CoordSpherical, MagneticModel->nMax, &LegendreFunction); /* Compute ALF */ WMM_Summation(&LegendreFunction, &SphVariables, CoordSpherical, &MagneticResultsSph); /* Accumulate the spherical harmonic coefficients*/ WMM_SecVarSummation(&LegendreFunction, &SphVariables, CoordSpherical, &MagneticResultsSphVar); /*Sum the Secular Variation Coefficients */ WMM_RotateMagneticVector(CoordSpherical, CoordGeodetic, &MagneticResultsSph, &MagneticResultsGeo); /* Map the computed Magnetic fields to Geodeitic coordinates */ @@ -180,10 +191,10 @@ uint16_t WMM_ComputeSphericalHarmonicVariables(WMMtype_CoordSpherical *CoordSphe sin_lambda = sin(DEG2RAD(CoordSpherical->lambda)); /* for n = 0 ... model_order, compute (Radius of Earth / Spherica radius r)^(n+2) for n 1..nMax-1 (this is much faster than calling pow MAX_N+1 times). */ - SphVariables->RelativeRadiusPower[0] = (Ellip.re / CoordSpherical->r) * (Ellip.re / CoordSpherical->r); + SphVariables->RelativeRadiusPower[0] = (Ellip->re / CoordSpherical->r) * (Ellip->re / CoordSpherical->r); for (n = 1; n <= nMax; n++) { - SphVariables->RelativeRadiusPower[n] = SphVariables->RelativeRadiusPower[n-1] * (Ellip.re / CoordSpherical->r); + SphVariables->RelativeRadiusPower[n] = SphVariables->RelativeRadiusPower[n-1] * (Ellip->re / CoordSpherical->r); } /* @@ -270,7 +281,7 @@ uint16_t WMM_Summation(WMMtype_LegendreFunction *LegendreFunction, WMMtype_Spher MagneticResults->Bz = 0.0; MagneticResults->By = 0.0; MagneticResults->Bx = 0.0; - for (n = 1; n <= MagneticModel.nMax; n++) + for (n = 1; n <= MagneticModel->nMax; n++) { for (m=0;m<=n;m++) { @@ -281,8 +292,8 @@ uint16_t WMM_Summation(WMMtype_LegendreFunction *LegendreFunction, WMMtype_Spher n=1 m=0 n n n */ /* Equation 12 in the WMM Technical report. Derivative with respect to radius.*/ MagneticResults->Bz -= SphVariables->RelativeRadiusPower[n] * - ( MagneticModel.Main_Field_Coeff_G[index]*SphVariables->cos_mlambda[m] + - MagneticModel.Main_Field_Coeff_H[index]*SphVariables->sin_mlambda[m] ) + ( MagneticModel->Main_Field_Coeff_G[index]*SphVariables->cos_mlambda[m] + + MagneticModel->Main_Field_Coeff_H[index]*SphVariables->sin_mlambda[m] ) * (float) (n+1) * LegendreFunction-> Pcup[index]; /* 1 nMax (n+2) n m m m @@ -290,8 +301,8 @@ uint16_t WMM_Summation(WMMtype_LegendreFunction *LegendreFunction, WMMtype_Spher n=1 m=0 n n n */ /* Equation 11 in the WMM Technical report. Derivative with respect to longitude, divided by radius. */ MagneticResults->By += SphVariables->RelativeRadiusPower[n] * - ( MagneticModel.Main_Field_Coeff_G[index]*SphVariables->sin_mlambda[m] - - MagneticModel.Main_Field_Coeff_H[index]*SphVariables->cos_mlambda[m] ) + ( MagneticModel->Main_Field_Coeff_G[index]*SphVariables->sin_mlambda[m] - + MagneticModel->Main_Field_Coeff_H[index]*SphVariables->cos_mlambda[m] ) * (float) (m) * LegendreFunction-> Pcup[index]; /* nMax (n+2) n m m m Bx = - SUM (a/r) SUM [g cos(m p) + h sin(m p)] dP (sin(phi)) @@ -299,8 +310,8 @@ uint16_t WMM_Summation(WMMtype_LegendreFunction *LegendreFunction, WMMtype_Spher /* Equation 10 in the WMM Technical report. Derivative with respect to latitude, divided by radius. */ MagneticResults->Bx -= SphVariables->RelativeRadiusPower[n] * - ( MagneticModel.Main_Field_Coeff_G[index]*SphVariables->cos_mlambda[m] + - MagneticModel.Main_Field_Coeff_H[index]*SphVariables->sin_mlambda[m] ) + ( MagneticModel->Main_Field_Coeff_G[index]*SphVariables->cos_mlambda[m] + + MagneticModel->Main_Field_Coeff_H[index]*SphVariables->sin_mlambda[m] ) * LegendreFunction-> dPcup[index]; @@ -340,11 +351,11 @@ uint16_t WMM_SecVarSummation(WMMtype_LegendreFunction *LegendreFunction, WMMtype */ uint16_t m, n, index; float cos_phi; - MagneticModel.SecularVariationUsed = TRUE; + MagneticModel->SecularVariationUsed = TRUE; MagneticResults->Bz = 0.0; MagneticResults->By = 0.0; MagneticResults->Bx = 0.0; - for (n = 1; n <= MagneticModel.nMaxSecVar; n++) + for (n = 1; n <= MagneticModel->nMaxSecVar; n++) { for (m=0;m<=n;m++) { @@ -355,8 +366,8 @@ uint16_t WMM_SecVarSummation(WMMtype_LegendreFunction *LegendreFunction, WMMtype n=1 m=0 n n n */ /* Derivative with respect to radius.*/ MagneticResults->Bz -= SphVariables->RelativeRadiusPower[n] * - ( MagneticModel.Secular_Var_Coeff_G[index]*SphVariables->cos_mlambda[m] + - MagneticModel.Secular_Var_Coeff_H[index]*SphVariables->sin_mlambda[m] ) + ( MagneticModel->Secular_Var_Coeff_G[index]*SphVariables->cos_mlambda[m] + + MagneticModel->Secular_Var_Coeff_H[index]*SphVariables->sin_mlambda[m] ) * (float) (n+1) * LegendreFunction-> Pcup[index]; /* 1 nMax (n+2) n m m m @@ -364,8 +375,8 @@ uint16_t WMM_SecVarSummation(WMMtype_LegendreFunction *LegendreFunction, WMMtype n=1 m=0 n n n */ /* Derivative with respect to longitude, divided by radius. */ MagneticResults->By += SphVariables->RelativeRadiusPower[n] * - ( MagneticModel.Secular_Var_Coeff_G[index]*SphVariables->sin_mlambda[m] - - MagneticModel.Secular_Var_Coeff_H[index]*SphVariables->cos_mlambda[m] ) + ( MagneticModel->Secular_Var_Coeff_G[index]*SphVariables->sin_mlambda[m] - + MagneticModel->Secular_Var_Coeff_H[index]*SphVariables->cos_mlambda[m] ) * (float) (m) * LegendreFunction-> Pcup[index]; /* nMax (n+2) n m m m Bx = - SUM (a/r) SUM [g cos(m p) + h sin(m p)] dP (sin(phi)) @@ -373,8 +384,8 @@ uint16_t WMM_SecVarSummation(WMMtype_LegendreFunction *LegendreFunction, WMMtype /* Derivative with respect to latitude, divided by radius. */ MagneticResults->Bx -= SphVariables->RelativeRadiusPower[n] * - ( MagneticModel.Secular_Var_Coeff_G[index]*SphVariables->cos_mlambda[m] + - MagneticModel.Secular_Var_Coeff_H[index]*SphVariables->sin_mlambda[m] ) + ( MagneticModel->Secular_Var_Coeff_G[index]*SphVariables->cos_mlambda[m] + + MagneticModel->Secular_Var_Coeff_H[index]*SphVariables->sin_mlambda[m] ) * LegendreFunction-> dPcup[index]; } } @@ -762,7 +773,7 @@ uint16_t WMM_SummationSpecial(WMMtype_SphericalHarmonicVariables * SphVariables, MagneticResults->By = 0.0; sin_phi = sin ( DEG2RAD ( CoordSpherical->phig ) ); - for (n = 1; n <= MagneticModel.nMax; n++) + for (n = 1; n <= MagneticModel->nMax; n++) { /*Compute the ration between the Gauss-normalized associated Legendre @@ -789,8 +800,8 @@ uint16_t WMM_SummationSpecial(WMMtype_SphericalHarmonicVariables * SphVariables, /* Equation 11 in the WMM Technical report. Derivative with respect to longitude, divided by radius. */ MagneticResults->By += SphVariables->RelativeRadiusPower[n] * - ( MagneticModel.Main_Field_Coeff_G[index]*SphVariables->sin_mlambda[1] - - MagneticModel.Main_Field_Coeff_H[index]*SphVariables->cos_mlambda[1] ) + ( MagneticModel->Main_Field_Coeff_G[index]*SphVariables->sin_mlambda[1] - + MagneticModel->Main_Field_Coeff_H[index]*SphVariables->cos_mlambda[1] ) * PcupS[n] * schmidtQuasiNorm3; } @@ -819,7 +830,7 @@ uint16_t WMM_SecVarSummationSpecial(WMMtype_SphericalHarmonicVariables * SphVari MagneticResults->By = 0.0; sin_phi = sin ( DEG2RAD ( CoordSpherical->phig ) ); - for (n = 1; n <= MagneticModel.nMaxSecVar; n++) + for (n = 1; n <= MagneticModel->nMaxSecVar; n++) { index = (n * (n + 1) / 2 + 1); schmidtQuasiNorm2 = schmidtQuasiNorm1 * (float) (2 * n - 1) / (float) n; @@ -841,8 +852,8 @@ uint16_t WMM_SecVarSummationSpecial(WMMtype_SphericalHarmonicVariables * SphVari /* Derivative with respect to longitude, divided by radius. */ MagneticResults->By += SphVariables->RelativeRadiusPower[n] * - ( MagneticModel.Secular_Var_Coeff_G[index]*SphVariables->sin_mlambda[1] - - MagneticModel.Secular_Var_Coeff_H[index]*SphVariables->cos_mlambda[1] ) + ( MagneticModel->Secular_Var_Coeff_G[index]*SphVariables->sin_mlambda[1] - + MagneticModel->Secular_Var_Coeff_H[index]*SphVariables->cos_mlambda[1] ) * PcupS[n] * schmidtQuasiNorm3; } @@ -857,17 +868,17 @@ void WMM_TimelyModifyMagneticModel(WMMtype_Date UserDate) { uint16_t n, m, index, a, b; - a = MagneticModel.nMaxSecVar; + a = MagneticModel->nMaxSecVar; b = (a * (a + 1) / 2 + a); - for (n = 1; n <= MagneticModel.nMax; n++) + for (n = 1; n <= MagneticModel->nMax; n++) { for (m=0;m<=n;m++) { index = (n * (n + 1) / 2 + m); if(index <= b) { - MagneticModel.Main_Field_Coeff_H[index] += (UserDate.DecimalYear - MagneticModel.epoch) * MagneticModel.Secular_Var_Coeff_H[index]; - MagneticModel.Main_Field_Coeff_G[index] += (UserDate.DecimalYear - MagneticModel.epoch) * MagneticModel.Secular_Var_Coeff_G[index]; + MagneticModel->Main_Field_Coeff_H[index] += (UserDate.DecimalYear - MagneticModel->epoch) * MagneticModel->Secular_Var_Coeff_H[index]; + MagneticModel->Main_Field_Coeff_G[index] += (UserDate.DecimalYear - MagneticModel->epoch) * MagneticModel->Secular_Var_Coeff_G[index]; } } } @@ -906,7 +917,7 @@ uint16_t WMM_DateToYear (WMMtype_Date *CalendarDate, char *Error) return 1; } /*WMM_DateToYear*/ -void WMM_GeodeticToSpherical(WMMtype_Ellipsoid Ellip, WMMtype_CoordGeodetic CoordGeodetic, WMMtype_CoordSpherical *CoordSpherical) +void WMM_GeodeticToSpherical(WMMtype_CoordGeodetic *CoordGeodetic, WMMtype_CoordSpherical *CoordSpherical) // Converts Geodetic coordinates to Spherical coordinates // Convert geodetic coordinates, (defined by the WGS-84 // reference ellipsoid), to Earth Centered Earth Fixed Cartesian @@ -914,22 +925,22 @@ void WMM_GeodeticToSpherical(WMMtype_Ellipsoid Ellip, WMMtype_CoordGeodetic Coor { float CosLat, SinLat, rc, xp, zp; // all local variables - CosLat = cos(DEG2RAD(CoordGeodetic.phi)); - SinLat = sin(DEG2RAD(CoordGeodetic.phi)); + CosLat = cos(DEG2RAD(CoordGeodetic->phi)); + SinLat = sin(DEG2RAD(CoordGeodetic->phi)); // compute the local radius of curvature on the WGS-84 reference ellipsoid - rc = Ellip.a / sqrt(1.0 - Ellip.epssq * SinLat * SinLat); + rc = Ellip->a / sqrt(1.0 - Ellip->epssq * SinLat * SinLat); // compute ECEF Cartesian coordinates of specified point (for longitude=0) - xp = (rc + CoordGeodetic.HeightAboveEllipsoid) * CosLat; - zp = (rc*(1.0 - Ellip.epssq) + CoordGeodetic.HeightAboveEllipsoid) * SinLat; + xp = (rc + CoordGeodetic->HeightAboveEllipsoid) * CosLat; + zp = (rc*(1.0 - Ellip->epssq) + CoordGeodetic->HeightAboveEllipsoid) * SinLat; // compute spherical radius and angle lambda and phi of specified point CoordSpherical->r = sqrt(xp * xp + zp * zp); CoordSpherical->phig = RAD2DEG(asin(zp / CoordSpherical->r)); // geocentric latitude - CoordSpherical->lambda = CoordGeodetic.lambda; // longitude + CoordSpherical->lambda = CoordGeodetic->lambda; // longitude }// WMM_GeodeticToSpherical @@ -1031,10 +1042,10 @@ void WMM_Set_Coeff_Array() // TODO: If this works here, delete first two columns to save space for(uint16_t i=0; iMain_Field_Coeff_G[i]=CoeffFile[i][2]; + MagneticModel->Main_Field_Coeff_H[i]=CoeffFile[i][3]; + MagneticModel->Secular_Var_Coeff_G[i]=CoeffFile[i][4]; + MagneticModel->Secular_Var_Coeff_H[i]=CoeffFile[i][5]; } } diff --git a/flight/Libraries/inc/WMMInternal.h b/flight/Libraries/inc/WMMInternal.h index 1b86b9190..500f1b13b 100644 --- a/flight/Libraries/inc/WMMInternal.h +++ b/flight/Libraries/inc/WMMInternal.h @@ -120,7 +120,7 @@ // Internal Function Prototypes void WMM_Set_Coeff_Array(); - void WMM_GeodeticToSpherical(WMMtype_Ellipsoid Ellip, WMMtype_CoordGeodetic CoordGeodetic, WMMtype_CoordSpherical *CoordSpherical); + void WMM_GeodeticToSpherical(WMMtype_CoordGeodetic * CoordGeodetic, WMMtype_CoordSpherical *CoordSpherical); uint16_t WMM_DateToYear (WMMtype_Date *CalendarDate, char *Error); void WMM_TimelyModifyMagneticModel(WMMtype_Date UserDate); uint16_t WMM_Geomag(WMMtype_CoordSpherical * CoordSpherical, diff --git a/flight/OpenPilot/Modules/GPS/GPS.c b/flight/OpenPilot/Modules/GPS/GPS.c index f6c17a53e..3b10b6a1c 100644 --- a/flight/OpenPilot/Modules/GPS/GPS.c +++ b/flight/OpenPilot/Modules/GPS/GPS.c @@ -79,7 +79,8 @@ void nmeaProcessGPGSA(char* packet); // Global variables // Private constants -#define STACK_SIZE configMINIMAL_STACK_SIZE+5000 +// Unfortunately need a good size stack for the WMM calculation +#define STACK_SIZE configMINIMAL_STACK_SIZE+3500 #define TASK_PRIORITY (tskIDLE_PRIORITY + 3) // Private types @@ -120,8 +121,6 @@ int32_t GPSInitialize(void) return 0; } -static bool homeLocationSet = 0; - /** * gps task. Processes input buffer. It does not return. */ @@ -132,8 +131,6 @@ static void gpsTask(void* parameters) portTickType xDelay = 100 / portTICK_RATE_MS; PositionActualData GpsData; uint32_t timeNowMs; - - homeLocationSet = 0; // Loop forever while(1) @@ -161,13 +158,14 @@ static void gpsTask(void* parameters) } else { // Had an update + HomeLocationData home; + HomeLocationGet(&home); + PositionActualGet(&GpsData); - if(homeLocationSet == FALSE) { + if( (GpsData.Status == POSITIONACTUAL_STATUS_FIX3D) && (home.Set == HOMELOCATION_SET_FALSE) ) { setHomeLocation(&GpsData); - homeLocationSet = TRUE; } } - setHomeLocation(&GpsData); // Block task until next update vTaskDelay(xDelay); @@ -179,10 +177,6 @@ static void setHomeLocation(PositionActualData * gpsData) HomeLocationData home; HomeLocationGet(&home); - gpsData->Latitude = -29; - gpsData->Longitude = 93; - gpsData->Altitude = 30; - // Store LLA home.Latitude = (int32_t) (gpsData->Latitude * 10e6); home.Longitude = (int32_t) (gpsData->Longitude * 10e6); @@ -203,10 +197,9 @@ static void setHomeLocation(PositionActualData * gpsData) memcpy(&home.RNE[0], &RNE[0][0], 9 * sizeof(RNE[0][0])); // Compute magnetic flux direction at home location - WMM_Initialize(); - // TODO: Extract time/date from GPS to seed this WMM_GetMagVector(LLA[0], LLA[1], LLA[2], 8, 17, 2010, &home.Be[0]); + home.Set = HOMELOCATION_SET_TRUE; HomeLocationSet(&home); } diff --git a/flight/OpenPilot/UAVObjects/inc/homelocation.h b/flight/OpenPilot/UAVObjects/inc/homelocation.h index e87122b88..4ec221a72 100644 --- a/flight/OpenPilot/UAVObjects/inc/homelocation.h +++ b/flight/OpenPilot/UAVObjects/inc/homelocation.h @@ -33,7 +33,7 @@ #define HOMELOCATION_H // Object constants -#define HOMELOCATION_OBJID 933172238U +#define HOMELOCATION_OBJID 576787928U #define HOMELOCATION_NAME "HomeLocation" #define HOMELOCATION_METANAME "HomeLocationMeta" #define HOMELOCATION_ISSINGLEINST 1 @@ -57,6 +57,7 @@ // Object data typedef struct { + uint8_t Set; int32_t Latitude; int32_t Longitude; float Altitude; @@ -67,6 +68,9 @@ typedef struct { } __attribute__((packed)) HomeLocationData; // Field information +// Field Set information +/* Enumeration options for field Set */ +typedef enum { HOMELOCATION_SET_FALSE=0, HOMELOCATION_SET_TRUE=1 } HomeLocationSetOptions; // Field Latitude information // Field Longitude information // Field Altitude information diff --git a/flight/Project/OpenPilotOSX/OpenPilotOSX.xcodeproj/project.pbxproj b/flight/Project/OpenPilotOSX/OpenPilotOSX.xcodeproj/project.pbxproj index caa7cf148..d7e85b7f6 100644 --- a/flight/Project/OpenPilotOSX/OpenPilotOSX.xcodeproj/project.pbxproj +++ b/flight/Project/OpenPilotOSX/OpenPilotOSX.xcodeproj/project.pbxproj @@ -22,6 +22,7 @@ 651CF9F3120B700D00EEFD70 /* usb_conf.h */ = {isa = PBXFileReference; fileEncoding = 4; lastKnownFileType = sourcecode.c.h; path = usb_conf.h; sourceTree = ""; }; 654330231218E9780063F913 /* insgps.c */ = {isa = PBXFileReference; fileEncoding = 4; lastKnownFileType = sourcecode.c.c; name = insgps.c; path = ../../AHRS/insgps.c; sourceTree = SOURCE_ROOT; }; 6543304F121980300063F913 /* insgps.h */ = {isa = PBXFileReference; fileEncoding = 4; lastKnownFileType = sourcecode.c.h; path = insgps.h; sourceTree = ""; }; + 655268BC121FBD2900410C6E /* ahrscalibration.xml */ = {isa = PBXFileReference; fileEncoding = 4; lastKnownFileType = text.xml; path = ahrscalibration.xml; sourceTree = ""; }; 657CEEAD121DB6C8007A1FBE /* homelocation.xml */ = {isa = PBXFileReference; fileEncoding = 4; lastKnownFileType = text.xml; path = homelocation.xml; sourceTree = ""; }; 657CEEB7121DBC63007A1FBE /* CoordinateConversions.c */ = {isa = PBXFileReference; fileEncoding = 4; lastKnownFileType = sourcecode.c.c; path = CoordinateConversions.c; sourceTree = ""; }; 657CEEB9121DBC63007A1FBE /* CoordinateConversions.h */ = {isa = PBXFileReference; fileEncoding = 4; lastKnownFileType = sourcecode.c.h; path = CoordinateConversions.h; sourceTree = ""; }; @@ -32,6 +33,7 @@ 657CEEC1121DC054007A1FBE /* homelocation.c */ = {isa = PBXFileReference; fileEncoding = 4; lastKnownFileType = sourcecode.c.c; path = homelocation.c; sourceTree = ""; }; 657CEEC2121DC054007A1FBE /* navigationdesired.c */ = {isa = PBXFileReference; fileEncoding = 4; lastKnownFileType = sourcecode.c.c; path = navigationdesired.c; sourceTree = ""; }; 657CEEC3121DC054007A1FBE /* navigationsettings.c */ = {isa = PBXFileReference; fileEncoding = 4; lastKnownFileType = sourcecode.c.c; path = navigationsettings.c; sourceTree = ""; }; + 657CF024121F49CD007A1FBE /* WMMInternal.h */ = {isa = PBXFileReference; fileEncoding = 4; lastKnownFileType = sourcecode.c.h; path = WMMInternal.h; sourceTree = ""; }; 65B35D7F121C261E003EAD18 /* bin.pro */ = {isa = PBXFileReference; fileEncoding = 4; lastKnownFileType = text; path = bin.pro; sourceTree = ""; }; 65B35D80121C261E003EAD18 /* openpilotgcs */ = {isa = PBXFileReference; fileEncoding = 4; lastKnownFileType = text.script.sh; path = openpilotgcs; sourceTree = ""; }; 65B35D81121C261E003EAD18 /* openpilotgcs.pri */ = {isa = PBXFileReference; fileEncoding = 4; lastKnownFileType = text; path = openpilotgcs.pri; sourceTree = ""; }; @@ -2890,8 +2892,8 @@ isa = PBXGroup; children = ( 65B7E6AC120DF1CD000C1123 /* AHRS */, - 657CEEB6121DBC63007A1FBE /* Libraries */, 65E8EF1E11EEA61E00BBF654 /* OpenPilot */, + 657CEEB6121DBC63007A1FBE /* Libraries */, 65E8F02F11EFF25C00BBF654 /* PiOS */, C6A0FF2B0290797F04C91782 /* Documentation */, 1AB674ADFE9D54B511CA2CBB /* Products */, @@ -2913,6 +2915,7 @@ 657CEEB8121DBC63007A1FBE /* inc */ = { isa = PBXGroup; children = ( + 657CF024121F49CD007A1FBE /* WMMInternal.h */, 657CEEB9121DBC63007A1FBE /* CoordinateConversions.h */, 657CEEBA121DBC63007A1FBE /* WorldMagModel.h */, ); @@ -6664,6 +6667,7 @@ 65B367E3121C2620003EAD18 /* uavobjectdefinition */ = { isa = PBXGroup; children = ( + 655268BC121FBD2900410C6E /* ahrscalibration.xml */, 65B367E4121C2620003EAD18 /* actuatorcommand.xml */, 65B367E5121C2620003EAD18 /* actuatordesired.xml */, 65B367E6121C2620003EAD18 /* actuatorsettings.xml */, diff --git a/ground/src/plugins/uavobjects/homelocation.cpp b/ground/src/plugins/uavobjects/homelocation.cpp index 848ef98bd..266a92ed7 100644 --- a/ground/src/plugins/uavobjects/homelocation.cpp +++ b/ground/src/plugins/uavobjects/homelocation.cpp @@ -42,6 +42,12 @@ HomeLocation::HomeLocation(): UAVDataObject(OBJID, ISSINGLEINST, ISSETTINGS, NAM { // Create fields QList fields; + QStringList SetElemNames; + SetElemNames.append("0"); + QStringList SetEnumOptions; + SetEnumOptions.append("FALSE"); + SetEnumOptions.append("TRUE"); + fields.append( new UAVObjectField(QString("Set"), QString(""), UAVObjectField::ENUM, SetElemNames, SetEnumOptions) ); QStringList LatitudeElemNames; LatitudeElemNames.append("0"); fields.append( new UAVObjectField(QString("Latitude"), QString("deg * 10e6"), UAVObjectField::INT32, LatitudeElemNames, QStringList()) ); diff --git a/ground/src/plugins/uavobjects/homelocation.h b/ground/src/plugins/uavobjects/homelocation.h index 43976290d..934d3d7ba 100644 --- a/ground/src/plugins/uavobjects/homelocation.h +++ b/ground/src/plugins/uavobjects/homelocation.h @@ -43,6 +43,7 @@ class UAVOBJECTS_EXPORT HomeLocation: public UAVDataObject public: // Field structure typedef struct { + quint8 Set; qint32 Latitude; qint32 Longitude; float Altitude; @@ -53,6 +54,9 @@ public: } __attribute__((packed)) DataFields; // Field information + // Field Set information + /* Enumeration options for field Set */ + typedef enum { SET_FALSE=0, SET_TRUE=1 } SetOptions; // Field Latitude information // Field Longitude information // Field Altitude information @@ -68,7 +72,7 @@ public: // Constants - static const quint32 OBJID = 933172238U; + static const quint32 OBJID = 576787928U; static const QString NAME; static const bool ISSINGLEINST = 1; static const bool ISSETTINGS = 0; diff --git a/ground/src/plugins/uavobjects/homelocation.py b/ground/src/plugins/uavobjects/homelocation.py index 7b47e63bd..b46c81630 100644 --- a/ground/src/plugins/uavobjects/homelocation.py +++ b/ground/src/plugins/uavobjects/homelocation.py @@ -37,6 +37,18 @@ from collections import namedtuple # This is a list of instances of the data fields contained in this object _fields = [ \ + uavobject.UAVObjectField( + 'Set', + 'b', + 1, + [ + '0', + ], + { + '0' : 'FALSE', + '1' : 'TRUE', + } + ), uavobject.UAVObjectField( 'Latitude', 'i', @@ -114,7 +126,7 @@ _fields = [ \ class HomeLocation(uavobject.UAVObject): ## Object constants - OBJID = 933172238 + OBJID = 576787928 NAME = "HomeLocation" METANAME = "HomeLocationMeta" ISSINGLEINST = 1 diff --git a/ground/src/shared/uavobjectdefinition/homelocation.xml b/ground/src/shared/uavobjectdefinition/homelocation.xml index 7e45588f0..d014236e2 100644 --- a/ground/src/shared/uavobjectdefinition/homelocation.xml +++ b/ground/src/shared/uavobjectdefinition/homelocation.xml @@ -1,14 +1,15 @@ - - - - - - - - - - - - + + + + + + + + + + + + +