rooty/LibrePilot
1
0
Fork 0
mirror of https://bitbucket.org/librepilot/librepilot.git synced 2025-02-20 10:54:14 +01:00
Code Issues Releases Activity

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.

Browse Source 
git-svn-id: svn://svn.openpilot.org/OpenPilot/trunk@1515 ebee16cc-31ac-478f-84a7-5cbb03baadba
This commit is contained in:
peabody124 2010-09-04 02:19:00 +00:00 committed by peabody124
parent 72e64e1009
commit a406034ed9
5 changed files with 119 additions and 35 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

36
flight/AHRS/ahrs.c
View File

@ -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;

2
flight/AHRS/inc/insgps.h
View File

@ -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_ */

40
flight/AHRS/insgps.c
View File

@ -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];

35
flight/OpenPilot/Modules/AHRSComms/ahrs_comms.c
View File

@ -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)

41
flight/Project/OpenPilotOSX/OpenPilotOSX.xcodeproj/project.pbxproj
View File

@ -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 = (