diff --git a/rosplane/include/estimator_base.hpp b/rosplane/include/estimator_base.hpp index 9af90aa..774aa90 100644 --- a/rosplane/include/estimator_base.hpp +++ b/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. */ diff --git a/rosplane/params/anaconda_autopilot_params.yaml b/rosplane/params/anaconda_autopilot_params.yaml index efee3f2..e6c0b63 100644 --- a/rosplane/params/anaconda_autopilot_params.yaml +++ b/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 \ No newline at end of file + frequency: 100 \ No newline at end of file diff --git a/rosplane/src/estimator_base.cpp b/rosplane/src/estimator_base.cpp index cd4b6a2..d524cbb 100644 --- a/rosplane/src/estimator_base.cpp +++ b/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("estimated_state", 10); gnss_fix_sub_ = this->create_subscription( @@ -24,32 +23,43 @@ estimator_base::estimator_base() status_sub_ = this->create_subscription( 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(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 & parameters) { @@ -65,6 +75,16 @@ estimator_base::parametersCallback(const std::vector & 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(1.0 / params.get_double("estimator_update_frequency") * 1'000'000)); + if (update_period_ != curr_period) { + update_timer_->cancel(); + set_timer(); + } + + } + return result; } diff --git a/rosplane/src/estimator_example.cpp b/rosplane/src/estimator_example.cpp index ab392a4..d8c1ff3 100644 --- a/rosplane/src/estimator_example.cpp +++ b/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.);