void commands_init(void) {
chThdCreateStatic(detect_thread_wa, sizeof(detect_thread_wa), NORMALPRIO, detect_thread, NULL);
}
void buttonInit(){
extStart(&EXTD1, &extcfg);
chThdCreateStatic(waThreadButton, sizeof(waThreadButton),
NORMALPRIO + 10, ThreadButton, NULL);
}
BaseThread::BaseThread(void *workspace, size_t wsize, tprio_t prio) {
thread_ref = chThdCreateStatic(workspace, wsize, prio, thdstart, this);
}
//-----------------------------------------------------------------------------
int
kuroBoxInit(void)
{
halInit();
chSysInit();
PWR->CSR &= ~PWR_CSR_EWUP;
kbg_setLED1(1);
kbg_setLED2(1);
kbg_setLED3(1);
// Serial
kuroBoxSerialInit(NULL, NULL);
// start the SPI bus, just use the LCD's SPI config since
// it's shared with the eeprom
spiStart(&SPID1, &lcd_cfg.spicfg);
memset(lcd_buffer, 0, sizeof(lcd_buffer));
st7565Start(&ST7565D1, &lcd_cfg, &SPID1, lcd_buffer);
// EEPROM
spiEepromStart(&spiEepromD1, &eeprom_cfg, &SPID1);
// SDIO
sdcStart(&SDCD1, &sdio_cfg);
// just blink to indicate we haven't crashed
#ifdef HAVE_BLINK_THREAD
blinkerThread = chThdCreateStatic(waBlinker, sizeof(waBlinker), NORMALPRIO, thBlinker, NULL);
#endif // HAVE_BLINK_THREAD
// read config goes here
// @TODO: add config reading from eeprom
// ADC to monitor voltage levels.
// start all the ADC stuff!
kuroBoxADCInit();
// init the screen, this will spawn a thread to keep it updated
kuroBoxScreenInit();
// LTC's, interrupt driven, very quick now
// Note, this module must be init'd after the screen
kuroBoxTimeInit();
// the actual logging thread
kuroBoxWriterInit();
// this turns on Layer 1 power, this turns on the mosfets controlling the
// VCC rail. After this, we can start GPS, VectorNav and altimeter
kbg_setL1PowerOn();
// wait for it to stabilise, poweron devices, and let them start before continuing
chThdSleepMilliseconds(500);
// gps uart
kuroBoxGPSInit(&SD6);
VND1.spip = &SPID2;
VND1.gpdp = &GPTD14;
kuroBoxVectorNavInit(&VND1, NULL); // use the defaults
// start I2C here, since it's a shared bus
i2cStart(&I2CD2, &i2cfg2);
// and the altimeter, also spawns a thread.
kuroBoxAltimeterInit();
// init the menu structure, and thread, if needed
kuroBoxMenuInit();
// set initial button state, AFTER menus!
kuroBoxButtonsInit();
// read all configs from bksram and load them up thoughout the system
kuroBoxConfigInit();
// Feature "A" - just a test feature that sends updates over serial
kuroBoxExternalDisplayInit();
// indicate we're ready
chprintf(DEBG, "%s\n\r\n\r", BOARD_NAME);
chThdSleepMilliseconds(50);
kbg_setLED1(0);
kbg_setLED2(0);
kbg_setLED3(0);
// all external interrupts, all the system should now be ready for it
extStart(&EXTD1, &extcfg);
return KB_OK;
}
/*
* Application entry point.
*/
int main(void) {
unsigned i;
gptcnt_t interval, threshold, worst;
/* Enables FPU exceptions.*/
nvicEnableVector(FPU_IRQn, 1);
/*
* 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();
/*
* Prepares the Serial driver 2 and GPT drivers 2 and 3.
*/
sdStart(&SD2, NULL); /* Default is 38400-8-N-1.*/
palSetPadMode(GPIOA, 2, PAL_MODE_ALTERNATE(7));
palSetPadMode(GPIOA, 3, PAL_MODE_ALTERNATE(7));
gptStart(&GPTD4, &gpt4cfg);
gptStart(&GPTD3, &gpt3cfg);
/*
* Initializes the worker threads.
*/
chThdCreateStatic(waWorkerThread, sizeof waWorkerThread,
NORMALPRIO - 20, WorkerThread, NULL);
chThdCreateStatic(waPeriodicThread, sizeof waPeriodicThread,
NORMALPRIO - 10, PeriodicThread, NULL);
/*
* Test procedure.
*/
println("");
println("*** ChibiOS/RT IRQ-STORM-FPU long duration test");
println("***");
print("*** Kernel: ");
println(CH_KERNEL_VERSION);
print("*** Compiled: ");
println(__DATE__ " - " __TIME__);
#ifdef PORT_COMPILER_NAME
print("*** Compiler: ");
println(PORT_COMPILER_NAME);
#endif
print("*** Architecture: ");
println(PORT_ARCHITECTURE_NAME);
#ifdef PORT_CORE_VARIANT_NAME
print("*** Core Variant: ");
println(PORT_CORE_VARIANT_NAME);
#endif
#ifdef PORT_INFO
print("*** Port Info: ");
println(PORT_INFO);
#endif
#ifdef PLATFORM_NAME
print("*** Platform: ");
println(PLATFORM_NAME);
#endif
#ifdef BOARD_NAME
print("*** Test Board: ");
println(BOARD_NAME);
#endif
println("***");
print("*** System Clock: ");
printn(STM32_SYSCLK);
println("");
print("*** Iterations: ");
printn(ITERATIONS);
println("");
print("*** Randomize: ");
printn(RANDOMIZE);
println("");
println("");
worst = 0;
for (i = 1; i <= ITERATIONS; i++){
print("Iteration ");
printn(i);
println("");
saturated = FALSE;
threshold = 0;
for (interval = 2000; interval >= 10; interval -= interval / 10) {
gptStartContinuous(&GPTD4, interval - 1); /* Slightly out of phase.*/
gptStartContinuous(&GPTD3, interval + 1); /* Slightly out of phase.*/
chThdSleepMilliseconds(1000);
gptStopTimer(&GPTD4);
gptStopTimer(&GPTD3);
if (!saturated)
print(".");
else {
print("#");
if (threshold == 0)
threshold = interval;
}
}
/* Gives the worker threads a chance to empty the mailboxes before next
cycle.*/
chThdSleepMilliseconds(20);
println("");
print("Saturated at ");
printn(threshold);
println(" uS");
println("");
if (threshold > worst)
worst = threshold;
}
gptStopTimer(&GPTD4);
gptStopTimer(&GPTD3);
print("Worst case at ");
printn(worst);
println(" uS");
println("");
println("Test Complete");
/*
* Normal main() thread activity, nothing in this test.
*/
while (true) {
chThdSleepMilliseconds(5000);
}
}
/**
* @brief IRQ storm execution.
*
* @param[in] cfg pointer to the test configuration structure
*
* @api
*/
void irq_storm_execute(const irq_storm_config_t *cfg) {
unsigned i;
gptcnt_t interval, threshold, worst;
/* Global configuration pointer.*/
config = cfg;
/* Starting timers using the stored configurations.*/
gptStart(cfg->gpt1p, cfg->gptcfg1p);
gptStart(cfg->gpt2p, cfg->gptcfg2p);
/*
* Initializes the mailboxes and creates the worker threads.
*/
for (i = 0; i < IRQ_STORM_CFG_NUM_THREADS; i++) {
chMBObjectInit(&mb[i], b[i], IRQ_STORM_CFG_MAILBOX_SIZE);
threads[i] = chThdCreateStatic(irq_storm_thread_wa[i],
sizeof irq_storm_thread_wa[i],
IRQ_STORM_CFG_THREADS_PRIORITY,
irq_storm_thread,
(void *)i);
}
/* Printing environment information.*/
chprintf(cfg->out, "");
chprintf(cfg->out, "\r\n*** ChibiOS/RT IRQ-STORM long duration test\r\n***\r\n");
chprintf(cfg->out, "*** Kernel: %s\r\n", CH_KERNEL_VERSION);
chprintf(cfg->out, "*** Compiled: %s\r\n", __DATE__ " - " __TIME__);
#ifdef PORT_COMPILER_NAME
chprintf(cfg->out, "*** Compiler: %s\r\n", PORT_COMPILER_NAME);
#endif
chprintf(cfg->out, "*** Architecture: %s\r\n", PORT_ARCHITECTURE_NAME);
#ifdef PORT_CORE_VARIANT_NAME
chprintf(cfg->out, "*** Core Variant: %s\r\n", PORT_CORE_VARIANT_NAME);
#endif
chprintf(cfg->out, "*** System Clock: %d\r\n", cfg->sysclk);
#ifdef PORT_INFO
chprintf(cfg->out, "*** Port Info: %s\r\n", PORT_INFO);
#endif
#ifdef PLATFORM_NAME
chprintf(cfg->out, "*** Platform: %s\r\n", PLATFORM_NAME);
#endif
#ifdef BOARD_NAME
chprintf(cfg->out, "*** Test Board: %s\r\n", BOARD_NAME);
#endif
chprintf(cfg->out, "***\r\n");
chprintf(cfg->out, "*** Iterations: %d\r\n", IRQ_STORM_CFG_ITERATIONS);
chprintf(cfg->out, "*** Randomize: %d\r\n", IRQ_STORM_CFG_RANDOMIZE);
chprintf(cfg->out, "*** Threads: %d\r\n", IRQ_STORM_CFG_NUM_THREADS);
chprintf(cfg->out, "*** Mailbox size: %d\r\n\r\n", IRQ_STORM_CFG_MAILBOX_SIZE);
/* Test loop.*/
worst = 0;
for (i = 1; i <= IRQ_STORM_CFG_ITERATIONS; i++){
chprintf(cfg->out, "Iteration %d\r\n", i);
saturated = false;
threshold = 0;
/* Timer intervals starting at 2mS then decreased by 10% after each
cycle.*/
for (interval = 2000; interval >= 2; interval -= (interval + 9) / 10) {
/* Timers programmed slightly out of phase each other.*/
gptStartContinuous(cfg->gpt1p, interval - 1); /* Slightly out of phase.*/
gptStartContinuous(cfg->gpt2p, interval + 1); /* Slightly out of phase.*/
/* Storming for one second.*/
chThdSleepMilliseconds(1000);
/* Timers stopped.*/
gptStopTimer(cfg->gpt1p);
gptStopTimer(cfg->gpt2p);
/* Did the storm saturate the threads chain?*/
if (!saturated)
chprintf(cfg->out, ".");
else {
chprintf(cfg->out, "#");
if (threshold == 0)
threshold = interval;
break;
}
}
/* Gives threads a chance to empty the mailboxes before next cycle.*/
chThdSleepMilliseconds(20);
chprintf(cfg->out, "\r\nSaturated at %d uS\r\n\r\n", threshold);
if (threshold > worst)
worst = threshold;
}
gptStopTimer(cfg->gpt1p);
gptStopTimer(cfg->gpt2p);
chprintf(cfg->out, "Worst case at %d uS\r\n", worst);
chprintf(cfg->out, "\r\nTest Complete\r\n");
/* Terminating threads and cleaning up.*/
for (i = 0; i < IRQ_STORM_CFG_NUM_THREADS; i++) {
chThdTerminate(threads[i]);
chThdWait(threads[i]);
threads[i] = NULL;
}
}
/*
* Application entry point.
*/
int main(void) {
thread_t *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();
/*
* Shell manager initialization.
*/
shellInit();
/*
* Initializes a serial-over-USB CDC driver.
*/
sduObjectInit(&SDU1);
sduStart(&SDU1, &serusbcfg);
/*
* 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.
*/
usbDisconnectBus(serusbcfg.usbp);
chThdSleepMilliseconds(1000);
usbStart(serusbcfg.usbp, &usbcfg);
usbConnectBus(serusbcfg.usbp);
/*
* Creating the blinker threads.
*/
chThdCreateStatic(waThread1, sizeof(waThread1),
NORMALPRIO + 10, Thread1, NULL);
chThdCreateStatic(waThread2, sizeof(waThread2),
NORMALPRIO + 10, Thread2, NULL);
/*
* Normal main() thread activity, in this demo it just performs
* a shell respawn upon its termination.
*/
while (true) {
if (!shelltp) {
if (SDU1.config->usbp->state == USB_ACTIVE) {
/* Spawns a new shell.*/
shelltp = shellCreate(&shell_cfg1, SHELL_WA_SIZE, NORMALPRIO);
}
}
else {
/* If the previous shell exited.*/
if (chThdTerminatedX(shelltp)) {
/* Recovers memory of the previous shell.*/
chThdRelease(shelltp);
shelltp = NULL;
}
}
chThdSleepMilliseconds(500);
}
}
void cmd_ledctrl(BaseSequentialStream *chp, int argc, char *argv[])
{
int i;
if (argc >= 1 && strcmp(argv[0], "test1") == 0)
{
chprintf(chp, "Red ...\r\n");
for (i = 0; i < LEDSTRIPE_FRAMEBUFFER_SIZE; i++)
{
ledstripe_framebuffer[i].red = 255;
ledstripe_framebuffer[i].green = 0;
ledstripe_framebuffer[i].blue = 0;
}
chThdSleepMilliseconds(5000);
chprintf(chp, "Green ...\r\n");
for (i = 0; i < LEDSTRIPE_FRAMEBUFFER_SIZE; i++)
{
ledstripe_framebuffer[i].green = 255;
ledstripe_framebuffer[i].red = 0;
ledstripe_framebuffer[i].blue = 0;
}
chThdSleepMilliseconds(5000);
chprintf(chp, "Blue ...\r\n");
for (i = 0; i < LEDSTRIPE_FRAMEBUFFER_SIZE; i++)
{
ledstripe_framebuffer[i].blue = 255;
ledstripe_framebuffer[i].red = 0;
ledstripe_framebuffer[i].green = 0;
}
chThdSleepMilliseconds(5000);
}
else if (argc >= 1 && strcmp(argv[0], "on") == 0)
{
for (i = 0; i < LEDSTRIPE_FRAMEBUFFER_SIZE; i++)
{
ledstripe_framebuffer[i].red = 255;
ledstripe_framebuffer[i].green = 255;
ledstripe_framebuffer[i].blue = 255;
}
}
else if (argc >= 1 && strcmp(argv[0], "off") == 0)
{
for (i = 0; i < LEDSTRIPE_FRAMEBUFFER_SIZE; i++)
{
ledstripe_framebuffer[i].red = 0;
ledstripe_framebuffer[i].green = 0;
ledstripe_framebuffer[i].blue = 0;
}
}
else if (argc >= 4 && strcmp(argv[0], "end") == 0)
{
int red = atoi(argv[1]);
int green = atoi(argv[2]);
int blue = atoi(argv[3]);
for (i = 0; i < LEDSTRIPE_FRAMEBUFFER_SIZE; i++)
{
ledstripe_framebuffer[i].red = 0;
ledstripe_framebuffer[i].green = 0;
ledstripe_framebuffer[i].blue = 0;
}
for (i = LED_END_POSTION; i < LEDSTRIPE_FRAMEBUFFER_SIZE; i++)
{
ledstripe_framebuffer[i].red = red;
ledstripe_framebuffer[i].green = green;
ledstripe_framebuffer[i].blue = blue;
}
}
else if (argc >= 1 && strcmp(argv[0], "start") == 0)
{
chprintf(chp, "Start led thread ...");
chThdCreateStatic(waLEDstripBlink, sizeof(waLEDstripBlink), NORMALPRIO,
ledThread, NULL);
chprintf(chp, " Done\r\n");
}
else if (argc >= 1 && strcmp(argv[0], "show") == 0)
{
chprintf(chp, "\r\n");
for (i = 0; i < LEDSTRIPE_FRAMEBUFFER_SIZE; i++)
{
chprintf(chp, "%.2X%.2X%.2X ",
ledstripe_framebuffer[i].red,
ledstripe_framebuffer[i].green,
ledstripe_framebuffer[i].blue);
}
chprintf(chp, "\r\n");
}
else if (argc >= 1) /* Update the LEDs directly */
{
int i,j= 0, color = 0;
long int number = 0;
int length = strlen(argv[0]);
char pEnd[2] = { '0', '0' };
for(i=0; i < length; i+=2){
memcpy(pEnd, argv[0] +i, 2);
number = strtol(pEnd, NULL, 16);
chprintf(chp, "%d %d\r\n", j, number);
switch(color)
{
case 0:
ledstripe_framebuffer[j].red = number;
break;
case 1:
ledstripe_framebuffer[j].green = number;
break;
case 2:
ledstripe_framebuffer[j].blue = number;
j++;
color=-1;
break;
}
color++;
}
}
else /* Usage */
{
chprintf(chp, "possible arguments are:\r\n"
"-test1\r\n"
"-start\r\n"
"-end (red) (green) (blue)\tSet the last box\r\n"
"-on\r\n"
"-off\r\n"
"-show\r\n");
}
}
void app_gurgalof_init(void) {
chThdCreateStatic(gurgalof_thread_wa, sizeof(gurgalof_thread_wa), NORMALPRIO, gurgalof_thread, NULL);
}
static void evt2_execute(void) {
eventmask_t m;
event_listener_t el1, el2;
systime_t target_time;
/*
* Test on chEvtWaitOne() without wait.
*/
chEvtAddEvents(7);
m = chEvtWaitOne(ALL_EVENTS);
test_assert(1, m == 1, "single event error");
m = chEvtWaitOne(ALL_EVENTS);
test_assert(2, m == 2, "single event error");
m = chEvtWaitOne(ALL_EVENTS);
test_assert(3, m == 4, "single event error");
m = chEvtGetAndClearEvents(ALL_EVENTS);
test_assert(4, m == 0, "stuck event");
/*
* Test on chEvtWaitOne() with wait.
*/
test_wait_tick();
target_time = chVTGetSystemTime() + MS2ST(50);
threads[0] = chThdCreateStatic(wa[0], WA_SIZE, chThdGetPriorityX() - 1,
thread1, chThdGetSelfX());
m = chEvtWaitOne(ALL_EVENTS);
test_assert_time_window(5, target_time, target_time + ALLOWED_DELAY);
test_assert(6, m == 1, "single event error");
m = chEvtGetAndClearEvents(ALL_EVENTS);
test_assert(7, m == 0, "stuck event");
test_wait_threads();
/*
* Test on chEvtWaitAny() without wait.
*/
chEvtAddEvents(5);
m = chEvtWaitAny(ALL_EVENTS);
test_assert(8, m == 5, "unexpected pending bit");
m = chEvtGetAndClearEvents(ALL_EVENTS);
test_assert(9, m == 0, "stuck event");
/*
* Test on chEvtWaitAny() with wait.
*/
test_wait_tick();
target_time = chVTGetSystemTime() + MS2ST(50);
threads[0] = chThdCreateStatic(wa[0], WA_SIZE, chThdGetPriorityX() - 1,
thread1, chThdGetSelfX());
m = chEvtWaitAny(ALL_EVENTS);
test_assert_time_window(10, target_time, target_time + ALLOWED_DELAY);
test_assert(11, m == 1, "single event error");
m = chEvtGetAndClearEvents(ALL_EVENTS);
test_assert(12, m == 0, "stuck event");
test_wait_threads();
/*
* Test on chEvtWaitAll().
*/
chEvtObjectInit(&es1);
chEvtObjectInit(&es2);
chEvtRegisterMask(&es1, &el1, 1);
chEvtRegisterMask(&es2, &el2, 4);
test_wait_tick();
target_time = chVTGetSystemTime() + MS2ST(50);
threads[0] = chThdCreateStatic(wa[0], WA_SIZE, chThdGetPriorityX() - 1,
thread2, "A");
m = chEvtWaitAll(5);
test_assert_time_window(13, target_time, target_time + ALLOWED_DELAY);
m = chEvtGetAndClearEvents(ALL_EVENTS);
test_assert(14, m == 0, "stuck event");
test_wait_threads();
chEvtUnregister(&es1, &el1);
chEvtUnregister(&es2, &el2);
test_assert(15, !chEvtIsListeningI(&es1), "stuck listener");
test_assert(16, !chEvtIsListeningI(&es2), "stuck listener");
}
/**
* @brief Application entry point.
* @details
*/
int main(void) {
/* 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();
/* Initializes a serial-over-USB CDC driver. */
sduObjectInit(&SDU1);
sduStart(&SDU1, &serusbcfg);
/* 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.
*/
usbStop(serusbcfg.usbp);
usbDisconnectBus(serusbcfg.usbp);
chThdSleepMilliseconds(500);
usbConnectBus(serusbcfg.usbp);
usbStart(serusbcfg.usbp, &usbcfg);
/* Activates the serial driver 4 using the driver's default configuration. */
sdStart(&SD4, NULL);
/* Activates the I2C driver 2. */
i2cInit();
i2cStart(&I2CD2, &i2cfg_d2);
/* Enables the CRC peripheral clock. */
rccEnableCRC(FALSE);
/* Loads settings from external EEPROM chip. */
if (eepromLoadSettings()) {
g_boardStatus |= EEPROM_24C02_DETECTED;
}
/* Initializes the MPU6050 sensor. */
if (mpu6050Init()) {
g_boardStatus |= MPU6050_LOW_DETECTED;
/* Creates a taken binary semaphore. */
chBSemInit(&bsemNewDataReady, TRUE);
/* Creates the MPU6050 polling thread and attitude calculation thread. */
chThdCreateStatic(waPollMPU6050Thread, sizeof(waPollMPU6050Thread),
NORMALPRIO + 1, PollMPU6050Thread, NULL);
chThdCreateStatic(waAttitudeThread, sizeof(waAttitudeThread),
HIGHPRIO, AttitudeThread, NULL);
/* Starts motor drivers. */
pwmOutputStart();
/* Starts ADC and ICU input drivers. */
mixedInputStart();
}
/* Creates the blinker thread. */
chThdCreateStatic(waBlinkerThread, sizeof(waBlinkerThread),
LOWPRIO, BlinkerThread, NULL);
/* Normal main() thread activity. */
while (TRUE) {
g_chnp = serusbcfg.usbp->state == USB_ACTIVE ? (BaseChannel *)&SDU1 : (BaseChannel *)&SD4;
telemetryReadSerialData();
if (eepromIsDataLeft()) {
eepromContinueSaving();
}
chThdSleepMilliseconds(TELEMETRY_SLEEP_MS);
}
/* This point should never be reached. */
return 0;
}
/* Use this AFTER ChibiOS threads have started */
void iwdgPostStart(void) {
iwdg_lld_reload();
chThdCreateStatic(wa, sizeof(wa), NORMALPRIO, wdthread, &timeout_ms);
}
void app_sten_init(void) {
chThdCreateStatic(uart_thread_wa, sizeof(uart_thread_wa), NORMALPRIO - 1, uart_thread, NULL);
}
int main(void) {
static const evhandler_t evhndl_main[] = {
extdetail_WKUP_button_handler
};
struct EventListener el0;
/*
* 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();
extdetail_init();
palSetPad(GPIOC, GPIOC_LED);
/*!
* GPIO Pins for generating pulses at data input detect and data output send.
* Used for measuring latency timing of data
*
* \sa board.h
*/
palClearPad( TIMEOUTPUT_PORT, TIMEOUTPUT_PIN);
palSetPadMode(TIMEOUTPUT_PORT, TIMEOUTPUT_PIN, PAL_MODE_OUTPUT_PUSHPULL);
palSetPad( TIMEINPUT_PORT, TIMEINPUT_PIN);
palSetPadMode(TIMEINPUT_PORT, TIMEINPUT_PIN, PAL_MODE_OUTPUT_PUSHPULL );
/*
* I2C2 I/O pins setup
*/
palSetPadMode(si_i2c_connections.i2c_sda_port , si_i2c_connections.i2c_sda_pad,
PAL_MODE_ALTERNATE(4) | PAL_STM32_OTYPE_OPENDRAIN | PAL_STM32_OSPEED_HIGHEST |PAL_STM32_PUDR_FLOATING );
palSetPadMode(si_i2c_connections.i2c_scl_port, si_i2c_connections.i2c_scl_pad,
PAL_MODE_ALTERNATE(4) | PAL_STM32_OSPEED_HIGHEST | PAL_STM32_PUDR_FLOATING);
palSetPad(si_i2c_connections.i2c_scl_port, si_i2c_connections.i2c_scl_pad );
static const ShellCommand commands[] = {
{"mem", cmd_mem},
{"threads", cmd_threads},
{NULL, NULL}
};
usbSerialShellStart(commands);
mpu9150_start(&I2CD2);
i2cStart(mpu9150_driver.i2c_instance, &si_i2c_config);
mpu9150_init(mpu9150_driver.i2c_instance);
/* Administrative threads */
chThdCreateStatic(waThread_blinker, sizeof(waThread_blinker), NORMALPRIO, Thread_blinker, NULL);
chThdCreateStatic(waThread_indwatchdog, sizeof(waThread_indwatchdog), NORMALPRIO, Thread_indwatchdog, NULL);
/* MAC */
/*!
* Use a locally administered MAC address second LSbit of MSB of MAC should be 1
* Use unicast address LSbit of MSB of MAC should be 0
*/
data_udp_init();
chThdCreateStatic(wa_lwip_thread , sizeof(wa_lwip_thread) , NORMALPRIO + 2, lwip_thread , SENSOR_LWIP);
chThdCreateStatic(wa_data_udp_send_thread , sizeof(wa_data_udp_send_thread) , NORMALPRIO , data_udp_send_thread , NULL);
/* i2c MPU9150 */
chThdCreateStatic(waThread_mpu9150_int, sizeof(waThread_mpu9150_int) , NORMALPRIO , Thread_mpu9150_int, NULL);
/* SPI ADIS */
//chThdCreateStatic(waThread_adis_dio1, sizeof(waThread_adis_dio1), NORMALPRIO, Thread_adis_dio1, NULL);
//chThdCreateStatic(waThread_adis_newdata, sizeof(waThread_adis_newdata), NORMALPRIO, Thread_adis_newdata, NULL);
/*! Activates the EXT driver 1. */
extStart(&EXTD1, &extcfg);
chEvtRegister(&extdetail_wkup_event, &el0, 0);
while (TRUE) {
chEvtDispatch(evhndl_main, chEvtWaitOneTimeout((eventmask_t)1, MS2ST(500)));
}
}
/*
* Application entry point.
*/
int main(void) {
thread_t *shelltp1 = NULL;
thread_t *shelltp2 = 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();
/*
* Initializes a serial-over-USB CDC driver.
*/
sduObjectInit(&SDU1);
sduStart(&SDU1, &serusbcfg1);
sduObjectInit(&SDU2);
sduStart(&SDU2, &serusbcfg2);
/*
* 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.
*/
usbDisconnectBus(serusbcfg1.usbp);
chThdSleepMilliseconds(1500);
usbStart(serusbcfg1.usbp, &usbcfg);
usbConnectBus(serusbcfg1.usbp);
/*
* Shell manager initialization.
*/
shellInit();
/*
* Creates the blinker thread.
*/
chThdCreateStatic(waThread1, sizeof(waThread1), NORMALPRIO, Thread1, NULL);
/*
* Normal main() thread activity, in this demo it does nothing except
* sleeping in a loop and check the button state.
*/
while (true) {
if (!shelltp1 && (SDU1.config->usbp->state == USB_ACTIVE))
shelltp1 = shellCreate(&shell_cfg1, SHELL_WA_SIZE, NORMALPRIO);
else if (chThdTerminatedX(shelltp1)) {
chThdRelease(shelltp1); /* Recovers memory of the previous shell. */
shelltp1 = NULL; /* Triggers spawning of a new shell. */
}
if (!shelltp2 && (SDU2.config->usbp->state == USB_ACTIVE))
shelltp2 = shellCreate(&shell_cfg2, SHELL_WA_SIZE, NORMALPRIO);
else if (chThdTerminatedX(shelltp2)) {
chThdRelease(shelltp2); /* Recovers memory of the previous shell. */
shelltp2 = NULL; /* Triggers spawning of a new shell. */
}
chThdSleepMilliseconds(1000);
}
}
/*
* Application entry point.
*/
int main(void) {
/*
* 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();
/*
* Activates the serial driver 2 and SDC driver 1 using default
* configuration.
*/
sdStart(&SD6, NULL);
BaseSequentialStream *chp = (BaseSequentialStream *)&SD6;
chprintf(chp, "running main()\r\n");
chThdSleepMilliseconds(50);
#if STM32_USB_USE_OTG2
USBDriver *usb_driver = &USBD2;
#else
USBDriver *usb_driver = &USBD1;
#endif
/*
* Activates the card insertion monitor.
*/
init_sd();
chprintf(chp, "done starting SDC\r\n");
const bool_t sdcConnectStatus = sdcConnect(&SDCD1);
if( sdcConnectStatus != CH_SUCCESS ) {
chprintf(chp, "failed to connect to SD Card, sdcConnectStatus = %u\r\n", sdcConnectStatus);
for(;;) {
chThdSleepMilliseconds(3000);
}
}
chprintf(chp, "setting up MSD\r\n");
const usb_msd_driver_state_t msd_driver_state = msdInit(usb_driver, (BaseBlockDevice*)&SDCD1, &UMSD1, USB_MS_DATA_EP, USB_MSD_INTERFACE_NUMBER);
if( msd_driver_state != USB_MSD_DRIVER_OK ) {
chprintf(chp, "Error initing USB MSD, %d %s\r\n", msd_driver_state, usb_msd_driver_state_t_to_str(msd_driver_state));
}
UMSD1.chp = chp;
chprintf(chp, "Initializing SDU1...\r\n");
serusbcfg.usbp = usb_driver;
sduObjectInit(&SDU1);
/*Disconnect the USB Bus*/
usbDisconnectBus(usb_driver);
chThdSleepMilliseconds(200);
/*Start the useful functions*/
sduStart(&SDU1, &serusbcfg);
msdStart(&UMSD1);
usbStart(usb_driver, &msd_usb_config);
/*Connect the USB Bus*/
usbConnectBus(usb_driver);
/*
* Creates the blinker thread.
*/
chprintf(chp, "starting blinker thread\r\n");
chThdCreateStatic(waThread1, sizeof(waThread1), NORMALPRIO, Thread1, NULL);
while (TRUE) {
palTogglePad(GPIOC, GPIOC_LED);
chThdSleepMilliseconds(500);
}
}
/*
* Application entry point.
*/
int main(void) {
size_t start = 1100;
size_t end = 1150;
volatile int32_t adc_its = 0;
volatile int32_t spi_its = 0;
volatile int32_t uart_its = 0;
volatile int32_t adc_its_idle = 0;
volatile int32_t spi_its_idle = 0;
volatile int32_t uart_its_idle = 0;
volatile uint32_t idle_thread_cnt = 0;
#if EMCNAND_USE_KILL_TEST
size_t kill = 8000;
#endif
/*
* 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();
emcStart(&EMCD1, &emccfg);
#if !STM32_EMC_EMCNAND_USE_FSMC_INT
extStart(&EXTD1, &extcfg);
#endif
emcnandStart(&EMCNANDD1, &nandcfg);
chThdSleepMilliseconds(4000);
chThdCreateStatic(fsmcIdleThreadWA,
sizeof(fsmcIdleThreadWA),
NORMALPRIO - 20,
fsmcIdleThread,
NULL);
nand_wp_release();
dma_storm_adc_start();
dma_storm_uart_start();
dma_storm_spi_start();
T = chVTGetSystemTimeX();
general_test(&EMCNANDD1, start, end, 1);
T = chVTGetSystemTimeX() - T;
adc_its = dma_storm_adc_stop();
uart_its = dma_storm_uart_stop();
spi_its = dma_storm_spi_stop();
chSysLock();
idle_thread_cnt = IdleCnt;
IdleCnt = 0;
chSysUnlock();
dma_storm_adc_start();
dma_storm_uart_start();
dma_storm_spi_start();
chThdSleep(T);
adc_its_idle = dma_storm_adc_stop();
uart_its_idle = dma_storm_uart_stop();
spi_its_idle = dma_storm_spi_stop();
osalDbgCheck(idle_thread_cnt > (IdleCnt / 4));
osalDbgCheck(abs(adc_its - adc_its_idle) < (adc_its_idle / 20));
osalDbgCheck(abs(uart_its - uart_its_idle) < (uart_its_idle / 20));
osalDbgCheck(abs(spi_its - spi_its_idle) < (spi_its_idle / 10));
ecc_test(&EMCNANDD1, end);
#if EMCNAND_USE_KILL_TEST
kill_block(&EMCNANDD1, kill);
#endif
nand_wp_assert();
/*
* Normal main() thread activity, in this demo it does nothing.
*/
while (TRUE) {
chThdSleepMilliseconds(500);
}
}
/**
* @brief Create statically allocated shell thread.
*
* @param[in] scp pointer to a @p ShellConfig object
* @param[in] wsp pointer to a working area dedicated to the shell thread stack
* @param[in] size size of the shell working area
* @param[in] prio priority level for the new shell
* @return A pointer to the shell thread.
*/
Thread *shellCreateStatic(const ShellConfig *scp, void *wsp,
size_t size, tprio_t prio) {
return chThdCreateStatic(wsp, size, prio, shell_thread, (void *)scp);
}
/*
* Application entry point.
*/
int main(void) {
color_t color = Black;
uint16_t pen = 0;
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();
gdispInit();
ginputGetMouse(0);
gdispSetOrientation(GDISP_ROTATE_90);
drawScreen();
/*
* Initializes a serial-over-USB CDC driver.
*/
sduObjectInit(&SDU1);
sduStart(&SDU1, &serusbcfg);
/*
* 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.
*/
usbDisconnectBus(serusbcfg.usbp);
chThdSleepMilliseconds(1000);
usbStart(serusbcfg.usbp, &usbcfg);
usbConnectBus(serusbcfg.usbp);
/*
* Shell manager initialization.
*/
shellInit();
/*
* Creates the blinker thread.
*/
chThdCreateStatic(waThread1, sizeof(waThread1), NORMALPRIO, Thread1, NULL);
/*
* Normal main() thread activity, in this demo it does nothing except
* sleeping in a loop and check the button state.
*/
while (TRUE) {
if (!shelltp && (SDU1.config->usbp->state == USB_ACTIVE))
shelltp = shellCreate(&shell_cfg1, SHELL_WA_SIZE, NORMALPRIO);
else if (chThdTerminated(shelltp)) {
chThdRelease(shelltp); /* Recovers memory of the previous shell. */
shelltp = NULL; /* Triggers spawning of a new shell. */
}
ginputGetMouseStatus(0, &ev);
if (!(ev.current_buttons & GINPUT_MOUSE_BTN_LEFT))
{
chThdSleepMicroseconds(500); // Senza questo sleep l'USB non parte
continue;
}
/* inside color box ? */
if(ev.y >= OFFSET && ev.y <= COLOR_SIZE) {
if(GET_COLOR(0)) color = Black;
else if(GET_COLOR(1)) color = Red;
else if(GET_COLOR(2)) color = Yellow;
else if(GET_COLOR(3)) color = Green;
else if(GET_COLOR(4)) color = Blue;
else if(GET_COLOR(5)) color = White;
/* inside pen box ? */
} else if(ev.x >= OFFSET && ev.x <= PEN_SIZE) {
if(GET_PEN(1)) pen = 0;
else if(GET_PEN(2)) pen = 1;
else if(GET_PEN(3)) pen = 2;
else if(GET_PEN(4)) pen = 3;
else if(GET_PEN(5)) pen = 4;
/* inside drawing area ? */
} else if(DRAW_AREA(ev.x, ev.y)) {
if(pen == 0)
gdispDrawPixel(ev.x, ev.y, color);
else
gdispFillCircle(ev.x, ev.y, pen, color);
}
}
}
/**
* @brief Initialize the HMC5883 sensor thread
*/
void hmc5883_init(const struct hmc5883_cfg *cfg)
{
dev_cfg = cfg;
thdp_hmc5883 = chThdCreateStatic(wa_hmc5883, sizeof(wa_hmc5883), HIGHPRIO, thd_hmc5883, 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();
/*
* Shell manager initialization.
*/
shellInit();
/*
* Initializes a serial-over-USB CDC driver.
*/
sduObjectInit(&SDU1);
sduStart(&SDU1, &serusbcfg);
/*
* 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.
*/
usbDisconnectBus(serusbcfg.usbp);
chThdSleepMilliseconds(1000);
usbStart(serusbcfg.usbp, &usbcfg);
usbConnectBus(serusbcfg.usbp);
/*
* Initialise SDRAM, board.h has already configured GPIO correctly (except that ST example uses 50MHz not 100MHz?)
*/
SDRAM_Init();
sdram_bulk_erase();
/*
* Activates the LCD-related drivers.
*/
spiStart(&SPID5, &spi_cfg5);
ili9341Start(&ILI9341D1, &ili9341_cfg);
initialize_lcd();
ltdcStart(<DCD1, <dc_cfg);
/*
* Activates the DMA2D-related drivers.
*/
dma2dStart(&DMA2DD1, &dma2d_cfg);
dma2d_test();
/*
* Creating the blinker threads.
*/
chThdCreateStatic(waThread1, sizeof(waThread1), NORMALPRIO + 10,
Thread1, NULL);
chThdCreateStatic(waThread2, sizeof(waThread2), NORMALPRIO + 10,
Thread2, NULL);
/*
* Normal main() thread activity, in this demo it just performs
* a shell respawn upon its termination.
*/
while (TRUE) {
if (!shelltp) {
if (SDU1.config->usbp->state == USB_ACTIVE) {
/* Spawns a new shell.*/
shelltp = shellCreate(&shell_cfg1, SHELL_WA_SIZE, NORMALPRIO);
}
}
else {
/* If the previous shell exited.*/
if (chThdTerminated(shelltp)) {
/* Recovers memory of the previous shell.*/
chThdRelease(shelltp);
shelltp = NULL;
}
}
chThdSleepMilliseconds(500);
}
}
/*
* Application entry point.
*/
int main(void) {
/*
* 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();
/*
* Initializes a serial-over-USB CDC driver.
*/
sduObjectInit(&SDU1);
sduStart(&SDU1, &serusbcfg);
/*
* 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.
*/
usbDisconnectBus(serusbcfg.usbp);
chThdSleepMilliseconds(1500);
usbStart(serusbcfg.usbp, &usbcfg);
usbConnectBus(serusbcfg.usbp);
/*
* Creates the blinker thread.
*/
chThdCreateStatic(waThread1, sizeof(waThread1), NORMALPRIO + 1, Thread1, NULL);
/*
* L3GD20 Object Initialization
*/
l3gd20ObjectInit(&L3GD20D1);
/*
* Activates the L3GD20 driver.
*/
l3gd20Start(&L3GD20D1, &l3gd20cfg);
while(!palReadLine(LINE_BUTTON)){
chprintf(chp, "Press BTN to calibrate gyroscope...\r\n");
chThdSleepMilliseconds(150);
#if CHPRINTF_USE_ANSI_CODE
chprintf(chp, "\033[2J\033[1;1H");
#endif
}
chprintf(chp, "Calibrating Gyroscope sampling bias...\r\n");
chprintf(chp, "Keep it in the rest position while red LED is on\r\n");
chThdSleepMilliseconds(3000);
palSetLine(LINE_LED10_RED);
chThdSleepMilliseconds(1000);
gyroscopeSampleBias(&L3GD20D1);
palClearLine(LINE_LED10_RED);
#if CHPRINTF_USE_ANSI_CODE
chprintf(chp, "\033[2J\033[1;1H");
#endif
while (TRUE) {
palToggleLine(LINE_LED10_RED);
gyroscopeReadRaw(&L3GD20D1, rawdata);
for(i = 0; i < L3GD20_NUMBER_OF_AXES; i++)
chprintf(chp, "RAW-%c:%d\r\n", axesID[i], rawdata[i]);
gyroscopeReadCooked(&L3GD20D1, cookeddata);
for(i = 0; i < L3GD20_NUMBER_OF_AXES; i++)
chprintf(chp, "COOKED-%c:%.3f\r\n", axesID[i], cookeddata[i]);
gyroscopeGetTemp(&L3GD20D1, &temperature);
chprintf(chp, "TEMP:%.1f C°\r\n", temperature);
chThdSleepMilliseconds(150);
#if CHPRINTF_USE_ANSI_CODE
chprintf(chp, "\033[2J\033[1;1H");
#endif
}
l3gd20Stop(&L3GD20D1);
}
/*
* Application entry point.
*/
int main(void) {
unsigned i;
gptcnt_t interval, threshold, worst;
/*
* 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();
/*
* Prepares the Serial driver 2 and GPT drivers 1 and 2.
*/
sdStart(&SD1, NULL); /* Default is 38400-8-N-1.*/
gptStart(&GPTD1, &gpt1cfg);
gptStart(&GPTD2, &gpt2cfg);
/*
* Initializes the mailboxes and creates the worker threads.
*/
for (i = 0; i < NUM_THREADS; i++) {
chMBInit(&mb[i], b[i], MAILBOX_SIZE);
chThdCreateStatic(waWorkerThread[i], sizeof waWorkerThread[i],
NORMALPRIO - 20, WorkerThread, (void *)i);
}
/*
* Test procedure.
*/
println("");
println("*** ChibiOS/RT IRQ-STORM long duration test");
println("***");
print("*** Kernel: ");
println(CH_KERNEL_VERSION);
#ifdef CH_COMPILER_NAME
print("*** Compiler: ");
println(CH_COMPILER_NAME);
#endif
print("*** Architecture: ");
println(CH_ARCHITECTURE_NAME);
#ifdef CH_CORE_VARIANT_NAME
print("*** Core Variant: ");
println(CH_CORE_VARIANT_NAME);
#endif
#ifdef CH_PORT_INFO
print("*** Port Info: ");
println(CH_PORT_INFO);
#endif
#ifdef PLATFORM_NAME
print("*** Platform: ");
println(PLATFORM_NAME);
#endif
#ifdef BOARD_NAME
print("*** Test Board: ");
println(BOARD_NAME);
#endif
println("***");
print("*** System Clock: ");
printn(LPC13xx_SYSCLK);
println("");
print("*** Iterations: ");
printn(ITERATIONS);
println("");
print("*** Randomize: ");
printn(RANDOMIZE);
println("");
print("*** Threads: ");
printn(NUM_THREADS);
println("");
print("*** Mailbox size: ");
printn(MAILBOX_SIZE);
println("");
println("");
worst = 0;
for (i = 1; i <= ITERATIONS; i++){
print("Iteration ");
printn(i);
println("");
saturated = FALSE;
threshold = 0;
for (interval = 2000; interval >= 20; interval -= interval / 10) {
gptStartContinuous(&GPTD1, interval - 1); /* Slightly out of phase.*/
gptStartContinuous(&GPTD2, interval + 1); /* Slightly out of phase.*/
chThdSleepMilliseconds(1000);
gptStopTimer(&GPTD1);
gptStopTimer(&GPTD2);
if (!saturated)
print(".");
else {
print("#");
if (threshold == 0)
threshold = interval;
}
}
/* Gives the worker threads a chance to empty the mailboxes before next
cycle.*/
chThdSleepMilliseconds(20);
println("");
print("Saturated at ");
printn(threshold);
println(" uS");
println("");
if (threshold > worst)
worst = threshold;
}
gptStopTimer(&GPTD1);
gptStopTimer(&GPTD2);
print("Worst case at ");
printn(worst);
println(" uS");
println("");
println("Test Complete");
/*
* Normal main() thread activity, nothing in this test.
*/
while (TRUE) {
chThdSleepMilliseconds(5000);
}
return 0;
}
/*
* Application entry point.
*/
int main(void) {
/*
* 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.
*/
uint32_t tempR, tempP, tempY;
float Roll,Pitch,Yaw;
halInit();
chSysInit();
i2c_setup();
palSetPadMode(IOPORT1, 8, PAL_MODE_INPUT); // PA8 - RADIO INPUT
palSetPadMode(IOPORT2, 13, PAL_MODE_STM32_ALTERNATE_OPENDRAIN); /* SCK. */
palSetPadMode(IOPORT2, 14, PAL_MODE_STM32_ALTERNATE_PUSHPULL); /* MISO.*/
palSetPadMode(IOPORT2, 15, PAL_MODE_STM32_ALTERNATE_OPENDRAIN); /* MOSI.*/
palSetPadMode(IOPORT2, 12, PAL_MODE_STM32_ALTERNATE_OPENDRAIN);
palSetPadMode(IOPORT3, 11, PAL_MODE_OUTPUT_PUSHPULL);
chMtxInit(&mtx_imu);
chMtxInit(&mutex_motors);
palClearPad(IOPORT3,10);
//palClearPad(IOPORT3,11);
/*
* Activates the USB driver and then the USB bus pull-up on D+.
*/
sduObjectInit(&SDU1);
sduStart(&SDU1, &serusbcfg);
usbConnectBus(serusbcfg.usbp);
//palClearPad(GPIOC, GPIOC_USB_DISC);
icuStart(&ICUD1, &icucfg);
icuEnable(&ICUD1);
chThdSleepSeconds(1);
chThdCreateStatic(waThread1, sizeof(waThread1), HIGHPRIO, Thread1, NULL);
//chThdCreateStatic(waMotorsThread, sizeof(waMotorsThread), NORMALPRIO, MotorsThread, NULL);
spiAcquireBus(&SPID2); /* Acquire ownership of the bus. */
spiStart(&SPID2, &ls_spicfg); /* Setup transfer parameters. */
spiSelect(&SPID2);
KP = 1;
KI = 1;
KD = 0;
while (TRUE) {
while(!(SPID2.spi->SR & SPI_SR_RXNE));
spiReceive(&SPID2,24,rxbuf_16bit);
// Floating point calculation of Roll/Pitch/Yaw inside the microcontroller. Decomment next 6 lines if you want to implement
// the control Law.
tempR = (uint32_t)((rxbuf_16bit[0] << 31) | (rxbuf_16bit[1] << 23) | (rxbuf_16bit[2] << 11) | rxbuf_16bit[3]);
tempP = (uint32_t)((rxbuf_16bit[4] << 31) | (rxbuf_16bit[5] << 23) | (rxbuf_16bit[6] << 11) | rxbuf_16bit[7]);
tempY = (uint32_t)((rxbuf_16bit[8] << 31) | (rxbuf_16bit[9] << 23) | (rxbuf_16bit[10] << 11) | rxbuf_16bit[11]);
Roll = (*(float*)&tempR);
Pitch = (*(float*)&tempP);
Yaw = (*(float*)&tempY);
// CONTROL LAW HERE
//++ (ROLL,PITCH,YAW ERRORS) -----> [CONTROLLER] -----> (MOTORS INPUT)
// PID Control Law
roll_error = 0 - Roll;
pitch_error = 0 - Pitch;
yaw_error = 0 - Yaw;
roll_I = roll_I + roll_error*0.01;
pitch_I = pitch_I + pitch_error*0.01;
roll_D = (roll_error - roll_prev_error)/0.01;
pitch_D = (pitch_error - pitch_prev_error)/0.01;
roll_controller_output = (int8_t)(KP*roll_error + KI*roll_I + KD*roll_D);
pitch_controller_output = (int8_t)(KP*pitch_error + KI*pitch_I + KD*pitch_D);
//roll_controller_output = (int8_t)roll_error;
//pitch_controller_output = (int8_t)pitch_error;
roll_prev_error = roll_error;
pitch_prev_error = pitch_error;
//-------------------------------------------------------------------
blctrl20_set_velocity();
palSetPad(IOPORT3,11);
/* SPI FLOATING POINT TEST PACKET (sending 5.6F)
rxbuf_16bit[0] = 0;
rxbuf_16bit[1] = 129;
rxbuf_16bit[2] = 1638;
rxbuf_16bit[3] = 819;
rxbuf_16bit[4] = 0;
rxbuf_16bit[5] = 129;
rxbuf_16bit[6] = 1638;
rxbuf_16bit[7] = 819;
rxbuf_16bit[8] = 1;
rxbuf_16bit[9] = 129;
rxbuf_16bit[10] = 1638;
rxbuf_16bit[11] = 819;
*/
if(SDU1.config->usbp->state==USB_ACTIVE)
{
// chprintf((BaseChannel *)&SDU1,"S:%6d:%6d:%6d:%6d:%6d:%6d:%6d:%6d:%6d:%6d:%6d:%6d:E\r\n",rxbuf_16bit[0],rxbuf_16bit[1],rxbuf_16bit[2],rxbuf_16bit[3],rxbuf_16bit[4],rxbuf_16bit[5],rxbuf_16bit[6],rxbuf_16bit[7],rxbuf_16bit[8],rxbuf_16bit[9],rxbuf_16bit[10],rxbuf_16bit[11]);
chprintf((BaseChannel *)&SDU1, "S:%6D:%6D:%6D:%6D:%6D:%6D:%6D:%6D:%6D:%6D:%6D:%6D:%6D:%6D:%6D:%6D:%6D:%6D:%6D:%6D:E\r\n",rxbuf_16bit[0],rxbuf_16bit[1],rxbuf_16bit[2],rxbuf_16bit[3],rxbuf_16bit[4],rxbuf_16bit[5],rxbuf_16bit[6],rxbuf_16bit[7],rxbuf_16bit[8],rxbuf_16bit[9],rxbuf_16bit[10],rxbuf_16bit[11],(int8_t)Roll,(int8_t)Pitch,(int8_t)Yaw,icu_ch[3],icu_ch[4],roll_controller_output,pitch_controller_output,yaw_controller_output);
}
chThdSleepMilliseconds(10);
}
spiUnselect(&SPID2);
spiReleaseBus(&SPID2); /* Ownership release. */
}
/*
* Application entry point.
*/
int main(void) {
thread_t *shelltp1 = NULL;
thread_t *shelltp2 = NULL;
event_listener_t shell_el;
/*
* 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();
/*
* Initializes two serial-over-USB CDC drivers.
*/
sduObjectInit(&SDU1);
sduStart(&SDU1, &serusbcfg1);
sduObjectInit(&SDU2);
sduStart(&SDU2, &serusbcfg2);
/*
* 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.
*/
usbDisconnectBus(serusbcfg1.usbp);
chThdSleepMilliseconds(1500);
usbStart(serusbcfg1.usbp, &usbcfg);
usbConnectBus(serusbcfg1.usbp);
/*
* Shell manager initialization.
* Event zero is shell exit.
*/
shellInit();
chEvtRegister(&shell_terminated, &shell_el, 0);
/*
* Creates the blinker thread.
*/
chThdCreateStatic(waThread1, sizeof(waThread1), NORMALPRIO, Thread1, NULL);
/*
* Normal main() thread activity, managing two shells.
*/
while (true) {
if (SDU1.config->usbp->state == USB_ACTIVE) {
/* Starting shells.*/
if (shelltp1 == NULL) {
shelltp1 = chThdCreateFromHeap(NULL, SHELL_WA_SIZE,
"shell1", NORMALPRIO + 1,
shellThread, (void *)&shell_cfg1);
}
if (shelltp2 == NULL) {
shelltp2 = chThdCreateFromHeap(NULL, SHELL_WA_SIZE,
"shell2", NORMALPRIO + 1,
shellThread, (void *)&shell_cfg2);
}
/* Waiting for an exit event then freeing terminated shells.*/
chEvtWaitAny(EVENT_MASK(0));
if (chThdTerminatedX(shelltp1)) {
chThdRelease(shelltp1);
shelltp1 = NULL;
}
if (chThdTerminatedX(shelltp2)) {
chThdRelease(shelltp2);
shelltp2 = NULL;
}
}
else {
chThdSleepMilliseconds(1000);
}
}
}
void Lcd_t::Init() {
// ==== Backlight: Timer15 Ch2 ====
// Setup pin
PinSetupAlterFuncOutput(LCD_GPIO, LCD_BCKLT, omPushPull, ps50MHz);
// Remap Timer15 to PB14 & PB15
AFIO->MAPR2 |= 0x00000001;
// Setup timer15
rccEnableAPB2(RCC_APB2ENR_TIM15EN, false);
TIM15->CR1 = 0x01; // Enable timer
TIM15->CR2 = 0;
TIM15->PSC = 0; // Do not divide input freq
TIM15->ARR = 100; // Autoreload register: full brightness=100
TIM15->BDTR = 0xC000; // Main output Enable
TIM15->CCMR1 = 0x6000; // PWM mode1 on Ch2 enabled
TIM15->CCER = 0x0010; // Output2 enabled, polarity not inverted
BackligthValue = 0;
// ==== GPIOs ====
// Configure LCD_XRES, LCD_XCS, LCD_SCLK & LCD_SDA as Push-Pull output
InitGpios();
// ========================= Init LCD ======================================
SCLK_Lo();
XCS_Hi();
// Reset display
XRES_Lo();
chThdSleepMilliseconds(9);
XRES_Hi();
WriteCmd(0xAF); // display ON
// Reset display again
XRES_Lo();
chThdSleepMilliseconds(7);
XRES_Hi();
chThdSleepMilliseconds(7);
// Initial commands
WriteCmd(0xAF); // display ON
WriteCmd(0xA4); // Set normal display mode
WriteCmd(0x2F); // Charge pump on
WriteCmd(0x40); // Set start row address = 0
// WriteCmd(0xC8); // mirror Y axis
// WriteCmd(0xA1); // Mirror X axis
// Set x=0, y=0
WriteCmd(0xB3); // Y axis initialization
WriteCmd(0x10); // X axis initialisation1
WriteCmd(0x08); // X axis initialisation2
Cls(); // clear LCD buffer
draw_mode = OVERWRITE;
// ====================== Switch to USART + DMA ============================
#ifdef ENABLE_DMAUSART_MODE
PinSetupAlterFuncOutput(LCD_GPIO, LCD_SCLK, omPushPull, ps50MHz);
PinSetupAlterFuncOutput(LCD_GPIO, LCD_SDA, omPushPull, ps50MHz);
// Workaround hardware bug with disabled CK3 when SPI2 is enabled
SPI2->CR2 |= SPI_CR2_SSOE;
// ==== USART init ====
rccEnableUSART3(false);
// Usart clock: enabled, idle low, first edge, enable last bit pulse
// Usart itself
LCD_USART->BRR = Clk.APB1FreqHz / 100000;
LCD_USART->CR1 = USART_CR1_TE | /* Transmitter enabled */ \
USART_CR1_M; /* 9 bit */
LCD_USART->CR2 = USART_CR2_CLKEN | \
USART_CR2_LBCL;
LCD_USART->CR3 = USART_CR3_DMAT; // Enable DMA at transmitter
LCD_USART->CR1 |= USART_CR1_UE; // Enable USART
// ==== DMA ====
rccEnableDMA1();
dmaStreamAllocate(LCD_DMA, IRQ_PRIO_MEDIUM, LcdDmaCompIrq, NULL);
dmaStreamSetPeripheral(LCD_DMA, &USART3->DR);
dmaStreamSetMemory0(LCD_DMA, (uint32_t)&IBuf[0]);
dmaStreamSetMode (LCD_DMA, LCD_DMA_MODE);
// Start transmission
XCS_Lo();
// DMA_Cmd(DMA1_Channel2, ENABLE); // Enable USARTy DMA TX Channel
#else
for(int i=0; i < 864; i++) WriteData(0x00); // Clear all screen
#endif
Backlight(0);
chThdCreateStatic(waLcdThread, sizeof(waLcdThread), NORMALPRIO, (tfunc_t)LcdThread, 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();
/*
* Shell manager initialization.
*/
shellInit();
/*
* Initializes a serial-over-USB CDC driver.
*/
sduObjectInit(&SDU1);
sduStart(&SDU1, &serusbcfg);
/*
* 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.
*/
usbDisconnectBus(serusbcfg.usbp);
chThdSleepMilliseconds(1000);
usbStart(serusbcfg.usbp, &usbcfg);
usbConnectBus(serusbcfg.usbp);
/*
* Activates the serial driver 2 using the driver default configuration.
* PA2(TX) and PA3(RX) are routed to USART2.
*/
sdStart(&SD2, NULL);
palSetPadMode(GPIOA, 2, PAL_MODE_ALTERNATE(7));
palSetPadMode(GPIOA, 3, PAL_MODE_ALTERNATE(7));
/*
* Initializes the SPI driver 1 in order to access the MEMS. The signals
* are already initialized in the board file.
*/
spiStart(&SPID1, &spi1cfg);
/*
* Initializes the SPI driver 2. The SPI2 signals are routed as follow:
* PB12 - NSS.
* PB13 - SCK.
* PB14 - MISO.
* PB15 - MOSI.
*/
spiStart(&SPID2, &spi2cfg);
palSetPad(GPIOB, 12);
palSetPadMode(GPIOB, 12, PAL_MODE_OUTPUT_PUSHPULL |
PAL_STM32_OSPEED_HIGHEST); /* NSS. */
palSetPadMode(GPIOB, 13, PAL_MODE_ALTERNATE(5) |
PAL_STM32_OSPEED_HIGHEST); /* SCK. */
palSetPadMode(GPIOB, 14, PAL_MODE_ALTERNATE(5)); /* MISO. */
palSetPadMode(GPIOB, 15, PAL_MODE_ALTERNATE(5) |
PAL_STM32_OSPEED_HIGHEST); /* MOSI. */
/*
* Initializes the PWM driver 4, routes the TIM4 outputs to the board LEDs.
*/
pwmStart(&PWMD4, &pwmcfg);
palSetPadMode(GPIOD, GPIOD_LED4, PAL_MODE_ALTERNATE(2)); /* Green. */
palSetPadMode(GPIOD, GPIOD_LED3, PAL_MODE_ALTERNATE(2)); /* Orange. */
palSetPadMode(GPIOD, GPIOD_LED5, PAL_MODE_ALTERNATE(2)); /* Red. */
palSetPadMode(GPIOD, GPIOD_LED6, PAL_MODE_ALTERNATE(2)); /* Blue. */
/*
* Creates the example thread.
*/
chThdCreateStatic(waThread1, sizeof(waThread1),
NORMALPRIO + 10, Thread1, NULL);
/*
* Normal main() thread activity, in this demo it just performs
* a shell respawn upon its termination.
*/
while (TRUE) {
if (!shelltp) {
if (SDU1.config->usbp->state == USB_ACTIVE) {
/* Spawns a new shell.*/
shelltp = shellCreate(&shell_cfg1, SHELL_WA_SIZE, NORMALPRIO);
}
}
else {
/* If the previous shell exited.*/
if (chThdTerminated(shelltp)) {
/* Recovers memory of the previous shell.*/
chThdRelease(shelltp);
shelltp = NULL;
}
}
chThdSleepMilliseconds(500);
}
}
int main(void) {
static Thread *shelltp = NULL;
static const evhandler_t evhndl_main[] = {
extdetail_WKUP_button_handler
};
struct EventListener el0;
/*
* 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();
extdetail_init();
palSetPad(GPIOC, GPIOC_LED);
palSetPad(GPIOA, GPIOA_SPI1_SCK);
palSetPad(GPIOA, GPIOA_SPI1_NSS);
/*
* SPI1 I/O pins setup.
*/
palSetPadMode(adis_connections.spi_sck_port, adis_connections.spi_sck_pad,
PAL_MODE_ALTERNATE(5) |
PAL_STM32_OSPEED_HIGHEST);
palSetPadMode(adis_connections.spi_miso_port, adis_connections.spi_miso_pad,
PAL_MODE_ALTERNATE(5) |
PAL_STM32_OSPEED_HIGHEST| PAL_STM32_PUDR_FLOATING);
palSetPadMode(adis_connections.spi_mosi_port, adis_connections.spi_mosi_pad,
PAL_MODE_ALTERNATE(5) |
PAL_STM32_OSPEED_HIGHEST );
palSetPadMode(adis_connections.spi_cs_port, adis_connections.spi_cs_pad,
PAL_MODE_OUTPUT_PUSHPULL |
PAL_STM32_OSPEED_HIGHEST);
palSetPad(GPIOA, GPIOA_SPI1_SCK);
palSetPad(GPIOA, GPIOA_SPI1_NSS);
/*!
* Initializes a serial-over-USB CDC driver.
*/
sduObjectInit(&SDU_PSAS);
sduStart(&SDU_PSAS, &serusbcfg);
/*!
* 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.
*/
usbDisconnectBus(serusbcfg.usbp);
chThdSleepMilliseconds(1000);
usbStart(serusbcfg.usbp, &usbcfg);
usbConnectBus(serusbcfg.usbp);
shellInit();
iwdg_begin();
/*!
* Activates the serial driver 6 and SDC driver 1 using default
* configuration.
*/
sdStart(&SD6, NULL);
spiStart(&SPID1, &adis_spicfg); /* Set transfer parameters. */
chThdSleepMilliseconds(300);
adis_init();
adis_reset();
/*! Activates the EXT driver 1. */
extStart(&EXTD1, &extcfg);
chThdCreateStatic(waThread_blinker, sizeof(waThread_blinker), NORMALPRIO, Thread_blinker, NULL);
chThdCreateStatic(waThread_adis_dio1, sizeof(waThread_adis_dio1), NORMALPRIO, Thread_adis_dio1, NULL);
chThdCreateStatic(waThread_adis_newdata, sizeof(waThread_adis_newdata), NORMALPRIO, Thread_adis_newdata, NULL);
chThdCreateStatic(waThread_indwatchdog, sizeof(waThread_indwatchdog), NORMALPRIO, Thread_indwatchdog, NULL);
chEvtRegister(&extdetail_wkup_event, &el0, 0);
while (TRUE) {
if (!shelltp && (SDU_PSAS.config->usbp->state == USB_ACTIVE))
shelltp = shellCreate(&shell_cfg1, SHELL_WA_SIZE, NORMALPRIO);
else if (chThdTerminated(shelltp)) {
chThdRelease(shelltp); /* Recovers memory of the previous shell. */
shelltp = NULL; /* Triggers spawning of a new shell. */
}
chEvtDispatch(evhndl_main, chEvtWaitOneTimeout((eventmask_t)1, MS2ST(500)));
}
}
/*
* Application entry point.
*/
int main(void) {
static const evhandler_t evhndl[] = {
InsertHandler,
RemoveHandler,
ShellHandler
};
event_listener_t el0, el1, el2;
/*
* 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.
* - lwIP subsystem initialization using the default configuration.
*/
halInit();
chSysInit();
lwipInit(NULL);
/*
* Initializes a serial-over-USB CDC driver.
*/
sduObjectInit(&SDU1);
sduStart(&SDU1, &serusbcfg);
/*
* 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.
*/
usbDisconnectBus(serusbcfg.usbp);
chThdSleepMilliseconds(1500);
usbStart(serusbcfg.usbp, &usbcfg);
usbConnectBus(serusbcfg.usbp);
/*
* Shell manager initialization.
*/
shellInit();
/*
* Activates the serial driver 6 and SDC driver 1 using default
* configuration.
*/
sdStart(&SD6, NULL);
sdcStart(&SDCD1, NULL);
/*
* Activates the card insertion monitor.
*/
tmr_init(&SDCD1);
/*
* Creates the blinker thread.
*/
chThdCreateStatic(waThread1, sizeof(waThread1), NORMALPRIO, Thread1, NULL);
/*
* Creates the HTTP thread (it changes priority internally).
*/
chThdCreateStatic(wa_http_server, sizeof(wa_http_server), NORMALPRIO + 1,
http_server, NULL);
/*
* Normal main() thread activity, handling SD card events and shell
* start/exit.
*/
chEvtRegister(&inserted_event, &el0, 0);
chEvtRegister(&removed_event, &el1, 1);
chEvtRegister(&shell_terminated, &el2, 2);
while (true) {
if (!shelltp && (SDU1.config->usbp->state == USB_ACTIVE)) {
shelltp = chThdCreateFromHeap(NULL, SHELL_WA_SIZE,
"shell", NORMALPRIO + 1,
shellThread, (void *)&shell_cfg1);
}
chEvtDispatch(evhndl, chEvtWaitOneTimeout(ALL_EVENTS, MS2ST(500)));
}
}
void pt_init(void)
{
chThdCreateStatic(wa_protocol, sizeof(wa_protocol), NORMALPRIO, thd_protocol, NULL);
}