From 5e2d65de29cb15f70aa5a43c5056dfd0c82749fb Mon Sep 17 00:00:00 2001 From: Laurens Valk Date: Sat, 28 Oct 2023 10:38:19 +0200 Subject: [PATCH] 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. --- CHANGELOG.md | 1 + pybricks/hubs/pb_type_movehub.c | 92 ++++++++++++---------- pybricks/parameters.h | 9 ++- pybricks/parameters/pb_module_parameters.c | 2 - pybricks/parameters/pb_type_axis.c | 13 ++- 5 files changed, 70 insertions(+), 47 deletions(-) diff --git a/CHANGELOG.md b/CHANGELOG.md index 45336f605..c84d067de 100644 --- a/CHANGELOG.md +++ b/CHANGELOG.md @@ -6,6 +6,7 @@ ### Added - Added `MoveHub.imu.tilt()` ([support#539]). +- Enabled hub init orientation support for Move Hub ([support#539]). ### Fixed - Fixed Move Hub accelerometer not working since v3.3.0b5 ([support#1269]). diff --git a/pybricks/hubs/pb_type_movehub.c b/pybricks/hubs/pb_type_movehub.c index 731c82b6a..946982f0d 100644 --- a/pybricks/hubs/pb_type_movehub.c +++ b/pybricks/hubs/pb_type_movehub.c @@ -134,8 +134,20 @@ static void motion_get_acceleration_raw(int8_t *data) { data[1] = -data[1]; } +static void motion_get_acceleration_rotated(int8_t *accel, uint8_t *index, int8_t *sign) { + int8_t raw[3]; + motion_get_acceleration_raw(raw); + + // Based on given orientation, shift and flip the data. + for (uint8_t i = 0; i < 3; i++) { + accel[index[i]] = raw[i] * sign[i]; + } +} + typedef struct { mp_obj_base_t base; + uint8_t transform_index[3]; + int8_t transform_sign[3]; } hubs_MoveHub_IMU_obj_t; // This is an integer version of pybricks._common.IMU.up @@ -183,11 +195,13 @@ MP_DEFINE_CONST_FUN_OBJ_1(hubs_MoveHub_IMU_up_obj, hubs_MoveHub_IMU_up); STATIC mp_obj_t hubs_MoveHub_IMU_acceleration(mp_obj_t self_in) { - // Gets signed acceleration data + hubs_MoveHub_IMU_obj_t *self = MP_OBJ_TO_PTR(self_in); + + // Gets signed acceleration data. int8_t data[3]; - motion_get_acceleration_raw(data); + motion_get_acceleration_rotated(data, self->transform_index, self->transform_sign); - // Convert to appropriate units and return as tuple + // Convert to appropriate units and return as tuple. mp_obj_t values[3]; for (uint8_t i = 0; i < 3; i++) { values[i] = MP_OBJ_NEW_SMALL_INT(data[i] * 158); @@ -197,41 +211,13 @@ STATIC mp_obj_t hubs_MoveHub_IMU_acceleration(mp_obj_t self_in) { STATIC MP_DEFINE_CONST_FUN_OBJ_1(hubs_MoveHub_IMU_acceleration_obj, hubs_MoveHub_IMU_acceleration); // pybricks._common.SimpleAccelerometer.tilt -STATIC mp_obj_t hubs_MoveHub_IMU_tilt(size_t n_args, const mp_obj_t *pos_args, mp_map_t *kw_args) { - - PB_PARSE_ARGS_METHOD(n_args, pos_args, kw_args, - hubs_MoveHub_IMU_obj_t, self, - PB_ARG_DEFAULT_OBJ(up, pb_Side_TOP_obj), - PB_ARG_DEFAULT_OBJ(forward, pb_Side_FRONT_obj)); - - (void)self; - - // REVISIT: Re-orientation should be a one-off in the hub init like - // we do on the other hubs. - - // Get the local X and Z axes corresponding to given sides. - uint8_t index[3]; - int8_t sign[3]; - pbio_geometry_side_get_axis(pb_type_enum_get_value(up_in, &pb_enum_type_Side), &index[2], &sign[2]); - pbio_geometry_side_get_axis(pb_type_enum_get_value(forward_in, &pb_enum_type_Side), &index[0], &sign[0]); - - // Given axes may not be parallel (i.e. not same or opposite sides). - if (index[2] == index[0]) { - pb_assert(PBIO_ERROR_INVALID_ARG); - } +STATIC mp_obj_t hubs_MoveHub_IMU_tilt(mp_obj_t self_in) { - // Get the remaining Y axis to complete the coordinate system. - pbio_geometry_get_complementary_axis(index, sign); + hubs_MoveHub_IMU_obj_t *self = MP_OBJ_TO_PTR(self_in); // Get acceleration data in arbitrary units. - int8_t data[3]; - motion_get_acceleration_raw(data); - - // Based on given orientation, shift and flip the data. - int32_t accel[3]; - for (uint8_t i = 0; i < 3; i++) { - accel[index[i]] = data[i] * sign[index[i]]; - } + int8_t accel[3]; + motion_get_acceleration_rotated(accel, self->transform_index, self->transform_sign); // Now we can compute the tilt values. mp_obj_t tilt[2]; @@ -245,7 +231,7 @@ STATIC mp_obj_t hubs_MoveHub_IMU_tilt(size_t n_args, const mp_obj_t *pos_args, m // Return as tuple return mp_obj_new_tuple(2, tilt); } -STATIC MP_DEFINE_CONST_FUN_OBJ_KW(hubs_MoveHub_IMU_tilt_obj, 1, hubs_MoveHub_IMU_tilt); +MP_DEFINE_CONST_FUN_OBJ_1(hubs_MoveHub_IMU_tilt_obj, hubs_MoveHub_IMU_tilt); STATIC const mp_rom_map_elem_t hubs_MoveHub_IMU_locals_dict_table[] = { { MP_ROM_QSTR(MP_QSTR_acceleration), MP_ROM_PTR(&hubs_MoveHub_IMU_acceleration_obj) }, @@ -259,9 +245,28 @@ STATIC MP_DEFINE_CONST_OBJ_TYPE(hubs_MoveHub_IMU_type, MP_TYPE_FLAG_NONE, locals_dict, &hubs_MoveHub_IMU_locals_dict); -STATIC mp_obj_t hubs_MoveHub_IMU_make_new(void) { +STATIC mp_obj_t hubs_MoveHub_IMU_make_new(mp_obj_t top_side_in, mp_obj_t front_side_in) { hubs_MoveHub_IMU_obj_t *self = mp_obj_malloc(hubs_MoveHub_IMU_obj_t, &hubs_MoveHub_IMU_type); + // Save base orientation. + int8_t top_side_axis = mp_obj_get_int(top_side_in); + int8_t front_side_axis = mp_obj_get_int(front_side_in); + if (top_side_axis == front_side_axis || + top_side_axis == 0 || front_side_axis == 0 || + pbio_int_math_abs(top_side_axis) > 3 || + pbio_int_math_abs(front_side_axis) > 3) { + pb_assert(PBIO_ERROR_INVALID_ARG); + } + + // Get the local X and Z axes corresponding to given sides. + self->transform_index[0] = pbio_int_math_abs(front_side_axis) - 1, + self->transform_index[2] = pbio_int_math_abs(top_side_axis) - 1, + self->transform_sign[0] = pbio_int_math_sign(front_side_axis), + self->transform_sign[2] = pbio_int_math_sign(top_side_axis), + + // Get the remaining Y axis to complete the coordinate system. + pbio_geometry_get_complementary_axis(self->transform_index, self->transform_sign); + // PA4 gpio output - used for CS pbdrv_gpio_t gpio = { .bank = GPIOA, .pin = 4 }; pbdrv_gpio_out_high(&gpio); @@ -313,11 +318,14 @@ static const pb_obj_enum_member_t *movehub_buttons[] = { }; STATIC mp_obj_t hubs_MoveHub_make_new(const mp_obj_type_t *type, size_t n_args, size_t n_kw, const mp_obj_t *args) { - #if PYBRICKS_PY_COMMON_BLE PB_PARSE_ARGS_CLASS(n_args, n_kw, args, - PB_ARG_DEFAULT_INT(broadcast_channel, 0), - PB_ARG_DEFAULT_OBJ(observe_channels, mp_const_empty_tuple_obj)); - #endif + PB_ARG_DEFAULT_INT(top_side, pb_type_Axis_Z_int_enum), + PB_ARG_DEFAULT_INT(front_side, pb_type_Axis_X_int_enum) + #if PYBRICKS_PY_COMMON_BLE + , PB_ARG_DEFAULT_INT(broadcast_channel, 0) + , PB_ARG_DEFAULT_OBJ(observe_channels, mp_const_empty_tuple_obj) + #endif + ); hubs_MoveHub_obj_t *self = mp_obj_malloc(hubs_MoveHub_obj_t, type); self->battery = MP_OBJ_FROM_PTR(&pb_module_battery); @@ -325,7 +333,7 @@ STATIC mp_obj_t hubs_MoveHub_make_new(const mp_obj_type_t *type, size_t n_args, self->ble = pb_type_BLE_new(broadcast_channel_in, observe_channels_in); #endif self->button = pb_type_Keypad_obj_new(MP_ARRAY_SIZE(movehub_buttons), movehub_buttons, pbio_button_is_pressed); - self->imu = hubs_MoveHub_IMU_make_new(); + self->imu = hubs_MoveHub_IMU_make_new(top_side_in, front_side_in); self->light = common_ColorLight_internal_obj_new(pbsys_status_light); self->system = MP_OBJ_FROM_PTR(&pb_type_System); return MP_OBJ_FROM_PTR(self); diff --git a/pybricks/parameters.h b/pybricks/parameters.h index 9c63449c1..a99aab49c 100644 --- a/pybricks/parameters.h +++ b/pybricks/parameters.h @@ -15,11 +15,18 @@ #include #include -#if MICROPY_PY_BUILTINS_FLOAT extern const mp_obj_type_t pb_enum_type_Axis; +#if MICROPY_PY_BUILTINS_FLOAT extern const pb_type_Matrix_obj_t pb_type_Axis_X_obj; extern const pb_type_Matrix_obj_t pb_type_Axis_Y_obj; extern const pb_type_Matrix_obj_t pb_type_Axis_Z_obj; +#else +// Systems without floating point don't support vector math but the +// axes are still used as setup parameters. We use nonzero int indexes +// so that they may still be negated to get the opposite direction. +#define pb_type_Axis_X_int_enum (1) +#define pb_type_Axis_Y_int_enum (2) +#define pb_type_Axis_Z_int_enum (3) #endif // MICROPY_PY_BUILTINS_FLOAT #if PYBRICKS_PY_PARAMETERS_BUTTON diff --git a/pybricks/parameters/pb_module_parameters.c b/pybricks/parameters/pb_module_parameters.c index bb304f3d1..a9bf0b00d 100644 --- a/pybricks/parameters/pb_module_parameters.c +++ b/pybricks/parameters/pb_module_parameters.c @@ -9,9 +9,7 @@ STATIC const mp_rom_map_elem_t parameters_globals_table[] = { { MP_ROM_QSTR(MP_QSTR___name__), MP_ROM_QSTR(MP_QSTR_parameters) }, - #if MICROPY_PY_BUILTINS_FLOAT { MP_ROM_QSTR(MP_QSTR_Axis), MP_ROM_PTR(&pb_enum_type_Axis) }, - #endif #if PYBRICKS_PY_PARAMETERS_BUTTON { MP_ROM_QSTR(MP_QSTR_Button), MP_ROM_PTR(&pb_enum_type_Button) }, #endif diff --git a/pybricks/parameters/pb_type_axis.c b/pybricks/parameters/pb_type_axis.c index cabf689c9..a234597f0 100644 --- a/pybricks/parameters/pb_type_axis.c +++ b/pybricks/parameters/pb_type_axis.c @@ -3,7 +3,7 @@ #include "py/mpconfig.h" -#if PYBRICKS_PY_PARAMETERS && MICROPY_PY_BUILTINS_FLOAT +#if PYBRICKS_PY_PARAMETERS #include @@ -12,6 +12,8 @@ #include #include +#if MICROPY_PY_BUILTINS_FLOAT + STATIC const float pb_type_Axis_X_data[] = {1.0f, 0.0f, 0.0f}; const pb_type_Matrix_obj_t pb_type_Axis_X_obj = { @@ -41,11 +43,18 @@ const pb_type_Matrix_obj_t pb_type_Axis_Z_obj = { .m = 3, .n = 1, }; +#endif // MICROPY_PY_BUILTINS_FLOAT STATIC const mp_rom_map_elem_t pb_type_Axis_table[] = { + #if MICROPY_PY_BUILTINS_FLOAT { MP_ROM_QSTR(MP_QSTR_X), MP_ROM_PTR(&pb_type_Axis_X_obj)}, { MP_ROM_QSTR(MP_QSTR_Y), MP_ROM_PTR(&pb_type_Axis_Y_obj)}, { MP_ROM_QSTR(MP_QSTR_Z), MP_ROM_PTR(&pb_type_Axis_Z_obj)}, + #else + { MP_ROM_QSTR(MP_QSTR_X), MP_ROM_INT(pb_type_Axis_X_int_enum)}, + { MP_ROM_QSTR(MP_QSTR_Y), MP_ROM_INT(pb_type_Axis_Y_int_enum)}, + { MP_ROM_QSTR(MP_QSTR_Z), MP_ROM_INT(pb_type_Axis_Z_int_enum)}, + #endif // MICROPY_PY_BUILTINS_FLOAT }; STATIC MP_DEFINE_CONST_DICT(pb_type_Axis_locals_dict, pb_type_Axis_table); @@ -54,4 +63,4 @@ MP_DEFINE_CONST_OBJ_TYPE(pb_enum_type_Axis, MP_TYPE_FLAG_NONE, locals_dict, &(pb_type_Axis_locals_dict)); -#endif // PYBRICKS_PY_PARAMETERS && MICROPY_PY_BUILTINS_FLOAT +#endif // PYBRICKS_PY_PARAMETERS