dark0707
/
px4-firmware
forked from Archive/PX4-Autopilot
1
0
Fork 0
Code Pull Requests Activity

vehicle_local_position_setpoint: make acceleration and jerk arrays

This commit is contained in:
Matthias Grob 2019-10-22 16:41:52 +02:00
parent 0062390c46
commit 9c25b987bc
9 changed files with 25 additions and 41 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

8
msg/vehicle_local_position_setpoint.msg
View File

@ -11,12 +11,8 @@ float32 yawspeed # in radians/sec
float32 vx # in meters/sec
float32 vy # in meters/sec
float32 vz # in meters/sec
float32 acc_x # in meters/(sec*sec)
float32 acc_y # in meters/(sec*sec)
float32 acc_z # in meters/(sec*sec)
float32 jerk_x # in meters/(sec*sec*sec)
float32 jerk_y # in meters/(sec*sec*sec)
float32 jerk_z # in meters/(sec*sec*sec)
float32[3] acceleration # in meters/sec^2
float32[3] jerk # in meters/sec^3
float32[3] thrust # normalized thrust vector in NED
# TOPICS vehicle_local_position_setpoint trajectory_setpoint

11
src/lib/FlightTasks/tasks/AutoLineSmoothVel/FlightTaskAutoLineSmoothVel.cpp
View File

