From 38cf89ee9ceb2d9bd380657465f3ea5b6925c556 Mon Sep 17 00:00:00 2001 From: Matthias Grob Date: Tue, 27 Nov 2018 21:09:11 +0100 Subject: [PATCH] mc_pos_control: also use vertical smoothing in altitude Smoothing can be configured by MPC_POS_MODE parameter put before only for position mode and not for altitude mode. --- .../mc_pos_control/mc_pos_control_main.cpp | 21 +++++++++++++------ 1 file changed, 15 insertions(+), 6 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 94e1380cc3..90f964c7d4 100644 --- a/src/modules/mc_pos_control/mc_pos_control_main.cpp +++ b/src/modules/mc_pos_control/mc_pos_control_main.cpp @@ -949,15 +949,10 @@ 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()) { - case 0: - error = _flight_tasks.switchTask(FlightTaskIndex::ManualPosition); - break; - case 1: error = _flight_tasks.switchTask(FlightTaskIndex::ManualPositionSmooth); break; @@ -970,6 +965,7 @@ MulticopterPositionControl::start_flight_task() error = _flight_tasks.switchTask(FlightTaskIndex::ManualPositionSmoothVel); break; + case 0: default: error = _flight_tasks.switchTask(FlightTaskIndex::ManualPosition); break; @@ -991,7 +987,20 @@ 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); + int error = 0; + + switch (MPC_POS_MODE.get()) { + case 1: + error = _flight_tasks.switchTask(FlightTaskIndex::ManualAltitudeSmooth); + break; + + case 0: + case 2: + case 3: + default: + error = _flight_tasks.switchTask(FlightTaskIndex::ManualAltitude); + break; + } if (error != 0) { if (prev_failure_count == 0) {