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

Publish correct orbit status with goto (#19102)

Browse Source 
For fixedwings, the orbit status was publishing zero radius when a goto waypoint was being passed.

This commit corrects this by passing the default loiter radius as the guidance logic was using
This commit is contained in:
JaeyoungLim 2022-02-03 10:16:39 +01:00 committed by GitHub
parent 917910f3e2
commit f82c722653
No known key found for this signature in database
GPG Key ID: 4AEE18F83AFDEB23
1 changed files with 9 additions and 1 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

10
src/modules/fw_pos_control_l1/FixedwingPositionControl.cpp
View File

@ -2605,7 +2605,15 @@ void FixedwingPositionControl::publishOrbitStatus(const position_setpoint_s pos_
{
orbit_status_s orbit_status{};
orbit_status.timestamp = hrt_absolute_time();
orbit_status.radius = static_cast<float>(pos_sp.loiter_direction) * pos_sp.loiter_radius;
float loiter_radius = pos_sp.loiter_radius;
uint8_t loiter_direction = pos_sp.loiter_direction;
if (fabsf(loiter_radius) < FLT_EPSILON) {
loiter_radius = _param_nav_loiter_rad.get();
loiter_direction = (loiter_radius > 0) ? 1 : -1;
}
orbit_status.radius = static_cast<float>(loiter_direction) * loiter_radius;
orbit_status.frame = 0; // MAV_FRAME::MAV_FRAME_GLOBAL
orbit_status.x = static_cast<double>(pos_sp.lat);
orbit_status.y = static_cast<double>(pos_sp.lon);