aboutsummaryrefslogtreecommitdiff
path: root/src/modules
diff options
context:
space:
mode:
Diffstat (limited to 'src/modules')
-rw-r--r--src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp237
-rw-r--r--src/modules/fw_pos_control_l1/module.mk4
-rw-r--r--src/modules/fw_pos_control_l1/mtecs/mTecs.cpp284
-rw-r--r--src/modules/fw_pos_control_l1/mtecs/mTecs.h147
-rw-r--r--src/modules/fw_pos_control_l1/mtecs/mTecs_blocks.h219
-rw-r--r--src/modules/fw_pos_control_l1/mtecs/mTecs_params.c380
-rw-r--r--src/modules/sdlog2/sdlog2.c23
-rw-r--r--src/modules/sdlog2/sdlog2_messages.h19
-rw-r--r--src/modules/uORB/Publication.cpp2
-rw-r--r--src/modules/uORB/objects_common.cpp4
-rw-r--r--src/modules/uORB/topics/tecs_status.h81
11 files changed, 1322 insertions, 78 deletions
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 b8d94fe18..7a4cb66c5 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
@@ -91,6 +91,7 @@
#include <external_lgpl/tecs/tecs.h>
#include <drivers/drv_range_finder.h>
#include "landingslope.h"
+#include "mtecs/mTecs.h"
/**
@@ -199,6 +200,8 @@ private:
ECL_L1_Pos_Controller _l1_control;
TECS _tecs;
+ fwPosctrl::mTecs _mTecs;
+ bool _was_pos_control_mode;
struct {
float l1_period;
@@ -345,11 +348,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.
@@ -370,6 +373,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,
+ fwPosctrl::mTecs::tecs_mode mode = fwPosctrl::mTecs::TECS_MODE_NORMAL);
+
};
namespace l1_control
@@ -432,6 +448,8 @@ FixedwingPositionControl::FixedwingPositionControl() :
_global_pos(),
_pos_sp_triplet(),
_sensor_combined(),
+ _mTecs(),
+ _was_pos_control_mode(false),
_range_finder()
{
_nav_capabilities.turn_distance = 0.0f;
@@ -592,6 +610,9 @@ FixedwingPositionControl::parameters_update()
/* Update Launch Detector Parameters */
launchDetector.updateParams();
+ /* Update the mTecs */
+ _mTecs.updateParams();
+
return OK;
}
@@ -736,7 +757,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 (pos_sp_triplet.current.valid && !(pos_sp_triplet.current.type == SETPOINT_TYPE_LOITER)) {
@@ -744,7 +765,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;
@@ -803,12 +824,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
@@ -831,6 +853,15 @@ FixedwingPositionControl::control_position(const math::Vector<2> &current_positi
// else integrators should be constantly reset.
if (pos_sp_triplet.current.valid) {
+ if (!_was_pos_control_mode) {
+ /* reset integrators */
+ if (_mTecs.getEnabled()) {
+ _mTecs.resetIntegrators();
+ }
+ }
+
+ _was_pos_control_mode = true;
+
/* get circle mode */
bool was_circle_mode = _l1_control.circle_mode();
@@ -863,29 +894,27 @@ FixedwingPositionControl::control_position(const math::Vector<2> &current_positi
if (pos_sp_triplet.current.type == SETPOINT_TYPE_POSITION) {
/* 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) {
@@ -910,7 +939,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));
@@ -920,7 +949,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();
@@ -944,7 +973,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);
@@ -979,11 +1008,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 ? fwPosctrl::mTecs::TECS_MODE_LAND_THROTTLELIM : fwPosctrl::mTecs::TECS_MODE_LAND);
if (!land_noreturn_vertical) {
mavlink_log_info(_mavlink_fd, "#audio: Landing, flaring");
@@ -996,11 +1027,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) {
@@ -1009,14 +1044,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) {
@@ -1043,7 +1084,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();
@@ -1054,22 +1095,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,
+ fwPosctrl::mTecs::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 {
@@ -1103,19 +1158,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 */
@@ -1123,25 +1180,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 */) {
@@ -1174,18 +1232,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 */
@@ -1202,9 +1261,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.getEnabled() ? _mTecs.getThrottleSetpoint() :_tecs.get_throttle_demand(), throttle_max);
}
- _att_sp.pitch_body = _tecs.get_pitch_demand();
+ _att_sp.pitch_body = _mTecs.getEnabled() ? _mTecs.getPitchSetpoint() : _tecs.get_pitch_demand();
if (_control_mode.flag_control_position_enabled) {
last_manual = false;
@@ -1318,7 +1377,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);
/*
@@ -1380,6 +1439,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,
+ fwPosctrl::mTecs::tecs_mode mode)
+{
+ if (_mTecs.getEnabled()) {
+ 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);
+ }
+ _mTecs.updateAltitudeSpeed(flightPathAngle, altitude, alt_sp, _airspeed.true_airspeed_m_s, v_sp, mode);
+ } else {
+ _tecs.update_pitch_throttle(_R_nb, _att.pitch, altitude, alt_sp, v_sp,
+ _airspeed.indicated_airspeed_m_s, eas2tas,
+ climbout_mode, climbout_pitch_min_rad,
+ throttle_min, throttle_max, throttle_cruise,
+ pitch_min_rad, pitch_max_rad);
+ }
+}
+
int
FixedwingPositionControl::start()
{
diff --git a/src/modules/fw_pos_control_l1/module.mk b/src/modules/fw_pos_control_l1/module.mk
index cf419ec7e..c887223f4 100644
--- a/src/modules/fw_pos_control_l1/module.mk
+++ b/src/modules/fw_pos_control_l1/module.mk
@@ -39,4 +39,6 @@ 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/mTecs_params.c
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 000000000..087e1b476
--- /dev/null
+++ b/src/modules/fw_pos_control_l1/mtecs/mTecs.cpp
@@ -0,0 +1,284 @@
+/****************************************************************************
+ *
+ * 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"),
+ _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)
+{
+ /* 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);
+}
+
+int mTecs::updateFlightPathAngleSpeed(float flightPathAngle, float flightPathAngleSp, float airspeed, float airspeedSp, tecs_mode mode)
+{
+ /* 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();
+
+ /* calculate longitudinal acceleration setpoint from airspeed setpoint*/
+ float accelerationLongitudinalSp = _controlAirSpeed.update(airspeedSp - airspeed);
+
+ /* Debug output */
+ if (_counter % 10 == 0) {
+ debug("updateFlightPathAngleSpeed airspeedSp %.4f, airspeed %.4f, accelerationLongitudinalSp%.4f", (double)airspeedSp, (double)airspeed, (double)accelerationLongitudinalSp);
+ }
+
+ /* Write part of the status message */
+ _status.flightPathAngleSp = flightPathAngleSp;
+ _status.flightPathAngle = flightPathAngle;
+ _status.airspeedSp = airspeedSp;
+ _status.airspeed = airspeed;
+
+
+ /* use longitudinal acceleration setpoint for total energy control */
+ return updateFlightPathAngleAcceleration(flightPathAngle, flightPathAngleSp, airspeed, accelerationLongitudinalSp, mode);
+}
+
+int mTecs::updateFlightPathAngleAcceleration(float flightPathAngle, float flightPathAngleSp, float airspeed, float accelerationLongitudinalSp, tecs_mode mode)
+{
+ /* check if all input arguments are numbers and abort if not so */
+ if (!isfinite(flightPathAngle) || !isfinite(flightPathAngleSp) ||
+ !isfinite(airspeed) || !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(airspeed);
+ }
+ 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 (airspeed < _airspeedMin.get()) {
+ mode = TECS_MODE_UNDERSPEED;
+ }
+
+ /* Set special ouput limiters if we are not in TECS_MODE_NORMAL */
+ BlockOutputLimiter *outputLimiterThrottle = NULL; // NULL --> use standard inflight limits
+ BlockOutputLimiter *outputLimiterPitch = NULL; // NULL --> use standard inflight limits
+ 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;
+ }
+
+ /* Write part of the status message */
+ _status.airspeedDerivativeSp = airspeedDerivativeSp;
+ _status.airspeedDerivative = airspeedDerivative;
+ _status.totalEnergyRateSp = totalEnergyRateSp;
+ _status.totalEnergyRate = totalEnergyRate;
+ _status.energyDistributionRateSp = energyDistributionRateSp;
+ _status.energyDistributionRate = energyDistributionRate;
+
+ /** 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::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 000000000..376d39698
--- /dev/null
+++ b/src/modules/fw_pos_control_l1/mtecs/mTecs.h
@@ -0,0 +1,147 @@
+/****************************************************************************
+ *
+ * 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 <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();
+
+ typedef enum {
+ TECS_MODE_NORMAL,
+ TECS_MODE_UNDERSPEED,
+ TECS_MODE_TAKEOFF,
+ TECS_MODE_LAND,
+ TECS_MODE_LAND_THROTTLELIM
+ } tecs_mode;
+
+ /*
+ * Control in altitude setpoint and speed mode
+ */
+ int updateAltitudeSpeed(float flightPathAngle, float altitude, float altitudeSp, float airspeed, float airspeedSp, tecs_mode mode);
+
+ /*
+ * Control in flightPathAngle setpoint (flollow a slope etc.) and speed mode
+ */
+ int updateFlightPathAngleSpeed(float flightPathAngle, float flightPathAngleSp, float airspeed, float airspeedSp, tecs_mode mode);
+
+ /*
+ * Control in flightPathAngle setpoint (flollow a slope etc.) and acceleration mode (base case)
+ */
+ int updateFlightPathAngleAcceleration(float flightPathAngle, float flightPathAngleSp, float airspeed, float accelerationLongitudinalSp, tecs_mode mode);
+
+ /*
+ * Reset all integrators
+ */
+ void resetIntegrators();
+
+
+ /* Accessors */
+ bool getEnabled() {return _mTecsEnabled.get() > 0;}
+ float getThrottleSetpoint() {return _throttleSp;}
+ float getPitchSetpoint() {return _pitchSp;}
+
+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; /**< P controller for altitude: output is the flight path angle setpoint */
+ BlockPLimited _controlAirSpeed; /**< P controller for airspeed: output is acceleration setpoint */
+
+ /* Other calculation Blocks */
+ control::BlockDerivative _airspeedDerivative;
+
+ /* 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 000000000..a7acd95de
--- /dev/null
+++ b/src/modules/fw_pos_control_l1/mtecs/mTecs_blocks.h
@@ -0,0 +1,219 @@
+/****************************************************************************
+ *
+ * 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(); }
+protected:
+//attributes
+ bool _isAngularLimit;
+ control::BlockParamFloat _min;
+ control::BlockParamFloat _max;
+};
+
+typedef
+
+/* 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 000000000..591257611
--- /dev/null
+++ b/src/modules/fw_pos_control_l1/mtecs/mTecs_params.c
@@ -0,0 +1,380 @@
+/****************************************************************************
+ *
+ * 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 FF
+ *
+ * @min 0.0
+ * @max 10.0
+ * @group mTECS
+ */
+PARAM_DEFINE_FLOAT(MT_THR_FF, 0.2f);
+
+/**
+ * Total Energy Rate Control P
+ *
+ * @min 0.0
+ * @max 10.0
+ * @group mTECS
+ */
+PARAM_DEFINE_FLOAT(MT_THR_P, 0.03f);
+
+/**
+ * Total Energy Rate Control I
+ *
+ * @min 0.0
+ * @max 10.0
+ * @group mTECS
+ */
+PARAM_DEFINE_FLOAT(MT_THR_I, 0.1f);
+
+/**
+ * Total Energy Rate Control OFF (Cruise throttle)
+ *
+ * @min 0.0
+ * @max 10.0
+ * @group mTECS
+ */
+PARAM_DEFINE_FLOAT(MT_THR_OFF, 0.7f);
+
+/**
+ * Energy Distribution Rate Control FF
+ *
+ * @min 0.0
+ * @max 10.0
+ * @group mTECS
+ */
+PARAM_DEFINE_FLOAT(MT_PIT_FF, 0.1f);
+
+/**
+ * Energy Distribution Rate Control P
+ *
+ * @min 0.0
+ * @max 10.0
+ * @group mTECS
+ */
+PARAM_DEFINE_FLOAT(MT_PIT_P, 0.03f);
+
+/**
+ * Energy Distribution Rate Control I
+ *
+ * @min 0.0
+ * @max 10.0
+ * @group mTECS
+ */
+PARAM_DEFINE_FLOAT(MT_PIT_I, 0.03f);
+
+
+/**
+ * Total Energy Distribution OFF (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
+ *
+ * @min 0.0f
+ * @max 10.0f
+ * @group mTECS
+ */
+PARAM_DEFINE_FLOAT(MT_FPA_P, 0.2f);
+
+/**
+ * D gain for the altitude control
+ *
+ * @min 0.0f
+ * @max 10.0f
+ * @group mTECS
+ */
+PARAM_DEFINE_FLOAT(MT_FPA_D, 0.0f);
+
+/**
+ * Lowpass for FPA error derivative (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, -10.0f);
+
+/**
+ * Maximal flight path angle setpoint
+ *
+ * @min -90.0f
+ * @max 90.0f
+ * @unit deg
+ * @group mTECS
+ */
+PARAM_DEFINE_FLOAT(MT_FPA_MAX, 30.0f);
+
+
+/**
+ * P gain for the airspeed control
+ *
+ * @min 0.0f
+ * @max 10.0f
+ * @group mTECS
+ */
+PARAM_DEFINE_FLOAT(MT_ACC_P, 1.5f);
+
+/**
+ * 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 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 2b6caf159..dadfe40b2 100644
--- a/src/modules/sdlog2/sdlog2.c
+++ b/src/modules/sdlog2/sdlog2.c
@@ -84,6 +84,7 @@
#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>
@@ -939,6 +940,7 @@ 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;
} buf;
@@ -979,6 +981,7 @@ 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;
} body;
} log_msg = {
LOG_PACKET_HEADER_INIT(0)
@@ -1010,6 +1013,7 @@ 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;
} subs;
@@ -1037,6 +1041,7 @@ 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));
@@ -1488,6 +1493,24 @@ 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.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;
+ LOGBUFFER_WRITE_AND_COUNT(TECS);
+ }
+
/* 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 f4d88f079..8a0a0ad45 100644
--- a/src/modules/sdlog2/sdlog2_messages.h
+++ b/src/modules/sdlog2/sdlog2_messages.h
@@ -346,6 +346,24 @@ 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 airspeedDerivativeSp;
+ float airspeedDerivative;
+
+ float totalEnergyRateSp;
+ float totalEnergyRate;
+ float energyDistributionRateSp;
+ float energyDistributionRate;
+};
+
/********** SYSTEM MESSAGES, ID > 0x80 **********/
/* --- TIME - TIME STAMP --- */
@@ -401,6 +419,7 @@ 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, "ffffffffffff", "AltSP,Alt,FpaSP,Fpa,AsSP,As,AsDSP,AsD,TERSP,TER,EDRSP,EDR"),
/* 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 5a5981617..cd0b30dd6 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<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>;
}
diff --git a/src/modules/uORB/objects_common.cpp b/src/modules/uORB/objects_common.cpp
index 90675bb2e..bae46e14d 100644
--- a/src/modules/uORB/objects_common.cpp
+++ b/src/modules/uORB/objects_common.cpp
@@ -199,3 +199,7 @@ 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);
+
diff --git a/src/modules/uORB/topics/tecs_status.h b/src/modules/uORB/topics/tecs_status.h
new file mode 100644
index 000000000..f3d33ec20
--- /dev/null
+++ b/src/modules/uORB/topics/tecs_status.h
@@ -0,0 +1,81 @@
+/****************************************************************************
+ *
+ * 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
+ * @{
+ */
+
+ /**
+ * 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 airspeedDerivativeSp;
+ float airspeedDerivative;
+
+ float totalEnergyRateSp;
+ float totalEnergyRate;
+ float energyDistributionRateSp;
+ float energyDistributionRate;
+};
+
+/**
+ * @}
+ */
+
+/* register this as object request broker structure */
+ORB_DECLARE(tecs_status);
+
+#endif
generated by cgit v1.2.3 (git 2.39.1) at 2024-07-08 14:11:06 +0000