14329424534

Committed 31 Mar 2025 01:50PM UTC coverage: 56.638% (+0.2%) from 56.477%

Build # 14329424534

Build Type

push

github

Committed by

laurensvalk

Commit Message

pbio/os: Move IRQ hooks to platform.

Proceed to make pbio more independent from MicroPython.

Also restore them as static inline as they used to be
prior to https://github.com/pybricks/pybricks-micropython/pull/298
which reduces build size.

Run Details

0 of 6 new or added lines in 1 file covered. (0.0%)

204 existing lines in 11 files now uncovered.

3878 of 6847 relevant lines covered (56.64%)

20623019.84 hits per line

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

95.24

/lib/pbio/src/integrator.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 <stdbool.h>
#include <stdint.h>
#include <stdlib.h>

#include <pbio/control_settings.h>
#include <pbio/integrator.h>
#include <pbio/int_math.h>

/**
 * Pauses the speed integrator at the current position error.
 *
 * @param [in]    itg              Speed integrator instance.
 * @param [in]    time_now         The wall time (ticks).
 * @param [in]    position_error   Current position error (control units).
 */
void pbio_speed_integrator_pause(pbio_speed_integrator_t *itg, uint32_t time_now, int32_t position_error) {

    // Pause only if running
    if (!itg->running) {
        return;
    }

    // The integrator is not running anymore
    itg->running = false;

    // Increment the paused integrator state with the integrated amount between the last resume and the newly enforced pause
    itg->speed_err_integral_paused += position_error - itg->position_error_resumed;

    // Store time at which we started pausing, used only for stall flag hysteresis.
    itg->time_pause_begin = time_now;
}

/**
 * Resumes the speed integrator from the current position error.
 *
 * @param [in]    itg              Speed integrator instance.
 * @param [in]    position_error   Current position error (control units).
 */
void pbio_speed_integrator_resume(pbio_speed_integrator_t *itg, int32_t position_error) {

    // Resume only if paused
    if (itg->running) {
        return;
    }

    // The integrator is running again
    itg->running = true;

    // Set starting point from which we resume, if needed
    // Begin integrating again from the current point
    itg->position_error_resumed = position_error;
}

/**
 * Resets the speed integrator state.
 *
 * @param [in]    itg       Speed integrator instance.
 * @param [in]    settings  Control settings instance from which to read stall settings.
 */
void pbio_speed_integrator_reset(pbio_speed_integrator_t *itg, pbio_control_settings_t *settings) {

    // Save reference to settings.
    itg->settings = settings;

    // Reset built up integral to 0.
    itg->speed_err_integral_paused = 0;

    // Set state to paused. It will resume immediately on start.
    itg->running = false;

    // Resume integration
    pbio_speed_integrator_resume(itg, 0);
}

/**
 * Gets the speed error integral accumulated thus far.
 *
 * @param [in]    itg              Speed integrator instance.
 * @param [in]    position_error   Current position error (control units).
 * @return                         Speed error integral (position control units).
 */
int32_t pbio_speed_integrator_get_error(const pbio_speed_integrator_t *itg, int32_t position_error) {

    // The speed error integral is at least the value at which we paused it last
    int32_t speed_err_integral = itg->speed_err_integral_paused;

    // If integrator is active, add the exact integral since its last restart
    if (itg->running) {
        speed_err_integral += position_error - itg->position_error_resumed;
    }
    return speed_err_integral;
}

/**
 * Checks if the speed integrator state indicates stalling.
 *
 * @param [in]    itg              Speed integrator instance.
 * @param [in]    time_now         The wall time (ticks).
 * @param [in]    speed_now        Current speed (control units).
 * @param [in]    speed_ref        Reference speed (control units).
 * @return                         True if stalled, false if not.
 */
