Skip to content

Commit d0efd8a

Browse files
potato77potato77
committed
px4_command_utils.h: add motor model to calculate the throttle
1 parent 889bacb commit d0efd8a

9 files changed

+134
-210
lines changed

‎config/Parameter_for_control.yaml

-4
Original file line numberDiff line numberDiff line change
@@ -1,8 +1,6 @@
11
## 飞机参数
22
Quad:
33
mass: 0.8
4-
throttle_a: 20.0
5-
throttle_b: 0.0
64

75
##geo fence for fsc
86
geo_fence:
@@ -22,8 +20,6 @@ Limit:
2220
pxy_int_max : 5.0
2321
pz_int_max : 5.0
2422
tilt_max: 20.0
25-
THR_MIN: 0.1
26-
THR_MAX: 0.9
2723
int_start_error: 1.0
2824

2925
XY_VEL_MAX : 0.5

‎include/pos_controller_NE.h

+6-33
Original file line numberDiff line numberDiff line change
@@ -38,8 +38,6 @@ class pos_controller_NE
3838
pos_NE_nh("~")
3939
{
4040
pos_NE_nh.param<float>("Quad/mass", Quad_MASS, 1.0);
41-
pos_NE_nh.param<float>("Quad/throttle_a", throttle_a, 20.0);
42-
pos_NE_nh.param<float>("Quad/throttle_b", throttle_b, 0.0);
4341

4442
pos_NE_nh.param<float>("Pos_ne/Kp_xy", Kp[0], 1.0);
4543
pos_NE_nh.param<float>("Pos_ne/Kp_xy", Kp[1], 1.0);
@@ -61,8 +59,6 @@ class pos_controller_NE
6159
pos_NE_nh.param<float>("Limit/pxy_int_max" , int_max[0], 0.5);
6260
pos_NE_nh.param<float>("Limit/pxy_int_max" , int_max[1], 0.5);
6361
pos_NE_nh.param<float>("Limit/pz_int_max" , int_max[2], 0.5);
64-
pos_NE_nh.param<float>("Limit/THR_MIN", THR_MIN, 0.1);
65-
pos_NE_nh.param<float>("Limit/THR_MAX", THR_MAX, 0.9);
6662
pos_NE_nh.param<float>("Limit/tilt_max", tilt_max, 20.0);
6763
pos_NE_nh.param<float>("Limit/int_start_error" , int_start_error, 0.3);
6864

@@ -72,23 +68,20 @@ class pos_controller_NE
7268
integral_LLF = Eigen::Vector3f(0.0,0.0,0.0);
7369
NoiseEstimator = Eigen::Vector3f(0.0,0.0,0.0);
7470
output_LLF = Eigen::Vector3f(0.0,0.0,0.0);
75-
thrust_sp = Eigen::Vector3d(0.0,0.0,0.0);
71+
throttle_sp = Eigen::Vector3d(0.0,0.0,0.0);
7672

7773
set_filter();
7874
}
7975

8076

8177
//Quadrotor Parameter
8278
float Quad_MASS;
83-
float throttle_a;
84-
float throttle_b;
8579

8680
//Limitation
8781
Eigen::Vector3f pos_error_max;
8882
Eigen::Vector3f vel_error_max;
8983
Eigen::Vector3f int_max;
90-
float THR_MIN;
91-
float THR_MAX;
84+
9285
float tilt_max;
9386
float int_start_error;
9487

@@ -124,7 +117,7 @@ class pos_controller_NE
124117

125118
Eigen::Vector3f NoiseEstimator;
126119

127-
Eigen::Vector3d thrust_sp;
120+
Eigen::Vector3d throttle_sp;
128121

