diff --git a/libraries/AP_Camera/AP_Camera.cpp b/libraries/AP_Camera/AP_Camera.cpp index a44243c8ca..76f3524e22 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 356901ab26..71ef3ffda4 100644 --- a/libraries/AP_Camera/AP_Camera.h +++ b/libraries/AP_Camera/AP_Camera.h @@ -182,6 +182,9 @@ public: 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 a48adeb348..db8fc2adec 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);