/*
* 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);
}
}
