fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-10-19 09:13:33 +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);
}
}
}
src/modules/uuv_pos_control/uuv_pos_control.hpp
+3 -4
View File
@@ -62,9 +62,9 @@
#include <uORB/Publication.hpp>
#include <uORB/topics/manual_control_setpoint.h>
#include <uORB/topics/parameter_update.h>
#include <uORB/topics/position_setpoint_triplet.h>
#include <uORB/topics/vehicle_attitude.h>
#include <uORB/topics/vehicle_local_position.h>
#include <uORB/topics/vehicle_local_position_setpoint.h>
#include <uORB/topics/vehicle_attitude_setpoint.h>
#include <uORB/topics/vehicle_angular_velocity.h>
#include <uORB/topics/vehicle_rates_setpoint.h>
@@ -105,16 +105,15 @@ private:
uORB::Subscription _vehicle_attitude_sub{ORB_ID(vehicle_attitude)}; /**< current vehicle attitude */
uORB::Subscription _manual_control_setpoint_sub{ORB_ID(manual_control_setpoint)}; /**< notification of manual control updates */
uORB::Subscription _trajectory_setpoint_sub{ORB_ID(trajectory_setpoint)};
uORB::Subscription _vcontrol_mode_sub{ORB_ID(vehicle_control_mode)}; /**< vehicle status subscription */
uORB::Subscription _pos_sp_triplet_sub{ORB_ID(position_setpoint_triplet)}; /**< position setpoint */
uORB::SubscriptionCallbackWorkItem _vehicle_local_position_sub{this, ORB_ID(vehicle_local_position)};
//actuator_controls_s _actuators {}; /**< actuator control inputs */
manual_control_setpoint_s _manual_control_setpoint {}; /**< r/c channel data */
vehicle_attitude_s _vehicle_attitude {}; /**< vehicle attitude */
position_setpoint_triplet_s _pos_setpoint {}; /**< vehicle position setpoint */
vehicle_local_position_setpoint_s _trajectory_setpoint{}; /**< vehicle position setpoint */
vehicle_control_mode_s _vcontrol_mode {}; /**< vehicle control mode */
perf_counter_t _loop_perf; /**< loop performance counter */