int main(void) {
halInit();
chSysInit();
r2p::Middleware::instance.initialize(wa_info, sizeof(wa_info), r2p::Thread::LOWEST);
rtcantra.initialize(rtcan_config);
r2p::Middleware::instance.start();
r2p::ledsub_conf ledsub_conf = { "leds" };
r2p::Thread::create_heap(NULL, THD_WA_SIZE(512), NORMALPRIO + 1, r2p::ledsub_node, (void *)&ledsub_conf);
r2p::ledpub_conf ledpub_conf = {"leds", 1};
r2p::Thread::create_heap(NULL, THD_WA_SIZE(512), NORMALPRIO + 1, r2p::ledpub_node, (void *) &ledpub_conf);
r2p::Thread::create_heap(NULL, THD_WA_SIZE(2048), NORMALPRIO + 3, madgwick_node, NULL);
r2p::Thread::sleep(r2p::Time::ms(5000));
r2p::Thread::create_heap(NULL, THD_WA_SIZE(2048), NORMALPRIO + 3, balance_node, NULL);
r2p::Thread::sleep(r2p::Time::ms(500));
r2p::Thread::create_heap(NULL, THD_WA_SIZE(2048), NORMALPRIO + 2, velocity_node, NULL);
for (;;) {
r2p::Thread::sleep(r2p::Time::ms(500));
}
return CH_SUCCESS;
}
static void dyn1_execute(void) {
size_t n, sz;
void *p1;
tprio_t prio = chThdGetPriority();
(void)chHeapStatus(&heap1, &sz);
/* Starting threads from the heap. */
threads[0] = chThdCreateFromHeap(&heap1, THD_WA_SIZE(THREADS_STACK_SIZE),
prio-1, thread, "A");
threads[1] = chThdCreateFromHeap(&heap1, THD_WA_SIZE(THREADS_STACK_SIZE),
prio-2, thread, "B");
/* Allocating the whole heap in order to make the thread creation fail.*/
(void)chHeapStatus(&heap1, &n);
p1 = chHeapAlloc(&heap1, n);
threads[2] = chThdCreateFromHeap(&heap1, THD_WA_SIZE(THREADS_STACK_SIZE),
prio-3, thread, "C");
chHeapFree(p1);
test_assert(1, (threads[0] != NULL) &&
(threads[1] != NULL) &&
(threads[2] == NULL) &&
(threads[3] == NULL) &&
(threads[4] == NULL),
"thread creation failed");
/* Claiming the memory from terminated threads. */
test_wait_threads();
test_assert_sequence(2, "AB");
/* Heap status checked again.*/
test_assert(3, chHeapStatus(&heap1, &n) == 1, "heap fragmented");
test_assert(4, n == sz, "heap size changed");
}
/*
* Application entry point.
*/
int main(void) {
/* Shell thread */
Thread *shelltp = NULL;
/* UART Configuration Structure */
SerialConfig sc;
// Dummy ADC Configuration structure
ADCConfig adc_conf;
/*
* System initializations.
* - HAL initialization, this also initializes the configured device drivers
* and performs the board-specific initializations.
* - Kernel initialization, the main() function becomes a thread and the
* RTOS is active.
*/
halInit();
chSysInit();
GPIO_Configuration();
//ADC_Configuration();
I2C_Configuration();
/* Start Serial Driver */
sc.sc_speed = 115200;
sc.sc_cr1 = 0;
sc.sc_cr2 = USART_CR2_STOP1_BITS | USART_CR2_LINEN;
sc.sc_cr3 = 0;
sdStart(&SD1, &sc);
/* Shell manager initialization. */
shellInit();
/*
* Creates the example thread.
*/
chThdCreateStatic(waThread1, sizeof(waI2CThread), NORMALPRIO, I2CThread, NULL);
/*
* Normal main() thread activity, in this demo it does nothing except
* sleeping in a loop and check the button state, when the button is
* pressed the test procedure is launched with output on the serial
* driver 1.
*/
while (TRUE) {
if(!shelltp)
{
shelltp = shellCreate( &shell_cfg, THD_WA_SIZE(2048), NORMALPRIO);
}
else if(chThdTerminated(shelltp))
{
// Recovers memory of the previous shell.
chThdRelease(shelltp);
shelltp = NULL;
}
}
}
/*-----------------------------------------------------------------------------*/
Thread *
StartTaskWithPriority(tfunc_t pf, tprio_t priority, ... )
{
static bool_t init = TRUE;
int16_t i;
bool_t memfree = FALSE;
// First time initialization
if(init)
{
for(i=0;i<RC_TASKS;i++)
{
rcTasks[i].pf = NULL;
rcTasks[i].tp = NULL;
}
// not next time
init = FALSE;
}
// Check to see of this function is already in the robotc task list
for(i=0;i<RC_TASKS;i++)
{
if( rcTasks[i].pf == pf )
return(NULL);
}
// check to see of we have exhausted the task array
for(i=0;i<RC_TASKS;i++)
{
if( rcTasks[i].pf == NULL )
{
memfree = TRUE;
break;
}
}
if( !memfree )
return(NULL);
Thread *tp;
tp = chThdCreateFromHeap(NULL, THD_WA_SIZE(TC_THREAD_STACK), priority, vexRobotcTask, (void *)pf );
// tp will be NULL if memory is exhausted
if( tp != NULL )
{
// Save association of thread and callback
for(i=0;i<RC_TASKS;i++)
{
if( rcTasks[i].tp == NULL )
{
rcTasks[i].tp = tp;
rcTasks[i].pf = pf;
break;
}
}
}
return(tp);
}
sys_thread_t sys_thread_new(const char *name, lwip_thread_fn thread,
void *arg, int stacksize, int prio) {
(void)name;
size_t wsz = THD_WA_SIZE(stacksize);
void *wsp = chCoreAlloc(wsz);
if (wsp == NULL)
return NULL;
return (sys_thread_t)chThdCreateStatic(wsp, wsz, prio, (tfunc_t)thread, arg);
}
/**
* @brief Creates a new thread into a static memory area.
* @details The new thread is initialized but not inserted in the ready list,
* the initial state is @p THD_STATE_SUSPENDED.
* @post The initialized thread can be subsequently started by invoking
* @p chThdResume(), @p chThdResumeI() or @p chSchWakeupS()
* depending on the execution context.
* @note A thread can terminate by calling @p chThdExit() or by simply
* returning from its main function.
* @note Threads created using this function do not obey to the
* @p CH_DBG_FILL_THREADS debug option because it would keep
* the kernel locked for too much time.
*
* @param[out] wsp pointer to a working area dedicated to the thread stack
* @param[in] size size of the working area
* @param[in] prio the priority level for the new thread
* @param[in] pf the thread function
* @param[in] arg an argument passed to the thread function. It can be
* @p NULL.
* @return The pointer to the @p Thread structure allocated for
* the thread into the working space area.
*
* @iclass
*/
Thread *chThdCreateI(void *wsp, size_t size,
tprio_t prio, tfunc_t pf, void *arg) {
/* Thread structure is layed out in the lower part of the thread workspace */
Thread *tp = wsp;
chDbgCheck((wsp != NULL) && (size >= THD_WA_SIZE(0)) &&
(prio <= HIGHPRIO) && (pf != NULL),
"chThdCreateI");
SETUP_CONTEXT(wsp, size, pf, arg);
return _thread_init(tp, prio);
}
int main(void) {
halInit();
chSysInit();
r2p::Middleware::instance.initialize(wa_info, sizeof(wa_info), r2p::Thread::LOWEST);
rtcantra.initialize(rtcan_config);
r2p::Middleware::instance.start();
r2p::ledsub_conf ledsub_conf = { "led" };
r2p::Thread::create_heap(NULL, THD_WA_SIZE(512), NORMALPRIO, r2p::ledsub_node, &ledsub_conf);
pid_conf pid_conf = {100, 0, 0, 0.01};
r2p::Thread::create_heap(NULL, THD_WA_SIZE(2048), NORMALPRIO + 1, pid_node, &pid_conf);
r2p::Thread::create_heap(NULL, THD_WA_SIZE(2048), NORMALPRIO + 2, encoder_node, NULL);
for (;;) {
r2p::Thread::sleep(r2p::Time::ms(500));
}
return CH_SUCCESS;
}
int spawnShell(void) {
const int ONE_THOUSAND_MILLISECONDS = 1000;
const int THREAD_WORKING_AREA_SIZE = 2048;
Thread *shellThread = NULL;
while (TRUE) {
// If we don't have a shell but a living USB connection, create a new thread with the shell.
if (!shellThread && isUSBActive()) {
shellThread = shellCreate(&shellConfiguration, THD_WA_SIZE(THREAD_WORKING_AREA_SIZE), NORMALPRIO);
}
else if (chThdTerminated(shellThread)) {
// Recover memory of the previous shell.
chThdRelease(shellThread);
// Trigger spawning of a new shell.
shellThread = NULL;
}
chThdSleepMilliseconds(ONE_THOUSAND_MILLISECONDS);
}
}
static void pools1_setup(void) {
chPoolInit(&mp1, THD_WA_SIZE(THREADS_STACK_SIZE), NULL);
}
OsTask *osCreateTask(const char_t *name, OsTaskCode taskCode,
void *params, size_t stackSize, int_t priority)
{
uint_t i;
void *wa;
OsTask *task = NULL;
//Compute the size of the stack in bytes
stackSize *= sizeof(uint_t);
//Allocate a memory block to hold the working area
wa = osAllocMem(THD_WA_SIZE(stackSize));
//Successful memory allocation?
if(wa != NULL)
{
//Enter critical section
chSysLock();
//Loop through task table
for(i = 0; i < OS_PORT_MAX_TASKS; i++)
{
//Check whether the current entry is free
if(taskTable[i].tp == NULL)
break;
}
//Any entry available in the table?
if(i < OS_PORT_MAX_TASKS)
{
//Create a new task
taskTable[i].tp = chThdCreateI(wa, THD_WA_SIZE(stackSize),
priority, (tfunc_t) taskCode, params);
//Check whether the task was successfully created
if(taskTable[i].tp != NULL)
{
//Insert the newly created task in the ready list
chSchWakeupS(taskTable[i].tp, RDY_OK);
//Save task pointer
task = &taskTable[i];
//Save working area base address
waTable[i] = wa;
//Leave critical section
chSysUnlock();
}
else
{
//Leave critical section
chSysUnlock();
//Clean up side effects
osFreeMem(wa);
}
}
else
{
//Leave critical section
chSysUnlock();
//No entry available in the table
osFreeMem(wa);
}
}
//Return a pointer to the newly created task
return task;
}
void forkOS_createThread_init() {
chPoolInit(&pool_descriptor, THD_WA_SIZE(THREADS_STACK_SIZE), NULL);
chPoolLoadArray(&pool_descriptor, (void *) pool_buf, NUM_THREADS);
}
#include "rts/conc.h"
#define THREADS_STACK_SIZE 584
#define NUM_THREADS 5
static char pool_buf[(THD_WA_SIZE(THREADS_STACK_SIZE)) * NUM_THREADS];
static MEMORYPOOL_DECL(pool_descriptor, THD_WA_SIZE(THREADS_STACK_SIZE), NULL);
/* xxx ChibiOS/RT's Mutex does not support locking on interrupt. */
void jhc_mutex_init(jhc_mutex_t *mutex) {
chMtxInit(mutex);
}
void jhc_mutex_lock(jhc_mutex_t *mutex) {
chMtxLock(mutex);
}
void jhc_mutex_unlock(jhc_mutex_t *mutex) {
jhc_mutex_t *unlocked = chMtxUnlock();
if (unlocked != mutex)
abort();
}
void forkOS_createThread_init() {
chPoolInit(&pool_descriptor, THD_WA_SIZE(THREADS_STACK_SIZE), NULL);
chPoolLoadArray(&pool_descriptor, (void *) pool_buf, NUM_THREADS);
}
jhc_threadid_t forkOS_createThread(void *(*wrapper)(void *), void *entry, int *err) {
Thread *tid;
int main(void) {
Thread * pub_tp = NULL;
Thread * sub_tp = NULL;
halInit();
chSysInit();
/*
* Initializes a serial-over-USB CDC driver.
*/
#if USE_USB_SERIAL
sduObjectInit(&SDU1);
sduStart(&SDU1, &serusbcfg);
#endif
/*
* Activates the USB driver and then the USB bus pull-up on D+.
* Note, a delay is inserted in order to not have to disconnect the cable
* after a reset.
*/
#if USE_USB_SERIAL
usbDisconnectBus(serusbcfg.usbp);
chThdSleepMilliseconds(500);
usbStart(serusbcfg.usbp, &usbcfg);
usbConnectBus(serusbcfg.usbp);
#endif
/* Start the serial driver. */
#if !USE_USB_SERIAL
sdStart(&SD3, NULL);
#endif
for (;;) {
#if USE_USB_SERIAL
if (!pub_tp && (SDU1.config->usbp->state == USB_ACTIVE)) {
#else
if (!pub_tp) {
#endif
pub_tp = chThdCreateFromHeap(NULL, THD_WA_SIZE(2048), NORMALPRIO, rosserial_pub_thread, NULL);
} else if (chThdTerminated(pub_tp)) {
chThdRelease(pub_tp);
pub_tp = NULL;
}
chThdSleepMilliseconds(123);
#if USE_USB_SERIAL
if (!sub_tp && (SDU1.config->usbp->state == USB_ACTIVE)) {
#else
if (!sub_tp) {
#endif
sub_tp = chThdCreateFromHeap(NULL, THD_WA_SIZE(2048), NORMALPRIO, rosserial_sub_thread, NULL);
} else if (chThdTerminated(sub_tp)) {
chThdRelease(sub_tp);
sub_tp = NULL;
}
chThdSleepMilliseconds(500);
}
return CH_SUCCESS;
}
}
return -1;
}
static void send_response( int s, char * buffer, int len, int maxlen){
// FIXME: check return of write, and continue to attempt to write if needed
len = MIN(len, maxlen-2);
if(len > 0){
buffer[len] = '\r';
buffer[len + 1] = '\n';
if(write(s, buffer, MIN(len + 2, maxlen)) < 0){
return;
}
}
}
WORKING_AREA(wa_rci, THD_WA_SIZE(2048 + ETH_MTU*2));
static msg_t rci_thread(void *p){
chRegSetThreadName("RCI");
struct RCICommand * commands = (struct RCICommand *)p;
struct sockaddr from;
socklen_t fromlen;
struct sockaddr own;
set_sockaddr(&own, "0.0.0.0", 23);
char rx_buf[ETH_MTU];
char tx_buf[ETH_MTU];
int sock = socket(AF_INET, SOCK_STREAM, 0);
chDbgAssert(sock >= 0, "Could not get RCI socket", NULL);
/*
* Application entry point.
*/
int main(void) {
Thread *shelltp = NULL;
/*
* System initializations.
* - HAL initialization, this also initializes the configured device drivers
* and performs the board-specific initializations.
* - Kernel initialization, the main() function becomes a thread and the
* RTOS is active.
*/
halInit();
chSysInit();
palClearPad(LED1_GPIO, LED1);
palClearPad(LED2_GPIO, LED2);
palClearPad(LED3_GPIO, LED3);
palClearPad(LED4_GPIO, LED4);
chThdSleepMilliseconds(500);
palSetPad(LED1_GPIO, LED1);
palSetPad(LED2_GPIO, LED2);
palSetPad(LED3_GPIO, LED3);
palSetPad(LED4_GPIO, LED4);
/*
* Activates the serial driver 1 using the driver default configuration.
*/
sdStart(&SERIAL_DRIVER, NULL);
r2p::Middleware::instance.initialize(wa_info, sizeof(wa_info), r2p::Thread::LOWEST);
rtcantra.initialize(rtcan_config);
r2p::Middleware::instance.start();
/* Make the PHY wake up.*/
palSetPad(GPIOC, GPIOC_ETH_NOT_PWRDN);
/* Creates the LWIP thread (it changes priority internally).*/
chThdCreateStatic(wa_lwip_thread, THD_WA_SIZE(LWIP_THREAD_STACK_SIZE), NORMALPRIO + 5, lwip_thread, NULL);
chThdSleepMilliseconds(100);
r2p::ledsub_conf ledsub_conf = { "led" };
r2p::Thread::create_heap(NULL, THD_WA_SIZE(256), NORMALPRIO, r2p::ledsub_node, &ledsub_conf);
//Setup subscribers
//current_node.subscribe(current_sub, "bits_packed");
//current_node.set_enabled(true);
vel_node.advertise(vel_pub, "current2", r2p::Time::INFINITE);
vel_node.set_enabled(true);
urosInit();
urosNodeCreateThread();
/*
* Normal main() thread activity, in this demo it does nothing except
* sleeping in a loop and check the button state.
*/
while (TRUE) {
r2p::Thread::sleep(r2p::Time::s(20));
}
}
