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.
pybricks · Oct 28, 2023 · 5e2d65d · 5e2d65d
1 parent c42a85f
commit 5e2d65d
Show file tree

Hide file tree

Showing 5 changed files with 70 additions and 47 deletions.
diff --git 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
@@ -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,19 +318,22 @@ 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);
     #if PYBRICKS_PY_COMMON_BLE
     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
@@ -15,11 +15,18 @@
 #include <pybricks/util_mp/pb_type_enum.h>
 #include <pybricks/tools/pb_type_matrix.h>
 
-#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
@@ -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
@@ -3,7 +3,7 @@
 
 #include "py/mpconfig.h"
 
-#if PYBRICKS_PY_PARAMETERS && MICROPY_PY_BUILTINS_FLOAT
+#if PYBRICKS_PY_PARAMETERS
 
 #include <pbio/control.h>
 
@@ -12,6 +12,8 @@
 #include <pybricks/util_mp/pb_type_enum.h>
 #include <pybricks/tools/pb_type_matrix.h>
 
+#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