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