Setpoints of client
Posted: Mon Aug 26, 2019 4:31 pm
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 !
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);
}