forked from Archive/PX4-Autopilot
Refactor: Use new matrix::Vector2f constructor
This commit is contained in:
Matthias Grob
Daniel Agar
parent
ada0179cda
commit
bee6a6b8b0
|
|
@ -124,7 +124,7 @@ bool FlightTaskAuto::_evaluateTriplets()
|
|||
}
|
||||
|
||||
// Temporary target variable where we save the local reprojection of the latest navigator current triplet.
|
||||
matrix::Vector3f tmp_target;
|
||||
Vector3f tmp_target;
|
||||
|
||||
if (!PX4_ISFINITE(_sub_triplet_setpoint->get().current.lat)
|
||||
|| !PX4_ISFINITE(_sub_triplet_setpoint->get().current.lon)) {
|
||||
|
|
@ -235,18 +235,18 @@ bool FlightTaskAuto::_evaluateTriplets()
|
|||
void FlightTaskAuto::_set_heading_from_mode()
|
||||
{
|
||||
|
||||
matrix::Vector2f v; // Vector that points towards desired location
|
||||
Vector2f v; // Vector that points towards desired location
|
||||
|
||||
switch (MPC_YAW_MODE.get()) {
|
||||
|
||||
case 0: { // Heading points towards the current waypoint.
|
||||
v = Vector2f(_target(0), _target(1)) - Vector2f(_position(0), _position(1));
|
||||
v = Vector2f(_target) - Vector2f(_position);
|
||||
break;
|
||||
}
|
||||
|
||||
case 1: { // Heading points towards home.
|
||||
if (_sub_home_position->get().valid_hpos) {
|
||||
v = Vector2f(_sub_home_position->get().x, _sub_home_position->get().y) - Vector2f(&_position(0));
|
||||
v = Vector2f(&_sub_home_position->get().x) - Vector2f(_position);
|
||||
}
|
||||
|
||||
break;
|
||||
|
|
@ -254,7 +254,7 @@ void FlightTaskAuto::_set_heading_from_mode()
|
|||
|
||||
case 2: { // Heading point away from home.
|
||||
if (_sub_home_position->get().valid_hpos) {
|
||||
v = Vector2f(&_position(0)) - Vector2f(_sub_home_position->get().x, _sub_home_position->get().y);
|
||||
v = Vector2f(_position) - Vector2f(&_sub_home_position->get().x);
|
||||
}
|
||||
|
||||
break;
|
||||
|
|
@ -318,13 +318,13 @@ void FlightTaskAuto::_checkAvoidanceProgress()
|
|||
pos_control_status.timestamp = hrt_absolute_time();
|
||||
|
||||
// vector from previous triplet to current target
|
||||
Vector2f prev_to_target = Vector2f(_triplet_target(0) - _triplet_prev_wp(0), _triplet_target(1) - _triplet_prev_wp(1));
|
||||
Vector2f prev_to_target = Vector2f(_triplet_target - _triplet_prev_wp);
|
||||
// vector from previous triplet to the vehicle projected position on the line previous-target triplet
|
||||
Vector2f prev_to_closest_pt = Vector2f(_closest_pt(0) - _triplet_prev_wp(0), _closest_pt(1) - _triplet_prev_wp(1));
|
||||
Vector2f prev_to_closest_pt = _closest_pt - Vector2f(_triplet_prev_wp);
|
||||
// fraction of the previous-tagerget line that has been flown
|
||||
const float prev_curr_travelled = prev_to_closest_pt.length() / prev_to_target.length();
|
||||
|
||||
Vector2f pos_to_target = Vector2f(_triplet_target(0) - _position(0), _triplet_target(1) - _position(1));
|
||||
Vector2f pos_to_target = Vector2f(_triplet_target - _position);
|
||||
|
||||
if (prev_curr_travelled > 1.0f) {
|
||||
// if the vehicle projected position on the line previous-target is past the target waypoint,
|
||||
|
|
@ -412,7 +412,7 @@ void FlightTaskAuto::_setDefaultConstraints()
|
|||
}
|
||||
}
|
||||
|
||||
matrix::Vector2f FlightTaskAuto::_getTargetVelocityXY()
|
||||
Vector2f FlightTaskAuto::_getTargetVelocityXY()
|
||||
{
|
||||
// guard against any bad velocity values
|
||||
const float vx = _sub_triplet_setpoint->get().current.vx;
|
||||
|
|
@ -421,22 +421,22 @@ matrix::Vector2f FlightTaskAuto::_getTargetVelocityXY()
|
|||
_sub_triplet_setpoint->get().current.velocity_valid;
|
||||
|
||||
if (velocity_valid) {
|
||||
return matrix::Vector2f(vx, vy);
|
||||
return Vector2f(vx, vy);
|
||||
|
||||
} else {
|
||||
// just return zero speed
|
||||
return matrix::Vector2f{};
|
||||
return Vector2f{};
|
||||
}
|
||||
}
|
||||
|
||||
State FlightTaskAuto::_getCurrentState()
|
||||
{
|
||||
// Calculate the vehicle current state based on the Navigator triplets and the current position.
|
||||
Vector2f u_prev_to_target = Vector2f(&(_triplet_target - _triplet_prev_wp)(0)).unit_or_zero();
|
||||
Vector2f pos_to_target = Vector2f(&(_triplet_target - _position)(0));
|
||||
Vector2f prev_to_pos = Vector2f(&(_position - _triplet_prev_wp)(0));
|
||||
Vector2f u_prev_to_target = Vector2f(_triplet_target - _triplet_prev_wp).unit_or_zero();
|
||||
Vector2f pos_to_target(_triplet_target - _position);
|
||||
Vector2f prev_to_pos(_position - _triplet_prev_wp);
|
||||
// Calculate the closest point to the vehicle position on the line prev_wp - target
|
||||
_closest_pt = Vector2f(&_triplet_prev_wp(0)) + u_prev_to_target * (prev_to_pos * u_prev_to_target);
|
||||
_closest_pt = Vector2f(_triplet_prev_wp) + u_prev_to_target * (prev_to_pos * u_prev_to_target);
|
||||
|
||||
State return_state = State::none;
|
||||
|
||||
|
|
@ -448,7 +448,7 @@ State FlightTaskAuto::_getCurrentState()
|
|||
// Current position is more than cruise speed in front of previous setpoint.
|
||||
return_state = State::previous_infront;
|
||||
|
||||
} else if (Vector2f(Vector2f(&_position(0)) - _closest_pt).length() > _mc_cruise_speed) {
|
||||
} else if (Vector2f(Vector2f(_position) - _closest_pt).length() > _mc_cruise_speed) {
|
||||
// Vehicle is more than cruise speed off track.
|
||||
return_state = State::offtrack;
|
||||
|
||||
|
|
@ -481,11 +481,11 @@ void FlightTaskAuto::_updateInternalWaypoints()
|
|||
// angle = cos(x) + 1.0
|
||||
// angle goes from 0 to 2 with 0 = large angle, 2 = small angle: 0 = PI ; 2 = PI*0
|
||||
|
||||
if (Vector2f(&(_target - _next_wp)(0)).length() > 0.001f &&
|
||||
(Vector2f(&(_target - _prev_wp)(0)).length() > NAV_ACC_RAD.get())) {
|
||||
if (Vector2f(_target - _next_wp).length() > 0.001f &&
|
||||
(Vector2f(_target - _prev_wp).length() > NAV_ACC_RAD.get())) {
|
||||
|
||||
angle = Vector2f(&(_target - _prev_wp)(0)).unit_or_zero()
|
||||
* Vector2f(&(_target - _next_wp)(0)).unit_or_zero()
|
||||
angle = Vector2f(_target - _prev_wp).unit_or_zero()
|
||||
* Vector2f(_target - _next_wp).unit_or_zero()
|
||||
+ 1.0f;
|
||||
_speed_at_target = _getVelocityFromAngle(angle);
|
||||
}
|
||||
|
|
@ -502,11 +502,11 @@ void FlightTaskAuto::_updateInternalWaypoints()
|
|||
|
||||
// angle = cos(x) + 1.0
|
||||
// angle goes from 0 to 2 with 0 = large angle, 2 = small angle: 0 = PI ; 2 = PI*0
|
||||
if (Vector2f(&(_target - _next_wp)(0)).length() > 0.001f &&
|
||||
(Vector2f(&(_target - _prev_wp)(0)).length() > NAV_ACC_RAD.get())) {
|
||||
if (Vector2f(_target - _next_wp).length() > 0.001f &&
|
||||
(Vector2f(_target - _prev_wp).length() > NAV_ACC_RAD.get())) {
|
||||
|
||||
angle = Vector2f(&(_target - _prev_wp)(0)).unit_or_zero()
|
||||
* Vector2f(&(_target - _next_wp)(0)).unit_or_zero()
|
||||
angle = Vector2f(_target - _prev_wp).unit_or_zero()
|
||||
* Vector2f(_target - _next_wp).unit_or_zero()
|
||||
+ 1.0f;
|
||||
_speed_at_target = _getVelocityFromAngle(angle);
|
||||
}
|
||||
|
|
@ -515,7 +515,7 @@ void FlightTaskAuto::_updateInternalWaypoints()
|
|||
|
||||
case State::offtrack: {
|
||||
_next_wp = _triplet_target;
|
||||
_target = matrix::Vector3f(_closest_pt(0), _closest_pt(1), _triplet_target(2));
|
||||
_target = Vector3f(_closest_pt(0), _closest_pt(1), _triplet_target(2));
|
||||
_prev_wp = _position;
|
||||
|
||||
float angle = 2.0f;
|
||||
|
|
@ -523,11 +523,11 @@ void FlightTaskAuto::_updateInternalWaypoints()
|
|||
|
||||
// angle = cos(x) + 1.0
|
||||
// angle goes from 0 to 2 with 0 = large angle, 2 = small angle: 0 = PI ; 2 = PI*0
|
||||
if (Vector2f(&(_target - _next_wp)(0)).length() > 0.001f &&
|
||||
(Vector2f(&(_target - _prev_wp)(0)).length() > NAV_ACC_RAD.get())) {
|
||||
if (Vector2f(_target - _next_wp).length() > 0.001f &&
|
||||
(Vector2f(_target - _prev_wp).length() > NAV_ACC_RAD.get())) {
|
||||
|
||||
angle = Vector2f(&(_target - _prev_wp)(0)).unit_or_zero()
|
||||
* Vector2f(&(_target - _next_wp)(0)).unit_or_zero()
|
||||
angle = Vector2f(_target - _prev_wp).unit_or_zero()
|
||||
* Vector2f(_target - _next_wp).unit_or_zero()
|
||||
+ 1.0f;
|
||||
_speed_at_target = _getVelocityFromAngle(angle);
|
||||
}
|
||||
|
|
@ -544,12 +544,12 @@ void FlightTaskAuto::_updateInternalWaypoints()
|
|||
|
||||
// angle = cos(x) + 1.0
|
||||
// angle goes from 0 to 2 with 0 = large angle, 2 = small angle: 0 = PI ; 2 = PI*0
|
||||
if (Vector2f(&(_target - _next_wp)(0)).length() > 0.001f &&
|
||||
(Vector2f(&(_target - _prev_wp)(0)).length() > NAV_ACC_RAD.get())) {
|
||||
if (Vector2f(_target - _next_wp).length() > 0.001f &&
|
||||
(Vector2f(_target - _prev_wp).length() > NAV_ACC_RAD.get())) {
|
||||
|
||||
angle =
|
||||
Vector2f(&(_target - _prev_wp)(0)).unit_or_zero()
|
||||
* Vector2f(&(_target - _next_wp)(0)).unit_or_zero()
|
||||
Vector2f(_target - _prev_wp).unit_or_zero()
|
||||
* Vector2f(_target - _next_wp).unit_or_zero()
|
||||
+ 1.0f;
|
||||
_speed_at_target = _getVelocityFromAngle(angle);
|
||||
}
|
||||
|
|
@ -562,7 +562,7 @@ void FlightTaskAuto::_updateInternalWaypoints()
|
|||
}
|
||||
}
|
||||
|
||||
bool FlightTaskAuto::_compute_heading_from_2D_vector(float &heading, matrix::Vector2f v)
|
||||
bool FlightTaskAuto::_compute_heading_from_2D_vector(float &heading, Vector2f v)
|
||||
{
|
||||
if (PX4_ISFINITE(v.length()) && v.length() > SIGMA_NORM) {
|
||||
v.normalize();
|
||||
|
|
|
|||
|
|
@ -55,14 +55,14 @@ void FlightTaskAutoLine::_generateSetpoints()
|
|||
|
||||
void FlightTaskAutoLine::_generateHeadingAlongTrack()
|
||||
{
|
||||
Vector2f prev_to_dest = Vector2f(&(_target - _prev_wp)(0));
|
||||
Vector2f prev_to_dest(_target - _prev_wp);
|
||||
_compute_heading_from_2D_vector(_yaw_setpoint, prev_to_dest);
|
||||
|
||||
}
|
||||
|
||||
void FlightTaskAutoLine::_generateXYsetpoints()
|
||||
{
|
||||
Vector2f pos_sp_to_dest = Vector2f(&(_target - _position_setpoint)(0));
|
||||
Vector2f pos_sp_to_dest(_target - _position_setpoint);
|
||||
const bool has_reached_altitude = fabsf(_target(2) - _position(2)) < NAV_ACC_RAD.get();
|
||||
|
||||
if ((_speed_at_target < 0.001f && pos_sp_to_dest.length() < NAV_ACC_RAD.get()) ||
|
||||
|
|
@ -76,13 +76,13 @@ void FlightTaskAutoLine::_generateXYsetpoints()
|
|||
} else {
|
||||
|
||||
// Get various path specific vectors. */
|
||||
Vector2f u_prev_to_dest = Vector2f(&(_target - _prev_wp)(0)).unit_or_zero();
|
||||
Vector2f prev_to_pos(&(_position - _prev_wp)(0));
|
||||
Vector2f closest_pt = Vector2f(&_prev_wp(0)) + u_prev_to_dest * (prev_to_pos * u_prev_to_dest);
|
||||
Vector2f closest_to_dest = Vector2f(&(_target - _position)(0));
|
||||
Vector2f prev_to_dest = Vector2f(&(_target - _prev_wp)(0));
|
||||
Vector2f u_prev_to_dest = Vector2f(_target - _prev_wp).unit_or_zero();
|
||||
Vector2f prev_to_pos(_position - _prev_wp);
|
||||
Vector2f closest_pt = Vector2f(_prev_wp) + u_prev_to_dest * (prev_to_pos * u_prev_to_dest);
|
||||
Vector2f closest_to_dest(_target - _position);
|
||||
Vector2f prev_to_dest(_target - _prev_wp);
|
||||
float speed_sp_track = _mc_cruise_speed;
|
||||
float speed_sp_prev_track = math::max(Vector2f(&_velocity_setpoint(0)) * u_prev_to_dest, 0.0f);
|
||||
float speed_sp_prev_track = math::max(Vector2f(_velocity_setpoint) * u_prev_to_dest, 0.0f);
|
||||
|
||||
// Distance to target when brake should occur. The assumption is made that
|
||||
// 1.5 * cruising speed is enough to break.
|
||||
|
|
|
|||
|
|
@ -143,7 +143,7 @@ void FlightTaskAutoMapper::_generateVelocitySetpoints()
|
|||
// TODO: Remove velocity force logic from navigator, since
|
||||
// navigator should only send out waypoints.
|
||||
_position_setpoint = Vector3f(NAN, NAN, _position(2));
|
||||
Vector2f vel_sp_xy = Vector2f(&_velocity(0)).unit_or_zero() * _mc_cruise_speed;
|
||||
Vector2f vel_sp_xy = Vector2f(_velocity).unit_or_zero() * _mc_cruise_speed;
|
||||
_velocity_setpoint = Vector3f(vel_sp_xy(0), vel_sp_xy(1), NAN);
|
||||
}
|
||||
|
||||
|
|
|
|||
|
|
@ -101,7 +101,7 @@ void FlightTaskManualAltitude::_updateAltitudeLock()
|
|||
// when terrain hold behaviour has been selected.
|
||||
if (MPC_ALT_MODE.get() == 2) {
|
||||
// Use horizontal speed as a transition criteria
|
||||
float spd_xy = Vector2f(&_velocity(0)).length();
|
||||
float spd_xy = Vector2f(_velocity).length();
|
||||
|
||||
// Use presence of horizontal stick inputs as a transition criteria
|
||||
float stick_xy = Vector2f(&_sticks_expo(0)).length();
|
||||
|
|
@ -260,7 +260,7 @@ void FlightTaskManualAltitude::_updateSetpoints()
|
|||
// thrust along xy is demanded. The maximum thrust along xy depends on the thrust
|
||||
// setpoint along z-direction, which is computed in PositionControl.cpp.
|
||||
|
||||
Vector2f sp{_sticks(0), _sticks(1)};
|
||||
Vector2f sp(&_sticks(0));
|
||||
_rotateIntoHeadingFrame(sp);
|
||||
|
||||
if (sp.length() > 1.0f) {
|
||||
|
|
|
|||
|
|
@ -78,7 +78,7 @@ void FlightTaskManualPosition::_scaleSticks()
|
|||
FlightTaskManualAltitude::_scaleSticks();
|
||||
|
||||
/* Constrain length of stick inputs to 1 for xy*/
|
||||
Vector2f stick_xy(_sticks_expo(0), _sticks_expo(1));
|
||||
Vector2f stick_xy(&_sticks_expo(0));
|
||||
|
||||
float mag = math::constrain(stick_xy.length(), 0.0f, 1.0f);
|
||||
|
||||
|
|
@ -119,8 +119,8 @@ void FlightTaskManualPosition::_scaleSticks()
|
|||
void FlightTaskManualPosition::_updateXYlock()
|
||||
{
|
||||
/* If position lock is not active, position setpoint is set to NAN.*/
|
||||
const float vel_xy_norm = Vector2f(&_velocity(0)).length();
|
||||
const bool apply_brake = Vector2f(&_velocity_setpoint(0)).length() < FLT_EPSILON;
|
||||
const float vel_xy_norm = Vector2f(_velocity).length();
|
||||
const bool apply_brake = Vector2f(_velocity_setpoint).length() < FLT_EPSILON;
|
||||
const bool stopped = (MPC_HOLD_MAX_XY.get() < FLT_EPSILON || vel_xy_norm < MPC_HOLD_MAX_XY.get());
|
||||
|
||||
if (apply_brake && stopped && !PX4_ISFINITE(_position_setpoint(0))) {
|
||||
|
|
|
|||
|
|
@ -40,7 +40,7 @@
|
|||
using namespace matrix;
|
||||
|
||||
FlightTaskManualPositionSmooth::FlightTaskManualPositionSmooth() :
|
||||
_smoothingXY(this, matrix::Vector2f(&_velocity(0))),
|
||||
_smoothingXY(this, Vector2f(_velocity)),
|
||||
_smoothingZ(this, _velocity(2), _sticks(2))
|
||||
{}
|
||||
|
||||
|
|
@ -50,8 +50,8 @@ void FlightTaskManualPositionSmooth::_updateSetpoints()
|
|||
FlightTaskManualPosition::_updateSetpoints();
|
||||
|
||||
/* Smooth velocity setpoint in xy.*/
|
||||
matrix::Vector2f vel(&_velocity(0));
|
||||
Vector2f vel_sp_xy = Vector2f(&_velocity_setpoint(0));
|
||||
Vector2f vel(_velocity);
|
||||
Vector2f vel_sp_xy(_velocity_setpoint);
|
||||
_smoothingXY.updateMaxVelocity(_constraints.speed_xy);
|
||||
_smoothingXY.smoothVelocity(vel_sp_xy, vel, _yaw, _yawspeed_setpoint, _deltatime);
|
||||
_velocity_setpoint(0) = vel_sp_xy(0);
|
||||
|
|
|
|||
|
|
@ -99,7 +99,7 @@ void FlightTaskManualStabilized::_updateHeadingSetpoints()
|
|||
void FlightTaskManualStabilized::_updateThrustSetpoints()
|
||||
{
|
||||
/* Rotate setpoint into local frame. */
|
||||
Vector2f sp{_sticks(0), _sticks(1)};
|
||||
Vector2f sp(&_sticks(0));
|
||||
_rotateIntoHeadingFrame(sp);
|
||||
|
||||
/* Ensure that maximum tilt is in [0.001, Pi] */
|
||||
|
|
|
|||
|
|
@ -122,7 +122,7 @@ bool FlightTaskOrbit::activate()
|
|||
bool ret = FlightTaskManualAltitudeSmooth::activate();
|
||||
_r = _radius_min;
|
||||
_v = 1.f;
|
||||
_center = Vector2f(_position.data());
|
||||
_center = Vector2f(_position);
|
||||
_center(0) -= _r;
|
||||
|
||||
// need a valid position and velocity
|
||||
|
|
@ -149,8 +149,8 @@ bool FlightTaskOrbit::update()
|
|||
setVelocity(v);
|
||||
|
||||
// xy velocity to go around in a circle
|
||||
Vector2f center_to_position = Vector2f(_position.data()) - _center;
|
||||
Vector2f velocity_xy = Vector2f(-center_to_position(1), center_to_position(0));
|
||||
Vector2f center_to_position = Vector2f(_position) - _center;
|
||||
Vector2f velocity_xy(-center_to_position(1), center_to_position(0));
|
||||
velocity_xy = velocity_xy.unit_or_zero();
|
||||
velocity_xy *= _v;
|
||||
|
||||
|
|
|
|||
|
|
@ -40,7 +40,9 @@
|
|||
#include <mathlib/mathlib.h>
|
||||
#include <float.h>
|
||||
|
||||
ManualSmoothingXY::ManualSmoothingXY(ModuleParams *parent, const matrix::Vector2f &vel) :
|
||||
using namespace matrix;
|
||||
|
||||
ManualSmoothingXY::ManualSmoothingXY(ModuleParams *parent, const Vector2f &vel) :
|
||||
ModuleParams(parent), _vel_sp_prev(vel)
|
||||
|
||||
{
|
||||
|
|
@ -49,7 +51,7 @@ ManualSmoothingXY::ManualSmoothingXY(ModuleParams *parent, const matrix::Vector2
|
|||
}
|
||||
|
||||
void
|
||||
ManualSmoothingXY::smoothVelocity(matrix::Vector2f &vel_sp, const matrix::Vector2f &vel, const float &yaw,
|
||||
ManualSmoothingXY::smoothVelocity(Vector2f &vel_sp, const Vector2f &vel, const float &yaw,
|
||||
const float &yawrate_sp, const float dt)
|
||||
{
|
||||
_updateAcceleration(vel_sp, vel, yaw, yawrate_sp, dt);
|
||||
|
|
@ -58,7 +60,7 @@ ManualSmoothingXY::smoothVelocity(matrix::Vector2f &vel_sp, const matrix::Vector
|
|||
}
|
||||
|
||||
void
|
||||
ManualSmoothingXY::_updateAcceleration(matrix::Vector2f &vel_sp, const matrix::Vector2f &vel, const float &yaw,
|
||||
ManualSmoothingXY::_updateAcceleration(Vector2f &vel_sp, const Vector2f &vel, const float &yaw,
|
||||
const float &yawrate_sp, const float dt)
|
||||
{
|
||||
Intention intention = _getIntention(vel_sp, vel, yaw, yawrate_sp);
|
||||
|
|
@ -69,7 +71,7 @@ ManualSmoothingXY::_updateAcceleration(matrix::Vector2f &vel_sp, const matrix::V
|
|||
}
|
||||
|
||||
ManualSmoothingXY::Intention
|
||||
ManualSmoothingXY::_getIntention(const matrix::Vector2f &vel_sp, const matrix::Vector2f &vel, const float &yaw,
|
||||
ManualSmoothingXY::_getIntention(const Vector2f &vel_sp, const Vector2f &vel, const float &yaw,
|
||||
const float &yawrate_sp)
|
||||
{
|
||||
|
||||
|
|
@ -81,8 +83,8 @@ ManualSmoothingXY::_getIntention(const matrix::Vector2f &vel_sp, const matrix::V
|
|||
// that the user demanded a direction change.
|
||||
// The detection is done in body frame.
|
||||
// Rotate velocity setpoint into body frame
|
||||
matrix::Vector2f vel_sp_heading = _getWorldToHeadingFrame(vel_sp, yaw);
|
||||
matrix::Vector2f vel_heading = _getWorldToHeadingFrame(vel, yaw);
|
||||
Vector2f vel_sp_heading = _getWorldToHeadingFrame(vel_sp, yaw);
|
||||
Vector2f vel_heading = _getWorldToHeadingFrame(vel, yaw);
|
||||
|
||||
if (vel_sp_heading.length() > FLT_EPSILON) {
|
||||
vel_sp_heading.normalize();
|
||||
|
|
@ -117,7 +119,7 @@ ManualSmoothingXY::_getIntention(const matrix::Vector2f &vel_sp, const matrix::V
|
|||
}
|
||||
|
||||
void
|
||||
ManualSmoothingXY::_setStateAcceleration(const matrix::Vector2f &vel_sp, const matrix::Vector2f &vel,
|
||||
ManualSmoothingXY::_setStateAcceleration(const Vector2f &vel_sp, const Vector2f &vel,
|
||||
const Intention &intention, const float dt)
|
||||
{
|
||||
switch (intention) {
|
||||
|
|
@ -194,10 +196,10 @@ ManualSmoothingXY::_setStateAcceleration(const matrix::Vector2f &vel_sp, const m
|
|||
}
|
||||
|
||||
void
|
||||
ManualSmoothingXY::_velocitySlewRate(matrix::Vector2f &vel_sp, const float dt)
|
||||
ManualSmoothingXY::_velocitySlewRate(Vector2f &vel_sp, const float dt)
|
||||
{
|
||||
// Adjust velocity setpoint if demand exceeds acceleration. /
|
||||
matrix::Vector2f acc{};
|
||||
Vector2f acc{};
|
||||
|
||||
if (dt > FLT_EPSILON) {
|
||||
acc = (vel_sp - _vel_sp_prev) / dt;
|
||||
|
|
@ -208,18 +210,18 @@ ManualSmoothingXY::_velocitySlewRate(matrix::Vector2f &vel_sp, const float dt)
|
|||
}
|
||||
}
|
||||
|
||||
matrix::Vector2f
|
||||
ManualSmoothingXY::_getWorldToHeadingFrame(const matrix::Vector2f &vec, const float &yaw)
|
||||
Vector2f
|
||||
ManualSmoothingXY::_getWorldToHeadingFrame(const Vector2f &vec, const float &yaw)
|
||||
{
|
||||
matrix::Quatf q_yaw = matrix::AxisAnglef(matrix::Vector3f(0.0f, 0.0f, 1.0f), yaw);
|
||||
matrix::Vector3f vec_heading = q_yaw.conjugate_inversed(matrix::Vector3f(vec(0), vec(1), 0.0f));
|
||||
return matrix::Vector2f(&vec_heading(0));
|
||||
Quatf q_yaw = AxisAnglef(Vector3f(0.0f, 0.0f, 1.0f), yaw);
|
||||
Vector3f vec_heading = q_yaw.conjugate_inversed(Vector3f(vec(0), vec(1), 0.0f));
|
||||
return Vector2f(vec_heading);
|
||||
}
|
||||
|
||||
matrix::Vector2f
|
||||
ManualSmoothingXY::_getHeadingToWorldFrame(const matrix::Vector2f &vec, const float &yaw)
|
||||
Vector2f
|
||||
ManualSmoothingXY::_getHeadingToWorldFrame(const Vector2f &vec, const float &yaw)
|
||||
{
|
||||
matrix::Quatf q_yaw = matrix::AxisAnglef(matrix::Vector3f(0.0f, 0.0f, 1.0f), yaw);
|
||||
matrix::Vector3f vec_world = q_yaw.conjugate(matrix::Vector3f(vec(0), vec(1), 0.0f));
|
||||
return matrix::Vector2f(&vec_world(0));
|
||||
Quatf q_yaw = AxisAnglef(Vector3f(0.0f, 0.0f, 1.0f), yaw);
|
||||
Vector3f vec_world = q_yaw.conjugate(Vector3f(vec(0), vec(1), 0.0f));
|
||||
return Vector2f(vec_world);
|
||||
}
|
||||
|
|
|
|||
|
|
@ -113,7 +113,7 @@ float StraightLine::getMaxAcc()
|
|||
Vector3f u_orig_to_target = (_target - _origin).unit_or_zero();
|
||||
|
||||
// calculate the maximal horizontal acceleration
|
||||
float divider = Vector2f(u_orig_to_target(0), u_orig_to_target(1)).length();
|
||||
float divider = Vector2f(u_orig_to_target).length();
|
||||
float max_acc_hor = MPC_ACC_HOR_MAX.get();
|
||||
|
||||
if (divider > FLT_EPSILON) {
|
||||
|
|
@ -148,7 +148,7 @@ float StraightLine::getMaxVel()
|
|||
Vector3f u_orig_to_target = (_target - _origin).unit_or_zero();
|
||||
|
||||
// calculate the maximal horizontal velocity
|
||||
float divider = Vector2f(u_orig_to_target.data()).length();
|
||||
float divider = Vector2f(u_orig_to_target).length();
|
||||
float max_vel_hor = MPC_XY_VEL_MAX.get();
|
||||
|
||||
if (divider > FLT_EPSILON) {
|
||||
|
|
|
|||
|
|
@ -219,7 +219,7 @@ GroundRoverPositionControl::control_position(const matrix::Vector2f ¤t_pos
|
|||
prev_wp(1) = (float)pos_sp_triplet.previous.lon;
|
||||
}
|
||||
|
||||
matrix::Vector2f ground_speed_2d = {ground_speed(0), ground_speed(1)};
|
||||
matrix::Vector2f ground_speed_2d(ground_speed);
|
||||
|
||||
float mission_throttle = _parameters.throttle_cruise;
|
||||
|
||||
|
|
|
|||
|
|
@ -219,8 +219,8 @@ void PositionControl::_positionController()
|
|||
|
||||
// Constrain horizontal velocity by prioritizing the velocity component along the
|
||||
// the desired position setpoint over the feed-forward term.
|
||||
const Vector2f vel_sp_xy = ControlMath::constrainXY(Vector2f(vel_sp_position(0), vel_sp_position(1)),
|
||||
Vector2f(&(_vel_sp - vel_sp_position)(0)), _constraints.speed_xy);
|
||||
const Vector2f vel_sp_xy = ControlMath::constrainXY(Vector2f(vel_sp_position),
|
||||
Vector2f(_vel_sp - vel_sp_position), _constraints.speed_xy);
|
||||
_vel_sp(0) = vel_sp_xy(0);
|
||||
_vel_sp(1) = vel_sp_xy(1);
|
||||
// Constrain velocity in z-direction.
|
||||
|
|
@ -307,7 +307,7 @@ void PositionControl::_velocityController(const float &dt)
|
|||
}
|
||||
|
||||
// Get the direction of (r-y) in NE-direction.
|
||||
float direction_NE = Vector2f(vel_err(0), vel_err(1)) * Vector2f(_vel_sp(0), _vel_sp(1));
|
||||
float direction_NE = Vector2f(vel_err) * Vector2f(_vel_sp);
|
||||
|
||||
// Apply Anti-Windup in NE-direction.
|
||||
bool stop_integral_NE = (thrust_desired_NE * thrust_desired_NE >= thrust_max_NE * thrust_max_NE &&
|
||||
|
|
@ -318,7 +318,7 @@ void PositionControl::_velocityController(const float &dt)
|
|||
_thr_int(1) += vel_err(1) * MPC_XY_VEL_I.get() * dt;
|
||||
|
||||
// magnitude of thrust integral can never exceed maximum throttle in NE
|
||||
float integral_mag_NE = Vector2f(&_thr_int(0)).length();
|
||||
float integral_mag_NE = Vector2f(_thr_int).length();
|
||||
|
||||
if (integral_mag_NE > 0.0f && integral_mag_NE > thrust_max_NE) {
|
||||
_thr_int(0) = _thr_int(0) / integral_mag_NE * thrust_max_NE;
|
||||
|
|
|
|||
|
|
@ -41,15 +41,16 @@
|
|||
#include <float.h>
|
||||
#include <mathlib/mathlib.h>
|
||||
|
||||
using namespace matrix;
|
||||
namespace ControlMath
|
||||
{
|
||||
vehicle_attitude_setpoint_s thrustToAttitude(const matrix::Vector3f &thr_sp, const float yaw_sp)
|
||||
vehicle_attitude_setpoint_s thrustToAttitude(const Vector3f &thr_sp, const float yaw_sp)
|
||||
{
|
||||
vehicle_attitude_setpoint_s att_sp = {};
|
||||
att_sp.yaw_body = yaw_sp;
|
||||
|
||||
// desired body_z axis = -normalize(thrust_vector)
|
||||
matrix::Vector3f body_x, body_y, body_z;
|
||||
Vector3f body_x, body_y, body_z;
|
||||
|
||||
if (thr_sp.length() > 0.00001f) {
|
||||
body_z = -thr_sp.normalized();
|
||||
|
|
@ -61,7 +62,7 @@ vehicle_attitude_setpoint_s thrustToAttitude(const matrix::Vector3f &thr_sp, con
|
|||
}
|
||||
|
||||
// vector of desired yaw direction in XY plane, rotated by PI/2
|
||||
matrix::Vector3f y_C(-sinf(att_sp.yaw_body), cosf(att_sp.yaw_body), 0.0f);
|
||||
Vector3f y_C(-sinf(att_sp.yaw_body), cosf(att_sp.yaw_body), 0.0f);
|
||||
|
||||
if (fabsf(body_z(2)) > 0.000001f) {
|
||||
// desired body_x axis, orthogonal to body_z
|
||||
|
|
@ -84,7 +85,7 @@ vehicle_attitude_setpoint_s thrustToAttitude(const matrix::Vector3f &thr_sp, con
|
|||
// desired body_y axis
|
||||
body_y = body_z % body_x;
|
||||
|
||||
matrix::Dcmf R_sp;
|
||||
Dcmf R_sp;
|
||||
|
||||
// fill rotation matrix
|
||||
for (int i = 0; i < 3; i++) {
|
||||
|
|
@ -94,12 +95,12 @@ vehicle_attitude_setpoint_s thrustToAttitude(const matrix::Vector3f &thr_sp, con
|
|||
}
|
||||
|
||||
//copy quaternion setpoint to attitude setpoint topic
|
||||
matrix::Quatf q_sp = R_sp;
|
||||
Quatf q_sp = R_sp;
|
||||
q_sp.copyTo(att_sp.q_d);
|
||||
att_sp.q_d_valid = true;
|
||||
|
||||
// calculate euler angles, for logging only, must not be used for control
|
||||
matrix::Eulerf euler = R_sp;
|
||||
Eulerf euler = R_sp;
|
||||
att_sp.roll_body = euler(0);
|
||||
att_sp.pitch_body = euler(1);
|
||||
att_sp.thrust = thr_sp.length();
|
||||
|
|
@ -107,9 +108,9 @@ vehicle_attitude_setpoint_s thrustToAttitude(const matrix::Vector3f &thr_sp, con
|
|||
return att_sp;
|
||||
}
|
||||
|
||||
matrix::Vector2f constrainXY(const matrix::Vector2f &v0, const matrix::Vector2f &v1, const float &max)
|
||||
Vector2f constrainXY(const Vector2f &v0, const Vector2f &v1, const float &max)
|
||||
{
|
||||
if (matrix::Vector2f(v0 + v1).norm() <= max) {
|
||||
if (Vector2f(v0 + v1).norm() <= max) {
|
||||
// vector does not exceed maximum magnitude
|
||||
return v0 + v1;
|
||||
|
||||
|
|
@ -117,7 +118,7 @@ matrix::Vector2f constrainXY(const matrix::Vector2f &v0, const matrix::Vector2f
|
|||
// the magnitude along v0, which has priority, already exceeds maximum.
|
||||
return v0.normalized() * max;
|
||||
|
||||
} else if (fabsf(matrix::Vector2f(v1 - v0).norm()) < 0.001f) {
|
||||
} else if (fabsf(Vector2f(v1 - v0).norm()) < 0.001f) {
|
||||
// the two vectors are equal
|
||||
return v0.normalized() * max;
|
||||
|
||||
|
|
@ -164,7 +165,7 @@ matrix::Vector2f constrainXY(const matrix::Vector2f &v0, const matrix::Vector2f
|
|||
// - s (=scaling factor) needs to be positive
|
||||
// - (max - ||v||) always larger than zero, otherwise it never entered this if-statement
|
||||
|
||||
matrix::Vector2f u1 = v1.normalized();
|
||||
Vector2f u1 = v1.normalized();
|
||||
float m = u1.dot(v0);
|
||||
float c = v0.dot(v0) - max * max;
|
||||
float s = -m + sqrtf(m * m - c);
|
||||
|
|
@ -172,18 +173,18 @@ matrix::Vector2f constrainXY(const matrix::Vector2f &v0, const matrix::Vector2f
|
|||
}
|
||||
}
|
||||
|
||||
bool cross_sphere_line(const matrix::Vector3f &sphere_c, const float sphere_r,
|
||||
const matrix::Vector3f &line_a, const matrix::Vector3f &line_b, matrix::Vector3f &res)
|
||||
bool cross_sphere_line(const Vector3f &sphere_c, const float sphere_r,
|
||||
const Vector3f &line_a, const Vector3f &line_b, Vector3f &res)
|
||||
{
|
||||
// project center of sphere on line normalized AB
|
||||
matrix::Vector3f ab_norm = line_b - line_a;
|
||||
Vector3f ab_norm = line_b - line_a;
|
||||
|
||||
if (ab_norm.length() < 0.01f) {
|
||||
return true;
|
||||
}
|
||||
|
||||
ab_norm.normalize();
|
||||
matrix::Vector3f d = line_a + ab_norm * ((sphere_c - line_a) * ab_norm);
|
||||
Vector3f d = line_a + ab_norm * ((sphere_c - line_a) * ab_norm);
|
||||
float cd_len = (sphere_c - d).length();
|
||||
|
||||
if (sphere_r > cd_len) {
|
||||
|
|
|
|||
Loading…
Reference in New Issue