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);
}
}
