Mirror
/
ardupilot
mirror of https://github.com/ArduPilot/ardupilot
5
0
Fork 0
Code Issues Packages Projects Releases Wiki Activity

AC_InputManager: Initial class creation

This commit is contained in:
Robert Lefebvre 2015-10-10 19:51:37 -04:00 committed by Randy Mackay
parent c66af9b788
commit afcc304cbf
5 changed files with 230 additions and 0 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

1
ArduCopter/make.inc
View File

@ -59,3 +59,4 @@ LIBRARIES += AP_Terrain
LIBRARIES += AP_RPM
LIBRARIES += AC_PrecLand
LIBRARIES += AP_IRLock
LIBRARIES += AC_InputManager

14
libraries/AC_InputManager/AC_InputManager.cpp Normal file
View File

@ -0,0 +1,14 @@
// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*-
#include "AC_InputManager.h"
#include <AP_Math/AP_Math.h>
#include <AP_HAL/AP_HAL.h>
extern const AP_HAL::HAL& hal;
const AP_Param::GroupInfo AC_InputManager::var_info[] = {
// Placeholder for future parameters in this class.
AP_GROUPEND
};

33
libraries/AC_InputManager/AC_InputManager.h Normal file
View File

@ -0,0 +1,33 @@
// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*-
/// @file AC_InputManager.h
/// @brief Pilot manual control input library
#ifndef AC_INPUTMANAGER_H
#define AC_INPUTMANAGER_H
#include <AP_Common/AP_Common.h>
#include <AP_Param/AP_Param.h>
#include <AP_Math/AP_Math.h>
/// @class AC_InputManager
/// @brief Class managing the pilot's control inputs
class AC_InputManager{
public:
AC_InputManager(uint16_t loop_rate):
_loop_rate(loop_rate)
{
// setup parameter defaults
AP_Param::setup_object_defaults(this, var_info);
}
static const struct AP_Param::GroupInfo var_info[];
protected:
// internal variables
uint16_t _loop_rate; // rate at which output() function is called (normally 400hz)
};
#endif /* AC_INPUTMANAGER_H */

124
libraries/AC_InputManager/AC_InputManager_Heli.cpp Normal file
View File

@ -0,0 +1,124 @@
// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*-
#include "AC_InputManager_Heli.h"
#include <AP_Math/AP_Math.h>
#include <AP_HAL/AP_HAL.h>
extern const AP_HAL::HAL& hal;
const AP_Param::GroupInfo AC_InputManager_Heli::var_info[] = {
// parameters from parent vehicle
AP_NESTEDGROUPINFO(AC_InputManager, 0),
// @Param: STAB_COL_1
// @DisplayName: Stabilize Mode Collective Point 1
// @Description: Helicopter's minimum collective pitch setting at zero throttle input in Stabilize mode
// @Range: 0 500
// @Units: Percent*10
// @Increment: 1
// @User: Standard
AP_GROUPINFO("STAB_COL_1", 1, AC_InputManager_Heli, _heli_stab_col_min, AC_ATTITUDE_HELI_STAB_COLLECTIVE_MIN_DEFAULT),
// @Param: STAB_COL_2
// @DisplayName: Stabilize Mode Collective Point 2
// @Description: Helicopter's collective pitch setting at mid-low throttle input in Stabilize mode
// @Range: 0 500
// @Units: Percent*10
// @Increment: 1
// @User: Standard
AP_GROUPINFO("STAB_COL_2", 2, AC_InputManager_Heli, _heli_stab_col_low, AC_ATTITUDE_HELI_STAB_COLLECTIVE_LOW_DEFAULT),
// @Param: STAB_COL_3
// @DisplayName: Stabilize Mode Collective Point 3
// @Description: Helicopter's collective pitch setting at mid-high throttle input in Stabilize mode
// @Range: 500 1000
// @Units: Percent*10
// @Increment: 1
// @User: Standard
AP_GROUPINFO("STAB_COL_3", 3, AC_InputManager_Heli, _heli_stab_col_high, AC_ATTITUDE_HELI_STAB_COLLECTIVE_HIGH_DEFAULT),
// @Param: STAB_COL_4
// @DisplayName: Stabilize Mode Collective Point 4
// @Description: Helicopter's maximum collective pitch setting at full throttle input in Stabilize mode
// @Range: 500 1000
// @Units: Percent*10
// @Increment: 1
// @User: Standard
AP_GROUPINFO("STAB_COL_4", 4, AC_InputManager_Heli, _heli_stab_col_max, AC_ATTITUDE_HELI_STAB_COLLECTIVE_MAX_DEFAULT),
// @Param: ACRO_COL_EXP
// @DisplayName: Acro Mode Collective Expo
// @Description: Used to soften collective pitch inputs near center point in Acro mode.
// @Values: 0:Disabled,0.1:Very Low,0.2:Low,0.3:Medium,0.4:High,0.5:Very High
// @User: Advanced
AP_GROUPINFO("ACRO_COL_EXP", 5, AC_InputManager_Heli, _acro_col_expo, 0),
AP_GROUPEND
};
// get_pilot_desired_collective - rescale's pilot collective pitch input in Stabilize and Acro modes
int16_t AC_InputManager_Heli::get_pilot_desired_collective(int16_t control_in)
{
float slope_low, slope_high, slope_range, slope_run, scalar;
int16_t stab_col_out, acro_col_out;
// calculate stabilize collective value which scales pilot input to reduced collective range
// code implements a 3-segment curve with knee points at 40% and 60% throttle input
if (control_in < 400){
slope_low = _heli_stab_col_min;
slope_high = _heli_stab_col_low;
slope_range = 400;
slope_run = control_in;
} else if(control_in <600){
slope_low = _heli_stab_col_low;
slope_high = _heli_stab_col_high;
slope_range = 200;
slope_run = control_in - 400;
} else {
slope_low = _heli_stab_col_high;
slope_high = _heli_stab_col_max;
slope_range = 400;
slope_run = control_in - 600;
}
scalar = (slope_high - slope_low)/slope_range;
stab_col_out = slope_low + slope_run * scalar;
stab_col_out = constrain_int16(stab_col_out, 0, 1000);
//
// calculate expo-scaled acro collective
// range check expo
if (_acro_col_expo > 1.0f) {
_acro_col_expo = 1.0f;
}
if (_acro_col_expo <= 0) {
acro_col_out = control_in;
} else {
// expo variables
float col_in, col_in3, col_out;
col_in = (float)(control_in-500)/500.0f;
col_in3 = col_in*col_in*col_in;
col_out = (_acro_col_expo * col_in3) + ((1-_acro_col_expo)*col_in);
acro_col_out = 500 + col_out*500;
}
acro_col_out = constrain_int16(acro_col_out, 0, 1000);
// ramp to and from stab col over 1/2 second
if (_im_flags_heli.use_stab_col && (_stab_col_ramp < 1.0)){
_stab_col_ramp += 2.0f/(float)_loop_rate;
} else if(!_im_flags_heli.use_stab_col && (_stab_col_ramp > 0.0)){
_stab_col_ramp -= 2.0f/(float)_loop_rate;
}
_stab_col_ramp = constrain_float(_stab_col_ramp, 0.0, 1.0);
// scale collective output smoothly between acro and stab col
int16_t collective_out;
collective_out = (float)((1.0-_stab_col_ramp)*acro_col_out + _stab_col_ramp*stab_col_out);
collective_out = constrain_int16(collective_out, 0, 1000);
return collective_out;
}

