Merge pull request #1760 from pweids/main

Fix to allow non-flight controller heartbeats to be processed
mavlink · May 9, 2022 · 01e1d55 · 01e1d55
2 parents 00fd132 + 302e9ab
commit 01e1d55
Showing 1 changed file with 9 additions and 8 deletions.
diff --git a/src/mavsdk/core/system_impl.cpp b/src/mavsdk/core/system_impl.cpp
@@ -216,15 +216,16 @@ void SystemImpl::process_heartbeat(const mavlink_message_t& message)
     // Only set the vehicle type if the heartbeat is from an autopilot component
     // This check only works if the MAV_TYPE::MAV_TYPE_ENUM_END is actually the
     // last enumerator.
-    if (heartbeat.autopilot != MAV_AUTOPILOT_INVALID &&
-        MAV_TYPE::MAV_TYPE_ENUM_END > heartbeat.type) {
-        auto new_vehicle_type = static_cast<MAV_TYPE>(heartbeat.type);
-        if (_vehicle_type != new_vehicle_type)
-            LogWarn() << "Vehicle type changed! New type: " << heartbeat.type
-                      << " Old type: " << static_cast<uint8_t>(_vehicle_type);
-        _vehicle_type = new_vehicle_type;
-    } else {
+    if (MAV_TYPE::MAV_TYPE_ENUM_END < heartbeat.type) {
         LogErr() << "type received in HEARTBEAT was not recognized";
+    } else {
+        const auto new_vehicle_type = static_cast<MAV_TYPE>(heartbeat.type);
+        if (heartbeat.autopilot != MAV_AUTOPILOT_INVALID && _vehicle_type != new_vehicle_type &&
+            _vehicle_type != MAV_TYPE_GENERIC) {
+            LogWarn() << "Vehicle type changed (new type: " << static_cast<unsigned>(heartbeat.type)
+                      << ", old type: " << static_cast<unsigned>(_vehicle_type) << ")";
+            _vehicle_type = new_vehicle_type;
+        }
     }
 
     if (message.compid == MavlinkCommandSender::DEFAULT_COMPONENT_ID_AUTOPILOT) {