Skip to content

Commit 889bacb

Browse files
potato77potato77
committed
add px4_command_utils.h
1 parent 64bb5b9 commit 889bacb

21 files changed

+467
-570
lines changed

‎config/Parameter_for_control.yaml

+4-24
Original file line numberDiff line numberDiff line change
@@ -4,38 +4,16 @@ Quad:
44
throttle_a: 20.0
55
throttle_b: 0.0
66

7-
##geo fence
7+
##geo fence for fsc
88
geo_fence:
9-
## x_min: -50.0
10-
## x_max: 50.0
11-
## y_min: -50.0
12-
## y_max: 50.0
13-
## z_min: -0.5
14-
## z_max: 50.0
159
x_min: -2.0
1610
x_max: 2.0
1711
y_min: -1.8
1812
y_max: 1.8
1913
z_min: -0.5
2014
z_max: 1.5
2115

22-
## 限制参数 for 点追踪
23-
#Limit:
24-
# pxy_error_max: 0.8
25-
# vxy_error_max: 1.0
26-
# pz_error_max: 0.5
27-
# vz_error_max: 1.0
28-
# pxy_int_max : 5.0
29-
# pz_int_max : 5.0
30-
# tilt_max: 20.0
31-
# THR_MIN: 0.1
32-
# THR_MAX: 0.9
33-
# int_start_error: 100.0
34-
35-
# XY_VEL_MAX : 0.8
36-
# Z_VEL_MAX : 0.5
37-
38-
## 限制参数 for 轨迹追踪
16+
## 限制参数
3917
Limit:
4018
pxy_error_max: 1.0
4119
vxy_error_max: 1.0
@@ -59,6 +37,8 @@ Disarm_height : 0.15
5937
Use_mocap_raw : 0
6038
## 1 for use the accel command
6139
Use_accel : 1.0
40+
## 1 for printf the state, 0 for block
41+
Flag_printf : 1.0
6242

6343
# 位置环参数 - cascade pid
6444
Pos_cascade_pid:

‎include/pos_controller_NE.h

+7-32
Original file line numberDiff line numberDiff line change
@@ -13,7 +13,7 @@
1313
#include <Eigen/Eigen>
1414
#include <math.h>
1515
#include <command_to_mavros.h>
16-
#include <pos_controller_utils.h>
16+
#include <px4_command_utils.h>
1717
#include <math_utils.h>
1818

