Chris Majoros
Interface to access to Avago ADNS-9500 laser mouse sensors.
Fork of
ADNS9500
by 
Chris Majoros
Diff: adns9500.cpp
- Revision:
- 1:fa3052be61b5
- Parent:
- 0:782f2061a8f5
- Child:
- 2:ee0c13ef1320
--- /dev/null Thu Jan 01 00:00:00 1970 +0000
+++ b/adns9500.cpp Mon Feb 13 11:39:24 2012 +0000
@@ -0,0 +1,485 @@
+/*
+ * 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();
+ }
+}
\ No newline at end of file