Mirror/inav
1
0
Fork 0
mirror of https://github.com/iNavFlight/inav.git synced 2025-07-25 17:25:18 +03:00
Code Issues Wiki Activity

Merge pull request #6708 from avsaase/avs-automatic-servo-trim

Browse source 
Automatic servo trim
This commit is contained in:
Alexander van Saase 2021-05-08 14:41:42 +02:00 • committed by GitHub
commit bd3cd6976c
No known key found for this signature in database
GPG key ID: 4AEE18F83AFDEB23
13 changed files with 183 additions and 25 deletions
Download patch file Download diff file Expand all files Collapse all files

1
docs/Settings.md
View file

@ -485,6 +485,7 @@
| serialrx_halfduplex | AUTO | | | Allow serial receiver to operate on UART TX pin. With some receivers will allow control and telemetry over a single wire. |
| serialrx_inverted | OFF | | | Reverse the serial inversion of the serial RX protocol. When this value is OFF, each protocol will use its default signal (e.g. SBUS will use an inverted signal). Some OpenLRS receivers produce a non-inverted SBUS signal. This setting supports this type of receivers (including modified FrSKY). |
| serialrx_provider | _target default_ | | | When feature SERIALRX is enabled, this allows connection to several receivers which output data via digital interface resembling serial. See RX section. |
| servo_autotrim_rotation_limit | 15 | 1 | 60 | Servo midpoints are only updated when total aircraft rotation is less than this threshold [deg/s]. Only applies when using `feature FW_AUTOTRIM`. |
| servo_center_pulse | 1500 | PWM_RANGE_MIN | PWM_RANGE_MAX | Servo midpoint |
| servo_lpf_hz | 20 | 0 | 400 | Selects the servo PWM output cutoff frequency. Value is in [Hz] |
| servo_protocol | PWM | | | An option to chose the protocol/option that would be used to output servo data. Possible options `PWM` (FC servo outputs), `SERVO_DRIVER` (I2C PCA9685 peripheral), `SBUS` (S.Bus protocol output via a configured serial port) |

1
src/main/build/debug.h
View file

@ -85,5 +85,6 @@ typedef enum {
DEBUG_ALTITUDE,
DEBUG_GYRO_ALPHA_BETA_GAMMA,
DEBUG_SMITH_PREDICTOR,
DEBUG_AUTOTRIM,
DEBUG_COUNT
} debugType_e;

2
src/main/fc/cli.c
View file

@ -156,7 +156,7 @@ static const char * const featureNames[] = {
"", "RSSI_ADC", "LED_STRIP", "DASHBOARD", "",
"BLACKBOX", "", "TRANSPONDER", "AIRMODE",
"SUPEREXPO", "VTX", "", "", "", "PWM_OUTPUT_ENABLE",
"OSD", "FW_LAUNCH", NULL
"OSD", "FW_LAUNCH", "FW_AUTOTRIM", NULL
};
/* Sensor names (used in lookup tables for *_hardware settings and in status command output) */

1
src/main/fc/config.h
View file

