72 lines
1.7 KiB
C
72 lines
1.7 KiB
C
#include <stdbool.h>
|
|
#include <stdint.h>
|
|
#include <math.h>
|
|
|
|
#include "common/axis.h"
|
|
#include "common/maths.h"
|
|
|
|
#include "flight_common.h"
|
|
|
|
// To adjust how agressive the tuning is, adjust the AUTOTUNEMAXOSCILLATION value. A larger
|
|
// value will result in more agressive tuning. A lower value will result in softer tuning.
|
|
// It will rock back and forth between -AUTOTUNE_TARGET_ANGLE and AUTOTUNE_TARGET_ANGLE degrees
|
|
// AUTOTUNE_D_MULTIPLIER is a multiplier that puts in a little extra D when autotuning is done. This helps damp
|
|
// the wobbles after a quick angle change.
|
|
// Always autotune on a full battery.
|
|
#define AUTOTUNE_MAX_OSCILLATION 1.0
|
|
#define AUTOTUNE_TARGET_ANGLE 20.0
|
|
#define AUTOTUNE_D_MULTIPLIER 1.2
|
|
|
|
static pidProfile_t *pidProfile;
|
|
static uint8_t pidController;
|
|
static bool rising;
|
|
static float targetAngle = 0;
|
|
|
|
static angle_index_t autoTuneAngleIndex;
|
|
|
|
float autotune(angle_index_t angleIndex, rollAndPitchInclination_t *inclination, float errorAngle)
|
|
{
|
|
if (autoTuneAngleIndex != angleIndex) {
|
|
// Not tuning this angle yet.
|
|
return errorAngle;
|
|
}
|
|
|
|
// TODO autotune!
|
|
|
|
if (rising) {
|
|
targetAngle = AUTOTUNE_TARGET_ANGLE;
|
|
}
|
|
else {
|
|
targetAngle = -AUTOTUNE_TARGET_ANGLE;
|
|
}
|
|
|
|
|
|
return targetAngle - inclination->rawAngles[angleIndex];
|
|
}
|
|
|
|
void autotuneReset(void)
|
|
{
|
|
targetAngle = 0;
|
|
rising = true;
|
|
autoTuneAngleIndex = AI_ROLL;
|
|
}
|
|
|
|
void autotuneBegin(pidProfile_t *pidProfileToTune, uint8_t pidControllerInUse)
|
|
{
|
|
autotuneReset();
|
|
|
|
pidProfile = pidProfileToTune;
|
|
pidController = pidControllerInUse;
|
|
}
|
|
|
|
void autotuneEnd(void)
|
|
{
|
|
|
|
}
|
|
|
|
bool havePidsBeenUpdatedByAutotune(void)
|
|
{
|
|
return targetAngle != 0;
|
|
}
|
|
|