/// Retrieve a condition given a Detector Element and the conditions name
RangeConditions
ConditionsManagerObject::getRange(Condition::key_type key, const Condition::iov_type& iov)
{
RC conditions;
__check_values__<Range>(this, key, &iov);
bool rc = select_range(key, iov, conditions);
if ( rc ) {
return conditions;
}
else {
dd4hep_lock_t locked_load(m_updateLock);
m_loader->load_range(key, iov, conditions);
if ( conditions.empty() ) {
except("ConditionsManager","+++ Conditions %08X for IOV %s do not exist.",
key, iov.str().c_str());
}
conditions.clear();
}
rc = select_range(key, iov, conditions);
if ( !rc ) {
except("ConditionsManager","+++ Conditions %08X for IOV %s do not exist.",
key, iov.str().c_str());
}
return conditions;
}
SP(const SP<T>& sp) : pData(sp.pData), reference(sp.reference)
{
// Copy constructor
// Copy the data and reference pointer
// and increment the reference count
reference->AddRef();
}
SP(T* pValue) : pData(pValue), reference(0)
{
// Create a new reference
reference = new RC();
// Increment the reference count
reference->AddRef();
}
void setup(void)
{
board.init();
// sleep for 100ms
board.delayMilliseconds(100);
// flash the LEDs to indicate startup
board.ledRedOn();
board.ledGreenOff();
for (uint8_t i = 0; i < 10; i++) {
board.ledRedToggle();
board.ledGreenToggle();
board.delayMilliseconds(50);
}
board.ledRedOff();
board.ledGreenOff();
// initialize our RC, IMU, mixer, and PID controller
rc.init(&board);
imu.init(&board);
pid.init();
mixer.init(&board, &rc, &pid);
msp.init(&board, &imu, &mixer, &rc);
// set initial time
previousTime = board.getMicros();
// always do gyro calibration at startup
calibratingG = CONFIG_CALIBRATING_GYRO_CYCLES;
// trigger accelerometer calibration requirement
haveSmallAngle = true;
} // setup
TypeIdentifierSyntax
TypeIdentifierSyntax::withIdentifier(RC<TokenSyntax> NewIdentifier) const {
assert(NewIdentifier->getTokenKind() == tok::identifier);
auto NewRaw = getRaw()->replaceChild(Cursor::Identifier,
NewIdentifier);
return Data->replaceSelf<TypeIdentifierSyntax>(NewRaw);
}
SP<T>& reset ()
{
if(reference->Release() == 0)
{
delete pData;
delete reference;
}
}
~SP()
{
// if reference become zero delete the data
if(reference->Release() == 0)
{
delete pData;
delete reference;
}
}
void
Semantics::recordSyntaxMapping(RC<syntax::SyntaxData> FromNode,
ASTNode ToNode) {
if (FromNode->getKind() == SyntaxKind::Unknown) {
return;
}
SyntaxMap[FromNode] = ToNode;
}
~SP()
{
// Destructor
// Decrement the reference count
// if reference become zero delete the data
if(reference->Release() == 0)
{
delete pData;
delete reference;
}
}
SP<T>& operator = (const SP<T>& sp)
{
// Assignment operator
if (this != &sp) // Avoid self assignment
{
// Decrement the old reference count
// if reference become zero delete the old data
if(reference->Release() == 0)
{
delete pData;
delete reference;
}
// Copy the data and reference pointer
// and increment the reference count
pData = sp.pData;
reference = sp.reference;
reference->AddRef();
}
return *this;
}
/// Retrieve a condition given a Detector Element and the conditions name
Condition
ConditionsManagerObject::get(Condition::key_type key, const Condition::iov_type& iov)
{
RC conditions;
__check_values__<Discrete>(this, key, &iov);
bool rc = select(key, iov, conditions);
if ( !rc ) {
dd4hep_lock_t locked_load(m_updateLock);
m_loader->load(key, iov, conditions);
}
if ( conditions.size() == 1 ) {
conditions[0]->flags |= Condition::ACTIVE;
return conditions[0];
}
else if ( conditions.empty() ) {
except("ConditionsManager","+++ Condition %08X for the requested IOV %s do not exist.",
key, iov.str().c_str());
}
else if ( conditions.size() > 1 ) {
RC::const_iterator start = conditions.begin();
Condition first = *start;
printout(ERROR,"ConditionsManager","+++ Condition %s [%08X] is ambiguous for IOV %s:",
first.name().c_str(), key, iov.str().c_str());
for(RC::const_iterator i=start; i!=conditions.end(); ++i) {
Condition c = *i;
printout(ERROR,"ConditionsManager","+++ %s [%s] = %s",
c.name().c_str(), c->iov->str().c_str(), c->value.c_str());
}
except("ConditionsManager","+++ Condition %s [%08X] is ambiguous for IOV %s:",
first.name().c_str(), key, iov.str().c_str());
}
return Condition();
}
void setup(void)
{
uint32_t calibratingGyroMsec;
// Get particulars for board
board.init(imuLooptimeUsec, calibratingGyroMsec);
// sleep for 100ms
board.delayMilliseconds(100);
// flash the LEDs to indicate startup
board.ledRedOn();
board.ledGreenOff();
for (uint8_t i = 0; i < 10; i++) {
board.ledRedToggle();
board.ledGreenToggle();
board.delayMilliseconds(50);
}
board.ledRedOff();
board.ledGreenOff();
// compute cycles for calibration based on board's time constant
calibratingGyroCycles = (uint16_t)(1000. * calibratingGyroMsec / imuLooptimeUsec);
calibratingAccCycles = (uint16_t)(1000. * CONFIG_CALIBRATING_ACC_MSEC / imuLooptimeUsec);
// initializing timing tasks
imuTask.init(imuLooptimeUsec);
rcTask.init(CONFIG_RC_LOOPTIME_MSEC * 1000);
accelCalibrationTask.init(CONFIG_CALIBRATE_ACCTIME_MSEC * 1000);
altitudeEstimationTask.init(CONFIG_ALTITUDE_UPDATE_MSEC * 1000);
// initialize our external objects with objects they need
rc.init(&board);
stab.init(&rc, &imu);
imu.init(&board, calibratingGyroCycles, calibratingAccCycles);
mixer.init(&board, &rc, &stab);
msp.init(&board, &imu, &nav, &mixer, &rc);
nav.init(&board, &imu, &baro, &rc);
// always do gyro calibration at startup
calibratingG = calibratingGyroCycles;
// assume shallow angle (no accelerometer calibration needed)
haveSmallAngle = true;
// ensure not armed
armed = false;
// attempt to initialize barometer
baro.init(&board);
} // setup
SameTypeRequirementSyntaxData::
SameTypeRequirementSyntaxData(RC<RawSyntax> Raw,
const SyntaxData *Parent,
CursorIndex IndexInParent)
: GenericRequirementSyntaxData(Raw, Parent, IndexInParent) {
assert(Raw->Kind == SyntaxKind::SameTypeRequirement);
assert(Raw->Layout.size() == 3);
syntax_assert_child_kind(Raw,
SameTypeRequirementSyntax::Cursor::LeftTypeIdentifier,
SyntaxKind::TypeIdentifier);
syntax_assert_child_token_text(Raw,
SameTypeRequirementSyntax::Cursor::EqualityToken,
tok::oper_binary_spaced, "==");
assert(Raw->getChild(SameTypeRequirementSyntax::Cursor::RightType)->isType());
}
BalancedTokensSyntax
BalancedTokensSyntax::addBalancedToken(RC<TokenSyntax> NewBalancedToken) const {
#ifndef NDEBUG
assert(NewBalancedToken->getTokenKind() != tok::l_paren);
assert(NewBalancedToken->getTokenKind() != tok::r_paren);
assert(NewBalancedToken->getTokenKind() != tok::l_square);
assert(NewBalancedToken->getTokenKind() != tok::r_square);
assert(NewBalancedToken->getTokenKind() != tok::l_brace);
assert(NewBalancedToken->getTokenKind() != tok::r_brace);
auto IsIdentifier = NewBalancedToken->getTokenKind() == tok::identifier;
auto IsKeyword = NewBalancedToken->isKeyword();
auto IsLiteral = NewBalancedToken->isLiteral();
auto IsOperator = NewBalancedToken->isOperator();
auto IsPunctuation = NewBalancedToken->isPunctuation();
assert(IsIdentifier || IsKeyword || IsLiteral || IsOperator ||
IsPunctuation);
#endif
auto Layout = getRaw()->Layout;
Layout.push_back(NewBalancedToken);
auto NewRaw = RawSyntax::make(SyntaxKind::BalancedTokens, Layout,
SourcePresence::Present);
return Data->replaceSelf<BalancedTokensSyntax>(NewRaw);
}
TupleTypeElementSyntaxData::
TupleTypeElementSyntaxData(RC<RawSyntax> Raw,
const SyntaxData *Parent,
CursorIndex IndexInParent)
: SyntaxData(Raw, Parent, IndexInParent) {
assert(Raw->Kind == SyntaxKind::TupleTypeElement);
assert(Raw->Layout.size() == 6);
syntax_assert_child_token(Raw, TupleTypeElementSyntax::Cursor::Label,
tok::identifier);
syntax_assert_child_token_text(Raw,
TupleTypeElementSyntax::Cursor::ColonToken,
tok::colon, ":");
syntax_assert_child_kind(Raw, TupleTypeElementSyntax::Cursor::Attributes,
SyntaxKind::TypeAttributes);
syntax_assert_child_token_text(Raw,
TupleTypeElementSyntax::Cursor::InoutToken,
tok::kw_inout,
"inout");
syntax_assert_child_token_text(Raw,
TupleTypeElementSyntax::Cursor::CommaToken,
tok::comma, ",");
assert(Raw->getChild(TupleTypeElementSyntax::Cursor::Type)->isType());
}
TypeAttributeSyntax
TypeAttributeSyntax::withIdentifier(RC<TokenSyntax> NewIdentifier) const {
assert(NewIdentifier->getTokenKind() == tok::identifier);
return Data->replaceChild<TypeAttributeSyntax>(NewIdentifier,
Cursor::Identifier);
};
void loop(void)
{
static bool accCalibrated;
static uint16_t calibratingA;
static uint32_t currentTime;
static uint32_t disarmTime;
if (rcTask.checkAndUpdate(currentTime)) {
// update RC channels
rc.update();
// when landed, reset integral component of PID
if (rc.throttleIsDown())
stab.resetIntegral();
if (rc.changed()) {
if (armed) { // actions during armed
// Disarm on throttle down + yaw
if (rc.sticks == THR_LO + YAW_LO + PIT_CE + ROL_CE) {
if (armed) {
armed = false;
// Reset disarm time so that it works next time we arm the board.
if (disarmTime != 0)
disarmTime = 0;
}
}
} else { // actions during not armed
// gyro calibration
if (rc.sticks == THR_LO + YAW_LO + PIT_LO + ROL_CE)
calibratingG = calibratingGyroCycles;
// Arm via throttle-low / yaw-right
if (rc.sticks == THR_LO + YAW_HI + PIT_CE + ROL_CE)
if (calibratingG == 0 && accCalibrated)
if (!rc.auxState()) // aux switch must be in zero position
if (!armed)
armed = true;
// accel calibration
if (rc.sticks == THR_HI + YAW_LO + PIT_LO + ROL_CE)
calibratingA = calibratingAccCycles;
} // not armed
} // rc.changed()
// Switch to alt-hold when switch moves to position 1 or 2
nav.checkSwitch();
} else { // not in rc loop
static int taskOrder; // never call all functions in the same loop, to avoid high delay spikes
switch (taskOrder) {
case 0:
if (baro.available())
baro.update();
taskOrder++;
break;
case 1:
if (baro.available() && altitudeEstimationTask.checkAndUpdate(currentTime)) {
nav.updateAltitudePid(armed);
}
taskOrder++;
break;
case 2:
taskOrder++;
break;
case 3:
taskOrder++;
break;
case 4:
taskOrder = 0;
break;
}
}
currentTime = board.getMicros();
if (imuTask.checkAndUpdate(currentTime)) {
imu.update(currentTime, armed, calibratingA, calibratingG);
haveSmallAngle = abs(imu.angle[0]) < CONFIG_SMALL_ANGLE && abs(imu.angle[1]) < CONFIG_SMALL_ANGLE;
// measure loop rate just afer reading the sensors
currentTime = board.getMicros();
// compute exponential RC commands
rc.computeExpo();
// use LEDs to indicate calibration status
if (calibratingA > 0 || calibratingG > 0)
board.ledGreenOn();
else {
if (accCalibrated)
board.ledGreenOff();
if (armed)
board.ledRedOn();
else
board.ledRedOff();
}
// periodically update accelerometer calibration status
if (accelCalibrationTask.check(currentTime)) {
if (!haveSmallAngle) {
accCalibrated = false;
board.ledGreenToggle();
accelCalibrationTask.update(currentTime);
} else {
accCalibrated = true;
}
}
// handle serial communications
msp.update(armed);
// perform navigation tasks (alt-hold etc.)
nav.perform();
// update stability PID controller
stab.update();
// update mixer
mixer.update(armed);
} // IMU update
} // loop()
void loop(void)
{
static uint32_t rcTime = 0;
static uint32_t loopTime;
static uint32_t calibratedAccTime;
static bool accCalibrated;
static bool armed;
static uint16_t calibratingA;
static uint32_t currentTime;
static uint32_t disarmTime = 0;
if (check_and_update_timed_task(&rcTime, CONFIG_RC_LOOPTIME_USEC, currentTime)) {
// update RC channels
rc.update();
// when landed, reset integral component of PID
if (rc.throttleIsDown())
pid.resetIntegral();
if (rc.changed()) {
if (armed) { // actions during armed
// Disarm on throttle down + yaw
if (rc.sticks == THR_LO + YAW_LO + PIT_CE + ROL_CE) {
if (armed) {
armed = false;
// Reset disarm time so that it works next time we arm the board.
if (disarmTime != 0)
disarmTime = 0;
}
}
} else { // actions during not armed
// GYRO calibration
if (rc.sticks == THR_LO + YAW_LO + PIT_LO + ROL_CE) {
calibratingG = CONFIG_CALIBRATING_GYRO_CYCLES;
}
// Arm via YAW
if ((rc.sticks == THR_LO + YAW_HI + PIT_CE + ROL_CE)) {
if (calibratingG == 0 && accCalibrated) {
if (!armed) { // arm now!
armed = true;
}
} else if (!armed) {
blinkLED(2, 255, 1);
}
}
// Calibrating Acc
else if (rc.sticks == THR_HI + YAW_LO + PIT_LO + ROL_CE)
calibratingA = CONFIG_CALIBRATING_ACC_CYCLES;
} // not armed
} // rc.changed()
} else { // not in rc loop
static int taskOrder; // never call all functions in the same loop, to avoid high delay spikes
switch (taskOrder) {
case 0:
taskOrder++;
//sensorsGetBaro();
case 1:
taskOrder++;
//sensorsGetSonar();
case 2:
taskOrder++;
case 3:
taskOrder++;
case 4:
taskOrder = 0;
break;
}
}
currentTime = board.getMicros();
if (check_and_update_timed_task(&loopTime, CONFIG_IMU_LOOPTIME_USEC, currentTime)) {
imu.update(armed, calibratingA, calibratingG);
haveSmallAngle = abs(imu.angle[0]) < CONFIG_SMALL_ANGLE && abs(imu.angle[1]) < CONFIG_SMALL_ANGLE;
// measure loop rate just afer reading the sensors
currentTime = board.getMicros();
previousTime = currentTime;
// compute exponential RC commands
rc.computeExpo();
// use LEDs to indicate calibration status
if (calibratingA > 0 || calibratingG > 0) {
board.ledGreenToggle();
} else {
if (accCalibrated)
board.ledGreenOff();
if (armed)
board.ledRedOn();
else
board.ledRedOff();
}
if (check_timed_task(calibratedAccTime, currentTime)) {
if (!haveSmallAngle) {
accCalibrated = false; // accelerometer not calibrated or angle too steep
board.ledGreenToggle();
update_timed_task(&calibratedAccTime, CONFIG_CALIBRATE_ACCTIME_USEC, currentTime);
} else {
accCalibrated = true;
}
}
// handle serial communications
msp.update(armed);
// update PID controller
pid.update(&rc, &imu);
// update mixer
mixer.update(armed);
} // IMU update
} // loop()
DeclSyntaxData::DeclSyntaxData(RC<RawSyntax> Raw,
const SyntaxData *Parent,
CursorIndex IndexInParent)
: SyntaxData(Raw, Parent, IndexInParent) {
assert(Raw->isDecl());
}
AbsolutePosition RawSyntax::getAbsolutePosition(RC<RawSyntax> Root) const {
AbsolutePosition Pos;
Root->accumulateAbsolutePosition(Pos, this);
return Pos;
}
TupleTypeElementSyntax
TupleTypeElementSyntax::withLabel(RC<TokenSyntax> NewLabel) const {
assert(NewLabel->getTokenKind() == tok::identifier);
auto NewRaw = getRaw()->replaceChild(Cursor::Label, NewLabel);
return Data->replaceSelf<TupleTypeElementSyntax>(NewRaw);
}
SP(const SP<T>& sp) : pData(sp.pData), reference(sp.reference)
{
reference->AddRef();
}
TypeAttributeSyntax TypeAttributeSyntax::
withRightParenToken(RC<TokenSyntax> NewRightParenToken) const {
assert(NewRightParenToken->getTokenKind() == tok::r_paren);
return Data->replaceChild<TypeAttributeSyntax>(NewRightParenToken,
Cursor::RightParenToken);
};
