Merge pull request #44 from byu-magicc/param_counter

Param counter - RC 1.0

include/fcu_io.h

@@ -81,13 +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::NodeHandle nh_;
 
   ros::Subscriber command_sub_;
@@ -116,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,8 +39,16 @@ class ParamManager : public MavlinkListenerInterface
   bool save_to_file(std::string filename);
   bool load_from_file(std::string filename);
 
+  int get_num_params();
+  int get_params_received();
+  bool got_all_params();
+
+  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);
@@ -56,9 +64,10 @@ 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_;
 };
 
 } // namespace mavrosflight

src/fcu_io.cpp

@@ -33,6 +33,8 @@ fcuIO::fcuIO()
   std::string port = nh_private.param<std::string>("port", "/dev/ttyUSB0");
   int baud_rate = nh_private.param<int>("baud_rate", 921600);
 
+  ROS_INFO("FCU_IO");
+  ROS_INFO("Connecting to %s, at %d baud", port.c_str(), baud_rate);
 
   try
   {
@@ -42,11 +44,16 @@ fcuIO::fcuIO()
   {
     ROS_FATAL("%s", e.what());
     ros::shutdown();
-  }
+  }  
 
   mavrosflight_->serial.register_mavlink_listener(this);
   mavrosflight_->param.register_param_listener(this);
 
+  // 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);
@@ -154,7 +161,7 @@ void fcuIO::handle_heartbeat_msg(const mavlink_message_t &msg)
     prev_armed_state = heartbeat.base_mode;
   }
   else if(heartbeat.base_mode == MAV_MODE_ENUM_END)
-    ROS_ERROR_THROTTLE(1,"FCU FAILSAFE");
+    ROS_ERROR_THROTTLE(5,"FCU FAILSAFE");
 
 
   // Print if change in control mode
@@ -181,8 +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;
   }
-
-
 }
 
 void fcuIO::handle_command_ack_msg(const mavlink_message_t &msg)
@@ -609,7 +614,8 @@ bool fcuIO::calibrateImuBiasSrvCallback(std_srvs::Trigger::Request &req, std_srv
 {
   mavlink_message_t msg;
   mavlink_msg_command_int_pack(1, 50, &msg, 1, MAV_COMP_ID_ALL,
-                               0, MAV_CMD_PREFLIGHT_CALIBRATION, 0, 0, 1, 0, 0, 0, 1, 0, 0);
+                               0, MAV_CMD_PREFLIGHT_CALIBRATION, 0, 0,
+                               1.0, 0.0, 0.0, 0.0, 1, 0, 0.0);
   mavrosflight_->serial.send_message(msg);
 
   res.success = true;
@@ -626,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

@@ -4,7 +4,7 @@
  */
 
 #include <mavrosflight/param_manager.h>
-
+#include <ros/ros.h>
 #include <yaml-cpp/yaml.h>
 
 #include <fstream>
@@ -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);
-  request_param_list();
 }
 
 ParamManager::~ParamManager()
 {
-  if (param_count_ > 0)
+  if (first_param_received_)
   {
     delete[] received_;
   }
@@ -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
@@ -225,7 +251,14 @@ void ParamManager::handle_param_value_msg(const mavlink_message_t &msg)
   if (!is_param_id(name)) // if we haven't received this param before, add it
   {
     params_[name] = Param(param);
-    received_[param.param_index] = true; //! \todo Implement check that all parameters have been received
+    received_[param.param_index] = true;
+
+    // increase the param count
+    received_count_++;
+    if(received_count_ == num_params_)
+    {
+      got_all_params_ = true;
+    }
 
     for (int i = 0; i < listeners_.size(); i++)
       listeners_[i]->on_new_param_received(name, params_[name].getValue());
@@ -251,13 +284,23 @@ void ParamManager::handle_command_ack_msg(const mavlink_message_t &msg)
     mavlink_command_ack_t ack;
     mavlink_msg_command_ack_decode(&msg, &ack);
 
-    if (ack.command == MAV_CMD_PREFLIGHT_STORAGE && ack.result == MAV_RESULT_ACCEPTED)
+    if (ack.command == MAV_CMD_PREFLIGHT_STORAGE)
     {
       write_request_in_progress_ = false;
-      unsaved_changes_ = false;
+      if(ack.result == MAV_RESULT_ACCEPTED)
+      {
+        ROS_INFO("Param write succeeded");
+        unsaved_changes_ = false;
 
-      for (int i = 0; i < listeners_.size(); i++)
-        listeners_[i]->on_params_saved_change(unsaved_changes_);
+        for (int i = 0; i < listeners_.size(); i++)
+          listeners_[i]->on_params_saved_change(unsaved_changes_);
+      }
+      else
+      {
+        ROS_INFO("Param write failed - maybe disarm the aricraft and try again?");
+        write_request_in_progress_ = false;
+        unsaved_changes_ = true;
+      }
     }
   }
 }
@@ -267,4 +310,26 @@ bool ParamManager::is_param_id(std::string name)
   return (params_.find(name) != params_.end());
 }
 
+int ParamManager::get_num_params()
+{
+  if (first_param_received_)
+  {
+    return num_params_;
+  }
+  else
+  {
+    return 0;
+  }
+}
+
+int ParamManager::get_params_received()
+{
+  return received_count_;
+}
+
+bool ParamManager::got_all_params()
+{
+  return got_all_params_;
+}
+
 } // namespace mavrosflight

src/mavrosflight/sensors/baro.cpp

@@ -36,6 +36,11 @@ bool Baro::correct(mavlink_small_baro_t baro, double *alt)
   double pressure = (double)baro.pressure;
   double temperature = (double)baro.temperature;
 
+  if(pressure < 0)
+  {
+    return false;
+  }
+
   if( calibration_counter_ > calibration_count_ + settling_count_)
   {
     double alt_tmp = (1.0f - pow(pressure/101325.0f, 0.190295f)) * 4430.0f; // in meters