Funai Ryotaro
/
mag_sensor
テスト
Fork of mag_sensor by
Diff: main.cpp
- Revision:
- 2:8ba36609face
- Parent:
- 1:21ba826811d6
diff -r 21ba826811d6 -r 8ba36609face main.cpp --- a/main.cpp Sun Aug 12 06:34:00 2018 +0000 +++ b/main.cpp Sun Aug 12 08:28:02 2018 +0000 @@ -76,8 +76,11 @@ pc.printf("%f,%f\n\r", magX, magY); //ID = IDcheck(); //pc.printf("%f\n\r", ID); - rad = atan2(magZ * sin(roll) - magY * cos(roll), magX * cos(pitch) + magY * sin(pitch) * sin(roll) + magZ * sin(pitch) * cos(roll)); - degree = -((int)(rad * 180.0 / 3.141592 + 270.0 - 7.5) % 360 - 360.0); + double y_houi = magZ * sin(roll) - magY * cos(roll); + double x_houi = magX * cos(pitch) + magY * sin(pitch) * sin(roll) + magZ * sin(pitch) * cos(roll); + double num = y_houi / x_houi; + rad = atan(num); + degree = -((int)(rad * 360.0 / 3.141592 + 270.0 - 7.5) % 360 - 360.0); /* if(degree < 0){ degree += 360;