+static float get_group_max_sampling_rate (int s)
+{
+ /* Review the sampling rates of linked sensors and return the maximum */
+
+ int i, vi;
+
+ float arbitrated_rate = 0;
+
+ if (is_enabled(s))
+ arbitrated_rate = sensor[s].requested_rate;
+
+ /* If any of the currently active sensors built on top of this one need a higher sampling rate, switch to this rate */
+ for (i = 0; i < sensor_count; i++)
+ for (vi = 0; vi < sensor[i].base_count; vi++)
+ if (sensor[i].base[vi] == s && is_enabled(i) && sensor[i].requested_rate > arbitrated_rate) /* If sensor i depends on sensor s */
+ arbitrated_rate = sensor[i].requested_rate;
+
+ /* If any of the currently active sensors we rely on is using a higher sampling rate, switch to this rate */
+ for (vi = 0; vi < sensor[s].base_count; vi++) {
+ i = sensor[s].base[vi];
+ if (is_enabled(i) && sensor[i].requested_rate > arbitrated_rate)
+ arbitrated_rate = sensor[i].requested_rate;
+ }
+
+ return arbitrated_rate;
+}
+
+extern float sensor_get_max_freq (int s);
+
+static float select_closest_available_rate(int s, float requested_rate)
+{
+ float sr;
+ int j;
+ float selected_rate = 0;
+ float max_rate_from_prop = sensor_get_max_freq(s);
+ int dev_num = sensor[s].dev_num;
+
+ if (!sensor[s].avail_freqs_count)
+ return requested_rate;
+
+ for (j = 0; j < sensor[s].avail_freqs_count; j++) {
+
+ sr = sensor[s].avail_freqs[j];
+
+ /* If this matches the selected rate, we're happy. Have some tolerance for rounding errors and avoid needless jumps to higher rates */
+ if ((fabs(requested_rate - sr) <= 0.01) && (sr <= max_rate_from_prop)) {
+ return sr;
+ }
+
+ /* Select rate if it's less than max freq */
+ if ((sr > selected_rate) && (sr <= max_rate_from_prop)) {
+ selected_rate = sr;
+ }
+
+ /*
+ * If we reached a higher value than the desired rate, adjust selected rate so it matches the first higher available one and
+ * stop parsing - this makes the assumption that rates are sorted by increasing value in the allowed frequencies string.
+ */
+ if (sr > requested_rate) {
+ return selected_rate;
+ }
+ }
+
+ /* Check for wrong values */
+ if (selected_rate < 0.1) {
+ return requested_rate;
+ } else {
+ return selected_rate;
+ }
+}
+