From 6d3ade4947d160ec3f196d97bccf1f9feed74cf5 Mon Sep 17 00:00:00 2001 From: Bertrand Oresve Date: Tue, 29 Apr 2014 22:25:49 +0200 Subject: [PATCH 01/19] OP-1308 Set the same logic for alarms --- flight/libraries/sanitycheck.c | 40 +++++----- .../fixedwingpathfollower.c | 4 +- flight/modules/ManualControl/armhandler.c | 2 +- flight/modules/PathPlanner/pathplanner.c | 2 +- flight/modules/Stabilization/stabilization.c | 2 +- flight/modules/StateEstimation/filterair.c | 6 +- .../modules/StateEstimation/filteraltitude.c | 77 +++++++------------ flight/modules/StateEstimation/filterbaro.c | 9 ++- flight/modules/StateEstimation/filtercf.c | 28 +++---- flight/modules/StateEstimation/filterekf.c | 14 ++-- flight/modules/StateEstimation/filterlla.c | 8 +- flight/modules/StateEstimation/filtermag.c | 6 +- .../StateEstimation/filterstationary.c | 6 +- .../StateEstimation/inc/stateestimation.h | 13 +++- .../modules/StateEstimation/stateestimation.c | 19 ++--- flight/modules/System/systemmod.c | 8 +- .../VtolPathFollower/vtolpathfollower.c | 6 +- shared/uavobjectdefinition/systemalarms.xml | 3 +- 18 files changed, 120 insertions(+), 133 deletions(-) diff --git a/flight/libraries/sanitycheck.c b/flight/libraries/sanitycheck.c index 04015dfb7..3b075d96b 100644 --- a/flight/libraries/sanitycheck.c +++ b/flight/libraries/sanitycheck.c @@ -94,7 +94,7 @@ int32_t configuration_check() switch (modes[i]) { case FLIGHTMODESETTINGS_FLIGHTMODEPOSITION_MANUAL: if (multirotor) { - severity = SYSTEMALARMS_ALARM_ERROR; + severity = SYSTEMALARMS_ALARM_CRITICAL; } break; case FLIGHTMODESETTINGS_FLIGHTMODEPOSITION_STABILIZED1: @@ -108,51 +108,51 @@ int32_t configuration_check() break; case FLIGHTMODESETTINGS_FLIGHTMODEPOSITION_AUTOTUNE: if (!PIOS_TASK_MONITOR_IsRunning(TASKINFO_RUNNING_AUTOTUNE)) { - severity = SYSTEMALARMS_ALARM_ERROR; + severity = SYSTEMALARMS_ALARM_CRITICAL; } break; case FLIGHTMODESETTINGS_FLIGHTMODEPOSITION_ALTITUDEHOLD: case FLIGHTMODESETTINGS_FLIGHTMODEPOSITION_ALTITUDEVARIO: if (coptercontrol) { - severity = SYSTEMALARMS_ALARM_ERROR; + severity = SYSTEMALARMS_ALARM_CRITICAL; } // TODO: put check equivalent to TASK_MONITOR_IsRunning // here as soon as available for delayed callbacks break; case FLIGHTMODESETTINGS_FLIGHTMODEPOSITION_VELOCITYCONTROL: if (coptercontrol) { - severity = SYSTEMALARMS_ALARM_ERROR; + severity = SYSTEMALARMS_ALARM_CRITICAL; } else if (!PIOS_TASK_MONITOR_IsRunning(TASKINFO_RUNNING_PATHFOLLOWER)) { // Revo supports altitude hold - severity = SYSTEMALARMS_ALARM_ERROR; + severity = SYSTEMALARMS_ALARM_CRITICAL; } break; case FLIGHTMODESETTINGS_FLIGHTMODEPOSITION_POSITIONHOLD: if (coptercontrol) { - severity = SYSTEMALARMS_ALARM_ERROR; + severity = SYSTEMALARMS_ALARM_CRITICAL; } else if (!PIOS_TASK_MONITOR_IsRunning(TASKINFO_RUNNING_PATHFOLLOWER)) { // Revo supports Position Hold - severity = SYSTEMALARMS_ALARM_ERROR; + severity = SYSTEMALARMS_ALARM_CRITICAL; } break; case FLIGHTMODESETTINGS_FLIGHTMODEPOSITION_LAND: if (coptercontrol) { - severity = SYSTEMALARMS_ALARM_ERROR; + severity = SYSTEMALARMS_ALARM_CRITICAL; } else if (!PIOS_TASK_MONITOR_IsRunning(TASKINFO_RUNNING_PATHFOLLOWER)) { // Revo supports AutoLand Mode - severity = SYSTEMALARMS_ALARM_ERROR; + severity = SYSTEMALARMS_ALARM_CRITICAL; } break; case FLIGHTMODESETTINGS_FLIGHTMODEPOSITION_POI: if (coptercontrol) { - severity = SYSTEMALARMS_ALARM_ERROR; + severity = SYSTEMALARMS_ALARM_CRITICAL; } else if (!PIOS_TASK_MONITOR_IsRunning(TASKINFO_RUNNING_PATHFOLLOWER)) { // Revo supports POI Mode - severity = SYSTEMALARMS_ALARM_ERROR; + severity = SYSTEMALARMS_ALARM_CRITICAL; } break; case FLIGHTMODESETTINGS_FLIGHTMODEPOSITION_PATHPLANNER: if (coptercontrol) { - severity = SYSTEMALARMS_ALARM_ERROR; + severity = SYSTEMALARMS_ALARM_CRITICAL; } else { // Revo supports PathPlanner and that must be OK or we are not sane // PathPlan alarm is uninitialized if not running @@ -160,21 +160,21 @@ int32_t configuration_check() SystemAlarmsAlarmData alarms; SystemAlarmsAlarmGet(&alarms); if (alarms.PathPlan != SYSTEMALARMS_ALARM_OK) { - severity = SYSTEMALARMS_ALARM_ERROR; + severity = SYSTEMALARMS_ALARM_CRITICAL; } } break; case FLIGHTMODESETTINGS_FLIGHTMODEPOSITION_RETURNTOBASE: if (coptercontrol) { - severity = SYSTEMALARMS_ALARM_ERROR; + severity = SYSTEMALARMS_ALARM_CRITICAL; } else if (!PIOS_TASK_MONITOR_IsRunning(TASKINFO_RUNNING_PATHFOLLOWER)) { // Revo supports ReturnToBase - severity = SYSTEMALARMS_ALARM_ERROR; + severity = SYSTEMALARMS_ALARM_CRITICAL; } break; default: // Uncovered modes are automatically an error - severity = SYSTEMALARMS_ALARM_ERROR; + severity = SYSTEMALARMS_ALARM_CRITICAL; } // mark the first encountered erroneous setting in status and substatus if ((severity != SYSTEMALARMS_ALARM_OK) && (alarmstatus == SYSTEMALARMS_EXTENDEDALARMSTATUS_NONE)) { @@ -197,14 +197,14 @@ int32_t configuration_check() * Checks the stabiliation settings for a paritcular mode and makes * sure it is appropriate for the airframe * @param[in] index Which stabilization mode to check - * @returns SYSTEMALARMS_ALARM_OK or SYSTEMALARMS_ALARM_ERROR + * @returns SYSTEMALARMS_ALARM_OK or SYSTEMALARMS_ALARM_CRITICAL */ static int32_t check_stabilization_settings(int index, bool multirotor) { // Make sure the modes have identical sizes if (FLIGHTMODESETTINGS_STABILIZATION1SETTINGS_NUMELEM != FLIGHTMODESETTINGS_STABILIZATION2SETTINGS_NUMELEM || FLIGHTMODESETTINGS_STABILIZATION1SETTINGS_NUMELEM != FLIGHTMODESETTINGS_STABILIZATION3SETTINGS_NUMELEM) { - return SYSTEMALARMS_ALARM_ERROR; + return SYSTEMALARMS_ALARM_CRITICAL; } uint8_t modes[FLIGHTMODESETTINGS_STABILIZATION1SETTINGS_NUMELEM]; @@ -221,14 +221,14 @@ static int32_t check_stabilization_settings(int index, bool multirotor) FlightModeSettingsStabilization3SettingsArrayGet(modes); break; default: - return SYSTEMALARMS_ALARM_ERROR; + return SYSTEMALARMS_ALARM_CRITICAL; } // For multirotors verify that nothing is set to "none" if (multirotor) { for (uint32_t i = 0; i < NELEMENTS(modes); i++) { if (modes[i] == FLIGHTMODESETTINGS_STABILIZATION1SETTINGS_NONE) { - return SYSTEMALARMS_ALARM_ERROR; + return SYSTEMALARMS_ALARM_CRITICAL; } } } diff --git a/flight/modules/FixedWingPathFollower/fixedwingpathfollower.c b/flight/modules/FixedWingPathFollower/fixedwingpathfollower.c index 9a2e52651..e61ad0da4 100644 --- a/flight/modules/FixedWingPathFollower/fixedwingpathfollower.c +++ b/flight/modules/FixedWingPathFollower/fixedwingpathfollower.c @@ -192,7 +192,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; @@ -220,7 +220,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 b16f1e39b..a642c5393 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 8cc50c4aa..32e582814 100644 --- a/flight/modules/PathPlanner/pathplanner.c +++ b/flight/modules/PathPlanner/pathplanner.c @@ -185,7 +185,7 @@ static void pathPlannerTask() pathDesired.Mode = PATHDESIRED_MODE_FLYENDPOINT; PathDesiredSet(&pathDesired); } - AlarmsSet(SYSTEMALARMS_ALARM_PATHPLAN, SYSTEMALARMS_ALARM_ERROR); + AlarmsSet(SYSTEMALARMS_ALARM_PATHPLAN, SYSTEMALARMS_ALARM_CRITICAL); return; } diff --git a/flight/modules/Stabilization/stabilization.c b/flight/modules/Stabilization/stabilization.c index 87c149ffe..6be3280ee 100644 --- a/flight/modules/Stabilization/stabilization.c +++ b/flight/modules/Stabilization/stabilization.c @@ -638,7 +638,7 @@ static void stabilizationTask(__attribute__((unused)) void *parameters) // Clear or set alarms. Done like this to prevent toggline each cycle // and hammering system alarms if (error) { - AlarmsSet(SYSTEMALARMS_ALARM_STABILIZATION, SYSTEMALARMS_ALARM_ERROR); + AlarmsSet(SYSTEMALARMS_ALARM_STABILIZATION, SYSTEMALARMS_ALARM_CRITICAL); } else { AlarmsClear(SYSTEMALARMS_ALARM_STABILIZATION); } diff --git a/flight/modules/StateEstimation/filterair.c b/flight/modules/StateEstimation/filterair.c index bf5c83a32..44d05313c 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 26f9598b4..4ffd7ecc7 100644 --- a/flight/modules/StateEstimation/filteraltitude.c +++ b/flight/modules/StateEstimation/filteraltitude.c @@ -38,23 +38,16 @@ // Private constants -// duration of accel bias initialization phase -#define INITIALIZATION_DURATION_MS 5000 - #define STACK_REQUIRED 128 #define DT_ALPHA 1e-2f #define DT_MIN 1e-6f #define DT_MAX 1.0f #define DT_AVERAGE 1e-3f -static volatile bool reloadSettings; // Private types struct data { - float altitudeState; // state = altitude,velocity,accel_offset,accel - float velocityState; - float accelBiasState; - float accelState; + float state[4]; // state = altitude,velocity,accel_offset,accel float pos[3]; // position updates from other filters float vel[3]; // position updates from other filters @@ -63,7 +56,6 @@ struct data { float accelLast; float baroLast; bool first_run; - portTickType initTimer; AltitudeFilterSettingsData settings; }; @@ -72,8 +64,7 @@ struct data { // Private functions static int32_t init(stateFilter *self); -static int32_t filter(stateFilter *self, stateEstimation *state); -static void settingsUpdatedCb(UAVObjEvent *ev); +static filterResult filter(stateFilter *self, stateEstimation *state); int32_t filterAltitudeInitialize(stateFilter *handle) @@ -83,8 +74,6 @@ int32_t filterAltitudeInitialize(stateFilter *handle) handle->localdata = pvPortMalloc(sizeof(struct data)); AttitudeStateInitialize(); AltitudeFilterSettingsInitialize(); - AltitudeFilterSettingsConnectCallback(&settingsUpdatedCb); - reloadSettings = true; return STACK_REQUIRED; } @@ -92,10 +81,10 @@ static int32_t init(stateFilter *self) { struct data *this = (struct data *)self->localdata; - this->altitudeState = 0.0f; - this->velocityState = 0.0f; - this->accelBiasState = 0.0f; - this->accelState = 0.0f; + this->state[0] = 0.0f; + this->state[1] = 0.0f; + this->state[2] = 0.0f; + this->state[3] = 0.0f; this->pos[0] = 0.0f; this->pos[1] = 0.0f; this->pos[2] = 0.0f; @@ -107,23 +96,18 @@ static int32_t init(stateFilter *self) this->baroLast = 0.0f; this->accelLast = 0.0f; this->first_run = 1; + AltitudeFilterSettingsGet(&this->settings); return 0; } -static int32_t filter(stateFilter *self, stateEstimation *state) +static filterResult filter(stateFilter *self, stateEstimation *state) { struct data *this = (struct data *)self->localdata; - if (reloadSettings) { - reloadSettings = false; - AltitudeFilterSettingsGet(&this->settings); - } - if (this->first_run) { // Initialize to current altitude reading at initial location if (IS_SET(state->updated, SENSORUPDATES_baro)) { this->first_run = 0; - this->initTimer = xTaskGetTickCount(); } } else { // save existing position and velocity updates so GPS will still work @@ -131,13 +115,13 @@ static int32_t filter(stateFilter *self, stateEstimation *state) this->pos[0] = state->pos[0]; this->pos[1] = state->pos[1]; this->pos[2] = state->pos[2]; - state->pos[2] = -this->altitudeState; + state->pos[2] = -this->state[0]; } if (IS_SET(state->updated, SENSORUPDATES_vel)) { this->vel[0] = state->vel[0]; this->vel[1] = state->vel[1]; this->vel[2] = state->vel[2]; - state->vel[2] = -this->velocityState; + state->vel[2] = -this->state[1]; } if (IS_SET(state->updated, SENSORUPDATES_accel)) { // rotate accels into global coordinate frame @@ -148,67 +132,58 @@ static int32_t filter(stateFilter *self, stateEstimation *state) float current = -(Rbe[0][2] * state->accel[0] + Rbe[1][2] * state->accel[1] + Rbe[2][2] * state->accel[2] + 9.81f); // low pass filter accelerometers - this->accelState = (1.0f - this->settings.AccelLowPassKp) * this->accelState + this->settings.AccelLowPassKp * current; - if (((xTaskGetTickCount() - this->initTimer) / portTICK_RATE_MS) < INITIALIZATION_DURATION_MS) { - // allow the offset to reach quickly the target value in case of small AccelDriftKi - this->accelBiasState = (1.0f - this->settings.InitializationAccelDriftKi) * this->accelBiasState + this->settings.InitializationAccelDriftKi * this->accelState; - } else { - // correct accel offset (low pass zeroing) - this->accelBiasState = (1.0f - this->settings.AccelDriftKi) * this->accelBiasState + this->settings.AccelDriftKi * this->accelState; - } + this->state[3] = (1.0f - this->settings.AccelLowPassKp) * this->state[3] + this->settings.AccelLowPassKp * current; + + // correct accel offset (low pass zeroing) + this->state[2] = (1.0f - this->settings.AccelDriftKi) * this->state[2] + this->settings.AccelDriftKi * this->state[3]; + // correct velocity and position state (integration) // low pass for average dT, compensate timing jitter from scheduler // float dT = PIOS_DELTATIME_GetAverageSeconds(&this->dt1config); - float speedLast = this->velocityState; + float speedLast = this->state[1]; - this->velocityState += 0.5f * (this->accelLast + (this->accelState - this->accelBiasState)) * dT; - this->accelLast = this->accelState - this->accelBiasState; + this->state[1] += 0.5f * (this->accelLast + (this->state[3] - this->state[2])) * dT; + this->accelLast = this->state[3] - this->state[2]; - this->altitudeState += 0.5f * (speedLast + this->velocityState) * dT; + this->state[0] += 0.5f * (speedLast + this->state[1]) * dT; state->pos[0] = this->pos[0]; state->pos[1] = this->pos[1]; - state->pos[2] = -this->altitudeState; + state->pos[2] = -this->state[0]; state->updated |= SENSORUPDATES_pos; state->vel[0] = this->vel[0]; state->vel[1] = this->vel[1]; - state->vel[2] = -this->velocityState; + state->vel[2] = -this->state[1]; state->updated |= SENSORUPDATES_vel; } if (IS_SET(state->updated, SENSORUPDATES_baro)) { // correct the altitude state (simple low pass) - this->altitudeState = (1.0f - this->settings.BaroKp) * this->altitudeState + this->settings.BaroKp * state->baro[0]; + this->state[0] = (1.0f - this->settings.BaroKp) * this->state[0] + this->settings.BaroKp * state->baro[0]; // correct the velocity state (low pass differentiation) // low pass for average dT, compensate timing jitter from scheduler float dT = PIOS_DELTATIME_GetAverageSeconds(&this->dt2config); - this->velocityState = (1.0f - (this->settings.BaroKp * this->settings.BaroKp)) * this->velocityState + (this->settings.BaroKp * this->settings.BaroKp) * (state->baro[0] - this->baroLast) / dT; + this->state[1] = (1.0f - (this->settings.BaroKp * this->settings.BaroKp)) * this->state[1] + (this->settings.BaroKp * this->settings.BaroKp) * (state->baro[0] - this->baroLast) / dT; this->baroLast = state->baro[0]; state->pos[0] = this->pos[0]; state->pos[1] = this->pos[1]; - state->pos[2] = -this->altitudeState; + state->pos[2] = -this->state[0]; state->updated |= SENSORUPDATES_pos; state->vel[0] = this->vel[0]; state->vel[1] = this->vel[1]; - state->vel[2] = -this->velocityState; + state->vel[2] = -this->state[1]; state->updated |= SENSORUPDATES_vel; } } - return 0; + return FILTERRESULT_OK; } -void settingsUpdatedCb(UAVObjEvent *ev) -{ - if (ev->obj == AltitudeFilterSettingsHandle()) { - reloadSettings = true; - } -} /** * @} diff --git a/flight/modules/StateEstimation/filterbaro.c b/flight/modules/StateEstimation/filterbaro.c index 4715d7762..82654e488 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 992eef8ef..d15e38c68 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; - 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; @@ -343,7 +343,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 @@ -354,7 +354,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); @@ -447,13 +447,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 0aa906dda..ee9530126 100644 --- a/flight/modules/StateEstimation/filterekf.c +++ b/flight/modules/StateEstimation/filterekf.c @@ -82,7 +82,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); @@ -192,7 +192,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; @@ -221,7 +221,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); @@ -315,11 +315,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]) }; @@ -427,9 +427,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 644c48eab..33fcfdc69 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 7997016e0..e1b388ab6 100644 --- a/flight/modules/StateEstimation/filtermag.c +++ b/flight/modules/StateEstimation/filtermag.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); static void magOffsetEstimation(struct data *this, float mag[3]); @@ -76,7 +76,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; @@ -86,7 +86,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..de38713c0 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 5a146475c..c6878fb62 100644 --- a/flight/modules/StateEstimation/stateestimation.c +++ b/flight/modules/StateEstimation/stateestimation.c @@ -223,6 +223,7 @@ static const filterPipeline *ekf13iQueue = &(filterPipeline) { } } }; + static const filterPipeline *ekf13Queue = &(filterPipeline) { .filter = &magFilter, .next = &(filterPipeline) { @@ -333,8 +334,8 @@ 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 filterResult alarm = FILTERRESULT_OK; + static filterResult lastAlarm = FILTERRESULT_UNINITIALISED; static uint16_t alarmcounter = 0; static const filterPipeline *current; static stateEstimation states; @@ -351,12 +352,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(); @@ -436,7 +437,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; } @@ -487,12 +488,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 648e2d3a0..419a56e42 100644 --- a/flight/modules/System/systemmod.c +++ b/flight/modules/System/systemmod.c @@ -246,10 +246,10 @@ static void systemTask(__attribute__((unused)) void *parameters) // Turn on the error LED if an alarm is set #if defined(PIOS_LED_ALARM) - if (AlarmsHasCritical()) { + if (AlarmsHasErrors()) { PIOS_LED_On(PIOS_LED_ALARM); - } else if ((AlarmsHasErrors() && (cycleCount & 0x1)) || - (!AlarmsHasErrors() && AlarmsHasWarnings() && (cycleCount & 0x4))) { + } else if ((AlarmsHasCritical() && (cycleCount & 0x1)) || + (!AlarmsHasCritical() && AlarmsHasWarnings() && (cycleCount & 0x4))) { PIOS_LED_On(PIOS_LED_ALARM); } else { PIOS_LED_Off(PIOS_LED_ALARM); @@ -429,7 +429,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); } } diff --git a/flight/modules/VtolPathFollower/vtolpathfollower.c b/flight/modules/VtolPathFollower/vtolpathfollower.c index 3c65e1b29..ad58066e3 100644 --- a/flight/modules/VtolPathFollower/vtolpathfollower.c +++ b/flight/modules/VtolPathFollower/vtolpathfollower.c @@ -214,7 +214,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) { @@ -222,7 +222,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 { @@ -247,7 +247,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); diff --git a/shared/uavobjectdefinition/systemalarms.xml b/shared/uavobjectdefinition/systemalarms.xml index e1fcf6d32..cfab551bf 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 @@ -23,7 +23,6 @@ FlightTime I2C GPS - Power From 70ba6850d059c2aa97451f011dbdaa9bee926247 Mon Sep 17 00:00:00 2001 From: Bertrand Oresve Date: Wed, 30 Apr 2014 08:49:50 +0200 Subject: [PATCH 02/19] OP-1308 Uncrustify operation --- flight/modules/ManualControl/manualcontrol.c | 2 +- flight/modules/StateEstimation/filtercf.c | 4 ++-- flight/modules/StateEstimation/inc/stateestimation.h | 10 +++++----- flight/modules/StateEstimation/stateestimation.c | 4 ++-- 4 files changed, 10 insertions(+), 10 deletions(-) diff --git a/flight/modules/ManualControl/manualcontrol.c b/flight/modules/ManualControl/manualcontrol.c index 4de61af46..a721b0a91 100644 --- a/flight/modules/ManualControl/manualcontrol.c +++ b/flight/modules/ManualControl/manualcontrol.c @@ -113,7 +113,7 @@ static const controlHandler handler_PATHPLANNER = { .handler = &pathPlannerHandler, }; -#endif +#endif /* ifndef PIOS_EXCLUDE_ADVANCED_FEATURES */ // Private variables static DelayedCallbackInfo *callbackHandle; diff --git a/flight/modules/StateEstimation/filtercf.c b/flight/modules/StateEstimation/filtercf.c index d15e38c68..70e98d981 100644 --- a/flight/modules/StateEstimation/filtercf.c +++ b/flight/modules/StateEstimation/filtercf.c @@ -167,9 +167,9 @@ static int32_t maininit(stateFilter *self) */ static filterResult filter(stateFilter *self, stateEstimation *state) { - struct data *this = (struct data *)self->localdata; + struct data *this = (struct data *)self->localdata; - filterResult result = FILTERRESULT_OK; + filterResult result = FILTERRESULT_OK; if (IS_SET(state->updated, SENSORUPDATES_mag)) { this->magUpdated = 1; diff --git a/flight/modules/StateEstimation/inc/stateestimation.h b/flight/modules/StateEstimation/inc/stateestimation.h index de38713c0..d93235f4e 100644 --- a/flight/modules/StateEstimation/inc/stateestimation.h +++ b/flight/modules/StateEstimation/inc/stateestimation.h @@ -35,11 +35,11 @@ // Enumeration for filter result typedef enum { - FILTERRESULT_UNINITIALISED=-1, - FILTERRESULT_OK=0, - FILTERRESULT_WARNING=1, - FILTERRESULT_CRITICAL=2, - FILTERRESULT_ERROR=3, + FILTERRESULT_UNINITIALISED = -1, + FILTERRESULT_OK = 0, + FILTERRESULT_WARNING = 1, + FILTERRESULT_CRITICAL = 2, + FILTERRESULT_ERROR = 3, } filterResult; diff --git a/flight/modules/StateEstimation/stateestimation.c b/flight/modules/StateEstimation/stateestimation.c index c6878fb62..5b71b252d 100644 --- a/flight/modules/StateEstimation/stateestimation.c +++ b/flight/modules/StateEstimation/stateestimation.c @@ -334,9 +334,9 @@ MODULE_INITCALL(StateEstimationInitialize, StateEstimationStart); static void StateEstimationCb(void) { static enum { RUNSTATE_LOAD = 0, RUNSTATE_FILTER = 1, RUNSTATE_SAVE = 2 } runState = RUNSTATE_LOAD; - static filterResult alarm = FILTERRESULT_OK; + static filterResult alarm = FILTERRESULT_OK; static filterResult lastAlarm = FILTERRESULT_UNINITIALISED; - static uint16_t alarmcounter = 0; + static uint16_t alarmcounter = 0; static const filterPipeline *current; static stateEstimation states; static uint32_t last_time; From 743cf4e4b58f43ac69b11a4ad38a916ba6d03ff0 Mon Sep 17 00:00:00 2001 From: Bertrand Oresve Date: Thu, 8 May 2014 22:33:45 +0200 Subject: [PATCH 03/19] OP-1308 Merge with next (conflict with stabilization) --- flight/libraries/sanitycheck.c | 1 + flight/modules/Stabilization/innerloop.c | 12 ++++++------ flight/modules/Stabilization/stabilization.c | 1 + 3 files changed, 8 insertions(+), 6 deletions(-) diff --git a/flight/libraries/sanitycheck.c b/flight/libraries/sanitycheck.c index 52134794b..939851d84 100644 --- a/flight/libraries/sanitycheck.c +++ b/flight/libraries/sanitycheck.c @@ -39,6 +39,7 @@ #include #include + /**************************** * Current checks: * 1. If a flight mode switch allows autotune and autotune module not running 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 15b8c3caf..2180ee9c4 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 From a46a1e09fb106b1fc4e8cf1d8cc047167e475ae2 Mon Sep 17 00:00:00 2001 From: Laurent Lalanne Date: Mon, 16 Jun 2014 22:03:23 +0200 Subject: [PATCH 04/19] OP-1339 System Health fixes : Alignments, text center, text sizes, round corners over layers, red cross. --- .../diagrams/default/system-health.svg | 493 ++++++++++-------- 1 file changed, 274 insertions(+), 219 deletions(-) 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"> From 8988e4901b715d5bb8c674ce8fff74a4ede9dcac Mon Sep 17 00:00:00 2001 From: Laurent Lalanne Date: Tue, 17 Jun 2014 14:29:24 +0200 Subject: [PATCH 05/19] OP-1354 Current_voltage_fixes_and_more_in_PFD : Solid border with round corners, better resizing --- .../share/openpilotgcs/pfd/default/Info.qml | 27 +- .../share/openpilotgcs/pfd/default/Pfd.qml | 20 +- .../openpilotgcs/pfd/default/Warnings.qml | 8 +- .../share/openpilotgcs/pfd/default/pfd.svg | 296 +++++++++--------- 4 files changed, 187 insertions(+), 164 deletions(-) diff --git a/ground/openpilotgcs/share/openpilotgcs/pfd/default/Info.qml b/ground/openpilotgcs/share/openpilotgcs/pfd/default/Info.qml index 9dedc4365..346ce891b 100644 --- a/ground/openpilotgcs/share/openpilotgcs/pfd/default/Info.qml +++ b/ground/openpilotgcs/share/openpilotgcs/pfd/default/Info.qml @@ -6,8 +6,10 @@ Item { SvgElementImage { id: info_bg - elementName: "info-bg" sceneSize: info.sceneSize + elementName: "info-bg" + width: parent.width + anchors.fill: parent.top } SvgElementImage { @@ -25,11 +27,6 @@ Item { } } - SvgElementImage { - id: energy_label - elementName: "battery-milliamp-label" - sceneSize: info.sceneSize - } Repeater { id: satNumberBar @@ -53,7 +50,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 +63,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" } } @@ -110,7 +107,7 @@ Item { color: "white" font { family: "Arial" - pixelSize: parent.height * 1.3 + pixelSize: Math.floor(parent.height * 1.2) } } } @@ -126,7 +123,7 @@ Item { color: "white" font { family: "Arial" - pixelSize: parent.height * 1.3 + pixelSize: Math.floor(parent.height * 1.2) } } } @@ -142,7 +139,7 @@ Item { color: "white" font { family: "Arial" - pixelSize: parent.height * 1.3 + pixelSize: Math.floor(parent.height * 1.2) } } } @@ -184,4 +181,12 @@ Item { elementName: "rx-mask" sceneSize: info.sceneSize } + + SvgElementImage { + id: info_border + elementName: "info-border" + sceneSize: info.sceneSize + width: Math.floor(parent.width * 1.01) + anchors.fill: parent.top + } } diff --git a/ground/openpilotgcs/share/openpilotgcs/pfd/default/Pfd.qml b/ground/openpilotgcs/share/openpilotgcs/pfd/default/Pfd.qml index cca29f743..46eb673b6 100644 --- a/ground/openpilotgcs/share/openpilotgcs/pfd/default/Pfd.qml +++ b/ground/openpilotgcs/share/openpilotgcs/pfd/default/Pfd.qml @@ -8,18 +8,32 @@ Rectangle { elementName: "pfd-window" fillMode: Image.PreserveAspectFit anchors.fill: parent - sceneSize: Qt.size(width, height) + + Rectangle { + width: Math.floor(parent.paintedHeight * 1.32) + 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.015)) + 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 + //onWidthChanged:console.log("Width/Height : "+width+" "+ height+" scale : "+width/height+" border : "+parent.paintedHeight * 0.006 ) + Loader { id: worldLoader anchors.fill: parent diff --git a/ground/openpilotgcs/share/openpilotgcs/pfd/default/Warnings.qml b/ground/openpilotgcs/share/openpilotgcs/pfd/default/Warnings.qml index 0f86755e0..50ac85398 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: parent.width + 4 + anchors.fill: 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 } } diff --git a/ground/openpilotgcs/share/openpilotgcs/pfd/default/pfd.svg b/ground/openpilotgcs/share/openpilotgcs/pfd/default/pfd.svg index ca9d72433..233d90cf8 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" /> - - - image/svg+xml - + @@ -1136,57 +1128,48 @@ id="layer3" inkscape:label="info" style="display:inline" - transform="translate(0,-4)" - sodipodi:insensitive="true"> + 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)"> - + style="display:inline"> + style="display:inline"> + transform="translate(0,16.75)"> + style="display:inline"> + transform="translate(0,-1.231522)"> + + + + + + + + + + + + + + + + + + + style="display:inline"> + style="display:inline"> + style="display:none"> - + style="display:inline"> + + + + + style="display:inline"> @@ -4858,8 +4861,7 @@ inkscape:groupmode="layer" id="layer33" inkscape:label="warning-rc-input" - style="display:inline" - sodipodi:insensitive="true"> + style="display:inline"> @@ -5075,12 +5077,12 @@ transform="translate(0,-4)" sodipodi:insensitive="true"> From 98af0a544ff543b0ebe90d6557bcfec45b970177 Mon Sep 17 00:00:00 2001 From: Laurent Lalanne Date: Tue, 17 Jun 2014 19:37:09 +0200 Subject: [PATCH 06/19] OP-1354 Current_voltage_fixes_and_more_in_PFD : Minor changes to remove some glitches --- .../openpilotgcs/share/openpilotgcs/pfd/default/Info.qml | 6 +++--- ground/openpilotgcs/share/openpilotgcs/pfd/default/Pfd.qml | 4 ++-- .../share/openpilotgcs/pfd/default/Warnings.qml | 7 +++---- 3 files changed, 8 insertions(+), 9 deletions(-) diff --git a/ground/openpilotgcs/share/openpilotgcs/pfd/default/Info.qml b/ground/openpilotgcs/share/openpilotgcs/pfd/default/Info.qml index 346ce891b..d7cafba26 100644 --- a/ground/openpilotgcs/share/openpilotgcs/pfd/default/Info.qml +++ b/ground/openpilotgcs/share/openpilotgcs/pfd/default/Info.qml @@ -9,7 +9,7 @@ Item { sceneSize: info.sceneSize elementName: "info-bg" width: parent.width - anchors.fill: parent.top + //anchors.top: parent.top } SvgElementImage { @@ -154,6 +154,7 @@ Item { property int minThrottleNumber : index+1 elementName: "eng" + minThrottleNumber sceneSize: info.sceneSize + visible: throttleNumberBar.throttleNumber >= minThrottleNumber } } @@ -186,7 +187,6 @@ Item { id: info_border elementName: "info-border" sceneSize: info.sceneSize - width: Math.floor(parent.width * 1.01) - anchors.fill: parent.top + 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 46eb673b6..851070e5f 100644 --- a/ground/openpilotgcs/share/openpilotgcs/pfd/default/Pfd.qml +++ b/ground/openpilotgcs/share/openpilotgcs/pfd/default/Pfd.qml @@ -11,7 +11,7 @@ Rectangle { sceneSize: Qt.size(width, height) Rectangle { - width: Math.floor(parent.paintedHeight * 1.32) + width: Math.floor(parent.paintedHeight * 1.319) height: Math.floor(parent.paintedHeight - parent.paintedHeight * 0.008) color: "transparent" @@ -25,7 +25,7 @@ Rectangle { id: sceneItem - width: Math.floor((parent.paintedHeight * 1.32) - (parent.paintedHeight * 0.015)) + 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) diff --git a/ground/openpilotgcs/share/openpilotgcs/pfd/default/Warnings.qml b/ground/openpilotgcs/share/openpilotgcs/pfd/default/Warnings.qml index 50ac85398..8e2fc89d1 100644 --- a/ground/openpilotgcs/share/openpilotgcs/pfd/default/Warnings.qml +++ b/ground/openpilotgcs/share/openpilotgcs/pfd/default/Warnings.qml @@ -10,8 +10,8 @@ Item { id: warning_bg elementName: "warnings-bg" sceneSize: warnings.sceneSize - width: parent.width + 4 - anchors.fill: parent.bottom + width: background.width + anchors.bottom: parent.bottom } SvgElementPositionItem { @@ -103,7 +103,7 @@ Item { id: warning_battery elementName: "warning-battery" sceneSize: warnings.sceneSize - + anchors.right: parent.right visible: SystemAlarms.Alarm_Battery > 1 } @@ -111,7 +111,6 @@ Item { id: warning_attitude elementName: "warning-attitude" sceneSize: warnings.sceneSize - visible: SystemAlarms.Alarm_Attitude > 1 } } From 29d0d31e0948a22fe17c7ae988d259642e7096c7 Mon Sep 17 00:00:00 2001 From: Laurent Lalanne Date: Thu, 19 Jun 2014 00:49:45 +0200 Subject: [PATCH 07/19] OP-1354 PFD fixes : World size (grey background appears when board is tilted) + Removed empty foreground item (add flickering on render) --- .../openpilotgcs/share/openpilotgcs/pfd/default/Pfd.qml | 8 -------- .../share/openpilotgcs/pfd/default/PfdWorldView.qml | 2 +- 2 files changed, 1 insertion(+), 9 deletions(-) diff --git a/ground/openpilotgcs/share/openpilotgcs/pfd/default/Pfd.qml b/ground/openpilotgcs/share/openpilotgcs/pfd/default/Pfd.qml index 851070e5f..3e0e7be5c 100644 --- a/ground/openpilotgcs/share/openpilotgcs/pfd/default/Pfd.qml +++ b/ground/openpilotgcs/share/openpilotgcs/pfd/default/Pfd.qml @@ -53,14 +53,6 @@ Rectangle { anchors.fill: parent } - SvgElementImage { - id: foreground - elementName: "foreground" - sceneSize: sceneItem.viewportSize - - anchors.centerIn: parent - } - SvgElementImage { id: side_slip_fixed elementName: "sideslip-fixed" diff --git a/ground/openpilotgcs/share/openpilotgcs/pfd/default/PfdWorldView.qml b/ground/openpilotgcs/share/openpilotgcs/pfd/default/PfdWorldView.qml index eddc2f49f..5c8baa1ca 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 From 5dc0cb7d8e749e15f67d7792159361b95ced0b58 Mon Sep 17 00:00:00 2001 From: Laurent Lalanne Date: Thu, 19 Jun 2014 20:50:47 +0200 Subject: [PATCH 08/19] OP-1354 Fixes center-arrows position on resize, disable sideslip moving (flicker issue on SmoothedAnimation) --- .../share/openpilotgcs/pfd/default/HorizontCenter.qml | 6 ++++++ ground/openpilotgcs/share/openpilotgcs/pfd/default/Pfd.qml | 5 ++++- .../share/openpilotgcs/pfd/default/RollScale.qml | 7 +++++-- 3 files changed, 15 insertions(+), 3 deletions(-) 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/Pfd.qml b/ground/openpilotgcs/share/openpilotgcs/pfd/default/Pfd.qml index 3e0e7be5c..c8c85c035 100644 --- a/ground/openpilotgcs/share/openpilotgcs/pfd/default/Pfd.qml +++ b/ground/openpilotgcs/share/openpilotgcs/pfd/default/Pfd.qml @@ -61,6 +61,8 @@ Rectangle { x: scaledBounds.x * sceneItem.width } +// Disable Side_slip moving because flickering issue on whole Pfd. +/* SvgElementImage { id: side_slip elementName: "sideslip-moving" @@ -77,11 +79,12 @@ Rectangle { } } - anchors.horizontalCenter: foreground.horizontalCenter + anchors.horizontalCenter: horizontCenterItem.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 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 From 8e2efe0c5fc3193be0d25d080bd6650123e4adf6 Mon Sep 17 00:00:00 2001 From: Laurent Lalanne Date: Fri, 20 Jun 2014 00:50:44 +0200 Subject: [PATCH 09/19] OP-1354 Added compass-home (Enabled if TakeOffLocation.Status is Valid) and compass-waypoint (Currently disabled) --- .../openpilotgcs/pfd/default/Compass.qml | 36 +++++++++++ .../share/openpilotgcs/pfd/default/pfd.svg | 59 +++++++++++-------- .../src/plugins/pfdqml/pfdqmlgadgetwidget.cpp | 3 +- 3 files changed, 71 insertions(+), 27 deletions(-) diff --git a/ground/openpilotgcs/share/openpilotgcs/pfd/default/Compass.qml b/ground/openpilotgcs/share/openpilotgcs/pfd/default/Compass.qml index 198b9502c..e8dfa9248 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: false + } + + + Item { id: compass_text_box diff --git a/ground/openpilotgcs/share/openpilotgcs/pfd/default/pfd.svg b/ground/openpilotgcs/share/openpilotgcs/pfd/default/pfd.svg index 233d90cf8..c121f3c80 100644 --- a/ground/openpilotgcs/share/openpilotgcs/pfd/default/pfd.svg +++ b/ground/openpilotgcs/share/openpilotgcs/pfd/default/pfd.svg @@ -50,11 +50,11 @@ borderopacity="1.0" inkscape:pageopacity="0.0" inkscape:pageshadow="2" - inkscape:zoom="3.7432465" - inkscape:cx="152.24569" - inkscape:cy="438.16076" + inkscape:zoom="1.3234375" + inkscape:cx="320" + inkscape:cy="240" inkscape:document-units="px" - inkscape:current-layer="layer71" + inkscape:current-layer="layer8" showgrid="false" fit-margin-top="0" fit-margin-left="0" @@ -76,7 +76,9 @@ inkscape:snap-to-guides="true" inkscape:snap-nodes="true" inkscape:bbox-paths="false" - inkscape:snap-global="true"> + inkscape:snap-global="true" + inkscape:snap-intersection-paths="false" + inkscape:snap-object-midpoints="true"> image/svg+xml - + @@ -2821,14 +2823,12 @@ id="layer7" inkscape:label="compass" style="display:inline" - transform="translate(0,-4)" - sodipodi:insensitive="true"> + transform="translate(0,-4)"> + style="display:inline"> - + - + + + + getObject(); From 1921f929f38237b88b489857c66d75b9ee2b618f Mon Sep 17 00:00:00 2001 From: Laurent Lalanne Date: Fri, 20 Jun 2014 18:26:31 +0200 Subject: [PATCH 10/19] OP-1354 Fixed pitch0 line, some centering. Added Home info : distance/bearing to home --- .../share/openpilotgcs/pfd/default/Info.qml | 49 ++++++++ .../share/openpilotgcs/pfd/default/Pfd.qml | 2 +- .../openpilotgcs/pfd/default/PfdWorldView.qml | 10 ++ .../openpilotgcs/pfd/default/Warnings.qml | 1 + .../share/openpilotgcs/pfd/default/pfd.svg | 113 ++++++++---------- 5 files changed, 114 insertions(+), 61 deletions(-) diff --git a/ground/openpilotgcs/share/openpilotgcs/pfd/default/Info.qml b/ground/openpilotgcs/share/openpilotgcs/pfd/default/Info.qml index d7cafba26..0addbb094 100644 --- a/ground/openpilotgcs/share/openpilotgcs/pfd/default/Info.qml +++ b/ground/openpilotgcs/share/openpilotgcs/pfd/default/Info.qml @@ -4,6 +4,12 @@ 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((TakeOffLocation.East - PositionState.East)*(TakeOffLocation.East - PositionState.East) + + (TakeOffLocation.North - PositionState.North)*(TakeOffLocation.North - PositionState.North)) + SvgElementImage { id: info_bg sceneSize: info.sceneSize @@ -183,6 +189,49 @@ Item { sceneSize: info.sceneSize } + SvgElementImage { + id: home_bg + elementName: "home-bg" + sceneSize: info.sceneSize + + visible: TakeOffLocation.Status == 0 + } + + SvgElementPositionItem { + sceneSize: info.sceneSize + elementName: "home-heading-text" + + visible: TakeOffLocation.Status == 0 + + Text { + text: home_heading.toFixed(1)+"°" + anchors.fill: parent + color: "cyan" + font { + family: "Arial" + pixelSize: Math.floor(parent.height * 1.2) + } + } + } + + SvgElementPositionItem { + sceneSize: info.sceneSize + elementName: "home-distance-text" + + visible: TakeOffLocation.Status == 0 + + Text { + text: home_distance.toFixed(0)+" m" + anchors.fill: parent + color: "cyan" + font { + family: "Arial" + pixelSize: Math.floor(parent.height * 1.2) + } + } + } + + SvgElementImage { id: info_border elementName: "info-border" diff --git a/ground/openpilotgcs/share/openpilotgcs/pfd/default/Pfd.qml b/ground/openpilotgcs/share/openpilotgcs/pfd/default/Pfd.qml index c8c85c035..5ad121d9a 100644 --- a/ground/openpilotgcs/share/openpilotgcs/pfd/default/Pfd.qml +++ b/ground/openpilotgcs/share/openpilotgcs/pfd/default/Pfd.qml @@ -32,7 +32,7 @@ Rectangle { anchors.centerIn: parent clip: true - //onWidthChanged:console.log("Width/Height : "+width+" "+ height+" scale : "+width/height+" border : "+parent.paintedHeight * 0.006 ) + onWidthChanged:console.log("Width/Height : "+width+" "+ height+" scale : "+width/height+" border : "+parent.paintedHeight * 0.006+" TakeOffLocation.Status" + TakeOffLocation.Status) Loader { id: worldLoader diff --git a/ground/openpilotgcs/share/openpilotgcs/pfd/default/PfdWorldView.qml b/ground/openpilotgcs/share/openpilotgcs/pfd/default/PfdWorldView.qml index 5c8baa1ca..b2b1d2e6a 100644 --- a/ground/openpilotgcs/share/openpilotgcs/pfd/default/PfdWorldView.qml +++ b/ground/openpilotgcs/share/openpilotgcs/pfd/default/PfdWorldView.qml @@ -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/Warnings.qml b/ground/openpilotgcs/share/openpilotgcs/pfd/default/Warnings.qml index 8e2fc89d1..82030e16b 100644 --- a/ground/openpilotgcs/share/openpilotgcs/pfd/default/Warnings.qml +++ b/ground/openpilotgcs/share/openpilotgcs/pfd/default/Warnings.qml @@ -111,6 +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 c121f3c80..aca54ae7a 100644 --- a/ground/openpilotgcs/share/openpilotgcs/pfd/default/pfd.svg +++ b/ground/openpilotgcs/share/openpilotgcs/pfd/default/pfd.svg @@ -50,11 +50,11 @@ borderopacity="1.0" inkscape:pageopacity="0.0" inkscape:pageshadow="2" - inkscape:zoom="1.3234375" - inkscape:cx="320" - inkscape:cy="240" + inkscape:zoom="9.3092382" + inkscape:cx="599.99681" + inkscape:cy="46" inkscape:document-units="px" - inkscape:current-layer="layer8" + inkscape:current-layer="layer2" showgrid="false" fit-margin-top="0" fit-margin-left="0" @@ -124,8 +124,7 @@ id="layer2" inkscape:label="horizon" style="display:inline" - transform="translate(0,-4)" - sodipodi:insensitive="true"> + transform="translate(0,-4)"> + transform="translate(0,-4)"> + style="display:inline"> + style="display:inline"> + style="display:inline"> - + + inkscape:connector-curvature="0" + sodipodi:nodetypes="scccss" /> + id="home-eta-label" + transform="translate(0,-1.6584)"> + id="home-distance-label" + transform="translate(0,-1.6584)"> + id="home-heading-label" + transform="translate(0,-1.6584)"> + style="display:inline"> + id="home-eta-text" + transform="translate(0,-1.5)"> + style="display:inline"> + id="home-distance-text" + transform="translate(0,-1.5)"> + style="display:inline"> + id="home-heading-text" + transform="translate(0,-1.5)"> + style="display:inline"> + inkscape:label="waypoint-eta-text"> + inkscape:label="waypoint-distance-text"> + inkscape:label="waypoint-heading-text"> + inkscape:label="waypoint-mode-text"> + style="display:inline"> + style="display:inline"> + style="display:inline"> + transform="translate(0,1.5)"> @@ -2455,7 +2445,7 @@ inkscape:connector-curvature="0" /> @@ -2467,20 +2457,20 @@ sodipodi:nodetypes="cc" /> @@ -4685,7 +4675,8 @@ inkscape:groupmode="layer" id="layer67" inkscape:label="warnings" - style="display:none"> + style="display:inline" + sodipodi:insensitive="true"> + style="display:inline" + sodipodi:insensitive="true"> @@ -4868,7 +4860,8 @@ inkscape:groupmode="layer" id="layer33" inkscape:label="warning-rc-input" - style="display:inline"> + style="display:inline" + sodipodi:insensitive="true"> From 21fcbf024d825db03e2fa64dda19199cd00f7c15 Mon Sep 17 00:00:00 2001 From: Laurent Lalanne Date: Fri, 20 Jun 2014 20:33:43 +0200 Subject: [PATCH 11/19] OP-1354 ETA for home based on current velocity and distance, time conversions/formating --- .../share/openpilotgcs/pfd/default/Info.qml | 37 ++++++++++++++++++- 1 file changed, 35 insertions(+), 2 deletions(-) diff --git a/ground/openpilotgcs/share/openpilotgcs/pfd/default/Info.qml b/ground/openpilotgcs/share/openpilotgcs/pfd/default/Info.qml index 0addbb094..241a991a9 100644 --- a/ground/openpilotgcs/share/openpilotgcs/pfd/default/Info.qml +++ b/ground/openpilotgcs/share/openpilotgcs/pfd/default/Info.qml @@ -7,9 +7,25 @@ Item { property real home_heading: 180/3.1415 * Math.atan2(TakeOffLocation.East - PositionState.East, TakeOffLocation.North - PositionState.North) - property real home_distance: Math.sqrt((TakeOffLocation.East - PositionState.East)*(TakeOffLocation.East - PositionState.East) - + (TakeOffLocation.North - PositionState.North)*(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 current_velocity: Math.sqrt(Math.pow(VelocityState.North,2)+Math.pow(VelocityState.East,2)) + + property real home_eta: Math.round(home_distance/current_velocity) + property real home_eta_h: Math.floor(home_eta / 3600) + property real home_eta_m: Math.floor((home_eta - home_eta_h*3600)/60) + property real home_eta_s: Math.floor(home_eta - home_eta_h*3600 - home_eta_m*60) + + function formatTime(time) { + if (time === 0) + return "00" + if (time < 10) + return "0" + time; + else + return time.toString(); + } + SvgElementImage { id: info_bg sceneSize: info.sceneSize @@ -231,6 +247,23 @@ Item { } } + SvgElementPositionItem { + sceneSize: info.sceneSize + elementName: "home-eta-text" + + visible: TakeOffLocation.Status == 0 + + Text { + text: formatTime(home_eta_h) + ":" + formatTime(home_eta_m) + ":" + formatTime(home_eta_s) + anchors.fill: parent + color: "cyan" + font { + family: "Arial" + pixelSize: Math.floor(parent.height * 1.2) + } + } + } + SvgElementImage { id: info_border From ae0e92b9a0f28733609682a6c1cd8172fea59e66 Mon Sep 17 00:00:00 2001 From: Laurent Lalanne Date: Sat, 21 Jun 2014 01:31:50 +0200 Subject: [PATCH 12/19] OP-1354 Compass-waypoint visibility, Home info animation. --- .../openpilotgcs/pfd/default/Compass.qml | 2 +- .../share/openpilotgcs/pfd/default/Info.qml | 88 +++++++++++++++---- .../share/openpilotgcs/pfd/default/Pfd.qml | 2 +- .../share/openpilotgcs/pfd/default/pfd.svg | 32 ++++--- 4 files changed, 87 insertions(+), 37 deletions(-) diff --git a/ground/openpilotgcs/share/openpilotgcs/pfd/default/Compass.qml b/ground/openpilotgcs/share/openpilotgcs/pfd/default/Compass.qml index e8dfa9248..2fb92cc19 100644 --- a/ground/openpilotgcs/share/openpilotgcs/pfd/default/Compass.qml +++ b/ground/openpilotgcs/share/openpilotgcs/pfd/default/Compass.qml @@ -68,7 +68,7 @@ Item { transformOrigin: Item.Center smooth: true - visible: false + visible: PathDesired.End_East !== 0.0 && PathDesired.End_East !== 0.0 } diff --git a/ground/openpilotgcs/share/openpilotgcs/pfd/default/Info.qml b/ground/openpilotgcs/share/openpilotgcs/pfd/default/Info.qml index 241a991a9..c11784949 100644 --- a/ground/openpilotgcs/share/openpilotgcs/pfd/default/Info.qml +++ b/ground/openpilotgcs/share/openpilotgcs/pfd/default/Info.qml @@ -7,15 +7,15 @@ Item { 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 home_distance: Math.sqrt(Math.pow((TakeOffLocation.East - PositionState.East),2) + + Math.pow((TakeOffLocation.North - PositionState.North),2)) property real current_velocity: Math.sqrt(Math.pow(VelocityState.North,2)+Math.pow(VelocityState.East,2)) - property real home_eta: Math.round(home_distance/current_velocity) - property real home_eta_h: Math.floor(home_eta / 3600) - property real home_eta_m: Math.floor((home_eta - home_eta_h*3600)/60) - property real home_eta_s: Math.floor(home_eta - home_eta_h*3600 - home_eta_m*60) + 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) function formatTime(time) { if (time === 0) @@ -209,57 +209,109 @@ Item { id: home_bg elementName: "home-bg" sceneSize: info.sceneSize + y: Math.floor(scaledBounds.y * sceneItem.height) - visible: TakeOffLocation.Status == 0 + 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) - visible: TakeOffLocation.Status == 0 - + 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.fill: parent + text: " "+home_heading.toFixed(1)+"°" + anchors.centerIn: parent color: "cyan" font { family: "Arial" - pixelSize: Math.floor(parent.height * 1.2) + 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) - visible: TakeOffLocation.Status == 0 + 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.fill: parent + anchors.centerIn: parent color: "cyan" font { family: "Arial" - pixelSize: Math.floor(parent.height * 1.2) + 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) - visible: TakeOffLocation.Status == 0 + 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.fill: parent + anchors.centerIn: parent color: "cyan" font { family: "Arial" - pixelSize: Math.floor(parent.height * 1.2) + pixelSize: parent.height * 1.2 } } } diff --git a/ground/openpilotgcs/share/openpilotgcs/pfd/default/Pfd.qml b/ground/openpilotgcs/share/openpilotgcs/pfd/default/Pfd.qml index 5ad121d9a..0c4afe9b9 100644 --- a/ground/openpilotgcs/share/openpilotgcs/pfd/default/Pfd.qml +++ b/ground/openpilotgcs/share/openpilotgcs/pfd/default/Pfd.qml @@ -32,7 +32,7 @@ Rectangle { anchors.centerIn: parent clip: true - onWidthChanged:console.log("Width/Height : "+width+" "+ height+" scale : "+width/height+" border : "+parent.paintedHeight * 0.006+" TakeOffLocation.Status" + TakeOffLocation.Status) + //onWidthChanged:console.log("TakeOffLocation.Status " + TakeOffLocation.Status) Loader { id: worldLoader diff --git a/ground/openpilotgcs/share/openpilotgcs/pfd/default/pfd.svg b/ground/openpilotgcs/share/openpilotgcs/pfd/default/pfd.svg index aca54ae7a..bca747fc0 100644 --- a/ground/openpilotgcs/share/openpilotgcs/pfd/default/pfd.svg +++ b/ground/openpilotgcs/share/openpilotgcs/pfd/default/pfd.svg @@ -50,12 +50,12 @@ borderopacity="1.0" inkscape:pageopacity="0.0" inkscape:pageshadow="2" - inkscape:zoom="9.3092382" - inkscape:cx="599.99681" - inkscape:cy="46" + inkscape:zoom="9.9282028" + inkscape:cx="593.25555" + inkscape:cy="43.480232" inkscape:document-units="px" - inkscape:current-layer="layer2" - showgrid="false" + inkscape:current-layer="layer25" + showgrid="true" fit-margin-top="0" fit-margin-left="0" fit-margin-right="0" @@ -906,7 +906,7 @@ + transform="matrix(1,0,0,1.0973877,0,-46.442937)"> + transform="matrix(1,0,0,1.0577142,0,-27.456636)"> + transform="matrix(1,0,0,1.0577142,0,-26.706351)"> + transform="matrix(1,0,0,0.99160769,0,2.0646588)"> + transform="matrix(1,0,0,0.99160769,0,2.0975587)"> + transform="matrix(1,0,0,0.99160769,15.28151,1.9884587)"> + inkscape:groupmode="layer"> + style="display:inline"> Date: Sat, 21 Jun 2014 13:09:23 +0200 Subject: [PATCH 13/19] OP-1354 Added waypoint info : wp# / wp count and mode --- .../share/openpilotgcs/pfd/default/Info.qml | 37 +++++++++ .../share/openpilotgcs/pfd/default/pfd.svg | 78 ++++++++++++------- .../src/plugins/pfdqml/pfdqmlgadgetwidget.cpp | 3 +- 3 files changed, 87 insertions(+), 31 deletions(-) diff --git a/ground/openpilotgcs/share/openpilotgcs/pfd/default/Info.qml b/ground/openpilotgcs/share/openpilotgcs/pfd/default/Info.qml index c11784949..95b198cef 100644 --- a/ground/openpilotgcs/share/openpilotgcs/pfd/default/Info.qml +++ b/ground/openpilotgcs/share/openpilotgcs/pfd/default/Info.qml @@ -118,6 +118,43 @@ Item { } } + SvgElementPositionItem { + sceneSize: info.sceneSize + elementName: "waypoint-description-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+" / "+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" diff --git a/ground/openpilotgcs/share/openpilotgcs/pfd/default/pfd.svg b/ground/openpilotgcs/share/openpilotgcs/pfd/default/pfd.svg index bca747fc0..c55acfb1e 100644 --- a/ground/openpilotgcs/share/openpilotgcs/pfd/default/pfd.svg +++ b/ground/openpilotgcs/share/openpilotgcs/pfd/default/pfd.svg @@ -50,11 +50,11 @@ borderopacity="1.0" inkscape:pageopacity="0.0" inkscape:pageshadow="2" - inkscape:zoom="9.9282028" - inkscape:cx="593.25555" - inkscape:cy="43.480232" + inkscape:zoom="3.3787325" + inkscape:cx="305.54267" + inkscape:cy="416.93352" inkscape:document-units="px" - inkscape:current-layer="layer25" + inkscape:current-layer="layer18" showgrid="true" fit-margin-top="0" fit-margin-left="0" @@ -115,7 +115,7 @@ image/svg+xml - + @@ -124,7 +124,8 @@ id="layer2" inkscape:label="horizon" style="display:inline" - transform="translate(0,-4)"> + transform="translate(0,-4)" + sodipodi:insensitive="true"> + transform="translate(0,-4)" + sodipodi:insensitive="true"> + style="display:inline" + sodipodi:insensitive="true"> + style="display:inline" + sodipodi:insensitive="true"> + style="display:inline" + sodipodi:insensitive="true"> + style="display:inline" + sodipodi:insensitive="true"> + style="display:inline" + sodipodi:insensitive="true"> + style="display:inline" + sodipodi:insensitive="true"> + style="display:inline" + sodipodi:insensitive="true"> + 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)"> + transform="translate(0,0.595158)"> + transform="translate(0,0.595158)"> + transform="translate(0,0.595158)"> + transform="translate(0,0.595158)"> @@ -1845,7 +1859,7 @@ + transform="translate(0,0.595158)"> @@ -2813,7 +2827,8 @@ id="layer7" inkscape:label="compass" style="display:inline" - transform="translate(0,-4)"> + transform="translate(0,-4)" + sodipodi:insensitive="true"> + inkscape:groupmode="layer" + sodipodi:insensitive="true"> + style="display:inline" + sodipodi:insensitive="true"> + style="display:inline" + sodipodi:insensitive="true"> getObject(); From 56e3b43e9fc4ca90ac9cb4eea2d1fbd3d46289ec Mon Sep 17 00:00:00 2001 From: Laurent Lalanne Date: Sat, 21 Jun 2014 13:20:09 +0200 Subject: [PATCH 14/19] OP-1354 Missing WaypointActive --- ground/openpilotgcs/src/plugins/pfdqml/pfdqmlgadgetwidget.cpp | 3 ++- 1 file changed, 2 insertions(+), 1 deletion(-) diff --git a/ground/openpilotgcs/src/plugins/pfdqml/pfdqmlgadgetwidget.cpp b/ground/openpilotgcs/src/plugins/pfdqml/pfdqmlgadgetwidget.cpp index a95ae10e9..e1978108d 100644 --- a/ground/openpilotgcs/src/plugins/pfdqml/pfdqmlgadgetwidget.cpp +++ b/ground/openpilotgcs/src/plugins/pfdqml/pfdqmlgadgetwidget.cpp @@ -65,7 +65,8 @@ PfdQmlGadgetWidget::PfdQmlGadgetWidget(QWindow *parent) : "FlightBatteryState" << "ActuatorDesired" << "TakeOffLocation" << - "PathPlan"; + "PathPlan" << + "WaypointActive"; ExtensionSystem::PluginManager *pm = ExtensionSystem::PluginManager::instance(); UAVObjectManager *objManager = pm->getObject(); From 6bdb2fe0b306677b7a06f71eabf65fb48d81cab4 Mon Sep 17 00:00:00 2001 From: Laurent Lalanne Date: Sat, 21 Jun 2014 21:19:21 +0200 Subject: [PATCH 15/19] OP-1354 Waypoint heading/distance info, basic distance counter. --- .../share/openpilotgcs/pfd/default/Info.qml | 111 ++++++++++++++++- .../share/openpilotgcs/pfd/default/pfd.svg | 115 +++++++++++++++--- 2 files changed, 205 insertions(+), 21 deletions(-) diff --git a/ground/openpilotgcs/share/openpilotgcs/pfd/default/Info.qml b/ground/openpilotgcs/share/openpilotgcs/pfd/default/Info.qml index 95b198cef..c9ab07ab5 100644 --- a/ground/openpilotgcs/share/openpilotgcs/pfd/default/Info.qml +++ b/ground/openpilotgcs/share/openpilotgcs/pfd/default/Info.qml @@ -10,6 +10,12 @@ Item { 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) @@ -17,7 +23,28 @@ Item { 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) - function formatTime(time) { + 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 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) @@ -120,14 +147,90 @@ Item { SvgElementPositionItem { sceneSize: info.sceneSize - elementName: "waypoint-description-text" + 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: WaypointActive.Index+" / "+PathPlan.WaypointCount+" " + 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 //total_distance > 5 + + property real total_distance: 0 + + 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 @@ -153,8 +256,6 @@ Item { } } - - SvgElementPositionItem { sceneSize: info.sceneSize elementName: "battery-volt-text" diff --git a/ground/openpilotgcs/share/openpilotgcs/pfd/default/pfd.svg b/ground/openpilotgcs/share/openpilotgcs/pfd/default/pfd.svg index c55acfb1e..eddbc6a21 100644 --- a/ground/openpilotgcs/share/openpilotgcs/pfd/default/pfd.svg +++ b/ground/openpilotgcs/share/openpilotgcs/pfd/default/pfd.svg @@ -50,11 +50,11 @@ borderopacity="1.0" inkscape:pageopacity="0.0" inkscape:pageshadow="2" - inkscape:zoom="3.3787325" - inkscape:cx="305.54267" - inkscape:cy="416.93352" + inkscape:zoom="5.9383334" + inkscape:cx="248.68909" + inkscape:cy="451.58086" inkscape:document-units="px" - inkscape:current-layer="layer18" + inkscape:current-layer="layer60" showgrid="true" fit-margin-top="0" fit-margin-left="0" @@ -1378,11 +1378,48 @@ style="font-size:8px;fill:#ffffff" id="path6377" inkscape:connector-curvature="0" /> + + + + + + + + + + transform="translate(2,0.595158)"> + transform="translate(30,0.595158)"> + + + 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" /> + + + + + + @@ -2855,7 +2938,7 @@ transform="translate(0,78)"> Date: Sun, 22 Jun 2014 17:06:09 +0200 Subject: [PATCH 16/19] OP-1354 Sumdist reset --- .../share/openpilotgcs/pfd/default/Info.qml | 15 +++++++++------ 1 file changed, 9 insertions(+), 6 deletions(-) diff --git a/ground/openpilotgcs/share/openpilotgcs/pfd/default/Info.qml b/ground/openpilotgcs/share/openpilotgcs/pfd/default/Info.qml index c9ab07ab5..d30167330 100644 --- a/ground/openpilotgcs/share/openpilotgcs/pfd/default/Info.qml +++ b/ground/openpilotgcs/share/openpilotgcs/pfd/default/Info.qml @@ -33,6 +33,10 @@ Item { 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) { @@ -187,7 +191,7 @@ Item { y: Math.floor(scaledBounds.y * sceneItem.height) visible: true //total_distance > 5 - property real total_distance: 0 + MouseArea { id: total_dist_mouseArea; anchors.fill: parent; onClicked: reset_distance()} Text { text: " "+total_distance.toFixed(0)+" m" @@ -197,11 +201,10 @@ Item { color: "magenta" } - Timer { - interval: 1000; running: true; repeat: true; - onTriggered: {if (GPSPositionSensor.Status == 3) compute_distance(PositionState.East,PositionState.North)} - } - + Timer { + interval: 1000; running: true; repeat: true; + onTriggered: {if (GPSPositionSensor.Status == 3) compute_distance(PositionState.East,PositionState.North)} + } } SvgElementPositionItem { From c6245260ec19210b756a122dcc0f7695710cadd1 Mon Sep 17 00:00:00 2001 From: Laurent Lalanne Date: Sun, 22 Jun 2014 20:37:22 +0200 Subject: [PATCH 17/19] OP-1354 Removed commented code --- .../share/openpilotgcs/pfd/default/Info.qml | 3 +- .../share/openpilotgcs/pfd/default/Pfd.qml | 30 +------------------ 2 files changed, 2 insertions(+), 31 deletions(-) diff --git a/ground/openpilotgcs/share/openpilotgcs/pfd/default/Info.qml b/ground/openpilotgcs/share/openpilotgcs/pfd/default/Info.qml index d30167330..699e0b2f0 100644 --- a/ground/openpilotgcs/share/openpilotgcs/pfd/default/Info.qml +++ b/ground/openpilotgcs/share/openpilotgcs/pfd/default/Info.qml @@ -62,7 +62,6 @@ Item { sceneSize: info.sceneSize elementName: "info-bg" width: parent.width - //anchors.top: parent.top } SvgElementImage { @@ -189,7 +188,7 @@ Item { width: scaledBounds.width * sceneItem.width height: scaledBounds.height * sceneItem.height y: Math.floor(scaledBounds.y * sceneItem.height) - visible: true //total_distance > 5 + visible: true MouseArea { id: total_dist_mouseArea; anchors.fill: parent; onClicked: reset_distance()} diff --git a/ground/openpilotgcs/share/openpilotgcs/pfd/default/Pfd.qml b/ground/openpilotgcs/share/openpilotgcs/pfd/default/Pfd.qml index 0c4afe9b9..336a0617a 100644 --- a/ground/openpilotgcs/share/openpilotgcs/pfd/default/Pfd.qml +++ b/ground/openpilotgcs/share/openpilotgcs/pfd/default/Pfd.qml @@ -23,7 +23,6 @@ Rectangle { Item { id: sceneItem - width: Math.floor((parent.paintedHeight * 1.32) - (parent.paintedHeight * 0.013)) height: Math.floor(parent.paintedHeight - parent.paintedHeight * 0.02) @@ -31,9 +30,7 @@ Rectangle { anchors.centerIn: parent clip: true - - //onWidthChanged:console.log("TakeOffLocation.Status " + TakeOffLocation.Status) - + Loader { id: worldLoader anchors.fill: parent @@ -61,31 +58,6 @@ Rectangle { x: scaledBounds.x * sceneItem.width } -// Disable Side_slip moving because flickering issue on whole Pfd. -/* - 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: horizontCenterItem.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 From 645f1a16ac0eb0f8db5ac6ac6b1c8d5b254b627e Mon Sep 17 00:00:00 2001 From: Philippe Renon Date: Sun, 29 Jun 2014 21:10:04 +0200 Subject: [PATCH 18/19] OP-1376 calibration config panel now propoperly saves result to board SD --- ground/openpilotgcs/src/plugins/config/configrevowidget.cpp | 4 ++++ 1 file changed, 4 insertions(+) diff --git a/ground/openpilotgcs/src/plugins/config/configrevowidget.cpp b/ground/openpilotgcs/src/plugins/config/configrevowidget.cpp index 54e1d1a9b..ade15643e 100644 --- a/ground/openpilotgcs/src/plugins/config/configrevowidget.cpp +++ b/ground/openpilotgcs/src/plugins/config/configrevowidget.cpp @@ -89,6 +89,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 From 1497d45eee64423f65eb08ded89656c9ca70dd18 Mon Sep 17 00:00:00 2001 From: Philippe Renon Date: Sun, 29 Jun 2014 21:11:34 +0200 Subject: [PATCH 19/19] OP-1377 calibration config panel Apply button is now visible only in Expert mode --- .../src/plugins/config/configrevowidget.cpp | 10 +++++++++- 1 file changed, 9 insertions(+), 1 deletion(-) diff --git a/ground/openpilotgcs/src/plugins/config/configrevowidget.cpp b/ground/openpilotgcs/src/plugins/config/configrevowidget.cpp index ade15643e..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);