6675885095

Committed 28 Oct 2023 08:38AM UTC coverage: 56.053% (+10.0%) from 46.074%

Build # 6675885095

Build Type

push

github

Committed by

laurensvalk

Commit Message

pybricks.hubs.MoveHub: Use standard hub init.

The Move Hub cannot have true vector axes, but we can still use this API to initialize the hub using numeric indices.

This ensures we can use the custom orientation not just in tilt, but also in acceleration like we do on other hubs.

Run Details

3616 of 6451 relevant lines covered (56.05%)

20895680.75 hits per line

Source File
Press 'n' to go to next uncovered line, 'b' for previous

98.84

/lib/pbio/src/int_math.c

// SPDX-License-Identifier: MIT
// Copyright (c) 2018-2023 The Pybricks Authors

// SPDX-License-Identifier: BSD-3-Clause
// Copyright (c) 2020-2023 LEGO System A/S

#include <assert.h>
#include <inttypes.h>
#include <stdbool.h>

#include <pbio/int_math.h>
#include <pbio/util.h>

/**
 * Gets the absolute value.
 *
 * @param [in]  value   The value.
 * @return              The absolute (positive) value.
 */
int32_t pbio_int_math_abs(int32_t value) {
    return __builtin_abs(value);
}

/**
 * Gets the maximum of two values.
 *
 * @param [in]  a       Value.
 * @param [in]  b       Value.
 * @return              a if it is greater than b, else b.
 */
int32_t pbio_int_math_max(int32_t a, int32_t b) {
    if (a > b) {
        return a;
    }
    return b;
}

/**
 * Gets the minimum of two values.
 *
 * @param [in]  a       Value.
 * @param [in]  b       Value.
 * @return              a if it is less than b, else b.
 */
int32_t pbio_int_math_min(int32_t a, int32_t b) {
    if (a < b) {
        return a;
    }
    return b;
}

/**
 * Get the sign of @p a.
 *
 * @param [in]  a   A signed integer value.
 * @return          1 if @p a is positive, -1 if @p a is negative or 0 if @p a
 *                  is 0.
 */
int32_t pbio_int_math_sign(int32_t a) {
    if (a == 0) {
        return 0;
    }
    return a > 0 ? 1 : -1;
}

/**
 * Checks that the signs of @p a and @p b are not opposite.
 *
 * @param [in]  a   A signed integer value.
 * @param [in]  b   A signed integer value.
 * @return          True if either value is zero or if the signs are the same,
 *                  else false.
 */
bool pbio_int_math_sign_not_opposite(int32_t a, int32_t b) {
    if (a == 0 || b == 0) {
        return true;
    }
    return (a > 0) == (b > 0);
}

/**
 * Binds a value between a lower and upper limit.
 *
 * If @p value is greater than @p max, then @p max is
 * returned. If @p value is less than @p min, then
 * @p min is returned. Otherwise @p value is returned.
 *
 * @param [in]  value   The value to bind.
 * @param [in]  min     The lower boundary value.
 * @param [in]  max     The upper boundary value.
 * @return              The bounded value.
 */
int32_t pbio_int_math_bind(int32_t value, int32_t min, int32_t max) {
    assert(max >= min);

    if (value > max) {
        return max;
    }
    if (value < min) {
        return min;
    }
    return value;
}

/**
 * Clamps a value to a +/- limit.
 *
 * If @p value is greater than positive @p abs_max, then positive @p abs_max is
 * returned. If @p value is less than negative @p abs_max, then negative
 * @p abs_max is returned. Otherwise @p value is returned.
 *
 * @param [in]  value   The value to clamp.
 * @param [in]  abs_max The clamp limit. This must be a positive value.
 * @return              The clamped value.
 */
int32_t pbio_int_math_clamp(int32_t value, int32_t abs_max) {
    assert(abs_max >= 0);

    return pbio_int_math_bind(value, -abs_max, abs_max);
}

/**
 * Gets the square root.
 *
 * If @p n <= 0, it returns 0.
 *
 * @param [in]  n       The input value
 * @return              The square root.
 */
