20 #include <mavros_msgs/State.h> 21 #include <mavros_msgs/EstimatorStatus.h> 22 #include <mavros_msgs/ExtendedState.h> 23 #include <mavros_msgs/StreamRate.h> 24 #include <mavros_msgs/SetMode.h> 25 #include <mavros_msgs/CommandLong.h> 26 #include <mavros_msgs/StatusText.h> 27 #include <mavros_msgs/VehicleInfo.h> 28 #include <mavros_msgs/VehicleInfoGet.h> 29 #include <mavros_msgs/MessageInterval.h> 32 #ifdef HAVE_SENSOR_MSGS_BATTERYSTATE_MSG 33 #include <sensor_msgs/BatteryState.h> 36 #include <mavros_msgs/BatteryStatus.h> 41 namespace std_plugins {
42 using mavlink::minimal::MAV_TYPE;
43 using mavlink::minimal::MAV_AUTOPILOT;
44 using mavlink::minimal::MAV_STATE;
47 #define MAX_NR_BATTERY_STATUS 10 74 std::lock_guard<std::mutex> lock(
mutex);
86 void tick(uint8_t type_, uint8_t autopilot_,
87 std::string &mode_, uint8_t system_status_)
89 std::lock_guard<std::mutex> lock(
mutex);
93 autopilot =
static_cast<MAV_AUTOPILOT
>(autopilot_);
100 std::lock_guard<std::mutex> lock(
mutex);
106 double freq = events / window;
112 stat.
summary(diagnostic_msgs::DiagnosticStatus::ERROR,
"No events recorded.");
115 stat.
summary(diagnostic_msgs::DiagnosticStatus::WARN,
"Frequency too low.");
118 stat.
summary(diagnostic_msgs::DiagnosticStatus::WARN,
"Frequency too high.");
121 stat.
summary(diagnostic_msgs::DiagnosticStatus::OK,
"Normal");
124 stat.
addf(
"Heartbeats since startup",
"%d",
count_);
125 stat.
addf(
"Frequency (Hz)",
"%f", freq);
161 void set(mavlink::common::msg::SYS_STATUS &st)
163 std::lock_guard<std::mutex> lock(
mutex);
168 std::lock_guard<std::mutex> lock(
mutex);
170 if ((last_st.onboard_control_sensors_health & last_st.onboard_control_sensors_enabled)
171 != last_st.onboard_control_sensors_enabled)
172 stat.
summary(diagnostic_msgs::DiagnosticStatus::ERROR,
"Sensor health");
174 stat.
summary(diagnostic_msgs::DiagnosticStatus::OK,
"Normal");
176 stat.
addf(
"Sensor present",
"0x%08X", last_st.onboard_control_sensors_present);
177 stat.
addf(
"Sensor enabled",
"0x%08X", last_st.onboard_control_sensors_enabled);
178 stat.
addf(
"Sensor health",
"0x%08X", last_st.onboard_control_sensors_health);
180 using STS = mavlink::common::MAV_SYS_STATUS_SENSOR;
205 if (last_st.onboard_control_sensors_enabled &
enum_value(STS::SENSOR_3D_GYRO))
206 stat.
add(
"3D gyro", (last_st.onboard_control_sensors_health &
enum_value(STS::SENSOR_3D_GYRO)) ?
"Ok" :
"Fail");
207 if (last_st.onboard_control_sensors_enabled &
enum_value(STS::SENSOR_3D_ACCEL))
208 stat.
add(
"3D accelerometer", (last_st.onboard_control_sensors_health &
enum_value(STS::SENSOR_3D_ACCEL)) ?
"Ok" :
"Fail");
209 if (last_st.onboard_control_sensors_enabled &
enum_value(STS::SENSOR_3D_MAG))
210 stat.
add(
"3D magnetometer", (last_st.onboard_control_sensors_health &
enum_value(STS::SENSOR_3D_MAG)) ?
"Ok" :
"Fail");
211 if (last_st.onboard_control_sensors_enabled &
enum_value(STS::ABSOLUTE_PRESSURE))
212 stat.
add(
"absolute pressure", (last_st.onboard_control_sensors_health &
enum_value(STS::ABSOLUTE_PRESSURE)) ?
"Ok" :
"Fail");
213 if (last_st.onboard_control_sensors_enabled &
enum_value(STS::DIFFERENTIAL_PRESSURE))
214 stat.
add(
"differential pressure", (last_st.onboard_control_sensors_health &
enum_value(STS::DIFFERENTIAL_PRESSURE)) ?
"Ok" :
"Fail");
215 if (last_st.onboard_control_sensors_enabled &
enum_value(STS::GPS))
216 stat.
add(
"GPS", (last_st.onboard_control_sensors_health &
enum_value(STS::GPS)) ?
"Ok" :
"Fail");
217 if (last_st.onboard_control_sensors_enabled &
enum_value(STS::OPTICAL_FLOW))
218 stat.
add(
"optical flow", (last_st.onboard_control_sensors_health &
enum_value(STS::OPTICAL_FLOW)) ?
"Ok" :
"Fail");
219 if (last_st.onboard_control_sensors_enabled &
enum_value(STS::VISION_POSITION))
220 stat.
add(
"computer vision position", (last_st.onboard_control_sensors_health &
enum_value(STS::VISION_POSITION)) ?
"Ok" :
"Fail");
221 if (last_st.onboard_control_sensors_enabled &
enum_value(STS::LASER_POSITION))
222 stat.
add(
"laser based position", (last_st.onboard_control_sensors_health &
enum_value(STS::LASER_POSITION)) ?
"Ok" :
"Fail");
223 if (last_st.onboard_control_sensors_enabled &
enum_value(STS::EXTERNAL_GROUND_TRUTH))
224 stat.
add(
"external ground truth (Vicon or Leica)", (last_st.onboard_control_sensors_health &
enum_value(STS::EXTERNAL_GROUND_TRUTH)) ?
"Ok" :
"Fail");
225 if (last_st.onboard_control_sensors_enabled &
enum_value(STS::ANGULAR_RATE_CONTROL))
226 stat.
add(
"3D angular rate control", (last_st.onboard_control_sensors_health &
enum_value(STS::ANGULAR_RATE_CONTROL)) ?
"Ok" :
"Fail");
227 if (last_st.onboard_control_sensors_enabled &
enum_value(STS::ATTITUDE_STABILIZATION))
228 stat.
add(
"attitude stabilization", (last_st.onboard_control_sensors_health &
enum_value(STS::ATTITUDE_STABILIZATION)) ?
"Ok" :
"Fail");
229 if (last_st.onboard_control_sensors_enabled &
enum_value(STS::YAW_POSITION))
230 stat.
add(
"yaw position", (last_st.onboard_control_sensors_health &
enum_value(STS::YAW_POSITION)) ?
"Ok" :
"Fail");
231 if (last_st.onboard_control_sensors_enabled &
enum_value(STS::Z_ALTITUDE_CONTROL))
232 stat.
add(
"z/altitude control", (last_st.onboard_control_sensors_health &
enum_value(STS::Z_ALTITUDE_CONTROL)) ?
"Ok" :
"Fail");
233 if (last_st.onboard_control_sensors_enabled &
enum_value(STS::XY_POSITION_CONTROL))
234 stat.
add(
"x/y position control", (last_st.onboard_control_sensors_health &
enum_value(STS::XY_POSITION_CONTROL)) ?
"Ok" :
"Fail");
235 if (last_st.onboard_control_sensors_enabled &
enum_value(STS::MOTOR_OUTPUTS))
236 stat.
add(
"motor outputs / control", (last_st.onboard_control_sensors_health &
enum_value(STS::MOTOR_OUTPUTS)) ?
"Ok" :
"Fail");
237 if (last_st.onboard_control_sensors_enabled &
enum_value(STS::RC_RECEIVER))
238 stat.
add(
"rc receiver", (last_st.onboard_control_sensors_health &
enum_value(STS::RC_RECEIVER)) ?
"Ok" :
"Fail");
239 if (last_st.onboard_control_sensors_enabled &
enum_value(STS::SENSOR_3D_GYRO2))
240 stat.
add(
"2nd 3D gyro", (last_st.onboard_control_sensors_health &
enum_value(STS::SENSOR_3D_GYRO2)) ?
"Ok" :
"Fail");
241 if (last_st.onboard_control_sensors_enabled &
enum_value(STS::SENSOR_3D_ACCEL2))
242 stat.
add(
"2nd 3D accelerometer", (last_st.onboard_control_sensors_health &
enum_value(STS::SENSOR_3D_ACCEL2)) ?
"Ok" :
"Fail");
243 if (last_st.onboard_control_sensors_enabled &
enum_value(STS::SENSOR_3D_MAG2))
244 stat.
add(
"2nd 3D magnetometer", (last_st.onboard_control_sensors_health &
enum_value(STS::SENSOR_3D_MAG2)) ?
"Ok" :
"Fail");
245 if (last_st.onboard_control_sensors_enabled &
enum_value(STS::GEOFENCE))
246 stat.
add(
"geofence", (last_st.onboard_control_sensors_health &
enum_value(STS::GEOFENCE)) ?
"Ok" :
"Fail");
247 if (last_st.onboard_control_sensors_enabled &
enum_value(STS::AHRS))
248 stat.
add(
"AHRS subsystem health", (last_st.onboard_control_sensors_health &
enum_value(STS::AHRS)) ?
"Ok" :
"Fail");
249 if (last_st.onboard_control_sensors_enabled &
enum_value(STS::TERRAIN))
250 stat.
add(
"Terrain subsystem health", (last_st.onboard_control_sensors_health &
enum_value(STS::TERRAIN)) ?
"Ok" :
"Fail");
251 if (last_st.onboard_control_sensors_enabled &
enum_value(STS::REVERSE_MOTOR))
252 stat.
add(
"Motors are reversed", (last_st.onboard_control_sensors_health &
enum_value(STS::REVERSE_MOTOR)) ?
"Ok" :
"Fail");
253 if (last_st.onboard_control_sensors_enabled &
enum_value(STS::LOGGING))
254 stat.
add(
"Logging", (last_st.onboard_control_sensors_health &
enum_value(STS::LOGGING)) ?
"Ok" :
"Fail");
255 if (last_st.onboard_control_sensors_enabled &
enum_value(STS::BATTERY))
256 stat.
add(
"Battery", (last_st.onboard_control_sensors_health &
enum_value(STS::BATTERY)) ?
"Ok" :
"Fail");
257 if (last_st.onboard_control_sensors_enabled &
enum_value(STS::PROXIMITY))
258 stat.
add(
"Proximity", (last_st.onboard_control_sensors_health &
enum_value(STS::PROXIMITY)) ?
"Ok" :
"Fail");
259 if (last_st.onboard_control_sensors_enabled &
enum_value(STS::SATCOM))
260 stat.
add(
"Satellite Communication", (last_st.onboard_control_sensors_health &
enum_value(STS::SATCOM)) ?
"Ok" :
"Fail");
261 if (last_st.onboard_control_sensors_enabled &
enum_value(STS::PREARM_CHECK))
262 stat.
add(
"pre-arm check status. Always healthy when armed", (last_st.onboard_control_sensors_health &
enum_value(STS::PREARM_CHECK)) ?
"Ok" :
"Fail");
263 if (last_st.onboard_control_sensors_enabled &
enum_value(STS::OBSTACLE_AVOIDANCE))
264 stat.
add(
"Avoidance/collision prevention", (last_st.onboard_control_sensors_health &
enum_value(STS::OBSTACLE_AVOIDANCE)) ?
"Ok" :
"Fail");
265 if (last_st.onboard_control_sensors_enabled &
enum_value(STS::PROPULSION))
266 stat.
add(
"propulsion (actuator, esc, motor or propellor)", (last_st.onboard_control_sensors_health &
enum_value(STS::PROPULSION)) ?
"Ok" :
"Fail");
269 stat.
addf(
"CPU Load (%)",
"%.1f", last_st.load / 10.0);
270 stat.
addf(
"Drop rate (%)",
"%.1f", last_st.drop_rate_comm / 10.0);
271 stat.
addf(
"Errors comm",
"%d", last_st.errors_comm);
272 stat.
addf(
"Errors count #1",
"%d", last_st.errors_count1);
273 stat.
addf(
"Errors count #2",
"%d", last_st.errors_count2);
274 stat.
addf(
"Errors count #3",
"%d", last_st.errors_count3);
275 stat.
addf(
"Errors count #4",
"%d", last_st.errors_count4);
307 *
this = std::move(other);
315 *
this = std::move(other);
321 std::lock_guard<std::mutex> lock(
mutex);
325 void set(
float volt,
float curr,
float rem) {
326 std::lock_guard<std::mutex> lock(
mutex);
333 std::lock_guard<std::mutex> lock(
mutex);
334 cell_voltage = voltages;
339 std::lock_guard<std::mutex> lock(
mutex);
342 stat.
summary(diagnostic_msgs::DiagnosticStatus::ERROR,
"No data");
343 else if (voltage < min_voltage)
344 stat.
summary(diagnostic_msgs::DiagnosticStatus::WARN,
"Low voltage");
346 stat.
summary(diagnostic_msgs::DiagnosticStatus::OK,
"Normal");
348 stat.
addf(
"Voltage",
"%.2f", voltage);
349 stat.
addf(
"Current",
"%.1f", current);
350 stat.
addf(
"Remaining",
"%.1f", remaining * 100.0
f);
351 const int nr_cells = cell_voltage.size();
354 for (
int i = 1; i <= nr_cells ; ++i) {
383 void set(uint32_t
f, uint16_t
b) {
392 size_t freemem_ = freemem;
393 uint16_t brkval_ = brkval;
395 last_rcd_.
fromNSec(last_rcd.load());
396 constexpr
int timeout = 10.0;
400 stat.
summary(diagnostic_msgs::DiagnosticStatus::ERROR,
"Not initialised");
402 stat.
summary(diagnostic_msgs::DiagnosticStatus::STALE,
"Not received for more than " +
std::to_string(timeout) +
"s");
404 if (freemem == UINT32_MAX)
405 stat.
summary(diagnostic_msgs::DiagnosticStatus::ERROR,
"No data");
406 else if (freemem < 200)
407 stat.
summary(diagnostic_msgs::DiagnosticStatus::WARN,
"Low mem");
409 stat.
summary(diagnostic_msgs::DiagnosticStatus::OK,
"Normal");
411 stat.
addf(
"Free memory (B)",
"%zd", freemem_);
412 stat.
addf(
"Heap top",
"0x%04X", brkval_);
436 void set(uint16_t v, uint8_t
e) {
437 std::lock_guard<std::mutex> lock(
mutex);
445 std::lock_guard<std::mutex> lock(
mutex);
446 constexpr
int timeout = 10.0;
447 if (last_rcd.isZero()) {
448 stat.
summary(diagnostic_msgs::DiagnosticStatus::ERROR,
"Not initialised");
450 stat.
summary(diagnostic_msgs::DiagnosticStatus::STALE,
"Not received for more than " +
std::to_string(timeout) +
"s");
453 stat.
summary(diagnostic_msgs::DiagnosticStatus::ERROR,
"No data");
455 stat.
summary(diagnostic_msgs::DiagnosticStatus::WARN,
"Low voltage");
456 else if (i2cerr != i2cerr_last) {
457 i2cerr_last = i2cerr;
458 stat.
summary(diagnostic_msgs::DiagnosticStatus::WARN,
"New I2C error");
461 stat.
summary(diagnostic_msgs::DiagnosticStatus::OK,
"Normal");
463 stat.
addf(
"Core voltage",
"%f", vcc);
464 stat.
addf(
"I2C errors",
"%zu", i2cerr);
486 hb_diag(
"Heartbeat", 10),
487 mem_diag(
"APM Memory"),
488 hwst_diag(
"APM Hardware"),
490 conn_heartbeat_mav_type(
MAV_TYPE::ONBOARD_CONTROLLER),
491 version_retries(RETRIES_COUNT),
493 has_battery_status0(false)
496 batt_diag.emplace_back(
"Battery");
504 PluginBase::initialize(uas_);
508 double conn_timeout_d;
509 double conn_heartbeat_d;
510 std::vector<double> min_voltage;
511 std::string conn_heartbeat_mav_type_str;
513 nh.param(
"conn/timeout", conn_timeout_d, 10.0);
514 nh.param(
"sys/min_voltage", min_voltage, {10.0});
515 nh.param(
"sys/disable_diag", disable_diag,
false);
518 if (nh.getParam(
"conn/heartbeat_rate", conn_heartbeat_d) && conn_heartbeat_d != 0.0) {
523 if (nh.getParam(
"conn/heartbeat_mav_type", conn_heartbeat_mav_type_str)) {
532 batt_diag[i].set_min_voltage(min_voltage[i]);
543 if (!conn_heartbeat.
isZero()) {
544 heartbeat_timer = nh.createWallTimer(conn_heartbeat,
552 autopilot_version_timer.stop();
554 state_pub = nh.advertise<mavros_msgs::State>(
"state", 10,
true);
555 extended_state_pub = nh.advertise<mavros_msgs::ExtendedState>(
"extended_state", 10);
556 batt_pub = nh.advertise<
BatteryMsg>(
"battery", 10);
557 estimator_status_pub = nh.advertise<mavros_msgs::EstimatorStatus>(
"estimator_status", 10);
558 statustext_pub = nh.advertise<mavros_msgs::StatusText>(
"statustext/recv", 10);
566 publish_disconnection();
567 enable_connection_cb();
608 static constexpr
int RETRIES_COUNT = 6;
613 using M_VehicleInfo = std::unordered_map<uint16_t, mavros_msgs::VehicleInfo>;
620 return sysid << 8 | compid;
625 auto key = get_vehicle_key(sysid, compid);
626 M_VehicleInfo::iterator ret = vehicles.find(key);
628 if (ret == vehicles.end()) {
630 mavros_msgs::VehicleInfo v;
633 v.available_info = 0;
635 auto res = vehicles.emplace(key, v);
650 using mavlink::common::MAV_SEVERITY;
692 memcpy(&b, array.data(),
sizeof(uint64_t));
701 std::snprintf(prefix,
sizeof(prefix),
"VER: %d.%d", sysid, compid);
703 ROS_INFO_NAMED(
"sys",
"%s: Capabilities 0x%016llx", prefix, (
long long int)apv.capabilities);
706 apv.flight_sw_version,
707 custom_version_to_hex_string(apv.flight_custom_version).c_str());
710 apv.middleware_sw_version,
711 custom_version_to_hex_string(apv.middleware_custom_version).c_str());
715 custom_version_to_hex_string(apv.os_custom_version).c_str());
716 ROS_INFO_NAMED(
"sys",
"%s: Board hardware: %08x", prefix, apv.board_version);
717 ROS_INFO_NAMED(
"sys",
"%s: VID/PID: %04x:%04x", prefix, apv.vendor_id, apv.product_id);
718 ROS_INFO_NAMED(
"sys",
"%s: UID: %016llx", prefix, (
long long int)apv.uid);
724 std::snprintf(prefix,
sizeof(prefix),
"VER: %d.%d", sysid, compid);
728 ROS_INFO_NAMED(
"sys",
"%s: Capabilities 0x%016llx", prefix, (
long long int)apv.capabilities);
731 apv.flight_sw_version,
732 8, apv.flight_custom_version.data());
735 apv.middleware_sw_version,
736 8, apv.middleware_custom_version.data());
740 8, apv.os_custom_version.data());
741 ROS_INFO_NAMED(
"sys",
"%s: Board hardware: %08x", prefix, apv.board_version);
742 ROS_INFO_NAMED(
"sys",
"%s: VID/PID: %04x:%04x", prefix, apv.vendor_id, apv.product_id);
743 ROS_INFO_NAMED(
"sys",
"%s: UID: %016llx", prefix, (
long long int)apv.uid);
747 auto state_msg = boost::make_shared<mavros_msgs::State>();
749 state_msg->connected =
false;
750 state_msg->armed =
false;
751 state_msg->guided =
false;
752 state_msg->mode =
"";
753 state_msg->system_status =
enum_value(MAV_STATE::UNINIT);
760 void handle_heartbeat(
const mavlink::mavlink_message_t *msg, mavlink::minimal::msg::HEARTBEAT &hb)
762 using mavlink::minimal::MAV_MODE_FLAG;
765 auto it = find_or_create_vehicle_info(msg->sysid, msg->compid);
767 auto vehicle_mode = m_uas->str_mode_v10(hb.base_mode, hb.custom_mode);
771 it->second.header.stamp = stamp;
772 it->second.available_info |= mavros_msgs::VehicleInfo::HAVE_INFO_HEARTBEAT;
773 it->second.autopilot = hb.autopilot;
774 it->second.type = hb.type;
775 it->second.system_status = hb.system_status;
776 it->second.base_mode = hb.base_mode;
777 it->second.custom_mode = hb.custom_mode;
778 it->second.mode = vehicle_mode;
780 if (!(hb.base_mode &
enum_value(MAV_MODE_FLAG::CUSTOM_MODE_ENABLED))) {
781 it->second.mode_id = hb.base_mode;
783 it->second.mode_id = hb.custom_mode;
787 if (!m_uas->is_my_target(msg->sysid, msg->compid)) {
788 ROS_DEBUG_NAMED(
"sys",
"HEARTBEAT from [%d, %d] dropped.", msg->sysid, msg->compid);
793 m_uas->update_heartbeat(hb.type, hb.autopilot, hb.base_mode);
794 m_uas->update_connection_status(
true);
795 timeout_timer.
stop();
796 timeout_timer.
start();
799 auto state_msg = boost::make_shared<mavros_msgs::State>();
800 state_msg->header.stamp = stamp;
801 state_msg->connected =
true;
802 state_msg->armed = !!(hb.base_mode &
enum_value(MAV_MODE_FLAG::SAFETY_ARMED));
803 state_msg->guided = !!(hb.base_mode &
enum_value(MAV_MODE_FLAG::GUIDED_ENABLED));
804 state_msg->manual_input = !!(hb.base_mode &
enum_value(MAV_MODE_FLAG::MANUAL_INPUT_ENABLED));
805 state_msg->mode = vehicle_mode;
806 state_msg->system_status = hb.system_status;
809 hb_diag.
tick(hb.type, hb.autopilot, state_msg->mode, hb.system_status);
814 auto state_msg = boost::make_shared<mavros_msgs::ExtendedState>();
816 state_msg->vtol_state = state.vtol_state;
817 state_msg->landed_state = state.landed_state;
819 extended_state_pub.
publish(state_msg);
822 void handle_sys_status(
const mavlink::mavlink_message_t *msg, mavlink::common::msg::SYS_STATUS &stat)
824 using MC = mavlink::minimal::MAV_COMPONENT;
825 if (static_cast<MC>(msg->compid) == MC::COMP_ID_GIMBAL) {
829 float volt = stat.voltage_battery * 0.001f;
830 float curr = stat.current_battery * 0.01f;
831 float rem = stat.battery_remaining * 0.01f;
835 if (has_battery_status0)
838 batt_diag[0].set(volt, curr, rem);
839 auto batt_msg = boost::make_shared<BatteryMsg>();
842 #ifdef HAVE_SENSOR_MSGS_BATTERYSTATE_MSG 843 batt_msg->voltage = volt;
844 batt_msg->current = -curr;
845 batt_msg->charge = NAN;
846 batt_msg->capacity = NAN;
847 batt_msg->design_capacity = NAN;
848 batt_msg->percentage = rem;
849 batt_msg->power_supply_status = BatteryMsg::POWER_SUPPLY_STATUS_DISCHARGING;
850 batt_msg->power_supply_health = BatteryMsg::POWER_SUPPLY_HEALTH_UNKNOWN;
851 batt_msg->power_supply_technology = BatteryMsg::POWER_SUPPLY_TECHNOLOGY_UNKNOWN;
852 batt_msg->present =
true;
853 batt_msg->cell_voltage.clear();
854 batt_msg->location =
"";
855 batt_msg->serial_number =
"";
856 #else // mavros_msgs::BatteryStatus 857 batt_msg->voltage = volt;
858 batt_msg->current = curr;
859 batt_msg->remaining = rem;
865 void handle_statustext(
const mavlink::mavlink_message_t *msg, mavlink::common::msg::STATUSTEXT &textm)
868 process_statustext_normal(textm.severity, text);
870 auto st_msg = boost::make_shared<mavros_msgs::StatusText>();
872 st_msg->severity = textm.severity;
874 statustext_pub.
publish(st_msg);
877 void handle_meminfo(
const mavlink::mavlink_message_t *msg, mavlink::ardupilotmega::msg::MEMINFO &mem)
879 mem_diag.
set(std::max(static_cast<uint32_t>(mem.freemem), mem.freemem32), mem.brkval);
882 void handle_hwstatus(
const mavlink::mavlink_message_t *msg, mavlink::ardupilotmega::msg::HWSTATUS &hwst)
884 hwst_diag.
set(hwst.Vcc, hwst.I2Cerr);
890 if (m_uas->is_my_target(msg->sysid, msg->compid)) {
891 autopilot_version_timer.
stop();
892 m_uas->update_capabilities(
true, apv.capabilities);
896 if (m_uas->is_ardupilotmega())
897 process_autopilot_version_apm_quirk(apv, msg->sysid, msg->compid);
899 process_autopilot_version_normal(apv, msg->sysid, msg->compid);
902 auto it = find_or_create_vehicle_info(msg->sysid, msg->compid);
906 it->second.available_info |= mavros_msgs::VehicleInfo::HAVE_INFO_AUTOPILOT_VERSION;
907 it->second.capabilities = apv.capabilities;
908 it->second.flight_sw_version = apv.flight_sw_version;
909 it->second.middleware_sw_version = apv.middleware_sw_version;
910 it->second.os_sw_version = apv.os_sw_version;
911 it->second.board_version = apv.board_version;
912 it->second.flight_custom_version = custom_version_to_hex_string(apv.flight_custom_version);
913 it->second.vendor_id = apv.vendor_id;
914 it->second.product_id = apv.product_id;
915 it->second.uid = apv.uid;
920 #ifdef HAVE_SENSOR_MSGS_BATTERYSTATE_MSG 921 using BT = mavlink::common::MAV_BATTERY_TYPE;
922 auto batt_msg = boost::make_shared<BatteryMsg>();
925 batt_msg->current = bs.current_battery==-1?NAN:-(bs.current_battery * 0.01f);
926 batt_msg->charge = NAN;
927 batt_msg->capacity = NAN;
928 batt_msg->design_capacity = NAN;
929 batt_msg->percentage = bs.battery_remaining * 0.01f;
930 batt_msg->power_supply_status = BatteryMsg::POWER_SUPPLY_STATUS_DISCHARGING;
931 batt_msg->power_supply_health = BatteryMsg::POWER_SUPPLY_HEALTH_UNKNOWN;
948 batt_msg->power_supply_technology = BatteryMsg::POWER_SUPPLY_TECHNOLOGY_LIPO;
951 batt_msg->power_supply_technology = BatteryMsg::POWER_SUPPLY_TECHNOLOGY_LIFE;
954 batt_msg->power_supply_technology = BatteryMsg::POWER_SUPPLY_TECHNOLOGY_LION;
957 batt_msg->power_supply_technology = BatteryMsg::POWER_SUPPLY_TECHNOLOGY_NIMH;
961 batt_msg->power_supply_technology = BatteryMsg::POWER_SUPPLY_TECHNOLOGY_UNKNOWN;
966 batt_msg->present =
true;
968 batt_msg->cell_voltage.clear();
969 batt_msg->cell_voltage.reserve(bs.voltages.size() + bs.voltages_ext.size());
971 float voltage_acc = 0.0f;
972 float total_voltage = 0.0f;
973 constexpr
float coalesce_voltage = (UINT16_MAX-1) * 0.001
f;
974 for (
auto v : bs.voltages) {
978 if (v == UINT16_MAX-1)
980 voltage_acc += coalesce_voltage;
983 cell_voltage = voltage_acc + (v * 0.001f);
985 batt_msg->cell_voltage.push_back(cell_voltage);
986 total_voltage += cell_voltage;
988 for (
auto v : bs.voltages_ext) {
989 if (v == UINT16_MAX || v == 0)
992 if (v == UINT16_MAX-1)
994 voltage_acc += coalesce_voltage;
997 cell_voltage = voltage_acc + (v * 0.001f);
999 batt_msg->cell_voltage.push_back(cell_voltage);
1000 total_voltage += cell_voltage;
1002 batt_msg->voltage = total_voltage;
1005 batt_msg->serial_number =
"";
1009 has_battery_status0 =
true;
1013 batt_diag[bs.id].set(total_voltage, batt_msg->current, batt_msg->percentage);
1014 batt_diag[bs.id].setcell_v(batt_msg->cell_voltage);
1021 using ESF = mavlink::common::ESTIMATOR_STATUS_FLAGS;
1023 auto est_status_msg = boost::make_shared<mavros_msgs::EstimatorStatus>();
1046 est_status_msg->attitude_status_flag = !!(status.flags &
enum_value(ESF::ATTITUDE));
1047 est_status_msg->velocity_horiz_status_flag = !!(status.flags &
enum_value(ESF::VELOCITY_HORIZ));
1048 est_status_msg->velocity_vert_status_flag = !!(status.flags &
enum_value(ESF::VELOCITY_VERT));
1049 est_status_msg->pos_horiz_rel_status_flag = !!(status.flags &
enum_value(ESF::POS_HORIZ_REL));
1050 est_status_msg->pos_horiz_abs_status_flag = !!(status.flags &
enum_value(ESF::POS_HORIZ_ABS));
1051 est_status_msg->pos_vert_abs_status_flag = !!(status.flags &
enum_value(ESF::POS_VERT_ABS));
1052 est_status_msg->pos_vert_agl_status_flag = !!(status.flags &
enum_value(ESF::POS_VERT_AGL));
1053 est_status_msg->const_pos_mode_status_flag = !!(status.flags &
enum_value(ESF::CONST_POS_MODE));
1054 est_status_msg->pred_pos_horiz_rel_status_flag = !!(status.flags &
enum_value(ESF::PRED_POS_HORIZ_REL));
1055 est_status_msg->pred_pos_horiz_abs_status_flag = !!(status.flags &
enum_value(ESF::PRED_POS_HORIZ_ABS));
1056 est_status_msg->gps_glitch_status_flag = !!(status.flags &
enum_value(ESF::GPS_GLITCH));
1057 est_status_msg->accel_error_status_flag = !!(status.flags &
enum_value(ESF::ACCEL_ERROR));
1060 estimator_status_pub.
publish(est_status_msg);
1067 m_uas->update_connection_status(
false);
1072 using mavlink::common::MAV_MODE;
1074 mavlink::minimal::msg::HEARTBEAT hb {};
1076 hb.type =
enum_value(conn_heartbeat_mav_type);
1077 hb.autopilot =
enum_value(MAV_AUTOPILOT::INVALID);
1078 hb.base_mode =
enum_value(MAV_MODE::MANUAL_ARMED);
1080 hb.system_status =
enum_value(MAV_STATE::ACTIVE);
1082 UAS_FCU(m_uas)->send_message_ignore_drop(hb);
1087 using mavlink::common::MAV_CMD;
1092 bool do_broadcast = version_retries > RETRIES_COUNT / 2;
1095 auto client = nh.
serviceClient<mavros_msgs::CommandLong>(
"cmd/command");
1097 mavros_msgs::CommandLong
cmd{};
1099 cmd.request.broadcast = do_broadcast;
1100 cmd.request.command =
enum_value(MAV_CMD::REQUEST_AUTOPILOT_CAPABILITIES);
1101 cmd.request.confirmation =
false;
1102 cmd.request.param1 = 1.0;
1105 (do_broadcast) ?
"broadcast" :
"unicast");
1106 ret = client.call(
cmd);
1114 if (version_retries > 0) {
1117 "VER: %s request timeout, retries left %d",
1118 (do_broadcast) ?
"broadcast" :
"unicast",
1122 m_uas->update_capabilities(
false);
1123 autopilot_version_timer.
stop();
1124 ROS_WARN_NAMED(
"sys",
"VER: your FCU don't support AUTOPILOT_VERSION, " 1125 "switched to default capabilities");
1131 has_battery_status0 =
false;
1134 version_retries = RETRIES_COUNT;
1136 autopilot_version_timer.
start();
1138 autopilot_version_timer.
stop();
1141 if (connected && !disable_diag && m_uas->is_ardupilotmega()) {
1152 publish_disconnection();
1162 mavlink::common::msg::STATUSTEXT statustext {};
1163 statustext.severity = req->severity;
1167 "Status text too long: truncating...");
1170 UAS_FCU(m_uas)->send_message_ignore_drop(statustext);
1176 mavros_msgs::StreamRate::Response &res)
1178 mavlink::common::msg::REQUEST_DATA_STREAM rq = {};
1180 rq.target_system = m_uas->get_tgt_system();
1181 rq.target_component = m_uas->get_tgt_component();
1182 rq.req_stream_id = req.stream_id;
1183 rq.req_message_rate = req.message_rate;
1184 rq.start_stop = (req.on_off) ? 1 : 0;
1186 UAS_FCU(m_uas)->send_message_ignore_drop(rq);
1191 mavros_msgs::SetMode::Response &res)
1193 using mavlink::minimal::MAV_MODE_FLAG;
1195 uint8_t base_mode = req.base_mode;
1196 uint32_t custom_mode = 0;
1198 if (req.custom_mode !=
"") {
1199 if (!m_uas->cmode_from_str(req.custom_mode, custom_mode)) {
1200 res.mode_sent =
false;
1209 base_mode |= (m_uas->get_armed()) ?
enum_value(MAV_MODE_FLAG::SAFETY_ARMED) : 0;
1210 base_mode |= (m_uas->get_hil_state()) ?
enum_value(MAV_MODE_FLAG::HIL_ENABLED) : 0;
1211 base_mode |=
enum_value(MAV_MODE_FLAG::CUSTOM_MODE_ENABLED);
1214 mavlink::common::msg::SET_MODE sm = {};
1215 sm.target_system = m_uas->get_tgt_system();
1216 sm.base_mode = base_mode;
1217 sm.custom_mode = custom_mode;
1219 UAS_FCU(m_uas)->send_message_ignore_drop(sm);
1220 res.mode_sent =
true;
1225 mavros_msgs::VehicleInfoGet::Response &res)
1229 for (
const auto &got : vehicles) {
1230 res.vehicles.emplace_back(got.second);
1237 uint8_t req_sysid = req.sysid;
1238 uint8_t req_compid = req.compid;
1240 if (req.sysid == 0 && req.compid == 0) {
1242 req_sysid = m_uas->get_tgt_system();
1243 req_compid = m_uas->get_tgt_component();
1246 uint16_t key = get_vehicle_key(req_sysid, req_compid);
1247 auto it = vehicles.find(key);
1249 if (it == vehicles.end()) {
1251 res.success =
false;
1255 res.vehicles.emplace_back(it->second);
1261 mavros_msgs::MessageInterval::Response &res)
1263 using mavlink::common::MAV_CMD;
1266 auto client = nh.
serviceClient<mavros_msgs::CommandLong>(
"cmd/command");
1270 if (req.message_rate < 0) {
1271 interval_us = -1.0f;
1272 }
else if (req.message_rate == 0) {
1275 interval_us = 1000000.0f / req.message_rate;
1278 mavros_msgs::CommandLong
cmd{};
1280 cmd.request.broadcast =
false;
1281 cmd.request.command =
enum_value(MAV_CMD::SET_MESSAGE_INTERVAL);
1282 cmd.request.confirmation =
false;
1283 cmd.request.param1 = req.message_id;
1284 cmd.request.param2 = interval_us;
1286 ROS_DEBUG_NAMED(
"sys",
"SetMessageInterval: Request msgid %u at %f hz",
1287 req.message_id, req.message_rate);
1288 res.success = client.call(
cmd);
1294 ROS_ERROR_COND_NAMED(!res.success,
"sys",
"SetMessageInterval: command plugin service call failed!");
MemInfo(const std::string &name)
MAVROS Plugin base class.
ServiceClient serviceClient(const std::string &service_name, bool persistent=false, const M_string &header_values=M_string())
#define ROS_ERROR_COND_NAMED(cond, name,...)
#define ROS_WARN_COND_NAMED(cond, name,...)
MAV_TYPE conn_heartbeat_mav_type
std::shared_ptr< MAVConnInterface const > ConstPtr
std::string format(const std::string &fmt, Args ... args)
ros::Publisher statustext_pub
BatteryStatusDiag & operator=(BatteryStatusDiag &&other) noexcept
bool set_mode_cb(mavros_msgs::SetMode::Request &req, mavros_msgs::SetMode::Response &res)
#define ROS_INFO_NAMED(name,...)
void run(diagnostic_updater::DiagnosticStatusWrapper &stat)
std::atomic< uint16_t > brkval
#define ROS_DEBUG_STREAM_NAMED(name, args)
std::vector< ros::Time > times_
mavros_msgs::BatteryStatus BatteryMsg
bool vehicle_info_get_cb(mavros_msgs::VehicleInfoGet::Request &req, mavros_msgs::VehicleInfoGet::Response &res)
#define ROS_ERROR_STREAM_NAMED(name, args)
#define ROS_WARN_NAMED(name,...)
System status diagnostic updater.
const size_t window_size_
void handle_heartbeat(const mavlink::mavlink_message_t *msg, mavlink::minimal::msg::HEARTBEAT &hb)
Time & fromNSec(uint64_t t)
ros::Publisher estimator_status_pub
void summary(unsigned char lvl, const std::string s)
std::unordered_map< uint16_t, mavros_msgs::VehicleInfo > M_VehicleInfo
void statustext_cb(const mavros_msgs::StatusText::ConstPtr &req)
ros::Subscriber statustext_sub
void process_autopilot_version_apm_quirk(mavlink::common::msg::AUTOPILOT_VERSION &apv, uint8_t sysid, uint8_t compid)
void handle_autopilot_version(const mavlink::mavlink_message_t *msg, mavlink::common::msg::AUTOPILOT_VERSION &apv)
void handle_sys_status(const mavlink::mavlink_message_t *msg, mavlink::common::msg::SYS_STATUS &stat)
void tick(uint8_t type_, uint8_t autopilot_, std::string &mode_, uint8_t system_status_)
void setcell_v(const std::vector< float > voltages)
void initialize(UAS &uas_) override
Plugin initializer.
void handle_battery_status(const mavlink::mavlink_message_t *msg, mavlink::common::msg::BATTERY_STATUS &bs)
void timeout_cb(const ros::WallTimerEvent &event)
#define UAS_DIAG(uasobjptr)
helper accessor to diagnostic updater
#define ROS_INFO_STREAM_NAMED(name, args)
std::vector< BatteryStatusDiag > batt_diag
void process_autopilot_version_normal(mavlink::common::msg::AUTOPILOT_VERSION &apv, uint8_t sysid, uint8_t compid)
void addf(const std::string &key, const char *format,...)
Memory usage diag (APM-only)
void run(diagnostic_updater::DiagnosticStatusWrapper &stat)
#define MAX_NR_BATTERY_STATUS
SystemStatusDiag sys_diag
mavlink::common::msg::SYS_STATUS last_st
bool set_message_interval_cb(mavros_msgs::MessageInterval::Request &req, mavros_msgs::MessageInterval::Response &res)
bool set_rate_cb(mavros_msgs::StreamRate::Request &req, mavros_msgs::StreamRate::Response &res)
uint16_t get_vehicle_key(uint8_t sysid, uint8_t compid)
ros::WallTimer heartbeat_timer
ros::ServiceServer rate_srv
BatteryStatusDiag(BatteryStatusDiag &&other) noexcept
#define ROS_DEBUG_NAMED(name,...)
void handle_extended_sys_state(const mavlink::mavlink_message_t *msg, mavlink::common::msg::EXTENDED_SYS_STATE &state)
std::atomic< size_t > freemem
void publish(const boost::shared_ptr< M > &message) const
ros::ServiceServer vehicle_info_get_srv
void run(diagnostic_updater::DiagnosticStatusWrapper &stat)
void handle_hwstatus(const mavlink::mavlink_message_t *msg, mavlink::ardupilotmega::msg::HWSTATUS &hwst)
void handle_statustext(const mavlink::mavlink_message_t *msg, mavlink::common::msg::STATUSTEXT &textm)
mavlink::minimal::MAV_TYPE mav_type_from_str(const std::string &mav_type)
Retreive MAV_TYPE from name.
void handle_meminfo(const mavlink::mavlink_message_t *msg, mavlink::ardupilotmega::msg::MEMINFO &mem)
ros::WallTimer timeout_timer
#define UAS_FCU(uasobjptr)
helper accessor to FCU link interface
void set_string_z(std::array< char, _N > &a, const std::string &s)
Subscriptions get_subscriptions() override
Return vector of MAVLink message subscriptions (handlers)
HeartbeatStatus(const std::string &name, size_t win_size)
std::string to_string(timesync_mode e)
SystemStatusDiag(const std::string &name)
ros::ServiceServer mode_srv
void publish_disconnection()
HwStatus(const std::string &name)
Battery diagnostic updater.
void run(diagnostic_updater::DiagnosticStatusWrapper &stat)
ros::Publisher extended_state_pub
void set(uint32_t f, uint16_t b)
const std::string & getName()
DiagnosticTask(const std::string name)
BatteryStatusDiag(const std::string &name)
std::vector< HandlerInfo > Subscriptions
Subscriptions vector.
void autopilot_version_cb(const ros::WallTimerEvent &event)
#define ROS_ERROR_NAMED(name,...)
ros::ServiceServer message_interval_srv
void connection_cb(bool connected) override
M_VehicleInfo::iterator find_or_create_vehicle_info(uint8_t sysid, uint8_t compid)
std::vector< float > cell_voltage
std::atomic< uint64_t > last_rcd
ros::WallTimer autopilot_version_timer
static std::string custom_version_to_hex_string(std::array< uint8_t, 8 > &array)
void add(const std::string &key, const T &val)
#define PLUGINLIB_EXPORT_CLASS(class_type, base_class_type)
void set(uint16_t v, uint8_t e)
void run(diagnostic_updater::DiagnosticStatusWrapper &stat)
std::string to_string(const std::array< char, _N > &a)
Hardware status (APM-only)
std::vector< int > seq_nums_
void set(mavlink::common::msg::SYS_STATUS &st)
void process_statustext_normal(uint8_t severity, std::string &text)
constexpr std::underlying_type< _T >::type enum_value(_T e)
std::recursive_mutex mutex
#define ROS_WARN_STREAM_NAMED(name, args)
void heartbeat_cb(const ros::WallTimerEvent &event)
void set_min_voltage(float volt)
void handle_estimator_status(const mavlink::mavlink_message_t *msg, mavlink::common::msg::ESTIMATOR_STATUS &status)