laser.cpp

00001 
00002 #include "laser.h"
00003 
00004 #define LASER_BAUD_RATE 115200
00005 
00006 extern DigitalOut disableLRF;
00007 extern DigitalOut nReset;
00008 
00009 uint32_t base = 0x40002000;
00010 uint32_t rxOffset = 0x514;
00011 uint32_t txOffset = 0x50c;
00012 
00013 uint32_t rx = base + rxOffset;
00014 uint32_t tx = base + txOffset;
00015 
00016 uint32_t* prx = (uint32_t*)rx;
00017 uint32_t* ptx = (uint32_t*)tx;
00018 
00019 Laser::Laser(Serial& serial, int n) : timerRunning(false), idx(0), serial(serial), powerOffState (true), busy(false), nSamples(n)
00020 {
00021 }
00022 
00023 void Laser::discardResponse()
00024 {
00025     // char c = 0;
00026     wait_ms(30);        // wait for the response
00027     while(serial.readable()) {
00028         serial.getc();
00029     }
00030 }
00031 
00032 void Laser::triggerDistanceMeasurement()
00033 {
00034     if(!busy){
00035         const int bufSize = 30;
00036         char cmd[bufSize];
00037         snprintf(&cmd[0], bufSize, "0 -1 %d doMeasDistExt\n", nSamples);
00038         //char cmd[] = "0 -1 10 doMeasDistExt\n";   // single reading averaged over 10 measurements
00039         //char cmd[] = "0 -1 100 doMeasDistExt\n";   // single reading averaged over 100 measurements
00040         //char cmd[] = "0 -1 -1 doMeasDistExt\n";     // single reading auto choice of number of averages - This could make the laser to lock up and may need reseting
00041     
00042         sendCommand(cmd);
00043         processResponse();
00044         setRedDot(true);
00045     }
00046 }
00047 
00048 bool Laser::sendCommand(char cmd[])
00049 {
00050     // start timer before the first of the command is sent to the laser
00051     timer.reset();
00052     timer.start();
00053     timerRunning = true;
00054 
00055     for(int i = 0; i < strlen(cmd); i++) {
00056         serial.putc(cmd[i]);
00057     }
00058 
00059     return true;
00060 }
00061 
00062 bool Laser::processResponse()
00063 {
00064 #define TIMEOUT     25000  // in units of 100us
00065     int i = 0;
00066     char c = 0;
00067     uint16_t count = 0;
00068     busy = true;
00069     
00070     do {
00071         if(serial.readable()) {
00072             // stop timer as soon as we have received the first byte of the response. We need to subtract the time of this receiption
00073             if(timerRunning) {
00074                 timer.stop();
00075                 timerRunning = false;
00076             }
00077             c = serial.getc();
00078             buf[i++] = c;
00079         } else {
00080             wait_us(100);
00081         }
00082     } while (c != '\n' && i < bufSize && count++ < TIMEOUT);  // timeout after about 500ms and ensure no overflow
00083 
00084 
00085 
00086     if (count >= TIMEOUT || i >= bufSize) { // timeout or overflow
00087         // need to reset the LRF module before talking to it again, otherwise it may lock up
00088          nReset = 0;
00089         wait_ms(100);
00090         nReset = 1;
00091         wait_ms(1000);
00092         enableMeasurement(true);
00093         busy = false;
00094     } else {
00095         buf[i -1] = 0;
00096     }
00097 
00098     float distance = -5;
00099     vector<char*> v;
00100 
00101     split(buf, ' ', v);
00102 
00103     if (v.size() != 6 || atoi(v[1]) != 0 || strcmp(v[5], "Reply") != 0) {
00104         // there is an error
00105         distanceCallback(-1.0, 0.0);
00106     } else {
00107         float elapsed =  (float)(timer.read_us()/1000.0);      // elapsed in ms
00108         distance = atoi(v[2]) / 1000000.0; // distance in m
00109         distanceCallback(distance, elapsed);
00110     }
00111     
00112     busy = false;
00113 
00114     return true;
00115 }
00116 
00117 void Laser::enableMeasurement(bool enable)
00118 {
00119     if (enable) {
00120         sendCommand(";\n");
00121         discardResponse();
00122 
00123     } else {
00124         sendCommand("switchMeasOff\n");
00125         discardResponse(); 
00126         sendCommand("2 0 startPwrSave;");
00127         discardResponse();       
00128     }
00129 }
00130 
00131 void Laser::setRedDot(bool on)
00132 {
00133     if(on) {
00134         char cmd[] = "0 3 0 -1 3 doLaserCmd;\n";
00135         sendCommand(cmd);
00136         wait_ms(120);       // take a while for a response for to measurement command
00137         discardResponse();
00138     } else {
00139         sendCommand("switchMeasOff\n");
00140         discardResponse();
00141     }
00142 }
00143 
00144 void Laser::split(char str[], char c, std::vector<char*>& v)
00145 {
00146     char * pch;
00147     char limiter[] = {c};
00148     pch = strtok (str, limiter);
00149     while (pch != NULL) {
00150         v.push_back(pch);
00151         pch = strtok (NULL, limiter);
00152     }
00153 }
00154 
00155 void Laser::setDistaceCallback(void (*callback)(float, float))
00156 {
00157     distanceCallback = callback;
00158 }
00159 
00160 void Laser::setDebugCallback(void (*callback)(char*))
00161 {
00162     debugCallback = callback;
00163 }
00164 
00165 void Laser::turnLaserPowerOn()
00166 {
00167     if(powerOffState == true) {
00168         powerOffState = false;
00169 
00170         connectPower();
00171         enableMeasurement(true);
00172         setRedDot(true);
00173     }
00174 }
00175 
00176 void Laser::turnLaserPowerOff()
00177 {
00178     if(powerOffState == false) {
00179         powerOffState = true;
00180         setRedDot(false);   // this disables measurements
00181 
00182         removePower();
00183     }
00184 }
00185 
00186 // when connceting power to the laser module ensure that tx and rx pins are
00187 // reassigned to the serial port
00188 void Laser::connectPower()
00189 {
00190     disableLRF = 0;
00191     nReset = 1;
00192     *ptx = 27;  // p27 for tx
00193     *prx = 26;  // p26 for rx
00194     wait_ms(1000);
00195 }
00196 
00197 // when removing power from the laser module ensure that rx and tx pins are not
00198 // driving voltage into the laser module to avoid hardware damage
00199 void Laser::removePower()
00200 {
00201     *ptx = 0xffffffff;      // no pin for tx
00202     *prx = 0xffffffff;      // no pin for rx
00203     DigitalOut rx(p26);
00204     DigitalOut tx(p27);
00205     rx = 0;
00206     tx = 0;
00207     nReset = 0;
00208     disableLRF = 1;
00209 }
00210 
00211 Laser::~Laser()
00212 {
00213     enableMeasurement(false);
00214 }