Defined doublets and 3211 maneuvers + command line interface

src/examples/maneuver_control/ManeuverControl.hpp

@@ -15,26 +15,33 @@
 #include <px4_defines.h>
 #include <px4_tasks.h>
 #include <px4_posix.h>
+#include <px4_cli.h>
+#include <px4_getopt.h>
+#include <px4_module.h>
 
 #include <drivers/drv_hrt.h>
 #include <mathlib/mathlib.h>
 #include <matrix/math.hpp>
 #include <parameters/param.h>
-#include <pid/pid.h>
-#include <perf/perf_counter.h>
 #include <uORB/topics/actuator_controls.h>
-#include <uORB/topics/battery_status.h>
 #include <uORB/topics/manual_control_setpoint.h>
 #include <uORB/topics/parameter_update.h>
 #include <uORB/topics/vehicle_attitude.h>
 #include <uORB/topics/vehicle_attitude_setpoint.h>
 #include <uORB/topics/vehicle_control_mode.h>
-#include <uORB/topics/vehicle_rates_setpoint.h>
 #include <uORB/topics/manual_control_setpoint.h>
 #include <uORB/uORB.h>
 
 #include "maneuver_control_setpoint.h"
 
+#define PC_SAFETY_ON 0
+#define PC_SAFETY_OFF 81980085
+#define START_MANEUVER_ON 255
+#define START_MANEUVER_OFF 0
+#define MAN_ID_DOUBLET 1
+#define MAN_ID_3211 2
+#define TX_SAFETY_OFF 1800
+
 class ManeuverControl
 {
 public:
@@ -44,24 +51,54 @@ public:
 	int start();
 	bool task_running() { return _task_running; }
 
+	// Getters
+	int32_t get_maneuver_id() {return _parameters.maneuver_id; }
+	int32_t get_safety_pc() {return _parameters.safety_switch_pc; }
+	int32_t get_safety_tx() {return _parameters.safety_switch_tx; }
+	int32_t get_duration() {return _parameters.duration; }
+	float get_amplitude() {return _parameters.amplitude; }
+	float get_center_pos() {return _parameters.center_pos; }
+	int32_t get_control_surface() {return _parameters.control_surface; }
+
+	// Setters 
+	void set_start_maneuver(int32_t option){(param_set(_parameter_handles.start_maneuver, &option));} 
+	void set_safety_switch(int32_t option) {param_set(_parameter_handles.safety_switch_pc, &option);}
+
+	void set_maneuver_id(int32_t id) {param_set(_parameter_handles.maneuver_id, &id); } 
+	void set_amplitude(float amplitude) {param_set(_parameter_handles.amplitude, &amplitude); } 
+	void set_maneuver_duration(int32_t duration) {param_set(_parameter_handles.duration, &duration); } 
+	void set_control_surface(int32_t surface) {param_set(_parameter_handles.control_surface, &surface); } 
+
+	// Substitude run_maneuver by a private run_maneuver + getter + setter 
+
 private:
 	bool		_task_should_exit{false};		/**< if true, attitude control task should exit */
 	bool		_task_running{false};			/**< if true, task is running in its mainloop */
-	int			_control_task{-1};			/**< task handle */
+	int			_control_task{-1};			    /**< task handle */
 	bool 		_disable_output{true}; 			// Sets the output to zero when true 
 
+    uint64_t _start_time{0}; 			// us
+    uint64_t _time_last_run{0}; 		// us 
+    uint64_t _sampling_time{40000}; // us 
+    bool 	_run_controller{false};
+    bool     _start_maneuver{false};  
+
 	int		_att_sp_sub{-1};			/**< vehicle attitude setpoint */
 	int		_att_sub{-1};		/**< control state subscription */
 	int		_manual_sub{-1};			/**< notification of manual control updates */
 	int		_params_sub{-1};			/**< notification of parameter updates */
 	int		_vcontrol_mode_sub{-1};		/**< vehicle status subscription */
 
-
-
 	orb_advert_t	_actuators_0_pub{nullptr};		/**< actuator control group 0 setpoint */
+	orb_advert_t	_actuators_2_pub{nullptr};		/**< actuator control group 1 setpoint */
+
+    struct {
+    	float pitch; 
+    	float roll; 
+    	float yaw;
+    } _actuator_commands{}; 
 
 	actuator_controls_s 			_actuators{}; 			/**< actuator control inputs */
-	//maneuver_control_setpoint_s		_maneuver_sp {};		/**< actuator control inputs */
 	manual_control_setpoint_s		_manual_sp {};			/**< r/c channel data */
 	vehicle_attitude_s				_att {};				/**< control state */
 	vehicle_attitude_setpoint_s		_att_sp {};				/**< vehicle attitude setpoint */
@@ -69,22 +106,24 @@ private:
 
 	bool		_debug{true};				/**< if set to true, print debug output */
 	struct {
+		int32_t safety_switch_pc; 
+		int32_t safety_switch_tx; 
+		int32_t start_maneuver;		/**< Maneuver starting time [ms] */
 		int32_t maneuver_id; 
-		int32_t start_time;		/**< Maneuver starting time [ms] */
+		int32_t control_surface;
 		int32_t duration; 		/**< Total duration [ms] */
 		float amplitude; 		/**< Amplitude [0-1] */
-		float max_deflection; 
-		float min_deflection; 
 		float center_pos;
 	} _parameters{};			/**< local copies of required parameters */
 
 	struct {
+		param_t safety_switch_pc; 
+		param_t safety_switch_tx; 
+		param_t start_maneuver;
 		param_t maneuver_id;
-		param_t start_time;
+		param_t control_surface;
 		param_t duration;
 		param_t amplitude;
-		param_t max_deflection; 
-		param_t min_deflection; 
 		param_t center_pos; 
 	} _parameter_handles{};		/**< handles for listed parameters */
 
@@ -93,8 +132,13 @@ private:
 	void		vehicle_control_mode_poll();
 	void		manual_control_setpoint_poll();
 	void		vehicle_attitude_setpoint_poll();
+	void 		set_start_flag(int option);  	
+	float 		compute_doublet(uint64_t maneuver_time, uint64_t maneuver_duration, float amplitude); 
+	float		compute_3211(uint64_t maneuver_time, uint64_t maneuver_duration, float amplitude);
 
 	static int	task_main_trampoline(int argc, char *argv[]);
 	void		task_main();
 
 };
