fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-10-19 06:43:34 +08:00
Code Issues Packages Projects Releases Wiki Activity

position_estimator_inav, mc_pos_control: precise position reprojection on home position changes

Browse Source
This commit is contained in:
Anton Babushkin
2014-04-05 18:11:51 +04:00
parent 0fd6fb53f3
commit 79d2247b44
2 changed files with 41 additions and 25 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/modules/mc_pos_control/mc_pos_control_main.cpp
+16 -8
View File
@@ -473,18 +473,26 @@ void
MulticopterPositionControl::update_ref()
{
if (_local_pos.ref_timestamp != _ref_timestamp) {
double lat_sp, lon_sp;
float alt_sp;
if (_ref_timestamp != 0) {
/* reproject local position setpoint to new reference */
float dx, dy;
map_projection_project(&_ref_pos, _local_pos.ref_lat, _local_pos.ref_lon, &dx, &dy);
_pos_sp(0) -= dx;
_pos_sp(1) -= dy;
/* calculate current position setpoint in global frame */
map_projection_reproject(&_ref_pos, _pos_sp(0), _pos_sp(1), &lat_sp, &lon_sp);
alt_sp = _ref_alt - _pos_sp(2);
}
/* update local projection reference */
map_projection_init(&_ref_pos, _local_pos.ref_lat, _local_pos.ref_lon);
_ref_alt = _local_pos.ref_alt;
if (_ref_timestamp != 0) {
/* reproject position setpoint to new reference */
map_projection_project(&_ref_pos, lat_sp, lon_sp, &_pos_sp.data[0], &_pos_sp.data[1]);
_pos_sp(2) = -(alt_sp - _ref_alt);
}
_ref_timestamp = _local_pos.ref_timestamp;
map_projection_init(&_ref_pos, _local_pos.ref_lat, _local_pos.ref_lon);
_ref_alt = _local_pos.ref_alt;
}
}