diff --git a/flight/libraries/inc/insgps.h b/flight/libraries/inc/insgps.h index b5736dea3..c77d44510 100644 --- a/flight/libraries/inc/insgps.h +++ b/flight/libraries/inc/insgps.h @@ -67,7 +67,6 @@ void INSSetAccelVar(float accel_var[3]); void INSSetGyroVar(float gyro_var[3]); void INSSetGyroBiasVar(float gyro_bias_var[3]); void INSSetMagNorth(float B[3]); -void INSSetG(float g_e); void INSSetMagVar(float scaled_mag_var[3]); void INSSetBaroVar(float baro_var); void INSPosVelReset(float pos[3], float vel[3]); diff --git a/flight/libraries/insgps13state.c b/flight/libraries/insgps13state.c index b65aa2bde..d014babb1 100644 --- a/flight/libraries/insgps13state.c +++ b/flight/libraries/insgps13state.c @@ -91,8 +91,6 @@ static struct EKFData { float H[NUMV][NUMX]; // local magnetic unit vector in NED frame float Be[3]; - // local gravity vector - float g_e; // covariance matrix and state vector float P[NUMX][NUMX]; float X[NUMX]; @@ -288,11 +286,6 @@ void INSSetMagNorth(float B[3]) ekf.Be[2] = B[2] / mag; } -void INSSetG(float g_e) -{ - ekf.g_e = g_e; -} - void INSStatePrediction(float gyro_data[3], float accel_data[3], float dT) { float U[6]; @@ -648,7 +641,7 @@ void StateEq(float X[NUMX], float U[NUMU], float Xdot[NUMX]) az; Xdot[5] = 2.0f * (q1 * q3 - q0 * q2) * ax + 2 * (q2 * q3 + q0 * q1) * ay + - (q0 * q0 - q1 * q1 - q2 * q2 + q3 * q3) * az + ekf.g_e; + (q0 * q0 - q1 * q1 - q2 * q2 + q3 * q3) * az + 9.81f; // qdot = Q*w Xdot[6] = (-q1 * wx - q2 * wy - q3 * wz) / 2.0f; diff --git a/flight/libraries/sanitycheck.c b/flight/libraries/sanitycheck.c index 67fe55e43..cda3b443a 100644 --- a/flight/libraries/sanitycheck.c +++ b/flight/libraries/sanitycheck.c @@ -42,7 +42,7 @@ #include // a number of useful macros -#define ADDSEVERITY(check) severity = (severity != SYSTEMALARMS_ALARM_OK ? severity : ((check) ? SYSTEMALARMS_ALARM_OK : SYSTEMALARMS_ALARM_ERROR)) +#define ADDSEVERITY(check) severity = (severity != SYSTEMALARMS_ALARM_OK ? severity : ((check) ? SYSTEMALARMS_ALARM_OK : SYSTEMALARMS_ALARM_CRITICAL)) /**************************** diff --git a/flight/modules/Airspeed/airspeed.c b/flight/modules/Airspeed/airspeed.c index 86a6b027a..f859b950f 100644 --- a/flight/modules/Airspeed/airspeed.c +++ b/flight/modules/Airspeed/airspeed.c @@ -45,6 +45,7 @@ #include "baro_airspeed_etasv3.h" #include "baro_airspeed_mpxv.h" #include "gps_airspeed.h" +#include "airspeedalarm.h" #include "taskinfo.h" // Private constants @@ -153,7 +154,7 @@ static void airspeedTask(__attribute__((unused)) void *parameters) // if sensor type changed reset Airspeed alarm if (airspeedSettings.AirspeedSensorType != lastAirspeedSensorType) { - AlarmsSet(SYSTEMALARMS_ALARM_AIRSPEED, SYSTEMALARMS_ALARM_DEFAULT); + AirspeedAlarm(SYSTEMALARMS_ALARM_DEFAULT); lastAirspeedSensorType = airspeedSettings.AirspeedSensorType; switch (airspeedSettings.AirspeedSensorType) { case AIRSPEEDSETTINGS_AIRSPEEDSENSORTYPE_NONE: diff --git a/flight/modules/Airspeed/airspeedalarm.c b/flight/modules/Airspeed/airspeedalarm.c new file mode 100644 index 000000000..c14b62ca1 --- /dev/null +++ b/flight/modules/Airspeed/airspeedalarm.c @@ -0,0 +1,65 @@ +/** + ****************************************************************************** + * @addtogroup OpenPilotModules OpenPilot Modules + * @{ + * @addtogroup AirspeedModule Airspeed Module + * @brief Handle locally airspeed alarms issue changes to PIOS only when necessary + * @{ + * + * @file airspeedalarm.c + * @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2014. + * @brief Airspeed module + * + * @see The GNU Public License (GPL) Version 3 + * + *****************************************************************************/ +/* + * This program is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation; either version 3 of the License, or + * (at your option) any later version. + * + * This program is distributed in the hope that it will be useful, but + * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY + * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License + * for more details. + * + * You should have received a copy of the GNU General Public License along + * with this program; if not, write to the Free Software Foundation, Inc., + * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA + */ + +/** + * Output object: none + * + * Handle locally airspeed alarms issue changes to PIOS only when necessary + * + */ + +#include "airspeedalarm.h" + +// local variable + +static SystemAlarmsAlarmOptions severitySet = SYSTEMALARMS_ALARM_UNINITIALISED; + +// functions + +/** + * Handle airspeed alarms and isuue an Alarm to PIOS only if necessary + */ +bool AirspeedAlarm(SystemAlarmsAlarmOptions severity) +{ + if (severity == severitySet) { + return false; + } + + severitySet = severity; + + return AlarmsSet(SYSTEMALARMS_ALARM_AIRSPEED, severity) == 0; +} + + +/** + * @} + * @} + */ diff --git a/flight/modules/Airspeed/baro_airspeed_etasv3.c b/flight/modules/Airspeed/baro_airspeed_etasv3.c index f9666fc76..66f047fb8 100644 --- a/flight/modules/Airspeed/baro_airspeed_etasv3.c +++ b/flight/modules/Airspeed/baro_airspeed_etasv3.c @@ -40,6 +40,7 @@ #include "hwsettings.h" #include "airspeedsettings.h" #include "airspeedsensor.h" // object that will be updated by the module +#include "airspeedalarm.h" #if defined(PIOS_INCLUDE_ETASV3) @@ -64,13 +65,13 @@ void baro_airspeedGetETASV3(AirspeedSensorData *airspeedSensor, AirspeedSettings if (airspeedSensor->SensorValue == (uint16_t)-1) { airspeedSensor->SensorConnected = AIRSPEEDSENSOR_SENSORCONNECTED_FALSE; airspeedSensor->CalibratedAirspeed = 0; - AlarmsSet(SYSTEMALARMS_ALARM_AIRSPEED, SYSTEMALARMS_ALARM_ERROR); + AirspeedAlarm(SYSTEMALARMS_ALARM_ERROR); return; } // only calibrate if no stored calibration is available if (!airspeedSettings->ZeroPoint) { - AlarmsSet(SYSTEMALARMS_ALARM_AIRSPEED, SYSTEMALARMS_ALARM_WARNING); + AirspeedAlarm(SYSTEMALARMS_ALARM_WARNING); // Calibrate sensor by averaging zero point value if (calibrationCount <= CALIBRATION_IDLE_MS / airspeedSettings->SamplePeriod) { calibrationCount++; @@ -95,7 +96,7 @@ void baro_airspeedGetETASV3(AirspeedSensorData *airspeedSensor, AirspeedSettings // Compute airspeed airspeedSensor->CalibratedAirspeed = airspeedSettings->Scale * sqrtf((float)abs(airspeedSensor->SensorValue - airspeedSettings->ZeroPoint)); airspeedSensor->SensorConnected = AIRSPEEDSENSOR_SENSORCONNECTED_TRUE; - AlarmsSet(SYSTEMALARMS_ALARM_AIRSPEED, SYSTEMALARMS_ALARM_OK); + AirspeedAlarm(SYSTEMALARMS_ALARM_OK); } diff --git a/flight/modules/Airspeed/baro_airspeed_mpxv.c b/flight/modules/Airspeed/baro_airspeed_mpxv.c index 6a02780e3..086159e49 100644 --- a/flight/modules/Airspeed/baro_airspeed_mpxv.c +++ b/flight/modules/Airspeed/baro_airspeed_mpxv.c @@ -40,6 +40,7 @@ #include "hwsettings.h" #include "airspeedsettings.h" #include "airspeedsensor.h" // object that will be updated by the module +#include "airspeedalarm.h" #if defined(PIOS_INCLUDE_MPXV) @@ -63,7 +64,7 @@ void baro_airspeedGetMPXV(AirspeedSensorData *airspeedSensor, AirspeedSettingsDa // Ensure that the ADC pin is properly configured if (airspeedADCPin < 0) { airspeedSensor->SensorConnected = AIRSPEEDSENSOR_SENSORCONNECTED_FALSE; - AlarmsSet(SYSTEMALARMS_ALARM_AIRSPEED, SYSTEMALARMS_ALARM_ERROR); + AirspeedAlarm(SYSTEMALARMS_ALARM_ERROR); return; } if (sensor.type == PIOS_MPXV_UNKNOWN) { @@ -76,7 +77,7 @@ void baro_airspeedGetMPXV(AirspeedSensorData *airspeedSensor, AirspeedSettingsDa break; default: airspeedSensor->SensorConnected = AIRSPEEDSENSOR_SENSORCONNECTED_FALSE; - AlarmsSet(SYSTEMALARMS_ALARM_AIRSPEED, SYSTEMALARMS_ALARM_ERROR); + AirspeedAlarm(SYSTEMALARMS_ALARM_ERROR); return; } } @@ -84,7 +85,7 @@ void baro_airspeedGetMPXV(AirspeedSensorData *airspeedSensor, AirspeedSettingsDa airspeedSensor->SensorValue = PIOS_MPXV_Measure(&sensor); if (!airspeedSettings->ZeroPoint) { - AlarmsSet(SYSTEMALARMS_ALARM_AIRSPEED, SYSTEMALARMS_ALARM_WARNING); + AirspeedAlarm(SYSTEMALARMS_ALARM_WARNING); // Calibrate sensor by averaging zero point value if (calibrationCount < CALIBRATION_IDLE_MS / airspeedSettings->SamplePeriod) { // First let sensor warm up and stabilize. calibrationCount++; @@ -109,7 +110,7 @@ void baro_airspeedGetMPXV(AirspeedSensorData *airspeedSensor, AirspeedSettingsDa airspeedSensor->CalibratedAirspeed = PIOS_MPXV_CalcAirspeed(&sensor, airspeedSensor->SensorValue) * (alpha) + airspeedSensor->CalibratedAirspeed * (1.0f - alpha); airspeedSensor->SensorConnected = AIRSPEEDSENSOR_SENSORCONNECTED_TRUE; - AlarmsSet(SYSTEMALARMS_ALARM_AIRSPEED, SYSTEMALARMS_ALARM_OK); + AirspeedAlarm(SYSTEMALARMS_ALARM_OK); } #endif /* if defined(PIOS_INCLUDE_MPXV) */ diff --git a/flight/modules/Airspeed/baro_airspeed_ms4525do.c b/flight/modules/Airspeed/baro_airspeed_ms4525do.c index 2a850d884..181f07c47 100644 --- a/flight/modules/Airspeed/baro_airspeed_ms4525do.c +++ b/flight/modules/Airspeed/baro_airspeed_ms4525do.c @@ -40,6 +40,7 @@ #include "hwsettings.h" #include "airspeedsettings.h" #include "airspeedsensor.h" // object that will be updated by the module +#include "airspeedalarm.h" #include "taskinfo.h" #if defined(PIOS_INCLUDE_MS4525DO) @@ -74,7 +75,7 @@ void baro_airspeedGetMS4525DO(AirspeedSensorData *airspeedSensor, AirspeedSettin airspeedSensor->SensorValueTemperature = -1; airspeedSensor->SensorConnected = AIRSPEEDSENSOR_SENSORCONNECTED_FALSE; airspeedSensor->CalibratedAirspeed = 0; - AlarmsSet(SYSTEMALARMS_ALARM_AIRSPEED, SYSTEMALARMS_ALARM_ERROR); + AirspeedAlarm(SYSTEMALARMS_ALARM_ERROR); return; } @@ -87,7 +88,7 @@ void baro_airspeedGetMS4525DO(AirspeedSensorData *airspeedSensor, AirspeedSettin if (calibrationCount <= CALIBRATION_IDLE_MS / airspeedSettings->SamplePeriod) { calibrationCount++; filter_reg = (airspeedSensor->SensorValue << FILTER_SHIFT); - AlarmsSet(SYSTEMALARMS_ALARM_AIRSPEED, SYSTEMALARMS_ALARM_WARNING); + AirspeedAlarm(SYSTEMALARMS_ALARM_WARNING); return; } else if (calibrationCount <= (CALIBRATION_IDLE_MS + CALIBRATION_COUNT_MS) / airspeedSettings->SamplePeriod) { calibrationCount++; @@ -101,7 +102,7 @@ void baro_airspeedGetMS4525DO(AirspeedSensorData *airspeedSensor, AirspeedSettin AirspeedSettingsZeroPointSet(&airspeedSettings->ZeroPoint); calibrationCount = 0; } - AlarmsSet(SYSTEMALARMS_ALARM_AIRSPEED, SYSTEMALARMS_ALARM_WARNING); + AirspeedAlarm(SYSTEMALARMS_ALARM_WARNING); return; } } @@ -128,7 +129,7 @@ void baro_airspeedGetMS4525DO(AirspeedSensorData *airspeedSensor, AirspeedSettin airspeedSensor->TrueAirspeed = airspeedSensor->CalibratedAirspeed * TASFACTOR * sqrtf(T); airspeedSensor->SensorConnected = AIRSPEEDSENSOR_SENSORCONNECTED_TRUE; // everything is fine so set ALARM_OK - AlarmsSet(SYSTEMALARMS_ALARM_AIRSPEED, SYSTEMALARMS_ALARM_OK); + AirspeedAlarm(SYSTEMALARMS_ALARM_OK); } #endif /* if defined(PIOS_INCLUDE_MS4525DO) */ diff --git a/flight/modules/Airspeed/gps_airspeed.c b/flight/modules/Airspeed/gps_airspeed.c index fa169e692..1a56f353d 100644 --- a/flight/modules/Airspeed/gps_airspeed.c +++ b/flight/modules/Airspeed/gps_airspeed.c @@ -37,6 +37,7 @@ #include "airspeedsettings.h" #include "gps_airspeed.h" #include "CoordinateConversions.h" +#include "airspeedalarm.h" #include @@ -45,7 +46,6 @@ #define GPS_AIRSPEED_BIAS_KI 0.1f // Needs to be settable in a UAVO #define SAMPLING_DELAY_MS_GPS 100 // Needs to be settable in a UAVO #define GPS_AIRSPEED_TIME_CONSTANT_MS 500.0f // Needs to be settable in a UAVO -#define COSINE_OF_5_DEG 0.9961947f // Private types struct GPSGlobals { @@ -60,12 +60,6 @@ struct GPSGlobals { static struct GPSGlobals *gps; // Private functions -// a simple square inline function based on multiplication faster than powf(x,2.0f) -static inline float Sq(float x) -{ - return x * x; -} - /* * Initialize function loads first data sets, and allocates memory for structure. @@ -109,11 +103,6 @@ void gps_airspeedInitialize() * where "f" is the fuselage vector in earth coordinates. * We then solve for |V| = |V_gps_2-V_gps_1|/ |f_2 - f1|. */ -/* Remark regarding "IMU Wind Estimation": The approach includes errors when |V| is - * not constant, i.e. when the change in V_gps does not solely come from a reorientation - * this error depends strongly on the time scale considered. Is the time between t1 and t2 too - * small, "spikes" absorving unconsidred acceleration will arise - */ void gps_airspeedGet(AirspeedSensorData *airspeedData, AirspeedSettingsData *airspeedSettings) { float Rbe[3][3]; @@ -132,39 +121,35 @@ void gps_airspeedGet(AirspeedSensorData *airspeedData, AirspeedSettingsData *air float cosDiff = (Rbe[0][0] * gps->RbeCol1_old[0]) + (Rbe[0][1] * gps->RbeCol1_old[1]) + (Rbe[0][2] * gps->RbeCol1_old[2]); // If there's more than a 5 degree difference between two fuselage measurements, then we have sufficient delta to continue. - if (fabsf(cosDiff) < COSINE_OF_5_DEG) { + if (fabsf(cosDiff) < cosf(DEG2RAD(5.0f))) { VelocityStateData gpsVelData; VelocityStateGet(&gpsVelData); if (gpsVelData.North * gpsVelData.North + gpsVelData.East * gpsVelData.East + gpsVelData.Down * gpsVelData.Down < 1.0f) { airspeedData->CalibratedAirspeed = 0; airspeedData->SensorConnected = AIRSPEEDSENSOR_SENSORCONNECTED_FALSE; - AlarmsSet(SYSTEMALARMS_ALARM_AIRSPEED, SYSTEMALARMS_ALARM_WARNING); + AirspeedAlarm(SYSTEMALARMS_ALARM_ERROR); return; // do not calculate if gps velocity is insufficient... } // Calculate the norm^2 of the difference between the two GPS vectors - float normDiffGPS2 = Sq(gpsVelData.North - gps->gpsVelOld_N) + Sq(gpsVelData.East - gps->gpsVelOld_E) + Sq(gpsVelData.Down - gps->gpsVelOld_D); + 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); // Calculate the norm^2 of the difference between the two fuselage vectors - float normDiffAttitude2 = Sq(Rbe[0][0] - gps->RbeCol1_old[0]) + Sq(Rbe[0][1] - gps->RbeCol1_old[1]) + Sq(Rbe[0][2] - gps->RbeCol1_old[2]); + float normDiffAttitude2 = powf(Rbe[0][0] - gps->RbeCol1_old[0], 2.0f) + powf(Rbe[0][1] - gps->RbeCol1_old[1], 2.0f) + powf(Rbe[0][2] - gps->RbeCol1_old[2], 2.0f); // Airspeed magnitude is the ratio between the two difference norms float airspeed = sqrtf(normDiffGPS2 / normDiffAttitude2); if (!IS_REAL(airspeedData->CalibratedAirspeed)) { airspeedData->CalibratedAirspeed = 0; airspeedData->SensorConnected = AIRSPEEDSENSOR_SENSORCONNECTED_FALSE; - AlarmsSet(SYSTEMALARMS_ALARM_AIRSPEED, SYSTEMALARMS_ALARM_ERROR); - } else if (!IS_REAL(airspeed)) { - airspeedData->CalibratedAirspeed = 0; - airspeedData->SensorConnected = AIRSPEEDSENSOR_SENSORCONNECTED_FALSE; - AlarmsSet(SYSTEMALARMS_ALARM_AIRSPEED, SYSTEMALARMS_ALARM_WARNING); + AirspeedAlarm(SYSTEMALARMS_ALARM_ERROR); } else { // need a low pass filter to filter out spikes in non coordinated maneuvers airspeedData->CalibratedAirspeed = (1.0f - airspeedSettings->GroundSpeedBasedEstimationLowPassAlpha) * gps->oldAirspeed + airspeedSettings->GroundSpeedBasedEstimationLowPassAlpha * airspeed; gps->oldAirspeed = airspeedData->CalibratedAirspeed; airspeedData->SensorConnected = AIRSPEEDSENSOR_SENSORCONNECTED_TRUE; - AlarmsSet(SYSTEMALARMS_ALARM_AIRSPEED, SYSTEMALARMS_ALARM_OK); + AirspeedAlarm(SYSTEMALARMS_ALARM_OK); } // Save old variables for next pass diff --git a/flight/modules/Airspeed/inc/airspeedalarm.h b/flight/modules/Airspeed/inc/airspeedalarm.h new file mode 100644 index 000000000..0a5d6c25f --- /dev/null +++ b/flight/modules/Airspeed/inc/airspeedalarm.h @@ -0,0 +1,45 @@ +/** + ****************************************************************************** + * @addtogroup OpenPilotModules OpenPilot Modules + * @{ + * @addtogroup AirspeedModule Airspeed Module + * @brief Handle locally airspeed alarms issue changes to PIOS only when necessary + * @{ + * + * @file airspeedalarm.h + * @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2014. + * @brief Airspeed module, reads temperature and pressure from MS4525DO + * + * @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 AIRSPEEDALARM_H +#define AIRSPEEDALARM_H + +#include +#include "alarms.h" + + +bool AirspeedAlarm(SystemAlarmsAlarmOptions severity); + +#endif // AIRSPEEDALARM_H + +/** + * @} + * @} + */ diff --git a/flight/modules/FixedWingPathFollower/fixedwingpathfollower.c b/flight/modules/FixedWingPathFollower/fixedwingpathfollower.c index ff1b0a507..5cdbe7c9e 100644 --- a/flight/modules/FixedWingPathFollower/fixedwingpathfollower.c +++ b/flight/modules/FixedWingPathFollower/fixedwingpathfollower.c @@ -193,7 +193,7 @@ static void pathfollowerTask(__attribute__((unused)) void *parameters) AlarmsSet(SYSTEMALARMS_ALARM_GUIDANCE, SYSTEMALARMS_ALARM_WARNING); } } else { - AlarmsSet(SYSTEMALARMS_ALARM_GUIDANCE, SYSTEMALARMS_ALARM_ERROR); + AlarmsSet(SYSTEMALARMS_ALARM_GUIDANCE, SYSTEMALARMS_ALARM_CRITICAL); } } else { pathStatus.UID = pathDesired.UID; @@ -221,7 +221,7 @@ static void pathfollowerTask(__attribute__((unused)) void *parameters) break; default: pathStatus.Status = PATHSTATUS_STATUS_CRITICAL; - AlarmsSet(SYSTEMALARMS_ALARM_GUIDANCE, SYSTEMALARMS_ALARM_ERROR); + AlarmsSet(SYSTEMALARMS_ALARM_GUIDANCE, SYSTEMALARMS_ALARM_CRITICAL); break; } } diff --git a/flight/modules/ManualControl/armhandler.c b/flight/modules/ManualControl/armhandler.c index 76833678e..1615ac91b 100644 --- a/flight/modules/ManualControl/armhandler.c +++ b/flight/modules/ManualControl/armhandler.c @@ -258,7 +258,7 @@ static bool okToArm(void) // Check each alarm for (int i = 0; i < SYSTEMALARMS_ALARM_NUMELEM; i++) { - if (cast_struct_to_array(alarms.Alarm, alarms.Alarm.Actuator)[i] >= SYSTEMALARMS_ALARM_ERROR) { // found an alarm thats set + if (cast_struct_to_array(alarms.Alarm, alarms.Alarm.Actuator)[i] >= SYSTEMALARMS_ALARM_CRITICAL) { // found an alarm thats set if (i == SYSTEMALARMS_ALARM_GPS || i == SYSTEMALARMS_ALARM_TELEMETRY) { continue; } diff --git a/flight/modules/PathPlanner/pathplanner.c b/flight/modules/PathPlanner/pathplanner.c index c25d3a0b7..745a8c17e 100644 --- a/flight/modules/PathPlanner/pathplanner.c +++ b/flight/modules/PathPlanner/pathplanner.c @@ -173,7 +173,7 @@ static void pathPlannerTask() // copy pasta: same calculation as in manualcontrol, set return to home coordinates plan_setup_positionHold(); } - AlarmsSet(SYSTEMALARMS_ALARM_PATHPLAN, SYSTEMALARMS_ALARM_ERROR); + AlarmsSet(SYSTEMALARMS_ALARM_PATHPLAN, SYSTEMALARMS_ALARM_CRITICAL); return; } diff --git a/flight/modules/Stabilization/innerloop.c b/flight/modules/Stabilization/innerloop.c index b04556cb6..e7254b933 100644 --- a/flight/modules/Stabilization/innerloop.c +++ b/flight/modules/Stabilization/innerloop.c @@ -119,21 +119,21 @@ static void stabilizationInnerloopTask() warn = true; } if (stabSettings.monitor.rateupdates < -(4 * OUTERLOOP_SKIPCOUNT)) { - // error if rate loop skipped more than 4 executions - error = true; + // critical if rate loop skipped more than 4 executions + crit = true; } // check if gyro keeps updating if (stabSettings.monitor.gyroupdates < 1) { - // critical if gyro didn't update at all! - crit = true; + // error if gyro didn't update at all! + error = true; } if (stabSettings.monitor.gyroupdates > 1) { // warning if we missed a gyro update warn = true; } if (stabSettings.monitor.gyroupdates > 3) { - // error if we missed 3 gyro updates - error = true; + // critical if we missed 3 gyro updates + crit = true; } stabSettings.monitor.gyroupdates = 0; diff --git a/flight/modules/Stabilization/stabilization.c b/flight/modules/Stabilization/stabilization.c index b9ff0541a..92b9081d0 100644 --- a/flight/modules/Stabilization/stabilization.c +++ b/flight/modules/Stabilization/stabilization.c @@ -31,6 +31,7 @@ * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA */ + #include #include #include diff --git a/flight/modules/StateEstimation/filterair.c b/flight/modules/StateEstimation/filterair.c index bd977e191..0de2ef7f0 100644 --- a/flight/modules/StateEstimation/filterair.c +++ b/flight/modules/StateEstimation/filterair.c @@ -50,7 +50,7 @@ struct data { // Private functions static int32_t init(stateFilter *self); -static int32_t filter(stateFilter *self, stateEstimation *state); +static filterResult filter(stateFilter *self, stateEstimation *state); int32_t filterAirInitialize(stateFilter *handle) @@ -69,7 +69,7 @@ static int32_t init(stateFilter *self) return 0; } -static int32_t filter(stateFilter *self, stateEstimation *state) +static filterResult filter(stateFilter *self, stateEstimation *state) { struct data *this = (struct data *)self->localdata; @@ -82,7 +82,7 @@ static int32_t filter(stateFilter *self, stateEstimation *state) state->airspeed[1] = state->airspeed[0] * IAS2TAS(this->altitude); } - return 0; + return FILTERRESULT_OK; } diff --git a/flight/modules/StateEstimation/filteraltitude.c b/flight/modules/StateEstimation/filteraltitude.c index 07c771e6e..a20a1be53 100644 --- a/flight/modules/StateEstimation/filteraltitude.c +++ b/flight/modules/StateEstimation/filteraltitude.c @@ -74,7 +74,7 @@ struct data { // Private functions static int32_t init(stateFilter *self); -static int32_t filter(stateFilter *self, stateEstimation *state); +static filterResult filter(stateFilter *self, stateEstimation *state); static void settingsUpdatedCb(UAVObjEvent *ev); @@ -114,7 +114,7 @@ static int32_t init(stateFilter *self) return 0; } -static int32_t filter(stateFilter *self, stateEstimation *state) +static filterResult filter(stateFilter *self, stateEstimation *state) { struct data *this = (struct data *)self->localdata; @@ -204,7 +204,7 @@ static int32_t filter(stateFilter *self, stateEstimation *state) } } - return 0; + return FILTERRESULT_OK; } void settingsUpdatedCb(UAVObjEvent *ev) diff --git a/flight/modules/StateEstimation/filterbaro.c b/flight/modules/StateEstimation/filterbaro.c index 35d4479c0..1a804649b 100644 --- a/flight/modules/StateEstimation/filterbaro.c +++ b/flight/modules/StateEstimation/filterbaro.c @@ -56,7 +56,7 @@ struct data { static int32_t initwithgps(stateFilter *self); static int32_t initwithoutgps(stateFilter *self); static int32_t maininit(stateFilter *self); -static int32_t filter(stateFilter *self, stateEstimation *state); +static filterResult filter(stateFilter *self, stateEstimation *state); int32_t filterBaroInitialize(stateFilter *handle) @@ -105,7 +105,7 @@ static int32_t maininit(stateFilter *self) return 0; } -static int32_t filter(stateFilter *self, stateEstimation *state) +static filterResult filter(stateFilter *self, stateEstimation *state) { struct data *this = (struct data *)self->localdata; @@ -128,7 +128,8 @@ static int32_t filter(stateFilter *self, stateEstimation *state) } // make sure we raise an error until properly initialized - would not be good if people arm and // use altitudehold without initialized barometer filter - return 2; + // Here, the filter is not initialized, return ERROR + return FILTERRESULT_CRITICAL; } else { // Track barometric altitude offset with a low pass filter // based on GPS altitude if available @@ -141,7 +142,7 @@ static int32_t filter(stateFilter *self, stateEstimation *state) this->baroAlt = state->baro[0]; state->baro[0] -= this->baroOffset; } - return 0; + return FILTERRESULT_OK; } } diff --git a/flight/modules/StateEstimation/filtercf.c b/flight/modules/StateEstimation/filtercf.c index f0386d9e5..807618004 100644 --- a/flight/modules/StateEstimation/filtercf.c +++ b/flight/modules/StateEstimation/filtercf.c @@ -81,8 +81,8 @@ static FlightStatusData flightStatus; 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 filterResult filter(stateFilter *self, stateEstimation *state); +static filterResult complementaryFilter(struct data *this, float gyro[3], float accel[3], float mag[3], float attitude[4]); static void flightStatusUpdatedCb(UAVObjEvent *ev); @@ -165,11 +165,11 @@ static int32_t maininit(stateFilter *self) /** * Collect all required state variables, then run complementary filter */ -static int32_t filter(stateFilter *self, stateEstimation *state) +static filterResult filter(stateFilter *self, stateEstimation *state) { - struct data *this = (struct data *)self->localdata; + struct data *this = (struct data *)self->localdata; - int32_t result = 0; + filterResult result = FILTERRESULT_OK; if (IS_SET(state->updated, SENSORUPDATES_mag)) { this->magUpdated = 1; @@ -187,7 +187,7 @@ static int32_t filter(stateFilter *self, stateEstimation *state) if (this->accelUpdated) { float attitude[4]; result = complementaryFilter(this, state->gyro, this->currentAccel, this->currentMag, attitude); - if (!result) { + if (result == FILTERRESULT_OK) { state->attitude[0] = attitude[0]; state->attitude[1] = attitude[1]; state->attitude[2] = attitude[2]; @@ -215,7 +215,7 @@ static inline void apply_accel_filter(const struct data *this, const float *raw, } } -static int32_t complementaryFilter(struct data *this, float gyro[3], float accel[3], float mag[3], float attitude[4]) +static filterResult complementaryFilter(struct data *this, float gyro[3], float accel[3], float mag[3], float attitude[4]) { float dT; @@ -224,7 +224,7 @@ static int32_t complementaryFilter(struct data *this, float gyro[3], float accel #if defined(PIOS_INCLUDE_HMC5883) // wait until mags have been updated if (!this->magUpdated) { - return 1; + return FILTERRESULT_ERROR; } #else mag[0] = 100.0f; @@ -280,13 +280,13 @@ static int32_t complementaryFilter(struct data *this, float gyro[3], float accel this->timeval = PIOS_DELAY_GetRaw(); // Cycle counter used for precise timing this->starttime = xTaskGetTickCount(); // Tick counter used for long time intervals - return 0; // must return zero on initial initialization, so attitude will init with a valid quaternion + return FILTERRESULT_OK; // must return OK on initial initialization, so attitude will init with a valid quaternion } if (this->init == 0 && xTaskGetTickCount() - this->starttime < CALIBRATION_DELAY_MS / portTICK_RATE_MS) { // 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; + return FILTERRESULT_ERROR; } else if (this->init == 0 && xTaskGetTickCount() - this->starttime < (CALIBRATION_DELAY_MS + CALIBRATION_DURATION_MS) / portTICK_RATE_MS) { // For first 6 seconds use accels to get gyro bias this->attitudeSettings.AccelKp = 1.0f; @@ -345,7 +345,7 @@ static int32_t complementaryFilter(struct data *this, float gyro[3], float accel // Account for accel magnitude float accel_mag = sqrtf(this->accels_filtered[0] * this->accels_filtered[0] + this->accels_filtered[1] * this->accels_filtered[1] + this->accels_filtered[2] * this->accels_filtered[2]); if (accel_mag < 1.0e-3f) { - return 2; // safety feature copied from CC + return FILTERRESULT_CRITICAL; // safety feature copied from CC } // Account for filtered gravity vector magnitude @@ -356,7 +356,7 @@ static int32_t complementaryFilter(struct data *this, float gyro[3], float accel grot_mag = 1.0f; } if (grot_mag < 1.0e-3f) { - return 2; + return FILTERRESULT_CRITICAL; } accel_err[0] /= (accel_mag * grot_mag); @@ -449,13 +449,13 @@ static int32_t complementaryFilter(struct data *this, float gyro[3], float accel // THIS SHOULD NEVER ACTUALLY HAPPEN if ((fabsf(qmag) < 1.0e-3f) || isnan(qmag)) { this->first_run = 1; - return 2; + return FILTERRESULT_WARNING; } if (this->init) { - return 0; + return FILTERRESULT_OK; } else { - return 2; // return "critical" for now, so users can see the zeroing period, switch to more graceful notification later + return FILTERRESULT_CRITICAL; // return "critical" for now, so users can see the zeroing period, switch to more graceful notification later } } diff --git a/flight/modules/StateEstimation/filterekf.c b/flight/modules/StateEstimation/filterekf.c index 139edb321..bf9404add 100644 --- a/flight/modules/StateEstimation/filterekf.c +++ b/flight/modules/StateEstimation/filterekf.c @@ -83,7 +83,7 @@ static bool initialized = 0; 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 filterResult filter(stateFilter *self, stateEstimation *state); static inline bool invalid_var(float data); static void globalInit(void); @@ -193,7 +193,7 @@ static int32_t maininit(stateFilter *self) /** * Collect all required state variables, then run complementary filter */ -static int32_t filter(stateFilter *self, stateEstimation *state) +static filterResult filter(stateFilter *self, stateEstimation *state) { struct data *this = (struct data *)self->localdata; @@ -222,7 +222,7 @@ static int32_t filter(stateFilter *self, stateEstimation *state) UNSET_MASK(state->updated, SENSORUPDATES_vel); UNSET_MASK(state->updated, SENSORUPDATES_attitude); UNSET_MASK(state->updated, SENSORUPDATES_gyro); - return 0; + return FILTERRESULT_OK; } dT = PIOS_DELTATIME_GetAverageSeconds(&this->dtconfig); @@ -316,11 +316,11 @@ static int32_t filter(stateFilter *self, stateEstimation *state) this->inited = true; } - return 0; + return FILTERRESULT_OK; } if (!this->inited) { - return 3; + return FILTERRESULT_CRITICAL; } float gyros[3] = { DEG2RAD(this->work.gyro[0]), DEG2RAD(this->work.gyro[1]), DEG2RAD(this->work.gyro[2]) }; @@ -361,7 +361,6 @@ static int32_t filter(stateFilter *self, stateEstimation *state) } INSSetMagNorth(this->homeLocation.Be); - INSSetG(this->homeLocation.g_e); if (!this->usePos) { // position and velocity variance used in indoor mode @@ -433,9 +432,9 @@ static int32_t filter(stateFilter *self, stateEstimation *state) this->work.updated = 0; if (this->init_stage < 0) { - return 1; + return FILTERRESULT_WARNING; } else { - return 0; + return FILTERRESULT_OK; } } diff --git a/flight/modules/StateEstimation/filterlla.c b/flight/modules/StateEstimation/filterlla.c index 034247c82..327fa9a00 100644 --- a/flight/modules/StateEstimation/filterlla.c +++ b/flight/modules/StateEstimation/filterlla.c @@ -53,7 +53,7 @@ struct data { // Private functions static int32_t init(stateFilter *self); -static int32_t filter(stateFilter *self, stateEstimation *state); +static filterResult filter(stateFilter *self, stateEstimation *state); int32_t filterLLAInitialize(stateFilter *handle) @@ -86,13 +86,13 @@ static int32_t init(__attribute__((unused)) stateFilter *self) return 0; } -static int32_t filter(__attribute__((unused)) stateFilter *self, stateEstimation *state) +static filterResult filter(__attribute__((unused)) stateFilter *self, stateEstimation *state) { struct data *this = (struct data *)self->localdata; // cannot update local NED if home location is unset if (this->home.Set != HOMELOCATION_SET_TRUE) { - return 0; + return FILTERRESULT_WARNING; } // only do stuff if we have a valid GPS update @@ -116,7 +116,7 @@ static int32_t filter(__attribute__((unused)) stateFilter *self, stateEstimation } } - return 0; + return FILTERRESULT_OK; } /** diff --git a/flight/modules/StateEstimation/filtermag.c b/flight/modules/StateEstimation/filtermag.c index 0fd617092..c48c4463e 100644 --- a/flight/modules/StateEstimation/filtermag.c +++ b/flight/modules/StateEstimation/filtermag.c @@ -59,7 +59,7 @@ struct data { // Private functions static int32_t init(stateFilter *self); -static int32_t filter(stateFilter *self, stateEstimation *state); +static filterResult filter(stateFilter *self, stateEstimation *state); static void checkMagValidity(struct data *this, float mag[3]); static void magOffsetEstimation(struct data *this, float mag[3]); @@ -87,7 +87,7 @@ static int32_t init(stateFilter *self) return 0; } -static int32_t filter(stateFilter *self, stateEstimation *state) +static filterResult filter(stateFilter *self, stateEstimation *state) { struct data *this = (struct data *)self->localdata; @@ -98,7 +98,7 @@ static int32_t filter(stateFilter *self, stateEstimation *state) } } - return 0; + return FILTERRESULT_OK; } /** diff --git a/flight/modules/StateEstimation/filterstationary.c b/flight/modules/StateEstimation/filterstationary.c index 14075fd1c..56a0a7c69 100644 --- a/flight/modules/StateEstimation/filterstationary.c +++ b/flight/modules/StateEstimation/filterstationary.c @@ -42,7 +42,7 @@ // Private functions static int32_t init(stateFilter *self); -static int32_t filter(stateFilter *self, stateEstimation *state); +static filterResult filter(stateFilter *self, stateEstimation *state); int32_t filterStationaryInitialize(stateFilter *handle) @@ -58,7 +58,7 @@ static int32_t init(__attribute__((unused)) stateFilter *self) return 0; } -static int32_t filter(__attribute__((unused)) stateFilter *self, stateEstimation *state) +static filterResult filter(__attribute__((unused)) stateFilter *self, stateEstimation *state) { state->pos[0] = 0.0f; state->pos[1] = 0.0f; @@ -70,7 +70,7 @@ static int32_t filter(__attribute__((unused)) stateFilter *self, stateEstimation state->vel[2] = 0.0f; state->updated |= SENSORUPDATES_vel; - return 0; + return FILTERRESULT_OK; } /** diff --git a/flight/modules/StateEstimation/inc/stateestimation.h b/flight/modules/StateEstimation/inc/stateestimation.h index 298edb31e..d93235f4e 100644 --- a/flight/modules/StateEstimation/inc/stateestimation.h +++ b/flight/modules/StateEstimation/inc/stateestimation.h @@ -32,6 +32,17 @@ #include + +// Enumeration for filter result +typedef enum { + FILTERRESULT_UNINITIALISED = -1, + FILTERRESULT_OK = 0, + FILTERRESULT_WARNING = 1, + FILTERRESULT_CRITICAL = 2, + FILTERRESULT_ERROR = 3, +} filterResult; + + typedef enum { SENSORUPDATES_gyro = 1 << 0, SENSORUPDATES_accel = 1 << 1, @@ -58,7 +69,7 @@ typedef struct { typedef struct stateFilterStruct { int32_t (*init)(struct stateFilterStruct *self); - int32_t (*filter)(struct stateFilterStruct *self, stateEstimation *state); + filterResult (*filter)(struct stateFilterStruct *self, stateEstimation *state); void *localdata; } stateFilter; diff --git a/flight/modules/StateEstimation/stateestimation.c b/flight/modules/StateEstimation/stateestimation.c index 3b5f4b9b3..079db8ca1 100644 --- a/flight/modules/StateEstimation/stateestimation.c +++ b/flight/modules/StateEstimation/stateestimation.c @@ -224,6 +224,7 @@ static const filterPipeline *ekf13iQueue = &(filterPipeline) { } } }; + static const filterPipeline *ekf13Queue = &(filterPipeline) { .filter = &magFilter, .next = &(filterPipeline) { @@ -334,9 +335,9 @@ MODULE_INITCALL(StateEstimationInitialize, StateEstimationStart); 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 filterResult alarm = FILTERRESULT_OK; + static filterResult lastAlarm = FILTERRESULT_UNINITIALISED; + static uint16_t alarmcounter = 0; static const filterPipeline *current; static stateEstimation states; static uint32_t last_time; @@ -352,12 +353,12 @@ static void StateEstimationCb(void) switch (runState) { case RUNSTATE_LOAD: - alarm = 0; + alarm = FILTERRESULT_OK; // set alarm to warning if called through timeout if (updatedSensors == 0) { if (PIOS_DELAY_DiffuS(last_time) > 1000 * TIMEOUT_MS) { - alarm = 1; + alarm = FILTERRESULT_WARNING; } } else { last_time = PIOS_DELAY_GetRaw(); @@ -442,7 +443,7 @@ static void StateEstimationCb(void) case RUNSTATE_FILTER: if (current != NULL) { - int32_t result = current->filter->filter((stateFilter *)current->filter, &states); + filterResult result = current->filter->filter((stateFilter *)current->filter, &states); if (result > alarm) { alarm = result; } @@ -498,12 +499,12 @@ static void StateEstimationCb(void) } // clear alarms if everything is alright, then schedule callback execution after timeout - if (lastAlarm == 1) { + if (lastAlarm == FILTERRESULT_WARNING) { AlarmsSet(SYSTEMALARMS_ALARM_ATTITUDE, SYSTEMALARMS_ALARM_WARNING); - } else if (lastAlarm == 2) { - AlarmsSet(SYSTEMALARMS_ALARM_ATTITUDE, SYSTEMALARMS_ALARM_ERROR); - } else if (lastAlarm >= 3) { + } else if (lastAlarm == FILTERRESULT_CRITICAL) { AlarmsSet(SYSTEMALARMS_ALARM_ATTITUDE, SYSTEMALARMS_ALARM_CRITICAL); + } else if (lastAlarm >= FILTERRESULT_ERROR) { + AlarmsSet(SYSTEMALARMS_ALARM_ATTITUDE, SYSTEMALARMS_ALARM_ERROR); } else { AlarmsClear(SYSTEMALARMS_ALARM_ATTITUDE); } diff --git a/flight/modules/System/systemmod.c b/flight/modules/System/systemmod.c index abd4d960c..8595642dc 100644 --- a/flight/modules/System/systemmod.c +++ b/flight/modules/System/systemmod.c @@ -403,7 +403,7 @@ static void hwSettingsUpdatedCb(__attribute__((unused)) UAVObjEvent *ev) HwSettingsGet(¤tHwSettings); // check whether the Hw Configuration has changed from the one used at boot time if (memcmp(&bootHwSettings, ¤tHwSettings, sizeof(HwSettingsData)) != 0) { - ExtendedAlarmsSet(SYSTEMALARMS_ALARM_BOOTFAULT, SYSTEMALARMS_ALARM_ERROR, SYSTEMALARMS_EXTENDEDALARMSTATUS_REBOOTREQUIRED, 0); + ExtendedAlarmsSet(SYSTEMALARMS_ALARM_BOOTFAULT, SYSTEMALARMS_ALARM_CRITICAL, SYSTEMALARMS_EXTENDEDALARMSTATUS_REBOOTREQUIRED, 0); } } @@ -607,7 +607,7 @@ static void updateSystemAlarms() UAVObjClearStats(); EventClearStats(); if (objStats.eventCallbackErrors > 0 || objStats.eventQueueErrors > 0 || evStats.eventErrors > 0) { - AlarmsSet(SYSTEMALARMS_ALARM_EVENTSYSTEM, SYSTEMALARMS_ALARM_ERROR); + AlarmsSet(SYSTEMALARMS_ALARM_EVENTSYSTEM, SYSTEMALARMS_ALARM_WARNING); } else { AlarmsClear(SYSTEMALARMS_ALARM_EVENTSYSTEM); } diff --git a/flight/modules/VtolPathFollower/vtolpathfollower.c b/flight/modules/VtolPathFollower/vtolpathfollower.c index 36428a2a9..259d81fe4 100644 --- a/flight/modules/VtolPathFollower/vtolpathfollower.c +++ b/flight/modules/VtolPathFollower/vtolpathfollower.c @@ -139,7 +139,6 @@ int32_t VtolPathFollowerInitialize() AccessoryDesiredInitialize(); PoiLearnSettingsInitialize(); PoiLocationInitialize(); - HomeLocationInitialize(); vtolpathfollower_enabled = true; } else { vtolpathfollower_enabled = false; @@ -159,7 +158,6 @@ static float eastPosIntegral = 0; static float downPosIntegral = 0; static float thrustOffset = 0; -static float gravity; /** * Module thread, should not return. */ @@ -183,7 +181,6 @@ static void vtolPathFollowerTask(__attribute__((unused)) void *parameters) // 2. Flight mode is PositionHold and PathDesired.Mode is Endpoint OR // FlightMode is PathPlanner and PathDesired.Mode is Endpoint or Path - HomeLocationg_eGet(&gravity); SystemSettingsGet(&systemSettings); if ((systemSettings.AirframeType != SYSTEMSETTINGS_AIRFRAMETYPE_VTOL) && (systemSettings.AirframeType != SYSTEMSETTINGS_AIRFRAMETYPE_QUADP) && (systemSettings.AirframeType != SYSTEMSETTINGS_AIRFRAMETYPE_OCTOCOAXX) && (systemSettings.AirframeType != SYSTEMSETTINGS_AIRFRAMETYPE_QUADX) @@ -216,7 +213,7 @@ static void vtolPathFollowerTask(__attribute__((unused)) void *parameters) updateVtolDesiredAttitude(true); updatePOIBearing(); } else { - AlarmsSet(SYSTEMALARMS_ALARM_GUIDANCE, SYSTEMALARMS_ALARM_ERROR); + AlarmsSet(SYSTEMALARMS_ALARM_GUIDANCE, SYSTEMALARMS_ALARM_CRITICAL); } } else { if (pathDesired.Mode == PATHDESIRED_MODE_FLYENDPOINT) { @@ -224,7 +221,7 @@ static void vtolPathFollowerTask(__attribute__((unused)) void *parameters) updateVtolDesiredAttitude(false); AlarmsSet(SYSTEMALARMS_ALARM_GUIDANCE, SYSTEMALARMS_ALARM_OK); } else { - AlarmsSet(SYSTEMALARMS_ALARM_GUIDANCE, SYSTEMALARMS_ALARM_ERROR); + AlarmsSet(SYSTEMALARMS_ALARM_GUIDANCE, SYSTEMALARMS_ALARM_CRITICAL); } } } else { @@ -249,7 +246,7 @@ static void vtolPathFollowerTask(__attribute__((unused)) void *parameters) break; default: pathStatus.Status = PATHSTATUS_STATUS_CRITICAL; - AlarmsSet(SYSTEMALARMS_ALARM_GUIDANCE, SYSTEMALARMS_ALARM_ERROR); + AlarmsSet(SYSTEMALARMS_ALARM_GUIDANCE, SYSTEMALARMS_ALARM_CRITICAL); break; } PathStatusSet(&pathStatus); @@ -686,7 +683,7 @@ static void updateNedAccel() accel_ned[i] += Rbe[j][i] * accel[j]; } } - accel_ned[2] += gravity; + accel_ned[2] += 9.81f; NedAccelData accelData; NedAccelGet(&accelData); diff --git a/flight/targets/boards/coptercontrol/firmware/inc/pios_config.h b/flight/targets/boards/coptercontrol/firmware/inc/pios_config.h index e7da296e9..3878d90d0 100644 --- a/flight/targets/boards/coptercontrol/firmware/inc/pios_config.h +++ b/flight/targets/boards/coptercontrol/firmware/inc/pios_config.h @@ -171,10 +171,10 @@ #endif #define PIOS_TELEM_RX_STACK_SIZE 410 #define PIOS_TELEM_TX_STACK_SIZE 560 -#define PIOS_EVENTDISPATCHER_STACK_SIZE 95 +#define PIOS_EVENTDISPATCHER_STACK_SIZE 95 /* This can't be too high to stop eventdispatcher thread overflowing */ -#define PIOS_EVENTDISAPTCHER_QUEUE 10 +#define PIOS_EVENTDISAPTCHER_QUEUE 10 /* Revolution series */ /* #define REVOLUTION */ diff --git a/ground/openpilotgcs/share/openpilotgcs/diagrams/default/system-health.svg b/ground/openpilotgcs/share/openpilotgcs/diagrams/default/system-health.svg index 2a65f1202..ec21f2821 100644 --- a/ground/openpilotgcs/share/openpilotgcs/diagrams/default/system-health.svg +++ b/ground/openpilotgcs/share/openpilotgcs/diagrams/default/system-health.svg @@ -27,16 +27,16 @@ borderopacity="1.0" inkscape:pageopacity="0.0" inkscape:pageshadow="2" - inkscape:zoom="5.2771064" - inkscape:cx="50.100274" - inkscape:cy="37.563212" - inkscape:current-layer="layer36" + inkscape:zoom="7.4934872" + inkscape:cx="56.515743" + inkscape:cy="39.787525" + inkscape:current-layer="background" id="namedview3608" showgrid="true" - inkscape:window-width="1366" - inkscape:window-height="712" - inkscape:window-x="-4" - inkscape:window-y="-4" + inkscape:window-width="1280" + inkscape:window-height="928" + inkscape:window-x="0" + inkscape:window-y="27" inkscape:window-maximized="1" showguides="true" inkscape:guide-bbox="true" @@ -687,7 +687,7 @@ image/svg+xml - + @@ -698,13 +698,14 @@ inkscape:groupmode="layer" inkscape:label="Background"> + style="fill:#918a6f;fill-opacity:1;fill-rule:evenodd;stroke:#000000;stroke-width:1;stroke-linecap:butt;stroke-linejoin:round;stroke-miterlimit:4;stroke-opacity:1;stroke-dasharray:none" + rx="1.6" /> SENSR AUTO MISC + x="-463.67752" + y="481.89038">MISC PWR + x="-472.0209" + y="432.35162">PWR SYS I/O + x="-423.85706" + y="462.40958">I/O LINK + d="M 7.9615204,18.056455 25.751516,26.834614" + style="fill:#ff0000;stroke:#ff0000;stroke-width:1.19737208;stroke-linecap:butt;stroke-linejoin:miter;stroke-miterlimit:4;stroke-opacity:1;stroke-dasharray:none;display:inline" + sodipodi:nodetypes="cc" /> + d="M 7.9364397,26.848957 25.793388,17.935735" + style="fill:#ff0000;stroke:#ff0000;stroke-width:1.19791019;stroke-linecap:butt;stroke-linejoin:miter;stroke-miterlimit:4;stroke-opacity:1;stroke-dasharray:none;display:inline" + sodipodi:nodetypes="cc" /> + d="M 73.398615,3.9044559 91.236289,12.771269" + style="fill:#ff0000;stroke:#ff0000;stroke-width:1.19480562;stroke-linecap:butt;stroke-linejoin:miter;stroke-miterlimit:4;stroke-opacity:1;stroke-dasharray:none;display:inline" + sodipodi:nodetypes="cc" /> + d="M 73.388721,12.684336 91.265942,3.8889847" + style="fill:#ff0000;stroke:#ff0000;stroke-width:1.19222152;stroke-linecap:butt;stroke-linejoin:miter;stroke-miterlimit:4;stroke-opacity:1;stroke-dasharray:none;display:inline" + sodipodi:nodetypes="cc" /> + d="M 51.597094,3.9170519 69.379793,12.712435" + style="fill:#ff0000;stroke:#ff0000;stroke-width:1.19661069;stroke-linecap:butt;stroke-linejoin:miter;stroke-miterlimit:4;stroke-opacity:1;stroke-dasharray:none;display:inline" + sodipodi:nodetypes="cc" /> + d="M 51.571412,12.7076 69.339433,3.9056189" + style="fill:#ff0000;stroke:#ff0000;stroke-width:1.19490099;stroke-linecap:butt;stroke-linejoin:miter;stroke-miterlimit:4;stroke-opacity:1;stroke-dasharray:none;display:inline" + sodipodi:nodetypes="cc" /> + d="m 29.767185,3.9554513 17.82295,8.7855817" + style="fill:#ff0000;stroke:#ff0000;stroke-width:1.19630003;stroke-linecap:butt;stroke-linejoin:miter;stroke-miterlimit:4;stroke-opacity:1;stroke-dasharray:none;display:inline" + sodipodi:nodetypes="cc" /> + d="M 29.774605,12.683025 47.630198,3.8801725" + style="fill:#ff0000;stroke:#ff0000;stroke-width:1.19484198;stroke-linecap:butt;stroke-linejoin:miter;stroke-miterlimit:4;stroke-opacity:1;stroke-dasharray:none;display:inline" + sodipodi:nodetypes="cc" /> + d="m 51.59088,18.071902 17.837782,8.731147" + style="fill:#ff0000;stroke:#ff0000;stroke-width:1.19476616;stroke-linecap:butt;stroke-linejoin:miter;stroke-miterlimit:4;stroke-opacity:1;stroke-dasharray:none;display:inline" + sodipodi:nodetypes="cc" /> + d="M 51.580734,26.818576 69.410873,18.158384" + style="fill:#ff0000;stroke:#ff0000;stroke-width:1.19167542;stroke-linecap:butt;stroke-linejoin:miter;stroke-miterlimit:4;stroke-opacity:1;stroke-dasharray:none;display:inline" + sodipodi:nodetypes="cc" /> + d="m 51.59088,18.071902 17.83838,8.809586" + style="fill:#ff0000;stroke:#ff0000;stroke-width:1.19476616;stroke-linecap:butt;stroke-linejoin:miter;stroke-miterlimit:4;stroke-opacity:1;stroke-dasharray:none;display:inline" + sodipodi:nodetypes="cc" /> + d="M 51.580734,26.818576 69.472946,18.119165" + style="fill:#ff0000;stroke:#ff0000;stroke-width:1.19167542;stroke-linecap:butt;stroke-linejoin:miter;stroke-miterlimit:4;stroke-opacity:1;stroke-dasharray:none;display:inline" + sodipodi:nodetypes="cc" /> + d="m 29.801735,18.056972 17.816818,8.754762" + style="fill:#ff0000;stroke:#ff0000;stroke-width:1.19840479;stroke-linecap:butt;stroke-linejoin:miter;stroke-miterlimit:4;stroke-opacity:1;stroke-dasharray:none;display:inline" + sodipodi:nodetypes="cc" /> + d="M 29.739026,26.85257 47.628333,18.112687" + style="fill:#ff0000;stroke:#ff0000;stroke-width:1.19068551;stroke-linecap:butt;stroke-linejoin:miter;stroke-miterlimit:4;stroke-opacity:1;stroke-dasharray:none;display:inline" + sodipodi:nodetypes="cc" /> + d="m 77.702794,38.300164 32.075816,8.825309" + style="fill:#ff0000;stroke:#ff0000;stroke-width:1.41732287;stroke-linecap:butt;stroke-linejoin:miter;stroke-miterlimit:4;stroke-opacity:1;stroke-dasharray:none;display:inline" + sodipodi:nodetypes="cc" /> + d="M 77.775534,47.107864 109.81707,38.374127" + style="fill:#ff0000;stroke:#ff0000;stroke-width:1.41732287;stroke-linecap:butt;stroke-linejoin:miter;stroke-miterlimit:4;stroke-opacity:1;stroke-dasharray:none;display:inline" + sodipodi:nodetypes="cc" /> + d="M 7.9615204,18.056455 25.840469,26.827764" + style="fill:#ff0000;stroke:#ff0000;stroke-width:1.19737208;stroke-linecap:butt;stroke-linejoin:miter;stroke-miterlimit:4;stroke-opacity:1;stroke-dasharray:none;display:inline" + sodipodi:nodetypes="cc" /> + d="M 7.9364397,26.848957 25.771399,18.172035" + style="fill:#ff0000;stroke:#ff0000;stroke-width:1.19791019;stroke-linecap:butt;stroke-linejoin:miter;stroke-miterlimit:4;stroke-opacity:1;stroke-dasharray:none;display:inline" + sodipodi:nodetypes="cc" /> + d="m 32.903038,38.329601 22.553035,8.772063" + style="fill:#ff0000;stroke:#ff0000;stroke-width:1.41732287;stroke-linecap:butt;stroke-linejoin:miter;stroke-miterlimit:4;stroke-opacity:1;stroke-dasharray:none;display:inline" + sodipodi:nodetypes="cc" /> + d="M 32.871366,47.123473 55.485522,38.281112" + style="fill:#ff0000;stroke:#ff0000;stroke-width:1.41732287;stroke-linecap:butt;stroke-linejoin:miter;stroke-miterlimit:4;stroke-opacity:1;stroke-dasharray:none;display:inline" + sodipodi:nodetypes="cc" /> + d="m 57.85366,38.087518 11.410467,9.1927" + style="fill:#ff0000;stroke:#ff0000;stroke-width:1.41732287;stroke-linecap:butt;stroke-linejoin:miter;stroke-miterlimit:4;stroke-opacity:1;stroke-dasharray:none;display:inline" + sodipodi:nodetypes="cc" /> + d="m 57.837196,47.205999 11.404975,-9.09581" + style="fill:#ff0000;stroke:#ff0000;stroke-width:1.41732287;stroke-linecap:butt;stroke-linejoin:miter;stroke-miterlimit:4;stroke-opacity:1;stroke-dasharray:none;display:inline" + sodipodi:nodetypes="cc" /> + d="M 7.5498082,52.475403 25.019735,61.594628" + style="fill:#ff0000;stroke:#ff0000;stroke-width:1.41732287;stroke-linecap:butt;stroke-linejoin:miter;stroke-miterlimit:4;stroke-opacity:1;stroke-dasharray:none;display:inline" + sodipodi:nodetypes="cc" /> + d="m 7.476225,61.518003 17.53748,-9.04343" + style="fill:#ff0000;stroke:#ff0000;stroke-width:1.41732287;stroke-linecap:butt;stroke-linejoin:miter;stroke-miterlimit:4;stroke-opacity:1;stroke-dasharray:none;display:inline" + sodipodi:nodetypes="cc" /> + d="m 27.859968,52.440362 17.517339,9.103546" + style="fill:#ff0000;stroke:#ff0000;stroke-width:1.41732287;stroke-linecap:butt;stroke-linejoin:miter;stroke-miterlimit:4;stroke-opacity:1;stroke-dasharray:none;display:inline" + sodipodi:nodetypes="cc" /> + d="M 27.809675,61.534279 45.3487,52.417168" + style="fill:#ff0000;stroke:#ff0000;stroke-width:1.41732287;stroke-linecap:butt;stroke-linejoin:miter;stroke-miterlimit:4;stroke-opacity:1;stroke-dasharray:none;display:inline" + sodipodi:nodetypes="cc" /> + d="m 55.345762,52.390536 22.271395,9.258692" + style="fill:#ff0000;stroke:#ff0000;stroke-width:1.36421418;stroke-linecap:butt;stroke-linejoin:miter;stroke-miterlimit:4;stroke-opacity:1;stroke-dasharray:none;marker-end:none;display:inline" + sodipodi:nodetypes="cc" /> + d="M 55.34692,61.535992 77.626653,52.32014" + style="fill:#ff0000;stroke:#ff0000;stroke-width:1.36357629;stroke-linecap:butt;stroke-linejoin:miter;stroke-miterlimit:4;stroke-opacity:1;stroke-dasharray:none;marker-end:none;display:inline" + sodipodi:nodetypes="cc" /> + d="M 7.3536543,66.894221 25.864561,75.610148" + style="fill:#ff0000;stroke:#ff0000;stroke-width:1.41732287;stroke-linecap:butt;stroke-linejoin:miter;stroke-miterlimit:4;stroke-opacity:1;stroke-dasharray:none;display:inline" + sodipodi:nodetypes="cc" /> + d="M 7.2898997,75.446981 25.947862,66.780317" + style="fill:#ff0000;stroke:#ff0000;stroke-width:1.41732287;stroke-linecap:butt;stroke-linejoin:miter;stroke-miterlimit:4;stroke-opacity:1;stroke-dasharray:none;display:inline" + sodipodi:nodetypes="cc" /> + d="M 7.431639,4.1245053 30.695783,12.997805" + style="fill:#ff0000;stroke:#ff0000;stroke-width:1.36696184;stroke-linecap:butt;stroke-linejoin:miter;stroke-miterlimit:4;stroke-opacity:1;stroke-dasharray:none;display:inline" + sodipodi:nodetypes="cc" /> + d="M 7.4395349,12.867421 30.774871,4.1608652" + style="fill:#ff0000;stroke:#ff0000;stroke-width:1.36225235;stroke-linecap:butt;stroke-linejoin:miter;stroke-miterlimit:4;stroke-opacity:1;stroke-dasharray:none;display:inline" + sodipodi:nodetypes="cc" /> + d="m 28.467187,75.543139 18.52199,-8.559226" + style="fill:#ff0000;stroke:#ff0000;stroke-width:1.41732287;stroke-linecap:butt;stroke-linejoin:miter;stroke-miterlimit:4;stroke-opacity:1;stroke-dasharray:none;display:inline" + sodipodi:nodetypes="cc" /> + d="m 28.485471,66.956843 18.446261,8.828211" + style="fill:#ff0000;stroke:#ff0000;stroke-width:1.41732287;stroke-linecap:butt;stroke-linejoin:miter;stroke-miterlimit:4;stroke-opacity:1;stroke-dasharray:none;display:inline" + sodipodi:nodetypes="cc" /> + d="M 70.524713,75.503806 88.987511,66.864708" + style="fill:#ff0000;stroke:#ff0000;stroke-width:1.41732287;stroke-linecap:butt;stroke-linejoin:miter;stroke-miterlimit:4;stroke-opacity:1;stroke-dasharray:none;display:inline" + sodipodi:nodetypes="cc" /> + d="m 70.587491,66.948946 18.391837,8.701908" + style="fill:#ff0000;stroke:#ff0000;stroke-width:1.41732287;stroke-linecap:butt;stroke-linejoin:miter;stroke-miterlimit:4;stroke-opacity:1;stroke-dasharray:none;display:inline" + sodipodi:nodetypes="cc" /> + d="M 49.577304,75.608168 67.82064,66.95305" + style="fill:#ff0000;stroke:#ff0000;stroke-width:1.41732287;stroke-linecap:butt;stroke-linejoin:miter;stroke-miterlimit:4;stroke-opacity:1;stroke-dasharray:none;display:inline" + sodipodi:nodetypes="cc" /> + d="m 49.568595,67.011059 18.339119,8.674658" + style="fill:#ff0000;stroke:#ff0000;stroke-width:1.41732287;stroke-linecap:butt;stroke-linejoin:miter;stroke-miterlimit:4;stroke-opacity:1;stroke-dasharray:none;display:inline" + sodipodi:nodetypes="cc" /> + d="M 91.332053,75.577785 109.83459,66.909922" + style="fill:#ff0000;stroke:#ff0000;stroke-width:1.41732287;stroke-linecap:butt;stroke-linejoin:miter;stroke-miterlimit:4;stroke-opacity:1;stroke-dasharray:none;display:inline" + sodipodi:nodetypes="cc" /> + d="m 91.383458,66.928045 18.469592,8.816982" + style="fill:#ff0000;stroke:#ff0000;stroke-width:1.41732287;stroke-linecap:butt;stroke-linejoin:miter;stroke-miterlimit:4;stroke-opacity:1;stroke-dasharray:none;display:inline" + sodipodi:nodetypes="cc" /> + ry="1.6" + rx="1.6" /> PATH + x="80.022575" + y="9.1953688">PATH PLAN + x="111.87186" + y="9.1953688">PLAN + + + + + + ATTI - STAB AIRSPD MAG + MAG - CPU + x="114.76076" + y="61.392075">CPU EVENT + x="87.079033" + y="60.440926">EVENT MEM + x="38.892857" + y="60.440926">MEM STACK + x="60.293201" + y="61.407112">STACK I2C + x="73.139465" + y="37.083118">I2C BOOT TELEMETRY + x="95.926994" + y="37.154144">TELEMETRY BATT + x="11.603705" + y="48.797535">BATT TIME + x="37.547398" + y="48.797535">TIME + style="fill:none;stroke:#ffffff;stroke-width:0.70866144;stroke-linecap:round;stroke-linejoin:round;stroke-miterlimit:4;stroke-opacity:1;stroke-dasharray:none;display:inline" + rx="1.1183628" /> INPUT + x="13.469692" + y="37.395981">INPUT + style="fill:none;stroke:#ffffff;stroke-width:0.70866144;stroke-linecap:round;stroke-linejoin:round;stroke-miterlimit:4;stroke-opacity:1;stroke-dasharray:none;display:inline" + rx="0.99532819" /> + style="display:none" + sodipodi:insensitive="true"> @@ -2867,7 +2921,8 @@ height="79.056633" x="0.27291748" y="0.2592113" - ry="1.6285406" /> + ry="1.6" + rx="1.6" /> @@ -2925,7 +2980,7 @@ id="g4267-4" style="display:inline"> diff --git a/ground/openpilotgcs/share/openpilotgcs/pfd/default/Compass.qml b/ground/openpilotgcs/share/openpilotgcs/pfd/default/Compass.qml index 198b9502c..2fb92cc19 100644 --- a/ground/openpilotgcs/share/openpilotgcs/pfd/default/Compass.qml +++ b/ground/openpilotgcs/share/openpilotgcs/pfd/default/Compass.qml @@ -37,6 +37,42 @@ Item { smooth: true } + SvgElementImage { + id: compass_home + elementName: "compass-home" // Cyan point + sceneSize: sceneItem.sceneSize + smooth: true + + x: Math.floor(scaledBounds.x * sceneItem.width) + y: Math.floor(scaledBounds.y * sceneItem.height) + + property real home_degrees: 180/3.1415 * Math.atan2(TakeOffLocation.East - PositionState.East, TakeOffLocation.North - PositionState.North) + + rotation: -AttitudeState.Yaw + home_degrees + transformOrigin: Item.Bottom + visible: TakeOffLocation.Status == 0 + + } + + SvgElementImage { + id: compass_waypoint // Double Purple arrow + elementName: "compass-waypoint" + sceneSize: sceneItem.sceneSize + + x: Math.floor(scaledBounds.x * sceneItem.width) + y: Math.floor(scaledBounds.y * sceneItem.height) + + property real course_degrees: 180/3.1415 * Math.atan2(PathDesired.End_East - PositionState.East, PathDesired.End_North - PositionState.North) + + rotation: -AttitudeState.Yaw + course_degrees + transformOrigin: Item.Center + + smooth: true + visible: PathDesired.End_East !== 0.0 && PathDesired.End_East !== 0.0 + } + + + Item { id: compass_text_box diff --git a/ground/openpilotgcs/share/openpilotgcs/pfd/default/HorizontCenter.qml b/ground/openpilotgcs/share/openpilotgcs/pfd/default/HorizontCenter.qml index c5f7c36f1..6dee7b633 100644 --- a/ground/openpilotgcs/share/openpilotgcs/pfd/default/HorizontCenter.qml +++ b/ground/openpilotgcs/share/openpilotgcs/pfd/default/HorizontCenter.qml @@ -10,6 +10,9 @@ Item { elementName: "center-arrows" sceneSize: background.sceneSize + width: Math.floor(scaledBounds.width * sceneItem.width) + height: Math.floor(scaledBounds.height * sceneItem.height) + x: Math.floor(scaledBounds.x * sceneItem.width) y: Math.floor(scaledBounds.y * sceneItem.height) } @@ -19,6 +22,9 @@ Item { elementName: "center-plane" sceneSize: background.sceneSize + width: Math.floor(scaledBounds.width * sceneItem.width) + height: Math.floor(scaledBounds.height * sceneItem.height) + x: Math.floor(scaledBounds.x * sceneItem.width) y: Math.floor(scaledBounds.y * sceneItem.height) } diff --git a/ground/openpilotgcs/share/openpilotgcs/pfd/default/Info.qml b/ground/openpilotgcs/share/openpilotgcs/pfd/default/Info.qml index 9dedc4365..699e0b2f0 100644 --- a/ground/openpilotgcs/share/openpilotgcs/pfd/default/Info.qml +++ b/ground/openpilotgcs/share/openpilotgcs/pfd/default/Info.qml @@ -4,10 +4,64 @@ Item { id: info property variant sceneSize + property real home_heading: 180/3.1415 * Math.atan2(TakeOffLocation.East - PositionState.East, + TakeOffLocation.North - PositionState.North) + + property real home_distance: Math.sqrt(Math.pow((TakeOffLocation.East - PositionState.East),2) + + Math.pow((TakeOffLocation.North - PositionState.North),2)) + + property real wp_heading: 180/3.1415 * Math.atan2(PathDesired.End_East - PositionState.East, + PathDesired.End_North - PositionState.North) + + property real wp_distance: Math.sqrt(Math.pow((PathDesired.End_East - PositionState.East),2) + + Math.pow(( PathDesired.End_North - PositionState.North),2)) + + property real current_velocity: Math.sqrt(Math.pow(VelocityState.North,2)+Math.pow(VelocityState.East,2)) + + property real home_eta: (home_distance > 0 && current_velocity > 0 ? Math.round(home_distance/current_velocity) : 0) + property real home_eta_h: (home_eta > 0 ? Math.floor(home_eta / 3600) : 0 ) + property real home_eta_m: (home_eta > 0 ? Math.floor((home_eta - home_eta_h*3600)/60) : 0) + property real home_eta_s: (home_eta > 0 ? Math.floor(home_eta - home_eta_h*3600 - home_eta_m*60) : 0) + + property real wp_eta: (wp_distance > 0 && current_velocity > 0 ? Math.round(wp_distance/current_velocity) : 0) + property real wp_eta_h: (wp_eta > 0 ? Math.floor(wp_eta / 3600) : 0 ) + property real wp_eta_m: (wp_eta > 0 ? Math.floor((wp_eta - wp_eta_h*3600)/60) : 0) + property real wp_eta_s: (wp_eta > 0 ? Math.floor(wp_eta - wp_eta_h*3600 - wp_eta_m*60) : 0) + + property real posEast_old + property real posNorth_old + property real total_distance + property bool init_dist: false + + function reset_distance(){ + total_distance = 0; + } + + function compute_distance(posEast,posNorth) { + if (total_distance == 0 && !init_dist){init_dist = "true"; posEast_old = posEast; posNorth_old = posNorth;} + if (posEast > posEast_old+3 || posEast < posEast_old-3 || posNorth > posNorth_old+3 || posNorth < posNorth_old-3) { + total_distance += Math.sqrt(Math.pow((posEast - posEast_old ),2) + Math.pow((posNorth - posNorth_old),2)); + + posEast_old = posEast; + posNorth_old = posNorth; + return total_distance; + } + } + + function formatTime(time) { + if (time === 0) + return "00" + if (time < 10) + return "0" + time; + else + return time.toString(); + } + SvgElementImage { id: info_bg - elementName: "info-bg" sceneSize: info.sceneSize + elementName: "info-bg" + width: parent.width } SvgElementImage { @@ -25,11 +79,6 @@ Item { } } - SvgElementImage { - id: energy_label - elementName: "battery-milliamp-label" - sceneSize: info.sceneSize - } Repeater { id: satNumberBar @@ -53,7 +102,7 @@ Item { Text { text: ["No GPS", "No Fix", "Fix2D", "Fix3D"][GPSPositionSensor.Status] anchors.centerIn: parent - font.pixelSize: parent.height*1.2 + font.pixelSize: Math.floor(parent.height*1.2) color: "white" } } @@ -66,7 +115,7 @@ Item { text: ["Disconnected","HandshakeReq","HandshakeAck","Connected"][GCSTelemetryStats.Status] anchors.centerIn: parent - font.pixelSize: parent.height*1.2 + font.pixelSize: Math.floor(parent.height*1.2) color: "white" } } @@ -99,6 +148,116 @@ Item { } } + SvgElementPositionItem { + sceneSize: info.sceneSize + elementName: "waypoint-heading-text" + width: scaledBounds.width * sceneItem.width + height: scaledBounds.height * sceneItem.height + y: Math.floor(scaledBounds.y * sceneItem.height) + visible: SystemAlarms.Alarm_PathPlan == 1 + + Text { + text: " "+wp_heading.toFixed(1)+"°" + + anchors.centerIn: parent + font.pixelSize: parent.height*1.1 + color: "magenta" + } + } + + SvgElementPositionItem { + sceneSize: info.sceneSize + elementName: "waypoint-distance-text" + width: scaledBounds.width * sceneItem.width + height: scaledBounds.height * sceneItem.height + y: Math.floor(scaledBounds.y * sceneItem.height) + visible: SystemAlarms.Alarm_PathPlan == 1 + + Text { + text: " "+wp_distance.toFixed(0)+" m" + + anchors.centerIn: parent + font.pixelSize: parent.height*1.1 + color: "magenta" + } + } + + SvgElementPositionItem { + sceneSize: info.sceneSize + elementName: "waypoint-total-distance-text" + width: scaledBounds.width * sceneItem.width + height: scaledBounds.height * sceneItem.height + y: Math.floor(scaledBounds.y * sceneItem.height) + visible: true + + MouseArea { id: total_dist_mouseArea; anchors.fill: parent; onClicked: reset_distance()} + + Text { + text: " "+total_distance.toFixed(0)+" m" + + anchors.centerIn: parent + font.pixelSize: parent.height*1.1 + color: "magenta" + } + + Timer { + interval: 1000; running: true; repeat: true; + onTriggered: {if (GPSPositionSensor.Status == 3) compute_distance(PositionState.East,PositionState.North)} + } + } + + SvgElementPositionItem { + sceneSize: info.sceneSize + elementName: "waypoint-eta-text" + width: scaledBounds.width * sceneItem.width + height: scaledBounds.height * sceneItem.height + y: Math.floor(scaledBounds.y * sceneItem.height) + visible: SystemAlarms.Alarm_PathPlan == 1 + + Text { + text: formatTime(wp_eta_h) + ":" + formatTime(wp_eta_m) + ":" + formatTime(wp_eta_s) + + anchors.centerIn: parent + font.pixelSize: parent.height*1.1 + color: "magenta" + } + } + + SvgElementPositionItem { + sceneSize: info.sceneSize + elementName: "waypoint-number-text" + width: scaledBounds.width * sceneItem.width + height: scaledBounds.height * sceneItem.height + y: Math.floor(scaledBounds.y * sceneItem.height) + visible: SystemAlarms.Alarm_PathPlan == 1 + + Text { + text: (WaypointActive.Index+1)+" / "+PathPlan.WaypointCount + + anchors.centerIn: parent + font.pixelSize: parent.height*1.1 + color: "magenta" + } + } + + SvgElementPositionItem { + sceneSize: info.sceneSize + elementName: "waypoint-mode-text" + width: scaledBounds.width * sceneItem.width + height: scaledBounds.height * sceneItem.height + y: Math.floor(scaledBounds.y * sceneItem.height) + visible: SystemAlarms.Alarm_PathPlan == 1 + + Text { + text: ["Fly End Point","Fly Vector","Fly Circle Right","Fly Circle Left","Drive End Point","Drive Vector","Drive Circle Right", + "Drive Circle Left","Fixed Attitude","Set Accessory","Land","Disarm Alarm"][PathDesired.Mode] + + anchors.centerIn: parent + font.pixelSize: parent.height*1.1 + color: "magenta" + } + } + SvgElementPositionItem { sceneSize: info.sceneSize elementName: "battery-volt-text" @@ -110,7 +269,7 @@ Item { color: "white" font { family: "Arial" - pixelSize: parent.height * 1.3 + pixelSize: Math.floor(parent.height * 1.2) } } } @@ -126,7 +285,7 @@ Item { color: "white" font { family: "Arial" - pixelSize: parent.height * 1.3 + pixelSize: Math.floor(parent.height * 1.2) } } } @@ -142,7 +301,7 @@ Item { color: "white" font { family: "Arial" - pixelSize: parent.height * 1.3 + pixelSize: Math.floor(parent.height * 1.2) } } } @@ -157,6 +316,7 @@ Item { property int minThrottleNumber : index+1 elementName: "eng" + minThrottleNumber sceneSize: info.sceneSize + visible: throttleNumberBar.throttleNumber >= minThrottleNumber } } @@ -184,4 +344,123 @@ Item { elementName: "rx-mask" sceneSize: info.sceneSize } + + SvgElementImage { + id: home_bg + elementName: "home-bg" + sceneSize: info.sceneSize + y: Math.floor(scaledBounds.y * sceneItem.height) + + states: State { + name: "fading" + when: TakeOffLocation.Status !== 0 + PropertyChanges { target: home_bg; x: Math.floor(scaledBounds.x * sceneItem.width) + home_bg.width; } + } + + transitions: Transition { + SequentialAnimation { + PropertyAnimation { property: "x"; duration: 800 } + } + } + } + + SvgElementPositionItem { + sceneSize: info.sceneSize + id: home_heading_text + elementName: "home-heading-text" + width: scaledBounds.width * sceneItem.width + height: scaledBounds.height * sceneItem.height + y: Math.floor(scaledBounds.y * sceneItem.height) + + states: State { + name: "fading_heading" + when: TakeOffLocation.Status !== 0 + PropertyChanges { target: home_heading_text; x: Math.floor(scaledBounds.x * sceneItem.width) + home_bg.width } + } + + transitions: Transition { + SequentialAnimation { + PropertyAnimation { property: "x"; duration: 800 } + } + } + Text { + text: " "+home_heading.toFixed(1)+"°" + anchors.centerIn: parent + color: "cyan" + font { + family: "Arial" + pixelSize: parent.height * 1.2 + } + } + } + + SvgElementPositionItem { + sceneSize: info.sceneSize + id: home_distance_text + elementName: "home-distance-text" + width: scaledBounds.width * sceneItem.width + height: scaledBounds.height * sceneItem.height + y: Math.floor(scaledBounds.y * sceneItem.height) + + states: State { + name: "fading_distance" + when: TakeOffLocation.Status !== 0 + PropertyChanges { target: home_distance_text; x: Math.floor(scaledBounds.x * sceneItem.width) + home_bg.width; } + } + + transitions: Transition { + SequentialAnimation { + PropertyAnimation { property: "x"; duration: 800 } + } + } + + Text { + text: home_distance.toFixed(0)+" m" + anchors.centerIn: parent + color: "cyan" + font { + family: "Arial" + pixelSize: parent.height * 1.2 + } + } + } + + SvgElementPositionItem { + sceneSize: info.sceneSize + id: home_eta_text + elementName: "home-eta-text" + width: scaledBounds.width * sceneItem.width + height: scaledBounds.height * sceneItem.height + y: Math.floor(scaledBounds.y * sceneItem.height) + + states: State { + name: "fading_distance" + when: TakeOffLocation.Status !== 0 + PropertyChanges { target: home_eta_text; x: Math.floor(scaledBounds.x * sceneItem.width) + home_bg.width; } + } + + transitions: Transition { + SequentialAnimation { + PropertyAnimation { property: "x"; duration: 800 } + } + } + + Text { + text: formatTime(home_eta_h) + ":" + formatTime(home_eta_m) + ":" + formatTime(home_eta_s) + anchors.centerIn: parent + color: "cyan" + font { + family: "Arial" + pixelSize: parent.height * 1.2 + } + } + } + + + SvgElementImage { + id: info_border + elementName: "info-border" + sceneSize: info.sceneSize + width: Math.floor(parent.width * 1.009) + } } diff --git a/ground/openpilotgcs/share/openpilotgcs/pfd/default/Pfd.qml b/ground/openpilotgcs/share/openpilotgcs/pfd/default/Pfd.qml index cca29f743..336a0617a 100644 --- a/ground/openpilotgcs/share/openpilotgcs/pfd/default/Pfd.qml +++ b/ground/openpilotgcs/share/openpilotgcs/pfd/default/Pfd.qml @@ -8,18 +8,29 @@ Rectangle { elementName: "pfd-window" fillMode: Image.PreserveAspectFit anchors.fill: parent - sceneSize: Qt.size(width, height) + + Rectangle { + width: Math.floor(parent.paintedHeight * 1.319) + height: Math.floor(parent.paintedHeight - parent.paintedHeight * 0.008) + + color: "transparent" + border.color: "white" + border.width: Math.floor(parent.paintedHeight * 0.008) + radius: Math.floor(parent.paintedHeight * 0.01) + anchors.centerIn: parent + } Item { id: sceneItem + + width: Math.floor((parent.paintedHeight * 1.32) - (parent.paintedHeight * 0.013)) + height: Math.floor(parent.paintedHeight - parent.paintedHeight * 0.02) property variant viewportSize : Qt.size(width, height) - width: parent.paintedWidth - height: parent.paintedHeight anchors.centerIn: parent clip: true - + Loader { id: worldLoader anchors.fill: parent @@ -39,14 +50,6 @@ Rectangle { anchors.fill: parent } - SvgElementImage { - id: foreground - elementName: "foreground" - sceneSize: sceneItem.viewportSize - - anchors.centerIn: parent - } - SvgElementImage { id: side_slip_fixed elementName: "sideslip-fixed" @@ -55,28 +58,6 @@ Rectangle { x: scaledBounds.x * sceneItem.width } - SvgElementImage { - id: side_slip - elementName: "sideslip-moving" - sceneSize: sceneItem.viewportSize - smooth: true - - property real sideSlip: AccelState.y - //smooth side slip changes, a low pass filter replacement - //accels are updated once per second - Behavior on sideSlip { - SmoothedAnimation { - duration: 1000 - velocity: -1 - } - } - - anchors.horizontalCenter: foreground.horizontalCenter - //0.5 coefficient is empirical to limit indicator movement - anchors.horizontalCenterOffset: sideSlip*width*0.1 //was 0.5 - y: scaledBounds.y * sceneItem.height - } - Compass { anchors.fill: parent sceneSize: sceneItem.viewportSize diff --git a/ground/openpilotgcs/share/openpilotgcs/pfd/default/PfdWorldView.qml b/ground/openpilotgcs/share/openpilotgcs/pfd/default/PfdWorldView.qml index eddc2f49f..b2b1d2e6a 100644 --- a/ground/openpilotgcs/share/openpilotgcs/pfd/default/PfdWorldView.qml +++ b/ground/openpilotgcs/share/openpilotgcs/pfd/default/PfdWorldView.qml @@ -12,7 +12,7 @@ Item { property variant scaledBounds: svgRenderer.scaledElementBounds("pfd.svg", "horizon") width: Math.round(sceneItem.width*scaledBounds.width/2)*2 - height: Math.round(sceneItem.height*scaledBounds.height/2)*2 + height: Math.round(sceneItem.height*scaledBounds.height/2)*3 property double pitch1DegScaledHeight: (svgRenderer.scaledElementBounds("pfd.svg", "pitch-90").y - svgRenderer.scaledElementBounds("pfd.svg", "pitch90").y)/180.0 @@ -49,6 +49,16 @@ Item { border: 1 smooth: true } + + SvgElementImage { + id: pitch_0 + elementName: "pitch0" + + sceneSize: background.sceneSize + anchors.centerIn: parent + border: 1 + smooth: true + } } Item { diff --git a/ground/openpilotgcs/share/openpilotgcs/pfd/default/RollScale.qml b/ground/openpilotgcs/share/openpilotgcs/pfd/default/RollScale.qml index d02023e3b..f5ab5df1a 100644 --- a/ground/openpilotgcs/share/openpilotgcs/pfd/default/RollScale.qml +++ b/ground/openpilotgcs/share/openpilotgcs/pfd/default/RollScale.qml @@ -13,8 +13,11 @@ Item { elementName: "roll-scale" sceneSize: sceneItem.sceneSize - x: Math.floor(scaledBounds.x * sceneItem.width) - y: Math.floor(scaledBounds.y * sceneItem.height) + width: scaledBounds.width * sceneItem.width + height: scaledBounds.height * sceneItem.height + + x: scaledBounds.x * sceneItem.width + y: scaledBounds.y * sceneItem.height smooth: true diff --git a/ground/openpilotgcs/share/openpilotgcs/pfd/default/Warnings.qml b/ground/openpilotgcs/share/openpilotgcs/pfd/default/Warnings.qml index 0f86755e0..82030e16b 100644 --- a/ground/openpilotgcs/share/openpilotgcs/pfd/default/Warnings.qml +++ b/ground/openpilotgcs/share/openpilotgcs/pfd/default/Warnings.qml @@ -10,6 +10,8 @@ Item { id: warning_bg elementName: "warnings-bg" sceneSize: warnings.sceneSize + width: background.width + anchors.bottom: parent.bottom } SvgElementPositionItem { @@ -26,7 +28,7 @@ Item { text: "RC INPUT" font { family: "Arial" - pixelSize: parent.height * 0.8 + pixelSize: Math.floor(parent.height * 0.8) weight: Font.DemiBold } } @@ -53,7 +55,7 @@ Item { text: "MASTER CAUTION" font { family: "Arial" - pixelSize: parent.height * 0.8 + pixelSize: Math.floor(parent.height * 0.8) weight: Font.DemiBold } } @@ -74,7 +76,7 @@ Item { text: "AUTOPILOT" font { family: "Arial" - pixelSize: parent.height * 0.8 + pixelSize: Math.floor(parent.height * 0.8) weight: Font.DemiBold } } @@ -101,7 +103,7 @@ Item { id: warning_battery elementName: "warning-battery" sceneSize: warnings.sceneSize - + anchors.right: parent.right visible: SystemAlarms.Alarm_Battery > 1 } @@ -109,7 +111,7 @@ Item { id: warning_attitude elementName: "warning-attitude" sceneSize: warnings.sceneSize - + anchors.centerIn: background.centerIn visible: SystemAlarms.Alarm_Attitude > 1 } } diff --git a/ground/openpilotgcs/share/openpilotgcs/pfd/default/pfd.svg b/ground/openpilotgcs/share/openpilotgcs/pfd/default/pfd.svg index ca9d72433..eddbc6a21 100644 --- a/ground/openpilotgcs/share/openpilotgcs/pfd/default/pfd.svg +++ b/ground/openpilotgcs/share/openpilotgcs/pfd/default/pfd.svg @@ -42,14 +42,6 @@ offset="1" id="stop5010" /> - - - + inkscape:snap-global="true" + inkscape:snap-intersection-paths="false" + inkscape:snap-object-midpoints="true"> image/svg+xml - + @@ -903,20 +897,22 @@ inkscape:groupmode="layer" id="layer25" inkscape:label="home-bg" - style="display:inline"> + style="display:inline" + sodipodi:insensitive="true"> - + + inkscape:connector-curvature="0" + sodipodi:nodetypes="scccss" /> + id="home-eta-label" + transform="matrix(1,0,0,1.0973877,0,-46.442937)"> + id="home-distance-label" + transform="matrix(1,0,0,1.0577142,0,-27.456636)"> + id="home-heading-label" + transform="matrix(1,0,0,1.0577142,0,-26.706351)"> + id="home-eta-text" + transform="matrix(1,0,0,0.99160769,0,2.0646588)"> + id="home-distance-text" + transform="matrix(1,0,0,0.99160769,0,2.0975587)"> + id="home-heading-text" + transform="matrix(1,0,0,0.99160769,15.28151,1.9884587)"> + transform="translate(0,-4)"> + transform="translate(0,4)"> - + + + + + - - - - - + x="-0.5" + y="-0.5" /> - - - - - - - - - - - + transform="matrix(1,0,0,1.0375459,-5.79738,-3.7697649)"> - + transform="matrix(1,0,0,1.0375459,0,-3.0939387)"> + id="waypoint-label" + transform="matrix(1.0375459,0,0,1.0375459,-7.161678,-3.5667345)"> + id="waypoint-heading-label" + transform="translate(0,-3.00017)"> + id="waypoint-distance-label" + transform="translate(0,-3.00017)"> + + + + + + + + + + id="waypoint-mode-label" + transform="translate(0,-3.113201)"> + id="waypoint-eta-label" + transform="matrix(1.0375459,0,0,1.0375459,-14.202279,-4.0166731)"> - + style="display:inline"> + style="display:inline"> + transform="translate(0,16.75)"> + style="display:inline"> + transform="translate(0,-1.231522)"> + style="display:inline"> + inkscape:label="waypoint-eta-text"> + transform="translate(0,0.595158)"> + inkscape:label="waypoint-distance-text"> + transform="translate(0,0.595158)"> + inkscape:label="waypoint-heading-text"> + transform="translate(2,0.595158)"> + inkscape:label="waypoint-mode-text"> + transform="translate(0,0.595158)"> @@ -1924,12 +1892,11 @@ inkscape:groupmode="layer" id="layer56" inkscape:label="waypoint-description-text" - style="display:inline" - sodipodi:insensitive="true"> + style="display:inline"> + transform="translate(30,0.595158)"> @@ -1985,21 +1952,67 @@ style="font-size:12px;fill:#ff00ff" id="path6364" inkscape:connector-curvature="0" /> + + + d="m 309.28568,10.810623 -1.70508,0 -0.49219,1.957031 1.7168,0 0.48047,-1.957031 m -0.87891,-3.3339842 -0.60938,2.4316406 1.71094,0 0.61524,-2.4316406 0.9375,0 -0.60352,2.4316406 1.82813,0 0,0.9023436 -2.05665,0 -0.48046,1.957031 1.86328,0 0,0.896485 -2.0918,0 -0.60937,2.425781 -0.9375,0 0.60351,-2.425781 -1.7168,0 -0.60351,2.425781 -0.94336,0 0.60937,-2.425781 -1.8457,0 0,-0.896485 2.0625,0 0.49219,-1.957031 -1.88672,0 0,-0.9023436 2.11523,0 0.59766,-2.4316406 0.94922,0" /> + + + + + d="m 303.29349,23.12117 c -0.60938,8e-6 -1.06836,0.300789 -1.37696,0.902344 -0.30469,0.597663 -0.45703,1.498052 -0.45703,2.701172 0,1.199222 0.15234,2.099611 0.45703,2.701172 0.3086,0.597657 0.76758,0.896485 1.37696,0.896484 0.61327,1e-6 1.07226,-0.298827 1.37695,-0.896484 0.30859,-0.601561 0.46289,-1.50195 0.46289,-2.701172 0,-1.20312 -0.1543,-2.103509 -0.46289,-2.701172 -0.30469,-0.601555 -0.76368,-0.902336 -1.37695,-0.902344 m 0,-0.9375 c 0.98046,9e-6 1.72851,0.38868 2.24414,1.166016 0.51952,0.773444 0.77929,1.898443 0.7793,3.375 -1e-5,1.472659 -0.25978,2.597658 -0.7793,3.375 -0.51563,0.773437 -1.26368,1.160156 -2.24414,1.160156 -0.98047,0 -1.73047,-0.386719 -2.25,-1.160156 -0.51563,-0.777342 -0.77344,-1.902341 -0.77344,-3.375 0,-1.476557 0.25781,-2.601556 0.77344,-3.375 0.51953,-0.777336 1.26953,-1.166007 2.25,-1.166016" /> + d="m 310.93411,23.12117 c -0.60938,8e-6 -1.06836,0.300789 -1.37695,0.902344 -0.30469,0.597663 -0.45703,1.498052 -0.45703,2.701172 0,1.199222 0.15234,2.099611 0.45703,2.701172 0.30859,0.597657 0.76757,0.896485 1.37695,0.896484 0.61328,1e-6 1.07226,-0.298827 1.37696,-0.896484 0.30858,-0.601561 0.46288,-1.50195 0.46289,-2.701172 -10e-6,-1.20312 -0.15431,-2.103509 -0.46289,-2.701172 -0.3047,-0.601555 -0.76368,-0.902336 -1.37696,-0.902344 m 0,-0.9375 c 0.98047,9e-6 1.72851,0.38868 2.24414,1.166016 0.51953,0.773444 0.77929,1.898443 0.7793,3.375 -1e-5,1.472659 -0.25977,2.597658 -0.7793,3.375 -0.51563,0.773437 -1.26367,1.160156 -2.24414,1.160156 -0.98047,0 -1.73047,-0.386719 -2.25,-1.160156 -0.51562,-0.777342 -0.77344,-1.902341 -0.77343,-3.375 -1e-5,-1.476557 0.25781,-2.601556 0.77343,-3.375 0.51953,-0.777336 1.26953,-1.166007 2.25,-1.166016" /> + + + + + + @@ -2385,14 +2398,12 @@ inkscape:groupmode="layer" id="layer57" inkscape:label="gps" - style="display:inline" - sodipodi:insensitive="true"> + style="display:inline"> + style="display:inline"> + transform="translate(0,1.5)"> + + + + + + + + + + + + + + + + + + + style="display:inline"> + style="display:inline"> - + - + + + + - + style="display:inline"> + + + + diff --git a/ground/openpilotgcs/src/plugins/config/config_cc_hw_widget.cpp b/ground/openpilotgcs/src/plugins/config/config_cc_hw_widget.cpp index 4fe823e6a..f13aca0e5 100644 --- a/ground/openpilotgcs/src/plugins/config/config_cc_hw_widget.cpp +++ b/ground/openpilotgcs/src/plugins/config/config_cc_hw_widget.cpp @@ -86,7 +86,7 @@ ConfigCCHWWidget::ConfigCCHWWidget(QWidget *parent) : ConfigTaskWidget(parent) HwSettings *hwSettings = HwSettings::GetInstance(getObjectManager()); HwSettings::DataFields hwSettingsData = hwSettings->getData(); - if (hwSettingsData.OptionalModules[HwSettings::OPTIONALMODULES_GPS] != HwSettings::OPTIONALMODULES_ENABLED) { + if(hwSettingsData.OptionalModules[HwSettings::OPTIONALMODULES_GPS] != HwSettings::OPTIONALMODULES_ENABLED) { m_telemetry->gpsProtocol->setEnabled(false); m_telemetry->gpsProtocol->setToolTip(tr("Enable GPS module and reboot the board to be able to select GPS protocol")); } else { diff --git a/ground/openpilotgcs/src/plugins/config/configrevowidget.cpp b/ground/openpilotgcs/src/plugins/config/configrevowidget.cpp index 54e1d1a9b..5c6feabed 100644 --- a/ground/openpilotgcs/src/plugins/config/configrevowidget.cpp +++ b/ground/openpilotgcs/src/plugins/config/configrevowidget.cpp @@ -32,9 +32,11 @@ #include #include #include - #include +#include +#include + #include "assertions.h" #include "calibration.h" #include "calibration/calibrationutils.h" @@ -79,6 +81,12 @@ ConfigRevoWidget::ConfigRevoWidget(QWidget *parent) : addApplySaveButtons(m_ui->revoCalSettingsSaveRAM, m_ui->revoCalSettingsSaveSD); + ExtensionSystem::PluginManager *pm = ExtensionSystem::PluginManager::instance(); + Core::Internal::GeneralSettings *settings = pm->getObject(); + if (!settings->useExpertMode()) { + m_ui->revoCalSettingsSaveRAM->setVisible(false); + } + // Initialization of the visual help m_ui->calibrationVisualHelp->setScene(new QGraphicsScene(this)); m_ui->calibrationVisualHelp->setRenderHint(QPainter::HighQualityAntialiasing, true); @@ -89,6 +97,10 @@ ConfigRevoWidget::ConfigRevoWidget(QWidget *parent) : // Must set up the UI (above) before setting up the UAVO mappings or refreshWidgetValues // will be dealing with some null pointers addUAVObject("HomeLocation"); + addUAVObject("RevoCalibration"); + addUAVObject("AttitudeSettings"); + addUAVObject("RevoSettings"); + addUAVObject("AccelGyroSettings"); autoLoadWidgets(); // accel calibration diff --git a/ground/openpilotgcs/src/plugins/pfdqml/pfdqmlgadgetwidget.cpp b/ground/openpilotgcs/src/plugins/pfdqml/pfdqmlgadgetwidget.cpp index 60b391505..e1978108d 100644 --- a/ground/openpilotgcs/src/plugins/pfdqml/pfdqmlgadgetwidget.cpp +++ b/ground/openpilotgcs/src/plugins/pfdqml/pfdqmlgadgetwidget.cpp @@ -63,7 +63,10 @@ PfdQmlGadgetWidget::PfdQmlGadgetWidget(QWindow *parent) : "SystemAlarms" << "NedAccel" << "FlightBatteryState" << - "ActuatorDesired"; + "ActuatorDesired" << + "TakeOffLocation" << + "PathPlan" << + "WaypointActive"; ExtensionSystem::PluginManager *pm = ExtensionSystem::PluginManager::instance(); UAVObjectManager *objManager = pm->getObject(); diff --git a/shared/uavobjectdefinition/systemalarms.xml b/shared/uavobjectdefinition/systemalarms.xml index 704b4f308..65cf57717 100644 --- a/shared/uavobjectdefinition/systemalarms.xml +++ b/shared/uavobjectdefinition/systemalarms.xml @@ -1,7 +1,7 @@ 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. - + SystemConfiguration BootFault @@ -24,7 +24,6 @@ FlightTime I2C GPS - Power