pbio/tacho: Read absolute count during reset.
When resetting to the absolute value, it is helpful to know which value it was set to. This will be used by the servo in subsequent commits.
@@ -42,19 +42,18 @@ static pbio_error_t pbio_tacho_reset_count(pbio_tacho_t *tacho, int32_t reset_co
return PBIO_SUCCESS;
}
-static pbio_error_t pbio_tacho_reset_count_to_abs(pbio_tacho_t *tacho) {
+static pbio_error_t pbio_tacho_reset_count_to_abs(pbio_tacho_t *tacho, int32_t *abs_count) {
- int32_t abs_count;
- pbio_error_t err = pbdrv_counter_get_abs_count(tacho->counter, &abs_count);
+ pbio_error_t err = pbdrv_counter_get_abs_count(tacho->counter, abs_count);
if (err != PBIO_SUCCESS) {
return err;
}
if (tacho->direction == PBIO_DIRECTION_COUNTERCLOCKWISE) {
- abs_count = -abs_count;
+ *abs_count = -*abs_count;
}
- return pbio_tacho_reset_count(tacho, abs_count);
+ return pbio_tacho_reset_count(tacho, *abs_count);
}
static pbio_error_t pbio_tacho_setup(pbio_tacho_t *tacho, uint8_t counter_id, pbio_direction_t direction, fix16_t gear_ratio) {
@@ -75,7 +74,8 @@ static pbio_error_t pbio_tacho_setup(pbio_tacho_t *tacho, uint8_t counter_id, pb
}
// Reset count to absolute value if supported
- err = pbio_tacho_reset_count_to_abs(tacho);
+ int32_t abs_count;
+ err = pbio_tacho_reset_count_to_abs(tacho, &abs_count);
if (err == PBIO_ERROR_NOT_SUPPORTED) {
// If not available, set it to 0
err = pbio_tacho_reset_count(tacho, 0);
@@ -141,7 +141,8 @@ pbio_error_t pbio_tacho_get_angle(pbio_tacho_t *tacho, int32_t *angle) {
pbio_error_t pbio_tacho_reset_angle(pbio_tacho_t *tacho, int32_t reset_angle, bool reset_to_abs) {
if (reset_to_abs) {
- return pbio_tacho_reset_count_to_abs(tacho);
+ int32_t abs_count;
+ return pbio_tacho_reset_count_to_abs(tacho, &abs_count);
} else {
return pbio_tacho_reset_count(tacho, pbio_math_mul_i32_fix16(reset_angle, tacho->counts_per_degree));
}
