fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-11-05 03:01:35 +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-03 11:09:06 +01:00
committed by JaeyoungLim
parent 4127dfa791
commit bdec85fdd0
2 changed files with 17 additions and 4 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/modules/fw_pos_control_l1/FixedwingPositionControl.cpp
+16 -3
View File
@@ -864,7 +864,9 @@ FixedwingPositionControl::control_auto(const hrt_abstime &now, const Vector2d &c
_att_sp.pitch_body = get_tecs_pitch(); _att_sp.pitch_body = get_tecs_pitch();
} }
publishLocalPositionSetpoint(current_sp); if (!_vehicle_status.in_transition_to_fw) {
publishLocalPositionSetpoint(current_sp);
}
} }
void void
@@ -2322,18 +2324,29 @@ FixedwingPositionControl::tecs_update_pitch_throttle(const hrt_abstime &now, flo
tecs_status_publish(); 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()) { if (_global_local_proj_ref.isInitialized()) {
Vector2f current_setpoint = _global_local_proj_ref.project(current_waypoint.lat, current_waypoint.lon); Vector2f current_setpoint = _global_local_proj_ref.project(current_waypoint.lat, current_waypoint.lon);
vehicle_local_position_setpoint_s local_position_setpoint{}; 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.x = current_setpoint(0);
local_position_setpoint.y = current_setpoint(1); local_position_setpoint.y = current_setpoint(1);
local_position_setpoint.z = _global_local_alt0 - current_waypoint.alt; 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[0] = _att_sp.thrust_body[0];
local_position_setpoint.thrust[1] = _att_sp.thrust_body[1]; local_position_setpoint.thrust[1] = _att_sp.thrust_body[1];
local_position_setpoint.thrust[2] = _att_sp.thrust_body[2]; 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); _local_pos_sp_pub.publish(local_position_setpoint);
} }
} }
src/modules/fw_pos_control_l1/FixedwingPositionControl.hpp
+1 -1
View File
@@ -286,7 +286,7 @@ private:
void status_publish(); void status_publish();
void landing_status_publish(); void landing_status_publish();
void tecs_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); void abort_landing(bool abort);