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

Disable local vehicle setpoints while in transition

Browse Source 
This commit adds disabling vehicle setpoints while in transition
This commit is contained in:
Jaeyoung-Lim 2022-01-01 19:48:01 +01:00 committed by JaeyoungLim
parent 4127dfa791
commit bdec85fdd0
2 changed files with 17 additions and 4 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

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

@ -864,7 +864,9 @@ FixedwingPositionControl::control_auto(const hrt_abstime &now, const Vector2d &c
_att_sp.pitch_body = get_tecs_pitch();
}
publishLocalPositionSetpoint(current_sp);
if (!_vehicle_status.in_transition_to_fw) {
publishLocalPositionSetpoint(current_sp);
}
}
void
@ -2322,18 +2324,29 @@ FixedwingPositionControl::tecs_update_pitch_throttle(const hrt_abstime &now, flo
tecs_status_publish();
}
void FixedwingPositionControl::publishLocalPositionSetpoint(const position_setpoint_s current_waypoint)
void FixedwingPositionControl::publishLocalPositionSetpoint(const position_setpoint_s &current_waypoint)
{
if (_global_local_proj_ref.isInitialized()) {
Vector2f current_setpoint = _global_local_proj_ref.project(current_waypoint.lat, current_waypoint.lon);
vehicle_local_position_setpoint_s local_position_setpoint{};
local_position_setpoint.timestamp = hrt_absolute_time();
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.yaw = NAN;
local_position_setpoint.yawspeed = NAN;
local_position_setpoint.vx = NAN;
local_position_setpoint.vy = NAN;
local_position_setpoint.vz = NAN;
local_position_setpoint.acceleration[0] = NAN;
local_position_setpoint.acceleration[1] = NAN;
local_position_setpoint.acceleration[2] = NAN;
local_position_setpoint.jerk[0] = NAN;
local_position_setpoint.jerk[1] = NAN;
local_position_setpoint.jerk[2] = NAN;
local_position_setpoint.thrust[0] = _att_sp.thrust_body[0];
local_position_setpoint.thrust[1] = _att_sp.thrust_body[1];
local_position_setpoint.thrust[2] = _att_sp.thrust_body[2];
local_position_setpoint.timestamp = hrt_absolute_time();
_local_pos_sp_pub.publish(local_position_setpoint);
}
}

2
src/modules/fw_pos_control_l1/FixedwingPositionControl.hpp
View File

@ -286,7 +286,7 @@ private:
void status_publish();
void landing_status_publish();
void tecs_status_publish();
void publishLocalPositionSetpoint(const position_setpoint_s current_waypoint);
void publishLocalPositionSetpoint(const position_setpoint_s &current_waypoint);
void abort_landing(bool abort);