diff --git a/CMakeLists.txt b/CMakeLists.txt index cc92dd3..9d44444 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -101,6 +101,7 @@ generate_messages( ## Generate dynamic reconfigure parameters in the 'cfg' folder generate_dynamic_reconfigure_options( cfg/controllerDyn.cfg + cfg/controllerGS1Dyn.cfg ) ################################### diff --git a/README.md b/README.md index 4ab3d75..84c40d9 100644 --- a/README.md +++ b/README.md @@ -36,3 +36,43 @@ doi: 10.1109/ICUAS.2017.7991516 month={June} } ``` + +## Scenario de test +* Gazebo test + - Code + + Simulink Code Generation: 200Hz => Ts = 1/200 + + Node: ros::Rate r(200); + - Test + + Launch + - roslaunch rotors_gazebo mav.launch + - roslaunch gsft_control tuning_nominal_Gazebo.launch + + Gazebo: click run + - first odometry mesage + - rqt dynamic reconfigure "gazebo": max_update_rate from 100Hz to 200 + - dynamic reconfigure "firefly/controller": mode, gain, LOE, ... => enable_take_off +* Compare GS and Baseline + - Code + + Simulink Code Generation: 200Hz => Ts = 1/200 + + Node: ros::Rate r(200); + + Cfg + - controller: enable_take_off = True + - + - Test + + Launch + - roslaunch rotors_gazebo mav.launch + - roslaunch gsft_control tuning_nominal_Gazebo.launch + + Gazebo: click run => first odometry mesage and rqt dynamic reconfigure "gazebo" + + rqt + - dynamic reconfigure "gazebo": max_update_rate = 200 + - dynamic reconfigure "firefly/controller": mode, gain, LOE, ... then click enable_take_off +* Experimental test + - Code + + Simulink Code Generation: 1KHz => Ts = 1/1000 + + Node: ros::Rate r(1000); + + git status, git add, git commit and git push + - Vicon server: firefly object, 200Hz + - Ground Station: roslaunch vicon, rqt, rosbag record + - Asctec Firefly + + git pull and build + + roslaunch: fcu, msf, controller + + check frequencies: Vicon 200HZ, other (imu, msf, controller) 1KHz diff --git a/cfg/controllerDyn.cfg b/cfg/controllerDyn.cfg index b23ce91..e23920b 100755 --- a/cfg/controllerDyn.cfg +++ b/cfg/controllerDyn.cfg @@ -1,5 +1,8 @@ #! /usr/bin/env python +# cd ~/cfg/ +# chmod +x your_file.cfg + PACKAGE='gsft_control' import roslib roslib.load_manifest(PACKAGE) @@ -48,12 +51,12 @@ group5.add("LOE_3", double_t, 0.0, "lost of control group5.add("LOE_4", double_t, 0.0, "lost of control 4", 0.0, 0.0, 1.0) group5.add("LOE_5", double_t, 0.0, "lost of control 5", 0.0, 0.0, 1.0) group5.add("LOE_6", double_t, 0.0, "lost of control 6", 0.0, 0.0, 1.0) -group5.add("LOE_t1", double_t, 0.0, "occurs time 1", 0.0, 0.0, 100.0) -group5.add("LOE_t2", double_t, 0.0, "occurs time 2", 0.0, 0.0, 100.0) -group5.add("LOE_t3", double_t, 0.0, "occurs time 3", 0.0, 0.0, 100.0) -group5.add("LOE_t4", double_t, 0.0, "occurs time 4", 0.0, 0.0, 100.0) -group5.add("LOE_t5", double_t, 0.0, "occurs time 5", 0.0, 0.0, 100.0) -group5.add("LOE_t6", double_t, 0.0, "occurs time 6", 0.0, 0.0, 100.0) +group5.add("LOE_t1", double_t, 0.0, "fault time 1", 0.0, 0.0, 100.0) +group5.add("LOE_t2", double_t, 0.0, "fault time 2", 0.0, 0.0, 100.0) +group5.add("LOE_t3", double_t, 0.0, "fault time 3", 0.0, 0.0, 100.0) +group5.add("LOE_t4", double_t, 0.0, "fault time 4", 0.0, 0.0, 100.0) +group5.add("LOE_t5", double_t, 0.0, "fault time 5", 0.0, 0.0, 100.0) +group5.add("LOE_t6", double_t, 0.0, "fault time 6", 0.0, 0.0, 100.0) #group3.add("kT", double_t, NEW_PARAMETERS["value"], "thrust constant", 8.54858e-6, 1e-7, 1e-5) #group3.add("factor", double_t, NEW_PARAMETERS["value"], "scaling factor", 0.035, 0.0, 0.05) diff --git a/cfg/controllerGS1Dyn.cfg b/cfg/controllerGS1Dyn.cfg new file mode 100755 index 0000000..4545872 --- /dev/null +++ b/cfg/controllerGS1Dyn.cfg @@ -0,0 +1,109 @@ +#! /usr/bin/env python + +# cd ~/cfg/ +# chmod +x your_file.cfg + +PACKAGE='gsft_control' +import roslib +roslib.load_manifest(PACKAGE) + +from dynamic_reconfigure.parameter_generator_catkin import * + +gen = ParameterGenerator() + +ENABLE_CTRL = gen.const("ENABLE_CTRL", int_t, 0x00000001, "enable_ctrl") +NEW_REFERENCES = gen.const("NEW_REFERENCES", int_t, 0x00000002, "new_references") +#NEW_PARAMETERS = gen.const("NEW_PARAMETERS", int_t, 0x00000004, "drone_parameter") +CONTROLLER_GAIN = gen.const("CONTROLLER_GAIN", int_t, 0x00000008, "controller_gain") +#LOE = gen.const("LOE", int_t, 0x01000000, "lost_of_control_effectiveness") + +group1 = gen.add_group("group_control") +group2 = gen.add_group("group_waypoint_references") +#group3 = gen.add_group("group3_drone_parameters") +group4 = gen.add_group("group_controller_gains") +group5 = gen.add_group("group_LOE") + +# Name Type Reconfiguration level Description Default Min Max +gen.add("RESET", bool_t, 0, "reset parameters, gains", False) + +test_mode_enum = gen.enum([ gen.const("TEST_MANUAL", int_t, 0, "Manual test"), + gen.const("TEST_WAYPOINT", int_t, 1, "Waypoint test"), + gen.const("TEST_TRACKING", int_t, 2, "Circular tracking test")], + "An enum to chose test mode") +gen.add("test_mode", int_t, 0, "test mode", 0, 0, 2, edit_method=test_mode_enum) + +LOE_est_enum = gen.enum([ gen.const("NO", int_t, 0, "Nominal test"), + gen.const("LOE_FICTIVE", int_t, 1, "LOE fictive"), + gen.const("LOE_CALCUL", int_t, 2, "LOE calcul"), + gen.const("LOE_FDD", int_t, 3, "LOE fdd")], + "An enum to chose LOE estimation mode") +gen.add("LOE_estimation", int_t, 0, "LOE estimation", 0, 0, 3, edit_method=LOE_est_enum) + +group1.add("enable_take_off", bool_t, ENABLE_CTRL["value"], "enable take off", False) # ENABLE_CTRL["value"] = 0x00000001 +group1.add("enable_landing", bool_t, ENABLE_CTRL["value"], "enable landing", False) +group1.add("send_waypoint", bool_t, ENABLE_CTRL["value"], "send new waypoint", False) +group1.add("new_controller_gains", bool_t, ENABLE_CTRL["value"], "change controller gains", False) +#group1.add("new_drone_parameters", bool_t, ENABLE_CTRL["value"], "change drone parameters", False) # to do later +#group1.add("enable_yaw", bool_t, ENABLE_CTRL["value"], "enable yaw-control", True) # to do later +#group1.add("enable_gohome", bool_t, ENABLE_CTRL["value"], "enable go home", False) # to do later + +group2.add("ref_x", double_t, NEW_REFERENCES["value"], "X position reference", 0.0, -2.0, 2.0) +group2.add("ref_y", double_t, NEW_REFERENCES["value"], "Y position reference", 0.0, -2.0, 2.0) +group2.add("ref_z", double_t, NEW_REFERENCES["value"], "Z position reference", 0.5, 0.0, 1.5) +group2.add("ref_yaw_deg", double_t, NEW_REFERENCES["value"], "yaw angle reference", 0.0, -180.0, 180.0) + +group5.add("LOE_1", double_t, 0.0, "lost of control 1", 0.0, 0.0, 1.0) +group5.add("LOE_2", double_t, 0.0, "lost of control 2", 0.0, 0.0, 1.0) +group5.add("LOE_3", double_t, 0.0, "lost of control 3", 0.0, 0.0, 1.0) +group5.add("LOE_4", double_t, 0.0, "lost of control 4", 0.0, 0.0, 1.0) +group5.add("LOE_5", double_t, 0.0, "lost of control 5", 0.0, 0.0, 1.0) +group5.add("LOE_6", double_t, 0.0, "lost of control 6", 0.0, 0.0, 1.0) +group5.add("LOE_t1", double_t, 0.0, "fault time 1", 0.0, 0.0, 100.0) +group5.add("LOE_t2", double_t, 0.0, "fault time 2", 0.0, 0.0, 100.0) +group5.add("LOE_t3", double_t, 0.0, "fault time 3", 0.0, 0.0, 100.0) +group5.add("LOE_t4", double_t, 0.0, "fault time 4", 0.0, 0.0, 100.0) +group5.add("LOE_t5", double_t, 0.0, "fault time 5", 0.0, 0.0, 100.0) +group5.add("LOE_t6", double_t, 0.0, "fault time 6", 0.0, 0.0, 100.0) + +#group3.add("kT", double_t, NEW_PARAMETERS["value"], "thrust constant", 8.54858e-6, 1e-7, 1e-5) +#group3.add("factor", double_t, NEW_PARAMETERS["value"], "scaling factor", 0.035, 0.0, 0.05) + +group4.add("kz", double_t, CONTROLLER_GAIN["value"],"k-z", 14.7226, 0.0, 23.2925) +group4.add("kvz", double_t, CONTROLLER_GAIN["value"],"k-zd", 6.8078, 0.0, 8.47) +group4.add("kiz", double_t, CONTROLLER_GAIN["value"],"k-iz", 12.2474, 0.0, 25.025) + +group4.add("kx", double_t, CONTROLLER_GAIN["value"],"k-x", 1.2426, 0.0, 2.0) +group4.add("kvx", double_t, CONTROLLER_GAIN["value"],"k-xd", 0.7085, 0.0, 2.0) +group4.add("kix", double_t, CONTROLLER_GAIN["value"],"k-ix", 0.9913, 0.0, 2.0) +group4.add("ky", double_t, CONTROLLER_GAIN["value"],"k-y", -0.9479, -2.0, 0.0) +group4.add("kvy", double_t, CONTROLLER_GAIN["value"],"k-yd", -0.5505, -2.0, 0.0) +group4.add("kiy", double_t, CONTROLLER_GAIN["value"],"k-iy", -0.7657, -2.0, 0.0) + +#group4.add("kx", double_t, CONTROLLER_GAIN["value"],"k-x", 1.6651, 0.0, 2.0) +#group4.add("kvx", double_t, CONTROLLER_GAIN["value"],"k-xd", 0.9236, 0.0, 2.0) +#group4.add("kix", double_t, CONTROLLER_GAIN["value"],"k-ix", 1.3486, 0.0, 2.0) +#group4.add("ky", double_t, CONTROLLER_GAIN["value"],"k-y", -1.5145, -2.0, 0.0) +#group4.add("kvy", double_t, CONTROLLER_GAIN["value"],"k-yd", -0.8551, -2.0, 0.0) +#group4.add("kiy", double_t, CONTROLLER_GAIN["value"],"k-iy", -1.1930, -2.0, 0.0) + +group4.add("kphi", double_t, CONTROLLER_GAIN["value"],"k-phi", 1.7378, 0.0, 10.0) +group4.add("kp", double_t, CONTROLLER_GAIN["value"],"k-p", 0.3476, 0.0, 1.0) +group4.add("kiphi", double_t, CONTROLLER_GAIN["value"],"k-iphi", 0.0, 0.0, 20.0) +group4.add("ktheta", double_t, CONTROLLER_GAIN["value"],"k-theta", 2.2946, 0.0, 10.0) +group4.add("kq", double_t, CONTROLLER_GAIN["value"],"k-q", 0.4589, 0.0, 1.0) +group4.add("kitheta", double_t, CONTROLLER_GAIN["value"],"k-itheta", 0.0, 0.0, 20.0) +group4.add("kpsi", double_t, CONTROLLER_GAIN["value"],"k-psi", 0.3036, 0.0, 0.498) +group4.add("kr", double_t, CONTROLLER_GAIN["value"],"k-p", 0.2440, 0.0, 0.3130) +group4.add("kipsi", double_t, CONTROLLER_GAIN["value"],"k-ipsi", 0.1581, 0.0, 0.3162) + +#group4.add("kphi", double_t, CONTROLLER_GAIN["value"],"k-phi", 2.6042, 0.0, 10.0) +#group4.add("kp", double_t, CONTROLLER_GAIN["value"],"k-p", 0.4431, 0.0, 1.0) +#group4.add("kiphi", double_t, CONTROLLER_GAIN["value"],"k-iphi", 0.0, 0.0, 20.0) +#group4.add("ktheta", double_t, CONTROLLER_GAIN["value"],"k-theta", 2.8913 , 0.0, 10.0) +#group4.add("kq", double_t, CONTROLLER_GAIN["value"],"k-q", 0.5163, 0.0, 1.0) +#group4.add("kitheta", double_t, CONTROLLER_GAIN["value"],"k-itheta", 0.0, 0.0, 20.0) +#group4.add("kpsi", double_t, CONTROLLER_GAIN["value"],"k-psi", 0.7660, 0.0, 1.0) +#group4.add("kr", double_t, CONTROLLER_GAIN["value"],"k-p", 0.4006, 0.0, 1.0) +#group4.add("kipsi", double_t, CONTROLLER_GAIN["value"],"k-ipsi", 0.5774, 0.0, 1.0) + +exit(gen.generate(PACKAGE, "gsft_control", "controllerGS1Dyn")) diff --git a/include/parameters.h b/include/parameters.h index 4f561ff..2989f08 100644 --- a/include/parameters.h +++ b/include/parameters.h @@ -31,7 +31,7 @@ static constexpr double kDefaultPi = 3.14159; // Default constraints static constexpr double kDefaultXmax = 2.5; static constexpr double kDefaultYmax = 2.5; -static constexpr double kDefaultZmax = 1.5; +static constexpr double kDefaultZmax = 1.75; static constexpr double kDefaultCutoffAltitude = 0.175; // CutOff motor (landing) diff --git a/launch/gs1_Gazebo.launch b/launch/tuning_compare_Gazebo.launch similarity index 83% rename from launch/gs1_Gazebo.launch rename to launch/tuning_compare_Gazebo.launch index b3a8a50..d435b4f 100644 --- a/launch/gs1_Gazebo.launch +++ b/launch/tuning_compare_Gazebo.launch @@ -6,14 +6,14 @@ - + - + diff --git a/launch/gs1.launch b/launch/tuning_gs1.launch similarity index 100% rename from launch/gs1.launch rename to launch/tuning_gs1.launch diff --git a/launch/tuning_gs1_Gazebo.launch b/launch/tuning_gs1_Gazebo.launch new file mode 100644 index 0000000..5fa2452 --- /dev/null +++ b/launch/tuning_gs1_Gazebo.launch @@ -0,0 +1,13 @@ + + + + + + + + + + + + + diff --git a/src/tuning_GS1_node.cpp b/src/tuning_GS1_node.cpp index e06d5f5..bfba9b7 100644 --- a/src/tuning_GS1_node.cpp +++ b/src/tuning_GS1_node.cpp @@ -15,10 +15,11 @@ #include #include -#include +#include tuning_GS1ModelClass gController; +bool gFirst_odom; bool gPublish; bool gInit_flag; bool gLanding_flag; @@ -35,12 +36,12 @@ Eigen::VectorXd gThrust_measure(6); gsft_control::EigenOdometry gOdometry; -void controller_dyn_callback(gsft_control::controllerDynConfig &config, uint32_t level) { +void controller_dyn_callback(gsft_control::controllerGS1DynConfig &config, uint32_t level) { // callback if there is a change in dyn_reconfigure if (config.RESET) { // TO DO : reset parameters, gains config.RESET = false; } - else if (level & gsft_control::controllerDyn_ENABLE_CTRL){ + else if (level & gsft_control::controllerGS1Dyn_ENABLE_CTRL){ if (config.new_controller_gains){ gGain[0] = config.kx; // x gGain[1] = config.kvx; // vx @@ -67,12 +68,12 @@ void controller_dyn_callback(gsft_control::controllerDynConfig &config, uint32_t gTest_mode = config.test_mode; - if (config.enable_take_off && !gInit_flag){ // only once - gY0[0] = gOdometry.position.x(); + if (config.enable_take_off && !gInit_flag && gFirst_odom){ // only once + gY0[0] = gOdometry.position.x(); // First position before take off gY0[1] = gOdometry.position.y(); gY0[2] = gOdometry.position.z(); gY0[3] = gPsi; - // if (config.test_mode == gsft_control::controllerDyn_TEST_MANUAL){ + // if (config.test_mode == gsft_control::controllerGS1Dyn_TEST_MANUAL){ gRef[0] = gY0[0]; gRef[1] = gY0[1]; gRef[2] = config.ref_z; @@ -111,7 +112,8 @@ void controller_dyn_callback(gsft_control::controllerDynConfig &config, uint32_t gLOE_t[5] = config.LOE_t6; gInit_flag = true; - ROS_INFO("Take-off Request: %s with Test_mode = %d",config.enable_take_off?"True":"False",gTest_mode); + ROS_INFO("Test_mode = %d",gTest_mode); + ROS_INFO("Take-off Request: %s at x_ref = %f, y_ref = %f, z_ref = %f, psi_ref = %f(deg)",config.enable_take_off?"True":"False",gRef[0],gRef[1],gRef[2],gRef[3]*180.0/gsft_control::kDefaultPi); config.enable_take_off = false; @@ -121,18 +123,18 @@ void controller_dyn_callback(gsft_control::controllerDynConfig &config, uint32_t gRef[1] = gOdometry.position.y(); gRef[2] = gY0[2]; gRef[3] = gPsi; - gTest_mode = gsft_control::controllerDyn_TEST_MANUAL; + gTest_mode = gsft_control::controllerGS1Dyn_TEST_MANUAL; gLanding_flag = true; - ROS_INFO("Landing Request: %s at x_ref = %f, y_ref = %f,psi_ref = %f(deg)",config.enable_landing?"True":"False",gRef[0],gRef[1],gRef[3]*180.0/gsft_control::kDefaultPi); + ROS_INFO("Landing Request: %s at x_ref = %f, y_ref = %f, z_ref = %f, psi_ref = %f(deg)",config.enable_landing?"True":"False",gRef[0],gRef[1],gRef[2],gRef[3]*180.0/gsft_control::kDefaultPi); config.enable_landing = false; } - else if (gTest_mode == gsft_control::controllerDyn_TEST_MANUAL){ + else if (gTest_mode == gsft_control::controllerGS1Dyn_TEST_MANUAL){ if (config.send_waypoint){ gRef[0] = config.ref_x; gRef[1] = config.ref_y; gRef[2] = config.ref_z; gRef[3] = config.ref_yaw_deg*gsft_control::kDefaultPi/180.0; - ROS_INFO("Waypoint Request: x_ref = %f, y_ref = %f, z_ref = %f, psi_ref = %f(deg)",gRef[0],gRef[1],gRef[2],gRef[3]); + ROS_INFO("Waypoint Request: x_ref = %f, y_ref = %f, z_ref = %f, psi_ref = %f(deg)",gRef[0],gRef[1],gRef[2],gRef[3]*180.0/gsft_control::kDefaultPi); config.send_waypoint = false; } } @@ -162,6 +164,9 @@ void OdometryCallback(const nav_msgs::OdometryConstPtr& odom_msg) { gEmergency_status = true; } } + if (!gFirst_odom){ + ROS_INFO("First odometry: x = %f, y = %f, z = %f",gOdometry.position.x(),gOdometry.position.y(),gOdometry.position.z()); + gFirst_odom = true;} } /*void LostControlCallback(const gsft_control::LOEConstPtr& loe_msg) { @@ -172,13 +177,13 @@ void OdometryCallback(const nav_msgs::OdometryConstPtr& odom_msg) { "]: ---------- TND LOE0 = " << gLOE[0] << std::endl; }*/ -// ASCTEC test +// ASCTEC test: command 0..200 void MotorSpeedCallback(const asctec_hl_comm::MotorSpeedConstPtr& motor_msg) { gsft_control::motorMsg2Thrust(motor_msg,&gThrust_measure); //ROS_INFO("Thrust T1 = %f(N)",gThrust_measure[0]); } -// Gazebo +// Gazebo: command rad/sec void MotorSpeedGazeboCallback(const mav_msgs::ActuatorsConstPtr& motor_msg) { gsft_control::speedMsg2Thrust(motor_msg,&gThrust_measure); // ROS_INFO("Thrust T1 = %f(N)",gThrust_measure[0]); @@ -232,7 +237,8 @@ int main(int argc, char** argv) { ros::Rate r(1000); - gPublish = false; + gFirst_odom = false; + gPublish = false; gInit_flag = false; gLanding_flag = false; gEmergency_status = false; @@ -246,8 +252,8 @@ int main(int argc, char** argv) { gLOE_t = Eigen::VectorXd::Zero(6); gThrust_measure = Eigen::VectorXd::Zero(6); - dynamic_reconfigure::Server server; - dynamic_reconfigure::Server::CallbackType f; + dynamic_reconfigure::Server server; + dynamic_reconfigure::Server::CallbackType f; f = boost::bind(&controller_dyn_callback, _1, _2); server.setCallback(f); @@ -277,6 +283,10 @@ int main(int argc, char** argv) { gPsi = euler_angles[2]; // gPsi = gOdometry.getYaw(); // same result + if (gFirst_odom && !controller_active){ + server.setCallback(f); + } + if (gInit_flag && !controller_active) { // only once when controller is not actived gController.initialize(); controller_active = true; @@ -287,9 +297,9 @@ int main(int argc, char** argv) { for (unsigned int i=0; i< 6; i++) { gController.tuning_GS1_U.LOE_t[i] = gLOE_t[i]; // fault time } - for (unsigned int i=0; i< 19; i++) { + /*for (unsigned int i=0; i< 19; i++) { ROS_INFO("Controller gain k[%d] = %f",i,gGain[i]); - } + }*/ } if (controller_active) { // controller active after take-off request @@ -402,6 +412,7 @@ int main(int argc, char** argv) { uav_state_msg->rotation_speed_B.x = gOdometry.angular_velocity.x(); uav_state_msg->rotation_speed_B.y = gOdometry.angular_velocity.y(); uav_state_msg->rotation_speed_B.z = gOdometry.angular_velocity.z(); + uav_state_msg->total_thrust = gController.tuning_GS1_Y.virtual_control[0]; uav_state_msg->moment.x = gController.tuning_GS1_Y.virtual_control[1]; uav_state_msg->moment.y = gController.tuning_GS1_Y.virtual_control[2]; diff --git a/src/tuning_nominal_grt_rtw/multiword_types.h b/src/tuning_nominal_grt_rtw/multiword_types.h index 9eed723..a479f02 100644 --- a/src/tuning_nominal_grt_rtw/multiword_types.h +++ b/src/tuning_nominal_grt_rtw/multiword_types.h @@ -7,9 +7,9 @@ * * Code generation for model "tuning_nominal". * - * Model version : 1.1498 + * Model version : 1.1505 * Simulink Coder version : 8.12 (R2017a) 16-Feb-2017 - * C++ source code generated on : Wed Oct 10 17:23:02 2018 + * C++ source code generated on : Thu Oct 11 16:37:28 2018 * * Target selection: grt.tlc * Note: GRT includes extra infrastructure and instrumentation for prototyping diff --git a/src/tuning_nominal_grt_rtw/rtmodel.h b/src/tuning_nominal_grt_rtw/rtmodel.h index 2daf05f..b1a9c07 100644 --- a/src/tuning_nominal_grt_rtw/rtmodel.h +++ b/src/tuning_nominal_grt_rtw/rtmodel.h @@ -7,9 +7,9 @@ * * Code generation for model "tuning_nominal". * - * Model version : 1.1498 + * Model version : 1.1505 * Simulink Coder version : 8.12 (R2017a) 16-Feb-2017 - * C++ source code generated on : Wed Oct 10 17:23:02 2018 + * C++ source code generated on : Thu Oct 11 16:37:28 2018 * * Target selection: grt.tlc * Note: GRT includes extra infrastructure and instrumentation for prototyping diff --git a/src/tuning_nominal_grt_rtw/rtwtypes.h b/src/tuning_nominal_grt_rtw/rtwtypes.h index 8c62ac1..4af2d86 100644 --- a/src/tuning_nominal_grt_rtw/rtwtypes.h +++ b/src/tuning_nominal_grt_rtw/rtwtypes.h @@ -7,9 +7,9 @@ * * Code generation for model "tuning_nominal". * - * Model version : 1.1498 + * Model version : 1.1505 * Simulink Coder version : 8.12 (R2017a) 16-Feb-2017 - * C++ source code generated on : Wed Oct 10 17:23:02 2018 + * C++ source code generated on : Thu Oct 11 16:37:28 2018 * * Target selection: grt.tlc * Note: GRT includes extra infrastructure and instrumentation for prototyping diff --git a/src/tuning_nominal_grt_rtw/tuning_nominal.cpp b/src/tuning_nominal_grt_rtw/tuning_nominal.cpp index c4b7b67..897389b 100644 --- a/src/tuning_nominal_grt_rtw/tuning_nominal.cpp +++ b/src/tuning_nominal_grt_rtw/tuning_nominal.cpp @@ -7,9 +7,9 @@ * * Code generation for model "tuning_nominal". * - * Model version : 1.1498 + * Model version : 1.1505 * Simulink Coder version : 8.12 (R2017a) 16-Feb-2017 - * C++ source code generated on : Wed Oct 10 17:23:02 2018 + * C++ source code generated on : Thu Oct 11 16:37:28 2018 * * Target selection: grt.tlc * Note: GRT includes extra infrastructure and instrumentation for prototyping @@ -21,25 +21,6 @@ #include "tuning_nominal.h" #include "tuning_nominal_private.h" -static void rate_scheduler(RT_MODEL_tuning_nominal_T *const tuning_nominal_M); - -/* - * This function updates active task flag for each subrate. - * The function is called at model base rate, hence the - * generated code self-manages all its subrates. - */ -static void rate_scheduler(RT_MODEL_tuning_nominal_T *const tuning_nominal_M) -{ - /* Compute which subrates run during the next base time step. Subrates - * are an integer multiple of the base rate counter. Therefore, the subtask - * counter is reset when it reaches its limit (zero means run). - */ - (tuning_nominal_M->Timing.TaskCounters.TID[2])++; - if ((tuning_nominal_M->Timing.TaskCounters.TID[2]) > 4) {/* Sample time: [0.005s, 0.0s] */ - tuning_nominal_M->Timing.TaskCounters.TID[2] = 0; - } -} - /* * This function updates continuous states using the ODE4 fixed-step * solver algorithm @@ -158,94 +139,81 @@ void tuning_nominalModelClass::step() real_T rtb_ff_idx_0; real_T u0; - /* Sum: '/Sum5' incorporates: + /* Sum: '/Sum5' incorporates: * Inport: '/X' * Inport: '/Y0' */ rtb_d_z = tuning_nominal_U.X[2] - tuning_nominal_U.Y0[2]; - /* Sum: '/Sum1' incorporates: + /* Sum: '/Sum1' incorporates: * Inport: '/X' * Inport: '/Y0' */ rtb_d_x = tuning_nominal_U.X[0] - tuning_nominal_U.Y0[0]; - /* Sum: '/Sum2' incorporates: + /* Sum: '/Sum2' incorporates: * Inport: '/X' * Inport: '/gain' - * Integrator: '/Integrator1' - * Product: '/Product2' - * Product: '/Product3' - * SignalConversion: '/TmpSignal ConversionAtProduct3Inport2' + * Integrator: '/Integrator1' + * Product: '/Product2' + * Product: '/Product3' + * SignalConversion: '/TmpSignal ConversionAtProduct3Inport2' */ tuning_nominal_B.Sum2 = tuning_nominal_U.gain[2] * - tuning_nominal_X.Integrator1_CSTATE_d - (tuning_nominal_U.gain[0] * + tuning_nominal_X.Integrator1_CSTATE_f - (tuning_nominal_U.gain[0] * rtb_d_x + tuning_nominal_U.gain[1] * tuning_nominal_U.X[3]); - /* Sum: '/Sum4' incorporates: + /* Sum: '/Sum4' incorporates: * Inport: '/X' * Inport: '/Y0' */ rtb_d_y = tuning_nominal_U.X[1] - tuning_nominal_U.Y0[1]; - /* Sum: '/Sum3' incorporates: + /* Sum: '/Sum3' incorporates: * Inport: '/X' * Inport: '/gain' - * Integrator: '/Integrator' - * Product: '/Product1' - * Product: '/Product4' - * SignalConversion: '/TmpSignal ConversionAtProduct1Inport2' + * Integrator: '/Integrator' + * Product: '/Product1' + * Product: '/Product4' + * SignalConversion: '/TmpSignal ConversionAtProduct1Inport2' */ tuning_nominal_B.Sum3 = tuning_nominal_U.gain[5] * tuning_nominal_X.Integrator_CSTATE - (tuning_nominal_U.gain[3] * rtb_d_y + tuning_nominal_U.gain[4] * tuning_nominal_U.X[4]); - - /* RateTransition: '/T_outer' incorporates: - * Inport: '/X' - */ - if ((rtmIsMajorTimeStep((&tuning_nominal_M)) && - (&tuning_nominal_M)->Timing.TaskCounters.TID[1] == 0) && - (rtmIsMajorTimeStep((&tuning_nominal_M)) && - (&tuning_nominal_M)->Timing.TaskCounters.TID[2] == 0)) { - tuning_nominal_B.T_outer[0] = tuning_nominal_B.Sum2; - tuning_nominal_B.T_outer[1] = tuning_nominal_B.Sum3; - tuning_nominal_B.T_outer[2] = tuning_nominal_U.X[8]; - } - - /* End of RateTransition: '/T_outer' */ - if (rtmIsMajorTimeStep((&tuning_nominal_M)) && - (&tuning_nominal_M)->Timing.TaskCounters.TID[2] == 0) { - /* Fcn: '/Fcn1' */ - tuning_nominal_B.Fcn1 = tuning_nominal_B.T_outer[1] * std::cos - (tuning_nominal_B.T_outer[2]) + tuning_nominal_B.T_outer[0] * std::sin - (tuning_nominal_B.T_outer[2]); + if (rtmIsMajorTimeStep((&tuning_nominal_M))) { + /* Fcn: '/Fcn1' incorporates: + * Inport: '/X' + */ + tuning_nominal_B.Fcn1 = tuning_nominal_B.Sum3 * std::cos + (tuning_nominal_U.X[8]) + tuning_nominal_B.Sum2 * std::sin + (tuning_nominal_U.X[8]); } /* Clock: '/Clock' */ rtb_Clock = (&tuning_nominal_M)->Timing.t[0]; - /* MATLAB Function: '/FFW' incorporates: + /* MATLAB Function: '/FFW' incorporates: * Inport: '/X' * Inport: '/mode' */ - /* MATLAB Function 'Test_config_and_data/FFW': ':1' */ - /* ':1:2' ff = [0;0]; */ + /* MATLAB Function 'Test_config_and_data/FFW': ':1' */ + /* ':1:2' ff = [0;0]; */ rtb_ff_idx_0 = 0.0; rtb_ff_idx_1 = 0.0; - /* ':1:3' g = 9.81; */ + /* ':1:3' g = 9.81; */ /* [x;y] = [cos(t); sin(t)] */ - /* ':1:5' if (test_mode == 2) */ + /* ':1:5' if (test_mode == 2) */ if ((tuning_nominal_U.mode == 2.0) && ((rtb_Clock >= 10.0) && (rtb_Clock <= 50.0))) { - /* ':1:6' if (t >=10) && (t<= 50) */ - /* ':1:7' axref_N = -cos(t); */ - /* ':1:8' ayref_N = -sin(t); */ - /* ':1:10' axref_b = cos(X(8))*cos(X(9))*axref_N + cos(X(8))*sin(X(9))*ayref_N; */ - /* ':1:11' ayref_b = (sin(X(7))*sin(X(8))*cos(X(9)) - cos(X(7))*sin(X(9)))*axref_N + (sin(X(7))*sin(X(8))*sin(X(9)) + cos(X(7))*cos(X(9)))*ayref_N; */ + /* ':1:6' if (t >=10) && (t<= 50) */ + /* ':1:7' axref_N = -cos(t); */ + /* ':1:8' ayref_N = -sin(t); */ + /* ':1:10' axref_b = cos(X(8))*cos(X(9))*axref_N + cos(X(8))*sin(X(9))*ayref_N; */ + /* ':1:11' ayref_b = (sin(X(7))*sin(X(8))*cos(X(9)) - cos(X(7))*sin(X(9)))*axref_N + (sin(X(7))*sin(X(8))*sin(X(9)) + cos(X(7))*cos(X(9)))*ayref_N; */ /* azref_b = (cos(X(7))*sin(X(8))*cos(X(9)) + sin(X(7))*sin(X(9)))*axref_N + (cos(X(7))*sin(X(8))*sin(X(9)) - sin(X(7))*cos(X(9)))*ayref_N; */ /* */ - /* ':1:14' ff = [-ayref_b/g; axref_b/g]; */ + /* ':1:14' ff = [-ayref_b/g; axref_b/g]; */ rtb_ff_idx_0 = -((std::sin(tuning_nominal_U.X[6]) * std::sin (tuning_nominal_U.X[7]) * std::cos(tuning_nominal_U.X[8]) - std::cos(tuning_nominal_U.X[6]) * std::sin @@ -258,51 +226,50 @@ void tuning_nominalModelClass::step() (tuning_nominal_U.X[7]) * std::sin(tuning_nominal_U.X[8]) * -std::sin(rtb_Clock)) / 9.81; } else { - /* ':1:15' else */ - /* ':1:16' ff = [0;0]; */ + /* ':1:15' else */ + /* ':1:16' ff = [0;0]; */ } - /* End of MATLAB Function: '/FFW' */ - if (rtmIsMajorTimeStep((&tuning_nominal_M)) && - (&tuning_nominal_M)->Timing.TaskCounters.TID[2] == 0) { - /* Fcn: '/Fcn' */ - tuning_nominal_B.Fcn = -tuning_nominal_B.T_outer[1] * std::sin - (tuning_nominal_B.T_outer[2]) + tuning_nominal_B.T_outer[0] * std::cos - (tuning_nominal_B.T_outer[2]); + /* End of MATLAB Function: '/FFW' */ + if (rtmIsMajorTimeStep((&tuning_nominal_M))) { + /* Fcn: '/Fcn' incorporates: + * Inport: '/X' + */ + tuning_nominal_B.Fcn = -tuning_nominal_B.Sum3 * std::sin + (tuning_nominal_U.X[8]) + tuning_nominal_B.Sum2 * std::cos + (tuning_nominal_U.X[8]); } - /* Sum: '/Sum6' incorporates: + /* Sum: '/Sum6' incorporates: * Inport: '/X' * Inport: '/Y0' */ rtb_d_psi = tuning_nominal_U.X[8] - tuning_nominal_U.Y0[3]; /* SignalConversion: '/TmpSignal ConversionAtControl AllocationInport1' incorporates: - * Constant: '/ ' - * Gain: '/Gain1' - * Gain: '/Gain' + * Constant: '/ ' * Inport: '/X' * Inport: '/gain' - * Integrator: '/Integrator1' - * Product: '/Product' - * Product: '/Product1' - * SignalConversion: '/TmpSignal ConversionAtProduct1Inport2' - * Sum: '/Sum1' - * Sum: '/Sum1' + * Integrator: '/Integrator1' + * Product: '/Product' + * Product: '/Product1' + * SignalConversion: '/TmpSignal ConversionAtProduct1Inport2' + * Sum: '/Sum1' + * Sum: '/Sum1' */ - rtb_ref_idx_0 = ((tuning_nominal_U.gain[8] * - tuning_nominal_X.Integrator1_CSTATE - - (tuning_nominal_U.gain[6] * rtb_d_z + - tuning_nominal_U.gain[7] * tuning_nominal_U.X[5])) * - 0.64935064935064934 + 9.81) * 1.54; + rtb_ref_idx_0 = (tuning_nominal_U.gain[8] * + tuning_nominal_X.Integrator1_CSTATE - + (tuning_nominal_U.gain[6] * rtb_d_z + + tuning_nominal_U.gain[7] * tuning_nominal_U.X[5])) + + 15.107400000000002; - /* Saturate: '/2Nm ' incorporates: + /* Saturate: '/2Nm ' incorporates: * Inport: '/X' * Inport: '/gain' - * Product: '/Product' - * SignalConversion: '/TmpSignal ConversionAtProductInport2' - * Sum: '/Sum7' - * Sum: '/Sum1' + * Product: '/Product' + * SignalConversion: '/TmpSignal ConversionAtProductInport2' + * Sum: '/Sum7' + * Sum: '/Sum1' */ rtb_ff_idx_0 = (tuning_nominal_B.Fcn1 + rtb_ff_idx_0) - (tuning_nominal_U.gain[9] * tuning_nominal_U.X[6] + tuning_nominal_U.gain @@ -317,15 +284,15 @@ void tuning_nominalModelClass::step() } } - /* End of Saturate: '/2Nm ' */ + /* End of Saturate: '/2Nm ' */ - /* Saturate: '/2Nm' incorporates: + /* Saturate: '/2Nm' incorporates: * Inport: '/X' * Inport: '/gain' - * Product: '/Product' - * SignalConversion: '/TmpSignal ConversionAtProductInport2' - * Sum: '/Sum8' - * Sum: '/Sum1' + * Product: '/Product' + * SignalConversion: '/TmpSignal ConversionAtProductInport2' + * Sum: '/Sum8' + * Sum: '/Sum1' */ rtb_ff_idx_1 = (tuning_nominal_B.Fcn + rtb_ff_idx_1) - (tuning_nominal_U.gain[12] * tuning_nominal_U.X[7] + @@ -340,19 +307,19 @@ void tuning_nominalModelClass::step() } } - /* End of Saturate: '/2Nm' */ + /* End of Saturate: '/2Nm' */ - /* Saturate: '/1Nm' incorporates: + /* Saturate: '/1Nm' incorporates: * Inport: '/X' * Inport: '/gain' - * Integrator: '/Integrator1' - * Product: '/Product' - * Product: '/Product1' - * SignalConversion: '/TmpSignal ConversionAtProduct1Inport2' - * Sum: '/Sum1' + * Integrator: '/Integrator1' + * Product: '/Product' + * Product: '/Product1' + * SignalConversion: '/TmpSignal ConversionAtProduct1Inport2' + * Sum: '/Sum1' */ rtb_ref_idx_3 = tuning_nominal_U.gain[17] * - tuning_nominal_X.Integrator1_CSTATE_j - (tuning_nominal_U.gain[15] * + tuning_nominal_X.Integrator1_CSTATE_a - (tuning_nominal_U.gain[15] * rtb_d_psi + tuning_nominal_U.gain[16] * tuning_nominal_U.X[11]); if (rtb_ref_idx_3 > 1.0) { /* SignalConversion: '/TmpSignal ConversionAtControl AllocationInport1' */ @@ -364,10 +331,10 @@ void tuning_nominalModelClass::step() } } - /* End of Saturate: '/1Nm' */ - /* MATLAB Function 'Test_config_and_data/LOE_': ':1' */ - /* ':1:2' LOE_out = [0;0;0;0;0;0]; */ - /* ':1:3' for i = 1:6 */ + /* End of Saturate: '/1Nm' */ + /* MATLAB Function 'Test_config_and_data/LOE_': ':1' */ + /* ':1:2' LOE_out = [0;0;0;0;0;0]; */ + /* ':1:3' for i = 1:6 */ for (i = 0; i < 6; i++) { /* Gain: '/Control Allocation' incorporates: * Saturate: '/ ' @@ -388,19 +355,19 @@ void tuning_nominalModelClass::step() tuning_nominal_B.u[i] = u0; } - /* MATLAB Function: '/LOE_' incorporates: + /* MATLAB Function: '/LOE_' incorporates: * Inport: '/LOE_a' * Inport: '/LOE_t' */ rtb_LOE_out[i] = 0.0; - /* ':1:4' if t>= LOE_t(i) */ + /* ':1:4' if t>= LOE_t(i) */ if (rtb_Clock >= tuning_nominal_U.LOE_t[i]) { - /* ':1:5' LOE_out(i) = LOE(i); */ + /* ':1:5' LOE_out(i) = LOE(i); */ rtb_LOE_out[i] = tuning_nominal_U.LOE_a[i]; } - /* End of MATLAB Function: '/LOE_' */ + /* End of MATLAB Function: '/LOE_' */ } /* MATLAB Function: '/Actuator_Fault' */ @@ -421,16 +388,16 @@ void tuning_nominalModelClass::step() /* Outport: '/motor_command' */ for (i = 0; i < 6; i++) { - /* Gain: '/mapping_0_200' incorporates: - * Gain: '/ ' - * Gain: '/rads_to_RPM' - * Sqrt: '/Sqrt1' - * Sum: '/Sum3' + /* Gain: '/mapping_0_200' incorporates: + * Gain: '/ ' + * Gain: '/rads_to_RPM' + * Sqrt: '/Sqrt1' + * Sum: '/Sum3' */ u0 = (std::sqrt(116978.4923343994 * rtb_T_f[i]) * 9.5493 - 1250.0) * 0.022857142857142857; - /* Saturate: '/Saturation' */ + /* Saturate: '/Saturation' */ if (u0 > 200.0) { tuning_nominal_Y.motor_command[i] = 200.0; } else if (u0 < 0.0) { @@ -439,7 +406,7 @@ void tuning_nominalModelClass::step() tuning_nominal_Y.motor_command[i] = u0; } - /* End of Saturate: '/Saturation' */ + /* End of Saturate: '/Saturation' */ } /* End of Outport: '/motor_command' */ @@ -450,19 +417,19 @@ void tuning_nominalModelClass::step() tuning_nominal_Y.virtual_control[2] = rtb_ff_idx_1; tuning_nominal_Y.virtual_control[3] = rtb_ref_idx_3; - /* MATLAB Function: '/MATLAB Function' incorporates: + /* MATLAB Function: '/MATLAB Function' incorporates: * Inport: '/Y0' * Inport: '/mode' * Inport: '/ref' */ - /* MATLAB Function 'Test_config_and_data/MATLAB Function': ':1' */ - /* ':1:2' ref = Y0; */ - /* ':1:3' switch test_mode */ + /* MATLAB Function 'Test_config_and_data/MATLAB Function': ':1' */ + /* ':1:2' ref = Y0; */ + /* ':1:3' switch test_mode */ switch ((int32_T)tuning_nominal_U.mode) { case 0: - /* ':1:4' case 0 % manual test */ + /* ':1:4' case 0 % manual test */ /* manual test */ - /* ':1:5' ref = ref_manual; */ + /* ':1:5' ref = ref_manual; */ rtb_ref_idx_0 = tuning_nominal_U.ref[0]; rtb_ff_idx_0 = tuning_nominal_U.ref[1]; rtb_ff_idx_1 = tuning_nominal_U.ref[2]; @@ -470,46 +437,46 @@ void tuning_nominalModelClass::step() break; case 1: - /* ':1:6' case 1 % waypoint */ + /* ':1:6' case 1 % waypoint */ /* waypoint */ - /* ':1:7' if t<=15 */ + /* ':1:7' if t<=15 */ if (rtb_Clock <= 15.0) { - /* ':1:8' ref = [Y0(1); Y0(2); 1; Y0(4)]; */ + /* ':1:8' ref = [Y0(1); Y0(2); 1; Y0(4)]; */ rtb_ref_idx_0 = tuning_nominal_U.Y0[0]; rtb_ff_idx_0 = tuning_nominal_U.Y0[1]; rtb_ff_idx_1 = 1.0; rtb_ref_idx_3 = tuning_nominal_U.Y0[3]; } else if (rtb_Clock <= 25.0) { - /* ':1:9' elseif t <= 25 */ - /* ':1:10' ref = [Y0(1)+1; Y0(2)-1; 1; Y0(4)]; */ + /* ':1:9' elseif t <= 25 */ + /* ':1:10' ref = [Y0(1)+1; Y0(2)-1; 1; Y0(4)]; */ rtb_ref_idx_0 = tuning_nominal_U.Y0[0] + 1.0; rtb_ff_idx_0 = tuning_nominal_U.Y0[1] - 1.0; rtb_ff_idx_1 = 1.0; rtb_ref_idx_3 = tuning_nominal_U.Y0[3]; } else if (rtb_Clock <= 35.0) { - /* ':1:11' elseif t <=35 */ - /* ':1:12' ref = [Y0(1)-1; Y0(2)+1; 1; Y0(4)]; */ + /* ':1:11' elseif t <=35 */ + /* ':1:12' ref = [Y0(1)-1; Y0(2)+1; 1; Y0(4)]; */ rtb_ref_idx_0 = tuning_nominal_U.Y0[0] - 1.0; rtb_ff_idx_0 = tuning_nominal_U.Y0[1] + 1.0; rtb_ff_idx_1 = 1.0; rtb_ref_idx_3 = tuning_nominal_U.Y0[3]; } else if (rtb_Clock <= 45.0) { - /* ':1:13' elseif t <=45 */ - /* ':1:14' ref = [Y0(1)-1; Y0(2)+1; 1; Y0(4)]; */ + /* ':1:13' elseif t <=45 */ + /* ':1:14' ref = [Y0(1)-1; Y0(2)+1; 1; Y0(4)]; */ rtb_ref_idx_0 = tuning_nominal_U.Y0[0] - 1.0; rtb_ff_idx_0 = tuning_nominal_U.Y0[1] + 1.0; rtb_ff_idx_1 = 1.0; rtb_ref_idx_3 = tuning_nominal_U.Y0[3]; } else if (rtb_Clock <= 55.0) { - /* ':1:15' elseif t <=55 */ - /* ':1:16' ref = [Y0(1); Y0(2); 1; Y0(4)]; */ + /* ':1:15' elseif t <=55 */ + /* ':1:16' ref = [Y0(1); Y0(2); 1; Y0(4)]; */ rtb_ref_idx_0 = tuning_nominal_U.Y0[0]; rtb_ff_idx_0 = tuning_nominal_U.Y0[1]; rtb_ff_idx_1 = 1.0; rtb_ref_idx_3 = tuning_nominal_U.Y0[3]; } else { - /* ':1:17' else */ - /* ':1:18' ref = Y0; */ + /* ':1:17' else */ + /* ':1:18' ref = Y0; */ rtb_ref_idx_0 = tuning_nominal_U.Y0[0]; rtb_ff_idx_0 = tuning_nominal_U.Y0[1]; rtb_ff_idx_1 = tuning_nominal_U.Y0[2]; @@ -530,32 +497,32 @@ void tuning_nominalModelClass::step() break; case 2: - /* ':1:31' case 2 % circular tracking */ + /* ':1:31' case 2 % circular tracking */ /* circular tracking */ - /* ':1:32' if t<=10 */ + /* ':1:32' if t<=10 */ if (rtb_Clock <= 10.0) { - /* ':1:33' ref = [Y0(1); Y0(2); 0.75; Y0(4)]; */ + /* ':1:33' ref = [Y0(1); Y0(2); 0.75; Y0(4)]; */ rtb_ref_idx_0 = tuning_nominal_U.Y0[0]; rtb_ff_idx_0 = tuning_nominal_U.Y0[1]; rtb_ff_idx_1 = 0.75; rtb_ref_idx_3 = tuning_nominal_U.Y0[3]; } else if (rtb_Clock <= 50.0) { - /* ':1:34' elseif t <= 50 */ - /* ':1:35' ref = [Y0(1)+cos(t); Y0(2)+sin(t); 0.75; Y0(4)]; */ + /* ':1:34' elseif t <= 50 */ + /* ':1:35' ref = [Y0(1)+cos(t); Y0(2)+sin(t); 0.75; Y0(4)]; */ rtb_ref_idx_0 = tuning_nominal_U.Y0[0] + std::cos(rtb_Clock); rtb_ff_idx_0 = tuning_nominal_U.Y0[1] + std::sin(rtb_Clock); rtb_ff_idx_1 = 0.75; rtb_ref_idx_3 = tuning_nominal_U.Y0[3]; } else if (rtb_Clock <= 60.0) { - /* ':1:36' elseif t <= 60 */ - /* ':1:37' ref = [Y0(1); Y0(2); 0.75; Y0(4)]; */ + /* ':1:36' elseif t <= 60 */ + /* ':1:37' ref = [Y0(1); Y0(2); 0.75; Y0(4)]; */ rtb_ref_idx_0 = tuning_nominal_U.Y0[0]; rtb_ff_idx_0 = tuning_nominal_U.Y0[1]; rtb_ff_idx_1 = 0.75; rtb_ref_idx_3 = tuning_nominal_U.Y0[3]; } else { - /* ':1:38' else */ - /* ':1:39' ref = Y0; */ + /* ':1:38' else */ + /* ':1:39' ref = Y0; */ rtb_ref_idx_0 = tuning_nominal_U.Y0[0]; rtb_ff_idx_0 = tuning_nominal_U.Y0[1]; rtb_ff_idx_1 = tuning_nominal_U.Y0[2]; @@ -566,8 +533,8 @@ void tuning_nominalModelClass::step() break; default: - /* ':1:42' otherwise */ - /* ':1:43' ref = Y0; */ + /* ':1:42' otherwise */ + /* ':1:43' ref = Y0; */ rtb_ref_idx_0 = tuning_nominal_U.Y0[0]; rtb_ff_idx_0 = tuning_nominal_U.Y0[1]; rtb_ff_idx_1 = tuning_nominal_U.Y0[2]; @@ -575,7 +542,7 @@ void tuning_nominalModelClass::step() break; } - /* End of MATLAB Function: '/MATLAB Function' */ + /* End of MATLAB Function: '/MATLAB Function' */ /* Outport: '/ref_out' */ tuning_nominal_Y.ref_out[0] = rtb_ref_idx_0; @@ -589,8 +556,7 @@ void tuning_nominalModelClass::step() } /* End of Outport: '/LOE_true' */ - if (rtmIsMajorTimeStep((&tuning_nominal_M)) && - (&tuning_nominal_M)->Timing.TaskCounters.TID[1] == 0) { + if (rtmIsMajorTimeStep((&tuning_nominal_M))) { /* Delay: '/MemoryX' */ for (i = 0; i < 6; i++) { if (tuning_nominal_DW.icLoad != 0) { @@ -601,19 +567,19 @@ void tuning_nominalModelClass::step() } /* Outputs for Atomic SubSystem: '/UseCurrentEstimator' */ - /* Outputs for Enabled SubSystem: '/Enabled Subsystem' incorporates: - * EnablePort: '/Enable' + /* Outputs for Enabled SubSystem: '/Enabled Subsystem' incorporates: + * EnablePort: '/Enable' */ if (!tuning_nominal_DW.EnabledSubsystem_MODE) { tuning_nominal_DW.EnabledSubsystem_MODE = true; } - /* Sum: '/Add1' incorporates: + /* Sum: '/Add1' incorporates: * Constant: '/C' * Delay: '/MemoryX' * Inport: '/X' - * Product: '/Product' - * Product: '/Product2' + * Product: '/Product' + * Product: '/Product2' */ for (i = 0; i < 3; i++) { u0 = 0.0; @@ -625,10 +591,10 @@ void tuning_nominalModelClass::step() rtb_Sum1_l[i] = tuning_nominal_U.X[9 + i] - u0; } - /* End of Sum: '/Add1' */ + /* End of Sum: '/Add1' */ for (i = 0; i < 6; i++) { - /* Product: '/Product2' incorporates: - * Constant: '/KalmanGainM' + /* Product: '/Product2' incorporates: + * Constant: '/KalmanGainM' */ tuning_nominal_B.Product2[i] = 0.0; tuning_nominal_B.Product2[i] += @@ -638,14 +604,14 @@ void tuning_nominalModelClass::step() tuning_nominal_B.Product2[i] += tuning_nominal_ConstP.KalmanGainM_Value[i + 12] * rtb_Sum1_l[2]; - /* Sum: '/Add' incorporates: + /* Sum: '/Add' incorporates: * Delay: '/MemoryX' */ rtb_Add_a[i] = tuning_nominal_B.Product2[i] + tuning_nominal_DW.MemoryX_DSTATE[i]; } - /* End of Outputs for SubSystem: '/Enabled Subsystem' */ + /* End of Outputs for SubSystem: '/Enabled Subsystem' */ /* End of Outputs for SubSystem: '/UseCurrentEstimator' */ /* Product: '/Product1' incorporates: @@ -660,13 +626,13 @@ void tuning_nominalModelClass::step() /* End of Product: '/Product1' */ - /* Sum: '/Sum1' incorporates: - * Product: '/i x k' - * Product: '/j x i' - * Product: '/k x j' - * Product: '/i x j' - * Product: '/j x k' - * Product: '/k x i' + /* Sum: '/Sum1' incorporates: + * Product: '/i x j' + * Product: '/j x k' + * Product: '/k x i' + * Product: '/i x k' + * Product: '/j x i' + * Product: '/k x j' */ rtb_Add_f[0] = rtb_Add_a[1] * rtb_Sum1_l[2]; rtb_Add_f[1] = rtb_Add_a[2] * rtb_Sum1_l[0]; @@ -679,7 +645,7 @@ void tuning_nominalModelClass::step() * Constant: '/Constant' * Constant: '/Constant1' * Product: '/Product' - * Sum: '/Sum1' + * Sum: '/Sum1' */ for (i = 0; i < 3; i++) { rtb_Sum3_i[i] = (((tuning_nominal_ConstP.Constant1_Value[i + 3] * @@ -694,16 +660,16 @@ void tuning_nominalModelClass::step() /* End of Sum: '/Sum3' */ /* MATLAB Function: '/MATLAB Function1' */ - /* MATLAB Function 'FDD_Kalman/MATLAB Function1': ':1' */ - /* ':1:2' arm = 0.215; */ - /* ':1:2' factor = 0.0365; */ - /* ':1:4' M = [arm/2 arm arm/2; */ - /* ':1:5' -sqrt(3)*arm/2 0 sqrt(3)*arm/2; */ - /* ':1:6' -factor factor -factor]; */ - /* ':1:8' diff = [u(1) - u(4) ; */ - /* ':1:9' u(2) - u(5) ; */ - /* ':1:10' u(3) - u(6) ]; */ - /* ':1:11' Residu = diff - M\y; */ + /* MATLAB Function 'FDD_Kalman/MATLAB Function1': ':1' */ + /* ':1:2' arm = 0.215; */ + /* ':1:2' factor = 0.0365; */ + /* ':1:4' M = [arm/2 arm arm/2; */ + /* ':1:5' -sqrt(3)*arm/2 0 sqrt(3)*arm/2; */ + /* ':1:6' -factor factor -factor]; */ + /* ':1:8' diff = [u(1) - u(4) ; */ + /* ':1:9' u(2) - u(5) ; */ + /* ':1:10' u(3) - u(6) ]; */ + /* ':1:11' Residu = diff - M\y; */ rtb_Sum1_l[1] = rtb_Sum3_i[0] - rtb_Sum3_i[1] * -0.57735026918962584; rtb_Sum1_l[2] = (rtb_Sum3_i[2] - rtb_Sum3_i[1] * 0.19603055651554735) - rtb_Sum1_l[1] * 0.16976744186046511; @@ -719,9 +685,9 @@ void tuning_nominalModelClass::step() * Memory: '/Memory' */ /* Residu_1_4 */ - /* ':1:14' gamma = [Residu(1)/u(1) */ - /* ':1:15' Residu(2)/u(2) */ - /* ':1:16' Residu(3)/u(3)]; */ + /* ':1:14' gamma = [Residu(1)/u(1) */ + /* ':1:15' Residu(2)/u(2) */ + /* ':1:16' Residu(3)/u(3)]; */ tuning_nominal_Y.LOE13_estimated[0] = ((tuning_nominal_DW.Memory_PreviousInput[0] - tuning_nominal_DW.Memory_PreviousInput[3]) - rtb_Sum1_l[0]) / @@ -744,8 +710,7 @@ void tuning_nominalModelClass::step() tuning_nominal_Y.thrust_after[i] = rtb_T_f[i]; } - if (rtmIsMajorTimeStep((&tuning_nominal_M)) && - (&tuning_nominal_M)->Timing.TaskCounters.TID[1] == 0) { + if (rtmIsMajorTimeStep((&tuning_nominal_M))) { /* Outport: '/acc_Kalman' */ tuning_nominal_Y.acc_Kalman[0] = rtb_Add_a[3]; @@ -773,8 +738,8 @@ void tuning_nominalModelClass::step() /* Outport: '/vel_Kalman' */ tuning_nominal_Y.vel_Kalman[2] = rtb_Add_a[2]; - /* Outputs for Enabled SubSystem: '/MeasurementUpdate' incorporates: - * EnablePort: '/Enable' + /* Outputs for Enabled SubSystem: '/MeasurementUpdate' incorporates: + * EnablePort: '/Enable' */ if (rtmIsMajorTimeStep((&tuning_nominal_M)) && (!tuning_nominal_DW.MeasurementUpdate_MODE)) { @@ -783,9 +748,9 @@ void tuning_nominalModelClass::step() if (tuning_nominal_DW.MeasurementUpdate_MODE) { for (i = 0; i < 3; i++) { - /* Product: '/C[k]*xhat[k|k-1]' incorporates: + /* Product: '/C[k]*xhat[k|k-1]' incorporates: * Constant: '/C' - * Sum: '/Add1' + * Sum: '/Add1' */ rtb_Sum1_l[i] = 0.0; for (i_0 = 0; i_0 < 6; i_0++) { @@ -793,18 +758,18 @@ void tuning_nominalModelClass::step() rtb_MemoryX[i_0]; } - /* End of Product: '/C[k]*xhat[k|k-1]' */ + /* End of Product: '/C[k]*xhat[k|k-1]' */ - /* Sum: '/Sum' incorporates: + /* Sum: '/Sum' incorporates: * Inport: '/X' - * Product: '/Product3' - * Sum: '/Add1' + * Product: '/Product3' + * Sum: '/Add1' */ rtb_Add_f[i] = tuning_nominal_U.X[9 + i] - rtb_Sum1_l[i]; } - /* Product: '/Product3' incorporates: - * Constant: '/KalmanGainL' + /* Product: '/Product3' incorporates: + * Constant: '/KalmanGainL' */ for (i = 0; i < 6; i++) { tuning_nominal_B.Product3[i] = 0.0; @@ -817,11 +782,11 @@ void tuning_nominalModelClass::step() } } - /* End of Outputs for SubSystem: '/MeasurementUpdate' */ + /* End of Outputs for SubSystem: '/MeasurementUpdate' */ for (i = 0; i < 6; i++) { - /* Product: '/A[k]*xhat[k|k-1]' incorporates: + /* Product: '/A[k]*xhat[k|k-1]' incorporates: * Constant: '/A' - * Sum: '/Add' + * Sum: '/Add' */ rtb_LOE_out[i] = 0.0; for (i_0 = 0; i_0 < 6; i_0++) { @@ -829,14 +794,14 @@ void tuning_nominalModelClass::step() rtb_MemoryX[i_0]; } - /* End of Product: '/A[k]*xhat[k|k-1]' */ + /* End of Product: '/A[k]*xhat[k|k-1]' */ - /* Sum: '/Add' */ + /* Sum: '/Add' */ rtb_Add[i] = rtb_LOE_out[i] + tuning_nominal_B.Product3[i]; } } - /* Sum: '/Sum' incorporates: + /* Sum: '/Sum' incorporates: * Inport: '/Y0' */ rtb_ref_idx_0 -= tuning_nominal_U.Y0[0]; @@ -844,7 +809,7 @@ void tuning_nominalModelClass::step() rtb_ff_idx_1 -= tuning_nominal_U.Y0[2]; rtb_Clock = rtb_ref_idx_3 - tuning_nominal_U.Y0[3]; - /* Saturate: '/x' */ + /* Saturate: '/x' */ if (rtb_ref_idx_0 > 2.0) { rtb_ref_idx_0 = 2.0; } else { @@ -853,12 +818,12 @@ void tuning_nominalModelClass::step() } } - /* End of Saturate: '/x' */ + /* End of Saturate: '/x' */ - /* Sum: '/Sum1' */ + /* Sum: '/Sum1' */ tuning_nominal_B.Sum1 = rtb_ref_idx_0 - rtb_d_x; - /* Saturate: '/y' */ + /* Saturate: '/y' */ if (rtb_ff_idx_0 > 2.0) { rtb_ff_idx_0 = 2.0; } else { @@ -867,12 +832,12 @@ void tuning_nominalModelClass::step() } } - /* End of Saturate: '/y' */ + /* End of Saturate: '/y' */ - /* Sum: '/Sum4' */ + /* Sum: '/Sum4' */ tuning_nominal_B.Sum4 = rtb_ff_idx_0 - rtb_d_y; - /* Saturate: '/yaw' */ + /* Saturate: '/yaw' */ if (rtb_Clock > 3.1415926535897931) { rtb_Clock = 3.1415926535897931; } else { @@ -881,12 +846,12 @@ void tuning_nominalModelClass::step() } } - /* End of Saturate: '/yaw' */ + /* End of Saturate: '/yaw' */ - /* Sum: '/Sum3' */ - tuning_nominal_B.Sum3_n = rtb_Clock - rtb_d_psi; + /* Sum: '/Sum3' */ + tuning_nominal_B.Sum3_f = rtb_Clock - rtb_d_psi; - /* Saturate: '/z' */ + /* Saturate: '/z' */ if (rtb_ff_idx_1 > 1.75) { rtb_ff_idx_1 = 1.75; } else { @@ -895,16 +860,15 @@ void tuning_nominalModelClass::step() } } - /* End of Saturate: '/z' */ + /* End of Saturate: '/z' */ - /* Sum: '/Sum3' */ - tuning_nominal_B.Sum3_h = rtb_ff_idx_1 - rtb_d_z; + /* Sum: '/Sum3' */ + tuning_nominal_B.Sum3_n = rtb_ff_idx_1 - rtb_d_z; } if (rtmIsMajorTimeStep((&tuning_nominal_M))) { int32_T i; - if (rtmIsMajorTimeStep((&tuning_nominal_M)) && - (&tuning_nominal_M)->Timing.TaskCounters.TID[1] == 0) { + if (rtmIsMajorTimeStep((&tuning_nominal_M))) { /* Update for Delay: '/MemoryX' */ tuning_nominal_DW.icLoad = 0U; for (i = 0; i < 6; i++) { @@ -937,9 +901,9 @@ void tuning_nominalModelClass::step() ->solverInfo); { - /* Update absolute timer for sample time: [0.001s, 0.0s] */ + /* Update absolute timer for sample time: [0.005s, 0.0s] */ /* The "clockTick1" counts the number of times the code of this task has - * been executed. The resolution of this integer timer is 0.001, which is the step size + * been executed. The resolution of this integer timer is 0.005, which is the step size * of the task. Size of "clockTick1" ensures timer will not overflow during the * application lifespan selected. * Timer of this task consists of two 32 bit unsigned integers. @@ -951,8 +915,6 @@ void tuning_nominalModelClass::step() (&tuning_nominal_M)->Timing.clockTickH1++; } } - - rate_scheduler((&tuning_nominal_M)); } /* end MajorTimeStep */ } @@ -962,17 +924,17 @@ void tuning_nominalModelClass::tuning_nominal_derivatives() XDot_tuning_nominal_T *_rtXdot; _rtXdot = ((XDot_tuning_nominal_T *) (&tuning_nominal_M)->derivs); - /* Derivatives for Integrator: '/Integrator1' */ - _rtXdot->Integrator1_CSTATE = tuning_nominal_B.Sum3_h; + /* Derivatives for Integrator: '/Integrator1' */ + _rtXdot->Integrator1_CSTATE = tuning_nominal_B.Sum3_n; - /* Derivatives for Integrator: '/Integrator1' */ - _rtXdot->Integrator1_CSTATE_d = tuning_nominal_B.Sum1; + /* Derivatives for Integrator: '/Integrator1' */ + _rtXdot->Integrator1_CSTATE_f = tuning_nominal_B.Sum1; - /* Derivatives for Integrator: '/Integrator' */ + /* Derivatives for Integrator: '/Integrator' */ _rtXdot->Integrator_CSTATE = tuning_nominal_B.Sum4; - /* Derivatives for Integrator: '/Integrator1' */ - _rtXdot->Integrator1_CSTATE_j = tuning_nominal_B.Sum3_n; + /* Derivatives for Integrator: '/Integrator1' */ + _rtXdot->Integrator1_CSTATE_a = tuning_nominal_B.Sum3_f; } /* Model initialize function */ @@ -1018,7 +980,7 @@ void tuning_nominalModelClass::initialize() &(&tuning_nominal_M)->intgData); rtsiSetSolverName(&(&tuning_nominal_M)->solverInfo,"ode4"); rtmSetTPtr(getRTM(), &(&tuning_nominal_M)->Timing.tArray[0]); - (&tuning_nominal_M)->Timing.stepSize0 = 0.001; + (&tuning_nominal_M)->Timing.stepSize0 = 0.005; /* block I/O */ (void) memset(((void *) &tuning_nominal_B), 0, @@ -1044,38 +1006,38 @@ void tuning_nominalModelClass::initialize() { int32_T i; - /* InitializeConditions for Integrator: '/Integrator1' */ + /* InitializeConditions for Integrator: '/Integrator1' */ tuning_nominal_X.Integrator1_CSTATE = 0.0; - /* InitializeConditions for Integrator: '/Integrator1' */ - tuning_nominal_X.Integrator1_CSTATE_d = 0.0; + /* InitializeConditions for Integrator: '/Integrator1' */ + tuning_nominal_X.Integrator1_CSTATE_f = 0.0; - /* InitializeConditions for Integrator: '/Integrator' */ + /* InitializeConditions for Integrator: '/Integrator' */ tuning_nominal_X.Integrator_CSTATE = 0.0; - /* InitializeConditions for Integrator: '/Integrator1' */ - tuning_nominal_X.Integrator1_CSTATE_j = 0.0; + /* InitializeConditions for Integrator: '/Integrator1' */ + tuning_nominal_X.Integrator1_CSTATE_a = 0.0; /* InitializeConditions for Delay: '/MemoryX' */ tuning_nominal_DW.icLoad = 1U; - /* SystemInitialize for Enabled SubSystem: '/MeasurementUpdate' */ + /* SystemInitialize for Enabled SubSystem: '/MeasurementUpdate' */ /* SystemInitialize for Atomic SubSystem: '/UseCurrentEstimator' */ - /* SystemInitialize for Enabled SubSystem: '/Enabled Subsystem' */ + /* SystemInitialize for Enabled SubSystem: '/Enabled Subsystem' */ for (i = 0; i < 6; i++) { /* InitializeConditions for Memory: '/Memory' */ tuning_nominal_DW.Memory_PreviousInput[i] = 2.5179000000000005; - /* SystemInitialize for Outport: '/deltax' */ + /* SystemInitialize for Outport: '/deltax' */ tuning_nominal_B.Product2[i] = 0.0; - /* SystemInitialize for Outport: '/L*(y[k]-yhat[k|k-1])' */ + /* SystemInitialize for Outport: '/L*(y[k]-yhat[k|k-1])' */ tuning_nominal_B.Product3[i] = 0.0; } - /* End of SystemInitialize for SubSystem: '/Enabled Subsystem' */ + /* End of SystemInitialize for SubSystem: '/Enabled Subsystem' */ /* End of SystemInitialize for SubSystem: '/UseCurrentEstimator' */ - /* End of SystemInitialize for SubSystem: '/MeasurementUpdate' */ + /* End of SystemInitialize for SubSystem: '/MeasurementUpdate' */ } } diff --git a/src/tuning_nominal_grt_rtw/tuning_nominal.h b/src/tuning_nominal_grt_rtw/tuning_nominal.h index ffba357..73a7a83 100644 --- a/src/tuning_nominal_grt_rtw/tuning_nominal.h +++ b/src/tuning_nominal_grt_rtw/tuning_nominal.h @@ -7,9 +7,9 @@ * * Code generation for model "tuning_nominal". * - * Model version : 1.1498 + * Model version : 1.1505 * Simulink Coder version : 8.12 (R2017a) 16-Feb-2017 - * C++ source code generated on : Wed Oct 10 17:23:02 2018 + * C++ source code generated on : Thu Oct 11 16:37:28 2018 * * Target selection: grt.tlc * Note: GRT includes extra infrastructure and instrumentation for prototyping @@ -149,18 +149,17 @@ /* Block signals (auto storage) */ typedef struct { - real_T Sum2; /* '/Sum2' */ - real_T Sum3; /* '/Sum3' */ - real_T T_outer[3]; /* '/T_outer' */ - real_T Fcn1; /* '/Fcn1' */ - real_T Fcn; /* '/Fcn' */ + real_T Sum2; /* '/Sum2' */ + real_T Sum3; /* '/Sum3' */ + real_T Fcn1; /* '/Fcn1' */ + real_T Fcn; /* '/Fcn' */ real_T u[6]; /* '/ ' */ - real_T Sum1; /* '/Sum1' */ - real_T Sum4; /* '/Sum4' */ - real_T Sum3_n; /* '/Sum3' */ - real_T Sum3_h; /* '/Sum3' */ - real_T Product2[6]; /* '/Product2' */ - real_T Product3[6]; /* '/Product3' */ + real_T Sum1; /* '/Sum1' */ + real_T Sum4; /* '/Sum4' */ + real_T Sum3_f; /* '/Sum3' */ + real_T Sum3_n; /* '/Sum3' */ + real_T Product2[6]; /* '/Product2' */ + real_T Product3[6]; /* '/Product3' */ } B_tuning_nominal_T; /* Block states (auto storage) for system '' */ @@ -168,32 +167,32 @@ typedef struct { real_T MemoryX_DSTATE[6]; /* '/MemoryX' */ real_T Memory_PreviousInput[6]; /* '/Memory' */ uint8_T icLoad; /* '/MemoryX' */ - boolean_T MeasurementUpdate_MODE; /* '/MeasurementUpdate' */ - boolean_T EnabledSubsystem_MODE; /* '/Enabled Subsystem' */ + boolean_T MeasurementUpdate_MODE; /* '/MeasurementUpdate' */ + boolean_T EnabledSubsystem_MODE; /* '/Enabled Subsystem' */ } DW_tuning_nominal_T; /* Continuous states (auto storage) */ typedef struct { - real_T Integrator1_CSTATE; /* '/Integrator1' */ - real_T Integrator1_CSTATE_d; /* '/Integrator1' */ - real_T Integrator_CSTATE; /* '/Integrator' */ - real_T Integrator1_CSTATE_j; /* '/Integrator1' */ + real_T Integrator1_CSTATE; /* '/Integrator1' */ + real_T Integrator1_CSTATE_f; /* '/Integrator1' */ + real_T Integrator_CSTATE; /* '/Integrator' */ + real_T Integrator1_CSTATE_a; /* '/Integrator1' */ } X_tuning_nominal_T; /* State derivatives (auto storage) */ typedef struct { - real_T Integrator1_CSTATE; /* '/Integrator1' */ - real_T Integrator1_CSTATE_d; /* '/Integrator1' */ - real_T Integrator_CSTATE; /* '/Integrator' */ - real_T Integrator1_CSTATE_j; /* '/Integrator1' */ + real_T Integrator1_CSTATE; /* '/Integrator1' */ + real_T Integrator1_CSTATE_f; /* '/Integrator1' */ + real_T Integrator_CSTATE; /* '/Integrator' */ + real_T Integrator1_CSTATE_a; /* '/Integrator1' */ } XDot_tuning_nominal_T; /* State disabled */ typedef struct { - boolean_T Integrator1_CSTATE; /* '/Integrator1' */ - boolean_T Integrator1_CSTATE_d; /* '/Integrator1' */ - boolean_T Integrator_CSTATE; /* '/Integrator' */ - boolean_T Integrator1_CSTATE_j; /* '/Integrator1' */ + boolean_T Integrator1_CSTATE; /* '/Integrator1' */ + boolean_T Integrator1_CSTATE_f; /* '/Integrator1' */ + boolean_T Integrator_CSTATE; /* '/Integrator' */ + boolean_T Integrator1_CSTATE_a; /* '/Integrator1' */ } XDis_tuning_nominal_T; #ifndef ODE4_INTG @@ -225,7 +224,7 @@ typedef struct { real_T Constant1_Value[9]; /* Expression: pInitialization.M - * Referenced by: '/KalmanGainM' + * Referenced by: '/KalmanGainM' */ real_T KalmanGainM_Value[18]; @@ -240,7 +239,7 @@ typedef struct { real_T A_Value[36]; /* Expression: pInitialization.L - * Referenced by: '/KalmanGainL' + * Referenced by: '/KalmanGainL' */ real_T KalmanGainL_Value[18]; } ConstP_tuning_nominal_T; @@ -309,14 +308,10 @@ struct tag_RTM_tuning_nominal_T { time_T stepSize0; uint32_T clockTick1; uint32_T clockTickH1; - struct { - uint8_T TID[3]; - } TaskCounters; - SimTimeStep simTimeStep; boolean_T stopRequestedFlag; time_T *t; - time_T tArray[3]; + time_T tArray[2]; } Timing; }; @@ -384,20 +379,28 @@ class tuning_nominalModelClass { /*- * These blocks were eliminated from the model due to optimizations: * - * Block '/ConstantP' : Unused code path elimination - * Block '/CovarianceZ' : Unused code path elimination - * Block '/Data Type Duplicate' : Unused code path elimination - * Block '/Data Type Duplicate' : Unused code path elimination - * Block '/Conversion' : Unused code path elimination - * Block '/Data Type Duplicate' : Unused code path elimination - * Block '/Conversion' : Unused code path elimination - * Block '/Data Type Duplicate' : Unused code path elimination - * Block '/Add1' : Unused code path elimination - * Block '/Product' : Unused code path elimination - * Block '/Product1' : Unused code path elimination + * Block '/ConstantP' : Unused code path elimination + * Block '/CovarianceZ' : Unused code path elimination + * Block '/Data Type Duplicate' : Unused code path elimination + * Block '/Data Type Duplicate' : Unused code path elimination + * Block '/Conversion' : Unused code path elimination + * Block '/Data Type Duplicate' : Unused code path elimination + * Block '/Conversion' : Unused code path elimination + * Block '/Data Type Duplicate' : Unused code path elimination + * Block '/Add1' : Unused code path elimination + * Block '/Product' : Unused code path elimination + * Block '/Product1' : Unused code path elimination + * Block '/Data Type Duplicate' : Unused code path elimination + * Block '/Data Type Duplicate' : Unused code path elimination + * Block '/Data Type Duplicate' : Unused code path elimination + * Block '/Data Type Duplicate' : Unused code path elimination + * Block '/Conversion' : Unused code path elimination * Block '/Data Type Duplicate' : Unused code path elimination + * Block '/Conversion' : Unused code path elimination * Block '/Data Type Duplicate' : Unused code path elimination + * Block '/Conversion' : Unused code path elimination * Block '/Data Type Duplicate' : Unused code path elimination + * Block '/Conversion' : Unused code path elimination * Block '/Data Type Duplicate' : Unused code path elimination * Block '/Conversion' : Unused code path elimination * Block '/Data Type Duplicate' : Unused code path elimination @@ -407,17 +410,9 @@ class tuning_nominalModelClass { * Block '/Data Type Duplicate' : Unused code path elimination * Block '/Conversion' : Unused code path elimination * Block '/Data Type Duplicate' : Unused code path elimination - * Block '/Conversion' : Unused code path elimination * Block '/Data Type Duplicate' : Unused code path elimination - * Block '/Conversion' : Unused code path elimination * Block '/Data Type Duplicate' : Unused code path elimination - * Block '/Conversion' : Unused code path elimination * Block '/Data Type Duplicate' : Unused code path elimination - * Block '/Conversion' : Unused code path elimination - * Block '/Data Type Duplicate' : Unused code path elimination - * Block '/Data Type Duplicate' : Unused code path elimination - * Block '/Data Type Duplicate' : Unused code path elimination - * Block '/Data Type Duplicate' : Unused code path elimination * Block '/G' : Unused code path elimination * Block '/H' : Unused code path elimination * Block '/ManualSwitchPZ' : Unused code path elimination @@ -425,35 +420,36 @@ class tuning_nominalModelClass { * Block '/P0' : Unused code path elimination * Block '/Q' : Unused code path elimination * Block '/R' : Unused code path elimination - * Block '/Constant' : Unused code path elimination + * Block '/Constant' : Unused code path elimination * Block '/Reset' : Unused code path elimination * Block '/Reshapeyhat' : Unused code path elimination - * Block '/CheckSignalProperties' : Unused code path elimination + * Block '/CheckSignalProperties' : Unused code path elimination * Block '/ ' : Eliminated since input and output rates are identical * Block '/ ' : Eliminated since input and output rates are identical * Block '/ ' : Eliminated since input and output rates are identical - * Block '/Conversion' : Eliminate redundant data type conversion - * Block '/Conversion' : Eliminate redundant data type conversion - * Block '/Conversion' : Eliminate redundant data type conversion - * Block '/Conversion' : Eliminate redundant data type conversion - * Block '/Conversion' : Eliminate redundant data type conversion - * Block '/Conversion' : Eliminate redundant data type conversion + * Block '/Conversion' : Eliminate redundant data type conversion + * Block '/Conversion' : Eliminate redundant data type conversion + * Block '/Conversion' : Eliminate redundant data type conversion + * Block '/Conversion' : Eliminate redundant data type conversion + * Block '/Conversion' : Eliminate redundant data type conversion + * Block '/Conversion' : Eliminate redundant data type conversion * Block '/DataTypeConversionEnable' : Eliminate redundant data type conversion - * Block '/Conversion' : Eliminate redundant data type conversion - * Block '/Conversion' : Eliminate redundant data type conversion - * Block '/Conversion' : Eliminate redundant data type conversion - * Block '/Reshape' : Reshape block reduction + * Block '/Conversion' : Eliminate redundant data type conversion + * Block '/Conversion' : Eliminate redundant data type conversion + * Block '/Conversion' : Eliminate redundant data type conversion + * Block '/Reshape' : Reshape block reduction * Block '/ReshapeX0' : Reshape block reduction * Block '/Reshapeu' : Reshape block reduction * Block '/Reshapexhat' : Reshape block reduction * Block '/Reshapey' : Reshape block reduction + * Block '/T_outer' : Eliminated since input and output rates are identical + * Block '/Reshape' : Reshape block reduction + * Block '/Reshape1' : Reshape block reduction + * Block '/Reshape' : Reshape block reduction + * Block '/Reshape' : Reshape block reduction + * Block '/Reshape' : Reshape block reduction + * Block '/Reshape' : Reshape block reduction * Block '/Rate Transition' : Eliminated since input and output rates are identical - * Block '/Reshape' : Reshape block reduction - * Block '/Reshape1' : Reshape block reduction - * Block '/Reshape' : Reshape block reduction - * Block '/Reshape' : Reshape block reduction - * Block '/Reshape' : Reshape block reduction - * Block '/Reshape' : Reshape block reduction */ /*- @@ -474,64 +470,65 @@ class tuning_nominalModelClass { * '' : 'tuning_nominal/Actuator_Fault' * '' : 'tuning_nominal/FDD_Kalman' * '' : 'tuning_nominal/Kalman Filter' - * '' : 'tuning_nominal/Test_config_and_data' - * '' : 'tuning_nominal/Thrust2command' - * '' : 'tuning_nominal/XY State Feedback' - * '' : 'tuning_nominal/pitch_controller_SF' - * '' : 'tuning_nominal/roll_controller_SF' - * '' : 'tuning_nominal/yaw_controller' - * '' : 'tuning_nominal/z_controller' - * '' : 'tuning_nominal/FDD_Kalman/Cross Product' - * '' : 'tuning_nominal/FDD_Kalman/MATLAB Function1' - * '' : 'tuning_nominal/FDD_Kalman/Cross Product/Subsystem' - * '' : 'tuning_nominal/FDD_Kalman/Cross Product/Subsystem2' - * '' : 'tuning_nominal/Kalman Filter/CalculatePL' - * '' : 'tuning_nominal/Kalman Filter/CalculateYhat' - * '' : 'tuning_nominal/Kalman Filter/DataTypeConversionA' - * '' : 'tuning_nominal/Kalman Filter/DataTypeConversionB' - * '' : 'tuning_nominal/Kalman Filter/DataTypeConversionC' - * '' : 'tuning_nominal/Kalman Filter/DataTypeConversionD' - * '' : 'tuning_nominal/Kalman Filter/DataTypeConversionG' - * '' : 'tuning_nominal/Kalman Filter/DataTypeConversionH' - * '' : 'tuning_nominal/Kalman Filter/DataTypeConversionN' - * '' : 'tuning_nominal/Kalman Filter/DataTypeConversionP' - * '' : 'tuning_nominal/Kalman Filter/DataTypeConversionP0' - * '' : 'tuning_nominal/Kalman Filter/DataTypeConversionQ' - * '' : 'tuning_nominal/Kalman Filter/DataTypeConversionR' - * '' : 'tuning_nominal/Kalman Filter/DataTypeConversionReset' - * '' : 'tuning_nominal/Kalman Filter/DataTypeConversionX' - * '' : 'tuning_nominal/Kalman Filter/DataTypeConversionX0' - * '' : 'tuning_nominal/Kalman Filter/DataTypeConversionu' - * '' : 'tuning_nominal/Kalman Filter/MemoryP' - * '' : 'tuning_nominal/Kalman Filter/Observer' - * '' : 'tuning_nominal/Kalman Filter/ReducedQRN' - * '' : 'tuning_nominal/Kalman Filter/ScalarExpansionP0' - * '' : 'tuning_nominal/Kalman Filter/ScalarExpansionQ' - * '' : 'tuning_nominal/Kalman Filter/ScalarExpansionR' - * '' : 'tuning_nominal/Kalman Filter/UseCurrentEstimator' - * '' : 'tuning_nominal/Kalman Filter/checkA' - * '' : 'tuning_nominal/Kalman Filter/checkB' - * '' : 'tuning_nominal/Kalman Filter/checkC' - * '' : 'tuning_nominal/Kalman Filter/checkD' - * '' : 'tuning_nominal/Kalman Filter/checkEnable' - * '' : 'tuning_nominal/Kalman Filter/checkG' - * '' : 'tuning_nominal/Kalman Filter/checkH' - * '' : 'tuning_nominal/Kalman Filter/checkN' - * '' : 'tuning_nominal/Kalman Filter/checkP0' - * '' : 'tuning_nominal/Kalman Filter/checkQ' - * '' : 'tuning_nominal/Kalman Filter/checkR' - * '' : 'tuning_nominal/Kalman Filter/checkReset' - * '' : 'tuning_nominal/Kalman Filter/checkX0' - * '' : 'tuning_nominal/Kalman Filter/checku' - * '' : 'tuning_nominal/Kalman Filter/checky' - * '' : 'tuning_nominal/Kalman Filter/CalculatePL/DataTypeConversionL' - * '' : 'tuning_nominal/Kalman Filter/CalculatePL/DataTypeConversionM' - * '' : 'tuning_nominal/Kalman Filter/CalculatePL/DataTypeConversionP' - * '' : 'tuning_nominal/Kalman Filter/CalculatePL/DataTypeConversionZ' - * '' : 'tuning_nominal/Kalman Filter/Observer/MeasurementUpdate' - * '' : 'tuning_nominal/Kalman Filter/UseCurrentEstimator/Enabled Subsystem' - * '' : 'tuning_nominal/Test_config_and_data/FFW' - * '' : 'tuning_nominal/Test_config_and_data/LOE_' - * '' : 'tuning_nominal/Test_config_and_data/MATLAB Function' + * '' : 'tuning_nominal/Nominal_Controller' + * '' : 'tuning_nominal/Test_config_and_data' + * '' : 'tuning_nominal/Thrust2command' + * '' : 'tuning_nominal/FDD_Kalman/Cross Product' + * '' : 'tuning_nominal/FDD_Kalman/MATLAB Function1' + * '' : 'tuning_nominal/FDD_Kalman/Cross Product/Subsystem' + * '' : 'tuning_nominal/FDD_Kalman/Cross Product/Subsystem2' + * '' : 'tuning_nominal/Kalman Filter/CalculatePL' + * '' : 'tuning_nominal/Kalman Filter/CalculateYhat' + * '' : 'tuning_nominal/Kalman Filter/DataTypeConversionA' + * '' : 'tuning_nominal/Kalman Filter/DataTypeConversionB' + * '' : 'tuning_nominal/Kalman Filter/DataTypeConversionC' + * '' : 'tuning_nominal/Kalman Filter/DataTypeConversionD' + * '' : 'tuning_nominal/Kalman Filter/DataTypeConversionG' + * '' : 'tuning_nominal/Kalman Filter/DataTypeConversionH' + * '' : 'tuning_nominal/Kalman Filter/DataTypeConversionN' + * '' : 'tuning_nominal/Kalman Filter/DataTypeConversionP' + * '' : 'tuning_nominal/Kalman Filter/DataTypeConversionP0' + * '' : 'tuning_nominal/Kalman Filter/DataTypeConversionQ' + * '' : 'tuning_nominal/Kalman Filter/DataTypeConversionR' + * '' : 'tuning_nominal/Kalman Filter/DataTypeConversionReset' + * '' : 'tuning_nominal/Kalman Filter/DataTypeConversionX' + * '' : 'tuning_nominal/Kalman Filter/DataTypeConversionX0' + * '' : 'tuning_nominal/Kalman Filter/DataTypeConversionu' + * '' : 'tuning_nominal/Kalman Filter/MemoryP' + * '' : 'tuning_nominal/Kalman Filter/Observer' + * '' : 'tuning_nominal/Kalman Filter/ReducedQRN' + * '' : 'tuning_nominal/Kalman Filter/ScalarExpansionP0' + * '' : 'tuning_nominal/Kalman Filter/ScalarExpansionQ' + * '' : 'tuning_nominal/Kalman Filter/ScalarExpansionR' + * '' : 'tuning_nominal/Kalman Filter/UseCurrentEstimator' + * '' : 'tuning_nominal/Kalman Filter/checkA' + * '' : 'tuning_nominal/Kalman Filter/checkB' + * '' : 'tuning_nominal/Kalman Filter/checkC' + * '' : 'tuning_nominal/Kalman Filter/checkD' + * '' : 'tuning_nominal/Kalman Filter/checkEnable' + * '' : 'tuning_nominal/Kalman Filter/checkG' + * '' : 'tuning_nominal/Kalman Filter/checkH' + * '' : 'tuning_nominal/Kalman Filter/checkN' + * '' : 'tuning_nominal/Kalman Filter/checkP0' + * '' : 'tuning_nominal/Kalman Filter/checkQ' + * '' : 'tuning_nominal/Kalman Filter/checkR' + * '' : 'tuning_nominal/Kalman Filter/checkReset' + * '' : 'tuning_nominal/Kalman Filter/checkX0' + * '' : 'tuning_nominal/Kalman Filter/checku' + * '' : 'tuning_nominal/Kalman Filter/checky' + * '' : 'tuning_nominal/Kalman Filter/CalculatePL/DataTypeConversionL' + * '' : 'tuning_nominal/Kalman Filter/CalculatePL/DataTypeConversionM' + * '' : 'tuning_nominal/Kalman Filter/CalculatePL/DataTypeConversionP' + * '' : 'tuning_nominal/Kalman Filter/CalculatePL/DataTypeConversionZ' + * '' : 'tuning_nominal/Kalman Filter/Observer/MeasurementUpdate' + * '' : 'tuning_nominal/Kalman Filter/UseCurrentEstimator/Enabled Subsystem' + * '' : 'tuning_nominal/Nominal_Controller/XY State Feedback' + * '' : 'tuning_nominal/Nominal_Controller/pitch_controller_SF' + * '' : 'tuning_nominal/Nominal_Controller/roll_controller_SF' + * '' : 'tuning_nominal/Nominal_Controller/yaw_controller' + * '' : 'tuning_nominal/Nominal_Controller/z_controller' + * '' : 'tuning_nominal/Test_config_and_data/FFW' + * '' : 'tuning_nominal/Test_config_and_data/LOE_' + * '' : 'tuning_nominal/Test_config_and_data/MATLAB Function' */ #endif /* RTW_HEADER_tuning_nominal_h_ */ diff --git a/src/tuning_nominal_grt_rtw/tuning_nominal_data.cpp b/src/tuning_nominal_grt_rtw/tuning_nominal_data.cpp index 8c80510..6aa72ce 100644 --- a/src/tuning_nominal_grt_rtw/tuning_nominal_data.cpp +++ b/src/tuning_nominal_grt_rtw/tuning_nominal_data.cpp @@ -7,9 +7,9 @@ * * Code generation for model "tuning_nominal". * - * Model version : 1.1498 + * Model version : 1.1505 * Simulink Coder version : 8.12 (R2017a) 16-Feb-2017 - * C++ source code generated on : Wed Oct 10 17:23:02 2018 + * C++ source code generated on : Thu Oct 11 16:37:28 2018 * * Target selection: grt.tlc * Note: GRT includes extra infrastructure and instrumentation for prototyping @@ -46,11 +46,11 @@ const ConstP_tuning_nominal_T tuning_nominal_ConstP = { { 0.0347563, 0.0, 0.0, 0.0, 0.0458929, 0.0, 0.0, 0.0, 0.0977 }, /* Expression: pInitialization.M - * Referenced by: '/KalmanGainM' + * Referenced by: '/KalmanGainM' */ - { 0.0013828503617152289, 0.0, 0.0, 0.000956799230965245, 0.0, 0.0, 0.0, - 0.001382850361721501, 0.0, 0.0, 0.00095679923096881356, 0.0, 0.0, 0.0, - 0.00055009940149860072, 0.0, 0.0, 0.00015134630638216127 }, + { 0.0068951521934753892, 0.0, 0.0, 0.00477077420942562, 0.0, 0.0, 0.0, + 0.0068951521934776322, 0.0, 0.0, 0.0047707742094272752, 0.0, 0.0, 0.0, + 0.0027474723704753777, 0.0, 0.0, 0.00075589920585899834 }, /* Expression: pInitialization.C * Referenced by: '/C' @@ -62,13 +62,13 @@ const ConstP_tuning_nominal_T tuning_nominal_ConstP = { * Referenced by: '/A' */ { 1.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1.0, - 0.0, 0.0, 0.0, 0.001, 0.0, 0.0, 1.0, 0.0, 0.0, 0.0, 0.001, 0.0, 0.0, 1.0, - 0.0, 0.0, 0.0, 0.001, 0.0, 0.0, 1.0 }, + 0.0, 0.0, 0.0, 0.005, 0.0, 0.0, 1.0, 0.0, 0.0, 0.0, 0.005, 0.0, 0.0, 1.0, + 0.0, 0.0, 0.0, 0.005, 0.0, 0.0, 1.0 }, /* Expression: pInitialization.L - * Referenced by: '/KalmanGainL' + * Referenced by: '/KalmanGainL' */ - { 0.001383807160946194, 0.0, 0.0, 0.000956799230965245, 0.0, 0.0, 0.0, - 0.0013838071609524698, 0.0, 0.0, 0.00095679923096881356, 0.0, 0.0, 0.0, - 0.00055025074780498283, 0.0, 0.0, 0.00015134630638216127 } + { 0.0069190060645225177, 0.0, 0.0, 0.00477077420942562, 0.0, 0.0, 0.0, + 0.0069190060645247693, 0.0, 0.0, 0.0047707742094272752, 0.0, 0.0, 0.0, + 0.0027512518665046728, 0.0, 0.0, 0.00075589920585899834 } }; diff --git a/src/tuning_nominal_grt_rtw/tuning_nominal_private.h b/src/tuning_nominal_grt_rtw/tuning_nominal_private.h index 28fdfaa..6ba1863 100644 --- a/src/tuning_nominal_grt_rtw/tuning_nominal_private.h +++ b/src/tuning_nominal_grt_rtw/tuning_nominal_private.h @@ -7,9 +7,9 @@ * * Code generation for model "tuning_nominal". * - * Model version : 1.1498 + * Model version : 1.1505 * Simulink Coder version : 8.12 (R2017a) 16-Feb-2017 - * C++ source code generated on : Wed Oct 10 17:23:02 2018 + * C++ source code generated on : Thu Oct 11 16:37:28 2018 * * Target selection: grt.tlc * Note: GRT includes extra infrastructure and instrumentation for prototyping diff --git a/src/tuning_nominal_grt_rtw/tuning_nominal_types.h b/src/tuning_nominal_grt_rtw/tuning_nominal_types.h index c3c11d1..29453bd 100644 --- a/src/tuning_nominal_grt_rtw/tuning_nominal_types.h +++ b/src/tuning_nominal_grt_rtw/tuning_nominal_types.h @@ -7,9 +7,9 @@ * * Code generation for model "tuning_nominal". * - * Model version : 1.1498 + * Model version : 1.1505 * Simulink Coder version : 8.12 (R2017a) 16-Feb-2017 - * C++ source code generated on : Wed Oct 10 17:23:02 2018 + * C++ source code generated on : Thu Oct 11 16:37:28 2018 * * Target selection: grt.tlc * Note: GRT includes extra infrastructure and instrumentation for prototyping diff --git a/src/tuning_nominal_node.cpp b/src/tuning_nominal_node.cpp index ddf897f..9835cbb 100644 --- a/src/tuning_nominal_node.cpp +++ b/src/tuning_nominal_node.cpp @@ -19,6 +19,7 @@ tuning_nominalModelClass gController; +bool gFirst_odom; bool gPublish; bool gInit_flag; bool gLanding_flag; @@ -35,7 +36,7 @@ Eigen::VectorXd gThrust_measure(6); gsft_control::EigenOdometry gOdometry; -void controller_dyn_callback(gsft_control::controllerDynConfig &config, uint32_t level) { +void controller_dyn_callback(gsft_control::controllerDynConfig &config, uint32_t level) { // callback if there is a change in dyn_reconfigure if (config.RESET) { // TO DO : reset parameters, gains config.RESET = false; @@ -67,8 +68,8 @@ void controller_dyn_callback(gsft_control::controllerDynConfig &config, uint32_t gTest_mode = config.test_mode; - if (config.enable_take_off && !gInit_flag){ // only once - gY0[0] = gOdometry.position.x(); + if (config.enable_take_off && !gInit_flag && gFirst_odom){ // only once + gY0[0] = gOdometry.position.x(); // First position before take off gY0[1] = gOdometry.position.y(); gY0[2] = gOdometry.position.z(); gY0[3] = gPsi; @@ -111,7 +112,8 @@ void controller_dyn_callback(gsft_control::controllerDynConfig &config, uint32_t gLOE_t[5] = config.LOE_t6; gInit_flag = true; - ROS_INFO("Take-off Request: %s with Test_mode = %d",config.enable_take_off?"True":"False",gTest_mode); + ROS_INFO("Test_mode = %d",gTest_mode); + ROS_INFO("Take-off Request: %s at x_ref = %f, y_ref = %f, z_ref = %f, psi_ref = %f(deg)",config.enable_take_off?"True":"False",gRef[0],gRef[1],gRef[2],gRef[3]*180.0/gsft_control::kDefaultPi); config.enable_take_off = false; @@ -123,7 +125,7 @@ void controller_dyn_callback(gsft_control::controllerDynConfig &config, uint32_t gRef[3] = gPsi; gTest_mode = gsft_control::controllerDyn_TEST_MANUAL; gLanding_flag = true; - ROS_INFO("Landing Request: %s at x_ref = %f, y_ref = %f,psi_ref = %f(deg)",config.enable_landing?"True":"False",gRef[0],gRef[1],gRef[3]*180.0/gsft_control::kDefaultPi); + ROS_INFO("Landing Request: %s at x_ref = %f, y_ref = %f, z_ref = %f, psi_ref = %f(deg)",config.enable_landing?"True":"False",gRef[0],gRef[1],gRef[2],gRef[3]*180.0/gsft_control::kDefaultPi); config.enable_landing = false; } else if (gTest_mode == gsft_control::controllerDyn_TEST_MANUAL){ @@ -132,7 +134,7 @@ void controller_dyn_callback(gsft_control::controllerDynConfig &config, uint32_t gRef[1] = config.ref_y; gRef[2] = config.ref_z; gRef[3] = config.ref_yaw_deg*gsft_control::kDefaultPi/180.0; - ROS_INFO("Waypoint Request: x_ref = %f, y_ref = %f, z_ref = %f, psi_ref = %f(deg)",gRef[0],gRef[1],gRef[2],gRef[3]); + ROS_INFO("Waypoint Request: x_ref = %f, y_ref = %f, z_ref = %f, psi_ref = %f(deg)",gRef[0],gRef[1],gRef[2],gRef[3]*180.0/gsft_control::kDefaultPi); config.send_waypoint = false; } } @@ -162,6 +164,9 @@ void OdometryCallback(const nav_msgs::OdometryConstPtr& odom_msg) { gEmergency_status = true; } } + if (!gFirst_odom){ + ROS_INFO("First odometry: x = %f, y = %f, z = %f",gOdometry.position.x(),gOdometry.position.y(),gOdometry.position.z()); + gFirst_odom = true;} } /*void LostControlCallback(const gsft_control::LOEConstPtr& loe_msg) { @@ -172,13 +177,13 @@ void OdometryCallback(const nav_msgs::OdometryConstPtr& odom_msg) { "]: ---------- TND LOE0 = " << gLOE[0] << std::endl; }*/ -// ASCTEC test +// ASCTEC test: command 0..200 void MotorSpeedCallback(const asctec_hl_comm::MotorSpeedConstPtr& motor_msg) { gsft_control::motorMsg2Thrust(motor_msg,&gThrust_measure); //ROS_INFO("Thrust T1 = %f(N)",gThrust_measure[0]); } -// Gazebo +// Gazebo: command rad/sec void MotorSpeedGazeboCallback(const mav_msgs::ActuatorsConstPtr& motor_msg) { gsft_control::speedMsg2Thrust(motor_msg,&gThrust_measure); // ROS_INFO("Thrust T1 = %f(N)",gThrust_measure[0]); @@ -232,7 +237,8 @@ int main(int argc, char** argv) { ros::Rate r(1000); - gPublish = false; + gFirst_odom = false; + gPublish = false; gInit_flag = false; gLanding_flag = false; gEmergency_status = false; @@ -277,6 +283,10 @@ int main(int argc, char** argv) { gPsi = euler_angles[2]; // gPsi = gOdometry.getYaw(); // same result + if (gFirst_odom && !controller_active){ + server.setCallback(f); + } + if (gInit_flag && !controller_active) { // only once when controller is not actived gController.initialize(); controller_active = true; @@ -287,9 +297,9 @@ int main(int argc, char** argv) { for (unsigned int i=0; i< 6; i++) { gController.tuning_nominal_U.LOE_t[i] = gLOE_t[i]; // fault time } - for (unsigned int i=0; i< 19; i++) { + /*for (unsigned int i=0; i< 19; i++) { ROS_INFO("Controller gain k[%d] = %f",i,gGain[i]); - } + }*/ } if (controller_active) { // controller active after take-off request @@ -402,6 +412,7 @@ int main(int argc, char** argv) { uav_state_msg->rotation_speed_B.x = gOdometry.angular_velocity.x(); uav_state_msg->rotation_speed_B.y = gOdometry.angular_velocity.y(); uav_state_msg->rotation_speed_B.z = gOdometry.angular_velocity.z(); + uav_state_msg->total_thrust = gController.tuning_nominal_Y.virtual_control[0]; uav_state_msg->moment.x = gController.tuning_nominal_Y.virtual_control[1]; uav_state_msg->moment.y = gController.tuning_nominal_Y.virtual_control[2];