129122
//Printf the NE parameter
130123
void printf_param();
@@ -171,7 +164,7 @@ Eigen::Vector3d pos_controller_NE::pos_controller(
171164
px4_command::TrajectoryPoint _Reference_State, float dt)
172165
{
173166
Eigen::Vector3d accel_sp;
174-
Eigen::Vector3d thrust_sp;
167+
Eigen::Vector3d throttle_sp;
175168
// 计算误差项
176169
Eigen::Vector3f pos_error = px4_command_utils::cal_pos_error(_DroneState, _Reference_State);
177170
Eigen::Vector3f vel_error = px4_command_utils::cal_vel_error(_DroneState, _Reference_State);
@@ -239,24 +232,9 @@ Eigen::Vector3d pos_controller_NE::pos_controller(
239232

240233
// 期望推力 = 期望加速度 × 质量
241234
// 归一化推力 ： 根据电机模型，反解出归一化推力
242-
for (int i=0; i<3; i++)
243-
{
244-
thrust_sp[i] = (accel_sp[i] * Quad_MASS - throttle_b) / throttle_a;
245-
}
246-
247-
// 推力限幅，根据最大倾斜角及最大油门
248-
// Get maximum allowed thrust in XY based on tilt angle and excess thrust.
249-
float thrust_max_XY_tilt = fabs(thrust_sp[2]) * tanf(tilt_max/180.0*M_PI);
250-
float thrust_max_XY = sqrtf(THR_MAX * THR_MAX - pow(thrust_sp[2],2));
251-
thrust_max_XY = min(thrust_max_XY_tilt, thrust_max_XY);
252-
253-
if ((pow(thrust_sp[0],2) + pow(thrust_sp[1],2)) > thrust_max_XY * thrust_max_XY) {
254-
float mag = sqrtf((pow(thrust_sp[0],2) + pow(thrust_sp[1],2)));
255-
thrust_sp[0] = thrust_sp[0] / mag * thrust_max_XY;
256-
thrust_sp[1] = thrust_sp[1] / mag * thrust_max_XY;
257-
}
235+
throttle_sp = px4_command_utils::accelToThrottle(accel_sp, Quad_MASS, tilt_max);
258236

259-
return thrust_sp;
237+
return throttle_sp;
260238
}
261239

262240
void pos_controller_NE::printf_result()
@@ -281,7 +259,6 @@ void pos_controller_NE::printf_result()
281259
cout << "output_LLF [X Y Z] : " << output_LLF[0] << " [N] "<< output_LLF[1]<<" [N] "<<output_LLF[2]<<" [N] "<<endl;
282260

283261
cout << "u_d [X Y Z] : " << u_d[0] << " [N] "<< u_d[1]<<" [N] "<<u_d[2]<<" [N] "<<endl;
284-
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;
285262
}
286263

287264
// 【打印参数函数】
@@ -290,8 +267,6 @@ void pos_controller_NE::printf_param()
290267
cout <<">>>>>>>>>>>>>>>>>>>>>>>>>>NE Parameter <<<<<<<<<<<<<<<<<<<<<<<<<" <<endl;
291268

292269
cout <<"Quad_MASS : "<< Quad_MASS << endl;
293-
cout <<"throttle_a : "<< throttle_a << endl;
294-
cout <<"throttle_b : "<< throttle_b << endl;
295270

296271
cout <<"Kp_X : "<< Kp[0] << endl;
297272
cout <<"Kp_Y : "<< Kp[1] << endl;
@@ -313,8 +288,6 @@ void pos_controller_NE::printf_param()
313288
cout <<"vz_error_max : "<< vel_error_max[2] << endl;
314289
cout <<"pxy_int_max : "<< int_max[0] << endl;
315290
cout <<"pz_int_max : "<< int_max[2] << endl;
316-
cout <<"THR_MIN : "<< THR_MIN << endl;
317-
cout <<"THR_MAX : "<< THR_MAX << endl;
318291
cout <<"tilt_max : "<< tilt_max << endl;
319292
cout <<"int_start_error : "<< int_start_error << endl;
320293

‎include/pos_controller_PID.h

+6-43
Original file line numberDiff line numberDiff line change
@@ -32,8 +32,6 @@ class pos_controller_PID
3232
pos_pid_nh("~")
3333
{
3434
pos_pid_nh.param<float>("Quad/mass", Quad_MASS, 1.0);
35-
pos_pid_nh.param<float>("Quad/throttle_a", throttle_a, 20.0);
36-
pos_pid_nh.param<float>("Quad/throttle_b", throttle_b, 0.0);
3735

3836
pos_pid_nh.param<float>("Pos_pid/Kp_xy", Kp[0], 1.0);
3937
pos_pid_nh.param<float>("Pos_pid/Kp_xy", Kp[1], 1.0);
@@ -54,19 +52,15 @@ class pos_controller_PID
5452
pos_pid_nh.param<float>("Limit/pxy_int_max" , int_max[0], 0.5);
5553
pos_pid_nh.param<float>("Limit/pxy_int_max" , int_max[1], 0.5);
5654
pos_pid_nh.param<float>("Limit/pz_int_max" , int_max[2], 0.5);
57-
pos_pid_nh.param<float>("Limit/THR_MIN", THR_MIN, 0.1);
58-
pos_pid_nh.param<float>("Limit/THR_MAX", THR_MAX, 0.9);
5955
pos_pid_nh.param<float>("Limit/tilt_max", tilt_max, 20.0);
6056
pos_pid_nh.param<float>("Limit/int_start_error" , int_start_error, 0.3);
6157

6258
integral = Eigen::Vector3f(0.0,0.0,0.0);
63-
thrust_sp = Eigen::Vector3d(0.0,0.0,0.0);
59+
throttle_sp = Eigen::Vector3d(0.0,0.0,0.0);
6460
}
6561

