Revision 2:ee0c13ef1320, committed 2012-03-22

Comitter:

aplatanado

Date:

Thu Mar 22 15:04:33 2012 +0000

Parent:

1:fa3052be61b5

Child:

3:898ed1944119

Commit message:

Fix motion burst

Changed in this revision

--- a/adns9500.cpp	Mon Feb 13 11:39:24 2012 +0000
+++ b/adns9500.cpp	Thu Mar 22 15:04:33 2012 +0000
@@ -16,6 +16,7 @@
  * limitations under the License.
  */
 
+#include <cstdlib>
 #include <fstream>
 #include <math.h>
 #include <mbed.h>
@@ -24,18 +25,30 @@
 #include <adns9500.hpp>
 
 #define WAIT_TSRAD()        wait_us(100)
-#define WAIT_TSRWR()        wait_us(20)
+#define WAIT_TSRR()         wait_us(20)
+#define WAIT_TSRW()         wait_us(20)
+#define WAIT_TSWR()         wait_us(120)
+#define WAIT_TSWW()         wait_us(120)
 #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 LONG_WAIT_MS(x)     \
+    WAIT_TSCLKNCS(); ncs_.write(1); wait_ms(x); ncs_.write(0); WAIT_TNCSSCLK()
+#define LONG_WAIT_US(x)     \
+    WAIT_TSCLKNCS(); ncs_.write(1); wait_us(x); ncs_.write(0); WAIT_TNCSSCLK()
+
 #define DEFAULT_MAX_FPS             1958
-#define DEFAULT_MAX_FRAME_PERIOD    ceil(1.0e6 / DEFAULT_MAX_FPS)   // in us
+#define DEFAULT_MAX_FRAME_PERIOD    ((int)ceil(1e6 / DEFAULT_MAX_FPS))  // in us
 #define DEFAULT_X_CPI               1620
 #define DEFAULT_Y_CPI               1620
 #define CPI_CHANGE_UNIT             90
 
+#define SPI_BITS_PER_FRAME          8
+#define SPI_MODE                    3
+#define SPI_WRITE_MODE              0x80
+
 #define SET_BIT(word, mask)         (word | mask)
 #define CLEAR_BIT(word, mask)       (word & (~mask))
 
@@ -47,9 +60,11 @@
           motion_(motion),
           ncs_(ncs),
           enabled_(false),
-          dx_(0), dy_(0),
           xCpi_(DEFAULT_X_CPI), yCpi_(DEFAULT_Y_CPI)
     {
+        spi_.format(SPI_BITS_PER_FRAME, SPI_MODE);
+
+        motion_.mode(PullUp);
         motion_.fall(this, &ADNS9500::motionTrigger);
     }
 
@@ -67,46 +82,41 @@
         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);
+        spiSend(POWER_UP_RESET, 0x5a);
+        LONG_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);
+        spiSend(OBSERVATION, 0x00);
+        LONG_WAIT_US(DEFAULT_MAX_FRAME_PERIOD);
         
         // check observation register bits [5:0]
-        spi_.write(OBSERVATION);
-        WAIT_TSRAD();
-        int observation = spi_.write(0x00);
-        WAIT_TSRAD();
-
+        int observation = spiReceive(OBSERVATION);
         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();
+        WAIT_TSRR();
+        spiReceive(MOTION);
+        WAIT_TSRR();
+        spiReceive(DELTA_X_L);
+        WAIT_TSRR();
+        spiReceive(DELTA_X_H);
+        WAIT_TSRR();
+        spiReceive(DELTA_Y_L);
+        WAIT_TSRR();
+        spiReceive(DELTA_Y_H);
         
-        // read product and revision id to test the connection        
+        // read product and revision id to test the connection
+        WAIT_TSRR();
         spi_.write(PRODUCT_ID);
         WAIT_TSRAD();
-        int product_id = spi_.write(REVISION_ID);
+        int product_id = spi_.write(0x00);
+        WAIT_TSRR();
+        spi_.write(REVISION_ID);
         WAIT_TSRAD();
         int revision_id = spi_.write(0x00);
+
         WAIT_TSCLKNCS();
