Skip to content

Commit

Permalink
Control/AUV/Attitude: Improve altitude controller.
Browse files Browse the repository at this point in the history 
The following changes were made to the altitude controller:

  - Add a dedicated PID loop, so that the PID gains for altitude control
    can be different from the PID gains for depth control;

  - Add reference derivative estimation during altitude control.
    The depth value corresponding to the altitude reference is smoothed
    using a FIR filter so that good derivative estimates are obtained.

  - Add pitch reference derivative estimation.
    (This also affects depth control.)
  • Loading branch information
@mcpca
mcpca committed Jan 22, 2020
1 parent 2f1a216 commit 5380c8c
Show file tree
Hide file tree
Showing 2 changed files with 100 additions and 13 deletions.
34 changes: 34 additions & 0 deletions etc/auv/control.ini
View file
Original file line number Diff line number Diff line change
Expand Up @@ -61,6 +61,7 @@ Enable roll controller = true
Roll PID Gains = 2.1, 0.0, 0.4
Roll Integral Limit = -1.0
Depth PID Gains = 0.6, 0.0, 0.7
Altitude PID Gains = 0.8, 0.0, 0.7
Depth Integral Limit = -1.0
Use Fixed Depth Offset = false
Depth Offset = 0.2
Expand Down Expand Up @@ -93,6 +94,39 @@ Roll Compensation -- Offset Angle = 0.0
Roll Compensation -- Use Speed = false
Roll Compensation -- Speed Bounds = 1.0, 1.8
Roll Compensation -- Speed Gain = 0.5
Altitude Control -- Filter Weights = -1.27482175e-06,-3.65738774e-06,
-1.17070428e-05,-3.11243736e-05,
-7.00674137e-05,-1.39508991e-04,
-2.52830535e-04,-4.24392614e-04,
-6.66914986e-04,-9.87650957e-04,
-1.38354874e-03,-1.83583825e-03,
-2.30473202e-03,-2.72513810e-03,
-3.00439909e-03,-3.02304749e-03,
-2.63937080e-03,-1.69820293e-03,
-4.38264254e-05,2.46375916e-03,
5.93058693e-03,1.04104220e-02,
1.58886929e-02,2.22710063e-02,
2.93783362e-02,3.69503802e-02,
4.46577048e-02,5.21222453e-02,
5.89446365e-02,6.47358766e-02,
6.91501237e-02,7.19151017e-02,
7.28567216e-02,7.19151017e-02,
6.91501237e-02,6.47358766e-02,
5.89446365e-02,5.21222453e-02,
4.46577048e-02,3.69503802e-02,
2.93783362e-02,2.22710063e-02,
1.58886929e-02,1.04104220e-02,
5.93058693e-03,2.46375916e-03,
-4.38264254e-05,-1.69820293e-03,
-2.63937080e-03,-3.02304749e-03,
-3.00439909e-03,-2.72513810e-03,
-2.30473202e-03,-1.83583825e-03,
-1.38354874e-03,-9.87650957e-04,
-6.66914986e-04,-4.24392614e-04,
-2.52830535e-04,-1.39508991e-04,
-7.00674137e-05,-3.11243736e-05,
-1.17070428e-05,-3.65738774e-06,
-1.27482175e-06

[Control.AUV.LMI]
Enabled = Never
Expand Down
79 changes: 66 additions & 13 deletions src/Control/AUV/Attitude/Task.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -31,6 +31,7 @@

// ISO C++ 98 headers.
#include <cmath>
#include <memory>
#include <string>
#include <vector>

Expand Down Expand Up @@ -60,11 +61,13 @@ namespace Control
//! Required loops.
static const uint32_t c_required = IMC::CL_TORQUE;
//! Loops names.
static const std::string c_loop_name[] = {DTR_RT("Roll"), DTR_RT("Pitch"),
DTR_RT("Depth"), DTR_RT("Heading"),
DTR_RT("Heading Rate")};
static const std::string c_loop_name[]
= { DTR_RT("Roll"), DTR_RT("Pitch"), DTR_RT("Depth"),
DTR_RT("Altitude"), DTR_RT("Heading"), DTR_RT("Heading Rate") };
//! Loops units.
static const unsigned c_loop_unit[] = {Units::Degree, Units::Degree, Units::Degree, Units::DegreePerSecond, Units::Degree};
static const unsigned c_loop_unit[]
= { Units::Degree, Units::Degree, Units::Degree,
Units::Degree, Units::DegreePerSecond, Units::Degree };

enum Loops
{
Expand All @@ -74,6 +77,8 @@ namespace Control
LP_PITCH,
//! Depth loop.
LP_DEPTH,
//! Altitude loop.
LP_ALT,
//! Heading loop.
LP_HEADING,
//! Heading rate loop.
Expand Down Expand Up @@ -140,6 +145,8 @@ namespace Control
RollCompensation rc;
//!
int sampling_rate_relation;
//! Weights of the vertical reference filter.
std::vector<float> vref_filt_weights;
};

struct Task: public DUNE::Control::BasicAutopilot
Expand All @@ -165,10 +172,20 @@ namespace Control

int m_sampling_rate_relation;

