Cleaned up parameters received check

include/fcu_io.h

@@ -81,14 +81,15 @@ class fcuIO :
   bool calibrateImuTempSrvCallback(std_srvs::Trigger::Request &req, std_srvs::Trigger::Response &res);
   bool calibrateRCTrimSrvCallback(std_srvs::Trigger::Request &req, std_srvs::Trigger::Response &res);
 
+  // timer callbacks
+  void paramTimerCallback(const ros::TimerEvent &e);
+
   // helpers
   template<class T> inline T saturate(T value, T min, T max)
   {
     return value < min ? min : (value > max ? max : value);
   }
 
-  ros::Time last_param_request_time_;
-
   ros::NodeHandle nh_;
 
   ros::Subscriber command_sub_;
@@ -117,6 +118,8 @@ class fcuIO :
   ros::ServiceServer imu_calibrate_temp_srv_;
   ros::ServiceServer calibrate_rc_srv_;
 
+  ros::Timer param_timer_;
+
   mavrosflight::MavROSflight *mavrosflight_;
   mavrosflight::sensors::DifferentialPressure diff_pressure_;
   mavrosflight::sensors::Imu imu_;

include/mavrosflight/param_manager.h

@@ -39,13 +39,17 @@ class ParamManager : public MavlinkListenerInterface
   bool save_to_file(std::string filename);
   bool load_from_file(std::string filename);
 
-  int get_param_count();
+  int get_num_params();
+  int get_params_received();
   bool got_all_params();
 
-  void request_param_list();
+  void request_params();
 
 private:
 
+  void request_param_list();
+  void request_param(int index);
+
   void handle_param_value_msg(const mavlink_message_t &msg);
   void handle_command_ack_msg(const mavlink_message_t &msg);
 
@@ -60,9 +64,9 @@ class ParamManager : public MavlinkListenerInterface
   bool write_request_in_progress_;
 
   bool first_param_received_;
-  size_t param_count_;
+  size_t num_params_;
+  size_t received_count_;
   bool *received_;
-  bool initialized_;
   bool got_all_params_;
 };
 

src/fcu_io.cpp

@@ -49,10 +49,11 @@ fcuIO::fcuIO()
   mavrosflight_->serial.register_mavlink_listener(this);
   mavrosflight_->param.register_param_listener(this);
 
-  // Request the param list
-  mavrosflight_->param.request_param_list();
-  last_param_request_time_ = ros::Time::now();
+  // request the param list
+  mavrosflight_->param.request_params();
+  param_timer_ = nh_.createTimer(ros::Duration(1.0), &fcuIO::paramTimerCallback, this);
 
+  // initialize latched "unsaved parameters" message value
   std_msgs::Bool unsaved_msg;
   unsaved_msg.data = false;
   unsaved_params_pub_.publish(unsaved_msg);
@@ -187,18 +188,6 @@ void fcuIO::handle_heartbeat_msg(const mavlink_message_t &msg)
     ROS_WARN_STREAM("FCU now in " << mode_string << " mode");
     prev_control_mode = heartbeat.custom_mode;
   }
-
-  // Check if we need to ask for parameters again
-  // If it has been more than 5 seconds and we are still missing some parameters,
-  // just ask for the list again
-  if( (last_param_request_time_ - ros::Time::now()) > ros::Duration(3)
-      && !mavrosflight_->param.got_all_params())
-  {
-    mavrosflight_->param.request_param_list();
-  }
-
-
-
 }
 
 void fcuIO::handle_command_ack_msg(const mavlink_message_t &msg)
@@ -643,6 +632,22 @@ bool fcuIO::calibrateRCTrimSrvCallback(std_srvs::Trigger::Request &req, std_srvs
   return true;
 }
 