@ -62,6 +62,7 @@
FEATURE_PWM_OUTPUT_ENABLE = 1 << 28,
FEATURE_OSD = 1 << 29,
FEATURE_FW_LAUNCH = 1 << 30,
FEATURE_FW_AUTOTRIM = 1 << 31,
} features_e;
typedef struct systemConfig_s {

12
src/main/fc/fc_core.c
View file

@ -118,6 +118,7 @@ typedef struct emergencyArmingState_s {
timeDelta_t cycleTime = 0; // this is the number in micro second to achieve a full loop, it can differ a little and is taken into account in the PID loop
static timeUs_t flightTime = 0;
static timeUs_t armTime = 0;
EXTENDED_FASTRAM float dT;
@ -838,8 +839,12 @@ void taskMainPidLoop(timeUs_t currentTimeUs)
if (ARMING_FLAG(ARMED) && (!STATE(FIXED_WING_LEGACY) || !isNavLaunchEnabled() || (isNavLaunchEnabled() && (isFixedWingLaunchDetected() || isFixedWingLaunchFinishedOrAborted())))) {
flightTime += cycleTime;
armTime += cycleTime;
updateAccExtremes();
}
if (!ARMING_FLAG(ARMED)) {
armTime = 0;
}
gyroFilter();
@ -901,7 +906,7 @@ void taskMainPidLoop(timeUs_t currentTimeUs)
if (isMixerUsingServos()) {
servoMixer(dT);
processServoAutotrim();
processServoAutotrim(dT);
}
//Servos should be filtered or written only when mixer is using servos or special feaures are enabled
@ -958,6 +963,11 @@ float getFlightTime()
return (float)(flightTime / 1000) / 1000;
}
float getArmTime()
{
return (float)(armTime / 1000) / 1000;
}
void fcReboot(bool bootLoader)
{
// stop motor/servo outputs

1
src/main/fc/fc_core.h
View file

@ -45,5 +45,6 @@ void emergencyArmingUpdate(bool armingSwitchIsOn);
bool isCalibrating(void);
float getFlightTime(void);
float getArmTime(void);
void fcReboot(bool bootLoader);

4
src/main/fc/fc_msp_box.c
View file

@ -242,7 +242,11 @@ void initActiveBoxIds(void)
if (!feature(FEATURE_FW_LAUNCH)) {
activeBoxIds[activeBoxIdCount++] = BOXNAVLAUNCH;
}
if (!feature(FEATURE_FW_AUTOTRIM)) {
activeBoxIds[activeBoxIdCount++] = BOXAUTOTRIM;
}
#if defined(USE_AUTOTUNE_FIXED_WING)
activeBoxIds[activeBoxIdCount++] = BOXAUTOTUNE;
#endif

7
src/main/fc/settings.yaml
View file

@ -94,7 +94,7 @@ tables:
"VIBE", "CRUISE", "REM_FLIGHT_TIME", "SMARTAUDIO", "ACC",
"ERPM", "RPM_FILTER", "RPM_FREQ", "NAV_YAW", "DYNAMIC_FILTER", "DYNAMIC_FILTER_FREQUENCY",
"IRLOCK", "CD", "KALMAN_GAIN", "PID_MEASUREMENT", "SPM_CELLS", "SPM_VS600", "SPM_VARIO", "PCF8574", "DYN_GYRO_LPF", "FW_D", "IMU2", "ALTITUDE",
"GYRO_ALPHA_BETA_GAMMA", "SMITH_PREDICTOR"]
"GYRO_ALPHA_BETA_GAMMA", "SMITH_PREDICTOR", "AUTOTRIM"]
- name: async_mode
values: ["NONE", "GYRO", "ALL"]
- name: aux_operator
@ -1215,6 +1215,11 @@ groups:
description: "On tricopter mix only, if this is set to ON, servo will always be correcting regardless of armed state. to disable this, set it to OFF."
default_value: ON
type: bool
- name: servo_autotrim_rotation_limit
description: "Servo midpoints are only updated when total aircraft rotation is less than this threshold [deg/s]. Only applies when using `feature FW_AUTOTRIM`."
default_value: 15
min: 1
max: 60
- name: PG_CONTROL_RATE_PROFILES
type: controlRateConfig_t

4
src/main/flight/mixer.h
View file

@ -121,7 +121,9 @@ void mixerUpdateStateFlags(void);
void mixerResetDisarmedMotors(void);
void mixTable(const float dT);
void writeMotors(void);
void processServoAutotrim(void);
void processServoAutotrim(const float dT);
void processServoAutotrimMode(void);
void processContinuousServoAutotrim(const float dT);
void stopMotors(void);
void stopPwmAllMotors(void);

16
src/main/flight/pid.c
View file

