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

TECS and L1 switch to matrix math library (#9101)

Browse Source
This commit is contained in:
Daniel Agar
2018-03-19 18:09:54 -04:00
committed by GitHub
parent e63f9d9bf6
commit b6b7fddb9f
7 changed files with 47 additions and 59 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/modules/gnd_pos_control/GroundRoverPositionControl.cpp
+8 -8
View File
@@ -205,8 +205,8 @@ void GroundRoverPositionControl::gnd_pos_ctrl_status_publish()
}
bool
GroundRoverPositionControl::control_position(const math::Vector<2> &current_position,
const math::Vector<3> &ground_speed, const position_setpoint_triplet_s &pos_sp_triplet)
GroundRoverPositionControl::control_position(const matrix::Vector2f &current_position,
const matrix::Vector3f &ground_speed, const position_setpoint_triplet_s &pos_sp_triplet)
{
float dt = 0.01; // Using non zero value to a avoid division by zero
@@ -227,17 +227,17 @@ GroundRoverPositionControl::control_position(const math::Vector<2> &current_posi
bool was_circle_mode = _gnd_control.circle_mode();
/* current waypoint (the one currently heading for) */
math::Vector<2> curr_wp((float)pos_sp_triplet.current.lat, (float)pos_sp_triplet.current.lon);
matrix::Vector2f curr_wp((float)pos_sp_triplet.current.lat, (float)pos_sp_triplet.current.lon);
/* previous waypoint */
math::Vector<2> prev_wp = curr_wp;
matrix::Vector2f prev_wp = curr_wp;
if (pos_sp_triplet.previous.valid) {
prev_wp(0) = (float)pos_sp_triplet.previous.lat;
prev_wp(1) = (float)pos_sp_triplet.previous.lon;
}
math::Vector<2> ground_speed_2d = {ground_speed(0), ground_speed(1)};
matrix::Vector2f ground_speed_2d = {ground_speed(0), ground_speed(1)};
float mission_throttle = _parameters.throttle_cruise;
@@ -412,8 +412,8 @@ GroundRoverPositionControl::task_main()
_sub_attitude.update();
_sub_sensors.update();
math::Vector<3> ground_speed(_global_pos.vel_n, _global_pos.vel_e, _global_pos.vel_d);
math::Vector<2> current_position((float)_global_pos.lat, (float)_global_pos.lon);
matrix::Vector3f ground_speed(_global_pos.vel_n, _global_pos.vel_e, _global_pos.vel_d);
matrix::Vector2f current_position((float)_global_pos.lat, (float)_global_pos.lon);
/*
* Attempt to control position, on success (= sensors present and not in manual mode),
@@ -460,7 +460,7 @@ GroundRoverPositionControl::task_main()
_gnd_pos_ctrl_status.target_bearing = _gnd_control.target_bearing();
_gnd_pos_ctrl_status.xtrack_error = _gnd_control.crosstrack_error();
math::Vector<2> curr_wp((float)_pos_sp_triplet.current.lat, (float)_pos_sp_triplet.current.lon);
matrix::Vector2f curr_wp((float)_pos_sp_triplet.current.lat, (float)_pos_sp_triplet.current.lon);
_gnd_pos_ctrl_status.wp_dist = get_distance_to_next_waypoint(current_position(0), current_position(1), curr_wp(0),
curr_wp(1));