From 6b9498906cf94290904a269cbf3b48e17c495b07 Mon Sep 17 00:00:00 2001 From: AhmedHumais Date: Mon, 4 Apr 2022 12:15:43 +0400 Subject: [PATCH] updated testing node --- example_node/test_node.cpp | 24 ++++++++++++------------ 1 file changed, 12 insertions(+), 12 deletions(-) diff --git a/example_node/test_node.cpp b/example_node/test_node.cpp index 9b0d729..58b31b3 100644 --- a/example_node/test_node.cpp +++ b/example_node/test_node.cpp @@ -26,12 +26,12 @@ #include "HEAR_ROS_BRIDGE/ROSUnit_ControlOutputSubscriber.hpp" -const float TAKE_OFF_HEIGHT = 1.25; +const float TAKE_OFF_HEIGHT = 1.0; const float LAND_HEIGHT = -0.01; //#define STORE_KF_BIAS //#define TRAJ_TEST -#define SLAM_TEST +//#define SLAM_TEST //#define AUTO_TEST #define TESTING //#define BIG_HEXA @@ -219,45 +219,45 @@ int main(int argc, char** argv) { //*************Setting Flight Elements************* #ifdef SMALL_HEXA - ((UpdateController*)update_controller_pid_x)->pid_data.kp = 14.33366959*0.9; //0.583430204; //0.8786*0.5; //0.51639 * 0.8; + ((UpdateController*)update_controller_pid_x)->pid_data.kp = 12.67*0.5; //14.53157937; //0.583430204; //0.8786*0.5; //0.51639 * 0.8; ((UpdateController*)update_controller_pid_x)->pid_data.ki = 0.0; - ((UpdateController*)update_controller_pid_x)->pid_data.kd = 5.208322871*0.9; //0.211996855; //0.3441*0.5; //0.21192 * 0.8; + ((UpdateController*)update_controller_pid_x)->pid_data.kd = 5.69*0.5; //4.783743612; //0.211996855; //0.3441*0.5; //0.21192 * 0.8; ((UpdateController*)update_controller_pid_x)->pid_data.kdd = 0.0; ((UpdateController*)update_controller_pid_x)->pid_data.anti_windup = 0; ((UpdateController*)update_controller_pid_x)->pid_data.en_pv_derivation = 1; ((UpdateController*)update_controller_pid_x)->pid_data.dt = (float)1.0/120.0; ((UpdateController*)update_controller_pid_x)->pid_data.id = block_id::PID_X; - ((UpdateController*)update_controller_pid_y)->pid_data.kp = 15.00427998*0.9; //0.592289033;// 0.6673*0.75;// 0.6714;// 0.51639 * 0.8; + ((UpdateController*)update_controller_pid_y)->pid_data.kp = 12.3*0.5; // 14.96570691; //0.592289033;// 0.6673*0.75;// 0.6714;// 0.51639 * 0.8; ((UpdateController*)update_controller_pid_y)->pid_data.ki = 0.0; - ((UpdateController*)update_controller_pid_y)->pid_data.kd = 5.451997767*0.9; //0.215215824;// 0.2583*0.75; //-0.2440;// * 0.8; + ((UpdateController*)update_controller_pid_y)->pid_data.kd = 5.52*0.5; //4.926656836; //0.215215824;// 0.2583*0.75; //-0.2440;// * 0.8; ((UpdateController*)update_controller_pid_y)->pid_data.kdd = 0.0; ((UpdateController*)update_controller_pid_y)->pid_data.anti_windup = 0; ((UpdateController*)update_controller_pid_y)->pid_data.en_pv_derivation = 1; ((UpdateController*)update_controller_pid_y)->pid_data.dt = (float)1.0/120.0; ((UpdateController*)update_controller_pid_y)->pid_data.id = block_id::PID_Y; - ((UpdateController*)update_controller_pid_z)->pid_data.kp = 27.62993448; //0.613969957; // 1.2414*0.75; + ((UpdateController*)update_controller_pid_z)->pid_data.kp = 24.809892; //32.00005633*0.8; //0.613969957; // 1.2414*0.75; ((UpdateController*)update_controller_pid_z)->pid_data.ki = 0.0; - ((UpdateController*)update_controller_pid_z)->pid_data.kd = 8.13692084; //0.214920534; // 0.3316*0.75; + ((UpdateController*)update_controller_pid_z)->pid_data.kd = 7.4476; //8.328107017*0.8; //0.214920534; // 0.3316*0.75; ((UpdateController*)update_controller_pid_z)->pid_data.kdd = 0.0; ((UpdateController*)update_controller_pid_z)->pid_data.anti_windup = 0; ((UpdateController*)update_controller_pid_z)->pid_data.en_pv_derivation = 1; ((UpdateController*)update_controller_pid_z)->pid_data.dt = (float)1.0/120.0; ((UpdateController*)update_controller_pid_z)->pid_data.id = block_id::PID_Z; - ((UpdateController*)update_controller_pid_roll)->pid_data.kp = 0.423827421; //0.225*0.8; //0.172195; //0.3302; //0.286708; //0.225 * 0.8; + ((UpdateController*)update_controller_pid_roll)->pid_data.kp = 0.3336; // 0.423827421; //0.225*0.8; //0.172195; //0.3302; //0.286708; //0.225 * 0.8; ((UpdateController*)update_controller_pid_roll)->pid_data.ki = 0.0; - ((UpdateController*)update_controller_pid_roll)->pid_data.kd = 0.063652685; //0.04*0.8; //0.042464; //0.0931; //0.056559; //0.04 * 0.8; + ((UpdateController*)update_controller_pid_roll)->pid_data.kd = 0.0538; //0.063652685; //0.04*0.8; //0.042464; //0.0931; //0.056559; //0.04 * 0.8; ((UpdateController*)update_controller_pid_roll)->pid_data.kdd = 0.0; ((UpdateController*)update_controller_pid_roll)->pid_data.anti_windup = 0; ((UpdateController*)update_controller_pid_roll)->pid_data.en_pv_derivation = 1; ((UpdateController*)update_controller_pid_roll)->pid_data.dt = 1.f/200.f; ((UpdateController*)update_controller_pid_roll)->pid_data.id = block_id::PID_ROLL; - ((UpdateController*)update_controller_pid_pitch)->pid_data.kp = 0.434061228; //0.225*0.8;// 0.3360; //0.2811;//0.275252; //0.225 * 0.8; + ((UpdateController*)update_controller_pid_pitch)->pid_data.kp = 0.3561; // 0.434061228; //0.225*0.8;// 0.3360; //0.2811;//0.275252; //0.225 * 0.8; ((UpdateController*)update_controller_pid_pitch)->pid_data.ki = 0.0; - ((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; + ((UpdateController*)update_controller_pid_pitch)->pid_data.kd = 0.0574; // 0.065189653; //0.04*0.8;//0.0684; //0.053100; //0.0868;// 0.051266; //0.04 * 0.8; ((UpdateController*)update_controller_pid_pitch)->pid_data.kdd = 0.0; ((UpdateController*)update_controller_pid_pitch)->pid_data.anti_windup = 0; ((UpdateController*)update_controller_pid_pitch)->pid_data.en_pv_derivation = 1;