Skip to content

Commit b97b1a8

Browse files
AhmedHumaisAhmedHumais
committed
updated
1 parent 8491d15 commit b97b1a8

File tree

2 files changed

+32
-32
lines changed

2 files changed

+32
-32
lines changed

‎example_node/SLAM_tuning_node.cpp‎

Lines changed: 14 additions & 14 deletions
Original file line numberDiff line numberDiff line change
@@ -26,12 +26,12 @@
2626
#include "HEAR_ROS_BRIDGE/ROSUnit_RestNormSettingsClnt.hpp"
2727
#include "HEAR_ROS_BRIDGE/ROSUnit_ControlOutputSubscriber.hpp"
2828

29-
//#define MRFT_POS_X
29+
#define MRFT_POS_X
3030
//#define MRFT_POS_Y
3131
//#define MRFT_POS_Z
3232

33-
#define MRFT_ATT_ROLL
34-
#define MRFT_ATT_PITCH
33+
//#define MRFT_ATT_ROLL
34+
//#define MRFT_ATT_PITCH
3535

3636
#define MRFT_SLAM
3737
//#define KF
@@ -328,18 +328,18 @@ int main(int argc, char** argv) {
328328
((UpdateController*)update_controller_pid_z)->pid_data.id = block_id::PID_Z;
329329

330330

331-
((UpdateController*)update_controller_pid_roll)->pid_data.kp = 0.4991; //0.225*0.8; //0.172195; //0.3302; //0.286708; //0.225 * 0.8;
331+
((UpdateController*)update_controller_pid_roll)->pid_data.kp = 0.423827421; //0.225*0.8; //0.172195; //0.3302; //0.286708; //0.225 * 0.8;
332332
((UpdateController*)update_controller_pid_roll)->pid_data.ki = 0.0;
333-
((UpdateController*)update_controller_pid_roll)->pid_data.kd = 0.0750; //0.04*0.8; //0.042464; //0.0931; //0.056559; //0.04 * 0.8;
333+
((UpdateController*)update_controller_pid_roll)->pid_data.kd = 0.063652685; //0.04*0.8; //0.042464; //0.0931; //0.056559; //0.04 * 0.8;
334334
((UpdateController*)update_controller_pid_roll)->pid_data.kdd = 0.0;
335335
((UpdateController*)update_controller_pid_roll)->pid_data.anti_windup = 0;
336336
((UpdateController*)update_controller_pid_roll)->pid_data.en_pv_derivation = 1;
337337
((UpdateController*)update_controller_pid_roll)->pid_data.dt = 1.f/200.f;
338338
((UpdateController*)update_controller_pid_roll)->pid_data.id = block_id::PID_ROLL;
339339

340-
((UpdateController*)update_controller_pid_pitch)->pid_data.kp = 0.5271; //0.225*0.8;// 0.3360; //0.2811;//0.275252; //0.225 * 0.8;
340+
((UpdateController*)update_controller_pid_pitch)->pid_data.kp = 0.434061228; //0.225*0.8;// 0.3360; //0.2811;//0.275252; //0.225 * 0.8;
341341
((UpdateController*)update_controller_pid_pitch)->pid_data.ki = 0.0;
342-
((UpdateController*)update_controller_pid_pitch)->pid_data.kd = 0.0792; //0.04*0.8;//0.0684; //0.053100; //0.0868;// 0.051266; //0.04 * 0.8;
342+
((UpdateController*)update_controller_pid_pitch)->pid_data.kd = 0.065189653; //0.04*0.8;//0.0684; //0.053100; //0.0868;// 0.051266; //0.04 * 0.8;
343343
((UpdateController*)update_controller_pid_pitch)->pid_data.kdd = 0.0;
344344
((UpdateController*)update_controller_pid_pitch)->pid_data.anti_windup = 0;
345345
((UpdateController*)update_controller_pid_pitch)->pid_data.en_pv_derivation = 1;
@@ -366,27 +366,27 @@ int main(int argc, char** argv) {
366366

367367
#ifdef MRFT_POS_X
368368
((UpdateController*)update_controller_mrft_x)->mrft_data.beta = -0.735;
369-
((UpdateController*)update_controller_mrft_x)->mrft_data.relay_amp = 1.5;
369+
((UpdateController*)update_controller_mrft_x)->mrft_data.relay_amp = 2.0;
370370
((UpdateController*)update_controller_mrft_x)->mrft_data.bias = 0.0;
371-
((UpdateController*)update_controller_mrft_x)->mrft_data.num_of_peak_conf_samples = 20;
371+
((UpdateController*)update_controller_mrft_x)->mrft_data.num_of_peak_conf_samples = 14;
372372
((UpdateController*)update_controller_mrft_x)->mrft_data.no_switch_delay_in_ms = 100;
373373
((UpdateController*)update_controller_mrft_x)->mrft_data.id = block_id::MRFT_X;
374374
#endif
375375

376376
#ifdef MRFT_POS_Y
377377
((UpdateController*)update_controller_mrft_y)->mrft_data.beta = -0.735;
378-
((UpdateController*)update_controller_mrft_y)->mrft_data.relay_amp = 2.0;
378+
((UpdateController*)update_controller_mrft_y)->mrft_data.relay_amp = 2.5;
379379
((UpdateController*)update_controller_mrft_y)->mrft_data.bias = 0.0;
380380
((UpdateController*)update_controller_mrft_y)->mrft_data.no_switch_delay_in_ms = 100;
381-
((UpdateController*)update_controller_mrft_y)->mrft_data.num_of_peak_conf_samples = 24;
381+
((UpdateController*)update_controller_mrft_y)->mrft_data.num_of_peak_conf_samples = 14;
382382
((UpdateController*)update_controller_mrft_y)->mrft_data.id = block_id::MRFT_Y;
383383
#endif
384384

385385
#ifdef MRFT_POS_Z
386386
((UpdateController*)update_controller_mrft_z)->mrft_data.beta = -0.73;
387387
((UpdateController*)update_controller_mrft_z)->mrft_data.relay_amp = 2.5; //0.1;
388388
((UpdateController*)update_controller_mrft_z)->mrft_data.bias = 0.0;
389-
((UpdateController*)update_controller_mrft_z)->mrft_data.num_of_peak_conf_samples = 20;
389+
((UpdateController*)update_controller_mrft_z)->mrft_data.num_of_peak_conf_samples = 10;
390390
((UpdateController*)update_controller_mrft_z)->mrft_data.id = block_id::MRFT_Z;
391391
#endif
392392

@@ -423,9 +423,9 @@ int main(int argc, char** argv) {
423423
((UpdateController*)update_controller_pid_slam_y)->pid_data.dt = (float)1.0/SLAM_FREQ;
424424
((UpdateController*)update_controller_pid_slam_y)->pid_data.id = block_id::PID_Y;
425425

426-
((UpdateController*)update_controller_pid_slam_z)->pid_data.kp = 13.8;
426+
((UpdateController*)update_controller_pid_slam_z)->pid_data.kp = 12.17657513;//15.7*0.8;
427427
((UpdateController*)update_controller_pid_slam_z)->pid_data.ki = 0.0;
428-
((UpdateController*)update_controller_pid_slam_z)->pid_data.kd = 6.0;
428+
((UpdateController*)update_controller_pid_slam_z)->pid_data.kd = 5.553992435; //5.5*0.8;
429429
((UpdateController*)update_controller_pid_slam_z)->pid_data.kdd = 0.0;
430430
((UpdateController*)update_controller_pid_slam_z)->pid_data.anti_windup = 0;
431431
((UpdateController*)update_controller_pid_slam_z)->pid_data.en_pv_derivation = 1;

‎example_node/test_node.cpp‎

Lines changed: 18 additions & 18 deletions
Original file line numberDiff line numberDiff line change
@@ -26,11 +26,11 @@
2626
#include "HEAR_ROS_BRIDGE/ROSUnit_ControlOutputSubscriber.hpp"
2727

2828

29-
const float TAKE_OFF_HEIGHT = 1.0;
29+
const float TAKE_OFF_HEIGHT = 1.25;
3030
const float LAND_HEIGHT = -0.01;
3131

3232
//#define STORE_KF_BIAS
33-
#define TRAJ_TEST
33+
//#define TRAJ_TEST
3434
#define SLAM_TEST
3535
//#define AUTO_TEST
3636
#define TESTING
@@ -219,45 +219,45 @@ int main(int argc, char** argv) {
219219

220220
//*************Setting Flight Elements*************
221221
#ifdef SMALL_HEXA
222-
((UpdateController*)update_controller_pid_x)->pid_data.kp = 9.28; //0.583430204; //0.8786*0.5; //0.51639 * 0.8;
222+
((UpdateController*)update_controller_pid_x)->pid_data.kp = 14.33366959*0.9; //0.583430204; //0.8786*0.5; //0.51639 * 0.8;
223223
((UpdateController*)update_controller_pid_x)->pid_data.ki = 0.0;
224-
((UpdateController*)update_controller_pid_x)->pid_data.kd = 3.37; //0.211996855; //0.3441*0.5; //0.21192 * 0.8;
224+
((UpdateController*)update_controller_pid_x)->pid_data.kd = 5.208322871*0.9; //0.211996855; //0.3441*0.5; //0.21192 * 0.8;
225225
((UpdateController*)update_controller_pid_x)->pid_data.kdd = 0.0;
226226
((UpdateController*)update_controller_pid_x)->pid_data.anti_windup = 0;
227227
((UpdateController*)update_controller_pid_x)->pid_data.en_pv_derivation = 1;
228228
((UpdateController*)update_controller_pid_x)->pid_data.dt = (float)1.0/120.0;
229229
((UpdateController*)update_controller_pid_x)->pid_data.id = block_id::PID_X;
230230

231-
((UpdateController*)update_controller_pid_y)->pid_data.kp = 9.5; //0.592289033;// 0.6673*0.75;// 0.6714;// 0.51639 * 0.8;
231+
((UpdateController*)update_controller_pid_y)->pid_data.kp = 15.00427998*0.9; //0.592289033;// 0.6673*0.75;// 0.6714;// 0.51639 * 0.8;
232232
((UpdateController*)update_controller_pid_y)->pid_data.ki = 0.0;
233-
((UpdateController*)update_controller_pid_y)->pid_data.kd = 3.45; //0.215215824;// 0.2583*0.75; //-0.2440;// * 0.8;
233+
((UpdateController*)update_controller_pid_y)->pid_data.kd = 5.451997767*0.9; //0.215215824;// 0.2583*0.75; //-0.2440;// * 0.8;
234234
((UpdateController*)update_controller_pid_y)->pid_data.kdd = 0.0;
235235
((UpdateController*)update_controller_pid_y)->pid_data.anti_windup = 0;
236236
((UpdateController*)update_controller_pid_y)->pid_data.en_pv_derivation = 1;
237237
((UpdateController*)update_controller_pid_y)->pid_data.dt = (float)1.0/120.0;
238238
((UpdateController*)update_controller_pid_y)->pid_data.id = block_id::PID_Y;
239239

240-
((UpdateController*)update_controller_pid_z)->pid_data.kp = 21.5; //0.613969957; // 1.2414*0.75;
240+
((UpdateController*)update_controller_pid_z)->pid_data.kp = 27.62993448; //0.613969957; // 1.2414*0.75;
241241
((UpdateController*)update_controller_pid_z)->pid_data.ki = 0.0;
242-
((UpdateController*)update_controller_pid_z)->pid_data.kd = 6.8; //0.214920534; // 0.3316*0.75;
242+
((UpdateController*)update_controller_pid_z)->pid_data.kd = 8.13692084; //0.214920534; // 0.3316*0.75;
243243
((UpdateController*)update_controller_pid_z)->pid_data.kdd = 0.0;
244244
((UpdateController*)update_controller_pid_z)->pid_data.anti_windup = 0;
245245
((UpdateController*)update_controller_pid_z)->pid_data.en_pv_derivation = 1;
246246
((UpdateController*)update_controller_pid_z)->pid_data.dt = (float)1.0/120.0;
247247
((UpdateController*)update_controller_pid_z)->pid_data.id = block_id::PID_Z;
248248

249-
((UpdateController*)update_controller_pid_roll)->pid_data.kp = 0.4991; //0.225*0.8; //0.172195; //0.3302; //0.286708; //0.225 * 0.8;
249+
((UpdateController*)update_controller_pid_roll)->pid_data.kp = 0.423827421; //0.225*0.8; //0.172195; //0.3302; //0.286708; //0.225 * 0.8;
250250
((UpdateController*)update_controller_pid_roll)->pid_data.ki = 0.0;
251-
((UpdateController*)update_controller_pid_roll)->pid_data.kd = 0.0750; //0.04*0.8; //0.042464; //0.0931; //0.056559; //0.04 * 0.8;
251+
((UpdateController*)update_controller_pid_roll)->pid_data.kd = 0.063652685; //0.04*0.8; //0.042464; //0.0931; //0.056559; //0.04 * 0.8;
252252
((UpdateController*)update_controller_pid_roll)->pid_data.kdd = 0.0;
253253
((UpdateController*)update_controller_pid_roll)->pid_data.anti_windup = 0;
254254
((UpdateController*)update_controller_pid_roll)->pid_data.en_pv_derivation = 1;
255255
((UpdateController*)update_controller_pid_roll)->pid_data.dt = 1.f/200.f;
256256
((UpdateController*)update_controller_pid_roll)->pid_data.id = block_id::PID_ROLL;
257257

258-
((UpdateController*)update_controller_pid_pitch)->pid_data.kp = 0.5271; //0.225*0.8;// 0.3360; //0.2811;//0.275252; //0.225 * 0.8;
258+
((UpdateController*)update_controller_pid_pitch)->pid_data.kp = 0.434061228; //0.225*0.8;// 0.3360; //0.2811;//0.275252; //0.225 * 0.8;
259259
((UpdateController*)update_controller_pid_pitch)->pid_data.ki = 0.0;
260-
((UpdateController*)update_controller_pid_pitch)->pid_data.kd = 0.0792; //0.04*0.8;//0.0684; //0.053100; //0.0868;// 0.051266; //0.04 * 0.8;
260+
((UpdateController*)update_controller_pid_pitch)->pid_data.kd = 0.065189653; //0.04*0.8;//0.0684; //0.053100; //0.0868;// 0.051266; //0.04 * 0.8;
261261
((UpdateController*)update_controller_pid_pitch)->pid_data.kdd = 0.0;
262262
((UpdateController*)update_controller_pid_pitch)->pid_data.anti_windup = 0;
263263
((UpdateController*)update_controller_pid_pitch)->pid_data.en_pv_derivation = 1;
@@ -283,27 +283,27 @@ int main(int argc, char** argv) {
283283
((UpdateController*)update_controller_pid_yaw_rate)->pid_data.id = block_id::PID_YAW_RATE;
284284

285285
#ifdef SLAM_TEST
286-
((UpdateController*)update_pid_slam_x)->pid_data.kp = 7.64; //0.311; //0.51639 * 0.8;
286+
((UpdateController*)update_pid_slam_x)->pid_data.kp = 7.35*1.4; //0.311; //0.51639 * 0.8;
287287
((UpdateController*)update_pid_slam_x)->pid_data.ki = 0.0;
288-
((UpdateController*)update_pid_slam_x)->pid_data.kd = 4.086; //0.151; //0.21192 * 0.8;
288+
((UpdateController*)update_pid_slam_x)->pid_data.kd = 3.93*1.4; //0.151; //0.21192 * 0.8;
289289
((UpdateController*)update_pid_slam_x)->pid_data.kdd = 0.0;
290290
((UpdateController*)update_pid_slam_x)->pid_data.anti_windup = 0;
291291
((UpdateController*)update_pid_slam_x)->pid_data.en_pv_derivation = 1;
292292
((UpdateController*)update_pid_slam_x)->pid_data.dt = (float)1.0/120.0;
293293
((UpdateController*)update_pid_slam_x)->pid_data.id = block_id::PID_X;
294294

295-
((UpdateController*)update_pid_slam_y)->pid_data.kp = 8.695; //0.332;// 0.6714;// 0.51639 * 0.8;
295+
((UpdateController*)update_pid_slam_y)->pid_data.kp = 8.02; //0.332;// 0.6714;// 0.51639 * 0.8;
296296
((UpdateController*)update_pid_slam_y)->pid_data.ki = 0.0;
297-
((UpdateController*)update_pid_slam_y)->pid_data.kd = 4.65; //0.161; //-0.2440;// * 0.8;
297+
((UpdateController*)update_pid_slam_y)->pid_data.kd = 4.29; //0.161; //-0.2440;// * 0.8;
298298
((UpdateController*)update_pid_slam_y)->pid_data.kdd = 0.0;
299299
((UpdateController*)update_pid_slam_y)->pid_data.anti_windup = 0;
300300
((UpdateController*)update_pid_slam_y)->pid_data.en_pv_derivation = 1;
301301
((UpdateController*)update_pid_slam_y)->pid_data.dt = (float)1.0/120.0;
302302
((UpdateController*)update_pid_slam_y)->pid_data.id = block_id::PID_Y;
303303

304-
((UpdateController*)update_pid_slam_z)->pid_data.kp = 13.73; //0.454*0.9;
304+
((UpdateController*)update_pid_slam_z)->pid_data.kp = 12.176; //0.454*0.9;
305305
((UpdateController*)update_pid_slam_z)->pid_data.ki = 0.0;
306-
((UpdateController*)update_pid_slam_z)->pid_data.kd = 4.8; //0.197*0.9;
306+
((UpdateController*)update_pid_slam_z)->pid_data.kd = 5.554; //0.197*0.9;
307307
((UpdateController*)update_pid_slam_z)->pid_data.kdd = 0.0;
308308
((UpdateController*)update_pid_slam_z)->pid_data.anti_windup = 0;
309309
((UpdateController*)update_pid_slam_z)->pid_data.en_pv_derivation = 1;

0 commit comments

Comments
 (0)