Skip to content
GitLab
Explore
Sign in
Register
Primary navigation
Search or go to…
Project
B
Baitboat
Manage
Activity
Members
Labels
Plan
Issues
0
Issue boards
Milestones
Wiki
Code
Merge requests
0
Repository
Branches
Commits
Tags
Repository graph
Compare revisions
Snippets
Build
Pipelines
Jobs
Pipeline schedules
Artifacts
Deploy
Releases
Model registry
Operate
Environments
Monitor
Incidents
Analyze
Value stream analytics
Contributor analytics
CI/CD analytics
Repository analytics
Model experiments
Help
Help
Support
GitLab documentation
Compare GitLab plans
Community forum
Contribute to GitLab
Provide feedback
Keyboard shortcuts
?
Snippets
Groups
Projects
Show more breadcrumbs
OpenSource
Baitboat
Commits
1e50875d
Commit
1e50875d
authored
12 years ago
by
rmackay9
Browse files
Options
Downloads
Patches
Plain Diff
AP_InertialNav: correct lat/lon to cm
parent
ebe33f1b
No related branches found
Branches containing commit
No related tags found
Tags containing commit
No related merge requests found
Changes
2
Hide whitespace changes
Inline
Side-by-side
Showing
2 changed files
libraries/AP_InertialNav/AP_InertialNav.cpp
+6
-14
6 additions, 14 deletions
libraries/AP_InertialNav/AP_InertialNav.cpp
libraries/AP_InertialNav/AP_InertialNav.h
+2
-0
2 additions, 0 deletions
libraries/AP_InertialNav/AP_InertialNav.h
with
8 additions
and
14 deletions
libraries/AP_InertialNav/AP_InertialNav.cpp
+
6
−
14
View file @
1e50875d
...
...
@@ -166,10 +166,8 @@ void AP_InertialNav::correct_with_gps(int32_t lon, int32_t lat, float dt)
}
// calculate distance from base location
//x = (float)(lat - _base_lat) * 1.113195;
//y = (float)(lon - _base_lon) * _lon_to_m_scaling * 1.113195;
x
=
(
float
)(
lat
-
_base_lat
);
y
=
(
float
)(
lon
-
_base_lon
)
*
_lon_to_m_scaling
;
x
=
(
float
)(
lat
-
_base_lat
)
*
AP_INERTIALNAV_LATLON_TO_CM
;
y
=
(
float
)(
lon
-
_base_lon
)
*
_lon_to_m_scaling
*
AP_INERTIALNAV_LATLON_TO_CM
;
// convert accelerometer readings to earth frame
Matrix3f
dcm
=
_ahrs
->
get_dcm_matrix
();
...
...
@@ -213,8 +211,7 @@ int32_t AP_InertialNav::get_latitude()
return
0
;
}
//return _base_lat - (int32_t)(_position.x / 1.113195);
return
_base_lat
-
(
int32_t
)(
_position_base
.
x
+
_position_correction
.
x
);
return
_base_lat
-
(
int32_t
)((
_position_base
.
x
+
_position_correction
.
x
)
/
AP_INERTIALNAV_LATLON_TO_CM
);
}
// get accel based longitude
...
...
@@ -225,8 +222,7 @@ int32_t AP_InertialNav::get_longitude()
return
0
;
}
//return _base_lon - (int32_t)(_position.y / (_lon_to_m_scaling * 1.113195) );
return
_base_lon
-
(
int32_t
)((
_position_base
.
y
+
_position_correction
.
y
)
/
_lon_to_m_scaling
);
return
_base_lon
-
(
int32_t
)((
_position_base
.
y
+
_position_correction
.
y
)
/
(
_lon_to_m_scaling
*
AP_INERTIALNAV_LATLON_TO_CM
));
}
// set_current_position - all internal calculations are recorded as the distances from this point
...
...
@@ -262,9 +258,7 @@ float AP_InertialNav::get_latitude_diff()
return
0
;
}
//return _base_lat + (int32_t)_position.x;
//return -_position.x / 1.113195;
return
-
(
_position_base
.
x
+
_position_correction
.
x
);
return
-
((
_position_base
.
x
+
_position_correction
.
x
)
/
AP_INERTIALNAV_LATLON_TO_CM
);
}
// get accel based longitude
...
...
@@ -275,9 +269,7 @@ float AP_InertialNav::get_longitude_diff()
return
0
;
}
//return _base_lon - (int32_t)(_position.x / _lon_to_m_scaling);
//return -_position.y / (_lon_to_m_scaling * 1.113195);
return
-
(
_position_base
.
y
+
_position_correction
.
y
)
/
_lon_to_m_scaling
;
return
-
(
_position_base
.
y
+
_position_correction
.
y
)
/
(
_lon_to_m_scaling
*
AP_INERTIALNAV_LATLON_TO_CM
);
}
// get velocity in latitude & longitude directions
...
...
This diff is collapsed.
Click to expand it.
libraries/AP_InertialNav/AP_InertialNav.h
+
2
−
0
View file @
1e50875d
...
...
@@ -17,6 +17,8 @@
#define AP_INTERTIALNAV_SAVE_POS_AFTER_ITERATIONS 10
#define AP_INTERTIALNAV_GPS_LAG_IN_10HZ_INCREMENTS 4 // must not be larger than size of _hist_position_estimate_x and _hist_position_estimate_y
#define AP_INERTIALNAV_LATLON_TO_CM 1.1113175
/*
* AP_InertialNav is an attempt to use accelerometers to augment other sensors to improve altitud e position hold
*/
...
...
This diff is collapsed.
Click to expand it.
Preview
0%
Loading
Try again
or
attach a new file
.
Cancel
You are about to add
0
people
to the discussion. Proceed with caution.
Finish editing this message first!
Save comment
Cancel
Please
register
or
sign in
to comment