Skip to content

Commit 8491d15

Browse files
AhmedHumaisAhmedHumais
committed
added mrft for inner loops
1 parent 46a0a3a commit 8491d15

File tree

2 files changed

+92
-26
lines changed

2 files changed

+92
-26
lines changed

‎example_node/SLAM_tuning_node.cpp‎

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

2929
//#define MRFT_POS_X
30-
#define MRFT_POS_Y
30+
//#define MRFT_POS_Y
3131
//#define MRFT_POS_Z
32-
////////////////////////////////////// increase relay amplitude in Y
32+
33+
#define MRFT_ATT_ROLL
34+
#define MRFT_ATT_PITCH
35+
3336
#define MRFT_SLAM
3437
//#define KF
3538

@@ -93,6 +96,18 @@ int main(int argc, char** argv) {
9396
"mrft_switch_z");
9497
#endif
9598

99+
#ifdef MRFT_ATT_ROLL
100+
ROSUnit* ros_mrft_trigger_roll = ROSUnit_Factory_main.CreateROSUnit(ROSUnit_tx_rx_type::Client,
101+
ROSUnit_msg_type::ROSUnit_Float,
102+
"mrft_switch_roll");
103+
#endif
104+
105+
#ifdef MRFT_ATT_PITCH
106+
ROSUnit* ros_mrft_trigger_pitch = ROSUnit_Factory_main.CreateROSUnit(ROSUnit_tx_rx_type::Client,
107+
ROSUnit_msg_type::ROSUnit_Float,
108+
"mrft_switch_pitch");
109+
#endif
110+
96111
#ifdef MRFT_SLAM
97112
ROSUnit* ros_slam_sw_trig = ROSUnit_Factory_main.CreateROSUnit(ROSUnit_tx_rx_type::Client,
98113
ROSUnit_msg_type::ROSUnit_Bool,
@@ -155,6 +170,18 @@ int main(int argc, char** argv) {
155170
MissionElement* mrft_switch_off_z=new SwitchTrigger(0);
156171
#endif
157172

173+
#ifdef MRFT_ATT_ROLL
174+
MissionElement* update_controller_mrft_roll = new UpdateController();
175+
MissionElement* mrft_switch_on_roll = new SwitchTrigger(1);
176+
MissionElement* mrft_switch_off_roll = new SwitchTrigger(0);
177+
#endif
178+
179+
#ifdef MRFT_ATT_PITCH
180+
MissionElement* update_controller_mrft_pitch = new UpdateController();
181+
MissionElement* mrft_switch_on_pitch = new SwitchTrigger(1);
182+
MissionElement* mrft_switch_off_pitch = new SwitchTrigger(0);
183+
#endif
184+
158185
#ifdef MRFT_SLAM
159186
MissionElement* update_controller_pid_slam_x = new UpdateController();
160187
MissionElement* update_controller_pid_slam_y = new UpdateController();
@@ -221,6 +248,18 @@ int main(int argc, char** argv) {
221248
mrft_switch_off_z->getPorts()[(int)SwitchTrigger::ports_id::OP_0]->connect(ros_mrft_trigger_z->getPorts()[(int)ROSUnit_SetFloatClnt::ports_id::IP_0]);
222249
#endif
223250

251+
#ifdef MRFT_ATT_ROLL
252+
update_controller_mrft_roll->getPorts()[(int)UpdateController::ports_id::OP_0]->connect(ros_updt_ctr->getPorts()[(int)ROSUnit_UpdateControllerClnt::ports_id::IP_1_MRFT]);
253+
mrft_switch_on_roll->getPorts()[(int)SwitchTrigger::ports_id::OP_0]->connect(ros_mrft_trigger_roll->getPorts()[(int)ROSUnit_SetFloatClnt::ports_id::IP_0]);
254+
mrft_switch_off_roll->getPorts()[(int)SwitchTrigger::ports_id::OP_0]->connect(ros_mrft_trigger_roll->getPorts()[(int)ROSUnit_SetFloatClnt::ports_id::IP_0]);
255+
#endif
256+
257+
#ifdef MRFT_ATT_PITCH
258+
update_controller_mrft_pitch->getPorts()[(int)UpdateController::ports_id::OP_0]->connect(ros_updt_ctr->getPorts()[(int)ROSUnit_UpdateControllerClnt::ports_id::IP_1_MRFT]);
259+
mrft_switch_on_pitch->getPorts()[(int)SwitchTrigger::ports_id::OP_0]->connect(ros_mrft_trigger_pitch->getPorts()[(int)ROSUnit_SetFloatClnt::ports_id::IP_0]);
260+
mrft_switch_off_pitch->getPorts()[(int)SwitchTrigger::ports_id::OP_0]->connect(ros_mrft_trigger_pitch->getPorts()[(int)ROSUnit_SetFloatClnt::ports_id::IP_0]);
261+
#endif
262+
224263
#ifdef MRFT_SLAM
225264
update_controller_pid_slam_x->getPorts()[(int)UpdateController::ports_id::OP_0]->connect(ros_updt_ctr->getPorts()[(int)ROSUnit_UpdateControllerClnt::ports_id::IP_0_PID]);
226265
update_controller_pid_slam_y->getPorts()[(int)UpdateController::ports_id::OP_0]->connect(ros_updt_ctr->getPorts()[(int)ROSUnit_UpdateControllerClnt::ports_id::IP_0_PID]);
@@ -351,6 +390,20 @@ int main(int argc, char** argv) {
351390
((UpdateController*)update_controller_mrft_z)->mrft_data.id = block_id::MRFT_Z;
352391
#endif
353392

393+
#ifdef MRFT_ATT_ROLL
394+
((UpdateController*)update_controller_mrft_roll)->mrft_data.beta = -0.73;
395+
((UpdateController*)update_controller_mrft_roll)->mrft_data.relay_amp = 0.04;
396+
((UpdateController*)update_controller_mrft_roll)->mrft_data.bias = 0.0;
397+
((UpdateController*)update_controller_mrft_roll)->mrft_data.id = block_id::MRFT_ROLL;
398+
#endif
399+
400+
#ifdef MRFT_ATT_PITCH
401+
((UpdateController*)update_controller_mrft_pitch)->mrft_data.beta = -0.73;
402+
((UpdateController*)update_controller_mrft_pitch)->mrft_data.relay_amp = 0.04;
403+
((UpdateController*)update_controller_mrft_pitch)->mrft_data.bias = 0.0;
404+
((UpdateController*)update_controller_mrft_pitch)->mrft_data.id = block_id::MRFT_PITCH;
405+
#endif
406+
354407
#ifdef MRFT_SLAM
355408
((UpdateController*)update_controller_pid_slam_x)->pid_data.kp = 3.35;
356409
((UpdateController*)update_controller_pid_slam_x)->pid_data.ki = 0.0;
@@ -400,11 +453,8 @@ int main(int argc, char** argv) {
400453
mrft_pipeline.addElement((MissionElement*)update_controller_pid_y);
401454
mrft_pipeline.addElement((MissionElement*)update_controller_pid_z);
402455
mrft_pipeline.addElement((MissionElement*)update_controller_pid_roll);
403-
mrft_pipeline.addElement((MissionElement*)&wait_100ms);
404456
mrft_pipeline.addElement((MissionElement*)update_controller_pid_pitch);
405-
mrft_pipeline.addElement((MissionElement*)&wait_100ms);
406457
mrft_pipeline.addElement((MissionElement*)update_controller_pid_yaw);
407-
mrft_pipeline.addElement((MissionElement*)&wait_100ms);
408458
mrft_pipeline.addElement((MissionElement*)update_controller_pid_yaw_rate);
409459
mrft_pipeline.addElement((MissionElement*)&wait_100ms);
410460

@@ -420,16 +470,12 @@ int main(int argc, char** argv) {
420470
mrft_pipeline.addElement((MissionElement*)update_controller_mrft_z);
421471
#endif
422472

423-
#ifdef PID_X_SLAM
424-
mrft_pipeline.addElement((MissionElement*)update_controller_pid_slam_x);
473+
#ifdef MRFT_ATT_ROLL
474+
mrft_pipeline.addElement((MissionElement*)update_controller_mrft_roll);
425475
#endif
426476

427-
#ifdef PID_Y_SLAM
428-
mrft_pipeline.addElement((MissionElement*)update_controller_pid_slam_y);
429-
#endif
430-
431-
#ifdef PID_Z_SLAM
432-
mrft_pipeline.addElement((MissionElement*)update_controller_pid_slam_z);
477+
#ifdef MRFT_ATT_PITCH
478+
mrft_pipeline.addElement((MissionElement*)update_controller_mrft_pitch);
433479
#endif
434480

435481
mrft_pipeline.addElement((MissionElement*)set_height_offset); //TODO: (CHECK Desc) Set a constant height command/reference based on the current pos
@@ -560,6 +606,24 @@ int main(int argc, char** argv) {
560606
#endif
561607
#endif
562608

609+
#ifdef MRFT_ATT_ROLL
610+
mrft_pipeline.addElement((MissionElement*)user_command);
611+
mrft_pipeline.addElement((MissionElement*)mrft_switch_on_roll);
612+
613+
mrft_pipeline.addElement((MissionElement*)user_command);
614+
mrft_pipeline.addElement((MissionElement*)send_curr_opti_ref);
615+
mrft_pipeline.addElement((MissionElement*)mrft_switch_off_roll);
616+
#endif
617+
618+
#ifdef MRFT_ATT_PITCH
619+
mrft_pipeline.addElement((MissionElement*)user_command);
620+
mrft_pipeline.addElement((MissionElement*)mrft_switch_on_pitch);
621+
622+
mrft_pipeline.addElement((MissionElement*)user_command);
623+
mrft_pipeline.addElement((MissionElement*)send_curr_opti_ref);
624+
mrft_pipeline.addElement((MissionElement*)mrft_switch_off_pitch);
625+
#endif
626+
563627
mrft_pipeline.addElement((MissionElement*)user_command);
564628
// mrft_pipeline.addElement((MissionElement*)&wait_1s);
565629

‎example_node/test_node.cpp‎

Lines changed: 15 additions & 13 deletions
Original file line numberDiff line numberDiff line change
@@ -31,7 +31,7 @@ const float LAND_HEIGHT = -0.01;
3131

3232
//#define STORE_KF_BIAS
3333
#define TRAJ_TEST
34-
//#define SLAM_TEST
34+
#define SLAM_TEST
3535
//#define AUTO_TEST
3636
#define TESTING
3737
//#define BIG_HEXA
@@ -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 = 3.35; //0.311; //0.51639 * 0.8;
286+
((UpdateController*)update_pid_slam_x)->pid_data.kp = 7.64; //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 = 1.63; //0.151; //0.21192 * 0.8;
288+
((UpdateController*)update_pid_slam_x)->pid_data.kd = 4.086; //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 = 4.28; //0.332;// 0.6714;// 0.51639 * 0.8;
295+
((UpdateController*)update_pid_slam_y)->pid_data.kp = 8.695; //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 = 2.086; //0.161; //-0.2440;// * 0.8;
297+
((UpdateController*)update_pid_slam_y)->pid_data.kd = 4.65; //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.8; //0.454*0.9;
304+
((UpdateController*)update_pid_slam_z)->pid_data.kp = 13.73; //0.454*0.9;
305305
((UpdateController*)update_pid_slam_z)->pid_data.ki = 0.0;
306-
((UpdateController*)update_pid_slam_z)->pid_data.kd = 6.0; //0.197*0.9;
306+
((UpdateController*)update_pid_slam_z)->pid_data.kd = 4.8; //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;
@@ -436,6 +436,14 @@ int main(int argc, char** argv) {
436436
testing_pipeline.addElement((MissionElement*)user_command);
437437
#endif
438438

439+
testing_pipeline.addElement((MissionElement*)rec_hover_thrust);
440+
testing_pipeline.addElement((MissionElement*)&wait_100ms);
441+
testing_pipeline.addElement((MissionElement*)adjust_act_gain);
442+
testing_pipeline.addElement((MissionElement*)&wait_5s);
443+
testing_pipeline.addElement((MissionElement*)correct_biases);
444+
testing_pipeline.addElement((MissionElement*)&wait_1s);
445+
testing_pipeline.addElement((MissionElement*)user_command);
446+
439447
#ifdef SLAM_TEST
440448
testing_pipeline.addElement((MissionElement*)update_pid_slam_x);
441449
testing_pipeline.addElement((MissionElement*)update_pid_slam_y);
@@ -446,12 +454,6 @@ int main(int argc, char** argv) {
446454
#endif
447455

448456
#ifdef TRAJ_TEST
449-
testing_pipeline.addElement((MissionElement*)rec_hover_thrust);
450-
testing_pipeline.addElement((MissionElement*)&wait_100ms);
451-
testing_pipeline.addElement((MissionElement*)adjust_act_gain);
452-
testing_pipeline.addElement((MissionElement*)&wait_1s);
453-
testing_pipeline.addElement((MissionElement*)correct_biases);
454-
testing_pipeline.addElement((MissionElement*)&wait_1s);
455457

456458
testing_pipeline.addElement((MissionElement*)start_trajectory);
457459
testing_pipeline.addElement((MissionElement*)user_command);

0 commit comments

Comments
 (0)