mirror of
https://github.com/betaflight/betaflight.git
synced 2025-07-17 13:25:30 +03:00
Some fixes for persistent stats.
This commit is contained in:
parent
ad00c6b66b
commit
1920a61226
4 changed files with 91 additions and 43 deletions
|
@ -984,11 +984,6 @@ bool processRx(timeUs_t currentTimeUs)
|
||||||
|
|
||||||
pidSetAntiGravityState(IS_RC_MODE_ACTIVE(BOXANTIGRAVITY) || featureIsEnabled(FEATURE_ANTI_GRAVITY));
|
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;
|
return true;
|
||||||
}
|
}
|
||||||
|
|
||||||
|
|
|
@ -270,7 +270,6 @@ static int adjustmentRangeValue = -1;
|
||||||
|
|
||||||
static int applyStepAdjustment(controlRateConfig_t *controlRateConfig, uint8_t adjustmentFunction, int delta)
|
static int applyStepAdjustment(controlRateConfig_t *controlRateConfig, uint8_t adjustmentFunction, int delta)
|
||||||
{
|
{
|
||||||
|
|
||||||
beeperConfirmationBeeps(delta > 0 ? 2 : 1);
|
beeperConfirmationBeeps(delta > 0 ? 2 : 1);
|
||||||
int newValue;
|
int newValue;
|
||||||
switch (adjustmentFunction) {
|
switch (adjustmentFunction) {
|
||||||
|
@ -427,7 +426,6 @@ static int applyStepAdjustment(controlRateConfig_t *controlRateConfig, uint8_t a
|
||||||
break;
|
break;
|
||||||
};
|
};
|
||||||
|
|
||||||
setConfigDirty();
|
|
||||||
return newValue;
|
return newValue;
|
||||||
}
|
}
|
||||||
|
|
||||||
|
@ -435,10 +433,6 @@ static int applyAbsoluteAdjustment(controlRateConfig_t *controlRateConfig, adjus
|
||||||
{
|
{
|
||||||
int newValue;
|
int newValue;
|
||||||
|
|
||||||
if (!controlRateConfig || !currentPidProfile) {
|
|
||||||
return 0;
|
|
||||||
}
|
|
||||||
|
|
||||||
switch (adjustmentFunction) {
|
switch (adjustmentFunction) {
|
||||||
case ADJUSTMENT_RC_RATE:
|
case ADJUSTMENT_RC_RATE:
|
||||||
case ADJUSTMENT_ROLL_RC_RATE:
|
case ADJUSTMENT_ROLL_RC_RATE:
|
||||||
|
@ -593,7 +587,6 @@ static int applyAbsoluteAdjustment(controlRateConfig_t *controlRateConfig, adjus
|
||||||
break;
|
break;
|
||||||
};
|
};
|
||||||
|
|
||||||
setConfigDirty();
|
|
||||||
return newValue;
|
return newValue;
|
||||||
}
|
}
|
||||||
|
|
||||||
|
@ -653,7 +646,6 @@ static uint8_t applySelectAdjustment(adjustmentFunction_e adjustmentFunction, ui
|
||||||
beeperConfirmationBeeps(beeps);
|
beeperConfirmationBeeps(beeps);
|
||||||
}
|
}
|
||||||
|
|
||||||
setConfigDirty();
|
|
||||||
return position;
|
return position;
|
||||||
}
|
}
|
||||||
|
|
||||||
|
@ -757,6 +749,9 @@ static void processStepwiseAdjustments(controlRateConfig_t *controlRateConfig, c
|
||||||
}
|
}
|
||||||
|
|
||||||
int newValue = applyStepAdjustment(controlRateConfig, adjustmentFunction, delta);
|
int newValue = applyStepAdjustment(controlRateConfig, adjustmentFunction, delta);
|
||||||
|
|
||||||
|
setConfigDirty();
|
||||||
|
|
||||||
pidInitConfig(currentPidProfile);
|
pidInitConfig(currentPidProfile);
|
||||||
|
|
||||||
adjustmentState->ready = false;
|
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)
|
static void processContinuosAdjustments(controlRateConfig_t *controlRateConfig)
|
||||||
{
|
{
|
||||||
for (int i = 0; i < continuosAdjustmentCount; i++) {
|
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 uint16_t rangeWidth = (2100 - 900) / switchPositions;
|
||||||
const uint8_t position = (constrain(rcData[channelIndex], 900, 2100 - 1) - 900) / rangeWidth;
|
const uint8_t position = (constrain(rcData[channelIndex], 900, 2100 - 1) - 900) / rangeWidth;
|
||||||
newValue = applySelectAdjustment(adjustmentFunction, position);
|
newValue = applySelectAdjustment(adjustmentFunction, position);
|
||||||
|
|
||||||
|
setConfigDirtyIfNotPermanent(&adjustmentRange->range);
|
||||||
} else {
|
} else {
|
||||||
// If setting is defined for step adjustment and center value has been specified, apply values directly (scaled) from aux channel
|
// If setting is defined for step adjustment and center value has been specified, apply values directly (scaled) from aux channel
|
||||||
if (adjustmentRange->adjustmentCenter &&
|
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;
|
int value = (((rcData[channelIndex] - PWM_RANGE_MIDDLE) * adjustmentRange->adjustmentScale) / (PWM_RANGE_MIDDLE - PWM_RANGE_MIN)) + adjustmentRange->adjustmentCenter;
|
||||||
|
|
||||||
newValue = applyAbsoluteAdjustment(controlRateConfig, adjustmentFunction, value);
|
newValue = applyAbsoluteAdjustment(controlRateConfig, adjustmentFunction, value);
|
||||||
|
|
||||||
|
setConfigDirtyIfNotPermanent(&adjustmentRange->range);
|
||||||
|
|
||||||
pidInitConfig(currentPidProfile);
|
pidInitConfig(currentPidProfile);
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
|
|
|
@ -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 <http://www.gnu.org/licenses/>.
|
||||||
|
*/
|
||||||
|
|
||||||
#include "platform.h"
|
#include "platform.h"
|
||||||
|
|
||||||
|
@ -6,6 +25,7 @@
|
||||||
#include "drivers/time.h"
|
#include "drivers/time.h"
|
||||||
|
|
||||||
#include "fc/config.h"
|
#include "fc/config.h"
|
||||||
|
#include "fc/dispatch.h"
|
||||||
#include "fc/runtime_config.h"
|
#include "fc/runtime_config.h"
|
||||||
#include "fc/stats.h"
|
#include "fc/stats.h"
|
||||||
|
|
||||||
|
@ -15,12 +35,13 @@
|
||||||
#include "pg/stats.h"
|
#include "pg/stats.h"
|
||||||
|
|
||||||
|
|
||||||
#define MIN_FLIGHT_TIME_TO_RECORD_STATS_S 10 //prevent recording stats for that short "flights" [s]
|
#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 STATS_SAVE_DELAY_US 500000 // Let disarming complete and save stats after this time
|
||||||
|
|
||||||
static timeMs_t arm_millis;
|
static timeMs_t arm_millis;
|
||||||
static uint32_t arm_distance_cm;
|
static uint32_t arm_distance_cm;
|
||||||
static timeMs_t save_pending_millis; // 0 = none
|
|
||||||
|
static bool saveRequired = false;
|
||||||
|
|
||||||
#ifdef USE_GPS
|
#ifdef USE_GPS
|
||||||
#define DISTANCE_FLOWN_CM (GPS_distanceFlownInCm)
|
#define DISTANCE_FLOWN_CM (GPS_distanceFlownInCm)
|
||||||
|
@ -28,6 +49,29 @@ static timeMs_t save_pending_millis; // 0 = none
|
||||||
#define DISTANCE_FLOWN_CM (0)
|
#define DISTANCE_FLOWN_CM (0)
|
||||||
#endif
|
#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)
|
void statsOnArm(void)
|
||||||
{
|
{
|
||||||
arm_millis = millis();
|
arm_millis = millis();
|
||||||
|
@ -42,35 +86,16 @@ void statsOnDisarm(void)
|
||||||
statsConfigMutable()->stats_total_flights += 1; //arm/flight counter
|
statsConfigMutable()->stats_total_flights += 1; //arm/flight counter
|
||||||
statsConfigMutable()->stats_total_time_s += dt; //[s]
|
statsConfigMutable()->stats_total_time_s += dt; //[s]
|
||||||
statsConfigMutable()->stats_total_dist_m += (DISTANCE_FLOWN_CM - arm_distance_cm) / 100; //[m]
|
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
|
/* 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 */
|
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
|
#endif
|
||||||
|
|
|
@ -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 <http://www.gnu.org/licenses/>.
|
||||||
|
*/
|
||||||
|
|
||||||
|
#pragma once
|
||||||
|
|
||||||
void statsOnArm(void);
|
void statsOnArm(void);
|
||||||
void statsOnDisarm(void);
|
void statsOnDisarm(void);
|
||||||
void statsOnLoop(void);
|
|
||||||
|
|
Loading…
Add table
Add a link
Reference in a new issue