Skip to content
Snippets Groups Projects
Commit 5cddc235 authored by alber's avatar alber
Browse files

Defined doublets and 3211 maneuvers + command line interface

parent 4a44cf9a
No related branches found
No related tags found
No related merge requests found
This diff is collapsed.
......@@ -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();
};
......@@ -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);
0% Loading or .
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment