diff --git a/src/lib/FlightTasks/tasks/FlightTaskOffboard.cpp b/src/lib/FlightTasks/tasks/FlightTaskOffboard.cpp index a6a982d775798a0cd1afabf5c06976672973aa2c..2952da89312bfb4dba77105ea01e99f4e711165d 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 4b3c081322cfa8239d9968d51856b68e8dc1a2ba..1b82ddb55eac23c026cafc989075b452002a5afa 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) {