1919
#include <LowPassFilter.h>
@@ -173,8 +173,8 @@ Eigen::Vector3d pos_controller_NE::pos_controller(
173173
Eigen::Vector3d accel_sp;
174174
Eigen::Vector3d thrust_sp;
175175
// 计算误差项
176-
Eigen::Vector3f pos_error = pos_controller_utils::cal_pos_error(_DroneState, _Reference_State);
177-
Eigen::Vector3f vel_error = pos_controller_utils::cal_vel_error(_DroneState, _Reference_State);
176+
Eigen::Vector3f pos_error = px4_command_utils::cal_pos_error(_DroneState, _Reference_State);
177+
Eigen::Vector3f vel_error = px4_command_utils::cal_vel_error(_DroneState, _Reference_State);
178178

179179
// 误差项限幅
180180
for (int i=0; i<3; i++)
@@ -257,31 +257,6 @@ Eigen::Vector3d pos_controller_NE::pos_controller(
257257
}
258258

259259
return thrust_sp;
260-
261-
// cout <<">>>>>>>>>>>>>>>>>>>>>>PID Position Controller<<<<<<<<<<<<<<<<<<<<<" <<endl;
262-
263-
// //固定的浮点显示
264-
// cout.setf(ios::fixed);
265-
// //左对齐
266-
// cout.setf(ios::left);
267-
// // 强制显示小数点
268-
// cout.setf(ios::showpoint);
269-
// // 强制显示符号
270-
// cout.setf(ios::showpos);
271-
272-
// cout<<setprecision(2);
273-
274-
// cout << "e_p [X Y Z] : " << pos_error[0] << " [m] "<< pos_error[1]<<" [m] "<<pos_error[2]<<" [m] "<<endl;
275-
// cout << "e_v [X Y Z] : " << vel_error[0] << " [m/s] "<< vel_error[1]<<" [m/s] "<<vel_error[2]<<" [m/s] "<<endl;
276-
// cout << "acc_ref [X Y Z] : " << _Reference_State.acceleration_ref[0] << " [m/s^2] "<< _Reference_State.acceleration_ref[1]<<" [m/s^2] "<<_Reference_State.acceleration_ref[2]<<" [m/s^2] "<<endl;
277-
278-
// cout << "desired_acceleration [X Y Z] : " << _AttitudeReference.desired_acceleration[0] << " [m/s^2] "<< _AttitudeReference.desired_acceleration[1]<<" [Nm/s^2] "<<_AttitudeReference.desired_acceleration[2]<<" [m/s^2] "<<endl;
279-
// cout << "desired_thrust [X Y Z] : " << _AttitudeReference.desired_thrust[0] << " [N] "<< _AttitudeReference.desired_thrust[1]<<" [N] "<<_AttitudeReference.desired_thrust[2]<<" [N] "<<endl;
280-
// cout << "desired_thrust_normalized [X Y Z] : " << _AttitudeReference.desired_thrust_normalized[0] << " [N] "<< _AttitudeReference.desired_thrust_normalized[1]<<" [N] "<<_AttitudeReference.desired_thrust_normalized[2]<<" [N] "<<endl;
281-
// cout << "desired_attitude [R P Y] : " << _AttitudeReference.desired_attitude[0] * 180/M_PI <<" [deg] "<<_AttitudeReference.desired_attitude[1] * 180/M_PI << " [deg] "<< _AttitudeReference.desired_attitude[2] * 180/M_PI<<" [deg] "<<endl;
282-
// cout << "desired_throttle [0-1] : " << _AttitudeReference.desired_throttle <<endl;
283-
284-
// return _AttitudeReference;
285260
}
286261

287262
void pos_controller_NE::printf_result()
@@ -299,13 +274,13 @@ void pos_controller_NE::printf_result()
299274

300275
cout<<setprecision(2);
301276

302-
// cout << "NoiseEstimator [X Y Z] : " << Quad_MASS *Kd[0] * NoiseEstimator[0] << " [N] "<<Quad_MASS *Kd[1] * NoiseEstimator[1]<<" [N] "<<Quad_MASS *Kd[2] *NoiseEstimator[2]<<" [N] "<<endl;
277+
cout << "NoiseEstimator [X Y Z] : " << Quad_MASS *Kd[0] * NoiseEstimator[0] << " [N] "<<Quad_MASS *Kd[1] * NoiseEstimator[1]<<" [N] "<<Quad_MASS *Kd[2] *NoiseEstimator[2]<<" [N] "<<endl;
303278

304-
// cout << "u_l [X Y Z] : " << u_l[0] << " [N] "<< u_l[1]<<" [N] "<<u_l[2]<<" [N] "<<endl;
279+
cout << "u_l [X Y Z] : " << u_l[0] << " [N] "<< u_l[1]<<" [N] "<<u_l[2]<<" [N] "<<endl;
305280

306-
// cout << "output_LLF [X Y Z] : " << output_LLF[0] << " [N] "<< output_LLF[1]<<" [N] "<<output_LLF[2]<<" [N] "<<endl;
281+
cout << "output_LLF [X Y Z] : " << output_LLF[0] << " [N] "<< output_LLF[1]<<" [N] "<<output_LLF[2]<<" [N] "<<endl;
307282