//! Smoothed altitude reference.
std::unique_ptr<Math::FIRFilter<float>> m_vref;
//! Derivative of the altitude reference.
Math::Derivative<float> m_vref_d;
//! Derivative of the pitch reference.
Math::Derivative<float> m_pref_d;

Task(const std::string& name, Tasks::Context& ctx):
DUNE::Control::BasicAutopilot(name, ctx, c_controllable, c_required),
m_ca(NULL),
m_extra_pitch(false)
m_extra_pitch(false),
m_vref(nullptr),
m_vref_d(),
m_pref_d()
{
// Load controller gains and integral limits.
for (unsigned i = 0; i < LP_MAX_LOOPS; ++i)
Expand Down Expand Up @@ -321,6 +338,10 @@ namespace Control
.maximumValue("5")
.description("Depth-to-pitch sampling rate relation");

param("Altitude Control -- Filter Weights", m_args.vref_filt_weights)
.description("Impulse response of the FIR filter used to smooth the "
"depth reference during altitude control.");

m_ctx.config.get("General", "Underwater Depth Threshold", "0.3", m_args.depth_threshold);
}

Expand All @@ -340,13 +361,16 @@ namespace Control

if (m_ca_args.enabled)
m_ca = new CoarseAltitude(&m_ca_args);

m_vref = std::make_unique<FIRFilter<float>>(m_args.vref_filt_weights);
}

//! Release Resources.
void
onResourceRelease(void)
{
Memory::clear(m_ca);
m_vref.reset();

BasicAutopilot::onResourceRelease();
}
Expand Down Expand Up @@ -427,6 +451,7 @@ namespace Control
output_limits[LP_ROLL] = m_args.max_fin_rot;
output_limits[LP_PITCH] = m_args.max_pitch_act;
output_limits[LP_DEPTH] = m_args.max_pitch;
output_limits[LP_ALT] = m_args.max_pitch;
output_limits[LP_HEADING] = m_args.max_hrate;
output_limits[LP_HRATE] = m_args.max_fin_rot;

Expand All @@ -449,6 +474,12 @@ namespace Control
{
BasicAutopilot::reset();

if (m_vref)
m_vref->clear();

m_vref_d.clear();
m_pref_d.clear();

for (unsigned i = 0; i < LP_MAX_LOOPS; ++i)
m_pid[i].reset();
}
Expand Down Expand Up @@ -547,11 +578,15 @@ namespace Control
case VERTICAL_MODE_ALTITUDE:
if (msg->alt < m_args.min_dvl_alt && msg->depth < m_args.min_dvl_depth)
{
m_vref->clear();
m_vref_d.clear();

z_error = c_min_depth_ref;
}
else
{
float bfd = getBottomFollowDepth();
m_vref->update(bfd);

if (m_ca != NULL)
{
Expand Down Expand Up @@ -598,6 +633,8 @@ namespace Control
m_extra_pitch = false;
m_pid[LP_DEPTH].setOutputLimits(-m_args.max_pitch,
m_args.max_pitch);
m_pid[LP_ALT].setOutputLimits(-m_args.max_pitch,
m_args.max_pitch);
}
}
else
Expand All @@ -611,21 +648,31 @@ namespace Control
m_extra_pitch = true;
float pitch = m_args.max_pitch + m_args.extra_pitch;
m_pid[LP_DEPTH].setOutputLimits(-pitch, pitch);
m_pid[LP_ALT].setOutputLimits(-pitch, pitch);
}
}

double val
= -(-sin(msg->theta) * msg->u
+ cos(msg->theta)
* (sin(msg->phi) * msg->v + cos(msg->phi) * msg->w));
const float z_rate
= -sin(msg->theta) * msg->u
+ cos(msg->theta)
* (sin(msg->phi) * msg->v + cos(msg->phi) * msg->w);

// Positive depth implies negative pitch
cmd = -m_pid[LP_DEPTH].step(timestep, z_error, val);
// Positive depth rate implies negative pitch, so the PID output
// is inverted.
if (getVerticalMode() == VERTICAL_MODE_DEPTH)
cmd = -m_pid[LP_DEPTH].step(timestep, z_error, -z_rate);
else
{
const float ref_rate = m_vref_d.update(m_vref->get());
cmd
= -m_pid[LP_ALT].step(timestep, z_error, ref_rate - z_rate);
}
}
else
{
cmd = m_args.surface_pitch;
}

// Log the desired pitch
m_pitch_ref.value = cmd;
dispatch(m_pitch_ref);
Expand All @@ -637,14 +684,20 @@ namespace Control
}
}

//Now, track pitch
// Now, track pitch
float pitch_err = (cmd - msg->theta);

// With attitude compensation we use a different approach
if (m_args.error_attitude)
return pitch_err;

cmd = m_pid[LP_PITCH].step(timestep, pitch_err, -(msg->q * cos(msg->phi) - msg->r * sin(msg->phi)));
const float ref_rate = m_pref_d.update(cmd);
const float pitch_rate
= msg->q * cos(msg->phi) - msg->r * sin(msg->phi);

cmd
= m_pid[LP_PITCH].step(timestep, pitch_err, ref_rate - pitch_rate);

return cmd;
}

Expand Down

0 comments on commit 5380c8c

Please sign in to comment.