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

GotoControl: add go-to control interface to mc position controller

Browse Source 
goto control class handles smoothing of goto setpoints, outputs trajectory setpoint for mc pos control
some minor encapsulation done in mc pos control for readability
new param MPC_YAWAAUTO_MAX limiting heading accelerations in heading smoother
This commit is contained in:
Thomas Stastny
2023-11-15 11:29:10 +01:00
committed by Matthias Grob
parent e47aba8bc9
commit 4b920a6628
9 changed files with 550 additions and 52 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/modules/mc_pos_control/MulticopterPositionControl.cpp
+93 -47
View File
@@ -368,52 +368,57 @@ void MulticopterPositionControl::Run()
}
}
_trajectory_setpoint_sub.update(&_setpoint);
// adjust existing (or older) setpoint with any EKF reset deltas
if ((_setpoint.timestamp != 0) && (_setpoint.timestamp < vehicle_local_position.timestamp)) {
if (vehicle_local_position.vxy_reset_counter != _vxy_reset_counter) {
_setpoint.velocity[0] += vehicle_local_position.delta_vxy[0];
_setpoint.velocity[1] += vehicle_local_position.delta_vxy[1];
}
if (vehicle_local_position.vz_reset_counter != _vz_reset_counter) {
_setpoint.velocity[2] += vehicle_local_position.delta_vz;
}
if (vehicle_local_position.xy_reset_counter != _xy_reset_counter) {
_setpoint.position[0] += vehicle_local_position.delta_xy[0];
_setpoint.position[1] += vehicle_local_position.delta_xy[1];
}
if (vehicle_local_position.z_reset_counter != _z_reset_counter) {
_setpoint.position[2] += vehicle_local_position.delta_z;
}
if (vehicle_local_position.heading_reset_counter != _heading_reset_counter) {
_setpoint.yaw = wrap_pi(_setpoint.yaw + vehicle_local_position.delta_heading);
}
}
if (vehicle_local_position.vxy_reset_counter != _vxy_reset_counter) {
_vel_x_deriv.reset();
_vel_y_deriv.reset();
}
if (vehicle_local_position.vz_reset_counter != _vz_reset_counter) {
_vel_z_deriv.reset();
}
// save latest reset counters
_vxy_reset_counter = vehicle_local_position.vxy_reset_counter;
_vz_reset_counter = vehicle_local_position.vz_reset_counter;
_xy_reset_counter = vehicle_local_position.xy_reset_counter;
_z_reset_counter = vehicle_local_position.z_reset_counter;
_heading_reset_counter = vehicle_local_position.heading_reset_counter;
PositionControlStates states{set_vehicle_states(vehicle_local_position)};
_trajectory_setpoint_sub.update(&_setpoint);
const hrt_abstime last_goto_timestamp = _goto_setpoint.timestamp;
_goto_setpoint_sub.update(&_goto_setpoint);
if ((_goto_setpoint.timestamp != 0)
&& (_goto_setpoint.timestamp >= _time_position_control_enabled)
&& (hrt_elapsed_time(&last_goto_timestamp) < 200_ms)
&& _vehicle_control_mode.flag_multicopter_position_control_enabled) {
// take position heading setpoint as priority over trajectory setpoint
if (_last_active_setpoint_interface == SetpointInterface::kTrajectory) {
_goto_control.resetPositionSmoother(states.position);
_goto_control.resetHeadingSmoother(states.yaw);
}
const GotoControl::VehicleConstraints vehicle_constraints = {
_param_mpc_xy_cruise.get(),
_param_mpc_z_v_auto_dn.get(),
_param_mpc_z_v_auto_up.get(),
_param_mpc_acc_hor.get(),
_param_mpc_acc_down_max.get(),
_param_mpc_acc_up_max.get(),
_param_mpc_jerk_auto.get(),
_param_mpc_yawrauto_max.get(),
_param_mpc_yawaauto_max.get()
};
_goto_control.setVehicleConstraints(vehicle_constraints);
_goto_control.setMaxAllowedHorizontalPositionError(_param_mpc_xy_err_max.get());
_goto_control.update(dt, states.position, states.yaw, _goto_setpoint, _setpoint);
// for logging
_trajectory_setpoint_pub.publish(_setpoint);
_vehicle_constraints.timestamp = hrt_absolute_time();
_vehicle_constraints.want_takeoff = false;
_vehicle_constraints.speed_up = _param_mpc_z_vel_max_up.get();
_vehicle_constraints.speed_down = _param_mpc_z_vel_max_dn.get();
_last_active_setpoint_interface = SetpointInterface::kGoto;
} else {
_last_active_setpoint_interface = SetpointInterface::kTrajectory;
_vehicle_constraints_sub.update(&_vehicle_constraints);
}
adjustSetpointForEKFResets(vehicle_local_position, _setpoint);
if (_vehicle_control_mode.flag_multicopter_position_control_enabled) {
// set failsafe setpoint if there hasn't been a new
@@ -428,9 +433,6 @@ void MulticopterPositionControl::Run()
if (_vehicle_control_mode.flag_multicopter_position_control_enabled
&& (_setpoint.timestamp >= _time_position_control_enabled)) {
// update vehicle constraints and handle smooth takeoff
_vehicle_constraints_sub.update(&_vehicle_constraints);
// fix to prevent the takeoff ramp to ramp to a too high value or get stuck because of NAN
// TODO: this should get obsolete once the takeoff limiting moves into the flight tasks
if (!PX4_ISFINITE(_vehicle_constraints.speed_up) || (_vehicle_constraints.speed_up > _param_mpc_z_vel_max_up.get())) {
@@ -633,6 +635,50 @@ trajectory_setpoint_s MulticopterPositionControl::generateFailsafeSetpoint(const
return failsafe_setpoint;
}
void MulticopterPositionControl::adjustSetpointForEKFResets(const vehicle_local_position_s &vehicle_local_position,
trajectory_setpoint_s &setpoint)
{
if ((setpoint.timestamp != 0) && (setpoint.timestamp < vehicle_local_position.timestamp)) {
if (vehicle_local_position.vxy_reset_counter != _vxy_reset_counter) {
setpoint.velocity[0] += vehicle_local_position.delta_vxy[0];
setpoint.velocity[1] += vehicle_local_position.delta_vxy[1];
}
if (vehicle_local_position.vz_reset_counter != _vz_reset_counter) {
setpoint.velocity[2] += vehicle_local_position.delta_vz;
}
if (vehicle_local_position.xy_reset_counter != _xy_reset_counter) {
setpoint.position[0] += vehicle_local_position.delta_xy[0];
setpoint.position[1] += vehicle_local_position.delta_xy[1];
}
if (vehicle_local_position.z_reset_counter != _z_reset_counter) {
setpoint.position[2] += vehicle_local_position.delta_z;
}
if (vehicle_local_position.heading_reset_counter != _heading_reset_counter) {
setpoint.yaw = wrap_pi(setpoint.yaw + vehicle_local_position.delta_heading);
}
}
if (vehicle_local_position.vxy_reset_counter != _vxy_reset_counter) {
_vel_x_deriv.reset();
_vel_y_deriv.reset();
}
if (vehicle_local_position.vz_reset_counter != _vz_reset_counter) {
_vel_z_deriv.reset();
}
// save latest reset counters
_vxy_reset_counter = vehicle_local_position.vxy_reset_counter;
_vz_reset_counter = vehicle_local_position.vz_reset_counter;
_xy_reset_counter = vehicle_local_position.xy_reset_counter;
_z_reset_counter = vehicle_local_position.z_reset_counter;
_heading_reset_counter = vehicle_local_position.heading_reset_counter;
}
int MulticopterPositionControl::task_spawn(int argc, char *argv[])
{
bool vtol = false;