From a406034ed903ff98377a10bfbee3334589d01ca3 Mon Sep 17 00:00:00 2001 From: peabody124 Date: Sat, 4 Sep 2010 02:19:00 +0000 Subject: [PATCH] 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 --- flight/AHRS/ahrs.c | 36 +++++++++++----- flight/AHRS/inc/insgps.h | 2 + flight/AHRS/insgps.c | 40 ++++++++++++++++++ .../OpenPilot/Modules/AHRSComms/ahrs_comms.c | 35 +++++++++++----- .../OpenPilotOSX.xcodeproj/project.pbxproj | 41 +++++++++++++------ 5 files changed, 119 insertions(+), 35 deletions(-) diff --git a/flight/AHRS/ahrs.c b/flight/AHRS/ahrs.c index 8a6a7d70f..5af6ce439 100644 --- a/flight/AHRS/ahrs.c +++ b/flight/AHRS/ahrs.c @@ -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; diff --git a/flight/AHRS/inc/insgps.h b/flight/AHRS/inc/insgps.h index d9d570728..e4908ae8c 100644 --- a/flight/AHRS/inc/insgps.h +++ b/flight/AHRS/inc/insgps.h @@ -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_ */ diff --git a/flight/AHRS/insgps.c b/flight/AHRS/insgps.c index 1eaf70ada..184369bc3 100644 --- a/flight/AHRS/insgps.c +++ b/flight/AHRS/insgps.c @@ -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]; diff --git a/flight/OpenPilot/Modules/AHRSComms/ahrs_comms.c b/flight/OpenPilot/Modules/AHRSComms/ahrs_comms.c index 1455f3a9c..266035366 100644 --- a/flight/OpenPilot/Modules/AHRSComms/ahrs_comms.c +++ b/flight/OpenPilot/Modules/AHRSComms/ahrs_comms.c @@ -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) diff --git a/flight/Project/OpenPilotOSX/OpenPilotOSX.xcodeproj/project.pbxproj b/flight/Project/OpenPilotOSX/OpenPilotOSX.xcodeproj/project.pbxproj index 7ed93838c..966774dd3 100644 --- a/flight/Project/OpenPilotOSX/OpenPilotOSX.xcodeproj/project.pbxproj +++ b/flight/Project/OpenPilotOSX/OpenPilotOSX.xcodeproj/project.pbxproj @@ -23,6 +23,8 @@ 65209A1912208B0600453371 /* gpsposition.xml */ = {isa = PBXFileReference; fileEncoding = 4; lastKnownFileType = text.xml; path = gpsposition.xml; sourceTree = ""; }; 6526645A122DF972006F9A3C /* pios_i2c_priv.h */ = {isa = PBXFileReference; fileEncoding = 4; lastKnownFileType = sourcecode.c.h; path = pios_i2c_priv.h; sourceTree = ""; }; 6526645B122DF972006F9A3C /* pios_wdg.h */ = {isa = PBXFileReference; fileEncoding = 4; lastKnownFileType = sourcecode.c.h; path = pios_wdg.h; sourceTree = ""; }; + 65311473122F4C4B00420041 /* stabilization.h */ = {isa = PBXFileReference; fileEncoding = 4; lastKnownFileType = sourcecode.c.h; path = stabilization.h; sourceTree = ""; }; + 65311474122F4C4B00420041 /* stabilization.c */ = {isa = PBXFileReference; fileEncoding = 4; lastKnownFileType = sourcecode.c.c; path = stabilization.c; sourceTree = ""; }; 65322D2F12283CCD0046CD7C /* NMEA.c */ = {isa = PBXFileReference; fileEncoding = 4; lastKnownFileType = sourcecode.c.c; path = NMEA.c; sourceTree = ""; }; 65322D3012283CD60046CD7C /* NMEA.h */ = {isa = PBXFileReference; fileEncoding = 4; lastKnownFileType = sourcecode.c.h; path = NMEA.h; sourceTree = ""; }; 65322D3B122841F60046CD7C /* gpstime.xml */ = {isa = PBXFileReference; fileEncoding = 4; lastKnownFileType = text.xml; path = gpstime.xml; sourceTree = ""; }; @@ -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 = ""; }; + 65311470122F4C4B00420041 /* simple */ = { + isa = PBXGroup; + children = ( + 65311471122F4C4B00420041 /* Stabilization */, + ); + path = simple; + sourceTree = ""; + }; + 65311471122F4C4B00420041 /* Stabilization */ = { + isa = PBXGroup; + children = ( + 65311472122F4C4B00420041 /* inc */, + 65311474122F4C4B00420041 /* stabilization.c */, + ); + path = Stabilization; + sourceTree = ""; + }; + 65311472122F4C4B00420041 /* inc */ = { + isa = PBXGroup; + children = ( + 65311473122F4C4B00420041 /* stabilization.h */, + ); + path = inc; + sourceTree = ""; + }; 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 = (