DMU.cpp

00001 #include "DMU.h"
00002 
00003 DMU::DMU(PinName MOSI, PinName MISO, PinName SCLK, 
00004          PinName S0,   PinName S1,   PinName S2){
00005     _spi = new SPI(MOSI, MISO, SCLK);               //communication
00006     _cs = new BusOut(S0, S1, S2);                   //slave select/axis selct
00007                                                
00008     _init();
00009     
00010     
00011 } 
00012 
00013 float DMU::acceleration(char axis){
00014 
00015     
00016     bool correctRead;
00017     do{
00018         _selectAxis(axis);
00019         correctRead = _write(0x01);
00020     }while (!correctRead);
00021     
00022     short dispA = (data2<<8) | data3;
00023     int negTester = 0x8000;
00024     if (negTester & dispA) dispA = -1 * (65536 - dispA);
00025     
00026     return dispA * .00366;
00027 
00028 }
00029 
00030 float DMU::roll(){
00031 
00032     bool correctRead;
00033     do{
00034         _selectAxis('x');
00035         correctRead = _write(0x01);
00036     }while (!correctRead);
00037     
00038     short dispA = (data0<<8) | data1;
00039     int negTester = 0x8000;
00040     if (negTester & dispA) dispA = -1 * (65536 - dispA);
00041     
00042     return dispA * .03125;
00043 
00044 }
00045 float DMU::pitch(){
00046 
00047     bool correctRead;
00048     do{
00049         _selectAxis('y');
00050         correctRead = _write(0x01);
00051     }while (!correctRead);
00052     
00053     short dispA = (data0<<8) | data1;
00054     int negTester = 0x8000;
00055     if (negTester & dispA) dispA = -1 * (65536 - dispA);
00056     
00057     return dispA * .03125;
00058 
00059 }
00060 float DMU::yaw(){
00061 
00062     bool correctRead;
00063     do{
00064         _selectAxis('z');
00065         correctRead = _write(0x01);
00066     }while (!correctRead);
00067     
00068     short dispA = (data0<<8) | data1;
00069     int negTester = 0x8000;
00070     if (negTester & dispA) dispA = -1 * (65536 - dispA);
00071     
00072     return dispA * .03125;
00073 
00074 }
00075 
00076 void DMU::test(){
00077 
00078     bool correctRead;
00079     do{
00080         _selectAxis('z');
00081         correctRead = _write(0x05);
00082     }while (!correctRead);
00083     
00084     do{
00085         _selectAxis('y');
00086         correctRead = _write(0x05);
00087     }while (!correctRead);
00088     
00089     do{
00090         _selectAxis('x');
00091         correctRead = _write(0x05);
00092     }while (!correctRead);
00093 
00094 }
00095 
00096  
00097  
00098 /*  Init the DMU
00099  *  
00100  *  
00101  */
00102 void DMU::_init() {
00103     _repClock.start();              // start clock to allow first call of select axis to read from clock
00104     _spi->frequency(1150000);       // f = 1.15MHz, max f = 1.25MHz, min f = 4kHz
00105     _spi->format(8,0);              // 8 bits of data, CPOL = 0, CPHA = 0
00106   
00107 }
00108 
00109 void DMU::_selectAxis(char axis) {
00110     while (_repClock.read_ms() < 100);  //check to ensure device isn't read more than once every 100 ms
00111     _repClock.stop();
00112     _repClock.reset();
00113     switch (axis){
00114         case 'x':
00115             _repClock.start();
00116             _cs->write(6);              //drive SS to set x line low
00117             break;
00118         case 'y':
00119             _repClock.start();
00120             _cs->write(3);              //drive SS to set y line low
00121             break;
00122         case 'z':
00123             _repClock.start();
00124             _cs->write(5);              //drive SS to set z line low
00125     }
00126     wait(.000010);                      // wait time needed for device setup
00127 }
00128  
00129 void DMU::_deselectAxis() {
00130 
00131     _cs->write(7);
00132 
00133 }
00134 
00135 bool DMU::_write(char controlByte) {
00136 
00137     char status = _spi->write(controlByte);
00138     wait(.0000015);
00139     
00140     data0 = _spi->write(0x00);
00141     wait(.0000015);                             //min wait time between successive bytes is 1.1us
00142     
00143     data1 = _spi->write(0x00);
00144     wait(.0000015);
00145     
00146     data2 = _spi->write(0x00);
00147     wait(.0000015);
00148     
00149     data3 = _spi->write(0x00);
00150     wait(.0000015);
00151 
00152     char rCS = _spi->write(~controlByte);
00153     _deselectAxis();                            //deselect chip to ensure message duration is under 270 us
00154     
00155     
00156     return _correctChecksumReceived(status, rCS);
00157 
00158 
00159 
00160 }
00161 
00162 bool DMU::_correctChecksumReceived(char statusByte, char chkSum){
00163 
00164     char calculatedChecksum = statusByte + data0 + data1 + data2 + data3;
00165     
00166     if ((char)~chkSum != calculatedChecksum) return 0;
00167     return 1;
00168 
00169 }