6662
//Quadrotor Parameter
6763
float Quad_MASS;
68-
float throttle_a;
69-
float throttle_b;
7064

7165
//PID parameter for the control law
7266
Eigen::Vector3f Kp;
@@ -77,16 +71,14 @@ class pos_controller_PID
7771
Eigen::Vector3f pos_error_max;
7872
Eigen::Vector3f vel_error_max;
7973
Eigen::Vector3f int_max;
80-
float THR_MIN;
81-
float THR_MAX;
8274
float tilt_max;
8375
float int_start_error;
8476

8577
//积分项
8678
Eigen::Vector3f integral;
8779

8880
//输出
89-
Eigen::Vector3d thrust_sp;
81+
Eigen::Vector3d throttle_sp;
9082

9183
//Printf the PID parameter
9284
void printf_param();
@@ -107,7 +99,7 @@ Eigen::Vector3d pos_controller_PID::pos_controller(
10799
px4_command::TrajectoryPoint _Reference_State, float dt)
108100
{
109101
Eigen::Vector3d accel_sp;
110-
Eigen::Vector3d thrust_sp;
102+
Eigen::Vector3d throttle_sp;
111103

112104
// 计算误差项
113105
Eigen::Vector3f pos_error = px4_command_utils::cal_pos_error(_DroneState, _Reference_State);
@@ -154,36 +146,11 @@ Eigen::Vector3d pos_controller_PID::pos_controller(
154146
}
155147
}
156148

157-
// cout << "ff [X Y Z] : " << _Reference_State.acceleration_ref[0] << " [m/s] "<< _Reference_State.acceleration_ref[1]<<" [m/s] "<<_Reference_State.acceleration_ref[2]<<" [m/s] "<<endl;
158-
159-
160-
// cout << "Vel_P_output [X Y Z] : " << Kp[0] * pos_error[0] << " [m/s] "<< Kp[1] * pos_error[1]<<" [m/s] "<<Kp[2] * pos_error[2]<<" [m/s] "<<endl;
161-
162-
// cout << "Vel_I_output [X Y Z] : " << integral[0] << " [m/s] "<< integral[1]<<" [m/s] "<<integral[2]<<" [m/s] "<<endl;
163-
164-
// cout << "Vel_D_output [X Y Z] : " << Kd[0] * vel_error[0] << " [m/s] "<< Kd[1] * vel_error[1]<<" [m/s] "<<Kd[2] * vel_error[2]<<" [m/s] "<<endl;
165-
166-
167149
// 期望推力 = 期望加速度 × 质量
168150
// 归一化推力 ： 根据电机模型，反解出归一化推力
169-
for (int i=0; i<3; i++)
170-
{
171-
thrust_sp[i] = (accel_sp[i] * Quad_MASS - throttle_b) / throttle_a;
172-
}
173-
174-
// 推力限幅，根据最大倾斜角及最大油门
175-
// Get maximum allowed thrust in XY based on tilt angle and excess thrust.
176-
float thrust_max_XY_tilt = fabs(thrust_sp[2]) * tanf(tilt_max/180.0*M_PI);
177-
float thrust_max_XY = sqrtf(THR_MAX * THR_MAX - pow(thrust_sp[2],2));
178-
thrust_max_XY = min(thrust_max_XY_tilt, thrust_max_XY);
179-
180-
if ((pow(thrust_sp[0],2) + pow(thrust_sp[1],2)) > thrust_max_XY * thrust_max_XY) {
181-
float mag = sqrtf((pow(thrust_sp[0],2) + pow(thrust_sp[1],2)));
182-
thrust_sp[0] = thrust_sp[0] / mag * thrust_max_XY;
183-
thrust_sp[1] = thrust_sp[1] / mag * thrust_max_XY;
184-
}
151+
throttle_sp = px4_command_utils::accelToThrottle(accel_sp, Quad_MASS, tilt_max);
185152

186-
return thrust_sp;
153+
return throttle_sp;
187154
}
188155

