From d3bd251ffcd7d2fc4316af128fcaadada465caa6 Mon Sep 17 00:00:00 2001 From: Matthias Grob Date: Wed, 16 Oct 2019 16:28:05 +0200 Subject: [PATCH] mc_pos_control: start flight task already when disarmed such that the controller always publishes attitude setpoints and there are no initialization issues. --- .../mc_pos_control/mc_pos_control_main.cpp | 19 ++++++------------- 1 file changed, 6 insertions(+), 13 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 f8b23c8d82..bdeb594612 100644 --- a/src/modules/mc_pos_control/mc_pos_control_main.cpp +++ b/src/modules/mc_pos_control/mc_pos_control_main.cpp @@ -568,15 +568,8 @@ MulticopterPositionControl::Run() _takeoff.updateTakeoffState(_control_mode.flag_armed, _vehicle_land_detected.landed, false, 10.f, !_control_mode.flag_control_climb_rate_enabled, time_stamp_now); - // takeoff delay for motors to reach idle speed - if (_takeoff.getTakeoffState() >= TakeoffState::ready_for_takeoff) { - // when vehicle is ready switch to the required flighttask - start_flight_task(); - - } else { - // stop flighttask while disarmed - _flight_tasks.switchTask(FlightTaskIndex::None); - } + // switch to the required flighttask + start_flight_task(); // check if any task is active if (_flight_tasks.isAnyTaskActive()) { @@ -705,10 +698,10 @@ MulticopterPositionControl::Run() } // publish attitude setpoint - // Note: this requires review. The reason for not sending - // 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. + // It's important to publish also when disarmed otheriwse the attitude setpoint stays uninitialized. + // Not publishing when not running a flight task + // in stabilized mode attitude setpoints get ignored + // in offboard with attitude setpoints they come from MAVLink directly if (_attitude_setpoint_id != nullptr) { orb_publish_auto(_attitude_setpoint_id, &_vehicle_attitude_setpoint_pub, &attitude_setpoint, nullptr, ORB_PRIO_DEFAULT); }