void readSensors(DataPacket_t *packet)
{
PRINTF("reading sensors...\n");
uint32_t start = getJiffies();
ledOn();
humidityOn();
packet->timestamp = getUptime();
if (!islRead(&packet->islLight, true)) {
PRINT("islRead failed\n");
packet->islLight = 0xffff;
// } else {
// PRINT("islRead OK\n");
}
packet->internalVoltage = adcRead(ADC_INTERNAL_VOLTAGE);
packet->internalTemperature = adcRead(ADC_INTERNAL_TEMPERATURE);
packet->sht75Humidity = humidityRead();
packet->sht75Temperature = temperatureRead();
humidityOff();
ledOff();
uint32_t end = getJiffies();
PRINTF(" time spent: %u ms\n", end - start);
}
uint16_t signalTriangleWave(uint16_t period, uint16_t low, uint16_t high)
{
const uint16_t amplitude = high - low;
const uint16_t halfPeriod = period / 2;
uint16_t positionX = getJiffies() % (halfPeriod + 1);
const bool invert = (getJiffies() % period) > halfPeriod;
if (invert) {
positionX = halfPeriod - positionX;
}
const uint16_t positionY = (positionX * amplitude) / halfPeriod + low;
return positionY;
}
void alarmCallback(void *param)
{
(void) param;
// lock the mutex in kernel context
uint32_t start, end;
start = getJiffies();
mutexLock(&testMutex);
end = getJiffies();
PRINTF("in alarm callback, mutex lock time=%lu\n", end - start);
mdelay(1300);
mutexUnlock(&testMutex);
// reschedule the alarm
alarmSchedule(&alarm, ALARM_INTERVAL);
}
//
// Calculate approximate sawtooth wave value at given point of time
//
uint16_t signalSawtoothWave(uint16_t period, uint16_t low, uint16_t high)
{
const uint16_t amplitude = high - low;
uint16_t positionX = getJiffies() % period;
const uint16_t positionY = (positionX * amplitude) / period + low;
return positionY;
}
void alarmSchedule(Alarm_t *alarm, uint32_t milliseconds)
{
// we want to avoid inserting local variables in the global alarm list
// (but this warning, not an error, because the user function may never return)
WARN_ON(isStackAddress(alarm));
// PRINTF("alarmSchedule %p, ms=%lu\n", alarm, milliseconds);
alarm->jiffies = (uint32_t)getJiffies() + milliseconds;
// locking is required, because both kernel and user threads can be using this function
Handle_t h;
ATOMIC_START(h);
// unschedule the alarm, if it was already scheduled
SLIST_REMOVE_SAFE(&alarmListHead, alarm, Alarm_s, chain);
// insert it in appropriate position
Alarm_t **prev = &SLIST_FIRST(&alarmListHead);
Alarm_t *a = *prev;
while (a && timeAfter32(alarm->jiffies, a->jiffies)) {
prev = &SLIST_NEXT(a, chain);
a = *prev;
}
SLIST_INSERT(prev, alarm, chain);
#if USE_THREADS
// always reschedule alarm processing in case some alarm was added
// that might need to be processed before end of current kernel sleep time
processFlags.bits.alarmsProcess = true;
// and make sure the kernel thread is awake and ready to deal with it
threadWakeup(KERNEL_THREAD_INDEX, THREAD_READY);
#endif
ATOMIC_END(h);
}
void alarmsProcess(void)
{
//
// Note that this code works in the way that neither locking the list
// nor disabling interrupts is required
//
uint32_t now = (uint32_t) getJiffies();
// PRINTF("processAlarms: jiffies=%lu\n", now);
Alarm_t **a = &SLIST_FIRST(&alarmListHead);
while (*a) {
Alarm_t *ap = *a;
// the alarm must have valid callback
ASSERT(ap->callback != NULL);
// PRINTF("processAlarms: ap=%p, ap->jiffies=%lu\n", ap, ap->jiffies);
if (timeAfter32(ap->jiffies, now)) {
break;
}
// save next alarm for later reference
Alarm_t **next = &SLIST_NEXT(ap, chain);
// remove the alarm from the list
*a = *next;
// call the callback
ap->callback(ap->data);
// move to the next
a = next;
}
}
static uint8_t markAsSeen(MosShortAddr address, bool addNew)
{
uint32_t now = (uint32_t) getJiffies();
// MOTE_INFO_VALID_TIME - ?
uint8_t i;
for (i = 0; i < MAX_MOTES; ++i) {
if (motes[i].address == address) {
break;
}
}
if (i == MAX_MOTES && addNew) {
for (i = 0; i < MAX_MOTES; ++i) {
if (motes[i].address == 0
|| timeAfter32(now, motes[i].validUntil)) {
break;
}
}
// save its address
motes[i].address = address;
}
if (i == MAX_MOTES) {
if (addNew) PRINTF("recv rreq: no more space!\n");
return 0xff;
}
// mark it as seen
motes[i].validUntil = now + MOTE_INFO_VALID_TIME;
return i;
}
void onAlarm(void *x) {
redLedToggle();
// schedule the next alarm *exactly* 1000 milliseconds after this one
nextPeriod += PERIOD;
alarmSchedule(&alarm, nextPeriod - getJiffies());
}
void appMain(void)
{
uint16_t i;
// ------------------------- serial number
DPRINTF("Mote %#04x starting...\n", localAddress);
// ------------------------- external flash
#if WRITE_TO_FLASH
prepareExtFlash();
#endif
// ------------------------- light sensors
if (localAddress != 0x0796) {
PRINT("init isl\n");
islInit();
islOn();
} else {
PRINT("init ads\n");
adsInit();
adsSelectInput(0);
}
// ------------------------- main loop
mdelay(3000);
DPRINTF("starting main loop...\n");
for (i = 0; i < 6; ++i) {
redLedToggle();
mdelay(100);
}
ledOff();
uint32_t nextDataReadTime = 0;
uint32_t nextBlinkTime = 0;
for (;;) {
uint32_t now = getRealTime();
if (timeAfter32(now, nextDataReadTime)) {
if (getJiffies() < 300 * 1000ul ) {
nextDataReadTime = now + DATA_INTERVAL_SMALL;
} else {
nextDataReadTime = now + DATA_INTERVAL;
}
DataPacket_t packet;
readSensors(&packet);
#if PRINT_PACKET
printPacket(&packet);
#endif
}
if (timeAfter32(now, nextBlinkTime)) {
nextBlinkTime = now + BLINK_INTERVAL;
ledOn();
msleep(100);
ledOff();
}
msleep(1000);
}
}
static inline bool isRoutingInfoValid(void)
{
if (hopCountToRoot >= MAX_HOP_COUNT) return false;
bool old = timeAfter32((uint32_t)getJiffies(), lastRootMessageTime + ROUTING_INFO_VALID_TIME);
if (old) {
hopCountToRoot = MAX_HOP_COUNT;
return false;
}
return true;
}
void appMain(void)
{
// initialize the alarm
alarmInit(&alarm, alarmCallback, NULL);
// schedule the alarm after specific interval
alarmSchedule(&alarm, ALARM_INTERVAL);
for (;;) {
ledToggle();
// lock the mutex in user context
uint32_t start, end;
start = getJiffies();
mutexLock(&testMutex);
end = getJiffies();
PRINTF("in user main, mutex lock time=%lu\n", end - start);
mdelay(1000);
mutexUnlock(&testMutex);
}
}
void readSensors(DataPacket_t *packet)
{
DPRINTF("reading sensors...\n");
ledOn();
humidityOn();
packet->timestamp = getJiffies();
packet->sourceAddress = localAddress;
packet->dataSeqnum = ++dataSeqnum;
if (localAddress != 0x0796) {
if (!islRead(&packet->islLight, true)) {
PRINT("islRead failed\n");
packet->islLight = 0xffff;
}
packet->sq100Light = 0xffff;
} else {
packet->islLight = 0xffff;
if (!readAds(&packet->sq100Light)) {
PRINT("readAdsRegister failed\n");
packet->sq100Light = 0xffff;
}
}
packet->internalVoltage = adcRead(ADC_INTERNAL_VOLTAGE);
packet->internalTemperature = adcRead(ADC_INTERNAL_TEMPERATURE);
DPRINT("read hum\n");
packet->sht75Humidity = humidityRead();
packet->sht75Temperature = temperatureRead();
DPRINT("read done\n");
packet->crc = crc16((uint8_t *) packet, sizeof(*packet) - 2);
#if WRITE_TO_FLASH
if (extFlashAddress < EXT_FLASH_SIZE) {
DPRINT("Writing to flash\n");
extFlashWrite(extFlashAddress, packet, sizeof(*packet));
DataPacket_t verifyRecord;
memset(&verifyRecord, 0, sizeof(verifyRecord));
extFlashRead(extFlashAddress, &verifyRecord, sizeof(verifyRecord));
if (memcmp(packet, &verifyRecord, sizeof(verifyRecord))) {
ASSERT("writing in flash failed!" && false);
}
extFlashAddress += sizeof(verifyRecord);
}
#endif
humidityOff();
ledOff();
}
void loop() {
unsigned long jiffies = getJiffies();
if (jiffies % 50 == 0)
{
LED_PORT.OUTTGL = LED_USR_0_PIN_bm; // toggle red light every 50 ms (20 Hz)
servo_motor_control();
}
if (jiffies % 100 == 0)
{
swarm_communication();
swarm_calculation();
}
}
void appMain(void)
{
redLedToggle();
// initialize and schedule the alarm
alarmInit(&alarm, onAlarm, NULL);
nextPeriod = PERIOD;
// jiffie counter starts from 0 (zero) after reset
alarmSchedule(&alarm, PERIOD - getJiffies());
// enter infinite low-power loop
for (;;) {
sleep(10);
}
}
uint32_t readSensorU32(uint16_t code, ReadFunctionU32 func, uint16_t expireTime)
{
uint32_t now = getJiffies();
SensorCache_t *cacheValue = &sensorCache[code];
if (cacheValue->expireTime && !timeAfter32(now, cacheValue->expireTime)) {
// take from cache
return cacheValue->value.u32;
}
uint32_t result = func();
if (expireTime) {
// add to cache
cacheValue->value.u32 = result;
cacheValue->expireTime = now + expireTime;
}
return result;
}
int32_t cacheReadSensor32(uint16_t code, ReadFunction32 func,
uint16_t expireTime, bool *isFilteredOut)
{
ticks_t now = getJiffies();
SensorCache_t *cacheValue = &sensorCache[code];
if (cacheValue->expireTime && !timeAfter(now, cacheValue->expireTime)) {
// take from cache
return cacheValue->value.i32;
}
int32_t result = func(isFilteredOut);
if (expireTime) {
// add to cache
cacheValue->value.i32 = result;
cacheValue->expireTime = now + expireTime;
}
return result;
}
static void roForwardTimerCb(void *x)
{
static uint8_t moteToProcess;
bool roOK = isRoutingInfoValid();
if (motes[moteToProcess].address != 0
&& timeAfter32(motes[moteToProcess].validUntil, (uint32_t)getJiffies())
&& hopCountToRoot < MAX_HOP_COUNT
&& roOK) {
forwardRoutingInfo(moteToProcess);
}
moteToProcess++;
if (moteToProcess == MAX_MOTES) moteToProcess = 0;
alarmSchedule(&roForwardTimer, calcNextForwardTime(moteToProcess));
}
static void routingReceive(Socket_t *s, uint8_t *data, uint16_t len)
{
// PRINTF("routingReceive %d bytes from %#04x\n", len,
// s->recvMacInfo->originalSrc.shortAddr);
// PRINTF("routing rx\n");
if (len == 0) {
PRINTF("routingReceive: no data!\n");
return;
}
#if PRECONFIGURED_NH_TO_ROOT
if (s->recvMacInfo->originalSrc.shortAddr != PRECONFIGURED_NH_TO_ROOT) {
PRINTF("Dropping routing info: not from the nexthop, but from %#04x\n",
s->recvMacInfo->originalSrc.shortAddr);
return;
}
PRINTF("Got routing info from the nexthop\n");
#endif
uint8_t type = data[0];
if (type == ROUTING_REQUEST) {
uint8_t senderType = data[1];
if (senderType != SENDER_MOTE) return;
uint8_t idx = markAsSeen(s->recvMacInfo->originalSrc.shortAddr, true);
if (gotRreq == 0xff) {
gotRreq = idx;
alarmSchedule(&roOutOfOrderForwardTimer, randomNumberBounded(400));
}
return;
}
if (type != ROUTING_INFORMATION) {
PRINTF("routingReceive: unknown type!\n");
return;
}
if (len < sizeof(RoutingInfoPacket_t)) {
PRINTF("routingReceive: too short for info packet!\n");
return;
}
RoutingInfoPacket_t ri;
memcpy(&ri, data, sizeof(RoutingInfoPacket_t));
bool update = false;
if (!isRoutingInfoValid() || timeAfter16(ri.seqnum, lastSeenSeqnum)) {
// XXX: theoretically should add some time to avoid switching to
// worse path only because packets from it travel faster
update = true;
TPRINTF("RI updated: > seqnum\n");
}
else if (ri.seqnum == lastSeenSeqnum) {
if (ri.hopCount < hopCountToRoot) {
update = true;
TPRINTF("RI updated: < metric\n");
}
else if (ri.hopCount == hopCountToRoot && !seenRoutingInThisFrame) {
update = true;
TPRINTF("RI updated: == metric\n");
}
}
if (ri.hopCount > MAX_HOP_COUNT) update = false;
if (update) {
if (timeSinceFrameStart() < 2000 || timeSinceFrameStart() > 4000) {
PRINTF("*** forwarder (?) sends out of time!\n");
}
seenRoutingInThisFrame = true;
rootAddress = ri.rootAddress;
nexthopToRoot = s->recvMacInfo->originalSrc.shortAddr;
lastSeenSeqnum = ri.seqnum;
hopCountToRoot = ri.hopCount;
lastRootMessageTime = (uint32_t) getJiffies();
int64_t oldRootClockDeltaMs = rootClockDeltaMs;
rootClockDeltaMs = ri.rootClockMs - getTimeMs64();
if (abs((int32_t)oldRootClockDeltaMs - (int32_t)rootClockDeltaMs) > 500) {
PRINTF("large delta change=%ld, time sync off?!\n", (int32_t)rootClockDeltaMs - (int32_t)oldRootClockDeltaMs);
PRINTF("delta: old=%ld, new=%ld\n", (int32_t)oldRootClockDeltaMs, (int32_t)rootClockDeltaMs);
}
// TPRINTF("OK!%s\n", isListening ? "" : " (not listening)");
// reschedule next listen start after this timesync
alarmSchedule(&roStartListeningTimer, timeToNextFrame() + 2000);
}
else {
TPRINTF("RI not updated!\n");
}
}
uint32_t getAlarmTime(Alarm_t *alarm)
{
return (uint32_t)(alarm->jiffies - getJiffies());
}