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

fw_pos_control_l1: OFFBOARD fully populate position_setpoint_triplet

Browse Source
This commit is contained in:
Daniel Agar
2021-03-16 00:58:04 -04:00
committed by GitHub
parent bcf93304c7
commit 29d4ad1848
1 changed files with 42 additions and 25 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/modules/fw_pos_control_l1/FixedwingPositionControl.cpp
+42 -25
View File
@@ -1687,9 +1687,48 @@ FixedwingPositionControl::Run()
_alt_reset_counter = _local_pos.vz_reset_counter;
_pos_reset_counter = _local_pos.vxy_reset_counter;
if (_pos_sp_triplet_sub.update(&_pos_sp_triplet)) {
// reset the altitude foh (first order hold) logic
_min_current_sp_distance_xy = FLT_MAX;
if (_control_mode.flag_control_offboard_enabled) {
// Convert Local setpoints to global setpoints
if (!map_projection_initialized(&_global_local_proj_ref)
|| (_global_local_proj_ref.timestamp != _local_pos.ref_timestamp)) {
map_projection_init_timestamped(&_global_local_proj_ref, _local_pos.ref_lat, _local_pos.ref_lon,
_local_pos.ref_timestamp);
_global_local_alt0 = _local_pos.ref_alt;
}
vehicle_local_position_setpoint_s trajectory_setpoint;
if (_trajectory_setpoint_sub.update(&trajectory_setpoint)) {
if (PX4_ISFINITE(trajectory_setpoint.x) && PX4_ISFINITE(trajectory_setpoint.y) && PX4_ISFINITE(trajectory_setpoint.z)) {
double lat;
double lon;
if (map_projection_reproject(&_global_local_proj_ref, trajectory_setpoint.x, trajectory_setpoint.y, &lat, &lon) == 0) {
_pos_sp_triplet = {}; // clear any existing
_pos_sp_triplet.timestamp = trajectory_setpoint.timestamp;
_pos_sp_triplet.current.timestamp = trajectory_setpoint.timestamp;
_pos_sp_triplet.current.valid = true;
_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.z;
_pos_sp_triplet.current.cruising_speed = NAN; // ignored
_pos_sp_triplet.current.cruising_throttle = NAN; // ignored
}
} else {
mavlink_log_critical(&_mavlink_log_pub, "Invalid offboard setpoint");
}
}
} else {
if (_pos_sp_triplet_sub.update(&_pos_sp_triplet)) {
// reset the altitude foh (first order hold) logic
_min_current_sp_distance_xy = FLT_MAX;
}
}
airspeed_poll();
@@ -1711,28 +1750,6 @@ FixedwingPositionControl::Run()
Vector2d curr_pos(_current_latitude, _current_longitude);
Vector2f ground_speed(_local_pos.vx, _local_pos.vy);
// Convert Local setpoints to global setpoints
if (_control_mode.flag_control_offboard_enabled) {
if (!map_projection_initialized(&_global_local_proj_ref)
|| (_global_local_proj_ref.timestamp != _local_pos.ref_timestamp)) {
map_projection_init_timestamped(&_global_local_proj_ref, _local_pos.ref_lat, _local_pos.ref_lon,
_local_pos.ref_timestamp);
_global_local_alt0 = _local_pos.ref_alt;
}
vehicle_local_position_setpoint_s trajectory_setpoint;
if (_trajectory_setpoint_sub.update(&trajectory_setpoint)) {
map_projection_reproject(&_global_local_proj_ref,
trajectory_setpoint.x, trajectory_setpoint.y,
&_pos_sp_triplet.current.lat, &_pos_sp_triplet.current.lon);
_pos_sp_triplet.current.alt = _global_local_alt0 - trajectory_setpoint.z;
_pos_sp_triplet.current.valid = true;
}
}
/*
* Attempt to control position, on success (= sensors present and not in manual mode),
* publish setpoint.