From b6b7fddb9f87d163f7305bf8e58bb7780eb585a0 Mon Sep 17 00:00:00 2001 From: Daniel Agar Date: Mon, 19 Mar 2018 18:09:54 -0400 Subject: [PATCH] TECS and L1 switch to matrix math library (#9101) --- src/lib/ecl | 2 +- .../FixedwingPositionControl.cpp | 52 +++++++++---------- .../FixedwingPositionControl.hpp | 26 +++------- .../runway_takeoff/RunwayTakeoff.cpp | 4 +- .../runway_takeoff/RunwayTakeoff.h | 4 +- .../GroundRoverPositionControl.cpp | 16 +++--- .../GroundRoverPositionControl.hpp | 2 +- 7 files changed, 47 insertions(+), 59 deletions(-) diff --git a/src/lib/ecl b/src/lib/ecl index 341f8962d2..35bc2cfcd9 160000 --- a/src/lib/ecl +++ b/src/lib/ecl @@ -1 +1 @@ -Subproject commit 341f8962d23ac5cd17297dc72952db0d6c284664 +Subproject commit 35bc2cfcd9512ec2a5a044a0f463d8f986bacb1b diff --git a/src/modules/fw_pos_control_l1/FixedwingPositionControl.cpp b/src/modules/fw_pos_control_l1/FixedwingPositionControl.cpp index d529d326c5..df2d691f46 100644 --- a/src/modules/fw_pos_control_l1/FixedwingPositionControl.cpp +++ b/src/modules/fw_pos_control_l1/FixedwingPositionControl.cpp @@ -356,11 +356,9 @@ FixedwingPositionControl::vehicle_attitude_poll() } /* set rotation matrix and euler angles */ - math::Quaternion q_att(_att.q); - _R_nb = q_att.to_dcm(); + _R_nb = Quatf(_att.q); - math::Vector<3> euler_angles; - euler_angles = _R_nb.to_euler(); + Eulerf euler_angles(_R_nb); _roll = euler_angles(0); _pitch = euler_angles(1); _yaw = euler_angles(2); @@ -432,13 +430,13 @@ FixedwingPositionControl::calculate_target_airspeed(float airspeed_demand) } void -FixedwingPositionControl::calculate_gndspeed_undershoot(const math::Vector<2> &curr_pos, - const math::Vector<2> &ground_speed, +FixedwingPositionControl::calculate_gndspeed_undershoot(const Vector2f &curr_pos, + const Vector2f &ground_speed, const position_setpoint_s &pos_sp_prev, const position_setpoint_s &pos_sp_curr) { if (pos_sp_curr.valid && !_l1_control.circle_mode()) { /* rotate ground speed vector with current attitude */ - math::Vector<2> yaw_vector(_R_nb(0, 0), _R_nb(1, 0)); + Vector2f yaw_vector(_R_nb(0, 0), _R_nb(1, 0)); yaw_vector.normalize(); float ground_speed_body = yaw_vector * ground_speed; @@ -625,7 +623,7 @@ FixedwingPositionControl::do_takeoff_help(float *hold_altitude, float *pitch_lim } bool -FixedwingPositionControl::control_position(const math::Vector<2> &curr_pos, const math::Vector<2> &ground_speed, +FixedwingPositionControl::control_position(const Vector2f &curr_pos, const Vector2f &ground_speed, const position_setpoint_s &pos_sp_prev, const position_setpoint_s &pos_sp_curr) { float dt = 0.01f; @@ -651,8 +649,8 @@ FixedwingPositionControl::control_position(const math::Vector<2> &curr_pos, cons // l1 navigation logic breaks down when wind speed exceeds max airspeed // compute 2D groundspeed from airspeed-heading projection - math::Vector<2> air_speed_2d{_airspeed * cosf(_yaw), _airspeed * sinf(_yaw)}; - math::Vector<2> nav_speed_2d{0.0f, 0.0f}; + Vector2f air_speed_2d{_airspeed * cosf(_yaw), _airspeed * sinf(_yaw)}; + Vector2f nav_speed_2d{0.0f, 0.0f}; // angle between air_speed_2d and ground_speed float air_gnd_angle = acosf((air_speed_2d * ground_speed) / (air_speed_2d.length() * ground_speed.length())); @@ -704,7 +702,7 @@ FixedwingPositionControl::control_position(const math::Vector<2> &curr_pos, cons _tecs.set_speed_weight(_parameters.speed_weight); /* current waypoint (the one currently heading for) */ - math::Vector<2> curr_wp((float)pos_sp_curr.lat, (float)pos_sp_curr.lon); + Vector2f curr_wp((float)pos_sp_curr.lat, (float)pos_sp_curr.lon); /* Initialize attitude controller integrator reset flags to 0 */ _att_sp.roll_reset_integral = false; @@ -712,7 +710,7 @@ FixedwingPositionControl::control_position(const math::Vector<2> &curr_pos, cons _att_sp.yaw_reset_integral = false; /* previous waypoint */ - math::Vector<2> prev_wp{0.0f, 0.0f}; + Vector2f prev_wp{0.0f, 0.0f}; if (pos_sp_prev.valid) { prev_wp(0) = (float)pos_sp_prev.lat; @@ -908,8 +906,8 @@ FixedwingPositionControl::control_position(const math::Vector<2> &curr_pos, cons get_waypoint_heading_distance(_hdg_hold_yaw, _hdg_hold_prev_wp, _hdg_hold_curr_wp, false); } - math::Vector<2> prev_wp{(float)_hdg_hold_prev_wp.lat, (float)_hdg_hold_prev_wp.lon}; - math::Vector<2> curr_wp{(float)_hdg_hold_curr_wp.lat, (float)_hdg_hold_curr_wp.lon}; + Vector2f prev_wp{(float)_hdg_hold_prev_wp.lat, (float)_hdg_hold_prev_wp.lon}; + Vector2f curr_wp{(float)_hdg_hold_curr_wp.lat, (float)_hdg_hold_curr_wp.lon}; /* populate l1 control setpoint */ _l1_control.navigate_waypoints(prev_wp, curr_wp, curr_pos, ground_speed); @@ -1058,12 +1056,12 @@ FixedwingPositionControl::control_position(const math::Vector<2> &curr_pos, cons } void -FixedwingPositionControl::control_takeoff(const math::Vector<2> &curr_pos, const math::Vector<2> &ground_speed, +FixedwingPositionControl::control_takeoff(const Vector2f &curr_pos, const Vector2f &ground_speed, const position_setpoint_s &pos_sp_prev, const position_setpoint_s &pos_sp_curr) { /* current waypoint (the one currently heading for) */ - math::Vector<2> curr_wp((float)pos_sp_curr.lat, (float)pos_sp_curr.lon); - math::Vector<2> prev_wp{0.0f, 0.0f}; /* previous waypoint */ + Vector2f curr_wp((float)pos_sp_curr.lat, (float)pos_sp_curr.lon); + Vector2f prev_wp{0.0f, 0.0f}; /* previous waypoint */ if (pos_sp_prev.valid) { prev_wp(0) = (float)pos_sp_prev.lat; @@ -1224,12 +1222,12 @@ FixedwingPositionControl::control_takeoff(const math::Vector<2> &curr_pos, const } void -FixedwingPositionControl::control_landing(const math::Vector<2> &curr_pos, const math::Vector<2> &ground_speed, +FixedwingPositionControl::control_landing(const Vector2f &curr_pos, const Vector2f &ground_speed, const position_setpoint_s &pos_sp_prev, const position_setpoint_s &pos_sp_curr) { /* current waypoint (the one currently heading for) */ - math::Vector<2> curr_wp((float)pos_sp_curr.lat, (float)pos_sp_curr.lon); - math::Vector<2> prev_wp{0.0f, 0.0f}; /* previous waypoint */ + Vector2f curr_wp((float)pos_sp_curr.lat, (float)pos_sp_curr.lon); + Vector2f prev_wp{0.0f, 0.0f}; /* previous waypoint */ if (pos_sp_prev.valid) { prev_wp(0) = (float)pos_sp_prev.lat; @@ -1637,8 +1635,8 @@ FixedwingPositionControl::run() vehicle_land_detected_poll(); vehicle_status_poll(); - math::Vector<2> curr_pos((float)_global_pos.lat, (float)_global_pos.lon); - math::Vector<2> ground_speed(_global_pos.vel_n, _global_pos.vel_e); + Vector2f curr_pos((float)_global_pos.lat, (float)_global_pos.lon); + Vector2f ground_speed(_global_pos.vel_n, _global_pos.vel_e); /* * Attempt to control position, on success (= sensors present and not in manual mode), @@ -1694,7 +1692,7 @@ FixedwingPositionControl::run() _fw_pos_ctrl_status.target_bearing = _l1_control.target_bearing(); _fw_pos_ctrl_status.xtrack_error = _l1_control.crosstrack_error(); - math::Vector<2> curr_wp((float)_pos_sp_triplet.current.lat, (float)_pos_sp_triplet.current.lon); + Vector2f curr_wp((float)_pos_sp_triplet.current.lat, (float)_pos_sp_triplet.current.lon); _fw_pos_ctrl_status.wp_dist = get_distance_to_next_waypoint(curr_pos(0), curr_pos(1), curr_wp(0), curr_wp(1)); @@ -1829,15 +1827,13 @@ FixedwingPositionControl::tecs_update_pitch_throttle(float alt_sp, float airspee // if the vehicle is a tailsitter we have to rotate the attitude by the pitch offset // between multirotor and fixed wing flight if (_parameters.vtol_type == vtol_type::TAILSITTER && _vehicle_status.is_vtol) { - math::Matrix<3, 3> R_offset; - R_offset.from_euler(0, M_PI_2_F, 0); - math::Matrix<3, 3> R_fixed_wing = _R_nb * R_offset; - math::Vector<3> euler = R_fixed_wing.to_euler(); + Dcmf R_offset = Eulerf(0, M_PI_2_F, 0); + Eulerf euler = Eulerf(_R_nb * R_offset); pitch_for_tecs = euler(1); } /* filter speed and altitude for controller */ - math::Vector<3> accel_body(_sub_sensors.get().accel_x, _sub_sensors.get().accel_y, _sub_sensors.get().accel_z); + Vector3f accel_body(_sub_sensors.get().accel_x, _sub_sensors.get().accel_y, _sub_sensors.get().accel_z); // tailsitters use the multicopter frame as reference, in fixed wing // we need to use the fixed wing frame diff --git a/src/modules/fw_pos_control_l1/FixedwingPositionControl.hpp b/src/modules/fw_pos_control_l1/FixedwingPositionControl.hpp index 034007bdd2..c44e179e94 100644 --- a/src/modules/fw_pos_control_l1/FixedwingPositionControl.hpp +++ b/src/modules/fw_pos_control_l1/FixedwingPositionControl.hpp @@ -237,7 +237,7 @@ private: float _groundspeed_undershoot{0.0f}; ///< ground speed error to min. speed in m/s - math::Matrix<3, 3> _R_nb; ///< current attitude + Dcmf _R_nb; ///< current attitude float _roll{0.0f}; float _pitch{0.0f}; float _yaw{0.0f}; @@ -437,19 +437,19 @@ private: */ bool update_desired_altitude(float dt); - bool control_position(const math::Vector<2> &curr_pos, const math::Vector<2> &ground_speed, - const position_setpoint_s &pos_sp_prev, const position_setpoint_s &pos_sp_curr); - void control_takeoff(const math::Vector<2> &curr_pos, const math::Vector<2> &ground_speed, - const position_setpoint_s &pos_sp_prev, const position_setpoint_s &pos_sp_curr); - void control_landing(const math::Vector<2> &curr_pos, const math::Vector<2> &ground_speed, - const position_setpoint_s &pos_sp_prev, const position_setpoint_s &pos_sp_curr); + bool control_position(const Vector2f &curr_pos, const Vector2f &ground_speed, const position_setpoint_s &pos_sp_prev, + const position_setpoint_s &pos_sp_curr); + void control_takeoff(const Vector2f &curr_pos, const Vector2f &ground_speed, const position_setpoint_s &pos_sp_prev, + const position_setpoint_s &pos_sp_curr); + void control_landing(const Vector2f &curr_pos, const Vector2f &ground_speed, const position_setpoint_s &pos_sp_prev, + const position_setpoint_s &pos_sp_curr); float get_tecs_pitch(); float get_tecs_thrust(); float get_demanded_airspeed(); float calculate_target_airspeed(float airspeed_demand); - void calculate_gndspeed_undershoot(const math::Vector<2> &curr_pos, const math::Vector<2> &ground_speed, + void calculate_gndspeed_undershoot(const Vector2f &curr_pos, const Vector2f &ground_speed, const position_setpoint_s &pos_sp_prev, const position_setpoint_s &pos_sp_curr); /** @@ -457,16 +457,6 @@ private: */ void handle_command(); - /** - * Shim for calling task_main from task_create. - */ - static void task_main_trampoline(int argc, char *argv[]); - - /** - * Main sensor collection task. - */ - void task_main(); - void reset_takeoff_state(); void reset_landing_state(); diff --git a/src/modules/fw_pos_control_l1/runway_takeoff/RunwayTakeoff.cpp b/src/modules/fw_pos_control_l1/runway_takeoff/RunwayTakeoff.cpp index 178dc42ce8..678008246c 100644 --- a/src/modules/fw_pos_control_l1/runway_takeoff/RunwayTakeoff.cpp +++ b/src/modules/fw_pos_control_l1/runway_takeoff/RunwayTakeoff.cpp @@ -48,6 +48,8 @@ #include #include +using matrix::Vector2f; + namespace runwaytakeoff { @@ -271,7 +273,7 @@ float RunwayTakeoff::getMaxPitch(float max) /* * Returns the "previous" (start) WP for navigation. */ -math::Vector<2> RunwayTakeoff::getStartWP() +Vector2f RunwayTakeoff::getStartWP() { return _start_wp; } diff --git a/src/modules/fw_pos_control_l1/runway_takeoff/RunwayTakeoff.h b/src/modules/fw_pos_control_l1/runway_takeoff/RunwayTakeoff.h index 0191d82b88..cfe28d5537 100644 --- a/src/modules/fw_pos_control_l1/runway_takeoff/RunwayTakeoff.h +++ b/src/modules/fw_pos_control_l1/runway_takeoff/RunwayTakeoff.h @@ -87,7 +87,7 @@ public: bool resetIntegrators(); float getMinPitch(float sp_min, float climbout_min, float min); float getMaxPitch(float max); - math::Vector<2> getStartWP(); + matrix::Vector2f getStartWP(); void reset(); @@ -100,7 +100,7 @@ private: float _init_yaw; bool _climbout; unsigned _throttle_ramp_time; - math::Vector<2> _start_wp; + matrix::Vector2f _start_wp; /** parameters **/ control::BlockParamBool _runway_takeoff_enabled; diff --git a/src/modules/gnd_pos_control/GroundRoverPositionControl.cpp b/src/modules/gnd_pos_control/GroundRoverPositionControl.cpp index 279aab4faa..25e3a89375 100644 --- a/src/modules/gnd_pos_control/GroundRoverPositionControl.cpp +++ b/src/modules/gnd_pos_control/GroundRoverPositionControl.cpp @@ -205,8 +205,8 @@ void GroundRoverPositionControl::gnd_pos_ctrl_status_publish() } bool -GroundRoverPositionControl::control_position(const math::Vector<2> ¤t_position, - const math::Vector<3> &ground_speed, const position_setpoint_triplet_s &pos_sp_triplet) +GroundRoverPositionControl::control_position(const matrix::Vector2f ¤t_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> ¤t_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)); diff --git a/src/modules/gnd_pos_control/GroundRoverPositionControl.hpp b/src/modules/gnd_pos_control/GroundRoverPositionControl.hpp index 8498f8d17b..51754247a0 100644 --- a/src/modules/gnd_pos_control/GroundRoverPositionControl.hpp +++ b/src/modules/gnd_pos_control/GroundRoverPositionControl.hpp @@ -196,7 +196,7 @@ private: /** * Control position. */ - bool control_position(const math::Vector<2> &global_pos, const math::Vector<3> &ground_speed, + bool control_position(const matrix::Vector2f &global_pos, const matrix::Vector3f &ground_speed, const position_setpoint_triplet_s &_pos_sp_triplet); /**