Setpoints of client

Firmware/software/electronics/mechanics
Post Reply
RyanMco
Expert
Posts: 159
Joined: Tue Apr 09, 2019 6:15 am

Setpoints of client

Post by RyanMco »

Hi guys!
I started understanding the code of firmware and how crazyflie is working in generally ..
what's still confusing me that how the crazyflie after getting the data from kalman filter (final estimated pose) doing some manipulation regarding to my setpoint that I entered in client and then the crazyflie tries/attempts to adjust itself to go to the setpoint that I entered it through the client ..
I know that eventually the kalman filter generates an estimated pose including the current estimated height.then, the controller, outputs motor control in order to get the current estimated height closer to the desired height setpoint. The height PID is generating a velocity setpoint to get closer to the height setpoint there ..
but what does the the function(code) that's leading my crazyflie to get closer to my setpoint? for instance my crazyflie would increase its speed or increase its hieght in order to get to the setpoint that I entered to it .. which rows from firmware code is incharged of? I searched in position_controller_pid but didn't understand what's going there .. may anyone explain to me what code does and what function it uses in order to get closer to the desired setpoint that I entered it?!
in brief I want to know the code and its functions that're a "mediator" between estimated position and motor orders - exactly how functionally(code) we go from kalman estimation position(estimated setpoint) to get closer to the setpoint that we entered to my crazyflie ...
thanks alot!!

I'm attaching the code that I deeply believe it contains all the information about the manipulation of setpoints over crazyflie ! but I'm not understanding what its functions DO exactly !! , may please get a concept/explanation about those functions that are related to the setpoints's manipulation and how we are getting closer to the setpoint's point that we entered to in the client? what its targets for? thanks alot !

Code: Select all

void positionControllerInit()
{
  pidInit(&this.pidX.pid, this.pidX.setpoint, this.pidX.init.kp, this.pidX.init.ki, this.pidX.init.kd,
      this.pidX.pid.dt, POSITION_RATE, POSITION_LPF_CUTOFF_FREQ, POSITION_LPF_ENABLE);
  pidInit(&this.pidY.pid, this.pidY.setpoint, this.pidY.init.kp, this.pidY.init.ki, this.pidY.init.kd,
      this.pidY.pid.dt, POSITION_RATE, POSITION_LPF_CUTOFF_FREQ, POSITION_LPF_ENABLE);
  pidInit(&this.pidZ.pid, this.pidZ.setpoint, this.pidZ.init.kp, this.pidZ.init.ki, this.pidZ.init.kd,
      this.pidZ.pid.dt, POSITION_RATE, POSITION_LPF_CUTOFF_FREQ, POSITION_LPF_ENABLE);

  pidInit(&this.pidVX.pid, this.pidVX.setpoint, this.pidVX.init.kp, this.pidVX.init.ki, this.pidVX.init.kd,
      this.pidVX.pid.dt, POSITION_RATE, POSITION_LPF_CUTOFF_FREQ, POSITION_LPF_ENABLE);
  pidInit(&this.pidVY.pid, this.pidVY.setpoint, this.pidVY.init.kp, this.pidVY.init.ki, this.pidVY.init.kd,
      this.pidVY.pid.dt, POSITION_RATE, POSITION_LPF_CUTOFF_FREQ, POSITION_LPF_ENABLE);
  pidInit(&this.pidVZ.pid, this.pidVZ.setpoint, this.pidVZ.init.kp, this.pidVZ.init.ki, this.pidVZ.init.kd,
      this.pidVZ.pid.dt, POSITION_RATE, POSITION_LPF_CUTOFF_FREQ, POSITION_LPF_ENABLE);
}

static float runPid(float input, struct pidAxis_s *axis, float setpoint, float dt) {
  axis->setpoint = setpoint;

  pidSetDesired(&axis->pid, axis->setpoint);
  return pidUpdate(&axis->pid, input, true);
}

void positionController(float* thrust, attitude_t *attitude, setpoint_t *setpoint,
                                                             const state_t *state)
{
  this.pidX.pid.outputLimit = xyVelMax * velMaxOverhead;
  this.pidY.pid.outputLimit = xyVelMax * velMaxOverhead;
  // The ROS landing detector will prematurely trip if
  // this value is below 0.5
  this.pidZ.pid.outputLimit = fmaxf(zVelMax, 0.5f)  * velMaxOverhead;

  float cosyaw = cosf(state->attitude.yaw * (float)M_PI / 180.0f);
  float sinyaw = sinf(state->attitude.yaw * (float)M_PI / 180.0f);
  float bodyvx = setpoint->velocity.x;
  float bodyvy = setpoint->velocity.y;

  // X, Y
  if (setpoint->mode.x == modeAbs) {
    setpoint->velocity.x = runPid(state->position.x, &this.pidX, setpoint->position.x, DT);
  } else if (setpoint->velocity_body) {
    setpoint->velocity.x = bodyvx * cosyaw - bodyvy * sinyaw;
  }
  if (setpoint->mode.y == modeAbs) {
    setpoint->velocity.y = runPid(state->position.y, &this.pidY, setpoint->position.y, DT);
  } else if (setpoint->velocity_body) {
    setpoint->velocity.y = bodyvy * cosyaw + bodyvx * sinyaw;
  }
  if (setpoint->mode.z == modeAbs) {
    setpoint->velocity.z = runPid(state->position.z, &this.pidZ, setpoint->position.z, DT);
  }

  velocityController(thrust, attitude, setpoint, state);
}