int32_t pbio_int_math_sqrt(int32_t n) {
    if (n <= 0) {
        return 0;
    }
    int32_t x0 = n;
    int32_t x1;

    while (true) {
        x1 = (x0 + n / x0) / 2;
        if (x1 == x0 || x1 == x0 + 1) {
            return x0;
        }
        x0 = x1;
    }
}

/**
 * Points on a curve for linear interpolation.
 */
typedef struct {
    int16_t x;
    int16_t y;
} point_t;

/**
 * Sample points along the curve y = atan(x / 1024) * 8.
 *
 * This is used to get the atan2(b, a) curve in the first quadrant. The
 * intermediate scaling is used to avoid excessive rounding errors.
 *
 * x = Ratios b / a, upscaled by 1024.
 * y = Matching atan(b / b) output, upscaled by 8 * 180 / pi (eighth of a degree).
 */
static const point_t atan_points[] = {
    { .x = 0, .y = 0 },
    { .x = 409, .y = 178 },
    { .x = 972, .y = 348 },
    { .x = 1638, .y = 472 },
    { .x = 2560, .y = 545 },
    { .x = 3891, .y = 605 },
    { .x = 5120, .y = 632 },
    { .x = 7168, .y = 660 },
    { .x = 15360, .y = 692 },
    { .x = 25600, .y = 705 },
};

/**
 * Interpolates a constant set of (X, Y) sample points on a curve y = f(x)
 * to estimate y for given input value x.
 *
 * If x < x[first] then it returns y[first].
 * If x >= x[last] then it returns y[last].
 *
 * @param [in]   points  Data points between which to interpolate.
 * @param [in]   len     Number of data points.
 * @param [in]   x       Value for which to estimate y = f(x)
 * @return               Estimated value for y = f(x)
 */
static int32_t pbio_int_math_interpolate(const point_t *points, size_t len, int32_t x) {

    // If x is below the minimum x, return the minimum y.
    if (x < points[0].x) {
        return points[0].y;
    }

    // Find nearest match and interpolate.
    for (size_t i = 0; i < len - 1; i++) {
        const point_t *p0 = &points[i];
        const point_t *p1 = &points[i + 1];

        if (x < p1->x) {
            return p0->y + (x - p0->x) * (p1->y - p0->y) / (p1->x - p0->x);
        }
    }

    // If x is above the maximum x, return the maximum y.
    return points[len - 1].y;
}

/**
 * Gets atan2 in degrees from integer inputs.
 *
 * @param [in]  y  Opposite side of the triangle.
 * @param [in]  x  Adjacent side of the triangle.
 * @return         atan2(y, x) in degrees.
 */
int32_t pbio_int_math_atan2(int32_t y, int32_t x) {

    // On y zero, the triangle is flat. Use X to find sign.
    if (y == 0) {
        return x > 0 ? 0 : -180;
    }

    // On x zero, the triangle height tends to infinity. Use y for sign.
    if (x == 0) {
        return 90 * pbio_int_math_sign(y);
    }

    // Get absolute ratio of y / x, upscaled to preserve resolution.
    int32_t ratio = 1024 * y / x;
    if (ratio < 0) {
        ratio = -ratio;
    }

    // Interpolate and scale to get corresponding atan value.
    int32_t atan = pbio_int_math_interpolate(atan_points, PBIO_ARRAY_SIZE(atan_points), ratio) / 8;

    // We took the absolute ratio, but must now account for sign.
    // So, negate if x and y had opposite sign.
    if ((x > 0) != (y > 0)) {
        atan = -atan;
    }

    // For small angles, we're done.
    if (x > 0) {
        return atan;
    }

    // Get atan2 result for larger angles.
    return atan > 0 ? atan - 180 : atan + 180;
}

