PyMODINIT_FUNC
initins(void)
{
(void) Py_InitModule("ins", InsMethods);
import_array();
init(NULL, NULL);
INSGPSInit();
}
static PyObject*
init(PyObject* self, PyObject* args)
{
INSGPSInit();
const float Be[] = {400, 0, 1600};
INSSetMagNorth(Be);
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 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;
}