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

Added posDecelerationTime (controlled by Pos I) to control braking strength

Browse source
This commit is contained in:
Konstantin Sharlaimov (DigitalEntity) 2015-11-30 20:14:30 +10:00
parent b40df56f16
commit fd28138535
4 changed files with 9 additions and 5 deletions
Download patch file Download diff file Expand all files Collapse all files

2
src/main/config/config.c
View file

@ -163,7 +163,7 @@ static void resetPidProfile(pidProfile_t *pidProfile)
pidProfile->I8[PIDALT] = 0; // not used pidProfile->I8[PIDALT] = 0; // not used
pidProfile->D8[PIDALT] = 0; // not used pidProfile->D8[PIDALT] = 0; // not used
pidProfile->P8[PIDPOS] = 15; // NAV_POS_XY_P * 100 pidProfile->P8[PIDPOS] = 15; // NAV_POS_XY_P * 100
pidProfile->I8[PIDPOS] = 0; // not used pidProfile->I8[PIDPOS] = 100; // posDecelerationTime * 100
pidProfile->D8[PIDPOS] = 0; // not used pidProfile->D8[PIDPOS] = 0; // not used
pidProfile->P8[PIDPOSR] = 90; // NAV_VEL_XY_P * 100 pidProfile->P8[PIDPOSR] = 90; // NAV_VEL_XY_P * 100
pidProfile->I8[PIDPOSR] = 15; // NAV_VEL_XY_I * 100 pidProfile->I8[PIDPOSR] = 15; // NAV_VEL_XY_I * 100

5
src/main/flight/navigation_rewrite.c
View file

@ -1909,7 +1909,10 @@ void navigationUsePIDs(pidProfile_t *initialPidProfile)
posControl.pidProfile = initialPidProfile; posControl.pidProfile = initialPidProfile;
// Initialize position hold PI-controller // Brake time parameter
posControl.posDecelerationTime = (float)posControl.pidProfile->I8[PIDPOS] / 100.0f;
// Initialize position hold P-controller
for (axis = 0; axis < 2; axis++) { for (axis = 0; axis < 2; axis++) {
navPInit(&posControl.pids.pos[axis], (float)posControl.pidProfile->P8[PIDPOS] / 100.0f); navPInit(&posControl.pids.pos[axis], (float)posControl.pidProfile->P8[PIDPOS] / 100.0f);

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

@ -474,8 +474,8 @@ void applyMulticopterEmergencyLandingController(uint32_t currentTime)
*-----------------------------------------------------------*/ *-----------------------------------------------------------*/
void calculateMulticopterInitialHoldPosition(t_fp_vector * pos) void calculateMulticopterInitialHoldPosition(t_fp_vector * pos)
{ {
float stoppingDistanceX = posControl.actualState.vel.V.X / posControl.pids.pos[X].param.kP; float stoppingDistanceX = posControl.actualState.vel.V.X * posControl.posDecelerationTime;
float stoppingDistanceY = posControl.actualState.vel.V.Y / posControl.pids.pos[Y].param.kP; float stoppingDistanceY = posControl.actualState.vel.V.Y * posControl.posDecelerationTime;
pos->V.X = posControl.actualState.pos.V.X + stoppingDistanceX; pos->V.X = posControl.actualState.pos.V.X + stoppingDistanceX;
pos->V.Y = posControl.actualState.pos.V.Y + stoppingDistanceY; pos->V.Y = posControl.actualState.pos.V.Y + stoppingDistanceY;

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

@ -223,8 +223,9 @@ typedef struct {
bool enabled; bool enabled;
navigationFlags_t flags; navigationFlags_t flags;
/* Navigation PID controllers */ /* Navigation PID controllers + pre-computed flight parameters */
navigationPIDControllers_t pids; navigationPIDControllers_t pids;
float posDecelerationTime;
/* Local system state, both actual (estimated) and desired (target setpoint)*/ /* Local system state, both actual (estimated) and desired (target setpoint)*/
navigationEstimatedState_t actualState; navigationEstimatedState_t actualState;