Browse Source

MixingOutput: remove safety button check

The assumption is that the system can only ever get into armed state if
the safety button is already off.
sbg
Beat Küng 5 years ago
parent
commit
f956bafa4e
  1. 28
      src/lib/mixer_module/mixer_module.cpp
  2. 7
      src/lib/mixer_module/mixer_module.hpp

28
src/lib/mixer_module/mixer_module.cpp

@ -33,7 +33,6 @@
#include "mixer_module.hpp" #include "mixer_module.hpp"
#include <lib/circuit_breaker/circuit_breaker.h>
#include <uORB/PublicationQueued.hpp> #include <uORB/PublicationQueued.hpp>
#include <px4_log.h> #include <px4_log.h>
@ -67,11 +66,6 @@ _control_latency_perf(perf_alloc(PC_ELAPSED, "control latency"))
_armed.force_failsafe = false; _armed.force_failsafe = false;
_armed.in_esc_calibration_mode = false; _armed.in_esc_calibration_mode = false;
// If there is no safety button, disable it on init.
#ifndef GPIO_BTN_SAFETY
_safety_off = true;
#endif
px4_sem_init(&_lock, 0, 1); px4_sem_init(&_lock, 0, 1);
// Enforce the existence of the test_motor topic, so we won't miss initial publications // Enforce the existence of the test_motor topic, so we won't miss initial publications
@ -108,12 +102,6 @@ void MixingOutput::updateParams()
{ {
ModuleParams::updateParams(); ModuleParams::updateParams();
bool safety_disabled = circuit_breaker_enabled_by_val(_param_cbrk_io_safety.get(), CBRK_IO_SAFETY_KEY);
if (safety_disabled) {
_safety_off = true;
}
// update mixer if we have one // update mixer if we have one
if (_mixers) { if (_mixers) {
if (_param_mot_slew_max.get() <= FLT_EPSILON) { if (_param_mot_slew_max.get() <= FLT_EPSILON) {
@ -296,7 +284,7 @@ bool MixingOutput::update()
/* Update the armed status and check that we're not locked down. /* Update the armed status and check that we're not locked down.
* We also need to arm throttle for the ESC calibration. */ * We also need to arm throttle for the ESC calibration. */
_throttle_armed = (_safety_off && _armed.armed && !_armed.lockdown) || (_safety_off && _armed.in_esc_calibration_mode); _throttle_armed = (_armed.armed && !_armed.lockdown) || _armed.in_esc_calibration_mode;
if (_armed.armed) { if (_armed.armed) {
_motor_test.in_test_mode = false; _motor_test.in_test_mode = false;
@ -313,7 +301,6 @@ bool MixingOutput::update()
setAndPublishActuatorOutputs(num_motor_test, actuator_outputs); setAndPublishActuatorOutputs(num_motor_test, actuator_outputs);
} }
checkSafetyButton();
handleCommands(); handleCommands();
return true; return true;
} }
@ -385,24 +372,11 @@ bool MixingOutput::update()
updateLatencyPerfCounter(actuator_outputs); updateLatencyPerfCounter(actuator_outputs);
} }
checkSafetyButton();
handleCommands(); handleCommands();
return true; return true;
} }
void
MixingOutput::checkSafetyButton()
{
if (_safety_sub.updated()) {
safety_s safety;
if (_safety_sub.copy(&safety)) {
_safety_off = !safety.safety_switch_available || safety.safety_off;
}
}
}
void void
MixingOutput::setAndPublishActuatorOutputs(unsigned num_outputs, actuator_outputs_s &actuator_outputs) MixingOutput::setAndPublishActuatorOutputs(unsigned num_outputs, actuator_outputs_s &actuator_outputs)
{ {

7
src/lib/mixer_module/mixer_module.hpp

@ -50,7 +50,6 @@
#include <uORB/topics/actuator_outputs.h> #include <uORB/topics/actuator_outputs.h>
#include <uORB/topics/multirotor_motor_limits.h> #include <uORB/topics/multirotor_motor_limits.h>
#include <uORB/topics/parameter_update.h> #include <uORB/topics/parameter_update.h>
#include <uORB/topics/safety.h>
#include <uORB/topics/test_motor.h> #include <uORB/topics/test_motor.h>
@ -193,7 +192,6 @@ private:
unsigned motorTest(); unsigned motorTest();
void updateOutputSlewrate(); void updateOutputSlewrate();
void checkSafetyButton();
void setAndPublishActuatorOutputs(unsigned num_outputs, actuator_outputs_s &actuator_outputs); void setAndPublishActuatorOutputs(unsigned num_outputs, actuator_outputs_s &actuator_outputs);
void publishMixerStatus(const actuator_outputs_s &actuator_outputs); void publishMixerStatus(const actuator_outputs_s &actuator_outputs);
void updateLatencyPerfCounter(const actuator_outputs_s &actuator_outputs); void updateLatencyPerfCounter(const actuator_outputs_s &actuator_outputs);
@ -238,7 +236,6 @@ private:
output_limit_t _output_limit; output_limit_t _output_limit;
uORB::Subscription _armed_sub{ORB_ID(actuator_armed)}; uORB::Subscription _armed_sub{ORB_ID(actuator_armed)};
uORB::Subscription _safety_sub{ORB_ID(safety)};
uORB::SubscriptionCallbackWorkItem _control_subs[actuator_controls_s::NUM_ACTUATOR_CONTROL_GROUPS]; uORB::SubscriptionCallbackWorkItem _control_subs[actuator_controls_s::NUM_ACTUATOR_CONTROL_GROUPS];
uORB::PublicationMulti<actuator_outputs_s> _outputs_pub{ORB_ID(actuator_outputs), ORB_PRIO_DEFAULT}; uORB::PublicationMulti<actuator_outputs_s> _outputs_pub{ORB_ID(actuator_outputs), ORB_PRIO_DEFAULT};
@ -250,7 +247,6 @@ private:
hrt_abstime _time_last_mix{0}; hrt_abstime _time_last_mix{0};
unsigned _max_topic_update_interval_us{0}; ///< max _control_subs topic update interval (0=unlimited) unsigned _max_topic_update_interval_us{0}; ///< max _control_subs topic update interval (0=unlimited)
bool _safety_off{false}; ///< State of the safety button from the subscribed _safety_sub topic
bool _throttle_armed{false}; bool _throttle_armed{false};
bool _ignore_lockdown{false}; ///< if true, ignore the _armed.lockdown flag (for HIL outputs) bool _ignore_lockdown{false}; ///< if true, ignore the _armed.lockdown flag (for HIL outputs)
@ -279,8 +275,7 @@ private:
(ParamInt<px4::params::MC_AIRMODE>) _param_mc_airmode, ///< multicopter air-mode (ParamInt<px4::params::MC_AIRMODE>) _param_mc_airmode, ///< multicopter air-mode
(ParamFloat<px4::params::MOT_SLEW_MAX>) _param_mot_slew_max, (ParamFloat<px4::params::MOT_SLEW_MAX>) _param_mot_slew_max,
(ParamFloat<px4::params::THR_MDL_FAC>) _param_thr_mdl_fac, ///< thrust to motor control signal modelling factor (ParamFloat<px4::params::THR_MDL_FAC>) _param_thr_mdl_fac, ///< thrust to motor control signal modelling factor
(ParamInt<px4::params::MOT_ORDERING>) _param_mot_ordering, (ParamInt<px4::params::MOT_ORDERING>) _param_mot_ordering
(ParamInt<px4::params::CBRK_IO_SAFETY>) _param_cbrk_io_safety
) )
}; };

Loading…
Cancel
Save