diff --git a/src/lib/FlightTasks/tasks/Auto/FlightTaskAuto.cpp b/src/lib/FlightTasks/tasks/Auto/FlightTaskAuto.cpp index f3e8e2033c..7d3038cfe9 100644 --- a/src/lib/FlightTasks/tasks/Auto/FlightTaskAuto.cpp +++ b/src/lib/FlightTasks/tasks/Auto/FlightTaskAuto.cpp @@ -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(); diff --git a/src/lib/FlightTasks/tasks/AutoLine/FlightTaskAutoLine.cpp b/src/lib/FlightTasks/tasks/AutoLine/FlightTaskAutoLine.cpp index 96274ebb5a..a5a0400c87 100644 --- a/src/lib/FlightTasks/tasks/AutoLine/FlightTaskAutoLine.cpp +++ b/src/lib/FlightTasks/tasks/AutoLine/FlightTaskAutoLine.cpp @@ -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. diff --git a/src/lib/FlightTasks/tasks/AutoMapper/FlightTaskAutoMapper.cpp b/src/lib/FlightTasks/tasks/AutoMapper/FlightTaskAutoMapper.cpp index 630e858ac6..bbe177e509 100644 --- a/src/lib/FlightTasks/tasks/AutoMapper/FlightTaskAutoMapper.cpp +++ b/src/lib/FlightTasks/tasks/AutoMapper/FlightTaskAutoMapper.cpp @@ -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); } diff --git a/src/lib/FlightTasks/tasks/ManualAltitude/FlightTaskManualAltitude.cpp b/src/lib/FlightTasks/tasks/ManualAltitude/FlightTaskManualAltitude.cpp index 168fe33fb0..939f40d858 100644 --- a/src/lib/FlightTasks/tasks/ManualAltitude/FlightTaskManualAltitude.cpp +++ b/src/lib/FlightTasks/tasks/ManualAltitude/FlightTaskManualAltitude.cpp @@ -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) { diff --git a/src/lib/FlightTasks/tasks/ManualPosition/FlightTaskManualPosition.cpp b/src/lib/FlightTasks/tasks/ManualPosition/FlightTaskManualPosition.cpp index 0aa0040b66..9e98b93fc0 100644 --- a/src/lib/FlightTasks/tasks/ManualPosition/FlightTaskManualPosition.cpp +++ b/src/lib/FlightTasks/tasks/ManualPosition/FlightTaskManualPosition.cpp @@ -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))) { diff --git a/src/lib/FlightTasks/tasks/ManualPositionSmooth/FlightTaskManualPositionSmooth.cpp b/src/lib/FlightTasks/tasks/ManualPositionSmooth/FlightTaskManualPositionSmooth.cpp index 1a8d20f296..117ab43f14 100644 --- a/src/lib/FlightTasks/tasks/ManualPositionSmooth/FlightTaskManualPositionSmooth.cpp +++ b/src/lib/FlightTasks/tasks/ManualPositionSmooth/FlightTaskManualPositionSmooth.cpp @@ -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); diff --git a/src/lib/FlightTasks/tasks/ManualStabilized/FlightTaskManualStabilized.cpp b/src/lib/FlightTasks/tasks/ManualStabilized/FlightTaskManualStabilized.cpp index 25b273f577..4e712355df 100644 --- a/src/lib/FlightTasks/tasks/ManualStabilized/FlightTaskManualStabilized.cpp +++ b/src/lib/FlightTasks/tasks/ManualStabilized/FlightTaskManualStabilized.cpp @@ -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] */ diff --git a/src/lib/FlightTasks/tasks/Orbit/FlightTaskOrbit.cpp b/src/lib/FlightTasks/tasks/Orbit/FlightTaskOrbit.cpp index 94e08235ab..2cf0d6cad6 100644 --- a/src/lib/FlightTasks/tasks/Orbit/FlightTaskOrbit.cpp +++ b/src/lib/FlightTasks/tasks/Orbit/FlightTaskOrbit.cpp @@ -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; diff --git a/src/lib/FlightTasks/tasks/Utility/ManualSmoothingXY.cpp b/src/lib/FlightTasks/tasks/Utility/ManualSmoothingXY.cpp index a4617d0e1e..972e8c7c2a 100644 --- a/src/lib/FlightTasks/tasks/Utility/ManualSmoothingXY.cpp +++ b/src/lib/FlightTasks/tasks/Utility/ManualSmoothingXY.cpp @@ -40,7 +40,9 @@ #include #include -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); } diff --git a/src/lib/FlightTasks/tasks/Utility/StraightLine.cpp b/src/lib/FlightTasks/tasks/Utility/StraightLine.cpp index 2f89edeaaa..501bd20ba9 100644 --- a/src/lib/FlightTasks/tasks/Utility/StraightLine.cpp +++ b/src/lib/FlightTasks/tasks/Utility/StraightLine.cpp @@ -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) { diff --git a/src/modules/gnd_pos_control/GroundRoverPositionControl.cpp b/src/modules/gnd_pos_control/GroundRoverPositionControl.cpp index cf8991b90e..8593b807a0 100644 --- a/src/modules/gnd_pos_control/GroundRoverPositionControl.cpp +++ b/src/modules/gnd_pos_control/GroundRoverPositionControl.cpp @@ -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; diff --git a/src/modules/mc_pos_control/PositionControl.cpp b/src/modules/mc_pos_control/PositionControl.cpp index 446df4b6a3..f5d6f81c71 100644 --- a/src/modules/mc_pos_control/PositionControl.cpp +++ b/src/modules/mc_pos_control/PositionControl.cpp @@ -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; diff --git a/src/modules/mc_pos_control/Utility/ControlMath.cpp b/src/modules/mc_pos_control/Utility/ControlMath.cpp index 517e35dafc..fda1de5cb6 100644 --- a/src/modules/mc_pos_control/Utility/ControlMath.cpp +++ b/src/modules/mc_pos_control/Utility/ControlMath.cpp @@ -41,15 +41,16 @@ #include #include +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) {