-
         ncs_.write(1);
 
         if (product_id != 0x33) {
@@ -134,11 +144,9 @@
         WAIT_TNCSSCLK();
         
         // send 0x3a to POWER_UP_RESET
-        spi_.write(POWER_UP_RESET);
-        WAIT_TSRAD();
-        spi_.write(0x5a);
+        spiSend(POWER_UP_RESET, 0x5a);
+        
         WAIT_TSCLKNCS();
-        
         ncs_.write(1);
         
         enabled_ = false;
@@ -153,12 +161,11 @@
         WAIT_TNCSSCLK();
         
         // send the command to read the register
-        spi_.write(lregister);
-        WAIT_TSRAD();
-        int value = spi_.write(0x00);
+        int value = spiReceive(lregister);
+        
         WAIT_TSCLKNCS();
-        
         ncs_.write(1);
+
         return value;
     }
 
@@ -171,13 +178,11 @@
         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);
+        int lvalue = spiReceive(lregister);
+        WAIT_TSRR();
+        int uvalue = spiReceive(uregister);
+
         WAIT_TSCLKNCS();
-
         ncs_.write(1);
 
         return ADNS9500_UINT16(uvalue, lvalue);
@@ -192,25 +197,22 @@
         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
+        spiSend(CONFIGURATION_IV, ADNS9500_CONFIGURATION_IV_SROM_SIZE);
+        WAIT_TSWW();
+        spiSend(SROM_ENABLE, 0x1d);
+        LONG_WAIT_US(DEFAULT_MAX_FRAME_PERIOD);
+
+        spiSend(SROM_ENABLE, 0x18);
+        WAIT_TSWW();
+        spi_.write(SET_BIT(SROM_LOAD_BURST, SPI_WRITE_MODE));
+
+        // we expect a line per byte in hex without 0x prefix
+        char buffer[4];
         ifstream ifs(filename, ifstream::in);
-        while(ifs.good()) {
+        while(ifs.getline(buffer, sizeof(buffer)).good()) {
             WAIT_TLOAD();
-            spi_.write(ifs.get());
+            int byte = strtol(buffer, NULL, 16);
+            spi_.write(byte);
         }
         WAIT_TSCLKNCS();
         ncs_.write(1);
@@ -223,9 +225,9 @@
         wait_us(160);
         ncs_.write(0);
         WAIT_TNCSSCLK();
-        spi_.write(SROM_ID);
-        WAIT_TSRAD();
-        int srom_id = spi_.write(0x00);
+        
+        int srom_id = spiReceive(SROM_ID);
+        
         WAIT_TSCLKNCS();
         ncs_.write(1);
         
@@ -235,9 +237,9 @@
         // test laser fault condition
         ncs_.write(0);
         WAIT_TNCSSCLK();
-        spi_.write(MOTION);
-        WAIT_TSRAD();
-        int motion = spi_.write(0x00);
+        
+        int motion = spiReceive(MOTION);
+        
         WAIT_TSCLKNCS();
         ncs_.write(1);
         
@@ -248,15 +250,12 @@
         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);
+        spiSend(SROM_ENABLE, 0x15);
+        LONG_WAIT_MS(10);
+        
+        int lcrc = spiReceive(DATA_OUT_LOWER);
+        WAIT_TSRR();
+        int ucrc = spiReceive(DATA_OUT_UPPER);
         
         WAIT_TSCLKNCS();
         ncs_.write(1);
@@ -268,19 +267,17 @@
     {
         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);
+            spiSend(LASER_CTRL0, laser_ctrl0);
         }
         else {
             int laser_ctrl0 = SET_BIT(0x00, ADNS9500_LASER_CTRL0_FORCE_DISABLED);
-            spi_.write(laser_ctrl0);
+            spiSend(LASER_CTRL0, laser_ctrl0);
         }        
 
         WAIT_TSCLKNCS();
@@ -294,34 +291,26 @@
 
         ncs_.write(0);
         WAIT_TNCSSCLK();
-    
-        spi_.write(MOTION);
-        WAIT_TSRAD();
-        int motion = spi_.write(DELTA_X_L);
-        WAIT_TSRAD();
-        
+
+        dx = 0;
+        dy = 0;
+
+        int motion = spiReceive(MOTION);
         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);
+            WAIT_TSRR();
+            int dxl = spiReceive(DELTA_X_L);
+            WAIT_TSRR();
+            dx += ADNS9500_INT16(spiReceive(DELTA_X_H), dxl);
             
