21 #ifndef ROTORS_CONTROL_ROLL_PITCH_YAWRATE_THRUST_CONTROLLER_H 22 #define ROTORS_CONTROL_ROLL_PITCH_YAWRATE_THRUST_CONTROLLER_H 38 EIGEN_MAKE_ALIGNED_OPERATOR_NEW
55 void InitializeParameters();
56 void CalculateRotorVelocities(Eigen::VectorXd* rotor_velocities)
const;
59 void SetRollPitchYawrateThrust(
65 EIGEN_MAKE_ALIGNED_OPERATOR_NEW
77 void ComputeDesiredAngularAcc(Eigen::Vector3d* angular_acceleration)
const;
81 #endif // ROTORS_CONTROL_ROLL_PITCH_YAWRATE_THRUST_CONTROLLER_H
Eigen::Vector3d normalized_attitude_gain_
static const Eigen::Vector3d kDefaultAngularRateGain
Eigen::Vector3d normalized_angular_rate_gain_
mav_msgs::EigenRollPitchYawrateThrust roll_pitch_yawrate_thrust_
static const Eigen::Vector3d kDefaultAttitudeGain
EIGEN_MAKE_ALIGNED_OPERATOR_NEW RollPitchYawrateThrustControllerParameters()
RotorConfiguration rotor_configuration_
Eigen::Matrix4Xd allocation_matrix_
Eigen::Vector3d angular_rate_gain_
RollPitchYawrateThrustControllerParameters controller_parameters_
VehicleParameters vehicle_parameters_
void calculateAllocationMatrix(const RotorConfiguration &rotor_configuration, Eigen::Matrix4Xd *allocation_matrix)
Eigen::MatrixX4d angular_acc_to_rotor_velocities_
Eigen::Vector3d attitude_gain_