Skip to content

Commit 4d0eeba

Browse files
AhmedHumaisAhmedHumais
committed
updated
1 parent 979ef90 commit 4d0eeba

File tree

2 files changed

+47
-8
lines changed

2 files changed

+47
-8
lines changed

‎example_node/SLAM_tuning_node.cpp‎

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

29-
#define MRFT_POS_X
30-
//#define MRFT_POS_Y
29+
//#define MRFT_POS_X
30+
#define MRFT_POS_Y
3131
//#define MRFT_POS_Z
3232
////////////////////////////////////// increase relay amplitude in Y
3333
#define MRFT_SLAM
@@ -38,7 +38,7 @@
3838
const float SLAM_FREQ = 90.0;
3939
const float KF_FREQ = 200.0;
4040
const float OPTI_FREQ = 90.0;
41-
const float TAKE_OFF_HEIGHT = 1.2;
41+
const float TAKE_OFF_HEIGHT = 1.3;
4242
const float LAND_HEIGHT = -0.01;
4343

4444

@@ -105,6 +105,10 @@ int main(int argc, char** argv) {
105105
"kf_switch");
106106
#endif
107107

108+
ROSUnit* ros_rec_hover_thrust = ROSUnit_Factory_main.CreateROSUnit(ROSUnit_tx_rx_type::Client,
109+
ROSUnit_msg_type::ROSUnit_Bool,
110+
"record_hover_thrust");
111+
108112
ROSUnit* ros_flight_command = ROSUnit_Factory_main.CreateROSUnit(ROSUnit_tx_rx_type::Server,
109113
ROSUnit_msg_type::ROSUnit_Empty,
110114
"flight_command");//TODO: Change to user_command
@@ -118,6 +122,11 @@ int main(int argc, char** argv) {
118122
ROSUnit_msg_type::ROSUnit_Empty,
119123
"send_curr_pos_slam");
120124

125+
ROSUnit* ros_adjust_act_gain = ROSUnit_Factory_main.CreateROSUnit(ROSUnit_tx_rx_type::Client,
126+
ROSUnit_msg_type::ROSUnit_Bool,
127+
"use_adjusted_act_gain");
128+
129+
121130
// //*****************Flight Elements*************
122131

