From e8b2393399718868c92396891f05c9965e28ee2c Mon Sep 17 00:00:00 2001 From: Wataru Taniguchi Date: Thu, 10 Jul 2025 10:53:16 +0900 Subject: [PATCH 1/2] IMU angular API added --- sample/raspike/raspike.c | 51 +++++++++++++++++++++++++-- sample/raspike/raspike.cfg | 3 ++ sample/raspike/raspike.h | 1 + sample/raspike/raspike_protocol_com.h | 2 ++ sample/raspike/spike_version | 2 +- 5 files changed, 56 insertions(+), 3 deletions(-) diff --git a/sample/raspike/raspike.c b/sample/raspike/raspike.c index 5d1efe82..b5ce8d77 100644 --- a/sample/raspike/raspike.c +++ b/sample/raspike/raspike.c @@ -182,6 +182,9 @@ static void send_ack(RasPikePort port, const char cmd_id, int32_t data) raspike_send_data(port,RP_CMD_ID_ACK,(char*)send_data,sizeof(send_data)); } +/* IMU用の配列 0:ロール(x) 1:ピッチ(y) 2:ヨー(z) */ +float ang[3] = {0}; // 角度(角位置) +float angv_offset[3] = {0}; // 角速度オフセット void update_hub_status(RPProtocolSpikeStatus *status) { @@ -192,6 +195,9 @@ void update_hub_status(RPProtocolSpikeStatus *status) status->button = button; hub_imu_get_angular_velocity(status->angular_velocity); hub_imu_get_acceleration(status->acceleration); + status->angular[0] = ang[0]; + status->angular[1] = ang[1]; + status->angular[2] = ang[2]; } void update_port_device_colorsensor(unsigned char cmd_id,pup_device_t *dev,RPProtocolPortStatus *status) @@ -728,6 +734,9 @@ static void process_hub_cmd(RasPikePort port, const int cmd_id, const char *para case RP_CMD_ID_HUB_SPK_STP: hub_speaker_stop(); break; + case RP_CMD_ID_HUB_RST_ANG: + ang[0] = ang[1] = ang[2] = 0; + break; default: RP_ASSERT(fgDevices[port].device==0,99); @@ -815,17 +824,40 @@ static uint8_t raspike2_startup_image[5][5] = { { 0, 0, 0, 0, 0} }; +/* Gyro sensor implementation is based upon */ +/* https://qiita.com/koushiro/items/f6cc865e5b2e5b3dd662 */ +/* by @koushiro (Koushiro A) */ + +void identify_angv_offset(float offset[3]) +{ + float angv[3]; // IMU角速度 格納用配列 + + // オフセット同定 (1秒間で1000回測定して平均取る) + for(int i=0; i<1000; i++){ + hub_imu_get_angular_velocity(angv); //角速度取得 + offset[0] += angv[0]; + offset[1] += angv[1]; + offset[2] += angv[2]; + dly_tsk(1*1000); // 1ms待機 + } + // オフセットをサンプル取得回数で割る + offset[0] /= 1000; + offset[1] /= 1000; + offset[2] /= 1000; +} void main_task(intptr_t exinf) { + dly_tsk(3*1000*1000); // wait 3 seconds hub_imu_init(); serial_opn_por(SIO_USB_PORTID); serial_ctl_por(SIO_USB_PORTID,0); - // 1秒待たせる - dly_tsk(1000000); + // 1秒待ちながらIMUの各速度オフセットを同定する + identify_angv_offset(angv_offset); + sta_cyc(APP_GYRO_CYC); //hub_display_image((uint8_t*)raspike2_image); @@ -908,4 +940,19 @@ void soner_task(intptr_t exinf) update_ultrasonicsensor_port_devices(fgDevices,RP_MAX_DEVICES,&fgCurrentStatus); ext_tsk(); +} + +/* gyro sensor task */ +void gyro_task(intptr_t exinf) +{ + float angv[3]; // IMU角速度 格納用配列 + + hub_imu_get_angular_velocity(angv); + + // 角度(角位置)積算 + ang[0] += (angv[0] - angv_offset[0]) * 0.001; + ang[1] += (angv[1] - angv_offset[1]) * 0.001; + ang[2] += (angv[2] - angv_offset[2]) * 0.001; + + ext_tsk(); } \ No newline at end of file diff --git a/sample/raspike/raspike.cfg b/sample/raspike/raspike.cfg index 3b943212..68c19b32 100644 --- a/sample/raspike/raspike.cfg +++ b/sample/raspike/raspike.cfg @@ -13,5 +13,8 @@ CRE_CYC(APP_NOTIFY_CYC, { TA_NULL , {TNFY_ACTTSK,APP_NOTIFY_TASK}, 10*1000, 0}); CRE_TSK(APP_SONER_TASK, { TA_NULL, 0, soner_task, MAIN_PRIORITY + 2, MAIN_STACK_SIZE, NULL }); CRE_CYC(APP_SONER_CYC, { TA_NULL , {TNFY_ACTTSK,APP_SONER_TASK}, 10*1000, 0}); +CRE_TSK(APP_GYRO_TASK, { TA_NULL, 0, gyro_task, MAIN_PRIORITY + 2, MAIN_STACK_SIZE, NULL }); +CRE_CYC(APP_GYRO_CYC, { TA_NULL, {TNFY_ACTTSK,APP_GYRO_TASK}, 1*1000, 0}); + CRE_MTX(APP_STATUS_MUTEX,{ TA_CEILING, APP_HIGHEST_PRIORITY }); CRE_MTX(APP_SEND_MUTEX,{ TA_CEILING, APP_HIGHEST_PRIORITY }); diff --git a/sample/raspike/raspike.h b/sample/raspike/raspike.h index 2951f467..0233e2b7 100644 --- a/sample/raspike/raspike.h +++ b/sample/raspike/raspike.h @@ -29,6 +29,7 @@ extern void main_task(intptr_t exinf); extern void notify_task(intptr_t exinf); extern void soner_task(intptr_t exinf); +extern void gyro_task(intptr_t exinf); #endif /* TOPPERS_MACRO_ONLY */ diff --git a/sample/raspike/raspike_protocol_com.h b/sample/raspike/raspike_protocol_com.h index c2147150..e58eaf57 100644 --- a/sample/raspike/raspike_protocol_com.h +++ b/sample/raspike/raspike_protocol_com.h @@ -131,6 +131,7 @@ #define RP_CMD_ID_HUB_SPK_SET_VOL (MAKE_CMD(RP_CMD_TYPE_HUB,0x11)) // 0xb1 #define RP_CMD_ID_HUB_SPK_PLY_TON (MAKE_CMD(RP_CMD_TYPE_HUB,0x12)) // 0xb2 #define RP_CMD_ID_HUB_SPK_STP (MAKE_CMD(RP_CMD_TYPE_HUB,0x13)) // 0xb3 +#define RP_CMD_ID_HUB_RST_ANG (MAKE_CMD(RP_CMD_TYPE_HUB,0x14)) // 0xb4 @@ -192,6 +193,7 @@ typedef struct { uint16_t current; float acceleration[3]; float angular_velocity[3]; + float angular[3]; uint32_t button; RPProtocolPortStatus ports[6]; } RPProtocolSpikeStatus; diff --git a/sample/raspike/spike_version b/sample/raspike/spike_version index 99d85ecd..5c4511c3 100644 --- a/sample/raspike/spike_version +++ b/sample/raspike/spike_version @@ -1 +1 @@ -0.0.6 \ No newline at end of file +0.0.7 \ No newline at end of file From fa11d6e8d7d5c01b533c47cbde754c4a4c74098d Mon Sep 17 00:00:00 2001 From: Wataru Taniguchi Date: Tue, 22 Jul 2025 10:38:56 +0900 Subject: [PATCH 2/2] IMU API ported from pybricks --- sample/raspike/Makefile.inc | 2 + sample/raspike/pbio/geometry.c | 347 ++++++++++++ sample/raspike/pbio/geometry.h | 112 ++++ sample/raspike/raspike.c | 66 +-- sample/raspike/raspike.cfg | 2 +- sample/raspike/raspike.h | 3 + sample/raspike/raspike_imu.c | 735 ++++++++++++++++++++++++++ sample/raspike/raspike_imu.h | 69 +++ sample/raspike/raspike_protocol_com.h | 19 +- sample/raspike/spike_version | 2 +- 10 files changed, 1309 insertions(+), 48 deletions(-) create mode 100644 sample/raspike/pbio/geometry.c create mode 100644 sample/raspike/pbio/geometry.h create mode 100644 sample/raspike/raspike_imu.c create mode 100644 sample/raspike/raspike_imu.h diff --git a/sample/raspike/Makefile.inc b/sample/raspike/Makefile.inc index 10890c0d..4f4e3493 100644 --- a/sample/raspike/Makefile.inc +++ b/sample/raspike/Makefile.inc @@ -1,3 +1,5 @@ +APPL_COBJS += pbio/geometry.o raspike_imu.o + VERSION=$(shell cat ../../sample/raspike/spike_version) MAJOR := $(word 1,$(subst ., ,$(VERSION))) diff --git a/sample/raspike/pbio/geometry.c b/sample/raspike/pbio/geometry.c new file mode 100644 index 00000000..c77c3b57 --- /dev/null +++ b/sample/raspike/pbio/geometry.c @@ -0,0 +1,347 @@ +// SPDX-License-Identifier: MIT +// Copyright (c) 2023 The Pybricks Authors + +#include + +#include + +#include + +/** + * Gets @p index and @p sign of the axis that passes through given @p side of + * a box. + * @param [in] side The requested side of the box. + * @param [out] index The index 0 (x), 1 (y), or 2 (z) of the axis. + * @param [out] sign The sign of the axis: 1 or -1. + */ +void pbio_geometry_side_get_axis(pbio_geometry_side_t side, uint8_t *index, int8_t *sign) { + *index = side & 0x03; + *sign = (side & 0x04) ? -1 : 1; +} + +/** + * Given index and sign of two base axes, finds the axis to complete a right + * handed coordinate system. + * @param [in] index Array of three axis indexes. First and last are given, + * middle axis index is computed. + * @param [in] sign Array of three axis signs. First and last are given, + * middle axis sign is computed. + */ +void pbio_geometry_get_complementary_axis(uint8_t *index, int8_t *sign) { + + // Inputs must have valid axis index and sign. + assert(index[0] < 3); + assert(index[2] < 3); + assert(sign[0] == 1 || sign[2] == 1); + + // Two axis cannot be parallel. + assert(index[2] != index[0]); + + // The final axis is determined as the cross product v_1 = v_2 (x) v_0. + // Since we know the axes are just base axis up to a sign, the result C + // simply has the index that isn't used yet: + index[1] = 3 - index[2] - index[0]; + + // We still have so evaluate the cross product to get the sign. With inputs + // j = {0, 2}, both input vectors v_0 and v_2 can be written in + // terms of known information as: + // + // [ i_j == 0 ] + // v_j = s_j * [ i_j == 1 ] + // [ i_j == 2 ] + // + // where s_j is the sign of vector j, and i_j is the axis index of j. + // + // For example, if the first input has sign -1 and index 2, it is + // + // [ 0] + // v_0 = [ 0] + // [-1] + // + // So it is the negative Z vector. + // + // Given the vectors v_0 and v_2, we evaluate the cross product formula: + // + // v_1 = + s_0 * s_2 * ((i_2==1)*(i_0==2)-(i_0==1)*(i_2==2)) * E_0 + // - s_0 * s_2 * ((i_2==0)*(i_0==2)-(i_0==0)*(i_2==2)) * E_1 + // + s_0 * s_2 * ((i_2==0)*(i_0==1)-(i_0==0)*(i_2==1)) * E_2 + // + // But we already know which of these three vectors is nonzero, so we just + // need to take the scalar value in front of it. This leaves us with one of + // three results. Since the formulas are so similar, we can generalize it + // to one result that depends on the known final axis index. This gives: + sign[1] = sign[0] * sign[2] * ( + (index[0] == (index[1] + 2) % 3) * (index[2] == (index[1] + 1) % 3) - + (index[0] == (index[1] + 1) % 3) * (index[2] == (index[1] + 2) % 3) + ); +} + +/** + * Gets the side of a unit-sized box through which a given vector passes first. + * + * @param [in] vector The input vector. + * @return The side through which the vector passes first. + */ +pbio_geometry_side_t pbio_geometry_side_from_vector(pbio_geometry_xyz_t *vector) { + + // Find index and sign of maximum component + float abs_max = 0; + uint8_t axis = 0; + bool negative = true; + for (uint8_t i = 0; i < 3; i++) { + if (vector->values[i] > abs_max) { + abs_max = vector->values[i]; + negative = false; + axis = i; + } else if (-vector->values[i] > abs_max) { + abs_max = -vector->values[i]; + negative = true; + axis = i; + } + } + + // Return as side enum value. + 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. + */ +pbio_error_t pbio_geometry_vector_normalize(pbio_geometry_xyz_t *input, pbio_geometry_xyz_t *output) { + + // Compute the norm. + float norm = pbio_geometry_vector_norm(input); + + // If the vector norm is zero, do nothing. + if (norm == 0.0f) { + return PBIO_ERROR_INVALID_ARG; + } + + // Otherwise, normalize. + output->x = input->x / norm; + output->y = input->y / norm; + output->z = input->z / norm; + return PBIO_SUCCESS; +} + +/** + * Gets the cross product of two vectors. + * + * @param [in] a The first vector. + * @param [in] b The first vector. + * @param [out] output The result. + */ +void pbio_geometry_vector_cross_product(pbio_geometry_xyz_t *a, pbio_geometry_xyz_t *b, pbio_geometry_xyz_t *output) { + output->x = a->y * b->z - a->z * b->y; + output->y = a->z * b->x - a->x * b->z; + output->z = a->x * b->y - a->y * b->x; +} + +/** + * Gets the scalar projection of one vector onto the line spanned by another. + * + * @param [in] axis The axis on which to project. + * @param [in] input The input vector. + * @param [out] projection The projection. + * @return ::PBIO_ERROR_INVALID_ARG if the axis has zero length, otherwise ::PBIO_SUCCESS. + */ +pbio_error_t pbio_geometry_vector_project(pbio_geometry_xyz_t *axis, pbio_geometry_xyz_t *input, float *projection) { + + // Normalize the given axis so its magnitude does not matter. + pbio_geometry_xyz_t unit_axis; + pbio_error_t err = pbio_geometry_vector_normalize(axis, &unit_axis); + if (err != PBIO_SUCCESS) { + return err; + } + + // Compute the projection. + *projection = unit_axis.x * input->x + unit_axis.y * input->y + unit_axis.z * input->z; + return PBIO_SUCCESS; +} + +/** + * Maps the input vector using: output = map * input + * + * @param [in] map The 3x3 map, such as a rotation matrix. + * @param [in] input The input vector + * @param [out] output The resulting output vector. + */ +void pbio_geometry_vector_map(pbio_geometry_matrix_3x3_t *map, pbio_geometry_xyz_t *input, pbio_geometry_xyz_t *output) { + output->x = input->x * map->m11 + input->y * map->m12 + input->z * map->m13; + output->y = input->x * map->m21 + input->y * map->m22 + input->z * map->m23; + 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. + * + * @param [in] x_axis The X axis. Need not be normalized. + * @param [in] z_axis The Z axis. Need not be normalized. + * @param [out] map The completed map, including the computed y_axis. + * @return ::PBIO_ERROR_INVALID_ARG if the axes are zero or not orthogonal, otherwise ::PBIO_SUCCESS. + */ +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 *map) { + + pbio_geometry_xyz_t x_axis_normal; + pbio_error_t err = pbio_geometry_vector_normalize(x_axis, &x_axis_normal); + if (err != PBIO_SUCCESS) { + return err; + } + + pbio_geometry_xyz_t z_axis_normal; + err = pbio_geometry_vector_normalize(z_axis, &z_axis_normal); + if (err != PBIO_SUCCESS) { + return err; + } + + // Assert that X and Z are orthogonal. + float inner_product = x_axis_normal.x * z_axis_normal.x + x_axis_normal.y * z_axis_normal.y + x_axis_normal.z * z_axis_normal.z; + if (inner_product > 0.001f || inner_product < -0.001f) { + return PBIO_ERROR_INVALID_ARG; + } + + pbio_geometry_xyz_t y_axis_normal; + pbio_geometry_vector_cross_product(&z_axis_normal, &x_axis_normal, &y_axis_normal); + + // Build the resulting matrix as [x_axis y_axis z_axis] + *map = (pbio_geometry_matrix_3x3_t) { + .m11 = x_axis_normal.x, .m12 = y_axis_normal.x, .m13 = z_axis_normal.x, + .m21 = x_axis_normal.y, .m22 = y_axis_normal.y, .m23 = z_axis_normal.y, + .m31 = x_axis_normal.z, .m32 = y_axis_normal.z, .m33 = z_axis_normal.z, + }; + + 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/sample/raspike/pbio/geometry.h b/sample/raspike/pbio/geometry.h new file mode 100644 index 00000000..be176d83 --- /dev/null +++ b/sample/raspike/pbio/geometry.h @@ -0,0 +1,112 @@ +// SPDX-License-Identifier: MIT +// Copyright (c) 2023 The Pybricks Authors + +/** + * @addtogroup Geometry pbio/geometry: Linear algebra and geometry utilities + * + * Geometry utilities used by the pbio library. + * @{ + */ + +#ifndef _PBIO_GEOMETRY_H_ +#define _PBIO_GEOMETRY_H_ + +#include +#include + +#include + +/** + * Identifier for one side of a rectangle (e.g. screen) or box (e.g. a hub). + */ +typedef enum { + PBIO_GEOMETRY_SIDE_FRONT = (0 << 2) | 0, /**< +X: The front side of a rectangular box */ + PBIO_GEOMETRY_SIDE_LEFT = (0 << 2) | 1, /**< +Y: The left side of a rectangular box or screen */ + PBIO_GEOMETRY_SIDE_TOP = (0 << 2) | 2, /**< +Z: The top side of a rectangular box or screen */ + PBIO_GEOMETRY_SIDE_BACK = (1 << 2) | 0, /**< -X: The back side of a rectangular box */ + PBIO_GEOMETRY_SIDE_RIGHT = (1 << 2) | 1, /**< -Y: The right side of a rectangular box or screen */ + PBIO_GEOMETRY_SIDE_BOTTOM = (1 << 2) | 2, /**< -Z: The bottom side of a rectangular box or screen */ +} pbio_geometry_side_t; + +/** + * Coordinate-like type with x, y, and z floating point values. + */ +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 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); + +pbio_error_t pbio_geometry_vector_project(pbio_geometry_xyz_t *axis, pbio_geometry_xyz_t *input, float *projection); + +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/sample/raspike/raspike.c b/sample/raspike/raspike.c index b5ce8d77..677bea52 100644 --- a/sample/raspike/raspike.c +++ b/sample/raspike/raspike.c @@ -24,7 +24,7 @@ #include #include - +#include "raspike_imu.h" #include "raspike.h" #define RP_DEFINE_CMD_SIZE #include "raspike_protocol_com.h" @@ -182,9 +182,7 @@ static void send_ack(RasPikePort port, const char cmd_id, int32_t data) raspike_send_data(port,RP_CMD_ID_ACK,(char*)send_data,sizeof(send_data)); } -/* IMU用の配列 0:ロール(x) 1:ピッチ(y) 2:ヨー(z) */ -float ang[3] = {0}; // 角度(角位置) -float angv_offset[3] = {0}; // 角速度オフセット +float test_data[3]; // for debugging void update_hub_status(RPProtocolSpikeStatus *status) { @@ -193,11 +191,10 @@ void update_hub_status(RPProtocolSpikeStatus *status) status->current = hub_battery_get_current(); hub_button_is_pressed(&button); status->button = button; - hub_imu_get_angular_velocity(status->angular_velocity); - hub_imu_get_acceleration(status->acceleration); - status->angular[0] = ang[0]; - status->angular[1] = ang[1]; - status->angular[2] = ang[2]; + raspike_imu_get_angular_velocity((pbio_geometry_xyz_t *)status->angular_velocity, true); + raspike_imu_get_acceleration((pbio_geometry_xyz_t *)status->acceleration, true); + raspike_imu_get_orientation((pbio_geometry_matrix_3x3_t *)status->rotation_matrix); + status->heading = raspike_imu_get_heading(RASPIKE_IMU_HEADING_TYPE_3D); } void update_port_device_colorsensor(unsigned char cmd_id,pup_device_t *dev,RPProtocolPortStatus *status) @@ -734,8 +731,19 @@ static void process_hub_cmd(RasPikePort port, const int cmd_id, const char *para case RP_CMD_ID_HUB_SPK_STP: hub_speaker_stop(); break; - case RP_CMD_ID_HUB_RST_ANG: - ang[0] = ang[1] = ang[2] = 0; + case RP_CMD_ID_HUB_IMU_INIT: + { + float gyro_stationary_threshold = *(float*)(param+RP_HUB_IMU_INIT_INDEX_GYRO_STAT_THRESH); + float accel_stationary_threshold = *(float*)(param+RP_HUB_IMU_INIT_INDEX_ACCEL_STAT_THRESH); + float angular_velocity_bias[3]; + memcpy(angular_velocity_bias,param+RP_HUB_IMU_INIT_INDEX_ANGV_BIAS,3*sizeof(float)); + float angular_velocity_scale[3]; + memcpy(angular_velocity_scale,param+RP_HUB_IMU_INIT_INDEX_ANGV_SCALE,3*sizeof(float)); + float acceleration_correction[6]; + memcpy(acceleration_correction,param+RP_HUB_IMU_INIT_INDEX_ACCEL_CORRECT,6*sizeof(float)); + raspike_imu_initialize(gyro_stationary_threshold, accel_stationary_threshold, + angular_velocity_bias, angular_velocity_scale, acceleration_correction); + } break; default: @@ -824,40 +832,18 @@ static uint8_t raspike2_startup_image[5][5] = { { 0, 0, 0, 0, 0} }; -/* Gyro sensor implementation is based upon */ -/* https://qiita.com/koushiro/items/f6cc865e5b2e5b3dd662 */ -/* by @koushiro (Koushiro A) */ - -void identify_angv_offset(float offset[3]) -{ - float angv[3]; // IMU角速度 格納用配列 - - // オフセット同定 (1秒間で1000回測定して平均取る) - for(int i=0; i<1000; i++){ - hub_imu_get_angular_velocity(angv); //角速度取得 - offset[0] += angv[0]; - offset[1] += angv[1]; - offset[2] += angv[2]; - dly_tsk(1*1000); // 1ms待機 - } - // オフセットをサンプル取得回数で割る - offset[0] /= 1000; - offset[1] /= 1000; - offset[2] /= 1000; -} void main_task(intptr_t exinf) { - dly_tsk(3*1000*1000); // wait 3 seconds hub_imu_init(); serial_opn_por(SIO_USB_PORTID); serial_ctl_por(SIO_USB_PORTID,0); - // 1秒待ちながらIMUの各速度オフセットを同定する - identify_angv_offset(angv_offset); sta_cyc(APP_GYRO_CYC); + // 1秒待たせる + dly_tsk(1000000); //hub_display_image((uint8_t*)raspike2_image); @@ -945,14 +931,6 @@ void soner_task(intptr_t exinf) /* gyro sensor task */ void gyro_task(intptr_t exinf) { - float angv[3]; // IMU角速度 格納用配列 - - hub_imu_get_angular_velocity(angv); - - // 角度(角位置)積算 - ang[0] += (angv[0] - angv_offset[0]) * 0.001; - ang[1] += (angv[1] - angv_offset[1]) * 0.001; - ang[2] += (angv[2] - angv_offset[2]) * 0.001; - + raspike_imu_handle_frame_data(PERIOD_GYRO_TSK); ext_tsk(); } \ No newline at end of file diff --git a/sample/raspike/raspike.cfg b/sample/raspike/raspike.cfg index 68c19b32..9b3210cb 100644 --- a/sample/raspike/raspike.cfg +++ b/sample/raspike/raspike.cfg @@ -14,7 +14,7 @@ CRE_TSK(APP_SONER_TASK, { TA_NULL, 0, soner_task, MAIN_PRIORITY + 2, MAIN_STACK_ CRE_CYC(APP_SONER_CYC, { TA_NULL , {TNFY_ACTTSK,APP_SONER_TASK}, 10*1000, 0}); CRE_TSK(APP_GYRO_TASK, { TA_NULL, 0, gyro_task, MAIN_PRIORITY + 2, MAIN_STACK_SIZE, NULL }); -CRE_CYC(APP_GYRO_CYC, { TA_NULL, {TNFY_ACTTSK,APP_GYRO_TASK}, 1*1000, 0}); +CRE_CYC(APP_GYRO_CYC, { TA_NULL, {TNFY_ACTTSK,APP_GYRO_TASK}, PERIOD_GYRO_TSK, 0}); CRE_MTX(APP_STATUS_MUTEX,{ TA_CEILING, APP_HIGHEST_PRIORITY }); CRE_MTX(APP_SEND_MUTEX,{ TA_CEILING, APP_HIGHEST_PRIORITY }); diff --git a/sample/raspike/raspike.h b/sample/raspike/raspike.h index 0233e2b7..91504ff3 100644 --- a/sample/raspike/raspike.h +++ b/sample/raspike/raspike.h @@ -21,6 +21,9 @@ #define MAIN_STACK_SIZE 4096 /* タスクのスタックサイズ */ +/* task periods in micro seconds */ +#define PERIOD_GYRO_TSK (1 * 1000) + /* * 関数のプロトタイプ宣言 */ diff --git a/sample/raspike/raspike_imu.c b/sample/raspike/raspike_imu.c new file mode 100644 index 00000000..40857454 --- /dev/null +++ b/sample/raspike/raspike_imu.c @@ -0,0 +1,735 @@ +// SPDX-License-Identifier: MIT +/** + * We implement the IMU API by reusing: + * lib/pbio/src/imu.c + * lib/pbio/drv/imu/imu_lsm6ds3tr_c_stm32.c + * from the snapshot of pybricks-micropython as of 2025/02/25 @57793e3. + * + * Original codes Copyright (c) 2022-2023 The Pybricks Authors + * Modifications for TOPPERS/APS3 Kernel Copyright (c) 2022 Embedded and Real-Time Systems Laboratory, + * Graduate School of Information Science, Nagoya Univ., JAPAN + * + * The following files have been copied from the same snapshot: + * lib/pbio/include/pbio/geometry.h -> sample/raspike/pbio/geometry.h + * lib/pbio/src/geometry.c -> sample/raspike/pbio/geometry.c +*/ +#include +#include +#include +#include + +#include + +#include + +#include +#include "raspike_imu.h" + +/** + * User Configuration Values + * + * These values should be determined for each hub by doing a calibration procedure + * and are set by invoking raspike_imu_initilize(). + * For details of IMU calibration, refer to the following post: + * https://github.com/pybricks/support/issues/1907 + */ +bool raspike_imu_initialized = false; +/** Angular velocity threshold below which the IMU is considered stationary. */ +float config_gyro_stationary_threshold; +/** Acceleration threshold below which the IMU is considered stationary. */ +float config_accel_stationary_threshold; +/** Angular velocity scale (unadjusted measured degrees per whole rotation) */ +pbio_geometry_xyz_t config_angular_velocity_scale; +/** Positive acceleration values */ +pbio_geometry_xyz_t config_gravity_pos; +/** Negative acceleration values */ +pbio_geometry_xyz_t config_gravity_neg; + + +/** + * 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. + */ +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. + */ +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. + */ +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. + */ +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. + */ +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. + */ +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. + */ +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. + */ +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. + */ +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. + */ +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. + * + */ +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. + */ +int32_t heading_rotations; + + +/** + * Standard gravity in mm/s^2. + */ +const float standard_gravity = 9806.65f; + + +/** + * 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; +} + +// This counter is a measure for calibration accuracy, roughly equivalent +// to the accumulative number of seconds it has been stationary in total. +uint32_t stationary_counter = 0; +SYSTIM stationary_time_last; + +/* + * Tests if the imu is ready for use in a user program. + * + * @return True if it has been stationary at least once in the last 10 minutes. +*/ +bool raspike_imu_is_ready(void) { + SYSTIM current_time; + get_tim(¤t_time); + return stationary_counter > 0 && current_time - stationary_time_last < 10 * 60 * 1000000; +} + +typedef union { + float data[6]; + struct { + float gyro[3]; + float accel[3]; + }; +} _raspike_imu_raw_data; + +/** Most recent slow moving average of raw data. */ +float data_slow[6]; +/** Sum of raw data for slow moving average. */ +float 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)*/ +SYSTIM stationary_time_start; +/** Raw data point to which new samples are compared to detect stationary. */ +float stationary_data_start[6]; +/** Sum of gyro samples during the stationary period. */ +float stationary_gyro_data_sum[3]; +/** Sum of accelerometer samples during the stationary period. */ +float stationary_accel_data_sum[3]; +/** Number of sequential stationary samples. */ +uint32_t stationary_sample_count; +/** Whether it is currently stationary, to be polled by higher level APIs. */ +bool stationary_now; + +static inline bool is_bounded(float diff, float threshold) { + return diff < threshold && diff > -threshold; +} + +static void reset_stationary_buffer(void) { + stationary_sample_count = 0; + get_tim(&stationary_time_start); + for (uint8_t i = 0; i < 3; i++) { + stationary_gyro_data_sum[i] = 0; + stationary_accel_data_sum[i] = 0; + } +} + +static void update_slow_moving_average(_raspike_imu_raw_data *imu_data) { + for (uint8_t i = 0; i < 6; i++) { + data_slow_sum[i] += imu_data->data[i]; + } + data_slow_count++; + if (data_slow_count == 125) { + for (uint8_t i = 0; i < 6; i++) { + data_slow[i] = data_slow_sum[i] / data_slow_count; + data_slow_sum[i] = 0; + } + data_slow_count = 0; + } +} + +static void update_stationary_status(_raspike_imu_raw_data *imu_data) { + + // Update slow moving average of raw data, used as starting point for stationary detection. + update_slow_moving_average(imu_data); + + // Check whether still stationary compared to constant start sample. + if (!is_bounded(imu_data->data[0] - stationary_data_start[0], config_gyro_stationary_threshold) || + !is_bounded(imu_data->data[1] - stationary_data_start[1], config_gyro_stationary_threshold) || + !is_bounded(imu_data->data[2] - stationary_data_start[2], config_gyro_stationary_threshold) || + !is_bounded(imu_data->data[3] - stationary_data_start[3], config_accel_stationary_threshold) || + !is_bounded(imu_data->data[4] - stationary_data_start[4], config_accel_stationary_threshold) || + !is_bounded(imu_data->data[5] - stationary_data_start[5], config_accel_stationary_threshold) + ) { + // Not stationary anymore, so reset counter and gyro sum data so we can start over. + stationary_now = false; + + // Slow moving average becomes new starting value to compare to. + for (uint8_t i = 0; i < 6; i++) { + stationary_data_start[i] = data_slow[i]; + } + + reset_stationary_buffer(); + return; + } + + // Updating running sum of stationary data. + stationary_sample_count++; + stationary_gyro_data_sum[0] += imu_data->data[0]; + stationary_gyro_data_sum[1] += imu_data->data[1]; + stationary_gyro_data_sum[2] += imu_data->data[2]; + stationary_accel_data_sum[0] += imu_data->data[3]; + stationary_accel_data_sum[1] += imu_data->data[4]; + stationary_accel_data_sum[2] += imu_data->data[5]; + + // Exit if we don't have enough samples yet. + if (stationary_sample_count < 1000) { // 1 sec worth of data + return; + } + + // This tells external APIs that we are really stationary. + stationary_now = true; + + // If the IMU calibration hasn't been updated in a long time, reset the + // stationary counter so that the calibration values get a large weight. + if (!raspike_imu_is_ready()) { + stationary_counter = 0; + } + + get_tim(&stationary_time_last); + stationary_counter++; + + // The relative weight of the new data in order to build a long term + // average of the data without maintaining a data buffer. + float weight = stationary_counter >= 20 ? 0.05f : 1.0f / stationary_counter; + + for (uint8_t i = 0; i < PBIO_ARRAY_SIZE(gyro_bias.values); i++) { + // Average gyro rate while stationary, indicating current bias. + float average_now = stationary_gyro_data_sum[i] / stationary_sample_count; + + // Update bias at decreasing rate. + gyro_bias.values[i] = gyro_bias.values[i] * (1.0f - weight) + weight * average_now; + } + + // Reset counter and gyro sum data so we can start over. + reset_stationary_buffer(); +} + +// Called by periodic handler to process one frame of unfiltered gyro and accelerometer data. +void raspike_imu_handle_frame_data(uint32_t periodic_task_interval) { + if (!raspike_imu_initialized) return; // until config set, do nothing + + _raspike_imu_raw_data imu_data; + hub_imu_get_angular_velocity(imu_data.gyro); // raw angular velocity reading from IMU + hub_imu_get_acceleration(imu_data.accel); // raw acceleration reading from IMU + + // 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 = imu_data.accel[0], .y = imu_data.accel[1], .z = imu_data.accel[2]}; + 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_uncalibrated.values[i] = imu_data.gyro[i]; + acceleration_uncalibrated.values[i] = imu_data.accel[i]; + + // Maintain calibrated cached values. + float acceleration_offset = (config_gravity_pos.values[i] + config_gravity_neg.values[i]) / 2; + float acceleration_scale = (config_gravity_pos.values[i] - config_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 / config_angular_velocity_scale.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_calibrated.values[i] * periodic_task_interval / 1000000; + } + + // 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] * periodic_task_interval / 1000000; + } + pbio_geometry_quaternion_normalize(&quaternion); + + update_stationary_status(&imu_data); +} + + +/** + * Sets the hub base orientation. + * + * @param [in] front_side_axis Which way the hub front side points when it is + * in the base orientation. + * @param [in] top_side_axis Which way the hub top side points when it is + * in the base orientation. + * @return ::PBIO_SUCCESS on success, ::PBIO_ERROR_INVALID_ARG for incorrect axis values. + */ +pbio_error_t raspike_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_imu_base_orientation); + if (err != PBIO_SUCCESS) { + return err; + } + + // 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. + raspike_imu_set_heading(0.0f); + return PBIO_SUCCESS; +} + +/** + * 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 / 10; +} + +/** + * Tests if a value is close to 360 degrees. + * + * @param [in] value The value to test. + * @return True if the value is within +/-15 degrees of 360, false otherwise. + */ +static bool pbio_imu_setting_close_to_360(float value) { + return pbio_geometry_absf(value - 360.0f) < 15.0f; +} + +/** + * Initialize the IMU settings with values calibrated using Pybrick's micropython _imu_calibrate class. + * For IMU calibration process details, refer to https://github.com/pybricks/support/issues/1907 + * + * @param [in] gyro_stationary_threshold + * The threshold for variations in the angular velocity below which the hub is considered + * stationary enough to calibrate. Default value should be 2 deg/s. + * @param [in] accel_stationary_threshold + * The threshold for variations in acceleration below which the hub is considered + * stationary enough to calibrate. Default value should be 2500 mm/s². + * @param [in] angular_velocity_bias + * Initial bias for angular velocity measurements along x, y, and z immediately after boot. + * Default values should be {0, 0, 0} deg/s. + * @param [in] angular_velocity_scale + * Scale adjustment for x, y, and z rotation to account for manufacturing differences. + * Default values should be {360, 360, 360} deg/s + * @param [in] acceleration_correction + * Scale adjustment for x, y, and z gravity magnitude in both directions to account for manufacturing differences. + * Default values should be {9806.65, -9806.65, 9806.65, -9806.65, 9806.65, -9806.65} mm/s². + * @returns ::PBIO_ERROR_INVALID_ARG if a value is out of range, otherwise ::PBIO_SUCCESS. + */ +pbio_error_t raspike_imu_initialize(float gyro_stationary_threshold, float accel_stationary_threshold, + float angular_velocity_bias[3], float angular_velocity_scale[3], float acceleration_correction[6]) { + + if (gyro_stationary_threshold < 0 || + accel_stationary_threshold < 0) { + return PBIO_ERROR_INVALID_ARG; + } else { + config_gyro_stationary_threshold = gyro_stationary_threshold; + config_accel_stationary_threshold = accel_stationary_threshold; + } + + for (uint8_t i = 0; i < 3; i++) { + gyro_bias.values[i] = angular_velocity_bias[i]; + + if (!pbio_imu_setting_close_to_360(angular_velocity_scale[i])) { + return PBIO_ERROR_INVALID_ARG; + } else { + config_angular_velocity_scale.values[i] = angular_velocity_scale[i]; + } + + config_gravity_pos.values[i] = acceleration_correction[2*i]; + config_gravity_neg.values[i] = acceleration_correction[2*i + 1]; + if (pbio_imu_stationary_acceleration_out_of_range(config_gravity_pos.values[i], true) || + pbio_imu_stationary_acceleration_out_of_range(config_gravity_neg.values[i], false)) { + return PBIO_ERROR_INVALID_ARG; + } + } + + raspike_imu_initialized = true; + 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 raspike_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 raspike_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 tilt-based vector that is parallel to the acceleration measurement. + * + * @param [out] values The acceleration vector. + */ +void raspike_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); +} + +/** + * Gets the rotation along a particular axis of the robot frame. + * + * The resulting value makes sense only for one-dimensional rotations. + * + * @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 raspike_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] *= config_angular_velocity_scale.values[i] / 360.0f; + } + } + + // Transform the single axis rotations to the robot frame. + 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_user, angle); +} + +/** + * Gets which side of a hub points upwards. + * + * @param [in] calibrated Whether to use calibrated/fused (true) or raw data (false). + * + * @return Which side is up. + */ +pbio_geometry_side_t raspike_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. + // This is either the raw acceleration or the third row of the fused + // rotation matrix. + + pbio_geometry_xyz_t *vector = &acceleration_uncalibrated; + if (calibrated) { + // This is similar to pbio_imu_get_tilt_vector, but this should stay + // in the hub frame rather than projected into user frame. + vector->x = pbio_imu_rotation.m31; + vector->y = pbio_imu_rotation.m32; + vector->z = pbio_imu_rotation.m33; + } + return pbio_geometry_side_from_vector(vector); +} + +float heading_offset_1d = 0; +float heading_offset_3d = 0; + +/** + * Reads the estimated IMU heading in degrees, accounting for user offset and + * user-specified heading correction scaling constant. + * + * Heading is defined as clockwise positive. + * + * @param [in] type The type of heading to get. + * + * @return Heading angle in the base frame. + */ +float raspike_imu_get_heading(raspike_imu_heading_type_t type) { + + float correction = 1.0f; + + // 3D. Mapping into user frame is already accounted for in the projection. + if (type == RASPIKE_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 * correction - heading_offset_1d; +} + +/** + * Sets the IMU heading. + * + * This only adjusts the user offset without resetting anything in the + * algorithm, so this can be called at any time. + * + * @param [in] desired_heading The desired heading value. + */ +void raspike_imu_set_heading(float desired_heading) { + heading_rotations = 0; + heading_offset_3d = raspike_imu_get_heading(RASPIKE_IMU_HEADING_TYPE_3D) + heading_offset_3d - desired_heading; + heading_offset_1d = raspike_imu_get_heading(RASPIKE_IMU_HEADING_TYPE_1D) + heading_offset_1d - desired_heading; +} + +/** + * Gets the estimated IMU heading in control units through a given scale. + * + * This is mainly used to convert the heading to the right format for a + * drivebase, which measures heading as the half the difference of the two + * motor positions in millidegrees. + * + * 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 raspike_imu_get_heading_scaled(raspike_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 = raspike_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)); + + // The truncated part represents everything else. NB: The scaling factor + // is a float here to ensure we don't lose precision while scaling. + float truncated = heading_degrees - heading->rotations * (360000.0f / ctl_steps_per_degree); + heading->millidegrees = (int32_t)(truncated * ctl_steps_per_degree); + + // The heading rate can be obtained by a simple scale because it always fits. + pbio_geometry_xyz_t angular_rate; + raspike_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 raspike_imu_get_orientation(pbio_geometry_matrix_3x3_t *rotation) { + *rotation = pbio_imu_rotation; +} + + diff --git a/sample/raspike/raspike_imu.h b/sample/raspike/raspike_imu.h new file mode 100644 index 00000000..21be40bb --- /dev/null +++ b/sample/raspike/raspike_imu.h @@ -0,0 +1,69 @@ +// SPDX-License-Identifier: MIT + +/** + * We define the IMU API by reusing lib/pbio/include/pbio/imu.h + * from the snapshot of pybricks-micropython as of 2025/02/25 @57793e3. + * + * Original codes Copyright (c) 2022-2023 The Pybricks Authors + * Modifications for TOPPERS/APS3 Kernel Copyright (c) 2022 Embedded and Real-Time Systems Laboratory, + * Graduate School of Information Science, Nagoya Univ., JAPAN + */ + +#ifndef __RASPIKE_IMU_H__ +#define __RASPIKE_IMU_H__ + +#include + +#include +#include +#include + +/** + * Heading type to use, set, or get. + */ +typedef enum { + /** + * Heading should not be used. + */ + RASPIKE_IMU_HEADING_TYPE_NONE, + /** + * The heading is the integrated gyro rate along one fixed axis. + */ + RASPIKE_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. + */ + RASPIKE_IMU_HEADING_TYPE_3D, +} raspike_imu_heading_type_t; + +bool raspike_imu_is_ready(void); + +void raspike_imu_handle_frame_data(uint32_t periodic_task_interval); + +pbio_error_t raspike_imu_set_base_orientation(pbio_geometry_xyz_t *x_axis, pbio_geometry_xyz_t *z_axis); + +pbio_error_t raspike_imu_initialize(float gyro_stationary_threshold, float accel_stationary_threshold, + float angular_velocity_bias[3], float angular_velocity_scale[3], float acceleration_correction[6]); + +void raspike_imu_get_angular_velocity(pbio_geometry_xyz_t *values, bool calibrated); + +void raspike_imu_get_acceleration(pbio_geometry_xyz_t *values, bool calibrated); + +void raspike_imu_get_tilt_vector(pbio_geometry_xyz_t *values); + +pbio_error_t raspike_imu_get_single_axis_rotation(pbio_geometry_xyz_t *axis, float *angle, bool calibrated); + +pbio_geometry_side_t raspike_imu_get_up_side(bool calibrated); + +float raspike_imu_get_heading(raspike_imu_heading_type_t type); + +void raspike_imu_set_heading(float desired_heading); + +void raspike_imu_get_heading_scaled(raspike_imu_heading_type_t type, pbio_angle_t *heading, int32_t *heading_rate, int32_t ctl_steps_per_degree); + +void raspike_imu_get_orientation(pbio_geometry_matrix_3x3_t *rotation); + +#endif // __RASPIKE_IMU_H__ + +/** @} */ diff --git a/sample/raspike/raspike_protocol_com.h b/sample/raspike/raspike_protocol_com.h index e58eaf57..93c82acc 100644 --- a/sample/raspike/raspike_protocol_com.h +++ b/sample/raspike/raspike_protocol_com.h @@ -131,7 +131,7 @@ #define RP_CMD_ID_HUB_SPK_SET_VOL (MAKE_CMD(RP_CMD_TYPE_HUB,0x11)) // 0xb1 #define RP_CMD_ID_HUB_SPK_PLY_TON (MAKE_CMD(RP_CMD_TYPE_HUB,0x12)) // 0xb2 #define RP_CMD_ID_HUB_SPK_STP (MAKE_CMD(RP_CMD_TYPE_HUB,0x13)) // 0xb3 -#define RP_CMD_ID_HUB_RST_ANG (MAKE_CMD(RP_CMD_TYPE_HUB,0x14)) // 0xb4 +#define RP_CMD_ID_HUB_IMU_INIT (MAKE_CMD(RP_CMD_TYPE_HUB,0x14)) // 0xb4 @@ -171,6 +171,20 @@ #define RP_HUB_SPK_PLY_TON_INDEX_DUR (0) #define RP_HUB_SPK_PLY_TON_INDEX_FRQ (4) +/* Hub cmd protocol + RP_CMD_ID_HUB_IMU_INIT + data[ 0]-[ 3] : gyro_stationary_threshold (float) + data[ 4]-[ 7] : accel_stationary_threshold (float) + data[ 8]-[19] : angular_velocity_bias (float[3]) + data[20]-[31] : angular_velocity_scale (float[3]) + data[32]-[55] : acceleration_correction (float[6]) +*/ +#define RP_HUB_IMU_INIT_INDEX_GYRO_STAT_THRESH (0) +#define RP_HUB_IMU_INIT_INDEX_ACCEL_STAT_THRESH (4) +#define RP_HUB_IMU_INIT_INDEX_ANGV_BIAS (8) +#define RP_HUB_IMU_INIT_INDEX_ANGV_SCALE (20) +#define RP_HUB_IMU_INIT_INDEX_ACCEL_CORRECT (32) + /* In SPIKE-RT, ID port is defined as 'A' to 'F'. RasPike treat them as 0 to 5 @@ -193,7 +207,8 @@ typedef struct { uint16_t current; float acceleration[3]; float angular_velocity[3]; - float angular[3]; + float rotation_matrix[3*3]; + float heading; uint32_t button; RPProtocolPortStatus ports[6]; } RPProtocolSpikeStatus; diff --git a/sample/raspike/spike_version b/sample/raspike/spike_version index 5c4511c3..7d6b3eb3 100644 --- a/sample/raspike/spike_version +++ b/sample/raspike/spike_version @@ -1 +1 @@ -0.0.7 \ No newline at end of file +0.0.8 \ No newline at end of file