Skip to content

Commit 979ef90

Browse files
AhmedHumaisAhmedHumais
committed
changed gains
1 parent 603c2ae commit 979ef90

File tree

2 files changed

+44
-38
lines changed

2 files changed

+44
-38
lines changed

‎example_node/SLAM_tuning_node.cpp‎

Lines changed: 30 additions & 24 deletions
Original file line numberDiff line numberDiff line change
@@ -27,9 +27,9 @@
2727
#include "HEAR_ROS_BRIDGE/ROSUnit_ControlOutputSubscriber.hpp"
2828

2929
#define MRFT_POS_X
30-
#define MRFT_POS_Y
31-
#define MRFT_POS_Z
32-
30+
//#define MRFT_POS_Y
31+
//#define MRFT_POS_Z
32+
////////////////////////////////////// increase relay amplitude in Y
3333
#define MRFT_SLAM
3434
//#define KF
3535

@@ -244,33 +244,34 @@ int main(int argc, char** argv) {
244244

245245
//*************Setting Flight Elements*************
246246

247-
((UpdateController*)update_controller_pid_x)->pid_data.kp = 0.583430204; //0.8786*0.5; //0.51639 * 0.8;
247+
((UpdateController*)update_controller_pid_x)->pid_data.kp = 9.28; //0.583430204; //0.8786*0.5; //0.51639 * 0.8;
248248
((UpdateController*)update_controller_pid_x)->pid_data.ki = 0.0;
249-
((UpdateController*)update_controller_pid_x)->pid_data.kd = 0.211996855; //0.3441*0.5; //0.21192 * 0.8;
249+
((UpdateController*)update_controller_pid_x)->pid_data.kd = 3.37; //0.211996855; //0.3441*0.5; //0.21192 * 0.8;
250250
((UpdateController*)update_controller_pid_x)->pid_data.kdd = 0.0;
251251
((UpdateController*)update_controller_pid_x)->pid_data.anti_windup = 0;
252252
((UpdateController*)update_controller_pid_x)->pid_data.en_pv_derivation = 1;
253253
((UpdateController*)update_controller_pid_x)->pid_data.dt = (float)1.0/120.0;
254254
((UpdateController*)update_controller_pid_x)->pid_data.id = block_id::PID_X;
255255

256-
((UpdateController*)update_controller_pid_y)->pid_data.kp = 0.592289033;// 0.6673*0.75;// 0.6714;// 0.51639 * 0.8;
256+
((UpdateController*)update_controller_pid_y)->pid_data.kp = 9.5; //0.592289033;// 0.6673*0.75;// 0.6714;// 0.51639 * 0.8;
257257
((UpdateController*)update_controller_pid_y)->pid_data.ki = 0.0;
258-
((UpdateController*)update_controller_pid_y)->pid_data.kd = 0.215215824;// 0.2583*0.75; //-0.2440;// * 0.8;
258+
((UpdateController*)update_controller_pid_y)->pid_data.kd = 3.45; //0.215215824;// 0.2583*0.75; //-0.2440;// * 0.8;
259259
((UpdateController*)update_controller_pid_y)->pid_data.kdd = 0.0;
260260
((UpdateController*)update_controller_pid_y)->pid_data.anti_windup = 0;
261261
((UpdateController*)update_controller_pid_y)->pid_data.en_pv_derivation = 1;
262262
((UpdateController*)update_controller_pid_y)->pid_data.dt = (float)1.0/120.0;
263263
((UpdateController*)update_controller_pid_y)->pid_data.id = block_id::PID_Y;
264264

265-
((UpdateController*)update_controller_pid_z)->pid_data.kp = 0.613969957; // 1.2414*0.75;
265+
((UpdateController*)update_controller_pid_z)->pid_data.kp = 21.5; //0.613969957; // 1.2414*0.75;
266266
((UpdateController*)update_controller_pid_z)->pid_data.ki = 0.0;
267-
((UpdateController*)update_controller_pid_z)->pid_data.kd = 0.214920534; // 0.3316*0.75;
267+
((UpdateController*)update_controller_pid_z)->pid_data.kd = 6.8; //0.214920534; // 0.3316*0.75;
268268
((UpdateController*)update_controller_pid_z)->pid_data.kdd = 0.0;
269269
((UpdateController*)update_controller_pid_z)->pid_data.anti_windup = 0;
270270
((UpdateController*)update_controller_pid_z)->pid_data.en_pv_derivation = 1;
271271
((UpdateController*)update_controller_pid_z)->pid_data.dt = (float)1.0/120.0;
272272
((UpdateController*)update_controller_pid_z)->pid_data.id = block_id::PID_Z;
273273

274+
274275
((UpdateController*)update_controller_pid_roll)->pid_data.kp = 0.4991; //0.225*0.8; //0.172195; //0.3302; //0.286708; //0.225 * 0.8;
275276
((UpdateController*)update_controller_pid_roll)->pid_data.ki = 0.0;
276277
((UpdateController*)update_controller_pid_roll)->pid_data.kd = 0.0750; //0.04*0.8; //0.042464; //0.0931; //0.056559; //0.04 * 0.8;
@@ -309,52 +310,52 @@ int main(int argc, char** argv) {
309310

310311
#ifdef MRFT_POS_X
311312
((UpdateController*)update_controller_mrft_x)->mrft_data.beta = -0.735;
312-
((UpdateController*)update_controller_mrft_x)->mrft_data.relay_amp = 0.07;
313+
((UpdateController*)update_controller_mrft_x)->mrft_data.relay_amp = 1.5;
313314
((UpdateController*)update_controller_mrft_x)->mrft_data.bias = 0.0;
314-
((UpdateController*)update_controller_mrft_x)->mrft_data.num_of_peak_conf_samples = 12;
315+
((UpdateController*)update_controller_mrft_x)->mrft_data.num_of_peak_conf_samples = 20;
315316
((UpdateController*)update_controller_mrft_x)->mrft_data.no_switch_delay_in_ms = 100;
316317
((UpdateController*)update_controller_mrft_x)->mrft_data.id = block_id::MRFT_X;
317318
#endif
318319

319320
#ifdef MRFT_POS_Y
320321
((UpdateController*)update_controller_mrft_y)->mrft_data.beta = -0.735;
321-
((UpdateController*)update_controller_mrft_y)->mrft_data.relay_amp = 0.07;
322+
((UpdateController*)update_controller_mrft_y)->mrft_data.relay_amp = 1.5;
322323
((UpdateController*)update_controller_mrft_y)->mrft_data.bias = 0.0;
323324
((UpdateController*)update_controller_mrft_y)->mrft_data.no_switch_delay_in_ms = 100;
324-
((UpdateController*)update_controller_mrft_y)->mrft_data.num_of_peak_conf_samples = 12;
325+
((UpdateController*)update_controller_mrft_y)->mrft_data.num_of_peak_conf_samples = 20;
325326
((UpdateController*)update_controller_mrft_y)->mrft_data.id = block_id::MRFT_Y;
326327
#endif
327328

328329
#ifdef MRFT_POS_Z
329330
((UpdateController*)update_controller_mrft_z)->mrft_data.beta = -0.73;
330-
((UpdateController*)update_controller_mrft_z)->mrft_data.relay_amp = 0.15; //0.1;
331+
((UpdateController*)update_controller_mrft_z)->mrft_data.relay_amp = 2.5; //0.1;
331332
((UpdateController*)update_controller_mrft_z)->mrft_data.bias = 0.0;
332-
((UpdateController*)update_controller_mrft_z)->mrft_data.num_of_peak_conf_samples = 12;
333+
((UpdateController*)update_controller_mrft_z)->mrft_data.num_of_peak_conf_samples = 20;
333334
((UpdateController*)update_controller_mrft_z)->mrft_data.id = block_id::MRFT_Z;
334335
#endif
335336

336337
#ifdef MRFT_SLAM
337-
((UpdateController*)update_controller_pid_slam_x)->pid_data.kp = 0.15;
338+
((UpdateController*)update_controller_pid_slam_x)->pid_data.kp = 3.35;
338339
((UpdateController*)update_controller_pid_slam_x)->pid_data.ki = 0.0;
339-
((UpdateController*)update_controller_pid_slam_x)->pid_data.kd = 0.075;
340+
((UpdateController*)update_controller_pid_slam_x)->pid_data.kd = 1.63;
340341
((UpdateController*)update_controller_pid_slam_x)->pid_data.kdd = 0.0;
341342
((UpdateController*)update_controller_pid_slam_x)->pid_data.anti_windup = 0;
342343
((UpdateController*)update_controller_pid_slam_x)->pid_data.en_pv_derivation = 1;
343344
((UpdateController*)update_controller_pid_slam_x)->pid_data.dt = (float)1.0/SLAM_FREQ;
344345
((UpdateController*)update_controller_pid_slam_x)->pid_data.id = block_id::PID_X;
345346

346-
((UpdateController*)update_controller_pid_slam_y)->pid_data.kp = 0.15;
347+
((UpdateController*)update_controller_pid_slam_y)->pid_data.kp = 4.28;
347348
((UpdateController*)update_controller_pid_slam_y)->pid_data.ki = 0.0;
348-
((UpdateController*)update_controller_pid_slam_y)->pid_data.kd = 0.075;
349+
((UpdateController*)update_controller_pid_slam_y)->pid_data.kd = 2.08;
349350
((UpdateController*)update_controller_pid_slam_y)->pid_data.kdd = 0.0;
350351
((UpdateController*)update_controller_pid_slam_y)->pid_data.anti_windup = 0;
351352
((UpdateController*)update_controller_pid_slam_y)->pid_data.en_pv_derivation = 1;
352353
((UpdateController*)update_controller_pid_slam_y)->pid_data.dt = (float)1.0/SLAM_FREQ;
353354
((UpdateController*)update_controller_pid_slam_y)->pid_data.id = block_id::PID_Y;
354355

355-
((UpdateController*)update_controller_pid_slam_z)->pid_data.kp = 0.2;
356+
((UpdateController*)update_controller_pid_slam_z)->pid_data.kp = 13.8;
356357
((UpdateController*)update_controller_pid_slam_z)->pid_data.ki = 0.0;
357-
((UpdateController*)update_controller_pid_slam_z)->pid_data.kd = 0.09;
358+
((UpdateController*)update_controller_pid_slam_z)->pid_data.kd = 6.0;
358359
((UpdateController*)update_controller_pid_slam_z)->pid_data.kdd = 0.0;
359360
((UpdateController*)update_controller_pid_slam_z)->pid_data.anti_windup = 0;
360361
((UpdateController*)update_controller_pid_slam_z)->pid_data.en_pv_derivation = 1;
@@ -441,7 +442,9 @@ int main(int argc, char** argv) {
441442
mrft_pipeline.addElement((MissionElement*)update_controller_pid_slam_z);
442443
mrft_pipeline.addElement((MissionElement*)slam_switch_on);
443444
mrft_pipeline.addElement((MissionElement*)send_curr_slam_ref);
444-
mrft_pipeline.addElement((MissionElement*)&wait_100ms);
445+
// mrft_pipeline.addElement((MissionElement*)&wait_100ms);
446+
mrft_pipeline.addElement((MissionElement*)user_command);
447+
445448
#endif
446449

447450
mrft_pipeline.addElement((MissionElement*)mrft_switch_on_x);
@@ -476,7 +479,9 @@ int main(int argc, char** argv) {
476479
mrft_pipeline.addElement((MissionElement*)update_controller_pid_slam_z);
477480
mrft_pipeline.addElement((MissionElement*)slam_switch_on);
478481
mrft_pipeline.addElement((MissionElement*)send_curr_slam_ref);
479-
mrft_pipeline.addElement((MissionElement*)&wait_100ms);
482+
// mrft_pipeline.addElement((MissionElement*)&wait_100ms);
483+
mrft_pipeline.addElement((MissionElement*)user_command);
484+
480485
#endif
481486

482487
mrft_pipeline.addElement((MissionElement*)mrft_switch_on_y);
@@ -509,7 +514,8 @@ int main(int argc, char** argv) {
509514
mrft_pipeline.addElement((MissionElement*)update_controller_pid_slam_z);
510515
mrft_pipeline.addElement((MissionElement*)slam_switch_on);
511516
mrft_pipeline.addElement((MissionElement*)send_curr_slam_ref);
512-
mrft_pipeline.addElement((MissionElement*)&wait_100ms);
517+
// mrft_pipeline.addElement((MissionElement*)&wait_100ms);
518+
mrft_pipeline.addElement((MissionElement*)user_command);
513519
#endif
514520

515521
mrft_pipeline.addElement((MissionElement*)mrft_switch_on_z);

‎example_node/test_node.cpp‎

Lines changed: 14 additions & 14 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.5;
29+
const float TAKE_OFF_HEIGHT = 1.0;
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
@@ -200,27 +200,27 @@ int main(int argc, char** argv) {
200200

201201
//*************Setting Flight Elements*************
202202
#ifdef SMALL_HEXA
203-
((UpdateController*)update_controller_pid_x)->pid_data.kp = 0.583430204; //0.8786*0.5; //0.51639 * 0.8;
203+
((UpdateController*)update_controller_pid_x)->pid_data.kp = 9.28; //0.583430204; //0.8786*0.5; //0.51639 * 0.8;
204204
((UpdateController*)update_controller_pid_x)->pid_data.ki = 0.0;
205-
((UpdateController*)update_controller_pid_x)->pid_data.kd = 0.211996855; //0.3441*0.5; //0.21192 * 0.8;
205+
((UpdateController*)update_controller_pid_x)->pid_data.kd = 3.37; //0.211996855; //0.3441*0.5; //0.21192 * 0.8;
206206
((UpdateController*)update_controller_pid_x)->pid_data.kdd = 0.0;
207207
((UpdateController*)update_controller_pid_x)->pid_data.anti_windup = 0;
208208
((UpdateController*)update_controller_pid_x)->pid_data.en_pv_derivation = 1;
209209
((UpdateController*)update_controller_pid_x)->pid_data.dt = (float)1.0/120.0;
210210
((UpdateController*)update_controller_pid_x)->pid_data.id = block_id::PID_X;
211211

212-
((UpdateController*)update_controller_pid_y)->pid_data.kp = 0.592289033;// 0.6673*0.75;// 0.6714;// 0.51639 * 0.8;
212+
((UpdateController*)update_controller_pid_y)->pid_data.kp = 9.5; //0.592289033;// 0.6673*0.75;// 0.6714;// 0.51639 * 0.8;
213213
((UpdateController*)update_controller_pid_y)->pid_data.ki = 0.0;
214-
((UpdateController*)update_controller_pid_y)->pid_data.kd = 0.215215824;// 0.2583*0.75; //-0.2440;// * 0.8;
214+
((UpdateController*)update_controller_pid_y)->pid_data.kd = 3.45; //0.215215824;// 0.2583*0.75; //-0.2440;// * 0.8;
215215
((UpdateController*)update_controller_pid_y)->pid_data.kdd = 0.0;
216216
((UpdateController*)update_controller_pid_y)->pid_data.anti_windup = 0;
217217
((UpdateController*)update_controller_pid_y)->pid_data.en_pv_derivation = 1;
218218
((UpdateController*)update_controller_pid_y)->pid_data.dt = (float)1.0/120.0;
219219
((UpdateController*)update_controller_pid_y)->pid_data.id = block_id::PID_Y;
220220

221-
((UpdateController*)update_controller_pid_z)->pid_data.kp = 0.613969957; // 1.2414*0.75;
221+
((UpdateController*)update_controller_pid_z)->pid_data.kp = 21.5; //0.613969957; // 1.2414*0.75;
222222
((UpdateController*)update_controller_pid_z)->pid_data.ki = 0.0;
223-
((UpdateController*)update_controller_pid_z)->pid_data.kd = 0.214920534; // 0.3316*0.75;
223+
((UpdateController*)update_controller_pid_z)->pid_data.kd = 6.8; //0.214920534; // 0.3316*0.75;
224224
((UpdateController*)update_controller_pid_z)->pid_data.kdd = 0.0;
225225
((UpdateController*)update_controller_pid_z)->pid_data.anti_windup = 0;
226226
((UpdateController*)update_controller_pid_z)->pid_data.en_pv_derivation = 1;
@@ -264,27 +264,27 @@ int main(int argc, char** argv) {
264264
((UpdateController*)update_controller_pid_yaw_rate)->pid_data.id = block_id::PID_YAW_RATE;
265265

266266
#ifdef SLAM_TEST
267-
((UpdateController*)update_pid_slam_x)->pid_data.kp = 0.311; //0.51639 * 0.8;
267+
((UpdateController*)update_pid_slam_x)->pid_data.kp = 3.35; //0.311; //0.51639 * 0.8;
268268
((UpdateController*)update_pid_slam_x)->pid_data.ki = 0.0;
269-
((UpdateController*)update_pid_slam_x)->pid_data.kd = 0.151; //0.21192 * 0.8;
269+
((UpdateController*)update_pid_slam_x)->pid_data.kd = 1.63; //0.151; //0.21192 * 0.8;
270270
((UpdateController*)update_pid_slam_x)->pid_data.kdd = 0.0;
271271
((UpdateController*)update_pid_slam_x)->pid_data.anti_windup = 0;
272272
((UpdateController*)update_pid_slam_x)->pid_data.en_pv_derivation = 1;
273273
((UpdateController*)update_pid_slam_x)->pid_data.dt = (float)1.0/120.0;
274274
((UpdateController*)update_pid_slam_x)->pid_data.id = block_id::PID_X;
275275

276-
((UpdateController*)update_pid_slam_y)->pid_data.kp = 0.332;// 0.6714;// 0.51639 * 0.8;
276+
((UpdateController*)update_pid_slam_y)->pid_data.kp = 4.28; //0.332;// 0.6714;// 0.51639 * 0.8;
277277
((UpdateController*)update_pid_slam_y)->pid_data.ki = 0.0;
278-
((UpdateController*)update_pid_slam_y)->pid_data.kd = 0.161; //-0.2440;// * 0.8;
278+
((UpdateController*)update_pid_slam_y)->pid_data.kd = 2.086; //0.161; //-0.2440;// * 0.8;
279279
((UpdateController*)update_pid_slam_y)->pid_data.kdd = 0.0;
280280
((UpdateController*)update_pid_slam_y)->pid_data.anti_windup = 0;
281281
((UpdateController*)update_pid_slam_y)->pid_data.en_pv_derivation = 1;
282282
((UpdateController*)update_pid_slam_y)->pid_data.dt = (float)1.0/120.0;
283283
((UpdateController*)update_pid_slam_y)->pid_data.id = block_id::PID_Y;
284284

285-
((UpdateController*)update_pid_slam_z)->pid_data.kp = 0.454*0.9;
285+
((UpdateController*)update_pid_slam_z)->pid_data.kp = 13.8; //0.454*0.9;
286286
((UpdateController*)update_pid_slam_z)->pid_data.ki = 0.0;
287-
((UpdateController*)update_pid_slam_z)->pid_data.kd = 0.197*0.9;
287+
((UpdateController*)update_pid_slam_z)->pid_data.kd = 6.0; //0.197*0.9;
288288
((UpdateController*)update_pid_slam_z)->pid_data.kdd = 0.0;
289289
((UpdateController*)update_pid_slam_z)->pid_data.anti_windup = 0;
290290
((UpdateController*)update_pid_slam_z)->pid_data.en_pv_derivation = 1;

0 commit comments

Comments
 (0)