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 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 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_ 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_ 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() 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); 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; 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 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 diff --git a/libraries/AP_Relay/AP_Relay.cpp b/libraries/AP_Relay/AP_Relay.cpp index 1c4cb143f7b0f..921163fbd6262 100644 --- a/libraries/AP_Relay/AP_Relay.cpp +++ b/libraries/AP_Relay/AP_Relay.cpp @@ -15,6 +15,15 @@ #include #include +#include +#include +#include + +#include +#if APM_BUILD_TYPE(APM_BUILD_Rover) +#include +#endif + #if CONFIG_HAL_BOARD == HAL_BOARD_SITL #define RELAY1_PIN_DEFAULT 13 @@ -60,54 +69,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,61 +120,236 @@ AP_Relay::AP_Relay(void) singleton = this; } +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 + +#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); + + // grab the old values if they were set + 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; + } + + 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)) { + // Copy old pin parameter if valid + _params[i].pin.set_and_save(pin); + } + + // Work out what function this relay should be + AP_Relay_Params::FUNCTION new_fun; + if (i == chute_relay) { + new_fun = AP_Relay_Params::FUNCTION::PARACHUTE; + + } else if (i == ice_relay) { + new_fun = AP_Relay_Params::FUNCTION::IGNITION; + + } else if (i == cam_relay) { + 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 { + 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) { + _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(ARRAY_SIZE(_params), 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 < ARRAY_SIZE(_params); 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::NONE || 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) { - return; - } - if (_pin[instance] == -1) { + const int8_t pin = _params[instance].pin; + if (pin == -1) { + // no valid pin to set it on, skip it 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 >= ARRAY_SIZE(_params)) { + 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 < ARRAY_SIZE(_params)) { + set(instance, !get(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[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 +362,40 @@ bool AP_Relay::arming_checks(size_t buflen, char *buffer) const return true; } +bool AP_Relay::get(uint8_t instance) const +{ + if (instance >= ARRAY_SIZE(_params)) { + // 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 < 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 < ARRAY_SIZE(_params); instance++) { + if ((_params[instance].function == function) && (_params[instance].pin != -1)) { + return true; + } + } + return false; +} #if AP_MAVLINK_MSG_RELAY_STATUS_ENABLED // this method may only return false if there is no space in the @@ -207,14 +408,14 @@ 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 +#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 + // @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), + + // @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..7239dda9c9106 --- /dev/null +++ b/libraries/AP_Relay/AP_Relay_Params.h @@ -0,0 +1,37 @@ +#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, + 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_Int16 pin; // gpio pin number + AP_Enum default_state; // default state +}; 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