308-
// cout << "u_d [X Y Z] : " << u_d[0] << " [N] "<< u_d[1]<<" [N] "<<u_d[2]<<" [N] "<<endl;
283+
cout << "u_d [X Y Z] : " << u_d[0] << " [N] "<< u_d[1]<<" [N] "<<u_d[2]<<" [N] "<<endl;
309284
cout << "thrust_sp [X Y Z] : " << thrust_sp[0] << " [m/s^2] "<< thrust_sp[1]<<" [m/s^2] "<<thrust_sp[2]<<" [m/s^2] "<<endl;
310285
}
311286

‎include/pos_controller_PID.h

+4-35
Original file line numberDiff line numberDiff line change
@@ -13,7 +13,7 @@
1313

1414
#include <math.h>
1515
#include <command_to_mavros.h>
16-
#include <pos_controller_utils.h>
16+
#include <px4_command_utils.h>
1717
#include <px4_command/data_log.h>
1818
#include <px4_command/DroneState.h>
1919
#include <px4_command/TrajectoryPoint.h>
@@ -110,8 +110,8 @@ Eigen::Vector3d pos_controller_PID::pos_controller(
110110
Eigen::Vector3d thrust_sp;
111111

112112
// 计算误差项
113-
Eigen::Vector3f pos_error = pos_controller_utils::cal_pos_error(_DroneState, _Reference_State);
114-
Eigen::Vector3f vel_error = pos_controller_utils::cal_vel_error(_DroneState, _Reference_State);
113+
Eigen::Vector3f pos_error = px4_command_utils::cal_pos_error(_DroneState, _Reference_State);
114+
Eigen::Vector3f vel_error = px4_command_utils::cal_vel_error(_DroneState, _Reference_State);
115115

116116
// 误差项限幅
117117
for (int i=0; i<3; i++)
@@ -184,41 +184,10 @@ Eigen::Vector3d pos_controller_PID::pos_controller(
184184
}
185185

186186
return thrust_sp;
187-
188-
189-
// cout <<">>>>>>>>>>>>>>>>>>>>>>PID Position Controller<<<<<<<<<<<<<<<<<<<<<" <<endl;
190-
191-
// //固定的浮点显示
192-
// cout.setf(ios::fixed);
193-
// //左对齐
194-
// cout.setf(ios::left);
195-
// // 强制显示小数点
196-
// cout.setf(ios::showpoint);
197-
// // 强制显示符号
198-
// cout.setf(ios::showpos);
199-
200-
// cout<<setprecision(2);
201-
202-
// cout << "Pos_ref [XYZ]: " << _Reference_State.position_ref[0] << " [ m ]" << _Reference_State.position_ref[1] << " [ m ]"<< _Reference_State.position_ref[2] << " [ m ]" << endl;
203-
// cout << "Vel_ref [XYZ]: " << _Reference_State.velocity_ref[0] << " [m/s]" << _Reference_State.velocity_ref[1] << " [m/s]" << _Reference_State.velocity_ref[2] << " [m/s]" <<endl;
204-
// cout << "Acc_ref [XYZ]: " << _Reference_State.acceleration_ref[0] << " [m/s^2]" << _Reference_State.acceleration_ref[1] << " [m/s^2]" << _Reference_State.acceleration_ref[2] << " [m/s^2]" <<endl;
205-
// cout << "Yaw_setpoint : " << _Reference_State.yaw_ref * 180/M_PI << " [deg] " <<endl;
206-
207-
// cout << "e_p [X Y Z] : " << pos_error[0] << " [m] "<< pos_error[1]<<" [m] "<<pos_error[2]<<" [m] "<<endl;
208-
// cout << "e_v [X Y Z] : " << vel_error[0] << " [m/s] "<< vel_error[1]<<" [m/s] "<<vel_error[2]<<" [m/s] "<<endl;
209-
// cout << "acc_ref [X Y Z] : " << _Reference_State.acceleration_ref[0] << " [m/s^2] "<< _Reference_State.acceleration_ref[1]<<" [m/s^2] "<<_Reference_State.acceleration_ref[2]<<" [m/s^2] "<<endl;
210-
211-
// cout << "desired_acceleration [X Y Z] : " << _AttitudeReference.desired_acceleration[0] << " [m/s^2] "<< _AttitudeReference.desired_acceleration[1]<<" [Nm/s^2] "<<_AttitudeReference.desired_acceleration[2]<<" [m/s^2] "<<endl;
212-
// cout << "desired_thrust [X Y Z] : " << _AttitudeReference.desired_thrust[0] << " [N] "<< _AttitudeReference.desired_thrust[1]<<" [N] "<<_AttitudeReference.desired_thrust[2]<<" [N] "<<endl;
213-
// cout << "desired_thrust_normalized [X Y Z] : " << _AttitudeReference.desired_thrust_normalized[0] << " [N] "<< _AttitudeReference.desired_thrust_normalized[1]<<" [N] "<<_AttitudeReference.desired_thrust_normalized[2]<<" [N] "<<endl;
214-
// cout << "desired_attitude [R P Y] : " << _AttitudeReference.desired_attitude[0] * 180/M_PI <<" [deg] "<<_AttitudeReference.desired_attitude[1] * 180/M_PI << " [deg] "<< _AttitudeReference.desired_attitude[2] * 180/M_PI<<" [deg] "<<endl;
215-
// cout << "desired_throttle [0-1] : " << _AttitudeReference.desired_throttle <<endl;
216187
}
217188

