1 //
2 // Copyright 2010-2011,2014 Ettus Research LLC
3 // Copyright 2018 Ettus Research, a National Instruments Company
4 //
5 // SPDX-License-Identifier: GPL-3.0-or-later
6 //
7
8 #include <uhd/exception.hpp>
9 #include <uhd/types/dict.hpp>
10 #include <uhd/utils/algorithm.hpp>
11 #include <uhd/utils/gain_group.hpp>
12 #include <uhd/utils/log.hpp>
13 #include <algorithm>
14 #include <functional>
15 #include <vector>
16
17 using namespace uhd;
18
compare_by_step_size(const size_t & rhs,const size_t & lhs,std::vector<gain_fcns_t> & fcns)19 static bool compare_by_step_size(
20 const size_t& rhs, const size_t& lhs, std::vector<gain_fcns_t>& fcns)
21 {
22 return fcns.at(rhs).get_range().step() > fcns.at(lhs).get_range().step();
23 }
24
25 /*!
26 * Get a multiple of step with the following relation:
27 * result = step*floor(num/step)
28 *
29 * Due to small doubleing-point inaccuracies:
30 * num = n*step + e, where e is a small inaccuracy.
31 * When e is negative, floor would yield (n-1)*step,
32 * despite that n*step is really the desired result.
33 * This function is designed to mitigate that issue.
34 *
35 * \param num the number to approximate
36 * \param step the step size to round with
37 * \param e the small inaccuracy to account for
38 * \return a multiple of step approximating num
39 */
40 template <typename T>
floor_step(T num,T step,T e=T (0.001))41 static T floor_step(T num, T step, T e = T(0.001))
42 {
43 if (num < T(0)) {
44 return step * int(num / step - e);
45 } else {
46 return step * int(num / step + e);
47 }
48 }
49
~gain_group(void)50 gain_group::~gain_group(void)
51 {
52 /* NOP */
53 }
54
55 /***********************************************************************
56 * gain group implementation
57 **********************************************************************/
58 class gain_group_impl : public gain_group
59 {
60 public:
gain_group_impl(void)61 gain_group_impl(void)
62 {
63 /*NOP*/
64 }
65
get_range(const std::string & name)66 gain_range_t get_range(const std::string& name)
67 {
68 if (not name.empty())
69 return _name_to_fcns.get(name).get_range();
70
71 double overall_min = 0, overall_max = 0, overall_step = 0;
72 for (const gain_fcns_t& fcns : get_all_fcns()) {
73 const gain_range_t range = fcns.get_range();
74 overall_min += range.start();
75 overall_max += range.stop();
76 // the overall step is the min (zero is invalid, first run)
77 if (overall_step == 0) {
78 overall_step = range.step();
79 } else if (range.step()) {
80 overall_step = std::min(overall_step, range.step());
81 }
82 }
83 return gain_range_t(overall_min, overall_max, overall_step);
84 }
85
get_value(const std::string & name)86 double get_value(const std::string& name)
87 {
88 if (not name.empty())
89 return _name_to_fcns.get(name).get_value();
90
91 double overall_gain = 0;
92 for (const gain_fcns_t& fcns : get_all_fcns()) {
93 overall_gain += fcns.get_value();
94 }
95 return overall_gain;
96 }
97
set_value(double gain,const std::string & name)98 void set_value(double gain, const std::string& name)
99 {
100 if (not name.empty())
101 return _name_to_fcns.get(name).set_value(gain);
102
103 std::vector<gain_fcns_t> all_fcns = get_all_fcns();
104 if (all_fcns.size() == 0)
105 return; // nothing to set!
106
107 // get the max step size among the gains
108 double max_step = 0;
109 for (const gain_fcns_t& fcns : all_fcns) {
110 max_step = std::max(max_step, fcns.get_range().step());
111 }
112
113 // create gain bucket to distribute power
114 std::vector<double> gain_bucket;
115
116 // distribute power according to priority (round to max step)
117 double gain_left_to_distribute = gain;
118 for (const gain_fcns_t& fcns : all_fcns) {
119 const gain_range_t range = fcns.get_range();
120 gain_bucket.push_back(floor_step(
121 uhd::clip(gain_left_to_distribute, range.start(), range.stop()),
122 max_step));
123 gain_left_to_distribute -= gain_bucket.back();
124 }
125
126 // get a list of indexes sorted by step size large to small
127 std::vector<size_t> indexes_step_size_dec;
128 for (size_t i = 0; i < all_fcns.size(); i++) {
129 indexes_step_size_dec.push_back(i);
130 }
131 std::sort(indexes_step_size_dec.begin(),
132 indexes_step_size_dec.end(),
133 std::bind(&compare_by_step_size,
134 std::placeholders::_1,
135 std::placeholders::_2,
136 all_fcns));
137 UHD_ASSERT_THROW(all_fcns.at(indexes_step_size_dec.front()).get_range().step()
138 >= all_fcns.at(indexes_step_size_dec.back()).get_range().step());
139
140 // distribute the remainder (less than max step)
141 // fill in the largest step sizes first that are less than the remainder
142 for (size_t i : indexes_step_size_dec) {
143 const gain_range_t range = all_fcns.at(i).get_range();
144 double additional_gain =
145 floor_step(uhd::clip(gain_bucket.at(i) + gain_left_to_distribute,
146 range.start(),
147 range.stop()),
148 range.step())
149 - gain_bucket.at(i);
150 gain_bucket.at(i) += additional_gain;
151 gain_left_to_distribute -= additional_gain;
152 }
153
154 // now write the bucket out to the individual gain values
155 for (size_t i = 0; i < gain_bucket.size(); i++) {
156 all_fcns.at(i).set_value(gain_bucket.at(i));
157 }
158 }
159
get_names(void)160 const std::vector<std::string> get_names(void)
161 {
162 return _name_to_fcns.keys();
163 }
164
register_fcns(const std::string & name,const gain_fcns_t & gain_fcns,size_t priority)165 void register_fcns(
166 const std::string& name, const gain_fcns_t& gain_fcns, size_t priority)
167 {
168 if (name.empty() or _name_to_fcns.has_key(name)) {
169 // ensure the name name is unique and non-empty
170 return register_fcns(name + "_", gain_fcns, priority);
171 }
172 _registry[priority].push_back(gain_fcns);
173 _name_to_fcns[name] = gain_fcns;
174 }
175
176 private:
177 //! get the gain function sets in order (highest priority first)
get_all_fcns(void)178 std::vector<gain_fcns_t> get_all_fcns(void)
179 {
180 std::vector<gain_fcns_t> all_fcns;
181 for (size_t key : uhd::sorted(_registry.keys())) {
182 const std::vector<gain_fcns_t>& fcns = _registry[key];
183 all_fcns.insert(all_fcns.begin(), fcns.begin(), fcns.end());
184 }
185 return all_fcns;
186 }
187
188 uhd::dict<size_t, std::vector<gain_fcns_t>> _registry;
189 uhd::dict<std::string, gain_fcns_t> _name_to_fcns;
190 };
191
192 /***********************************************************************
193 * gain group factory function
194 **********************************************************************/
make(void)195 gain_group::sptr gain_group::make(void)
196 {
197 return sptr(new gain_group_impl());
198 }
199
make_zero()200 gain_group::sptr gain_group::make_zero()
201 {
202 gain_fcns_t gain_fcns;
203 gain_fcns.get_range = []() { return meta_range_t(0.0, 0.0); };
204 gain_fcns.get_value = []() { return 0.0; };
205 gain_fcns.set_value = [](const double) {};
206 auto gg = make();
207 gg->register_fcns("null", gain_fcns);
208 return gg;
209 }
210