189156
void pos_controller_PID::printf_result()
@@ -201,7 +168,7 @@ void pos_controller_PID::printf_result()
201168

202169
cout<<setprecision(2);
203170

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;
171+
cout << "throttle_sp [X Y Z] : " << throttle_sp[0] << " [m/s^2] "<< throttle_sp[1]<<" [m/s^2] "<<throttle_sp[2]<<" [m/s^2] "<<endl;
205172

206173
}
207174

@@ -211,8 +178,6 @@ void pos_controller_PID::printf_param()
211178
cout <<">>>>>>>>>>>>>>>>>>>>>>>>>>>>>>PID Parameter <<<<<<<<<<<<<<<<<<<<<<<<<<<" <<endl;
212179

213180
cout <<"Quad_MASS : "<< Quad_MASS << endl;
214-
cout <<"throttle_a : "<< throttle_a << endl;
215-
cout <<"throttle_b : "<< throttle_b << endl;
216181

217182
cout <<"Kp_x : "<< Kp[0] << endl;
218183
cout <<"Kp_y : "<< Kp[1] << endl;
@@ -233,8 +198,6 @@ void pos_controller_PID::printf_param()
233198
cout <<"vz_error_max : "<< vel_error_max[2] << endl;
234199
cout <<"pxy_int_max : "<< int_max[0] << endl;
235200
cout <<"pz_int_max : "<< int_max[2] << endl;
236-
cout <<"THR_MIN : "<< THR_MIN << endl;
237-
cout <<"THR_MAX : "<< THR_MAX << endl;
238201
cout <<"tilt_max : "<< tilt_max << endl;
239202
cout <<"int_start_error : "<< int_start_error << endl;
240203

‎include/pos_controller_Passivity.h

