dark0707
/
px4-firmware
forked from Archive/PX4-Autopilot
1
0
Fork 0
Code Pull Requests Activity

Merge pull request #1037 from PX4/mtecs 

mTECS
This commit is contained in:
Lorenz Meier 2014-06-29 12:39:04 +02:00
parent 852d973e74 7b15a424f0
commit e5bd00e628
20 changed files with 1710 additions and 122 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

1
makefiles/config_aerocore_default.mk
View File

@ -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

7
makefiles/config_px4fmu-v1_default.mk
View File

@ -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

1
makefiles/config_px4fmu-v2_default.mk
View File

@ -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

2
src/lib/launchdetection/module.mk
View File

@ -38,3 +38,5 @@
SRCS = LaunchDetector.cpp \
CatapultLaunchMethod.cpp \
launchdetection_params.c
MAXOPTIMIZATION = -Os

33
src/modules/ekf_att_pos_estimator/ekf_att_pos_estimator_main.cpp
View File

@ -74,6 +74,7 @@
#include <uORB/topics/estimator_status.h>
#include <uORB/topics/actuator_armed.h>
#include <uORB/topics/home_position.h>
#include <uORB/topics/wind_estimate.h>
#include <systemlib/param/param.h>
#include <systemlib/err.h>
#include <geo/geo.h>
@ -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",

4
src/modules/ekf_att_pos_estimator/estimator.cpp
View File

@ -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;
}

270
src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp
View File

@ -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 <mavlink/mavlink_log.h>
#include <launchdetection/LaunchDetector.h>
#include <ecl/l1/ecl_l1_pos_controller.h>
#include <external_lgpl/tecs/tecs.h>
#include <drivers/drv_range_finder.h>
#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> &current_position, const math::Vector<2> &ground_speed, const struct position_setpoint_triplet_s &pos_sp_triplet);
void calculate_gndspeed_undershoot(const math::Vector<2> &current_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> &current_position, const math::Vector<2> &ground_speed, const struct position_setpoint_triplet_s &pos_sp_triplet)
FixedwingPositionControl::calculate_gndspeed_undershoot(const math::Vector<2> &current_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> &current_position, const math::Vector<2> &ground_speed,
FixedwingPositionControl::control_position(const math::Vector<2> &current_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> &current_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> &current_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> &current_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> &current_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> &current_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> &current_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> &current_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> &current_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> &current_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> &current_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> &current_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> &current_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> &current_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> &current_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> &current_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> &current_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()
{

5
src/modules/fw_pos_control_l1/module.mk
View File

@ -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

71
src/modules/fw_pos_control_l1/mtecs/limitoverride.cpp Normal file
View File

@ -0,0 +1,71 @@
/****************************************************************************
*
* Copyright (c) 2013 PX4 Development Team. All rights reserved.
* Author: @author Thomas Gubler <thomasgubler@gmail.com>
*
* 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 <thomasgubler@gmail.com>
*/
#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 */

107
src/modules/fw_pos_control_l1/mtecs/limitoverride.h Normal file
View File

@ -0,0 +1,107 @@
/****************************************************************************
*
* Copyright (c) 2013 PX4 Development Team. All rights reserved.
* Author: @author Thomas Gubler <thomasgubler@gmail.com>
*
* 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 <thomasgubler@gmail.com>
*/
#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_ */

308
src/modules/fw_pos_control_l1/mtecs/mTecs.cpp Normal file
View File

@ -0,0 +1,308 @@
/****************************************************************************
*
* Copyright (c) 2013 PX4 Development Team. All rights reserved.
* Author: @author Thomas Gubler <thomasgubler@gmail.com>
*
* 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 <thomasgubler@gmail.com>
*/
#include "mTecs.h"
#include <lib/geo/geo.h>
#include <stdio.h>
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 */

149
src/modules/fw_pos_control_l1/mtecs/mTecs.h Normal file
View File

@ -0,0 +1,149 @@
/****************************************************************************
*
* Copyright (c) 2013 PX4 Development Team. All rights reserved.
* Author: @author Thomas Gubler <thomasgubler@gmail.com>
*
* 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 <thomasgubler@gmail.com>
*/
#ifndef MTECS_H_
#define MTECS_H_
#include "mTecs_blocks.h"
#include "limitoverride.h"
#include <controllib/block/BlockParam.hpp>
#include <drivers/drv_hrt.h>
#include <uORB/Publication.hpp>
#include <uORB/topics/tecs_status.h>
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<tecs_status_s> _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_ */

220
src/modules/fw_pos_control_l1/mtecs/mTecs_blocks.h Normal file
View File

@ -0,0 +1,220 @@
/****************************************************************************
*
* Copyright (c) 2013 PX4 Development Team. All rights reserved.
* Author: @author Thomas Gubler <thomasgubler@gmail.com>
*
* 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 <thomasgubler@gmail.com>
*/
#pragma once
#include <controllib/blocks.hpp>
#include <systemlib/err.h>
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;
};
}

412
src/modules/fw_pos_control_l1/mtecs/mTecs_params.c Normal file
View File

@ -0,0 +1,412 @@
/****************************************************************************
*
* Copyright (c) 2013 PX4 Development Team. All rights reserved.
* Author: @author Thomas Gubler <thomasgubler@gmail.com>
*
* 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 <thomasgubler@gmail.com>
*/
#include <nuttx/config.h>
#include <systemlib/param/param.h>
/*
* 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);

42
src/modules/sdlog2/sdlog2.c
View File

@ -84,8 +84,10 @@
#include <uORB/topics/esc_status.h>
#include <uORB/topics/telemetry_status.h>
#include <uORB/topics/estimator_status.h>
#include <uORB/topics/tecs_status.h>
#include <uORB/topics/system_power.h>
#include <uORB/topics/servorail_status.h>
#include <uORB/topics/wind_estimate.h>
#include <systemlib/systemlib.h>
#include <systemlib/param/param.h>
@ -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 */

32
src/modules/sdlog2/sdlog2_messages.h
View File

@ -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 */

2
src/modules/uORB/Publication.cpp
View File

@ -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<vehicle_attitude_setpoint_s>;
template class __EXPORT Publication<vehicle_rates_setpoint_s>;
template class __EXPORT Publication<actuator_outputs_s>;
template class __EXPORT Publication<encoders_s>;
template class __EXPORT Publication<tecs_status_s>;
}

6
src/modules/uORB/objects_common.cpp
View File

@ -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);

92
src/modules/uORB/topics/tecs_status.h Normal file
View File

@ -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 <thomasgubler@student.ethz.ch>
*/
#ifndef TECS_STATUS_T_H_
#define TECS_STATUS_T_H_
#include <stdint.h>
#include <stdbool.h>
#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

68
src/modules/uORB/topics/wind_estimate.h Normal file
View File

@ -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 <stdint.h>
#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