Skip to content

Explanation of Position Control algorithm #10

Description

@Avi241

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

Metadata

Metadata

Assignees

No one assigned

    Labels

    No labels
    No labels

    Projects

    No projects

    Milestone

    No milestone

    Relationships

    None yet

    Development

    No branches or pull requests

    Issue actions