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

AHRS: Add a weak mag correction in indoor mode to remove gyro bias and try and

Browse Source 
get a reliable heading

git-svn-id: svn://svn.openpilot.org/OpenPilot/trunk@1765 ebee16cc-31ac-478f-84a7-5cbb03baadba
This commit is contained in:
peabody124 2010-09-26 03:06:51 +00:00 committed by peabody124
parent a91e5502b4
commit a3ea9a5461
4 changed files with 35 additions and 21 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

16
flight/AHRS/ahrs.c
View File

@ -362,6 +362,8 @@ int main()
mag_data.updated = 0;
} else if (gps_data.quality != -1
&& mag_data.updated == 1) {
float mag_var[3] = {mag_data.calibration.variance[1], mag_data.calibration.variance[0], mag_data.calibration.variance[2]};
INSSetMagVar(mag_var);
MagCorrection(mag); // only trust mags if outdoors
mag_data.updated = 0;
} else {
@ -371,9 +373,14 @@ int main()
vel[1] = 0;
vel[2] = 0;
VelBaroCorrection(vel,
altitude_data.altitude);
// MagVelBaroCorrection(mag,vel,altitude_data.altitude); // only trust mags if outdoors
if(mag_data.updated == 1) {
float mag_var[3] = {10,10,10};
INSSetMagVar(mag_var);
MagVelBaroCorrection(mag,vel,altitude_data.altitude); // only trust mags if outdoors
mag_data.updated = 0;
} else {
VelBaroCorrection(vel, altitude_data.altitude);
}
}
attitude_data.quaternion.q1 = Nav.q[0];
@ -791,7 +798,8 @@ void process_spi_request(void)
INSSetGyroBias(zeros); //gyro bias corrects in preprocessing
INSSetAccelVar(accel_data.calibration.variance);
INSSetGyroVar(gyro_data.calibration.variance);
INSSetMagVar(mag_data.calibration.variance);
float mag_var[3] = {mag_data.calibration.variance[1], mag_data.calibration.variance[0], mag_data.calibration.variance[2]};
INSSetMagVar(mag_var);
}
// echo back the values used

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

