limitting parameters to reasonable values & cleanup

src/modules/interface/crtp_commander_high_level.h

@@ -214,10 +214,10 @@ int crtpCommanderHighLevelGoTo2(const float x, const float y, const float z, con
 /**
  * @brief Follow a spiral segment (spline approximation of and arc for <= 90-degree segments)
  *
- * @param phi         spiral angle (rad), positive for left turn, negative for right turn
- * @param r0          initial radius (m)
- * @param rf          final radius (m)
- * @param dz          altitude gain (m)
+ * @param phi         spiral angle (rad), limited to +/- 2pi
+ * @param r0          initial radius (m), must be positive
+ * @param rf          final radius (m), must be positive
+ * @param dz          altitude gain (m), positive to climb, negative to descent
  * @param duration_s  time it should take to reach the end of the spiral (s)
  * @param sideways    true if crazyflie should spiral sideways instead of forward
  * @param clockwise   true if crazyflie should spiral clockwise instead of counter-clockwise

src/modules/src/planner.c

@@ -37,6 +37,7 @@ implementation of planning state machine
 */
 #include <stddef.h>
 #include "planner.h"
+#include "arm_math.h"
 
 static struct traj_eval plan_eval(struct planner *p, float t);
 
@@ -219,22 +220,36 @@ int plan_go_to(struct planner *p, bool relative, bool linear, struct vec hover_p
 
 int plan_spiral_from(struct planner *p, const struct traj_eval *curr_eval, bool sideways, bool clockwise, float spiral_angle, float radius0, float radiusF, float ascent, float duration, float t)
 {
-	int sense = -1 + 2*clockwise; // -1 counter-clockwise, +1 clockwise
+	// Limitting the inputs
+  if (spiral_angle > 2*PI) {
+    spiral_angle = 2*PI;
+    DEBUG_PRINT("Warning: spiral angle saturated at 2pi\n");
+  }
+  else if (spiral_angle < -2*PI) {
+    spiral_angle = -2*PI;
+    DEBUG_PRINT("Warning: spiral angle saturated at -2pi\n");
+  }
+  if (radius0 < 0) {
+    radius0 = 0;
+    DEBUG_PRINT("Warning: radius set to 0 (was negative) \n");
+  }
+  if (radiusF < 0) {
+    radiusF = 0;
+    DEBUG_PRINT("Warning: radius set to 0 (was negative) \n");
+  }
+
+  int sense = -1 + 2*clockwise; // -1 counter-clockwise, +1 clockwise
 	float omz =  -sense*spiral_angle/duration;
 	float dz = ascent/duration;
 	float dr = (radiusF - radius0)/duration;
 
-	// struct traj_eval setpoint = plan_current_goal(p, t);
-
 	struct vec pos0 = curr_eval->pos;
 
 	float phi0 = normalize_radians(curr_eval->yaw);
 
 	float yawF = phi0 - sense*spiral_angle;
 	float phiF = phi0 - sense*spiral_angle;
 
-	// TODO sideways, now assuming forward/backward
-
 	float c0 = cosf(phi0);
 	float s0 = sinf(phi0);
 	float cF = cosf(phiF);