HAPSRG / Mbed 2 deprecated optWingforHAPS_Eigen

Eigen Revision

Dependencies:   mbed LPS25HB_I2C LSM9DS1 PIDcontroller Autopilot_Eigen LoopTicker GPSUBX_UART_Eigen SBUS_without_mainfile MedianFilter Eigen UsaPack solaESKF_Eigen Vector3 CalibrateMagneto FastPWM

setup.cpp@76:7fd3ac1afe3e, 2021-08-07 (annotated)

Committer:
NaotoMorita
Date:
Sat Aug 07 06:54:58 2021 +0000
Revision:
76:7fd3ac1afe3e
Parent:
73:84ffa0166e6c
Child:
77:2bf856e3eca4
async update

Who changed what in which revision?

UserRevisionLine numberNew contents of line
cocorlow 56:888379912f81 1 #include "global.hpp"
cocorlow 56:888379912f81 2
cocorlow 56:888379912f81 3 void setup()
cocorlow 56:888379912f81 4 {
cocorlow 56:888379912f81 5 pitchPID.setSetPoint(0.0);
cocorlow 56:888379912f81 6 pitchratePID.setSetPoint(0.0);
NaotoMorita 70:99f974d8960e 7 rollPID.setSetPoint(0.0);
NaotoMorita 70:99f974d8960e 8 rollratePID.setSetPoint(0.0);
cocorlow 56:888379912f81 9 pitchPID.setBias(0.0);
NaotoMorita 70:99f974d8960e 10 pitchratePID.setBias(0.0);
NaotoMorita 70:99f974d8960e 11 rollPID.setBias(0.0);
NaotoMorita 70:99f974d8960e 12 rollratePID.setBias(0.0);
cocorlow 56:888379912f81 13 pitchPID.setOutputLimits(-1.0,1.0);
cocorlow 56:888379912f81 14 pitchratePID.setOutputLimits(-1.0,1.0);
NaotoMorita 70:99f974d8960e 15 rollPID.setOutputLimits(-1.0,1.0);
NaotoMorita 70:99f974d8960e 16 rollratePID.setOutputLimits(-1.0,1.0);
NaotoMorita 70:99f974d8960e 17 pitchPID.setInputLimits(-M_PI,M_PI);
NaotoMorita 70:99f974d8960e 18 pitchratePID.setInputLimits(-M_PI,M_PI);
NaotoMorita 70:99f974d8960e 19 rollPID.setInputLimits(-M_PI,M_PI);
NaotoMorita 70:99f974d8960e 20 rollratePID.setInputLimits(-M_PI,M_PI);
cocorlow 56:888379912f81 21
NaotoMorita 70:99f974d8960e 22 servoRight.period_us(15000.0);
NaotoMorita 70:99f974d8960e 23 servoLeft.period_us(15000.0);
NaotoMorita 70:99f974d8960e 24 servoThrust.period_us(15000.0);
NaotoMorita 70:99f974d8960e 25 servoRight.pulsewidth_us(1500.0);
NaotoMorita 70:99f974d8960e 26 servoLeft.pulsewidth_us(1500.0);
NaotoMorita 70:99f974d8960e 27 servoThrust.pulsewidth_us(1100.0);
cocorlow 56:888379912f81 28
NaotoMorita 76:7fd3ac1afe3e 29 sd.baud(57600);
NaotoMorita 76:7fd3ac1afe3e 30 sd.printf("\r\nFlight Start\r\n");
cocorlow 56:888379912f81 31 accelgyro.initialize();
cocorlow 56:888379912f81 32 //加速度計のフルスケールレンジを設定
cocorlow 56:888379912f81 33 accelgyro.setFullScaleAccelRange(ACCEL_FSR);
cocorlow 56:888379912f81 34 //角速度計のフルスケールレンジを設定
cocorlow 56:888379912f81 35 accelgyro.setFullScaleGyroRange(GYRO_FSR);
cocorlow 56:888379912f81 36 //MPU6050のLPFを設定
cocorlow 56:888379912f81 37 accelgyro.setDLPFMode(MPU6050_LPF);
NaotoMorita 73:84ffa0166e6c 38 //MPU6050のレートを設定
NaotoMorita 73:84ffa0166e6c 39 accelgyro.setRate(MPU6050_SAMPLERATE);
cocorlow 56:888379912f81 40 //地磁気
cocorlow 56:888379912f81 41 mag_sensor.enable();
cocorlow 56:888379912f81 42 }
cocorlow 56:888379912f81 43
cocorlow 56:888379912f81 44 void calibrate()
cocorlow 56:888379912f81 45 {
NaotoMorita 65:ea184054e659 46 pc.serial.printf("\r\nEnter to Calibration Mode\r\n");
cocorlow 56:888379912f81 47 wait(5);
NaotoMorita 65:ea184054e659 48 pc.serial.printf("Acc and Gyro Calibration Start\r\n");
cocorlow 56:888379912f81 49
cocorlow 56:888379912f81 50 int iter_n = 10000;
cocorlow 56:888379912f81 51
NaotoMorita 68:b9f6938fab9d 52 long axs = 0;
NaotoMorita 68:b9f6938fab9d 53 long ays = 0;
NaotoMorita 68:b9f6938fab9d 54 long azs = 0;
NaotoMorita 68:b9f6938fab9d 55 double axs2 = 0.0f;
NaotoMorita 68:b9f6938fab9d 56 double ays2 = 0.0f;
NaotoMorita 68:b9f6938fab9d 57 double azs2 = 0.0f;
NaotoMorita 68:b9f6938fab9d 58 long gxs = 0;
NaotoMorita 68:b9f6938fab9d 59 long gys = 0;
NaotoMorita 68:b9f6938fab9d 60 long gzs = 0;
NaotoMorita 68:b9f6938fab9d 61 double gxs2 = 0.0f;
NaotoMorita 68:b9f6938fab9d 62 double gys2 = 0.0f;
NaotoMorita 68:b9f6938fab9d 63 double gzs2 = 0.0f;
cocorlow 56:888379912f81 64 for(int i = 0;i<iter_n ;i++)
cocorlow 56:888379912f81 65 {
NaotoMorita 68:b9f6938fab9d 66 accelgyro.getMotion6(&ax, &ay, &az, &gx, &gy, &gz);
NaotoMorita 68:b9f6938fab9d 67 axs += ax;
NaotoMorita 68:b9f6938fab9d 68 ays += ay;
NaotoMorita 68:b9f6938fab9d 69 azs += az;
NaotoMorita 68:b9f6938fab9d 70 axs2 += double(ax*ax)/iter_n;
NaotoMorita 68:b9f6938fab9d 71 ays2 += double(ay*ay)/iter_n;
NaotoMorita 68:b9f6938fab9d 72 azs2 += double(az*az)/iter_n;
NaotoMorita 68:b9f6938fab9d 73
cocorlow 56:888379912f81 74 gxs += gx;
cocorlow 56:888379912f81 75 gys += gy;
NaotoMorita 68:b9f6938fab9d 76 gzs += gz;
NaotoMorita 68:b9f6938fab9d 77 gxs2 += double(gx*gx)/iter_n;
NaotoMorita 68:b9f6938fab9d 78 gys2 += double(gy*gy)/iter_n;
NaotoMorita 68:b9f6938fab9d 79 gzs2 += double(gz*gz)/iter_n;
cocorlow 56:888379912f81 80 //wait(0.01);
cocorlow 56:888379912f81 81 }
NaotoMorita 68:b9f6938fab9d 82 axs = axs /iter_n;
NaotoMorita 68:b9f6938fab9d 83 ays = ays /iter_n;
NaotoMorita 68:b9f6938fab9d 84 azs = azs /iter_n;
cocorlow 56:888379912f81 85 gxs = gxs /iter_n;
cocorlow 56:888379912f81 86 gys = gys /iter_n;
NaotoMorita 68:b9f6938fab9d 87 gzs = gzs /iter_n;
NaotoMorita 68:b9f6938fab9d 88 double var_accx = (axs2 - double(axs*axs))/ ACCEL_SSF / ACCEL_SSF;
NaotoMorita 68:b9f6938fab9d 89 double var_accy = (ays2 - double(ays*ays))/ ACCEL_SSF / ACCEL_SSF;
NaotoMorita 68:b9f6938fab9d 90 double var_accz = (azs2 - double(azs*azs))/ ACCEL_SSF / ACCEL_SSF;
NaotoMorita 68:b9f6938fab9d 91 double var_gyrox = (gxs2 - double(gxs*gxs))/ GYRO_SSF * 0.0174533f / GYRO_SSF * 0.0174533f;
NaotoMorita 68:b9f6938fab9d 92 double var_gyroy = (gys2 - double(gys*gys))/ GYRO_SSF * 0.0174533f / GYRO_SSF * 0.0174533f;
NaotoMorita 68:b9f6938fab9d 93 double var_gyroz = (gzs2 - double(gzs*gzs))/ GYRO_SSF * 0.0174533f / GYRO_SSF * 0.0174533f;
NaotoMorita 68:b9f6938fab9d 94 pc.serial.printf("AccCovariance : %f, %f, %f \r\n",var_accx,var_accy,var_accz);
NaotoMorita 68:b9f6938fab9d 95 pc.serial.printf("GyroCovariance : %f, %f, %f \r\n",var_gyrox,var_gyroy,var_gyroz);
NaotoMorita 68:b9f6938fab9d 96
NaotoMorita 65:ea184054e659 97 pc.serial.printf("Gyrooffset : 0, 0, 0, %d, %d, %d \r\n",gxs,gys,gzs);
NaotoMorita 65:ea184054e659 98
NaotoMorita 76:7fd3ac1afe3e 99 /*
NaotoMorita 65:ea184054e659 100 pc.serial.printf("Initial Magbias (Min) : %f, %f, %f\r\n", magbiasMin[0], magbiasMin[1], magbiasMin[2]);
NaotoMorita 65:ea184054e659 101 pc.serial.printf("Initial Magbias (Max) : %f, %f, %f\r\n", magbiasMax[0], magbiasMax[1], magbiasMax[2]);
NaotoMorita 76:7fd3ac1afe3e 102 */
NaotoMorita 76:7fd3ac1afe3e 103 pc.serial.printf("Initial Magbias : %f, %f, %f, %f \r\n", magbias[0], magbias[1], magbias[2], magbias[3]);
NaotoMorita 70:99f974d8960e 104 pc.serial.printf("Acc Scale and Mag Calibration Start\r\n");
NaotoMorita 70:99f974d8960e 105 for(int i = 0;i<3;i++){
NaotoMorita 70:99f974d8960e 106 accMin[i] = -1.0f;
NaotoMorita 70:99f974d8960e 107 accMax[i] = 1.0f;
NaotoMorita 70:99f974d8960e 108 }
cocorlow 56:888379912f81 109
NaotoMorita 70:99f974d8960e 110 accMax[2] = accScaleCalibrate(5);
NaotoMorita 70:99f974d8960e 111 accMin[0] = accScaleCalibrate(1);
NaotoMorita 76:7fd3ac1afe3e 112 accMax[0] = accScaleCalibrate(2);
NaotoMorita 76:7fd3ac1afe3e 113 accMin[1] = accScaleCalibrate(3);
NaotoMorita 76:7fd3ac1afe3e 114 accMax[1] = accScaleCalibrate(4);
NaotoMorita 76:7fd3ac1afe3e 115 accMin[2] = accScaleCalibrate(6);
NaotoMorita 70:99f974d8960e 116
NaotoMorita 76:7fd3ac1afe3e 117 /*
NaotoMorita 61:c05353850017 118 magCalibrator.getExtremes(magbiasMin,magbiasMax);
NaotoMorita 65:ea184054e659 119 pc.serial.printf("Magbias (Min) : %f, %f, %f\r\n", magbiasMin[0], magbiasMin[1], magbiasMin[2]);
NaotoMorita 65:ea184054e659 120 pc.serial.printf("Magbias (Max) : %f, %f, %f\r\n", magbiasMax[0], magbiasMax[1], magbiasMax[2]);
NaotoMorita 68:b9f6938fab9d 121 magCalibrator.setExtremes(magbiasMin,magbiasMax);
NaotoMorita 76:7fd3ac1afe3e 122 */
NaotoMorita 76:7fd3ac1afe3e 123 pc.serial.printf("Magbias : %f, %f, %f, %f \r\n", magbias[0], magbias[1], magbias[2], magbias[3]);
NaotoMorita 70:99f974d8960e 124 pc.serial.printf("accMin : %f, %f, %f\r\n", accMin[0], accMin[1], accMin[2]);
NaotoMorita 70:99f974d8960e 125 pc.serial.printf("accMax : %f, %f, %f\r\n", accMax[0], accMax[1], accMax[2]);
NaotoMorita 70:99f974d8960e 126
NaotoMorita 70:99f974d8960e 127 pc.serial.printf("Keep Level \r\n");
NaotoMorita 70:99f974d8960e 128 wait(5);
NaotoMorita 70:99f974d8960e 129
NaotoMorita 65:ea184054e659 130 pc.serial.printf("Calculating pitch/roll Offset \r\n");
cocorlow 56:888379912f81 131 //姿勢オフセットを計算
cocorlow 56:888379912f81 132 rpy_align.y = 0.0f*M_PI/180.0f;
cocorlow 56:888379912f81 133 rpy_align.x = 0.0f*M_PI/180.0f;
cocorlow 56:888379912f81 134 float ave_pitch = 0.0f;
cocorlow 56:888379912f81 135 float ave_roll = 0.0f;
NaotoMorita 76:7fd3ac1afe3e 136 ekf.setQgbias(0.0f);
NaotoMorita 76:7fd3ac1afe3e 137 ekf.setQab(0.0f);
NaotoMorita 68:b9f6938fab9d 138 getIMUval();
NaotoMorita 76:7fd3ac1afe3e 139 //ekf.triad(acc/acc.Norm(), accref/accref.Norm(), mag/mag.Norm(), magref/magref.Norm());
cocorlow 56:888379912f81 140 Timer _t;
cocorlow 56:888379912f81 141 _t.start();
cocorlow 56:888379912f81 142 for (int i = 0 ; i < 2200; i++)
cocorlow 56:888379912f81 143 {
cocorlow 56:888379912f81 144 float tstart = _t.read();
cocorlow 56:888379912f81 145 //姿勢角を更新
cocorlow 56:888379912f81 146 getIMUval();
NaotoMorita 68:b9f6938fab9d 147 ekf.updateQhat(gyro, att_dt);
NaotoMorita 68:b9f6938fab9d 148 ekf.updateErrState(gyro, att_dt);
NaotoMorita 68:b9f6938fab9d 149 ekf.updateStaticAccMeasures(acc,accref);
NaotoMorita 68:b9f6938fab9d 150 ekf.fuseErr2Qhat();
NaotoMorita 68:b9f6938fab9d 151 ekf.updateMagMeasures(mag);
NaotoMorita 68:b9f6938fab9d 152 ekf.computeAngles(rpy, rpy_align);
cocorlow 56:888379912f81 153 if(i>199)
cocorlow 56:888379912f81 154 {
NaotoMorita 70:99f974d8960e 155 ave_pitch += rpy.x;
NaotoMorita 70:99f974d8960e 156 ave_roll += rpy.y;
cocorlow 56:888379912f81 157 }
cocorlow 56:888379912f81 158 wait(0.001);
cocorlow 56:888379912f81 159 float tend = _t.read();
cocorlow 56:888379912f81 160 att_dt = (tend-tstart);
cocorlow 56:888379912f81 161 }
cocorlow 56:888379912f81 162
NaotoMorita 70:99f974d8960e 163 pc.serial.printf("aliginment data(rpy.x, rpy.y, rpy.z) : %ff*M_PI/180.0f, %ff*M_PI/180.0f, 0.0f*M_PI/180.0f\r\n",ave_pitch/2000.0f*180.0f/M_PI,ave_roll/2000.0f*180.0f/M_PI);
NaotoMorita 70:99f974d8960e 164
NaotoMorita 70:99f974d8960e 165
NaotoMorita 70:99f974d8960e 166 pc.serial.printf("Calibration Complete\r\n");
cocorlow 56:888379912f81 167
cocorlow 56:888379912f81 168 while(1)
cocorlow 56:888379912f81 169 {
cocorlow 56:888379912f81 170 wait(1000);
cocorlow 56:888379912f81 171 }
NaotoMorita 70:99f974d8960e 172 }
NaotoMorita 70:99f974d8960e 173
NaotoMorita 70:99f974d8960e 174 float accScaleCalibrate(int attNo)
NaotoMorita 70:99f974d8960e 175 {
NaotoMorita 70:99f974d8960e 176 //attNo 1:Right down (acc.x Negative)
NaotoMorita 70:99f974d8960e 177 //attNo 2:Left down (acc.x Positive)
NaotoMorita 70:99f974d8960e 178 //attNo 3:Nose down (acc.y Negative)
NaotoMorita 70:99f974d8960e 179 //attNo 4:Tail down (acc.y Positive)
NaotoMorita 70:99f974d8960e 180 //attNo 5:Level (acc.z Positive)
NaotoMorita 70:99f974d8960e 181 //attNo 6:upside down (acc.z Negative)
NaotoMorita 70:99f974d8960e 182 //acc scale calibration
NaotoMorita 70:99f974d8960e 183 switch(attNo){
NaotoMorita 70:99f974d8960e 184 case 1:
NaotoMorita 70:99f974d8960e 185 pc.serial.printf("Right down (acc.x Negative)\r\n");
NaotoMorita 70:99f974d8960e 186 break;
NaotoMorita 70:99f974d8960e 187 case 2:
NaotoMorita 70:99f974d8960e 188 pc.serial.printf("Left down (acc.x Positive)\r\n");
NaotoMorita 70:99f974d8960e 189 break;
NaotoMorita 70:99f974d8960e 190 case 3:
NaotoMorita 70:99f974d8960e 191 pc.serial.printf("Nose down (acc.y Negative)\r\n");
NaotoMorita 70:99f974d8960e 192 break;
NaotoMorita 70:99f974d8960e 193 case 4:
NaotoMorita 70:99f974d8960e 194 pc.serial.printf("Tail down (acc.y Positive)\r\n");
NaotoMorita 70:99f974d8960e 195 break;
NaotoMorita 70:99f974d8960e 196 case 5:
NaotoMorita 70:99f974d8960e 197 pc.serial.printf("Level (acc.z Positive)\r\n");
NaotoMorita 70:99f974d8960e 198 break;
NaotoMorita 70:99f974d8960e 199 case 6:
NaotoMorita 70:99f974d8960e 200 pc.serial.printf("Upside down (acc.z Negative)\r\n");
NaotoMorita 70:99f974d8960e 201 break;
NaotoMorita 70:99f974d8960e 202 default :
NaotoMorita 70:99f974d8960e 203 pc.serial.printf("error");
NaotoMorita 70:99f974d8960e 204 break;
NaotoMorita 70:99f974d8960e 205 }
NaotoMorita 70:99f974d8960e 206
NaotoMorita 70:99f974d8960e 207 while(1){
NaotoMorita 70:99f974d8960e 208 double accx = 0.0;
NaotoMorita 70:99f974d8960e 209 double accy = 0.0;
NaotoMorita 70:99f974d8960e 210 double accz = 0.0;
NaotoMorita 70:99f974d8960e 211 for(int i = 0;i<100 ;i++)
NaotoMorita 70:99f974d8960e 212 {
NaotoMorita 70:99f974d8960e 213 accelgyro.getMotion6(&ax, &ay, &az, &gx, &gy, &gz);
NaotoMorita 70:99f974d8960e 214 accx += ax/ ACCEL_SSF;
NaotoMorita 70:99f974d8960e 215 accy += ay/ ACCEL_SSF;
NaotoMorita 70:99f974d8960e 216 accz += az/ ACCEL_SSF;
NaotoMorita 70:99f974d8960e 217 wait(0.01);
NaotoMorita 70:99f974d8960e 218 }
NaotoMorita 70:99f974d8960e 219 bool breakFlag = false;
NaotoMorita 70:99f974d8960e 220 switch(attNo){
NaotoMorita 70:99f974d8960e 221 case 1:
NaotoMorita 70:99f974d8960e 222 if(abs(accx/100.0+1.0)<0.1){
NaotoMorita 70:99f974d8960e 223 breakFlag = true;
NaotoMorita 70:99f974d8960e 224 };
NaotoMorita 70:99f974d8960e 225 break;
NaotoMorita 70:99f974d8960e 226 case 2:
NaotoMorita 70:99f974d8960e 227 if(abs(accx/100.0-1.0)<0.1){
NaotoMorita 70:99f974d8960e 228 breakFlag = true;
NaotoMorita 70:99f974d8960e 229 };
NaotoMorita 70:99f974d8960e 230 break;
NaotoMorita 70:99f974d8960e 231 case 3:
NaotoMorita 70:99f974d8960e 232 if(abs(accy/100.0+1.0)<0.1){
NaotoMorita 70:99f974d8960e 233 breakFlag = true;
NaotoMorita 70:99f974d8960e 234 };
NaotoMorita 70:99f974d8960e 235 break;
NaotoMorita 70:99f974d8960e 236 case 4:
NaotoMorita 70:99f974d8960e 237 if(abs(accy/100.0-1.0)<0.1){
NaotoMorita 70:99f974d8960e 238 breakFlag = true;
NaotoMorita 70:99f974d8960e 239 };
NaotoMorita 70:99f974d8960e 240 break;
NaotoMorita 70:99f974d8960e 241 case 5:
NaotoMorita 70:99f974d8960e 242 if(abs(accz/100.0-1.0)<0.1){
NaotoMorita 70:99f974d8960e 243 breakFlag = true;
NaotoMorita 70:99f974d8960e 244 };
NaotoMorita 70:99f974d8960e 245 break;
NaotoMorita 70:99f974d8960e 246 case 6:
NaotoMorita 70:99f974d8960e 247 if(abs(accz/100.0+1.0)<0.1){
NaotoMorita 70:99f974d8960e 248 breakFlag = true;
NaotoMorita 70:99f974d8960e 249 };
NaotoMorita 70:99f974d8960e 250 break;
NaotoMorita 70:99f974d8960e 251 }
NaotoMorita 70:99f974d8960e 252 if(breakFlag){break;};
NaotoMorita 70:99f974d8960e 253 pc.serial.printf("acc %f %f %f\r\n", accx/100.0,accy/100.0,accz/100.0);
NaotoMorita 70:99f974d8960e 254 }
NaotoMorita 70:99f974d8960e 255 pc.serial.printf("Keep it hold\r\n");
NaotoMorita 70:99f974d8960e 256 int iter_n = 1000;
NaotoMorita 70:99f974d8960e 257 double accx = 0.0;
NaotoMorita 70:99f974d8960e 258 double accy = 0.0;
NaotoMorita 70:99f974d8960e 259 double accz = 0.0;
NaotoMorita 76:7fd3ac1afe3e 260 float magval[3] = {0,0,0};
NaotoMorita 70:99f974d8960e 261 for(int i = 0;i<iter_n ;i++)
NaotoMorita 70:99f974d8960e 262 {
NaotoMorita 70:99f974d8960e 263 accelgyro.getMotion6(&ax, &ay, &az, &gx, &gy, &gz);
NaotoMorita 70:99f974d8960e 264 accx += ax/ ACCEL_SSF;
NaotoMorita 70:99f974d8960e 265 accy += ay/ ACCEL_SSF;
NaotoMorita 70:99f974d8960e 266 accz += az/ ACCEL_SSF;
NaotoMorita 70:99f974d8960e 267
NaotoMorita 70:99f974d8960e 268 mag_sensor.getAxis(mdata); // flush the magnetmeter
NaotoMorita 76:7fd3ac1afe3e 269 magval[0] = (mdata.x - magbias[0]);
NaotoMorita 76:7fd3ac1afe3e 270 magval[1] = (mdata.y - magbias[1]);
NaotoMorita 76:7fd3ac1afe3e 271 magval[2] = (mdata.z - magbias[2]);
NaotoMorita 76:7fd3ac1afe3e 272 float mag_r = magval[0]*magval[0] + magval[1]*magval[1] + magval[2]*magval[2];
NaotoMorita 76:7fd3ac1afe3e 273 float lr = 0.00001f;
NaotoMorita 76:7fd3ac1afe3e 274 float f = mag_r - magbias[3]*magbias[3];
NaotoMorita 76:7fd3ac1afe3e 275 magbias[0] = magbias[0] + 4 * lr * f * magval[0];
NaotoMorita 76:7fd3ac1afe3e 276 magbias[1] = magbias[1] + 4 * lr * f * magval[1];
NaotoMorita 76:7fd3ac1afe3e 277 magbias[2] = magbias[2] + 4 * lr * f * magval[2];
NaotoMorita 76:7fd3ac1afe3e 278 magbias[3] = magbias[3] + 4 * lr * f * magbias[3];
NaotoMorita 70:99f974d8960e 279 wait(0.001);
NaotoMorita 70:99f974d8960e 280 }
NaotoMorita 70:99f974d8960e 281 float returnval = 0.0f;
NaotoMorita 70:99f974d8960e 282 switch(attNo){
NaotoMorita 70:99f974d8960e 283 case 1:
NaotoMorita 70:99f974d8960e 284 returnval = accx/1000.0f;
NaotoMorita 70:99f974d8960e 285 break;
NaotoMorita 70:99f974d8960e 286 case 2:
NaotoMorita 70:99f974d8960e 287 returnval = accx/1000.0f;
NaotoMorita 70:99f974d8960e 288 break;
NaotoMorita 70:99f974d8960e 289 case 3:
NaotoMorita 70:99f974d8960e 290 returnval = accy/1000.0f;
NaotoMorita 70:99f974d8960e 291 break;
NaotoMorita 70:99f974d8960e 292 case 4:
NaotoMorita 70:99f974d8960e 293 returnval = accy/1000.0f;
NaotoMorita 70:99f974d8960e 294 break;
NaotoMorita 70:99f974d8960e 295 case 5:
NaotoMorita 70:99f974d8960e 296 returnval = accz/1000.0f;
NaotoMorita 70:99f974d8960e 297 break;
NaotoMorita 70:99f974d8960e 298 case 6:
NaotoMorita 70:99f974d8960e 299 returnval = accz/1000.0f;
NaotoMorita 70:99f974d8960e 300 break;
NaotoMorita 70:99f974d8960e 301 }
NaotoMorita 70:99f974d8960e 302
NaotoMorita 70:99f974d8960e 303 return returnval;
cocorlow 56:888379912f81 304 }