File content as of revision 1:fa3052be61b5:

/*
 * adns9500.hpp - Interface to access to Avago ADNS-9500 laser mouse sensors
 *
 *   Copyright 2012 Jesus Torres <jmtorres@ull.es>
 *
 * Licensed under the Apache License, Version 2.0 (the "License");
 * you may not use this file except in compliance with the License.
 * You may obtain a copy of the License at
 *
 *     http://www.apache.org/licenses/LICENSE-2.0
 *
 * Unless required by applicable law or agreed to in writing, software
 * distributed under the License is distributed on an "AS IS" BASIS,
 * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
 * See the License for the specific language governing permissions and
 * limitations under the License.
 */

#include <fstream>
#include <math.h>
#include <mbed.h>
#include <string>

#include <adns9500.hpp>

#define WAIT_TSRAD()        wait_us(100)
#define WAIT_TSRWR()        wait_us(20)
#define WAIT_TBEXIT()       wait_us(1)      // 500ns
#define WAIT_TNCSSCLK()     wait_us(1)      // 120ns
#define WAIT_TSCLKNCS()     wait_us(20)
#define WAIT_TLOAD()        wait_us(15)

#define DEFAULT_MAX_FPS             1958
#define DEFAULT_MAX_FRAME_PERIOD    ceil(1.0e6 / DEFAULT_MAX_FPS)   // in us
#define DEFAULT_X_CPI               1620
#define DEFAULT_Y_CPI               1620
#define CPI_CHANGE_UNIT             90

#define SET_BIT(word, mask)         (word | mask)
#define CLEAR_BIT(word, mask)       (word & (~mask))

namespace adns9500 {

    ADNS9500::ADNS9500(PinName mosi, PinName miso, PinName sclk, PinName ncs,
        int spi_frequency, PinName motion)
        : spi_(mosi, miso, sclk),
          motion_(motion),
          ncs_(ncs),
          enabled_(false),
          dx_(0), dy_(0),
          xCpi_(DEFAULT_X_CPI), yCpi_(DEFAULT_Y_CPI)
    {
        motion_.fall(this, &ADNS9500::motionTrigger);
    }

    ADNS9500::~ADNS9500()
    {
        shutdown();
    }

    void ADNS9500::reset(const char* firmware)
    {
        // SPI port reset
        ncs_.write(1);
        WAIT_TNCSSCLK();
        ncs_.write(0);
        WAIT_TNCSSCLK();
        
        // send 0x3a to POWER_UP_RESET and wait for at least 50ms
        spi_.write(POWER_UP_RESET);
        WAIT_TSRAD();
        spi_.write(0x5a);
        wait_ms(50);
        
        // clear observation register. Only required to deassert shutdown mode.
        spi_.write(OBSERVATION);
        WAIT_TSRAD();
        spi_.write(0x00);
        wait_us(DEFAULT_MAX_FRAME_PERIOD);
        
        // check observation register bits [5:0]
        spi_.write(OBSERVATION);
        WAIT_TSRAD();
        int observation = spi_.write(0x00);
        WAIT_TSRAD();

        if (! ADNS9500_IF_OBSERVATION_TEST(observation))
            error("ADNS9500::reset : observation register test failed: 0x%x\n", observation);

        // read motion data
        spi_.write(MOTION);
        WAIT_TSRAD();
        spi_.write(DELTA_X_L);
        WAIT_TSRAD();
        spi_.write(DELTA_X_H);
        WAIT_TSRAD();
        spi_.write(DELTA_Y_L);
        WAIT_TSRAD();
        spi_.write(DELTA_Y_H);
        WAIT_TSRAD();
        
        // read product and revision id to test the connection        
        spi_.write(PRODUCT_ID);
        WAIT_TSRAD();
        int product_id = spi_.write(REVISION_ID);
        WAIT_TSRAD();
        int revision_id = spi_.write(0x00);
        WAIT_TSCLKNCS();

        ncs_.write(1);

        if (product_id != 0x33) {
            error("ADNS9500::reset : bad product ID: 0x%x\n", product_id);
        }

        if (revision_id != 0x03) {
            error("ADNS9500::reset : bad revision ID: 0x%x\n", revision_id);
        }

        enabled_ = true;

        if (firmware) {
            sromDownload(firmware);
            enableLaser();
        }
    }

