ErrorHandler.cpp

00001 /* Copyright C2014 ARM, MIT License
00002  *
00003  * Permission is hereby granted, free of charge, to any person obtaining a copy of this software
00004  * and associated documentation files the "Software", to deal in the Software without restriction,
00005  * including without limitation the rights to use, copy, modify, merge, publish, distribute,
00006  * sublicense, and/or sell copies of the Software, and to permit persons to whom the Software is
00007  * furnished to do so, subject to the following conditions:
00008  *
00009  * The above copyright notice and this permission notice shall be included in all copies or
00010  * substantial portions of the Software.
00011  *
00012  * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR IMPLIED, INCLUDING
00013  * BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND
00014  * NONINFRINGEMENT. IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM,
00015  * DAMAGES OR OTHER LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
00016  * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE.
00017  */
00018  
00019  #include "ErrorHandler.h"
00020  
00021  #if _UBLOX_PLATFORM    
00022     // Annunciations
00023     DigitalOut led1(P3_25);
00024     DigitalOut led2(P3_25);
00025     DigitalOut led3(P3_25);
00026     DigitalOut led4(P3_25);
00027     
00028     // Multi-color LED support
00029     PwmOut r(D5);
00030     PwmOut g(D9);
00031     PwmOut b(D8);  
00032 #endif
00033 
00034 #if _NXP_PLATFORM    
00035     // Annunciations
00036     DigitalOut led1(LED1);
00037     DigitalOut led2(LED2);
00038     DigitalOut led3(LED3);
00039     DigitalOut led4(LED4);  
00040     
00041     // Multi-color LED support
00042     PwmOut r(p23);
00043     PwmOut g(p24);
00044     PwmOut b(p25);  
00045 #endif
00046 
00047 #if _K64F_PLATFORM
00048     // Annunciations
00049     DigitalOut led1(LED1);
00050     DigitalOut led2(LED2);
00051     DigitalOut led3(LED1);
00052     DigitalOut led4(LED2);  
00053    
00054     // Multi-color LED support
00055     DigitalOut r(PTB22);
00056     DigitalOut g(PTE26);
00057     DigitalOut b(PTB21); 
00058 #endif
00059 
00060 // Memory statistics macro
00061 #define ERROR_HANDLER_MEM_STATS(x) \
00062     int s##x=0;\
00063     int *h##x = new int [1];\
00064     if (this->m_pc != NULL) this->m_pc->printf("\r\nMEMORY: stack: 0x%08x  heap: 0x%08x  avail: %d bytes\r\n", &s##x, h##x, &s##x-h##x);\
00065     if (h##x > &s##x)\
00066     error("collision\n");\
00067     delete [] h##x;\
00068     __nop();
00069     
00070  // constructor
00071  ErrorHandler::ErrorHandler(RawSerial *pc,LCDCLASS *lcd) {
00072      this->m_pc = pc;
00073      this->m_lcd = lcd;
00074      memset(this->m_message,0,MAX_LOG_MESSAGE+1);
00075      led1 = 0; led2 = 0; led3 = 0; led4 = 0;
00076  }
00077 
00078  // destructor
00079  ErrorHandler::~ErrorHandler() {
00080  }
00081  
00082  // VARARGS: log 
00083  void ErrorHandler::log(const char *format, ...)  { 
00084     // construct the log message
00085     memset(this->m_message,0,MAX_LOG_MESSAGE+1);
00086     va_list args;
00087     va_start(args, format);
00088     vsprintf(this->m_message, format, args);
00089     va_end(args);
00090     
00091     // make sure we have a message to log
00092     if (strlen(this->m_message) > 0) {
00093        // Log to serial...
00094        if (this->m_pc != NULL) {
00095             this->m_pc->printf(this->m_message);
00096             this->m_pc->printf("\r\n");
00097        }
00098         
00099        // Log to the LCD panel...
00100        if (this->m_lcd != NULL) {
00101             this->m_lcd->cls();
00102             this->m_lcd->locate(0,0);
00103             this->m_lcd->printf(this->m_message);
00104        }
00105     }
00106  } 
00107  
00108  // VARARGS: log 
00109  void ErrorHandler::logConsole(const char *format, ...)  { 
00110     // construct the log message
00111     memset(this->m_message,0,MAX_LOG_MESSAGE+1);
00112     va_list args;
00113     va_start(args, format);
00114     vsprintf(this->m_message, format, args);
00115     va_end(args);
00116     
00117     // make sure we have a message to log
00118     if (strlen(this->m_message) > 0) {
00119        // Log to serial...
00120        if (this->m_pc != NULL) {
00121             this->m_pc->printf(this->m_message);
00122             this->m_pc->printf("\r\n");
00123        }
00124     }
00125  } 
00126  
00127  // set the color LED 
00128  void ErrorHandler::setRGBLED(float H, float S, float V) {
00129     float f,h,p,q,t;
00130     int i;
00131     if( S == (float)0.0) {
00132         r = (float)1.0 - V;  // invert pwm !
00133         g = (float)1.0 - V;
00134         b = (float)1.0 - V;
00135         return;
00136     }
00137     if(H > (float)360.0) H = (float)0.0;   // check values
00138     if(S > (float)1.0) S = (float)1.0; 
00139     if(S < (float)0.0) S = (float)0.0;
00140     if(V > (float)1.0) V =(float) 1.0;
00141     if(V < (float)0.0) V = (float)0.0;
00142     h = (float)H / (float)60.0;
00143     i = (int) h;
00144     f = h - i;
00145     p = (float)V * ((float)1.0 - S);
00146     q = (float)V * ((float)1.0 - (S * f));
00147     t = (float)V * ((float)1.0 - (S * ((float)1.0 - f)));
00148 
00149     switch(i) {
00150         case 0:
00151             r = (float)1.0 - V;  // invert pwm !
00152             g = (float)1.0 - t;
00153             b = (float)1.0 - p;
00154             break;
00155         case 1:
00156             r = (float)1.0 - q;
00157             g = (float)1.0 - V;
00158             b = (float)1.0 - p;
00159             break;
00160         case 2:
00161             r = (float)1.0 - p;
00162             g = (float)1.0 - V;
00163             b = (float)1.0 - t;
00164             break;
00165         case 3:
00166             r = (float)1.0 - p;
00167             g = (float)1.0 - q;
00168             b = (float)1.0 - V;
00169             break;
00170         case 4:
00171             r = (float)1.0 - t;
00172             g = (float)1.0 - p;
00173             b = (float)1.0 - V;
00174             break;
00175         case 5:
00176         default:
00177             r = (float)1.0 - V;
00178             g = (float)1.0 - p;
00179             b = (float)1.0 - q;
00180             break;
00181     }
00182  }
00183 
00184  // turn the RGB LED specific colors
00185  void ErrorHandler::turnLEDRed() { this->setRGBLED(0.0,1.0,0.2); }
00186  void ErrorHandler::turnLEDGreen() { this->setRGBLED(120.0,1.0,0.2); }
00187  void ErrorHandler::turnLEDBlue() { this->setRGBLED(200.0,1.0,0.2); }
00188  void ErrorHandler::turnLEDPurple() { this->setRGBLED(261.9,1.0,0.2); }
00189  void ErrorHandler::turnLEDOrange() { this->setRGBLED(51.0,1.0,0.2); }
00190  void ErrorHandler::turnLEDBlack() { this->setRGBLED(0,0,0); }
00191  void ErrorHandler::turnLEDYellow() { this->setRGBLED(60.0,1.0,0.133); }
00192  
00193  void ErrorHandler::blinkTransportTxLED() { this->blinkLED(led1); }
00194  void ErrorHandler::blinkTransportRxLED() { this->blinkLED(led2); }
00195 
00196  // blink an LED
00197  void ErrorHandler::blinkLED(DigitalOut led) {
00198     led = 1;
00199     wait_ms(BLINK_TIME);
00200     led = 0;
00201  }
00202