218189
void pos_controller_PID::printf_result()
219190
{
220-
221-
222191
cout <<">>>>>>>>>>>>>>>>>>>>PID Position Controller<<<<<<<<<<<<<<<<<<<<<" <<endl;
223192

224193
//固定的浮点显示
@@ -232,7 +201,7 @@ void pos_controller_PID::printf_result()
232201

233202
cout<<setprecision(2);
234203

235-
cout << "thrust_sp [X Y Z] : " << thrust_sp[0] << " [m/s^2] "<< thrust_sp[1]<<" [m/s^2] "<<thrust_sp[2]<<" [m/s^2] "<<endl;
204+
cout << "thrust_sp [X Y Z] : " << thrust_sp[0] << " [m/s^2] "<< thrust_sp[1]<<" [m/s^2] "<<thrust_sp[2]<<" [m/s^2] "<<endl;
236205

237206
}
238207

‎include/pos_controller_Passivity.h

+10-35
Original file line numberDiff line numberDiff line change
@@ -13,7 +13,7 @@
1313
#include <Eigen/Eigen>
1414
#include <math.h>
1515
#include <command_to_mavros.h>
16-
#include <pos_controller_utils.h>
16+
#include <px4_command_utils.h>
1717
#include <math_utils.h>
1818
#include <LowPassFilter.h>
1919
#include <HighPassFilter.h>
@@ -166,8 +166,8 @@ Eigen::Vector3d pos_controller_passivity::pos_controller(
166166
Eigen::Vector3d accel_sp;
167167
Eigen::Vector3d thrust_sp;
168168
// 计算误差项
169-
Eigen::Vector3f pos_error = pos_controller_utils::cal_pos_error(_DroneState, _Reference_State);
170-
Eigen::Vector3f vel_error = pos_controller_utils::cal_vel_error(_DroneState, _Reference_State);
169+
Eigen::Vector3f pos_error = px4_command_utils::cal_pos_error(_DroneState, _Reference_State);
170+
Eigen::Vector3f vel_error = px4_command_utils::cal_vel_error(_DroneState, _Reference_State);
171171

172172
// 误差项限幅
173173
for (int i=0; i<3; i++)
@@ -250,31 +250,6 @@ Eigen::Vector3d pos_controller_passivity::pos_controller(
250250
}
251251

252252
return thrust_sp;
253-
254-
// cout <<">>>>>>>>>>>>>>>>>>>>>>PID Position Controller<<<<<<<<<<<<<<<<<<<<<" <<endl;
255-
256-
// //固定的浮点显示
257-
// cout.setf(ios::fixed);
258-
// //左对齐
259-
// cout.setf(ios::left);
260-
// // 强制显示小数点
261-
// cout.setf(ios::showpoint);
262-
// // 强制显示符号
263-
// cout.setf(ios::showpos);
264-
265-
// cout<<setprecision(2);
266-
267-
// cout << "e_p [X Y Z] : " << pos_error[0] << " [m] "<< pos_error[1]<<" [m] "<<pos_error[2]<<" [m] "<<endl;
268-
// cout << "e_v [X Y Z] : " << vel_error[0] << " [m/s] "<< vel_error[1]<<" [m/s] "<<vel_error[2]<<" [m/s] "<<endl;
269-
// cout << "acc_ref [X Y Z] : " << _Reference_State.acceleration_ref[0] << " [m/s^2] "<< _Reference_State.acceleration_ref[1]<<" [m/s^2] "<<_Reference_State.acceleration_ref[2]<<" [m/s^2] "<<endl;
270-
271-
// cout << "desired_acceleration [X Y Z] : " << _AttitudeReference.desired_acceleration[0] << " [m/s^2] "<< _AttitudeReference.desired_acceleration[1]<<" [Nm/s^2] "<<_AttitudeReference.desired_acceleration[2]<<" [m/s^2] "<<endl;
272-
// cout << "desired_thrust [X Y Z] : " << _AttitudeReference.desired_thrust[0] << " [N] "<< _AttitudeReference.desired_thrust[1]<<" [N] "<<_AttitudeReference.desired_thrust[2]<<" [N] "<<endl;
273-
// cout << "desired_thrust_normalized [X Y Z] : " << _AttitudeReference.desired_thrust_normalized[0] << " [N] "<< _AttitudeReference.desired_thrust_normalized[1]<<" [N] "<<_AttitudeReference.desired_thrust_normalized[2]<<" [N] "<<endl;
274-
// cout << "desired_attitude [R P Y] : " << _AttitudeReference.desired_attitude[0] * 180/M_PI <<" [deg] "<<_AttitudeReference.desired_attitude[1] * 180/M_PI << " [deg] "<< _AttitudeReference.desired_attitude[2] * 180/M_PI<<" [deg] "<<endl;
275-
// cout << "desired_throttle [0-1] : " << _AttitudeReference.desired_throttle <<endl;
276-
277-
//return _AttitudeReference;
278253
}
279254

