Masahiro Furukawa / Mbed 2 deprecated MPU-9250-Ch2_AHRS_test

Important changes to repositories hosted on mbed.com

Mbed hosted mercurial repositories are deprecated and are due to be permanently deleted in July 2026.

To keep a copy of this software download the repository Zip archive or clone locally using Mercurial.

It is also possible to export all your personal repositories from the account settings page.

Dependencies:   MPU9250_SPI mbed

Fork of MPU9250_AHRS by maedalab

main.cpp@5:abfc7660fde9, 2016-06-13 (annotated)

Committer:
mfurukawa
Date:
Mon Jun 13 07:59:46 2016 +0000
Revision:
5:abfc7660fde9
Parent:
4:5a9aa5ae928a
Child:
6:ea0804dc7cae
stable;

Who changed what in which revision?

UserRevisionLine numberNew contents of line
mfurukawa 3:07aa20aa678d 1 /**
mfurukawa 3:07aa20aa678d 2 * Masahiro FURUKAWA - m.furukawa@ist.osaka-u.ac.jp
mfurukawa 3:07aa20aa678d 3 *
mfurukawa 4:5a9aa5ae928a 4 * June 7, 2016
mfurukawa 3:07aa20aa678d 5 *
mfurukawa 3:07aa20aa678d 6 * LDXL362 Acceleration Sensor (Extended to Ch1 ~ Ch3)
mfurukawa 3:07aa20aa678d 7 *
mfurukawa 3:07aa20aa678d 8 **/
mfurukawa 3:07aa20aa678d 9
mfurukawa 1:f1e4ee4fc335 10
adisuciu 0:83fda1bfaffe 11 #include "mbed.h"
adisuciu 0:83fda1bfaffe 12 #include "ADXL362.h"
adisuciu 0:83fda1bfaffe 13
mfurukawa 1:f1e4ee4fc335 14 Serial pc(USBTX, USBRX);
mfurukawa 1:f1e4ee4fc335 15
mfurukawa 1:f1e4ee4fc335 16 /*
mfurukawa 1:f1e4ee4fc335 17 ~CS (Chip Select) p8
mfurukawa 1:f1e4ee4fc335 18 MOSI (Master Out Slave In) p5
mfurukawa 1:f1e4ee4fc335 19 MISO (Master In Slave Out p6
mfurukawa 1:f1e4ee4fc335 20 SCK (Serial Clock) p7
mfurukawa 1:f1e4ee4fc335 21 */
adisuciu 0:83fda1bfaffe 22
mfurukawa 4:5a9aa5ae928a 23
adisuciu 0:83fda1bfaffe 24 int main() {
mfurukawa 2:9ef7a594159c 25 pc.baud(115200);
adisuciu 0:83fda1bfaffe 26
mfurukawa 4:5a9aa5ae928a 27 ADXL362 *adxl362[6];
mfurukawa 1:f1e4ee4fc335 28
mfurukawa 4:5a9aa5ae928a 29 adxl362[0] = new ADXL362( p8, p5, p6, p7);
mfurukawa 4:5a9aa5ae928a 30 adxl362[1] = new ADXL362( p9, p5, p6, p7);
mfurukawa 4:5a9aa5ae928a 31 adxl362[2] = new ADXL362( p10, p5, p6, p7);
mfurukawa 4:5a9aa5ae928a 32 adxl362[3] = new ADXL362( p11, p5, p6, p7);
mfurukawa 4:5a9aa5ae928a 33 adxl362[4] = new ADXL362( p12, p5, p6, p7);
mfurukawa 4:5a9aa5ae928a 34 adxl362[5] = new ADXL362( p13, p5, p6, p7);
mfurukawa 3:07aa20aa678d 35
mfurukawa 5:abfc7660fde9 36
mfurukawa 3:07aa20aa678d 37 // we need to wait at least 500ms after ADXL362 reset
mfurukawa 4:5a9aa5ae928a 38 printf("\r\nInitializing . \r\n");
mfurukawa 4:5a9aa5ae928a 39 for (int i=0; i<6; i++) {
mfurukawa 4:5a9aa5ae928a 40 adxl362[i]->reset();
mfurukawa 4:5a9aa5ae928a 41 wait_ms(600);
mfurukawa 4:5a9aa5ae928a 42 }
mfurukawa 4:5a9aa5ae928a 43
mfurukawa 4:5a9aa5ae928a 44
mfurukawa 4:5a9aa5ae928a 45 wait_ms(600);
mfurukawa 3:07aa20aa678d 46
mfurukawa 4:5a9aa5ae928a 47 for (int i=0; i<6; i++)
mfurukawa 4:5a9aa5ae928a 48 {
mfurukawa 5:abfc7660fde9 49 adxl362[i]->frequency(20000);
mfurukawa 4:5a9aa5ae928a 50 adxl362[i]->frequency(1000000);
mfurukawa 4:5a9aa5ae928a 51 adxl362[i]->set_mode(ADXL362::MEASUREMENT);
mfurukawa 4:5a9aa5ae928a 52 }
mfurukawa 4:5a9aa5ae928a 53 //while(1) {
mfurukawa 4:5a9aa5ae928a 54 for (int i=0; i<6; i++)
mfurukawa 4:5a9aa5ae928a 55 {
mfurukawa 4:5a9aa5ae928a 56 uint8_t err = false;
mfurukawa 4:5a9aa5ae928a 57 uint8_t ad = static_cast<uint8_t>(adxl362[i]->read_reg(ADXL362::DEVID_AD));
mfurukawa 4:5a9aa5ae928a 58 uint8_t mst = static_cast<uint8_t>(adxl362[i]->read_reg(ADXL362::DEVID_MST));
mfurukawa 4:5a9aa5ae928a 59 uint8_t pid = static_cast<uint8_t>(adxl362[i]->read_reg(ADXL362::PARTID));
mfurukawa 4:5a9aa5ae928a 60 uint8_t rid = static_cast<uint8_t>(adxl362[i]->read_reg(ADXL362::REVID));
mfurukawa 4:5a9aa5ae928a 61 err |= (0xAD != ad);
mfurukawa 4:5a9aa5ae928a 62 err |= (0x1D != mst);
mfurukawa 4:5a9aa5ae928a 63 err |= (0xF2 != pid);
mfurukawa 4:5a9aa5ae928a 64 err |= (0x02 != rid);
mfurukawa 4:5a9aa5ae928a 65 if (err) printf("CH %d has error DevID_AD %02x DevID_MST %02x PartID %02x RevID %02x\r\n", i, ad, mst, pid, rid);
mfurukawa 4:5a9aa5ae928a 66 }
mfurukawa 4:5a9aa5ae928a 67 //}
mfurukawa 2:9ef7a594159c 68
mfurukawa 4:5a9aa5ae928a 69 uint64_t xyzt[6];
mfurukawa 5:abfc7660fde9 70 short xi[6],yi[6],zi[6];
mfurukawa 2:9ef7a594159c 71
adisuciu 0:83fda1bfaffe 72 while(1) {
mfurukawa 3:07aa20aa678d 73
mfurukawa 4:5a9aa5ae928a 74
mfurukawa 3:07aa20aa678d 75
mfurukawa 4:5a9aa5ae928a 76 /* pc.putc('#');
mfurukawa 4:5a9aa5ae928a 77 char ch;
mfurukawa 4:5a9aa5ae928a 78 for(int i=8;i>2;i--)
mfurukawa 4:5a9aa5ae928a 79 {
mfurukawa 4:5a9aa5ae928a 80 ch = static_cast<uint8_t>(0xF&( t1>>(i*8) ) );
mfurukawa 4:5a9aa5ae928a 81 pc.putc(ch);
mfurukawa 4:5a9aa5ae928a 82 }
mfurukawa 4:5a9aa5ae928a 83 pc.putc(',');
mfurukawa 4:5a9aa5ae928a 84 for(int i=8;i>2;i--)
mfurukawa 4:5a9aa5ae928a 85 {
mfurukawa 4:5a9aa5ae928a 86 ch = static_cast<uint8_t>(0xF&( t2>>(i*8) ) );
mfurukawa 4:5a9aa5ae928a 87 pc.putc(ch);
mfurukawa 4:5a9aa5ae928a 88 }
mfurukawa 4:5a9aa5ae928a 89 pc.putc(',');
mfurukawa 4:5a9aa5ae928a 90 for(int i=8;i>2;i--)
mfurukawa 4:5a9aa5ae928a 91 {
mfurukawa 4:5a9aa5ae928a 92 ch = static_cast<uint8_t>(0xF&( t3>>(i*8) ) );
mfurukawa 4:5a9aa5ae928a 93 pc.putc(ch);
mfurukawa 4:5a9aa5ae928a 94 }
mfurukawa 4:5a9aa5ae928a 95 */
mfurukawa 3:07aa20aa678d 96
mfurukawa 4:5a9aa5ae928a 97 for (int i=0; i<6; i++)
mfurukawa 4:5a9aa5ae928a 98 xyzt[i] = adxl362[i]->scan();
mfurukawa 5:abfc7660fde9 99
mfurukawa 4:5a9aa5ae928a 100 for (int i=0; i<6; i++) {
mfurukawa 5:abfc7660fde9 101 uint16_t x = static_cast<uint16_t>(0xFFFF&(xyzt[i]>>48));
mfurukawa 5:abfc7660fde9 102 uint16_t y = static_cast<uint16_t>(0xFFFF&(xyzt[i]>>32));
mfurukawa 5:abfc7660fde9 103 uint16_t z = static_cast<uint16_t>(0xFFFF&(xyzt[i]>>16));
mfurukawa 5:abfc7660fde9 104
mfurukawa 5:abfc7660fde9 105 if(x & 1<<16) xi[i] = -1 * static_cast<short>(~x) + 1; else xi[i] = x;
mfurukawa 5:abfc7660fde9 106 if(y & 1<<16) yi[i] = -1 * static_cast<short>(~y) + 1; else yi[i] = y;
mfurukawa 5:abfc7660fde9 107 if(z & 1<<16) zi[i] = -1 * static_cast<short>(~z) + 1; else zi[i] = z;
mfurukawa 5:abfc7660fde9 108 }
mfurukawa 5:abfc7660fde9 109 for (int i=0; i<6; i++) {
mfurukawa 5:abfc7660fde9 110 printf("%d %d %d ", xi[i], yi[i], zi[i]);
mfurukawa 4:5a9aa5ae928a 111 }
mfurukawa 4:5a9aa5ae928a 112 printf("\r\n");
mfurukawa 5:abfc7660fde9 113 wait_us(1);
adisuciu 0:83fda1bfaffe 114 }
adisuciu 0:83fda1bfaffe 115 }