update_gyroの変化

2017年時点のGYRO取得関数

//割り込み周期：1kHz
void update_gyro(void) {
	static double past_dps_raw = 0;
	short dps_raw = 0;

	dps_raw = SPI_Gyro_Zaxis_get() - mouse.gyro.offset_raw;
//角速度に対する返り値の傾斜補正
	if (dps_raw >= 0) {
		dps_raw *= Z_GYRO_GAIN_FORWARD;
	} else {
		dps_raw *= Z_GYRO_GAIN_BACKWARD;
	}

	mouse.now_angular_velocity = dps_raw / 16.4F;
	mouse.deg += (0.5F) * (dps_raw + past_dps_raw) * 0.001F / 16.4F;
	past_dps_raw = dps_raw;

}

2018年時点のGYRO取得関数(発散しない)

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;
}