CopterExpress/clover
1
0
Fork 0
mirror of https://github.com/CopterExpress/clover.git synced 2026-05-26 11:43:25 +00:00
Code Issues Packages Projects Releases Wiki Activity

Change thrust_msg to setpoint_thrust

Browse Source
This commit is contained in:
Oleg Kalachev
2022-12-08 11:40:02 +03:00
parent 68677fccdc
commit 2a74940140
1 changed files with 7 additions and 5 deletions
Download Patch File Download Diff File Expand all files Collapse all files

12
clover/src/simple_offboard.cpp
View File

@@ -94,7 +94,6 @@ tf::Quaternion tq;
PoseStamped position_msg;
PositionTarget position_raw_msg;
AttitudeTarget att_raw_msg;
Thrust thrust_msg;
//TwistStamped rates_msg;
TransformStamped target, setpoint;
geometry_msgs::TransformStamped body;
@@ -106,6 +105,7 @@ PointStamped setpoint_altitude;
Vector3Stamped setpoint_velocity, setpoint_velocity_transformed;
QuaternionStamped setpoint_attitude, setpoint_attitude_transformed; // attitude or only the yaw
Vector3 setpoint_rates;
float setpoint_thrust;
float nav_speed;
float setpoint_lat, setpoint_lon;
bool busy = false;
@@ -394,7 +394,6 @@ void publish(const ros::Time stamp)
if (setpoint_type == NONE) return;
position_raw_msg.header.stamp = stamp;
thrust_msg.header.stamp = stamp;
//rates_msg.header.stamp = stamp;
if (setpoint_type == NAVIGATE || setpoint_type == NAVIGATE_GLOBAL || setpoint_type == POSITION) {
@@ -504,6 +503,9 @@ void publish(const ros::Time stamp)
if (setpoint_type == ATTITUDE) {
attitude_pub.publish(setpoint_position_transformed);
Thrust thrust_msg;
thrust_msg.header.stamp = stamp;
thrust_msg.thrust = setpoint_thrust;
thrust_pub.publish(thrust_msg);
}
@@ -516,7 +518,7 @@ void publish(const ros::Time stamp)
att_raw_msg.header.frame_id = fcu_frame;
att_raw_msg.type_mask = AttitudeTarget::IGNORE_ATTITUDE;
att_raw_msg.body_rate = setpoint_rates;
att_raw_msg.thrust = thrust_msg.thrust;
att_raw_msg.thrust = setpoint_thrust;
attitude_raw_pub.publish(att_raw_msg);
}
}
@@ -580,7 +582,7 @@ void publishState()
msg.roll_rate = setpoint_rates.x;
msg.pitch_rate = setpoint_rates.y;
msg.yaw_rate = setpoint_rates.z;
msg.thrust = thrust_msg.thrust;
msg.thrust = setpoint_thrust;
if (setpoint_type == VELOCITY) {
msg.xy_frame_id = setpoint_velocity.header.frame_id;
@@ -816,7 +818,7 @@ bool serve(enum setpoint_type_t sp_type, float x, float y, float z, float vx, fl
if (sp_type == ATTITUDE || sp_type == RATES) {
// keep current value if NaN
thrust_msg.thrust = isnan(thrust) ? thrust_msg.thrust : thrust;
setpoint_thrust = isnan(thrust) ? setpoint_thrust : thrust;
}
if (sp_type == RATES) {