fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-07-13 17:20:35 +08:00
Code Issues Packages Projects Releases Wiki Activity

position_setpoint: replaced loiter_direction integer by boolean (#20317)

Browse Source 
* position_setpoint: replaced loiter_direction integer by boolean (loiter_direction_counter_clockwise)

Signed-off-by: RomanBapst <bapstroman@gmail.com>
This commit is contained in:
Roman Bapst
2022-09-29 11:06:10 +02:00
committed by GitHub
parent 5d39fdba6d
commit 20457c5e2e
9 changed files with 36 additions and 36 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/modules/fw_pos_control_l1/FixedwingPositionControl.cpp
+6 -8
View File
@@ -1303,11 +1303,9 @@ FixedwingPositionControl::control_auto_loiter(const float control_interval, cons
/* waypoint is a loiter waypoint */
float loiter_radius = pos_sp_curr.loiter_radius;
uint8_t loiter_direction = pos_sp_curr.loiter_direction;
if (fabsf(pos_sp_curr.loiter_radius) < FLT_EPSILON) {
loiter_radius = _param_nav_loiter_rad.get();
loiter_direction = signNoZero(loiter_radius);
}
const bool in_circle_mode = (_param_fw_use_npfg.get()) ? _npfg.circleMode() : _l1_control.circle_mode();
@@ -1336,13 +1334,15 @@ FixedwingPositionControl::control_auto_loiter(const float control_interval, cons
if (_param_fw_use_npfg.get()) {
_npfg.setAirspeedNom(target_airspeed * _eas2tas);
_npfg.setAirspeedMax(_param_fw_airspd_max.get() * _eas2tas);
_npfg.navigateLoiter(curr_wp_local, curr_pos_local, loiter_radius, loiter_direction, get_nav_speed_2d(ground_speed),
_npfg.navigateLoiter(curr_wp_local, curr_pos_local, loiter_radius, pos_sp_curr.loiter_direction_counter_clockwise,
get_nav_speed_2d(ground_speed),
_wind_vel);
_att_sp.roll_body = _npfg.getRollSetpoint();
target_airspeed = _npfg.getAirspeedRef() / _eas2tas;
} else {
_l1_control.navigate_loiter(curr_wp_local, curr_pos_local, loiter_radius, loiter_direction,
_l1_control.navigate_loiter(curr_wp_local, curr_pos_local, loiter_radius,
pos_sp_curr.loiter_direction_counter_clockwise,
get_nav_speed_2d(ground_speed));
_att_sp.roll_body = _l1_control.get_roll_setpoint();
}
@@ -2739,15 +2739,13 @@ void FixedwingPositionControl::publishOrbitStatus(const position_setpoint_s pos_
{
orbit_status_s orbit_status{};
orbit_status.timestamp = hrt_absolute_time();
float loiter_radius = pos_sp.loiter_radius;
int8_t loiter_direction = pos_sp.loiter_direction;
float loiter_radius = pos_sp.loiter_radius * (pos_sp.loiter_direction_counter_clockwise ? -1.f : 1.f);
if (fabsf(loiter_radius) < FLT_EPSILON) {
loiter_radius = _param_nav_loiter_rad.get();
loiter_direction = signNoZero(loiter_radius);
}
orbit_status.radius = static_cast<float>(loiter_direction) * loiter_radius;
orbit_status.radius = 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);