    void ADNS9500::shutdown()
    {
        if (! enabled_)
            error("ADNS9500::shutdown : the sensor is not enabled\n");
        
        ncs_.write(0);
        WAIT_TNCSSCLK();
        
        // send 0x3a to POWER_UP_RESET
        spi_.write(POWER_UP_RESET);
        WAIT_TSRAD();
        spi_.write(0x5a);
        WAIT_TSCLKNCS();
        
        ncs_.write(1);
        
        enabled_ = false;
    }

    int ADNS9500::read(Register lregister)
    {
        if (! enabled_)
            error("ADNS9500::read : the sensor is not enabled\n");
    
        ncs_.write(0);
        WAIT_TNCSSCLK();
        
        // send the command to read the register
        spi_.write(lregister);
        WAIT_TSRAD();
        int value = spi_.write(0x00);
        WAIT_TSCLKNCS();
        
        ncs_.write(1);
        return value;
    }

    int ADNS9500::read(Register uregister, Register lregister)
    {
        if (! enabled_)
            error("ADNS9500::read : the sensor is not enabled\n");

        ncs_.write(0);
        WAIT_TNCSSCLK();

        // send the command to read the registers
        spi_.write(lregister);
        WAIT_TSRAD();
        int lvalue = spi_.write(uregister);
        WAIT_TSRAD();
        int uvalue = spi_.write(0x00);
        WAIT_TSCLKNCS();

        ncs_.write(1);

        return ADNS9500_UINT16(uvalue, lvalue);
    }
    
    int ADNS9500::sromDownload(const char* filename)
    {
        if (! enabled_)
            error("ADNS9500::sromDownload : the sensor is not enabled\n");
    
        ncs_.write(0);
        WAIT_TNCSSCLK();

        // SROM download
        spi_.write(CONFIGURATION_IV);
        WAIT_TSRAD();
        spi_.write(ADNS9500_CONFIGURATION_IV_SROM_SIZE);
        WAIT_TSRAD();
        spi_.write(SROM_ENABLE);
        WAIT_TSRAD();
        spi_.write(0x1d);
        wait_us(DEFAULT_MAX_FRAME_PERIOD);
        spi_.write(SROM_ENABLE);
        WAIT_TSRAD();
        spi_.write(0x18);
        WAIT_TSRAD();
        spi_.write(SROM_LOAD_BURST);
        
        // TODO: Comprobar que pasa si el archivo no existe
        ifstream ifs(filename, ifstream::in);
        while(ifs.good()) {
            WAIT_TLOAD();
            spi_.write(ifs.get());
        }
        WAIT_TSCLKNCS();
        ncs_.write(1);
        WAIT_TBEXIT();

        if (! ifs.eof())
            error("ADNS9500::sromDownload : error reading from file: %s\n", filename);

        // test if SROM was downloaded successfully
        wait_us(160);
        ncs_.write(0);
        WAIT_TNCSSCLK();
        spi_.write(SROM_ID);
        WAIT_TSRAD();
        int srom_id = spi_.write(0x00);
        WAIT_TSCLKNCS();
        ncs_.write(1);
        
        if (! srom_id)
            error("ADNS9500::sromDownload : the firmware was not successful downloaded\n");

        // test laser fault condition
        ncs_.write(0);
        WAIT_TNCSSCLK();
        spi_.write(MOTION);
        WAIT_TSRAD();
        int motion = spi_.write(0x00);
        WAIT_TSCLKNCS();
        ncs_.write(1);
        
        if (ADNS9500_IF_LASER_FAULT(motion))
            error("ADNS9500::sromDownload : laser fault condition detected\n");
        
        // return the SROM CRC value
        ncs_.write(0);
        WAIT_TNCSSCLK();
        
        spi_.write(SROM_ENABLE);
        WAIT_TSRAD();
        spi_.write(0x15);
        wait_us(10);
        spi_.write(DATA_OUT_LOWER);
        WAIT_TSRAD();
        int lcrc = spi_.write(DATA_OUT_UPPER);
        WAIT_TSRAD();
        int ucrc = spi_.write(0x00);
        
        WAIT_TSCLKNCS();
        ncs_.write(1);
        
        return ADNS9500_UINT16(ucrc, lcrc);
    }

