void alt_filter_init(void) { SigAltiGPS = 5.; SigAltiAltimetre = 5.; MarcheAleaBiaisAltimetre = 0.1; MarcheAleaAccelerometre = 0.5; last_gps_alt = 0.; last_baro_alt = 0.; kalmanInit(&alt_filter); }
/** * @brief Initialise the Accelerometer * @param None * @retval None */ void Accelerometer_Config(void) { /* initialise accelerometer */ LIS3DSH_InitTypeDef Acc_InitDef; /* define field of the accelerometer initialisation structure */ Acc_InitDef.Power_Mode_Output_DataRate = LIS3DSH_DATARATE_25; /* 25Hz */ Acc_InitDef.Axes_Enable = LIS3DSH_XYZ_ENABLE; /* XYZ */ Acc_InitDef.Continous_Update = LIS3DSH_ContinousUpdate_Disabled; /* continuous update */ Acc_InitDef.AA_Filter_BW = LIS3DSH_AA_BW_50; /* 50Hz to filter gravity*/ Acc_InitDef.Full_Scale = LIS3DSH_FULLSCALE_2; /* 2g */ LIS3DSH_Init(&Acc_InitDef); /* Init Kalman Filter for the accelerometer */ xState = malloc(sizeof(kalmanState)); yState = malloc(sizeof(kalmanState)); zState = malloc(sizeof(kalmanState)); kalmanInit(xState, INIT_q, INIT_r, INIT_x, INIT_p, INIT_k); kalmanInit(yState, INIT_q, INIT_r, INIT_y, INIT_p, INIT_k); kalmanInit(zState, INIT_q, INIT_r, INIT_z, INIT_p, INIT_k); }
Barometer::Barometer() : Sensor(KALMAN_PROCESS_NOISE, KALMAN_MEASUREMENT_NOISE, KALMAN_ERROR) { thermometer = Thermometer(); altitude = kalmanInit(0); }
int main(void) { /* MCU Configuration----------------------------------------------------------*/ HAL_Init(); /* Reset of all peripherals, Initializes the Flash interface and the Systick. */ SystemClock_Config(); /* Configure the system clock */ ADC1_Config(); /* configure ADC1 */ Display_GPIO_Config(); /* Configure 7-Segment Displays */ Alarm_GPIO_Config(); /* Configure alarm pins */ initLCD(); /* configure LCD */ /* print temperature on the first line of the LCD */ returnHome(); /* just makes sure that start writing at the right place */ LCD_WriteString(" Temperature"); /* The 2 initial space are for centering */ adcState = malloc(sizeof(kalmanState)); /* Init Kalman Filter */ kalmanInit(adcState, INIT_q, INIT_r, INIT_x, INIT_p, INIT_k); /* main program to run in this infinite loop */ while (1) { if (adcTimer >= ADC_PERIOD) { /* 100Hz */ adcTimer = 0; HAL_ADC_Start(&ADC1_Handle); /* start ADC conversion */ /* wait for the conversion to be done and get data */ if (HAL_ADC_PollForConversion(&ADC1_Handle, 1000000) == HAL_OK) { adc_val = HAL_ADC_GetValue(&ADC1_Handle); /* get the value */ kalmanUpdate(adcState, adc_val); /* filter the data and update the filter parameters */ /* DON'T DELETE printf for matlab script */ //printf("%f,%f,%f,%f,%f,%f\n",adc_val, adcState->q,adcState->r, adcState->x, adcState->p, adcState->k); temp = convertTemp(adcState->x); /* convert the filterd value of the ADC into temperature */ /* Alarm triggering */ if (temp > THRESHHOLD_TEMP) { if ( filterAlarmCounter > 5 ){ /* 5 consecutive to avoid false positive */ trigger_alarm(); } else { filterAlarmCounter++; } } else { shutoff_alarm(); filterAlarmCounter = 0; } /* Update Measurement to Display at 2Hz */ if (updateMeasureForDisplayTimer >= UPDATE_MEASURE_PERIOD) { updateMeasureForDisplayTimer = 0; /* reset the displayTimer tick */ displayTemp = temp; displayTemp = floor(10 * displayTemp) / 10; /* truncate to 1 decimal without rounding */ /* LCD DISPLAY */ SetAdress(64); /* go to line 2 of LCD */ sprintf(tempToLCD, " %.1f", displayTemp); /* convert the float to a string and formats it */ LCD_WriteString(tempToLCD); /* print value to the LCD display */ /* LCD DISPLAY END */ } } } /* here display runs at DISPLAY_7_SEGMENT_PERIOD speed, but displayTemp gets updated at 2Hz */ if(display7segTimer >= DISPLAY_7_SEGMENT_PERIOD) { display7segTimer = 0; display(displayTemp); /* display on 7-segment display */ } } }
Accelerometer::Accelerometer() : Sensor(KALMAN_PROCESS_NOISE, KALMAN_MEASUREMENT_NOISE, KALMAN_ERROR) { roll = kalmanInit(0); pitch = kalmanInit(90); heading = kalmanInit(0); acceleration = kalmanInit(1); }
int main(int argc, char *argv[]) { int done=0; double gpsUpdateRate, imuUpdateRate, eulerUpdateRate; long int gpsUpdateCount, imuUpdateCount, eulerUpdateCount, kalmanUpdateCount; double lastTime; // Program begins with the usual logging of data and so on... captureQuitSignal(); shared = (Q_SHARED * )attach_memory(QBOATShareKey, sizeof(Q_SHARED)); sharedDIAG = (DIAG_SHARED *)attach_memory(DIAGShareKey, sizeof(DIAG_SHARED)); createDirectory(); sleep(1); openKalmanlogfile(); fprintf(stderr,"\nExtended Kalman Filter v 1.0 for the Q-boat (based on the Heli INS by Srik)"); initEverything(); lastTime = get_time(); // Now begin main loop... while (!done) { // Run a loop to check if we've received new data from the sensors... if((get_time()-lastTime)>=1.0) { fprintf(stderr,"\nGPS %dHz, IMU %d Hz, EUL %d Hz, KAL %dHz, LastTime %f",gpsUpdateCount, imuUpdateCount, eulerUpdateCount,kalmanUpdateCount,lastTime); gpsUpdateCount = 0; imuUpdateCount = 0; eulerUpdateCount = 0; kalmanUpdateCount = 0; lastTime = get_time(); } if(shared->SensorData.lastEulerKalmanTime<shared->SensorData.eulerUpdateTime) { ++eulerUpdateCount; if(!init_compass) compassInit(); else updateCompass(); } #ifdef __USE3DMG_DATA__ if(shared->SensorData.lastImuKalmanTime<shared->SensorData.imuUpdateTime) { if(!init_imu) imuInit(); else updateImu(); ++imuUpdateCount; } #endif #ifdef __USEXBOW_DATA__ if(shared->SensorData.lastImuKalmanTime<sharedDIAG->xbowData.lastXbowTime) { if(!init_imu) imuInit(); else updateImu(); ++imuUpdateCount; } #endif if(shared->SensorData.lastGpsKalmanTime<shared->SensorData.gpsUpdateTime) { if(!init_gps) gpsInit(); else updateGPS(); ++gpsUpdateCount; } if(!init_kf) { if(init_gps && init_compass && init_imu) kalmanInit(); } // Update the State... if(init_kf && (get_time() - shared->kalmanData.lastKalmanUpdateTime)>=0.01) { updateState(); ++kalmanUpdateCount; } usleep(50); } }