edge_mgr.cpp

00001 #include "mbed.h"
00002 #include "edge_mgr.h"
00003 #include "af_attributes.h"
00004 
00005 #include "edge_time.h"
00006 #include "edge_pin.h"
00007 #include "MMA8451Q.h"
00008 #include "VEML6040.h"
00009 #include "LM75B.h"
00010 #include "SMTC502AT.h"
00011 #include "PSE530.h"
00012 #include <ILI9341.h>
00013 #include "Arial12x12.h"
00014 #include "Arial24x23.h"
00015 #include "Arial28x28.h"
00016 
00017 #include "edge_sensor.h"
00018 #include "edge_accel.h"
00019 #include "edge_color.h"
00020 #include "edge_temp.h"
00021 #include "edge_pressure.h"
00022 #include "edge_reset_mgr.h"
00023 #include "edge_chart.h"
00024 
00025 #define MMA8451Q_I2C_ADDRESS 0x1C
00026 #define VEML6040_I2C_ADDRESS 0x10
00027 #define LM75B_I2C_ADDRESS    0x48
00028 #define SO1602A_I2C_ADDRESS  0x3C
00029 
00030 #define NUM_MAX_SENSOR 5
00031 
00032 uint16_t attr_to_set[] = {
00033 ATTR_ACCEL_PRESENT,
00034 ATTR_COLOR0_PRESENT,
00035 ATTR_COLOR1_PRESENT,
00036 ATTR_TEMP0_PRESENT,
00037 ATTR_GAS_PRESENT,
00038 } ;
00039 
00040 uint16_t attr_to_get[] = {
00041 // accel
00042 ATTR_ACCEL_ENABLE,
00043 ATTR_ACCEL_INTERVAL,
00044 // Color0
00045 ATTR_COLOR0_ENABLE,
00046 ATTR_COLOR0_INTERVAL,
00047 ATTR_COLOR0_ITIME,
00048 ATTR_COLOR0_PWM_PERIOD,
00049 ATTR_COLOR0_PWM_TARGET,
00050 ATTR_COLOR0_PWM_R,
00051 ATTR_COLOR0_PWM_G,
00052 ATTR_COLOR0_PWM_B,
00053 // Color1
00054 ATTR_COLOR1_ENABLE,
00055 ATTR_COLOR1_INTERVAL,
00056 ATTR_COLOR1_ITIME,
00057 ATTR_COLOR1_PWM_PERIOD,
00058 ATTR_COLOR1_PWM_TARGET,
00059 ATTR_COLOR1_PWM_R,
00060 ATTR_COLOR1_PWM_G,
00061 ATTR_COLOR1_PWM_B,
00062 // Temp
00063 ATTR_TEMP0_INTERVAL,
00064 ATTR_TEMP0_ENABLE,
00065 // Gas Pressure
00066 ATTR_GAS_ENABLE,
00067 ATTR_GAS_INTERVAL,
00068 ATTR_GAS_THR_MODE,
00069 ATTR_GAS_THR_HIGH,
00070 ATTR_GAS_THR_LOW,
00071 0 } ;
00072 
00073 bool            verbos = true ;
00074 edge_sensor     *sensor[NUM_MAX_SENSOR] ;
00075 int             num_sensor   = 0 ;
00076 
00077 edge_accel      *accel       = 0 ;
00078 edge_color      *color[2]    = {0, 0} ;
00079 edge_temp       *temp        = 0 ;
00080 edge_pressure   *pressure    = 0 ;
00081 
00082 PwmOut          *led[3]      = {0, 0, 0} ;
00083 uint16_t        pwm[3]       = { 0x5FA2, 0xB09B, 0x83DF } ;
00084 I2C             *edge_i2c0   = 0 ;
00085 I2C             *edge_i2c1   = 0 ;
00086 ILI9341         *display     = 0 ;
00087 MMA8451Q        *mma8451q    = 0 ;
00088 VEML6040        *veml6040[2] = { 0, 0 } ;
00089 LM75B           *lm75b0      = 0 ; /* for temp1 */
00090 AnalogIn        *an0         = 0 ; /* for temp2 */
00091 SMTC502AT       *smtc502at0  = 0 ;
00092 AnalogIn        *an1         = 0 ; /* for temp3 */
00093 SMTC502AT       *smtc502at1  = 0 ;
00094 LM75B           *lm75b1      = 0 ; /* for temp4 */
00095 AnalogIn        *an2         = 0 ; /* for gas pressure */
00096 PSE530          *pse530      = 0 ; /* gas pressure sensor */
00097 
00098 DigitalOut      *tft_reset   = 0 ;
00099 DigitalOut      *tft_backlight = 0 ;
00100 DigitalOut      *tft_cs       = 0 ;
00101 DigitalOut      *pse530_en    = 0 ;
00102 DigitalOut      *color_en     = 0 ;
00103 
00104 static int error_tolerance   = 100 ;
00105 static int loop_interval     = 100 ; // 1000 ; 
00106 static int accel_interval    = 10 ;
00107 int        edge_mgr_status   = EDGE_MGR_INIT ;
00108 char       *reset_reason_str = 0 ;
00109 int        display_mode      = 1 ;
00110 bool       reboot_requested  = false ;
00111 
00112 /* following two functions are for test power on/off of color sensor */
00113 void   enable_color_sensor(void) 
00114 {
00115     if (color_en == 0) {
00116         printf("Color Enable Pin is not initiated\n") ;
00117     } else {
00118         *color_en = 0 ;
00119     }
00120 }   
00121     
00122 void   disable_color_sensor(void) 
00123 {
00124     if (color_en == 0) {
00125         printf("Color Enable Pin is not initiated\n") ;
00126     } else {
00127         *color_en = 1 ;
00128     }    
00129 }
00130 
00131 void init_display(void)
00132 {
00133 reset_watch_dog() ;
00134     printf("TFT Initializing\n") ;
00135     tft_reset = new DigitalOut(PIN_RESET_TFT, 1) ;
00136     tft_backlight = new DigitalOut(PIN_BL_TFT, 0) ;
00137     tft_cs = new DigitalOut(PIN_CS_TFT, 1) ;
00138 
00139 reset_watch_dog() ;
00140     display = new ILI9341(SPI_8, 10000000,
00141                 PIN_MOSI, PIN_MISO, PIN_SCK,
00142                 PIN_CS_TFT, PIN_RESET_TFT, PIN_DC_TFT, "LaSuno") ;
00143 
00144 reset_watch_dog() ;
00145     display->BusEnable(true) ;
00146     display->set_orientation(1) ;
00147     
00148 reset_watch_dog() ;
00149     display->cls() ;
00150     *tft_backlight = 1 ;
00151     display->BusEnable(false) ;
00152     printf("TFT Initialized\n") ;
00153 }
00154 
00155 void   edge_splash(void) 
00156 {
00157     printf("Sensor loop started!\n") ;
00158     if (display) {
00159         reset_watch_dog() ;
00160         display->BusEnable(true) ;
00161         display->cls() ;
00162         display->foreground(Green) ;
00163         display->locate(40, 20) ;
00164         display->printf("Sensor Loop") ;
00165         display->locate(40, 60) ;
00166         display->printf("  Started!") ;
00167         display->BusEnable(false) ;
00168         reset_watch_dog() ;
00169     }
00170 }
00171 
00172 int init_edge_attribute(void)
00173 {
00174     static int sensor_index = 0 ;
00175     static int attr_index = 0 ;
00176     static int error_count = 0 ;
00177     int return_value = 1 ;
00178     int result ;
00179     
00180     reset_watch_dog() ;
00181     
00182     if (reset_reason_str) { 
00183         result = afero->setAttribute(ATTR_MCU_RESET_REASON, reset_reason_str) ;
00184         if (result == afSUCCESS) {
00185             error_count = 0 ;
00186             reset_reason_str = 0 ;
00187         } else {
00188             error_count++ ;
00189         }
00190         reset_watch_dog() ;
00191     }
00192     if (sensor_index < NUM_MAX_SENSOR) {// for each sensor send presence
00193 // printf("Setting sensor[%d] presence\n", sensor_index) ;
00194         if (sensor_index == 3) { /* for temp lm75b0 is used */
00195             result = afero->setAttributeBool(attr_to_set[sensor_index], lm75b0) ;
00196         } else {
00197             result = afero->setAttributeBool(attr_to_set[sensor_index], sensor[sensor_index]) ;
00198         }
00199         if (result == afSUCCESS) {
00200             error_count = 0 ;
00201             sensor_index++ ;
00202         } else {
00203             error_count++ ;
00204         }
00205         reset_watch_dog() ;
00206     } else { // all sensor presence sent, now get attributes
00207         if (attr_to_get[attr_index] != 0) {
00208 // printf("getting attribute [%d]\n", attr_index) ;
00209             result = afero->getAttribute(attr_to_get[attr_index]) ;
00210             if (result == afSUCCESS) {
00211                 error_count = 0 ;
00212                 attr_index++ ;
00213             } else {
00214                 error_count++ ;
00215             }
00216         }
00217         reset_watch_dog() ;
00218     }
00219     
00220     if (error_count > error_tolerance) { // too many fails, trying reset
00221         reset_watch_dog() ;
00222         reboot_edge() ;
00223     }
00224  
00225     if ((sensor_index >= NUM_MAX_SENSOR)&&(attr_to_get[attr_index] == 0)) { /* all sensors attributes done */
00226         sensor_index = 0 ;
00227         attr_index = 0 ;
00228         return_value = 0 ;
00229     }
00230     return(return_value) ;
00231 }
00232 
00233 void edge_loop(uint32_t count_robin)
00234 {
00235     static int sensor_index = 0 ;
00236     int result ;
00237     
00238     reset_watch_dog() ;
00239     
00240     if ((count_robin % accel_interval) == 0) {
00241         if (accel) {
00242             accel->accum() ; /* get and accum accel data */
00243         }
00244         reset_watch_dog() ;
00245     }
00246 
00247     if ((count_robin % loop_interval) == 0) {
00248         reset_watch_dog() ;
00249         loop_interval = 10 ;
00250         if ((sensor[sensor_index])&&(sensor[sensor_index]->isEnabled())) {
00251             switch(sensor_index) {
00252             case SENSOR_ID_COLOR1: /* color0 */
00253                 if (((edge_color*)sensor[sensor_index])->calibration_requested()) {
00254                     ((edge_color*)sensor[sensor_index])->calibrate(color0_target, color0_pwm, 10) ;
00255                     reset_watch_dog() ;
00256                     while((result = afero->setAttribute32(ATTR_COLOR0_PWM_R, color0_pwm[0])) != afSUCCESS) { 
00257                         reset_watch_dog() ;
00258                         print_af_error(result) ;
00259                         wait_ms(10) ;
00260                     }
00261                     while((result = afero->setAttribute32(ATTR_COLOR0_PWM_G, color0_pwm[1])) != afSUCCESS) {
00262                         reset_watch_dog() ;
00263                         print_af_error(result) ;
00264                         wait_ms(10) ;
00265                     } 
00266                     while((result = afero->setAttribute32(ATTR_COLOR0_PWM_B, color0_pwm[2])) != afSUCCESS) {
00267                         reset_watch_dog() ;
00268                         print_af_error(result) ;
00269                         wait_ms(10) ;
00270                     }
00271                     while((afero->setAttributeBool(ATTR_COLOR0_CALIBRATE, false)) != afSUCCESS) {
00272                         reset_watch_dog() ;     
00273                         print_af_error(result) ;
00274                         wait_ms(10) ;
00275                     }
00276                 } else { 
00277                     sensor[sensor_index]->runStateMachine() ;
00278                 }
00279                 break ;
00280             case SENSOR_ID_COLOR2: /* color1 */
00281                 if (((edge_color*)sensor[sensor_index])->calibration_requested()) {
00282                     ((edge_color*)sensor[sensor_index])->calibrate(color1_target, color1_pwm, 10) ;
00283                     reset_watch_dog() ;
00284                     if ((result = afero->setAttribute32(ATTR_COLOR1_PWM_R, color1_pwm[0])) != afSUCCESS) {
00285                         reset_watch_dog() ;
00286                         print_af_error(result) ;
00287                         wait_ms(10) ;
00288                     }
00289                     if ((result = afero->setAttribute32(ATTR_COLOR1_PWM_G, color1_pwm[1])) != afSUCCESS) {
00290                         reset_watch_dog() ;
00291                         print_af_error(result) ;
00292                         wait_ms(10) ;
00293                     }
00294                     reset_watch_dog() ;
00295                     if ((result = afero->setAttribute32(ATTR_COLOR1_PWM_B, color1_pwm[2])) != afSUCCESS) {
00296                         reset_watch_dog() ;       
00297                         print_af_error(result) ;
00298                         wait_ms(10) ;
00299                     }
00300                     while((afero->setAttributeBool(ATTR_COLOR1_CALIBRATE, false)) != afSUCCESS) {
00301                         reset_watch_dog() ;     
00302                         print_af_error(result) ;
00303                         wait_ms(10) ;
00304                     }
00305                 } else { 
00306                     sensor[sensor_index]->runStateMachine() ;
00307                 }
00308                 break ;
00309             default:
00310                 sensor[sensor_index]->runStateMachine() ;
00311                 break ;
00312             }
00313         }
00314         sensor_index = (sensor_index + 1) % NUM_MAX_SENSOR ;
00315     }
00316     reset_watch_dog() ;
00317 }
00318 
00319 int is_present(I2C *i2c, int address)
00320 {
00321     char t[1] = { 0 } ;
00322     char data[2] = { 0, 0 } ;
00323     int result ;
00324     address <<= 1 ;
00325     result = i2c->write(address, t, 1, true) ;
00326     if (result == 0) {
00327         result = i2c->read(address, data, 2) ;
00328     }
00329     return((result == 0)) ;    
00330 }
00331 
00332 /**
00333  * check_i2c_pins
00334  * To avoid I2C dead-lock condition,
00335  * check status of SDA and SCL.
00336  * As they are supposed to be HIGH
00337  * in case one of them is/are LOW,
00338  * change SCL pin to a digital out pin and
00339  * generate forced clock for a several cycles.
00340  * and when SDA come back to High returns
00341  * or I2C_UNLOCK_TRIAL_CYCLE exceeds, give up.
00342  */ 
00343 #define I2C_UNLOCK_TRIAL_CYCLE 50
00344 
00345 void check_i2c_pins(PinName sda_pin, PinName scl_pin, int number)
00346 {
00347     DigitalIn *sda_in = 0 ; 
00348     DigitalIn *scl_in = 0 ;
00349     DigitalOut *scl_out = 0 ;
00350     int count = 0 ;
00351     sda_in = new DigitalIn(sda_pin, PullUp) ;
00352     scl_in = new DigitalIn(scl_pin, PullUp) ;
00353     printf("I2C%d pin ", number) ;
00354     if ((*sda_in == 0) || (*scl_in == 0)) { /* bus hang! */
00355         printf("hang detected, trying to clear ... ") ;
00356         delete scl_in ;
00357         scl_in = 0 ;
00358         scl_out = new DigitalOut(scl_pin) ;
00359         while((*sda_in == 0)&&(count++ > I2C_UNLOCK_TRIAL_CYCLE)) {
00360             *scl_out = 0 ;
00361             wait(0.01) ;
00362             *scl_out = 1 ;
00363             wait(0.01) ;
00364         }
00365         if (*sda_in != 0) {
00366             printf("Cleared!\n") ;
00367         } else {
00368             printf("Failed to Clear, proceeding\n") ;
00369         }
00370     } else {
00371         printf("condition OK\n") ;
00372     }
00373     if (sda_in) { delete sda_in ; }
00374     if (scl_in) { delete scl_in ; }
00375     if (scl_out) { delete scl_out ; }
00376 }
00377 
00378 void init_sensors(void)
00379 {
00380     printf("=== Initializing Sensor(s) ===\n") ;    
00381 #if 1 
00382     color_en = new DigitalOut(PIN_COLOR_EN, 0) ;
00383     *color_en = 0 ; /* enable */
00384     *color_en = 1 ; /* disable */
00385     wait_ms(100) ;
00386     *color_en = 0 ; /* enable */
00387     wait_ms(10) ;
00388 #endif
00389     
00390     check_i2c_pins(PIN_I2C0_SDA, PIN_I2C0_SCL, 0) ;
00391     edge_i2c0 = new I2C(PIN_I2C0_SDA, PIN_I2C0_SCL) ;
00392     
00393     check_i2c_pins(PIN_I2C1_SDA, PIN_I2C1_SCL, 1) ;
00394     edge_i2c1 = new I2C(PIN_I2C1_SDA, PIN_I2C1_SCL) ;
00395                 
00396     if (display) {
00397 reset_watch_dog() ;
00398 printf("printing inital string to TFT\n") ;
00399         display->BusEnable(true) ;
00400 
00401 
00402     display->background(Black) ;
00403     display->foreground(White) ;
00404 reset_watch_dog() ;
00405     display->cls() ;
00406 reset_watch_dog() ;
00407         display->set_font((unsigned char*) Arial24x23);
00408         display->foreground(Green) ;
00409         display->locate(70, 5) ;
00410         display->printf("Suntory") ;
00411         display->locate(30, 30) ;
00412         display->printf("Server Monitor") ;
00413         display->set_font((unsigned char*) Arial28x28);
00414         display->foreground(White) ;
00415         display->locate(30, 60) ;
00416         display->printf("La Suno") ;
00417         display->locate(30, 100) ;
00418         display->foreground(Red) ;
00419         display->printf("Preparing...") ;
00420     display->BusEnable(true) ;
00421     printf("Done\n") ;
00422     wait(0.1) ;
00423 reset_watch_dog() ;
00424     display->cls() ;
00425     display->foreground(Yellow) ;
00426     display->locate(40, 5) ;
00427     display->printf("Probing sensors...") ;
00428     display->foreground(Green) ;
00429     display->BusEnable(false) ;
00430     }
00431 reset_watch_dog() ;
00432     if (is_present(edge_i2c1, MMA8451Q_I2C_ADDRESS)) {
00433         printf("MMA8451Q on I2C1 is present\n") ;
00434         if (display) {
00435             display->BusEnable(true) ;
00436             display->locate(30, num_sensor * 30 + 40) ;
00437             display->printf("ACCEL is present") ;
00438             display->BusEnable(false) ;
00439         }
00440         mma8451q = new MMA8451Q(edge_i2c1, MMA8451Q_I2C_ADDRESS) ;
00441         accel    = new edge_accel(mma8451q) ;
00442         sensor[SENSOR_ID_ACCEL] = accel ;
00443         sensor[SENSOR_ID_ACCEL]->setId(SENSOR_ID_ACCEL) ;
00444         num_sensor++ ;
00445     } else {
00446         sensor[SENSOR_ID_ACCEL] = 0 ;
00447         printf("MMA8451Q is absent\n") ;
00448     }
00449 reset_watch_dog() ;   
00450     if (is_present(edge_i2c1, VEML6040_I2C_ADDRESS)) {
00451         printf("VEML6040 on I2C1 is present\n") ;  
00452         if (display) {
00453             display->BusEnable(true) ;
00454             display->locate(30, num_sensor * 30 + 40) ;
00455             display->printf("COLOR1 is present") ;
00456             display->BusEnable(false) ;
00457         }  
00458         veml6040[0] = new VEML6040(edge_i2c1, VEML6040_I2C_ADDRESS) ;
00459         led[0] = new PwmOut(PIN_LED_R) ; 
00460         led[1] = new PwmOut(PIN_LED_G) ; 
00461         led[2] = new PwmOut(PIN_LED_B) ; 
00462         color[0] = new edge_color(veml6040[0], led, pwm) ;
00463         sensor[SENSOR_ID_COLOR1] = color[0] ;
00464         sensor[SENSOR_ID_COLOR1]->setId(SENSOR_ID_COLOR1) ;
00465         num_sensor++ ;
00466     } else {
00467         sensor[SENSOR_ID_COLOR1] = 0 ;
00468         printf("VEML6040 on I2C1 is absent\n") ;
00469     }
00470 reset_watch_dog() ;    
00471     if (is_present(edge_i2c0, VEML6040_I2C_ADDRESS)) {
00472         printf("VEML6040 on I2C0 is present\n") ;  
00473         if (display) {
00474             display->BusEnable(true) ;
00475             display->locate(30, num_sensor * 30 + 40) ;
00476             display->printf("COLOR2 is present") ;
00477             display->BusEnable(false) ;
00478         }   
00479         veml6040[1] = new VEML6040(edge_i2c0, VEML6040_I2C_ADDRESS) ;
00480         if (led[0] == 0) {
00481             led[0] = new PwmOut(PIN_LED_R) ;
00482             led[1] = new PwmOut(PIN_LED_G) ;
00483             led[2] = new PwmOut(PIN_LED_B) ;
00484         }
00485         color[1] = new edge_color(veml6040[1], led, pwm) ;
00486         sensor[SENSOR_ID_COLOR2] = color[1] ;
00487         sensor[SENSOR_ID_COLOR2]->setId(SENSOR_ID_COLOR2) ;
00488         num_sensor++ ;
00489     } else {
00490         sensor[SENSOR_ID_COLOR2] = 0 ;
00491         printf("VEML6040 on I2C0 is absent\n") ;
00492     }
00493 reset_watch_dog() ;    
00494     if (is_present(edge_i2c1, LM75B_I2C_ADDRESS)) {
00495         printf("LM75B on I2C1 is present\n") ;
00496         if (display) {
00497             display->BusEnable(true) ;
00498             display->locate(30, num_sensor * 30 + 40) ;
00499             display->printf("TEMP1 is present") ;
00500             display->BusEnable(false) ;
00501         }   
00502         lm75b0 = new LM75B(edge_i2c1, LM75B_I2C_ADDRESS) ;
00503     } else {
00504         printf("LM75B on I2C1 is absent\n") ;
00505     }
00506 #if 0    
00507     if (is_present(edge_i2c0, LM75B_I2C_ADDRESS)) {
00508         printf("LM75B on I2C0 is present\n") ;
00509         lm75b1 = new LM75B(edge_i2c0, LM75B_I2C_ADDRESS) ;
00510     } else {
00511         printf("LM75B on I2C0 is absent\n") ;
00512     }
00513 #endif  
00514      if (display) { /* press is present anyway */
00515         display->BusEnable(true) ;
00516         if (lm75b0) {
00517             display->locate(30, (num_sensor+1) * 30 + 40) ;
00518         } else {
00519             display->locate(30, num_sensor * 30 + 40) ;
00520         }
00521         display->printf("PRESS is present") ;
00522         display->BusEnable(false) ;
00523     }  
00524 reset_watch_dog() ;    
00525     an0        = new AnalogIn(PIN_AN0) ;
00526     smtc502at0 = new SMTC502AT(an0) ;
00527     an1        = new AnalogIn(PIN_AN1) ;
00528     smtc502at1 = new SMTC502AT(an1) ;
00529     temp       = new edge_temp(lm75b0, smtc502at0, smtc502at1, lm75b1) ;
00530     sensor[SENSOR_ID_TEMP]  = temp ;
00531     sensor[SENSOR_ID_TEMP]->setId(SENSOR_ID_TEMP) ;
00532     num_sensor++ ;
00533     
00534 
00535 reset_watch_dog() ;    
00536     an2        = new AnalogIn(PIN_AN2) ;
00537     pse530_en  = new DigitalOut(PIN_PRESS_EN, 0) ;
00538     pse530     = new PSE530(an2) ;
00539     pressure   = new edge_pressure(pse530, pse530_en) ;
00540     sensor[SENSOR_ID_PRESS]  = pressure ;
00541     sensor[SENSOR_ID_PRESS]->setId(SENSOR_ID_PRESS) ;
00542     num_sensor++ ;
00543  
00544 reset_watch_dog() ;
00545     if (num_sensor > 0) {
00546         printf("%d edge_sensor(s) registered\n", num_sensor) ;
00547         printf("Edge is waiting for ASR to link\n") ;
00548         if (display) {
00549             display->BusEnable(true) ;
00550             display->foreground(White) ;
00551             display->locate(40, 200) ;
00552             display->printf("Waiting for ASR") ;
00553             display->BusEnable(false) ;
00554         }
00555     }
00556 reset_watch_dog() ;
00557 }
00558 
00559 void enable_sensors(void) 
00560 {
00561     int i ;
00562     for (i = 0 ; i < NUM_MAX_SENSOR ; i++ ) {
00563         if (sensor[i]) {
00564             sensor[i]->enable() ;
00565         }
00566     }
00567 }
00568 
00569 void disable_sensors(void)
00570 {
00571     int i ;
00572     for (i = 0 ; i < NUM_MAX_SENSOR ; i++ ) {
00573         if (sensor[i]) {
00574             sensor[i]->disable() ;
00575         }
00576     }
00577 }
00578 
00579 void   reboot_edge(void) 
00580 {
00581     int i ;
00582     reset_watch_dog() ;
00583     disable_sensors() ;
00584     reset_watch_dog() ;
00585     if (display) {
00586         delete display ;
00587         display = 0 ;
00588     }
00589     for (i = 0 ; i < NUM_MAX_SENSOR ; i++ ) {
00590         if (sensor[i]) {
00591             reset_watch_dog() ;
00592             delete sensor[i] ;
00593             sensor[i] = 0 ;
00594         }
00595     }
00596     reset_watch_dog() ;
00597     software_reset() ;
00598 }