Skip to content
New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Trad Heli: [GSoC] Autonomous Autorotation - SITL only #12108

Open
wants to merge 11 commits into
base: master
from

Temp Libaries param changes

  • Loading branch information...
IamPete1 committed Oct 9, 2019
commit b371dc3830fc2493ff51244e0298cb2bc41306a4
@@ -7,24 +7,24 @@ const AP_Param::GroupInfo AC_Autorotation::var_info[] = {
// @Description: Allows you to enable (1) or disable (0) the autonomous autorotation capability.
// @Values: 0:Disabled,1:Enabled
// @User: Advanced
AP_GROUPINFO_FLAGS("ENABLE", 0, AC_Autorotation, _param_enable, 0, AP_PARAM_FLAG_ENABLE),
AP_GROUPINFO_FLAGS("ENABLE", 1, AC_Autorotation, _param_enable, 0, AP_PARAM_FLAG_ENABLE),

// @Param: HS_P
// @DisplayName: P gain for head spead controller
// @Description: Increase value to increase sensitivity of head speed controller during autonomous autorotation.
// @Range: 0.3 1
// @Increment: 0.01
// @User: Advanced
AP_SUBGROUPINFO(_p_hs, "HS_", 1, AC_Autorotation, AC_P),
AP_SUBGROUPINFO(_p_hs, "HS_", 2, AC_Autorotation, AC_P),

// @Param: HS_SET_PT
// @DisplayName: Target Head Speed
// @Description: The target head speed in RPM during autorotation. Start by setting to desired hover speed and tune from there as neccassary.
// @Description: The target head speed in RPM during autorotation. Start by setting to desired hover speed and tune from there as necessary.
// @Units: RPM
// @Range: 1000 2800
// @Increment: 1
// @User: Advanced
AP_GROUPINFO("HS_SET_PT", 2, AC_Autorotation, _param_head_speed_set_point, 1500.0f),
AP_GROUPINFO("HS_SET_PT", 3, AC_Autorotation, _param_head_speed_set_point, 1500),

// @Param: TARG_SP
// @DisplayName: Target Glide Ground Speed
@@ -33,25 +33,25 @@ const AP_Param::GroupInfo AC_Autorotation::var_info[] = {
// @Range: 800 2000
// @Increment: 50
// @User: Advanced
AP_GROUPINFO("TARG_SP", 3, AC_Autorotation, _param_target_speed, SPD_HGT_CONTROLLER_GND_SPEED_TARGET),
AP_GROUPINFO("TARG_SP", 4, AC_Autorotation, _param_target_speed, SPD_HGT_CONTROLLER_GND_SPEED_TARGET),

// @Param: COL_FILT_E
// @DisplayName: Entry Phase Collective Filter
// @Description: Cut-off frequency for collective low pass filter. For the entry phase. Acts as a following trim. Must be higher than COL_FILT_G.
// @Description: Cut-off frequency for collective low pass filter. For the entry phase. Acts as a following trim. Must be higher than AROT_COL_FILT_G.
// @Units: Hz
// @Range: 0.2 0.5
// @Increment: 0.01
// @User: Advanced
AP_GROUPINFO("COL_FILT_E", 4, AC_Autorotation, _param_col_entry_cutoff_freq, HS_CONTROLLER_ENTRY_COL_FLITER),
AP_GROUPINFO("COL_FILT_E", 5, AC_Autorotation, _param_col_entry_cutoff_freq, HS_CONTROLLER_ENTRY_COL_FILTER),

// @Param: COL_FILT_G
// @DisplayName: Glide Phase Collective Filter
// @Description: Cut-off frequency for collective low pass filter. For the glide phase. Acts as a following trim. Must be lower than COL_FILT_E.
// @Description: Cut-off frequency for collective low pass filter. For the glide phase. Acts as a following trim. Must be lower than AROT_COL_FILT_E.
// @Units: Hz
// @Range: 0.03 0.15
// @Increment: 0.01
// @User: Advanced
AP_GROUPINFO("COL_FILT_G", 5, AC_Autorotation, _param_col_glide_cutoff_freq, HS_CONTROLLER_GLIDE_COL_FLITER),
AP_GROUPINFO("COL_FILT_G", 6, AC_Autorotation, _param_col_glide_cutoff_freq, HS_CONTROLLER_GLIDE_COL_FILTER),

// @Param: AS_ACC_MAX
// @DisplayName: Forward Acceleration Limit
@@ -60,7 +60,7 @@ const AP_Param::GroupInfo AC_Autorotation::var_info[] = {
// @Range: 50 200
// @Increment: 10
// @User: Advanced
AP_GROUPINFO("AS_ACC_MAX", 6, AC_Autorotation, _param_accel_max, SPD_HGT_CONTROLLER_MAX_ACCEL),
AP_GROUPINFO("AS_ACC_MAX", 7, AC_Autorotation, _param_accel_max, SPD_HGT_CONTROLLER_MAX_ACCEL),

// @Param: BAIL_TIME
// @DisplayName: Bail Out Timer
@@ -69,7 +69,7 @@ const AP_Param::GroupInfo AC_Autorotation::var_info[] = {
// @Range: 0.5 4
// @Increment: 0.1
// @User: Advanced
AP_GROUPINFO("BAIL_TIME", 7, AC_Autorotation, _param_bail_time, AROT_BAIL_OUT_TIME),
AP_GROUPINFO("BAIL_TIME", 8, AC_Autorotation, _param_bail_time, AROT_BAIL_OUT_TIME),

// @Param: HS_SENSOR
// @DisplayName: Main Rotor RPM Sensor
@@ -78,7 +78,7 @@ const AP_Param::GroupInfo AC_Autorotation::var_info[] = {
// @Range: 0.5 3
// @Increment: 0.1
// @User: Advanced
AP_GROUPINFO("HS_SENSOR", 8, AC_Autorotation, _param_rpm_instance, 0),
AP_GROUPINFO("HS_SENSOR", 9, AC_Autorotation, _param_rpm_instance, 0),

AP_GROUPEND
};
@@ -162,6 +162,7 @@ void AC_Autorotation::set_collective(float collective_filter_cutoff)
//function that sets parameter values in flight mode
void AC_Autorotation::set_param_values(int16_t* set_point_hs, int16_t* accel, int16_t* targ_s, float* td_alt, float* ent_freq, float* glide_freq, float* bail_time)
{
}/*
*set_point_hs = _param_head_speed_set_point;
*accel = _param_accel_max;
*targ_s = _param_target_speed;
@@ -170,7 +171,7 @@ void AC_Autorotation::set_param_values(int16_t* set_point_hs, int16_t* accel, in
*glide_freq = _param_col_glide_cutoff_freq;
*bail_time = _param_bail_time;
}

*/

//function that gets rpm and does rpm signal checking to ensure signal is reliable
//before using it in the controller
@@ -20,12 +20,12 @@
// Head Speed (HS) controller specific default definitions
#define HS_CONTROLLER_COLLECTIVE_CUTOFF_FREQ 2.0f // low-pass filter on accel error (unit: hz)
#define HS_CONTROLLER_HEADSPEED_P 0.7f // Default P gain for head speed controller (unit: -)
#define HS_CONTROLLER_ENTRY_COL_FLITER 0.35f // Default low pass filter frequency during the entry phase (unit: Hz)
#define HS_CONTROLLER_GLIDE_COL_FLITER 0.05f // Default low pass filter frequency during the glide phase (unit: Hz)
#define HS_CONTROLLER_ENTRY_COL_FILTER 0.35f // Default low pass filter frequency during the entry phase (unit: Hz)
#define HS_CONTROLLER_GLIDE_COL_FILTER 0.05f // Default low pass filter frequency during the glide phase (unit: Hz)

// Speed Height controller specific default definitions for autorotation use
#define SPD_HGT_CONTROLLER_GND_SPEED_TARGET 1100.0f // Default target ground speed for speed height controller (unit: cm/s)
#define SPD_HGT_CONTROLLER_MAX_ACCEL 100.0f // Default acceleration limit for speed height controller (unit: cm/s/s)
#define SPD_HGT_CONTROLLER_GND_SPEED_TARGET 1100 // Default target ground speed for speed height controller (unit: cm/s)
#define SPD_HGT_CONTROLLER_MAX_ACCEL 100 // Default acceleration limit for speed height controller (unit: cm/s/s)

class AC_Autorotation
{
@@ -48,8 +48,7 @@ class AC_Autorotation
// User Settable Parameters
static const struct AP_Param::GroupInfo var_info[];


protected:
private:

//--------Internal Variables--------
float _current_rpm;
@@ -64,18 +63,18 @@ class AC_Autorotation
float _ff_term_hs; //Following trim feed forward contribution to collective setting

//--------Parameter Values--------
AC_P _p_hs;
AP_Int8 _param_enable;

This comment has been minimized.

Copy link
@rmackay9

rmackay9 Oct 4, 2019

Contributor

normally we don't prefix our parameter names with "param".

AP_Float _param_hs_p;
AC_P _p_hs;
//AP_Float _param_hs_p;
AP_Int16 _param_head_speed_set_point;
AP_Int16 _param_accel_max;
AP_Int16 _param_target_speed;
AP_Float _param_col_entry_cutoff_freq;
AP_Float _param_col_glide_cutoff_freq;
AP_Int16 _param_accel_max;
AP_Float _param_bail_time;
AP_Int8 _param_rpm_instance;

AP_Float _param_td_alt; //currently no parameter. Will be used in future work.
float _param_td_alt; //currently no parameter. Will be used in future work.

//--------Internal Flags--------
struct controller_flags {
@@ -93,4 +92,4 @@ class AC_Autorotation
//--------References to Other Libraries--------
AP_Motors& _motors;

};
};
@@ -11,7 +11,7 @@ const AP_Param::GroupInfo AP_SpdHgtControl_Heli::var_info[] = {
// @Description: This parameter is used by the set by the autorotation controller and is based on AROT_ENABLE.
// @Values: 0:Disabled,1:Enabled
// @User: Advanced
AP_GROUPINFO_FLAGS("ENABLE", 0, AP_SpdHgtControl_Heli, _param_enable, 0, AP_PARAM_FLAG_ENABLE),
AP_GROUPINFO_FLAGS("ENABLE", 1, AP_SpdHgtControl_Heli, _param_enable, 0, AP_PARAM_FLAG_ENABLE),

// @Param: VEL_P
// @DisplayName: Velocity (horizontal) P gain
@@ -66,8 +66,8 @@ const AP_Param::GroupInfo AP_SpdHgtControl_Heli::var_info[] = {
// @Description: No D term in this controller. Not used. Leave as 0.
// @Units: Hz
// @User: Advanced
AP_SUBGROUPINFO(_pid_vel, "VEL_", 1, AP_SpdHgtControl_Heli, AC_PID),
AP_SUBGROUPINFO(_pid_vel, "VEL_", 2, AP_SpdHgtControl_Heli, AC_PID),

AP_GROUPEND
};

@@ -86,9 +86,9 @@ class AP_SpdHgtControl_Heli {
AP_AHRS& _ahrs;
AP_InertialNav& _inav;


AC_PID _pid_vel;
AP_Int8 _param_enable;
AC_PID _pid_vel;

float _vel_target;
float _pitch_target;
float _vel_error;
ProTip! Use n and p to navigate between commits in a pull request.
You can’t perform that action at this time.