58
libraries/AC_InputManager/AC_InputManager_Heli.h Normal file
View File

@ -0,0 +1,58 @@
// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*-
/// @file AC_InputManager_Heli.h
/// @brief Pilot manual control input library for Conventional Helicopter
#ifndef AC_INPUTMANAGER_HELI_H
#define AC_INPUTMANAGER_HELI_H
#include <AP_Param/AP_Param.h>
#include <AP_Common/AP_Common.h>
#include "AC_InputManager.h"
# define AC_ATTITUDE_HELI_STAB_COLLECTIVE_MIN_DEFAULT 0
# define AC_ATTITUDE_HELI_STAB_COLLECTIVE_LOW_DEFAULT 400
# define AC_ATTITUDE_HELI_STAB_COLLECTIVE_HIGH_DEFAULT 600
# define AC_ATTITUDE_HELI_STAB_COLLECTIVE_MAX_DEFAULT 1000
/// @class AP_InputManager_Heli
/// @brief Class managing the pilot's control inputs for Conventional Helicopter
class AC_InputManager_Heli : public AC_InputManager {
public:
// Constructor
AC_InputManager_Heli(uint16_t loop_rate):
AC_InputManager(loop_rate)
{
// setup parameter defaults
AP_Param::setup_object_defaults(this, var_info);
}
// get_pilot_desired_collective - rescale's pilot collective pitch input in Stabilize and Acro modes
int16_t get_pilot_desired_collective(int16_t control_in);
// set_use_stab_col - setter function
void set_use_stab_col(bool use) { _im_flags_heli.use_stab_col = use; }
// set_heli_stab_col_ramp - setter function
void set_stab_col_ramp(float ramp) { _stab_col_ramp = constrain_float(ramp, 0.0, 1.0); }
static const struct AP_Param::GroupInfo var_info[];
private:
struct InputManagerHeliFlags {
uint8_t use_stab_col : 1; // 1 if we should use Stabilise mode collective range, 0 for Acro range
} _im_flags_heli;
// factor used to smoothly ramp collective from Acro value to Stab-Col value
float _stab_col_ramp = 0;
AP_Int16 _heli_stab_col_min; // minimum collective pitch setting at zero throttle input in Stabilize mode
AP_Int16 _heli_stab_col_low; // collective pitch setting at mid-low throttle input in Stabilize mode
AP_Int16 _heli_stab_col_high; // collective pitch setting at mid-high throttle input in Stabilize mode
AP_Int16 _heli_stab_col_max; // maximum collective pitch setting at full throttle input in Stabilize mode
AP_Float _acro_col_expo; // used to soften collective pitch inputs near center point in Acro mode
};
#endif /* AC_INPUTMANAGER_HELI_H */