fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-04-14 10:07:39 +08:00
Code Issues Packages Projects Releases Wiki Activity

FW Positon Controller: set references to 0 if not provided by local_position (#22101)

Browse Source 
* FW Positon Controller: set altitude_ref to 0 if not provided by GPS

Signed-off-by: Silvan Fuhrer <silvan@auterion.com>

* FW Positon Controller: set lat/lon reference to 0 if not provided in local_position

Signed-off-by: Silvan Fuhrer <silvan@auterion.com>

---------

Signed-off-by: Silvan Fuhrer <silvan@auterion.com>
This commit is contained in:
Silvan Fuhrer 2023-09-20 15:45:32 +02:00 committed by GitHub
parent 7ff00db9c5
commit ec7db4b30d
No known key found for this signature in database
GPG Key ID: 4AEE18F83AFDEB23
2 changed files with 20 additions and 5 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

22
src/modules/fw_pos_control/FixedwingPositionControl.cpp
View File

@ -2144,6 +2144,13 @@ FixedwingPositionControl::Run()
_current_longitude = gpos.lon;
}
if (_local_pos.z_global && PX4_ISFINITE(_local_pos.ref_alt)) {
_reference_altitude = _local_pos.ref_alt;
} else {
_reference_altitude = 0.f;
}
_current_altitude = -_local_pos.z + _local_pos.ref_alt; // Altitude AMSL in meters
// handle estimator reset events. we only adjust setpoins for manual modes
@ -2171,9 +2178,16 @@ FixedwingPositionControl::Run()
|| (_global_local_proj_ref.getProjectionReferenceTimestamp() != _local_pos.ref_timestamp)
|| (_local_pos.vxy_reset_counter != _pos_reset_counter)) {
_global_local_proj_ref.initReference(_local_pos.ref_lat, _local_pos.ref_lon,
double reference_latitude = 0.;
double reference_longitude = 0.;
if (_local_pos.xy_global && PX4_ISFINITE(_local_pos.ref_lat) && PX4_ISFINITE(_local_pos.ref_lon)) {
reference_latitude = _local_pos.ref_lat;
reference_longitude = _local_pos.ref_lon;
}
_global_local_proj_ref.initReference(reference_latitude, reference_longitude,
_local_pos.ref_timestamp);
_global_local_alt0 = _local_pos.ref_alt;
}
if (_control_mode.flag_control_offboard_enabled) {
@ -2202,7 +2216,7 @@ FixedwingPositionControl::Run()
_pos_sp_triplet.current.type = position_setpoint_s::SETPOINT_TYPE_POSITION;
_pos_sp_triplet.current.lat = lat;
_pos_sp_triplet.current.lon = lon;
_pos_sp_triplet.current.alt = _global_local_alt0 - trajectory_setpoint.position[2];
_pos_sp_triplet.current.alt = _reference_altitude - trajectory_setpoint.position[2];
}
}
@ -2782,7 +2796,7 @@ void FixedwingPositionControl::publishLocalPositionSetpoint(const position_setpo
local_position_setpoint.x = current_setpoint(0);
local_position_setpoint.y = current_setpoint(1);
local_position_setpoint.z = _global_local_alt0 - current_waypoint.alt;
local_position_setpoint.z = _reference_altitude - current_waypoint.alt;
local_position_setpoint.yaw = NAN;
local_position_setpoint.yawspeed = NAN;
local_position_setpoint.vx = NAN;

3
src/modules/fw_pos_control/FixedwingPositionControl.hpp
View File

@ -266,7 +266,8 @@ private:
float _body_velocity_x{0.f};
MapProjection _global_local_proj_ref{};
float _global_local_alt0{NAN};
float _reference_altitude{NAN}; // [m AMSL] altitude of the local projection reference point
bool _landed{true};