diff --git a/libraries/AP_Mount/AP_Mount_Backend.cpp b/libraries/AP_Mount/AP_Mount_Backend.cpp index 5fc2a3cff0..59956ac89a 100644 --- a/libraries/AP_Mount/AP_Mount_Backend.cpp +++ b/libraries/AP_Mount/AP_Mount_Backend.cpp @@ -118,6 +118,34 @@ void AP_Mount_Backend::set_rate_target(float roll_degs, float pitch_degs, float } } +void AP_Mount_Backend::control_mount_open_servo() { + // check if the open function is assigned to a servo channel. + uint8_t open_chan; + if (!SRV_Channels::find_channel(SRV_Channel::k_mount_open, open_chan) && + !SRV_Channels::find_channel(SRV_Channel::k_mount2_open, open_chan)) { + return; // open function not assigned, do nothing. + } + + // get the configured PWM values for open and closed states. + SRV_Channel *channel = SRV_Channels::srv_channel(open_chan); + if (!channel) { + return; // channel not found. + } + uint16_t pwm_open = channel->get_output_max(); + uint16_t pwm_closed = channel->get_output_min(); + + // determine the desired open state based on the current mode. + bool mount_open = true; + if (get_mode() == MAV_MOUNT_MODE_RETRACT) { + mount_open = false; + } + + // move the servo to the appropriate position. + uint16_t target_pwm = mount_open ? pwm_open : pwm_closed; + SRV_Channels::move_servo(SRV_Channel::Aux_servo_function_t(open_chan), + target_pwm, pwm_closed, pwm_open); +} + // set_roi_target - sets target location that mount should attempt to point towards void AP_Mount_Backend::set_roi_target(const Location &target_loc) { diff --git a/libraries/AP_Mount/AP_Mount_Backend.h b/libraries/AP_Mount/AP_Mount_Backend.h index 56706d7463..d8a26d8402 100644 --- a/libraries/AP_Mount/AP_Mount_Backend.h +++ b/libraries/AP_Mount/AP_Mount_Backend.h @@ -293,6 +293,9 @@ protected: // sent warning to GCS void send_warning_to_GCS(const char* warning_str); + // control the open/close servo based on current mode and parameters. + void control_mount_open_servo(); + AP_Mount &_frontend; // reference to the front end which holds parameters AP_Mount_Params &_params; // parameters for this backend uint8_t _instance; // this instance's number diff --git a/libraries/AP_Mount/AP_Mount_Servo.cpp b/libraries/AP_Mount/AP_Mount_Servo.cpp index 2f926c7d84..29ce414eb3 100644 --- a/libraries/AP_Mount/AP_Mount_Servo.cpp +++ b/libraries/AP_Mount/AP_Mount_Servo.cpp @@ -30,6 +30,9 @@ void AP_Mount_Servo::update() // change to RC_TARGETING mode if RC input has changed set_rctargeting_on_rcinput_change(); + // control the open/close servo. + control_mount_open_servo(); + auto mount_mode = get_mode(); switch (mount_mode) { // move mount to a "retracted position" or to a position where a fourth servo can retract the entire mount into the fuselage @@ -109,9 +112,6 @@ void AP_Mount_Servo::update() break; } - // move mount to a "retracted position" into the fuselage with a fourth servo - const bool mount_open = (mount_mode == MAV_MOUNT_MODE_RETRACT) ? 0 : 1; - move_servo(_open_idx, mount_open, 0, 1); // write the results to the servos move_servo(_roll_idx, degrees(_angle_bf_output_rad.x)*10, _params.roll_angle_min*10, _params.roll_angle_max*10);