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

@@ -39,6 +39,7 @@
          <param name="ahrs_bias_time" value="1.25"/> -->
 
     # optional param algorithm_magnetometer_gain, default is 0.005
+    # WARNING: do not set on PhidgetSpatial MOT0110 onwards (not supported)!
     <!-- <param name="algorithm_magnetometer_gain" value="0.005"/> -->
 
     # optional param heating_enabled, not modified by default

phidgets_spatial/src/spatial_ros_i.cpp

@@ -96,11 +96,14 @@ SpatialRosI::SpatialRosI(ros::NodeHandle nh, ros::NodeHandle nh_private)
         nh_private_.getParam("ahrs_bias_time", ahrsBiasTime);
 
     double algorithm_magnetometer_gain;
+    bool set_algorithm_magnetometer_gain = true;
     if (!nh_private_.getParam("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;
@@ -272,7 +275,9 @@ SpatialRosI::SpatialRosI(ros::NodeHandle nh, ros::NodeHandle nh_private)
                                             ahrsBiasTime);
             }
 
-            spatial_->setAlgorithmMagnetometerGain(algorithm_magnetometer_gain);
+            if (set_algorithm_magnetometer_gain)
+                spatial_->setAlgorithmMagnetometerGain(
+                    algorithm_magnetometer_gain);
         }
 
         if (has_compass_params)