#include #include "LayeredControlIF.h" #include "SerialDevice.h" #include "BluefinBatt.h" #define BAUD 9600 #define PARITY "NONE" #define STOP_BITS 1 #define DATA_BITS 8 #define READ_TIMEOUT 2500 #define ADDR_QUERY "!?\xd\xa" #define GetVoltageCmd "$4RD\xd\xa" #define GetCurrentCmd "$2RD\xd\xa" #define WatchdogDisable "wf" #define BLUEFINBATT_PERIOD 10000 BluefinBatt::BluefinBatt(SerialDevice *device, int numBatts, float abortVoltage, float terminateVoltage) : _device(device), _numBatts(numBatts), _abortVoltage(abortVoltage), _terminateVoltage(terminateVoltage), _depthSensor(0), _depth(0.), PeriodicTask("BluefinBatt") { if (init() != DeviceIF::Ok) { Syslog::write("BluefinBatt: init failed!\n"); throw Exception("BluefinBatt -- init failed!\n"); } addPeriodicCallback(BLUEFINBATT_PERIOD, (CallbackMethod)BluefinBatt::sample); _log = new BluefinBattLog(this, DataLog::BinaryFormat, _numBatts); } BluefinBatt::~BluefinBatt() { delete _output; } DeviceIF::Status BluefinBatt::init() { char buf[30]; Syslog::write("BluefineBatt::init() - %d batteries on %d second interval", _numBatts, BLUEFINBATT_PERIOD/1000); _output = new BluefinBattOutput(_numBatts,SharedData::ReadWrite); for (int i = 0; i < _numBatts; i++) { _output->data[i].state = BluefinBattOutput::Unknown; _output->data[i].fault = BluefinBattOutput::Clear; _output->data[i].voltage = 0.; _output->data[i].current = 0.; _output->data[i].temp = 0.; _output->data[i].minvoltage = 0.; _output->data[i].maxvoltage = 0.; _output->data[i].waterleak = 0; _output->data[i].capacity = 0.; _output->data[i].sampleTime.tv_sec = 0; _output->data[i].sampleTime.tv_nsec = 0; } try { _output->write(); } catch(SharedData::AccessError error) { fprintf(stderr,"%s", error.msg); return DeviceIF::Error; } Syslog::write("BluefinBatt.cc - Looking for Parosci server"); try { _depthSensor = new DepthSensorIF("Parosci", 20); // Try for 20 seconds } catch (...) { Syslog::write("BluefinBatt.cc - Parosci not found: Logged depth data will not be valid!"); _depthSensor = NULL; } _device->commsDebugMode(SerialDevice::DebugOff); // _device->printTermios(); // initialize serial port _device->setLineFormat(BAUD, DATA_BITS, STOP_BITS, PARITY); // clear serial port _device->clearPort(); _device->flushReceiveBuf(); char reply[256]; _device->write(ADDR_QUERY, 4); tcdrain(_device->getFd()); Syslog::write("BluefinBatt::init() - querying for batt ids"); try { // First read the echoed command // // Batteries do not echo the "!?" command, so skipping this section // _device->readUntil(reply, sizeof(reply), "\xa", READ_TIMEOUT); // //write echo to log Syslog::write("BluefinBatt::init() - battery echo is %s\n", reply); // // Now read the battery addresses // int nchars = _device->readNChars(reply, 256, 5000); Syslog::write("BluefinBatt::init() - battery ids are %s ", reply); } catch (Exception e) { Syslog::write("BluefinBatt::init() - caught read() exception: %s (%s)\n", e.msg, reply); return DeviceIF::Error; } int nb = 0; char *tok = strtok(reply, " "); while (tok != NULL) { unsigned int addr; if (sscanf(tok, "%x", &addr) != 1) break; _output->data[nb].battSN = addr; Syslog::write("BluefinBatt::init() - found battery %x", addr); nb++; tok = strtok(NULL, " "); } if (nb != _numBatts) { Syslog::write("BluefinBatt::init() - number of batteries found(%d) does not match config(%d)", nb, _numBatts); return DeviceIF::Error; } //disable watchdogs for (i = 0; i < _numBatts; i++) { sprintf(buf,"#%xwf\xd\xa",_output->data[i].battSN); printf("wrote batt string %s", buf); _device->write(buf,strlen(buf));tcdrain(_device->getFd()); _device->readUntil(reply, sizeof(reply), "\xa", READ_TIMEOUT); //write echo to log Syslog::write("BluefinBatt::init() - battery echo is %s\n", reply); int nchars = _device->readNChars(reply, 256, 5000); reply[nchars-1] = 0x0; Syslog::write("BluefinBatt::init() - battery watchdog state is %s ", reply); } _aborted = False; return DeviceIF::Ok; } void BluefinBatt::sample(void) { char cmd[16], reply[100]; int numBattsOnline = 0; float minCellVoltage = 1.0e6; float sumMinCellVoltage = 0.0; float meanMinCellVoltage = 0.0; TimeIF::TimeSpec depthTime; double currentDepth; if (_depthSensor != NULL) { if (DeviceIF::Ok == _depthSensor->depth(¤tDepth, &depthTime)) _depth = currentDepth; } for (int i = 0; i < _numBatts; i++) { sprintf(cmd, "#%xq0\n", _output->data[i].battSN); _device->write(cmd, strlen(cmd));tcdrain(_device->getFd()); try { // if (_device->confirm(cmd, READ_TIMEOUT) != DeviceIF::Ok) // { // Syslog::write("BluefinBatt::sample() - query not confirmed (%s)", // _output->data[i].battSN, reply); // } // Now read the battery info // int hr, min, sec; _device->readUntil(reply, sizeof(reply), "$", READ_TIMEOUT); reply[0] = '$'; int nchars = _device->readNChars(reply+1, 100, 1000); int nmatch = sscanf(reply+9, "%f %f %f %f %f %d %f", &_output->data[i].voltage, &_output->data[i].current, &_output->data[i].temp, &_output->data[i].minvoltage, &_output->data[i].maxvoltage, &_output->data[i].waterleak, &_output->data[i].capacity); if (nmatch != 7) // Bad parse { Syslog::write("BluefinBatt::sample() - Only parsed %d from reply %s", nmatch, reply); return; } /*Syslog::write("Bluefin::sample() Parsed voltage %f from reply", _output->data[i].voltage);*/ Syslog::write("BluefinBatt Reply: %s", reply); } catch (Exception e) { Syslog::write("BluefinBatt::sample()-caught read() exception: %s (%s)\n", e.msg, reply); return; } //parse out battery state switch(reply[6]) { case 'f' : _output->data[i].state = BluefinBattOutput::Shutdown; break; case 'd' : _output->data[i].state = BluefinBattOutput::Discharging; break; case 'c' : _output->data[i].state = BluefinBattOutput::Charging; break; case 'b' : _output->data[i].state = BluefinBattOutput::Balancing; break; default : _output->data[i].state = BluefinBattOutput::Unknown; } switch(reply[7]) { case '-' : _output->data[i].fault = BluefinBattOutput::Clear; break; case 'V' : _output->data[i].fault = BluefinBattOutput::OverVolt; break; case 'v' : _output->data[i].fault = BluefinBattOutput::UnderVolt; break; case 'I' : _output->data[i].fault = BluefinBattOutput::OverCurrent; break; case 'C' : _output->data[i].fault = BluefinBattOutput::MaxCellOverVolt; break; case 'c' : _output->data[i].fault = BluefinBattOutput::MinCellUnderVolt; break; case 'T' : _output->data[i].fault = BluefinBattOutput::OverTemp; break; case 'W' : _output->data[i].fault = BluefinBattOutput::WaterLeak; break; case 'H' : _output->data[i].fault = BluefinBattOutput::HardwareProt; break; case 'h' : _output->data[i].fault = BluefinBattOutput::HardwareTransient; break; case 'm' : _output->data[i].fault = BluefinBattOutput::WatchdogTimeout; break; default : _output->data[i].fault = BluefinBattOutput::Clear; } if (_output->data[i].state == BluefinBattOutput::Discharging && _output->data[i].fault == BluefinBattOutput::Clear) { if (_output->data[i].minvoltage < minCellVoltage) { minCellVoltage = _output->data[i].minvoltage; } numBattsOnline++; sumMinCellVoltage += _output->data[i].minvoltage; meanMinCellVoltage = sumMinCellVoltage / numBattsOnline; } if (_output->data[i].state == BluefinBattOutput::Shutdown) { //reset battery Syslog::write("BluefinBatt::Reseting battery %x\n", _output->data[i].battSN); sprintf(cmd, "#%xbf\n", _output->data[i].battSN); _device->write(cmd, strlen(cmd));tcdrain(_device->getFd()); _device->readUntil(reply, sizeof(reply), "\n", READ_TIMEOUT); //echo _device->readNChars(reply, sizeof(reply), 5000); //reply sprintf(cmd, "#%xbd\n", _output->data[i].battSN); _device->write(cmd, strlen(cmd)); tcdrain(_device->getFd()); _device->readUntil(reply, sizeof(reply), "\n", READ_TIMEOUT); //echo _device->readNChars(reply, sizeof(reply), 5000); //reply sprintf(cmd, "#%xwf\n", _output->data[i].battSN); _device->write(cmd, strlen(cmd));tcdrain(_device->getFd()); _device->readUntil(reply, sizeof(reply), "\n", READ_TIMEOUT); //echo _device->readNChars(reply, sizeof(reply), 5000); //reply } clock_gettime(CLOCK_REALTIME, &_output->data[i].sampleTime); sleep(1); } _output->write(); _log->write(); //check for abort conditions, abort missions if batteries get too low Syslog::write("batts online %d, min cell voltage %f, mean min cell voltage %f\n", numBattsOnline, minCellVoltage, meanMinCellVoltage); if (meanMinCellVoltage <= _abortVoltage) { try { Syslog::write("BluefinBatt.cc: aborting mission because of low voltage\n"); // Create an interface to LayeredControl LayeredControlIF *layeredControl = new LayeredControlIF("layeredControl"); // LayeredControl is running; abort the mission if (layeredControl) { if (!layeredControl->abortingMission()) { layeredControl->abortMission(); } // Done with interface; delete it to avoid memory leaks delete layeredControl; } } catch (...) { Syslog::write("BluefinBatt.cc: Couldn't abort mission -- Layered Control not running!\n"); // LayeredControl isn't running throw Exception("BluefinBatt.cc: terminating mission because of low voltage and no Layered Control!\n"); } } if (meanMinCellVoltage <= _terminateVoltage) { Syslog::write("BluefinBatt.cc: terminating mission because of really " "low voltage\n"); throw Exception("BluefinBatt.cc: terminating mission because of really " "low voltage\n"); } //if (meanMin... } // void Sample(void) int BluefinBatt::getData(BluefinBatt::DataType dataType) { return 0; }