From 4776ae653caa8a6253d8cf609b2eebfcd54e5660 Mon Sep 17 00:00:00 2001 From: Peter Barker Date: Fri, 23 Dec 2022 10:55:03 +1100 Subject: [PATCH] AP_Parachute: use relay singleton in Parachute --- libraries/AP_Parachute/AP_Parachute.cpp | 19 ++++++++++++++----- libraries/AP_Parachute/AP_Parachute.h | 5 +---- .../AP_Parachute_test/AP_Parachute_test.cpp | 2 +- 3 files changed, 16 insertions(+), 10 deletions(-) diff --git a/libraries/AP_Parachute/AP_Parachute.cpp b/libraries/AP_Parachute/AP_Parachute.cpp index 1d6f33f434..59dc39143d 100644 --- a/libraries/AP_Parachute/AP_Parachute.cpp +++ b/libraries/AP_Parachute/AP_Parachute.cpp @@ -139,7 +139,10 @@ void AP_Parachute::update() SRV_Channels::set_output_pwm(SRV_Channel::k_parachute_release, _servo_on_pwm); } else if (_release_type <= AP_PARACHUTE_TRIGGER_TYPE_RELAY_3) { // set relay - _relay.on(_release_type); + AP_Relay*_relay = AP::relay(); + if (_relay != nullptr) { + _relay->on(_release_type); + } } _release_in_progress = true; _released = true; @@ -151,7 +154,10 @@ void AP_Parachute::update() SRV_Channels::set_output_pwm(SRV_Channel::k_parachute_release, _servo_off_pwm); } else if (_release_type <= AP_PARACHUTE_TRIGGER_TYPE_RELAY_3) { // set relay back to zero volts - _relay.off(_release_type); + AP_Relay*_relay = AP::relay(); + if (_relay != nullptr) { + _relay->off(_release_type); + } } // reset released flag and release_time _release_in_progress = false; @@ -205,9 +211,12 @@ bool AP_Parachute::arming_checks(size_t buflen, char *buffer) const hal.util->snprintf(buffer, buflen, "Chute has no channel"); return false; } - } else if (!_relay.enabled(_release_type)) { - hal.util->snprintf(buffer, buflen, "Chute invalid relay %d", int(_release_type)); - return false; + } else { + AP_Relay*_relay = AP::relay(); + if (_relay == nullptr || !_relay->enabled(_release_type)) { + hal.util->snprintf(buffer, buflen, "Chute invalid relay %d", int(_release_type)); + return false; + } } if (_release_initiated) { hal.util->snprintf(buffer, buflen, "Chute is released"); diff --git a/libraries/AP_Parachute/AP_Parachute.h b/libraries/AP_Parachute/AP_Parachute.h index 6d978c6b4e..7793a202d2 100644 --- a/libraries/AP_Parachute/AP_Parachute.h +++ b/libraries/AP_Parachute/AP_Parachute.h @@ -4,7 +4,6 @@ #include #include -#include #define AP_PARACHUTE_TRIGGER_TYPE_RELAY_0 0 #define AP_PARACHUTE_TRIGGER_TYPE_RELAY_1 1 @@ -35,8 +34,7 @@ class AP_Parachute { public: /// Constructor - AP_Parachute(AP_Relay &relay) - : _relay(relay) + AP_Parachute() { // setup parameter defaults #if CONFIG_HAL_BOARD == HAL_BOARD_SITL @@ -105,7 +103,6 @@ private: AP_Float _critical_sink; // critical sink rate to trigger emergency parachute // internal variables - AP_Relay &_relay; // pointer to relay object from the base class Relay. uint32_t _release_time; // system time that parachute is ordered to be released (actual release will happen 0.5 seconds later) bool _release_initiated:1; // true if the parachute release initiated (may still be waiting for engine to be suppressed etc.) bool _release_in_progress:1; // true if the parachute release is in progress diff --git a/libraries/AP_Parachute/examples/AP_Parachute_test/AP_Parachute_test.cpp b/libraries/AP_Parachute/examples/AP_Parachute_test/AP_Parachute_test.cpp index 6fba8bd2b2..327d41c188 100644 --- a/libraries/AP_Parachute/examples/AP_Parachute_test/AP_Parachute_test.cpp +++ b/libraries/AP_Parachute/examples/AP_Parachute_test/AP_Parachute_test.cpp @@ -23,7 +23,7 @@ static AP_Relay relay; #if HAL_PARACHUTE_ENABLED // Parachute -static AP_Parachute parachute{relay}; +static AP_Parachute parachute; #endif void setup()