Tilt-Hex Pos-Ctrl: (WIP) Started code for attitude setpoint generation for omni vehicles

src/modules/mc_pos_control/PositionControl/ControlMath.cpp

@@ -44,13 +44,19 @@ using namespace matrix;
 
 namespace ControlMath
 {
-void thrustToAttitude(const Vector3f &thr_sp, const float yaw_sp, vehicle_attitude_setpoint_s &att_sp)
+void thrustToAttitude(const Vector3f &thr_sp, const float yaw_sp, vehicle_attitude_setpoint_s &att_sp, const bool omni)
 {
-	bodyzToAttitude(-thr_sp, yaw_sp, att_sp);
+	bodyzToAttitude(-thr_sp, yaw_sp, att_sp, omni);
 	att_sp.thrust_body[2] = -thr_sp.length();
+
+	if (omni == true) {
+		att_sp.thrust_body[0] = thr_sp(0);
+		att_sp.thrust_body[1] = thr_sp(1);
+		att_sp.thrust_body[2] = thr_sp(2);
+	}
 }
 
-void bodyzToAttitude(Vector3f body_z, const float yaw_sp, vehicle_attitude_setpoint_s &att_sp)
+void bodyzToAttitude(Vector3f body_z, const float yaw_sp, vehicle_attitude_setpoint_s &att_sp, const bool omni)
 {
 	// zero vector, no direction, set safe level value
 	if (body_z.norm_squared() < FLT_EPSILON) {
@@ -101,6 +107,19 @@ void bodyzToAttitude(Vector3f body_z, const float yaw_sp, vehicle_attitude_setpo
 	att_sp.roll_body = euler(0);
 	att_sp.pitch_body = euler(1);
 	att_sp.yaw_body = euler(2);
+
+	if (omni == true) {
+		// Define attitude and thrust for the omni-directional vehicles
+		Eulerf omni_euler;
+		omni_euler(0) = 0;
+		omni_euler(1) = 0;
+		omni_euler(2) = att_sp.yaw_body;
+		//Quatf omni_q_sp = omni_euler;
+		//omni_q_sp.copyTo(att_sp.q_d);
+
+		att_sp.roll_body = omni_euler(0);
+		att_sp.pitch_body = omni_euler(1);
+	}
 }
 
 Vector2f constrainXY(const Vector2f &v0, const Vector2f &v1, const float &max)

src/modules/mc_pos_control/PositionControl/ControlMath.hpp

@@ -51,14 +51,16 @@ namespace ControlMath
  * @param yaw_sp the desired yaw
  * @param att_sp attitude setpoint to fill
  */
-void thrustToAttitude(const matrix::Vector3f &thr_sp, const float yaw_sp, vehicle_attitude_setpoint_s &att_sp);
+void thrustToAttitude(const matrix::Vector3f &thr_sp, const float yaw_sp, vehicle_attitude_setpoint_s &att_sp,
+		      const bool omni = true);
 /**
  * Converts a body z vector and yaw set-point to a desired attitude.
  * @param body_z a world frame 3D vector in direction of the desired body z axis
  * @param yaw_sp the desired yaw setpoint
  * @param att_sp attitude setpoint to fill
  */
-void bodyzToAttitude(matrix::Vector3f body_z, const float yaw_sp, vehicle_attitude_setpoint_s &att_sp);
+void bodyzToAttitude(matrix::Vector3f body_z, const float yaw_sp, vehicle_attitude_setpoint_s &att_sp,
+		     const bool omni = true);
 
 /**
  * Outputs the sum of two vectors but respecting the limits and priority.

src/modules/mc_pos_control/mc_pos_control_main.cpp

@@ -910,6 +910,8 @@ MulticopterPositionControl::limit_thrust_during_landing(vehicle_attitude_setpoin
 	if (_vehicle_land_detected.ground_contact
 	    || _vehicle_land_detected.maybe_landed) {
 		// we set the collective thrust to zero, this will help to decide if we are actually landed or not
+		setpoint.thrust_body[0] = 0.f;
+		setpoint.thrust_body[1] = 0.f;
 		setpoint.thrust_body[2] = 0.f;
 		// go level to avoid corrections but keep the heading we have
 		Quatf(AxisAngle<float>(0, 0, _states.yaw)).copyTo(setpoint.q_d);