Mirror/betaflight
1
0
Fork 0
mirror of https://github.com/betaflight/betaflight.git synced 2025-07-26 09:45:37 +03:00
Code Issues Wiki Activity

first working example with smoothed acceleration

Browse source
This commit is contained in:
ctzsnooze 2024-10-09 12:44:48 +11:00
parent 70b1097f08
commit 897b9a9a61
16 changed files with 154 additions and 102 deletions
Download patch file Download diff file Expand all files Collapse all files

6
src/main/blackbox/blackbox.c
View file

@ -1813,7 +1813,11 @@ static bool blackboxWriteSysinfo(void)
#endif // USE_GPS
#ifdef USE_ALT_HOLD_MODE
BLACKBOX_PRINT_HEADER_LINE(PARAM_NAME_ALT_HOLD_TARGET_ADJUST_RATE, "%d", altholdConfig()->alt_hold_target_adjust_rate);
BLACKBOX_PRINT_HEADER_LINE(PARAM_NAME_ALT_HOLD_ADJUST_RATE, "%d", altHoldConfig()->alt_hold_adjust_rate);
#endif
#ifdef USE_POS_HOLD_MODE
BLACKBOX_PRINT_HEADER_LINE(PARAM_NAME_POS_HOLD_ADJUST_RATE, "%d", posHoldConfig()->pos_hold_adjust_rate);
#endif
#ifdef USE_WING

6
src/main/cli/settings.c
View file

