fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-10-23 00:53:34 +08:00
Code Issues Packages Projects Releases Wiki Activity

update UUV and rover controllers to use trajectory_setpoint and cleanup unused position_setpoint fields

Browse Source
This commit is contained in:
Daniel Agar
2021-03-09 10:36:34 -05:00
committed by GitHub
parent a0b9b44ff6
commit d37510a43d
7 changed files with 29 additions and 46 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/modules/uuv_pos_control/uuv_pos_control.cpp
+7 -15
View File
@@ -178,21 +178,16 @@ void UUVPOSControl::Run()
&& _vcontrol_mode.flag_control_attitude_enabled
&& _vcontrol_mode.flag_control_rates_enabled) {
_vehicle_attitude_sub.update(&_vehicle_attitude);//get current vehicle attitude
_pos_sp_triplet_sub.update(&_pos_setpoint);
_trajectory_setpoint_sub.update(&_trajectory_setpoint);
float roll_des, pitch_des, yaw_des, x_pos_des, y_pos_des, z_pos_des;
roll_des = 0;
pitch_des = 0;
yaw_des = _pos_setpoint.current.yaw;
x_pos_des = _pos_setpoint.current.x;
y_pos_des = _pos_setpoint.current.y;
z_pos_des = _pos_setpoint.current.z;
float roll_des = 0;
float pitch_des = 0;
float yaw_des = _trajectory_setpoint.yaw;
float x_pos_des = _trajectory_setpoint.x;
float y_pos_des = _trajectory_setpoint.y;
float z_pos_des = _trajectory_setpoint.z;
//stabilization controller(keep pos and hold depth + angle) vs position controller(global + yaw)
if (_param_stabilization.get() == 0) {
@@ -203,9 +198,6 @@ void UUVPOSControl::Run()
stabilization_controller_6dof(x_pos_des, y_pos_des, z_pos_des,
roll_des, pitch_des, yaw_des, _vehicle_attitude, vlocal_pos);
}
}
}