@ -46,12 +46,11 @@ bool FlightTaskAutoLineSmoothVel::activate(vehicle_local_position_setpoint_s las
bool ret = FlightTaskAutoMapper2::activate(last_setpoint);
checkSetpoints(last_setpoint);
const Vector3f accel_prev(last_setpoint.acc_x, last_setpoint.acc_y, last_setpoint.acc_z);
const Vector3f vel_prev(last_setpoint.vx, last_setpoint.vy, last_setpoint.vz);
const Vector3f pos_prev(last_setpoint.x, last_setpoint.y, last_setpoint.z);
for (int i = 0; i < 3; ++i) {
_trajectory[i].reset(accel_prev(i), vel_prev(i), pos_prev(i));
_trajectory[i].reset(last_setpoint.acceleration[i], vel_prev(i), pos_prev(i));
}
_yaw_sp_prev = last_setpoint.yaw;
@ -87,11 +86,9 @@ void FlightTaskAutoLineSmoothVel::checkSetpoints(vehicle_local_position_setpoint
if (!PX4_ISFINITE(setpoints.vz)) { setpoints.vz = _velocity(2); }
// No acceleration estimate available, set to zero if the setpoint is NAN
if (!PX4_ISFINITE(setpoints.acc_x)) { setpoints.acc_x = 0.f; }
if (!PX4_ISFINITE(setpoints.acc_y)) { setpoints.acc_y = 0.f; }
if (!PX4_ISFINITE(setpoints.acc_z)) { setpoints.acc_z = 0.f; }
for (int i = 0; i < 3; i++) {
if (!PX4_ISFINITE(setpoints.acceleration[i])) { setpoints.acceleration[i] = 0.f; }
}
if (!PX4_ISFINITE(setpoints.yaw)) { setpoints.yaw = _yaw; }
}

12
src/lib/FlightTasks/tasks/FlightTask/FlightTask.cpp
View File

@ -4,7 +4,7 @@
constexpr uint64_t FlightTask::_timeout;
// First index of empty_setpoint corresponds to time-stamp and requires a finite number.
const vehicle_local_position_setpoint_s FlightTask::empty_setpoint = {0, NAN, NAN, NAN, NAN, NAN, NAN, NAN, NAN, NAN, NAN, NAN, NAN, NAN, NAN, {NAN, NAN, NAN}};
const vehicle_local_position_setpoint_s FlightTask::empty_setpoint = {0, NAN, NAN, NAN, NAN, NAN, NAN, NAN, NAN, {NAN, NAN, NAN}, {NAN, NAN, NAN}, {NAN, NAN, NAN}, {}};
const vehicle_constraints_s FlightTask::empty_constraints = {0, NAN, NAN, NAN, NAN, NAN, NAN, NAN, false, {}};
const landing_gear_s FlightTask::empty_landing_gear_default_keep = {0, landing_gear_s::GEAR_KEEP, {}};
@ -92,14 +92,8 @@ const vehicle_local_position_setpoint_s FlightTask::getPositionSetpoint()
vehicle_local_position_setpoint.vy = _velocity_setpoint(1);
vehicle_local_position_setpoint.vz = _velocity_setpoint(2);
vehicle_local_position_setpoint.acc_x = _acceleration_setpoint(0);
vehicle_local_position_setpoint.acc_y = _acceleration_setpoint(1);
vehicle_local_position_setpoint.acc_z = _acceleration_setpoint(2);
vehicle_local_position_setpoint.jerk_x = _jerk_setpoint(0);
vehicle_local_position_setpoint.jerk_y = _jerk_setpoint(1);
vehicle_local_position_setpoint.jerk_z = _jerk_setpoint(2);
_acceleration_setpoint.copyTo(vehicle_local_position_setpoint.acceleration);
_jerk_setpoint.copyTo(vehicle_local_position_setpoint.jerk);
_thrust_setpoint.copyTo(vehicle_local_position_setpoint.thrust);
vehicle_local_position_setpoint.yaw = _yaw_setpoint;
vehicle_local_position_setpoint.yawspeed = _yawspeed_setpoint;

5
src/lib/FlightTasks/tasks/ManualAltitudeSmoothVel/FlightTaskManualAltitudeSmoothVel.cpp
View File

@ -48,7 +48,7 @@ bool FlightTaskManualAltitudeSmoothVel::activate(vehicle_local_position_setpoint
// Check if the previous FlightTask provided setpoints
checkSetpoints(last_setpoint);
_smoothing.reset(last_setpoint.acc_z, last_setpoint.vz, last_setpoint.z);
_smoothing.reset(last_setpoint.acceleration[2], last_setpoint.vz, last_setpoint.z);
return ret;
}
@ -69,7 +69,7 @@ void FlightTaskManualAltitudeSmoothVel::checkSetpoints(vehicle_local_position_se
if (!PX4_ISFINITE(setpoints.vz)) { setpoints.vz = _velocity(2); }
// No acceleration estimate available, set to zero if the setpoint is NAN
if (!PX4_ISFINITE(setpoints.acc_z)) { setpoints.acc_z = 0.f; }
if (!PX4_ISFINITE(setpoints.acceleration[2])) { setpoints.acceleration[2] = 0.f; }
}
void FlightTaskManualAltitudeSmoothVel::_ekfResetHandlerPositionZ()
@ -91,7 +91,6 @@ void FlightTaskManualAltitudeSmoothVel::_updateSetpoints()
_smoothing.setVelSpFeedback(_velocity_setpoint_feedback(2));
_smoothing.setCurrentPositionEstimate(_position(2));
// Get yaw setpoint, un-smoothed position setpoints
FlightTaskManualAltitude::_updateSetpoints();

12
src/lib/FlightTasks/tasks/ManualPositionSmoothVel/FlightTaskManualPositionSmoothVel.cpp
View File

@ -44,12 +44,12 @@ bool FlightTaskManualPositionSmoothVel::activate(vehicle_local_position_setpoint
// Check if the previous FlightTask provided setpoints
checkSetpoints(last_setpoint);
const Vector2f accel_prev(last_setpoint.acc_x, last_setpoint.acc_y);
const Vector2f accel_prev(last_setpoint.acceleration[0], last_setpoint.acceleration[1]);
const Vector2f vel_prev(last_setpoint.vx, last_setpoint.vy);
const Vector2f pos_prev(last_setpoint.x, last_setpoint.y);
_smoothing_xy.reset(accel_prev, vel_prev, pos_prev);
_smoothing_z.reset(last_setpoint.acc_z, last_setpoint.vz, last_setpoint.z);
_smoothing_z.reset(last_setpoint.acceleration[2], last_setpoint.vz, last_setpoint.z);
return ret;
}
@ -79,11 +79,9 @@ void FlightTaskManualPositionSmoothVel::checkSetpoints(vehicle_local_position_se
if (!PX4_ISFINITE(setpoints.vz)) { setpoints.vz = _velocity(2); }
// No acceleration estimate available, set to zero if the setpoint is NAN
if (!PX4_ISFINITE(setpoints.acc_x)) { setpoints.acc_x = 0.f; }
if (!PX4_ISFINITE(setpoints.acc_y)) { setpoints.acc_y = 0.f; }
if (!PX4_ISFINITE(setpoints.acc_z)) { setpoints.acc_z = 0.f; }
for (int i = 0; i < 3; i++) {
if (!PX4_ISFINITE(setpoints.acceleration[i])) { setpoints.acceleration[i] = 0.f; }
}
}
void FlightTaskManualPositionSmoothVel::_ekfResetHandlerPositionXY()

12
src/modules/mavlink/mavlink_messages.cpp
View File

@ -3259,9 +3259,9 @@ protected:
msg.vz = lpos_sp.vz;
// acceleration
msg.afx = lpos_sp.acc_x;
msg.afy = lpos_sp.acc_y;
msg.afz = lpos_sp.acc_z;
msg.afx = lpos_sp.acceleration[0];
msg.afy = lpos_sp.acceleration[1];
msg.afz = lpos_sp.acceleration[2];
// yaw
msg.yaw = lpos_sp.yaw;
@ -3343,9 +3343,9 @@ protected:
msg.vx = pos_sp.vx;
msg.vy = pos_sp.vy;
msg.vz = pos_sp.vz;
msg.afx = pos_sp.acc_x;
msg.afy = pos_sp.acc_y;
msg.afz = pos_sp.acc_z;
msg.afx = pos_sp.acceleration[0];
msg.afy = pos_sp.acceleration[1];
msg.afz = pos_sp.acceleration[2];
mavlink_msg_position_target_local_ned_send_struct(_mavlink->get_channel(), &msg);

2
src/modules/mc_pos_control/PositionControl/PositionControl.cpp
View File

@ -69,7 +69,7 @@ bool PositionControl::updateSetpoint(const vehicle_local_position_setpoint_s &se
_pos_sp = Vector3f(setpoint.x, setpoint.y, setpoint.z);
_vel_sp = Vector3f(setpoint.vx, setpoint.vy, setpoint.vz);
_acc_sp = Vector3f(setpoint.acc_x, setpoint.acc_y, setpoint.acc_z);
_acc_sp = Vector3f(setpoint.acceleration);
_thr_sp = Vector3f(setpoint.thrust);
_yaw_sp = setpoint.yaw;
_yawspeed_sp = setpoint.yawspeed;

2
src/modules/mc_pos_control/mc_pos_control_main.cpp
View File

@ -1027,7 +1027,7 @@ MulticopterPositionControl::reset_setpoint_to_nan(vehicle_local_position_setpoin
setpoint.x = setpoint.y = setpoint.z = NAN;
setpoint.vx = setpoint.vy = setpoint.vz = NAN;
setpoint.yaw = setpoint.yawspeed = NAN;
setpoint.acc_x = setpoint.acc_y = setpoint.acc_z = NAN;
setpoint.acceleration[0] = setpoint.acceleration[1] = setpoint.acceleration[2] = NAN;
setpoint.thrust[0] = setpoint.thrust[1] = setpoint.thrust[2] = NAN;
}