+7-33
Original file line numberDiff line numberDiff line change
@@ -35,8 +35,7 @@ class pos_controller_passivity
3535
pos_passivity_nh("~")
3636
{
3737
pos_passivity_nh.param<float>("Quad/mass", Quad_MASS, 1.0);
38-
pos_passivity_nh.param<float>("Quad/throttle_a", throttle_a, 20.0);
39-
pos_passivity_nh.param<float>("Quad/throttle_b", throttle_b, 0.0);
38+
4039

4140
pos_passivity_nh.param<float>("Pos_passivity/Kp_xy", Kp[0], 1.0);
4241
pos_passivity_nh.param<float>("Pos_passivity/Kp_xy", Kp[1], 1.0);
@@ -58,15 +57,13 @@ class pos_controller_passivity
5857
pos_passivity_nh.param<float>("Limit/pxy_int_max" , int_max[0], 0.5);
5958
pos_passivity_nh.param<float>("Limit/pxy_int_max" , int_max[1], 0.5);
6059
pos_passivity_nh.param<float>("Limit/pz_int_max" , int_max[2], 0.5);
61-
pos_passivity_nh.param<float>("Limit/THR_MIN", THR_MIN, 0.1);
62-
pos_passivity_nh.param<float>("Limit/THR_MAX", THR_MAX, 0.9);
6360
pos_passivity_nh.param<float>("Limit/tilt_max", tilt_max, 20.0);
6461
pos_passivity_nh.param<float>("Limit/int_start_error" , int_start_error, 0.3);
6562

6663
u_l = Eigen::Vector3f(0.0,0.0,0.0);
6764
u_d = Eigen::Vector3f(0.0,0.0,0.0);
6865
integral = Eigen::Vector3f(0.0,0.0,0.0);
69-
thrust_sp = Eigen::Vector3d(0.0,0.0,0.0);
66+
throttle_sp = Eigen::Vector3d(0.0,0.0,0.0);
7067

7168
y1_k = Eigen::Vector3f(0.0,0.0,0.0);
7269
y2_k = Eigen::Vector3f(0.0,0.0,0.0);
@@ -78,8 +75,6 @@ class pos_controller_passivity
7875

7976
//Quadrotor Parameter
8077
float Quad_MASS;
81-
float throttle_a;
82-
float throttle_b;
8378

8479
//passivity control parameter
8580
Eigen::Vector3f Kp;
@@ -91,15 +86,13 @@ class pos_controller_passivity
9186
Eigen::Vector3f pos_error_max;
9287
Eigen::Vector3f vel_error_max;
9388
Eigen::Vector3f int_max;
94-
float THR_MIN;
95-
float THR_MAX;
9689
float tilt_max;
9790
float int_start_error;
9891

9992
//u_l for nominal contorol(PD), u_d for passivity control(disturbance estimator)
10093
Eigen::Vector3f u_l,u_d;
10194
Eigen::Vector3f integral;
102-
Eigen::Vector3d thrust_sp;
95+
Eigen::Vector3d throttle_sp;
10396

10497
HighPassFilter HPF_pos_error_x;
10598
HighPassFilter HPF_pos_error_y;
@@ -164,7 +157,7 @@ Eigen::Vector3d pos_controller_passivity::pos_controller(
164157
px4_command::TrajectoryPoint _Reference_State, float dt)
165158
{
166159
Eigen::Vector3d accel_sp;
167-
Eigen::Vector3d thrust_sp;
160+
Eigen::Vector3d throttle_sp;
168161
// 计算误差项
169162
Eigen::Vector3f pos_error = px4_command_utils::cal_pos_error(_DroneState, _Reference_State);
170163
Eigen::Vector3f vel_error = px4_command_utils::cal_vel_error(_DroneState, _Reference_State);
@@ -232,24 +225,9 @@ Eigen::Vector3d pos_controller_passivity::pos_controller(
232225

233226
// 期望推力 = 期望加速度 × 质量
234227
// 归一化推力 ： 根据电机模型，反解出归一化推力
235-
for (int i=0; i<3; i++)
236-
{
237-
thrust_sp[i] = (accel_sp[i] * Quad_MASS - throttle_b) / throttle_a;
238-
}
239-
240-
// 推力限幅，根据最大倾斜角及最大油门
241-
// Get maximum allowed thrust in XY based on tilt angle and excess thrust.
242-
float thrust_max_XY_tilt = fabs(thrust_sp[2]) * tanf(tilt_max/180.0*M_PI);
243-
float thrust_max_XY = sqrtf(THR_MAX * THR_MAX - pow(thrust_sp[2],2));
244-
thrust_max_XY = min(thrust_max_XY_tilt, thrust_max_XY);
245-
246-
if ((pow(thrust_sp[0],2) + pow(thrust_sp[1],2)) > thrust_max_XY * thrust_max_XY) {
247-
float mag = sqrtf((pow(thrust_sp[0],2) + pow(thrust_sp[1],2)));
248-
thrust_sp[0] = thrust_sp[0] / mag * thrust_max_XY;
249-
thrust_sp[1] = thrust_sp[1] / mag * thrust_max_XY;
250-
}
228+
throttle_sp = px4_command_utils::accelToThrottle(accel_sp, Quad_MASS, tilt_max);
251229

252-
return thrust_sp;
230+
return throttle_sp;
253231
}
254232

255233

@@ -259,8 +237,6 @@ void pos_controller_passivity::printf_param()
259237
cout <<">>>>>>>>>>>>>>>>>>>>>>>>>>passivity Parameter <<<<<<<<<<<<<<<<<<<<<<<<<" <<endl;
260238

261239
cout <<"Quad_MASS : "<< Quad_MASS << endl;
262-
cout <<"throttle_a : "<< throttle_a << endl;
263-
cout <<"throttle_b : "<< throttle_b << endl;
264240

265241
cout <<"Kp_X : "<< Kp[0] << endl;
266242
cout <<"Kp_Y : "<< Kp[1] << endl;
@@ -282,8 +258,6 @@ void pos_controller_passivity::printf_param()
282258
cout <<"vz_error_max : "<< vel_error_max[2] << endl;
283259
cout <<"pxy_int_max : "<< int_max[0] << endl;
284260
cout <<"pz_int_max : "<< int_max[2] << endl;
285-
cout <<"THR_MIN : "<< THR_MIN << endl;
286-
cout <<"THR_MAX : "<< THR_MAX << endl;
287261
cout <<"tilt_max : "<< tilt_max << endl;
288262
cout <<"int_start_error : "<< int_start_error << endl;
289263

@@ -317,7 +291,7 @@ void pos_controller_passivity::printf_result()
317291

318292
cout << "u_d [X Y Z] : " << u_d[0] << " [N] "<< u_d[1]<<" [N] "<<u_d[2]<<" [N] "<<endl;
319293

320-
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;
294+
cout << "throttle_sp [X Y Z] : " << throttle_sp[0] << " [m/s^2] "<< throttle_sp[1]<<" [m/s^2] "<<throttle_sp[2]<<" [m/s^2] "<<endl;
321295

322296
}
323297

0 commit comments

Comments
 (0)