From a2f35b3150157db3522690fedea597602389a888 Mon Sep 17 00:00:00 2001 From: Peter Barker Date: Wed, 13 Nov 2024 08:09:59 +1100 Subject: [PATCH] ArduCopter: make SRV_Channels::cork non-static for symmetry with the push function --- ArduCopter/compassmot.cpp | 5 +++-- ArduCopter/esc_calibration.cpp | 18 ++++++++++-------- ArduCopter/motors.cpp | 6 ++++-- 3 files changed, 17 insertions(+), 12 deletions(-) diff --git a/ArduCopter/compassmot.cpp b/ArduCopter/compassmot.cpp index c0d26041c0..28282c3539 100644 --- a/ArduCopter/compassmot.cpp +++ b/ArduCopter/compassmot.cpp @@ -146,9 +146,10 @@ MAV_RESULT Copter::mavlink_compassmot(const GCS_MAVLINK &gcs_chan) read_radio(); // pass through throttle to motors - SRV_Channels::cork(); + auto &srv = AP::srv(); + srv.cork(); motors->set_throttle_passthrough_for_esc_calibration(channel_throttle->get_control_in() * 0.001f); - AP::srv().push(); + srv.push(); // read some compass values compass.read(); diff --git a/ArduCopter/esc_calibration.cpp b/ArduCopter/esc_calibration.cpp index 722127898e..0c1d7ea70e 100644 --- a/ArduCopter/esc_calibration.cpp +++ b/ArduCopter/esc_calibration.cpp @@ -95,9 +95,10 @@ void Copter::esc_calibration_passthrough() hal.scheduler->delay(3); // pass through to motors - SRV_Channels::cork(); + auto &srv = AP::srv(); + srv.cork(); motors->set_throttle_passthrough_for_esc_calibration(channel_throttle->get_control_in() * 0.001f); - AP::srv().push(); + srv.push(); } #endif // FRAME_CONFIG != HELI_FRAME } @@ -112,25 +113,26 @@ void Copter::esc_calibration_auto() esc_calibration_setup(); // raise throttle to maximum - SRV_Channels::cork(); + auto &srv = AP::srv(); + srv.cork(); motors->set_throttle_passthrough_for_esc_calibration(1.0f); - AP::srv().push(); + srv.push(); // delay for 5 seconds while outputting pulses uint32_t tstart = millis(); while (millis() - tstart < 5000) { - SRV_Channels::cork(); + srv.cork(); motors->set_throttle_passthrough_for_esc_calibration(1.0f); - AP::srv().push(); + srv.push(); esc_calibration_notify(); hal.scheduler->delay(3); } // block until we restart while(1) { - SRV_Channels::cork(); + srv.cork(); motors->set_throttle_passthrough_for_esc_calibration(0.0f); - AP::srv().push(); + srv.push(); esc_calibration_notify(); hal.scheduler->delay(3); } diff --git a/ArduCopter/motors.cpp b/ArduCopter/motors.cpp index 328eb83150..c37a4d55a2 100644 --- a/ArduCopter/motors.cpp +++ b/ArduCopter/motors.cpp @@ -156,8 +156,10 @@ void Copter::motors_output() // output any servo channels SRV_Channels::calc_pwm(); + auto &srv = AP::srv(); + // cork now, so that all channel outputs happen at once - SRV_Channels::cork(); + srv.cork(); // update output on any aux channels, for manual passthru SRV_Channels::output_ch_all(); @@ -181,7 +183,7 @@ void Copter::motors_output() } // push all channels - AP::srv().push(); + srv.push(); } // check for pilot stick input to trigger lost vehicle alarm