static msg_t thread8(void *p) {
do {
chThdYield();
chThdYield();
chThdYield();
chThdYield();
(*(uint32_t *)p) += 4;
#if defined(SIMULATOR)
ChkIntSources();
#endif
} while(!chThdShouldTerminate());
return 0;
}
int main(void)
{
halInit();
chSysInit();
palSetPadMode(GPIOA, GPIOA_BUTTON, PAL_MODE_INPUT_PULLDOWN);
palSetPadMode(GPIOD, GPIOD_LED4, PAL_MODE_OUTPUT_PUSHPULL);
palSetPadMode(GPIOD, GPIOD_LED3, PAL_MODE_OUTPUT_PUSHPULL);
palSetPadMode(GPIOD, GPIOD_LED5, PAL_MODE_OUTPUT_PUSHPULL);
palSetPadMode(GPIOD, GPIOD_LED6, PAL_MODE_OUTPUT_PUSHPULL);
chThdSleepSeconds(1);
initUsbShell();
SPIInit();
fc_nrf_init(NULL, NRF_MODE_PTX);
if(fc_nrf_test_spi_connection() == 1) {
palSetPad(GPIOD, GPIOD_LED3);
}
while (TRUE)
{
palTogglePad(GPIOD, GPIOD_LED4);
keepShellAlive();
chThdYield();
}
}
void gfxSleepMilliseconds(delaytime_t ms) {
switch(ms) {
case TIME_IMMEDIATE: chThdYield(); return;
case TIME_INFINITE: chThdSleep(TIME_INFINITE); return;
default: chThdSleepMilliseconds(ms); return;
}
}
static msg_t Thread2(void *arg) {
while (TRUE) {
count++;
uint32_t t = micros();
// yield so other threads can run
chThdYield();
t = micros() - t;
if (t > maxDelay) maxDelay = t;
}
}
// move to the given setpoint
void motionTarget (const Setpoint& s) {
LPC_TMR32B0->MR3 = ~0; // set timer count to max, so it won't change now
// determine target step to end on, rounding from a 24.8 to a 24.0 int
int target = posToStep(s.position);
if (s.relative)
target += motionPosition();
// enforce soft position limits
if (target < posToStep(motion.params.minPos)) {
target = posToStep(motion.params.minPos);
motion.errors |= 1<<F_HITMIN; // bumped into min position limit
}
if (target > posToStep(motion.params.maxPos)) {
target = posToStep(motion.params.maxPos);
motion.errors |= 1<<F_HITMAX; // bumped into max position limit
}
// set the next target
int stepDiff = target - motionPosition();
motion.direction = stepDiff > 0 ? 1 : -1;
motion.stepsToGo = stepDiff * motion.direction;
motion.targetStep = target;
if (motion.stepsToGo > 0) {
uint32_t clocks = MHZ * USPT * s.time + motion.residue;
uint32_t rate = (clocks + motion.stepsToGo / 2) / motion.stepsToGo;
motion.residue = clocks - rate * motion.stepsToGo;
if (rate < MHZ * 10) { // enforce a soft timer rate limit
rate = MHZ * 10; // at least 10 µs, i.e. max 100 KHz
motion.errors |= 1<<F_TOOFAST; // rate limiting took place
}
// TODO: could check that rate > TC, i.e. no overrun has occurred
LPC_TMR32B0->MR3 = rate;
palWritePad(GPIO1, GPIO1_MOTOR_DIR, stepDiff > 0 ? 1 : 0);
palClearPad(GPIO3, GPIO3_MOTOR_EN); // active low, on
LPC_TMR32B0->TCR = 1; // start timer if it wasn't running
while (motion.stepsToGo > 0) // move!
chThdYield(); // uses idle polling
} else if (s.time > 0) {
LPC_TMR32B0->TCR = 0; // stop timer
chThdSleepMilliseconds(s.time); // dwell
}
if (s.velocity == 0)
palSetPad(GPIO3, GPIO3_MOTOR_EN); // active low, off
}
static THD_FUNCTION(can_reader_thread, arg)
{
t_hydra_console *con;
con = arg;
chRegSetThreadName("CAN reader");
chThdSleepMilliseconds(10);
can_rx_frame rx_msg;
mode_config_proto_t* proto = &con->mode->proto;
while (!chThdShouldTerminateX()) {
if(bsp_can_rxne(proto->dev_num)) {
chSysLock();
bsp_can_read(proto->dev_num, &rx_msg);
can_slcan_out(con, &rx_msg);
chSysUnlock();
} else {
chThdYield();
}
}
chThdExit((msg_t)1);
}
int main () {
wiringPiSetup();
int myFd = wiringPiSPISetup (0, 4000000);
if (myFd < 0) {
printf("Can't open the SPI bus: %d\n", errno);
return 1;
}
printf("\n[rf69try]\n");
rf.init(1, 42, 8686);
//rf.encrypt("mysecret");
rf.txPower(15); // 0 = min .. 31 = max
uint16_t cnt = 0;
uint8_t txBuf[62];
for (int i = 0; i < sizeof txBuf; ++i)
txBuf[i] = i;
while (true) {
if (++cnt % 1024 == 0) {
int txLen = ++txBuf[0] % (sizeof txBuf + 1);
printf(" > #%d, %db\n", txBuf[0], txLen);
rf.send(0, txBuf, txLen);
}
int len = rf.receive(rxBuf, sizeof rxBuf);
if (len >= 0) {
printf("OK ");
for (int i = 0; i < len; ++i)
printf("%02x", rxBuf[i]);
printf(" (%d%s%d:%d)\n",
rf.rssi, rf.afc < 0 ? "" : "+", rf.afc, rf.lna);
}
chThdYield();
}
}
static THD_FUNCTION(bridge_thread, arg)
{
t_hydra_console *con;
con = arg;
chRegSetThreadName("UART reader");
chThdSleepMilliseconds(10);
uint8_t rx_data[UART_BRIDGE_BUFF_SIZE];
uint8_t bytes_read;
mode_config_proto_t* proto = &con->mode->proto;
while (!USER_BUTTON) {
if(bsp_uart_rxne(proto->dev_num)) {
bytes_read = bsp_uart_read_u8_timeout(proto->dev_num,
rx_data,
UART_BRIDGE_BUFF_SIZE,
US2ST(100));
if(bytes_read > 0) {
cprint(con, (char *)rx_data, bytes_read);
}
} else {
chThdYield();
}
}
}
void Debug::start()
{
byte param = 0;
char readChar = 0;
s_DebugMode = true;
// Print help
printHelp();
while (s_DebugMode == true) {
readChar = 0;
param = 0;
chThdYield();
// Process data from Serial
if (Serial.available() < 1) {
chThdYield();
continue;
}
byte inChar = Serial.read();
switch (inChar) {
case 'h':
printHelp();
break;
case 'a':
param = Serial.parseInt();
s_Alarm.setLevel((Alarm::AlarmLevel)param);
Serial.print("Setting Alarm to level ");
Serial.println(param);
break;
case 'b':
param = Serial.parseInt();
s_Buzzer.setTone((Buzzer::BuzzerTone)param);
Serial.print("Setting Buzzer to tone ");
Serial.println(param);
break;
case 'r':
param = Serial.parseInt();
s_LEDs.setPattern(LEDs::LEDRed, (LEDs::BlinkType)param);
Serial.println("Setting Red LED to pattern ");
Serial.println(param);
break;
case 'g':
param = Serial.parseInt();
s_LEDs.setPattern(LEDs::LEDGreen, (LEDs::BlinkType)param);
Serial.println("Setting Green LED to pattern ");
Serial.println(param);
break;
case 'k':
Serial.println("Reading Keypad. Press any key on Keypad.");
while (readChar == 0) {
readChar = s_Keypad.getKey();
}
Serial.print("Key pressed is: ");
Serial.println(readChar);
break;
case 's':
param = Serial.parseInt();
Serial.println("Switching comm. system.");
s_Radio.switchCommSystem(param);
break;
case 'q':
Serial.println("Exiting debug mode.");
s_DebugMode = false;
break;
default:
Serial.println("Invalid command");
}
chThdYield();
// Drain buffer before looping.
drainSerial();
}
}
/*
* @brief Radio thread.
* @details This thread performs switch between different states
of radio module. It also executes defined callbacks.
*/
static void radio_thread(void *radio_driver) {
//
RadioDriver *radio = (RadioDriver *)radio_driver;
radio_callback_t cb = NULL;
//
if (radio->config->name != NULL) {
chRegSetThreadName(radio->config->name);
}
consoleDebug("started\r\n");
//
while (true) {
consoleDevel("loop start\r\n");
cb = NULL;
if (ithacaLock(&radio->lock) == true) {
switch (radio->state) {
case RADIO_UNINIT:
consoleDebug("RADIO_UNINIT\r\n");
if (radio_lld_init(radio) == true) {
radio->state = RADIO_STOP;
consoleDevel("init ok\r\n");
} else {
consoleDebug("init failed\r\n");
}
break;
case RADIO_STOP:
consoleDebug("RADIO_STOP\r\n");
if (radioIdleI(radio) == true) {
consoleDevel("stop -> idle ok\r\n");
} else {
consoleWarn("stop -> idle failed\r\n");
radio->state = RADIO_UNINIT;
}
break;
case RADIO_IDLE:
consoleDebug("RADIO_IDLE\r\n");
if (radio->config->idle_cb != NULL) {
cb = radio->config->idle_cb;
} else {
consoleDevel("idle_cb == NULL\r\n");
}
break;
case RADIO_RX:
consoleDebug("RADIO_RX\r\n");
if ((radio_lld_is_error(radio) == true) ||
(radio_lld_is_timeout_expired(radio) == true)) {
if (radio->config->recv_error_cb != NULL) {
cb = radio->config->recv_error_cb;
} else {
radio->state = RADIO_ERROR;
consoleDevel("recv_error_cb == NULL\r\n");
}
consoleWarn("rx failed\r\n");
} else if (radio_lld_receive_is_completed(radio) == true) {
if (radio->config->recv_done_cb != NULL) {
cb = radio->config->recv_done_cb;
} else {
radio->state = RADIO_IDLE;
consoleDevel("recv_done_cb == NULL\r\n");
}
consoleDebug("rx completed ok\r\n");
} else {
consoleDevel("rx in progress\r\n");
}
break;
case RADIO_TX:
consoleDebug("RADIO_TX\r\n");
if ((radio_lld_is_error(radio) == true) ||
(radio_lld_is_timeout_expired(radio) == true)) {
if (radio->config->send_error_cb != NULL) {
cb = radio->config->send_error_cb;
} else {
radio->state = RADIO_ERROR;
consoleDevel("send_error_cb == NULL\r\n");
}
consoleWarn("tx failed\r\n");
} else if (radio_lld_send_is_completed(radio) == true) {
if (radio->config->send_done_cb != NULL) {
cb = radio->config->send_done_cb;
} else {
radio->state = RADIO_IDLE;
}
consoleDebug("tx completed ok\r\n");
} else {
consoleDevel("tx in progress\r\n");
}
break;
case RADIO_ERROR:
consoleDebug("RADIO_ERROR\r\n");
if (radioIdleI(radio) == true) {
consoleDevel("error -> idle ok\r\n");
} else {
consoleWarn("eror -> idle failed\r\n");
radio->state = RADIO_UNINIT;
}
break;
}
if (cb != NULL ) {
consoleDevel("callback execute\r\n");
cb(radio);
}
ithacaUnlock(&radio->lock);
}
consoleDevel("loop yield\r\n");
chThdYield();
}
}
void BaseThread::yield(void) {
chThdYield();
}
void osSwitchTask(void)
{
//Force a context switch
chThdYield();
}
int main (int argc, const char** argv) {
if (argc < 2) {
fprintf(stderr, "Usage: rf69bridge <filepathprefix>\n");
return 1;
}
filePath = argv[1];
sprintf(myTopic, "raw/%s/%d-%d", NAME, RF_FREQ, RF_GROUP);
printf("\n[rf69bridge] %s @ %s using: %s*\n", myTopic, SERVER, filePath);
wiringPiSetup();
if (wiringPiSPISetup (0, 4000000) < 0) {
printf("Can't open the SPI bus: %d\n", errno);
return 1;
}
rf.init(RF_ID, RF_GROUP, RF_FREQ);
//rf.encrypt("mysecret");
rf.txPower(15); // 0 = min .. 31 = max
BootServer<MyFileAccess> server;
mqtt.connect(SERVER);
mqtt.subscribe(0, myTopic);
struct {
int16_t afc;
uint8_t rssi;
uint8_t lna;
uint8_t buf [64];
} rx;
while (true) {
int len = rf.receive(rx.buf, sizeof rx.buf);
if (len >= 0) {
#if DEBUG
printf("OK ");
for (int i = 0; i < len; ++i)
printf("%02x", rx.buf[i]);
printf(" (%d%s%d:%d)\n",
rf.rssi, rf.afc < 0 ? "" : "+", rf.afc, rf.lna);
#endif
if (rx.buf[1] == 0xC0) {
BootReply reply;
int len2 = server.request(rx.buf + 2, len - 2, &reply);
if (len2 < 0) {
printf("ignoring %d -> %d bytes\n", len - 2, len2);
} else {
if (len - 2 != sizeof (FetchRequest) ||
len2 != sizeof (FetchReply))
printf("sending %d -> %d bytes\n", len - 2, len2);
rf.send(0xC0, (const uint8_t*) &reply + 2, len2);
}
}
rx.afc = rf.afc;
rx.rssi = rf.rssi;
rx.lna = rf.lna;
// the topic includes frequency, net group, and origin node id
char topic [30];
sprintf(topic, "%s/%d", myTopic, rx.buf[1] & 0x3F);
// construct a JSON-compatible hex string representation
//
// the format is:
// 2-byte AFC value (little-endian)
// 1-byte RSSI (raw, 0..255, as in RFM69)
// 1-byte LNA
// followed by actual receive data:
// 1-byte destination (6 bits) and parity (2 bits)
// 1-byte origin (6 bits) and header flags (2 bits)
// ... actual payload data
char hex [2 * sizeof rx + 3];
for (int i = 0; i < 4 + len; ++i)
sprintf(hex+1+2*i, "%02x", ((const uint8_t*) &rx)[i]);
hex[0] = '"';
hex[9+2*len] = '"';
mqtt.publish(0, topic, 10+2*len, (const uint8_t*) hex);
}
chThdYield();
}
}
static msg_t GTimerThreadHandler(void *arg) {
(void)arg;
GTimer *pt;
systime_t tm;
systime_t nxtTimeout;
systime_t lastTime;
GTimerFunction fn;
void *param;
#if CH_USE_REGISTRY
chRegSetThreadName("GTimer");
#endif
nxtTimeout = TIME_INFINITE;
lastTime = 0;
while(1) {
/* Wait for work to do. */
chThdYield(); // Give someone else a go no matter how busy we are
chBSemWaitTimeout(&waitsem, nxtTimeout);
restartTimerChecks:
// Our reference time
tm = chTimeNow();
nxtTimeout = TIME_INFINITE;
/* We need to obtain the mutex */
chMtxLock(&mutex);
if (pTimerHead) {
pt = pTimerHead;
do {
// Do we have something to do for this timer?
if ((pt->flags & GTIMER_FLG_JABBED) || (!(pt->flags & GTIMER_FLG_INFINITE) && TimeIsWithin(pt->when, lastTime, tm))) {
// Is this timer periodic?
if ((pt->flags & GTIMER_FLG_PERIODIC) && pt->period != TIME_IMMEDIATE) {
// Yes - Update ready for the next period
if (!(pt->flags & GTIMER_FLG_INFINITE)) {
// We may have skipped a period.
// We use this complicated formulae rather than a loop
// because the gcc compiler stuffs up the loop so that it
// either loops forever or doesn't get executed at all.
pt->when += ((tm + pt->period - pt->when) / pt->period) * pt->period;
}
// We are definitely no longer jabbed
pt->flags &= ~GTIMER_FLG_JABBED;
} else {
// No - get us off the timers list
if (pt->next == pt->prev)
pTimerHead = 0;
else {
pt->next->prev = pt->prev;
pt->prev->next = pt->next;
if (pTimerHead == pt)
pTimerHead = pt->next;
}
pt->flags = 0;
}
// Call the callback function
fn = pt->fn;
param = pt->param;
chMtxUnlock();
fn(param);
// We no longer hold the mutex, the callback function may have taken a while
// and our list may have been altered so start again!
goto restartTimerChecks;
}
// Find when we next need to wake up
if (!(pt->flags & GTIMER_FLG_INFINITE) && pt->when - tm < nxtTimeout)
nxtTimeout = pt->when - tm;
pt = pt->next;
} while(pt != pTimerHead);
}
// Ready for the next loop
lastTime = tm;
chMtxUnlock();
}
return 0;
}
/* Core thread */
static msg_t ncoreDrawThread(void *msg) {
GEventMouse ev, evPrev;
coord_t dx, dy;
int state = 0, dist;
(void)msg;
ginputGetMouseStatus(0, &evPrev);
while (1) {
// Exit signal received? If yes, terminate.
if (chThdShouldTerminate())
return 0;
ginputGetMouseStatus(0, &ev);
switch(state) {
case 0: if (ev.meta == GMETA_MOUSE_DOWN) {
state = 1;
if (nMode == NCORE_MODE_FILL && PEN_IN_DRAWING_AREA(ev)) {
// Set bgcolor to current color, clear the display.
ncoreDrawingArea->bgcolor = ncoreDrawingArea->color;
gwinClear(ncoreDrawingArea);
}
}
else
chThdYield();
break;
case 1: if (ev.meta == GMETA_MOUSE_UP) {
state = 0;
//chprintf(nStatusConsole, "\nPen Up: (%d, %d)", ev.x, ev.y);
break;
}
dx = abs(ev.x - evPrev.x);
dy = abs(ev.y - evPrev.y);
dist = dx * dx + dy * dy;
if (dist > 0)
{
gdispSetClip(ncoreDrawingArea->x,
ncoreDrawingArea->y,
ncoreDrawingArea->width,
ncoreDrawingArea->height);
if (PEN_IN_DRAWING_AREA(ev)){
// Do Interpolation
if (dist <= 2) {
draw_point(ev.x, ev.y);
}
else if (dist <= 5) {
// Line drawing does not give good results for this case.
// So draw two pixels directly
draw_point(ev.x, ev.y);
draw_point((ev.x + evPrev.x) / 2, (ev.y + evPrev.y) / 2);
}
else if (dx * dx <= MAX_DX && dy * dy <= MAX_DY) {
draw_line(ev.x, ev.y, evPrev.x, evPrev.y);
}
}
//chprintf(nStatusConsole, "\nPen Down: (%d, %d)", ev.x, ev.y);
}
break;
}
evPrev = ev;
}
return 0;
}
