estimator param updates

rosplane/include/estimator_base.hpp

@@ -104,6 +104,8 @@ class estimator_base : public rclcpp::Node
   void statusCallback(const rosflight_msgs::msg::Status::SharedPtr msg);
 
   rclcpp::TimerBase::SharedPtr update_timer_;
+  std::chrono::microseconds update_period_;
+  bool params_initialized_;
   std::string gnss_fix_topic_ = "navsat_compat/fix";
   std::string gnss_vel_topic_ = "navsat_compat/vel";
   std::string imu_topic_ = "imu/data";
@@ -127,6 +129,11 @@ class estimator_base : public rclcpp::Node
    */
   void declare_parameters();
 
+  /**
+   * @brief Determines the period of a timer based on the ROS2 parameter and starts it 
+   */
+  void set_timer();
+
   /**
    * ROS2 parameter system interface. This connects ROS2 parameters with the defined update callback, parametersCallback.
    */

rosplane/params/anaconda_autopilot_params.yaml

@@ -60,38 +60,15 @@ path_follower:
 
 estimator:
   ros__parameters:
-    estimator_update_frequency: 100.0
     rho: 1.225
     gravity: 9.8
-    gps_ground_speed_threshold: 0.3
-    baro_measurement_gate: 1.35
-    airspeed_measurement_gate: 5.0
-    baro_calibration_count: 100
     sigma_n_gps: 0.01
     sigma_e_gps: 0.01
     sigma_Vg_gps: 0.005
     sigma_course_gps: 0.00025
     sigma_accel: 0.024525
-    sigma_pseudo_wind_n: 0.01
-    sigma_pseudo_wind_e: 0.01
-    sigma_heading: 0.01
     lpf_a: 50.0
     lpf_a1: 8.0
     gps_n_lim: 10000.
     gps_e_lim: 10000.
-    roll_process_noise: 0.0001
-    pitch_process_noise: 0.0000001
-    gyro_process_noise: 0.13
-    pos_process_noise: 0.1
-    attitude_initial_cov: 5.0
-    pos_n_initial_cov: 0.03
-    pos_e_initial_cov: 0.03
-    vg_initial_cov: 0.01
-    chi_initial_cov: 5.0
-    wind_n_initial_cov: 0.04
-    wind_e_initial_cov: 0.04
-    psi_initial_cov: 5.0
-    num_propagation_steps: 10
-    max_estimated_phi: 85.0
-    max_estimated_theta: 80.0
-    estimator_max_buffer: 3.0
+    frequency: 100

rosplane/src/estimator_base.cpp