@ -404,6 +404,22 @@ void pidResetErrorAccumulators(void)
}
}
void pidReduceErrorAccumulators(int8_t delta, uint8_t axis)
{
pidState[axis].errorGyroIf -= delta;
pidState[axis].errorGyroIfLimit -= delta;
}
float getTotalRateTarget(void)
{
return sqrtf(sq(pidState[FD_ROLL].rateTarget) + sq(pidState[FD_PITCH].rateTarget) + sq(pidState[FD_YAW].rateTarget));
}
float getAxisIterm(uint8_t axis)
{
return pidState[axis].errorGyroIf;
}
static float pidRcCommandToAngle(int16_t stick, int16_t maxInclination)
{
stick = constrain(stick, -500, 500);

3
src/main/flight/pid.h
View file

@ -188,6 +188,9 @@ extern int32_t axisPID_P[], axisPID_I[], axisPID_D[], axisPID_Setpoint[];
void pidInit(void);
bool pidInitFilters(void);
void pidResetErrorAccumulators(void);
void pidReduceErrorAccumulators(int8_t delta, uint8_t axis);
float getAxisIterm(uint8_t axis);
float getTotalRateTarget(void);
void pidResetTPAFilter(void);
struct controlRateConfig_s;

150
src/main/flight/servos.c
View file

@ -40,6 +40,7 @@
#include "drivers/time.h"
#include "fc/config.h"
#include "fc/fc_core.h"
#include "fc/rc_controls.h"
#include "fc/rc_modes.h"
#include "fc/runtime_config.h"
@ -51,11 +52,13 @@
#include "flight/pid.h"
#include "flight/servos.h"
#include "io/gps.h"
#include "rx/rx.h"
#include "sensors/gyro.h"
PG_REGISTER_WITH_RESET_TEMPLATE(servoConfig_t, servoConfig, PG_SERVO_CONFIG, 1);
PG_REGISTER_WITH_RESET_TEMPLATE(servoConfig_t, servoConfig, PG_SERVO_CONFIG, 2);
PG_RESET_TEMPLATE(servoConfig_t, servoConfig,
.servoCenterPulse = SETTING_SERVO_CENTER_PULSE_DEFAULT,
@ -63,7 +66,8 @@ PG_RESET_TEMPLATE(servoConfig_t, servoConfig,
.servo_lowpass_freq = SETTING_SERVO_LPF_HZ_DEFAULT, // Default servo update rate is 50Hz, everything above Nyquist frequency (25Hz) is going to fold and cause distortions
.servo_protocol = SETTING_SERVO_PROTOCOL_DEFAULT,
.flaperon_throw_offset = SETTING_FLAPERON_THROW_OFFSET_DEFAULT,
.tri_unarmed_servo = SETTING_TRI_UNARMED_SERVO_DEFAULT
.tri_unarmed_servo = SETTING_TRI_UNARMED_SERVO_DEFAULT,
.servo_autotrim_rotation_limit = SETTING_SERVO_AUTOTRIM_ROTATION_LIMIT_DEFAULT
);
PG_REGISTER_ARRAY_WITH_RESET_FN(servoMixer_t, MAX_SERVO_RULES, customServoMixers, PG_SERVO_MIXER, 1);
@ -83,7 +87,7 @@ void pgResetFn_customServoMixers(servoMixer_t *instance)
}
}
PG_REGISTER_ARRAY_WITH_RESET_FN(servoParam_t, MAX_SUPPORTED_SERVOS, servoParams, PG_SERVO_PARAMS, 2);
PG_REGISTER_ARRAY_WITH_RESET_FN(servoParam_t, MAX_SUPPORTED_SERVOS, servoParams, PG_SERVO_PARAMS, 3);
void pgResetFn_servoParams(servoParam_t *instance)
{
@ -113,6 +117,9 @@ static bool servoFilterIsSet;
static servoMetadata_t servoMetadata[MAX_SUPPORTED_SERVOS];
static rateLimitFilter_t servoSpeedLimitFilter[MAX_SERVO_RULES];
STATIC_FASTRAM pt1Filter_t rotRateFilter;
STATIC_FASTRAM pt1Filter_t targetRateFilter;
int16_t getFlaperonDirection(uint8_t servoPin)
{
if (servoPin == SERVO_FLAPPERON_2) {
@ -385,27 +392,31 @@ typedef enum {
AUTOTRIM_DONE,
} servoAutotrimState_e;
void processServoAutotrim(void)
void processServoAutotrimMode(void)
{
static servoAutotrimState_e trimState = AUTOTRIM_IDLE;
static timeMs_t trimStartedAt;
static int16_t servoMiddleBackup[MAX_SUPPORTED_SERVOS];
static int32_t servoMiddleAccum[MAX_SUPPORTED_SERVOS];
static int32_t servoMiddleAccumCount;
static int32_t servoMiddleAccumCount[MAX_SUPPORTED_SERVOS];
if (IS_RC_MODE_ACTIVE(BOXAUTOTRIM)) {
switch (trimState) {
case AUTOTRIM_IDLE:
if (ARMING_FLAG(ARMED)) {
// We are activating servo trim - backup current middles and prepare to average the data
for (int servoIndex = SERVO_ELEVATOR; servoIndex <= MIN(SERVO_RUDDER, MAX_SUPPORTED_SERVOS); servoIndex++) {
servoMiddleBackup[servoIndex] = servoParams(servoIndex)->middle;
servoMiddleAccum[servoIndex] = 0;
for (int axis = FD_ROLL; axis <= FD_YAW; axis++) {
for (int i = 0; i < servoRuleCount; i++) {
const uint8_t target = currentServoMixer[i].targetChannel;
const uint8_t source = currentServoMixer[i].inputSource;
if (source == axis) {
servoMiddleBackup[target] = servoParams(target)->middle;
servoMiddleAccum[target] = 0;
servoMiddleAccumCount[target] = 0;
}
}
}
trimStartedAt = millis();
servoMiddleAccumCount = 0;
trimState = AUTOTRIM_COLLECTING;
}
else {
@ -415,15 +426,26 @@ void processServoAutotrim(void)
case AUTOTRIM_COLLECTING:
if (ARMING_FLAG(ARMED)) {
servoMiddleAccumCount++;
for (int servoIndex = SERVO_ELEVATOR; servoIndex <= MIN(SERVO_RUDDER, MAX_SUPPORTED_SERVOS); servoIndex++) {
servoMiddleAccum[servoIndex] += servo[servoIndex];
for (int axis = FD_ROLL; axis <= FD_YAW; axis++) {
for (int i = 0; i < servoRuleCount; i++) {
const uint8_t target = currentServoMixer[i].targetChannel;
const uint8_t source = currentServoMixer[i].inputSource;
if (source == axis) {
servoMiddleAccum[target] += servo[target];
servoMiddleAccumCount[target]++;
}
}
}
if ((millis() - trimStartedAt) > SERVO_AUTOTRIM_TIMER_MS) {
for (int servoIndex = SERVO_ELEVATOR; servoIndex <= MIN(SERVO_RUDDER, MAX_SUPPORTED_SERVOS); servoIndex++) {
servoParamsMutable(servoIndex)->middle = servoMiddleAccum[servoIndex] / servoMiddleAccumCount;
for (int axis = FD_ROLL; axis <= FD_YAW; axis++) {
for (int i = 0; i < servoRuleCount; i++) {
const uint8_t target = currentServoMixer[i].targetChannel;
const uint8_t source = currentServoMixer[i].inputSource;
if (source == axis) {
servoParamsMutable(target)->middle = servoMiddleAccum[target] / servoMiddleAccumCount[target];
}
}
}
trimState = AUTOTRIM_SAVE_PENDING;
pidResetErrorAccumulators(); //Reset Iterm since new midpoints override previously acumulated errors
@ -449,8 +471,14 @@ void processServoAutotrim(void)
else {
// We are deactivating servo trim - restore servo midpoints
if (trimState == AUTOTRIM_SAVE_PENDING) {
for (int servoIndex = SERVO_ELEVATOR; servoIndex <= MIN(SERVO_RUDDER, MAX_SUPPORTED_SERVOS); servoIndex++) {
servoParamsMutable(servoIndex)->middle = servoMiddleBackup[servoIndex];
for (int axis = FD_ROLL; axis <= FD_YAW; axis++) {
for (int i = 0; i < servoRuleCount; i++) {
const uint8_t target = currentServoMixer[i].targetChannel;
const uint8_t source = currentServoMixer[i].inputSource;
if (source == axis) {
servoParamsMutable(target)->middle = servoMiddleBackup[target];
}
}
}
}
@ -458,6 +486,90 @@ void processServoAutotrim(void)
}
}
#define SERVO_AUTOTRIM_FILTER_CUTOFF 1 // LPF cutoff frequency
#define SERVO_AUTOTRIM_CENTER_MIN 1300
#define SERVO_AUTOTRIM_CENTER_MAX 1700
#define SERVO_AUTOTRIM_UPDATE_SIZE 5
void processContinuousServoAutotrim(const float dT)
{
static timeMs_t lastUpdateTimeMs;
static servoAutotrimState_e trimState = AUTOTRIM_IDLE;
static uint32_t servoMiddleUpdateCount;
const float rotRateMagnitude = sqrtf(vectorNormSquared(&imuMeasuredRotationBF));
const float rotRateMagnitudeFiltered = pt1FilterApply4(&rotRateFilter, rotRateMagnitude, SERVO_AUTOTRIM_FILTER_CUTOFF, dT);
const float targetRateMagnitude = getTotalRateTarget();
const float targetRateMagnitudeFiltered = pt1FilterApply4(&targetRateFilter, targetRateMagnitude, SERVO_AUTOTRIM_FILTER_CUTOFF, dT);
if (ARMING_FLAG(ARMED)) {
trimState = AUTOTRIM_COLLECTING;
if ((millis() - lastUpdateTimeMs) > 500) {
const bool planeIsFlyingStraight = rotRateMagnitudeFiltered <= DEGREES_TO_RADIANS(servoConfig()->servo_autotrim_rotation_limit);
const bool noRotationCommanded = targetRateMagnitudeFiltered <= servoConfig()->servo_autotrim_rotation_limit;
const bool planeIsFlyingLevel = calculateCosTiltAngle() >= 0.878153032f;
if (
planeIsFlyingStraight &&
noRotationCommanded &&
planeIsFlyingLevel &&
!FLIGHT_MODE(MANUAL_MODE) &&
isGPSHeadingValid() // TODO: proper flying detection
) {
// Plane is flying straight and level: trim servos
for (int axis = FD_ROLL; axis <= FD_YAW; axis++) {
// For each stabilized axis, add 5 units of I-term to all associated servo midpoints
const float axisIterm = getAxisIterm(axis);
if (fabsf(axisIterm) > SERVO_AUTOTRIM_UPDATE_SIZE) {
const int8_t ItermUpdate = axisIterm > 0.0f ? SERVO_AUTOTRIM_UPDATE_SIZE : -SERVO_AUTOTRIM_UPDATE_SIZE;
for (int i = 0; i < servoRuleCount; i++) {
#ifdef USE_PROGRAMMING_FRAMEWORK
if (!logicConditionGetValue(currentServoMixer[i].conditionId)) {
continue;
}
#endif
const uint8_t target = currentServoMixer[i].targetChannel;
const uint8_t source = currentServoMixer[i].inputSource;
if (source == axis) {
// Convert axis I-term to servo PWM and add to midpoint
const float mixerRate = currentServoMixer[i].rate / 100.0f;
const float servoRate = servoParams(target)->rate / 100.0f;
servoParamsMutable(target)->middle += ItermUpdate * mixerRate * servoRate;
servoParamsMutable(target)->middle = constrain(servoParamsMutable(target)->middle, SERVO_AUTOTRIM_CENTER_MIN, SERVO_AUTOTRIM_CENTER_MAX);
}
}
pidReduceErrorAccumulators(ItermUpdate, axis);
}
}
servoMiddleUpdateCount++;
}
// Reset timer
lastUpdateTimeMs = millis();
}
} else if (trimState == AUTOTRIM_COLLECTING) {
// We have disarmed, save midpoints to EEPROM
writeEEPROM();
trimState = AUTOTRIM_IDLE;
}
// Debug
DEBUG_SET(DEBUG_AUTOTRIM, 0, servoParams(2)->middle);
DEBUG_SET(DEBUG_AUTOTRIM, 2, servoParams(3)->middle);
DEBUG_SET(DEBUG_AUTOTRIM, 4, servoParams(4)->middle);
DEBUG_SET(DEBUG_AUTOTRIM, 6, servoParams(5)->middle);
DEBUG_SET(DEBUG_AUTOTRIM, 1, servoMiddleUpdateCount);
DEBUG_SET(DEBUG_AUTOTRIM, 3, MAX(RADIANS_TO_DEGREES(rotRateMagnitudeFiltered), targetRateMagnitudeFiltered));
DEBUG_SET(DEBUG_AUTOTRIM, 5, axisPID_I[FD_ROLL]);
DEBUG_SET(DEBUG_AUTOTRIM, 7, axisPID_I[FD_PITCH]);
}
void processServoAutotrim(const float dT) {
if (feature(FEATURE_FW_AUTOTRIM)) {
processContinuousServoAutotrim(dT);
} else {
processServoAutotrimMode();
}
}
bool isServoOutputEnabled(void)
{
return servoOutputEnabled;

2
src/main/flight/servos.h
View file

@ -138,6 +138,8 @@ typedef struct servoConfig_s {
uint16_t flaperon_throw_offset;
uint8_t servo_protocol; // See servoProtocolType_e
uint8_t tri_unarmed_servo; // send tail servo correction pulses even when unarmed
uint8_t servo_autotrim_rotation_limit; // Max rotation for servo midpoints to be updated
uint8_t servo_autotrim_iterm_threshold; // How much of the Iterm is carried over to the servo midpoints on each update
} servoConfig_t;
PG_DECLARE(servoConfig_t, servoConfig);