diff --git a/include/fcu_io.h b/include/fcu_io.h index 881458d..4b0dcdb 100644 --- a/include/fcu_io.h +++ b/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 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_; diff --git a/include/mavrosflight/param_manager.h b/include/mavrosflight/param_manager.h index 7253b62..73213d9 100644 --- a/include/mavrosflight/param_manager.h +++ b/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 diff --git a/src/fcu_io.cpp b/src/fcu_io.cpp index 69efa78..8f0f443 100644 --- a/src/fcu_io.cpp +++ b/src/fcu_io.cpp @@ -33,6 +33,8 @@ fcuIO::fcuIO() std::string port = nh_private.param("port", "/dev/ttyUSB0"); int baud_rate = nh_private.param("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 diff --git a/src/mavrosflight/param_manager.cpp b/src/mavrosflight/param_manager.cpp index efa2bcb..7d66f5a 100644 --- a/src/mavrosflight/param_manager.cpp +++ b/src/mavrosflight/param_manager.cpp @@ -4,7 +4,7 @@ */ #include - +#include #include #include @@ -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,6 +196,24 @@ 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; @@ -204,6 +221,14 @@ void ParamManager::request_param_list() 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, ¶m_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 diff --git a/src/mavrosflight/sensors/baro.cpp b/src/mavrosflight/sensors/baro.cpp index e1cfb8a..99bb36c 100644 --- a/src/mavrosflight/sensors/baro.cpp +++ b/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