sys_status.cpp
Go to the documentation of this file.
00001 
00009 /*
00010  * Copyright 2013,2014,2015 Vladimir Ermakov.
00011  *
00012  * This file is part of the mavros package and subject to the license terms
00013  * in the top-level LICENSE file of the mavros repository.
00014  * https://github.com/mavlink/mavros/tree/master/LICENSE.md
00015  */
00016 
00017 #include <mavros/mavros_plugin.h>
00018 #include <pluginlib/class_list_macros.h>
00019 
00020 #include <mavros_msgs/State.h>
00021 #include <mavros_msgs/ExtendedState.h>
00022 #include <mavros_msgs/BatteryStatus.h>
00023 #include <mavros_msgs/StreamRate.h>
00024 #include <mavros_msgs/SetMode.h>
00025 #include <mavros_msgs/CommandLong.h>
00026 
00027 namespace mavplugin {
00033 class HeartbeatStatus : public diagnostic_updater::DiagnosticTask
00034 {
00035 public:
00036         HeartbeatStatus(const std::string &name, size_t win_size) :
00037                 diagnostic_updater::DiagnosticTask(name),
00038                 times_(win_size),
00039                 seq_nums_(win_size),
00040                 window_size_(win_size),
00041                 min_freq_(0.2),
00042                 max_freq_(100),
00043                 tolerance_(0.1),
00044                 autopilot(MAV_AUTOPILOT_GENERIC),
00045                 type(MAV_TYPE_GENERIC),
00046                 system_status(MAV_STATE_UNINIT)
00047         {
00048                 clear();
00049         }
00050 
00051         void clear() {
00052                 lock_guard lock(mutex);
00053                 ros::Time curtime = ros::Time::now();
00054                 count_ = 0;
00055 
00056                 for (size_t i = 0; i < window_size_; i++) {
00057                         times_[i] = curtime;
00058                         seq_nums_[i] = count_;
00059                 }
00060 
00061                 hist_indx_ = 0;
00062         }
00063 
00064         void tick(uint8_t type_, uint8_t autopilot_,
00065                         std::string &mode_, uint8_t system_status_) {
00066                 lock_guard lock(mutex);
00067                 count_++;
00068 
00069                 type = static_cast<enum MAV_TYPE>(type_);
00070                 autopilot = static_cast<enum MAV_AUTOPILOT>(autopilot_);
00071                 mode = mode_;
00072                 system_status = static_cast<enum MAV_STATE>(system_status_);
00073         }
00074 
00075         void run(diagnostic_updater::DiagnosticStatusWrapper &stat) {
00076                 lock_guard lock(mutex);
00077                 ros::Time curtime = ros::Time::now();
00078                 int curseq = count_;
00079                 int events = curseq - seq_nums_[hist_indx_];
00080                 double window = (curtime - times_[hist_indx_]).toSec();
00081                 double freq = events / window;
00082                 seq_nums_[hist_indx_] = curseq;
00083                 times_[hist_indx_] = curtime;
00084                 hist_indx_ = (hist_indx_ + 1) % window_size_;
00085 
00086                 if (events == 0) {
00087                         stat.summary(2, "No events recorded.");
00088                 }
00089                 else if (freq < min_freq_ * (1 - tolerance_)) {
00090                         stat.summary(1, "Frequency too low.");
00091                 }
00092                 else if (freq > max_freq_ * (1 + tolerance_)) {
00093                         stat.summary(1, "Frequency too high.");
00094                 }
00095                 else {
00096                         stat.summary(0, "Normal");
00097                 }
00098 
00099                 stat.addf("Heartbeats since startup", "%d", count_);
00100                 stat.addf("Frequency (Hz)", "%f", freq);
00101                 stat.add("Vehicle type", mavros::UAS::str_type(type));
00102                 stat.add("Autopilot type", mavros::UAS::str_autopilot(autopilot));
00103                 stat.add("Mode", mode);
00104                 stat.add("System status", mavros::UAS::str_system_status(system_status));
00105         }
00106 
00107 private:
00108         int count_;
00109         std::vector<ros::Time> times_;
00110         std::vector<int> seq_nums_;
00111         int hist_indx_;
00112         std::recursive_mutex mutex;
00113         const size_t window_size_;
00114         const double min_freq_;
00115         const double max_freq_;
00116         const double tolerance_;
00117 
00118         enum MAV_AUTOPILOT autopilot;
00119         enum MAV_TYPE type;
00120         std::string mode;
00121         enum MAV_STATE system_status;
00122 };
00123 
00124 
00128 class SystemStatusDiag : public diagnostic_updater::DiagnosticTask
00129 {
00130 public:
00131         SystemStatusDiag(const std::string &name) :
00132                 diagnostic_updater::DiagnosticTask(name),
00133                 last_st {}
00134         { };
00135 
00136         void set(mavlink_sys_status_t &st) {
00137                 lock_guard lock(mutex);
00138                 last_st = st;
00139         }
00140 
00141         void run(diagnostic_updater::DiagnosticStatusWrapper &stat) {
00142                 lock_guard lock(mutex);
00143 
00144                 if ((last_st.onboard_control_sensors_health & last_st.onboard_control_sensors_enabled)
00145                                 != last_st.onboard_control_sensors_enabled)
00146                         stat.summary(2, "Sensor helth");
00147                 else
00148                         stat.summary(0, "Normal");
00149 
00150                 stat.addf("Sensor present", "0x%08X", last_st.onboard_control_sensors_present);
00151                 stat.addf("Sensor enabled", "0x%08X", last_st.onboard_control_sensors_enabled);
00152                 stat.addf("Sensor helth", "0x%08X", last_st.onboard_control_sensors_health);
00153 
00154                 // decode sensor health mask
00155 #define STAT_ADD_SENSOR(msg, sensor_mask)       \
00156         if (last_st.onboard_control_sensors_enabled & sensor_mask)      \
00157                 stat.add(msg, (last_st.onboard_control_sensors_health & sensor_mask) ? "Ok" : "Fail")
00158 
00159                 STAT_ADD_SENSOR("Sensor 3D Gyro", MAV_SYS_STATUS_SENSOR_3D_GYRO);
00160                 STAT_ADD_SENSOR("Sensor 3D Accel", MAV_SYS_STATUS_SENSOR_3D_ACCEL);
00161                 STAT_ADD_SENSOR("Sensor 3D Mag", MAV_SYS_STATUS_SENSOR_3D_MAG);
00162                 STAT_ADD_SENSOR("Sensor Abs Pressure", MAV_SYS_STATUS_SENSOR_ABSOLUTE_PRESSURE);
00163                 STAT_ADD_SENSOR("Sensor Diff Pressure", MAV_SYS_STATUS_SENSOR_DIFFERENTIAL_PRESSURE);
00164                 STAT_ADD_SENSOR("Sensor GPS", MAV_SYS_STATUS_SENSOR_GPS);
00165                 STAT_ADD_SENSOR("Sensor Optical Flow", MAV_SYS_STATUS_SENSOR_OPTICAL_FLOW);
00166                 STAT_ADD_SENSOR("Sensor Vision Position", MAV_SYS_STATUS_SENSOR_VISION_POSITION);
00167                 STAT_ADD_SENSOR("Sensor Laser Position", MAV_SYS_STATUS_SENSOR_LASER_POSITION);
00168                 STAT_ADD_SENSOR("Sensor Ext Grount Truth", MAV_SYS_STATUS_SENSOR_EXTERNAL_GROUND_TRUTH);
00169                 STAT_ADD_SENSOR("Sensor Ang Rate Control", MAV_SYS_STATUS_SENSOR_ANGULAR_RATE_CONTROL);
00170                 STAT_ADD_SENSOR("Sensor Attitude Stab", MAV_SYS_STATUS_SENSOR_ATTITUDE_STABILIZATION);
00171                 STAT_ADD_SENSOR("Sensor Yaw Position", MAV_SYS_STATUS_SENSOR_YAW_POSITION);
00172                 STAT_ADD_SENSOR("Sensor Z/Alt Control", MAV_SYS_STATUS_SENSOR_Z_ALTITUDE_CONTROL);
00173                 STAT_ADD_SENSOR("Sensor X/Y Pos Control", MAV_SYS_STATUS_SENSOR_XY_POSITION_CONTROL);
00174                 STAT_ADD_SENSOR("Sensor Motor Outputs", MAV_SYS_STATUS_SENSOR_MOTOR_OUTPUTS);
00175                 STAT_ADD_SENSOR("Sensor RC Receiver", MAV_SYS_STATUS_SENSOR_RC_RECEIVER);
00176                 STAT_ADD_SENSOR("Sensor 3D Gyro 2", MAV_SYS_STATUS_SENSOR_3D_GYRO2);
00177                 STAT_ADD_SENSOR("Sensor 3D Accel 2", MAV_SYS_STATUS_SENSOR_3D_ACCEL2);
00178                 STAT_ADD_SENSOR("Sensor 3D Mag 2", MAV_SYS_STATUS_SENSOR_3D_MAG2);
00179                 STAT_ADD_SENSOR("Geofence health", MAV_SYS_STATUS_GEOFENCE);
00180                 STAT_ADD_SENSOR("AHRS health", MAV_SYS_STATUS_AHRS);
00181                 STAT_ADD_SENSOR("Terrain health", MAV_SYS_STATUS_TERRAIN);
00182 
00183 #undef STAT_ADD_SENSOR
00184 
00185                 stat.addf("CPU Load (%)", "%.1f", last_st.load / 10.0);
00186                 stat.addf("Drop rate (%)", "%.1f", last_st.drop_rate_comm / 10.0);
00187                 stat.addf("Errors comm", "%d", last_st.errors_comm);
00188                 stat.addf("Errors count #1", "%d", last_st.errors_count1);
00189                 stat.addf("Errors count #2", "%d", last_st.errors_count2);
00190                 stat.addf("Errors count #3", "%d", last_st.errors_count3);
00191                 stat.addf("Errors count #4", "%d", last_st.errors_count4);
00192         }
00193 
00194 private:
00195         std::recursive_mutex mutex;
00196         mavlink_sys_status_t last_st;
00197 };
00198 
00199 
00203 class BatteryStatusDiag : public diagnostic_updater::DiagnosticTask
00204 {
00205 public:
00206         BatteryStatusDiag(const std::string &name) :
00207                 diagnostic_updater::DiagnosticTask(name),
00208                 voltage(-1.0),
00209                 current(0.0),
00210                 remaining(0.0),
00211                 min_voltage(6)
00212         {};
00213 
00214         void set_min_voltage(float volt) {
00215                 lock_guard lock(mutex);
00216                 min_voltage = volt;
00217         }
00218 
00219         void set(float volt, float curr, float rem) {
00220                 lock_guard lock(mutex);
00221                 voltage = volt;
00222                 current = curr;
00223                 remaining = rem;
00224         }
00225 
00226         void run(diagnostic_updater::DiagnosticStatusWrapper &stat) {
00227                 lock_guard lock(mutex);
00228 
00229                 if (voltage < 0)
00230                         stat.summary(2, "No data");
00231                 else if (voltage < min_voltage)
00232                         stat.summary(1, "Low voltage");
00233                 else
00234                         stat.summary(0, "Normal");
00235 
00236                 stat.addf("Voltage", "%.2f", voltage);
00237                 stat.addf("Current", "%.1f", current);
00238                 stat.addf("Remaining", "%.1f", remaining * 100);
00239         }
00240 
00241 private:
00242         std::recursive_mutex mutex;
00243         float voltage;
00244         float current;
00245         float remaining;
00246         float min_voltage;
00247 };
00248 
00249 
00253 class MemInfo : public diagnostic_updater::DiagnosticTask
00254 {
00255 public:
00256         MemInfo(const std::string &name) :
00257                 diagnostic_updater::DiagnosticTask(name),
00258                 freemem(-1),
00259                 brkval(0)
00260         {};
00261 
00262         void set(uint16_t f, uint16_t b) {
00263                 freemem = f;
00264                 brkval = b;
00265         }
00266 
00267         void run(diagnostic_updater::DiagnosticStatusWrapper &stat) {
00268                 ssize_t freemem_ = freemem;
00269                 uint16_t brkval_ = brkval;
00270 
00271                 if (freemem < 0)
00272                         stat.summary(2, "No data");
00273                 else if (freemem < 200)
00274                         stat.summary(1, "Low mem");
00275                 else
00276                         stat.summary(0, "Normal");
00277 
00278                 stat.addf("Free memory (B)", "%zd", freemem_);
00279                 stat.addf("Heap top", "0x%04X", brkval_);
00280         }
00281 
00282 private:
00283         std::atomic<ssize_t> freemem;
00284         std::atomic<uint16_t> brkval;
00285 };
00286 
00287 
00291 class HwStatus : public diagnostic_updater::DiagnosticTask
00292 {
00293 public:
00294         HwStatus(const std::string &name) :
00295                 diagnostic_updater::DiagnosticTask(name),
00296                 vcc(-1.0),
00297                 i2cerr(0),
00298                 i2cerr_last(0)
00299         {};
00300 
00301         void set(uint16_t v, uint8_t e) {
00302                 lock_guard lock(mutex);
00303                 vcc = v / 1000.0;
00304                 i2cerr = e;
00305         }
00306 
00307         void run(diagnostic_updater::DiagnosticStatusWrapper &stat) {
00308                 lock_guard lock(mutex);
00309 
00310                 if (vcc < 0)
00311                         stat.summary(2, "No data");
00312                 else if (vcc < 4.5)
00313                         stat.summary(1, "Low voltage");
00314                 else if (i2cerr != i2cerr_last) {
00315                         i2cerr_last = i2cerr;
00316                         stat.summary(1, "New I2C error");
00317                 }
00318                 else
00319                         stat.summary(0, "Normal");
00320 
00321                 stat.addf("Core voltage", "%f", vcc);
00322                 stat.addf("I2C errors", "%zu", i2cerr);
00323         }
00324 
00325 private:
00326         std::recursive_mutex mutex;
00327         float vcc;
00328         size_t i2cerr;
00329         size_t i2cerr_last;
00330 };
00331 
00332 
00338 class SystemStatusPlugin : public MavRosPlugin
00339 {
00340 public:
00341         SystemStatusPlugin() :
00342                 nh("~"),
00343                 uas(nullptr),
00344                 hb_diag("Heartbeat", 10),
00345                 mem_diag("APM Memory"),
00346                 hwst_diag("APM Hardware"),
00347                 sys_diag("System"),
00348                 batt_diag("Battery"),
00349                 version_retries(RETRIES_COUNT),
00350                 disable_diag(false)
00351         {};
00352 
00353         void initialize(UAS &uas_)
00354         {
00355                 uas = &uas_;
00356 
00357                 ros::Duration conn_heartbeat;
00358 
00359                 double conn_timeout_d;
00360                 double conn_heartbeat_d;
00361                 double min_voltage;
00362 
00363                 nh.param("conn/timeout", conn_timeout_d, 30.0);
00364                 nh.param("sys/min_voltage", min_voltage, 6.0);
00365                 nh.param("sys/disable_diag", disable_diag, false);
00366 
00367                 // rate parameter
00368                 if (nh.getParam("conn/heartbeat_rate", conn_heartbeat_d) && conn_heartbeat_d != 0.0) {
00369                         conn_heartbeat = ros::Duration(ros::Rate(conn_heartbeat_d));
00370                 }
00371                 else if (nh.getParam("conn/heartbeat", conn_heartbeat_d)) {
00372                         // XXX deprecated parameter
00373                         ROS_WARN_NAMED("sys", "SYS: parameter `~conn/heartbeat` deprecated, "
00374                                         "please use `~conn/heartbeat_rate` instead!");
00375                         conn_heartbeat = ros::Duration(conn_heartbeat_d);
00376                 }
00377 
00378                 // heartbeat diag always enabled
00379                 UAS_DIAG(uas).add(hb_diag);
00380                 if (!disable_diag) {
00381                         UAS_DIAG(uas).add(sys_diag);
00382                         UAS_DIAG(uas).add(batt_diag);
00383 
00384                         batt_diag.set_min_voltage(min_voltage);
00385                 }
00386 
00387 
00388                 // one-shot timeout timer
00389                 timeout_timer = nh.createTimer(ros::Duration(conn_timeout_d),
00390                                 &SystemStatusPlugin::timeout_cb, this, true);
00391                 timeout_timer.start();
00392 
00393                 if (!conn_heartbeat.isZero()) {
00394                         heartbeat_timer = nh.createTimer(conn_heartbeat,
00395                                         &SystemStatusPlugin::heartbeat_cb, this);
00396                         heartbeat_timer.start();
00397                 }
00398 
00399                 // version request timer
00400                 autopilot_version_timer = nh.createTimer(ros::Duration(1.0),
00401                                 &SystemStatusPlugin::autopilot_version_cb, this);
00402                 autopilot_version_timer.stop();
00403 
00404                 // subscribe to connection event
00405                 uas->sig_connection_changed.connect(boost::bind(&SystemStatusPlugin::connection_cb, this, _1));
00406 
00407                 state_pub = nh.advertise<mavros_msgs::State>("state", 10, true);
00408                 extended_state_pub = nh.advertise<mavros_msgs::ExtendedState>("extended_state", 10);
00409                 batt_pub = nh.advertise<mavros_msgs::BatteryStatus>("battery", 10);
00410                 rate_srv = nh.advertiseService("set_stream_rate", &SystemStatusPlugin::set_rate_cb, this);
00411                 mode_srv = nh.advertiseService("set_mode", &SystemStatusPlugin::set_mode_cb, this);
00412 
00413                 // init state topic
00414                 publish_disconnection();
00415         }
00416 
00417         const message_map get_rx_handlers() {
00418                 return {
00419                                MESSAGE_HANDLER(MAVLINK_MSG_ID_HEARTBEAT, &SystemStatusPlugin::handle_heartbeat),
00420                                MESSAGE_HANDLER(MAVLINK_MSG_ID_SYS_STATUS, &SystemStatusPlugin::handle_sys_status),
00421                                MESSAGE_HANDLER(MAVLINK_MSG_ID_STATUSTEXT, &SystemStatusPlugin::handle_statustext),
00422 #ifdef MAVLINK_MSG_ID_MEMINFO
00423                                MESSAGE_HANDLER(MAVLINK_MSG_ID_MEMINFO, &SystemStatusPlugin::handle_meminfo),
00424 #endif
00425 #ifdef MAVLINK_MSG_ID_HWSTATUS
00426                                MESSAGE_HANDLER(MAVLINK_MSG_ID_HWSTATUS, &SystemStatusPlugin::handle_hwstatus),
00427 #endif
00428                                MESSAGE_HANDLER(MAVLINK_MSG_ID_AUTOPILOT_VERSION, &SystemStatusPlugin::handle_autopilot_version),
00429                                MESSAGE_HANDLER(MAVLINK_MSG_ID_EXTENDED_SYS_STATE, &SystemStatusPlugin::handle_extended_sys_state),
00430                 };
00431         }
00432 
00433 private:
00434         ros::NodeHandle nh;
00435         UAS *uas;
00436 
00437         HeartbeatStatus hb_diag;
00438         MemInfo mem_diag;
00439         HwStatus hwst_diag;
00440         SystemStatusDiag sys_diag;
00441         BatteryStatusDiag batt_diag;
00442         ros::Timer timeout_timer;
00443         ros::Timer heartbeat_timer;
00444         ros::Timer autopilot_version_timer;
00445 
00446         ros::Publisher state_pub;
00447         ros::Publisher extended_state_pub;
00448         ros::Publisher batt_pub;
00449         ros::ServiceServer rate_srv;
00450         ros::ServiceServer mode_srv;
00451 
00452         static constexpr int RETRIES_COUNT = 6;
00453         int version_retries;
00454         bool disable_diag;
00455 
00456         /* -*- mid-level helpers -*- */
00457 
00463         void process_statustext_normal(uint8_t severity, std::string &text) {
00464                 switch (severity) {
00465                 case MAV_SEVERITY_EMERGENCY:
00466                 case MAV_SEVERITY_ALERT:
00467                 case MAV_SEVERITY_CRITICAL:
00468                 case MAV_SEVERITY_ERROR:
00469                         ROS_ERROR_STREAM_NAMED("fcu", "FCU: " << text);
00470                         break;
00471 
00472                 case MAV_SEVERITY_WARNING:
00473                 case MAV_SEVERITY_NOTICE:
00474                         ROS_WARN_STREAM_NAMED("fcu", "FCU: " << text);
00475                         break;
00476 
00477                 case MAV_SEVERITY_INFO:
00478                         ROS_INFO_STREAM_NAMED("fcu", "FCU: " << text);
00479                         break;
00480 
00481                 case MAV_SEVERITY_DEBUG:
00482                         ROS_DEBUG_STREAM_NAMED("fcu", "FCU: " << text);
00483                         break;
00484 
00485                 default:
00486                         ROS_WARN_STREAM_NAMED("fcu", "FCU: UNK(" <<
00487                                         int(severity) << "): " << text);
00488                         break;
00489                 };
00490         }
00491 
00492         static inline std::string custom_version_to_hex_string(uint8_t array[8])
00493         {
00494                 // inefficient, but who care for one time call function?
00495 
00496                 std::ostringstream ss;
00497                 ss << std::setfill('0');
00498 
00499                 for (ssize_t i = 7; i >= 0; i--)
00500                         ss << std::hex << std::setw(2) << int(array[i]);
00501 
00502                 return ss.str();
00503         }
00504 
00505         void process_autopilot_version_normal(mavlink_autopilot_version_t &apv, uint8_t sysid, uint8_t compid)
00506         {
00507                 char prefix[16];
00508                 ::snprintf(prefix, sizeof(prefix) - 1, "VER: %d.%d", sysid, compid);
00509 
00510                 ROS_INFO_NAMED("sys", "%s: Capabilities 0x%016llx", prefix, (long long int)apv.capabilities);
00511                 ROS_INFO_NAMED("sys", "%s: Flight software:     %08x (%s)",
00512                                 prefix,
00513                                 apv.flight_sw_version,
00514                                 custom_version_to_hex_string(apv.flight_custom_version).c_str());
00515                 ROS_INFO_NAMED("sys", "%s: Middleware software: %08x (%s)",
00516                                 prefix,
00517                                 apv.middleware_sw_version,
00518                                 custom_version_to_hex_string(apv.middleware_custom_version).c_str());
00519                 ROS_INFO_NAMED("sys", "%s: OS software:         %08x (%s)",
00520                                 prefix,
00521                                 apv.os_sw_version,
00522                                 custom_version_to_hex_string(apv.os_custom_version).c_str());
00523                 ROS_INFO_NAMED("sys", "%s: Board hardware:      %08x", prefix, apv.board_version);
00524                 ROS_INFO_NAMED("sys", "%s: VID/PID: %04x:%04x", prefix, apv.vendor_id, apv.product_id);
00525                 ROS_INFO_NAMED("sys", "%s: UID: %016llx", prefix, (long long int)apv.uid);
00526         }
00527 
00528         void process_autopilot_version_apm_quirk(mavlink_autopilot_version_t &apv, uint8_t sysid, uint8_t compid)
00529         {
00530                 char prefix[16];
00531                 ::snprintf(prefix, sizeof(prefix) - 1, "VER: %d.%d", sysid, compid);
00532 
00533                 // Note based on current APM's impl.
00534                 // APM uses custom version array[8] as a string
00535                 ROS_INFO_NAMED("sys", "%s: Capabilities 0x%016llx", prefix, (long long int)apv.capabilities);
00536                 ROS_INFO_NAMED("sys", "%s: Flight software:     %08x (%*s)",
00537                                 prefix,
00538                                 apv.flight_sw_version,
00539                                 8, apv.flight_custom_version);
00540                 ROS_INFO_NAMED("sys", "%s: Middleware software: %08x (%*s)",
00541                                 prefix,
00542                                 apv.middleware_sw_version,
00543                                 8, apv.middleware_custom_version);
00544                 ROS_INFO_NAMED("sys", "%s: OS software:         %08x (%*s)",
00545                                 prefix,
00546                                 apv.os_sw_version,
00547                                 8, apv.os_custom_version);
00548                 ROS_INFO_NAMED("sys", "%s: Board hardware:      %08x", prefix, apv.board_version);
00549                 ROS_INFO_NAMED("sys", "%s: VID/PID: %04x:%04x", prefix, apv.vendor_id, apv.product_id);
00550                 ROS_INFO_NAMED("sys", "%s: UID: %016llx", prefix, (long long int)apv.uid);
00551         }
00552 
00553         void publish_disconnection() {
00554                 auto state_msg = boost::make_shared<mavros_msgs::State>();
00555                 state_msg->header.stamp = ros::Time::now();
00556                 state_msg->connected = false;
00557                 state_msg->armed = false;
00558                 state_msg->guided = false;
00559                 state_msg->mode = "";
00560 
00561                 state_pub.publish(state_msg);
00562         }
00563 
00564         /* -*- message handlers -*- */
00565 
00566         void handle_heartbeat(const mavlink_message_t *msg, uint8_t sysid, uint8_t compid) {
00567                 if (!uas->is_my_target(sysid)) {
00568                         ROS_DEBUG_NAMED("sys", "HEARTBEAT from [%d, %d] dropped.", sysid, compid);
00569                         return;
00570                 }
00571 
00572                 mavlink_heartbeat_t hb;
00573                 mavlink_msg_heartbeat_decode(msg, &hb);
00574 
00575                 // update context && setup connection timeout
00576                 uas->update_heartbeat(hb.type, hb.autopilot, hb.base_mode);
00577                 uas->update_connection_status(true);
00578                 timeout_timer.stop();
00579                 timeout_timer.start();
00580 
00581                 // build state message after updating uas
00582                 auto state_msg = boost::make_shared<mavros_msgs::State>();
00583                 state_msg->header.stamp = ros::Time::now();
00584                 state_msg->connected = true;
00585                 state_msg->armed = hb.base_mode & MAV_MODE_FLAG_SAFETY_ARMED;
00586                 state_msg->guided = hb.base_mode & MAV_MODE_FLAG_GUIDED_ENABLED;
00587                 state_msg->mode = uas->str_mode_v10(hb.base_mode, hb.custom_mode);
00588 
00589                 state_pub.publish(state_msg);
00590                 hb_diag.tick(hb.type, hb.autopilot, state_msg->mode, hb.system_status);
00591         }
00592 
00593         void handle_extended_sys_state(const mavlink_message_t *msg, uint8_t sysid, uint8_t compid) {
00594                 mavlink_extended_sys_state_t state;
00595                 mavlink_msg_extended_sys_state_decode(msg, &state);
00596 
00597                 auto state_msg = boost::make_shared<mavros_msgs::ExtendedState>();
00598                 state_msg->header.stamp = ros::Time::now();
00599                 state_msg->vtol_state = state.vtol_state;
00600                 state_msg->landed_state = state.landed_state;
00601 
00602                 extended_state_pub.publish(state_msg);
00603         }
00604 
00605         void handle_sys_status(const mavlink_message_t *msg, uint8_t sysid, uint8_t compid) {
00606                 mavlink_sys_status_t stat;
00607                 mavlink_msg_sys_status_decode(msg, &stat);
00608 
00609                 float volt = stat.voltage_battery / 1000.0f;    // mV
00610                 float curr = stat.current_battery / 100.0f;     // 10 mA or -1
00611                 float rem = stat.battery_remaining / 100.0f;    // or -1
00612 
00613                 auto batt_msg = boost::make_shared<mavros_msgs::BatteryStatus>();
00614                 batt_msg->header.stamp = ros::Time::now();
00615                 batt_msg->voltage = volt;
00616                 batt_msg->current = curr;
00617                 batt_msg->remaining = rem;
00618 
00619                 sys_diag.set(stat);
00620                 batt_diag.set(volt, curr, rem);
00621                 batt_pub.publish(batt_msg);
00622         }
00623 
00624         void handle_statustext(const mavlink_message_t *msg, uint8_t sysid, uint8_t compid) {
00625                 mavlink_statustext_t textm;
00626                 mavlink_msg_statustext_decode(msg, &textm);
00627 
00628                 std::string text(textm.text,
00629                                 strnlen(textm.text, sizeof(textm.text)));
00630 
00631                 process_statustext_normal(textm.severity, text);
00632         }
00633 
00634 #ifdef MAVLINK_MSG_ID_MEMINFO
00635         void handle_meminfo(const mavlink_message_t *msg, uint8_t sysid, uint8_t compid) {
00636                 mavlink_meminfo_t mem;
00637                 mavlink_msg_meminfo_decode(msg, &mem);
00638                 mem_diag.set(mem.freemem, mem.brkval);
00639         }
00640 #endif
00641 
00642 #ifdef MAVLINK_MSG_ID_HWSTATUS
00643         void handle_hwstatus(const mavlink_message_t *msg, uint8_t sysid, uint8_t compid) {
00644                 mavlink_hwstatus_t hwst;
00645                 mavlink_msg_hwstatus_decode(msg, &hwst);
00646                 hwst_diag.set(hwst.Vcc, hwst.I2Cerr);
00647         }
00648 #endif
00649 
00650         void handle_autopilot_version(const mavlink_message_t *msg, uint8_t sysid, uint8_t compid) {
00651                 mavlink_autopilot_version_t apv;
00652                 mavlink_msg_autopilot_version_decode(msg, &apv);
00653 
00654                 // we want to store only FCU caps
00655                 if (uas->is_my_target(sysid, compid)) {
00656                         autopilot_version_timer.stop();
00657                         uas->update_capabilities(true, apv.capabilities);
00658                 }
00659 
00660                 // but print all version responses
00661                 if (uas->is_ardupilotmega())
00662                         process_autopilot_version_apm_quirk(apv, sysid, compid);
00663                 else
00664                         process_autopilot_version_normal(apv, sysid, compid);
00665         }
00666 
00667         /* -*- timer callbacks -*- */
00668 
00669         void timeout_cb(const ros::TimerEvent &event) {
00670                 uas->update_connection_status(false);
00671         }
00672 
00673         void heartbeat_cb(const ros::TimerEvent &event) {
00674                 mavlink_message_t msg;
00675                 mavlink_msg_heartbeat_pack_chan(UAS_PACK_CHAN(uas), &msg,
00676                                 MAV_TYPE_ONBOARD_CONTROLLER,
00677                                 MAV_AUTOPILOT_INVALID,
00678                                 MAV_MODE_MANUAL_ARMED,
00679                                 0,
00680                                 MAV_STATE_ACTIVE
00681                                 );
00682 
00683                 UAS_FCU(uas)->send_message(&msg);
00684         }
00685 
00686         void autopilot_version_cb(const ros::TimerEvent &event) {
00687                 bool ret = false;
00688 
00689                 // Request from all first 3 times, then fallback to unicast
00690                 bool do_broadcast = version_retries > RETRIES_COUNT / 2;
00691 
00692                 try {
00693                         auto client = nh.serviceClient<mavros_msgs::CommandLong>("cmd/command");
00694 
00695                         mavros_msgs::CommandLong cmd{};
00696 
00697                         cmd.request.broadcast = do_broadcast;
00698                         cmd.request.command = MAV_CMD_REQUEST_AUTOPILOT_CAPABILITIES;
00699                         cmd.request.confirmation = false;
00700                         cmd.request.param1 = 1.0;
00701 
00702                         ROS_DEBUG_NAMED("sys", "VER: Sending %s request.",
00703                                         (do_broadcast) ? "broadcast" : "unicast");
00704                         ret = client.call(cmd);
00705                 }
00706                 catch (ros::InvalidNameException &ex) {
00707                         ROS_ERROR_NAMED("sys", "VER: %s", ex.what());
00708                 }
00709 
00710                 ROS_ERROR_COND_NAMED(!ret, "sys", "VER: command plugin service call failed!");
00711 
00712                 if (version_retries > 0) {
00713                         version_retries--;
00714                         ROS_WARN_COND_NAMED(version_retries != RETRIES_COUNT - 1, "sys",
00715                                         "VER: %s request timeout, retries left %d",
00716                                         (do_broadcast) ? "broadcast" : "unicast",
00717                                         version_retries);
00718                 }
00719                 else {
00720                         uas->update_capabilities(false);
00721                         autopilot_version_timer.stop();
00722                         ROS_WARN_NAMED("sys", "VER: your FCU don't support AUTOPILOT_VERSION, "
00723                                         "switched to default capabilities");
00724                 }
00725         }
00726 
00727         void connection_cb(bool connected) {
00728                 // if connection changes, start delayed version request
00729                 version_retries = RETRIES_COUNT;
00730                 if (connected)
00731                         autopilot_version_timer.start();
00732                 else
00733                         autopilot_version_timer.stop();
00734 
00735                 // add/remove APM diag tasks
00736                 if (connected && disable_diag && uas->is_ardupilotmega()) {
00737 #ifdef MAVLINK_MSG_ID_MEMINFO
00738                         UAS_DIAG(uas).add(mem_diag);
00739 #endif
00740 #ifdef MAVLINK_MSG_ID_HWSTATUS
00741                         UAS_DIAG(uas).add(hwst_diag);
00742 #endif
00743 #if !defined(MAVLINK_MSG_ID_MEMINFO) || !defined(MAVLINK_MSG_ID_HWSTATUS)
00744                         ROS_INFO_NAMED("sys", "SYS: APM detected, but mavros uses different dialect. "
00745                                         "Extra diagnostic disabled.");
00746 #else
00747                         ROS_DEBUG_NAMED("sys", "SYS: APM extra diagnostics enabled.");
00748 #endif
00749                 }
00750                 else {
00751                         UAS_DIAG(uas).removeByName(mem_diag.getName());
00752                         UAS_DIAG(uas).removeByName(hwst_diag.getName());
00753                         ROS_DEBUG_NAMED("sys", "SYS: APM extra diagnostics disabled.");
00754                 }
00755 
00756                 if (!connected) {
00757                         // publish connection change
00758                         publish_disconnection();
00759                 }
00760         }
00761 
00762         /* -*- ros callbacks -*- */
00763 
00764         bool set_rate_cb(mavros_msgs::StreamRate::Request &req,
00765                         mavros_msgs::StreamRate::Response &res) {
00766                 mavlink_message_t msg;
00767                 mavlink_msg_request_data_stream_pack_chan(UAS_PACK_CHAN(uas), &msg,
00768                                 UAS_PACK_TGT(uas),
00769                                 req.stream_id,
00770                                 req.message_rate,
00771                                 (req.on_off) ? 1 : 0
00772                                 );
00773 
00774                 UAS_FCU(uas)->send_message(&msg);
00775                 return true;
00776         }
00777 
00778         bool set_mode_cb(mavros_msgs::SetMode::Request &req,
00779                         mavros_msgs::SetMode::Response &res) {
00780                 mavlink_message_t msg;
00781                 uint8_t base_mode = req.base_mode;
00782                 uint32_t custom_mode = 0;
00783 
00784                 if (req.custom_mode != "") {
00785                         if (!uas->cmode_from_str(req.custom_mode, custom_mode)) {
00786                                 res.success = false;
00787                                 return true;
00788                         }
00789 
00795                         base_mode |= (uas->get_armed()) ? MAV_MODE_FLAG_SAFETY_ARMED : 0;
00796                         base_mode |= (uas->get_hil_state()) ? MAV_MODE_FLAG_HIL_ENABLED : 0;
00797                         base_mode |= MAV_MODE_FLAG_CUSTOM_MODE_ENABLED;
00798                 }
00799 
00800                 mavlink_msg_set_mode_pack_chan(UAS_PACK_CHAN(uas), &msg,
00801                                 uas->get_tgt_system(),
00802                                 base_mode,
00803                                 custom_mode);
00804                 UAS_FCU(uas)->send_message(&msg);
00805                 res.success = true;
00806                 return true;
00807         }
00808 };
00809 };      // namespace mavplugin
00810 
00811 PLUGINLIB_EXPORT_CLASS(mavplugin::SystemStatusPlugin, mavplugin::MavRosPlugin)
00812 


mavros
Author(s): Vladimir Ermakov
autogenerated on Thu Feb 9 2017 04:00:17