Only set magnetometer gain if param is set

This is required because the function
`PhidgetSpatial_setAlgorithmMagnetometerGain` is present in the Phidgets
Spatial MOT0109 API (https://www.phidgets.com/?prodid=1204#Tab_API), but
was removed in the MOT0110 API
(https://www.phidgets.com/?prodid=1205#Tab_API). This caused an
exception to be thrown on the MOT0110
(ros-drivers#168 (comment)).

This commit allows not setting the parameter on the MOT0110 and onwards,
thereby avoiding the function call and the exception.

Author: mintar

phidgets_spatial/launch/spatial-launch.py

@@ -38,7 +38,8 @@ def generate_launch_description():
         'ahrs_bias_time': 1.25,
 
         # optional param algorithm_magnetometer_gain, default is 0.005
-        'algorithm_magnetometer_gain': 0.005,
+        # WARNING: do not set on PhidgetSpatial MOT0110 onwards (not supported)!
+        # 'algorithm_magnetometer_gain': 0.005,
 
         # optional param heating_enabled, not modified by default
         'heating_enabled': False,

phidgets_spatial/src/spatial_ros_i.cpp

@@ -92,11 +92,14 @@ SpatialRosI::SpatialRosI(const rclcpp::NodeOptions &options)
         this->get_parameter("ahrs_bias_time", ahrsBiasTime);
 
     double algorithm_magnetometer_gain;
+    bool set_algorithm_magnetometer_gain = true;
     if (!this->get_parameter("algorithm_magnetometer_gain",
                              algorithm_magnetometer_gain))
     {
-        algorithm_magnetometer_gain =
-            0.005;  // default to 0.005 (similar to phidgets api)
+        algorithm_magnetometer_gain = 0.0;
+        set_algorithm_magnetometer_gain =
+            false;  // if parameter not set, do not call api (because this
+                    // function is not available from MOT0110 onwards)
     }
 
     bool heating_enabled;
@@ -267,7 +270,9 @@ SpatialRosI::SpatialRosI(const rclcpp::NodeOptions &options)
                                             ahrsBiasTime);
             }
 
-            spatial_->setAlgorithmMagnetometerGain(algorithm_magnetometer_gain);
+            if (set_algorithm_magnetometer_gain)
+                spatial_->setAlgorithmMagnetometerGain(
+                    algorithm_magnetometer_gain);
         }
 
         if (has_compass_params)