@ -32,6 +32,8 @@
654330231218E9780063F913 /* insgps.c */ = {isa = PBXFileReference; fileEncoding = 4; lastKnownFileType = sourcecode.c.c; name = insgps.c; path = ../../AHRS/insgps.c; sourceTree = SOURCE_ROOT; };
6543304F121980300063F913 /* insgps.h */ = {isa = PBXFileReference; fileEncoding = 4; lastKnownFileType = sourcecode.c.h; path = insgps.h; sourceTree = "<group>"; };
655268BC121FBD2900410C6E /* ahrscalibration.xml */ = {isa = PBXFileReference; fileEncoding = 4; lastKnownFileType = text.xml; path = ahrscalibration.xml; sourceTree = "<group>"; };
65632CBF124E09D900469B77 /* guidance.c */ = {isa = PBXFileReference; fileEncoding = 4; lastKnownFileType = sourcecode.c.c; path = guidance.c; sourceTree = "<group>"; };
65632CC1124E09D900469B77 /* guidance.h */ = {isa = PBXFileReference; fileEncoding = 4; lastKnownFileType = sourcecode.c.h; path = guidance.h; sourceTree = "<group>"; };
657CEEAD121DB6C8007A1FBE /* homelocation.xml */ = {isa = PBXFileReference; fileEncoding = 4; lastKnownFileType = text.xml; path = homelocation.xml; sourceTree = "<group>"; };
657CEEB7121DBC63007A1FBE /* CoordinateConversions.c */ = {isa = PBXFileReference; fileEncoding = 4; lastKnownFileType = sourcecode.c.c; path = CoordinateConversions.c; sourceTree = "<group>"; };
657CEEB9121DBC63007A1FBE /* CoordinateConversions.h */ = {isa = PBXFileReference; fileEncoding = 4; lastKnownFileType = sourcecode.c.h; path = CoordinateConversions.h; sourceTree = "<group>"; };
@ -2564,13 +2566,10 @@
65B367EE121C2620003EAD18 /* exampleobject2.xml */ = {isa = PBXFileReference; fileEncoding = 4; lastKnownFileType = text.xml; path = exampleobject2.xml; sourceTree = "<group>"; };
65B367EF121C2620003EAD18 /* examplesettings.xml */ = {isa = PBXFileReference; fileEncoding = 4; lastKnownFileType = text.xml; path = examplesettings.xml; sourceTree = "<group>"; };
65B367F0121C2620003EAD18 /* flightbatterystate.xml */ = {isa = PBXFileReference; fileEncoding = 4; lastKnownFileType = text.xml; path = flightbatterystate.xml; sourceTree = "<group>"; };
65B367F1121C2620003EAD18 /* flightsituationactual.xml */ = {isa = PBXFileReference; fileEncoding = 4; lastKnownFileType = text.xml; path = flightsituationactual.xml; sourceTree = "<group>"; };
65B367F2121C2620003EAD18 /* flighttelemetrystats.xml */ = {isa = PBXFileReference; fileEncoding = 4; lastKnownFileType = text.xml; path = flighttelemetrystats.xml; sourceTree = "<group>"; };
65B367F3121C2620003EAD18 /* gcstelemetrystats.xml */ = {isa = PBXFileReference; fileEncoding = 4; lastKnownFileType = text.xml; path = gcstelemetrystats.xml; sourceTree = "<group>"; };
65B367F4121C2620003EAD18 /* manualcontrolcommand.xml */ = {isa = PBXFileReference; fileEncoding = 4; lastKnownFileType = text.xml; path = manualcontrolcommand.xml; sourceTree = "<group>"; };
65B367F5121C2620003EAD18 /* manualcontrolsettings.xml */ = {isa = PBXFileReference; fileEncoding = 4; lastKnownFileType = text.xml; path = manualcontrolsettings.xml; sourceTree = "<group>"; };
65B367F6121C2620003EAD18 /* navigationdesired.xml */ = {isa = PBXFileReference; fileEncoding = 4; lastKnownFileType = text.xml; path = navigationdesired.xml; sourceTree = "<group>"; };
65B367F7121C2620003EAD18 /* navigationsettings.xml */ = {isa = PBXFileReference; fileEncoding = 4; lastKnownFileType = text.xml; path = navigationsettings.xml; sourceTree = "<group>"; };
65B367F8121C2620003EAD18 /* objectpersistence.xml */ = {isa = PBXFileReference; fileEncoding = 4; lastKnownFileType = text.xml; path = objectpersistence.xml; sourceTree = "<group>"; };
65B367F9121C2620003EAD18 /* positionactual.xml */ = {isa = PBXFileReference; fileEncoding = 4; lastKnownFileType = text.xml; path = positionactual.xml; sourceTree = "<group>"; };
65B367FA121C2620003EAD18 /* stabilizationsettings.xml */ = {isa = PBXFileReference; fileEncoding = 4; lastKnownFileType = text.xml; path = stabilizationsettings.xml; sourceTree = "<group>"; };
@ -2929,6 +2928,23 @@
path = inc;
sourceTree = "<group>";
};
65632CBE124E09D900469B77 /* Guidance */ = {
isa = PBXGroup;
children = (
65632CBF124E09D900469B77 /* guidance.c */,
65632CC0124E09D900469B77 /* inc */,
);
path = Guidance;
sourceTree = "<group>";
};
65632CC0124E09D900469B77 /* inc */ = {
isa = PBXGroup;
children = (
65632CC1124E09D900469B77 /* guidance.h */,
);
path = inc;
sourceTree = "<group>";
};
657CEEB5121DBC49007A1FBE /* flight */ = {
isa = PBXGroup;
children = (
@ -6727,7 +6743,6 @@
65B367EE121C2620003EAD18 /* exampleobject2.xml */,
65B367EF121C2620003EAD18 /* examplesettings.xml */,
65B367F0121C2620003EAD18 /* flightbatterystate.xml */,
65B367F1121C2620003EAD18 /* flightsituationactual.xml */,
65B367F2121C2620003EAD18 /* flighttelemetrystats.xml */,
65B367F3121C2620003EAD18 /* gcstelemetrystats.xml */,
65209A1912208B0600453371 /* gpsposition.xml */,
@ -6735,8 +6750,6 @@
657CEEAD121DB6C8007A1FBE /* homelocation.xml */,
65B367F4121C2620003EAD18 /* manualcontrolcommand.xml */,
65B367F5121C2620003EAD18 /* manualcontrolsettings.xml */,
65B367F6121C2620003EAD18 /* navigationdesired.xml */,
65B367F7121C2620003EAD18 /* navigationsettings.xml */,
65B367F8121C2620003EAD18 /* objectpersistence.xml */,
65B367F9121C2620003EAD18 /* positionactual.xml */,
65B367FA121C2620003EAD18 /* stabilizationsettings.xml */,
@ -6806,6 +6819,7 @@
65E8EF3011EEA61E00BBF654 /* Battery */,
65E8EF3411EEA61E00BBF654 /* Example */,
65E8EF3D11EEA61E00BBF654 /* GPS */,
65632CBE124E09D900469B77 /* Guidance */,
65E8EF4311EEA61E00BBF654 /* ManualControl */,
65E8EF4711EEA61E00BBF654 /* MK */,
65E8EF4C11EEA61E00BBF654 /* Osd */,

12
ground/src/plugins/hitlnew/il2simulator.cpp
View File

@ -245,16 +245,8 @@ void IL2Simulator::processUpdate(const QByteArray& inp)
while (gpsData.Longitude<-180.0) gpsData.Longitude+=360.0;
while (gpsData.Longitude>180.0) gpsData.Longitude-=360.0;
gpsData.Satellites = 7;
gpsData.Status = PositionActual::STATUS_FIX3D;
gpsData.NED[0]=current.Y;
gpsData.NED[1]=current.X;
gpsData.NED[2]=-current.Z;
gpsData.Vel[0]=current.dY;
gpsData.Vel[1]=current.dX;
gpsData.Vel[2]=-current.dZ;
gpsData.Airspeed=current.ias;
gpsData.Climbrate=current.dZ;
posActual->setData(gpsData);
gpsData.Status = GPSPosition::STATUS_FIX3D;
gpsPos->setData(gpsData);
// issue manual update
attActual->updated();

2
ground/src/plugins/hitlnew/xplanesimulator.cpp
View File

@ -227,7 +227,7 @@ void XplaneSimulator::processUpdate(const QByteArray& dataBuf)
// gpsData.Groundspeed = speed[0];
gpsData.Latitude = latitude;
gpsData.Longitude = longitude;
gpsData->setData(gpsData);
gpsPos->setData(gpsData);
}
// issue manual update
//attActual->updated();