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

Fix confusion between trajectory_setpoint and vehicle_local_postion_setpoint

Browse Source
This commit is contained in:
Matthias Grob
2022-10-17 16:07:11 +02:00
committed by Daniel Agar
parent 7c237fca74
commit 75c63aee2a
13 changed files with 91 additions and 157 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/modules/mc_pos_control/MulticopterPositionControl.cpp
+23 -52
View File
@@ -52,7 +52,6 @@ MulticopterPositionControl::MulticopterPositionControl(bool vtol) :
{
parameters_update(true);
_tilt_limit_slew_rate.setSlewRate(.2f);
reset_setpoint_to_nan(_setpoint);
_takeoff_status_pub.advertise();
}
@@ -355,7 +354,7 @@ void MulticopterPositionControl::Run()
} else if (previous_position_control_enabled && !_vehicle_control_mode.flag_multicopter_position_control_enabled) {
// clear existing setpoint when controller is no longer active
reset_setpoint_to_nan(_setpoint);
_setpoint = PositionControl::empty_trajectory_setpoint;
}
}
}
@@ -372,43 +371,26 @@ void MulticopterPositionControl::Run()
}
}
if (_trajectory_setpoint_sub.updated()) {
trajectory_setpoint_s trajectory_setpoint;
if (_trajectory_setpoint_sub.copy(&trajectory_setpoint)) {
_setpoint.timestamp = trajectory_setpoint.timestamp;
_setpoint.x = trajectory_setpoint.position[0];
_setpoint.y = trajectory_setpoint.position[1];
_setpoint.z = trajectory_setpoint.position[2];
_setpoint.vx = trajectory_setpoint.velocity[0];
_setpoint.vy = trajectory_setpoint.velocity[1];
_setpoint.vz = trajectory_setpoint.velocity[2];
_setpoint.acceleration[0] = trajectory_setpoint.acceleration[0];
_setpoint.acceleration[1] = trajectory_setpoint.acceleration[1];
_setpoint.acceleration[2] = trajectory_setpoint.acceleration[2];
_setpoint.yaw = trajectory_setpoint.yaw;
_setpoint.yawspeed = trajectory_setpoint.yawspeed;
}
}
_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.vx += vehicle_local_position.delta_vxy[0];
_setpoint.vy += vehicle_local_position.delta_vxy[1];
_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.vz += vehicle_local_position.delta_vz;
_setpoint.velocity[2] += vehicle_local_position.delta_vz;
}
if (vehicle_local_position.xy_reset_counter != _xy_reset_counter) {
_setpoint.x += vehicle_local_position.delta_xy[0];
_setpoint.y += vehicle_local_position.delta_xy[1];
_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.z += vehicle_local_position.delta_z;
_setpoint.position[2] += vehicle_local_position.delta_z;
}
if (vehicle_local_position.heading_reset_counter != _heading_reset_counter) {
@@ -463,13 +445,13 @@ void MulticopterPositionControl::Run()
const bool want_takeoff = _vehicle_control_mode.flag_armed
&& (vehicle_local_position.timestamp_sample < _setpoint.timestamp + 1_s);
if (want_takeoff && PX4_ISFINITE(_setpoint.z)
&& (_setpoint.z < states.position(2))) {
if (want_takeoff && PX4_ISFINITE(_setpoint.position[2])
&& (_setpoint.position[2] < states.position(2))) {
_vehicle_constraints.want_takeoff = true;
} else if (want_takeoff && PX4_ISFINITE(_setpoint.vz)
&& (_setpoint.vz < 0.f)) {
} else if (want_takeoff && PX4_ISFINITE(_setpoint.velocity[2])
&& (_setpoint.velocity[2] < 0.f)) {
_vehicle_constraints.want_takeoff = true;
@@ -507,7 +489,7 @@ void MulticopterPositionControl::Run()
if (not_taken_off || flying_but_ground_contact) {
// we are not flying yet and need to avoid any corrections
reset_setpoint_to_nan(_setpoint);
_setpoint = PositionControl::empty_trajectory_setpoint;
_setpoint.timestamp = vehicle_local_position.timestamp_sample;
Vector3f(0.f, 0.f, 100.f).copyTo(_setpoint.acceleration); // High downwards acceleration to make sure there's no thrust
@@ -544,15 +526,15 @@ void MulticopterPositionControl::Run()
_control.setInputSetpoint(_setpoint);
// update states
if (!PX4_ISFINITE(_setpoint.z)
&& PX4_ISFINITE(_setpoint.vz) && (fabsf(_setpoint.vz) > FLT_EPSILON)
if (!PX4_ISFINITE(_setpoint.position[2])
&& PX4_ISFINITE(_setpoint.velocity[2]) && (fabsf(_setpoint.velocity[2]) > FLT_EPSILON)
&& PX4_ISFINITE(vehicle_local_position.z_deriv) && vehicle_local_position.z_valid && vehicle_local_position.v_z_valid) {
// A change in velocity is demanded and the altitude is not controlled.
// Set velocity to the derivative of position
// because it has less bias but blend it in across the landing speed range
// < MPC_LAND_SPEED: ramp up using altitude derivative without a step
// >= MPC_LAND_SPEED: use altitude derivative
float weighting = fminf(fabsf(_setpoint.vz) / _param_mpc_land_speed.get(), 1.f);
float weighting = fminf(fabsf(_setpoint.velocity[2]) / _param_mpc_land_speed.get(), 1.f);
states.velocity(2) = vehicle_local_position.z_deriv * weighting + vehicle_local_position.vz * (1.f - weighting);
}
@@ -603,7 +585,7 @@ void MulticopterPositionControl::Run()
perf_end(_cycle_perf);
}
vehicle_local_position_setpoint_s MulticopterPositionControl::generateFailsafeSetpoint(const hrt_abstime &now,
trajectory_setpoint_s MulticopterPositionControl::generateFailsafeSetpoint(const hrt_abstime &now,
const PositionControlStates &states)
{
// do not warn while we are disarmed, as we might not have valid setpoints yet
@@ -614,13 +596,12 @@ vehicle_local_position_setpoint_s MulticopterPositionControl::generateFailsafeSe
_last_warn = now;
}
vehicle_local_position_setpoint_s failsafe_setpoint{};
reset_setpoint_to_nan(failsafe_setpoint);
trajectory_setpoint_s failsafe_setpoint = PositionControl::empty_trajectory_setpoint;
failsafe_setpoint.timestamp = now;
if (PX4_ISFINITE(states.velocity(0)) && PX4_ISFINITE(states.velocity(1))) {
// don't move along xy
failsafe_setpoint.vx = failsafe_setpoint.vy = 0.f;
failsafe_setpoint.velocity[0] = failsafe_setpoint.velocity[1] = 0.f;
if (warn) {
PX4_WARN("Failsafe: stop and wait");
@@ -629,7 +610,7 @@ vehicle_local_position_setpoint_s MulticopterPositionControl::generateFailsafeSe
} else {
// descend with land speed since we can't stop
failsafe_setpoint.acceleration[0] = failsafe_setpoint.acceleration[1] = 0.f;
failsafe_setpoint.vz = _param_mpc_land_speed.get();
failsafe_setpoint.velocity[2] = _param_mpc_land_speed.get();
if (warn) {
PX4_WARN("Failsafe: blind land");
@@ -638,13 +619,13 @@ vehicle_local_position_setpoint_s MulticopterPositionControl::generateFailsafeSe
if (PX4_ISFINITE(states.velocity(2))) {
// don't move along z if we can stop in all dimensions
if (!PX4_ISFINITE(failsafe_setpoint.vz)) {
failsafe_setpoint.vz = 0.f;
if (!PX4_ISFINITE(failsafe_setpoint.velocity[2])) {
failsafe_setpoint.velocity[2] = 0.f;
}
} else {
// emergency descend with a bit below hover thrust
failsafe_setpoint.vz = NAN;
failsafe_setpoint.velocity[2] = NAN;
failsafe_setpoint.acceleration[2] = .3f;
if (warn) {
@@ -655,16 +636,6 @@ vehicle_local_position_setpoint_s MulticopterPositionControl::generateFailsafeSe
return failsafe_setpoint;
}
void MulticopterPositionControl::reset_setpoint_to_nan(vehicle_local_position_setpoint_s &setpoint)
{
setpoint.timestamp = 0;
setpoint.x = setpoint.y = setpoint.z = NAN;
setpoint.yaw = setpoint.yawspeed = NAN;
setpoint.vx = setpoint.vy = setpoint.vz = NAN;
setpoint.acceleration[0] = setpoint.acceleration[1] = setpoint.acceleration[2] = NAN;
setpoint.thrust[0] = setpoint.thrust[1] = setpoint.thrust[2] = NAN;
}
int MulticopterPositionControl::task_spawn(int argc, char *argv[])
{
bool vtol = false;