Skip to content
New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Explanation of Position Control algorithm #10

Open
Avi241 opened this issue May 18, 2022 · 0 comments
Open

Explanation of Position Control algorithm #10

Avi241 opened this issue May 18, 2022 · 0 comments

Comments

@Avi241
Copy link

Avi241 commented May 18, 2022

Hello,
I want to understand this section of code in the file plugin_drone.cpp if anyone knows the details please help me with it.

//position control
        if(navi_state == FLYING_MODEL){
            double vx = controllers_.pos_x.update(cmd_val.linear.x, position.X(), poschange.X(), dt); //cmd_val.linear.x this is position value
            double vy = controllers_.pos_y.update(cmd_val.linear.y, position.Y(), poschange.Y(), dt);
            double vz = controllers_.pos_z.update(cmd_val.linear.z, position.Z(), poschange.Z(), dt);

            

            ignition::math::Vector3d vb = heading_quaternion.RotateVectorReverse(ignition::math::Vector3d(vx,vy,vz));
            std::cout << vx << vb.X() << velocity_xy.X() << std::endl;
            
            double pitch_command =  controllers_.velocity_x.update(vb.X(), velocity_xy.X(), acceleration_xy.X(), dt) / gravity;
            double roll_command  = -controllers_.velocity_y.update(vb.Y(), velocity_xy.Y(), acceleration_xy.Y(), dt) / gravity;
            torque.X() = inertia.X() *  controllers_.roll.update(roll_command, euler.X(), angular_velocity_body.X(), dt);
            torque.Y() = inertia.Y() *  controllers_.pitch.update(pitch_command, euler.Y(), angular_velocity_body.Y(), dt);            
            force.Z()  = mass      * (controllers_.velocity_z.update(vz,  velocity.Z(), acceleration.Z(), dt) + load_factor * gravity);
        }

or if there is any theory text explaining this please share it.
Thank you

Sign up for free to join this conversation on GitHub. Already have an account? Sign in to comment
Labels
None yet
Projects
None yet
Development

No branches or pull requests

1 participant