Added the override to the launch script and node.

You can now override the calibration for GPS and baro with whatever
value is in the parameters. The parameters are update on every
calibration that does occur. This means that in flight you can
immeadiatley restart as long as the seed_estimator flag is passed to the
launch or run command.

rosplane/include/estimator_base.hpp

@@ -89,6 +89,7 @@ class estimator_base : public rclcpp::Node
   double init_lat_ = 0.0;                 /**< Initial latitude in degrees */
   double init_lon_ = 0.0;                 /**< Initial longitude in degrees */
   float init_alt_ = 0.0;                  /**< Initial altitude in meters above MSL  */
+  float init_static_;                     /**< Initial static pressure (mbar)  */
 
 private:
   rclcpp::Publisher<rosplane_msgs::msg::State>::SharedPtr vehicle_state_pub_;
@@ -129,7 +130,6 @@ class estimator_base : public rclcpp::Node
 
   bool gps_new_;
   bool armed_first_time_;                 /**< Arm before starting estimation  */
-  float init_static_;                     /**< Initial static pressure (mbar)  */
   int baro_count_;                        /**< Used to grab the first set of baro measurements */
   std::vector<float> init_static_vector_; /**< Used to grab the first set of baro measurements */
 

rosplane/include/estimator_example.hpp

@@ -14,7 +14,7 @@ class estimator_example : public estimator_base
 {
 public:
   estimator_example();
-  estimator_example(float init_lat, float init_long, float init_alt);
+  estimator_example(bool use_params);
 
 private:
   virtual void estimate(const input_s & input, output_s & output);

rosplane/launch/rosplane.launch.py

@@ -11,10 +11,7 @@ def generate_launch_description():
     # Determine the appropriate control scheme.
     control_type = "default"
     aircraft = "skyhunter" # Default aircraft 
-    init_lat = "0.0" # init lat if seeding the estimator (typically not done)
-    init_long = "0.0"
-    init_alt = "0.0"
-    init_baro_alt = "0.0"
+    use_params = 'false'
 
     for arg in sys.argv:
         if arg.startswith("control_type:="):
@@ -23,20 +20,15 @@ def generate_launch_description():
         if arg.startswith("aircraft:="):
             aircraft = arg.split(":=")[1]
 
-        if arg.startswith("init_lat:="):
-            init_lat = float(arg.split(":=")[1])
-            assert isinstance(init_lat, float)
-            init_lat = str(init_lat)
+        if arg.startswith("seed_estimator:="):
+            use_params = arg.split(":=")[1].lower()
 
-        if arg.startswith("init_long:="):
-            init_long = float(arg.split(":=")[1])
-            assert isinstance(init_long, float)
-            init_long = str(init_long)
+            print(use_params)
 
-        if arg.startswith("init_alt:="):
-            init_alt = float(arg.split(":=")[1])
-            assert isinstance(init_alt, float)
-            init_alt = str(init_alt)
+            if use_params != 'true' and use_params != 'false':
+                print('Not a valid value for seed_estimator. Enter true or false.')
+                print('Defaulting to false.')
+                use_params = 'false'
 
     autopilot_params = os.path.join(
         rosplane_dir,
@@ -76,6 +68,6 @@ def generate_launch_description():
             name='estimator',
             output='screen',
             parameters = [autopilot_params],
-            arguments = [init_lat, init_long, init_alt, init_baro_alt]
+            arguments = [use_params]
         )
     ])

rosplane/src/estimator_base.cpp

@@ -6,7 +6,6 @@
 #include <fstream>
 #include <iostream>
 #include <rclcpp/logging.hpp>
-//#include <sensor_msgs/nav_sat_status.hpp>
 
 namespace rosplane
 {
@@ -330,14 +329,11 @@ int main(int argc, char ** argv)
 
   rclcpp::init(argc, argv);
 
-  double init_lat = std::atof(argv[1]);
-  double init_long = std::atof(argv[2]);
-  double init_alt = std::atof(argv[3]);
+  char* use_params = argv[1];
 
-
-  if (init_lat != 0.0 || init_long != 0.0 || init_alt != 0.0)
+  if (!strcmp(use_params, "true"))
   {
-    rclcpp::spin(std::make_shared<rosplane::estimator_example>(init_lat, init_long, init_alt));
+    rclcpp::spin(std::make_shared<rosplane::estimator_example>(use_params));
   }
 
   rclcpp::spin(std::make_shared<rosplane::estimator_example>());

rosplane/src/estimator_example.cpp

@@ -54,8 +54,13 @@ estimator_example::estimator_example()
   N_ = params.get_int("num_propagation_steps");
 }
 
-estimator_example::estimator_example(float init_lat, float init_long, float init_alt) : estimator_example()
+estimator_example::estimator_example(bool use_params) : estimator_example()
 {
+  double init_lat = params.get_double("init_lat");
+  double init_long = params.get_double("init_lon");
+  double init_alt = params.get_double("init_alt");
+  double init_static = params.get_double("baro_calibration_val");
+
   RCLCPP_INFO_STREAM(this->get_logger(), "init_lat: " << init_lat);
   RCLCPP_INFO_STREAM(this->get_logger(), "init_long: " << init_long);
   RCLCPP_INFO_STREAM(this->get_logger(), "init_alt: " << init_alt);
@@ -64,6 +69,9 @@ estimator_example::estimator_example(float init_lat, float init_long, float init
   init_lat_ = init_lat;
   init_lon_ = init_long;
   init_alt_ = init_alt;
+
+  baro_init_ = true;
+  init_static_ = init_static;
 }
 
 void estimator_example::initialize_state_covariances() {