Skip to content
New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Log vehicle local position setpoints for fixedwing position control #18929

Merged
merged 2 commits into from
Jan 3, 2022
Merged

Log vehicle local position setpoints for fixedwing position control #18929

Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
2 commits
Select commit Hold shift + click to select a range
fb52386
Log vehicle local position setpoints
Jaeyoung-Lim Dec 31, 2021
30a3ca9
Disable local vehicle setpoints while in transition
Jaeyoung-Lim Jan 1, 2022
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
48 changes: 39 additions & 9 deletions src/modules/fw_pos_control_l1/FixedwingPositionControl.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -859,6 +859,10 @@ FixedwingPositionControl::control_auto(const hrt_abstime &now, const Vector2d &c
if (use_tecs_pitch) {
_att_sp.pitch_body = get_tecs_pitch();
}

if (!_vehicle_status.in_transition_to_fw) {
publishLocalPositionSetpoint(current_sp);
}
}

void
Expand Down Expand Up @@ -1918,17 +1922,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)) {

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;
}
_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) {
vehicle_local_position_setpoint_s trajectory_setpoint;

if (_trajectory_setpoint_sub.update(&trajectory_setpoint)) {
Expand Down Expand Up @@ -2237,6 +2240,33 @@ 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.timestamp = hrt_absolute_time();
local_position_setpoint.x = current_setpoint(0);
Copy link
Member

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

populate the unused fields with NANs?

local_position_setpoint.vx = NAN, etc

Copy link
Member

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

Alternatively does populating any other existing data make sense (for casual interest)?

  • acceleration from L1?
  • velocities from TECS (remove wind?)

Copy link
Member Author

@Jaeyoung-Lim Jaeyoung-Lim Jan 1, 2022 •
edited
Loading

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

I think for now IMO there is not much more to add - e.g. velocity setpoints are not used byt the controller / accelerations are not used by the controller. I think we can add these by cleaning up the controller one by one (e.g. change L1 controller to command accelerations, rather than roll angles)

Copy link
Contributor

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

Some stuff - e.g. lateral acceleration- will be published with the new L1 controller (#18810) in npfg_status. I would also find it nicer if we publish whatever we can in local_position_setpoint. That though also requires some controller interface alignment - using local coordinates prob is one thing of that.

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_pos_sp_pub.publish(local_position_setpoint);
}
}

void FixedwingPositionControl::publishOrbitStatus(const position_setpoint_s pos_sp)
{
orbit_status_s orbit_status{};
Expand Down
2 changes: 2 additions & 0 deletions src/modules/fw_pos_control_l1/FixedwingPositionControl.hpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -154,6 +154,7 @@ class FixedwingPositionControl final : public ModuleBase<FixedwingPositionContro
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
Expand Down Expand Up @@ -285,6 +286,7 @@ class FixedwingPositionControl final : public ModuleBase<FixedwingPositionContro
void status_publish();
void landing_status_publish();
void tecs_status_publish();
void publishLocalPositionSetpoint(const position_setpoint_s &current_waypoint);

void abort_landing(bool abort);

Expand Down