From b5731e0ccd8e1883dcdafada20c8f76c8ae4171d Mon Sep 17 00:00:00 2001
From: Dennis Mannhart <dennis.mannhart@gmail.com>
Date: Mon, 25 Jun 2018 11:33:30 +0200
Subject: [PATCH] FlightTaskOffboard: only start task if control mode flags are
met
---
src/lib/FlightTasks/tasks/FlightTaskOffboard.cpp | 3 ++-
src/modules/mc_pos_control/mc_pos_control_main.cpp | 12 +++++++++---
2 files changed, 11 insertions(+), 4 deletions(-)
diff --git a/src/lib/FlightTasks/tasks/FlightTaskOffboard.cpp b/src/lib/FlightTasks/tasks/FlightTaskOffboard.cpp
index a6a982d775..2952da8931 100644
--- a/src/lib/FlightTasks/tasks/FlightTaskOffboard.cpp
+++ b/src/lib/FlightTasks/tasks/FlightTaskOffboard.cpp
@@ -55,6 +55,8 @@ bool FlightTaskOffboard::initializeSubscriptions(SubscriptionArray &subscription
bool FlightTaskOffboard::updateInitialize()
{
bool ret = FlightTask::updateInitialize();
+ // require a valid triplet
+ ret = ret && _sub_triplet_setpoint->get().current.valid;
// require valid position / velocity in xy
return ret && PX4_ISFINITE(_position(0))
&& PX4_ISFINITE(_position(1))
@@ -245,5 +247,4 @@ bool FlightTaskOffboard::update()
}
return true;
-
}
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 4b3c081322..1b82ddb55e 100644
--- a/src/modules/mc_pos_control/mc_pos_control_main.cpp
+++ b/src/modules/mc_pos_control/mc_pos_control_main.cpp
@@ -519,7 +519,7 @@ MulticopterPositionControl::task_main()
setDt(dt);
if (_control_mode.flag_armed) {
- // as soon vehicle is armed, start flighttask
+ // as soon vehicle is armed check for flighttask
start_flight_task();
// arm hysteresis prevents vehicle to takeoff
// before propeller reached idle speed.
@@ -646,7 +646,7 @@ MulticopterPositionControl::task_main()
// publish attitude setpoint
// Note: this requires review. The reason for not sending
- // an attitude setpoint is because for none-flighttask modes
+ // an attitude setpoint is because for non-flighttask modes
// the attitude septoint should come from another source, otherwise
// they might conflict with each other such as in offboard attitude control.
publish_attitude();
@@ -678,7 +678,13 @@ MulticopterPositionControl::start_flight_task()
bool task_failure = false;
// offboard
- if (_vehicle_status.nav_state == vehicle_status_s::NAVIGATION_STATE_OFFBOARD) {
+ if (_vehicle_status.nav_state == vehicle_status_s::NAVIGATION_STATE_OFFBOARD
+ && (_control_mode.flag_control_altitude_enabled ||
+ _control_mode.flag_control_position_enabled ||
+ _control_mode.flag_control_climb_rate_enabled ||
+ _control_mode.flag_control_velocity_enabled ||
+ _control_mode.flag_control_acceleration_enabled)) {
+
int error = _flight_tasks.switchTask(FlightTaskIndex::Offboard);
if (error != 0) {
--
GitLab