aboutsummaryrefslogtreecommitdiff
path: root/src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp
diff options
context:
space:
mode:
Diffstat (limited to 'src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp')
-rw-r--r--src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp987
1 files changed, 647 insertions, 340 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 a9648b207..08c996ebc 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
@@ -1,7 +1,6 @@
/****************************************************************************
*
- * Copyright (c) 2013 PX4 Development Team. All rights reserved.
- * Author: Lorenz Meier
+ * Copyright (c) 2013, 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
@@ -43,12 +42,13 @@
* 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.
*
* @author Lorenz Meier <lm@inf.ethz.ch>
+ * @author Thomas Gubler <thomasgubler@gmail.com>
*/
#include <nuttx/config.h>
@@ -67,7 +67,7 @@
#include <uORB/uORB.h>
#include <uORB/topics/airspeed.h>
#include <uORB/topics/vehicle_global_position.h>
-#include <uORB/topics/vehicle_global_position_set_triplet.h>
+#include <uORB/topics/position_setpoint_triplet.h>
#include <uORB/topics/vehicle_attitude_setpoint.h>
#include <uORB/topics/manual_control_setpoint.h>
#include <uORB/topics/actuator_controls.h>
@@ -75,6 +75,7 @@
#include <uORB/topics/vehicle_attitude.h>
#include <uORB/topics/vehicle_control_mode.h>
#include <uORB/topics/navigation_capabilities.h>
+#include <uORB/topics/sensor_combined.h>
#include <uORB/topics/parameter_update.h>
#include <systemlib/param/param.h>
#include <systemlib/err.h>
@@ -83,9 +84,13 @@
#include <systemlib/perf_counter.h>
#include <systemlib/systemlib.h>
#include <mathlib/mathlib.h>
-
+#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"
+
/**
* L1 control app start / stop handling function
@@ -114,66 +119,94 @@ public:
*/
int start();
+ /**
+ * Task status
+ *
+ * @return true if the mainloop is running
+ */
+ bool task_running() { return _task_running; }
+
private:
+ int _mavlink_fd;
bool _task_should_exit; /**< if true, sensor task should exit */
+ bool _task_running; /**< if true, task is running in its mainloop */
int _control_task; /**< task handle for sensor task */
int _global_pos_sub;
- int _global_set_triplet_sub;
+ int _pos_sp_triplet_sub;
int _att_sub; /**< vehicle attitude subscription */
int _attitude_sub; /**< raw rc channels data subscription */
int _airspeed_sub; /**< airspeed subscription */
- int _control_mode_sub; /**< vehicle status subscription */
+ int _control_mode_sub; /**< vehicle status subscription */
int _params_sub; /**< notification of parameter updates */
int _manual_control_sub; /**< notification of manual control updates */
- int _accel_sub; /**< body frame accelerations */
+ int _sensor_combined_sub; /**< for body frame accelerations */
+ int _range_finder_sub; /**< range finder subscription */
orb_advert_t _attitude_sp_pub; /**< attitude setpoint */
orb_advert_t _nav_capabilities_pub; /**< navigation capabilities publication */
- struct vehicle_attitude_s _att; /**< vehicle attitude */
- struct vehicle_attitude_setpoint_s _att_sp; /**< vehicle attitude setpoint */
- struct navigation_capabilities_s _nav_capabilities; /**< navigation capabilities */
- struct manual_control_setpoint_s _manual; /**< r/c channel data */
- struct airspeed_s _airspeed; /**< airspeed */
- struct vehicle_control_mode_s _control_mode; /**< vehicle status */
- struct vehicle_global_position_s _global_pos; /**< global vehicle position */
- struct vehicle_global_position_set_triplet_s _global_triplet; /**< triplet of global setpoints */
- struct accel_report _accel; /**< body frame accelerations */
+ struct vehicle_attitude_s _att; /**< vehicle attitude */
+ struct vehicle_attitude_setpoint_s _att_sp; /**< vehicle attitude setpoint */
+ struct navigation_capabilities_s _nav_capabilities; /**< navigation capabilities */
+ struct manual_control_setpoint_s _manual; /**< r/c channel data */
+ struct airspeed_s _airspeed; /**< airspeed */
+ struct vehicle_control_mode_s _control_mode; /**< vehicle status */
+ struct vehicle_global_position_s _global_pos; /**< global vehicle position */
+ struct position_setpoint_triplet_s _pos_sp_triplet; /**< triplet of mission items */
+ struct sensor_combined_s _sensor_combined; /**< for body frame accelerations */
+ struct range_finder_report _range_finder; /**< range finder report */
perf_counter_t _loop_perf; /**< loop performance counter */
- bool _setpoint_valid; /**< flag if the position control setpoint is valid */
-
/** manual control states */
- float _seatbelt_hold_heading; /**< heading the system should hold in seatbelt mode */
- float _loiter_hold_lat;
- float _loiter_hold_lon;
+ float _altctrl_hold_heading; /**< heading the system should hold in altctrl mode */
+ double _loiter_hold_lat;
+ double _loiter_hold_lon;
float _loiter_hold_alt;
bool _loiter_hold;
- float _launch_lat;
- float _launch_lon;
+ double _launch_lat;
+ double _launch_lon;
float _launch_alt;
bool _launch_valid;
/* land states */
/* not in non-abort mode for landing yet */
- bool land_noreturn;
+ bool land_noreturn_horizontal;
+ bool land_noreturn_vertical;
+ bool land_stayonground;
+ bool land_motor_lim;
+ bool land_onslope;
+
+ /* takeoff/launch states */
+ bool launch_detected;
+ bool usePreTakeoffThrust;
+
+ bool last_manual; ///< true if the last iteration was in manual mode (used to determine when a reset is needed)
+
+ /* Landingslope object */
+ Landingslope landingslope;
+
+ float flare_curve_alt_rel_last;
/* heading hold */
float target_bearing;
+ /* Launch detection */
+ launchdetection::LaunchDetector launchDetector;
+
/* throttle and airspeed states */
float _airspeed_error; ///< airspeed error to setpoint in m/s
bool _airspeed_valid; ///< flag if a valid airspeed estimate exists
uint64_t _airspeed_last_valid; ///< last time airspeed was valid. Used to detect sensor failures
float _groundspeed_undershoot; ///< ground speed error to min. speed in m/s
bool _global_pos_valid; ///< global position is valid
- math::Dcm _R_nb; ///< current attitude
+ 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;
@@ -205,7 +238,16 @@ private:
float throttle_land_max;
- float loiter_hold_radius;
+ float heightrate_p;
+ float speedrate_p;
+
+ float land_slope_angle;
+ float land_H1_virt;
+ float land_flare_alt_relative;
+ float land_thrust_lim_alt_relative;
+ float land_heading_hold_horizontal_distance;
+ float range_finder_rel_alt;
+
} _parameters; /**< local copies of interesting parameters */
struct {
@@ -239,7 +281,16 @@ private:
param_t throttle_land_max;
- param_t loiter_hold_radius;
+ param_t heightrate_p;
+ param_t speedrate_p;
+
+ param_t land_slope_angle;
+ param_t land_H1_virt;
+ param_t land_flare_alt_relative;
+ param_t land_thrust_lim_alt_relative;
+ param_t land_heading_hold_horizontal_distance;
+ param_t range_finder_rel_alt;
+
} _parameter_handles; /**< handles for interesting parameters */
@@ -265,6 +316,12 @@ private:
bool vehicle_airspeed_poll();
/**
+ * Check for range finder updates.
+ */
+ bool range_finder_poll();
+
+
+ /**
* Check for position updates.
*/
void vehicle_attitude_poll();
@@ -272,7 +329,7 @@ private:
/**
* Check for accel updates.
*/
- void vehicle_accel_poll();
+ void vehicle_sensor_combined_poll();
/**
* Check for set triplet updates.
@@ -280,13 +337,23 @@ private:
void vehicle_setpoint_poll();
/**
+ * Publish navigation capabilities
+ */
+ void navigation_capabilities_publish();
+
+ /**
+ * Get the relative alt either from the difference between estimate and waypoint or from the laser range finder
+ */
+ float get_relative_landingalt(float land_setpoint_alt, float current_alt, const struct range_finder_report &range_finder, float range_finder_use_relative_alt);
+
+ /**
* Control position.
*/
- bool control_position(const math::Vector2f &global_pos, const math::Vector2f &ground_speed,
- const struct vehicle_global_position_set_triplet_s &global_triplet);
+ 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();
+ 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.
@@ -296,7 +363,30 @@ private:
/**
* Main sensor collection task.
*/
- void task_main() __attribute__((noreturn));
+ void task_main();
+
+ /*
+ * Reset takeoff state
+ */
+ void reset_takeoff_state();
+
+ /*
+ * 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
@@ -308,43 +398,67 @@ namespace l1_control
#endif
static const int ERROR = -1;
-FixedwingPositionControl *g_control;
+FixedwingPositionControl *g_control = nullptr;
}
FixedwingPositionControl::FixedwingPositionControl() :
+ _mavlink_fd(-1),
_task_should_exit(false),
+ _task_running(false),
_control_task(-1),
/* subscriptions */
_global_pos_sub(-1),
- _global_set_triplet_sub(-1),
+ _pos_sp_triplet_sub(-1),
_att_sub(-1),
_airspeed_sub(-1),
_control_mode_sub(-1),
_params_sub(-1),
_manual_control_sub(-1),
+ _range_finder_sub(-1),
/* publications */
_attitude_sp_pub(-1),
_nav_capabilities_pub(-1),
+ _att(),
+ _att_sp(),
+ _nav_capabilities(),
+ _manual(),
+ _airspeed(),
+ _control_mode(),
+ _global_pos(),
+ _pos_sp_triplet(),
+ _sensor_combined(),
+ _range_finder(),
+
/* performance counters */
_loop_perf(perf_alloc(PC_ELAPSED, "fw l1 control")),
+
/* states */
- _setpoint_valid(false),
_loiter_hold(false),
+ land_noreturn_horizontal(false),
+ land_noreturn_vertical(false),
+ land_stayonground(false),
+ land_motor_lim(false),
+ land_onslope(false),
+ launch_detected(false),
+ usePreTakeoffThrust(false),
+ last_manual(false),
+ flare_curve_alt_rel_last(0.0f),
+ launchDetector(),
_airspeed_error(0.0f),
_airspeed_valid(false),
_groundspeed_undershoot(0.0f),
_global_pos_valid(false),
- land_noreturn(false)
+ _mTecs(),
+ _was_pos_control_mode(false)
{
_nav_capabilities.turn_distance = 0.0f;
_parameter_handles.l1_period = param_find("FW_L1_PERIOD");
_parameter_handles.l1_damping = param_find("FW_L1_DAMPING");
- _parameter_handles.loiter_hold_radius = param_find("FW_LOITER_R");
_parameter_handles.airspeed_min = param_find("FW_AIRSPD_MIN");
_parameter_handles.airspeed_trim = param_find("FW_AIRSPD_TRIM");
@@ -358,6 +472,13 @@ FixedwingPositionControl::FixedwingPositionControl() :
_parameter_handles.throttle_cruise = param_find("FW_THR_CRUISE");
_parameter_handles.throttle_land_max = param_find("FW_THR_LND_MAX");
+ _parameter_handles.land_slope_angle = param_find("FW_LND_ANG");
+ _parameter_handles.land_H1_virt = param_find("FW_LND_HVIRT");
+ _parameter_handles.land_flare_alt_relative = param_find("FW_LND_FLALT");
+ _parameter_handles.land_thrust_lim_alt_relative = param_find("FW_LND_TLALT");
+ _parameter_handles.land_heading_hold_horizontal_distance = param_find("FW_LND_HHDIST");
+ _parameter_handles.range_finder_rel_alt = param_find("FW_LND_RFRALT");
+
_parameter_handles.time_const = param_find("FW_T_TIME_CONST");
_parameter_handles.min_sink_rate = param_find("FW_T_SINK_MIN");
_parameter_handles.max_sink_rate = param_find("FW_T_SINK_MAX");
@@ -370,6 +491,8 @@ FixedwingPositionControl::FixedwingPositionControl() :
_parameter_handles.roll_throttle_compensation = param_find("FW_T_RLL2THR");
_parameter_handles.speed_weight = param_find("FW_T_SPDWEIGHT");
_parameter_handles.pitch_damping = param_find("FW_T_PTCH_DAMP");
+ _parameter_handles.heightrate_p = param_find("FW_T_HRATE_P");
+ _parameter_handles.speedrate_p = param_find("FW_T_SRATE_P");
/* fetch initial parameter values */
parameters_update();
@@ -407,7 +530,6 @@ FixedwingPositionControl::parameters_update()
/* L1 control parameters */
param_get(_parameter_handles.l1_damping, &(_parameters.l1_damping));
param_get(_parameter_handles.l1_period, &(_parameters.l1_period));
- param_get(_parameter_handles.loiter_hold_radius, &(_parameters.loiter_hold_radius));
param_get(_parameter_handles.airspeed_min, &(_parameters.airspeed_min));
param_get(_parameter_handles.airspeed_trim, &(_parameters.airspeed_trim));
@@ -435,25 +557,21 @@ FixedwingPositionControl::parameters_update()
param_get(_parameter_handles.pitch_damping, &(_parameters.pitch_damping));
param_get(_parameter_handles.max_climb_rate, &(_parameters.max_climb_rate));
+ param_get(_parameter_handles.heightrate_p, &(_parameters.heightrate_p));
+ param_get(_parameter_handles.speedrate_p, &(_parameters.speedrate_p));
+
+ param_get(_parameter_handles.land_slope_angle, &(_parameters.land_slope_angle));
+ param_get(_parameter_handles.land_H1_virt, &(_parameters.land_H1_virt));
+ param_get(_parameter_handles.land_flare_alt_relative, &(_parameters.land_flare_alt_relative));
+ param_get(_parameter_handles.land_thrust_lim_alt_relative, &(_parameters.land_thrust_lim_alt_relative));
+ param_get(_parameter_handles.land_heading_hold_horizontal_distance, &(_parameters.land_heading_hold_horizontal_distance));
+
+ param_get(_parameter_handles.range_finder_rel_alt, &(_parameters.range_finder_rel_alt));
+
_l1_control.set_l1_damping(_parameters.l1_damping);
_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(math::radians(_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);
-
/* sanity check parameters */
if (_parameters.airspeed_max < _parameters.airspeed_min ||
_parameters.airspeed_max < 5.0f ||
@@ -464,6 +582,21 @@ FixedwingPositionControl::parameters_update()
return 1;
}
+ /* Update the landing slope */
+ landingslope.update(math::radians(_parameters.land_slope_angle), _parameters.land_flare_alt_relative, _parameters.land_thrust_lim_alt_relative, _parameters.land_H1_virt);
+
+ /* Update and publish the navigation capabilities */
+ _nav_capabilities.landing_slope_angle_rad = landingslope.landing_slope_angle_rad();
+ _nav_capabilities.landing_horizontal_slope_displacement = landingslope.horizontal_slope_displacement();
+ _nav_capabilities.landing_flare_length = landingslope.flare_length();
+ navigation_capabilities_publish();
+
+ /* Update Launch Detector Parameters */
+ launchDetector.updateParams();
+
+ /* Update the mTecs */
+ _mTecs.updateParams();
+
return OK;
}
@@ -482,8 +615,8 @@ FixedwingPositionControl::vehicle_control_mode_poll()
orb_copy(ORB_ID(vehicle_control_mode), _control_mode_sub, &_control_mode);
if (!was_armed && _control_mode.flag_armed) {
- _launch_lat = _global_pos.lat / 1e7f;
- _launch_lon = _global_pos.lon / 1e7f;
+ _launch_lat = _global_pos.lat;
+ _launch_lon = _global_pos.lon;
_launch_alt = _global_pos.alt;
_launch_valid = true;
}
@@ -510,12 +643,23 @@ FixedwingPositionControl::vehicle_airspeed_poll()
}
}
- /* update TECS state */
- _tecs.enable_airspeed(_airspeed_valid);
-
return airspeed_updated;
}
+bool
+FixedwingPositionControl::range_finder_poll()
+{
+ /* check if there is a range finder measurement */
+ bool range_finder_updated;
+ orb_check(_range_finder_sub, &range_finder_updated);
+
+ if (range_finder_updated) {
+ orb_copy(ORB_ID(sensor_range_finder), _range_finder_sub, &_range_finder);
+ }
+
+ return range_finder_updated;
+}
+
void
FixedwingPositionControl::vehicle_attitude_poll()
{
@@ -533,14 +677,14 @@ FixedwingPositionControl::vehicle_attitude_poll()
}
void
-FixedwingPositionControl::vehicle_accel_poll()
+FixedwingPositionControl::vehicle_sensor_combined_poll()
{
/* check if there is a new position */
- bool accel_updated;
- orb_check(_accel_sub, &accel_updated);
+ bool sensors_updated;
+ orb_check(_sensor_combined_sub, &sensors_updated);
- if (accel_updated) {
- orb_copy(ORB_ID(sensor_accel), _accel_sub, &_accel);
+ if (sensors_updated) {
+ orb_copy(ORB_ID(sensor_combined), _sensor_combined_sub, &_sensor_combined);
}
}
@@ -548,12 +692,11 @@ void
FixedwingPositionControl::vehicle_setpoint_poll()
{
/* check if there is a new setpoint */
- bool global_sp_updated;
- orb_check(_global_set_triplet_sub, &global_sp_updated);
+ bool pos_sp_triplet_updated;
+ orb_check(_pos_sp_triplet_sub, &pos_sp_triplet_updated);
- if (global_sp_updated) {
- orb_copy(ORB_ID(vehicle_global_position_set_triplet), _global_set_triplet_sub, &_global_triplet);
- _setpoint_valid = true;
+ if (pos_sp_triplet_updated) {
+ orb_copy(ORB_ID(position_setpoint_triplet), _pos_sp_triplet_sub, &_pos_sp_triplet);
}
}
@@ -595,17 +738,29 @@ FixedwingPositionControl::calculate_target_airspeed(float airspeed_demand)
}
void
-FixedwingPositionControl::calculate_gndspeed_undershoot()
+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) {
- /* get ground speed vector */
- math::Vector2f ground_speed_vector(_global_pos.vx, _global_pos.vy);
+ if (pos_sp_triplet.current.valid && !(pos_sp_triplet.current.type == SETPOINT_TYPE_LOITER)) {
- /* rotate with current attitude */
- math::Vector2f yaw_vector(_R_nb(0, 0), _R_nb(1, 0));
+ /* 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_vector;
+ 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;
+ float delta_altitude = 0.0f;
+ if (pos_sp_triplet.previous.valid) {
+ distance = get_distance_to_next_waypoint(pos_sp_triplet.previous.lat, pos_sp_triplet.previous.lon, pos_sp_triplet.current.lat, pos_sp_triplet.current.lon);
+ delta_altitude = pos_sp_triplet.current.alt - pos_sp_triplet.previous.alt;
+ } else {
+ distance = get_distance_to_next_waypoint(current_position(0), current_position(1), pos_sp_triplet.current.lat, pos_sp_triplet.current.lon);
+ delta_altitude = pos_sp_triplet.current.alt - _global_pos.alt;
+ }
+
+ float ground_speed_desired = _parameters.airspeed_min * cosf(atan2f(delta_altitude, distance));
+
/*
* Ground speed undershoot is the amount of ground velocity not reached
@@ -616,32 +771,54 @@ FixedwingPositionControl::calculate_gndspeed_undershoot()
* not exceeded) travels towards a waypoint (and is not pushed more and more away
* by wind). Not countering this would lead to a fly-away.
*/
- _groundspeed_undershoot = math::max(_parameters.airspeed_min - ground_speed_body, 0.0f);
+ _groundspeed_undershoot = math::max(ground_speed_desired - ground_speed_body, 0.0f);
} else {
_groundspeed_undershoot = 0;
}
}
+void FixedwingPositionControl::navigation_capabilities_publish()
+{
+ if (_nav_capabilities_pub > 0) {
+ orb_publish(ORB_ID(navigation_capabilities), _nav_capabilities_pub, &_nav_capabilities);
+ } else {
+ _nav_capabilities_pub = orb_advertise(ORB_ID(navigation_capabilities), &_nav_capabilities);
+ }
+}
+
+float FixedwingPositionControl::get_relative_landingalt(float land_setpoint_alt, float current_alt, const struct range_finder_report &range_finder, float range_finder_use_relative_alt)
+{
+ float rel_alt_estimated = current_alt - land_setpoint_alt;
+
+ /* only use range finder if:
+ * parameter (range_finder_use_relative_alt) > 0
+ * the measurement is valid
+ * the estimated relative altitude (from global altitude estimate and landing waypoint) <= range_finder_use_relative_alt
+ */
+ if (range_finder_use_relative_alt < 0 || !range_finder.valid || rel_alt_estimated > range_finder_use_relative_alt ) {
+ return rel_alt_estimated;
+ }
+
+ return range_finder.distance;
+
+}
+
bool
-FixedwingPositionControl::control_position(const math::Vector2f &current_position, const math::Vector2f &ground_speed,
- const struct vehicle_global_position_set_triplet_s &global_triplet)
+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();
+ 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
- // XXX re-visit
- float baro_altitude = _global_pos.alt;
-
/* filter speed and altitude for controller */
- math::Vector3 accel_body(_accel.x, _accel.y, _accel.z);
- math::Vector3 accel_earth = _R_nb.transpose() * accel_body;
+ math::Vector<3> accel_body(_sensor_combined.accelerometer_m_s2);
- _tecs.update_50hz(baro_altitude, _airspeed.indicated_airspeed_m_s, _R_nb, accel_body, accel_earth);
- float altitude_error = _global_triplet.current.altitude - _global_pos.alt;
+ float altitude_error = _pos_sp_triplet.current.alt - _global_pos.alt;
/* no throttle limit as default */
float throttle_max = 1.0f;
@@ -650,260 +827,311 @@ FixedwingPositionControl::control_position(const math::Vector2f &current_positio
// XXX this should only execute if auto AND safety off (actuators active),
// else integrators should be constantly reset.
- if (_control_mode.flag_control_position_enabled) {
+ if (pos_sp_triplet.current.valid) {
- /* get circle mode */
- bool was_circle_mode = _l1_control.circle_mode();
+ if (!_was_pos_control_mode) {
+ /* reset integrators */
+ if (_mTecs.getEnabled()) {
+ _mTecs.resetIntegrators();
+ _mTecs.resetDerivatives(_airspeed.true_airspeed_m_s);
+ }
+ }
- /* restore speed weight, in case changed intermittently (e.g. in landing handling) */
- _tecs.set_speed_weight(_parameters.speed_weight);
+ _was_pos_control_mode = true;
- /* execute navigation once we have a setpoint */
- if (_setpoint_valid) {
+ /* get circle mode */
+ bool was_circle_mode = _l1_control.circle_mode();
- /* current waypoint (the one currently heading for) */
- math::Vector2f next_wp(global_triplet.current.lat / 1e7f, global_triplet.current.lon / 1e7f);
+ /* current waypoint (the one currently heading for) */
+ math::Vector<2> next_wp((float)pos_sp_triplet.current.lat, (float)pos_sp_triplet.current.lon);
- /* previous waypoint */
- math::Vector2f prev_wp;
+ /* current waypoint (the one currently heading for) */
+ math::Vector<2> curr_wp((float)pos_sp_triplet.current.lat, (float)pos_sp_triplet.current.lon);
- if (global_triplet.previous_valid) {
- prev_wp.setX(global_triplet.previous.lat / 1e7f);
- prev_wp.setY(global_triplet.previous.lon / 1e7f);
+ /* Initialize attitude controller integrator reset flags to 0 */
+ _att_sp.roll_reset_integral = false;
+ _att_sp.pitch_reset_integral = false;
+ _att_sp.yaw_reset_integral = false;
- } else {
- /*
- * No valid previous waypoint, go for the current wp.
- * This is automatically handled by the L1 library.
- */
- prev_wp.setX(global_triplet.current.lat / 1e7f);
- prev_wp.setY(global_triplet.current.lon / 1e7f);
+ /* previous waypoint */
+ math::Vector<2> prev_wp;
- }
+ if (pos_sp_triplet.previous.valid) {
+ prev_wp(0) = (float)pos_sp_triplet.previous.lat;
+ prev_wp(1) = (float)pos_sp_triplet.previous.lon;
- // XXX add RTL switch
- if (global_triplet.current.nav_cmd == NAV_CMD_RETURN_TO_LAUNCH && _launch_valid) {
+ } else {
+ /*
+ * No valid previous waypoint, go for the current wp.
+ * This is automatically handled by the L1 library.
+ */
+ prev_wp(0) = (float)pos_sp_triplet.current.lat;
+ prev_wp(1) = (float)pos_sp_triplet.current.lon;
- math::Vector2f rtl_pos(_launch_lat, _launch_lon);
+ }
- _l1_control.navigate_waypoints(rtl_pos, rtl_pos, current_position, ground_speed);
- _att_sp.roll_body = _l1_control.nav_roll();
- _att_sp.yaw_body = _l1_control.nav_bearing();
+ 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_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, _launch_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);
- // XXX handle case when having arrived at home (loiter)
+ } else if (pos_sp_triplet.current.type == SETPOINT_TYPE_LOITER) {
- } else if (global_triplet.current.nav_cmd == NAV_CMD_WAYPOINT) {
- /* waypoint is a plain navigation waypoint */
- _l1_control.navigate_waypoints(prev_wp, next_wp, current_position, ground_speed);
- _att_sp.roll_body = _l1_control.nav_roll();
- _att_sp.yaw_body = _l1_control.nav_bearing();
+ /* 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_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_triplet.current.altitude, 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 (global_triplet.current.nav_cmd == NAV_CMD_LOITER_TURN_COUNT ||
- global_triplet.current.nav_cmd == NAV_CMD_LOITER_TIME_LIMIT ||
- global_triplet.current.nav_cmd == NAV_CMD_LOITER_UNLIMITED) {
+ } else if (pos_sp_triplet.current.type == SETPOINT_TYPE_LAND) {
- /* waypoint is a loiter waypoint */
- _l1_control.navigate_loiter(next_wp, current_position, global_triplet.current.loiter_radius,
- global_triplet.current.loiter_direction, ground_speed);
- _att_sp.roll_body = _l1_control.nav_roll();
- _att_sp.yaw_body = _l1_control.nav_bearing();
+ float bearing_lastwp_currwp = get_bearing_to_next_waypoint(prev_wp(0), prev_wp(1), curr_wp(0), curr_wp(1));
- _tecs.update_pitch_throttle(_R_nb, _att.pitch, _global_pos.alt, _global_triplet.current.altitude, 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));
+ /* Horizontal landing control */
+ /* switch to heading hold for the last meters, continue heading hold after */
+ float wp_distance = get_distance_to_next_waypoint(current_position(0), current_position(1), curr_wp(0), curr_wp(1));
+ //warnx("wp dist: %d, alt err: %d, noret: %s", (int)wp_distance, (int)altitude_error, (land_noreturn) ? "YES" : "NO");
+ if (wp_distance < _parameters.land_heading_hold_horizontal_distance || land_noreturn_horizontal) {
- } else if (global_triplet.current.nav_cmd == NAV_CMD_LAND) {
+ /* heading hold, along the line connecting this and the last waypoint */
- /* switch to heading hold for the last meters, continue heading hold after */
+ if (!land_noreturn_horizontal) {//set target_bearing in first occurrence
+ if (pos_sp_triplet.previous.valid) {
+ target_bearing = bearing_lastwp_currwp;
+ } else {
+ target_bearing = _att.yaw;
+ }
+ mavlink_log_info(_mavlink_fd, "#audio: Landing, heading hold");
+ }
- float wp_distance = get_distance_to_next_waypoint(prev_wp.getX(), prev_wp.getY(), current_position.getX(), current_position.getY());
- //warnx("wp dist: %d, alt err: %d, noret: %s", (int)wp_distance, (int)altitude_error, (land_noreturn) ? "YES" : "NO");
- if (wp_distance < 15.0f || land_noreturn) {
+// warnx("NORET: %d, target_bearing: %d, yaw: %d", (int)land_noreturn_horizontal, (int)math::degrees(target_bearing), (int)math::degrees(_att.yaw));
- /* heading hold, along the line connecting this and the last waypoint */
-
+ _l1_control.navigate_heading(target_bearing, _att.yaw, ground_speed_2d);
- // if (global_triplet.previous_valid) {
- // target_bearing = get_bearing_to_next_waypoint(prev_wp.getX(), prev_wp.getY(), next_wp.getX(), next_wp.getY());
- // } else {
+ /* 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));
- if (!land_noreturn)
- target_bearing = _att.yaw;
- //}
+ land_noreturn_horizontal = true;
- warnx("NORET: %d, target_bearing: %d, yaw: %d", (int)land_noreturn, (int)math::degrees(target_bearing), (int)math::degrees(_att.yaw));
+ } else {
- _l1_control.navigate_heading(target_bearing, _att.yaw, ground_speed);
+ /* normal navigation */
+ _l1_control.navigate_waypoints(prev_wp, curr_wp, current_position, ground_speed_2d);
+ }
- if (altitude_error > -5.0f)
- land_noreturn = true;
+ _att_sp.roll_body = _l1_control.nav_roll();
+ _att_sp.yaw_body = _l1_control.nav_bearing();
- } else {
- /* normal navigation */
- _l1_control.navigate_waypoints(prev_wp, next_wp, current_position, ground_speed);
- }
+ /* Vertical landing control */
+ //xxx: using the tecs altitude controller for slope control for now
- /* do not go down too early */
- if (wp_distance > 50.0f) {
- altitude_error = (_global_triplet.current.altitude + 25.0f) - _global_pos.alt;
- }
+// /* do not go down too early */
+// if (wp_distance > 50.0f) {
+// altitude_error = (_global_triplet.current.alt + 25.0f) - _global_pos.alt;
+// }
+ /* apply minimum pitch (flare) and limit roll if close to touch down, altitude error is negative (going down) */
+ // XXX this could make a great param
+ float flare_pitch_angle_rad = -math::radians(5.0f);//math::radians(pos_sp_triplet.current.param1)
+ float throttle_land = _parameters.throttle_min + (_parameters.throttle_max - _parameters.throttle_min) * 0.1f;
+ float airspeed_land = 1.3f * _parameters.airspeed_min;
+ float airspeed_approach = 1.3f * _parameters.airspeed_min;
- _att_sp.roll_body = _l1_control.nav_roll();
- _att_sp.yaw_body = _l1_control.nav_bearing();
+ /* Calculate altitude of last ordinary waypoint L */
+ float L_altitude_rel = _pos_sp_triplet.previous.valid ? _pos_sp_triplet.previous.alt - _pos_sp_triplet.current.alt : 0.0f;
- /* apply minimum pitch (flare) and limit roll if close to touch down, altitude error is negative (going down) */
- // XXX this could make a great param
+ 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);
- float flare_angle_rad = math::radians(10.0f);//math::radians(global_triplet.current.param1)
- float land_pitch_min = math::radians(5.0f);
- float throttle_land = _parameters.throttle_min + (_parameters.throttle_max - _parameters.throttle_min) * 0.1f;
- float airspeed_land = _parameters.airspeed_min;
- float airspeed_approach = (_parameters.airspeed_min + _parameters.airspeed_trim) / 2.0f;
+ float relative_alt = get_relative_landingalt(_pos_sp_triplet.current.alt, _global_pos.alt, _range_finder, _parameters.range_finder_rel_alt);
- if (altitude_error > -4.0f) {
+ if ( (relative_alt < landingslope.flare_relative_alt()) || land_noreturn_vertical) { //checking for land_noreturn to avoid unwanted climb out
- /* land with minimal speed */
+ /* land with minimal speed */
- /* force TECS to only control speed with pitch, altitude is only implicitely controlled now */
- _tecs.set_speed_weight(2.0f);
+// /* force TECS to only control speed with pitch, altitude is only implicitely controlled now */
+// _tecs.set_speed_weight(2.0f);
- _tecs.update_pitch_throttle(_R_nb, _att.pitch, _global_pos.alt, _global_triplet.current.altitude, calculate_target_airspeed(airspeed_land),
- _airspeed.indicated_airspeed_m_s, eas2tas,
- false, flare_angle_rad,
- 0.0f, _parameters.throttle_max, throttle_land,
- math::radians(-10.0f), math::radians(15.0f));
+ /* kill the throttle if param requests it */
+ throttle_max = _parameters.throttle_max;
- /* kill the throttle if param requests it */
+ if (relative_alt < landingslope.motor_lim_relative_alt() || land_motor_lim) {
throttle_max = math::min(throttle_max, _parameters.throttle_land_max);
+ if (!land_motor_lim) {
+ land_motor_lim = true;
+ mavlink_log_info(_mavlink_fd, "#audio: Landing, limiting throttle");
+ }
- /* 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));
+ }
- } else if (wp_distance < 60.0f && altitude_error > -20.0f) {
+ float flare_curve_alt_rel = landingslope.getFlareCurveRelativeAltitudeSave(wp_distance, bearing_lastwp_currwp, bearing_airplane_currwp);
- /* minimize speed to approach speed */
+ /* avoid climbout */
+ if ((flare_curve_alt_rel_last < flare_curve_alt_rel && land_noreturn_vertical) || land_stayonground)
+ {
+ flare_curve_alt_rel = 0.0f; // stay on ground
+ land_stayonground = true;
+ }
+
+ 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");
+ land_noreturn_vertical = true;
+ }
+ //warnx("Landing: flare, _global_pos.alt %.1f, flare_curve_alt %.1f, flare_curve_alt_last %.1f, flare_length %.1f, wp_distance %.1f", _global_pos.alt, flare_curve_alt, flare_curve_alt_last, flare_length, wp_distance);
- _tecs.update_pitch_throttle(_R_nb, _att.pitch, _global_pos.alt, _global_triplet.current.altitude, calculate_target_airspeed(airspeed_approach),
- _airspeed.indicated_airspeed_m_s, eas2tas,
- false, flare_angle_rad,
- _parameters.throttle_min, _parameters.throttle_max, _parameters.throttle_cruise,
- math::radians(_parameters.pitch_limit_min), math::radians(_parameters.pitch_limit_max));
+ flare_curve_alt_rel_last = flare_curve_alt_rel;
+ } else {
+ /* intersect glide slope:
+ * minimize speed to approach speed
+ * 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 || land_onslope) {
+ /* stay on slope */
+ altitude_desired_rel = landing_slope_alt_rel_desired;
+ if (!land_onslope) {
+ mavlink_log_info(_mavlink_fd, "#audio: Landing, on slope");
+ land_onslope = true;
+ }
} else {
+ /* continue horizontally */
+ altitude_desired_rel = _pos_sp_triplet.previous.valid ? L_altitude_rel : relative_alt;
+ }
- /* normal cruise speed */
+ 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) {
- _tecs.update_pitch_throttle(_R_nb, _att.pitch, _global_pos.alt, _global_triplet.current.altitude, 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));
+ /* Perform launch detection */
+ if(!launch_detected) { //do not do further checks once a launch was detected
+ if (launchDetector.launchDetectionEnabled()) {
+ static hrt_abstime last_sent = 0;
+ if(hrt_absolute_time() - last_sent > 4e6) {
+ mavlink_log_info(_mavlink_fd, "#audio: Launchdetection running");
+ last_sent = hrt_absolute_time();
+ }
+
+ /* Tell the attitude controller to stop integrating while we are waiting
+ * for the launch */
+ _att_sp.roll_reset_integral = true;
+ _att_sp.pitch_reset_integral = true;
+ _att_sp.yaw_reset_integral = true;
+
+ /* Detect launch */
+ launchDetector.update(_sensor_combined.accelerometer_m_s2[0]);
+ if (launchDetector.getLaunchDetected()) {
+ launch_detected = true;
+ mavlink_log_info(_mavlink_fd, "#audio: Takeoff");
+ }
+ } else {
+ /* no takeoff detection --> fly */
+ launch_detected = true;
+ warnx("launchdetection off");
}
+ }
- } else if (global_triplet.current.nav_cmd == NAV_CMD_TAKEOFF) {
+ _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();
- _l1_control.navigate_waypoints(prev_wp, next_wp, current_position, ground_speed);
- _att_sp.roll_body = _l1_control.nav_roll();
- _att_sp.yaw_body = _l1_control.nav_bearing();
+ if (launch_detected) {
+ usePreTakeoffThrust = false;
/* apply minimum pitch and limit roll if target altitude is not within 10 meters */
- if (altitude_error > 10.0f) {
+ 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, _global_triplet.current.altitude, calculate_target_airspeed(_parameters.airspeed_min),
- _airspeed.indicated_airspeed_m_s, eas2tas,
- true, math::max(math::radians(global_triplet.current.param1), 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, _global_triplet.current.altitude, 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);
}
- }
-
- // warnx("nav bearing: %8.4f bearing err: %8.4f target bearing: %8.4f", (double)_l1_control.nav_bearing(),
- // (double)_l1_control.bearing_error(), (double)_l1_control.target_bearing());
- // warnx("prev wp: %8.4f/%8.4f, next wp: %8.4f/%8.4f prev:%s", (double)prev_wp.getX(), (double)prev_wp.getY(),
- // (double)next_wp.getX(), (double)next_wp.getY(), (global_triplet.previous_valid) ? "valid" : "invalid");
-
- // XXX at this point we always want no loiter hold if a
- // mission is active
- _loiter_hold = false;
-
- } else if (_control_mode.flag_armed) {
-
- /* hold position, but only if armed, climb 20m in case this is engaged on ground level */
-
- // XXX rework with smarter state machine
-
- if (!_loiter_hold) {
- _loiter_hold_lat = _global_pos.lat / 1e7f;
- _loiter_hold_lon = _global_pos.lon / 1e7f;
- _loiter_hold_alt = _global_pos.alt + 25.0f;
- _loiter_hold = true;
- }
-
- altitude_error = _loiter_hold_alt - _global_pos.alt;
-
- math::Vector2f loiter_hold_pos(_loiter_hold_lat, _loiter_hold_lon);
-
- /* loiter around current position */
- _l1_control.navigate_loiter(loiter_hold_pos, current_position, _parameters.loiter_hold_radius,
- 1, ground_speed);
- _att_sp.roll_body = _l1_control.nav_roll();
- _att_sp.yaw_body = _l1_control.nav_bearing();
-
- /* climb with full throttle if the altitude error is bigger than 5 meters */
- bool climb_out = (altitude_error > 3);
-
- float min_pitch;
-
- if (climb_out) {
- min_pitch = math::radians(20.0f);
} else {
- min_pitch = math::radians(_parameters.pitch_limit_min);
+ usePreTakeoffThrust = true;
}
+ }
- _tecs.update_pitch_throttle(_R_nb, _att.pitch, _global_pos.alt, _loiter_hold_alt, calculate_target_airspeed(_parameters.airspeed_trim),
- _airspeed.indicated_airspeed_m_s, eas2tas,
- climb_out, min_pitch,
- _parameters.throttle_min, _parameters.throttle_max, _parameters.throttle_cruise,
- math::radians(_parameters.pitch_limit_min), math::radians(_parameters.pitch_limit_max));
+ // warnx("nav bearing: %8.4f bearing err: %8.4f target bearing: %8.4f", (double)_l1_control.nav_bearing(),
+ // (double)_l1_control.bearing_error(), (double)_l1_control.target_bearing());
+ // warnx("prev wp: %8.4f/%8.4f, next wp: %8.4f/%8.4f prev:%s", (double)prev_wp(0), (double)prev_wp(1),
+ // (double)next_wp(0), (double)next_wp(1), (pos_sp_triplet.previous_valid) ? "valid" : "invalid");
- if (climb_out) {
- /* 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));
- }
+ // XXX at this point we always want no loiter hold if a
+ // mission is active
+ _loiter_hold = false;
+
+ /* reset landing state */
+ if (pos_sp_triplet.current.type != SETPOINT_TYPE_LAND) {
+ reset_landing_state();
}
- /* reset land state */
- if (global_triplet.current.nav_cmd != NAV_CMD_LAND) {
- land_noreturn = false;
+ /* reset takeoff/launch state */
+ if (pos_sp_triplet.current.type != SETPOINT_TYPE_TAKEOFF) {
+ reset_takeoff_state();
}
if (was_circle_mode && !_l1_control.circle_mode()) {
@@ -911,97 +1139,119 @@ FixedwingPositionControl::control_position(const math::Vector2f &current_positio
_att_sp.roll_reset_integral = true;
}
- } else if (0/* easy mode enabled */) {
+ } else if (0/* posctrl mode enabled */) {
- /** EASY FLIGHT **/
+ _was_pos_control_mode = false;
- if (0/* switched from another mode to easy */) {
- _seatbelt_hold_heading = _att.yaw;
- }
+ /** POSCTRL FLIGHT **/
- if (0/* easy on and manual control yaw non-zero */) {
- _seatbelt_hold_heading = _att.yaw + _manual.yaw;
- }
+ if (0/* switched from another mode to posctrl */) {
+ _altctrl_hold_heading = _att.yaw;
+ }
- /* climb out control */
- bool climb_out = false;
+ if (0/* posctrl on and manual control yaw non-zero */) {
+ _altctrl_hold_heading = _att.yaw + _manual.r;
+ }
- /* user wants to climb out */
- if (_manual.pitch > 0.3f && _manual.throttle > 0.8f) {
- climb_out = true;
- }
+ //XXX not used
- /* if in seatbelt mode, set airspeed based on manual control */
+ /* climb out control */
+// bool climb_out = false;
+//
+// /* user wants to climb out */
+// if (_manual.pitch > 0.3f && _manual.throttle > 0.8f) {
+// climb_out = true;
+// }
- // XXX check if ground speed undershoot should be applied here
- float seatbelt_airspeed = _parameters.airspeed_min +
- (_parameters.airspeed_max - _parameters.airspeed_min) *
- _manual.throttle;
+ /* if in altctrl mode, set airspeed based on manual control */
- _l1_control.navigate_heading(_seatbelt_hold_heading, _att.yaw, ground_speed);
+ // 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_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.pitch * 2.0f,
- seatbelt_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);
- } else if (0/* seatbelt mode enabled */) {
+ 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;
- /** SEATBELT FLIGHT **/
+ /** ALTCTRL FLIGHT **/
- if (0/* switched from another mode to seatbelt */) {
- _seatbelt_hold_heading = _att.yaw;
+ if (0/* switched from another mode to altctrl */) {
+ _altctrl_hold_heading = _att.yaw;
}
- if (0/* seatbelt on and manual control yaw non-zero */) {
- _seatbelt_hold_heading = _att.yaw + _manual.yaw;
+ if (0/* altctrl on and manual control yaw non-zero */) {
+ _altctrl_hold_heading = _att.yaw + _manual.r;
}
- /* if in seatbelt 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 seatbelt_airspeed = _parameters.airspeed_min +
+ float altctrl_airspeed = _parameters.airspeed_min +
(_parameters.airspeed_max - _parameters.airspeed_min) *
- _manual.throttle;
+ _manual.z;
/* user switched off throttle */
- if (_manual.throttle < 0.1f) {
+ 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 */
bool climb_out = false;
/* user wants to climb out */
- if (_manual.pitch > 0.3f && _manual.throttle > 0.8f) {
+ if (_manual.x > 0.3f && _manual.z > 0.8f) {
climb_out = true;
}
- _l1_control.navigate_heading(_seatbelt_hold_heading, _att.yaw, ground_speed);
- _att_sp.roll_body = _manual.roll;
- _att_sp.yaw_body = _manual.yaw;
- _tecs.update_pitch_throttle(_R_nb, _att.pitch, _global_pos.alt, _global_pos.alt + _manual.pitch * 2.0f,
- seatbelt_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);
+ _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(_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 */
setpoint = false;
+
+ /* reset landing and takeoff state */
+ if (!last_manual) {
+ reset_landing_state();
+ reset_takeoff_state();
+ }
+ }
+
+ if (usePreTakeoffThrust) {
+ _att_sp.thrust = launchDetector.getThrottlePreTakeoff();
+ }
+ else {
+ _att_sp.thrust = math::min(_mTecs.getThrottleSetpoint(), throttle_max);
+ }
+ _att_sp.pitch_body = _mTecs.getPitchSetpoint();
+
+ if (_control_mode.flag_control_position_enabled) {
+ last_manual = false;
+ } else {
+ last_manual = true;
}
- _att_sp.pitch_body = _tecs.get_pitch_demand();
- _att_sp.thrust = math::min(_tecs.get_throttle_demand(), throttle_max);
return setpoint;
}
@@ -1018,13 +1268,14 @@ FixedwingPositionControl::task_main()
* do subscriptions
*/
_global_pos_sub = orb_subscribe(ORB_ID(vehicle_global_position));
- _global_set_triplet_sub = orb_subscribe(ORB_ID(vehicle_global_position_set_triplet));
+ _pos_sp_triplet_sub = orb_subscribe(ORB_ID(position_setpoint_triplet));
_att_sub = orb_subscribe(ORB_ID(vehicle_attitude));
- _accel_sub = orb_subscribe(ORB_ID(sensor_accel));
+ _sensor_combined_sub = orb_subscribe(ORB_ID(sensor_combined));
_control_mode_sub = orb_subscribe(ORB_ID(vehicle_control_mode));
_airspeed_sub = orb_subscribe(ORB_ID(airspeed));
_params_sub = orb_subscribe(ORB_ID(parameter_update));
_manual_control_sub = orb_subscribe(ORB_ID(manual_control_setpoint));
+ _range_finder_sub = orb_subscribe(ORB_ID(sensor_range_finder));
/* rate limit vehicle status updates to 5Hz */
orb_set_interval(_control_mode_sub, 200);
@@ -1047,6 +1298,8 @@ FixedwingPositionControl::task_main()
fds[1].fd = _global_pos_sub;
fds[1].events = POLLIN;
+ _task_running = true;
+
while (!_task_should_exit) {
/* wait for up to 500ms for data */
@@ -1080,6 +1333,11 @@ FixedwingPositionControl::task_main()
/* only run controller if position changed */
if (fds[1].revents & POLLIN) {
+ /* XXX Hack to get mavlink output going */
+ if (_mavlink_fd < 0) {
+ /* try to open the mavlink log device every once in a while */
+ _mavlink_fd = open(MAVLINK_LOG_DEVICE, 0);
+ }
static uint64_t last_run = 0;
float deltaT = (hrt_absolute_time() - last_run) / 1000000.0f;
@@ -1097,18 +1355,19 @@ FixedwingPositionControl::task_main()
vehicle_attitude_poll();
vehicle_setpoint_poll();
- vehicle_accel_poll();
+ vehicle_sensor_combined_poll();
vehicle_airspeed_poll();
+ range_finder_poll();
// vehicle_baro_poll();
- math::Vector2f ground_speed(_global_pos.vx, _global_pos.vy);
- math::Vector2f current_position(_global_pos.lat / 1e7f, _global_pos.lon / 1e7f);
+ 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);
/*
* Attempt to control position, on success (= sensors present and not in manual mode),
* publish setpoint.
*/
- if (control_position(current_position, ground_speed, _global_triplet)) {
+ if (control_position(current_position, ground_speed, _pos_sp_triplet)) {
_att_sp.timestamp = hrt_absolute_time();
/* lazily publish the setpoint only once available */
@@ -1121,19 +1380,17 @@ FixedwingPositionControl::task_main()
_attitude_sp_pub = orb_advertise(ORB_ID(vehicle_attitude_setpoint), &_att_sp);
}
- float turn_distance = _l1_control.switch_distance(_global_triplet.current.turn_distance_xy);
+ /* XXX check if radius makes sense here */
+ float turn_distance = _l1_control.switch_distance(100.0f);
/* lazily publish navigation capabilities */
- if (turn_distance != _nav_capabilities.turn_distance && turn_distance > 0) {
+ if (fabsf(turn_distance - _nav_capabilities.turn_distance) > FLT_EPSILON && turn_distance > 0) {
/* set new turn distance */
_nav_capabilities.turn_distance = turn_distance;
- if (_nav_capabilities_pub > 0) {
- orb_publish(ORB_ID(navigation_capabilities), _nav_capabilities_pub, &_nav_capabilities);
- } else {
- _nav_capabilities_pub = orb_advertise(ORB_ID(navigation_capabilities), &_nav_capabilities);
- }
+ navigation_capabilities_publish();
+
}
}
@@ -1143,12 +1400,54 @@ FixedwingPositionControl::task_main()
perf_end(_loop_perf);
}
+ _task_running = false;
+
warnx("exiting.\n");
_control_task = -1;
_exit(0);
}
+void FixedwingPositionControl::reset_takeoff_state()
+{
+ launch_detected = false;
+ usePreTakeoffThrust = false;
+ launchDetector.reset();
+}
+
+void FixedwingPositionControl::reset_landing_state()
+{
+ land_noreturn_horizontal = false;
+ land_noreturn_vertical = false;
+ land_stayonground = false;
+ land_motor_lim = false;
+ 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()
{
@@ -1158,7 +1457,7 @@ FixedwingPositionControl::start()
_control_task = task_spawn_cmd("fw_pos_control_l1",
SCHED_DEFAULT,
SCHED_PRIORITY_MAX - 5,
- 4048,
+ 2000,
(main_t)&FixedwingPositionControl::task_main_trampoline,
nullptr);
@@ -1191,6 +1490,14 @@ int fw_pos_control_l1_main(int argc, char *argv[])
err(1, "start failed");
}
+ /* avoid memory fragmentation by not exiting start handler until the task has fully started */
+ while (l1_control::g_control == nullptr || !l1_control::g_control->task_running()) {
+ usleep(50000);
+ printf(".");
+ fflush(stdout);
+ }
+ printf("\n");
+
exit(0);
}
generated by cgit v1.2.3 (git 2.39.1) at 2024-06-12 04:33:57 +0000