diff --git a/.vscode/tasks.json b/.vscode/tasks.json index 73cba86f3..d7244047d 100644 --- a/.vscode/tasks.json +++ b/.vscode/tasks.json @@ -18,6 +18,34 @@ "type": "shell", "command": "make movehub -j" }, + { + "label": "build and deploy primehub", + "type": "shell", + "command": "make", + "args": ["-j", "deploy"], + "group": { + "kind": "build", + "isDefault": true + }, + "options": { + "cwd": "${workspaceFolder}/bricks/primehub" + }, + "problemMatcher": [ + { + "owner": "cpp", + "fileLocation": ["relative", "${workspaceFolder}/bricks/primehub"], + "pattern": { + "regexp": "^(.*?):(\\d+):(\\d+):\\s+(warning|error):\\s+(.*)$", + "file": 1, + "line": 2, + "column": 3, + "severity": 4, + "message": 5 + } + } + ], + "detail": "Build the PrimeHub project" + }, { "label": "build virtualhub", "type": "shell", diff --git a/CHANGELOG.md b/CHANGELOG.md index c2d8e3154..4a2c540c9 100644 --- a/CHANGELOG.md +++ b/CHANGELOG.md @@ -4,17 +4,33 @@ ## [Unreleased] +### Added + +- Added optional `calibrated=True` parameter to `acceleration()` and `up()` and + `angular_velocity()` methods of the IMU ([support#943]). +- Implemented `hub.imu.orientation()` to give the rotation matrix of the hub or + robot with respect to the inertial frame. +- Added calibration parameters that can be set for angular velocity offset and + scale and acceleration offset and scale. + ### Changed - The method `DriveBase.angle()` now returns a float ([support#1844]). This makes it properly equivalent to `hub.imu.heading`. +- Re-implemented tilt using the gyro data by default. Pure accelerometer tilt + can still be obtained with `hub.imu.tilt(use_gyro=False)`. +- Re-implemented `hub.imu.heading()` to use optionally use the projection of 3D + orientation to improve performance when the hub is lifted off the ground. + The 1D-based heading remains the default for now. ### Fixed - Fixed `DriveBase.angle()` getting an incorrectly rounded gyro value, which could cause `turn(360)` to be off by a degree ([support#1844]). +- Fixed `hub` silently ignoring non-orthogonal base axis when it should raise. -[support#1844]: https://github.com/pybricks/support/issues/1844 +[support#943]: https://github.com/pybricks/support/issues/943 [support#1886]: https://github.com/pybricks/support/issues/1886 +[support#1844]: https://github.com/pybricks/support/issues/1844 ## [3.6.0b2] - 2024-10-15 diff --git a/bricks/_common/arm_none_eabi.mk b/bricks/_common/arm_none_eabi.mk index c7d624183..19a5550b7 100644 --- a/bricks/_common/arm_none_eabi.mk +++ b/bricks/_common/arm_none_eabi.mk @@ -78,9 +78,7 @@ include $(PBTOP)/micropython/py/mkenv.mk # Include common frozen modules. ifeq ($(PB_FROZEN_MODULES),1) -ifneq ("$(wildcard $(PBTOP)/bricks/_common/modules/*.py)","") -FROZEN_MANIFEST ?= ../_common/manifest.py -endif +FROZEN_MANIFEST ?= manifest.py endif # qstr definitions (must come before including py.mk) diff --git a/bricks/_common/manifest.py b/bricks/_common/manifest.py index 84670e398..ea646d2c7 100644 --- a/bricks/_common/manifest.py +++ b/bricks/_common/manifest.py @@ -2,8 +2,5 @@ modules = list(pathlib.Path("./modules").glob("*.py")) -if any(modules): - for m in modules: - path, file = m.parts - print(f"Including {m.stem} as a module.") - freeze_as_mpy(path, file) +for m in modules: + freeze_as_mpy(str(m.parent), m.name) diff --git a/bricks/_common/modules/.gitignore b/bricks/_common/modules/.gitignore index aed42152a..4e5d1d755 100644 --- a/bricks/_common/modules/.gitignore +++ b/bricks/_common/modules/.gitignore @@ -1,3 +1,2 @@ *.py -!_hub_extra.py !_builtin_port_view.py diff --git a/bricks/_common/modules/_hub_extra.py b/bricks/_common/modules/_hub_extra.py deleted file mode 100644 index 8614afb65..000000000 --- a/bricks/_common/modules/_hub_extra.py +++ /dev/null @@ -1,115 +0,0 @@ -from pybricks.parameters import Axis -from pybricks.tools import wait - - -def light_matrix_text_async(display, text, on, off): - for char in text: - display.char(char) - yield from wait(on) - display.off() - yield from wait(off) - - -def imu_update_heading_correction(hub): - - # Number of turns to confirm the result. - NUMBER_CONFIRM_TURNS = 5 - - # Maximum speed values before we consider the result invalid. - MAX_XY_SPEED = 50 - MAX_Z_SPEED = 800 - - # Routine to wait on a button, with some extra time to avoid vibration directly after. - def wait_for_click(): - while hub.buttons.pressed(): - wait(1) - while not hub.buttons.pressed(): - wait(1) - print("Processing...") - while hub.buttons.pressed(): - wait(1) - wait(1500) - - # Disable stop buttons - hub.system.set_stop_button(None) - print( - "Put the hub on a flat table. Align against a fixed reference like a wall or heavy book. Press hub button when ready." - ) - - # Wait for fixed reference and store the initial angle value. - wait_for_click() - while not hub.imu.ready() or not hub.imu.stationary(): - wait(1) - start_z = hub.imu.rotation(-Axis.Z) - - # Wait for a full rotation and get the result. - print( - "Keep the hub flat and slide it to make a full turn clockwise. Put it against the same reference. Press hub button when ready." - ) - wait_for_click() - one_turn = hub.imu.rotation(-Axis.Z) - start_z - - # Require clockwise... - if one_turn < 0: - raise ValueError("You turned it the wrong way. Please try again.") - - # Sanity check. Should be close to 360. - if not (350 < one_turn < 370): - print(one_turn) - raise ValueError( - "The error was more than 10 degrees, which is unexpected. Please try again." - ) - - # Instruct to make more turns. - print("So far so good! Now make", NUMBER_CONFIRM_TURNS, "full clockwise turns.") - - for i in range(NUMBER_CONFIRM_TURNS): - - # The rotation target is just under a rotation so we can show the next - # message to keep going in time, avoiding doubts about what to do. - target = one_turn * (i + 2) - 10 - - # Wait until the hub reaches the target. - while hub.imu.rotation(-Axis.Z) - start_z < target: - wait(1) - - if hub.buttons.pressed(): - raise RuntimeError("Don't press the button until all turns are complete!") - - if abs(hub.imu.angular_velocity(Axis.Z)) > MAX_Z_SPEED: - raise RuntimeError("Not so fast! Try again.") - - if ( - abs(hub.imu.angular_velocity(Axis.X)) + abs(hub.imu.angular_velocity(Axis.Y)) - > MAX_XY_SPEED - ): - - raise RuntimeError("Please keep the hub flat! Try again.") - - # Inform user of status. - print("Completed", i + 1, "out of", NUMBER_CONFIRM_TURNS, "turns. ", end="") - if i < NUMBER_CONFIRM_TURNS - 1: - print("Keep going!") - else: - print("Put it against the same reference. Press hub button when ready.") - - # Wait for final confirmation. - wait_for_click() - - # Verify the result. - expected = one_turn * (NUMBER_CONFIRM_TURNS + 1) - result = hub.imu.rotation(-Axis.Z) - start_z - - if abs(expected - result) > NUMBER_CONFIRM_TURNS / 2: - print( - "The", - NUMBER_CONFIRM_TURNS, - "extra turns where different from the first turn. Try again.", - ) - print("Expected", expected, "but got", result) - - # Get the final result to save. - average_turn = result / (NUMBER_CONFIRM_TURNS + 1) - print("For every 360-degree turn your gyro Z-axis reports:", average_turn) - hub.imu.settings(heading_correction=average_turn) - print("Saved! Now the heading() method will report the adjusted value.") diff --git a/bricks/cityhub/manifest.py b/bricks/cityhub/manifest.py new file mode 100644 index 000000000..d901ff875 --- /dev/null +++ b/bricks/cityhub/manifest.py @@ -0,0 +1 @@ +include("../_common/manifest.py") diff --git a/bricks/debug/manifest.py b/bricks/debug/manifest.py new file mode 100644 index 000000000..d901ff875 --- /dev/null +++ b/bricks/debug/manifest.py @@ -0,0 +1 @@ +include("../_common/manifest.py") diff --git a/bricks/essentialhub/manifest.py b/bricks/essentialhub/manifest.py new file mode 100644 index 000000000..fc6d0ca0e --- /dev/null +++ b/bricks/essentialhub/manifest.py @@ -0,0 +1,5 @@ +# Common modules +include("../_common/manifest.py") + +# Essential Hub IMU is the same as the one in Prime Hub +freeze_as_mpy("../primehub/modules", "_imu_calibrate.py") diff --git a/bricks/ev3/manifest.py b/bricks/ev3/manifest.py new file mode 100644 index 000000000..d901ff875 --- /dev/null +++ b/bricks/ev3/manifest.py @@ -0,0 +1 @@ +include("../_common/manifest.py") diff --git a/bricks/nxt/manifest.py b/bricks/nxt/manifest.py new file mode 100644 index 000000000..d901ff875 --- /dev/null +++ b/bricks/nxt/manifest.py @@ -0,0 +1 @@ +include("../_common/manifest.py") diff --git a/bricks/primehub/manifest.py b/bricks/primehub/manifest.py new file mode 100644 index 000000000..dc7d2cdb2 --- /dev/null +++ b/bricks/primehub/manifest.py @@ -0,0 +1,5 @@ +# Common modules +include("../_common/manifest.py") + +freeze_as_mpy("../primehub/modules", "_imu_calibrate.py") +freeze_as_mpy("../primehub/modules", "_light_matrix.py") diff --git a/bricks/primehub/modules/_imu_calibrate.py b/bricks/primehub/modules/_imu_calibrate.py new file mode 100644 index 000000000..521a151ca --- /dev/null +++ b/bricks/primehub/modules/_imu_calibrate.py @@ -0,0 +1,149 @@ +from pybricks.hubs import ThisHub +from pybricks.parameters import Side, Axis +from pybricks.tools import wait, vector + + +hub = ThisHub() + + +def beep(freq): + try: + hub.speaker.beep(freq, 100) + except AttributeError: + # Technic hub does not have a speaker. + pass + wait(10) + + +def wait_for_stationary(side): + while not hub.imu.stationary() or hub.imu.up() != side: + wait(10) + + +up_sides = { + Side.FRONT: (0, 0), + Side.BACK: (1, 0), + Side.LEFT: (2, 1), + Side.RIGHT: (3, 1), + Side.TOP: (4, 2), + Side.BOTTOM: (5, 2), +} + +gravity = [0] * 6 +bias = vector(0, 0, 0) + +STATIONARY_COUNT = 1000 + + +def roll_over_axis(axis, new_side): + + global bias, bias_count + + print("Roll it towards you, without lifting the hub up!") + + angle_start = hub.imu.rotation(axis, calibrated=False) + while hub.imu.up() != new_side or not hub.imu.stationary(): + + _, _, z = hub.imu.orientation() * axis + if abs(z) > 0.07: + print(hub.imu.orientation() * axis) + raise RuntimeError("Lifted it!") + wait(100) + + uncalibrated_90_deg_rotation = abs(hub.imu.rotation(axis, calibrated=False) - angle_start) + if abs(uncalibrated_90_deg_rotation - 90) > 10: + raise RuntimeError("Not 90 deg!") + + print("Calibrating...") + beep(1000) + + rotation_start = vector( + hub.imu.rotation(Axis.X, calibrated=False), + hub.imu.rotation(Axis.Y, calibrated=False), + hub.imu.rotation(Axis.Z, calibrated=False), + ) + + acceleration = vector(0, 0, 0) + + for i in range(STATIONARY_COUNT): + acceleration += hub.imu.acceleration(calibrated=False) + bias += hub.imu.angular_velocity(calibrated=False) + wait(1) + + acceleration /= STATIONARY_COUNT + + rotation_end = vector( + hub.imu.rotation(Axis.X, calibrated=False), + hub.imu.rotation(Axis.Y, calibrated=False), + hub.imu.rotation(Axis.Z, calibrated=False), + ) + if abs(rotation_end - rotation_start) > 1: + raise RuntimeError("Moved it!") + + side_index, axis_index = up_sides[new_side] + + # Store the gravity value for the current side being up. + # We will visit each side several times. We'll divide by the number + # of visits later. + gravity[side_index] += acceleration[axis_index] + + beep(500) + + return uncalibrated_90_deg_rotation + + +calibrate_x = """ +Going to calibrate X now! +- Put the hub on the table in front of you. +- top side (display) facing up +- right side (ports BDF) towards you. +""" + +calibrate_y = """ +Going to calibrate Y now +- Put the hub on the table in front of you. +- top side (display) facing up +- back side (speaker) towards you. +""" + +calibrate_z = """ +Going to calibrate Z now! +- Put the hub on the table in front of you. +- front side (USB port) facing up +- left side (ports ACE) towards you +""" + +REPEAT = 2 + +# For each 3-axis run, we will visit each side twice. +SIDE_COUNT = REPEAT * 2 + + +def roll_hub(axis, message, start_side, sides): + print(message) + wait_for_stationary(start_side) + beep() + rotation = 0 + for _ in range(REPEAT): + for side in sides: + rotation += roll_over_axis(axis, side) + return rotation / REPEAT + + +rotation_x = roll_hub( + Axis.X, calibrate_x, Side.TOP, [Side.LEFT, Side.BOTTOM, Side.RIGHT, Side.TOP] +) +rotation_y = roll_hub( + Axis.Y, calibrate_y, Side.TOP, [Side.FRONT, Side.BOTTOM, Side.BACK, Side.TOP] +) +rotation_z = roll_hub( + Axis.Z, calibrate_z, Side.FRONT, [Side.RIGHT, Side.BACK, Side.LEFT, Side.FRONT] +) + +hub.imu.settings( + angular_velocity_bias=tuple(bias / SIDE_COUNT / STATIONARY_COUNT / 6), + angular_velocity_scale=(rotation_x, rotation_y, rotation_z), + acceleration_correction=[g / SIDE_COUNT for g in gravity], +) + +print("Result: ", hub.imu.settings()) diff --git a/bricks/primehub/modules/_light_matrix.py b/bricks/primehub/modules/_light_matrix.py new file mode 100644 index 000000000..1ac7ed5c9 --- /dev/null +++ b/bricks/primehub/modules/_light_matrix.py @@ -0,0 +1,9 @@ +from pybricks.tools import wait + + +def light_matrix_text_async(display, text, on, off): + for char in text: + display.char(char) + yield from wait(on) + display.off() + yield from wait(off) diff --git a/bricks/technichub/manifest.py b/bricks/technichub/manifest.py new file mode 100644 index 000000000..852c2b2d1 --- /dev/null +++ b/bricks/technichub/manifest.py @@ -0,0 +1,5 @@ +# Common modules +include("../_common/manifest.py") + +# Technic Hub IMU is the same as the one in Prime Hub +freeze_as_mpy("../primehub/modules", "_imu_calibrate.py") diff --git a/lib/pbio/drv/imu/imu_lsm6ds3tr_c_stm32.c b/lib/pbio/drv/imu/imu_lsm6ds3tr_c_stm32.c index 69139b624..d75ce7528 100644 --- a/lib/pbio/drv/imu/imu_lsm6ds3tr_c_stm32.c +++ b/lib/pbio/drv/imu/imu_lsm6ds3tr_c_stm32.c @@ -47,8 +47,14 @@ struct _pbdrv_imu_dev_t { pbdrv_imu_handle_frame_data_func_t handle_frame_data; /* Callback to process unfiltered gyro and accelerometer data recorded while stationary. */ pbdrv_imu_handle_stationary_data_func_t handle_stationary_data; - /** Raw data. */ + /** Latest raw data. */ int16_t data[6]; + /** Most recent slow moving average of raw data. */ + int16_t data_slow[6]; + /** Sum of raw data for slow moving average. */ + int32_t data_slow_sum[6]; + /** Raw data count used for slow moving average. */ + int32_t data_slow_count; /** Start time of window in which stationary samples are recorded (us)*/ uint32_t stationary_time_start; /** Raw data point to which new samples are compared to detect stationary. */ @@ -265,8 +271,25 @@ static void pbdrv_imu_lsm6ds3tr_c_stm32_reset_stationary_buffer(pbdrv_imu_dev_t memset(&imu_dev->stationary_gyro_data_sum, 0, sizeof(imu_dev->stationary_gyro_data_sum)); } +static void pbdrv_imu_lsm6ds3tr_c_stm32_update_slow_moving_average(pbdrv_imu_dev_t *imu_dev) { + for (uint32_t i = 0; i < 6; i++) { + imu_dev->data_slow_sum[i] += imu_dev->data[i]; + } + imu_dev->data_slow_count++; + if (imu_dev->data_slow_count == 125) { + for (uint32_t i = 0; i < 6; i++) { + imu_dev->data_slow[i] = imu_dev->data_slow_sum[i] / imu_dev->data_slow_count; + imu_dev->data_slow_sum[i] = 0; + } + imu_dev->data_slow_count = 0; + } +} + static void pbdrv_imu_lsm6ds3tr_c_stm32_update_stationary_status(pbdrv_imu_dev_t *imu_dev) { + // Update slow moving average of raw data, used as starting point for stationary detection. + pbdrv_imu_lsm6ds3tr_c_stm32_update_slow_moving_average(imu_dev); + // Check whether still stationary compared to constant start sample. if (!is_bounded(imu_dev->data[0] - imu_dev->stationary_data_start[0], imu_dev->config.gyro_stationary_threshold) || !is_bounded(imu_dev->data[1] - imu_dev->stationary_data_start[1], imu_dev->config.gyro_stationary_threshold) || @@ -277,10 +300,11 @@ static void pbdrv_imu_lsm6ds3tr_c_stm32_update_stationary_status(pbdrv_imu_dev_t ) { // Not stationary anymore, so reset counter and gyro sum data so we can start over. imu_dev->stationary_now = false; - pbdrv_imu_lsm6ds3tr_c_stm32_reset_stationary_buffer(imu_dev); - // Current sample becomes new starting value to compare to. - memcpy(&imu_dev->stationary_data_start[0], &imu_dev->data[0], NUM_DATA_BYTES); + // Slow moving average becomes new starting value to compare to. + memcpy(&imu_dev->stationary_data_start[0], &imu_dev->data_slow[0], sizeof(imu_dev->stationary_data_start)); + + pbdrv_imu_lsm6ds3tr_c_stm32_reset_stationary_buffer(imu_dev); return; } diff --git a/lib/pbio/include/pbio/drivebase.h b/lib/pbio/include/pbio/drivebase.h index 0a117dd7a..309c63887 100644 --- a/lib/pbio/include/pbio/drivebase.h +++ b/lib/pbio/include/pbio/drivebase.h @@ -16,13 +16,15 @@ #include +#include + #if PBIO_CONFIG_NUM_DRIVEBASES > 0 typedef struct _pbio_drivebase_t { /** - * True if a gyro or compass is used for heading control, else false. + * Whether to use the gyro for heading control, and if so which type. */ - bool use_gyro; + pbio_imu_heading_type_t gyro_heading_type; /** * Synchronization state to indicate that one or more controllers are paused. */ @@ -79,7 +81,7 @@ pbio_error_t pbio_drivebase_get_state_user_angle(pbio_drivebase_t *db, float *an pbio_error_t pbio_drivebase_reset(pbio_drivebase_t *db, int32_t distance, int32_t angle); pbio_error_t pbio_drivebase_get_drive_settings(const pbio_drivebase_t *db, int32_t *drive_speed, int32_t *drive_acceleration, int32_t *drive_deceleration, int32_t *turn_rate, int32_t *turn_acceleration, int32_t *turn_deceleration); pbio_error_t pbio_drivebase_set_drive_settings(pbio_drivebase_t *db, int32_t drive_speed, int32_t drive_acceleration, int32_t drive_deceleration, int32_t turn_rate, int32_t turn_acceleration, int32_t turn_deceleration); -pbio_error_t pbio_drivebase_set_use_gyro(pbio_drivebase_t *db, bool use_gyro); +pbio_error_t pbio_drivebase_set_use_gyro(pbio_drivebase_t *db, pbio_imu_heading_type_t heading_type); #if PBIO_CONFIG_DRIVEBASE_SPIKE diff --git a/lib/pbio/include/pbio/geometry.h b/lib/pbio/include/pbio/geometry.h index 77e8e90e1..be176d835 100644 --- a/lib/pbio/include/pbio/geometry.h +++ b/lib/pbio/include/pbio/geometry.h @@ -31,43 +31,58 @@ typedef enum { /** * Coordinate-like type with x, y, and z floating point values. */ -typedef struct _pbio_geometry_xyz_t { - union { - struct { - float x; /**< X coordinate.*/ - float y; /**< Y coordinate.*/ - float z; /**< Z coordinate.*/ - }; - float values[3]; +typedef union { + struct { + float x; /**< X coordinate.*/ + float y; /**< Y coordinate.*/ + float z; /**< Z coordinate.*/ }; + float values[3]; } pbio_geometry_xyz_t; /** * Representation of a 3x3 matrix. */ -typedef struct _pbio_geometry_matrix_3x3_t { - union { - struct { - float m11; - float m12; - float m13; - float m21; - float m22; - float m23; - float m31; - float m32; - float m33; - }; - float values[9]; +typedef union { + struct { + float m11; + float m12; + float m13; + float m21; + float m22; + float m23; + float m31; + float m32; + float m33; }; + float values[9]; } pbio_geometry_matrix_3x3_t; +/** + * Quaternion orientation or its time derivative. + */ +typedef union { + struct { + float q1; /**< q1 coordinate.*/ + float q2; /**< q2 coordinate.*/ + float q3; /**< q3 coordinate.*/ + float q4; /**< q4 coordinate.*/ + }; + float values[4]; +} pbio_geometry_quaternion_t; + +#define pbio_geometry_degrees_to_radians(degrees) ((degrees) * 0.017453293f) + +#define pbio_geometry_radians_to_degrees(radians) ((radians) * 57.29577951f) + void pbio_geometry_side_get_axis(pbio_geometry_side_t side, uint8_t *index, int8_t *sign); void pbio_geometry_get_complementary_axis(uint8_t *index, int8_t *sign); pbio_geometry_side_t pbio_geometry_side_from_vector(pbio_geometry_xyz_t *vector); +float pbio_geometry_vector_norm(pbio_geometry_xyz_t *input); + pbio_error_t pbio_geometry_vector_normalize(pbio_geometry_xyz_t *input, pbio_geometry_xyz_t *output); void pbio_geometry_vector_cross_product(pbio_geometry_xyz_t *a, pbio_geometry_xyz_t *b, pbio_geometry_xyz_t *output); @@ -76,8 +91,22 @@ pbio_error_t pbio_geometry_vector_project(pbio_geometry_xyz_t *axis, pbio_geomet void pbio_geometry_vector_map(pbio_geometry_matrix_3x3_t *map, pbio_geometry_xyz_t *input, pbio_geometry_xyz_t *output); +void pbio_geometry_matrix_multiply(pbio_geometry_matrix_3x3_t *a, pbio_geometry_matrix_3x3_t *b, pbio_geometry_matrix_3x3_t *output); + pbio_error_t pbio_geometry_map_from_base_axes(pbio_geometry_xyz_t *x_axis, pbio_geometry_xyz_t *z_axis, pbio_geometry_matrix_3x3_t *rotation); +void pbio_geometry_quaternion_to_rotation_matrix(pbio_geometry_quaternion_t *q, pbio_geometry_matrix_3x3_t *R); + +void pbio_geometry_quaternion_from_gravity_unit_vector(pbio_geometry_xyz_t *g, pbio_geometry_quaternion_t *q); + +void pbio_geometry_quaternion_get_rate_of_change(pbio_geometry_quaternion_t *q, pbio_geometry_xyz_t *w, pbio_geometry_quaternion_t *dq); + +void pbio_geometry_quaternion_normalize(pbio_geometry_quaternion_t *q); + +float pbio_geometry_maxf(float a, float b); + +float pbio_geometry_absf(float a); + #endif // _PBIO_GEOMETRY_H_ /** @} */ diff --git a/lib/pbio/include/pbio/imu.h b/lib/pbio/include/pbio/imu.h index 6807f3bdf..5099c20dd 100644 --- a/lib/pbio/include/pbio/imu.h +++ b/lib/pbio/include/pbio/imu.h @@ -18,74 +18,186 @@ #include #include +/** + * Heading type to use, set, or get. + */ +typedef enum { + /** + * Heading should not be used. + */ + PBIO_IMU_HEADING_TYPE_NONE, + /** + * The heading is the integrated gyro rate along one fixed axis. + */ + PBIO_IMU_HEADING_TYPE_1D, + /** + * The heading is angle between the projection of the line coming out of + * the front of the hub onto the horizontal plane and the x-axis. + */ + PBIO_IMU_HEADING_TYPE_3D, +} pbio_imu_heading_type_t; + +/** + * IMU settings flags. + * + * Note: Add new flags such that false is the default value. + */ +typedef enum { + /** + * The gyro stationary threshold has been updated. + */ + PBIO_IMU_SETTINGS_FLAGS_GYRO_STATIONARY_THRESHOLD_SET = (1 << 0), + /** + * The accelerometer stationary threshold has been updated. + */ + PBIO_IMU_SETTINGS_FLAGS_ACCEL_STATIONARY_THRESHOLD_SET = (1 << 1), + /** + * The initial values for the gyro bias have been set. + */ + PBIO_IMU_SETTINGS_FLAGS_GYRO_BIAS_INITIAL_SET = (1 << 3), + /** + * The per-axis angular velocity scale has been set. + */ + PBIO_IMU_SETTINGS_FLAGS_GYRO_SCALE_SET = (1 << 4), + /** + * The accelerometer offsets and scale has been calibrated. + */ + PBIO_IMU_SETTINGS_FLAGS_ACCEL_CALIBRATED = (1 << 5), + /** + * The heading correction for 1D rotation has been set. + */ + PBIO_IMU_SETTINGS_FLAGS_HEADING_CORRECTION_1D_SET = (1 << 6), +} pbio_imu_persistent_settings_flags_t; + +/** + * Persistent IMU settings. All data types are little-endian. + */ +typedef struct { + /** + * Flags indicating which persistent settings have been updated by + * the user or a calibration routine. In settings setter functions, this + * flag value is used to indicate which values are being set. + */ + uint32_t flags; + /** Angular velocity threshold below which the IMU is considered stationary, in deg/s. */ + float gyro_stationary_threshold; + /** Acceleration threshold below which the IMU is considered stationary, in mm/s^2. */ + float accel_stationary_threshold; + /** Positive acceleration values */ + pbio_geometry_xyz_t gravity_pos; + /** Negative acceleration values */ + pbio_geometry_xyz_t gravity_neg; + /** Angular velocity stationary bias initially used on boot */ + pbio_geometry_xyz_t angular_velocity_bias_start; + /** Angular velocity scale (unadjusted measured degrees per whole rotation) */ + pbio_geometry_xyz_t angular_velocity_scale; + /** + * Additional correction for 1D rotation in the user frame. Works on top + * of other calibration settings and values. Only used for 1D heading. + * + * This setting may be removed in the future when removing 1D support. + */ + float heading_correction_1d; +} pbio_imu_persistent_settings_t; + #if PBIO_CONFIG_IMU void pbio_imu_init(void); +void pbio_imu_set_default_settings(pbio_imu_persistent_settings_t *settings); + +void pbio_imu_apply_loaded_settings(pbio_imu_persistent_settings_t *settings); + pbio_error_t pbio_imu_set_base_orientation(pbio_geometry_xyz_t *x_axis, pbio_geometry_xyz_t *z_axis); bool pbio_imu_is_stationary(void); bool pbio_imu_is_ready(void); -void pbio_imu_get_settings(float *angular_velocity, float *acceleration, float *heading_correction); +pbio_error_t pbio_imu_get_settings(pbio_imu_persistent_settings_t **settings); -pbio_error_t pbio_imu_set_settings(float angular_velocity, float acceleration, float heading_correction); +pbio_error_t pbio_imu_set_settings(pbio_imu_persistent_settings_t *new_settings); -void pbio_imu_get_angular_velocity(pbio_geometry_xyz_t *values); +void pbio_imu_get_angular_velocity(pbio_geometry_xyz_t *values, bool calibrated); -void pbio_imu_get_acceleration(pbio_geometry_xyz_t *values); +void pbio_imu_get_acceleration(pbio_geometry_xyz_t *values, bool calibrated); -pbio_error_t pbio_imu_get_single_axis_rotation(pbio_geometry_xyz_t *axis, float *angle); +void pbio_imu_get_tilt_vector(pbio_geometry_xyz_t *values); -pbio_geometry_side_t pbio_imu_get_up_side(void); +pbio_error_t pbio_imu_get_single_axis_rotation(pbio_geometry_xyz_t *axis, float *angle, bool calibrated); -float pbio_imu_get_heading(void); +pbio_geometry_side_t pbio_imu_get_up_side(bool calibrated); + +float pbio_imu_get_heading(pbio_imu_heading_type_t type); void pbio_imu_set_heading(float desired_heading); -void pbio_imu_get_heading_scaled(pbio_angle_t *heading, int32_t *heading_rate, int32_t ctl_steps_per_degree); +void pbio_imu_get_heading_scaled(pbio_imu_heading_type_t type, pbio_angle_t *heading, int32_t *heading_rate, int32_t ctl_steps_per_degree); + +void pbio_orientation_imu_get_orientation(pbio_geometry_matrix_3x3_t *rotation); #else // PBIO_CONFIG_IMU static inline void pbio_imu_init(void) { } +static inline void pbio_imu_set_default_settings(pbio_imu_persistent_settings_t *settings) { +} + +static inline void pbio_imu_apply_loaded_settings(pbio_imu_persistent_settings_t *settings) { +} + static inline pbio_error_t pbio_imu_set_base_orientation(pbio_geometry_xyz_t *x_axis, pbio_geometry_xyz_t *z_axis) { - return PBIO_ERROR_NOT_IMPLEMENTED; + return PBIO_ERROR_NOT_SUPPORTED; } static inline bool pbio_imu_is_stationary(void) { return false; } -static inline void pbio_imu_get_settings(float *angular_velocity, float *acceleration, float *heading_correction) { +static inline bool pbio_imu_is_ready(void) { + return false; +} + +static inline pbio_error_t pbio_imu_get_settings(pbio_imu_persistent_settings_t **settings) { + return PBIO_ERROR_NOT_SUPPORTED; } -static inline pbio_error_t pbio_imu_set_settings(float angular_velocity, float acceleration, float heading_correction) { +static inline pbio_error_t pbio_imu_set_settings(pbio_imu_persistent_settings_t *new_settings) { return PBIO_ERROR_NOT_SUPPORTED; } -static inline void pbio_imu_get_angular_velocity(pbio_geometry_xyz_t *values) { +static inline void pbio_imu_get_angular_velocity(pbio_geometry_xyz_t *values, bool calibrated) { } -static inline void pbio_imu_get_acceleration(pbio_geometry_xyz_t *values) { +static inline void pbio_imu_get_acceleration(pbio_geometry_xyz_t *values, bool calibrated) { } -static inline pbio_geometry_side_t pbio_imu_get_up_side(void) { +static inline void pbio_imu_get_tilt_vector(pbio_geometry_xyz_t *values) { +} + +static inline pbio_error_t pbio_imu_get_single_axis_rotation(pbio_geometry_xyz_t *axis, float *angle, bool calibrated) { + return PBIO_ERROR_NOT_SUPPORTED; +} + +static inline pbio_geometry_side_t pbio_imu_get_up_side(bool calibrated) { return PBIO_GEOMETRY_SIDE_TOP; } -static inline float pbio_imu_get_heading(void) { +static inline float pbio_imu_get_heading(pbio_imu_heading_type_t type) { return 0.0f; } static inline void pbio_imu_set_heading(float desired_heading) { } -static inline void pbio_imu_get_heading_scaled(pbio_angle_t *heading, int32_t *heading_rate, int32_t ctl_steps_per_degree) { +static inline void pbio_imu_get_heading_scaled(pbio_imu_heading_type_t type, pbio_angle_t *heading, int32_t *heading_rate, int32_t ctl_steps_per_degree) { } +static inline void pbio_orientation_imu_get_orientation(pbio_geometry_matrix_3x3_t *rotation) { +} + + #endif // PBIO_CONFIG_IMU #endif // _PBIO_IMU_H_ diff --git a/lib/pbio/include/pbsys/storage.h b/lib/pbio/include/pbsys/storage.h index 1d94cffd9..16331d75e 100644 --- a/lib/pbio/include/pbsys/storage.h +++ b/lib/pbio/include/pbsys/storage.h @@ -25,6 +25,8 @@ pbio_error_t pbsys_storage_set_user_data(uint32_t offset, const uint8_t *data, u pbio_error_t pbsys_storage_get_user_data(uint32_t offset, uint8_t **data, uint32_t size); +void pbsys_storage_request_write(void); + #else static inline uint32_t pbsys_storage_get_maximum_program_size(void) { @@ -39,6 +41,9 @@ static inline pbio_error_t pbsys_storage_get_user_data(uint32_t offset, uint8_t return PBIO_ERROR_NOT_SUPPORTED; } +static inline void pbsys_storage_request_write(void) { +} + #endif // PBSYS_CONFIG_STORAGE #endif // _PBSYS_STORAGE_H_ diff --git a/lib/pbio/include/pbsys/storage_settings.h b/lib/pbio/include/pbsys/storage_settings.h index 98df2f695..ff3a528bf 100644 --- a/lib/pbio/include/pbsys/storage_settings.h +++ b/lib/pbio/include/pbsys/storage_settings.h @@ -18,6 +18,7 @@ #include #include +#include #include /** @@ -37,16 +38,7 @@ typedef struct _pbsys_storage_settings_t { /** System setting flags. */ uint32_t flags; #if PBIO_CONFIG_IMU - /** Angular velocity threshold below which the IMU is considered stationary, in deg/s. */ - float gyro_stationary_threshold; - /** Acceleration threshold below which the IMU is considered stationary, in mm/s^2. */ - float accel_stationary_threshold; - /** - * Number of degrees measured for one full turn along the user Z axis. This - * is used to correct the heading value. Other rotation methods are not - * affected. - */ - float heading_correction; + pbio_imu_persistent_settings_t imu_settings; #endif } pbsys_storage_settings_t; @@ -60,8 +52,6 @@ bool pbsys_storage_settings_bluetooth_enabled(void); void pbsys_storage_settings_bluetooth_enabled_request_toggle(void); -void pbsys_storage_settings_save_imu_settings(void); - #else static inline void pbsys_storage_settings_set_defaults(pbsys_storage_settings_t *settings) { @@ -74,9 +64,6 @@ static inline bool pbsys_storage_settings_bluetooth_enabled(void) { static inline void pbsys_storage_settings_bluetooth_enabled_request_toggle(void) { } -static inline void pbsys_storage_settings_save_imu_settings(void) { -} - #endif // PBSYS_CONFIG_STORAGE #endif // _PBSYS_STORAGE_SETTINGS_H_ diff --git a/lib/pbio/src/drivebase.c b/lib/pbio/src/drivebase.c index 36a31508c..16099174d 100644 --- a/lib/pbio/src/drivebase.c +++ b/lib/pbio/src/drivebase.c @@ -182,8 +182,8 @@ static pbio_error_t pbio_drivebase_get_state_control(pbio_drivebase_t *db, pbio_ // Note that the heading speed estimate (used for derivative control still // uses the motor estimate rather than the gyro speed, to guarantee the // same stability properties to stabilize the motors.) - if (db->use_gyro) { - pbio_imu_get_heading_scaled(&state_heading->position, &state_heading->speed, db->control_heading.settings.ctl_steps_per_app_step); + if (db->gyro_heading_type != PBIO_IMU_HEADING_TYPE_NONE) { + pbio_imu_get_heading_scaled(db->gyro_heading_type, &state_heading->position, &state_heading->speed, db->control_heading.settings.ctl_steps_per_app_step); } return PBIO_SUCCESS; @@ -365,7 +365,7 @@ pbio_error_t pbio_drivebase_get_drivebase(pbio_drivebase_t **db_address, pbio_se pbio_drivebase_reset(db, 0, 0); // By default, don't use gyro for steering control. - db->use_gyro = false; + db->gyro_heading_type = PBIO_IMU_HEADING_TYPE_NONE; return PBIO_SUCCESS; } @@ -376,13 +376,13 @@ pbio_error_t pbio_drivebase_get_drivebase(pbio_drivebase_t **db_address, pbio_se * This function will stop the drivebase if it is running. * * @param [in] db Drivebase instance. - * @param [in] use_gyro Whether to use the gyro for heading control. + * @param [in] heading_type Whether to use the gyro for heading control, and if so which type. * @return Error code. */ -pbio_error_t pbio_drivebase_set_use_gyro(pbio_drivebase_t *db, bool use_gyro) { +pbio_error_t pbio_drivebase_set_use_gyro(pbio_drivebase_t *db, pbio_imu_heading_type_t heading_type) { // No need to reset controls if already in correct mode. - if (db->use_gyro == use_gyro) { + if (db->gyro_heading_type == heading_type) { return PBIO_SUCCESS; } @@ -393,7 +393,7 @@ pbio_error_t pbio_drivebase_set_use_gyro(pbio_drivebase_t *db, bool use_gyro) { return err; } - db->use_gyro = use_gyro; + db->gyro_heading_type = heading_type; return PBIO_SUCCESS; } @@ -860,7 +860,7 @@ pbio_error_t pbio_drivebase_reset(pbio_drivebase_t *db, int32_t distance, int32_ pbio_angle_diff(&measured_heading.position, &reported_new, &db->heading_offset); // Synchronize heading and drivebase angle state if gyro in use. - if (db->use_gyro) { + if (db->gyro_heading_type != PBIO_IMU_HEADING_TYPE_NONE) { pbio_imu_set_heading(angle); } @@ -877,7 +877,7 @@ bool pbio_drivebase_any_uses_gyro(void) { pbio_drivebase_t *db = &drivebases[i]; // Only consider activated drive bases that use the gyro. - if (!pbio_drivebase_update_loop_is_running(db) || !db->use_gyro) { + if (!pbio_drivebase_update_loop_is_running(db) || db->gyro_heading_type == PBIO_IMU_HEADING_TYPE_NONE) { continue; } diff --git a/lib/pbio/src/geometry.c b/lib/pbio/src/geometry.c index e137e9fda..c77c3b574 100644 --- a/lib/pbio/src/geometry.c +++ b/lib/pbio/src/geometry.c @@ -104,9 +104,21 @@ pbio_geometry_side_t pbio_geometry_side_from_vector(pbio_geometry_xyz_t *vector) return axis | (negative << 2); } +/** + * Gets the norm of a vector. + * + * @param [in] input The vector. + * @return The norm of the vector. + */ +float pbio_geometry_vector_norm(pbio_geometry_xyz_t *input) { + return sqrtf(input->x * input->x + input->y * input->y + input->z * input->z); +} + /** * Normalizes a vector so it has unit length. * + * Output is allowed to be same as input, in which case input is normalized. + * * @param [in] input The vector to normalize. * @param [out] output The normalized vector. * @return ::PBIO_ERROR_INVALID_ARG if the input has zero length, otherwise ::PBIO_SUCCESS. @@ -114,7 +126,7 @@ pbio_geometry_side_t pbio_geometry_side_from_vector(pbio_geometry_xyz_t *vector) pbio_error_t pbio_geometry_vector_normalize(pbio_geometry_xyz_t *input, pbio_geometry_xyz_t *output) { // Compute the norm. - float norm = sqrtf(input->x * input->x + input->y * input->y + input->z * input->z); + float norm = pbio_geometry_vector_norm(input); // If the vector norm is zero, do nothing. if (norm == 0.0f) { @@ -176,6 +188,25 @@ void pbio_geometry_vector_map(pbio_geometry_matrix_3x3_t *map, pbio_geometry_xyz output->z = input->x * map->m31 + input->y * map->m32 + input->z * map->m33; } +/** + * Multiplies two 3x3 matrices. + * + * @param [in] a The first 3x3 matrix. + * @param [in] b The second 3x3 matrix. + * @param [out] output The resulting 3x3 matrix after multiplication. + */ +void pbio_geometry_matrix_multiply(pbio_geometry_matrix_3x3_t *a, pbio_geometry_matrix_3x3_t *b, pbio_geometry_matrix_3x3_t *output) { + output->m11 = a->m11 * b->m11 + a->m12 * b->m21 + a->m13 * b->m31; + output->m12 = a->m11 * b->m12 + a->m12 * b->m22 + a->m13 * b->m32; + output->m13 = a->m11 * b->m13 + a->m12 * b->m23 + a->m13 * b->m33; + output->m21 = a->m21 * b->m11 + a->m22 * b->m21 + a->m23 * b->m31; + output->m22 = a->m21 * b->m12 + a->m22 * b->m22 + a->m23 * b->m32; + output->m23 = a->m21 * b->m13 + a->m22 * b->m23 + a->m23 * b->m33; + output->m31 = a->m31 * b->m11 + a->m32 * b->m21 + a->m33 * b->m31; + output->m32 = a->m31 * b->m12 + a->m32 * b->m22 + a->m33 * b->m32; + output->m33 = a->m31 * b->m13 + a->m32 * b->m23 + a->m33 * b->m33; +} + /** * Gets a mapping (a rotation matrix) from two orthogonal base axes. * @@ -216,3 +247,101 @@ pbio_error_t pbio_geometry_map_from_base_axes(pbio_geometry_xyz_t *x_axis, pbio_ return PBIO_SUCCESS; } + +/** + * Computes a rotation matrix for a quaternion. + * + * @param [in] q The quaternion. + * @param [out] R The rotation matrix. + */ +void pbio_geometry_quaternion_to_rotation_matrix(pbio_geometry_quaternion_t *q, pbio_geometry_matrix_3x3_t *R) { + R->m11 = 1 - 2 * (q->q2 * q->q2 + q->q3 * q->q3); + R->m21 = 2 * (q->q1 * q->q2 + q->q3 * q->q4); + R->m31 = 2 * (q->q1 * q->q3 - q->q2 * q->q4); + R->m12 = 2 * (q->q1 * q->q2 - q->q3 * q->q4); + R->m22 = 1 - 2 * (q->q1 * q->q1 + q->q3 * q->q3); + R->m32 = 2 * (q->q2 * q->q3 + q->q1 * q->q4); + R->m13 = 2 * (q->q1 * q->q3 + q->q2 * q->q4); + R->m23 = 2 * (q->q2 * q->q3 - q->q1 * q->q4); + R->m33 = 1 - 2 * (q->q1 * q->q1 + q->q2 * q->q2); +} + +/** + * Computes a quaternion from a gravity unit vector. + * + * @param [in] g The gravity unit vector. + * @param [out] q The resulting quaternion. + */ +void pbio_geometry_quaternion_from_gravity_unit_vector(pbio_geometry_xyz_t *g, pbio_geometry_quaternion_t *q) { + if (g->z >= 0) { + q->q4 = sqrtf((g->z + 1) / 2); + q->q1 = g->y / sqrtf(2 * (g->z + 1)); + q->q2 = -g->x / sqrtf(2 * (g->z + 1)); + q->q3 = 0; + } else { + q->q4 = -g->y / sqrtf(2 * (1 - g->z)); + q->q1 = -sqrtf((1 - g->z) / 2); + q->q2 = 0; + q->q3 = -g->x / sqrtf(2 * (1 - g->z)); + } +} + +/** + * Computes the rate of change of a quaternion given the angular velocity vector. + * + * @param [in] q Quaternion of the current orientation. + * @param [in] angular_velocity The angular velocity vector in the body frame. + * @param [out] dq The rate of change of the quaternion. + */ +void pbio_geometry_quaternion_get_rate_of_change(pbio_geometry_quaternion_t *q, pbio_geometry_xyz_t *angular_velocity, pbio_geometry_quaternion_t *dq) { + + pbio_geometry_xyz_t w = { + .x = pbio_geometry_degrees_to_radians(angular_velocity->x), + .y = pbio_geometry_degrees_to_radians(angular_velocity->y), + .z = pbio_geometry_degrees_to_radians(angular_velocity->z), + }; + + dq->q1 = 0.5f * (w.z * q->q2 - w.y * q->q3 + w.x * q->q4); + dq->q2 = 0.5f * (-w.z * q->q1 + w.x * q->q3 + w.y * q->q4); + dq->q3 = 0.5f * (w.y * q->q1 - w.x * q->q2 + w.z * q->q4); + dq->q4 = 0.5f * (-w.x * q->q1 - w.y * q->q2 - w.z * q->q3); +} + +/** + * Normalizes a quaternion so it has unit length. + * + * @param [inout] q The quaternion to normalize. + */ +void pbio_geometry_quaternion_normalize(pbio_geometry_quaternion_t *q) { + float norm = sqrtf(q->q1 * q->q1 + q->q2 * q->q2 + q->q3 * q->q3 + q->q4 * q->q4); + + if (norm < 0.0001f && norm > -0.0001f) { + return; + } + + q->q1 /= norm; + q->q2 /= norm; + q->q3 /= norm; + q->q4 /= norm; +} + +/** + * Returns the maximum of two floating-point numbers. + * + * @param a The first floating-point number. + * @param b The second floating-point number. + * @return The maximum of the two floating-point numbers. + */ +float pbio_geometry_maxf(float a, float b) { + return a > b ? a : b; +} + +/** + * Returns the absolute value of a floating-point number. + * + * @param a The floating-point number. + * @return The absolute value of the floating-point number. + */ +float pbio_geometry_absf(float a) { + return a < 0 ? -a : a; +} diff --git a/lib/pbio/src/imu.c b/lib/pbio/src/imu.c index ad773f24b..e66090e8f 100644 --- a/lib/pbio/src/imu.c +++ b/lib/pbio/src/imu.c @@ -6,6 +6,8 @@ #include #include +#include + #include #include @@ -17,31 +19,343 @@ #include #include +#include +#include + #if PBIO_CONFIG_IMU +/** + * Driver device. + */ static pbdrv_imu_dev_t *imu_dev; + +/** + * Driver configuration. Contains settings like gyro and accelerometer scale + * from raw units to (uncalibrated) physical units and thresholds that say + * when the hub is stationary to be considered ready for calibration. + */ static pbdrv_imu_config_t *imu_config; -// Cached sensor values that can be read at any time without polling again. -static pbio_geometry_xyz_t angular_velocity; // deg/s, in hub frame, already adjusted for bias. -static pbio_geometry_xyz_t acceleration; // mm/s^2, in hub frame +/** + * Uncalibrated angular velocity in the hub frame. + * + * These are scaled from raw units to degrees per second using only the + * datasheet/hal conversion constant, but otherwise not further adjusted. + */ +static pbio_geometry_xyz_t angular_velocity_uncalibrated; + +/** + * Estimated gyro bias value in degrees per second. + * + * This is a measure for the uncalibrated angular velocity above, averaged over + * time. If specified, the value starts at the last saved user value, then + * updates over time. + */ static pbio_geometry_xyz_t gyro_bias; + +/** + * Calibrated angular velocity in the hub frame degrees per second. + * + * This takes the uncalibrated value above, subtracts the bias estimate, and + * rescales by a user calibration factor to ensure that integrating over one + * full rotation adds up to 360 degrees. + */ +static pbio_geometry_xyz_t angular_velocity_calibrated; + +/** + * Uncalibrated acceleration in the hub frame in mm/s^2. + * + * These are scaled from raw units to mm/s^2 using only the + * datasheet/hal conversion constant, but otherwise not further adjusted. + */ +static pbio_geometry_xyz_t acceleration_uncalibrated; + +/** + * Calibrated acceleration in the hub frame mm/s^2. + * + * This takes the uncalibrated value above, and subtracts a constant user offset + * and scales by a previously determined user factor to normalize to gravity magnitude. + */ +static pbio_geometry_xyz_t acceleration_calibrated; // mm/s^2, in hub frame + +/** + * 1D integrated angular velocity for each body axis. + * + * This is based on integrating the calibrated angular velocity over time, so + * including its bias and adjustments to achieve 360 degrees per rotation. + * + * This is not used for 3D attitude estimation, but serves as a useful way to + * estimate 1D rotations without being effected by accelerometer fusion which + * may leads to unwanted adjustments in applications like balancing robots. + */ static pbio_geometry_xyz_t single_axis_rotation; // deg, in hub frame +/** + * Rotation of the hub with respect to the inertial frame, see R(q) below. + * + * Initialized as the identity quaternion. Updated on first gravity sample. + */ +static pbio_geometry_quaternion_t quaternion = { + .q1 = 0.0f, + .q2 = 0.0f, + .q3 = 0.0f, + .q4 = 1.0f, +}; + +/** + * Flag to indicate if the quaternion has been initialized to the very first + * gravity sample. + */ +static bool quaternion_initialized = false; + +/** + * Rotation of the hub with respect to the inertial frame. + * + * Does *not* use the user application frame. + * + * The matrix R(q) is defined such that it transforms hub body frame vectors to + * vectors in the inertial frame as: + * + * v_inertial = R(q) * v_body + * + * Initialized as the identity matrix. Must match initial value of quaternion. + */ +static pbio_geometry_matrix_3x3_t pbio_imu_rotation = { + .m11 = 1.0f, .m12 = 0.0f, .m13 = 0.0f, + .m21 = 0.0f, .m22 = 1.0f, .m23 = 0.0f, + .m31 = 0.0f, .m32 = 0.0f, .m33 = 1.0f, +}; + + +/** + * The "neutral" base orientation of the hub, describing how it is mounted + * in the robot. All getters (tilt, acceleration, rotation, etc) give results + * relative to this base orientation: + * + * vector_reported = R_base * vector_in_hub_body_frame + * + * Default orientation is identity, hub flat. + */ +static pbio_geometry_matrix_3x3_t pbio_imu_base_orientation = { + .m11 = 1.0f, .m12 = 0.0f, .m13 = 0.0f, + .m21 = 0.0f, .m22 = 1.0f, .m23 = 0.0f, + .m31 = 0.0f, .m32 = 0.0f, .m33 = 1.0f, +}; + +/** + * The heading is defined as follows. + * + * Take the x-axis (after transformation to application frame) and project + * into the inertial frame. Then project onto the horizontal (X-Y) plane. Then + * take the angle between the projection and the x-axis, counterclockwise + * positive. + * + * In practice, this means that when you look at a robot from the top, it is + * the angle that its "forward direction vector" makes with respect to the + * x-axis, even when the robot isn't perfectly flat. + * + */ +static float heading_projection; + +/** + * When the heading_projection flips from 180 to -180 or vice versa, we + * increment or decrement the overal rotation counter to maintain a continuous + * heading. + */ +static int32_t heading_rotations; + + +/** + * Hub calibration settings. Cannot be used until loaded. + */ +static pbio_imu_persistent_settings_t *persistent_settings = NULL; + +/** + * Standard gravity in mm/s^2. + */ +const float standard_gravity = 9806.65f; + +/** + * Applies (newly set) settings to the driver. + */ +static void pbio_imu_apply_pbdrv_settings(pbio_imu_persistent_settings_t *settings) { + + // First occurence of this being called is when pbsys loads or resets + // the settings, so this should never happen. + if (!imu_config) { + return; + } + + imu_config->gyro_stationary_threshold = pbio_int_math_bind(settings->gyro_stationary_threshold / imu_config->gyro_scale, 1, INT16_MAX); + imu_config->accel_stationary_threshold = pbio_int_math_bind(settings->accel_stationary_threshold / imu_config->accel_scale, 1, INT16_MAX); +} + +/** + * Sets default settings. This is called by the storage module if it has to + * erase the settings and reinitialize them, including when a different + * firmware version is detected. + * + * @param [in] settings The loaded settings to apply. + */ +void pbio_imu_set_default_settings(pbio_imu_persistent_settings_t *settings) { + settings->flags = 0; + settings->gyro_stationary_threshold = 2.0f; + settings->accel_stationary_threshold = 2500.0f; + settings->gravity_pos.x = settings->gravity_pos.y = settings->gravity_pos.z = standard_gravity; + settings->gravity_neg.x = settings->gravity_neg.y = settings->gravity_neg.z = -standard_gravity; + settings->angular_velocity_bias_start.x = settings->angular_velocity_bias_start.y = settings->angular_velocity_bias_start.z = 0.0f; + settings->angular_velocity_scale.x = settings->angular_velocity_scale.y = settings->angular_velocity_scale.z = 360.0f; + settings->heading_correction_1d = 360.0f; + pbio_imu_apply_pbdrv_settings(settings); +} + +/** + * Applies settings loaded from storage to this imu module. + * + * @param [in] settings The loaded settings to apply. + */ +void pbio_imu_apply_loaded_settings(pbio_imu_persistent_settings_t *settings) { + // This is called on load, so we can now access the settings directly. + persistent_settings = settings; + + // The saved angular velocity bias only sets the initial value. We still + // update the bias continuously from stationary data. + gyro_bias.x = settings->angular_velocity_bias_start.x; + gyro_bias.y = settings->angular_velocity_bias_start.y; + gyro_bias.z = settings->angular_velocity_bias_start.z; + + pbio_imu_apply_pbdrv_settings(settings); +} + +/** + * Given current orientation matrix, update the heading projection. + * + * This is called from the update loop so we can catch the projection jumping + * across the 180/-180 boundary, and increment or decrement the rotation to + * have a continuous heading. + * + * This is also called when the orientation frame is changed because this sets + * the application x-axis used for the heading projection. + */ +static void update_heading_projection(void) { + + // Transform application x axis back into the hub frame (R_base^T * x_unit). + pbio_geometry_xyz_t x_application = { + .x = pbio_imu_base_orientation.m11, + .y = pbio_imu_base_orientation.m12, + .z = pbio_imu_base_orientation.m13 + }; + + // Transform application x axis into the inertial frame via quaternion matrix. + pbio_geometry_xyz_t x_inertial; + pbio_geometry_vector_map(&pbio_imu_rotation, &x_application, &x_inertial); + + // Project onto the horizontal plane and use atan2 to get the angle. + float heading_now = pbio_geometry_radians_to_degrees(atan2f(-x_inertial.y, x_inertial.x)); + + // Update full rotation counter if the projection jumps across the 180/-180 boundary. + if (heading_now < -90 && heading_projection > 90) { + heading_rotations++; + } else if (heading_now > 90 && heading_projection < -90) { + heading_rotations--; + } + heading_projection = heading_now; +} + // Called by driver to process one frame of unfiltered gyro and accelerometer data. static void pbio_imu_handle_frame_data_func(int16_t *data) { - for (uint8_t i = 0; i < PBIO_ARRAY_SIZE(angular_velocity.values); i++) { + + // Initialize quaternion from first gravity sample as a best-effort estimate. + // From here, fusion will gradually converge the quaternion to the true value. + if (!quaternion_initialized) { + pbio_geometry_xyz_t g = { .x = data[3], .y = data[4], .z = data[5]}; + pbio_error_t err = pbio_geometry_vector_normalize(&g, &g); + if (err != PBIO_SUCCESS) { + // First sample not suited, try again on next sample. + return; + } + pbio_geometry_quaternion_from_gravity_unit_vector(&g, &quaternion); + quaternion_initialized = true; + } + + // Compute current orientation matrix to obtain the current heading. + pbio_geometry_quaternion_to_rotation_matrix(&quaternion, &pbio_imu_rotation); + + // Projects application x-axis into the inertial frame to compute the heading. + update_heading_projection(); + + for (uint8_t i = 0; i < PBIO_ARRAY_SIZE(angular_velocity_calibrated.values); i++) { // Update angular velocity and acceleration cache so user can read them. - angular_velocity.values[i] = data[i] * imu_config->gyro_scale - gyro_bias.values[i]; - acceleration.values[i] = data[i + 3] * imu_config->accel_scale; + angular_velocity_uncalibrated.values[i] = data[i] * imu_config->gyro_scale; + acceleration_uncalibrated.values[i] = data[i + 3] * imu_config->accel_scale; + + // Once settings loaded, maintain calibrated cached values. + if (persistent_settings) { + float acceleration_offset = (persistent_settings->gravity_pos.values[i] + persistent_settings->gravity_neg.values[i]) / 2; + float acceleration_scale = (persistent_settings->gravity_pos.values[i] - persistent_settings->gravity_neg.values[i]) / 2; + acceleration_calibrated.values[i] = (acceleration_uncalibrated.values[i] - acceleration_offset) * standard_gravity / acceleration_scale; + angular_velocity_calibrated.values[i] = (angular_velocity_uncalibrated.values[i] - gyro_bias.values[i]) * 360.0f / persistent_settings->angular_velocity_scale.values[i]; + } else { + acceleration_calibrated.values[i] = acceleration_uncalibrated.values[i]; + angular_velocity_calibrated.values[i] = angular_velocity_uncalibrated.values[i]; + } // Update "heading" on all axes. This is not useful for 3D attitude // estimation, but it allows the user to get a 1D heading even with // the hub mounted at an arbitrary orientation. Such a 1D heading // is numerically more accurate, which is useful in drive base // applications so long as the vehicle drives on a flat surface. - single_axis_rotation.values[i] += angular_velocity.values[i] * imu_config->sample_time; + single_axis_rotation.values[i] += angular_velocity_calibrated.values[i] * imu_config->sample_time; } + + // Estimate for gravity vector based on orientation estimate. + pbio_geometry_xyz_t s = { + .x = pbio_imu_rotation.m31, + .y = pbio_imu_rotation.m32, + .z = pbio_imu_rotation.m33, + }; + + // We would like to adjust the attitude such that the gravity estimate + // converges to the gravity value in the stationary case. If we subtract + // both vectors we get the required direction of changes. This can be + // thought of as a virtual spring between both vectors. This produces a + // moment about the origin, which ultimately simplies to the following, + // which we inject to the attitude integration. + pbio_geometry_xyz_t correction; + pbio_geometry_vector_cross_product(&s, &acceleration_calibrated, &correction); + + // Qualitative measures for how far the current state is from being stationary. + float accl_stationary_error = pbio_geometry_absf(pbio_geometry_vector_norm(&acceleration_calibrated) - standard_gravity); + float gyro_stationary_error = pbio_geometry_absf(pbio_geometry_vector_norm(&angular_velocity_calibrated)); + + // Cut off value below which value is considered stationary enough for fusion. + const float gyro_stationary_min = 10; + const float accl_stationary_min = 150; + + // Measure for being statinonary ranging from 0 (moving) to 1 (moving less than above thresholds). + float stationary_measure = accl_stationary_min / pbio_geometry_maxf(accl_stationary_error, accl_stationary_min) * + gyro_stationary_min / pbio_geometry_maxf(gyro_stationary_error, gyro_stationary_min); + + // The virtual moment would produce motion in that direction, so we can + // simulate that effect by injecting it into the attitude integration, the + // strength of which is based on the stationary measure. It is scaled down + // by the gravity amount since one of the two vectors to produce this has + // units of gravity. Hence if the hub is stationary (measure = 1), and the + // error is 90 degrees (which is unlikely), the correction is at + // most 200 deg/s, but usually much less. + float fusion = -stationary_measure / standard_gravity * 200; + pbio_geometry_xyz_t adjusted_angular_velocity; + adjusted_angular_velocity.x = angular_velocity_calibrated.x + correction.x * fusion; + adjusted_angular_velocity.y = angular_velocity_calibrated.y + correction.y * fusion; + adjusted_angular_velocity.z = angular_velocity_calibrated.z + correction.z * fusion; + + // Update 3D attitude, basic forward integration. + pbio_geometry_quaternion_t dq; + pbio_geometry_quaternion_get_rate_of_change(&quaternion, &adjusted_angular_velocity, &dq); + for (uint8_t i = 0; i < PBIO_ARRAY_SIZE(dq.values); i++) { + quaternion.values[i] += dq.values[i] * imu_config->sample_time; + } + pbio_geometry_quaternion_normalize(&quaternion); } // This counter is a measure for calibration accuracy, roughly equivalent @@ -86,6 +400,16 @@ static void pbio_imu_handle_stationary_data_func(const int32_t *gyro_data_sum, c // Update bias at decreasing rate. gyro_bias.values[i] = gyro_bias.values[i] * (1.0f - weight) + weight * average_now; } + + // If persistent gyro bias has never been set, do so now and request saving. + // This ensures a better starting point for the next boot. We do this only + // once to avoid unnecessary writes on every shutdown. It can be further + // refined with a calibration routine performed by the user. + if (persistent_settings && !(persistent_settings->flags & PBIO_IMU_SETTINGS_FLAGS_GYRO_BIAS_INITIAL_SET) && stationary_counter > 2) { + persistent_settings->angular_velocity_bias_start = gyro_bias; + persistent_settings->flags |= PBIO_IMU_SETTINGS_FLAGS_GYRO_BIAS_INITIAL_SET; + pbsys_storage_request_write(); + } } /** @@ -99,17 +423,6 @@ void pbio_imu_init(void) { pbdrv_imu_set_data_handlers(imu_dev, pbio_imu_handle_frame_data_func, pbio_imu_handle_stationary_data_func); } -/** - * The "neutral" base orientation of the hub, describing how it is mounted - * in the robot. All getters (tilt, acceleration, rotation, etc) give results - * relative to this base orientation. Initial orientation is identity, hub flat. - */ -static pbio_geometry_matrix_3x3_t pbio_orientation_base_orientation = { - .m11 = 1.0f, .m12 = 0.0f, .m13 = 0.0f, - .m21 = 0.0f, .m22 = 1.0f, .m23 = 0.0f, - .m31 = 0.0f, .m32 = 0.0f, .m33 = 1.0f, -}; - /** * Sets the hub base orientation. * @@ -121,13 +434,16 @@ static pbio_geometry_matrix_3x3_t pbio_orientation_base_orientation = { */ pbio_error_t pbio_imu_set_base_orientation(pbio_geometry_xyz_t *front_side_axis, pbio_geometry_xyz_t *top_side_axis) { - pbio_error_t err = pbio_geometry_map_from_base_axes(front_side_axis, top_side_axis, &pbio_orientation_base_orientation); + pbio_error_t err = pbio_geometry_map_from_base_axes(front_side_axis, top_side_axis, &pbio_imu_base_orientation); if (err != PBIO_SUCCESS) { return err; } - pbio_imu_set_heading(0.0f); + // Need to update heading projection since the application axes were changed. + update_heading_projection(); + // Reset offsets such that the new frame starts with zero heading. + pbio_imu_set_heading(0.0f); return PBIO_SUCCESS; } @@ -140,10 +456,17 @@ bool pbio_imu_is_stationary(void) { return pbdrv_imu_is_stationary(imu_dev) && pbio_dcmotor_all_coasting(); } -// Measured rotation of the Z axis in the user frame for exactly one rotation -// of the hub. This will be used to adjust the heading value, which is slightly -// different for each hub. -static float heading_degrees_per_rotation = 360.0f; +/** + * Tests if the acceleration value is within a reasonable range for a stationary hub. + * + * @param [in] value The acceleration value to test. + * @return True if the value is within 10% off from standard gravity. + */ +static bool pbio_imu_stationary_acceleration_out_of_range(float value, bool expect_positive) { + const float expected_value = expect_positive ? standard_gravity : -standard_gravity; + const float absolute_error = value > expected_value ? value - expected_value : expected_value - value; + return absolute_error > standard_gravity / 15; +} /** * Sets the IMU settings. This includes the thresholds that define when the hub @@ -151,59 +474,123 @@ static float heading_degrees_per_rotation = 360.0f; * orientation module automatically recalibrates. Also includes the hub-specific * correction value to get a more accurate heading value. * - * If a value is nan, it is ignored. + * Note: the flags in this setter are not used to reset the flags value in the + * persistent settings but to select which settings are being updated here. * - * @param [in] angular_velocity Angular velocity threshold in deg/s. - * @param [in] acceleration Acceleration threshold in mm/s^2 - * @param [in] heading_correction Measured degrees per full rotation of the hub. - * @returns ::PBIO_ERROR_INVALID_ARG if the heading correction is out of range, - * otherwise ::PBIO_SUCCESS. + * @param [in] new_settings Incomplete set of new settings to apply according to the flags.. + * @returns ::PBIO_ERROR_INVALID_ARG if a value is out of range, otherwise ::PBIO_SUCCESS. */ -pbio_error_t pbio_imu_set_settings(float angular_velocity, float acceleration, float heading_correction) { - if (!isnan(angular_velocity)) { - imu_config->gyro_stationary_threshold = pbio_int_math_bind(angular_velocity / imu_config->gyro_scale, 1, INT16_MAX); +pbio_error_t pbio_imu_set_settings(pbio_imu_persistent_settings_t *new_settings) { + + // Can't set settings if storage not loaded. + if (!persistent_settings) { + return PBIO_ERROR_FAILED; + } + + if (new_settings->flags & PBIO_IMU_SETTINGS_FLAGS_ACCEL_STATIONARY_THRESHOLD_SET) { + persistent_settings->accel_stationary_threshold = new_settings->accel_stationary_threshold; + } + + if (new_settings->flags & PBIO_IMU_SETTINGS_FLAGS_GYRO_STATIONARY_THRESHOLD_SET) { + persistent_settings->gyro_stationary_threshold = new_settings->gyro_stationary_threshold; + } + + for (uint8_t i = 0; i < 3; i++) { + if (new_settings->flags & PBIO_IMU_SETTINGS_FLAGS_GYRO_BIAS_INITIAL_SET) { + persistent_settings->angular_velocity_bias_start.values[i] = new_settings->angular_velocity_bias_start.values[i]; + } + + if (new_settings->flags & PBIO_IMU_SETTINGS_FLAGS_GYRO_SCALE_SET) { + if (new_settings->angular_velocity_scale.values[i] < 350 || new_settings->angular_velocity_scale.values[i] > 370) { + return PBIO_ERROR_INVALID_ARG; + } + persistent_settings->angular_velocity_scale.values[i] = new_settings->angular_velocity_scale.values[i]; + } } - if (!isnan(acceleration)) { - imu_config->accel_stationary_threshold = pbio_int_math_bind(acceleration / imu_config->accel_scale, 1, INT16_MAX); + + if (new_settings->flags & PBIO_IMU_SETTINGS_FLAGS_ACCEL_CALIBRATED) { + if (pbio_imu_stationary_acceleration_out_of_range(new_settings->gravity_pos.x, true) || + pbio_imu_stationary_acceleration_out_of_range(new_settings->gravity_neg.x, false) || + pbio_imu_stationary_acceleration_out_of_range(new_settings->gravity_pos.y, true) || + pbio_imu_stationary_acceleration_out_of_range(new_settings->gravity_neg.y, false) || + pbio_imu_stationary_acceleration_out_of_range(new_settings->gravity_pos.z, true) || + pbio_imu_stationary_acceleration_out_of_range(new_settings->gravity_neg.z, false)) { + return PBIO_ERROR_INVALID_ARG; + } + persistent_settings->gravity_pos = new_settings->gravity_pos; + persistent_settings->gravity_neg = new_settings->gravity_neg; } - if (!isnan(heading_correction)) { - if (heading_correction < 350 || heading_correction > 370) { + + if (new_settings->flags & PBIO_IMU_SETTINGS_FLAGS_HEADING_CORRECTION_1D_SET) { + if (new_settings->heading_correction_1d < 350 || new_settings->heading_correction_1d > 370) { return PBIO_ERROR_INVALID_ARG; } - heading_degrees_per_rotation = heading_correction; + persistent_settings->heading_correction_1d = new_settings->heading_correction_1d; } + + // If any settings were changed, request saving. + if (new_settings->flags) { + persistent_settings->flags |= new_settings->flags; + pbsys_storage_request_write(); + } + + // The persistent settings have now been updated as applicable. Use the + // complete set of settings and apply them to the driver. + pbio_imu_apply_pbdrv_settings(persistent_settings); + return PBIO_SUCCESS; } /** - * Gets the thresholds that define when the hub is stationary. + * Gets the IMU settings * - * @param [out] angular_velocity Angular velocity threshold in deg/s. - * @param [out] acceleration Acceleration threshold in mm/s^2 - * @param [out] heading_correction Measured degrees per full rotation of the hub. + * @param [out] settings Complete set of new settings. + * @returns ::PBIO_ERROR_FAILED if settings not available, otherwise ::PBIO_SUCCESS. */ -void pbio_imu_get_settings(float *angular_velocity, float *acceleration, float *heading_correction) { - *angular_velocity = imu_config->gyro_stationary_threshold * imu_config->gyro_scale; - *acceleration = imu_config->accel_stationary_threshold * imu_config->accel_scale; - *heading_correction = heading_degrees_per_rotation; +pbio_error_t pbio_imu_get_settings(pbio_imu_persistent_settings_t **settings) { + // Can't set settings if storage not loaded. + if (!persistent_settings) { + return PBIO_ERROR_FAILED; + } + *settings = persistent_settings; + return PBIO_SUCCESS; } /** * Gets the cached IMU angular velocity in deg/s, compensated for gyro bias. * * @param [out] values The angular velocity vector. + * @param [in] calibrated Whether to get calibrated or uncalibrated data. */ -void pbio_imu_get_angular_velocity(pbio_geometry_xyz_t *values) { - pbio_geometry_vector_map(&pbio_orientation_base_orientation, &angular_velocity, values); +void pbio_imu_get_angular_velocity(pbio_geometry_xyz_t *values, bool calibrated) { + pbio_geometry_xyz_t *angular_velocity = calibrated ? &angular_velocity_calibrated : &angular_velocity_uncalibrated; + pbio_geometry_vector_map(&pbio_imu_base_orientation, angular_velocity, values); } /** * Gets the cached IMU acceleration in mm/s^2. * + * @param [in] calibrated Whether to use calibrated or uncalibrated data. + * * @param [out] values The acceleration vector. */ -void pbio_imu_get_acceleration(pbio_geometry_xyz_t *values) { - pbio_geometry_vector_map(&pbio_orientation_base_orientation, &acceleration, values); +void pbio_imu_get_acceleration(pbio_geometry_xyz_t *values, bool calibrated) { + pbio_geometry_xyz_t *acceleration = calibrated ? &acceleration_calibrated : &acceleration_uncalibrated; + pbio_geometry_vector_map(&pbio_imu_base_orientation, acceleration, values); +} + +/** + * Gets the vector that is parallel to the acceleration measurement of the stationary case. + * + * @param [out] values The acceleration vector. + */ +void pbio_imu_get_tilt_vector(pbio_geometry_xyz_t *values) { + pbio_geometry_xyz_t direction = { + .x = pbio_imu_rotation.m31, + .y = pbio_imu_rotation.m32, + .z = pbio_imu_rotation.m33, + }; + pbio_geometry_vector_map(&pbio_imu_base_orientation, &direction, values); } /** @@ -213,32 +600,50 @@ void pbio_imu_get_acceleration(pbio_geometry_xyz_t *values) { * * @param [in] axis The axis to project the rotation onto. * @param [out] angle The angle of rotation in degrees. + * @param [in] calibrated Whether to use the adjusted scale (true) or the raw scale (false). * @return ::PBIO_SUCCESS on success, ::PBIO_ERROR_INVALID_ARG if axis has zero length. */ -pbio_error_t pbio_imu_get_single_axis_rotation(pbio_geometry_xyz_t *axis, float *angle) { +pbio_error_t pbio_imu_get_single_axis_rotation(pbio_geometry_xyz_t *axis, float *angle, bool calibrated) { + + // Local copy so we can change it in-place. + pbio_geometry_xyz_t axis_rotation = single_axis_rotation; + if (!calibrated) { + // In this context, calibrated means that the angular velocity values + // were scaled by the user calibration factors before integrating. This + // is done within the update loop since we need it for 3D integration. + // Since this method only returns the separate 1D rotations, we can + // undo this scaling here to get the "uncalibrated" values. + for (uint8_t i = 0; i < PBIO_ARRAY_SIZE(axis_rotation.values); i++) { + axis_rotation.values[i] *= persistent_settings->angular_velocity_scale.values[i] / 360.0f; + } + } // Transform the single axis rotations to the robot frame. - pbio_geometry_xyz_t rotation; - pbio_geometry_vector_map(&pbio_orientation_base_orientation, &single_axis_rotation, &rotation); + pbio_geometry_xyz_t rotation_user; + pbio_geometry_vector_map(&pbio_imu_base_orientation, &axis_rotation, &rotation_user); // Get the requested scalar rotation along the given axis. - return pbio_geometry_vector_project(axis, &rotation, angle); + return pbio_geometry_vector_project(axis, &rotation_user, angle); } /** * Gets which side of a hub points upwards. * + * @param [in] calibrated Whether to use calibrated or uncalibrated data. + * * @return Which side is up. */ -pbio_geometry_side_t pbio_imu_get_up_side(void) { +pbio_geometry_side_t pbio_imu_get_up_side(bool calibrated) { // Up is which side of a unit box intersects the +Z vector first. // So read +Z vector of the inertial frame, in the body frame. // For now, this is the gravity vector. In the future, we can make this // slightly more accurate by using the full IMU orientation. - return pbio_geometry_side_from_vector(&acceleration); + pbio_geometry_xyz_t *acceleration = calibrated ? &acceleration_calibrated : &acceleration_uncalibrated; + return pbio_geometry_side_from_vector(acceleration); } -static float heading_offset = 0; +static float heading_offset_1d = 0; +static float heading_offset_3d = 0; /** * Reads the estimated IMU heading in degrees, accounting for user offset and @@ -246,15 +651,29 @@ static float heading_offset = 0; * * Heading is defined as clockwise positive. * + * @param [in] type The type of heading to get. + * * @return Heading angle in the base frame. */ -float pbio_imu_get_heading(void) { +float pbio_imu_get_heading(pbio_imu_heading_type_t type) { - pbio_geometry_xyz_t heading_mapped; + float correction = (persistent_settings && (persistent_settings->flags & PBIO_IMU_SETTINGS_FLAGS_HEADING_CORRECTION_1D_SET)) ? + // If set, adjust by the user-specified scaling constant. + (360.0f / persistent_settings->heading_correction_1d): + // No (additional) correction. + 1.0f; - pbio_geometry_vector_map(&pbio_orientation_base_orientation, &single_axis_rotation, &heading_mapped); + // 3D. Mapping into user frame is already accounted for in the projection. + if (type == PBIO_IMU_HEADING_TYPE_3D) { + return (heading_rotations * 360.0f + heading_projection) * correction - heading_offset_3d; + } + + // 1D. Map the per-axis integrated rotation to the user frame, then take + // the negative z component as the heading for positive-clockwise convention. + pbio_geometry_xyz_t heading_mapped; + pbio_geometry_vector_map(&pbio_imu_base_orientation, &single_axis_rotation, &heading_mapped); - return -heading_mapped.z * 360.0f / heading_degrees_per_rotation - heading_offset; + return -heading_mapped.z * correction - heading_offset_1d; } /** @@ -266,7 +685,9 @@ float pbio_imu_get_heading(void) { * @param [in] desired_heading The desired heading value. */ void pbio_imu_set_heading(float desired_heading) { - heading_offset = pbio_imu_get_heading() + heading_offset - desired_heading; + heading_rotations = 0; + heading_offset_3d = pbio_imu_get_heading(PBIO_IMU_HEADING_TYPE_3D) + heading_offset_3d - desired_heading; + heading_offset_1d = pbio_imu_get_heading(PBIO_IMU_HEADING_TYPE_1D) + heading_offset_1d - desired_heading; } /** @@ -278,14 +699,15 @@ void pbio_imu_set_heading(float desired_heading) { * * Heading is defined as clockwise positive. * + * @param [in] type Heading type to get. * @param [out] heading The heading angle in control units. * @param [out] heading_rate The heading rate in control units. * @param [in] ctl_steps_per_degree The number of control steps per heading degree. */ -void pbio_imu_get_heading_scaled(pbio_angle_t *heading, int32_t *heading_rate, int32_t ctl_steps_per_degree) { +void pbio_imu_get_heading_scaled(pbio_imu_heading_type_t type, pbio_angle_t *heading, int32_t *heading_rate, int32_t ctl_steps_per_degree) { // Heading in degrees of the robot. - float heading_degrees = pbio_imu_get_heading(); + float heading_degrees = pbio_imu_get_heading(type); // Number of whole rotations in control units (in terms of wheels, not robot). heading->rotations = (int32_t)(heading_degrees / (360000.0f / ctl_steps_per_degree)); @@ -297,8 +719,17 @@ void pbio_imu_get_heading_scaled(pbio_angle_t *heading, int32_t *heading_rate, i // The heading rate can be obtained by a simple scale because it always fits. pbio_geometry_xyz_t angular_rate; - pbio_imu_get_angular_velocity(&angular_rate); + pbio_imu_get_angular_velocity(&angular_rate, true); *heading_rate = (int32_t)(-angular_rate.z * ctl_steps_per_degree); } +/** + * Reads the current rotation matrix. + * + * @param [out] rotation The rotation matrix + */ +void pbio_orientation_imu_get_orientation(pbio_geometry_matrix_3x3_t *rotation) { + *rotation = pbio_imu_rotation; +} + #endif // PBIO_CONFIG_IMU diff --git a/lib/pbio/sys/storage.h b/lib/pbio/sys/storage.h index cd64d73ab..2c9bae2c9 100644 --- a/lib/pbio/sys/storage.h +++ b/lib/pbio/sys/storage.h @@ -19,7 +19,6 @@ pbio_error_t pbsys_storage_set_program_size(uint32_t size); pbio_error_t pbsys_storage_set_program_data(uint32_t offset, const void *data, uint32_t size); void pbsys_storage_get_program_data(pbsys_main_program_t *program); pbsys_storage_settings_t *pbsys_storage_settings_get_settings(void); -void pbsys_storage_request_write(void); #else static inline void pbsys_storage_init(void) { @@ -37,8 +36,6 @@ static inline pbio_error_t pbsys_storage_set_program_data(uint32_t offset, const } static inline void pbsys_storage_get_program_data(pbsys_main_program_t *program) { } -static inline void pbsys_storage_request_write(void) { -} #endif // PBSYS_CONFIG_STORAGE diff --git a/lib/pbio/sys/storage_settings.c b/lib/pbio/sys/storage_settings.c index 90d45fb45..c63c0cbf1 100644 --- a/lib/pbio/sys/storage_settings.c +++ b/lib/pbio/sys/storage_settings.c @@ -14,6 +14,7 @@ #include #include #include +#include #include #include "bluetooth.h" @@ -29,9 +30,7 @@ void pbsys_storage_settings_set_defaults(pbsys_storage_settings_t *settings) { settings->flags |= PBSYS_STORAGE_SETTINGS_FLAGS_BLUETOOTH_ENABLED; #endif #if PBIO_CONFIG_IMU - settings->gyro_stationary_threshold = 3.0f; - settings->accel_stationary_threshold = 2500.0f; - settings->heading_correction = 360.0f; + pbio_imu_set_default_settings(&settings->imu_settings); #endif // PBIO_CONFIG_IMU } @@ -42,32 +41,7 @@ void pbsys_storage_settings_set_defaults(pbsys_storage_settings_t *settings) { */ void pbsys_storage_settings_apply_loaded_settings(pbsys_storage_settings_t *settings) { #if PBIO_CONFIG_IMU - // return value not checked, assumed to be set valid previously - pbio_imu_set_settings( - settings->gyro_stationary_threshold, - settings->accel_stationary_threshold, - settings->heading_correction - ); - #endif // PBIO_CONFIG_IMU -} - -/** - * Copies the configured IMU settings to storage and requests them to be saved. - * - * @param [in] settings Settings to populate. - */ -void pbsys_storage_settings_save_imu_settings(void) { - pbsys_storage_settings_t *settings = pbsys_storage_settings_get_settings(); - if (!settings) { - return; - } - #if PBIO_CONFIG_IMU - pbio_imu_get_settings( - &settings->gyro_stationary_threshold, - &settings->accel_stationary_threshold, - &settings->heading_correction - ); - pbsys_storage_request_write(); + pbio_imu_apply_loaded_settings(&settings->imu_settings); #endif // PBIO_CONFIG_IMU } diff --git a/pybricks/common/pb_type_control.c b/pybricks/common/pb_type_control.c index 662e0b12a..29de973c6 100644 --- a/pybricks/common/pb_type_control.c +++ b/pybricks/common/pb_type_control.c @@ -95,7 +95,7 @@ static mp_obj_t pb_type_Control_limits(size_t n_args, const mp_obj_t *pos_args, torque = pbio_control_settings_get_actuation_limit(&self->control->settings); // If all given values are none, return current values - if (speed_in == mp_const_none && acceleration_in == mp_const_none && torque_in == mp_const_none) { + if (PB_PARSE_ARGS_METHOD_ALL_NONE()) { mp_obj_t ret[] = { mp_obj_new_int(speed), make_acceleration_return_value(acceleration, deceleration), @@ -134,8 +134,7 @@ static mp_obj_t pb_type_Control_pid(size_t n_args, const mp_obj_t *pos_args, mp_ pbio_control_settings_get_pid(&self->control->settings, &kp, &ki, &kd, &integral_deadzone, &integral_change_max); // If all given values are none, return current values - if (kp_in == mp_const_none && ki_in == mp_const_none && kd_in == mp_const_none && - integral_rate_in == mp_const_none) { + if (PB_PARSE_ARGS_METHOD_ALL_NONE()) { mp_obj_t ret[5]; ret[0] = mp_obj_new_int(kp); ret[1] = mp_obj_new_int(ki); @@ -171,7 +170,7 @@ static mp_obj_t pb_type_Control_target_tolerances(size_t n_args, const mp_obj_t pbio_control_settings_get_target_tolerances(&self->control->settings, &speed, &position); // If all given values are none, return current values - if (speed_in == mp_const_none && position_in == mp_const_none) { + if (PB_PARSE_ARGS_METHOD_ALL_NONE()) { mp_obj_t ret[2]; ret[0] = mp_obj_new_int(speed); ret[1] = mp_obj_new_int(position); @@ -202,7 +201,7 @@ static mp_obj_t pb_type_Control_stall_tolerances(size_t n_args, const mp_obj_t * pbio_control_settings_get_stall_tolerances(&self->control->settings, &speed, &time); // If all given values are none, return current values - if (speed_in == mp_const_none && time_in == mp_const_none) { + if (PB_PARSE_ARGS_METHOD_ALL_NONE()) { mp_obj_t ret[2]; ret[0] = mp_obj_new_int(speed); ret[1] = mp_obj_new_int_from_uint(time); diff --git a/pybricks/common/pb_type_imu.c b/pybricks/common/pb_type_imu.c index 1e8f74994..706ef3188 100644 --- a/pybricks/common/pb_type_imu.c +++ b/pybricks/common/pb_type_imu.c @@ -31,8 +31,14 @@ typedef struct _pb_type_imu_obj_t { } pb_type_imu_obj_t; // pybricks._common.IMU.up -static mp_obj_t pb_type_imu_up(mp_obj_t self_in) { - switch (pbio_imu_get_up_side()) { +static mp_obj_t pb_type_imu_up(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, + pb_type_imu_obj_t, self, + PB_ARG_DEFAULT_TRUE(calibrated)); + + (void)self; + + switch (pbio_imu_get_up_side(mp_obj_is_true(calibrated_in))) { default: case PBIO_GEOMETRY_SIDE_FRONT: return MP_OBJ_FROM_PTR(&pb_Side_FRONT_obj); @@ -48,26 +54,36 @@ static mp_obj_t pb_type_imu_up(mp_obj_t self_in) { return MP_OBJ_FROM_PTR(&pb_Side_BOTTOM_obj); } } -MP_DEFINE_CONST_FUN_OBJ_1(pb_type_imu_up_obj, pb_type_imu_up); +static MP_DEFINE_CONST_FUN_OBJ_KW(pb_type_imu_up_obj, 1, pb_type_imu_up); // pybricks._common.IMU.tilt -static mp_obj_t pb_type_imu_tilt(mp_obj_t self_in) { +static mp_obj_t pb_type_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, + pb_type_imu_obj_t, self, + PB_ARG_DEFAULT_TRUE(use_gyro), + PB_ARG_DEFAULT_TRUE(calibrated)); + + (void)self; // Read acceleration in the user frame. pbio_geometry_xyz_t accl; - pbio_imu_get_acceleration(&accl); + if (mp_obj_is_true(use_gyro_in)) { + pbio_imu_get_tilt_vector(&accl); + } else { + pbio_imu_get_acceleration(&accl, mp_obj_is_true(calibrated_in)); + } mp_obj_t tilt[2]; // Pitch float pitch = atan2f(-accl.x, sqrtf(accl.z * accl.z + accl.y * accl.y)); - tilt[0] = mp_obj_new_int_from_float(pitch * 57.296f); + tilt[0] = mp_obj_new_float_from_f(pitch * 57.296f); // Roll float roll = atan2f(accl.y, accl.z); - tilt[1] = mp_obj_new_int_from_float(roll * 57.296f); + tilt[1] = mp_obj_new_float_from_f(roll * 57.296f); return mp_obj_new_tuple(2, tilt); } -MP_DEFINE_CONST_FUN_OBJ_1(pb_type_imu_tilt_obj, pb_type_imu_tilt); +static MP_DEFINE_CONST_FUN_OBJ_KW(pb_type_imu_tilt_obj, 1, pb_type_imu_tilt); static void pb_type_imu_extract_axis(mp_obj_t obj_in, pbio_geometry_xyz_t *vector) { if (!mp_obj_is_type(obj_in, &pb_type_Matrix)) { @@ -86,11 +102,12 @@ static void pb_type_imu_extract_axis(mp_obj_t obj_in, pbio_geometry_xyz_t *vecto static mp_obj_t pb_type_imu_acceleration(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, pb_type_imu_obj_t, self, - PB_ARG_DEFAULT_NONE(axis)); + PB_ARG_DEFAULT_NONE(axis), + PB_ARG_DEFAULT_TRUE(calibrated)); (void)self; pbio_geometry_xyz_t acceleration; - pbio_imu_get_acceleration(&acceleration); + pbio_imu_get_acceleration(&acceleration, mp_obj_is_true(calibrated_in)); // If no axis is specified, return a vector of values. if (axis_in == mp_const_none) { @@ -111,11 +128,12 @@ static MP_DEFINE_CONST_FUN_OBJ_KW(pb_type_imu_acceleration_obj, 1, pb_type_imu_a static mp_obj_t pb_type_imu_angular_velocity(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, pb_type_imu_obj_t, self, - PB_ARG_DEFAULT_NONE(axis)); + PB_ARG_DEFAULT_NONE(axis), + PB_ARG_DEFAULT_TRUE(calibrated)); (void)self; pbio_geometry_xyz_t angular_velocity; - pbio_imu_get_angular_velocity(&angular_velocity); + pbio_imu_get_angular_velocity(&angular_velocity, mp_obj_is_true(calibrated_in)); // If no axis is specified, return a vector of values. if (axis_in == mp_const_none) { @@ -136,7 +154,8 @@ static MP_DEFINE_CONST_FUN_OBJ_KW(pb_type_imu_angular_velocity_obj, 1, pb_type_i static mp_obj_t pb_type_imu_rotation(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, pb_type_imu_obj_t, self, - PB_ARG_DEFAULT_NONE(axis)); + PB_ARG_DEFAULT_NONE(axis), + PB_ARG_DEFAULT_TRUE(calibrated)); (void)self; @@ -145,7 +164,7 @@ static mp_obj_t pb_type_imu_rotation(size_t n_args, const mp_obj_t *pos_args, mp pb_type_imu_extract_axis(axis_in, &axis); float rotation_angle; - pb_assert(pbio_imu_get_single_axis_rotation(&axis, &rotation_angle)); + pb_assert(pbio_imu_get_single_axis_rotation(&axis, &rotation_angle, mp_obj_is_true(calibrated_in))); return mp_obj_new_float_from_f(rotation_angle); } static MP_DEFINE_CONST_FUN_OBJ_KW(pb_type_imu_rotation_obj, 1, pb_type_imu_rotation); @@ -168,45 +187,145 @@ static mp_obj_t pb_type_imu_settings(size_t n_args, const mp_obj_t *pos_args, mp pb_type_imu_obj_t, self, PB_ARG_DEFAULT_NONE(angular_velocity_threshold), PB_ARG_DEFAULT_NONE(acceleration_threshold), + PB_ARG_DEFAULT_NONE(angular_velocity_bias), + PB_ARG_DEFAULT_NONE(angular_velocity_scale), + PB_ARG_DEFAULT_NONE(acceleration_correction), PB_ARG_DEFAULT_NONE(heading_correction)); (void)self; // Return current values if no arguments are given. - if (angular_velocity_threshold_in == mp_const_none && - acceleration_threshold_in == mp_const_none && - heading_correction_in == mp_const_none) { - float angular_velocity; - float acceleration; - float heading_correction; - pbio_imu_get_settings(&angular_velocity, &acceleration, &heading_correction); + if (PB_PARSE_ARGS_METHOD_ALL_NONE()) { + // Raises if not set, so can safely dereference. + pbio_imu_persistent_settings_t *get_settings; + pb_assert(pbio_imu_get_settings(&get_settings)); + + mp_obj_t acceleration_corrections[] = { + mp_obj_new_float_from_f(get_settings->gravity_pos.x), + mp_obj_new_float_from_f(get_settings->gravity_neg.x), + mp_obj_new_float_from_f(get_settings->gravity_pos.y), + mp_obj_new_float_from_f(get_settings->gravity_neg.y), + mp_obj_new_float_from_f(get_settings->gravity_pos.z), + mp_obj_new_float_from_f(get_settings->gravity_neg.z), + }; + + mp_obj_t angular_velocity_bias[] = { + mp_obj_new_float_from_f(get_settings->angular_velocity_bias_start.x), + mp_obj_new_float_from_f(get_settings->angular_velocity_bias_start.y), + mp_obj_new_float_from_f(get_settings->angular_velocity_bias_start.z), + }; + + mp_obj_t angular_velocity_scale[] = { + mp_obj_new_float_from_f(get_settings->angular_velocity_scale.x), + mp_obj_new_float_from_f(get_settings->angular_velocity_scale.y), + mp_obj_new_float_from_f(get_settings->angular_velocity_scale.z), + }; + mp_obj_t ret[] = { - mp_obj_new_float_from_f(angular_velocity), - mp_obj_new_float_from_f(acceleration), - mp_obj_new_float_from_f(heading_correction), + mp_obj_new_float_from_f(get_settings->gyro_stationary_threshold), + mp_obj_new_float_from_f(get_settings->accel_stationary_threshold), + mp_obj_new_tuple(MP_ARRAY_SIZE(angular_velocity_bias), angular_velocity_bias), + mp_obj_new_tuple(MP_ARRAY_SIZE(angular_velocity_scale), angular_velocity_scale), + mp_obj_new_tuple(MP_ARRAY_SIZE(acceleration_corrections), acceleration_corrections), + mp_obj_new_float_from_f(get_settings->heading_correction_1d), }; return mp_obj_new_tuple(MP_ARRAY_SIZE(ret), ret); } - // Otherwise set new values, only if given. - pb_assert(pbio_imu_set_settings( - angular_velocity_threshold_in == mp_const_none ? NAN : mp_obj_get_float(angular_velocity_threshold_in), - acceleration_threshold_in == mp_const_none ? NAN : mp_obj_get_float(acceleration_threshold_in), - heading_correction_in == mp_const_none ? NAN : mp_obj_get_float(heading_correction_in) - )); + // Apply new settings, using flags to indicate which should be updated. + pbio_imu_persistent_settings_t set_settings = { 0 }; + if (angular_velocity_threshold_in != mp_const_none) { + set_settings.flags |= PBIO_IMU_SETTINGS_FLAGS_GYRO_STATIONARY_THRESHOLD_SET; + set_settings.gyro_stationary_threshold = mp_obj_get_float(angular_velocity_threshold_in); + } + + if (acceleration_threshold_in != mp_const_none) { + set_settings.flags |= PBIO_IMU_SETTINGS_FLAGS_ACCEL_STATIONARY_THRESHOLD_SET; + set_settings.accel_stationary_threshold = mp_obj_get_float(acceleration_threshold_in); + } + + if (angular_velocity_bias_in != mp_const_none) { + mp_obj_t *bias; + size_t size; + mp_obj_get_array(angular_velocity_bias_in, &size, &bias); + if (size != 3) { + mp_raise_ValueError(MP_ERROR_TEXT("Angular velocity bias must be a 3-element tuple.")); + } + set_settings.angular_velocity_bias_start.x = mp_obj_get_float(bias[0]); + set_settings.angular_velocity_bias_start.y = mp_obj_get_float(bias[1]); + set_settings.angular_velocity_bias_start.z = mp_obj_get_float(bias[2]); + set_settings.flags |= PBIO_IMU_SETTINGS_FLAGS_GYRO_BIAS_INITIAL_SET; + } + + if (angular_velocity_scale_in != mp_const_none) { + mp_obj_t *scale; + size_t size; + mp_obj_get_array(angular_velocity_scale_in, &size, &scale); + if (size != 3) { + mp_raise_ValueError(MP_ERROR_TEXT("Angular velocity scale must be a 3-element tuple.")); + } + set_settings.angular_velocity_scale.x = mp_obj_get_float(scale[0]); + set_settings.angular_velocity_scale.y = mp_obj_get_float(scale[1]); + set_settings.angular_velocity_scale.z = mp_obj_get_float(scale[2]); + set_settings.flags |= PBIO_IMU_SETTINGS_FLAGS_GYRO_SCALE_SET; + } + + if (acceleration_correction_in != mp_const_none) { + mp_obj_t *gravity; + size_t size; + mp_obj_get_array(acceleration_correction_in, &size, &gravity); + if (size != 6) { + mp_raise_ValueError(MP_ERROR_TEXT("Acceleration correction must be a 6-element tuple.")); + } + set_settings.flags |= PBIO_IMU_SETTINGS_FLAGS_ACCEL_CALIBRATED; + set_settings.gravity_pos.x = mp_obj_get_float(gravity[0]); + set_settings.gravity_neg.x = mp_obj_get_float(gravity[1]); + set_settings.gravity_pos.y = mp_obj_get_float(gravity[2]); + set_settings.gravity_neg.y = mp_obj_get_float(gravity[3]); + set_settings.gravity_pos.z = mp_obj_get_float(gravity[4]); + set_settings.gravity_neg.z = mp_obj_get_float(gravity[5]); + } + + if (heading_correction_in != mp_const_none) { + set_settings.flags |= PBIO_IMU_SETTINGS_FLAGS_HEADING_CORRECTION_1D_SET; + set_settings.heading_correction_1d = mp_obj_get_float(heading_correction_in); + } + + pb_assert(pbio_imu_set_settings(&set_settings)); - // Request that changed settings are saved on shutdown. - pbsys_storage_settings_save_imu_settings(); return mp_const_none; } static MP_DEFINE_CONST_FUN_OBJ_KW(pb_type_imu_settings_obj, 1, pb_type_imu_settings); // pybricks._common.IMU.heading -static mp_obj_t pb_type_imu_heading(mp_obj_t self_in) { - (void)self_in; - return mp_obj_new_float(pbio_imu_get_heading()); +static mp_obj_t pb_type_imu_heading(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, + pb_type_imu_obj_t, self, + PB_ARG_DEFAULT_NONE(heading_type)); + + (void)self; + pbio_imu_heading_type_t type = heading_type_in == MP_OBJ_NEW_QSTR(MP_QSTR_3D) ? + PBIO_IMU_HEADING_TYPE_3D : PBIO_IMU_HEADING_TYPE_1D; + + return mp_obj_new_float(pbio_imu_get_heading(type)); + ; } -MP_DEFINE_CONST_FUN_OBJ_1(pb_type_imu_heading_obj, pb_type_imu_heading); +static MP_DEFINE_CONST_FUN_OBJ_KW(pb_type_imu_heading_obj, 1, pb_type_imu_heading); + +// pybricks._common.IMU.orientation +STATIC mp_obj_t common_IMU_orientation(mp_obj_t self_in) { + + // Make matrix. REVISIT: Dedicated call from orientation matrix. + pb_type_Matrix_obj_t *matrix = MP_OBJ_TO_PTR(pb_type_Matrix_make_bitmap(3, 3, 1.0f, 0)); + + pbio_geometry_matrix_3x3_t orientation; + pbio_orientation_imu_get_orientation(&orientation); + + memcpy(matrix->data, orientation.values, sizeof(orientation.values)); + + return MP_OBJ_FROM_PTR(matrix); +} +MP_DEFINE_CONST_FUN_OBJ_1(common_IMU_orientation_obj, common_IMU_orientation); // pybricks._common.IMU.reset_heading static mp_obj_t pb_type_imu_reset_heading(size_t n_args, const mp_obj_t *pos_args, mp_map_t *kw_args) { @@ -225,28 +344,6 @@ static mp_obj_t pb_type_imu_reset_heading(size_t n_args, const mp_obj_t *pos_arg } static MP_DEFINE_CONST_FUN_OBJ_KW(pb_type_imu_reset_heading_obj, 1, pb_type_imu_reset_heading); -// pybricks._common.IMU.update_heading_correction -static mp_obj_t pb_type_imu_update_heading_correction(mp_obj_t self_in) { - pb_type_imu_obj_t *self = MP_OBJ_TO_PTR(self_in); - pb_module_tools_assert_blocking(); - - // Disable stop button and cache original setting to restore later. - pbio_button_flags_t stop_button = pbsys_program_stop_get_buttons(); - - nlr_buf_t nlr; - if (nlr_push(&nlr) == 0) { - mp_obj_t func = pb_function_import_helper(MP_QSTR__hub_extra, MP_QSTR_imu_update_heading_correction); - mp_call_function_1(func, self->hub); - pbsys_program_stop_set_buttons(stop_button); - nlr_pop(); - } else { - pbsys_program_stop_set_buttons(stop_button); - nlr_jump(nlr.ret_val); - } - return mp_const_none; -} -MP_DEFINE_CONST_FUN_OBJ_1(pb_type_imu_update_heading_correction_obj, pb_type_imu_update_heading_correction); - // dir(pybricks.common.IMU) static const mp_rom_map_elem_t pb_type_imu_locals_dict_table[] = { { MP_ROM_QSTR(MP_QSTR_acceleration), MP_ROM_PTR(&pb_type_imu_acceleration_obj) }, @@ -259,7 +356,7 @@ static const mp_rom_map_elem_t pb_type_imu_locals_dict_table[] = { { MP_ROM_QSTR(MP_QSTR_stationary), MP_ROM_PTR(&pb_type_imu_stationary_obj) }, { MP_ROM_QSTR(MP_QSTR_tilt), MP_ROM_PTR(&pb_type_imu_tilt_obj) }, { MP_ROM_QSTR(MP_QSTR_up), MP_ROM_PTR(&pb_type_imu_up_obj) }, - { MP_ROM_QSTR(MP_QSTR_update_heading_correction), MP_ROM_PTR(&pb_type_imu_update_heading_correction_obj)}, + { MP_ROM_QSTR(MP_QSTR_orientation), MP_ROM_PTR(&common_IMU_orientation_obj) }, }; static MP_DEFINE_CONST_DICT(pb_type_imu_locals_dict, pb_type_imu_locals_dict_table); @@ -283,7 +380,7 @@ mp_obj_t pb_type_IMU_obj_new(mp_obj_t hub_in, mp_obj_t top_side_axis_in, mp_obj_ pbio_geometry_xyz_t top_side_axis; pb_type_imu_extract_axis(top_side_axis_in, &top_side_axis); - pbio_imu_set_base_orientation(&front_side_axis, &top_side_axis); + pb_assert(pbio_imu_set_base_orientation(&front_side_axis, &top_side_axis)); // Return singleton instance. singleton_imu_obj.hub = hub_in; diff --git a/pybricks/common/pb_type_lightmatrix.c b/pybricks/common/pb_type_lightmatrix.c index b8e1b485d..73f5ad97c 100644 --- a/pybricks/common/pb_type_lightmatrix.c +++ b/pybricks/common/pb_type_lightmatrix.c @@ -283,7 +283,7 @@ static mp_obj_t common_LightMatrix_text(size_t n_args, const mp_obj_t *pos_args, if (pb_module_tools_run_loop_is_active()) { if (self->async_text_method == MP_OBJ_NULL) { - self->async_text_method = pb_function_import_helper(MP_QSTR__hub_extra, MP_QSTR_light_matrix_text_async); + self->async_text_method = pb_function_import_helper(MP_QSTR__light_matrix, MP_QSTR_light_matrix_text_async); } mp_obj_t args[] = { MP_OBJ_FROM_PTR(self), diff --git a/pybricks/robotics/pb_type_drivebase.c b/pybricks/robotics/pb_type_drivebase.c index e89a61290..e94a75388 100644 --- a/pybricks/robotics/pb_type_drivebase.c +++ b/pybricks/robotics/pb_type_drivebase.c @@ -346,11 +346,7 @@ static mp_obj_t pb_type_DriveBase_settings(size_t n_args, const mp_obj_t *pos_ar &turn_rate, &turn_acceleration, &turn_deceleration); // If all given values are none, return current values - if (straight_speed_in == mp_const_none && - straight_acceleration_in == mp_const_none && - turn_rate_in == mp_const_none && - turn_acceleration_in == mp_const_none - ) { + if (PB_PARSE_ARGS_METHOD_ALL_NONE()) { mp_obj_t ret[] = { mp_obj_new_int(straight_speed), make_acceleration_return_value(straight_acceleration, straight_deceleration), @@ -381,7 +377,19 @@ static mp_obj_t pb_type_DriveBase_use_gyro(size_t n_args, const mp_obj_t *pos_ar PB_PARSE_ARGS_METHOD(n_args, pos_args, kw_args, pb_type_DriveBase_obj_t, self, PB_ARG_REQUIRED(use_gyro)); - pb_assert(pbio_drivebase_set_use_gyro(self->db, mp_obj_is_true(use_gyro_in))); + + pbio_imu_heading_type_t type = PBIO_IMU_HEADING_TYPE_NONE; + if (mp_obj_is_true(use_gyro_in)) { + type = PBIO_IMU_HEADING_TYPE_1D; + } + + // Allows testing of 3D heading calculation before it becomes the default + // in a future release. + if (use_gyro_in == MP_OBJ_NEW_QSTR(MP_QSTR_3D)) { + type = PBIO_IMU_HEADING_TYPE_3D; + } + + pb_assert(pbio_drivebase_set_use_gyro(self->db, type)); return mp_const_none; } static MP_DEFINE_CONST_FUN_OBJ_KW(pb_type_DriveBase_use_gyro_obj, 1, pb_type_DriveBase_use_gyro); diff --git a/pybricks/util_mp/pb_kwarg_helper.h b/pybricks/util_mp/pb_kwarg_helper.h index 67447e29d..9de0ed3d7 100644 --- a/pybricks/util_mp/pb_kwarg_helper.h +++ b/pybricks/util_mp/pb_kwarg_helper.h @@ -101,4 +101,7 @@ // Optional keyword argument with default None value #define PB_ARG_DEFAULT_NONE(name)(name, MP_ARG_OBJ, {.u_rom_obj = MP_ROM_NONE}) +// Test if all parsed arguments are None. +#define PB_PARSE_ARGS_METHOD_ALL_NONE() (pb_obj_parsed_args_all_none(parsed_args, MP_ARRAY_SIZE(parsed_args))) + #endif // PYBRICKS_INCLUDED_PBKWARG_H diff --git a/pybricks/util_mp/pb_obj_helper.c b/pybricks/util_mp/pb_obj_helper.c index c574c2e7b..f5ad41e3f 100644 --- a/pybricks/util_mp/pb_obj_helper.c +++ b/pybricks/util_mp/pb_obj_helper.c @@ -244,3 +244,18 @@ mp_obj_t pb_function_import_helper(qstr module_name, qstr function_name) { } return dest[0]; } + +/** + * Tests that all parsed arguments are None. + * + * @param parsed_args [in] The parsed arguments. + * @param num_parsed_args [in] The number of parsed arguments. + */ +bool pb_obj_parsed_args_all_none(mp_arg_val_t *parsed_args, size_t num_parsed_args) { + for (size_t i = 0; i < num_parsed_args; i++) { + if (parsed_args[i].u_obj != mp_const_none) { + return false; + } + } + return true; +} diff --git a/pybricks/util_mp/pb_obj_helper.h b/pybricks/util_mp/pb_obj_helper.h index 940fc4fb1..339e72286 100644 --- a/pybricks/util_mp/pb_obj_helper.h +++ b/pybricks/util_mp/pb_obj_helper.h @@ -10,6 +10,7 @@ #include #include "py/obj.h" +#include "py/runtime.h" // like mp_obj_get_int() but also allows float #if MICROPY_PY_BUILTINS_FLOAT @@ -68,4 +69,6 @@ void pb_attribute_handler(mp_obj_t self_in, qstr attr, mp_obj_t *dest); mp_obj_t pb_function_import_helper(qstr module_name, qstr function_name); +bool pb_obj_parsed_args_all_none(mp_arg_val_t *parsed_args, size_t num_parsed_args); + #endif // PYBRICKS_INCLUDED_PBOBJ_H