-            dx_ = dx;
-            dy_ = dy;
+            WAIT_TSRR();
+            int dyl = spiReceive(DELTA_Y_L);
+            WAIT_TSRR();
+            dy += ADNS9500_INT16(spiReceive(DELTA_Y_H), dyl);
         }
-        else {
-            spi_.write(0x00);
 
-            dx = dx_;
-            dy = dy_;
-        }
-        
         WAIT_TSCLKNCS();
         ncs_.write(1);
-
+        
         return ADNS9500_IF_MOTION(motion);
     }
     
@@ -345,25 +334,21 @@
         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);
+        WAIT_TSRAD();   // see the chronogram
         
         // 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);
+        data.dx = ADNS9500_INT16(spi_.write(0x00), ldx);
         
         int ldy = spi_.write(0x00);
-        data.dy = ADNS9500_UINT16(spi_.write(0x00), ldy);
+        data.dy = ADNS9500_INT16(spi_.write(0x00), ldy);
         
-        data.squal = spi_.write(0x00);
-        data.pixelSum = spi_.write(0x00);
+        data.surfaceQuality = spi_.write(0x00) * 4;
+        data.averagePixel = spi_.write(0x00) / 1.76;
         data.maximumPixel = spi_.write(0x00);
         data.minimumPixel = spi_.write(0x00);
         
@@ -376,7 +361,19 @@
         WAIT_TSCLKNCS();
         ncs_.write(1);
         WAIT_TBEXIT();
+
+        data.dxMM = (float)data.dx / xCpi_ * 25.4;
+        data.dyMM = (float)data.dy / yCpi_ * 25.4;
+
+        // write a value to Motion register to clear motion bit
+        ncs_.write(0);
+        WAIT_TNCSSCLK();
         
+        spiSend(MOTION, 0x00);
+
+        WAIT_TSCLKNCS();
+        ncs_.write(1);
+
         return ADNS9500_IF_MOTION(data.motion);
     }
     
@@ -389,17 +386,14 @@
         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();
+        int rpt_mod = spiReceive(CONFIGURATION_II);
+        rpt_mod = CLEAR_BIT(rpt_mod, ADNS9500_CONFIGURATION_II_RPT_MOD);
+        WAIT_TSRW();
+        spiSend(CONFIGURATION_II, rpt_mod);
 
         // set resolution for X-axis and Y-axis
-        spi_.write(CONFIGURATION_I);
-        WAIT_TSRAD();
-        spi_.write(xy_resolution);
+        WAIT_TSWW();
+        spiSend(CONFIGURATION_I, xy_resolution);
 
         WAIT_TSCLKNCS();
         ncs_.write(1);
@@ -417,23 +411,18 @@
         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();
+        int rpt_mod = spiReceive(CONFIGURATION_II);
+        rpt_mod = SET_BIT(rpt_mod, ADNS9500_CONFIGURATION_II_RPT_MOD);
+        WAIT_TSRW();
+        spiSend(CONFIGURATION_II, rpt_mod);
         
         // set resolution for X-axis
-        spi_.write(CONFIGURATION_I);
-        WAIT_TSRAD();
-        spi_.write(x_resolution);
-        WAIT_TSRAD();
+        WAIT_TSWW();
+        spiSend(CONFIGURATION_I, x_resolution);
                 
         // set resolution for Y-axis
-        spi_.write(CONFIGURATION_V);
-        WAIT_TSRAD();
-        spi_.write(y_resolution);
+        WAIT_TSWW();
+        spiSend(CONFIGURATION_V, y_resolution);
 
         WAIT_TSCLKNCS();
         ncs_.write(1);
@@ -450,25 +439,20 @@
         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);
-        
+        spiSend(FRAME_CAPTURE, 0x93);
+        WAIT_TSWW();
+        spiSend(FRAME_CAPTURE, 0xc5);
+        LONG_WAIT_US(2*DEFAULT_MAX_FRAME_PERIOD);
+
         // check for first pixel reading motion bit
-        spi_.write(MOTION);
         while(true) {
-            WAIT_TSRAD();
-            int motion = spi_.write(MOTION);
+            int motion = spiReceive(MOTION);
             if (ADNS9500_IF_MOTION(motion))
                 break;
+            WAIT_TSRR();
         }