/**
 * Multiplies two numbers and scales down the result.
 *
 * The result is @p a * @p b / @p c .
 *
 * The product of @p a and @p b must not exceed 2**47, and the result after
 * division must not exceed 2**31. @p c must not exceed 2**16.
 *
 * Adapted from https://stackoverflow.com/a/57727180, CC BY-SA 4.0
 *
 * @param [in]  a    Positive or negative number.
 * @param [in]  b    Positive or negative number.
 * @param [in]  c    Small positive or negative number.
 * @return           The result of a * b / c.
 */
int32_t pbio_int_math_mult_then_div(int32_t a, int32_t b, int32_t c) {

    // Get long product.
    uint64_t x = (uint64_t)(a < 0 ? -a : a) * (uint64_t)(b < 0 ? -b : b);

    assert(x < ((int64_t)1) << 47);
    assert(c != 0);

    uint32_t div = (c < 0 ? -c : c);

    // Set d1 to the high two base-65536 digits (bits 17 to 31) and d0 to
    // the low digit (bits 0 to 15).
    uint32_t d1 = (uint32_t)(x >> 16);
    uint32_t d0 = x & 0xffffu;

    //  Get the quotient and remainder of dividing d1 by div.
    uint32_t y1 = d1 / div;
    uint32_t r1 = d1 % div;

    // Combine previous remainder with the low digit of dividend and divide.
    uint32_t y0 = (r1 << 16 | d0) / div;

    // Return a quotient formed from the two quotient digits, signed by inputs.
    int32_t sign = (a < 0 ? -1 : 1) * (b < 0 ? -1 : 1) * (c < 0 ? -1 : 1);
    int32_t result = ((int32_t)(y1 << 16 | y0)) * sign;

    // Assert result is correct and return.
    assert(result == (int64_t)a * (int64_t)b / (int64_t)c);
    return result;
}

/**
 * Approximates first 90-degree segment of a sine in degrees, output
 * upscaled by 10000.
 *
 * @param [in]  x        Angle in degrees (0-90).
 * @returns              Approximately sin(x) * 10000.
 */
static int32_t pbio_int_math_sin_deg_branch0(int32_t x) {
    return (201 - x) * x;
}

// integer sine approximation from degrees to (-10000, 10000)

/**
 * Approximates sine of an angle in degrees, output upscaled by 10000.
 *
 * @param [in]  x        Angle in degrees.
 * @returns              Approximately sin(x) * 10000.
 */
int32_t pbio_int_math_sin_deg(int32_t x) {
    x = x % 360;
    if (x < 90) {
        return pbio_int_math_sin_deg_branch0(x);
    }
    if (x < 180) {
        return pbio_int_math_sin_deg_branch0(180 - x);
    }
    if (x < 270) {
        return -pbio_int_math_sin_deg_branch0(x - 180);
    }
    return -pbio_int_math_sin_deg_branch0(360 - x);
}

/**
 * Approximates cosine of an angle in degrees, output upscaled by 10000.
 *
 * @param [in]  x        Angle in degrees.
 * @returns              Approximately cos(x) * 10000.
 */
int32_t pbio_int_math_cos_deg(int32_t x) {
    return pbio_int_math_sin_deg(x + 90);
}

