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