-        
+        WAIT_TSRR();
+
         // read pixel values
         spi_.write(PIXEL_BURST);
         WAIT_TSRAD();
@@ -482,4 +466,17 @@
         ncs_.write(1);
         WAIT_TBEXIT();
     }
+    
+    void ADNS9500::spiSend(Register address, int value)
+    {
+        spi_.write(SET_BIT(address, SPI_WRITE_MODE));
+        spi_.write(value);
+    }
+
+    int ADNS9500::spiReceive(Register address)
+    {
+        spi_.write(CLEAR_BIT(address, SPI_WRITE_MODE));
+        WAIT_TSRAD();
+        return spi_.write(0x00);
+    }
 }
\ No newline at end of file

--- a/adns9500.hpp	Mon Feb 13 11:39:24 2012 +0000
+++ b/adns9500.hpp	Thu Mar 22 15:04:33 2012 +0000
@@ -32,18 +32,22 @@
 #define ADNS9500_IF_LASER_FAULT(x)          (bool)(x & 0x40)
 #define ADNS9500_IF_RUNNING_SROM_CODE(x)    (bool)(x & 0x80)
 #define ADNS9500_IF_OBSERVATION_TEST(x)     (bool)(x & ADNS9500_OBSERVATION_CHECK_BITS)
-#define ADNS9500_UINT16(ub, lb)             ((uint16_t)ub << 8 | (uint16_t)lb)
+#define ADNS9500_UINT16(ub, lb)             (((uint16_t)ub << 8) + (uint16_t)lb)
+#define ADNS9500_INT16(ub, lb)              ((((int16_t)(int8_t)ub) << 8) + (int16_t)lb)
 
 namespace adns9500
 {
     // Maximum SPI clock frequency supported by the sensor
-    const int MAX_SPI_FREQUENCY = 2000000;
+    const int MAX_SPI_FREQUENCY = 200;
     
     // Internal oscillator norminal frequency
     const int INTERNAL_OSCILLATOR_FREQUENCY = 47000000;
     
     // Number of pixels per frame
     const int NUMBER_OF_PIXELS_PER_FRAME = 900;
+    
+    // Maximum surface quality
+    const int MAX_SURFACE_QUALITY = 676;    // 169 * 4
 
     //
     // Sensor registers
@@ -101,12 +105,20 @@
     
     struct MotionData
     {
+        MotionData()
+            : motion(0), observation(0), dx(0), dy(0), dxMM(0), dyMM(0),
+              surfaceQuality(0), averagePixel(0), maximumPixel(0),
+              minimumPixel(0), shutter(0), framePeriod(0)
+        {}
+    
         int motion;
         int observation;
         int dx;
         int dy;
-        int squal;
-        int pixelSum;
+        float dxMM;
+        float dyMM;
+        int surfaceQuality;
+        float averagePixel;
         int maximumPixel;
         int minimumPixel;
         int shutter;
@@ -281,12 +293,27 @@
             SPI spi_;
             InterruptIn motion_;
             DigitalOut ncs_;
-            
+
             bool enabled_;            
-            int dx_, dy_;
             int xCpi_, yCpi_;
             
             FunctionPointer motionTrigger_;
+            
+            //
+            // Write a byte to the specified register
+            //
+            // @param address The register address
+            // @param value The value to be written to the register
+            //
+            void spiSend(Register address, int value);
+
+            //
+            // Read a byte from the specified register
+            //
+            // @param address The register address
+            // @return The value of the register
+            //
+            int spiReceive(Register address);
     };
 }
 

--- a/examples/main.cpp	Mon Feb 13 11:39:24 2012 +0000
+++ /dev/null	Thu Jan 01 00:00:00 1970 +0000
@@ -1,8 +0,0 @@
-#include "adns9500.hpp"
-#include "mbed.h"
-
-adns9500::ADNS9500 sensor();
-
-int main()
-{
-}

--- a/mbed.lib	Mon Feb 13 11:39:24 2012 +0000
+++ b/mbed.lib	Thu Mar 22 15:04:33 2012 +0000
@@ -1,1 +1,1 @@
-http://mbed.org/projects/libraries/svn/mbed/trunk@35
\ No newline at end of file
+http://mbed.org/projects/libraries/svn/mbed/trunk@43
\ No newline at end of file