/* * Heartbeat thread */ static __attribute__((noreturn)) msg_t thd_heartbeat(void *arg) { (void) arg; chRegSetThreadName("pprz heartbeat"); chThdSleepSeconds (SDLOG_START_DELAY); if (usbStorageIsItRunning ()) chThdSleepSeconds (20000); // stuck here for hours else sdOk = chibios_logInit(); while (TRUE) { palTogglePad (GPIOC, GPIOC_LED3); chThdSleepMilliseconds (sdOk == TRUE ? 1000 : 200); static uint32_t timestamp = 0; // we sync gps time to rtc every 5 seconds if (chTimeNow() - timestamp > 5000) { timestamp = chTimeNow(); if (getGpsTimeOfWeek() != 0) { setRtcFromGps (getGpsWeek(), getGpsTimeOfWeek()); } } } }
/** * Main function. */ int main(void){ halInit(); chSysInit(); chThdCreateStatic(blinkWA, sizeof(blinkWA), NORMALPRIO, blink_thd, NULL); #if !WAKEUP_TEST /* switch off wakeup */ rtcSetPeriodicWakeup_v2(&RTCD1, NULL); /* Shell initialization.*/ sdStart(&SD2, &ser_cfg); shellInit(); static WORKING_AREA(waShell, 1024); shellCreateStatic(&shell_cfg1, waShell, sizeof(waShell), NORMALPRIO); /* wait until user do not want to test wakeup */ while (TRUE){ chThdSleepMilliseconds(200); } #else /* set wakeup */ wakeupspec.wakeup = ((uint32_t)4) << 16; /* select 1 Hz clock source */ wakeupspec.wakeup |= 9; /* set counter value to 9. Period will be 9+1 seconds. */ rtcSetPeriodicWakeup_v2(&RTCD1, &wakeupspec); chThdSleepSeconds(3); func_sleep(); #endif /* !WAKEUP_TEST */ return 0; }
static void dma2d_test(void) { DMA2DDriver *const dma2dp = &DMA2DD1; LTDCDriver *const ltdcp = <DCD1; chThdSleepSeconds(1); ltdcBgSetConfig(ltdcp, <dc_screen_laycfg1); ltdcReload(ltdcp, TRUE); dma2dAcquireBus(dma2dp); /* Target the frame buffer by default.*/ dma2dBgSetConfig(dma2dp, &dma2d_frame_laycfg); dma2dFgSetConfig(dma2dp, &dma2d_frame_laycfg); dma2dOutSetConfig(dma2dp, &dma2d_frame_laycfg); /* Copy the background.*/ dma2dFgSetConfig(dma2dp, &dma2d_bg_laycfg); dma2dJobSetMode(dma2dp, DMA2D_JOB_CONVERT); dma2dJobSetSize(dma2dp, 240, 320); dma2dJobExecute(dma2dp); /* Draw the splashscren picture at (8, 0).*/ dma2dFgSetConfig(dma2dp, &dma2d_fg_laycfg); dma2dOutSetAddress(dma2dp, dma2dComputeAddress( frame_buffer, ltdc_screen_frmcfg1.pitch, DMA2D_FMT_RGB888, 8, 0 )); dma2dOutSetWrapOffset(dma2dp, ltdc_screen_frmcfg1.width - 200); dma2dJobSetMode(dma2dp, DMA2D_JOB_CONVERT); dma2dJobSetSize(dma2dp, 200, 320); dma2dJobExecute(dma2dp); dma2dReleaseBus(dma2dp); }
/* * Application entry point. */ int main(void) { /** * Hardware initialization, in this simple demo just the systick timer is * initialized. */ STBase->RVR = SYSTEM_CLOCK / CH_FREQUENCY - 1; STBase->CVR = 0; STBase->CSR = CLKSOURCE_CORE_BITS | ENABLE_ON_BITS | TICKINT_ENABLED_BITS; /* * System initializations. * - Kernel initialization, the main() function becomes a thread and the * RTOS is active. */ chSysInit(); /* * Creates the example thread. */ chThdCreateStatic(waThread1, sizeof(waThread1), NORMALPRIO, Thread1, NULL); /* * Normal main() thread activity, in this demo it does nothing except * increasing the minutes counter. */ while (TRUE) { chThdSleepSeconds(60); minutes_counter++; } }
static void thd4_execute(void) { systime_t time; test_wait_tick(); /* Timeouts in microseconds.*/ time = chTimeNow(); chThdSleepMicroseconds(100000); test_assert_time_window(1, time + US2ST(100000), time + US2ST(100000) + 1); /* Timeouts in milliseconds.*/ time = chTimeNow(); chThdSleepMilliseconds(100); test_assert_time_window(2, time + MS2ST(100), time + MS2ST(100) + 1); /* Timeouts in seconds.*/ time = chTimeNow(); chThdSleepSeconds(1); test_assert_time_window(3, time + S2ST(1), time + S2ST(1) + 1); /* Absolute timelines.*/ time = chTimeNow() + MS2ST(100); chThdSleepUntil(time); test_assert_time_window(4, time, time + 1); }
void initBoardTest(void) { is_board_test_mode = true; addConsoleAction("n", nextStep); addConsoleActionI("set", setIndex); chThdCreateStatic(btThreadStack, sizeof(btThreadStack), NORMALPRIO, (tfunc_t) ivThread, NULL); // this code is ugly as hell, I had no time to think. Todo: refactor processAdcPin(&fastAdc, 0, "fast"); while (currentIndex < slowAdc.size()) { processAdcPin(&slowAdc, currentIndex, "slow"); currentIndex++; } currentIndex = 0; int pinsCount = sizeof(BLINK_PINS) / sizeof(brain_pin_e); while (currentIndex < pinsCount) { currentPin = BLINK_PINS[currentIndex]; printBoardTestState(); mySetPadMode2("test", currentPin, PAL_STM32_MODE_OUTPUT); currentIndex++; waitForKey(); } // no buffered logger still, just plain old stdout while (1) { print("Board test done, thank you! Time to remove that jumper and reboot\r\n"); print("Bye!\r\n"); chThdSleepSeconds(1); } }
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(); } }
/* * Application entry point. */ int main(void) { /* * Hardware initialization, in this simple demo just the systick timer is * initialized. */ SysTick->LOAD = SYSTEM_CLOCK / CH_CFG_ST_FREQUENCY - (systime_t)1; SysTick->VAL = (uint32_t)0; SysTick->CTRL = SysTick_CTRL_CLKSOURCE_Msk | SysTick_CTRL_ENABLE_Msk | SysTick_CTRL_TICKINT_Msk; /* IRQ enabled.*/ NVIC_SetPriority(SysTick_IRQn, 8); /* * System initializations. * - Kernel initialization, the main() function becomes a thread and the * RTOS is active. */ chSysInit(); /* * Creates the example thread. */ (void) chThdCreateStatic(waThread1, sizeof(waThread1), NORMALPRIO, Thread1, NULL); /* * Normal main() thread activity, in this demo it does nothing except * increasing the minutes counter. */ while (true) { chThdSleepSeconds(60); minutes_counter++; } }
static THD_FUNCTION(Thread2, arg) { (void)arg; while (true) { chThdSleepSeconds(60); minutes_counter++; } }
static void test_001_002_execute(void) { systime_t time; /* The current system time is read then a sleep is performed for 100 system ticks and on exit the system time is verified again.*/ test_set_step(1); { time = chVTGetSystemTimeX(); chThdSleep(100); test_assert_time_window(time + 100, time + 100 + 1, "out of time window"); } /* The current system time is read then a sleep is performed for 100000 microseconds and on exit the system time is verified again.*/ test_set_step(2); { time = chVTGetSystemTimeX(); chThdSleepMicroseconds(100); test_assert_time_window(time + US2ST(100), time + US2ST(100) + 1, "out of time window"); } /* The current system time is read then a sleep is performed for 100 milliseconds and on exit the system time is verified again.*/ test_set_step(3); { time = chVTGetSystemTimeX(); chThdSleepMilliseconds(100); test_assert_time_window(time + MS2ST(100), time + MS2ST(100) + 1, "out of time window"); } /* The current system time is read then a sleep is performed for 1 second and on exit the system time is verified again.*/ test_set_step(4); { time = chVTGetSystemTimeX(); chThdSleepSeconds(1); test_assert_time_window(time + S2ST(1), time + S2ST(1) + 1, "out of time window"); } test_set_step(5); { time = chVTGetSystemTimeX(); chThdSleepUntil(time + 100); test_assert_time_window(time + 100, time + 100 + 1, "out of time window"); } }
static THD_FUNCTION(waThread, arg) { (void)arg; chRegSetThreadName("Wave Analyzer"); #if EFI_ENGINE_SNIFFER while (true) { chThdSleepSeconds(CHART_RESET_DELAY); waveChart.publishIfFull(); } #endif /* EFI_ENGINE_SNIFFER */ }
static msg_t waThread(void *arg) { (void)arg; chRegSetThreadName("Wave Analyzer"); #if EFI_WAVE_CHART while (TRUE) { chThdSleepSeconds(CHART_RESET_DELAY); waveChart.publishChartIfFull(); } #endif /* EFI_WAVE_CHART */ #if defined __GNUC__ return -1; #endif }
static void nap_version_check(void) { /* Check we are running a compatible version of the NAP firmware. */ char nap_version_string[64] = {0}; nap_conf_rd_version_string(nap_version_string); if (compare_version(nap_version_string, REQUIRED_NAP_VERSION_STR) < 0) { while (1) { log_error("NAP firmware version >= %s required, please update!" "(instructions can be found at http://docs.swift-nav.com/)", REQUIRED_NAP_VERSION_STR); chThdSleepSeconds(2); } } }
static msg_t Power( void *arg ) { (void)arg; //while ( 1 ) // chThdSleepSeconds( 1 ); chRegSetThreadName( "pwr" ); // First wait for power on. g_nextOnDelay = g_firstOnDelay; while ( 1 ) { setPower( 0 ); // Wait for next power on. g_timer = g_nextOnDelay; while ( g_timer-- > 0 ) chThdSleepSeconds( 1 ); // Power on. g_nextOnDelay = g_offDelay; setPower( 1 ); chMtxLock( &g_mutex ); g_timer = g_offDelay; static int t; t = g_timer; chMtxUnlock(); while ( t > 0 ) { chThdSleepSeconds( 1 ); chMtxLock( &g_mutex ); g_timer--; t = g_timer; chMtxUnlock(); } } return 0; }
static msg_t mfiThread(void) #endif { chRegSetThreadName("MFIndicator"); error_codes_set_s localErrorCopy; while (true) { chThdSleepSeconds(10); getErrorCodes(&localErrorCopy); for (int p = 0; p < localErrorCopy.count; p++) { // Calculate how many digits in this integer and display error code from start to end int code = localErrorCopy.error_codes[p]; DisplayErrorCode(DigitLength(code), code); } } }
static msg_t PeriodicThread(void *arg) { (void)arg; while(1) { float f1, f2, f3, f4, f5; f1 = ff1(4.0f); f2 = ff1(5.0f); f3 = ff1(6.0f); f5 = f1 + f2 + f3; f4 = ff1(7.0f); f5 = ff2(f5, f4, f5, f4); if (f5 != 484.0f) chSysHalt(); chThdSleepSeconds(1); } }
static THD_FUNCTION(PeriodicThread, arg) { (void)arg; while(1) { float f1, f2, f3, f4, f5; f1 = ff1(4.0f); f2 = ff1(5.0f); f3 = ff1(6.0f); f5 = f1 + f2 + f3; f4 = ff1(7.0f); f5 = ff2(f5, f4, f5, f4); if (f5 != 484.0f) chSysHalt("float corrupion #2"); chThdSleepSeconds(1); } }
static void bmk8_execute(void) { uint32_t n; n = 0; test_wait_tick(); threads[0] = chThdCreateStatic(wa[0], WA_SIZE, chThdGetPriority()-1, thread8, (void *)&n); threads[1] = chThdCreateStatic(wa[1], WA_SIZE, chThdGetPriority()-1, thread8, (void *)&n); threads[2] = chThdCreateStatic(wa[2], WA_SIZE, chThdGetPriority()-1, thread8, (void *)&n); threads[3] = chThdCreateStatic(wa[3], WA_SIZE, chThdGetPriority()-1, thread8, (void *)&n); threads[4] = chThdCreateStatic(wa[4], WA_SIZE, chThdGetPriority()-1, thread8, (void *)&n); chThdSleepSeconds(1); test_terminate_threads(); test_wait_threads(); test_print("--- Score : "); test_printn(n); test_println(" ctxswc/S"); }
int main(void) { halInit(); chSysInit(); chThdCreateStatic(blinkWA, sizeof(blinkWA), NORMALPRIO, blink_thd, NULL); /* set alarm in near future */ rtcGetTime(&RTCD1, ×pec); alarmspec.tv_sec = timespec.tv_sec + 30; rtcSetAlarm(&RTCD1, 0, &alarmspec); while (TRUE){ chThdSleepSeconds(10); chSysLock(); /* going to anabiosis*/ PWR->CR |= (PWR_CR_PDDS | PWR_CR_LPDS | PWR_CR_CSBF | PWR_CR_CWUF); SCB->SCR |= SCB_SCR_SLEEPDEEP_Msk; __WFI(); } 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. */ halInit(); chSysInit(); oca_led_init(); oca_usb_init(); canStart(&CAND1, &cancfg); //set_led(led_act, led_blink_250); oca_led_set(oca_led_stat, oca_led_blink_500); //set_led(led_err, led_blink_1000); /* * Creates the blinker thread. */ chThdCreateStatic(can_rx_wa, sizeof(can_rx_wa), NORMALPRIO + 5, can_rx, NULL); chThdCreateStatic(waThread1, sizeof(waThread1), NORMALPRIO, Thread1, NULL); chThdCreateStatic(can_tx_wa, sizeof(can_tx_wa), NORMALPRIO, can_tx, NULL); /* * Normal main() thread activity, in this demo it does nothing except * sleeping in a loop and check the button state. */ while (true) { chThdSleepSeconds(1); } }
int main(void) { halInit(); chSysInit(); sdStart(&SD1, &serialConfig); pilotSetup(); powerSetup(); motorsSetup(); imuSetup(); pidInit(&pitchPID, 1.0 / 20000.0, 1.0 / 10000, 0); pidInit(&rollPID, 1.0 / 20000.0, 1.0 / 10000, 0); pidInit(&yawPID, 1.0 / 20000.0, 1.0 / 10000, 0); while (TRUE) { printf("yaw rate %f\r\n", imuGetYawRate()); printf("pitch %f\r\n", imuGetPitch()); printf("roll %f\r\n", imuGetRoll()); // double pitch = pilotGetPitch() - pidUpdate(&pitchPID, 0.0, gyroGetPitchRotation()); // double roll = pilotGetRoll() - pidUpdate(&rollPID, 0, gyroGetRollRotation()); // double yaw = pilotGetYaw() - pidUpdate(&yawPID, 0, gyroGetYawRotation()); // motorsSetControl(CLIP(pitch), CLIP(roll), CLIP(yaw), pilotGetThrottle()); // uint16_t throttle = receiverGetRaw(THROTTLE_CH); // uint16_t pitch = receiverGetRaw(PITCH_CH); // uint16_t roll = receiverGetRaw(ROLL_CH); // uint16_t yaw = receiverGetRaw(YAW_CH); // printf("%u, %u, %u, %u\r\n", throttle, pitch, roll, yaw); // printf("%u\r\n", receiverGetRaw(4)); // printf("%f, %f, %f\r\n", receiverGetDouble(PITCH_CH), receiverGetDouble(ROLL_CH), receiverGetDouble(YAW_CH)); // printf("%d, %d, %d\r\n", gyroGetPitchRotation(), gyroGetRollRotation(), gyroGetYawRotation()); // printf("%f, %f, %f\r\n", pitch, roll, yaw); // printf("%f, %f, %f\r\n", pitchPID.integral, rollPID.integral, yawPID.integral); chThdSleepSeconds(1); } }
static msg_t screen_saver_thread(void* arg) { (void)arg; chRegSetThreadName("screen_saver"); while (1) { screen_saver_t* s = arg; quantity_t screen_saver_timeout = app_cfg_get_screen_saver(); if (!s->active && screen_saver_timeout.value > .5f && (chTimeNow() - s->last_touch_time) > S2ST(60 * screen_saver_timeout.value)) { s->active = true; active = true; lcd_set_brightness(0); gui_push_screen(s->screen); } chThdSleepSeconds(10); } return 0; }
/** * @brief Sleeps for some seconds. * @details Puts the thread in sleep state for the provided amount of time. * * @param[in] sec * Number of seconds to sleep. */ void uros_lld_thread_sleepsec(uint32_t sec) { chThdSleepSeconds((systime_t)sec); }
int main(void) { /* Initialise SysTick timer that will be used as the ChibiOS kernel tick * timer. */ STBase->RVR = SYSTEM_CLOCK / CH_FREQUENCY - 1; STBase->CVR = 0; STBase->CSR = CLKSOURCE_CORE_BITS | ENABLE_ON_BITS | TICKINT_ENABLED_BITS; /* Kernel initialization, the main() function becomes a thread and the RTOS * is active. */ chSysInit(); /* Piksi hardware initialization. */ init(); settings_setup(); usarts_setup(); check_nap_auth(); static char nap_version_string[64] = {0}; nap_conf_rd_version_string(nap_version_string); log_info("NAP firmware version: %s\n", nap_version_string); /* Check we are running a compatible version of the NAP firmware. */ const char *required_nap_version = "v0.9-46"; if (compare_version(nap_version_string, required_nap_version) < 0) { log_error("NAP firmware version newer than %s required, please update!\n" "(instructions can be found at http://docs.swift-nav.com/)\n", required_nap_version); while (1) { chThdSleepSeconds(60); } } static s32 serial_number; serial_number = nap_conf_rd_serial_number(); max2769_setup(); timing_setup(); position_setup(); manage_acq_setup(); manage_track_setup(); system_monitor_setup(); base_obs_setup(); solution_setup(); simulator_setup(); sbp_fileio_setup(); ext_setup(); if (serial_number < 0) { READ_ONLY_PARAMETER("system_info", "serial_number", "(unknown)", TYPE_STRING); } else { READ_ONLY_PARAMETER("system_info", "serial_number", serial_number, TYPE_INT); } READ_ONLY_PARAMETER("system_info", "firmware_version", GIT_VERSION, TYPE_STRING); READ_ONLY_PARAMETER("system_info", "firmware_built", __DATE__ " " __TIME__, TYPE_STRING); static struct setting hw_rev = { "system_info", "hw_revision", NULL, 0, settings_read_only_notify, NULL, NULL, false }; hw_rev.addr = (char *)nap_conf_rd_hw_rev_string(); hw_rev.len = strlen(hw_rev.addr); settings_register(&hw_rev, TYPE_STRING); READ_ONLY_PARAMETER("system_info", "nap_version", nap_version_string, TYPE_STRING); READ_ONLY_PARAMETER("system_info", "nap_channels", nap_track_n_channels, TYPE_INT); READ_ONLY_PARAMETER("system_info", "nap_fft_index_bits", nap_acq_fft_index_bits, TYPE_INT); chThdCreateStatic(wa_nav_msg_thread, sizeof(wa_nav_msg_thread), NORMALPRIO-1, nav_msg_thread, NULL); /* Send message to inform host we are up and running. */ u32 startup_flags = 0; sbp_send_msg(SBP_MSG_STARTUP, sizeof(startup_flags), (u8 *)&startup_flags); while (1) { chThdSleepSeconds(60); } }
static void test_002_001_execute(void) { systime_t time; /* [2.1.1] The current system time is read then a sleep is performed for 100 system ticks and on exit the system time is verified again.*/ test_set_step(1); { time = chVTGetSystemTimeX(); chThdSleep(100); test_assert_time_window(time + 100, time + 100 + CH_CFG_ST_TIMEDELTA + 1, "out of time window"); } /* [2.1.2] The current system time is read then a sleep is performed for 100000 microseconds and on exit the system time is verified again.*/ test_set_step(2); { time = chVTGetSystemTimeX(); chThdSleepMicroseconds(100000); test_assert_time_window(time + US2ST(100000), time + US2ST(100000) + CH_CFG_ST_TIMEDELTA + 1, "out of time window"); } /* [2.1.3] The current system time is read then a sleep is performed for 100 milliseconds and on exit the system time is verified again.*/ test_set_step(3); { time = chVTGetSystemTimeX(); chThdSleepMilliseconds(100); test_assert_time_window(time + MS2ST(100), time + MS2ST(100) + CH_CFG_ST_TIMEDELTA + 1, "out of time window"); } /* [2.1.4] The current system time is read then a sleep is performed for 1 second and on exit the system time is verified again.*/ test_set_step(4); { time = chVTGetSystemTimeX(); chThdSleepSeconds(1); test_assert_time_window(time + S2ST(1), time + S2ST(1) + CH_CFG_ST_TIMEDELTA + 1, "out of time window"); } /* [2.1.5] Function chThdSleepUntil() is tested with a timeline of "now" + 100 ticks.*/ test_set_step(5); { time = chVTGetSystemTimeX(); chThdSleepUntil(time + 100); test_assert_time_window(time + 100, time + 100 + CH_CFG_ST_TIMEDELTA + 1, "out of time window"); } }
void inf_loop(void){ while(1){ chThdSleepSeconds(1); } }
/* * 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. */ }
int main(void) { halInit(); /* Kernel initialization, the main() function becomes a thread with * priority NORMALPRIO and the RTOS is active. */ chSysInit(); /* Piksi hardware initialization. */ init(); settings_setup(); signal_init(); check_nap_auth(); static char nap_version_string[64] = {0}; nap_conf_rd_version_string(nap_version_string); log_info("NAP firmware version: %s", nap_version_string); /* Check we are running a compatible version of the NAP firmware. */ const char *required_nap_version = "v0.16"; if (compare_version(nap_version_string, required_nap_version) < 0) { while (1) { log_error("NAP firmware version >= %s required, please update!" "(instructions can be found at http://docs.swift-nav.com/)", required_nap_version); chThdSleepSeconds(2); } } static s32 serial_number; serial_number = nap_conf_rd_serial_number(); frontend_setup(); timing_setup(); ext_event_setup(); position_setup(); track_setup(); track_gps_l1ca_register(); decode_setup(); decode_gps_l1_register(); manage_acq_setup(); manage_track_setup(); system_monitor_setup(); base_obs_setup(); solution_setup(); simulator_setup(); sbp_fileio_setup(); ext_setup(); pps_setup(); READ_ONLY_PARAMETER("system_info", "serial_number", serial_number, TYPE_INT); READ_ONLY_PARAMETER("system_info", "firmware_version", GIT_VERSION, TYPE_STRING); READ_ONLY_PARAMETER("system_info", "firmware_built", __DATE__ " " __TIME__, TYPE_STRING); static struct setting hw_rev = { "system_info", "hw_revision", NULL, 0, settings_read_only_notify, NULL, NULL, false }; hw_rev.addr = (char *)nap_conf_rd_hw_rev_string(); hw_rev.len = strlen(hw_rev.addr); settings_register(&hw_rev, TYPE_STRING); READ_ONLY_PARAMETER("system_info", "nap_version", nap_version_string, TYPE_STRING); READ_ONLY_PARAMETER("system_info", "nap_channels", nap_track_n_channels, TYPE_INT); READ_ONLY_PARAMETER("system_info", "nap_fft_index_bits", nap_acq_fft_index_bits, TYPE_INT); ephemeris_setup(); /* Send message to inform host we are up and running. */ u32 startup_flags = 0; sbp_send_msg(SBP_MSG_STARTUP, sizeof(startup_flags), (u8 *)&startup_flags); while (1) { chThdSleepSeconds(60); } }