Skip to content

Instantly share code, notes, and snippets.

@zilmina
Created September 16, 2018 18:07
Show Gist options
  • Star 0 You must be signed in to star a gist
  • Fork 0 You must be signed in to fork a gist
  • Save zilmina/120db2c7164e23ffc0227112bb31e22a to your computer and use it in GitHub Desktop.
Save zilmina/120db2c7164e23ffc0227112bb31e22a to your computer and use it in GitHub Desktop.
変更後です
void update_gyro(void) {
static float past_dps_x_raw = 0;
static float dps_x_list[GYRO_SMOOZINGLENGTH] = { 0 };
float dps_x_raw = 0;
dps_x_raw = (float) (SPI_Gyro_Xaxis_get() - mouse.gyro.x.offset_raw);
//角速度に対する返り値の傾斜補正
if (dps_x_raw >= 0) {
dps_x_raw *= X_GYRO_GAIN_FORWARD;
} else {
dps_x_raw *= X_GYRO_GAIN_BACKWARD;
}
for (int i = 1; i < GYRO_SMOOZINGLENGTH; i++) {
dps_x_list[i] = dps_x_list[i - 1];
}
dps_x_list[0] = dps_x_raw;
for (int i = 1; i < GYRO_SMOOZINGLENGTH; i++) {
dps_x_raw += dps_x_list[i];
}
dps_x_raw /= (float) GYRO_SMOOZINGLENGTH;
mouse.gyro.x.deg += (0.5F) * (dps_x_raw + past_dps_x_raw) * 0.001F / 16.4F;
past_dps_x_raw = dps_x_raw;
static float past_dps_z_raw = 0;
static float dps_z_list[GYRO_SMOOZINGLENGTH] = { 0 };
float dps_z_raw = 0;
dps_z_raw = (float) (SPI_Gyro_Zaxis_get() - mouse.gyro.z.offset_raw);
//角速度に対する返り値の傾斜補正
if (dps_z_raw >= 0) {
dps_z_raw *= Z_GYRO_GAIN_FORWARD;
} else {
dps_z_raw *= Z_GYRO_GAIN_BACKWARD;
}
for (int i = 1; i < GYRO_SMOOZINGLENGTH; i++) {
dps_z_list[i] = dps_z_list[i - 1];
}
dps_z_list[0] = dps_z_raw;
for (int i = 1; i < GYRO_SMOOZINGLENGTH; i++) {
dps_z_raw += dps_z_list[i];
}
dps_z_raw /= (float) GYRO_SMOOZINGLENGTH;
mouse.now_angular_velocity = dps_z_raw / 16.4F;
mouse.deg += (0.5F) * (dps_z_raw + past_dps_z_raw) * 0.001F / 16.4F;
past_dps_z_raw = dps_z_raw;
}
Sign up for free to join this conversation on GitHub. Already have an account? Sign in to comment