00001 #include "tibi_dabo_battery_monitor_driver.h" 00002 #include "exceptions.h" 00003 00004 int TibiDaboBatteryMonitorDriver::warning_threshold=15; 00005 int TibiDaboBatteryMonitorDriver::alarm_threshold=5; 00006 00007 TibiDaboBatteryMonitorDriver::TibiDaboBatteryMonitorDriver(void) 00008 { 00009 //setDriverId(driver string id); 00010 this->battery=NULL; 00011 this->serial_num=""; 00012 this->warning_time_to_discharge="time_to_discharge_warning"; 00013 this->alarm_time_to_discharge="time_to_discharge_alarm"; 00014 } 00015 00016 bool TibiDaboBatteryMonitorDriver::openDriver(void) 00017 { 00018 //setDriverId(driver string id); 00019 if(this->serial_num!="") 00020 { 00021 try{ 00022 this->battery=new CSegway_Battery(this->serial_num); 00023 // add the default alarms 00024 this->warning_time_to_discharge_event_id=this->battery->create_alarm(warning_time_to_discharge,this->warning_condition); 00025 this->alarm_time_to_discharge_event_id=this->battery->create_alarm(alarm_time_to_discharge,this->alarm_condition); 00026 return true; 00027 }catch(CException &e){ 00028 ROS_INFO("[tibi_dabo_battery_monitor] - %s",e.what().c_str()); 00029 return false; 00030 } 00031 } 00032 else 00033 return false; 00034 } 00035 00036 bool TibiDaboBatteryMonitorDriver::closeDriver(void) 00037 { 00038 if(this->battery!=NULL) 00039 { 00040 delete this->battery; 00041 this->battery=NULL; 00042 } 00043 00044 return true; 00045 } 00046 00047 bool TibiDaboBatteryMonitorDriver::startDriver(void) 00048 { 00049 return true; 00050 } 00051 00052 bool TibiDaboBatteryMonitorDriver::stopDriver(void) 00053 { 00054 return true; 00055 } 00056 00057 void TibiDaboBatteryMonitorDriver::config_update(Config& new_cfg, uint32_t level) 00058 { 00059 this->lock(); 00060 00061 // depending on current state 00062 // update driver with new_cfg data 00063 switch(this->getState()) 00064 { 00065 case TibiDaboBatteryMonitorDriver::CLOSED: 00066 this->serial_num=new_cfg.serial_num; 00067 break; 00068 00069 case TibiDaboBatteryMonitorDriver::OPENED: 00070 break; 00071 00072 case TibiDaboBatteryMonitorDriver::RUNNING: 00073 this->warning_threshold=new_cfg.warning_threshold; 00074 this->alarm_threshold=new_cfg.alarm_threshold; 00075 break; 00076 } 00077 00078 // save the current configuration 00079 this->config_=new_cfg; 00080 00081 this->unlock(); 00082 } 00083 00084 bool TibiDaboBatteryMonitorDriver::check_alarm(void) 00085 { 00086 CEventServer *event_server=CEventServer::instance(); 00087 00088 if(event_server->event_is_set(this->alarm_time_to_discharge_event_id)) 00089 { 00090 event_server->reset_event(this->alarm_time_to_discharge_event_id); 00091 return true; 00092 } 00093 else 00094 return false; 00095 } 00096 00097 bool TibiDaboBatteryMonitorDriver::check_warning(void) 00098 { 00099 CEventServer *event_server=CEventServer::instance(); 00100 00101 if(event_server->event_is_set(this->warning_time_to_discharge_event_id)) 00102 { 00103 event_server->reset_event(this->warning_time_to_discharge_event_id); 00104 return true; 00105 } 00106 else 00107 return false; 00108 } 00109 00110 void TibiDaboBatteryMonitorDriver::get_battery_info(TBatteryInfo *info) 00111 { 00112 this->battery->get_battery_info(info); 00113 } 00114 00115 bool TibiDaboBatteryMonitorDriver::warning_condition(TBatteryInfo *info) 00116 { 00117 if(info->time_to_discharged<TibiDaboBatteryMonitorDriver::warning_threshold) 00118 return true; 00119 else 00120 return false; 00121 } 00122 00123 bool TibiDaboBatteryMonitorDriver::alarm_condition(TBatteryInfo *info) 00124 { 00125 if(info->time_to_discharged<TibiDaboBatteryMonitorDriver::alarm_threshold) 00126 return true; 00127 else 00128 return false; 00129 } 00130 00131 TibiDaboBatteryMonitorDriver::~TibiDaboBatteryMonitorDriver(void) 00132 { 00133 }