From 2f833a26d14b0364ccbd0cd4589123971d142adc Mon Sep 17 00:00:00 2001 From: Dennis Mannhart Date: Wed, 12 Sep 2018 10:18:41 +0200 Subject: [PATCH] mc_pos_control: check if position-control setpoint update succeeded. refactor use_obstacle_avoidance method --- .../mc_pos_control/PositionControl.cpp | 10 +++++++--- .../mc_pos_control/PositionControl.hpp | 10 ++++++++-- .../mc_pos_control/mc_pos_control_main.cpp | 19 +++++++++---------- 3 files changed, 24 insertions(+), 15 deletions(-) diff --git a/src/modules/mc_pos_control/PositionControl.cpp b/src/modules/mc_pos_control/PositionControl.cpp index f5d6f81c71..d46d94b5aa 100644 --- a/src/modules/mc_pos_control/PositionControl.cpp +++ b/src/modules/mc_pos_control/PositionControl.cpp @@ -54,7 +54,7 @@ void PositionControl::updateState(const PositionControlStates &states) _vel_dot = states.acceleration; } -void PositionControl::updateSetpoint(const vehicle_local_position_setpoint_s &setpoint) +bool PositionControl::updateSetpoint(const vehicle_local_position_setpoint_s &setpoint) { // If full manual is required (thrust already generated), don't run position/velocity // controller and just return thrust. @@ -66,11 +66,13 @@ void PositionControl::updateSetpoint(const vehicle_local_position_setpoint_s &se _thr_sp = Vector3f(setpoint.thrust); _yaw_sp = setpoint.yaw; _yawspeed_sp = setpoint.yawspeed; - _interfaceMapping(); + bool mapping_succeeded = _interfaceMapping(); if (PX4_ISFINITE(setpoint.thrust[0]) && PX4_ISFINITE(setpoint.thrust[1]) && PX4_ISFINITE(setpoint.thrust[2])) { _skip_controller = true; } + + return mapping_succeeded; } void PositionControl::generateThrustYawSetpoint(const float dt) @@ -98,7 +100,7 @@ void PositionControl::generateThrustYawSetpoint(const float dt) } } -void PositionControl::_interfaceMapping() +bool PositionControl::_interfaceMapping() { // if noting is valid, then apply failsafe landing bool failsafe = false; @@ -209,6 +211,8 @@ void PositionControl::_interfaceMapping() // 70% of throttle range between min and hover _thr_sp(2) = -(MPC_THR_MIN.get() + (MPC_THR_HOVER.get() - MPC_THR_MIN.get()) * 0.7f); } + + return !(failsafe); } void PositionControl::_positionController() diff --git a/src/modules/mc_pos_control/PositionControl.hpp b/src/modules/mc_pos_control/PositionControl.hpp index cd92d1f246..5dfa1825a1 100644 --- a/src/modules/mc_pos_control/PositionControl.hpp +++ b/src/modules/mc_pos_control/PositionControl.hpp @@ -94,8 +94,9 @@ public: /** * Update the desired setpoints. * @param setpoint a vehicle_local_position_setpoint_s structure + * @return true if setpoint has updated correctly */ - void updateSetpoint(const vehicle_local_position_setpoint_s &setpoint); + bool updateSetpoint(const vehicle_local_position_setpoint_s &setpoint); /** * Set constraints that are stricter than the global limits. @@ -165,7 +166,12 @@ protected: void updateParams() override; private: - void _interfaceMapping(); /** maps set-points to internal member set-points */ + /** + * Maps setpoints to internal-setpoints. + * @return true if mapping succeeded. + */ + bool _interfaceMapping(); + void _positionController(); /** applies the P-position-controller */ void _velocityController(const float &dt); /** applies the PID-velocity-controller */ 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 cb29e75f3a..2bb1ecb0ec 100644 --- a/src/modules/mc_pos_control/mc_pos_control_main.cpp +++ b/src/modules/mc_pos_control/mc_pos_control_main.cpp @@ -267,7 +267,7 @@ private: /** * Overwrite setpoint with waypoint coming from obstacle avoidance */ - void execute_avoidance_waypoint(); + void execute_avoidance_waypoint(vehicle_local_position_setpoint_s &setpoint); /** * Publish desired vehicle_trajectory_waypoint @@ -703,11 +703,14 @@ MulticopterPositionControl::run() _control.updateConstraints(constraints); _control.updateState(_states); - if (!use_obstacle_avoidance()) { - _control.updateSetpoint(setpoint); + // adjust setpoints based on avoidance + if (use_obstacle_avoidance()) { + execute_avoidance_waypoint(setpoint); + } - } else { - execute_avoidance_waypoint(); + // update position controller setpoints + if (!_control.updateSetpoint(setpoint)) { + warn_rate_limited("Position-Control Setpoint-Update failed"); } // Generate desired thrust and yaw. @@ -1076,10 +1079,8 @@ MulticopterPositionControl::update_avoidance_waypoint_desired(PositionControlSta } void -MulticopterPositionControl::execute_avoidance_waypoint() +MulticopterPositionControl::execute_avoidance_waypoint(vehicle_local_position_setpoint_s &setpoint) { - vehicle_local_position_setpoint_s setpoint; - setpoint.x = _traj_wp_avoidance.waypoints[vehicle_trajectory_waypoint_s::POINT_0].position[0]; setpoint.y = _traj_wp_avoidance.waypoints[vehicle_trajectory_waypoint_s::POINT_0].position[1]; setpoint.z = _traj_wp_avoidance.waypoints[vehicle_trajectory_waypoint_s::POINT_0].position[2]; @@ -1093,8 +1094,6 @@ MulticopterPositionControl::execute_avoidance_waypoint() setpoint.yaw = _traj_wp_avoidance.waypoints[vehicle_trajectory_waypoint_s::POINT_0].yaw; setpoint.yawspeed = _traj_wp_avoidance.waypoints[vehicle_trajectory_waypoint_s::POINT_0].yaw_speed; Vector3f(NAN, NAN, NAN).copyTo(setpoint.thrust); - - _control.updateSetpoint(setpoint); } bool