280255

@@ -329,18 +304,18 @@ void pos_controller_passivity::printf_result()
329304

330305
cout<<setprecision(2);
331306

332-
// cout << "u_l [X Y Z] : " << u_l[0] << " [N] "<< u_l[1]<<" [N] "<<u_l[2]<<" [N] "<<endl;
333-
// cout << "z_k [X Y Z] : " << z_k[0] << " [N] "<< z_k[1]<<" [N] "<<z_k[2]<<" [N] "<<endl;
307+
cout << "u_l [X Y Z] : " << u_l[0] << " [N] "<< u_l[1]<<" [N] "<<u_l[2]<<" [N] "<<endl;
308+
cout << "z_k [X Y Z] : " << z_k[0] << " [N] "<< z_k[1]<<" [N] "<<z_k[2]<<" [N] "<<endl;
334309

335-
// cout << "y1 [X Y Z] : " << y1_k[0] << " [N] "<< y1_k[1]<<" [N] "<<y1_k[2]<<" [N] "<<endl;
310+
cout << "y1 [X Y Z] : " << y1_k[0] << " [N] "<< y1_k[1]<<" [N] "<<y1_k[2]<<" [N] "<<endl;
336311

337-
// cout << "y2 [X Y Z] : " << y2_k[0] << " [N] "<< y2_k[1]<<" [N] "<<y2_k[2]<<" [N] "<<endl;
312+
cout << "y2 [X Y Z] : " << y2_k[0] << " [N] "<< y2_k[1]<<" [N] "<<y2_k[2]<<" [N] "<<endl;
338313

