/** * OpenPilot Main function */ int main() { // Init PIOS_SYS_Init(); PIOS_DEBUG_Init(); PIOS_DELAY_Init(); PIOS_COM_Init(); PIOS_USB_Init(0); PIOS_I2C_Init(); // Both leds off PIOS_LED_Off(LED1); PIOS_LED_Off(LED2); // Create task xTaskCreate(Task1, (signed portCHAR *)"Task1", 1024 , NULL, 1, NULL); xTaskCreate(Task2, (signed portCHAR *)"Task2", 1024 , NULL, 2, NULL); // Start the FreeRTOS scheduler vTaskStartScheduler(); /* If all is well we will never reach here as the scheduler will now be running. */ /* If we do get here, it will most likely be because we ran out of heap space. */ return 0; }
/** * OpenPilot Main function: * * Initialize PiOS<BR> * Create the "System" task (SystemModInitializein Modules/System/systemmod.c) <BR> * Start FreeRTOS Scheduler (vTaskStartScheduler) (Now handled by caller) * If something goes wrong, blink LED1 and LED2 every 100ms * */ int main() { /* NOTE: Do NOT modify the following start-up sequence */ /* Any new initialization functions should be added in OpenPilotInit() */ /* Brings up System using CMSIS functions, enables the LEDs. */ PIOS_SYS_Init(); /* Architecture dependant Hardware and * core subsystem initialisation * (see pios_board.c for your arch) * */ PIOS_Board_Init(); /* Initialize modules */ MODULE_INITIALISE_ALL(PIOS_WDG_Clear); /* swap the stack to use the IRQ stack */ Stack_Change(); /* Start the FreeRTOS scheduler which should never returns.*/ vTaskStartScheduler(); /* If all is well we will never reach here as the scheduler will now be running. */ /* Do some indication to user that something bad just happened */ while (1) { #if defined(PIOS_LED_HEARTBEAT) PIOS_LED_Toggle(PIOS_LED_HEARTBEAT); #endif /* PIOS_LED_HEARTBEAT */ PIOS_DELAY_WaitmS(100); } return 0; }
/** * Tau Labs Main function: * * Initialize PiOS<BR> * Create the "System" task (SystemModInitializein Modules/System/systemmod.c) <BR> * Start FreeRTOS Scheduler (vTaskStartScheduler)<BR> * If something goes wrong, blink LED1 and LED2 every 100ms * */ int main() { int result; /* NOTE: Do NOT modify the following start-up sequence */ /* Any new initialization functions should be added in OpenPilotInit() */ vPortInitialiseBlocks(); /* Brings up System using CMSIS functions, enables the LEDs. */ PIOS_SYS_Init(); /* For Revolution we use a FreeRTOS task to bring up the system so we can */ /* always rely on FreeRTOS primitive */ result = xTaskCreate(initTask, (const signed char *)"init", INIT_TASK_STACK, NULL, INIT_TASK_PRIORITY, &initTaskHandle); PIOS_Assert(result == pdPASS); /* Start the FreeRTOS scheduler */ vTaskStartScheduler(); /* If all is well we will never reach here as the scheduler will now be running. */ /* Do some PIOS_LED_HEARTBEAT to user that something bad just happened */ PIOS_LED_Off(PIOS_LED_HEARTBEAT); \ for(;;) { \ PIOS_LED_Toggle(PIOS_LED_HEARTBEAT); \ PIOS_DELAY_WaitmS(100); \ }; return 0; }
/** * PIOS_Board_Init() * initializes all the core subsystems on this specific hardware * called from System/openpilot.c */ void PIOS_Board_Init(void) { /* Brings up System using CMSIS functions, enables the LEDs. */ PIOS_SYS_Init(); /* Delay system */ PIOS_DELAY_Init(); /* IAP System Setup */ PIOS_IAP_Init(); #if defined(PIOS_INCLUDE_COM) #if defined(PIOS_INCLUDE_GPS) uint32_t pios_usart_gps_id; if (PIOS_USART_Init(&pios_usart_gps_id, &pios_usart_gps_cfg)) { PIOS_DEBUG_Assert(0); } if (PIOS_COM_Init(&pios_com_gps_id, &pios_usart_com_driver, pios_usart_gps_id)) { PIOS_DEBUG_Assert(0); } #endif /* PIOS_INCLUDE_GPS */ #endif /* PIOS_INCLUDE_COM */ #if defined (PIOS_INCLUDE_I2C) if (PIOS_I2C_Init(&pios_i2c_pres_mag_adapter_id, &pios_i2c_pres_mag_adapter_cfg)) { PIOS_DEBUG_Assert(0); } #if defined (PIOS_INCLUDE_BMP085) PIOS_BMP085_Init(); #endif /* PIOS_INCLUDE_BMP085 */ #if defined (PIOS_INCLUDE_HMC5883) PIOS_HMC5883_Init(); #endif /* PIOS_INCLUDE_HMC5883 */ #if defined(PIOS_INCLUDE_IMU3000) if (PIOS_I2C_Init(&pios_i2c_gyro_adapter_id, &pios_i2c_gyro_adapter_cfg)) { PIOS_DEBUG_Assert(0); } PIOS_IMU3000_Init(); #endif /* PIOS_INCLUDE_IMU3000 */ #endif /* PIOS_INCLUDE_I2C */ #if defined(PIOS_INCLUDE_SPI) /* Set up the SPI interface to the accelerometer*/ if (PIOS_SPI_Init(&pios_spi_accel_id, &pios_spi_accel_cfg)) { PIOS_DEBUG_Assert(0); } PIOS_BMA180_Attach(pios_spi_accel_id); // #include "ahrs_spi_comm.h" // InsInitComms(); // // /* Set up the SPI interface to the OP board */ // if (PIOS_SPI_Init(&pios_spi_op_id, &pios_spi_op_cfg)) { // PIOS_DEBUG_Assert(0); // } // // InsConnect(pios_spi_op_id); #endif /* PIOS_INCLUDE_SPI */ }
/** * PIOS_Board_Init() * initializes all the core subsystems on this specific hardware * called from System/openpilot.c */ void PIOS_Board_Init(void) { /* Brings up System using CMSIS functions, enables the LEDs. */ PIOS_SYS_Init(); PIOS_LED_On(LED1); /* Delay system */ PIOS_DELAY_Init(); /* Communication system */ #if !defined(PIOS_ENABLE_DEBUG_PINS) #if defined(PIOS_INCLUDE_COM) if (PIOS_USART_Init(&pios_usart_aux_id, &pios_usart_aux_cfg)) { PIOS_DEBUG_Assert(0); } if (PIOS_COM_Init(&pios_com_aux_id, &pios_usart_com_driver, pios_usart_aux_id)) { PIOS_DEBUG_Assert(0); } #endif /* PIOS_INCLUDE_COM */ #endif /* IAP System Setup */ PIOS_IAP_Init(); /* ADC system */ PIOS_ADC_Init(); extern uint8_t adc_oversampling; PIOS_ADC_Config(adc_oversampling); extern void adc_callback(float *); PIOS_ADC_SetCallback(adc_callback); /* ADC buffer */ extern t_fifo_buffer adc_fifo_buffer; fifoBuf_init(&adc_fifo_buffer, adc_fifo_buf, sizeof(adc_fifo_buf)); /* Setup the Accelerometer FS (Full-Scale) GPIO */ PIOS_GPIO_Enable(0); SET_ACCEL_6G; #if defined(PIOS_INCLUDE_HMC5843) && defined(PIOS_INCLUDE_I2C) /* Magnetic sensor system */ if (PIOS_I2C_Init(&pios_i2c_main_adapter_id, &pios_i2c_main_adapter_cfg)) { PIOS_DEBUG_Assert(0); } PIOS_HMC5843_Init(); #endif #if defined(PIOS_INCLUDE_SPI) #include "ahrs_spi_comm.h" AhrsInitComms(); /* Set up the SPI interface to the OP board */ if (PIOS_SPI_Init(&pios_spi_op_id, &pios_spi_op_cfg)) { PIOS_DEBUG_Assert(0); } AhrsConnect(pios_spi_op_id); #endif }
/** * Tau Labs Main function: * * Initialize PiOS<BR> * Create the "System" task (SystemModInitializein Modules/System/systemmod.c) <BR> * Start FreeRTOS Scheduler (vTaskStartScheduler) (Now handled by caller) * If something goes wrong, blink LED1 and LED2 every 100ms * */ int main() { /* NOTE: Do NOT modify the following start-up sequence */ /* Any new initialization functions should be added in OpenPilotInit() */ /* Brings up System using CMSIS functions, enables the LEDs. */ PIOS_SYS_Init(); /* Architecture dependant Hardware and * core subsystem initialisation * (see pios_board.c for your arch) * */ PIOS_Board_Init(); PIOS_WDG_Clear(); #ifdef ERASE_FLASH PIOS_Flash_Jedec_EraseChip(); #if defined(PIOS_LED_HEARTBEAT) PIOS_LED_Off(PIOS_LED_HEARTBEAT); #endif /* PIOS_LED_HEARTBEAT */ while (1) ; #endif /* Initialize modules */ MODULE_INITIALISE_ALL(PIOS_WDG_Clear); /* swap the stack to use the IRQ stack */ Stack_Change(); /* Start the FreeRTOS scheduler, which should never return. * * NOTE: OpenPilot runs an operating system (FreeRTOS), which constantly calls * (schedules) function files (modules). These functions never return from their * while loops, which explains why each module has a while(1){} segment. Thus, * the OpenPilot software actually starts at the vTaskStartScheduler() function, * even though this is somewhat obscure. * * In addition, there are many main() functions in the OpenPilot firmware source tree * This is because each main() refers to a separate hardware platform. Of course, * C only allows one main(), so only the relevant main() function is compiled when * making a specific firmware. * */ vTaskStartScheduler(); /* If all is well we will never reach here as the scheduler will now be running. */ /* Do some indication to user that something bad just happened */ while (1) { #if defined(PIOS_LED_HEARTBEAT) PIOS_LED_Toggle(PIOS_LED_HEARTBEAT); #endif /* PIOS_LED_HEARTBEAT */ PIOS_DELAY_WaitmS(100); } return 0; }
/** * @brief Bootloader Main function */ int main() { uint8_t GO_dfu = false; /* Brings up System using CMSIS functions, enables the LEDs. */ PIOS_SYS_Init(); /* Enable Prefetch Buffer */ FLASH_PrefetchBufferCmd(FLASH_PrefetchBuffer_Enable); /* Flash 2 wait state */ FLASH_SetLatency(FLASH_Latency_2); RCC_AHBPeriphClockCmd(RCC_AHBPeriph_CRC, ENABLE); /* Delay system */ PIOS_DELAY_Init(); for (uint32_t t = 0; t < 10000000; ++t) { if (NSS_HOLD_STATE == 1) GO_dfu = TRUE; else { GO_dfu = FALSE; break; } } //while(TRUE) // { // PIOS_LED_Toggle(LED1); // PIOS_DELAY_WaitmS(1000); // } //GO_dfu = TRUE; PIOS_IAP_Init(); GO_dfu = GO_dfu | PIOS_IAP_CheckRequest();// OR with app boot request if (GO_dfu == FALSE) { jump_to_app(); } if (PIOS_IAP_CheckRequest()) { PIOS_DELAY_WaitmS(1000); PIOS_IAP_ClearRequest(); } PIOS_Board_Init(); boot_status = idle; Fw_crc = PIOS_BL_HELPER_CRC_Memory_Calc(); PIOS_LED_On(LED1); while (1) { process_spi_request(); } return 0; }
/** * Test Main function */ int main() { // Init PIOS_SYS_Init(); PIOS_DELAY_Init(); PIOS_COM_Init(); PIOS_ADC_Init(); PIOS_I2C_Init(); xTaskCreate(TaskTesting, (signed portCHAR *)"Test", configMINIMAL_STACK_SIZE , NULL, 1, NULL); // Start the FreeRTOS scheduler vTaskStartScheduler(); // Should not get here PIOS_DEBUG_Assert(0); return 0; }
/** * OpenPilot Main function: * * Initialize PiOS<BR> * Create the "System" task (SystemModInitializein Modules/System/systemmod.c) <BR> * Start FreeRTOS Scheduler (vTaskStartScheduler)<BR> * If something goes wrong, blink LED1 and LED2 every 100ms * */ int main() { /* NOTE: Do NOT modify the following start-up sequence */ /* Any new initialization functions should be added in OpenPilotInit() */ /* Brings up System using CMSIS functions, enables the LEDs. */ PIOS_SYS_Init(); /* Initialize the system thread */ SystemModInitialize(); /* Start the FreeRTOS scheduler */ vTaskStartScheduler(); /* If all is well we will never reach here as the scheduler will now be running. */ /* If we do get here, it will most likely be because we ran out of heap space. */ return 0; }
/** * OpenPilot Main function: * * Initialize PiOS<BR> * Create the "System" task (SystemModInitializein Modules/System/systemmod.c) <BR> * Start FreeRTOS Scheduler (vTaskStartScheduler)<BR> * If something goes wrong, blink LED1 and LED2 every 100ms * */ int main() { /* NOTE: Do NOT modify the following start-up sequence */ /* Any new initialization functions should be added in OpenPilotInit() */ /* Brings up System using CMSIS functions, enables the LEDs. */ PIOS_SYS_Init(); /* Architecture dependant Hardware and * core subsystem initialisation * (see pios_board.c for your arch) * */ PIOS_Board_Init(); /* Initialize modules */ MODULE_INITIALISE_ALL #if INCLUDE_TEST_TASKS /* Create test tasks */ xTaskCreate(TaskTesting, (signed portCHAR *)"Testing", configMINIMAL_STACK_SIZE , NULL, 4, NULL); xTaskCreate(TaskHIDTest, (signed portCHAR *)"HIDTest", configMINIMAL_STACK_SIZE , NULL, 3, NULL); xTaskCreate(TaskServos, (signed portCHAR *)"Servos", configMINIMAL_STACK_SIZE , NULL, 3, NULL); xTaskCreate(TaskSDCard, (signed portCHAR *)"SDCard", configMINIMAL_STACK_SIZE, NULL, (tskIDLE_PRIORITY + 2), NULL); #endif /* swap the stack to use the IRQ stack (does nothing in sim mode) */ Stack_Change_Weak(); /* Start the FreeRTOS scheduler which should never returns.*/ vTaskStartScheduler(); /* If all is well we will never reach here as the scheduler will now be running. */ /* Do some indication to user that something bad just happened */ PIOS_LED_Off(LED1); \ for(;;) { \ PIOS_LED_Toggle(LED1); \ PIOS_DELAY_WaitmS(100); \ }; return 0; }
int main(int argc, char *argv[]) { PIOS_SYS_Args(argc, argv); #else int main() { #endif /* NOTE: Do NOT modify the following start-up sequence */ /* Any new initialization functions should be added in OpenPilotInit() */ PIOS_heap_initialize_blocks(); #if defined(PIOS_INCLUDE_CHIBIOS) halInit(); chSysInit(); boardInit(); #endif /* defined(PIOS_INCLUDE_CHIBIOS) */ /* Brings up System using CMSIS functions, enables the LEDs. */ PIOS_SYS_Init(); /* For Revolution we use a FreeRTOS task to bring up the system so we can */ /* always rely on FreeRTOS primitive */ initTaskHandle = PIOS_Thread_Create(initTask, "init", INIT_TASK_STACK, NULL, INIT_TASK_PRIORITY); PIOS_Assert(initTaskHandle != NULL); #if defined(PIOS_INCLUDE_FREERTOS) /* Start the FreeRTOS scheduler */ vTaskStartScheduler(); /* If all is well we will never reach here as the scheduler will now be running. */ /* Do some PIOS_LED_HEARTBEAT to user that something bad just happened */ PIOS_LED_Off(PIOS_LED_HEARTBEAT); \ for(;;) { \ PIOS_LED_Toggle(PIOS_LED_HEARTBEAT); \ PIOS_DELAY_WaitmS(100); \ }; #elif defined(PIOS_INCLUDE_CHIBIOS) PIOS_Thread_Sleep(PIOS_THREAD_TIMEOUT_MAX); #endif /* defined(PIOS_INCLUDE_CHIBIOS) */ return 0; }
int main() { PIOS_SYS_Init(); if (BSL_HOLD_STATE == 0) USB_connected = TRUE; PIOS_IAP_Init(); if (PIOS_IAP_CheckRequest() == TRUE) { PIOS_Board_Init(); PIOS_DELAY_WaitmS(1000); User_DFU_request = TRUE; PIOS_IAP_ClearRequest(); } GO_dfu = (USB_connected == TRUE) || (User_DFU_request == TRUE); if (GO_dfu == TRUE) { PIOS_Board_Init(); if (User_DFU_request == TRUE) DeviceState = DFUidle; else DeviceState = BLidle; STOPWATCH_Init(100, LED_PWM_TIMER); } else JumpToApp = TRUE; STOPWATCH_Reset(LED_PWM_TIMER); while (TRUE) { if (JumpToApp == TRUE) jump_to_app(); switch (DeviceState) { case Last_operation_Success: case uploadingStarting: case DFUidle: period1 = 50; sweep_steps1 = 100; PIOS_LED_Off(BLUE); period2 = 0; break; case uploading: period1 = 50; sweep_steps1 = 100; period2 = 25; sweep_steps2 = 50; break; case downloading: period1 = 25; sweep_steps1 = 50; PIOS_LED_Off(BLUE); period2 = 0; break; case BLidle: period1 = 0; PIOS_LED_On(BLUE); period2 = 0; break; default://error period1 = 50; sweep_steps1 = 100; period2 = 50; sweep_steps2 = 100; } if (period1 != 0) { if (LedPWM(period1, sweep_steps1, STOPWATCH_ValueGet(LED_PWM_TIMER))) PIOS_LED_On(BLUE); else PIOS_LED_Off(BLUE); } else PIOS_LED_On(BLUE); if (period2 != 0) { if (LedPWM(period2, sweep_steps2, STOPWATCH_ValueGet(LED_PWM_TIMER))) PIOS_LED_On(BLUE); else PIOS_LED_Off(BLUE); } else PIOS_LED_Off(BLUE); if (STOPWATCH_ValueGet(LED_PWM_TIMER) > 100 * 50 * 100) STOPWATCH_Reset(LED_PWM_TIMER); if ((STOPWATCH_ValueGet(LED_PWM_TIMER) > 60000) && (DeviceState == BLidle)) JumpToApp = TRUE; processRX(); DataDownload(start); } }
int main() { float gyro[3], accel[3], mag[3]; float vel[3] = { 0, 0, 0 }; /* Normaly we get/set UAVObjects but this one only needs to be set. We will never expect to get this from another module*/ AttitudeActualData attitude_actual; AHRSSettingsData ahrs_settings; /* Brings up System using CMSIS functions, enables the LEDs. */ PIOS_SYS_Init(); /* Delay system */ PIOS_DELAY_Init(); /* Communication system */ PIOS_COM_Init(); /* ADC system */ AHRS_ADC_Config( adc_oversampling ); /* Setup the Accelerometer FS (Full-Scale) GPIO */ PIOS_GPIO_Enable( 0 ); SET_ACCEL_2G; #if defined(PIOS_INCLUDE_HMC5843) && defined(PIOS_INCLUDE_I2C) /* Magnetic sensor system */ PIOS_I2C_Init(); PIOS_HMC5843_Init(); // Get 3 ID bytes strcpy(( char * )mag_data.id, "ZZZ" ); PIOS_HMC5843_ReadID( mag_data.id ); #endif /* SPI link to master */ // PIOS_SPI_Init(); AhrsInitComms(); AHRSCalibrationConnectCallback( calibration_callback ); GPSPositionConnectCallback( gps_callback ); ahrs_state = AHRS_IDLE; while( !AhrsLinkReady() ) { AhrsPoll(); while( ahrs_state != AHRS_DATA_READY ) ; ahrs_state = AHRS_PROCESSING; downsample_data(); ahrs_state = AHRS_IDLE; if(( total_conversion_blocks % 50 ) == 0 ) PIOS_LED_Toggle( LED1 ); } AHRSSettingsGet(&ahrs_settings); /* Use simple averaging filter for now */ for( int i = 0; i < adc_oversampling; i++ ) fir_coeffs[i] = 1; fir_coeffs[adc_oversampling] = adc_oversampling; if( ahrs_settings.Algorithm == AHRSSETTINGS_ALGORITHM_INSGPS) { // compute a data point and initialize INS downsample_data(); converge_insgps(); } #ifdef DUMP_RAW int previous_conversion; while( 1 ) { AhrsPoll(); int result; uint8_t framing[16] = { 0, 1, 2, 3, 4, 5, 6, 7, 8, 9, 10, 11, 12, 13, 14, 15 }; while( ahrs_state != AHRS_DATA_READY ) ; ahrs_state = AHRS_PROCESSING; if( total_conversion_blocks != previous_conversion + 1 ) PIOS_LED_On( LED1 ); // not keeping up else PIOS_LED_Off( LED1 ); previous_conversion = total_conversion_blocks; downsample_data(); ahrs_state = AHRS_IDLE;; // Dump raw buffer result = PIOS_COM_SendBuffer( PIOS_COM_AUX, &framing[0], 16 ); // framing header result += PIOS_COM_SendBuffer( PIOS_COM_AUX, ( uint8_t * ) & total_conversion_blocks, sizeof( total_conversion_blocks ) ); // dump block number result += PIOS_COM_SendBuffer( PIOS_COM_AUX, ( uint8_t * ) & valid_data_buffer[0], ADC_OVERSAMPLE * ADC_CONTINUOUS_CHANNELS * sizeof( valid_data_buffer[0] ) ); if( result == 0 ) PIOS_LED_Off( LED1 ); else { PIOS_LED_On( LED1 ); } } #endif timer_start(); /******************* Main EKF loop ****************************/ while( 1 ) { AhrsPoll(); AHRSCalibrationData calibration; AHRSCalibrationGet( &calibration ); BaroAltitudeData baro_altitude; BaroAltitudeGet( &baro_altitude ); GPSPositionData gps_position; GPSPositionGet( &gps_position ); AHRSSettingsGet(&ahrs_settings); // Alive signal if(( total_conversion_blocks % 100 ) == 0 ) PIOS_LED_Toggle( LED1 ); #if defined(PIOS_INCLUDE_HMC5843) && defined(PIOS_INCLUDE_I2C) // Get magnetic readings if( PIOS_HMC5843_NewDataAvailable() ) { PIOS_HMC5843_ReadMag( mag_data.raw.axis ); mag_data.updated = 1; } attitude_raw.magnetometers[0] = mag_data.raw.axis[0]; attitude_raw.magnetometers[2] = mag_data.raw.axis[1]; attitude_raw.magnetometers[2] = mag_data.raw.axis[2]; #endif // Delay for valid data counter_val = timer_count(); running_counts = counter_val - last_counter_idle_end; last_counter_idle_start = counter_val; while( ahrs_state != AHRS_DATA_READY ) ; counter_val = timer_count(); idle_counts = counter_val - last_counter_idle_start; last_counter_idle_end = counter_val; ahrs_state = AHRS_PROCESSING; downsample_data(); /***************** SEND BACK SOME RAW DATA ************************/ // Hacky - grab one sample from buffer to populate this. Need to send back // all raw data if it's happening accel_data.raw.x = valid_data_buffer[0]; accel_data.raw.y = valid_data_buffer[2]; accel_data.raw.z = valid_data_buffer[4]; gyro_data.raw.x = valid_data_buffer[1]; gyro_data.raw.y = valid_data_buffer[3]; gyro_data.raw.z = valid_data_buffer[5]; gyro_data.temp.xy = valid_data_buffer[6]; gyro_data.temp.z = valid_data_buffer[7]; if( ahrs_settings.Algorithm == AHRSSETTINGS_ALGORITHM_INSGPS) { /******************** INS ALGORITHM **************************/ // format data for INS algo gyro[0] = gyro_data.filtered.x; gyro[1] = gyro_data.filtered.y; gyro[2] = gyro_data.filtered.z; accel[0] = accel_data.filtered.x, accel[1] = accel_data.filtered.y, accel[2] = accel_data.filtered.z, // Note: The magnetometer driver returns registers X,Y,Z from the chip which are // (left, backward, up). Remapping to (forward, right, down). mag[0] = -( mag_data.raw.axis[1] - calibration.mag_bias[1] ); mag[1] = -( mag_data.raw.axis[0] - calibration.mag_bias[0] ); mag[2] = -( mag_data.raw.axis[2] - calibration.mag_bias[2] ); INSStatePrediction( gyro, accel, 1 / ( float )EKF_RATE ); INSCovariancePrediction( 1 / ( float )EKF_RATE ); if( gps_updated && gps_position.Status == GPSPOSITION_STATUS_FIX3D ) { // Compute velocity from Heading and groundspeed vel[0] = gps_position.Groundspeed * cos( gps_position.Heading * M_PI / 180 ); vel[1] = gps_position.Groundspeed * sin( gps_position.Heading * M_PI / 180 ); // Completely unprincipled way to make the position variance // increase as data quality decreases but keep it bounded // Variance becomes 40 m^2 and 40 (m/s)^2 when no gps INSSetPosVelVar( 0.004 ); HomeLocationData home; HomeLocationGet( &home ); float ned[3]; double lla[3] = {( double ) gps_position.Latitude / 1e7, ( double ) gps_position.Longitude / 1e7, ( double )( gps_position.GeoidSeparation + gps_position.Altitude )}; // convert from cm back to meters double ecef[3] = {( double )( home.ECEF[0] / 100 ), ( double )( home.ECEF[1] / 100 ), ( double )( home.ECEF[2] / 100 )}; LLA2Base( lla, ecef, ( float( * )[3] ) home.RNE, ned ); if( gps_updated ) { //FIXME: Is this correct? //TOOD: add check for altitude updates FullCorrection( mag, ned, vel, baro_altitude.Altitude ); gps_updated = false; } else { GpsBaroCorrection( ned, vel, baro_altitude.Altitude ); } gps_updated = false; mag_data.updated = 0; } else if( gps_position.Status == GPSPOSITION_STATUS_FIX3D && mag_data.updated == 1 ) { MagCorrection( mag ); // only trust mags if outdoors mag_data.updated = 0; } else { // Indoors, update with zero position and velocity and high covariance INSSetPosVelVar( 0.1 ); vel[0] = 0; vel[1] = 0; vel[2] = 0; VelBaroCorrection( vel, baro_altitude.Altitude ); // MagVelBaroCorrection(mag,vel,altitude_data.altitude); // only trust mags if outdoors } attitude_actual.q1 = Nav.q[0]; attitude_actual.q2 = Nav.q[1]; attitude_actual.q3 = Nav.q[2]; attitude_actual.q4 = Nav.q[3]; } else if( ahrs_settings.Algorithm == AHRSSETTINGS_ALGORITHM_SIMPLE ) { float q[4]; float rpy[3]; /***************** SIMPLE ATTITUDE FROM NORTH AND ACCEL ************/ /* Very simple computation of the heading and attitude from accel. */ rpy[2] = atan2(( mag_data.raw.axis[0] ), ( -1 * mag_data.raw.axis[1] ) ) * 180 / M_PI; rpy[1] = atan2( accel_data.filtered.x, accel_data.filtered.z ) * 180 / M_PI; rpy[0] = atan2( accel_data.filtered.y, accel_data.filtered.z ) * 180 / M_PI; RPY2Quaternion( rpy, q ); attitude_actual.q1 = q[0]; attitude_actual.q2 = q[1]; attitude_actual.q3 = q[2]; attitude_actual.q4 = q[3]; } ahrs_state = AHRS_IDLE; #ifdef DUMP_FRIENDLY PIOS_COM_SendFormattedStringNonBlocking( PIOS_COM_AUX, "b: %d\r\n", total_conversion_blocks ); PIOS_COM_SendFormattedStringNonBlocking( PIOS_COM_AUX, "a: %d %d %d\r\n", ( int16_t )( accel_data.filtered.x * 1000 ), ( int16_t )( accel_data.filtered.y * 1000 ), ( int16_t )( accel_data.filtered.z * 1000 ) ); PIOS_COM_SendFormattedStringNonBlocking( PIOS_COM_AUX, "g: %d %d %d\r\n", ( int16_t )( gyro_data.filtered.x * 1000 ), ( int16_t )( gyro_data.filtered.y * 1000 ), ( int16_t )( gyro_data.filtered.z * 1000 ) ); PIOS_COM_SendFormattedStringNonBlocking( PIOS_COM_AUX, "m: %d %d %d\r\n", mag_data.raw.axis[0], mag_data.raw.axis[1], mag_data.raw.axis[2] ); PIOS_COM_SendFormattedStringNonBlocking( PIOS_COM_AUX, "q: %d %d %d %d\r\n", ( int16_t )( Nav.q[0] * 1000 ), ( int16_t )( Nav.q[1] * 1000 ), ( int16_t )( Nav.q[2] * 1000 ), ( int16_t )( Nav.q[3] * 1000 ) ); #endif #ifdef DUMP_EKF uint8_t framing[16] = { 15, 14, 13, 12, 11, 10, 9, 8, 7, 6, 5, 4, 3, 2, 1, 0 }; extern float F[NUMX][NUMX], G[NUMX][NUMW], H[NUMV][NUMX]; // linearized system matrices extern float P[NUMX][NUMX], X[NUMX]; // covariance matrix and state vector extern float Q[NUMW], R[NUMV]; // input noise and measurement noise variances extern float K[NUMX][NUMV]; // feedback gain matrix // Dump raw buffer int8_t result; result = PIOS_COM_SendBuffer( PIOS_COM_AUX, &framing[0], 16 ); // framing header result += PIOS_COM_SendBuffer( PIOS_COM_AUX, ( uint8_t * ) & total_conversion_blocks, sizeof( total_conversion_blocks ) ); // dump block number result += PIOS_COM_SendBuffer( PIOS_COM_AUX, ( uint8_t * ) & mag_data, sizeof( mag_data ) ); result += PIOS_COM_SendBuffer( PIOS_COM_AUX, ( uint8_t * ) & gps_data, sizeof( gps_data ) ); result += PIOS_COM_SendBuffer( PIOS_COM_AUX, ( uint8_t * ) & accel_data, sizeof( accel_data ) ); result += PIOS_COM_SendBuffer( PIOS_COM_AUX, ( uint8_t * ) & gyro_data, sizeof( gyro_data ) ); result += PIOS_COM_SendBuffer( PIOS_COM_AUX, ( uint8_t * ) & Q, sizeof( float ) * NUMX * NUMX ); result += PIOS_COM_SendBuffer( PIOS_COM_AUX, ( uint8_t * ) & K, sizeof( float ) * NUMX * NUMV ); result += PIOS_COM_SendBuffer( PIOS_COM_AUX, ( uint8_t * ) & X, sizeof( float ) * NUMX * NUMX ); if( result == 0 ) PIOS_LED_Off( LED1 ); else { PIOS_LED_On( LED1 ); } #endif AttitudeActualSet( &attitude_actual ); /*FIXME: This is dangerous. There is no locking for UAVObjects so it could stomp all over the airspeed/climb rate etc. This used to be done in the OP module which was bad. Having ~4ms latency for the round trip makes it worse here. */ PositionActualData pos; PositionActualGet( &pos ); for( int ct = 0; ct < 3; ct++ ) { pos.NED[ct] = Nav.Pos[ct]; pos.Vel[ct] = Nav.Vel[ct]; } PositionActualSet( &pos ); static bool was_calibration = false; AhrsStatusData status; AhrsStatusGet( &status ); if( was_calibration != status.CalibrationSet ) { was_calibration = status.CalibrationSet; if( status.CalibrationSet ) { calibrate_sensors(); AhrsStatusGet( &status ); status.CalibrationSet = true; } } status.CPULoad = (( float )running_counts / ( float )( idle_counts + running_counts ) ) * 100; status.IdleTimePerCyle = idle_counts / ( TIMER_RATE / 10000 ); status.RunningTimePerCyle = running_counts / ( TIMER_RATE / 10000 ); status.DroppedUpdates = ekf_too_slow; AhrsStatusSet( &status ); } return 0; }
int main() { PIOS_SYS_Init(); PIOS_Board_Init(); PIOS_IAP_Init(); USB_connected = PIOS_USB_CheckAvailable(0); if (PIOS_IAP_CheckRequest() == true) { PIOS_DELAY_WaitmS(1000); User_DFU_request = true; PIOS_IAP_ClearRequest(); } GO_dfu = (USB_connected == true) || (User_DFU_request == true); if (GO_dfu == true) { PIOS_Board_Init(); if (User_DFU_request == true) DeviceState = DFUidle; else DeviceState = BLidle; } else JumpToApp = true; uint32_t stopwatch = 0; uint32_t prev_ticks = PIOS_DELAY_GetuS(); while (true) { /* Update the stopwatch */ uint32_t elapsed_ticks = PIOS_DELAY_GetuSSince(prev_ticks); prev_ticks += elapsed_ticks; stopwatch += elapsed_ticks; if (JumpToApp == true) jump_to_app(); switch (DeviceState) { case Last_operation_Success: case uploadingStarting: case DFUidle: period1 = 5000; sweep_steps1 = 100; PIOS_LED_Off(PIOS_LED_HEARTBEAT); period2 = 0; break; case uploading: period1 = 5000; sweep_steps1 = 100; period2 = 2500; sweep_steps2 = 50; break; case downloading: period1 = 2500; sweep_steps1 = 50; PIOS_LED_Off(PIOS_LED_HEARTBEAT); period2 = 0; break; case BLidle: period1 = 0; PIOS_LED_On(PIOS_LED_HEARTBEAT); period2 = 0; break; default://error period1 = 5000; sweep_steps1 = 100; period2 = 5000; sweep_steps2 = 100; } if (period1 != 0) { if (LedPWM(period1, sweep_steps1, stopwatch)) PIOS_LED_On(PIOS_LED_HEARTBEAT); else PIOS_LED_Off(PIOS_LED_HEARTBEAT); } else PIOS_LED_On(PIOS_LED_HEARTBEAT); if (period2 != 0) { if (LedPWM(period2, sweep_steps2, stopwatch)) PIOS_LED_On(PIOS_LED_HEARTBEAT); else PIOS_LED_Off(PIOS_LED_HEARTBEAT); } else PIOS_LED_Off(PIOS_LED_HEARTBEAT); if (stopwatch > 50 * 1000 * 1000) stopwatch = 0; if ((stopwatch > 6 * 1000 * 1000) && (DeviceState == BLidle)) JumpToApp = true; processRX(); DataDownload(start); } }
int main() { PIOS_SYS_Init(); PIOS_Board_Init(); PIOS_LED_On(PIOS_LED_HEARTBEAT); PIOS_DELAY_WaitmS(3000); PIOS_LED_Off(PIOS_LED_HEARTBEAT); /// Self overwrite check uint32_t base_address = SCB->VTOR; if ((0x08000000 + embedded_image_size) > base_address) error(PIOS_LED_HEARTBEAT, 1); /// /* * Make sure the bootloader we're carrying is for the same * board type and board revision as the one we're running on. * * Assume the bootloader in flash and the bootloader contained in * the updater both carry a board_info_blob at the end of the image. */ /* Calculate how far the board_info_blob is from the beginning of the bootloader */ uint32_t board_info_blob_offset = (uint32_t) &pios_board_info_blob - (uint32_t)0x08000000; /* Use the same offset into our embedded bootloader image */ struct pios_board_info * new_board_info_blob = (struct pios_board_info *) ((uint32_t)embedded_image_start + board_info_blob_offset); /* Compare the two board info blobs to make sure they're for the same HW revision */ if ((pios_board_info_blob.magic != new_board_info_blob->magic) || (pios_board_info_blob.board_type != new_board_info_blob->board_type) || (pios_board_info_blob.board_rev != new_board_info_blob->board_rev)) { error(PIOS_LED_HEARTBEAT, 2); } /* Embedded bootloader looks like it's the right one for this HW, proceed... */ FLASH_Unlock(); /// Bootloader memory space erase uint32_t pageAddress; pageAddress = 0x08000000; bool fail = false; while ((pageAddress < base_address) && (fail == false)) { for (int retry = 0; retry < MAX_DEL_RETRYS; ++retry) { if (FLASH_ErasePage(pageAddress) == FLASH_COMPLETE) { fail = false; break; } else { fail = true; } } #ifdef STM32F10X_HD pageAddress += 2048; #elif defined (STM32F10X_MD) pageAddress += 1024; #endif } if (fail == true) error(PIOS_LED_HEARTBEAT, 3); /// /// Bootloader programing for (uint32_t offset = 0; offset < embedded_image_size / sizeof(uint32_t); ++offset) { bool result = false; PIOS_LED_Toggle(PIOS_LED_HEARTBEAT); for (uint8_t retry = 0; retry < MAX_WRI_RETRYS; ++retry) { if (result == false) { result = (FLASH_ProgramWord(0x08000000 + (offset * 4), embedded_image_start[offset]) == FLASH_COMPLETE) ? true : false; } } if (result == false) error(PIOS_LED_HEARTBEAT, 4); } /// for (uint8_t x = 0; x < 3; ++x) { PIOS_LED_On(PIOS_LED_HEARTBEAT); PIOS_DELAY_WaitmS(1000); PIOS_LED_Off(PIOS_LED_HEARTBEAT); PIOS_DELAY_WaitmS(1000); } /// Invalidate the bootloader updater so we won't run /// the update again on the next power cycle. FLASH_ProgramWord(base_address, 0); FLASH_Lock(); for (;;) { PIOS_DELAY_WaitmS(1000); } }
int main() { /* NOTE: Do NOT modify the following start-up sequence */ /* Any new initialization functions should be added in OpenPilotInit() */ /* Brings up System using CMSIS functions, enables the LEDs. */ PIOS_SYS_Init(); if (BSL_HOLD_STATE == 0) USB_connected = TRUE; PIOS_IAP_Init(); if (PIOS_IAP_CheckRequest() == TRUE) { PIOS_Board_Init(); PIOS_DELAY_WaitmS(1000); User_DFU_request = TRUE; PIOS_IAP_ClearRequest(); } GO_dfu = (USB_connected == TRUE) || (User_DFU_request == TRUE); if (GO_dfu == TRUE) { PIOS_Board_Init(); if(User_DFU_request == TRUE) DeviceState = DFUidle; else DeviceState = BLidle; STOPWATCH_Init(100,LED_PWM_TIMER); } else JumpToApp = TRUE; STOPWATCH_Reset(LED_PWM_TIMER); while (TRUE) { if (JumpToApp == TRUE) jump_to_app(); //pwm_period = 50; // *100 uS -> 5 mS //pwm_sweep_steps =100; // * 5 mS -> 500 mS switch (DeviceState) { case Last_operation_Success: case uploadingStarting: case DFUidle: period1 = 50; sweep_steps1 = 100; PIOS_LED_Off(BLUE); period2 = 0; break; case uploading: period1 = 50; sweep_steps1 = 100; period2 = 25; sweep_steps2 = 50; break; case downloading: period1 = 25; sweep_steps1 = 50; PIOS_LED_Off(BLUE); period2 = 0; break; case BLidle: period1 = 0; PIOS_LED_On(BLUE); period2 = 0; break; default://error period1 = 50; sweep_steps1 = 100; period2 = 50; sweep_steps2 = 100; } if (period1 != 0) { if (LedPWM(period1, sweep_steps1, STOPWATCH_ValueGet(LED_PWM_TIMER))) PIOS_LED_On(BLUE); else PIOS_LED_Off(BLUE); } else PIOS_LED_On(BLUE); if (period2 != 0) { if (LedPWM(period2, sweep_steps2, STOPWATCH_ValueGet(LED_PWM_TIMER))) PIOS_LED_On(BLUE); else PIOS_LED_Off(BLUE); } else PIOS_LED_Off(BLUE); if (STOPWATCH_ValueGet(LED_PWM_TIMER) > 100 * 50 * 100) STOPWATCH_Reset(LED_PWM_TIMER); if ((STOPWATCH_ValueGet(LED_PWM_TIMER) > 60000) && (DeviceState == BLidle)) JumpToApp = TRUE; processRX(); DataDownload(start); } }
int main() { PIOS_SYS_Init(); PIOS_Board_Init(); PIOS_IAP_Init(); // Make sure the brown out reset value for this chip // is 2.7 volts check_bor(); #ifdef PIOS_INCLUDE_USB USB_connected = PIOS_USB_CheckAvailable(0); #endif if (PIOS_IAP_CheckRequest() == true) { PIOS_DELAY_WaitmS(1000); User_DFU_request = true; PIOS_IAP_ClearRequest(); } GO_dfu = (USB_connected == true) || (User_DFU_request == true); if (GO_dfu == true) { if (User_DFU_request == true) { DeviceState = DFUidle; } else { DeviceState = BLidle; } } else { JumpToApp = true; } uint32_t stopwatch = 0; uint32_t prev_ticks = PIOS_DELAY_GetuS(); while (true) { /* Update the stopwatch */ uint32_t elapsed_ticks = PIOS_DELAY_GetuSSince(prev_ticks); prev_ticks += elapsed_ticks; stopwatch += elapsed_ticks; if (JumpToApp == true) { jump_to_app(); } switch (DeviceState) { case Last_operation_Success: case uploadingStarting: case DFUidle: period1 = 5000; sweep_steps1 = 100; PIOS_LED_Off(PIOS_LED_HEARTBEAT); period2 = 0; break; case uploading: period1 = 5000; sweep_steps1 = 100; period2 = 2500; sweep_steps2 = 50; break; case downloading: period1 = 2500; sweep_steps1 = 50; PIOS_LED_Off(PIOS_LED_HEARTBEAT); period2 = 0; break; case BLidle: period1 = 0; PIOS_LED_On(PIOS_LED_HEARTBEAT); period2 = 0; break; default: // error period1 = 5000; sweep_steps1 = 100; period2 = 5000; sweep_steps2 = 100; } if (period1 != 0) { if (LedPWM(period1, sweep_steps1, stopwatch)) { PIOS_LED_On(PIOS_LED_HEARTBEAT); } else { PIOS_LED_Off(PIOS_LED_HEARTBEAT); } } else { PIOS_LED_On(PIOS_LED_HEARTBEAT); } if (period2 != 0) { if (LedPWM(period2, sweep_steps2, stopwatch)) { PIOS_LED_On(PIOS_LED_HEARTBEAT); } else { PIOS_LED_Off(PIOS_LED_HEARTBEAT); } } else { PIOS_LED_Off(PIOS_LED_HEARTBEAT); } if (stopwatch > 50 * 1000 * 1000) { stopwatch = 0; } if ((stopwatch > 6 * 1000 * 1000) && ((DeviceState == BLidle) || (DeviceState == DFUidle && !USB_connected))) { JumpToApp = true; } processRX(); DataDownload(start); } }
int main() { /* NOTE: Do NOT modify the following start-up sequence */ /* Any new initialization functions should be added in OpenPilotInit() */ /* Brings up System using CMSIS functions, enables the LEDs. */ PIOS_SYS_Init(); if (BSL_HOLD_STATE == 0) USB_connected = TRUE; PIOS_IAP_Init(); if (PIOS_IAP_CheckRequest() == TRUE) { PIOS_Board_Init(); PIOS_DELAY_WaitmS(1000); User_DFU_request = TRUE; PIOS_IAP_ClearRequest(); } GO_dfu = (USB_connected == TRUE) || (User_DFU_request == TRUE); if (GO_dfu == TRUE) { if (USB_connected) ProgPort = Usb; else ProgPort = Serial; PIOS_Board_Init(); if(User_DFU_request == TRUE) DeviceState = DFUidle; else DeviceState = BLidle; STOPWATCH_Init(100,LED_PWM_TIMER); if (ProgPort == Serial) { fifoBuf_init(&ssp_buffer,rx_buffer,UART_BUFFER_SIZE); STOPWATCH_Init(100,SSP_TIMER);//nao devia ser 1000? STOPWATCH_Reset(SSP_TIMER); ssp_Init(&ssp_port, &SSP_PortConfig); } PIOS_OPAHRS_ForceSlaveSelected(true); } else JumpToApp = TRUE; STOPWATCH_Reset(LED_PWM_TIMER); while (TRUE) { if (ProgPort == Serial) { ssp_ReceiveProcess(&ssp_port); status=ssp_SendProcess(&ssp_port); while((status!=SSP_TX_IDLE) && (status!=SSP_TX_ACKED)){ ssp_ReceiveProcess(&ssp_port); status=ssp_SendProcess(&ssp_port); } } if (JumpToApp == TRUE) jump_to_app(); //pwm_period = 50; // *100 uS -> 5 mS //pwm_sweep_steps =100; // * 5 mS -> 500 mS switch (DeviceState) { case Last_operation_Success: case uploadingStarting: case DFUidle: period1 = 50; sweep_steps1 = 100; PIOS_LED_Off(RED); period2 = 0; break; case uploading: period1 = 50; sweep_steps1 = 100; period2 = 25; sweep_steps2 = 50; break; case downloading: period1 = 25; sweep_steps1 = 50; PIOS_LED_Off(RED); period2 = 0; break; case BLidle: period1 = 0; PIOS_LED_On(BLUE); period2 = 0; break; default://error period1 = 50; sweep_steps1 = 100; period2 = 50; sweep_steps2 = 100; } if (period1 != 0) { if (LedPWM(period1, sweep_steps1, STOPWATCH_ValueGet(LED_PWM_TIMER))) PIOS_LED_On(BLUE); else PIOS_LED_Off(BLUE); } else PIOS_LED_On(BLUE); if (period2 != 0) { if (LedPWM(period2, sweep_steps2, STOPWATCH_ValueGet(LED_PWM_TIMER))) PIOS_LED_On(RED); else PIOS_LED_Off(RED); } else PIOS_LED_Off(RED); if (STOPWATCH_ValueGet(LED_PWM_TIMER) > 100 * 50 * 100) STOPWATCH_Reset(LED_PWM_TIMER); if ((STOPWATCH_ValueGet(LED_PWM_TIMER) > 60000) && (DeviceState == BLidle)) JumpToApp = TRUE; processRX(); DataDownload(start); } }