@ -1111,11 +1111,11 @@ const clivalue_t valueTable[] = {
{ "yaw_control_reversed", VAR_INT8 | MASTER_VALUE | MODE_LOOKUP, .config.lookup = { TABLE_OFF_ON }, PG_RC_CONTROLS_CONFIG, offsetof(rcControlsConfig_t, yaw_control_reversed) },
#ifdef USE_ALT_HOLD_MODE
{ PARAM_NAME_ALT_HOLD_TARGET_ADJUST_RATE, VAR_UINT8 | MASTER_VALUE, .config.minmaxUnsigned = { 0, 200 }, PG_ALTHOLD_CONFIG, offsetof(altholdConfig_t, alt_hold_target_adjust_rate) },
{ PARAM_NAME_ALT_HOLD_ADJUST_RATE, VAR_UINT8 | MASTER_VALUE, .config.minmaxUnsigned = { 0, 200 }, PG_ALTHOLD_CONFIG, offsetof(altHoldConfig_t, alt_hold_adjust_rate) },
#endif
#ifdef USE_POSs_HOLD_MODE
{ PARAM_NAME_pos_hold_adjust_rate, VAR_UINT8 | MASTER_VALUE, .config.minmaxUnsigned = { 0, 200 }, PG_POSHOLD_CONFIG, offsetof(posHoldConfig_t, pos_hold_adjust_rate) },
#ifdef USE_POS_HOLD_MODE
{ PARAM_NAME_POS_HOLD_ADJUST_RATE, VAR_UINT8 | MASTER_VALUE, .config.minmaxUnsigned = { 0, 200 }, PG_POSHOLD_CONFIG, offsetof(posHoldConfig_t, pos_hold_adjust_rate) },
#endif
// PG_PID_CONFIG

31
src/main/common/maths.c
View file

@ -49,14 +49,33 @@
#define sinPolyCoef7 -1.980661520e-4f // Double: -1.980661520135080504411629636078917643846e-4
#define sinPolyCoef9 2.600054768e-6f // Double: 2.600054767890361277123254766503271638682e-6
#endif
// float sin_approx(float x)
// {
// int32_t xint = x;
// if (xint < -32 || xint > 32) return 0.0f; // Stop here on error input (5 * 360 Deg)
// while (x > M_PIf) x -= (2.0f * M_PIf); // always wrap input angle to -PI..PI
// while (x < -M_PIf) x += (2.0f * M_PIf);
// if (x > (0.5f * M_PIf)) x = (0.5f * M_PIf) - (x - (0.5f * M_PIf)); // We just pick -90..+90 Degree
// else if (x < -(0.5f * M_PIf)) x = -(0.5f * M_PIf) - ((0.5f * M_PIf) + x);
// float x2 = x * x;
// return x + x * x2 * (sinPolyCoef3 + x2 * (sinPolyCoef5 + x2 * (sinPolyCoef7 + x2 * sinPolyCoef9)));
// }
// alternate method suggested by chatgpt
float sin_approx(float x)
{
int32_t xint = x;
if (xint < -32 || xint > 32) return 0.0f; // Stop here on error input (5 * 360 Deg)
while (x > M_PIf) x -= (2.0f * M_PIf); // always wrap input angle to -PI..PI
while (x < -M_PIf) x += (2.0f * M_PIf);
if (x > (0.5f * M_PIf)) x = (0.5f * M_PIf) - (x - (0.5f * M_PIf)); // We just pick -90..+90 Degree
else if (x < -(0.5f * M_PIf)) x = -(0.5f * M_PIf) - ((0.5f * M_PIf) + x);
// Normalize input to the range -PI to PI
x = fmodf(x, 2.0f * M_PIf);
if (x < -M_PIf) x += 2.0f * M_PIf;
if (x > M_PIf) x -= 2.0f * M_PIf;
// Use symmetry to handle inputs greater than ±90 degrees
if (x > M_PIf / 2) {
x = M_PIf - x; // Reflect
} else if (x < -M_PIf / 2) {
x = -M_PIf - x; // Reflect
}
float x2 = x * x;
return x + x * x2 * (sinPolyCoef3 + x2 * (sinPolyCoef5 + x2 * (sinPolyCoef7 + x2 * sinPolyCoef9)));
}

6
src/main/fc/parameter_names.h
View file

@ -245,9 +245,13 @@
#endif // USE_GPS
#ifdef USE_ALT_HOLD_MODE
#define PARAM_NAME_ALT_HOLD_TARGET_ADJUST_RATE "alt_hold_target_adjust_rate"
#define PARAM_NAME_ALT_HOLD_ADJUST_RATE "alt_hold_adjust_rate"
#endif // USE_ALT_HOLD_MODE
#ifdef USE_POS_HOLD_MODE
#define PARAM_NAME_POS_HOLD_ADJUST_RATE "pos_hold_adjust_rate"
#endif // USE_POS_HOLD_MODE
#define PARAM_NAME_IMU_DCM_KP "imu_dcm_kp"
#define PARAM_NAME_IMU_DCM_KI "imu_dcm_ki"
#define PARAM_NAME_IMU_SMALL_ANGLE "small_angle"

2
src/main/fc/rc_modes.h
View file

@ -34,10 +34,10 @@ typedef enum {
BOXHORIZON,
BOXMAG,
BOXALTHOLD,
BOXPOSHOLD,
BOXHEADFREE,
BOXPASSTHRU,
BOXFAILSAFE,
BOXPOSHOLD,
BOXGPSRESCUE,
BOXID_FLIGHTMODE_LAST = BOXGPSRESCUE,

4
src/main/flight/alt_hold.c
View file

@ -128,7 +128,7 @@ void altHoldUpdateTargetAltitude(void)
// constant (set) deceleration target in the last 2m
throttleAdjustmentFactor = -(0.9f + constrainf(getAltitudeCm() / 2000.0f, 0.1f, 9.0f));
}
altHoldState.targetAltitudeAdjustRate = throttleAdjustmentFactor * altholdConfig()->alt_hold_target_adjust_rate;
altHoldState.targetAltitudeAdjustRate = throttleAdjustmentFactor * altHoldConfig()->alt_hold_adjust_rate;
// if taskRate is 100Hz, default adjustRate of 100 adds/subtracts 1m every second, or 1cm per task run, at full stick position
altHoldState.targetAltitudeCm += altHoldState.targetAltitudeAdjustRate * taskIntervalSeconds;
@ -137,7 +137,7 @@ void altHoldUpdateTargetAltitude(void)
void altHoldUpdate(void)
{
// check if the user has changed the target altitude using sticks
if (altholdConfig()->alt_hold_target_adjust_rate) {
if (altHoldConfig()->alt_hold_adjust_rate) {
altHoldUpdateTargetAltitude();
}

62
src/main/flight/autopilot.c
View file

@ -22,7 +22,9 @@
#include "platform.h"
#include "build/debug.h"
#include "common/filter.h"
#include "common/maths.h"
#include "flight/imu.h"
#include "flight/pid.h"
#include "flight/position.h"
@ -34,15 +36,20 @@
#define ALTITUDE_I_SCALE 0.003f
#define ALTITUDE_D_SCALE 0.01f
#define ALTITUDE_F_SCALE 0.01f
#define POSITION_P_SCALE 0.01f
#define POSITION_I_SCALE 0.01f
#define POSITION_D_SCALE 0.01f
static pidCoefficient_t altitudePidCoeffs;
static pidCoefficient_t positionPidCoeffs;
static float altitudeI = 0.0f;
static float throttleOut = 0.0f;
static pidCoefficient_t positionPidCoeffs;
static float previousDistanceCm = 0.0f;
static float previousVelocity = 0.0f;
static bool newGPSData = false;
float posHoldAngle[ANGLE_INDEX_COUNT];
static pt1Filter_t accelerationLpf;
static float positionAccelerationCutoffHz;
void autopilotInit(const autopilotConfig_t *config)
{
@ -50,15 +57,13 @@ void autopilotInit(const autopilotConfig_t *config)
altitudePidCoeffs.Ki = config->altitude_I * ALTITUDE_I_SCALE;
altitudePidCoeffs.Kd = config->altitude_D * ALTITUDE_D_SCALE;
altitudePidCoeffs.Kf = config->altitude_F * ALTITUDE_F_SCALE;
positionPidCoeffs.Kp = config->position_P * ALTITUDE_P_SCALE;
positionPidCoeffs.Ki = config->position_I * ALTITUDE_I_SCALE;
positionPidCoeffs.Kd = config->position_D * ALTITUDE_D_SCALE;
// positionPidCoeffs.Kf = config->position_F * ALTITUDE_F_SCALE;
}
positionPidCoeffs.Kp = config->position_P * POSITION_P_SCALE;
positionPidCoeffs.Ki = config->position_I * POSITION_I_SCALE;
positionPidCoeffs.Kd = config->position_D * POSITION_D_SCALE;
void posHoldNewGpsData(void)
{
newGPSData = true;
positionAccelerationCutoffHz = config->position_filter_hz / 100.0f;
const float gain = pt1FilterGain(positionAccelerationCutoffHz, 0.1f); // assume 10Hz GPS connection at start
pt1FilterInit(&accelerationLpf, gain);
}
const pidCoefficient_t *getAltitudePidCoeffs(void)
@ -123,9 +128,8 @@ void positionControl(gpsLocation_t targetLocation) {
// gpsSol.llh = current gps location
// get distance and bearing from current location to target location
// void GPS_distance_cm_bearing(const gpsLocation_t *from, const gpsLocation_t* to, bool dist3d, uint32_t *pDist, int32_t *pBearing)
float rollSetpoint = 0.0f;
float pitchSetpoint = 0.0f;
if (newGPSData) {
static float rollSetpoint = 0.0f;
static float pitchSetpoint = 0.0f;
uint32_t distanceCm;
int32_t bearing; // degrees * 100
GPS_distance_cm_bearing(&gpsSol.llh, &targetLocation, false, &distanceCm, &bearing);
@ -137,7 +141,6 @@ void positionControl(gpsLocation_t targetLocation) {
normalisedErrorAngle -= 360.0f; // Range: -180 to 180
}
// Convert to radians for sine and cosine functions
float errorAngleRadians = normalisedErrorAngle * (M_PI / 180.0f);
// Calculate correction factors using sine and cosine
@ -152,37 +155,38 @@ void positionControl(gpsLocation_t targetLocation) {
previousVelocity = velocity;
// ** THIS WILL NEED A FILTER LIKE FOR GPS RESCUE**
float velocityP = velocity * positionPidCoeffs.Kp; // proportional to velocity
float velocityI = distanceCm * positionPidCoeffs.Ki; // proportional to distance error
float velocityD = acceleration * positionPidCoeffs.Kd; // proportional to acceleration *needs filtering*
// F would be a value proportional to target change (not done yet)
const float gain = pt1FilterGain(positionAccelerationCutoffHz, gpsDataIntervalS);
pt1FilterUpdateCutoff(&accelerationLpf, gain);
acceleration = pt1FilterApply(&accelerationLpf, acceleration);
float velocityP = velocity * positionPidCoeffs.Kp; // velocity away from intended location
float velocityI = distanceCm * positionPidCoeffs.Ki; // integral of velocity is distance from intended location
float velocityD = acceleration * positionPidCoeffs.Kd; // acceleration away from intended location
float pidSum = velocityP + velocityI + velocityD; // greater when position error is bad and getting worse
// value sent to
rollSetpoint = rollCorrection * pidSum; // with some gain adjustment to give reasonable number
rollSetpoint = rollCorrection * pidSum;
pitchSetpoint = pitchCorrection * pidSum;
DEBUG_SET(DEBUG_AUTOPILOT_POSITION, 0, lrintf(normalisedErrorAngle));
DEBUG_SET(DEBUG_AUTOPILOT_POSITION, 1, lrintf(rollCorrection));
DEBUG_SET(DEBUG_AUTOPILOT_POSITION, 2, lrintf(pitchCorrection));
DEBUG_SET(DEBUG_AUTOPILOT_POSITION, 0, lrintf(normalisedErrorAngle)); //-180 to +180
DEBUG_SET(DEBUG_AUTOPILOT_POSITION, 1, lrintf(rollCorrection * 100));
DEBUG_SET(DEBUG_AUTOPILOT_POSITION, 2, lrintf(pitchCorrection * 100));
DEBUG_SET(DEBUG_AUTOPILOT_POSITION, 3, lrintf(distanceCm));
DEBUG_SET(DEBUG_AUTOPILOT_POSITION, 4, lrintf(velocityP));
DEBUG_SET(DEBUG_AUTOPILOT_POSITION, 5, lrintf(velocityI));
DEBUG_SET(DEBUG_AUTOPILOT_POSITION, 6, lrintf(velocityD));
newGPSData = false;
}
// send setpoints to pid.c using the same method as for gpsRescueAngle
// value sent should be in degrees * 100 (why??)
// send setpoints to pid.c using a method similar to that in gpsRescueAngle[axis]
// value sent needs shoiuld be in degrees * 100
// values will have steps at GPS rate, if too jumpy we would need to upsample smooth them
posHoldAngle[AI_ROLL] = rollSetpoint;
posHoldAngle[AI_PITCH] = pitchSetpoint;
DEBUG_SET(DEBUG_AUTOPILOT_POSITION, 7, lrintf(rollSetpoint));
// but for now let's not really do that until we get the PIDs sorted out :-)
posHoldAngle[AI_PITCH] = 0.0f;
posHoldAngle[AI_ROLL] = 0.0f;
// posHoldAngle[AI_PITCH] = 0.0f;
// posHoldAngle[AI_ROLL] = 0.0f;
}
bool isBelowLandingAltitude(void)

18
src/main/flight/pos_hold.c
View file

@ -53,7 +53,6 @@ void posHoldProcessTransitions(void) {
if (FLIGHT_MODE(POS_HOLD_MODE)) {
if (!posHoldState.isPosHoldActive) {
posHoldReset();
DEBUG_SET(DEBUG_AUTOPILOT_POSITION, 0, 22);
posHoldState.isPosHoldActive = true;
}
} else {
@ -68,26 +67,27 @@ void posHoldUpdateTargetLocation(void)
// We need to provide a big deadband in rc.c
if (!failsafeIsActive()) {
// most easily...
// fly the quad, in angle mode, enabling a deadband via rc.c (?)
// while sticks are inside the deadband,
const float deadband = 0.2f;
if ((getRcDeflectionAbs(FD_ROLL) > deadband) || (getRcDeflectionAbs(FD_PITCH) > deadband)) {
// allow user to fly the quad, in angle mode, enabling a 20% deadband via rc.c (?)
// while sticks are outside the deadband,
// set the target location to the current GPS location each iteration
// posHoldState.targetLocation = currentLocation;
posHoldState.targetLocation = gpsSol.llh;
}
}
}
void posHoldUpdate(void)
{
// check if the user has changed the target altitude using sticks
// check if the user wants to change the target position using sticks
if (posHoldConfig()->pos_hold_adjust_rate) {
posHoldUpdateTargetLocation();
}
// run a function in autopilot.c to adjust position
if (getIsNewDataForPosHold()) {
positionControl(posHoldState.targetLocation);
} else {
}
}
void updatePosHoldState(timeUs_t currentTimeUs) {

15
src/main/io/gps.c
View file

@ -53,9 +53,9 @@
#include "fc/gps_lap_timer.h"
#include "fc/runtime_config.h"
#include "flight/gps_rescue.h"
#include "flight/imu.h"
#include "flight/pid.h"
#include "flight/gps_rescue.h"
#include "scheduler/scheduler.h"
@ -93,6 +93,7 @@ GPS_svinfo_t GPS_svinfo[GPS_SV_MAXSATS_M8N];
static serialPort_t *gpsPort;
static float gpsDataIntervalSeconds;
static bool newDataForPosHold = false;
typedef struct gpsInitData_s {
uint8_t index;
@ -2608,13 +2609,17 @@ void onGpsNewData(void)
gpsRescueNewGpsData();
#endif
#ifdef USE_POS_HOLD
posHoldNewGpsData();
#endif
#ifdef USE_GPS_LAP_TIMER
gpsLapTimerNewGpsData();
#endif // USE_GPS_LAP_TIMER
newDataForPosHold = true;
}
bool getIsNewDataForPosHold(void) {
bool currentState = newDataForPosHold; // true only when new data arrives
newDataForPosHold = false; // clear flag once new data has been handled
return currentState;
}
void gpsSetFixState(bool state)

3
src/main/io/gps.h
View file

@ -389,6 +389,9 @@ void onGpsNewData(void);
void GPS_reset_home_position(void);
void GPS_calc_longitude_scaling(int32_t lat);
void GPS_distance_cm_bearing(const gpsLocation_t *from, const gpsLocation_t *to, bool dist3d, uint32_t *dist, int32_t *bearing);
void gpsSetFixState(bool state);
float getGpsDataIntervalSeconds(void); // sends GPS Nav Data interval to GPS Rescue
bool getIsNewDataForPosHold(void);
baudRate_e getGpsPortActualBaudRateIndex(void);

6
src/main/pg/alt_hold.c
View file

@ -29,9 +29,9 @@
#include "alt_hold.h"
PG_REGISTER_WITH_RESET_TEMPLATE(altholdConfig_t, altholdConfig, PG_ALTHOLD_CONFIG, 4);
PG_REGISTER_WITH_RESET_TEMPLATE(altHoldConfig_t, altHoldConfig, PG_ALTHOLD_CONFIG, 4);
PG_RESET_TEMPLATE(altholdConfig_t, altholdConfig,
.alt_hold_target_adjust_rate = 100, // max rate of change of altitude target using sticks in cm/s
PG_RESET_TEMPLATE(altHoldConfig_t, altHoldConfig,
.alt_hold_adjust_rate = 100, // max rate of change of altitude target using sticks in cm/s
);
#endif

8
src/main/pg/alt_hold.h
View file

@ -24,9 +24,9 @@
#include "pg/pg.h"
typedef struct altholdConfig_s {
uint8_t alt_hold_target_adjust_rate;
} altholdConfig_t;
typedef struct altHoldConfig_s {
uint8_t alt_hold_adjust_rate;
} altHoldConfig_t;
PG_DECLARE(altholdConfig_t, altholdConfig);
PG_DECLARE(altHoldConfig_t, altHoldConfig);

3
src/main/pg/autopilot.c
View file

@ -38,8 +38,9 @@ PG_RESET_TEMPLATE(autopilotConfig_t, autopilotConfig,
.altitude_I = 15,
.altitude_D = 15,
.altitude_F = 15,
.position_P = 8,
.position_P = 15,
.position_I = 40,
.position_D = 15,
// .position_F = 0,
.position_filter_hz = 75,
);

1
src/main/pg/autopilot.h
View file

@ -37,6 +37,7 @@ typedef struct autopilotConfig_s {
uint8_t position_I;
uint8_t position_D;
// uint8_t position_F;
uint8_t position_filter_hz;
} autopilotConfig_t;
PG_DECLARE(autopilotConfig_t, autopilotConfig);

2
src/main/telemetry/crsf.c
View file

@ -459,7 +459,7 @@ void crsfFrameFlightMode(sbuf_t *dst)
flightMode = "ANGL";
} else if (FLIGHT_MODE(ALT_HOLD_MODE)) {
flightMode = "ALTH";
} else if (FLIGHT_MODE(ALT_HOLD_MODE)) {
} else if (FLIGHT_MODE(POS_HOLD_MODE)) {
flightMode = "POSH";
} else if (FLIGHT_MODE(HORIZON_MODE)) {
flightMode = "HOR";

13
src/test/unit/althold_unittest.cc
View file

@ -44,7 +44,7 @@ extern "C" {
PG_REGISTER(accelerometerConfig_t, accelerometerConfig, PG_ACCELEROMETER_CONFIG, 0);
PG_REGISTER(positionConfig_t, positionConfig, PG_POSITION, 0);
PG_REGISTER(autopilotConfig_t, autopilotConfig, PG_AUTOPILOT, 0);
PG_REGISTER(altholdConfig_t, altholdConfig, PG_ALTHOLD_CONFIG, 0);
PG_REGISTER(altHoldConfig_t, altHoldConfig, PG_ALTHOLD_CONFIG, 0);
extern altHoldState_t altHoldState;
void altHoldInit(void);
@ -103,6 +103,17 @@ extern "C" {
float getCosTiltAngle(void) { return 0.0f; }
float rcCommand[4];
void GPS_distance_cm_bearing(&gpsSol.llh, &targetLocation, false, &distanceCm, &bearing)
{
UNUSED(gpsSol.llh);
UNUSED(targetLocation);
UNUSED(false);
UNUSED(distanceCm);
UNUSED(bearing);
}
void parseRcChannels(const char *input, rxConfig_t *rxConfig)
{
UNUSED(input);