forked from Archive/PX4-Autopilot
uuv_att_control: Variable definition in processing (#22697)
This commit is contained in:
parent
077baeae52
commit
e8b3778f81
|
@ -138,13 +138,6 @@ void UUVAttitudeControl::control_attitude_geo(const vehicle_attitude_s &attitude
|
||||||
*/
|
*/
|
||||||
Eulerf euler_angles(matrix::Quatf(attitude.q));
|
Eulerf euler_angles(matrix::Quatf(attitude.q));
|
||||||
|
|
||||||
float roll_u;
|
|
||||||
float pitch_u;
|
|
||||||
float yaw_u;
|
|
||||||
float thrust_x;
|
|
||||||
float thrust_y;
|
|
||||||
float thrust_z;
|
|
||||||
|
|
||||||
float roll_body = attitude_setpoint.roll_body;
|
float roll_body = attitude_setpoint.roll_body;
|
||||||
float pitch_body = attitude_setpoint.pitch_body;
|
float pitch_body = attitude_setpoint.pitch_body;
|
||||||
float yaw_body = attitude_setpoint.yaw_body;
|
float yaw_body = attitude_setpoint.yaw_body;
|
||||||
|
@ -186,14 +179,14 @@ void UUVAttitudeControl::control_attitude_geo(const vehicle_attitude_s &attitude
|
||||||
torques(1) = torques(1) - omega(1) * _param_pitch_d.get(); /**< Pitch */
|
torques(1) = torques(1) - omega(1) * _param_pitch_d.get(); /**< Pitch */
|
||||||
torques(2) = torques(2) - omega(2) * _param_yaw_d.get(); /**< Yaw */
|
torques(2) = torques(2) - omega(2) * _param_yaw_d.get(); /**< Yaw */
|
||||||
|
|
||||||
roll_u = torques(0);
|
float roll_u = torques(0);
|
||||||
pitch_u = torques(1);
|
float pitch_u = torques(1);
|
||||||
yaw_u = torques(2);
|
float yaw_u = torques(2);
|
||||||
|
|
||||||
// take thrust as
|
// take thrust as
|
||||||
thrust_x = attitude_setpoint.thrust_body[0];
|
float thrust_x = attitude_setpoint.thrust_body[0];
|
||||||
thrust_y = attitude_setpoint.thrust_body[1];
|
float thrust_y = attitude_setpoint.thrust_body[1];
|
||||||
thrust_z = attitude_setpoint.thrust_body[2];
|
float thrust_z = attitude_setpoint.thrust_body[2];
|
||||||
|
|
||||||
|
|
||||||
constrain_actuator_commands(roll_u, pitch_u, yaw_u, thrust_x, thrust_y, thrust_z);
|
constrain_actuator_commands(roll_u, pitch_u, yaw_u, thrust_x, thrust_y, thrust_z);
|
||||||
|
|
Loading…
Reference in New Issue