fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-11-01 08:41:36 +08:00
Code Issues Packages Projects Releases Wiki Activity

Refactor: Use new matrix::Vector2f constructor

Browse Source
This commit is contained in:
Matthias Grob
2018-10-01 08:10:21 -04:00
committed by Daniel Agar
parent ada0179cda
commit bee6a6b8b0
13 changed files with 98 additions and 95 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/lib/FlightTasks/tasks/Auto/FlightTaskAuto.cpp
+34 -34
View File
@@ -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();