+void fcuIO::paramTimerCallback(const ros::TimerEvent &e)
+{
+  if (mavrosflight_->param.got_all_params())
+  {
+    param_timer_.stop();
+    ROS_INFO("Received all parameters");
+  }
+  else
+  {
+    mavrosflight_->param.request_params();
+    ROS_INFO("Received %d of %d parameters. Requesting missing parameters...",
+             mavrosflight_->param.get_params_received(),
+             mavrosflight_->param.get_num_params());
+  }
+}
+
 bool fcuIO::calibrateImuTempSrvCallback(std_srvs::Trigger::Request &req, std_srvs::Trigger::Response &res)
 {
   // First, reset the previous calibration

src/mavrosflight/param_manager.cpp

@@ -17,16 +17,15 @@ ParamManager::ParamManager(MavlinkSerial * const serial) :
   unsaved_changes_(false),
   write_request_in_progress_(false),
   first_param_received_(false),
-  param_count_(0),
-  initialized_(false),
+  received_count_(0),
   got_all_params_(false)
 {
   serial_->register_mavlink_listener(this);
 }
 
 ParamManager::~ParamManager()
 {
-  if (param_count_ > 0)
+  if (first_param_received_)
   {
     delete[] received_;
   }
@@ -36,12 +35,12 @@ void ParamManager::handle_mavlink_message(const mavlink_message_t &msg)
 {
   switch (msg.msgid)
   {
-    case MAVLINK_MSG_ID_PARAM_VALUE:
-      handle_param_value_msg(msg);
-      break;
-    case MAVLINK_MSG_ID_COMMAND_ACK:
-      handle_command_ack_msg(msg);
-      break;
+  case MAVLINK_MSG_ID_PARAM_VALUE:
+    handle_param_value_msg(msg);
+    break;
+  case MAVLINK_MSG_ID_COMMAND_ACK:
+    handle_command_ack_msg(msg);
+    break;
   }
 }
 
@@ -197,13 +196,39 @@ bool ParamManager::load_from_file(std::string filename)
   }
 }
 
+void ParamManager::request_params()
+{
+  if (!first_param_received_)
+  {
+    request_param_list();
+  }
+  else
+  {
+    for (size_t i = 0; i < num_params_; i++)
+    {
+      if (!received_[i])
+      {
+        request_param(i);
+      }
+    }
+  }
+}
+
 void ParamManager::request_param_list()
 {
   mavlink_message_t param_list_msg;
   mavlink_msg_param_request_list_pack(1, 50, &param_list_msg, 1, MAV_COMP_ID_ALL);
   serial_->send_message(param_list_msg);
 }
 
+void ParamManager::request_param(int index)
+{
+  mavlink_message_t param_request_msg;
+  char empty[MAVLINK_MSG_PARAM_VALUE_FIELD_PARAM_ID_LEN];
+  mavlink_msg_param_request_read_pack(1, 50, &param_request_msg, 1, MAV_COMP_ID_ALL, empty, (int16_t) index);
+  serial_->send_message(param_request_msg);
+}
+
 void ParamManager::handle_param_value_msg(const mavlink_message_t &msg)
 {
   mavlink_param_value_t param;
@@ -212,7 +237,8 @@ void ParamManager::handle_param_value_msg(const mavlink_message_t &msg)
   if (!first_param_received_)
   {
     first_param_received_ = true;
-    received_ = new bool[param.param_count];
+    num_params_ = param.param_count;
+    received_ = new bool[num_params_];
   }
 
   // ensure null termination of name
@@ -228,16 +254,11 @@ void ParamManager::handle_param_value_msg(const mavlink_message_t &msg)
     received_[param.param_index] = true;
 
     // increase the param count
-    param_count_++;
-    if(param_count_ == param.param_count)
+    received_count_++;
+    if(received_count_ == num_params_)
     {
-      ROS_INFO("GOT ALL PARAMS");
       got_all_params_ = true;
     }
-    else
-    {
-      //        ROS_INFO_STREAM("Got param " << param_count_ << " of " << param.param_count);
-    }
 
     for (int i = 0; i < listeners_.size(); i++)
       listeners_[i]->on_new_param_received(name, params_[name].getValue());
@@ -289,9 +310,21 @@ bool ParamManager::is_param_id(std::string name)
   return (params_.find(name) != params_.end());
 }
 
-int ParamManager::get_param_count()
+int ParamManager::get_num_params()
+{
+  if (first_param_received_)
+  {
+    return num_params_;
+  }
+  else
+  {
+    return 0;
+  }
+}
+
+int ParamManager::get_params_received()
 {
-  return param_count_;
+  return received_count_;
 }
 
 bool ParamManager::got_all_params()