Hochschule München / Mbed 2 deprecated SatelitenSimulator

UM6 on AirBearing

Dependencies:   MODSERIAL mbed

main.cpp@0:8a2c63ece2a9, 2014-04-10 (annotated)

Committer:
HMFK03LST1
Date:
Thu Apr 10 11:17:09 2014 +0000
Revision:
0:8a2c63ece2a9
UM6 on AirBearing

Who changed what in which revision?

UserRevisionLine numberNew contents of line
HMFK03LST1 0:8a2c63ece2a9 1 #include "mbed.h"
HMFK03LST1 0:8a2c63ece2a9 2 #include "math.h"
HMFK03LST1 0:8a2c63ece2a9 3 #include "MODSERIAL.h" // MBED BUFFERED SERIAL HEADER
HMFK03LST1 0:8a2c63ece2a9 4 #include "UM6_usart.h" // UM6 USART HEADER
HMFK03LST1 0:8a2c63ece2a9 5 #include "UM6_config.h" // UM6 CONFIG HEADER
HMFK03LST1 0:8a2c63ece2a9 6
HMFK03LST1 0:8a2c63ece2a9 7 #define PI 3.1415926
HMFK03LST1 0:8a2c63ece2a9 8 #define acc_acu 8
HMFK03LST1 0:8a2c63ece2a9 9 #define mag_acu 8
HMFK03LST1 0:8a2c63ece2a9 10
HMFK03LST1 0:8a2c63ece2a9 11
HMFK03LST1 0:8a2c63ece2a9 12
HMFK03LST1 0:8a2c63ece2a9 13 //Serial ios(p28, p27); // Serielle Verbi. mit XBee über Pin: tx-28, rx-27
HMFK03LST1 0:8a2c63ece2a9 14 Serial ios(USBTX, USBRX); // Serielle Verbi. über USB Port vom PC
HMFK03LST1 0:8a2c63ece2a9 15
HMFK03LST1 0:8a2c63ece2a9 16 DigitalOut rst(p11); // Digital Reset für the XBee, 200ns für reset
HMFK03LST1 0:8a2c63ece2a9 17 PwmOut x_kreisel(p21); // Pin21-PwmOut ist für Drehung um X-Achse
HMFK03LST1 0:8a2c63ece2a9 18 PwmOut y_kreisel(p23); // Pin23-PwmOut ist für Drehung um Y-Achse
HMFK03LST1 0:8a2c63ece2a9 19 PwmOut z_kreisel(p22); // Pin22-PwmOut ist für Drehung um Z-Achse
HMFK03LST1 0:8a2c63ece2a9 20
HMFK03LST1 0:8a2c63ece2a9 21 DigitalOut uart_activity(LED1); // LED1 = UM6 SERIAL für Kommunikation
HMFK03LST1 0:8a2c63ece2a9 22
HMFK03LST1 0:8a2c63ece2a9 23 //acc struct
HMFK03LST1 0:8a2c63ece2a9 24 struct {
HMFK03LST1 0:8a2c63ece2a9 25 float x;
HMFK03LST1 0:8a2c63ece2a9 26 float y;
HMFK03LST1 0:8a2c63ece2a9 27 float z;
HMFK03LST1 0:8a2c63ece2a9 28 }raw_acc;
HMFK03LST1 0:8a2c63ece2a9 29
HMFK03LST1 0:8a2c63ece2a9 30 struct {
HMFK03LST1 0:8a2c63ece2a9 31 float g;
HMFK03LST1 0:8a2c63ece2a9 32 float xy;
HMFK03LST1 0:8a2c63ece2a9 33 float zx;
HMFK03LST1 0:8a2c63ece2a9 34 float zy;
HMFK03LST1 0:8a2c63ece2a9 35 float x;
HMFK03LST1 0:8a2c63ece2a9 36 float y;
HMFK03LST1 0:8a2c63ece2a9 37 float z;
HMFK03LST1 0:8a2c63ece2a9 38 }norm_acc;
HMFK03LST1 0:8a2c63ece2a9 39
HMFK03LST1 0:8a2c63ece2a9 40 struct {
HMFK03LST1 0:8a2c63ece2a9 41 float x;
HMFK03LST1 0:8a2c63ece2a9 42 float y;
HMFK03LST1 0:8a2c63ece2a9 43 float z;
HMFK03LST1 0:8a2c63ece2a9 44 }euler_acc_raw;
HMFK03LST1 0:8a2c63ece2a9 45
HMFK03LST1 0:8a2c63ece2a9 46 struct {
HMFK03LST1 0:8a2c63ece2a9 47 double x;
HMFK03LST1 0:8a2c63ece2a9 48 double y;
HMFK03LST1 0:8a2c63ece2a9 49 double z;
HMFK03LST1 0:8a2c63ece2a9 50 char x_acu;
HMFK03LST1 0:8a2c63ece2a9 51 char z_acu;
HMFK03LST1 0:8a2c63ece2a9 52 char y_acu;
HMFK03LST1 0:8a2c63ece2a9 53 }euler_acc;
HMFK03LST1 0:8a2c63ece2a9 54
HMFK03LST1 0:8a2c63ece2a9 55 struct {
HMFK03LST1 0:8a2c63ece2a9 56 double x;
HMFK03LST1 0:8a2c63ece2a9 57 double y;
HMFK03LST1 0:8a2c63ece2a9 58 double z;
HMFK03LST1 0:8a2c63ece2a9 59
HMFK03LST1 0:8a2c63ece2a9 60 }euler_m;
HMFK03LST1 0:8a2c63ece2a9 61
HMFK03LST1 0:8a2c63ece2a9 62
HMFK03LST1 0:8a2c63ece2a9 63 //mag struct
HMFK03LST1 0:8a2c63ece2a9 64 struct {
HMFK03LST1 0:8a2c63ece2a9 65 float x;
HMFK03LST1 0:8a2c63ece2a9 66 float y;
HMFK03LST1 0:8a2c63ece2a9 67 float z;
HMFK03LST1 0:8a2c63ece2a9 68 }raw_mag;
HMFK03LST1 0:8a2c63ece2a9 69
HMFK03LST1 0:8a2c63ece2a9 70 struct {
HMFK03LST1 0:8a2c63ece2a9 71 float g;
HMFK03LST1 0:8a2c63ece2a9 72 float xy;
HMFK03LST1 0:8a2c63ece2a9 73 float zx;
HMFK03LST1 0:8a2c63ece2a9 74 float zy;
HMFK03LST1 0:8a2c63ece2a9 75 float x;
HMFK03LST1 0:8a2c63ece2a9 76 float y;
HMFK03LST1 0:8a2c63ece2a9 77 float z;
HMFK03LST1 0:8a2c63ece2a9 78 }norm_mag;
HMFK03LST1 0:8a2c63ece2a9 79
HMFK03LST1 0:8a2c63ece2a9 80 struct {
HMFK03LST1 0:8a2c63ece2a9 81 float x;
HMFK03LST1 0:8a2c63ece2a9 82 float y;
HMFK03LST1 0:8a2c63ece2a9 83 float z;
HMFK03LST1 0:8a2c63ece2a9 84 }euler_mag_raw;
HMFK03LST1 0:8a2c63ece2a9 85
HMFK03LST1 0:8a2c63ece2a9 86 struct {
HMFK03LST1 0:8a2c63ece2a9 87 float x;
HMFK03LST1 0:8a2c63ece2a9 88 char x_acu;
HMFK03LST1 0:8a2c63ece2a9 89 float y;
HMFK03LST1 0:8a2c63ece2a9 90 char y_acu;
HMFK03LST1 0:8a2c63ece2a9 91 float z;
HMFK03LST1 0:8a2c63ece2a9 92 char z_acu;
HMFK03LST1 0:8a2c63ece2a9 93 float bias_x;
HMFK03LST1 0:8a2c63ece2a9 94 float bias_y;
HMFK03LST1 0:8a2c63ece2a9 95 float bias_z;
HMFK03LST1 0:8a2c63ece2a9 96 }euler_mag;
HMFK03LST1 0:8a2c63ece2a9 97
HMFK03LST1 0:8a2c63ece2a9 98 bool recive;
HMFK03LST1 0:8a2c63ece2a9 99
HMFK03LST1 0:8a2c63ece2a9 100
HMFK03LST1 0:8a2c63ece2a9 101 ///////////////////////////////////////////////////////////////////////////////////////////////////
HMFK03LST1 0:8a2c63ece2a9 102 // rxCallback // FUNKTION FÜR INTERRUPT
HMFK03LST1 0:8a2c63ece2a9 103
HMFK03LST1 0:8a2c63ece2a9 104 void rxCallback(MODSERIAL_IRQ_INFO *q)
HMFK03LST1 0:8a2c63ece2a9 105 {
HMFK03LST1 0:8a2c63ece2a9 106 if (um6_uart.rxBufferGetCount() >= MAX_PACKET_DATA)
HMFK03LST1 0:8a2c63ece2a9 107 {
HMFK03LST1 0:8a2c63ece2a9 108 uart_activity = !uart_activity; // LED leuchtet wenn RxBuff hat > 40 Bytes
HMFK03LST1 0:8a2c63ece2a9 109 Process_um6_packet();
HMFK03LST1 0:8a2c63ece2a9 110 }
HMFK03LST1 0:8a2c63ece2a9 111 }
HMFK03LST1 0:8a2c63ece2a9 112
HMFK03LST1 0:8a2c63ece2a9 113
HMFK03LST1 0:8a2c63ece2a9 114 float deg_diff(float start_angle, float end_angle)
HMFK03LST1 0:8a2c63ece2a9 115 {
HMFK03LST1 0:8a2c63ece2a9 116 float left;
HMFK03LST1 0:8a2c63ece2a9 117
HMFK03LST1 0:8a2c63ece2a9 118 left = end_angle - start_angle;
HMFK03LST1 0:8a2c63ece2a9 119
HMFK03LST1 0:8a2c63ece2a9 120 if (left > 180) {left = ((end_angle - 360)-start_angle);}
HMFK03LST1 0:8a2c63ece2a9 121 if (left < -180) {left = ((end_angle + 360)-start_angle);}
HMFK03LST1 0:8a2c63ece2a9 122 return left;
HMFK03LST1 0:8a2c63ece2a9 123 }
HMFK03LST1 0:8a2c63ece2a9 124
HMFK03LST1 0:8a2c63ece2a9 125 float rad_to_deg(float rad)
HMFK03LST1 0:8a2c63ece2a9 126 { return((rad) * (180.0 / PI));}
HMFK03LST1 0:8a2c63ece2a9 127
HMFK03LST1 0:8a2c63ece2a9 128 // Acc Sensor
HMFK03LST1 0:8a2c63ece2a9 129
HMFK03LST1 0:8a2c63ece2a9 130 int sgn(double x)
HMFK03LST1 0:8a2c63ece2a9 131 {
HMFK03LST1 0:8a2c63ece2a9 132 if (x > 0.0L)
HMFK03LST1 0:8a2c63ece2a9 133 return 1.0L;
HMFK03LST1 0:8a2c63ece2a9 134 else if (x < 0.0L)
HMFK03LST1 0:8a2c63ece2a9 135 return -1.0L;
HMFK03LST1 0:8a2c63ece2a9 136 else
HMFK03LST1 0:8a2c63ece2a9 137 return 0.0L;
HMFK03LST1 0:8a2c63ece2a9 138 }
HMFK03LST1 0:8a2c63ece2a9 139
HMFK03LST1 0:8a2c63ece2a9 140 void make_acc_norm()
HMFK03LST1 0:8a2c63ece2a9 141 {
HMFK03LST1 0:8a2c63ece2a9 142 norm_acc.g = sqrt((data.Accel_Proc_X * data.Accel_Proc_X) + (data.Accel_Proc_Y * data.Accel_Proc_Y) + (data.Accel_Proc_Z * data.Accel_Proc_Z));
HMFK03LST1 0:8a2c63ece2a9 143 norm_acc.xy = sqrt((data.Accel_Proc_X * data.Accel_Proc_X) + (data.Accel_Proc_Y * data.Accel_Proc_Y));
HMFK03LST1 0:8a2c63ece2a9 144 norm_acc.zx = sqrt((data.Accel_Proc_Z * data.Accel_Proc_Z) + (data.Accel_Proc_X * data.Accel_Proc_X));
HMFK03LST1 0:8a2c63ece2a9 145 norm_acc.zy = sqrt((data.Accel_Proc_Z * data.Accel_Proc_Z) + (data.Accel_Proc_Y * data.Accel_Proc_Y));
HMFK03LST1 0:8a2c63ece2a9 146
HMFK03LST1 0:8a2c63ece2a9 147 norm_acc.x = data.Accel_Proc_X / norm_acc.g;
HMFK03LST1 0:8a2c63ece2a9 148 norm_acc.y = data.Accel_Proc_Y / norm_acc.g;
HMFK03LST1 0:8a2c63ece2a9 149 norm_acc.z = data.Accel_Proc_Z / norm_acc.g;
HMFK03LST1 0:8a2c63ece2a9 150 }
HMFK03LST1 0:8a2c63ece2a9 151
HMFK03LST1 0:8a2c63ece2a9 152 char nicken_y_acc_raw()
HMFK03LST1 0:8a2c63ece2a9 153 {
HMFK03LST1 0:8a2c63ece2a9 154 if(norm_acc.z != 0)
HMFK03LST1 0:8a2c63ece2a9 155 {euler_acc_raw.y = atan(norm_acc.x / norm_acc.z);}
HMFK03LST1 0:8a2c63ece2a9 156 else {return (0);}
HMFK03LST1 0:8a2c63ece2a9 157
HMFK03LST1 0:8a2c63ece2a9 158 return (acc_acu * norm_acc.zx/norm_acc.g);
HMFK03LST1 0:8a2c63ece2a9 159 }
HMFK03LST1 0:8a2c63ece2a9 160
HMFK03LST1 0:8a2c63ece2a9 161 char rollen_x_acc_raw()
HMFK03LST1 0:8a2c63ece2a9 162 {
HMFK03LST1 0:8a2c63ece2a9 163 if(norm_acc.z != 0)
HMFK03LST1 0:8a2c63ece2a9 164 {euler_acc_raw.x = atan(norm_acc.y / norm_acc.z);}
HMFK03LST1 0:8a2c63ece2a9 165 else {return (0);}
HMFK03LST1 0:8a2c63ece2a9 166
HMFK03LST1 0:8a2c63ece2a9 167 return (acc_acu * norm_acc.zy/norm_acc.g);
HMFK03LST1 0:8a2c63ece2a9 168 }
HMFK03LST1 0:8a2c63ece2a9 169
HMFK03LST1 0:8a2c63ece2a9 170 char drehen_z_acc_raw()
HMFK03LST1 0:8a2c63ece2a9 171 {
HMFK03LST1 0:8a2c63ece2a9 172 if((norm_acc.y != 0)&&(norm_acc.xy != 0))
HMFK03LST1 0:8a2c63ece2a9 173 {euler_acc_raw.z = asin(norm_acc.y / norm_acc.xy);
HMFK03LST1 0:8a2c63ece2a9 174 if (sgn(norm_acc.x) < 0) { if (euler_acc_raw.z > 0) {euler_acc_raw.z = PI - euler_acc_raw.z ;} else {euler_acc_raw.z = -PI - euler_acc_raw.z ;} }
HMFK03LST1 0:8a2c63ece2a9 175 }
HMFK03LST1 0:8a2c63ece2a9 176 else {return (0);}
HMFK03LST1 0:8a2c63ece2a9 177
HMFK03LST1 0:8a2c63ece2a9 178 if (euler_acc_raw.z < 0) {euler_acc_raw.z = 2 * PI + euler_acc_raw.z;}
HMFK03LST1 0:8a2c63ece2a9 179
HMFK03LST1 0:8a2c63ece2a9 180 return (acc_acu * norm_acc.xy/norm_acc.g);
HMFK03LST1 0:8a2c63ece2a9 181 }
HMFK03LST1 0:8a2c63ece2a9 182
HMFK03LST1 0:8a2c63ece2a9 183 void position_acc ()
HMFK03LST1 0:8a2c63ece2a9 184 {
HMFK03LST1 0:8a2c63ece2a9 185
HMFK03LST1 0:8a2c63ece2a9 186 make_acc_norm();
HMFK03LST1 0:8a2c63ece2a9 187
HMFK03LST1 0:8a2c63ece2a9 188 euler_acc.x_acu = rollen_x_acc_raw();
HMFK03LST1 0:8a2c63ece2a9 189 euler_acc.y_acu = nicken_y_acc_raw();
HMFK03LST1 0:8a2c63ece2a9 190 euler_acc.z_acu = drehen_z_acc_raw();
HMFK03LST1 0:8a2c63ece2a9 191
HMFK03LST1 0:8a2c63ece2a9 192 euler_acc.x = rad_to_deg(euler_acc_raw.x);
HMFK03LST1 0:8a2c63ece2a9 193 euler_acc.y = rad_to_deg(euler_acc_raw.y);
HMFK03LST1 0:8a2c63ece2a9 194 euler_acc.z = rad_to_deg(euler_acc_raw.z);
HMFK03LST1 0:8a2c63ece2a9 195 }
HMFK03LST1 0:8a2c63ece2a9 196
HMFK03LST1 0:8a2c63ece2a9 197
HMFK03LST1 0:8a2c63ece2a9 198
HMFK03LST1 0:8a2c63ece2a9 199 // Magnetometer
HMFK03LST1 0:8a2c63ece2a9 200
HMFK03LST1 0:8a2c63ece2a9 201 void make_mag_norm()
HMFK03LST1 0:8a2c63ece2a9 202 {
HMFK03LST1 0:8a2c63ece2a9 203 norm_mag.g = sqrt((data.Mag_Proc_X * data.Mag_Proc_X) + (data.Mag_Proc_Y * data.Mag_Proc_Y) + (data.Mag_Proc_Z * data.Mag_Proc_Z));
HMFK03LST1 0:8a2c63ece2a9 204 norm_mag.xy = sqrt((data.Mag_Proc_X * data.Mag_Proc_X) + (data.Mag_Proc_Y * data.Mag_Proc_Y));
HMFK03LST1 0:8a2c63ece2a9 205 norm_mag.zx = sqrt((data.Mag_Proc_Z * data.Mag_Proc_Z) + (data.Mag_Proc_X * data.Mag_Proc_X));
HMFK03LST1 0:8a2c63ece2a9 206 norm_mag.zy = sqrt((data.Mag_Proc_Z * data.Mag_Proc_Z) + (data.Mag_Proc_Y * data.Mag_Proc_Y));
HMFK03LST1 0:8a2c63ece2a9 207
HMFK03LST1 0:8a2c63ece2a9 208 norm_mag.x = data.Mag_Proc_X / norm_mag.g;
HMFK03LST1 0:8a2c63ece2a9 209 norm_mag.y = data.Mag_Proc_Y / norm_mag.g;
HMFK03LST1 0:8a2c63ece2a9 210 norm_mag.z = data.Mag_Proc_Z / norm_mag.g;
HMFK03LST1 0:8a2c63ece2a9 211 }
HMFK03LST1 0:8a2c63ece2a9 212
HMFK03LST1 0:8a2c63ece2a9 213 char nicken_y_mag_raw()
HMFK03LST1 0:8a2c63ece2a9 214 {
HMFK03LST1 0:8a2c63ece2a9 215 if(norm_mag.z != 0)
HMFK03LST1 0:8a2c63ece2a9 216 {euler_mag_raw.y = atan(norm_mag.x / norm_mag.z);}
HMFK03LST1 0:8a2c63ece2a9 217 else {return (0);}
HMFK03LST1 0:8a2c63ece2a9 218
HMFK03LST1 0:8a2c63ece2a9 219 return (acc_acu * norm_mag.zx/norm_mag.g);
HMFK03LST1 0:8a2c63ece2a9 220 }
HMFK03LST1 0:8a2c63ece2a9 221
HMFK03LST1 0:8a2c63ece2a9 222 char rollen_x_mag_raw()
HMFK03LST1 0:8a2c63ece2a9 223 {
HMFK03LST1 0:8a2c63ece2a9 224 if(norm_mag.z != 0)
HMFK03LST1 0:8a2c63ece2a9 225 {euler_mag_raw.x = atan(norm_mag.y / norm_mag.z);}
HMFK03LST1 0:8a2c63ece2a9 226 else {return (0);}
HMFK03LST1 0:8a2c63ece2a9 227
HMFK03LST1 0:8a2c63ece2a9 228 return (acc_acu *norm_mag.zy/norm_mag.g);
HMFK03LST1 0:8a2c63ece2a9 229 }
HMFK03LST1 0:8a2c63ece2a9 230
HMFK03LST1 0:8a2c63ece2a9 231 char drehen_z_mag_raw()
HMFK03LST1 0:8a2c63ece2a9 232 {
HMFK03LST1 0:8a2c63ece2a9 233 if((norm_mag.y != 0)&&(norm_mag.xy != 0))
HMFK03LST1 0:8a2c63ece2a9 234 {euler_mag_raw.z = asin(norm_mag.x / norm_mag.xy) * (norm_mag.y / abs(norm_mag.y));}
HMFK03LST1 0:8a2c63ece2a9 235 else {return (0);}
HMFK03LST1 0:8a2c63ece2a9 236
HMFK03LST1 0:8a2c63ece2a9 237 if (euler_mag_raw.z < 0) {euler_mag_raw.z = 6.28 + euler_mag_raw.z;}
HMFK03LST1 0:8a2c63ece2a9 238
HMFK03LST1 0:8a2c63ece2a9 239 return (acc_acu *norm_mag.xy/norm_mag.g);
HMFK03LST1 0:8a2c63ece2a9 240 }
HMFK03LST1 0:8a2c63ece2a9 241
HMFK03LST1 0:8a2c63ece2a9 242 void position_mag ()
HMFK03LST1 0:8a2c63ece2a9 243 {
HMFK03LST1 0:8a2c63ece2a9 244
HMFK03LST1 0:8a2c63ece2a9 245 make_mag_norm();
HMFK03LST1 0:8a2c63ece2a9 246
HMFK03LST1 0:8a2c63ece2a9 247 euler_mag.x_acu = rollen_x_mag_raw();
HMFK03LST1 0:8a2c63ece2a9 248 euler_mag.y_acu = nicken_y_mag_raw();
HMFK03LST1 0:8a2c63ece2a9 249 euler_mag.z_acu = drehen_z_mag_raw();
HMFK03LST1 0:8a2c63ece2a9 250
HMFK03LST1 0:8a2c63ece2a9 251 euler_mag.x = euler_mag_raw.x - euler_mag.bias_x;
HMFK03LST1 0:8a2c63ece2a9 252 euler_mag.y = euler_mag_raw.y - euler_mag.bias_y;
HMFK03LST1 0:8a2c63ece2a9 253 euler_mag.z = euler_mag_raw.z;
HMFK03LST1 0:8a2c63ece2a9 254 }
HMFK03LST1 0:8a2c63ece2a9 255
HMFK03LST1 0:8a2c63ece2a9 256 void print_data()
HMFK03LST1 0:8a2c63ece2a9 257 {
HMFK03LST1 0:8a2c63ece2a9 258 ios.printf("\n\r Data: Acc Mag");
HMFK03LST1 0:8a2c63ece2a9 259 ios.printf("\n\r x: %5.1f %d x: %5.1f %d",euler_m.x,euler_acc.x_acu, rad_to_deg(euler_mag.x),euler_mag.x_acu);
HMFK03LST1 0:8a2c63ece2a9 260 ios.printf("\n\r y: %5.1f %d y: %5.1f %d",euler_m.y,euler_acc.y_acu, rad_to_deg(euler_mag.y),euler_mag.y_acu);
HMFK03LST1 0:8a2c63ece2a9 261 ios.printf("\n\r z: %5.1f %d z: %5.1f %d",euler_m.z,euler_acc.z_acu, rad_to_deg(euler_mag.z),euler_mag.z_acu);
HMFK03LST1 0:8a2c63ece2a9 262 ios.printf("\n\r");
HMFK03LST1 0:8a2c63ece2a9 263 }
HMFK03LST1 0:8a2c63ece2a9 264
HMFK03LST1 0:8a2c63ece2a9 265
HMFK03LST1 0:8a2c63ece2a9 266 int main() {
HMFK03LST1 0:8a2c63ece2a9 267 unsigned int count = 0;
HMFK03LST1 0:8a2c63ece2a9 268
HMFK03LST1 0:8a2c63ece2a9 269 ios.baud(115200); // Baudrate XBee Funkmodul
HMFK03LST1 0:8a2c63ece2a9 270 um6_uart.baud(115200); // Baudrate UM6-lt
HMFK03LST1 0:8a2c63ece2a9 271 um6_uart.attach(&rxCallback, MODSERIAL::RxIrq); // Interrupt Funktion für UART
HMFK03LST1 0:8a2c63ece2a9 272
HMFK03LST1 0:8a2c63ece2a9 273 euler_m.x = 0.0;
HMFK03LST1 0:8a2c63ece2a9 274 euler_m.y = 0.0;
HMFK03LST1 0:8a2c63ece2a9 275 euler_m.z = 0.0;
HMFK03LST1 0:8a2c63ece2a9 276 euler_acc.x = 0.0;
HMFK03LST1 0:8a2c63ece2a9 277 euler_acc.y = 0.0;
HMFK03LST1 0:8a2c63ece2a9 278 euler_acc.z = 0.0;
HMFK03LST1 0:8a2c63ece2a9 279
HMFK03LST1 0:8a2c63ece2a9 280
HMFK03LST1 0:8a2c63ece2a9 281 while(1) {
HMFK03LST1 0:8a2c63ece2a9 282 if (recive) {
HMFK03LST1 0:8a2c63ece2a9 283 recive = 0;
HMFK03LST1 0:8a2c63ece2a9 284 count++;
HMFK03LST1 0:8a2c63ece2a9 285
HMFK03LST1 0:8a2c63ece2a9 286 if ((count%2) == 1)
HMFK03LST1 0:8a2c63ece2a9 287 {
HMFK03LST1 0:8a2c63ece2a9 288 position_acc();
HMFK03LST1 0:8a2c63ece2a9 289 position_mag();
HMFK03LST1 0:8a2c63ece2a9 290
HMFK03LST1 0:8a2c63ece2a9 291 euler_m.x = euler_m.x + ((euler_acc.x - euler_m.x)/24);
HMFK03LST1 0:8a2c63ece2a9 292 euler_m.y = euler_m.y + ((euler_acc.y - euler_m.y)/24);
HMFK03LST1 0:8a2c63ece2a9 293 euler_m.z = euler_m.z + ((euler_acc.z - euler_m.z)/24);
HMFK03LST1 0:8a2c63ece2a9 294 }
HMFK03LST1 0:8a2c63ece2a9 295
HMFK03LST1 0:8a2c63ece2a9 296 if ((count%20) == 1)
HMFK03LST1 0:8a2c63ece2a9 297 {
HMFK03LST1 0:8a2c63ece2a9 298 //euler_m.x = euler_m.x/3;
HMFK03LST1 0:8a2c63ece2a9 299 print_data();
HMFK03LST1 0:8a2c63ece2a9 300 }
HMFK03LST1 0:8a2c63ece2a9 301
HMFK03LST1 0:8a2c63ece2a9 302 }
HMFK03LST1 0:8a2c63ece2a9 303 }
HMFK03LST1 0:8a2c63ece2a9 304 }