SES / Mbed 2 deprecated LPC824mother

test

Dependencies:   mbed

Fork of BNO055_HelloWorld by Dave Turner

main.cpp@1:b178e785986d, 2018-05-22 (annotated)

Committer:
SES01
Date:
Tue May 22 11:29:05 2018 +0000
Revision:
1:b178e785986d
Parent:
0:a8e6e26ebe0f 
test

Who changed what in which revision?

UserRevisionLine numberNew contents of line
StressedDave 0:a8e6e26ebe0f 1 #include "mbed.h"
SES01 1:b178e785986d 2 #include <math.h>
SES01 1:b178e785986d 3
StressedDave 0:a8e6e26ebe0f 4 #include "BNO055.h"
StressedDave 0:a8e6e26ebe0f 5
SES01 1:b178e785986d 6 #define DIGITS 8
SES01 1:b178e785986d 7 //円周率
SES01 1:b178e785986d 8 #define PI 3.14159265358979
SES01 1:b178e785986d 9 //モータードライバ
SES01 1:b178e785986d 10 #define HF_MOTRE 0x01
SES01 1:b178e785986d 11 #define HB_MOTRE 0x02
SES01 1:b178e785986d 12 #define MF_MOTRE 0x03
SES01 1:b178e785986d 13 #define MB_MOTRE 0x04
SES01 1:b178e785986d 14 #define ARM_UD 0x05
SES01 1:b178e785986d 15 #define ARM_OC 0x06
SES01 1:b178e785986d 16 #define RERAY 0x07
SES01 1:b178e785986d 17 #define R1 0x0A
SES01 1:b178e785986d 18 //モータードライバへの命令
SES01 1:b178e785986d 19 #define STOP 0
SES01 1:b178e785986d 20 #define CW 1
SES01 1:b178e785986d 21 #define CCW -1
SES01 1:b178e785986d 22 #define bSTOP 10
SES01 1:b178e785986d 23 #define bCW 11
SES01 1:b178e785986d 24 #define bCCW -11
SES01 1:b178e785986d 25
SES01 1:b178e785986d 26 #define sCW 21
SES01 1:b178e785986d 27 #define sCCW -21
SES01 1:b178e785986d 28 #define shCW 31
SES01 1:b178e785986d 29 #define shCCW -31
SES01 1:b178e785986d 30
SES01 1:b178e785986d 31 DigitalOut led1(LED1);
SES01 1:b178e785986d 32 DigitalOut led2(LED2);
SES01 1:b178e785986d 33 DigitalOut led3(LED3);
SES01 1:b178e785986d 34 //DigitalOut ledx(LED4);
SES01 1:b178e785986d 35 Serial serial(P0_15, P0_1);
SES01 1:b178e785986d 36 Serial s485(P0_26, P0_16);
SES01 1:b178e785986d 37 DigitalOut DE(P0_27);
SES01 1:b178e785986d 38
SES01 1:b178e785986d 39 AnalogIn test(P0_4);
SES01 1:b178e785986d 40
SES01 1:b178e785986d 41 //Serial serial2(p28, p27);
SES01 1:b178e785986d 42 I2C md_i2c_tx(P0_11, P0_10);
SES01 1:b178e785986d 43 Ticker timer;
StressedDave 0:a8e6e26ebe0f 44 Serial pc(USBTX, USBRX);
SES01 1:b178e785986d 45
SES01 1:b178e785986d 46 BNO055 imu(dp15, dp25);
SES01 1:b178e785986d 47 double yaw, roll, pitch;
SES01 1:b178e785986d 48
SES01 1:b178e785986d 49
SES01 1:b178e785986d 50 int cnt_now = -1;
SES01 1:b178e785986d 51 volatile unsigned char dualshock_cmd_getflag = 0;
SES01 1:b178e785986d 52 volatile unsigned char dualshock_data[9] = {0x00, 0x00, 0x00, 0x80, 0x80, 0x80, 0x80};
SES01 1:b178e785986d 53 volatile unsigned char dualshock_data_number = 0;
SES01 1:b178e785986d 54
SES01 1:b178e785986d 55 uint16_t cnt1 = 0;
SES01 1:b178e785986d 56 volatile unsigned char dualshock_cmd_getflag1 = 0;
SES01 1:b178e785986d 57 volatile unsigned char dualshock_data1[9] = {0x00, 0x00, 0x00, 0x80, 0x80, 0x80, 0x80};
SES01 1:b178e785986d 58 volatile unsigned char dualshock_data_number1 = 0;
SES01 1:b178e785986d 59
SES01 1:b178e785986d 60 uint16_t cnt2 = 0;
SES01 1:b178e785986d 61 volatile unsigned char dualshock_cmd_getflag2 = 0;
SES01 1:b178e785986d 62 volatile unsigned char dualshock_data2[9] = {0x00, 0x00, 0x00, 0x80, 0x80, 0x80, 0x80};
SES01 1:b178e785986d 63 volatile unsigned char dualshock_data_number2 = 0;
SES01 1:b178e785986d 64 char str[20];
SES01 1:b178e785986d 65 char usart_rx = 0;
SES01 1:b178e785986d 66 double lx, ly, lraw_rad, lclearance, ldistance, langle;
SES01 1:b178e785986d 67 double rx, ry, rraw_rad, rclearance, rdistance, rangle;
SES01 1:b178e785986d 68
SES01 1:b178e785986d 69 void cv2bin( unsigned char n ) {
SES01 1:b178e785986d 70 int i;
SES01 1:b178e785986d 71
SES01 1:b178e785986d 72 for( i = DIGITS-1; i >= 0; i-- ) {
SES01 1:b178e785986d 73 printf( "%d", ( n >> i ) & 1 );
SES01 1:b178e785986d 74 }
SES01 1:b178e785986d 75 printf( "\n" );
SES01 1:b178e785986d 76 }
SES01 1:b178e785986d 77
SES01 1:b178e785986d 78 void joy_cal() {
SES01 1:b178e785986d 79 //以下ジョイスティック計算
SES01 1:b178e785986d 80 rx = -(128-dualshock_data[3])*2;
SES01 1:b178e785986d 81 ry = (128-dualshock_data[4])*2;
SES01 1:b178e785986d 82 if (rx >= 255) rx = 255;
SES01 1:b178e785986d 83 if (rx <= -255) rx = -255;
SES01 1:b178e785986d 84 if (ry >= 255) ry = 255;
SES01 1:b178e785986d 85 if (ry <= -255) ry = -255;
SES01 1:b178e785986d 86 rraw_rad = atan2(ry, rx)/PI;
SES01 1:b178e785986d 87 rdistance = hypot(rx, ry) - rclearance;
SES01 1:b178e785986d 88 rdistance *= (255 / (255 - rclearance));
SES01 1:b178e785986d 89 if (rdistance < 0) rdistance = 0;
SES01 1:b178e785986d 90 if (rdistance >= 255) rdistance = 255;
SES01 1:b178e785986d 91 rangle = (rraw_rad < 0 ? 2 + rraw_rad : rraw_rad) * 180;
SES01 1:b178e785986d 92 rangle = rdistance == 0 ? 0 : rangle;
SES01 1:b178e785986d 93
SES01 1:b178e785986d 94 if (rx > 0) {
SES01 1:b178e785986d 95 rx -= rclearance;
SES01 1:b178e785986d 96 if (rx < 0) rx = 0;
SES01 1:b178e785986d 97 } else if (rx < 0) {
SES01 1:b178e785986d 98 rx += rclearance;
SES01 1:b178e785986d 99 if (rx > 0) rx = 0;
SES01 1:b178e785986d 100 }
SES01 1:b178e785986d 101 rx *= (255 / (255 - rclearance));
SES01 1:b178e785986d 102
SES01 1:b178e785986d 103 lx = -(128-dualshock_data[5])*2;
SES01 1:b178e785986d 104 ly = (128-dualshock_data[6])*2;
SES01 1:b178e785986d 105 if (lx >= 255) lx = 255;
SES01 1:b178e785986d 106 if (lx <= -255) lx = -255;
SES01 1:b178e785986d 107 if (ly >= 255) ly = 255;
SES01 1:b178e785986d 108 if (ly <= -255) ly = -255;
SES01 1:b178e785986d 109 lraw_rad = atan2(ly, lx)/PI;
SES01 1:b178e785986d 110 ldistance = hypot(lx, ly) - lclearance;
SES01 1:b178e785986d 111 ldistance *= (255 / (255 - lclearance));
SES01 1:b178e785986d 112 if (ldistance < 0) ldistance = 0;
SES01 1:b178e785986d 113 if (ldistance >= 255) ldistance = 255;
SES01 1:b178e785986d 114 langle = (lraw_rad < 0 ? 2 + lraw_rad : lraw_rad) * 180;
SES01 1:b178e785986d 115 langle = ldistance == 0 ? 0 : langle;
SES01 1:b178e785986d 116 }
SES01 1:b178e785986d 117
SES01 1:b178e785986d 118 //タイマ割り込み
SES01 1:b178e785986d 119 void time() {
SES01 1:b178e785986d 120
SES01 1:b178e785986d 121 if (dualshock_cmd_getflag & 0x02) {
SES01 1:b178e785986d 122 dualshock_cmd_getflag = 0;
SES01 1:b178e785986d 123 dualshock_data[1] = 0x00;
SES01 1:b178e785986d 124 dualshock_data[2] = 0x00;
SES01 1:b178e785986d 125 dualshock_data[3] = 0x80;
SES01 1:b178e785986d 126 dualshock_data[4] = 0x80;
SES01 1:b178e785986d 127 dualshock_data[5] = 0x80;
SES01 1:b178e785986d 128 dualshock_data[6] = 0x80;
SES01 1:b178e785986d 129 led2 = 0;
SES01 1:b178e785986d 130 }
SES01 1:b178e785986d 131 else
SES01 1:b178e785986d 132 dualshock_cmd_getflag |= 0x02;
SES01 1:b178e785986d 133 }
SES01 1:b178e785986d 134
SES01 1:b178e785986d 135
SES01 1:b178e785986d 136 void usart(){
SES01 1:b178e785986d 137 unsigned char rxdata;
SES01 1:b178e785986d 138
SES01 1:b178e785986d 139 rxdata = serial.getc();
SES01 1:b178e785986d 140 serial.putc(dualshock_data_number);
SES01 1:b178e785986d 141 if (dualshock_data_number == 0 && rxdata == 'S')
SES01 1:b178e785986d 142 dualshock_data_number++;
SES01 1:b178e785986d 143 else if(dualshock_data_number >= 1 && dualshock_data_number <= 6) {
SES01 1:b178e785986d 144 if (dualshock_data_number <= 2)
SES01 1:b178e785986d 145 dualshock_data[dualshock_data_number] = ~rxdata;
SES01 1:b178e785986d 146 else
SES01 1:b178e785986d 147 dualshock_data[dualshock_data_number] = rxdata;
SES01 1:b178e785986d 148 dualshock_data_number++;
SES01 1:b178e785986d 149 }
SES01 1:b178e785986d 150 else if (dualshock_data_number == 7 && rxdata == 'E') {
SES01 1:b178e785986d 151 dualshock_cmd_getflag = 0x01;
SES01 1:b178e785986d 152 dualshock_data_number = 0;
SES01 1:b178e785986d 153 led2 = 1;
SES01 1:b178e785986d 154 joy_cal();
SES01 1:b178e785986d 155 }
SES01 1:b178e785986d 156 //ledx = !ledx;
SES01 1:b178e785986d 157 }
SES01 1:b178e785986d 158
SES01 1:b178e785986d 159 void ics() {
SES01 1:b178e785986d 160 wait_ms(1);
SES01 1:b178e785986d 161 led3 = 0;
SES01 1:b178e785986d 162 //EN_IN = 0;
SES01 1:b178e785986d 163 }
SES01 1:b178e785986d 164
SES01 1:b178e785986d 165
SES01 1:b178e785986d 166 //i2c通信
SES01 1:b178e785986d 167 void md_setoutput(unsigned char address, int rotate, int duty) {
SES01 1:b178e785986d 168 char val;
SES01 1:b178e785986d 169 char data[2];
SES01 1:b178e785986d 170
SES01 1:b178e785986d 171 /* H30新型：処理最適化 */
SES01 1:b178e785986d 172 switch (rotate) {
SES01 1:b178e785986d 173 case CW: data[0] = 'F'; break;// 電圧比例制御モード（PWMがLの区間はフリー）
SES01 1:b178e785986d 174 case CCW: data[0] = 'R'; break;
SES01 1:b178e785986d 175 case STOP: data[0] = 'S'; break;
SES01 1:b178e785986d 176
SES01 1:b178e785986d 177 case bCW: data[0] = 'f'; break; // 電圧比例制御モード（PWMがLの区間はブレーキ）
SES01 1:b178e785986d 178 case bCCW: data[0] = 'r'; break;
SES01 1:b178e785986d 179 case bSTOP: data[0] = 's'; break;
SES01 1:b178e785986d 180
SES01 1:b178e785986d 181 case sCW: data[0] = 'A'; break; // 速度比例制御モード
SES01 1:b178e785986d 182 case sCCW: data[0] = 'B'; break;
SES01 1:b178e785986d 183
SES01 1:b178e785986d 184 case shCW: data[0] = 'a'; break; // 速度比例制御モード（命令パケット最上位ビットに指令値9ビット目を入れることで分解能2倍）
SES01 1:b178e785986d 185 case shCCW: data[0] = 'b'; break;
SES01 1:b178e785986d 186 }
SES01 1:b178e785986d 187
SES01 1:b178e785986d 188 data[1] = duty & 0xFF;
SES01 1:b178e785986d 189
SES01 1:b178e785986d 190 val = md_i2c_tx.write(address << 1, data, 2); //duty送信
SES01 1:b178e785986d 191 }
SES01 1:b178e785986d 192
SES01 1:b178e785986d 193 void set_pos(int id, float angle) {
SES01 1:b178e785986d 194 float pos = 0;
SES01 1:b178e785986d 195 int data = 0; //3500～11500
SES01 1:b178e785986d 196 angle += 135;
SES01 1:b178e785986d 197 pos = (8000 / 270) * angle;
SES01 1:b178e785986d 198 data = (int)(pos > 0.5 ? pos + 0.5 : pos - 0.5);
SES01 1:b178e785986d 199 data += 3500;
SES01 1:b178e785986d 200
SES01 1:b178e785986d 201 //int CMD = 0b10000000 + id;
SES01 1:b178e785986d 202 //char pos_H = (uint16_t)((data & 0b11111110000000) >> 7);
SES01 1:b178e785986d 203 //char pos_L = (uint16_t)(data & 0b00000001111111);
SES01 1:b178e785986d 204
SES01 1:b178e785986d 205
SES01 1:b178e785986d 206 led3 = 1;
SES01 1:b178e785986d 207 //EN_IN = 1;
SES01 1:b178e785986d 208 //servo.putc(CMD);
SES01 1:b178e785986d 209 //servo.putc(pos_H);
SES01 1:b178e785986d 210 //servo.putc(pos_L);
SES01 1:b178e785986d 211 }
StressedDave 0:a8e6e26ebe0f 212
StressedDave 0:a8e6e26ebe0f 213 int main() {
SES01 1:b178e785986d 214 wait_ms(10);
SES01 1:b178e785986d 215 unsigned char stick_gain = 4;
SES01 1:b178e785986d 216 //led1 = 1;
SES01 1:b178e785986d 217 led2 = 0;
SES01 1:b178e785986d 218 led3 = 0;
SES01 1:b178e785986d 219 //ledx = 1;
SES01 1:b178e785986d 220 serial.baud(19200);//シリアル通信
SES01 1:b178e785986d 221 s485.baud(19200);
SES01 1:b178e785986d 222
SES01 1:b178e785986d 223
SES01 1:b178e785986d 224 //serial2.baud(19200);//シリアル通信
SES01 1:b178e785986d 225 pc.baud(230400);
SES01 1:b178e785986d 226
SES01 1:b178e785986d 227 md_i2c_tx.frequency(20000000UL/(16 + 2 * 2 * 16)/2);//I2C通信
SES01 1:b178e785986d 228 timer.attach_us(time, 100000); //タイマ割り込み
SES01 1:b178e785986d 229 serial.attach(&usart, Serial::RxIrq); //受信割り込み
SES01 1:b178e785986d 230 //serial2.attach(&usart2, Serial::RxIrq); //受信割り込み
SES01 1:b178e785986d 231
SES01 1:b178e785986d 232 //EN_IN = 0;
SES01 1:b178e785986d 233 //servo.baud(115200);
SES01 1:b178e785986d 234 //servo.format(8, Serial::Even, 1);
SES01 1:b178e785986d 235 //servo.attach(&ics, Serial::TxIrq);
SES01 1:b178e785986d 236
SES01 1:b178e785986d 237
StressedDave 0:a8e6e26ebe0f 238 imu.reset();
SES01 1:b178e785986d 239 imu.check();
SES01 1:b178e785986d 240
SES01 1:b178e785986d 241
SES01 1:b178e785986d 242 // テスト用回転モード切り替え記録
SES01 1:b178e785986d 243 int Mcw = CW;
SES01 1:b178e785986d 244 int Mccw = CCW;
SES01 1:b178e785986d 245 int Mstop = STOP;
SES01 1:b178e785986d 246
SES01 1:b178e785986d 247 lclearance = 60; //平行移動
SES01 1:b178e785986d 248 rclearance = 90; //旋回
SES01 1:b178e785986d 249 float power_up = 0;
SES01 1:b178e785986d 250 while (1) {
SES01 1:b178e785986d 251 DE = 1;
SES01 1:b178e785986d 252 s485.printf("S");
SES01 1:b178e785986d 253 //DE = 0;
SES01 1:b178e785986d 254 //led1 = EMS_STATE;
SES01 1:b178e785986d 255 //set_pos(0, -65);
SES01 1:b178e785986d 256 //CS = 0;
SES01 1:b178e785986d 257 //isoSPI.write(165);
SES01 1:b178e785986d 258 //CS = 1;
SES01 1:b178e785986d 259 /********************以下速度アップ処理********************/
SES01 1:b178e785986d 260 if (dualshock_data[2] & 0x04) power_up = 1;
SES01 1:b178e785986d 261 else power_up = 0.8;
SES01 1:b178e785986d 262 /********************以下メカナムメイン処理********************/
SES01 1:b178e785986d 263 if (rx > 0) {
SES01 1:b178e785986d 264 ////////////////////頭を右方向に旋回
SES01 1:b178e785986d 265 md_setoutput(HF_MOTRE,Mcw,rx*power_up);
SES01 1:b178e785986d 266 md_setoutput(HB_MOTRE,Mcw,rx*power_up);
SES01 1:b178e785986d 267 md_setoutput(MF_MOTRE,Mcw,rx*power_up);
SES01 1:b178e785986d 268 md_setoutput(MB_MOTRE,Mcw,rx*power_up);
SES01 1:b178e785986d 269 } else if (rx < 0) {
SES01 1:b178e785986d 270 ////////////////////頭を左方向に旋回
SES01 1:b178e785986d 271 md_setoutput(HF_MOTRE,Mccw,-rx*power_up);
SES01 1:b178e785986d 272 md_setoutput(HB_MOTRE,Mccw,-rx*power_up);
SES01 1:b178e785986d 273 md_setoutput(MF_MOTRE,Mccw,-rx*power_up);
SES01 1:b178e785986d 274 md_setoutput(MB_MOTRE,Mccw,-rx*power_up);
SES01 1:b178e785986d 275 } else {
SES01 1:b178e785986d 276 ////////////////////旋回をしない場合平行移動を行う
SES01 1:b178e785986d 277 //
SES01 1:b178e785986d 278 if ((ldistance != 0) && ((langle < 45) || (langle >= 315))){ //右
SES01 1:b178e785986d 279 md_setoutput(HF_MOTRE,Mcw,ldistance*power_up);
SES01 1:b178e785986d 280 md_setoutput(HB_MOTRE,Mcw,ldistance*power_up);
SES01 1:b178e785986d 281 md_setoutput(MF_MOTRE,Mccw,ldistance*power_up);
SES01 1:b178e785986d 282 md_setoutput(MB_MOTRE,Mccw,ldistance*power_up);
SES01 1:b178e785986d 283 } else if ((ldistance != 0) && (langle < 135)) { //前
SES01 1:b178e785986d 284 md_setoutput(HF_MOTRE,Mccw,ldistance*power_up);
SES01 1:b178e785986d 285 md_setoutput(HB_MOTRE,Mcw,ldistance*power_up);
SES01 1:b178e785986d 286 md_setoutput(MF_MOTRE,Mccw,ldistance*power_up);
SES01 1:b178e785986d 287 md_setoutput(MB_MOTRE,Mcw,ldistance*power_up);
SES01 1:b178e785986d 288 } else if ((ldistance != 0) && (langle < 225)) { //左
SES01 1:b178e785986d 289 md_setoutput(HF_MOTRE,Mccw,ldistance*power_up);
SES01 1:b178e785986d 290 md_setoutput(HB_MOTRE,Mccw,ldistance*power_up);
SES01 1:b178e785986d 291 md_setoutput(MF_MOTRE,Mcw,ldistance*power_up);
SES01 1:b178e785986d 292 md_setoutput(MB_MOTRE,Mcw,ldistance*power_up);
SES01 1:b178e785986d 293 } else if ((ldistance != 0) && (langle < 315)) { //後
SES01 1:b178e785986d 294 md_setoutput(HF_MOTRE,Mcw,ldistance*power_up);
SES01 1:b178e785986d 295 md_setoutput(HB_MOTRE,Mccw,ldistance*power_up);
SES01 1:b178e785986d 296 md_setoutput(MF_MOTRE,Mcw,ldistance*power_up);
SES01 1:b178e785986d 297 md_setoutput(MB_MOTRE,Mccw,ldistance*power_up);
SES01 1:b178e785986d 298 } else { //停止
SES01 1:b178e785986d 299 md_setoutput(HF_MOTRE,Mstop,0);
SES01 1:b178e785986d 300 md_setoutput(HB_MOTRE,Mstop,0);
SES01 1:b178e785986d 301 md_setoutput(MF_MOTRE,Mstop,0);
SES01 1:b178e785986d 302 md_setoutput(MB_MOTRE,Mstop,0);
StressedDave 0:a8e6e26ebe0f 303 }
SES01 1:b178e785986d 304 }
SES01 1:b178e785986d 305
SES01 1:b178e785986d 306 /* 回転モード設定（テスト用） */
SES01 1:b178e785986d 307 if (dualshock_data[2] & 0x40) { //通常フリー
SES01 1:b178e785986d 308 Mcw = CW;
SES01 1:b178e785986d 309 Mccw = CCW;
SES01 1:b178e785986d 310 Mstop = STOP;
SES01 1:b178e785986d 311 led1 = 0;
SES01 1:b178e785986d 312 } else if (dualshock_data[2] & 0x20) { //通常ブレーキ
SES01 1:b178e785986d 313 Mcw = bCW;
SES01 1:b178e785986d 314 Mccw = bCCW;
SES01 1:b178e785986d 315 Mstop = bSTOP;
SES01 1:b178e785986d 316 led1 = 1;
SES01 1:b178e785986d 317 } else if (dualshock_data[2] & 0x10) { //速調
SES01 1:b178e785986d 318 Mcw = sCW;
SES01 1:b178e785986d 319 Mccw = sCCW;
SES01 1:b178e785986d 320 Mstop = bSTOP;
SES01 1:b178e785986d 321 led1 = 1;
SES01 1:b178e785986d 322 }
SES01 1:b178e785986d 323 {
SES01 1:b178e785986d 324 imu.setmode(OPERATION_MODE_IMUPLUS);
SES01 1:b178e785986d 325 imu.get_calib();
SES01 1:b178e785986d 326 imu.get_angles();
SES01 1:b178e785986d 327 imu.get_quat();
SES01 1:b178e785986d 328
SES01 1:b178e785986d 329 double q1, q2, q3, q4;
SES01 1:b178e785986d 330 q1 = imu.quat.w;
SES01 1:b178e785986d 331 q2 = imu.quat.x;
SES01 1:b178e785986d 332 q3 = imu.quat.y;
SES01 1:b178e785986d 333 q4 = imu.quat.z;
SES01 1:b178e785986d 334
SES01 1:b178e785986d 335 double q3q3 = q3 * q3;
SES01 1:b178e785986d 336
SES01 1:b178e785986d 337 double m1 = +2.0 * (q1 * q2 + q3 * q4);
SES01 1:b178e785986d 338 double m2 = +1.0 - 2.0 * (q2 * q2 + q3q3);
SES01 1:b178e785986d 339 //roll = atan2(m1, m2) * 57.2957795131;
SES01 1:b178e785986d 340
SES01 1:b178e785986d 341 m1 = +2.0 * (q1 * q3 - q4 * q2);
SES01 1:b178e785986d 342 m1 = (m1 > 1.0)? 1.0 : m1;
SES01 1:b178e785986d 343 m1 = (m1 < -1.0)? -1.0 : m1;
SES01 1:b178e785986d 344 //pitch = asin(m1) * 57.2957795131;
SES01 1:b178e785986d 345
SES01 1:b178e785986d 346 m1 = +2.0 * (q1 * q4 + q2 * q3);
SES01 1:b178e785986d 347 m2 = +1.0 - 2.0 * (q3q3 + q4 * q4);
SES01 1:b178e785986d 348 //yaw = atan2(m1, m2) * 57.2957795131;
SES01 1:b178e785986d 349
SES01 1:b178e785986d 350 //pc.printf("yaw\t = %05d, roll\t = %05d, pitch\t = %05d\r\n", (int)yaw, (int)roll, (int)pitch);
SES01 1:b178e785986d 351 pc.printf("%05d\r\n", test.read_u16());
SES01 1:b178e785986d 352 }
SES01 1:b178e785986d 353
SES01 1:b178e785986d 354 wait_ms(10);
StressedDave 0:a8e6e26ebe0f 355 }
SES01 1:b178e785986d 356 }