adns9500.cpp

00001 /*
00002  * adns9500.hpp - Interface to access to Avago ADNS-9500 laser mouse sensors
00003  *
00004  *   Copyright 2012 Jesus Torres <jmtorres@ull.es>
00005  *
00006  * Licensed under the Apache License, Version 2.0 (the "License");
00007  * you may not use this file except in compliance with the License.
00008  * You may obtain a copy of the License at
00009  *
00010  *     http://www.apache.org/licenses/LICENSE-2.0
00011  *
00012  * Unless required by applicable law or agreed to in writing, software
00013  * distributed under the License is distributed on an "AS IS" BASIS,
00014  * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
00015  * See the License for the specific language governing permissions and
00016  * limitations under the License.
00017  */
00018 
00019 #include <cstdio>
00020 #include <cstdlib>
00021 #include <mbed.h>
00022 #include <string>
00023 
00024 #include <adns9500.hpp>
00025 
00026 #define WAIT_TSRAD()        wait_us(100)
00027 #define WAIT_TSRR()         wait_us(20)
00028 #define WAIT_TSRW()         wait_us(20)
00029 #define WAIT_TSWR()         wait_us(120)
00030 #define WAIT_TSWW()         wait_us(120)
00031 #define WAIT_TBEXIT()       wait_us(1)      // 500ns
00032 #define WAIT_TNCSSCLK()     wait_us(1)      // 120ns
00033 #define WAIT_TSCLKNCS()     wait_us(20)
00034 #define WAIT_TLOAD()        wait_us(15)
00035 
00036 #define LONG_WAIT_MS(x)     \
00037     WAIT_TSCLKNCS(); ncs_.write(1); wait_ms(x); ncs_.write(0); WAIT_TNCSSCLK()
00038 #define LONG_WAIT_US(x)     \
00039     WAIT_TSCLKNCS(); ncs_.write(1); wait_us(x); ncs_.write(0); WAIT_TNCSSCLK()
00040 
00041 #define DEFAULT_MAX_FPS             1958
00042 #define DEFAULT_MAX_FRAME_PERIOD    (1000000 / DEFAULT_MAX_FPS + 1) // in us
00043 #define DEFAULT_X_CPI               1620
00044 #define DEFAULT_Y_CPI               1620
00045 #define CPI_CHANGE_UNIT             90
00046 
00047 #define SPI_BITS_PER_FRAME          8
00048 #define SPI_MODE                    3
00049 #define SPI_WRITE_MODE              0x80
00050 
00051 #define SET_BIT(word, mask)         (word | mask)
00052 #define CLEAR_BIT(word, mask)       (word & (~mask))
00053 
00054 namespace adns9500 {
00055 
00056     ADNS9500::ADNS9500(PinName mosi, PinName miso, PinName sclk, PinName ncs,
00057         int spi_frequency, PinName motion)
00058         : spi_(mosi, miso, sclk),
00059           motion_(motion),
00060           ncs_(ncs),
00061           enabled_(false),
00062           xCpi_(DEFAULT_X_CPI), yCpi_(DEFAULT_Y_CPI)
00063     {
00064         spi_.format(SPI_BITS_PER_FRAME, SPI_MODE);
00065         spi_.frequency(spi_frequency);
00066 
00067         motion_.mode(PullUp);
00068         motion_.fall(this, &ADNS9500::motionTrigger);
00069     }
00070 
00071     ADNS9500::~ADNS9500()
00072     {
00073         shutdown();
00074     }
00075 
00076     void ADNS9500::reset(const uint8_t* fw, uint16_t fw_len)
00077     {
00078         // SPI port reset
00079         ncs_.write(1);
00080         WAIT_TNCSSCLK();
00081         ncs_.write(0);
00082         WAIT_TNCSSCLK();
00083         
00084         // send 0x5a to POWER_UP_RESET and wait for at least 50ms
00085         spiSend(POWER_UP_RESET, 0x5a);
00086         LONG_WAIT_MS(50);
00087         
00088         // clear observation register. Only required to deassert shutdown mode.
00089         spiSend(OBSERVATION, 0x00);
00090         LONG_WAIT_US(DEFAULT_MAX_FRAME_PERIOD);
00091 
00092         // check observation register bits [5:0]
00093         int observation = spiReceive(OBSERVATION);
00094         if (! ADNS9500_IF_OBSERVATION_TEST(observation)) {
00095             WAIT_TSCLKNCS();
00096             ncs_.write(1);
00097 
00098             error("ADNS9500::reset : observation register test failed: 0x%x\n", observation);
00099         }
00100 
00101         // read motion data
00102         WAIT_TSRR();
00103         spiReceive(MOTION);
00104         WAIT_TSRR();
00105         spiReceive(DELTA_X_L);
00106         WAIT_TSRR();
00107         spiReceive(DELTA_X_H);
00108         WAIT_TSRR();
00109         spiReceive(DELTA_Y_L);
00110         WAIT_TSRR();
00111         spiReceive(DELTA_Y_H);
00112         
00113         // read product and revision id to test the connection
00114         WAIT_TSRR();
00115         int product_id = spiReceive(PRODUCT_ID);
00116         WAIT_TSRR();
00117         int revision_id = spiReceive(REVISION_ID);
00118 
00119         WAIT_TSCLKNCS();
00120         ncs_.write(1);
00121 
00122         if (product_id != 0x33) {
00123             error("ADNS9500::reset : bad product ID: 0x%x\n", product_id);
00124         }
00125 
00126         if (revision_id != 0x03) {
00127             error("ADNS9500::reset : bad revision ID: 0x%x\n", revision_id);
00128         }
00129 
00130         enabled_ = true;
00131 
00132         if (fw) {
00133             sromDownload(fw, fw_len);
00134             enableLaser();
00135         }
00136     }
00137 
00138     void ADNS9500::shutdown()
00139     {
00140         if (! enabled_)
00141             error("ADNS9500::shutdown : the sensor is not enabled\n");
00142         
00143         ncs_.write(0);
00144         WAIT_TNCSSCLK();
00145         
00146         // send 0x5a to POWER_UP_RESET
00147         spiSend(POWER_UP_RESET, 0x5a);
00148         
00149         WAIT_TSCLKNCS();
00150         ncs_.write(1);
00151         
00152         enabled_ = false;
00153     }
00154 
00155     int ADNS9500::read(Register lregister)
00156     {
00157         if (! enabled_)
00158             error("ADNS9500::read : the sensor is not enabled\n");
00159     
00160         ncs_.write(0);
00161         WAIT_TNCSSCLK();
00162         
00163         // send the command to read the register
00164         int value = spiReceive(lregister);
00165         
00166         WAIT_TSCLKNCS();
00167         ncs_.write(1);
00168 
00169         return value;
00170     }
00171 
00172     int ADNS9500::read(Register uregister, Register lregister)
00173     {
00174         if (! enabled_)
00175             error("ADNS9500::read : the sensor is not enabled\n");
00176 
00177         ncs_.write(0);
00178         WAIT_TNCSSCLK();
00179 
00180         // send the command to read the registers
00181         int lvalue = spiReceive(lregister);
00182         WAIT_TSRR();
00183         int uvalue = spiReceive(uregister);
00184 
00185         WAIT_TSCLKNCS();
00186         ncs_.write(1);
00187 
00188         return ADNS9500_UINT16(uvalue, lvalue);
00189     }
00190     
00191     int ADNS9500::sromDownload(const uint8_t* fw, uint16_t fw_len)
00192     {
00193         if (! enabled_)
00194             error("ADNS9500::sromDownload : the sensor is not enabled\n");
00195     
00196         ncs_.write(0);
00197         WAIT_TNCSSCLK();
00198 
00199         // SROM download
00200         spiSend(CONFIGURATION_IV, ADNS9500_CONFIGURATION_IV_SROM_SIZE);
00201         WAIT_TSWW();
00202         spiSend(SROM_ENABLE, 0x1d);
00203         LONG_WAIT_US(DEFAULT_MAX_FRAME_PERIOD);
00204 
00205         spiSend(SROM_ENABLE, 0x18);
00206         WAIT_TSWW();
00207         spi_.write(SET_BIT(SROM_LOAD_BURST, SPI_WRITE_MODE));
00208        
00209         for( uint16_t i = 0; i < fw_len; i++){
00210             WAIT_TLOAD();
00211             spi_.write(fw[i]);
00212         }
00213         WAIT_TSCLKNCS();
00214         ncs_.write(1);
00215         WAIT_TBEXIT();
00216 
00217         // test if SROM was downloaded successfully
00218         wait_us(160);
00219         ncs_.write(0);
00220         WAIT_TNCSSCLK();
00221         
00222         int srom_id = spiReceive(SROM_ID);
00223         
00224         printf("SROM ID: %x %i\r\n", srom_id, srom_id );
00225         WAIT_TSCLKNCS();
00226         ncs_.write(1);
00227         
00228         if (! srom_id)
00229             error("ADNS9500::sromDownload : the firmware was not successful downloaded\n");
00230 
00231         // test laser fault condition
00232         ncs_.write(0);
00233         WAIT_TNCSSCLK();
00234         
00235         int motion = spiReceive(MOTION);
00236         
00237         WAIT_TSCLKNCS();
00238         ncs_.write(1);
00239         
00240         if (ADNS9500_IF_LASER_FAULT(motion))
00241             error("ADNS9500::sromDownload : laser fault condition detected\n");
00242         
00243         // return the SROM CRC value
00244         ncs_.write(0);
00245         WAIT_TNCSSCLK();
00246         
00247         spiSend(SROM_ENABLE, 0x15);
00248         LONG_WAIT_MS(10);
00249         
00250         int lcrc = spiReceive(DATA_OUT_LOWER);
00251         WAIT_TSRR();
00252         int ucrc = spiReceive(DATA_OUT_UPPER);
00253         
00254         WAIT_TSCLKNCS();
00255         ncs_.write(1);
00256         return ADNS9500_UINT16(ucrc, lcrc);
00257     }
00258 
00259     void ADNS9500::enableLaser(bool enable)
00260     {
00261         if (! enabled_ ){
00262             error("ADNS9500::enableLaser : the sensor is not enabled\n");
00263         }
00264         
00265         ncs_.write(0);
00266         WAIT_TNCSSCLK();
00267 
00268         int laser_ctrl0 = spiReceive(LASER_CTRL0);
00269         
00270         ncs_.write(0);
00271         WAIT_TNCSSCLK();
00272   
00273         if (enable) {
00274             int laser_ctrl0 = CLEAR_BIT(0x00, ADNS9500_LASER_CTRL0_FORCE_DISABLED);
00275             spiSend(LASER_CTRL0, laser_ctrl0);
00276         }
00277         else {
00278             int laser_ctrl0 = SET_BIT(0x01, ADNS9500_LASER_CTRL0_FORCE_DISABLED);
00279             spiSend(LASER_CTRL0, laser_ctrl0);
00280         }        
00281 
00282         WAIT_TSCLKNCS();
00283         ncs_.write(1);
00284     }
00285     
00286     bool ADNS9500::getMotionDelta(int16_t& dx, int16_t& dy)
00287     {
00288         if (! enabled_)
00289             error("ADNS9500::getMotionDelta : the sensor is not enabled\n");
00290 
00291         ncs_.write(0);
00292         WAIT_TNCSSCLK();
00293 
00294         dx = 0;
00295         dy = 0;
00296 
00297         int motion = spiReceive(MOTION);
00298         
00299         if (ADNS9500_IF_MOTION(motion)) {
00300             WAIT_TSRR();
00301             int dxl = spiReceive(DELTA_X_L);
00302             WAIT_TSRR();
00303             dx = ADNS9500_INT16(spiReceive(DELTA_X_H), dxl);
00304             
00305             WAIT_TSRR();
00306             int dyl = spiReceive(DELTA_Y_L);
00307             WAIT_TSRR();
00308             dy = ADNS9500_INT16(spiReceive(DELTA_Y_H), dyl);
00309         }
00310         WAIT_TSCLKNCS();
00311         ncs_.write(1);
00312         
00313         return ADNS9500_IF_MOTION(motion);
00314     }
00315     
00316     bool ADNS9500::getMotionDeltaMM(float& dx, float& dy)
00317     {
00318         int16_t rawDx, rawDy;
00319         bool motion = getMotionDelta(rawDx, rawDy);
00320         dx = (float)rawDx / xCpi_ * 25.4;
00321         dy = (float)rawDy / yCpi_ * 25.4;
00322 
00323         return motion;
00324     }
00325 
00326     bool ADNS9500::getMotionData(MotionData& data)
00327     {
00328         if (! enabled_)
00329             error("ADNS9500::getMotionData : the sensor is not enabled\n");
00330     
00331         ncs_.write(0);
00332         WAIT_TNCSSCLK();
00333         
00334         // activate motion burst mode
00335         spi_.write(0x50);
00336         WAIT_TSRAD();   // see the chronogram
00337         
00338         // read motion burst data
00339         data.motion = spi_.write(0x00);
00340         data.observation = spi_.write(0x00);
00341         
00342         int ldx = spi_.write(0x00);
00343         data.dx = ADNS9500_INT16(spi_.write(0x00), ldx);
00344         
00345         int ldy = spi_.write(0x00);
00346         data.dy = ADNS9500_INT16(spi_.write(0x00), ldy);
00347         
00348         data.surfaceQuality = spi_.write(0x00) * 4;
00349         data.averagePixel = spi_.write(0x00) / 1.76;
00350         data.maximumPixel = spi_.write(0x00);
00351         data.minimumPixel = spi_.write(0x00);
00352         
00353         int ushutter = spi_.write(0x00);
00354         data.shutter = ADNS9500_UINT16(ushutter, spi_.write(0x00));
00355         
00356         int uframe_period = spi_.write(0x00);
00357         data.framePeriod = ADNS9500_UINT16(uframe_period, spi_.write(0x00));
00358 
00359         WAIT_TSCLKNCS();
00360         ncs_.write(1);
00361         WAIT_TBEXIT();
00362 
00363         data.dxMM = (float)data.dx / xCpi_ * 25.4;
00364         data.dyMM = (float)data.dy / yCpi_ * 25.4;
00365 
00366         // write a value to Motion register to clear motion bit
00367         ncs_.write(0);
00368         WAIT_TNCSSCLK();
00369         
00370         spiSend(MOTION, 0x00);
00371 
00372         WAIT_TSCLKNCS();
00373         ncs_.write(1);
00374 
00375         return ADNS9500_IF_MOTION(data.motion);
00376     }
00377     
00378     void ADNS9500::setResolution( uint16_t cpi_xy )
00379     {
00380         
00381         if (! enabled_)
00382             error("ADNS9500::setResolution : the sensor is not enabled\n");
00383 
00384         int res_xy = cpi_to_res(cpi_xy);
00385         
00386         ncs_.write(0);
00387         WAIT_TNCSSCLK();
00388         
00389         // enable XY axes CPI in sync mode
00390         int rpt_mod = spiReceive(CONFIGURATION_II);
00391         rpt_mod = CLEAR_BIT(rpt_mod, ADNS9500_CONFIGURATION_II_RPT_MOD);
00392         WAIT_TSRW();
00393         spiSend(CONFIGURATION_II, rpt_mod);
00394 
00395         // set resolution for X-axis and Y-axis
00396         WAIT_TSWW();
00397         spiSend(CONFIGURATION_I, res_xy);
00398 
00399         WAIT_TSCLKNCS();
00400         ncs_.write(1);
00401         
00402         xCpi_ = res_xy * CPI_CHANGE_UNIT;
00403         yCpi_ = res_xy * CPI_CHANGE_UNIT;
00404     }
00405     
00406     void ADNS9500::setResolution(uint16_t cpi_x, uint16_t cpi_y)
00407     {
00408         if (! enabled_)
00409             error("ADNS9500::setResolution : the sensor is not enabled\n");
00410         
00411         int res_x = cpi_to_res(cpi_x);
00412         int res_y = cpi_to_res(cpi_y);
00413            
00414         ncs_.write(0);
00415         WAIT_TNCSSCLK();
00416 
00417         // disable XY axes CPI in sync mode
00418         int rpt_mod = spiReceive(CONFIGURATION_II);
00419         rpt_mod = SET_BIT(rpt_mod, ADNS9500_CONFIGURATION_II_RPT_MOD);
00420         WAIT_TSRW();
00421         spiSend(CONFIGURATION_II, rpt_mod);
00422         
00423         // set resolution for X-axis
00424         WAIT_TSWW();
00425         spiSend(CONFIGURATION_I, res_x);
00426                 
00427         // set resolution for Y-axis
00428         WAIT_TSWW();
00429         spiSend(CONFIGURATION_V, res_y);
00430 
00431         WAIT_TSCLKNCS();
00432         ncs_.write(1);
00433 
00434         xCpi_ = res_x * CPI_CHANGE_UNIT;
00435         yCpi_ = res_y * CPI_CHANGE_UNIT;
00436     }
00437     
00438     void ADNS9500::captureFrame(uint8_t* pixels)
00439     {
00440         if (! enabled_)
00441             error("ADNS9500::captureFrame : the sensor is not enabled\n");
00442             
00443         ncs_.write(0);
00444         WAIT_TNCSSCLK();
00445         
00446         spiSend(FRAME_CAPTURE, 0x93);
00447         WAIT_TSWW();
00448         spiSend(FRAME_CAPTURE, 0xc5);
00449         LONG_WAIT_US(2*DEFAULT_MAX_FRAME_PERIOD);
00450 
00451         // check for first pixel reading motion bit
00452         int motion = spiReceive(MOTION);
00453         WAIT_TSRR();
00454         while(! ADNS9500_IF_FRAME_FIRST_PIXEL(motion)) {
00455             int motion = spiReceive(MOTION);
00456             WAIT_TSRR();
00457         }
00458 
00459         // read pixel values
00460         spi_.write(PIXEL_BURST);
00461         WAIT_TSRAD();
00462         for (uint8_t* p = pixels; p != pixels + NUMBER_OF_PIXELS_PER_FRAME; ++p) {
00463             *p = spi_.write(0x00);
00464             WAIT_TLOAD();
00465         }
00466 
00467         // burst exit
00468         ncs_.write(1);
00469         WAIT_TBEXIT();
00470     }
00471     
00472     void ADNS9500::spiSend(Register address, int value)
00473     {
00474         spi_.write(SET_BIT(address, SPI_WRITE_MODE));
00475         spi_.write(value);
00476     }
00477 
00478     int ADNS9500::spiReceive(Register address)
00479     {
00480         spi_.write(CLEAR_BIT(address, SPI_WRITE_MODE));
00481         WAIT_TSRAD();
00482         return spi_.write(0x00);
00483     }
00484 }