rooty/LibrePilot
1
0
Fork 0
mirror of https://bitbucket.org/librepilot/librepilot.git synced 2025-03-15 07:29:15 +01:00
Code Issues Releases Activity

AHRS: If using mags then don't initialize until get a good mag update within

Browse Source 
250 ms of init call (mag works at 10 Hz).

git-svn-id: svn://svn.openpilot.org/OpenPilot/trunk@2289 ebee16cc-31ac-478f-84a7-5cbb03baadba
This commit is contained in:
peabody124 2010-12-26 18:59:32 +00:00 committed by peabody124
parent 398ae4f90a
commit c364f45fb7
1 changed files with 22 additions and 4 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

26
flight/AHRS/ahrs.c
View File

@ -306,17 +306,35 @@ void ins_init_algorithm()
using_mags = (ahrs_algorithm == AHRSSETTINGS_ALGORITHM_INSGPS_OUTDOOR) || (ahrs_algorithm == AHRSSETTINGS_ALGORITHM_INSGPS_INDOOR);
using_mags &= (home.Be[0] != 0) || (home.Be[1] != 0) || (home.Be[2] != 0); /* only use mags when valid home location */
using_gps = (ahrs_algorithm == AHRSSETTINGS_ALGORITHM_INSGPS_OUTDOOR) && (gps_data.quality != 0);
if (using_mags){
using_gps = (ahrs_algorithm == AHRSSETTINGS_ALGORITHM_INSGPS_OUTDOOR) && (gps_data.quality != 0);
/* Block till a data update */
get_accel_gyro_data();
/* Ensure we get mag data in a timely manner */
uint16_t fail_count = 50; // 50 at 200 Hz is up to 0.25 sec
while(using_mags && !mag_data.updated & fail_count--) {
get_accel_gyro_data();
AhrsPoll();
}
using_mags &= mag_data.updated;
if (using_mags) {
/* Spin waiting for mag data */
while(!mag_data.updated) {
get_accel_gyro_data();
AhrsPoll();
}
mag_data.updated = false;
RotFrom2Vectors(accels, ge, mag_data.scaled.axis, home.Be, Rbe);
R2Quaternion(Rbe,q);
if (using_gps)
INSSetState(gps_data.NED, zeros, q, zeros, zeros);
else
INSSetState(zeros, zeros, q, zeros, zeros);
}
else{
} else {
// assume yaw = 0
mag = VectorMagnitude(accels);
rpy[1] = asinf(-accels[0]/mag);