dark0707
/
px4-firmware
forked from Archive/PX4-Autopilot
1
0
Fork 0
Code Pull Requests Activity

FD: add dedicated topic to log more internal states 

log imbalanced propeller check metric

add failure_detector_status message
This commit is contained in:
bresch 2021-04-23 11:52:28 +02:00 committed by Daniel Agar
parent b8ed457371
commit 3f1025fb1e
5 changed files with 28 additions and 1 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

1
msg/CMakeLists.txt
View File

@ -75,6 +75,7 @@ set(msg_files
estimator_status_flags.msg
event.msg
follow_target.msg
failure_detector_status.msg
generator_status.msg
geofence_result.msg
gimbal_device_attitude_status.msg

16
msg/failure_detector_status.msg Normal file
View File

@ -0,0 +1,16 @@
uint64 timestamp # time since system start (microseconds)
# FailureDetector status
uint8 FAILURE_NONE = 0
uint8 FAILURE_ROLL = 1 # (1 << 0)
uint8 FAILURE_PITCH = 2 # (1 << 1)
uint8 FAILURE_ALT = 4 # (1 << 2)
uint8 FAILURE_EXT = 8 # (1 << 3)
uint8 FAILURE_ARM_ESC = 16 # (1 << 4)
uint8 FAILURE_HIGH_WIND = 32 # (1 << 5)
uint8 FAILURE_BATTERY = 64 # (1 << 6)
uint8 FAILURE_IMBALANCED_PROP = 128 # (1 << 7)
uint8 failure_status # Bitmask containing FailureDetector status
float32 imbalanced_prop_metric # Metric of the imbalanced propeller check (low-passed)

9
src/modules/commander/Commander.cpp
View File

@ -2776,7 +2776,7 @@ Commander::run()
_failsafe_old = _status.failsafe;
}
/* publish states (armed, control_mode, vehicle_status, commander_state, vehicle_status_flags) at 2 Hz or immediately when changed */
/* publish states (armed, control_mode, vehicle_status, commander_state, vehicle_status_flags, failure_detector_status) at 2 Hz or immediately when changed */
if (hrt_elapsed_time(&_status.timestamp) >= 500_ms || _status_changed || nav_state_changed) {
update_control_mode();
@ -2839,6 +2839,13 @@ Commander::run()
/* publish vehicle_status_flags */
_status_flags.timestamp = hrt_absolute_time();
_vehicle_status_flags_pub.publish(_status_flags);
/* publish failure_detector data */
failure_detector_status_s fd_status{};
fd_status.timestamp = hrt_absolute_time();
fd_status.failure_status = _failure_detector.getStatus();
fd_status.imbalanced_prop_metric = _failure_detector.getImbalancedPropMetric();
_failure_detector_status_pub.publish(fd_status);
}
/* play arming and battery warning tunes */

2
src/modules/commander/Commander.hpp
View File

@ -49,6 +49,7 @@
// publications
#include <uORB/Publication.hpp>
#include <uORB/topics/actuator_armed.h>
#include <uORB/topics/failure_detector_status.h>
#include <uORB/topics/home_position.h>
#include <uORB/topics/landing_gear.h>
#include <uORB/topics/test_motor.h>
@ -433,6 +434,7 @@ private:
// Publications
uORB::Publication<actuator_armed_s> _armed_pub{ORB_ID(actuator_armed)};
uORB::Publication<commander_state_s> _commander_state_pub{ORB_ID(commander_state)};
uORB::Publication<failure_detector_status_s> _failure_detector_status_pub{ORB_ID(failure_detector_status)};
uORB::Publication<test_motor_s> _test_motor_pub{ORB_ID(test_motor)};
uORB::Publication<vehicle_control_mode_s> _control_mode_pub{ORB_ID(vehicle_control_mode)};
uORB::Publication<vehicle_status_flags_s> _vehicle_status_flags_pub{ORB_ID(vehicle_status_flags)};

1
src/modules/logger/logged_topics.cpp
View File

@ -60,6 +60,7 @@ void LoggedTopics::add_default_topics()
add_topic("commander_state");
add_topic("cpuload");
add_topic("esc_status", 250);
add_topic("failure_detector_status", 100);
add_topic("follow_target", 500);
add_topic("generator_status");
add_topic("heater_status");