Automatic maneuvers for System Identification implemented

src/examples/maneuver_control/ManeuverControl.hpp

@@ -27,20 +27,20 @@
 #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/manual_control_setpoint.h>
 #include <uORB/uORB.h>
 
-#include "maneuver_control_setpoint.h"
-
 #define PC_SAFETY_ON 0
 #define PC_SAFETY_OFF 81980085
+#define TX_SAFETY_ON 0
+#define TX_SAFETY_OFF 219371
 #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
+#define MANEUVER_ID_DOUBLET 1
+#define MANEUVER_ID_3211 2
+#define PILOT_ABORT_POS 0.2f // Max allowed pilot input before aborting the maneuver
+#define PILOT_ABORT_NEG -0.2f // Max allowed pilot (negative) input before aborting the maneuver
 
 class ManeuverControl
 {
@@ -51,7 +51,6 @@ 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; }
@@ -60,61 +59,70 @@ public:
 	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_safety_pc(int32_t option) {param_set(_parameter_handles.safety_switch_pc, &option);}
+	void set_safety_tx(int32_t option) {param_set(_parameter_handles.safety_switch_tx, &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 */
-	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 */
-
+//	bool		_debug{true};					//if set to true, print debug output 
+
+	// Control flags
+	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
+    bool 	_run_controller{false};				// if true, runs controller and executes maneuver				
+    bool    _start_maneuver{false};  			// Triggers the start of the maneuver
+
+    // Timers
+    uint64_t _start_time{0}; 					// Start time [us]
+    uint64_t _time_last_run{0}; 				// Time of last run of the controller [us] 
+
+    // uORB 
+	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_1_pub{nullptr};			// actuator control group 2 setpoint 
+	orb_advert_t	_actuators_2_pub{nullptr};			// actuator control group 2 setpoint 
+
+	actuator_controls_s 			_actuators{}; 		// actuator control inputs 
+	manual_control_setpoint_s		_manual_sp {};		// r/c channel data 
+	vehicle_attitude_s				_att {};			// control state 
+	vehicle_control_mode_s			_vcontrol_mode {};	// vehicle control mode 
+
+	// Actuator commands 
     struct {
     	float pitch; 
     	float roll; 
     	float yaw;
+	float throttle; 
     } _actuator_commands{}; 
 
-	actuator_controls_s 			_actuators{}; 			/**< 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 */
-	vehicle_control_mode_s			_vcontrol_mode {};		/**< vehicle control mode */
 
-	bool		_debug{true};				/**< if set to true, print debug output */
+    // Parameters
 	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 control_surface;
-		int32_t duration; 		/**< Total duration [ms] */
-		float amplitude; 		/**< Amplitude [0-1] */
-		float center_pos;
-	} _parameters{};			/**< local copies of required parameters */
+		int32_t safety_switch_pc; 	// PC safety flag
+		int32_t safety_switch_tx; 	// TX safey flag
+		int32_t start_maneuver;		// Start maneuver flag 
+		int32_t maneuver_id; 		// Maneuver index
+		int32_t control_surface;	// Control surface index 
+		int32_t duration; 		// Total duration [ms] 
+		float amplitude; 		// Amplitude [0-1] 
+		float center_pos;		// Center position [0-1]
+		float trim_roll; 		// Roll trim 
+		float trim_pitch; 		// Pitch trim
+		float trim_yaw; 		// Yaw trim
+		float man_roll_scale; 		// Manual roll scale
+		float man_pitch_scale; 		// Manual trim scale 
+		float man_yaw_scale; 		// Manual yaw scale	
+	} _parameters{};			// Local copies for required parameters
 
 	struct {
 		param_t safety_switch_pc; 
@@ -125,20 +133,28 @@ private:
 		param_t duration;
 		param_t amplitude;
 		param_t center_pos; 
-	} _parameter_handles{};		/**< handles for listed parameters */
+		param_t trim_roll; 
+		param_t trim_pitch; 
+		param_t trim_yaw; 
+		param_t man_roll_scale; 
+		param_t man_pitch_scale; 
+		param_t man_yaw_scale; 
+	} _parameter_handles{};		// Handles
+
 
+	void 		set_start_flag(int option);  	
 	void 		parameters_init(); 
 	void		parameters_update();
 	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);
-
+	float		compute_3211(uint64_t maneuver_time, uint64_t maneuver_duration, float amplitude); 
+	float 		compute_maneuver(uint64_t maneuver_time); 
+	int 		check_manual_setpoint(struct manual_control_setpoint_s manual_sp, float lower_limit, float upper_limit); 
 	static int	task_main_trampoline(int argc, char *argv[]);
 	void		task_main();
 
 };
 
 
+

src/examples/maneuver_control/maneuver_control_setpoint.h

-/****************************************************************************
- *
- *   Copyright (C) 2013-2016 PX4 Development Team. All rights reserved.
- *
- * Redistribution and use in source and binary forms, with or without
- * modification, are permitted provided that the following conditions
- * are met:
- *
- * 1. Redistributions of source code must retain the above copyright
- *    notice, this list of conditions and the following disclaimer.
- * 2. Redistributions in binary form must reproduce the above copyright
- *    notice, this list of conditions and the following disclaimer in
- *    the documentation and/or other materials provided with the
- *    distribution.
- * 3. Neither the name PX4 nor the names of its contributors may be
- *    used to endorse or promote products derived from this software
- *    without specific prior written permission.
- *
- * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
- * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
- * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
- * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
- * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
- * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
- * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
- * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
- * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
- * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
- * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
- * POSSIBILITY OF SUCH DAMAGE.
- *
- ****************************************************************************/
-
-/* Auto-generated by genmsg_cpp from file actuator_controls.msg */
-
-
-#pragma once
-
-
-#include <uORB/uORB.h>
-
-
-#ifndef __cplusplus
-#define ACTUATOR_CONTROLS_INDEX_ROLL 0
-#define ACTUATOR_CONTROLS_INDEX_PITCH 1
-#define ACTUATOR_CONTROLS_INDEX_YAW 2
-#endif
-
-
-#ifdef __cplusplus
-struct __EXPORT maneuver_control_setpoint_s {
-#else
-struct maneuver_control_setpoint_s {
-#endif
-	uint64_t timestamp;
-	uint64_t timestamp_sample;
-	float pitch;
-	float roll; 
-	float yaw; 
-
-
-#ifdef __cplusplus
-	static constexpr uint8_t INDEX_ROLL = 0;
-	static constexpr uint8_t INDEX_PITCH = 1;
-	static constexpr uint8_t INDEX_YAW = 2;
-
-#endif
-};
-
-/* register this as object request broker structure */
-ORB_DECLARE(maneuver_control_setpoint);
-
-#ifdef __cplusplus
-void print_message(const maneuver_control_setpoint_s& message);
-#endif