guide

CMakeLists.txt

@@ -141,8 +141,8 @@ add_library(matlab_simulink
   src/tuning_nominal_grt_rtw/tuning_nominal_data.cpp
   src/tuning_nominal_grt_rtw/tuning_nominal.cpp
 
-#  src/tuning_GS1_grt_rtw/tuning_GS1_data.cpp
-#  src/tuning_GS1_grt_rtw/tuning_GS1.cpp
+  src/tuning_GS1_grt_rtw/tuning_GS1_data.cpp
+  src/tuning_GS1_grt_rtw/tuning_GS1.cpp
 
   #src/tuning_nominal_grt_rtw/rtGetInf.cpp
   #src/tuning_nominal_grt_rtw/rtGetNaN.cpp
@@ -169,18 +169,18 @@ target_link_libraries(tuning_nominal_node
 )
 
 
-#add_executable(tuning_GS1_node
-#    src/tuning_GS1_node.cpp
-#)
-#add_dependencies(tuning_GS1_node
-#    ${${PROJECT_NAME}_EXPORTED_TARGETS}
-#    ${catkin_EXPORTED_TARGETS}
-#    matlab_simulink
-#)
-#target_link_libraries(tuning_GS1_node
-#    matlab_simulink
-#    ${catkin_LIBRARIES}
-#)
+add_executable(tuning_GS1_node
+    src/tuning_GS1_node.cpp
+)
+add_dependencies(tuning_GS1_node
+    ${${PROJECT_NAME}_EXPORTED_TARGETS}
+    ${catkin_EXPORTED_TARGETS}
+    matlab_simulink
+)
+target_link_libraries(tuning_GS1_node
+    matlab_simulink
+    ${catkin_LIBRARIES}
+)
 
 #############
 ## Install ##

README.md

@@ -37,42 +37,62 @@ doi: 10.1109/ICUAS.2017.7991516
 }
 ```
 
-## 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
+# Test guide
+## Gazebo Clock
+Gazebo -> World -> Physics OR rqt -> gazebo dynamic_reconfigure (appears after unpaused)
+  * `max_step_size` (Gazebo) or `time_step` (rqt): for example max_step_size = 0.01sec <=> max_step_freq = 100Hz
+    + gazebo object topics at 100Hz (/imu, /motor_speed, ...)
+    + posibility to publish messages at maximum 2*100Hz = 200Hz (/command/motor_speed, ...)
+  * `real_time_update_rate` (Gazebo) or `max_update_rate` (rqt): 0 - 1KHZ => gazebo video speed
+  * `time_factor` = `real_time_update_rate/max_step_freq`
+    + real_time_update_rate = max_step_freq: time_factor = 1 => normal gazebo video speed (sim time = real time)
+    + real_time_update_rate < max_step_freq: time_factor < 1 => slow gazebo video speed (sim time < real time)
+    + real_time_update_rate > max_step_freq: incorrect dynamic simulation, can cause crash if time_factor too big
+
+## Gazebo test
+  * 1 mav test
+      + Gazebo
+        - `roslaunch rotors_gazebo mav.launch`
+        - change max_step_size = 0.001, then real_time_update_rate = 1000Hz (must after max_step_size)
+      + Controller
+        - `roslaunch gsft_control tuning_nominal_Gazebo.launch`
+        - dynamic reconfigure "firefly/controller": mode, gain, LOE, ...
+      + Run
+        - Gazebo: click run => saw first odometry mesage
+        - Controller: click enable_take_off
+  * 2 mav compare test: same as 1 mav test except
+      + `mav_2.launch` and `tuning_compare_Gazebo.launch`
+      + click run (Gazebo) after chose enable_take_off (to compare 2 mav but can not click 2 enable_take_off buttons at the same time)
+
+## Gazebo videos
+  * Slow video speed: real time_factor is lowered (machine cannot handle computing with the given parameters)
+    + 1 mav: max_step_freq = real_time_update_rate = 1kHz but time factor = 0.5 => 1/2 slow video speed
+    + 2 mav: max_step_freq = real_time_update_rate = 1kHz but time factor = 0.25 => 1/4 slow video speed
+  * 1 mav test
+    + Solution 1: test performed at 500Hz (both Gazebo and controller)
+        - max_step_size = 0.002, real_time_update_rate = 500Hz
+        - modifying Simulink code generation Ts = 1/500 and ros::Rate r(500)
+    + Solution 2: Gazebo at 500Hz but controller still at 1kHz  
+        - max_step_size = 0.002 and real_time_update_rate = 500Hz => /imu, /odometry, ... available at 500Hz
+        - Ts = 1/1000 and ros::Rate r(1000) => still able to publish /motor_speed at 1kHz (see above)
+        - degraded performance controller but still acceptable
+  * 2 mav test: test have to be performed at < 250Hz (both Gazebo and controller) because of big degraded performance if Gazebo at max freq 250Hz but controller still at 1kHz
+
+## 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
+  * Vicon server: calibration (at least once a day), chose firefly object, 200Hz
+  * Asctec Firefly
       + git pull and build
-      + roslaunch: fcu, msf, controller
-      + check frequencies: Vicon 200HZ, other (imu, msf, controller) 1KHz
+      + **kill default autopilot rosnodes (clearpath)**
+      + launch: fcu, msf, controller
+  * Ground Station: ros_vrpn_client mrasl_vicon.launch, rqt => init filter and !!! verify filter output !!!
+  * Test
+      + check frequencies: Vicon -> 200HZ, state and command (imu, odometry, /fcu/command/direct_motor) -> 1kHz
+      + **check battery (fcu/status)**
+      + **check manual mode before turn on motor**, rosbag record
+      + **check error (fcu, msf)**
+      + **check /fcu/command/direct_motor = 0**
+      + pass to automatic mode then click enable_take_off

src/tuning_GS1_grt_rtw/multiword_types.h

@@ -9,7 +9,7 @@
  *
  * Model version              : 1.1576
  * Simulink Coder version : 8.12 (R2017a) 16-Feb-2017
- * C++ source code generated on : Wed Oct 10 17:23:31 2018
+ * C++ source code generated on : Fri Oct 12 17:48:55 2018
  *
  * Target selection: grt.tlc
  * Note: GRT includes extra infrastructure and instrumentation for prototyping

src/tuning_GS1_grt_rtw/rtmodel.h

@@ -9,7 +9,7 @@
  *
  * Model version              : 1.1576
  * Simulink Coder version : 8.12 (R2017a) 16-Feb-2017
- * C++ source code generated on : Wed Oct 10 17:23:31 2018
+ * C++ source code generated on : Fri Oct 12 17:48:55 2018
  *
  * Target selection: grt.tlc
  * Note: GRT includes extra infrastructure and instrumentation for prototyping

src/tuning_GS1_grt_rtw/rtwtypes.h

@@ -9,7 +9,7 @@
  *
  * Model version              : 1.1576
  * Simulink Coder version : 8.12 (R2017a) 16-Feb-2017
- * C++ source code generated on : Wed Oct 10 17:23:31 2018
+ * C++ source code generated on : Fri Oct 12 17:48:55 2018
  *
  * Target selection: grt.tlc
  * Note: GRT includes extra infrastructure and instrumentation for prototyping

src/tuning_GS1_grt_rtw/tuning_GS1.cpp

@@ -9,7 +9,7 @@
  *
  * Model version              : 1.1576
  * Simulink Coder version : 8.12 (R2017a) 16-Feb-2017
- * C++ source code generated on : Wed Oct 10 17:23:31 2018
+ * C++ source code generated on : Fri Oct 12 17:48:55 2018
  *
  * Target selection: grt.tlc
  * Note: GRT includes extra infrastructure and instrumentation for prototyping

src/tuning_GS1_grt_rtw/tuning_GS1.h

@@ -9,7 +9,7 @@
  *
  * Model version              : 1.1576
  * Simulink Coder version : 8.12 (R2017a) 16-Feb-2017
- * C++ source code generated on : Wed Oct 10 17:23:31 2018
+ * C++ source code generated on : Fri Oct 12 17:48:55 2018
  *
  * Target selection: grt.tlc
  * Note: GRT includes extra infrastructure and instrumentation for prototyping

src/tuning_GS1_grt_rtw/tuning_GS1_data.cpp

@@ -9,7 +9,7 @@
  *
  * Model version              : 1.1576
  * Simulink Coder version : 8.12 (R2017a) 16-Feb-2017
- * C++ source code generated on : Wed Oct 10 17:23:31 2018
+ * C++ source code generated on : Fri Oct 12 17:48:55 2018
  *
  * Target selection: grt.tlc
  * Note: GRT includes extra infrastructure and instrumentation for prototyping

src/tuning_GS1_grt_rtw/tuning_GS1_private.h

@@ -9,7 +9,7 @@
  *
  * Model version              : 1.1576
  * Simulink Coder version : 8.12 (R2017a) 16-Feb-2017
- * C++ source code generated on : Wed Oct 10 17:23:31 2018
+ * C++ source code generated on : Fri Oct 12 17:48:55 2018
  *
  * Target selection: grt.tlc
  * Note: GRT includes extra infrastructure and instrumentation for prototyping

src/tuning_GS1_grt_rtw/tuning_GS1_types.h

@@ -9,7 +9,7 @@
  *
  * Model version              : 1.1576
  * Simulink Coder version : 8.12 (R2017a) 16-Feb-2017
- * C++ source code generated on : Wed Oct 10 17:23:31 2018
+ * C++ source code generated on : Fri Oct 12 17:48:55 2018
  *
  * Target selection: grt.tlc
  * Note: GRT includes extra infrastructure and instrumentation for prototyping

src/tuning_GS1_node.cpp

@@ -68,8 +68,14 @@ void controller_dyn_callback(gsft_control::controllerGS1DynConfig &config, uint3
 
       gTest_mode = config.test_mode;
 
+      // Note: dynamic reconfigure is called when created by server_dyn.setCallback(f_dyn)
+      //               i.e. before the first Odometry available
+      // Compare mode: because config.enable_take_off always TRUE
+      //               => need gFirst_odom condition: do not get initial value of gOdometry (all 0)
+      //               => need recall this function after first odom available to get correct initial point, then take-off
+      // Experimental test: initial point before take off command, not after first odom
       if (config.enable_take_off && !gInit_flag && gFirst_odom){     // only once
-        gY0[0]    = gOdometry.position.x();           // First position before take off
+        gY0[0]    = gOdometry.position.x();           // Initial point
         gY0[1]    = gOdometry.position.y();
         gY0[2]    = gOdometry.position.z();
         gY0[3]    = gPsi;
@@ -252,10 +258,10 @@ int main(int argc, char** argv) {
   gLOE_t  = Eigen::VectorXd::Zero(6);
   gThrust_measure = Eigen::VectorXd::Zero(6);
 
-  dynamic_reconfigure::Server<gsft_control::controllerGS1DynConfig> server;
-  dynamic_reconfigure::Server<gsft_control::controllerGS1DynConfig>::CallbackType f;
-  f = boost::bind(&controller_dyn_callback, _1, _2);
-  server.setCallback(f);
+  dynamic_reconfigure::Server<gsft_control::controllerGS1DynConfig> server_dyn;
+  dynamic_reconfigure::Server<gsft_control::controllerGS1DynConfig>::CallbackType f_dyn;
+  f_dyn = boost::bind(&controller_dyn_callback, _1, _2);
+  server_dyn.setCallback(f_dyn);
 
   Eigen::Vector3d velocity_W ;
   Eigen::Vector3d euler_angles;
@@ -268,26 +274,29 @@ int main(int argc, char** argv) {
   thrust_prev_sent    = Eigen::VectorXd::Zero(6);
 
   bool controller_active = false;
-//  gPrev_it = ros::Time::now();
+  //  gPrev_it = ros::Time::now();
 
   static int seq = 0;
   while(ros::ok()) {
-    /*Eigen::Matrix3d R_W_B = gOdometry.orientation.toRotationMatrix();
+    /* https://github.com/libigl/eigen/blob/master/Eigen/src/Geometry/Quaternion.h
+    Eigen::Matrix3d R_W_B = gOdometry.orientation.toRotationMatrix(); % Ned to body (not Euler matrix), ZYX order
     velocity_W =  R_W_B * gOdometry.velocity;
     double psi = atan2(R_W_B(1,0),R_W_B(0,0));
     double phi  = atan2(R_W_B(2,1),R_W_B(2,2));
     double theta = asin(-R_W_B(2,0));*/
 
     velocity_W = gOdometry.getVelocityWorld();
-    gOdometry.getEulerAngles(&euler_angles);
+
+    gOdometry.getEulerAngles(&euler_angles);      // getEulerAnglesFromQuaternion: http://docs.ros.org/jade/api/mav_msgs/html/common_8h_source.html
     gPsi = euler_angles[2];
-    // gPsi = gOdometry.getYaw();                             // same result
+    // gPsi = gOdometry.getYaw();                 // result
 
-    if (gFirst_odom && !controller_active){
-      server.setCallback(f);
+    if (test_scenario == "compare" && gFirst_odom && !controller_active){
+      server_dyn.setCallback(f_dyn);             // get initial point when first odom available, then take
+      test_scenario = " ";
     }
 
-    if (gInit_flag && !controller_active) {                     // only once when controller is not actived
+    if (gInit_flag && !controller_active) {      // only once when controller is not actived
         gController.initialize();
         controller_active = true;
 

src/tuning_nominal_grt_rtw/multiword_types.h

@@ -9,7 +9,7 @@
  *
  * Model version              : 1.1505
  * Simulink Coder version : 8.12 (R2017a) 16-Feb-2017
- * C++ source code generated on : Fri Oct 12 12:24:48 2018
+ * C++ source code generated on : Fri Oct 12 17:49:40 2018
  *
  * Target selection: grt.tlc
  * Note: GRT includes extra infrastructure and instrumentation for prototyping

src/tuning_nominal_grt_rtw/rtmodel.h

@@ -9,7 +9,7 @@
  *
  * Model version              : 1.1505
  * Simulink Coder version : 8.12 (R2017a) 16-Feb-2017
- * C++ source code generated on : Fri Oct 12 12:24:48 2018
+ * C++ source code generated on : Fri Oct 12 17:49:40 2018
  *
  * Target selection: grt.tlc
  * Note: GRT includes extra infrastructure and instrumentation for prototyping

src/tuning_nominal_grt_rtw/rtwtypes.h

@@ -9,7 +9,7 @@
  *
  * Model version              : 1.1505
  * Simulink Coder version : 8.12 (R2017a) 16-Feb-2017
- * C++ source code generated on : Fri Oct 12 12:24:48 2018
+ * C++ source code generated on : Fri Oct 12 17:49:40 2018
  *
  * Target selection: grt.tlc
  * Note: GRT includes extra infrastructure and instrumentation for prototyping

src/tuning_nominal_grt_rtw/tuning_nominal.cpp

@@ -9,7 +9,7 @@
  *
  * Model version              : 1.1505
  * Simulink Coder version : 8.12 (R2017a) 16-Feb-2017
- * C++ source code generated on : Fri Oct 12 12:24:48 2018
+ * C++ source code generated on : Fri Oct 12 17:49:40 2018
  *
  * Target selection: grt.tlc
  * Note: GRT includes extra infrastructure and instrumentation for prototyping
@@ -35,7 +35,7 @@ static void rate_scheduler(RT_MODEL_tuning_nominal_T *const tuning_nominal_M)
    * 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]) > 1) {/* Sample time: [0.005s, 0.0s] */
+  if ((tuning_nominal_M->Timing.TaskCounters.TID[2]) > 4) {/* Sample time: [0.005s, 0.0s] */
     tuning_nominal_M->Timing.TaskCounters.TID[2] = 0;
   }
 }
@@ -935,9 +935,9 @@ void tuning_nominalModelClass::step()
       ->solverInfo);
 
     {
-      /* Update absolute timer for sample time: [0.0025s, 0.0s] */
+      /* Update absolute timer for sample time: [0.001s, 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.0025, which is the step size
+       * been executed. The resolution of this integer timer is 0.001, 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.
@@ -1016,7 +1016,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.0025;
+  (&tuning_nominal_M)->Timing.stepSize0 = 0.001;
 
   /* block I/O */
   (void) memset(((void *) &tuning_nominal_B), 0,

src/tuning_nominal_grt_rtw/tuning_nominal.h

@@ -9,7 +9,7 @@
  *
  * Model version              : 1.1505
  * Simulink Coder version : 8.12 (R2017a) 16-Feb-2017
- * C++ source code generated on : Fri Oct 12 12:24:48 2018
+ * C++ source code generated on : Fri Oct 12 17:49:40 2018
  *
  * Target selection: grt.tlc
  * Note: GRT includes extra infrastructure and instrumentation for prototyping

src/tuning_nominal_grt_rtw/tuning_nominal_data.cpp

@@ -9,7 +9,7 @@
  *
  * Model version              : 1.1505
  * Simulink Coder version : 8.12 (R2017a) 16-Feb-2017
- * C++ source code generated on : Fri Oct 12 12:24:48 2018
+ * C++ source code generated on : Fri Oct 12 17:49:40 2018
  *
  * Target selection: grt.tlc
  * Note: GRT includes extra infrastructure and instrumentation for prototyping
@@ -48,9 +48,9 @@ const ConstP_tuning_nominal_T tuning_nominal_ConstP = {
   /* Expression: pInitialization.M
    * Referenced by: '<S11>/KalmanGainM'
    */
-  { 0.0034535408539148734, 0.0, 0.0, 0.0023895168172179296, 0.0, 0.0, 0.0,
-    0.0034535408539183013, 0.0, 0.0, 0.002389516817220575, 0.0, 0.0, 0.0,
-    0.0013746811404775731, 0.0, 0.0, 0.00037820965113614696 },
+  { 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 },
 
   /* Expression: pInitialization.C
    * Referenced by: '<S3>/C'
@@ -62,13 +62,13 @@ const ConstP_tuning_nominal_T tuning_nominal_ConstP = {
    * Referenced by: '<S3>/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.0025, 0.0, 0.0, 1.0, 0.0, 0.0, 0.0, 0.0025, 0.0, 0.0, 1.0,
-    0.0, 0.0, 0.0, 0.0025, 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 },
 
   /* Expression: pInitialization.L
    * Referenced by: '<S11>/KalmanGainL'
    */
-  { 0.0034595146459579181, 0.0, 0.0, 0.0023895168172179296, 0.0, 0.0, 0.0,
-    0.0034595146459613525, 0.0, 0.0, 0.002389516817220575, 0.0, 0.0, 0.0,
-    0.0013756266646054135, 0.0, 0.0, 0.00037820965113614696 }
+  { 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 }
 };