void velocityController(float* thrust, attitude_t *attitude, setpoint_t *setpoint,
                                                             const state_t *state)
{
  this.pidVX.pid.outputLimit = rpLimit * rpLimitOverhead;
  this.pidVY.pid.outputLimit = rpLimit * rpLimitOverhead;
  // Set the output limit to the maximum thrust range
  this.pidVZ.pid.outputLimit = (UINT16_MAX / 2 / thrustScale);
  //this.pidVZ.pid.outputLimit = (this.thrustBase - this.thrustMin) / thrustScale;

  // Roll and Pitch
  float rollRaw  = runPid(state->velocity.x, &this.pidVX, setpoint->velocity.x, DT);
  float pitchRaw = runPid(state->velocity.y, &this.pidVY, setpoint->velocity.y, DT);

  float yawRad = state->attitude.yaw * (float)M_PI / 180;
  attitude->pitch = -(rollRaw  * cosf(yawRad)) - (pitchRaw * sinf(yawRad));
  attitude->roll  = -(pitchRaw * cosf(yawRad)) + (rollRaw  * sinf(yawRad));

  attitude->roll  = constrain(attitude->roll,  -rpLimit, rpLimit);
  attitude->pitch = constrain(attitude->pitch, -rpLimit, rpLimit);

  // Thrust
  float thrustRaw = runPid(state->velocity.z, &this.pidVZ, setpoint->velocity.z, DT);
  // Scale the thrust and add feed forward term
  *thrust = thrustRaw*thrustScale + this.thrustBase;
  // Check for minimum thrust
  if (*thrust < this.thrustMin) {
    *thrust = this.thrustMin;
  }
}

void positionControllerResetAllPID()
{
  pidReset(&this.pidX.pid);
  pidReset(&this.pidY.pid);
  pidReset(&this.pidZ.pid);
  pidReset(&this.pidVX.pid);
  pidReset(&this.pidVY.pid);
  pidReset(&this.pidVZ.pid);
}
RyanMco
Expert
Posts: 159
Joined: Tue Apr 09, 2019 6:15 am

Re: Setpoints of client

Post by RyanMco »

Any help please? if the question isn't understandable as I can make it more detailed and explain it more simple .. I really stuck in understanding that code
arnaud
Bitcraze
Posts: 2538
Joined: Tue Feb 06, 2007 12:36 pm

Re: Setpoints of client

Post by arnaud »

This file is indeed implenting the controller. It is a cascaded PID controller.

The line:

Code: Select all

setpoint->velocity.z = runPid(state->position.z, &this.pidZ, setpoint->position.z, DT);
Runs one step of a PID loop with the estimated Z position and the wanted Z position setpoing as an input and output a Z velocity setpoint.

Similarly there is a loop that will take this velocity setpoint, the current velocity estimate and outputs a thrust setpoint. The thrust is what will control the acceleration of the platform and what is fed to the motors.
RyanMco
Expert
Posts: 159
Joined: Tue Apr 09, 2019 6:15 am

Re: Setpoints of client

Post by RyanMco »

first, thanks alot arnoud on that information!
but what are the structs state, setpoint, position, DT are? I mean what does the struct "state" imply? what information it has? same about position, what does it imply in aspect of crazyflie?!
for instance this function takes parameters, state/position/DT/THIS.PIDZ ..what are they about?
runPid(state->position.z, &this.pidZ, setpoint->position.z, DT);


About the controller, you said they are cascade .. so we have more than one .. where is the code for every subcontroller? I see just one complete controller and not cascade ..

thanks alot
arnaud
Bitcraze
Posts: 2538
Joined: Tue Feb 06, 2007 12:36 pm

Re: Setpoints of client

Post by arnaud »

I am not sure to understand your question. If you are interested about the content of the variables you can look in the code for there type definition. Most of them are defined in the same header file. The structures used in the controller are initialized by the state estimator and the controller.

Each call to runPid() is running one control loop. For example, for Z, one loop takes position estimate and setpoint to generate a velocity setpoint. Then a look takes the velocity estimate and setpoint to generate the thrust.
Post Reply