int usbDialog::sendCMD(unsigned char *cmd)
{
int rc;
rc = hid_write(dev, cmd, 1 + cmd[1]);
dumpRawData(true, rc, cmd);
rc = hid_read_timeout(dev, rawdata, sizeof(rawdata), 1000);
dumpRawData(false, rc, rawdata);
return rc;
}
//TODO: See the processing function below for a more detailed explanation - can't send so many setParam commands in a row
void ADAFRUITBLE::handleTick() {
MotorController* motorController = DeviceManager::getInstance()->getMotorController();
Throttle *accelerator = DeviceManager::getInstance()->getAccelerator();
Throttle *brake = DeviceManager::getInstance()->getBrake();
BatteryManager *bms = reinterpret_cast<BatteryManager *>(DeviceManager::getInstance()->getDeviceByType(DEVICE_BMS));
uint32_t ms = millis();
uint32_t IOTemp = 0;
uint8_t brklt;
tickCounter++;
if (ms < 1000) return; //wait 1 seconds for things to settle before doing a thing
// Do a delayed parameter load once about a second after startup
if (!didParamLoad && ms > 5000) {
loadParameters();
Logger::console("BLE characteristics loaded...");
bleBitFields.bitfield1 = motorController->getStatusBitfield1();
bleBitFields.bitfield2 = motorController->getStatusBitfield2();
bleBitFields.doUpdate = 1;
// DeviceManager::getInstance()->updateWifiByID(BRUSA_DMC5);
didParamLoad = true;
}
if (paramCache.timeRunning != (ms / 1000))
{
paramCache.timeRunning = ms / 1000;
if (!gatt.setChar(MeasureCharId[0], (uint8_t*)¶mCache.timeRunning, 4))
{
Logger::error("Could not update timeRunning");
}
else Logger::debug(ADABLUE, "Updated timeRunning");
dumpRawData((uint8_t *)¶mCache.timeRunning, 4);
}
//every other time - 80ms by default
if (tickCounter & 1)
{
if (motorController) {
if ( bleTrqReqAct.torqueRequested != motorController->getTorqueRequested() ) {
bleTrqReqAct.torqueRequested = motorController->getTorqueRequested();
bleTrqReqAct.doUpdate = 1;
}
if ( bleTrqReqAct.torqueActual != motorController->getTorqueActual() ) {
bleTrqReqAct.torqueActual = motorController->getTorqueActual();
bleTrqReqAct.doUpdate = 1;
}
if ( bleSpeeds.speedRequested != motorController->getSpeedRequested() ) {
bleSpeeds.speedRequested = motorController->getSpeedRequested();
bleSpeeds.doUpdate = 1;
}
if ( bleSpeeds.speedActual != motorController->getSpeedActual() ) {
bleSpeeds.speedActual = motorController->getSpeedActual();
bleSpeeds.doUpdate = 1;
}
}
if (accelerator) {
RawSignalData *rawSignal = accelerator->acquireRawSignal();
if ( bleThrBrkLevels.throttleRawLevel1 != rawSignal->input1)
{
bleThrBrkLevels.throttleRawLevel1 = rawSignal->input1;
bleThrBrkLevels.doUpdate = 1;
}
if ( bleThrBrkLevels.throttleRawLevel2 != rawSignal->input2)
{
bleThrBrkLevels.throttleRawLevel2 = rawSignal->input2;
bleThrBrkLevels.doUpdate = 1;
}
if (bleThrBrkLevels.throttlePercentage != accelerator->getLevel() / 10)
{
bleThrBrkLevels.throttlePercentage = accelerator->getLevel() / 10;
bleThrBrkLevels.doUpdate = 1;
}
}
if (brake) {
RawSignalData *rawSignal = brake->acquireRawSignal();
if (bleThrBrkLevels.brakeRawLevel != rawSignal->input1) {
bleThrBrkLevels.brakeRawLevel = rawSignal->input1;
paramCache.brakeNotAvailable = false;
bleThrBrkLevels.doUpdate = 1;
}
if (bleThrBrkLevels.brakePercentage != brake->getLevel() / 10) {
bleThrBrkLevels.brakePercentage = brake->getLevel() / 10;
bleThrBrkLevels.doUpdate = 1;
}
} else {
if ( paramCache.brakeNotAvailable == true ) {
paramCache.brakeNotAvailable = false; // no need to keep sending this
bleThrBrkLevels.brakeRawLevel = 0;
bleThrBrkLevels.doUpdate = 1;
}
}
}
if (tickCounter == 2) {
if (bms)
{
if (blePowerStatus.SOC != bms->getSOC())
{
blePowerStatus.SOC = bms->getSOC();
blePowerStatus.doUpdate = 1;
}
}
if (motorController) {
//Logger::console("Wifi tick counter 2...");
if ( blePowerStatus.busVoltage != motorController->getDcVoltage() ) {
blePowerStatus.busVoltage = motorController->getDcVoltage();
if(blePowerStatus.busVoltage<0) blePowerStatus.busVoltage=0;
//if(blePowerStatus.busVoltage>4500) blePowerStatus.busVoltage=4500;
blePowerStatus.doUpdate = 1;
}
if ( blePowerStatus.busCurrent != motorController->getDcCurrent() ) {
blePowerStatus.busCurrent = motorController->getDcCurrent();
blePowerStatus.doUpdate = 1;
}
if ( blePowerStatus.motorCurrent != motorController->getAcCurrent() ) {
blePowerStatus.motorCurrent = motorController->getAcCurrent();
blePowerStatus.doUpdate = 1;
}
if ( blePowerStatus.kwHours != motorController->getKiloWattHours()/3600000 ) {
blePowerStatus.kwHours = motorController->getKiloWattHours()/3600000;
if(blePowerStatus.kwHours<0)blePowerStatus.kwHours = 0;
if(blePowerStatus.kwHours>300)blePowerStatus.kwHours = 300;
blePowerStatus.doUpdate = 1;
}
if ( blePowerStatus.mechPower != motorController->getMechanicalPower() ) {
blePowerStatus.mechPower = motorController->getMechanicalPower();
if (blePowerStatus.mechPower<-250)blePowerStatus.mechPower=-250;
if (blePowerStatus.mechPower>1500)blePowerStatus.mechPower=1500;
blePowerStatus.doUpdate = 1;
}
//DeviceManager::getInstance()->updateWifi();
}
} else if (tickCounter == 3) {
if (motorController) {
//Logger::console("Wifi tick counter 3...");
if ( bleMaxParams.nomVoltage != motorController->getnominalVolt() ) {
bleMaxParams.nomVoltage = motorController->getnominalVolt();
bleMaxParams.doUpdate = 1;
}
if ( bleBitFields.bitfield1 != motorController->getStatusBitfield1() ) {
bleBitFields.bitfield1 = motorController->getStatusBitfield1();
bleBitFields.doUpdate = 1;
}
if ( bleBitFields.bitfield2 != motorController->getStatusBitfield2() ) {
bleBitFields.bitfield2 = motorController->getStatusBitfield2();
bleBitFields.doUpdate = 1;
}
IOTemp = 0;
for (int i = 0; i < 4; i++)
{
if (getDigital(i)) bleBitFields.digitalInputs |= 1 << i;
}
if ( bleBitFields.digitalInputs != IOTemp ) {
bleBitFields.digitalInputs = IOTemp;
bleBitFields.doUpdate = 1;
}
IOTemp = 0;
for (int i = 0; i < 8; i++)
{
if (getOutput(i)) bleBitFields.digitalOutputs |= 1 << i;
}
if ( bleBitFields.digitalOutputs != IOTemp ) {
bleBitFields.digitalOutputs = IOTemp;
bleBitFields.doUpdate = 1;
}
if ( bleModes.isRunning != motorController->isRunning() ) {
bleModes.isRunning = motorController->isRunning();
bleModes.doUpdate = 1;
}
if ( bleModes.isFaulted != motorController->isFaulted() ) {
bleModes.isFaulted = motorController->isFaulted();
bleModes.doUpdate = 1;
}
if ( bleModes.isWarning != motorController->isWarning() ) {
bleModes.isWarning = motorController->isWarning();
bleModes.doUpdate = 1;
}
if ( bleModes.gear != motorController->getSelectedGear() ) {
bleModes.gear = motorController->getSelectedGear();
bleModes.doUpdate = 1;
}
if ( bleModes.powerMode != motorController->getPowerMode() ) {
bleModes.powerMode = motorController->getPowerMode();
bleModes.doUpdate = 1;
}
}
} else if (tickCounter == 4) {
if (motorController) {
//Logger::console("Wifi tick counter 4...");
if ( bleDigIO.prechargeDuration != motorController->getprechargeR() ) {
bleDigIO.prechargeDuration = motorController->getprechargeR();
bleDigIO.doUpdate = 1;
}
if ( bleDigIO.prechargeRelay != motorController->getprechargeRelay() ) {
bleDigIO.prechargeRelay = motorController->getprechargeRelay();
bleDigIO.doUpdate = 1;
//Logger::console("Precharge Relay %i", paramCache.prechargeRelay);
//Logger::console("motorController:prechargeRelay:%d, paramCache.prechargeRelay:%d, Constants:prechargeRelay:%s", motorController->getprechargeRelay(),paramCache.prechargeRelay, Constants::prechargeRelay);
}
if ( bleDigIO.mainContRelay != motorController->getmainContactorRelay() ) {
bleDigIO.mainContRelay = motorController->getmainContactorRelay();
bleDigIO.doUpdate = 1;
}
if ( bleDigIO.coolingRelay != motorController->getCoolFan() ) {
bleDigIO.coolingRelay = motorController->getCoolFan();
bleDigIO.doUpdate = 1;
}
if ( bleDigIO.coolOnTemp != motorController->getCoolOn() ) {
bleDigIO.coolOnTemp = motorController->getCoolOn();
bleDigIO.doUpdate = 1;
}
if ( bleDigIO.coolOffTemp != motorController->getCoolOff() ) {
bleDigIO.coolOffTemp = motorController->getCoolOff();
bleDigIO.doUpdate = 1;
}
if ( bleDigIO.brakeLightOut != motorController->getBrakeLight() ) {
bleDigIO.brakeLightOut = motorController->getBrakeLight();
bleDigIO.doUpdate = 1;
}
if ( bleDigIO.reverseLightOut != motorController->getRevLight() ) {
bleDigIO.reverseLightOut = motorController->getRevLight();
bleDigIO.doUpdate = 1;
}
if ( bleDigIO.enableIn != motorController->getEnableIn() ) {
bleDigIO.enableIn = motorController->getEnableIn();
bleDigIO.doUpdate = 1;
}
if ( bleDigIO.reverseIn != motorController->getReverseIn() ) {
bleDigIO.reverseIn = motorController->getReverseIn();
bleDigIO.doUpdate = 1;
}
}
} else if (tickCounter > 4) {
if (motorController) {
//Logger::console("Wifi tick counter 5...");
if ( bleTemperatures.motorTemperature != motorController->getTemperatureMotor() ) {
bleTemperatures.motorTemperature = motorController->getTemperatureMotor();
bleTemperatures.doUpdate = 1;
}
if ( bleTemperatures.inverterTemperature != motorController->getTemperatureInverter() ) {
bleTemperatures.inverterTemperature = motorController->getTemperatureInverter();
bleTemperatures.doUpdate = 1;
}
if ( bleTemperatures.systemTemperature != motorController->getTemperatureSystem() ) {
bleTemperatures.systemTemperature = motorController->getTemperatureSystem();
bleTemperatures.doUpdate = 1;
}
}
tickCounter = 0;
}
transferUpdates(); //send out any updates required
}
static status_t
catAttr(const char *attribute, const char *fileName, bool keepRaw = false)
{
int fd = open(fileName, O_RDONLY);
if (fd < 0)
return errno;
attr_info info;
if (fs_stat_attr(fd, attribute, &info) < 0)
return errno;
// limit size of the attribute, only the first 64k will make it on screen
off_t size = info.size;
bool cut = false;
if (size > 64 * 1024) {
size = 64 * 1024;
cut = true;
}
char* buffer = (char*)malloc(size);
if (!buffer) {
fprintf(stderr, "Could not allocate read buffer!\n");
return B_NO_MEMORY;
}
ssize_t bytesRead = fs_read_attr(fd, attribute, info.type, 0, buffer, size);
if (bytesRead < 0) {
free(buffer);
return errno;
}
if (bytesRead != size) {
fprintf(stderr, "Could only read %ld bytes from attribute!\n",
bytesRead);
free(buffer);
return B_ERROR;
}
if (keepRaw) {
off_t pos = 0;
ssize_t written = 0;
while (pos < info.size) {
// write what we have read so far
written = write(STDOUT_FILENO, buffer, bytesRead);
// check for write error
if (written < bytesRead) {
if (written >= 0) {
fprintf(stderr, "Could only write %ld bytes to stream!\n",
written);
written = B_ERROR;
} else {
fprintf(stderr, "Failed to write to stream: %s\n",
strerror(written));
}
break;
}
// read next chunk of data at pos
pos += bytesRead;
bytesRead = fs_read_attr(fd, attribute, info.type, pos, buffer,
size);
// check for read error
if (bytesRead < size && pos + bytesRead < info.size) {
if (bytesRead >= 0) {
fprintf(stderr, "Could only read %ld bytes from "
"attribute!\n", bytesRead);
} else {
fprintf(stderr, "Failed to read from attribute: %s\n",
strerror(bytesRead));
}
written = B_ERROR;
break;
}
}
free(buffer);
if (written > 0)
written = B_OK;
return written;
}
switch (info.type) {
case B_INT8_TYPE:
printf("%s : int8 : %d\n", fileName, *((int8 *)buffer));
break;
case B_UINT8_TYPE:
printf("%s : uint8 : %u\n", fileName, *((uint8 *)buffer));
break;
case B_INT16_TYPE:
printf("%s : int16 : %d\n", fileName, *((int16 *)buffer));
break;
case B_UINT16_TYPE:
printf("%s : uint16 : %u\n", fileName, *((uint16 *)buffer));
break;
case B_INT32_TYPE:
printf("%s : int32 : %ld\n", fileName, *((int32 *)buffer));
break;
case B_UINT32_TYPE:
printf("%s : uint32 : %lu\n", fileName, *((uint32 *)buffer));
break;
case B_INT64_TYPE:
printf("%s : int64 : %Ld\n", fileName, *((int64 *)buffer));
break;
case B_UINT64_TYPE:
printf("%s : uint64 : %Lu\n", fileName, *((uint64 *)buffer));
break;
case B_FLOAT_TYPE:
printf("%s : float : %f\n", fileName, *((float *)buffer));
break;
case B_DOUBLE_TYPE:
printf("%s : double : %f\n", fileName, *((double *)buffer));
break;
case B_BOOL_TYPE:
printf("%s : bool : %d\n", fileName, *((unsigned char *)buffer));
break;
case B_STRING_TYPE:
printf("%s : string : %s\n", fileName, buffer);
break;
case B_MIME_STRING_TYPE:
case 'MSIG':
case 'MSDC':
case 'MPTH':
printf("%s : %s : %s\n", fileName, type_to_string(info.type),
buffer);
break;
case B_MESSAGE_TYPE:
{
BMessage message;
if (!cut && message.Unflatten(buffer) == B_OK) {
printf("%s : message :\n", fileName);
message.PrintToStream();
break;
}
// supposed to fall through
}
default:
// The rest of the attributes types are displayed as raw data
printf("%s : %s : \n", fileName, type_to_string(info.type));
dumpRawData(buffer, size);
break;
}
free(buffer);
return B_OK;
}
void ADAFRUITBLE::transferUpdates()
{
if (bleTrqReqAct.doUpdate != 0)
{
bleTrqReqAct.doUpdate = 0;
if (!gatt.setChar(MeasureCharId[1], (uint8_t *)&bleTrqReqAct, sizeof(bleTrqReqAct) - 1))
{
Logger::error("Could not update bleTrqReqAct");
}
else Logger::debug(ADABLUE, "Updated bleTrqReqAct");
dumpRawData((uint8_t *)&bleTrqReqAct, sizeof(bleTrqReqAct) - 1);
}
if (bleThrBrkLevels.doUpdate != 0)
{
bleThrBrkLevels.doUpdate = 0;
if (!gatt.setChar(MeasureCharId[2], (uint8_t *)&bleThrBrkLevels, sizeof(bleThrBrkLevels) - 1))
{
Logger::error("Could not update bleThrBrkLevels");
}
else Logger::debug(ADABLUE, "Updated bleThrBrkLevels");
dumpRawData((uint8_t *)&bleThrBrkLevels, sizeof(bleThrBrkLevels) - 1);
}
if (bleSpeeds.doUpdate != 0)
{
bleSpeeds.doUpdate = 0;
if (!gatt.setChar(MeasureCharId[3], (uint8_t *)&bleSpeeds, sizeof(bleSpeeds) - 1))
{
Logger::error("Could not update bleSpeeds");
}
else Logger::debug(ADABLUE, "Updated bleSpeeds");
dumpRawData((uint8_t *)&bleSpeeds, sizeof(bleSpeeds) - 1);
}
if (bleModes.doUpdate != 0)
{
bleModes.doUpdate = 0;
if (!gatt.setChar(MeasureCharId[4], (uint8_t *)&bleModes, sizeof(bleModes) - 1))
{
Logger::error("Could not update bleModes");
}
else Logger::debug(ADABLUE, "Updated bleModes");
dumpRawData((uint8_t *)&bleModes, sizeof(bleModes) - 1);
}
if (blePowerStatus.doUpdate != 0)
{
blePowerStatus.doUpdate = 0;
if (!gatt.setChar(MeasureCharId[5], (uint8_t *)&blePowerStatus, sizeof(blePowerStatus) - 1))
{
Logger::error("Could not update blePowerStatus");
}
else Logger::debug(ADABLUE, "Updated blePowerStatus");
dumpRawData((uint8_t *)&blePowerStatus, sizeof(blePowerStatus) - 1);
}
if (bleBitFields.doUpdate != 0)
{
bleBitFields.doUpdate = 0;
if (!gatt.setChar(MeasureCharId[6], (uint8_t *)&bleBitFields, sizeof(bleBitFields) - 1))
{
Logger::error("Could not update bleBitFields");
}
else Logger::debug(ADABLUE, "Updated bleBitFields");
dumpRawData((uint8_t *)&bleBitFields, sizeof(bleBitFields) - 1);
}
if (bleTemperatures.doUpdate != 0)
{
bleTemperatures.doUpdate = 0;
if (!gatt.setChar(MeasureCharId[7], (uint8_t *)&bleTemperatures, sizeof(bleTemperatures) - 1))
{
Logger::error("Could not update bleTemperatures");
}
else Logger::debug(ADABLUE, "Updated bleTemperatures");
dumpRawData((uint8_t *)&bleTemperatures, sizeof(bleTemperatures) - 1);
}
if (bleDigIO.doUpdate != 0)
{
bleDigIO.doUpdate = 0;
if (!gatt.setChar(MeasureCharId[8], (uint8_t *)&bleDigIO, sizeof(bleDigIO) - 1))
{
Logger::error("Could not update bleDigIO");
}
else Logger::debug(ADABLUE, "Updated bleDigIO");
dumpRawData((uint8_t *)&bleDigIO, sizeof(bleDigIO) - 1);
}
if (bleThrottleIO.doUpdate != 0)
{
bleThrottleIO.doUpdate = 0;
if (!gatt.setChar(MeasureCharId[9], (uint8_t *)&bleThrottleIO, sizeof(bleThrottleIO) - 1))
{
Logger::error("Could not update bleThrottleIO");
}
else Logger::debug(ADABLUE, "Updated bleThrottleIO");
dumpRawData((uint8_t *)&bleThrottleIO, sizeof(bleThrottleIO) - 1);
}
if (bleThrottleMap.doUpdate != 0)
{
bleThrottleMap.doUpdate = 0;
if (!gatt.setChar(MeasureCharId[10], (uint8_t *)&bleThrottleMap, sizeof(bleThrottleMap) - 1))
{
Logger::error("Could not update bleThrottleMap");
}
else Logger::debug(ADABLUE, "Updated bleThrottleMap");
dumpRawData((uint8_t *)&bleThrottleMap, sizeof(bleThrottleMap) - 1);
}
if (bleBrakeParam.doUpdate != 0)
{
bleBrakeParam.doUpdate = 0;
if (!gatt.setChar(MeasureCharId[11], (uint8_t *)&bleBrakeParam, sizeof(bleBrakeParam) - 1))
{
Logger::error("Could not update bleBrakeParam");
}
else Logger::debug(ADABLUE, "Updated bleBrakeParam");
dumpRawData((uint8_t *)&bleBrakeParam, sizeof(bleBrakeParam) - 1);
}
if (bleMaxParams.doUpdate != 0)
{
bleMaxParams.doUpdate = 0;
if (!gatt.setChar(MeasureCharId[12], (uint8_t *)&bleMaxParams, sizeof(bleMaxParams) - 1))
{
Logger::error("Could not update bleMaxParams");
}
else Logger::debug(ADABLUE, "Updated bleMaxParams");
dumpRawData((uint8_t *)&bleMaxParams, sizeof(bleMaxParams) - 1);
}
}
