Mirrors_Framework/PX4-Autopilot
1
0
Fork 0
mirror of https://github.com/PX4/PX4-Autopilot.git synced 2026-05-22 14:24:21 +08:00
Code Issues Actions 8 Packages Projects Releases Wiki Activity

Log vehicle local position setpoints

Browse Source 
This commit enables the local position setpoints to be logged by publishing vehicle_local_position_setpoint
This commit is contained in:
Jaeyoung-Lim
2021-12-31 09:15:59 +01:00
committed by JaeyoungLim
parent 52418f13b0
commit 4127dfa791
2 changed files with 28 additions and 9 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/modules/fw_pos_control_l1/FixedwingPositionControl.cpp
+26 -9
View File
@@ -863,6 +863,8 @@ FixedwingPositionControl::control_auto(const hrt_abstime &now, const Vector2d &c
if (use_tecs_pitch) {
_att_sp.pitch_body = get_tecs_pitch();
}
publishLocalPositionSetpoint(current_sp);
}
void
@@ -1987,17 +1989,16 @@ FixedwingPositionControl::Run()
_alt_reset_counter = _local_pos.vz_reset_counter;
_pos_reset_counter = _local_pos.vxy_reset_counter;
// Convert Local setpoints to global setpoints
if (!_global_local_proj_ref.isInitialized()
|| (_global_local_proj_ref.getProjectionReferenceTimestamp() != _local_pos.ref_timestamp)) {
_global_local_proj_ref.initReference(_local_pos.ref_lat, _local_pos.ref_lon,
_local_pos.ref_timestamp);
_global_local_alt0 = _local_pos.ref_alt;
}
if (_control_mode.flag_control_offboard_enabled) {
// Convert Local setpoints to global setpoints
if (!_global_local_proj_ref.isInitialized()
|| (_global_local_proj_ref.getProjectionReferenceTimestamp() != _local_pos.ref_timestamp)) {
_global_local_proj_ref.initReference(_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)) {
@@ -2321,6 +2322,22 @@ FixedwingPositionControl::tecs_update_pitch_throttle(const hrt_abstime &now, flo
tecs_status_publish();
}
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.x = current_setpoint(0);
local_position_setpoint.y = current_setpoint(1);
local_position_setpoint.z = _global_local_alt0 - current_waypoint.alt;
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);
}
}
void FixedwingPositionControl::publishOrbitStatus(const position_setpoint_s pos_sp)
{
orbit_status_s orbit_status{};
src/modules/fw_pos_control_l1/FixedwingPositionControl.hpp
+2
View File
@@ -154,6 +154,7 @@ private:
uORB::Subscription _vehicle_status_sub{ORB_ID(vehicle_status)};
uORB::Publication<vehicle_attitude_setpoint_s> _attitude_sp_pub;
uORB::Publication<vehicle_local_position_setpoint_s> _local_pos_sp_pub{ORB_ID(vehicle_local_position_setpoint)}; ///< vehicle local position setpoint publication
uORB::Publication<position_controller_status_s> _pos_ctrl_status_pub{ORB_ID(position_controller_status)}; ///< navigation capabilities publication
uORB::Publication<position_controller_landing_status_s> _pos_ctrl_landing_status_pub{ORB_ID(position_controller_landing_status)}; ///< landing status publication
uORB::Publication<tecs_status_s> _tecs_status_pub{ORB_ID(tecs_status)}; ///< TECS status publication
@@ -285,6 +286,7 @@ private:
void status_publish();
void landing_status_publish();
void tecs_status_publish();
void publishLocalPositionSetpoint(const position_setpoint_s current_waypoint);
void abort_landing(bool abort);