Mirror/ardupilot
5
0
Fork 0
mirror of https://github.com/ArduPilot/ardupilot synced 2025-01-21 16:18:29 -04:00
Code Issues Packages Projects Releases Wiki Activity

AP_HAL_PX4: defer calls to safety_state switch to queue an async attempt.

Browse Source 
- this allows for auto-retries if the state does not set correctly
This commit is contained in:
Tom Pittenger 2016-05-26 23:18:05 -07:00 committed by Andrew Tridgell
parent 97e2203e70
commit 67aaf7e226
2 changed files with 38 additions and 5 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

39
libraries/AP_HAL_PX4/RCOutput.cpp
View File

@ -264,15 +264,42 @@ void PX4RCOutput::set_failsafe_pwm(uint32_t chmask, uint16_t period_us)
bool PX4RCOutput::force_safety_on(void)
{
int ret = ioctl(_pwm_fd, PWM_SERVO_SET_FORCE_SAFETY_ON, 0);
return (ret == OK);
_safety_state_request = AP_HAL::Util::SAFETY_DISARMED;
_safety_state_request_last_ms = AP_HAL::millis();
return true;
}
void PX4RCOutput::force_safety_off(void)
{
int ret = ioctl(_pwm_fd, PWM_SERVO_SET_FORCE_SAFETY_OFF, 0);
if (ret != OK) {
hal.console->printf("Failed to force safety off\n");
_safety_state_request = AP_HAL::Util::SAFETY_ARMED;
_safety_state_request_last_ms = AP_HAL::millis();
}
void PX4RCOutput::force_safety_pending_requests(void)
{
// check if there is a pending saftey_state change. If so (timer != 0) then set it.
if (_safety_state_request_last_ms != 0 &&
AP_HAL::millis() - _safety_state_request_last_ms >= 100) {
if (hal.util->safety_switch_state() == _safety_state_request) {
// current switch state matches request, stop attempting
_safety_state_request_last_ms = 0;
} else if (_safety_state_request == AP_HAL::Util::SAFETY_DISARMED) {
// current != requested, set it
ioctl(_pwm_fd, PWM_SERVO_SET_FORCE_SAFETY_ON, 0);
_safety_state_request_last_ms = AP_HAL::millis();
} else if (_safety_state_request == AP_HAL::Util::SAFETY_ARMED) {
// current != requested, set it
ioctl(_pwm_fd, PWM_SERVO_SET_FORCE_SAFETY_OFF, 0);
_safety_state_request_last_ms = AP_HAL::millis();
}
}
}
void PX4RCOutput::force_safety_no_wait(void)
{
if (_safety_state_request_last_ms != 0) {
_safety_state_request_last_ms = 1;
force_safety_pending_requests();
}
}
@ -516,6 +543,8 @@ void PX4RCOutput::_timer_tick(void)
* sent from push() */
_send_outputs();
}
force_safety_pending_requests();
}
/*

4
libraries/AP_HAL_PX4/RCOutput.h
View File

@ -24,6 +24,7 @@ public:
void set_failsafe_pwm(uint32_t chmask, uint16_t period_us) override;
bool force_safety_on(void) override;
void force_safety_off(void) override;
void force_safety_no_wait(void) override;
void set_esc_scaling(uint16_t min_pwm, uint16_t max_pwm) override {
_esc_pwm_min = min_pwm;
_esc_pwm_max = max_pwm;
@ -69,4 +70,7 @@ private:
bool _corking;
enum output_mode _output_mode = MODE_PWM_NORMAL;
void _send_outputs(void);
enum AP_HAL::Util::safety_state _safety_state_request = AP_HAL::Util::SAFETY_NONE;
uint32_t _safety_state_request_last_ms = 0;
void force_safety_pending_requests(void);
};