From e7e06dfe38322e1fe9b7062293b8160e133b8a88 Mon Sep 17 00:00:00 2001
From: =?UTF-8?q?Beat=20K=C3=BCng?= <beat-kueng@gmx.net>
Date: Sun, 28 Oct 2018 16:48:05 +0100
Subject: [PATCH] fix mc_pos_control: disable flight tasks if none of them
should be running
Previously when switching e.g. from stabilized from acro, the stabilized
flight task kept running and publishing setpoints.
Luckily it caused no problems, but the log showed arbitrary attitude
setpoints.
---
src/modules/mc_pos_control/mc_pos_control_main.cpp | 14 ++++++++++++--
1 file changed, 12 insertions(+), 2 deletions(-)
diff --git a/src/modules/mc_pos_control/mc_pos_control_main.cpp b/src/modules/mc_pos_control/mc_pos_control_main.cpp
index 948562d82f..ba80c87451 100644
--- a/src/modules/mc_pos_control/mc_pos_control_main.cpp
+++ b/src/modules/mc_pos_control/mc_pos_control_main.cpp
@@ -775,7 +775,7 @@ MulticopterPositionControl::run()
local_pos_sp.vz = _control.getVelSp()(2);
thr_sp.copyTo(local_pos_sp.thrust);
- // Publish local position setpoint (for logging only) and attitude setpoint (for attitude controller).
+ // Publish local position setpoint (for logging only)
publish_local_pos_sp(local_pos_sp);
_flight_tasks.updateVelocityControllerIO(_control.getVelSp(), thr_sp); // Inform FlightTask about the input and output of the velocity controller
@@ -831,6 +831,7 @@ void
MulticopterPositionControl::start_flight_task()
{
bool task_failure = false;
+ bool should_disable_task = true;
int prev_failure_count = _task_failure_count;
if (!_vehicle_status.is_rotary_wing) {
@@ -839,6 +840,7 @@ MulticopterPositionControl::start_flight_task()
}
if (_vehicle_status.in_transition_mode) {
+ should_disable_task = false;
int error = _flight_tasks.switchTask(FlightTaskIndex::Transition);
if (error != 0) {
@@ -862,6 +864,7 @@ MulticopterPositionControl::start_flight_task()
_control_mode.flag_control_velocity_enabled ||
_control_mode.flag_control_acceleration_enabled)) {
+ should_disable_task = false;
int error = _flight_tasks.switchTask(FlightTaskIndex::Offboard);
if (error != 0) {
@@ -879,6 +882,7 @@ MulticopterPositionControl::start_flight_task()
// Auto-follow me
if (_vehicle_status.nav_state == vehicle_status_s::NAVIGATION_STATE_AUTO_FOLLOW_TARGET) {
+ should_disable_task = false;
int error = _flight_tasks.switchTask(FlightTaskIndex::AutoFollowMe);
if (error != 0) {
@@ -894,7 +898,8 @@ MulticopterPositionControl::start_flight_task()
}
} else if (_control_mode.flag_control_auto_enabled) {
- // Auto relate maneuvers
+ // Auto related maneuvers
+ should_disable_task = false;
int error = 0;
switch (MPC_AUTO_MODE.get()) {
case 0:
@@ -928,6 +933,7 @@ MulticopterPositionControl::start_flight_task()
// manual position control
if (_vehicle_status.nav_state == vehicle_status_s::NAVIGATION_STATE_POSCTL || task_failure) {
+ should_disable_task = false;
int error = 0;
switch (MPC_POS_MODE.get()) {
@@ -967,6 +973,7 @@ MulticopterPositionControl::start_flight_task()
// manual altitude control
if (_vehicle_status.nav_state == vehicle_status_s::NAVIGATION_STATE_ALTCTL || task_failure) {
+ should_disable_task = false;
int error = _flight_tasks.switchTask(FlightTaskIndex::ManualAltitude);
if (error != 0) {
@@ -985,6 +992,7 @@ MulticopterPositionControl::start_flight_task()
// manual stabilized control
if (_vehicle_status.nav_state == vehicle_status_s::NAVIGATION_STATE_MANUAL
|| _vehicle_status.nav_state == vehicle_status_s::NAVIGATION_STATE_STAB || task_failure) {
+ should_disable_task = false;
int error = _flight_tasks.switchTask(FlightTaskIndex::ManualStabilized);
if (error != 0) {
@@ -1011,6 +1019,8 @@ MulticopterPositionControl::start_flight_task()
// No task was activated.
_flight_tasks.switchTask(FlightTaskIndex::None);
}
+ } else if (should_disable_task) {
+ _flight_tasks.switchTask(FlightTaskIndex::None);
}
}
--
GitLab