void AP_IOMCU_FW::init()
{
thread_ctx = chThdGetSelfX();
if (palReadLine(HAL_GPIO_PIN_IO_HW_DETECT1) == 1 && palReadLine(HAL_GPIO_PIN_IO_HW_DETECT2) == 0) {
has_heater = true;
}
adc_init();
sbus_out_init();
}
/*
* Application entry point.
*/
int main(void) {
uint32 blinker_id;
/* HAL initialization, this also initializes the configured device drivers
and performs the board-specific initializations.*/
halInit();
/* OS initialization.*/
(void) OS_API_Init();
/* Activates the serial driver 1 using the driver default configuration.*/
sdStart(&SD1, NULL);
/* ARD_D13 is programmed as output (board LED).*/
palClearLine(LINE_ARD_D13);
palSetLineMode(LINE_ARD_D13, PAL_MODE_OUTPUT_PUSHPULL);
/* Starting the blinker thread.*/
(void) OS_TaskCreate(&blinker_id, "blinker", blinker,
(uint32 *)wa_blinker, sizeof wa_blinker,
128, 0);
/* In the ChibiOS/RT OSAL implementation the main() function is an
usable thread with priority 128 (NORMALPRIO), here we just sleep
waiting for a button event, then the test suite is executed.*/
while (true) {
if (palReadLine(LINE_BUTTON_USER))
test_execute((BaseSequentialStream *)&SD1, &nasa_osal_test_suite);
OS_TaskDelay(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();
/*
* Activates the serial driver 1 using the driver default configuration.
*/
sdStart(&SD1, NULL);
/*
* 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 (!palReadLine(LINE_ARD_D3)) {
test_execute((BaseSequentialStream *)&SD1, &rt_test_suite);
// test_execute((BaseSequentialStream *)&SD1, &oslib_test_suite);
}
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();
/*
* Activates the serial driver 2 using the driver default configuration.
*/
sdStart(&SD2, NULL);
/*
* 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 (palReadLine(LINE_JOY_CENTER))
TestThread(&SD2);
chThdSleepMilliseconds(500);
}
}
/*
update safety state
*/
void AP_IOMCU_FW::safety_update(void)
{
uint32_t now = AP_HAL::millis();
if (now - safety_update_ms < 100) {
// update safety at 10Hz
return;
}
safety_update_ms = now;
bool safety_pressed = palReadLine(HAL_GPIO_PIN_SAFETY_INPUT);
if (safety_pressed) {
if (reg_status.flag_safety_off && (reg_setup.arming & P_SETUP_ARMING_SAFETY_DISABLE_ON)) {
safety_pressed = false;
} else if ((!reg_status.flag_safety_off) && (reg_setup.arming & P_SETUP_ARMING_SAFETY_DISABLE_OFF)) {
safety_pressed = false;
}
}
if (safety_pressed) {
safety_button_counter++;
} else {
safety_button_counter = 0;
}
if (safety_button_counter == 10) {
// safety has been pressed for 1 second, change state
reg_status.flag_safety_off = !reg_status.flag_safety_off;
}
led_counter = (led_counter+1) % 16;
const uint16_t led_pattern = reg_status.flag_safety_off?0xFFFF:0x5500;
palWriteLine(HAL_GPIO_PIN_SAFETY_LED, (led_pattern & (1U << led_counter))?0:1);
}
static THD_FUNCTION(Thread1, arg) {
(void)arg;
chRegSetThreadName("blinker");
while (true) {
systime_t time = palReadLine(PORTAB_LINE_BUTTON) == PORTAB_BUTTON_PRESSED ? 250 : 500;
palToggleLine(PORTAB_LINE_LED2);
chThdSleepMilliseconds(time);
}
}
static THD_FUNCTION(ButtonThread, arg) {
(void)arg;
chRegSetThreadName("buttonThread");
uint8_t newstate, state = PAL_HIGH;
while(true) {
if(palReadLine(LINE_BUTTON) != state) {
chThdSleepMilliseconds(20); /* debounce */
newstate = palReadLine(LINE_BUTTON);
if(newstate != state) {
state = newstate;
if(newstate == PAL_LOW) {
table_pos = (table_pos + 120)%TABLE_SIZE;
}
}
}
chThdSleepMilliseconds(20);
}
}
/*
* 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();
/*
* Route USART3 to PC4/TX/pin1 PC5/RX/pin0
*/
palSetLineMode(LINE_PIN0, PAL_MODE_ALTERNATE(7));
palSetLineMode(LINE_PIN1, PAL_MODE_ALTERNATE(7));
/*
* Activates the serial driver 2 using the driver default configuration.
*/
sdStart(&SD3, NULL);
// palSetLineMode(LINE_LED_GREEN, PAL_MODE_OUTPUT_PUSHPULL);
palClearLine(LINE_LED_GREEN);
chThdSleepMilliseconds(500);
palSetLine(LINE_LED_GREEN);
// palSetPadMode(GPIOC, 10, PAL_MODE_OUTPUT_PUSHPULL);
// palTogglePad(GPIOC, 10);
/*
* 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 (palReadLine(LINE_BUTTON)) {
palToggleLine(LINE_LED_BLUE);
// test_execute((BaseSequentialStream *)&SD3);
}
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();
/*
* ARD_D13 is programmed as output (board LED).
*/
palClearLine(LINE_ARD_D13);
palSetLineMode(LINE_ARD_D13, PAL_MODE_OUTPUT_PUSHPULL);
/*
* Activates the serial driver 1 using the driver default configuration.
*/
sdStart(&SD1, NULL);
/*
* Creates the example 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 (palReadLine(LINE_BUTTON_USER))
test_execute((BaseSequentialStream *)&SD1);
chThdSleepMilliseconds(500);
}
}
static void nap_exti_thread(void *arg)
{
(void)arg;
chRegSetThreadName("NAP ISR");
while (TRUE) {
/* Waiting for the IRQ to happen.*/
chBSemWaitTimeout(&nap_exti_sem, MS2ST(PROCESS_PERIOD_ms));
/* We need a level (not edge) sensitive interrupt -
* if there is another interrupt pending on the Swift
* NAP then the IRQ line will stay high. Therefore if
* the line is still high, don't suspend the thread.
*/
spi_lock(SPI_SLAVE_FPGA);
while (palReadLine(LINE_NAP_IRQ)) {
handle_nap_exti();
}
tracking_channels_process();
spi_unlock(SPI_SLAVE_FPGA);
}
}
/*
* 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);
}