+
+

src/examples/maneuver_control/maneuver_control_params.c

@@ -32,22 +32,40 @@
  ****************************************************************************/
 
 /**
- * @file gnd_att_control_params.c
- *
- * Parameters defined by the attitude control task for ground rovers
+ * @file maneuver_control_params.c
+ * 
+ * Definition of parameters to define and control the performance 
+ * of automatic preprogrammed maneuvers.
+ * 
+ *  
+ * @author Alberto Ruiz Garcia <aruizgarcia-1@tudelft.nl>
+ */
+
+/*
+ * Controller parameters, accessible via MAVLink
  *
- * This is a modification of the fixed wing params and it is designed for ground rovers.
- * It has been developed starting from the fw  module, simplified and improved with dedicated items.
+ */
+
+/**
+ * Safety switch PC 
  *
- * All the ackowledgments and credits for the fw wing app are reported in those files.
+ * [DESCRIPTION]
  *
- * @author Marco Zorzi <mzorzi@student.ethz.ch>
+ * @unit -
+ * @group maneuver control
  */
+PARAM_DEFINE_INT32(MAN_PC_SWITCH, 0);
 
-/*
- * Controller parameters, accessible via MAVLink
+/**
+ * Safety switch TX 
+ *
+ * [DESCRIPTION]
  *
+ * @unit -
+ * @group maneuver control
  */
+PARAM_DEFINE_INT32(MAN_TX_SWITCH, 0);
+
 
 /**
  * Maneuver id 
@@ -68,7 +86,7 @@ PARAM_DEFINE_INT32(MAN_ID, 0);
  * @unit ms
  * @group maneuver control
  */
-PARAM_DEFINE_INT32(MAN_START_TIME, 0);
+PARAM_DEFINE_INT32(MAN_START, 0);
 
 /**
  * Maneuver duration 
@@ -99,45 +117,30 @@ PARAM_DEFINE_FLOAT(MAN_AMPLITUDE, 0.0f);
 
 
 /**
- * Maneuver maximum deflection 
- *
- * [DESCRIPTION]
- *
- * @unit -
- * @min 0.0
- * @max 0.8
- * @increment 0.05 
- * @group maneuver control
- */
-PARAM_DEFINE_FLOAT(MAN_MAX_DEF, 0.0f);
-
-/**
- * Maneuver minimum deflection 
+ * Maneuver center position 
  *
  * [DESCRIPTION]
  *
  * @unit -
- * @min -0.8
- * @max 0.0
+ * @min -0.4
+ * @max 0.4
  * @increment 0.05 
  * @group maneuver control
  */
-PARAM_DEFINE_FLOAT(MAN_MIN_DEF, 0.0f);
+PARAM_DEFINE_FLOAT(MAN_CENTER_POS, 0.0f);
 
 /**
- * Maneuver center position 
+ * Desired control surface for the maneuver 
  *
  * [DESCRIPTION]
  *
  * @unit -
- * @min -0.4
- * @max 0.4
- * @increment 0.05 
+ * @min 0 
+ * @max 3
+ * @increment 1
  * @group maneuver control
  */
-PARAM_DEFINE_FLOAT(MAN_CENTER_POS, 0.0f);
-
-
+PARAM_DEFINE_INT32(MAN_CTRL_SURF, 0);