339-
// cout << "y3 [X Y Z] : " << y3_k[0] << " [N] "<< y3_k[1]<<" [N] "<<y3_k[2]<<" [N] "<<endl;
314+
cout << "y3 [X Y Z] : " << y3_k[0] << " [N] "<< y3_k[1]<<" [N] "<<y3_k[2]<<" [N] "<<endl;
340315

341-
// cout << "u_l [X Y Z] : " << u_l[0] << " [N] "<< u_l[1]<<" [N] "<<u_l[2]<<" [N] "<<endl;
316+
cout << "u_l [X Y Z] : " << u_l[0] << " [N] "<< u_l[1]<<" [N] "<<u_l[2]<<" [N] "<<endl;
342317

343-
// cout << "u_d [X Y Z] : " << u_d[0] << " [N] "<< u_d[1]<<" [N] "<<u_d[2]<<" [N] "<<endl;
318+
cout << "u_d [X Y Z] : " << u_d[0] << " [N] "<< u_d[1]<<" [N] "<<u_d[2]<<" [N] "<<endl;
344319

345320
cout << "thrust_sp [X Y Z] : " << thrust_sp[0] << " [m/s^2] "<< thrust_sp[1]<<" [m/s^2] "<<thrust_sp[2]<<" [m/s^2] "<<endl;
346321

‎include/pos_controller_UDE.h

+7-37
Original file line numberDiff line numberDiff line change
@@ -19,7 +19,7 @@
1919
#include <Eigen/Eigen>
2020
#include <math.h>
2121
#include <command_to_mavros.h>
22-
#include <pos_controller_utils.h>
22+
#include <px4_command_utils.h>
2323
#include <math_utils.h>
2424
#include <px4_command/data_log.h>
2525

@@ -124,8 +124,8 @@ Eigen::Vector3d pos_controller_UDE::pos_controller(
124124
Eigen::Vector3d thrust_sp;
125125

126126
// 计算误差项
127-
Eigen::Vector3f pos_error = pos_controller_utils::cal_pos_error(_DroneState, _Reference_State);
128-
Eigen::Vector3f vel_error = pos_controller_utils::cal_vel_error(_DroneState, _Reference_State);
127+
Eigen::Vector3f pos_error = px4_command_utils::cal_pos_error(_DroneState, _Reference_State);
128+
Eigen::Vector3f vel_error = px4_command_utils::cal_vel_error(_DroneState, _Reference_State);
129129

130130
// 误差项限幅
131131
for (int i=0; i<3; i++)
@@ -191,31 +191,6 @@ Eigen::Vector3d pos_controller_UDE::pos_controller(
191191
}
192192

193193
return thrust_sp;
194-
195-
// cout <<">>>>>>>>>>>>>>>>>>>>>>UDE Position Controller<<<<<<<<<<<<<<<<<<<<<" <<endl;
196-
197-
// //固定的浮点显示
198-
// cout.setf(ios::fixed);
199-
// //左对齐
200-
// cout.setf(ios::left);
201-
// // 强制显示小数点
202-
// cout.setf(ios::showpoint);
203-
// // 强制显示符号
204-
// cout.setf(ios::showpos);
205-
206-
// cout<<setprecision(2);
207-
208-
// cout << "e_p [X Y Z] : " << pos_error[0] << " [m] "<< pos_error[1]<<" [m] "<<pos_error[2]<<" [m] "<<endl;
209-
// cout << "e_v [X Y Z] : " << vel_error[0] << " [m/s] "<< vel_error[1]<<" [m/s] "<<vel_error[2]<<" [m/s] "<<endl;
210-
// cout << "acc_ref [X Y Z] : " << _Reference_State.acceleration_ref[0] << " [m/s^2] "<< _Reference_State.acceleration_ref[1]<<" [m/s^2] "<<_Reference_State.acceleration_ref[2]<<" [m/s^2] "<<endl;
211-
212-
// cout << "desired_acceleration [X Y Z] : " << _AttitudeReference.desired_acceleration[0] << " [m/s^2] "<< _AttitudeReference.desired_acceleration[1]<<" [Nm/s^2] "<<_AttitudeReference.desired_acceleration[2]<<" [m/s^2] "<<endl;
213-
// cout << "desired_thrust [X Y Z] : " << _AttitudeReference.desired_thrust[0] << " [N] "<< _AttitudeReference.desired_thrust[1]<<" [N] "<<_AttitudeReference.desired_thrust[2]<<" [N] "<<endl;
214-
// cout << "desired_thrust_normalized [X Y Z] : " << _AttitudeReference.desired_thrust_normalized[0] << " [N] "<< _AttitudeReference.desired_thrust_normalized[1]<<" [N] "<<_AttitudeReference.desired_thrust_normalized[2]<<" [N] "<<endl;
215-
// cout << "desired_attitude [R P Y] : " << _AttitudeReference.desired_attitude[0] * 180/M_PI <<" [deg] "<<_AttitudeReference.desired_attitude[1] * 180/M_PI << " [deg] "<< _AttitudeReference.desired_attitude[2] * 180/M_PI<<" [deg] "<<endl;
216-
// cout << "desired_throttle [0-1] : " << _AttitudeReference.desired_throttle <<endl;
217-
218-
//return _AttitudeReference;
219194
}
220195