bool pbio_speed_integrator_stalled(const pbio_speed_integrator_t *itg, uint32_t time_now, int32_t speed_now, int32_t speed_ref) {
    // If were running, we're not stalled
    if (itg->running) {
        return false;
    }

    // Equivalent to checking both directions, flip to positive for simplicity.
    if (speed_ref < 0) {
        speed_ref *= -1;
        speed_now *= -1;
    }

    // If we're still running faster than the stall limit, we're certainly not stalled.
    if (speed_ref != 0 && speed_now > itg->settings->stall_speed_limit) {
        return false;
    }

    // If the integrator is paused for less than the stall time, we're still not stalled for now.
    if (!pbio_control_settings_time_is_later(time_now, itg->time_pause_begin + itg->settings->stall_time)) {
        return false;
    }

    // All checks have failed, so we are stalled
    return true;
}

/**
 * Gets reference time compensated for stall duration of position controller.
 *
 * @param [in]    itg              Position integrator instance.
 * @param [in]    time_now         The wall time (ticks).
 * @return                         Wall time compensated for time spent stalling.
 */
uint32_t pbio_position_integrator_get_ref_time(const pbio_position_integrator_t *itg, uint32_t time_now) {
    // The wall time at which we are is either the current time, or whenever we stopped last.
    uint32_t real_time = itg->trajectory_running ? time_now : itg->time_pause_begin;

    // But we want to evaluate the reference compensating for the time we spent waiting.
    return real_time - itg->time_paused_total;
}

/**
 * Pauses the position integrator at the current time.
 *
 * @param [in]    itg              Speed integrator instance.
 * @param [in]    time_now         The wall time (ticks).
 */
void pbio_position_integrator_pause(pbio_position_integrator_t *itg, uint32_t time_now) {

    // Return if already paused.
    if (!itg->trajectory_running) {
        return;
    }

    // Disable the integrator.
    itg->trajectory_running = false;
    itg->time_pause_begin = time_now;
}

/**
 * Tests if the position integrator is paused.
 *
 * @param [in]    itg              Speed integrator instance.
 * @return                         True if integration is paused, false if not.
 */
bool pbio_position_integrator_is_paused(const pbio_position_integrator_t *itg) {
    return !itg->trajectory_running;
}

/**
 * Resumes the position integrator at the current time.
 *
 * @param [in]    itg              Speed integrator instance.
 * @param [in]    time_now         The wall time (ticks).
 */
void pbio_position_integrator_resume(pbio_position_integrator_t *itg, uint32_t time_now) {

    // Return if already trajectory_running
    if (itg->trajectory_running) {
        return;
    }

    // Then we must restart the time
    itg->trajectory_running = true;

    // Increment total wait time by time elapsed since we started pausing
    itg->time_paused_total += time_now - itg->time_pause_begin;
}

/**
 * Resets the position integrator state.
 *
 * @param [in]    itg       Speed integrator instance.
 * @param [in]    settings  Control settings instance from which to read stall settings.
 * @param [in]    time_now  The wall time (ticks).
 */
void pbio_position_integrator_reset(pbio_position_integrator_t *itg, pbio_control_settings_t *settings, uint32_t time_now) {

    // Save reference to settings.
    itg->settings = settings;

    // Reset integrator state variables
    itg->count_err_integral = 0;
    itg->time_paused_total = 0;
    itg->time_pause_begin = time_now;
    itg->trajectory_running = false;

    // Resume integration
    pbio_position_integrator_resume(itg, time_now);

}

/**
 * Updates the position integrator state with the latest error.
 *
 * @param [in]    itg              Speed integrator instance.
 * @param [in]    position_error   Current position error (position control units).
 * @param [in]    target_error     Remaining error to the endpoint (position control units).
 * @return                         Integrator state value (position control units).
 */
