/** * correction - perform a correction of the EKF * @params[in] self * @params[in] args * - Z - vector of measurements (position, velocity, mag, baro) * - sensors - binary flags for which sensors should be used * @return state */ static PyObject* correction(PyObject* self, PyObject* args) { PyArrayObject *vec_z; float z[10]; int sensors; if (!PyArg_ParseTuple(args, "O!i", &PyArray_Type, &vec_z, &sensors)) return NULL; if (NULL == vec_z) return NULL; if (!parseFloatVecN(vec_z, z, 10)) return NULL; INSCorrection(&z[6], &z[0], &z[3], z[9], sensors); return pack_state(self); }
/** * @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; }