void System::setupWatchdog()
{
cli();
resetWatchdog();
WDTCSR |= (1<<WDCE) | (1<<WDE); // enter WD configuration mode
WDTCSR = (1<<WDIE) | (1<<WDE) | (1<<WDP3); // enable WD interrupt, enable system reset, 4000ms time-out
sei();
}
void init()
{
Chip_SYSCTL_SetBODLevels(SYSCTL_BODRSTLVL_2_06V, SYSCTL_BODINTVAL_RESERVED1);
Chip_SYSCTL_EnableBODReset();
initWatchdog();
initClock();
initGpio();
resetWatchdog();
}
void System::update()
{
updateSensors();
unsigned long dt = millis() - lastRadioUpdate;
if(dt >= RADIO_UPDATE_RATE_MS) {
digitalWrite(INFO_LED_PIN, HIGH);
updatePacketData();
lastRadioUpdate = millis();
sendPacket();
digitalWrite(INFO_LED_PIN, LOW);
resetWatchdog();
#if DEBUG
Serial.print("Radio update, dt= ");
Serial.println(dt);
#endif
}
}
void
TcpHandler::handleConnect(const boost::system::error_code& error)
{
if (error) {
doClose(error);
} else {
unsigned int port = Options::commandPort();
if (port != 0) {
boost::asio::ip::tcp::endpoint cmdEndpoint(boost::asio::ip::tcp::v4(), port);
m_cmdHandler.reset(new CommandHandler(*this, cmdEndpoint));
m_pcMessageCallback = boost::bind(&CommandHandler::handlePcMessage,
m_cmdHandler, _1);
}
port = Options::dataPort();
if (port != 0) {
boost::asio::ip::tcp::endpoint dataEndpoint(boost::asio::ip::tcp::v4(), port);
m_dataHandler.reset(new DataHandler(*this, dataEndpoint));
m_valueCallback = boost::bind(&DataHandler::handleValue, m_dataHandler, _1);
}
resetWatchdog();
readStart();
}
}
void taskWatchdog( xCoRoutineHandle xHandle, unsigned portBASE_TYPE uxIndex )
{
crSTART( xHandle );
// Task loop
for ( ;; )
{
if ( g_wdLeft > 0 )
// Count down
g_wdLeft--;
else
{
// Turn all motors off
pst->pwm1 = 0;
pst->pwm2 = 0;
MOTORS_PORT &= ~( MOTORS_EN | SERVO_EN | MOTORS_MASK | SERVO_MASK );
// To begin new count down.
resetWatchdog();
}
crDELAY( xHandle, 100 );
}
crEND();
}
void
TcpHandler::readComplete(const boost::system::error_code& error, size_t bytesTransferred)
{
resetWatchdog();
IoHandler::readComplete(error, bytesTransferred);
}
bool Tasks::work(uint32_t exclude)
{
/*
* We sample pendingMask exactly once, and use this to quickly make
* the determination of whether any work is to be done or not.
*
* If we do have some amount of work to do, we atomically transfer
* all pending flags from pendingMask to iterationMask, where we
* clear the flags as we invoke each callback.
*
* This method is efficient in the common cases, and it is safe
* even in tricky edge cases like tasks which re-trigger themselves
* but may be cancelled by a different task. The iterationMask
* is also cancelled if we cancel a task, to handle the case where
* the cancelled task was already pulled from pendingMask.
*/
resetWatchdog();
// Quickest possible early-exit
uint32_t tasks = pendingMask & ~exclude;
if (LIKELY(!tasks))
return false;
/*
* Elapse some time in simulation. Besides factoring in as part of our
* timing model, this ensures that a Tasks::work() loop or Tasks::idle()
* will always elaps a nonzero amount of time, even if there's a task
* queued which is always triggered. This prevents us from getting into
* an infinite loop in such cases.
*/
#ifdef SIFTEO_SIMULATOR
SystemMC::elapseTicks(MCTiming::TICKS_PER_TASKS_WORK);
#endif
// Clear only the bits we managed to capture above
Atomic::And(pendingMask, ~tasks);
/*
* Merge the tasks captured above with any existing iterationMask.
* We need to be careful of edge cases introduced by running work()
* while already in a task handler. Tasks already in iterationMask
* but not yet run may need to be either kept or moved back
* to pendingMask, depending on whether they're excluded.
*
* Also note that iterationMask doesn't need to be atomic here, since
* we aren't required to catch changes to it made outside of task handlers.
*/
tasks |= iterationMask;
uint32_t pendingExcluded = tasks & exclude;
if (pendingExcluded) {
Atomic::Or(pendingMask, pendingExcluded);
tasks ^= pendingExcluded;
}
ASSERT((tasks & exclude) == 0);
iterationMask = tasks;
do {
unsigned idx = Intrinsic::CLZ(tasks);
taskInvoke(idx);
tasks = (iterationMask &= ~Intrinsic::LZ(idx));
} while (tasks);
return true;
}