int32_t pbio_position_integrator_update(pbio_position_integrator_t *itg, int32_t position_error, int32_t target_error) {

    int32_t error_now = position_error;

    // Check if integrator magnitude would decrease due to this error
    bool decrease = pbio_int_math_abs(itg->count_err_integral + pbio_control_settings_mul_by_loop_time(error_now)) < pbio_int_math_abs(itg->count_err_integral);

    // Integrate and update position error
    if (itg->trajectory_running || decrease) {

        // If not deceasing, so growing, limit error growth by maximum integral rate
        if (!decrease) {
            error_now = error_now > itg->settings->integral_change_max ? itg->settings->integral_change_max : error_now;
            error_now = error_now < -itg->settings->integral_change_max ? -itg->settings->integral_change_max : error_now;

            // It might be decreasing now after all (due to integral sign change), so re-evaluate
            decrease = pbio_int_math_abs(itg->count_err_integral + pbio_control_settings_mul_by_loop_time(error_now)) < pbio_int_math_abs(itg->count_err_integral);
        }

        // Specify in which region integral control should be active. This is
        // at least the error that would still lead to maximum  proportional
        // control, with a factor of 2 so we begin integrating a bit sooner.
        int32_t integral_range_upper = pbio_control_settings_div_by_gain(itg->settings->actuation_max, itg->settings->pid_kp) * 2;

        // Add change if we are near (but not too near) target, or always if it decreases the integral magnitude.
        if ((pbio_int_math_abs(target_error) >= itg->settings->integral_deadzone &&
             pbio_int_math_abs(target_error) <= integral_range_upper) || decrease) {
            itg->count_err_integral += pbio_control_settings_mul_by_loop_time(error_now);
        }

        // Limit integral to value that leads to maximum actuation, i.e. max actuation / ki.
        itg->count_err_integral = pbio_int_math_clamp(itg->count_err_integral,
            pbio_control_settings_div_by_gain(itg->settings->actuation_max, itg->settings->pid_ki));
    }

    // Return current value.
    return itg->count_err_integral;
}

/**
 * Checks if the position integrator state indicates stalling.
 *
 * @param [in]    itg              Speed integrator instance.
 * @param [in]    time_now         The wall time (ticks).
 * @param [in]    speed_now        Current speed (control units).
 * @param [in]    speed_ref        Reference speed (control units).
 * @return                         True if stalled, false if not.
 */
