Change System ID and Component ID defaults and update logic

src/CameraServer.cpp

@@ -46,8 +46,9 @@ CameraServer::CameraServer(const ConfFile &conf)
     if (!getImgCapSettings(conf, imgSetting)) {
         // Send the setting to camera comp
         isImgCapSetting = true;
-    } else
+    } else {
         log_info("Image Capture Settings not found, use default");
+    }
 
     // Read image capture file location
     std::string imgPath = getImgCapLocation(conf);
@@ -57,17 +58,20 @@ CameraServer::CameraServer(const ConfFile &conf)
     if (!getVidCapSettings(conf, vidSetting)) {
         // Send the setting to camera comp
         isVidCapSetting = true;
-    } else
+    } else {
         log_info("Video Capture Settings not found, use default");
+    }
 
     std::string vidPath = getVidCapLocation(conf);
 
     std::vector<CameraComponent *>::iterator it;
     for (it = cameraList.begin(); it != cameraList.end(); it++) {
         if (*it) {
 #ifdef ENABLE_MAVLINK
-            if (mavlink_server.addCameraComponent(*it) == -1)
+            log_error("Adding Camera Component");
+            if (mavlink_server.addCameraComponent(*it) == -1) {
                 log_error("Error in adding Camera Component");
+            }
 #endif
             if (isImgCapSetting)
                 (*it)->setImageCaptureSettings(imgSetting);

src/mavlink_server.cpp

@@ -32,6 +32,7 @@ using namespace std::placeholders;
 #define DEFAULT_MAVLINK_BROADCAST_ADDR "255.255.255.255"
 #define DEFAULT_RTSP_SERVER_ADDR "0.0.0.0"
 #define MAX_MAVLINK_MESSAGE_SIZE 1024
+#define DEFAULT_SYSTEM_ID 1
 
 static const float epsilon = std::numeric_limits<float>::epsilon();
 
@@ -41,8 +42,9 @@ MavlinkServer::MavlinkServer(const ConfFile &conf, std::vector<std::unique_ptr<S
     , _is_running(false)
     , _timeout_handler(0)
     , _broadcast_addr{}
-    , _system_id(-1)
-    , _comp_id(MAV_COMP_ID_CAMERA2)
+    , _is_sys_id_found(false)
+    , _system_id(DEFAULT_SYSTEM_ID)
+    , _comp_id(MAV_COMP_ID_CAMERA)
     , _rtsp_server_addr(nullptr)
     , _rtsp(rtsp)
 {
@@ -56,7 +58,6 @@ MavlinkServer::MavlinkServer(const ConfFile &conf, std::vector<std::unique_ptr<S
     static const ConfFile::OptionsTable option_table[] = {
         {"port", false, ConfFile::parse_ul, OPTIONS_TABLE_STRUCT_FIELD(options, port)},
         {"system_id", false, ConfFile::parse_i, OPTIONS_TABLE_STRUCT_FIELD(options, sysid)},
-        {"component_id", false, ConfFile::parse_i, OPTIONS_TABLE_STRUCT_FIELD(options, compid)},
         {"rtsp_server_addr", false, ConfFile::parse_str_dup, OPTIONS_TABLE_STRUCT_FIELD(options, rtsp_server_addr)},
         {"broadcast_addr", false, ConfFile::parse_str_buf, OPTIONS_TABLE_STRUCT_FIELD(options, broadcast)},
     };
@@ -68,20 +69,18 @@ MavlinkServer::MavlinkServer(const ConfFile &conf, std::vector<std::unique_ptr<S
         _broadcast_addr.sin_port = htons(DEFAULT_MAVLINK_PORT);
 
     if (opt.sysid) {
-        if (opt.sysid < 1 || opt.sysid >= 255)
-            log_error("Invalid System ID for MAVLink communication (%d).Waiting for heartbeat from \
-             Vehicle",opt.sysid);
-        else
+        if (opt.sysid > 0 && opt.sysid < 255) {
+            log_info("Use System ID %d, ignore heartbeat from Vehicle", opt.sysid);
             _system_id = opt.sysid;
-    }
+            _is_sys_id_found = true;
 
-    if (opt.compid) {
-        if (opt.compid <= 1 || opt.compid >= 255)
-            log_error("Invalid Component ID for MAVLink communication (%d). Using default "
-                      "MAV_COMP_ID_CAMERA2 (%d)",
-                      opt.compid, MAV_COMP_ID_CAMERA2);
-        else
-            _comp_id = opt.compid;
+        } else {
+            log_error("Invalid System ID for MAVLink communication (%d)", opt.sysid);
+            log_info("Use System ID %d, till heartbeat received from Vehicle", DEFAULT_SYSTEM_ID);
+            _system_id = DEFAULT_SYSTEM_ID;
+        }
+    } else {
+        log_info("Use System ID %d, till heartbeat received from Vehicle", DEFAULT_SYSTEM_ID);
     }
 
     if (opt.broadcast[0])
@@ -573,9 +572,13 @@ void MavlinkServer::_handle_heartbeat(const struct sockaddr_in &addr, mavlink_me
     mavlink_heartbeat_t heartbeat;
     mavlink_msg_heartbeat_decode(msg, &heartbeat);
 
-    if (heartbeat.autopilot == 12)
-        _system_id = msg->sysid;
-
+    if (heartbeat.autopilot == MAV_AUTOPILOT_PX4) {
+        if (msg->sysid > 0 && msg->sysid < 255) {
+            log_info("Heartbeat received, System ID = %d", msg->sysid);
+            _system_id = msg->sysid;
+            _is_sys_id_found = true;
+        }
+    }
 }
 
 void MavlinkServer::_handle_mavlink_message(const struct sockaddr_in &addr, mavlink_message_t *msg)
@@ -649,7 +652,8 @@ void MavlinkServer::_handle_mavlink_message(const struct sockaddr_in &addr, mavl
     } else {
         switch (msg->msgid) {
         case MAVLINK_MSG_ID_HEARTBEAT:
-            this->_handle_heartbeat(addr, msg);
+            if (!_is_sys_id_found)
+                this->_handle_heartbeat(addr, msg);
             break;
         case MAVLINK_MSG_ID_SET_VIDEO_STREAM_SETTINGS:
             this->_handle_camera_set_video_stream_settings(addr, msg);
@@ -778,14 +782,21 @@ int MavlinkServer::addCameraComponent(CameraComponent *camComp)
 {
     log_debug("%s", __func__);
     int ret = -1;
-    int id = MAV_COMP_ID_CAMERA2;
-    while (id < MAV_COMP_ID_CAMERA6 + 1) {
-        if (compIdToObj.find(id) == compIdToObj.end()) {
-            compIdToObj.insert(std::make_pair(id, camComp));
-            ret = id;
+    int compid;
+#ifdef ENABLE_GAZEBO
+    // PX4-SITL Gazebo is running a camera component with id MAV_COMP_ID_CAMERA
+    compid = MAV_COMP_ID_CAMERA2;
+#else
+    compid = MAV_COMP_ID_CAMERA;
+#endif
+
+    while (compid <= MAV_COMP_ID_CAMERA6) {
+        if (compIdToObj.find(compid) == compIdToObj.end()) {
+            compIdToObj.insert(std::make_pair(compid, camComp));
+            ret = compid;
             break;
         }
-        id++;
+        compid++;
     }
 
     return ret;

src/mavlink_server.h

@@ -50,6 +50,7 @@ class MavlinkServer {
     unsigned int _timeout_handler;
     UDPSocket _udp;
     struct sockaddr_in _broadcast_addr = {};
+    bool _is_sys_id_found;
     int _system_id;
     int _comp_id;
     char *_rtsp_server_addr;

src/stream_manager.cpp

@@ -33,9 +33,6 @@ StreamManager::StreamManager(const ConfFile &conf)
     , avahi_publisher(streams, DEFAULT_SERVICE_PORT, DEFAULT_SERVICE_TYPE)
 #endif
     , rtsp_server(streams, DEFAULT_SERVICE_PORT)
-#ifdef ENABLE_MAVLINK
-    , mavlink_server(conf, streams, rtsp_server)
-#endif
 {
     GstreamerPipelineBuilder::get_instance().apply_configs(conf);
     for (StreamBuilder *builder : StreamBuilder::get_builders())

src/stream_manager.h

@@ -26,9 +26,6 @@
 #include <avahi-common/watch.h>
 #endif
 #include "conf_file.h"
-#ifdef ENABLE_MAVLINK
-#include "mavlink_server.h"
-#endif
 #include "rtsp_server.h"
 #include "stream.h"
 
@@ -47,7 +44,4 @@ class StreamManager {
     AvahiPublisher avahi_publisher;
 #endif
     RTSPServer rtsp_server;
-#ifdef ENABLE_MAVLINK
-    MavlinkServer mavlink_server;
-#endif
 };