fix #435

Author: tmori

drone_api/sample/rc_config/xbox-control.json

@@ -0,0 +1,88 @@
+{
+    "Os": [ "Darwin" ],
+    "mode": 2,
+    "stick": {
+        "Left": {
+            "UD": {
+                "index": 1,
+                "average": false,
+                "valueInverse": true,
+                "conversion": {
+                    "paramA": 0.9,
+                    "paramB": 0.1,
+                    "paramC": 0.0
+                }
+            },
+            "LR": {
+                "index": 0,
+                "average": false,
+                "valueInverse": false,
+                "conversion": {
+                    "paramA": 0.9,
+                    "paramB": 0.1,
+                    "paramC": 0.0
+                }
+            }
+        },
+        "Right": {
+            "UD": {
+                "index": 3,
+                "average": false,
+                "valueInverse": true,
+                "conversion": {
+                    "paramA": 0.9,
+                    "paramB": 0.1,
+                    "paramC": 0.0
+                }
+            },
+            "LR": {
+                "index": 2,
+                "average": false,
+                "valueInverse": false,
+                "conversion": {
+                    "paramA": 0.9,
+                    "paramB": 0.1,
+                    "paramC": 0.0
+                }
+            }
+        }
+    },
+    "Event": {
+        "RadioControlEnable": {
+            "index": 0,
+            "type": "switch",
+            "off": "up",
+            "on": "down"
+        },
+        "Camera": {
+            "index": 2,
+            "type": "switch",
+            "off": "up",
+            "on": "down"
+        },
+        "GrabBaggage": {
+            "index": 1,
+            "type": "toggle",
+            "off": "up",
+            "on": "down"
+        },
+        "CameraMoveUp": {
+            "index": 11,
+            "type": "switch",
+            "off": "up",
+            "on": "down"
+        },
+        "CameraMoveDown": {
+            "index": 12,
+            "type": "switch",
+            "off": "up",
+            "on": "down"
+        },
+        "ReturnHome": {
+            "index": 3,
+            "type": "switch",
+            "off": "up",
+            "on": "down"
+        }
+    }
+}

drone_control/config/param-api-mixer.txt

@@ -42,8 +42,8 @@ PID_POS_VY_Kd           1.0
 HEAD_CONTROL_CYCLE          0.0
 ANGULAR_CONTROL_CYCLE       0.0
 ANGULAR_RATE_CONTROL_CYCLE  0.0
-PID_POS_MAX_ROLL            30.0
-PID_POS_MAX_PITCH           30.0
+PID_POS_MAX_ROLL            25.0
+PID_POS_MAX_PITCH           25.0
 PID_ROLL_RPM_MAX            200.0
 PID_PITCH_RPM_MAX           200.0
 PID_ROLL_TORQUE_MAX         100.0

hakoniwa/src/modules/hako_ext.cpp

@@ -28,6 +28,11 @@ void hako_ext_main(bool master)
         std::cerr << "ERROR: can not find drone config file on " << drone_config_directory << std::endl;
         return;
     }
+    int max_delay_time_usec;
+    if (hako_param_env_get_integer(HAKO_MAXDELAY_TIME_USEC, &max_delay_time_usec) == false) {
+        HAKO_ABORT("Failed to get HAKO_MAXDELAY_TIME_USEC");
+    }
+    std::cout << "INFO: max_delay_time_usec: " << max_delay_time_usec << std::endl;
 
     CsvLogger::enable();
     DroneConfig drone_config;
@@ -43,7 +48,7 @@ void hako_ext_main(bool master)
         else {
             std::cout << "INFO: hako_master_init() success" << std::endl;
         }
-        hako_master_set_config_simtime((drone_config.getSimTimeStep()*1000000), (drone_config.getSimTimeStep()*1000000));
+        hako_master_set_config_simtime((drone_config.getSimTimeStep()*1000000), (hako_time_t)max_delay_time_usec);
 
 
         try {

hakoniwa/src/modules/hako_phys.cpp

@@ -33,6 +33,11 @@ void hako_phys_main()
         std::cerr << "ERROR: can not find drone config file on " << drone_config_path << std::endl;
         return;
     }
