/** * @brief Main module task */ static void actuatorTask(void* parameters) { // UAVObjEvent ev; portTickType lastSysTime; ActuatorCommandData command; ActuatorSettingsData settings; // Set servo update frequency (done only on start-up) ActuatorSettingsGet(&settings); PIOS_Servo_SetHz(settings.ChannelUpdateFreq[0], settings.ChannelUpdateFreq[1]); // Go to the neutral (failsafe) values until an ActuatorDesired update is received setFailsafe(); // Main task loop lastSysTime = xTaskGetTickCount(); while (1) { ActuatorCommandGet(&command); ActuatorSettingsGet(&settings); if ( RunMixers(&command, &settings) == -1 ) { AlarmsSet(SYSTEMALARMS_ALARM_ACTUATOR, SYSTEMALARMS_ALARM_CRITICAL); } else { AlarmsClear(SYSTEMALARMS_ALARM_ACTUATOR); } // Update output object ActuatorCommandSet(&command); // Update in case read only (eg. during servo configuration) ActuatorCommandGet(&command); // Update servo outputs for (int n = 0; n < ACTUATORCOMMAND_CHANNEL_NUMELEM; ++n) { PIOS_Servo_Set( n, command.Channel[n] ); } // Wait until next update vTaskDelayUntil(&lastSysTime, settings.UpdatePeriod / portTICK_RATE_MS ); } }
void PIOS_Video_Init(const struct pios_video_cfg *cfg) { dev_cfg = cfg; // store config before enabling interrupt configure_hsync_timers(); /* needed for HW hack */ const GPIO_InitTypeDef initStruct = { .GPIO_Pin = GPIO_Pin_12, .GPIO_Speed = GPIO_Speed_100MHz, .GPIO_Mode = GPIO_Mode_IN, .GPIO_OType = GPIO_OType_PP, .GPIO_PuPd = GPIO_PuPd_NOPULL }; GPIO_Init(GPIOC, &initStruct); /* SPI3 - MASKBUFFER */ GPIO_Init(cfg->mask.sclk.gpio, (GPIO_InitTypeDef *)&(cfg->mask.sclk.init)); GPIO_Init(cfg->mask.miso.gpio, (GPIO_InitTypeDef *)&(cfg->mask.miso.init)); /* SPI1 SLAVE FRAMEBUFFER */ GPIO_Init(cfg->level.sclk.gpio, (GPIO_InitTypeDef *)&(cfg->level.sclk.init)); GPIO_Init(cfg->level.miso.gpio, (GPIO_InitTypeDef *)&(cfg->level.miso.init)); if (cfg->mask.remap) { GPIO_PinAFConfig(cfg->mask.sclk.gpio, __builtin_ctz(cfg->mask.sclk.init.GPIO_Pin), cfg->mask.remap); GPIO_PinAFConfig(cfg->mask.miso.gpio, __builtin_ctz(cfg->mask.miso.init.GPIO_Pin), cfg->mask.remap); } if (cfg->level.remap) { GPIO_PinAFConfig(cfg->level.sclk.gpio, __builtin_ctz(cfg->level.sclk.init.GPIO_Pin), cfg->level.remap); GPIO_PinAFConfig(cfg->level.miso.gpio, __builtin_ctz(cfg->level.miso.init.GPIO_Pin), cfg->level.remap); } /* Initialize the SPI block */ SPI_Init(cfg->level.regs, (SPI_InitTypeDef *)&(cfg->level.init)); SPI_Init(cfg->mask.regs, (SPI_InitTypeDef *)&(cfg->mask.init)); /* Enable SPI */ SPI_Cmd(cfg->level.regs, ENABLE); SPI_Cmd(cfg->mask.regs, ENABLE); /* Configure DMA for SPI Tx SLAVE Maskbuffer */ DMA_Cmd(cfg->mask.dma.tx.channel, DISABLE); DMA_Init(cfg->mask.dma.tx.channel, (DMA_InitTypeDef *)&(cfg->mask.dma.tx.init)); /* Configure DMA for SPI Tx SLAVE Framebuffer*/ DMA_Cmd(cfg->level.dma.tx.channel, DISABLE); DMA_Init(cfg->level.dma.tx.channel, (DMA_InitTypeDef *)&(cfg->level.dma.tx.init)); /* Trigger interrupt when for half conversions too to indicate double buffer */ DMA_ITConfig(cfg->level.dma.tx.channel, DMA_IT_TC, ENABLE); /* Configure and clear buffers */ draw_buffer_level = buffer0_level; draw_buffer_mask = buffer0_mask; disp_buffer_level = buffer1_level; disp_buffer_mask = buffer1_mask; memset(disp_buffer_mask, 0, GRAPHICS_WIDTH * GRAPHICS_HEIGHT); memset(disp_buffer_level, 0, GRAPHICS_WIDTH * GRAPHICS_HEIGHT); memset(draw_buffer_mask, 0, GRAPHICS_WIDTH * GRAPHICS_HEIGHT); memset(draw_buffer_level, 0, GRAPHICS_WIDTH * GRAPHICS_HEIGHT); /* Configure DMA interrupt */ NVIC_Init(&cfg->level.dma.irq.init); /* Enable SPI interrupts to DMA */ SPI_I2S_DMACmd(cfg->mask.regs, SPI_I2S_DMAReq_Tx, ENABLE); SPI_I2S_DMACmd(cfg->level.regs, SPI_I2S_DMAReq_Tx, ENABLE); mask_dma = DMA1; main_dma = DMA2; main_stream = cfg->level.dma.tx.channel; mask_stream = cfg->mask.dma.tx.channel; /* Configure the Video Line interrupt */ PIOS_EXTI_Init(cfg->hsync); PIOS_EXTI_Init(cfg->vsync); // set levels to zero PIOS_Servo_Set(0, 0); PIOS_Servo_Set(1, 0); }
static void TaskServos(void *pvParameters) { /* For testing servo outputs */ portTickType xDelay; /* Used to test servos, cycles all servos from one side to the other */ for(;;) { /*xDelay = 250 / portTICK_RATE_MS; PIOS_Servo_Set(0, 2000); vTaskDelay(xDelay); PIOS_Servo_Set(1, 2000); vTaskDelay(xDelay); PIOS_Servo_Set(2, 2000); vTaskDelay(xDelay); PIOS_Servo_Set(3, 2000); vTaskDelay(xDelay); PIOS_Servo_Set(4, 2000); vTaskDelay(xDelay); PIOS_Servo_Set(5, 2000); vTaskDelay(xDelay); PIOS_Servo_Set(6, 2000); vTaskDelay(xDelay); PIOS_Servo_Set(7, 2000); vTaskDelay(xDelay); PIOS_Servo_Set(7, 1000); vTaskDelay(xDelay); PIOS_Servo_Set(6, 1000); vTaskDelay(xDelay); PIOS_Servo_Set(5, 1000); vTaskDelay(xDelay); PIOS_Servo_Set(4, 1000); vTaskDelay(xDelay); PIOS_Servo_Set(3, 1000); vTaskDelay(xDelay); PIOS_Servo_Set(2, 1000); vTaskDelay(xDelay); PIOS_Servo_Set(1, 1000); vTaskDelay(xDelay); PIOS_Servo_Set(0, 1000); vTaskDelay(xDelay);*/ xDelay = 1 / portTICK_RATE_MS; for(int i = 1000; i < 2000; i++) { PIOS_Servo_Set(0, i); PIOS_Servo_Set(1, i); PIOS_Servo_Set(2, i); PIOS_Servo_Set(3, i); PIOS_Servo_Set(4, i); PIOS_Servo_Set(5, i); PIOS_Servo_Set(6, i); PIOS_Servo_Set(7, i); vTaskDelay(xDelay); } for(int i = 2000; i > 1000; i--) { PIOS_Servo_Set(0, i); PIOS_Servo_Set(1, i); PIOS_Servo_Set(2, i); PIOS_Servo_Set(3, i); PIOS_Servo_Set(4, i); PIOS_Servo_Set(5, i); PIOS_Servo_Set(6, i); PIOS_Servo_Set(7, i); vTaskDelay(xDelay); } } }