mirror of
https://bitbucket.org/librepilot/librepilot.git
synced 2025-02-20 10:54:14 +01:00
OP-132 Flight/AHRS: Changed the heuristics regarding GPS quality and INSGPS updates. Now if satellites < 7 or PDOP > 3.5 it does not perform GPS updates. Professor is working on some code to fix parts of the covariance matrix. If indoor updates high a 0 position and velocity and high variance.
git-svn-id: svn://svn.openpilot.org/OpenPilot/trunk@1515 ebee16cc-31ac-478f-84a7-5cbb03baadba
This commit is contained in:
parent
72e64e1009
commit
a406034ed9
@ -358,17 +358,30 @@ int main()
|
||||
|
||||
if ( gps_data.updated )
|
||||
{
|
||||
// Compute velocity from Heading and groundspeed
|
||||
vel[0] = gps_data.groundspeed * cos(gps_data.heading * M_PI / 180);
|
||||
vel[1] = gps_data.groundspeed * sin(gps_data.heading * M_PI / 180);
|
||||
vel[2] = 0;
|
||||
|
||||
// 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 / (.00001 + gps_data.quality));
|
||||
FullCorrection(mag, gps_data.NED, vel, altitude_data.altitude);
|
||||
gps_data.updated = false;
|
||||
if(gps_data.quality == 1)
|
||||
{
|
||||
// Compute velocity from Heading and groundspeed
|
||||
vel[0] = gps_data.groundspeed * cos(gps_data.heading * M_PI / 180);
|
||||
vel[1] = gps_data.groundspeed * sin(gps_data.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);
|
||||
FullCorrection(mag, gps_data.NED, vel, altitude_data.altitude);
|
||||
gps_data.updated = false;
|
||||
} else if(gps_data.quality == 0) {
|
||||
// Not indoors but lost lock.
|
||||
// TODO: Want to clamp coviance matrix to initial conditions on diagonal
|
||||
MagCorrection(mag);
|
||||
} else if(gps_data.quality == -1) {
|
||||
// Indoors, update with zero position and velocity and high covariance
|
||||
INSSetPosVelVar(10);
|
||||
vel[0] = 0;
|
||||
vel[1] = 0;
|
||||
vel[2] = 0;
|
||||
FullCorrection(mag, gps_data.NED, vel, altitude_data.altitude);
|
||||
}
|
||||
}
|
||||
else
|
||||
MagCorrection(mag);
|
||||
@ -692,6 +705,7 @@ void process_spi_request(void)
|
||||
case OPAHRS_MSG_V1_REQ_NORTH:
|
||||
opahrs_msg_v1_init_user_tx (&user_tx_v1, OPAHRS_MSG_V1_RSP_NORTH);
|
||||
INSSetMagNorth(user_rx_v1.payload.user.v.req.north.Be);
|
||||
INSGPSInit(); // TODO: Better reinitialization when North is finally established
|
||||
dump_spi_message(PIOS_COM_AUX, "N", (uint8_t *)&user_rx_v1, sizeof(user_rx_v1));
|
||||
lfsm_user_set_tx_v1 (&user_tx_v1);
|
||||
break;
|
||||
|
@ -39,6 +39,7 @@ void INSSetMagVar(float scaled_mag_var[3]);
|
||||
void MagCorrection(float mag_data[3]);
|
||||
void FullCorrection(float mag_data[3], float Pos[3], float Vel[3], float BaroAlt);
|
||||
void GndSpeedAndMagCorrection(float Speed, float Heading, float mag_data[3]);
|
||||
void GpsMagCorrection(float mag_data[3], float Pos[3], float Vel[2]);
|
||||
|
||||
// Nav structure containing current solution
|
||||
struct NavStruct {
|
||||
@ -50,6 +51,7 @@ struct NavStruct {
|
||||
// constants
|
||||
#define MagSensors 0x1C0
|
||||
#define FullSensors 0x3FF
|
||||
#define GpsMagSensors 0x1DF
|
||||
#define GndSpeedAndMagSensors 0x1D8
|
||||
|
||||
#endif /* EKF_H_ */
|
||||
|
@ -234,6 +234,46 @@ void FullCorrection(float mag_data[3], float Pos[3], float Vel[3], float BaroAlt
|
||||
Nav.q[3] = X[9];
|
||||
}
|
||||
|
||||
void GpsMagCorrection(float mag_data[3], float Pos[3], float Vel[2])
|
||||
{
|
||||
float Z[10], Y[10];
|
||||
float Bmag, qmag;
|
||||
|
||||
// GPS Position in meters and in local NED frame
|
||||
Z[0]=Pos[0];
|
||||
Z[1]=Pos[1];
|
||||
Z[2]=Pos[2];
|
||||
|
||||
// GPS Velocity in meters and in local NED frame
|
||||
Z[3]=Vel[0];
|
||||
Z[4]=Vel[1];
|
||||
|
||||
// magnetometer data in any units (use unit vector) and in body frame
|
||||
Bmag = sqrt(mag_data[0]*mag_data[0] + mag_data[1]*mag_data[1] + mag_data[2]*mag_data[2]);
|
||||
Z[6] = mag_data[0]/Bmag;
|
||||
Z[7] = mag_data[1]/Bmag;
|
||||
Z[8] = mag_data[2]/Bmag;
|
||||
|
||||
// EKF correction step
|
||||
LinearizeH(X,Be,H);
|
||||
MeasurementEq(X,Be,Y);
|
||||
SerialUpdate(H,R,Z,Y,P,X,GpsMagSensors);
|
||||
qmag=sqrt(X[6]*X[6] + X[7]*X[7] + X[8]*X[8] + X[9]*X[9]);
|
||||
X[6] /= qmag; X[7] /= qmag; X[8] /= qmag; X[9] /= qmag;
|
||||
|
||||
// Update Nav solution structure
|
||||
Nav.Pos[0] = X[0];
|
||||
Nav.Pos[1] = X[1];
|
||||
Nav.Pos[2] = X[2];
|
||||
Nav.Vel[0] = X[3];
|
||||
Nav.Vel[1] = X[4];
|
||||
Nav.Vel[2] = X[5];
|
||||
Nav.q[0] = X[6];
|
||||
Nav.q[1] = X[7];
|
||||
Nav.q[2] = X[8];
|
||||
Nav.q[3] = X[9];
|
||||
}
|
||||
|
||||
void GndSpeedAndMagCorrection(float Speed, float Heading, float mag_data[3])
|
||||
{
|
||||
float Z[10], Y[10];
|
||||
|
@ -121,6 +121,8 @@ static void AHRSCalibrationUpdatedCb(UAVObjEvent * ev)
|
||||
AHRSCalibrationIsUpdatedFlag = true;
|
||||
}
|
||||
|
||||
static uint32_t GPSGoodUpdates;
|
||||
|
||||
/**
|
||||
* Initialise the module, called on startup
|
||||
* \returns 0 on success or -1 if initialisation failed
|
||||
@ -154,6 +156,8 @@ static void ahrscommsTask(void* parameters)
|
||||
{
|
||||
enum opahrs_result result;
|
||||
|
||||
GPSGoodUpdates = 0;
|
||||
|
||||
// Main task loop
|
||||
while (1) {
|
||||
struct opahrs_msg_v1 rsp;
|
||||
@ -416,18 +420,27 @@ static void load_gps_position(struct opahrs_msg_v1_req_update * update)
|
||||
update->gps.NED[2] = 0;
|
||||
update->gps.groundspeed = 0;
|
||||
update->gps.heading = 0;
|
||||
update->gps.quality = 0;
|
||||
update->gps.quality = -1; // indicates indoor mode, high variance zeros update
|
||||
} else {
|
||||
update->gps.groundspeed = data.Groundspeed;
|
||||
update->gps.heading = data.Heading;
|
||||
//Crude measure of quality that decreases with increasing dilution of precision
|
||||
update->gps.quality = 1 / (data.HDOP + data.VDOP + data.PDOP);
|
||||
double LLA[3] = {(double) data.Latitude / 1e7, (double) data.Longitude / 1e7, (double) (data.GeoidSeparation + data.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, update->gps.NED);
|
||||
}
|
||||
|
||||
// TODO: Parameterize these heuristics into the settings
|
||||
if(data.Satellites >= 7 && data.PDOP < 3.5) {
|
||||
if(GPSGoodUpdates < 30) {
|
||||
GPSGoodUpdates++;
|
||||
update->gps.quality = 0;
|
||||
} else {
|
||||
update->gps.groundspeed = data.Groundspeed;
|
||||
update->gps.heading = data.Heading;
|
||||
double LLA[3] = {(double) data.Latitude / 1e7, (double) data.Longitude / 1e7, (double) (data.GeoidSeparation + data.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, update->gps.NED);
|
||||
update->gps.quality = 1;
|
||||
}
|
||||
} else {
|
||||
GPSGoodUpdates = 0;
|
||||
update->gps.quality = 0;
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
static void process_update(struct opahrs_msg_v1_rsp_update * update)
|
||||
|
@ -23,6 +23,8 @@
|
||||
65209A1912208B0600453371 /* gpsposition.xml */ = {isa = PBXFileReference; fileEncoding = 4; lastKnownFileType = text.xml; path = gpsposition.xml; sourceTree = "<group>"; };
|
||||
6526645A122DF972006F9A3C /* pios_i2c_priv.h */ = {isa = PBXFileReference; fileEncoding = 4; lastKnownFileType = sourcecode.c.h; path = pios_i2c_priv.h; sourceTree = "<group>"; };
|
||||
6526645B122DF972006F9A3C /* pios_wdg.h */ = {isa = PBXFileReference; fileEncoding = 4; lastKnownFileType = sourcecode.c.h; path = pios_wdg.h; sourceTree = "<group>"; };
|
||||
65311473122F4C4B00420041 /* stabilization.h */ = {isa = PBXFileReference; fileEncoding = 4; lastKnownFileType = sourcecode.c.h; path = stabilization.h; sourceTree = "<group>"; };
|
||||
65311474122F4C4B00420041 /* stabilization.c */ = {isa = PBXFileReference; fileEncoding = 4; lastKnownFileType = sourcecode.c.c; path = stabilization.c; sourceTree = "<group>"; };
|
||||
65322D2F12283CCD0046CD7C /* NMEA.c */ = {isa = PBXFileReference; fileEncoding = 4; lastKnownFileType = sourcecode.c.c; path = NMEA.c; sourceTree = "<group>"; };
|
||||
65322D3012283CD60046CD7C /* NMEA.h */ = {isa = PBXFileReference; fileEncoding = 4; lastKnownFileType = sourcecode.c.h; path = NMEA.h; sourceTree = "<group>"; };
|
||||
65322D3B122841F60046CD7C /* gpstime.xml */ = {isa = PBXFileReference; fileEncoding = 4; lastKnownFileType = text.xml; path = gpstime.xml; sourceTree = "<group>"; };
|
||||
@ -2614,8 +2616,6 @@
|
||||
65E8EF4A11EEA61E00BBF654 /* MkSerial.h */ = {isa = PBXFileReference; fileEncoding = 4; lastKnownFileType = sourcecode.c.h; name = MkSerial.h; path = ../../OpenPilot/Modules/MK/MKSerial/inc/MkSerial.h; sourceTree = SOURCE_ROOT; };
|
||||
65E8EF4B11EEA61E00BBF654 /* MKSerial.c */ = {isa = PBXFileReference; fileEncoding = 4; lastKnownFileType = sourcecode.c.c; name = MKSerial.c; path = ../../OpenPilot/Modules/MK/MKSerial/MKSerial.c; sourceTree = SOURCE_ROOT; };
|
||||
65E8EF4E11EEA61E00BBF654 /* OsdEtStd.c */ = {isa = PBXFileReference; fileEncoding = 4; lastKnownFileType = sourcecode.c.c; name = OsdEtStd.c; path = ../../OpenPilot/Modules/Osd/OsdEtStd/OsdEtStd.c; sourceTree = SOURCE_ROOT; };
|
||||
65E8EF5111EEA61E00BBF654 /* stabilization.h */ = {isa = PBXFileReference; fileEncoding = 4; lastKnownFileType = sourcecode.c.h; name = stabilization.h; path = ../../OpenPilot/Modules/Stabilization/inc/stabilization.h; sourceTree = SOURCE_ROOT; };
|
||||
65E8EF5211EEA61E00BBF654 /* stabilization.c */ = {isa = PBXFileReference; fileEncoding = 4; lastKnownFileType = sourcecode.c.c; name = stabilization.c; path = ../../OpenPilot/Modules/Stabilization/stabilization.c; sourceTree = SOURCE_ROOT; };
|
||||
65E8EF5511EEA61E00BBF654 /* systemmod.h */ = {isa = PBXFileReference; fileEncoding = 4; lastKnownFileType = sourcecode.c.h; name = systemmod.h; path = ../../OpenPilot/Modules/System/inc/systemmod.h; sourceTree = SOURCE_ROOT; };
|
||||
65E8EF5611EEA61E00BBF654 /* systemmod.c */ = {isa = PBXFileReference; fileEncoding = 4; lastKnownFileType = sourcecode.c.c; name = systemmod.c; path = ../../OpenPilot/Modules/System/systemmod.c; sourceTree = SOURCE_ROOT; };
|
||||
65E8EF5911EEA61E00BBF654 /* telemetry.h */ = {isa = PBXFileReference; fileEncoding = 4; lastKnownFileType = sourcecode.c.h; name = telemetry.h; path = ../../OpenPilot/Modules/Telemetry/inc/telemetry.h; sourceTree = SOURCE_ROOT; };
|
||||
@ -2890,6 +2890,31 @@
|
||||
path = inc;
|
||||
sourceTree = "<group>";
|
||||
};
|
||||
65311470122F4C4B00420041 /* simple */ = {
|
||||
isa = PBXGroup;
|
||||
children = (
|
||||
65311471122F4C4B00420041 /* Stabilization */,
|
||||
);
|
||||
path = simple;
|
||||
sourceTree = "<group>";
|
||||
};
|
||||
65311471122F4C4B00420041 /* Stabilization */ = {
|
||||
isa = PBXGroup;
|
||||
children = (
|
||||
65311472122F4C4B00420041 /* inc */,
|
||||
65311474122F4C4B00420041 /* stabilization.c */,
|
||||
);
|
||||
path = Stabilization;
|
||||
sourceTree = "<group>";
|
||||
};
|
||||
65311472122F4C4B00420041 /* inc */ = {
|
||||
isa = PBXGroup;
|
||||
children = (
|
||||
65311473122F4C4B00420041 /* stabilization.h */,
|
||||
);
|
||||
path = inc;
|
||||
sourceTree = "<group>";
|
||||
};
|
||||
657CEEB5121DBC49007A1FBE /* flight */ = {
|
||||
isa = PBXGroup;
|
||||
children = (
|
||||
@ -6976,22 +7001,12 @@
|
||||
65E8EF4F11EEA61E00BBF654 /* Stabilization */ = {
|
||||
isa = PBXGroup;
|
||||
children = (
|
||||
65E8EF5011EEA61E00BBF654 /* inc */,
|
||||
65E8EF5211EEA61E00BBF654 /* stabilization.c */,
|
||||
65311470122F4C4B00420041 /* simple */,
|
||||
);
|
||||
name = Stabilization;
|
||||
path = ../../OpenPilot/Modules/Stabilization;
|
||||
sourceTree = SOURCE_ROOT;
|
||||
};
|
||||
65E8EF5011EEA61E00BBF654 /* inc */ = {
|
||||
isa = PBXGroup;
|
||||
children = (
|
||||
65E8EF5111EEA61E00BBF654 /* stabilization.h */,
|
||||
);
|
||||
name = inc;
|
||||
path = ../../OpenPilot/Modules/Stabilization/inc;
|
||||
sourceTree = SOURCE_ROOT;
|
||||
};
|
||||
65E8EF5311EEA61E00BBF654 /* System */ = {
|
||||
isa = PBXGroup;
|
||||
children = (
|
||||
|
Loading…
x
Reference in New Issue
Block a user