aboutsummaryrefslogtreecommitdiff
path: root/src/modules/fw_att_control/fw_att_control_main.cpp
diff options
context:
space:
mode:
Diffstat (limited to 'src/modules/fw_att_control/fw_att_control_main.cpp')
-rw-r--r--src/modules/fw_att_control/fw_att_control_main.cpp69
1 files changed, 58 insertions, 11 deletions
diff --git a/src/modules/fw_att_control/fw_att_control_main.cpp b/src/modules/fw_att_control/fw_att_control_main.cpp
index 0cea13cc4..e770c11a2 100644
--- a/src/modules/fw_att_control/fw_att_control_main.cpp
+++ b/src/modules/fw_att_control/fw_att_control_main.cpp
@@ -63,6 +63,7 @@
#include <uORB/topics/vehicle_control_mode.h>
#include <uORB/topics/parameter_update.h>
#include <uORB/topics/vehicle_global_position.h>
+#include <uORB/topics/vehicle_status.h>
#include <systemlib/param/param.h>
#include <systemlib/err.h>
#include <systemlib/pid/pid.h>
@@ -124,6 +125,7 @@ private:
int _params_sub; /**< notification of parameter updates */
int _manual_sub; /**< notification of manual control updates */
int _global_pos_sub; /**< global position subscription */
+ int _vehicle_status_sub; /**< vehicle status subscription */
orb_advert_t _rate_sp_pub; /**< rate setpoint publication */
orb_advert_t _attitude_sp_pub; /**< attitude setpoint point */
@@ -139,12 +141,14 @@ private:
struct actuator_controls_s _actuators; /**< actuator control inputs */
struct actuator_controls_s _actuators_airframe; /**< actuator control inputs */
struct vehicle_global_position_s _global_pos; /**< global position */
+ struct vehicle_status_s _vehicle_status; /**< vehicle status */
perf_counter_t _loop_perf; /**< loop performance counter */
perf_counter_t _nonfinite_input_perf; /**< performance counter for non finite input */
perf_counter_t _nonfinite_output_perf; /**< performance counter for non finite output */
bool _setpoint_valid; /**< flag if the position control setpoint is valid */
+ bool _debug; /**< if set to true, print debug output */
struct {
float tconst;
@@ -275,6 +279,11 @@ private:
void global_pos_poll();
/**
+ * Check for vehicle status updates.
+ */
+ void vehicle_status_poll();
+
+ /**
* Shim for calling task_main from task_create.
*/
static void task_main_trampoline(int argc, char *argv[]);
@@ -312,6 +321,7 @@ FixedwingAttitudeControl::FixedwingAttitudeControl() :
_params_sub(-1),
_manual_sub(-1),
_global_pos_sub(-1),
+ _vehicle_status_sub(-1),
/* publications */
_rate_sp_pub(-1),
@@ -324,7 +334,8 @@ FixedwingAttitudeControl::FixedwingAttitudeControl() :
_nonfinite_input_perf(perf_alloc(PC_COUNT, "fw att control nonfinite input")),
_nonfinite_output_perf(perf_alloc(PC_COUNT, "fw att control nonfinite output")),
/* states */
- _setpoint_valid(false)
+ _setpoint_valid(false),
+ _debug(false)
{
/* safely initialize structs */
_att = {};
@@ -336,6 +347,7 @@ FixedwingAttitudeControl::FixedwingAttitudeControl() :
_actuators = {};
_actuators_airframe = {};
_global_pos = {};
+ _vehicle_status = {};
_parameter_handles.tconst = param_find("FW_ATT_TC");
@@ -559,6 +571,18 @@ FixedwingAttitudeControl::global_pos_poll()
}
void
+FixedwingAttitudeControl::vehicle_status_poll()
+{
+ /* check if there is new status information */
+ bool vehicle_status_updated;
+ orb_check(_vehicle_status_sub, &vehicle_status_updated);
+
+ if (vehicle_status_updated) {
+ orb_copy(ORB_ID(vehicle_status), _vehicle_status_sub, &_vehicle_status);
+ }
+}
+
+void
FixedwingAttitudeControl::task_main_trampoline(int argc, char *argv[])
{
att_control::g_control->task_main();
@@ -583,6 +607,7 @@ FixedwingAttitudeControl::task_main()
_params_sub = orb_subscribe(ORB_ID(parameter_update));
_manual_sub = orb_subscribe(ORB_ID(manual_control_setpoint));
_global_pos_sub = orb_subscribe(ORB_ID(vehicle_global_position));
+ _vehicle_status_sub = orb_subscribe(ORB_ID(vehicle_status));
/* rate limit vehicle status updates to 5Hz */
orb_set_interval(_vcontrol_mode_sub, 200);
@@ -597,6 +622,7 @@ FixedwingAttitudeControl::task_main()
vehicle_accel_poll();
vehicle_control_mode_poll();
vehicle_manual_poll();
+ vehicle_status_poll();
/* wakeup source(s) */
struct pollfd fds[2];
@@ -665,6 +691,8 @@ FixedwingAttitudeControl::task_main()
global_pos_poll();
+ vehicle_status_poll();
+
/* lock integrator until control is started */
bool lock_integrator;
@@ -700,7 +728,8 @@ FixedwingAttitudeControl::task_main()
perf_count(_nonfinite_input_perf);
}
} else {
- airspeed = _airspeed.true_airspeed_m_s;
+ /* prevent numerical drama by requiring 0.5 m/s minimal speed */
+ airspeed = math::max(0.5f, _airspeed.true_airspeed_m_s);
}
/*
@@ -717,7 +746,14 @@ FixedwingAttitudeControl::task_main()
float pitch_sp = _parameters.pitchsp_offset_rad;
float throttle_sp = 0.0f;
- if (_vcontrol_mode.flag_control_velocity_enabled || _vcontrol_mode.flag_control_position_enabled) {
+ /* Read attitude setpoint from uorb if
+ * - velocity control or position control is enabled (pos controller is running)
+ * - manual control is disabled (another app may send the setpoint, but it should
+ * for sure not be set from the remote control values)
+ */
+ if (_vcontrol_mode.flag_control_velocity_enabled ||
+ _vcontrol_mode.flag_control_position_enabled ||
+ !_vcontrol_mode.flag_control_manual_enabled) {
/* read in attitude setpoint from attitude setpoint uorb topic */
roll_sp = _att_sp.roll_body + _parameters.rollsp_offset_rad;
pitch_sp = _att_sp.pitch_body + _parameters.pitchsp_offset_rad;
@@ -776,6 +812,13 @@ FixedwingAttitudeControl::task_main()
}
}
+ /* If the aircraft is on ground reset the integrators */
+ if (_vehicle_status.condition_landed) {
+ _roll_ctrl.reset_integrator();
+ _pitch_ctrl.reset_integrator();
+ _yaw_ctrl.reset_integrator();
+ }
+
/* Prepare speed_body_u and speed_body_w */
float speed_body_u = 0.0f;
float speed_body_v = 0.0f;
@@ -785,7 +828,7 @@ FixedwingAttitudeControl::task_main()
speed_body_v = _att.R[0][1] * _global_pos.vel_n + _att.R[1][1] * _global_pos.vel_e + _att.R[2][1] * _global_pos.vel_d;
speed_body_w = _att.R[0][2] * _global_pos.vel_n + _att.R[1][2] * _global_pos.vel_e + _att.R[2][2] * _global_pos.vel_d;
} else {
- if (loop_counter % 10 == 0) {
+ if (_debug && loop_counter % 10 == 0) {
warnx("Did not get a valid R\n");
}
}
@@ -808,7 +851,7 @@ FixedwingAttitudeControl::task_main()
_roll_ctrl.reset_integrator();
perf_count(_nonfinite_output_perf);
- if (loop_counter % 10 == 0) {
+ if (_debug && loop_counter % 10 == 0) {
warnx("roll_u %.4f", (double)roll_u);
}
}
@@ -821,7 +864,7 @@ FixedwingAttitudeControl::task_main()
if (!isfinite(pitch_u)) {
_pitch_ctrl.reset_integrator();
perf_count(_nonfinite_output_perf);
- if (loop_counter % 10 == 0) {
+ if (_debug && loop_counter % 10 == 0) {
warnx("pitch_u %.4f, _yaw_ctrl.get_desired_rate() %.4f,"
" airspeed %.4f, airspeed_scaling %.4f,"
" roll_sp %.4f, pitch_sp %.4f,"
@@ -845,21 +888,25 @@ FixedwingAttitudeControl::task_main()
if (!isfinite(yaw_u)) {
_yaw_ctrl.reset_integrator();
perf_count(_nonfinite_output_perf);
- if (loop_counter % 10 == 0) {
+ if (_debug && loop_counter % 10 == 0) {
warnx("yaw_u %.4f", (double)yaw_u);
}
}
- /* throttle passed through */
- _actuators.control[3] = (isfinite(throttle_sp)) ? throttle_sp : 0.0f;
+ /* throttle passed through if it is finite and if no engine failure was
+ * detected */
+ _actuators.control[3] = (isfinite(throttle_sp) &&
+ !(_vehicle_status.engine_failure ||
+ _vehicle_status.engine_failure_cmd)) ?
+ throttle_sp : 0.0f;
if (!isfinite(throttle_sp)) {
- if (loop_counter % 10 == 0) {
+ if (_debug && loop_counter % 10 == 0) {
warnx("throttle_sp %.4f", (double)throttle_sp);
}
}
} else {
perf_count(_nonfinite_input_perf);
- if (loop_counter % 10 == 0) {
+ if (_debug && loop_counter % 10 == 0) {
warnx("Non-finite setpoint roll_sp: %.4f, pitch_sp %.4f", (double)roll_sp, (double)pitch_sp);
}
}
generated by cgit v1.2.3 (git 2.39.1) at 2024-07-19 03:04:11 +0000