bool pbio_position_integrator_stalled(const pbio_position_integrator_t *itg, uint32_t time_now, int32_t speed_now, int32_t speed_ref) {

    // Get integral value that would lead to maximum actuation.
    int32_t integral_max = pbio_control_settings_div_by_gain(itg->settings->actuation_max, itg->settings->pid_ki);

    // Whether the integrator is saturated.
    bool saturated = integral_max != 0 && pbio_int_math_abs(itg->count_err_integral) >= integral_max;

    // If we're running and the integrator is not saturated, we're not stalled.
    if (itg->trajectory_running && !saturated) {
        return false;
    }

    // Equivalent to checking both directions, flip to positive for simplicity.
    if (speed_ref < 0) {
        speed_ref *= -1;
        speed_now *= -1;
    }

    // If we're still running faster than the stall limit, we're certainly not stalled.
    if (speed_ref != 0 && speed_now > itg->settings->stall_speed_limit) {
        return false;
    }

    // If the integrator is paused for less than the stall time, we're still not stalled for now.
    if (!pbio_control_settings_time_is_later(time_now, itg->time_pause_begin + itg->settings->stall_time)) {
        return false;
    }

    // All checks have failed, so we are stalled
    return true;
};

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 <stdbool.h>
8	#include <stdint.h>
9	#include <stdlib.h>
10
11	#include <pbio/control_settings.h>
12	#include <pbio/integrator.h>
13	#include <pbio/int_math.h>
14
15	/**
16	* Pauses the speed integrator at the current position error.
17	*
18	* @param [in] itg Speed integrator instance.
19	* @param [in] time_now The wall time (ticks).
20	* @param [in] position_error Current position error (control units).
21	*/
22	void pbio_speed_integrator_pause(pbio_speed_integrator_t *itg, uint32_t time_now, int32_t position_error) {	663✔
23
24	// Pause only if running
25	if (!itg->running) {	663✔
26	return;	495✔
27	}
28
29	// The integrator is not running anymore
30	itg->running = false;	8✔
31
32	// Increment the paused integrator state with the integrated amount between the last resume and the newly enforced pause
33	itg->speed_err_integral_paused += position_error - itg->position_error_resumed;	8✔
34
35	// Store time at which we started pausing, used only for stall flag hysteresis.
36	itg->time_pause_begin = time_now;	8✔
37	}
38
39	/**
40	* Resumes the speed integrator from the current position error.
41	*
42	* @param [in] itg Speed integrator instance.
43	* @param [in] position_error Current position error (control units).
44	*/
45	void pbio_speed_integrator_resume(pbio_speed_integrator_t *itg, int32_t position_error) {	3,525✔
46
47	// Resume only if paused
48	if (itg->running) {	3,525✔
49	return;	1,299✔
50	}
51
52	// The integrator is running again
53	itg->running = true;	25✔
54
55	// Set starting point from which we resume, if needed
56	// Begin integrating again from the current point
57	itg->position_error_resumed = position_error;	25✔
58	}
59
60	/**
61	* Resets the speed integrator state.
62	*
63	* @param [in] itg Speed integrator instance.
64	* @param [in] settings Control settings instance from which to read stall settings.
65	*/
66	void pbio_speed_integrator_reset(pbio_speed_integrator_t itg, pbio_control_settings_t settings) {	22✔
67
68	// Save reference to settings.
69	itg->settings = settings;	22✔
70
71	// Reset built up integral to 0.
72	itg->speed_err_integral_paused = 0;	22✔
73
74	// Set state to paused. It will resume immediately on start.
75	itg->running = false;	22✔
76
77	// Resume integration
78	pbio_speed_integrator_resume(itg, 0);	22✔
79	}	22✔
80
81	/**
82	* Gets the speed error integral accumulated thus far.
83	*
84	* @param [in] itg Speed integrator instance.
85	* @param [in] position_error Current position error (control units).
86	* @return Speed error integral (position control units).
87	*/
88	int32_t pbio_speed_integrator_get_error(const pbio_speed_integrator_t *itg, int32_t position_error) {	4,170✔
89
90	// The speed error integral is at least the value at which we paused it last
91	int32_t speed_err_integral = itg->speed_err_integral_paused;	4,170✔
92
93	// If integrator is active, add the exact integral since its last restart
94	if (itg->running) {	4,170✔
95	speed_err_integral += position_error - itg->position_error_resumed;	3,512✔
96	}
97	return speed_err_integral;	4,170✔
98	}
99
100	/**
101	* Checks if the speed integrator state indicates stalling.
102	*
103	* @param [in] itg Speed integrator instance.
104	* @param [in] time_now The wall time (ticks).
105	* @param [in] speed_now Current speed (control units).
106	* @param [in] speed_ref Reference speed (control units).
107	* @return True if stalled, false if not.
108	*/
109	bool pbio_speed_integrator_stalled(const pbio_speed_integrator_t *itg, uint32_t time_now, int32_t speed_now, int32_t speed_ref) {	4,166✔
110	// If were running, we're not stalled
111	if (itg->running) {	4,166✔
112	return false;	1,302✔
113	}
114
115	// Equivalent to checking both directions, flip to positive for simplicity.
116	if (speed_ref < 0) {	663✔
117	speed_ref *= -1;	396✔
118	speed_now *= -1;	396✔
119	}
120
121	// If we're still running faster than the stall limit, we're certainly not stalled.
122	if (speed_ref != 0 && speed_now > itg->settings->stall_speed_limit) {	663✔
123	return false;	40✔
124	}
125
126	// If the integrator is paused for less than the stall time, we're still not stalled for now.
127	if (!pbio_control_settings_time_is_later(time_now, itg->time_pause_begin + itg->settings->stall_time)) {	584✔
128	return false;	170✔
129	}
130
131	// All checks have failed, so we are stalled
132	return true;	410✔
133	}
134
135	/**
136	* Gets reference time compensated for stall duration of position controller.
137	*
138	* @param [in] itg Position integrator instance.
139	* @param [in] time_now The wall time (ticks).
140	* @return Wall time compensated for time spent stalling.
141	*/
142	uint32_t pbio_position_integrator_get_ref_time(const pbio_position_integrator_t *itg, uint32_t time_now) {	25,670✔
143	// The wall time at which we are is either the current time, or whenever we stopped last.
144	uint32_t real_time = itg->trajectory_running ? time_now : itg->time_pause_begin;	25,670✔
145
146	// But we want to evaluate the reference compensating for the time we spent waiting.
147	return real_time - itg->time_paused_total;	25,670✔
148	}
149
150	/**
151	* Pauses the position integrator at the current time.
152	*
153	* @param [in] itg Speed integrator instance.
154	* @param [in] time_now The wall time (ticks).
155	*/
156	void pbio_position_integrator_pause(pbio_position_integrator_t *itg, uint32_t time_now) {	494✔
157
158	// Return if already paused.
159	if (!itg->trajectory_running) {	494✔
160	return;	461✔
161	}
162
163	// Disable the integrator.
164	itg->trajectory_running = false;	15✔
165	itg->time_pause_begin = time_now;	15✔
166	}
167
168	/**
169	* Tests if the position integrator is paused.
170	*
171	* @param [in] itg Speed integrator instance.
172	* @return True if integration is paused, false if not.
173	*/
174	bool pbio_position_integrator_is_paused(const pbio_position_integrator_t *itg) {	×
175	return !itg->trajectory_running;	×
176	}
177
178	/**
179	* Resumes the position integrator at the current time.
180	*
181	* @param [in] itg Speed integrator instance.
182	* @param [in] time_now The wall time (ticks).
183	*/
184	void pbio_position_integrator_resume(pbio_position_integrator_t *itg, uint32_t time_now) {	25,062✔
185
186	// Return if already trajectory_running
187	if (itg->trajectory_running) {	25,062✔
188	return;	7,445✔
189	}
190
191	// Then we must restart the time
192	itg->trajectory_running = true;	41✔
193
194	// Increment total wait time by time elapsed since we started pausing
195	itg->time_paused_total += time_now - itg->time_pause_begin;	41✔
196	}
197
198	/**
199	* Resets the position integrator state.
200	*
201	* @param [in] itg Speed integrator instance.
202	* @param [in] settings Control settings instance from which to read stall settings.
203	* @param [in] time_now The wall time (ticks).
204	*/
205	void pbio_position_integrator_reset(pbio_position_integrator_t itg, pbio_control_settings_t settings, uint32_t time_now) {	29✔
206
207	// Save reference to settings.
208	itg->settings = settings;	29✔
209
210	// Reset integrator state variables
211	itg->count_err_integral = 0;	29✔
212	itg->time_paused_total = 0;	29✔
213	itg->time_pause_begin = time_now;	29✔
214	itg->trajectory_running = false;	29✔
215
216	// Resume integration
217	pbio_position_integrator_resume(itg, time_now);	29✔
218
219	}	29✔
220
221	/**
222	* Updates the position integrator state with the latest error.
223	*
224	* @param [in] itg Speed integrator instance.
225	* @param [in] position_error Current position error (position control units).
226	* @param [in] target_error Remaining error to the endpoint (position control units).
227	* @return Integrator state value (position control units).
228	*/
229	int32_t pbio_position_integrator_update(pbio_position_integrator_t *itg, int32_t position_error, int32_t target_error) {	25,527✔
230
231	int32_t error_now = position_error;	25,527✔
232
233	// Check if integrator magnitude would decrease due to this error
234	bool decrease = pbio_int_math_abs(itg->count_err_integral + pbio_control_settings_mul_by_loop_time(error_now)) < pbio_int_math_abs(itg->count_err_integral);	25,527✔
235
236	// Integrate and update position error
237	if (itg->trajectory_running \|\| decrease) {	25,527✔
238
239	// If not deceasing, so growing, limit error growth by maximum integral rate
240	if (!decrease) {	25,036✔
241	error_now = error_now > itg->settings->integral_change_max ? itg->settings->integral_change_max : error_now;	24,149✔
242	error_now = error_now < -itg->settings->integral_change_max ? -itg->settings->integral_change_max : error_now;	24,149✔
243
244	// It might be decreasing now after all (due to integral sign change), so re-evaluate
245	decrease = pbio_int_math_abs(itg->count_err_integral + pbio_control_settings_mul_by_loop_time(error_now)) < pbio_int_math_abs(itg->count_err_integral);	24,149✔
246	}
247
248	// Specify in which region integral control should be active. This is
249	// at least the error that would still lead to maximum proportional
250	// control, with a factor of 2 so we begin integrating a bit sooner.
251	int32_t integral_range_upper = pbio_control_settings_div_by_gain(itg->settings->actuation_max, itg->settings->pid_kp) * 2;	25,036✔
252
253	// Add change if we are near (but not too near) target, or always if it decreases the integral magnitude.
254	if ((pbio_int_math_abs(target_error) >= itg->settings->integral_deadzone &&	36,343✔
255	pbio_int_math_abs(target_error) <= integral_range_upper) \|\| decrease) {	35,884✔
256	itg->count_err_integral += pbio_control_settings_mul_by_loop_time(error_now);	1,348✔
257	}
258
259	// Limit integral to value that leads to maximum actuation, i.e. max actuation / ki.
260	itg->count_err_integral = pbio_int_math_clamp(itg->count_err_integral,	25,036✔
261	pbio_control_settings_div_by_gain(itg->settings->actuation_max, itg->settings->pid_ki));	25,036✔
262	}
263
264	// Return current value.
265	return itg->count_err_integral;	25,527✔
266	}
267
268	/**
269	* Checks if the position integrator state indicates stalling.
270	*
271	* @param [in] itg Speed integrator instance.
272	* @param [in] time_now The wall time (ticks).
273	* @param [in] speed_now Current speed (control units).
274	* @param [in] speed_ref Reference speed (control units).
275	* @return True if stalled, false if not.
276	*/
277	bool pbio_position_integrator_stalled(const pbio_position_integrator_t *itg, uint32_t time_now, int32_t speed_now, int32_t speed_ref) {	25,527✔
278
279	// Get integral value that would lead to maximum actuation.
280	int32_t integral_max = pbio_control_settings_div_by_gain(itg->settings->actuation_max, itg->settings->pid_ki);	25,527✔
281
282	// Whether the integrator is saturated.
283	bool saturated = integral_max != 0 && pbio_int_math_abs(itg->count_err_integral) >= integral_max;	25,527✔
284
285	// If we're running and the integrator is not saturated, we're not stalled.
286	if (itg->trajectory_running && !saturated) {	25,527✔
287	return false;	7,449✔
288	}
289
290	// Equivalent to checking both directions, flip to positive for simplicity.
291	if (speed_ref < 0) {	494✔
UNCOV 292	speed_ref *= -1;	×
UNCOV 293	speed_now *= -1;	×
294	}
295
296	// If we're still running faster than the stall limit, we're certainly not stalled.
297	if (speed_ref != 0 && speed_now > itg->settings->stall_speed_limit) {	494✔
298	return false;	46✔
299	}
300
301	// If the integrator is paused for less than the stall time, we're still not stalled for now.
302	if (!pbio_control_settings_time_is_later(time_now, itg->time_pause_begin + itg->settings->stall_time)) {	444✔
303	return false;	172✔
304	}
305
306	// All checks have failed, so we are stalled
307	return true;	272✔
308	};

pybricks / pybricks-micropython / 14329424534

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