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()); }