From 33e4f4443453d51e18c1d80c0f0a0f7a829d684d Mon Sep 17 00:00:00 2001
From: "Kirill A. Kornilov" <kirill@tp>
Date: Tue, 25 Nov 2014 08:19:27 +1100
Subject: [PATCH] RC_Channel: added set_radio_trimmed()
take into account trim value in set_radio()
---
libraries/RC_Channel/RC_Channel_aux.cpp | 18 ++++++++++++++++++
libraries/RC_Channel/RC_Channel_aux.h | 3 +++
2 files changed, 21 insertions(+)
diff --git a/libraries/RC_Channel/RC_Channel_aux.cpp b/libraries/RC_Channel/RC_Channel_aux.cpp
index 545d7fd7b..69300856a 100644
--- a/libraries/RC_Channel/RC_Channel_aux.cpp
+++ b/libraries/RC_Channel/RC_Channel_aux.cpp
@@ -162,6 +162,24 @@ RC_Channel_aux::set_radio(RC_Channel_aux::Aux_servo_function_t function, int16_t
}
}
+/*
+ set radio_out for all channels matching the given function type, allow radio_trim to center servo
+ */
+void
+RC_Channel_aux::set_radio_trimmed(RC_Channel_aux::Aux_servo_function_t function, int16_t value)
+{
+ if (!function_assigned(function)) {
+ return;
+ }
+ for (uint8_t i = 0; i < RC_AUX_MAX_CHANNELS; i++) {
+ if (_aux_channels[i] && _aux_channels[i]->function.get() == function) {
+ int16_t value2 = value - 1500 + _aux_channels[i]->radio_trim;
+ _aux_channels[i]->radio_out = constrain_int16(value2,_aux_channels[i]->radio_min,_aux_channels[i]->radio_max);
+ _aux_channels[i]->output();
+ }
+ }
+}
+
/*
set and save the trim value to radio_in for all channels matching
the given function type
diff --git a/libraries/RC_Channel/RC_Channel_aux.h b/libraries/RC_Channel/RC_Channel_aux.h
index 2eac1927d..44fcc5d43 100644
--- a/libraries/RC_Channel/RC_Channel_aux.h
+++ b/libraries/RC_Channel/RC_Channel_aux.h
@@ -82,6 +82,9 @@ public:
// set radio_out for a function channel
static void set_radio(Aux_servo_function_t function, int16_t value);
+ // set radio_out for all channels matching the given function type, allow radio_trim to center servo
+ static void set_radio_trimmed(Aux_servo_function_t function, int16_t value);
+
// set and save the trim for a function channel to radio_in
static void set_radio_trim(Aux_servo_function_t function);
--
GitLab