diff --git a/src/modules/fw_dyn_soar_control/FixedwingPositionINDIControl.cpp b/src/modules/fw_dyn_soar_control/FixedwingPositionINDIControl.cpp index b901e4a8a5..952e782a16 100644 --- a/src/modules/fw_dyn_soar_control/FixedwingPositionINDIControl.cpp +++ b/src/modules/fw_dyn_soar_control/FixedwingPositionINDIControl.cpp @@ -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"); } diff --git a/src/modules/fw_dyn_soar_control/fw_dyn_soar_control_params.c b/src/modules/fw_dyn_soar_control/fw_dyn_soar_control_params.c index 60ddea4574..af63df0f9b 100644 --- a/src/modules/fw_dyn_soar_control/fw_dyn_soar_control_params.c +++ b/src/modules/fw_dyn_soar_control/fw_dyn_soar_control_params.c @@ -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); // ======================================================