From 9c2ec3c2299acd48b1223df07d3366a4fc5c324a Mon Sep 17 00:00:00 2001
From: Dennis Mannhart <dennis@px4.io>
Date: Mon, 30 Apr 2018 17:56:42 +0200
Subject: [PATCH] PositionControl: add control states structure

---
 .../mc_pos_control/PositionControl.cpp        | 22 ++++++++++++++-----
 .../mc_pos_control/PositionControl.hpp        | 12 +++++++---
 2 files changed, 25 insertions(+), 9 deletions(-)

diff --git a/src/modules/mc_pos_control/PositionControl.cpp b/src/modules/mc_pos_control/PositionControl.cpp
index 251b74ca8f..bfa6cfa54e 100644
--- a/src/modules/mc_pos_control/PositionControl.cpp
+++ b/src/modules/mc_pos_control/PositionControl.cpp
@@ -46,12 +46,12 @@ PositionControl::PositionControl(ModuleParams *parent) :
 	ModuleParams(parent)
 {}
 
-void PositionControl::updateState(const vehicle_local_position_s &state, const Vector3f &vel_dot)
+void PositionControl::updateState(const PositionControlStates &states)
 {
-	_pos = Vector3f(&state.x);
-	_vel = Vector3f(&state.vx);
-	_yaw = state.yaw;
-	_vel_dot = vel_dot;
+	_pos = states.position;
+	_vel = states.velocity;
+	_yaw = states.yaw;
+	_vel_dot = states.acceleration;
 }
 
 void PositionControl::updateSetpoint(const vehicle_local_position_setpoint_s &setpoint)
@@ -119,6 +119,11 @@ void PositionControl::_interfaceMapping()
 			// thrust setpoint is not supported in position control
 			_thr_sp(i) = 0.0f;
 
+			// to run position control, we require valid position and velocity
+			if (!PX4_ISFINITE(_pos(i)) || !PX4_ISFINITE(_vel(i))) {
+				failsafe = true;
+			}
+
 		} else if (PX4_ISFINITE(_vel_sp(i))) {
 
 			// Velocity controller is active without position control.
@@ -128,6 +133,11 @@ void PositionControl::_interfaceMapping()
 			// thrust setpoint is not supported in position control
 			_thr_sp(i) = 0.0f;
 
+			// to run velocity control, we require valid velocity
+			if (!PX4_ISFINITE(_vel(i))) {
+				failsafe = true;
+			}
+
 		} else if (PX4_ISFINITE(_thr_sp(i))) {
 
 			// Thrust setpoint was generated from sticks directly.
@@ -142,7 +152,6 @@ void PositionControl::_interfaceMapping()
 		} else {
 			// nothing is valid. do failsafe
 			failsafe = true;
-			PX4_WARN("TODO: add safety");
 		}
 	}
 
@@ -160,6 +169,7 @@ void PositionControl::_interfaceMapping()
 
 	// check failsafe
 	if (failsafe) {
+		_skip_controller = true;
 		// point the thrust upwards
 		_thr_sp(0) = _thr_sp(1) = 0.0f;
 		// throttle down such that vehicle goes down with
diff --git a/src/modules/mc_pos_control/PositionControl.hpp b/src/modules/mc_pos_control/PositionControl.hpp
index f07498bfaf..3db844920b 100644
--- a/src/modules/mc_pos_control/PositionControl.hpp
+++ b/src/modules/mc_pos_control/PositionControl.hpp
@@ -45,6 +45,13 @@
 #include <px4_module_params.h>
 #pragma once
 
+struct PositionControlStates {
+	matrix::Vector3f position;
+	matrix::Vector3f velocity;
+	matrix::Vector3f acceleration;
+	float yaw;
+};
+
 /**
  * 	Core Position-Control for MC.
  * 	This class contains P-controller for position and
@@ -80,10 +87,9 @@ public:
 
 	/**
 	 * Update the current vehicle state.
-	 * @param state a vehicle_local_position_s structure
-	 * @param vel_dot the derivative of the vehicle velocity
+	 * @param PositionControlStates structure
 	 */
-	void updateState(const vehicle_local_position_s &state, const matrix::Vector3f &vel_dot);
+	void updateState(const PositionControlStates &states);
 
 	/**
 	 * Update the desired setpoints.