bool
BLECharacteristicImp::setValue(const unsigned char value[], uint16_t length)
{
_setValue(value, length, 0);
if (BLEUtils::isLocalBLE(_ble_device) == true)
{
// GATT server
// Write request for GATT server
if (_event_handlers[BLEWritten])
{
BLECharacteristic chrcTmp(this, &_ble_device);
_event_handlers[BLEWritten](_ble_device, chrcTmp);
}
if (_oldevent_handlers[BLEWritten])
{
BLECharacteristic chrcTmp(this, &_ble_device);
BLECentral central(_ble_device);
_oldevent_handlers[BLEWritten](central, chrcTmp);
}
}
else
{
// GATT client
// Discovered attribute
// Read response/Notification/Indication for GATT client
if (_reading)
{
// Read response received. Not block the other reading.
_reading = false;
}
if (_event_handlers[BLEValueUpdated])
{
BLECharacteristic chrcTmp(this, &_ble_device);
_event_handlers[BLEValueUpdated](_ble_device, chrcTmp);
}
if (_oldevent_handlers[BLEValueUpdated])
{
BLECharacteristic chrcTmp(this, &_ble_device);
BLECentral central(_ble_device);
_oldevent_handlers[BLEValueUpdated](central, chrcTmp);
}
}
return true;
}
bool MAXMessage::checkAccess(std::shared_ptr<MAXPacket> packet, std::shared_ptr<PacketQueue> queue)
{
try
{
std::shared_ptr<MAXCentral> central(std::dynamic_pointer_cast<MAXCentral>(GD::family->getCentral()));
if(!central || !packet) return false;
int32_t access = central->isInPairingMode() ? _accessPairing : _access;
if(access == NOACCESS) return false;
if(queue && !queue->isEmpty() && packet->destinationAddress() == central->getAddress())
{
if(queue->front()->getType() == QueueEntryType::PACKET)
{
std::shared_ptr<MAXPacket> backup = queue->front()->getPacket();
queue->pop(); //Popping takes place here to be able to process resent messages.
if(!queue->isEmpty() && queue->front()->getType() == QueueEntryType::MESSAGE && !typeIsEqual(queue->front()->getMessage()))
{
queue->pushFront(backup, false, false, false);
return false;
}
}
}
if(access & FULLACCESS) return true;
if((access & ACCESSDESTISME) && packet->destinationAddress() != central->getAddress())
{
//GD::out.printMessage( "Access denied, because the destination address is not me: " << packet->hexString() << std::endl;
return false;
}
if((access & ACCESSUNPAIRING) && queue != nullptr && queue->getQueueType() == PacketQueueType::UNPAIRING)
{
return true;
}
if(access & ACCESSPAIREDTOSENDER)
{
std::shared_ptr<MAXPeer> currentPeer;
if(central->isInPairingMode() && queue && queue->peer && queue->peer->getAddress() == packet->senderAddress()) currentPeer = queue->peer;
if(!currentPeer) currentPeer = central->getPeer(packet->senderAddress());
if(!currentPeer) return false;
}
if((access & ACCESSCENTRAL) && central->getCentralAddress() != packet->senderAddress())
{
//GD::out.printMessage( "Access denied, because it is only granted to a paired central: " << packet->hexString() << std::endl;
return false;
}
return true;
}
catch(const std::exception& ex)
{
GD::out.printEx(__FILE__, __LINE__, __PRETTY_FUNCTION__, ex.what());
}
catch(BaseLib::Exception& ex)
{
GD::out.printEx(__FILE__, __LINE__, __PRETTY_FUNCTION__, ex.what());
}
catch(...)
{
GD::out.printEx(__FILE__, __LINE__, __PRETTY_FUNCTION__);
}
return false;
}
std::shared_ptr<BaseLib::Systems::ICentral> BidCoS::initializeCentral(uint32_t deviceId, int32_t address, std::string serialNumber)
{
int32_t addressFromSettings = 0;
std::string addressHex = GD::settings->get("centraladdress");
if(!addressHex.empty()) addressFromSettings = BaseLib::Math::getNumber(addressHex);
if(addressFromSettings != 0)
{
std::shared_ptr<HomeMaticCentral> central(new HomeMaticCentral(deviceId, serialNumber, addressFromSettings, this));
if(address != addressFromSettings) central->save(true);
GD::out.printInfo("Info: Central address set to 0x" + BaseLib::HelperFunctions::getHexString(addressFromSettings, 6) + ".");
return central;
}
if(address == 0)
{
address = (0xfd << 16) + BaseLib::HelperFunctions::getRandomNumber(0, 0xFFFF);
std::shared_ptr<HomeMaticCentral> central(new HomeMaticCentral(deviceId, serialNumber, address, this));
central->save(true);
GD::out.printInfo("Info: Central address set to 0x" + BaseLib::HelperFunctions::getHexString(address, 6) + ".");
return central;
}
GD::out.printInfo("Info: Central address set to 0x" + BaseLib::HelperFunctions::getHexString(address, 6) + ".");
return std::shared_ptr<HomeMaticCentral>(new HomeMaticCentral(deviceId, serialNumber, address, this));
}
int main()
{
BLE& ble = BLE::Instance();
while(1) {
{
events::EventQueue queue;
printf("\r\n * Device is a peripheral *\r\n\r\n");
PrivacyPeripheral peripheral(ble, queue);
peripheral.run();
}
{
events::EventQueue queue;
printf("\r\n * Device is a central *\r\n\r\n");
PrivacyCentral central(ble, queue);
central.run();
}
}
return 0;
}
void MAXMessage::invokeMessageHandler(std::shared_ptr<MAXPacket> packet)
{
try
{
std::shared_ptr<MAXCentral> central(std::dynamic_pointer_cast<MAXCentral>(GD::family->getCentral()));
if(!central || _messageHandler == nullptr || packet == nullptr) return;
(central.get()->*(_messageHandler))((int32_t)packet->messageCounter(), packet);
}
catch(const std::exception& ex)
{
GD::out.printEx(__FILE__, __LINE__, __PRETTY_FUNCTION__, ex.what());
}
catch(BaseLib::Exception& ex)
{
GD::out.printEx(__FILE__, __LINE__, __PRETTY_FUNCTION__, ex.what());
}
catch(...)
{
GD::out.printEx(__FILE__, __LINE__, __PRETTY_FUNCTION__);
}
}
dgFloat32 dgCollisionCone::CalculateMassProperties (dgVector& inertia, dgVector& crossInertia, dgVector& centerOfMass) const
{
dgFloat32 volume;
dgFloat32 inertaxx;
dgFloat32 inertayyzz;
//dgVector centerOfMass1;
//dgVector inertia1;
//dgVector crossInertia1;
//volume = dgCollisionConvex::CalculateMassProperties (inertia1, crossInertia1, centerOfMass1);
volume = dgFloat32 (3.1616f * 2.0f / 3.0f) * m_radius * m_radius * m_height;
centerOfMass = GetOffsetMatrix().m_posit - GetOffsetMatrix().m_front.Scale (dgFloat32 (0.5f) * m_height);
inertaxx = dgFloat32 (3.0f / 10.0f) * m_radius * m_radius * volume;
inertayyzz = (dgFloat32 (3.0f / 20.0f) * m_radius * m_radius + dgFloat32 (4.0f / 10.0f) * m_height * m_height) * volume;
dgMatrix inertiaTensor (dgGetIdentityMatrix());
inertiaTensor[0][0] = inertaxx;
inertiaTensor[1][1] = inertayyzz;
inertiaTensor[2][2] = inertayyzz;
inertiaTensor = GetOffsetMatrix().Inverse() * inertiaTensor * GetOffsetMatrix();
crossInertia.m_x = inertiaTensor[1][2] - volume * centerOfMass.m_y * centerOfMass.m_z;
crossInertia.m_y = inertiaTensor[0][2] - volume * centerOfMass.m_z * centerOfMass.m_x;
crossInertia.m_z = inertiaTensor[0][1] - volume * centerOfMass.m_x * centerOfMass.m_y;
dgVector central (centerOfMass.CompProduct(centerOfMass));
inertia.m_x = inertiaTensor[0][0] + volume * (central.m_y + central.m_z);
inertia.m_y = inertiaTensor[1][1] + volume * (central.m_z + central.m_x);
inertia.m_z = inertiaTensor[2][2] + volume * (central.m_x + central.m_y);
centerOfMass = centerOfMass.Scale (volume);
return volume;
}
float Rounder::errPos()
{
return fabs(central()-CLhi());
}
///
/// Compute rounded negative error.
/// Use the rounded central values and interval boundaries for this.
/// Would we round the +/- errors themselves, we'd be geting inconsistent intervals!
/// \return minus error (always positive)
///
float Rounder::errNeg()
{
return fabs(central()-CLlo());
}
