#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 "!?\r\n" #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) : _device(device), _numBatts(numBatts), 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); for (int i = 0; i < _numBatts; i++) { _output->data[i].state = Unknown; _output->data[i].fault = 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; } _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, sizeof(ADDR_QUERY)); Syslog::write("BluefinBatt::init() - querying for batt ids"); try { // First read the echoed command // _device->readUntil(reply, sizeof(reply), "\n", READ_TIMEOUT); //write echo to log Syslog::write("BluefinBatt::init() - battery echo is %s\n", reply); // Now read the battery addresses // _device->flushReceiveBuf(); 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\n",_output->data[i].battSN); printf("wrote batt string %s", buf); _device->write(buf,strlen(buf)); _device->readUntil(reply, sizeof(reply), "\n", 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]; for (int i = 0; i < _numBatts; i++) { sprintf(cmd, "#%xq0\n", _output->data[i].battSN); _device->write(cmd, strlen(cmd)); try { // First read the echoed command // _device->readUntil(reply, sizeof(reply), "\n", READ_TIMEOUT); //write echo to log Syslog::write("BluefinBatt::init() - battery echo is %s\n", reply); // 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; int nchars = _device->readNChars(reply, 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 = off; break; case 'd' : _output->data[i].state = Discharging; break; case 'c' : _output->data[i].state = Charging; break; case 'b' : _output->data[i].state = Balancing; break; default : _output->data[i].state = Unknown; } switch(reply[7]) { case '-' : _output->data[i].fault = Clear; break; case 'V' : _output->data[i].fault = OverVolt; break; case 'v' : _output->data[i].fault = UnderVolt;break; case 'I' : _output->data[i].fault = OverCurrent; break; case 'C' : _output->data[i].fault = MaxCellOverVolt; break; case 'c' : _output->data[i].fault = MinCellUnderVolt; break; case 'T' : _output->data[i].fault = OverTemp; break; case 'W' : _output->data[i].fault = WaterLeak; break; case 'H' : _output->data[i].fault = HardwareProt; break; case 'h' : _output->data[i].fault = HardwareTransient; break; case 'm' : _output->data[i].fault = Watchdog; break; default : _output->data[i].fault = Clear; } 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 for (int i = 0; i < _numBatts; i++) { if ((_output->data[i].minvoltage<_abortVoltage) && !_aborted) { 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 layeredControl->abortMission(); _aborted = True; // 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 } } } } int BluefinBatt::getData(BluefinBatt::DataType dataType) { return 0; }