mirror of
https://bitbucket.org/librepilot/librepilot.git
synced 2024-11-29 07:24:13 +01:00
Work on the INS algorithm
This commit is contained in:
James Cotton
parent
7034a45e44
commit
55ee568fe2
@ -39,6 +39,8 @@
|
||||
* @{
|
||||
*/
|
||||
#define POS_SENSORS 0x007
|
||||
#define HORIZ_POS_SENSORS 0x003
|
||||
#define VER_POS_SENSORS 0x004
|
||||
#define HORIZ_SENSORS 0x018
|
||||
#define VERT_SENSORS 0x020
|
||||
#define MAG_SENSORS 0x1C0
|
||||
|
||||
@ -62,6 +62,7 @@
|
||||
#include "baroaltitude.h"
|
||||
#include "flightstatus.h"
|
||||
#include "homelocation.h"
|
||||
#include "gpsposition.h"
|
||||
#include "CoordinateConversions.h"
|
||||
|
||||
// Private constants
|
||||
@ -95,8 +96,6 @@ static float accelKp = 0;
|
||||
static float yawBiasRate = 0;
|
||||
static float gyroGain = 0.42;
|
||||
static int16_t accelbias[3];
|
||||
static float R[3][3];
|
||||
static int8_t rotate = 0;
|
||||
static bool zero_during_arming = false;
|
||||
|
||||
|
||||
@ -140,9 +139,9 @@ int32_t AttitudeInitialize(void)
|
||||
gyrosBias.z = 0;
|
||||
GyrosBiasSet(&gyrosBias);
|
||||
|
||||
for(uint8_t i = 0; i < 3; i++)
|
||||
for(uint8_t j = 0; j < 3; j++)
|
||||
R[i][j] = 0;
|
||||
//for(uint8_t i = 0; i < 3; i++)
|
||||
// for(uint8_t j = 0; j < 3; j++)
|
||||
// R[i][j] = 0;
|
||||
|
||||
AttitudeSettingsConnectCallback(&settingsUpdatedCb);
|
||||
|
||||
@ -197,7 +196,7 @@ static void AttitudeTask(void *parameters)
|
||||
while (1) {
|
||||
|
||||
// This function blocks on data queue
|
||||
if(1)
|
||||
if(0)
|
||||
updateAttitudeComplimentary(first_run);
|
||||
else
|
||||
updateAttitudeINSGPS(first_run);
|
||||
@ -398,6 +397,9 @@ static int32_t updateAttitudeComplimentary(bool first_run)
|
||||
#include "insgps.h"
|
||||
int32_t ins_failed = 0;
|
||||
extern struct NavStruct Nav;
|
||||
bool outdoor_mode = false;
|
||||
int32_t init_stage = 0;
|
||||
|
||||
static int32_t updateAttitudeINSGPS(bool first_run)
|
||||
{
|
||||
UAVObjEvent ev;
|
||||
@ -405,12 +407,25 @@ static int32_t updateAttitudeINSGPS(bool first_run)
|
||||
AccelsData accelsData;
|
||||
MagnetometerData magData;
|
||||
BaroAltitudeData baroData;
|
||||
GPSPositionData gpsData;
|
||||
GyrosBiasData gyrosBias;
|
||||
|
||||
static bool mag_updated;
|
||||
static bool baro_updated;
|
||||
static bool gps_updated;
|
||||
|
||||
static uint32_t ins_last_time = 0;
|
||||
|
||||
static bool inited;
|
||||
if (first_run)
|
||||
inited = false;
|
||||
|
||||
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;
|
||||
|
||||
inited = first_run ? false : inited;
|
||||
|
||||
// Wait until the gyro and accel object is updated, if a timeout then go to failsafe
|
||||
if ( (xQueueReceive(gyroQueue, &ev, 10 / portTICK_RATE_MS) != pdTRUE) ||
|
||||
@ -420,69 +435,120 @@ static int32_t updateAttitudeINSGPS(bool first_run)
|
||||
return -1;
|
||||
}
|
||||
|
||||
// Get most recent data
|
||||
// TODO: Acquire all data in a queue
|
||||
GyrosGet(&gyrosData);
|
||||
AccelsGet(&accelsData);
|
||||
MagnetometerGet(&magData);
|
||||
BaroAltitudeGet(&baroData);
|
||||
|
||||
bool mag_updated;
|
||||
bool baro_updated;
|
||||
bool gps_updated;
|
||||
|
||||
if (inited) {
|
||||
mag_updated = 0;
|
||||
baro_updated = 0;
|
||||
gps_updated = 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;
|
||||
gps_updated |= (xQueueReceive(gpsQueue, &ev, 0 / portTICK_RATE_MS) == pdTRUE) && outdoor_mode;
|
||||
|
||||
if (!inited && (!mag_updated || !baro_updated || !gps_updated)) {
|
||||
// Get most recent data
|
||||
GyrosGet(&gyrosData);
|
||||
AccelsGet(&accelsData);
|
||||
MagnetometerGet(&magData);
|
||||
BaroAltitudeGet(&baroData);
|
||||
GPSPositionGet(&gpsData);
|
||||
|
||||
// Have a minimum requirement for gps usage
|
||||
gps_updated &= (gpsData.Satellites >= 7) && (gpsData.PDOP >= 3.0f);
|
||||
|
||||
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
|
||||
return -1;
|
||||
} else if (!inited ) {
|
||||
inited = true;
|
||||
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 var[3] = {500.0f, 500.0f, 500.0f};
|
||||
float pos[3] = {0.0f, 0.0f, 0.0f};
|
||||
pos[2] = baroData.Altitude * -1.0f;
|
||||
|
||||
float Rbe[3][3], q[4];
|
||||
float ge[3]={0.0f,0.0f,-9.81f};
|
||||
float zeros[3]={0.0f,0.0f,0.0f};
|
||||
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 vel[3], NED[3];
|
||||
// Reset the INS algorithm
|
||||
INSGPSInit();
|
||||
var[0] = var[1] = var[2] = 5e-3f;
|
||||
INSSetMagVar(var);
|
||||
var[0] = var[1] = var[2] = 1.5e-5f;
|
||||
INSSetAccelVar(var);
|
||||
var[0] = var[1] = var[2] = 2.0e-4f;
|
||||
INSSetGyroVar(var);
|
||||
|
||||
INSGPSInit();
|
||||
// Set initial attitude
|
||||
float rpy[3];
|
||||
rpy[0] = atan2f(accelsData.x, accelsData.z) * 180.0f / F_PI;
|
||||
rpy[1] = atan2f(accelsData.y, accelsData.z) * 180.0f / F_PI;
|
||||
rpy[2] = atan2f(magData.x, -magData.y) * 180.0f / F_PI;
|
||||
RPY2Quaternion(rpy,q);
|
||||
|
||||
HomeLocationData home;
|
||||
HomeLocationGet(&home);
|
||||
INSSetState(pos, zeros, q, zeros, zeros);
|
||||
INSResetP(Pdiag);
|
||||
} else if (init_stage == 0 && outdoor_mode) {
|
||||
float q[4], rpy[3];
|
||||
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 NED[3];
|
||||
float var[3];
|
||||
|
||||
GPSPositionData gpsPosition;
|
||||
GPSPositionGet(&gpsPosition);
|
||||
// Reset the INS algorithm
|
||||
INSGPSInit();
|
||||
var[0] = var[1] = var[2] = 5e-3f;
|
||||
INSSetMagVar(var);
|
||||
var[0] = var[1] = var[2] = 1.5e-5f;
|
||||
INSSetAccelVar(var);
|
||||
var[0] = var[1] = var[2] = 2.0e-4f;
|
||||
INSSetGyroVar(var);
|
||||
|
||||
vel[0] = gpsPosition.Groundspeed * cosf(gpsPosition.Heading * F_PI / 180.0f);
|
||||
vel[1] = gpsPosition.Groundspeed * sinf(gpsPosition.Heading * F_PI / 180.0f);
|
||||
vel[2] = 0;
|
||||
HomeLocationData home;
|
||||
HomeLocationGet(&home);
|
||||
INSSetMagNorth(home.Be);
|
||||
|
||||
// convert from cm back to meters
|
||||
float LLA[3] = {(float) gpsPosition.Latitude / 1e7f, (float) gpsPosition.Longitude / 1e7f, (float) (gpsPosition.GeoidSeparation + gpsPosition.Altitude)};
|
||||
// put in local NED frame
|
||||
float ECEF[3] = {(float) (home.ECEF[0] / 100.0f), (float) (home.ECEF[1] / 100.0f), (float) (home.ECEF[2] / 100.0f)};
|
||||
LLA2Base(LLA, ECEF, (float (*)[3]) home.RNE, NED);
|
||||
GPSPositionData gpsPosition;
|
||||
GPSPositionGet(&gpsPosition);
|
||||
|
||||
RotFrom2Vectors(&accelsData.x, ge, &magData.x, home.Be, Rbe);
|
||||
R2Quaternion(Rbe,q);
|
||||
INSSetState(NED, vel, q, &gyrosData.x, zeros);
|
||||
INSSetGyroBias(&gyrosData.x);
|
||||
INSResetP(Pdiag);
|
||||
// convert from cm back to meters
|
||||
float LLA[3] = {(float) gpsPosition.Latitude / 1e7f,
|
||||
(float) gpsPosition.Longitude / 1e7f,
|
||||
(float) (gpsPosition.GeoidSeparation + gpsPosition.Altitude)};
|
||||
// put in local NED frame
|
||||
float ECEF[3] = {(float) (home.ECEF[0] / 100.0f),
|
||||
(float) (home.ECEF[1] / 100.0f),
|
||||
(float) (home.ECEF[2] / 100.0f)};
|
||||
LLA2Base(LLA, ECEF, (float (*)[3]) home.RNE, NED);
|
||||
|
||||
// Set initial attitude
|
||||
rpy[0] = atan2f(accelsData.x, accelsData.z) * 180.0f / F_PI;
|
||||
rpy[1] = atan2f(accelsData.y, accelsData.z) * 180.0f / F_PI;
|
||||
rpy[2] = atan2f(magData.x, -magData.y) * 180.0f / F_PI;
|
||||
RPY2Quaternion(rpy,q);
|
||||
|
||||
INSSetState(NED, zeros, q, zeros, zeros);
|
||||
INSResetP(Pdiag);
|
||||
} else if (init_stage > 0) {
|
||||
// Run prediction a bit before any corrections
|
||||
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, 0.002f);
|
||||
}
|
||||
|
||||
init_stage++;
|
||||
if(init_stage > 500)
|
||||
inited = true;
|
||||
|
||||
ins_last_time = PIOS_DELAY_GetRaw();
|
||||
return 0;
|
||||
|
||||
return -1;
|
||||
}
|
||||
|
||||
// Perform the update
|
||||
uint16_t sensors = 0;
|
||||
float dT;
|
||||
if (!inited)
|
||||
return -1;
|
||||
|
||||
dT = PIOS_DELAY_DiffuS(ins_last_time) / 1.0e6f;
|
||||
ins_last_time = PIOS_DELAY_GetRaw();
|
||||
@ -493,10 +559,9 @@ static int32_t updateAttitudeINSGPS(bool first_run)
|
||||
else if(dT <= 0.001f)
|
||||
dT = 0.001f;
|
||||
|
||||
|
||||
GyrosBiasData gyrosBias;
|
||||
// Because the sensor module remove the bias we need to add it
|
||||
// back in here so that the INS algorithm can track it correctly
|
||||
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};
|
||||
@ -523,17 +588,21 @@ static int32_t updateAttitudeINSGPS(bool first_run)
|
||||
// Advance the covariance estimate
|
||||
INSCovariancePrediction(dT);
|
||||
|
||||
if(mag_updated)
|
||||
sensors |= MAG_SENSORS;
|
||||
//if(mag_updated)
|
||||
// sensors |= MAG_SENSORS;
|
||||
|
||||
if(baro_updated)
|
||||
sensors |= BARO_SENSOR;
|
||||
|
||||
float NED[3] = {0,0,0};
|
||||
float vel[3] = {0,0,0};
|
||||
HomeLocationData home;
|
||||
HomeLocationGet(&home);
|
||||
|
||||
if(gps_updated)
|
||||
INSSetMagNorth(home.Be);
|
||||
|
||||
if(gps_updated && outdoor_mode)
|
||||
{
|
||||
sensors = HORIZ_SENSORS | VERT_SENSORS;
|
||||
INSSetPosVelVar(1.0f, 1.0f);
|
||||
sensors = POS_SENSORS; //HORIZ_SENSORS | VERT_SENSORS;
|
||||
GPSPositionData gpsPosition;
|
||||
GPSPositionGet(&gpsPosition);
|
||||
|
||||
@ -541,21 +610,30 @@ static int32_t updateAttitudeINSGPS(bool first_run)
|
||||
vel[1] = gpsPosition.Groundspeed * sinf(gpsPosition.Heading * F_PI / 180.0f);
|
||||
vel[2] = 0;
|
||||
|
||||
HomeLocationData home;
|
||||
HomeLocationGet(&home);
|
||||
|
||||
// convert from cm back to meters
|
||||
float LLA[3] = {(float) gpsPosition.Latitude / 1e7f, (float) gpsPosition.Longitude / 1e7f, (float) (gpsPosition.GeoidSeparation + gpsPosition.Altitude)};
|
||||
float LLA[3] = {(float) gpsPosition.Latitude / 1e7f,
|
||||
(float) gpsPosition.Longitude / 1e7f,
|
||||
(float) (gpsPosition.GeoidSeparation + gpsPosition.Altitude)};
|
||||
// put in local NED frame
|
||||
float ECEF[3] = {(float) (home.ECEF[0] / 100.0f), (float) (home.ECEF[1] / 100.0f), (float) (home.ECEF[2] / 100.0f)};
|
||||
float ECEF[3] = {(float) (home.ECEF[0] / 100.0f),
|
||||
(float) (home.ECEF[1] / 100.0f),
|
||||
(float) (home.ECEF[2] / 100.0f)};
|
||||
LLA2Base(LLA, ECEF, (float (*)[3]) home.RNE, NED);
|
||||
} else if (!outdoor_mode) {
|
||||
//INSSetPosVelVar(0.1f, 0.1f);
|
||||
vel[0] = vel[1] = vel[2] = 0;
|
||||
NED[0] = NED[1] = 0;
|
||||
NED[2] = baroData.Altitude;
|
||||
sensors |= HORIZ_POS_SENSORS;
|
||||
//sensors |= HORIZ_SENSORS | VERT_SENSORS | POS_SENSORS;
|
||||
}
|
||||
|
||||
/*
|
||||
* TODO: Need to add a general sanity check for all the inputs to make sure their kosher
|
||||
* although probably should occur within INS itself
|
||||
*/
|
||||
INSCorrection(&magData.x, NED, vel, baroData.Altitude, sensors);
|
||||
if (sensors)
|
||||
INSCorrection(&magData.x, NED, vel, baroData.Altitude, sensors);
|
||||
|
||||
// Copy the position and velocity into the UAVO
|
||||
PositionActualData positionActual;
|
||||
@ -572,7 +650,6 @@ static int32_t updateAttitudeINSGPS(bool first_run)
|
||||
velocityActual.Down = Nav.Vel[2];
|
||||
VelocityActualSet(&velocityActual);
|
||||
|
||||
|
||||
if(fabs(Nav.gyro_bias[0]) > 0.1f || fabs(Nav.gyro_bias[1]) > 0.1f || fabs(Nav.gyro_bias[2]) > 0.1f) {
|
||||
float zeros[3] = {0.0f,0.0f,0.0f};
|
||||
INSSetGyroBias(zeros);
|
||||
@ -586,7 +663,6 @@ static void settingsUpdatedCb(UAVObjEvent * objEv)
|
||||
AttitudeSettingsData attitudeSettings;
|
||||
AttitudeSettingsGet(&attitudeSettings);
|
||||
|
||||
|
||||
accelKp = attitudeSettings.AccelKp;
|
||||
accelKi = attitudeSettings.AccelKi;
|
||||
yawBiasRate = attitudeSettings.YawBiasRate;
|
||||
@ -604,26 +680,8 @@ static void settingsUpdatedCb(UAVObjEvent * objEv)
|
||||
gyrosBias.y = attitudeSettings.GyroBias[ATTITUDESETTINGS_GYROBIAS_Y] / 100.0f;
|
||||
gyrosBias.z = attitudeSettings.GyroBias[ATTITUDESETTINGS_GYROBIAS_Z] / 100.0f;
|
||||
GyrosBiasSet(&gyrosBias);
|
||||
|
||||
// Indicates not to expend cycles on rotation
|
||||
if(attitudeSettings.BoardRotation[0] == 0 && attitudeSettings.BoardRotation[1] == 0 &&
|
||||
attitudeSettings.BoardRotation[2] == 0) {
|
||||
rotate = 0;
|
||||
|
||||
// Shouldn't be used but to be safe
|
||||
float rotationQuat[4] = {1,0,0,0};
|
||||
Quaternion2R(rotationQuat, R);
|
||||
} else {
|
||||
float rotationQuat[4];
|
||||
const float rpy[3] = {attitudeSettings.BoardRotation[ATTITUDESETTINGS_BOARDROTATION_ROLL],
|
||||
attitudeSettings.BoardRotation[ATTITUDESETTINGS_BOARDROTATION_PITCH],
|
||||
attitudeSettings.BoardRotation[ATTITUDESETTINGS_BOARDROTATION_YAW]};
|
||||
RPY2Quaternion(rpy, rotationQuat);
|
||||
Quaternion2R(rotationQuat, R);
|
||||
rotate = 1;
|
||||
}
|
||||
}
|
||||
/**
|
||||
* @}
|
||||
* @}
|
||||
*/
|
||||
* @}
|
||||
* @}
|
||||
*/
|
||||
|
||||
@ -231,6 +231,17 @@ void MagCorrection(float mag_data[3])
|
||||
INSCorrection(mag_data, zeros, zeros, zeros[0], MAG_SENSORS);
|
||||
}
|
||||
|
||||
void BaroCorrection(float baro)
|
||||
{
|
||||
INSCorrection(zeros, zeros, zeros, baro, BARO_SENSOR);
|
||||
}
|
||||
void GpsCorrection(float Pos[3], float Vel[3])
|
||||
{
|
||||
INSCorrection(zeros, Pos, Vel, zeros[0],
|
||||
POS_SENSORS); // | HORIZ_SENSORS | VERT_SENSORS);
|
||||
}
|
||||
|
||||
|
||||
void MagVelBaroCorrection(float mag_data[3], float Vel[3], float BaroAlt)
|
||||
{
|
||||
INSCorrection(mag_data, zeros, Vel, BaroAlt,
|
||||
@ -241,7 +252,7 @@ void MagVelBaroCorrection(float mag_data[3], float Vel[3], float BaroAlt)
|
||||
void GpsBaroCorrection(float Pos[3], float Vel[3], float BaroAlt)
|
||||
{
|
||||
INSCorrection(zeros, Pos, Vel, BaroAlt,
|
||||
HORIZ_SENSORS | VERT_SENSORS | BARO_SENSOR);
|
||||
POS_SENSORS | HORIZ_SENSORS | VERT_SENSORS | BARO_SENSOR);
|
||||
}
|
||||
|
||||
void FullCorrection(float mag_data[3], float Pos[3], float Vel[3],
|
||||
|
||||
@ -62,6 +62,8 @@ void INSSetMagVar(float scaled_mag_var[3]);
|
||||
void INSPosVelReset(float pos[3], float vel[3]);
|
||||
|
||||
void MagCorrection(float mag_data[3]);
|
||||
void BaroCorrection(float baro);
|
||||
void GpsCorrection(float Pos[3], float Vel[3]);
|
||||
void MagVelBaroCorrection(float mag_data[3], float Vel[3], float BaroAlt);
|
||||
void FullCorrection(float mag_data[3], float Pos[3], float Vel[3],
|
||||
float BaroAlt);
|
||||
|
||||
Loading…
Reference in New Issue
Block a user