+    int max_delay_time_usec;
+    if (hako_param_env_get_integer(HAKO_MAXDELAY_TIME_USEC, &max_delay_time_usec) == false) {
+        HAKO_ABORT("Failed to get HAKO_MAXDELAY_TIME_USEC");
+    }
+    std::cout << "INFO: max_delay_time_usec: " << max_delay_time_usec << std::endl;
     CsvLogger::enable();
     DroneConfig drone_config;
     if (drone_config_manager.getConfig(0, drone_config) == false) {
@@ -46,7 +51,7 @@ void hako_phys_main()
     else {
         std::cout << "INFO: hako_master_init() success" << std::endl;
     }
-    hako_master_set_config_simtime((drone_config.getSimTimeStep()*1000000), (drone_config.getSimTimeStep()*1000000));
+    hako_master_set_config_simtime((drone_config.getSimTimeStep()*1000000), (hako_time_t)max_delay_time_usec);
     try {
         std::thread thread(hako_px4_master_thread_run, (void*)nullptr);
         thread.detach();

hakoniwa/src/modules/hako_pid.cpp

@@ -28,6 +28,11 @@ void hako_pid_main(bool master)
         std::cerr << "ERROR: can not find drone config file on " << drone_config_directory << std::endl;
         return;
     }
+    int max_delay_time_usec;
+    if (hako_param_env_get_integer(HAKO_MAXDELAY_TIME_USEC, &max_delay_time_usec) == false) {
+        HAKO_ABORT("Failed to get HAKO_MAXDELAY_TIME_USEC");
+    }
+    std::cout << "INFO: max_delay_time_usec: " << max_delay_time_usec << std::endl;
 
     CsvLogger::enable();
     DroneConfig drone_config;
@@ -43,7 +48,7 @@ void hako_pid_main(bool master)
         else {
             std::cout << "INFO: hako_master_init() success" << std::endl;
         }
-        hako_master_set_config_simtime((drone_config.getSimTimeStep()*1000000), (drone_config.getSimTimeStep()*1000000));
+        hako_master_set_config_simtime((drone_config.getSimTimeStep()*1000000), (hako_time_t)max_delay_time_usec);
 
 
         try {

hakoniwa/src/modules/hako_replay.cpp

@@ -27,6 +27,11 @@ void hako_replay_main(bool master)
         std::cerr << "ERROR: can not find drone config file on " << drone_config_directory << std::endl;
         return;
     }
+    int max_delay_time_usec;
+    if (hako_param_env_get_integer(HAKO_MAXDELAY_TIME_USEC, &max_delay_time_usec) == false) {
+        HAKO_ABORT("Failed to get HAKO_MAXDELAY_TIME_USEC");
+    }
+    std::cout << "INFO: max_delay_time_usec: " << max_delay_time_usec << std::endl;
 
     DroneConfig drone_config;
     if (drone_config_manager.getConfig(0, drone_config) == false) {

hakoniwa/src/modules/hako_sim.cpp

@@ -35,6 +35,12 @@ void hako_sim_main(bool master, hako::px4::comm::IcommEndpointType serverEndpoin
         std::cerr << "ERROR: can not find drone config file on " << drone_config_directory << std::endl;
         return;
     }
+    int max_delay_time_usec;
+    if (hako_param_env_get_integer(HAKO_MAXDELAY_TIME_USEC, &max_delay_time_usec) == false) {
+        HAKO_ABORT("Failed to get HAKO_MAXDELAY_TIME_USEC");
+    }
+    std::cout << "INFO: max_delay_time_usec: " << max_delay_time_usec << std::endl;
+
     hako::px4::comm::TcpServer server;
     DroneConfig drone_config;
     if (drone_config_manager.getConfig(0, drone_config) == false) {
@@ -49,7 +55,7 @@ void hako_sim_main(bool master, hako::px4::comm::IcommEndpointType serverEndpoin
         else {
             std::cout << "INFO: hako_master_init() success" << std::endl;
         }
-        hako_master_set_config_simtime((drone_config.getSimTimeStep()*1000000), (drone_config.getSimTimeStep()*1000000));
+        hako_master_set_config_simtime((drone_config.getSimTimeStep()*1000000), (hako_time_t)max_delay_time_usec);
 
         try {
             std::thread thread(hako_px4_master_thread_run, (void*)nullptr);

hakoniwa/src/utils/hako_params.cpp

@@ -32,12 +32,16 @@ static HakoParamStringType hako_param_string[HAKO_PARAM_STRING_NUM] = {
         "../config"
     },
 };
-#define HAKO_PARAM_INTEGER_NUM 1
+#define HAKO_PARAM_INTEGER_NUM 2
 static HakoParamIntegerType hako_param_integer[HAKO_PARAM_INTEGER_NUM] = {
     {
         HAKO_BYPASS_PORTNO,
         54001
     },
+    {
+        HAKO_MAXDELAY_TIME_USEC,
+        20000 // 20msec
+    },
 };
 
 void hako_param_env_init()

hakoniwa/src/utils/hako_params.hpp

@@ -13,6 +13,7 @@
  * integer params
  */
 #define HAKO_BYPASS_PORTNO "HAKO_BYPASS_PORTNO"
+#define HAKO_MAXDELAY_TIME_USEC "HAKO_MAXDELAY_TIME_USEC"
 
 extern void hako_param_env_init();
 extern const char* hako_param_env_get_string(const char* param_name);