1	// SPDX-License-Identifier: MIT
2	// Copyright (c) 2018-2023 The Pybricks Authors
3
4	// SPDX-License-Identifier: BSD-3-Clause
5	// Copyright (c) 2020-2023 LEGO System A/S
6
7	#include <assert.h>
8	#include <inttypes.h>
9	#include <stdbool.h>
10
11	#include <pbio/int_math.h>
12	#include <pbio/util.h>
13
14	/**
15	* Gets the absolute value.
16	*
17	* @param [in] value The value.
18	* @return The absolute (positive) value.
19	*/
20	int32_t pbio_int_math_abs(int32_t value) {	2,147,576,818✔
21	return __builtin_abs(value);	2,147,576,818✔
22	}
23
24	/**
25	* Gets the maximum of two values.
26	*
27	* @param [in] a Value.
28	* @param [in] b Value.
29	* @return a if it is greater than b, else b.
30	*/
31	int32_t pbio_int_math_max(int32_t a, int32_t b) {	197,686✔
32	if (a > b) {	197,686✔
33	return a;	75,604✔
34	}
35	return b;	122,064✔
36	}
37
38	/**
39	* Gets the minimum of two values.
40	*
41	* @param [in] a Value.
42	* @param [in] b Value.
43	* @return a if it is less than b, else b.
44	*/
45	int32_t pbio_int_math_min(int32_t a, int32_t b) {	433,455✔
46	if (a < b) {	433,455✔
47	return a;	227,466✔
48	}
49	return b;	205,946✔
50	}
51
52	/**
53	* Get the sign of @p a.
54	*
55	* @param [in] a A signed integer value.
56	* @return 1 if @p a is positive, -1 if @p a is negative or 0 if @p a
57	* is 0.
58	*/
59	int32_t pbio_int_math_sign(int32_t a) {	2,147,507,913✔
60	if (a == 0) {	2,147,507,913✔
61	return 0;	2,147,483,647✔
62	}
63	return a > 0 ? 1 : -1;	2,147,505,391✔
64	}
65
66	/**
67	* Checks that the signs of @p a and @p b are not opposite.
68	*
69	* @param [in] a A signed integer value.
70	* @param [in] b A signed integer value.
71	* @return True if either value is zero or if the signs are the same,
72	* else false.
73	*/
74	bool pbio_int_math_sign_not_opposite(int32_t a, int32_t b) {	2,147,483,647✔
75	if (a == 0 \|\| b == 0) {	2,147,483,647✔
76	return true;	2,061,672,672✔
77	}
78	return (a > 0) == (b > 0);	620,376,186✔
79	}
80
81	/**
82	* Binds a value between a lower and upper limit.
83	*
84	* If @p value is greater than @p max, then @p max is
85	* returned. If @p value is less than @p min, then
86	* @p min is returned. Otherwise @p value is returned.
87	*
88	* @param [in] value The value to bind.
89	* @param [in] min The lower boundary value.
90	* @param [in] max The upper boundary value.
91	* @return The bounded value.
92	*/
93	int32_t pbio_int_math_bind(int32_t value, int32_t min, int32_t max) {	1,236,179✔
94	assert(max >= min);	1,236,179✔
95
96	if (value > max) {	1,122,812✔
97	return max;	937✔
98	}
99	if (value < min) {	1,234,235✔
100	return min;	1,483✔
101	}
102	return value;	1,120,362✔
103	}
104
105	/**
106	* Clamps a value to a +/- limit.
107	*
108	* If @p value is greater than positive @p abs_max, then positive @p abs_max is
109	* returned. If @p value is less than negative @p abs_max, then negative
110	* @p abs_max is returned. Otherwise @p value is returned.
111	*
112	* @param [in] value The value to clamp.
113	* @param [in] abs_max The clamp limit. This must be a positive value.
114	* @return The clamped value.
115	*/
116	int32_t pbio_int_math_clamp(int32_t value, int32_t abs_max) {	840,839✔
117	assert(abs_max >= 0);	840,839✔
118
119	return pbio_int_math_bind(value, -abs_max, abs_max);	840,839✔
120	}
121
122	/**
123	* Gets the square root.
124	*
125	* If @p n <= 0, it returns 0.
126	*
127	* @param [in] n The input value
128	* @return The square root.
129	*/
130	int32_t pbio_int_math_sqrt(int32_t n) {	8,470,116✔
131	if (n <= 0) {	8,470,116✔
132	return 0;	2,010✔
133	}
134	int32_t x0 = n;	8,468,106✔
135	int32_t x1;
136
137	while (true) {
138	x1 = (x0 + n / x0) / 2;	159,005,167✔
139	if (x1 == x0 \|\| x1 == x0 + 1) {	159,005,167✔
140	return x0;	8,468,106✔
141	}
142	x0 = x1;	150,537,061✔
143	}
144	}
145
146	/**
147	* Points on a curve for linear interpolation.
148	*/
149	typedef struct {
150	int16_t x;
151	int16_t y;
152	} point_t;
153
154	/**
155	* Sample points along the curve y = atan(x / 1024) * 8.
156	*
157	* This is used to get the atan2(b, a) curve in the first quadrant. The
158	* intermediate scaling is used to avoid excessive rounding errors.
159	*
160	* x = Ratios b / a, upscaled by 1024.
161	* y = Matching atan(b / b) output, upscaled by 8 * 180 / pi (eighth of a degree).
162	*/
163	static const point_t atan_points[] = {
164	{ .x = 0, .y = 0 },
165	{ .x = 409, .y = 178 },
166	{ .x = 972, .y = 348 },
167	{ .x = 1638, .y = 472 },
168	{ .x = 2560, .y = 545 },
169	{ .x = 3891, .y = 605 },
170	{ .x = 5120, .y = 632 },
171	{ .x = 7168, .y = 660 },
172	{ .x = 15360, .y = 692 },
173	{ .x = 25600, .y = 705 },
174	};
175
176	/**
177	* Interpolates a constant set of (X, Y) sample points on a curve y = f(x)
178	* to estimate y for given input value x.
179	*
180	* If x < x[first] then it returns y[first].
181	* If x >= x[last] then it returns y[last].
182	*
183	* @param [in] points Data points between which to interpolate.
184	* @param [in] len Number of data points.
185	* @param [in] x Value for which to estimate y = f(x)
186	* @return Estimated value for y = f(x)
187	*/
188	static int32_t pbio_int_math_interpolate(const point_t *points, size_t len, int32_t x) {	4,351,645✔
189
190	// If x is below the minimum x, return the minimum y.
191	if (x < points[0].x) {	4,351,645✔
192	return points[0].y;	×
193	}
194
195	// Find nearest match and interpolate.
196	for (size_t i = 0; i < len - 1; i++) {	13,932,711✔
197	const point_t *p0 = &points[i];	13,846,639✔
198	const point_t *p1 = &points[i + 1];	13,846,639✔
199
200	if (x < p1->x) {	13,846,639✔
201	return p0->y + (x - p0->x) * (p1->y - p0->y) / (p1->x - p0->x);	4,265,573✔
202	}
203	}
204
205	// If x is above the maximum x, return the maximum y.
206	return points[len - 1].y;	86,072✔
207	}
208
209	/**
210	* Gets atan2 in degrees from integer inputs.
211	*
212	* @param [in] y Opposite side of the triangle.
213	* @param [in] x Adjacent side of the triangle.
214	* @return atan2(y, x) in degrees.
215	*/
216	int32_t pbio_int_math_atan2(int32_t y, int32_t x) {	4,359,640✔
217
218	// On y zero, the triangle is flat. Use X to find sign.
219	if (y == 0) {	4,359,640✔
220	return x > 0 ? 0 : -180;	3,998✔
221	}
222
223	// On x zero, the triangle height tends to infinity. Use y for sign.
224	if (x == 0) {	4,355,642✔
225	return 90 * pbio_int_math_sign(y);	3,997✔
226	}
227
228	// Get absolute ratio of y / x, upscaled to preserve resolution.
229	int32_t ratio = 1024 * y / x;	4,351,645✔
230	if (ratio < 0) {	4,351,645✔
231	ratio = -ratio;	2,175,822✔
232	}
233
234	// Interpolate and scale to get corresponding atan value.
235	int32_t atan = pbio_int_math_interpolate(atan_points, PBIO_ARRAY_SIZE(atan_points), ratio) / 8;	4,351,645✔
236
237	// We took the absolute ratio, but must now account for sign.
238	// So, negate if x and y had opposite sign.
239	if ((x > 0) != (y > 0)) {	4,351,645✔
240	atan = -atan;	2,175,822✔
241	}
242
243	// For small angles, we're done.
244	if (x > 0) {	4,351,645✔
245	return atan;	2,174,823✔
246	}
247
248	// Get atan2 result for larger angles.
249	return atan > 0 ? atan - 180 : atan + 180;	2,176,822✔
250	}
251
252	/**
253	* Multiplies two numbers and scales down the result.
254	*
255	* The result is @p a * @p b / @p c .
256	*
257	* The product of @p a and @p b must not exceed 2**47, and the result after
258	* division must not exceed 231. @p c must not exceed 216.
259	*
260	* Adapted from https://stackoverflow.com/a/57727180, CC BY-SA 4.0
261	*
262	* @param [in] a Positive or negative number.
263	* @param [in] b Positive or negative number.
264	* @param [in] c Small positive or negative number.
265	* @return The result of a * b / c.
266	*/
267	int32_t pbio_int_math_mult_then_div(int32_t a, int32_t b, int32_t c) {	1,701,314,889✔
268
269	// Get long product.
270	uint64_t x = (uint64_t)(a < 0 ? -a : a) * (uint64_t)(b < 0 ? -b : b);	1,701,314,889✔
271
272	assert(x < ((int64_t)1) << 47);	1,701,314,889✔
273	assert(c != 0);	1,701,314,889✔
274
275	uint32_t div = (c < 0 ? -c : c);	1,701,314,889✔
276
277	// Set d1 to the high two base-65536 digits (bits 17 to 31) and d0 to
278	// the low digit (bits 0 to 15).
279	uint32_t d1 = (uint32_t)(x >> 16);	1,701,314,889✔
280	uint32_t d0 = x & 0xffffu;	1,701,314,889✔
281
282	// Get the quotient and remainder of dividing d1 by div.
283	uint32_t y1 = d1 / div;	1,701,314,889✔
284	uint32_t r1 = d1 % div;	1,701,314,889✔
285
286	// Combine previous remainder with the low digit of dividend and divide.
287	uint32_t y0 = (r1 << 16 \| d0) / div;	1,701,314,889✔
288
289	// Return a quotient formed from the two quotient digits, signed by inputs.
290	int32_t sign = (a < 0 ? -1 : 1) * (b < 0 ? -1 : 1) * (c < 0 ? -1 : 1);	1,701,422,671✔
291	int32_t result = ((int32_t)(y1 << 16 \| y0)) * sign;	1,701,314,889✔
292
293	// Assert result is correct and return.
294	assert(result == (int64_t)a * (int64_t)b / (int64_t)c);	1,701,314,889✔
295	return result;	1,701,314,889✔
296	}
297
298	/**
299	* Approximates first 90-degree segment of a sine in degrees, output
300	* upscaled by 10000.
301	*
302	* @param [in] x Angle in degrees (0-90).
303	* @returns Approximately sin(x) * 10000.
304	*/
305	static int32_t pbio_int_math_sin_deg_branch0(int32_t x) {	456✔
306	return (201 - x) * x;	456✔
307	}
308
309	// integer sine approximation from degrees to (-10000, 10000)
310
311	/**
312	* Approximates sine of an angle in degrees, output upscaled by 10000.
313	*
314	* @param [in] x Angle in degrees.
315	* @returns Approximately sin(x) * 10000.
316	*/
317	int32_t pbio_int_math_sin_deg(int32_t x) {	456✔
318	x = x % 360;	456✔
319	if (x < 90) {	456✔
320	return pbio_int_math_sin_deg_branch0(x);	45✔
321	}
322	if (x < 180) {	411✔
323	return pbio_int_math_sin_deg_branch0(180 - x);	44✔
324	}
325	if (x < 270) {	367✔
326	return -pbio_int_math_sin_deg_branch0(x - 180);	183✔
327	}
328	return -pbio_int_math_sin_deg_branch0(360 - x);	184✔
329	}
330
331	/**
332	* Approximates cosine of an angle in degrees, output upscaled by 10000.
333	*
334	* @param [in] x Angle in degrees.
335	* @returns Approximately cos(x) * 10000.
336	*/
337	int32_t pbio_int_math_cos_deg(int32_t x) {	228✔
338	return pbio_int_math_sin_deg(x + 90);	228✔
339	}

pybricks / pybricks-micropython / 6675885095

Source File Press 'n' to go to next uncovered line, 'b' for previous

Source File
Press 'n' to go to next uncovered line, 'b' for previous