    void ADNS9500::enableLaser(bool enable)
    {
        if (! enabled_)
            error("ADNS9500::enableLaser : the sensor is not enabled\n");
    
        ncs_.write(0);
        WAIT_TNCSSCLK();

        spi_.write(LASER_CTRL0);
        WAIT_TSRAD();
        if (enable) {
            int laser_ctrl0 = CLEAR_BIT(0x00, ADNS9500_LASER_CTRL0_FORCE_DISABLED);
            spi_.write(laser_ctrl0);
        }
        else {
            int laser_ctrl0 = SET_BIT(0x00, ADNS9500_LASER_CTRL0_FORCE_DISABLED);
            spi_.write(laser_ctrl0);
        }        

        WAIT_TSCLKNCS();
        ncs_.write(1);
    }

    bool ADNS9500::getMotionDelta(int& dx, int& dy)
    {
        if (! enabled_)
            error("ADNS9500::getMotionDelta : the sensor is not enabled\n");

        ncs_.write(0);
        WAIT_TNCSSCLK();
    
        spi_.write(MOTION);
        WAIT_TSRAD();
        int motion = spi_.write(DELTA_X_L);
        WAIT_TSRAD();
        
        if (ADNS9500_IF_MOTION(motion)) {
            int tmp = spi_.write(DELTA_X_L);
            WAIT_TSRAD();
            dx = ADNS9500_UINT16(spi_.write(DELTA_X_H), tmp);
            WAIT_TSRAD();
            tmp = spi_.write(DELTA_Y_L);
            WAIT_TSRAD();
            dy = ADNS9500_UINT16(spi_.write(DELTA_Y_H), tmp);
            
            dx_ = dx;
            dy_ = dy;
        }
        else {
            spi_.write(0x00);

            dx = dx_;
            dy = dy_;
        }
        
        WAIT_TSCLKNCS();
        ncs_.write(1);

        return ADNS9500_IF_MOTION(motion);
    }
    
    bool ADNS9500::getMotionDeltaMM(float& dx, float& dy)
    {
        int rawDx, rawDy;

        bool motion = getMotionDelta(rawDx, rawDy);
        dx = (float)rawDx / xCpi_ * 25.4;
        dy = (float)rawDy / yCpi_ * 25.4;

        return motion;
    }

    bool ADNS9500::getMotionData(MotionData& data)
    {
        if (! enabled_)
            error("ADNS9500::getMotionData : the sensor is not enabled\n");
    
        ncs_.write(0);
        WAIT_TNCSSCLK();
        
        // activate motion burst mode
        spi_.write(MOTION_BURST);
        WAIT_TSRAD();
        spi_.write(0x50);
        
        // if in run mode, wait for 1 frame
        wait_us(DEFAULT_MAX_FRAME_PERIOD);
        
        // read motion burst data
        data.motion = spi_.write(0x00);
        data.observation = spi_.write(0x00);
        
        int ldx = spi_.write(0x00);
        data.dx = ADNS9500_UINT16(spi_.write(0x00), ldx);
        
        int ldy = spi_.write(0x00);
        data.dy = ADNS9500_UINT16(spi_.write(0x00), ldy);
        
        data.squal = spi_.write(0x00);
        data.pixelSum = spi_.write(0x00);
        data.maximumPixel = spi_.write(0x00);
        data.minimumPixel = spi_.write(0x00);
        
        int ushutter = spi_.write(0x00);
        data.shutter = ADNS9500_UINT16(ushutter, spi_.write(0x00));
        
        int uframe_period = spi_.write(0x00);
        data.framePeriod = ADNS9500_UINT16(uframe_period, spi_.write(0x00));

        WAIT_TSCLKNCS();
        ncs_.write(1);
        WAIT_TBEXIT();
        
        return ADNS9500_IF_MOTION(data.motion);
    }
    
