From 1920a61226cae9eb25abeb2a6d5081f0e3bb13cb Mon Sep 17 00:00:00 2001 From: mikeller Date: Tue, 11 Jun 2019 01:55:24 +1200 Subject: [PATCH] Some fixes for persistent stats. --- src/main/fc/core.c | 5 --- src/main/fc/rc_adjustments.c | 24 +++++++---- src/main/fc/stats.c | 83 +++++++++++++++++++++++------------- src/main/fc/stats.h | 22 +++++++++- 4 files changed, 91 insertions(+), 43 deletions(-) diff --git a/src/main/fc/core.c b/src/main/fc/core.c index c94055060c..75385e0867 100644 --- a/src/main/fc/core.c +++ b/src/main/fc/core.c @@ -984,11 +984,6 @@ bool processRx(timeUs_t currentTimeUs) pidSetAntiGravityState(IS_RC_MODE_ACTIVE(BOXANTIGRAVITY) || featureIsEnabled(FEATURE_ANTI_GRAVITY)); -#ifdef USE_PERSISTENT_STATS - /* allow the stats collector to do periodic tasks */ - statsOnLoop(); -#endif - return true; } diff --git a/src/main/fc/rc_adjustments.c b/src/main/fc/rc_adjustments.c index 811b0a137a..d86f71385a 100644 --- a/src/main/fc/rc_adjustments.c +++ b/src/main/fc/rc_adjustments.c @@ -270,7 +270,6 @@ static int adjustmentRangeValue = -1; static int applyStepAdjustment(controlRateConfig_t *controlRateConfig, uint8_t adjustmentFunction, int delta) { - beeperConfirmationBeeps(delta > 0 ? 2 : 1); int newValue; switch (adjustmentFunction) { @@ -427,7 +426,6 @@ static int applyStepAdjustment(controlRateConfig_t *controlRateConfig, uint8_t a break; }; - setConfigDirty(); return newValue; } @@ -435,10 +433,6 @@ static int applyAbsoluteAdjustment(controlRateConfig_t *controlRateConfig, adjus { int newValue; - if (!controlRateConfig || !currentPidProfile) { - return 0; - } - switch (adjustmentFunction) { case ADJUSTMENT_RC_RATE: case ADJUSTMENT_ROLL_RC_RATE: @@ -593,7 +587,6 @@ static int applyAbsoluteAdjustment(controlRateConfig_t *controlRateConfig, adjus break; }; - setConfigDirty(); return newValue; } @@ -653,7 +646,6 @@ static uint8_t applySelectAdjustment(adjustmentFunction_e adjustmentFunction, ui beeperConfirmationBeeps(beeps); } - setConfigDirty(); return position; } @@ -757,6 +749,9 @@ static void processStepwiseAdjustments(controlRateConfig_t *controlRateConfig, c } int newValue = applyStepAdjustment(controlRateConfig, adjustmentFunction, delta); + + setConfigDirty(); + pidInitConfig(currentPidProfile); adjustmentState->ready = false; @@ -770,6 +765,14 @@ static void processStepwiseAdjustments(controlRateConfig_t *controlRateConfig, c } } +static void setConfigDirtyIfNotPermanent(const channelRange_t *range) +{ + if (range->startStep == MIN_MODE_RANGE_STEP && range->endStep == MAX_MODE_RANGE_STEP) { + // Only set the configuration dirty if this range is NOT permanently enabled (and the config thus never used). + setConfigDirty(); + } +} + static void processContinuosAdjustments(controlRateConfig_t *controlRateConfig) { for (int i = 0; i < continuosAdjustmentCount; i++) { @@ -797,6 +800,8 @@ static void processContinuosAdjustments(controlRateConfig_t *controlRateConfig) const uint16_t rangeWidth = (2100 - 900) / switchPositions; const uint8_t position = (constrain(rcData[channelIndex], 900, 2100 - 1) - 900) / rangeWidth; newValue = applySelectAdjustment(adjustmentFunction, position); + + setConfigDirtyIfNotPermanent(&adjustmentRange->range); } else { // If setting is defined for step adjustment and center value has been specified, apply values directly (scaled) from aux channel if (adjustmentRange->adjustmentCenter && @@ -804,6 +809,9 @@ static void processContinuosAdjustments(controlRateConfig_t *controlRateConfig) int value = (((rcData[channelIndex] - PWM_RANGE_MIDDLE) * adjustmentRange->adjustmentScale) / (PWM_RANGE_MIDDLE - PWM_RANGE_MIN)) + adjustmentRange->adjustmentCenter; newValue = applyAbsoluteAdjustment(controlRateConfig, adjustmentFunction, value); + + setConfigDirtyIfNotPermanent(&adjustmentRange->range); + pidInitConfig(currentPidProfile); } } diff --git a/src/main/fc/stats.c b/src/main/fc/stats.c index 8bc0303858..87340167d8 100644 --- a/src/main/fc/stats.c +++ b/src/main/fc/stats.c @@ -1,3 +1,22 @@ +/* + * This file is part of Cleanflight and Betaflight. + * + * Cleanflight and Betaflight are free software. You can redistribute + * this software and/or modify this software under the terms of the + * GNU General Public License as published by the Free Software + * Foundation, either version 3 of the License, or (at your option) + * any later version. + * + * Cleanflight and Betaflight are distributed in the hope that they + * will be useful, but WITHOUT ANY WARRANTY; without even the implied + * warranty of MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. + * See the GNU General Public License for more details. + * + * You should have received a copy of the GNU General Public License + * along with this software. + * + * If not, see . + */ #include "platform.h" @@ -6,6 +25,7 @@ #include "drivers/time.h" #include "fc/config.h" +#include "fc/dispatch.h" #include "fc/runtime_config.h" #include "fc/stats.h" @@ -15,12 +35,13 @@ #include "pg/stats.h" -#define MIN_FLIGHT_TIME_TO_RECORD_STATS_S 10 //prevent recording stats for that short "flights" [s] -#define STATS_SAVE_DELAY_MS 500 //let disarming complete and save stats after this time +#define MIN_FLIGHT_TIME_TO_RECORD_STATS_S 10 // Prevent recording stats for that short "flights" [s] +#define STATS_SAVE_DELAY_US 500000 // Let disarming complete and save stats after this time static timeMs_t arm_millis; static uint32_t arm_distance_cm; -static timeMs_t save_pending_millis; // 0 = none + +static bool saveRequired = false; #ifdef USE_GPS #define DISTANCE_FLOWN_CM (GPS_distanceFlownInCm) @@ -28,6 +49,29 @@ static timeMs_t save_pending_millis; // 0 = none #define DISTANCE_FLOWN_CM (0) #endif +void writeStats(struct dispatchEntry_s* self) +{ + UNUSED(self); + + if (!ARMING_FLAG(ARMED)) { + // Don't save if the user made config changes that have not yet been saved. + if (!isConfigDirty()) { + writeEEPROM(); + + // Repeat disarming beep indicating the stats save is complete + beeper(BEEPER_DISARMING); + } + + saveRequired = false; + } +} + +dispatchEntry_t writeStatsEntry = +{ + writeStats, 0, NULL, false +}; + + void statsOnArm(void) { arm_millis = millis(); @@ -42,35 +86,16 @@ void statsOnDisarm(void) statsConfigMutable()->stats_total_flights += 1; //arm/flight counter statsConfigMutable()->stats_total_time_s += dt; //[s] statsConfigMutable()->stats_total_dist_m += (DISTANCE_FLOWN_CM - arm_distance_cm) / 100; //[m] + + saveRequired = true; + } + + if (saveRequired) { /* signal that stats need to be saved but don't execute time consuming flash operation now - let the disarming process complete and then execute the actual save */ - save_pending_millis = millis(); + dispatchEnable(); + dispatchAdd(&writeStatsEntry, STATS_SAVE_DELAY_US); } } } - -void statsOnLoop(void) -{ - /* check for pending flash write */ - if (save_pending_millis && millis()-save_pending_millis > STATS_SAVE_DELAY_MS) { - if (ARMING_FLAG(ARMED)) { - /* re-armed - don't save! */ - } - else { - if (isConfigDirty()) { - /* There are some adjustments made in the configuration and we don't want - to implicitly save it... We can't currently save part of the configuration, - so we simply don't execute the stats save operation at all. This will result - in missing stats update *if* rc-adjustments were made during the flight. */ - } - else { - writeEEPROM(); - /* repeat disarming beep indicating the stats save is complete */ - beeper(BEEPER_DISARMING); - } - } - save_pending_millis = 0; - } -} - #endif diff --git a/src/main/fc/stats.h b/src/main/fc/stats.h index d9a7336fbf..01554930ca 100644 --- a/src/main/fc/stats.h +++ b/src/main/fc/stats.h @@ -1,4 +1,24 @@ +/* + * This file is part of Cleanflight and Betaflight. + * + * Cleanflight and Betaflight are free software. You can redistribute + * this software and/or modify this software under the terms of the + * GNU General Public License as published by the Free Software + * Foundation, either version 3 of the License, or (at your option) + * any later version. + * + * Cleanflight and Betaflight are distributed in the hope that they + * will be useful, but WITHOUT ANY WARRANTY; without even the implied + * warranty of MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. + * See the GNU General Public License for more details. + * + * You should have received a copy of the GNU General Public License + * along with this software. + * + * If not, see . + */ + +#pragma once void statsOnArm(void); void statsOnDisarm(void); -void statsOnLoop(void);