bool_t gdispInit(void) {
bool_t res;
unsigned i;
/* Mark all the Messages as free */
for(i=0; i < GDISP_QUEUE_SIZE; i++)
gdispMsgs[i].action = GDISP_LLD_MSG_NOP;
/* Initialise our Mailbox, Mutex's and Counting Semaphore.
* A Mutex is required as well as the Mailbox and Thread because some calls have to be synchronous.
* Synchronous calls get handled by the calling thread, asynchronous by our worker thread.
*/
chMBInit(&gdispMailbox, gdispMailboxQueue, sizeof(gdispMailboxQueue)/sizeof(gdispMailboxQueue[0]));
chMtxInit(&gdispMutex);
chMtxInit(&gdispMsgsMutex);
chSemInit(&gdispMsgsSem, GDISP_QUEUE_SIZE);
lldThread = chThdCreateStatic(waGDISPThread, sizeof(waGDISPThread), NORMALPRIO, GDISPThreadHandler, NULL);
/* Initialise driver - synchronous */
chMtxLock(&gdispMutex);
res = gdisp_lld_init();
chMtxUnlock();
return res;
}
extern "C" void mainFunc(){
Serial.begin(38400);
Serial1.begin(38400, SERIAL_8N1);
delay(3000);
Serial.printf("mainFunc\r\n");
chMtxInit(&kalman_st_mut);
chMtxInit(&control_mut);
//chThdCreateStatic(wa_led_thread, sizeof(wa_led_thread), NORMALPRIO+3, led_thread, NULL);
chThdCreateStatic(wa_flash_thread, sizeof(wa_flash_thread), NORMALPRIO+4, flash_thread, NULL);
chThdCreateStatic(wa_meas_thread, sizeof(wa_meas_thread), NORMALPRIO+3, meas_thread, NULL);
chThdCreateStatic(wa_communication_thread, sizeof(wa_communication_thread), NORMALPRIO+2, communication_thread, NULL);
chThdCreateStatic(wa_receive_thread, sizeof(wa_receive_thread), NORMALPRIO+1, receive_thread, NULL);
chThdCreateStatic(wa_orientation_thread, sizeof(wa_orientation_thread), NORMALPRIO, orientation_thread, NULL);
}
FrameBuffer< pages, columns >::FrameBuffer(){
limits.x_min = columns - 1;
limits.y_min = pages - 1;
limits.x_max = 0;
limits.y_max = 0;
chMtxInit(&mutex);
}
/**
* @brief Initializes the standard part of a @p I2CDriver structure.
*
* @param[out] i2cp pointer to the @p I2CDriver object
*
* @init
*/
void i2cObjectInit(I2CDriver *i2cp) {
i2cp->id_state = I2C_STOP;
i2cp->id_config = NULL;
i2cp->rxbuff_p = NULL;
i2cp->txbuff_p = NULL;
i2cp->rxbuf = NULL;
i2cp->txbuf = NULL;
i2cp->id_slave_config = NULL;
#if I2C_USE_WAIT
i2cp->id_thread = NULL;
#endif /* I2C_USE_WAIT */
#if I2C_USE_MUTUAL_EXCLUSION
#if CH_USE_MUTEXES
chMtxInit(&i2cp->id_mutex);
#else
chSemInit(&i2cp->id_semaphore, 1);
#endif /* CH_USE_MUTEXES */
#endif /* I2C_USE_MUTUAL_EXCLUSION */
#if defined(I2C_DRIVER_EXT_INIT_HOOK)
I2C_DRIVER_EXT_INIT_HOOK(i2cp);
#endif
}
int motor_init(void)
{
_watchdog_id = watchdog_create(WATCHDOG_TIMEOUT_MSEC);
if (_watchdog_id < 0) {
return _watchdog_id;
}
int ret = motor_rtctl_init();
if (ret) {
return ret;
}
chMtxInit(&_mutex);
chEvtInit(&_setpoint_update_event);
configure();
init_filters();
if (_state.input_voltage < MIN_VALID_INPUT_VOLTAGE || _state.input_voltage > MAX_VALID_INPUT_VOLTAGE) {
lowsyslog("Motor: Invalid input voltage: %f\n", _state.input_voltage);
return -1;
}
ret = rpmctl_init();
if (ret) {
return ret;
}
motor_rtctl_stop();
assert_always(chThdCreateStatic(_wa_control_thread, sizeof(_wa_control_thread),
HIGHPRIO, control_thread, NULL));
return 0;
}
void oledInit (oledConfig *oledConfig, struct SerialDriver *oled, const uint32_t baud,
GPIO_TypeDef *rstGpio, uint32_t rstPin, enum OledConfig_Device dev)
{
oledConfig->rstGpio = rstGpio;
oledConfig->rstPin = rstPin;
oledConfig->deviceType = dev;
oledHardReset (oledConfig);
oledPreInit (oledConfig, 9600);
oledConfig->serial = (BaseSequentialStream *) oled;
chMtxInit(&(oledConfig->omutex));
sdStart(oled, &(oledConfig->serialConfig));
chThdSleepMilliseconds(10);
// opaque background
if (oledConfig->deviceType != TERM_VT100) {
OLED_KOF (KOF_INT16, "%c%c%c%c", 0xff, 0xdf, 0x00, 0x01);
}
oledClearScreen (oledConfig);
// use greater speed
if (baud != 9600)
oledSetBaud (oledConfig, baud);
}
MessageQueue(
uint8_t* const data,
size_t k
) : fifo { data, k }
{
chMtxInit(&mutex_write);
}
void errorLedInit(void)
{
#if PROTECT_DATA_WITH_MUTEX
chMtxInit(&mutex);
#endif
chThdCreateStatic(waErrorLedThd, sizeof(waErrorLedThd), NORMALPRIO-1, errorLedThd, 0);
}
void initI2c( void )
{
// Address pins
palSetPadMode( ADDR_PORT, ADDR_0_PIN, PAL_MODE_INPUT );
palSetPadMode( ADDR_PORT, ADDR_1_PIN, PAL_MODE_INPUT );
palSetPadMode( ADDR_PORT, ADDR_2_PIN, PAL_MODE_INPUT );
palSetPadMode( GPIOB, 6, PAL_MODE_STM32_ALTERNATE_OPENDRAIN );
palSetPadMode( GPIOB, 7, PAL_MODE_STM32_ALTERNATE_OPENDRAIN );
chThdSleepMilliseconds( 100 );
i2cInit();
//chThdSleepMilliseconds( 100 );
i2cStart( &I2CD1, &i2cfg1 );
//chThdSleepMilliseconds( 200 );
// Initial values for IOs.
int16_t i;
for ( i=0; i<I2C_SLAVES_CNT; i++ )
{
outs[i] = 0;
pendOuts[i] = 0;
ins[i] = 0;
}
// Initializing mutex.
chMtxInit( &mutex );
// Creating thread.
chThdCreateStatic( waI2c, sizeof(waI2c), NORMALPRIO, i2cThread, NULL );
}
void initPower( void )
{
chMtxInit( &g_mutex );
setPower( 0 );
chThdCreateStatic( waPower, sizeof(waPower), NORMALPRIO, Power, NULL );
}
static clarityError clarityMgmtInit(clarityAccessPointInformation * apInfo,
clarityUnresponsiveCallback cb)
{
memset(&mgmtData, 0, sizeof(mgmtData));
chMtxInit(&mgmtData.mutex);
clarityMgmtMtxLock();
mgmtData.ap = apInfo;
clarityMgmtMtxUnlock();
#if 0
connectivityMonThd = chThdCreateStatic(connectivityMonThdWorkingArea,
sizeof(connectivityMonThdWorkingArea),
NORMALPRIO + 1,
clarityMgmtConnectivityMonitoringThd,
NULL);
#endif
if (cb != NULL)
{
unresponsiveCb = cb;
responseMonThd = chThdCreateStatic(responseMonThdWorkingArea,
sizeof(responseMonThdWorkingArea),
HIGHPRIO-1,
clarityMgmtResponseMonitoringThd,
NULL);
}
return CLARITY_SUCCESS;
}
bool_t osCreateMutex(OsMutex *mutex)
{
//Initialize the mutex object
chMtxInit(mutex);
//Mutex successfully created
return TRUE;
}
void _heap_init(void) {
#if CH_USE_MUTEXES
chMtxInit(&hmtx);
#else
chSemInit(&hsem, 1);
#endif
}
void initRead( void )
{
palSetPadMode( SEN_PORT, SEN_1, PAL_MODE_INPUT );
palSetPadMode( SEN_PORT, SEN_2, PAL_MODE_INPUT );
// Initializing mutex.
chMtxInit( &mutex );
// Creating thread.
chThdCreateStatic( waRead, sizeof(waRead), NORMALPRIO, readThread, NULL );
}
/**
* @brief Initializes the default heap.
*
* @notapi
*/
void _heap_init(void) {
default_heap.h_provider = chCoreAlloc;
default_heap.h_free.h.u.next = (union heap_header *)NULL;
default_heap.h_free.h.size = 0;
#if CH_USE_MUTEXES || defined(__DOXYGEN__)
chMtxInit(&default_heap.h_mtx);
#else
chSemInit(&default_heap.h_sem, 1);
#endif
}
void comm_init(void) {
myUSBinit();
packet_init(send_packet, process_packet);
chMtxInit(&send_mutex);
// Threads
chThdCreateStatic(serial_read_thread_wa, sizeof(serial_read_thread_wa), NORMALPRIO, serial_read_thread, NULL);
chThdCreateStatic(serial_process_thread_wa, sizeof(serial_process_thread_wa), NORMALPRIO, serial_process_thread, NULL);
chThdCreateStatic(timer_thread_wa, sizeof(timer_thread_wa), NORMALPRIO, timer_thread, NULL);
}
/**
*
* @brief Creates a non recursive mutex.
*
* @returns instance of @p struct pios_mutex or NULL on failure
*
*/
struct pios_mutex *PIOS_Mutex_Create(void)
{
struct pios_mutex *mtx = PIOS_malloc(sizeof(struct pios_mutex));
if (mtx == NULL)
return NULL;
chMtxInit(&mtx->mtx);
return mtx;
}
void initLed( void )
{
palClearPad( LED_0_PORT, LED_0_PIN );
palClearPad( LED_1_PORT, LED_1_PIN );
palSetPadMode( LED_0_PORT, LED_0_PIN, PAL_MODE_OUTPUT_PUSHPULL );
palSetPadMode( LED_1_PORT, LED_1_PIN, PAL_MODE_OUTPUT_PUSHPULL );
// Initializing mutex.
chMtxInit( &mutex );
// Creating thread.
chThdCreateStatic( waLeds, sizeof(waLeds), NORMALPRIO, ledsThread, NULL );
}
/**
*
* @brief Creates a recursive mutex.
*
* @returns instance of @p struct pios_recursive mutex or NULL on failure
*
*/
struct pios_recursive_mutex *PIOS_Recursive_Mutex_Create(void)
{
struct pios_recursive_mutex *mtx = PIOS_malloc(sizeof(struct pios_recursive_mutex));
if (mtx == NULL)
return NULL;
chMtxInit(&mtx->mtx);
mtx->count = 0;
return mtx;
}
bool_t gdispInit(void) {
bool_t res;
/* Initialise Mutex */
chMtxInit(&gdispMutex);
/* Initialise driver */
chMtxLock(&gdispMutex);
res = gdisp_lld_init();
chMtxUnlock();
return res;
}
void rawd_init(void)
{
chMtxInit(&rawd_mutex);
memset(ncs, sizeof(ncs), 0);
rawd_th = chThdCreateStatic(rawd_wa, sizeof(rawd_wa),
LOWPRIO + 2,
rawd_loop, NULL);
rawd_push_th = chThdCreateStatic(rawd_push_wa, sizeof(rawd_push_wa),
LOWPRIO + 2,
rawd_push_loop, NULL);
}
void mySPIinit(void){
chMtxInit(&accelMtx);
// Initializes the SPI driver 1 in order to access the MEMS. The signals are already initialized in the board file.
spiStart(&SPID1, &spi1cfg);
// LIS302DL initialization.
lis302dlWriteRegister(&SPID1, LIS302DL_CTRL_REG1, 0x43);
lis302dlWriteRegister(&SPID1, LIS302DL_CTRL_REG2, 0x00);
lis302dlWriteRegister(&SPID1, LIS302DL_CTRL_REG3, 0x00);
// Start accelerator reading thread.
chThdCreateStatic(waThread1, sizeof(waThread1),
NORMALPRIO + 10, Thread1, NULL);
}
bool oscUsbEnable(bool on)
{
if (on && osc.usbThd == 0) {
chMtxInit(&osc.usb.lock);
osc.usb.sendMessage = usbserialWriteSlip;
osc.usbThd = chThdCreateStatic(waUsbThd, sizeof(waUsbThd), NORMALPRIO, OscUsbSerialThread, NULL);
return true;
}
if (!on && osc.usbThd != 0) {
chThdTerminate(osc.usbThd);
osc.usbThd = 0;
return true;
}
return false;
}
/*
* Initialize the SPI interface
*/
void SPIInit(void)
{
/*
* SPI1 I/O pins setup.
*/
palSetPadMode(IOPORT1, 5, PAL_MODE_STM32_ALTERNATE_PUSHPULL); /* SCK. */
palSetPadMode(IOPORT1, 6, PAL_MODE_STM32_ALTERNATE_PUSHPULL); /* MISO.*/
palSetPadMode(IOPORT1, 7, PAL_MODE_STM32_ALTERNATE_PUSHPULL); /* MOSI.*/
palSetPadMode(IOPORT1, GPIOA_SPI1NSS, PAL_MODE_OUTPUT_PUSHPULL);
palSetPad(IOPORT1, GPIOA_SPI1NSS);
/*
* Initialize Mutex
*/
chMtxInit(&SPIMtx); /* Mutex initialization before use */
}
void encabsInit( void )
{
palSetPadMode( ENCABS_CLK_PORT, ENCABS_CLK_PIN, PAL_MODE_OUTPUT_PUSHPULL );
palSetPad( ENCABS_CLK_PORT, ENCABS_CLK_PIN );
palSetPadMode( ENCABS_DATA_PORT, ENCABS_DATA_PIN, PAL_MODE_INPUT );
encabsBits = ENCABS_BITS;
delayTicks = 4096;
delayFrontTicks = 1;
delayBackTicks = 1;
// Initializing mutex.
chMtxInit( &mutex );
// Creating thread.
chThdCreateStatic( waEncabs, sizeof(waEncabs), NORMALPRIO, encabsThread, NULL );
}
/**
* @brief Initializes a memory heap from a static memory area.
* @pre Both the heap buffer base and the heap size must be aligned to
* the @p stkalign_t type size.
* @pre In order to use this function the option @p CH_USE_MALLOC_HEAP
* must be disabled.
*
* @param[out] heapp pointer to the memory heap descriptor to be initialized
* @param[in] buf heap buffer base
* @param[in] size heap size
*
* @init
*/
void chHeapInit(MemoryHeap *heapp, void *buf, size_t size) {
union heap_header *hp;
chDbgCheck(MEM_IS_ALIGNED(buf) && MEM_IS_ALIGNED(size), "chHeapInit");
heapp->h_provider = (memgetfunc_t)NULL;
heapp->h_free.h.u.next = hp = buf;
heapp->h_free.h.size = 0;
hp->h.u.next = NULL;
hp->h.size = size - sizeof(union heap_header);
#if CH_USE_MUTEXES || defined(__DOXYGEN__)
chMtxInit(&heapp->h_mtx);
#else
chSemInit(&heapp->h_sem, 1);
#endif
}
/**
* @brief Initializes the standard part of a @p I2CDriver structure.
*
* @param[out] i2cp pointer to the @p I2CDriver object
*
* @init
*/
void i2cObjectInit(I2CDriver *i2cp) {
i2cp->state = I2C_STOP;
i2cp->config = NULL;
#if I2C_USE_MUTUAL_EXCLUSION
#if CH_USE_MUTEXES
chMtxInit(&i2cp->mutex);
#else
chSemInit(&i2cp->semaphore, 1);
#endif /* CH_USE_MUTEXES */
#endif /* I2C_USE_MUTUAL_EXCLUSION */
#if defined(I2C_DRIVER_EXT_INIT_HOOK)
I2C_DRIVER_EXT_INIT_HOOK(i2cp);
#endif
}
bool oscUdpEnable(bool on)
{
if (on && osc.udpThd == 0) {
osc.udpListenPort = OSC_UDP_DEFAULT_PORT;
oscUdpReplyPort();
osc.udp.sendMessage = oscSendMessageUDP;
chMtxInit(&osc.udp.lock);
osc.udpThd = chThdCreateStatic(waUdpThd, sizeof(waUdpThd), NORMALPRIO, OscUdpThread, NULL);
return true;
}
if (!on && osc.udpThd != 0) {
chThdTerminate(osc.udpThd);
osc.udpThd = 0;
return true;
}
return false;
}
/**
* @brief Initializes the standard part of a @p SPIDriver structure.
*
* @param[out] spip pointer to the @p SPIDriver object
*
* @init
*/
void spiObjectInit(SPIDriver *spip) {
spip->state = SPI_STOP;
spip->config = NULL;
#if SPI_USE_WAIT
spip->thread = NULL;
#endif /* SPI_USE_WAIT */
#if SPI_USE_MUTUAL_EXCLUSION
#if CH_USE_MUTEXES
chMtxInit(&spip->mutex);
#else
chSemInit(&spip->semaphore, 1);
#endif
#endif /* SPI_USE_MUTUAL_EXCLUSION */
#if defined(SPI_DRIVER_EXT_INIT_HOOK)
SPI_DRIVER_EXT_INIT_HOOK(spip);
#endif
}
/**
* @brief Initializes the standard part of a @p DACDriver structure.
*
* @param[out] dacp pointer to the @p DACDriver object
*
* @init
*/
void dacObjectInit(DACDriver *dacp) {
dacp->state = DAC_STOP;
dacp->config = NULL;
#if DAC_USE_WAIT
dacp->thread = NULL;
#endif /* DAC_USE_WAIT */
#if DAC_USE_MUTUAL_EXCLUSION
#if CH_USE_MUTEXES
chMtxInit(&dacp->mutex);
#else
chSemInit(&dacp->semaphore, 1);
#endif
#endif /* DAC_USE_MUTUAL_EXCLUSION */
#if defined(DAC_DRIVER_EXT_INIT_HOOK)
DAC_DRIVER_EXT_INIT_HOOK(dacp);
#endif
}