123132
MissionElement* update_controller_pid_x = new UpdateController();
@@ -181,6 +190,10 @@ int main(int argc, char** argv) {
181190
MissionElement* send_curr_slam_ref = new Arm();
182191
MissionElement* send_curr_opti_ref = new Arm();
183192

193+
MissionElement* rec_hover_thrust = new Arm();
194+
MissionElement* adjust_act_gain = new Arm();
195+
MissionElement* original_act_gain = new Disarm();
196+
184197
//******************Connections***************
185198
update_controller_pid_x->getPorts()[(int)UpdateController::ports_id::OP_0]->connect(ros_updt_ctr->getPorts()[(int)ROSUnit_UpdateControllerClnt::ports_id::IP_0_PID]);
186199
update_controller_pid_y->getPorts()[(int)UpdateController::ports_id::OP_0]->connect(ros_updt_ctr->getPorts()[(int)ROSUnit_UpdateControllerClnt::ports_id::IP_0_PID]);
@@ -242,6 +255,10 @@ int main(int argc, char** argv) {
242255
change_to_opti_rate->getPorts()[(int)SwitchTrigger::ports_id::OP_0]->connect(ros_outer_rate_client->getPorts()[(int)ROSUnit_SetFloatClnt::ports_id::IP_0]);
243256
#endif
244257

258+
rec_hover_thrust->getPorts()[(int)Arm::ports_id::OP_0]->connect(ros_rec_hover_thrust->getPorts()[(int)ROSUnit_SetBoolClnt::ports_id::IP_0]);
259+
adjust_act_gain->getPorts()[(int)Arm::ports_id::OP_0]->connect(ros_adjust_act_gain->getPorts()[(int)ROSUnit_SetBoolClnt::ports_id::IP_0]);
260+
original_act_gain->getPorts()[(int)Disarm::ports_id::OP_0]->connect(ros_adjust_act_gain->getPorts()[(int)ROSUnit_SetBoolClnt::ports_id::IP_0]);
261+
245262
//*************Setting Flight Elements*************
246263

247264
((UpdateController*)update_controller_pid_x)->pid_data.kp = 9.28; //0.583430204; //0.8786*0.5; //0.51639 * 0.8;
@@ -319,10 +336,10 @@ int main(int argc, char** argv) {
319336

320337
#ifdef MRFT_POS_Y
321338
((UpdateController*)update_controller_mrft_y)->mrft_data.beta = -0.735;
322-
((UpdateController*)update_controller_mrft_y)->mrft_data.relay_amp = 1.5;
339+
((UpdateController*)update_controller_mrft_y)->mrft_data.relay_amp = 2.0;
323340
((UpdateController*)update_controller_mrft_y)->mrft_data.bias = 0.0;
324341
((UpdateController*)update_controller_mrft_y)->mrft_data.no_switch_delay_in_ms = 100;
325-
((UpdateController*)update_controller_mrft_y)->mrft_data.num_of_peak_conf_samples = 20;
342+
((UpdateController*)update_controller_mrft_y)->mrft_data.num_of_peak_conf_samples = 24;
326343
((UpdateController*)update_controller_mrft_y)->mrft_data.id = block_id::MRFT_Y;
327344
#endif
328345

@@ -423,11 +440,18 @@ int main(int argc, char** argv) {
423440
mrft_pipeline.addElement((MissionElement*)&wait_100ms);
424441
mrft_pipeline.addElement((MissionElement*)set_vo_offset);
425442
mrft_pipeline.addElement((MissionElement*)arm_motors);
426-
443+
427444
mrft_pipeline.addElement((MissionElement*)user_command);
428445
mrft_pipeline.addElement((MissionElement*)arm_motors);
429446
mrft_pipeline.addElement((MissionElement*)take_off);
430447

448+
mrft_pipeline.addElement((MissionElement*)user_command);
449+
mrft_pipeline.addElement((MissionElement*)rec_hover_thrust);
450+
mrft_pipeline.addElement((MissionElement*)&wait_100ms);
451+
mrft_pipeline.addElement((MissionElement*)adjust_act_gain);
452+
mrft_pipeline.addElement((MissionElement*)&wait_1s);
453+
454+
431455
// mrft_pipeline.addElement((MissionElement*)&wait_5s);
432456

433457
#ifdef MRFT_POS_X

‎example_node/test_node.cpp‎

Lines changed: 17 additions & 2 deletions
Original file line numberDiff line numberDiff line change
@@ -30,8 +30,8 @@ const float TAKE_OFF_HEIGHT = 1.0;
3030
const float LAND_HEIGHT = -0.01;
3131

3232
//#define STORE_KF_BIAS
33-
//#define TRAJ_TEST
34-
#define SLAM_TEST
33+
#define TRAJ_TEST
34+
//#define SLAM_TEST
3535
//#define AUTO_TEST
3636
#define TESTING
3737
//#define BIG_HEXA
@@ -112,6 +112,11 @@ int main(int argc, char** argv) {
112112
ROSUnit_msg_type::ROSUnit_Bool,
113113
"kf_freeze_bias");
114114
#endif
115+
116+
ROSUnit* ros_adjust_act_gain = ROSUnit_Factory_main.CreateROSUnit(ROSUnit_tx_rx_type::Client,
117+
ROSUnit_msg_type::ROSUnit_Bool,
118+
"use_adjusted_act_gain");
119+
115120

116121
// //*****************Flight Elements*************
117122

@@ -153,6 +158,9 @@ int main(int argc, char** argv) {
153158
MissionElement* send_current_pos_ref_opti = new Arm();
154159
MissionElement* send_current_pos_ref_slam = new Arm();
155160

161+
MissionElement* adjust_act_gain = new Arm();
162+
MissionElement* original_act_gain = new Disarm();
163+
156164
#ifdef STORE_KF_BIAS
157165
MissionElement* freeze_kf_bias = new Arm();
158166
#endif
@@ -198,6 +206,9 @@ int main(int argc, char** argv) {
198206
#endif
199207
//absolute_zero_Z_relative_waypoint->connect(ros_set_path_clnt);
200208

209+
adjust_act_gain->getPorts()[(int)Arm::ports_id::OP_0]->connect(ros_adjust_act_gain->getPorts()[(int)ROSUnit_SetBoolClnt::ports_id::IP_0]);
210+
original_act_gain->getPorts()[(int)Disarm::ports_id::OP_0]->connect(ros_adjust_act_gain->getPorts()[(int)ROSUnit_SetBoolClnt::ports_id::IP_0]);
211+
201212
//*************Setting Flight Elements*************
202213
#ifdef SMALL_HEXA
203214
((UpdateController*)update_controller_pid_x)->pid_data.kp = 9.28; //0.583430204; //0.8786*0.5; //0.51639 * 0.8;
@@ -428,6 +439,10 @@ int main(int argc, char** argv) {
428439

429440
#ifdef TRAJ_TEST
430441
testing_pipeline.addElement((MissionElement*)rec_hover_thrust);
442+
testing_pipeline.addElement((MissionElement*)&wait_100ms);
443+
testing_pipeline.addElement((MissionElement*)adjust_act_gain);
444+
testing_pipeline.addElement((MissionElement*)&wait_1s);
445+
431446
testing_pipeline.addElement((MissionElement*)start_trajectory);
432447
testing_pipeline.addElement((MissionElement*)user_command);
433448
#endif

0 commit comments

Comments
 (0)