Removing quadcopter drift to the side

Question

I wrote my own quadcopter firmware which is based on some older code. This code shall stabilize the copter to be always in equilibrium. The model is behaving relatively nice. I can control it with my laptop. However I noticed, that the copter is hovering to the side (if not manually controlled), likely because of wind, not well balanced or turbulence.

My idea was maybe to fuse GPS and accelerometer data to implement a function which shall help to hold the position. But this will likely only work if I have a hold altitude function, because changes in pitch or roll change the height, because the thrust is changed slightly. This is why I recently added a routine which shall allow to hold the altitude.

Is someone having experiences with this? I mean with avoiding side drifts of the model because of whatever by software? The problem is in my opinion, that I don't know whether the position change is wanted (by remote control) or not. Additionally it is hard to localize the correct position and calculate the distance caused by drift from it (just with GPS, but this is not precise).

void hold_altitude(int_fast16_t &iFL, int_fast16_t &iBL, int_fast16_t &iFR, int_fast16_t &iBR, 
                   const int_fast32_t rcalt_m) 
{
  // Enhance the performance:
  // This function is only needed for (semi-)autonomous flight mode like:
  // * Hold altitude
  // * GPS auto-navigation
  if(_RECVR.m_Waypoint.m_eMode == GPSPosition::NOTHING_F) {
    return;
  }

  // Return estimated altitude by GPS and barometer 
  float fCurAlti_cm       = _HAL_BOARD.get_alti_m() * 100.f;
  // Estimate current climb rate
  float fBaroClimb_cms    = _HAL_BOARD.get_baro().climb_rate_ms * 100;
  float fAcclClimb_cms    = _HAL_BOARD.get_accel_ms().z * 100;
  // calculate the 
  float fAltStabOut      = _HAL_BOARD.m_rgPIDS[PID_THR_STAB].get_pid(fCurAlti_cm - (float)(rcalt_m*100), 1);
  float fBarAcclOut      = _HAL_BOARD.m_rgPIDS[PID_THR_ACCL].get_pid(fAltStabOut - fBaroClimb_cms, 1);
  float fAccAcclOut      = _HAL_BOARD.m_rgPIDS[PID_THR_ACCL].get_pid(fAltStabOut - fAcclClimb_cms, 1);
  int_fast16_t iAltOutput = _HAL_BOARD.m_rgPIDS[PID_THR_RATE].get_pid(fAltStabOut - (fBarAcclOut + fAccAcclOut), 1);
  // Modify the speed of the motors
  iFL += iAltOutput;
  iBL += iAltOutput;
  iFR += iAltOutput;
  iBR += iAltOutput;
}

Copter control:

// Stabilise PIDS
float pit_stab_output = constrain_float(_HAL_BOARD.m_rgPIDS[PID_PIT_STAB].get_pid((float)rcpit - vAtti.x, 1), -250, 250);
float rol_stab_output = constrain_float(_HAL_BOARD.m_rgPIDS[PID_ROL_STAB].get_pid((float)rcrol - vAtti.y, 1), -250, 250);
float yaw_stab_output = constrain_float(_HAL_BOARD.m_rgPIDS[PID_YAW_STAB].get_pid(wrap180_f(targ_yaw - vAtti.z), 1), -360, 360);

// is pilot asking for yaw change - if so feed directly to rate pid (overwriting yaw stab output)
if(abs(rcyaw ) > 5.f) {
  yaw_stab_output = rcyaw;
  targ_yaw = vAtti.z; // remember this yaw for when pilot stops
}

// rate PIDS
int_fast16_t pit_output = (int_fast16_t)constrain_float(_HAL_BOARD.m_rgPIDS[PID_PIT_RATE].get_pid(pit_stab_output - vGyro.x, 1), -500, 500);
int_fast16_t rol_output = (int_fast16_t)constrain_float(_HAL_BOARD.m_rgPIDS[PID_ROL_RATE].get_pid(rol_stab_output - vGyro.y, 1), -500, 500);
int_fast16_t yaw_output = (int_fast16_t)constrain_float(_HAL_BOARD.m_rgPIDS[PID_YAW_RATE].get_pid(yaw_stab_output - vGyro.z, 1), -500, 500);

int_fast16_t iFL = rcthr + rol_output + pit_output - yaw_output;
int_fast16_t iBL = rcthr + rol_output - pit_output + yaw_output;
int_fast16_t iFR = rcthr - rol_output + pit_output + yaw_output;
int_fast16_t iBR = rcthr - rol_output - pit_output - yaw_output;
// Hold the altitude
hold_altitude(iFL, iBL, iFR, iBR, rcalt);

hal.rcout->write(MOTOR_FL, iFL);
hal.rcout->write(MOTOR_BL, iBL);
hal.rcout->write(MOTOR_FR, iFR);
hal.rcout->write(MOTOR_BR, iBR);

score 2 · Accepted Answer · answered Mar 21 '14 at 17:03

2

The drift is being caused either by your position data being consistently wrong, or by your roll/pitch PIDs not correcting the drift error with their integral term.

In any case, it is the integral term of a PID that (when working properly) prevents drift.

answered Mar 21 '14 at 17:03

Ian

11,013
2
23
65

At the moment my copter has no position data at all. Only attitude/altitude. I reduced my PIDs to a relatively low value, because I noticed, that the copter was overcompensating. Maybe I should try it with increasing the PIDs again stepwise first. – dgrat Mar 21 '14 at 21:38
Oh, if you're not controlling position (i.e. reading position and adjusting for error) then you have no way to eliminate the drift. Under perfect conditions you can make the drift very small, but the only practical way to hold position is to measure position. – Ian Mar 23 '14 at 19:10

score 0 · Answer 2 · answered Mar 21 '14 at 04:23

0

Do you have Gyros (roll pitch and yaw) installed? I would recommend those, since they give stability feedback to your code. If possible, you can even add a control board (from Hobbyking, kkV2). This board has 3 gyros, inputs to ale, rud, thr, ele. But the best thing is, it has gain adjusters for all 3 gyros, which means you can adjust them till your quad is stable (they adjust the power given to motors to keep the quad flat)

answered Mar 21 '14 at 04:23

AVJ

271
1
5

I have an APM 2.5 board from Hobby King. This board has a 3-axis gyrometer/accelerometer and a barometer :D Most of the interial sensor fusion is working already (with an elegant way). I just don't know exactly how to remove the side drift theoretically in a nice way. – dgrat Mar 21 '14 at 09:50

Removing quadcopter drift to the side

2 Answers2