diff --git a/flight/libraries/CoordinateConversions.c b/flight/libraries/CoordinateConversions.c index 5d286aac1..1b0a51277 100644 --- a/flight/libraries/CoordinateConversions.c +++ b/flight/libraries/CoordinateConversions.c @@ -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]; diff --git a/flight/libraries/inc/CoordinateConversions.h b/flight/libraries/inc/CoordinateConversions.h index 46b100e0f..5e25f2053 100644 --- a/flight/libraries/inc/CoordinateConversions.h +++ b/flight/libraries/inc/CoordinateConversions.h @@ -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 **** diff --git a/flight/libraries/insgps13state.c b/flight/libraries/insgps13state.c index 414cbfb33..d014babb1 100644 --- a/flight/libraries/insgps13state.c +++ b/flight/libraries/insgps13state.c @@ -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; } } } diff --git a/flight/modules/Airspeed/revolution/gps_airspeed.c b/flight/modules/Airspeed/revolution/gps_airspeed.c index d81f66fef..7f63f622f 100644 --- a/flight/modules/Airspeed/revolution/gps_airspeed.c +++ b/flight/modules/Airspeed/revolution/gps_airspeed.c @@ -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 @@ -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); diff --git a/flight/modules/Altitude/altitude.c b/flight/modules/Altitude/altitude.c index 6675fea65..5514763f4 100644 --- a/flight/modules/Altitude/altitude.c +++ b/flight/modules/Altitude/altitude.c @@ -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) */ diff --git a/flight/modules/Altitude/revolution/altitude.c b/flight/modules/Altitude/revolution/altitude.c index 63e825cbd..c66f52ae8 100644 --- a/flight/modules/Altitude/revolution/altitude.c +++ b/flight/modules/Altitude/revolution/altitude.c @@ -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 #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); } } } diff --git a/flight/modules/AltitudeHold/altitudehold.c b/flight/modules/AltitudeHold/altitudehold.c index 3f2fe7909..a886de64c 100644 --- a/flight/modules/AltitudeHold/altitudehold.c +++ b/flight/modules/AltitudeHold/altitudehold.c @@ -28,7 +28,7 @@ /** * Input object: ActiveWaypoint - * Input object: PositionActual + * Input object: PositionState * Input object: ManualControlCommand * Output object: AttitudeDesired * @@ -48,14 +48,14 @@ #include #include #include -#include +#include #include #include // object that will be updated by the module -#include -#include +#include +#include #include #include -#include +#include #include #include // 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; diff --git a/flight/modules/Attitude/attitude.c b/flight/modules/Attitude/attitude.c index d625661f9..81c1f947d 100644 --- a/flight/modules/Attitude/attitude.c +++ b/flight/modules/Attitude/attitude.c @@ -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 #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]; + 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,13 +429,13 @@ 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]; - gyrosData->z = gyros[2]; + gyrosData->x = gyros[0]; + gyrosData->y = gyros[1]; + gyrosData->z = gyros[2]; if (bias_correct_gyro) { // Applying integral component here so it can be seen on the gyros and correct bias @@ -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) diff --git a/flight/modules/Attitude/revolution/attitude.c b/flight/modules/Attitude/revolution/attitude.c index 65d193610..fba41c75f 100644 --- a/flight/modules/Attitude/revolution/attitude.c +++ b/flight/modules/Attitude/revolution/attitude.c @@ -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 #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 +} + + /** * @} * @} diff --git a/flight/modules/CameraStab/camerastab.c b/flight/modules/CameraStab/camerastab.c index 0f927cbb9..0088c4015 100644 --- a/flight/modules/CameraStab/camerastab.c +++ b/flight/modules/CameraStab/camerastab.c @@ -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]; } diff --git a/flight/modules/Extensions/MagBaro/inc/magbaro.h b/flight/modules/Extensions/MagBaro/inc/magbaro.h index ad495fb1a..ce77f1ce4 100644 --- a/flight/modules/Extensions/MagBaro/inc/magbaro.h +++ b/flight/modules/Extensions/MagBaro/inc/magbaro.h @@ -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 diff --git a/flight/modules/Extensions/MagBaro/magbaro.c b/flight/modules/Extensions/MagBaro/magbaro.c index ef478b57f..cd6d485f1 100644 --- a/flight/modules/Extensions/MagBaro/magbaro.c +++ b/flight/modules/Extensions/MagBaro/magbaro.c @@ -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 diff --git a/flight/modules/FixedWingPathFollower/fixedwingpathfollower.c b/flight/modules/FixedWingPathFollower/fixedwingpathfollower.c index ef7cf19ff..641331393 100644 --- a/flight/modules/FixedWingPathFollower/fixedwingpathfollower.c +++ b/flight/modules/FixedWingPathFollower/fixedwingpathfollower.c @@ -28,7 +28,7 @@ /** * Input object: ActiveWaypoint - * Input object: PositionActual + * Input object: PositionState * Input object: ManualControlCommand * Output object: AttitudeDesired * @@ -45,17 +45,14 @@ #include -#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, + groundspeedDesired = sqrtf(velocityDesired.North * velocityDesired.North + velocityDesired.East * velocityDesired.East); + 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. } diff --git a/flight/modules/GPS/GPS.c b/flight/modules/GPS/GPS.c index caa61260f..b2b96a15d 100644 --- a/flight/modules/GPS/GPS.c +++ b/flight/modules/GPS/GPS.c @@ -32,11 +32,12 @@ #include -#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 diff --git a/flight/modules/GPS/NMEA.c b/flight/modules/GPS/NMEA.c index d5be3131d..cc16b64c0 100644 --- a/flight/modules/GPS/NMEA.c +++ b/flight/modules/GPS/NMEA.c @@ -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 */ diff --git a/flight/modules/GPS/UBX.c b/flight/modules/GPS/UBX.c index bf8fe8039..a58e926e4 100644 --- a/flight/modules/GPS/UBX.c +++ b/flight/modules/GPS/UBX.c @@ -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; } diff --git a/flight/modules/GPS/inc/GPS.h b/flight/modules/GPS/inc/GPS.h index ecc7470d8..6cecaa845 100644 --- a/flight/modules/GPS/inc/GPS.h +++ b/flight/modules/GPS/inc/GPS.h @@ -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 diff --git a/flight/modules/GPS/inc/NMEA.h b/flight/modules/GPS/inc/NMEA.h index a1e94a24e..04a7dc4ce 100644 --- a/flight/modules/GPS/inc/NMEA.h +++ b/flight/modules/GPS/inc/NMEA.h @@ -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 */ diff --git a/flight/modules/GPS/inc/UBX.h b/flight/modules/GPS/inc/UBX.h index 9a1c86eeb..fd1ee0fb1 100644 --- a/flight/modules/GPS/inc/UBX.h +++ b/flight/modules/GPS/inc/UBX.h @@ -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 */ diff --git a/flight/modules/MK/MKSerial/MKSerial.c b/flight/modules/MK/MKSerial/MKSerial.c index 53f19d737..b1d99edc4 100644 --- a/flight/modules/MK/MKSerial/MKSerial.c +++ b/flight/modules/MK/MKSerial/MKSerial.c @@ -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) { diff --git a/flight/modules/ManualControl/manualcontrol.c b/flight/modules/ManualControl/manualcontrol.c index 9dc1c5850..0c0d07a04 100644 --- a/flight/modules/ManualControl/manualcontrol.c +++ b/flight/modules/ManualControl/manualcontrol.c @@ -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(¤tDown); + PositionStateDownGet(¤tDown); if (changed) { // After not being in this mode for a while init at current height altitudeHoldDesiredData.Altitude = 0; diff --git a/flight/modules/Osd/OsdEtStd/OsdEtStd.c b/flight/modules/Osd/OsdEtStd/OsdEtStd.c index 363f125e1..fdf74f072 100644 --- a/flight/modules/Osd/OsdEtStd/OsdEtStd.c +++ b/flight/modules/Osd/OsdEtStd/OsdEtStd.c @@ -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); diff --git a/flight/modules/Osd/osdgen/osdgen.c b/flight/modules/Osd/osdgen/osdgen.c index 41c0f217b..a197366b1 100644 --- a/flight/modules/Osd/osdgen/osdgen.c +++ b/flight/modules/Osd/osdgen/osdgen.c @@ -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; diff --git a/flight/modules/Osd/osdinput/osdinput.c b/flight/modules/Osd/osdinput/osdinput.c index 78db66f2f..513f1f5b9 100644 --- a/flight/modules/Osd/osdinput/osdinput.c +++ b/flight/modules/Osd/osdinput/osdinput.c @@ -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); diff --git a/flight/modules/Osd/osdoutput/osdoutput.c b/flight/modules/Osd/osdoutput/osdoutput.c index 904a1b8e9..2157f3761 100644 --- a/flight/modules/Osd/osdoutput/osdoutput.c +++ b/flight/modules/Osd/osdoutput/osdoutput.c @@ -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; diff --git a/flight/modules/PathPlanner/pathplanner.c b/flight/modules/PathPlanner/pathplanner.c index 949919d5f..f4ece087b 100644 --- a/flight/modules/PathPlanner/pathplanner.c +++ b/flight/modules/PathPlanner/pathplanner.c @@ -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 diff --git a/flight/modules/Sensors/sensors.c b/flight/modules/Sensors/sensors.c index cf659013b..3b2397b90 100644 --- a/flight/modules/Sensors/sensors.c +++ b/flight/modules/Sensors/sensors.c @@ -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 #include -#include -#include -#include -#include -#include -#include +#include +#include +#include +#include #include #include #include @@ -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 && diff --git a/flight/modules/Sensors/simulated/sensors.c b/flight/modules/Sensors/simulated/Sensors/sensors.c similarity index 73% rename from flight/modules/Sensors/simulated/sensors.c rename to flight/modules/Sensors/simulated/Sensors/sensors.c index 769c17b79..0e583f016 100644 --- a/flight/modules/Sensors/simulated/sensors.c +++ b/flight/modules/Sensors/simulated/Sensors/sensors.c @@ -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 -#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 */ -} /** * @} diff --git a/flight/modules/Stabilization/inc/relay_tuning.h b/flight/modules/Stabilization/inc/relay_tuning.h index 2fd69cafa..6af8a3592 100644 --- a/flight/modules/Stabilization/inc/relay_tuning.h +++ b/flight/modules/Stabilization/inc/relay_tuning.h @@ -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 diff --git a/flight/modules/Stabilization/inc/stabilization.h b/flight/modules/Stabilization/inc/stabilization.h index 9d8192c43..396a91e89 100644 --- a/flight/modules/Stabilization/inc/stabilization.h +++ b/flight/modules/Stabilization/inc/stabilization.h @@ -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 diff --git a/flight/modules/Stabilization/relay_tuning.c b/flight/modules/Stabilization/relay_tuning.c index f9b85c190..7fc25aa3e 100644 --- a/flight/modules/Stabilization/relay_tuning.c +++ b/flight/modules/Stabilization/relay_tuning.c @@ -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 diff --git a/flight/modules/Stabilization/stabilization.c b/flight/modules/Stabilization/stabilization.c index 36bfeacc6..6cfeed50f 100644 --- a/flight/modules/Stabilization/stabilization.c +++ b/flight/modules/Stabilization/stabilization.c @@ -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; diff --git a/flight/modules/StateEstimation/filterair.c b/flight/modules/StateEstimation/filterair.c new file mode 100644 index 000000000..3ed89b99d --- /dev/null +++ b/flight/modules/StateEstimation/filterair.c @@ -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; +} + + +/** + * @} + * @} + */ diff --git a/flight/modules/StateEstimation/filterbaro.c b/flight/modules/StateEstimation/filterbaro.c new file mode 100644 index 000000000..7c5793717 --- /dev/null +++ b/flight/modules/StateEstimation/filterbaro.c @@ -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; +} + + +/** + * @} + * @} + */ diff --git a/flight/modules/StateEstimation/filtercf.c b/flight/modules/StateEstimation/filtercf.c new file mode 100644 index 000000000..ae896905a --- /dev/null +++ b/flight/modules/StateEstimation/filtercf.c @@ -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 +#include +#include +#include +#include + +#include + +// 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); +} + +/** + * @} + * @} + */ diff --git a/flight/modules/StateEstimation/filterekf.c b/flight/modules/StateEstimation/filterekf.c new file mode 100644 index 000000000..46b59723b --- /dev/null +++ b/flight/modules/StateEstimation/filterekf.c @@ -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 +#include +#include +#include + +#include +#include + +// 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; +} + +/** + * @} + * @} + */ diff --git a/flight/modules/StateEstimation/filtermag.c b/flight/modules/StateEstimation/filtermag.c new file mode 100644 index 000000000..7997016e0 --- /dev/null +++ b/flight/modules/StateEstimation/filtermag.c @@ -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 +#include +#include + +#include + +// 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 +} + + +/** + * @} + * @} + */ diff --git a/flight/modules/StateEstimation/filterstationary.c b/flight/modules/StateEstimation/filterstationary.c new file mode 100644 index 000000000..14075fd1c --- /dev/null +++ b/flight/modules/StateEstimation/filterstationary.c @@ -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; +} + +/** + * @} + * @} + */ diff --git a/flight/modules/StateEstimation/inc/stateestimation.h b/flight/modules/StateEstimation/inc/stateestimation.h new file mode 100644 index 000000000..9b954874a --- /dev/null +++ b/flight/modules/StateEstimation/inc/stateestimation.h @@ -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 + +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 diff --git a/flight/modules/StateEstimation/stateestimation.c b/flight/modules/StateEstimation/stateestimation.c new file mode 100644 index 000000000..5ed371844 --- /dev/null +++ b/flight/modules/StateEstimation/stateestimation.c @@ -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 +#include +#include +#include +#include +#include +#include + +#include +#include +#include +#include +#include +#include +#include + +#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); +} + + +/** + * @} + * @} + */ diff --git a/flight/modules/VtolPathFollower/vtolpathfollower.c b/flight/modules/VtolPathFollower/vtolpathfollower.c index 83e0ee553..63829b434 100644 --- a/flight/modules/VtolPathFollower/vtolpathfollower.c +++ b/flight/modules/VtolPathFollower/vtolpathfollower.c @@ -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); } } diff --git a/flight/pios/inc/pios_math.h b/flight/pios/inc/pios_math.h index 47c8d798b..dc843999d 100644 --- a/flight/pios/inc/pios_math.h +++ b/flight/pios/inc/pios_math.h @@ -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(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 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 diff --git a/flight/targets/boards/coptercontrol/firmware/Makefile b/flight/targets/boards/coptercontrol/firmware/Makefile index 35b1692f1..6ca39951e 100644 --- a/flight/targets/boards/coptercontrol/firmware/Makefile +++ b/flight/targets/boards/coptercontrol/firmware/Makefile @@ -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 diff --git a/flight/targets/boards/osd/firmware/Makefile b/flight/targets/boards/osd/firmware/Makefile index 77f7f22ae..9ffd1385f 100644 --- a/flight/targets/boards/osd/firmware/Makefile +++ b/flight/targets/boards/osd/firmware/Makefile @@ -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 diff --git a/flight/targets/boards/revolution/firmware/Makefile b/flight/targets/boards/revolution/firmware/Makefile index 888f73fcb..95eb42bca 100644 --- a/flight/targets/boards/revolution/firmware/Makefile +++ b/flight/targets/boards/revolution/firmware/Makefile @@ -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 diff --git a/flight/targets/boards/revolution/firmware/UAVObjects.inc b/flight/targets/boards/revolution/firmware/UAVObjects.inc index 57c9f180e..188b15bab 100644 --- a/flight/targets/boards/revolution/firmware/UAVObjects.inc +++ b/flight/targets/boards/revolution/firmware/UAVObjects.inc @@ -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 diff --git a/flight/targets/boards/revolution/firmware/inc/pios_config.h b/flight/targets/boards/revolution/firmware/inc/pios_config.h index f170eebfe..0ce0ff3ab 100644 --- a/flight/targets/boards/revolution/firmware/inc/pios_config.h +++ b/flight/targets/boards/revolution/firmware/inc/pios_config.h @@ -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 */ diff --git a/flight/targets/boards/revolution/firmware/pios_board_sim.c b/flight/targets/boards/revolution/firmware/pios_board_sim.c index 8af566e93..9def851ae 100644 --- a/flight/targets/boards/revolution/firmware/pios_board_sim.c +++ b/flight/targets/boards/revolution/firmware/pios_board_sim.c @@ -31,12 +31,11 @@ #include #include -#include -#include -#include -#include -#include -#include +#include +#include +#include +#include +#include #include 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(); diff --git a/flight/targets/boards/revoproto/firmware/Makefile b/flight/targets/boards/revoproto/firmware/Makefile index ecf9e24d1..c2fe5e312 100644 --- a/flight/targets/boards/revoproto/firmware/Makefile +++ b/flight/targets/boards/revoproto/firmware/Makefile @@ -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 diff --git a/flight/targets/boards/revoproto/firmware/UAVObjects.inc b/flight/targets/boards/revoproto/firmware/UAVObjects.inc index c7e12bdea..996708520 100644 --- a/flight/targets/boards/revoproto/firmware/UAVObjects.inc +++ b/flight/targets/boards/revoproto/firmware/UAVObjects.inc @@ -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 diff --git a/flight/targets/boards/revoproto/firmware/inc/pios_config.h b/flight/targets/boards/revoproto/firmware/inc/pios_config.h index 30be6bb7a..8b1bba11c 100644 --- a/flight/targets/boards/revoproto/firmware/inc/pios_config.h +++ b/flight/targets/boards/revoproto/firmware/inc/pios_config.h @@ -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 */ diff --git a/flight/targets/boards/revoproto/firmware/pios_board_sim.c b/flight/targets/boards/revoproto/firmware/pios_board_sim.c index a73beba28..9b4c60bc0 100644 --- a/flight/targets/boards/revoproto/firmware/pios_board_sim.c +++ b/flight/targets/boards/revoproto/firmware/pios_board_sim.c @@ -31,12 +31,11 @@ #include #include -#include -#include -#include -#include -#include -#include +#include +#include +#include +#include +#include #include 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(); diff --git a/flight/targets/boards/simposix/firmware/Makefile b/flight/targets/boards/simposix/firmware/Makefile index f78bbce23..badf65db1 100644 --- a/flight/targets/boards/simposix/firmware/Makefile +++ b/flight/targets/boards/simposix/firmware/Makefile @@ -38,6 +38,8 @@ MODULES += VtolPathFollower MODULES += CameraStab MODULES += Telemetry MODULES += FirmwareIAP +MODULES += StateEstimation +MODULES += Sensors/simulated/Sensors #MODULES += OveroSync # Paths diff --git a/flight/targets/boards/simposix/firmware/UAVObjects.inc b/flight/targets/boards/simposix/firmware/UAVObjects.inc index 23ffde045..58ad986ca 100644 --- a/flight/targets/boards/simposix/firmware/UAVObjects.inc +++ b/flight/targets/boards/simposix/firmware/UAVObjects.inc @@ -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 @@ -90,8 +94,11 @@ UAVOBJSRCFILENAMES += hwsettings UAVOBJSRCFILENAMES += receiveractivity UAVOBJSRCFILENAMES += cameradesired UAVOBJSRCFILENAMES += camerastabsettings -UAVOBJSRCFILENAMES += altitudeholdsettings +UAVOBJSRCFILENAMES += altitudeholdsettings +UAVOBJSRCFILENAMES += revosettings UAVOBJSRCFILENAMES += altitudeholddesired +UAVOBJSRCFILENAMES += ekfconfiguration +UAVOBJSRCFILENAMES += ekfstatevariance UAVOBJSRC = $(foreach UAVOBJSRCFILE,$(UAVOBJSRCFILENAMES),$(UAVOBJSYNTHDIR)/$(UAVOBJSRCFILE).c ) UAVOBJDEFINE = $(foreach UAVOBJSRCFILE,$(UAVOBJSRCFILENAMES),-DUAVOBJ_INIT_$(UAVOBJSRCFILE) ) diff --git a/flight/targets/boards/simposix/firmware/inc/pios_config.h b/flight/targets/boards/simposix/firmware/inc/pios_config.h index 70e3ed2b6..afa9557fa 100644 --- a/flight/targets/boards/simposix/firmware/inc/pios_config.h +++ b/flight/targets/boards/simposix/firmware/inc/pios_config.h @@ -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 diff --git a/ground/openpilotgcs/share/openpilotgcs/default_configurations/Developer.xml b/ground/openpilotgcs/share/openpilotgcs/default_configurations/Developer.xml index 824f123b0..430eeac3f 100644 --- a/ground/openpilotgcs/share/openpilotgcs/default_configurations/Developer.xml +++ b/ground/openpilotgcs/share/openpilotgcs/default_configurations/Developer.xml @@ -49,7 +49,7 @@ default - Accels + AccelState 0 false @@ -99,19 +99,19 @@ needle needle3 Lucida Grande,13,-1,5,50,0,0,0,0,0 - AttitudeActual + AttitudeState -1 360 0 Rotate Roll - AttitudeActual + AttitudeState 75 20 0 Vertical Pitch - AttitudeActual + AttitudeState -1 360 0 @@ -134,19 +134,19 @@ needle2 needle3 Lucida Grande,13,-1,5,50,0,0,0,0,0 - BaroAltitude + BaroSensor 1 10 0 Rotate Altitude - BaroAltitude + BaroSensor 1 100 0 Rotate Altitude - BaroAltitude + BaroSensor 1 1000 0 @@ -169,19 +169,19 @@ Lucida Grande,13,-1,5,50,0,0,0,0,0 - BaroAltitude + BaroSensor 10 1120 1000 Rotate Pressure - BaroAltitude + BaroSensor 1 100 0 Rotate Altitude - BaroAltitude + BaroSensor 1 1000 0 @@ -204,19 +204,19 @@ Lucida Grande,13,-1,5,50,0,0,0,0,0 - VelocityActual + VelocityState 0.01 12 -12 Rotate Down - BaroAltitude + BaroSensor 1 100 0 Rotate Altitude - BaroAltitude + BaroSensor 1 1000 0 @@ -239,19 +239,19 @@ Lucida Grande,13,-1,5,50,0,0,0,0,0 - AttitudeActual + AttitudeState -1 360 0 Rotate Yaw - BaroAltitude + BaroSensor 1 100 0 Rotate Altitude - BaroAltitude + BaroSensor 1 1000 0 @@ -274,19 +274,19 @@ needle needle3 Lucida Grande,13,-1,5,50,0,0,0,0,0 - AttitudeActual + AttitudeState -1 360 0 Rotate Roll - AttitudeActual + AttitudeState 75 20 0 Vertical Pitch - AttitudeActual + AttitudeState -1 360 0 @@ -309,19 +309,19 @@ needle2 needle3 Lucida Grande,13,-1,5,50,0,0,0,0,0 - BaroAltitude + BaroSensor 1 10 0 Rotate Altitude - BaroAltitude + BaroSensor 1 100 0 Rotate Altitude - BaroAltitude + BaroSensor 1 1000 0 @@ -344,19 +344,19 @@ Lucida Grande,13,-1,5,50,0,0,0,0,0 - BaroAltitude + BaroSensor 10 1120 1000 Rotate Pressure - BaroAltitude + BaroSensor 1 100 0 Rotate Altitude - BaroAltitude + BaroSensor 1 1000 0 @@ -379,19 +379,19 @@ Lucida Grande,13,-1,5,50,0,0,0,0,0 - VelocityActual + VelocityState 0.01 11.2 -11.2 Rotate Down - BaroAltitude + BaroSensor 1 100 0 Rotate Altitude - BaroAltitude + BaroSensor 1 1000 0 @@ -414,19 +414,19 @@ Lucida Grande,13,-1,5,50,0,0,0,0,0 - AttitudeActual + AttitudeState -1 360 0 Rotate Yaw - BaroAltitude + BaroSensor 1 100 0 Rotate Altitude - BaroAltitude + BaroSensor 1 1000 0 @@ -449,19 +449,19 @@ Lucida Grande,13,-1,5,50,0,0,0,0,0 - GPSPosition + GPSPositionSensor 3.6 120 0 Rotate Groundspeed - BaroAltitude + BaroSensor 1 100 0 Rotate Altitude - BaroAltitude + BaroSensor 1 1000 0 @@ -484,19 +484,19 @@ needle2 needle3 Lucida Grande,13,-1,5,50,0,0,0,0,0 - BaroAltitude + BaroSensor 1 120 0 Rotate Temperature - BaroAltitude + BaroSensor 1 100 0 Rotate Altitude - BaroAltitude + BaroSensor 1 1000 0 @@ -519,19 +519,19 @@ needle2 needle2 Lucida Grande,13,-1,5,50,0,0,0,0,0 - AttitudeActual + AttitudeState -1 360 0 Rotate Roll - Accels + AccelState 1 20 -20 Horizontal x - Accels + AccelState -1 360 0 @@ -554,19 +554,19 @@ Lucida Grande,13,-1,5,50,0,0,0,0,0 - GPSPosition + GPSPositionSensor 3.6 120 0 Rotate Groundspeed - BaroAltitude + BaroSensor 1 100 0 Rotate Altitude - BaroAltitude + BaroSensor 1 1000 0 @@ -589,19 +589,19 @@ needle needle3 Lucida Grande,13,-1,5,50,0,0,0,0,0 - AttitudeActual + AttitudeState -1 360 0 Rotate Roll - AttitudeActual + AttitudeState 75 20 0 Vertical Pitch - AttitudeActual + AttitudeState -1 360 0 @@ -624,19 +624,19 @@ needle2 needle3 Lucida Grande,13,-1,5,50,0,0,0,0,0 - BaroAltitude + BaroSensor 1 10 0 Rotate Altitude - BaroAltitude + BaroSensor 1 100 0 Rotate Altitude - BaroAltitude + BaroSensor 1 1000 0 @@ -659,19 +659,19 @@ Lucida Grande,13,-1,5,50,0,0,0,0,0 - BaroAltitude + BaroSensor 10 1120 1000 Rotate Pressure - BaroAltitude + BaroSensor 1 100 0 Rotate Altitude - BaroAltitude + BaroSensor 1 1000 0 @@ -694,19 +694,19 @@ Lucida Grande,13,-1,5,50,0,0,0,0,0 - VelocityActual + VelocityState 0.01 12 -12 Rotate Down - BaroAltitude + BaroSensor 1 100 0 Rotate Altitude - BaroAltitude + BaroSensor 1 1000 0 @@ -729,19 +729,19 @@ Lucida Grande,13,-1,5,50,0,0,0,0,0 - AttitudeActual + AttitudeState -1 360 0 Rotate Yaw - BaroAltitude + BaroSensor 1 100 0 Rotate Altitude - BaroAltitude + BaroSensor 1 1000 0 @@ -764,19 +764,19 @@ Lucida Grande,13,-1,5,50,0,0,0,0,0 - GPSPosition + GPSPositionSensor 3.6 120 0 Rotate Groundspeed - BaroAltitude + BaroSensor 1 100 0 Rotate Altitude - BaroAltitude + BaroSensor 1 1000 0 @@ -799,19 +799,19 @@ needle2 needle3 Lucida Grande,13,-1,5,50,0,0,0,0,0 - BaroAltitude + BaroSensor 1 120 0 Rotate Temperature - BaroAltitude + BaroSensor 1 100 0 Rotate Altitude - BaroAltitude + BaroSensor 1 1000 0 @@ -840,13 +840,13 @@ 1000 Rotate Channel-3 - BaroAltitude + BaroSensor 1 100 0 Rotate Altitude - BaroAltitude + BaroSensor 1 1000 0 @@ -869,19 +869,19 @@ needle2 needle3 Lucida Grande,13,-1,5,50,0,0,0,0,0 - BaroAltitude + BaroSensor 1 120 0 Rotate Temperature - BaroAltitude + BaroSensor 1 100 0 Rotate Altitude - BaroAltitude + BaroSensor 1 1000 0 @@ -1022,7 +1022,7 @@ false false true - true + true true 20 @@ -1064,7 +1064,7 @@ -11 11 -11 - Accels + AccelState x false -5 @@ -1087,7 +1087,7 @@ -11 11 -11 - Accels + AccelState y false -5 @@ -1110,7 +1110,7 @@ -11 11 -11 - Accels + AccelState z false -5 @@ -1202,7 +1202,7 @@ 0 0 0 - GPSPosition + GPSPositionSensor Satellites false 0 @@ -1225,7 +1225,7 @@ 0 33 0 - GPSPosition + GPSPositionSensor Status false 66 @@ -1301,7 +1301,7 @@ -0.8 - + false 0.0.0 @@ -1317,13 +1317,13 @@ -90 1 0 - AttitudeActual + AttitudeState Pitch false 0.9 0.1 - + false @@ -1847,7 +1847,7 @@ 4294901760 None x - Accels + AccelState 0 1 6.92920863607354e-310 @@ -1857,7 +1857,7 @@ 4283782655 None y - Accels + AccelState 1.78017180972965e-306 1 9.34609790258712e-307 @@ -1867,7 +1867,7 @@ 4283804160 None z - Accels + AccelState 2.6501977682312e-318 1 6.92920723246466e-310 @@ -1949,7 +1949,7 @@ 4283760895 None Roll - AttitudeActual + AttitudeState 6.92921152826347e-310 1 6.92921152826031e-310 @@ -1959,7 +1959,7 @@ 4278233600 None Yaw - AttitudeActual + AttitudeState 0 1 0 @@ -1969,7 +1969,7 @@ 4294901760 None Pitch - AttitudeActual + AttitudeState 0 1 0 @@ -1995,7 +1995,7 @@ 4278190080 None Pressure - BaroAltitude + BaroSensor 1.72723371102043e-77 1 3.86203233966055e-312 @@ -2102,7 +2102,7 @@ 200 - + false 0.0.0 @@ -2117,7 +2117,7 @@ 4294901760 None x - Accels + AccelState 0 1 0 @@ -2127,7 +2127,7 @@ 4283782655 None y - Accels + AccelState 0 1 0 @@ -2137,7 +2137,7 @@ 4283804160 None z - Accels + AccelState 0 1 0 @@ -2147,8 +2147,8 @@ 1 500 - - + + false 0.0.0 @@ -2163,7 +2163,7 @@ 4283804160 None z - Gyros + gyroState 1.02360527502876e-306 1 6.92921152846347e-310 @@ -2173,7 +2173,7 @@ 4283782655 None y - Gyros + gyroState 0 1 0 @@ -2183,7 +2183,7 @@ 4294901760 None x - Gyros + gyroState 0 1 0 @@ -2193,7 +2193,7 @@ 1 500 - + false @@ -2209,7 +2209,7 @@ 4294901760 None x - Magnetometer + MagState 6.92916505601691e-310 1 3.86203233966055e-312 @@ -2219,7 +2219,7 @@ 4283782655 None y - Magnetometer + MagState 1.72723371101889e-77 1 -1 @@ -2229,7 +2229,7 @@ 4283804160 None z - Magnetometer + MagState 1.72723371102028e-77 1 7.21478569792807e-313 @@ -2670,7 +2670,7 @@ ScopeGadget - Raw Gyros + Raw gyroState uavGadget diff --git a/ground/openpilotgcs/share/openpilotgcs/default_configurations/OpenPilotGCS.xml b/ground/openpilotgcs/share/openpilotgcs/default_configurations/OpenPilotGCS.xml index 25a31aa3b..cb6d21ffb 100644 --- a/ground/openpilotgcs/share/openpilotgcs/default_configurations/OpenPilotGCS.xml +++ b/ground/openpilotgcs/share/openpilotgcs/default_configurations/OpenPilotGCS.xml @@ -118,19 +118,19 @@ needle needle3 MS Shell Dlg 2,8.25,-1,5,50,0,0,0,0,0 - AttitudeActual + AttitudeState -1 360 0 Rotate Roll - AttitudeActual + AttitudeState 75 20 0 Vertical Pitch - AttitudeActual + AttitudeState -1 360 0 @@ -153,19 +153,19 @@ needle2 needle3 MS Shell Dlg 2,8.25,-1,5,50,0,0,0,0,0 - BaroAltitude + BaroSensor 1 10 0 Rotate Altitude - BaroAltitude + BaroSensor 1 100 0 Rotate Altitude - BaroAltitude + BaroSensor 1 1000 0 @@ -185,19 +185,19 @@ %%DATAPATH%%dials/default/barometer.svg needle MS Shell Dlg 2,8.25,-1,5,50,0,0,0,0,0 - BaroAltitude + BaroSensor 10 1120 1000 Rotate Pressure - BaroAltitude + BaroSensor 1 100 0 Rotate Altitude - BaroAltitude + BaroSensor 1 1000 0 @@ -217,19 +217,19 @@ %%DATAPATH%%dials/default/vsi.svg needle MS Shell Dlg 2,8.25,-1,5,50,0,0,0,0,0 - VelocityActual + VelocityState 0.01 12 -12 Rotate Down - BaroAltitude + BaroSensor 1 100 0 Rotate Altitude - BaroAltitude + BaroSensor 1 1000 0 @@ -250,19 +250,19 @@ foreground needle MS Shell Dlg 2,8.25,-1,5,50,0,0,0,0,0 - AttitudeActual + AttitudeState -1 360 0 Rotate Yaw - BaroAltitude + BaroSensor 1 100 0 Rotate Altitude - BaroAltitude + BaroSensor 1 1000 0 @@ -285,19 +285,19 @@ needle needle3 MS Shell Dlg 2,8.25,-1,5,50,0,0,0,0,0 - AttitudeActual + AttitudeState -1 360 0 Rotate Roll - AttitudeActual + AttitudeState 75 20 0 Vertical Pitch - AttitudeActual + AttitudeState -1 360 0 @@ -320,19 +320,19 @@ needle2 needle3 MS Shell Dlg 2,8.25,-1,5,50,0,0,0,0,0 - BaroAltitude + BaroSensor 1 10 0 Rotate Altitude - BaroAltitude + BaroSensor 1 100 0 Rotate Altitude - BaroAltitude + BaroSensor 1 1000 0 @@ -353,19 +353,19 @@ foreground needle MS Shell Dlg 2,8.25,-1,5,50,0,0,0,0,0 - BaroAltitude + BaroSensor 10 1120 1000 Rotate Pressure - BaroAltitude + BaroSensor 1 100 0 Rotate Altitude - BaroAltitude + BaroSensor 1 1000 0 @@ -386,19 +386,19 @@ foreground needle MS Shell Dlg 2,8.25,-1,5,50,0,0,0,0,0 - VelocityActual + VelocityState 0.01 11.2 -11.2 Rotate Down - BaroAltitude + BaroSensor 1 100 0 Rotate Altitude - BaroAltitude + BaroSensor 1 1000 0 @@ -419,19 +419,19 @@ foreground needle MS Shell Dlg 2,8.25,-1,5,50,0,0,0,0,0 - AttitudeActual + AttitudeState -1 360 0 Rotate Yaw - BaroAltitude + BaroSensor 1 100 0 Rotate Altitude - BaroAltitude + BaroSensor 1 1000 0 @@ -452,19 +452,19 @@ foreground needle MS Shell Dlg 2,8.25,-1,5,50,0,0,0,0,0 - GPSPosition + GPSPositionSensor 3.6 120 0 Rotate Groundspeed - BaroAltitude + BaroSensor 1 100 0 Rotate Altitude - BaroAltitude + BaroSensor 1 1000 0 @@ -487,19 +487,19 @@ needle2 needle3 MS Shell Dlg 2,8.25,-1,5,50,0,0,0,0,0 - BaroAltitude + BaroSensor 1 120 0 Rotate Temperature - BaroAltitude + BaroSensor 1 100 0 Rotate Altitude - BaroAltitude + BaroSensor 1 1000 0 @@ -522,19 +522,19 @@ needle2 needle2 MS Shell Dlg 2,8.25,-1,5,50,0,0,0,0,0 - AttitudeActual + AttitudeState -1 360 0 Rotate Roll - Accels + AccelState 1 20 -20 Horizontal x - Accels + AccelState -1 360 0 @@ -554,19 +554,19 @@ %%DATAPATH%%dials/default/speed.svg needle MS Shell Dlg 2,8.25,-1,5,50,0,0,0,0,0 - GPSPosition + GPSPositionSensor 3.6 120 0 Rotate Groundspeed - BaroAltitude + BaroSensor 1 100 0 Rotate Altitude - BaroAltitude + BaroSensor 1 1000 0 @@ -589,19 +589,19 @@ needle needle3 MS Shell Dlg 2,8.25,-1,5,50,0,0,0,0,0 - AttitudeActual + AttitudeState -1 360 0 Rotate Roll - AttitudeActual + AttitudeState 75 20 0 Vertical Pitch - AttitudeActual + AttitudeState -1 360 0 @@ -624,19 +624,19 @@ needle2 needle3 MS Shell Dlg 2,8.25,-1,5,50,0,0,0,0,0 - BaroAltitude + BaroSensor 1 10 0 Rotate Altitude - BaroAltitude + BaroSensor 1 100 0 Rotate Altitude - BaroAltitude + BaroSensor 1 1000 0 @@ -656,19 +656,19 @@ %%DATAPATH%%dials/hi-contrast/barometer.svg needle MS Shell Dlg 2,8.25,-1,5,50,0,0,0,0,0 - BaroAltitude + BaroSensor 10 1120 1000 Rotate Pressure - BaroAltitude + BaroSensor 1 100 0 Rotate Altitude - BaroAltitude + BaroSensor 1 1000 0 @@ -688,19 +688,19 @@ %%DATAPATH%%dials/hi-contrast/vsi.svg needle MS Shell Dlg 2,8.25,-1,5,50,0,0,0,0,0 - VelocityActual + VelocityState 0.01 12 -12 Rotate Down - BaroAltitude + BaroSensor 1 100 0 Rotate Altitude - BaroAltitude + BaroSensor 1 1000 0 @@ -721,19 +721,19 @@ foreground needle MS Shell Dlg 2,8.25,-1,5,50,0,0,0,0,0 - AttitudeActual + AttitudeState -1 360 0 Rotate Yaw - BaroAltitude + BaroSensor 1 100 0 Rotate Altitude - BaroAltitude + BaroSensor 1 1000 0 @@ -753,19 +753,19 @@ %%DATAPATH%%dials/hi-contrast/speed.svg needle MS Shell Dlg 2,8.25,-1,5,50,0,0,0,0,0 - GPSPosition + GPSPositionSensor 3.6 120 0 Rotate Groundspeed - BaroAltitude + BaroSensor 1 100 0 Rotate Altitude - BaroAltitude + BaroSensor 1 1000 0 @@ -787,19 +787,19 @@ needle2 needle3 MS Shell Dlg 2,8.25,-1,5,50,0,0,0,0,0 - BaroAltitude + BaroSensor 1 120 0 Rotate Temperature - BaroAltitude + BaroSensor 1 100 0 Rotate Altitude - BaroAltitude + BaroSensor 1 1000 0 @@ -827,13 +827,13 @@ 1000 Rotate Channel-3 - BaroAltitude + BaroSensor 1 100 0 Rotate Altitude - BaroAltitude + BaroSensor 1 1000 0 @@ -855,19 +855,19 @@ needle2 needle3 MS Shell Dlg 2,8.25,-1,5,50,0,0,0,0,0 - BaroAltitude + BaroSensor 1 120 0 Rotate Temperature - BaroAltitude + BaroSensor 1 100 0 Rotate Altitude - BaroAltitude + BaroSensor 1 1000 0 @@ -968,7 +968,7 @@ false false true - true + true true 20 50 @@ -1004,7 +1004,7 @@ false false true - true + true true 20 50 @@ -1040,7 +1040,7 @@ false false true - true + true true 20 50 @@ -1084,7 +1084,7 @@ -11 11 -11 - Accels + AccelState x false -5 @@ -1107,7 +1107,7 @@ -11 11 -11 - Accels + AccelState y false -5 @@ -1130,7 +1130,7 @@ -11 11 -11 - Accels + AccelState z false -5 @@ -1222,7 +1222,7 @@ 0 0 0 - GPSPosition + GPSPositionSensor Satellites false 0 @@ -1245,7 +1245,7 @@ 0 33 0 - GPSPosition + GPSPositionSensor Status false 66 @@ -1321,7 +1321,7 @@ -0.8 - + false 0.0.0 @@ -1337,13 +1337,13 @@ -90 1 0 - AttitudeActual + AttitudeState Pitch false 0.9 0.1 - + false @@ -1888,7 +1888,7 @@ 4294901760 None x - Accels + AccelState 0 1 0 @@ -1898,7 +1898,7 @@ 4283782655 None y - Accels + AccelState 0 1 0 @@ -1908,7 +1908,7 @@ 4283804160 None z - Accels + AccelState 0 1 0 @@ -1988,7 +1988,7 @@ 4283760895 None Roll - AttitudeActual + AttitudeState 0 1 0 @@ -1998,7 +1998,7 @@ 4278233600 None Yaw - AttitudeActual + AttitudeState 0 1 0 @@ -2008,7 +2008,7 @@ 4294901760 None Pitch - AttitudeActual + AttitudeState 0 1 0 @@ -2033,7 +2033,7 @@ 4278190080 None Pressure - BaroAltitude + BaroSensor 0 1 0 @@ -2139,7 +2139,7 @@ 200 - + false 0.0.0 @@ -2153,7 +2153,7 @@ 4294901760 None x - Accels + AccelState 0 1 0 @@ -2163,7 +2163,7 @@ 4283782655 None y - Accels + AccelState 0 1 0 @@ -2173,7 +2173,7 @@ 4283804160 None z - Accels + AccelState 0 1 0 @@ -2183,8 +2183,8 @@ 1 500 - - + + false 0.0.0 @@ -2198,7 +2198,7 @@ 4283804160 None z - Gyros + GyroState 0 1 0 @@ -2208,7 +2208,7 @@ 4283782655 None y - Gyros + GyroState 0 1 0 @@ -2218,7 +2218,7 @@ 4294901760 None x - Gyros + GyroState 0 1 0 @@ -2228,7 +2228,7 @@ 1 500 - + false @@ -2243,7 +2243,7 @@ 4294901760 None x - Magnetometer + MagState 0 1 0 @@ -2253,7 +2253,7 @@ 4283782655 None y - Magnetometer + MagState 0 1 0 @@ -2263,7 +2263,7 @@ 4283804160 None z - Magnetometer + MagState 0 1 0 @@ -2654,7 +2654,7 @@ ScopeGadget - Raw Gyros + Raw GyroState uavGadget diff --git a/ground/openpilotgcs/share/openpilotgcs/default_configurations/OpenPilotGCS_wide.xml b/ground/openpilotgcs/share/openpilotgcs/default_configurations/OpenPilotGCS_wide.xml index a73874fef..c66b7188a 100644 --- a/ground/openpilotgcs/share/openpilotgcs/default_configurations/OpenPilotGCS_wide.xml +++ b/ground/openpilotgcs/share/openpilotgcs/default_configurations/OpenPilotGCS_wide.xml @@ -115,19 +115,19 @@ needle needle3 MS Shell Dlg 2,8.25,-1,5,50,0,0,0,0,0 - AttitudeActual + AttitudeState -1 360 0 Rotate Roll - AttitudeActual + AttitudeState 75 20 0 Vertical Pitch - AttitudeActual + AttitudeState -1 360 0 @@ -150,19 +150,19 @@ needle2 needle3 MS Shell Dlg 2,8.25,-1,5,50,0,0,0,0,0 - BaroAltitude + BaroSensor 1 10 0 Rotate Altitude - BaroAltitude + BaroSensor 1 100 0 Rotate Altitude - BaroAltitude + BaroSensor 1 1000 0 @@ -182,19 +182,19 @@ %%DATAPATH%%dials/default/barometer.svg needle MS Shell Dlg 2,8.25,-1,5,50,0,0,0,0,0 - BaroAltitude + BaroSensor 10 1120 1000 Rotate Pressure - BaroAltitude + BaroSensor 1 100 0 Rotate Altitude - BaroAltitude + BaroSensor 1 1000 0 @@ -214,19 +214,19 @@ %%DATAPATH%%dials/default/vsi.svg needle MS Shell Dlg 2,8.25,-1,5,50,0,0,0,0,0 - VelocityActual + VelocityState 0.01 12 -12 Rotate Down - BaroAltitude + BaroSensor 1 100 0 Rotate Altitude - BaroAltitude + BaroSensor 1 1000 0 @@ -247,19 +247,19 @@ foreground needle MS Shell Dlg 2,8.25,-1,5,50,0,0,0,0,0 - AttitudeActual + AttitudeState -1 360 0 Rotate Yaw - BaroAltitude + BaroSensor 1 100 0 Rotate Altitude - BaroAltitude + BaroSensor 1 1000 0 @@ -282,19 +282,19 @@ needle needle3 MS Shell Dlg 2,8.25,-1,5,50,0,0,0,0,0 - AttitudeActual + AttitudeState -1 360 0 Rotate Roll - AttitudeActual + AttitudeState 75 20 0 Vertical Pitch - AttitudeActual + AttitudeState -1 360 0 @@ -317,19 +317,19 @@ needle2 needle3 MS Shell Dlg 2,8.25,-1,5,50,0,0,0,0,0 - BaroAltitude + BaroSensor 1 10 0 Rotate Altitude - BaroAltitude + BaroSensor 1 100 0 Rotate Altitude - BaroAltitude + BaroSensor 1 1000 0 @@ -350,19 +350,19 @@ foreground needle MS Shell Dlg 2,8.25,-1,5,50,0,0,0,0,0 - BaroAltitude + BaroSensor 10 1120 1000 Rotate Pressure - BaroAltitude + BaroSensor 1 100 0 Rotate Altitude - BaroAltitude + BaroSensor 1 1000 0 @@ -383,19 +383,19 @@ foreground needle MS Shell Dlg 2,8.25,-1,5,50,0,0,0,0,0 - VelocityActual + VelocityState 0.01 11.2 -11.2 Rotate Down - BaroAltitude + BaroSensor 1 100 0 Rotate Altitude - BaroAltitude + BaroSensor 1 1000 0 @@ -416,19 +416,19 @@ foreground needle MS Shell Dlg 2,8.25,-1,5,50,0,0,0,0,0 - AttitudeActual + AttitudeState -1 360 0 Rotate Yaw - BaroAltitude + BaroSensor 1 100 0 Rotate Altitude - BaroAltitude + BaroSensor 1 1000 0 @@ -449,19 +449,19 @@ foreground needle MS Shell Dlg 2,8.25,-1,5,50,0,0,0,0,0 - GPSPosition + GPSPositionSensor 3.6 120 0 Rotate Groundspeed - BaroAltitude + BaroSensor 1 100 0 Rotate Altitude - BaroAltitude + BaroSensor 1 1000 0 @@ -484,19 +484,19 @@ needle2 needle3 MS Shell Dlg 2,8.25,-1,5,50,0,0,0,0,0 - BaroAltitude + BaroSensor 1 120 0 Rotate Temperature - BaroAltitude + BaroSensor 1 100 0 Rotate Altitude - BaroAltitude + BaroSensor 1 1000 0 @@ -519,19 +519,19 @@ needle2 needle2 MS Shell Dlg 2,8.25,-1,5,50,0,0,0,0,0 - AttitudeActual + AttitudeState -1 360 0 Rotate Roll - Accels + AccelState 1 20 -20 Horizontal x - Accels + AccelState -1 360 0 @@ -551,19 +551,19 @@ %%DATAPATH%%dials/default/speed.svg needle MS Shell Dlg 2,8.25,-1,5,50,0,0,0,0,0 - GPSPosition + GPSPositionSensor 3.6 120 0 Rotate Groundspeed - BaroAltitude + BaroSensor 1 100 0 Rotate Altitude - BaroAltitude + BaroSensor 1 1000 0 @@ -586,19 +586,19 @@ needle needle3 MS Shell Dlg 2,8.25,-1,5,50,0,0,0,0,0 - AttitudeActual + AttitudeState -1 360 0 Rotate Roll - AttitudeActual + AttitudeState 75 20 0 Vertical Pitch - AttitudeActual + AttitudeState -1 360 0 @@ -621,19 +621,19 @@ needle2 needle3 MS Shell Dlg 2,8.25,-1,5,50,0,0,0,0,0 - BaroAltitude + BaroSensor 1 10 0 Rotate Altitude - BaroAltitude + BaroSensor 1 100 0 Rotate Altitude - BaroAltitude + BaroSensor 1 1000 0 @@ -653,19 +653,19 @@ %%DATAPATH%%dials/hi-contrast/barometer.svg needle MS Shell Dlg 2,8.25,-1,5,50,0,0,0,0,0 - BaroAltitude + BaroSensor 10 1120 1000 Rotate Pressure - BaroAltitude + BaroSensor 1 100 0 Rotate Altitude - BaroAltitude + BaroSensor 1 1000 0 @@ -685,19 +685,19 @@ %%DATAPATH%%dials/hi-contrast/vsi.svg needle MS Shell Dlg 2,8.25,-1,5,50,0,0,0,0,0 - VelocityActual + VelocityState 0.01 12 -12 Rotate Down - BaroAltitude + BaroSensor 1 100 0 Rotate Altitude - BaroAltitude + BaroSensor 1 1000 0 @@ -718,19 +718,19 @@ foreground needle MS Shell Dlg 2,8.25,-1,5,50,0,0,0,0,0 - AttitudeActual + AttitudeState -1 360 0 Rotate Yaw - BaroAltitude + BaroSensor 1 100 0 Rotate Altitude - BaroAltitude + BaroSensor 1 1000 0 @@ -750,19 +750,19 @@ %%DATAPATH%%dials/hi-contrast/speed.svg needle MS Shell Dlg 2,8.25,-1,5,50,0,0,0,0,0 - GPSPosition + GPSPositionSensor 3.6 120 0 Rotate Groundspeed - BaroAltitude + BaroSensor 1 100 0 Rotate Altitude - BaroAltitude + BaroSensor 1 1000 0 @@ -784,19 +784,19 @@ needle2 needle3 MS Shell Dlg 2,8.25,-1,5,50,0,0,0,0,0 - BaroAltitude + BaroSensor 1 120 0 Rotate Temperature - BaroAltitude + BaroSensor 1 100 0 Rotate Altitude - BaroAltitude + BaroSensor 1 1000 0 @@ -824,13 +824,13 @@ 1000 Rotate Channel-3 - BaroAltitude + BaroSensor 1 100 0 Rotate Altitude - BaroAltitude + BaroSensor 1 1000 0 @@ -852,19 +852,19 @@ needle2 needle3 MS Shell Dlg 2,8.25,-1,5,50,0,0,0,0,0 - BaroAltitude + BaroSensor 1 120 0 Rotate Temperature - BaroAltitude + BaroSensor 1 100 0 Rotate Altitude - BaroAltitude + BaroSensor 1 1000 0 @@ -965,7 +965,7 @@ false false true - true + true true 20 50 @@ -1001,7 +1001,7 @@ false false true - true + true true 20 50 @@ -1037,7 +1037,7 @@ false false true - true + true true 20 50 @@ -1081,7 +1081,7 @@ -11 11 -11 - Accels + AccelState x false -5 @@ -1104,7 +1104,7 @@ -11 11 -11 - Accels + AccelState y false -5 @@ -1127,7 +1127,7 @@ -11 11 -11 - Accels + AccelState z false -5 @@ -1219,7 +1219,7 @@ 0 0 0 - GPSPosition + GPSPositionSensor Satellites false 0 @@ -1242,7 +1242,7 @@ 0 33 0 - GPSPosition + GPSPositionSensor Status false 66 @@ -1318,7 +1318,7 @@ -0.8 - + false 0.0.0 @@ -1334,13 +1334,13 @@ -90 1 0 - AttitudeActual + AttitudeState Pitch false 0.9 0.1 - + false @@ -1863,7 +1863,7 @@ 4294901760 None x - Accels + AccelState 0 1 0 @@ -1873,7 +1873,7 @@ 4283782655 None y - Accels + AccelState 0 1 0 @@ -1883,7 +1883,7 @@ 4283804160 None z - Accels + AccelState 0 1 0 @@ -1963,7 +1963,7 @@ 4283760895 None Roll - AttitudeActual + AttitudeState 0 1 0 @@ -1973,7 +1973,7 @@ 4278233600 None Yaw - AttitudeActual + AttitudeState 0 1 0 @@ -1983,7 +1983,7 @@ 4294901760 None Pitch - AttitudeActual + AttitudeState 0 1 0 @@ -2008,7 +2008,7 @@ 4278190080 None Pressure - BaroAltitude + BaroSensor 0 1 0 @@ -2114,7 +2114,7 @@ 200 - + false 0.0.0 @@ -2128,7 +2128,7 @@ 4294901760 None x - Accels + AccelState 0 1 0 @@ -2138,7 +2138,7 @@ 4283782655 None y - Accels + AccelState 0 1 0 @@ -2148,7 +2148,7 @@ 4283804160 None z - Accels + AccelState 0 1 0 @@ -2158,8 +2158,8 @@ 1 500 - - + + false 0.0.0 @@ -2173,7 +2173,7 @@ 4283804160 None z - Gyros + GyroState 0 1 0 @@ -2183,7 +2183,7 @@ 4283782655 None y - Gyros + GyroState 0 1 0 @@ -2193,7 +2193,7 @@ 4294901760 None x - Gyros + GyroState 0 1 0 @@ -2203,7 +2203,7 @@ 1 500 - + false @@ -2218,7 +2218,7 @@ 4294901760 None x - Magnetometer + MagState 0 1 0 @@ -2228,7 +2228,7 @@ 4283782655 None y - Magnetometer + MagState 0 1 0 @@ -2238,7 +2238,7 @@ 4283804160 None z - Magnetometer + MagState 0 1 0 @@ -2641,7 +2641,7 @@ ScopeGadget - Raw Gyros + Raw GyroState uavGadget diff --git a/ground/openpilotgcs/share/openpilotgcs/pfd/default/AltitudeScale.qml b/ground/openpilotgcs/share/openpilotgcs/pfd/default/AltitudeScale.qml index 4d0ef2c89..05a701f35 100644 --- a/ground/openpilotgcs/share/openpilotgcs/pfd/default/AltitudeScale.qml +++ b/ground/openpilotgcs/share/openpilotgcs/pfd/default/AltitudeScale.qml @@ -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" diff --git a/ground/openpilotgcs/share/openpilotgcs/pfd/default/Compass.qml b/ground/openpilotgcs/share/openpilotgcs/pfd/default/Compass.qml index 3c956d526..db2ba4d65 100644 --- a/ground/openpilotgcs/share/openpilotgcs/pfd/default/Compass.qml +++ b/ground/openpilotgcs/share/openpilotgcs/pfd/default/Compass.qml @@ -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 diff --git a/ground/openpilotgcs/share/openpilotgcs/pfd/default/Pfd.qml b/ground/openpilotgcs/share/openpilotgcs/pfd/default/Pfd.qml index e393b185e..e03a20e02 100644 --- a/ground/openpilotgcs/share/openpilotgcs/pfd/default/Pfd.qml +++ b/ground/openpilotgcs/share/openpilotgcs/pfd/default/Pfd.qml @@ -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 { diff --git a/ground/openpilotgcs/share/openpilotgcs/pfd/default/PfdIndicators.qml b/ground/openpilotgcs/share/openpilotgcs/pfd/default/PfdIndicators.qml index fd6f175d4..641bbbb2e 100644 --- a/ground/openpilotgcs/share/openpilotgcs/pfd/default/PfdIndicators.qml +++ b/ground/openpilotgcs/share/openpilotgcs/pfd/default/PfdIndicators.qml @@ -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) diff --git a/ground/openpilotgcs/share/openpilotgcs/pfd/default/PfdTerrainView.qml b/ground/openpilotgcs/share/openpilotgcs/pfd/default/PfdTerrainView.qml index f56f6d89c..2beebf05f 100644 --- a/ground/openpilotgcs/share/openpilotgcs/pfd/default/PfdTerrainView.qml +++ b/ground/openpilotgcs/share/openpilotgcs/pfd/default/PfdTerrainView.qml @@ -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 } diff --git a/ground/openpilotgcs/share/openpilotgcs/pfd/default/PfdWorldView.qml b/ground/openpilotgcs/share/openpilotgcs/pfd/default/PfdWorldView.qml index 9a461464f..4ce07956b 100644 --- a/ground/openpilotgcs/share/openpilotgcs/pfd/default/PfdWorldView.qml +++ b/ground/openpilotgcs/share/openpilotgcs/pfd/default/PfdWorldView.qml @@ -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 } diff --git a/ground/openpilotgcs/share/openpilotgcs/pfd/default/SpeedScale.qml b/ground/openpilotgcs/share/openpilotgcs/pfd/default/SpeedScale.qml index 6d8643e66..bbb7d50e2 100644 --- a/ground/openpilotgcs/share/openpilotgcs/pfd/default/SpeedScale.qml +++ b/ground/openpilotgcs/share/openpilotgcs/pfd/default/SpeedScale.qml @@ -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 diff --git a/ground/openpilotgcs/share/openpilotgcs/pfd/default/VsiScale.qml b/ground/openpilotgcs/share/openpilotgcs/pfd/default/VsiScale.qml index 171c048e0..dbcf6aa63 100644 --- a/ground/openpilotgcs/share/openpilotgcs/pfd/default/VsiScale.qml +++ b/ground/openpilotgcs/share/openpilotgcs/pfd/default/VsiScale.qml @@ -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 diff --git a/ground/openpilotgcs/share/openpilotgcs/translations/openpilotgcs_de.ts b/ground/openpilotgcs/share/openpilotgcs/translations/openpilotgcs_de.ts index 106433d33..e69a702ca 100644 --- a/ground/openpilotgcs/share/openpilotgcs/translations/openpilotgcs_de.ts +++ b/ground/openpilotgcs/share/openpilotgcs/translations/openpilotgcs_de.ts @@ -1767,7 +1767,7 @@ p, li { white-space: pre-wrap; } - AttitudeActual + AttitudeState @@ -1807,7 +1807,7 @@ p, li { white-space: pre-wrap; } - GPSPosition + GPSPositionSensor diff --git a/ground/openpilotgcs/share/openpilotgcs/translations/openpilotgcs_es.ts b/ground/openpilotgcs/share/openpilotgcs/translations/openpilotgcs_es.ts index 1a9d98543..8f5d3d6cb 100644 --- a/ground/openpilotgcs/share/openpilotgcs/translations/openpilotgcs_es.ts +++ b/ground/openpilotgcs/share/openpilotgcs/translations/openpilotgcs_es.ts @@ -1768,7 +1768,7 @@ p, li { white-space: pre-wrap; } - AttitudeActual + AttitudeState @@ -1808,7 +1808,7 @@ p, li { white-space: pre-wrap; } - GPSPosition + GPSPositionSensor diff --git a/ground/openpilotgcs/share/openpilotgcs/translations/openpilotgcs_fr.ts b/ground/openpilotgcs/share/openpilotgcs/translations/openpilotgcs_fr.ts index 165057d2f..b4f7cf54b 100644 --- a/ground/openpilotgcs/share/openpilotgcs/translations/openpilotgcs_fr.ts +++ b/ground/openpilotgcs/share/openpilotgcs/translations/openpilotgcs_fr.ts @@ -1789,7 +1789,7 @@ p, li { white-space: pre-wrap; } - AttitudeActual + AttitudeState @@ -1829,7 +1829,7 @@ p, li { white-space: pre-wrap; } - GPSPosition + GPSPositionSensor diff --git a/ground/openpilotgcs/share/openpilotgcs/translations/openpilotgcs_ru.ts b/ground/openpilotgcs/share/openpilotgcs/translations/openpilotgcs_ru.ts index 5d625d8a2..75687dfff 100644 --- a/ground/openpilotgcs/share/openpilotgcs/translations/openpilotgcs_ru.ts +++ b/ground/openpilotgcs/share/openpilotgcs/translations/openpilotgcs_ru.ts @@ -1721,7 +1721,7 @@ Sat SNR is displayed above (in dBHz) - AttitudeActual + AttitudeState @@ -1761,7 +1761,7 @@ Sat SNR is displayed above (in dBHz) - GPSPosition + GPSPositionSensor diff --git a/ground/openpilotgcs/share/openpilotgcs/translations/openpilotgcs_zh_CN.ts b/ground/openpilotgcs/share/openpilotgcs/translations/openpilotgcs_zh_CN.ts index c1c16f9ed..6d9c4593e 100644 --- a/ground/openpilotgcs/share/openpilotgcs/translations/openpilotgcs_zh_CN.ts +++ b/ground/openpilotgcs/share/openpilotgcs/translations/openpilotgcs_zh_CN.ts @@ -2607,7 +2607,7 @@ Sat SNR is displayed above (in dBHz) - AttitudeActual + AttitudeState @@ -2647,7 +2647,7 @@ Sat SNR is displayed above (in dBHz) - GPSPosition + GPSPositionSensor diff --git a/ground/openpilotgcs/src/plugins/antennatrack/telemetryparser.cpp b/ground/openpilotgcs/src/plugins/antennatrack/telemetryparser.cpp index 6bbdef701..3b9669521 100644 --- a/ground/openpilotgcs/src/plugins/antennatrack/telemetryparser.cpp +++ b/ground/openpilotgcs/src/plugins/antennatrack/telemetryparser.cpp @@ -39,12 +39,12 @@ TelemetryParser::TelemetryParser(QObject *parent) : GPSParser(parent) { ExtensionSystem::PluginManager *pm = ExtensionSystem::PluginManager::instance(); UAVObjectManager *objManager = pm->getObject(); - UAVDataObject *gpsObj = dynamic_cast(objManager->getObject("GPSPosition")); + UAVDataObject *gpsObj = dynamic_cast(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(objManager->getObject("HomeLocation")); diff --git a/ground/openpilotgcs/src/plugins/config/configccattitudewidget.cpp b/ground/openpilotgcs/src/plugins/config/configccattitudewidget.cpp index 79d69382e..17a4269ff 100644 --- a/ground/openpilotgcs/src/plugins/config/configccattitudewidget.cpp +++ b/ground/openpilotgcs/src/plugins/config/configccattitudewidget.cpp @@ -33,8 +33,8 @@ #include #include #include -#include "accels.h" -#include "gyros.h" +#include "accelstate.h" +#include "gyrostate.h" #include #include @@ -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())); } diff --git a/ground/openpilotgcs/src/plugins/config/configccattitudewidget.h b/ground/openpilotgcs/src/plugins/config/configccattitudewidget.h index c9b835050..0c74326b8 100644 --- a/ground/openpilotgcs/src/plugins/config/configccattitudewidget.h +++ b/ground/openpilotgcs/src/plugins/config/configccattitudewidget.h @@ -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; diff --git a/ground/openpilotgcs/src/plugins/config/configrevowidget.cpp b/ground/openpilotgcs/src/plugins/config/configrevowidget.cpp index 5827699d4..2e2bf9a33 100644 --- a/ground/openpilotgcs/src/plugins/config/configrevowidget.cpp +++ b/ground/openpilotgcs/src/plugins/config/configrevowidget.cpp @@ -44,9 +44,9 @@ #include #include #include -#include -#include -#include +#include +#include +#include #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()); diff --git a/ground/openpilotgcs/src/plugins/config/configrevowidget.h b/ground/openpilotgcs/src/plugins/config/configrevowidget.h index 9eb349612..6e26abf91 100644 --- a/ground/openpilotgcs/src/plugins/config/configrevowidget.h +++ b/ground/openpilotgcs/src/plugins/config/configrevowidget.h @@ -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; diff --git a/ground/openpilotgcs/src/plugins/gpsdisplay/telemetryparser.cpp b/ground/openpilotgcs/src/plugins/gpsdisplay/telemetryparser.cpp index 70fc4bc1a..780fc7854 100644 --- a/ground/openpilotgcs/src/plugins/gpsdisplay/telemetryparser.cpp +++ b/ground/openpilotgcs/src/plugins/gpsdisplay/telemetryparser.cpp @@ -39,12 +39,12 @@ TelemetryParser::TelemetryParser(QObject *parent) : GPSParser(parent) { ExtensionSystem::PluginManager *pm = ExtensionSystem::PluginManager::instance(); UAVObjectManager *objManager = pm->getObject(); - UAVDataObject *gpsObj = dynamic_cast(objManager->getObject("GPSPosition")); + UAVDataObject *gpsObj = dynamic_cast(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(objManager->getObject("GPSTime")); diff --git a/ground/openpilotgcs/src/plugins/hitl/fgsimulator.cpp b/ground/openpilotgcs/src/plugins/hitl/fgsimulator.cpp index 790265ea2..a73501b8e 100644 --- a/ground/openpilotgcs/src/plugins/hitl/fgsimulator.cpp +++ b/ground/openpilotgcs/src/plugins/hitl/fgsimulator.cpp @@ -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; diff --git a/ground/openpilotgcs/src/plugins/hitl/hitlconfiguration.cpp b/ground/openpilotgcs/src/plugins/hitl/hitlconfiguration.cpp index 244c27b6c..a59a6c9d6 100644 --- a/ground/openpilotgcs/src/plugins/hitl/hitlconfiguration.cpp +++ b/ground/openpilotgcs/src/plugins/hitl/hitlconfiguration.cpp @@ -31,40 +31,40 @@ HITLConfiguration::HITLConfiguration(QString classId, QSettings *qSettings, QObj IUAVGadgetConfiguration(classId, parent) { // Default settings values - settings.simulatorId = ""; - settings.binPath = ""; - settings.dataPath = ""; - settings.manualControlEnabled = true; - settings.startSim = false; - settings.addNoise = false; - settings.hostAddress = "127.0.0.1"; - settings.remoteAddress = "127.0.0.1"; - settings.outPort = 0; + settings.simulatorId = ""; + settings.binPath = ""; + settings.dataPath = ""; + settings.manualControlEnabled = true; + settings.startSim = false; + settings.addNoise = false; + settings.hostAddress = "127.0.0.1"; + settings.remoteAddress = "127.0.0.1"; + settings.outPort = 0; settings.inPort = 0; - settings.latitude = ""; - settings.longitude = ""; + settings.latitude = ""; + settings.longitude = ""; - settings.attRawEnabled = false; - settings.attRawRate = 20; + settings.attRawEnabled = false; + settings.attRawRate = 20; - settings.attActualEnabled = true; - settings.attActHW = false; - settings.attActSim = true; - settings.attActCalc = false; + settings.attStateEnabled = true; + settings.attActHW = false; + settings.attActSim = true; + settings.attActCalc = false; - settings.gpsPositionEnabled = false; - settings.gpsPosRate = 100; + settings.gpsPositionEnabled = false; + settings.gpsPosRate = 100; - settings.groundTruthEnabled = false; - settings.groundTruthRate = 100; + settings.groundTruthEnabled = false; + settings.groundTruthRate = 100; - settings.inputCommand = false; - settings.gcsReceiverEnabled = false; - settings.manualControlEnabled = false; - settings.minOutputPeriod = 100; + settings.inputCommand = false; + settings.gcsReceiverEnabled = false; + 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 @@ -86,28 +86,28 @@ HITLConfiguration::HITLConfiguration(QString classId, QSettings *qSettings, QObj settings.gcsReceiverEnabled = qSettings->value("gcsReceiverEnabled").toBool(); settings.manualControlEnabled = qSettings->value("manualControlEnabled").toBool(); - settings.attRawEnabled = qSettings->value("attRawEnabled").toBool(); - settings.attRawRate = qSettings->value("attRawRate").toInt(); + 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.attActCalc = qSettings->value("attActCalc").toBool(); - settings.baroAltitudeEnabled = qSettings->value("baroAltitudeEnabled").toBool(); - settings.baroAltRate = qSettings->value("baroAltRate").toInt(); + settings.baroSensorEnabled = qSettings->value("baroSensorEnabled").toBool(); + settings.baroAltRate = qSettings->value("baroAltRate").toInt(); - settings.gpsPositionEnabled = qSettings->value("gpsPositionEnabled").toBool(); - settings.gpsPosRate = qSettings->value("gpsPosRate").toInt(); + settings.gpsPositionEnabled = qSettings->value("gpsPositionEnabled").toBool(); + settings.gpsPosRate = qSettings->value("gpsPosRate").toInt(); - settings.groundTruthEnabled = qSettings->value("groundTruthEnabled").toBool(); - settings.groundTruthRate = qSettings->value("groundTruthRate").toInt(); + settings.groundTruthEnabled = qSettings->value("groundTruthEnabled").toBool(); + settings.groundTruthRate = qSettings->value("groundTruthRate").toInt(); - settings.inputCommand = qSettings->value("inputCommand").toBool(); - settings.minOutputPeriod = qSettings->value("minOutputPeriod").toInt(); + 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); } diff --git a/ground/openpilotgcs/src/plugins/hitl/hitlnoisegeneration.cpp b/ground/openpilotgcs/src/plugins/hitl/hitlnoisegeneration.cpp index 637707c44..a9ec1414e 100644 --- a/ground/openpilotgcs/src/plugins/hitl/hitlnoisegeneration.cpp +++ b/ground/openpilotgcs/src/plugins/hitl/hitlnoisegeneration.cpp @@ -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; @@ -54,25 +54,25 @@ Noise HitlNoiseGeneration::generateNoise() noise.gpsPosData.Heading = 0; noise.gpsPosData.Altitude = 0; - noise.gpsVelData.North = 0; - noise.gpsVelData.East = 0; - noise.gpsVelData.Down = 0; + noise.gpsVelData.North = 0; + noise.gpsVelData.East = 0; + noise.gpsVelData.Down = 0; 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; } diff --git a/ground/openpilotgcs/src/plugins/hitl/hitlnoisegeneration.h b/ground/openpilotgcs/src/plugins/hitl/hitlnoisegeneration.h index ead9ba9dd..22df94b72 100644 --- a/ground/openpilotgcs/src/plugins/hitl/hitlnoisegeneration.h +++ b/ground/openpilotgcs/src/plugins/hitl/hitlnoisegeneration.h @@ -37,16 +37,16 @@ #include struct Noise { - Accels::DataFields accelData; - AttitudeActual::DataFields attActualData; - BaroAltitude::DataFields baroAltData; - AirspeedActual::DataFields airspeedActual; - GPSPosition::DataFields gpsPosData; - GPSVelocity::DataFields gpsVelData; - Gyros::DataFields gyroData; - HomeLocation::DataFields homeData; - PositionActual::DataFields positionActualData; - VelocityActual::DataFields velocityActualData; + AccelState::DataFields accelStateData; + AttitudeState::DataFields attStateData; + BaroSensor::DataFields baroAltData; + AirspeedState::DataFields airspeedState; + GPSPositionSensor::DataFields gpsPosData; + GPSVelocitySensor::DataFields gpsVelData; + GyroState::DataFields gyroStateData; + HomeLocation::DataFields homeData; + PositionState::DataFields positionStateData; + VelocityState::DataFields velocityStateData; }; class HitlNoiseGeneration { diff --git a/ground/openpilotgcs/src/plugins/hitl/hitloptionspage.cpp b/ground/openpilotgcs/src/plugins/hitl/hitloptionspage.cpp index c85b2fb11..6cb69ddc7 100644 --- a/ground/openpilotgcs/src/plugins/hitl/hitloptionspage.cpp +++ b/ground/openpilotgcs/src/plugins/hitl/hitloptionspage.cpp @@ -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; } @@ -121,46 +121,46 @@ void HITLOptionsPage::apply() SimulatorSettings settings; int i = m_optionsPage->chooseFlightSimulator->currentIndex(); - settings.simulatorId = m_optionsPage->chooseFlightSimulator->itemData(i).toString(); - settings.binPath = m_optionsPage->executablePath->path(); - settings.dataPath = m_optionsPage->dataPath->path(); - settings.startSim = m_optionsPage->startSim->isChecked(); - settings.addNoise = m_optionsPage->noiseCheckBox->isChecked(); - settings.hostAddress = m_optionsPage->hostAddress->text(); - settings.remoteAddress = m_optionsPage->remoteAddress->text(); + settings.simulatorId = m_optionsPage->chooseFlightSimulator->itemData(i).toString(); + settings.binPath = m_optionsPage->executablePath->path(); + settings.dataPath = m_optionsPage->dataPath->path(); + settings.startSim = m_optionsPage->startSim->isChecked(); + settings.addNoise = m_optionsPage->noiseCheckBox->isChecked(); + settings.hostAddress = m_optionsPage->hostAddress->text(); + settings.remoteAddress = m_optionsPage->remoteAddress->text(); settings.inPort = m_optionsPage->inputPort->text().toInt(); - settings.outPort = m_optionsPage->outputPort->text().toInt(); - settings.longitude = m_optionsPage->longitude->text(); - settings.latitude = m_optionsPage->latitude->text(); + settings.outPort = m_optionsPage->outputPort->text().toInt(); + settings.longitude = m_optionsPage->longitude->text(); + settings.latitude = m_optionsPage->latitude->text(); - settings.addNoise = m_optionsPage->noiseCheckBox->isChecked(); + settings.addNoise = m_optionsPage->noiseCheckBox->isChecked(); - settings.attRawEnabled = m_optionsPage->attRawCheckbox->isChecked(); - settings.attRawRate = m_optionsPage->attRawRateSpinbox->value(); + 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(); + settings.gpsPositionEnabled = m_optionsPage->gpsPositionCheckbox->isChecked(); + settings.gpsPosRate = m_optionsPage->gpsPosRateSpinbox->value(); - settings.groundTruthEnabled = m_optionsPage->groundTruthCheckbox->isChecked(); - settings.groundTruthRate = m_optionsPage->groundTruthRateSpinbox->value(); + settings.groundTruthEnabled = m_optionsPage->groundTruthCheckbox->isChecked(); + settings.groundTruthRate = m_optionsPage->groundTruthRateSpinbox->value(); - settings.baroAltitudeEnabled = m_optionsPage->baroAltitudeCheckbox->isChecked(); - settings.baroAltRate = m_optionsPage->baroAltRateSpinbox->value(); + settings.baroSensorEnabled = m_optionsPage->baroAltitudeCheckbox->isChecked(); + settings.baroAltRate = m_optionsPage->baroAltRateSpinbox->value(); - settings.minOutputPeriod = m_optionsPage->minOutputPeriodSpinbox->value(); + settings.minOutputPeriod = m_optionsPage->minOutputPeriodSpinbox->value(); - settings.manualControlEnabled = m_optionsPage->manualControlRadioButton->isChecked(); - settings.gcsReceiverEnabled = m_optionsPage->gcsReceiverRadioButton->isChecked(); + settings.manualControlEnabled = m_optionsPage->manualControlRadioButton->isChecked(); + settings.gcsReceiverEnabled = m_optionsPage->gcsReceiverRadioButton->isChecked(); - settings.attActHW = m_optionsPage->attActHW->isChecked(); - settings.attActSim = m_optionsPage->attActSim->isChecked(); - settings.attActCalc = m_optionsPage->attActCalc->isChecked(); + settings.attActHW = m_optionsPage->attActHW->isChecked(); + 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); diff --git a/ground/openpilotgcs/src/plugins/hitl/hitloptionspage.ui b/ground/openpilotgcs/src/plugins/hitl/hitloptionspage.ui index 543feff6a..1c822fa45 100644 --- a/ground/openpilotgcs/src/plugins/hitl/hitloptionspage.ui +++ b/ground/openpilotgcs/src/plugins/hitl/hitloptionspage.ui @@ -510,12 +510,12 @@ - + true - AttitudeActual + AttitudeState true @@ -760,7 +760,7 @@ 0 - + 0 @@ -768,7 +768,7 @@ - AirspeedActual + AirspeedState true diff --git a/ground/openpilotgcs/src/plugins/hitl/il2simulator.cpp b/ground/openpilotgcs/src/plugins/hitl/il2simulator.cpp index c4936d33d..ffe1764d4 100644 --- a/ground/openpilotgcs/src/plugins/hitl/il2simulator.cpp +++ b/ground/openpilotgcs/src/plugins/hitl/il2simulator.cpp @@ -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; diff --git a/ground/openpilotgcs/src/plugins/hitl/isimulator.h b/ground/openpilotgcs/src/plugins/hitl/isimulator.h index 842e69a8f..12124a5bf 100644 --- a/ground/openpilotgcs/src/plugins/hitl/isimulator.h +++ b/ground/openpilotgcs/src/plugins/hitl/isimulator.h @@ -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; diff --git a/ground/openpilotgcs/src/plugins/hitl/simulator.cpp b/ground/openpilotgcs/src/plugins/hitl/simulator.cpp index f3d594d36..67e0de65b 100644 --- a/ground/openpilotgcs/src/plugins/hitl/simulator.cpp +++ b/ground/openpilotgcs/src/plugins/hitl/simulator.cpp @@ -64,13 +64,13 @@ Simulator::Simulator(const SimulatorSettings & params) : emit myStart(); QTime currentTime = QTime::currentTime(); - gpsPosTime = currentTime; - groundTruthTime = currentTime; - gcsRcvrTime = currentTime; - attRawTime = currentTime; - baroAltTime = currentTime; + gpsPosTime = currentTime; + groundTruthTime = currentTime; + gcsRcvrTime = currentTime; + attRawTime = currentTime; + baroAltTime = currentTime; battTime = currentTime; - airspeedActualTime = currentTime; + airspeedStateTime = currentTime; // Define standard atmospheric constants airParameters.univGasConstant = 8.31447; // [J/(mol·K)] @@ -146,20 +146,20 @@ void Simulator::onStart() manCtrlCommand = ManualControlCommand::GetInstance(objManager); gcsReceiver = GCSReceiver::GetInstance(objManager); flightStatus = FlightStatus::GetInstance(objManager); - posHome = HomeLocation::GetInstance(objManager); - velActual = VelocityActual::GetInstance(objManager); - posActual = PositionActual::GetInstance(objManager); - baroAlt = BaroAltitude::GetInstance(objManager); - flightBatt = FlightBatteryState::GetInstance(objManager); - airspeedActual = AirspeedActual::GetInstance(objManager); - attActual = AttitudeActual::GetInstance(objManager); - attSettings = AttitudeSettings::GetInstance(objManager); - accels = Accels::GetInstance(objManager); - gyros = Gyros::GetInstance(objManager); - gpsPos = GPSPosition::GetInstance(objManager); - gpsVel = GPSVelocity::GetInstance(objManager); - telStats = GCSTelemetryStats::GetInstance(objManager); - groundTruth = GroundTruth::GetInstance(objManager); + posHome = HomeLocation::GetInstance(objManager); + velState = VelocityState::GetInstance(objManager); + posState = PositionState::GetInstance(objManager); + baroAlt = BaroSensor::GetInstance(objManager); + flightBatt = FlightBatteryState::GetInstance(objManager); + airspeedState = AirspeedState::GetInstance(objManager); + attState = AttitudeState::GetInstance(objManager); + attSettings = AttitudeSettings::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); // Listen to autopilot connection events TelemetryManager *telMngr = pm->getObject(); @@ -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,22 +757,22 @@ 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); + attRawTime = attRawTime.addMSecs(settings.attRawRate); } } } diff --git a/ground/openpilotgcs/src/plugins/hitl/simulator.h b/ground/openpilotgcs/src/plugins/hitl/simulator.h index e9136a031..7d9ac4ad9 100644 --- a/ground/openpilotgcs/src/plugins/hitl/simulator.h +++ b/ground/openpilotgcs/src/plugins/hitl/simulator.h @@ -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; diff --git a/ground/openpilotgcs/src/plugins/hitl/xplanesimulator.cpp b/ground/openpilotgcs/src/plugins/hitl/xplanesimulator.cpp index f26dc3229..f450c25b1 100644 --- a/ground/openpilotgcs/src/plugins/hitl/xplanesimulator.cpp +++ b/ground/openpilotgcs/src/plugins/hitl/xplanesimulator.cpp @@ -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(); } diff --git a/ground/openpilotgcs/src/plugins/magicwaypoint/magicwaypointgadgetwidget.cpp b/ground/openpilotgcs/src/plugins/magicwaypoint/magicwaypointgadgetwidget.cpp index bd7d9e585..91a46e787 100644 --- a/ground/openpilotgcs/src/plugins/magicwaypoint/magicwaypointgadgetwidget.cpp +++ b/ground/openpilotgcs/src/plugins/magicwaypoint/magicwaypointgadgetwidget.cpp @@ -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(); - 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); } /** diff --git a/ground/openpilotgcs/src/plugins/magicwaypoint/magicwaypointgadgetwidget.h b/ground/openpilotgcs/src/plugins/magicwaypoint/magicwaypointgadgetwidget.h index 49e8f1251..17994072d 100644 --- a/ground/openpilotgcs/src/plugins/magicwaypoint/magicwaypointgadgetwidget.h +++ b/ground/openpilotgcs/src/plugins/magicwaypoint/magicwaypointgadgetwidget.h @@ -30,7 +30,7 @@ #include #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; }; diff --git a/ground/openpilotgcs/src/plugins/modelview/modelviewgadgetwidget.cpp b/ground/openpilotgcs/src/plugins/modelview/modelviewgadgetwidget.cpp index 11047c7b5..0c7a16a2c 100644 --- a/ground/openpilotgcs/src/plugins/modelview/modelviewgadgetwidget.cpp +++ b/ground/openpilotgcs/src/plugins/modelview/modelviewgadgetwidget.cpp @@ -57,7 +57,7 @@ ModelViewGadgetWidget::ModelViewGadgetWidget(QWidget *parent) // Get required UAVObjects ExtensionSystem::PluginManager *pm = ExtensionSystem::PluginManager::instance(); UAVObjectManager *objManager = pm->getObject(); - 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; diff --git a/ground/openpilotgcs/src/plugins/modelview/modelviewgadgetwidget.h b/ground/openpilotgcs/src/plugins/modelview/modelviewgadgetwidget.h index 353aeba7a..930b462e3 100644 --- a/ground/openpilotgcs/src/plugins/modelview/modelviewgadgetwidget.h +++ b/ground/openpilotgcs/src/plugins/modelview/modelviewgadgetwidget.h @@ -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_ */ diff --git a/ground/openpilotgcs/src/plugins/opmap/opmapgadgetwidget.cpp b/ground/openpilotgcs/src/plugins/opmap/opmapgadgetwidget.cpp index 75cd66c73..c950239fc 100644 --- a/ground/openpilotgcs/src/plugins/opmap/opmapgadgetwidget.cpp +++ b/ground/openpilotgcs/src/plugins/opmap/opmapgadgetwidget.cpp @@ -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(obm->getObject(QString("AttitudeActual"))); + UAVObject *obj = dynamic_cast(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]; diff --git a/ground/openpilotgcs/src/plugins/opmap/opmapgadgetwidget.h b/ground/openpilotgcs/src/plugins/opmap/opmapgadgetwidget.h index 7de84fce2..f5db4d5a0 100644 --- a/ground/openpilotgcs/src/plugins/opmap/opmapgadgetwidget.h +++ b/ground/openpilotgcs/src/plugins/opmap/opmapgadgetwidget.h @@ -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); diff --git a/ground/openpilotgcs/src/plugins/osgearthview/osgearthviewwidget.cpp b/ground/openpilotgcs/src/plugins/osgearthview/osgearthviewwidget.cpp index c96ab3f77..006579148 100644 --- a/ground/openpilotgcs/src/plugins/osgearthview/osgearthviewwidget.cpp +++ b/ground/openpilotgcs/src/plugins/osgearthview/osgearthviewwidget.cpp @@ -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; diff --git a/ground/openpilotgcs/src/plugins/osgearthview/osgviewerwidget.cpp b/ground/openpilotgcs/src/plugins/osgearthview/osgviewerwidget.cpp index 2b2d11b73..fce21f931 100644 --- a/ground/openpilotgcs/src/plugins/osgearthview/osgviewerwidget.cpp +++ b/ground/openpilotgcs/src/plugins/osgearthview/osgviewerwidget.cpp @@ -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(); - 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; diff --git a/ground/openpilotgcs/src/plugins/pfdqml/pfdqmlgadgetwidget.cpp b/ground/openpilotgcs/src/plugins/pfdqml/pfdqmlgadgetwidget.cpp index b28e1e418..aa6879b7e 100644 --- a/ground/openpilotgcs/src/plugins/pfdqml/pfdqmlgadgetwidget.cpp +++ b/ground/openpilotgcs/src/plugins/pfdqml/pfdqmlgadgetwidget.cpp @@ -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) { diff --git a/ground/openpilotgcs/src/plugins/qmlview/qmlviewgadgetwidget.cpp b/ground/openpilotgcs/src/plugins/qmlview/qmlviewgadgetwidget.cpp index 4bc172fee..9abbb4b23 100644 --- a/ground/openpilotgcs/src/plugins/qmlview/qmlviewgadgetwidget.cpp +++ b/ground/openpilotgcs/src/plugins/qmlview/qmlviewgadgetwidget.cpp @@ -49,10 +49,10 @@ QmlViewGadgetWidget::QmlViewGadgetWidget(QWidget *parent) : setResizeMode(SizeRootObjectToView); QStringList objectsToExport; - objectsToExport << "VelocityActual" << - "PositionActual" << - "AttitudeActual" << - "GPSPosition" << + objectsToExport << "VelocityState" << + "PositionState" << + "AttitudeState" << + "GPSPositionSensor" << "GCSTelemetryStats" << "FlightBatteryState"; diff --git a/ground/openpilotgcs/src/plugins/setupwizard/biascalibrationutil.cpp b/ground/openpilotgcs/src/plugins/setupwizard/biascalibrationutil.cpp index d7523abd0..ffbc3e09c 100644 --- a/ground/openpilotgcs/src/plugins/setupwizard/biascalibrationutil.cpp +++ b/ground/openpilotgcs/src/plugins/setupwizard/biascalibrationutil.cpp @@ -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(); 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(); 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); diff --git a/ground/openpilotgcs/src/plugins/uavobjects/OPPlots.m b/ground/openpilotgcs/src/plugins/uavobjects/OPPlots.m index b350458fd..9b0303a39 100644 --- a/ground/openpilotgcs/src/plugins/uavobjects/OPPlots.m +++ b/ground/openpilotgcs/src/plugins/uavobjects/OPPlots.m @@ -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)'); diff --git a/ground/openpilotgcs/src/plugins/uavobjects/uavobjects.pro b/ground/openpilotgcs/src/plugins/uavobjects/uavobjects.pro index 43a772af5..a4c5c51d7 100644 --- a/ground/openpilotgcs/src/plugins/uavobjects/uavobjects.pro +++ b/ground/openpilotgcs/src/plugins/uavobjects/uavobjects.pro @@ -24,11 +24,11 @@ OTHER_FILES += UAVObjects.pluginspec # Add in all of the synthetic/generated uavobject files HEADERS += $$UAVOBJECT_SYNTHETICS/accessorydesired.h \ - $$UAVOBJECT_SYNTHETICS/baroaltitude.h \ + $$UAVOBJECT_SYNTHETICS/barosensor.h \ $$UAVOBJECT_SYNTHETICS/airspeedsensor.h \ $$UAVOBJECT_SYNTHETICS/airspeedsettings.h \ - $$UAVOBJECT_SYNTHETICS/airspeedactual.h \ - $$UAVOBJECT_SYNTHETICS/attitudeactual.h \ + $$UAVOBJECT_SYNTHETICS/airspeedstate.h \ + $$UAVOBJECT_SYNTHETICS/attitudestate.h \ $$UAVOBJECT_SYNTHETICS/attitudesimulated.h \ $$UAVOBJECT_SYNTHETICS/altholdsmoothed.h \ $$UAVOBJECT_SYNTHETICS/altitudeholddesired.h \ @@ -38,11 +38,12 @@ HEADERS += $$UAVOBJECT_SYNTHETICS/accessorydesired.h \ $$UAVOBJECT_SYNTHETICS/revocalibration.h \ $$UAVOBJECT_SYNTHETICS/revosettings.h \ $$UAVOBJECT_SYNTHETICS/gcstelemetrystats.h \ - $$UAVOBJECT_SYNTHETICS/gyros.h \ - $$UAVOBJECT_SYNTHETICS/gyrosbias.h \ - $$UAVOBJECT_SYNTHETICS/accels.h \ - $$UAVOBJECT_SYNTHETICS/magnetometer.h \ - $$UAVOBJECT_SYNTHETICS/magbias.h \ + $$UAVOBJECT_SYNTHETICS/gyrostate.h \ + $$UAVOBJECT_SYNTHETICS/gyrosensor.h \ + $$UAVOBJECT_SYNTHETICS/accelsensor.h \ + $$UAVOBJECT_SYNTHETICS/accelstate.h \ + $$UAVOBJECT_SYNTHETICS/magsensor.h \ + $$UAVOBJECT_SYNTHETICS/magstate.h \ $$UAVOBJECT_SYNTHETICS/camerastabsettings.h \ $$UAVOBJECT_SYNTHETICS/flighttelemetrystats.h \ $$UAVOBJECT_SYNTHETICS/systemstats.h \ @@ -58,20 +59,21 @@ HEADERS += $$UAVOBJECT_SYNTHETICS/accessorydesired.h \ $$UAVOBJECT_SYNTHETICS/actuatorsettings.h \ $$UAVOBJECT_SYNTHETICS/actuatordesired.h \ $$UAVOBJECT_SYNTHETICS/actuatorcommand.h \ - $$UAVOBJECT_SYNTHETICS/gpsposition.h \ + $$UAVOBJECT_SYNTHETICS/gpspositionsensor.h \ $$UAVOBJECT_SYNTHETICS/gpstime.h \ $$UAVOBJECT_SYNTHETICS/gpssatellites.h \ $$UAVOBJECT_SYNTHETICS/pathaction.h \ $$UAVOBJECT_SYNTHETICS/pathdesired.h \ $$UAVOBJECT_SYNTHETICS/pathstatus.h \ - $$UAVOBJECT_SYNTHETICS/gpsvelocity.h \ - $$UAVOBJECT_SYNTHETICS/positionactual.h \ + $$UAVOBJECT_SYNTHETICS/gpsvelocitysensor.h \ + $$UAVOBJECT_SYNTHETICS/positionsensor.h \ + $$UAVOBJECT_SYNTHETICS/positionstate.h \ $$UAVOBJECT_SYNTHETICS/flightbatterystate.h \ $$UAVOBJECT_SYNTHETICS/homelocation.h \ $$UAVOBJECT_SYNTHETICS/mixersettings.h \ $$UAVOBJECT_SYNTHETICS/mixerstatus.h \ $$UAVOBJECT_SYNTHETICS/velocitydesired.h \ - $$UAVOBJECT_SYNTHETICS/velocityactual.h \ + $$UAVOBJECT_SYNTHETICS/velocitystate.h \ $$UAVOBJECT_SYNTHETICS/groundtruth.h \ $$UAVOBJECT_SYNTHETICS/fixedwingpathfollowersettings.h \ $$UAVOBJECT_SYNTHETICS/fixedwingpathfollowerstatus.h \ @@ -107,11 +109,11 @@ HEADERS += $$UAVOBJECT_SYNTHETICS/accessorydesired.h \ $$UAVOBJECT_SYNTHETICS/mpu6000settings.h SOURCES += $$UAVOBJECT_SYNTHETICS/accessorydesired.cpp \ - $$UAVOBJECT_SYNTHETICS/baroaltitude.cpp \ + $$UAVOBJECT_SYNTHETICS/barosensor.cpp \ $$UAVOBJECT_SYNTHETICS/airspeedsensor.cpp \ $$UAVOBJECT_SYNTHETICS/airspeedsettings.cpp \ - $$UAVOBJECT_SYNTHETICS/airspeedactual.cpp \ - $$UAVOBJECT_SYNTHETICS/attitudeactual.cpp \ + $$UAVOBJECT_SYNTHETICS/airspeedstate.cpp \ + $$UAVOBJECT_SYNTHETICS/attitudestate.cpp \ $$UAVOBJECT_SYNTHETICS/attitudesimulated.cpp \ $$UAVOBJECT_SYNTHETICS/altholdsmoothed.cpp \ $$UAVOBJECT_SYNTHETICS/altitudeholddesired.cpp \ @@ -121,11 +123,12 @@ SOURCES += $$UAVOBJECT_SYNTHETICS/accessorydesired.cpp \ $$UAVOBJECT_SYNTHETICS/revocalibration.cpp \ $$UAVOBJECT_SYNTHETICS/revosettings.cpp \ $$UAVOBJECT_SYNTHETICS/gcstelemetrystats.cpp \ - $$UAVOBJECT_SYNTHETICS/accels.cpp \ - $$UAVOBJECT_SYNTHETICS/gyros.cpp \ - $$UAVOBJECT_SYNTHETICS/gyrosbias.cpp \ - $$UAVOBJECT_SYNTHETICS/magnetometer.cpp \ - $$UAVOBJECT_SYNTHETICS/magbias.cpp \ + $$UAVOBJECT_SYNTHETICS/accelsensor.cpp \ + $$UAVOBJECT_SYNTHETICS/accelstate.cpp \ + $$UAVOBJECT_SYNTHETICS/gyrostate.cpp \ + $$UAVOBJECT_SYNTHETICS/gyrosensor.cpp \ + $$UAVOBJECT_SYNTHETICS/magsensor.cpp \ + $$UAVOBJECT_SYNTHETICS/magstate.cpp \ $$UAVOBJECT_SYNTHETICS/camerastabsettings.cpp \ $$UAVOBJECT_SYNTHETICS/flighttelemetrystats.cpp \ $$UAVOBJECT_SYNTHETICS/systemstats.cpp \ @@ -141,20 +144,21 @@ SOURCES += $$UAVOBJECT_SYNTHETICS/accessorydesired.cpp \ $$UAVOBJECT_SYNTHETICS/actuatorsettings.cpp \ $$UAVOBJECT_SYNTHETICS/actuatordesired.cpp \ $$UAVOBJECT_SYNTHETICS/actuatorcommand.cpp \ - $$UAVOBJECT_SYNTHETICS/gpsposition.cpp \ + $$UAVOBJECT_SYNTHETICS/gpspositionsensor.cpp \ $$UAVOBJECT_SYNTHETICS/gpstime.cpp \ $$UAVOBJECT_SYNTHETICS/gpssatellites.cpp \ $$UAVOBJECT_SYNTHETICS/pathaction.cpp \ $$UAVOBJECT_SYNTHETICS/pathdesired.cpp \ $$UAVOBJECT_SYNTHETICS/pathstatus.cpp \ - $$UAVOBJECT_SYNTHETICS/gpsvelocity.cpp \ - $$UAVOBJECT_SYNTHETICS/positionactual.cpp \ + $$UAVOBJECT_SYNTHETICS/gpsvelocitysensor.cpp \ + $$UAVOBJECT_SYNTHETICS/positionsensor.cpp \ + $$UAVOBJECT_SYNTHETICS/positionstate.cpp \ $$UAVOBJECT_SYNTHETICS/flightbatterystate.cpp \ $$UAVOBJECT_SYNTHETICS/homelocation.cpp \ $$UAVOBJECT_SYNTHETICS/mixersettings.cpp \ $$UAVOBJECT_SYNTHETICS/mixerstatus.cpp \ $$UAVOBJECT_SYNTHETICS/velocitydesired.cpp \ - $$UAVOBJECT_SYNTHETICS/velocityactual.cpp \ + $$UAVOBJECT_SYNTHETICS/velocitystate.cpp \ $$UAVOBJECT_SYNTHETICS/groundtruth.cpp \ $$UAVOBJECT_SYNTHETICS/fixedwingpathfollowersettings.cpp \ $$UAVOBJECT_SYNTHETICS/fixedwingpathfollowerstatus.cpp \ diff --git a/ground/openpilotgcs/src/plugins/uavobjectutil/uavobjectutilmanager.cpp b/ground/openpilotgcs/src/plugins/uavobjectutil/uavobjectutilmanager.cpp index 34133de24..a8acfc752 100644 --- a/ground/openpilotgcs/src/plugins/uavobjectutil/uavobjectutilmanager.cpp +++ b/ground/openpilotgcs/src/plugins/uavobjectutil/uavobjectutilmanager.cpp @@ -38,7 +38,7 @@ #include "firmwareiapobj.h" #include "homelocation.h" -#include "gpsposition.h" +#include "gpspositionsensor.h" // ****************************** // constructor/destructor @@ -377,13 +377,13 @@ int UAVObjectUtilManager::getHomeLocation(bool &set, double LLA[3]) // ****************************** // GPS -int UAVObjectUtilManager::getGPSPosition(double LLA[3]) +int UAVObjectUtilManager::getGPSPositionSensor(double LLA[3]) { - GPSPosition *gpsPosition = GPSPosition::GetInstance(obm); + GPSPositionSensor *gpsPosition = GPSPositionSensor::GetInstance(obm); Q_ASSERT(gpsPosition != NULL); - GPSPosition::DataFields gpsPositionData = gpsPosition->getData(); + GPSPositionSensor::DataFields gpsPositionData = gpsPosition->getData(); LLA[0] = gpsPositionData.Latitude; LLA[1] = gpsPositionData.Longitude; diff --git a/ground/openpilotgcs/src/plugins/uavobjectutil/uavobjectutilmanager.h b/ground/openpilotgcs/src/plugins/uavobjectutil/uavobjectutilmanager.h index 68260693c..7ad27b8d1 100644 --- a/ground/openpilotgcs/src/plugins/uavobjectutil/uavobjectutilmanager.h +++ b/ground/openpilotgcs/src/plugins/uavobjectutil/uavobjectutilmanager.h @@ -55,7 +55,7 @@ public: int setHomeLocation(double LLA[3], bool save_to_sdcard); int getHomeLocation(bool &set, double LLA[3]); - int getGPSPosition(double LLA[3]); + int getGPSPositionSensor(double LLA[3]); int getBoardModel(); QByteArray getBoardCPUSerial(); diff --git a/shared/uavobjectdefinition/accels.xml b/shared/uavobjectdefinition/accelsensor.xml similarity index 72% rename from shared/uavobjectdefinition/accels.xml rename to shared/uavobjectdefinition/accelsensor.xml index 3a5e34b84..66b97a3f4 100644 --- a/shared/uavobjectdefinition/accels.xml +++ b/shared/uavobjectdefinition/accelsensor.xml @@ -1,13 +1,13 @@ - - The accel data. + + Calibrated sensor data from 3 axis accelerometer in m/s². - + diff --git a/shared/uavobjectdefinition/accelstate.xml b/shared/uavobjectdefinition/accelstate.xml new file mode 100644 index 000000000..19060ebe4 --- /dev/null +++ b/shared/uavobjectdefinition/accelstate.xml @@ -0,0 +1,12 @@ + + + The filtered acceleration data. + + + + + + + + + diff --git a/shared/uavobjectdefinition/accessorydesired.xml b/shared/uavobjectdefinition/accessorydesired.xml index dbeb583d9..bce35a7d7 100644 --- a/shared/uavobjectdefinition/accessorydesired.xml +++ b/shared/uavobjectdefinition/accessorydesired.xml @@ -1,5 +1,5 @@ - + Desired Auxillary actuator settings. Comes from @ref ManualControlModule. diff --git a/shared/uavobjectdefinition/actuatorcommand.xml b/shared/uavobjectdefinition/actuatorcommand.xml index 2ee3a29b2..4bd41a009 100644 --- a/shared/uavobjectdefinition/actuatorcommand.xml +++ b/shared/uavobjectdefinition/actuatorcommand.xml @@ -1,5 +1,5 @@ - + Contains the pulse duration sent to each of the channels. Set by @ref ActuatorModule diff --git a/shared/uavobjectdefinition/actuatordesired.xml b/shared/uavobjectdefinition/actuatordesired.xml index 4c4bbba8b..371d025b1 100644 --- a/shared/uavobjectdefinition/actuatordesired.xml +++ b/shared/uavobjectdefinition/actuatordesired.xml @@ -1,5 +1,5 @@ - + Desired raw, pitch and yaw actuator settings. Comes from either @ref StabilizationModule or @ref ManualControlModule depending on FlightMode. diff --git a/shared/uavobjectdefinition/actuatorsettings.xml b/shared/uavobjectdefinition/actuatorsettings.xml index 98681555e..c32be9c6c 100644 --- a/shared/uavobjectdefinition/actuatorsettings.xml +++ b/shared/uavobjectdefinition/actuatorsettings.xml @@ -1,5 +1,5 @@ - + Settings for the @ref ActuatorModule that controls the channel assignments for the mixer based on AircraftType diff --git a/shared/uavobjectdefinition/airspeedsensor.xml b/shared/uavobjectdefinition/airspeedsensor.xml index 0ac1f3ecd..8f55cbeae 100644 --- a/shared/uavobjectdefinition/airspeedsensor.xml +++ b/shared/uavobjectdefinition/airspeedsensor.xml @@ -1,5 +1,5 @@ - + The raw data from the dynamic pressure sensor with pressure, temperature and airspeed. diff --git a/shared/uavobjectdefinition/airspeedsettings.xml b/shared/uavobjectdefinition/airspeedsettings.xml index 697c9a3ec..a208ff692 100644 --- a/shared/uavobjectdefinition/airspeedsettings.xml +++ b/shared/uavobjectdefinition/airspeedsettings.xml @@ -1,5 +1,5 @@ - + Settings for the @ref BaroAirspeed module used on CopterControl or Revolution diff --git a/shared/uavobjectdefinition/airspeedactual.xml b/shared/uavobjectdefinition/airspeedstate.xml similarity index 69% rename from shared/uavobjectdefinition/airspeedactual.xml rename to shared/uavobjectdefinition/airspeedstate.xml index d5b0f90ae..3a7db51d9 100644 --- a/shared/uavobjectdefinition/airspeedactual.xml +++ b/shared/uavobjectdefinition/airspeedstate.xml @@ -1,6 +1,6 @@ - - UAVO for true airspeed, calibrated airspeed, angle of attack, and angle of slip + + UAVO for true airspeed and calibrated airspeed state estimation. diff --git a/shared/uavobjectdefinition/altholdsmoothed.xml b/shared/uavobjectdefinition/altholdsmoothed.xml index ef1b5f144..b32e977c2 100644 --- a/shared/uavobjectdefinition/altholdsmoothed.xml +++ b/shared/uavobjectdefinition/altholdsmoothed.xml @@ -1,5 +1,5 @@ - + The output on the kalman filter on altitude. diff --git a/shared/uavobjectdefinition/altitudeholddesired.xml b/shared/uavobjectdefinition/altitudeholddesired.xml index 8cb004011..cf056cfa0 100644 --- a/shared/uavobjectdefinition/altitudeholddesired.xml +++ b/shared/uavobjectdefinition/altitudeholddesired.xml @@ -1,5 +1,5 @@ - + Holds the desired altitude (from manual control) as well as the desired attitude to pass through diff --git a/shared/uavobjectdefinition/altitudeholdsettings.xml b/shared/uavobjectdefinition/altitudeholdsettings.xml index 61f36d37f..3a2cf6149 100644 --- a/shared/uavobjectdefinition/altitudeholdsettings.xml +++ b/shared/uavobjectdefinition/altitudeholdsettings.xml @@ -1,5 +1,5 @@ - + Settings for the @ref AltitudeHold module diff --git a/shared/uavobjectdefinition/attitudesettings.xml b/shared/uavobjectdefinition/attitudesettings.xml index 8c86e2af3..34f281636 100644 --- a/shared/uavobjectdefinition/attitudesettings.xml +++ b/shared/uavobjectdefinition/attitudesettings.xml @@ -1,5 +1,5 @@ - + Settings for the @ref Attitude module used on CopterControl diff --git a/shared/uavobjectdefinition/attitudesimulated.xml b/shared/uavobjectdefinition/attitudesimulated.xml index be688b877..47c4257cc 100644 --- a/shared/uavobjectdefinition/attitudesimulated.xml +++ b/shared/uavobjectdefinition/attitudesimulated.xml @@ -1,5 +1,5 @@ - + The simulated Attitude estimation from @ref Sensors. diff --git a/shared/uavobjectdefinition/attitudeactual.xml b/shared/uavobjectdefinition/attitudestate.xml similarity index 79% rename from shared/uavobjectdefinition/attitudeactual.xml rename to shared/uavobjectdefinition/attitudestate.xml index b9f143b35..98ceaa3c7 100644 --- a/shared/uavobjectdefinition/attitudeactual.xml +++ b/shared/uavobjectdefinition/attitudestate.xml @@ -1,6 +1,6 @@ - - The updated Attitude estimation from @ref AHRSCommsModule. + + The updated Attitude estimation from @ref StateEstimationModule. diff --git a/shared/uavobjectdefinition/baroaltitude.xml b/shared/uavobjectdefinition/barosensor.xml similarity index 87% rename from shared/uavobjectdefinition/baroaltitude.xml rename to shared/uavobjectdefinition/barosensor.xml index 8335b56dd..0730f3078 100644 --- a/shared/uavobjectdefinition/baroaltitude.xml +++ b/shared/uavobjectdefinition/barosensor.xml @@ -1,5 +1,5 @@ - + The raw data from the barometric sensor with pressure, temperature and altitude estimate. diff --git a/shared/uavobjectdefinition/cameradesired.xml b/shared/uavobjectdefinition/cameradesired.xml index c842edf2a..fe98c940d 100644 --- a/shared/uavobjectdefinition/cameradesired.xml +++ b/shared/uavobjectdefinition/cameradesired.xml @@ -1,5 +1,5 @@ - + Desired camera outputs. Comes from @ref CameraStabilization module. diff --git a/shared/uavobjectdefinition/camerastabsettings.xml b/shared/uavobjectdefinition/camerastabsettings.xml index 0a3626183..0a017dff8 100644 --- a/shared/uavobjectdefinition/camerastabsettings.xml +++ b/shared/uavobjectdefinition/camerastabsettings.xml @@ -1,5 +1,5 @@ - + Settings for the @ref CameraStab mmodule diff --git a/shared/uavobjectdefinition/ekfconfiguration.xml b/shared/uavobjectdefinition/ekfconfiguration.xml index 73fafdfdc..4f0afeac5 100644 --- a/shared/uavobjectdefinition/ekfconfiguration.xml +++ b/shared/uavobjectdefinition/ekfconfiguration.xml @@ -1,9 +1,9 @@ - + Extended Kalman Filter initialisation @@ -23,9 +23,9 @@ + 0.0000001, 0.0000001, 0.0000001"> GyroX GyroY @@ -39,10 +39,10 @@ + 10, 10, 1000, + 1, 1, 1, + 1000, 1000, 1000, + 1"> GPSPosNorth GPSPosEast @@ -57,9 +57,9 @@ + 10, + 1, + 1000"> FakeGPSPosIndoor FakeGPSVelIndoor diff --git a/shared/uavobjectdefinition/ekfstatevariance.xml b/shared/uavobjectdefinition/ekfstatevariance.xml index 05b684dbd..cd9cc412a 100644 --- a/shared/uavobjectdefinition/ekfstatevariance.xml +++ b/shared/uavobjectdefinition/ekfstatevariance.xml @@ -1,5 +1,5 @@ - + Extended Kalman Filter state covariance diff --git a/shared/uavobjectdefinition/faultsettings.xml b/shared/uavobjectdefinition/faultsettings.xml index 6750c4f99..43c2a854f 100644 --- a/shared/uavobjectdefinition/faultsettings.xml +++ b/shared/uavobjectdefinition/faultsettings.xml @@ -1,5 +1,5 @@ - + Allows testers to simulate various fault scenarios. diff --git a/shared/uavobjectdefinition/firmwareiapobj.xml b/shared/uavobjectdefinition/firmwareiapobj.xml index 4703ae448..f83c8aa67 100644 --- a/shared/uavobjectdefinition/firmwareiapobj.xml +++ b/shared/uavobjectdefinition/firmwareiapobj.xml @@ -1,5 +1,5 @@ - + Queries board for SN, model, revision, and sends reset command diff --git a/shared/uavobjectdefinition/fixedwingpathfollowersettings.xml b/shared/uavobjectdefinition/fixedwingpathfollowersettings.xml index ac987e866..f792fd7dc 100644 --- a/shared/uavobjectdefinition/fixedwingpathfollowersettings.xml +++ b/shared/uavobjectdefinition/fixedwingpathfollowersettings.xml @@ -1,5 +1,5 @@ - + Settings for the @ref FixedWingPathFollowerModule diff --git a/shared/uavobjectdefinition/fixedwingpathfollowerstatus.xml b/shared/uavobjectdefinition/fixedwingpathfollowerstatus.xml index a8425dbd0..7bdca082d 100644 --- a/shared/uavobjectdefinition/fixedwingpathfollowerstatus.xml +++ b/shared/uavobjectdefinition/fixedwingpathfollowerstatus.xml @@ -1,5 +1,5 @@ - + Object Storing Debugging Information on PID internals diff --git a/shared/uavobjectdefinition/flightbatterysettings.xml b/shared/uavobjectdefinition/flightbatterysettings.xml index 4fe5ba025..38d8ee646 100644 --- a/shared/uavobjectdefinition/flightbatterysettings.xml +++ b/shared/uavobjectdefinition/flightbatterysettings.xml @@ -1,5 +1,5 @@ - + Flight Battery configuration. diff --git a/shared/uavobjectdefinition/flightbatterystate.xml b/shared/uavobjectdefinition/flightbatterystate.xml index be20caec9..1f7dc19ea 100644 --- a/shared/uavobjectdefinition/flightbatterystate.xml +++ b/shared/uavobjectdefinition/flightbatterystate.xml @@ -1,5 +1,5 @@ - + Battery status information. diff --git a/shared/uavobjectdefinition/flightplancontrol.xml b/shared/uavobjectdefinition/flightplancontrol.xml index 62144edc8..586794af8 100644 --- a/shared/uavobjectdefinition/flightplancontrol.xml +++ b/shared/uavobjectdefinition/flightplancontrol.xml @@ -1,5 +1,5 @@ - + Control the flight plan script diff --git a/shared/uavobjectdefinition/flightplansettings.xml b/shared/uavobjectdefinition/flightplansettings.xml index 98fad1f2e..6357aad75 100644 --- a/shared/uavobjectdefinition/flightplansettings.xml +++ b/shared/uavobjectdefinition/flightplansettings.xml @@ -1,5 +1,5 @@ - + Settings for the flight plan module, control the execution of the script diff --git a/shared/uavobjectdefinition/flightplanstatus.xml b/shared/uavobjectdefinition/flightplanstatus.xml index c5d14ca6a..e5b31a6e6 100644 --- a/shared/uavobjectdefinition/flightplanstatus.xml +++ b/shared/uavobjectdefinition/flightplanstatus.xml @@ -1,5 +1,5 @@ - + Status of the flight plan script diff --git a/shared/uavobjectdefinition/flightstatus.xml b/shared/uavobjectdefinition/flightstatus.xml index 24befac63..9c9336c1a 100644 --- a/shared/uavobjectdefinition/flightstatus.xml +++ b/shared/uavobjectdefinition/flightstatus.xml @@ -1,5 +1,5 @@ - + Contains major flight status information for other modules. diff --git a/shared/uavobjectdefinition/flighttelemetrystats.xml b/shared/uavobjectdefinition/flighttelemetrystats.xml index 57a19850c..352bb7de0 100644 --- a/shared/uavobjectdefinition/flighttelemetrystats.xml +++ b/shared/uavobjectdefinition/flighttelemetrystats.xml @@ -1,5 +1,5 @@ - + Maintains the telemetry statistics from the OpenPilot flight computer. diff --git a/shared/uavobjectdefinition/gcsreceiver.xml b/shared/uavobjectdefinition/gcsreceiver.xml index ee4fa696f..5e76d1928 100644 --- a/shared/uavobjectdefinition/gcsreceiver.xml +++ b/shared/uavobjectdefinition/gcsreceiver.xml @@ -1,5 +1,5 @@ - + A receiver channel group carried over the telemetry link. diff --git a/shared/uavobjectdefinition/gcstelemetrystats.xml b/shared/uavobjectdefinition/gcstelemetrystats.xml index 83ce10cc1..9cdf9985d 100644 --- a/shared/uavobjectdefinition/gcstelemetrystats.xml +++ b/shared/uavobjectdefinition/gcstelemetrystats.xml @@ -1,5 +1,5 @@ - + The telemetry statistics from the ground computer diff --git a/shared/uavobjectdefinition/gpsposition.xml b/shared/uavobjectdefinition/gpspositionsensor.xml similarity index 85% rename from shared/uavobjectdefinition/gpsposition.xml rename to shared/uavobjectdefinition/gpspositionsensor.xml index 7a8ae82db..8686a76ef 100644 --- a/shared/uavobjectdefinition/gpsposition.xml +++ b/shared/uavobjectdefinition/gpspositionsensor.xml @@ -1,6 +1,6 @@ - - Raw GPS data from @ref GPSModule. Should only be used by @ref AHRSCommsModule. + + Raw GPS data from @ref GPSModule. diff --git a/shared/uavobjectdefinition/gpssatellites.xml b/shared/uavobjectdefinition/gpssatellites.xml index d23cddb4b..47c4c0f56 100644 --- a/shared/uavobjectdefinition/gpssatellites.xml +++ b/shared/uavobjectdefinition/gpssatellites.xml @@ -1,5 +1,5 @@ - + Contains information about the GPS satellites in view from @ref GPSModule. diff --git a/shared/uavobjectdefinition/gpstime.xml b/shared/uavobjectdefinition/gpstime.xml index c397a5108..ab93d4e9c 100644 --- a/shared/uavobjectdefinition/gpstime.xml +++ b/shared/uavobjectdefinition/gpstime.xml @@ -1,5 +1,5 @@ - + Contains the GPS time from @ref GPSModule. Required to compute the world magnetic model correctly when setting the home location. diff --git a/shared/uavobjectdefinition/gpsvelocity.xml b/shared/uavobjectdefinition/gpsvelocitysensor.xml similarity index 71% rename from shared/uavobjectdefinition/gpsvelocity.xml rename to shared/uavobjectdefinition/gpsvelocitysensor.xml index 8673463a8..c94474ca2 100644 --- a/shared/uavobjectdefinition/gpsvelocity.xml +++ b/shared/uavobjectdefinition/gpsvelocitysensor.xml @@ -1,6 +1,6 @@ - - Raw GPS data from @ref GPSModule. + + Raw GPS velocity in NED frame and m/s from @ref GPSModule. diff --git a/shared/uavobjectdefinition/groundtruth.xml b/shared/uavobjectdefinition/groundtruth.xml index c852754ee..923078847 100644 --- a/shared/uavobjectdefinition/groundtruth.xml +++ b/shared/uavobjectdefinition/groundtruth.xml @@ -1,5 +1,5 @@ - + Ground truth data output by a simulator. diff --git a/shared/uavobjectdefinition/gyros.xml b/shared/uavobjectdefinition/gyrosensor.xml similarity index 74% rename from shared/uavobjectdefinition/gyros.xml rename to shared/uavobjectdefinition/gyrosensor.xml index 8240d2a68..3107f952e 100644 --- a/shared/uavobjectdefinition/gyros.xml +++ b/shared/uavobjectdefinition/gyrosensor.xml @@ -1,6 +1,6 @@ - - The gyro data. + + Calibrated sensor data from 3 axis gyroscope in deg/s. diff --git a/shared/uavobjectdefinition/gyrosbias.xml b/shared/uavobjectdefinition/gyrostate.xml similarity index 74% rename from shared/uavobjectdefinition/gyrosbias.xml rename to shared/uavobjectdefinition/gyrostate.xml index b8543e641..a4d497578 100644 --- a/shared/uavobjectdefinition/gyrosbias.xml +++ b/shared/uavobjectdefinition/gyrostate.xml @@ -1,6 +1,6 @@ - - The gyro data. + + The filtered rotation sensor data. diff --git a/shared/uavobjectdefinition/homelocation.xml b/shared/uavobjectdefinition/homelocation.xml index a0503517f..3f99eb24b 100644 --- a/shared/uavobjectdefinition/homelocation.xml +++ b/shared/uavobjectdefinition/homelocation.xml @@ -1,5 +1,5 @@ - + HomeLocation setting which contains the constants to tranlate from longitutde and latitude to NED reference frame. Automatically set by @ref GPSModule after acquiring a 3D lock. Used by @ref AHRSCommsModule. diff --git a/shared/uavobjectdefinition/hwsettings.xml b/shared/uavobjectdefinition/hwsettings.xml index 5d0a29fec..8149a7d62 100644 --- a/shared/uavobjectdefinition/hwsettings.xml +++ b/shared/uavobjectdefinition/hwsettings.xml @@ -1,5 +1,5 @@ - + Selection of optional hardware configurations. diff --git a/shared/uavobjectdefinition/i2cstats.xml b/shared/uavobjectdefinition/i2cstats.xml index 40663eba6..3e79d79ea 100644 --- a/shared/uavobjectdefinition/i2cstats.xml +++ b/shared/uavobjectdefinition/i2cstats.xml @@ -1,5 +1,5 @@ - + Tracks statistics on the I2C bus. diff --git a/shared/uavobjectdefinition/magbias.xml b/shared/uavobjectdefinition/magbias.xml deleted file mode 100644 index 63319466f..000000000 --- a/shared/uavobjectdefinition/magbias.xml +++ /dev/null @@ -1,12 +0,0 @@ - - - The gyro data. - - - - - - - - - diff --git a/shared/uavobjectdefinition/magsensor.xml b/shared/uavobjectdefinition/magsensor.xml new file mode 100644 index 000000000..8622bddd9 --- /dev/null +++ b/shared/uavobjectdefinition/magsensor.xml @@ -0,0 +1,12 @@ + + + Calibrated sensor data from 3 axis magnetometer in MilliGauss. + + + + + + + + + diff --git a/shared/uavobjectdefinition/magnetometer.xml b/shared/uavobjectdefinition/magstate.xml similarity index 75% rename from shared/uavobjectdefinition/magnetometer.xml rename to shared/uavobjectdefinition/magstate.xml index 006c40298..1cd0fefaa 100644 --- a/shared/uavobjectdefinition/magnetometer.xml +++ b/shared/uavobjectdefinition/magstate.xml @@ -1,6 +1,6 @@ - - The mag data. + + The filtered magnet vector. diff --git a/shared/uavobjectdefinition/manualcontrolcommand.xml b/shared/uavobjectdefinition/manualcontrolcommand.xml index 2a7eb9c78..d8cc27017 100644 --- a/shared/uavobjectdefinition/manualcontrolcommand.xml +++ b/shared/uavobjectdefinition/manualcontrolcommand.xml @@ -1,5 +1,5 @@ - + The output from the @ref ManualControlModule which descodes the receiver inputs. Overriden by GCS for fly-by-wire control. diff --git a/shared/uavobjectdefinition/manualcontrolsettings.xml b/shared/uavobjectdefinition/manualcontrolsettings.xml index ebf5aa7f6..52a45ef0f 100644 --- a/shared/uavobjectdefinition/manualcontrolsettings.xml +++ b/shared/uavobjectdefinition/manualcontrolsettings.xml @@ -1,5 +1,5 @@ - + Settings to indicate how to decode receiver input by @ref ManualControlModule. - + Settings for the @ref ActuatorModule that controls the channel assignments for the mixer based on AircraftType diff --git a/shared/uavobjectdefinition/mixerstatus.xml b/shared/uavobjectdefinition/mixerstatus.xml index 314af78b1..c7491a92e 100644 --- a/shared/uavobjectdefinition/mixerstatus.xml +++ b/shared/uavobjectdefinition/mixerstatus.xml @@ -1,5 +1,5 @@ - + Status for the matrix mixer showing the output of each mixer after all scaling diff --git a/shared/uavobjectdefinition/nedaccel.xml b/shared/uavobjectdefinition/nedaccel.xml index 274fde9e2..ce1636530 100644 --- a/shared/uavobjectdefinition/nedaccel.xml +++ b/shared/uavobjectdefinition/nedaccel.xml @@ -1,5 +1,5 @@ - + The projection of acceleration in the NED reference frame used by @ref Guidance. diff --git a/shared/uavobjectdefinition/objectpersistence.xml b/shared/uavobjectdefinition/objectpersistence.xml index ba21631e3..9b29721df 100644 --- a/shared/uavobjectdefinition/objectpersistence.xml +++ b/shared/uavobjectdefinition/objectpersistence.xml @@ -1,5 +1,5 @@ - + Someone who knows please enter this diff --git a/shared/uavobjectdefinition/oplinksettings.xml b/shared/uavobjectdefinition/oplinksettings.xml index b78b8d4c9..4822b961e 100644 --- a/shared/uavobjectdefinition/oplinksettings.xml +++ b/shared/uavobjectdefinition/oplinksettings.xml @@ -1,5 +1,5 @@ - + OPLink configurations options. diff --git a/shared/uavobjectdefinition/oplinkstatus.xml b/shared/uavobjectdefinition/oplinkstatus.xml index 1e79630fe..9280ef818 100644 --- a/shared/uavobjectdefinition/oplinkstatus.xml +++ b/shared/uavobjectdefinition/oplinkstatus.xml @@ -1,5 +1,5 @@ - + OPLink device status. diff --git a/shared/uavobjectdefinition/osdsettings.xml b/shared/uavobjectdefinition/osdsettings.xml index c3ceef706..aa82c7e85 100644 --- a/shared/uavobjectdefinition/osdsettings.xml +++ b/shared/uavobjectdefinition/osdsettings.xml @@ -1,5 +1,5 @@ - + OSD settings used by the OSDgen module diff --git a/shared/uavobjectdefinition/overosyncsettings.xml b/shared/uavobjectdefinition/overosyncsettings.xml index 2c16b5ad3..2d884bc1a 100644 --- a/shared/uavobjectdefinition/overosyncsettings.xml +++ b/shared/uavobjectdefinition/overosyncsettings.xml @@ -1,5 +1,5 @@ - + Settings to control the behavior of the overo sync module diff --git a/shared/uavobjectdefinition/overosyncstats.xml b/shared/uavobjectdefinition/overosyncstats.xml index 7c120f18d..de069e38c 100644 --- a/shared/uavobjectdefinition/overosyncstats.xml +++ b/shared/uavobjectdefinition/overosyncstats.xml @@ -1,5 +1,5 @@ - + Maintains statistics on transfer rate to and from over diff --git a/shared/uavobjectdefinition/pathaction.xml b/shared/uavobjectdefinition/pathaction.xml index 70936f58d..88f6af2da 100644 --- a/shared/uavobjectdefinition/pathaction.xml +++ b/shared/uavobjectdefinition/pathaction.xml @@ -1,5 +1,5 @@ - + A waypoint command the pathplanner is to use at a certain waypoint + The endpoint or path the craft is trying to achieve. Can come from @ref ManualControl or @ref PathPlanner diff --git a/shared/uavobjectdefinition/pathstatus.xml b/shared/uavobjectdefinition/pathstatus.xml index 814e62baf..746d15c47 100644 --- a/shared/uavobjectdefinition/pathstatus.xml +++ b/shared/uavobjectdefinition/pathstatus.xml @@ -1,5 +1,5 @@ - + Status of the current path mode Can come from any @ref PathFollower module diff --git a/shared/uavobjectdefinition/poilearnsettings.xml b/shared/uavobjectdefinition/poilearnsettings.xml index 281d064e3..21ff01bbf 100644 --- a/shared/uavobjectdefinition/poilearnsettings.xml +++ b/shared/uavobjectdefinition/poilearnsettings.xml @@ -1,5 +1,5 @@ - + Settings for the @ref Point of Interest feature diff --git a/shared/uavobjectdefinition/poilocation.xml b/shared/uavobjectdefinition/poilocation.xml index 70022a86a..6ab6f0273 100644 --- a/shared/uavobjectdefinition/poilocation.xml +++ b/shared/uavobjectdefinition/poilocation.xml @@ -1,5 +1,5 @@ - + Contains the current Point of interest relative to @ref HomeLocation diff --git a/shared/uavobjectdefinition/positionactual.xml b/shared/uavobjectdefinition/positionsensor.xml similarity index 60% rename from shared/uavobjectdefinition/positionactual.xml rename to shared/uavobjectdefinition/positionsensor.xml index 0d5eaf006..286163871 100644 --- a/shared/uavobjectdefinition/positionactual.xml +++ b/shared/uavobjectdefinition/positionsensor.xml @@ -1,11 +1,11 @@ - - Contains the current position relative to @ref HomeLocation + + Contains the position in NED frame measured relative to @ref HomeLocation. - + diff --git a/shared/uavobjectdefinition/positionstate.xml b/shared/uavobjectdefinition/positionstate.xml new file mode 100644 index 000000000..f6d689741 --- /dev/null +++ b/shared/uavobjectdefinition/positionstate.xml @@ -0,0 +1,12 @@ + + + Contains the estimate of the current position relative to @ref HomeLocation, in NED coordinates + + + + + + + + + diff --git a/shared/uavobjectdefinition/ratedesired.xml b/shared/uavobjectdefinition/ratedesired.xml index d9266517c..5d68597ee 100644 --- a/shared/uavobjectdefinition/ratedesired.xml +++ b/shared/uavobjectdefinition/ratedesired.xml @@ -1,5 +1,5 @@ - + Status for the matrix mixer showing the output of each mixer after all scaling diff --git a/shared/uavobjectdefinition/receiveractivity.xml b/shared/uavobjectdefinition/receiveractivity.xml index 5f9ed8278..9360f877c 100644 --- a/shared/uavobjectdefinition/receiveractivity.xml +++ b/shared/uavobjectdefinition/receiveractivity.xml @@ -1,5 +1,5 @@ - + Monitors which receiver channels have been active within the last second. - + The input to the relay tuning. diff --git a/shared/uavobjectdefinition/relaytuningsettings.xml b/shared/uavobjectdefinition/relaytuningsettings.xml index b55e165a6..8daa2698f 100644 --- a/shared/uavobjectdefinition/relaytuningsettings.xml +++ b/shared/uavobjectdefinition/relaytuningsettings.xml @@ -1,5 +1,5 @@ - + Setting to run a relay tuning algorithm diff --git a/shared/uavobjectdefinition/revocalibration.xml b/shared/uavobjectdefinition/revocalibration.xml index ed620769e..5f4778791 100644 --- a/shared/uavobjectdefinition/revocalibration.xml +++ b/shared/uavobjectdefinition/revocalibration.xml @@ -1,5 +1,5 @@ - + Settings for the INS to control the algorithm and what is updated diff --git a/shared/uavobjectdefinition/revosettings.xml b/shared/uavobjectdefinition/revosettings.xml index 56d11927e..26e129bdd 100644 --- a/shared/uavobjectdefinition/revosettings.xml +++ b/shared/uavobjectdefinition/revosettings.xml @@ -1,7 +1,7 @@ - + Settings for the revo to control the algorithm and what is updated - + diff --git a/shared/uavobjectdefinition/sonaraltitude.xml b/shared/uavobjectdefinition/sonaraltitude.xml index 98b678413..11ee33933 100644 --- a/shared/uavobjectdefinition/sonaraltitude.xml +++ b/shared/uavobjectdefinition/sonaraltitude.xml @@ -1,5 +1,5 @@ - + The raw data from the ultrasound sonar sensor with altitude estimate. diff --git a/shared/uavobjectdefinition/stabilizationdesired.xml b/shared/uavobjectdefinition/stabilizationdesired.xml index 7e147a48f..bd70011bb 100644 --- a/shared/uavobjectdefinition/stabilizationdesired.xml +++ b/shared/uavobjectdefinition/stabilizationdesired.xml @@ -1,5 +1,5 @@ - + The desired attitude that @ref StabilizationModule will try and achieve if FlightMode is Stabilized. Comes from @ref ManaulControlModule. diff --git a/shared/uavobjectdefinition/stabilizationsettings.xml b/shared/uavobjectdefinition/stabilizationsettings.xml index 3c8928b50..9434313e0 100644 --- a/shared/uavobjectdefinition/stabilizationsettings.xml +++ b/shared/uavobjectdefinition/stabilizationsettings.xml @@ -1,5 +1,5 @@ - + PID settings used by the Stabilization module to combine the @ref AttitudeActual and @ref AttitudeDesired to compute @ref ActuatorDesired diff --git a/shared/uavobjectdefinition/systemalarms.xml b/shared/uavobjectdefinition/systemalarms.xml index fedeef14a..faa1cc9b1 100644 --- a/shared/uavobjectdefinition/systemalarms.xml +++ b/shared/uavobjectdefinition/systemalarms.xml @@ -1,5 +1,5 @@ - + Alarms from OpenPilot to indicate failure conditions or warnings. Set by various modules. Some modules may have a module defined Status and Substatus fields that details its condition. diff --git a/shared/uavobjectdefinition/systemsettings.xml b/shared/uavobjectdefinition/systemsettings.xml index bc7a98019..196292638 100644 --- a/shared/uavobjectdefinition/systemsettings.xml +++ b/shared/uavobjectdefinition/systemsettings.xml @@ -1,5 +1,5 @@ - + Select airframe type. Currently used by @ref ActuatorModule to choose mixing from @ref ActuatorDesired to @ref ActuatorCommand diff --git a/shared/uavobjectdefinition/systemstats.xml b/shared/uavobjectdefinition/systemstats.xml index d271c6517..e09c817a4 100644 --- a/shared/uavobjectdefinition/systemstats.xml +++ b/shared/uavobjectdefinition/systemstats.xml @@ -1,5 +1,5 @@ - + CPU and memory usage from OpenPilot computer. @@ -18,4 +18,4 @@ - \ No newline at end of file + diff --git a/shared/uavobjectdefinition/taskinfo.xml b/shared/uavobjectdefinition/taskinfo.xml index 869e3b84b..40b27265a 100644 --- a/shared/uavobjectdefinition/taskinfo.xml +++ b/shared/uavobjectdefinition/taskinfo.xml @@ -1,5 +1,5 @@ - + Task information diff --git a/shared/uavobjectdefinition/txpidsettings.xml b/shared/uavobjectdefinition/txpidsettings.xml index 6d8f1498f..41c7d06ca 100644 --- a/shared/uavobjectdefinition/txpidsettings.xml +++ b/shared/uavobjectdefinition/txpidsettings.xml @@ -1,5 +1,5 @@ - + Settings used by @ref TxPID optional module to tune PID settings using R/C transmitter - + Used within @ref GuidanceModule to communicate between the task computing the desired velocity and the PID loop to achieve it (running at different rates). diff --git a/shared/uavobjectdefinition/velocityactual.xml b/shared/uavobjectdefinition/velocitystate.xml similarity index 70% rename from shared/uavobjectdefinition/velocityactual.xml rename to shared/uavobjectdefinition/velocitystate.xml index a4173b033..90a89a4dd 100644 --- a/shared/uavobjectdefinition/velocityactual.xml +++ b/shared/uavobjectdefinition/velocitystate.xml @@ -1,6 +1,6 @@ - - Updated by @ref AHRSCommsModule and used within @ref GuidanceModule for velocity control + + Updated by @ref StateEstimationModule, velocity relative to @ref HomeLocation. diff --git a/shared/uavobjectdefinition/vtolpathfollowersettings.xml b/shared/uavobjectdefinition/vtolpathfollowersettings.xml index 20adeea52..35089b9e0 100644 --- a/shared/uavobjectdefinition/vtolpathfollowersettings.xml +++ b/shared/uavobjectdefinition/vtolpathfollowersettings.xml @@ -1,5 +1,5 @@ - + Settings for the @ref VtolPathFollowerModule diff --git a/shared/uavobjectdefinition/watchdogstatus.xml b/shared/uavobjectdefinition/watchdogstatus.xml index 2c09abac8..2adef2c87 100644 --- a/shared/uavobjectdefinition/watchdogstatus.xml +++ b/shared/uavobjectdefinition/watchdogstatus.xml @@ -1,5 +1,5 @@ - + For monitoring the flags in the watchdog and especially the bootup flags @@ -8,4 +8,4 @@ - \ No newline at end of file + diff --git a/shared/uavobjectdefinition/waypoint.xml b/shared/uavobjectdefinition/waypoint.xml index 45107e7d4..bcadf49ac 100644 --- a/shared/uavobjectdefinition/waypoint.xml +++ b/shared/uavobjectdefinition/waypoint.xml @@ -1,5 +1,5 @@ - + A waypoint the aircraft can try and hit. Used by the @ref PathPlanner module diff --git a/shared/uavobjectdefinition/waypointactive.xml b/shared/uavobjectdefinition/waypointactive.xml index c8dff749e..3343715b5 100644 --- a/shared/uavobjectdefinition/waypointactive.xml +++ b/shared/uavobjectdefinition/waypointactive.xml @@ -1,5 +1,5 @@ - + Indicates the currently active waypoint