//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
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.
or if there is any theory text explaining this please share it.
Thank you