    void ADNS9500::setResolution(Resolution xy_resolution)
    {
        if (! enabled_)
            error("ADNS9500::setResolution : the sensor is not enabled\n");
        
        ncs_.write(0);
        WAIT_TNCSSCLK();
        
        // enable XY axes CPI in sync mode
        spi_.write(CONFIGURATION_II);
        WAIT_TSRAD();
        int rpt_mod = spi_.write(0x00);
        WAIT_TSRAD();
        spi_.write(CLEAR_BIT(rpt_mod, ADNS9500_CONFIGURATION_II_RPT_MOD));
        WAIT_TSRAD();

        // set resolution for X-axis and Y-axis
        spi_.write(CONFIGURATION_I);
        WAIT_TSRAD();
        spi_.write(xy_resolution);

        WAIT_TSCLKNCS();
        ncs_.write(1);
        
        xCpi_ = xy_resolution * CPI_CHANGE_UNIT;
        yCpi_ = xy_resolution * CPI_CHANGE_UNIT;
    }
    
    void ADNS9500::setResolution(Resolution x_resolution, Resolution y_resolution)
    {
        if (! enabled_)
            error("ADNS9500::setResolution : the sensor is not enabled\n");
            
        ncs_.write(0);
        WAIT_TNCSSCLK();

        // disable XY axes CPI in sync mode
        spi_.write(CONFIGURATION_II);
        WAIT_TSRAD();
        int rpt_mod = spi_.write(0x00);
        WAIT_TSRAD();
        spi_.write(SET_BIT(rpt_mod, ADNS9500_CONFIGURATION_II_RPT_MOD));
        WAIT_TSRAD();
        
        // set resolution for X-axis
        spi_.write(CONFIGURATION_I);
        WAIT_TSRAD();
        spi_.write(x_resolution);
        WAIT_TSRAD();
                
        // set resolution for Y-axis
        spi_.write(CONFIGURATION_V);
        WAIT_TSRAD();
        spi_.write(y_resolution);

        WAIT_TSCLKNCS();
        ncs_.write(1);

        xCpi_ = x_resolution * CPI_CHANGE_UNIT;
        yCpi_ = y_resolution * CPI_CHANGE_UNIT;
    }
    
    void ADNS9500::captureFrame(uint8_t pixels[NUMBER_OF_PIXELS_PER_FRAME])
    {
        if (! enabled_)
            error("ADNS9500::captureFrame : the sensor is not enabled\n");
            
        ncs_.write(0);
        WAIT_TNCSSCLK();
        
        spi_.write(FRAME_CAPTURE);
        WAIT_TSRAD();
        spi_.write(0x93);
        WAIT_TSRAD();
        spi_.write(FRAME_CAPTURE);
        WAIT_TSRAD();
        spi_.write(0xc5);
        wait_us(DEFAULT_MAX_FRAME_PERIOD);
        wait_us(DEFAULT_MAX_FRAME_PERIOD);
        
        // check for first pixel reading motion bit
        spi_.write(MOTION);
        while(true) {
            WAIT_TSRAD();
            int motion = spi_.write(MOTION);
            if (ADNS9500_IF_MOTION(motion))
                break;
        }
        
        // read pixel values
        spi_.write(PIXEL_BURST);
        WAIT_TSRAD();
        for (uint8_t* p = pixels; p != pixels + sizeof(pixels); ++p) {
            WAIT_TLOAD();
            *p = spi_.write(PIXEL_BURST);
        }

        // burst exit        
        WAIT_TSCLKNCS();
        ncs_.write(1);
        WAIT_TBEXIT();
    }
}