/* =================================================
FUNCTION: initAcc
CREATED: 16-05-2014
DESCRIPTION: initializes the sensor
PARAMETERS: None
GLOBAL VARIABLES: None.
RETURNS: None.
AUTHOR: P. Kantue
================================================== */
void initAcc() {
//Turning on the ADXL345
writeTo(ACC, 0x2D, 0);
writeTo(ACC, 0x2D, 16);
writeTo(ACC, 0x2D, 8);
//by default the device is in +-2g range reading
}
/* =================================================
FUNCTION: initGyro
CREATED: 16-05-2014
DESCRIPTION: initializes the sensor
PARAMETERS: None
GLOBAL VARIABLES: None.
RETURNS: None.
AUTHOR: P. Kantue
================================================== */
void initGyro()
{
writeTo(GYRO, G_PWR_MGM, 0x00);
writeTo(GYRO, G_SMPLRT_DIV, 0x07); // EB, 50, 80, 7F, DE, 23, 20, FF
writeTo(GYRO, G_DLPF_FS, 0x1E); // Full Scale Range +/- 2000 deg/sec, 1KHz, 1E, 19
writeTo(GYRO, G_INT_CFG, 0x00);
}
void Accelerometer::powerOn() {
Wire.begin(); // join i2c bus (address optional for master)
//Turning on the ADXL345
writeTo(DEVICE, ADXL345_POWER_CTL, 0);
writeTo(DEVICE, ADXL345_POWER_CTL, 16);
writeTo(DEVICE, ADXL345_POWER_CTL, 8);
}
void CAdxl345::powerOn() {
begin(); // enable I2C Bus
//Turning on the ADXL345
writeTo(DEVICE, ADXL345_POWER_CTL, 0);
writeTo(DEVICE, ADXL345_POWER_CTL, 16);
writeTo(DEVICE, ADXL345_POWER_CTL, 8);
}
void Sensors::initMagnetometer()
{
// Enable the magnetometer
writeTo(HMC5883_ADDRESS_MAG, HMC5883_REGISTER_MAG_MR_REG_M, 0x00);
// Set the gain to +/-1.3 (max sensitivity)
writeTo(HMC5883_ADDRESS_MAG, HMC5883_REGISTER_MAG_CRB_REG_M, 0x20);
}
/* does not return */
void be_child(int fd_from_master, int fd_to_master) {
int start = 0, end = 0;
while (1) {
writeTo(fd_to_master, &start);
writeTo(fd_to_master, &end);
/* got EOF from master = exit */
if (readFrom(fd_from_master, &start) == 0) exit(0);
if (readFrom(fd_from_master, &end) == 0) exit(0);
doit(start, end);
}
}
void IMU3000::setSampleRate(byte divider, byte lowpass)
{
//writing divider
writeTo(IMU3000_REG_SMPLRT_DIV,divider);
//writing lowpass rate freq
byte lpf;
readFrom(IMU3000_REG_DLPF, 1, &lpf);
lpf &= ~IMU3000_DLPF_CFG_MASK;
lpf |= lowpass;
writeTo(IMU3000_REG_DLPF, lpf);
}
ISCORE_PLUGIN_SCENARIO_EXPORT void Visitor<Writer<DataStream>>::writeTo(Scenario::StateModel& s)
{
// Common metadata
writeTo(s.metadata);
m_stream >> s.m_eventId
>> s.m_previousConstraint
>> s.m_nextConstraint
>> s.m_heightPercentage;
// Message tree
Process::MessageNode n;
m_stream >> n;
s.m_messageItemModel = new Scenario::MessageItemModel{s.m_stack, s, &s};
s.messages() = n;
// Processes plugins
int32_t process_count;
m_stream >> process_count;
auto& pl = context.components.factory<Process::StateProcessList>();
for(; process_count -- > 0;)
{
s.stateProcesses.add(deserialize_interface(pl, *this, &s));
}
checkDelimiter();
}
std::string Element::toString() const {
if (!ok())
return "INVALID-MUTABLE-ELEMENT";
if (hasValue())
return getValue().toString();
const BSONType type = getType();
// The only types that sometimes don't have a value are Object and Array nodes.
dassert((type == mongo::Object) || (type == mongo::Array));
if (type == mongo::Object) {
BSONObjBuilder builder;
writeTo(&builder);
BSONObj obj = builder.obj();
return obj.firstElement().toString();
} else {
// It must be an array.
BSONObjBuilder builder;
BSONArrayBuilder arrayBuilder(builder.subarrayStart(getFieldName()));
writeArrayTo(&arrayBuilder);
arrayBuilder.done();
BSONObj obj = builder.obj();
return obj.firstElement().toString();
}
}
template<> void Visitor<Writer<JSONObject>>::writeTo(Scenario::BaseScenario& base_scenario)
{
writeTo(static_cast<Scenario::BaseScenarioContainer&>(base_scenario));
Deserializer<JSONValue> elementPluginDeserializer(m_obj["PluginsMetadata"]);
base_scenario.pluginModelList = iscore::ElementPluginModelList{elementPluginDeserializer, &base_scenario};
}
template<> void Visitor<Writer<DataStream>>::writeTo(Scenario::BaseScenario& base_scenario)
{
writeTo(static_cast<Scenario::BaseScenarioContainer&>(base_scenario));
base_scenario.pluginModelList = iscore::ElementPluginModelList{*this, &base_scenario};
checkDelimiter();
}
void Node::resize(NodeSize newSize) {
if (size() == newSize) return;
hasChanged_ = true;
// Flush all data blocks, so that IDs of
// deleted blocks aren't updated incorrectly
// at some later time.
cache_.flush();
const size_t oldBlockCount = BYTES_TO_BLOCKS(size());
const size_t newBlockCount = BYTES_TO_BLOCKS(newSize);
// Set the size field.
BlockWriter sizeWriter(cache_.getWriteBlock(BlockPath::Root()), NODE_SIZE_OFFSET);
Binary::WriteUint64(sizeWriter, newSize);
size_ = newSize;
// Replace block IDs with zero for deleted blocks.
if (oldBlockCount > newBlockCount) {
const auto zeroId = BlockId::Zero();
for (size_t i = newBlockCount; i < oldBlockCount; i++) {
const auto path = BlockPath::Index(i);
auto& parentBlock = cache_.getWriteBlock(path.parent());
BlockWriter writer(parentBlock, NodeBlockIdOffset(path));
zeroId.writeTo(writer);
}
}
}
int ITG3200::setClockSource(byte _CLKsource) {
int a=0;
a=readFrom( _dev_address,PWR_MGM, 1, &_buff[0]);
MYASSERT(a,"Failed to read clock source\n\r")
a=writeTo( _dev_address,PWR_MGM, ((_buff[0] & ~PWRMGM_CLK_SEL) | _CLKsource));
MYASSERT(a,"Failed to write clock source\n\r")
return 0;
}
int ITG3200::setFilterBW(byte _BW) {
int a=0;
a=readFrom( _dev_address,DLPF_FS, 1, &_buff[0]);
MYASSERT(a,"Failed to get BW\n\r")
a=writeTo( _dev_address,DLPF_FS, ((_buff[0] & ~DLPFFS_DLPF_CFG) | _BW));
MYASSERT(a,"Failed to set BW\n\r")
return 0;
}
int ITG3200::setITGReady(bool _State) {
int a=0;
a=readFrom( _dev_address,INT_CFG, 1, &_buff[0]);
MYASSERT(a,"Failed to set interrupt mode\n\r")
a=writeTo( _dev_address,INT_CFG, ((_buff[0] & ~INTCFG_ITG_RDY_EN) | _State << 2));
MYASSERT(a,"Failed to set sample rate div\n\r")
return 0;
}
int ITG3200::setRawDataReady(bool _State) {
int a=0;
a=readFrom( _dev_address,INT_CFG, 1, &_buff[0]);
MYASSERT(a,"Fialed to read raw data ready\n\r")
a=writeTo( _dev_address,INT_CFG, ((_buff[0] & ~INTCFG_RAW_RDY_EN) | _State));
MYASSERT(a,"Failed to set raw data ready\n\r")
return 0;
}
int ITG3200::setFSRange(byte _Range) {
int a=0;
a=readFrom( _dev_address,DLPF_FS, 1, &_buff[0]);
MYASSERT(a,"Failed to get DLPF_FS\n\r")
a=writeTo( _dev_address,DLPF_FS, ((_buff[0] & ~DLPFFS_FS_SEL) | (_Range << 3)) );
MYASSERT(a,"Failed to set FSrange\n\r")
return 0;
}
void CAdxl345::setRegisterBit(byte regAdress, int bitPos, bool state) {
byte _b;
readFrom(DEVICE, regAdress, 1, &_b);
if (state) {
_b |= (1 << bitPos); // forces nth bit of _b to be 1. all other bits left alone.
} else {
_b &= ~(1 << bitPos); // forces nth bit of _b to be 0. all other bits left alone.
}
writeTo(DEVICE, regAdress, _b);
}
void IMU3000::setRange(byte range)
{
byte rng;
readFrom(IMU3000_REG_DLPF, 1, &rng);
rng &= ~IMU3000_FS_SEL_MASK;
rng |= range;
writeTo(IMU3000_REG_DLPF, rng);
}
/*
* setGain method:
* set HMC5883L_SCALE_FACTOR based on 'fieldRange' value
* write to HMC5883L_ConfigurationRegisterB register the appropriate value for the specified 'fieldRange'
*/
void HMC5883L::setGain(float fieldRange)
{
if (fieldRange==0.88) // Nominal gain configuration (HMC5883L_ConfigurationRegisterB)
{
HMC5883L_SCALE_FACTOR = (1000.0f / 1370.0f);
writeTo(HMC5883L_ConfigurationRegisterB, 0x00);
}
else if (fieldRange==1.3)
{
HMC5883L_SCALE_FACTOR = (1000.0f / 1090.0f);
writeTo(HMC5883L_ConfigurationRegisterB, 0x20);
}
else if (fieldRange==1.9)
{
HMC5883L_SCALE_FACTOR = (1000.0f / 820.0f);
writeTo(HMC5883L_ConfigurationRegisterB, 0x40);
}
else if (fieldRange==2.5)
{
HMC5883L_SCALE_FACTOR = (1000.0f / 660.0f);
writeTo(HMC5883L_ConfigurationRegisterB, 0x60);
}
else if (fieldRange==4.0)
{
HMC5883L_SCALE_FACTOR = (1000.0f / 440.0f);
writeTo(HMC5883L_ConfigurationRegisterB, 0x80);
}
else if (fieldRange==4.7)
{
HMC5883L_SCALE_FACTOR = (1000.0f / 390.0f);
writeTo(HMC5883L_ConfigurationRegisterB, 0xA0);
}
else if (fieldRange==5.6)
{
HMC5883L_SCALE_FACTOR = (1000.0f / 330.0f);
writeTo(HMC5883L_ConfigurationRegisterB, 0xC0);
}
else if (fieldRange==8.1)
{
HMC5883L_SCALE_FACTOR = (1000.0f / 230.0f);
writeTo(HMC5883L_ConfigurationRegisterB, 0xE0);
}
else // out of range - return to defaults // default configuration: field range 1.3Ga
{
HMC5883L_SCALE_FACTOR = (1000 / 1090);
writeTo(HMC5883L_ConfigurationRegisterB, 0x20);
}
}
void Writer::writeTo(QTextStream *stream, OrgElement::Pointer element)
{
const QString line = element->line();
if (!line.isNull()) {
*stream << line << endl;
}
auto children = element->children();
for(auto child : children) {
writeTo(stream, child);
}
}
Client::State Client::recvPackets() {
packet inpkt;
packet outpkt;
// Check for timeout
switch (recvPacket(inpkt)) {
case 1:
// Receive error.
return ERROR;
case 2:
// Bad checksum or timeout.
outpkt = rejpkt(mvSequence);
break;
default:
// if sequence is less than or equal to our own, send RR. Even if it's
// lower than it's supposed to be, we'll just send the RR to make the
// server feel better about itself.
if (inpkt.sequence <= mvSequence) {
mvRetries = PKT_TRNSMAX;
outpkt = rrpkt(inpkt.sequence);
if (inpkt.sequence == mvSequence) {
mvSequence++;
if (writeTo(inpkt) == 1) {
return ERROR;
}
if (inpkt.type == PKT_TYPE_DAT && inpkt.size < mvBufferSize) {
// A valid, less-than-maximum sized packet indicates
// end-of-file
return DONE;
}
}
} else {
// if the sequence is outright wrong, however...
std::cout << "Received packet with incorrect sequence. Expected "
<< mvSequence << " or lower. Received " << inpkt.sequence
<< std::endl;
outpkt = rejpkt(mvSequence);
}
}
// Send response packet
if (sendtoErr(mvSocket, &outpkt, sizeof(packet), 0, (sockaddr *)&mvAddr,
mvAddrLen) == -1)
{
std::cerr << "sendto (" << __LINE__ << "): " << strerror(errno)
<< std::endl;
return ERROR;
}
return RECV_PACKETS;
}
void IMU3000::setRegisterBit(byte regAdress, int bitPos, bool state)
{
byte _b;
readFrom(regAdress, 1, &_b);
if (state) {
_b |= (1 << bitPos);
}
else {
_b &= ~(1 << bitPos);
}
writeTo(regAdress, _b);
}
void MAG3110::setDataRate(byte dataRate, byte osRatio)
{
byte dr;
readFrom(MAG3110_REG_CTRL_REG1, 1, &dr);
dr &= ~(MAG3110_MASK_DR | MAG3110_MASK_OSR);
dr |= dataRate;
dr |= osRatio;
writeTo(MAG3110_REG_CTRL_REG1, dr);
}
ValueTree DrawableRectangle::createValueTree (ComponentBuilder::ImageProvider* imageProvider) const
{
ValueTree tree (valueTreeType);
ValueTreeWrapper v (tree);
v.setID (getComponentID());
writeTo (v, imageProvider, nullptr);
v.setRectangle (bounds, nullptr);
v.setCornerSize (cornerSize, nullptr);
return tree;
}
void be_master(int DoneSoFar) {
int i, start, end, rangeSize, lastNumberAssigned = DoneSoFar;
/* for 11 to 100, we want the rangeSize to be equal */
rangeSize = (DoneSoFar*DoneSoFar - DoneSoFar) / NPROC;
while(1) {
for (i=0; i < NPROC; i++) {
readFrom(DonePipe[i], &start);
readFrom(DonePipe[i], &end);
if (end > 0) {
printf("child #%d report completion to %d\n", i, end);
/* check that no number is missed */
if (start != DoneSoFar+1)
fprintf(stderr, "Error: some number is missed\n");
DoneSoFar = end;
rangeSize = (DoneSoFar*DoneSoFar - lastNumberAssigned) / NPROC;
}
/* goal reached, exit now */
if (DoneSoFar >= GOAL) exit(0);
/* update new ranges to hand out */
start = lastNumberAssigned + 1;
end = lastNumberAssigned + rangeSize;
if (end > GOAL)
end = GOAL;
printf("asking child #%d to do from %d to %d\n", i, start, end);
writeTo(AssignmentPipe[i], &start);
writeTo(AssignmentPipe[i], &end);
lastNumberAssigned = end;
}
}
}
void CAdxl345::setRate(float rate) {
byte _b, _s;
int v = (int) (rate / 6.25);
int r = 0;
while (v >>= 1) {
r++;
}
if (r <= 9) {
readFrom(DEVICE, ADXL345_BW_RATE, 1, &_b);
_s = (byte)(r + 6) | (_b & 0x00F0);
writeTo(DEVICE, ADXL345_BW_RATE, _s);
}
}
void Accelerometer::setRate(float rate){
byte _b,_s;
int v = (int) (rate / 6.25);
int r = 0;
while (v >>= 1)
{
r++;
}
if (r <= 9) {
readFrom(DEVICE, ADXL345_BW_RATE, 1, &_b);
_s = (byte) (r + 6) | (_b & B11110000);
writeTo(DEVICE, ADXL345_BW_RATE, _s);
}
}
void testErase()
{
int i;
uint16_t addr = 0x1F;
getReady();
// Enable write
shiftOutBits(EWEN_OPCODE, EWEN_OPCODE_BITS);
shiftOutBits(0, EWEN_ADDR_BITS);
standby();
if (writeTo(addr, addr)) {
stop();
getReady();
// Write successful -- continue
CPPUNIT_ASSERT_EQUAL(addr, readAt(addr));
stop();
getReady();
shiftOutBits(ERASE_OPCODE, ERASE_OPCODE_BITS);
shiftOutBits(addr, ERASE_ADDR_BITS);
standby();
for (i = 0; i < 200; i++) {
if (eeprom->read() & DO)
break;
//usec_delay(50);
}
if (i == 200) {
CPPUNIT_FAIL("EEPROM erase was not completed");
stop();
return;
}
standby();
shiftOutBits(EWDS_OPCODE, EWDS_OPCODE_BITS);
shiftOutBits(0, EWDS_ADDR_BITS);
stop();
getReady();
CPPUNIT_ASSERT_EQUAL((uint16_t)0xFFFF, readAt(addr));
}
else {
CPPUNIT_FAIL("EEPROM write was not completed");
}
stop();
}
int OsDatagramSocket::write(const char* buffer, int bufferLength)
{
int returnCode;
if(mSimulatedConnect)
{
returnCode = writeTo(buffer, bufferLength);
}
else
{
returnCode = OsSocket::write(buffer, bufferLength);
}
return(returnCode);
}
