/** * prediction - perform a state prediction step * @params[in] self * @params[in] args * - gyro * - accel * - dT * @return state */ static PyObject* prediction(PyObject* self, PyObject* args) { PyArrayObject *vec_gyro, *vec_accel; float gyro_data[3], accel_data[3]; float dT; if (!PyArg_ParseTuple(args, "O!O!f", &PyArray_Type, &vec_gyro, &PyArray_Type, &vec_accel, &dT)) return NULL; if (NULL == vec_gyro) return NULL; if (NULL == vec_accel) return NULL; if (!parseFloatVec3(vec_gyro, gyro_data)) return NULL; if (!parseFloatVec3(vec_accel, accel_data)) return NULL; INSStatePrediction(gyro_data, accel_data, dT); INSCovariancePrediction(dT); if (false) { const float zeros[3] = {0,0,0}; INSSetGyroBias(zeros); INSSetAccelBias(zeros); } return pack_state(self); }
/** * @brief Quickly initialize INS assuming stationary and gravity is down * * Currently this is done iteratively but I'm sure it can be directly computed * when I sit down and work it out */ void converge_insgps() { AHRSCalibrationData calibration; AHRSCalibrationGet( &calibration ); float pos[3] = { 0, 0, 0 }, vel[3] = { 0, 0, 0 }, BaroAlt = 0, mag[3], accel[3], temp_gyro[3] = { 0, 0, 0 }; INSGPSInit(); INSSetAccelVar( calibration.accel_var ); INSSetGyroBias( temp_gyro ); // set this to zero - crude bias corrected from downsample_data INSSetGyroVar( calibration.gyro_var ); INSSetMagVar( calibration.mag_var ); float temp_var[3] = { 10, 10, 10 }; INSSetGyroVar( temp_var ); // ignore gyro's accel[0] = accel_data.filtered.x; accel[1] = accel_data.filtered.y; accel[2] = accel_data.filtered.z; // Iteratively constrain pitch and roll while updating yaw to align magnetic axis. for( int i = 0; i < 50; i++ ) { // This should be done directly but I'm too dumb. float rpy[3]; Quaternion2RPY( Nav.q, rpy ); 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; // Get magnetic readings #if defined(PIOS_INCLUDE_HMC5843) && defined(PIOS_INCLUDE_I2C) PIOS_HMC5843_ReadMag( mag_data.raw.axis ); #endif mag[0] = -mag_data.raw.axis[1]; mag[1] = -mag_data.raw.axis[0]; mag[2] = -mag_data.raw.axis[2]; RPY2Quaternion( rpy, Nav.q ); INSStatePrediction( temp_gyro, accel, 1 / ( float )EKF_RATE ); INSCovariancePrediction( 1 / ( float )EKF_RATE ); FullCorrection( mag, pos, vel, BaroAlt ); } INSSetGyroVar( calibration.gyro_var ); }
void mexFunction(int nlhs, mxArray *plhs[], int nrhs, const mxArray *prhs[]) { char * function_name; float accel_data[3]; float gyro_data[3]; float mag_data[3]; float pos_data[3]; float vel_data[3]; float baro_data; float dT; //All code and internal function calls go in here! if(!mxIsChar(prhs[0])) { mexErrMsgTxt("First parameter must be name of a function\n"); return; } if(mlStringCompare(prhs[0], "INSGPSInit")) { INSGPSInit(); } else if(mlStringCompare(prhs[0], "INSStatePrediction")) { if(nrhs != 4) { mexErrMsgTxt("Incorrect number of inputs for state prediction\n"); return; } if(!mlGetFloatArray(prhs[1], gyro_data, 3) || !mlGetFloatArray(prhs[2], accel_data, 3) || !mlGetFloatArray(prhs[3], &dT, 1)) return; INSStatePrediction(gyro_data, accel_data, dT); INSCovariancePrediction(dT); } else if(mlStringCompare(prhs[0], "INSFullCorrection")) { if(nrhs != 5) { mexErrMsgTxt("Incorrect number of inputs for correction\n"); return; } if(!mlGetFloatArray(prhs[1], mag_data, 3) || !mlGetFloatArray(prhs[2], pos_data, 3) || !mlGetFloatArray(prhs[3], vel_data ,3) || !mlGetFloatArray(prhs[4], &baro_data, 1)) { mexErrMsgTxt("Error with the input parameters\n"); return; } FullCorrection(mag_data, pos_data, vel_data, baro_data); } else if(mlStringCompare(prhs[0], "INSMagCorrection")) { if(nrhs != 2) { mexErrMsgTxt("Incorrect number of inputs for correction\n"); return; } if(!mlGetFloatArray(prhs[1], mag_data, 3)) { mexErrMsgTxt("Error with the input parameters\n"); return; } MagCorrection(mag_data); } else if(mlStringCompare(prhs[0], "INSBaroCorrection")) { if(nrhs != 2) { mexErrMsgTxt("Incorrect number of inputs for correction\n"); return; } if(!mlGetFloatArray(prhs[1], &baro_data, 1)) { mexErrMsgTxt("Error with the input parameters\n"); return; } BaroCorrection(baro_data); } else if(mlStringCompare(prhs[0], "INSMagVelBaroCorrection")) { if(nrhs != 4) { mexErrMsgTxt("Incorrect number of inputs for correction\n"); return; } if(!mlGetFloatArray(prhs[1], mag_data, 3) || !mlGetFloatArray(prhs[2], vel_data ,3) || !mlGetFloatArray(prhs[3], &baro_data, 1)) { mexErrMsgTxt("Error with the input parameters\n"); return; } MagVelBaroCorrection(mag_data, vel_data, baro_data); } else if(mlStringCompare(prhs[0], "INSGpsCorrection")) { if(nrhs != 3) { mexErrMsgTxt("Incorrect number of inputs for correction\n"); return; } if(!mlGetFloatArray(prhs[1], pos_data, 3) || !mlGetFloatArray(prhs[2], vel_data ,3)) { mexErrMsgTxt("Error with the input parameters\n"); return; } GpsCorrection(pos_data, vel_data); } else if(mlStringCompare(prhs[0], "INSVelBaroCorrection")) { if(nrhs != 3) { mexErrMsgTxt("Incorrect number of inputs for correction\n"); return; } if(!mlGetFloatArray(prhs[1], vel_data, 3) || !mlGetFloatArray(prhs[2], &baro_data, 1)) { mexErrMsgTxt("Error with the input parameters\n"); return; } VelBaroCorrection(vel_data, baro_data); } else if (mlStringCompare(prhs[0], "INSSetPosVelVar")) { float pos_var; float vel_var; if((nrhs != 3) || !mlGetFloatArray(prhs[1], &pos_var, 1) || !mlGetFloatArray(prhs[2], &vel_var, 1)) { mexErrMsgTxt("Error with input parameters\n"); return; } INSSetPosVelVar(pos_var, vel_var); } else if (mlStringCompare(prhs[0], "INSSetGyroBias")) { float gyro_bias[3]; if((nrhs != 2) || !mlGetFloatArray(prhs[1], gyro_bias, 3)) { mexErrMsgTxt("Error with input parameters\n"); return; } INSSetGyroBias(gyro_bias); } else if (mlStringCompare(prhs[0], "INSSetAccelVar")) { float accel_var[3]; if((nrhs != 2) || !mlGetFloatArray(prhs[1], accel_var, 3)) { mexErrMsgTxt("Error with input parameters\n"); return; } INSSetAccelVar(accel_var); } else if (mlStringCompare(prhs[0], "INSSetGyroVar")) { float gyro_var[3]; if((nrhs != 2) || !mlGetFloatArray(prhs[1], gyro_var, 3)) { mexErrMsgTxt("Error with input parameters\n"); return; } INSSetGyroVar(gyro_var); } else if (mlStringCompare(prhs[0], "INSSetMagNorth")) { float mag_north[3]; float Bmag; if((nrhs != 2) || !mlGetFloatArray(prhs[1], mag_north, 3)) { mexErrMsgTxt("Error with input parameters\n"); return; } Bmag = sqrt(mag_north[0] * mag_north[0] + mag_north[1] * mag_north[1] + mag_north[2] * mag_north[2]); mag_north[0] = mag_north[0] / Bmag; mag_north[1] = mag_north[1] / Bmag; mag_north[2] = mag_north[2] / Bmag; INSSetMagNorth(mag_north); } else if (mlStringCompare(prhs[0], "INSSetMagVar")) { float mag_var[3]; if((nrhs != 2) || !mlGetFloatArray(prhs[1], mag_var, 3)) { mexErrMsgTxt("Error with input parameters\n"); return; } INSSetMagVar(mag_var); } else if (mlStringCompare(prhs[0], "INSSetState")) { int i; float new_state[NUMX]; if((nrhs != 2) || !mlGetFloatArray(prhs[1], new_state, NUMX)) { mexErrMsgTxt("Error with input parameters\n"); return; } for(i = 0; i < NUMX; i++) X[i] = new_state[i]; } else { mexErrMsgTxt("Unknown function"); } if(nlhs > 0) { // return current state vector double * data_out; int i; plhs[0] = mxCreateDoubleMatrix(1,13,0); data_out = mxGetData(plhs[0]); for(i = 0; i < NUMX; i++) data_out[i] = X[i]; } if(nlhs > 1) { //return covariance estimate double * data_copy = mxCalloc(NUMX*NUMX, sizeof(double)); int i, j, k; plhs[1] = mxCreateDoubleMatrix(13,13,0); for(i = 0; i < NUMX; i++) for(j = 0; j < NUMX; j++) { data_copy[j + i * NUMX] = P[j][i]; } mxSetData(plhs[1], data_copy); } if(nlhs > 2) { //return covariance estimate double * data_copy = mxCalloc(NUMX*NUMV, sizeof(double)); int i, j, k; plhs[2] = mxCreateDoubleMatrix(NUMX,NUMV,0); for(i = 0; i < NUMX; i++) for(j = 0; j < NUMV; j++) { data_copy[j + i * NUMX] = K[i][j]; } mxSetData(plhs[2], data_copy); } return; }
/** * @brief This function runs the EKF, talks to aux sensors with a state machine and applys control loops to servos (main control task loop) * @param None * @retval None */ void run_imu(void) { //Static variables static uint32_t Iterations=0,millis_pitot;//Number of ekf iterations, pitot sample timestamp static Control_type control=DEFAULT_CONTROL;//The control structure static Ubx_Gps_Type gps; //This is our local copy - theres is also a global, be careful with copying static float ac[3],Wind[3]; //The accel is not always avaliable - 100hz update static float AirSpeed=0,Baro_Alt,Body_x_To_x=0,Body_x_To_y=0,Mean_Baro_Offset; static uint32_t Baro_Pressure; //Baro pressure is static for use in air density calculations static uint8_t PWM_Disabled; //Keeps track of PWM hardware passthrough state //Non Static float ma[3],gy[3],gps_velocity[3],gps_position[3],target_vector[3],waypoint[3]={0,0,0},old_density; float Delta_Time=DELTA_TIME,x_down,y_down,h_offset,N_t_x,N_t_y,time_to_waypoint,Horiz_t,GPS_Errors[4]; uint16_t SensorsUsed=0; //We by default have no sensors int32_t Baro_Temperature; //In units of 0.1C //Now read the gyro buffer, convert to float from uint16_t and apply the calibration Calibrate_3(gy,&(Gyro_Data_Buffer[1]),&Gyr_Cal_Dat);//Read gyro data buffer - skip the temperature //Now read the accel downconvertor buffer and apply calibration Accel_Access_Flag=LOCKED; //Lock the data /*LOCKED*/Calibrate_3(ac,Accel_Data_Vector,&Acc_Cal_Dat);//Grab the data from the accel downsampler/DSP filter Accel_Access_Flag=UNLOCKED; //Unlock the data //Run the EKF - we feed it float pointers but it expects float arrays - alignment has to be correct INSStatePrediction(gy,ac,Delta_Time); //Run the EKF and incorporate the avaliable sensors INSCovariancePrediction(Delta_Time); //Process the GPS data while(Bytes_In_Buffer(&Gps_Buffer)) //Dump all the data from DMA Gps_Process_Byte((uint8_t)(Pop_From_Buffer(&Gps_Buffer)),&gps); if(gps.packetflag==REQUIRED_DATA){ gps_position[0]=(float)(gps.latitude-Home_Position.Latitude)*LAT_TO_METERS;//Remember, NED frame, and gps altitude needs *=-1 gps_position[1]=(float)(gps.longitude-Home_Position.Longitude)*Long_To_Meters_Home;//This is a global set with home position gps_position[2]=-((float)gps.mslaltitude*0.001)+Home_Position.Altitude;//Home is in raw gps coordinates - apart from altitude in m NED gps_velocity[0]=(float)gps.vnorth*0.01;//Ublox velocity is in cm/s gps_velocity[1]=(float)gps.veast*0.01; gps_velocity[2]=(float)gps.vdown*0.01; GPS_Errors[0]=(float)gps.horizontal_error*(float)gps.horizontal_error*1e-6*GPS_SPECTRAL_FUDGE_FACTOR/2.0;//Fudge factor for spectral noise density GPS_Errors[1]=(float)gps.vertical_error*(float)gps.vertical_error*1e-6*GPS_SPECTRAL_FUDGE_FACTOR;//Fudge factor is defined in ubx.h/gps header file GPS_Errors[2]=(float)gps.speedacc*(float)gps.speedacc*1e-4*GPS_SPECTRAL_FUDGE_FACTOR*GPS_Errors[0]/(GPS_Errors[0]+GPS_Errors[1]); GPS_Errors[3]=GPS_Errors[2]*GPS_Errors[1]/GPS_Errors[0];//Ublox5 only gives us 3D speed accuracy? Weight with position errors if(OUTDOOR==Operating_Mode) INSResetRGPS(GPS_Errors);//Adjust the measurement covariance matrix with reported gps error SensorsUsed|=POS_SENSORS|HORIZ_SENSORS|VERT_SENSORS;//Set the flagbits for the gps update //Correct baro pressure offset - average the gps altitude over first 100 seconds and apply correction when filter initialised old_density=((((float)gps.mslaltitude*0.001-Home_Position.Altitude)*Air_Density*Home_Position.g_e)+(float)Baro_Pressure-Baro_Offset\ -Mean_Baro_Offset)*(0.01/(float)GPS_RATE);//reuse variable here if(Iterations++>100*GPS_RATE) Baro_Offset+=old_density;//fixed tau: 0.01s^-1, use the averaged offset to take out gps altitude error in the home pos else Mean_Baro_Offset+=old_density; if(!Gps.packetflag)Gps=gps; //Copy the data over to the main 'thread' if the global unlocked gps.packetflag=0x00; //Reset the flag } //Now the state dependant I2C stuff if(Completed_Jobs&(1<<MAGNO_READ)) { //This I2C job will run whilst the prediction runs Completed_Jobs&=~(1<<MAGNO_READ);//Wipe the job completed bit Calibrate_3(ma,Magno_Data_Buffer,&Mag_Cal_Dat);//Calibrate - the EKF can take a magnetometer input in any units SensorsUsed|=MAG_SENSORS; //Let the EKF know what we used } if(Completed_Jobs&(1<<BMP_24BIT)) { Completed_Jobs&=~(1<<BMP_24BIT);//Wipe the job completed bit Baro_Pressure=Bmp_Press_Buffer; //Copy over from the read buffer flip_adc24(&Baro_Pressure); //Fix endianess Bmp_Simp_Conv(&Baro_Temperature,&Baro_Pressure);//Convert to a pressure in Pa Baro_Alt=(Baro_Offset-(float)Baro_Pressure)/(Home_Position.g_e*Air_Density);//Use the air density calculation (also used in pitot), linear approx SensorsUsed|=BARO_SENSOR; //We have used the baro sensor Balt=Baro_Alt; if(READ==UAVtalk_conf.semaphores[BARO_ACTUAL]) {//If this data has been read, we can update it (avoids risk of overwriting data mid packet tx) UAVtalk_Altitude_Array[0]=Baro_Alt;//+Home_Position.Altitude;//Populate Baro_altitude UAVtalk packet here - Altitude is MSL altitude in m UAVtalk_Altitude_Array[1]=(float)Baro_Temperature/10.0;//Note that as baro_actual has three independant 32bit UAVtalk_Altitude_Array[2]=(float)Baro_Pressure*1e-3;//Variables, can be set directly here without passing data back - note pressure in kPa UAVtalk_conf.semaphores[BARO_ACTUAL]=WRITE;//Set the semaphore to indicate this has been written } } if(Completed_Jobs&(1<<BMP_16BIT)) { Completed_Jobs&=~(1<<BMP_16BIT);//Wipe the job completed bit Bmp_Copy_Temp(); //Copy the 16 bit temperature out of its buffer into the temperature global old_density=Air_Density; //Save old air density so we can find the delta Air_Density=Calc_Air_Density((float)gps.mslaltitude*1e-3,(float)Baro_Pressure);//Find air density using atmospheric model (Kgm^-3) Baro_Offset+=Baro_Alt*(Air_Density-old_density)*Home_Position.g_e;//Correct the baro offset term to account for changing density } if(Completed_Jobs&(1<<PITOT_READ)) { Completed_Jobs&=~(1<<PITOT_READ);//Wipe the job completed bit if((Millis-millis_pitot)<2*PITOT_PERIOD) {//An uncorrupted pitot data sample - the bmp085 has same address as ltc2481 global config Pitot_Pressure=Pitot_Conv((uint32_t)Pitot_Pressure);//Align and sign the adc value - 1lsb=~0.24Pa AirSpeed=Pitot_convert_Airspeed(Pitot_Pressure, Air_Density);//Windspeed Wind[0]*=WIND_TAU;Wind[1]*=WIND_TAU;//Low pass filter Wind[0]+=(1.0-WIND_TAU)*(Nav.Vel[0]-AirSpeed*Body_x_To_x);//This assumes horizontal wind and neglidgible slip Wind[1]+=(1.0-WIND_TAU)*(Nav.Vel[1]-AirSpeed*Body_x_To_y); Balt=(float)AirSpeed; //Pitot_Pressure;//Note debug } millis_pitot=Millis; //Save a timestamp so we can monitor conversion time, BMP085 corruption will double this } INSCorrection(ma,gps_position,gps_velocity,-Baro_Alt,SensorsUsed);//Note that Baro has to be in NED frame if(!Nav_Flag) {Nav_Global=Nav; Nav_Flag=(uint32_t)0x01;}//Copy over Nav state if its been unlocked //EKF is finished, time to run the guidance if(New_Waypoint_Flagged) {memcpy(waypoint,Waypoint_Global,sizeof(waypoint)); New_Waypoint_Flagged=0;}//Check for any new waypoints set target_vector[0]=waypoint[0]-Nav.Pos[0];//A float vector to the waypoint in NED space target_vector[1]=waypoint[1]-Nav.Pos[1]; target_vector[2]=waypoint[2]-Nav.Pos[2];//Now work out the eta at the waypoint (just the x,y/North,East position) Horiz_t=sqrtf(target_vector[0]*target_vector[0]+target_vector[1]*target_vector[1]);//Horizontal distance to target N_t_x=target_vector[0]/Horiz_t; N_t_y=target_vector[1]/Horiz_t; //Normalised horizontal target vector time_to_waypoint=Horiz_t/(control.airframe.airspeed+Wind[0]*N_t_x+Wind[1]*N_t_y);//Time if we travel in a straight line - ignores vertical offset N_t_x=target_vector[0]-Wind[0]*time_to_waypoint; N_t_y=target_vector[1]-Wind[1]*time_to_waypoint;//Modify the aiming point to account for cross track error AirSpeed=Nav.Vel[0]-Wind[0];AirSpeed*=AirSpeed;//Do this here to decrease runtime a little AirSpeed=sqrtf(AirSpeed+(Nav.Vel[1]-Wind[1])*(Nav.Vel[1]-Wind[1])+Nav.Vel[2]*Nav.Vel[2]);//Work out true airspeed assume horizontal wind //Move the body x,y axes into earth NED frame using Reb, looking at z=down components of body x and y x_down=2 * (Nav.q[1] * Nav.q[3] - Nav.q[0] * Nav.q[2]); y_down=2 * (Nav.q[2] * Nav.q[3] + Nav.q[0] * Nav.q[1]); //Work out the heading offset - z component of target vector cross body x axis Body_x_To_x=Nav.q[0] * Nav.q[0] + Nav.q[1] * Nav.q[1] - Nav.q[2] * Nav.q[2] - Nav.q[3] * Nav.q[3]; Body_x_To_y=2*Nav.q[1] * Nav.q[2] + Nav.q[0] * Nav.q[3];//These are saved for subsequent iterations as static variables Horiz_t=sqrtf(N_t_x*N_t_x+N_t_y*N_t_y); //Normalise the wind corrected target vector N_t_x/Horiz_t; N_t_y/Horiz_t; h_offset=Body_x_To_x * N_t_x + Body_x_To_y * N_t_y -1;//We do dot product to get cos(angle), then subtract 1 if(Body_x_To_x * N_t_y - Body_x_To_y * N_t_x<0)//Multiply by -1 depending on the direction - cos(-angle)==cos(angle) h_offset*=-1; //Run the control loops if we arent controlled from the ground if(Ground_Flag&0x0F) {//TODO implement ground control using PWM input capture and sanity check, as well as PWM passthrough? if(PWM_Disabled) {//If PWM isnt already enabled, enable it Timer_GPIO_Enable(); PWM_Disabled=0; } //Pitch setpoint control pid (actually a PI) -- Note- uses dynamic pressure Run_PID(&(control.pitch_setpoint),&(control.airframe.pitch_setpoint),control.airframe.airspeed-AirSpeed,0); //Roll setpoint set by heading error Run_PID(&(control.roll_setpoint),&(control.airframe.roll_setpoint),h_offset,0); if(Ground_Flag&0xF0) {//If we are Armed, the motor can be run - defaults to disarmed //Throttle set according to altitude error Run_PID(&(control.throttle),&(control.airframe.throttle),\ target_vector[2]-((AirSpeed*AirSpeed)-(control.airframe.airspeed*control.airframe.airspeed))/(2*Home_Position.g_e),Nav.Vel[2]); control.throttle.out+=control.airframe.throttle_optimal; } else control.throttle.out=-1.0;//Apply zero throttle to stop motor spinning //Elevator, remember x_down is reversed sign Run_PID(&(control.elevator),&(control.airframe.elevator),control.pitch_setpoint.out+x_down,gy[1]-Nav.gyro_bias[1]); //Ailerons, TODO - work out if signs sane Run_PID(&(control.ailerons),&(control.airframe.ailerons),control.roll_setpoint.out-y_down,gy[0]-Nav.gyro_bias[0]); //Rudder, D term takes out turbulence, and I term for roll-bank (no P?) Run_PID(&(control.rudder),&(control.airframe.rudder),ac[1],gy[2]-Nav.gyro_bias[2]); //Apply the feedforward, linking rudder to roll offset control.rudder.out+=control.airframe.rudder_feedforward*control.roll_setpoint.out; Apply_Servos(&control);//Servo driver function called here using pwm } else {//any control code to run whilst in ground mode goes here if(!PWM_Disabled) {//If PWM was previously enabled, disable it Timer_GPIO_Enable(); PWM_Disabled=1; } } }
/** * @brief Use the INSGPS fusion algorithm in either indoor or outdoor mode (use GPS) * @params[in] first_run This is the first run so trigger reinitialization * @params[in] outdoor_mode If true use the GPS for position, if false weakly pull to (0,0) * @return 0 for success, -1 for failure */ static int32_t updateAttitudeINSGPS(bool first_run, bool outdoor_mode) { UAVObjEvent ev; GyrosData gyrosData; AccelsData accelsData; MagnetometerData magData; BaroAltitudeData baroData; GPSPositionData gpsData; GPSVelocityData gpsVelData; GyrosBiasData gyrosBias; static bool mag_updated = false; static bool baro_updated; static bool gps_updated; static bool gps_vel_updated; static float baroOffset = 0; static uint32_t ins_last_time = 0; static bool inited; float NED[3] = {0.0f, 0.0f, 0.0f}; float vel[3] = {0.0f, 0.0f, 0.0f}; float zeros[3] = {0.0f, 0.0f, 0.0f}; // Perform the update uint16_t sensors = 0; float dT; // Wait until the gyro and accel object is updated, if a timeout then go to failsafe if ( (xQueueReceive(gyroQueue, &ev, FAILSAFE_TIMEOUT_MS / portTICK_RATE_MS) != pdTRUE) || (xQueueReceive(accelQueue, &ev, 1 / portTICK_RATE_MS) != pdTRUE) ) { // Do not set attitude timeout warnings in simulation mode if (!AttitudeActualReadOnly()){ AlarmsSet(SYSTEMALARMS_ALARM_ATTITUDE,SYSTEMALARMS_ALARM_WARNING); return -1; } } if (inited) { mag_updated = 0; baro_updated = 0; gps_updated = 0; gps_vel_updated = 0; } if (first_run) { inited = false; init_stage = 0; mag_updated = 0; baro_updated = 0; gps_updated = 0; gps_vel_updated = 0; ins_last_time = PIOS_DELAY_GetRaw(); return 0; } mag_updated |= (xQueueReceive(magQueue, &ev, 0 / portTICK_RATE_MS) == pdTRUE); baro_updated |= xQueueReceive(baroQueue, &ev, 0 / portTICK_RATE_MS) == pdTRUE; gps_updated |= (xQueueReceive(gpsQueue, &ev, 0 / portTICK_RATE_MS) == pdTRUE) && outdoor_mode; gps_vel_updated |= (xQueueReceive(gpsVelQueue, &ev, 0 / portTICK_RATE_MS) == pdTRUE) && outdoor_mode; // Get most recent data GyrosGet(&gyrosData); AccelsGet(&accelsData); MagnetometerGet(&magData); BaroAltitudeGet(&baroData); GPSPositionGet(&gpsData); GPSVelocityGet(&gpsVelData); GyrosBiasGet(&gyrosBias); // Discard mag if it has NAN (normally from bad calibration) mag_updated &= (magData.x == magData.x && magData.y == magData.y && magData.z == magData.z); // Don't require HomeLocation.Set to be true but at least require a mag configuration (allows easily // switching between indoor and outdoor mode with Set = false) mag_updated &= (homeLocation.Be[0] != 0 || homeLocation.Be[1] != 0 || homeLocation.Be[2]); // Have a minimum requirement for gps usage gps_updated &= (gpsData.Satellites >= 7) && (gpsData.PDOP <= 4.0f) && (homeLocation.Set == HOMELOCATION_SET_TRUE); if (!inited) AlarmsSet(SYSTEMALARMS_ALARM_ATTITUDE,SYSTEMALARMS_ALARM_ERROR); else if (outdoor_mode && gpsData.Satellites < 7) AlarmsSet(SYSTEMALARMS_ALARM_ATTITUDE,SYSTEMALARMS_ALARM_ERROR); else AlarmsClear(SYSTEMALARMS_ALARM_ATTITUDE); if (!inited && mag_updated && baro_updated && (gps_updated || !outdoor_mode)) { // Don't initialize until all sensors are read if (init_stage == 0 && !outdoor_mode) { float Pdiag[16]={25.0f,25.0f,25.0f,5.0f,5.0f,5.0f,1e-5f,1e-5f,1e-5f,1e-5f,1e-5f,1e-5f,1e-5f,1e-4f,1e-4f,1e-4f}; float q[4]; float pos[3] = {0.0f, 0.0f, 0.0f}; // Initialize barometric offset to homelocation altitude baroOffset = -baroData.Altitude; pos[2] = -(baroData.Altitude + baroOffset); // Reset the INS algorithm INSGPSInit(); INSSetMagVar(revoCalibration.mag_var); INSSetAccelVar(revoCalibration.accel_var); INSSetGyroVar(revoCalibration.gyro_var); INSSetBaroVar(revoCalibration.baro_var); // Initialize the gyro bias from the settings float gyro_bias[3] = {gyrosBias.x * F_PI / 180.0f, gyrosBias.y * F_PI / 180.0f, gyrosBias.z * F_PI / 180.0f}; INSSetGyroBias(gyro_bias); xQueueReceive(magQueue, &ev, 100 / portTICK_RATE_MS); MagnetometerGet(&magData); // Set initial attitude AttitudeActualData attitudeActual; attitudeActual.Roll = atan2f(-accelsData.y, -accelsData.z) * 180.0f / F_PI; attitudeActual.Pitch = atan2f(accelsData.x, -accelsData.z) * 180.0f / F_PI; attitudeActual.Yaw = atan2f(-magData.y, magData.x) * 180.0f / F_PI; RPY2Quaternion(&attitudeActual.Roll,&attitudeActual.q1); AttitudeActualSet(&attitudeActual); q[0] = attitudeActual.q1; q[1] = attitudeActual.q2; q[2] = attitudeActual.q3; q[3] = attitudeActual.q4; INSSetState(pos, zeros, q, zeros, zeros); INSResetP(Pdiag); } else if (init_stage == 0 && outdoor_mode) { float Pdiag[16]={25.0f,25.0f,25.0f,5.0f,5.0f,5.0f,1e-5f,1e-5f,1e-5f,1e-5f,1e-5f,1e-5f,1e-5f,1e-4f,1e-4f,1e-4f}; float q[4]; float NED[3]; // Reset the INS algorithm INSGPSInit(); INSSetMagVar(revoCalibration.mag_var); INSSetAccelVar(revoCalibration.accel_var); INSSetGyroVar(revoCalibration.gyro_var); INSSetBaroVar(revoCalibration.baro_var); INSSetMagNorth(homeLocation.Be); // Initialize the gyro bias from the settings float gyro_bias[3] = {gyrosBias.x * F_PI / 180.0f, gyrosBias.y * F_PI / 180.0f, gyrosBias.z * F_PI / 180.0f}; INSSetGyroBias(gyro_bias); GPSPositionData gpsPosition; GPSPositionGet(&gpsPosition); // Transform the GPS position into NED coordinates getNED(&gpsPosition, NED); // Initialize barometric offset to cirrent GPS NED coordinate baroOffset = -NED[2] - baroData.Altitude; xQueueReceive(magQueue, &ev, 100 / portTICK_RATE_MS); MagnetometerGet(&magData); // Set initial attitude AttitudeActualData attitudeActual; attitudeActual.Roll = atan2f(-accelsData.y, -accelsData.z) * 180.0f / F_PI; attitudeActual.Pitch = atan2f(accelsData.x, -accelsData.z) * 180.0f / F_PI; attitudeActual.Yaw = atan2f(-magData.y, magData.x) * 180.0f / F_PI; RPY2Quaternion(&attitudeActual.Roll,&attitudeActual.q1); AttitudeActualSet(&attitudeActual); q[0] = attitudeActual.q1; q[1] = attitudeActual.q2; q[2] = attitudeActual.q3; q[3] = attitudeActual.q4; INSSetState(NED, zeros, q, zeros, zeros); INSResetP(Pdiag); } else if (init_stage > 0) { // Run prediction a bit before any corrections dT = PIOS_DELAY_DiffuS(ins_last_time) / 1.0e6f; GyrosBiasGet(&gyrosBias); float gyros[3] = {(gyrosData.x + gyrosBias.x) * F_PI / 180.0f, (gyrosData.y + gyrosBias.y) * F_PI / 180.0f, (gyrosData.z + gyrosBias.z) * F_PI / 180.0f}; INSStatePrediction(gyros, &accelsData.x, dT); AttitudeActualData attitude; AttitudeActualGet(&attitude); attitude.q1 = Nav.q[0]; attitude.q2 = Nav.q[1]; attitude.q3 = Nav.q[2]; attitude.q4 = Nav.q[3]; Quaternion2RPY(&attitude.q1,&attitude.Roll); AttitudeActualSet(&attitude); } init_stage++; if(init_stage > 10) inited = true; ins_last_time = PIOS_DELAY_GetRaw(); return 0; } if (!inited) return 0; dT = PIOS_DELAY_DiffuS(ins_last_time) / 1.0e6f; ins_last_time = PIOS_DELAY_GetRaw(); // This should only happen at start up or at mode switches if(dT > 0.01f) dT = 0.01f; else if(dT <= 0.001f) dT = 0.001f; // If the gyro bias setting was updated we should reset // the state estimate of the EKF if(gyroBiasSettingsUpdated) { float gyro_bias[3] = {gyrosBias.x * F_PI / 180.0f, gyrosBias.y * F_PI / 180.0f, gyrosBias.z * F_PI / 180.0f}; INSSetGyroBias(gyro_bias); gyroBiasSettingsUpdated = false; } // Because the sensor module remove the bias we need to add it // back in here so that the INS algorithm can track it correctly float gyros[3] = {gyrosData.x * F_PI / 180.0f, gyrosData.y * F_PI / 180.0f, gyrosData.z * F_PI / 180.0f}; if (revoCalibration.BiasCorrectedRaw == REVOCALIBRATION_BIASCORRECTEDRAW_TRUE) { gyros[0] += gyrosBias.x * F_PI / 180.0f; gyros[1] += gyrosBias.y * F_PI / 180.0f; gyros[2] += gyrosBias.z * F_PI / 180.0f; } // Advance the state estimate INSStatePrediction(gyros, &accelsData.x, dT); // Copy the attitude into the UAVO AttitudeActualData attitude; AttitudeActualGet(&attitude); attitude.q1 = Nav.q[0]; attitude.q2 = Nav.q[1]; attitude.q3 = Nav.q[2]; attitude.q4 = Nav.q[3]; Quaternion2RPY(&attitude.q1,&attitude.Roll); AttitudeActualSet(&attitude); // Advance the covariance estimate INSCovariancePrediction(dT); if(mag_updated) sensors |= MAG_SENSORS; if(baro_updated) sensors |= BARO_SENSOR; INSSetMagNorth(homeLocation.Be); if (gps_updated && outdoor_mode) { INSSetPosVelVar(revoCalibration.gps_var[REVOCALIBRATION_GPS_VAR_POS], revoCalibration.gps_var[REVOCALIBRATION_GPS_VAR_VEL]); sensors |= POS_SENSORS; if (0) { // Old code to take horizontal velocity from GPS Position update sensors |= HORIZ_SENSORS; vel[0] = gpsData.Groundspeed * cosf(gpsData.Heading * F_PI / 180.0f); vel[1] = gpsData.Groundspeed * sinf(gpsData.Heading * F_PI / 180.0f); vel[2] = 0; } // Transform the GPS position into NED coordinates getNED(&gpsData, NED); // Track barometric altitude offset with a low pass filter baroOffset = BARO_OFFSET_LOWPASS_ALPHA * baroOffset + (1.0f - BARO_OFFSET_LOWPASS_ALPHA ) * ( -NED[2] - baroData.Altitude ); } else if (!outdoor_mode) { INSSetPosVelVar(1e2f, 1e2f); vel[0] = vel[1] = vel[2] = 0; NED[0] = NED[1] = 0; NED[2] = -(baroData.Altitude + baroOffset); sensors |= HORIZ_SENSORS | HORIZ_POS_SENSORS; sensors |= POS_SENSORS |VERT_SENSORS; } if (gps_vel_updated && outdoor_mode) { sensors |= HORIZ_SENSORS | VERT_SENSORS; vel[0] = gpsVelData.North; vel[1] = gpsVelData.East; vel[2] = gpsVelData.Down; } /* * TODO: Need to add a general sanity check for all the inputs to make sure their kosher * although probably should occur within INS itself */ if (sensors) INSCorrection(&magData.x, NED, vel, ( baroData.Altitude + baroOffset ), sensors); // Copy the position and velocity into the UAVO PositionActualData positionActual; PositionActualGet(&positionActual); positionActual.North = Nav.Pos[0]; positionActual.East = Nav.Pos[1]; positionActual.Down = Nav.Pos[2]; PositionActualSet(&positionActual); VelocityActualData velocityActual; VelocityActualGet(&velocityActual); velocityActual.North = Nav.Vel[0]; velocityActual.East = Nav.Vel[1]; velocityActual.Down = Nav.Vel[2]; VelocityActualSet(&velocityActual); if (revoCalibration.BiasCorrectedRaw == REVOCALIBRATION_BIASCORRECTEDRAW_TRUE && !gyroBiasSettingsUpdated) { // Copy the gyro bias into the UAVO except when it was updated // from the settings during the calculation, then consume it // next cycle gyrosBias.x = Nav.gyro_bias[0] * 180.0f / F_PI; gyrosBias.y = Nav.gyro_bias[1] * 180.0f / F_PI; gyrosBias.z = Nav.gyro_bias[2] * 180.0f / F_PI; GyrosBiasSet(&gyrosBias); } return 0; }
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; }