221196
void pos_controller_UDE::printf_result()
@@ -232,17 +207,12 @@ void pos_controller_UDE::printf_result()
232207
cout.setf(ios::showpos);
233208

234209
cout<<setprecision(2);
235-
236-
// cout << "dt : " << dt<< " [s] " <<endl;
237-
238-
// cout << "e_p [X Y Z] : " << pos_error[0] << " [N] "<< pos_error[1]<<" [N] "<<pos_error[2]<<" [N] "<<endl;
239-
// cout << "e_v [X Y Z] : " << vel_error[0] << " [N] "<< vel_error[1]<<" [N] "<<vel_error[2]<<" [N] "<<endl;
240210

241-
//cout << "u_l [X Y Z] : " << u_l[0] << " [N] "<< u_l[1]<<" [N] "<<u_l[2]<<" [N] "<<endl;
242-
//cout << "int [X Y Z] : " << integral[0] << " [N] "<< integral[1]<<" [N] "<<integral[2]<<" [N] "<<endl;
243-
//cout << "u_d [X Y Z] : " << u_d[0] << " [N] "<< u_d[1]<<" [N] "<<u_d[2]<<" [N] "<<endl;
211+
cout << "u_l [X Y Z] : " << u_l[0] << " [N] "<< u_l[1]<<" [N] "<<u_l[2]<<" [N] "<<endl;
212+
cout << "int [X Y Z] : " << integral[0] << " [N] "<< integral[1]<<" [N] "<<integral[2]<<" [N] "<<endl;
213+
cout << "u_d [X Y Z] : " << u_d[0] << " [N] "<< u_d[1]<<" [N] "<<u_d[2]<<" [N] "<<endl;
244214

245-
cout << "thrust_sp [X Y Z] : " << thrust_sp[0] << " [m/s^2] "<< thrust_sp[1]<<" [m/s^2] "<<thrust_sp[2]<<" [m/s^2] "<<endl;
215+
cout << "thrust_sp [X Y Z] : " << thrust_sp[0] << " [m/s^2] "<< thrust_sp[1]<<" [m/s^2] "<<thrust_sp[2]<<" [m/s^2] "<<endl;
246216

247217
}
248218

‎include/pos_controller_cascade_PID.h

+1-1
Original file line numberDiff line numberDiff line change
@@ -23,7 +23,7 @@
2323
#include <math.h>
2424
#include <math_utils.h>
2525
#include <px4_command/data_log.h>
26-
#include <pos_controller_utils.h>
26+
#include <px4_command_utils.h>
2727
#include <px4_command/data_log.h>
2828
#include <px4_command/DroneState.h>
2929
#include <px4_command/TrajectoryPoint.h>

0 commit comments

Comments
 (0)