fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-10-15 01:03:35 +08:00
Code Issues Packages Projects Releases Wiki Activity

corrected local_frame projection error

Browse Source
This commit is contained in:
Marvin Harms
2022-06-13 11:01:39 +02:00
parent df9291a73c
commit 80f43de103
2 changed files with 26 additions and 18 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/modules/fw_dyn_soar_control/FixedwingPositionINDIControl.cpp
+12 -4
View File
@@ -154,9 +154,17 @@ FixedwingPositionINDIControl::parameters_update()
_origin_lat = _param_origin_lat.get();
_origin_lon = _param_origin_lon.get();
_origin_alt = _param_origin_alt.get();
if (map_projection_project(&_global_local_proj_ref, _origin_lat, _origin_lon, &_origin_E, &_origin_N)) {
// TODO: do stuff
}
// check if local NED reference frame origin has changed:
// || (_local_pos.vxy_reset_counter != _pos_reset_counter
// initialize projection
map_projection_init_timestamped(&_global_local_proj_ref, _local_pos.ref_lat, _local_pos.ref_lon,
_local_pos.ref_timestamp);
// project the origin of the soaring ENU frame to the current NED frame
map_projection_project(&_global_local_proj_ref, _origin_lat, _origin_lon, &_origin_N, &_origin_E);
_origin_D = _local_pos.ref_alt - _origin_alt;
PX4_INFO("local reference frame updated");
_loiter = _param_loiter.get();
_select_trajectory(0.0f);
@@ -540,7 +548,7 @@ FixedwingPositionINDIControl::Run()
map_projection_init_timestamped(&_global_local_proj_ref, _local_pos.ref_lat, _local_pos.ref_lon,
_local_pos.ref_timestamp);
// project the origin of the soaring ENU frame to the current NED frame
map_projection_project(&_global_local_proj_ref, _origin_lat, _origin_lon, &_origin_E, &_origin_N);
map_projection_project(&_global_local_proj_ref, _origin_lat, _origin_lon, &_origin_N, &_origin_E);
_origin_D = _local_pos.ref_alt - _origin_alt;
PX4_INFO("local reference frame updated");
}
src/modules/fw_dyn_soar_control/fw_dyn_soar_control_params.c
+14 -14
View File
@@ -510,25 +510,25 @@ PARAM_DEFINE_FLOAT(DS_K_DAMP_YAW, 0.0f);
* latitude of trajectory start point (WGS84)
*
* @unit
* @min -180
* @max 180
* @decimal 7
* @increment 0.0000001
* @group FW DYN SOAR Control
*/
PARAM_DEFINE_FLOAT(DS_ORIGIN_LAT, 47.39797f);
/**
* longitude of trajectory start point (WGS84)
*
* @unit
* @min -90
* @max 90
* @decimal 7
* @increment 0.0000001
* @group FW DYN SOAR Control
*/
PARAM_DEFINE_FLOAT(DS_ORIGIN_LON, 8.54554f);
PARAM_DEFINE_FLOAT(DS_ORIGIN_LAT, 47.3135000f);
/**
* longitude of trajectory start point (WGS84)
*
* @unit
* @min -180
* @max 180
* @decimal 7
* @increment 0.0000001
* @group FW DYN SOAR Control
*/
PARAM_DEFINE_FLOAT(DS_ORIGIN_LON, 8.8101000f);
/**
* altitude of trajectory start point (WGS84)
@@ -540,7 +540,7 @@ PARAM_DEFINE_FLOAT(DS_ORIGIN_LON, 8.54554f);
* @increment 0.1
* @group FW DYN SOAR Control
*/
PARAM_DEFINE_FLOAT(DS_ORIGIN_ALT, 488.0f);
PARAM_DEFINE_FLOAT(DS_ORIGIN_ALT, 537.0f);
// ======================================================