diff --git a/makefiles/config_aerocore_default.mk b/makefiles/config_aerocore_default.mk index 0d3934bd01..53a2ad1ab0 100644 --- a/makefiles/config_aerocore_default.mk +++ b/makefiles/config_aerocore_default.mk @@ -80,7 +80,6 @@ LIBRARIES += lib/mathlib/CMSIS MODULES += lib/mathlib MODULES += lib/mathlib/math/filter MODULES += lib/ecl -MODULES += lib/external_lgpl MODULES += lib/geo MODULES += lib/geo_lookup MODULES += lib/conversion diff --git a/makefiles/config_px4fmu-v1_default.mk b/makefiles/config_px4fmu-v1_default.mk index 9c7f326323..4d727aa4de 100644 --- a/makefiles/config_px4fmu-v1_default.mk +++ b/makefiles/config_px4fmu-v1_default.mk @@ -52,7 +52,6 @@ MODULES += systemcmds/pwm MODULES += systemcmds/esc_calib MODULES += systemcmds/reboot MODULES += systemcmds/top -MODULES += systemcmds/tests MODULES += systemcmds/config MODULES += systemcmds/nshterm MODULES += systemcmds/dumpfile @@ -67,12 +66,11 @@ MODULES += modules/mavlink MODULES += modules/gpio_led # -# Estimation modules (EKF/ SO3 / other filters) +# Estimation modules (EKF / other filters) # MODULES += modules/attitude_estimator_ekf MODULES += modules/ekf_att_pos_estimator MODULES += modules/position_estimator_inav -#MODULES += examples/flow_position_estimator # # Vehicle Control @@ -81,8 +79,6 @@ MODULES += modules/fw_pos_control_l1 MODULES += modules/fw_att_control MODULES += modules/mc_att_control MODULES += modules/mc_pos_control -#MODULES += examples/flow_position_control -#MODULES += examples/flow_speed_control # # Logging @@ -111,7 +107,6 @@ LIBRARIES += lib/mathlib/CMSIS MODULES += lib/mathlib MODULES += lib/mathlib/math/filter MODULES += lib/ecl -MODULES += lib/external_lgpl MODULES += lib/geo MODULES += lib/geo_lookup MODULES += lib/conversion diff --git a/makefiles/config_px4fmu-v2_default.mk b/makefiles/config_px4fmu-v2_default.mk index 29fb9ebac0..4b6b0a4d2f 100644 --- a/makefiles/config_px4fmu-v2_default.mk +++ b/makefiles/config_px4fmu-v2_default.mk @@ -120,7 +120,6 @@ LIBRARIES += lib/mathlib/CMSIS MODULES += lib/mathlib MODULES += lib/mathlib/math/filter MODULES += lib/ecl -MODULES += lib/external_lgpl MODULES += lib/geo MODULES += lib/geo_lookup MODULES += lib/conversion diff --git a/src/lib/launchdetection/module.mk b/src/lib/launchdetection/module.mk index de0174e372..d92f0bb45a 100644 --- a/src/lib/launchdetection/module.mk +++ b/src/lib/launchdetection/module.mk @@ -38,3 +38,5 @@ SRCS = LaunchDetector.cpp \ CatapultLaunchMethod.cpp \ launchdetection_params.c + +MAXOPTIMIZATION = -Os diff --git a/src/modules/ekf_att_pos_estimator/ekf_att_pos_estimator_main.cpp b/src/modules/ekf_att_pos_estimator/ekf_att_pos_estimator_main.cpp index 0a6d3fa8fe..272ad53440 100644 --- a/src/modules/ekf_att_pos_estimator/ekf_att_pos_estimator_main.cpp +++ b/src/modules/ekf_att_pos_estimator/ekf_att_pos_estimator_main.cpp @@ -74,6 +74,7 @@ #include #include #include +#include #include #include #include @@ -158,6 +159,7 @@ private: orb_advert_t _global_pos_pub; /**< global position */ orb_advert_t _local_pos_pub; /**< position in local frame */ orb_advert_t _estimator_status_pub; /**< status of the estimator */ + orb_advert_t _wind_pub; /**< wind estimate */ struct vehicle_attitude_s _att; /**< vehicle attitude */ struct gyro_report _gyro; @@ -169,6 +171,7 @@ private: struct vehicle_global_position_s _global_pos; /**< global vehicle position */ struct vehicle_local_position_s _local_pos; /**< local vehicle position */ struct vehicle_gps_position_s _gps; /**< GPS position */ + struct wind_estimate_s _wind; /**< Wind estimate */ struct gyro_scale _gyro_offsets; struct accel_scale _accel_offsets; @@ -312,6 +315,7 @@ FixedwingEstimator::FixedwingEstimator() : _global_pos_pub(-1), _local_pos_pub(-1), _estimator_status_pub(-1), + _wind_pub(-1), _att({}), _gyro({}), @@ -323,6 +327,7 @@ FixedwingEstimator::FixedwingEstimator() : _global_pos({}), _local_pos({}), _gps({}), + _wind({}), _gyro_offsets({}), _accel_offsets({}), @@ -1328,13 +1333,31 @@ FixedwingEstimator::task_main() /* lazily publish the global position only once available */ if (_global_pos_pub > 0) { - /* publish the attitude setpoint */ + /* publish the global position */ orb_publish(ORB_ID(vehicle_global_position), _global_pos_pub, &_global_pos); } else { /* advertise and publish */ _global_pos_pub = orb_advertise(ORB_ID(vehicle_global_position), &_global_pos); } + + if (hrt_elapsed_time(&_wind.timestamp) > 99000) { + _wind.timestamp = _global_pos.timestamp; + _wind.windspeed_north = _ekf->states[14]; + _wind.windspeed_east = _ekf->states[15]; + _wind.covariance_north = _ekf->P[14][14]; + _wind.covariance_east = _ekf->P[15][15]; + + /* lazily publish the wind estimate only once available */ + if (_wind_pub > 0) { + /* publish the wind estimate */ + orb_publish(ORB_ID(wind_estimate), _wind_pub, &_wind); + + } else { + /* advertise and publish */ + _wind_pub = orb_advertise(ORB_ID(wind_estimate), &_wind); + } + } } } @@ -1396,9 +1419,11 @@ FixedwingEstimator::print_status() printf("states (vel m/s) [5-7]: %8.4f, %8.4f, %8.4f\n", (double)_ekf->states[4], (double)_ekf->states[5], (double)_ekf->states[6]); printf("states (pos m) [8-10]: %8.4f, %8.4f, %8.4f\n", (double)_ekf->states[7], (double)_ekf->states[8], (double)_ekf->states[9]); printf("states (delta ang) [11-13]: %8.4f, %8.4f, %8.4f\n", (double)_ekf->states[10], (double)_ekf->states[11], (double)_ekf->states[12]); - printf("states (wind) [14-15]: %8.4f, %8.4f\n", (double)_ekf->states[13], (double)_ekf->states[14]); - printf("states (earth mag) [16-18]: %8.4f, %8.4f, %8.4f\n", (double)_ekf->states[15], (double)_ekf->states[16], (double)_ekf->states[17]); - printf("states (body mag) [19-21]: %8.4f, %8.4f, %8.4f\n", (double)_ekf->states[18], (double)_ekf->states[19], (double)_ekf->states[20]); + printf("states (accel offs) [14]: %8.4f\n", (double)_ekf->states[13]); + printf("states (wind) [15-16]: %8.4f, %8.4f\n", (double)_ekf->states[14], (double)_ekf->states[15]); + printf("states (earth mag) [17-19]: %8.4f, %8.4f, %8.4f\n", (double)_ekf->states[16], (double)_ekf->states[17], (double)_ekf->states[18]); + printf("states (body mag) [20-22]: %8.4f, %8.4f, %8.4f\n", (double)_ekf->states[19], (double)_ekf->states[20], (double)_ekf->states[21]); + printf("states (terrain) [23]: %8.4f\n", (double)_ekf->states[22]); printf("states: %s %s %s %s %s %s %s %s %s %s\n", (_ekf->statesInitialised) ? "INITIALIZED" : "NON_INIT", (_ekf->onGround) ? "ON_GROUND" : "AIRBORNE", diff --git a/src/modules/ekf_att_pos_estimator/estimator.cpp b/src/modules/ekf_att_pos_estimator/estimator.cpp index 5db1adbb3c..1320b46689 100644 --- a/src/modules/ekf_att_pos_estimator/estimator.cpp +++ b/src/modules/ekf_att_pos_estimator/estimator.cpp @@ -2042,10 +2042,10 @@ float AttPosEKF::ConstrainFloat(float val, float min, float max) float ret; if (val > max) { ret = max; - ekf_debug("> max: %8.4f, val: %8.4f", max, val); + ekf_debug("> max: %8.4f, val: %8.4f", (double)max, (double)val); } else if (val < min) { ret = min; - ekf_debug("< min: %8.4f, val: %8.4f", min, val); + ekf_debug("< min: %8.4f, val: %8.4f", (double)min, (double)val); } else { ret = val; } diff --git a/src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp b/src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp index 0e065211e9..abab74c08f 100644 --- a/src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp +++ b/src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp @@ -43,8 +43,8 @@ * Proceedings of the AIAA Guidance, Navigation and Control * Conference, Aug 2004. AIAA-2004-4900. * - * Original implementation for total energy control class: - * Paul Riseborough and Andrew Tridgell, 2013 (code in lib/external_lgpl) + * Implementation for total energy control class: + * Thomas Gubler * * More details and acknowledgements in the referenced library headers. * @@ -88,9 +88,9 @@ #include #include #include -#include #include #include "landingslope.h" +#include "mtecs/mTecs.h" /** @@ -200,7 +200,8 @@ private: math::Matrix<3, 3> _R_nb; ///< current attitude ECL_L1_Pos_Controller _l1_control; - TECS _tecs; + fwPosctrl::mTecs _mTecs; + bool _was_pos_control_mode; struct { float l1_period; @@ -343,11 +344,11 @@ private: /** * Control position. */ - bool control_position(const math::Vector<2> &global_pos, const math::Vector<2> &ground_speed, + bool control_position(const math::Vector<2> &global_pos, const math::Vector<3> &ground_speed, const struct position_setpoint_triplet_s &_pos_sp_triplet); float calculate_target_airspeed(float airspeed_demand); - void calculate_gndspeed_undershoot(const math::Vector<2> ¤t_position, const math::Vector<2> &ground_speed, const struct position_setpoint_triplet_s &pos_sp_triplet); + void calculate_gndspeed_undershoot(const math::Vector<2> ¤t_position, const math::Vector<2> &ground_speed_2d, const struct position_setpoint_triplet_s &pos_sp_triplet); /** * Shim for calling task_main from task_create. @@ -368,6 +369,19 @@ private: * Reset landing state */ void reset_landing_state(); + + /* + * Call TECS : a wrapper function to call one of the TECS implementations (mTECS is called only if enabled via parameter) + * XXX need to clean up/remove this function once mtecs fully replaces TECS + */ + void tecs_update_pitch_throttle(float alt_sp, float v_sp, float eas2tas, + float pitch_min_rad, float pitch_max_rad, + float throttle_min, float throttle_max, float throttle_cruise, + bool climbout_mode, float climbout_pitch_min_rad, + float altitude, + const math::Vector<3> &ground_speed, + tecs_mode mode = TECS_MODE_NORMAL); + }; namespace l1_control @@ -431,6 +445,8 @@ FixedwingPositionControl::FixedwingPositionControl() : _global_pos(), _pos_sp_triplet(), _sensor_combined(), + _mTecs(), + _was_pos_control_mode(false), _range_finder() { _nav_capabilities.turn_distance = 0.0f; @@ -550,23 +566,6 @@ FixedwingPositionControl::parameters_update() _l1_control.set_l1_period(_parameters.l1_period); _l1_control.set_l1_roll_limit(math::radians(_parameters.roll_limit)); - _tecs.set_time_const(_parameters.time_const); - _tecs.set_min_sink_rate(_parameters.min_sink_rate); - _tecs.set_max_sink_rate(_parameters.max_sink_rate); - _tecs.set_throttle_damp(_parameters.throttle_damp); - _tecs.set_integrator_gain(_parameters.integrator_gain); - _tecs.set_vertical_accel_limit(_parameters.vertical_accel_limit); - _tecs.set_height_comp_filter_omega(_parameters.height_comp_filter_omega); - _tecs.set_speed_comp_filter_omega(_parameters.speed_comp_filter_omega); - _tecs.set_roll_throttle_compensation(_parameters.roll_throttle_compensation); - _tecs.set_speed_weight(_parameters.speed_weight); - _tecs.set_pitch_damping(_parameters.pitch_damping); - _tecs.set_indicated_airspeed_min(_parameters.airspeed_min); - _tecs.set_indicated_airspeed_max(_parameters.airspeed_max); - _tecs.set_max_climb_rate(_parameters.max_climb_rate); - _tecs.set_heightrate_p(_parameters.heightrate_p); - _tecs.set_speedrate_p(_parameters.speedrate_p); - /* sanity check parameters */ if (_parameters.airspeed_max < _parameters.airspeed_min || _parameters.airspeed_max < 5.0f || @@ -589,6 +588,9 @@ FixedwingPositionControl::parameters_update() /* Update Launch Detector Parameters */ launchDetector.updateParams(); + /* Update the mTecs */ + _mTecs.updateParams(); + return OK; } @@ -635,9 +637,6 @@ FixedwingPositionControl::vehicle_airspeed_poll() } } - /* update TECS state */ - _tecs.enable_airspeed(_airspeed_valid); - return airspeed_updated; } @@ -734,7 +733,7 @@ FixedwingPositionControl::calculate_target_airspeed(float airspeed_demand) } void -FixedwingPositionControl::calculate_gndspeed_undershoot(const math::Vector<2> ¤t_position, const math::Vector<2> &ground_speed, const struct position_setpoint_triplet_s &pos_sp_triplet) +FixedwingPositionControl::calculate_gndspeed_undershoot(const math::Vector<2> ¤t_position, const math::Vector<2> &ground_speed_2d, const struct position_setpoint_triplet_s &pos_sp_triplet) { if (_global_pos_valid && !(pos_sp_triplet.current.type == SETPOINT_TYPE_LOITER)) { @@ -742,7 +741,7 @@ FixedwingPositionControl::calculate_gndspeed_undershoot(const math::Vector<2> &c /* rotate ground speed vector with current attitude */ math::Vector<2> yaw_vector(_R_nb(0, 0), _R_nb(1, 0)); yaw_vector.normalize(); - float ground_speed_body = yaw_vector * ground_speed; + float ground_speed_body = yaw_vector * ground_speed_2d; /* The minimum desired ground speed is the minimum airspeed projected on to the ground using the altitude and horizontal difference between the waypoints if available*/ float distance = 0.0f; @@ -801,12 +800,13 @@ float FixedwingPositionControl::get_relative_landingalt(float land_setpoint_alt, } bool -FixedwingPositionControl::control_position(const math::Vector<2> ¤t_position, const math::Vector<2> &ground_speed, +FixedwingPositionControl::control_position(const math::Vector<2> ¤t_position, const math::Vector<3> &ground_speed, const struct position_setpoint_triplet_s &pos_sp_triplet) { bool setpoint = true; - calculate_gndspeed_undershoot(current_position, ground_speed, pos_sp_triplet); + math::Vector<2> ground_speed_2d = {ground_speed(0), ground_speed(1)}; + calculate_gndspeed_undershoot(current_position, ground_speed_2d, pos_sp_triplet); float eas2tas = 1.0f; // XXX calculate actual number based on current measurements @@ -817,7 +817,6 @@ FixedwingPositionControl::control_position(const math::Vector<2> ¤t_positi math::Vector<3> accel_body(_sensor_combined.accelerometer_m_s2); math::Vector<3> accel_earth = _R_nb * accel_body; - _tecs.update_50hz(baro_altitude, _airspeed.indicated_airspeed_m_s, _R_nb, accel_body, accel_earth); float altitude_error = _pos_sp_triplet.current.alt - _global_pos.alt; /* no throttle limit as default */ @@ -829,12 +828,19 @@ FixedwingPositionControl::control_position(const math::Vector<2> ¤t_positi // else integrators should be constantly reset. if (_control_mode.flag_control_position_enabled) { + if (!_was_pos_control_mode) { + /* reset integrators */ + if (_mTecs.getEnabled()) { + _mTecs.resetIntegrators(); + _mTecs.resetDerivatives(_airspeed.true_airspeed_m_s); + } + } + + _was_pos_control_mode = true; + /* get circle mode */ bool was_circle_mode = _l1_control.circle_mode(); - /* restore speed weight, in case changed intermittently (e.g. in landing handling) */ - _tecs.set_speed_weight(_parameters.speed_weight); - /* current waypoint (the one currently heading for) */ math::Vector<2> next_wp((float)pos_sp_triplet.current.lat, (float)pos_sp_triplet.current.lon); @@ -865,29 +871,27 @@ FixedwingPositionControl::control_position(const math::Vector<2> ¤t_positi if (pos_sp_triplet.current.type == SETPOINT_TYPE_NORMAL) { /* waypoint is a plain navigation waypoint */ - _l1_control.navigate_waypoints(prev_wp, curr_wp, current_position, ground_speed); + _l1_control.navigate_waypoints(prev_wp, curr_wp, current_position, ground_speed_2d); _att_sp.roll_body = _l1_control.nav_roll(); _att_sp.yaw_body = _l1_control.nav_bearing(); - _tecs.update_pitch_throttle(_R_nb, _att.pitch, _global_pos.alt, _pos_sp_triplet.current.alt, calculate_target_airspeed(_parameters.airspeed_trim), - _airspeed.indicated_airspeed_m_s, eas2tas, - false, math::radians(_parameters.pitch_limit_min), - _parameters.throttle_min, _parameters.throttle_max, _parameters.throttle_cruise, - math::radians(_parameters.pitch_limit_min), math::radians(_parameters.pitch_limit_max)); + tecs_update_pitch_throttle(_pos_sp_triplet.current.alt, calculate_target_airspeed(_parameters.airspeed_trim), eas2tas, + math::radians(_parameters.pitch_limit_min), math::radians(_parameters.pitch_limit_max), + _parameters.throttle_min, _parameters.throttle_max, _parameters.throttle_cruise, + false, math::radians(_parameters.pitch_limit_min), _global_pos.alt, ground_speed); } else if (pos_sp_triplet.current.type == SETPOINT_TYPE_LOITER) { /* waypoint is a loiter waypoint */ _l1_control.navigate_loiter(curr_wp, current_position, pos_sp_triplet.current.loiter_radius, - pos_sp_triplet.current.loiter_direction, ground_speed); + pos_sp_triplet.current.loiter_direction, ground_speed_2d); _att_sp.roll_body = _l1_control.nav_roll(); _att_sp.yaw_body = _l1_control.nav_bearing(); - _tecs.update_pitch_throttle(_R_nb, _att.pitch, _global_pos.alt, _pos_sp_triplet.current.alt, calculate_target_airspeed(_parameters.airspeed_trim), - _airspeed.indicated_airspeed_m_s, eas2tas, - false, math::radians(_parameters.pitch_limit_min), - _parameters.throttle_min, _parameters.throttle_max, _parameters.throttle_cruise, - math::radians(_parameters.pitch_limit_min), math::radians(_parameters.pitch_limit_max)); + tecs_update_pitch_throttle(_pos_sp_triplet.current.alt, calculate_target_airspeed(_parameters.airspeed_trim), eas2tas, + math::radians(_parameters.pitch_limit_min), math::radians(_parameters.pitch_limit_max), + _parameters.throttle_min, _parameters.throttle_max, _parameters.throttle_cruise, + false, math::radians(_parameters.pitch_limit_min), _global_pos.alt, ground_speed); } else if (pos_sp_triplet.current.type == SETPOINT_TYPE_LAND) { @@ -912,7 +916,7 @@ FixedwingPositionControl::control_position(const math::Vector<2> ¤t_positi // warnx("NORET: %d, target_bearing: %d, yaw: %d", (int)land_noreturn_horizontal, (int)math::degrees(target_bearing), (int)math::degrees(_att.yaw)); - _l1_control.navigate_heading(target_bearing, _att.yaw, ground_speed); + _l1_control.navigate_heading(target_bearing, _att.yaw, ground_speed_2d); /* limit roll motion to prevent wings from touching the ground first */ _att_sp.roll_body = math::constrain(_att_sp.roll_body, math::radians(-10.0f), math::radians(10.0f)); @@ -922,7 +926,7 @@ FixedwingPositionControl::control_position(const math::Vector<2> ¤t_positi } else { /* normal navigation */ - _l1_control.navigate_waypoints(prev_wp, curr_wp, current_position, ground_speed); + _l1_control.navigate_waypoints(prev_wp, curr_wp, current_position, ground_speed_2d); } _att_sp.roll_body = _l1_control.nav_roll(); @@ -946,7 +950,7 @@ FixedwingPositionControl::control_position(const math::Vector<2> ¤t_positi /* Calculate distance (to landing waypoint) and altitude of last ordinary waypoint L */ float L_wp_distance = get_distance_to_next_waypoint(prev_wp(0), prev_wp(1), curr_wp(0), curr_wp(1)); - float L_altitude_rel = landingslope.getLandingSlopeRelativeAltitude(L_wp_distance); + float L_altitude_rel = _pos_sp_triplet.previous.valid ? _pos_sp_triplet.previous.alt - _pos_sp_triplet.current.alt : 0.0f; float bearing_airplane_currwp = get_bearing_to_next_waypoint(current_position(0), current_position(1), curr_wp(0), curr_wp(1)); float landing_slope_alt_rel_desired = landingslope.getLandingSlopeRelativeAltitudeSave(wp_distance, bearing_lastwp_currwp, bearing_airplane_currwp); @@ -981,11 +985,13 @@ FixedwingPositionControl::control_position(const math::Vector<2> ¤t_positi land_stayonground = true; } - _tecs.update_pitch_throttle(_R_nb, _att.pitch, _pos_sp_triplet.current.alt + relative_alt, _pos_sp_triplet.current.alt + flare_curve_alt_rel, calculate_target_airspeed(airspeed_land), - _airspeed.indicated_airspeed_m_s, eas2tas, - false, flare_pitch_angle_rad, - 0.0f, throttle_max, throttle_land, - flare_pitch_angle_rad, math::radians(15.0f)); + tecs_update_pitch_throttle(_pos_sp_triplet.current.alt + flare_curve_alt_rel, + calculate_target_airspeed(airspeed_land), eas2tas, + flare_pitch_angle_rad, math::radians(15.0f), + 0.0f, throttle_max, throttle_land, + false, flare_pitch_angle_rad, + _pos_sp_triplet.current.alt + relative_alt, ground_speed, + land_motor_lim ? TECS_MODE_LAND_THROTTLELIM : TECS_MODE_LAND); if (!land_noreturn_vertical) { mavlink_log_info(_mavlink_fd, "#audio: Landing, flaring"); @@ -998,11 +1004,15 @@ FixedwingPositionControl::control_position(const math::Vector<2> ¤t_positi /* intersect glide slope: * minimize speed to approach speed - * if current position is higher or within 10m of slope follow the glide slope - * if current position is below slope -10m continue on maximum of previous wp altitude or L_altitude until the intersection with the slope + * if current position is higher than the slope follow the glide slope (sink to the + * glide slope) + * also if the system captures the slope it should stay + * on the slope (bool land_onslope) + * if current position is below the slope continue at previous wp altitude + * until the intersection with slope * */ float altitude_desired_rel = relative_alt; - if (relative_alt > landing_slope_alt_rel_desired - 10.0f) { + if (relative_alt > landing_slope_alt_rel_desired || land_onslope) { /* stay on slope */ altitude_desired_rel = landing_slope_alt_rel_desired; if (!land_onslope) { @@ -1011,14 +1021,20 @@ FixedwingPositionControl::control_position(const math::Vector<2> ¤t_positi } } else { /* continue horizontally */ - altitude_desired_rel = math::max(relative_alt, L_altitude_rel); + altitude_desired_rel = _pos_sp_triplet.previous.valid ? L_altitude_rel : relative_alt; } - _tecs.update_pitch_throttle(_R_nb, _att.pitch, _pos_sp_triplet.current.alt + relative_alt, _pos_sp_triplet.current.alt + altitude_desired_rel, calculate_target_airspeed(airspeed_approach), - _airspeed.indicated_airspeed_m_s, eas2tas, - false, math::radians(_parameters.pitch_limit_min), - _parameters.throttle_min, _parameters.throttle_max, _parameters.throttle_cruise, - math::radians(_parameters.pitch_limit_min), math::radians(_parameters.pitch_limit_max)); + tecs_update_pitch_throttle(_pos_sp_triplet.current.alt + altitude_desired_rel, + calculate_target_airspeed(airspeed_approach), eas2tas, + math::radians(_parameters.pitch_limit_min), + math::radians(_parameters.pitch_limit_max), + _parameters.throttle_min, + _parameters.throttle_max, + _parameters.throttle_cruise, + false, + math::radians(_parameters.pitch_limit_min), + _pos_sp_triplet.current.alt + relative_alt, + ground_speed); } } else if (pos_sp_triplet.current.type == SETPOINT_TYPE_TAKEOFF) { @@ -1051,7 +1067,7 @@ FixedwingPositionControl::control_position(const math::Vector<2> ¤t_positi } } - _l1_control.navigate_waypoints(prev_wp, curr_wp, current_position, ground_speed); + _l1_control.navigate_waypoints(prev_wp, curr_wp, current_position, ground_speed_2d); _att_sp.roll_body = _l1_control.nav_roll(); _att_sp.yaw_body = _l1_control.nav_bearing(); @@ -1062,22 +1078,36 @@ FixedwingPositionControl::control_position(const math::Vector<2> ¤t_positi if (altitude_error > 15.0f) { /* enforce a minimum of 10 degrees pitch up on takeoff, or take parameter */ - _tecs.update_pitch_throttle(_R_nb, _att.pitch, _global_pos.alt, _pos_sp_triplet.current.alt, calculate_target_airspeed(1.3f * _parameters.airspeed_min), - _airspeed.indicated_airspeed_m_s, eas2tas, - true, math::max(math::radians(pos_sp_triplet.current.pitch_min), math::radians(10.0f)), - _parameters.throttle_min, _parameters.throttle_max, _parameters.throttle_cruise, - math::radians(_parameters.pitch_limit_min), math::radians(_parameters.pitch_limit_max)); + tecs_update_pitch_throttle(_pos_sp_triplet.current.alt, + calculate_target_airspeed(1.3f * _parameters.airspeed_min), + eas2tas, + math::radians(_parameters.pitch_limit_min), + math::radians(_parameters.pitch_limit_max), + _parameters.throttle_min, _parameters.throttle_max, + _parameters.throttle_cruise, + true, + math::max(math::radians(pos_sp_triplet.current.pitch_min), + math::radians(10.0f)), + _global_pos.alt, + ground_speed, + TECS_MODE_TAKEOFF); /* limit roll motion to ensure enough lift */ _att_sp.roll_body = math::constrain(_att_sp.roll_body, math::radians(-15.0f), math::radians(15.0f)); } else { - - _tecs.update_pitch_throttle(_R_nb, _att.pitch, _global_pos.alt, _pos_sp_triplet.current.alt, calculate_target_airspeed(_parameters.airspeed_trim), - _airspeed.indicated_airspeed_m_s, eas2tas, - false, math::radians(_parameters.pitch_limit_min), - _parameters.throttle_min, _parameters.throttle_max, _parameters.throttle_cruise, - math::radians(_parameters.pitch_limit_min), math::radians(_parameters.pitch_limit_max)); + tecs_update_pitch_throttle(_pos_sp_triplet.current.alt, + calculate_target_airspeed(_parameters.airspeed_trim), + eas2tas, + math::radians(_parameters.pitch_limit_min), + math::radians(_parameters.pitch_limit_max), + _parameters.throttle_min, + _parameters.throttle_max, + _parameters.throttle_cruise, + false, + math::radians(_parameters.pitch_limit_min), + _global_pos.alt, + ground_speed); } } else { @@ -1111,19 +1141,21 @@ FixedwingPositionControl::control_position(const math::Vector<2> ¤t_positi } else if (0/* posctrl mode enabled */) { + _was_pos_control_mode = false; + /** POSCTRL FLIGHT **/ - if (0/* switched from another mode to posctrl */) { - _altctrl_hold_heading = _att.yaw; - } + if (0/* switched from another mode to posctrl */) { + _altctrl_hold_heading = _att.yaw; + } - if (0/* posctrl on and manual control yaw non-zero */) { - _altctrl_hold_heading = _att.yaw + _manual.r; - } + if (0/* posctrl on and manual control yaw non-zero */) { + _altctrl_hold_heading = _att.yaw + _manual.r; + } - //XXX not used + //XXX not used - /* climb out control */ + /* climb out control */ // bool climb_out = false; // // /* user wants to climb out */ @@ -1131,25 +1163,26 @@ FixedwingPositionControl::control_position(const math::Vector<2> ¤t_positi // climb_out = true; // } - /* if in altctrl mode, set airspeed based on manual control */ + /* if in altctrl mode, set airspeed based on manual control */ - // XXX check if ground speed undershoot should be applied here - float altctrl_airspeed = _parameters.airspeed_min + - (_parameters.airspeed_max - _parameters.airspeed_min) * - _manual.z; + // XXX check if ground speed undershoot should be applied here + float altctrl_airspeed = _parameters.airspeed_min + + (_parameters.airspeed_max - _parameters.airspeed_min) * + _manual.z; - _l1_control.navigate_heading(_altctrl_hold_heading, _att.yaw, ground_speed); + _l1_control.navigate_heading(_altctrl_hold_heading, _att.yaw, ground_speed_2d); _att_sp.roll_body = _l1_control.nav_roll(); _att_sp.yaw_body = _l1_control.nav_bearing(); - _tecs.update_pitch_throttle(_R_nb, _att.pitch, _global_pos.alt, _global_pos.alt + _manual.x * 2.0f, - altctrl_airspeed, - _airspeed.indicated_airspeed_m_s, eas2tas, - false, _parameters.pitch_limit_min, - _parameters.throttle_min, _parameters.throttle_max, _parameters.throttle_cruise, - _parameters.pitch_limit_min, _parameters.pitch_limit_max); + + tecs_update_pitch_throttle(_global_pos.alt + _manual.x * 2.0f, altctrl_airspeed, eas2tas, + math::radians(_parameters.pitch_limit_min), math::radians(_parameters.pitch_limit_max), + _parameters.throttle_min, _parameters.throttle_max, _parameters.throttle_cruise, + false, math::radians(_parameters.pitch_limit_min), _global_pos.alt, ground_speed); } else if (0/* altctrl mode enabled */) { + _was_pos_control_mode = false; + /** ALTCTRL FLIGHT **/ if (0/* switched from another mode to altctrl */) { @@ -1170,8 +1203,6 @@ FixedwingPositionControl::control_position(const math::Vector<2> ¤t_positi /* user switched off throttle */ if (_manual.z < 0.1f) { throttle_max = 0.0f; - /* switch to pure pitch based altitude control, give up speed */ - _tecs.set_speed_weight(0.0f); } /* climb out control */ @@ -1182,18 +1213,19 @@ FixedwingPositionControl::control_position(const math::Vector<2> ¤t_positi climb_out = true; } - _l1_control.navigate_heading(_altctrl_hold_heading, _att.yaw, ground_speed); + _l1_control.navigate_heading(_altctrl_hold_heading, _att.yaw, ground_speed_2d); _att_sp.roll_body = _manual.y; _att_sp.yaw_body = _manual.r; - _tecs.update_pitch_throttle(_R_nb, _att.pitch, _global_pos.alt, _global_pos.alt + _manual.x * 2.0f, - altctrl_airspeed, - _airspeed.indicated_airspeed_m_s, eas2tas, - climb_out, _parameters.pitch_limit_min, - _parameters.throttle_min, _parameters.throttle_max, _parameters.throttle_cruise, - _parameters.pitch_limit_min, _parameters.pitch_limit_max); + tecs_update_pitch_throttle(_global_pos.alt + _manual.x * 2.0f, altctrl_airspeed, eas2tas, + math::radians(_parameters.pitch_limit_min), math::radians(_parameters.pitch_limit_max), + _parameters.throttle_min, _parameters.throttle_max, _parameters.throttle_cruise, + climb_out, math::radians(_parameters.pitch_limit_min), + _global_pos.alt, ground_speed); } else { + _was_pos_control_mode = false; + /** MANUAL FLIGHT **/ /* no flight mode applies, do not publish an attitude setpoint */ @@ -1210,9 +1242,9 @@ FixedwingPositionControl::control_position(const math::Vector<2> ¤t_positi _att_sp.thrust = launchDetector.getThrottlePreTakeoff(); } else { - _att_sp.thrust = math::min(_tecs.get_throttle_demand(), throttle_max); + _att_sp.thrust = math::min(_mTecs.getThrottleSetpoint(), throttle_max); } - _att_sp.pitch_body = _tecs.get_pitch_demand(); + _att_sp.pitch_body = _mTecs.getPitchSetpoint(); if (_control_mode.flag_control_position_enabled) { last_manual = false; @@ -1326,7 +1358,7 @@ FixedwingPositionControl::task_main() range_finder_poll(); // vehicle_baro_poll(); - math::Vector<2> ground_speed(_global_pos.vel_n, _global_pos.vel_e); + 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); /* @@ -1388,6 +1420,30 @@ void FixedwingPositionControl::reset_landing_state() land_onslope = false; } +void FixedwingPositionControl::tecs_update_pitch_throttle(float alt_sp, float v_sp, float eas2tas, + float pitch_min_rad, float pitch_max_rad, + float throttle_min, float throttle_max, float throttle_cruise, + bool climbout_mode, float climbout_pitch_min_rad, + float altitude, + const math::Vector<3> &ground_speed, + tecs_mode mode) +{ + /* Using mtecs library: prepare arguments for mtecs call */ + float flightPathAngle = 0.0f; + float ground_speed_length = ground_speed.length(); + if (ground_speed_length > FLT_EPSILON) { + flightPathAngle = -asinf(ground_speed(2)/ground_speed_length); + } + fwPosctrl::LimitOverride limitOverride; + if (climbout_mode) { + limitOverride.enablePitchMinOverride(M_RAD_TO_DEG_F * climbout_pitch_min_rad); + } else { + limitOverride.disablePitchMinOverride(); + } + _mTecs.updateAltitudeSpeed(flightPathAngle, altitude, alt_sp, _airspeed.true_airspeed_m_s, v_sp, mode, + limitOverride); +} + int FixedwingPositionControl::start() { diff --git a/src/modules/fw_pos_control_l1/module.mk b/src/modules/fw_pos_control_l1/module.mk index cf419ec7ea..af155fe089 100644 --- a/src/modules/fw_pos_control_l1/module.mk +++ b/src/modules/fw_pos_control_l1/module.mk @@ -39,4 +39,7 @@ MODULE_COMMAND = fw_pos_control_l1 SRCS = fw_pos_control_l1_main.cpp \ fw_pos_control_l1_params.c \ - landingslope.cpp + landingslope.cpp \ + mtecs/mTecs.cpp \ + mtecs/limitoverride.cpp \ + mtecs/mTecs_params.c diff --git a/src/modules/fw_pos_control_l1/mtecs/limitoverride.cpp b/src/modules/fw_pos_control_l1/mtecs/limitoverride.cpp new file mode 100644 index 0000000000..58795edb65 --- /dev/null +++ b/src/modules/fw_pos_control_l1/mtecs/limitoverride.cpp @@ -0,0 +1,71 @@ +/**************************************************************************** + * + * Copyright (c) 2013 PX4 Development Team. All rights reserved. + * Author: @author Thomas Gubler + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ + + +/** + * @file limitoverride.cpp + * + * @author Thomas Gubler + */ + +#include "limitoverride.h" + +namespace fwPosctrl { + +bool LimitOverride::applyOverride(BlockOutputLimiter &outputLimiterThrottle, + BlockOutputLimiter &outputLimiterPitch) +{ + bool ret = false; + + if (overrideThrottleMinEnabled) { + outputLimiterThrottle.setMin(overrideThrottleMin); + ret = true; + } + if (overrideThrottleMaxEnabled) { + outputLimiterThrottle.setMax(overrideThrottleMax); + ret = true; + } + if (overridePitchMinEnabled) { + outputLimiterPitch.setMin(overridePitchMin); + ret = true; + } + if (overridePitchMaxEnabled) { + outputLimiterPitch.setMax(overridePitchMax); + ret = true; + } + + return ret; +} + +} /* namespace fwPosctrl */ diff --git a/src/modules/fw_pos_control_l1/mtecs/limitoverride.h b/src/modules/fw_pos_control_l1/mtecs/limitoverride.h new file mode 100644 index 0000000000..64c2e7bbde --- /dev/null +++ b/src/modules/fw_pos_control_l1/mtecs/limitoverride.h @@ -0,0 +1,107 @@ +/**************************************************************************** + * + * Copyright (c) 2013 PX4 Development Team. All rights reserved. + * Author: @author Thomas Gubler + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ + + +/** + * @file limitoverride.h + * + * @author Thomas Gubler + */ + + +#ifndef LIMITOVERRIDE_H_ +#define LIMITOVERRIDE_H_ + +#include "mTecs_blocks.h" + +namespace fwPosctrl +{ + +/* A small class which provides helper functions to override control output limits which are usually set by +* parameters in special cases +*/ +class LimitOverride +{ +public: + LimitOverride() : + overrideThrottleMinEnabled(false), + overrideThrottleMaxEnabled(false), + overridePitchMinEnabled(false), + overridePitchMaxEnabled(false) + {}; + + ~LimitOverride() {}; + + /* + * Override the limits of the outputlimiter instances given by the arguments with the limits saved in + * this class (if enabled) + * @return true if the limit was applied + */ + bool applyOverride(BlockOutputLimiter &outputLimiterThrottle, + BlockOutputLimiter &outputLimiterPitch); + + /* Functions to enable or disable the override */ + void enableThrottleMinOverride(float value) { enable(&overrideThrottleMinEnabled, + &overrideThrottleMin, value); } + void disableThrottleMinOverride() { disable(&overrideThrottleMinEnabled); } + void enableThrottleMaxOverride(float value) { enable(&overrideThrottleMaxEnabled, + &overrideThrottleMax, value); } + void disableThrottleMaxOverride() { disable(&overrideThrottleMaxEnabled); } + void enablePitchMinOverride(float value) { enable(&overridePitchMinEnabled, + &overridePitchMin, value); } + void disablePitchMinOverride() { disable(&overridePitchMinEnabled); } + void enablePitchMaxOverride(float value) { enable(&overridePitchMaxEnabled, + &overridePitchMax, value); } + void disablePitchMaxOverride() { disable(&overridePitchMaxEnabled); } + +protected: + bool overrideThrottleMinEnabled; + float overrideThrottleMin; + bool overrideThrottleMaxEnabled; + float overrideThrottleMax; + bool overridePitchMinEnabled; + float overridePitchMin; //in degrees (replaces param values) + bool overridePitchMaxEnabled; + float overridePitchMax; //in degrees (replaces param values) + + /* Enable a specific limit override */ + void enable(bool *flag, float *limit, float value) { *flag = true; *limit = value; }; + + /* Disable a specific limit override */ + void disable(bool *flag) { *flag = false; }; +}; + +} /* namespace fwPosctrl */ + +#endif /* LIMITOVERRIDE_H_ */ diff --git a/src/modules/fw_pos_control_l1/mtecs/mTecs.cpp b/src/modules/fw_pos_control_l1/mtecs/mTecs.cpp new file mode 100644 index 0000000000..48c9079a48 --- /dev/null +++ b/src/modules/fw_pos_control_l1/mtecs/mTecs.cpp @@ -0,0 +1,308 @@ +/**************************************************************************** + * + * Copyright (c) 2013 PX4 Development Team. All rights reserved. + * Author: @author Thomas Gubler + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ + + +/** + * @file mTecs.cpp + * + * @author Thomas Gubler + */ + +#include "mTecs.h" + +#include +#include + +namespace fwPosctrl { + +mTecs::mTecs() : + SuperBlock(NULL, "MT"), + /* Parameters */ + _mTecsEnabled(this, "ENABLED"), + _airspeedMin(this, "FW_AIRSPD_MIN", false), + /* Publications */ + _status(&getPublications(), ORB_ID(tecs_status)), + /* control blocks */ + _controlTotalEnergy(this, "THR"), + _controlEnergyDistribution(this, "PIT", true), + _controlAltitude(this, "FPA", true), + _controlAirSpeed(this, "ACC"), + _airspeedLowpass(this, "A_LP"), + _airspeedDerivative(this, "AD"), + _throttleSp(0.0f), + _pitchSp(0.0f), + _BlockOutputLimiterTakeoffThrottle(this, "TKF_THR"), + _BlockOutputLimiterTakeoffPitch(this, "TKF_PIT", true), + _BlockOutputLimiterUnderspeedThrottle(this, "USP_THR"), + _BlockOutputLimiterUnderspeedPitch(this, "USP_PIT", true), + _BlockOutputLimiterLandThrottle(this, "LND_THR"), + _BlockOutputLimiterLandPitch(this, "LND_PIT", true), + timestampLastIteration(hrt_absolute_time()), + _firstIterationAfterReset(true), + _dtCalculated(false), + _counter(0), + _debug(false) +{ +} + +mTecs::~mTecs() +{ +} + +int mTecs::updateAltitudeSpeed(float flightPathAngle, float altitude, float altitudeSp, float airspeed, + float airspeedSp, tecs_mode mode, LimitOverride limitOverride) +{ + /* check if all input arguments are numbers and abort if not so */ + if (!isfinite(flightPathAngle) || !isfinite(altitude) || + !isfinite(altitudeSp) || !isfinite(airspeed) || !isfinite(airspeedSp) || !isfinite(mode)) { + return -1; + } + + /* time measurement */ + updateTimeMeasurement(); + + /* calculate flight path angle setpoint from altitude setpoint */ + float flightPathAngleSp = _controlAltitude.update(altitudeSp - altitude); + + /* Debug output */ + if (_counter % 10 == 0) { + debug("***"); + debug("updateAltitudeSpeed: altitudeSp %.4f, altitude %.4f, flightPathAngleSp %.4f", (double)altitudeSp, (double)altitude, (double)flightPathAngleSp); + } + + /* Write part of the status message */ + _status.altitudeSp = altitudeSp; + _status.altitude = altitude; + + + /* use flightpath angle setpoint for total energy control */ + return updateFlightPathAngleSpeed(flightPathAngle, flightPathAngleSp, airspeed, + airspeedSp, mode, limitOverride); +} + +int mTecs::updateFlightPathAngleSpeed(float flightPathAngle, float flightPathAngleSp, float airspeed, + float airspeedSp, tecs_mode mode, LimitOverride limitOverride) +{ + /* check if all input arguments are numbers and abort if not so */ + if (!isfinite(flightPathAngle) || !isfinite(flightPathAngleSp) || + !isfinite(airspeed) || !isfinite(airspeedSp) || !isfinite(mode)) { + return -1; + } + + /* time measurement */ + updateTimeMeasurement(); + + /* Filter arispeed */ + float airspeedFiltered = _airspeedLowpass.update(airspeed); + + /* calculate longitudinal acceleration setpoint from airspeed setpoint*/ + float accelerationLongitudinalSp = _controlAirSpeed.update(airspeedSp - airspeedFiltered); + + /* Debug output */ + if (_counter % 10 == 0) { + debug("updateFlightPathAngleSpeed airspeedSp %.4f, airspeed %.4f airspeedFiltered %.4f," + "accelerationLongitudinalSp%.4f", + (double)airspeedSp, (double)airspeed, + (double)airspeedFiltered, (double)accelerationLongitudinalSp); + } + + /* Write part of the status message */ + _status.flightPathAngleSp = flightPathAngleSp; + _status.flightPathAngle = flightPathAngle; + _status.airspeedSp = airspeedSp; + _status.airspeed = airspeed; + _status.airspeedFiltered = airspeedFiltered; + + + /* use longitudinal acceleration setpoint for total energy control */ + return updateFlightPathAngleAcceleration(flightPathAngle, flightPathAngleSp, airspeedFiltered, + accelerationLongitudinalSp, mode, limitOverride); +} + +int mTecs::updateFlightPathAngleAcceleration(float flightPathAngle, float flightPathAngleSp, float airspeedFiltered, + float accelerationLongitudinalSp, tecs_mode mode, LimitOverride limitOverride) +{ + /* check if all input arguments are numbers and abort if not so */ + if (!isfinite(flightPathAngle) || !isfinite(flightPathAngleSp) || + !isfinite(airspeedFiltered) || !isfinite(accelerationLongitudinalSp) || !isfinite(mode)) { + return -1; + } + /* time measurement */ + updateTimeMeasurement(); + + /* update parameters first */ + updateParams(); + + /* calculate values (energies) */ + float flightPathAngleError = flightPathAngleSp - flightPathAngle; + float airspeedDerivative = 0.0f; + if(_airspeedDerivative.getDt() > 0.0f) { + airspeedDerivative = _airspeedDerivative.update(airspeedFiltered); + } + float airspeedDerivativeNorm = airspeedDerivative / CONSTANTS_ONE_G; + float airspeedDerivativeSp = accelerationLongitudinalSp; + float airspeedDerivativeNormSp = airspeedDerivativeSp / CONSTANTS_ONE_G; + float airspeedDerivativeNormError = airspeedDerivativeNormSp - airspeedDerivativeNorm; + + float totalEnergyRate = flightPathAngle + airspeedDerivativeNorm; + float totalEnergyRateError = flightPathAngleError + airspeedDerivativeNormError; + float totalEnergyRateSp = flightPathAngleSp + airspeedDerivativeNormSp; + float totalEnergyRateError2 = totalEnergyRateSp - totalEnergyRate; + + float energyDistributionRate = flightPathAngle - airspeedDerivativeNorm; + float energyDistributionRateError = flightPathAngleError - airspeedDerivativeNormError; + float energyDistributionRateSp = flightPathAngleSp - airspeedDerivativeNormSp; + float energyDistributionRateError2 = energyDistributionRateSp - energyDistributionRate; + + /* Debug output */ + if (_counter % 10 == 0) { + debug("totalEnergyRateSp %.2f, totalEnergyRate %.2f, totalEnergyRateError %.2f totalEnergyRateError2 %.2f airspeedDerivativeNorm %.4f", + (double)totalEnergyRateSp, (double)totalEnergyRate, (double)totalEnergyRateError, (double)totalEnergyRateError2, (double)airspeedDerivativeNorm); + debug("energyDistributionRateSp %.2f, energyDistributionRate %.2f, energyDistributionRateError %.2f energyDistributionRateError2 %.2f", + (double)energyDistributionRateSp, (double)energyDistributionRate, (double)energyDistributionRateError, (double)energyDistributionRateError2); + } + + /* Check airspeed: if below safe value switch to underspeed mode (if not in land or takeoff mode) */ + if (mode != TECS_MODE_LAND && mode != TECS_MODE_TAKEOFF && airspeedFiltered < _airspeedMin.get()) { + mode = TECS_MODE_UNDERSPEED; + } + + /* Set special ouput limiters if we are not in TECS_MODE_NORMAL */ + BlockOutputLimiter *outputLimiterThrottle = &_controlTotalEnergy.getOutputLimiter(); + BlockOutputLimiter *outputLimiterPitch = &_controlEnergyDistribution.getOutputLimiter(); + if (mode == TECS_MODE_TAKEOFF) { + outputLimiterThrottle = &_BlockOutputLimiterTakeoffThrottle; //XXX: accept prelaunch values from launchdetector + outputLimiterPitch = &_BlockOutputLimiterTakeoffPitch; + } else if (mode == TECS_MODE_LAND) { + // only limit pitch but do not limit throttle + outputLimiterPitch = &_BlockOutputLimiterLandPitch; + } else if (mode == TECS_MODE_LAND_THROTTLELIM) { + outputLimiterThrottle = &_BlockOutputLimiterLandThrottle; + outputLimiterPitch = &_BlockOutputLimiterLandPitch; + } else if (mode == TECS_MODE_UNDERSPEED) { + outputLimiterThrottle = &_BlockOutputLimiterUnderspeedThrottle; + outputLimiterPitch = &_BlockOutputLimiterUnderspeedPitch; + } + + /* Apply overrride given by the limitOverride argument (this is used for limits which are not given by + * parameters such as pitch limits with takeoff waypoints or throttle limits when the launchdetector + * is running) */ + bool limitApplied = limitOverride.applyOverride(*outputLimiterThrottle, *outputLimiterPitch); + + /* Write part of the status message */ + _status.airspeedDerivativeSp = airspeedDerivativeSp; + _status.airspeedDerivative = airspeedDerivative; + _status.totalEnergyRateSp = totalEnergyRateSp; + _status.totalEnergyRate = totalEnergyRate; + _status.energyDistributionRateSp = energyDistributionRateSp; + _status.energyDistributionRate = energyDistributionRate; + _status.mode = mode; + + /** update control blocks **/ + /* update total energy rate control block */ + _throttleSp = _controlTotalEnergy.update(totalEnergyRateSp, totalEnergyRateError, outputLimiterThrottle); + + /* update energy distribution rate control block */ + _pitchSp = _controlEnergyDistribution.update(energyDistributionRateSp, energyDistributionRateError, outputLimiterPitch); + + + if (_counter % 10 == 0) { + debug("_throttleSp %.1f, _pitchSp %.1f, flightPathAngleSp %.1f, flightPathAngle %.1f accelerationLongitudinalSp %.1f, airspeedDerivative %.1f", + (double)_throttleSp, (double)_pitchSp, + (double)flightPathAngleSp, (double)flightPathAngle, + (double)accelerationLongitudinalSp, (double)airspeedDerivative); + } + + /* publish status messge */ + _status.update(); + + /* clean up */ + _firstIterationAfterReset = false; + _dtCalculated = false; + + _counter++; + + return 0; +} + +void mTecs::resetIntegrators() +{ + _controlTotalEnergy.getIntegral().setY(0.0f); + _controlEnergyDistribution.getIntegral().setY(0.0f); + timestampLastIteration = hrt_absolute_time(); + _firstIterationAfterReset = true; +} + +void mTecs::resetDerivatives(float airspeed) +{ + _airspeedDerivative.setU(airspeed); +} + + +void mTecs::updateTimeMeasurement() +{ + if (!_dtCalculated) { + float deltaTSeconds = 0.0f; + if (!_firstIterationAfterReset) { + hrt_abstime timestampNow = hrt_absolute_time(); + deltaTSeconds = (float)(timestampNow - timestampLastIteration) * 1e-6f; + timestampLastIteration = timestampNow; + } + setDt(deltaTSeconds); + + _dtCalculated = true; + } +} + +void mTecs::debug_print(const char *fmt, va_list args) +{ + fprintf(stderr, "%s: ", "[mtecs]"); + vfprintf(stderr, fmt, args); + + fprintf(stderr, "\n"); +} + +void mTecs::debug(const char *fmt, ...) { + + if (!_debug) { + return; + } + + va_list args; + + va_start(args, fmt); + debug_print(fmt, args); +} + +} /* namespace fwPosctrl */ diff --git a/src/modules/fw_pos_control_l1/mtecs/mTecs.h b/src/modules/fw_pos_control_l1/mtecs/mTecs.h new file mode 100644 index 0000000000..c102f5dee5 --- /dev/null +++ b/src/modules/fw_pos_control_l1/mtecs/mTecs.h @@ -0,0 +1,149 @@ +/**************************************************************************** + * + * Copyright (c) 2013 PX4 Development Team. All rights reserved. + * Author: @author Thomas Gubler + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ + + +/** + * @file mTecs.h + * + * @author Thomas Gubler + */ + + +#ifndef MTECS_H_ +#define MTECS_H_ + +#include "mTecs_blocks.h" +#include "limitoverride.h" + +#include +#include +#include +#include + +namespace fwPosctrl +{ + +/* Main class of the mTecs */ +class mTecs : public control::SuperBlock +{ +public: + mTecs(); + virtual ~mTecs(); + + /* + * Control in altitude setpoint and speed mode + */ + int updateAltitudeSpeed(float flightPathAngle, float altitude, float altitudeSp, float airspeed, + float airspeedSp, tecs_mode mode, LimitOverride limitOverride); + + /* + * Control in flightPathAngle setpoint (flollow a slope etc.) and speed mode + */ + int updateFlightPathAngleSpeed(float flightPathAngle, float flightPathAngleSp, float airspeed, + float airspeedSp, tecs_mode mode, LimitOverride limitOverride); + + /* + * Control in flightPathAngle setpoint (flollow a slope etc.) and acceleration mode (base case) + */ + int updateFlightPathAngleAcceleration(float flightPathAngle, float flightPathAngleSp, float airspeedFiltered, + float accelerationLongitudinalSp, tecs_mode mode, LimitOverride limitOverride); + + /* + * Reset all integrators + */ + void resetIntegrators(); + + /* + * Reset all derivative calculations + */ + void resetDerivatives(float airspeed); + + /* Accessors */ + bool getEnabled() { return _mTecsEnabled.get() > 0; } + float getThrottleSetpoint() { return _throttleSp; } + float getPitchSetpoint() { return _pitchSp; } + float airspeedLowpassUpdate(float input) { return _airspeedLowpass.update(input); } + +protected: + /* parameters */ + control::BlockParamInt _mTecsEnabled; /**< 1 if mTecs is enabled */ + control::BlockParamFloat _airspeedMin; /**< minimal airspeed */ + + /* Publications */ + uORB::Publication _status; /**< publish internal values for logging */ + + /* control blocks */ + BlockFFPILimitedCustom _controlTotalEnergy; /**< FFPI controller for total energy control: output is throttle */ + BlockFFPILimitedCustom _controlEnergyDistribution; /**< FFPI controller for energy distribution control: output is pitch */ + BlockPDLimited _controlAltitude; /**< PD controller for altitude: output is the flight path angle setpoint */ + BlockPDLimited _controlAirSpeed; /**< PD controller for airspeed: output is acceleration setpoint */ + + /* Other calculation Blocks */ + control::BlockLowPass _airspeedLowpass; /**< low pass filter for airspeed */ + control::BlockDerivative _airspeedDerivative; /**< airspeed derivative calulation */ + + /* Output setpoints */ + float _throttleSp; /**< Throttle Setpoint from 0 to 1 */ + float _pitchSp; /**< Pitch Setpoint from -pi to pi */ + + /* Output Limits in special modes */ + BlockOutputLimiter _BlockOutputLimiterTakeoffThrottle; /**< Throttle Limits during takeoff */ + BlockOutputLimiter _BlockOutputLimiterTakeoffPitch; /**< Pitch Limit during takeoff */ + BlockOutputLimiter _BlockOutputLimiterUnderspeedThrottle; /**< Throttle Limits when underspeed is detected */ + BlockOutputLimiter _BlockOutputLimiterUnderspeedPitch; /**< Pitch Limit when underspeed is detected */ + BlockOutputLimiter _BlockOutputLimiterLandThrottle; /**< Throttle Limits during landing (only in last phase)*/ + BlockOutputLimiter _BlockOutputLimiterLandPitch; /**< Pitch Limit during landing */ + + /* Time measurements */ + hrt_abstime timestampLastIteration; /**< Saves the result of hrt_absolute_time() of the last iteration */ + + bool _firstIterationAfterReset; /**< True during the first iteration after a reset */ + bool _dtCalculated; /**< True if dt has been calculated in this iteration */ + + int _counter; + bool _debug; ///< Set true to enable debug output + + + static void debug_print(const char *fmt, va_list args); + void debug(const char *fmt, ...); + + /* + * Measure and update the time step dt if this was not already done in the current iteration + */ + void updateTimeMeasurement(); +}; + +} /* namespace fwPosctrl */ + +#endif /* MTECS_H_ */ diff --git a/src/modules/fw_pos_control_l1/mtecs/mTecs_blocks.h b/src/modules/fw_pos_control_l1/mtecs/mTecs_blocks.h new file mode 100644 index 0000000000..e4e405227e --- /dev/null +++ b/src/modules/fw_pos_control_l1/mtecs/mTecs_blocks.h @@ -0,0 +1,220 @@ +/**************************************************************************** + * + * Copyright (c) 2013 PX4 Development Team. All rights reserved. + * Author: @author Thomas Gubler + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ + + +/** + * @file mTecs_blocks.h + * + * Custom blocks for the mTecs + * + * @author Thomas Gubler + */ + +#pragma once + +#include +#include + +namespace fwPosctrl +{ + +using namespace control; + +/* An block which can be used to limit the output */ +class BlockOutputLimiter: public SuperBlock +{ +public: +// methods + BlockOutputLimiter(SuperBlock *parent, const char *name, bool isAngularLimit = false) : + SuperBlock(parent, name), + _isAngularLimit(isAngularLimit), + _min(this, "MIN"), + _max(this, "MAX") + {}; + virtual ~BlockOutputLimiter() {}; + /* + * Imposes the limits given by _min and _max on value + * + * @param value is changed to be on the interval _min to _max + * @param difference if the value is changed this corresponds to the change of value * (-1) + * otherwise unchanged + * @return: true if the limit is applied, false otherwise + */ + bool limit(float& value, float& difference) { + float minimum = isAngularLimit() ? getMin() * M_DEG_TO_RAD_F : getMin(); + float maximum = isAngularLimit() ? getMax() * M_DEG_TO_RAD_F : getMax(); + if (value < minimum) { + difference = value - minimum; + value = minimum; + return true; + } else if (value > maximum) { + difference = value - maximum; + value = maximum; + return true; + } + return false; + } +//accessor: + bool isAngularLimit() {return _isAngularLimit ;} + float getMin() { return _min.get(); } + float getMax() { return _max.get(); } + void setMin(float value) { _min.set(value); } + void setMax(float value) { _max.set(value); } +protected: +//attributes + bool _isAngularLimit; + control::BlockParamFloat _min; + control::BlockParamFloat _max; +}; + + +/* A combination of feed forward, P and I gain using the output limiter*/ +class BlockFFPILimited: public SuperBlock +{ +public: +// methods + BlockFFPILimited(SuperBlock *parent, const char *name, bool isAngularLimit = false) : + SuperBlock(parent, name), + _outputLimiter(this, "", isAngularLimit), + _integral(this, "I"), + _kFF(this, "FF"), + _kP(this, "P"), + _kI(this, "I"), + _offset(this, "OFF") + {}; + virtual ~BlockFFPILimited() {}; + float update(float inputValue, float inputError) { return calcLimitedOutput(inputValue, inputError, _outputLimiter); } +// accessors + BlockIntegral &getIntegral() { return _integral; } + float getKFF() { return _kFF.get(); } + float getKP() { return _kP.get(); } + float getKI() { return _kI.get(); } + float getOffset() { return _offset.get(); } + BlockOutputLimiter &getOutputLimiter() { return _outputLimiter; }; +protected: + BlockOutputLimiter _outputLimiter; + + float calcUnlimitedOutput(float inputValue, float inputError) {return getOffset() + getKFF() * inputValue + getKP() * inputError + getKI() * getIntegral().update(inputError);} + float calcLimitedOutput(float inputValue, float inputError, BlockOutputLimiter &outputLimiter) { + float difference = 0.0f; + float integralYPrevious = _integral.getY(); + float output = calcUnlimitedOutput(inputValue, inputError); + if(outputLimiter.limit(output, difference) && + (((difference < 0) && (getKI() * getIntegral().getY() < 0)) || + ((difference > 0) && (getKI() * getIntegral().getY() > 0)))) { + getIntegral().setY(integralYPrevious); + } + return output; + } +private: + BlockIntegral _integral; + BlockParamFloat _kFF; + BlockParamFloat _kP; + BlockParamFloat _kI; + BlockParamFloat _offset; +}; + +/* A combination of feed forward, P and I gain using the output limiter with the option to provide a special output limiter (for example for takeoff)*/ +class BlockFFPILimitedCustom: public BlockFFPILimited +{ +public: +// methods + BlockFFPILimitedCustom(SuperBlock *parent, const char *name, bool isAngularLimit = false) : + BlockFFPILimited(parent, name, isAngularLimit) + {}; + virtual ~BlockFFPILimitedCustom() {}; + float update(float inputValue, float inputError, BlockOutputLimiter *outputLimiter = NULL) { + return calcLimitedOutput(inputValue, inputError, outputLimiter == NULL ? _outputLimiter : *outputLimiter); + } +}; + +/* A combination of P gain and output limiter */ +class BlockPLimited: public SuperBlock +{ +public: +// methods + BlockPLimited(SuperBlock *parent, const char *name, bool isAngularLimit = false) : + SuperBlock(parent, name), + _kP(this, "P"), + _outputLimiter(this, "", isAngularLimit) + {}; + virtual ~BlockPLimited() {}; + float update(float input) { + float difference = 0.0f; + float output = getKP() * input; + getOutputLimiter().limit(output, difference); + return output; + } +// accessors + BlockOutputLimiter &getOutputLimiter() { return _outputLimiter; }; + float getKP() { return _kP.get(); } +private: + control::BlockParamFloat _kP; + BlockOutputLimiter _outputLimiter; +}; + +/* A combination of P, D gains and output limiter */ +class BlockPDLimited: public SuperBlock +{ +public: +// methods + BlockPDLimited(SuperBlock *parent, const char *name, bool isAngularLimit = false) : + SuperBlock(parent, name), + _kP(this, "P"), + _kD(this, "D"), + _derivative(this, "D"), + _outputLimiter(this, "", isAngularLimit) + {}; + virtual ~BlockPDLimited() {}; + float update(float input) { + float difference = 0.0f; + float output = getKP() * input + (getDerivative().getDt() > 0.0f ? getKD() * getDerivative().update(input) : 0.0f); + getOutputLimiter().limit(output, difference); + + return output; + } +// accessors + float getKP() { return _kP.get(); } + float getKD() { return _kD.get(); } + BlockDerivative &getDerivative() { return _derivative; } + BlockOutputLimiter &getOutputLimiter() { return _outputLimiter; }; +private: + control::BlockParamFloat _kP; + control::BlockParamFloat _kD; + BlockDerivative _derivative; + BlockOutputLimiter _outputLimiter; +}; + +} + diff --git a/src/modules/fw_pos_control_l1/mtecs/mTecs_params.c b/src/modules/fw_pos_control_l1/mtecs/mTecs_params.c new file mode 100644 index 0000000000..bbbb8f9dbe --- /dev/null +++ b/src/modules/fw_pos_control_l1/mtecs/mTecs_params.c @@ -0,0 +1,412 @@ +/**************************************************************************** + * + * Copyright (c) 2013 PX4 Development Team. All rights reserved. + * Author: @author Thomas Gubler + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ + + +/** + * @file mTecs_params.c + * + * @author Thomas Gubler + */ + +#include +#include + +/* + * Controller parameters, accessible via MAVLink + */ + +/** + * mTECS enabled + * + * Set to 1 to enable mTECS + * + * @min 0 + * @max 1 + * @group mTECS + */ +PARAM_DEFINE_INT32(MT_ENABLED, 1); + +/** + * Total Energy Rate Control Feedforward + * Maps the total energy rate setpoint to the throttle setpoint + * + * @min 0.0 + * @max 10.0 + * @group mTECS + */ +PARAM_DEFINE_FLOAT(MT_THR_FF, 0.7f); + +/** + * Total Energy Rate Control P + * Maps the total energy rate error to the throttle setpoint + * + * @min 0.0 + * @max 10.0 + * @group mTECS + */ +PARAM_DEFINE_FLOAT(MT_THR_P, 0.1f); + +/** + * Total Energy Rate Control I + * Maps the integrated total energy rate to the throttle setpoint + * + * @min 0.0 + * @max 10.0 + * @group mTECS + */ +PARAM_DEFINE_FLOAT(MT_THR_I, 0.25f); + +/** + * Total Energy Rate Control Offset (Cruise throttle sp) + * + * @min 0.0 + * @max 10.0 + * @group mTECS + */ +PARAM_DEFINE_FLOAT(MT_THR_OFF, 0.7f); + +/** + * Energy Distribution Rate Control Feedforward + * Maps the energy distribution rate setpoint to the pitch setpoint + * + * @min 0.0 + * @max 10.0 + * @group mTECS + */ +PARAM_DEFINE_FLOAT(MT_PIT_FF, 0.4f); + +/** + * Energy Distribution Rate Control P + * Maps the energy distribution rate error to the pitch setpoint + * + * @min 0.0 + * @max 10.0 + * @group mTECS + */ +PARAM_DEFINE_FLOAT(MT_PIT_P, 0.03f); + +/** + * Energy Distribution Rate Control I + * Maps the integrated energy distribution rate error to the pitch setpoint + * + * @min 0.0 + * @max 10.0 + * @group mTECS + */ +PARAM_DEFINE_FLOAT(MT_PIT_I, 0.03f); + + +/** + * Total Energy Distribution Offset (Cruise pitch sp) + * + * @min 0.0 + * @max 10.0 + * @group mTECS + */ +PARAM_DEFINE_FLOAT(MT_PIT_OFF, 0.0f); + +/** + * Minimal Throttle Setpoint + * + * @min 0.0 + * @max 1.0 + * @group mTECS + */ +PARAM_DEFINE_FLOAT(MT_THR_MIN, 0.0f); + +/** + * Maximal Throttle Setpoint + * + * @min 0.0 + * @max 1.0 + * @group mTECS + */ +PARAM_DEFINE_FLOAT(MT_THR_MAX, 1.0f); + +/** + * Minimal Pitch Setpoint in Degrees + * + * @min -90.0f + * @max 90.0f + * @unit deg + * @group mTECS + */ +PARAM_DEFINE_FLOAT(MT_PIT_MIN, -45.0f); + +/** + * Maximal Pitch Setpoint in Degrees + * + * @min -90.0f + * @max 90.0f + * @unit deg + * @group mTECS + */ +PARAM_DEFINE_FLOAT(MT_PIT_MAX, 20.0f); + +/** + * P gain for the altitude control + * Maps the altitude error to the flight path angle setpoint + * + * @min 0.0f + * @max 10.0f + * @group mTECS + */ +PARAM_DEFINE_FLOAT(MT_FPA_P, 0.3f); + +/** + * D gain for the altitude control + * Maps the change of altitude error to the flight path angle setpoint + * + * @min 0.0f + * @max 10.0f + * @group mTECS + */ +PARAM_DEFINE_FLOAT(MT_FPA_D, 0.0f); + +/** + * Lowpass for FPA error derivative calculation (see MT_FPA_D) + * + * @group mTECS + */ +PARAM_DEFINE_FLOAT(MT_FPA_D_LP, 1.0f); + + +/** + * Minimal flight path angle setpoint + * + * @min -90.0f + * @max 90.0f + * @unit deg + * @group mTECS + */ +PARAM_DEFINE_FLOAT(MT_FPA_MIN, -20.0f); + +/** + * Maximal flight path angle setpoint + * + * @min -90.0f + * @max 90.0f + * @unit deg + * @group mTECS + */ +PARAM_DEFINE_FLOAT(MT_FPA_MAX, 30.0f); + +/** + * Lowpass (cutoff freq.) for airspeed + * + * @group mTECS + */ +PARAM_DEFINE_FLOAT(MT_A_LP, 1.0f); + +/** + * P gain for the airspeed control + * Maps the airspeed error to the acceleration setpoint + * + * @min 0.0f + * @max 10.0f + * @group mTECS + */ +PARAM_DEFINE_FLOAT(MT_ACC_P, 0.3f); + +/** + * D gain for the airspeed control + * Maps the change of airspeed error to the acceleration setpoint + * + * @min 0.0f + * @max 10.0f + * @group mTECS + */ +PARAM_DEFINE_FLOAT(MT_ACC_D, 0.0f); + +/** + * Lowpass for ACC error derivative calculation (see MT_ACC_D) + * + * @group mTECS + */ +PARAM_DEFINE_FLOAT(MT_ACC_D_LP, 1.0f); + +/** + * Minimal acceleration (air) + * + * @unit m/s^2 + * @group mTECS + */ +PARAM_DEFINE_FLOAT(MT_ACC_MIN, -40.0f); + +/** + * Maximal acceleration (air) + * +* @unit m/s^2 + * @group mTECS + */ +PARAM_DEFINE_FLOAT(MT_ACC_MAX, 40.0f); + +/** + * Airspeed derivative calculation lowpass + * + * @group mTECS + */ +PARAM_DEFINE_FLOAT(MT_AD_LP, 1.0f); + +/** + * Minimal throttle during takeoff + * + * @min 0.0f + * @max 1.0f + * @group mTECS + */ +PARAM_DEFINE_FLOAT(MT_TKF_THR_MIN, 1.0f); + +/** + * Maximal throttle during takeoff + * + * @min 0.0f + * @max 1.0f + * @group mTECS + */ +PARAM_DEFINE_FLOAT(MT_TKF_THR_MAX, 1.0f); + +/** + * Minimal pitch during takeoff + * + * @min -90.0f + * @max 90.0f + * @unit deg + * @group mTECS + */ +PARAM_DEFINE_FLOAT(MT_TKF_PIT_MIN, 0.0f); + +/** + * Maximal pitch during takeoff + * + * @min -90.0f + * @max 90.0f + * @unit deg + * @group mTECS + */ +PARAM_DEFINE_FLOAT(MT_TKF_PIT_MAX, 45.0f); + +/** + * Minimal throttle in underspeed mode + * + * @min 0.0f + * @max 1.0f + * @group mTECS + */ +PARAM_DEFINE_FLOAT(MT_USP_THR_MIN, 1.0f); + +/** + * Maximal throttle in underspeed mode + * + * @min 0.0f + * @max 1.0f + * @group mTECS + */ +PARAM_DEFINE_FLOAT(MT_USP_THR_MAX, 1.0f); + +/** + * Minimal pitch in underspeed mode + * + * @min -90.0f + * @max 90.0f + * @unit deg + * @group mTECS + */ +PARAM_DEFINE_FLOAT(MT_USP_PIT_MIN, -45.0f); + +/** + * Maximal pitch in underspeed mode + * + * @min -90.0f + * @max 90.0f + * @unit deg + * @group mTECS + */ +PARAM_DEFINE_FLOAT(MT_USP_PIT_MAX, 0.0f); + +/** + * Minimal throttle in landing mode (only last phase of landing) + * + * @min 0.0f + * @max 1.0f + * @group mTECS + */ +PARAM_DEFINE_FLOAT(MT_LND_THR_MIN, 0.0f); + +/** + * Maximal throttle in landing mode (only last phase of landing) + * + * @min 0.0f + * @max 1.0f + * @group mTECS + */ +PARAM_DEFINE_FLOAT(MT_LND_THR_MAX, 0.0f); + +/** + * Minimal pitch in landing mode + * + * @min -90.0f + * @max 90.0f + * @unit deg + * @group mTECS + */ +PARAM_DEFINE_FLOAT(MT_LND_PIT_MIN, -5.0f); + +/** + * Maximal pitch in landing mode + * + * @min -90.0f + * @max 90.0f + * @unit deg + * @group mTECS + */ +PARAM_DEFINE_FLOAT(MT_LND_PIT_MAX, 15.0f); + +/** + * Integrator Limit for Total Energy Rate Control + * + * @min 0.0f + * @max 10.0f + * @group mTECS + */ +PARAM_DEFINE_FLOAT(MT_THR_I_MAX, 10.0f); + +/** + * Integrator Limit for Energy Distribution Rate Control + * + * @min 0.0f + * @max 10.0f + * @group mTECS + */ +PARAM_DEFINE_FLOAT(MT_PIT_I_MAX, 10.0f); diff --git a/src/modules/sdlog2/sdlog2.c b/src/modules/sdlog2/sdlog2.c index 577cadfbbe..0813bf7b0b 100644 --- a/src/modules/sdlog2/sdlog2.c +++ b/src/modules/sdlog2/sdlog2.c @@ -84,8 +84,10 @@ #include #include #include +#include #include #include +#include #include #include @@ -939,8 +941,10 @@ int sdlog2_thread_main(int argc, char *argv[]) struct telemetry_status_s telemetry; struct range_finder_report range_finder; struct estimator_status_report estimator_status; + struct tecs_status_s tecs_status; struct system_power_s system_power; struct servorail_status_s servorail_status; + struct wind_estimate_s wind_estimate; } buf; memset(&buf, 0, sizeof(buf)); @@ -979,6 +983,8 @@ int sdlog2_thread_main(int argc, char *argv[]) struct log_GS0B_s log_GS0B; struct log_GS1A_s log_GS1A; struct log_GS1B_s log_GS1B; + struct log_TECS_s log_TECS; + struct log_WIND_s log_WIND; } body; } log_msg = { LOG_PACKET_HEADER_INIT(0) @@ -1010,8 +1016,10 @@ int sdlog2_thread_main(int argc, char *argv[]) int telemetry_sub; int range_finder_sub; int estimator_status_sub; + int tecs_status_sub; int system_power_sub; int servorail_status_sub; + int wind_sub; } subs; subs.cmd_sub = orb_subscribe(ORB_ID(vehicle_command)); @@ -1037,8 +1045,12 @@ int sdlog2_thread_main(int argc, char *argv[]) subs.telemetry_sub = orb_subscribe(ORB_ID(telemetry_status)); subs.range_finder_sub = orb_subscribe(ORB_ID(sensor_range_finder)); subs.estimator_status_sub = orb_subscribe(ORB_ID(estimator_status)); + subs.tecs_status_sub = orb_subscribe(ORB_ID(tecs_status)); subs.system_power_sub = orb_subscribe(ORB_ID(system_power)); subs.servorail_status_sub = orb_subscribe(ORB_ID(servorail_status)); + subs.wind_sub = orb_subscribe(ORB_ID(wind_estimate)); + /* we need to rate-limit wind, as we do not need the full update rate */ + orb_set_interval(subs.wind_sub, 90); thread_running = true; @@ -1488,6 +1500,36 @@ int sdlog2_thread_main(int argc, char *argv[]) LOGBUFFER_WRITE_AND_COUNT(ESTM); } + /* --- TECS STATUS --- */ + if (copy_if_updated(ORB_ID(tecs_status), subs.tecs_status_sub, &buf.tecs_status)) { + log_msg.msg_type = LOG_TECS_MSG; + log_msg.body.log_TECS.altitudeSp = buf.tecs_status.altitudeSp; + log_msg.body.log_TECS.altitude = buf.tecs_status.altitude; + log_msg.body.log_TECS.flightPathAngleSp = buf.tecs_status.flightPathAngleSp; + log_msg.body.log_TECS.flightPathAngle = buf.tecs_status.flightPathAngle; + log_msg.body.log_TECS.airspeedSp = buf.tecs_status.airspeedSp; + log_msg.body.log_TECS.airspeed = buf.tecs_status.airspeed; + log_msg.body.log_TECS.airspeedFiltered = buf.tecs_status.airspeedFiltered; + log_msg.body.log_TECS.airspeedDerivativeSp = buf.tecs_status.airspeedDerivativeSp; + log_msg.body.log_TECS.airspeedDerivative = buf.tecs_status.airspeedDerivative; + log_msg.body.log_TECS.totalEnergyRateSp = buf.tecs_status.totalEnergyRateSp; + log_msg.body.log_TECS.totalEnergyRate = buf.tecs_status.totalEnergyRate; + log_msg.body.log_TECS.energyDistributionRateSp = buf.tecs_status.energyDistributionRateSp; + log_msg.body.log_TECS.energyDistributionRate = buf.tecs_status.energyDistributionRate; + log_msg.body.log_TECS.mode = (uint8_t)buf.tecs_status.mode; + LOGBUFFER_WRITE_AND_COUNT(TECS); + } + + /* --- WIND ESTIMATE --- */ + if (copy_if_updated(ORB_ID(wind_estimate), subs.wind_sub, &buf.wind_estimate)) { + log_msg.msg_type = LOG_WIND_MSG; + log_msg.body.log_WIND.x = buf.wind_estimate.windspeed_north; + log_msg.body.log_WIND.y = buf.wind_estimate.windspeed_east; + log_msg.body.log_WIND.cov_x = buf.wind_estimate.covariance_north; + log_msg.body.log_WIND.cov_y = buf.wind_estimate.covariance_east; + LOGBUFFER_WRITE_AND_COUNT(WIND); + } + /* signal the other thread new data, but not yet unlock */ if (logbuffer_count(&lb) > MIN_BYTES_TO_WRITE) { /* only request write if several packets can be written at once */ diff --git a/src/modules/sdlog2/sdlog2_messages.h b/src/modules/sdlog2/sdlog2_messages.h index f4d88f0795..5645ebcf19 100644 --- a/src/modules/sdlog2/sdlog2_messages.h +++ b/src/modules/sdlog2/sdlog2_messages.h @@ -346,6 +346,36 @@ struct log_GS1B_s { uint8_t satellite_snr[16]; /**< dBHz, Signal to noise ratio of satellite C/N0, range 0..99 */ }; +/* --- TECS - TECS STATUS --- */ +#define LOG_TECS_MSG 30 +struct log_TECS_s { + float altitudeSp; + float altitude; + float flightPathAngleSp; + float flightPathAngle; + float airspeedSp; + float airspeed; + float airspeedFiltered; + float airspeedDerivativeSp; + float airspeedDerivative; + + float totalEnergyRateSp; + float totalEnergyRate; + float energyDistributionRateSp; + float energyDistributionRate; + + uint8_t mode; +}; + +/* --- WIND - WIND ESTIMATE --- */ +#define LOG_WIND_MSG 31 +struct log_WIND_s { + float x; + float y; + float cov_x; + float cov_y; +}; + /********** SYSTEM MESSAGES, ID > 0x80 **********/ /* --- TIME - TIME STAMP --- */ @@ -401,6 +431,8 @@ static const struct log_format_s log_formats[] = { LOG_FORMAT(GS0B, "BBBBBBBBBBBBBBBB", "s0,s1,s2,s3,s4,s5,s6,s7,s8,s9,s10,s11,s12,s13,s14,s15"), LOG_FORMAT(GS1A, "BBBBBBBBBBBBBBBB", "s0,s1,s2,s3,s4,s5,s6,s7,s8,s9,s10,s11,s12,s13,s14,s15"), LOG_FORMAT(GS1B, "BBBBBBBBBBBBBBBB", "s0,s1,s2,s3,s4,s5,s6,s7,s8,s9,s10,s11,s12,s13,s14,s15"), + LOG_FORMAT(TECS, "fffffffffffffB", "AltSP,Alt,FpaSP,Fpa,AsSP,As,AsF,AsDSP,AsD,TERSP,TER,EDRSP,EDR,M"), + LOG_FORMAT(WIND, "ffff", "X,Y,CovX,CovY"), /* system-level messages, ID >= 0x80 */ /* FMT: don't write format of format message, it's useless */ diff --git a/src/modules/uORB/Publication.cpp b/src/modules/uORB/Publication.cpp index 5a59816172..cd0b30dd61 100644 --- a/src/modules/uORB/Publication.cpp +++ b/src/modules/uORB/Publication.cpp @@ -47,6 +47,7 @@ #include "topics/vehicle_rates_setpoint.h" #include "topics/actuator_outputs.h" #include "topics/encoders.h" +#include "topics/tecs_status.h" namespace uORB { @@ -76,5 +77,6 @@ template class __EXPORT Publication; template class __EXPORT Publication; template class __EXPORT Publication; template class __EXPORT Publication; +template class __EXPORT Publication; } diff --git a/src/modules/uORB/objects_common.cpp b/src/modules/uORB/objects_common.cpp index 90675bb2e9..7c3bb0009e 100644 --- a/src/modules/uORB/objects_common.cpp +++ b/src/modules/uORB/objects_common.cpp @@ -199,3 +199,9 @@ ORB_DEFINE(encoders, struct encoders_s); #include "topics/estimator_status.h" ORB_DEFINE(estimator_status, struct estimator_status_report); + +#include "topics/tecs_status.h" +ORB_DEFINE(tecs_status, struct tecs_status_s); + +#include "topics/wind_estimate.h" +ORB_DEFINE(wind_estimate, struct wind_estimate_s); diff --git a/src/modules/uORB/topics/tecs_status.h b/src/modules/uORB/topics/tecs_status.h new file mode 100644 index 0000000000..05310e9062 --- /dev/null +++ b/src/modules/uORB/topics/tecs_status.h @@ -0,0 +1,92 @@ +/**************************************************************************** + * + * Copyright (c) 2012-2014 PX4 Development Team. All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ + +/** + * @file vehicle_global_position.h + * Definition of the global fused WGS84 position uORB topic. + * + * @author Thomas Gubler + */ + +#ifndef TECS_STATUS_T_H_ +#define TECS_STATUS_T_H_ + +#include +#include +#include "../uORB.h" + +/** + * @addtogroup topics + * @{ + */ + +typedef enum { + TECS_MODE_NORMAL, + TECS_MODE_UNDERSPEED, + TECS_MODE_TAKEOFF, + TECS_MODE_LAND, + TECS_MODE_LAND_THROTTLELIM +} tecs_mode; + + /** + * Internal values of the (m)TECS fixed wing speed alnd altitude control system + */ +struct tecs_status_s { + uint64_t timestamp; /**< timestamp, in microseconds since system start */ + + float altitudeSp; + float altitude; + float flightPathAngleSp; + float flightPathAngle; + float airspeedSp; + float airspeed; + float airspeedFiltered; + float airspeedDerivativeSp; + float airspeedDerivative; + + float totalEnergyRateSp; + float totalEnergyRate; + float energyDistributionRateSp; + float energyDistributionRate; + + tecs_mode mode; +}; + +/** + * @} + */ + +/* register this as object request broker structure */ +ORB_DECLARE(tecs_status); + +#endif diff --git a/src/modules/uORB/topics/wind_estimate.h b/src/modules/uORB/topics/wind_estimate.h new file mode 100644 index 0000000000..58333a64f8 --- /dev/null +++ b/src/modules/uORB/topics/wind_estimate.h @@ -0,0 +1,68 @@ +/**************************************************************************** + * + * Copyright (c) 2014 PX4 Development Team. All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ + +/** + * @file wind_estimate.h + * + * Wind estimate topic topic + * + */ + +#ifndef TOPIC_WIND_ESTIMATE_H +#define TOPIC_WIND_ESTIMATE_H + +#include +#include "../uORB.h" + +/** + * @addtogroup topics + * @{ + */ + +/** Wind estimate */ +struct wind_estimate_s { + + uint64_t timestamp; /**< Microseconds since system boot */ + float windspeed_north; /**< Wind component in north / X direction */ + float windspeed_east; /**< Wind component in east / Y direction */ + float covariance_north; /**< Uncertainty - set to zero (no uncertainty) if not estimated */ + float covariance_east; /**< Uncertainty - set to zero (no uncertainty) if not estimated */ +}; + +/** + * @} + */ + +ORB_DECLARE(wind_estimate); + +#endif \ No newline at end of file