From 52146cfc2b18f82d85a282b02ea78eb1df5156ff Mon Sep 17 00:00:00 2001 From: Laurens Valk Date: Sat, 1 Apr 2023 12:32:42 +0200 Subject: [PATCH] pbio/imu: Rename from pbio/orientation_imu. Now that all pure orientation functions have been moved to pbio/geometry only the orientation_imu functions remain. So we can just rename it imu, and make it an independent module that can be enabled just like other hardware modules. --- bricks/_common/sources.mk | 2 +- lib/pbio/doc/doxygen.conf | 2 +- lib/pbio/include/pbio/orientation.h | 42 +++++++++---------- lib/pbio/platform/city_hub/pbioconfig.h | 2 +- lib/pbio/platform/essential_hub/pbioconfig.h | 2 +- lib/pbio/platform/ev3dev_stretch/pbioconfig.h | 2 +- lib/pbio/platform/move_hub/pbioconfig.h | 2 +- lib/pbio/platform/nxt/pbioconfig.h | 2 +- lib/pbio/platform/prime_hub/pbioconfig.h | 2 +- lib/pbio/platform/technic_hub/pbioconfig.h | 2 +- lib/pbio/platform/test/pbioconfig.h | 2 +- lib/pbio/platform/virtual_hub/pbioconfig.h | 2 +- lib/pbio/src/drivebase.c | 2 +- lib/pbio/src/{orientation.c => imu.c} | 28 ++++++------- lib/pbio/src/main.c | 2 +- pybricks/common/pb_type_imu.c | 18 ++++---- pybricks/robotics/pb_type_drivebase.c | 2 +- 17 files changed, 58 insertions(+), 58 deletions(-) rename lib/pbio/src/{orientation.c => imu.c} (89%) diff --git a/bricks/_common/sources.mk b/bricks/_common/sources.mk index 919aa98ae..efcae136e 100644 --- a/bricks/_common/sources.mk +++ b/bricks/_common/sources.mk @@ -200,6 +200,7 @@ PBIO_SRC_C = $(addprefix lib/pbio/,\ src/drivebase.c \ src/error.c \ src/geometry.c \ + src/imu.c \ src/int_math.c \ src/integrator.c \ src/iodev.c \ @@ -211,7 +212,6 @@ PBIO_SRC_C = $(addprefix lib/pbio/,\ src/motor_process.c \ src/motor/servo_settings.c \ src/observer.c \ - src/orientation.c \ src/parent.c \ src/protocol/lwp3.c \ src/protocol/nus.c \ diff --git a/lib/pbio/doc/doxygen.conf b/lib/pbio/doc/doxygen.conf index ef381e3db..658594ede 100644 --- a/lib/pbio/doc/doxygen.conf +++ b/lib/pbio/doc/doxygen.conf @@ -2133,7 +2133,7 @@ PREDEFINED = \ PBIO_CONFIG_LIGHT=1 \ PBIO_CONFIG_MOTOR_PROCESS=1 \ PBIO_CONFIG_NUM_DRIVEBASES=1 \ - PBIO_CONFIG_ORIENTATION_IMU=1 \ + PBIO_CONFIG_IMU=1 \ PBIO_CONFIG_SERVO=1 \ PBIO_CONFIG_SERVO_NUM_DEV=4 \ PBIO_CONFIG_TACHO=1 \ diff --git a/lib/pbio/include/pbio/orientation.h b/lib/pbio/include/pbio/orientation.h index 1ece544f7..099f9c5bf 100644 --- a/lib/pbio/include/pbio/orientation.h +++ b/lib/pbio/include/pbio/orientation.h @@ -20,65 +20,65 @@ #include -#if PBIO_CONFIG_ORIENTATION_IMU +#if PBIO_CONFIG_IMU -void pbio_orientation_imu_init(void); +void pbio_imu_init(void); pbio_error_t pbio_orientation_set_base_orientation(pbio_geometry_xyz_t *x_axis, pbio_geometry_xyz_t *z_axis); -bool pbio_orientation_imu_is_stationary(void); +bool pbio_imu_is_stationary(void); -void pbio_orientation_imu_set_stationary_thresholds(float angular_velocity, float acceleration); +void pbio_imu_set_stationary_thresholds(float angular_velocity, float acceleration); -void pbio_orientation_imu_get_angular_velocity(pbio_geometry_xyz_t *values); +void pbio_imu_get_angular_velocity(pbio_geometry_xyz_t *values); -void pbio_orientation_imu_get_acceleration(pbio_geometry_xyz_t *values); +void pbio_imu_get_acceleration(pbio_geometry_xyz_t *values); -pbio_geometry_side_t pbio_orientation_imu_get_up_side(void); +pbio_geometry_side_t pbio_imu_get_up_side(void); -float pbio_orientation_imu_get_heading(void); +float pbio_imu_get_heading(void); -void pbio_orientation_imu_set_heading(float desired_heading); +void pbio_imu_set_heading(float desired_heading); -void pbio_orientation_imu_get_heading_scaled(pbio_angle_t *heading, int32_t ctl_steps_per_degree); +void pbio_imu_get_heading_scaled(pbio_angle_t *heading, int32_t ctl_steps_per_degree); -#else // PBIO_CONFIG_ORIENTATION_IMU +#else // PBIO_CONFIG_IMU -static inline void pbio_orientation_imu_init(void) { +static inline void pbio_imu_init(void) { } static inline pbio_error_t pbio_orientation_set_base_orientation(pbio_geometry_xyz_t *x_axis, pbio_geometry_xyz_t *z_axis) { return PBIO_ERROR_NOT_IMPLEMENTED; } -static inline bool pbio_orientation_imu_is_stationary(void) { +static inline bool pbio_imu_is_stationary(void) { return false; } -static inline void pbio_orientation_imu_set_stationary_thresholds(float angular_velocity, float acceleration) { +static inline void pbio_imu_set_stationary_thresholds(float angular_velocity, float acceleration) { } -static inline void pbio_orientation_imu_get_angular_velocity(pbio_geometry_xyz_t *values) { +static inline void pbio_imu_get_angular_velocity(pbio_geometry_xyz_t *values) { } -static inline void pbio_orientation_imu_get_acceleration(pbio_geometry_xyz_t *values) { +static inline void pbio_imu_get_acceleration(pbio_geometry_xyz_t *values) { } -static inline pbio_geometry_side_t pbio_orientation_imu_get_up_side(void) { +static inline pbio_geometry_side_t pbio_imu_get_up_side(void) { return PBIO_GEOMETRY_SIDE_TOP; } -static inline float pbio_orientation_imu_get_heading(void) { +static inline float pbio_imu_get_heading(void) { return 0.0f; } -static inline void pbio_orientation_imu_set_heading(float desired_heading) { +static inline void pbio_imu_set_heading(float desired_heading) { } -static inline void pbio_orientation_imu_get_heading_scaled(pbio_angle_t *heading, int32_t ctl_steps_per_degree) { +static inline void pbio_imu_get_heading_scaled(pbio_angle_t *heading, int32_t ctl_steps_per_degree) { } -#endif // PBIO_CONFIG_ORIENTATION_IMU +#endif // PBIO_CONFIG_IMU #endif // _PBIO_ORIENTATION_H_ diff --git a/lib/pbio/platform/city_hub/pbioconfig.h b/lib/pbio/platform/city_hub/pbioconfig.h index 4e5749a76..257ae74cb 100644 --- a/lib/pbio/platform/city_hub/pbioconfig.h +++ b/lib/pbio/platform/city_hub/pbioconfig.h @@ -5,10 +5,10 @@ #define PBIO_CONFIG_DCMOTOR (1) #define PBIO_CONFIG_DCMOTOR_NUM_DEV (2) #define PBIO_CONFIG_DRIVEBASE_SPIKE (0) +#define PBIO_CONFIG_IMU (0) #define PBIO_CONFIG_LIGHT (1) #define PBIO_CONFIG_LOGGER (1) #define PBIO_CONFIG_MOTOR_PROCESS (1) -#define PBIO_CONFIG_ORIENTATION_IMU (0) #define PBIO_CONFIG_SERVO (1) #define PBIO_CONFIG_SERVO_NUM_DEV (2) #define PBIO_CONFIG_SERVO_EV3_NXT (0) diff --git a/lib/pbio/platform/essential_hub/pbioconfig.h b/lib/pbio/platform/essential_hub/pbioconfig.h index b4080b2fd..7b46df4e6 100644 --- a/lib/pbio/platform/essential_hub/pbioconfig.h +++ b/lib/pbio/platform/essential_hub/pbioconfig.h @@ -5,11 +5,11 @@ #define PBIO_CONFIG_DCMOTOR (1) #define PBIO_CONFIG_DCMOTOR_NUM_DEV (2) #define PBIO_CONFIG_DRIVEBASE_SPIKE (1) +#define PBIO_CONFIG_IMU (1) #define PBIO_CONFIG_LIGHT (1) #define PBIO_CONFIG_LOGGER (1) #define PBIO_CONFIG_LIGHT_MATRIX (0) #define PBIO_CONFIG_MOTOR_PROCESS (1) -#define PBIO_CONFIG_ORIENTATION_IMU (1) #define PBIO_CONFIG_SERVO (1) #define PBIO_CONFIG_SERVO_NUM_DEV (2) #define PBIO_CONFIG_SERVO_EV3_NXT (0) diff --git a/lib/pbio/platform/ev3dev_stretch/pbioconfig.h b/lib/pbio/platform/ev3dev_stretch/pbioconfig.h index 2fd83c83d..b6fccf011 100644 --- a/lib/pbio/platform/ev3dev_stretch/pbioconfig.h +++ b/lib/pbio/platform/ev3dev_stretch/pbioconfig.h @@ -6,11 +6,11 @@ #define PBIO_CONFIG_DCMOTOR_NUM_DEV (4) #define PBIO_CONFIG_DRIVEBASE_SPIKE (0) #define PBIO_CONFIG_EV3_INPUT_DEVICE (1) +#define PBIO_CONFIG_IMU (0) #define PBIO_CONFIG_LIGHT (1) #define PBIO_CONFIG_LOGGER (1) #define PBIO_CONFIG_SERIAL (1) #define PBIO_CONFIG_MOTOR_PROCESS (1) -#define PBIO_CONFIG_ORIENTATION_IMU (0) #define PBIO_CONFIG_SERVO (1) #define PBIO_CONFIG_SERVO_NUM_DEV (4) #define PBIO_CONFIG_SERVO_EV3_NXT (1) diff --git a/lib/pbio/platform/move_hub/pbioconfig.h b/lib/pbio/platform/move_hub/pbioconfig.h index 5655c968f..d91e6ea49 100644 --- a/lib/pbio/platform/move_hub/pbioconfig.h +++ b/lib/pbio/platform/move_hub/pbioconfig.h @@ -5,10 +5,10 @@ #define PBIO_CONFIG_DCMOTOR (1) #define PBIO_CONFIG_DCMOTOR_NUM_DEV (4) #define PBIO_CONFIG_DRIVEBASE_SPIKE (0) +#define PBIO_CONFIG_IMU (0) #define PBIO_CONFIG_LIGHT (1) #define PBIO_CONFIG_LOGGER (0) #define PBIO_CONFIG_MOTOR_PROCESS (1) -#define PBIO_CONFIG_ORIENTATION_IMU (0) #define PBIO_CONFIG_SERVO (1) #define PBIO_CONFIG_SERVO_NUM_DEV (4) #define PBIO_CONFIG_SERVO_EV3_NXT (0) diff --git a/lib/pbio/platform/nxt/pbioconfig.h b/lib/pbio/platform/nxt/pbioconfig.h index b750a4b0a..7eb2d3686 100644 --- a/lib/pbio/platform/nxt/pbioconfig.h +++ b/lib/pbio/platform/nxt/pbioconfig.h @@ -5,10 +5,10 @@ #define PBIO_CONFIG_DCMOTOR (1) #define PBIO_CONFIG_DCMOTOR_NUM_DEV (3) #define PBIO_CONFIG_DRIVEBASE_SPIKE (0) +#define PBIO_CONFIG_IMU (0) #define PBIO_CONFIG_LIGHT (0) #define PBIO_CONFIG_LOGGER (1) #define PBIO_CONFIG_MOTOR_PROCESS (1) -#define PBIO_CONFIG_ORIENTATION_IMU (0) #define PBIO_CONFIG_SERVO (1) #define PBIO_CONFIG_SERVO_NUM_DEV (3) #define PBIO_CONFIG_SERVO_EV3_NXT (1) diff --git a/lib/pbio/platform/prime_hub/pbioconfig.h b/lib/pbio/platform/prime_hub/pbioconfig.h index 75f8452be..98bd7a486 100644 --- a/lib/pbio/platform/prime_hub/pbioconfig.h +++ b/lib/pbio/platform/prime_hub/pbioconfig.h @@ -5,11 +5,11 @@ #define PBIO_CONFIG_DCMOTOR (1) #define PBIO_CONFIG_DCMOTOR_NUM_DEV (6) #define PBIO_CONFIG_DRIVEBASE_SPIKE (1) +#define PBIO_CONFIG_IMU (1) #define PBIO_CONFIG_LIGHT (1) #define PBIO_CONFIG_LOGGER (1) #define PBIO_CONFIG_LIGHT_MATRIX (1) #define PBIO_CONFIG_MOTOR_PROCESS (1) -#define PBIO_CONFIG_ORIENTATION_IMU (1) #define PBIO_CONFIG_SERVO (1) #define PBIO_CONFIG_SERVO_NUM_DEV (6) #define PBIO_CONFIG_SERVO_EV3_NXT (0) diff --git a/lib/pbio/platform/technic_hub/pbioconfig.h b/lib/pbio/platform/technic_hub/pbioconfig.h index 531013a70..4ae6d66ef 100644 --- a/lib/pbio/platform/technic_hub/pbioconfig.h +++ b/lib/pbio/platform/technic_hub/pbioconfig.h @@ -5,10 +5,10 @@ #define PBIO_CONFIG_DCMOTOR (1) #define PBIO_CONFIG_DCMOTOR_NUM_DEV (4) #define PBIO_CONFIG_DRIVEBASE_SPIKE (0) +#define PBIO_CONFIG_IMU (1) #define PBIO_CONFIG_LIGHT (1) #define PBIO_CONFIG_LOGGER (1) #define PBIO_CONFIG_MOTOR_PROCESS (1) -#define PBIO_CONFIG_ORIENTATION_IMU (1) #define PBIO_CONFIG_SERVO (1) #define PBIO_CONFIG_SERVO_NUM_DEV (4) #define PBIO_CONFIG_SERVO_EV3_NXT (0) diff --git a/lib/pbio/platform/test/pbioconfig.h b/lib/pbio/platform/test/pbioconfig.h index 0958e4728..cd7210fea 100644 --- a/lib/pbio/platform/test/pbioconfig.h +++ b/lib/pbio/platform/test/pbioconfig.h @@ -3,6 +3,7 @@ #define PBIO_CONFIG_DCMOTOR (1) #define PBIO_CONFIG_DCMOTOR_NUM_DEV (6) #define PBIO_CONFIG_DRIVEBASE_SPIKE (0) +#define PBIO_CONFIG_IMU (0) #define PBIO_CONFIG_LIGHT (1) #define PBIO_CONFIG_LOGGER (1) @@ -10,7 +11,6 @@ #define PBIO_CONFIG_MOTOR_PROCESS (1) #define PBIO_CONFIG_MOTOR_PROCESS_AUTO_START (0) -#define PBIO_CONFIG_ORIENTATION_IMU (0) #define PBIO_CONFIG_SERVO (1) #define PBIO_CONFIG_SERVO_NUM_DEV (6) #define PBIO_CONFIG_SERVO_EV3_NXT (1) diff --git a/lib/pbio/platform/virtual_hub/pbioconfig.h b/lib/pbio/platform/virtual_hub/pbioconfig.h index 293e2a779..45400ed89 100644 --- a/lib/pbio/platform/virtual_hub/pbioconfig.h +++ b/lib/pbio/platform/virtual_hub/pbioconfig.h @@ -9,7 +9,7 @@ #define PBIO_CONFIG_LOGGER (1) #define PBIO_CONFIG_LIGHT_MATRIX (0) #define PBIO_CONFIG_MOTOR_PROCESS (1) -#define PBIO_CONFIG_ORIENTATION_IMU (0) +#define PBIO_CONFIG_IMU (0) #define PBIO_CONFIG_SERVO (1) #define PBIO_CONFIG_SERVO_NUM_DEV (6) #define PBIO_CONFIG_SERVO_EV3_NXT (1) diff --git a/lib/pbio/src/drivebase.c b/lib/pbio/src/drivebase.c index 8d41c2f5a..d5b1c69bb 100644 --- a/lib/pbio/src/drivebase.c +++ b/lib/pbio/src/drivebase.c @@ -147,7 +147,7 @@ static pbio_error_t pbio_drivebase_get_state_control(pbio_drivebase_t *db, pbio_ // Optionally use gyro to override the heading source for more accuracy. if (db->use_gyro) { - pbio_orientation_imu_get_heading_scaled(&state_heading->position, db->control_heading.settings.ctl_steps_per_app_step); + pbio_imu_get_heading_scaled(&state_heading->position, db->control_heading.settings.ctl_steps_per_app_step); } return PBIO_SUCCESS; diff --git a/lib/pbio/src/orientation.c b/lib/pbio/src/imu.c similarity index 89% rename from lib/pbio/src/orientation.c rename to lib/pbio/src/imu.c index 2a0af68e4..6a92f9666 100644 --- a/lib/pbio/src/orientation.c +++ b/lib/pbio/src/imu.c @@ -14,7 +14,7 @@ #include #include -#if PBIO_CONFIG_ORIENTATION_IMU +#if PBIO_CONFIG_IMU static pbdrv_imu_dev_t *imu_dev; static pbdrv_imu_config_t *imu_config; @@ -66,7 +66,7 @@ static void pbio_imu_handle_stationary_data_func(const int32_t *gyro_data_sum, c /** * Initializes global imu module. */ -void pbio_orientation_imu_init(void) { +void pbio_imu_init(void) { pbio_error_t err = pbdrv_imu_get_imu(&imu_dev, &imu_config); if (err != PBIO_SUCCESS) { return; @@ -101,7 +101,7 @@ pbio_error_t pbio_orientation_set_base_orientation(pbio_geometry_xyz_t *front_si return err; } - pbio_orientation_imu_set_heading(0.0f); + pbio_imu_set_heading(0.0f); return PBIO_SUCCESS; } @@ -111,7 +111,7 @@ pbio_error_t pbio_orientation_set_base_orientation(pbio_geometry_xyz_t *front_si * * @return True if it has been stationary for about a second, false if moving. */ -bool pbio_orientation_imu_is_stationary(void) { +bool pbio_imu_is_stationary(void) { return pbdrv_imu_is_stationary(imu_dev); } @@ -123,7 +123,7 @@ bool pbio_orientation_imu_is_stationary(void) { * @param [in] angular_velocity Angular velocity threshold in deg/s. * @param [in] acceleration Acceleration threshold in mm/s^2 */ -void pbio_orientation_imu_set_stationary_thresholds(float angular_velocity, float acceleration) { +void pbio_imu_set_stationary_thresholds(float angular_velocity, float acceleration) { int16_t gyro_threshold = pbio_int_math_bind(angular_velocity / imu_config->gyro_scale, 1, INT16_MAX); int16_t accl_threshold = pbio_int_math_bind(acceleration / imu_config->accel_scale, 1, INT16_MAX); pbdrv_imu_set_stationary_thresholds(imu_dev, gyro_threshold, accl_threshold); @@ -134,7 +134,7 @@ void pbio_orientation_imu_set_stationary_thresholds(float angular_velocity, floa * * @param [out] values The angular velocity vector. */ -void pbio_orientation_imu_get_angular_velocity(pbio_geometry_xyz_t *values) { +void pbio_imu_get_angular_velocity(pbio_geometry_xyz_t *values) { pbio_geometry_vector_map(&pbio_orientation_base_orientation, &angular_velocity, values); } @@ -143,7 +143,7 @@ void pbio_orientation_imu_get_angular_velocity(pbio_geometry_xyz_t *values) { * * @param [out] values The acceleration vector. */ -void pbio_orientation_imu_get_acceleration(pbio_geometry_xyz_t *values) { +void pbio_imu_get_acceleration(pbio_geometry_xyz_t *values) { pbio_geometry_vector_map(&pbio_orientation_base_orientation, &acceleration, values); } @@ -152,7 +152,7 @@ void pbio_orientation_imu_get_acceleration(pbio_geometry_xyz_t *values) { * * @return Which side is up. */ -pbio_geometry_side_t pbio_orientation_imu_get_up_side(void) { +pbio_geometry_side_t pbio_imu_get_up_side(void) { // 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 @@ -167,7 +167,7 @@ static float heading_offset = 0; * * @return Heading angle in the base frame. */ -float pbio_orientation_imu_get_heading(void) { +float pbio_imu_get_heading(void) { pbio_geometry_xyz_t heading_mapped; @@ -184,8 +184,8 @@ float pbio_orientation_imu_get_heading(void) { * * @param [in] desired_heading The desired heading value. */ -void pbio_orientation_imu_set_heading(float desired_heading) { - heading_offset = pbio_orientation_imu_get_heading() + heading_offset - desired_heading; +void pbio_imu_set_heading(float desired_heading) { + heading_offset = pbio_imu_get_heading() + heading_offset - desired_heading; } /** @@ -198,10 +198,10 @@ void pbio_orientation_imu_set_heading(float desired_heading) { * @param [out] heading The output angle object. * @param [in] ctl_steps_per_degree The number of control steps per heading degree. */ -void pbio_orientation_imu_get_heading_scaled(pbio_angle_t *heading, int32_t ctl_steps_per_degree) { +void pbio_imu_get_heading_scaled(pbio_angle_t *heading, int32_t ctl_steps_per_degree) { // Heading in degrees of the robot. - float heading_degrees = pbio_orientation_imu_get_heading(); + float heading_degrees = pbio_imu_get_heading(); // Number of whole rotations in control units (in terms of wheels, not robot). heading->rotations = heading_degrees / (360000 / ctl_steps_per_degree); @@ -211,4 +211,4 @@ void pbio_orientation_imu_get_heading_scaled(pbio_angle_t *heading, int32_t ctl_ heading->millidegrees = truncated * ctl_steps_per_degree; } -#endif // PBIO_CONFIG_ORIENTATION_IMU +#endif // PBIO_CONFIG_IMU diff --git a/lib/pbio/src/main.c b/lib/pbio/src/main.c index c0689036d..2f0ac2ef1 100644 --- a/lib/pbio/src/main.c +++ b/lib/pbio/src/main.c @@ -55,7 +55,7 @@ void pbio_init(void) { pbio_motor_process_start(); #endif - pbio_orientation_imu_init(); + pbio_imu_init(); } /** diff --git a/pybricks/common/pb_type_imu.c b/pybricks/common/pb_type_imu.c index be249ea57..c536b5991 100644 --- a/pybricks/common/pb_type_imu.c +++ b/pybricks/common/pb_type_imu.c @@ -27,7 +27,7 @@ typedef struct _common_IMU_obj_t { // pybricks._common.IMU.up STATIC mp_obj_t common_IMU_up(mp_obj_t self_in) { - switch (pbio_orientation_imu_get_up_side()) { + switch (pbio_imu_get_up_side()) { default: case PBIO_GEOMETRY_SIDE_FRONT: return MP_OBJ_FROM_PTR(&pb_Side_FRONT_obj); @@ -50,7 +50,7 @@ STATIC mp_obj_t common_IMU_tilt(mp_obj_t self_in) { // Read acceleration in the user frame. pbio_geometry_xyz_t accl; - pbio_orientation_imu_get_acceleration(&accl); + pbio_imu_get_acceleration(&accl); mp_obj_t tilt[2]; // Pitch @@ -85,7 +85,7 @@ STATIC mp_obj_t common_IMU_acceleration(size_t n_args, const mp_obj_t *pos_args, (void)self; pbio_geometry_xyz_t acceleration; - pbio_orientation_imu_get_acceleration(&acceleration); + pbio_imu_get_acceleration(&acceleration); // If no axis is specified, return a vector of values. if (axis_in == mp_const_none) { @@ -110,7 +110,7 @@ STATIC mp_obj_t common_IMU_angular_velocity(size_t n_args, const mp_obj_t *pos_a (void)self; pbio_geometry_xyz_t angular_velocity; - pbio_orientation_imu_get_angular_velocity(&angular_velocity); + pbio_imu_get_angular_velocity(&angular_velocity); // If no axis is specified, return a vector of values. if (axis_in == mp_const_none) { @@ -129,7 +129,7 @@ STATIC MP_DEFINE_CONST_FUN_OBJ_KW(common_IMU_angular_velocity_obj, 1, common_IMU // pybricks._common.IMU.stationary STATIC mp_obj_t common_IMU_stationary(mp_obj_t self_in) { - return mp_obj_new_bool(pbio_orientation_imu_is_stationary()); + return mp_obj_new_bool(pbio_imu_is_stationary()); } MP_DEFINE_CONST_FUN_OBJ_1(common_IMU_stationary_obj, common_IMU_stationary); @@ -141,7 +141,7 @@ STATIC mp_obj_t common_IMU_set_stationary_thresholds(size_t n_args, const mp_obj PB_ARG_REQUIRED(acceleration)); (void)self; - pbio_orientation_imu_set_stationary_thresholds(mp_obj_get_float(angular_velocity_in), mp_obj_get_float(acceleration_in)); + pbio_imu_set_stationary_thresholds(mp_obj_get_float(angular_velocity_in), mp_obj_get_float(acceleration_in)); return mp_const_none; } STATIC MP_DEFINE_CONST_FUN_OBJ_KW(common_IMU_set_stationary_thresholds_obj, 1, common_IMU_set_stationary_thresholds); @@ -149,7 +149,7 @@ STATIC MP_DEFINE_CONST_FUN_OBJ_KW(common_IMU_set_stationary_thresholds_obj, 1, c // pybricks._common.IMU.heading STATIC mp_obj_t common_IMU_heading(mp_obj_t self_in) { (void)self_in; - return mp_obj_new_float(pbio_orientation_imu_get_heading()); + return mp_obj_new_float(pbio_imu_get_heading()); } MP_DEFINE_CONST_FUN_OBJ_1(common_IMU_heading_obj, common_IMU_heading); @@ -161,7 +161,7 @@ STATIC mp_obj_t common_IMU_reset_heading(size_t n_args, const mp_obj_t *pos_args // Set the new angle (void)self; - pbio_orientation_imu_set_heading(mp_obj_get_float(angle_in)); + pbio_imu_set_heading(mp_obj_get_float(angle_in)); return mp_const_none; } STATIC MP_DEFINE_CONST_FUN_OBJ_KW(common_IMU_reset_heading_obj, 1, common_IMU_reset_heading); @@ -203,7 +203,7 @@ mp_obj_t pb_type_IMU_obj_new(mp_obj_t top_side_axis_in, mp_obj_t front_side_axis pbio_orientation_set_base_orientation(&front_side_axis, &top_side_axis); // Default noise thresholds. - pbio_orientation_imu_set_stationary_thresholds(1.5f, 250.0f); + pbio_imu_set_stationary_thresholds(1.5f, 250.0f); // Return singleton instance. return MP_OBJ_FROM_PTR(&singleton_imu_obj); diff --git a/pybricks/robotics/pb_type_drivebase.c b/pybricks/robotics/pb_type_drivebase.c index 7a5e76e4b..d777c7451 100644 --- a/pybricks/robotics/pb_type_drivebase.c +++ b/pybricks/robotics/pb_type_drivebase.c @@ -61,7 +61,7 @@ STATIC mp_obj_t pb_type_DriveBase_make_new(const mp_obj_type_t *type, size_t n_a // REVISIT: Allow angle getter callable on any platform. bool use_gyro = mp_obj_is_true(use_gyro_in); - #if !PBIO_CONFIG_ORIENTATION_IMU + #if !PBIO_CONFIG_IMU if (use_gyro) { pb_assert(PBIO_ERROR_NOT_SUPPORTED); }