From e30e8bcc18e6758a3175ea4df3a8f40a98c40814 Mon Sep 17 00:00:00 2001 From: Michael du Breuil Date: Tue, 26 Sep 2023 14:58:26 -0700 Subject: [PATCH 01/16] AP_Relay: Refactor to support RELAYx_FUNCTION --- libraries/AP_Relay/AP_Relay.cpp | 255 +++++++++++++++++-------- libraries/AP_Relay/AP_Relay.h | 26 +-- libraries/AP_Relay/AP_Relay_Params.cpp | 32 ++++ libraries/AP_Relay/AP_Relay_Params.h | 30 +++ 4 files changed, 255 insertions(+), 88 deletions(-) create mode 100644 libraries/AP_Relay/AP_Relay_Params.cpp create mode 100644 libraries/AP_Relay/AP_Relay_Params.h diff --git a/libraries/AP_Relay/AP_Relay.cpp b/libraries/AP_Relay/AP_Relay.cpp index 1c4cb143f7b0f..4ffe1d3562bef 100644 --- a/libraries/AP_Relay/AP_Relay.cpp +++ b/libraries/AP_Relay/AP_Relay.cpp @@ -60,54 +60,37 @@ const AP_Param::GroupInfo AP_Relay::var_info[] = { - // @Param: PIN - // @DisplayName: First Relay Pin - // @Description: Digital pin number for first relay control. This is the pin used for camera shutter control. Some common values are given, but see the Wiki's "GPIOs" page for how to determine the pin number for a given autopilot. - // @User: Standard - // @Values: -1:Disabled,49:BB Blue GP0 pin 4,50:AUXOUT1,51:AUXOUT2,52:AUXOUT3,53:AUXOUT4,54:AUXOUT5,55:AUXOUT6,57:BB Blue GP0 pin 3,113:BB Blue GP0 pin 6,116:BB Blue GP0 pin 5,27:BBBMini Pin P8.17,101:MainOut1,102:MainOut2,103:MainOut3,104:MainOut4,105:MainOut5,106:MainOut6,107:MainOut7,108:MainOut8 - AP_GROUPINFO("PIN", 0, AP_Relay, _pin[0], RELAY1_PIN_DEFAULT), - - // @Param: PIN2 - // @DisplayName: Second Relay Pin - // @Description: Digital pin number for 2nd relay control. Some common values are given, but see the Wiki's "GPIOs" page for how to determine the pin number for a given autopilot. - // @User: Standard - // @Values: -1:Disabled,49:BB Blue GP0 pin 4,50:AUXOUT1,51:AUXOUT2,52:AUXOUT3,53:AUXOUT4,54:AUXOUT5,55:AUXOUT6,57:BB Blue GP0 pin 3,113:BB Blue GP0 pin 6,116:BB Blue GP0 pin 5,65:BBBMini Pin P8.18,101:MainOut1,102:MainOut2,103:MainOut3,104:MainOut4,105:MainOut5,106:MainOut6,107:MainOut7,108:MainOut8 - AP_GROUPINFO("PIN2", 1, AP_Relay, _pin[1], RELAY2_PIN_DEFAULT), - - // @Param: PIN3 - // @DisplayName: Third Relay Pin - // @Description: Digital pin number for 3rd relay control. Some common values are given, but see the Wiki's "GPIOs" page for how to determine the pin number for a given autopilot. - // @User: Standard - // @Values: -1:Disabled,49:BB Blue GP0 pin 4,50:AUXOUT1,51:AUXOUT2,52:AUXOUT3,53:AUXOUT4,54:AUXOUT5,55:AUXOUT6,57:BB Blue GP0 pin 3,113:BB Blue GP0 pin 6,116:BB Blue GP0 pin 5,22:BBBMini Pin P8.19,101:MainOut1,102:MainOut2,103:MainOut3,104:MainOut4,105:MainOut5,106:MainOut6,107:MainOut7,108:MainOut8 - AP_GROUPINFO("PIN3", 2, AP_Relay, _pin[2], RELAY3_PIN_DEFAULT), - - // @Param: PIN4 - // @DisplayName: Fourth Relay Pin - // @Description: Digital pin number for 4th relay control. Some common values are given, but see the Wiki's "GPIOs" page for how to determine the pin number for a given autopilot. - // @User: Standard - // @Values: -1:Disabled,49:BB Blue GP0 pin 4,50:AUXOUT1,51:AUXOUT2,52:AUXOUT3,53:AUXOUT4,54:AUXOUT5,55:AUXOUT6,57:BB Blue GP0 pin 3,113:BB Blue GP0 pin 6,116:BB Blue GP0 pin 5,63:BBBMini Pin P8.34,101:MainOut1,102:MainOut2,103:MainOut3,104:MainOut4,105:MainOut5,106:MainOut6,107:MainOut7,108:MainOut8 - AP_GROUPINFO("PIN4", 3, AP_Relay, _pin[3], RELAY4_PIN_DEFAULT), - - // @Param: DEFAULT - // @DisplayName: Default relay state - // @Description: The state of the relay on boot. - // @User: Standard - // @Values: 0:Off,1:On,2:NoChange - AP_GROUPINFO("DEFAULT", 4, AP_Relay, _default, 0), - - // @Param: PIN5 - // @DisplayName: Fifth Relay Pin - // @Description: Digital pin number for 5th relay control. Some common values are given, but see the Wiki's "GPIOs" page for how to determine the pin number for a given autopilot. - // @User: Standard - // @Values: -1:Disabled,49:BB Blue GP0 pin 4,50:AUXOUT1,51:AUXOUT2,52:AUXOUT3,53:AUXOUT4,54:AUXOUT5,55:AUXOUT6,57:BB Blue GP0 pin 3,113:BB Blue GP0 pin 6,116:BB Blue GP0 pin 5,62:BBBMini Pin P8.13,101:MainOut1,102:MainOut2,103:MainOut3,104:MainOut4,105:MainOut5,106:MainOut6,107:MainOut7,108:MainOut8 - AP_GROUPINFO("PIN5", 5, AP_Relay, _pin[4], RELAY5_PIN_DEFAULT), - - // @Param: PIN6 - // @DisplayName: Sixth Relay Pin - // @Description: Digital pin number for 6th relay control. Some common values are given, but see the Wiki's "GPIOs" page for how to determine the pin number for a given autopilot. - // @User: Standard - // @Values: -1:Disabled,49:BB Blue GP0 pin 4,50:AUXOUT1,51:AUXOUT2,52:AUXOUT3,53:AUXOUT4,54:AUXOUT5,55:AUXOUT6,57:BB Blue GP0 pin 3,113:BB Blue GP0 pin 6,116:BB Blue GP0 pin 5,37:BBBMini Pin P8.14,101:MainOut1,102:MainOut2,103:MainOut3,104:MainOut4,105:MainOut5,106:MainOut6,107:MainOut7,108:MainOut8 - AP_GROUPINFO("PIN6", 6, AP_Relay, _pin[5], RELAY6_PIN_DEFAULT), + // 0 was PIN + // 1 was PIN2 + // 2 was PIN3 + // 3 was PIN4 + // 4 was DEFAULT + // 5 was PIN5 + // 6 was PIN6 + + // @Group: 1_ + // @Path: AP_Relay_Params.cpp + AP_SUBGROUPINFO(_params[0], "1_", 7, AP_Relay, AP_Relay_Params), + + // @Group: 2_ + // @Path: AP_Relay_Params.cpp + AP_SUBGROUPINFO(_params[1], "2_", 8, AP_Relay, AP_Relay_Params), + + // @Group: 3_ + // @Path: AP_Relay_Params.cpp + AP_SUBGROUPINFO(_params[2], "3_", 9, AP_Relay, AP_Relay_Params), + + // @Group: 4_ + // @Path: AP_Relay_Params.cpp + AP_SUBGROUPINFO(_params[3], "4_", 10, AP_Relay, AP_Relay_Params), + + // @Group: 5_ + // @Path: AP_Relay_Params.cpp + AP_SUBGROUPINFO(_params[4], "5_", 11, AP_Relay, AP_Relay_Params), + + // @Group: 6_ + // @Path: AP_Relay_Params.cpp + AP_SUBGROUPINFO(_params[5], "6_", 12, AP_Relay, AP_Relay_Params), AP_GROUPEND }; @@ -128,48 +111,147 @@ AP_Relay::AP_Relay(void) singleton = this; } +void AP_Relay::convert_params() +{ + // Find old default param + int8_t default_state = 0; // off was the old behaviour + const bool have_default = AP_Param::get_param_by_index(this, 4, AP_PARAM_INT8, &default_state); + + // grab the old values if they were set + for (uint8_t i = 0; i < MIN(AP_RELAY_NUM_RELAYS, 6); i++) { + + uint8_t param_index = i; + if (i > 3) { + // Skip over the old DEFAULT parameter at index 4 + param_index += 1; + } + + int8_t pin = 0; + if (!_params[i].pin.configured() && AP_Param::get_param_by_index(this, param_index, AP_PARAM_INT8, &pin) && (pin >= 0)) { + // if the pin isn't negative we can assume the user might have been using it, map it to the old school relay interface + _params[i].pin.set_and_save(pin); + _params[i].function.set_and_save(int8_t(AP_Relay_Params::Function::relay)); + + if (have_default) { + _params[i].default_state.set_and_save(default_state); + } + } + + } +} + +void AP_Relay::set_defaults() { + const int8_t pins[] = { RELAY1_PIN_DEFAULT, + RELAY2_PIN_DEFAULT, + RELAY3_PIN_DEFAULT, + RELAY4_PIN_DEFAULT, + RELAY5_PIN_DEFAULT, + RELAY6_PIN_DEFAULT }; + + for (uint8_t i = 0; i < MIN(uint8_t(AP_RELAY_NUM_RELAYS), ARRAY_SIZE(pins)); i++) { + // set the default + if (pins[i] != -1) { + _params[i].pin.set_default(pins[i]); + } + } +} void AP_Relay::init() { - if (_default != 0 && _default != 1) { + set_defaults(); + + convert_params(); + + // setup the actual default values of all the pins + for (uint8_t instance = 0; instance < AP_RELAY_NUM_RELAYS; instance++) { + const int8_t pin = _params[instance].pin; + if (pin == -1) { + // no valid pin to set it on, skip it + continue; + } + + const AP_Relay_Params::Function function = _params[instance].function; + if (function < AP_Relay_Params::Function::relay || function >= AP_Relay_Params::Function::num_functions) { + // invalid function, skip it + continue; + } + + if (function == AP_Relay_Params::Function::relay) { + // relay by instance number, set the state to match our output + const AP_Relay_Params::DefaultState default_state = _params[instance].default_state; + if ((default_state == AP_Relay_Params::DefaultState::OFF) || + (default_state == AP_Relay_Params::DefaultState::ON)) { + + set_pin_by_instance(instance, (bool)default_state); + } + } else { + // all functions are supposed to be off by default + // this will need revisiting when we support inversion + set_pin_by_instance(instance, false); + } + } +} + +void AP_Relay::set(const AP_Relay_Params::Function function, const bool value) { + if (function <= AP_Relay_Params::Function::none && function >= AP_Relay_Params::Function::num_functions) { + // invalid function return; } - for (uint8_t i=0; i= AP_RELAY_NUM_RELAYS) { + const int8_t pin = _params[instance].pin; + if (pin == -1) { + // no valid pin to set it on, skip it return; } - if (_pin[instance] == -1) { - return; - } - const uint32_t now = AP_HAL::millis(); - _pin_states = value ? _pin_states | (1U< 10)) { - AP::logger().Write("RELY", "TimeUS,State", "s-", "F-", "QB", - AP_HAL::micros64(), - _pin_states); - _last_log_ms = now; - _last_logged_pin_states = _pin_states; - } + #if AP_SIM_ENABLED if (!(AP::sitl()->on_hardware_relay_enable_mask & (1U << instance))) { return; } #endif - hal.gpio->pinMode(_pin[instance], HAL_GPIO_OUTPUT); - hal.gpio->write(_pin[instance], value); + + hal.gpio->pinMode(pin, HAL_GPIO_OUTPUT); + const bool initial_value = (bool)hal.gpio->read(pin); + + if (initial_value != value) { + hal.gpio->write(pin, value); + AP::logger().Write("RELY", "TimeUS,Instance,State", "s#-", "F--", "QBB", + AP_HAL::micros64(), + instance, + value); + } +} + +void AP_Relay::set(const uint8_t instance, const bool value) +{ + if (instance >= AP_RELAY_NUM_RELAYS) { + return; + } + + if (_params[instance].function != AP_Relay_Params::Function::relay) { + return; + } + + set_pin_by_instance(instance, value); } void AP_Relay::toggle(uint8_t instance) { - if (instance < AP_RELAY_NUM_RELAYS && _pin[instance] != -1) { - bool ison = hal.gpio->read(_pin[instance]); - set(instance, !ison); + if (instance < AP_RELAY_NUM_RELAYS) { + set(instance, !get(instance)); } } @@ -177,12 +259,10 @@ void AP_Relay::toggle(uint8_t instance) bool AP_Relay::arming_checks(size_t buflen, char *buffer) const { for (uint8_t i=0; ivalid_pin(pin)) { - char param_name_buf[11] = "RELAY_PIN"; - if (i > 0) { - hal.util->snprintf(param_name_buf, ARRAY_SIZE(param_name_buf), "RELAY_PIN%u", unsigned(i+1)); - } + char param_name_buf[14]; + hal.util->snprintf(param_name_buf, ARRAY_SIZE(param_name_buf), "RELAY%u_PIN", unsigned(i+1)); uint8_t servo_ch; if (hal.gpio->pin_to_servo_channel(pin, servo_ch)) { hal.util->snprintf(buffer, buflen, "%s=%d, set SERVO%u_FUNCTION=-1", param_name_buf, int(pin), unsigned(servo_ch+1)); @@ -195,6 +275,29 @@ bool AP_Relay::arming_checks(size_t buflen, char *buffer) const return true; } +bool AP_Relay::get(uint8_t instance) const +{ + if (instance >= AP_RELAY_NUM_RELAYS) { + // invalid instance + return false; + } + + const int8_t pin = _params[instance].pin.get(); + + if (pin < 0) { + // invalid pin + return false; + } + + return (bool)hal.gpio->read(pin); +} + +// see if the relay is enabled +bool AP_Relay::enabled(uint8_t instance) const +{ + // Must be a valid instance with function relay and pin set + return (instance < AP_RELAY_NUM_RELAYS) && (_params[instance].pin != -1) && (_params[instance].function == AP_Relay_Params::Function::relay); +} #if AP_MAVLINK_MSG_RELAY_STATUS_ENABLED // this method may only return false if there is no space in the @@ -214,7 +317,7 @@ bool AP_Relay::send_relay_status(const GCS_MAVLINK &link) const const uint16_t relay_bit_mask = 1U << i; present_mask |= relay_bit_mask; - if (_pin_states & relay_bit_mask) { + if (get(i)) { on_mask |= relay_bit_mask; } } diff --git a/libraries/AP_Relay/AP_Relay.h b/libraries/AP_Relay/AP_Relay.h index c14e964c54ea4..5a5237d18cec5 100644 --- a/libraries/AP_Relay/AP_Relay.h +++ b/libraries/AP_Relay/AP_Relay.h @@ -14,6 +14,8 @@ #if AP_RELAY_ENABLED #include +#include +#include #define AP_RELAY_NUM_RELAYS 6 @@ -29,9 +31,6 @@ class AP_Relay { // setup the relay pin void init(); - // set relay to state - void set(uint8_t instance, bool value); - // activate the relay void on(uint8_t instance) { set(instance, true); } @@ -39,12 +38,10 @@ class AP_Relay { void off(uint8_t instance) { set(instance, false); } // get state of relay - uint8_t get(uint8_t instance) const { - return instance < AP_RELAY_NUM_RELAYS ? _pin_states & (1U< +#include "AP_Relay_Params.h" + +const AP_Param::GroupInfo AP_Relay_Params::var_info[] = { + // @Param: FUNCTION + // @DisplayName: Relay function + // @Description: The function the relay channel is mapped to. + // @Values: 0:None,1:Relay,2:Ignition,3:Starter + // @User: Standard + AP_GROUPINFO_FLAGS("FUNCTION", 1, AP_Relay_Params, function, (float)Function::none, AP_PARAM_FLAG_ENABLE), + + // @Param: PIN + // @DisplayName: Relay pin + // @Description: Digital pin number for relay control. Some common values are given, but see the Wiki's "GPIOs" page for how to determine the pin number for a given autopilot. + // @Values: -1:Disabled,49:BB Blue GP0 pin 4,50:AUXOUT1,51:AUXOUT2,52:AUXOUT3,53:AUXOUT4,54:AUXOUT5,55:AUXOUT6,57:BB Blue GP0 pin 3,113:BB Blue GP0 pin 6,116:BB Blue GP0 pin 5,62:BBBMini Pin P8.13,101:MainOut1,102:MainOut2,103:MainOut3,104:MainOut4,105:MainOut5,106:MainOut6,107:MainOut7,108:MainOut8 + // @User: Standard + AP_GROUPINFO("PIN", 2, AP_Relay_Params, pin, -1), + + // @Param: DEFAULT + // @DisplayName: Relay default state + // @Description: Should the relay default to on or off, this only applies to function Relay. All other uses will pick the appropriate default output state. + // @Values: 0: Off,1:On,2:NoChange + // @User: Standard + AP_GROUPINFO("DEFAULT", 3, AP_Relay_Params, default_state, (float)DefaultState::OFF), + + AP_GROUPEND + +}; + +AP_Relay_Params::AP_Relay_Params(void) { + AP_Param::setup_object_defaults(this, var_info); +} diff --git a/libraries/AP_Relay/AP_Relay_Params.h b/libraries/AP_Relay/AP_Relay_Params.h new file mode 100644 index 0000000000000..cb1815156c0a2 --- /dev/null +++ b/libraries/AP_Relay/AP_Relay_Params.h @@ -0,0 +1,30 @@ +#pragma once + +#include +#include "AP_Relay_config.h" + +class AP_Relay_Params { +public: + static const struct AP_Param::GroupInfo var_info[]; + + AP_Relay_Params(void); + + /* Do not allow copies */ + CLASS_NO_COPY(AP_Relay_Params); + + enum class DefaultState : uint8_t { + OFF = 0, + ON = 1, + NO_CHANGE = 2, + }; + + enum class Function : uint8_t { + none = 0, + relay = 1, + num_functions // must be the last entry + }; + + AP_Enum function; // relay function + AP_Int16 pin; // gpio pin number + AP_Enum default_state; // default state +}; From 09a26efa0f5fdcf195f6e7c1d889de74824e29fd Mon Sep 17 00:00:00 2001 From: Michael du Breuil Date: Tue, 3 Oct 2023 17:17:23 -0700 Subject: [PATCH 02/16] Copter: Fix AP_Relay param naming --- ArduCopter/Parameters.cpp | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/ArduCopter/Parameters.cpp b/ArduCopter/Parameters.cpp index 80a79da030a7c..f56ef3b020555 100644 --- a/ArduCopter/Parameters.cpp +++ b/ArduCopter/Parameters.cpp @@ -451,9 +451,9 @@ const AP_Param::Info Copter::var_info[] = { #endif #if AP_RELAY_ENABLED - // @Group: RELAY_ + // @Group: RELAY // @Path: ../libraries/AP_Relay/AP_Relay.cpp - GOBJECT(relay, "RELAY_", AP_Relay), + GOBJECT(relay, "RELAY", AP_Relay), #endif #if PARACHUTE == ENABLED From 490068d9c9f57588641f83169779378475207ce4 Mon Sep 17 00:00:00 2001 From: Michael du Breuil Date: Tue, 3 Oct 2023 17:17:44 -0700 Subject: [PATCH 03/16] Plane: Fix AP_Relay param naming --- ArduPlane/Parameters.cpp | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/ArduPlane/Parameters.cpp b/ArduPlane/Parameters.cpp index fbecd0ee40b6c..bb4a2a86a5fba 100644 --- a/ArduPlane/Parameters.cpp +++ b/ArduPlane/Parameters.cpp @@ -757,9 +757,9 @@ const AP_Param::Info Plane::var_info[] = { GOBJECT(arming, "ARMING_", AP_Arming_Plane), #if AP_RELAY_ENABLED - // @Group: RELAY_ + // @Group: RELAY // @Path: ../libraries/AP_Relay/AP_Relay.cpp - GOBJECT(relay, "RELAY_", AP_Relay), + GOBJECT(relay, "RELAY", AP_Relay), #endif #if PARACHUTE == ENABLED From 8dbaede20f137f9c3818a756a1ae337b943f7944 Mon Sep 17 00:00:00 2001 From: Michael du Breuil Date: Tue, 3 Oct 2023 17:17:55 -0700 Subject: [PATCH 04/16] Sub: Fix AP_Relay param naming --- ArduSub/Parameters.cpp | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/ArduSub/Parameters.cpp b/ArduSub/Parameters.cpp index d43b149820f74..5711f4fe31dc8 100644 --- a/ArduSub/Parameters.cpp +++ b/ArduSub/Parameters.cpp @@ -476,9 +476,9 @@ const AP_Param::Info Sub::var_info[] = { #endif #if AP_RELAY_ENABLED - // @Group: RELAY_ + // @Group: RELAY // @Path: ../libraries/AP_Relay/AP_Relay.cpp - GOBJECT(relay, "RELAY_", AP_Relay), + GOBJECT(relay, "RELAY", AP_Relay), #endif // @Group: COMPASS_ From 09a8c2c9534a644116f8a1610535a4d04637b93b Mon Sep 17 00:00:00 2001 From: Michael du Breuil Date: Tue, 3 Oct 2023 17:18:04 -0700 Subject: [PATCH 05/16] Rover: Fix AP_Relay param naming --- Rover/Parameters.cpp | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/Rover/Parameters.cpp b/Rover/Parameters.cpp index 5d4a84fc9b08b..ac1bdb29abdc1 100644 --- a/Rover/Parameters.cpp +++ b/Rover/Parameters.cpp @@ -223,9 +223,9 @@ const AP_Param::Info Rover::var_info[] = { GOBJECT(barometer, "BARO", AP_Baro), #if AP_RELAY_ENABLED - // @Group: RELAY_ + // @Group: RELAY // @Path: ../libraries/AP_Relay/AP_Relay.cpp - GOBJECT(relay, "RELAY_", AP_Relay), + GOBJECT(relay, "RELAY", AP_Relay), #endif // @Group: RCMAP_ From 159b8ed33b57751f7e86db5b1016b1c7827306a5 Mon Sep 17 00:00:00 2001 From: Michael du Breuil Date: Tue, 7 Nov 2023 09:42:15 -0700 Subject: [PATCH 06/16] autotest: Update tests for relay --- Tools/autotest/arduplane.py | 7 +++++-- Tools/autotest/default_params/rover.parm | 4 ++-- .../default_params/vee-gull 005.param | 5 +---- Tools/autotest/rover.py | 20 ++++++++++++++++--- 4 files changed, 25 insertions(+), 11 deletions(-) diff --git a/Tools/autotest/arduplane.py b/Tools/autotest/arduplane.py index 09041b17dab92..3ffea1aaeb2a1 100644 --- a/Tools/autotest/arduplane.py +++ b/Tools/autotest/arduplane.py @@ -1124,9 +1124,12 @@ def TestFlaps(self): def TestRCRelay(self): '''Test Relay RC Channel Option''' - self.set_parameter("RC12_OPTION", 28) # Relay On/Off + self.set_parameters({ + "RELAY1_FUNCTION": 1, # Enable relay as a standard relay pin + "RC12_OPTION": 28 # Relay On/Off + }) self.set_rc(12, 1000) - self.reboot_sitl() # needed for RC12_OPTION to take effect + self.reboot_sitl() # needed for RC12_OPTION and RELAY1_FUNCTION to take effect off = self.get_parameter("SIM_PIN_MASK") if off: diff --git a/Tools/autotest/default_params/rover.parm b/Tools/autotest/default_params/rover.parm index 9095f40720d5d..58e064d81b3f5 100644 --- a/Tools/autotest/default_params/rover.parm +++ b/Tools/autotest/default_params/rover.parm @@ -24,8 +24,8 @@ RC1_MAX 2000 RC1_MIN 1000 RC3_MAX 2000 RC3_MIN 1000 -RELAY_PIN 1 -RELAY_PIN2 2 +RELAY1_PIN 1 +RELAY2_PIN 2 SERVO1_MIN 1000 SERVO1_MAX 2000 SERVO3_MAX 2000 diff --git a/Tools/autotest/default_params/vee-gull 005.param b/Tools/autotest/default_params/vee-gull 005.param index 2ae334ae8b7fc..6e5143632d452 100644 --- a/Tools/autotest/default_params/vee-gull 005.param +++ b/Tools/autotest/default_params/vee-gull 005.param @@ -456,10 +456,7 @@ RCMAP_ROLL,1 RCMAP_THROTTLE,3 RCMAP_YAW,4 RELAY_DEFAULT,0 -RELAY_PIN,13 -RELAY_PIN2,-1 -RELAY_PIN3,-1 -RELAY_PIN4,-1 +RELAY1_PIN,13 RLL_RATE_D,0.000000 RLL_RATE_FF,0.255000 RLL_RATE_I,0.050000 diff --git a/Tools/autotest/rover.py b/Tools/autotest/rover.py index f1eeb76c61801..b4042020fee8c 100644 --- a/Tools/autotest/rover.py +++ b/Tools/autotest/rover.py @@ -568,6 +568,13 @@ def ServoRelayEvents(self): '''Test ServoRelayEvents''' for method in self.run_cmd, self.run_cmd_int: self.context_push() + + self.set_parameters({ + "RELAY1_FUNCTION": 1, # Enable relay 1 as a standard relay pin + "RELAY2_FUNCTION": 1, # Enable relay 2 as a standard relay pin + }) + self.reboot_sitl() # Needed for relay functions to take effect + method(mavutil.mavlink.MAV_CMD_DO_SET_RELAY, p1=0, p2=0) off = self.get_parameter("SIM_PIN_MASK") method(mavutil.mavlink.MAV_CMD_DO_SET_RELAY, p1=0, p2=1) @@ -602,8 +609,14 @@ def ServoRelayEvents(self): "on": 0, }) - # add another servo: - self.set_parameter("RELAY_PIN6", 14) + # add another relay and ensure that it changes the "present field" + self.set_parameters({ + "RELAY6_FUNCTION": 1, # Enable relay 6 as a standard relay pin + "RELAY6_PIN": 14, # Set pin number + }) + self.reboot_sitl() # Needed for relay function to take effect + self.set_message_rate_hz("RELAY_STATUS", 10) # Need to re-request the message since reboot + self.assert_received_message_field_values('RELAY_STATUS', { "present": 35, "on": 0, @@ -5352,7 +5365,8 @@ def test_scripting_auxfunc(self): self.context_collect("STATUSTEXT") self.set_parameters({ "SCR_ENABLE": 1, - "RELAY_PIN": 1, + "RELAY1_FUNCTION": 1, + "RELAY1_PIN": 1 }) self.install_example_script_context("RCIN_test.lua") self.reboot_sitl() From 849d964e03aaba1dbcf95bb8cea7b28f3a14f1f6 Mon Sep 17 00:00:00 2001 From: Iampete1 Date: Fri, 8 Dec 2023 04:27:06 +0000 Subject: [PATCH 07/16] AP_LandingGear: remove unneeded relay include --- libraries/AP_LandingGear/AP_LandingGear.cpp | 1 - 1 file changed, 1 deletion(-) diff --git a/libraries/AP_LandingGear/AP_LandingGear.cpp b/libraries/AP_LandingGear/AP_LandingGear.cpp index b7a7bc3ebb45b..8f77d5e3df750 100644 --- a/libraries/AP_LandingGear/AP_LandingGear.cpp +++ b/libraries/AP_LandingGear/AP_LandingGear.cpp @@ -2,7 +2,6 @@ #if AP_LANDINGGEAR_ENABLED -#include #include #include #include From 3873f1b5ce3359ac10519d45d481f7cdc254eaf6 Mon Sep 17 00:00:00 2001 From: Iampete1 Date: Fri, 8 Dec 2023 04:29:31 +0000 Subject: [PATCH 08/16] AP_Camera: move to new relay functions --- libraries/AP_Camera/AP_Camera.cpp | 20 ++++++++++++++++++++ libraries/AP_Camera/AP_Camera.h | 3 +++ libraries/AP_Camera/AP_Camera_Relay.cpp | 12 ++---------- 3 files changed, 25 insertions(+), 10 deletions(-) diff --git a/libraries/AP_Camera/AP_Camera.cpp b/libraries/AP_Camera/AP_Camera.cpp index a44243c8cacca..76f3524e22e2b 100644 --- a/libraries/AP_Camera/AP_Camera.cpp +++ b/libraries/AP_Camera/AP_Camera.cpp @@ -771,6 +771,26 @@ void AP_Camera::convert_params() } } +#if AP_RELAY_ENABLED +// Return true and the relay index if relay camera backend is selected, used for conversion to relay functions +bool AP_Camera::get_legacy_relay_index(int8_t &index) const +{ + // PARAMETER_CONVERSION - Added: Dec-2023 + + // Note that this assumes that the camera param conversion has already been done + // Copter, Plane, Sub and Rover all have both relay and camera and all init relay first + // This will only be a issue if the relay and camera conversion were done at once, if the user skipped 4.4 + for (uint8_t i = 0; i < AP_CAMERA_MAX_INSTANCES; i++) { + if ((CameraType)_params[i].type.get() == CameraType::RELAY) { + // Camera was hard coded to relay 0 + index = 0; + return true; + } + } + return false; +} +#endif + // singleton instance AP_Camera *AP_Camera::_singleton; diff --git a/libraries/AP_Camera/AP_Camera.h b/libraries/AP_Camera/AP_Camera.h index 356901ab26281..71ef3ffda4614 100644 --- a/libraries/AP_Camera/AP_Camera.h +++ b/libraries/AP_Camera/AP_Camera.h @@ -182,6 +182,9 @@ class AP_Camera { bool get_state(uint8_t instance, camera_state_t& cam_state); #endif + // Return true and the relay index if relay camera backend is selected, used for conversion to relay functions + bool get_legacy_relay_index(int8_t &index) const; + // allow threads to lock against AHRS update HAL_Semaphore &get_semaphore() { return _rsem; } diff --git a/libraries/AP_Camera/AP_Camera_Relay.cpp b/libraries/AP_Camera/AP_Camera_Relay.cpp index a48adeb348d51..db8fc2adec481 100644 --- a/libraries/AP_Camera/AP_Camera_Relay.cpp +++ b/libraries/AP_Camera/AP_Camera_Relay.cpp @@ -14,11 +14,7 @@ void AP_Camera_Relay::update() if (ap_relay == nullptr) { return; } - if (_params.relay_on) { - ap_relay->off(0); - } else { - ap_relay->on(0); - } + ap_relay->set(AP_Relay_Params::FUNCTION::CAMERA, !_params.relay_on); } // call parent update @@ -39,11 +35,7 @@ bool AP_Camera_Relay::trigger_pic() return false; } - if (_params.relay_on) { - ap_relay->on(0); - } else { - ap_relay->off(0); - } + ap_relay->set(AP_Relay_Params::FUNCTION::CAMERA, _params.relay_on); // set counter to move servo to off position after this many iterations of update (assumes 50hz update rate) trigger_counter = constrain_float(_params.trigger_duration * 50, 0, UINT16_MAX); From a22446880dfa52301534f5277801ae6a32557e30 Mon Sep 17 00:00:00 2001 From: Iampete1 Date: Fri, 8 Dec 2023 04:30:34 +0000 Subject: [PATCH 09/16] AP_ICEngine: move to new relay functions --- libraries/AP_ICEngine/AP_ICEngine.cpp | 34 ++++++++++++++++----------- libraries/AP_ICEngine/AP_ICEngine.h | 10 ++++---- 2 files changed, 25 insertions(+), 19 deletions(-) diff --git a/libraries/AP_ICEngine/AP_ICEngine.cpp b/libraries/AP_ICEngine/AP_ICEngine.cpp index d7afd09e92fd8..ad66e49ded7d0 100644 --- a/libraries/AP_ICEngine/AP_ICEngine.cpp +++ b/libraries/AP_ICEngine/AP_ICEngine.cpp @@ -165,14 +165,8 @@ const AP_Param::GroupInfo AP_ICEngine::var_info[] = { AP_GROUPINFO("REDLINE_RPM", 17, AP_ICEngine, redline_rpm, 0), #endif -#if AP_RELAY_ENABLED - // @Param: IGNITION_RLY - // @DisplayName: Ignition relay channel - // @Description: This is a a relay channel to use for ignition control - // @User: Standard - // @Values: 0:None,1:Relay1,2:Relay2,3:Relay3,4:Relay4,5:Relay5,6:Relay6 - AP_GROUPINFO("IGNITION_RLY", 18, AP_ICEngine, ignition_relay, 0), -#endif + // 18 was IGNITION_RLY + AP_GROUPEND }; @@ -608,15 +602,14 @@ void AP_ICEngine::update_idle_governor(int8_t &min_throttle) void AP_ICEngine::set_ignition(bool on) { SRV_Channels::set_output_pwm(SRV_Channel::k_ignition, on? pwm_ignition_on : pwm_ignition_off); + #if AP_RELAY_ENABLED - // optionally use a relay as well - if (ignition_relay > 0) { - auto *relay = AP::relay(); - if (relay != nullptr) { - relay->set(ignition_relay-1, on); - } + AP_Relay *relay = AP::relay(); + if (relay != nullptr) { + relay->set(AP_Relay_Params::FUNCTION::IGNITION, on); } #endif // AP_RELAY_ENABLED + } /* @@ -638,6 +631,19 @@ bool AP_ICEngine::allow_throttle_while_disarmed() const hal.util->safety_switch_state() != AP_HAL::Util::SAFETY_DISARMED; } +#if AP_RELAY_ENABLED +bool AP_ICEngine::get_legacy_ignition_relay_index(int8_t &num) +{ + // PARAMETER_CONVERSION - Added: Dec-2023 + if (!enable || !AP_Param::get_param_by_index(this, 18, AP_PARAM_INT8, &num)) { + return false; + } + // convert to zero indexed + num -= 1; + return true; +} +#endif + // singleton instance. Should only ever be set in the constructor. AP_ICEngine *AP_ICEngine::_singleton; namespace AP { diff --git a/libraries/AP_ICEngine/AP_ICEngine.h b/libraries/AP_ICEngine/AP_ICEngine.h index 919e763b28201..351302e7638f0 100644 --- a/libraries/AP_ICEngine/AP_ICEngine.h +++ b/libraries/AP_ICEngine/AP_ICEngine.h @@ -66,6 +66,11 @@ class AP_ICEngine { // do we have throttle while disarmed enabled? bool allow_throttle_while_disarmed(void) const; +#if AP_RELAY_ENABLED + // Needed for param conversion from relay numbers to functions + bool get_legacy_ignition_relay_index(int8_t &num); +#endif + static AP_ICEngine *get_singleton() { return _singleton; } private: @@ -136,11 +141,6 @@ class AP_ICEngine { AP_Float idle_slew; #endif -#if AP_RELAY_ENABLED - // relay number for ignition - AP_Int8 ignition_relay; -#endif - // height when we enter ICE_START_HEIGHT_DELAY float initial_height; From 8cba2f968322c9df2b9f121d4aea5cc8505da2e7 Mon Sep 17 00:00:00 2001 From: Iampete1 Date: Fri, 8 Dec 2023 04:36:03 +0000 Subject: [PATCH 10/16] AP_Relay: add enabled method by function --- libraries/AP_Relay/AP_Relay.cpp | 12 ++++++++++++ libraries/AP_Relay/AP_Relay.h | 3 +++ 2 files changed, 15 insertions(+) diff --git a/libraries/AP_Relay/AP_Relay.cpp b/libraries/AP_Relay/AP_Relay.cpp index 4ffe1d3562bef..e5cdde29c0b23 100644 --- a/libraries/AP_Relay/AP_Relay.cpp +++ b/libraries/AP_Relay/AP_Relay.cpp @@ -299,6 +299,18 @@ bool AP_Relay::enabled(uint8_t instance) const return (instance < AP_RELAY_NUM_RELAYS) && (_params[instance].pin != -1) && (_params[instance].function == AP_Relay_Params::Function::relay); } +// see if the relay is enabled +bool AP_Relay::enabled(AP_Relay_Params::Function function) const +{ + bool valid = false; + for (uint8_t instance = 0; instance < AP_RELAY_NUM_RELAYS; instance++) { + if ((_params[instance].function == function) && (_params[instance].pin != -1)) { + valid = true; + } + } + return valid; +} + #if AP_MAVLINK_MSG_RELAY_STATUS_ENABLED // this method may only return false if there is no space in the // supplied link for the message. diff --git a/libraries/AP_Relay/AP_Relay.h b/libraries/AP_Relay/AP_Relay.h index 5a5237d18cec5..a668e9144c71a 100644 --- a/libraries/AP_Relay/AP_Relay.h +++ b/libraries/AP_Relay/AP_Relay.h @@ -57,6 +57,9 @@ class AP_Relay { void set(AP_Relay_Params::Function function, bool value); + // see if the relay is enabled + bool enabled(AP_Relay_Params::Function function) const; + private: static AP_Relay *singleton; From d8be26d18477d7e858295cc567e9855e9ffcbaa4 Mon Sep 17 00:00:00 2001 From: Iampete1 Date: Fri, 8 Dec 2023 04:36:47 +0000 Subject: [PATCH 11/16] AP_Parachute: move to new relay functions --- libraries/AP_Parachute/AP_Parachute.cpp | 21 +++++++++++++++++---- libraries/AP_Parachute/AP_Parachute.h | 5 ++++- 2 files changed, 21 insertions(+), 5 deletions(-) diff --git a/libraries/AP_Parachute/AP_Parachute.cpp b/libraries/AP_Parachute/AP_Parachute.cpp index 9b4075dc71964..6ed1618b1904a 100644 --- a/libraries/AP_Parachute/AP_Parachute.cpp +++ b/libraries/AP_Parachute/AP_Parachute.cpp @@ -142,7 +142,7 @@ void AP_Parachute::update() // set relay AP_Relay*_relay = AP::relay(); if (_relay != nullptr) { - _relay->on(_release_type); + _relay->set(AP_Relay_Params::FUNCTION::PARACHUTE, true); } #endif } @@ -159,7 +159,7 @@ void AP_Parachute::update() // set relay back to zero volts AP_Relay*_relay = AP::relay(); if (_relay != nullptr) { - _relay->off(_release_type); + _relay->set(AP_Relay_Params::FUNCTION::PARACHUTE, false); } #endif } @@ -218,8 +218,8 @@ bool AP_Parachute::arming_checks(size_t buflen, char *buffer) const } else { #if AP_RELAY_ENABLED AP_Relay*_relay = AP::relay(); - if (_relay == nullptr || !_relay->enabled(_release_type)) { - hal.util->snprintf(buffer, buflen, "Chute invalid relay %d", int(_release_type)); + if (_relay == nullptr || !_relay->enabled(AP_Relay_Params::FUNCTION::PARACHUTE)) { + hal.util->snprintf(buffer, buflen, "Chute has no relay"); return false; } #else @@ -235,6 +235,19 @@ bool AP_Parachute::arming_checks(size_t buflen, char *buffer) const return true; } +#if AP_RELAY_ENABLED +// Return the relay index that would be used for param conversion to relay functions +bool AP_Parachute::get_legacy_relay_index(int8_t &index) const +{ + // PARAMETER_CONVERSION - Added: Dec-2023 + if (!enabled() || (_release_type > AP_PARACHUTE_TRIGGER_TYPE_RELAY_3)) { + return false; + } + index = _release_type; + return true; +} +#endif + // singleton instance AP_Parachute *AP_Parachute::_singleton; diff --git a/libraries/AP_Parachute/AP_Parachute.h b/libraries/AP_Parachute/AP_Parachute.h index 7793a202d23bb..30005e7543067 100644 --- a/libraries/AP_Parachute/AP_Parachute.h +++ b/libraries/AP_Parachute/AP_Parachute.h @@ -85,7 +85,10 @@ class AP_Parachute { // check settings are valid bool arming_checks(size_t buflen, char *buffer) const; - + + // Return the relay index that would be used for param conversion to relay functions + bool get_legacy_relay_index(int8_t &index) const; + static const struct AP_Param::GroupInfo var_info[]; // get singleton instance From 7b9c702893eea845aad514c23f391b978332249a Mon Sep 17 00:00:00 2001 From: Iampete1 Date: Fri, 8 Dec 2023 04:37:15 +0000 Subject: [PATCH 12/16] AP_Relay: param conversion from ICE, chute and camera --- libraries/AP_Relay/AP_Relay.cpp | 69 +++++++++++++++++++++++--- libraries/AP_Relay/AP_Relay_Params.cpp | 4 +- libraries/AP_Relay/AP_Relay_Params.h | 3 ++ 3 files changed, 67 insertions(+), 9 deletions(-) diff --git a/libraries/AP_Relay/AP_Relay.cpp b/libraries/AP_Relay/AP_Relay.cpp index e5cdde29c0b23..02b9816a089ae 100644 --- a/libraries/AP_Relay/AP_Relay.cpp +++ b/libraries/AP_Relay/AP_Relay.cpp @@ -15,6 +15,10 @@ #include #include +#include +#include +#include + #if CONFIG_HAL_BOARD == HAL_BOARD_SITL #define RELAY1_PIN_DEFAULT 13 @@ -113,12 +117,45 @@ AP_Relay::AP_Relay(void) void AP_Relay::convert_params() { + // PARAMETER_CONVERSION - Added: Dec-2023 + + // Before converting local params we must find any relays being used by index from external libs + int8_t relay_index; + + int8_t ice_relay = -1; +#if AP_ICENGINE_ENABLED + AP_ICEngine *ice = AP::ice(); + if (ice != nullptr && ice->get_legacy_ignition_relay_index(relay_index)) { + ice_relay = relay_index; + } +#endif + + int8_t chute_relay = -1; +#if HAL_PARACHUTE_ENABLED + AP_Parachute *parachute = AP::parachute(); + if (parachute != nullptr && parachute->get_legacy_relay_index(relay_index)) { + chute_relay = relay_index; + } +#endif + + int8_t cam_relay = -1; +#if AP_CAMERA_ENABLED + AP_Camera *camera = AP::camera(); + if ((camera != nullptr) && (camera->get_legacy_relay_index(relay_index))) { + cam_relay = relay_index; + } +#endif + // Find old default param int8_t default_state = 0; // off was the old behaviour const bool have_default = AP_Param::get_param_by_index(this, 4, AP_PARAM_INT8, &default_state); // grab the old values if they were set for (uint8_t i = 0; i < MIN(AP_RELAY_NUM_RELAYS, 6); i++) { + if (_params[i].function.configured()) { + // Conversion already done, or user has configured manually + continue; + } uint8_t param_index = i; if (i > 3) { @@ -128,15 +165,34 @@ void AP_Relay::convert_params() int8_t pin = 0; if (!_params[i].pin.configured() && AP_Param::get_param_by_index(this, param_index, AP_PARAM_INT8, &pin) && (pin >= 0)) { - // if the pin isn't negative we can assume the user might have been using it, map it to the old school relay interface + // Copy old pin parameter _params[i].pin.set_and_save(pin); + } + if (_params[i].pin < 0) { + // Don't enable if pin is invalid + continue; + } + // Valid pin, this is either due to the param conversion or default value + + // Work out what function this relay should be + if (i == chute_relay) { + _params[i].function.set_and_save(int8_t(AP_Relay_Params::Function::parachute)); + + } else if (i == ice_relay) { + _params[i].function.set_and_save(int8_t(AP_Relay_Params::Function::ignition)); + + } else if (i == cam_relay) { + _params[i].function.set_and_save(int8_t(AP_Relay_Params::Function::camera)); + + } else { _params[i].function.set_and_save(int8_t(AP_Relay_Params::Function::relay)); - if (have_default) { - _params[i].default_state.set_and_save(default_state); - } } + // Set the default state + if (have_default) { + _params[i].default_state.set_and_save(default_state); + } } } @@ -302,13 +358,12 @@ bool AP_Relay::enabled(uint8_t instance) const // see if the relay is enabled bool AP_Relay::enabled(AP_Relay_Params::Function function) const { - bool valid = false; for (uint8_t instance = 0; instance < AP_RELAY_NUM_RELAYS; instance++) { if ((_params[instance].function == function) && (_params[instance].pin != -1)) { - valid = true; + return true; } } - return valid; + return false; } #if AP_MAVLINK_MSG_RELAY_STATUS_ENABLED diff --git a/libraries/AP_Relay/AP_Relay_Params.cpp b/libraries/AP_Relay/AP_Relay_Params.cpp index 01eb6374451e3..9ea593cea6f65 100644 --- a/libraries/AP_Relay/AP_Relay_Params.cpp +++ b/libraries/AP_Relay/AP_Relay_Params.cpp @@ -5,7 +5,7 @@ const AP_Param::GroupInfo AP_Relay_Params::var_info[] = { // @Param: FUNCTION // @DisplayName: Relay function // @Description: The function the relay channel is mapped to. - // @Values: 0:None,1:Relay,2:Ignition,3:Starter + // @Values: 0:None,1:Relay,2:Ignition,3:Parachute,4:Camera // @User: Standard AP_GROUPINFO_FLAGS("FUNCTION", 1, AP_Relay_Params, function, (float)Function::none, AP_PARAM_FLAG_ENABLE), @@ -15,7 +15,7 @@ const AP_Param::GroupInfo AP_Relay_Params::var_info[] = { // @Values: -1:Disabled,49:BB Blue GP0 pin 4,50:AUXOUT1,51:AUXOUT2,52:AUXOUT3,53:AUXOUT4,54:AUXOUT5,55:AUXOUT6,57:BB Blue GP0 pin 3,113:BB Blue GP0 pin 6,116:BB Blue GP0 pin 5,62:BBBMini Pin P8.13,101:MainOut1,102:MainOut2,103:MainOut3,104:MainOut4,105:MainOut5,106:MainOut6,107:MainOut7,108:MainOut8 // @User: Standard AP_GROUPINFO("PIN", 2, AP_Relay_Params, pin, -1), - + // @Param: DEFAULT // @DisplayName: Relay default state // @Description: Should the relay default to on or off, this only applies to function Relay. All other uses will pick the appropriate default output state. diff --git a/libraries/AP_Relay/AP_Relay_Params.h b/libraries/AP_Relay/AP_Relay_Params.h index cb1815156c0a2..6e9105dbc7ddb 100644 --- a/libraries/AP_Relay/AP_Relay_Params.h +++ b/libraries/AP_Relay/AP_Relay_Params.h @@ -21,6 +21,9 @@ class AP_Relay_Params { enum class Function : uint8_t { none = 0, relay = 1, + ignition = 2, + parachute = 3, + camera = 4, num_functions // must be the last entry }; From c8720c5787b03185135cbab4ea2b6b397f4695b8 Mon Sep 17 00:00:00 2001 From: Iampete1 Date: Sat, 9 Dec 2023 19:11:44 +0000 Subject: [PATCH 13/16] AR_Motors: Move to new relay functions --- libraries/AR_Motors/AP_MotorsUGV.cpp | 95 +++++++++++++++++++++++----- libraries/AR_Motors/AP_MotorsUGV.h | 3 + 2 files changed, 81 insertions(+), 17 deletions(-) diff --git a/libraries/AR_Motors/AP_MotorsUGV.cpp b/libraries/AR_Motors/AP_MotorsUGV.cpp index c1b2177a46b9e..061c1b3a25bd9 100644 --- a/libraries/AR_Motors/AP_MotorsUGV.cpp +++ b/libraries/AR_Motors/AP_MotorsUGV.cpp @@ -16,7 +16,7 @@ #include #include #include "AP_MotorsUGV.h" -#include +#include #define SERVO_MAX 4500 // This value represents 45 degrees and is just an arbitrary representation of servo max travel. @@ -147,6 +147,33 @@ void AP_MotorsUGV::init(uint8_t frtype) } } +bool AP_MotorsUGV::get_legacy_relay_index(int8_t &index1, int8_t &index2, int8_t &index3, int8_t &index4) const +{ + if (_pwm_type != PWM_TYPE_BRUSHED_WITH_RELAY) { + // Relays only used if PWM type is set to brushed with relay + return false; + } + + // First relay is always used, throttle, throttle left or motor 1 + index1 = 0; + + // Second relay is used for right throttle on skid steer and motor 2 for omni + if (have_skid_steering()) { + index2 = 1; + } + + // Omni can have a variable number of motors + if (is_omni()) { + // Omni has at least 3 motors + index2 = 2; + if (_motors_num >= 4) { + index2 = 3; + } + } + + return true; +} + // setup output in case of main CPU failure void AP_MotorsUGV::setup_safety_output() { @@ -457,10 +484,14 @@ bool AP_MotorsUGV::output_test_pwm(motor_test_order motor_seq, float pwm) // returns true if checks pass, false if they fail. report should be true to send text messages to GCS bool AP_MotorsUGV::pre_arm_check(bool report) const { + const bool have_throttle = SRV_Channels::function_assigned(SRV_Channel::k_throttle); + const bool have_throttle_left = SRV_Channels::function_assigned(SRV_Channel::k_throttleLeft); + const bool have_throttle_right = SRV_Channels::function_assigned(SRV_Channel::k_throttleRight); + // check that there's defined outputs, inc scripting and sail - if(!SRV_Channels::function_assigned(SRV_Channel::k_throttleLeft) && - !SRV_Channels::function_assigned(SRV_Channel::k_throttleRight) && - !SRV_Channels::function_assigned(SRV_Channel::k_throttle) && + if(!have_throttle_left && + !have_throttle_right && + !have_throttle && !SRV_Channels::function_assigned(SRV_Channel::k_steering) && !SRV_Channels::function_assigned(SRV_Channel::k_scripting1) && !has_sail()) { @@ -470,14 +501,14 @@ bool AP_MotorsUGV::pre_arm_check(bool report) const return false; } // check if only one of skid-steering output has been configured - if (SRV_Channels::function_assigned(SRV_Channel::k_throttleLeft) != SRV_Channels::function_assigned(SRV_Channel::k_throttleRight)) { + if (have_throttle_left != have_throttle_right) { if (report) { GCS_SEND_TEXT(MAV_SEVERITY_CRITICAL, "PreArm: check skid steering config"); } return false; } // check if only one of throttle or steering outputs has been configured, if has a sail allow no throttle - if ((has_sail() || SRV_Channels::function_assigned(SRV_Channel::k_throttle)) != SRV_Channels::function_assigned(SRV_Channel::k_steering)) { + if ((has_sail() || have_throttle) != SRV_Channels::function_assigned(SRV_Channel::k_steering)) { if (report) { GCS_SEND_TEXT(MAV_SEVERITY_CRITICAL, "PreArm: check steering and throttle config"); } @@ -493,6 +524,35 @@ bool AP_MotorsUGV::pre_arm_check(bool report) const return false; } } + + // Check relays are configured for brushed with relay outputs +#if AP_RELAY_ENABLED + AP_Relay*relay = AP::relay(); + if ((_pwm_type == PWM_TYPE_BRUSHED_WITH_RELAY) && (relay != nullptr)) { + // If a output is configured its relay must be enabled + struct RelayTable { + bool output_assigned; + AP_Relay_Params::FUNCTION fun; + }; + + const RelayTable relay_table[] = { + { have_throttle || have_throttle_left || (SRV_Channels::function_assigned(SRV_Channel::k_motor1) && (_motors_num >= 1)), AP_Relay_Params::FUNCTION::BRUSHED_REVERSE_1 }, + { have_throttle_right || (SRV_Channels::function_assigned(SRV_Channel::k_motor2) && (_motors_num >= 2)), AP_Relay_Params::FUNCTION::BRUSHED_REVERSE_2 }, + { SRV_Channels::function_assigned(SRV_Channel::k_motor3) && (_motors_num >= 3), AP_Relay_Params::FUNCTION::BRUSHED_REVERSE_3 }, + { SRV_Channels::function_assigned(SRV_Channel::k_motor4) && (_motors_num >= 4), AP_Relay_Params::FUNCTION::BRUSHED_REVERSE_4 }, + }; + + for (uint8_t i=0; ienabled(relay_table[i].fun)) { + if (report) { + gcs().send_text(MAV_SEVERITY_CRITICAL, "PreArm: relay function %u unassigned", uint8_t(relay_table[i].fun)); + } + return false; + } + } + } +#endif + return true; } @@ -918,9 +978,9 @@ void AP_MotorsUGV::output_throttle(SRV_Channel::Aux_servo_function_t function, f throttle = get_rate_controlled_throttle(function, throttle, dt); // set relay if necessary -#if AP_SERVORELAYEVENTS_ENABLED && AP_RELAY_ENABLED - if (_pwm_type == PWM_TYPE_BRUSHED_WITH_RELAY) { - auto &_relayEvents { *AP::servorelayevents() }; +#if AP_RELAY_ENABLED + AP_Relay*relay = AP::relay(); + if ((_pwm_type == PWM_TYPE_BRUSHED_WITH_RELAY) && (relay != nullptr)) { // find the output channel, if not found return const SRV_Channel *out_chan = SRV_Channels::get_channel_for(function); @@ -930,30 +990,31 @@ void AP_MotorsUGV::output_throttle(SRV_Channel::Aux_servo_function_t function, f const int8_t reverse_multiplier = out_chan->get_reversed() ? -1 : 1; bool relay_high = is_negative(reverse_multiplier * throttle); + AP_Relay_Params::FUNCTION relay_function; switch (function) { case SRV_Channel::k_throttle: case SRV_Channel::k_throttleLeft: case SRV_Channel::k_motor1: - _relayEvents.do_set_relay(0, relay_high); + default: + relay_function = AP_Relay_Params::FUNCTION::BRUSHED_REVERSE_1; break; case SRV_Channel::k_throttleRight: case SRV_Channel::k_motor2: - _relayEvents.do_set_relay(1, relay_high); + relay_function = AP_Relay_Params::FUNCTION::BRUSHED_REVERSE_2; break; case SRV_Channel::k_motor3: - _relayEvents.do_set_relay(2, relay_high); + relay_function = AP_Relay_Params::FUNCTION::BRUSHED_REVERSE_3; break; case SRV_Channel::k_motor4: - _relayEvents.do_set_relay(3, relay_high); - break; - default: - // do nothing + relay_function = AP_Relay_Params::FUNCTION::BRUSHED_REVERSE_4; break; } + relay->set(relay_function, relay_high); + // invert the output to always have positive value calculated by calc_pwm throttle = reverse_multiplier * fabsf(throttle); } -#endif // AP_SERVORELAYEVENTS_ENABLED && AP_RELAY_ENABLED +#endif // AP_RELAY_ENABLED // output to servo channel switch (function) { diff --git a/libraries/AR_Motors/AP_MotorsUGV.h b/libraries/AR_Motors/AP_MotorsUGV.h index df61e8734d974..42c2df5377ae9 100644 --- a/libraries/AR_Motors/AP_MotorsUGV.h +++ b/libraries/AR_Motors/AP_MotorsUGV.h @@ -114,6 +114,9 @@ class AP_MotorsUGV { // returns true if the vehicle is omni bool is_omni() const { return _frame_type != FRAME_TYPE_UNDEFINED && _motors_num > 0; } + // Return the relay index that would be used for param conversion to relay functions + bool get_legacy_relay_index(int8_t &index1, int8_t &index2, int8_t &index3, int8_t &index4) const; + // structure for holding motor limit flags struct AP_MotorsUGV_limit { uint8_t steer_left : 1; // we have reached the steering controller's left most limit From 227113fbbf552b3d53d4fcbc904ee1163a45ee1f Mon Sep 17 00:00:00 2001 From: Iampete1 Date: Sat, 9 Dec 2023 19:12:10 +0000 Subject: [PATCH 14/16] AP_Relay: add rover motor reverse functions --- libraries/AP_Relay/AP_Relay.cpp | 51 +++++++++++++++++++++----- libraries/AP_Relay/AP_Relay_Params.cpp | 11 +++++- libraries/AP_Relay/AP_Relay_Params.h | 4 ++ 3 files changed, 55 insertions(+), 11 deletions(-) diff --git a/libraries/AP_Relay/AP_Relay.cpp b/libraries/AP_Relay/AP_Relay.cpp index 02b9816a089ae..f9c7cbfa35d5c 100644 --- a/libraries/AP_Relay/AP_Relay.cpp +++ b/libraries/AP_Relay/AP_Relay.cpp @@ -19,6 +19,11 @@ #include #include +#include +#if APM_BUILD_TYPE(APM_BUILD_Rover) +#include +#endif + #if CONFIG_HAL_BOARD == HAL_BOARD_SITL #define RELAY1_PIN_DEFAULT 13 @@ -146,6 +151,14 @@ void AP_Relay::convert_params() } #endif +#if APM_BUILD_TYPE(APM_BUILD_Rover) + int8_t rover_relay[] = { -1, -1, -1, -1 }; + AP_MotorsUGV *motors = AP::motors_ugv(); + if (motors != nullptr) { + motors->get_legacy_relay_index(rover_relay[0], rover_relay[1], rover_relay[2], rover_relay[3]); + } +#endif + // Find old default param int8_t default_state = 0; // off was the old behaviour const bool have_default = AP_Param::get_param_by_index(this, 4, AP_PARAM_INT8, &default_state); @@ -165,29 +178,47 @@ void AP_Relay::convert_params() int8_t pin = 0; if (!_params[i].pin.configured() && AP_Param::get_param_by_index(this, param_index, AP_PARAM_INT8, &pin) && (pin >= 0)) { - // Copy old pin parameter + // Copy old pin parameter if valid _params[i].pin.set_and_save(pin); } - if (_params[i].pin < 0) { - // Don't enable if pin is invalid - continue; - } - // Valid pin, this is either due to the param conversion or default value // Work out what function this relay should be + AP_Relay_Params::Function new_fun; if (i == chute_relay) { - _params[i].function.set_and_save(int8_t(AP_Relay_Params::Function::parachute)); + new_fun = AP_Relay_Params::Function::parachute; } else if (i == ice_relay) { - _params[i].function.set_and_save(int8_t(AP_Relay_Params::Function::ignition)); + new_fun = AP_Relay_Params::Function::ignition; } else if (i == cam_relay) { - _params[i].function.set_and_save(int8_t(AP_Relay_Params::Function::camera)); + new_fun = AP_Relay_Params::Function::camera; + +#if APM_BUILD_TYPE(APM_BUILD_Rover) + } else if (i == rover_relay[0]) { + new_fun = AP_Relay_Params::Function::brushed_reverse_1; + + } else if (i == rover_relay[1]) { + new_fun = AP_Relay_Params::Function::brushed_reverse_2; + + } else if (i == rover_relay[2]) { + new_fun = AP_Relay_Params::Function::brushed_reverse_3; + + } else if (i == rover_relay[3]) { + new_fun = AP_Relay_Params::Function::brushed_reverse_4; +#endif } else { - _params[i].function.set_and_save(int8_t(AP_Relay_Params::Function::relay)); + if (_params[i].pin < 0) { + // Don't enable as numbered relay if pin is invalid + // Other functions should be enabled with a invalid pin + // This will result in a pre-arm promoting the user to resolve + continue; + } + new_fun = AP_Relay_Params::Function::relay; } + _params[i].function.set_and_save(int8_t(new_fun)); + // Set the default state if (have_default) { diff --git a/libraries/AP_Relay/AP_Relay_Params.cpp b/libraries/AP_Relay/AP_Relay_Params.cpp index 9ea593cea6f65..37b04e89d34be 100644 --- a/libraries/AP_Relay/AP_Relay_Params.cpp +++ b/libraries/AP_Relay/AP_Relay_Params.cpp @@ -5,7 +5,16 @@ const AP_Param::GroupInfo AP_Relay_Params::var_info[] = { // @Param: FUNCTION // @DisplayName: Relay function // @Description: The function the relay channel is mapped to. - // @Values: 0:None,1:Relay,2:Ignition,3:Parachute,4:Camera + // @Values: 0:None + // @Values: 1:Relay + // @Values{Plane}: 2:Ignition + // @Values{Plane, Copter}: 3:Parachute + // @Values: 4:Camera + // @Values{Rover}: 5:Bushed motor reverse 1 throttle or throttle-left or omni motor 1 + // @Values{Rover}: 6:Bushed motor reverse 2 throttle-right or omni motor 2 + // @Values{Rover}: 7:Bushed motor reverse 3 omni motor 3 + // @Values{Rover}: 8:Bushed motor reverse 4 omni motor 4 + // @User: Standard AP_GROUPINFO_FLAGS("FUNCTION", 1, AP_Relay_Params, function, (float)Function::none, AP_PARAM_FLAG_ENABLE), diff --git a/libraries/AP_Relay/AP_Relay_Params.h b/libraries/AP_Relay/AP_Relay_Params.h index 6e9105dbc7ddb..29161adf32911 100644 --- a/libraries/AP_Relay/AP_Relay_Params.h +++ b/libraries/AP_Relay/AP_Relay_Params.h @@ -24,6 +24,10 @@ class AP_Relay_Params { ignition = 2, parachute = 3, camera = 4, + brushed_reverse_1 = 5, + brushed_reverse_2 = 6, + brushed_reverse_3 = 7, + brushed_reverse_4 = 8, num_functions // must be the last entry }; From b177be22ebe4685b3be9f88585a4f13042fd2140 Mon Sep 17 00:00:00 2001 From: Iampete1 Date: Tue, 12 Dec 2023 14:02:14 +0000 Subject: [PATCH 15/16] AP_Relay: capitalize function enum --- libraries/AP_Relay/AP_Relay.cpp | 34 +++++++++++++------------- libraries/AP_Relay/AP_Relay.h | 4 +-- libraries/AP_Relay/AP_Relay_Params.cpp | 2 +- libraries/AP_Relay/AP_Relay_Params.h | 24 +++++++++--------- 4 files changed, 32 insertions(+), 32 deletions(-) diff --git a/libraries/AP_Relay/AP_Relay.cpp b/libraries/AP_Relay/AP_Relay.cpp index f9c7cbfa35d5c..8c4f3f08bc31b 100644 --- a/libraries/AP_Relay/AP_Relay.cpp +++ b/libraries/AP_Relay/AP_Relay.cpp @@ -183,28 +183,28 @@ void AP_Relay::convert_params() } // Work out what function this relay should be - AP_Relay_Params::Function new_fun; + AP_Relay_Params::FUNCTION new_fun; if (i == chute_relay) { - new_fun = AP_Relay_Params::Function::parachute; + new_fun = AP_Relay_Params::FUNCTION::PARACHUTE; } else if (i == ice_relay) { - new_fun = AP_Relay_Params::Function::ignition; + new_fun = AP_Relay_Params::FUNCTION::IGNITION; } else if (i == cam_relay) { - new_fun = AP_Relay_Params::Function::camera; + new_fun = AP_Relay_Params::FUNCTION::CAMERA; #if APM_BUILD_TYPE(APM_BUILD_Rover) } else if (i == rover_relay[0]) { - new_fun = AP_Relay_Params::Function::brushed_reverse_1; + new_fun = AP_Relay_Params::FUNCTION::BRUSHED_REVERSE_1; } else if (i == rover_relay[1]) { - new_fun = AP_Relay_Params::Function::brushed_reverse_2; + new_fun = AP_Relay_Params::FUNCTION::BRUSHED_REVERSE_2; } else if (i == rover_relay[2]) { - new_fun = AP_Relay_Params::Function::brushed_reverse_3; + new_fun = AP_Relay_Params::FUNCTION::BRUSHED_REVERSE_3; } else if (i == rover_relay[3]) { - new_fun = AP_Relay_Params::Function::brushed_reverse_4; + new_fun = AP_Relay_Params::FUNCTION::BRUSHED_REVERSE_4; #endif } else { @@ -214,7 +214,7 @@ void AP_Relay::convert_params() // This will result in a pre-arm promoting the user to resolve continue; } - new_fun = AP_Relay_Params::Function::relay; + new_fun = AP_Relay_Params::FUNCTION::RELAY; } _params[i].function.set_and_save(int8_t(new_fun)); @@ -257,13 +257,13 @@ void AP_Relay::init() continue; } - const AP_Relay_Params::Function function = _params[instance].function; - if (function < AP_Relay_Params::Function::relay || function >= AP_Relay_Params::Function::num_functions) { + const AP_Relay_Params::FUNCTION function = _params[instance].function; + if (function <= AP_Relay_Params::FUNCTION::NONE || function >= AP_Relay_Params::FUNCTION::NUM_FUNCTIONS) { // invalid function, skip it continue; } - if (function == AP_Relay_Params::Function::relay) { + if (function == AP_Relay_Params::FUNCTION::RELAY) { // relay by instance number, set the state to match our output const AP_Relay_Params::DefaultState default_state = _params[instance].default_state; if ((default_state == AP_Relay_Params::DefaultState::OFF) || @@ -279,8 +279,8 @@ void AP_Relay::init() } } -void AP_Relay::set(const AP_Relay_Params::Function function, const bool value) { - if (function <= AP_Relay_Params::Function::none && function >= AP_Relay_Params::Function::num_functions) { +void AP_Relay::set(const AP_Relay_Params::FUNCTION function, const bool value) { + if (function <= AP_Relay_Params::FUNCTION::NONE && function >= AP_Relay_Params::FUNCTION::NUM_FUNCTIONS) { // invalid function return; } @@ -328,7 +328,7 @@ void AP_Relay::set(const uint8_t instance, const bool value) return; } - if (_params[instance].function != AP_Relay_Params::Function::relay) { + if (_params[instance].function != AP_Relay_Params::FUNCTION::RELAY) { return; } @@ -383,11 +383,11 @@ bool AP_Relay::get(uint8_t instance) const bool AP_Relay::enabled(uint8_t instance) const { // Must be a valid instance with function relay and pin set - return (instance < AP_RELAY_NUM_RELAYS) && (_params[instance].pin != -1) && (_params[instance].function == AP_Relay_Params::Function::relay); + return (instance < AP_RELAY_NUM_RELAYS) && (_params[instance].pin != -1) && (_params[instance].function == AP_Relay_Params::FUNCTION::RELAY); } // see if the relay is enabled -bool AP_Relay::enabled(AP_Relay_Params::Function function) const +bool AP_Relay::enabled(AP_Relay_Params::FUNCTION function) const { for (uint8_t instance = 0; instance < AP_RELAY_NUM_RELAYS; instance++) { if ((_params[instance].function == function) && (_params[instance].pin != -1)) { diff --git a/libraries/AP_Relay/AP_Relay.h b/libraries/AP_Relay/AP_Relay.h index a668e9144c71a..51e08a4015a01 100644 --- a/libraries/AP_Relay/AP_Relay.h +++ b/libraries/AP_Relay/AP_Relay.h @@ -55,10 +55,10 @@ class AP_Relay { bool send_relay_status(const class GCS_MAVLINK &link) const; - void set(AP_Relay_Params::Function function, bool value); + void set(AP_Relay_Params::FUNCTION function, bool value); // see if the relay is enabled - bool enabled(AP_Relay_Params::Function function) const; + bool enabled(AP_Relay_Params::FUNCTION function) const; private: static AP_Relay *singleton; diff --git a/libraries/AP_Relay/AP_Relay_Params.cpp b/libraries/AP_Relay/AP_Relay_Params.cpp index 37b04e89d34be..afce285cd4fe4 100644 --- a/libraries/AP_Relay/AP_Relay_Params.cpp +++ b/libraries/AP_Relay/AP_Relay_Params.cpp @@ -16,7 +16,7 @@ const AP_Param::GroupInfo AP_Relay_Params::var_info[] = { // @Values{Rover}: 8:Bushed motor reverse 4 omni motor 4 // @User: Standard - AP_GROUPINFO_FLAGS("FUNCTION", 1, AP_Relay_Params, function, (float)Function::none, AP_PARAM_FLAG_ENABLE), + AP_GROUPINFO_FLAGS("FUNCTION", 1, AP_Relay_Params, function, (float)FUNCTION::NONE, AP_PARAM_FLAG_ENABLE), // @Param: PIN // @DisplayName: Relay pin diff --git a/libraries/AP_Relay/AP_Relay_Params.h b/libraries/AP_Relay/AP_Relay_Params.h index 29161adf32911..7239dda9c9106 100644 --- a/libraries/AP_Relay/AP_Relay_Params.h +++ b/libraries/AP_Relay/AP_Relay_Params.h @@ -18,20 +18,20 @@ class AP_Relay_Params { NO_CHANGE = 2, }; - enum class Function : uint8_t { - none = 0, - relay = 1, - ignition = 2, - parachute = 3, - camera = 4, - brushed_reverse_1 = 5, - brushed_reverse_2 = 6, - brushed_reverse_3 = 7, - brushed_reverse_4 = 8, - num_functions // must be the last entry + enum class FUNCTION : uint8_t { + NONE = 0, + RELAY = 1, + IGNITION = 2, + PARACHUTE = 3, + CAMERA = 4, + BRUSHED_REVERSE_1 = 5, + BRUSHED_REVERSE_2 = 6, + BRUSHED_REVERSE_3 = 7, + BRUSHED_REVERSE_4 = 8, + NUM_FUNCTIONS // must be the last entry }; - AP_Enum function; // relay function + AP_Enum function; // relay function AP_Int16 pin; // gpio pin number AP_Enum default_state; // default state }; From ce2f4ffe89ecc52f629868377ddbbc2724f2b2fb Mon Sep 17 00:00:00 2001 From: Iampete1 Date: Mon, 18 Dec 2023 03:24:09 +0000 Subject: [PATCH 16/16] AP_Relay: move from using AP_RELAY_NUM_RELAYS to ARRAY_SIZE(_params) --- libraries/AP_Relay/AP_Relay.cpp | 22 +++++++++++----------- 1 file changed, 11 insertions(+), 11 deletions(-) diff --git a/libraries/AP_Relay/AP_Relay.cpp b/libraries/AP_Relay/AP_Relay.cpp index 8c4f3f08bc31b..921163fbd6262 100644 --- a/libraries/AP_Relay/AP_Relay.cpp +++ b/libraries/AP_Relay/AP_Relay.cpp @@ -164,7 +164,7 @@ void AP_Relay::convert_params() const bool have_default = AP_Param::get_param_by_index(this, 4, AP_PARAM_INT8, &default_state); // grab the old values if they were set - for (uint8_t i = 0; i < MIN(AP_RELAY_NUM_RELAYS, 6); i++) { + for (uint8_t i = 0; i < MIN(ARRAY_SIZE(_params), 6U); i++) { if (_params[i].function.configured()) { // Conversion already done, or user has configured manually continue; @@ -235,7 +235,7 @@ void AP_Relay::set_defaults() { RELAY5_PIN_DEFAULT, RELAY6_PIN_DEFAULT }; - for (uint8_t i = 0; i < MIN(uint8_t(AP_RELAY_NUM_RELAYS), ARRAY_SIZE(pins)); i++) { + for (uint8_t i = 0; i < MIN(ARRAY_SIZE(_params), ARRAY_SIZE(pins)); i++) { // set the default if (pins[i] != -1) { _params[i].pin.set_default(pins[i]); @@ -250,7 +250,7 @@ void AP_Relay::init() convert_params(); // setup the actual default values of all the pins - for (uint8_t instance = 0; instance < AP_RELAY_NUM_RELAYS; instance++) { + for (uint8_t instance = 0; instance < ARRAY_SIZE(_params); instance++) { const int8_t pin = _params[instance].pin; if (pin == -1) { // no valid pin to set it on, skip it @@ -285,7 +285,7 @@ void AP_Relay::set(const AP_Relay_Params::FUNCTION function, const bool value) { return; } - for (uint8_t instance = 0; instance < AP_RELAY_NUM_RELAYS; instance++) { + for (uint8_t instance = 0; instance < ARRAY_SIZE(_params); instance++) { if (function != _params[instance].function) { continue; } @@ -324,7 +324,7 @@ void AP_Relay::set_pin_by_instance(uint8_t instance, bool value) void AP_Relay::set(const uint8_t instance, const bool value) { - if (instance >= AP_RELAY_NUM_RELAYS) { + if (instance >= ARRAY_SIZE(_params)) { return; } @@ -337,7 +337,7 @@ void AP_Relay::set(const uint8_t instance, const bool value) void AP_Relay::toggle(uint8_t instance) { - if (instance < AP_RELAY_NUM_RELAYS) { + if (instance < ARRAY_SIZE(_params)) { set(instance, !get(instance)); } } @@ -345,7 +345,7 @@ void AP_Relay::toggle(uint8_t instance) // check settings are valid bool AP_Relay::arming_checks(size_t buflen, char *buffer) const { - for (uint8_t i=0; ivalid_pin(pin)) { char param_name_buf[14]; @@ -364,7 +364,7 @@ bool AP_Relay::arming_checks(size_t buflen, char *buffer) const bool AP_Relay::get(uint8_t instance) const { - if (instance >= AP_RELAY_NUM_RELAYS) { + if (instance >= ARRAY_SIZE(_params)) { // invalid instance return false; } @@ -383,13 +383,13 @@ bool AP_Relay::get(uint8_t instance) const bool AP_Relay::enabled(uint8_t instance) const { // Must be a valid instance with function relay and pin set - return (instance < AP_RELAY_NUM_RELAYS) && (_params[instance].pin != -1) && (_params[instance].function == AP_Relay_Params::FUNCTION::RELAY); + return (instance < ARRAY_SIZE(_params)) && (_params[instance].pin != -1) && (_params[instance].function == AP_Relay_Params::FUNCTION::RELAY); } // see if the relay is enabled bool AP_Relay::enabled(AP_Relay_Params::FUNCTION function) const { - for (uint8_t instance = 0; instance < AP_RELAY_NUM_RELAYS; instance++) { + for (uint8_t instance = 0; instance < ARRAY_SIZE(_params); instance++) { if ((_params[instance].function == function) && (_params[instance].pin != -1)) { return true; } @@ -408,7 +408,7 @@ bool AP_Relay::send_relay_status(const GCS_MAVLINK &link) const uint16_t present_mask = 0; uint16_t on_mask = 0; - for (auto i=0; i