/** * System task, periodically executes every SYSTEM_UPDATE_PERIOD_MS */ static void systemTask(void *parameters) { portTickType lastSysTime; /* create all modules thread */ MODULE_TASKCREATE_ALL // Initialize vars idleCounter = 0; idleCounterClear = 0; lastSysTime = xTaskGetTickCount(); // Listen for SettingPersistance object updates, connect a callback function ObjectPersistenceConnectCallback(&objectUpdatedCb); // Main system loop while (1) { // Update the system statistics updateStats(); // Update the system alarms updateSystemAlarms(); #if defined(DIAGNOSTICS) updateI2Cstats(); updateWDGstats(); #endif // Update the task status object TaskMonitorUpdateAll(); // Flash the heartbeat LED PIOS_LED_Toggle(LED1); // Turn on the error LED if an alarm is set #if (PIOS_LED_NUM > 1) if (AlarmsHasErrors()) { PIOS_LED_Toggle(LED2); } else if (AlarmsHasWarnings()) { PIOS_LED_On(LED2); } else { PIOS_LED_Off(LED2); } #endif FlightStatusData flightStatus; FlightStatusGet(&flightStatus); // Wait until next period if(flightStatus.Armed == FLIGHTSTATUS_ARMED_ARMED) { vTaskDelayUntil(&lastSysTime, SYSTEM_UPDATE_PERIOD_MS / portTICK_RATE_MS / (LED_BLINK_RATE_HZ * 2) ); } else { vTaskDelayUntil(&lastSysTime, SYSTEM_UPDATE_PERIOD_MS / portTICK_RATE_MS); } } }
/** * Indicate a target-specific error code when a component fails to initialize * 1 pulse - flash chip * 2 pulses - MPU6000 * 3 pulses - HMC5883 * 4 pulses - MS5611 * 5 pulses - I2C bus locked */ void panic(int32_t code) { while(1){ for (int32_t i = 0; i < code; i++) { PIOS_LED_Toggle(PIOS_LED_ALARM); PIOS_DELAY_WaitmS(200); PIOS_LED_Toggle(PIOS_LED_ALARM); PIOS_DELAY_WaitmS(200); } PIOS_DELAY_WaitmS(500); } }
static void TaskTesting(void *pvParameters) { portTickType xDelay = 500 / portTICK_RATE_MS; portTickType xTimeout = 10 / portTICK_RATE_MS; PIOS_BMP085_Init(); for(;;) { PIOS_LED_Toggle(LED2); int16_t temp; int32_t pressure; PIOS_BMP085_StartADC(TemperatureConv); xSemaphoreTake(PIOS_BMP085_EOC, xTimeout); PIOS_BMP085_ReadADC(); PIOS_BMP085_StartADC(PressureConv); xSemaphoreTake(PIOS_BMP085_EOC, xTimeout); PIOS_BMP085_ReadADC(); temp = PIOS_BMP085_GetTemperature(); pressure = PIOS_BMP085_GetPressure(); PIOS_COM_SendFormattedStringNonBlocking(PIOS_COM_TELEM_RF, "%3u,%4u\r", temp, pressure); vTaskDelay(xDelay); } }
/** * 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; }
/** * @brief Execute the whole chip * @returns 0 if successful, -1 if unable to claim bus */ int32_t PIOS_Flash_Jedec_EraseChip() { if(PIOS_Flash_Jedec_Validate(flash_dev) != 0) return -1; uint8_t ret; uint8_t out[] = {flash_dev->cfg->chip_erase}; if((ret = PIOS_Flash_Jedec_WriteEnable()) != 0) return ret; if(PIOS_Flash_Jedec_ClaimBus() != 0) return -1; if(PIOS_SPI_TransferBlock(flash_dev->spi_id,out,NULL,sizeof(out),NULL) < 0) { PIOS_Flash_Jedec_ReleaseBus(); return -2; } PIOS_Flash_Jedec_ReleaseBus(); // Keep polling when bus is busy too int i = 0; while(PIOS_Flash_Jedec_Busy() != 0) { #if defined(FLASH_FREERTOS) vTaskDelay(1); #endif if ((i++) % 10000 == 0) PIOS_LED_Toggle(PIOS_LED_HEARTBEAT); } return 0; }
/** * 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; }
/** * Reports the name of the source file and the source line number * where the assert_param error has occurred. * \param[in] file pointer to the source file name * \param[in] line assert_param error line source number * \retval None */ void assert_failed(uint8_t *file, uint32_t line) { /* When serial debugging is implemented, use something like this. */ /* printf("Wrong parameters value: file %s on line %d\r\n", file, line); */ printf("Wrong parameters value: file %s on line %d\r\n", file, line); /* Setup the LEDs to Alternate */ PIOS_LED_On(LED1); PIOS_LED_Off(LED2); /* Infinite loop */ while (1) { PIOS_LED_Toggle(LED1); PIOS_LED_Toggle(LED2); for (int i = 0; i < 1000000; i++) { ; } } }
/** * Executed by event dispatcher callback to reset INS before resetting OP */ static void resetTask(UAVObjEvent * ev) { PIOS_LED_Toggle(LED1); #if (PIOS_LED_NUM > 1) PIOS_LED_Toggle(LED2); #endif if((portTickType) (xTaskGetTickCount() - lastResetSysTime) > RESET_DELAY / portTICK_RATE_MS) { lastResetSysTime = xTaskGetTickCount(); data.BoardType=0xFF; data.ArmReset=1; data.crc=reset_count; /* Must change a value for this to get to INS */ FirmwareIAPObjSet(&data); ++reset_count; if(reset_count>3) { PIOS_SYS_Reset(); } } }
static void OnError(void) { PIOS_LED_Off(LED1); while(1) { DebugPinHigh(DEBUG_PIN_ERROR); PIOS_LED_Toggle(LED2); vTaskDelay(50 / portTICK_RATE_MS); DebugPinLow(DEBUG_PIN_ERROR); } }
/** * @brief Initialize the flash object setting FS * @return 0 if success, -1 if failure */ int32_t PIOS_FLASHFS_Init() { // Check for valid object table or create one uint32_t object_table_magic; uint32_t magic_fail_count = 0; bool magic_good = false; while(!magic_good) { if (PIOS_Flash_W25X_ReadData(0, (uint8_t *)&object_table_magic, sizeof(object_table_magic)) != 0) return -1; if(object_table_magic != OBJECT_TABLE_MAGIC) { if(magic_fail_count++ > MAX_BADMAGIC) { PIOS_FLASHFS_ClearObjectTableHeader(); PIOS_LED_Toggle(LED1); magic_fail_count = 0; magic_good = true; } else { PIOS_DELAY_WaituS(1000); } } else { magic_good = true; } } int32_t addr = OBJECT_TABLE_START; struct objectHeader header; numObjects = 0; // Loop through header area while objects detect to count how many saved while(addr < OBJECT_TABLE_END) { // Read the instance data if (PIOS_Flash_W25X_ReadData(addr, (uint8_t *)&header, sizeof(header)) != 0) return -1; // Counting number of valid headers if(header.objMagic != OBJ_MAGIC) break; numObjects++; addr += sizeof(header); } 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; }
/** * System task, periodically executes every SYSTEM_UPDATE_PERIOD_MS */ static void systemTask(void *parameters) { portTickType lastSysTime; /* create all modules thread */ MODULE_TASKCREATE_ALL; if (mallocFailed) { /* We failed to malloc during task creation, * system behaviour is undefined. Reset and let * the BootFault code recover for us. */ PIOS_SYS_Reset(); } #if defined(PIOS_INCLUDE_IAP) /* Record a successful boot */ PIOS_IAP_WriteBootCount(0); #endif // Initialize vars idleCounter = 0; idleCounterClear = 0; lastSysTime = xTaskGetTickCount(); // Listen for SettingPersistance object updates, connect a callback function ObjectPersistenceConnectCallback(&objectUpdatedCb); // Main system loop while (1) { // Update the system statistics updateStats(); // Update the system alarms updateSystemAlarms(); #if defined(DIAGNOSTICS) updateI2Cstats(); updateWDGstats(); #endif #if defined(DIAG_TASKS) // Update the task status object TaskMonitorUpdateAll(); #endif // Flash the heartbeat LED #if defined(PIOS_LED_HEARTBEAT) PIOS_LED_Toggle(PIOS_LED_HEARTBEAT); #endif /* PIOS_LED_HEARTBEAT */ // Turn on the error LED if an alarm is set #if defined (PIOS_LED_ALARM) if (AlarmsHasWarnings()) { PIOS_LED_On(PIOS_LED_ALARM); } else { PIOS_LED_Off(PIOS_LED_ALARM); } #endif /* PIOS_LED_ALARM */ FlightStatusData flightStatus; FlightStatusGet(&flightStatus); // Wait until next period if(flightStatus.Armed == FLIGHTSTATUS_ARMED_ARMED) { vTaskDelayUntil(&lastSysTime, SYSTEM_UPDATE_PERIOD_MS / portTICK_RATE_MS / (LED_BLINK_RATE_HZ * 2) ); } else { vTaskDelayUntil(&lastSysTime, SYSTEM_UPDATE_PERIOD_MS / portTICK_RATE_MS); } } }
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; }
/** * System task, periodically executes every SYSTEM_UPDATE_PERIOD_MS */ static void systemTask(void *parameters) { portTickType lastSysTime; uint16_t prev_tx_count = 0; uint16_t prev_rx_count = 0; bool first_time = true; /* create all modules thread */ MODULE_TASKCREATE_ALL; if (mallocFailed) { /* We failed to malloc during task creation, * system behaviour is undefined. Reset and let * the BootFault code recover for us. */ PIOS_SYS_Reset(); } // Initialize vars idleCounter = 0; idleCounterClear = 0; lastSysTime = xTaskGetTickCount(); // Main system loop while (1) { // Flash the heartbeat LED #if defined(PIOS_LED_HEARTBEAT) PIOS_LED_Toggle(PIOS_LED_HEARTBEAT); #endif /* PIOS_LED_HEARTBEAT */ // Update the PipXstatus UAVO OPLinkStatusData oplinkStatus; uint32_t pairID; OPLinkStatusGet(&oplinkStatus); OPLinkSettingsPairIDGet(&pairID); // Get the other device stats. PIOS_RFM2B_GetPairStats(pios_rfm22b_id, oplinkStatus.PairIDs, oplinkStatus.PairSignalStrengths, OPLINKSTATUS_PAIRIDS_NUMELEM); // Get the stats from the radio device struct rfm22b_stats radio_stats; PIOS_RFM22B_GetStats(pios_rfm22b_id, &radio_stats); // Update the status oplinkStatus.DeviceID = PIOS_RFM22B_DeviceID(pios_rfm22b_id); oplinkStatus.RxGood = radio_stats.rx_good; oplinkStatus.RxCorrected = radio_stats.rx_corrected; oplinkStatus.RxErrors = radio_stats.rx_error; oplinkStatus.RxMissed = radio_stats.rx_missed; oplinkStatus.RxFailure = radio_stats.rx_failure; oplinkStatus.TxDropped = radio_stats.tx_dropped; oplinkStatus.TxResent = radio_stats.tx_resent; oplinkStatus.TxFailure = radio_stats.tx_failure; oplinkStatus.Resets = radio_stats.resets; oplinkStatus.Timeouts = radio_stats.timeouts; oplinkStatus.RSSI = radio_stats.rssi; oplinkStatus.AFCCorrection = radio_stats.afc_correction; oplinkStatus.LinkQuality = radio_stats.link_quality; if (first_time) first_time = false; else { uint16_t tx_count = radio_stats.tx_byte_count; uint16_t rx_count = radio_stats.rx_byte_count; uint16_t tx_bytes = (tx_count < prev_tx_count) ? (0xffff - prev_tx_count + tx_count) : (tx_count - prev_tx_count); uint16_t rx_bytes = (rx_count < prev_rx_count) ? (0xffff - prev_rx_count + rx_count) : (rx_count - prev_rx_count); oplinkStatus.TXRate = (uint16_t)((float)(tx_bytes * 1000) / SYSTEM_UPDATE_PERIOD_MS); oplinkStatus.RXRate = (uint16_t)((float)(rx_bytes * 1000) / SYSTEM_UPDATE_PERIOD_MS); prev_tx_count = tx_count; prev_rx_count = rx_count; } oplinkStatus.TXSeq = radio_stats.tx_seq; oplinkStatus.RXSeq = radio_stats.rx_seq; oplinkStatus.LinkState = radio_stats.link_state; if (radio_stats.link_state == OPLINKSTATUS_LINKSTATE_CONNECTED) LINK_LED_ON; else LINK_LED_OFF; // Update the object OPLinkStatusSet(&oplinkStatus); // Wait until next period vTaskDelayUntil(&lastSysTime, MS2TICKS(SYSTEM_UPDATE_PERIOD_MS)); } }
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); } }
/** * System task, periodically executes every SYSTEM_UPDATE_PERIOD_MS */ static void systemTask(void *parameters) { /* create all modules thread */ MODULE_TASKCREATE_ALL; if (PIOS_heap_malloc_failed_p()) { /* We failed to malloc during task creation, * system behaviour is undefined. Reset and let * the BootFault code recover for us. */ PIOS_SYS_Reset(); } #if defined(PIOS_INCLUDE_IAP) /* Record a successful boot */ PIOS_IAP_WriteBootCount(0); #endif // Initialize vars idleCounter = 0; idleCounterClear = 0; // Listen for SettingPersistance object updates, connect a callback function ObjectPersistenceConnectQueue(objectPersistenceQueue); #if (defined(COPTERCONTROL) || defined(REVOLUTION) || defined(SIM_OSX)) && ! (defined(SIM_POSIX)) // Run this initially to make sure the configuration is checked configuration_check(); // Whenever the configuration changes, make sure it is safe to fly if (StabilizationSettingsHandle()) StabilizationSettingsConnectCallback(configurationUpdatedCb); if (SystemSettingsHandle()) SystemSettingsConnectCallback(configurationUpdatedCb); if (ManualControlSettingsHandle()) ManualControlSettingsConnectCallback(configurationUpdatedCb); if (FlightStatusHandle()) FlightStatusConnectCallback(configurationUpdatedCb); #endif #if (defined(REVOLUTION) || defined(SIM_OSX)) && ! (defined(SIM_POSIX)) if (StateEstimationHandle()) StateEstimationConnectCallback(configurationUpdatedCb); #endif // Main system loop while (1) { // Update the system statistics updateStats(); // Update the system alarms updateSystemAlarms(); #if defined(WDG_STATS_DIAGNOSTICS) updateWDGstats(); #endif #if defined(DIAG_TASKS) // Update the task status object TaskMonitorUpdateAll(); #endif // Flash the heartbeat LED #if defined(PIOS_LED_HEARTBEAT) PIOS_LED_Toggle(PIOS_LED_HEARTBEAT); DEBUG_MSG("+ 0x%08x\r\n", 0xDEADBEEF); #endif /* PIOS_LED_HEARTBEAT */ // Turn on the error LED if an alarm is set if (indicateError()) { #if defined (PIOS_LED_ALARM) PIOS_LED_On(PIOS_LED_ALARM); #endif /* PIOS_LED_ALARM */ } else { #if defined (PIOS_LED_ALARM) PIOS_LED_Off(PIOS_LED_ALARM); #endif /* PIOS_LED_ALARM */ } FlightStatusData flightStatus; FlightStatusGet(&flightStatus); UAVObjEvent ev; int delayTime = flightStatus.Armed == FLIGHTSTATUS_ARMED_ARMED ? SYSTEM_UPDATE_PERIOD_MS / (LED_BLINK_RATE_HZ * 2) : SYSTEM_UPDATE_PERIOD_MS; if (PIOS_Queue_Receive(objectPersistenceQueue, &ev, delayTime) == true) { // If object persistence is updated call the callback objectUpdatedCb(&ev); } } }
/** * System task, periodically executes every SYSTEM_UPDATE_PERIOD_MS */ static void systemTask(void *parameters) { /* create all modules thread */ MODULE_TASKCREATE_ALL; if (mallocFailed) { /* We failed to malloc during task creation, * system behaviour is undefined. Reset and let * the BootFault code recover for us. */ PIOS_SYS_Reset(); } #if defined(PIOS_INCLUDE_IAP) /* Record a successful boot */ PIOS_IAP_WriteBootCount(0); #endif // Initialize vars idleCounter = 0; idleCounterClear = 0; // Listen for SettingPersistance object updates, connect a callback function ObjectPersistenceConnectQueue(objectPersistenceQueue); // Main system loop while (1) { // Update the system statistics updateStats(); // Update the system alarms updateSystemAlarms(); #if defined(I2C_WDG_STATS_DIAGNOSTICS) updateI2Cstats(); updateWDGstats(); #endif #if defined(DIAG_TASKS) // Update the task status object TaskMonitorUpdateAll(); #endif // Flash the heartbeat LED #if defined(PIOS_LED_HEARTBEAT) PIOS_LED_Toggle(PIOS_LED_HEARTBEAT); #endif /* PIOS_LED_HEARTBEAT */ // Turn on the error LED if an alarm is set #if defined (PIOS_LED_ALARM) if (AlarmsHasWarnings()) { PIOS_LED_On(PIOS_LED_ALARM); } else { PIOS_LED_Off(PIOS_LED_ALARM); } #endif /* PIOS_LED_ALARM */ FlightStatusData flightStatus; FlightStatusGet(&flightStatus); UAVObjEvent ev; int delayTime = flightStatus.Armed == FLIGHTSTATUS_ARMED_ARMED ? SYSTEM_UPDATE_PERIOD_MS / portTICK_RATE_MS / (LED_BLINK_RATE_HZ * 2) : SYSTEM_UPDATE_PERIOD_MS / portTICK_RATE_MS; if(xQueueReceive(objectPersistenceQueue, &ev, delayTime) == pdTRUE) { // If object persistence is updated call the callback objectUpdatedCb(&ev); } } }
// // This is a low priority task that will continuously access one I2C device // Frequently it will release the I2C device so that others can also use I2C // static void Task1(void *pvParameters) { int i = 0; if (PIOS_I2C_LockDevice(MAX_LOCK_WAIT / portTICK_RATE_MS)) { if (PIOS_I2C_Transfer(I2C_Write, DEVICE_1_ADDRESS<<1, (uint8_t*)"\x20\xB0\xB1\xB2", 4) != 0) OnError(); PIOS_I2C_UnlockDevice(); } else { OnError(); } for(;;) { i++; if (i==100) { PIOS_LED_Toggle(LED1); i = 0; } DebugPinHigh(DEBUG_PIN_TASK1_WAIT); if (PIOS_I2C_LockDevice(MAX_LOCK_WAIT / portTICK_RATE_MS)) { uint8_t buf[20]; DebugPinLow(DEBUG_PIN_TASK1_WAIT); DebugPinHigh(DEBUG_PIN_TASK1_LOCKED); // Write A0 A1 A2 at address 0x10 DebugPinHigh(DEBUG_PIN_TRANSFER); if (PIOS_I2C_Transfer(I2C_Write, DEVICE_1_ADDRESS<<1, (uint8_t*)"\x10\xA0\xA1\xA2", 4) != 0) OnError(); DebugPinLow(DEBUG_PIN_TRANSFER); // Read 3 bytes at address 0x20 and check DebugPinHigh(DEBUG_PIN_TRANSFER); if (PIOS_I2C_Transfer(I2C_Write_WithoutStop, DEVICE_1_ADDRESS<<1, (uint8_t*)"\x20", 1) != 0) OnError(); DebugPinLow(DEBUG_PIN_TRANSFER); DebugPinHigh(DEBUG_PIN_TRANSFER); if (PIOS_I2C_Transfer(I2C_Read, DEVICE_1_ADDRESS<<1, buf, 3) != 0) OnError(); DebugPinLow(DEBUG_PIN_TRANSFER); if (memcmp(buf, "\xB0\xB1\xB2",3) != 0) OnError(); // Read 3 bytes at address 0x10 and check DebugPinHigh(DEBUG_PIN_TRANSFER); if (PIOS_I2C_Transfer(I2C_Write_WithoutStop, DEVICE_1_ADDRESS<<1, (uint8_t*)"\x10", 1) != 0) OnError(); DebugPinLow(DEBUG_PIN_TRANSFER); DebugPinHigh(DEBUG_PIN_TRANSFER); if (PIOS_I2C_Transfer(I2C_Read, DEVICE_1_ADDRESS<<1, buf, 3) != 0) OnError(); DebugPinLow(DEBUG_PIN_TRANSFER); if (memcmp(buf, "\xA0\xA1\xA2",3) != 0) OnError(); DebugPinLow(DEBUG_PIN_TASK1_LOCKED); PIOS_I2C_UnlockDevice(); } else { OnError(); } } }