From 3fc7aba178ebac76d166b9d888d65775dcfc94b7 Mon Sep 17 00:00:00 2001 From: Paul Riseborough Date: Tue, 5 Sep 2017 11:54:08 +1000 Subject: [PATCH] TECS: Use version in ECL library This change updates a number of interfaces to use the new TECS implementation from the ECL library. --- msg/tecs_status.msg | 1 - msg/vehicle_local_position.msg | 4 + src/lib/ecl | 2 +- src/modules/ekf2/ekf2_main.cpp | 7 ++ .../FixedwingPositionControl.cpp | 89 +++++++++---------- .../FixedwingPositionControl.hpp | 18 ++-- 6 files changed, 62 insertions(+), 59 deletions(-) diff --git a/msg/tecs_status.msg b/msg/tecs_status.msg index 7576112c57..45d1bd005d 100644 --- a/msg/tecs_status.msg +++ b/msg/tecs_status.msg @@ -11,7 +11,6 @@ float32 altitudeSp float32 altitude_filtered float32 flightPathAngleSp float32 flightPathAngle -float32 flightPathAngleFiltered float32 airspeedSp float32 airspeed_filtered float32 airspeedDerivativeSp diff --git a/msg/vehicle_local_position.msg b/msg/vehicle_local_position.msg index 2deb285a9c..6ad65e6d10 100644 --- a/msg/vehicle_local_position.msg +++ b/msg/vehicle_local_position.msg @@ -29,6 +29,10 @@ uint8 vxy_reset_counter float32 delta_vz uint8 vz_reset_counter +# Acceleration in NED frame +float32 ax # North velocity derivative in NED earth-fixed frame, (metres/sec^2) +float32 ay # East velocity derivative in NED earth-fixed frame, (metres/sec^2) +float32 az # Down velocity derivative in NED earth-fixed frame, (metres/sec^2) # Heading float32 yaw # Euler yaw angle transforming the tangent plane relative to NED earth-fixed frame, -PI..+PI, (radians) diff --git a/src/lib/ecl b/src/lib/ecl index 2bae55753d..f640dbcb53 160000 --- a/src/lib/ecl +++ b/src/lib/ecl @@ -1 +1 @@ -Subproject commit 2bae55753dc3cb83ae8bd92be5dcc76f27cf8948 +Subproject commit f640dbcb538c9d033385ad454fd3288cfd0efd4f diff --git a/src/modules/ekf2/ekf2_main.cpp b/src/modules/ekf2/ekf2_main.cpp index 985c175e8b..14ef0fbfae 100644 --- a/src/modules/ekf2/ekf2_main.cpp +++ b/src/modules/ekf2/ekf2_main.cpp @@ -904,6 +904,13 @@ void Ekf2::run() _ekf.get_pos_d_deriv(&pos_d_deriv); lpos.z_deriv = pos_d_deriv; // vertical position time derivative (m/s) + // Acceleration of body origin in local NED frame + float vel_deriv[3] = {}; + _ekf.get_vel_deriv_ned(vel_deriv); + lpos.ax = vel_deriv[0]; + lpos.ay = vel_deriv[1]; + lpos.az = vel_deriv[2]; + // TODO: better status reporting lpos.xy_valid = _ekf.local_position_is_valid() && !_vel_innov_preflt_fail; lpos.z_valid = !_vel_innov_preflt_fail; diff --git a/src/modules/fw_pos_control_l1/FixedwingPositionControl.cpp b/src/modules/fw_pos_control_l1/FixedwingPositionControl.cpp index 3b4fc230d8..c9f1467e3e 100644 --- a/src/modules/fw_pos_control_l1/FixedwingPositionControl.cpp +++ b/src/modules/fw_pos_control_l1/FixedwingPositionControl.cpp @@ -668,29 +668,6 @@ FixedwingPositionControl::control_position(const math::Vector<2> &curr_pos, cons _att_sp.fw_control_yaw = false; // by default we don't want yaw to be contoller directly with rudder _att_sp.apply_flaps = false; // by default we don't use flaps - /* 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); - - // tailsitters use the multicopter frame as reference, in fixed wing - // we need to use the fixed wing frame - if (_parameters.vtol_type == vtol_type::TAILSITTER && _vehicle_status.is_vtol) { - float tmp = accel_body(0); - accel_body(0) = -accel_body(2); - accel_body(2) = tmp; - } - - math::Vector<3> accel_earth{_R_nb * accel_body}; - - /* tell TECS to update its state, but let it know when it cannot actually control the plane */ - bool in_air_alt_control = (!_vehicle_land_detected.landed && - (_control_mode.flag_control_auto_enabled || - _control_mode.flag_control_velocity_enabled || - _control_mode.flag_control_altitude_enabled)); - - /* update TECS filters */ - _tecs.update_state(_global_pos.alt, _airspeed, _R_nb, - accel_body, accel_earth, (_global_pos.timestamp > 0), in_air_alt_control); - calculate_gndspeed_undershoot(curr_pos, ground_speed, pos_sp_prev, pos_sp_curr); // l1 navigation logic breaks down when wind speed exceeds max airspeed @@ -1474,7 +1451,7 @@ float FixedwingPositionControl::get_tecs_pitch() { if (_is_tecs_running) { - return _tecs.get_pitch_demand(); + return _tecs.get_pitch_setpoint(); } // return 0 to prevent stale tecs state when it's not running @@ -1485,7 +1462,7 @@ float FixedwingPositionControl::get_tecs_thrust() { if (_is_tecs_running) { - return _tecs.get_throttle_demand(); + return _tecs.get_throttle_setpoint(); } // return 0 to prevent stale tecs state when it's not running @@ -1514,6 +1491,7 @@ FixedwingPositionControl::task_main() * do subscriptions */ _global_pos_sub = orb_subscribe(ORB_ID(vehicle_global_position)); + _local_pos_sub = orb_subscribe(ORB_ID(vehicle_local_position)); _pos_sp_triplet_sub = orb_subscribe(ORB_ID(position_setpoint_triplet)); _control_mode_sub = orb_subscribe(ORB_ID(vehicle_control_mode)); _vehicle_attitude_sub = orb_subscribe(ORB_ID(vehicle_attitude)); @@ -1589,6 +1567,7 @@ FixedwingPositionControl::task_main() /* load local copies */ orb_copy(ORB_ID(vehicle_global_position), _global_pos_sub, &_global_pos); + orb_copy(ORB_ID(vehicle_local_position), _local_pos_sub, &_local_pos); // handle estimator reset events. we only adjust setpoins for manual modes if (_control_mode.flag_control_manual_enabled) { @@ -1821,6 +1800,28 @@ FixedwingPositionControl::tecs_update_pitch_throttle(float alt_sp, float airspee 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); + + // tailsitters use the multicopter frame as reference, in fixed wing + // we need to use the fixed wing frame + if (_parameters.vtol_type == vtol_type::TAILSITTER && _vehicle_status.is_vtol) { + float tmp = accel_body(0); + accel_body(0) = -accel_body(2); + accel_body(2) = tmp; + } + + /* tell TECS to update its state, but let it know when it cannot actually control the plane */ + bool in_air_alt_control = (!_vehicle_land_detected.landed && + (_control_mode.flag_control_auto_enabled || + _control_mode.flag_control_velocity_enabled || + _control_mode.flag_control_altitude_enabled)); + + /* update TECS vehicle state estimates */ + _tecs.update_vehicle_state_estimates(_airspeed, _R_nb, + accel_body, (_global_pos.timestamp > 0), in_air_alt_control, + _global_pos.alt, _local_pos.v_z_valid, _local_pos.vz, _local_pos.az); + _tecs.update_pitch_throttle(_R_nb, pitch_for_tecs, _global_pos.alt, alt_sp, airspeed_sp, _airspeed, _eas2tas, @@ -1828,13 +1829,10 @@ FixedwingPositionControl::tecs_update_pitch_throttle(float alt_sp, float airspee throttle_min, throttle_max, throttle_cruise, pitch_min_rad, pitch_max_rad); - TECS::tecs_state s {}; - _tecs.get_tecs_state(s); + tecs_status_s t; + t.timestamp = _tecs.timestamp(); - tecs_status_s t {}; - t.timestamp = s.timestamp; - - switch (s.mode) { + switch (_tecs.tecs_mode()) { case TECS::ECL_TECS_MODE_NORMAL: t.mode = tecs_status_s::TECS_MODE_NORMAL; break; @@ -1852,25 +1850,24 @@ FixedwingPositionControl::tecs_update_pitch_throttle(float alt_sp, float airspee break; } - t.altitudeSp = s.altitude_sp; - t.altitude_filtered = s.altitude_filtered; - t.airspeedSp = s.airspeed_sp; - t.airspeed_filtered = s.airspeed_filtered; + t.altitudeSp = _tecs.hgt_setpoint_adj(); + t.altitude_filtered = _tecs.vert_pos_state(); + t.airspeedSp = _tecs.TAS_setpoint_adj(); + t.airspeed_filtered = _tecs.tas_state(); - t.flightPathAngleSp = s.altitude_rate_sp; - t.flightPathAngle = s.altitude_rate; - t.flightPathAngleFiltered = s.altitude_rate; + t.flightPathAngleSp = _tecs.hgt_rate_setpoint(); + t.flightPathAngle = _tecs.vert_vel_state(); - t.airspeedDerivativeSp = s.airspeed_rate_sp; - t.airspeedDerivative = s.airspeed_rate; + t.airspeedDerivativeSp = _tecs.TAS_rate_setpoint(); + t.airspeedDerivative = _tecs.speed_derivative(); - t.totalEnergyError = s.total_energy_error; - t.totalEnergyRateError = s.total_energy_rate_error; - t.energyDistributionError = s.energy_distribution_error; - t.energyDistributionRateError = s.energy_distribution_rate_error; + t.totalEnergyError = _tecs.STE_error(); + t.totalEnergyRateError = _tecs.STE_rate_error(); + t.energyDistributionError = _tecs.SEB_error(); + t.energyDistributionRateError = _tecs.SEB_rate_error(); - t.throttle_integ = s.throttle_integ; - t.pitch_integ = s.pitch_integ; + t.throttle_integ = _tecs.throttle_integ_state(); + t.pitch_integ = _tecs.pitch_integ_state(); if (_tecs_status_pub != nullptr) { orb_publish(ORB_ID(tecs_status), _tecs_status_pub, &t); diff --git a/src/modules/fw_pos_control_l1/FixedwingPositionControl.hpp b/src/modules/fw_pos_control_l1/FixedwingPositionControl.hpp index 3753a2603e..1ab65cc2aa 100644 --- a/src/modules/fw_pos_control_l1/FixedwingPositionControl.hpp +++ b/src/modules/fw_pos_control_l1/FixedwingPositionControl.hpp @@ -33,21 +33,14 @@ /** - * @file fw_pos_control_l1_main.c + * @file fw_pos_control_l1_main.hpp * Implementation of a generic position controller based on the L1 norm. Outputs a bank / roll * angle, equivalent to a lateral motion (for copters and rovers). * - * Original publication for horizontal control class: - * S. Park, J. Deyst, and J. P. How, "A New Nonlinear Guidance Logic for Trajectory Tracking," - * Proceedings of the AIAA Guidance, Navigation and Control - * Conference, Aug 2004. AIAA-2004-4900. + * The implementation for the controllers is in the ECL library. This class only + * interfaces to the library. * - * Original implementation for total energy control class: - * Paul Riseborough, ECL Library, 2017 - * - * More details and acknowledgements in the referenced library headers. - * - * @author Lorenz Meier + * @author Lorenz Meier * @author Thomas Gubler * @author Andreas Antener */ @@ -85,6 +78,7 @@ #include #include #include +#include #include #include #include @@ -152,6 +146,7 @@ private: bool _task_running{false}; ///< if true, task is running in its mainloop */ int _global_pos_sub{-1}; + int _local_pos_sub{-1}; int _pos_sp_triplet_sub{-1}; int _control_mode_sub{-1}; ///< control mode subscription */ int _vehicle_attitude_sub{-1}; ///< vehicle attitude subscription */ @@ -175,6 +170,7 @@ private: vehicle_command_s _vehicle_command {}; ///< vehicle commands */ vehicle_control_mode_s _control_mode {}; ///< control mode */ vehicle_global_position_s _global_pos {}; ///< global vehicle position */ + vehicle_local_position_s _local_pos {}; ///< vehicle local position */ vehicle_land_detected_s _vehicle_land_detected {}; ///< vehicle land detected */ vehicle_status_s _vehicle_status {}; ///< vehicle status */