fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-10-17 01:33:34 +08:00
Code Issues Packages Projects Releases Wiki Activity

commander: move ownership of vehicle_status_flags_s to HealthAndArmingChecks

Browse Source
This commit is contained in:
Beat Küng
2022-10-11 22:31:20 -04:00
committed by Daniel Agar
parent cf2eb69d25
commit 6fda555cba
8 changed files with 42 additions and 28 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/modules/commander/Arming/ArmStateMachine/ArmStateMachineTest.cpp
+1 -2
View File
@@ -242,7 +242,6 @@ TEST(ArmStateMachineTest, ArmingStateTransitionTest)
};
struct vehicle_status_s status {};
struct vehicle_status_flags_s status_flags {};
struct actuator_armed_s armed {};
size_t cArmingTransitionTests = sizeof(rgArmingTransitionTests) / sizeof(rgArmingTransitionTests[0]);
@@ -254,7 +253,7 @@ TEST(ArmStateMachineTest, ArmingStateTransitionTest)
arm_state_machine.forceArmState(test->current_state.arming_state);
status.hil_state = test->hil_state;
HealthAndArmingChecks health_and_arming_checks(nullptr, status_flags, status);
HealthAndArmingChecks health_and_arming_checks(nullptr, status);
// Attempt transition
transition_result_t result = arm_state_machine.arming_state_transition(
src/modules/commander/Commander.cpp
-4
View File
@@ -1805,10 +1805,6 @@ Commander::run()
_vehicle_status.timestamp = hrt_absolute_time();
_vehicle_status_pub.publish(_vehicle_status);
// publish vehicle_status_flags (after prearm/preflight updates above)
_vehicle_status_flags.timestamp = hrt_absolute_time();
_vehicle_status_flags_pub.publish(_vehicle_status_flags);
// failure_detector_status publish
failure_detector_status_s fd_status{};
fd_status.fd_roll = _failure_detector.getStatusFlags().roll;
src/modules/commander/Commander.hpp
+3 -4
View File
@@ -58,7 +58,6 @@
#include <uORB/topics/vehicle_command_ack.h>
#include <uORB/topics/vehicle_control_mode.h>
#include <uORB/topics/vehicle_status.h>
#include <uORB/topics/vehicle_status_flags.h>
// subscriptions
#include <uORB/Subscription.hpp>
@@ -277,7 +276,6 @@ private:
actuator_armed_s _actuator_armed{};
vehicle_control_mode_s _vehicle_control_mode{};
vehicle_status_s _vehicle_status{};
vehicle_status_flags_s _vehicle_status_flags{};
Safety _safety;
@@ -309,7 +307,6 @@ private:
uORB::Publication<failure_detector_status_s> _failure_detector_status_pub{ORB_ID(failure_detector_status)};
uORB::Publication<actuator_test_s> _actuator_test_pub{ORB_ID(actuator_test)};
uORB::Publication<vehicle_control_mode_s> _vehicle_control_mode_pub{ORB_ID(vehicle_control_mode)};
uORB::Publication<vehicle_status_flags_s> _vehicle_status_flags_pub{ORB_ID(vehicle_status_flags)};
uORB::Publication<vehicle_status_s> _vehicle_status_pub{ORB_ID(vehicle_status)};
uORB::Publication<vehicle_command_ack_s> _vehicle_command_ack_pub{ORB_ID(vehicle_command_ack)};
@@ -319,7 +316,9 @@ private:
perf_counter_t _loop_perf{perf_alloc(PC_ELAPSED, MODULE_NAME": cycle")};
perf_counter_t _preflight_check_perf{perf_alloc(PC_ELAPSED, MODULE_NAME": preflight check")};
HealthAndArmingChecks _health_and_arming_checks{this, _vehicle_status_flags, _vehicle_status};
HealthAndArmingChecks _health_and_arming_checks{this, _vehicle_status};
const vehicle_status_flags_s &_vehicle_status_flags{_health_and_arming_checks.failsafeFlags()};
HomePosition _home_position{_vehicle_status_flags};
Failsafe _failsafe_instance{this};
FailsafeBase &_failsafe{_failsafe_instance};
src/modules/commander/HealthAndArmingChecks/Common.cpp
+5 -2
View File
@@ -172,6 +172,7 @@ void Report::reset()
_results[_current_result].reset();
_next_buffer_idx = 0;
_buffer_overflowed = false;
_results_changed = false;
}
void Report::prepare(uint8_t vehicle_type)
@@ -186,16 +187,18 @@ NavModes Report::getModeGroup(uint8_t nav_state) const
return (NavModes)(1u << nav_state);
}
void Report::finalize()
bool Report::finalize()
{
_results[_current_result].arming_checks.valid = true;
_already_reported = false;
_results_changed = _results[0] != _results[1];
return _results_changed;
}
bool Report::report(bool is_armed, bool force)
{
const hrt_abstime now = hrt_absolute_time();
const bool has_difference = _had_unreported_difference || _results[0] != _results[1];
const bool has_difference = _had_unreported_difference || _results_changed;
if (now - _last_report < _min_reporting_interval && !force) {
if (has_difference) {
src/modules/commander/HealthAndArmingChecks/Common.hpp
+5 -1
View File
@@ -323,7 +323,10 @@ private:
*/
void reset();
void prepare(uint8_t vehicle_type);
void finalize();
/**
* Called after all checks are run. Returns true if the results changed
*/
bool finalize();
bool report(bool is_armed, bool force);
@@ -337,6 +340,7 @@ private:
bool _already_reported{false};
bool _had_unreported_difference{false}; ///< true if there was a difference not reported yet (due to rate limitation)
bool _results_changed{false};
hrt_abstime _last_report{0};
Results _results[2]; ///< Previous and current results to check for changes
src/modules/commander/HealthAndArmingChecks/HealthAndArmingChecks.cpp
+14 -7
View File
@@ -33,11 +33,10 @@
#include "HealthAndArmingChecks.hpp"
HealthAndArmingChecks::HealthAndArmingChecks(ModuleParams *parent, vehicle_status_flags_s &status_flags,
vehicle_status_s &status)
HealthAndArmingChecks::HealthAndArmingChecks(ModuleParams *parent, vehicle_status_s &status)
: ModuleParams(parent),
_context(status),
_reporter(status_flags)
_reporter(_failsafe_flags)
{
// Initialize mode requirements to invalid
_failsafe_flags.angular_velocity_invalid = true;
@@ -67,9 +66,10 @@ bool HealthAndArmingChecks::update(bool force_reporting)
_checks[i]->checkAndReport(_context, _reporter);
}
_reporter.finalize();
const bool results_changed = _reporter.finalize();
const bool reported = _reporter.report(_context.isArmed(), force_reporting);
if (_reporter.report(_context.isArmed(), force_reporting)) {
if (reported) {
// LEGACY start
// Run the checks again, this time with the mavlink publication set.
@@ -97,10 +97,17 @@ bool HealthAndArmingChecks::update(bool force_reporting)
_reporter.getHealthReport(health_report);
health_report.timestamp = hrt_absolute_time();
_health_report_pub.publish(health_report);
return true;
}
return false;
// Check if we need to publish the failsafe flags
const hrt_abstime now = hrt_absolute_time();
if (now - _failsafe_flags.timestamp > 500_ms || results_changed) {
_failsafe_flags.timestamp = now;
_failsafe_flags_pub.publish(_failsafe_flags);
}
return reported;
}
void HealthAndArmingChecks::updateParams()
src/modules/commander/HealthAndArmingChecks/HealthAndArmingChecks.hpp
+7 -1
View File
@@ -38,6 +38,7 @@
#include <px4_platform_common/module_params.h>
#include <uORB/Publication.hpp>
#include <uORB/topics/health_report.h>
#include <uORB/topics/vehicle_status_flags.h>
#include "checks/accelerometerCheck.hpp"
#include "checks/airspeedCheck.hpp"
@@ -71,7 +72,7 @@
class HealthAndArmingChecks : public ModuleParams
{
public:
HealthAndArmingChecks(ModuleParams *parent, vehicle_status_flags_s &status_flags, vehicle_status_s &status);
HealthAndArmingChecks(ModuleParams *parent, vehicle_status_s &status);
~HealthAndArmingChecks() = default;
/**
@@ -97,6 +98,8 @@ public:
*/
bool modePreventsArming(uint8_t nav_state) const { return _reporter.modePreventsArming(nav_state); }
const vehicle_status_flags_s &failsafeFlags() const { return _failsafe_flags; }
protected:
void updateParams() override;
private:
@@ -104,7 +107,10 @@ private:
Report _reporter;
orb_advert_t _mavlink_log_pub{nullptr};
vehicle_status_flags_s _failsafe_flags{};
uORB::Publication<health_report_s> _health_report_pub{ORB_ID(health_report)};
uORB::Publication<vehicle_status_flags_s> _failsafe_flags_pub{ORB_ID(vehicle_status_flags)};
// all checks
AccelerometerChecks _accelerometer_checks;
src/systemcmds/microbench/test_microbench_uorb.cpp
+7 -7
View File
@@ -52,7 +52,7 @@
#include <uORB/topics/sensor_gyro.h>
#include <uORB/topics/sensor_gyro_fifo.h>
#include <uORB/topics/vehicle_local_position.h>
#include <uORB/topics/vehicle_status.h>
#include <uORB/topics/vehicle_status_flags.h>
namespace MicroBenchORB
{
@@ -105,7 +105,7 @@ private:
void reset();
vehicle_status_s status;
vehicle_status_flags_s status;
vehicle_local_position_s lpos;
sensor_gyro_s gyro;
sensor_gyro_fifo_s gyro_fifo;
@@ -146,7 +146,7 @@ ut_declare_test_c(test_microbench_uorb, MicroBenchORB)
bool MicroBenchORB::time_px4_uorb()
{
int fd_status = orb_subscribe(ORB_ID(vehicle_status));
int fd_status = orb_subscribe(ORB_ID(vehicle_status_flags));
int fd_lpos = orb_subscribe(ORB_ID(vehicle_local_position));
int fd_gyro = orb_subscribe(ORB_ID(sensor_gyro));
int fd_gyro_fifo = orb_subscribe(ORB_ID(sensor_gyro_fifo));
@@ -155,7 +155,7 @@ bool MicroBenchORB::time_px4_uorb()
bool updated = false;
PERF("orb_check vehicle_status", ret = orb_check(fd_status, &updated), 100);
PERF("orb_copy vehicle_status", ret = orb_copy(ORB_ID(vehicle_status), fd_status, &status), 100);
PERF("orb_copy vehicle_status", ret = orb_copy(ORB_ID(vehicle_status_flags), fd_status, &status), 100);
printf("\n");
@@ -198,9 +198,9 @@ bool MicroBenchORB::time_px4_uorb_direct()
{
bool ret = false;
uORB::Subscription vstatus{ORB_ID(vehicle_status)};
PERF("uORB::Subscription orb_check vehicle_status", ret = vstatus.updated(), 100);
PERF("uORB::Subscription orb_copy vehicle_status", ret = vstatus.copy(&status), 100);
uORB::Subscription vehicle_status_flags{ORB_ID(vehicle_status_flags)};
PERF("uORB::Subscription orb_check vehicle_status", ret = vehicle_status_flags.updated(), 100);
PERF("uORB::Subscription orb_copy vehicle_status", ret = vehicle_status_flags.copy(&status), 100);
printf("\n");