@@ -6,9 +6,8 @@ namespace rosplane
 {
 
 estimator_base::estimator_base()
-    : Node("estimator_base"), params(this)
+    : Node("estimator_base"), params(this), params_initialized_(false)
 {
-
   vehicle_state_pub_ = this->create_publisher<rosplane_msgs::msg::State>("estimated_state", 10);
 
   gnss_fix_sub_ = this->create_subscription<sensor_msgs::msg::NavSatFix>(
@@ -24,32 +23,43 @@ estimator_base::estimator_base()
   status_sub_ = this->create_subscription<rosflight_msgs::msg::Status>(
     status_topic_, 10, std::bind(&estimator_base::statusCallback, this, std::placeholders::_1));
 
-  // Set the parameter callback, for when parameters are changed.
-  parameter_callback_handle_ = this->add_on_set_parameters_callback(
-    std::bind(&estimator_base::parametersCallback, this, std::placeholders::_1));
-
   init_static_ = 0;
   baro_count_ = 0;
   armed_first_time_ = false;
+  baro_init_ = false;
+  gps_init_ = false;
+
+  // Set the parameter callback, for when parameters are changed.
+  parameter_callback_handle_ = this->add_on_set_parameters_callback(
+    std::bind(&estimator_base::parametersCallback, this, std::placeholders::_1));
 
   // Declare and set parameters with the ROS2 system
   declare_parameters();
   params.set_parameters();
 
-  //TODO: Change this timer frequency
-  update_timer_ = this->create_wall_timer(10ms, std::bind(&estimator_base::update, this));
+  params_initialized_ = true;
+
+  set_timer();
 }
 
 void estimator_base::declare_parameters()
 {
+  params.declare_double("estimator_update_frequency", 100.0);
   params.declare_double("rho", 1.225);
   params.declare_double("gravity", 9.8);
   params.declare_double("gps_ground_speed_threshold", 0.3);  // TODO: this is a magic number. What is it determined from?
   params.declare_double("baro_measurement_gate", 1.35);  // TODO: this is a magic number. What is it determined from?
-  params.declare_double("airspeed_meaurement_gate", 5.0);  // TODO: this is a magic number. What is it determined from?
+  params.declare_double("airspeed_measurement_gate", 5.0);  // TODO: this is a magic number. What is it determined from?
   params.declare_int("baro_calibration_count", 100);  // TODO: this is a magic number. What is it determined from?
 }
 
+void estimator_base::set_timer() {
+  double frequency = params.get_double("estimator_update_frequency");
+
+  update_period_ = std::chrono::microseconds(static_cast<long long>(1.0 / frequency * 1'000'000));
+  update_timer_ = this->create_wall_timer(update_period_, std::bind(&estimator_base::update, this));
+}
+
 rcl_interfaces::msg::SetParametersResult 
 estimator_base::parametersCallback(const std::vector<rclcpp::Parameter> & parameters)
 {
@@ -65,6 +75,16 @@ estimator_base::parametersCallback(const std::vector<rclcpp::Parameter> & parame
       "One of the parameters given is not a parameter of the estimator node.";
   }
 
+  // Check to see if the timer period was changed. If it was, recreate the timer with the new period
+  if (params_initialized_ && success) {
+    std::chrono::microseconds curr_period = std::chrono::microseconds(static_cast<long long>(1.0 / params.get_double("estimator_update_frequency") * 1'000'000));
+    if (update_period_ != curr_period) {
+      update_timer_->cancel();
+      set_timer();
+    }
+
+  }
+
   return result;
 }
 

rosplane/src/estimator_example.cpp

@@ -105,7 +105,7 @@ void estimator_example::update_measurement_model_parameters()
   double sigma_pseudo_wind_n = params.get_double("sigma_pseudo_wind_n");
   double sigma_pseudo_wind_e = params.get_double("sigma_pseudo_wind_e");
   double sigma_heading = params.get_double("sigma_heading");
-  int64_t frequency = params.get_int("frequency");
+  double frequency = params.get_double("estimator_update_frequency");
   double Ts = 1.0 / frequency;
   float lpf_a = params.get_double("lpf_a");
   float lpf_a1 = params.get_double("lpf_a1");
@@ -129,7 +129,7 @@ void estimator_example::estimate(const input_s & input, output_s & output)
   // For readability, declare the parameters here
   double rho = params.get_double("rho");
   double gravity = params.get_double("gravity");
-  int64_t frequency = params.get_int("frequency");
+  double frequency = params.get_double("estimator_update_frequency");
   double gps_n_lim = params.get_double("gps_n_lim");
   double gps_e_lim = params.get_double("gps_e_lim");
   double Ts = 1.0 / frequency;
@@ -452,6 +452,8 @@ void estimator_example::check_xhat_a()
   double max_theta = params.get_double("max_estimated_theta");
   double buff = params.get_double("estimator_max_buffer");
 
+  if (xhat_a_(0) > radians(85.0) || xhat_a_(0) < radians(-85.0) || !std::isfinite(xhat_a_(0))) {
+
   if (!std::isfinite(xhat_a_(0))) {
     xhat_a_(0) = 0;
     P_a_ = Eigen::Matrix2f::Identity();
@@ -464,6 +466,7 @@ void estimator_example::check_xhat_a()
     xhat_a_(0) = radians(-max_phi + buff);
     RCLCPP_WARN(this->get_logger(), "min roll angle");
   }
+  }
 
   if (!std::isfinite(xhat_a_(1))) {
     xhat_a_(1) = 0;
@@ -489,7 +492,6 @@ void estimator_example::declare_parameters()
   params.declare_double("sigma_pseudo_wind_n", 0.01);
   params.declare_double("sigma_pseudo_wind_e", 0.01);
   params.declare_double("sigma_heading", 0.01);
-  params.declare_int("frequency", 100);
   params.declare_double("lpf_a", 50.0);
   params.declare_double("lpf_a1", 8.0);
   params.declare_double("gps_n_lim", 10000.);